From 5785e73edbd27c81269852e47bb386bbd9f69b1e Mon Sep 17 00:00:00 2001 From: YikaiFu-cart Date: Mon, 24 Aug 2026 17:40:56 +0800 Subject: [PATCH] =?UTF-8?q?docs:=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=E5=AE=9E=E9=AA=8C=E6=96=B9?= =?UTF-8?q?=E6=A1=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../2026-08-24-rm75-ik-method-comparison.md | 1593 +++++++++++++++++ ...-08-24-rm75-ik-method-comparison-design.md | 415 +++++ 2 files changed, 2008 insertions(+) create mode 100644 docs/superpowers/plans/2026-08-24-rm75-ik-method-comparison.md create mode 100644 docs/superpowers/specs/2026-08-24-rm75-ik-method-comparison-design.md diff --git a/docs/superpowers/plans/2026-08-24-rm75-ik-method-comparison.md b/docs/superpowers/plans/2026-08-24-rm75-ik-method-comparison.md new file mode 100644 index 0000000..6d41623 --- /dev/null +++ b/docs/superpowers/plans/2026-08-24-rm75-ik-method-comparison.md @@ -0,0 +1,1593 @@ +# RM75 三种逆运动学方法离线对比实验实施计划 + +> **For agentic workers:** REQUIRED SUB-SKILL: Use `superpowers:subagent-driven-development` (recommended) or `superpowers:executing-plans` to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** 使用 `episode_0.hdf5` 的右臂目标位姿，在统一 90 Hz 离线运动学环境中比较 Jacobian 伪逆、固定阻尼 DLS 和当前优化 Placo QP，并生成三张报告图、逐采样数据、汇总指标和中文分析。 + +**Architecture:** 新增一个仅供离线实验使用的脚本，复用现有 `PlacoIkSolver`、双臂 URDF、右臂 YAML 参数和关节命令限速函数。脚本只读加载 episode，将 30 Hz 位姿重采样到 90 Hz，然后让三种方法从同一初始关节状态独立复放，最后统一计算指标并输出 CSV、JSON、SVG、PNG 和 Markdown。 + +**Tech Stack:** Python 3.11、NumPy 2.2.6、h5py 3.16.0、Matplotlib 3.10.9、PyYAML、Placo 0.9.4、pytest、ROS2 Humble/colcon。 + +--- + +## 文件结构 + +### 新增文件 + +- `xr_rm_teleop/test/ik_method_comparison.py` + - episode 读取和验证； + - 位姿重采样； + - 伪逆、DLS、现有 QP 三路求解； + - 共同安全输出和状态推进； + - 指标、CSV、JSON、绘图和中文结果分析； + - 命令行入口。 +- `xr_rm_teleop/test/test_ik_method_comparison.py` + - 只覆盖本实验新增的重采样、指标、DLS 选择、安全保持和真实模型冒烟逻辑。 + +### 只读复用文件 + +- `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py` +- `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py` +- `xr_rm_teleop/models/dual_rm75/Dual_arm.urdf` +- `xr_rm_bringup/config/right_arm_rm75.yaml` +- `/home/robot/ACT_Data/tomato_pick/episode_0.hdf5` + +生产节点、URDF、YAML、launch 和消息定义均不修改。 + +## 执行约定 + +所有命令从 `/home/robot/WS_xr` 执行： + +```bash +source /opt/ros/humble/setup.bash +source install/setup.bash +``` + +涉及 Placo、h5py 和 Matplotlib 的命令固定使用： + +```bash +/home/robot/miniconda3/envs/xr/bin/python +``` + +--- + +### Task 1：建立位姿类型、四元数插值和 90 Hz 重采样 + +**Files:** +- Create: `xr_rm_teleop/test/ik_method_comparison.py` +- Create: `xr_rm_teleop/test/test_ik_method_comparison.py` + +- [ ] **Step 1：写重采样失败测试** + +在 `test_ik_method_comparison.py` 中加入脚本目录导入和以下测试： + +```python +from __future__ import annotations + +import math +import sys +from pathlib import Path + +import numpy as np +import pytest + +TEST_DIR = Path(__file__).resolve().parent +if str(TEST_DIR) not in sys.path: + sys.path.insert(0, str(TEST_DIR)) + +import ik_method_comparison as comparison + + +def test_slerp_uses_shortest_arc_and_returns_unit_quaternion() -> None: + start = np.asarray([0.0, 0.0, 0.0, 1.0]) + end = -np.asarray([0.0, 0.0, math.sin(0.1), math.cos(0.1)]) + + actual = comparison._slerp_quaternion(start, end, 0.5) + + assert np.linalg.norm(actual) == pytest.approx(1.0) + assert actual == pytest.approx( + [0.0, 0.0, math.sin(0.05), math.cos(0.05)] + ) + + +def test_resample_trajectory_keeps_endpoints_and_uses_requested_rate() -> None: + trajectory = comparison.EpisodeTrajectory( + source_path=Path("episode.hdf5"), + times_s=np.asarray([0.0, 0.5, 1.0]), + target_poses=np.asarray( + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + [0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + ] + ), + initial_joints=np.zeros(7), + ) + + actual = comparison.resample_trajectory(trajectory, 4.0) + + assert actual.times_s == pytest.approx([0.0, 0.25, 0.5, 0.75, 1.0]) + assert actual.target_poses[0] == pytest.approx(trajectory.target_poses[0]) + assert actual.target_poses[-1] == pytest.approx(trajectory.target_poses[-1]) + assert actual.target_poses[:, 0] == pytest.approx(actual.times_s) +``` + +- [ ] **Step 2：运行测试并确认失败** + +Run: + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -m pytest \ + src/xr_rm_teleop/test/test_ik_method_comparison.py -q +``` + +Expected: FAIL，提示 `ik_method_comparison` 不存在或缺少 `EpisodeTrajectory`。 + +- [ ] **Step 3：实现最小重采样逻辑** + +在 `ik_method_comparison.py` 中加入： + +```python +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path + +import numpy as np + + +@dataclass(frozen=True) +class EpisodeTrajectory: + source_path: Path + times_s: np.ndarray + target_poses: np.ndarray + initial_joints: np.ndarray + + +def _normalized_quaternion(values: np.ndarray) -> np.ndarray: + quaternion = np.asarray(values, dtype=float) + if quaternion.shape != (4,) or not np.isfinite(quaternion).all(): + raise ValueError("quaternion must contain 4 finite values") + norm = float(np.linalg.norm(quaternion)) + if norm <= 1e-12: + raise ValueError("quaternion norm must be positive") + return quaternion / norm + + +def _slerp_quaternion( + start: np.ndarray, + end: np.ndarray, + fraction: float, +) -> np.ndarray: + first = _normalized_quaternion(start) + second = _normalized_quaternion(end) + dot = float(np.dot(first, second)) + if dot < 0.0: + second = -second + dot = -dot + dot = float(np.clip(dot, -1.0, 1.0)) + if dot > 1.0 - 1e-8: + return _normalized_quaternion( + first + float(fraction) * (second - first) + ) + angle = float(np.arccos(dot)) + sine = float(np.sin(angle)) + return _normalized_quaternion( + np.sin((1.0 - fraction) * angle) / sine * first + + np.sin(fraction * angle) / sine * second + ) + + +def resample_trajectory( + trajectory: EpisodeTrajectory, + sample_rate_hz: float, +) -> EpisodeTrajectory: + if not np.isfinite(sample_rate_hz) or sample_rate_hz <= 0.0: + raise ValueError("sample_rate_hz must be finite and positive") + source_times = np.asarray(trajectory.times_s, dtype=float) + poses = np.asarray(trajectory.target_poses, dtype=float) + if source_times.ndim != 1 or poses.shape != (source_times.size, 7): + raise ValueError("trajectory must contain N timestamps and N x 7 poses") + if source_times.size < 2 or np.any(np.diff(source_times) <= 0.0): + raise ValueError("trajectory timestamps must be strictly increasing") + + duration = float(source_times[-1] - source_times[0]) + count = int(round(duration * sample_rate_hz)) + 1 + target_times = np.linspace(source_times[0], source_times[-1], count) + target_poses = np.empty((count, 7), dtype=float) + for axis in range(3): + target_poses[:, axis] = np.interp( + target_times, + source_times, + poses[:, axis], + ) + for index, timestamp in enumerate(target_times): + right = int(np.searchsorted(source_times, timestamp, side="right")) + right = min(max(right, 1), source_times.size - 1) + left = right - 1 + interval = source_times[right] - source_times[left] + fraction = float((timestamp - source_times[left]) / interval) + target_poses[index, 3:] = _slerp_quaternion( + poses[left, 3:], poses[right, 3:], fraction + ) + target_poses[0] = poses[0] + target_poses[-1] = poses[-1] + return EpisodeTrajectory( + source_path=trajectory.source_path, + times_s=target_times - target_times[0], + target_poses=target_poses, + initial_joints=np.asarray(trajectory.initial_joints, dtype=float).copy(), + ) +``` + +- [ ] **Step 4：运行重采样测试** + +Run: 上一步的聚焦 pytest 命令。 + +Expected: 2 passed。 + +- [ ] **Step 5：提交任务 1** + +```bash +cd /home/robot/WS_xr/src +git add xr_rm_teleop/test/ik_method_comparison.py \ + xr_rm_teleop/test/test_ik_method_comparison.py +git commit -m "feat: 添加逆运动学轨迹重采样" +``` + +--- + +### Task 2：只读加载并验证 episode_0 + +**Files:** +- Modify: `xr_rm_teleop/test/ik_method_comparison.py` +- Modify: `xr_rm_teleop/test/test_ik_method_comparison.py` + +- [ ] **Step 1：写 HDF5 读取失败测试** + +在测试文件中加入： + +```python +import h5py + + +def _write_episode(path: Path) -> None: + poses = np.asarray( + [ + [0.1, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0], + [0.2, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0], + [0.3, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0], + [0.4, -0.2, 0.3, 0.0, 0.0, 0.0, 1.0], + ], + dtype=np.float32, + ) + with h5py.File(path, "w") as handle: + handle.attrs["arm"] = "right_rm75" + handle.attrs["pose_order"] = "x,y,z,qx,qy,qz,qw" + handle.create_dataset("debug/tcp/final_target_pose", data=poses) + handle.create_dataset( + "debug/timestamps/control_monotonic_ns", + data=np.asarray([0, 33_000_000, 66_000_000, 99_000_000]), + ) + handle.create_dataset( + "debug/control/teleop_active", data=[0, 1, 1, 0] + ) + handle.create_dataset( + "debug/control/action_valid", data=[1, 1, 1, 1] + ) + handle.create_dataset( + "debug/control/command_sent", data=[0, 1, 1, 0] + ) + qpos = np.zeros((4, 8), dtype=np.float32) + qpos[1, :7] = np.arange(7) * 0.1 + handle.create_dataset("observations/qpos", data=qpos) + + +def test_load_episode_uses_longest_valid_run_and_first_valid_qpos( + tmp_path: Path, +) -> None: + path = tmp_path / "episode.hdf5" + _write_episode(path) + + actual = comparison.load_episode(path) + + assert actual.times_s == pytest.approx([0.0, 0.033]) + assert actual.target_poses[:, 0] == pytest.approx([0.2, 0.3]) + assert actual.initial_joints == pytest.approx(np.arange(7) * 0.1) + + +def test_load_episode_rejects_wrong_arm(tmp_path: Path) -> None: + path = tmp_path / "episode.hdf5" + _write_episode(path) + with h5py.File(path, "r+") as handle: + handle.attrs.modify("arm", "left_rm75") + + with pytest.raises(ValueError, match="right_rm75"): + comparison.load_episode(path) +``` + +- [ ] **Step 2：运行测试并确认失败** + +Run: 聚焦 pytest 命令。 + +Expected: FAIL，提示 `load_episode` 不存在。 + +- [ ] **Step 3：实现数据读取和最长连续区间选择** + +在脚本中加入 `import h5py` 和： + +```python +def _longest_true_run(mask: np.ndarray) -> slice: + values = np.asarray(mask, dtype=bool) + best_start = best_stop = start = 0 + for index, enabled in enumerate(np.r_[values, False]): + if enabled: + continue + if index - start > best_stop - best_start: + best_start, best_stop = start, index + start = index + 1 + if best_stop - best_start < 2: + raise ValueError("episode has no valid teleoperation run") + return slice(best_start, best_stop) + + +def load_episode(path: Path) -> EpisodeTrajectory: + source = Path(path).expanduser().resolve() + if not source.is_file() or source.suffix.lower() not in (".h5", ".hdf5"): + raise FileNotFoundError(f"episode not found: {source}") + with h5py.File(source, "r") as handle: + if str(handle.attrs.get("arm", "")) != "right_rm75": + raise ValueError("episode arm must be right_rm75") + if str(handle.attrs.get("pose_order", "")) != "x,y,z,qx,qy,qz,qw": + raise ValueError("episode pose_order is unsupported") + required = ( + "debug/tcp/final_target_pose", + "debug/timestamps/control_monotonic_ns", + "debug/control/teleop_active", + "debug/control/action_valid", + "debug/control/command_sent", + "observations/qpos", + ) + missing = [name for name in required if name not in handle] + if missing: + raise ValueError(f"episode datasets missing: {missing}") + poses = np.asarray(handle[required[0]], dtype=float) + timestamps = np.asarray(handle[required[1]], dtype=np.int64) + mask = np.logical_and.reduce( + [ + np.asarray(handle[required[2]], dtype=bool), + np.asarray(handle[required[3]], dtype=bool), + np.asarray(handle[required[4]], dtype=bool), + ] + ) + qpos = np.asarray(handle[required[5]], dtype=float) + if poses.shape != (timestamps.size, 7) or qpos.shape != (timestamps.size, 8): + raise ValueError("episode arrays have inconsistent shapes") + if not np.isfinite(poses).all() or not np.isfinite(qpos).all(): + raise ValueError("episode contains NaN/Inf") + selected = _longest_true_run(mask) + selected_times = timestamps[selected] + if np.any(np.diff(selected_times) <= 0): + raise ValueError("episode timestamps must be strictly increasing") + selected_poses = poses[selected].copy() + selected_poses[:, 3:] = np.asarray( + [_normalized_quaternion(value) for value in selected_poses[:, 3:]] + ) + return EpisodeTrajectory( + source_path=source, + times_s=(selected_times - selected_times[0]) * 1e-9, + target_poses=selected_poses, + initial_joints=qpos[selected.start, :7].copy(), + ) +``` + +- [ ] **Step 4：运行 episode 读取测试** + +Run: 聚焦 pytest 命令。 + +Expected: 4 passed。 + +- [ ] **Step 5：只读检查真实 episode** + +Run: + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -c \ + "import sys; sys.path.insert(0, 'src/xr_rm_teleop/test'); \ +from pathlib import Path; from ik_method_comparison import load_episode; \ +t=load_episode(Path('/home/robot/ACT_Data/tomato_pick/episode_0.hdf5')); \ +print(len(t.times_s), t.times_s[-1], t.initial_joints.tolist())" +``` + +Expected: 输出 483 个有效样本、约 16.07 s 时长和 7 个有限初始关节角。 + +- [ ] **Step 6：提交任务 2** + +```bash +cd /home/robot/WS_xr/src +git add xr_rm_teleop/test/ik_method_comparison.py \ + xr_rm_teleop/test/test_ik_method_comparison.py +git commit -m "feat: 读取番茄采摘目标轨迹" +``` + +--- + +### Task 3：实现指标、DLS 选择和共同命令限速 + +**Files:** +- Modify: `xr_rm_teleop/test/ik_method_comparison.py` +- Modify: `xr_rm_teleop/test/test_ik_method_comparison.py` + +- [ ] **Step 1：写指标和选择规则失败测试** + +在测试文件中加入： + +```python +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 +``` + +- [ ] **Step 2：运行测试并确认失败** + +Run: 聚焦 pytest 命令。 + +Expected: FAIL，提示指标函数或 `MethodSummary` 不存在。 + +- [ ] **Step 3：实现指标和 DLS 选择** + +在脚本中导入现有旋转、限速工具，并加入： + +```python +import math + +from xr_rm_teleop.single_arm_velocity_teleop import ( + SingleArmVelocityTeleop, + _matrix_to_quaternion, + _quaternion_to_matrix, + _so3_log, +) + + +@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 _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), + ) +``` + +- [ ] **Step 4：运行指标测试** + +Run: 聚焦 pytest 命令。 + +Expected: 7 passed。 + +- [ ] **Step 5：提交任务 3** + +```bash +cd /home/robot/WS_xr/src +git add xr_rm_teleop/test/ik_method_comparison.py \ + xr_rm_teleop/test/test_ik_method_comparison.py +git commit -m "feat: 添加逆运动学对比指标" +``` + +--- + +### Task 4：实现伪逆、DLS、当前 QP 和安全离线复放 + +**Files:** +- Modify: `xr_rm_teleop/test/ik_method_comparison.py` +- Modify: `xr_rm_teleop/test/test_ik_method_comparison.py` + +- [ ] **Step 1：写失败保持和真实模型冒烟测试** + +在测试文件中加入： + +```python +class _FakeSolver: + def __init__(self, fail: bool) -> None: + self.fail = fail + self.joint_limits = np.asarray([[-2.0, 2.0]] * 7) + + def update_joint_state(self, joints: list[float]) -> np.ndarray: + pose = np.eye(4) + pose[0, 3] = joints[0] + return pose + + def solve(self, target: np.ndarray) -> list[float]: + if self.fail: + raise RuntimeError("not converged") + return [float(target[0, 3])] + [0.0] * 6 + + +def test_replay_holds_previous_state_on_solver_failure() -> None: + trajectory = comparison.EpisodeTrajectory( + source_path=Path("episode.hdf5"), + times_s=np.asarray([0.0, 0.1]), + target_poses=np.asarray( + [ + [0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + [0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + ] + ), + initial_joints=np.zeros(7), + ) + + result = comparison.run_replay( + "fake", + _FakeSolver(fail=True), + trajectory, + max_speed=1.0, + max_acceleration=10.0, + measure_time=False, + ) + + assert result.joints == pytest.approx(np.zeros((2, 7))) + assert not result.success.any() + assert result.velocities == pytest.approx(np.zeros((2, 7))) + + +def test_real_urdf_solvers_return_finite_safe_outputs() -> None: + pytest.importorskip("placo") + urdf = TEST_DIR.parent / "models" / "dual_rm75" / "Dual_arm.urdf" + 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), + ] + for solver in solvers: + target = solver.update_joint_state(joints.tolist()) + target = target.copy() + target[0, 3] += 0.004 + result = np.asarray(solver.solve(target), dtype=float) + assert result.shape == (7,) + 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) +``` + +- [ ] **Step 2：运行测试并确认失败** + +Run: 聚焦 pytest 命令。 + +Expected: FAIL，提示 `run_replay` 或求解器适配器不存在。 + +- [ ] **Step 3：实现位姿转换和差分 IK 求解器** + +在脚本中导入现有 QP 常量和校验函数： + +```python +from xr_rm_teleop.placo_ik_solver import ( + QP_MAX_ITERATIONS, + QP_ORIENTATION_TOLERANCE_RAD, + QP_POSITION_TOLERANCE_M, + PlacoIkSolver, + _validated_transform, +) +``` + +加入以下最小适配器；不新增抽象基类： + +```python +def _pose_to_transform(pose: np.ndarray) -> np.ndarray: + values = np.asarray(pose, dtype=float) + transform = np.eye(4) + transform[:3, 3] = values[:3] + transform[:3, :3] = _quaternion_to_matrix(tuple(values[3:])) + return transform + + +def _transform_to_pose(transform: np.ndarray) -> np.ndarray: + quaternion = _matrix_to_quaternion(transform[:3, :3]) + return np.asarray([*transform[:3, 3], *quaternion], dtype=float) + + +class DifferentialIkSolver: + def __init__( + self, + urdf_path: Path, + dt: float, + method: str, + damping: float = 0.0, + ) -> None: + if method not in ("pinv", "dls"): + raise ValueError("method must be pinv or dls") + if method == "dls" and damping <= 0.0: + raise ValueError("DLS damping must be positive") + self._kinematics = PlacoIkSolver(str(urdf_path), dt, "right") + self._dt = dt + self._method = method + self._damping = float(damping) + self._actual_joints: np.ndarray | None = None + + @property + def joint_limits(self) -> np.ndarray: + return self._kinematics._joint_limits.copy() + + def update_joint_state(self, joints: list[float]) -> np.ndarray: + self._actual_joints = np.asarray(joints, dtype=float).copy() + return self._kinematics.update_joint_state(joints) + + def _set_internal_joints(self, joints: np.ndarray) -> None: + robot = self._kinematics._robot + robot.state.q[self._kinematics._q_offsets] = joints + robot.update_kinematics() + + def _errors(self, target: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + robot = self._kinematics._robot + world_base = robot.get_T_world_frame(self._kinematics._base_frame) + world_tcp = robot.get_T_world_frame(self._kinematics._tcp_frame) + world_target = world_base @ target + position = world_target[:3, 3] - world_tcp[:3, 3] + orientation = _so3_log( + world_target[:3, :3] @ world_tcp[:3, :3].T + ) + return position, orientation + + def solve(self, target: np.ndarray) -> list[float]: + if self._actual_joints is None: + raise RuntimeError("joint state must be initialized before IK solve") + target = _validated_transform(target) + actual = self._actual_joints.copy() + result = actual.copy() + try: + for _ in range(QP_MAX_ITERATIONS): + self._set_internal_joints(result) + position, orientation = self._errors(target) + if ( + np.linalg.norm(position) <= QP_POSITION_TOLERANCE_M + and np.linalg.norm(orientation) + <= QP_ORIENTATION_TOLERANCE_RAD + ): + return result.tolist() + jacobian = self._kinematics._active_tcp_jacobian() + desired_twist = np.r_[position, orientation] / self._dt + if self._method == "pinv": + joint_velocity = np.linalg.pinv(jacobian) @ desired_twist + else: + system = ( + jacobian @ jacobian.T + + self._damping**2 * np.eye(6) + ) + joint_velocity = jacobian.T @ np.linalg.solve( + system, desired_twist + ) + candidate = result + joint_velocity * self._dt + self._kinematics._validate_result(candidate, result) + result = candidate + raise RuntimeError( + f"{self._method} did not converge after {QP_MAX_ITERATIONS} iterations" + ) + except Exception: + self._set_internal_joints(actual) + raise + + +class QpSolverAdapter: + def __init__(self, solver: PlacoIkSolver) -> None: + self._solver = solver + + @property + def joint_limits(self) -> np.ndarray: + return self._solver._joint_limits.copy() + + def update_joint_state(self, joints: list[float]) -> np.ndarray: + return self._solver.update_joint_state(joints) + + def solve(self, target: np.ndarray) -> list[float]: + return self._solver.solve(target) + + +def make_qp_solver(urdf_path: Path, dt: 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, + ) + ) +``` + +`make_qp_solver` 的硬编码参数只用于先让冒烟测试变绿；Task 5 必须改为从右臂 YAML 读取，避免配置重复。 + +- [ ] **Step 4：实现逐周期复放和失败保持** + +在脚本中加入： + +```python +import time + + +@dataclass(frozen=True) +class ReplayResult: + method: str + times_s: np.ndarray + target_poses: np.ndarray + actual_poses: np.ndarray + joints: np.ndarray + velocities: np.ndarray + position_errors_m: np.ndarray + orientation_errors_rad: np.ndarray + joint_margins: np.ndarray + solve_durations_ms: np.ndarray + success: np.ndarray + command_limited: np.ndarray + + +def run_replay( + method: str, + solver, + trajectory: EpisodeTrajectory, + *, + max_speed: float, + max_acceleration: float, + measure_time: bool = True, +) -> ReplayResult: + count = trajectory.times_s.size + dt = float(np.median(np.diff(trajectory.times_s))) + joints = np.empty((count, 7)) + velocities = np.zeros((count, 7)) + actual_poses = np.empty((count, 7)) + position_errors = np.empty(count) + orientation_errors = np.empty(count) + margins = np.empty(count) + durations = np.zeros(count) + success = np.zeros(count, dtype=bool) + command_limited = np.zeros(count, dtype=bool) + current = trajectory.initial_joints.copy() + previous_velocity = np.zeros(7) + lower, upper = solver.joint_limits.T + + for index, pose in enumerate(trajectory.target_poses): + solver.update_joint_state(current.tolist()) + started = time.perf_counter_ns() + try: + candidate = np.asarray( + solver.solve(_pose_to_transform(pose)), dtype=float + ) + success[index] = True + except Exception: + candidate = current.copy() + previous_velocity = np.zeros(7) + durations[index] = ( + (time.perf_counter_ns() - started) * 1e-6 if measure_time else 0.0 + ) + if success[index]: + current, previous_velocity, command_limited[index] = ( + limit_joint_command( + target=candidate, + previous_target=current, + previous_velocity=previous_velocity, + max_speed=max_speed, + max_acceleration=max_acceleration, + dt=dt, + ) + ) + actual_transform = solver.update_joint_state(current.tolist()) + actual_pose = _transform_to_pose(actual_transform) + joints[index] = current + velocities[index] = previous_velocity + actual_poses[index] = actual_pose + position_errors[index] = np.linalg.norm(pose[:3] - actual_pose[:3]) + orientation_errors[index] = orientation_error_rad( + _quaternion_to_matrix(tuple(actual_pose[3:])), + _quaternion_to_matrix(tuple(pose[3:])), + ) + margins[index] = normalized_joint_margin(current, lower, upper) + + return ReplayResult( + method=method, + times_s=trajectory.times_s.copy(), + target_poses=trajectory.target_poses.copy(), + actual_poses=actual_poses, + joints=joints, + velocities=velocities, + position_errors_m=position_errors, + orientation_errors_rad=orientation_errors, + joint_margins=margins, + solve_durations_ms=durations, + success=success, + command_limited=command_limited, + ) +``` + +- [ ] **Step 5：运行复放和真实模型测试** + +Run: 聚焦 pytest 命令。 + +Expected: 9 passed；真实 Placo 不可用时只跳过真实模型用例，其余测试通过。 + +- [ ] **Step 6：提交任务 4** + +```bash +cd /home/robot/WS_xr/src +git add xr_rm_teleop/test/ik_method_comparison.py \ + xr_rm_teleop/test/test_ik_method_comparison.py +git commit -m "feat: 添加三种逆运动学离线复放" +``` + +--- + +### Task 5：从 YAML 读取 QP 参数并完成汇总、计时与命令行 + +**Files:** +- Modify: `xr_rm_teleop/test/ik_method_comparison.py` +- Modify: `xr_rm_teleop/test/test_ik_method_comparison.py` + +- [ ] **Step 1：写配置读取、汇总和确定性失败测试** + +在测试文件中加入： + +```python +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) +``` + +- [ ] **Step 2：运行测试并确认失败** + +Run: 聚焦 pytest 命令。 + +Expected: FAIL，提示配置或汇总函数不存在。 + +- [ ] **Step 3：实现 YAML 配置、汇总和 QP 工厂** + +在脚本中加入 `import yaml`，扩展 `MethodSummary`： + +```python +@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 + 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 +``` + +加入： + +```python +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 _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 make_qp_solver( + urdf_path: Path, + dt: float, + config: dict[str, float], +) -> QpSolverAdapter: + return QpSolverAdapter( + PlacoIkSolver( + str(urdf_path), + dt, + "right", + 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"], + ) + ) +``` + +同时更新 Task 4 的真实模型测试，将 `make_qp_solver(urdf, dt)` 改为传入 +`load_right_config(...)`，删除硬编码参数工厂。 + +- [ ] **Step 4：实现 DLS 扫描、预热和 10 次计时** + +加入单一编排函数；计时重复只替换 `solve_durations_ms` 汇总，不改变首次复放的轨迹： + +```python +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]: + 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 +``` + +在脚本顶部使用 `from dataclasses import dataclass, replace`，不要导入整个 +`dataclasses` 模块。逐采样耗时保存 10 次重复的逐点平均值；汇总最大耗时则取全部 +重复中的最大值，保证 CSV 行数仍与统一时间轴一致。 + +- [ ] **Step 5：运行聚焦测试** + +Run: 聚焦 pytest 命令。 + +Expected: 11 passed，或 10 passed、1 skipped（Placo 不可用时）。 + +- [ ] **Step 6：提交任务 5** + +```bash +cd /home/robot/WS_xr/src +git add xr_rm_teleop/test/ik_method_comparison.py \ + xr_rm_teleop/test/test_ik_method_comparison.py +git commit -m "feat: 汇总逆运动学对比结果" +``` + +--- + +### Task 6：生成 CSV、JSON、三张论文图和中文分析 + +**Files:** +- Modify: `xr_rm_teleop/test/ik_method_comparison.py` +- Modify: `xr_rm_teleop/test/test_ik_method_comparison.py` +- Create at runtime: `output/ik_comparison/episode_0/*` + +- [ ] **Step 1：写产物失败测试** + +在测试文件中用两个样本的 `ReplayResult` 调用产物函数： + +```python +def test_write_outputs_creates_consistent_files(tmp_path: Path) -> None: + result = comparison.ReplayResult( + method="qp", + times_s=np.asarray([0.0, 0.1]), + target_poses=np.tile([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (2, 1)), + actual_poses=np.tile([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], (2, 1)), + joints=np.zeros((2, 7)), + velocities=np.zeros((2, 7)), + position_errors_m=np.asarray([0.001, 0.002]), + orientation_errors_rad=np.asarray([0.001, 0.002]), + joint_margins=np.asarray([0.2, 0.2]), + solve_durations_ms=np.asarray([0.5, 0.6]), + success=np.asarray([True, True]), + command_limited=np.asarray([False, False]), + ) + summary = comparison.summarize_result(result) + + comparison.write_outputs( + tmp_path, + {"pinv": result, "dls": result, "qp": result}, + {"pinv": summary, "dls": summary, "qp": summary}, + selected_damping=0.03, + source_path=Path("episode_0.hdf5"), + git_commit="abc1234", + ) + + expected = { + "samples.csv", + "summary.json", + "figure_2_11_tracking_error.svg", + "figure_2_11_tracking_error.png", + "figure_2_12_joint_constraints.svg", + "figure_2_12_joint_constraints.png", + "figure_2_13_summary.svg", + "figure_2_13_summary.png", + "analysis_2.3.4.md", + } + assert expected == {path.name for path in tmp_path.iterdir()} + assert all((tmp_path / name).stat().st_size > 0 for name in expected) +``` + +- [ ] **Step 2：运行测试并确认失败** + +Run: 聚焦 pytest 命令。 + +Expected: FAIL，提示 `write_outputs` 不存在。 + +- [ ] **Step 3：实现 CSV 和 JSON** + +使用标准库 `csv`、`json` 和 `subprocess`。CSV 每行写一个“方法 + 时间点”，字段固定为： + +```text +method,time_s,target_x,target_y,target_z,target_qx,target_qy,target_qz,target_qw, +actual_x,actual_y,actual_z,actual_qx,actual_qy,actual_qz,actual_qw, +position_error_m,orientation_error_rad,q1,q2,q3,q4,q5,q6,q7, +qd1,qd2,qd3,qd4,qd5,qd6,qd7,joint_margin,solve_ms,success,command_limited +``` + +JSON 使用： + +```python +payload = { + "source_episode": str(source_path), + "git_commit": git_commit, + "sample_rate_hz": 90.0, + "selected_dls_damping": selected_damping, + "methods": { + name: asdict(summary) + for name, summary in summaries.items() + }, +} +``` + +脚本顶部使用 `from dataclasses import asdict, dataclass, replace`，不要额外导入整个 +`dataclasses` 模块。JSON 采用 `ensure_ascii=False, indent=2`。 + +- [ ] **Step 4：实现三张图** + +使用 Matplotlib 的 `Noto Sans CJK SC` 字体和固定色盲友好样式： + +```python +METHOD_STYLE = { + "pinv": ("Jacobian 伪逆", "#D55E00", "-"), + "dls": ("DLS", "#0072B2", "--"), + "qp": ("优化 QP", "#009E73", "-.") +} +plt.rcParams.update( + { + "font.family": "sans-serif", + "font.sans-serif": ["Noto Sans CJK SC", "Droid Sans Fallback"], + "axes.unicode_minus": False, + "figure.dpi": 120, + "savefig.dpi": 300, + } +) +``` + +实现三个独立函数： + +```python +plot_tracking_error(results, output_dir) +plot_joint_constraints(results, output_dir) +plot_summary(summaries, output_dir) +``` + +每个函数用 `fig.savefig(...svg, bbox_inches="tight")` 和 +`fig.savefig(...png, dpi=300, bbox_inches="tight")` 保存，并在完成后 `plt.close(fig)`。 + +图 2-11：上下两图分别绘制 `position_errors_m * 1000` 和 +`degrees(orientation_errors_rad)`；失败点用稀疏 `x` 标记。 + +图 2-12：上下两图分别绘制每行 `degrees(abs(velocities)).max(axis=1)` 和 +`joint_margins`；速度图绘制 `180` 的红色虚线。 + +图 2-13：`2 x 3` 六面板柱状图，依次使用位置 RMSE、姿态 RMSE、最大速度、最小 +裕度、平均/最大耗时、成功率，柱顶标注保留 2--3 位有效小数的精确值。 + +- [ ] **Step 5：生成中文分析文件** + +`analysis_2.3.4.md` 必须由汇总数据格式化生成，包含： + +1. “基于真实遥操作目标轨迹的离线运动学对比”限定； +2. episode 路径、90 Hz 重采样和同一初始关节状态； +3. DLS 候选集合、选定阻尼和同轨迹选优限制； +4. 三张图题和图注； +5. 三种方法的 RMSE、最大速度、最小裕度、耗时和成功率； +6. 当前优化 QP 的 J3、J4 和动态可操作度补充说明； +7. 只依据实际数值生成的客观比较，不使用固定“显著优于”等结论模板。 + +- [ ] **Step 6：运行产物测试** + +Run: 聚焦 pytest 命令。 + +Expected: 12 passed，或 11 passed、1 skipped。 + +- [ ] **Step 7：提交任务 6** + +```bash +cd /home/robot/WS_xr/src +git add xr_rm_teleop/test/ik_method_comparison.py \ + xr_rm_teleop/test/test_ik_method_comparison.py +git commit -m "feat: 生成逆运动学论文对比图" +``` + +--- + +### Task 7：加入 CLI，执行完整 episode 并检查正式产物 + +**Files:** +- Modify: `xr_rm_teleop/test/ik_method_comparison.py` +- Create at runtime: `output/ik_comparison/episode_0/*` + +- [ ] **Step 1：实现命令行入口** + +在脚本中加入： + +```python +import argparse +import subprocess + +SOURCE_ROOT = Path(__file__).resolve().parents[2] +DEFAULT_EPISODE = Path("/home/robot/ACT_Data/tomato_pick/episode_0.hdf5") +DEFAULT_URDF = ( + SOURCE_ROOT / "xr_rm_teleop" / "models" / "dual_rm75" / "Dual_arm.urdf" +) +DEFAULT_CONFIG = ( + SOURCE_ROOT / "xr_rm_bringup" / "config" / "right_arm_rm75.yaml" +) +DEFAULT_OUTPUT = SOURCE_ROOT / "output" / "ik_comparison" / "episode_0" + + +def main() -> None: + parser = argparse.ArgumentParser( + description="离线比较 RM75 伪逆、DLS 和当前优化 QP" + ) + parser.add_argument("--episode", type=Path, default=DEFAULT_EPISODE) + parser.add_argument("--urdf", type=Path, default=DEFAULT_URDF) + parser.add_argument("--config", type=Path, default=DEFAULT_CONFIG) + parser.add_argument("--output-dir", type=Path, default=DEFAULT_OUTPUT) + parser.add_argument("--timing-repeats", type=int, default=10) + args = parser.parse_args() + if args.timing_repeats <= 0: + parser.error("--timing-repeats must be positive") + + source = load_episode(args.episode) + trajectory = resample_trajectory(source, 90.0) + config = load_right_config(args.config) + results, summaries, damping = evaluate_methods( + trajectory, + args.urdf, + config, + args.timing_repeats, + ) + commit = subprocess.run( + ["git", "rev-parse", "HEAD"], + cwd=SOURCE_ROOT, + check=True, + capture_output=True, + text=True, + ).stdout.strip() + write_outputs( + args.output_dir, + results, + summaries, + selected_damping=damping, + source_path=source.source_path, + git_commit=commit, + ) + print(f"results written to {args.output_dir}") + + +if __name__ == "__main__": + main() +``` + +- [ ] **Step 2：检查 CLI 帮助** + +Run: + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +/home/robot/miniconda3/envs/xr/bin/python \ + src/xr_rm_teleop/test/ik_method_comparison.py --help +``` + +Expected: 显示 episode、URDF、config、output-dir 和 timing-repeats 参数，不连接真机。 + +- [ ] **Step 3：执行完整对比实验** + +Run: + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +/home/robot/miniconda3/envs/xr/bin/python \ + src/xr_rm_teleop/test/ik_method_comparison.py +``` + +Expected: 成功选择一个 DLS 固定阻尼，在 +`src/output/ik_comparison/episode_0/` 生成 9 个非空产物；终端无真机连接日志。 + +- [ ] **Step 4：核对 CSV、JSON 和图片元数据** + +Run: + +```bash +cd /home/robot/WS_xr +/home/robot/miniconda3/envs/xr/bin/python - <<'PY' +import csv +import json +from pathlib import Path +from PIL import Image + +root = Path("src/output/ik_comparison/episode_0") +with (root / "samples.csv").open(newline="", encoding="utf-8") as stream: + rows = list(csv.DictReader(stream)) +with (root / "summary.json").open(encoding="utf-8") as stream: + summary = json.load(stream) +assert len(rows) > 3 * 1400 +assert set(summary["methods"]) == {"pinv", "dls", "qp"} +assert summary["selected_dls_damping"] in [0.001, 0.003, 0.01, 0.03, 0.1, 0.3] +for name in ( + "figure_2_11_tracking_error.png", + "figure_2_12_joint_constraints.png", + "figure_2_13_summary.png", +): + with Image.open(root / name) as image: + dpi = image.info.get("dpi", (0.0, 0.0)) + assert min(dpi) >= 299.0 +print(len(rows), summary["selected_dls_damping"]) +PY +``` + +Expected: 输出超过 4200 行逐采样记录和选定阻尼；断言全部通过。 + +- [ ] **Step 5：目视检查三张 PNG** + +使用 `view_image` 分别检查： + +```text +/home/robot/WS_xr/src/output/ik_comparison/episode_0/figure_2_11_tracking_error.png +/home/robot/WS_xr/src/output/ik_comparison/episode_0/figure_2_12_joint_constraints.png +/home/robot/WS_xr/src/output/ik_comparison/episode_0/figure_2_13_summary.png +``` + +Expected: 中文无方框、标题和图例不重叠、曲线可区分、坐标轴单位完整、柱顶数值未裁切。 +如有视觉缺陷，只调整绘图函数后重新运行 Task 6 聚焦测试和完整实验。 + +- [ ] **Step 6：提交 CLI 与正式实验产物** + +```bash +cd /home/robot/WS_xr/src +git add xr_rm_teleop/test/ik_method_comparison.py \ + output/ik_comparison/episode_0 +git commit -m "docs: 生成逆运动学对比实验结果" +``` + +--- + +### Task 8：完整回归验证和最终检查 + +**Files:** +- Verify only: all files above + +- [ ] **Step 1：运行聚焦测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -m pytest \ + src/xr_rm_teleop/test/test_ik_method_comparison.py -q +``` + +Expected: 全部通过，不跳过真实 Placo 冒烟测试。 + +- [ ] **Step 2：运行遥操作姿态回归测试** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +source install/setup.bash +/home/robot/miniconda3/envs/xr/bin/python -m pytest \ + src/xr_rm_teleop/test/test_orientation_control.py -q +``` + +Expected: 全部通过。 + +- [ ] **Step 3：构建工作空间** + +```bash +cd /home/robot/WS_xr +source /opt/ros/humble/setup.bash +colcon build --symlink-install +``` + +Expected: `xr_rm_input`、`xr_rm_interfaces`、`xr_rm_teleop`、`xr_rm_mujoco` 和 +`xr_rm_bringup` 构建成功。 + +- [ ] **Step 4：检查工作树和差异** + +```bash +cd /home/robot/WS_xr/src +git status --short +git diff --check HEAD~1..HEAD +``` + +Expected: 只包含本实验脚本、测试、规格、计划和正式结果；无 `.superpowers/`、临时 +渲染、HDF5 副本、真机配置改动或无关格式化。 + +- [ ] **Step 5：核对报告文字边界** + +打开 `output/ik_comparison/episode_0/analysis_2.3.4.md`，确认明确写出： + +- 离线运动学对比，不是新的真机在线实验； +- 使用 `episode_0.hdf5` 的相同目标轨迹和初始状态； +- DLS 在本轨迹上扫描固定阻尼； +- 当前 QP 包含 J3、J4 和动态六维可操作度任务； +- 分析结论与 `summary.json` 数值一致。 + +- [ ] **Step 6：如最后检查产生修正，创建一个准确的收尾提交** + +仅在 Task 8 实际修改文件时执行： + +```bash +cd /home/robot/WS_xr/src +git add xr_rm_teleop/test/ik_method_comparison.py \ + xr_rm_teleop/test/test_ik_method_comparison.py \ + output/ik_comparison/episode_0 +git commit -m "fix: 修正逆运动学对比实验输出" +``` + +如果没有文件变化，不创建空提交。 diff --git a/docs/superpowers/specs/2026-08-24-rm75-ik-method-comparison-design.md b/docs/superpowers/specs/2026-08-24-rm75-ik-method-comparison-design.md new file mode 100644 index 0000000..b8370d7 --- /dev/null +++ b/docs/superpowers/specs/2026-08-24-rm75-ik-method-comparison-design.md @@ -0,0 +1,415 @@ +# RM75 三种逆运动学方法离线对比实验设计 + +## 1. 目标 + +基于现有番茄采摘 episode 的右臂目标位姿轨迹，在完全一致的机械臂模型、初始关节 +状态、时间轴、收敛判据和输出安全限制下，对比以下三种七自由度逆运动学方法： + +1. Jacobian Moore-Penrose 伪逆法； +2. 阻尼最小二乘法（Damped Least Squares，DLS）； +3. 当前项目中的优化 Placo QP 方法。 + +实验输出用于补充中期报告 2.3.4 节预留的三张图，并同时生成逐采样数据、汇总指标和 +可直接粘贴到报告中的中文结果分析。实验必须由真实计算结果驱动，不预设或硬编码 +“QP 更优”的结论。 + +## 2. 现有上下文 + +### 2.1 报告要求 + +中期报告 2.3.4 节已经确定： + +- 三种方法使用同一机械臂模型、初始关节状态和末端目标轨迹； +- 统计位置 RMSE、姿态 RMSE、归一化关节安全裕度、最大关节速度、求解时间和 + 求解成功率； +- 章节末尾预留三张对比图。 + +现有图号从图 2-10 跳到图 2-14，因此本实验生成图 2-11、图 2-12 和图 2-13。 + +### 2.2 当前 QP 与报告文字的差异 + +报告 2.3.2 节主要描述六维末端软任务、动能正则化、关节位置和速度限制。当前分支的 +`PlacoIkSolver` 还包含： + +- J3 初始构型软引导； +- J4 硬下限和预警区软缓冲； +- 接近奇异区时动态启用的六维可操作度任务； +- QP 失败时恢复实际关节状态并保持上一安全输出。 + +本实验使用当前优化 QP，而不是关闭上述辅助任务的基础 QP。最终分析文件需要提供一段 +方法补充文字，避免报告方法描述与对比对象不一致。 + +## 3. 范围与安全边界 + +### 3.1 本次包含 + +- 只读加载一个现有右臂 episode； +- 离线重采样目标位姿； +- 在同一 URDF 上运行三种逆运动学方法； +- 复用当前 QP 代码和右臂 YAML 参数； +- 对三种方法使用相同的输出端安全处理； +- 生成 SVG、300 dpi PNG、CSV、JSON 和中文 Markdown 分析。 + +### 3.2 本次不包含 + +- 不连接真机，不移动机械臂，不操作夹爪； +- 不启动新的 PICO 录制； +- 不修改生产遥操作节点、launch、YAML 默认值或公开 API； +- 不使用 episode 中已经记录的 QP 关节结果充当本次 QP 结果； +- 不模拟电机、通信和接触动力学； +- 不直接编辑用户提供的 PDF。 + +实验只使用当前 Conda 环境已经安装的 NumPy、h5py、Matplotlib 和 Placo，不新增项目 +依赖。 + +因此，结果应表述为“基于真实遥操作目标轨迹的离线运动学对比”，不得表述为新的真机 +在线控制对比。 + +## 4. 数据源与质量基线 + +实验固定使用： + +```text +/home/robot/ACT_Data/tomato_pick/episode_0.hdf5 +``` + +该文件的已核对属性如下： + +- 机械臂：`right_rm75`； +- 样本数：484； +- 有效时长：约 16.1 s； +- 保存采样率：约 30 Hz； +- 位姿顺序：`x,y,z,qx,qy,qz,qw`； +- 所有目标位姿、当前位姿和关节状态均为有限值； +- 目标和当前四元数范数接近 1； +- 483 帧为遥操作激活且已发送命令； +- 记录时 QP 尝试 483 次并成功 483 次； +- 132 帧触发过目标限幅，轨迹本身包含足够的约束压力。 + +只使用满足以下条件的最长连续区间： + +```text +teleop_active && action_valid && command_sent +``` + +共同末端目标取 `debug/tcp/final_target_pose`。该字段已通过原系统的工作空间限制、目标 +平滑和单帧笛卡尔步长限制，适合作为三种逆运动学方法的共同安全输入。共同初始关节角 +取有效区间第一帧的 `observations/qpos[:7]`。 + +episode 中后续 `observations/qpos`、`debug/qp/raw_target`、QP 成功标志和耗时只用于 +数据质量核对，不替代任何方法在本实验中的离线计算结果。 + +## 5. 统一复放架构 + +数据流为： + +```text +episode_0.hdf5 + -> 有效区间与共同初始状态 + -> 30 Hz 目标位姿重采样到 90 Hz + -> 伪逆 / DLS / 当前优化 QP 三路独立复放 + -> 共同输出安全层 + -> 正向运动学和逐采样指标 + -> CSV / JSON / 三张图 / 中文分析 +``` + +三种方法各自维护独立的关节状态和上一周期关节速度。每个方法的下一状态只能由该方法 +本周期的安全输出推进，三路之间不共享可变状态。 + +离线状态推进采用理想位置跟随，即共同输出限速器给出的关节目标直接作为下一 90 Hz +周期的关节状态。这一简化隔离了逆运动学方法本身，不引入未建模的电机和网络差异。 + +## 6. 目标轨迹重采样 + +原 episode 按约 30 Hz 保存，而当前遥操作控制器使用 90 Hz。重采样使用 episode 的 +`debug/timestamps/control_monotonic_ns`，目标时间轴保持原始起止时刻并以 1/90 s +采样： + +- 位置使用分段线性插值； +- 姿态使用归一化四元数的最短弧 SLERP； +- 相邻四元数点积为负时先翻转后一四元数，避免绕长弧插值； +- 第一个和最后一个重采样位姿必须与原始有效区间端点一致； +- 不对目标轨迹额外放大、延长或人工加入困难片段。 + +## 7. 三种逆运动学方法 + +### 7.1 共同任务定义 + +当前关节状态为 `q`，正向运动学得到当前 TCP 位姿 `(p, R)`，目标位姿为 +`(p_d, R_d)`。位置误差和姿态误差分别为： + +```text +e_p = p_d - p +e_R = Log(R^T R_d) +``` + +求解时的角速度误差表达必须与所用 `local_world_aligned` Jacobian 的坐标表达一致； +姿态误差大小统一使用目标与实际旋转矩阵之间的最短夹角评价。伪逆和 DLS 使用相同的 +位置、姿态反馈增益、相同 Jacobian、相同 90 Hz 步长和相同数值迭代框架。 + +三种方法对单个目标最多执行 30 次数值迭代。满足以下两个条件时记为收敛： + +```text +位置误差 <= 0.002 m +姿态误差 <= 0.005 rad +``` + +### 7.2 Jacobian 伪逆法 + +伪逆法按报告公式计算： + +```text +q_dot = pinv(J) * v_d +``` + +其中 `v_d` 由共同的六维位姿反馈误差生成。实现直接使用 NumPy 的 Moore-Penrose +伪逆，不增加零空间任务、阻尼或自适应奇异值阈值，以保持基线定义清楚。 + +### 7.3 DLS 方法 + +DLS 按报告公式计算： + +```text +q_dot = J^T * inv(J * J^T + mu^2 * I) * v_d +``` + +公式保持与报告一致；数值实现使用线性方程求解，不显式计算矩阵逆。 + +只扫描固定阻尼系数，不实现自适应 DLS。候选值使用对数尺度的小集合： + +```text +0.001, 0.003, 0.01, 0.03, 0.1, 0.3 +``` + +每个候选均完整复放 episode，先按求解成功率从高到低选择，再在成功率相同的候选中 +最小化： + +```text +位置 RMSE / 0.002 + 姿态 RMSE / 0.005 +``` + +若仍并列，选择最大关节速度更小的候选。阻尼扫描使用同一条评价轨迹，因此最终文字 +必须说明该 DLS 是“在当前轨迹上选优的固定阻尼基线”；这一口径对 DLS 较有利，不能 +将其解释为跨轨迹最优参数。 + +阻尼扫描耗时不计入三种方法的在线求解时间对比。 + +### 7.4 当前优化 QP + +QP 直接实例化现有 `xr_rm_teleop.placo_ik_solver.PlacoIkSolver`，使用 90 Hz 步长、 +当前双臂 URDF 和右臂配置中的参数： + +```text +qp_j3_reference_deg: -89.57 +qp_j3_weight: 0.0001 +qp_j4_min_deg: 10.0 +qp_j4_warn_deg: 25.0 +qp_j4_weight: 0.0001 +qp_manipulability_sigma_stop: 0.01 +qp_manipulability_sigma_warn: 0.04 +qp_manipulability_weight: 0.0001 +``` + +保留现有六维末端软任务、`1e-6` 动能正则化、URDF 关节位置和速度限制、30 次迭代、 +收敛阈值、输入变换校验、失败恢复和结果有效性检查。实验脚本不复制或重写 QP。 + +## 8. 共同输出安全层与失败处理 + +三种方法使用同一安全口径： + +1. 每次数值迭代结果必须为 7 个有限关节值； +2. 数值迭代的关节状态不得超出 URDF 位置范围； +3. 相邻数值迭代的关节变化不得超过 URDF 速度上限乘以 `1/90 s`； +4. 有效求解结果继续经过生产控制器现有的关节速度/加速度限制逻辑； +5. 输出端最大关节速度为 `180 deg/s`，最大关节加速度为 `300 deg/s^2`； +6. 未在 30 次内收敛、出现非有限值或违反硬边界时，本周期记为失败并保持上一安全 + 关节状态； +7. 失败不会停止离线复放，时间轴继续推进，并记录失败次数和最长连续失败长度。 + +QP 在优化内部主动处理关节边界；伪逆和 DLS 在每次候选步之后接受同样的硬检查。 +共同输出层不会消除算法差异：基线仍可能因候选步无效而失败或保持，QP 则可能在优化 +过程中找到满足约束的解。 + +## 9. 评价指标 + +### 9.1 末端跟踪 + +- 逐采样位置误差 `||p_d - p||`，单位为 mm； +- 逐采样姿态夹角误差，单位为 degree； +- 全轨迹位置 RMSE，报告中仍以 m 给出，图中用 mm； +- 全轨迹姿态 RMSE，报告公式使用 rad，图中用 degree。 + +### 9.2 关节运动 + +- 每个采样时刻七关节绝对速度的最大值，单位为 `deg/s`； +- 全轨迹最大关节速度； +- 超过或触发共同速度/加速度限制器的周期数； +- 按报告式 (2-28) 计算的逐采样最小归一化关节安全裕度； +- 全轨迹最小归一化关节安全裕度。 + +速度图不再使用归一化速度。当前 URDF 中右臂七个关节的速度上限均为 `3.14 rad/s` +（约 `180 deg/s`），直接展示实际速度更直观且与报告文字一致。 + +### 9.3 求解性能 + +- 收敛成功周期数和成功率； +- 失败周期数和最长连续失败长度； +- 单周期 IK 求解平均时间和最大时间，单位为 ms。 + +耗时只覆盖单次 IK 求解，不包含 HDF5 读取、重采样、指标汇总和绘图。先执行一次完整 +预热复放，再对选定参数的三种方法各重复 10 次。轨迹和非耗时指标必须在重复复放间 +保持确定；平均和最大耗时从 10 次计时复放汇总。 + +## 10. 三张图设计 + +### 10.1 图 2-11 三种逆运动学方法末端位姿跟踪误差对比 + +使用上下两个共享时间轴的子图： + +- `(a)` 位置误差时序，单位 mm； +- `(b)` 姿态误差时序，单位 degree。 + +三种方法使用固定颜色、不同线型，并在失败保持区间添加不遮挡曲线的标记。图中不绘制 +episode 原始 QP 误差曲线。 + +### 10.2 图 2-12 三种逆运动学方法关节运动约束对比 + +使用两个共享时间轴的子图： + +- `(a)` 每个时刻的最大关节速度，单位 `deg/s`，并绘制 `180 deg/s` 虚线； +- `(b)` 每个时刻的最小归一化关节安全裕度，数值越大表示离关节上下限越远。 + +### 10.3 图 2-13 三种逆运动学方法综合性能指标对比 + +使用 `2 x 3` 六个小型分组柱状图，避免不同量纲共用坐标轴： + +1. 位置 RMSE； +2. 姿态 RMSE； +3. 最大关节速度； +4. 最小归一化关节安全裕度； +5. 平均和最大求解时间； +6. 求解成功率。 + +柱顶标注精确数值。最终配色需兼顾色盲识别和灰度打印，除颜色外再使用线型、标记和 +图例区分方法。 + +## 11. 文件与产物 + +实验实现优先保持最小范围： + +```text +xr_rm_teleop/test/ik_method_comparison.py +xr_rm_teleop/test/test_ik_method_comparison.py +``` + +前者包含命令行入口、HDF5 读取、重采样、三种方法复放、指标计算和绘图；后者只覆盖 +无法由现有测试保护的新非平凡逻辑，不新增测试框架或通用评测抽象。 + +默认输出目录为： + +```text +output/ik_comparison/episode_0/ +``` + +产物包括： + +```text +samples.csv +summary.json +figure_2_11_tracking_error.svg +figure_2_11_tracking_error.png +figure_2_12_joint_constraints.svg +figure_2_12_joint_constraints.png +figure_2_13_summary.svg +figure_2_13_summary.png +analysis_2.3.4.md +``` + +`samples.csv` 使用长表结构，每行对应“方法 + 时间点”，至少包含目标位姿、实际位姿、 +位置误差、姿态误差、七关节角、七关节速度、最小安全裕度、求解耗时、成功标志和限制 +触发标志。`summary.json` 保存输入路径、Git 提交、参数、选定 DLS 阻尼、指标和产物路径， +保证结果可追溯。 + +`analysis_2.3.4.md` 使用中文撰写，包含： + +- 数据来源和离线实验口径； +- DLS 最终阻尼和选择规则； +- 三张图的建议图题与图注； +- 与式 (2-25) 至式 (2-28) 对应的数值结果； +- 对优势、代价和异常结果的客观分析； +- 当前优化 QP 相对报告 2.3.2 节的补充方法说明。 + +## 12. 错误处理 + +以下情况在生成任何正式图前立即报错： + +- episode 路径不存在或不是 HDF5； +- 必需字段或属性缺失； +- 数组长度不一致； +- 找不到至少包含两个样本的连续有效遥操作区间； +- 时间戳非严格递增； +- 位姿、关节角或四元数含 NaN/Inf； +- 四元数无法正规化； +- episode 机械臂不是 `right_rm75`； +- URDF 或当前右臂配置不存在； +- Placo 版本不是项目固定的 0.9.4； +- 任一方法没有生成与统一时间轴等长的结果； +- CSV、JSON 和绘图使用的汇总数值不一致。 + +单个目标的逆运动学失败属于实验结果，按上一安全状态保持，不中止整条轨迹。输入数据 +结构错误、模型错误和结果长度错误属于实验无效，必须中止并说明原因。 + +## 13. 测试与验证 + +### 13.1 聚焦测试 + +最小测试至少覆盖： + +- 30 Hz 到 90 Hz 重采样保持首尾位置和姿态； +- SLERP 选择最短弧并输出单位四元数； +- 姿态夹角误差在单位旋转和已知小角度下正确； +- 关节安全裕度与式 (2-28) 一致； +- 无效候选触发失败保持而不是推进状态； +- DLS 选择规则按成功率、归一化误差和最大速度依次决策； +- 汇总指标与逐采样数据一致。 + +### 13.2 真实模型冒烟验证 + +使用当前双臂 URDF 和右臂初始关节角，对三种方法各运行一小段真实目标位姿序列，确认： + +- 输出始终为有限 7 维关节值； +- 没有输出越过 URDF 关节位置边界； +- 失败时保持上一安全状态； +- 当前 QP 直接走现有 `PlacoIkSolver`，没有本地复制实现。 + +### 13.3 项目级验证 + +从工作空间根目录 `/home/robot/WS_xr` 执行，并先加载 ROS2 Humble： + +```bash +source /opt/ros/humble/setup.bash +colcon build --symlink-install +pytest src/xr_rm_teleop/test/test_orientation_control.py +``` + +随后使用项目固定的 Conda Python 运行聚焦测试和完整离线实验。验证完成后还需检查： + +- 三张 PNG 无裁切、重叠、乱码或不可辨识曲线； +- SVG 可编辑且文字完整； +- PNG 为 300 dpi； +- 图题、坐标轴、单位和图例为中文论文风格； +- `summary.json` 与图中柱顶数值一致； +- 同一输入重复运行时，除耗时外的结果一致。 + +## 14. 验收标准 + +满足以下条件才视为完成： + +1. 三种方法从完全相同的 episode 目标轨迹和初始关节状态开始； +2. 当前优化 QP 复用现有实现和右臂参数； +3. 三种方法使用同一输出安全口径，任何失败均安全保持； +4. DLS 固定阻尼选择过程和最终值可追溯； +5. 生成三张与报告公式和图号一致的正式对比图； +6. 生成完整 CSV、JSON 和中文 2.3.4 分析文字； +7. 所有实际执行的测试和构建结果如实记录； +8. 不连接真机、不修改生产控制默认值、不新增依赖和重复 QP 实现。