feat: 添加逆运动学对比指标

xr_rm_teleop/test/ik_method_comparison.py

@@ -1,11 +1,14 @@
 from __future__ import annotations
 
+import math
 from dataclasses import dataclass
 from pathlib import Path
 
 import h5py
 import numpy as np
 
+from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop
+
 
 @dataclass(frozen=True)
 class EpisodeTrajectory:
@@ -15,6 +18,16 @@ class EpisodeTrajectory:
     initial_joints: np.ndarray
 
 
+@dataclass(frozen=True)
+class MethodSummary:
+    method: str
+    damping: float | None
+    success_rate: float
+    position_rmse_m: float
+    orientation_rmse_rad: float
+    max_joint_speed_deg_s: float
+
+
 def _normalized_quaternion(values: np.ndarray) -> np.ndarray:
     quaternion = np.asarray(values, dtype=float)
     if quaternion.shape != (4,) or not np.isfinite(quaternion).all():
@@ -153,3 +166,81 @@ def load_episode(path: Path) -> EpisodeTrajectory:
         target_poses=selected_poses,
         initial_joints=qpos[selected.start, :7].copy(),
     )
+
+
+def _rotation_z(angle: float) -> np.ndarray:
+    cosine, sine = math.cos(angle), math.sin(angle)
+    return np.asarray(
+        [
+            [cosine, -sine, 0.0],
+            [sine, cosine, 0.0],
+            [0.0, 0.0, 1.0],
+        ]
+    )
+
+
+def orientation_error_rad(actual: np.ndarray, target: np.ndarray) -> float:
+    delta = np.asarray(target) @ np.asarray(actual).T
+    cosine = float(np.clip((np.trace(delta) - 1.0) * 0.5, -1.0, 1.0))
+    return float(math.acos(cosine))
+
+
+def normalized_joint_margin(
+    joints: np.ndarray,
+    lower: np.ndarray,
+    upper: np.ndarray,
+) -> float:
+    values = np.asarray(joints, dtype=float)
+    lower_values = np.asarray(lower, dtype=float)
+    upper_values = np.asarray(upper, dtype=float)
+    span = upper_values - lower_values
+    if np.any(span <= 0.0):
+        raise ValueError("joint limits must have positive spans")
+    margins = np.minimum(
+        values - lower_values,
+        upper_values - values,
+    ) / span
+    return float(np.min(margins))
+
+
+def choose_dls_damping(candidates: list[MethodSummary]) -> float:
+    if not candidates or any(value.damping is None for value in candidates):
+        raise ValueError("DLS candidates must contain damping values")
+    selected = min(
+        candidates,
+        key=lambda value: (
+            -value.success_rate,
+            value.position_rmse_m / 0.002
+            + value.orientation_rmse_rad / 0.005,
+            value.max_joint_speed_deg_s,
+        ),
+    )
+    return float(selected.damping)
+
+
+def limit_joint_command(
+    *,
+    target: np.ndarray,
+    previous_target: np.ndarray,
+    previous_velocity: np.ndarray,
+    max_speed: float,
+    max_acceleration: float,
+    dt: float,
+) -> tuple[np.ndarray, np.ndarray, bool]:
+    limited_target, limited_velocity = (
+        SingleArmVelocityTeleop._limit_joint_command_step(
+            target=np.asarray(target, dtype=float).tolist(),
+            previous_target=np.asarray(previous_target, dtype=float).tolist(),
+            previous_velocity=np.asarray(previous_velocity, dtype=float).tolist(),
+            max_speed=max_speed,
+            max_acceleration=max_acceleration,
+            dt=dt,
+        )
+    )
+    target_array = np.asarray(limited_target, dtype=float)
+    velocity_array = np.asarray(limited_velocity, dtype=float)
+    return (
+        target_array,
+        velocity_array,
+        not np.allclose(target_array, target, atol=1e-12, rtol=0.0),
+    )

xr_rm_teleop/test/test_ik_method_comparison.py

@@ -102,3 +102,41 @@ def test_load_episode_rejects_wrong_arm(tmp_path: Path) -> None:
 
     with pytest.raises(ValueError, match="right_rm75"):
         comparison.load_episode(path)
+
+
+def test_orientation_error_and_joint_margin_match_definitions() -> None:
+    identity = np.eye(3)
+    quarter_turn = comparison._rotation_z(math.pi / 2.0)
+    joints = np.asarray([0.0, -0.5])
+    lower = np.asarray([-1.0, -1.0])
+    upper = np.asarray([1.0, 3.0])
+
+    assert comparison.orientation_error_rad(identity, quarter_turn) \
+        == pytest.approx(math.pi / 2.0)
+    assert comparison.normalized_joint_margin(joints, lower, upper) \
+        == pytest.approx(0.125)
+
+
+def test_choose_dls_damping_is_lexicographic() -> None:
+    candidates = [
+        comparison.MethodSummary("dls", 0.01, 0.90, 0.004, 0.01, 50.0),
+        comparison.MethodSummary("dls", 0.03, 0.95, 0.006, 0.02, 30.0),
+        comparison.MethodSummary("dls", 0.10, 0.95, 0.004, 0.01, 40.0),
+    ]
+
+    assert comparison.choose_dls_damping(candidates) == pytest.approx(0.10)
+
+
+def test_limit_joint_command_reuses_production_limiter() -> None:
+    target, velocity, limited = comparison.limit_joint_command(
+        target=np.full(7, 1.0),
+        previous_target=np.zeros(7),
+        previous_velocity=np.zeros(7),
+        max_speed=1.0,
+        max_acceleration=10.0,
+        dt=0.1,
+    )
+
+    assert target == pytest.approx([0.1] * 7)
+    assert velocity == pytest.approx([1.0] * 7)
+    assert limited