更新README.md

README.md

@@ -1,90 +1,44 @@
-# XR-RM75 双臂遥操作工作空间
+# XR-RM75 双臂遥操作
 
-本仓库是面向 **Ubuntu 22.04 + ROS2 Humble + PICO 4 Ultra + 睿尔曼 RM75** 的阶段一 XR 双臂遥操作项目。当前目标是先跑通一条低速、安全、可调试的闭环：
+基于 **Ubuntu 22.04、ROS2 Humble、PICO 4 Ultra 和睿尔曼 RM75** 的双臂 XR 遥操作
+工作空间，支持单臂/双臂 Mock 与真机控制，以及 MuJoCo 运动学显示。
+
+> [!WARNING]
+> 真机命令会连接并控制机械臂。首次运行必须从 Mock 和单臂低速验证开始，确保急停
+> 可用且工作区无人。当前项目没有双臂碰撞检测或避障。
+
+## 当前能力
+
+- PICO/XR 双手柄 UDP 输入，相对位姿目标与 Placo QP 七关节控制。
+- 单臂/双臂 Mock 与真机、手柄/话题夹爪控制，以及只读 MuJoCo 双臂显示。
+- 工作空间/圆柱限位、速度限制、指令超时和安全慢停。
+- 统一 launch、Tkinter 启动面板、调试话题和 Mock 输入工具。
+
+尚未完成：D405/D435 视频流、数据记录、相机标定、目标检测、双臂碰撞避障、任务级状态机，以及 PICO 与 ROS 的完整时间同步和状态回传。
+
+## 系统架构
 
 ```text
-PICO/XR 双手柄 UDP JSON
+PICO / XRoboToolkit
+  -> UDP JSON
   -> xr_rm_input/udp_controller_receiver
-  -> /xr/left_controller 与 /xr/right_controller
+  -> /xr/left_controller、/xr/right_controller
   -> xr_rm_teleop/single_arm_velocity_teleop
-  -> Placo QP 单步逆解
+  -> 相对 TCP 目标 + Placo QP
-  -> 左右 RM75 七关节角透传控制
+  -> Mock 或 RM75 rm_movej_canfd
-  -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
+  -> joint_states / 调试话题
+       -> 可选 xr_rm_mujoco/dual_arm_simulator
 ```
 
-当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP。姿态目标使用旋转矩阵和 SO(3) 最短路径完成死区、滤波与限速，不经过 RPY。每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip`、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。
+工作空间包含五个 ROS2 包：`xr_rm_input` 负责手柄输入，`xr_rm_interfaces` 定义消息，
+`xr_rm_teleop` 实现遥操作与真机适配，`xr_rm_bringup` 提供启动和配置，
+`xr_rm_mujoco` 负责只读运动学显示。
 
-## 当前范围
+`single_arm_velocity_teleop` 每个实例只控制一台机械臂；双臂模式分别启动 `left_arm_teleop` 和 `right_arm_teleop`。
 
-已完成：
+## 环境与构建
 
-- PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
+在工作空间根目录执行：
-- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
-- RM75 真机连接适配，包含关节反馈缓存、`rm_movej_canfd` 关节透传、安全速度/加速度配置、可选初始化点位移动。
-- Placo 0.9.4 RM75 QP 逆解；收到首帧有效关节反馈后才启用，求解失败时保留上一组有效关节目标。
-- 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
-- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
-- XRoboToolkit bridge 读取左右手柄 pose、Grip、Trigger、摇杆和主副按键。
-
-暂未完成：
-
-- D405/D435 视频流、数据记录、相机标定和目标检测链路。
-- 双臂碰撞模型、任务级状态机、自动采摘策略。
-- PICO 端与 ROS 端的完整时间同步和状态回传。
-
-## 项目结构
-
-```text
-src/
-├── README.md                       # 项目主文档
-├── AGENTS.md                       # Codex 项目工作流和安全规则
-├── docs/superpowers/               # Superpowers 设计与实施计划
-├── xr_rm_bringup/
-│   ├── config/
-│   │   ├── 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
-│   └── tools/
-│       ├── launcher_ui.py          # 图形化调试启动面板
-│       └── realman_dual_arm_state_monitor.py
-├── xr_rm_input/
-│   ├── launch/
-│   │   └── udp_receiver.launch.py  # 低层 UDP 接收测试入口
-│   ├── test/
-│   │   └── test_controller_fields.py
-│   └── xr_rm_input/
-│       ├── udp_controller_receiver.py
-│       ├── xrobotoolkit_to_udp_bridge.py
-│       └── sample_udp_sender.py    # 本机扫轴/正弦模拟手柄 UDP 数据
-├── xr_rm_interfaces/
-│   └── msg/
-│       └── XrController.msg        # 手柄状态与位姿
-└── xr_rm_teleop/
-    ├── models/
-    │   ├── rm75/                   # 旧 RM75 模型资源（launch 不再选用）
-    │   ├── rm75_omnipicker/        # 旧单臂 OmniPicker 模型资源
-    │   └── dual_rm75/              # 当前左右臂统一使用的双 RM75 URDF 与网格
-    └── xr_rm_teleop/
-        ├── placo_ik_solver.py      # Placo 0.9.4 单步 QP 逆解
-        ├── single_arm_velocity_teleop.py
-        ├── realman_adapter.py
-        └── fun_peripheral.py
-```
-
-`single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。
-
-## Superpowers Git 约束
-
-使用 Superpowers 执行任务时，只允许按 skill 工作流创建本地 Git 提交。
-禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。skill 如需
-独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
-
-## 环境准备
-
-在工作空间根目录，也就是包含 `src/` 的目录执行：
 
 ```bash
 cd /home/robot/WS_xr
@@ -95,397 +49,125 @@ colcon build --symlink-install
 source install/setup.bash
 ```
 
