Add Placo IK solver and associated tests.

xr_rm_teleop/test/test_joint_control.py

@@ -0,0 +1,164 @@
+import time
+from types import SimpleNamespace
+
+import pytest
+
+from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
+from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop
+
+
+class FakeLogger:
+    def warn(self, *args, **kwargs):
+        del args, kwargs
+
+    def error(self, *args, **kwargs):
+        del args, kwargs
+
+
+class FakeTime:
+    def __sub__(self, other):
+        del other
+        return SimpleNamespace(nanoseconds=0)
+
+
+def test_missing_or_stale_feedback_does_not_enable_qp() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._command_timeout_sec = 0.12
+    teleop._adapter = SimpleNamespace(get_latest_joint_state=lambda: None)
+
+    assert teleop._fresh_joint_state() is None
+
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.0] * 7,
+            time.monotonic() - 1.0,
+        )
+    )
+    assert teleop._fresh_joint_state() is None
+
+
+def test_stale_feedback_stops_before_active_control() -> None:
+    stopped = []
+    entered = []
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.0] * 7,
+            time.monotonic() - 1.0,
+        )
+    )
+    teleop._command_timeout_sec = 0.12
+    teleop._joint_feedback_ready = True
+    teleop._arm_name = "right_rm75"
+    teleop._last_msg = SimpleNamespace(
+        grip=True,
+        pose=SimpleNamespace(
+            position=SimpleNamespace(x=0.0, y=0.0, z=0.0),
+            orientation=SimpleNamespace(x=0.0, y=0.0, z=0.0, w=1.0),
+        ),
+    )
+    teleop._last_msg_time = FakeTime()
+    teleop._active = False
+    teleop._enable_orientation_control = False
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._safe_stop = lambda reset_active: stopped.append(reset_active)
+    teleop._enter_active_control = lambda *args: entered.append(args)
+
+    teleop._control_tick()
+
+    assert stopped == [True]
+    assert entered == []
+
+
+def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
+    class FakeSolver:
+        def __init__(self) -> None:
+            self.solve_calls = 0
+
+        def update_joint_state(self, joints):
+            assert joints == [0.1] * 7
+            return ArmPose(0.3, 0.0, 0.2)
+
+        def solve(self, target):
+            del target
+            self.solve_calls += 1
+            return [0.2] * 7
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = FakeSolver()
+    teleop._active = False
+    teleop._last_valid_joint_target = None
+    teleop._last_current_pose = None
+
+    pose = teleop._sync_joint_feedback(
+        JointStateSnapshot([0.1] * 7, time.monotonic())
+    )
+
+    assert pose == ArmPose(0.3, 0.0, 0.2)
+    assert teleop._last_valid_joint_target == [0.1] * 7
+    assert teleop._ik_solver.solve_calls == 0
+
+
+def test_qp_failure_returns_last_known_good_target() -> None:
+    class FailingSolver:
+        def solve(self, target):
+            del target
+            raise RuntimeError("NaN in QP solution")
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = FailingSolver()
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._arm_name = "right_rm75"
+    teleop.get_logger = lambda: FakeLogger()
+
+    target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
+
+    assert target == pytest.approx([0.1] * 7)
+    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
+
+
+def test_qp_success_updates_last_known_good_target() -> None:
+    class SuccessfulSolver:
+        def solve(self, target):
+            del target
+            return [0.2] * 7
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = SuccessfulSolver()
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._arm_name = "left_rm75"
+    teleop.get_logger = lambda: FakeLogger()
+
+    target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
+
+    assert target == pytest.approx([0.2] * 7)
+    assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
+
+
+def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
+    class FailingAdapter:
+        def __init__(self) -> None:
+            self.stop_calls = 0
+
+        def send_joint_target(self, joints, follow):
+            del joints, follow
+            raise RuntimeError("send failed")
+
+        def stop(self):
+            self.stop_calls += 1
+
+    reset_calls = []
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = FailingAdapter()
+    teleop._follow = False
+    teleop._arm_name = "left_rm75"
+    teleop._stop_sent = False
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._safe_stop = lambda reset_active: reset_calls.append(reset_active)
+
+    sent = teleop._send_joint_target([0.1] * 7)
+
+    assert not sent
+    assert teleop._adapter.stop_calls == 1
+    assert reset_calls == [True]