docs: 添加双臂 MuJoCo 设计与计划

2026-08-04 11:42:07 +08:00
parent 36f82fe2db
commit 1df09fef63
2 changed files with 1480 additions and 0 deletions
@@ -0,0 +1,249 @@
+# 双臂 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`。