-真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。
+遥操作和 MuJoCo 节点固定使用 `/home/robot/miniconda3/envs/xr/bin/python`，
+其中固定 Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0；不要从
+用户或系统 Python 覆盖这些版本。
 
-遥操作节点固定由 `/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。
+真机模式另需睿尔曼 Python API2；Mock 模式不依赖厂商 SDK。
 
-只读检查 Placo 版本：
+## 快速开始
+
+以下命令均在 `/home/robot/WS_xr` 执行，并先 source ROS2 与 `install/setup.bash`。
+
+### Mock
 
 ```bash
-/home/robot/miniconda3/envs/xr/bin/python -c \
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
-  "import importlib.metadata; print(importlib.metadata.version('placo'))"
 ```
 
-输出必须为 `0.9.4`。
+另开终端发送模拟手柄数据：
-
-如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：
 
 ```bash
-export XR_RM_WS=/home/robot/WS_xr
+ros2 run xr_rm_input sample_udp_sender \
-```
+  --hand both --host 127.0.0.1 --port 15000 \
-
-## 使用 launcher_ui.py 调试
-
-推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、source ROS2 与 `install/setup.bash`，并把每个命令放到独立终端中运行。
-
-源码方式启动：
-
-```bash
-cd /home/robot/WS_xr
-python3 src/xr_rm_bringup/tools/launcher_ui.py
-```
-
-构建后也可以通过 ROS2 入口启动：
-
-```bash
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 run xr_rm_bringup launcher_ui
-```
-
-面板顶部的 `Mode` 分为五类：
-
-- `Simulation`：左臂 mock、右臂 mock、双臂 mock、sample UDP 发送、one-click mock demo、controller 位置/频率监控。
-- `Left Arm`：左臂网络 ping、左臂真机 launch、左手 sample UDP。
-- `Right Arm`：右臂网络 ping、右臂真机 launch、右手 sample UDP。
-- `Dual Arm`：左右臂 ping、双臂真机 launch、双手 sample UDP。
-- `Diagnostics`：`ros2 doctor --report` 和核心包的 `ros2 pkg prefix` 检查。
-
-常用按钮：
-
-- `Run Selected`：运行当前选中的命令。双击列表项也可以运行。
-- `Check Env`：检查 ROS2 Humble、工作空间 build、终端、核心 ROS 包、睿尔曼 API2。
-- `Stop All`：结束由本工作空间启动的 launch、sample sender、topic monitor、相关 ROS 节点和终端窗口。
-
-每个模式都会附带基础监控入口：
-
-- `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`，用于快速看手柄位置字段和接收频率。
-
-分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：
-
-```bash
-sudo apt install terminator wmctrl xdotool
-sudo update-alternatives --config x-terminal-emulator
-```
-
-## 推荐调试顺序
-
-第一步：检查环境。
-
-打开 `launcher_ui.py`，点击 `Check Env`。如果 `install/setup.bash` 缺失，先回工作空间根目录重新执行 `colcon build --symlink-install`。
-
-第二步：分别跑左、右臂 mock 闭环。
-
-分两个终端依次验证左臂：
-
-```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
-ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port 15000 \
   --pattern axis_sweep --seconds 30
 ```
 
-停止左臂进程后，再分别验证右臂：
+单臂调试时将 `arm` 改为 `left` 或 `right`。推荐先分别完成左右单臂
+Mock，再进入双臂或真机验证。
+
+### MuJoCo
 
 ```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
