xyzwpr implementation

Author: mattj23

Diff for: .gitignore

@@ -1,5 +1,6 @@
 .idea/
 venv/
 __pycache__/
+.pytest_cache/
 
 *.pyc

Diff for: python/generate_tests.py

@@ -0,0 +1,45 @@
+import json
+from pathlib import Path
+import numpy
+
+raw_data = """
+{ -0.355708, -0.173709, 0.918312, 0.000000, 0.717742, -0.680099, 0.149368, 0.000000, 0.598597, 0.712243, 0.366595, 0.000000, 250.689172, 190.092170, 377.021336, 1.000000 }, new double[] { 250.689172, 190.092170, 377.021336, 22.168324, -66.680543, -153.971487 }) };
+{ 0.875453, 0.474534, 0.091654, 0.000000, 0.335633, -0.733383, 0.591185, 0.000000, 0.347755, -0.486792, -0.801312, 0.000000, 324.515443, -269.225543, 109.845856, 1.000000 }, new double[] { 324.515443, -269.225543, 109.845856, 143.581155, -5.258767, 28.459638 }) };
+{ 0.829857, -0.392800, 0.396290, 0.000000, -0.290899, -0.910637, -0.293458, 0.000000, 0.476147, 0.128247, -0.869964, 0.000000, 480.998803, -24.848409, 44.451180, 1.000000 }, new double[] { 480.998803, -24.848409, 44.451180, -161.359637, -23.346471, -25.329844 }) };
+{ 0.622543, 0.286126, 0.728404, 0.000000, 0.659905, -0.692256, -0.292073, 0.000000, 0.420672, 0.662506, -0.619775, 0.000000, 427.425001, 317.808911, 185.181686, 1.000000 }, new double[] { 427.425001, 317.808911, 185.181686, -154.767540, -46.752739, 24.683898 }) };
+{ 0.121273, 0.027579, 0.992236, 0.000000, 0.016312, -0.999534, 0.025788, 0.000000, 0.992485, 0.013058, -0.121667, 0.000000, 640.024599, 54.109546, 303.808089, 1.000000 }, new double[] { 640.024599, 54.109546, 303.808089, 168.032738, -82.855635, 12.811951 }) };
+{ -0.155532, 0.025165, 0.987510, 0.000000, -0.422104, -0.905508, -0.043406, 0.000000, 0.893106, -0.423583, 0.151457, 0.000000, 416.744163, -205.770414, 328.606008, 1.000000 }, new double[] { 416.744163, -205.770414, 328.606008, -15.991663, -80.935040, 170.809286 }) };
+{ -0.027822, -0.281693, 0.959101, 0.000000, -0.180557, -0.942275, -0.281989, 0.000000, 0.983171, -0.181018, -0.024646, 0.000000, 431.202555, -21.951723, 228.946720, 1.000000 }, new double[] { 431.202555, -21.951723, 228.946720, -94.994930, -73.556872, -95.640661 }) };
+{ -0.779100, 0.547427, 0.305495, 0.000000, -0.238741, -0.709679, 0.662841, 0.000000, 0.579661, 0.443485, 0.683604, 0.000000, 243.109205, 126.817247, 442.214652, 1.000000 }, new double[] { 243.109205, 126.817247, 442.214652, 44.116508, -17.787954, 144.906558 }) };
+{ -0.338094, 0.151820, 0.928786, 0.000000, -0.333176, -0.942296, 0.032746, 0.000000, 0.880162, -0.298378, 0.369167, 0.000000, 342.415569, -7.408160, 346.902494, 1.000000 }, new double[] { 342.415569, -7.408160, 346.902494, 5.069014, -68.246332, 155.817742 }) };
+
+"""
+
+
+def main():
+    print("@pytest.mark.parametrize('matrix,xyzwpr', [")
+    for line in raw_data.split("\n"):
+        if not line.strip():
+            continue
+
+        start, end = line.split("},")
+        start = _as_list(start.split("{")[1].strip())
+        end = _as_list(end.split("{")[1].strip().split("}")[0].strip())
+
+        t = numpy.matrix(start).reshape(4, 4).transpose().tolist()
+        print(f" ({t}, {end}),")
+    print("])")
+
+    # source_file = Path.cwd().parent / "test_cases" / "r30ib_forward_kinematics.json"
+    # with source_file.open("r") as handle:
+    #     data = json.load(handle)
+    #
+    # print(data)
+
+
+def _as_list(text: str) -> list[float]:
+    return [float(x.strip()) for x in text.split(",")]
+
+
+if __name__ == '__main__':
+    main()

