From fbd170c0beb9f1500c79c953a863e3e12b24b111 Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Mon, 24 Aug 2026 17:46:01 +0800 Subject: [PATCH] =?UTF-8?q?feat:=20=E6=B7=BB=E5=8A=A0=E9=80=86=E8=BF=90?= =?UTF-8?q?=E5=8A=A8=E5=AD=A6=E5=AF=B9=E6=AF=94=E6=8C=87=E6=A0=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- xr_rm_teleop/test/ik_method_comparison.py | 91 +++++++++++++++++++ .../test/test_ik_method_comparison.py | 38 ++++++++ 2 files changed, 129 insertions(+) diff --git a/xr_rm_teleop/test/ik_method_comparison.py b/xr_rm_teleop/test/ik_method_comparison.py index 97a4bf3..bd604c9 100644 --- a/xr_rm_teleop/test/ik_method_comparison.py +++ b/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), + ) diff --git a/xr_rm_teleop/test/test_ik_method_comparison.py b/xr_rm_teleop/test/test_ik_method_comparison.py index 3073e08..3f1db75 100644 --- a/xr_rm_teleop/test/test_ik_method_comparison.py +++ b/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