407 lines
12 KiB
Markdown
407 lines
12 KiB
Markdown
# RM75 双臂 J3 参考角仿真标定实施计划
|
||
|
||
> **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:** 使用当前双臂 URDF、Placo QP 和 MuJoCo 运动学模型运行可复现的左右臂 J3 参考角粗扫与细扫，并输出评分、稳定区间和推荐角度。
|
||
|
||
**Architecture:** 新增一个仅供离线实验使用的脚本，负责生成 18 条严格六维 TCP 轨迹、建立三类 QP 试验配置、运行候选角度扫描、计算硬门槛与并列评分，并生成 CSV/JSON/Markdown 结果。生产控制器、QP 求解器和 YAML 均不修改；测试只覆盖轨迹、评分和一个真实 Placo/MuJoCo 冒烟评估。
|
||
|
||
**Tech Stack:** Python 3.10、NumPy、Placo 0.9.4、MuJoCo 3.10、pytest、ROS2 Humble 工作空间。
|
||
|
||
---
|
||
|
||
## 文件结构
|
||
|
||
- 新增 `xr_rm_teleop/test/j3_reference_calibration.py`：离线轨迹生成、QP/MuJoCo 评估、评分、结果输出和命令行入口。
|
||
- 新增 `xr_rm_teleop/test/test_j3_reference_calibration.py`：轨迹端点、严格姿态插值、百分位评分、平台选择和真实模型冒烟测试。
|
||
- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/summary.csv`：候选角度汇总。
|
||
- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/trajectories.csv`：逐轨迹指标。
|
||
- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/result.json`：机器可读结果。
|
||
- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/report.md`：左右臂推荐角度、平台区间、基线对比和最差轨迹。
|
||
|
||
### Task 1：用失败测试固定轨迹与评分行为
|
||
|
||
**Files:**
|
||
- Create: `xr_rm_teleop/test/test_j3_reference_calibration.py`
|
||
- Create: `xr_rm_teleop/test/j3_reference_calibration.py`
|
||
|
||
- [ ] **Step 1：写轨迹生成失败测试**
|
||
|
||
测试使用以下公开接口：
|
||
|
||
```python
|
||
def build_task_trajectories(
|
||
initial_world_pose: np.ndarray,
|
||
control_rate_hz: float = 90.0,
|
||
max_linear_speed: float = 0.15,
|
||
max_angular_speed: float = 0.5,
|
||
) -> list[Trajectory]:
|
||
...
|
||
```
|
||
|
||
断言：
|
||
|
||
```python
|
||
def test_build_task_trajectories_creates_nine_strict_6d_routes() -> None:
|
||
initial = np.eye(4)
|
||
initial[:3, 3] = [0.35, 0.20, 0.10]
|
||
|
||
trajectories = build_task_trajectories(initial)
|
||
|
||
assert len(trajectories) == 9
|
||
assert {(route.harvest_y, route.harvest_z) for route in trajectories} == {
|
||
(y, z)
|
||
for y in (0.30, 0.40, 0.50)
|
||
for z in (-0.30, -0.20, -0.10)
|
||
}
|
||
for route in trajectories:
|
||
assert np.allclose(route.poses[0], initial)
|
||
assert np.allclose(route.poses[-1], initial)
|
||
basket = route.waypoints[5]
|
||
assert basket[0, 3] == pytest.approx(initial[0, 3])
|
||
assert basket[1, 3] == pytest.approx(initial[1, 3])
|
||
assert basket[2, 3] == pytest.approx(initial[2, 3] - 0.40)
|
||
assert basket[:3, 2] == pytest.approx([0.0, 0.0, -1.0], abs=1e-6)
|
||
```
|
||
|
||
- [ ] **Step 2：写评分和平台选择失败测试**
|
||
|
||
公开接口：
|
||
|
||
```python
|
||
def rank_candidates(rows: list[CandidateMetrics]) -> list[CandidateMetrics]:
|
||
...
|
||
|
||
def choose_stable_platform(
|
||
ranked: list[CandidateMetrics],
|
||
scan_step_deg: float,
|
||
) -> tuple[float, tuple[float, float]]:
|
||
...
|
||
```
|
||
|
||
测试构造三个硬门槛相同的候选，断言评分严格等于：
|
||
|
||
```python
|
||
score = (
|
||
0.45 * r_sigma
|
||
+ 0.20 * r_q4
|
||
+ 0.20 * r_elbow
|
||
+ 0.10 * r_smooth
|
||
+ 0.05 * r_track
|
||
)
|
||
```
|
||
|
||
并断言连续候选均达到最高分的 98% 时返回平台中点，而不是孤立端点。
|
||
|
||
- [ ] **Step 3：运行测试并确认按预期失败**
|
||
|
||
Run:
|
||
|
||
```bash
|
||
source /opt/ros/humble/setup.bash
|
||
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
|
||
src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
|
||
```
|
||
|
||
Expected: FAIL，原因是 `j3_reference_calibration` 或公开函数尚不存在。
|
||
|
||
### Task 2：实现最小轨迹与评分模块
|
||
|
||
**Files:**
|
||
- Create: `xr_rm_teleop/test/j3_reference_calibration.py`
|
||
- Test: `xr_rm_teleop/test/test_j3_reference_calibration.py`
|
||
|
||
- [ ] **Step 1：实现旋转和 SE(3) 插值**
|
||
|
||
只使用 NumPy 和标准库：
|
||
|
||
```python
|
||
def rotation_angle(rotation: np.ndarray) -> float:
|
||
cosine = np.clip((np.trace(rotation) - 1.0) * 0.5, -1.0, 1.0)
|
||
return float(math.acos(cosine))
|
||
|
||
|
||
def rotation_vector(rotation: np.ndarray) -> np.ndarray:
|
||
angle = rotation_angle(rotation)
|
||
if angle <= 1e-12:
|
||
return np.zeros(3)
|
||
axis = np.array([
|
||
rotation[2, 1] - rotation[1, 2],
|
||
rotation[0, 2] - rotation[2, 0],
|
||
rotation[1, 0] - rotation[0, 1],
|
||
]) / (2.0 * math.sin(angle))
|
||
return axis * angle
|
||
|
||
|
||
def interpolate_pose(start: np.ndarray, end: np.ndarray, count: int) -> list[np.ndarray]:
|
||
relative = end[:3, :3] @ start[:3, :3].T
|
||
vector = rotation_vector(relative)
|
||
return [
|
||
make_pose(
|
||
start[:3, 3] + alpha * (end[:3, 3] - start[:3, 3]),
|
||
so3_exp(alpha * vector) @ start[:3, :3],
|
||
)
|
||
for alpha in np.linspace(0.0, 1.0, count + 1)[1:]
|
||
]
|
||
```
|
||
|
||
`rotation_vector` 对接近 180° 的情况使用特征向量兜底，避免工具朝下转换产生除零。
|
||
|
||
- [ ] **Step 2：实现九条本侧完整轨迹**
|
||
|
||
定义不可变数据类：
|
||
|
||
```python
|
||
@dataclass(frozen=True)
|
||
class Trajectory:
|
||
name: str
|
||
harvest_y: float
|
||
harvest_z: float
|
||
waypoints: tuple[np.ndarray, ...]
|
||
poses: tuple[np.ndarray, ...]
|
||
```
|
||
|
||
航点固定为：初始、预接近、采摘、预接近、筐上方、筐内、筐上方、初始。每段点数为：
|
||
|
||
```python
|
||
duration = max(
|
||
translation_distance / max_linear_speed,
|
||
rotation_distance / max_angular_speed,
|
||
)
|
||
steps = max(1, math.ceil(duration * control_rate_hz))
|
||
```
|
||
|
||
- [ ] **Step 3：实现百分位排名和并列评分**
|
||
|
||
同值获得同一百分位，单一取值获得 1.0。平滑性和跟踪排名分别定义为：
|
||
|
||
```python
|
||
r_smooth = 0.5 * rank_low(motion_cost) + 0.5 * rank_low(max_joint_speed)
|
||
r_track = 0.5 * rank_low(max_position_error) + 0.5 * rank_low(max_orientation_error)
|
||
```
|
||
|
||
硬门槛按 `(N_fail, -N_complete)` 字典序先筛选；只有满足位置误差、姿态误差、J4、
|
||
关节限位、速度和跳变条件的候选进入综合评分。
|
||
|
||
- [ ] **Step 4：运行测试确认通过**
|
||
|
||
Run:
|
||
|
||
```bash
|
||
source /opt/ros/humble/setup.bash
|
||
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
|
||
src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
|
||
```
|
||
|
||
Expected: 轨迹与评分测试 PASS。
|
||
|
||
### Task 3：用失败测试固定真实 Placo/MuJoCo 单轨迹评估
|
||
|
||
**Files:**
|
||
- Modify: `xr_rm_teleop/test/test_j3_reference_calibration.py`
|
||
- Modify: `xr_rm_teleop/test/j3_reference_calibration.py`
|
||
|
||
- [ ] **Step 1：写真实模型冒烟失败测试**
|
||
|
||
接口：
|
||
|
||
```python
|
||
def evaluate_trajectory(
|
||
arm: str,
|
||
trajectory: Trajectory,
|
||
variant: Variant,
|
||
urdf_path: Path,
|
||
initial_joint_degrees: tuple[float, ...],
|
||
) -> TrajectoryMetrics:
|
||
...
|
||
```
|
||
|
||
使用左臂从初始 TCP 沿公共 `+Y` 移动 1 mm 的两点轨迹，断言：
|
||
|
||
```python
|
||
assert metrics.cycles == 2
|
||
assert metrics.failures == 0
|
||
assert math.isfinite(metrics.min_sigma)
|
||
assert metrics.min_q4_margin_deg > 0.0
|
||
assert metrics.max_position_error_m <= 2e-3
|
||
assert metrics.max_orientation_error_rad <= 5e-3
|
||
```
|
||
|
||
测试还将求得的七关节状态写入 `DualArmKinematicModel` 并断言按名称读回一致。
|
||
|
||
- [ ] **Step 2：运行冒烟测试并确认按预期失败**
|
||
|
||
Run:
|
||
|
||
```bash
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
|
||
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
|
||
src/xr_rm_teleop/test/test_j3_reference_calibration.py::test_evaluate_trajectory_uses_real_placo_and_mujoco -q
|
||
```
|
||
|
||
Expected: FAIL，原因是评估器尚未实现。
|
||
|
||
- [ ] **Step 3：实现三类 QP 变体**
|
||
|
||
```python
|
||
@dataclass(frozen=True)
|
||
class Variant:
|
||
name: str
|
||
q3_reference_deg: float | None
|
||
enable_manipulability: bool
|
||
q4_min_deg: float | None
|
||
```
|
||
|
||
- `original`：三个可选项均关闭；
|
||
- `manip_j4`：位置可操作度权重 `1e-4`，J4 硬下限 10°；
|
||
- `q3_<angle>`：在 `manip_j4` 基础上加入 J3 软任务，权重 `1e-5`。
|
||
|
||
J4 约束使用 Placo 0.9.4 的 `add_joint_space_half_spaces_constraint(A, b)`，构造
|
||
`-q4 <= -q4_min`。J3 使用 `add_joints_task()`；位置可操作度使用
|
||
`add_manipulability_task(tcp_frame, "position", 1.0)`。
|
||
|
||
- [ ] **Step 4：实现逐周期评估与失败保持**
|
||
|
||
每个目标点前将上一有效关节状态同步给 Placo。求解失败时：
|
||
|
||
```python
|
||
failures += 1
|
||
solver.update_joint_state(last_valid_joints)
|
||
current_joints = last_valid_joints.copy()
|
||
```
|
||
|
||
不把失败后的 Placo 内部迭代状态带到下一周期。成功状态写入 MuJoCo，并记录六维
|
||
雅可比最小奇异值、J4 余量、肘部外展量、TCP 误差、关节速度和运动代价。
|
||
|
||
- [ ] **Step 5：运行全部标定脚本测试**
|
||
|
||
Run:
|
||
|
||
```bash
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
|
||
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
|
||
src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
|
||
```
|
||
|
||
Expected: 全部 PASS。
|
||
|
||
### Task 4：运行粗扫、细扫并生成结果
|
||
|
||
**Files:**
|
||
- Modify: `xr_rm_teleop/test/j3_reference_calibration.py`
|
||
- Generate: `docs/superpowers/results/2026-08-12-rm75-j3-calibration/*`
|
||
|
||
- [ ] **Step 1：实现命令行和结果输出**
|
||
|
||
命令行：
|
||
|
||
```bash
|
||
python j3_reference_calibration.py \
|
||
--urdf <path> \
|
||
--config <dual_arm_rm75.yaml> \
|
||
--output-dir <directory> \
|
||
--phase coarse|fine|all
|
||
```
|
||
|
||
粗扫结束后对每侧选择最高分候选，在其 ±10°、原扫描边界内以 2° 细扫。CSV 使用
|
||
`csv.DictWriter`，JSON 使用 `json.dump`，Markdown 报告由同一汇总对象生成，不新增依赖。
|
||
|
||
- [ ] **Step 2：运行完整仿真标定**
|
||
|
||
Run:
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
|
||
/home/robot/miniconda3/envs/xr/bin/python \
|
||
src/xr_rm_teleop/test/j3_reference_calibration.py \
|
||
--urdf src/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf \
|
||
--config src/xr_rm_bringup/config/dual_arm_rm75.yaml \
|
||
--output-dir src/docs/superpowers/results/2026-08-12-rm75-j3-calibration \
|
||
--phase all
|
||
```
|
||
|
||
Expected: 左右臂粗扫和细扫完成；输出两个基线、全部候选、推荐角度和平台区间。
|
||
|
||
- [ ] **Step 3：检查结果完整性**
|
||
|
||
Run:
|
||
|
||
```bash
|
||
/home/robot/miniconda3/envs/xr/bin/python - <<'PY'
|
||
import json
|
||
from pathlib import Path
|
||
|
||
path = Path('src/docs/superpowers/results/2026-08-12-rm75-j3-calibration/result.json')
|
||
data = json.loads(path.read_text(encoding='utf-8'))
|
||
assert set(data['arms']) == {'left', 'right'}
|
||
for arm in data['arms'].values():
|
||
assert arm['coarse_candidates']
|
||
assert arm['fine_candidates']
|
||
assert arm['recommended_reference_deg'] is not None
|
||
assert len(arm['stable_interval_deg']) == 2
|
||
print('result integrity: OK')
|
||
PY
|
||
```
|
||
|
||
Expected: `result integrity: OK`。
|
||
|
||
### Task 5：工作空间验证与结果复核
|
||
|
||
**Files:**
|
||
- Verify only.
|
||
|
||
- [ ] **Step 1：运行新增测试和相关现有测试**
|
||
|
||
Run:
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
|
||
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
|
||
src/xr_rm_teleop/test/test_j3_reference_calibration.py \
|
||
src/xr_rm_teleop/test/test_placo_transforms.py \
|
||
src/xr_rm_mujoco/test/test_dual_arm_simulator.py -q
|
||
```
|
||
|
||
Expected: 全部 PASS。
|
||
|
||
- [ ] **Step 2：按项目规则构建工作空间**
|
||
|
||
Run:
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
colcon build --symlink-install
|
||
```
|
||
|
||
Expected: 相关 ROS2 包构建成功。
|
||
|
||
- [ ] **Step 3：运行姿态控制回归测试**
|
||
|
||
Run:
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
pytest src/xr_rm_teleop/test/test_orientation_control.py -q
|
||
```
|
||
|
||
Expected: 全部 PASS。
|
||
|
||
- [ ] **Step 4：人工复核结果报告**
|
||
|
||
确认：
|
||
|
||
- 每侧确有 9 条完整轨迹；
|
||
- `original`、`manip_j4` 和 J3 候选均存在；
|
||
- 推荐角来自硬门槛通过集合；
|
||
- 平台选择符合 98% 规则；
|
||
- 报告明确列出失败轨迹，且没有把失败更多的候选排到前面；
|
||
- 没有修改生产控制器和 YAML。
|