fix: 更新规范化逆运动学实验产物

.gitignore

@@ -43,4 +43,5 @@ AMENT_IGNORE
 # Built Visual Studio Code Extensions
 *.vsix
 
-.codex
+.codex
+.worktrees/

AGENTS.md

@@ -205,6 +205,7 @@
 * 类型标注方式。
 * 注释风格。
 * 测试组织方式。
+* 新生成的 Markdown（`.md`）文档统一使用中文撰写。
 
 如果项目已有 lint、format 或 test 命令，优先使用项目已有命令，不要擅自更换工具链。
 
@@ -227,7 +228,13 @@
 
 ## Git 与提交
 
-除非用户明确要求，否则不要自动提交、推送、创建分支或修改远程仓库。
+除非用户明确要求，否则不要自动提交、推送或修改远程仓库。
+
+使用 Superpowers 执行任务时，只允许按相关 skill 工作流创建本地 Git 提交；
+同一项变更生成的规格文档与实施计划必须合并为一次本地提交，不得分别提交。
+禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。相关 skill
+如需独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
+其他情况下，除非用户明确要求，不要自动创建分支。
 
 如果用户要求生成提交信息，提交信息应：
 

README.md

@@ -1,83 +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
-  -> 左右 RM75 笛卡尔相对位姿透传控制
-  -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
+  -> 相对 TCP 目标 + Placo QP
+  -> Mock 或 RM75 rm_movej_canfd
+  -> joint_states / 调试话题
+       -> 可选 xr_rm_mujoco/dual_arm_simulator
 ```
 
-当前控制方式是“手柄相对位姿透传”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，通过 `rm_movep_canfd` 下发目标位姿。松开 `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_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。
-- 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
-- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
-- 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。
-
-暂未完成：
-
-- D405/D435 视频流、数据记录、相机标定和目标检测链路。
-- 双臂碰撞模型、任务级状态机、自动采摘策略。
-- PICO 端与 ROS 端的完整时间同步和状态回传。
-
-## 项目结构
-
-```text
-src/
-├── README.md                       # 项目主文档
-├── CODEX.md                        # Codex/Claude Code 项目工作流和安全规则
-├── docs/
-│   └── pico_udp_sender_ubuntu22_setup.md   # Ubuntu 22.04 下 PICO UDP Sender 配置教程
-├── unity/
-│   ├── XR_RM_PICO_UDP_Sender/      # PICO 4 Ultra UDP Sender Unity 工程
-│   │   ├── Assets/Editor/          # Android/PICO 设置与 APK 构建菜单
-│   │   ├── Assets/Scripts/         # UDP sender、配置面板、KeepAwake
-│   │   ├── Assets/Resources/       # PICO 资源与 Roboto TMP 字体
-│   │   ├── Packages/               # Unity package manifest
-│   │   └── ProjectSettings/
-│   └── PICO-Unity-Integration-SDK-release_3.4.0/
-├── 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 接收测试入口
-│   └── xr_rm_input/
-│       ├── udp_controller_receiver.py
-│       └── sample_udp_sender.py    # 本机扫轴/正弦模拟手柄 UDP 数据
-├── xr_rm_interfaces/
-│   └── msg/
-│       └── XrController.msg        # hand/grip/trigger/pose
-└── xr_rm_teleop/
-    └── xr_rm_teleop/
-        ├── single_arm_velocity_teleop.py
-        ├── realman_adapter.py
-        └── fun_peripheral.py
-```
-
-`single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。
-
-## 环境准备
-
-在工作空间根目录，也就是包含 `src/` 的目录执行：
+在工作空间根目录执行：
 
 ```bash
 cd /home/robot/WS_xr
@@ -88,376 +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 覆盖这些版本。
 
-如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：
+真机模式另需睿尔曼 Python API2；Mock 模式不依赖厂商 SDK。
 
-```bash
-export XR_RM_WS=/home/robot/WS_xr
-```
+## 快速开始
 
-## 使用 launcher_ui.py 调试
+以下命令均在 `/home/robot/WS_xr` 执行，并先 source ROS2 与 `install/setup.bash`。
 
-推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、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 闭环。
-
-在 `Simulation` 模式运行 `One-Click Dual Mock Demo`，或分开运行：
+### Mock
 
 ```bash
 ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
-ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 \
-  --pattern axis_sweep --seconds 60 --both-mode staggered
 ```
 
-`sample_udp_sender` 默认使用 `axis_sweep` 成对扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。`--hand both --both-mode staggered --seconds 60` 会先左后右，适合肉眼确认左右臂方向；如果只想左右同时动，可用 `--both-mode synchronized`。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25`。
-
-观察：
+另开终端发送模拟手柄数据：
 
 ```bash
-ros2 topic echo /xr/left_controller
-ros2 topic echo /xr/right_controller
-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
+ros2 run xr_rm_input sample_udp_sender \
+  --hand both --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:=both use_mock:=true use_mujoco:=true
+```
+
+MuJoCo 只订阅关节状态，不参与控制。真机显示可将 `use_mock` 改为 `false`，
+但该命令会同时连接两台 RM75。
+
+### PICO 输入
+
+只保留一个 UDP 输入源，然后启动 XRoboToolkit bridge：
+
+```bash
+ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
+  --host 127.0.0.1 --port 15000 --hz 90
+```
+
+确认左右 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
 ```
 
-单臂真机默认执行配置文件中的 `movej(initial_joint_pose)`；现场需要跳过时，显式传入
-`move_to_initial_pose_on_connect:=false`。
-
-第四步：双臂真机。
+单臂方向、限位、急停、超时停止和夹爪均验证后，才能启动双臂：
 
 ```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
-  left_robot_ip:=192.168.192.18 \
-  right_robot_ip:=192.168.192.19
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
 ```
 
-双臂默认不会自动移动到初始化点。以后需要启用时，在确认安全区清空后显式打开：
+按住 `grip` 控制对应机械臂；松开后停止。点击 `trigger` 切换对应夹爪开/关。
+左手 X、右手 A 会请求对应机械臂回到配置的初始位姿；真机使用前必须清空安全区。
+
+## Launch 参数
+
+统一入口为 `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 轮询频率 |
+
+## 配置
+
+| 文件 | 用途 |
+| --- | --- |
+| `dual_arm_rm75.yaml` | 双臂节点、网络、控制与安全参数 |
+| `left_arm_rm75.yaml` | 左臂单独调试 |
+| `right_arm_rm75.yaml` | 右臂单独调试 |
+| `peripherals_rm75.yaml` | 工具坐标、负载和末端执行器 |
+| `dual_arm_mujoco.yaml` | MuJoCo 刷新频率 |
+
+修改某一侧控制参数时，同时检查单臂和双臂 YAML 是否需要同步。必须保留工作空间/
+圆柱限位、线速度与角速度限制、关节速度/加速度限制、指令超时和安全停止逻辑。
+
+`configure_safety_limits` 不得默认关闭；
+`move_to_initial_pose_on_connect` 必须保持默认 `false`。
+
+## 测试
+
+在工作空间根目录执行：
 
 ```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
-  move_to_initial_pose_on_connect:=true
-```
-
-## Launch 入口说明
-
-`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 mock、单臂真机和双臂真机按钮都调用它。
-
-常用参数：
-
-- `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`。
-- `left_robot_ip`：左臂 IP，默认 `192.168.192.18`。
-- `right_robot_ip`：右臂 IP，默认 `192.168.192.19`。
-- `robot_port`：RM75 TCP 端口，默认 `8080`。
-- `left_avoid_singularity` / `right_avoid_singularity`：左右臂避奇异参数，默认左 `0`、右 `1`。
-- `avoid_singularity`：非空时覆盖左右臂避奇异参数。
-- `frame_type`：`rm_movep_canfd` 坐标系类型，默认 `1`。
-- `control_rate_hz`：`rm_movep_canfd` 目标位姿发送频率，默认 `90.0`。
-- `follow`：传给 `rm_movep_canfd` 的跟随标志，默认 `false`。
-- `configure_safety_limits`：连接真机后是否配置速度/加速度安全参数，默认 `true`。
-- `enable_tool_control`：是否在遥操作节点内启用末端工具控制 topic，默认 `true`。
-- `enable_trigger_gripper_control`：是否允许用 `trigger` 点击切换对应夹爪状态，默认 `true`。
-- `trigger_close_threshold`：trigger 点击判定阈值，默认 `0.95`。
-- `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
-- `move_to_initial_pose_on_connect`：连接后是否执行 `movej(initial_joint_pose)`；默认 `auto`，单臂配置启用、双臂配置禁用，也可显式传 `true`/`false` 覆盖。
-
-## 配置文件说明
-
-`xr_rm_bringup/config/dual_arm_rm75.yaml` 是双臂配置主文件，包含两个 ROS 节点命名空间：
-
-- `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`、右臂 `scissorgripper=1`，真机连接阶段会初始化外设，后续开合命令复用同一个 RealMan 连接。
-
-重点控制参数：
-
-- `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`：目标 TCP 姿态低通和死区。
-- `max_orientation_speed`：目标姿态单帧步长限制对应的最大角速度。
-- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
-- `cyl_radius_limit`：基座圆柱半径限制。
-- `xr_to_robot_matrix`：`/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
-- `current_pose_poll_hz`：低频读取真机当前 TCP 的频率；控制中不再每帧阻塞读取状态。
-- `mock_initial_pose`：mock 模式初始 TCP 位姿。
-- `initial_joint_pose`：可选真机初始关节角。
-
-当前 `/xr/*_controller` 的 Project 坐标约定：
-
-- Project：`+X` 向右，`+Y` 向上，`+Z` 向后。
-- Unity APK 的 `Project (+Z back)` 会把 PXR `pxr_predict` 原始坐标转换为 `project.x=native.z`、`project.y=native.y`、`project.z=-native.x`。
-- `Source raw` 模式保留原始 pose source 坐标，只用于现场对照。
-- 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
-- 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。
-
-如果 `/xr/*_controller.pose.position` 已符合 Project 坐标，但某个机械臂方向相反，只改对应臂的 `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 数据格式
-
-当前 Unity APK 每个周期发送一个双手柄 JSON 包：
-
-```json
-{
-  "t": 12.345,
-  "source_time": 12.345,
-  "seq": 42,
-  "frame_id": "xr_world",
-  "controllers": {
-    "left": {
-      "grip": true,
-      "trigger": 0.0,
-      "pos": [-0.12, 1.05, 0.30],
-      "quat": [0.0, 0.0, 0.0, 1.0],
-      "pose_valid": true,
-      "pose_source": "pxr_predict",
-      "tracking_state": 3,
-      "controller_status": 2,
-      "grip_value": 1.0,
-      "axis": [0.0, 0.0],
-      "buttons": {
-        "grip": true,
-        "primary": false,
-        "secondary": false,
-        "menu": false,
-        "axis_click": false
-      }
-    },
-    "right": {
-      "grip": true,
-      "trigger": 0.4,
-      "pos": [0.12, 1.05, 0.30],
-      "quat": [0.0, 0.0, 0.0, 1.0],
-      "pose_valid": true,
-      "pose_source": "unity_xr",
-      "tracking_state": 3,
-      "controller_status": -1,
-      "grip_value": 0.8,
-      "axis": [0.0, 0.0],
-      "buttons": {
-        "grip": true,
-        "primary": false,
-        "secondary": false,
-        "menu": false,
-        "axis_click": false
-      }
-    }
-  }
-}
-```
-
-字段说明：
-
-- `t` / `source_time`：Unity 端 `Time.realtimeSinceStartupAsDouble`，用于后续延迟分析。
-- `seq`：Unity 端递增包序号，用于后续丢包分析。
-- `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
-- `grip`：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
-- `trigger`：扳机值，范围 `0.0-1.0`。真机模式下跨过 `0.95` 的上升沿会切换对应夹爪开/关状态。
-- `pos`：手柄位置，长度 3。
-- `quat`：手柄姿态四元数，默认按 `xyzw` 解析；遥操作节点会用 grip 锁定后的相对旋转控制 TCP 姿态。
-- `pose_valid`：姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。
-- `pose_source`：`pxr_predict`、`unity_xr`、`xrobotoolkit` 或 `none`，用于判断姿态来自 PICO 预测接口、Unity XR fallback 还是官方 XRoboToolkit SDK bridge。
-- `tracking_state` / `controller_status`：Unity/PICO 侧追踪诊断值，只用于日志和排查。
-- `grip_value`、`axis`、`buttons`：PICO 端输入诊断字段，当前不会写入 `XrController` 消息。
-
-`udp_controller_receiver` 仍兼容调试用的单手柄包：可以直接发送带 `hand`、`pos`、`quat` 的 JSON object，也可以用 `controllers` list、顶层 `left/right`、`pose.position`、`position`、`p`、`q` 等常见字段。四元数默认按 `xyzw` 解析，也可通过 `quat_order:=wxyz` 切换。
-
-PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并向 ROS2 主机发送 UDP 的详细步骤见 [docs/pico_udp_sender_ubuntu22_setup.md](docs/pico_udp_sender_ubuntu22_setup.md)。
-
-## 官方 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. 单臂启动，`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
-未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。
+```bash
+pytest src/xr_rm_teleop/test/test_orientation_control.py
 ```
 
-Controller topic 没有数据：
-
-- 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
-- 确认端口是 `15000`，或 launch 与发送端端口一致。
-- 用 `sample_udp_sender` 在本机验证接收链路。
-- 如果 Unity HUD 显示某个手柄 `invalid none`，ROS 侧会把该手柄 `grip` 强制置为 `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 没有报警或急停。
+真机验证不属于自动测试。默认使用 `use_mock:=true`，未经现场安全确认不要连接或
+移动机械臂。

docs/superpowers/plans/2026-07-29-rm75-feedback-recovery.md

@@ -0,0 +1,964 @@
+# RM75 关节反馈与故障恢复实施计划
+
+> **执行要求：** 使用 `superpowers:executing-plans` 按任务逐项实施。只有用户明确授权 subagent 后，才允许使用 `superpowers:subagent-driven-development`、独立 worktree 或本地分支。所有步骤使用复选框跟踪。
+
+**目标：** 启动时用 `rm_get_joint_degree()` 初始化 RM75 QP；运行时以 UDP `joint_position` 作为实际反馈；短暂丢包时保持最后安全目标；持续丢包或 CANFD 错误时安全同步、停止或等待人工重新使能。
+
+**实现方式：** 继续复用现有唯一 `RealManAdapter` 连接，只增加一个同步读取关节角的方法。恢复决策仍放在 `SingleArmVelocityTeleop`，用少量布尔状态复用现有 slow-stop、Grip 重新使能和关节限速逻辑，不新增状态机类、线程、连接或依赖。
+
+**技术栈：** Python 3.10、ROS2 Humble `rclpy`、睿尔曼 Python API2、Placo、pytest、ament/colcon。
+
+**设计文档：** `docs/superpowers/specs/2026-07-29-rm75-feedback-recovery-design.md`
+
+**厂商接口依据：**
+
+- `rm_get_joint_degree() -> tuple[int, list[float]]`：<https://develop.realman-robotics.com/robot/apipython/classes/armState/>
+- `rm_movej_canfd(..., follow=False, ...)` 为低跟随：<https://develop.realman-robotics.com/robot/apipython/classes/movePlan/>
+
+## 仓库约束
+
+- 所有构建、测试和启动命令均在工作空间根目录 `/home/robot/WS_xr` 执行。
+- 所有 Git 命令均在仓库根目录 `/home/robot/WS_xr/src` 执行。
+- 每次 ROS2 构建、测试或启动前先执行 `source /opt/ros/humble/setup.bash`。
+- 不自动提交、推送、创建分支或 worktree；只有用户明确要求后才执行。
+- 验证只通过 `xr_rm_bringup/launch/arm_debug.launch.py use_mock:=true`。
+- 不连接真机，不发送真实 CANFD，不移动机械臂，不操作夹爪或末端外设。
+- 不修改依赖、锁文件、CI、格式化配置、公开入口和无关代码。
+
+## 文件范围
+
+- 修改 `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
+  - 增加同步关节查询。
+  - 复用 UDP 与同步查询的角度校验。
+- 修改 `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+  - 启动同步、UDP保持/恢复、CANFD恢复和故障锁存。
+- 修改 `src/xr_rm_teleop/test/test_initial_joint_pose.py`
+  - 适配器同步查询测试。
+- 修改 `src/xr_rm_teleop/test/test_joint_control.py`
+  - 启动、超时、恢复和锁存测试。
+- 同步修改：
+  - `src/xr_rm_bringup/config/dual_arm_rm75.yaml`
+  - `src/xr_rm_bringup/config/left_arm_rm75.yaml`
+  - `src/xr_rm_bringup/config/right_arm_rm75.yaml`
+
+---
+
+## 任务一：给现有适配器增加同步关节查询
+
+**修改文件：**
+
+- `src/xr_rm_teleop/test/test_initial_joint_pose.py`
+- `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
+
+- [x] **步骤1：先写失败测试**
+
+在 `test_initial_joint_pose.py` 增加：
+
+```python
+def test_joint_degree_query_returns_validated_radians() -> None:
+    class FakeArm:
+        def rm_get_joint_degree(self):
+            return 0, [0.0, 10.0, -20.0, 30.0, -40.0, 50.0, -60.0]
+
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._arm = FakeArm()
+
+    snapshot = adapter.read_joint_state()
+
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [0, 10, -20, 30, -40, 50, -60]]
+    )
+    assert snapshot.read_duration_ms is not None
+
+
+@pytest.mark.parametrize(
+    "result",
+    [
+        (7, [0.0] * 7),
+        (0, [0.0] * 6),
+        (0, [0.0, 0.0, 0.0, math.nan, 0.0, 0.0, 0.0]),
+    ],
+)
+def test_joint_degree_query_rejects_sdk_errors_and_invalid_values(result) -> None:
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+    )
+    adapter._arm = SimpleNamespace(rm_get_joint_degree=lambda: result)
+
+    with pytest.raises((RuntimeError, ValueError)):
+        adapter.read_joint_state()
+
+
+def test_mock_joint_query_uses_current_mock_positions() -> None:
+    adapter = MockRealManAdapter([1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0])
+
+    snapshot = adapter.read_joint_state()
+
+    assert snapshot.positions == pytest.approx(
+        [math.radians(value) for value in [1, 2, 3, 4, 5, 6, 7]]
+    )
+```
+
+把现有 `test_invalid_udp_feedback_does_not_replace_snapshot` 的最终断言改为：
+
+```python
+after = adapter.get_latest_joint_state()
+assert after is not None
+assert before is not None
+assert after.positions == before.positions
+assert after.received_at == before.received_at
+assert after.motion_ready is False
+```
+
+该断言要求无效 UDP 帧保留最后已知角度，但立即禁止这些角度继续参与运动。
+
+- [x] **步骤2：运行测试并确认 RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
+  -k 'joint_degree_query or mock_joint_query or invalid_udp_feedback' -v
+```
+
+预期：测试失败；原因是适配器还没有 `read_joint_state()`，且无效 UDP 帧仍被标记为可运动。
+
+- [x] **步骤3：实现最小同步查询**
+
+给 `MockRealManAdapter` 增加：
+
+```python
+def read_joint_state(self) -> JointStateSnapshot:
+    return self.get_latest_joint_state()
+```
+
+给 `RealManAdapter` 增加：
+
+```python
+def read_joint_state(self) -> JointStateSnapshot:
+    self._require_arm()
+    started_at = time.monotonic()
+    result = self._arm.rm_get_joint_degree()
+    finished_at = time.monotonic()
+    if not isinstance(result, tuple) or len(result) != 2:
+        raise RuntimeError(
+            f"rm_get_joint_degree returned invalid result: {result!r}"
+        )
+    self._check_return(result, "rm_get_joint_degree")
+    return JointStateSnapshot(
+        self._joint_positions_from_degrees(
+            result[1],
+            "rm_get_joint_degree",
+        ),
+        finished_at,
+        (finished_at - started_at) * 1000.0,
+    )
+
+@staticmethod
+def _joint_positions_from_degrees(
+    values: Any,
+    source: str,
+) -> list[float]:
+    try:
+        degrees = list(values)
+    except TypeError as exc:
+        raise ValueError(f"{source} must contain 7 numeric joints") from exc
+    if len(degrees) != 7 or not all(
+        isinstance(value, Number) for value in degrees
+    ):
+        raise ValueError(f"{source} must contain 7 numeric joints")
+    positions = [math.radians(float(value)) for value in degrees]
+    if not all(math.isfinite(value) for value in positions):
+        raise ValueError(f"{source} contains NaN/Inf")
+    return positions
+```
+
+把 UDP 回调中重复的角度转换替换为：
+
+```python
+positions = self._joint_positions_from_degrees(
+    data.joint_status.joint_position,
+    "RM75 UDP feedback",
+)
+```
+
+在 UDP 回调的异常分支中，保留最后角度但标记为不可运动：
+
+```python
+with self._joint_state_lock:
+    if self._latest_joint_state is not None:
+        current = self._latest_joint_state
+        self._latest_joint_state = JointStateSnapshot(
+            list(current.positions),
+            current.received_at,
+            current.read_duration_ms,
+            current.update_interval_ms,
+            False,
+        )
+```
+
+- [x] **步骤4：运行适配器测试并确认 GREEN**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
+```
+
+预期：该文件全部测试通过。
+
+- [x] **步骤5：检查本任务差异**
+
+```bash
+git diff --check
+git diff -- \
+  xr_rm_teleop/xr_rm_teleop/realman_adapter.py \
+  xr_rm_teleop/test/test_initial_joint_pose.py
+```
+
+预期：只有同步查询、共用角度校验、无效反馈安全标记及对应测试。
+
+---
+
+## 任务二：用启动查询结果初始化 QP 和关节命令历史
+
+**修改文件：**
+
+- `src/xr_rm_teleop/test/test_joint_control.py`
+- `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- [x] **步骤1：先写启动同步失败测试**
+
+增加：
+
+```python
+def test_startup_joint_query_initializes_qp_and_command_history() -> None:
+    positions = [0.1] * 7
+    pose = np.eye(4)
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._adapter = SimpleNamespace(
+        read_joint_state=lambda: JointStateSnapshot(
+            positions,
+            time.monotonic(),
+        )
+    )
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: pose
+    )
+    teleop.get_logger = lambda: FakeLogger()
+
+    teleop._initialize_joint_state()
+
+    assert teleop._latest_joint_positions == positions
+    assert teleop._last_valid_joint_target == positions
+    assert teleop._last_joint_command_target == positions
+    assert teleop._last_joint_command_velocity == [0.0] * 7
+    assert teleop._last_current_pose is pose
+
+
+def test_startup_joint_query_failure_closes_adapter() -> None:
+    class FailingAdapter:
+        def __init__(self):
+            self.close_calls = 0
+
+        def read_joint_state(self):
+            raise RuntimeError("rm_get_joint_degree failed with code 7")
+
+        def close(self):
+            self.close_calls += 1
+
+    errors = []
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "left_rm75"
+    teleop._adapter = FailingAdapter()
+    teleop.get_logger = lambda: SimpleNamespace(
+        error=lambda message: errors.append(message)
+    )
+
+    with pytest.raises(RuntimeError, match="code 7"):
+        teleop._initialize_joint_state()
+
+    assert teleop._adapter.close_calls == 1
+    assert "left_rm75" in errors[0]
+    assert "启动关节同步失败" in errors[0]
+```
+
+- [x] **步骤2：运行测试并确认 RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'startup_joint_query' -v
+```
+
+预期：测试因 `_initialize_joint_state()` 尚不存在而失败。
+
+- [x] **步骤3：增加启动同步**
+
+增加：
+
+```python
+def _initialize_joint_state(self) -> None:
+    try:
+        self._reset_joint_state(self._adapter.read_joint_state())
+    except Exception as exc:
+        self.get_logger().error(
+            f"{self._arm_name} 启动关节同步失败：{exc}"
+        )
+        self._adapter.close()
+        raise
+
+def _reset_joint_state(
+    self,
+    snapshot: JointStateSnapshot,
+) -> np.ndarray:
+    current_pose = self._ik_solver.update_joint_state(snapshot.positions)
+    positions = list(snapshot.positions)
+    self._latest_joint_positions = positions
+    self._last_current_pose = current_pose
+    self._last_valid_joint_target = list(positions)
+    self._last_joint_command_target = list(positions)
+    self._last_joint_command_velocity = [0.0] * 7
+    return current_pose
+```
+
+在现有适配器连接之后、外设初始化之前调用：
+
+```python
+self._adapter = self._make_adapter()
+self._adapter.connect()
+self._initialize_joint_state()
+self._setup_tool_control()
+```
+
+同步查询结果不得写入 `RealManAdapter._latest_joint_state`；该缓存继续只代表 UDP 反馈。
+
+- [x] **步骤4：运行启动与适配器测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'startup_joint_query or first_feedback' -v
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
+```
+
+预期：所选控制测试和全部适配器测试通过。
+
+- [x] **步骤5：检查本任务差异**
+
+```bash
+git diff --check
+git diff -- \
+  xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  xr_rm_teleop/test/test_joint_control.py
+```
+
+预期：启动阶段只增加一次同步读取，并初始化现有 QP/关节命令字段。
+
+---
+
+## 任务三：UDP 短暂超时保持，持续超时重新同步
+
+**修改文件：**
+
+- `src/xr_rm_teleop/test/test_joint_control.py`
+- `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- [x] **步骤1：先写超时行为测试**
+
+在测试文件增加最小构造器：
+
+```python
+def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._adapter = adapter
+    teleop._arm_name = "right_rm75"
+    teleop._follow = False
+    teleop._active = True
+    teleop._joint_feedback_ready = True
+    teleop._grip_rearm_required = False
+    teleop._feedback_resync_attempted = False
+    teleop._control_fault_latched = False
+    teleop._last_joint_command_target = [0.1] * 7
+    teleop._last_joint_command_velocity = [0.0] * 7
+    teleop._latest_joint_positions = [0.1] * 7
+    teleop._last_valid_joint_target = [0.1] * 7
+    teleop._last_current_pose = np.eye(4)
+    teleop._controller_start = None
+    teleop._controller_orientation_start = None
+    teleop._robot_start_transform = None
+    teleop._filtered_target = None
+    teleop._filtered_orientation_target = None
+    teleop._last_sent_target = None
+    teleop._last_sent_orientation = None
+    teleop._last_command_time = None
+    teleop._ik_solver = SimpleNamespace(
+        update_joint_state=lambda joints: np.eye(4)
+    )
+    teleop._stop_sent = False
+    teleop._feedback_resync_timeout_sec = 0.5
+    teleop._publish_stop_debug = lambda: None
+    teleop.get_logger = lambda: FakeLogger()
+    return teleop
+```
+
+增加：
+
+```python
+def test_missing_or_disabled_joint_snapshot_is_not_motion_ready() -> None:
+    assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(None)
+    assert not SingleArmVelocityTeleop._joint_snapshot_is_motion_ready(
+        JointStateSnapshot(
+            [0.0] * 7,
+            time.monotonic(),
+            motion_ready=False,
+        )
+    )
+
+
+def test_short_udp_timeout_repeats_last_limited_target_without_query() -> None:
+    sends = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sends.append(
+            (list(joints), follow)
+        ),
+        read_joint_state=lambda: pytest.fail("query must not run"),
+        stop=lambda: pytest.fail("stop must not run"),
+    )
+    teleop = _timeout_teleop(adapter)
+
+    teleop._handle_stale_joint_feedback(0.2)
+
+    assert sends == [([0.1] * 7, False)]
+    assert teleop._last_joint_command_target == [0.1] * 7
+    assert teleop._grip_rearm_required
+
+
+def test_short_udp_timeout_without_active_target_stays_stopped() -> None:
+    stop_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: pytest.fail(
+            "inactive control must not start CANFD output"
+        ),
+        read_joint_state=lambda: pytest.fail("query must not run"),
+        stop=lambda: stop_calls.append(True),
+    )
+    teleop = _timeout_teleop(adapter)
+    teleop._active = False
+
+    teleop._handle_stale_joint_feedback(0.2)
+
+    assert len(stop_calls) == 1
+
+
+def test_persistent_udp_timeout_queries_once_and_holds_actual_position() -> None:
+    sends = []
+    query_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sends.append(list(joints)),
+        read_joint_state=lambda: (
+            query_calls.append(True)
+            or JointStateSnapshot([0.2] * 7, time.monotonic())
+        ),
+        stop=lambda: None,
+    )
+    teleop = _timeout_teleop(adapter)
+
+    teleop._handle_stale_joint_feedback(0.5)
+    teleop._handle_stale_joint_feedback(0.6)
+
+    assert len(query_calls) == 1
+    assert sends == [[0.2] * 7, [0.2] * 7]
+    assert teleop._last_valid_joint_target == [0.2] * 7
+    assert teleop._last_joint_command_velocity == [0.0] * 7
+
+
+def test_persistent_udp_timeout_query_failure_latches_control() -> None:
+    stop_calls = []
+    adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: pytest.fail(
+            "CANFD must stop after query failure"
+        ),
+        read_joint_state=lambda: (_ for _ in ()).throw(
+            RuntimeError("rm_get_joint_degree failed with code 7")
+        ),
+        stop=lambda: stop_calls.append(True),
+    )
+    teleop = _timeout_teleop(adapter)
+
+    teleop._handle_stale_joint_feedback(0.5)
+    teleop._handle_stale_joint_feedback(0.6)
+
+    assert teleop._control_fault_latched
+    assert len(stop_calls) == 1
+```
+
+删除旧的 `_fresh_joint_state()` 直接测试，并用
+`test_short_udp_timeout_without_active_target_stays_stopped` 替换旧的
+`test_stale_feedback_stops_before_active_control`。
+
+在 `test_feedback_fault_blocks_grip_until_release` 中补齐：
+
+```python
+teleop._control_fault_latched = False
+teleop._feedback_resync_attempted = False
+```
+
+- [x] **步骤2：运行测试并确认 RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'udp_timeout or joint_snapshot' -v
+```
+
+预期：测试因超时处理和锁存状态尚不存在而失败。
+
+- [x] **步骤3：增加参数与最小状态**
+
+参数默认值：
+
+```python
+self.declare_parameter("control_rate_hz", 90.0)
+self.declare_parameter("command_timeout_sec", 0.12)
+self.declare_parameter("feedback_resync_timeout_sec", 0.5)
+```
+
+读取并初始化：
+
+```python
+self._feedback_resync_timeout_sec = float(
+    self.get_parameter("feedback_resync_timeout_sec").value
+)
+self._feedback_resync_attempted = False
+self._control_fault_latched = False
+```
+
+在 `_validate_parameters()` 中增加：
+
+```python
+if self._feedback_resync_timeout_sec <= self._command_timeout_sec:
+    raise ValueError(
+        "feedback_resync_timeout_sec must be greater than command_timeout_sec"
+    )
+```
+
+- [x] **步骤4：增加保持、重新同步和锁存逻辑**
+
+增加：
+
+```python
+def _handle_stale_joint_feedback(self, age: float) -> None:
+    if self._control_fault_latched:
+        return
+    self._grip_rearm_required = True
+    if self._joint_feedback_ready:
+        self.get_logger().warn(
+            f"{self._arm_name} UDP关节反馈超时，保持最后安全目标。"
+        )
+    self._joint_feedback_ready = False
+
+    if (
+        age >= self._feedback_resync_timeout_sec
+        and not self._feedback_resync_attempted
+    ):
+        self._feedback_resync_attempted = True
+        self.get_logger().warn(
+            f"{self._arm_name} UDP关节反馈持续超时，"
+            "尝试rm_get_joint_degree重新同步。"
+        )
+        try:
+            self._reset_joint_state(self._adapter.read_joint_state())
+        except Exception as exc:
+            self._latch_control_fault(
+                f"UDP关节反馈持续超时且重新同步失败：{exc}"
+            )
+            return
+        self.get_logger().info(
+            f"{self._arm_name} 已通过rm_get_joint_degree重新同步，"
+            "继续保持并等待UDP恢复。"
+        )
+
+    if self._active and self._last_joint_command_target is not None:
+        self._repeat_last_joint_target()
+    else:
+        self._safe_stop(reset_active=True)
+
+def _repeat_last_joint_target(self) -> None:
+    target = self._last_joint_command_target
+    if target is None:
+        return
+    self._adapter.send_joint_target(list(target), self._follow)
+    self._stop_sent = False
+
+def _latch_control_fault(self, message: str) -> None:
+    if self._control_fault_latched:
+        return
+    self._control_fault_latched = True
+    self._grip_rearm_required = True
+    self.get_logger().error(
+        f"{self._arm_name} 控制故障已锁存：{message}"
+    )
+    self._safe_stop(reset_active=True)
+```
+
+- [x] **步骤5：在 QP 之前处理反馈状态**
+
+在 `_control_tick()` 开头用以下逻辑替换现有 `_fresh_joint_state()` 分支：
+
+```python
+if self._control_fault_latched:
+    return
+
+snapshot = self._adapter.get_latest_joint_state()
+if not self._joint_snapshot_is_motion_ready(snapshot):
+    self._grip_rearm_required = True
+    if self._joint_feedback_ready:
+        self.get_logger().warn(
+            f"{self._arm_name} 关节反馈无效或机械臂未就绪，机械臂停止。"
+        )
+    self._joint_feedback_ready = False
+    self._safe_stop(reset_active=True)
+    return
+
+feedback_age = time.monotonic() - snapshot.received_at
+if feedback_age < 0.0:
+    self._grip_rearm_required = True
+    self._joint_feedback_ready = False
+    self._safe_stop(reset_active=True)
+    return
+if feedback_age > self._command_timeout_sec:
+    self._handle_stale_joint_feedback(feedback_age)
+    return
+
+self._feedback_resync_attempted = False
+```
+
+在成功执行 `_sync_joint_feedback(snapshot)` 后，用以下逻辑替换现有首次反馈日志：
+
+```python
+if not self._joint_feedback_ready:
+    if self._grip_rearm_required:
+        message = (
+            f"{self._arm_name} UDP关节反馈已恢复，"
+            "等待Grip松开后重新使能。"
+        )
+    else:
+        message = (
+            f"{self._arm_name} 已收到首帧有效关节反馈，QP可以启用。"
+        )
+    self.get_logger().info(message)
+    self._joint_feedback_ready = True
+```
+
+用以下静态校验替换 `_fresh_joint_state()`：
+
+```python
+@staticmethod
+def _joint_snapshot_is_motion_ready(
+    snapshot: JointStateSnapshot | None,
+) -> bool:
+    return (
+        snapshot is not None
+        and len(snapshot.positions) == 7
+        and all(math.isfinite(value) for value in snapshot.positions)
+        and snapshot.motion_ready
+    )
+```
+
+XR手柄消息超时、Grip逻辑、工作空间/圆柱限位、姿态限速和关节限速保持原样。
+
+- [x] **步骤6：运行超时及反馈安全测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'udp_timeout or feedback_fault or joint_snapshot' -v
+```
+
+预期：所选测试通过；短暂超时不调用 QP 和同步查询，机械臂未就绪仍立即停止。
+
+- [x] **步骤7：检查本任务差异**
+
+```bash
+git diff --check
+git diff --stat
+```
+
+预期：没有新增状态机类、线程、依赖、连接或常态轮询。
+
+---
+
+## 任务四：CANFD 错误后停止、查询并等待人工恢复
+
+**修改文件：**
+
+- `src/xr_rm_teleop/test/test_joint_control.py`
+- `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- [x] **步骤1：先写 CANFD 恢复测试**
+
+用以下测试替换旧的 `test_joint_send_failure_requests_slow_stop_and_resets_control`：
+
+```python
+def test_canfd_error_stops_queries_and_requires_grip_rearm() -> None:
+    class RecoveringAdapter:
+        def __init__(self):
+            self.stop_calls = 0
+            self.read_calls = 0
+
+        def send_joint_target(self, joints, follow):
+            raise RuntimeError("rm_movej_canfd failed with code 9")
+
+        def stop(self):
+            self.stop_calls += 1
+
+        def read_joint_state(self):
+            self.read_calls += 1
+            return JointStateSnapshot([0.2] * 7, time.monotonic())
+
+    teleop = _timeout_teleop(RecoveringAdapter())
+    teleop._joint_command_max_speed = math.radians(180.0)
+    teleop._joint_command_max_acceleration = math.radians(300.0)
+    teleop._dt = 1.0 / 90.0
+
+    sent = teleop._send_joint_target([0.3] * 7)
+
+    assert not sent
+    assert teleop._adapter.stop_calls == 1
+    assert teleop._adapter.read_calls == 1
+    assert not teleop._control_fault_latched
+    assert teleop._grip_rearm_required
+    assert teleop._last_joint_command_target == [0.2] * 7
+
+
+def test_canfd_error_latches_when_joint_query_also_fails() -> None:
+    class FailingAdapter:
+        def __init__(self):
+            self.stop_calls = 0
+
+        def send_joint_target(self, joints, follow):
+            raise RuntimeError("rm_movej_canfd failed with code 9")
+
+        def stop(self):
+            self.stop_calls += 1
+
+        def read_joint_state(self):
+            raise RuntimeError("rm_get_joint_degree failed with code 7")
+
+    teleop = _timeout_teleop(FailingAdapter())
+    teleop._joint_command_max_speed = math.radians(180.0)
+    teleop._joint_command_max_acceleration = math.radians(300.0)
+    teleop._dt = 1.0 / 90.0
+
+    assert not teleop._send_joint_target([0.3] * 7)
+    assert teleop._control_fault_latched
+    assert teleop._adapter.stop_calls == 1
+```
+
+- [x] **步骤2：运行测试并确认 RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'canfd_error' -v
+```
+
+预期：测试失败；当前发送错误只会 slow-stop，不会查询实际关节角或锁存查询失败。
+
+- [x] **步骤3：增加统一 CANFD 恢复路径**
+
+增加：
+
+```python
+def _recover_from_canfd_error(self, send_error: Exception) -> None:
+    self.get_logger().error(
+        f"{self._arm_name} rm_movej_canfd发送失败：{send_error}"
+    )
+    self._grip_rearm_required = True
+    self._send_stop_once()
+    self._safe_stop(reset_active=True)
+    try:
+        snapshot = self._adapter.read_joint_state()
+        self._reset_joint_state(snapshot)
+    except Exception as query_error:
+        self._latch_control_fault(
+            "CANFD错误后关节同步失败："
+            f"send={send_error}; query={query_error}"
+        )
+        return
+    self.get_logger().info(
+        f"{self._arm_name} CANFD错误后已同步实际关节角，"
+        "等待UDP恢复及Grip重新使能。"
+    )
+```
+
+把 `_send_joint_target()` 的异常分支替换为：
+
+```python
+except Exception as exc:
+    self._recover_from_canfd_error(exc)
+    return False
+```
+
+让短暂超时重发也走相同错误恢复：
+
+```python
+def _repeat_last_joint_target(self) -> None:
+    target = self._last_joint_command_target
+    if target is None:
+        return
+    try:
+        self._adapter.send_joint_target(list(target), self._follow)
+        self._stop_sent = False
+    except Exception as exc:
+        self._recover_from_canfd_error(exc)
+```
+
+- [x] **步骤4：运行全部关节控制测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v
+```
+
+预期：全部关节控制测试通过。
+
+- [x] **步骤5：检查日志与差异**
+
+```bash
+rg -n "rm_movej_canfd发送失败|控制故障已锁存|rm_get_joint_degree" \
+  src/xr_rm_teleop/xr_rm_teleop
+git diff --check
+```
+
+预期：
+
+- CANFD错误和查询错误都包含机械臂名称及失败阶段。
+- 锁存分支不会每周期重复打印错误。
+- 正常QP输出和超时保持使用同一个CANFD恢复入口。
+
+---
+
+## 任务五：同步90 Hz配置并完成mock验证
+
+**修改文件：**
+
+- `src/xr_rm_bringup/config/dual_arm_rm75.yaml`
+- `src/xr_rm_bringup/config/left_arm_rm75.yaml`
+- `src/xr_rm_bringup/config/right_arm_rm75.yaml`
+
+- [x] **步骤1：只修改请求中的控制参数**
+
+三份配置的每个机械臂条目统一为：
+
+```yaml
+control_rate_hz: 90.0
+command_timeout_sec: 0.12
+feedback_resync_timeout_sec: 0.5
+```
+
+保留低跟随：
+
+```yaml
+follow: false
+```
+
+不得修改：
+
+- 工作空间与圆柱限位。
+- TCP线速度、角速度及关节速度/加速度限制。
+- `configure_safety_limits: true`。
+- `move_to_initial_pose_on_connect: false`。
+- 机械臂IP、端口、初始位姿和末端工具配置。
+- 双臂节点名 `left_arm_teleop`、`right_arm_teleop`。
+
+- [x] **步骤2：机械检查三份配置**
+
+```bash
+rg -n "control_rate_hz|command_timeout_sec|feedback_resync_timeout_sec|follow:" \
+  src/xr_rm_bringup/config/dual_arm_rm75.yaml \
+  src/xr_rm_bringup/config/left_arm_rm75.yaml \
+  src/xr_rm_bringup/config/right_arm_rm75.yaml
+```
+
+预期：共四个机械臂配置条目，每个条目均为90.0、0.12、0.5和`follow: false`；三份文件不再出现125.0。
+
+- [x] **步骤3：运行相关测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v
+python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py -v
+```
+
+预期：三个命令均以0退出且无失败。
+
+- [x] **步骤4：构建ROS2工作空间**
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`、`xr_rm_bringup` 构建成功。
+
+- [x] **步骤5：通过统一入口进行mock启动验证**
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+if timeout --signal=INT 10s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=true udp_port:=15123
+then
+  true
+else
+  launch_status=$?
+  test "$launch_status" -eq 124
+fi
+```
+
+预期：
+
+- `udp_controller_receiver` 与 `single_arm_velocity_teleop` 正常启动。
+- 遥操作节点报告90 Hz、低跟随，并完成mock关节状态初始化。
+- 不导入厂商SDK，不建立RealMan连接，不发送CANFD，不移动机械臂，不操作夹爪。
+- 10秒后由`timeout`结束；仅该超时允许退出码124。
+
+- [x] **步骤6：最终范围与安全审计**
+
+```bash
+git diff --check
+git status --short
+git diff --stat
+git diff -- \
+  xr_rm_teleop/xr_rm_teleop/realman_adapter.py \
+  xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  xr_rm_teleop/test/test_initial_joint_pose.py \
+  xr_rm_teleop/test/test_joint_control.py \
+  xr_rm_bringup/config/dual_arm_rm75.yaml \
+  xr_rm_bringup/config/left_arm_rm75.yaml \
+  xr_rm_bringup/config/right_arm_rm75.yaml
+```
+
+逐项确认：
+
+- 没有无关文件或格式化改动。
+- 没有提交、推送、分支、worktree、锁文件、CI、格式化规则或依赖变化。
+- 没有新增线程、ROS包、launch入口、并发RealMan连接或常态SDK轮询。
+- mock模式不导入、不依赖厂商SDK。
+- `configure_safety_limits` 默认仍为开启。
+- `move_to_initial_pose_on_connect` 默认仍为关闭。
+- `left_arm_teleop`、`right_arm_teleop` 节点名不变。
+- 工作空间/圆柱限位、TCP与关节限速、XR命令超时和slow-stop逻辑仍保留。
+- 验证期间未连接真机、移动机械臂或操作夹爪。

