每周期单步QP改为有界迭代QP

xr_rm_teleop/test/test_placo_transforms.py

@@ -1,3 +1,4 @@
+import math
 from pathlib import Path
 from xml.etree import ElementTree
 
@@ -45,6 +46,58 @@ def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
     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,
+        ]
+    ]
+    return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints
+
+
+def test_qp_solve_converges_to_reachable_tcp_target() -> None:
+    solver, joints = _rm75_placo_solver()
+    start_pose = solver.update_joint_state(joints)
+    target_pose = start_pose.copy()
+    target_pose[0, 3] += 0.07
+
+    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 position_error <= 1e-3
+    assert orientation_error <= 5e-3
+
+
 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]

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -1,4 +1,4 @@
-"""RM75 的 Placo 0.9.4 单步 QP 逆解。"""
+"""RM75 的 Placo 0.9.4 有界迭代 QP 逆解。"""
 
 from __future__ import annotations
 
@@ -10,6 +10,9 @@ import numpy as np
 EXPECTED_PLACO_VERSION = "0.9.4"
 RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
 RM75_Q_SLICE = slice(7, 14)
+QP_MAX_ITERATIONS = 30
+QP_POSITION_TOLERANCE_M = 1e-3
+QP_ORIENTATION_TOLERANCE_RAD = 5e-3
 
 
 def _validated_transform(transform: np.ndarray) -> np.ndarray:
@@ -118,25 +121,74 @@ class PlacoIkSolver:
             self._frame_task.T_world_frame = base_to_tool.copy()
         return base_to_tool.copy()
 
+    def _target_errors(self) -> tuple[float, float]:
+        position_task = self._frame_task.position()
+        orientation_task = self._frame_task.orientation()
+        position_task.update()
+        orientation_task.update()
+        return (
+            float(position_task.error_norm()),
+            float(orientation_task.error_norm()),
+        )
+
     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(target_tool_pose)
-        self._solver.solve(True)
+        self._frame_task.T_world_frame = _validated_transform(
+            target_tool_pose
+        )
         result = np.asarray(
             self._robot.state.q[RM75_Q_SLICE],
             dtype=float,
         ).copy()
-        self._validate_result(result)
-        return result.tolist()
+        position_error, orientation_error = self._target_errors()
+        if (
+            position_error <= QP_POSITION_TOLERANCE_M
+            and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+        ):
+            return result.tolist()
 
-    def _validate_result(self, result: np.ndarray) -> None:
+        for _ in range(QP_MAX_ITERATIONS):
+            previous = result
+            self._solver.solve(True)
+            self._robot.update_kinematics()
+            result = np.asarray(
+                self._robot.state.q[RM75_Q_SLICE],
+                dtype=float,
+            ).copy()
+            self._validate_result(result, previous)
+            position_error, orientation_error = self._target_errors()
+            if (
+                position_error <= QP_POSITION_TOLERANCE_M
+                and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+            ):
+                return result.tolist()
+
+        raise RuntimeError(
+            "QP did not converge after "
+            f"{QP_MAX_ITERATIONS} iterations: "
+            f"position_error={position_error:.6f} m, "
+            f"orientation_error={orientation_error:.6f} rad"
+        )
+
+    def _validate_result(
+        self,
+        result: np.ndarray,
+        reference: np.ndarray | None = None,
+    ) -> None:
         if result.shape != (7,) or not np.isfinite(result).all():
             raise ValueError("QP result must contain 7 finite values")
         lower = self._joint_limits[:, 0]
         upper = self._joint_limits[:, 1]
         if np.any(result < lower - 1e-9) or np.any(result > upper + 1e-9):
             raise ValueError("QP result violates RM75 joint position limits")
+        if reference is None:
+            reference = self._actual_joints
+        if reference is None:
+            raise RuntimeError("joint state has not been initialized")
+        reference = np.asarray(reference, dtype=float)
+        if reference.shape != (7,) or not np.isfinite(reference).all():
+            raise ValueError("QP reference must contain 7 finite values")
         max_step = self._velocity_limits * self._dt + 1e-9
-        if np.any(np.abs(result - self._actual_joints) > max_step):
+        if np.any(np.abs(result - reference) > max_step):
             raise ValueError("QP result violates RM75 one-cycle velocity limits")