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

xr_rm_teleop/test/ik_method_comparison.py

@@ -2,11 +2,12 @@ from __future__ import annotations
 
 import math
 import time
-from dataclasses import dataclass
+from dataclasses import dataclass, replace
 from pathlib import Path
 
 import h5py
 import numpy as np
+import yaml
 
 from xr_rm_teleop.placo_ik_solver import (
     QP_MAX_ITERATIONS,
@@ -39,6 +40,12 @@ class MethodSummary:
     position_rmse_m: float
     orientation_rmse_rad: float
     max_joint_speed_deg_s: float
+    min_joint_margin: float = 0.0
+    mean_solve_ms: float = 0.0
+    max_solve_ms: float = 0.0
+    failure_count: int = 0
+    longest_failure_streak: int = 0
+    command_limited_count: int = 0
 
 
 @dataclass(frozen=True)
@@ -197,6 +204,25 @@ def load_episode(path: Path) -> EpisodeTrajectory:
     )
 
 
+def load_right_config(path: Path) -> dict[str, float]:
+    with Path(path).open("r", encoding="utf-8") as stream:
+        document = yaml.safe_load(stream)
+    parameters = document["single_arm_velocity_teleop"]["ros__parameters"]
+    names = (
+        "qp_j3_reference_deg",
+        "qp_j3_weight",
+        "qp_j4_min_deg",
+        "qp_j4_warn_deg",
+        "qp_j4_weight",
+        "qp_manipulability_sigma_stop",
+        "qp_manipulability_sigma_warn",
+        "qp_manipulability_weight",
+        "joint_max_speed",
+        "joint_max_acc",
+    )
+    return {name: float(parameters[name]) for name in names}
+
+
 def _rotation_z(angle: float) -> np.ndarray:
     cosine, sine = math.cos(angle), math.sin(angle)
     return np.asarray(
@@ -247,6 +273,40 @@ def choose_dls_damping(candidates: list[MethodSummary]) -> float:
     return float(selected.damping)
 
 
+def _longest_failure_streak(success: np.ndarray) -> int:
+    longest = current = 0
+    for value in np.asarray(success, dtype=bool):
+        current = 0 if value else current + 1
+        longest = max(longest, current)
+    return longest
+
+
+def summarize_result(
+    result: ReplayResult,
+    damping: float | None = None,
+) -> MethodSummary:
+    return MethodSummary(
+        method=result.method,
+        damping=damping,
+        success_rate=float(np.mean(result.success)),
+        position_rmse_m=float(
+            np.sqrt(np.mean(result.position_errors_m**2))
+        ),
+        orientation_rmse_rad=float(
+            np.sqrt(np.mean(result.orientation_errors_rad**2))
+        ),
+        max_joint_speed_deg_s=float(
+            np.max(np.abs(np.degrees(result.velocities)))
+        ),
+        min_joint_margin=float(np.min(result.joint_margins)),
+        mean_solve_ms=float(np.mean(result.solve_durations_ms)),
+        max_solve_ms=float(np.max(result.solve_durations_ms)),
+        failure_count=int(np.count_nonzero(~result.success)),
+        longest_failure_streak=_longest_failure_streak(result.success),
+        command_limited_count=int(np.count_nonzero(result.command_limited)),
+    )
+
+
 def limit_joint_command(
     *,
     target: np.ndarray,
@@ -385,20 +445,28 @@ class QpSolverAdapter:
         return self._solver.solve(target)
 
 
-def make_qp_solver(urdf_path: Path, dt: float) -> QpSolverAdapter:
+def make_qp_solver(
+    urdf_path: Path,
+    dt: float,
+    config: dict[str, float],
+) -> QpSolverAdapter:
     return QpSolverAdapter(
         PlacoIkSolver(
             str(urdf_path),
             dt,
             "right",
-            j3_reference_deg=-89.57,
+            j3_reference_deg=config["qp_j3_reference_deg"],
-            j3_weight=1e-4,
+            j3_weight=config["qp_j3_weight"],
-            j4_min_deg=10.0,
+            j4_min_deg=config["qp_j4_min_deg"],
-            j4_warn_deg=25.0,
+            j4_warn_deg=config["qp_j4_warn_deg"],
-            j4_weight=1e-4,
+            j4_weight=config["qp_j4_weight"],
-            manipulability_sigma_stop=0.01,
+            manipulability_sigma_stop=config[
-            manipulability_sigma_warn=0.04,
+                "qp_manipulability_sigma_stop"
-            manipulability_weight=1e-4,
+            ],
+            manipulability_sigma_warn=config[
+                "qp_manipulability_sigma_warn"
+            ],
+            manipulability_weight=config["qp_manipulability_weight"],
         )
     )
 
@@ -478,3 +546,88 @@ def run_replay(
         success=success,
         command_limited=command_limited,
     )
+
+
+DLS_DAMPING_CANDIDATES = (0.001, 0.003, 0.01, 0.03, 0.1, 0.3)
+
+
+def evaluate_methods(
+    trajectory: EpisodeTrajectory,
+    urdf_path: Path,
+    config: dict[str, float],
+    timing_repeats: int,
+) -> tuple[dict[str, ReplayResult], dict[str, MethodSummary], float]:
+    if timing_repeats < 1:
+        raise ValueError("timing_repeats must be positive")
+    dt = float(np.median(np.diff(trajectory.times_s)))
+    speed = math.radians(config["joint_max_speed"])
+    acceleration = math.radians(config["joint_max_acc"])
+    damping_results = []
+    for damping in DLS_DAMPING_CANDIDATES:
+        replay = run_replay(
+            "dls",
+            DifferentialIkSolver(urdf_path, dt, "dls", damping),
+            trajectory,
+            max_speed=speed,
+            max_acceleration=acceleration,
+            measure_time=False,
+        )
+        damping_results.append(summarize_result(replay, damping))
+    selected_damping = choose_dls_damping(damping_results)
+
+    factories = {
+        "pinv": lambda: DifferentialIkSolver(urdf_path, dt, "pinv"),
+        "dls": lambda: DifferentialIkSolver(
+            urdf_path, dt, "dls", selected_damping
+        ),
+        "qp": lambda: make_qp_solver(urdf_path, dt, config),
+    }
+    results = {}
+    summaries = {}
+    for name, factory in factories.items():
+        run_replay(
+            name,
+            factory(),
+            trajectory,
+            max_speed=speed,
+            max_acceleration=acceleration,
+        )
+        result = run_replay(
+            name,
+            factory(),
+            trajectory,
+            max_speed=speed,
+            max_acceleration=acceleration,
+        )
+        timed = []
+        for _ in range(timing_repeats):
+            repeated = run_replay(
+                name,
+                factory(),
+                trajectory,
+                max_speed=speed,
+                max_acceleration=acceleration,
+            )
+            if not np.allclose(
+                repeated.joints,
+                result.joints,
+                atol=1e-10,
+                rtol=0.0,
+            ):
+                raise RuntimeError(f"{name} replay is not deterministic")
+            timed.append(repeated.solve_durations_ms)
+        timing_matrix = np.stack(timed)
+        result = replace(
+            result,
+            solve_durations_ms=np.mean(timing_matrix, axis=0),
+        )
+        results[name] = result
+        summaries[name] = replace(
+            summarize_result(
+                result,
+                selected_damping if name == "dls" else None,
+            ),
+            mean_solve_ms=float(np.mean(timing_matrix)),
+            max_solve_ms=float(np.max(timing_matrix)),
+        )
+    return results, summaries, selected_damping

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)