docs/superpowers/plans/2026-07-29-rm75-qp-convergence.md

@@ -0,0 +1,412 @@
+# RM75 QP 收敛优化实施计划
+
+> **执行要求：** 使用 `superpowers:executing-plans` 逐项执行。用户未授权
+> subagent、独立worktree或本地分支，因此本计划只允许当前会话内联实施。所有
+> 步骤使用复选框跟踪。
+
+**目标：** 将当前每周期单步QP改为有界迭代QP，使低跟随RM75在手柄移动10 cm
+后约1秒内稳定到位，并消除由近距离台阶目标造成的持续轻微晃动。
+
+**实现方式：** 每个正常控制周期仍先用UDP实际关节角同步Placo，然后在一次
+`PlacoIkSolver.solve()`内部最多迭代30次，提前达到1 mm位置误差和0.005 rad
+姿态误差即返回。最终关节解继续经过现有90 Hz关节速度与加速度限幅后，以
+`follow=false`发送；不增加预测状态、线程、连接、依赖或配置参数。
+
+**技术栈：** Python 3.10、ROS2 Humble、Placo 0.9.4、NumPy、pytest、
+ament/colcon。
+
+**设计文档：**
+`docs/superpowers/specs/2026-07-29-rm75-qp-convergence-design.md`
+
+---
+
+## 仓库与安全约束
+
+- 构建、测试和启动命令在 `/home/robot/WS_xr` 执行。
+- Git命令在 `/home/robot/WS_xr/src` 执行。
+- 每次构建、测试或启动前执行 `source /opt/ros/humble/setup.bash`。
+- 不自动提交、推送、创建分支或worktree。
+- 不连接真机，不发送真实CANFD，不移动机械臂，不操作夹爪。
+- 只通过 `arm_debug.launch.py arm:=right use_mock:=true`进行启动验证。
+- 不修改三份机械臂YAML、RealMan适配器、launch、UI、依赖或公开入口。
+- 保留工作空间、圆柱、TCP速度、姿态速度、关节速度、关节加速度、反馈超时、
+  CANFD恢复、Grip重新使能和安全停止逻辑。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/test/test_placo_transforms.py`
+  - 增加真实Placo 7 cm目标收敛回归测试。
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
+  - 增加固定上限、提前收敛和逐步安全校验。
+
+不需要修改 `single_arm_velocity_teleop.py`；现有 `_solve_joint_target()` 已负责
+QP异常时打印限频警告并保持上一组安全关节目标，现有
+`_limit_joint_command_step()` 已负责最终90 Hz真实命令限速。
+
+---
+
+## 任务一：用真实Placo复现单步QP不收敛
+
+**修改文件：**
+
+- `xr_rm_teleop/test/test_placo_transforms.py`
+
+- [x] **步骤1：增加测试辅助函数**
+
+在文件顶部增加：
+
+```python
+import math
+```
+
+在现有URDF结构测试之后增加：
+
+```python
+def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]:
+    pytest.importorskip("placo")
+    urdf_path = (
+        Path(__file__).resolve().parents[1]
+        / "models"
+        / "rm75_omnipicker"
+        / "urdf"
+        / "RM75-B_OmniPicker_fixed.urdf"
+    )
+    joints = [
+        math.radians(value)
+        for value in [
+            -90.14,
+            3.76,
+            -86.89,
+            87.89,
+            -96.53,
+            -79.62,
+            -90.04,
+        ]
+    ]
+    return PlacoIkSolver(str(urdf_path), 1.0 / 90.0), joints
+```
+
+`importorskip()`只让没有Placo的普通系统Python跳过真模型用例；下面的RED/GREEN
+命令会显式加入项目现有Placo 0.9.4路径，因此该用例必须实际执行而不能跳过。
+
+- [x] **步骤2：增加7 cm目标收敛测试**
+
+增加：
+
+```python
+def test_qp_solve_converges_to_reachable_tcp_target() -> None:
+    solver, joints = _rm75_placo_solver()
+    start_pose = solver.update_joint_state(joints)
+    target_pose = start_pose.copy()
+    target_pose[0, 3] += 0.07
+
+    result = solver.solve(target_pose)
+    reached_pose = solver.update_joint_state(result)
+    position_error = np.linalg.norm(
+        target_pose[:3, 3] - reached_pose[:3, 3]
+    )
+    rotation_delta = (
+        target_pose[:3, :3] @ reached_pose[:3, :3].T
+    )
+    orientation_error = math.acos(
+        float(
+            np.clip(
+                (np.trace(rotation_delta) - 1.0) * 0.5,
+                -1.0,
+                1.0,
+            )
+        )
+    )
+
+    assert position_error <= 1e-3
+    assert orientation_error <= 5e-3
+```
+
+该测试验证一次公开 `solve()` 调用返回当前TCP目标对应的收敛关节解，而不是验证
+内部迭代次数。
+
+- [x] **步骤3：运行测试并确认RED**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
+PYTHONPATH="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_converges_to_reachable_tcp_target \
+  -v
+```
+
+预期：测试以位置误差约0.063 m大于0.001 m失败，证明当前单步QP确实不能在一次
+调用内给出收敛关节目标。测试不得因导入错误或跳过而结束。
+
+---
+
+## 任务二：实现有界迭代QP
+
+**修改文件：**
+
+- `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
+
+- [x] **步骤1：增加固定收敛常量**
+
+把模块说明改为：
+
+```python
+"""RM75 的 Placo 0.9.4 有界迭代 QP 逆解。"""
+```
+
+在现有常量后增加：
+
+```python
+QP_MAX_ITERATIONS = 30
+QP_POSITION_TOLERANCE_M = 1e-3
+QP_ORIENTATION_TOLERANCE_RAD = 5e-3
+```
+
+这些值是本次已确认的算法边界，不新增ROS参数。
+
+- [x] **步骤2：增加任务误差读取**
+
+在 `solve()` 前增加：
+
+```python
+def _target_errors(self) -> tuple[float, float]:
+    position_task = self._frame_task.position()
+    orientation_task = self._frame_task.orientation()
+    position_task.update()
+    orientation_task.update()
+    return (
+        float(position_task.error_norm()),
+        float(orientation_task.error_norm()),
+    )
+```
+
+Placo在 `solve(True)` 后只更新关节状态；先更新机器人运动学，再显式更新两个任务，
+确保 `error_norm()`对应当前迭代后的状态而不是前一迭代。
+
+- [x] **步骤3：把单步求解改为最多30次且提前收敛**
+
+用以下实现替换现有 `solve()`：
+
+```python
+def solve(self, target_tool_pose: np.ndarray) -> list[float]:
+    if self._actual_joints is None:
+        raise RuntimeError(
+            "joint state must be initialized before QP solve"
+        )
+    self._frame_task.T_world_frame = _validated_transform(
+        target_tool_pose
+    )
+    result = np.asarray(
+        self._robot.state.q[RM75_Q_SLICE],
+        dtype=float,
+    ).copy()
+    position_error, orientation_error = self._target_errors()
+    if (
+        position_error <= QP_POSITION_TOLERANCE_M
+        and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+    ):
+        return result.tolist()
+
+    for _ in range(QP_MAX_ITERATIONS):
+        previous = result
+        self._solver.solve(True)
+        self._robot.update_kinematics()
+        result = np.asarray(
+            self._robot.state.q[RM75_Q_SLICE],
+            dtype=float,
+        ).copy()
+        self._validate_result(result, previous)
+        position_error, orientation_error = self._target_errors()
+        if (
+            position_error <= QP_POSITION_TOLERANCE_M
+            and orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+        ):
+            return result.tolist()
+
+    raise RuntimeError(
+        "QP did not converge after "
+        f"{QP_MAX_ITERATIONS} iterations: "
+        f"position_error={position_error:.6f} m, "
+        f"orientation_error={orientation_error:.6f} rad"
+    )
+```
+
+目标已到达时直接返回当前关节角，避免静止时进行不必要的数值迭代。
+
+- [x] **步骤4：让速度校验针对每次数值迭代**
+
+把 `_validate_result()` 签名改为：
+
+```python
+def _validate_result(
+    self,
+    result: np.ndarray,
+    reference: np.ndarray | None = None,
+) -> None:
+```
+
+保留现有有限值和关节位置检查，把速度检查替换为：
+
+```python
+if reference is None:
+    reference = self._actual_joints
+if reference is None:
+    raise RuntimeError("joint state has not been initialized")
+reference = np.asarray(reference, dtype=float)
+if reference.shape != (7,) or not np.isfinite(reference).all():
+    raise ValueError("QP reference must contain 7 finite values")
+max_step = self._velocity_limits * self._dt + 1e-9
+if np.any(np.abs(result - reference) > max_step):
+    raise ValueError(
+        "QP result violates RM75 one-cycle velocity limits"
+    )
+```
+
+这样每次内部数值迭代继续满足Placo的URDF关节速度边界；最终收敛解仍由节点现有
+`_limit_joint_command_step()`按真实90 Hz周期限制后才发送。
+
+- [x] **步骤5：运行目标测试并确认GREEN**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
+PYTHONPATH="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_converges_to_reachable_tcp_target \
+  src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_result_rejects_nan_position_and_velocity_violations \
+  -v
+```
+
+预期：两个测试通过；真实Placo用例不被跳过。
+
+- [x] **步骤6：运行Placo变换测试文件**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
+PYTHONPATH="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py \
+  -v
+```
+
+预期：全部通过，无失败或跳过。
+
+---
+
+## 任务三：回归、安全和mock验证
+
+**验证范围：**
+
+- `xr_rm_teleop`全部测试；
+- ROS2工作空间构建；
+- 统一launch的右臂mock启动；
+- 最终差异与安全配置审计。
+
+- [x] **步骤1：运行遥操作包全部测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_PLACO_TEST_PATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
+PYTHONPATH="${RM75_PLACO_TEST_PATH}:${PYTHONPATH:-}" \
+  python3 -m pytest src/xr_rm_teleop/test -v
+```
+
+预期：全部测试通过，真实Placo收敛用例被执行。
+
+- [x] **步骤2：按项目规则单独运行姿态控制测试**
+
+```bash
+source /opt/ros/humble/setup.bash
+python3 -m pytest \
+  src/xr_rm_teleop/test/test_orientation_control.py \
+  -v
+```
+
+预期：全部通过。
+
+- [x] **步骤3：构建ROS2工作空间**
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`和
+`xr_rm_bringup`全部构建成功。
+
+- [x] **步骤4：通过统一入口进行右臂mock启动验证**
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+if timeout --signal=INT 10s ros2 launch \
+  xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=true udp_port:=15123
+then
+  true
+else
+  launch_status=$?
+  test "$launch_status" -eq 124
+fi
+```
+
+预期：
+
+- `udp_controller_receiver`和`single_arm_velocity_teleop`正常启动；
+- 节点报告 `dt=0.0111s`、`follow=False`；
+- mock关节初始化成功；
+- 不导入RealMan SDK，不建立真机连接，不发送CANFD；
+- 10秒后仅由 `timeout`结束。
+
+- [x] **步骤5：最终差异和安全审计**
+
+在 `/home/robot/WS_xr/src` 执行：
+
+```bash
+git diff --check
+git status --short
+git diff -- \
+  xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
+  xr_rm_teleop/test/test_placo_transforms.py
+rg -n \
+  "control_rate_hz|follow:|configure_safety_limits|move_to_initial_pose_on_connect" \
+  xr_rm_bringup/config/dual_arm_rm75.yaml \
+  xr_rm_bringup/config/left_arm_rm75.yaml \
+  xr_rm_bringup/config/right_arm_rm75.yaml
+```
+
+预期：
+
+- 生产代码只修改Placo求解器；
+- 测试只增加真实模型收敛验证；
+- 三份配置继续使用90 Hz、`follow: false`、
+  `configure_safety_limits: true`和
+  `move_to_initial_pose_on_connect: false`；
+- 不改变此前由用户保留的 `AGENTS.md` 修改；
+- 不自动提交或推送。
+
+---
+
+## 真机交接验收
+
+Codex不执行本节。自动验证全部通过后，由用户在安全工作区使用：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=false
+```
+
+验收步骤：
+
+1. 急停可用、Grip松开、工作区无人后启动。
+2. 按住Grip，快速移动手柄约10 cm后保持不动。
+3. 机械臂应在约1秒内稳定到位，无持续肉眼可见晃动。
+4. 连续观察四个5秒 timing 窗口，`total max`均低于11.111 ms。
+5. 不应出现QP未收敛、反馈超时、CANFD错误或故障锁存日志。
+6. 松开Grip后机械臂按现有逻辑安全停止。
+
+若任一窗口 `total max`达到或超过11.111 ms，或机械臂出现明显振荡，立即松开
+Grip并停止测试，把完整timing和错误日志返回后再调整；不得直接提高控制频率、
+关闭限速或改成高跟随。

docs/superpowers/plans/2026-07-30-rm75-joint-command-braking.md

@@ -0,0 +1,309 @@
+# RM75 关节命令提前制动实施计划
+
+> **供智能体执行者：** 必须使用 `superpowers:subagent-driven-development`
+>（推荐）或 `superpowers:executing-plans` 逐项实施；所有步骤使用复选框跟踪。
+
+**目标：** 修复 90 Hz 关节命令在稳定目标附近反复越界的问题，使 RM75 在保留
+现有速度、加速度限制和低跟随模式的前提下提前制动并稳定停止。
+
+**架构：** 保留当前 QP、反馈和故障恢复链路，只替换
+`SingleArmVelocityTeleop._limit_joint_command_step()` 内部的关节命令生成规则。
+每个关节根据离散制动距离决定继续加速或开始减速，最终命令仍由现有
+`_send_joint_target()` 发送。
+
+**技术栈：** Python 3.10、ROS2 Humble、NumPy、pytest、ament/colcon。
+
+---
+
+## 执行约束
+
+- 设计文档：
+  `docs/superpowers/specs/2026-07-30-rm75-joint-command-braking-design.md`。
+- 构建、测试和启动命令在 `/home/robot/WS_xr` 执行，并先运行
+  `source /opt/ros/humble/setup.bash`。
+- 不连接真机，不发送真实 CANFD，不移动机械臂，不操作夹爪。
+- 启动验证只使用
+  `xr_rm_bringup/launch/arm_debug.launch.py arm:=right use_mock:=true`。
+- 不修改 QP、YAML、RealMan 适配器、launch、UI、依赖和公开 API。
+- 保留工作空间、圆柱、TCP 速度、姿态速度、关节速度、关节加速度、超时保持、
+  CANFD 恢复、Grip 重新使能和安全停止逻辑。
+- 用户未要求 Git 提交，因此本计划不执行 `git commit` 或 `git push`。
+- 保留工作区中已有的其他修改，不回退、不覆盖：
+  `placo_ik_solver.py`、`test_placo_transforms.py` 及现有 Superpowers 文档。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/test/test_joint_control.py`
+  - 增加固定目标提前制动回归测试。
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+  - 在现有关节限幅入口实现离散制动距离判断。
+- 不创建新的运行时代码文件或配置项。
+
+### 任务一：增加持续振荡回归测试
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_joint_control.py:206`
+- 测试：`xr_rm_teleop/test/test_joint_control.py`
+
+- [x] **步骤 1：在现有首周期加速度测试后增加固定目标测试**
+
+增加以下测试：
+
+```python
+def test_joint_command_step_brakes_before_fixed_target_without_overshoot() -> None:
+    dt = 1.0 / 90.0
+    max_speed = math.radians(180.0)
+    max_acceleration = math.radians(300.0)
+    target = np.radians(
+        [10.0, -10.0, 3.0, -3.0, 1.0, -1.0, 0.1]
+    ).tolist()
+    command = [0.0] * 7
+    velocity = [0.0] * 7
+
+    for _ in range(180):
+        previous_velocity = list(velocity)
+        command, velocity = (
+            SingleArmVelocityTeleop._limit_joint_command_step(
+                target=target,
+                previous_target=command,
+                previous_velocity=velocity,
+                max_speed=max_speed,
+                max_acceleration=max_acceleration,
+                dt=dt,
+            )
+        )
+
+        for index in range(7):
+            assert min(0.0, target[index]) - 1e-12 <= command[index]
+            assert command[index] <= max(0.0, target[index]) + 1e-12
+            assert abs(velocity[index]) <= max_speed + 1e-12
+            assert (
+                abs(velocity[index] - previous_velocity[index])
+                <= max_acceleration * dt + 1e-12
+            )
+
+    assert command == pytest.approx(target, abs=1e-12)
+    assert velocity == pytest.approx([0.0] * 7, abs=1e-12)
+```
+
+该测试同时覆盖正负方向、不同目标距离、最大速度、最大加速度、禁止越过固定目标
+和最终停止。
+
+- [x] **步骤 2：运行新增测试并确认失败**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_joint_control.py::test_joint_command_step_brakes_before_fixed_target_without_overshoot \
+  -v
+```
+
+预期：`FAIL`，现有实现会让至少一个关节命令越过固定目标。失败原因必须来自新增
+越界断言，不能是导入或环境错误。
+
+### 任务二：实现离散提前制动
+
+**文件：**
+
+- 修改：
+  `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:1232-1263`
+- 测试：`xr_rm_teleop/test/test_joint_control.py`
+
+- [x] **步骤 1：用离散制动逻辑替换现有限幅计算**
+
+保留方法签名和现有长度、参数校验，将
+`desired_velocity = np.clip(...)` 到返回值的部分替换为：
+
+```python
+        values = np.asarray(
+            [target, previous_target, previous_velocity],
+            dtype=float,
+        )
+        if not np.isfinite(values).all():
+            raise ValueError("joint command contains NaN/Inf")
+
+        velocity_step = max_acceleration * dt
+        arrival_distance = velocity_step * dt
+        limited_target = []
+        limited_velocity = []
+        for desired_target, last_target, last_velocity in zip(
+            target,
+            previous_target,
+            previous_velocity,
+        ):
+            error = desired_target - last_target
+            if (
+                abs(last_velocity) <= 1e-12
+                and abs(error) <= arrival_distance
+            ):
+                velocity = error / dt
+                position = desired_target
+            else:
+                direction = (
+                    math.copysign(1.0, error)
+                    if abs(error) > 1e-12
+                    else 0.0
+                )
+                accelerated_speed = min(
+                    abs(last_velocity) + velocity_step,
+                    max_speed,
+                )
+                braking_steps = max(
+                    0,
+                    math.ceil(accelerated_speed / velocity_step) - 1,
+                )
+                braking_distance = accelerated_speed * dt + dt * (
+                    braking_steps * accelerated_speed
+                    - velocity_step
+                    * braking_steps
+                    * (braking_steps + 1)
+                    / 2.0
+                )
+                desired_velocity = direction * max_speed
+                if (
+                    last_velocity * error > 0.0
+                    and abs(error) <= braking_distance
+                ):
+                    desired_velocity = 0.0
+                velocity = _clamp(
+                    desired_velocity,
+                    last_velocity - velocity_step,
+                    last_velocity + velocity_step,
+                )
+                velocity = _clamp(velocity, -max_speed, max_speed)
+                position = last_target + velocity * dt
+
+            limited_target.append(position)
+            limited_velocity.append(velocity)
+
+        if not np.isfinite(limited_target).all():
+            raise ValueError("joint command contains NaN/Inf")
+        return limited_target, limited_velocity
+```
+
+不要增加 ROS 参数或辅助类。制动距离直接使用当前方法已有的
+`max_acceleration`、`max_speed` 和 `dt`。
+
+- [x] **步骤 2：运行新增测试并确认通过**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_joint_control.py::test_joint_command_step_brakes_before_fixed_target_without_overshoot \
+  -v
+```
+
+预期：`PASS`。
+
+- [x] **步骤 3：运行关节控制测试文件**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -v
+```
+
+预期：全部通过；现有
+`test_joint_command_step_limits_acceleration_from_rest` 继续通过，证明首周期
+加速度行为没有回归。
+
+### 任务三：完整验证
+
+**文件：**
+
+- 不修改文件。
+
+- [x] **步骤 1：运行 `xr_rm_teleop` 全部测试**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+export RM75_TEST_PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages"
+PYTHONPATH="${RM75_TEST_PYTHONPATH}:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test -v
+```
+
+预期：全部测试通过，无失败；真实 Placo 回归测试必须执行，不能因缺少模块而跳过。
+
+- [x] **步骤 2：单独运行姿态控制测试**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH="/home/robot/WS_xr/src/xr_rm_teleop:${PYTHONPATH:-}" \
+  python3 -m pytest \
+  src/xr_rm_teleop/test/test_orientation_control.py -v
+```
+
+预期：全部通过。
+
+- [x] **步骤 3：构建工作空间**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：`xr_rm_input`、`xr_rm_interfaces`、`xr_rm_teleop` 和 `xr_rm_bringup`
+全部构建成功。
+
+- [x] **步骤 4：使用 mock 启动右臂统一 launch**
+
+运行：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+timeout 10s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=true
+```
+
+预期：
+
+- 节点日志显示控制周期约 `dt=0.0111s`；
+- 日志显示 `follow=False`；
+- 不连接厂商 SDK，不发送真实 CANFD；
+- 除 `timeout` 主动结束产生的退出状态外，没有 Python 异常或 ROS 错误。
+
+- [x] **步骤 5：检查最终差异**
+
+运行：
+
+```bash
+git -C /home/robot/WS_xr/src diff --check
+git -C /home/robot/WS_xr/src status --short
+git -C /home/robot/WS_xr/src diff -- \
+  xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  xr_rm_teleop/test/test_joint_control.py
+```
+
+预期：
+
+- `diff --check` 无输出；
+- 本次运行时代码改动只涉及上述两个文件；
+- 原有工作区修改仍保留；
+- 不存在提交或远程推送。
+
+## 用户真机验证边界
+
+自动验证完成后，只提供手动验证步骤，不由 Codex 操作真机：
+
+1. 保持低跟随，从安全姿态和小于 5 mm 的上下位移开始；
+2. 手柄停止后观察机械臂是否立即减振并稳定；
+3. 确认无持续 QP、UDP、CANFD 或故障锁存错误后，再测试 10 mm；
+4. 若不再振荡但仍有不可接受的整臂大幅构型变化，停止扩大位移，转入独立的奇异点
+   处理设计。

docs/superpowers/plans/2026-07-30-rm75-qp-udp-feedback-fix.md

@@ -0,0 +1,352 @@
+# RM75 QP 与 UDP 反馈周期修复实施计划
+
+> **供代理执行：** 必须使用 `superpowers:subagent-driven-development`（推荐）或
+> `superpowers:executing-plans` 子技能，按任务逐项实施。步骤使用复选框
+>（`- [ ]`）跟踪。
+
+**目标：** 将 QP 位置收敛阈值调整为 2 mm，并把项目配置的毫秒周期正确换算为
+睿尔曼 SDK 的 5 ms 周期单位，从根因上降低误触发 UDP 反馈超时的概率。
+
+**架构：** 保持 ROS 参数和 YAML 中 `realtime_push_cycle_ms` 的毫秒语义，仅在
+`RealManAdapter.connect()` 的 SDK 边界执行单位换算。QP 只调整现有位置收敛常量；
+反馈超时状态机保持不变，只在首次超时日志中增加实际反馈年龄。
+
+**技术栈：** Python 3.10、ROS2 Humble、pytest、ament/colcon、睿尔曼 Python
+API2、Placo 0.9.4。
+
+---
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`：QP 位置收敛阈值。
+- 修改 `xr_rm_teleop/test/test_placo_transforms.py`：1.5 mm 近收敛结果测试。
+- 修改 `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`：毫秒到 SDK 周期单位换算。
+- 修改 `xr_rm_teleop/test/test_initial_joint_pose.py`：5 ms、10 ms 换算测试。
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：超时日志增加年龄。
+- 修改 `xr_rm_teleop/test/test_joint_control.py`：超时年龄日志测试。
+- 不修改 YAML、launch、UI、消息定义或依赖。
+
+## 测试环境
+
+所有命令从工作空间根目录 `/home/robot/WS_xr` 执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+export PYTHONPATH=/home/robot/WS_xr/src/xr_rm_teleop:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages:/home/robot/miniconda3/envs/xr/lib/python3.10/site-packages/cmeel.prefix/lib/python3.10/site-packages:${PYTHONPATH}
+```
+
+### 任务 1：接受 2 mm 内的 QP 位置残差
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_placo_transforms.py`
+- 修改：`xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py:14`
+
+- [ ] **步骤 1：编写 1.5 mm 近收敛结果的失败测试**
+
+在 `test_placo_transforms.py` 的 QP 测试附近增加：
+
+```python
+def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._actual_joints = np.zeros(7)
+    solver._robot = SimpleNamespace(
+        state=SimpleNamespace(q=np.zeros(14))
+    )
+    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._target_errors = lambda: (1.5e-3, 0.0)
+
+    result = solver.solve(np.eye(4))
+
+    assert result == pytest.approx([0.0] * 7)
+```
+
+并在文件顶部加入现有标准库类型：
+
+```python
+from types import SimpleNamespace
+```
+
+同时从 `placo_ik_solver` 导入现有收敛常量：
+
+```python
+from xr_rm_teleop.placo_ik_solver import (
+    QP_POSITION_TOLERANCE_M,
+    PlacoIkSolver,
+    _validated_transform,
+)
+```
+
+将真实 Placo 可达目标测试的位置断言改为引用同一收敛常量：
+
+```python
+assert position_error <= QP_POSITION_TOLERANCE_M
+```
+
+再增加超过 2 mm 时仍拒绝结果的边界测试：
+
+```python
+def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._actual_joints = np.zeros(7)
+    solver._robot = SimpleNamespace(
+        state=SimpleNamespace(q=np.zeros(14)),
+        update_kinematics=lambda: None,
+    )
+    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._solver = SimpleNamespace(solve=lambda update: None)
+    solver._validate_result = lambda result, previous: None
+    solver._target_errors = lambda: (2.1e-3, 0.0)
+
+    with pytest.raises(RuntimeError, match="QP did not converge after 30"):
+        solver.solve(np.eye(4))
+```
+
+- [ ] **步骤 2：运行测试并确认当前实现失败**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_placo_transforms.py::test_qp_solve_accepts_position_error_within_two_millimeters -q
+```
+
+预期：失败；当前 1 mm 阈值不会直接接收 1.5 mm 残差，测试对象缺少后续 QP
+求解器。
+
+- [ ] **步骤 3：最小修改 QP 位置阈值**
+
+在 `placo_ik_solver.py` 修改现有常量：
+
+```python
+QP_POSITION_TOLERANCE_M = 2e-3
+```
+
+保留 `QP_MAX_ITERATIONS = 30`、姿态阈值、有限值检查、关节位置和速度限制不变。
+
+- [ ] **步骤 4：运行局部测试并确认通过**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_placo_transforms.py -q
+```
+
+预期：该文件全部测试通过；1.5 mm 残差被接受，2.1 mm 残差仍在 30 次后被拒绝。
+
+- [ ] **步骤 5：提交 QP 修改**
+
+```bash
+git add src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py src/xr_rm_teleop/test/test_placo_transforms.py
+git commit -m "fix: 放宽 RM75 QP 位置收敛阈值"
+```
+
+### 任务 2：修正睿尔曼 UDP 实时上报周期单位
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_initial_joint_pose.py`
+- 修改：`xr_rm_teleop/xr_rm_teleop/realman_adapter.py:190-196`
+
+- [ ] **步骤 1：将现有连接测试改为周期换算参数化测试**
+
+将
+`test_connect_configures_udp_feedback_and_waits_for_first_frame` 改为：
+
+```python
+@pytest.mark.parametrize(
+    ("cycle_ms", "sdk_cycle"),
+    [(5, 1), (10, 2)],
+)
+def test_connect_converts_udp_feedback_cycle_to_sdk_units(
+    monkeypatch,
+    cycle_ms,
+    sdk_cycle,
+) -> None:
+    fake_sdk = _install_fake_sdk(monkeypatch)
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "192.168.192.148",
+        8090,
+        realtime_push_cycle_ms=cycle_ms,
+        configure_safety_limits=False,
+    )
+
+    adapter.connect()
+
+    arm = fake_sdk.RoboticArm.instance
+    assert arm is not None
+    assert arm.config.args == (
+        sdk_cycle,
+        True,
+        8090,
+        0,
+        "192.168.192.148",
+    )
+    assert arm.callback is adapter._realtime_callback
+    assert adapter.get_latest_joint_state() is not None
+    assert not hasattr(adapter, "_feedback_thread")
+```
+
+- [ ] **步骤 2：运行参数化测试并确认当前实现失败**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py::test_connect_converts_udp_feedback_cycle_to_sdk_units -q
+```
+
+预期：两个用例均失败；当前代码把 5 和 10 直接传给 SDK，而预期分别为 1 和 2。
+
+- [ ] **步骤 3：在 SDK 边界执行单位换算**
+
+在 `RealManAdapter.connect()` 创建 `rm_realtime_push_config_t` 时修改第一个参数：
+
+```python
+config = rm_realtime_push_config_t(
+    self._realtime_push_cycle_ms // 5,
+    True,
+    self._realtime_push_port,
+    0,
+    self._realtime_push_host_ip,
+)
+```
+
+构造函数已有“正的 5 ms 整数倍”校验，因此不新增辅助函数或重复校验。启动日志继续
+输出项目参数的真实毫秒值。
+
+- [ ] **步骤 4：运行适配器测试并确认通过**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -q
+```
+
+预期：该文件全部测试通过；mock 测试仍不依赖厂商 SDK。
+
+- [ ] **步骤 5：提交周期换算修改**
+
+```bash
+git add src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py src/xr_rm_teleop/test/test_initial_joint_pose.py
+git commit -m "fix: 修正 RM75 UDP 上报周期单位"
+```
+
+### 任务 3：在 UDP 超时日志中打印反馈年龄
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_joint_control.py`
+- 修改：`xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:1036-1044`
+
+- [ ] **步骤 1：扩展现有短暂超时测试，捕获并断言日志**
+
+在
+`test_short_udp_timeout_repeats_last_limited_target_without_query` 中创建遥操对象后加入：
+
+```python
+warnings = []
+teleop.get_logger = lambda: SimpleNamespace(
+    warn=lambda message: warnings.append(message)
+)
+```
+
+在现有断言末尾加入：
+
+```python
+assert warnings == [
+    "right_rm75 UDP关节反馈超时（age=200.0 ms），保持最后安全目标。"
+]
+```
+
+- [ ] **步骤 2：运行测试并确认当前实现失败**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py::test_short_udp_timeout_repeats_last_limited_target_without_query -q
+```
+
+预期：失败；当前日志中没有 `age=200.0 ms`。
+
+- [ ] **步骤 3：给首次超时警告增加实际反馈年龄**
+
+在 `_handle_stale_joint_feedback()` 中仅修改现有警告：
+
+```python
+self.get_logger().warn(
+    f"{self._arm_name} UDP关节反馈超时"
+    f"（age={age * 1000.0:.1f} ms），保持最后安全目标。"
+)
+```
+
+不改变 `_joint_feedback_ready` 的一次性日志条件、最后安全目标重发、500 ms
+重新同步、Grip 重使能和故障锁存逻辑。
+
+- [ ] **步骤 4：运行关节控制测试并确认通过**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -q
+```
+
+预期：该文件全部测试通过。
+
+- [ ] **步骤 5：提交诊断日志修改**
+
+```bash
+git add src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py src/xr_rm_teleop/test/test_joint_control.py
+git commit -m "fix: 补充 RM75 UDP 超时反馈年龄"
+```
+
+### 任务 4：完整回归与 mock 启动验证
+
+**文件：**
+
+- 不修改生产代码。
+
+- [ ] **步骤 1：运行 `xr_rm_teleop` 全部测试**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test -q
+```
+
+预期：全部通过，无失败或错误。
+
+- [ ] **步骤 2：单独运行姿态控制测试**
+
+```bash
+python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py -q
+```
+
+预期：全部通过。
+
+- [ ] **步骤 3：构建完整工作空间**
+
+```bash
+colcon build --symlink-install
+```
+
+预期：`xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop`、`xr_rm_bringup`
+全部构建成功。
+
+- [ ] **步骤 4：使用右臂 mock 启动统一 launch**
+
+```bash
+timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
+```
+
+预期：节点正常启动，无 Python 异常或厂商 SDK 导入错误；因 `timeout` 主动终止，
+命令退出码可以是 124。禁止将 `use_mock` 改为 `false`。
+
+- [ ] **步骤 5：检查最终差异**
+
+```bash
+git status --short
+git diff HEAD~3 --check
+git diff HEAD~3 --stat
+```
+
+预期：除本实施计划文档外，代码差异仅包含计划列出的 6 个代码/测试文件；无空白
+错误，不包含 YAML、launch、UI、依赖或用户其他改动。
+
+## 真机验收
+
+自动验证完成后，由用户在安全条件下启动真机右臂模式并观察：
+
+1. `feedback_interval mean` 从约 25 ms 降到接近 5 ms；
+2. 正常遥操不再频繁触发 `UDP关节反馈超时`；
+3. 反馈确实超过 120 ms 时，日志显示实际 `age`，且仍保持最后安全目标；
+4. 位置残差小于 2 mm 时不再出现 QP 未收敛警告；
+5. 若仍频繁出现超过 120 ms 的中断，保留当前阈值，依据日志继续排查网络、SDK
+   回调或控制器固件。

docs/superpowers/plans/2026-07-30-xrobotoolkit-controller-inputs.md

@@ -0,0 +1,650 @@
+# XRoboToolkit 手柄输入扩展 Implementation Plan
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** 将 XRoboToolkit 左右手柄摇杆、主键和副键加入现有 `XrController` 链路，同时保持 Grip、Trigger、位姿和遥操作行为不变。
+
+**Architecture:** 直接扩展现有 ROS2 消息，继续使用当前 bridge 的嵌套 UDP `buttons` 结构，由 receiver 将按钮展平到消息字段。新增字段按可选输入解析，旧数据包或非法新增字段回退到安全默认值，不新增话题、依赖或控制分支。
+
+**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3.10、ament/colcon、pytest、XRoboToolkit PC-Service Python binding。
+
+## Global Constraints
+
+- 所有构建、测试和运行命令在 `/home/robot/WS_xr` 执行，并先运行 `source /opt/ros/humble/setup.bash`。
+- `XrController.msg` 的字段和顺序必须与批准的设计完全一致，不增加菜单键、摇杆按键、模拟 Grip/Trigger、SDK 时间戳或序号。
+- 保持现有 `grip`、`trigger`、`pose` 语义及控制行为不变。
+- 不新增依赖、ROS2 话题、节点或 LeRobot 录制实现。
+- 不修改 `xr_rm_teleop` 控制代码、机械臂 YAML、安全限位、超时或停止逻辑。
+- 启动验证只使用 `use_mock:=true`，不得连接真机、移动机械臂或操作夹爪。
+- 新增和修改的 Markdown 文档使用中文。
+- Superpowers 工作流只允许创建本地 Git 提交；禁止 push、合并本地分支、合并 PR 或执行任何远程写操作。
+- 每次提交只暂存当前任务列出的文件，不包含用户的其他工作区改动。
+
+---
+
+### Task 1: 扩展 `XrController` 消息接口
+
+**Files:**
+- Modify: `src/xr_rm_interfaces/msg/XrController.msg`
+
+**Interfaces:**
+- Consumes: 现有 `std_msgs/Header`、`geometry_msgs/Pose` 和 `hand/grip/trigger/pose` 字段。
+- Produces: `XrController.primary: bool`、`secondary: bool`、`axis: float32[2]`，供 Task 2 的 receiver 赋值。
+
+- [ ] **Step 1: 记录旧接口缺少新增字段**
+
+在工作空间根目录执行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+python3 - <<'PY'
+from xr_rm_interfaces.msg import XrController
+
+expected = [
+    "header",
+    "hand",
+    "grip",
+    "trigger",
+    "primary",
+    "secondary",
+    "axis",
+    "pose",
+]
+actual = list(XrController.get_fields_and_field_types())
+assert actual == expected, actual
+PY
+```
+
+Expected: FAIL；旧接口输出
+`['header', 'hand', 'grip', 'trigger', 'pose']`。
+
+- [ ] **Step 2: 用批准的顺序修改消息定义**
+
+将 `src/xr_rm_interfaces/msg/XrController.msg` 完整替换为：
+
+```text
+std_msgs/Header header
+string hand
+
+bool grip
+float32 trigger
+bool primary
+bool secondary
+float32[2] axis
+
+geometry_msgs/Pose pose
+```
+
+- [ ] **Step 3: 构建消息包**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install --packages-select xr_rm_interfaces
+```
+
+Expected: `xr_rm_interfaces` 构建成功，无 rosidl 错误。
+
+- [ ] **Step 4: 验证生成接口字段和顺序**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+python3 - <<'PY'
+from xr_rm_interfaces.msg import XrController
+
+expected = [
+    "header",
+    "hand",
+    "grip",
+    "trigger",
+    "primary",
+    "secondary",
+    "axis",
+    "pose",
+]
+actual = list(XrController.get_fields_and_field_types())
+assert actual == expected, actual
+assert XrController.get_fields_and_field_types()["axis"] == "float[2]"
+PY
+```
+
+Expected: PASS，无输出。
+
+- [ ] **Step 5: 提交消息接口**
+
+```bash
+cd /home/robot/WS_xr/src
+git add xr_rm_interfaces/msg/XrController.msg
+git commit -m "feat: 扩展 XR 手柄消息"
+```
+
+Expected: 只提交 `XrController.msg`；不执行 push 或 merge。
+
+---
+
+### Task 2: 透传摇杆与主副按键
+
+**Files:**
+- Create: `src/xr_rm_input/test/test_controller_fields.py`
+- Modify: `src/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py`
+- Modify: `src/xr_rm_input/xr_rm_input/udp_controller_receiver.py`
+
+**Interfaces:**
+- Consumes: Task 1 生成的 `XrController.primary`、`secondary`、`axis`。
+- Produces:
+  - `_buttons_payload(*, primary: Callable[[], Any], secondary: Callable[[], Any]) -> dict[str, bool]`
+  - `_controller_payload(*, hand: str, pose: Any, axis: Any, buttons: dict[str, bool], grip_pressed: bool, trigger_pressed: bool, pose_valid: bool = True) -> dict[str, Any]`
+  - `UdpControllerReceiver._optional_axis(value: Any) -> list[float]`
+  - `UdpControllerReceiver._optional_buttons(value: Any) -> tuple[bool, bool]`
+
+- [ ] **Step 1: 新增失败测试**
+
+创建 `src/xr_rm_input/test/test_controller_fields.py`：
+
+```python
+import math
+from types import SimpleNamespace
+
+from builtin_interfaces.msg import Time
+from xr_rm_input.udp_controller_receiver import UdpControllerReceiver
+from xr_rm_input.xrobotoolkit_to_udp_bridge import (
+    _buttons_payload,
+    _controller_payload,
+    _stop_controller_payload,
+)
+
+
+def _receiver_without_socket() -> UdpControllerReceiver:
+    receiver = object.__new__(UdpControllerReceiver)
+    receiver._quat_order = "xyzw"
+    receiver.get_clock = lambda: SimpleNamespace(
+        now=lambda: SimpleNamespace(to_msg=lambda: Time())
+    )
+    return receiver
+
+
+def test_bridge_payload_contains_only_selected_controller_inputs() -> None:
+    buttons = _buttons_payload(
+        primary=lambda: True,
+        secondary=lambda: False,
+    )
+    payload = _controller_payload(
+        hand="left",
+        pose=[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0],
+        axis=[2.0, -2.0],
+        buttons=buttons,
+        grip_pressed=True,
+        trigger_pressed=False,
+    )
+
+    assert payload == {
+        "hand": "left",
+        "grip": True,
+        "trigger": 0.0,
+        "pos": [1.0, 2.0, 3.0],
+        "quat": [0.0, 0.0, 0.0, 1.0],
+        "pose_valid": True,
+        "pose_source": "xrobotoolkit",
+        "axis": [1.0, -1.0],
+        "buttons": {
+            "primary": True,
+            "secondary": False,
+        },
+    }
+
+
+def test_stop_payload_uses_neutral_selected_inputs() -> None:
+    payload = _stop_controller_payload("right")
+
+    assert payload["axis"] == [0.0, 0.0]
+    assert payload["buttons"] == {
+        "primary": False,
+        "secondary": False,
+    }
+    assert "grip_value" not in payload
+    assert "trigger_value" not in payload
+
+
+def test_receiver_publishes_selected_controller_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 1.0,
+            "axis": [2.0, -2.0],
+            "buttons": {
+                "primary": True,
+                "secondary": False,
+            },
+            "pos": [1.0, 2.0, 3.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "left",
+    )
+
+    assert msg.primary is True
+    assert msg.secondary is False
+    assert list(msg.axis) == [1.0, -1.0]
+
+
+def test_receiver_defaults_invalid_optional_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 0.4,
+            "axis": [math.nan, 0.0],
+            "buttons": [],
+            "pos": [1.0, 2.0, 3.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "right",
+    )
+
+    assert msg.primary is False
+    assert msg.secondary is False
+    assert list(msg.axis) == [0.0, 0.0]
+    assert msg.grip is True
+    assert abs(msg.trigger - 0.4) < 1e-6
+    assert msg.pose.position.x == 1.0
+    assert msg.pose.position.y == 2.0
+    assert msg.pose.position.z == 3.0
+
+
+def test_receiver_defaults_missing_legacy_optional_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 0.0,
+            "pos": [0.0, 1.0, 0.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "left",
+    )
+
+    assert msg.primary is False
+    assert msg.secondary is False
+    assert list(msg.axis) == [0.0, 0.0]
+    assert msg.grip is True
+```
+
+- [ ] **Step 2: 运行测试并确认失败**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+pytest src/xr_rm_input/test/test_controller_fields.py -v
+```
+
+Expected: FAIL；旧 `_buttons_payload` 仍要求 `grip/menu/axis_click`，且 receiver
+尚无 `_optional_axis` 和 `_optional_buttons`。
+
+- [ ] **Step 3: 精简 bridge payload**
+
+在 `xrobotoolkit_to_udp_bridge.py` 中将 `_controller_payload` 改为：
+
+```python
+def _controller_payload(
+    *,
+    hand: str,
+    pose: Any,
+    axis: Any,
+    buttons: dict[str, bool],
+    grip_pressed: bool,
+    trigger_pressed: bool,
+    pose_valid: bool = True,
+) -> dict[str, Any]:
+    pos, quat = (
+        _pose_to_pos_quat(pose)
+        if pose_valid
+        else (ZERO_POS.copy(), IDENTITY_QUAT.copy())
+    )
+    return {
+        "hand": hand,
+        "grip": pose_valid and grip_pressed,
+        "trigger": 1.0 if pose_valid and trigger_pressed else 0.0,
+        "pos": pos,
+        "quat": quat,
+        "pose_valid": pose_valid,
+        "pose_source": POSE_SOURCE,
+        "axis": _safe_axis(axis),
+        "buttons": buttons,
+    }
+```
+
+将 `_stop_controller_payload` 的输入部分改为：
+
+```python
+        "axis": [0.0, 0.0],
+        "buttons": {
+            "primary": False,
+            "secondary": False,
+        },
+```
+
+并删除 `grip_value`、`trigger_value`、`buttons.grip`、`buttons.menu` 和
+`buttons.axis_click` 输出。
+
+将 `_buttons_payload` 改为：
+
+```python
+def _buttons_payload(
+    *,
+    primary: Callable[[], Any],
+    secondary: Callable[[], Any],
+) -> dict[str, bool]:
+    return {
+        "primary": _safe_bool(primary),
+        "secondary": _safe_bool(secondary),
+    }
+```
+
+修改主循环的左手调用：
+
+```python
+                    "left": _controller_payload(
+                        hand="left",
+                        pose=xrt.get_left_controller_pose(),
+                        axis=xrt.get_left_axis(),
+                        buttons=_buttons_payload(
+                            primary=xrt.get_X_button,
+                            secondary=xrt.get_Y_button,
+                        ),
+                        grip_pressed=left_grip,
+                        trigger_pressed=left_trigger,
+                    ),
+```
+
+修改主循环的右手调用：
+
+```python
+                    "right": _controller_payload(
+                        hand="right",
+                        pose=xrt.get_right_controller_pose(),
+                        axis=xrt.get_right_axis(),
+                        buttons=_buttons_payload(
+                            primary=xrt.get_A_button,
+                            secondary=xrt.get_B_button,
+                        ),
+                        grip_pressed=right_grip,
+                        trigger_pressed=right_trigger,
+                    ),
+```
+
+保留主循环中 `get_left/right_grip()`、`get_left/right_trigger()` 和现有滞回
+开关；仅从 `_controller_payload` 参数及 UDP 输出中删除原始模拟量。
+
+- [ ] **Step 4: 为 receiver 增加容错解析**
+
+在 `udp_controller_receiver.py` 导入区增加：
+
+```python
+import math
+```
+
+在 `_payload_to_msg` 中读取新增可选字段：
+
+```python
+        axis = self._optional_axis(payload.get("axis"))
+        primary, secondary = self._optional_buttons(payload.get("buttons"))
+```
+
+在现有 `msg.grip` 和 `msg.trigger` 赋值后加入：
+
+```python
+        msg.primary = primary
+        msg.secondary = secondary
+        msg.axis = axis
+```
+
+在 `_vector3` 附近增加两个无状态解析方法：
+
+```python
+    @staticmethod
+    def _optional_axis(value: Any) -> list[float]:
+        try:
+            axis = [float(item) for item in value]
+        except (TypeError, ValueError):
+            return [0.0, 0.0]
+        if len(axis) != 2 or not all(math.isfinite(item) for item in axis):
+            return [0.0, 0.0]
+        return [
+            min(max(axis[0], -1.0), 1.0),
+            min(max(axis[1], -1.0), 1.0),
+        ]
+
+    @classmethod
+    def _optional_buttons(cls, value: Any) -> tuple[bool, bool]:
+        if not isinstance(value, Mapping):
+            return False, False
+        return (
+            cls._as_bool(value.get("primary", False)),
+            cls._as_bool(value.get("secondary", False)),
+        )
+```
+
+不要把新增字段加入现有 pose 诊断条件；它们无效时不得改变 `grip`。
+
+- [ ] **Step 5: 运行新增测试**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+pytest src/xr_rm_input/test/test_controller_fields.py -v
+```
+
+Expected: `5 passed`。
+
+- [ ] **Step 6: 运行 Python 语法检查**
+
+```bash
+cd /home/robot/WS_xr
+python3 -m py_compile \
+  src/xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py \
+  src/xr_rm_input/xr_rm_input/udp_controller_receiver.py \
+  src/xr_rm_input/test/test_controller_fields.py
+```
+
+Expected: PASS，无输出。
+
+- [ ] **Step 7: 提交 bridge、receiver 和测试**
+
+```bash
+cd /home/robot/WS_xr/src
+git add \
+  xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py \
+  xr_rm_input/xr_rm_input/udp_controller_receiver.py \
+  xr_rm_input/test/test_controller_fields.py
+git commit -m "feat: 发布 XR 手柄摇杆与按键"
+```
+
+Expected: 只提交列出的三个文件；不执行 push 或 merge。
+
+---
+
+### Task 3: 更新文档并完成工作空间验证
+
+**Files:**
+- Modify: `src/README.md`
+- Modify: `src/AGENTS.md`
+
+**Interfaces:**
+- Consumes: Task 1 的最终 `XrController` 格式和 Task 2 的 UDP JSON。
+- Produces: 当前手柄接口说明，以及对后续 Superpowers 任务生效的本地 Git 边界。
+
+- [ ] **Step 1: 更新 README 的 Git 约束**
+
+在 README 的环境准备之前增加：
+
+```markdown
+## Superpowers Git 约束
+
+使用 Superpowers 执行任务时，只允许按 skill 工作流创建本地 Git 提交。
+禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。skill 如需
+独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
+```
+
+- [ ] **Step 2: 更新 README 的当前 UDP 示例**
+
+先修正 README 顶部的当前范围和项目结构：
+
+- 将“自定义 PICO 4 Ultra UDP Sender Unity 工程”完成项替换为
+  “XRoboToolkit bridge 读取左右手柄 pose、Grip、Trigger、摇杆和主副按键”。
+- 从项目结构树删除当前仓库中不存在的
+  `docs/pico_udp_sender_ubuntu22_setup.md` 和整个 `unity/` 子树。
+- 保留官方 XRoboToolkit APK、PC-Service、`PXREAClientUnity` 和
+  `RobotLinuxDemo` 的运行说明；这些是外部工具，不是仓库内已删除的 Unity 工程。
+
+将“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"
+    }
+  }
+}
+```
+
+字段说明更新为：
+
+```markdown
+- `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`。删除已经不存在的自定义 Unity 工程和安装文档链接，但保留 receiver
+对旧格式字段的兼容说明。
+
+将故障排查中的旧 Unity HUD 提示替换为：
+
+```markdown
+- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed；SDK
+  读取失败时 bridge 会发送 `pose_valid=false` 的停止包。
+```
+
+- [ ] **Step 3: 更新 AGENTS 的 Superpowers Git 规则**
+
+将 AGENTS“Git 与提交”中的 Superpowers 段落改为：
+
+```markdown
+使用 Superpowers 执行任务时，只允许按相关 skill 工作流创建本地 Git 提交；
+禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。相关 skill
+如需独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
+其他情况下，除非用户明确要求，不要自动创建分支。
+```
+
+- [ ] **Step 4: 运行输入包测试**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+pytest src/xr_rm_input/test/test_controller_fields.py -v
+```
+
+Expected: `5 passed`。
+
+- [ ] **Step 5: 运行完整工作空间构建**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+Expected: `xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop` 和
+`xr_rm_bringup` 全部构建成功。
+
+- [ ] **Step 6: 重新 source 后验证接口和遥操作回归**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 interface show xr_rm_interfaces/msg/XrController
+pytest src/xr_rm_teleop/test/test_orientation_control.py -v
+```
+
+Expected: 接口按 `header/hand/grip/trigger/primary/secondary/axis/pose` 顺序
+显示；姿态控制测试全部通过。
+
+- [ ] **Step 7: 使用 mock 验证统一启动入口**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+timeout --signal=INT 8s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=true
+```
+
+Expected: `udp_controller_receiver` 和 `single_arm_velocity_teleop` 正常启动；
+不出现消息类型、Placo 或 traceback 错误。`timeout` 到期退出属于预期。
+
+- [ ] **Step 8: 检查 diff 和格式**
+
+```bash
+cd /home/robot/WS_xr/src
+git diff --check
+git status --short
+```
+
+Expected: `git diff --check` 无输出；只剩 README、AGENTS 的计划内文档改动。
+
+- [ ] **Step 9: 提交文档**
+
+```bash
+cd /home/robot/WS_xr/src
+git add README.md AGENTS.md
+git commit -m "docs: 更新手柄输入与 Superpowers 规则"
+```
+
+Expected: 只提交 README 和 AGENTS；不执行 push 或 merge。

docs/superpowers/plans/2026-07-31-controller-primary-initial-pose.md

@@ -0,0 +1,486 @@
+# 手柄主键回初始位姿实施计划
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** 左手 X 键和右手 A 键分别让对应机械臂安全回到配置的初始关节位姿，并同步三份机械臂配置中的新关节角。
+
+**Architecture:** 继续使用现有左右手柄独立话题和单臂遥操作节点，不增加协调节点。遥操作节点检测自身 `XrController.primary` 的上升沿，先停止当前遥操作，再调用真机或 mock 适配器的同名回位方法并重新同步关节状态。
+
+**Tech Stack:** Ubuntu 22.04、ROS2 Humble、Python 3、rclpy、pytest、ament/colcon、RealMan Python API2（仅真机运行时）。
+
+## 全局约束
+
+- 构建、测试和运行命令在 `/home/robot/WS_xr` 执行，并先运行 `source /opt/ros/humble/setup.bash`。
+- 所有自动验证使用 mock 或假对象，不连接真机、不移动机械臂、不操作夹爪。
+- 保留工作空间与圆柱限位、线速度与角速度限制、指令超时和安全停止逻辑。
+- `configure_safety_limits` 保持启用；`move_to_initial_pose_on_connect` 默认值保持 `false`。
+- mock 模式不得导入或依赖睿尔曼厂商 SDK，不新增 RealMan 连接。
+- 只修改完成本功能所需文件，不新增依赖、节点、话题、服务或配置项。
+- 左臂初始关节角（度）：`[-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]`。
+- 右臂初始关节角（度）：`[-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]`。
+
+---
+
+### Task 1: 复用适配器初始位姿运动
+
+**Files:**
+- Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:42-82`
+- Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:178-182`
+- Modify: `src/xr_rm_teleop/xr_rm_teleop/realman_adapter.py:454-459`
+- Test: `src/xr_rm_teleop/test/test_initial_joint_pose.py:13-35`
+
+**Interfaces:**
+- Consumes: 现有 `initial_joint_pose: list[float]`（度）和 `init_move_speed: int`。
+- Produces: `MockRealManAdapter.move_to_initial_pose() -> None`。
+- Produces: `RealManAdapter.move_to_initial_pose() -> None`。
+
+- [ ] **Step 1: 先写失败测试**
+
+将真机测试改为调用公开方法，并增加 mock 恢复初始关节角的测试：
+
+```python
+def test_initial_pose_uses_joint_move_only() -> None:
+    class FakeArm:
+        def __init__(self) -> None:
+            self.calls = []
+
+        def rm_movej(self, *args):
+            self.calls.append(args)
+            return 0
+
+    joints = [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
+    adapter = RealManAdapter(
+        "127.0.0.1",
+        8080,
+        0,
+        "127.0.0.1",
+        8090,
+        initial_joint_pose=joints,
+    )
+    adapter._arm = FakeArm()
+
+    adapter.move_to_initial_pose()
+
+    assert adapter._arm.calls == [(joints, 20, 0, 0, 1)]
+
+
+def test_mock_initial_pose_restores_configured_joints() -> None:
+    initial_degrees = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
+    adapter = MockRealManAdapter(initial_degrees)
+    adapter.send_joint_target([0.0] * 7, follow=False)
+
+    adapter.move_to_initial_pose()
+
+    assert adapter.read_joint_state().positions == pytest.approx(
+        [math.radians(value) for value in initial_degrees]
+    )
+```
+
+- [ ] **Step 2: 运行测试并确认按预期失败**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
+  -k 'initial_pose_uses_joint_move_only or mock_initial_pose_restores_configured_joints' -v
+```
+
+Expected: FAIL，两个适配器都还没有公开的 `move_to_initial_pose` 方法。
+
+- [ ] **Step 3: 写最小实现**
+
+在 mock 中保存初始弧度值并实现恢复：
+
+```python
+self._initial_joint_positions = [
+    math.radians(value) for value in initial_joint_degrees
+]
+self._joint_positions = list(self._initial_joint_positions)
+```
+
+```python
+def move_to_initial_pose(self) -> None:
+    self._joint_positions = list(self._initial_joint_positions)
+    self.last_joint_target = list(self._joint_positions)
+```
+
+将真机 `_move_to_initial_pose` 改为公开方法，并保留原有阻塞式关节运动：
+
+```python
+def move_to_initial_pose(self) -> None:
+    self._require_arm()
+    if self._initial_joint_pose is None:
+        raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
+
+    ret = self._arm.rm_movej(
+        self._initial_joint_pose,
+        self._init_move_speed,
+        0,
+        0,
+        1,
+    )
+    self._check_return(ret, "rm_movej(initial_joint_pose)")
+```
+
+同时把 `connect()` 中的启动回位调用改为：
+
+```python
+if self._move_to_initial_pose_on_connect:
+    self.move_to_initial_pose()
+```
+
+- [ ] **Step 4: 运行测试并确认通过**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
+  -k 'initial_pose_uses_joint_move_only or mock_initial_pose_restores_configured_joints' -v
+```
+
+Expected: PASS。
+
+- [ ] **Step 5: 创建本地提交**
+
+```bash
+cd /home/robot/WS_xr/src
+git add xr_rm_teleop/xr_rm_teleop/realman_adapter.py \
+  xr_rm_teleop/test/test_initial_joint_pose.py
+git commit -m "feat: 复用适配器初始位姿运动"
+```
+
+### Task 2: 在遥操作节点处理主键上升沿
+
+**Files:**
+- Modify: `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:276-299`
+- Modify: `src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py:514-534`
+- Test: `src/xr_rm_teleop/test/test_joint_control.py:16-114`
+
+**Interfaces:**
+- Consumes: `XrController.primary: bool`。
+- Consumes: Task 1 的 `adapter.move_to_initial_pose() -> None`。
+- Produces: `SingleArmVelocityTeleop._handle_initial_pose_button(msg: XrController) -> None`。
+
+- [ ] **Step 1: 先写主键边沿失败测试**
+
+在 `test_joint_control.py` 增加测试辅助函数和成功路径测试：
+
+```python
+def _primary_button_teleop(*, move_error=None):
+    events = []
+    errors = []
+    snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())
+
+    class Adapter:
+        def move_to_initial_pose(self):
+            events.append("move")
+            if move_error is not None:
+                raise move_error
+
+        def read_joint_state(self):
+            events.append("read")
+            return snapshot
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._adapter = Adapter()
+    teleop._last_primary_pressed = None
+    teleop._grip_rearm_required = False
+    teleop._safe_stop = lambda reset_active: events.append(
+        ("stop", reset_active)
+    )
+    teleop._reset_joint_state = lambda value: events.append(("sync", value))
+    teleop._handle_trigger_gripper = lambda msg: None
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
+    teleop.get_logger = lambda: SimpleNamespace(
+        info=lambda message: None,
+        error=lambda message: errors.append(message),
+    )
+    return teleop, events, errors, snapshot
+
+
+def test_primary_button_rising_edge_moves_once_and_resyncs() -> None:
+    teleop, events, _, snapshot = _primary_button_teleop()
+    released = SimpleNamespace(primary=False)
+    pressed = SimpleNamespace(primary=True)
+
+    teleop._on_controller(released)
+    teleop._on_controller(pressed)
+    teleop._on_controller(pressed)
+    teleop._on_controller(released)
+    teleop._on_controller(pressed)
+
+    expected_once = [
+        ("stop", True),
+        "move",
+        "read",
+        ("sync", snapshot),
+    ]
+    assert events == expected_once * 2
+    assert teleop._grip_rearm_required
+```
+
+- [ ] **Step 2: 运行测试并确认按预期失败**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k primary_button_rising_edge_moves_once_and_resyncs -v
+```
+
+Expected: FAIL，因为 `_on_controller` 尚未处理 `primary`。
+
+- [ ] **Step 3: 写最小成功实现**
+
+在节点状态中增加与现有 trigger 相同的首次采样保护：
+
+```python
+self._last_primary_pressed: bool | None = None
+```
+
+在现有回调中接入主键处理：
+
+```python
+def _on_controller(self, msg: XrController) -> None:
+    self._last_msg = msg
+    self._last_msg_time = self.get_clock().now()
+    self._handle_initial_pose_button(msg)
+    self._handle_trigger_gripper(msg)
+```
+
+增加主键上升沿处理；首次采样只建立状态，避免节点启动时按键已经按住而意外运动：
+
+```python
+def _handle_initial_pose_button(self, msg: XrController) -> None:
+    if self._last_primary_pressed is None:
+        self._last_primary_pressed = msg.primary
+        return
+
+    rising_edge = msg.primary and not self._last_primary_pressed
+    self._last_primary_pressed = msg.primary
+    if not rising_edge:
+        return
+
+    self._grip_rearm_required = True
+    self._safe_stop(reset_active=True)
+    self._adapter.move_to_initial_pose()
+    self._reset_joint_state(self._adapter.read_joint_state())
+    self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
+```
+
+- [ ] **Step 4: 运行测试并确认通过**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k primary_button_rising_edge_moves_once_and_resyncs -v
+```
+
+Expected: PASS。
+
+- [ ] **Step 5: 先写失败路径测试**
+
+```python
+def test_primary_button_move_failure_logs_and_stays_stopped() -> None:
+    failure = RuntimeError("rm_movej failed")
+    teleop, events, errors, _ = _primary_button_teleop(
+        move_error=failure
+    )
+
+    teleop._on_controller(SimpleNamespace(primary=False))
+    teleop._on_controller(SimpleNamespace(primary=True))
+
+    assert events == [("stop", True), "move"]
+    assert teleop._grip_rearm_required
+    assert errors == [
+        "right_rm75 回初始位姿失败：rm_movej failed"
+    ]
+```
+
+- [ ] **Step 6: 运行失败路径测试并确认按预期失败**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py \
+  -k primary_button_move_failure_logs_and_stays_stopped -v
+```
+
+Expected: FAIL，并抛出 `RuntimeError: rm_movej failed`。
+
+- [ ] **Step 7: 增加最小异常处理**
+
+用 `try/except` 包住回位和状态同步，失败时记录错误并保持已经设置的停止与 Grip
+重新使能状态：
+
+```python
+try:
+    self._adapter.move_to_initial_pose()
+    self._reset_joint_state(self._adapter.read_joint_state())
+except Exception as exc:
+    self.get_logger().error(
+        f"{self._arm_name} 回初始位姿失败：{exc}"
+    )
+    return
+
+self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
+```
+
+- [ ] **Step 8: 运行两条主键测试并确认通过**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_joint_control.py -k primary_button -v
+```
+
+Expected: PASS。
+
+- [ ] **Step 9: 创建本地提交**
+
+```bash
+cd /home/robot/WS_xr/src
+git add xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  xr_rm_teleop/test/test_joint_control.py
+git commit -m "feat: 添加手柄主键回初始位姿"
+```
+
+### Task 3: 同步三份初始位姿配置
+
+**Files:**
+- Modify: `src/xr_rm_bringup/config/left_arm_rm75.yaml:57`
+- Modify: `src/xr_rm_bringup/config/right_arm_rm75.yaml:57`
+- Modify: `src/xr_rm_bringup/config/dual_arm_rm75.yaml:64`
+- Modify: `src/xr_rm_bringup/config/dual_arm_rm75.yaml:121`
+
+**Interfaces:**
+- Consumes: 用户确认的左右臂 7 个关节角，单位为度。
+- Produces: 单臂和双臂模式一致的对应臂 `initial_joint_pose`。
+
+- [ ] **Step 1: 只替换四处初始位姿**
+
+```yaml
+# left_arm_rm75.yaml
+initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
+
+# right_arm_rm75.yaml
+initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
+
+# dual_arm_rm75.yaml / left_arm_teleop
+initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
+
+# dual_arm_rm75.yaml / right_arm_teleop
+initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
+```
+
+- [ ] **Step 2: 解析 YAML 并验证四处值**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 - <<'PY'
+from pathlib import Path
+import yaml
+
+config_dir = Path("src/xr_rm_bringup/config")
+left = [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
+right = [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
+
+left_single = yaml.safe_load((config_dir / "left_arm_rm75.yaml").read_text())
+right_single = yaml.safe_load((config_dir / "right_arm_rm75.yaml").read_text())
+dual = yaml.safe_load((config_dir / "dual_arm_rm75.yaml").read_text())
+
+assert left_single["single_arm_velocity_teleop"]["ros__parameters"]["initial_joint_pose"] == left
+assert right_single["single_arm_velocity_teleop"]["ros__parameters"]["initial_joint_pose"] == right
+assert dual["left_arm_teleop"]["ros__parameters"]["initial_joint_pose"] == left
+assert dual["right_arm_teleop"]["ros__parameters"]["initial_joint_pose"] == right
+PY
+```
+
+Expected: exit code 0，无输出。
+
+- [ ] **Step 3: 确认没有改动其他 YAML 参数**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr/src
+git diff --word-diff=plain -- \
+  xr_rm_bringup/config/left_arm_rm75.yaml \
+  xr_rm_bringup/config/right_arm_rm75.yaml \
+  xr_rm_bringup/config/dual_arm_rm75.yaml
+```
+
+Expected: 只有四个 `initial_joint_pose` 列表发生变化。
+
+- [ ] **Step 4: 创建本地提交**
+
+```bash
+cd /home/robot/WS_xr/src
+git add xr_rm_bringup/config/left_arm_rm75.yaml \
+  xr_rm_bringup/config/right_arm_rm75.yaml \
+  xr_rm_bringup/config/dual_arm_rm75.yaml
+git commit -m "config: 更新左右臂初始位姿"
+```
+
+### Task 4: 完整验证
+
+**Files:**
+- Verify: `src/xr_rm_teleop/test/test_initial_joint_pose.py`
+- Verify: `src/xr_rm_teleop/test/test_joint_control.py`
+- Verify: `src/xr_rm_teleop/test/test_orientation_control.py`
+- Verify: 全部四个 ROS2 包
+
+**Interfaces:**
+- Consumes: Tasks 1–3 的本地提交。
+- Produces: mock 测试与 ROS2 构建通过的可验证结果。
+
+- [ ] **Step 1: 运行遥操作相关测试**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py \
+  src/xr_rm_teleop/test/test_joint_control.py \
+  src/xr_rm_teleop/test/test_orientation_control.py
+```
+
+Expected: PASS，无 error 或 warning。
+
+- [ ] **Step 2: 构建工作空间**
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+Expected: `xr_rm_interfaces`、`xr_rm_input`、`xr_rm_teleop` 和 `xr_rm_bringup`
+构建完成，无失败包。
+
+- [ ] **Step 3: 检查最终范围**
+
+```bash
+cd /home/robot/WS_xr/src
+git status --short
+git log -6 --oneline
+```
+
+Expected: 工作区干净；只有设计、计划、适配器、遥操作节点、两份测试和三份 YAML
+配置的相关本地提交，不存在远程写操作。