Diff for: python/lrmate/robot.py

@@ -13,11 +13,8 @@
 import math
 import os
 from typing import List, Optional, Dict, Union
-from dataclasses import dataclass
 from pathlib import Path
 
-import numpy
-from scipy.spatial.transform import Rotation
 
 from open3d import geometry
 import open3d
@@ -152,92 +149,3 @@ def link_meshes(self) -> List[open3d.geometry.TriangleMesh]:
         return [link.mesh for link in self.links.values()]
 
 
-@dataclass
-class Vector:
-    x: float
-    y: float
-    z: float
-
-    def __add__(self, other):
-        return Vector(self.x + other.x, self.y + other.y, self.z + other.z)
-
-    def __sub__(self, other):
-        return Vector(self.x - other.x, self.y - other.y, self.z - other.z)
-
-    def __mul__(self, other: Union[float, Transform]) -> Vector:
-        if isinstance(other, float):
-            return Vector(self.x * other, self.y * other, self.z * other)
-        elif isinstance(other, Transform):
-            moved = other * numpy.array([self.x, self.y, self.z, 1])
-            return Vector(moved[0], moved[1], moved[2])
-        else:
-            raise TypeError("Can only multiply by scalar")
-
-    def __truediv__(self, other: float) -> Vector:
-        return Vector(self.x / other, self.y / other, self.z / other)
-
-    def norm(self):
-        return numpy.sqrt(self.x**2 + self.y**2 + self.z**2)
-
-    def unit(self):
-        return self / self.norm()
-
-
-class Transform:
-    def __init__(self, matrix: numpy.ndarray):
-        self.matrix = matrix
-
-    def __mul__(self, other: Union[numpy.ndrray, Transform]) -> Transform:
-        if isinstance(other, numpy.ndarray):
-            return Transform(self.matrix @ other)
-        elif isinstance(other, Transform):
-            return Transform(self.matrix @ other.matrix)
-        else:
-            raise TypeError("Can only multiply by numpy array or Transform")
-
-    @staticmethod
-    def identity() -> Transform:
-        return Transform(numpy.eye(4))
-
-    @staticmethod
-    def from_euler(order: str, angles: List[float]):
-        matrix = numpy.identity(4)
-        matrix[:3, :3] = Rotation.from_euler(order, angles).as_matrix()
-        return Transform(matrix)
-
-    @staticmethod
-    def rotate_around_axis(theta, axis_vector):
-        from math import cos, sin
-        u = axis_vector.unit()
-        m = [
-            [cos(theta) + u.x ** 2 * (1 - cos(theta)), u.x * u.y * (1 - cos(theta)) - u.z * sin(theta),
-             u.x * u.z * (1 - cos(theta)) + u.y * sin(theta), 0],
-            [u.y * u.x * (1 - cos(theta)) + u.z * sin(theta), cos(theta) + u.y ** 2 * (1 - cos(theta)),
-             u.y * u.z * (1 - cos(theta)) - u.x * sin(theta), 0],
-            [u.z * u.x * (1 - cos(theta)) - u.y * sin(theta), u.z * u.y * (1 - cos(theta)) + u.x * sin(theta),
-             cos(theta) + u.z ** 2 * (1 - cos(theta)), 0],
-            [0, 0, 0, 1]
-        ]
-        return Transform(numpy.array(m))
-
-    @staticmethod
-    def translate(*args):
-        if len(args) == 3:
-            return Transform._translate(*args)
-        elif len(args) == 1:
-            arg = args[0]
-            if hasattr(arg, "x") and hasattr(arg, "y") and hasattr(arg, "z"):
-                return Transform.translate(arg.x, arg.y, arg.z)
-        raise TypeError(f"Could not create translation from {args}")
-
-    @staticmethod
-    def _translate(x, y, z):
-        m = numpy.identity(4)
-        m[0, 3] = x
-        m[1, 3] = y
-        m[2, 3] = z
-        return Transform(m)
-
-    def invert(self) -> Transform:
-        return Transform(numpy.linalg.inv(self.matrix))
-

