# 双臂 MuJoCo 运动学遥操作设计

## 背景与目标

当前项目已经通过 PICO/XR 手柄、两个独立的单臂遥操作节点和 Placo QP 完成双
RM75 遥操作。左右节点共同加载
`xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`，但现有 `use_mock:=true` 只在内存中
保存关节状态，没有可视化模型。

本次变更增加一个独立的 MuJoCo 运动学仿真包，使双臂在不连接真机时可以由 PICO
遥操作并可视化，也允许连接真机时把实际关节反馈同步显示在 MuJoCo 中。仿真用于更
方便地观察和改进现有 QP 算法，不替代现有控制与安全链路。

首版目标：

- 直接加载现有双臂 URDF，保持它是唯一模型源；
- 复用现有 PICO 输入、目标生成、工作空间限制和 Placo QP；
- 使用一个 MuJoCo 进程显示完整 14 关节双臂模型；
- 无真机时显示 Mock 关节状态，连接真机时显示实际关节反馈；
- 支持 Mock 模式下用左手 X、右手 A 立即 Reset 对应机械臂；
- 保持当前 mock、真机和夹爪功能的默认行为不变。

首版不实现 MuJoCo 动力学、执行器、接触、碰撞约束、双臂协同 QP、轨迹记录或
MuJoCo 对真机的任何控制。

## 目录与包边界

新增独立的 `ament_python` 包 `xr_rm_mujoco`，运行配置仍统一由
`xr_rm_bringup` 管理：

```text
src/
├── xr_rm_mujoco/
│   ├── package.xml
│   ├── setup.py
│   ├── setup.cfg
│   ├── resource/
│   │   └── xr_rm_mujoco
│   ├── xr_rm_mujoco/
│   │   ├── __init__.py
│   │   └── dual_arm_simulator.py
│   └── test/
│       └── test_dual_arm_simulator.py
├── xr_rm_bringup/
│   ├── config/
│   │   ├── dual_arm_rm75.yaml
│   │   └── dual_arm_mujoco.yaml
│   └── launch/
│       └── arm_debug.launch.py
└── xr_rm_teleop/
    ├── models/
    │   └── dual_rm75/
    │       └── Dual_arm.urdf
    └── xr_rm_teleop/
        └── single_arm_velocity_teleop.py
```

各部分职责：

- `xr_rm_mujoco` 只加载模型、接收关节状态、更新 MuJoCo `qpos` 和刷新画面；
- `xr_rm_teleop` 继续负责 PICO 映射、目标滤波、安全限幅、QP 和适配器选择，只
  增加关节目标及当前关节状态发布；
- `xr_rm_bringup` 保持唯一遥操作 launch 入口，并保存 MuJoCo 运行参数；
- `Dual_arm.urdf` 和现有 meshes 保持原位置，不复制或生成持久化 MJCF；
- 不拆出新的 description 包，不增加第二套遥操作实现。

## 模型与 MuJoCo 更新方式

`dual_arm_simulator` 从安装空间解析
`xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`，MuJoCo 直接加载该文件及其相对路径
网格。节点按 URDF 关节名称查找 MuJoCo qpos 地址，不写死 14 个数组下标。

左右首帧合法关节状态到达后，节点把状态写入相应 `qpos`，调用 `mj_forward()`
更新运动学，再由被动 viewer 显示。首版不调用 `mj_step()` 推进动力学，MuJoCo
不会生成控制量或新的关节运动。

画面按 `60 Hz` 刷新。`xr_rm_bringup/config/dual_arm_mujoco.yaml` 首版只包含：

```yaml
dual_arm_simulator:
  ros__parameters:
    render_rate_hz: 60.0
```

初始关节角不在该文件中重复配置。

## ROS 话题与状态来源

左右遥操作节点使用标准 `sensor_msgs/msg/JointState` 发布：

