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

2026-08-24 17:52:01 +08:00
parent 398a50b0b3
commit b0f4fafd54
2 changed files with 215 additions and 11 deletions
@@ -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_weight=1e-4,
-            j4_min_deg=10.0,
-            j4_warn_deg=25.0,
-            j4_weight=1e-4,
-            manipulability_sigma_stop=0.01,
-            manipulability_sigma_warn=0.04,
-            manipulability_weight=1e-4,
+            j3_reference_deg=config["qp_j3_reference_deg"],
+            j3_weight=config["qp_j3_weight"],
+            j4_min_deg=config["qp_j4_min_deg"],
+            j4_warn_deg=config["qp_j4_warn_deg"],
+            j4_weight=config["qp_j4_weight"],
+            manipulability_sigma_stop=config[
+                "qp_manipulability_sigma_stop"
+            ],
+            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