Add Placo IK solver and associated tests.

xr_rm_teleop/test/placo_ik_smoke.py

@@ -0,0 +1,79 @@
+from __future__ import annotations
+
+import math
+import sys
+import time
+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],
+    ),
+}
+
+
+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 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,
+        )
+
+        solve_durations = []
+        for _ in range(45):
+            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())
+        assert len(joints) == 7
+        assert np.isfinite(joints).all()
+        assert np.allclose(
+            solver.base_configuration,
+            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+        )
+        assert position_error <= 0.005
+        assert orientation_error <= math.radians(2.0)
+        print(
+            f"{arm}: position_error={position_error:.6f}m, "
+            f"orientation_error={math.degrees(orientation_error):.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)}"
+        )
+
+
+if __name__ == "__main__":
+    main()

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]]
+    )

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]

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

xr_rm_teleop/test/test_placo_transforms.py

@@ -0,0 +1,49 @@
+import math
+
+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,
+)
+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]
+
+    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)
+
+
+def test_transform_to_arm_pose_roundtrip() -> None:
+    expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)
+
+    actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))
+
+    assert actual.xyz() == pytest.approx(expected.xyz())
+    assert actual.rpy() == pytest.approx(expected.rpy())
+
+
+def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
+    solver._velocity_limits = np.ones(7)
+    solver._dt = 0.1
+    solver._actual_joints = np.zeros(7)
+
+    with pytest.raises(ValueError, match="finite"):
+        solver._validate_result(np.full(7, np.nan))
+    with pytest.raises(ValueError, match="position"):
+        solver._validate_result(np.full(7, 2.0))
+    with pytest.raises(ValueError, match="velocity"):
+        solver._validate_result(np.full(7, 0.2))