Add Placo IK solver and associated tests.

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