Diff for: python/lrmate/transforms.py

@@ -0,0 +1,146 @@
+from __future__ import annotations
+
+import math
+
+import numpy
+from dataclasses import dataclass
+from scipy.spatial.transform import Rotation
+from typing import List, Optional, Dict, Union
+
+
+@dataclass
+class Vector:
+    x: float
+    y: float
+    z: float
+
+    def __add__(self, other):
+        return Vector(self.x + other.x, self.y + other.y, self.z + other.z)
+
+    def __sub__(self, other):
+        return Vector(self.x - other.x, self.y - other.y, self.z - other.z)
+
+    def __mul__(self, other: Union[float, Transform]) -> Vector:
+        if isinstance(other, float):
+            return Vector(self.x * other, self.y * other, self.z * other)
+        elif isinstance(other, Transform):
+            moved = other * numpy.array([self.x, self.y, self.z, 1])
+            return Vector(moved[0], moved[1], moved[2])
+        else:
+            raise TypeError("Can only multiply by scalar")
+
+    def __truediv__(self, other: float) -> Vector:
+        return Vector(self.x / other, self.y / other, self.z / other)
+
+    def norm(self):
+        return numpy.sqrt(self.x ** 2 + self.y ** 2 + self.z ** 2)
+
+    def unit(self):
+        return self / self.norm()
+
+
+class Transform:
+    def __init__(self, matrix: numpy.ndarray):
+        self.matrix = matrix
+
+    def __mul__(self, other: Union[numpy.ndrray, Transform]) -> Transform:
+        if isinstance(other, numpy.ndarray):
+            return Transform(self.matrix @ other)
+        elif isinstance(other, Transform):
+            return Transform(self.matrix @ other.matrix)
+        else:
+            raise TypeError("Can only multiply by numpy array or Transform")
+
+    @staticmethod
+    def identity() -> Transform:
+        return Transform(numpy.eye(4))
+
+    @staticmethod
+    def from_euler(order: str, angles: List[float]):
+        matrix = numpy.identity(4)
+        matrix[:3, :3] = Rotation.from_euler(order, angles).as_matrix()
+        return Transform(matrix)
+
+    @staticmethod
+    def rotate_around_axis(theta, axis_vector):
+        from math import cos, sin
+        u = axis_vector.unit()
+        m = [
+            [cos(theta) + u.x ** 2 * (1 - cos(theta)), u.x * u.y * (1 - cos(theta)) - u.z * sin(theta),
+             u.x * u.z * (1 - cos(theta)) + u.y * sin(theta), 0],
+            [u.y * u.x * (1 - cos(theta)) + u.z * sin(theta), cos(theta) + u.y ** 2 * (1 - cos(theta)),
+             u.y * u.z * (1 - cos(theta)) - u.x * sin(theta), 0],
+            [u.z * u.x * (1 - cos(theta)) - u.y * sin(theta), u.z * u.y * (1 - cos(theta)) + u.x * sin(theta),
+             cos(theta) + u.z ** 2 * (1 - cos(theta)), 0],
+            [0, 0, 0, 1]
+        ]
+        return Transform(numpy.array(m))
+
+    @staticmethod
+    def translate(*args):
+        if len(args) == 3:
+            return Transform._translate(*args)
+        elif len(args) == 1:
+            arg = args[0]
+            if hasattr(arg, "x") and hasattr(arg, "y") and hasattr(arg, "z"):
+                return Transform.translate(arg.x, arg.y, arg.z)
+        raise TypeError(f"Could not create translation from {args}")
+
+    @staticmethod
+    def _translate(x, y, z):
+        m = numpy.identity(4)
+        m[0, 3] = x
+        m[1, 3] = y
+        m[2, 3] = z
+        return Transform(m)
+
+    def invert(self) -> Transform:
+        return Transform(numpy.linalg.inv(self.matrix))
+
+
+@dataclass
+class XyzWpr:
+    """ Represents a transform in Fanuc's XYZWPR format, where X, Y, and Z are in mm and W, P, and R are in degrees. """
+    x: float
+    y: float
+    z: float
+    w: float
+    p: float
+    r: float
+
+    def to_transform(self) -> Transform:
+        yaw = Transform.rotate_around_axis(numpy.radians(self.w), Vector(0, 0, 1))
+        pitch = Transform.rotate_around_axis(numpy.radians(self.p), Vector(0, 1, 0))
+        roll = Transform.rotate_around_axis(numpy.radians(self.r), Vector(1, 0, 0))
+        tilted = yaw * pitch * roll
+        return Transform.translate(self.x, self.y, self.z) * tilted
+
+    @staticmethod
+    def from_transform(transform: Transform) -> XyzWpr:
+        # Check that transform.matrix is a 4x4 matrix
+        if transform.matrix.shape != (4, 4):
+            raise ValueError(f"Expected a 4x4 matrix, got {transform.matrix.shape}")
+
+        x = transform.matrix[0, 3]
+        y = transform.matrix[1, 3]
+        z = transform.matrix[2, 3]
+
+        # Check that the rotation matrix is orthogonal
+        if transform.matrix[2, 0] > (1.0 - 1e-6):
+            p = -math.pi * 0.5
+            r = 0
+            w = math.atan2(-transform.matrix[0, 1], -transform.matrix[0, 2])
+        elif transform.matrix[2, 0] < (-1.0 + 1e-6):
+            p = math.pi * 0.5
+            r = 0
+            w = math.atan2(transform.matrix[1, 2], transform.matrix[1, 1])
+        else:
+            p = math.atan2(-transform.matrix[2, 0], math.sqrt(transform.matrix[0, 0] ** 2 + transform.matrix[1, 0] ** 2))
+            w = math.atan2(transform.matrix[1, 0], transform.matrix[0, 0])
+            r = math.atan2(transform.matrix[2, 1], transform.matrix[2, 2])
+
+        return XyzWpr(x, y, z, _from_radians(w), _from_radians(p), _from_radians(r))
+
+
+def _from_radians(radians: float) -> float:
+    return radians * 180 / math.pi