| 话题 | 内容 |
|---|---|
| `/xr_rm/left_rm75/joint_states` | 左臂当前适配器反馈 |
| `/xr_rm/right_rm75/joint_states` | 右臂当前适配器反馈 |
| `/xr_rm/left_rm75/joint_target` | 左臂经关节限速后实际下发的目标 |
| `/xr_rm/right_rm75/joint_target` | 右臂经关节限速后实际下发的目标 |

MuJoCo 只订阅两个 `joint_states` 话题。`joint_target` 用于后续记录和比较，不驱动
MuJoCo。消息必须携带对应侧完整的 7 个关节名称和位置，MuJoCo 按名称映射，不能
依赖消息数组顺序。

状态来源由现有 `use_mock` 唯一决定：

```text
use_mock:=true
PICO → Placo QP → MockRealManAdapter → joint_states → MuJoCo

use_mock:=false
PICO → Placo QP → RealManAdapter → 真机
                         真机实时反馈 → joint_states → MuJoCo
```

每个遥操作节点只创建一种适配器。真机连接或反馈失败时不得创建、切换或回退到
Mock。MuJoCo 不需要独立的状态来源参数；同一状态话题发现多个发布者时输出明确
报警，防止同时运行两套 launch 造成状态混合。

## 更新频率

两侧 `dual_arm_rm75.yaml` 的 `control_rate_hz` 均为 `90.0`：

- Mock 模式：Mock 状态在遥操作节点的 `90 Hz` 控制周期中读取并发布，MuJoCo
  名义关节接收频率为 `90 Hz`；
- 真机模式：RealMan 的 `realtime_push_cycle_ms: 5` 使适配器原始反馈名义频率为
  `200 Hz`，遥操作节点在 `90 Hz` 控制周期取最新快照并发布，因此 MuJoCo 名义
  关节接收频率仍为 `90 Hz`；
- 画面独立按 `render_rate_hz: 60.0` 刷新，每帧显示当时最新的 14 关节状态。

以上是名义频率，实际频率会受系统调度影响，运行时使用 `ros2 topic hz` 检查。

## 初始姿态与 A/X Reset

`dual_arm_rm75.yaml` 继续作为双臂初始姿态和控制限制的唯一配置源。Mock 适配器
创建时已经读取对应节点的 `initial_joint_pose`，将角度转换成弧度并作为初始关节
状态。左右遥操作节点初始化完成后立即各发布一帧状态，因此无真机 MuJoCo 的默认
姿态就是 YAML 中的左右初始姿态。

现有 `XrController.primary` 和按键上升沿逻辑继续复用：

```text
左手 X → 左臂立即 Reset 到左臂 initial_joint_pose
右手 A → 右臂立即 Reset 到右臂 initial_joint_pose
同时按 X、A → 双臂分别立即 Reset
```

Mock Reset 不生成平滑轨迹，而是立即更新对应 7 个关节并发布新状态。Reset 前先
退出旧的相对位姿控制；如果 Grip 仍保持按下，下一控制周期使用“当前手柄姿态 +
Reset 后机械臂姿态”自动建立新基准，随后可以继续遥操作，不要求先松开 Grip，
也不能沿用 Reset 前的相对位姿基准。

真机的 A/X 回位行为保持现状：调用 RealMan 初始位姿运动，完成后重新同步反馈，
并要求先松开 Grip 才能重新使能。该差异只由 `use_mock` 决定。

三份 RM75 配置中的 `move_to_initial_pose_on_connect` 默认继续保持 `false`。MuJoCo
初始显示和按键 Reset 都不依赖该开关，连接真机时不得默认自动移动双臂。

## 控制限制与安全隔离

Mock + MuJoCo 继续执行 `dual_arm_rm75.yaml` 中现有的软件控制约束：

- `workspace_min`、`workspace_max`、`cyl_radius_limit` 和低位圆柱限制；
- `max_linear_speed` 和 `max_orientation_speed`；
- `joint_max_speed` 和 `joint_max_acc`；
- Placo 的关节位置、速度和求解收敛检查；
- Grip 运动门控、XR/反馈超时、QP 失败保持和安全停止。

