Add Placo IK solver and associated tests.

xr_rm_teleop/test/test_orientation_control.py

@@ -1,9 +1,10 @@
 import math
+import time
 from types import SimpleNamespace
 
 import pytest
 
-from xr_rm_teleop.realman_adapter import ArmPose, MockRealManAdapter
+from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
 from xr_rm_teleop.single_arm_velocity_teleop import (
     SingleArmVelocityTeleop,
     _euler_to_quaternion,
@@ -95,6 +96,19 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
     teleop._arm_name = "test_rm75"
     teleop._command_timeout_sec = 0.12
     teleop._enable_orientation_control = True
+    teleop._adapter = SimpleNamespace(
+        get_latest_joint_state=lambda: JointStateSnapshot(
+            [0.1] * 7,
+            time.monotonic(),
+        )
+    )
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2)
+    )
+    teleop._active = False
+    teleop._last_valid_joint_target = None
+    teleop._last_current_pose = None
+    teleop._joint_feedback_ready = True
     stopped = []
     teleop.get_clock = lambda: FakeClock()
     teleop.get_logger = lambda: FakeLogger()
@@ -113,12 +127,3 @@ def test_quaternion_roundtrip_for_small_rpy() -> None:
 def test_zero_quaternion_is_invalid() -> None:
     with pytest.raises(ValueError):
         _normalize_quaternion([0.0, 0.0, 0.0, 0.0])
-
-
-def test_mock_adapter_uses_shortest_angular_velocity() -> None:
-    adapter = MockRealManAdapter([0.0, 0.0, 0.0, 3.13, 0.0, -3.13], 0.1)
-
-    adapter.send_cartesian_target(ArmPose(0.0, 0.0, 0.0, -3.13, 0.0, 3.13), False)
-
-    assert abs(adapter.last_velocity[3]) < 1.0
-    assert abs(adapter.last_velocity[5]) < 1.0