feat: 汇总逆运动学对比结果

xr_rm_teleop/test/test_ik_method_comparison.py

@@ -188,13 +188,23 @@ def test_replay_holds_previous_state_on_solver_failure() -> None:
 def test_real_urdf_solvers_return_finite_safe_outputs() -> None:
     pytest.importorskip("placo")
     urdf = TEST_DIR.parent / "models" / "dual_rm75" / "Dual_arm.urdf"
+    config_path = (
+        TEST_DIR.parents[1]
+        / "xr_rm_bringup"
+        / "config"
+        / "right_arm_rm75.yaml"
+    )
     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),
+        comparison.make_qp_solver(
+            urdf,
+            1.0 / 90.0,
+            comparison.load_right_config(config_path),
+        ),
     ]
     for solver in solvers:
         target = solver.update_joint_state(joints.tolist())
@@ -205,3 +215,44 @@ def test_real_urdf_solvers_return_finite_safe_outputs() -> None:
         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)
+
+
+def test_load_right_config_returns_qp_and_command_limits() -> None:
+    config = (
+        TEST_DIR.parents[1]
+        / "xr_rm_bringup"
+        / "config"
+        / "right_arm_rm75.yaml"
+    )
+
+    actual = comparison.load_right_config(config)
+
+    assert actual["qp_j3_reference_deg"] == pytest.approx(-89.57)
+    assert actual["qp_j4_min_deg"] == pytest.approx(10.0)
+    assert actual["qp_manipulability_weight"] == pytest.approx(1e-4)
+    assert actual["joint_max_speed"] == pytest.approx(180.0)
+    assert actual["joint_max_acc"] == pytest.approx(300.0)
+
+
+def test_summarize_result_uses_report_metrics() -> None:
+    result = comparison.ReplayResult(
+        method="pinv",
+        times_s=np.asarray([0.0, 0.1]),
+        target_poses=np.zeros((2, 7)),
+        actual_poses=np.zeros((2, 7)),
+        joints=np.zeros((2, 7)),
+        velocities=np.asarray([[0.0] * 7, [math.pi] + [0.0] * 6]),
+        position_errors_m=np.asarray([0.003, 0.004]),
+        orientation_errors_rad=np.asarray([0.01, 0.02]),
+        joint_margins=np.asarray([0.2, 0.1]),
+        solve_durations_ms=np.asarray([1.0, 2.0]),
+        success=np.asarray([True, False]),
+        command_limited=np.asarray([False, True]),
+    )
+
+    actual = comparison.summarize_result(result)
+
+    assert actual.success_rate == pytest.approx(0.5)
+    assert actual.position_rmse_m == pytest.approx(0.0035355339)
+    assert actual.orientation_rmse_rad == pytest.approx(0.0158113883)
+    assert actual.max_joint_speed_deg_s == pytest.approx(180.0)