# RM75-B 第三阶段双臂 QP 遥操验证记录

日期：2026-06-30  
随机种子：`20260630`  
快速验证机械臂：`right`  
MuJoCo：`3.10.0`  
RealMan API2 C API：`v1.1.5`

## 验收结果

完整 headless 基准全部通过，记录失败样本为 0。

| 检查项 | 样本 | 结果 |
|---|---:|---:|
| 右臂 MuJoCo/Pinocchio/Algo FK | 2,000 | PASS |
| 右臂近邻 IK | 200 / 200 | PASS |
| 右臂连续 IK | 1,000 / 1,000 | PASS |
| 双臂工具 TCP 初始化 | 左右各 1 | PASS |
| 双 grip 相对位姿控制 | 10 帧 | PASS |
| RGB/segmentation 与双臂最终帧 | 7 张 | PASS |
| ROS2 双臂 QP MuJoCo 启动冒烟测试 | 1 次 | PASS |

关键测量值：

- MuJoCo/Pinocchio 最大误差：`5.38e-13 m`、`1.52e-12 rad`。
- MuJoCo/RealMan Algo 最大误差：`0.003721 mm`、`0.000964 deg`。
- 右臂近邻 IK 成功率：`100%`，P99/最大耗时：`3.61 ms / 5.18 ms`。
- 右臂连续 IK 成功率：`100%`，最大关节步长：`0.153 deg`。
- grip 首帧左右关节变化均小于 `1e-10 rad`。

## 初始 TCP

`dual_arm_rm75.yaml` 的 `initial_tcp_pose` 被视为活动工具 TCP。左臂使用 `minisci`
的 `190 mm` 工具变换，右臂使用 `omnipic` 的 `160 mm` 工具变换，再由 QP IK 求出
初始关节角。

| 机械臂 | 位置残差 | 姿态残差 |
|---|---:|---:|
| left | `0.226 mm` | `0.00714 deg` |
| right | `0.422 mm` | `0.01259 deg` |

配置中的旧 `initial_joint_pose` 无法在标准 RM75-B 模型中复现这些 TCP：计入工具后，
左、右位置误差约为 `123 mm`、`95 mm`。因此它们只进入诊断报告，不用于 MuJoCo 初始化。

## 控制边界

独立核心按以下顺序处理每个控制周期：

```text
XrController-compatible sample
-> grip 起点锁定与左右坐标映射
-> TCP 工作空间、滤波和速度限制
-> 工具 TCP 到法兰变换
-> Pinocchio SE(3) + OSQP IK
-> 关节限位与关节速度限制
-> RobotBackend.command_joint_positions()
```

正常松开 grip 只停止对应侧；任一活动臂输入超时、IK 失败或后端异常都会使双臂进入
FAULT 并共同停止。恢复前必须收到左右两侧新的 grip 释放消息。

## ROS2 启动验证

在 `qp` Conda 环境和已构建的 ROS2 Humble 工作空间中执行：

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch xr_rm_bringup dual_arm_qp_sim.launch.py show_viewer:=false
```

验证中使用 `mujoco_gl` 的默认值 `glfw`，无需在命令行显式传入。UDP receiver 成功监听
`0.0.0.0:15000`，左右臂分别以 `0.226 mm`、`0.422 mm` 的位置残差完成初始化，随后
双臂控制节点进入 `dual-arm QP teleop ready` 状态。该测试只验证节点启动和 MuJoCo
初始化，不包含 XR 数据输入、持续遥操作或真实机械臂行为。

第三阶段只实现 `MujocoRobot`。`RealmanRobot` 仅由 `RobotBackend` 协议约束，尚未实现，
也未进行真实机械臂测试。本阶段不验证碰撞规避、动力学跟踪或硬件安全。
