diff --git a/xr_rm_teleop/test/test_placo_transforms.py b/xr_rm_teleop/test/test_placo_transforms.py index ae8da9a..34b5dee 100644 --- a/xr_rm_teleop/test/test_placo_transforms.py +++ b/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] diff --git a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py b/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py index f9f17cb..4820b54 100644 --- a/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py +++ b/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