acRealman_xr/docs/superpowers/specs/2026-08-04-dual-arm-mujoco-teleoperation-design.md

双臂 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 管理：

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 首版只包含：

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 唯一决定：

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 和按键上升沿逻辑继续复用：

左手 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，避免单臂启动时另一侧状态和初始姿态不明确。

无真机使用方式：

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

真机同步显示方式：

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/真机启动行为不变。

在工作空间根目录执行：

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。