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

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: