diff --git a/xr_rm_teleop/test/test_placo_transforms.py b/xr_rm_teleop/test/test_placo_transforms.py index 9a21140..a3ddb4d 100644 --- a/xr_rm_teleop/test/test_placo_transforms.py +++ b/xr_rm_teleop/test/test_placo_transforms.py @@ -26,6 +26,8 @@ ARM_CASES = ( list(range(14, 21)), list(range(13, 20)), "omnipic", + "scissor_base_link", + "scissor_scissor_tcp", ), ( "right", @@ -33,6 +35,8 @@ ARM_CASES = ( list(range(7, 14)), list(range(6, 13)), "scissor", + "omnipic_base_link", + "omnipic_OmniPic_tcp", ), ) @@ -101,7 +105,8 @@ def _dual_placo_solver( @pytest.mark.parametrize( - "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix", + "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix," + "expected_base_frame,expected_tcp_frame", ARM_CASES, ) def test_solver_uses_arm_specific_offsets( @@ -110,6 +115,8 @@ def test_solver_uses_arm_specific_offsets( 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) @@ -118,7 +125,8 @@ def test_solver_uses_arm_specific_offsets( @pytest.mark.parametrize( - "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix", + "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( @@ -127,18 +135,23 @@ def test_joint_state_pose_is_relative_to_selected_arm_base( 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) - world_base = solver._robot.get_T_world_frame(solver._base_frame) - world_tcp = solver._robot.get_T_world_frame(solver._tcp_frame) + 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", + "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( @@ -147,6 +160,8 @@ def test_qp_solve_converges_without_moving_inactive_arm( 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) @@ -186,8 +201,6 @@ def test_qp_solve_converges_without_moving_inactive_arm( def test_solver_rejects_unknown_arm() -> None: - pytest.importorskip("placo") - with pytest.raises(ValueError, match="arm must be left or right"): PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, "middle") @@ -195,10 +208,11 @@ def test_solver_rejects_unknown_arm() -> None: 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(14)) + state=SimpleNamespace(q=np.zeros(21)) ) - solver._frame_task = SimpleNamespace(T_world_frame=None) + solver._frame_task = SimpleNamespace(T_a_b=None) solver._target_errors = lambda: (1.5e-3, 0.0) result = solver.solve(np.eye(4)) @@ -209,11 +223,12 @@ def test_qp_solve_accepts_position_error_within_two_millimeters() -> None: 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(14)), + state=SimpleNamespace(q=np.zeros(21)), update_kinematics=lambda: None, ) - solver._frame_task = SimpleNamespace(T_world_frame=None) + solver._frame_task = SimpleNamespace(T_a_b=None) solver._solver = SimpleNamespace(solve=lambda update: None) solver._validate_result = lambda result, previous: None solver._target_errors = lambda: (2.1e-3, 0.0)