250 lines
10 KiB
Markdown
250 lines
10 KiB
Markdown
# 双臂 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`。
|