urkit 0.3.21__tar.gz → 0.3.22__tar.gz

{urkit-0.3.21 → urkit-0.3.22}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: urkit
-Version: 0.3.21
+Version: 0.3.22
 Summary: Universal Robots e-Series control toolkit built on ur_rtde
 Author: URKit Contributors
 License: MIT

{urkit-0.3.21 → urkit-0.3.22}/pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "urkit"
-version = "0.3.21"
+version = "0.3.22"
 description = "Universal Robots e-Series control toolkit built on ur_rtde"
 readme = "README.md"
 license = {text = "MIT"}

{urkit-0.3.21 → urkit-0.3.22}/src/urkit/__init__.py

@@ -24,7 +24,7 @@ Quick start::
 
 from __future__ import annotations
 
-__version__ = "0.3.21"
+__version__ = "0.3.22"
 
 from urkit.config import load_config, resolve_config
 from urkit.exceptions import (

{urkit-0.3.21 → urkit-0.3.22}/src/urkit/geometry.py

@@ -90,13 +90,13 @@ def rpy_to_quat(
 def orient_tcp(
     pose: list[float], direction: list[float]
 ) -> list[float]:
-    """Orient TCP along a target direction while preserving tool heading.
+    """Orient TCP along a target direction via minimal relative rotation.
 
-    Points the tool Z-axis along *direction* (base frame) while
-    preserving the tool's heading (X-axis direction projected onto
-    the plane perpendicular to the target). This avoids the ambiguity
-    of RPY yaw at extreme angles and produces a smooth, predictable
-    orientation change.
+    Computes the smallest rotation that maps the current tool Z-axis
+    onto *direction* (in the base frame), then applies that rotation
+    to the current pose. This is a relative rotation from where you
+    are — no absolute frame reconstruction, no heading-preservation
+    heuristics that cause unexpected large swings.
 
     Args:
         pose: [x, y, z, rx, ry, rz] rotation vector pose.
@@ -113,69 +113,66 @@ def orient_tcp(
     norm_d = math.sqrt(direction[0] ** 2 + direction[1] ** 2 + direction[2] ** 2)
     if norm_d < 1e-10:
         raise ValueError(f"Direction vector is zero: {direction}")
-    z_new = [d / norm_d for d in direction]
-
-    # Current rotation matrix (columns are X, Y, Z axes)
-    R = _rotvec_to_matrix(rv)
-
-    # Current X and Y axes
-    x_curr = [R[0][0], R[1][0], R[2][0]]
-    y_curr = [R[0][1], R[1][1], R[2][1]]
-
-    # Preserve heading: project current X axis onto plane perpendicular to z_new
-    dot_xz = x_curr[0] * z_new[0] + x_curr[1] * z_new[1] + x_curr[2] * z_new[2]
-    x_new = [x_curr[i] - dot_xz * z_new[i] for i in range(3)]
-    norm_x = math.sqrt(x_new[0] ** 2 + x_new[1] ** 2 + x_new[2] ** 2)
-
-    if norm_x < 1e-6:
-        # X is nearly parallel to target — use Y projection instead
-        dot_yz = y_curr[0] * z_new[0] + y_curr[1] * z_new[1] + y_curr[2] * z_new[2]
-        y_new = [y_curr[i] - dot_yz * z_new[i] for i in range(3)]
-        norm_y = math.sqrt(y_new[0] ** 2 + y_new[1] ** 2 + y_new[2] ** 2)
-
-        if norm_y < 1e-6:
-            # Both parallel — fallback: pick arbitrary orthogonal axis
-            # Use cross product with smallest component of z_new
-            if abs(z_new[0]) <= abs(z_new[1]) and abs(z_new[0]) <= abs(z_new[2]):
-                ref = [1.0, 0.0, 0.0]
-            elif abs(z_new[1]) <= abs(z_new[2]):
-                ref = [0.0, 1.0, 0.0]
-            else:
-                ref = [0.0, 0.0, 1.0]
-            y_new = [
-                z_new[1] * ref[2] - z_new[2] * ref[1],
-                z_new[2] * ref[0] - z_new[0] * ref[2],
-                z_new[0] * ref[1] - z_new[1] * ref[0],
-            ]
-            norm_y = math.sqrt(y_new[0] ** 2 + y_new[1] ** 2 + y_new[2] ** 2)
-
-        y_new[0] /= norm_y
-        y_new[1] /= norm_y
-        y_new[2] /= norm_y
-        # x = y cross z
-        x_new = [
-            y_new[1] * z_new[2] - y_new[2] * z_new[1],
-            y_new[2] * z_new[0] - y_new[0] * z_new[2],
-            y_new[0] * z_new[1] - y_new[1] * z_new[0],
+    z_target = [d / norm_d for d in direction]
+
+    # Current rotation matrix (columns are X, Y, Z axes in base frame)
+    R_curr = _rotvec_to_matrix(rv)
+
+    # Current tool Z-axis in base frame (3rd column of R_curr)
+    z_curr = [R_curr[i][2] for i in range(3)]
+
+    # Dot product to determine alignment
+    dot_cos = z_curr[0] * z_target[0] + z_curr[1] * z_target[1] + z_curr[2] * z_target[2]
+
+    # Already aligned within epsilon
+    if dot_cos > 1.0 - 1e-10:
+        return [pos[0], pos[1], pos[2], rv[0], rv[1], rv[2]]
+
+    # Compute rotation axis and angle
+    if dot_cos < -1.0 + 1e-10:
+        # 180° case — z_curr is opposite z_target, cross product is zero
+        # Pick a reference vector perpendicular to z_curr
+        if abs(z_curr[0]) <= abs(z_curr[1]) and abs(z_curr[0]) <= abs(z_curr[2]):
+            ref = [1.0, 0.0, 0.0]
+        elif abs(z_curr[1]) <= abs(z_curr[2]):
+            ref = [0.0, 1.0, 0.0]
+        else:
+            ref = [0.0, 0.0, 1.0]
+        axis = [
+            z_curr[1] * ref[2] - z_curr[2] * ref[1],
+            z_curr[2] * ref[0] - z_curr[0] * ref[2],
+            z_curr[0] * ref[1] - z_curr[1] * ref[0],
         ]
+        norm_axis = math.sqrt(axis[0] ** 2 + axis[1] ** 2 + axis[2] ** 2)
+        if norm_axis < 1e-10:
+            # Should not happen, but guard against it
+            axis, norm_axis = [1.0, 0.0, 0.0], 1.0
+        ax, ay, az = axis[0] / norm_axis, axis[1] / norm_axis, axis[2] / norm_axis
+        angle = math.pi
     else:
-        x_new[0] /= norm_x
-        x_new[1] /= norm_x
-        x_new[2] /= norm_x
-        # y = z cross x
-        y_new = [
-            z_new[1] * x_new[2] - z_new[2] * x_new[1],
-            z_new[2] * x_new[0] - z_new[0] * x_new[2],
-            z_new[0] * x_new[1] - z_new[1] * x_new[0],
+        # General case: rotation axis = z_curr × z_target
+        cross = [
+            z_curr[1] * z_target[2] - z_curr[2] * z_target[1],
+            z_curr[2] * z_target[0] - z_curr[0] * z_target[2],
+            z_curr[0] * z_target[1] - z_curr[1] * z_target[0],
         ]
+        norm_cross = math.sqrt(cross[0] ** 2 + cross[1] ** 2 + cross[2] ** 2)
+        ax, ay, az = cross[0] / norm_cross, cross[1] / norm_cross, cross[2] / norm_cross
+        angle = math.acos(max(-1.0, min(1.0, dot_cos)))
 
-    # Build rotation matrix from columns [x_new, y_new, z_new]
-    R_new = [
-        [x_new[0], y_new[0], z_new[0]],
-        [x_new[1], y_new[1], z_new[1]],
-        [x_new[2], y_new[2], z_new[2]],
+    # Build delta rotation matrix from axis-angle (Rodrigues)
+    s = math.sin(angle)
+    c = math.cos(angle)
+    oc = 1.0 - c
+    R_delta = [
+        [c + ax*ax*oc, ax*ay*oc - az*s, ax*az*oc + ay*s],
+        [ay*ax*oc + az*s, c + ay*ay*oc, ay*az*oc - ax*s],
+        [az*ax*oc - ay*s, az*ay*oc + ax*s, c + az*az*oc],
     ]
 
+    # Apply delta: R_new = R_delta @ R_curr (rotate current orientation by delta)
+    R_new = _mat_mul(R_delta, R_curr)
+
     rv_target = _matrix_to_rotvec(R_new)
 
     return [pos[0], pos[1], pos[2], rv_target[0], rv_target[1], rv_target[2]]

{urkit-0.3.21 → urkit-0.3.22}/src/urkit.egg-info/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: urkit
-Version: 0.3.21
+Version: 0.3.22
 Summary: Universal Robots e-Series control toolkit built on ur_rtde
 Author: URKit Contributors
 License: MIT

urkit-0.3.22/tests/test_geometry.py

@@ -0,0 +1,286 @@
+"""Tests for quaternion/rotation vector geometry helpers."""
+
+from __future__ import annotations
+
+import math
+
+import pytest
+
+from urkit.geometry import (
+    orient_tcp,
+    orient_tcp_down,
+    quat_to_rotvec,
+    quat_to_rpy,
+    rpy_to_quat,
+    rotvec_to_quat,
+    _rotvec_to_matrix,
+)
+
+
+class TestRotvecToQuat:
+    """Rotation vector to quaternion conversion."""
+
+    def test_identity(self):
+        q = rotvec_to_quat([0, 0, 0])
+        assert abs(q[3] - 1.0) < 1e-10  # w = 1
+        assert abs(q[0]) < 1e-10
+        assert abs(q[1]) < 1e-10
+        assert abs(q[2]) < 1e-10
+
+    def test_180_degrees_x_axis(self):
+        """180° rotation around X axis."""
+        q = rotvec_to_quat([math.pi, 0, 0])
+        assert abs(q[0] - 1.0) < 1e-10  # x = 1
+        assert abs(q[1]) < 1e-10
+        assert abs(q[2]) < 1e-10
+        assert abs(q[3]) < 1e-10  # w = 0
+
+    def test_90_degrees_z_axis(self):
+        """90° rotation around Z axis."""
+        q = rotvec_to_quat([0, 0, math.pi / 2])
+        assert abs(q[0]) < 1e-10
+        assert abs(q[1]) < 1e-10
+        assert abs(q[2] - math.sin(math.pi / 4)) < 1e-10
+        assert abs(q[3] - math.cos(math.pi / 4)) < 1e-10
+
+    def test_roundtrip(self):
+        """rotvec -> quat -> rotvec should be identity."""
+        rv = [0.5, 0.3, 0.1]
+        q = rotvec_to_quat(rv)
+        back = quat_to_rotvec(q)
+        for a, b in zip(rv, back):
+            assert abs(a - b) < 1e-10
+
+
+class TestQuatToRotvec:
+    """Quaternion to rotation vector conversion."""
+
+    def test_identity_quat(self):
+        rv = quat_to_rotvec((0, 0, 0, 1))
+        assert all(abs(v) < 1e-10 for v in rv)
+
+    def test_roundtrip(self):
+        """quat -> rotvec -> quat should be identity."""
+        q = (0.7071, 0, 0, 0.7071)  # 90° around X
+        rv = quat_to_rotvec(q)
+        back = rotvec_to_quat(rv)
+        for a, b in zip(q, back):
+            assert abs(a - b) < 1e-5
+
+
+class TestQuatToRpy:
+    """Quaternion to RPY extraction."""
+
+    def test_identity(self):
+        roll, pitch, yaw = quat_to_rpy((0, 0, 0, 1))
+        assert abs(roll) < 1e-10
+        assert abs(pitch) < 1e-10
+        assert abs(yaw) < 1e-10
+
+    def test_90_deg_roll(self):
+        """90° roll should give roll=pi/2."""
+        q = rotvec_to_quat([math.pi / 2, 0, 0])
+        roll, pitch, yaw = quat_to_rpy(q)
+        assert abs(roll - math.pi / 2) < 1e-10
+        assert abs(pitch) < 1e-10
+        assert abs(yaw) < 1e-10
+
+    def test_roundtrip(self):
+        """rpy -> quat -> rpy should be identity."""
+        r, p, y = 0.5, 0.3, 0.1
+        q = rpy_to_quat(r, p, y)
+        back_r, back_p, back_y = quat_to_rpy(q)
+        assert abs(r - back_r) < 1e-10
+        assert abs(p - back_p) < 1e-10
+        assert abs(y - back_y) < 1e-10
+
+
+class TestRpyToQuat:
+    """RPY to quaternion conversion."""
+
+    def test_zero_rpy(self):
+        q = rpy_to_quat(0, 0, 0)
+        assert abs(q[0]) < 1e-10
+        assert abs(q[1]) < 1e-10
+        assert abs(q[2]) < 1e-10
+        assert abs(q[3] - 1.0) < 1e-10
+
+    def test_180_roll(self):
+        """180° roll -> known quaternion."""
+        q = rpy_to_quat(math.pi, 0, 0)
+        assert abs(q[0] - 1.0) < 1e-10
+        assert abs(q[1]) < 1e-10
+        assert abs(q[2]) < 1e-10
+        assert abs(q[3]) < 1e-10
+
+
+class TestOrientTcpDown:
+    """TCP downward orientation."""
+
+    def test_preserves_position(self):
+        pose = [0.5, 0.3, 0.2, 0, 0, 0]
+        result = orient_tcp_down(pose)
+        assert result[0] == 0.5
+        assert result[1] == 0.3
+        assert result[2] == 0.2
+
+    def test_z_axis_points_down(self):
+        """Resulting tool Z-axis should point straight down in base frame."""
+        pose = [0.5, 0.3, 0.2, 0, 0, 0]
+        result = orient_tcp_down(pose)
+        rv = result[3:]
+        # For roll=π, Z-axis = (0, 0, -1)
+        angle = math.sqrt(sum(v**2 for v in rv))
+        if angle > 1e-10:
+            az = rv[2] / angle
+            c = math.cos(angle)
+            oc = 1.0 - c
+            # R[2][2] = c + az*az*oc — tool Z in base frame
+            z_z = c + az * az * oc
+            assert abs(z_z - (-1.0)) < 1e-10
+
+    def test_minimal_rotation_angle(self):
+        """Orient from yaw=0.5 to down should produce minimal rotation."""
+        q = rpy_to_quat(0, 0, 0.5)
+        rv = quat_to_rotvec(q)
+        pose = [0.5, 0.3, 0.2, rv[0], rv[1], rv[2]]
+        result = orient_tcp_down(pose)
+
+        # The minimal rotation from Z=[0,0,1] to Z=[0,0,-1] is 180°
+        result_angle = math.sqrt(sum(v**2 for v in result[3:]))
+        assert abs(result_angle - math.pi) < 1e-10
+
+    def test_handles_gimbal_lock(self):
+        """Pitch=90° (gimbal lock) should still produce valid down orientation."""
+        q = rpy_to_quat(0, math.pi / 2, 0.5)
+        rv = quat_to_rotvec(q)
+        pose = [0.5, 0.3, 0.2, rv[0], rv[1], rv[2]]
+        result = orient_tcp_down(pose)
+        # Should not crash, Z should point down
+        assert len(result) == 6
+        assert result[:3] == pose[:3]  # position preserved
+
+
+class TestFullRoundtrip:
+    """Full chain: rotvec -> quat -> rpy -> quat -> rotvec."""
+
+    def test_various_angles(self):
+        test_vectors = [
+            [0, 0, 0],
+            [math.pi / 6, 0, 0],
+            [0, math.pi / 4, 0],
+            [0, 0, math.pi / 3],
+            [0.5, 0.3, 0.2],
+            [1.0, 0.5, 0.3],
+        ]
+        for rv in test_vectors:
+            q = rotvec_to_quat(rv)
+            r, p, y = quat_to_rpy(q)
+            q2 = rpy_to_quat(r, p, y)
+            rv2 = quat_to_rotvec(q2)
+            for a, b in zip(rv, rv2):
+                assert abs(a - b) < 1e-10, f"Failed for {rv}"
+
+
+class TestOrientTcp:
+    """TCP orientation in arbitrary directions (minimal relative rotation)."""
+
+    def test_preserves_position(self):
+        pose = [0.5, 0.3, 0.2, 0, 0, 0]
+        result = orient_tcp(pose, [1, 0, 0])
+        assert result[0] == 0.5
+        assert result[1] == 0.3
+        assert result[2] == 0.2
+
+    def test_z_points_along_target(self):
+        """Tool Z-axis should point along the target direction."""
+        pose = [0.5, 0.3, 0.2, 0, 0, 0]
+        result = orient_tcp(pose, [1, 0, 0])
+        R = _rotvec_to_matrix(result[3:])
+        z = [R[i][2] for i in range(3)]
+        assert abs(z[0] - 1.0) < 1e-10
+        assert abs(z[1]) < 1e-10
+        assert abs(z[2]) < 1e-10
+
+    def test_z_points_along_neg_y(self):
+        pose = [0.5, 0.3, 0.2, 0, 0, 0]
+        result = orient_tcp(pose, [0, -1, 0])
+        R = _rotvec_to_matrix(result[3:])
+        z = [R[i][2] for i in range(3)]
+        assert abs(z[0]) < 1e-10
+        assert abs(z[1] - (-1.0)) < 1e-10
+        assert abs(z[2]) < 1e-10
+
+    def test_minimal_rotation_non_180(self):
+        """Z up (yaw=30°) → +X: delta should be 90°, not 120°."""
+        q = rpy_to_quat(0, 0, math.radians(30))
+        rv = quat_to_rotvec(q)
+        pose = [0.5, 0.3, 0.2, rv[0], rv[1], rv[2]]
+        result = orient_tcp(pose, [1, 0, 0])
+        # Compute delta rotation: R_delta = R_new @ R_curr^T
+        R_curr = _rotvec_to_matrix(pose[3:])
+        R_new = _rotvec_to_matrix(result[3:])
+        # Transpose of R_curr (since it's orthogonal, inverse = transpose)
+        R_curr_T = [[R_curr[0][0], R_curr[1][0], R_curr[2][0]],
+                    [R_curr[0][1], R_curr[1][1], R_curr[2][1]],
+                    [R_curr[0][2], R_curr[1][2], R_curr[2][2]]]
+        R_delta = [
+            [sum(R_new[i][k] * R_curr_T[k][j] for k in range(3)) for j in range(3)]
+            for i in range(3)
+        ]
+        trace = R_delta[0][0] + R_delta[1][1] + R_delta[2][2]
+        delta_angle = math.acos(max(-1, min(1, (trace - 1) / 2)))
+        # Minimal rotation from [0,0,1] to [1,0,0] is 90°
+        assert abs(delta_angle - math.pi / 2) < 1e-10, f"delta_angle={delta_angle}"
+
+    def test_already_aligned_returns_unchanged(self):
+        """If Z already points along target, pose should be unchanged."""
+        pose = [0.5, 0.3, 0.2, 0, 0, 0]
+        result = orient_tcp(pose, [0, 0, 1])
+        assert result[3:] == pose[3:]
+
+    def test_180_degree_flip(self):
+        """Z up → Z down should give a valid 180° rotation."""
+        pose = [0.5, 0.3, 0.2, 0, 0, 0]
+        result = orient_tcp(pose, [0, 0, -1])
+        angle = math.sqrt(sum(v**2 for v in result[3:]))
+        assert abs(angle - math.pi) < 1e-10
+        R = _rotvec_to_matrix(result[3:])
+        z = [R[i][2] for i in range(3)]
+        assert abs(z[0]) < 1e-10
+        assert abs(z[1]) < 1e-10
+        assert abs(z[2] - (-1.0)) < 1e-10
+
+    def test_raises_on_zero_direction(self):
+        pose = [0.5, 0.3, 0.2, 0, 0, 0]
+        with pytest.raises(ValueError):
+            orient_tcp(pose, [0, 0, 0])
+
+    def test_various_starting_poses(self):
+        """Multiple starting orientations should all produce valid results."""
+        targets = [[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1], [0, 0, -1]]
+        for roll in [0, math.radians(45), math.radians(90)]:
+            for pitch in [0, math.radians(-30), math.radians(60)]:
+                for yaw in [0, math.radians(30)]:
+                    q = rpy_to_quat(roll, pitch, yaw)
+                    rv = quat_to_rotvec(q)
+                    pose = [0.5, 0.3, 0.2, rv[0], rv[1], rv[2]]
+                    for target in targets:
+                        result = orient_tcp(pose, target)
+                        # Verify Z points along target
+                        R = _rotvec_to_matrix(result[3:])
+                        z = [R[i][2] for i in range(3)]
+                        norm_t = math.sqrt(target[0]**2 + target[1]**2 + target[2]**2)
+                        t = [target[i]/norm_t for i in range(3)]
+                        dot = z[0]*t[0] + z[1]*t[1] + z[2]*t[2]
+                        assert abs(dot - 1.0) < 1e-10, (
+                            f"Z not aligned: roll={roll}, pitch={pitch}, "
+                            f"yaw={yaw}, target={target}"
+                        )
+                        # Verify rotation matrix is proper (det = 1)
+                        det = (
+                            R[0][0]*(R[1][1]*R[2][2] - R[1][2]*R[2][1])
+                            - R[0][1]*(R[1][0]*R[2][2] - R[1][2]*R[2][0])
+                            + R[0][2]*(R[1][0]*R[2][1] - R[1][1]*R[2][0])
+                        )
+                        assert abs(det - 1.0) < 1e-10

urkit-0.3.21/tests/test_geometry.py

@@ -1,187 +0,0 @@
-"""Tests for quaternion/rotation vector geometry helpers."""
-
-from __future__ import annotations
-
-import math
-
-from urkit.geometry import (
-    orient_tcp_down,
-    quat_to_rotvec,
-    quat_to_rpy,
-    rpy_to_quat,
-    rotvec_to_quat,
-)
-
-
-class TestRotvecToQuat:
-    """Rotation vector to quaternion conversion."""
-
-    def test_identity(self):
-        q = rotvec_to_quat([0, 0, 0])
-        assert abs(q[3] - 1.0) < 1e-10  # w = 1
-        assert abs(q[0]) < 1e-10
-        assert abs(q[1]) < 1e-10
-        assert abs(q[2]) < 1e-10
-
-    def test_180_degrees_x_axis(self):
-        """180° rotation around X axis."""
-        q = rotvec_to_quat([math.pi, 0, 0])
-        assert abs(q[0] - 1.0) < 1e-10  # x = 1
-        assert abs(q[1]) < 1e-10
-        assert abs(q[2]) < 1e-10
-        assert abs(q[3]) < 1e-10  # w = 0
-
-    def test_90_degrees_z_axis(self):
-        """90° rotation around Z axis."""
-        q = rotvec_to_quat([0, 0, math.pi / 2])
-        assert abs(q[0]) < 1e-10
-        assert abs(q[1]) < 1e-10
-        assert abs(q[2] - math.sin(math.pi / 4)) < 1e-10
-        assert abs(q[3] - math.cos(math.pi / 4)) < 1e-10
-
-    def test_roundtrip(self):
-        """rotvec -> quat -> rotvec should be identity."""
-        rv = [0.5, 0.3, 0.1]
-        q = rotvec_to_quat(rv)
-        back = quat_to_rotvec(q)
-        for a, b in zip(rv, back):
-            assert abs(a - b) < 1e-10
-
-
-class TestQuatToRotvec:
-    """Quaternion to rotation vector conversion."""
-
-    def test_identity_quat(self):
-        rv = quat_to_rotvec((0, 0, 0, 1))
-        assert all(abs(v) < 1e-10 for v in rv)
-
-    def test_roundtrip(self):
-        """quat -> rotvec -> quat should be identity."""
-        q = (0.7071, 0, 0, 0.7071)  # 90° around X
-        rv = quat_to_rotvec(q)
-        back = rotvec_to_quat(rv)
-        for a, b in zip(q, back):
-            assert abs(a - b) < 1e-5
-
-
-class TestQuatToRpy:
-    """Quaternion to RPY extraction."""
-
-    def test_identity(self):
-        roll, pitch, yaw = quat_to_rpy((0, 0, 0, 1))
-        assert abs(roll) < 1e-10
-        assert abs(pitch) < 1e-10
-        assert abs(yaw) < 1e-10
-
-    def test_90_deg_roll(self):
-        """90° roll should give roll=pi/2."""
-        q = rotvec_to_quat([math.pi / 2, 0, 0])
-        roll, pitch, yaw = quat_to_rpy(q)
-        assert abs(roll - math.pi / 2) < 1e-10
-        assert abs(pitch) < 1e-10
-        assert abs(yaw) < 1e-10
-
-    def test_roundtrip(self):
-        """rpy -> quat -> rpy should be identity."""
-        r, p, y = 0.5, 0.3, 0.1
-        q = rpy_to_quat(r, p, y)
-        back_r, back_p, back_y = quat_to_rpy(q)
-        assert abs(r - back_r) < 1e-10
-        assert abs(p - back_p) < 1e-10
-        assert abs(y - back_y) < 1e-10
-
-
-class TestRpyToQuat:
-    """RPY to quaternion conversion."""
-
-    def test_zero_rpy(self):
-        q = rpy_to_quat(0, 0, 0)
-        assert abs(q[0]) < 1e-10
-        assert abs(q[1]) < 1e-10
-        assert abs(q[2]) < 1e-10
-        assert abs(q[3] - 1.0) < 1e-10
-
-    def test_180_roll(self):
-        """180° roll -> known quaternion."""
-        q = rpy_to_quat(math.pi, 0, 0)
-        assert abs(q[0] - 1.0) < 1e-10
-        assert abs(q[1]) < 1e-10
-        assert abs(q[2]) < 1e-10
-        assert abs(q[3]) < 1e-10
-
-
-class TestOrientTcpDown:
-    """TCP downward orientation."""
-
-    def test_preserves_position(self):
-        pose = [0.5, 0.3, 0.2, 0, 0, 0]
-        result = orient_tcp_down(pose)
-        assert result[0] == 0.5
-        assert result[1] == 0.3
-        assert result[2] == 0.2
-
-    def test_z_axis_points_down(self):
-        """Resulting tool Z-axis should point straight down in base frame."""
-        pose = [0.5, 0.3, 0.2, 0, 0, 0]
-        result = orient_tcp_down(pose)
-        rv = result[3:]
-        # For roll=π, Z-axis = (0, 0, -1)
-        angle = math.sqrt(sum(v**2 for v in rv))
-        if angle > 1e-10:
-            ax, ay, az = rv[0]/angle, rv[1]/angle, rv[2]/angle
-            s, c = math.sin(angle), math.cos(angle)
-            oc = 1 - c
-            # R[2][2] = c + az*az*oc — tool Z in base frame
-            z_z = c + az*az*oc
-            assert abs(z_z - (-1.0)) < 1e-10
-
-    def test_preserves_heading(self):
-        """Tool X-axis direction in XY plane should be preserved."""
-        # Start with yaw=0.5 (tool facing ~45° from X)
-        q = rpy_to_quat(0, 0, 0.5)
-        rv = quat_to_rotvec(q)
-        pose = [0.5, 0.3, 0.2, rv[0], rv[1], rv[2]]
-        result = orient_tcp_down(pose)
-
-        # Original X axis angle in XY plane
-        orig_q = rotvec_to_quat(pose[3:])
-        orig_rpy = quat_to_rpy(orig_q)
-        orig_heading = orig_rpy[2]  # yaw = heading in XY plane
-
-        # Result X axis angle in XY plane
-        result_q = rotvec_to_quat(result[3:])
-        result_rpy = quat_to_rpy(result_q)
-        result_heading = result_rpy[2]
-
-        assert abs(orig_heading - result_heading) < 1e-10
-
-    def test_handles_gimbal_lock(self):
-        """Pitch=90° (gimbal lock) should still produce valid down orientation."""
-        q = rpy_to_quat(0, math.pi / 2, 0.5)
-        rv = quat_to_rotvec(q)
-        pose = [0.5, 0.3, 0.2, rv[0], rv[1], rv[2]]
-        result = orient_tcp_down(pose)
-        # Should not crash, Z should point down
-        assert len(result) == 6
-        assert result[:3] == pose[:3]  # position preserved
-
-
-class TestFullRoundtrip:
-    """Full chain: rotvec -> quat -> rpy -> quat -> rotvec."""
-
-    def test_various_angles(self):
-        test_vectors = [
-            [0, 0, 0],
-            [math.pi / 6, 0, 0],
-            [0, math.pi / 4, 0],
-            [0, 0, math.pi / 3],
-            [0.5, 0.3, 0.2],
-            [1.0, 0.5, 0.3],
-        ]
-        for rv in test_vectors:
-            q = rotvec_to_quat(rv)
-            r, p, y = quat_to_rpy(q)
-            q2 = rpy_to_quat(r, p, y)
-            rv2 = quat_to_rotvec(q2)
-            for a, b in zip(rv, rv2):
-                assert abs(a - b) < 1e-10, f"Failed for {rv}"