MuJoCo 直接显示已经受限的离散关节状态，本身不额外模拟连续动力学。
`max_line_speed`、`max_angular_speed`、`max_line_acc`、`max_angular_acc` 以及
`configure_safety_limits` 是 RealMan 控制器配置，只在真机适配器中调用；这不影响
上述对 Mock 同样生效的软件限位。

MuJoCo 节点只订阅状态，不发布机器人控制指令，不导入 RealMan SDK，也不创建新的
RealMan 连接。MuJoCo 启动失败、运行异常或窗口关闭不得改变真机命令、安全停止或
夹爪行为。

## 启动设计

继续使用唯一入口 `xr_rm_bringup/launch/arm_debug.launch.py`，增加默认关闭的
`use_mujoco` 参数：

| `use_mock` | `use_mujoco` | 行为 |
|---|---|---|
| `true` | `false` | 现有内存 Mock，无 MuJoCo |
| `true` | `true` | Mock + MuJoCo 双臂显示 |
| `false` | `false` | 现有双臂真机遥操作 |
| `false` | `true` | 双臂真机遥操作 + 实际反馈同步显示 |

`use_mujoco` 不参与适配器选择。首版只接受
`arm:=both use_mujoco:=true`，避免单臂启动时另一侧状态和初始姿态不明确。

无真机使用方式：

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py \
  arm:=both use_mock:=true use_mujoco:=true
```

真机同步显示方式：

```bash
ros2 launch xr_rm_bringup arm_debug.launch.py \
  arm:=both use_mock:=false use_mujoco:=true
```

第二条命令会连接并控制真机，只能在完成现有真机安全检查后使用。所有自动化和首次
集成验收只运行 `use_mock:=true`。

MuJoCo 进程使用项目现有的 XR Conda Python，因为本机 MuJoCo 与 Placo 均安装在
该环境中。未启用 `use_mujoco` 时不启动或导入 MuJoCo，新包不能让现有 mock 模式
强制依赖厂商 SDK。

## 校验与异常处理

- URDF、网格或 MuJoCo 加载失败：MuJoCo 节点明确报错并退出，现有遥操节点不改变；
- 收到关节缺失、重复、数量错误或包含 NaN/Inf 的消息：拒绝整帧并保持上一姿态；
- 尚未收齐左右首帧状态：等待并报告缺失侧，不把零位姿冒充有效初始姿态；
- 任一侧状态暂时中断：保持该侧最后有效姿态，不生成运动、不切换来源；
- 同一状态话题存在多个发布者：输出明确报警；
- viewer 关闭：只结束 MuJoCo 显示，不触发或改变机器人运动。

## 测试与验收

使用现有 pytest、ROS2 Humble 和 colcon，不增加测试框架，不连接真机。

最小自动化覆盖：

- MuJoCo 可以直接加载现有双臂 URDF；
- 14 个活动关节名称与左右 qpos 映射正确，消息顺序变化不会串臂；
- YAML 初始角度经 Mock 转换后能正确写入 MuJoCo；
- 非法关节消息不会部分污染当前状态；
- Mock A/X Reset 后回到对应 YAML 姿态；
- Reset 时 Grip 保持按下能够重新锚定并继续控制；
- 真机路径仍保留 Grip 松开后重新使能要求；
- `use_mujoco` 默认关闭，现有三种 mock/真机启动行为不变。

在工作空间根目录执行：

```bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
/home/robot/miniconda3/envs/xr/bin/python -m pytest \
  src/xr_rm_mujoco/test/test_dual_arm_simulator.py -v
pytest src/xr_rm_teleop/test/test_joint_control.py -v
pytest src/xr_rm_teleop/test/test_orientation_control.py -v
colcon build --symlink-install
```

构建后只用 Mock 启动并通过 PICO 或 sample UDP 检查：左右模型初始姿态、独立运动、
A/X Reset、Reset 后继续遥操作、话题频率和关闭 viewer 后遥操作节点状态。不得在
自动化验收中使用 `use_mock:=false`。