test: 添加双 RM75 逆解回归

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 = (
-def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
-    urdf_path = (
     Path(__file__).resolve().parents[1]
     / "models"
-        / "rm75_omnipicker"
+    / "dual_rm75"
-        / "urdf"
+    / "Dual_arm.urdf"
-        / "RM75-B_OmniPicker_fixed.urdf"
 )
-    root = ElementTree.parse(urdf_path).getroot()
+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_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 moving_joint_names == [
-    assert all(
+        *[f"omnipic_joint_{index}" for index in range(1, 8)],
-        filename.startswith(
+        *[f"scissor_joint_{index}" for index in range(1, 8)],
-            "package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
+    ]
-        )
+    assert all(filename.startswith("meshes/") for filename in mesh_filenames)
-        for filename in mesh_filenames
+    for name, (parent, child, xyz) in fixed_joints.items():
-    )
+        joint = root.find(f"joint[@name='{name}']")
-    assert tcp_joint is not None
+        assert joint is not None
-    assert tcp_joint.attrib["type"] == "fixed"
+        assert joint.attrib["type"] == "fixed"
-    assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
+        assert joint.find("parent").attrib["link"] == parent
-    assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
+        assert joint.find("child").attrib["link"] == child
-    assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
+        if xyz is not None:
-    assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
+            assert joint.find("origin").attrib["xyz"] == xyz
 
 
-def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]:
+def _dual_placo_solver(
+    arm: str,
+    joint_degrees: list[float],
+) -> tuple[PlacoIkSolver, list[float]]:
     pytest.importorskip("placo")
-    urdf_path = (
+    joints = [math.radians(value) for value in joint_degrees]
-        Path(__file__).resolve().parents[1]
+    return PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, arm), joints
-        / "models"
+
-        / "rm75_omnipicker"
+
-        / "urdf"
+@pytest.mark.parametrize(
-        / "RM75-B_OmniPicker_fixed.urdf"
+    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix",
+    ARM_CASES,
 )
-    joints = [
+def test_solver_uses_arm_specific_offsets(
-        math.radians(value)
+    arm: str,
-        for value in [
+    joint_degrees: list[float],
-            -90.14,
+    q_offsets: list[int],
-            3.76,
+    v_offsets: list[int],
-            -86.89,
+    inactive_prefix: str,
-            87.89,
+) -> None:
-            -96.53,
+    solver, _ = _dual_placo_solver(arm, joint_degrees)
-            -79.62,
+
-            -90.04,
+    assert solver._q_offsets == q_offsets
-        ]
+    assert solver._v_offsets == v_offsets
-    ]
-    return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints
 
 
-def test_qp_solve_converges_to_reachable_tcp_target() -> None:
+@pytest.mark.parametrize(
-    solver, joints = _rm75_placo_solver()
+    "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: