fix: 更新 README 和配置文件，调整模式和工具设置

README.md

@@ -9,6 +9,8 @@ PICO/XR 双手柄 UDP JSON
   -> xr_rm_teleop/single_arm_velocity_teleop
   -> Placo QP 单步逆解
   -> 左右 RM75 七关节角透传控制
+  -> /xr_rm/<arm_name>/joint_states
+  -> 可选 xr_rm_mujoco/dual_arm_simulator
   -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
 ```
 
@@ -20,6 +22,7 @@ PICO/XR 双手柄 UDP JSON
 
 - PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
 - 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
+- 使用现有双臂 URDF 的 MuJoCo 运动学显示，可由 Mock 或真机反馈同步双臂姿态。
 - RM75 真机连接适配，包含关节反馈缓存、`rm_movej_canfd` 关节透传、安全速度/加速度配置、可选初始化点位移动。
 - Placo 0.9.4 RM75 QP 逆解；收到首帧有效关节反馈后才启用，求解失败时保留上一组有效关节目标。
 - 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
@@ -41,12 +44,13 @@ src/
 ├── docs/superpowers/               # Superpowers 设计与实施计划
 ├── xr_rm_bringup/
 │   ├── config/
+│   │   ├── dual_arm_mujoco.yaml    # MuJoCo 显示刷新参数
 │   │   ├── dual_arm_rm75.yaml      # 双臂配置：left_arm_teleop 与 right_arm_teleop
 │   │   ├── left_arm_rm75.yaml      # 左臂单独调试配置
 │   │   ├── right_arm_rm75.yaml     # 右臂单独调试配置
 │   │   └── peripherals_rm75.yaml   # 左右臂末端外设配置
 │   ├── launch/
-│   │   └── arm_debug.launch.py     # 统一入口：arm:=left/right/both, use_mock:=true/false
+│   │   └── arm_debug.launch.py     # 统一入口：单臂/双臂、Mock/真机、可选 MuJoCo
 │   └── tools/
 │       ├── launcher_ui.py          # 图形化调试启动面板
 │       └── realman_dual_arm_state_monitor.py
@@ -62,6 +66,9 @@ src/
 ├── xr_rm_interfaces/
 │   └── msg/
 │       └── XrController.msg        # 手柄状态与位姿
+├── xr_rm_mujoco/
+│   └── xr_rm_mujoco/
+│       └── dual_arm_simulator.py   # 双臂 URDF 运动学映射与 MuJoCo viewer
 └── xr_rm_teleop/
     ├── models/
     │   ├── rm75/                   # 旧 RM75 模型资源（launch 不再选用）
@@ -97,7 +104,7 @@ source install/setup.bash
 
 真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。
 
-遥操作节点固定由 `/home/robot/miniconda3/envs/xr/bin/python` 启动，并复用其中的 Python 3.10、Placo 0.9.4、Pinocchio 3.7.0 和 NumPy 2.2.6。`ros2`、`colcon` 和 `udp_controller_receiver` 仍使用系统 Python。禁止通过 `pip --user`、`sudo pip` 或系统安装升级 Placo、Pinocchio、EigenPy 和 NumPy。
+遥操作和 MuJoCo 节点固定由 `/home/robot/miniconda3/envs/xr/bin/python` 启动，并复用其中的 Python 3.10、Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0。`ros2`、`colcon`、pytest 和 `udp_controller_receiver` 仍使用系统 Python。禁止通过 `pip --user`、`sudo pip` 或系统安装升级 Placo、Pinocchio、EigenPy 和 NumPy。
 
 只读检查 Placo 版本：
 
@@ -108,6 +115,20 @@ source install/setup.bash
 
 输出必须为 `0.9.4`。
 
+同时检查 MuJoCo：
+
+```bash
+/home/robot/miniconda3/envs/xr/bin/python -c \
+  "import mujoco; print(mujoco.__version__)"
+```
+
+当前验证版本为 `3.10.0`。系统 pytest 会通过 `xr_rm_mujoco/test/conftest.py` 复用该固定 XR 环境中的 MuJoCo，因此新包可直接按 ROS2 标准方式测试：
+
+```bash
+colcon test --packages-select xr_rm_mujoco --event-handlers console_direct+
+colcon test-result --verbose
+```
+
 如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：
 
 ```bash
@@ -133,27 +154,27 @@ source install/setup.bash
 ros2 run xr_rm_bringup launcher_ui
 ```
 
-面板顶部的 `Mode` 分为五类：
+面板顶部的 `Mode` 分为四类：
 
-- `Simulation`：左臂 mock、右臂 mock、双臂 mock、sample UDP 发送、one-click mock demo、controller 位置/频率监控。
+- `Simulation`：双臂 mock、XRoboToolkit bridge、双手 sample UDP 和 controller 频率监控。
-- `Left Arm`：左臂网络 ping、左臂真机 launch、左手 sample UDP。
+- `MuJoCo`：双臂 Mock/真机 MuJoCo launch、XRoboToolkit bridge 和 controller 频率监控；真机命令会连接两台 RM75。
-- `Right Arm`：右臂网络 ping、右臂真机 launch、右手 sample UDP。
+- `Real Hardware`：左右臂网络 ping、左臂/右臂/双臂真机 launch、XRoboToolkit bridge 和左右夹爪开合。
-- `Dual Arm`：左右臂 ping、双臂真机 launch、双手 sample UDP。
+- `Diagnostics`：`ros2 doctor --report`、四个核心包的 `ros2 pkg prefix`、controller 位置/频率监控。
-- `Diagnostics`：`ros2 doctor --report` 和核心包的 `ros2 pkg prefix` 检查。
 
 常用按钮：
 
 - `Run Selected`：运行当前选中的命令。双击列表项也可以运行。
-- `Check Env`：检查 ROS2 Humble、工作空间 build、终端、核心 ROS 包、睿尔曼 API2。
+- `Check Env`：检查 ROS2 Humble、工作空间 build、终端、四个核心 ROS 包、睿尔曼 API2。
-- `Stop All`：结束由本工作空间启动的 launch、sample sender、topic monitor、相关 ROS 节点和终端窗口。
+- `Stop All`：结束由本工作空间启动的 launch、sample sender、topic monitor、MuJoCo viewer、相关 ROS 节点和终端窗口。
+
+`Stop All` 会保留现有 PC Service；点击启动器窗口 `X` 并确认退出时会额外停止 PC Service。两条清理路径都会停止 `dual_arm_simulator`，关闭 MuJoCo viewer。
 
 每个模式都会附带基础监控入口：
 
 - `Open Controller Topic Monitor`：同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
-- `Open Target Velocity Monitor`：同时查看 `/xr_rm/left_rm75/cmd_vel` 和 `/xr_rm/right_rm75/cmd_vel`；该话题表示目标位姿变化率，仅用于调试。
 - `Open ROS Topic/Node List Monitor`：每秒刷新 `ros2 topic list` 和 `ros2 node list`。
 
-`Simulation` 模式还提供 `Open Controller Position Monitor` 和 `Open Controller Hz Monitor`，用于快速看手柄位置字段和接收频率。
+`Simulation` 和 `MuJoCo` 模式还提供 `Open Controller Hz Monitor`；`Diagnostics` 同时提供 controller 位置与频率监控。
 
 分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：
 
@@ -223,12 +244,13 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 
 ## Launch 入口说明
 
-`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 mock、单臂真机和双臂真机按钮都调用它。
+`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 Simulation、MuJoCo 和 Real Hardware launch 命令都调用它。
 
 常用参数：
 
 - `arm`：`left`、`right`、`both`，默认 `right`。
 - `use_mock`：`true` 不连接真机，`false` 连接 RM75。
+- `use_mujoco`：`true` 额外启动双臂 MuJoCo 显示，默认 `false`，仅支持 `arm:=both`。
 - `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
 - `udp_port`：UDP 监听端口，默认 `15000`。
 - `udp_timer_hz`：UDP receiver 轮询频率，默认 `200.0`。
