feat: 使用双 RM75 局部相对逆解

xr_rm_teleop/test/placo_ik_smoke.py

@@ -11,8 +11,13 @@ from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
 
 
 CASES = {
-    "left": [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
+    "left": [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
-    "right": [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
+    "right": [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
+}
+
+TOOL_CHAINS = {
+    "left": ("scissor_base_link", "scissor_link_7", 0.165),
+    "right": ("omnipic_base_link", "omnipic_link_7", 0.14),
 }
 
 
@@ -37,13 +42,16 @@ def main() -> None:
     urdf_path = Path(sys.argv[1]).resolve()
     for arm, joint_degrees in CASES.items():
         initial_joints = np.deg2rad(joint_degrees)
-        drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
+        drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0, arm)
         joints = initial_joints.tolist()
         stationary_target = drift_solver.update_joint_state(joints)
-        flange = drift_solver._robot.get_T_world_frame("link_7")
+        base_frame, flange_frame, tcp_length = TOOL_CHAINS[arm]
-        flange_to_tcp = np.linalg.inv(flange) @ stationary_target
+        world_to_base = drift_solver._robot.get_T_world_frame(base_frame)
-        assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, 0.16])
+        world_to_flange = drift_solver._robot.get_T_world_frame(flange_frame)
-        assert np.allclose(flange_to_tcp[:3, :3], np.eye(3))
+        base_to_flange = np.linalg.inv(world_to_base) @ world_to_flange
+        flange_to_tcp = np.linalg.inv(base_to_flange) @ stationary_target
+        assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, tcp_length])
+        assert np.allclose(flange_to_tcp[:3, :3], np.eye(3), atol=1e-5)
         for _ in range(250):
             drift_solver.update_joint_state(joints)
             joints = drift_solver.solve(stationary_target)
@@ -54,7 +62,7 @@ def main() -> None:
             f"{arm} stationary target drifted {drift_degrees:.3f}deg"
         )
 
-        solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
+        solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0, arm)
         joints = initial_joints.tolist()
         current = solver.update_joint_state(joints)
         assert current.shape == (4, 4)

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -8,8 +8,24 @@ from pathlib import Path
 import numpy as np
 
 EXPECTED_PLACO_VERSION = "0.9.4"
-RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
+ARM_CHAINS = {
-RM75_Q_SLICE = slice(7, 14)
+    "left": (
+        "scissor_base_link",
+        "scissor_scissor_tcp",
+        "scissor",
+        "omnipic",
+    ),
+    "right": (
+        "omnipic_base_link",
+        "omnipic_OmniPic_tcp",
+        "omnipic",
+        "scissor",
+    ),
+}
+DUAL_RM75_JOINT_NAMES = [
+    *[f"omnipic_joint_{index}" for index in range(1, 8)],
+    *[f"scissor_joint_{index}" for index in range(1, 8)],
+]
 QP_MAX_ITERATIONS = 30
 QP_POSITION_TOLERANCE_M = 2e-3
 QP_ORIENTATION_TOLERANCE_RAD = 5e-3
@@ -43,9 +59,21 @@ class PlacoIkSolver:
         self,
         urdf_path: str,
         dt: float,
+        arm: str,
     ) -> None:
         if dt <= 0.0:
             raise ValueError("dt must be positive")
+        if arm not in ARM_CHAINS:
+            raise ValueError("arm must be left or right")
+        self._base_frame, self._tcp_frame, prefix, inactive_prefix = (
+            ARM_CHAINS[arm]
+        )
+        self._joint_names = [
+            f"{prefix}_joint_{index}" for index in range(1, 8)
+        ]
+        inactive_joint_names = [
+            f"{inactive_prefix}_joint_{index}" for index in range(1, 8)
+        ]
         try:
             installed_version = version("placo")
             import placo
@@ -65,30 +93,44 @@ class PlacoIkSolver:
 
         self._dt = dt
         self._robot = placo.RobotWrapper(str(model_path))
-        if self._robot.state.q.shape != (14,):
+        if self._robot.state.q.shape != (21,):
             raise RuntimeError(
-                f"expected Placo q shape (14,), got {self._robot.state.q.shape}"
+                "expected Placo q shape (21,), got "
+                f"{self._robot.state.q.shape}"
             )
