Add URDF model for RM75-B OmniPicker with detailed link and joint specifications

xr_rm_teleop/test/placo_ik_smoke.py

@@ -8,56 +8,67 @@ from pathlib import Path
 import numpy as np
 
 from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
-from xr_rm_teleop.realman_adapter import ArmPose
 
 
 CASES = {
-    "left": (
-        [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
-        [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],
-    ),
-    "right": (
-        [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
-        [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],
-    ),
+    "left": [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
+    "right": [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
 }
 
 
-def angle_error(actual: list[float], target: list[float]) -> float:
-    deltas = [
-        math.atan2(math.sin(a - b), math.cos(a - b))
-        for a, b in zip(actual, target)
-    ]
-    return math.sqrt(sum(value * value for value in deltas))
+def rotation_z(angle: float) -> np.ndarray:
+    cosine = math.cos(angle)
+    sine = math.sin(angle)
+    return np.asarray(
+        [
+            [cosine, -sine, 0.0],
+            [sine, cosine, 0.0],
+            [0.0, 0.0, 1.0],
+        ]
+    )
+
+
+def angle_error(actual: np.ndarray, target: np.ndarray) -> float:
+    cosine = np.clip((np.trace(target @ actual.T) - 1.0) * 0.5, -1.0, 1.0)
+    return float(math.acos(cosine))
 
 
 def main() -> None:
     urdf_path = Path(sys.argv[1]).resolve()
-    for arm, (joint_degrees, tool_pose) in CASES.items():
-        solver = PlacoIkSolver(str(urdf_path), tool_pose, 1.0 / 90.0)
-        joints = np.deg2rad(joint_degrees).tolist()
-        current = solver.update_joint_state(joints)
-        target = ArmPose(
-            current.x + 0.01,
-            current.y,
-            current.z,
-            current.rx,
-            current.ry,
-            current.rz + 0.05,
+    for arm, joint_degrees in CASES.items():
+        initial_joints = np.deg2rad(joint_degrees)
+        drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
+        joints = initial_joints.tolist()
+        stationary_target = drift_solver.update_joint_state(joints)
+        flange = drift_solver._robot.get_T_world_frame("link_7")
+        flange_to_tcp = np.linalg.inv(flange) @ stationary_target
+        assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, 0.16])
+        assert np.allclose(flange_to_tcp[:3, :3], np.eye(3))
+        for _ in range(250):
+            drift_solver.update_joint_state(joints)
+            joints = drift_solver.solve(stationary_target)
+        drift_degrees = float(
+            np.max(np.abs(np.rad2deg(np.asarray(joints) - initial_joints)))
         )
 
+        solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
+        joints = initial_joints.tolist()
+        current = solver.update_joint_state(joints)
+        assert current.shape == (4, 4)
+        target = current.copy()
+        target[0, 3] += 0.01
+        target[:3, :3] = rotation_z(0.05) @ target[:3, :3]
+
         solve_durations = []
-        for _ in range(45):
+        for _ in range(250):
             solver.update_joint_state(joints)
             started_at = time.perf_counter()
             joints = solver.solve(target)
             solve_durations.append(time.perf_counter() - started_at)
 
         actual = solver.update_joint_state(joints)
-        position_error = np.linalg.norm(
-            np.asarray(actual.xyz()) - np.asarray(target.xyz())
-        )
-        orientation_error = angle_error(actual.rpy(), target.rpy())
+        position_error = np.linalg.norm(actual[:3, 3] - target[:3, 3])
+        orientation_error = angle_error(actual[:3, :3], target[:3, :3])
         assert len(joints) == 7
         assert np.isfinite(joints).all()
         assert np.allclose(
@@ -69,9 +80,10 @@ def main() -> None:
         print(
             f"{arm}: position_error={position_error:.6f}m, "
             f"orientation_error={math.degrees(orientation_error):.3f}deg, "
+            f"stationary_drift={drift_degrees:.3f}deg, "
             f"solve_avg={1000.0 * np.mean(solve_durations):.3f}ms, "
             f"solve_max={1000.0 * max(solve_durations):.3f}ms, "
-            f"solve_overruns={sum(value > 1.0 / 90.0 for value in solve_durations)}"
+            f"solve_overruns={sum(value > 1.0 / 125.0 for value in solve_durations)}"
         )
 
 

xr_rm_teleop/test/test_joint_control.py

@@ -1,13 +1,21 @@
 import time
 from types import SimpleNamespace
 
+import numpy as np
 import pytest
 
-from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
-from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop
+from xr_rm_teleop.realman_adapter import JointStateSnapshot
+from xr_rm_teleop.single_arm_velocity_teleop import (
+    SingleArmVelocityTeleop,
+    _make_transform,
+    _so3_exp,
+)
 
 
 class FakeLogger:
+    def info(self, *args, **kwargs):
+        del args, kwargs
+
     def warn(self, *args, **kwargs):
         del args, kwargs
 
@@ -78,7 +86,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
 
         def update_joint_state(self, joints):
             assert joints == [0.1] * 7
-            return ArmPose(0.3, 0.0, 0.2)
+            transform = np.eye(4)
+            transform[:3, 3] = [0.3, 0.0, 0.2]
+            return transform
 
         def solve(self, target):
             del target
@@ -95,7 +105,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
         JointStateSnapshot([0.1] * 7, time.monotonic())
     )
 
-    assert pose == ArmPose(0.3, 0.0, 0.2)
+    assert pose == pytest.approx(
+        _make_transform([0.3, 0.0, 0.2], np.eye(3))
+    )
     assert teleop._last_valid_joint_target == [0.1] * 7
     assert teleop._ik_solver.solve_calls == 0
 
@@ -112,7 +124,7 @@ def test_qp_failure_returns_last_known_good_target() -> None:
     teleop._arm_name = "right_rm75"
     teleop.get_logger = lambda: FakeLogger()
 
-    target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
+    target = teleop._solve_joint_target(np.eye(4))
 
     assert target == pytest.approx([0.1] * 7)
     assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
@@ -130,12 +142,88 @@ def test_qp_success_updates_last_known_good_target() -> None:
     teleop._arm_name = "left_rm75"
     teleop.get_logger = lambda: FakeLogger()
 
-    target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
+    target = teleop._solve_joint_target(np.eye(4))
 
     assert target == pytest.approx([0.2] * 7)
     assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
 
 
+def test_enter_active_control_initializes_se3_orientation_state() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    transform = _make_transform(
+        [0.3, -0.1, 0.2],
+        _so3_exp(np.asarray([0.1, -0.2, 0.3])),
+    )
+    published = []
+    teleop._arm_name = "right_rm75"
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._publish_debug = lambda *args: published.append(args)
+
+    teleop._enter_active_control(
+        [0.0, 0.0, 0.0],
+        (0.0, 0.0, 0.0, 1.0),
+        transform,
+        FakeTime(),
+    )
+
+    assert teleop._robot_start_transform == pytest.approx(transform)
+    assert teleop._filtered_target == pytest.approx(transform[:3, 3])
+    assert teleop._filtered_orientation_target == pytest.approx(transform[:3, :3])
+    assert teleop._last_sent_orientation == pytest.approx(transform[:3, :3])
+    assert len(published) == 1
+
+
+def test_command_angular_velocity_uses_so3_rotation_vector() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._dt = 0.1
+    teleop._last_sent_target = [0.0, 0.0, 0.0]
+    teleop._last_sent_orientation = np.eye(3)
+    teleop._last_command_time = None
+
+    velocity = teleop._estimate_command_velocity(
+        [0.0, 0.0, 0.0],
+        _so3_exp(np.asarray([0.0, 0.0, 0.1])),
+        FakeTime(),
+    )
+
+    assert velocity == pytest.approx([0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
+
+
+def test_timing_stats_logs_summary_and_clears_window() -> None:
+    messages = []
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._dt = 0.008
+    teleop._timing_stats_window = 2
+    teleop._timing_samples = {
+        name: []
+        for name in ("period", "total", "qp", "send", "feedback_age")
+    }
+    teleop.get_logger = lambda: SimpleNamespace(
+        info=lambda message: messages.append(message)
+    )
+
+    teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
+    assert messages == []
+
+    teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)
+
+    assert len(messages) == 1
+    assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
+    assert (
+        "period[n=2 mean=8.000 p95=8.900 p99=8.980 "
+        "max=9.000 ms overruns=1]"
+    ) in messages[0]
+    assert (
+        "total[n=2 mean=8.000 p95=9.800 p99=9.960 "
+        "max=10.000 ms overruns=1]"
+    ) in messages[0]
+    assert "qp[n=2" in messages[0]
+    assert "send[n=2" in messages[0]
+    assert "feedback_age[n=2" in messages[0]
+    assert all(not samples for samples in teleop._timing_samples.values())
+
+
 def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
     class FailingAdapter:
         def __init__(self) -> None:

xr_rm_teleop/test/test_orientation_control.py

@@ -2,14 +2,19 @@ import math
 import time
 from types import SimpleNamespace
 
+import numpy as np
 import pytest
 
-from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
+from xr_rm_teleop.realman_adapter import JointStateSnapshot
 from xr_rm_teleop.single_arm_velocity_teleop import (
     SingleArmVelocityTeleop,
-    _euler_to_quaternion,
+    _make_transform,
+    _matrix_to_quaternion,
     _normalize_quaternion,
-    _quaternion_to_euler,
+    _project_rotation,
+    _quaternion_to_matrix,
+    _so3_exp,
+    _so3_log,
 )
 
 
@@ -18,7 +23,10 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
     teleop._enable_orientation_control = True
     teleop._enable_orientation_axes = [True, True, True]
     teleop._controller_orientation_start = (0.0, 0.0, 0.0, 1.0)
-    teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.1, -0.2, 0.3)
+    teleop._robot_start_transform = _make_transform(
+        [0.3, 0.0, 0.2],
+        _so3_exp(np.asarray([0.1, -0.2, 0.3])),
+    )
     teleop._xr_to_robot_matrix = [
         0.0, 1.0, 0.0,
         0.0, 0.0, 1.0,
@@ -27,47 +35,111 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
     return teleop
 
 
-def assert_angles_close(actual: list[float] | tuple[float, ...], expected: list[float]) -> None:
-    assert len(actual) == len(expected)
-    for actual_value, expected_value in zip(actual, expected):
-        assert math.atan2(math.sin(actual_value - expected_value), math.cos(actual_value - expected_value)) == pytest.approx(0.0)
-
-
 def test_identity_controller_orientation_keeps_tcp_orientation() -> None:
     teleop = _make_teleop_for_orientation()
 
     target = teleop._raw_orientation_from_controller((0.0, 0.0, 0.0, 1.0))
 
-    assert_angles_close(target, teleop._robot_start_pose.rpy())
+    assert target == pytest.approx(teleop._robot_start_transform[:3, :3])
 
 
 def test_xr_relative_rotation_maps_through_xr_to_robot_matrix() -> None:
     teleop = _make_teleop_for_orientation()
-    teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.0, 0.0, 0.0)
-    xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0)
+    teleop._robot_start_transform = np.eye(4)
+    xr_roll = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, 0.0, 0.0])))
 
     target = teleop._raw_orientation_from_controller(xr_roll)
 
