From 2d197a8928a065d3e88989f8ec9ea0ecb8f09d9f Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Mon, 3 Aug 2026 15:49:58 +0800 Subject: [PATCH] =?UTF-8?q?test:=20=E6=B7=BB=E5=8A=A0=E5=8F=8C=20RM75=20?= =?UTF-8?q?=E9=80=86=E8=A7=A3=E5=9B=9E=E5=BD=92?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- xr_rm_teleop/test/test_placo_transforms.py | 192 +++++++++++++++------ 1 file changed, 142 insertions(+), 50 deletions(-) diff --git a/xr_rm_teleop/test/test_placo_transforms.py b/xr_rm_teleop/test/test_placo_transforms.py index 34b5dee..917960f 100644 --- a/xr_rm_teleop/test/test_placo_transforms.py +++ b/xr_rm_teleop/test/test_placo_transforms.py @@ -7,76 +7,156 @@ import numpy as np import pytest 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", + ), + ( + "right", + [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35], + list(range(7, 14)), + list(range(6, 13)), + "scissor", + ), +) -def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None: - urdf_path = ( - Path(__file__).resolve().parents[1] - / "models" - / "rm75_omnipicker" - / "urdf" - / "RM75-B_OmniPicker_fixed.urdf" - ) - root = ElementTree.parse(urdf_path).getroot() + +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" ] - tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']") 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"joint_{index}" for index in range(1, 8)] - assert all( - filename.startswith( - "package://xr_rm_teleop/models/rm75_omnipicker/meshes/" - ) - for filename in mesh_filenames - ) - assert tcp_joint is not None - assert tcp_joint.attrib["type"] == "fixed" - assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link" - assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp" - assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16" - assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0" - - -def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]: - pytest.importorskip("placo") - urdf_path = ( - Path(__file__).resolve().parents[1] - / "models" - / "rm75_omnipicker" - / "urdf" - / "RM75-B_OmniPicker_fixed.urdf" - ) - joints = [ - math.radians(value) - for value in [ - -90.14, - 3.76, - -86.89, - 87.89, - -96.53, - -79.62, - -90.04, - ] + assert moving_joint_names == [ + *[f"omnipic_joint_{index}" for index in range(1, 8)], + *[f"scissor_joint_{index}" for index in range(1, 8)], ] - return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints + 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_qp_solve_converges_to_reachable_tcp_target() -> None: - solver, joints = _rm75_placo_solver() +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", + 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, +) -> None: + solver, _ = _dual_placo_solver(arm, joint_degrees) + + assert solver._q_offsets == q_offsets + assert solver._v_offsets == v_offsets + + +@pytest.mark.parametrize( + "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix", + 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, +) -> 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 actual_pose == pytest.approx(np.linalg.inv(world_base) @ world_tcp) + + +@pytest.mark.parametrize( + "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix", + 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, +) -> 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.07 + target_pose[0, 3] += 0.01 result = solver.solve(target_pose) reached_pose = solver.update_joint_state(result) @@ -96,8 +176,20 @@ def test_qp_solve_converges_to_reachable_tcp_target() -> None: ) ) + assert np.asarray(result).shape == (7,) + assert np.isfinite(result).all() assert position_error <= QP_POSITION_TOLERANCE_M - assert orientation_error <= 5e-3 + 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: + pytest.importorskip("placo") + + 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: