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),
+    )