-    assert_angles_close(target, [0.0, 0.0, 0.2])
+    assert _so3_log(target) == pytest.approx([0.0, 0.0, 0.2])
 
 
-def test_orientation_deadband_filter_and_speed_limit() -> None:
+def test_quaternion_sign_does_not_change_rotation() -> None:
+    quaternion = _normalize_quaternion((0.2, -0.3, 0.1, 0.9))
+
+    assert _quaternion_to_matrix(quaternion) == pytest.approx(
+        _quaternion_to_matrix(tuple(-value for value in quaternion))
+    )
+
+
+@pytest.mark.parametrize("pitch", [math.pi / 2.0 - 1e-5, -math.pi / 2.0 + 1e-5])
+def test_small_rotation_near_gimbal_lock_stays_small(pitch: float) -> None:
+    teleop = _make_teleop_for_orientation()
+    start_rotation = _so3_exp(np.asarray([0.0, pitch, 0.0]))
+    teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation)
+    teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
+    controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.01, 0.0, 0.0])))
+
+    target = teleop._raw_orientation_from_controller(controller)
+
+    error = _so3_log(target @ start_rotation.T)
+    assert np.linalg.norm(error) == pytest.approx(0.01)
+
+
+def test_crossing_old_rpy_branch_uses_shortest_rotation() -> None:
+    teleop = _make_teleop_for_orientation()
+    start_rotation = _so3_exp(np.asarray([0.0, math.pi / 2.0 - 0.001, 0.0]))
+    teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation)
+    teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
+    controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.0, 0.002, 0.0])))
+
+    target = teleop._raw_orientation_from_controller(controller)
+
+    assert _so3_log(target @ start_rotation.T) == pytest.approx(
+        [0.0, 0.002, 0.0],
+        abs=1e-9,
+    )
+
+
+def test_disabled_orientation_axis_zeros_robot_rotation_vector_component() -> None:
+    teleop = _make_teleop_for_orientation()
+    teleop._robot_start_transform = np.eye(4)
+    teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
+    teleop._enable_orientation_axes = [True, False, True]
+    controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.1, 0.2, 0.3])))
+
+    target = teleop._raw_orientation_from_controller(controller)
+
+    assert _so3_log(target) == pytest.approx([0.1, 0.0, 0.3])
+
+
+def test_orientation_deadband_filter_and_speed_limit_use_so3_angle() -> None:
     teleop = object.__new__(SingleArmVelocityTeleop)
     teleop._orientation_deadband_rad = 0.01
     teleop._orientation_filter_alpha = 0.5
     teleop._max_orientation_speed = 0.5