Diff for: python/requirements.txt

@@ -2,4 +2,5 @@ setuptools~=65.5.1
 numpy~=1.24.2
 open3d~=0.17.0
 scipy~=1.10.1
-urdf-parser-py~=0.0.4
+urdf-parser-py~=0.0.4
+pytest~=7.3.1

Diff for: python/tests/__init__.py

@@ -0,0 +1,29 @@
+_transforms_and_xyzwpr = [
+    ([[-0.355708, 0.717742, 0.598597, 250.689172], [-0.173709, -0.680099, 0.712243, 190.09217],
+      [0.918312, 0.149368, 0.366595, 377.021336], [0.0, 0.0, 0.0, 1.0]],
+     [250.689172, 190.09217, 377.021336, 22.168324, -66.680543, -153.971487]),
+    ([[0.875453, 0.335633, 0.347755, 324.515443], [0.474534, -0.733383, -0.486792, -269.225543],
+      [0.091654, 0.591185, -0.801312, 109.845856], [0.0, 0.0, 0.0, 1.0]],
+     [324.515443, -269.225543, 109.845856, 143.581155, -5.258767, 28.459638]),
+    ([[0.829857, -0.290899, 0.476147, 480.998803], [-0.3928, -0.910637, 0.128247, -24.848409],
+      [0.39629, -0.293458, -0.869964, 44.45118], [0.0, 0.0, 0.0, 1.0]],
+     [480.998803, -24.848409, 44.45118, -161.359637, -23.346471, -25.329844]),
+    ([[0.622543, 0.659905, 0.420672, 427.425001], [0.286126, -0.692256, 0.662506, 317.808911],
+      [0.728404, -0.292073, -0.619775, 185.181686], [0.0, 0.0, 0.0, 1.0]],
+     [427.425001, 317.808911, 185.181686, -154.76754, -46.752739, 24.683898]),
+    ([[0.121273, 0.016312, 0.992485, 640.024599], [0.027579, -0.999534, 0.013058, 54.109546],
+      [0.992236, 0.025788, -0.121667, 303.808089], [0.0, 0.0, 0.0, 1.0]],
+     [640.024599, 54.109546, 303.808089, 168.032738, -82.855635, 12.811951]),
+    ([[-0.155532, -0.422104, 0.893106, 416.744163], [0.025165, -0.905508, -0.423583, -205.770414],
+      [0.98751, -0.043406, 0.151457, 328.606008], [0.0, 0.0, 0.0, 1.0]],
+     [416.744163, -205.770414, 328.606008, -15.991663, -80.93504, 170.809286]),
+    ([[-0.027822, -0.180557, 0.983171, 431.202555], [-0.281693, -0.942275, -0.181018, -21.951723],
+      [0.959101, -0.281989, -0.024646, 228.94672], [0.0, 0.0, 0.0, 1.0]],
+     [431.202555, -21.951723, 228.94672, -94.99493, -73.556872, -95.640661]),
+    ([[-0.7791, -0.238741, 0.579661, 243.109205], [0.547427, -0.709679, 0.443485, 126.817247],
+      [0.305495, 0.662841, 0.683604, 442.214652], [0.0, 0.0, 0.0, 1.0]],
+     [243.109205, 126.817247, 442.214652, 44.116508, -17.787954, 144.906558]),
+    ([[-0.338094, -0.333176, 0.880162, 342.415569], [0.15182, -0.942296, -0.298378, -7.40816],
+      [0.928786, 0.032746, 0.369167, 346.902494], [0.0, 0.0, 0.0, 1.0]],
+     [342.415569, -7.40816, 346.902494, 5.069014, -68.246332, 155.817742]),
+]

