diff --git a/xr_rm_teleop/test/ik_method_comparison.py b/xr_rm_teleop/test/ik_method_comparison.py index 905eba7..e30732d 100644 --- a/xr_rm_teleop/test/ik_method_comparison.py +++ b/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_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 diff --git a/xr_rm_teleop/test/test_ik_method_comparison.py b/xr_rm_teleop/test/test_ik_method_comparison.py index 8affdd2..b7e6e20 100644 --- a/xr_rm_teleop/test/test_ik_method_comparison.py +++ b/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)