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 import placo_ik_solver from xr_rm_teleop.placo_ik_solver import ( QP_ORIENTATION_TOLERANCE_RAD, QP_POSITION_TOLERANCE_M, PlacoIkSolver, _validated_transform, ) DUAL_URDF_PATH = ( Path(__file__).resolve().parents[1] / "models" / "dual_rm75" / "Dual_arm.urdf" ) ARM_CASES = ( ( "left", [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55], list(range(14, 21)), list(range(13, 20)), "omnipic", "scissor_base_link", "scissor_scissor_tcp", ), ( "right", [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35], list(range(7, 14)), list(range(6, 13)), "scissor", "omnipic_base_link", "omnipic_OmniPic_tcp", ), ) def test_dual_urdf_has_expected_joints_meshes_and_tcp_frames() -> None: root = ElementTree.parse(DUAL_URDF_PATH).getroot() moving_joint_names = [ joint.attrib["name"] for joint in root.findall("joint") if joint.attrib["type"] != "fixed" ] mesh_filenames = [ mesh.attrib["filename"] for mesh in root.findall(".//mesh") ] fixed_joints = { "omnipic_base_mount_joint": ( "dual_arm_base_link", "omnipic_base_link", None, ), "scissor_base_mount_joint": ( "dual_arm_base_link", "scissor_base_link", None, ), "omnipic_OmniPic_tcp_fixed": ( "omnipic_gripper_link", "omnipic_OmniPic_tcp", "0 0 0.14", ), "scissor_scissor_tcp_fixed": ( "scissor_scissor_link", "scissor_scissor_tcp", "0 0 0", ), "scissor_scissor_fixed_joint": ( "scissor_link_7", "scissor_scissor_link", "0 0 0.165", ), } assert moving_joint_names == [ *[f"omnipic_joint_{index}" for index in range(1, 8)], *[f"scissor_joint_{index}" for index in range(1, 8)], ] assert all(filename.startswith("meshes/") for filename in mesh_filenames) for name, (parent, child, xyz) in fixed_joints.items(): joint = root.find(f"joint[@name='{name}']") assert joint is not None assert joint.attrib["type"] == "fixed" assert joint.find("parent").attrib["link"] == parent assert joint.find("child").attrib["link"] == child if xyz is not None: assert joint.find("origin").attrib["xyz"] == xyz def test_left_scissor_mesh_matches_physical_mount_rotation() -> None: root = ElementTree.parse(DUAL_URDF_PATH).getroot() link = root.find("link[@name='scissor_scissor_link']") assert link is not None assert link.find("visual/origin").attrib["rpy"] == "0 0 -1.5708" assert link.find("collision/origin").attrib["rpy"] == "0 0 -1.5708" tcp_joint = root.find("joint[@name='scissor_scissor_tcp_fixed']") assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0" def _dual_placo_solver( arm: str, joint_degrees: list[float], ) -> tuple[PlacoIkSolver, list[float]]: pytest.importorskip("placo") joints = [math.radians(value) for value in joint_degrees] return PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, arm), joints @pytest.mark.parametrize( "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix," "expected_base_frame,expected_tcp_frame", ARM_CASES, ) def test_solver_uses_arm_specific_offsets( arm: str, joint_degrees: list[float], q_offsets: list[int], v_offsets: list[int], inactive_prefix: str, expected_base_frame: str, expected_tcp_frame: str, ) -> None: solver, _ = _dual_placo_solver(arm, joint_degrees) assert solver._q_offsets.tolist() == q_offsets assert solver._v_offsets.tolist() == v_offsets @pytest.mark.parametrize( "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix," "expected_base_frame,expected_tcp_frame", ARM_CASES, ) def test_joint_state_pose_is_relative_to_selected_arm_base( arm: str, joint_degrees: list[float], q_offsets: list[int], v_offsets: list[int], inactive_prefix: str, expected_base_frame: str, expected_tcp_frame: str, ) -> None: solver, joints = _dual_placo_solver(arm, joint_degrees) actual_pose = solver.update_joint_state(joints) assert solver._base_frame == expected_base_frame assert solver._tcp_frame == expected_tcp_frame world_base = solver._robot.get_T_world_frame(expected_base_frame) world_tcp = solver._robot.get_T_world_frame(expected_tcp_frame) assert actual_pose == pytest.approx(np.linalg.inv(world_base) @ world_tcp) @pytest.mark.parametrize( "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix," "expected_base_frame,expected_tcp_frame", ARM_CASES, ) def test_qp_solve_converges_without_moving_inactive_arm( arm: str, joint_degrees: list[float], q_offsets: list[int], v_offsets: list[int], inactive_prefix: str, expected_base_frame: str, expected_tcp_frame: str, ) -> None: solver, joints = _dual_placo_solver(arm, joint_degrees) start_pose = solver.update_joint_state(joints) inactive_q_offsets = [ solver._robot.get_joint_offset(f"{inactive_prefix}_joint_{index}") for index in range(1, 8) ] inactive_before = solver._robot.state.q[inactive_q_offsets].copy() target_pose = start_pose.copy() target_pose[0, 3] += 0.01 result = solver.solve(target_pose) reached_pose = solver.update_joint_state(result) position_error = np.linalg.norm( target_pose[:3, 3] - reached_pose[:3, 3] ) rotation_delta = ( target_pose[:3, :3] @ reached_pose[:3, :3].T ) orientation_error = math.acos( float( np.clip( (np.trace(rotation_delta) - 1.0) * 0.5, -1.0, 1.0, ) ) ) assert np.asarray(result).shape == (7,) assert np.isfinite(result).all() assert position_error <= QP_POSITION_TOLERANCE_M assert orientation_error <= QP_ORIENTATION_TOLERANCE_RAD assert solver._robot.state.q[inactive_q_offsets] == pytest.approx( inactive_before ) def test_solver_rejects_unknown_arm() -> None: with pytest.raises(ValueError, match="arm must be left or right"): PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, "middle") def test_qp_solve_accepts_position_error_within_two_millimeters() -> None: solver = object.__new__(PlacoIkSolver) solver._actual_joints = np.zeros(7) solver._q_offsets = np.arange(7, 14) solver._robot = SimpleNamespace( state=SimpleNamespace(q=np.zeros(21)) ) solver._frame_task = SimpleNamespace(T_a_b=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._q_offsets = np.arange(7, 14) solver._robot = SimpleNamespace( state=SimpleNamespace(q=np.zeros(21)), update_kinematics=lambda: None, ) solver._frame_task = SimpleNamespace(T_a_b=None) solver._solver = SimpleNamespace(solve=lambda update: None) solver._validate_result = lambda result, previous: None solver._update_auxiliary_task_weights = lambda: 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] actual = _validated_transform(transform) assert actual == pytest.approx(transform) assert actual is not transform @pytest.mark.parametrize( "transform", [ np.eye(3), np.full((4, 4), np.nan), np.vstack([np.eye(3, 4), [0.0, 0.0, 0.0, 2.0]]), np.diag([2.0, 1.0, 1.0, 1.0]), ], ) def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None: with pytest.raises(ValueError): _validated_transform(transform) 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)) def test_lower_margin_activation_is_clamped_and_linear() -> None: activation = placo_ik_solver._lower_margin_activation assert activation(0.05, 0.01, 0.04) == 0.0 assert activation(0.025, 0.01, 0.04) == pytest.approx(0.5) assert activation(0.005, 0.01, 0.04) == 1.0 @pytest.mark.parametrize( "arm,joint_degrees,j3_reference_deg", [ ("left", ARM_CASES[0][1], 67.96), ("right", ARM_CASES[1][1], -89.57), ], ) def test_solver_configures_auxiliary_qp_tasks( arm: str, joint_degrees: list[float], j3_reference_deg: float, ) -> None: pytest.importorskip("placo") solver = PlacoIkSolver( str(DUAL_URDF_PATH), 1.0 / 90.0, arm, j3_reference_deg=j3_reference_deg, j3_weight=1e-5, j4_min_deg=10.0, j4_warn_deg=25.0, j4_weight=1e-4, manipulability_sigma_stop=0.01, manipulability_sigma_warn=0.04, manipulability_weight=1e-4, ) joints = np.radians(joint_degrees).tolist() solver.update_joint_state(joints) assert solver._j3_task.get_joint( solver._joint_names[2] ) == pytest.approx(math.radians(j3_reference_deg)) assert np.asarray(solver._j4_constraint.A)[ solver._q_offsets[3] ] == pytest.approx(-1.0) assert np.asarray(solver._j4_constraint.b) == pytest.approx( [-math.radians(10.0)] ) assert solver._j4_constraint.priority == "hard" jacobian = solver._active_tcp_jacobian() assert jacobian.shape == (6, 7) assert np.isfinite(jacobian).all() assert np.linalg.svd(jacobian, compute_uv=False)[-1] > 0.0 def test_failed_qp_restores_internal_state_to_actual_feedback() -> None: solver, joints = _dual_placo_solver("left", ARM_CASES[0][1]) current_pose = solver.update_joint_state(joints) unreachable = current_pose.copy() unreachable[2, 3] += 10.0 with pytest.raises((RuntimeError, ValueError)): solver.solve(unreachable) assert solver._robot.state.q[solver._q_offsets] == pytest.approx(joints) def test_solver_rejects_non_positive_manipulability_threshold() -> None: pytest.importorskip("placo") with pytest.raises(ValueError, match="manipulability thresholds"): PlacoIkSolver( str(DUAL_URDF_PATH), 1.0 / 90.0, "left", manipulability_sigma_stop=0.0, manipulability_sigma_warn=0.04, ) @pytest.mark.parametrize( "q4_deg,sigma_min,expected_activation", [ (25.0, 0.04, 0.0), (17.5, 0.025, 0.5), (10.0, 0.01, 1.0), ], ) def test_auxiliary_weights_activate_only_inside_warning_margins( q4_deg: float, sigma_min: float, expected_activation: float, ) -> None: class TaskSpy: def __init__(self) -> None: self.calls = [] def configure(self, name, priority, weight) -> None: self.calls.append((name, priority, weight)) solver = object.__new__(PlacoIkSolver) solver._q_offsets = np.arange(7, 14) solver._robot = SimpleNamespace( state=SimpleNamespace(q=np.zeros(21)) ) solver._robot.state.q[solver._q_offsets[3]] = math.radians(q4_deg) solver._j4_task = TaskSpy() solver._j4_min = math.radians(10.0) solver._j4_warn = math.radians(25.0) solver._j4_weight = 1e-4 solver._manipulability_task = TaskSpy() solver._manipulability_sigma_stop = 0.01 solver._manipulability_sigma_warn = 0.04 solver._manipulability_weight = 1e-4 jacobian = np.zeros((6, 7)) jacobian[:, :6] = np.diag([1.0] * 5 + [sigma_min]) solver._active_tcp_jacobian = lambda: jacobian solver._update_auxiliary_task_weights() expected_weight = 1e-4 * expected_activation assert solver._j4_task.calls == [ ("j4_soft_buffer", "soft", pytest.approx(expected_weight)) ] assert solver._manipulability_task.calls == [ ("tcp_6d_manipulability", "soft", pytest.approx(expected_weight)) ]