+ros2 launch xr_rm_bringup arm_debug.launch.py \
-ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port 15000 \
+  arm:=both use_mock:=true use_mujoco:=true
-  --pattern axis_sweep --seconds 30
 ```
 
-`sample_udp_sender` 默认使用 `axis_sweep` 扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
+MuJoCo 只订阅关节状态，不参与控制。真机显示可将 `use_mock` 改为 `false`，
+但该命令会同时连接两台 RM75。
 
-观察：
+### PICO 输入
+
+只保留一个 UDP 输入源，然后启动 XRoboToolkit bridge：
 
 ```bash
-ros2 topic echo /xr/left_controller
+ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
-ros2 topic echo /xr/right_controller
+  --host 127.0.0.1 --port 15000 --hz 90
-ros2 topic echo /xr_rm/left_rm75/target_pose
-ros2 topic echo /xr_rm/right_rm75/target_pose
-ros2 topic echo /xr_rm/left_rm75/cmd_vel
-ros2 topic echo /xr_rm/right_rm75/cmd_vel
 ```
 
-第三步：单臂真机。
+确认左右 topic 持续接收数据：
 
-先只上一个臂，确认网络、方向、急停和限幅：
+```bash
+ros2 topic hz /xr/left_controller
+ros2 topic hz /xr/right_controller
+```
+
+图形启动面板可运行 `python3 src/xr_rm_bringup/tools/launcher_ui.py`，提供
+Simulation、MuJoCo、Real Hardware 和 Diagnostics 模式。
+
+### 真机
+
+确认对应 YAML 中 `move_to_initial_pose_on_connect: false`，再从单臂开始：
 
 ```bash
 ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
 ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
 ```
 
-所有 YAML 默认都不会执行 `movej(initial_joint_pose)`。只有确认安全区清空后，才可在当前使用的
+单臂方向、限位、急停、超时停止和夹爪均验证后，才能启动双臂：
-`left_arm_rm75.yaml`、`right_arm_rm75.yaml` 或 `dual_arm_rm75.yaml` 中将
-`move_to_initial_pose_on_connect` 改为 `true`。
-
-第四步：双臂真机。
 
 ```bash
 ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 ```
 
-双臂默认不会自动移动到初始化点；机器人地址、控制参数和初始化移动开关均从
+按住 `grip` 控制对应机械臂；松开后停止。点击 `trigger` 切换对应夹爪开/关。
-`dual_arm_rm75.yaml` 读取。
+左手 X、右手 A 会请求对应机械臂回到配置的初始位姿；真机使用前必须清空安全区。
 
-## Launch 入口说明
+## Launch 参数
 
-`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 mock、单臂真机和双臂真机按钮都调用它。
+统一入口为 `xr_rm_bringup/launch/arm_debug.launch.py`：
 
-常用参数：
+| 参数 | 默认值 | 说明 |
+| --- | --- | --- |
+| `arm` | `right` | `left`、`right` 或 `both` |
+| `use_mock` | `true` | `false` 会连接真机 |
+| `use_mujoco` | `false` | 仅支持 `arm:=both` |
+| `udp_host` | `0.0.0.0` | UDP 监听地址 |
+| `udp_port` | `15000` | UDP 监听端口 |
+| `udp_timer_hz` | `200.0` | UDP receiver 轮询频率 |
 
