# 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: 修正逆运动学对比实验输出" ``` 如果没有文件变化，不创建空提交。