@@ -236,6 +258,32 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 机器人 IP/端口、控制频率、CANFD、限速、工具和初始化位姿等行为参数只由对应 YAML
 配置，launch 不再提供同名覆盖项。
 
+## MuJoCo 双臂仿真
+
+无真机时，由 Mock 关节状态驱动 MuJoCo：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=both use_mock:=true use_mujoco:=true
+```
+
+连接真机时，由两台 RM75 的实际关节反馈同步 MuJoCo。下面命令会连接真机，执行前必须完成真机安全检查：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=both use_mock:=false use_mujoco:=true
+```
+
+桌面 UI 的 `MuJoCo` 模式分别提供上述 Mock 和真机命令，并明确标记会连接真机的 `Dual Arm MuJoCo Real Hardware Launch`。
+
+MuJoCo 只订阅当前关节状态，不参与控制，也不向真机下发指令：
+
+- `/xr_rm/left_rm75/joint_states`、`/xr_rm/right_rm75/joint_states`：当前适配器反馈；Mock 与真机模式均按控制周期约 `90 Hz` 发布。真机底层原始反馈周期仍为 `5 ms`（约 `200 Hz`），由遥操作节点按 `90 Hz` 采样发布。
+- `/xr_rm/left_rm75/joint_target`、`/xr_rm/right_rm75/joint_target`：经 QP 和现有限制处理后的目标关节角，仅用于调试，MuJoCo 不订阅。
+- MuJoCo viewer 默认按 `dual_arm_mujoco.yaml` 中的 `60 Hz` 刷新。其初始姿态直接来自 `dual_arm_rm75.yaml` 的 `initial_joint_pose`，不会在 MuJoCo YAML 中重复保存。
+
+Mock 模式的遥操作目标仍经过 `dual_arm_rm75.yaml` 中的工作空间、圆柱、线速度、角速度、关节速度/加速度、超时和停止限制。左手 X、右手 A 分别立即复位对应 Mock 机械臂；Grip 保持按下时，下一控制周期会重新锚定并继续遥操作。真机复位完成后仍需松开 Grip 才能恢复遥操作，且 `move_to_initial_pose_on_connect` 保持为 `false`，连接真机不会自动移动。
+
 ## 配置文件说明
 
 `xr_rm_bringup/config/dual_arm_rm75.yaml` 是双臂配置主文件，包含两个 ROS 节点命名空间：
@@ -245,6 +293,8 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 
 `left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。
 
+`dual_arm_mujoco.yaml` 只保存 MuJoCo viewer 刷新频率；双臂初始关节角和遥操作限制继续统一读取 `dual_arm_rm75.yaml`。
+
 `xr_rm_bringup/config/peripherals_rm75.yaml` 保存真实控制器使用的末端工具坐标、负载和左右臂外设选择。左臂 `scissorgripper: 2` 是外设选择值，选择 `minisci`，TCP 的 Z 向偏移为 `0.165 m`；右臂 `scissorgripper: 1`，选择 `omnipic`，TCP 的 Z 向偏移为 `0.14 m`。真机连接阶段仍会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。
 
 Placo 使用 `xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`。左右 ROS 节点分别创建独立 solver：左臂从 `scissor_base_link` 到 `scissor_scissor_tcp`，并 mask 右臂；右臂从 `omnipic_base_link` 到 `omnipic_OmniPic_tcp`，并 mask 左臂。节点目标仍在各自局部基坐标系中，现有 PICO 映射不改为公共坐标系。
@@ -296,7 +346,7 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: tr
 ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
 ```
 
-桌面 UI 的 `Left Arm` 和 `Right Arm` 模式里也有对应的 Tool Open/Close 命令项；`Dual Arm` 真机模式下可直接通过左右手柄 `trigger` 分别切换夹爪。
+桌面 UI 的 `Real Hardware` 模式提供 `Left/Right Gripper Open/Close` 命令项；运行双臂真机 launch 时也可直接通过左右手柄 `trigger` 分别切换夹爪。
 
 ## UDP 数据格式
 
@@ -443,7 +493,7 @@ ros2 topic echo /xr/right_controller --field trigger
 6. 小角度转动手柄，确认 `/xr_rm/<arm>/target_pose` 姿态和 `/xr_rm/<arm>/cmd_vel.twist.angular` 变化符合预期。
 7. 点击对应 `trigger`，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
 8. 确认松开 `grip` 后机械臂慢停，`/xr_rm/<arm>/cmd_vel` 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
-9. 左右臂都确认后，再进入双臂模式。
+9. 左右臂都确认后，再运行 `Dual Arm RealMan Launch`。
 
 当前项目没有双臂碰撞检测/避障。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
 

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