-    teleop._dt = 0.1
-    teleop._last_sent_orientation = [0.0, 0.0, 0.0]
-    teleop._filtered_orientation_target = [0.0, 0.0, 0.0]
+    teleop._dt = 1.0 / 125.0
+    teleop._last_sent_orientation = np.eye(3)
+    teleop._filtered_orientation_target = np.eye(3)
 
-    assert teleop._apply_orientation_deadband([0.001, 0.0, 0.0]) == [0.0, 0.0, 0.0]
+    inside_deadband = _so3_exp(np.asarray([0.006, 0.006, 0.0]))
+    assert teleop._apply_orientation_deadband(inside_deadband) == pytest.approx(np.eye(3))
 
-    filtered = teleop._filter_orientation_target([0.2, 0.0, 0.0])
-    assert_angles_close(filtered, [0.1, 0.0, 0.0])
+    target = _so3_exp(np.asarray([0.2, 0.0, 0.0]))
+    filtered = teleop._filter_orientation_target(target)
+    assert _so3_log(filtered) == pytest.approx([0.1, 0.0, 0.0])
 
-    limited, was_limited = teleop._limit_orientation_step([0.2, 0.0, 0.0])
+    limited, was_limited = teleop._limit_orientation_step(target)
     assert was_limited
-    assert_angles_close(limited, [0.05, 0.0, 0.0])
+    assert np.linalg.norm(_so3_log(limited)) == pytest.approx(0.5 / 125.0)
+
+
+def test_rotation_matrix_to_debug_quaternion_is_normalized() -> None:
+    quaternion = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, -0.1, 0.3])))
+
+    assert np.isfinite(quaternion).all()
+    assert np.linalg.norm(quaternion) == pytest.approx(1.0)
+
+
+def test_rotation_projection_accepts_small_error_and_rejects_invalid_matrix() -> None:
+    near_rotation = np.eye(3)
+    near_rotation[0, 1] = 1e-5
+
+    projected = _project_rotation(near_rotation)
+
+    assert projected.T @ projected == pytest.approx(np.eye(3))
+    assert np.linalg.det(projected) == pytest.approx(1.0)
+    with pytest.raises(ValueError):
+        _project_rotation(np.diag([2.0, 1.0, 1.0]))
 
 
 def test_invalid_controller_quaternion_stops_current_tick() -> None:
@@ -102,9 +174,7 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
             time.monotonic(),
         )
     )
-    teleop._ik_solver = SimpleNamespace(
-        update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2)
-    )
+    teleop._ik_solver = SimpleNamespace(update_joint_state=lambda joints: np.eye(4))
     teleop._active = False
     teleop._last_valid_joint_target = None
     teleop._last_current_pose = None
@@ -119,11 +189,6 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
     assert stopped == [True]
 
 
-def test_quaternion_roundtrip_for_small_rpy() -> None:
-    quat = _normalize_quaternion(_euler_to_quaternion(0.2, -0.1, 0.3))
-    assert_angles_close(_quaternion_to_euler(quat), [0.2, -0.1, 0.3])
-
-
 def test_zero_quaternion_is_invalid() -> None:
     with pytest.raises(ValueError):
         _normalize_quaternion([0.0, 0.0, 0.0, 0.0])

xr_rm_teleop/test/test_placo_transforms.py

@@ -1,37 +1,72 @@
-import math
+from pathlib import Path
+from xml.etree import ElementTree
 
 import numpy as np
 import pytest
 
 from xr_rm_teleop.placo_ik_solver import (
     PlacoIkSolver,
-    _arm_pose_to_transform,
-    _tool_pose_to_transform,
-    _transform_to_arm_pose,
+    _validated_transform,
 )
