YikaiFu-cart/acRealman_xr
1
1
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
f30aac547b6fe84703044c66b30b70720ee4d40a
acRealman_xr/docs/superpowers/plans/2026-08-24-rm75-ik-method-comparison.md
T

1594 lines
50 KiB
Markdown
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
# 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.10、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: 修正逆运动学对比实验输出"
```
如果没有文件变化，不创建空提交。
Reference in New Issue View Git Blame Copy Permalink