Add Placo IK solver and associated tests.

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -1,4 +1,10 @@
+import math
+
+import pytest
+
 from xr_rm_teleop.realman_adapter import RealManAdapter
+from xr_rm_teleop.realman_adapter import MockRealManAdapter
+from xr_rm_teleop.fun_peripheral import PeripheralConfig
 
 
 def test_initial_pose_uses_joint_move_only() -> None:
@@ -17,3 +23,66 @@ def test_initial_pose_uses_joint_move_only() -> None:
     adapter._move_to_initial_pose()
 
     assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]
+
+
+def test_peripheral_config_exposes_selected_tool() -> None:
+    config = PeripheralConfig(
+        scissorgripper=1,
+        tools_in_ee={
+            "first": [[0.0] * 7, [0.0] * 7],
+            "second": [[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0], [0.0] * 7],
+        },
+    )
+
+    assert config.tool_name == "second"
+    assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
+
+
+def test_joint_feedback_is_cached_in_radians() -> None:
+    class FakeArm:
+        def rm_get_joint_degree(self):
+            return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
+
+    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
+    adapter._arm = FakeArm()
+
+    adapter._read_joint_state_once()
+    snapshot = adapter.get_latest_joint_state()
+
+    assert snapshot is not None
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
+    )
+
+
+def test_joint_target_uses_movej_canfd_in_degrees() -> None:
+    class FakeArm:
+        def __init__(self) -> None:
+            self.calls = []
+
+        def rm_movej_canfd(self, *args):
+            self.calls.append(args)
+            return 0
+
+    adapter = RealManAdapter("127.0.0.1", 8080, 0, 0.01)
+    adapter._arm = FakeArm()
+    target = [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
+
+    adapter.send_joint_target(target, follow=False)
+
+    assert len(adapter._arm.calls) == 1
+    degrees, follow, expand, trajectory_mode, radio = adapter._arm.calls[0]
+    assert degrees == pytest.approx([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
+    assert (follow, expand, trajectory_mode, radio) == (False, 0, 2, 0)
+
+
+def test_mock_joint_feedback_is_available_without_vendor_sdk() -> None:
+    adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
+
+    adapter.connect()
+    snapshot = adapter.get_latest_joint_state()
+
+    assert snapshot is not None
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
+    )