@@ -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`。

xr_rm_bringup/config/dual_arm_mujoco.yaml

@@ -0,0 +1,4 @@
+# 双 RM75 MuJoCo 运动学显示参数。初始姿态和控制限制仍由 dual_arm_rm75.yaml 管理。
+dual_arm_simulator:
+  ros__parameters:
+    render_rate_hz: 60.0

xr_rm_bringup/config/peripherals_rm75.yaml

@@ -9,7 +9,7 @@ set_initial_tool_state: false
 tools_in_ee:
   scissor:
     # x, y, z, qx, qy, qz, qw
-    pose: [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+    pose: [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
     # mass, center_x, center_y, center_z, reserved...
     load: [0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]
   omnipic:
@@ -24,6 +24,6 @@ tools_in_ee:
 
 arms:
   left:
-    scissorgripper: 2
+    scissorgripper: 0
   right:
     scissorgripper: 1

xr_rm_bringup/launch/arm_debug.launch.py

@@ -8,7 +8,7 @@
 from pathlib import Path
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -47,6 +47,9 @@ def _udp_receiver_node() -> Node:
         executable="udp_controller_receiver",
         name="udp_controller_receiver",
         output="screen",
+        on_exit=Shutdown(
+            reason="XR UDP receiver exited; stopping arm_debug launch"
+        ),
         parameters=[{
             "udp_host": LaunchConfiguration("udp_host"),
             "udp_port": LaunchConfiguration("udp_port"),
@@ -57,6 +60,26 @@ def _udp_receiver_node() -> Node:
     )
 
 
+def _mujoco_node() -> Node:
+    """启动只读双臂 MuJoCo 运动学显示节点。"""
+    return Node(
+        package="xr_rm_mujoco",
+        executable="dual_arm_simulator",
+        name="dual_arm_simulator",
+        output="screen",
+        prefix=[XR_PYTHON],
+        parameters=[
+            _config_file("dual_arm_mujoco.yaml"),
+            {"robot_urdf_path": _dual_rm75_urdf()},
+        ],
+    )
+
+
+def _validate_mujoco_mode(arm: str, use_mujoco: bool) -> None:
+    if use_mujoco and arm != "both":
+        raise ValueError("use_mujoco:=true requires arm:=both")
+
+
 def _single_arm_node(
     arm: str,
     use_mock: bool,
@@ -138,15 +161,21 @@ def _launch_setup(context, *args, **kwargs):
         )
     arm = LaunchConfiguration("arm").perform(context).strip().lower()
     use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
+    use_mujoco = _as_bool(
+        LaunchConfiguration("use_mujoco").perform(context)
+    )
 
     if arm not in ("left", "right", "both"):
         raise ValueError("arm must be one of: left, right, both")
+    _validate_mujoco_mode(arm, use_mujoco)
 
     nodes = [_udp_receiver_node()]
     if arm == "both":
         nodes.extend(_dual_arm_nodes(use_mock))
     else:
         nodes.append(_single_arm_node(arm, use_mock))
+    if use_mujoco:
+        nodes.append(_mujoco_node())
     return nodes
 
 
@@ -156,6 +185,8 @@ def generate_launch_description() -> LaunchDescription:
         DeclareLaunchArgument("arm", default_value="right"),
         # true 时只跑 mock，不连接 RM75；false 时通过 RealMan SDK 连接真机。
         DeclareLaunchArgument("use_mock", default_value="true"),
+        # true 时额外启动只读 MuJoCo 双臂显示，默认不改变现有启动行为。
+        DeclareLaunchArgument("use_mujoco", default_value="false"),
         # UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
         DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
         DeclareLaunchArgument("udp_port", default_value="15000"),

xr_rm_bringup/package.xml

@@ -10,6 +10,7 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <exec_depend>xr_rm_input</exec_depend>
+  <exec_depend>xr_rm_mujoco</exec_depend>
   <exec_depend>xr_rm_teleop</exec_depend>
   <exec_depend>python3-tk</exec_depend>
 

xr_rm_bringup/test/test_arm_debug_launch.py

@@ -0,0 +1,43 @@
+import importlib.util
+from pathlib import Path
+
+import pytest
+from launch import LaunchContext
+from launch.actions import DeclareLaunchArgument, Shutdown
+from launch.utilities import perform_substitutions
+
+
+MODULE_PATH = Path(__file__).parents[1] / "launch" / "arm_debug.launch.py"
+SPEC = importlib.util.spec_from_file_location("arm_debug_launch", MODULE_PATH)
+arm_debug_launch = importlib.util.module_from_spec(SPEC)
+assert SPEC.loader is not None
+SPEC.loader.exec_module(arm_debug_launch)
+
+
+def test_launch_declares_mujoco_disabled_by_default() -> None:
+    description = arm_debug_launch.generate_launch_description()
+    arguments = {
+        entity.name: entity
+        for entity in description.entities
+        if isinstance(entity, DeclareLaunchArgument)
+    }
+
+    assert "use_mujoco" in arguments
+    assert perform_substitutions(
+        LaunchContext(),
+        arguments["use_mujoco"].default_value,
+    ) == "false"
+
+
+def test_mujoco_mode_requires_both_arms() -> None:
+    arm_debug_launch._validate_mujoco_mode("both", True)
+    arm_debug_launch._validate_mujoco_mode("left", False)
+
+    with pytest.raises(ValueError, match="arm:=both"):
+        arm_debug_launch._validate_mujoco_mode("left", True)
+
+
+def test_udp_receiver_exit_shuts_down_launch() -> None:
+    receiver = arm_debug_launch._udp_receiver_node()
+
+    assert isinstance(receiver._ExecuteLocal__on_exit, Shutdown)

xr_rm_bringup/test/test_launcher_ui_cleanup.py

@@ -13,6 +13,100 @@ assert SPEC.loader is not None
 SPEC.loader.exec_module(launcher_ui)
 
 
+class LauncherCommandsTest(unittest.TestCase):
+    def test_modes_expose_the_confirmed_command_matrix(self) -> None:
+        expected_titles = {
+            "Simulation": [
+                "Dual Arm Mock Launch",
+                "XRobotoolkit UDP Bridge (90 Hz)",
+                "Sample UDP Sender (Both Staggered, 60s)",
+                "Open Controller Hz Monitor",
+                "Open ROS Topic/Node List Monitor",
+                "Open Controller Topic Monitor",
+            ],
+            "MuJoCo": [
+                "Dual Arm MuJoCo Mock Launch",
+                "Dual Arm MuJoCo Real Hardware Launch",
+                "XRobotoolkit UDP Bridge (90 Hz)",
+                "Open Controller Hz Monitor",
+                "Open ROS Topic/Node List Monitor",
+                "Open Controller Topic Monitor",
+            ],
+            "Real Hardware": [
+                "Ping Left RM75",
+                "Ping Right RM75",
+                "Left Arm RealMan Launch",
+                "Right Arm RealMan Launch",
+                "Dual Arm RealMan Launch",
+                "XRobotoolkit UDP Bridge (90 Hz)",
+                "Left Gripper Open",
+                "Left Gripper Close",
+                "Right Gripper Open",
+                "Right Gripper Close",
+                "Open ROS Topic/Node List Monitor",
+                "Open Controller Topic Monitor",
+            ],
+            "Diagnostics": [
+                "ROS Doctor Report",
+                "XR-RM Bringup Prefix",
+                "XR-RM Input Prefix",
+                "XR-RM Teleop Prefix",
+                "XR-RM MuJoCo Prefix",
+                "Open Controller Position Monitor",
+                "Open Controller Hz Monitor",
+                "Open ROS Topic/Node List Monitor",
+                "Open Controller Topic Monitor",
+            ],
+        }
+
+        self.assertEqual(launcher_ui.MODES, list(expected_titles))
+        for mode, titles in expected_titles.items():
+            actual = [
+                indexed_title.split(". ", 1)[1]
+                for indexed_title, _command in launcher_ui.build_commands_by_mode(mode)
+            ]
+            self.assertEqual(actual, titles)
+
+    def test_mujoco_launches_distinguish_mock_and_real_hardware(self) -> None:
+        commands = dict(launcher_ui.build_commands_by_mode("MuJoCo"))
+
+        self.assertIn(
+            "arm:=both use_mock:=true use_mujoco:=true",
+            commands["1. Dual Arm MuJoCo Mock Launch"],
+        )
+        self.assertIn(
+            "arm:=both use_mock:=false use_mujoco:=true",
+            commands["2. Dual Arm MuJoCo Real Hardware Launch"],
+        )
+
+    def test_cmd_vel_monitor_is_completely_removed(self) -> None:
+        self.assertFalse(hasattr(launcher_ui, "CMD_VEL_MONITOR_ACTION"))
+        for mode in launcher_ui.MODES:
+            self.assertNotIn("cmd_vel", repr(launcher_ui.build_commands_by_mode(mode)))
+
+    def test_environment_check_includes_mujoco_package(self) -> None:
+        app = object.__new__(launcher_ui.LauncherApp)
+        app.workspace_root = launcher_ui._find_workspace_root()
+        app.terminal_command = lambda _title, _script: ["terminal"]
+        app.x_terminal_target = lambda: "terminator"
+        checked_packages = []
+        app._ros_package_available = lambda package: checked_packages.append(package) or True
+        app.show_text_dialog = lambda *_args: None
+        app.status = mock.Mock()
+
+        with mock.patch.object(
+            launcher_ui.shutil,
+            "which",
+            return_value="/usr/bin/x-terminal-emulator",
+        ):
+            app.check_prerequisites()
+
+        self.assertEqual(
+            checked_packages,
+            ["xr_rm_bringup", "xr_rm_input", "xr_rm_teleop", "xr_rm_mujoco"],
+        )
+
+
 class LauncherCleanupTest(unittest.TestCase):
     def test_xrobotoolkit_cleanup_policy_only_stops_service_on_window_close(self) -> None:
         stop_all_patterns = set(
@@ -89,6 +183,34 @@ class LauncherCleanupTest(unittest.TestCase):
             ],
         )
 
+    def test_stop_all_and_window_close_stop_mujoco(self) -> None:
+        for stop_pc_service in (False, True):
+            app = object.__new__(launcher_ui.LauncherApp)
+            app.status = mock.Mock()
+            app.close_related_terminal_windows = lambda: 0
+
+            def fake_check_output(command, **_kwargs):
+                if command == ["pgrep", "-f", "dual_arm_simulator"]:
+                    return "404\n"
+                raise subprocess.CalledProcessError(1, command)
+
+            with (
+                mock.patch.object(
+                    launcher_ui.subprocess,
+                    "check_output",
+                    side_effect=fake_check_output,
+                ),
+                mock.patch.object(launcher_ui.os, "kill") as kill,
+                mock.patch.object(launcher_ui.time, "sleep"),
+            ):
+                app.stop_launched_processes(
+                    confirm=False,
+                    notify=False,
+                    stop_pc_service=stop_pc_service,
+                )
+
+            kill.assert_called_once_with(404, signal.SIGTERM)
+
 
 if __name__ == "__main__":
     unittest.main()

xr_rm_bringup/tools/launcher_ui.py

@@ -1,8 +1,8 @@
 #!/usr/bin/env python3
 """XR-RM 桌面调试启动器。
 
-提供 Tkinter 图形界面，按“仿真/左臂/右臂/双臂/诊断”组织常用 ROS2
+提供 Tkinter 图形界面，按“仿真/MuJoCo/真机/诊断”组织常用 ROS2 launch、
-launch、sample_udp_sender、topic 监控和环境检查命令，降低现场调试时的命令输入成本。
+sample_udp_sender、topic 监控和环境检查命令，降低现场调试时的命令输入成本。
 """
 
 from __future__ import annotations
@@ -44,8 +44,6 @@ SAMPLE_SENDER_ARGS = (
 TERMINAL_TITLE_PREFIX = "XR-RM Terminal - "
 TOPIC_MONITOR_TITLE = "XR-RM Topic Monitor"
 TOPIC_MONITOR_ACTION = "__xr_rm_topic_monitor__"
-CMD_VEL_MONITOR_TITLE = "XR-RM Target Velocity Monitor"
-CMD_VEL_MONITOR_ACTION = "__xr_rm_cmd_vel_monitor__"
 ROS_GRAPH_MONITOR_TITLE = "XR-RM ROS Graph Monitor"
 ROS_GRAPH_MONITOR_ACTION = "__xr_rm_ros_graph_monitor__"
 
@@ -54,11 +52,6 @@ TOPIC_MONITORS = [
     ("Right Controller", "/xr/right_controller"),
 ]
 
-CMD_VEL_MONITORS = [
-    ("Left Target Vel", "/xr_rm/left_rm75/cmd_vel"),
-    ("Right Target Vel", "/xr_rm/right_rm75/cmd_vel"),
-]
-
 CONTROLLER_POSITION_MONITOR_TITLE = "XR-RM Controller Position Monitor"
 CONTROLLER_POSITION_MONITOR_ACTION = "__xr_rm_controller_position_monitor__"
 CONTROLLER_HZ_MONITOR_TITLE = "XR-RM Controller Hz Monitor"
@@ -81,9 +74,8 @@ ROS_GRAPH_MONITORS = [
 
 MODES = [
     "Simulation",
-    "Left Arm",
+    "MuJoCo",
-    "Right Arm",
+    "Real Hardware",
-    "Dual Arm",
     "Diagnostics",
 ]
 
@@ -177,21 +169,6 @@ def _source_lines(workspace_root: Path) -> list[str]:
     return lines
 
 
-def _one_click_mock(arm: str, hand: str) -> str:
-    sender_seconds = SAMPLE_SENDER_STAGGERED_SECONDS if hand == "both" else SAMPLE_SENDER_SECONDS
-    both_mode = "staggered" if hand == "both" else "synchronized"
-    return "\n".join([
-        f"ros2 launch xr_rm_bringup arm_debug.launch.py arm:={arm} use_mock:=true &",
-        "launch_pid=$!",
-        "sleep 2",
-        _sample_udp_sender_command(hand, sender_seconds, both_mode),
-        "echo",
-        "echo 'Sample sender finished. The launch process is still running in this terminal.'",
-        "echo 'Press Ctrl-C here, or use the cleanup button in the launcher, to stop it.'",
-        "wait \"$launch_pid\"",
-    ])
-
-
 def _diagnostic_commands() -> list[tuple[str, str]]:
     return [
         ("Open ROS Topic/Node List Monitor", ROS_GRAPH_MONITOR_ACTION),
@@ -202,14 +179,9 @@ def _topic_monitor_item() -> tuple[str, str]:
     return ("Open Controller Topic Monitor", TOPIC_MONITOR_ACTION)
 
 
-def _cmd_vel_monitor_item() -> tuple[str, str]:
-    return ("Open Target Velocity Monitor", CMD_VEL_MONITOR_ACTION)
-
-
 def _is_topic_monitor_action(action: str) -> bool:
     return action in (
         TOPIC_MONITOR_ACTION,
-        CMD_VEL_MONITOR_ACTION,
         CONTROLLER_POSITION_MONITOR_ACTION,
         CONTROLLER_HZ_MONITOR_ACTION,
     )
@@ -232,14 +204,6 @@ def _topic_monitor_spec(action: str) -> tuple[str, list[tuple[str, str]], str, s
             "xr_rm_controller_hz_monitor_",
             "controller hz topic",
         )
-    if action == CMD_VEL_MONITOR_ACTION:
-        return (
-            CMD_VEL_MONITOR_TITLE,
-            [(title, f"ros2 topic echo {topic}") for title, topic in CMD_VEL_MONITORS],
-            "xr_rm_cmd_vel_monitor",
-            "xr_rm_cmd_vel_monitor_",
-            "target velocity topic",
-        )
     return (
         TOPIC_MONITOR_TITLE,
         [(title, f"ros2 topic echo {topic}") for title, topic in TOPIC_MONITORS],
@@ -249,16 +213,10 @@ def _topic_monitor_spec(action: str) -> tuple[str, list[tuple[str, str]], str, s
     )
 
 
-def _finalize_items(
+def _finalize_items(items: list[tuple[str, str]]) -> list[tuple[str, str]]:
-    items: list[tuple[str, str]],
-    one_click: tuple[str, str] | None = None,
-) -> list[tuple[str, str]]:
     final_items = items + _diagnostic_commands() + [
         _topic_monitor_item(),
-        _cmd_vel_monitor_item(),
     ]
-    if one_click is not None:
-        final_items.append(one_click)
     return _with_index(final_items)
 
 
@@ -266,25 +224,64 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
     # UI 列表只维护命令模板；真正执行时统一套上工作空间 source 和终端包装。
     if mode == "Simulation":
         items = [
-            ("Left Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true"),
-            ("Right Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true"),
             ("Dual Arm Mock Launch", "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true"),
             ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
-            (
-                "Sample UDP Sender (Left, 30s)",
-                _sample_udp_sender_command("left"),
-            ),
-            (
-                "Sample UDP Sender (Right, 30s)",
-                _sample_udp_sender_command("right"),
-            ),
             (
                 "Sample UDP Sender (Both Staggered, 60s)",
                 _sample_udp_sender_command("both", SAMPLE_SENDER_STAGGERED_SECONDS, "staggered"),
             ),
-            ("One-Click Left Mock Demo", _one_click_mock("left", "left")),
+            (
-            ("One-Click Right Mock Demo", _one_click_mock("right", "right")),
+                "Open Controller Hz Monitor",
-            ("One-Click Dual Mock Demo", _one_click_mock("both", "both")),
+                CONTROLLER_HZ_MONITOR_ACTION,
+            ),
+        ]
+    elif mode == "MuJoCo":
+        items = [
+            (
+                "Dual Arm MuJoCo Mock Launch",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py "
+                "arm:=both use_mock:=true use_mujoco:=true",
+            ),
+            (
+                "Dual Arm MuJoCo Real Hardware Launch",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py "
+                "arm:=both use_mock:=false use_mujoco:=true",
+            ),
+            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
+            (
+                "Open Controller Hz Monitor",
+                CONTROLLER_HZ_MONITOR_ACTION,
+            ),
+        ]
+    elif mode == "Real Hardware":
+        items = [
+            ("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"),
+            ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
+            (
+                "Left Arm RealMan Launch",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false",
+            ),
+            (
+                "Right Arm RealMan Launch",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false",
+            ),
+            (
+                "Dual Arm RealMan Launch",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false",
+            ),
+            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
+            ("Left Gripper Open", _tool_command("left", True)),
+            ("Left Gripper Close", _tool_command("left", False)),
+            ("Right Gripper Open", _tool_command("right", True)),
+            ("Right Gripper Close", _tool_command("right", False)),
+        ]
+    else:
+        items = [
+            ("ROS Doctor Report", "ros2 doctor --report"),
+            ("XR-RM Bringup Prefix", "ros2 pkg prefix xr_rm_bringup"),
+            ("XR-RM Input Prefix", "ros2 pkg prefix xr_rm_input"),
+            ("XR-RM Teleop Prefix", "ros2 pkg prefix xr_rm_teleop"),
+            ("XR-RM MuJoCo Prefix", "ros2 pkg prefix xr_rm_mujoco"),
             (
                 "Open Controller Position Monitor",
                 CONTROLLER_POSITION_MONITOR_ACTION,
@@ -294,64 +291,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
                 CONTROLLER_HZ_MONITOR_ACTION,
             ),
         ]
-        one_click = None
+    return _finalize_items(items)
-    elif mode == "Left Arm":
-        items = [
-            ("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"),
-            (
-                "Left Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false",
-            ),
-            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
-            ("Left Tool Open", _tool_command("left", True)),
-            ("Left Tool Close", _tool_command("left", False)),
-            (
-                "Sample UDP Sender (Left, 30s)",
-                _sample_udp_sender_command("left"),
-            ),
-        ]
-        one_click = None
-    elif mode == "Right Arm":
-        items = [
-            ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
-            (
-                "Right Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false",
-            ),
-            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
-            ("Right Tool Open", _tool_command("right", True)),
-            ("Right Tool Close", _tool_command("right", False)),
-            (
-                "Sample UDP Sender (Right, 30s)",
-                _sample_udp_sender_command("right"),
-            ),
-        ]
-        one_click = None
-    elif mode == "Dual Arm":
-        items = [
-            ("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"),
-            ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
-            (
-                "Dual Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false",
-            ),
-            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
-            (
-                "Sample UDP Sender (Both Staggered, 60s)",
-                _sample_udp_sender_command("both", SAMPLE_SENDER_STAGGERED_SECONDS, "staggered"),
-            ),
-        ]
-        one_click = None
-    else:
-        items = [
-            ("ROS Doctor Report", "ros2 doctor --report"),
-            ("XR-RM Bringup Prefix", "ros2 pkg prefix xr_rm_bringup"),
-            ("XR-RM Input Prefix", "ros2 pkg prefix xr_rm_input"),
-            ("XR-RM Teleop Prefix", "ros2 pkg prefix xr_rm_teleop"),
-            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
-        ]
-        one_click = None
-    return _finalize_items(items, one_click)
 
 
 class LauncherApp:
@@ -608,7 +548,7 @@ class LauncherApp:
         else:
             warnings.append("[WARN] Could not identify x-terminal-emulator target.")
 
-        for package in ("xr_rm_bringup", "xr_rm_input", "xr_rm_teleop"):
+        for package in ("xr_rm_bringup", "xr_rm_input", "xr_rm_teleop", "xr_rm_mujoco"):
             if self._ros_package_available(package):
                 ok.append(f"[OK] ROS package available: {package}")
             else:
@@ -1254,7 +1194,6 @@ class LauncherApp:
         title_patterns = (
             TERMINAL_TITLE_PREFIX,
             TOPIC_MONITOR_TITLE,
-            CMD_VEL_MONITOR_TITLE,
             CONTROLLER_POSITION_MONITOR_TITLE,
             CONTROLLER_HZ_MONITOR_TITLE,
             ROS_GRAPH_MONITOR_TITLE,
@@ -1326,24 +1265,21 @@ class LauncherApp:
             *_xrobotoolkit_cleanup_patterns(stop_pc_service=stop_pc_service),
             TERMINAL_TITLE_PREFIX,
             TOPIC_MONITOR_TITLE,
-            CMD_VEL_MONITOR_TITLE,
             ROS_GRAPH_MONITOR_TITLE,
             "xr_rm_topic_monitor_",
-            "xr_rm_cmd_vel_monitor_",
             "xr_rm_controller_position_monitor_",
             "xr_rm_controller_hz_monitor_",
             "xr_rm_ros_graph_monitor_",
             "udp_controller_receiver",
             "sample_udp_sender",
             "single_arm_velocity_teleop",
+            "dual_arm_simulator",
             "ros2 topic echo /xr/left_controller --field pose.position",
             "ros2 topic echo /xr/right_controller --field pose.position",
             "ros2 topic echo /xr/left_controller",
             "ros2 topic echo /xr/right_controller",
             "ros2 topic hz /xr/left_controller",
             "ros2 topic hz /xr/right_controller",
-            "ros2 topic echo /xr_rm/left_rm75/cmd_vel",
-            "ros2 topic echo /xr_rm/right_rm75/cmd_vel",
             "ros2 topic list",
             "ros2 node list",
         ]

xr_rm_mujoco/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>xr_rm_mujoco</name>
+  <version>0.1.0</version>
+  <description>MuJoCo kinematic visualization for the dual RM75 platform.</description>
+  <maintainer email="user@example.com">Yikai Fu</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_python</buildtool_depend>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>xr_rm_teleop</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>python3-pytest</test_depend>
+  <test_depend>python3-yaml</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

xr_rm_mujoco/resource/xr_rm_mujoco

@@ -0,0 +1 @@
+

xr_rm_mujoco/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/xr_rm_mujoco
+[install]
+install_scripts=$base/lib/xr_rm_mujoco

xr_rm_mujoco/setup.py

@@ -0,0 +1,28 @@
+"""MuJoCo 双 RM75 运动学显示包安装配置。"""
+
+from setuptools import setup
+
+
+package_name = "xr_rm_mujoco"
+
+setup(
+    name=package_name,
+    version="0.1.0",
+    packages=[package_name],
+    data_files=[
+        ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
+        (f"share/{package_name}", ["package.xml"]),
+    ],
+    install_requires=["setuptools"],
+    zip_safe=True,
+    maintainer="Yikai Fu",
+    maintainer_email="user@example.com",
+    description="MuJoCo kinematic visualization for the dual RM75 platform.",
+    license="Apache-2.0",
+    tests_require=["pytest"],
+    entry_points={
+        "console_scripts": [
+            "dual_arm_simulator = xr_rm_mujoco.dual_arm_simulator:main",
+        ],
+    },
+)

xr_rm_mujoco/test/conftest.py

@@ -0,0 +1,11 @@
+"""让 ROS2 系统 pytest 复用项目固定 XR 环境中的 MuJoCo。"""
+
+import sys
+from pathlib import Path
+
+
+XR_SITE_PACKAGES = Path(
+    "/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages"
+)
+if XR_SITE_PACKAGES.is_dir():
+    sys.path.append(str(XR_SITE_PACKAGES))

xr_rm_mujoco/test/test_dual_arm_simulator.py

@@ -0,0 +1,137 @@
+import math
+from pathlib import Path
+from types import SimpleNamespace
+from unittest.mock import Mock
+
+import pytest
+import yaml
+
+from xr_rm_mujoco import dual_arm_simulator as simulator_module
+from xr_rm_mujoco.dual_arm_simulator import (
+    ARM_JOINT_NAMES,
+    DualArmKinematicModel,
+)
+
+
+SRC_DIR = Path(__file__).resolve().parents[2]
+URDF_PATH = (
+    SRC_DIR / "xr_rm_teleop" / "models" / "dual_rm75" / "Dual_arm.urdf"
+)
+DUAL_CONFIG_PATH = SRC_DIR / "xr_rm_bringup" / "config" / "dual_arm_rm75.yaml"
+MUJOCO_CONFIG_PATH = (
+    SRC_DIR / "xr_rm_bringup" / "config" / "dual_arm_mujoco.yaml"
+)
+
+
+def test_dual_urdf_loads_with_expected_joint_mapping() -> None:
+    simulation = DualArmKinematicModel(str(URDF_PATH))
+
+    assert simulation.model.nq == 14
+    assert simulation.model.nv == 14
+    assert ARM_JOINT_NAMES == {
+        "left": tuple(f"scissor_joint_{index}" for index in range(1, 8)),
+        "right": tuple(f"omnipic_joint_{index}" for index in range(1, 8)),
+    }
+    assert not simulation.ready
+
+
+def test_joint_messages_are_mapped_by_name_not_array_order() -> None:
+    simulation = DualArmKinematicModel(str(URDF_PATH))
+    names = list(reversed(ARM_JOINT_NAMES["left"]))
+    values = [float(index) / 10.0 for index in range(7)]
+
+    simulation.apply_arm_state("left", names, values)
+
+    by_name = dict(zip(names, values))
+    assert simulation.joint_positions("left") == pytest.approx(
+        [by_name[name] for name in ARM_JOINT_NAMES["left"]]
+    )
+    assert not simulation.ready
+
+
+def test_yaml_initial_poses_populate_both_arms() -> None:
+    simulation = DualArmKinematicModel(str(URDF_PATH))
+    with DUAL_CONFIG_PATH.open(encoding="utf-8") as stream:
+        config = yaml.safe_load(stream)
+
+    for arm, node_name in (
+        ("left", "left_arm_teleop"),
+        ("right", "right_arm_teleop"),
+    ):
+        degrees = config[node_name]["ros__parameters"]["initial_joint_pose"]
+        radians = [math.radians(value) for value in degrees]
+        simulation.apply_arm_state(arm, ARM_JOINT_NAMES[arm], radians)
+        assert simulation.joint_positions(arm) == pytest.approx(radians)
+
+    assert simulation.ready
+
+
+def test_mujoco_config_contains_only_render_parameters() -> None:
+    with MUJOCO_CONFIG_PATH.open(encoding="utf-8") as stream:
+        parameters = yaml.safe_load(stream)["dual_arm_simulator"]["ros__parameters"]
+
+    assert parameters == {"render_rate_hz": 60.0}
+
+
+def test_main_closes_viewer_cleanly_on_keyboard_interrupt(monkeypatch) -> None:
+    close_viewer = simulator_module.DualArmSimulator.close_viewer
+    viewer = Mock()
+    node = SimpleNamespace(_viewer=viewer, destroy_node=Mock())
+    node.close_viewer = lambda: close_viewer(node)
+    init = Mock()
+    spin = Mock(side_effect=KeyboardInterrupt)
+    sleep = Mock(side_effect=[KeyboardInterrupt, None])
+    monotonic = Mock(side_effect=[0.0, 0.0, 0.04])
+    shutdown = Mock()
+
+    monkeypatch.setattr(simulator_module, "DualArmSimulator", lambda: node)
+    monkeypatch.setattr(simulator_module.rclpy, "init", init)
+    monkeypatch.setattr(simulator_module.rclpy, "spin", spin)
+    monkeypatch.setattr(simulator_module.rclpy, "ok", lambda: True)
+    monkeypatch.setattr(simulator_module.time, "sleep", sleep)
+    monkeypatch.setattr(simulator_module.time, "monotonic", monotonic)
+    monkeypatch.setattr(simulator_module.rclpy, "shutdown", shutdown)
+
+    simulator_module.main(["--test"])
+
+    init.assert_called_once_with(args=["--test"])
+    spin.assert_called_once_with(node)
+    viewer.close.assert_called_once_with()
+    assert [call.args[0] for call in sleep.call_args_list] == pytest.approx(
+        [0.1, 0.06]
+    )
+    node.destroy_node.assert_called_once_with()
+    shutdown.assert_called_once_with()
+    assert node._viewer is None
+
+
+@pytest.mark.parametrize(
+    ("names", "positions", "match"),
+    [
+        (list(ARM_JOINT_NAMES["left"][:-1]), [0.0] * 6, "expected"),
+        (list(ARM_JOINT_NAMES["left"]), [0.0] * 6, "same length"),
+        (
+            [ARM_JOINT_NAMES["left"][0]] * 7,
+            [0.0] * 7,
+            "unique",
+        ),
+        (
+            list(ARM_JOINT_NAMES["left"]),
+            [0.0] * 6 + [math.nan],
+            "finite",
+        ),
+    ],
+)
+def test_invalid_joint_state_is_rejected_without_partial_update(
+    names: list[str],
+    positions: list[float],
+    match: str,
+) -> None:
+    simulation = DualArmKinematicModel(str(URDF_PATH))
+    valid = [0.1] * 7
+    simulation.apply_arm_state("left", ARM_JOINT_NAMES["left"], valid)
+
+    with pytest.raises(ValueError, match=match):
+        simulation.apply_arm_state("left", names, positions)
+
+    assert simulation.joint_positions("left") == pytest.approx(valid)

xr_rm_mujoco/xr_rm_mujoco/__init__.py

@@ -0,0 +1 @@
+"""双 RM75 MuJoCo 运动学显示包。"""

xr_rm_mujoco/xr_rm_mujoco/dual_arm_simulator.py

@@ -0,0 +1,214 @@
+"""使用现有双 RM75 URDF 的 MuJoCo 运动学状态映射。"""
+
+from __future__ import annotations
+
+import math
+from pathlib import Path
+import time
+from typing import Callable
+
+import mujoco
+import mujoco.viewer
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+
+ARM_JOINT_NAMES = {
+    "left": tuple(f"scissor_joint_{index}" for index in range(1, 8)),
+    "right": tuple(f"omnipic_joint_{index}" for index in range(1, 8)),
+}
+STATE_TOPICS = {
+    "left": "/xr_rm/left_rm75/joint_states",
+    "right": "/xr_rm/right_rm75/joint_states",
+}
+
+
+class DualArmKinematicModel:
+    """加载双臂 URDF，并按关节名更新 MuJoCo qpos。"""
+
+    def __init__(self, urdf_path: str) -> None:
+        path = Path(urdf_path).expanduser().resolve()
+        if not path.is_file():
+            raise FileNotFoundError(f"dual RM75 URDF not found: {path}")
+
+        self.model = mujoco.MjModel.from_xml_path(str(path))
+        self.data = mujoco.MjData(self.model)
+        self._qpos_addresses: dict[str, dict[str, int]] = {}
+        self._received_arms: set[str] = set()
+
+        for arm, names in ARM_JOINT_NAMES.items():
+            addresses = {}
+            for name in names:
+                joint_id = mujoco.mj_name2id(
+                    self.model,
+                    mujoco.mjtObj.mjOBJ_JOINT,
+                    name,
+                )
+                if joint_id < 0:
+                    raise RuntimeError(f"MuJoCo joint not found: {name}")
+                if self.model.jnt_type[joint_id] != mujoco.mjtJoint.mjJNT_HINGE:
+                    raise RuntimeError(f"MuJoCo joint must be hinge: {name}")
+                addresses[name] = int(self.model.jnt_qposadr[joint_id])
+            self._qpos_addresses[arm] = addresses
+
+    @property
+    def ready(self) -> bool:
+        return self._received_arms == set(ARM_JOINT_NAMES)
+
+    def apply_arm_state(
+        self,
+        arm: str,
+        names: list[str] | tuple[str, ...],
+        positions: list[float] | tuple[float, ...],
+    ) -> None:
+        if arm not in ARM_JOINT_NAMES:
+            raise ValueError("arm must be left or right")
+        if len(names) != len(positions):
+            raise ValueError("joint names and positions must have the same length")
+        if len(set(names)) != len(names):
+            raise ValueError("joint names must be unique")
+
+        expected = set(ARM_JOINT_NAMES[arm])
+        if set(names) != expected:
+            raise ValueError(f"joint names must match expected {arm} joints")
+
+        values = [float(value) for value in positions]
+        if not all(math.isfinite(value) for value in values):
+            raise ValueError("joint positions must be finite")
+
+        by_name = dict(zip(names, values))
+        updates = [
+            (self._qpos_addresses[arm][name], by_name[name])
+            for name in ARM_JOINT_NAMES[arm]
+        ]
+        for address, value in updates:
+            self.data.qpos[address] = value
+        mujoco.mj_forward(self.model, self.data)
+        self._received_arms.add(arm)
+
+    def joint_positions(self, arm: str) -> list[float]:
+        if arm not in ARM_JOINT_NAMES:
+            raise ValueError("arm must be left or right")
+        return [
+            float(self.data.qpos[self._qpos_addresses[arm][name]])
+            for name in ARM_JOINT_NAMES[arm]
+        ]
+
+
+class DualArmSimulator(Node):
+    """订阅左右关节反馈并刷新一个 MuJoCo 双臂 viewer。"""
+
+    def __init__(
+        self,
+        viewer_factory: Callable = mujoco.viewer.launch_passive,
+    ) -> None:
+        super().__init__("dual_arm_simulator")
+        self.declare_parameter("robot_urdf_path", "")
+        self.declare_parameter("render_rate_hz", 60.0)
+
+        render_rate_hz = float(self.get_parameter("render_rate_hz").value)
+        if not math.isfinite(render_rate_hz) or render_rate_hz <= 0.0:
+            raise ValueError("render_rate_hz must be finite and > 0")
+
+        self._kinematics = DualArmKinematicModel(
+            str(self.get_parameter("robot_urdf_path").value)
+        )
+        self._viewer_factory = viewer_factory
+        self._viewer = None
+        self._subscriptions = [
+            self.create_subscription(
+                JointState,
+                topic,
+                lambda message, selected_arm=arm: self._on_joint_state(
+                    selected_arm,
+                    message,
+                ),
+                10,
+            )
+            for arm, topic in STATE_TOPICS.items()
+        ]
+        self.create_timer(1.0 / render_rate_hz, self._render)
+        self.get_logger().info(
+            "MuJoCo 双臂节点已启动，等待左右关节状态，"
+            f"render_rate_hz={render_rate_hz:.1f}"
+        )
+
+    def _on_joint_state(self, arm: str, message: JointState) -> None:
+        try:
+            if self._viewer is None:
+                self._kinematics.apply_arm_state(
+                    arm,
+                    list(message.name),
+                    list(message.position),
+                )
+            else:
+                with self._viewer.lock():
+                    self._kinematics.apply_arm_state(
+                        arm,
+                        list(message.name),
+                        list(message.position),
+                    )
+        except (RuntimeError, ValueError) as exc:
+            self.get_logger().warn(
+                f"拒绝 {arm} 关节状态：{exc}",
+                throttle_duration_sec=1.0,
+            )
+
+    def _render(self) -> None:
+        for topic in STATE_TOPICS.values():
+            publisher_count = self.count_publishers(topic)
+            if publisher_count > 1:
+                self.get_logger().warn(
+                    f"关节状态话题存在多个发布者：{topic}, count={publisher_count}",
+                    throttle_duration_sec=5.0,
+                )
+
+        if not self._kinematics.ready:
+            return
+        if self._viewer is None:
+            self._viewer = self._viewer_factory(
+                self._kinematics.model,
+                self._kinematics.data,
+            )
+        if not self._viewer.is_running():
+            self.get_logger().info("MuJoCo viewer 已关闭。")
+            rclpy.shutdown()
+            return
+        self._viewer.sync()
+
+    def close_viewer(self) -> None:
+        if self._viewer is not None:
+            self._viewer.close()
+            # MuJoCo 在后台 daemon 线程释放 GLX；立即退出解释器会触发段错误。
+            deadline = time.monotonic() + 0.1
+            while True:
+                remaining = deadline - time.monotonic()
+                if remaining <= 0.0:
+                    break
+                try:
+                    time.sleep(remaining)
+                    break
+                except KeyboardInterrupt:
+                    continue
+            self._viewer = None
+
+
+def main(args=None) -> None:
+    rclpy.init(args=args)
+    node = None
+    try:
+        node = DualArmSimulator()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.close_viewer()
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

xr_rm_teleop/package.xml

@@ -11,6 +11,7 @@
 
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
   <exec_depend>python3-yaml</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>xr_rm_interfaces</exec_depend>

xr_rm_teleop/test/test_joint_control.py

@@ -4,6 +4,7 @@ from types import SimpleNamespace
 
 import numpy as np
 import pytest
+from builtin_interfaces.msg import Time as TimeMsg
 
 from xr_rm_teleop.realman_adapter import JointStateSnapshot
 from xr_rm_teleop.single_arm_velocity_teleop import (
@@ -29,8 +30,82 @@ class FakeTime:
         del other
         return SimpleNamespace(nanoseconds=0)
 
+    def to_msg(self):
+        return TimeMsg()
 
-def _primary_button_teleop(*, move_error=None):
+
+class FakePublisher:
+    def __init__(self) -> None:
+        self.messages = []
+
+    def publish(self, message) -> None:
+        self.messages.append(message)
+
+
+def _joint_publishing_teleop() -> SingleArmVelocityTeleop:
+    names = [f"omnipic_joint_{index}" for index in range(1, 8)]
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = SimpleNamespace(
+        joint_names=names,
+        update_joint_state=lambda joints: np.eye(4),
+    )
+    teleop._joint_state_pub = FakePublisher()
+    teleop._joint_target_pub = FakePublisher()
+    teleop._active = False
+    teleop._last_valid_joint_target = None
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
+    return teleop
+
+
+def test_reset_joint_state_publishes_named_feedback() -> None:
+    teleop = _joint_publishing_teleop()
+    positions = [0.1 * index for index in range(7)]
+    snapshot = JointStateSnapshot(positions, time.monotonic())
+
+    teleop._reset_joint_state(snapshot)
+
+    message = teleop._joint_state_pub.messages[-1]
+    assert message.name == teleop._ik_solver.joint_names
+    assert message.position == pytest.approx(positions)
+
+
+def test_sync_joint_feedback_publishes_each_sample() -> None:
+    teleop = _joint_publishing_teleop()
+    positions = [0.2] * 7
+
+    teleop._sync_joint_feedback(
+        JointStateSnapshot(positions, time.monotonic())
+    )
+
+    assert len(teleop._joint_state_pub.messages) == 1
+    assert teleop._joint_state_pub.messages[0].position == pytest.approx(positions)
+
+
+def test_send_joint_target_publishes_limited_command() -> None:
+    sent = []
+    teleop = _joint_publishing_teleop()
+    teleop._adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sent.append((list(joints), follow))
+    )
+    teleop._follow = False
+    teleop._latest_joint_positions = [0.0] * 7
+    teleop._last_joint_command_target = [0.0] * 7
+    teleop._last_joint_command_velocity = [0.0] * 7
+    teleop._joint_command_max_speed = 1.0
+    teleop._joint_command_max_acceleration = 100.0
+    teleop._dt = 0.1
+
+    assert teleop._send_joint_target([0.5] * 7)
+
+    assert len(sent) == 1
+    assert sent[0][0] == pytest.approx([0.1] * 7)
+    assert sent[0][1] is False
+    message = teleop._joint_target_pub.messages[-1]
+    assert message.name == teleop._ik_solver.joint_names
+    assert message.position == pytest.approx(teleop._last_joint_command_target)
+
+
+def _primary_button_teleop(*, use_mock=False, move_error=None):
     events = []
     errors = []
     snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())
@@ -47,6 +122,7 @@ def _primary_button_teleop(*, move_error=None):
 
     teleop = object.__new__(SingleArmVelocityTeleop)
     teleop._arm_name = "right_rm75"
+    teleop._use_mock = use_mock
     teleop._adapter = Adapter()
     teleop._last_primary_pressed = None
     teleop._grip_rearm_required = False
@@ -100,6 +176,34 @@ def test_primary_button_move_failure_logs_and_stays_stopped() -> None:
     ]
 
 
+def test_mock_primary_reset_can_reanchor_without_grip_release() -> None:
+    teleop, events, _, snapshot = _primary_button_teleop(use_mock=True)
+
+    teleop._on_controller(SimpleNamespace(primary=False))
+    teleop._on_controller(SimpleNamespace(primary=True))
+
+    assert events == [
+        ("stop", True),
+        "move",
+        "read",
+        ("sync", snapshot),
+    ]
+    assert not teleop._grip_rearm_required
+
+
+def test_failed_mock_primary_reset_still_requires_grip_release() -> None:
+    failure = RuntimeError("mock reset failed")
+    teleop, _, _, _ = _primary_button_teleop(
+        use_mock=True,
+        move_error=failure,
+    )
+
+    teleop._on_controller(SimpleNamespace(primary=False))
+    teleop._on_controller(SimpleNamespace(primary=True))
+
+    assert teleop._grip_rearm_required
+
+
 def test_startup_joint_query_initializes_qp_and_command_history() -> None:
     positions = [0.1] * 7
     pose = np.eye(4)
@@ -112,8 +216,11 @@ def test_startup_joint_query_initializes_qp_and_command_history() -> None:
         )
     )
     teleop._ik_solver = SimpleNamespace(
-        update_joint_state=lambda joints: pose
+        joint_names=[f"omnipic_joint_{index}" for index in range(1, 8)],
+        update_joint_state=lambda joints: pose,
     )
+    teleop._joint_state_pub = FakePublisher()
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
     teleop.get_logger = lambda: FakeLogger()
 
     teleop._initialize_joint_state()
@@ -178,6 +285,12 @@ def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
     teleop._ik_solver = SimpleNamespace(
         update_joint_state=lambda joints: np.eye(4)
     )
+    teleop._ik_solver.joint_names = [
+        f"omnipic_joint_{index}" for index in range(1, 8)
+    ]
+    teleop._joint_state_pub = FakePublisher()
+    teleop._joint_target_pub = FakePublisher()
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
     teleop._stop_sent = False
     teleop._feedback_resync_timeout_sec = 0.5
     teleop._publish_stop_debug = lambda: None
@@ -433,6 +546,10 @@ def test_feedback_fault_blocks_grip_until_release() -> None:
     teleop._ik_solver = SimpleNamespace(
         update_joint_state=lambda joints: np.eye(4)
     )
+    teleop._ik_solver.joint_names = [
+        f"omnipic_joint_{index}" for index in range(1, 8)
+    ]
+    teleop._joint_state_pub = FakePublisher()
     teleop._grip_rearm_required = True
     teleop._control_fault_latched = False
     teleop._feedback_resync_attempted = False
@@ -459,6 +576,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
     class FakeSolver:
         def __init__(self) -> None:
             self.solve_calls = 0
+            self.joint_names = [
+                f"omnipic_joint_{index}" for index in range(1, 8)
+            ]
 
         def update_joint_state(self, joints):
             assert joints == [0.1] * 7
@@ -476,6 +596,8 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
     teleop._active = False
     teleop._last_valid_joint_target = None
     teleop._last_current_pose = None
+    teleop._joint_state_pub = FakePublisher()
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
 
     pose = teleop._sync_joint_feedback(
         JointStateSnapshot([0.1] * 7, time.monotonic())

xr_rm_teleop/test/test_orientation_control.py

@@ -4,6 +4,7 @@ from types import SimpleNamespace
 
 import numpy as np
 import pytest
+from builtin_interfaces.msg import Time as TimeMsg
 
 from xr_rm_teleop.realman_adapter import JointStateSnapshot
 from xr_rm_teleop.single_arm_velocity_teleop import (
@@ -148,6 +149,9 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
             del other
             return SimpleNamespace(nanoseconds=0)
 
+        def to_msg(self):
+            return TimeMsg()
+
     class FakeClock:
         def now(self):
             return FakeTime()
@@ -174,7 +178,11 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
             time.monotonic(),
         )
     )
-    teleop._ik_solver = SimpleNamespace(update_joint_state=lambda joints: np.eye(4))
+    teleop._ik_solver = SimpleNamespace(
+        joint_names=[f"omnipic_joint_{index}" for index in range(1, 8)],
+        update_joint_state=lambda joints: np.eye(4),
+    )
+    teleop._joint_state_pub = SimpleNamespace(publish=lambda message: None)
     teleop._active = False
     teleop._last_valid_joint_target = None
     teleop._last_current_pose = None

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -149,6 +149,10 @@ class PlacoIkSolver:
         self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
         self._solver.add_kinetic_energy_regularization_task(1e-6)
 
+    @property
+    def joint_names(self) -> list[str]:
+        return list(self._joint_names)
+
     @property
     def base_configuration(self) -> list[float]:
         return self._robot.state.q[:7].tolist()

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -17,6 +17,7 @@ import rclpy
 from geometry_msgs.msg import PoseStamped, TwistStamped
 from rclpy.node import Node
 from rclpy.time import Time
+from sensor_msgs.msg import JointState
 from std_msgs.msg import Bool
 
 from xr_rm_interfaces.msg import XrController
@@ -258,6 +259,7 @@ class SingleArmVelocityTeleop(Node):
         self._low_z_threshold = float(self.get_parameter("low_z_threshold").value)
         self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
         self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
+        self._use_mock = self._bool_parameter("use_mock")
         self._follow = self._bool_parameter("follow")
         self._enable_tool_control = self._bool_parameter("enable_tool_control")
         self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
@@ -327,12 +329,22 @@ class SingleArmVelocityTeleop(Node):
             self._dt,
             peripheral_arm,
         )
+        debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
+        self._joint_state_pub = self.create_publisher(
+            JointState,
+            f"{debug_ns}/joint_states",
+            10,
+        )
+        self._joint_target_pub = self.create_publisher(
+            JointState,
+            f"{debug_ns}/joint_target",
+            10,
+        )
         self._adapter = self._make_adapter()
         self._adapter.connect()
         self._initialize_joint_state()
         self._setup_tool_control()
 
-        debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
         self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
         self._raw_target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/raw_target_pose", 10)
         self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10)
@@ -352,7 +364,7 @@ class SingleArmVelocityTeleop(Node):
             "initial_joint_pose",
             7,
         )
-        if self._bool_parameter("use_mock"):
+        if self._use_mock:
             return MockRealManAdapter(initial_joint_pose)
 
         return RealManAdapter(
@@ -393,6 +405,13 @@ class SingleArmVelocityTeleop(Node):
             self._adapter.close()
             raise
 
+    def _publish_joint_positions(self, publisher, positions: list[float]) -> None:
+        message = JointState()
+        message.header.stamp = self.get_clock().now().to_msg()
+        message.name = self._ik_solver.joint_names
+        message.position = [float(value) for value in positions]
+        publisher.publish(message)
+
     def _reset_joint_state(
         self,
         snapshot: JointStateSnapshot,
@@ -406,6 +425,7 @@ class SingleArmVelocityTeleop(Node):
         self._last_valid_joint_target = list(positions)
         self._last_joint_command_target = list(positions)
         self._last_joint_command_velocity = [0.0] * 7
+        self._publish_joint_positions(self._joint_state_pub, positions)
         return current_pose
 
     def _setup_tool_control(self) -> None:
@@ -540,6 +560,8 @@ class SingleArmVelocityTeleop(Node):
             )
             return
 
+        if self._use_mock:
+            self._grip_rearm_required = False
         self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
 
     def _handle_trigger_gripper(self, msg: XrController) -> None:
@@ -1166,6 +1188,10 @@ class SingleArmVelocityTeleop(Node):
         )
         self._latest_joint_positions = list(snapshot.positions)
         self._last_current_pose = current_pose
+        self._publish_joint_positions(
+            self._joint_state_pub,
+            list(snapshot.positions),
+        )
         if not self._active or self._last_valid_joint_target is None:
             self._last_valid_joint_target = list(snapshot.positions)
         return current_pose
@@ -1254,6 +1280,10 @@ class SingleArmVelocityTeleop(Node):
             return False
         self._last_joint_command_target = limited_target
         self._last_joint_command_velocity = limited_velocity
+        self._publish_joint_positions(
+            self._joint_target_pub,
+            limited_target,
+        )
         return True
 
     @staticmethod