xr_rm_teleop/test/test_placo_transforms.py

@@ -1,11 +1,13 @@
 import math
 from pathlib import Path
+from types import SimpleNamespace
 from xml.etree import ElementTree
 
 import numpy as np
 import pytest
 
 from xr_rm_teleop.placo_ik_solver import (
+    QP_POSITION_TOLERANCE_M,
     PlacoIkSolver,
     _validated_transform,
 )
@@ -94,10 +96,40 @@ def test_qp_solve_converges_to_reachable_tcp_target() -> None:
         )
     )
 
-    assert position_error <= 1e-3
+    assert position_error <= QP_POSITION_TOLERANCE_M
     assert orientation_error <= 5e-3
 
 
+def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._actual_joints = np.zeros(7)
+    solver._robot = SimpleNamespace(
+        state=SimpleNamespace(q=np.zeros(14))
+    )
+    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._target_errors = lambda: (1.5e-3, 0.0)
+
+    result = solver.solve(np.eye(4))
+
+    assert result == pytest.approx([0.0] * 7)
+
+
+def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._actual_joints = np.zeros(7)
+    solver._robot = SimpleNamespace(
+        state=SimpleNamespace(q=np.zeros(14)),
+        update_kinematics=lambda: None,
+    )
+    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._solver = SimpleNamespace(solve=lambda update: None)
+    solver._validate_result = lambda result, previous: None
+    solver._target_errors = lambda: (2.1e-3, 0.0)
+
+    with pytest.raises(RuntimeError, match="QP did not converge after 30"):
+        solver.solve(np.eye(4))
+
+
 def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None:
     transform = np.eye(4)
     transform[:3, 3] = [0.3, -0.1, 0.2]

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -11,7 +11,7 @@ EXPECTED_PLACO_VERSION = "0.9.4"
 RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
 RM75_Q_SLICE = slice(7, 14)
 QP_MAX_ITERATIONS = 30
-QP_POSITION_TOLERANCE_M = 1e-3
+QP_POSITION_TOLERANCE_M = 2e-3
 QP_ORIENTATION_TOLERANCE_RAD = 5e-3