Diff for: python/tests/_test_data.py

@@ -0,0 +1,25 @@
+import pytest
+import numpy
+from lrmate.transforms import Transform, XyzWpr, Vector
+from ._test_data import _transforms_and_xyzwpr
+
+
+@pytest.mark.parametrize('matrix,xyzrpw', _transforms_and_xyzwpr)
+def test_xyzwpr_to_transform(matrix, xyzrpw):
+    transform = Transform(numpy.matrix(matrix))
+    frame = XyzWpr(*xyzrpw[:3], xyzrpw[5], xyzrpw[4], xyzrpw[3])
+    converted = frame.to_transform()
+    assert numpy.allclose(converted.matrix, transform.matrix, rtol=1e-4, atol=1e-5)
+
+
+@pytest.mark.parametrize('matrix,xyzrpw', _transforms_and_xyzwpr)
+def test_transform_to_xyzwpr(matrix, xyzrpw):
+    transform = Transform(numpy.matrix(matrix))
+    frame = XyzWpr(*xyzrpw[:3], xyzrpw[5], xyzrpw[4], xyzrpw[3])
+    converted = XyzWpr.from_transform(transform)
+    assert frame.x == pytest.approx(converted.x, abs=1e-4)
+    assert frame.y == pytest.approx(converted.y, abs=1e-4)
+    assert frame.z == pytest.approx(converted.z, abs=1e-4)
+    assert frame.w == pytest.approx(converted.w, abs=1e-3)
+    assert frame.p == pytest.approx(converted.p, abs=1e-3)
+    assert frame.r == pytest.approx(converted.r, abs=1e-3)