-from xr_rm_teleop.realman_adapter import ArmPose
 
 
-def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
-    flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
-    tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
+    urdf_path = (
+        Path(__file__).resolve().parents[1]
+        / "models"
+        / "rm75_omnipicker"
+        / "urdf"
+        / "RM75-B_OmniPicker_fixed.urdf"
+    )
+    root = ElementTree.parse(urdf_path).getroot()
+    moving_joint_names = [
+        joint.attrib["name"]
+        for joint in root.findall("joint")
+        if joint.attrib["type"] != "fixed"
+    ]
+    tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']")
+    mesh_filenames = [
+        mesh.attrib["filename"]
+        for mesh in root.findall(".//mesh")
+    ]
 
-    base_to_flange = _arm_pose_to_transform(flange_pose)
-    flange_to_tool = _tool_pose_to_transform(tool_pose)
-    base_to_tool = base_to_flange @ flange_to_tool
-    recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)
-
-    assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
-    assert recovered_flange == pytest.approx(base_to_flange)
+    assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
+    assert all(
+        filename.startswith(
+            "package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
+        )
+        for filename in mesh_filenames
+    )
+    assert tcp_joint is not None
+    assert tcp_joint.attrib["type"] == "fixed"
+    assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
+    assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
+    assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
+    assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
 
 
-def test_transform_to_arm_pose_roundtrip() -> None:
-    expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)
+def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None:
+    transform = np.eye(4)
+    transform[:3, 3] = [0.3, -0.1, 0.2]
 
-    actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))
+    actual = _validated_transform(transform)
 
-    assert actual.xyz() == pytest.approx(expected.xyz())
-    assert actual.rpy() == pytest.approx(expected.rpy())
+    assert actual == pytest.approx(transform)
+    assert actual is not transform
+
+
+@pytest.mark.parametrize(
+    "transform",
+    [
+        np.eye(3),
+        np.full((4, 4), np.nan),
+        np.vstack([np.eye(3, 4), [0.0, 0.0, 0.0, 2.0]]),
+        np.diag([2.0, 1.0, 1.0, 1.0]),
+    ],
+)
+def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
+    with pytest.raises(ValueError):
+        _validated_transform(transform)
 
 
 def test_qp_result_rejects_nan_position_and_velocity_violations() -> None: