每周期单步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]