-        if list(self._robot.joint_names()) != RM75_JOINT_NAMES:
+        if list(self._robot.joint_names()) != DUAL_RM75_JOINT_NAMES:
             raise RuntimeError(
-                f"unexpected RM75 joint order: {list(self._robot.joint_names())}"
+                "unexpected dual RM75 joint order: "
+                f"{list(self._robot.joint_names())}"
+            )
+
+        self._q_offsets = np.asarray(
+            [self._robot.get_joint_offset(name) for name in self._joint_names],
+            dtype=int,
+        )
+        self._v_offsets = np.asarray(
+            [
+                self._robot.get_joint_v_offset(name)
+                for name in self._joint_names
+            ],
+            dtype=int,
+        )
+        if len(set(self._q_offsets.tolist())) != 7:
+            raise RuntimeError(
+                f"invalid RM75 q offsets: {self._q_offsets.tolist()}"
+            )
+        if len(set(self._v_offsets.tolist())) != 7:
+            raise RuntimeError(
+                f"invalid RM75 v offsets: {self._v_offsets.tolist()}"
             )
-        offsets = [
-            self._robot.get_joint_offset(name) for name in RM75_JOINT_NAMES
-        ]
-        if offsets != list(range(7, 14)):
-            raise RuntimeError(f"unexpected RM75 q offsets: {offsets}")
 
         self._joint_limits = np.asarray(
-            [self._robot.get_joint_limits(name) for name in RM75_JOINT_NAMES]
+            [self._robot.get_joint_limits(name) for name in self._joint_names]
         )
-        velocity_offsets = [
-            self._robot.get_joint_v_offset(name) for name in RM75_JOINT_NAMES
-        ]
         self._velocity_limits = np.asarray(
             [
                 self._robot.model.velocityLimit[index]
-                for index in velocity_offsets
+                for index in self._v_offsets
             ]
         )
         self._actual_joints: np.ndarray | None = None
@@ -96,12 +138,15 @@ class PlacoIkSolver:
         self._solver = placo.KinematicsSolver(self._robot)
         self._solver.dt = dt
         self._solver.mask_fbase(True)
+        for name in inactive_joint_names:
+            self._solver.mask_dof(name)
         self._solver.enable_velocity_limits(True)
-        self._frame_task = self._solver.add_frame_task(
+        self._frame_task = self._solver.add_relative_frame_task(
-            "omnipicker_tcp",
+            self._base_frame,
+            self._tcp_frame,
             np.eye(4),
         )
-        self._frame_task.configure("rm75_frame", "soft", 1.0)
+        self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
         self._solver.add_kinetic_energy_regularization_task(1e-6)
 
     @property
@@ -114,11 +159,14 @@ class PlacoIkSolver:
             raise ValueError("joint state must contain 7 finite values")
         is_first_feedback = self._actual_joints is None
         self._actual_joints = values.copy()
-        self._robot.state.q[RM75_Q_SLICE] = values
+        self._robot.state.q[self._q_offsets] = values
         self._robot.update_kinematics()
-        base_to_tool = self._robot.get_T_world_frame("omnipicker_tcp")
+        base_to_tool = (
+            np.linalg.inv(self._robot.get_T_world_frame(self._base_frame))
+            @ self._robot.get_T_world_frame(self._tcp_frame)
+        )
         if is_first_feedback:
-            self._frame_task.T_world_frame = base_to_tool.copy()
+            self._frame_task.T_a_b = base_to_tool.copy()
         return base_to_tool.copy()
 
     def _target_errors(self) -> tuple[float, float]:
@@ -134,11 +182,11 @@ class PlacoIkSolver:
     def solve(self, target_tool_pose: np.ndarray) -> list[float]:
         if self._actual_joints is None:
             raise RuntimeError("joint state must be initialized before QP solve")
-        self._frame_task.T_world_frame = _validated_transform(
+        self._frame_task.T_a_b = _validated_transform(
             target_tool_pose
         )
         result = np.asarray(
-            self._robot.state.q[RM75_Q_SLICE],
+            self._robot.state.q[self._q_offsets],
             dtype=float,
         ).copy()
         position_error, orientation_error = self._target_errors()
@@ -153,7 +201,7 @@ class PlacoIkSolver:
             self._solver.solve(True)
             self._robot.update_kinematics()
             result = np.asarray(
-                self._robot.state.q[RM75_Q_SLICE],
+                self._robot.state.q[self._q_offsets],
                 dtype=float,
             ).copy()
             self._validate_result(result, previous)