urkit 0.3.20__tar.gz → 0.3.22__tar.gz

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

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

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

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

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

@@ -24,7 +24,7 @@ Quick start::
 
 from __future__ import annotations
 
-__version__ = "0.3.20"
+__version__ = "0.3.22"
 
 from urkit.config import load_config, resolve_config
 from urkit.exceptions import (
@@ -41,6 +41,7 @@ from urkit.exceptions import (
 )
 from urkit.geometry import (
     MoveFrame,
+    orient_tcp,
     orient_tcp_down,
     quat_to_rotvec,
     quat_to_rpy,
@@ -80,6 +81,7 @@ __all__ = [
     # Move frame
     "MoveFrame",
     # Geometry
+    "orient_tcp",
     "orient_tcp_down",
     "quat_to_rotvec",
     "quat_to_rpy",

{urkit-0.3.20 → urkit-0.3.22}/src/urkit/__main__.py

@@ -85,7 +85,7 @@ def main() -> None:
         "-e", "--expert",
         action="store_true",
         default=False,
-        help="Disable safety speed clamping (full speed for goto/tcp-down)",
+        help="Disable safety speed clamping (full speed for goto/tcp-orient)",
     )
     teach_parser.add_argument(
         "-v", "--verbose",

{urkit-0.3.20 → urkit-0.3.22}/src/urkit/cli/teach.py

@@ -40,7 +40,7 @@ from urkit.exceptions import (
     URKitConnectionError,
     URKitConnectionError as ConnectionError,
 )
-from urkit.geometry import MoveFrame, orient_tcp_down, transform_pose_delta
+from urkit.geometry import MoveFrame, orient_tcp, transform_pose_delta
 from urkit.gripper.presets import DigitalGripperConfig, PRESETS
 from urkit.motion import FreedriveMode
 from urkit.robot import URRobot
@@ -357,6 +357,13 @@ def _draw_screen(
     except Exception:
         pass
 
+    # IK reference
+    ik_ref = state.get("ik_reference")
+    if ik_ref is not None:
+        lines.append(f" {blue('IK Ref:'.ljust(lw))} {green(ik_ref if isinstance(ik_ref, str) else 'custom')}")
+    else:
+        lines.append(f" {blue('IK Ref:'.ljust(lw))} {dim('None')}")
+
     if state["freedrive"]:
         mode_label = state["freedrive_mode"].name
         if mode_label == "XYZ":
@@ -397,7 +404,7 @@ def _draw_screen(
     lines.append(f"  {yellow('STEP:')}    {yellow('1')}: Linear (mm)  {yellow('2')}: Angular (°)  {yellow('.')}: Reset")
     lines.append(f"  {yellow('GRIPPER:')} {yellow('X')}: Open  {yellow('C')}: Close  {yellow('V')}: Position  {yellow('6')}: Speed  {yellow('7')}: Force")
     lines.append(f"  {yellow('POINTS:')}  {yellow('B')}: Save  {yellow('G')}: Go To  {yellow('H')}: Delete  {yellow('R')}: Rename  {yellow('P')}: Explorer")
-    lines.append(f"  {yellow('OTHER:')}   {yellow('F')}: Freedrive  {yellow('M')}: Frame  {yellow('N')}: GoTo Mode  {yellow('T')}: TCP Down")
+    lines.append(f"  {yellow('OTHER:')}   {yellow('F')}: Freedrive  {yellow('M')}: Frame  {yellow('N')}: GoTo Mode  {yellow('T')}: TCP Orient")
     lines.append(f"  {yellow('      ')}   {yellow('0')}: Speed  {yellow('Y')}: Save Config")
     lines.append(f"  {yellow('EXIT:')}    {yellow('ESC')}")
     lines.append(dim("=" * width))
@@ -445,7 +452,7 @@ def _draw_help() -> None:
     lines.append("  OTHER:")
     lines.append("    F      → Cycle freedrive: OFF → ALL → XYZ+Rz → OFF")
     lines.append("    M      → Toggle move frame: BASE / TOOL")
-    lines.append("    T      → Orient TCP downward (roll=180°)")
+    lines.append("    T      → Open TCP orient submenu (select axis direction)")
     lines.append("    0      → Set speed slider (0-100%)")
     lines.append("    Y      → Save config (IP, gripper, points path)")
     lines.append("")
@@ -878,6 +885,110 @@ def _submenu_explore_points(robot: URRobot, messages: list[str]) -> None:
         messages.append(f"Error: {e}")
 
 
+# TCP orientation directions for the submenu
+_TCP_ORIENT_OPTIONS = [
+    ("−Z (down)",  [0.0, 0.0, -1.0]),
+    ("+Z (up)",    [0.0, 0.0,  1.0]),
+    ("−Y (back)",  [0.0, -1.0, 0.0]),
+    ("+Y (fwd)",   [0.0,  1.0, 0.0]),
+    ("−X (left)",  [-1.0, 0.0, 0.0]),
+    ("+X (right)", [ 1.0, 0.0, 0.0]),
+]
+
+
+def _submenu_orient_tcp(
+    robot: URRobot, state: dict, messages: list[str], expert_mode: bool = False
+) -> None:
+    """Interactive submenu to orient TCP along a base-axis direction."""
+    cursor = 0
+    old_settings = termios.tcgetattr(sys.stdin)
+    new_settings = termios.tcgetattr(sys.stdin)
+    new_settings[3] = new_settings[3] & ~(termios.ICANON | termios.ECHO)
+    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, new_settings)
+    fd = sys.stdin.fileno()
+
+    try:
+        while True:
+            sys.stdout.write("\033[2J\033[1;1H")
+            sys.stdout.write(cyan("  === ORIENT TCP ===") + "\n")
+            sys.stdout.write(dim("  Arrows navigate · Enter select · ESC cancel") + "\n\n")
+            sys.stdout.write(blue("  Point tool Z toward:") + "\n")
+            sys.stdout.write("  " + dim("─" * 60) + "\n")
+
+            for i, (label, _direction) in enumerate(_TCP_ORIENT_OPTIONS):
+                marker = green("►") if i == cursor else " "
+                sys.stdout.write(f"    {marker} {label}\n")
+
+            sys.stdout.write(f"\n  {dim('Enter')} to orient {green(_TCP_ORIENT_OPTIONS[cursor][0])}\n")
+            sys.stdout.flush()
+
+            ready, _, _ = select.select([fd], [], [], 0.1)
+            if not ready:
+                if _cli_monitor and _cli_monitor.fault_detected:
+                    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
+                    raise URKitConnectionError(
+                        f"Robot fault detected: {_cli_monitor._reason or 'RTDE connection lost'}. "
+                        "RTDE connection lost."
+                    )
+                continue
+
+            raw = os.read(fd, 64)
+            if not raw:
+                continue
+            text = raw.decode("ascii", errors="replace")
+            i = 0
+            selected = None
+            while i < len(text):
+                ch = text[i]
+                if ch == "\x1b":
+                    if i + 2 < len(text) and text[i + 1] == "[":
+                        key = text[i + 2]
+                        if key == "A":
+                            cursor = max(0, cursor - 1)
+                        elif key == "B":
+                            cursor = min(len(_TCP_ORIENT_OPTIONS) - 1, cursor + 1)
+                        i += 3
+                        continue
+                    else:
+                        return None  # ESC → cancel
+                if ch == "\n" or ch == "\r":
+                    selected = cursor
+                elif ch == "\x03":
+                    return None
+                i += 1
+
+            if selected is not None:
+                break
+    finally:
+        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
+
+    if selected is None:
+        messages.append("Cancelled")
+        return
+
+    label, direction = _TCP_ORIENT_OPTIONS[selected]
+    try:
+        if state["freedrive"]:
+            robot.disable_freedrive()
+            state["freedrive"] = False
+
+        pose = robot.get_tcp_pose()
+        target = orient_tcp(pose, direction)
+        try:
+            robot.inverse_kinematics(target)
+        except MotionError:
+            messages.append(f"Unreachable: TCP {label} — no IK solution")
+            return
+
+        if expert_mode:
+            robot.move_to(target, vel=0.5, acc=0.3)
+        else:
+            robot.move_to(target, vel=0.125, acc=0.1)
+        messages.append(f"TCP oriented {label}")
+    except MotionError as e:
+        messages.append(f"Error: {e}")
+
+
 # ------------------------------------------------------------------
 # Key input helpers
 # ------------------------------------------------------------------
@@ -944,7 +1055,7 @@ def _teach_pendant(
 
     Args:
         robot: Initialized URRobot instance.
-        expert_mode: When True, skip safety speed clamping on goto/tcp-down.
+        expert_mode: When True, skip safety speed clamping on goto/tcp-orient.
     """
     state: dict = {
         "linear_step": _DEFAULT_LINEAR_STEP,
@@ -954,6 +1065,7 @@ def _teach_pendant(
         "move_frame": robot.move_frame,
         "goto_mode": "cartesian",  # "cartesian" or "joint" for Go To
         "speed_slider": 1.0,
+        "ik_reference": robot._ik_reference,
     }
 
     # Read the robot's current speed slider so the display matches reality.
@@ -1193,31 +1305,13 @@ def _teach_pendant(
                     messages.append(f"Go To mode: {state['goto_mode'].capitalize()}")
                     command_handled = True
 
-                # --- TCP orient down ---
+                # --- TCP orient submenu ---
                 elif key == "t":
                     if state["move_frame"] == MoveFrame.TOOL:
-                        messages.append("TCP Down unavailable in TOOL frame")
+                        messages.append("TCP Orient unavailable in TOOL frame")
                         command_handled = True
                     else:
-                        try:
-                            if state["freedrive"]:
-                                robot.disable_freedrive()
-                                state["freedrive"] = False
-                            pose = robot.get_tcp_pose()
-                            target = orient_tcp_down(pose)
-                            try:
-                                robot.inverse_kinematics(target)
-                            except MotionError:
-                                messages.append("Unreachable: TCP Down — no IK solution")
-                                command_handled = True
-                            else:
-                                if expert_mode:
-                                    robot.move_to(target, vel=0.5, acc=0.3)
-                                else:
-                                    robot.move_to(target, vel=0.125, acc=0.1)
-                                messages.append("TCP oriented downward")
-                        except MotionError as e:
-                            messages.append(f"Error: {e}")
+                        _submenu_orient_tcp(robot, state, messages, expert_mode)
                         command_handled = True
 
                 # --- Gripper ---
@@ -1460,8 +1554,15 @@ def teach_command(args) -> None:
     if gripper_name:
         print(f"  Gripper: {gripper_name}")
     print(f"  Points:  {points_path}")
-    if ik_reference:
-        print(f"  IK ref:  {ik_reference}")
+    if gripper_config is not None:
+        tcp = gripper_config.tcp_offset
+        non_zero = [v for v in tcp if abs(v) > 1e-6]
+        if len(non_zero) == 1 and abs(tcp[0]) < 1e-6 and abs(tcp[1]) < 1e-6:
+            print(f"  TCP:     Z={tcp[2]*1000:.1f}mm")
+        elif len(non_zero) <= 3 and all(abs(tcp[i]) < 1e-6 for i in range(3, 6)):
+            print(f"  TCP:     X={tcp[0]*1000:.1f} Y={tcp[1]*1000:.1f} Z={tcp[2]*1000:.1f}mm")
+        else:
+            print(f"  TCP:     {list(tcp)}")
 
     # URRobot handles everything: safety recovery, remote mode check,
     # power on, brake release, program stop, and RTDE connection.

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

@@ -87,75 +87,113 @@ def rpy_to_quat(
     )
 
 
-def orient_tcp_down(pose: list[float]) -> list[float]:
-    """Orient TCP downward while preserving tool heading.
-
-    Points the tool Z-axis straight down (base frame -Z) while
-    preserving the tool's heading (X-axis direction projected to
-    the XY plane). This avoids the ambiguity of RPY yaw at roll=π
-    and produces a smooth, predictable orientation change.
+def orient_tcp(
+    pose: list[float], direction: list[float]
+) -> list[float]:
+    """Orient TCP along a target direction via minimal relative rotation.
 
-    Matches the approach used in ur_bag_picking's orient_face_down.
+    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.
+        direction: Target direction for tool Z-axis [dx, dy, dz]
+            (normalized automatically).
 
     Returns:
-        New pose with same position but TCP pointing downward.
+        New pose with same position but TCP pointing along direction.
     """
     pos = pose[:3]
     rv = pose[3:]
 
-    # Current rotation matrix (columns are X, Y, Z axes)
-    R = _rotvec_to_matrix(rv)
-
-    # Target: Z points down
-    z_new = [0.0, 0.0, -1.0]
-
-    # Preserve heading: project current X axis to XY plane
-    x_new = [R[0][0], R[1][0], 0.0]
-    norm_x = math.sqrt(x_new[0] ** 2 + x_new[1] ** 2)
-
-    if norm_x < 1e-6:
-        # X is nearly vertical — use Y projection instead
-        x_new = [0.0, 0.0, 0.0]
-        y_new = [R[0][1], R[1][1], 0.0]
-        norm_y = math.sqrt(y_new[0] ** 2 + y_new[1] ** 2)
-
-        if norm_y < 1e-6:
-            # Both vertical — fallback to roll=π
-            return [pos[0], pos[1], pos[2], math.pi, 0.0, 0.0]
-
-        y_new[0] /= norm_y
-        y_new[1] /= norm_y
-        # x = y cross z (right-hand rule: y × -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],
+    # Normalize target direction
+    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_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
-        # 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]]
 
 
+def orient_tcp_down(pose: list[float]) -> list[float]:
+    """Orient TCP downward while preserving tool heading.
+
+    Points the tool Z-axis straight down (base frame -Z) while
+    preserving the tool's heading. Convenience wrapper around
+    :func:`orient_tcp`.
+
+    Args:
+        pose: [x, y, z, rx, ry, rz] rotation vector pose.
+
+    Returns:
+        New pose with same position but TCP pointing downward.
+    """
+    return orient_tcp(pose, [0.0, 0.0, -1.0])
+
+
 # ------------------------------------------------------------------
 # Quaternion algebra helpers
 # ------------------------------------------------------------------

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

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: urkit
-Version: 0.3.20
+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.20/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}"