-- `arm`：`left`、`right`、`both`，默认 `right`。
+## 配置
-- `use_mock`：`true` 不连接真机，`false` 连接 RM75。
-- `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
-- `udp_port`：UDP 监听端口，默认 `15000`。
-- `udp_timer_hz`：UDP receiver 轮询频率，默认 `200.0`。
 
-机器人 IP/端口、控制频率、CANFD、限速、工具和初始化位姿等行为参数只由对应 YAML
+| 文件 | 用途 |
-配置，launch 不再提供同名覆盖项。
+| --- | --- |
+| `dual_arm_rm75.yaml` | 双臂节点、网络、控制与安全参数 |
+| `left_arm_rm75.yaml` | 左臂单独调试 |
+| `right_arm_rm75.yaml` | 右臂单独调试 |
+| `peripherals_rm75.yaml` | 工具坐标、负载和末端执行器 |
+| `dual_arm_mujoco.yaml` | MuJoCo 刷新频率 |
 
-## 配置文件说明
+修改某一侧控制参数时，同时检查单臂和双臂 YAML 是否需要同步。必须保留工作空间/
+圆柱限位、线速度与角速度限制、关节速度/加速度限制、指令超时和安全停止逻辑。
 
-`xr_rm_bringup/config/dual_arm_rm75.yaml` 是双臂配置主文件，包含两个 ROS 节点命名空间：
+`configure_safety_limits` 不得默认关闭；
+`move_to_initial_pose_on_connect` 必须保持默认 `false`。
 
-- `left_arm_teleop`
+## 测试
-- `right_arm_teleop`
 
-`left_arm_rm75.yaml` 和 `right_arm_rm75.yaml` 用于 `arm_debug.launch.py arm:=left/right` 的单臂调试，因为单臂节点名是 `single_arm_velocity_teleop`。
+在工作空间根目录执行：
-
-`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 映射不改为公共坐标系。
-
-重点控制参数：
-
-- `controller_topic`：订阅的手柄话题。
-- `scale`：手柄位移到 TCP 位移的比例。
-- `target_filter_alpha` / `target_filter_alpha_fast`：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
-- `target_filter_fast_threshold_m`：进入快速滤波区间的目标变化阈值。
-- `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
-- `enable_orientation_control`：是否把手柄相对旋转映射到 TCP 姿态。
-- `orientation_filter_alpha` / `orientation_deadband_rad`：按 SO(3) 最短旋转角处理的目标 TCP 姿态滤波和死区。
-- `max_orientation_speed`：目标 TCP 姿态沿 SO(3) 最短路径的最大角速度，当前为 `0.5 rad/s`。
-- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
-- `cyl_radius_limit`：基座圆柱半径限制。
-- `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
-- `robot_ip` / `robot_port`：RM75 TCP 控制连接地址。
-- `realtime_push_host_ip`：连接机械臂 Wi-Fi 后本机实际 IPv4；可用
-  `ip -4 route get 192.168.192.19` 查看输出中的 `src`，当前为 `192.168.192.148`。
-- `realtime_push_port`：UDP 主动反馈端口；左臂 `8089`、右臂 `8090`，同机双臂不能重复。
-- `realtime_push_cycle_ms`：UDP 主动反馈周期，当前为厂商支持的 `5 ms`。
-- `follow` / `canfd_trajectory_mode`：`rm_movej_canfd` 的高跟随和轨迹模式参数。
-- 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证；确认关节加速度与反馈稳定后，再单独测试高跟随。
-- `initial_joint_pose`：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。
-
-当前 `/xr/*_controller` 的坐标处理：
-
-- XRoboToolkit bridge 原样转发 SDK 的手柄位置和四元数，不额外转换坐标轴。
-- receiver 默认按 `xyzw` 解析四元数，也可通过 `quat_order:=wxyz` 切换。
-- 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
-- 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。
-- 两侧局部 `-Y` 均指向机器人前方；局部 `+Y` 指向后方，后方工作空间仅保留 `0.10 m`。
-- 左臂局部 `+X/+Y/+Z` 分别指向下/后/左外侧；右臂分别指向上/后/右外侧。
-
-如果某个机械臂方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
-
-## 末端工具开合
-
-真机 launch 默认会在遥操作节点内启用工具控制。左/右手柄 `trigger` 从低于阈值按到 `>= 0.95` 时，会切换一次对应夹爪开/关状态，并保持到下一次点击。`grip` 仍只控制机械臂运动，不影响夹爪 trigger 切换。
-
-也可以用 Bool 话题手动控制开合，`true` 表示打开，`false` 表示闭合：
 
 ```bash
-ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
-ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: false}"
-
-ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
-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` 分别切换夹爪。
-
-## UDP 数据格式
-
-当前 XRoboToolkit bridge 每个周期发送一个双手柄 JSON 包：
-
-```json
-{
-  "t": 12.345,
-  "source_time": 12.345,
-  "seq": 42,
-  "frame_id": "xr_world",
-  "controllers": {
-    "left": {
-      "hand": "left",
-      "grip": true,
-      "trigger": 0.0,
-      "axis": [0.2, -0.4],
-      "buttons": {
-        "primary": true,
-        "secondary": false
-      },
-      "pos": [-0.12, 1.05, 0.30],
-      "quat": [0.0, 0.0, 0.0, 1.0],
-      "pose_valid": true,
-      "pose_source": "xrobotoolkit"
-    },
-    "right": {
-      "hand": "right",
-      "grip": true,
-      "trigger": 1.0,
-      "axis": [-0.1, 0.3],
-      "buttons": {
-        "primary": false,
-        "secondary": true
-      },
-      "pos": [0.12, 1.05, 0.30],
-      "quat": [0.0, 0.0, 0.0, 1.0],
-      "pose_valid": true,
-      "pose_source": "xrobotoolkit"
-    }
-  }
-}
-```
-
-字段说明：
-
-- `t` / `source_time`：bridge 的 PC 单调时间，用于诊断发送周期。
-- `seq`：bridge 递增的 UDP 包序号，bridge 重启后重新计数。
-- `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
-- `grip`：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
-- `trigger`：经过 bridge 滞回处理的 `0.0/1.0` 值；上升沿切换对应夹爪状态。
-- `axis`：摇杆 `[x, y]`，每个分量限制在 `-1.0` 到 `1.0`。
-- `buttons.primary`：左手 X 键或右手 A 键。
-- `buttons.secondary`：左手 Y 键或右手 B 键。
-- `pos`：手柄位置，长度 3。
-- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
-- `pose_valid`：姿态是否可信；`false` 时接收端强制 `grip=false`。
-- `pose_source`：当前 bridge 使用 `xrobotoolkit`。
-
-`axis`、`buttons.primary` 和 `buttons.secondary` 会进入 `XrController`；旧 UDP
-包缺少这些字段时分别回退为 `[0,0]`、`false` 和 `false`。
-
-接收端发布的消息格式为：
-
-```text
-std_msgs/Header header
-string hand
-
-bool grip
-float32 trigger
-bool primary
-bool secondary
-float32[2] axis
-
-geometry_msgs/Pose pose
-```
-
-`udp_controller_receiver` 仍兼容调试用的单手柄包：可以直接发送带 `hand`、`pos`、
-`quat` 的 JSON object，也可以用 `controllers` list、顶层 `left/right`、
-`pose.position`、`position`、`p`、`q` 等常见字段。
-
-## 官方 XRoboToolkit bridge
-
-如果使用官方 XRoboToolkit APK 和 PC-Service，可以用 `xrobotoolkit_to_udp_bridge` 从本机 ROS Python 环境中的 `xrobotoolkit_sdk` 读取左右手柄数据，再转换成当前 `udp_controller_receiver` 支持的 UDP JSON。
-
-正式运行时不要同时启动官方 `PXREAClientUnity` / `RobotLinuxDemo` 可视化窗口。`/opt/apps/roboticsservice/run3D.sh` 会启动这个可视化 demo，适合单独确认 PICO 与 PC-Service 已连接；bridge 遥操作链路中只需要 PC-Service。
-
-运行前只保留一个 UDP 输入源。先清掉重复 bridge、sample sender 和官方 Unity 可视化 demo，再保留或启动 PC-Service：
-
-```bash
-pkill -f '[x]robotoolkit_to_udp_bridge'
-pkill -f '[s]ample_udp_sender'
-pkill -f '[R]obotLinuxDemo.x86_64'
-pkill -f '[P]XREAClientUnity'
-pgrep -af RoboticsServiceProcess || /opt/apps/roboticsservice/runService.sh
-```
-
-启动 ROS mock 接收链路：
-
-```bash
-cd /home/robot/WS_xr
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
-```
-
-另开终端启动 bridge：
-
-```bash
-cd /home/robot/WS_xr
-source ~/.bashrc
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
-  --host 127.0.0.1 --port 15000 --hz 90
-```
-
-bridge 默认对 grip/trigger 做轻量滞回：`grip` 按下阈值 `0.90`、松开阈值 `0.75`；`trigger` 按下阈值 `0.95`、松开阈值 `0.75`。启动日志会打印 PID、UDP endpoint 和阈值，便于确认当前只运行了一个 bridge。
-
-验证手柄数据是否进入 ROS：
-
-```bash
-ps -ef | grep -E 'xrobotoolkit_to_udp_bridge|sample_udp_sender|RobotLinuxDemo|PXREAClientUnity' | grep -v grep
-ros2 topic hz /xr/left_controller
-ros2 topic hz /xr/right_controller
-ros2 topic echo /xr/left_controller --field pose.position
-ros2 topic echo /xr/right_controller --field pose.position
-ros2 topic echo /xr/left_controller --field grip
-ros2 topic echo /xr/right_controller --field grip
-ros2 topic echo /xr/right_controller --field trigger
-```
-
-`/xr/left_controller` 和 `/xr/right_controller` 持续刷新、位置随手柄移动变化、`grip` 随握持键切换，即表示官方 XRoboToolkit 数据已经进入当前遥操作输入层。
-
-## 真机安全验证
-
-第一次接真机时按这个顺序走：
-
-1. 确认急停、网络、机械臂工作区和人员位置。
-2. `launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75`。
-3. 确认对应 YAML 中 `move_to_initial_pose_on_connect: false` 后单臂启动。
-4. 手握急停，按住 `grip` 后只做小幅单轴移动。
-5. 逐个确认上/下、前/后、左/右方向。
-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. 左右臂都确认后，再进入双臂模式。
-
-当前项目没有双臂碰撞检测/避障。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
-
-## 后续优化路线
-
-为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
-
-1. 稳定 PICO 数据链路：利用 `seq`、`source_time`、`pose_valid` 做频率、延迟、丢包和追踪状态统计，记录 `/xr/*_controller`、`/xr_rm/*/raw_target_pose`、`/xr_rm/*/target_pose`、`/xr_rm/*/target_clamped`、`/xr_rm/*/current_pose`。
-2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
-3. 细化末端执行器：增加夹爪状态反馈、力控比例、安全上限和现场可视化提示。
-4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
-5. 从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。
-
-## 常见问题
-
-`launcher_ui.py` 提示找不到 `install/setup.bash`：
-
-```bash
-cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 colcon build --symlink-install
-source install/setup.bash
+colcon test --event-handlers console_direct+
+colcon test-result --verbose
 ```
 
-真机模式提示缺少 `Robotic_Arm`：
+涉及遥操作姿态控制时，额外运行：
 
-```text
+```bash
-未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。
+pytest src/xr_rm_teleop/test/test_orientation_control.py
 ```
 
-Controller topic 没有数据：
+真机验证不属于自动测试。默认使用 `use_mock:=true`，未经现场安全确认不要连接或
-
+移动机械臂。
-- 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
-- 确认端口是 `15000`，或 launch 与发送端端口一致。
-- 用 `sample_udp_sender` 在本机验证接收链路。
-- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed；SDK
-  读取失败时 bridge 会发送 `pose_valid=false` 的停止包。
-
-机械臂不动：
-
-- 确认 `grip=true`。
-- 确认 `udp_controller_receiver` 终端没有持续 `pose_valid=false` 日志；该字段不会写入 `XrController` 消息，但会让接收端强制停止。
-- 确认 `/xr_rm/<arm>/raw_target_pose` 与 `/xr_rm/<arm>/target_pose` 是否在变化。
-- 确认 `/xr_rm/<arm>/target_clamped` 是否持续为 `true`，如果是，目标 TCP 可能被工作空间、圆柱半径或单帧步长限制夹住。
-- 确认真机 SDK 连接成功，且 RM75 没有报警或急停。

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/models/dual_rm75/Dual_arm.urdf

@@ -514,7 +514,7 @@
       <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
     </inertial>
     <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 -1.5708" />
       <geometry>
         <mesh filename="meshes/scissor.stl" scale="1 1 1" />
       </geometry>
@@ -523,7 +523,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 0" rpy="0 0 -1.5708" />
       <geometry>
         <mesh filename="meshes/scissor.stl" scale="1 1 1" />
       </geometry>

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/test/test_placo_transforms.py

@@ -95,6 +95,17 @@ def test_dual_urdf_has_expected_joints_meshes_and_tcp_frames() -> None:
             assert joint.find("origin").attrib["xyz"] == xyz
 
 
+def test_left_scissor_mesh_matches_physical_mount_rotation() -> None:
+    root = ElementTree.parse(DUAL_URDF_PATH).getroot()
+    link = root.find("link[@name='scissor_scissor_link']")
+
+    assert link is not None
+    assert link.find("visual/origin").attrib["rpy"] == "0 0 -1.5708"
+    assert link.find("collision/origin").attrib["rpy"] == "0 0 -1.5708"
+    tcp_joint = root.find("joint[@name='scissor_scissor_tcp_fixed']")
+    assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
+
+
 def _dual_placo_solver(
     arm: str,
     joint_degrees: list[float],

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