# 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。