feat: 添加三种逆运动学离线复放

2026-08-24 17:50:23 +08:00
parent fbd170c0be
commit 398a50b0b3
2 changed files with 300 additions and 1 deletions
@@ -140,3 +140,68 @@ def test_limit_joint_command_reuses_production_limiter() -> None:
    assert target == pytest.approx([0.1] * 7)
    assert velocity == pytest.approx([1.0] * 7)
    assert limited
+
+
+class _FakeSolver:
+    def __init__(self, fail: bool) -> None:
+        self.fail = fail
+        self.joint_limits = np.asarray([[-2.0, 2.0]] * 7)
+
+    def update_joint_state(self, joints: list[float]) -> np.ndarray:
+        pose = np.eye(4)
+        pose[0, 3] = joints[0]
+        return pose
+
+    def solve(self, target: np.ndarray) -> list[float]:
+        if self.fail:
+            raise RuntimeError("not converged")
+        return [float(target[0, 3])] + [0.0] * 6
+
+
+def test_replay_holds_previous_state_on_solver_failure() -> None:
+    trajectory = comparison.EpisodeTrajectory(
+        source_path=Path("episode.hdf5"),
+        times_s=np.asarray([0.0, 0.1]),
+        target_poses=np.asarray(
+            [
+                [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+                [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],
+            ]
+        ),
+        initial_joints=np.zeros(7),
+    )
+
+    result = comparison.run_replay(
+        "fake",
+        _FakeSolver(fail=True),
+        trajectory,
+        max_speed=1.0,
+        max_acceleration=10.0,
+        measure_time=False,
+    )
+
+    assert result.joints == pytest.approx(np.zeros((2, 7)))
+    assert not result.success.any()
+    assert result.velocities == pytest.approx(np.zeros((2, 7)))
+
+
+def test_real_urdf_solvers_return_finite_safe_outputs() -> None:
+    pytest.importorskip("placo")
+    urdf = TEST_DIR.parent / "models" / "dual_rm75" / "Dual_arm.urdf"
+    joints = np.radians(
+        [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
+    )
+    solvers = [
+        comparison.DifferentialIkSolver(urdf, 1.0 / 90.0, "pinv"),
+        comparison.DifferentialIkSolver(urdf, 1.0 / 90.0, "dls", 0.03),
+        comparison.make_qp_solver(urdf, 1.0 / 90.0),
+    ]
+    for solver in solvers:
+        target = solver.update_joint_state(joints.tolist())
+        target = target.copy()
+        target[0, 3] += 0.003
+        result = np.asarray(solver.solve(target), dtype=float)
+        assert result.shape == (7,)
+        assert np.isfinite(result).all()
+        assert np.all(result >= solver.joint_limits[:, 0] - 1e-9)
+        assert np.all(result <= solver.joint_limits[:, 1] + 1e-9)