docs/superpowers/plans/2026-08-03-dual-rm75-ik-model.md

@@ -0,0 +1,870 @@
+# 双 RM75 逆解模型替换实施计划
+
+> **面向执行代理：** 必须逐项执行本计划，并使用 `superpowers:test-driven-development`；可选择 `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`。
+
+**目标：** 让单臂和双臂遥操作统一加载 `dual_rm75`，左右节点分别使用本侧局部 base→TCP 相对任务求解 7 个关节，并同步前方工作空间与真机 TCP 配置。
+
+**架构：** 保留 `left_arm_teleop`、`right_arm_teleop` 两个独立节点和 RealMan 连接。每个节点创建独立 `PlacoIkSolver`，加载同一双臂 URDF，固定浮动基座、mask 另一臂关节，并通过当前侧关节名查询 q/v offset。节点继续在各自局部基坐标系生成目标，现有 PICO 映射与安全链路不变。
+
+**技术栈：** Ubuntu 22.04、ROS2 Humble、Python 3.10、ament_python、Placo 0.9.4、NumPy、pytest、colcon。
+
+---
+
+## 执行约束
+
+- 所有构建、测试和启动命令均在 `/home/robot/WS_xr` 执行，并先运行：
+
+  ```bash
+  source /opt/ros/humble/setup.bash
+  ```
+
+- 真实 Placo 测试使用 `/home/robot/miniconda3/envs/xr/bin/python`，不能把跳过测试当作通过。
+- 启动验收只允许 `use_mock:=true`，不得连接真机、移动机械臂或操作夹爪。
+- 不修改 `configure_safety_limits: true`、`move_to_initial_pose_on_connect: false`、左右节点名或现有限速/超时/安全停止逻辑。
+- 不增加碰撞约束、新依赖、第三个控制节点或公共坐标系控制路径。
+- 每个实现任务只提交列出的文件，不提交无关工作树内容。
+- `setup.py` 和 launch 路径属于配置集成；按已确认的测试设计使用完整构建、安装
+  资源检查和 mock 启动验收，不增加读取源码字符串的脆弱测试。
+
+## 文件结构
+
+**修改：**
+
+- `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`：选择左右运动链、查询 offset、建立相对位姿任务。
+- `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：把当前侧名称传给求解器。
+- `xr_rm_teleop/test/test_placo_transforms.py`：双臂 URDF、左右 offset、局部位姿和真实 Placo 收敛回归。
+- `xr_rm_teleop/test/placo_ik_smoke.py`：左右分支手工性能冒烟脚本。
+- `xr_rm_teleop/test/test_initial_joint_pose.py`：真机外设选择与三份工作空间配置回归。
+- `xr_rm_teleop/setup.py`：安装双臂 URDF 和混合大小写 STL。
+- `xr_rm_bringup/launch/arm_debug.launch.py`：单臂/双臂统一选择双臂 URDF。
+- `xr_rm_bringup/config/dual_arm_rm75.yaml`：左右局部 Y 上界改为 `0.10`。
+- `xr_rm_bringup/config/left_arm_rm75.yaml`：左臂局部 Y 上界改为 `0.10`。
+- `xr_rm_bringup/config/right_arm_rm75.yaml`：右臂局部 Y 上界改为 `0.10`。
+- `xr_rm_bringup/config/peripherals_rm75.yaml`：同步右臂 omnipic 和左臂编号 2 实际工具的 TCP。
+- `README.md`：更新模型、局部坐标与配置说明。
+
+**不创建新的生产模块或依赖。**
+
+### 任务一：用回归测试锁定外设 TCP 与前方工作空间
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_initial_joint_pose.py`
+- 修改：`xr_rm_bringup/config/peripherals_rm75.yaml`
+- 修改：`xr_rm_bringup/config/dual_arm_rm75.yaml`
+- 修改：`xr_rm_bringup/config/left_arm_rm75.yaml`
+- 修改：`xr_rm_bringup/config/right_arm_rm75.yaml`
+
+- [ ] **步骤 1：先写失败的真实配置测试**
+
+在 `test_initial_joint_pose.py` 顶部补充导入：
+
+```python
+from pathlib import Path
+
+import yaml
+
+from xr_rm_teleop.fun_peripheral import (
+    PeripheralConfig,
+    _configure_tool_frame,
+    load_peripheral_config,
+)
+```
+
+删除原来单行的 `PeripheralConfig, _configure_tool_frame` 导入，随后在
+`test_peripheral_config_exposes_selected_tool()` 后加入：
+
+```python
+CONFIG_DIR = Path(__file__).resolve().parents[2] / "xr_rm_bringup" / "config"
+
+
+def test_deployed_peripheral_config_matches_dual_urdf_tcps() -> None:
+    path = CONFIG_DIR / "peripherals_rm75.yaml"
+    left = load_peripheral_config(str(path), "left")
+    right = load_peripheral_config(str(path), "right")
+
+    assert left.scissorgripper == 2
+    assert left.tool_name == "minisci"
+    assert left.tool_pose == pytest.approx(
+        [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
+    )
+    assert right.scissorgripper == 1
+    assert right.tool_name == "omnipic"
+    assert right.tool_pose == pytest.approx(
+        [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
+    )
+
+
+@pytest.mark.parametrize(
+    ("filename", "node_name"),
+    [
+        ("left_arm_rm75.yaml", "single_arm_velocity_teleop"),
+        ("right_arm_rm75.yaml", "single_arm_velocity_teleop"),
+        ("dual_arm_rm75.yaml", "left_arm_teleop"),
+        ("dual_arm_rm75.yaml", "right_arm_teleop"),
+    ],
+)
+def test_deployed_workspaces_keep_only_ten_centimeters_behind(
+    filename: str,
+    node_name: str,
+) -> None:
+    with (CONFIG_DIR / filename).open("r", encoding="utf-8") as stream:
+        parameters = yaml.safe_load(stream)[node_name]["ros__parameters"]
+
+    assert parameters["workspace_min"] == [-0.70, -0.70, 0.10]
+    assert parameters["workspace_max"] == [0.70, 0.10, 0.75]
+```
+
+- [ ] **步骤 2：运行测试并确认按预期失败**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest \
+  src/xr_rm_teleop/test/test_initial_joint_pose.py::test_deployed_peripheral_config_matches_dual_urdf_tcps \
+  src/xr_rm_teleop/test/test_initial_joint_pose.py::test_deployed_workspaces_keep_only_ten_centimeters_behind \
+  -v
+```
+
+预期：FAIL；当前左臂 `minisci.pose.z` 为 `0.19`、右臂 `omnipic.pose.z` 为
+`0.16`，三份配置的 `workspace_max[1]` 为 `0.70`。
+
+- [ ] **步骤 3：做最小配置修改**
+
+在 `peripherals_rm75.yaml` 中只修改：
+
+```yaml
+  omnipic:
+    pose: [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
+  minisci:
+    pose: [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
+```
+
+保持以下内容不变：
+
+```yaml
+  scissor:
+    pose: [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+arms:
+  left:
+    scissorgripper: 2
+  right:
+    scissorgripper: 1
+```
+
+在 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 以及 `dual_arm_rm75.yaml` 的左右
+节点参数中只把：
+
+```yaml
+workspace_max: [0.70, 0.70, 0.75]
+```
+
+改为：
+
+```yaml
+workspace_max: [0.70, 0.10, 0.75]
+```
+
+- [ ] **步骤 4：运行配置测试并确认通过**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
+```
+
+预期：该文件全部通过，左臂索引仍为 `2`。
+
+- [ ] **步骤 5：提交配置与测试**
+
+```bash
+git add \
+  src/xr_rm_teleop/test/test_initial_joint_pose.py \
+  src/xr_rm_bringup/config/peripherals_rm75.yaml \
+  src/xr_rm_bringup/config/dual_arm_rm75.yaml \
+  src/xr_rm_bringup/config/left_arm_rm75.yaml \
+  src/xr_rm_bringup/config/right_arm_rm75.yaml
+git commit -m "config: 同步双臂 TCP 与前方工作空间"
+```
+
+### 任务二：为双臂局部相对逆解建立失败测试
+
+**文件：**
+
+- 修改：`xr_rm_teleop/test/test_placo_transforms.py`
+
+- [ ] **步骤 1：把旧单臂 URDF 结构测试替换为双臂结构测试**
+
+在测试文件导入中加入 `QP_ORIENTATION_TOLERANCE_RAD`，并定义模型路径：
+
+```python
+from xr_rm_teleop.placo_ik_solver import (
+    QP_ORIENTATION_TOLERANCE_RAD,
+    QP_POSITION_TOLERANCE_M,
+    PlacoIkSolver,
+    _validated_transform,
+)
+
+
+DUAL_URDF_PATH = (
+    Path(__file__).resolve().parents[1]
+    / "models"
+    / "dual_rm75"
+    / "Dual_arm.urdf"
+)
+```
+
+用下面测试替换 `test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp()`：
+
+```python
+def test_dual_urdf_has_two_rm75_chains_and_tool_tcps() -> None:
+    root = ElementTree.parse(DUAL_URDF_PATH).getroot()
+    moving_joint_names = [
+        joint.attrib["name"]
+        for joint in root.findall("joint")
+        if joint.attrib["type"] != "fixed"
+    ]
+
+    assert moving_joint_names == [
+        *[f"omnipic_joint_{index}" for index in range(1, 8)],
+        *[f"scissor_joint_{index}" for index in range(1, 8)],
+    ]
+    assert all(
+        mesh.attrib["filename"].startswith("meshes/")
+        for mesh in root.findall(".//mesh")
+    )
+
+    expected_fixed_joints = {
+        "omnipic_base_mount_joint": (
+            "dual_arm_base_link",
+            "omnipic_base_link",
+            None,
+        ),
+        "scissor_base_mount_joint": (
+            "dual_arm_base_link",
+            "scissor_base_link",
+            None,
+        ),
+        "omnipic_OmniPic_tcp_fixed": (
+            "omnipic_gripper_link",
+            "omnipic_OmniPic_tcp",
+            "0 0 0.14",
+        ),
+        "scissor_scissor_tcp_fixed": (
+            "scissor_scissor_link",
+            "scissor_scissor_tcp",
+            "0 0 0",
+        ),
+        "scissor_scissor_fixed_joint": (
+            "scissor_link_7",
+            "scissor_scissor_link",
+            "0 0 0.165",
+        ),
+    }
+    for name, (parent, child, xyz) in expected_fixed_joints.items():
+        joint = root.find(f"joint[@name='{name}']")
+        assert joint is not None
+        assert joint.attrib["type"] == "fixed"
+        assert joint.find("parent").attrib["link"] == parent
+        assert joint.find("child").attrib["link"] == child
+        if xyz is not None:
+            assert joint.find("origin").attrib["xyz"] == xyz
+```
+
+- [ ] **步骤 2：增加左右求解器、offset 与相对位姿测试**
+
+用下面代码替换 `_rm75_placo_solver()` 和旧的单臂收敛测试：
+
+```python
+ARM_CASES = [
+    pytest.param(
+        "left",
+        [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
+        list(range(14, 21)),
+        list(range(13, 20)),
+        "omnipic",
+        id="left",
+    ),
+    pytest.param(
+        "right",
+        [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
+        list(range(7, 14)),
+        list(range(6, 13)),
+        "scissor",
+        id="right",
+    ),
+]
+
+
+def _dual_placo_solver(
+    arm: str,
+    joint_degrees: list[float],
+) -> tuple[PlacoIkSolver, list[float]]:
+    pytest.importorskip("placo")
+    joints = [math.radians(value) for value in joint_degrees]
+    return PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, arm), joints
+
+
+@pytest.mark.parametrize(
+    ("arm", "joint_degrees", "q_offsets", "v_offsets", "inactive_prefix"),
+    ARM_CASES,
+)
+def test_solver_uses_arm_specific_offsets(
+    arm: str,
+    joint_degrees: list[float],
+    q_offsets: list[int],
+    v_offsets: list[int],
+    inactive_prefix: str,
+) -> None:
+    del inactive_prefix
+    solver, _ = _dual_placo_solver(arm, joint_degrees)
+
+    assert solver._q_offsets.tolist() == q_offsets
+    assert solver._v_offsets.tolist() == v_offsets
+
+
+@pytest.mark.parametrize(
+    ("arm", "joint_degrees", "q_offsets", "v_offsets", "inactive_prefix"),
+    ARM_CASES,
+)
+def test_joint_state_pose_is_relative_to_selected_arm_base(
+    arm: str,
+    joint_degrees: list[float],
+    q_offsets: list[int],
+    v_offsets: list[int],
+    inactive_prefix: str,
+) -> None:
+    del q_offsets, v_offsets, inactive_prefix
+    solver, joints = _dual_placo_solver(arm, joint_degrees)
+
+    actual = solver.update_joint_state(joints)
+    expected = (
+        np.linalg.inv(solver._robot.get_T_world_frame(solver._base_frame))
+        @ solver._robot.get_T_world_frame(solver._tcp_frame)
+    )
+
+    assert actual == pytest.approx(expected)
+
+
+@pytest.mark.parametrize(
+    ("arm", "joint_degrees", "q_offsets", "v_offsets", "inactive_prefix"),
+    ARM_CASES,
+)
+def test_qp_solve_converges_without_moving_inactive_arm(
+    arm: str,
+    joint_degrees: list[float],
+    q_offsets: list[int],
+    v_offsets: list[int],
+    inactive_prefix: str,
+) -> None:
+    del q_offsets, v_offsets
+    solver, joints = _dual_placo_solver(arm, joint_degrees)
+    inactive_offsets = [
+        solver._robot.get_joint_offset(f"{inactive_prefix}_joint_{index}")
+        for index in range(1, 8)
+    ]
+    inactive_before = solver._robot.state.q[inactive_offsets].copy()
+    start_pose = solver.update_joint_state(joints)
+    target_pose = start_pose.copy()
+    target_pose[0, 3] += 0.01
+
+    result = solver.solve(target_pose)
+    reached_pose = solver.update_joint_state(result)
+    rotation_delta = target_pose[:3, :3] @ reached_pose[:3, :3].T
+    orientation_error = math.acos(
+        float(
+            np.clip(
+                (np.trace(rotation_delta) - 1.0) * 0.5,
+                -1.0,
+                1.0,
+            )
+        )
+    )
+
+    assert len(result) == 7
+    assert np.isfinite(result).all()
+    assert np.linalg.norm(
+        target_pose[:3, 3] - reached_pose[:3, 3]
+    ) <= QP_POSITION_TOLERANCE_M
+    assert orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+    assert solver._robot.state.q[inactive_offsets] == pytest.approx(
+        inactive_before
+    )
+
+
+def test_solver_rejects_unknown_arm() -> None:
+    pytest.importorskip("placo")
+
+    with pytest.raises(ValueError, match="arm must be left or right"):
+        PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, "middle")
+```
+
+- [ ] **步骤 3：运行新测试并确认按预期失败**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py -v
+```
+
+预期：FAIL；当前 `PlacoIkSolver` 不接受 `arm` 参数，仍要求单臂 q shape 和
+`joint_1～7`。
+
+### 任务三：实现最小双臂分支相对求解器
+
+**文件：**
+
+- 修改：`xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
+- 修改：`xr_rm_teleop/test/test_placo_transforms.py`
+- 修改：`xr_rm_teleop/test/placo_ik_smoke.py`
+
+- [ ] **步骤 1：替换单臂固定常量**
+
+把 `RM75_JOINT_NAMES` 和 `RM75_Q_SLICE` 替换为：
+
+```python
+ARM_CHAINS = {
+    "left": (
+        "scissor_base_link",
+        "scissor_scissor_tcp",
+        "scissor",
+        "omnipic",
+    ),
+    "right": (
+        "omnipic_base_link",
+        "omnipic_OmniPic_tcp",
+        "omnipic",
+        "scissor",
+    ),
+}
+DUAL_RM75_JOINT_NAMES = [
+    *[f"omnipic_joint_{index}" for index in range(1, 8)],
+    *[f"scissor_joint_{index}" for index in range(1, 8)],
+]
+```
+
+- [ ] **步骤 2：按名称选择当前分支并建立相对任务**
+
+将 `PlacoIkSolver.__init__()` 签名改为：
+
+```python
+def __init__(
+    self,
+    urdf_path: str,
+    dt: float,
+    arm: str,
+) -> None:
+```
+
+在 `dt` 校验后先选择固定分支：
+
+```python
+if arm not in ARM_CHAINS:
+    raise ValueError("arm must be left or right")
+self._base_frame, self._tcp_frame, prefix, inactive_prefix = ARM_CHAINS[arm]
+self._joint_names = [f"{prefix}_joint_{index}" for index in range(1, 8)]
+inactive_joint_names = [
+    f"{inactive_prefix}_joint_{index}" for index in range(1, 8)
+]
+```
+
+加载 `RobotWrapper` 后，用下面代码替换单臂 q shape、关节顺序、offset 和限位初始化：
+
+```python
+if self._robot.state.q.shape != (21,):
+    raise RuntimeError(
+        f"expected Placo q shape (21,), got {self._robot.state.q.shape}"
+    )
+if list(self._robot.joint_names()) != DUAL_RM75_JOINT_NAMES:
+    raise RuntimeError(
+        "unexpected dual RM75 joint order: "
+        f"{list(self._robot.joint_names())}"
+    )
+
+self._q_offsets = np.asarray(
+    [self._robot.get_joint_offset(name) for name in self._joint_names],
+    dtype=int,
+)
+self._v_offsets = np.asarray(
+    [self._robot.get_joint_v_offset(name) for name in self._joint_names],
+    dtype=int,
+)
+if len(set(self._q_offsets.tolist())) != 7:
+    raise RuntimeError(f"invalid RM75 q offsets: {self._q_offsets.tolist()}")
+if len(set(self._v_offsets.tolist())) != 7:
+    raise RuntimeError(f"invalid RM75 v offsets: {self._v_offsets.tolist()}")
+
+self._joint_limits = np.asarray(
+    [self._robot.get_joint_limits(name) for name in self._joint_names]
+)
+self._velocity_limits = np.asarray(
+    [self._robot.model.velocityLimit[index] for index in self._v_offsets]
+)
+self._actual_joints: np.ndarray | None = None
+```
+
+用下面代码替换任务创建：
+
+```python
+self._solver = placo.KinematicsSolver(self._robot)
+self._solver.dt = dt
+self._solver.mask_fbase(True)
+for name in inactive_joint_names:
+    self._solver.mask_dof(name)
+self._solver.enable_velocity_limits(True)
+self._frame_task = self._solver.add_relative_frame_task(
+    self._base_frame,
+    self._tcp_frame,
+    np.eye(4),
+)
+self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
+self._solver.add_kinetic_energy_regularization_task(1e-6)
+```
+
+- [ ] **步骤 3：让反馈和结果使用当前侧 offset 与局部位姿**
+
+在 `update_joint_state()` 中用下面逻辑替换固定切片和绝对 TCP 查询：
+
+```python
+self._robot.state.q[self._q_offsets] = values
+self._robot.update_kinematics()
+base_to_tool = (
+    np.linalg.inv(self._robot.get_T_world_frame(self._base_frame))
+    @ self._robot.get_T_world_frame(self._tcp_frame)
+)
+if is_first_feedback:
+    self._frame_task.T_a_b = base_to_tool.copy()
+return base_to_tool.copy()
+```
+
+在 `solve()` 中把任务目标与两处结果读取分别改为：
+
+```python
+self._frame_task.T_a_b = _validated_transform(target_tool_pose)
+result = np.asarray(
+    self._robot.state.q[self._q_offsets],
+    dtype=float,
+).copy()
+```
+
+迭代后的结果读取使用同一段 `self._q_offsets` 代码。`base_configuration`、目标误差、
+结果校验和收敛循环保持不变。
+
+- [ ] **步骤 4：更新无真实 Placo 的小型求解测试桩**
+
+在 `test_qp_solve_accepts_position_error_within_two_millimeters()` 和
+`test_qp_solve_rejects_position_error_above_two_millimeters()` 中设置：
+
+```python
+solver._q_offsets = np.arange(7, 14)
+solver._robot = SimpleNamespace(
+    state=SimpleNamespace(q=np.zeros(21)),
+)
+solver._frame_task = SimpleNamespace(T_a_b=None)
+```
+
+第二个测试继续给 `_robot` 增加原有 `update_kinematics=lambda: None`，其他桩保持
+原样。这样测试仍只覆盖 2 mm 收敛边界，不伪造 Placo 相对任务。
+
+- [ ] **步骤 5：运行真实 Placo 测试并确认转绿**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py -v
+```
+
+预期：全部通过；左右真实 Placo 用例均执行，不能显示 skipped。
+
+- [ ] **步骤 6：更新手工 Placo 冒烟脚本**
+
+把 `placo_ik_smoke.py` 的 `CASES` 更新为当前左右初始角：
+
+```python
+CASES = {
+    "left": [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
+    "right": [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
+}
+
+TOOL_CHAINS = {
+    "left": ("scissor_base_link", "scissor_link_7", 0.165),
+    "right": ("omnipic_base_link", "omnipic_link_7", 0.14),
+}
+```
+
+两处求解器构造都改为：
+
+```python
+PlacoIkSolver(str(urdf_path), 1.0 / 125.0, arm)
+```
+
+把固定 `link_7`/`0.16` 检查替换为：
+
+```python
+base_frame, flange_frame, tcp_length = TOOL_CHAINS[arm]
+world_to_base = drift_solver._robot.get_T_world_frame(base_frame)
+world_to_flange = drift_solver._robot.get_T_world_frame(flange_frame)
+base_to_flange = np.linalg.inv(world_to_base) @ world_to_flange
+flange_to_tcp = np.linalg.inv(base_to_flange) @ stationary_target
+assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, tcp_length])
+assert np.allclose(flange_to_tcp[:3, :3], np.eye(3), atol=1e-5)
+```
+
+- [ ] **步骤 7：运行冒烟脚本**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop \
+  /home/robot/miniconda3/envs/xr/bin/python \
+  src/xr_rm_teleop/test/placo_ik_smoke.py \
+  src/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf
+```
+
+预期：左右各输出一行有限误差与耗时统计；位置误差不超过 `0.005 m`、姿态误差
+不超过 `2°`、静止漂移不超过 `0.05°`。
+
+- [ ] **步骤 8：提交求解器与测试**
+
+```bash
+git add \
+  src/xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
+  src/xr_rm_teleop/test/test_placo_transforms.py \
+  src/xr_rm_teleop/test/placo_ik_smoke.py
+git commit -m "feat: 使用双 RM75 局部相对逆解"
+```
+
+### 任务四：接入节点、安装空间与统一 launch
+
+**文件：**
+
+- 修改：`xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+- 修改：`xr_rm_teleop/setup.py`
+- 修改：`xr_rm_bringup/launch/arm_debug.launch.py`
+
+- [ ] **步骤 1：把节点当前侧传给求解器**
+
+将节点中的求解器构造改为：
+
+```python
+self._ik_solver = PlacoIkSolver(
+    str(self.get_parameter("robot_urdf_path").value),
+    self._dt,
+    peripheral_arm,
+)
+```
+
+复用已经用于外设加载的 `peripheral_arm`，不增加新的 ROS 参数。
+
+- [ ] **步骤 2：安装双臂模型资源**
+
+在 `xr_rm_teleop/setup.py` 的 `data_files` 中增加：
+
+```python
+(
+    f"share/{package_name}/models/dual_rm75",
+    ["models/dual_rm75/Dual_arm.urdf"],
+),
+(
+    f"share/{package_name}/models/dual_rm75/meshes",
+    glob("models/dual_rm75/meshes/*.STL")
+    + glob("models/dual_rm75/meshes/*.stl"),
+),
+```
+
+保留旧模型安装项，避免破坏仓库中其他手工路径；不修改锁文件或依赖。
+
+- [ ] **步骤 3：让所有 launch 模式选择双臂 URDF**
+
+将 `_rm75_urdf()` 改名并替换为：
+
+```python
+def _dual_rm75_urdf() -> PathJoinSubstitution:
+    return PathJoinSubstitution([
+        FindPackageShare("xr_rm_teleop"),
+        "models",
+        "dual_rm75",
+        "Dual_arm.urdf",
+    ])
+```
+
+把单臂节点和两个双臂节点中的：
+
+```python
+"robot_urdf_path": _rm75_urdf(),
+```
+
+全部替换为：
+
+```python
+"robot_urdf_path": _dual_rm75_urdf(),
+```
+
+- [ ] **步骤 4：构建完整工作空间**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：退出码 `0`，四个 ROS2 包构建成功。
+
+- [ ] **步骤 5：验证安装空间包含完整模型**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+test -f install/xr_rm_teleop/share/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf
+find install/xr_rm_teleop/share/xr_rm_teleop/models/dual_rm75/meshes \
+  -maxdepth 1 -type f | sort
+```
+
+预期：`test` 退出码 `0`；列表包含 `base_link.STL`、`OmniPic.stl`、
+`scissor.stl`、`dual_arm_base.stl` 和 7 个 link 网格等现有资源。
+
+- [ ] **步骤 6：运行双臂 mock 启动验收**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=both use_mock:=true
+```
+
+预期：日志显示 `left_rm75`、`right_rm75` 两个 Placo QP 节点启动，无模型路径、
+q shape、关节名、frame 或 traceback 错误。`timeout` 到期的退出码 `124` 属于预期；
+不得改用 `use_mock:=false`。
+
+- [ ] **步骤 7：提交接入修改**
+
+```bash
+git add \
+  src/xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  src/xr_rm_teleop/setup.py \
+  src/xr_rm_bringup/launch/arm_debug.launch.py
+git commit -m "feat: 接入双 RM75 逆解模型"
+```
+
+### 任务五：更新文档并完成全量验证
+
+**文件：**
+
+- 修改：`README.md`
+
+- [ ] **步骤 1：更新项目结构和模型说明**
+
+在 README 的模型树中保留旧模型并增加：
+
+```text
+│   ├── rm75/                   # 旧 RM75 模型资源（launch 不再选用）
+│   ├── rm75_omnipicker/        # 旧单臂 OmniPicker 模型资源
+│   └── dual_rm75/              # 当前左右臂统一使用的双 RM75 URDF 与网格
+```
+
+把“Placo 使用 `rm75_omnipicker` 和统一 `omnipicker_tcp`”段落替换为：
+
+```markdown
+Placo 使用 `xr_rm_teleop/models/dual_rm75/Dual_arm.urdf`。左右控制节点分别创建
+独立求解器：左臂控制 `scissor_base_link` 到 `scissor_scissor_tcp`，右臂控制
+`omnipic_base_link` 到 `omnipic_OmniPic_tcp`，并 mask 另一侧关节。节点目标仍在
+各自局部基坐标系表达，不把现有 PICO 映射改为公共坐标系。
+
+两侧局部 `-Y` 都指向机器人前方，工作空间在局部 `+Y` 后方只保留 `0.10 m`。
+左臂局部 `+X/+Y/+Z` 分别向下/向后/向左外侧；右臂分别向上/向后/向右外侧。
+真机工具坐标使用 URDF TCP：左臂硬件编号保持 `2`，实际选择的 `minisci` 工具
+长度为 `0.165 m`；右臂编号保持 `1`，`omnipic` 工具长度为 `0.14 m`。
+```
+
+不要把“当前没有双臂碰撞检测”的安全提示改成已完成。
+
+- [ ] **步骤 2：运行相关 Python 测试**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_initial_joint_pose.py -v
+pytest src/xr_rm_teleop/test/test_orientation_control.py -v
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py -v
+```
+
+预期：三个测试文件全部通过；真实 Placo 左右用例均执行。
+
+- [ ] **步骤 3：重新构建工作空间**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：退出码 `0`。
+
+- [ ] **步骤 4：重新运行最终 mock 验收**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=both use_mock:=true
+```
+
+预期：两个节点均启动且没有 traceback；退出码 `124` 仅由 `timeout` 产生。
+
+- [ ] **步骤 5：检查最终范围和格式**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr/src
+git diff --check
+git status --short
+git diff --stat
+```
+
+预期：无空白错误；变更仅包含本计划列出的求解器、测试、launch、安装、四份配置、
+README 和 Superpowers 文档。
+
+- [ ] **步骤 6：提交 README**
+
+```bash
+git add README.md
+git commit -m "docs: 更新双 RM75 逆解说明"
+```
+
+## 完成标准
+
+- 单臂和双臂 launch 均只选择安装空间中的 `dual_rm75/Dual_arm.urdf`。
+- 左右节点是独立求解器实例，各自使用正确 base、TCP、q/v offset 和相对位姿任务。
+- 当前侧小幅可达目标收敛，另一侧关节不漂移。
+- 左臂硬件编号保持 `2`，实际工具 TCP 为 `0.165 m`；右臂编号保持 `1`，TCP 为
+  `0.14 m`。
+- 三份控制配置的局部 Y 范围为 `[-0.70, 0.10]`，其他安全参数不变。
+- 相关测试、完整构建和 `arm:=both use_mock:=true` 启动验收取得新鲜证据。
+- 未连接真机，未增加碰撞控制、依赖或无关重构。

docs/superpowers/plans/2026-08-12-rm75-j3-simulation-calibration.md

@@ -0,0 +1,406 @@
+# RM75 双臂 J3 参考角仿真标定实施计划
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** 使用当前双臂 URDF、Placo QP 和 MuJoCo 运动学模型运行可复现的左右臂 J3 参考角粗扫与细扫，并输出评分、稳定区间和推荐角度。
+
+**Architecture:** 新增一个仅供离线实验使用的脚本，负责生成 18 条严格六维 TCP 轨迹、建立三类 QP 试验配置、运行候选角度扫描、计算硬门槛与并列评分，并生成 CSV/JSON/Markdown 结果。生产控制器、QP 求解器和 YAML 均不修改；测试只覆盖轨迹、评分和一个真实 Placo/MuJoCo 冒烟评估。
+
+**Tech Stack:** Python 3.10、NumPy、Placo 0.9.4、MuJoCo 3.10、pytest、ROS2 Humble 工作空间。
+
+---
+
+## 文件结构
+
+- 新增 `xr_rm_teleop/test/j3_reference_calibration.py`：离线轨迹生成、QP/MuJoCo 评估、评分、结果输出和命令行入口。
+- 新增 `xr_rm_teleop/test/test_j3_reference_calibration.py`：轨迹端点、严格姿态插值、百分位评分、平台选择和真实模型冒烟测试。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/summary.csv`：候选角度汇总。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/trajectories.csv`：逐轨迹指标。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/result.json`：机器可读结果。
+- 生成 `docs/superpowers/results/2026-08-12-rm75-j3-calibration/report.md`：左右臂推荐角度、平台区间、基线对比和最差轨迹。
+
+### Task 1：用失败测试固定轨迹与评分行为
+
+**Files:**
+- Create: `xr_rm_teleop/test/test_j3_reference_calibration.py`
+- Create: `xr_rm_teleop/test/j3_reference_calibration.py`
+
+- [ ] **Step 1：写轨迹生成失败测试**
+
+测试使用以下公开接口：
+
+```python
+def build_task_trajectories(
+    initial_world_pose: np.ndarray,
+    control_rate_hz: float = 90.0,
+    max_linear_speed: float = 0.15,
+    max_angular_speed: float = 0.5,
+) -> list[Trajectory]:
+    ...
+```
+
+断言：
+
+```python
+def test_build_task_trajectories_creates_nine_strict_6d_routes() -> None:
+    initial = np.eye(4)
+    initial[:3, 3] = [0.35, 0.20, 0.10]
+
+    trajectories = build_task_trajectories(initial)
+
+    assert len(trajectories) == 9
+    assert {(route.harvest_y, route.harvest_z) for route in trajectories} == {
+        (y, z)
+        for y in (0.30, 0.40, 0.50)
+        for z in (-0.30, -0.20, -0.10)
+    }
+    for route in trajectories:
+        assert np.allclose(route.poses[0], initial)
+        assert np.allclose(route.poses[-1], initial)
+        basket = route.waypoints[5]
+        assert basket[0, 3] == pytest.approx(initial[0, 3])
+        assert basket[1, 3] == pytest.approx(initial[1, 3])
+        assert basket[2, 3] == pytest.approx(initial[2, 3] - 0.40)
+        assert basket[:3, 2] == pytest.approx([0.0, 0.0, -1.0], abs=1e-6)
+```
+
+- [ ] **Step 2：写评分和平台选择失败测试**
+
+公开接口：
+
+```python
+def rank_candidates(rows: list[CandidateMetrics]) -> list[CandidateMetrics]:
+    ...
+
+def choose_stable_platform(
+    ranked: list[CandidateMetrics],
+    scan_step_deg: float,
+) -> tuple[float, tuple[float, float]]:
+    ...
+```
+
+测试构造三个硬门槛相同的候选，断言评分严格等于：
+
+```python
+score = (
+    0.45 * r_sigma
+    + 0.20 * r_q4
+    + 0.20 * r_elbow
+    + 0.10 * r_smooth
+    + 0.05 * r_track
+)
+```
+
+并断言连续候选均达到最高分的 98% 时返回平台中点，而不是孤立端点。
+
+- [ ] **Step 3：运行测试并确认按预期失败**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
+```
+
+Expected: FAIL，原因是 `j3_reference_calibration` 或公开函数尚不存在。
+
+### Task 2：实现最小轨迹与评分模块
+
+**Files:**
+- Create: `xr_rm_teleop/test/j3_reference_calibration.py`
+- Test: `xr_rm_teleop/test/test_j3_reference_calibration.py`
+
+- [ ] **Step 1：实现旋转和 SE(3) 插值**
+
+只使用 NumPy 和标准库：
+
+```python
+def rotation_angle(rotation: np.ndarray) -> float:
+    cosine = np.clip((np.trace(rotation) - 1.0) * 0.5, -1.0, 1.0)
+    return float(math.acos(cosine))
+
+
+def rotation_vector(rotation: np.ndarray) -> np.ndarray:
+    angle = rotation_angle(rotation)
+    if angle <= 1e-12:
+        return np.zeros(3)
+    axis = np.array([
+        rotation[2, 1] - rotation[1, 2],
+        rotation[0, 2] - rotation[2, 0],
+        rotation[1, 0] - rotation[0, 1],
+    ]) / (2.0 * math.sin(angle))
+    return axis * angle
+
+
+def interpolate_pose(start: np.ndarray, end: np.ndarray, count: int) -> list[np.ndarray]:
+    relative = end[:3, :3] @ start[:3, :3].T
+    vector = rotation_vector(relative)
+    return [
+        make_pose(
+            start[:3, 3] + alpha * (end[:3, 3] - start[:3, 3]),
+            so3_exp(alpha * vector) @ start[:3, :3],
+        )
+        for alpha in np.linspace(0.0, 1.0, count + 1)[1:]
+    ]
+```
+
+`rotation_vector` 对接近 180° 的情况使用特征向量兜底，避免工具朝下转换产生除零。
+
+- [ ] **Step 2：实现九条本侧完整轨迹**
+
+定义不可变数据类：
+
+```python
+@dataclass(frozen=True)
+class Trajectory:
+    name: str
+    harvest_y: float
+    harvest_z: float
+    waypoints: tuple[np.ndarray, ...]
+    poses: tuple[np.ndarray, ...]
+```
+
+航点固定为：初始、预接近、采摘、预接近、筐上方、筐内、筐上方、初始。每段点数为：
+
+```python
+duration = max(
+    translation_distance / max_linear_speed,
+    rotation_distance / max_angular_speed,
+)
+steps = max(1, math.ceil(duration * control_rate_hz))
+```
+
+- [ ] **Step 3：实现百分位排名和并列评分**
+
+同值获得同一百分位，单一取值获得 1.0。平滑性和跟踪排名分别定义为：
+
+```python
+r_smooth = 0.5 * rank_low(motion_cost) + 0.5 * rank_low(max_joint_speed)
+r_track = 0.5 * rank_low(max_position_error) + 0.5 * rank_low(max_orientation_error)
+```
+
+硬门槛按 `(N_fail, -N_complete)` 字典序先筛选；只有满足位置误差、姿态误差、J4、
+关节限位、速度和跳变条件的候选进入综合评分。
+
+- [ ] **Step 4：运行测试确认通过**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
+```
+
+Expected: 轨迹与评分测试 PASS。
+
+### Task 3：用失败测试固定真实 Placo/MuJoCo 单轨迹评估
+
+**Files:**
+- Modify: `xr_rm_teleop/test/test_j3_reference_calibration.py`
+- Modify: `xr_rm_teleop/test/j3_reference_calibration.py`
+
+- [ ] **Step 1：写真实模型冒烟失败测试**
+
+接口：
+
+```python
+def evaluate_trajectory(
+    arm: str,
+    trajectory: Trajectory,
+    variant: Variant,
+    urdf_path: Path,
+    initial_joint_degrees: tuple[float, ...],
+) -> TrajectoryMetrics:
+    ...
+```
+
+使用左臂从初始 TCP 沿公共 `+Y` 移动 1 mm 的两点轨迹，断言：
+
+```python
+assert metrics.cycles == 2
+assert metrics.failures == 0
+assert math.isfinite(metrics.min_sigma)
+assert metrics.min_q4_margin_deg > 0.0
+assert metrics.max_position_error_m <= 2e-3
+assert metrics.max_orientation_error_rad <= 5e-3
+```
+
+测试还将求得的七关节状态写入 `DualArmKinematicModel` 并断言按名称读回一致。
+
+- [ ] **Step 2：运行冒烟测试并确认按预期失败**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py::test_evaluate_trajectory_uses_real_placo_and_mujoco -q
+```
+
+Expected: FAIL，原因是评估器尚未实现。
+
+- [ ] **Step 3：实现三类 QP 变体**
+
+```python
+@dataclass(frozen=True)
+class Variant:
+    name: str
+    q3_reference_deg: float | None
+    enable_manipulability: bool
+    q4_min_deg: float | None
+```
+
+- `original`：三个可选项均关闭；
+- `manip_j4`：位置可操作度权重 `1e-4`，J4 硬下限 10°；
+- `q3_<angle>`：在 `manip_j4` 基础上加入 J3 软任务，权重 `1e-5`。
+
+J4 约束使用 Placo 0.9.4 的 `add_joint_space_half_spaces_constraint(A, b)`，构造
+`-q4 <= -q4_min`。J3 使用 `add_joints_task()`；位置可操作度使用
+`add_manipulability_task(tcp_frame, "position", 1.0)`。
+
+- [ ] **Step 4：实现逐周期评估与失败保持**
+
+每个目标点前将上一有效关节状态同步给 Placo。求解失败时：
+
+```python
+failures += 1
+solver.update_joint_state(last_valid_joints)
+current_joints = last_valid_joints.copy()
+```
+
+不把失败后的 Placo 内部迭代状态带到下一周期。成功状态写入 MuJoCo，并记录六维
+雅可比最小奇异值、J4 余量、肘部外展量、TCP 误差、关节速度和运动代价。
+
+- [ ] **Step 5：运行全部标定脚本测试**
+
+Run:
+
+```bash
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py -q
+```
+
+Expected: 全部 PASS。
+
+### Task 4：运行粗扫、细扫并生成结果
+
+**Files:**
+- Modify: `xr_rm_teleop/test/j3_reference_calibration.py`
+- Generate: `docs/superpowers/results/2026-08-12-rm75-j3-calibration/*`
+
+- [ ] **Step 1：实现命令行和结果输出**
+
+命令行：
+
+```bash
+python j3_reference_calibration.py \
+  --urdf <path> \
+  --config <dual_arm_rm75.yaml> \
+  --output-dir <directory> \
+  --phase coarse|fine|all
+```
+
+粗扫结束后对每侧选择最高分候选，在其 ±10°、原扫描边界内以 2° 细扫。CSV 使用
+`csv.DictWriter`，JSON 使用 `json.dump`，Markdown 报告由同一汇总对象生成，不新增依赖。
+
+- [ ] **Step 2：运行完整仿真标定**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python \
+  src/xr_rm_teleop/test/j3_reference_calibration.py \
+  --urdf src/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf \
+  --config src/xr_rm_bringup/config/dual_arm_rm75.yaml \
+  --output-dir src/docs/superpowers/results/2026-08-12-rm75-j3-calibration \
+  --phase all
+```
+
+Expected: 左右臂粗扫和细扫完成；输出两个基线、全部候选、推荐角度和平台区间。
+
+- [ ] **Step 3：检查结果完整性**
+
+Run:
+
+```bash
+/home/robot/miniconda3/envs/xr/bin/python - <<'PY'
+import json
+from pathlib import Path
+
+path = Path('src/docs/superpowers/results/2026-08-12-rm75-j3-calibration/result.json')
+data = json.loads(path.read_text(encoding='utf-8'))
+assert set(data['arms']) == {'left', 'right'}
+for arm in data['arms'].values():
+    assert arm['coarse_candidates']
+    assert arm['fine_candidates']
+    assert arm['recommended_reference_deg'] is not None
+    assert len(arm['stable_interval_deg']) == 2
+print('result integrity: OK')
+PY
+```
+
+Expected: `result integrity: OK`。
+
+### Task 5：工作空间验证与结果复核
+
+**Files:**
+- Verify only.
+
+- [ ] **Step 1：运行新增测试和相关现有测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:src/xr_rm_mujoco \
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_j3_reference_calibration.py \
+  src/xr_rm_teleop/test/test_placo_transforms.py \
+  src/xr_rm_mujoco/test/test_dual_arm_simulator.py -q
+```
+
+Expected: 全部 PASS。
+
+- [ ] **Step 2：按项目规则构建工作空间**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+Expected: 相关 ROS2 包构建成功。
+
+- [ ] **Step 3：运行姿态控制回归测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_orientation_control.py -q
+```
+
+Expected: 全部 PASS。
+
+- [ ] **Step 4：人工复核结果报告**
+
+确认：
+
+- 每侧确有 9 条完整轨迹；
+- `original`、`manip_j4` 和 J3 候选均存在；
+- 推荐角来自硬门槛通过集合；
+- 平台选择符合 98% 规则；
+- 报告明确列出失败轨迹，且没有把失败更多的候选排到前面；
+- 没有修改生产控制器和 YAML。

docs/superpowers/plans/2026-08-13-rm75-qp-robustness.md

@@ -0,0 +1,372 @@
+# RM75 双臂采摘 QP 稳健性优化实施计划
+
+> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
+
+**Goal:** 在当前双臂严格六维遥操作链路中实现 QP 失败参考状态保持、J3 初始姿态软引导、J4 硬下限与软缓冲，以及按六维奇异值动态启用的可操作度任务。
+
+**Architecture:** 保留 Placo 相对六维位姿主任务和下游关节速度/加速度限制。遥操作层将滤波结果作为候选值，只有 QP 求解和关节发送都成功后才提交；QP 求解器复用 Placo 现有 joints、half-space 和 manipulability 任务，不新增求解框架或依赖。
+
+**Tech Stack:** Python 3.10、ROS2 Humble、Placo 0.9.4、NumPy、pytest、ament/colcon。
+
+---
+
+## 文件结构
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：QP 失败状态和笛卡尔参考状态提交。
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`：J3、J4、动态六维可操作度和失败状态恢复。
+- 修改 `xr_rm_teleop/test/test_joint_control.py`：失败不发送、不提交和发送失败保持测试。
+- 修改 `xr_rm_teleop/test/test_placo_transforms.py`：辅助任务参数、激活函数和真实模型测试。
+- 修改 `xr_rm_teleop/test/test_initial_joint_pose.py`：三份 YAML 的 QP 参数一致性测试。
+- 修改 `xr_rm_bringup/config/dual_arm_rm75.yaml`：左右臂独立 QP 参数。
+- 修改 `xr_rm_bringup/config/left_arm_rm75.yaml`：左臂 QP 参数。
+- 修改 `xr_rm_bringup/config/right_arm_rm75.yaml`：右臂 QP 参数。
+
+### Task 1：QP 失败时不提交笛卡尔参考状态
+
+**Files:**
+- Modify: `xr_rm_teleop/test/test_joint_control.py`
+- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- [ ] **Step 1：修改 QP 失败测试并增加候选滤波测试**
+
+把现有失败测试改为要求 `_solve_joint_target()` 返回 `None`，同时增加位置和姿态滤波只计算候选、不直接修改已提交状态的断言：
+
+```python
+def test_qp_failure_returns_none_and_keeps_last_known_good_target() -> None:
+    ...
+    target = teleop._solve_joint_target(np.eye(4))
+    assert target is None
+    assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
+
+
+def test_target_filters_do_not_commit_candidate_state() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._filtered_target = [0.0, 0.0, 0.0]
+    teleop._filtered_orientation_target = np.eye(3)
+    teleop._target_filter_alpha = 0.5
+    teleop._target_filter_alpha_fast = 0.5
+    teleop._target_filter_fast_threshold_m = 1.0
+    teleop._orientation_filter_alpha = 0.5
+
+    position = teleop._filter_target([0.2, 0.0, 0.0])
+    orientation = teleop._filter_orientation_target(
+        _so3_exp(np.asarray([0.0, 0.0, 0.2]))
+    )
+
+    assert position == pytest.approx([0.1, 0.0, 0.0])
+    assert teleop._filtered_target == pytest.approx([0.0, 0.0, 0.0])
+    assert teleop._filtered_orientation_target == pytest.approx(np.eye(3))
+    assert np.linalg.norm(_so3_log(orientation)) == pytest.approx(0.1)
+```
+
+- [ ] **Step 2：运行新测试并确认按预期失败**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_joint_control.py \
+  -k 'qp_failure or target_filters_do_not_commit' -q
+```
+
+Expected: FAIL；当前失败路径仍返回旧关节数组，滤波函数会立即修改成员状态。
+
+- [ ] **Step 3：实现最小失败保持逻辑**
+
+修改 `_filter_target()` 和 `_filter_orientation_target()` 只返回候选值，不直接写成员。
+修改 `_solve_joint_target()` 在异常时返回 `None`，成功时也不提前更新
+`_last_valid_joint_target`。控制周期只在结果非空时发送，并在发送成功后统一提交：
+
+```python
+joint_target = self._solve_joint_target(target_pose)
+sent = (
+    joint_target is not None
+    and self._send_joint_target(joint_target)
+)
+if sent:
+    self._last_valid_joint_target = list(joint_target)
+    self._filtered_target = list(filtered_target)
+    self._filtered_orientation_target = filtered_orientation.copy()
+    self._last_sent_target = sent_target
+    self._last_sent_orientation = sent_orientation.copy()
+    self._last_command_time = now
+    self._stop_sent = False
+```
+
+失败时不调用 `_send_joint_target()`，因此不会把旧关节保持动作伪装成新 QP 成功；已
+存在的指令超时和反馈故障保持逻辑不改变。
+
+- [ ] **Step 4：运行关节控制测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_joint_control.py -q
+```
+
+Expected: PASS。
+
+### Task 2：J3、J4 与动态六维可操作度
+
+**Files:**
+- Modify: `xr_rm_teleop/test/test_placo_transforms.py`
+- Modify: `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`
+
+- [ ] **Step 1：写辅助任务激活和参数失败测试**
+
+增加纯激活函数测试：
+
+```python
+def test_lower_margin_activation_is_clamped_and_linear() -> None:
+    assert _lower_margin_activation(0.05, 0.01, 0.04) == 0.0
+    assert _lower_margin_activation(0.025, 0.01, 0.04) == pytest.approx(0.5)
+    assert _lower_margin_activation(0.005, 0.01, 0.04) == 1.0
+```
+
+增加真实左右臂求解器测试，构造时传入：
+
+```python
+solver = PlacoIkSolver(
+    str(DUAL_URDF_PATH),
+    1.0 / 90.0,
+    arm,
+    j3_reference_deg=j3_reference_deg,
+    j3_weight=1e-5,
+    j4_min_deg=10.0,
+    j4_warn_deg=25.0,
+    j4_weight=1e-4,
+    manipulability_sigma_stop=0.01,
+    manipulability_sigma_warn=0.04,
+    manipulability_weight=1e-4,
+)
+```
+
+断言 J3 任务目标等于该侧参考角、J4 half-space 为 `-q4 <= -10°`，六维雅可比为
+`6x7` 且奇异值有限。
+
+- [ ] **Step 2：运行新测试并确认按预期失败**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py \
+  -k 'lower_margin_activation or auxiliary_qp_tasks' -q
+```
+
+Expected: FAIL；激活函数和构造参数尚不存在。
+
+- [ ] **Step 3：实现 Placo 辅助任务**
+
+新增 `_lower_margin_activation(value, stop, warn)`，并在构造器中验证有限参数及
+`j4_warn > j4_min`、`sigma_warn > sigma_stop > 0`。复用 Placo 原生接口：
+
+```python
+self._j3_task = self._solver.add_joints_task()
+self._j3_task.set_joints({self._joint_names[2]: np.deg2rad(j3_reference_deg)})
+self._j3_task.configure("j3_reference", "soft", j3_weight)
+
+self._j4_task = self._solver.add_joints_task()
+self._j4_task.set_joints({self._joint_names[3]: np.deg2rad(j4_warn_deg)})
+
+matrix = np.zeros((1, self._robot.state.q.size))
+matrix[0, self._q_offsets[3]] = -1.0
+self._j4_constraint = self._solver.add_joint_space_half_spaces_constraint(
+    matrix,
+    np.asarray([-np.deg2rad(j4_min_deg)]),
+)
+self._j4_constraint.configure("j4_lower_bound", "hard")
+
+self._manipulability_task = self._solver.add_manipulability_task(
+    self._tcp_frame,
+    "both",
+    1.0,
+)
+```
+
+每次数值迭代前，从 `frame_jacobian(..., "local_world_aligned")` 的当前臂 `6x7`
+雅可比计算 `sigma_min`。J4 和可操作度任务分别使用线性夹紧激活系数重新配置软权重；
+J3 权重使用节点传入的左右臂独立配置。启用 Placo 原生关节限位，保留现有速度限位
+和结果校验。
+
+- [ ] **Step 4：失败时恢复 Placo 到实际关节反馈**
+
+在 `solve()` 入口保存实际关节状态；任何求解异常或 30 次未收敛时，将活动臂关节
+恢复到 `_actual_joints` 并更新运动学后重新抛出异常。测试制造不收敛，断言内部活动
+关节未停留在失败迭代结果。
+
+- [ ] **Step 5：运行 Placo 测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py -q
+```
+
+Expected: PASS。
+
+### Task 3：同步节点和三份控制配置
+
+**Files:**
+- Modify: `xr_rm_teleop/test/test_initial_joint_pose.py`
+- Modify: `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+- Modify: `xr_rm_bringup/config/dual_arm_rm75.yaml`
+- Modify: `xr_rm_bringup/config/left_arm_rm75.yaml`
+- Modify: `xr_rm_bringup/config/right_arm_rm75.yaml`
+
+- [ ] **Step 1：写三份 YAML 一致性失败测试**
+
+扩展现有 YAML 参数化测试，断言左右臂分别为：
+
+```python
+expected = {
+    "left": {
+        "qp_j3_reference_deg": 67.96,
+        "qp_j3_weight": 1e-5,
+    },
+    "right": {
+        "qp_j3_reference_deg": -89.57,
+        "qp_j3_weight": 1e-4,
+    },
+}
+shared = {
+    "qp_j4_min_deg": 10.0,
+    "qp_j4_warn_deg": 25.0,
+    "qp_j4_weight": 1e-4,
+    "qp_manipulability_sigma_stop": 0.01,
+    "qp_manipulability_sigma_warn": 0.04,
+    "qp_manipulability_weight": 1e-4,
+}
+```
+
+同时断言单臂 YAML 与双臂同侧节点值一致。
+
+- [ ] **Step 2：运行配置测试并确认按预期失败**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_initial_joint_pose.py -q
+```
+
+Expected: FAIL；QP 参数尚未写入 YAML。
+
+- [ ] **Step 3：声明、读取并传入 QP 参数**
+
+节点声明上述八个 `qp_*` 参数，进行有限性和大小关系验证，并作为关键字参数传入
+`PlacoIkSolver`。三份 YAML 同步写入相同共享参数，J3 只按左右臂设置不同参考角；
+不修改 `configure_safety_limits` 和 `move_to_initial_pose_on_connect`。
+
+- [ ] **Step 4：运行配置和遥操作姿态测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_initial_joint_pose.py \
+  src/xr_rm_teleop/test/test_orientation_control.py -q
+```
+
+Expected: PASS。
+
+### Task 4：完整验证和本地提交
+
+**Files:**
+- Verify all modified files.
+
+- [ ] **Step 1：运行遥操作包测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH \
+  /home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test -q
+```
+
+Expected: 全部 PASS，无失败。
+
+- [ ] **Step 2：运行真实 URDF 左右臂 QP 冒烟测试**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+PYTHONPATH=src/xr_rm_teleop:$PYTHONPATH /home/robot/miniconda3/envs/xr/bin/python \
+  src/xr_rm_teleop/test/placo_ik_smoke.py \
+  src/xr_rm_teleop/models/dual_rm75/Dual_arm.urdf
+```
+
+Expected: 左右臂保持位姿漂移和 1 cm 六维 QP 冒烟断言均通过。
+
+- [ ] **Step 3：构建 ROS2 工作空间**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+Expected: 所有工作空间包构建成功。
+
+- [ ] **Step 4：检查差异与安全配置**
+
+Run:
+
+```bash
+cd /home/robot/WS_xr/src
+git diff --check
+git diff --stat
+rg -n "configure_safety_limits: true|move_to_initial_pose_on_connect: false" \
+  xr_rm_bringup/config/{dual_arm_rm75,left_arm_rm75,right_arm_rm75}.yaml
+```
+
+Expected: 无空白错误，三份配置继续保留安全设置。
+
+- [ ] **Step 5：创建本地提交**
+
+规格文档和实施计划必须在同一个本地提交中；实现与测试一并纳入该提交，避免文档和
+代码版本不一致：
+
+```bash
+git add \
+  docs/superpowers/specs/2026-08-13-rm75-qp-robustness-design.md \
+  docs/superpowers/plans/2026-08-13-rm75-qp-robustness.md \
+  xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py \
+  xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py \
+  xr_rm_teleop/test/test_joint_control.py \
+  xr_rm_teleop/test/test_placo_transforms.py \
+  xr_rm_teleop/test/test_initial_joint_pose.py \
+  xr_rm_bringup/config/dual_arm_rm75.yaml \
+  xr_rm_bringup/config/left_arm_rm75.yaml \
+  xr_rm_bringup/config/right_arm_rm75.yaml
+git commit -m "feat: 优化双臂采摘QP稳健性"
+```
+
+禁止 `git push`、合并分支或连接真机。

docs/superpowers/specs/2026-07-29-rm75-feedback-recovery-design.md

@@ -0,0 +1,151 @@
+# RM75 关节反馈与故障恢复设计
+
+## 目标
+
+将当前 RM75 QP 遥操作链路调整为：
+
+- 启动时使用 `rm_get_joint_degree()` 获取实际关节角并初始化 QP。
+- 运行时只把 RealMan UDP `joint_position` 作为连续实际关节反馈。
+- UDP 短暂超时时保持最后一次已限速的安全关节目标，不生成新运动。
+- UDP 持续超时或 CANFD 发送错误时，按明确的同步、停止和人工恢复流程处理。
+- `rm_movej_canfd()` 保持低跟随，控制频率使用 `xr_rm_teleop` 分支的 90 Hz。
+
+不改变现有工作空间/圆柱限位、TCP与关节速度和加速度限制、XR命令超时、安全停止、外设控制及双臂节点名。
+
+## 控制数据源
+
+启动初始化与运行反馈使用不同的数据源：
+
+1. `RealManAdapter` 建立现有唯一厂商连接。
+2. 节点同步调用一次 `rm_get_joint_degree()`。
+3. 查询成功后把7个角度转换为弧度，用于初始化 Placo QP、最后安全目标和关节限速历史。
+4. 查询失败时关闭适配器、打印错误并使节点启动失败，不发送 CANFD。
+5. 正常运行后，QP 的连续实际状态只来自已校验且运动状态正常的 UDP `joint_position`。
+
+同步查询只用于启动、持续反馈超时恢复和 CANFD 错误恢复，不新增连接，不进行常态轮询。
+
+## 状态与转换
+
+### 正常运行
+
+控制定时器以 90 Hz 执行。每个周期读取最新 UDP 关节快照，同步 QP，执行现有目标生成、安全限位、单步 QP、关节速度/加速度限制，然后调用：
+
+```text
+rm_movej_canfd(target_degrees, follow=false, ...)
+```
+
+只有已经通过关节限速并成功发送的目标才能成为“最后安全目标”。
+
+### UDP 短暂超时
+
+UDP 快照年龄超过现有 `command_timeout_sec=0.12` 秒、但未达到 `feedback_resync_timeout_sec=0.5` 秒时：
+
+- 不使用过期反馈同步 QP。
+- 不运行目标生成和 QP。
+- 不更新任何目标、滤波器或限速历史。
+- 若超时前正在遥操作且已有成功发送的安全目标，以 90 Hz 原样重发该目标。
+- 若超时前未在遥操作或没有成功发送的目标，保持停止，不开始 CANFD 输出。
+- 首次进入时打印节流后的警告。
+
+机械臂报警、关节掉使能、关节错误或非有限关节值不是普通超时，仍立即执行安全停止。
+
+### UDP 持续超时
+
+UDP 快照年龄达到 0.5 秒时，每次中断只同步调用一次 `rm_get_joint_degree()`：
+
+- 查询成功：用实际角度重置 QP、最后安全目标和关节限速历史；不生成新运动，继续保持并等待 UDP 恢复。
+- 查询失败：调用 slow-stop，停止 CANFD，进入锁存故障并打印错误。
+
+同一次中断不会反复查询。收到新的有效 UDP 反馈后，查询标志才复位。
+
+### UDP 恢复
+
+UDP 恢复后先持续同步实际关节状态，但不能直接恢复运动：
+
+1. 当前 Grip 必须松开。
+2. 节点清除重新使能要求。
+3. 操作者再次按下 Grip，节点以新的手柄和机械臂实际位姿建立相对控制起点。
+
+### CANFD 错误
+
+`rm_movej_canfd()` 返回错误或抛出异常时：
+
+1. 立即停止后续 CANFD 发送。
+2. 调用 slow-stop。
+3. 打印包含机械臂名称、命令名称和原始错误的终端错误日志。
+4. 调用 `rm_get_joint_degree()` 查询实际关节角。
+5. 查询成功时重置 QP和关节命令历史，但不再发送保持命令；等待有效 UDP 和 Grip 松开后重新按下。
+6. 查询失败时进入锁存故障并打印查询错误。
+
+### 锁存故障
+
+锁存故障只作用于发生错误的机械臂节点：
+
+- 控制定时器不再查询、运行 QP或发送 CANFD。
+- slow-stop 只发送一次。
+- 后续 UDP 恢复或 Grip 操作不能自动解锁。
+- 终端保留明确错误信息，但不在每个周期重复刷屏。
+- 操作者检查后必须重启对应遥操作节点才能恢复。
+
+## 代码边界
+
+### `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`
+
+- 给真实与 mock 适配器增加同步关节角查询能力。
+- 复用现有厂商连接。
+- 校验返回码、数量和有限值，统一返回弧度。
+- 保留 UDP 回调作为运行时快照来源。
+
+### `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`
+
+- 启动时查询并初始化 QP。
+- 增加 `feedback_resync_timeout_sec` 参数，默认 0.5 秒。
+- 校验 `feedback_resync_timeout_sec > command_timeout_sec > 0`。
+- 在现有控制周期内加入保持、一次性重新同步、等待 Grip 重使能和锁存判断。
+- 复用现有 `_safe_stop()`、Grip 重使能和关节限速逻辑，不新增状态机类。
+
+### 配置
+
+以下配置的 `control_rate_hz` 从 125 Hz 改为 90 Hz，并增加相同的 0.5 秒持续超时参数：
+
+- `xr_rm_bringup/config/dual_arm_rm75.yaml`
+- `xr_rm_bringup/config/left_arm_rm75.yaml`
+- `xr_rm_bringup/config/right_arm_rm75.yaml`
+
+三份配置继续使用 `follow: false`，双臂节点名保持 `left_arm_teleop` 和 `right_arm_teleop`。
+
+## 错误日志
+
+以下转换必须写入 ROS2 终端日志：
+
+- 启动关节查询失败：`error`。
+- 首次进入 UDP 短暂超时：`warn`。
+- 持续超时查询开始及成功：`warn`/`info`。
+- 持续超时查询失败并锁存：`error`。
+- CANFD 发送失败：`error`。
+- CANFD 后关节查询失败并锁存：`error`。
+- UDP 恢复并等待 Grip 人工重使能：`info`。
+
+日志包含机械臂名称和失败阶段；周期性路径使用状态转换或节流避免刷屏。
+
+## 测试与验证
+
+使用现有 mock 和单元测试完成，不连接真机：
+
+1. 适配器正确解析 `rm_get_joint_degree()` 成功结果，并拒绝错误码、错误数量和 NaN/Inf。
+2. 启动查询结果初始化 QP 和安全目标；查询失败时节点不能进入控制。
+3. 0.12～0.5 秒反馈超时期间不调用 QP，只重发同一安全目标。
+4. 0.5 秒持续超时只查询一次；成功后等待 UDP 与 Grip，失败后锁存。
+5. CANFD 错误后停止发送并查询；查询成功要求 Grip 重使能，查询失败锁存。
+6. 机械臂报警或掉使能仍立即停止，不能进入保持路径。
+7. 三份配置均使用 90 Hz、0.5 秒持续超时和低跟随。
+
+在工作空间根目录 `/home/robot/WS_xr` 执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_initial_joint_pose.py
+pytest src/xr_rm_teleop/test/test_joint_control.py
+pytest src/xr_rm_teleop/test/test_orientation_control.py
+colcon build --symlink-install
+```

docs/superpowers/specs/2026-07-29-rm75-qp-convergence-design.md

@@ -0,0 +1,192 @@
+# RM75 QP 收敛与低跟随稳定性优化设计
+
+## 背景
+
+右臂真机以90 Hz、`follow: false`运行时，用户观察到：
+
+- 手柄移动约10 cm后，`target_pose`很快稳定；
+- `current_pose`仍需约3秒缓慢追赶；
+- 运动过程中机械臂存在肉眼可见的轻微晃动。
+
+现场 timing 日志同时表明：
+
+- 控制周期约11.111 ms；
+- 控制回调平均约2.6 ms，最大约6.0 ms；
+- QP平均约0.39 ms；
+- CANFD发送平均约0.21 ms；
+- UDP实际关节反馈平均约25 ms一帧，即约40 Hz。
+
+因此，控制线程、QP单次计算和CANFD调用本身没有耗尽90 Hz周期；慢速发生在
+`target_pose`生成之后。
+
+## 根因
+
+当前 `PlacoIkSolver.solve()` 每次只调用一次：
+
+```python
+self._solver.solve(True)
+```
+
+该调用把一次QP增量应用为 `q + Δq`。与此同时，90 Hz控制循环每次都会先用
+最新实际关节反馈重置Placo模型。由于实际反馈约40 Hz，同一帧反馈通常会被重复
+使用两到三次。
+
+结果是每次下发的关节目标只位于实际关节角前方一小步，而不是当前TCP目标对应的
+收敛关节解。低跟随控制器持续追逐这个短距离移动点，表现为：
+
+- 对稳定TCP目标呈缓慢的渐近追赶；
+- 实际反馈每约25 ms更新一次时，关节目标随反馈发生台阶式修正；
+- 低跟随内部平滑与台阶式关节目标叠加，形成轻微晃动。
+
+本地RM75模型对照结果支持该判断：从右臂初始姿态求解7 cm平移目标时，单次QP
+只产生约7.6 mm TCP位移；在同一次逆解中连续迭代30次后，目标误差可降至接近
+零，计算耗时约3.56 ms。
+
+## 目标
+
+保持现有安全基线并实现：
+
+- 手柄移动10 cm后，机械臂约1秒内稳定到位；
+- 运动和到位后无持续肉眼可见晃动；
+- 控制频率保持90 Hz；
+- `rm_movej_canfd()`保持低跟随；
+- 运行时仍以UDP `joint_position`作为实际关节反馈；
+- 保留工作空间、圆柱、TCP速度、姿态速度、关节速度与关节加速度限制；
+- 保留反馈超时、CANFD错误恢复、Grip重新使能和安全停止逻辑。
+
+## 不在本次范围
+
+- 不启用高跟随；
+- 不提高TCP或关节安全上限；
+- 不修改XR手柄滤波和坐标映射；
+- 不修改UDP反馈周期或增加反馈预测器；
+- 不新增线程、RealMan连接、依赖或状态机；
+- 不处理双臂碰撞检测。
+
+## 方案比较
+
+### 方案一：有限次数迭代QP
+
+每个控制周期仍从实际关节角开始，但在一次 `solve()` 调用内部迭代QP，直到TCP
+目标收敛或达到固定迭代上限。得到的完整关节目标继续经过现有关节速度与加速度
+限幅后才发送。
+
+优点：
+
+- 直接修复单步QP只生成近距离移动点的根因；
+- 不需要预测状态，不会在反馈中断时继续外推；
+- 复用现有限速、错误回退和CANFD发送路径；
+- 本地测量表明计算量可放入90 Hz周期。
+
+缺点：
+
+- 单周期QP耗时会高于当前单步求解；
+- 不可达目标需要明确的未收敛处理。
+
+### 方案二：反馈帧之间维护预测关节状态
+
+仅在新UDP反馈到达时校正模型，其余90 Hz周期从上一条关节命令继续积分QP。
+
+优点：
+
+- 每周期仍只求解一次QP；
+- 可避免同一反馈帧反复重置模型。
+
+缺点：
+
+- 引入预测状态、反馈校正和漂移处理；
+- 反馈与预测偏差可能在校正时产生新的关节跳动；
+- 超时与恢复逻辑需要同时管理实际状态和预测状态。
+
+### 方案三：只调整滤波、速度或高跟随参数
+
+`target_pose`已经快速稳定，继续提高 `max_linear_speed` 或减小目标滤波不能解决
+下游渐近追赶。启用高跟随则违反本次低跟随约束。
+
+## 决策
+
+采用方案一。它在不引入预测状态的情况下直接修复根因，改动范围只涉及Placo
+求解器及其测试。
+
+## 控制数据流
+
+正常运行时的数据流调整为：
+
+```text
+UDP实际关节反馈
+→ 更新Placo实际关节状态和current_pose
+→ 现有XR相对位姿、工作空间、圆柱、滤波和TCP限速
+→ 有限次数迭代QP，得到收敛关节目标
+→ 现有关节速度与加速度限幅
+→ rm_movej_canfd(..., follow=false)
+```
+
+反馈短暂超时仍只以90 Hz重发最后一次已通过限速的关节目标，不运行QP。反馈持续
+超时和CANFD错误仍沿用现有同步、停止与故障锁存逻辑。
+
+## QP迭代规则
+
+`PlacoIkSolver.solve()`按以下规则执行：
+
+1. 校验目标变换。
+2. 记录本次内部迭代前的关节状态。
+3. 调用一次 `self._solver.solve(True)`。
+4. 更新Placo运动学。
+5. 校验本次候选关节状态：
+   - 7个有限数值；
+   - 不违反RM75关节位置限制；
+   - 本次数值迭代步长不超过Placo按 `dt=1/90` 应用的URDF关节速度限制。
+6. 使用位置任务与姿态任务的 `error_norm()`检查收敛：
+   - 位置误差不超过1 mm；
+   - 姿态误差不超过0.005 rad。
+7. 未收敛则继续迭代，最多30次。
+
+30次后仍未收敛，或任一迭代产生非法结果时，抛出异常。节点复用现有
+`_solve_joint_target()`错误路径，在终端限频打印QP失败原因，并保持上一组安全
+关节目标。
+
+内部迭代得到的是逆解目标，不会直接绕过发送限速。最终下发仍必须经过
+`_limit_joint_command_step()`，因此每个90 Hz真实命令继续满足现有
+`joint_max_speed` 和 `joint_max_acc`。
+
+## 晃动抑制
+
+本次不再叠加新的低通滤波器。晃动通过两层现有机制抑制：
+
+1. QP先收敛到当前TCP目标对应的关节解，避免关节目标随40 Hz反馈只前进一小步；
+2. 最终关节目标由现有关节速度与加速度限幅器生成连续90 Hz命令。
+
+若真机仍存在晃动，再根据“目标关节角与实际关节角误差”追加诊断；本次不预先
+引入预测器或额外滤波。
+
+## 性能与安全验收
+
+自动验证：
+
+- 7 cm可达TCP平移目标在一次 `solve()` 后位置误差不超过1 mm；
+- 姿态误差满足0.005 rad阈值；
+- 非法结果和未收敛目标继续触发现有安全回退；
+- 关节命令速度与加速度限幅测试继续通过；
+- `xr_rm_teleop`全部pytest通过；
+- `colcon build --symlink-install`通过；
+- `arm_debug.launch.py arm:=right use_mock:=true`正常启动。
+
+真机由用户验证：
+
+- 手柄快速移动10 cm并保持不动，机械臂约1秒内稳定；
+- 无持续肉眼可见晃动；
+- 连续四个5秒 timing 窗口中 `total` 最大值低于11.111 ms；
+- 无QP失败、反馈超时、CANFD错误或故障锁存日志；
+- 松开Grip后仍立即退出遥操作并执行安全停止。
+
+若单周期 `total` 达到或超过11.111 ms，停止真机运动并降低最大QP迭代次数，
+不得通过提高控制频率或关闭安全检查规避计算超时。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`。
+- 修改 `xr_rm_teleop/test/test_placo_transforms.py`。
+- 如现有QP失败测试需要补充未收敛原因断言，只精确修改
+  `xr_rm_teleop/test/test_joint_control.py`。
+
+不修改三份机械臂YAML、RealMan适配器、launch、UI、依赖或公开入口。

docs/superpowers/specs/2026-07-30-rm75-joint-command-braking-design.md

@@ -0,0 +1,99 @@
+# RM75 关节命令提前制动设计
+
+## 背景
+
+右臂真机保持 90 Hz 和 `follow: false`。有界迭代 QP 提高跟随速度后，手柄上下
+移动约 10 mm 时出现整臂剧烈晃动，并且到达目标后仍持续振荡。
+
+现场日志表明控制计算未超时，但存在少量 QP 未收敛警告。离线检查确认：
+
+- 手柄上下移动按现有映射对应机器人 X 方向；
+- 当前初始姿态的关节雅可比条件数约为 116，该方向的逆解对关节运动较敏感；
+- 机器人 X 方向 10 mm 的收敛逆解可能包含最大约 17° 的关节变化；
+- 现有关节命令限幅器只限制速度和加速度，没有根据剩余距离提前制动。
+
+固定 10° 关节目标的离线复现中，现有限幅器运行 3 秒后仍处于约 10.56°、
+-20°/s，证明稳定目标本身也会被反复越过。这与真机“到位后继续晃动”的现象
+一致。
+
+## 目标
+
+- 关节目标稳定后，90 Hz 关节命令提前减速并停止在目标上；
+- 不再因命令限幅器反复越过目标而持续振荡；
+- 保留当前较快的有界迭代 QP；
+- 保留现有关节最大速度和最大加速度限制；
+- 保持 `rm_movej_canfd(..., follow=false)`；
+- 不改变 UDP 反馈、超时保持、CANFD 错误恢复和安全停止行为。
+
+## 不在本次范围
+
+- 不修改 QP 迭代次数、收敛阈值或失败回退；
+- 不修改初始姿态、XR 坐标映射或姿态控制；
+- 不增加奇异点阻尼、预测器、新线程、新依赖或新 ROS 参数；
+- 不修改三份机械臂 YAML、RealMan 适配器、launch 或 UI；
+- 不连接真机，不由 Codex 发送运动命令。
+
+若修复制动后机械臂运动已经平稳，但上下运动仍伴随不可接受的整臂大幅构型变化，
+再单独设计奇异点处理；本次不把两个问题混在同一改动中。
+
+## 方案
+
+只修改 `SingleArmVelocityTeleop._limit_joint_command_step()`。
+
+现有逻辑在目标仍位于运动方向前方时持续加速，只有到达或越过目标后才开始反向
+减速。新逻辑对每个关节使用同一组现有状态：
+
+- 上一次已发送的关节目标；
+- 上一次关节命令速度；
+- 当前 QP 关节目标；
+- 现有关节最大速度、最大加速度和控制周期。
+
+每周期按以下规则生成命令：
+
+1. 计算关节剩余距离和单周期最大速度变化
+   `velocity_step = max_acceleration * dt`。
+2. 根据 90 Hz 离散积分规则，计算“本周期再加速一次、随后以最大允许减速度制动”
+   所需的总距离。
+3. 若关节正在朝目标运动，并且剩余距离已经不大于该制动距离，则本周期开始减速；
+   否则继续朝目标加速，但不超过现有最大速度。
+4. 使用 `velocity_step` 限制本周期速度变化，保持现有加速度上限。
+5. 使用新速度积分得到本周期关节目标。
+6. 当关节已经停下且剩余距离不超过一个最大加速度位移
+   `max_acceleration * dt²` 时，在不违反单周期加速度限制的前提下精确落到目标，
+   避免离散步长形成极小往复振荡。
+
+该逻辑只负责命令轨迹制动，不改变 QP 输出。对于持续移动的目标，若目标突然越过
+当前命令位置，控制器仍优先遵守加速度限制，以最大允许减速度反向；不会为禁止
+瞬时越界而跳变速度。
+
+## 安全行为
+
+- 每周期命令速度绝对值不超过 `joint_max_speed`；
+- 相邻周期间速度变化不超过 `joint_max_acc * dt`；
+- 输出继续校验 NaN 和 Inf；
+- QP 异常仍保持上一组安全目标并在终端限频打印警告；
+- UDP 短暂超时仍以 90 Hz 重发最后一次已限速目标，不运行 QP；
+- UDP 持续超时、CANFD 错误、Grip 重新使能和安全停止逻辑保持不变。
+
+## 测试
+
+先增加失败测试，再修改实现：
+
+1. 对固定 10° 关节目标连续运行限幅器，验证命令不越过目标并最终停止；
+2. 在整个序列中验证速度不超过现有上限；
+3. 验证相邻命令速度变化不超过现有加速度上限；
+4. 保留现有“从静止开始限制首周期加速度”测试；
+5. 运行 `xr_rm_teleop` 全部测试；
+6. 按项目规则运行 `colcon build --symlink-install`；
+7. 使用 `arm_debug.launch.py arm:=right use_mock:=true`验证 90 Hz、低跟随和启动路径。
+
+真机只由用户分阶段验证：先小位移、低风险姿态，确认不再到位后持续振荡，再逐步
+增加位移。若仍有明显整臂构型变化但不再振荡，应停止扩大位移并转入奇异点处理，
+不得通过提高速度、加速度或启用高跟随规避。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`；
+- 修改 `xr_rm_teleop/test/test_joint_control.py`；
+- 新增本中文设计文档；
+- 后续新增一份中文实施计划。

docs/superpowers/specs/2026-07-30-rm75-qp-udp-feedback-fix-design.md

@@ -0,0 +1,176 @@
+# RM75 QP 阈值与 UDP 反馈周期修复设计
+
+## 背景
+
+右臂真机在 90 Hz、`follow: false` 遥操过程中频繁出现两类警告：
+
+```text
+QP did not converge after 30 iterations:
+position_error=0.001245~0.001609 m
+
+UDP关节反馈超时，保持最后安全目标。
+```
+
+现场 timing 日志同时表明：
+
+- 控制回调最大约 5.1 ms，没有耗尽 11.111 ms 周期；
+- UDP 反馈间隔均值约 25 ms，即实际约 40 Hz；
+- UDP 间隔存在 37~71 ms 的明显抖动；
+- UDP 短暂超时后约 44 ms 收到新帧，但现有安全状态机要求先松开 Grip。
+
+因此，控制线程计算量不是这两类警告的原因。
+
+## 根因
+
+### QP 近阈值失败
+
+当前 QP 最多迭代 30 次，并要求：
+
+```text
+位置误差 <= 1 mm
+姿态误差 <= 0.005 rad
+```
+
+现场失败时姿态误差约 0.0007 rad，已经满足要求；位置误差仅比 1 mm 高
+0.245~0.609 mm。30 次迭代中的每一步已经通过关节有限值、关节位置限制和单步
+速度限制校验，但最终结果仍因严格的 1 mm 判定被整体丢弃。
+
+用户已明确确认 2 mm 位置残差可接受；该数值也与现有 1 mm 手柄位置死区处于
+同一量级。将位置收敛阈值改为 2 mm，可以接收现场这类安全的近收敛
+结果，同时继续拒绝此前出现过的 7.5 mm 等明显未收敛结果。
+
+### UDP 周期单位错误
+
+项目参数 `realtime_push_cycle_ms` 的单位是毫秒，三份机械臂配置均填写 `5`。
+当前适配器把这个值原样传给：
+
+```python
+rm_realtime_push_config_t(cycle, ...)
+```
+
+但睿尔曼 SDK 的 `cycle` 单位不是毫秒，而是 5 ms 的倍数。因此：
+
+```text
+当前传入 cycle=5
+实际周期 = 5 × 5 ms = 25 ms
+实际频率 = 40 Hz
+```
+
+这与现场 `feedback_interval mean≈25 ms` 完全一致。期望 5 ms 上报时，SDK
+参数应为 `cycle=1`。
+
+在 120 ms 反馈超时窗口内，25 ms 上报只有约 5 次发送机会；修正为 5 ms 后有
+约 24 次发送机会，能显著提高对偶发丢包和调度抖动的容忍度。若网络或 SDK
+回调整体停顿超过 120 ms，仍应触发现有安全超时。
+
+## 目标
+
+- QP 位置收敛阈值由 1 mm 调整为 2 mm；
+- `realtime_push_cycle_ms: 5` 实际配置成 SDK `cycle=1`，恢复 5 ms 上报；
+- UDP 超时警告打印触发时的实际反馈年龄；
+- 保持控制频率 90 Hz 和 `follow: false`；
+- 保持 UDP 短超时保持、持续超时重同步、Grip 重使能、CANFD 恢复和故障锁存；
+- 保留工作空间、圆柱、TCP、姿态和关节安全限制。
+
+## 不在本次范围
+
+- 不增加 QP 最大迭代次数；
+- 不修改姿态收敛阈值；
+- 不放宽 `command_timeout_sec=0.12`；
+- 不修改 `feedback_resync_timeout_sec=0.5`；
+- 不允许短超时后自动恢复 Grip；
+- 不修改三份机械臂 YAML；
+- 不修改 UDP 目标 IP、端口、线程模式或增加新连接；
+- 不处理网卡、交换机或控制器固件问题；
+- 不修改用户当前未提交的 `right_arm_rm75.yaml` 参数调整。
+
+## 修改方案
+
+### QP 收敛
+
+在 `placo_ik_solver.py` 中只修改：
+
+```python
+QP_POSITION_TOLERANCE_M = 2e-3
+```
+
+30 次迭代、逐步关节安全校验、姿态阈值和未收敛异常格式保持不变。
+
+### UDP 周期换算
+
+保留公开参数 `realtime_push_cycle_ms` 的毫秒语义和“正数且为 5 ms 倍数”的现有
+校验。构造 SDK 配置时执行：
+
+```python
+sdk_cycle = self._realtime_push_cycle_ms // 5
+```
+
+示例：
+
+| 项目参数 | SDK `cycle` | 实际周期 |
+|---:|---:|---:|
+| 5 ms | 1 | 5 ms |
+| 10 ms | 2 | 10 ms |
+| 25 ms | 5 | 25 ms |
+
+启动日志仍打印毫秒值，避免把 SDK 内部单位暴露为用户配置。
+
+### UDP 超时日志
+
+首次进入短暂超时时打印：
+
+```text
+right_rm75 UDP关节反馈超时（age=xxx.x ms），保持最后安全目标。
+```
+
+日志只增加诊断值，不改变节流、保持目标、QP 停止和 Grip 重使能行为。
+
+## 数据流与安全
+
+正常路径：
+
+```text
+YAML 5 ms
+→ 适配器换算 SDK cycle=1
+→ 控制器约每 5 ms UDP 上报
+→ 回调校验并缓存 joint_position
+→ 90 Hz 控制读取最新实际反馈
+→ 最多 30 次 QP，位置阈值 2 mm
+→ 现有关节提前制动限幅
+→ rm_movej_canfd(..., follow=false)
+```
+
+反馈年龄超过 120 ms 时仍停止生成新目标和 QP，以 90 Hz 重发最后安全目标，并
+要求 Grip 松开后重新使能。达到 500 ms 时仍只尝试一次
+`rm_get_joint_degree()`；失败时仍停止并锁存故障。
+
+## 测试与验证
+
+自动测试：
+
+1. 增加 QP 1.5 mm 位置残差的行为测试，验证其被 2 mm 阈值接受；
+2. 保留明显未收敛结果抛出异常的行为；
+3. 修改 SDK 配置测试，验证项目 5 ms 参数传入 SDK 时为 `cycle=1`；
+4. 增加 10 ms 到 `cycle=2` 的换算覆盖；
+5. 验证 UDP 超时日志包含实际反馈年龄；
+6. 运行 `xr_rm_teleop` 全部测试和姿态控制测试；
+7. 运行 `colcon build --symlink-install`；
+8. 使用 `arm_debug.launch.py arm:=right use_mock:=true`验证启动路径。
+
+真机由用户验证：
+
+- 连续 timing 窗口中 `feedback_interval mean` 从约 25 ms 降到接近 5 ms；
+- 正常遥操不再频繁出现 UDP 超时；
+- 位置残差小于 2 mm 时不再出现 QP 未收敛警告；
+- 真正超过 120 ms 的反馈中断仍打印带 `age` 的警告并执行现有安全保持；
+- 若修正后仍频繁出现超过 120 ms 的中断，再依据 `age` 和间隔数据排查网络、
+  SDK 回调或控制器固件，不继续盲目放宽超时。
+
+## 文件范围
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py`；
+- 修改 `xr_rm_teleop/xr_rm_teleop/realman_adapter.py`；
+- 修改 `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`；
+- 修改相关现有测试；
+- 新增本中文设计文档和后续中文实施计划；
+- 不修改 YAML、launch、UI 或依赖。

docs/superpowers/specs/2026-07-30-xrobotoolkit-controller-inputs-design.md

@@ -0,0 +1,140 @@
+# XRoboToolkit 手柄输入扩展设计
+
+## 背景
+
+当前 `xrobotoolkit_to_udp_bridge` 已从 XRoboToolkit PC-Service SDK 读取左右
+手柄摇杆、主键和副键，但 `udp_controller_receiver` 只把 `grip`、`trigger`
+和位姿写入 `XrController`，其余信息在 UDP 到 ROS2 的转换中丢失。
+
+后续项目会使用 LeRobot 同时记录相机、RM75 状态和手柄输入。本次只补齐当前
+明确需要的手柄字段，不实现 LeRobot 录制，不改变现有机械臂控制逻辑。
+
+## 目标
+
+- 将左右手柄摇杆、主键和副键发布到现有 `XrController` 话题。
+- 保持现有 `grip`、`trigger` 和 `pose` 的语义及控制行为不变。
+- 兼容不包含新增字段的旧 UDP 数据包。
+- 使用现有节点、消息和 UDP 协议，不增加依赖或新话题。
+- 更新 README 和 AGENTS，记录接口及 Superpowers 的 Git 操作边界。
+
+## 不在本次范围
+
+- Grip 和 Trigger 原始模拟量。
+- 菜单键、摇杆按键、SDK 时间戳和 bridge 序号。
+- 头显位姿、26 点手部骨骼、身体追踪和 Motion Tracker。
+- LeRobot 数据集录制、相机同步和 RM75 状态采集。
+- 任何机械臂控制参数、安全逻辑或真机行为修改。
+
+## 方案选择
+
+采用直接扩展 `XrController` 的方案。相比新增 `sensor_msgs/Joy` 话题，该方案
+不需要额外同步左右手柄话题；相比继续只保留 UDP JSON，它能让 ROS2 和后续
+LeRobot 适配层直接读取类型明确的数据。
+
+修改消息定义后必须重新构建并重启相关节点。重新构建后的现有遥操作代码仍只读取
+原字段，不需要修改控制逻辑。
+
+## ROS2 消息格式
+
+`XrController.msg` 使用以下固定顺序：
+
+```text
+std_msgs/Header header
+string hand
+
+bool grip
+float32 trigger
+bool primary
+bool secondary
+float32[2] axis
+
+geometry_msgs/Pose pose
+```
+
+字段语义：
+
+- `primary`：左手 X 键，右手 A 键。
+- `secondary`：左手 Y 键，右手 B 键。
+- `axis`：对应手柄摇杆的 `[x, y]`，每个分量限制在 `[-1.0, 1.0]`。
+
+## 数据流
+
+正常链路保持不变：
+
+```text
+XRoboToolkit PC-Service SDK
+→ xrobotoolkit_to_udp_bridge
+→ UDP JSON
+→ udp_controller_receiver
+→ /xr/left_controller、/xr/right_controller
+→ single_arm_velocity_teleop
+```
+
+bridge 继续读取 Grip 和 Trigger 模拟量并应用现有滞回，只是不再把未使用的
+`grip_value`、`trigger_value`、`menu` 和 `axis_click` 放入 UDP JSON。
+
+UDP 中的按钮继续使用现有嵌套结构：
+
+```json
+{
+  "grip": true,
+  "trigger": 0.0,
+  "axis": [0.2, -0.4],
+  "buttons": {
+    "primary": true,
+    "secondary": false
+  },
+  "pos": [0.0, 1.0, 0.0],
+  "quat": [0.0, 0.0, 0.0, 1.0]
+}
+```
+
+`udp_controller_receiver` 将嵌套按钮展平到 ROS2 消息字段。现有遥操作节点忽略
+新增字段，因此目标位姿、夹爪触发和安全停止路径均不变化。
+
+## 兼容与异常处理
+
+- 旧 UDP 包缺少 `axis` 或 `buttons` 时，发布
+  `axis=[0.0, 0.0]`、`primary=false`、`secondary=false`。
+- 新增可选字段格式错误时使用上述默认值，不丢弃有效的 Grip、Trigger 和位姿。
+- bridge 和 receiver 均将摇杆分量限制在 `[-1.0, 1.0]`。
+- 旧包中存在 `menu`、`axis_click` 或其他按钮字段时忽略，不报错。
+- `sample_udp_sender` 保持旧格式，用它验证向后兼容，不为本次需求增加新参数。
+
+## 文件范围
+
+- `xr_rm_interfaces/msg/XrController.msg`
+- `xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py`
+- `xr_rm_input/xr_rm_input/udp_controller_receiver.py`
+- `xr_rm_input/test/` 下的一份最小兼容性测试
+- `README.md`
+- `AGENTS.md`
+
+不修改 `xr_rm_teleop` 控制实现及三个机械臂 YAML。
+
+## README 与 AGENTS 规则
+
+README 增加新的手柄字段、UDP 格式和兼容行为说明。
+
+AGENTS 和 README 同时明确：使用 Superpowers 执行任务时，只允许按 skill
+工作流创建本地 Git 提交；不得推送、合并或执行其他远程写操作。skill 如需本地
+worktree 或配套分支，可以创建，但不得将其合并到其他分支。
+
+## 验证
+
+自动验证包括：
+
+- bridge 生成的 UDP payload 只包含确认保留的按钮和摇杆字段。
+- 左手 X/Y 与右手 A/B 正确映射到 `primary/secondary`。
+- receiver 正确发布新增字段。
+- 旧 UDP 包继续发布，新增字段使用默认值。
+- 非法新增字段不会阻断现有 Grip、Trigger 和位姿。
+- 在 `/home/robot/WS_xr` source ROS2 Humble 后运行相关 pytest。
+- 运行 `colcon build --symlink-install`。
+
+运行验证只使用 mock，不连接真机、不移动机械臂、不操作夹爪。
+
+## Git 边界
+
+本设计和后续实现可以按 Superpowers 流程创建本地提交。禁止执行 `git push`、
+创建或合并 PR、合并本地分支以及任何远程写操作。

docs/superpowers/specs/2026-07-31-controller-primary-initial-pose-design.md

@@ -0,0 +1,73 @@
+# 手柄主键回初始位姿设计
+
+## 背景与目标
+
+有线连接已基本解决 UDP 超时和逆解失败问题。本次只增加一个明确操作：
+点击当前机械臂对应手柄的主键，使该机械臂按已配置的关节角回到初始位姿。
+
+- 右臂模式：右手 A 键控制右臂，左手 X 键无效。
+- 左臂模式：左手 X 键控制左臂，右手 A 键无效。
+- 双臂模式：右手 A 键控制右臂，左手 X 键控制左臂。
+
+## 最小方案
+
+`XrController.primary` 已表示左手 X 键或右手 A 键。每个遥操作节点继续只订阅
+自身的手柄话题，并在 `primary` 上升沿调用适配器的初始位姿运动：
+
+```text
+左手 X → /xr/left_controller  → left_arm_teleop  → 左臂初始位姿
+右手 A → /xr/right_controller → right_arm_teleop → 右臂初始位姿
+```
+
+单臂模式只启动对应节点，因此另一只手柄天然无效；双臂模式下两个节点独立处理，
+无需新增协调节点、话题、服务或配置项。
+
+## 运动与安全行为
+
+- 只在按键从未按下变为按下时触发，持续按住不重复执行。
+- 回位前调用现有安全停止逻辑，退出当前相对位姿遥操作。
+- 使用更新后的 `initial_joint_pose` 和现有 `init_move_speed`。
+- 复用现有 `rm_movej(..., block=1)` 阻塞运动，完成后重新同步关节状态和 QP 状态。
+- 回位后必须先松开 Grip，才能重新进入遥操作。
+- 回位失败时记录错误并保持停止，不自动重试。
+- mock 模式只更新模拟关节状态，不导入或调用厂商 SDK。
+
+## 初始位姿配置
+
+关节角单位沿用现有 YAML，均为度：
+
+- 左臂：`[-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]`
+- 右臂：`[-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]`
+
+`left_arm_rm75.yaml` 使用左臂值，`right_arm_rm75.yaml` 使用右臂值；
+`dual_arm_rm75.yaml` 中左右节点分别使用对应值。三个文件中的 IP、坐标映射、
+工作空间、安全限制及其他控制参数保持不变。
+
+## 代码范围
+
+- 在 `realman_adapter.py` 为真机和 mock 提供同名的公开回位方法，真机实现复用现有
+  私有初始位姿运动代码。
+- 在 `single_arm_velocity_teleop.py` 的现有手柄回调中增加主键上升沿处理。
+- 在现有测试文件中增加最小的适配器回位和按键边沿测试。
+- 同步 `left_arm_rm75.yaml`、`right_arm_rm75.yaml` 和 `dual_arm_rm75.yaml` 中的
+  `initial_joint_pose`。
+
+不修改消息定义、UDP 输入节点、launch、三个 YAML 中的其他参数、安全限位或
+夹爪控制。
+
+## 验证
+
+- 测试主键首次按下触发一次，持续按住不重复触发，松开后可以再次触发。
+- 测试 mock 回位后恢复配置的初始关节角。
+- 测试真机适配器仍只调用既有的关节运动命令；测试使用假 SDK 对象，不连接真机。
+- 检查单臂和双臂配置中的左右初始位姿与上述值一致。
+- 在 `/home/robot/WS_xr` 执行：
+
+  ```bash
+  source /opt/ros/humble/setup.bash
+  python3 -m pytest src/xr_rm_teleop/test/test_initial_joint_pose.py
+  python3 -m pytest src/xr_rm_teleop/test/test_orientation_control.py
+  colcon build --symlink-install
+  ```
+
+全部验证使用 mock 或假对象，不连接真机、不移动机械臂、不操作夹爪。

docs/superpowers/specs/2026-08-03-dual-rm75-ik-model-design.md

@@ -0,0 +1,180 @@
+# 双 RM75 逆解模型替换设计
+
+## 背景与目标
+
+当前左右遥操作节点都加载单臂 `rm75_omnipicker` URDF，求解器将 7 个关节名、
+`q[7:14]` 和 `omnipicker_tcp` 写死。项目新增的
+`xr_rm_teleop/models/dual_rm75/Dual_arm.urdf` 包含真实双臂布局：物理左臂为
+scissor 分支，物理右臂为 omnipic 分支，主要活动区域位于机器人前方。
+
+本次变更目标是：
+
+- 单臂和双臂调试都加载同一份 `dual_rm75` 模型；
+- 左右节点继续独立控制各自的 RM75，只求解当前侧 7 个关节；
+- 保留左右臂各自的局部控制坐标系和现有 PICO 映射；
+- 使用 URDF 中的 TCP 长度，并同步真机外设工具坐标；
+- 把局部后方工作空间余量限制为 `0.10 m`；
+- 保留现有速度、工作空间、圆柱、超时和安全停止逻辑。
+
+本次不增加双臂碰撞规避、公共坐标系目标、双臂协同任务，不合并左右控制节点，
+也不连接或移动真机。
+
+## 方案选择
+
+采用“完整双臂 URDF + 两个独立局部相对位姿任务”。
+
+未采用以下方案：
+
+1. 公共坐标系绝对位姿任务：需要重写 PICO 映射和现有安全限位，改动范围过大。
+2. 从双臂模型拆出两份单臂 URDF：会产生重复模型和后续同步风险。
+
+## 坐标系与控制语义
+
+`dual_arm_base_link` 是完整模型的公共根坐标系。左右控制节点仍以各自机械臂基座
+作为控制和安全坐标系：
+
+| 机械臂 | 局部基坐标系 | TCP | 活动关节 |
+|---|---|---|---|
+| 左臂 | `scissor_base_link` | `scissor_scissor_tcp` | `scissor_joint_1`～`scissor_joint_7` |
+| 右臂 | `omnipic_base_link` | `omnipic_OmniPic_tcp` | `omnipic_joint_1`～`omnipic_joint_7` |
+
+以公共坐标系 `+X` 向机器人右侧、`+Y` 向前、`+Z` 向上为参照，URDF 中局部轴
+朝向如下：
+
+| 局部轴 | 左臂 `scissor_base_link` | 右臂 `omnipic_base_link` |
+|---|---|---|
+| `+X` | 向下 | 向上 |
+| `+Y` | 向后 | 向后 |
+| `+Z` | 向左、远离机身 | 向右、远离机身 |
+| `-Y` | 向前 | 向前 |
+
+现有左右 `xr_to_robot_matrix` 继续把 PICO 相对位置和相对旋转映射到对应局部基
+坐标系。节点产生的目标仍是 `T_base_tcp`，不显式转换成
+`dual_arm_base_link` 下的绝对目标。
+
+## 求解器设计
+
+左右节点使用同一个 `PlacoIkSolver` 类，但每个节点创建自己的求解器实例、机器人
+状态和 QP 任务。两个实例都加载完整 `Dual_arm.urdf`，不共享可变状态。
+
+求解器构造时接收 `arm=left|right`，按固定映射选择局部基坐标系、TCP、当前侧
+关节和另一侧关节。每个实例执行以下设置：
+
+1. 使用 `mask_fbase(True)` 固定 Placo 浮动基座；
+2. mask 另一侧全部 7 个关节；
+3. 使用 Placo 原生
+   `add_relative_frame_task(base_frame, tcp_frame, target)` 创建局部 TCP 任务；
+4. 保留速度限制、动能正则化、最多 30 次有界迭代和现有收敛阈值。
+
+双臂模型的 Placo 状态为 21 个 q 分量：7 个浮动基座分量、右臂 7 个关节、
+左臂 7 个关节。求解器不再使用固定 `q[7:14]`，而是通过当前侧关节名查询：
+
+- `get_joint_offset()`：定位实际关节反馈和逆解结果在 q 中的位置；
+- `get_joint_v_offset()`：定位对应的 URDF 关节速度上限。
+
+当前 URDF 中右臂 q/v offset 分别为 `7～13`/`6～12`，左臂分别为
+`14～20`/`13～19`；实现仍通过名称查询并对这些预期结果做回归测试。
+
+查询 offset 不放宽模型校验。求解器仍检查完整左右关节集合、当前侧恰好 7 个关节、
+offset 唯一有效，以及所需 base 和 TCP 均存在。
+
+`update_joint_state()` 只写入当前侧 7 个关节反馈，并返回当前 TCP 相对当前侧基座的
+`T_base_tcp`。`solve()` 接受相同坐标语义的目标，设置相对位姿任务并只返回当前侧
+7 个关节结果。另一侧关节保持 mask，不参与本实例求解。
+
+## 启动、安装与配置
+
+`arm_debug.launch.py` 的 `arm:=left|right|both` 全部使用：
+
+```text
+xr_rm_teleop/models/dual_rm75/Dual_arm.urdf
+```
+
+双臂模式继续保留 `left_arm_teleop`、`right_arm_teleop` 节点名，`use_mock` 默认
+保持 `true`。`setup.py` 安装 `Dual_arm.urdf` 以及 `dual_rm75/meshes` 中现有的
+`.STL` 和 `.stl` 文件，不新增依赖。
+
+三份控制配置的局部工作空间统一为：
+
+```yaml
+workspace_min: [-0.70, -0.70, 0.10]
+workspace_max: [0.70, 0.10, 0.75]
+```
+
+其中两侧局部 `-Y` 都是机器人前方，`+Y` 后方最多保留 `0.10 m` 余量。其他工作
+空间轴、圆柱限位、线速度、角速度、关节速度、关节加速度和指令超时参数不变。
+
+真机外设配置采用 URDF TCP 长度，但保留当前硬件选择编号：
+
+```yaml
+tools_in_ee:
+  scissor:
+    pose: [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
+  omnipic:
+    pose: [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
+  minisci:
+    pose: [0.0, 0.0, 0.165, 0.0, 0.0, 0.0, 1.0]
+
+arms:
+  left:
+    scissorgripper: 2
+  right:
+    scissorgripper: 1
+```
+
+左臂保留编号 `2`，继续使用控制器 DO3/DO4；该编号按当前配置顺序选中
+`minisci` 工具坐标，因此更新 `minisci.pose.z`。右臂编号 `1` 继续选中
+`omnipic`。URDF 的左分支名 `scissor_*` 与真机外设编号/配置键是两套既有命名，
+不据此改写硬件编号。两侧负载参数和未选中 `scissor.pose` 保持不变。
+
+README 同步说明新模型路径、左右分支/TCP、局部坐标轴和前方工作区。
+
+## 校验与故障处理
+
+模型路径、arm、关节、frame 或 offset 校验失败时，节点在创建 RealMan 适配器前
+终止启动，不连接真机。
+
+运行期间保留现有行为：
+
+- 关节反馈必须包含 7 个有限数值；
+- TCP 目标必须是有限、合法的齐次变换和旋转矩阵；
+- QP 结果必须满足当前侧 URDF 关节位置和单周期速度限制；
+- QP 不收敛时保持上一组有效关节目标；
+- 反馈异常、反馈超时、XR 超时、Grip 松开和节点退出时执行现有安全停止；
+- `configure_safety_limits` 保持 `true`；
+- `move_to_initial_pose_on_connect` 默认保持 `false`。
+
+完整 URDF 虽包含两臂碰撞几何，本次不启用碰撞约束。真机验证不在本次执行范围；
+后续首次真机验证必须分别验证两臂并保持物理隔离。
+
+## 测试与验收
+
+采用现有 pytest、Placo 0.9.4 和 ROS2 构建流程，不新增测试框架。
+
+自动化测试覆盖：
+
+- 双臂 URDF 的 14 个活动关节、base、TCP、固定挂载和 TCP 长度；
+- 左右实例选择正确的关节、q/v offset 和相对任务 frame；
+- 当前实例只更新和返回本侧 7 个关节，另一侧保持不动；
+- 左右初始关节反馈能得到有限的局部 `T_base_tcp`；
+- 左右小幅可达目标能够收敛，结果满足位置、姿态和关节限制；
+- 非法目标、未初始化求解和不收敛故障路径；
+- 左臂编号 `2` 实际选择 `minisci` 且 TCP 为 `0.165 m`；
+- 右臂编号 `1` 实际选择 `omnipic` 且 TCP 为 `0.14 m`；
+- 三份配置的局部 Y 上界均为 `0.10 m`。
+
+所有命令在 `/home/robot/WS_xr` 执行，并先加载 ROS2 Humble：
+
+```bash
+source /opt/ros/humble/setup.bash
+/home/robot/miniconda3/envs/xr/bin/python -m pytest \
+  src/xr_rm_teleop/test/test_placo_transforms.py -v
+pytest src/xr_rm_teleop/test/test_orientation_control.py
+colcon build --symlink-install
+source install/setup.bash
+timeout 15s ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=both use_mock:=true
+```
+
+最后一条命令只验证安装空间中的新模型能被两个 mock 节点加载；`timeout` 到期退出
+属于预期。整个验收过程不得使用 `use_mock:=false`。

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

docs/superpowers/specs/2026-08-12-rm75-j3-simulation-calibration-design.md

@@ -0,0 +1,245 @@
+# RM75 双臂 J3 参考角仿真标定设计
+
+## 1. 目标
+
+在不连接真机、不修改现有生产控制参数的前提下，基于当前双臂 URDF、Placo QP
+求解器和 MuJoCo 运动学模型，分别标定左臂与右臂的第三关节软引导参考角：
+
+\[
+q_{3,\mathrm{ref}}^{L,*},\qquad q_{3,\mathrm{ref}}^{R,*}。
+\]
+
+标定结果只作为当前机器人初始姿态、采摘区域、工具安装和本侧收集筐布局下的
+仿真初值。后续必须通过 mock 完整控制链路和真机低速试验复验，允许根据实测结果
+更新参数。
+
+## 2. 范围
+
+本轮只做离线参数标定：
+
+- 左右臂分别从当前 YAML 初始关节姿态出发；
+- 左臂放入左臂初始 TCP 下方约 40 cm 的本侧收集筐；
+- 右臂放入右臂初始 TCP 下方约 40 cm 的本侧收集筐；
+- 每个轨迹点同时指定 TCP 位置与姿态，保持严格六维跟踪；
+- 第四关节下限暂定左右臂均为 10°；
+- 使用现有 Placo 任务接口临时加入 J3 软任务和位置可操作度任务；
+- 将每个有效关节结果同步写入现有 MuJoCo 双臂模型，检查关节映射和状态有效性；
+- 输出候选角度的逐轨迹指标、汇总排名和推荐平台区间。
+
+本轮不修改 `placo_ik_solver.py`、遥操作节点或 YAML，不测试真机，不加入任务阶段
+状态机、自动姿态放松、碰撞规划或新依赖。
+
+## 3. 坐标与姿态约定
+
+- 双臂机器人公共坐标系 `+Y` 为正前方；
+- 公共坐标系 `+Z` 为机器人垂直向上；
+- 左右方向使用公共坐标系 `X`；
+- 采摘点保持对应机械臂初始 TCP 的横向 `X` 位置；
+- 采摘与退出阶段保持初始 TCP 姿态；
+- 从退出点移动到收集筐上方时，TCP 姿态采用四元数球面插值，平滑旋转为工具工作
+  轴沿公共坐标系 `-Z`；
+- 收集筐上方至筐内的垂直下降段保持工具朝下姿态；
+- 返回初始位姿时平滑恢复初始 TCP 姿态。
+
+“严格六维”表示每个时刻的位置和姿态目标均参与同一 QP，仿真不会因接近奇异点
+而自动降低姿态权重。
+
+## 4. 轨迹族
+
+### 4.1 采摘点
+
+每条机械臂使用 3 个前向距离和 3 个高度：
+
+\[
+y_h\in\{0.30,0.40,0.50\}\ \mathrm{m},
+\]
+
+\[
+z_h\in\{-0.30,-0.20,-0.10\}\ \mathrm{m}。
+\]
+
+这些点位于用户给定的前方 30～50 cm、相对机械臂基座高度 ±50 cm 范围内，并且
+是当前固定初始工具姿态下离线预扫描得到的主要可解高度区间。左右臂各 9 条轨迹，
+共 18 条完整轨迹。
+
+### 4.2 单条完整轨迹
+
+每条轨迹由以下连续段组成：
+
+1. 初始 TCP 位姿；
+2. 采摘点前方 5 cm 的预接近点；
+3. 沿公共 `+Y` 直线进入采摘点；
+4. 沿原路径退回预接近点；
+5. 移动到本侧收集筐上方 10 cm，同时平滑旋转到工具朝下；
+6. 垂直下降 10 cm，到达初始 TCP 下方约 40 cm 的收集筐目标；
+7. 垂直抬升 10 cm；
+8. 返回初始 TCP 位姿。
+
+轨迹按当前控制频率 90 Hz 离散，平移速度不超过 0.15 m/s，角速度不超过
+0.5 rad/s。每条轨迹均从相同初始关节状态重新开始，避免上一候选角或上一轨迹的
+状态污染下一次评估。
+
+### 4.3 边界轨迹
+
+工作区边界、不可达目标和 QP 失败恢复轨迹不参与第一轮 J3 参数排名。J3 参数确定后，
+再使用这些轨迹验证失败保持和恢复逻辑，避免不可达点数量掩盖参考角本身的差异。
+
+## 5. QP 试验配置
+
+主任务保持现有严格六维相对位姿任务，内部等价于：
+
+\[
+\left\|J_p\Delta q-e_p\right\|^2
++\left\|J_R\Delta q-e_R\right\|^2，
+\]
+
+其中误差定义为目标减当前。仿真脚本不改变 Placo 的误差符号。
+
+在主任务之外临时加入：
+
+\[
+w_e\left(q_3+\Delta q_3-q_{3,\mathrm{ref}}\right)^2,
+\qquad w_e=10^{-5}，
+\]
+
+以及 TCP 位置可操作度任务：
+
+\[
+-w_m\nabla m_p(q)^T\Delta q,
+\qquad w_m=10^{-4}。
+\]
+
+保留当前动能正则、URDF 关节位置限制和关节速度限制。第四关节临时增加：
+
+\[
+q_4\ge10^\circ。
+\]
+
+## 6. 参数扫描
+
+### 6.1 粗扫
+
+左臂：
+
+\[
+q_{3,\mathrm{ref}}^L\in\{0^\circ,10^\circ,\ldots,100^\circ\}。
+\]
+
+右臂：
+
+\[
+q_{3,\mathrm{ref}}^R\in\{0^\circ,-10^\circ,\ldots,-120^\circ\}。
+\]
+
+### 6.2 细扫
+
+在粗扫最优候选附近 ±10° 内以 2° 为步长再次扫描。若多个相邻候选没有明显差异，
+选择稳定平台区的中心，而不是选择孤立的单点峰值。
+
+### 6.3 基线
+
+同时运行两组基线：
+
+- `original`：当前原始 QP，不含 J3 软任务、位置可操作度任务和 J4 额外下限；
+- `manip_j4`：不含 J3 软任务，但加入位置可操作度任务和 J4 额外下限。
+
+所有 J3 候选均在 `manip_j4` 基础上只改变 J3 参考角。最终结果必须同时报告：
+
+- 相对当前原始 QP 的改善；
+- 相对“只加位置可操作度”的改善；
+- J3 软任务是否降低六维跟踪成功率。
+
+## 7. 记录指标
+
+对每个候选角度、每条轨迹记录：
+
+- QP 求解失败周期数 `N_fail`；
+- 完成全部轨迹的数量 `N_complete`；
+- 整条轨迹六维雅可比的最小奇异值 `sigma_min`；
+- 第四关节最小安全余量 `m_q4 = min(q4 - 10°)`；
+- 肘部最小外展量 `d_elbow`；
+- 最大 TCP 位置误差和姿态误差；
+- 最大关节速度；
+- 累计关节运动代价 `E_q`；
+- 是否出现超过阈值的单周期关节构型跳变。
+
+肘部外展量使用公共坐标系中第四连杆位置计算：
+
+\[
+d_{\mathrm{elbow}}^L=-x_{\mathrm{elbow}}^L,
+\qquad
+d_{\mathrm{elbow}}^R=x_{\mathrm{elbow}}^R。
+\]
+
+当前 URDF 碰撞网格对相邻连杆存在已知自碰撞警告，因此本轮不把 MuJoCo/Placo
+碰撞距离加入评分，避免错误碰撞几何影响 J3 选择。
+
+## 8. 选择规则与评分函数
+
+### 8.1 硬门槛
+
+候选角度首先按以下顺序筛选：
+
+1. `N_fail` 最少；
+2. `N_complete` 最多；
+3. 最大位置误差不超过 2 mm；
+4. 最大姿态误差不超过 0.005 rad；
+5. 不违反第四关节、URDF 关节位置和速度限制；
+6. 不出现超过配置阈值的单周期关节跳变。
+
+只在通过同一组硬门槛的候选之间使用评分函数。这样不能用较高可操作度抵消更多的
+QP 失败或更差的 TCP 跟踪。
+
+### 8.2 并列候选评分函数
+
+对通过硬门槛的候选，将各项指标在同一机械臂的候选集合内转换为 `[0,1]` 的百分位
+排名。数值越大越好的指标直接排名，数值越小越好的指标反向排名：
+
+- `r_sigma`：全轨迹最小奇异值排名；
+- `r_q4`：第四关节最小安全余量排名；
+- `r_elbow`：肘部最小外展量排名；
+- `r_smooth`：累计关节运动代价与最大关节速度的联合反向排名；
+- `r_track`：最大六维 TCP 跟踪误差的反向排名。
+
+并列候选的综合评分为：
+
+\[
+S(q_{3,\mathrm{ref}})
+=0.45r_{\sigma}
++0.20r_{q4}
++0.20r_{\mathrm{elbow}}
++0.10r_{\mathrm{smooth}}
++0.05r_{\mathrm{track}}。
+\]
+
+选择：
+
+\[
+q_{3,\mathrm{ref}}^*=\arg\max S(q_{3,\mathrm{ref}})。
+\]
+
+最小奇异值权重最高，因为本轮首要目标是降低奇异点和 QP 失败风险；第四关节余量
+和肘部外展各占 0.20；平滑性和跟踪误差用于区分性能接近的候选。若评分最高点与
+相邻角度差异小于 2%，取相邻稳定平台的中心角度。
+
+### 8.3 结果报告
+
+左右臂分别输出：
+
+- 推荐参考角；
+- 推荐稳定区间；
+- 粗扫与细扫排名表；
+- 与两组基线的指标对比；
+- 最差轨迹及其失败位置；
+- 是否建议保留左右臂统一的第四关节 10° 下限。
+
+## 9. 实施边界与后续流程
+
+标定完成后的顺序为：
+
+1. 根据仿真结果形成正式 QP 修改规格；
+2. 将左右臂 J3 参考角作为独立可调参数写入对应 YAML；
+3. 在 `use_mock:=true` 下运行完整遥操作控制链路；
+4. 加入 QP 失败时不提交笛卡尔目标历史的修复并验证恢复；
+5. 经过安全评审后，在真机上以低速、小范围方式复验；
+6. 根据真机日志更新 J3 参考角，但不取消工作空间、速度、超时和安全停止限制。

docs/superpowers/specs/2026-08-13-rm75-qp-robustness-design.md

@@ -0,0 +1,175 @@
+# RM75 双臂采摘 QP 稳健性优化方案概述
+
+## 1. 目标与边界
+
+本方案面向当前双臂机器人从初始位姿向机器人公共坐标系 `+Y` 前方采摘，再移动到
+本侧机械臂初始 TCP 正下方约 40 cm、位于底盘车上的收集筐这一流程。首要目标是：
+
+- 保持手柄给出的 TCP 位置和姿态严格参与六维逆解；
+- 减少奇异点附近的构型恶化、QP 不收敛和连续失败；
+- QP 失败时保持上一安全关节解，并且不提交本周期笛卡尔参考状态；
+- 保留现有工作空间、速度、加速度、指令超时和安全停止限制。
+
+本轮不加入自动采摘状态机、自动放松姿态或真机自动运动，不取消现有安全限制。
+
+此前离线扫描中所有候选均未通过完整轨迹硬门槛，因此不能把扫描得到的左臂 34°、
+右臂 0°写成“最优 J3”。仿真只能说明：两臂 `q4 >= 10°` 均保持正余量，J4 的 10°
+硬下限不是当次 QP 失败的直接原因。
+
+## 2. 更新后的 QP 目标
+
+主任务和辅助任务写为：
+
+\[
+\begin{aligned}
+\min_{\Delta q}\quad
+&\left\|J_p\Delta q-e_p\right\|_{W_p}^2
++\left\|J_R\Delta q-e_R\right\|_{W_R}^2 \\
+&+\lambda\left\|\Delta q\right\|^2
+-w_m\alpha_m(\sigma)\nabla m_6(q)^T\Delta q \\
+&+w_3\left(q_3+\Delta q_3-q_{3,\mathrm{ref}}\right)^2 \\
+&+w_4\alpha_4(q_4)
+  \left[q_{4,\mathrm{warn}}-(q_4+\Delta q_4)\right]_+^2，
+\end{aligned}
+\]
+
+其中：
+
+- `e = 目标位姿 - 当前位姿`，因此主任务使用 `JΔq - e`。如果误差定义相反，公式
+  才写成加号；当前 Placo 代码不翻转误差符号。
+- 前两项是严格六维 TCP 位置和姿态任务，始终保持最高权重。
+- `λ||Δq||²` 是现有动能正则，用于抑制过大的关节增量和数值抖动。
+- `m6` 使用 Placo 支持的 `both` 类型六维可操作度；`αm` 只在完整六维雅可比的
+  最小奇异值进入预警区时逐渐激活，正常区域为零。
+- J3 是低权重软引导，不属于可行性硬门槛。
+- `[x]+ = max(0, x)`；J4 软项只在进入预警区后产生作用，提前远离 10° 硬下限。
+
+辅助项不能通过提高权重来抵消六维 TCP 跟踪。第一版复用 Placo 现有任务接口，不
+引入新的分层 QP 框架或外部依赖。
+
+## 3. 四处修改
+
+### 3.1 六维主任务、可操作度与数值迭代
+
+严格六维 TCP 主任务保持不变，可操作度由“全程恒定启用的位置任务”改为“接近奇异
+区才启用的六维任务”：
+
+- `sigma_min >= sigma_warn`：`αm = 0`，不干扰正常遥操作；
+- `sigma_stop < sigma_min < sigma_warn`：`αm` 从 0 平滑增加到 1；
+- `sigma_min <= sigma_stop`：保持最大辅助权重，但仍不降低六维 TCP 权重。
+
+`sigma_warn`、`sigma_stop` 和最大辅助权重先保留为仿真可调参数，根据现有完整轨迹
+日志确定；不直接沿用此前效果不明显的恒定位置可操作度权重。
+
+当前 30 次求解是同一目标的数值迭代，不是 30 个真实控制周期。生产控制链路仍由
+现有关节速度和加速度限制器约束实际运动，因此不把“单个物理周期到达完整目标”作为
+收敛要求。求解器逐次检查六维误差；只有达到当前位置和姿态阈值的结果才允许发送。
+30 次内未收敛则恢复到本周期实际关节反馈，不发送未收敛的中间结果。
+
+### 3.2 J3 初始姿态软参考
+
+取消继续扫描 J3 最优角，使用当前 YAML 初始姿态作为第一版参考：
+
+\[
+q_{3,\mathrm{ref}}^L=67.96^\circ，\qquad
+q_{3,\mathrm{ref}}^R=-89.57^\circ。
+\]
+
+J3 只使用低权重软任务，不设置 J3 硬限位，不因追踪参考角而放松 TCP 位姿任务。
+代表性严格六维 mock 路径显示：左臂使用 `1e-5` 可完成路径，提高到 `1e-4` 会提前
+触及关节限位；右臂使用 `1e-5` 时 J6 到达 URDF 下限，提高到 `1e-4` 后保留约
+23° J6 余量并完成前伸段。因此第一版分别取：
+
+\[
+w_3^L=10^{-5}，\qquad w_3^R=10^{-4}。
+\]
+
+左右臂参数分别配置，后续只在完整 mock 轨迹明显改善时再调整，不把参考角本身当作
+成功保证。
+
+### 3.3 J4 硬下限与软缓冲区
+
+左右臂暂时保持相同硬约束：
+
+\[
+q_4\geq q_{4,\min}=10^\circ。
+\]
+
+在硬下限上方增加预警区，第一版取 `q4_warn = 25°`：
+
+- `q4 >= 25°`：J4 软项关闭；
+- `10° < q4 < 25°`：软项随接近 10°逐渐增强；
+- `q4 <= 10°`：由硬约束禁止继续向下。
+
+这样保留收集筐下降阶段所需的可达空间，同时避免 QP 到达 10°附近才突然遇到约束
+边界。`25°` 是待仿真验证的缓冲起点，不是新的硬下限；左右臂允许分别调整预警角，
+但除非轨迹数据证明有必要，不增加更多参数。
+
+### 3.4 QP 失败恢复与笛卡尔参考状态提交
+
+这是除目标函数外最关键的修复。当前风险流程为：
+
+```text
+QP 失败
+→ 关节指令保持不动
+→ 笛卡尔目标历史仍向前更新
+→ 下一周期误差进一步增大
+→ 连续失败或恢复时突跳
+```
+
+修改后，QP 求解结果、关节目标和笛卡尔参考状态按同一周期提交。
+
+QP 成功时：
+
+```text
+QP 成功
+→ 发送新关节目标
+→ 关节目标发送成功
+→ 提交新的笛卡尔参考状态
+```
+
+QP 失败或关节目标发送失败时：
+
+```text
+QP 失败
+→ 丢弃失败后的 Placo 内部迭代结果
+→ 保持上一有效关节目标
+→ 不提交本周期笛卡尔参考状态
+→ 操作者把手柄移回可行区域后继续求解
+```
+
+“不提交笛卡尔参考状态”包括不更新本周期候选的滤波状态、
+`_last_sent_target`、`_last_sent_orientation` 和命令时间。下一周期仍从上一已提交的
+笛卡尔参考状态以及实际关节反馈出发计算，防止 QP 误差在机械臂不动时继续累积。
+
+手柄原始输入仍正常接收，不会被程序改写，也不会自动改变操作者给出的末端姿态。
+失败时机械臂不会为了恢复而自行移动；操作者主动将手柄移回可行区域后，QP 使用新的
+手柄输入重新求解。收集筐到达和松开夹爪仍以实际 TCP 反馈及位置、姿态容差为判据。
+
+## 4. 保留约束
+
+QP 和下游控制继续保留：
+
+- URDF 关节位置限制和 J4 的 10°额外硬下限；
+- 现有关节速度、关节加速度、TCP 线速度和角速度限制；
+- 工作空间/圆柱限位、指令超时和安全停止；
+- `configure_safety_limits` 默认启用；
+- `move_to_initial_pose_on_connect` 默认关闭；
+- mock 模式不依赖睿尔曼真机 SDK。
+
+## 5. 验证顺序与通过标准
+
+实施按以下顺序进行：
+
+1. 先实现 QP 失败保持，以及关节目标与笛卡尔参考状态的成功后统一提交；
+2. 加入 J4 的 10°硬下限与 25°软缓冲区；
+3. 加入左右臂 J3 初始姿态软参考；
+4. 加入按六维最小奇异值激活的 `both` 可操作度任务；
+5. 在 `use_mock:=true` 下运行初始位姿、前方 30～50 cm 采摘、本侧下方 40 cm 收集
+   筐和返回初始位姿的完整严格六维轨迹。
+
+至少记录并比较修改前后的：QP 成功/失败周期数、连续失败长度、失败周期参考状态是否
+保持不变、恢复时的关节跳变量、完整轨迹成功数、
+六维最小奇异值、最大位置/姿态误差、J4 最小余量、最大关节速度以及目标历史与实际
+TCP 的偏差。只有失败周期下降、完整轨迹成功率不降低、严格六维误差和全部安全约束
+仍满足时，辅助项才保留；否则首先回退可操作度或 J4 软项，不回退失败状态修复。

docs/superpowers/specs/2026-08-24-rm75-ik-method-comparison-design.md

@@ -0,0 +1,415 @@
+# RM75 三种逆运动学方法离线对比实验设计
+
+## 1. 目标
+
+基于现有番茄采摘 episode 的右臂目标位姿轨迹，在完全一致的机械臂模型、初始关节
+状态、时间轴、收敛判据和输出安全限制下，对比以下三种七自由度逆运动学方法：
+
+1. Jacobian Moore-Penrose 伪逆法；
+2. 阻尼最小二乘法（Damped Least Squares，DLS）；
+3. 当前项目中的优化 Placo QP 方法。
+
+实验输出用于补充中期报告 2.3.4 节预留的三张图，并同时生成逐采样数据、汇总指标和
+可直接粘贴到报告中的中文结果分析。实验必须由真实计算结果驱动，不预设或硬编码
+“QP 更优”的结论。
+
+## 2. 现有上下文
+
+### 2.1 报告要求
+
+中期报告 2.3.4 节已经确定：
+
+- 三种方法使用同一机械臂模型、初始关节状态和末端目标轨迹；
+- 统计位置 RMSE、姿态 RMSE、归一化关节安全裕度、最大关节速度、求解时间和
+  求解成功率；
+- 章节末尾预留三张对比图。
+
+现有图号从图 2-10 跳到图 2-14，因此本实验生成图 2-11、图 2-12 和图 2-13。
+
+### 2.2 当前 QP 与报告文字的差异
+
+报告 2.3.2 节主要描述六维末端软任务、动能正则化、关节位置和速度限制。当前分支的
+`PlacoIkSolver` 还包含：
+
+- J3 初始构型软引导；
+- J4 硬下限和预警区软缓冲；
+- 接近奇异区时动态启用的六维可操作度任务；
+- QP 失败时恢复实际关节状态并保持上一安全输出。
+
+本实验使用当前优化 QP，而不是关闭上述辅助任务的基础 QP。最终分析文件需要提供一段
+方法补充文字，避免报告方法描述与对比对象不一致。
+
+## 3. 范围与安全边界
+
+### 3.1 本次包含
+
+- 只读加载一个现有右臂 episode；
+- 离线重采样目标位姿；
+- 在同一 URDF 上运行三种逆运动学方法；
+- 复用当前 QP 代码和右臂 YAML 参数；
+- 对三种方法使用相同的输出端安全处理；
+- 生成 SVG、300 dpi PNG、CSV、JSON 和中文 Markdown 分析。
+
+### 3.2 本次不包含
+
+- 不连接真机，不移动机械臂，不操作夹爪；
+- 不启动新的 PICO 录制；
+- 不修改生产遥操作节点、launch、YAML 默认值或公开 API；
+- 不使用 episode 中已经记录的 QP 关节结果充当本次 QP 结果；
+- 不模拟电机、通信和接触动力学；
+- 不直接编辑用户提供的 PDF。
+
+实验只使用当前 Conda 环境已经安装的 NumPy、h5py、Matplotlib 和 Placo，不新增项目
+依赖。
+
+因此，结果应表述为“基于真实遥操作目标轨迹的离线运动学对比”，不得表述为新的真机
+在线控制对比。
+
+## 4. 数据源与质量基线
+
+实验固定使用：
+
+```text
+/home/robot/ACT_Data/tomato_pick/episode_0.hdf5
+```
+
+该文件的已核对属性如下：
+
+- 机械臂：`right_rm75`；
+- 样本数：484；
+- 有效时长：约 16.1 s；
+- 保存采样率：约 30 Hz；
+- 位姿顺序：`x,y,z,qx,qy,qz,qw`；
+- 所有目标位姿、当前位姿和关节状态均为有限值；
+- 目标和当前四元数范数接近 1；
+- 483 帧为遥操作激活且已发送命令；
+- 记录时 QP 尝试 483 次并成功 483 次；
+- 132 帧触发过目标限幅，轨迹本身包含足够的约束压力。
+
+只使用满足以下条件的最长连续区间：
+
+```text
+teleop_active && action_valid && command_sent
+```
+
+共同末端目标取 `debug/tcp/final_target_pose`。该字段已通过原系统的工作空间限制、目标
+平滑和单帧笛卡尔步长限制，适合作为三种逆运动学方法的共同安全输入。共同初始关节角
+取有效区间第一帧的 `observations/qpos[:7]`。
+
+episode 中后续 `observations/qpos`、`debug/qp/raw_target`、QP 成功标志和耗时只用于
+数据质量核对，不替代任何方法在本实验中的离线计算结果。
+
+## 5. 统一复放架构
+
+数据流为：
+
+```text
+episode_0.hdf5
+  -> 有效区间与共同初始状态
+  -> 30 Hz 目标位姿重采样到 90 Hz
+  -> 伪逆 / DLS / 当前优化 QP 三路独立复放
+  -> 共同输出安全层
+  -> 正向运动学和逐采样指标
+  -> CSV / JSON / 三张图 / 中文分析
+```
+
+三种方法各自维护独立的关节状态和上一周期关节速度。每个方法的下一状态只能由该方法
+本周期的安全输出推进，三路之间不共享可变状态。
+
+离线状态推进采用理想位置跟随，即共同输出限速器给出的关节目标直接作为下一 90 Hz
+周期的关节状态。这一简化隔离了逆运动学方法本身，不引入未建模的电机和网络差异。
+
+## 6. 目标轨迹重采样
+
+原 episode 按约 30 Hz 保存，而当前遥操作控制器使用 90 Hz。重采样使用 episode 的
+`debug/timestamps/control_monotonic_ns`，目标时间轴保持原始起止时刻并以 1/90 s
+采样：
+
+- 位置使用分段线性插值；
+- 姿态使用归一化四元数的最短弧 SLERP；
+- 相邻四元数点积为负时先翻转后一四元数，避免绕长弧插值；
+- 第一个和最后一个重采样位姿必须与原始有效区间端点一致；
+- 不对目标轨迹额外放大、延长或人工加入困难片段。
+
+## 7. 三种逆运动学方法
+
+### 7.1 共同任务定义
+
+当前关节状态为 `q`，正向运动学得到当前 TCP 位姿 `(p, R)`，目标位姿为
+`(p_d, R_d)`。位置误差和姿态误差分别为：
+
+```text
+e_p = p_d - p
+e_R = Log(R^T R_d)
+```
+
+求解时的角速度误差表达必须与所用 `local_world_aligned` Jacobian 的坐标表达一致；
+姿态误差大小统一使用目标与实际旋转矩阵之间的最短夹角评价。伪逆和 DLS 使用相同的
+位置、姿态反馈增益、相同 Jacobian、相同 90 Hz 步长和相同数值迭代框架。
+
+三种方法对单个目标最多执行 30 次数值迭代。满足以下两个条件时记为收敛：
+
+```text
+位置误差 <= 0.002 m
+姿态误差 <= 0.005 rad
+```
+
+### 7.2 Jacobian 伪逆法
+
+伪逆法按报告公式计算：
+
+```text
+q_dot = pinv(J) * v_d
+```
+
+其中 `v_d` 由共同的六维位姿反馈误差生成。实现直接使用 NumPy 的 Moore-Penrose
+伪逆，不增加零空间任务、阻尼或自适应奇异值阈值，以保持基线定义清楚。
+
+### 7.3 DLS 方法
+
+DLS 按报告公式计算：
+
+```text
+q_dot = J^T * inv(J * J^T + mu^2 * I) * v_d
+```
+
+公式保持与报告一致；数值实现使用线性方程求解，不显式计算矩阵逆。
+
+只扫描固定阻尼系数，不实现自适应 DLS。候选值使用对数尺度的小集合：
+
+```text
+0.001, 0.003, 0.01, 0.03, 0.1, 0.3
+```
+
+每个候选均完整复放 episode，先按求解成功率从高到低选择，再在成功率相同的候选中
+最小化：
+
+```text
+位置 RMSE / 0.002 + 姿态 RMSE / 0.005
+```
+
+若仍并列，选择最大关节速度更小的候选。阻尼扫描使用同一条评价轨迹，因此最终文字
+必须说明该 DLS 是“在当前轨迹上选优的固定阻尼基线”；这一口径对 DLS 较有利，不能
+将其解释为跨轨迹最优参数。
+
+阻尼扫描耗时不计入三种方法的在线求解时间对比。
+
+### 7.4 当前优化 QP
+
+QP 直接实例化现有 `xr_rm_teleop.placo_ik_solver.PlacoIkSolver`，使用 90 Hz 步长、
+当前双臂 URDF 和右臂配置中的参数：
+
+```text
+qp_j3_reference_deg: -89.57
+qp_j3_weight: 0.0001
+qp_j4_min_deg: 10.0
+qp_j4_warn_deg: 25.0
+qp_j4_weight: 0.0001
+qp_manipulability_sigma_stop: 0.01
+qp_manipulability_sigma_warn: 0.04
+qp_manipulability_weight: 0.0001
+```
+
+保留现有六维末端软任务、`1e-6` 动能正则化、URDF 关节位置和速度限制、30 次迭代、
+收敛阈值、输入变换校验、失败恢复和结果有效性检查。实验脚本不复制或重写 QP。
+
+## 8. 共同输出安全层与失败处理
+
+三种方法使用同一安全口径：
+
+1. 每次数值迭代结果必须为 7 个有限关节值；
+2. 数值迭代的关节状态不得超出 URDF 位置范围；
+3. 相邻数值迭代的关节变化不得超过 URDF 速度上限乘以 `1/90 s`；
+4. 有效求解结果继续经过生产控制器现有的关节速度/加速度限制逻辑；
+5. 输出端最大关节速度为 `180 deg/s`，最大关节加速度为 `300 deg/s^2`；
+6. 未在 30 次内收敛、出现非有限值或违反硬边界时，本周期记为失败并保持上一安全
+   关节状态；
+7. 失败不会停止离线复放，时间轴继续推进，并记录失败次数和最长连续失败长度。
+
+QP 在优化内部主动处理关节边界；伪逆和 DLS 在每次候选步之后接受同样的硬检查。
+共同输出层不会消除算法差异：基线仍可能因候选步无效而失败或保持，QP 则可能在优化
+过程中找到满足约束的解。
+
+## 9. 评价指标
+
+### 9.1 末端跟踪
+
+- 逐采样位置误差 `||p_d - p||`，单位为 mm；
+- 逐采样姿态夹角误差，单位为 degree；
+- 全轨迹位置 RMSE，报告中仍以 m 给出，图中用 mm；
+- 全轨迹姿态 RMSE，报告公式使用 rad，图中用 degree。
+
+### 9.2 关节运动
+
+- 每个采样时刻七关节绝对速度的最大值，单位为 `deg/s`；
+- 全轨迹最大关节速度；
+- 超过或触发共同速度/加速度限制器的周期数；
+- 按报告式 (2-28) 计算的逐采样最小归一化关节安全裕度；
+- 全轨迹最小归一化关节安全裕度。
+
+速度图不再使用归一化速度。当前 URDF 中右臂七个关节的速度上限均为 `3.14 rad/s`
+（约 `180 deg/s`），直接展示实际速度更直观且与报告文字一致。
+
+### 9.3 求解性能
+
+- 收敛成功周期数和成功率；
+- 失败周期数和最长连续失败长度；
+- 单周期 IK 求解平均时间和最大时间，单位为 ms。
+
+耗时只覆盖单次 IK 求解，不包含 HDF5 读取、重采样、指标汇总和绘图。先执行一次完整
+预热复放，再对选定参数的三种方法各重复 10 次。轨迹和非耗时指标必须在重复复放间
+保持确定；平均和最大耗时从 10 次计时复放汇总。
+
+## 10. 三张图设计
+
+### 10.1 图 2-11 三种逆运动学方法末端位姿跟踪误差对比
+
+使用上下两个共享时间轴的子图：
+
+- `(a)` 位置误差时序，单位 mm；
+- `(b)` 姿态误差时序，单位 degree。
+
+三种方法使用固定颜色、不同线型，并在失败保持区间添加不遮挡曲线的标记。图中不绘制
+episode 原始 QP 误差曲线。
+
+### 10.2 图 2-12 三种逆运动学方法关节运动约束对比
+
+使用两个共享时间轴的子图：
+
+- `(a)` 每个时刻的最大关节速度，单位 `deg/s`，并绘制 `180 deg/s` 虚线；
+- `(b)` 每个时刻的最小归一化关节安全裕度，数值越大表示离关节上下限越远。
+
+### 10.3 图 2-13 三种逆运动学方法综合性能指标对比
+
+使用 `2 x 3` 六个小型分组柱状图，避免不同量纲共用坐标轴：
+
+1. 位置 RMSE；
+2. 姿态 RMSE；
+3. 最大关节速度；
+4. 最小归一化关节安全裕度；
+5. 平均和最大求解时间；
+6. 求解成功率。
+
+柱顶标注精确数值。最终配色需兼顾色盲识别和灰度打印，除颜色外再使用线型、标记和
+图例区分方法。
+
+## 11. 文件与产物
+
+实验实现优先保持最小范围：
+
+```text
+xr_rm_teleop/test/ik_method_comparison.py
+xr_rm_teleop/test/test_ik_method_comparison.py
+```
+
+前者包含命令行入口、HDF5 读取、重采样、三种方法复放、指标计算和绘图；后者只覆盖
+无法由现有测试保护的新非平凡逻辑，不新增测试框架或通用评测抽象。
+
+默认输出目录为：
+
+```text
+output/ik_comparison/episode_0/
+```
+
+产物包括：
+
+```text
+samples.csv
+summary.json
+figure_2_11_tracking_error.svg
+figure_2_11_tracking_error.png
+figure_2_12_joint_constraints.svg
+figure_2_12_joint_constraints.png
+figure_2_13_summary.svg
+figure_2_13_summary.png
+analysis_2.3.4.md
+```
+
+`samples.csv` 使用长表结构，每行对应“方法 + 时间点”，至少包含目标位姿、实际位姿、
+位置误差、姿态误差、七关节角、七关节速度、最小安全裕度、求解耗时、成功标志和限制
+触发标志。`summary.json` 保存输入路径、Git 提交、参数、选定 DLS 阻尼、指标和产物路径，
+保证结果可追溯。
+
+`analysis_2.3.4.md` 使用中文撰写，包含：
+
+- 数据来源和离线实验口径；
+- DLS 最终阻尼和选择规则；
+- 三张图的建议图题与图注；
+- 与式 (2-25) 至式 (2-28) 对应的数值结果；
+- 对优势、代价和异常结果的客观分析；
+- 当前优化 QP 相对报告 2.3.2 节的补充方法说明。
+
+## 12. 错误处理
+
+以下情况在生成任何正式图前立即报错：
+
+- episode 路径不存在或不是 HDF5；
+- 必需字段或属性缺失；
+- 数组长度不一致；
+- 找不到至少包含两个样本的连续有效遥操作区间；
+- 时间戳非严格递增；
+- 位姿、关节角或四元数含 NaN/Inf；
+- 四元数无法正规化；
+- episode 机械臂不是 `right_rm75`；
+- URDF 或当前右臂配置不存在；
+- Placo 版本不是项目固定的 0.9.4；
+- 任一方法没有生成与统一时间轴等长的结果；
+- CSV、JSON 和绘图使用的汇总数值不一致。
+
+单个目标的逆运动学失败属于实验结果，按上一安全状态保持，不中止整条轨迹。输入数据
+结构错误、模型错误和结果长度错误属于实验无效，必须中止并说明原因。
+
+## 13. 测试与验证
+
+### 13.1 聚焦测试
+
+最小测试至少覆盖：
+
+- 30 Hz 到 90 Hz 重采样保持首尾位置和姿态；
+- SLERP 选择最短弧并输出单位四元数；
+- 姿态夹角误差在单位旋转和已知小角度下正确；
+- 关节安全裕度与式 (2-28) 一致；
+- 无效候选触发失败保持而不是推进状态；
+- DLS 选择规则按成功率、归一化误差和最大速度依次决策；
+- 汇总指标与逐采样数据一致。
+
+### 13.2 真实模型冒烟验证
+
+使用当前双臂 URDF 和右臂初始关节角，对三种方法各运行一小段真实目标位姿序列，确认：
+
+- 输出始终为有限 7 维关节值；
+- 没有输出越过 URDF 关节位置边界；
+- 失败时保持上一安全状态；
+- 当前 QP 直接走现有 `PlacoIkSolver`，没有本地复制实现。
+
+### 13.3 项目级验证
+
+从工作空间根目录 `/home/robot/WS_xr` 执行，并先加载 ROS2 Humble：
+
+```bash
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+pytest src/xr_rm_teleop/test/test_orientation_control.py
+```
+
+随后使用项目固定的 Conda Python 运行聚焦测试和完整离线实验。验证完成后还需检查：
+
+- 三张 PNG 无裁切、重叠、乱码或不可辨识曲线；
+- SVG 可编辑且文字完整；
+- PNG 为 300 dpi；
+- 图题、坐标轴、单位和图例为中文论文风格；
+- `summary.json` 与图中柱顶数值一致；
+- 同一输入重复运行时，除耗时外的结果一致。
+
+## 14. 验收标准
+
+满足以下条件才视为完成：
+
+1. 三种方法从完全相同的 episode 目标轨迹和初始关节状态开始；
+2. 当前优化 QP 复用现有实现和右臂参数；
+3. 三种方法使用同一输出安全口径，任何失败均安全保持；
+4. DLS 固定阻尼选择过程和最终值可追溯；
+5. 生成三张与报告公式和图号一致的正式对比图；
+6. 生成完整 CSV、JSON 和中文 2.3.4 分析文字；
+7. 所有实际执行的测试和构建结果如实记录；
+8. 不连接真机、不修改生产控制默认值、不新增依赖和重复 QP 实现。

openspec/config.yaml

@@ -1,20 +0,0 @@
-schema: spec-driven
-
-# Project context (optional)
-# This is shown to AI when creating artifacts.
-# Add your tech stack, conventions, style guides, domain knowledge, etc.
-# Example:
-#   context: |
-#     Tech stack: TypeScript, React, Node.js
-#     We use conventional commits
-#     Domain: e-commerce platform
-
-# Per-artifact rules (optional)
-# Add custom rules for specific artifacts.
-# Example:
-#   rules:
-#     proposal:
-#       - Keep proposals under 500 words
-#       - Always include a "Non-goals" section
-#     tasks:
-#       - Break tasks into chunks of max 2 hours

output/ik_comparison/episode_0/analysis_2.3.4.md

@@ -0,0 +1,29 @@
+# 2.3.4 三种逆运动学方法对比补充分析
+
+本结果是基于真实遥操作目标轨迹的离线运动学对比，不代表真机闭环实验。数据来自
+`/home/robot/ACT_Data/tomato_pick/episode_0.hdf5`，目标位姿以 90.0 Hz 重采样；三种方法使用同一初始
+关节状态、同一 URDF、相同收敛阈值和共同的输出速度/加速度限制。
+
+DLS 扫描的固定阻尼候选为 0.001, 0.003, 0.01, 0.03, 0.1, 0.3，本轨迹选定
+`0.3`。该参数是在当前评价轨迹上选优，不应解释为跨轨迹最优参数。
+
+| 方法 | 位置 RMSE (m) | 姿态 RMSE (rad) | 最大关节速度 (°/s) | 最小归一化裕度 | 平均/最大求解时间 (ms) | 成功率 |
+| --- | ---: | ---: | ---: | ---: | ---: | ---: |
+| Jacobian 伪逆 | 0.260542 | 0.713938 | 30.000 | 0.1509 | 0.136 / 1.061 | 1.17% |
+| DLS | 0.005845 | 0.011201 | 49.999 | 0.0607 | 0.391 / 1.942 | 100.00% |
+| 优化 QP | 0.004929 | 0.011075 | 46.666 | 0.0611 | 0.206 / 1.057 | 100.00% |
+
+图 2-11 三种逆运动学方法的末端位置与姿态跟踪误差。纵轴采用对数坐标以同时显示不同
+数量级的误差，叉号稀疏标记数值求解失败并保持上一安全关节状态的周期。
+
+图 2-12 三种逆运动学方法的最大关节速度与最小归一化关节安全裕度。红色虚线表示
+180°/s 输出速度上限，裕度越大表示离关节位置边界越远。
+
+图 2-13 三种逆运动学方法的综合性能对比，包括误差、关节运动、求解时间和成功率。
+
+按本次单轨迹数值比较，位置 RMSE 最低的方法为优化 QP，姿态 RMSE 最低的方法为
+优化 QP，成功率最高的方法为DLS、优化 QP。这些结论只描述本次离线复放，
+未进行统计显著性检验。
+
+当前优化 QP 除六维末端主任务外，还保留项目中的 J3 参考软任务、J4 硬下界与软缓冲，
+以及按最小奇异值动态激活的六维可操作度任务；伪逆和 DLS 基线不包含这些附加任务。

output/ik_comparison/episode_0/summary.json

@@ -0,0 +1,50 @@
+{
+  "source_episode": "/home/robot/ACT_Data/tomato_pick/episode_0.hdf5",
+  "git_commit": "f30aac547b6fe84703044c66b30b70720ee4d40a",
+  "sample_rate_hz": 90.00141938026849,
+  "selected_dls_damping": 0.3,
+  "methods": {
+    "pinv": {
+      "method": "pinv",
+      "damping": null,
+      "success_rate": 0.011748445058742226,
+      "position_rmse_m": 0.26054182896155575,
+      "orientation_rmse_rad": 0.713938311140316,
+      "max_joint_speed_deg_s": 29.999526880705357,
+      "min_joint_margin": 0.15094435580838983,
+      "mean_solve_ms": 0.13594157401520388,
+      "max_solve_ms": 1.061404,
+      "failure_count": 1430,
+      "longest_failure_streak": 1430,
+      "command_limited_count": 17
+    },
+    "dls": {
+      "method": "dls",
+      "damping": 0.3,
+      "success_rate": 1.0,
+      "position_rmse_m": 0.005845412147117041,
+      "orientation_rmse_rad": 0.011200514621088859,
+      "max_joint_speed_deg_s": 49.999211467842265,
+      "min_joint_margin": 0.060689206887586424,
+      "mean_solve_ms": 0.39115279765031097,
+      "max_solve_ms": 1.94196,
+      "failure_count": 0,
+      "longest_failure_streak": 0,
+      "command_limited_count": 1282
+    },
+    "qp": {
+      "method": "qp",
+      "damping": null,
+      "success_rate": 1.0,
+      "position_rmse_m": 0.004929450681893051,
+      "orientation_rmse_rad": 0.01107496419458487,
+      "max_joint_speed_deg_s": 46.66593070331945,
+      "min_joint_margin": 0.06105232712172298,
+      "mean_solve_ms": 0.20632571050449205,
+      "max_solve_ms": 1.057197,
+      "failure_count": 0,
+      "longest_failure_streak": 0,
+      "command_limited_count": 1267
+    }
+  }
+}

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/dual_arm_rm75.yaml

@@ -1,10 +1,10 @@
 # 阶段一：PICO 遥操作双 RM75 平台配置。
 #
-# 当前控制方式是“相对位姿透传”：
+# 当前控制方式是“相对 TCP + 单步 QP”：
 # 按下 grip 时锁定当前手柄位姿和 TCP 位姿，之后将手柄相对位移和相对旋转
 # 映射为目标 TCP 位姿，经过工作空间限幅、目标低通、姿态低通和单帧步长
-# 限制后，通过 rm_movep_canfd 下发。cmd_vel 仅作为目标位姿变化率调试话题，
-# 不是机械臂执行命令。
+# 限制后，通过 Placo 单步 QP 和 rm_movej_canfd 下发 7 个关节目标。
+# cmd_vel 仅作为目标位姿变化率调试话题，不是机械臂执行命令。
 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。
 
 left_arm_teleop:
@@ -13,27 +13,36 @@ left_arm_teleop:
     controller_topic: /xr/left_controller
     control_rate_hz: 90.0
     command_timeout_sec: 0.12
+    feedback_resync_timeout_sec: 0.5
 
     # 位姿目标生成与平滑参数。
-    scale: 0.75
+    scale: 0.7
     deadband_m: 0.001
     target_filter_alpha: 0.65
     target_filter_alpha_fast: 0.9
     target_filter_fast_threshold_m: 0.03
-    max_linear_speed: 0.2
+    max_linear_speed: 0.15
     enable_position_axes: [true, true, true]
     enable_orientation_control: true
     enable_orientation_axes: [true, true, true]
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
-    max_orientation_speed: 0.6
-    current_pose_poll_hz: 10.0
+    max_orientation_speed: 0.5
 
-    workspace_min: [-0.70, -0.60, 0.10]
-    workspace_max: [0.70, 0.40, 0.70]
-    cyl_radius_limit: [0.20, 0.60]
-    low_z_threshold: 0.20
-    low_z_min_radius: 0.21
+    # QP 辅助任务：严格六维 TCP 主任务保持最高权重。
+    qp_j3_reference_deg: 67.96
+    qp_j3_weight: 0.00001
+    qp_j4_min_deg: 10.0
+    qp_j4_warn_deg: 25.0
+    qp_j4_weight: 0.0001
+    qp_manipulability_sigma_stop: 0.01
+    qp_manipulability_sigma_warn: 0.04
+    qp_manipulability_weight: 0.0001
+    workspace_min: [-0.70, -0.70, 0.10]
+    workspace_max: [0.70, 0.10, 0.75]
+    cyl_radius_limit: [0.10, 0.80]
+    low_z_threshold: 0.1
+    low_z_min_radius: 0.1
 
     # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
     # 映射关系：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
@@ -41,24 +50,28 @@ left_arm_teleop:
                          0.0, 0.0, 1.0,
                          -1.0, 0.0, 0.0]
 
-    use_mock: false
-    mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
     robot_ip: 192.168.192.18
     robot_port: 8080
-    avoid_singularity: 0
-    frame_type: 1
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8089
+    realtime_push_cycle_ms: 5
+    avoid_singularity: 1
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
     configure_safety_limits: true
-    max_line_speed: 1.0
-    max_angular_speed: 1.5
-    max_line_acc: 1.0
-    max_angular_acc: 2.0
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
+    max_line_speed: 0.25
+    max_angular_speed: 0.6
+    max_line_acc: 1.3
+    max_angular_acc: 3.0
     joint_max_speed: 180.0
-    joint_max_acc: 180.0
+    joint_max_acc: 300.0
     move_to_initial_pose_on_connect: false
-    initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
+    initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
     init_move_speed: 20
     debug_topic_prefix: /xr_rm
 
@@ -68,26 +81,34 @@ right_arm_teleop:
     controller_topic: /xr/right_controller
     control_rate_hz: 90.0
     command_timeout_sec: 0.12
+    feedback_resync_timeout_sec: 0.5
 
-    scale: 0.75
+    scale: 0.7
     deadband_m: 0.001
     target_filter_alpha: 0.65
     target_filter_alpha_fast: 0.9
-    target_filter_fast_threshold_m: 0.03
-    max_linear_speed: 0.2
+    target_filter_fast_threshold_m: 0.05
+    max_linear_speed: 0.15
     enable_position_axes: [true, true, true]
     enable_orientation_control: true
     enable_orientation_axes: [true, true, true]
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
-    max_orientation_speed: 0.6
-    current_pose_poll_hz: 10.0
+    max_orientation_speed: 0.5
 
-    workspace_min: [-0.70, -0.60, 0.10]
-    workspace_max: [0.70, 0.40, 0.70]
-    cyl_radius_limit: [0.20, 0.60]
-    low_z_threshold: 0.20
-    low_z_min_radius: 0.21
+    qp_j3_reference_deg: -89.57
+    qp_j3_weight: 0.0001
+    qp_j4_min_deg: 10.0
+    qp_j4_warn_deg: 25.0
+    qp_j4_weight: 0.0001
+    qp_manipulability_sigma_stop: 0.01
+    qp_manipulability_sigma_warn: 0.04
+    qp_manipulability_weight: 0.0001
+    workspace_min: [-0.70, -0.70, 0.10]
+    workspace_max: [0.70, 0.10, 0.75]
+    cyl_radius_limit: [0.10, 0.80]
+    low_z_threshold: 0.1
+    low_z_min_radius: 0.1
 
     # PICO/OpenXR 位置坐标：+X 向右，+Y 向上，+Z 向后。
     # 映射关系：机器人位移增量 = [手柄y, 手柄z, 手柄x]。
@@ -95,23 +116,27 @@ right_arm_teleop:
                          0.0, 0.0, 1.0,
                          1.0, 0.0, 0.0]
 
-    use_mock: false
-    mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
     robot_ip: 192.168.192.19
     robot_port: 8080
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8090
+    realtime_push_cycle_ms: 5
     avoid_singularity: 1
-    frame_type: 1
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
     configure_safety_limits: true
-    max_line_speed: 1.0
-    max_angular_speed: 1.5
-    max_line_acc: 1.0
-    max_angular_acc: 2.0
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
+    max_line_speed: 0.25
+    max_angular_speed: 0.6
+    max_line_acc: 1.3
+    max_angular_acc: 3.0
     joint_max_speed: 180.0
-    joint_max_acc: 180.0
+    joint_max_acc: 300.0
     move_to_initial_pose_on_connect: false
-    initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
+    initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
     init_move_speed: 20
     debug_topic_prefix: /xr_rm

xr_rm_bringup/config/left_arm_rm75.yaml

@@ -1,4 +1,4 @@
-# 左臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
+# 左臂单独调试配置：XR TCP 目标经 Placo QP 转换为 RM75 关节目标。
 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。
 
 single_arm_velocity_teleop:
@@ -7,50 +7,63 @@ single_arm_velocity_teleop:
     controller_topic: /xr/left_controller
     control_rate_hz: 90.0
     command_timeout_sec: 0.12
+    feedback_resync_timeout_sec: 0.5
 
     # 手柄相对位姿 -> 目标 TCP 位姿；随后做目标低通、姿态低通和单帧步长限制。
-    scale: 1.0
+    scale: 0.7
     deadband_m: 0.001
     target_filter_alpha: 0.65
     target_filter_alpha_fast: 0.9
     target_filter_fast_threshold_m: 0.03
-    max_linear_speed: 0.3
+    max_linear_speed: 0.15
     enable_position_axes: [true, true, true]
     enable_orientation_control: true
     enable_orientation_axes: [true, true, true]
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
-    max_orientation_speed: 0.6
-    current_pose_poll_hz: 10.0
+    max_orientation_speed: 0.5
 
-    workspace_min: [-0.70, -0.60, 0.10]
-    workspace_max: [0.70, 0.40, 0.70]
-    cyl_radius_limit: [0.20, 0.60]
-    low_z_threshold: 0.20
-    low_z_min_radius: 0.21
+    # QP 辅助任务：严格六维 TCP 主任务保持最高权重。
+    qp_j3_reference_deg: 67.96
+    qp_j3_weight: 0.00001
+    qp_j4_min_deg: 10.0
+    qp_j4_warn_deg: 25.0
+    qp_j4_weight: 0.0001
+    qp_manipulability_sigma_stop: 0.01
+    qp_manipulability_sigma_warn: 0.04
+    qp_manipulability_weight: 0.0001
+    workspace_min: [-0.70, -0.70, 0.10]
+    workspace_max: [0.70, 0.10, 0.75]
+    cyl_radius_limit: [0.10, 0.80]
+    low_z_threshold: 0.1
+    low_z_min_radius: 0.1
 
     # 映射关系：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
     xr_to_robot_matrix: [0.0, -1.0, 0.0,
                          0.0, 0.0, 1.0,
                          -1.0, 0.0, 0.0]
 
-    use_mock: false
-    mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
     robot_ip: 192.168.192.18
     robot_port: 8080
-    avoid_singularity: 0
-    frame_type: 1
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8089
+    realtime_push_cycle_ms: 5
+    avoid_singularity: 1
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
     configure_safety_limits: true
-    max_line_speed: 1.0
-    max_angular_speed: 1.5
-    max_line_acc: 1.0
-    max_angular_acc: 2.0
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
+    max_line_speed: 0.25
+    max_angular_speed: 0.6
+    max_line_acc: 1.3
+    max_angular_acc: 3.0
     joint_max_speed: 180.0
-    joint_max_acc: 180.0
-    move_to_initial_pose_on_connect: true
-    initial_joint_pose: [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52]
+    joint_max_acc: 300.0
+    move_to_initial_pose_on_connect: false
+    initial_joint_pose: [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55]
     init_move_speed: 20
     debug_topic_prefix: /xr_rm

xr_rm_bringup/config/peripherals_rm75.yaml

@@ -9,14 +9,14 @@ 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:
-    pose: [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
+    pose: [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
     load: [0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]
   minisci:
-    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]
     load: [0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]
   no_tool:
     pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
@@ -24,6 +24,6 @@ tools_in_ee:
 
 arms:
   left:
-    scissorgripper: 2
+    scissorgripper: 0
   right:
     scissorgripper: 1

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -1,4 +1,4 @@
-# 右臂单独调试配置：XR 相对位姿透传控制 RM75 TCP。
+# 右臂单独调试配置：XR TCP 目标经 Placo QP 转换为 RM75 关节目标。
 # 末端外设由 peripherals_rm75.yaml 配置，真机连接阶段初始化后由遥操作节点复用。
 
 single_arm_velocity_teleop:
@@ -7,6 +7,7 @@ single_arm_velocity_teleop:
     controller_topic: /xr/right_controller
     control_rate_hz: 90.0
     command_timeout_sec: 0.12
+    feedback_resync_timeout_sec: 0.5
 
     scale: 0.7
     deadband_m: 0.001
@@ -20,11 +21,19 @@ single_arm_velocity_teleop:
     orientation_deadband_rad: 0.005
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
-    current_pose_poll_hz: 10.0
 
-    workspace_min: [-0.60, -0.60, 0.10]
-    workspace_max: [0.60, 0.70, 0.55]
-    cyl_radius_limit: [0.10, 0.70]
+    # QP 辅助任务：严格六维 TCP 主任务保持最高权重。
+    qp_j3_reference_deg: -89.57
+    qp_j3_weight: 0.0001
+    qp_j4_min_deg: 10.0
+    qp_j4_warn_deg: 25.0
+    qp_j4_weight: 0.0001
+    qp_manipulability_sigma_stop: 0.01
+    qp_manipulability_sigma_warn: 0.04
+    qp_manipulability_weight: 0.0001
+    workspace_min: [-0.70, -0.70, 0.10]
+    workspace_max: [0.70, 0.10, 0.75]
+    cyl_radius_limit: [0.10, 0.80]
     low_z_threshold: 0.1
     low_z_min_radius: 0.1
 
@@ -33,23 +42,28 @@ single_arm_velocity_teleop:
                          0.0, 0.0, 1.0,
                          1.0, 0.0, 0.0]
 
-    use_mock: false
-    mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
     robot_ip: 192.168.192.19
     robot_port: 8080
+    realtime_push_host_ip: 192.168.192.148
+    realtime_push_port: 8090
+    realtime_push_cycle_ms: 5
     avoid_singularity: 1
-    frame_type: 1
+    # 厂商 MovejCANFD 示例默认低跟随；高跟随仅在验证规划轨迹后单独开启。
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
     configure_safety_limits: true
-    max_line_speed: 1.0
-    max_angular_speed: 1.5
-    max_line_acc: 1.0
-    max_angular_acc: 2.0
+    enable_tool_control: true
+    enable_trigger_gripper_control: true
+    trigger_close_threshold: 0.95
+    configure_peripheral_on_connect: true
+    max_line_speed: 0.25
+    max_angular_speed: 0.6
+    max_line_acc: 1.3
+    max_angular_acc: 3.0
     joint_max_speed: 180.0
-    joint_max_acc: 180.0
-    move_to_initial_pose_on_connect: true
-    initial_joint_pose: [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04]
+    joint_max_acc: 300.0
+    move_to_initial_pose_on_connect: false
+    initial_joint_pose: [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35]
     init_move_speed: 20
     debug_topic_prefix: /xr_rm

xr_rm_bringup/launch/arm_debug.launch.py

@@ -5,13 +5,18 @@
 手柄接收节点，再根据 `arm:=left|right|both` 选择对应的遥操作节点。
 """
 
+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
 
 
+XR_PYTHON = "/home/robot/miniconda3/envs/xr/bin/python"
+
+
 def _as_bool(value: str) -> bool:
     """把 launch 字符串参数转换成 Python bool，便于在 OpaqueFunction 中分支。"""
     return value.strip().lower() in ("1", "true", "yes", "on")
@@ -26,8 +31,13 @@ def _config_file(name: str) -> PathJoinSubstitution:
     ])
 
 
-def _initial_pose_override(value: str) -> dict[str, bool]:
-    return {} if value == "auto" else {"move_to_initial_pose_on_connect": _as_bool(value)}
+def _dual_rm75_urdf() -> PathJoinSubstitution:
+    return PathJoinSubstitution([
+        FindPackageShare("xr_rm_teleop"),
+        "models",
+        "dual_rm75",
+        "Dual_arm.urdf",
+    ])
 
 
 def _udp_receiver_node() -> Node:
@@ -37,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"),
@@ -47,45 +60,44 @@ 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,
-    move_to_initial_pose: str,
-    avoid_singularity: int,
-    frame_type: int,
-    control_rate_hz: float,
-    follow: bool,
-    configure_safety_limits: bool,
-    enable_tool_control: bool,
-    enable_trigger_gripper_control: bool,
-    trigger_close_threshold: float,
-    configure_peripheral_on_connect: bool,
 ) -> Node:
     """创建单臂调试节点；左/右臂分别使用独立 YAML，节点名保持单臂默认名。"""
     config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml"
-    robot_ip = LaunchConfiguration("left_robot_ip" if arm == "left" else "right_robot_ip")
     arm_name = _arm_name(arm)
     return Node(
         package="xr_rm_teleop",
         executable="single_arm_velocity_teleop",
         name="single_arm_velocity_teleop",
         output="screen",
+        prefix=[XR_PYTHON],
         parameters=[
             _config_file(config_name),
             {
                 "use_mock": use_mock,
-                "robot_ip": robot_ip,
-                "robot_port": LaunchConfiguration("robot_port"),
-                "avoid_singularity": avoid_singularity,
-                "frame_type": frame_type,
-                "control_rate_hz": control_rate_hz,
-                "follow": follow,
-                "configure_safety_limits": configure_safety_limits,
-                **_initial_pose_override(move_to_initial_pose),
-                "enable_tool_control": enable_tool_control,
-                "enable_trigger_gripper_control": enable_trigger_gripper_control,
-                "trigger_close_threshold": trigger_close_threshold,
-                "configure_peripheral_on_connect": configure_peripheral_on_connect,
+                "robot_urdf_path": _dual_rm75_urdf(),
                 "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                 "peripheral_arm": arm,
                 "tool_command_topic": f"/xr_rm/{arm_name}/tool_enable",
@@ -98,20 +110,7 @@ def _arm_name(arm: str) -> str:
     return "left_rm75" if arm == "left" else "right_rm75"
 
 
-def _dual_arm_nodes(
-    use_mock: bool,
-    move_to_initial_pose: str,
-    left_avoid_singularity: int,
-    right_avoid_singularity: int,
-    frame_type: int,
-    control_rate_hz: float,
-    follow: bool,
-    configure_safety_limits: bool,
-    enable_tool_control: bool,
-    enable_trigger_gripper_control: bool,
-    trigger_close_threshold: float,
-    configure_peripheral_on_connect: bool,
-) -> list[Node]:
+def _dual_arm_nodes(use_mock: bool) -> list[Node]:
     """创建双臂节点；两个节点共用双臂 YAML，但节点名区分左右臂参数命名空间。"""
     config_file = _config_file("dual_arm_rm75.yaml")
     return [
@@ -120,22 +119,12 @@ def _dual_arm_nodes(
             executable="single_arm_velocity_teleop",
             name="left_arm_teleop",
             output="screen",
+            prefix=[XR_PYTHON],
             parameters=[
                 config_file,
                 {
                     "use_mock": use_mock,
-                    "robot_ip": LaunchConfiguration("left_robot_ip"),
-                    "robot_port": LaunchConfiguration("robot_port"),
-                    "avoid_singularity": left_avoid_singularity,
-                    "frame_type": frame_type,
-                    "control_rate_hz": control_rate_hz,
-                    "follow": follow,
-                    "configure_safety_limits": configure_safety_limits,
-                    **_initial_pose_override(move_to_initial_pose),
-                    "enable_tool_control": enable_tool_control,
-                    "enable_trigger_gripper_control": enable_trigger_gripper_control,
-                    "trigger_close_threshold": trigger_close_threshold,
-                    "configure_peripheral_on_connect": configure_peripheral_on_connect,
+                    "robot_urdf_path": _dual_rm75_urdf(),
                     "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                     "peripheral_arm": "left",
                     "tool_command_topic": "/xr_rm/left_rm75/tool_enable",
@@ -147,22 +136,12 @@ def _dual_arm_nodes(
             executable="single_arm_velocity_teleop",
             name="right_arm_teleop",
             output="screen",
+            prefix=[XR_PYTHON],
             parameters=[
                 config_file,
                 {
                     "use_mock": use_mock,
-                    "robot_ip": LaunchConfiguration("right_robot_ip"),
-                    "robot_port": LaunchConfiguration("robot_port"),
-                    "avoid_singularity": right_avoid_singularity,
-                    "frame_type": frame_type,
-                    "control_rate_hz": control_rate_hz,
-                    "follow": follow,
-                    "configure_safety_limits": configure_safety_limits,
-                    **_initial_pose_override(move_to_initial_pose),
-                    "enable_tool_control": enable_tool_control,
-                    "enable_trigger_gripper_control": enable_trigger_gripper_control,
-                    "trigger_close_threshold": trigger_close_threshold,
-                    "configure_peripheral_on_connect": configure_peripheral_on_connect,
+                    "robot_urdf_path": _dual_rm75_urdf(),
                     "peripheral_config_file": _config_file("peripherals_rm75.yaml"),
                     "peripheral_arm": "right",
                     "tool_command_topic": "/xr_rm/right_rm75/tool_enable",
@@ -175,76 +154,28 @@ def _dual_arm_nodes(
 def _launch_setup(context, *args, **kwargs):
     """运行时读取 launch 参数，决定启动单臂还是双臂。"""
     del args, kwargs
+    if not Path(XR_PYTHON).is_file():
+        raise RuntimeError(
+            f"XR Python not found: {XR_PYTHON}; "
+            "Placo 0.9.4 must not be installed globally"
+        )
     arm = LaunchConfiguration("arm").perform(context).strip().lower()
     use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
-    move_to_initial_pose = LaunchConfiguration(
-        "move_to_initial_pose_on_connect"
-    ).perform(context).strip().lower()
-    avoid_override = LaunchConfiguration("avoid_singularity").perform(context).strip()
-    left_avoid_singularity = int(
-        avoid_override or LaunchConfiguration("left_avoid_singularity").perform(context)
-    )
-    right_avoid_singularity = int(
-        avoid_override or LaunchConfiguration("right_avoid_singularity").perform(context)
-    )
-    frame_type = int(LaunchConfiguration("frame_type").perform(context))
-    control_rate_hz = float(LaunchConfiguration("control_rate_hz").perform(context))
-    follow = _as_bool(LaunchConfiguration("follow").perform(context))
-    configure_safety_limits = _as_bool(
-        LaunchConfiguration("configure_safety_limits").perform(context)
-    )
-    configure_peripheral_on_connect = _as_bool(
-        LaunchConfiguration("configure_peripheral_on_connect").perform(context)
-    )
-    enable_tool_control = _as_bool(
-        LaunchConfiguration("enable_tool_control").perform(context)
-    )
-    enable_trigger_gripper_control = _as_bool(
-        LaunchConfiguration("enable_trigger_gripper_control").perform(context)
-    )
-    trigger_close_threshold = float(
-        LaunchConfiguration("trigger_close_threshold").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,
-                move_to_initial_pose,
-                left_avoid_singularity,
-                right_avoid_singularity,
-                frame_type,
-                control_rate_hz,
-                follow,
-                configure_safety_limits,
-                enable_tool_control,
-                enable_trigger_gripper_control,
-                trigger_close_threshold,
-                configure_peripheral_on_connect,
-            )
-        )
+        nodes.extend(_dual_arm_nodes(use_mock))
     else:
-        avoid_singularity = left_avoid_singularity if arm == "left" else right_avoid_singularity
-        nodes.append(
-            _single_arm_node(
-                arm,
-                use_mock,
-                move_to_initial_pose,
-                avoid_singularity,
-                frame_type,
-                control_rate_hz,
-                follow,
-                configure_safety_limits,
-                enable_tool_control,
-                enable_trigger_gripper_control,
-                trigger_close_threshold,
-                configure_peripheral_on_connect,
-            )
-        )
+        nodes.append(_single_arm_node(arm, use_mock))
+    if use_mujoco:
+        nodes.append(_mujoco_node())
     return nodes
 
 
@@ -254,35 +185,13 @@ 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"),
         # UDP receiver 轮询频率高于 PICO 发送频率，减少 socket 中等待时间。
         DeclareLaunchArgument("udp_timer_hz", default_value="200.0"),
-        # 左右 RM75 默认 IP，可在命令行中按现场网络覆盖。
-        DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"),
-        DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"),
-        DeclareLaunchArgument("robot_port", default_value="8080"),
-        # 真机位姿透传与安全配置参数。
-        DeclareLaunchArgument("left_avoid_singularity", default_value="0"),
-        DeclareLaunchArgument("right_avoid_singularity", default_value="1"),
-        # 非空时作为左右臂全局覆盖，例如 avoid_singularity:=0。
-        DeclareLaunchArgument("avoid_singularity", default_value=""),
-        DeclareLaunchArgument("frame_type", default_value="1"),
-        # 现场调参入口：默认按 PICO 90Hz 输入节奏发送 rm_movep_canfd。
-        DeclareLaunchArgument("control_rate_hz", default_value="90.0"),
-        # 默认低跟随；高跟随请确认控制器和网络能稳定满足厂商周期要求后再打开。
-        DeclareLaunchArgument("follow", default_value="false"),
-        DeclareLaunchArgument("configure_safety_limits", default_value="true"),
-        # 工具控制通过遥操作节点复用同一个 RealMan 连接，避免两个进程抢同一机械臂连接。
-        DeclareLaunchArgument("enable_tool_control", default_value="true"),
-        # trigger 上升沿切换夹爪开/关；grip 仍只控制机械臂运动。
-        DeclareLaunchArgument("enable_trigger_gripper_control", default_value="true"),
-        DeclareLaunchArgument("trigger_close_threshold", default_value="0.95"),
-        # 连接成功后是否配置外设；关闭后仅订阅开合话题，但开合前需要另行完成外设配置。
-        DeclareLaunchArgument("configure_peripheral_on_connect", default_value="true"),
-        # auto 时由单/双臂 YAML 决定；也可显式传 true/false 覆盖。
-        DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="auto"),
         # OpaqueFunction 允许根据 arm/use_mock 等运行时参数动态生成节点。
         OpaqueFunction(function=_launch_setup),
     ])

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

@@ -0,0 +1,216 @@
+import importlib.util
+import signal
+import subprocess
+import unittest
+from pathlib import Path
+from unittest import mock
+
+
+MODULE_PATH = Path(__file__).parents[1] / "tools" / "launcher_ui.py"
+SPEC = importlib.util.spec_from_file_location("launcher_ui", MODULE_PATH)
+launcher_ui = importlib.util.module_from_spec(SPEC)
+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(
+            launcher_ui._xrobotoolkit_cleanup_patterns(stop_pc_service=False)
+        )
+        window_close_patterns = set(
+            launcher_ui._xrobotoolkit_cleanup_patterns(stop_pc_service=True)
+        )
+
+        self.assertLessEqual(
+            {"RobotLinuxDemo.x86_64", "PXREAClientUnity"},
+            stop_all_patterns,
+        )
+        self.assertNotIn("RoboticsServiceProcess", stop_all_patterns)
+        self.assertIn("RoboticsServiceProcess", window_close_patterns)
+
+    def test_close_and_stop_all_select_different_pc_service_policies(self) -> None:
+        app = object.__new__(launcher_ui.LauncherApp)
+        calls = []
+
+        class Root:
+            destroyed = False
+
+            def destroy(self) -> None:
+                self.destroyed = True
+
+        app.root = Root()
+        app.stop_launched_processes = lambda **kwargs: calls.append(kwargs) or True
+
+        app.kill_launched_processes()
+        app.on_close_requested()
+
+        self.assertEqual(
+            calls,
+            [
+                {"confirm": True, "notify": True, "stop_pc_service": False},
+                {"confirm": True, "notify": False, "stop_pc_service": True},
+            ],
+        )
+        self.assertTrue(app.root.destroyed)
+
+    def test_stop_all_keeps_oldest_pc_service_and_stops_duplicates(self) -> None:
+        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", "-o", "-f", "RoboticsServiceProcess"]:
+                return "101\n"
+            if command == ["pgrep", "-f", "RoboticsServiceProcess"]:
+                return "101\n202\n303\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=False,
+            )
+
+        self.assertEqual(
+            kill.call_args_list,
+            [
+                mock.call(202, signal.SIGTERM),
+                mock.call(303, signal.SIGTERM),
+            ],
+        )
+
+    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
-launch、sample_udp_sender、topic 监控和环境检查命令，降低现场调试时的命令输入成本。
+提供 Tkinter 图形界面，按“仿真/MuJoCo/真机/诊断”组织常用 ROS2 launch、
+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",
-    "Right Arm",
-    "Dual Arm",
+    "MuJoCo",
+    "Real Hardware",
     "Diagnostics",
 ]
 
@@ -121,6 +113,17 @@ def _xrobotoolkit_bridge_command() -> str:
     )
 
 
+def _xrobotoolkit_cleanup_patterns(*, stop_pc_service: bool) -> tuple[str, ...]:
+    patterns = (
+        XROBOTOOLKIT_BRIDGE_EXECUTABLE,
+        "RobotLinuxDemo.x86_64",
+        "PXREAClientUnity",
+    )
+    if stop_pc_service:
+        return (*patterns, "RoboticsServiceProcess")
+    return patterns
+
+
 def _tool_command(arm: str, open_tool: bool) -> str:
     arm_name = "left_rm75" if arm == "left" else "right_rm75"
     value = "true" if open_tool else "false"
@@ -166,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),
@@ -191,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,
     )
@@ -221,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],
@@ -238,16 +213,10 @@ def _topic_monitor_spec(action: str) -> tuple[str, list[tuple[str, str]], str, s
     )
 
 
-def _finalize_items(
-    items: list[tuple[str, str]],
-    one_click: tuple[str, str] | None = None,
-) -> list[tuple[str, str]]:
+def _finalize_items(items: list[tuple[str, str]]) -> 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)
 
 
@@ -255,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")),
-            ("One-Click Dual Mock Demo", _one_click_mock("both", "both")),
+            (
+                "Open Controller Hz Monitor",
+                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,
@@ -283,68 +291,7 @@ def build_commands_by_mode(mode: str) -> list[tuple[str, str]]:
                 CONTROLLER_HZ_MONITOR_ACTION,
             ),
         ]
-        one_click = None
-    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 "
-                f"left_robot_ip:={DEFAULT_LEFT_IP}",
-            ),
-            ("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 "
-                f"right_robot_ip:={DEFAULT_RIGHT_IP}",
-            ),
-            ("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 "
-                f"left_robot_ip:={DEFAULT_LEFT_IP} right_robot_ip:={DEFAULT_RIGHT_IP} "
-                "move_to_initial_pose_on_connect:=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)
+    return _finalize_items(items)
 
 
 class LauncherApp:
@@ -601,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:
@@ -1247,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,
@@ -1283,13 +1229,27 @@ class LauncherApp:
         return closed
 
     def on_close_requested(self) -> None:
-        if self.stop_launched_processes(confirm=True, notify=False):
+        if self.stop_launched_processes(
+            confirm=True,
+            notify=False,
+            stop_pc_service=True,
+        ):
             self.root.destroy()
 
     def kill_launched_processes(self) -> None:
-        self.stop_launched_processes(confirm=True, notify=True)
+        self.stop_launched_processes(
+            confirm=True,
+            notify=True,
+            stop_pc_service=False,
+        )
 
-    def stop_launched_processes(self, *, confirm: bool, notify: bool) -> bool:
+    def stop_launched_processes(
+        self,
+        *,
+        confirm: bool,
+        notify: bool,
+        stop_pc_service: bool,
+    ) -> bool:
         if confirm and not messagebox.askyesno(
             "Confirm Stop",
             "Stop XR-RM launcher terminals, topic monitors, ROS nodes, and bridge processes started from this workspace?",
@@ -1302,30 +1262,41 @@ class LauncherApp:
             "ros2 run xr_rm_input",
             "ros2 run xr_rm_teleop",
             "XR_RM_LAUNCHER_SESSION=1",
-            XROBOTOOLKIT_BRIDGE_EXECUTABLE,
+            *_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",
         ]
+        pc_service_keep_pid: int | None = None
+        if not stop_pc_service:
+            try:
+                output = subprocess.check_output(
+                    ["pgrep", "-o", "-f", "RoboticsServiceProcess"],
+                    text=True,
+                )
+                pc_service_keep_pid = int(output.strip())
+                patterns.append("RoboticsServiceProcess")
+            except (subprocess.CalledProcessError, ValueError):
+                pass
+            except Exception as exc:
+                print(f"Failed to find the oldest RoboticsServiceProcess: {exc}")
+
         protected = {os.getpid(), os.getppid()}
         killed: set[int] = set()
 
@@ -1345,6 +1316,8 @@ class LauncherApp:
                     continue
                 if pid in protected or pid in killed:
                     continue
+                if pattern == "RoboticsServiceProcess" and pid == pc_service_keep_pid:
+                    continue
                 try:
                     os.kill(pid, signal.SIGTERM)
                     killed.add(pid)

xr_rm_input/test/test_controller_fields.py

@@ -0,0 +1,122 @@
+import math
+from types import SimpleNamespace
+
+from builtin_interfaces.msg import Time
+from xr_rm_input.udp_controller_receiver import UdpControllerReceiver
+from xr_rm_input.xrobotoolkit_to_udp_bridge import (
+    _buttons_payload,
+    _controller_payload,
+    _stop_controller_payload,
+)
+
+
+def _receiver_without_socket() -> UdpControllerReceiver:
+    receiver = object.__new__(UdpControllerReceiver)
+    receiver._quat_order = "xyzw"
+    receiver.get_clock = lambda: SimpleNamespace(
+        now=lambda: SimpleNamespace(to_msg=lambda: Time())
+    )
+    return receiver
+
+
+def test_bridge_payload_contains_only_selected_controller_inputs() -> None:
+    buttons = _buttons_payload(
+        primary=lambda: True,
+        secondary=lambda: False,
+    )
+    payload = _controller_payload(
+        hand="left",
+        pose=[1.0, 2.0, 3.0, 0.0, 0.0, 0.0, 1.0],
+        axis=[2.0, -2.0],
+        buttons=buttons,
+        grip_pressed=True,
+        trigger_pressed=False,
+    )
+
+    assert payload == {
+        "hand": "left",
+        "grip": True,
+        "trigger": 0.0,
+        "pos": [1.0, 2.0, 3.0],
+        "quat": [0.0, 0.0, 0.0, 1.0],
+        "pose_valid": True,
+        "pose_source": "xrobotoolkit",
+        "axis": [1.0, -1.0],
+        "buttons": {
+            "primary": True,
+            "secondary": False,
+        },
+    }
+
+
+def test_stop_payload_uses_neutral_selected_inputs() -> None:
+    payload = _stop_controller_payload("right")
+
+    assert payload["axis"] == [0.0, 0.0]
+    assert payload["buttons"] == {
+        "primary": False,
+        "secondary": False,
+    }
+    assert "grip_value" not in payload
+    assert "trigger_value" not in payload
+
+
+def test_receiver_publishes_selected_controller_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 1.0,
+            "axis": [2.0, -2.0],
+            "buttons": {
+                "primary": True,
+                "secondary": False,
+            },
+            "pos": [1.0, 2.0, 3.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "left",
+    )
+
+    assert msg.primary is True
+    assert msg.secondary is False
+    assert list(msg.axis) == [1.0, -1.0]
+
+
+def test_receiver_defaults_invalid_optional_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 0.4,
+            "axis": [math.nan, 0.0],
+            "buttons": [],
+            "pos": [1.0, 2.0, 3.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "right",
+    )
+
+    assert msg.primary is False
+    assert msg.secondary is False
+    assert list(msg.axis) == [0.0, 0.0]
+    assert msg.grip is True
+    assert abs(msg.trigger - 0.4) < 1e-6
+    assert msg.pose.position.x == 1.0
+    assert msg.pose.position.y == 2.0
+    assert msg.pose.position.z == 3.0
+
+
+def test_receiver_defaults_missing_legacy_optional_inputs() -> None:
+    msg = _receiver_without_socket()._payload_to_msg(
+        {
+            "grip": True,
+            "trigger": 0.0,
+            "pos": [0.0, 1.0, 0.0],
+            "quat": [0.0, 0.0, 0.0, 1.0],
+        },
+        "left",
+    )
+
+    assert msg.primary is False
+    assert msg.secondary is False
+    assert list(msg.axis) == [0.0, 0.0]
+    assert msg.grip is True

xr_rm_input/xr_rm_input/udp_controller_receiver.py

@@ -1,10 +1,11 @@
 """XR 手柄 UDP 接收节点。
 
-从 UDP JSON 数据包中解析左右手柄位姿、握持键和扳机值，并发布为
-`xr_rm_interfaces/XrController` 消息，供遥操作和夹爪节点订阅。
+从 UDP JSON 数据包中解析左右手柄位姿、Grip、Trigger、摇杆和主副按键，
+并发布为 `xr_rm_interfaces/XrController` 消息，供遥操作和夹爪节点订阅。
 """
 
 import json
+import math
 import socket
 from collections.abc import Iterable, Mapping
 from typing import Any
@@ -124,6 +125,8 @@ class UdpControllerReceiver(Node):
         pos, quat = self._extract_pose(payload)
         if len(pos) != 3 or len(quat) != 4:
             raise ValueError("expected pos[3] and quat[4]")
+        axis = self._optional_axis(payload.get("axis"))
+        primary, secondary = self._optional_buttons(payload.get("buttons"))
 
         msg = XrController()
         msg.header.stamp = self.get_clock().now().to_msg()
@@ -144,6 +147,9 @@ class UdpControllerReceiver(Node):
 
         msg.grip = grip
         msg.trigger = self._clamp_float(payload.get("trigger", 0.0), 0.0, 1.0)
+        msg.primary = primary
+        msg.secondary = secondary
+        msg.axis = axis
         msg.pose.position.x = float(pos[0])
         msg.pose.position.y = float(pos[1])
         msg.pose.position.z = float(pos[2])
@@ -213,6 +219,28 @@ class UdpControllerReceiver(Node):
             raise ValueError("expected 3D position")
         return [float(item) for item in vector]
 
+    @staticmethod
+    def _optional_axis(value: Any) -> list[float]:
+        try:
+            axis = [float(item) for item in value]
+        except (TypeError, ValueError):
+            return [0.0, 0.0]
+        if len(axis) != 2 or not all(math.isfinite(item) for item in axis):
+            return [0.0, 0.0]
+        return [
+            min(max(axis[0], -1.0), 1.0),
+            min(max(axis[1], -1.0), 1.0),
+        ]
+
+    @classmethod
+    def _optional_buttons(cls, value: Any) -> tuple[bool, bool]:
+        if not isinstance(value, Mapping):
+            return False, False
+        return (
+            cls._as_bool(value.get("primary", False)),
+            cls._as_bool(value.get("secondary", False)),
+        )
+
     def _quaternion(self, value: Any) -> list[float]:
         if isinstance(value, Mapping):
             if self._quat_order == "wxyz":

xr_rm_input/xr_rm_input/xrobotoolkit_to_udp_bridge.py

@@ -1,8 +1,8 @@
 """XRoboToolkit SDK 到当前 UDP controller JSON 协议的桥接脚本。
 
 该脚本运行在安装了 `xrobotoolkit_sdk` 的 Python 环境中，从官方
-XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 pose / grip / trigger，
-再发送现有 `udp_controller_receiver` 已兼容的 UDP JSON 包。
+XRoboToolkit PC-Service SDK 读取 PICO 左右手柄 pose、Grip、Trigger、
+摇杆和主副按键，再发送 `udp_controller_receiver` 兼容的 UDP JSON 包。
 """
 
 import argparse
@@ -94,8 +94,6 @@ def _controller_payload(
     *,
     hand: str,
     pose: Any,
-    grip_value: Any,
-    trigger_value: Any,
     axis: Any,
     buttons: dict[str, bool],
     grip_pressed: bool,
@@ -103,8 +101,6 @@ def _controller_payload(
     pose_valid: bool = True,
 ) -> dict[str, Any]:
     pos, quat = _pose_to_pos_quat(pose) if pose_valid else (ZERO_POS.copy(), IDENTITY_QUAT.copy())
-    grip_float = _clamp_float(grip_value, 0.0, 1.0)
-    trigger_float = _clamp_float(trigger_value, 0.0, 1.0)
     return {
         "hand": hand,
         "grip": pose_valid and grip_pressed,
@@ -113,8 +109,6 @@ def _controller_payload(
         "quat": quat,
         "pose_valid": pose_valid,
         "pose_source": POSE_SOURCE,
-        "grip_value": grip_float,
-        "trigger_value": trigger_float,
         "axis": _safe_axis(axis),
         "buttons": buttons,
     }
@@ -129,15 +123,10 @@ def _stop_controller_payload(hand: str) -> dict[str, Any]:
         "quat": IDENTITY_QUAT.copy(),
         "pose_valid": False,
         "pose_source": POSE_SOURCE,
-        "grip_value": 0.0,
-        "trigger_value": 0.0,
         "axis": [0.0, 0.0],
         "buttons": {
-            "grip": False,
             "primary": False,
             "secondary": False,
-            "menu": False,
-            "axis_click": False,
         },
     }
 
@@ -181,18 +170,12 @@ def _send_stop_packets(
 
 def _buttons_payload(
     *,
-    grip: bool,
     primary: Callable[[], Any],
     secondary: Callable[[], Any],
-    menu: Callable[[], Any],
-    axis_click: Callable[[], Any],
 ) -> dict[str, bool]:
     return {
-        "grip": grip,
         "primary": _safe_bool(primary),
         "secondary": _safe_bool(secondary),
-        "menu": _safe_bool(menu),
-        "axis_click": _safe_bool(axis_click),
     }
 
 
@@ -321,15 +304,10 @@ def main(argv: Sequence[str] | None = None) -> None:
                     "left": _controller_payload(
                         hand="left",
                         pose=xrt.get_left_controller_pose(),
-                        grip_value=left_grip_value,
-                        trigger_value=left_trigger_value,
                         axis=xrt.get_left_axis(),
                         buttons=_buttons_payload(
-                            grip=left_grip,
                             primary=xrt.get_X_button,
                             secondary=xrt.get_Y_button,
-                            menu=xrt.get_left_menu_button,
-                            axis_click=xrt.get_left_axis_click,
                         ),
                         grip_pressed=left_grip,
                         trigger_pressed=left_trigger,
@@ -337,15 +315,10 @@ def main(argv: Sequence[str] | None = None) -> None:
                     "right": _controller_payload(
                         hand="right",
                         pose=xrt.get_right_controller_pose(),
-                        grip_value=right_grip_value,
-                        trigger_value=right_trigger_value,
                         axis=xrt.get_right_axis(),
                         buttons=_buttons_payload(
-                            grip=right_grip,
                             primary=xrt.get_A_button,
                             secondary=xrt.get_B_button,
-                            menu=xrt.get_right_menu_button,
-                            axis_click=xrt.get_right_axis_click,
                         ),
                         grip_pressed=right_grip,
                         trigger_pressed=right_trigger,

xr_rm_interfaces/msg/XrController.msg

@@ -1,5 +1,10 @@
 std_msgs/Header header
 string hand
+
 bool grip
 float32 trigger
+bool primary
+bool secondary
+float32[2] axis
+
 geometry_msgs/Pose pose

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

@@ -0,0 +1,555 @@
+<?xml version='1.0' encoding='utf-8'?>
+<robot name="rm75_dual_arm">
+  <!--Shared supporting base. Adjust the two mount joint origins to match the CAD mounting frames.-->
+  <link name="dual_arm_base_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/dual_arm_base.stl" scale="1 1 1" />
+      </geometry>
+      <material name="dual_arm_base_material">
+        <color rgba="0.5 0.5 0.5 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/dual_arm_base.stl" scale="1 1 1" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="omnipic_base_link">
+    <inertial>
+      <origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0" />
+      <mass value="1.862" />
+      <inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/base_link.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="omnipic_link_1">
+    <inertial>
+      <origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0" />
+      <mass value="1.574" />
+      <inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_1.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_1" type="revolute">
+    <origin xyz="0 0 0.2405" rpy="0 0 0" />
+    <parent link="omnipic_base_link" />
+    <child link="omnipic_link_1" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="60" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_2">
+    <inertial>
+      <origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0" />
+      <mass value="1.217" />
+      <inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_2.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_2" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="omnipic_link_1" />
+    <child link="omnipic_link_2" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_3">
+    <inertial>
+      <origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0" />
+      <mass value="1.11" />
+      <inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_3.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_3" type="revolute">
+    <origin xyz="0 -0.256 0" rpy="1.5708 0 0" />
+    <parent link="omnipic_link_2" />
+    <child link="omnipic_link_3" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="30" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_4">
+    <inertial>
+      <origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0" />
+      <mass value="0.685" />
+      <inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_4.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_4" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="omnipic_link_3" />
+    <child link="omnipic_link_4" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.356" upper="2.356" effort="30" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_5">
+    <inertial>
+      <origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0" />
+      <mass value="0.619" />
+      <inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_5.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_5" type="revolute">
+    <origin xyz="0 -0.21 0" rpy="1.5708 0 0" />
+    <parent link="omnipic_link_4" />
+    <child link="omnipic_link_5" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="10" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_6">
+    <inertial>
+      <origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0" />
+      <mass value="0.602" />
+      <inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_6.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_6" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="omnipic_link_5" />
+    <child link="omnipic_link_6" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.234" upper="2.234" effort="10" velocity="3.14" />
+  </joint>
+  <link name="omnipic_link_7">
+    <inertial>
+      <origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0" />
+      <mass value="0.107" />
+      <inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_7.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_joint_7" type="revolute">
+    <origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
+    <parent link="omnipic_link_6" />
+    <child link="omnipic_link_7" />
+    <axis xyz="0 0 1" />
+    <limit lower="-6.28" upper="6.28" effort="10" velocity="3.14" />
+  </joint>
+  <link name="omnipic_gripper_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0.1" />
+      <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" />
+      <geometry>
+        <mesh filename="meshes/OmniPic.stl" scale="1 1 1" />
+      </geometry>
+      <material name="omnipic_OmniPic_material">
+        <color rgba="0.7 0.7 0.7 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/OmniPic.stl" scale="1 1 1" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipic_OmniPic_fixed_joint" type="fixed">
+    <parent link="omnipic_link_7" />
+    <child link="omnipic_gripper_link" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+  <link name="omnipic_OmniPic_tcp" />
+  <joint name="omnipic_OmniPic_tcp_fixed" type="fixed">
+    <parent link="omnipic_gripper_link" />
+    <child link="omnipic_OmniPic_tcp" />
+    <origin xyz="0 0 0.14" rpy="0 0 0" />
+  </joint>
+  <!--Omnipic arm mount (physical right): edit xyz/rpy to match dual_arm_base.stl.-->
+  <joint name="omnipic_base_mount_joint" type="fixed">
+    <parent link="dual_arm_base_link" />
+    <child link="omnipic_base_link" />
+    <origin xyz="0.03 0 0" rpy="3.1416 -1.5708 0" />
+  </joint>
+  <link name="scissor_base_link">
+    <inertial>
+      <origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0" />
+      <mass value="1.862" />
+      <inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/base_link.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link name="scissor_link_1">
+    <inertial>
+      <origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0" />
+      <mass value="1.574" />
+      <inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_1.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_1" type="revolute">
+    <origin xyz="0 0 0.2405" rpy="0 0 0" />
+    <parent link="scissor_base_link" />
+    <child link="scissor_link_1" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="60" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_2">
+    <inertial>
+      <origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0" />
+      <mass value="1.217" />
+      <inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_2.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_2" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="scissor_link_1" />
+    <child link="scissor_link_2" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_3">
+    <inertial>
+      <origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0" />
+      <mass value="1.11" />
+      <inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_3.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_3" type="revolute">
+    <origin xyz="0 -0.256 0" rpy="1.5708 0 0" />
+    <parent link="scissor_link_2" />
+    <child link="scissor_link_3" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="30" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_4">
+    <inertial>
+      <origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0" />
+      <mass value="0.685" />
+      <inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_4.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_4" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="scissor_link_3" />
+    <child link="scissor_link_4" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.356" upper="2.356" effort="30" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_5">
+    <inertial>
+      <origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0" />
+      <mass value="0.619" />
+      <inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_5.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_5" type="revolute">
+    <origin xyz="0 -0.21 0" rpy="1.5708 0 0" />
+    <parent link="scissor_link_4" />
+    <child link="scissor_link_5" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.106" upper="3.106" effort="10" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_6">
+    <inertial>
+      <origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0" />
+      <mass value="0.602" />
+      <inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_6.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_6" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0" />
+    <parent link="scissor_link_5" />
+    <child link="scissor_link_6" />
+    <axis xyz="0 0 1" />
+    <limit lower="-2.234" upper="2.234" effort="10" velocity="3.14" />
+  </joint>
+  <link name="scissor_link_7">
+    <inertial>
+      <origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0" />
+      <mass value="0.107" />
+      <inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_7.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_joint_7" type="revolute">
+    <origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
+    <parent link="scissor_link_6" />
+    <child link="scissor_link_7" />
+    <axis xyz="0 0 1" />
+    <limit lower="-6.28" upper="6.28" effort="10" velocity="3.14" />
+  </joint>
+  <link name="scissor_scissor_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0.1" />
+      <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 -1.5708" />
+      <geometry>
+        <mesh filename="meshes/scissor.stl" scale="1 1 1" />
+      </geometry>
+      <material name="scissor_scissor_material">
+        <color rgba="0.7 0.7 0.7 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 -1.5708" />
+      <geometry>
+        <mesh filename="meshes/scissor.stl" scale="1 1 1" />
+      </geometry>
+    </collision>
+  </link>
+  <joint name="scissor_scissor_fixed_joint" type="fixed">
+    <parent link="scissor_link_7" />
+    <child link="scissor_scissor_link" />
+    <origin xyz="0 0 0.165" rpy="0 0 0" />
+  </joint>
+  <link name="scissor_scissor_tcp" />
+  <joint name="scissor_scissor_tcp_fixed" type="fixed">
+    <parent link="scissor_scissor_link" />
+    <child link="scissor_scissor_tcp" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
+  </joint>
+  <link name="scissor_camera_tcp" />
+  <joint name="scissor_camera_tcp_fixed" type="fixed">
+    <parent link="scissor_scissor_link" />
+    <child link="scissor_camera_tcp" />
+    <origin xyz="0.042 0 -0.0755" rpy="0 0 1.57" />
+  </joint>
+  <!--Scissor arm mount (physical left): edit xyz/rpy to match dual_arm_base.stl.-->
+  <joint name="scissor_base_mount_joint" type="fixed">
+    <parent link="dual_arm_base_link" />
+    <child link="scissor_base_link" />
+    <origin xyz="-0.030 0 0" rpy="-3.1416 1.5708 0" />
+  </joint>
+</robot>

xr_rm_teleop/models/rm75/RM75-B.urdf

@@ -0,0 +1,453 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-1-g15f4949  Build Version: 1.6.7594.29634
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+  name="RM75-B">
+  <link
+    name="base_link">
+    <inertial>
+      <origin
+        xyz="0.00049987 5.2709E-05 0.060019"
+        rpy="0 0 0" />
+      <mass
+        value="1.862" />
+      <inertia
+        ixx="0.0017232"
+        ixy="-3.1058E-06"
+        ixz="-3.7924E-05"
+        iyy="0.0017051"
+        iyz="1.3691E-06"
+        izz="0.00090158" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/base_link.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/base_link.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <link
+    name="link_1">
+    <inertial>
+      <origin
+        xyz="0.000241 -0.013273 -0.00995"
+        rpy="0 0 0" />
+      <mass
+        value="1.574" />
+      <inertia
+        ixx="0.002487573"
+        ixy="0.000009663"
+        ixz="-0.000007909"
+        iyy="0.002321038"
+        iyz="0.000179393"
+        izz="0.001450554" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_1.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_1"
+    type="revolute">
+    <origin
+      xyz="0 0 0.2405"
+      rpy="0 0 0" />
+    <parent
+      link="base_link" />
+    <child
+      link="link_1" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="60"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_2">
+    <inertial>
+      <origin
+        xyz="-0.000357 -0.106789 0.005329"
+        rpy="0 0 0" />
+      <mass
+        value="1.217" />
+      <inertia
+        ixx="0.003494121"
+        ixy="0.000002921"
+        ixz="-0.000005613"
+        iyy="0.000892721"
+        iyz="-0.000583884"
+        izz="0.003444080" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_2.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_2"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_1" />
+    <child
+      link="link_2" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.2689"
+      upper="2.2689"
+      effort="60"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_3">
+    <inertial>
+      <origin
+        xyz="0.000003 -0.01398 -0.011324"
+        rpy="0 0 0" />
+      <mass
+        value="1.11" />
+      <inertia
+        ixx="0.001836663"
+        ixy="0.000002259"
+        ixz="-0.000004216"
+        iyy="0.001498875"
+        iyz="0.000037167"
+        izz="0.001062545" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_3.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_3.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_3"
+    type="revolute">
+    <origin
+      xyz="0 -0.256 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_2" />
+    <child
+      link="link_3" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="30"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_4">
+    <inertial>
+      <origin
+        xyz="-0.000005 -0.084658 0.004747"
+        rpy="0 0 0" />
+      <mass
+        value="0.685" />
+      <inertia
+        ixx="0.001282444"
+        ixy="-0.000000551"
+        ixz="-0.000000630"
+        iyy="0.000373013"
+        iyz="-0.000232084"
+        izz="0.001256177" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_4.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_4.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_4"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_3" />
+    <child
+      link="link_4" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.356"
+      upper="2.356"
+      effort="30"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_5">
+    <inertial>
+      <origin
+        xyz="0.000078 -0.012937 -0.008781"
+        rpy="0 0 0" />
+      <mass
+        value="0.619" />
+      <inertia
+        ixx="0.000627336"
+        ixy="0.000001636"
+        ixz="-0.000001345"
+        iyy="0.000542455"
+        iyz="0.000034970"
+        izz="0.000370291" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_5.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_5.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_5"
+    type="revolute">
+    <origin
+      xyz="0 -0.21 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_4" />
+    <child
+      link="link_5" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-3.106"
+      upper="3.106"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_6">
+    <inertial>
+      <origin
+        xyz="-0.000014 -0.078524 0.002819"
+        rpy="0 0 0" />
+      <mass
+        value="0.602" />
+      <inertia
+        ixx="0.000780774"
+        ixy="-0.000000121"
+        ixz="-0.000000469"
+        iyy="0.000289973"
+        iyz="-0.000120513"
+        izz="0.000763955" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_6.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_6.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_6"
+    type="revolute">
+    <origin
+      xyz="0 0 0"
+      rpy="-1.5708 0 0" />
+    <parent
+      link="link_5" />
+    <child
+      link="link_6" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-2.234"
+      upper="2.234"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+  <link
+    name="link_7">
+    <inertial>
+      <origin
+        xyz="0.001094 -0.000077 -0.010119"
+        rpy="0 0 0" />
+      <mass
+        value="0.107" />
+      <inertia
+        ixx="0.000044123"
+        ixy="-0.000000064"
+        ixz="0.0000003"
+        iyy="0.000035078"
+        iyz="-0.000000029"
+        izz="0.000065445" />
+    </inertial>
+    <visual>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_7.STL" />
+      </geometry>
+      <material
+        name="">
+        <color
+          rgba="1 1 1 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh
+          filename="meshes/link_7.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint_7"
+    type="revolute">
+    <origin
+      xyz="0 -0.144 0"
+      rpy="1.5708 0 0" />
+    <parent
+      link="link_6" />
+    <child
+      link="link_7" />
+    <axis
+      xyz="0 0 1" />
+    <limit
+      lower="-6.28"
+      upper="6.28"
+      effort="10"
+      velocity="3.14" />
+  </joint>
+</robot>

xr_rm_teleop/models/rm75_omnipicker/urdf/RM75-B_OmniPicker_fixed.urdf

@@ -0,0 +1,503 @@
+<?xml version='1.0' encoding='UTF-8'?>
+<robot name="RM75_B_OmniPicker_fixed">
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0"/>
+      <mass value="1.862"/>
+      <inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="link_1">
+    <inertial>
+      <origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0"/>
+      <mass value="1.574"/>
+      <inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_1" type="revolute">
+    <origin xyz="0 0 0.2405" rpy="0 0 0"/>
+    <parent link="base_link"/>
+    <child link="link_1"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.106" upper="3.106" effort="60" velocity="3.14"/>
+  </joint>
+  <link name="link_2">
+    <inertial>
+      <origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0"/>
+      <mass value="1.217"/>
+      <inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_2" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0"/>
+    <parent link="link_1"/>
+    <child link="link_2"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14"/>
+  </joint>
+  <link name="link_3">
+    <inertial>
+      <origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0"/>
+      <mass value="1.11"/>
+      <inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_3" type="revolute">
+    <origin xyz="0 -0.256 0" rpy="1.5708 0 0"/>
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.106" upper="3.106" effort="30" velocity="3.14"/>
+  </joint>
+  <link name="link_4">
+    <inertial>
+      <origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0"/>
+      <mass value="0.685"/>
+      <inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_4" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0"/>
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.356" upper="2.356" effort="30" velocity="3.14"/>
+  </joint>
+  <link name="link_5">
+    <inertial>
+      <origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0"/>
+      <mass value="0.619"/>
+      <inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_5" type="revolute">
+    <origin xyz="0 -0.21 0" rpy="1.5708 0 0"/>
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.106" upper="3.106" effort="10" velocity="3.14"/>
+  </joint>
+  <link name="link_6">
+    <inertial>
+      <origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0"/>
+      <mass value="0.602"/>
+      <inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_6" type="revolute">
+    <origin xyz="0 0 0" rpy="-1.5708 0 0"/>
+    <parent link="link_5"/>
+    <child link="link_6"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-2.234" upper="2.234" effort="10" velocity="3.14"/>
+  </joint>
+  <link name="link_7">
+    <inertial>
+      <origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0"/>
+      <mass value="0.107"/>
+      <inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_7" type="revolute">
+    <origin xyz="0 -0.144 0" rpy="1.5708 0 0"/>
+    <parent link="link_6"/>
+    <child link="link_7"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-6.28" upper="6.28" effort="10" velocity="3.14"/>
+  </joint>
+  <!-- RM75 end-flange alias. link_7 is treated as the tool mounting frame. -->
+  <link name="rm75_flange"/>
+  <joint name="rm75_link7_to_flange" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="link_7"/>
+    <child link="rm75_flange"/>
+  </joint>
+  <!-- OmniPicker mounting transform. Adjust xyz/rpy here if an adapter plate or different clocking is used. -->
+  <joint name="rm75_flange_to_omnipicker" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="rm75_flange"/>
+    <child link="omnipicker_base_link"/>
+  </joint>
+  <link name="omnipicker_base_link">
+    <inertial>
+      <origin xyz="-0.00005520 1.2341E-05 0.03296193" rpy="0 0 0"/>
+      <mass value="0.25641368"/>
+      <inertia ixx="4.6351E-04" ixy="-1.0E-08" ixz="-1.04E-06" iyy="4.4525E-04" iyz="-5.00E-08" izz="1.0438E-04"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/base_link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.89804 0.91765 0.92941 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="omnipicker_hand_narrow1_Link">
+    <inertial>
+      <origin xyz="0.0094685 0.0068806 6.5437E-05" rpy="0 0 0"/>
+      <mass value="0.025428"/>
+      <inertia ixx="1.9574E-06" ixy="-4.2911E-07" ixz="-3.7111E-11" iyy="2.3919E-06" iyz="-3.008E-10" izz="1.5501E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow1_joint" type="fixed">
+    <origin xyz="0 -0.0195 0.0565" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_narrow1_Link"/>
+  </joint>
+  <link name="omnipicker_hand_narrow2_Link">
+    <inertial>
+      <origin xyz="0.0088027 -0.007035 1.6424E-05" rpy="0 0 0"/>
+      <mass value="0.0040132"/>
+      <inertia ixx="1.0307E-07" ixy="5.7851E-08" ixz="9.5801E-11" iyy="1.1385E-07" iyz="-2.81E-11" izz="1.7009E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow2_joint" type="fixed">
+    <origin xyz="0.030852 0.018551 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_narrow1_Link"/>
+    <child link="omnipicker_hand_narrow2_Link"/>
+  </joint>
+  <link name="omnipicker_hand_narrow3_Link">
+    <inertial>
+      <origin xyz="0.012508 -0.0079729 9.4339E-05" rpy="0 0 0"/>
+      <mass value="0.018029"/>
+      <inertia ixx="1.1403E-06" ixy="5.5159E-07" ixz="-4.0096E-13" iyy="2.5704E-06" iyz="1.0951E-12" izz="2.3252E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow3_joint" type="fixed">
+    <origin xyz="0.018118 -0.01574 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_narrow2_Link"/>
+    <child link="omnipicker_hand_narrow3_Link"/>
+  </joint>
+  <link name="omnipicker_hand_narrow4_Link">
+  </link>
+  <joint name="omnipicker_hand_narrow4_joint" type="fixed">
+    <origin xyz="0 -0.0104 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_narrow3_Link"/>
+    <child link="omnipicker_hand_narrow4_Link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
+  </joint>
+  <link name="omnipicker_hand_narrow_loop_Link">
+    <inertial>
+      <origin xyz="0.014869 -0.0036066 0.00029307" rpy="0 0 0"/>
+      <mass value="0.022591"/>
+      <inertia ixx="4.3916E-06" ixy="1.114E-07" ixz="-4.9655E-12" iyy="4.737E-06" iyz="1.7121E-11" izz="6.0445E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_narrow_loop_joint" type="fixed">
+    <origin xyz="0 -0.021633 0.07387" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_narrow_loop_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide1_Link">
+    <inertial>
+      <origin xyz="0.0095051 -0.0068479 6.8268E-05" rpy="0 0 0"/>
+      <mass value="0.025428"/>
+      <inertia ixx="1.9565E-06" ixy="4.2798E-07" ixz="2.3844E-10" iyy="2.3928E-06" iyz="-1.4454E-10" izz="1.5501E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide1_joint" type="fixed">
+    <origin xyz="0 0.0195 0.0565" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_wide1_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide2_Link">
+    <inertial>
+      <origin xyz="0.0088027 0.007035 -1.6424E-05" rpy="0 0 0"/>
+      <mass value="0.0040132"/>
+      <inertia ixx="1.0307E-07" ixy="-5.7851E-08" ixz="-9.58E-11" iyy="1.1385E-07" iyz="-2.81E-11" izz="1.7009E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide2_joint" type="fixed">
+    <origin xyz="0.030852 -0.018551 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_wide1_Link"/>
+    <child link="omnipicker_hand_wide2_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide3_Link">
+    <inertial>
+      <origin xyz="0.016206 0.0094593 4.7668E-05" rpy="0 0 0"/>
+      <mass value="0.035835"/>
+      <inertia ixx="8.5056E-06" ixy="-1.1363E-06" ixz="-4.2908E-11" iyy="1.1235E-05" iyz="-2.9251E-11" izz="4.6309E-06"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide3_joint" type="fixed">
+    <origin xyz="0.018118 0.01574 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_wide2_Link"/>
+    <child link="omnipicker_hand_wide3_Link"/>
+  </joint>
+  <link name="omnipicker_hand_wide4_Link">
+  </link>
+  <joint name="omnipicker_hand_wide4_joint" type="fixed">
+    <origin xyz="0 0.0104 0" rpy="0 0 0"/>
+    <parent link="omnipicker_hand_wide3_Link"/>
+    <child link="omnipicker_hand_wide4_Link"/>
+    <axis xyz="0 0 1"/>
+    <limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
+  </joint>
+  <link name="omnipicker_hand_wide_loop_Link">
+    <inertial>
+      <origin xyz="0.016268 0.0040555 0.00030323" rpy="0 0 0"/>
+      <mass value="0.025142"/>
+      <inertia ixx="5.887E-06" ixy="-1.1234E-07" ixz="2.1954E-11" iyy="6.236E-06" iyz="-9.473E-12" izz="6.2389E-07"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.75294 0.75294 0.75294 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="omnipicker_hand_wide_loop_joint" type="fixed">
+    <origin xyz="0 0.021633 0.07387" rpy="-2.9951 -1.5708 -0.15964"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_hand_wide_loop_Link"/>
+  </joint>
+  <link name="omnipicker_mount_frame"/>
+  <joint name="omnipicker_base_to_mount_frame" type="fixed">
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_mount_frame"/>
+  </joint>
+  <link name="omnipicker_tcp"/>
+  <joint name="omnipicker_tcp_joint" type="fixed">
+    <parent link="omnipicker_base_link"/>
+    <child link="omnipicker_tcp"/>
+    <origin xyz="0 0 0.16" rpy="0 0 0"/>
+  </joint>
+</robot>

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>