acRealman_xr/docs/superpowers/plans/2026-08-12-rm75-j3-simulation-calibration.md

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：写轨迹生成失败测试

测试使用以下公开接口：

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]:
    ...

断言：

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：写评分和平台选择失败测试

公开接口：

def rank_candidates(rows: list[CandidateMetrics]) -> list[CandidateMetrics]:
    ...

def choose_stable_platform(
    ranked: list[CandidateMetrics],
    scan_step_deg: float,
) -> tuple[float, tuple[float, float]]:
    ...

测试构造三个硬门槛相同的候选，断言评分严格等于：

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:

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 和标准库：

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：实现九条本侧完整轨迹

定义不可变数据类：

@dataclass(frozen=True)
class Trajectory:
    name: str
    harvest_y: float
    harvest_z: float
    waypoints: tuple[np.ndarray, ...]
    poses: tuple[np.ndarray, ...]

航点固定为：初始、预接近、采摘、预接近、筐上方、筐内、筐上方、初始。每段点数为：

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。平滑性和跟踪排名分别定义为：

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:

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：写真实模型冒烟失败测试

接口：

def evaluate_trajectory(
    arm: str,
    trajectory: Trajectory,
    variant: Variant,
    urdf_path: Path,
    initial_joint_degrees: tuple[float, ...],
) -> TrajectoryMetrics:
    ...

使用左臂从初始 TCP 沿公共 +Y 移动 1 mm 的两点轨迹，断言：

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:

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 变体

@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。求解失败时：

failures += 1
solver.update_joint_state(last_valid_joints)
current_joints = last_valid_joints.copy()

不把失败后的 Placo 内部迭代状态带到下一周期。成功状态写入 MuJoCo，并记录六维 雅可比最小奇异值、J4 余量、肘部外展量、TCP 误差、关节速度和运动代价。

• Step 5：运行全部标定脚本测试

Run:

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：实现命令行和结果输出

命令行：

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:

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:

/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:

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:

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install

Expected: 相关 ROS2 包构建成功。

• Step 3：运行姿态控制回归测试

Run:

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。