fix: 调整peripheral_cfg函数中的目标力矩设置值

.gitignore

@@ -44,3 +44,6 @@ AMENT_IGNORE
 *.vsix
 
 .codex
+
+# RealSense camera test snapshots
+/xr_rm_bringup/test/camera_test_output/

AGENTS.md

@@ -231,6 +231,7 @@
 除非用户明确要求，否则不要自动提交、推送或修改远程仓库。
 
 使用 Superpowers 执行任务时，只允许按相关 skill 工作流创建本地 Git 提交；
+同一项变更生成的规格文档与实施计划必须合并为一次本地提交，不得分别提交。
 禁止执行 `git push`、合并本地分支、合并 PR 或其他远程写操作。相关 skill
 如需独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
 其他情况下，除非用户明确要求，不要自动创建分支。
@@ -287,7 +288,7 @@ test: 添加 xxx 测试
 
 ## 项目专属规则
 
-* 本项目面向 Ubuntu 22.04 和 ROS2 Humble；构建、测试和运行命令应在工作空间根目录 `/home/robot/WS_xr` 执行，并先 `source /opt/ros/humble/setup.bash`。
+* 本项目面向 Ubuntu 22.04 和 ROS2 Humble；编译前必须先切换到工作空间根目录 `/home/robot/WS_xr`，并执行 `source /opt/ros/humble/setup.bash`。禁止在 `/home/robot/WS_xr/src` 中运行 `colcon build`，否则会在源码目录生成多余的 `build/`、`install/` 和 `log/`；测试和运行命令也应在工作空间根目录执行。
 * 工作空间包含 `xr_rm_input`、`xr_rm_teleop` 两个 `ament_python` 包，以及 `xr_rm_interfaces`、`xr_rm_bringup` 两个 `ament_cmake` 包；优先使用现有 ROS2 包、节点和消息，不要另建重复入口。
 * 修改 ROS 节点、launch、消息定义或安装配置后，至少运行 `colcon build --symlink-install`；涉及遥操作姿态控制时，再运行 `pytest src/xr_rm_teleop/test/test_orientation_control.py`。
 * 遥操作统一使用 `xr_rm_bringup/launch/arm_debug.launch.py`；调试和验证默认使用 `use_mock:=true`，未经用户明确要求不得连接真机、移动机械臂或操作夹爪。

README.md

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

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-13-act-camera-preview-alignment.md

@@ -0,0 +1,756 @@
+# ACT 双相机预览与采样质量判定修正实施计划
+
+> **供代理执行者使用：** 必须使用 `superpowers:subagent-driven-development`（推荐）或 `superpowers:executing-plans`，逐项执行本计划。所有步骤使用复选框（`- [ ]`）跟踪。
+
+**目标：** 保持 ACT 当前因果时间对齐，同时把真实相机丢帧与软件采样相位漂移分开判定，并在 ACT 数采启动时默认显示全局 D455 和右腕 D405 实时画面。
+
+**架构：** 继续由 `act_episode_recorder` 独占两台 RealSense，并按每三个 `90 Hz` 控制周期选择不晚于控制时刻的最新图像。`CameraBuffer` 负责真实采集质量，HDF5 校验只记录 ACT 样本重复/跨帧率；同一节点内新增一个只读 OpenCV 预览线程，复用现有帧缓冲且不进入机器人控制链路。
+
+**技术栈：** Ubuntu 22.04、ROS2 Humble、Python 3、rclpy、NumPy、h5py、pyrealsense2、OpenCV、pytest、HDF5。
+
+---
+
+## 文件结构
+
+本次不新建 ROS 包或运行进程，文件职责保持如下：
+
+- 修改 `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：真实采集统计、ACT 采样相位指标、质量判定、双路预览及关闭顺序；
+- 修改 `xr_rm_teleop/test/test_act_episode_recorder.py`：指标口径、误拒绝复现、帧号回退和预览隔离测试；
+- 修改 `README.md`：说明 ACT 数采默认预览、显示内容和关闭行为。
+
+不修改 `arm_debug.launch.py`、`launcher_ui.py`、ROS 消息、机器人控制节点和相机 YAML。工作区已有的 `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py` 未提交改动属于用户，不加入本任务提交。
+
+所有构建和测试命令从工作空间根目录执行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+```
+
+自动化测试不得启动真机 launch、移动机械臂或操作夹爪。
+
+### 任务 1：区分真实丢帧与 ACT 采样相位漂移
+
+**文件：**
+
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:408-579`
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:595-657`
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1472-1511`
+- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py:273-296`
+- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py:491-607`
+
+- [ ] **步骤 1：写异步采样不拒绝和指标测试**
+
+在测试导入中加入 `sample_frame_metrics`，并增加：
+
+```python
+def test_sample_frame_metrics_separate_repeats_skips_and_regressions():
+    repeat, skip, regression = sample_frame_metrics(
+        np.asarray((986, 986, 988), dtype=np.uint64)
+    )
+
+    assert repeat == pytest.approx(0.5)
+    assert skip == pytest.approx(0.5)
+    assert regression == 0
+
+
+@requires_h5py
+def test_validate_episode_accepts_async_camera_phase_drift(tmp_path):
+    path = _valid_episode(tmp_path)
+    with h5py.File(path, "r+") as root:
+        root["debug/cameras/cam_high_frame_number"][:] = (986, 986, 988)
+
+    report = validate_episode(path, _quality_limits())
+
+    assert report.accepted
+    assert report.metrics["cam_high_sample_repeat_ratio"] == pytest.approx(0.5)
+    assert report.metrics["cam_high_sample_skip_ratio"] == pytest.approx(0.5)
+```
+
+在 `_valid_episode()` 写入新增的真实采集属性：
+
+```python
+root.attrs["camera_high_frame_number_regression_count"] = 0
+root.attrs["camera_right_wrist_frame_number_regression_count"] = 0
+```
+
+- [ ] **步骤 2：写真实帧号回退测试**
+
+扩展现有 `CameraBuffer` 测试：
+
+```python
+def test_camera_buffer_counts_frame_number_regressions():
+    buffer = CameraBuffer(maxlen=4)
+    for frame_number in (100, 101, 1, 2):
+        buffer.push(
+            CameraFrame(
+                _image(frame_number),
+                frame_number,
+                float(frame_number),
+                time.monotonic_ns(),
+            )
+        )
+
+    stats = buffer.stats()
+
+    assert stats.frame_number_regression_count == 1
+    assert stats.dropped_frames == 0
+```
+
+测试文件顶部增加标准库导入：
+
+```python
+import time
+```
+
+并在稳定拒绝原因参数中增加真实采集帧号回退：
+
+```python
+elif mutation == "camera_frame_regression":
+    root.attrs["camera_high_frame_number_regression_count"] = 1
+```
+
+对应期望：
+
+```python
+("camera_frame_regression", "camera_frame_number_regression"),
+```
+
+- [ ] **步骤 3：运行新增测试并确认失败**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
+  -k "sample_frame_metrics or async_camera_phase_drift or frame_number_regressions or camera_frame_regression" -v
+```
+
+预期：测试因 `sample_frame_metrics` 尚不存在、`CameraStats` 没有回退计数，或旧校验仍以 `camera_sample_drop_ratio` 拒绝而失败。
+
+- [ ] **步骤 4：实现最小采样相位指标**
+
+在质量校验辅助函数附近增加纯函数：
+
+```python
+def sample_frame_metrics(
+    frame_numbers: np.ndarray,
+) -> tuple[float, float, int]:
+    diffs = np.diff(np.asarray(frame_numbers, dtype=np.int64))
+    denominator = max(1, len(diffs))
+    return (
+        float(np.count_nonzero(diffs == 0) / denominator),
+        float(np.count_nonzero(diffs > 1) / denominator),
+        int(np.count_nonzero(diffs < 0)),
+    )
+```
+
+把 `validate_episode()` 中原有“所有 `diff != 1` 都拒绝”的循环替换为：
+
+```python
+for camera in ("cam_high", "cam_wrist"):
+    frame_numbers = datasets[
+        f"debug/cameras/{camera}_frame_number"
+    ][:]
+    repeat_ratio, skip_ratio, regression_count = sample_frame_metrics(
+        frame_numbers
+    )
+    metrics[f"{camera}_sample_repeat_ratio"] = repeat_ratio
+    metrics[f"{camera}_sample_skip_ratio"] = skip_ratio
+    if regression_count:
+        return _quality_failure("camera_frame_number_regression", metrics)
+```
+
+这样 `986 → 986 → 988` 只产生统计，不再触发 `camera_sample_drop_ratio`。
+
+- [ ] **步骤 5：在采集层统计帧号回退**
+
+扩展 `CameraStats`：
+
+```python
+@dataclass(frozen=True)
+class CameraStats:
+    frame_count: int
+    dropped_frames: int
+    frame_number_regression_count: int
+    first_host_monotonic_ns: int | None
+    last_host_monotonic_ns: int | None
+```
+
+在 `CameraBuffer.__init__()` 增加：
+
+```python
+self._frame_number_regression_count = 0
+```
+
+在 `CameraBuffer.push()` 更新帧号前使用互斥分支：
+
+```python
+if self._last_frame_number is not None:
+    if frame.frame_number <= self._last_frame_number:
+        self._frame_number_regression_count += 1
+    elif frame.frame_number > self._last_frame_number + 1:
+        self._dropped_frames += (
+            frame.frame_number - self._last_frame_number - 1
+        )
+```
+
+在 `stats()` 返回：
+
+```python
+frame_number_regression_count=self._frame_number_regression_count,
+```
+
+- [ ] **步骤 6：把真实采集回退纳入 episode 属性和拒绝条件**
+
+把 `_interval_camera_metrics()` 的返回值扩展为 FPS、真实丢帧率和本 episode 新增的回退数：
+
+```python
+@staticmethod
+def _interval_camera_metrics(
+    baseline: CameraStats,
+    current: CameraStats,
+) -> tuple[float, float, int]:
+    frames = max(0, current.frame_count - baseline.frame_count)
+    dropped = max(0, current.dropped_frames - baseline.dropped_frames)
+    regressions = max(
+        0,
+        current.frame_number_regression_count
+        - baseline.frame_number_regression_count,
+    )
+    if (
+        baseline.last_host_monotonic_ns is None
+        or current.last_host_monotonic_ns is None
+    ):
+        return 0.0, 1.0, regressions
+    elapsed_ns = (
+        current.last_host_monotonic_ns
+        - baseline.last_host_monotonic_ns
+    )
+    fps = frames * 1e9 / elapsed_ns if elapsed_ns > 0 else 0.0
+    expected = frames + dropped
+    drop_ratio = dropped / expected if expected else 1.0
+    return fps, drop_ratio, regressions
+```
+
+`_write_camera_metrics()` 写入：
+
+```python
+"camera_high_frame_number_regression_count": high[2],
+"camera_right_wrist_frame_number_regression_count": wrist[2],
+```
+
+`validate_episode()` 在读取 FPS 和真实丢帧率后要求两个回退属性存在且为零：
+
+```python
+for name in (
+    "camera_high_frame_number_regression_count",
+    "camera_right_wrist_frame_number_regression_count",
+):
+    if name not in root.attrs:
+        return _quality_failure("camera_stats_missing", metrics)
+    value = int(root.attrs[name])
+    metrics[name] = value
+    if value:
+        return _quality_failure("camera_frame_number_regression", metrics)
+```
+
+- [ ] **步骤 7：确保采样指标写入保存和拒绝文件**
+
+增加读取 HDF5 根节点的辅助函数：
+
+```python
+def episode_sample_frame_metrics(root: Any) -> dict[str, int | float]:
+    metrics: dict[str, int | float] = {}
+    for camera in ("cam_high", "cam_wrist"):
+        repeat, skip, regression = sample_frame_metrics(
+            root[f"debug/cameras/{camera}_frame_number"][:]
+        )
+        metrics[f"{camera}_sample_repeat_ratio"] = repeat
+        metrics[f"{camera}_sample_skip_ratio"] = skip
+        metrics[f"{camera}_sample_frame_number_regression_count"] = regression
+    return metrics
+```
+
+`validate_episode()` 复用该函数更新 `report.metrics`。在 `_reject_closed_partial()` 已打开 HDF5 后也执行：
+
+```python
+for name, value in episode_sample_frame_metrics(root).items():
+    root.attrs[name] = value
+```
+
+保存路径继续由 `_complete_save()` 把 `report.metrics` 写入属性。保存日志追加紧凑摘要：
+
+```python
+self.get_logger().info(
+    "ACT相机采样相位："
+    f"high重复={report.metrics['cam_high_sample_repeat_ratio']:.2%}, "
+    f"high跨帧={report.metrics['cam_high_sample_skip_ratio']:.2%}, "
+    f"wrist重复={report.metrics['cam_wrist_sample_repeat_ratio']:.2%}, "
+    f"wrist跨帧={report.metrics['cam_wrist_sample_skip_ratio']:.2%}"
+)
+```
+
+- [ ] **步骤 8：运行相关测试并确认通过**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
+  -k "camera or validate_episode" -v
+```
+
+预期：所有选中测试通过；真实丢帧率仍使用 `camera_drop_ratio` 拒绝，异步重复/跨帧不拒绝。
+
+- [ ] **步骤 9：提交任务 1**
+
+```bash
+git add src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
+  src/xr_rm_teleop/test/test_act_episode_recorder.py
+git commit -m "fix: 修正ACT相机采样质量判定"
+```
+
+### 任务 2：在录制器中增加非阻塞双路预览
+
+**文件：**
+
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:595-657`
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1006-1152`
+- 修改：`xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py:1668-1671`
+- 测试：`xr_rm_teleop/test/test_act_episode_recorder.py`
+
+- [ ] **步骤 1：写两秒滚动 FPS 测试**
+
+扩展 `CameraBuffer` 测试，使用明确的单调时间：
+
+```python
+def test_camera_buffer_reports_two_second_rolling_fps():
+    buffer = CameraBuffer(maxlen=4)
+    start_ns = 10_000_000_000
+    for index in range(61):
+        buffer.push(
+            CameraFrame(
+                _image(index),
+                index,
+                float(index),
+                start_ns + index * 33_333_333,
+            )
+        )
+
+    assert buffer.stats().rolling_fps == pytest.approx(30.0, rel=0.02)
+```
+
+- [ ] **步骤 2：写预览显示异常隔离测试**
+
+构造不经过 ROS 初始化的录制器和抛错的 `cv2` 替身：
+
+```python
+def test_preview_failure_does_not_change_recording_state():
+    recorder = object.__new__(ActEpisodeRecorder)
+    recorder._session = _recording_session()
+    recorder._preview_stop = threading.Event()
+    recorder._preview_thread = None
+    recorder.get_logger = lambda: _Logger()
+
+    class FailingCv2:
+        WINDOW_NORMAL = 0
+
+        @staticmethod
+        def namedWindow(*_args):
+            raise RuntimeError("no display")
+
+    recorder._preview_loop(FailingCv2())
+
+    assert recorder.state is RecordingState.RECORDING
+```
+
+`_Logger` 增加 `warns` 收集和 `warn()`：
+
+```python
+self.warns = []
+
+def warn(self, message):
+    self.warns.append(message)
+```
+
+- [ ] **步骤 3：写预览关闭顺序测试**
+
+验证关闭节点时先停预览，再停相机：
+
+```python
+def test_close_stops_preview_before_cameras_and_releases_lock():
+    events = []
+    recorder = object.__new__(ActEpisodeRecorder)
+    recorder._stop_preview = lambda: events.append("preview")
+    recorder._high_camera = SimpleNamespace(
+        stop=lambda: events.append("high")
+    )
+    recorder._wrist_camera = SimpleNamespace(
+        stop=lambda: events.append("wrist")
+    )
+    recorder._directory_lock = SimpleNamespace(
+        release=lambda: events.append("lock")
+    )
+
+    recorder.close()
+
+    assert events == ["preview", "high", "wrist", "lock"]
+```
+
+- [ ] **步骤 4：运行新增预览测试并确认失败**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
+  -k "rolling_fps or preview or close_stops_preview" -v
+```
+
+预期：测试因 `rolling_fps`、`_preview_loop()` 和 `_stop_preview()` 尚不存在而失败。
+
+- [ ] **步骤 5：实现两秒滚动 FPS**
+
+在 `CameraBuffer.__init__()` 增加：
+
+```python
+self._recent_host_monotonic_ns: deque[int] = deque()
+```
+
+每次 `push()` 时裁剪两秒窗口：
+
+```python
+self._recent_host_monotonic_ns.append(frame.host_monotonic_ns)
+cutoff_ns = frame.host_monotonic_ns - 2_000_000_000
+while (
+    self._recent_host_monotonic_ns
+    and self._recent_host_monotonic_ns[0] < cutoff_ns
+):
+    self._recent_host_monotonic_ns.popleft()
+```
+
+`CameraStats` 增加字段和属性：
+
+```python
+recent_host_monotonic_ns: tuple[int, ...]
+
+@property
+def rolling_fps(self) -> float:
+    if len(self.recent_host_monotonic_ns) < 2:
+        return 0.0
+    elapsed_ns = (
+        self.recent_host_monotonic_ns[-1]
+        - self.recent_host_monotonic_ns[0]
+    )
+    return (
+        (len(self.recent_host_monotonic_ns) - 1) * 1e9 / elapsed_ns
+        if elapsed_ns > 0
+        else 0.0
+    )
+```
+
+`stats()` 使用：
+
+```python
+recent_host_monotonic_ns=tuple(self._recent_host_monotonic_ns),
+```
+
+- [ ] **步骤 6：实现最小预览渲染方法**
+
+在 `ActEpisodeRecorder` 增加固定窗口名：
+
+```python
+PREVIEW_WINDOW = "ACT - D455 Global / D405 Right Wrist"
+```
+
+增加生成单个画面块的方法。相机帧是 RGB，因此显示前转换成 BGR：
+
+```python
+@staticmethod
+def _preview_tile(role: str, frame, stats, cv2):
+    if frame is None:
+        tile = np.zeros((480, 640, 3), dtype=np.uint8)
+        frame_number = "-"
+    else:
+        tile = cv2.cvtColor(frame.image, cv2.COLOR_RGB2BGR)
+        frame_number = str(frame.frame_number)
+    tile = tile.copy()
+    cv2.rectangle(tile, (0, 0), (640, 74), (0, 0, 0), -1)
+    lines = (
+        f"{role}  FPS {stats.rolling_fps:.1f}  Frame {frame_number}",
+        f"Received {stats.frame_count}  Dropped "
+        f"{stats.dropped_frames} ({stats.drop_ratio:.2%})",
+    )
+    for index, line in enumerate(lines):
+        cv2.putText(
+            tile,
+            line,
+            (10, 28 + index * 30),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.65,
+            (255, 255, 255),
+            1,
+            cv2.LINE_AA,
+        )
+    return tile
+```
+
+增加 `_compose_preview()`：读取两个缓冲最新帧，水平拼接，并在底部显示状态：
+
+```python
+def _compose_preview(self, cv2):
+    high_frames = self._high_camera.buffer.snapshot()
+    wrist_frames = self._wrist_camera.buffer.snapshot()
+    high = high_frames[-1] if high_frames else None
+    wrist = wrist_frames[-1] if wrist_frames else None
+    high_stats = self._high_camera.buffer.stats()
+    wrist_stats = self._wrist_camera.buffer.stats()
+    image = np.hstack(
+        (
+            self._preview_tile("GLOBAL D455", high, high_stats, cv2),
+            self._preview_tile("RIGHT WRIST D405", wrist, wrist_stats, cv2),
+        )
+    )
+
+    now_ns = self._now_ns()
+    high_age = (
+        f"{(now_ns - high.host_monotonic_ns) * 1e-6:.1f} ms"
+        if high is not None
+        else "-"
+    )
+    wrist_age = (
+        f"{(now_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
+        if wrist is not None
+        else "-"
+    )
+    skew = (
+        f"{abs(high.host_monotonic_ns - wrist.host_monotonic_ns) * 1e-6:.1f} ms"
+        if high is not None and wrist is not None
+        else "-"
+    )
+    episode = (
+        f"episode_{self._episode_index}"
+        if self._episode_index is not None
+        else "-"
+    )
+    samples = self._store.count if self._store is not None else 0
+    footer = np.zeros((80, image.shape[1], 3), dtype=np.uint8)
+    lines = (
+        f"Age high={high_age} wrist={wrist_age}  Camera skew={skew}",
+        f"ACT {self.state.value}  {episode}  Samples {samples}",
+    )
+    for index, line in enumerate(lines):
+        cv2.putText(
+            footer,
+            line,
+            (10, 30 + index * 32),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.7,
+            (255, 255, 255),
+            1,
+            cv2.LINE_AA,
+        )
+    return np.vstack((image, footer))
+```
+
+若尚无帧，对应数值显示 `-`；该方法不修改任何录制器状态。
+
+- [ ] **步骤 7：实现预览线程生命周期和故障隔离**
+
+在相机成员创建前初始化：
+
+```python
+self._preview_stop = threading.Event()
+self._preview_thread: threading.Thread | None = None
+```
+
+两台相机成功启动后调用 `_start_preview()`：
+
+```python
+if self._camera_start_error is None:
+    self._start_preview()
+```
+
+实现：
+
+```python
+def _start_preview(self) -> None:
+    if not (os.environ.get("DISPLAY") or os.environ.get("WAYLAND_DISPLAY")):
+        self.get_logger().warn("未检测到桌面显示环境，ACT双相机预览已停用。")
+        return
+    try:
+        import cv2
+    except ImportError as exc:
+        self.get_logger().warn(f"OpenCV不可用，ACT双相机预览已停用：{exc}")
+        return
+    self._preview_stop.clear()
+    self._preview_thread = threading.Thread(
+        target=self._preview_loop,
+        args=(cv2,),
+        name="act_camera_preview",
+        daemon=True,
+    )
+    self._preview_thread.start()
+
+def _preview_loop(self, cv2) -> None:
+    try:
+        cv2.namedWindow(self.PREVIEW_WINDOW, cv2.WINDOW_NORMAL)
+        while not self._preview_stop.is_set():
+            cv2.imshow(self.PREVIEW_WINDOW, self._compose_preview(cv2))
+            key = cv2.waitKey(1) & 0xFF
+            if key in (ord("q"), ord("Q"), 27):
+                break
+            if cv2.getWindowProperty(
+                self.PREVIEW_WINDOW,
+                cv2.WND_PROP_VISIBLE,
+            ) < 1:
+                break
+            self._preview_stop.wait(0.1)
+    except Exception as exc:
+        self.get_logger().warn(f"ACT双相机预览已停用：{exc}")
+    finally:
+        try:
+            cv2.destroyWindow(self.PREVIEW_WINDOW)
+        except Exception:
+            pass
+
+def _stop_preview(self) -> None:
+    self._preview_stop.set()
+    if self._preview_thread is not None:
+        self._preview_thread.join(timeout=2.0)
+    self._preview_thread = None
+```
+
+`close()` 的第一步调用 `self._stop_preview()`，然后保持现有相机停止和目录锁释放顺序。
+
+- [ ] **步骤 8：运行预览相关测试并确认通过**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py \
+  -k "rolling_fps or preview or close_stops_preview" -v
+```
+
+预期：所有选中测试通过，测试过程不打开真实窗口或相机。
+
+- [ ] **步骤 9：运行 ACT 录制器完整单元测试**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -v
+```
+
+预期：全部通过。
+
+- [ ] **步骤 10：提交任务 2**
+
+```bash
+git add src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
+  src/xr_rm_teleop/test/test_act_episode_recorder.py
+git commit -m "feat: 添加ACT双相机实时预览"
+```
+
+### 任务 3：更新使用说明并完成工作空间验证
+
+**文件：**
+
+- 修改：`README.md:144-169`
+
+- [ ] **步骤 1：更新 ACT 数采说明**
+
+在 ACT episode 启动命令后补充：
+
+```markdown
+`record_act:=true` 启动后默认显示全局 D455 和右腕 D405 双路画面，并显示实时
+FPS、真实丢帧率、帧龄、双相机时间差、录制状态、episode 编号和样本数。按
+`Q`、`Esc` 或关闭窗口只会停止预览，ACT 相机采集和录制继续运行；没有桌面环境
+或 OpenCV 显示失败时也不会影响录制。
+
+相机采集线程观察到的真实掉帧仍会拒绝 episode。独立 `30 Hz` 控制和相机时钟
+造成的 ACT 样本重复/跨帧只写入 HDF5 质量指标，不再误报为相机丢包。
+```
+
+- [ ] **步骤 2：运行文档和 Python 静态检查**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+python -m py_compile \
+  src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
+  src/xr_rm_teleop/test/test_act_episode_recorder.py
+git diff --check
+```
+
+预期：命令返回码为 `0`，没有语法或空白错误。
+
+- [ ] **步骤 3：运行项目要求的工作空间构建**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+```
+
+预期：所有包构建成功。不得在 `/home/robot/WS_xr/src` 中运行该命令。
+
+- [ ] **步骤 4：构建后再次运行 ACT 录制器测试**
+
+运行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+pytest src/xr_rm_teleop/test/test_act_episode_recorder.py -v
+```
+
+预期：全部通过。
+
+- [ ] **步骤 5：确认提交范围并提交任务 3**
+
+运行：
+
+```bash
+git status --short
+git diff -- README.md \
+  src/xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py \
+  src/xr_rm_teleop/test/test_act_episode_recorder.py
+git add README.md
+git commit -m "docs: 更新ACT相机预览说明"
+```
+
+不得暂存或提交 `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`。
+
+## 现场验证
+
+自动化实施结束后，由用户在确认现场安全条件后运行现有 ACT 数采入口：
+
+```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:=right use_mock:=false record_act:=true
+```
+
+该命令会连接右臂真机，代理不得自行执行。现场确认：
+
+1. 双路画面默认出现且角色正确；
+2. FPS、真实丢帧率、帧龄、双相机时间差和 ACT 状态持续更新；
+3. 关闭窗口后录制状态和 HDF5 写入继续；
+4. 正常异步重复/跨帧的 episode 能保存；
+5. HDF5 属性包含真实采集质量与两路采样重复/跨帧指标。

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-10-act-tomato-pick-data-collection-design.md

@@ -0,0 +1,657 @@
+# 右臂番茄采摘 ACT 数据采集适配设计
+
+## 背景与目标
+
+当前项目已经通过 ROS2 Humble、PICO 手柄、Placo QP 和 RealMan Python API2
+完成 RM75 遥操作。右臂遥操节点以名义 `90 Hz` 读取实际关节反馈，根据 PICO
+相对位姿生成 TCP 目标，经过工作空间限制、QP、关节速度与加速度限制后，通过
+`rm_movej_canfd` 下发最终关节目标。
+
+本次变更在不重写现有遥操链路的前提下，增加一个独立的 ALOHA/ACT 风格数据采集
+节点。第一阶段只采集右臂番茄采摘任务，每个 episode 覆盖从初始位姿出发、抓取
+番茄、搬运至 RM75 下方收集篮并释放番茄的完整过程。
+
+核心目标如下：
+
+- 以右臂 7 个实际关节角和夹爪逻辑状态作为 `observations/qpos`；
+- 以实际成功下发或零阶保持的 7 个最终关节目标和夹爪目标作为 `action`；
+- 同步采集一台全局 D455 和一台右腕 D405 的 RGB 图像；
+- 以 `30 Hz` 形成同周期因果对齐的数据；
+- 每个完整任务流式保存为一个 ALOHA/ACT 核心结构兼容的 HDF5 文件；
+- 支持手柄开始、结束、丢弃、拒绝、质量检查、崩溃恢复和编号防覆盖；
+- 保留足够的 PICO、TCP、QP、夹爪和时间戳调试数据，但不让采集节点进入机器人
+  控制链路。
+
+参考实现为 ALOHA 官方仓库中的
+[`record_episodes.py`](https://github.com/tonyzhaozh/aloha/blob/master/aloha_scripts/record_episodes.py)。
+官方双臂数据使用 14 维状态和动作；本项目第一阶段采用右臂 `7+1=8` 维，因此只
+保证 HDF5 核心组织方式兼容，不声称官方旧训练加载器可以不修改直接训练。
+
+## 非目标
+
+第一阶段明确不实现：
+
+- 左臂或双臂 ACT 采集；
+- ACT 训练代码、数据加载器、策略部署或自动完成判定；
+- 深度图、红外图、点云、图像压缩或 ROS 图像话题；
+- rosbag、中间格式、离线转换工具、GUI、声音或手柄震动反馈；
+- 让 ACT 学习 A 键触发的回初始位姿运动；
+- 修改现有工作空间限制、圆柱限制、速度限制、指令超时、安全停止或
+  `move_to_initial_pose_on_connect` 默认值；
+- 新建重复的 ROS2 包或第二个 RealMan 连接。
+
+后续训练时建议将 ACT `chunk_size` 设为 `60`，对应约 2 秒动作长度；该参数属于
+训练配置，不写死在采集逻辑中。
+
+## 现有控制链路与关键约束
+
+现有 `single_arm_velocity_teleop` 在每个控制周期内依次完成：
+
+```text
+读取最新 RM75 反馈
+→ 同步 Placo 状态
+→ 读取 PICO 状态并生成 TCP 目标
+→ 工作空间、位姿步长与速度限制
+→ Placo QP
+→ 关节速度与加速度限制
+→ rm_movej_canfd 下发
+→ 发布 joint_target 调试话题
+```
+
+当前 `joint_states`、TCP 调试话题和 `joint_target` 分别发布并各自取时间戳。如果
+采集节点仅订阅这些分散话题，即使时间接近，也可能把第 100 个控制周期的
+`q_actual` 与第 99 个控制周期的 `q_target` 拼在一起。这里的“第 100 个周期”是
+控制序号，不是 `100 Hz`；现有控制频率仍是名义 `90 Hz`。
+
+此外，A 键回位调用的是一次阻塞式 `rm_movej(initial_joint_pose)`。轨迹由 RM75
+控制器内部生成，项目不能获得每个控制周期的中间目标，也不经过当前 QP 和
+`rm_movej_canfd` 链路，因此不能与遥操动作混用同一种标签语义。
+
+## 总体架构
+
+采用一个自定义原子控制采样消息和一个独立 ACT 采集节点：
+
+```text
+PICO 输入 + RM75 反馈
+          ↓
+single_arm_velocity_teleop（90 Hz）
+          ↓
+QP → 关节限速/限加速度 → RM75 下发
+          ↓
+ActControlSample（同周期、同时间戳、同序号）
+          ↓
+act_episode_recorder（每 3 个控制周期取 1 个）
+          ├──────────────┐
+          ↓              ↓
+   全局 D455 RGB     右腕 D405 RGB
+          └──────┬───────┘
+                 ↓
+        30 Hz 流式写入临时 HDF5
+                 ↓
+       裁剪 → 质量检查 → 保存/拒绝/丢弃
+```
+
+“原子”表示消息是一个逻辑上不可拆分的控制周期快照。订阅者要么收到该周期完整的
+反馈、求解结果、最终动作和状态，要么该周期整体缺失；不会自行拼接多个异步话题。
+
+职责边界如下：
+
+- 遥操节点继续唯一负责 RM75 连接、反馈、QP、限位、动作下发和安全停止；
+- 遥操节点只增加原子消息发布和夹爪逻辑状态记录，不读取相机、不写 HDF5；
+- 采集节点只订阅控制/PICO 数据、独占两台相机并写文件，不连接或控制 RM75；
+- 采集节点异常、退出或写盘过慢不得阻塞遥操发布或改变机器人动作；
+- 相机由采集节点通过 `pyrealsense2` 直接打开，不再发布和重新订阅 ROS 图像。
+
+原子消息使用本机低延迟、非阻塞的 best-effort QoS。采集节点检测任何需要保留的
+控制周期丢失并拒绝 episode，而不是让 DDS 反压影响遥操控制。
+
+## 原子控制采样消息
+
+在 `xr_rm_interfaces` 中新增 `ActControlSample.msg`，发布话题为：
+
+```text
+/xr_rm/right_rm75/act_control_sample
+```
+
+消息至少表达以下内容：
+
+| 类别 | 字段语义 |
+|---|---|
+| 周期标识 | ROS header、`control_seq`、控制周期单调时间戳 |
+| 关节反馈 | `q_actual[7]`、当前 URDF 关节上下限、反馈接收单调时间戳、反馈年龄、反馈有效状态 |
+| QP | QP 原始输出或失败时的保持目标 `q_qp_raw[7]`、是否尝试、成功状态、耗时 |
+| 最终动作 | 经关节限速后的 `q_target[7]`、动作时间戳、是否当前周期成功下发 |
+| TCP | 当前 TCP、PICO 映射前的原始目标 TCP、最终受限目标 TCP、命令速度 |
+| PICO | 当前右手位姿、Grip、Trigger、A、B 和摇杆值 |
+| 夹爪 | 请求目标、已确认逻辑状态、命令是否处理中、命令是否失败 |
+| 控制状态 | `teleop_active`、`action_valid`、QP 回退、目标限位和控制故障状态 |
+
+数值型关节字段在 ROS 消息中保持双精度，写入 HDF5 核心数据时显式转换成
+`float32`。位姿使用位置加四元数，不保存完整 Placo 对象、Hessian、约束矩阵或
+其他大体积求解器内部状态。
+
+消息在每个实际执行的 90 Hz 控制回调中发布，包括 Grip 松开和安全停止状态：
+
+- 当前周期成功发送动作时，`command_sent=true`；
+- 当前周期没有发送，但此前存在成功目标时，`q_target` 零阶保持上一个成功目标，
+  `command_sent=false`、`action_valid=true`；
+- 尚未形成任何有效目标或当前动作发送失败时，`action_valid=false`；
+- QP 失败但成功重发上次有效目标时，`qp_success=false`、`action_valid=true`；
+- 发送失败必须发布失败状态，并由采集节点拒绝当前 episode。
+
+相机数据不放进该消息。相机时间戳和帧号由采集节点在同一主机的单调时钟域中补充。
+
+## 夹爪数据语义
+
+右臂使用 Modbus 电动夹爪。由于不同番茄尺寸会导致实际停止开度不同，而本项目只
+关心抓取意图，第一阶段不读取或估算实际开度。
+
+统一约定：
+
+```text
+0 = closed
+1 = open
+```
+
+两类状态必须分开：
+
+- `action[7]` 是目标状态，在 Trigger 产生开合请求的控制周期立即改变；
+- `qpos[7]` 是已确认逻辑状态，只有 Modbus 命令正常返回后才改变；
+- 命令失败时 `qpos[7]` 保持原值，并拒绝当前 episode；
+- 采集前右臂夹爪必须成功初始化为完全打开 `1.0`，之后才允许把初始
+  `qpos[7]` 设为 `1`；
+- 不再使用原先含义不明确的 `0.75 → 0.15` 初始化序列。
+
+结束一个有效番茄采摘 episode 前，操作者应先请求打开夹爪，等待日志/状态确认
+逻辑状态已经变为 `open`，再松开 Grip 并按 B。结束时夹爪命令仍在执行，保存状态
+最多等待 3 秒；失败或超时则拒绝。最终裁剪后的数据若没有包含已确认的打开状态，
+同样拒绝，不能把保存后的成功状态回填到更早样本中。
+
+## 相机配置与采集
+
+第一阶段固定使用两台已确定序列号的 RealSense：
+
+| ACT 名称 | 型号与位置 | 序列号 |
+|---|---|---|
+| `cam_high` | 全局 D455 | `234222303366` |
+| `cam_right_wrist` | 右臂腕部 D405 | `412622272532` |
+
+两路图像参数统一为：
+
+```text
+分辨率：640 × 480
+帧率：30 FPS
+格式：RGB uint8
+HDF5 形状：(T, 480, 640, 3)
+```
+
+不采集深度、红外和点云，不使用 JPEG 压缩。采集线程直接请求 RealSense RGB8，
+避免为颜色通道转换引入 OpenCV 依赖。
+
+每台相机使用独立采集线程和一个很小的 `deque` 帧缓冲。每帧保存：
+
+- RealSense 帧号；
+- RealSense 硬件时间戳；
+- `wait_for_frames` 返回后立即读取的主机单调时间戳；
+- RGB 数组。
+
+两台设备的硬件时钟不能默认视为同一时钟域，因此正式对齐只使用同一主机的单调
+时钟；硬件时间戳只用于发现设备重启、帧号跳变和采集异常。
+
+采集节点独占相机。指定设备缺失、型号/序列号不匹配、流配置失败或已经被其他
+进程占用时，预检失败并停留在 `IDLE`，不自动替换成其他相机。
+
+## 30 Hz 采样与因果对齐
+
+正式采样不使用独立的 30 Hz ROS 定时器。采集节点在首次有效 Grip 控制周期记录
+`sample_origin_seq`，随后只选择：
+
+```text
+(control_seq - sample_origin_seq) % 3 == 0
+```
+
+因此 90 Hz 控制消息按 `0、3、6、9...` 的相对序号形成名义 30 Hz 数据，同时保证
+第一个正式样本就是首次有效动作，而不是等待一个全局取模相位。
+
+每个样本的定义为：
+
+```text
+observation[t]
+  = 当前控制周期开始时读取的 q_actual
+  + 对每台相机选择主机时间戳不晚于该控制周期的最新帧
+
+action[t]
+  = 同一控制周期经 QP、关节限速后成功发送的 q_target
+    或 Grip 暂停/QP 回退时明确定义的上次成功目标
+```
+
+采集节点收到消息时，相机缓冲中可能已经存在晚于控制周期的帧，因此不能简单取
+“回调时最新帧”，必须按 `host_monotonic_ns <= control_monotonic_ns` 选择最近帧。
+不存在满足条件且年龄不超过 50 ms 的帧时，当前 episode 拒绝。
+
+不采用官方旧加载器中的 `action[t-1]` 补丁。HDF5 根属性写入：
+
+```text
+action_alignment = "same_step_causal"
+```
+
+控制、反馈、动作和图像源时间戳全部保存在 `/debug`，未来只有在真实延迟测量证明
+存在稳定偏移时，才在训练加载器中调整；原始 HDF5 不进行不可逆移位。
+
+## Episode 边界与手柄状态机
+
+### 按键映射
+
+- 右手 B，即右手 `secondary` 单击：开始准备或结束保存；
+- 左手 Y，即左手 `secondary` 长按 1 秒：丢弃当前准备/录制；
+- 右手 A，即右手 `primary`：继续保持现有右臂回初始位姿功能；
+- Grip：继续只控制遥操离合，不作为“只在按下时才记录”的采集开关。
+
+右手 B 在右手 Grip 按下时始终忽略，避免运动中误触开始或结束。左手 Y 仅在
+`ARMED` 或 `RECORDING` 中长按有效，在 `IDLE` 中无作用，且永远不删除上一个已经
+保存的 episode。
+
+### 状态机
+
+```text
+IDLE
+  └─ Grip 松开时单击右手 B
+       ├─ 预检失败 → IDLE
+       └─ 预检通过 → ARMED
+
+ARMED
+  ├─ 第一次 Grip 有效动作 → RECORDING
+  ├─ 再次单击右手 B → 取消 → IDLE
+  ├─ 长按左手 Y 1 秒 → DISCARDED → IDLE
+  └─ 按右手 A → 取消 → IDLE
+
+RECORDING
+  ├─ Grip 松开后单击右手 B → SAVING
+  ├─ 长按左手 Y 1 秒 → DISCARDED → IDLE
+  ├─ 按右手 A → REJECTED → IDLE
+  └─ 硬质量故障/60 秒上限/Ctrl+C → REJECTED → IDLE
+
+SAVING
+  ├─ 质量检查通过 → SAVED → IDLE
+  └─ 质量检查失败 → REJECTED → IDLE
+```
+
+`IDLE`、`ARMED`、`RECORDING`、`SAVING` 是运行状态；`SAVED`、`DISCARDED`、
+`REJECTED` 是短暂结果状态，发布一次结果并输出日志后回到 `IDLE`。`SAVING`
+期间忽略 B/Y 录制按键，A 键仍属于原有遥操逻辑，但不会再进入已经结束的数据。
+
+状态通过 `std_msgs/msg/String` 话题 `/act/recording_status` 和终端日志报告，不增加
+新状态消息、声音或震动接口。
+
+### 连续记录与 Grip 暂停
+
+正式时间轴从 `ARMED` 后 Grip 按下且第一次
+`action_valid=true、command_sent=true` 的控制周期开始。Grip 刚按下的建基准周期
+尚未向 RM75 发送新的 CANFD 目标，因此不作为第一个训练样本。
+录制过程中临时松开 Grip 时仍以 30 Hz 保存图像和 `qpos`，`action` 零阶保持上次
+成功目标，并记录 `teleop_active=false`：
+
+- 松开后重新按 Grip：暂停区间保留，继续同一个 episode；
+- 最后一次松开后按 B：将该次松开至 B 之间的纯操作等待数据裁掉，episode 结束在
+  最后一次 Grip 松开附近；
+- 夹爪打开确认必须已经包含在裁剪终点之前，否则拒绝该 episode。
+
+只在 Grip 按下时保存数据会丢失接近任务开始、暂停恢复和完整视觉上下文，因此不
+采用该方案。
+
+### Episode 是否包含 A 键回位
+
+一个正式 episode 只包含：
+
+```text
+初始位姿、夹爪打开
+→ 接近番茄
+→ 闭合夹爪
+→ 搬运至收集篮
+→ 打开夹爪并确认成功
+→ 松开 Grip
+→ 按 B 结束
+```
+
+A 键的 `rm_movej(initial_joint_pose)` 必须在成功结束 episode 后执行，不写进
+episode。这样 ACT 始终学习同一种逐周期 `q_target` 动作语义。未来实时推理若要
+连续采摘，应由上层状态机执行：
+
+```text
+ACT 完成一次采摘 → 完成判定 → 固定 rm_movej 复位 → 下一次 ACT 采摘
+```
+
+若在 `RECORDING` 中误按 A，当前文件转入拒绝目录，原因写为：
+
+```text
+initial_pose_command_during_episode
+```
+
+拒绝数据不会拦截 A 键原有回位动作，也不会额外控制机器人。
+
+## HDF5 核心结构
+
+数据根目录和任务目录固定为：
+
+```text
+/home/robot/ACT_Data
+/home/robot/ACT_Data/tomato_pick
+```
+
+正式文件核心结构：
+
+```text
+/observations/qpos                       float32 (T, 8)
+/observations/images/cam_high            uint8   (T, 480, 640, 3)
+/observations/images/cam_right_wrist     uint8   (T, 480, 640, 3)
+/action                                  float32 (T, 8)
+/debug/...
+```
+
+`T` 是该次任务的实际样本数，不要求所有 episode 等长，不进行文件内 padding。
+最短有效 episode 为 `60` 个样本，即 2 秒；最长为 `1800` 个样本，即 60 秒。
+
+8 维字段顺序固定为：
+
+```text
+qpos[0:7]   = RM75 实际反馈关节角，单位 rad
+qpos[7]     = 已确认夹爪逻辑状态，0 closed、1 open
+
+action[0:7] = 最终成功下发或明确定义为保持的关节目标，单位 rad
+action[7]   = 夹爪请求目标，0 closed、1 open
+```
+
+根属性至少包括：
+
+| 属性 | 值或语义 |
+|---|---|
+| `sim` | `false`，与 ALOHA 真实数据约定一致 |
+| `task_name` | `tomato_pick` |
+| `sample_rate_hz` | `30` |
+| `action_alignment` | `same_step_causal` |
+| `arm` | `right_rm75` |
+| `episode_status` | `saved` 或 `rejected` |
+| `camera_high_serial` | `234222303366` |
+| `camera_right_wrist_serial` | `412622272532` |
+| `joint_names` | 7 个 RM75 关节名和 `gripper` 的固定顺序 |
+| `joint_lower_limits` | 来自当前 Placo/URDF 的 7 关节下限 |
+| `joint_upper_limits` | 来自当前 Placo/URDF 的 7 关节上限 |
+| `reject_reason` | 仅拒绝文件存在 |
+| `interrupted` | 正常文件为 `false`，Ctrl+C/异常恢复为 `true` |
+
+图像不压缩，每帧使用一个 HDF5 chunk；数值数据使用可扩展的一维时间轴并分块
+写入。按两路 `640×480×3×30` 计算，图像数据约为 3.3 GB/分钟，因此不能把完整
+episode 先缓存到内存再一次性保存。
+
+不创建 `/observations/qvel`、`/observations/effort`、压缩标记或 `compress_len`；
+也不使用零值、有限差分或其他伪数据填充缺失字段。后续训练加载器按存在的核心
+字段读取。
+
+## Debug 结构
+
+自定义消息是运行时传输载体，进程退出后不会保留；HDF5 `/debug` 是永久诊断记录。
+ACT 训练默认不读取该组。
+
+建议使用以下精简结构，布尔状态以 `uint8` 保存：
+
+```text
+/debug/timestamps/control_monotonic_ns          int64   (T,)
+/debug/timestamps/feedback_monotonic_ns         int64   (T,)
+/debug/timestamps/action_monotonic_ns           int64   (T,)
+/debug/timestamps/cam_high_host_monotonic_ns    int64   (T,)
+/debug/timestamps/cam_wrist_host_monotonic_ns   int64   (T,)
+/debug/timestamps/cam_high_hardware_ms           float64 (T,)
+/debug/timestamps/cam_wrist_hardware_ms          float64 (T,)
+/debug/timestamps/cam_high_age_ms                float32 (T,)
+/debug/timestamps/cam_wrist_age_ms               float32 (T,)
+/debug/timestamps/inter_camera_skew_ms           float32 (T,)
+
+/debug/cameras/cam_high_frame_number             uint64  (T,)
+/debug/cameras/cam_wrist_frame_number            uint64  (T,)
+
+/debug/control/control_seq                       uint64  (T,)
+/debug/control/teleop_active                     uint8   (T,)
+/debug/control/action_valid                      uint8   (T,)
+/debug/control/command_sent                      uint8   (T,)
+/debug/control/target_clamped                    uint8   (T,)
+/debug/control/control_fault                     uint8   (T,)
+
+/debug/qp/raw_target                             float32 (T, 7)
+/debug/qp/attempted                              uint8   (T,)
+/debug/qp/success                                uint8   (T,)
+/debug/qp/duration_ms                            float32 (T,)
+
+/debug/tcp/current_pose                          float32 (T, 7)
+/debug/tcp/raw_target_pose                       float32 (T, 7)
+/debug/tcp/final_target_pose                     float32 (T, 7)
+/debug/tcp/command_velocity                      float32 (T, 6)
+
+/debug/pico/right_pose                           float32 (T, 7)
+/debug/pico/right_inputs                         float32 (T, 6)
+/debug/pico/left_secondary                       uint8   (T,)
+
+/debug/gripper/target_open                       uint8   (T,)
+/debug/gripper/state_open                        uint8   (T,)
+/debug/gripper/command_pending                   uint8   (T,)
+/debug/gripper/command_failed                    uint8   (T,)
+```
+
+位姿顺序统一为 `[x, y, z, qx, qy, qz, qw]`，TCP 速度顺序统一为
+`[vx, vy, vz, wx, wy, wz]`。`right_inputs` 顺序在文件属性中写明。当前周期没有发送
+动作时，`action_monotonic_ns=-1`，并以 `command_sent=false` 消除歧义。
+
+episode 根属性额外保存 QP 失败次数、失败占比、最长连续失败次数、目标限位次数、
+相机帧率、丢帧率和最大时间偏差等汇总指标。
+
+## 数据质量规则
+
+### 开始前预检
+
+Grip 松开时单击 B 后，采集节点检查：
+
+- 输出目录存在或可以创建且可写；
+- 可用空间不少于 4 GiB，约为 60 秒原始图像估算值的 1.2 倍；
+- 两台指定相机均在线、已经连续预热 5 秒且当前帧率合格；
+- 最近 `q_actual` 合法，反馈年龄不超过 50 ms，RM75 无掉使能或控制故障；
+- 右臂夹爪初始化打开命令已经成功；
+- 左右 PICO 话题均在现有手柄超时范围内保持新鲜；
+- 没有第二个采集进程持有任务目录锁或相机设备；
+- 启动组合是 `arm:=right use_mock:=false record_act:=true`。
+
+任一预检失败时输出明确原因并停留在 `IDLE`，不生成空文件，也不改变机器人状态。
+
+### 硬拒绝条件
+
+以下任一情况使当前 episode 进入 `REJECTED`：
+
+- 样本少于 60 或达到 60 秒上限；
+- 控制周期序列缺失、有效平均采样率低于 27 Hz 或相邻样本间隔超过 100 ms；
+- `qpos/action` 不是 `(T,8)`、包含 NaN/Inf、违反配置关节限制或夹爪值不是
+  `0/1`；
+- `q_actual` 年龄超过 50 ms，反馈超时、掉使能或出现控制故障；
+- 最终关节动作发送失败，或消息表示的反馈和动作不属于同一控制周期；
+- 夹爪命令失败、超时，或最终裁剪数据没有包含已确认的打开状态；
+- 任一路相机平均帧率低于 27 FPS；
+- 任一路相机硬件帧号丢失率超过 1%，或采样后的重复/跳帧比例超过 1%；
+- 任一采样图像年龄超过 50 ms，或两路图像主机时间差超过 50 ms；
+- 图像形状、数据类型或 RGB 通道约定错误；
+- 录制中按 A；
+- HDF5 写入失败、磁盘空间不足或有界写入队列持续积压；
+- Ctrl+C、采集节点异常退出或启动时恢复崩溃残留文件。
+
+采集节点的拒绝只处理数据，不额外发送停止或运动命令。若原因来自控制故障，仍由
+现有遥操安全链路执行原有安全停止。
+
+### 允许但记录告警的情况
+
+QP 求解偶发失败时，现有逻辑保留上次有效关节目标。只要该保持目标最终成功发送、
+反馈和其他质量规则正常，就不自动拒绝 episode，而是保存：
+
+- QP 失败样本数；
+- 失败占比；
+- 最长连续失败样本数；
+- 每个样本的 `qp_success`。
+
+工作空间限位、TCP 步长限制或关节速度/加速度限制生效同样只记录，不自动拒绝。
+这些限制是正常安全控制的一部分。
+
+## 文件编号、保存、拒绝与恢复
+
+正式编号只扫描任务目录根部的 `episode_<数字>.hdf5`，取最大编号加一：
+
+```text
+已有 episode_0.hdf5 ... episode_9.hdf5
+重启后下一个正式文件仍为 episode_10.hdf5
+```
+
+不填补编号空洞，绝不覆盖已有正式文件。任务目录使用标准库文件锁保证同一时刻只有
+一个采集进程分配编号和写入；临时文件与目标文件位于同一文件系统，检查通过后使用
+不覆盖已有目标的原子发布方式。
+
+文件生命周期：
+
+```text
+录制中：
+/home/robot/ACT_Data/tomato_pick/episode_10.partial.hdf5
+
+检查通过：
+/home/robot/ACT_Data/tomato_pick/episode_10.hdf5
+
+检查失败：
+/home/robot/ACT_Data/tomato_pick/rejected/
+  episode_10_<reason>_<timestamp>.hdf5
+```
+
+- 只有正式保存成功才消耗编号；
+- 手动长按 Y 丢弃时关闭并删除当前临时文件，不生成拒绝文件；
+- 普通质量拒绝转入 `rejected/`，不消耗正式编号；
+- Ctrl+C 时尽力关闭可读 HDF5，根属性写入 `interrupted=true`，文件名原因使用
+  `interrupted`；
+- 下次启动先处理遗留临时文件：可读文件转入 `rejected/` 并标记
+  `crash_recovered`，不可读文件改成带时间戳的 `.partial.hdf5` 保留；
+- 拒绝原因同时写入文件名和根属性；
+- 任一步出现目标文件冲突时停止保存并报警，不能覆盖或自动删除已有 episode。
+
+写入采用单独工作线程和有界队列，ROS 回调只完成取样、对齐和入队。队列容量只需
+覆盖短暂磁盘抖动，不能无限增长掩盖磁盘吞吐不足；持续积压时拒绝数据。
+
+## 启动、配置与依赖
+
+继续使用唯一入口 `xr_rm_bringup/launch/arm_debug.launch.py`，新增参数：
+
+```text
+record_act:=false
+```
+
+默认 `false`，现有 mock、单臂、双臂和 MuJoCo 启动行为不变。第一阶段唯一允许的
+采集组合为：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py \
+  arm:=right use_mock:=false record_act:=true
+```
+
+`record_act:=true` 配合 `arm:=left|both` 或 `use_mock:=true` 时，在 launch 参数校验
+阶段明确拒绝。ACT 采集节点退出不触发整个 launch 的 `Shutdown`，保证数据进程故障
+不会终止遥操；终端必须清晰显示采集已经不可用。
+
+新增一份专用 YAML，集中保存：
+
+- 数据根目录和任务名；
+- 两个相机序列号、分辨率和帧率；
+- 30 Hz 采样率、2 秒最短时长和 60 秒最长时长；
+- 相机、反馈、磁盘和时间对齐质量阈值；
+- PICO 左右话题、原子采样话题和状态话题。
+
+硬件相关阈值保留为配置项，核心 HDF5 字段顺序和 8 维语义固定，不为未来可能的
+变体增加插件或通用框架。
+
+采集节点继续由 `/home/robot/miniconda3/envs/xr/bin/python` 启动。复用该环境已有的
+`pyrealsense2` 和 NumPy，只在该环境增加 HDF5 必需依赖 `h5py`；不修改系统 Python，
+不新增 Conda 环境，也不增加 OpenCV 依赖。依赖缺失时节点应在打开相机或创建文件前
+给出明确错误。
+
+## 最小代码范围
+
+预计只修改或新增以下位置：
+
+- `xr_rm_interfaces/msg/ActControlSample.msg`：原子消息；
+- `xr_rm_interfaces/CMakeLists.txt`、`package.xml`：生成并导出消息；
+- `xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py`：发布原子样本、跟踪夹爪
+  请求/成功状态和 A 键事件；
+- `xr_rm_teleop/xr_rm_teleop/fun_peripheral.py`：把请求的初始工具状态明确改为完全
+  打开；
+- `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：独立采集节点；
+- `xr_rm_teleop/setup.py`、`package.xml`：安装入口和 ROS 运行依赖声明；
+- `xr_rm_bringup/config/act_tomato_pick.yaml`：采集与硬件参数；
+- `xr_rm_bringup/config/peripherals_rm75.yaml`：只为右臂启用初始化打开；
+- `xr_rm_bringup/launch/arm_debug.launch.py`：默认关闭的 `record_act` 启动分支；
+- 现有测试目录中的最小相关测试。
+
+不改左臂/双臂运动参数，不改变 `left_arm_teleop`、`right_arm_teleop` 节点名，不创建
+第二个相机包、训练包或数据工具包。
+
+## 测试与验收
+
+### 自动化验证
+
+测试全部使用 mock、假适配器、合成相机帧和临时目录，不连接真机、不移动机械臂、
+不操作真实夹爪：
+
+- 原子消息中的 `q_actual`、QP 输出和最终 `q_target` 来自同一控制周期；
+- QP 失败时记录失败并保持上次目标，不自动拒绝；
+- 发送失败、夹爪失败和 A 键误触触发拒绝；
+- Trigger 请求立即改变 `action[7]`，成功返回后才改变 `qpos[7]`；
+- B/Y/Grip 的边沿、长按、忽略条件和所有状态转换正确；
+- 从首次有效样本开始按控制序号每 3 个周期取 1 个；
+- 相机只选择不晚于控制时间的最新帧，并能发现过期、偏斜和帧号异常；
+- Grip 中途暂停保留，最终松开到 B 的等待段正确裁剪；
+- HDF5 核心路径、形状、dtype、8 维顺序、根属性和 debug 字段正确；
+- 变量长度、最短/最长限制、质量拒绝、手动丢弃和 Ctrl+C 正确；
+- 编号从最大正式编号加一，拒绝不占号，已有文件不被覆盖；
+- 可读和不可读崩溃残留分别按设计恢复；
+- `record_act` 默认关闭，非法 arm/mock 组合被 launch 拒绝；
+- mock 模式不会导入或调用 RealMan SDK、RealSense 或 HDF5 采集链路。
+
+按照仓库要求，构建和测试从工作空间根目录执行：
+
+```bash
+cd /home/robot/WS_xr
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+pytest src/xr_rm_teleop/test/test_orientation_control.py
+```
+
+实施时还应运行新增测试及受影响的现有关节控制、初始位姿和外设配置测试。未看到
+实际通过输出前不得声称验证通过。
+
+### 真机手工验收
+
+真机验收必须由用户明确授权并在现有安全检查完成后进行，至少验证：
+
+1. 启动后右臂夹爪完全打开，状态未在命令成功前提前标记；
+2. 两台相机序列号和画面角色正确，持续 30 FPS 左右；
+3. B 开始、Grip 激活、B 结束、Y 丢弃和 A 误触拒绝符合状态机；
+4. 正常采摘文件包含完整抓取、搬运和释放，不包含 A 键回位；
+5. `qpos/action` 是有限的 `(T,8)` `float32`，图像是两路
+   `(T,480,640,3)` `uint8`；
+6. QP 短暂失败只增加 debug 计数，成功调整后仍可完成 episode；
+7. 重启后编号继续递增，丢弃和拒绝不会覆盖或占用正式编号；
+8. Ctrl+C、相机断流和磁盘不足产生带明确原因的拒绝文件；
+9. ACT 采集节点退出后，遥操安全链路仍按现有行为运行。
+
+## 后续训练与推理影响
+
+本设计生成 ALOHA/ACT 风格的核心数据，但原始 ACT 代码通常把状态维度硬编码为
+双臂 14 维，并可能假设固定 episode 长度。训练阶段需要单独适配：
+
+- `state_dim=8`；
+- 两个相机名 `cam_high`、`cam_right_wrist`；
+- 变量长度 episode 的 padding 和 mask；
+- `chunk_size≈60`；
+- 不使用 `action[t-1]` 旧补丁；
+- 忽略 `/debug`，除非用于筛选或诊断。
+
+实时推理只负责从初始位姿执行一次采摘到释放。回初始位姿继续调用当前确定性的
+`rm_movej`，由未来的上层任务状态机协调，避免让一个低层策略混合两种动作接口和
+任务阶段。

docs/superpowers/specs/2026-08-13-act-camera-preview-alignment-design.md

@@ -0,0 +1,218 @@
+# ACT 双相机预览与采样质量判定修正设计
+
+## 背景
+
+右臂番茄采摘 ACT 采集当前以 `90 Hz` 接收原子控制消息，每三个控制周期生成一个
+`30 Hz` 样本，并为该样本选择不晚于控制时刻的最新全局 D455 和右腕 D405 RGB
+帧。现阶段测试暴露出两个相机相关问题：
+
+1. ACT 采样帧号偶尔出现 `986 → 986 → 988` 一类重复和跨帧，episode 因
+   `camera_sample_drop_ratio` 被拒绝；
+2. ACT 数采启动后没有现场预览，操作者不能直观看到两路画面、相机质量和录制
+   状态。
+
+已保存 HDF5 的调查结果表明，被 `camera_sample_drop_ratio` 拒绝的 episode 中，
+相机采集线程统计的 `camera_high_drop_ratio` 和
+`camera_right_wrist_drop_ratio` 均为 `0.0`。异常主要出现在独立运行的两个
+`30 Hz` 时钟之间：控制采样早于下一张相机帧时会再次选择上一帧，下一个采样点
+则可能选择更新两号的帧。相机采集线程实际收到过中间帧，因此这不是 RealSense
+传输丢帧。
+
+## 目标
+
+- 保持现有因果时间对齐，不选择控制时刻之后的图像；
+- 只用相机采集线程观察到的真实帧号缺失率判定相机丢帧；
+- 将 ACT 样本中的重复帧和跨帧改为可观测指标，不再据此拒绝 episode；
+- `record_act:=true` 启动录制器时默认显示全局 D455 和右腕 D405 实时画面；
+- 在预览中显示相机质量和 ACT 录制状态；
+- 预览关闭或显示故障不得影响相机采集、HDF5 写入和机器人控制。
+
+## 非目标
+
+本次不实现：
+
+- 原始视频流保存和离线重采样；
+- D455 与 D405 硬件同步；
+- ROS 图像话题、Web 界面或新的相机进程；
+- 深度图、点云、图像压缩或 HDF5 核心训练字段变更；
+- 关节曲线、机器人控制按钮或预览截图；
+- 夹爪实际开度反馈或 episode 终点裁剪逻辑修改；
+- 修改 `record_act` 的全局默认值；
+- 修改工作空间/圆柱限位、速度限制、指令超时、安全停止或真机连接行为。
+
+夹爪录制继续使用现有操作顺序：保持 Grip，按 Trigger 打开并等待打开命令完成，
+然后松开 Grip，最后按 B 保存。
+
+## 方案选择
+
+### 采用：保持当前因果对齐
+
+数据流保持为：
+
+```text
+90 Hz ActControlSample
+        ↓ 每三个连续控制周期选择一次
+30 Hz ACT 目标时刻
+        ↓ 分别选择 host_monotonic_ns 不晚于目标时刻的最新帧
+D455 图像 + D405 图像 + qpos + action
+        ↓
+现有 HDF5
+```
+
+该方案维持现有训练数据语义，图像不会包含控制时刻之后的未来信息。少量重复帧和
+跨帧作为异步时钟相位漂移保留在数据中，并用明确指标量化。
+
+### 未采用：以 D455 为软件主时钟
+
+该方案可以避免 D455 重复帧，但会使控制序号间隔不再固定，并需要重新定义状态、
+动作和 D405 图像的对齐语义，当前收益不足以覆盖兼容性成本。
+
+### 未采用：保存原始流并离线重采样
+
+该方案最灵活，但需要新的原始存储结构和转换工具，无压缩双路 RGB 也会显著增加
+存储开销。只有后续训练表明快速接触或释放动作受到当前单帧级时间抖动影响时，才
+考虑升级。
+
+## 相机质量指标
+
+### 真实采集质量
+
+`CameraBuffer` 在每次收到 RealSense 帧时比较相邻原始帧号。原始帧号向前跳过的
+数量计入真实丢帧数；帧号不递增单独计为回退/重启异常。episode 期间的统计写入
+现有或新增 HDF5 属性：
+
+- `camera_high_fps`；
+- `camera_right_wrist_fps`；
+- `camera_high_drop_ratio`；
+- `camera_right_wrist_drop_ratio`；
+- `camera_high_frame_number_regression_count`；
+- `camera_right_wrist_frame_number_regression_count`。
+
+以下条件继续拒绝 episode：
+
+- 任一路实际采集 FPS 小于配置的 `min_camera_fps`，当前为 `27 Hz`；
+- 任一路真实丢帧率大于配置的 `max_drop_ratio`，当前为 `1%`；
+- 任一路图像帧龄超过 `max_camera_age_ms`，当前为 `50 ms`；
+- 两路所选图像的主机单调时间差超过 `max_camera_skew_ms`，当前为 `50 ms`；
+- 任一路原始帧号发生回退或重启；
+- 相机启动、取帧或图像格式发生错误。
+
+### ACT 采样相位指标
+
+对每路写入 HDF5 的 ACT 样本帧号计算相邻差值：
+
+```text
+diff == 0：重复使用同一相机帧
+diff == 1：理想连续取样
+diff > 1：相邻 ACT 样本跨过相机帧
+diff < 0：帧号回退，仍按异常拒绝
+```
+
+对 `N` 个 ACT 样本，分母为 `max(1, N - 1)`：
+
+```text
+sample_repeat_ratio = count(diff == 0) / max(1, N - 1)
+sample_skip_ratio   = count(diff > 1)  / max(1, N - 1)
+```
+
+分别写入：
+
+- `cam_high_sample_repeat_ratio`；
+- `cam_high_sample_skip_ratio`；
+- `cam_wrist_sample_repeat_ratio`；
+- `cam_wrist_sample_skip_ratio`。
+
+这些指标写入保存/拒绝文件属性，并在保存日志中摘要输出，但不参与 episode 接受
+判定。原有 `camera_sample_drop_ratio` 拒绝路径移除，避免把软件采样相位漂移误报
+为相机传输丢帧。
+
+## 双路实时预览
+
+### 生命周期
+
+`ActEpisodeRecorder` 成功启动两台相机后，默认启动一个独立的 OpenCV 预览线程。
+该线程只读取两个现有 `CameraBuffer` 的最新帧和统计，不打开新的 RealSense
+pipeline，也不发布 ROS 图像话题。
+
+预览约以 `10 Hz` 刷新，降低显示开销；相机采集和 HDF5 录制仍保持 `30 Hz`。
+节点退出时先通知并回收预览线程，再停止两台相机。关闭预览不会重新启动。
+
+`arm_debug.launch.py` 继续保持 `record_act:=false` 的安全默认值。tools 中现有 ACT
+数采入口已经显式传入 `arm:=right use_mock:=false record_act:=true`，因此无需修改
+launch 参数或增加新的预览开关；只要录制器节点启动，预览就默认启动。
+
+### 布局与信息
+
+窗口使用左右并排的两块画面：
+
+```text
+┌──────────────────────┬──────────────────────┐
+│ 全局 D455            │ 右腕 D405            │
+│ 实时画面             │ 实时画面             │
+│ FPS / 当前帧号       │ FPS / 当前帧号       │
+│ 接收数 / 真实丢帧率  │ 接收数 / 真实丢帧率  │
+├──────────────────────┴──────────────────────┤
+│ 两路帧龄 / 两相机时间差                     │
+│ ACT 状态 / episode 编号 / 已写入样本数      │
+└─────────────────────────────────────────────┘
+```
+
+实时 FPS 使用相机采集线程最近约两秒的到帧时间计算，而不是预览刷新率。未开始
+episode 时编号和样本数显示为空或 `-`；录制过程中读取当前录制器状态。
+
+### 关闭和异常处理
+
+- 按 `Q`、`Esc` 或点击窗口关闭按钮，只停止预览线程；
+- 没有 `DISPLAY` 和 `WAYLAND_DISPLAY` 时不创建窗口，只记录一次警告；
+- `cv2` 导入失败、窗口创建失败或显示过程中抛出异常时，记录一次警告并停止
+  预览；
+- 预览异常不改变 ACT 状态，不关闭相机，不丢弃或拒绝 episode；
+- RealSense 相机本身启动或采集失败仍沿用现有预检/拒绝行为；
+- 预览线程不得调用机器人适配器、发布夹爪命令或阻塞 ROS 控制样本回调。
+
+本次复用 `xr` 运行环境中现有的 OpenCV，不新增 Python 或 ROS 依赖。
+
+## 代码范围
+
+预计只修改：
+
+- `xr_rm_teleop/xr_rm_teleop/act_episode_recorder.py`：真实丢帧与采样相位指标、
+  双路预览及生命周期；
+- `xr_rm_teleop/test/test_act_episode_recorder.py`：质量判定和预览失败隔离测试；
+- `README.md`：补充 ACT 默认双路预览及关闭方式。
+
+无需修改 `arm_debug.launch.py`、`launcher_ui.py`、ROS 消息、相机配置或机器人控制
+节点。实现继续保留在现有录制器文件中，只提取必要的纯计算/渲染辅助函数，不创建
+通用相机框架。
+
+## 测试与验证
+
+自动化测试不连接 RealSense、RM75 或真实夹爪：
+
+1. 构造 `986 → 986 → 988`，验证重复率和跨帧率均被记录，episode 不再因
+   `camera_sample_drop_ratio` 被拒绝；
+2. 在 `CameraBuffer` 原始输入中跳过帧号，验证真实丢帧数和丢帧率仍触发拒绝；
+3. 构造原始帧号回退，验证 episode 被拒绝；
+4. 验证两路采样指标分别计算，且分母在单样本时安全；
+5. 使用替代显示函数验证按键关闭、窗口关闭、无显示环境和显示异常只停用预览，
+   不改变录制状态；
+6. 运行 `xr_rm_teleop/test/test_act_episode_recorder.py`；
+7. 从 `/home/robot/WS_xr` 执行：
+
+   ```bash
+   source /opt/ros/humble/setup.bash
+   colcon build --symlink-install
+   ```
+
+现场再通过现有 ACT 数采入口验证窗口布局、画面刷新和关闭行为。该验证会连接右臂
+真机，必须由用户明确执行；自动化过程不得启动真机 launch 或移动机器人。
+
+## 完成标准
+
+- 真实相机丢帧、帧龄超限、双相机偏差和相机错误仍能拒绝不合格 episode；
+- 正常异步相位漂移造成的样本重复/跨帧不再拒绝 episode；
+- HDF5 和日志能够区分真实丢帧与 ACT 采样相位指标；
+- ACT 数采启动时默认出现全局 D455 与右腕 D405 双路预览；
+- 关闭或损坏预览不影响 ACT 采集；
+- 现有 HDF5 核心结构、机器人控制和安全行为保持不变；
+- 相关测试与工作空间构建通过。

xr_rm_bringup/config/act_tomato_pick.yaml

@@ -0,0 +1,33 @@
+act_episode_recorder:
+  ros__parameters:
+    output_root: /home/robot/ACT_Data
+    task_name: tomato_pick
+    control_sample_topic: /xr_rm/right_rm75/act_control_sample
+    right_controller_topic: /xr/right_controller
+    left_controller_topic: /xr/left_controller
+    status_topic: /act/recording_status
+
+    cam_high_serial: "234222303366"
+    cam_high_model: D455
+    cam_right_wrist_serial: "412622272532"
+    cam_right_wrist_model: D405
+    image_width: 640
+    image_height: 480
+    camera_fps: 30
+    camera_warmup_sec: 5.0
+
+    control_rate_hz: 90.0
+    sample_rate_hz: 30.0
+    min_samples: 60
+    max_samples: 1800
+    min_control_hz: 27.0
+    max_control_gap_ms: 100.0
+    min_camera_fps: 27.0
+    max_drop_ratio: 0.01
+    max_feedback_age_ms: 50.0
+    max_camera_age_ms: 50.0
+    max_camera_skew_ms: 50.0
+    min_free_space_gib: 4.0
+    y_hold_sec: 1.0
+    gripper_completion_timeout_sec: 3.0
+    writer_queue_size: 8

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

@@ -16,23 +16,23 @@ left_arm_teleop:
     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.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
+    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]。
@@ -54,14 +54,14 @@ left_arm_teleop:
     enable_trigger_gripper_control: true
     trigger_close_threshold: 0.95
     configure_peripheral_on_connect: true
-    max_line_speed: 1.0
-    max_angular_speed: 1.5
-    max_line_acc: 1.0
-    max_angular_acc: 2.0
+    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
 
@@ -73,23 +73,23 @@ right_arm_teleop:
     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.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
+    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]。
@@ -102,7 +102,7 @@ right_arm_teleop:
     realtime_push_host_ip: 192.168.192.148
     realtime_push_port: 8090
     realtime_push_cycle_ms: 5
-    avoid_singularity: 1
+    avoid_singularity: 0
     follow: false
     canfd_trajectory_mode: 2
     canfd_radio: 0
@@ -111,13 +111,13 @@ right_arm_teleop:
     enable_trigger_gripper_control: true
     trigger_close_threshold: 0.95
     configure_peripheral_on_connect: true
-    max_line_speed: 1.0
-    max_angular_speed: 1.5
-    max_line_acc: 1.0
-    max_angular_acc: 2.0
+    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

@@ -10,23 +10,23 @@ single_arm_velocity_teleop:
     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.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
+    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,
@@ -47,13 +47,13 @@ single_arm_velocity_teleop:
     enable_trigger_gripper_control: true
     trigger_close_threshold: 0.95
     configure_peripheral_on_connect: true
-    max_line_speed: 1.0
-    max_angular_speed: 1.5
-    max_line_acc: 1.0
-    max_angular_acc: 2.0
+    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: [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52]
+    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,7 @@ tools_in_ee:
 
 arms:
   left:
-    scissorgripper: 2
+    scissorgripper: 0
   right:
     scissorgripper: 1
+    set_initial_tool_state: true

xr_rm_bringup/config/right_arm_rm75.yaml

@@ -22,7 +22,7 @@ single_arm_velocity_teleop:
     orientation_filter_alpha: 0.65
     max_orientation_speed: 0.5
     workspace_min: [-0.70, -0.70, 0.10]
-    workspace_max: [0.70, 0.70, 0.75]
+    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
@@ -37,7 +37,7 @@ single_arm_velocity_teleop:
     realtime_push_host_ip: 192.168.192.148
     realtime_push_port: 8090
     realtime_push_cycle_ms: 5
-    avoid_singularity: 1
+    avoid_singularity: 0
     # 厂商 MovejCANFD 示例默认低跟随；高跟随仅在验证规划轨迹后单独开启。
     follow: false
     canfd_trajectory_mode: 2
@@ -54,6 +54,6 @@ single_arm_velocity_teleop:
     joint_max_speed: 180.0
     joint_max_acc: 300.0
     move_to_initial_pose_on_connect: false
-    initial_joint_pose: [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04]
+    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

@@ -8,7 +8,7 @@
 from pathlib import Path
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -31,13 +31,12 @@ def _config_file(name: str) -> PathJoinSubstitution:
     ])
 
 
-def _rm75_urdf() -> PathJoinSubstitution:
+def _dual_rm75_urdf() -> PathJoinSubstitution:
     return PathJoinSubstitution([
         FindPackageShare("xr_rm_teleop"),
         "models",
-        "rm75_omnipicker",
-        "urdf",
-        "RM75-B_OmniPicker_fixed.urdf",
+        "dual_rm75",
+        "Dual_arm.urdf",
     ])
 
 
@@ -48,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"),
@@ -58,6 +60,49 @@ 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 _validate_act_mode(
+    arm: str,
+    use_mock: bool,
+    record_act: bool,
+) -> None:
+    if record_act and (arm != "right" or use_mock):
+        raise ValueError(
+            "record_act:=true requires arm:=right use_mock:=false"
+        )
+
+
+def _act_recorder_node() -> Node:
+    """创建独立 ACT 数据采集节点；退出时不终止遥操作。"""
+    return Node(
+        package="xr_rm_teleop",
+        executable="act_episode_recorder",
+        name="act_episode_recorder",
+        output="screen",
+        prefix=[XR_PYTHON],
+        parameters=[_config_file("act_tomato_pick.yaml")],
+    )
+
+
 def _single_arm_node(
     arm: str,
     use_mock: bool,
@@ -75,7 +120,7 @@ def _single_arm_node(
             _config_file(config_name),
             {
                 "use_mock": use_mock,
-                "robot_urdf_path": _rm75_urdf(),
+                "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",
@@ -102,7 +147,7 @@ def _dual_arm_nodes(use_mock: bool) -> list[Node]:
                 config_file,
                 {
                     "use_mock": use_mock,
-                    "robot_urdf_path": _rm75_urdf(),
+                    "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",
@@ -119,7 +164,7 @@ def _dual_arm_nodes(use_mock: bool) -> list[Node]:
                 config_file,
                 {
                     "use_mock": use_mock,
-                    "robot_urdf_path": _rm75_urdf(),
+                    "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",
@@ -139,15 +184,27 @@ def _launch_setup(context, *args, **kwargs):
         )
     arm = LaunchConfiguration("arm").perform(context).strip().lower()
     use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
+    use_mujoco = _as_bool(
+        LaunchConfiguration("use_mujoco").perform(context)
+    )
+    record_act = _as_bool(
+        LaunchConfiguration("record_act").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)
+    _validate_act_mode(arm, use_mock, record_act)
 
     nodes = [_udp_receiver_node()]
     if arm == "both":
         nodes.extend(_dual_arm_nodes(use_mock))
     else:
         nodes.append(_single_arm_node(arm, use_mock))
+    if use_mujoco:
+        nodes.append(_mujoco_node())
+    if record_act:
+        nodes.append(_act_recorder_node())
     return nodes
 
 
@@ -157,6 +214,10 @@ 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"),
+        # true 时只允许右臂真机，并启动独立 ACT 数据采集节点。
+        DeclareLaunchArgument("record_act", default_value="false"),
         # UDP 监听参数，需要与 PICO 端或 sample_udp_sender 保持一致。
         DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
         DeclareLaunchArgument("udp_port", default_value="15000"),

xr_rm_bringup/package.xml

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

xr_rm_bringup/test/test_arm_debug_launch.py

@@ -0,0 +1,77 @@
+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)
+
+
+def test_act_recording_is_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 perform_substitutions(
+        LaunchContext(),
+        arguments["record_act"].default_value,
+    ) == "false"
+
+
+@pytest.mark.parametrize(
+    ("arm", "use_mock"),
+    [("left", False), ("both", False), ("right", True)],
+)
+def test_act_recording_rejects_unsupported_modes(arm, use_mock) -> None:
+    with pytest.raises(ValueError, match="arm:=right use_mock:=false"):
+        arm_debug_launch._validate_act_mode(arm, use_mock, True)
+
+
+def test_act_recording_accepts_right_real_mode() -> None:
+    arm_debug_launch._validate_act_mode("right", False, True)
+    arm_debug_launch._validate_act_mode("both", True, False)
+
+
+def test_act_recorder_exit_does_not_shutdown_teleoperation() -> None:
+    recorder = arm_debug_launch._act_recorder_node()
+
+    assert recorder._ExecuteLocal__on_exit is None

xr_rm_bringup/test/test_launcher_ui_cleanup.py

@@ -13,6 +13,124 @@ 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",
+            ],
+            "ACT Data Collection": [
+                "Right Arm ACT Data Collection Launch",
+                "XRobotoolkit UDP Bridge (90 Hz)",
+                "Open ACT Recording Status",
+                "Open Right Arm ACT Control Sample Hz",
+                "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_act_mode_uses_confirmed_right_hardware_topics(self) -> None:
+        commands = dict(launcher_ui.build_commands_by_mode("ACT Data Collection"))
+
+        self.assertIn(
+            "arm:=right use_mock:=false record_act:=true",
+            commands["1. Right Arm ACT Data Collection Launch"],
+        )
+        self.assertEqual(
+            commands["3. Open ACT Recording Status"],
+            "ros2 topic echo /act/recording_status",
+        )
+        self.assertEqual(
+            commands["4. Open Right Arm ACT Control Sample Hz"],
+            "ros2 topic hz /xr_rm/right_rm75/act_control_sample",
+        )
+
+    def test_cmd_vel_monitor_is_completely_removed(self) -> None:
+        self.assertFalse(hasattr(launcher_ui, "CMD_VEL_MONITOR_ACTION"))
+        for mode in launcher_ui.MODES:
+            self.assertNotIn("cmd_vel", repr(launcher_ui.build_commands_by_mode(mode)))
+
+    def test_environment_check_includes_mujoco_package(self) -> None:
+        app = object.__new__(launcher_ui.LauncherApp)
+        app.workspace_root = launcher_ui._find_workspace_root()
+        app.terminal_command = lambda _title, _script: ["terminal"]
+        app.x_terminal_target = lambda: "terminator"
+        checked_packages = []
+        app._ros_package_available = lambda package: checked_packages.append(package) or True
+        app.show_text_dialog = lambda *_args: None
+        app.status = mock.Mock()
+
+        with mock.patch.object(
+            launcher_ui.shutil,
+            "which",
+            return_value="/usr/bin/x-terminal-emulator",
+        ):
+            app.check_prerequisites()
+
+        self.assertEqual(
+            checked_packages,
+            ["xr_rm_bringup", "xr_rm_input", "xr_rm_teleop", "xr_rm_mujoco"],
+        )
+
+
 class LauncherCleanupTest(unittest.TestCase):
     def test_xrobotoolkit_cleanup_policy_only_stops_service_on_window_close(self) -> None:
         stop_all_patterns = set(
@@ -89,6 +207,34 @@ class LauncherCleanupTest(unittest.TestCase):
             ],
         )
 
+    def test_stop_all_and_window_close_stop_mujoco(self) -> None:
+        for stop_pc_service in (False, True):
+            app = object.__new__(launcher_ui.LauncherApp)
+            app.status = mock.Mock()
+            app.close_related_terminal_windows = lambda: 0
+
+            def fake_check_output(command, **_kwargs):
+                if command == ["pgrep", "-f", "dual_arm_simulator"]:
+                    return "404\n"
+                raise subprocess.CalledProcessError(1, command)
+
+            with (
+                mock.patch.object(
+                    launcher_ui.subprocess,
+                    "check_output",
+                    side_effect=fake_check_output,
+                ),
+                mock.patch.object(launcher_ui.os, "kill") as kill,
+                mock.patch.object(launcher_ui.time, "sleep"),
+            ):
+                app.stop_launched_processes(
+                    confirm=False,
+                    notify=False,
+                    stop_pc_service=stop_pc_service,
+                )
+
+            kill.assert_called_once_with(404, signal.SIGTERM)
+
 
 if __name__ == "__main__":
     unittest.main()

xr_rm_bringup/test/test_realsense_multi_camera.py

@@ -0,0 +1,76 @@
+from __future__ import annotations
+
+import importlib.util
+from pathlib import Path
+import sys
+
+import pytest
+
+
+MODULE_PATH = (
+    Path(__file__).resolve().parents[1]
+    / "tools"
+    / "realsense_multi_camera_test.py"
+)
+SPEC = importlib.util.spec_from_file_location("realsense_multi_camera_test", MODULE_PATH)
+assert SPEC is not None and SPEC.loader is not None
+camera_test = importlib.util.module_from_spec(SPEC)
+sys.modules[SPEC.name] = camera_test
+SPEC.loader.exec_module(camera_test)
+
+
+def devices() -> list[camera_test.DeviceInfo]:
+    return [
+        camera_test.DeviceInfo("Intel RealSense D405", "D405", "412622272532", "3.2"),
+        camera_test.DeviceInfo("Intel RealSense D455", "D455", "234222303366", "3.2"),
+        camera_test.DeviceInfo("Intel RealSense D405", "D405", "260322272273", "3.2"),
+    ]
+
+
+def test_assigns_camera_roles_with_and_without_left_serial() -> None:
+    unidentified = camera_test.assign_camera_roles(devices(), None)
+    assert [camera.role for camera in unidentified] == ["GLOBAL", "D405-A", "D405-B"]
+    assert [camera.serial for camera in unidentified[1:]] == ["260322272273", "412622272532"]
+
+    identified = camera_test.assign_camera_roles(devices(), "412622272532")
+    assert {camera.role: camera.serial for camera in identified} == {
+        "GLOBAL": "234222303366",
+        "LEFT": "412622272532",
+        "RIGHT": "260322272273",
+    }
+
+
+def test_rejects_invalid_camera_selection() -> None:
+    with pytest.raises(ValueError, match="左臂序列号"):
+        camera_test.assign_camera_roles(devices(), "missing")
+
+    with pytest.raises(ValueError, match="2 台 D405 和 1 台 D455"):
+        camera_test.assign_camera_roles(devices()[:-1], None)
+
+
+def test_counts_frame_number_gaps() -> None:
+    stats = camera_test.FrameStats(target_fps=30, start_time=0.0)
+    stats.update(10, 0.0)
+    stats.update(11, 1.0 / 30.0)
+    stats.update(14, 2.0 / 30.0)
+
+    assert stats.received == 3
+    assert stats.dropped == 2
+    assert stats.drop_rate == pytest.approx(0.4)
+    assert stats.average_fps(2.0 / 30.0) == pytest.approx(30.0)
+
+
+def test_snapshot_names_follow_camera_roles() -> None:
+    identified = camera_test.assign_camera_roles(devices(), "412622272532")
+    assert camera_test.snapshot_filenames(identified) == {
+        "234222303366": "global.png",
+        "412622272532": "left.png",
+        "260322272273": "right.png",
+    }
+
+    unidentified = camera_test.assign_camera_roles(devices(), None)
+    assert camera_test.snapshot_filenames(unidentified) == {
+        "234222303366": "global.png",
+        "260322272273": "d405_260322272273.png",
+        "412622272532": "d405_412622272532.png",
+    }

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/真机/ACT采集/诊断”组织常用 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,9 @@ ROS_GRAPH_MONITORS = [
 
 MODES = [
     "Simulation",
-    "Left Arm",
-    "Right Arm",
-    "Dual Arm",
+    "MuJoCo",
+    "Real Hardware",
+    "ACT Data Collection",
     "Diagnostics",
 ]
 
@@ -177,21 +170,6 @@ def _source_lines(workspace_root: Path) -> list[str]:
     return lines
 
 
-def _one_click_mock(arm: str, hand: str) -> str:
-    sender_seconds = SAMPLE_SENDER_STAGGERED_SECONDS if hand == "both" else SAMPLE_SENDER_SECONDS
-    both_mode = "staggered" if hand == "both" else "synchronized"
-    return "\n".join([
-        f"ros2 launch xr_rm_bringup arm_debug.launch.py arm:={arm} use_mock:=true &",
-        "launch_pid=$!",
-        "sleep 2",
-        _sample_udp_sender_command(hand, sender_seconds, both_mode),
-        "echo",
-        "echo 'Sample sender finished. The launch process is still running in this terminal.'",
-        "echo 'Press Ctrl-C here, or use the cleanup button in the launcher, to stop it.'",
-        "wait \"$launch_pid\"",
-    ])
-
-
 def _diagnostic_commands() -> list[tuple[str, str]]:
     return [
         ("Open ROS Topic/Node List Monitor", ROS_GRAPH_MONITOR_ACTION),
@@ -202,14 +180,9 @@ def _topic_monitor_item() -> tuple[str, str]:
     return ("Open Controller Topic Monitor", TOPIC_MONITOR_ACTION)
 
 
-def _cmd_vel_monitor_item() -> tuple[str, str]:
-    return ("Open Target Velocity Monitor", CMD_VEL_MONITOR_ACTION)
-
-
 def _is_topic_monitor_action(action: str) -> bool:
     return action in (
         TOPIC_MONITOR_ACTION,
-        CMD_VEL_MONITOR_ACTION,
         CONTROLLER_POSITION_MONITOR_ACTION,
         CONTROLLER_HZ_MONITOR_ACTION,
     )
@@ -232,14 +205,6 @@ def _topic_monitor_spec(action: str) -> tuple[str, list[tuple[str, str]], str, s
             "xr_rm_controller_hz_monitor_",
             "controller hz topic",
         )
-    if action == CMD_VEL_MONITOR_ACTION:
-        return (
-            CMD_VEL_MONITOR_TITLE,
-            [(title, f"ros2 topic echo {topic}") for title, topic in CMD_VEL_MONITORS],
-            "xr_rm_cmd_vel_monitor",
-            "xr_rm_cmd_vel_monitor_",
-            "target velocity topic",
-        )
     return (
         TOPIC_MONITOR_TITLE,
         [(title, f"ros2 topic echo {topic}") for title, topic in TOPIC_MONITORS],
@@ -249,16 +214,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)
 
 
@@ -266,25 +225,81 @@ 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)),
+        ]
+    elif mode == "ACT Data Collection":
+        items = [
+            (
+                "Right Arm ACT Data Collection Launch",
+                "ros2 launch xr_rm_bringup arm_debug.launch.py "
+                "arm:=right use_mock:=false record_act:=true",
+            ),
+            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
+            (
+                "Open ACT Recording Status",
+                "ros2 topic echo /act/recording_status",
+            ),
+            (
+                "Open Right Arm ACT Control Sample Hz",
+                "ros2 topic hz /xr_rm/right_rm75/act_control_sample",
+            ),
+        ]
+    else:
+        items = [
+            ("ROS Doctor Report", "ros2 doctor --report"),
+            ("XR-RM Bringup Prefix", "ros2 pkg prefix xr_rm_bringup"),
+            ("XR-RM Input Prefix", "ros2 pkg prefix xr_rm_input"),
+            ("XR-RM Teleop Prefix", "ros2 pkg prefix xr_rm_teleop"),
+            ("XR-RM MuJoCo Prefix", "ros2 pkg prefix xr_rm_mujoco"),
             (
                 "Open Controller Position Monitor",
                 CONTROLLER_POSITION_MONITOR_ACTION,
@@ -294,64 +309,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",
-            ),
-            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
-            ("Left Tool Open", _tool_command("left", True)),
-            ("Left Tool Close", _tool_command("left", False)),
-            (
-                "Sample UDP Sender (Left, 30s)",
-                _sample_udp_sender_command("left"),
-            ),
-        ]
-        one_click = None
-    elif mode == "Right Arm":
-        items = [
-            ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
-            (
-                "Right Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false",
-            ),
-            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
-            ("Right Tool Open", _tool_command("right", True)),
-            ("Right Tool Close", _tool_command("right", False)),
-            (
-                "Sample UDP Sender (Right, 30s)",
-                _sample_udp_sender_command("right"),
-            ),
-        ]
-        one_click = None
-    elif mode == "Dual Arm":
-        items = [
-            ("Ping Left RM75", f"ping -c 4 {DEFAULT_LEFT_IP}"),
-            ("Ping Right RM75", f"ping -c 4 {DEFAULT_RIGHT_IP}"),
-            (
-                "Dual Arm RealMan Launch",
-                "ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false",
-            ),
-            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
-            (
-                "Sample UDP Sender (Both Staggered, 60s)",
-                _sample_udp_sender_command("both", SAMPLE_SENDER_STAGGERED_SECONDS, "staggered"),
-            ),
-        ]
-        one_click = None
-    else:
-        items = [
-            ("ROS Doctor Report", "ros2 doctor --report"),
-            ("XR-RM Bringup Prefix", "ros2 pkg prefix xr_rm_bringup"),
-            ("XR-RM Input Prefix", "ros2 pkg prefix xr_rm_input"),
-            ("XR-RM Teleop Prefix", "ros2 pkg prefix xr_rm_teleop"),
-            ("XRobotoolkit UDP Bridge (90 Hz)", _xrobotoolkit_bridge_command()),
-        ]
-        one_click = None
-    return _finalize_items(items, one_click)
+    return _finalize_items(items)
 
 
 class LauncherApp:
@@ -389,7 +347,7 @@ class LauncherApp:
             mode_frame,
             textvariable=self.mode_var,
             values=MODES,
-            width=16,
+            width=20,
             state="readonly",
         )
         self.mode_combo.grid(row=0, column=1, sticky="ew")
@@ -608,7 +566,7 @@ class LauncherApp:
         else:
             warnings.append("[WARN] Could not identify x-terminal-emulator target.")
 
-        for package in ("xr_rm_bringup", "xr_rm_input", "xr_rm_teleop"):
+        for package in ("xr_rm_bringup", "xr_rm_input", "xr_rm_teleop", "xr_rm_mujoco"):
             if self._ros_package_available(package):
                 ok.append(f"[OK] ROS package available: {package}")
             else:
@@ -1254,7 +1212,6 @@ class LauncherApp:
         title_patterns = (
             TERMINAL_TITLE_PREFIX,
             TOPIC_MONITOR_TITLE,
-            CMD_VEL_MONITOR_TITLE,
             CONTROLLER_POSITION_MONITOR_TITLE,
             CONTROLLER_HZ_MONITOR_TITLE,
             ROS_GRAPH_MONITOR_TITLE,
@@ -1326,24 +1283,21 @@ class LauncherApp:
             *_xrobotoolkit_cleanup_patterns(stop_pc_service=stop_pc_service),
             TERMINAL_TITLE_PREFIX,
             TOPIC_MONITOR_TITLE,
-            CMD_VEL_MONITOR_TITLE,
             ROS_GRAPH_MONITOR_TITLE,
             "xr_rm_topic_monitor_",
-            "xr_rm_cmd_vel_monitor_",
             "xr_rm_controller_position_monitor_",
             "xr_rm_controller_hz_monitor_",
             "xr_rm_ros_graph_monitor_",
             "udp_controller_receiver",
             "sample_udp_sender",
             "single_arm_velocity_teleop",
+            "dual_arm_simulator",
             "ros2 topic echo /xr/left_controller --field pose.position",
             "ros2 topic echo /xr/right_controller --field pose.position",
             "ros2 topic echo /xr/left_controller",
             "ros2 topic echo /xr/right_controller",
             "ros2 topic hz /xr/left_controller",
             "ros2 topic hz /xr/right_controller",
-            "ros2 topic echo /xr_rm/left_rm75/cmd_vel",
-            "ros2 topic echo /xr_rm/right_rm75/cmd_vel",
             "ros2 topic list",
             "ros2 node list",
         ]

xr_rm_bringup/tools/realsense_multi_camera_test.py

@@ -0,0 +1,494 @@
+#!/usr/bin/env python3
+"""同时预览并检查两台 D405 和一台 D455 的彩色画面。"""
+
+from __future__ import annotations
+
+import argparse
+from collections import deque
+from dataclasses import dataclass, field
+from datetime import datetime
+from pathlib import Path
+import threading
+import time
+from typing import Any
+
+
+DEFAULT_WIDTH = 640
+DEFAULT_HEIGHT = 480
+DEFAULT_FPS = 30
+DEFAULT_LEFT_SERIAL = "260322272273"
+FPS_WINDOW_SECONDS = 2.0
+WARMUP_SECONDS = 5.0
+MAX_DROP_RATE = 0.01
+MIN_FPS_RATIO = 0.9
+PREVIEW_TILE_WIDTH = 640
+WINDOW_NAME = "XR RM - Three RealSense Camera Test"
+OUTPUT_DIR = Path(__file__).resolve().parents[1] / "test" / "camera_test_output"
+
+
+@dataclass(frozen=True)
+class DeviceInfo:
+    name: str
+    model: str
+    serial: str
+    usb_type: str
+
+
+@dataclass(frozen=True)
+class CameraAssignment:
+    role: str
+    name: str
+    model: str
+    serial: str
+    usb_type: str
+
+
+def assign_camera_roles(
+    devices: list[DeviceInfo], left_serial: str | None
+) -> list[CameraAssignment]:
+    d405 = sorted(
+        (device for device in devices if device.model == "D405"),
+        key=lambda device: device.serial,
+    )
+    d455 = [device for device in devices if device.model == "D455"]
+    if len(d405) != 2 or len(d455) != 1 or len(devices) != 3:
+        raise ValueError(
+            f"需要连接 2 台 D405 和 1 台 D455，当前识别到 "
+            f"{len(d405)} 台 D405、{len(d455)} 台 D455、共 {len(devices)} 台 RealSense"
+        )
+
+    for device in devices:
+        if not device.usb_type.startswith("3"):
+            raise ValueError(
+                f"{device.model} ({device.serial}) 当前为 USB {device.usb_type}，"
+                "请检查扩展坞和数据线"
+            )
+
+    global_camera = CameraAssignment("GLOBAL", **d455[0].__dict__)
+    if left_serial is None:
+        arms = [
+            CameraAssignment(f"D405-{suffix}", **device.__dict__)
+            for suffix, device in zip(("A", "B"), d405)
+        ]
+    else:
+        matches = [device for device in d405 if device.serial == left_serial]
+        if not matches:
+            raise ValueError(f"左臂序列号 {left_serial} 不属于当前连接的 D405")
+        left = matches[0]
+        right = next(device for device in d405 if device.serial != left_serial)
+        arms = [
+            CameraAssignment("LEFT", **left.__dict__),
+            CameraAssignment("RIGHT", **right.__dict__),
+        ]
+    return [global_camera, *arms]
+
+
+def snapshot_filenames(assignments: list[CameraAssignment]) -> dict[str, str]:
+    role_names = {
+        "GLOBAL": "global.png",
+        "LEFT": "left.png",
+        "RIGHT": "right.png",
+    }
+    return {
+        camera.serial: role_names.get(camera.role, f"d405_{camera.serial}.png")
+        for camera in assignments
+    }
+
+
+@dataclass
+class FrameStats:
+    target_fps: int
+    start_time: float
+    received: int = 0
+    dropped: int = 0
+    last_frame_number: int | None = None
+    first_frame_time: float | None = None
+    recent_times: deque[float] = field(default_factory=deque)
+
+    def update(self, frame_number: int, now: float) -> None:
+        if self.last_frame_number is not None and frame_number > self.last_frame_number:
+            self.dropped += max(0, frame_number - self.last_frame_number - 1)
+        self.last_frame_number = frame_number
+        self.received += 1
+        if self.first_frame_time is None:
+            self.first_frame_time = now
+        self.recent_times.append(now)
+        cutoff = now - FPS_WINDOW_SECONDS
+        while self.recent_times and self.recent_times[0] < cutoff:
+            self.recent_times.popleft()
+
+    @property
+    def drop_rate(self) -> float:
+        expected = self.received + self.dropped
+        return self.dropped / expected if expected else 0.0
+
+    @property
+    def rolling_fps(self) -> float:
+        if len(self.recent_times) < 2:
+            return 0.0
+        elapsed = self.recent_times[-1] - self.recent_times[0]
+        return (len(self.recent_times) - 1) / elapsed if elapsed > 0 else 0.0
+
+    def average_fps(self, now: float) -> float:
+        if self.received < 2 or self.first_frame_time is None:
+            return 0.0
+        last_frame_time = self.recent_times[-1] if self.recent_times else now
+        elapsed = last_frame_time - self.first_frame_time
+        return (self.received - 1) / elapsed if elapsed > 0 else 0.0
+
+
+@dataclass(frozen=True)
+class CameraState:
+    frame: Any | None
+    actual_size: tuple[int, int]
+    received: int
+    dropped: int
+    drop_rate: float
+    rolling_fps: float
+    average_fps: float
+    elapsed: float
+    error: str
+
+
+class CameraWorker:
+    def __init__(
+        self,
+        assignment: CameraAssignment,
+        width: int,
+        height: int,
+        fps: int,
+        rs: Any,
+        np: Any,
+    ) -> None:
+        self.assignment = assignment
+        self.width = width
+        self.height = height
+        self.fps = fps
+        self._rs = rs
+        self._np = np
+        self._pipeline = rs.pipeline()
+        self._stop_event = threading.Event()
+        self._lock = threading.Lock()
+        self._thread: threading.Thread | None = None
+        self._frame: Any | None = None
+        self._actual_size = (width, height)
+        self._stats = FrameStats(fps, time.monotonic())
+        self._error = ""
+
+    def start(self) -> None:
+        config = self._rs.config()
+        config.enable_device(self.assignment.serial)
+        config.enable_stream(
+            self._rs.stream.color,
+            self.width,
+            self.height,
+            self._rs.format.bgr8,
+            self.fps,
+        )
+        profile = self._pipeline.start(config)
+        video_profile = profile.get_stream(self._rs.stream.color).as_video_stream_profile()
+        with self._lock:
+            self._actual_size = (video_profile.width(), video_profile.height())
+            self._stats = FrameStats(self.fps, time.monotonic())
+        self._thread = threading.Thread(
+            target=self._capture_loop,
+            name=f"camera-{self.assignment.serial}",
+            daemon=True,
+        )
+        self._thread.start()
+
+    def _capture_loop(self) -> None:
+        while not self._stop_event.is_set():
+            try:
+                frames = self._pipeline.wait_for_frames(timeout_ms=1000)
+            except RuntimeError as exc:
+                if self._stop_event.is_set():
+                    return
+                with self._lock:
+                    self._error = str(exc)
+                return
+
+            color_frame = frames.get_color_frame()
+            if not color_frame:
+                continue
+            frame = self._np.asanyarray(color_frame.get_data()).copy()
+            now = time.monotonic()
+            with self._lock:
+                self._frame = frame
+                self._stats.update(color_frame.get_frame_number(), now)
+
+    def state(self, now: float) -> CameraState:
+        with self._lock:
+            return CameraState(
+                frame=self._frame,
+                actual_size=self._actual_size,
+                received=self._stats.received,
+                dropped=self._stats.dropped,
+                drop_rate=self._stats.drop_rate,
+                rolling_fps=self._stats.rolling_fps,
+                average_fps=self._stats.average_fps(now),
+                elapsed=now - self._stats.start_time,
+                error=self._error,
+            )
+
+    def stop(self) -> None:
+        self._stop_event.set()
+        if self._thread is not None:
+            self._thread.join(timeout=1.2)
+        try:
+            self._pipeline.stop()
+        except RuntimeError:
+            pass
+        if self._thread is not None and self._thread.is_alive():
+            self._thread.join(timeout=1.0)
+
+
+def load_runtime_dependencies() -> tuple[Any, Any, Any]:
+    try:
+        import cv2
+        import numpy as np
+        import pyrealsense2 as rs
+    except ImportError as exc:
+        raise RuntimeError(
+            "缺少相机测试依赖。请使用 /home/robot/miniconda3/envs/xr/bin/python "
+            "运行，并确认 xr 环境已安装 pyrealsense2、numpy 和 opencv-python。"
+        ) from exc
+    return cv2, np, rs
+
+
+def enumerate_devices(rs: Any) -> list[DeviceInfo]:
+    devices = []
+    for device in rs.context().query_devices():
+        name = device.get_info(rs.camera_info.name)
+        if "D405" in name:
+            model = "D405"
+        elif "D455" in name:
+            model = "D455"
+        else:
+            model = name
+        usb_type = (
+            device.get_info(rs.camera_info.usb_type_descriptor)
+            if device.supports(rs.camera_info.usb_type_descriptor)
+            else "unknown"
+        )
+        devices.append(
+            DeviceInfo(
+                name=name,
+                model=model,
+                serial=device.get_info(rs.camera_info.serial_number),
+                usb_type=usb_type,
+            )
+        )
+    return devices
+
+
+def camera_status(state: CameraState, target_fps: int) -> tuple[str, tuple[int, int, int]]:
+    if state.error:
+        return "ERROR", (0, 0, 255)
+    if state.received == 0:
+        return "WAITING", (0, 215, 255)
+    if state.elapsed < WARMUP_SECONDS:
+        return "WARMUP", (0, 215, 255)
+    if state.rolling_fps >= target_fps * MIN_FPS_RATIO and state.drop_rate <= MAX_DROP_RATE:
+        return "PASS", (0, 200, 0)
+    return "FAIL", (0, 0, 255)
+
+
+def render_tile(
+    worker: CameraWorker,
+    state: CameraState,
+    tile_width: int,
+    tile_height: int,
+    cv2: Any,
+    np: Any,
+) -> Any:
+    if state.frame is None:
+        tile = np.zeros((tile_height, tile_width, 3), dtype=np.uint8)
+    else:
+        tile = cv2.resize(state.frame, (tile_width, tile_height))
+
+    status, color = camera_status(state, worker.fps)
+    cv2.rectangle(tile, (0, 0), (tile_width, 100), (0, 0, 0), -1)
+    width, height = state.actual_size
+    lines = [
+        f"{worker.assignment.role}  {worker.assignment.model}  {worker.assignment.serial}",
+        f"USB {worker.assignment.usb_type}  {width}x{height}@{worker.fps}",
+        f"FPS {state.rolling_fps:.1f}  Frames {state.received}  "
+        f"Dropped {state.dropped} ({state.drop_rate:.2%})",
+        status if not state.error else f"ERROR: {state.error[:70]}",
+    ]
+    for index, line in enumerate(lines):
+        cv2.putText(
+            tile,
+            line,
+            (10, 22 + index * 24),
+            cv2.FONT_HERSHEY_SIMPLEX,
+            0.55,
+            color if index == len(lines) - 1 else (255, 255, 255),
+            1,
+            cv2.LINE_AA,
+        )
+    return tile
+
+
+def compose_preview(
+    workers: list[CameraWorker],
+    states: dict[str, CameraState],
+    capture_width: int,
+    capture_height: int,
+    cv2: Any,
+    np: Any,
+) -> Any:
+    tile_width = min(capture_width, PREVIEW_TILE_WIDTH)
+    tile_height = round(tile_width * capture_height / capture_width)
+    tiles = {
+        worker.assignment.serial: render_tile(
+            worker,
+            states[worker.assignment.serial],
+            tile_width,
+            tile_height,
+            cv2,
+            np,
+        )
+        for worker in workers
+    }
+    global_worker = next(worker for worker in workers if worker.assignment.role == "GLOBAL")
+    arm_workers = [worker for worker in workers if worker.assignment.role != "GLOBAL"]
+
+    top = np.zeros((tile_height, tile_width * 2, 3), dtype=np.uint8)
+    offset = tile_width // 2
+    top[:, offset : offset + tile_width] = tiles[global_worker.assignment.serial]
+    bottom = np.hstack([tiles[worker.assignment.serial] for worker in arm_workers])
+    return np.vstack((top, bottom))
+
+
+def save_snapshots(
+    assignments: list[CameraAssignment],
+    states: dict[str, CameraState],
+    cv2: Any,
+) -> Path:
+    missing = [camera.role for camera in assignments if states[camera.serial].frame is None]
+    if missing:
+        raise RuntimeError(f"以下相机尚无有效画面，不能保存快照: {', '.join(missing)}")
+
+    timestamp = datetime.now().strftime("%Y%m%d_%H%M%S_%f")[:-3]
+    snapshot_dir = OUTPUT_DIR / timestamp
+    snapshot_dir.mkdir(parents=True, exist_ok=False)
+    filenames = snapshot_filenames(assignments)
+    for camera in assignments:
+        path = snapshot_dir / filenames[camera.serial]
+        if not cv2.imwrite(str(path), states[camera.serial].frame):
+            raise RuntimeError(f"保存快照失败: {path}")
+    return snapshot_dir
+
+
+def print_summary(workers: list[CameraWorker]) -> bool:
+    now = time.monotonic()
+    print("\n相机测试汇总:")
+    passed = True
+    for worker in workers:
+        state = worker.state(now)
+        status, _color = camera_status(state, worker.fps)
+        passed = passed and status == "PASS"
+        print(
+            f"  {worker.assignment.role:<7} {worker.assignment.serial}: "
+            f"平均 {state.average_fps:.1f} FPS, 接收 {state.received}, "
+            f"掉帧 {state.dropped} ({state.drop_rate:.2%}), {status}"
+        )
+    print("结论: " + ("三路链路满足当前阈值" if passed else "至少一路未满足当前阈值"))
+    return passed
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--width", type=int, default=DEFAULT_WIDTH, help="采集宽度")
+    parser.add_argument("--height", type=int, default=DEFAULT_HEIGHT, help="采集高度")
+    parser.add_argument("--fps", type=int, default=DEFAULT_FPS, help="目标帧率")
+    parser.add_argument(
+        "--left-serial",
+        default=DEFAULT_LEFT_SERIAL,
+        help=f"左臂 D405 的 RealSense 序列号（默认: {DEFAULT_LEFT_SERIAL}）",
+    )
+    args = parser.parse_args()
+    if args.width <= 0 or args.height <= 0 or args.fps <= 0:
+        parser.error("width、height 和 fps 必须为正数")
+    return args
+
+
+def main() -> int:
+    args = parse_args()
+    try:
+        cv2, np, rs = load_runtime_dependencies()
+        assignments = assign_camera_roles(enumerate_devices(rs), args.left_serial)
+    except RuntimeError as exc:
+        print(f"错误: {exc}")
+        return 2
+    except ValueError as exc:
+        print(f"设备检查失败: {exc}")
+        return 2
+
+    print("相机分配:")
+    for camera in assignments:
+        print(
+            f"  {camera.role:<7} {camera.model}  serial={camera.serial}  "
+            f"USB={camera.usb_type}"
+        )
+
+    workers: list[CameraWorker] = []
+    passed = False
+    try:
+        for assignment in assignments:
+            worker = CameraWorker(
+                assignment,
+                args.width,
+                args.height,
+                args.fps,
+                rs,
+                np,
+            )
+            workers.append(worker)
+            worker.start()
+
+        cv2.namedWindow(WINDOW_NAME, cv2.WINDOW_NORMAL)
+        print("按 S 保存三路快照，按 Q 或 Esc 退出。")
+        while True:
+            now = time.monotonic()
+            states = {worker.assignment.serial: worker.state(now) for worker in workers}
+            preview = compose_preview(
+                workers,
+                states,
+                args.width,
+                args.height,
+                cv2,
+                np,
+            )
+            cv2.imshow(WINDOW_NAME, preview)
+            key = cv2.waitKey(1) & 0xFF
+            if key in (ord("q"), ord("Q"), 27):
+                break
+            if key in (ord("s"), ord("S")):
+                try:
+                    output = save_snapshots(assignments, states, cv2)
+                    print(f"快照已保存: {output}")
+                except RuntimeError as exc:
+                    print(f"快照失败: {exc}")
+            if cv2.getWindowProperty(WINDOW_NAME, cv2.WND_PROP_VISIBLE) < 1:
+                break
+    except KeyboardInterrupt:
+        print("\n收到 Ctrl+C，正在停止相机。")
+    except Exception as exc:
+        print(f"相机启动或显示失败: {exc}")
+    finally:
+        for worker in reversed(workers):
+            worker.stop()
+        try:
+            cv2.destroyAllWindows()
+        except Exception:
+            pass
+        if workers:
+            passed = print_summary(workers)
+    return 0 if passed else 1
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

xr_rm_interfaces/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rosidl_default_generators REQUIRED)
 find_package(std_msgs REQUIRED)
 
 rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/ActControlSample.msg"
   "msg/XrController.msg"
   DEPENDENCIES geometry_msgs std_msgs
 )

xr_rm_interfaces/msg/ActControlSample.msg

@@ -0,0 +1,41 @@
+std_msgs/Header header
+uint64 control_seq
+int64 control_monotonic_ns
+int64 feedback_monotonic_ns
+int64 action_monotonic_ns
+
+float32 feedback_age_ms
+float32 qp_duration_ms
+
+float64[7] q_actual
+float64[7] q_qp_raw
+float64[7] q_target
+float64[7] joint_lower_limits
+float64[7] joint_upper_limits
+
+geometry_msgs/Pose tcp_current
+geometry_msgs/Pose tcp_raw_target
+geometry_msgs/Pose tcp_target
+geometry_msgs/Twist tcp_command_velocity
+geometry_msgs/Pose pico_pose
+
+bool pico_grip
+float32 pico_trigger
+bool pico_primary
+bool pico_secondary
+float32[2] pico_axis
+
+bool gripper_target_open
+bool gripper_state_open
+bool gripper_state_known
+bool gripper_command_pending
+bool gripper_command_failed
+
+bool teleop_active
+bool feedback_valid
+bool action_valid
+bool command_sent
+bool qp_attempted
+bool qp_success
+bool target_clamped
+bool control_fault

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/package.xml

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

xr_rm_teleop/setup.py

@@ -24,6 +24,15 @@ setup(
             f"share/{package_name}/models/rm75/meshes",
             glob("models/rm75/meshes/*.STL"),
         ),
+        (
+            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"),
+        ),
         (
             f"share/{package_name}/models/rm75_omnipicker/urdf",
             glob("models/rm75_omnipicker/urdf/*.urdf"),
@@ -46,7 +55,9 @@ setup(
     tests_require=["pytest"],
     entry_points={
         "console_scripts": [
-            "single_arm_velocity_teleop = xr_rm_teleop.single_arm_velocity_teleop:main",
+            "act_episode_recorder = xr_rm_teleop.act_episode_recorder:main",
+            "single_arm_velocity_teleop = "
+            "xr_rm_teleop.single_arm_velocity_teleop:main",
         ],
     },
 )

xr_rm_teleop/test/placo_ik_smoke.py

@@ -11,8 +11,13 @@ from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
 
 
 CASES = {
-    "left": [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
-    "right": [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
+    "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),
 }
 
 
@@ -37,13 +42,16 @@ def main() -> None:
     urdf_path = Path(sys.argv[1]).resolve()
     for arm, joint_degrees in CASES.items():
         initial_joints = np.deg2rad(joint_degrees)
-        drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
+        drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0, arm)
         joints = initial_joints.tolist()
         stationary_target = drift_solver.update_joint_state(joints)
-        flange = drift_solver._robot.get_T_world_frame("link_7")
-        flange_to_tcp = np.linalg.inv(flange) @ stationary_target
-        assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, 0.16])
-        assert np.allclose(flange_to_tcp[:3, :3], np.eye(3))
+        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)
         for _ in range(250):
             drift_solver.update_joint_state(joints)
             joints = drift_solver.solve(stationary_target)
@@ -54,7 +62,7 @@ def main() -> None:
             f"{arm} stationary target drifted {drift_degrees:.3f}deg"
         )
 
-        solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
+        solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0, arm)
         joints = initial_joints.tolist()
         current = solver.update_joint_state(joints)
         assert current.shape == (4, 4)

xr_rm_teleop/test/test_act_control_sample.py

@@ -0,0 +1,194 @@
+from types import SimpleNamespace
+
+import numpy as np
+import pytest
+from builtin_interfaces.msg import Time as TimeMsg
+
+from xr_rm_interfaces.msg import XrController
+from xr_rm_teleop.single_arm_velocity_teleop import (
+    SingleArmVelocityTeleop,
+    _ActCycleContext,
+)
+
+
+class FakePublisher:
+    def __init__(self, error=None) -> None:
+        self.error = error
+        self.messages = []
+
+    def publish(self, message) -> None:
+        if self.error is not None:
+            raise self.error
+        self.messages.append(message)
+
+
+class FakeLogger:
+    def __init__(self) -> None:
+        self.warnings = []
+
+    def warn(self, message, **kwargs) -> None:
+        del kwargs
+        self.warnings.append(message)
+
+
+def _controller(*, grip=True) -> XrController:
+    message = XrController()
+    message.hand = "right"
+    message.grip = grip
+    message.trigger = 0.25
+    message.primary = False
+    message.secondary = False
+    message.axis = [0.1, -0.2]
+    message.pose.orientation.w = 1.0
+    return message
+
+
+def _teleop(*, last_target=None, tool_state=True):
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._last_msg = _controller()
+    teleop._active = True
+    teleop._control_fault_latched = False
+    teleop._last_current_pose = np.eye(4)
+    teleop._robot_start_transform = None
+    teleop._last_sent_target = None
+    teleop._last_sent_orientation = None
+    teleop._last_successful_action_target = last_target
+    teleop._ik_solver = SimpleNamespace(
+        joint_position_limits=np.asarray([[-1.0, 1.0]] * 7)
+    )
+    teleop._tool_state_snapshot = lambda: (
+        True,
+        tool_state,
+        False,
+        False,
+    )
+    teleop.get_clock = lambda: SimpleNamespace(
+        now=lambda: SimpleNamespace(to_msg=lambda: TimeMsg())
+    )
+    teleop.get_logger = lambda: FakeLogger()
+    return teleop
+
+
+def _cycle(**overrides) -> _ActCycleContext:
+    values = {
+        "control_seq": 100,
+        "control_monotonic_ns": 1_000_000_000,
+        "feedback_monotonic_ns": 990_000_000,
+        "action_monotonic_ns": 1_005_000_000,
+        "feedback_age_ms": 10.0,
+        "q_actual": [0.1] * 7,
+        "q_qp_raw": [0.3] * 7,
+        "q_target": [0.2] * 7,
+        "current_pose": np.eye(4),
+        "raw_target_pose": np.eye(4),
+        "target_pose": np.eye(4),
+        "command_velocity": [0.0] * 6,
+        "feedback_valid": True,
+        "command_sent": True,
+        "qp_attempted": True,
+        "qp_success": True,
+    }
+    values.update(overrides)
+    return _ActCycleContext(**values)
+
+
+def test_act_sample_uses_feedback_and_limited_target_from_one_cycle() -> None:
+    teleop = _teleop(last_target=[0.2] * 7)
+
+    message = teleop._build_act_control_sample(_cycle())
+
+    assert message.control_seq == 100
+    assert message.q_actual == pytest.approx([0.1] * 7)
+    assert message.q_qp_raw == pytest.approx([0.3] * 7)
+    assert message.q_target == pytest.approx([0.2] * 7)
+    assert message.joint_lower_limits == pytest.approx([-1.0] * 7)
+    assert message.joint_upper_limits == pytest.approx([1.0] * 7)
+    assert message.command_sent
+    assert message.action_valid
+    assert message.qp_attempted
+    assert message.qp_success
+
+
+def test_act_sample_marks_qp_fallback_as_valid_held_action() -> None:
+    teleop = _teleop(last_target=[0.2] * 7)
+    cycle = _cycle(
+        q_qp_raw=[0.2] * 7,
+        q_target=[0.2] * 7,
+        qp_success=False,
+    )
+
+    message = teleop._build_act_control_sample(cycle)
+
+    assert message.q_target == pytest.approx([0.2] * 7)
+    assert message.action_valid
+    assert message.qp_attempted
+    assert not message.qp_success
+
+
+def test_act_sample_holds_last_action_while_grip_is_released() -> None:
+    teleop = _teleop(last_target=[0.4] * 7)
+    teleop._last_msg = _controller(grip=False)
+    teleop._active = False
+    cycle = _cycle(
+        q_qp_raw=None,
+        q_target=None,
+        command_sent=False,
+        qp_attempted=False,
+        qp_success=False,
+        action_monotonic_ns=-1,
+    )
+
+    message = teleop._build_act_control_sample(cycle)
+
+    assert message.q_target == pytest.approx([0.4] * 7)
+    assert message.action_valid
+    assert not message.command_sent
+    assert not message.teleop_active
+
+
+def test_act_sample_marks_send_failure_invalid() -> None:
+    teleop = _teleop(last_target=[0.4] * 7)
+
+    message = teleop._build_act_control_sample(
+        _cycle(send_failed=True, command_sent=False)
+    )
+
+    assert not message.action_valid
+
+
+def test_act_sample_marks_unknown_gripper_state() -> None:
+    teleop = _teleop(last_target=[0.2] * 7, tool_state=None)
+
+    message = teleop._build_act_control_sample(_cycle())
+
+    assert not message.gripper_state_known
+
+
+def test_act_sample_publish_failure_does_not_escape_control_path() -> None:
+    teleop = _teleop(last_target=[0.2] * 7)
+    logger = FakeLogger()
+    teleop._act_sample_pub = FakePublisher(RuntimeError("dds failed"))
+    teleop.get_logger = lambda: logger
+
+    teleop._publish_act_control_sample(_cycle())
+
+    assert logger.warnings == [
+        "right_rm75 ACT原子样本发布失败：dds failed"
+    ]
+
+
+def test_control_tick_wraps_one_impl_call_in_one_atomic_sample() -> None:
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    cycles = []
+    published = []
+    teleop._act_control_seq = 7
+    teleop._control_tick_impl = lambda cycle: cycles.append(cycle)
+    teleop._publish_act_control_sample = lambda cycle: published.append(cycle)
+
+    teleop._control_tick()
+
+    assert len(cycles) == 1
+    assert published == cycles
+    assert cycles[0].control_seq == 7
+    assert teleop._act_control_seq == 8

xr_rm_teleop/test/test_initial_joint_pose.py

@@ -1,13 +1,23 @@
 import math
 import sys
+from pathlib import Path
 from types import ModuleType, SimpleNamespace
 
 import pytest
+import yaml
 
-from xr_rm_teleop import realman_adapter
+from xr_rm_teleop import fun_peripheral, realman_adapter
 from xr_rm_teleop.realman_adapter import RealManAdapter
 from xr_rm_teleop.realman_adapter import MockRealManAdapter
-from xr_rm_teleop.fun_peripheral import PeripheralConfig, _configure_tool_frame
+from xr_rm_teleop.fun_peripheral import (
+    PeripheralConfig,
+    _configure_tool_frame,
+    load_peripheral_config,
+    peripheral_cfg,
+)
+
+
+CONFIG_DIR = Path(__file__).resolve().parents[2] / "xr_rm_bringup" / "config"
 
 
 def test_initial_pose_uses_joint_move_only() -> None:
@@ -30,11 +40,23 @@ def test_initial_pose_uses_joint_move_only() -> None:
     )
     adapter._arm = FakeArm()
 
-    adapter._move_to_initial_pose()
+    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]
+    )
+
+
 def test_peripheral_config_exposes_selected_tool() -> None:
     config = PeripheralConfig(
         scissorgripper=1,
@@ -48,6 +70,109 @@ def test_peripheral_config_exposes_selected_tool() -> None:
     assert config.tool_pose == [0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0]
 
 
+def test_deployed_peripheral_config_selects_left_and_right_tools() -> None:
+    config_file = CONFIG_DIR / "peripherals_rm75.yaml"
+    left = load_peripheral_config(str(config_file), "left")
+    right = load_peripheral_config(str(config_file), "right")
+
+    assert left.scissorgripper == 2
+    assert left.tool_name == "minisci"
+    assert left.tool_pose == [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 == [0.0, 0.0, 0.14, 0.0, 0.0, 0.0, 1.0]
+
+
+def test_right_tool_initializes_open_only_for_right_arm() -> None:
+    config_file = CONFIG_DIR / "peripherals_rm75.yaml"
+    left = load_peripheral_config(str(config_file), "left")
+    right = load_peripheral_config(str(config_file), "right")
+
+    assert not left.set_initial_tool_state
+    assert right.set_initial_tool_state
+
+
+def test_omnipic_initial_state_opens_fully(monkeypatch) -> None:
+    calls = []
+
+    class FakeArm:
+        def rm_set_voltage(self, *args):
+            del args
+
+        def rm_set_io_mode(self, *args):
+            del args
+
+        def rm_algo_quaternion2euler(self, quaternion):
+            del quaternion
+            return [0.0, 0.0, 0.0]
+
+        def rm_get_total_tool_frame(self):
+            return {"return_code": 0, "tool_names": []}
+
+        def rm_set_manual_tool_frame(self, *, frame):
+            del frame
+            return 0
+
+        def rm_change_tool_frame(self, tool_name):
+            del tool_name
+            return 0
+
+        def rm_set_modbus_mode(self, **kwargs):
+            del kwargs
+            return 0
+
+        def rm_write_single_register(self, params, value):
+            del params, value
+            return 0
+
+    sdk = ModuleType("Robotic_Arm.rm_robot_interface")
+    sdk.rm_frame_t = lambda *args: object()
+    sdk.rm_peripheral_read_write_params_t = lambda *args: object()
+    package = ModuleType("Robotic_Arm")
+    package.rm_robot_interface = sdk
+    monkeypatch.setitem(sys.modules, "Robotic_Arm", package)
+    monkeypatch.setitem(sys.modules, "Robotic_Arm.rm_robot_interface", sdk)
+    monkeypatch.setattr(fun_peripheral.time, "sleep", lambda seconds: None)
+    monkeypatch.setattr(
+        fun_peripheral,
+        "set_tool_position",
+        lambda robot, percent, device, scissorgripper: calls.append(
+            (percent, device, scissorgripper)
+        ),
+    )
+    tools = {
+        "scissor": [[0.0] * 7, [0.0] * 7],
+        "omnipic": [[0.0] * 7, [0.0] * 7],
+    }
+
+    peripheral_cfg(
+        FakeArm(),
+        1,
+        tools,
+        set_initial_tool_state=True,
+    )
+
+    assert calls == [(1.0, 1, 1)]
+
+
+@pytest.mark.parametrize(
+    ("config_name", "node_names"),
+    [
+        ("left_arm_rm75.yaml", ("single_arm_velocity_teleop",)),
+        ("right_arm_rm75.yaml", ("single_arm_velocity_teleop",)),
+        ("dual_arm_rm75.yaml", ("left_arm_teleop", "right_arm_teleop")),
+    ],
+)
+def test_deployed_workspace_is_in_front_of_robot(config_name, node_names) -> None:
+    with (CONFIG_DIR / config_name).open(encoding="utf-8") as stream:
+        config = yaml.safe_load(stream)
+
+    for node_name in node_names:
+        parameters = config[node_name]["ros__parameters"]
+        assert parameters["workspace_min"] == [-0.70, -0.70, 0.10]
+        assert parameters["workspace_max"] == [0.70, 0.10, 0.75]
+
+
 @pytest.mark.parametrize(
     ("existing", "expected_operation"),
     [(False, "create"), (True, "update")],

xr_rm_teleop/test/test_joint_control.py

@@ -1,9 +1,11 @@
 import math
+import threading
 import time
 from types import SimpleNamespace
 
 import numpy as np
 import pytest
+from builtin_interfaces.msg import Time as TimeMsg
 
 from xr_rm_teleop.realman_adapter import JointStateSnapshot
 from xr_rm_teleop.single_arm_velocity_teleop import (
@@ -29,6 +31,242 @@ class FakeTime:
         del other
         return SimpleNamespace(nanoseconds=0)
 
+    def to_msg(self):
+        return TimeMsg()
+
+
+class FakePublisher:
+    def __init__(self) -> None:
+        self.messages = []
+
+    def publish(self, message) -> None:
+        self.messages.append(message)
+
+
+def _tool_state_teleop(*, command_error=None):
+    started = threading.Event()
+    release = threading.Event()
+
+    class Adapter:
+        def set_tool_enabled(self, open_tool):
+            del open_tool
+            started.set()
+            assert release.wait(timeout=1.0)
+            if command_error is not None:
+                raise command_error
+
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._arm_name = "right_rm75"
+    teleop._adapter = Adapter()
+    teleop._tool_command_queue = None
+    teleop._tool_worker_stop = threading.Event()
+    teleop._tool_worker_thread = None
+    teleop._tool_state_lock = threading.Lock()
+    teleop._tool_target_open = True
+    teleop._tool_state_open = True
+    teleop._tool_command_pending = False
+    teleop._tool_command_failed = False
+    teleop.get_logger = lambda: FakeLogger()
+    teleop._start_tool_worker()
+    return teleop, started, release
+
+
+def test_tool_state_changes_only_after_command_succeeds() -> None:
+    teleop, started, release = _tool_state_teleop()
+    try:
+        teleop._enqueue_tool_command(False, "test")
+        assert started.wait(timeout=1.0)
+
+        assert teleop._tool_state_snapshot() == (False, True, True, False)
+
+        release.set()
+        assert teleop._tool_command_queue is not None
+        teleop._tool_command_queue.join()
+
+        assert teleop._tool_state_snapshot() == (False, False, False, False)
+    finally:
+        release.set()
+        teleop._shutdown_tool_worker()
+
+
+def test_tool_failure_keeps_previous_state_and_is_reported() -> None:
+    teleop, started, release = _tool_state_teleop(
+        command_error=RuntimeError("modbus failed")
+    )
+    try:
+        teleop._enqueue_tool_command(False, "test")
+        assert started.wait(timeout=1.0)
+        release.set()
+        assert teleop._tool_command_queue is not None
+        teleop._tool_command_queue.join()
+
+        assert teleop._tool_state_snapshot() == (False, True, False, True)
+    finally:
+        release.set()
+        teleop._shutdown_tool_worker()
+
+
+def _joint_publishing_teleop() -> SingleArmVelocityTeleop:
+    names = [f"omnipic_joint_{index}" for index in range(1, 8)]
+    teleop = object.__new__(SingleArmVelocityTeleop)
+    teleop._ik_solver = SimpleNamespace(
+        joint_names=names,
+        update_joint_state=lambda joints: np.eye(4),
+    )
+    teleop._joint_state_pub = FakePublisher()
+    teleop._joint_target_pub = FakePublisher()
+    teleop._active = False
+    teleop._last_valid_joint_target = None
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
+    return teleop
+
+
+def test_reset_joint_state_publishes_named_feedback() -> None:
+    teleop = _joint_publishing_teleop()
+    positions = [0.1 * index for index in range(7)]
+    snapshot = JointStateSnapshot(positions, time.monotonic())
+
+    teleop._reset_joint_state(snapshot)
+
+    message = teleop._joint_state_pub.messages[-1]
+    assert message.name == teleop._ik_solver.joint_names
+    assert message.position == pytest.approx(positions)
+
+
+def test_sync_joint_feedback_publishes_each_sample() -> None:
+    teleop = _joint_publishing_teleop()
+    positions = [0.2] * 7
+
+    teleop._sync_joint_feedback(
+        JointStateSnapshot(positions, time.monotonic())
+    )
+
+    assert len(teleop._joint_state_pub.messages) == 1
+    assert teleop._joint_state_pub.messages[0].position == pytest.approx(positions)
+
+
+def test_send_joint_target_publishes_limited_command() -> None:
+    sent = []
+    teleop = _joint_publishing_teleop()
+    teleop._adapter = SimpleNamespace(
+        send_joint_target=lambda joints, follow: sent.append((list(joints), follow))
+    )
+    teleop._follow = False
+    teleop._latest_joint_positions = [0.0] * 7
+    teleop._last_joint_command_target = [0.0] * 7
+    teleop._last_joint_command_velocity = [0.0] * 7
+    teleop._joint_command_max_speed = 1.0
+    teleop._joint_command_max_acceleration = 100.0
+    teleop._dt = 0.1
+
+    assert teleop._send_joint_target([0.5] * 7)
+
+    assert len(sent) == 1
+    assert sent[0][0] == pytest.approx([0.1] * 7)
+    assert sent[0][1] is False
+    message = teleop._joint_target_pub.messages[-1]
+    assert message.name == teleop._ik_solver.joint_names
+    assert message.position == pytest.approx(teleop._last_joint_command_target)
+
+
+def _primary_button_teleop(*, use_mock=False, move_error=None):
+    events = []
+    errors = []
+    snapshot = JointStateSnapshot([0.2] * 7, time.monotonic())
+
+    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._use_mock = use_mock
+    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
+
+
+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"
+    ]
+
+
+def test_mock_primary_reset_can_reanchor_without_grip_release() -> None:
+    teleop, events, _, snapshot = _primary_button_teleop(use_mock=True)
+
+    teleop._on_controller(SimpleNamespace(primary=False))
+    teleop._on_controller(SimpleNamespace(primary=True))
+
+    assert events == [
+        ("stop", True),
+        "move",
+        "read",
+        ("sync", snapshot),
+    ]
+    assert not teleop._grip_rearm_required
+
+
+def test_failed_mock_primary_reset_still_requires_grip_release() -> None:
+    failure = RuntimeError("mock reset failed")
+    teleop, _, _, _ = _primary_button_teleop(
+        use_mock=True,
+        move_error=failure,
+    )
+
+    teleop._on_controller(SimpleNamespace(primary=False))
+    teleop._on_controller(SimpleNamespace(primary=True))
+
+    assert teleop._grip_rearm_required
+
 
 def test_startup_joint_query_initializes_qp_and_command_history() -> None:
     positions = [0.1] * 7
@@ -42,8 +280,11 @@ def test_startup_joint_query_initializes_qp_and_command_history() -> None:
         )
     )
     teleop._ik_solver = SimpleNamespace(
-        update_joint_state=lambda joints: pose
+        joint_names=[f"omnipic_joint_{index}" for index in range(1, 8)],
+        update_joint_state=lambda joints: pose,
     )
+    teleop._joint_state_pub = FakePublisher()
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
     teleop.get_logger = lambda: FakeLogger()
 
     teleop._initialize_joint_state()
@@ -108,6 +349,12 @@ def _timeout_teleop(adapter) -> SingleArmVelocityTeleop:
     teleop._ik_solver = SimpleNamespace(
         update_joint_state=lambda joints: np.eye(4)
     )
+    teleop._ik_solver.joint_names = [
+        f"omnipic_joint_{index}" for index in range(1, 8)
+    ]
+    teleop._joint_state_pub = FakePublisher()
+    teleop._joint_target_pub = FakePublisher()
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
     teleop._stop_sent = False
     teleop._feedback_resync_timeout_sec = 0.5
     teleop._publish_stop_debug = lambda: None
@@ -363,6 +610,10 @@ def test_feedback_fault_blocks_grip_until_release() -> None:
     teleop._ik_solver = SimpleNamespace(
         update_joint_state=lambda joints: np.eye(4)
     )
+    teleop._ik_solver.joint_names = [
+        f"omnipic_joint_{index}" for index in range(1, 8)
+    ]
+    teleop._joint_state_pub = FakePublisher()
     teleop._grip_rearm_required = True
     teleop._control_fault_latched = False
     teleop._feedback_resync_attempted = False
@@ -389,6 +640,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
     class FakeSolver:
         def __init__(self) -> None:
             self.solve_calls = 0
+            self.joint_names = [
+                f"omnipic_joint_{index}" for index in range(1, 8)
+            ]
 
         def update_joint_state(self, joints):
             assert joints == [0.1] * 7
@@ -406,6 +660,8 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
     teleop._active = False
     teleop._last_valid_joint_target = None
     teleop._last_current_pose = None
+    teleop._joint_state_pub = FakePublisher()
+    teleop.get_clock = lambda: SimpleNamespace(now=lambda: FakeTime())
 
     pose = teleop._sync_joint_feedback(
         JointStateSnapshot([0.1] * 7, time.monotonic())
@@ -430,9 +686,10 @@ def test_qp_failure_returns_last_known_good_target() -> None:
     teleop._arm_name = "right_rm75"
     teleop.get_logger = lambda: FakeLogger()
 
-    target = teleop._solve_joint_target(np.eye(4))
+    target, qp_success = teleop._solve_joint_target(np.eye(4))
 
     assert target == pytest.approx([0.1] * 7)
+    assert not qp_success
     assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
 
 
@@ -448,9 +705,10 @@ def test_qp_success_updates_last_known_good_target() -> None:
     teleop._arm_name = "left_rm75"
     teleop.get_logger = lambda: FakeLogger()
 
-    target = teleop._solve_joint_target(np.eye(4))
+    target, qp_success = teleop._solve_joint_target(np.eye(4))
 
     assert target == pytest.approx([0.2] * 7)
+    assert qp_success
     assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
 
 

xr_rm_teleop/test/test_orientation_control.py

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

xr_rm_teleop/test/test_placo_transforms.py

@@ -7,76 +7,182 @@ import numpy as np
 import pytest
 
 from xr_rm_teleop.placo_ik_solver import (
+    QP_ORIENTATION_TOLERANCE_RAD,
     QP_POSITION_TOLERANCE_M,
     PlacoIkSolver,
     _validated_transform,
 )
 
-
-def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
-    urdf_path = (
+DUAL_URDF_PATH = (
     Path(__file__).resolve().parents[1]
     / "models"
-        / "rm75_omnipicker"
-        / "urdf"
-        / "RM75-B_OmniPicker_fixed.urdf"
+    / "dual_rm75"
+    / "Dual_arm.urdf"
 )
-    root = ElementTree.parse(urdf_path).getroot()
+ARM_CASES = (
+    (
+        "left",
+        [-78.81, 3.22, 67.96, 97.12, 95.08, -81.11, -74.55],
+        list(range(14, 21)),
+        list(range(13, 20)),
+        "omnipic",
+        "scissor_base_link",
+        "scissor_scissor_tcp",
+    ),
+    (
+        "right",
+        [-86.10, 22.80, -89.57, 93.98, -91.82, -87.32, -89.35],
+        list(range(7, 14)),
+        list(range(6, 13)),
+        "scissor",
+        "omnipic_base_link",
+        "omnipic_OmniPic_tcp",
+    ),
+)
+
+
+def test_dual_urdf_has_expected_joints_meshes_and_tcp_frames() -> 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"
     ]
-    tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']")
     mesh_filenames = [
         mesh.attrib["filename"]
         for mesh in root.findall(".//mesh")
     ]
+    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",
+        ),
+    }
 
-    assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
-    assert all(
-        filename.startswith(
-            "package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
-        )
-        for filename in mesh_filenames
-    )
-    assert tcp_joint is not None
-    assert tcp_joint.attrib["type"] == "fixed"
-    assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
-    assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
-    assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
+    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(filename.startswith("meshes/") for filename in mesh_filenames)
+    for name, (parent, child, xyz) in 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
+
+
+def test_left_scissor_mesh_matches_physical_mount_rotation() -> None:
+    root = ElementTree.parse(DUAL_URDF_PATH).getroot()
+    link = root.find("link[@name='scissor_scissor_link']")
+
+    assert link is not None
+    assert link.find("visual/origin").attrib["rpy"] == "0 0 -1.5708"
+    assert link.find("collision/origin").attrib["rpy"] == "0 0 -1.5708"
+    tcp_joint = root.find("joint[@name='scissor_scissor_tcp_fixed']")
     assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
 
 
-def _rm75_placo_solver() -> tuple[PlacoIkSolver, list[float]]:
+def _dual_placo_solver(
+    arm: str,
+    joint_degrees: list[float],
+) -> 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 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,"
+    "expected_base_frame,expected_tcp_frame",
+    ARM_CASES,
 )
-    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
+def test_solver_uses_arm_specific_offsets(
+    arm: str,
+    joint_degrees: list[float],
+    q_offsets: list[int],
+    v_offsets: list[int],
+    inactive_prefix: str,
+    expected_base_frame: str,
+    expected_tcp_frame: str,
+) -> None:
+    solver, _ = _dual_placo_solver(arm, joint_degrees)
+
+    assert solver._q_offsets.tolist() == q_offsets
+    assert solver._v_offsets.tolist() == v_offsets
 
 
-def test_qp_solve_converges_to_reachable_tcp_target() -> None:
-    solver, joints = _rm75_placo_solver()
+@pytest.mark.parametrize(
+    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
+    "expected_base_frame,expected_tcp_frame",
+    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,
+    expected_base_frame: str,
+    expected_tcp_frame: str,
+) -> None:
+    solver, joints = _dual_placo_solver(arm, joint_degrees)
+
+    actual_pose = solver.update_joint_state(joints)
+    assert solver._base_frame == expected_base_frame
+    assert solver._tcp_frame == expected_tcp_frame
+    world_base = solver._robot.get_T_world_frame(expected_base_frame)
+    world_tcp = solver._robot.get_T_world_frame(expected_tcp_frame)
+
+    assert actual_pose == pytest.approx(np.linalg.inv(world_base) @ world_tcp)
+
+
+@pytest.mark.parametrize(
+    "arm,joint_degrees,q_offsets,v_offsets,inactive_prefix,"
+    "expected_base_frame,expected_tcp_frame",
+    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,
+    expected_base_frame: str,
+    expected_tcp_frame: str,
+) -> None:
+    solver, joints = _dual_placo_solver(arm, joint_degrees)
     start_pose = solver.update_joint_state(joints)
+    inactive_q_offsets = [
+        solver._robot.get_joint_offset(f"{inactive_prefix}_joint_{index}")
+        for index in range(1, 8)
+    ]
+    inactive_before = solver._robot.state.q[inactive_q_offsets].copy()
     target_pose = start_pose.copy()
-    target_pose[0, 3] += 0.07
+    target_pose[0, 3] += 0.01
 
     result = solver.solve(target_pose)
     reached_pose = solver.update_joint_state(result)
@@ -96,17 +202,28 @@ def test_qp_solve_converges_to_reachable_tcp_target() -> None:
         )
     )
 
+    assert np.asarray(result).shape == (7,)
+    assert np.isfinite(result).all()
     assert position_error <= QP_POSITION_TOLERANCE_M
-    assert orientation_error <= 5e-3
+    assert orientation_error <= QP_ORIENTATION_TOLERANCE_RAD
+    assert solver._robot.state.q[inactive_q_offsets] == pytest.approx(
+        inactive_before
+    )
+
+
+def test_solver_rejects_unknown_arm() -> None:
+    with pytest.raises(ValueError, match="arm must be left or right"):
+        PlacoIkSolver(str(DUAL_URDF_PATH), 1.0 / 90.0, "middle")
 
 
 def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
     solver = object.__new__(PlacoIkSolver)
     solver._actual_joints = np.zeros(7)
+    solver._q_offsets = np.arange(7, 14)
     solver._robot = SimpleNamespace(
-        state=SimpleNamespace(q=np.zeros(14))
+        state=SimpleNamespace(q=np.zeros(21))
     )
-    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._frame_task = SimpleNamespace(T_a_b=None)
     solver._target_errors = lambda: (1.5e-3, 0.0)
 
     result = solver.solve(np.eye(4))
@@ -117,11 +234,12 @@ def test_qp_solve_accepts_position_error_within_two_millimeters() -> None:
 def test_qp_solve_rejects_position_error_above_two_millimeters() -> None:
     solver = object.__new__(PlacoIkSolver)
     solver._actual_joints = np.zeros(7)
+    solver._q_offsets = np.arange(7, 14)
     solver._robot = SimpleNamespace(
-        state=SimpleNamespace(q=np.zeros(14)),
+        state=SimpleNamespace(q=np.zeros(21)),
         update_kinematics=lambda: None,
     )
-    solver._frame_task = SimpleNamespace(T_world_frame=None)
+    solver._frame_task = SimpleNamespace(T_a_b=None)
     solver._solver = SimpleNamespace(solve=lambda update: None)
     solver._validate_result = lambda result, previous: None
     solver._target_errors = lambda: (2.1e-3, 0.0)
@@ -154,6 +272,20 @@ def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
         _validated_transform(transform)
 
 
+def test_joint_position_limits_returns_a_copy() -> None:
+    solver = object.__new__(PlacoIkSolver)
+    solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)
+
+    first = solver.joint_position_limits
+    second = solver.joint_position_limits
+
+    assert first.shape == (7, 2)
+    assert np.isfinite(first).all()
+    assert np.all(first[:, 0] < first[:, 1])
+    first[0, 0] = 999.0
+    assert second[0, 0] != 999.0
+
+
 def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
     solver = object.__new__(PlacoIkSolver)
     solver._joint_limits = np.asarray([[-1.0, 1.0]] * 7)

xr_rm_teleop/xr_rm_teleop/fun_peripheral.py

@@ -218,16 +218,19 @@ def peripheral_cfg(
 
         addr = 1
         # 依次设置目标速度、目标力矩、目标加速度和目标减速度。
-        reg_value = [255, 60, 255, 255]
+        reg_value = [255, 150, 255, 255]
         for i, reg_addr in enumerate([11, 12, 13, 14]):
             write_params = rm_peripheral_read_write_params_t(1, reg_addr, addr, 1)
             robot.rm_write_single_register(write_params, reg_value[i])
             time.sleep(0.5)
 
         if set_initial_tool_state:
-            set_tool_position(robot, percent=0.75, device=1, scissorgripper=scissorgripper)
-            time.sleep(1.5)
-            set_tool_position(robot, percent=0.15, device=1, scissorgripper=scissorgripper)
+            set_tool_position(
+                robot,
+                percent=1.0,
+                device=1,
+                scissorgripper=scissorgripper,
+            )
 
     elif scissorgripper == 2:
         # 小型剪刀夹爪通过控制器 DO3/DO4 控制。

xr_rm_teleop/xr_rm_teleop/placo_ik_solver.py

@@ -8,8 +8,24 @@ from pathlib import Path
 import numpy as np
 
 EXPECTED_PLACO_VERSION = "0.9.4"
-RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
-RM75_Q_SLICE = slice(7, 14)
+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)],
+]
 QP_MAX_ITERATIONS = 30
 QP_POSITION_TOLERANCE_M = 2e-3
 QP_ORIENTATION_TOLERANCE_RAD = 5e-3
@@ -43,9 +59,21 @@ class PlacoIkSolver:
         self,
         urdf_path: str,
         dt: float,
+        arm: str,
     ) -> None:
         if dt <= 0.0:
             raise ValueError("dt must be positive")
+        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)
+        ]
         try:
             installed_version = version("placo")
             import placo
@@ -65,30 +93,44 @@ class PlacoIkSolver:
 
         self._dt = dt
         self._robot = placo.RobotWrapper(str(model_path))
-        if self._robot.state.q.shape != (14,):
+        if self._robot.state.q.shape != (21,):
             raise RuntimeError(
-                f"expected Placo q shape (14,), got {self._robot.state.q.shape}"
+                "expected Placo q shape (21,), got "
+                f"{self._robot.state.q.shape}"
             )
-        if list(self._robot.joint_names()) != RM75_JOINT_NAMES:
+        if list(self._robot.joint_names()) != DUAL_RM75_JOINT_NAMES:
             raise RuntimeError(
-                f"unexpected RM75 joint order: {list(self._robot.joint_names())}"
+                "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()}"
             )
-        offsets = [
-            self._robot.get_joint_offset(name) for name in RM75_JOINT_NAMES
-        ]
-        if offsets != list(range(7, 14)):
-            raise RuntimeError(f"unexpected RM75 q offsets: {offsets}")
 
         self._joint_limits = np.asarray(
-            [self._robot.get_joint_limits(name) for name in RM75_JOINT_NAMES]
+            [self._robot.get_joint_limits(name) for name in self._joint_names]
         )
-        velocity_offsets = [
-            self._robot.get_joint_v_offset(name) for name in RM75_JOINT_NAMES
-        ]
         self._velocity_limits = np.asarray(
             [
                 self._robot.model.velocityLimit[index]
-                for index in velocity_offsets
+                for index in self._v_offsets
             ]
         )
         self._actual_joints: np.ndarray | None = None
@@ -96,29 +138,43 @@ class PlacoIkSolver:
         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_frame_task(
-            "omnipicker_tcp",
+        self._frame_task = self._solver.add_relative_frame_task(
+            self._base_frame,
+            self._tcp_frame,
             np.eye(4),
         )
-        self._frame_task.configure("rm75_frame", "soft", 1.0)
+        self._frame_task.configure("rm75_relative_frame", "soft", 1.0)
         self._solver.add_kinetic_energy_regularization_task(1e-6)
 
+    @property
+    def joint_names(self) -> list[str]:
+        return list(self._joint_names)
+
     @property
     def base_configuration(self) -> list[float]:
         return self._robot.state.q[:7].tolist()
 
+    @property
+    def joint_position_limits(self) -> np.ndarray:
+        return self._joint_limits.copy()
+
     def update_joint_state(self, joints: list[float]) -> np.ndarray:
         values = np.asarray(joints, dtype=float)
         if values.shape != (7,) or not np.isfinite(values).all():
             raise ValueError("joint state must contain 7 finite values")
         is_first_feedback = self._actual_joints is None
         self._actual_joints = values.copy()
-        self._robot.state.q[RM75_Q_SLICE] = values
+        self._robot.state.q[self._q_offsets] = values
         self._robot.update_kinematics()
-        base_to_tool = self._robot.get_T_world_frame("omnipicker_tcp")
+        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_world_frame = base_to_tool.copy()
+            self._frame_task.T_a_b = base_to_tool.copy()
         return base_to_tool.copy()
 
     def _target_errors(self) -> tuple[float, float]:
@@ -134,11 +190,11 @@ class PlacoIkSolver:
     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(
+        self._frame_task.T_a_b = _validated_transform(
             target_tool_pose
         )
         result = np.asarray(
-            self._robot.state.q[RM75_Q_SLICE],
+            self._robot.state.q[self._q_offsets],
             dtype=float,
         ).copy()
         position_error, orientation_error = self._target_errors()
@@ -153,7 +209,7 @@ class PlacoIkSolver:
             self._solver.solve(True)
             self._robot.update_kinematics()
             result = np.asarray(
-                self._robot.state.q[RM75_Q_SLICE],
+                self._robot.state.q[self._q_offsets],
                 dtype=float,
             ).copy()
             self._validate_result(result, previous)

xr_rm_teleop/xr_rm_teleop/realman_adapter.py

@@ -44,9 +44,10 @@ class MockRealManAdapter:
             math.isfinite(value) for value in initial_joint_degrees
         ):
             raise ValueError("initial joint pose must contain 7 finite values")
-        self._joint_positions = [
+        self._initial_joint_positions = [
             math.radians(value) for value in initial_joint_degrees
         ]
+        self._joint_positions = list(self._initial_joint_positions)
         self.last_joint_target: list[float] | None = None
         self.last_tool_open: bool | None = None
 
@@ -69,6 +70,10 @@ class MockRealManAdapter:
         self._joint_positions = list(joints)
         self.last_joint_target = list(joints)
 
+    def move_to_initial_pose(self) -> None:
+        self._joint_positions = list(self._initial_joint_positions)
+        self.last_joint_target = list(self._joint_positions)
+
     def stop(self) -> None:
         return
 
@@ -178,7 +183,7 @@ class RealManAdapter:
             if self._configure_safety_limits:
                 self._apply_safety_limits()
             if self._move_to_initial_pose_on_connect:
-                self._move_to_initial_pose()
+                self.move_to_initial_pose()
             self._feedback_ready.clear()
             self._accept_realtime_feedback = True
             self._realtime_callback = rm_realtime_arm_state_callback_ptr(
@@ -232,9 +237,9 @@ class RealManAdapter:
             )
 
     def read_joint_state(self) -> JointStateSnapshot:
-        self._require_arm()
+        arm = self._require_arm()
         started_at = time.monotonic()
-        result = self._arm.rm_get_joint_degree()
+        result = arm.rm_get_joint_degree()
         finished_at = time.monotonic()
         if not isinstance(result, tuple) or len(result) != 2:
             raise RuntimeError(
@@ -251,10 +256,10 @@ class RealManAdapter:
         )
 
     def send_joint_target(self, joints: list[float], follow: bool) -> None:
-        self._require_arm()
+        arm = self._require_arm()
         if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
             raise ValueError("joint target must contain 7 finite values")
-        ret = self._arm.rm_movej_canfd(
+        ret = arm.rm_movej_canfd(
             [math.degrees(value) for value in joints],
             follow,
             0,
@@ -310,9 +315,10 @@ class RealManAdapter:
             self._arm = None
             self._realtime_callback = None
 
-    def _require_arm(self) -> None:
+    def _require_arm(self) -> Any:
         if self._arm is None:
             raise RuntimeError("睿尔曼机械臂尚未连接")
+        return self._arm
 
     def _on_realtime_arm_state(self, data: Any) -> None:
         if not self._accept_realtime_feedback:
@@ -451,11 +457,18 @@ class RealManAdapter:
             self._try_call("rm_set_joint_max_speed", joint_index, self._joint_max_speed)
             self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
 
-    def _move_to_initial_pose(self) -> None:
+    def move_to_initial_pose(self) -> None:
+        arm = 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)
+        ret = arm.rm_movej(
+            self._initial_joint_pose,
+            self._init_move_speed,
+            0,
+            0,
+            1,
+        )
         self._check_return(ret, "rm_movej(initial_joint_pose)")
 
     def _try_call(self, name: str, *args: Any) -> None:

xr_rm_teleop/xr_rm_teleop/single_arm_velocity_teleop.py

@@ -10,16 +10,19 @@ import math
 import queue
 import threading
 import time
+from dataclasses import dataclass
 from typing import Iterable
 
 import numpy as np
 import rclpy
 from geometry_msgs.msg import PoseStamped, TwistStamped
 from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
 from rclpy.time import Time
+from sensor_msgs.msg import JointState
 from std_msgs.msg import Bool
 
-from xr_rm_interfaces.msg import XrController
+from xr_rm_interfaces.msg import ActControlSample, XrController
 
 from .fun_peripheral import load_peripheral_config
 from .placo_ik_solver import PlacoIkSolver
@@ -30,6 +33,30 @@ from .realman_adapter import (
 )
 
 
+@dataclass
+class _ActCycleContext:
+    control_seq: int
+    control_monotonic_ns: int
+    feedback_monotonic_ns: int = -1
+    action_monotonic_ns: int = -1
+    feedback_age_ms: float = math.inf
+    qp_duration_ms: float = 0.0
+    q_actual: list[float] | None = None
+    q_qp_raw: list[float] | None = None
+    q_target: list[float] | None = None
+    current_pose: np.ndarray | None = None
+    raw_target_pose: np.ndarray | None = None
+    target_pose: np.ndarray | None = None
+    command_velocity: list[float] | None = None
+    feedback_valid: bool = False
+    command_sent: bool = False
+    send_failed: bool = False
+    qp_attempted: bool = False
+    qp_success: bool = False
+    target_clamped: bool = False
+    control_fault: bool = False
+
+
 def _norm(values: Iterable[float]) -> float:
     return math.sqrt(sum(value * value for value in values))
 
@@ -258,6 +285,7 @@ class SingleArmVelocityTeleop(Node):
         self._low_z_threshold = float(self.get_parameter("low_z_threshold").value)
         self._low_z_min_radius = float(self.get_parameter("low_z_min_radius").value)
         self._xr_to_robot_matrix = self._float_list_parameter("xr_to_robot_matrix", 9)
+        self._use_mock = self._bool_parameter("use_mock")
         self._follow = self._bool_parameter("follow")
         self._enable_tool_control = self._bool_parameter("enable_tool_control")
         self._enable_trigger_gripper_control = self._bool_parameter("enable_trigger_gripper_control")
@@ -289,12 +317,20 @@ class SingleArmVelocityTeleop(Node):
         self._latest_joint_positions: list[float] | None = None
         self._last_joint_command_target: list[float] | None = None
         self._last_joint_command_velocity: list[float] | None = None
+        self._last_successful_action_target: list[float] | None = None
+        self._act_control_seq = 0
         self._joint_feedback_ready = False
         self._grip_rearm_required = False
         self._feedback_resync_attempted = False
         self._control_fault_latched = False
         self._stop_sent = True
         self._trigger_tool_open = True
+        self._tool_state_lock = threading.Lock()
+        self._tool_target_open = True
+        self._tool_state_open: bool | None = None
+        self._tool_command_pending = False
+        self._tool_command_failed = False
+        self._last_primary_pressed: bool | None = None
         self._last_trigger_pressed: bool | None = None
         self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
         self._tool_worker_stop = threading.Event()
@@ -324,13 +360,33 @@ class SingleArmVelocityTeleop(Node):
         self._ik_solver = PlacoIkSolver(
             str(self.get_parameter("robot_urdf_path").value),
             self._dt,
+            peripheral_arm,
+        )
+        debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
+        self._joint_state_pub = self.create_publisher(
+            JointState,
+            f"{debug_ns}/joint_states",
+            10,
+        )
+        self._joint_target_pub = self.create_publisher(
+            JointState,
+            f"{debug_ns}/joint_target",
+            10,
+        )
+        self._act_sample_pub = self.create_publisher(
+            ActControlSample,
+            f"{debug_ns}/act_control_sample",
+            QoSProfile(
+                history=HistoryPolicy.KEEP_LAST,
+                depth=10,
+                reliability=ReliabilityPolicy.BEST_EFFORT,
+            ),
         )
         self._adapter = self._make_adapter()
         self._adapter.connect()
         self._initialize_joint_state()
         self._setup_tool_control()
 
-        debug_ns = f"{self._debug_topic_prefix}/{self._arm_name}"
         self._current_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/current_pose", 10)
         self._raw_target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/raw_target_pose", 10)
         self._target_pose_pub = self.create_publisher(PoseStamped, f"{debug_ns}/target_pose", 10)
@@ -350,7 +406,7 @@ class SingleArmVelocityTeleop(Node):
             "initial_joint_pose",
             7,
         )
-        if self._bool_parameter("use_mock"):
+        if self._use_mock:
             return MockRealManAdapter(initial_joint_pose)
 
         return RealManAdapter(
@@ -391,6 +447,13 @@ class SingleArmVelocityTeleop(Node):
             self._adapter.close()
             raise
 
+    def _publish_joint_positions(self, publisher, positions: list[float]) -> None:
+        message = JointState()
+        message.header.stamp = self.get_clock().now().to_msg()
+        message.name = self._ik_solver.joint_names
+        message.position = [float(value) for value in positions]
+        publisher.publish(message)
+
     def _reset_joint_state(
         self,
         snapshot: JointStateSnapshot,
@@ -404,15 +467,25 @@ class SingleArmVelocityTeleop(Node):
         self._last_valid_joint_target = list(positions)
         self._last_joint_command_target = list(positions)
         self._last_joint_command_velocity = [0.0] * 7
+        self._publish_joint_positions(self._joint_state_pub, positions)
         return current_pose
 
     def _setup_tool_control(self) -> None:
         peripheral_arm = self._peripheral_arm_name()
-        if self._bool_parameter("configure_peripheral_on_connect"):
+        configure_on_connect = self._bool_parameter(
+            "configure_peripheral_on_connect"
+        )
+        if configure_on_connect:
             self._adapter.configure_peripheral(
                 self._peripheral_config,
                 peripheral_arm,
             )
+            if self._peripheral_config.set_initial_tool_state:
+                with self._tool_state_lock:
+                    self._tool_target_open = True
+                    self._tool_state_open = True
+                    self._tool_command_pending = False
+                    self._tool_command_failed = False
 
         if not self._enable_tool_control:
             if self._enable_trigger_gripper_control:
@@ -462,6 +535,10 @@ class SingleArmVelocityTeleop(Node):
             )
             return
 
+        with self._tool_state_lock:
+            self._tool_target_open = open_tool
+            self._tool_command_pending = True
+
         item = (open_tool, source)
         while True:
             try:
@@ -490,15 +567,34 @@ class SingleArmVelocityTeleop(Node):
                 try:
                     self._adapter.set_tool_enabled(open_tool)
                 except Exception as exc:
+                    with self._tool_state_lock:
+                        self._tool_command_failed = True
                     self.get_logger().error(
                         f"{self._arm_name} tool {action} failed from {source}: {exc}"
                     )
                     continue
+                with self._tool_state_lock:
+                    self._tool_state_open = open_tool
+                    self._tool_command_failed = False
                 self.get_logger().info(
                     f"{self._arm_name} tool {action} command sent from {source}"
                 )
             finally:
                 self._tool_command_queue.task_done()
+                if self._tool_command_queue.empty():
+                    with self._tool_state_lock:
+                        self._tool_command_pending = False
+
+    def _tool_state_snapshot(
+        self,
+    ) -> tuple[bool, bool | None, bool, bool]:
+        with self._tool_state_lock:
+            return (
+                self._tool_target_open,
+                self._tool_state_open,
+                self._tool_command_pending,
+                self._tool_command_failed,
+            )
 
     def _peripheral_arm_name(self) -> str:
         configured = str(self.get_parameter("peripheral_arm").value).strip().lower()
@@ -514,8 +610,34 @@ class SingleArmVelocityTeleop(Node):
     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)
 
+    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)
+        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
+
+        if self._use_mock:
+            self._grip_rearm_required = False
+        self.get_logger().info(f"{self._arm_name} 已回到初始位姿。")
+
     def _handle_trigger_gripper(self, msg: XrController) -> None:
         if not self._enable_tool_control or not self._enable_trigger_gripper_control:
             return
@@ -534,6 +656,18 @@ class SingleArmVelocityTeleop(Node):
         self._enqueue_tool_command(self._trigger_tool_open, "trigger")
 
     def _control_tick(self) -> None:
+        control_seq = getattr(self, "_act_control_seq", 0)
+        self._act_control_seq = control_seq + 1
+        cycle = _ActCycleContext(
+            control_seq=control_seq,
+            control_monotonic_ns=time.monotonic_ns(),
+        )
+        try:
+            self._control_tick_impl(cycle)
+        finally:
+            self._publish_act_control_sample(cycle)
+
+    def _control_tick_impl(self, cycle: _ActCycleContext) -> None:
         tick_started_ns = time.perf_counter_ns()
         last_tick_started_ns = getattr(
             self,
@@ -548,10 +682,12 @@ class SingleArmVelocityTeleop(Node):
         )
         now = self.get_clock().now()
         if self._control_fault_latched:
+            cycle.control_fault = True
             return
 
         snapshot = self._adapter.get_latest_joint_state()
         if not self._joint_snapshot_is_motion_ready(snapshot):
+            cycle.control_fault = True
             self._grip_rearm_required = True
             if self._joint_feedback_ready:
                 self.get_logger().warn(
@@ -562,13 +698,18 @@ class SingleArmVelocityTeleop(Node):
             self._safe_stop(reset_active=True)
             return
         assert snapshot is not None
+        cycle.q_actual = list(snapshot.positions)
+        cycle.feedback_monotonic_ns = int(snapshot.received_at * 1e9)
         feedback_age = time.monotonic() - snapshot.received_at
+        cycle.feedback_age_ms = feedback_age * 1000.0
         if feedback_age < 0.0:
+            cycle.control_fault = True
             self._grip_rearm_required = True
             self._joint_feedback_ready = False
             self._safe_stop(reset_active=True)
             return
         if feedback_age > self._command_timeout_sec:
+            cycle.control_fault = True
             self._handle_stale_joint_feedback(feedback_age)
             return
 
@@ -576,6 +717,7 @@ class SingleArmVelocityTeleop(Node):
         try:
             current_pose = self._sync_joint_feedback(snapshot)
         except Exception as exc:
+            cycle.control_fault = True
             self.get_logger().error(
                 f"{self._arm_name} 关节反馈同步到 Placo 失败：{exc}",
                 throttle_duration_sec=1.0,
@@ -584,6 +726,8 @@ class SingleArmVelocityTeleop(Node):
             self._grip_rearm_required = True
             self._safe_stop(reset_active=True)
             return
+        cycle.current_pose = current_pose
+        cycle.feedback_valid = True
         if not self._joint_feedback_ready:
             if self._grip_rearm_required:
                 message = (
@@ -632,6 +776,7 @@ class SingleArmVelocityTeleop(Node):
         try:
             controller_quat = self._controller_quaternion(self._last_msg)
         except ValueError as exc:
+            cycle.control_fault = True
             self.get_logger().warn(
                 f"{self._arm_name} XR 手柄姿态无效，停止输出：{exc}",
                 throttle_duration_sec=1.0,
@@ -676,19 +821,35 @@ class SingleArmVelocityTeleop(Node):
             sent_target,
             sent_orientation,
         )
+        cycle.raw_target_pose = raw_target_pose
+        cycle.target_pose = target_pose
+        cycle.command_velocity = list(velocity)
+        cycle.target_clamped = target_clamped
 
         self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
+        cycle.qp_attempted = True
         qp_started_ns = time.perf_counter_ns()
-        joint_target = self._solve_joint_target(target_pose)
+        joint_target, qp_success = self._solve_joint_target(target_pose)
         qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6
+        cycle.qp_duration_ms = qp_ms
+        cycle.q_qp_raw = list(joint_target)
+        cycle.qp_success = qp_success
         send_started_ns = time.perf_counter_ns()
         sent = self._send_joint_target(joint_target)
         send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6
         if sent:
+            assert self._last_joint_command_target is not None
+            final_target = list(self._last_joint_command_target)
+            self._last_successful_action_target = final_target
+            cycle.q_target = final_target
+            cycle.action_monotonic_ns = time.monotonic_ns()
+            cycle.command_sent = True
             self._last_sent_target = sent_target
             self._last_sent_orientation = sent_orientation.copy()
             self._last_command_time = now
             self._stop_sent = False
+        else:
+            cycle.send_failed = True
         total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6
         try:
             self._record_timing_sample(
@@ -1140,11 +1301,18 @@ class SingleArmVelocityTeleop(Node):
         )
         self._latest_joint_positions = list(snapshot.positions)
         self._last_current_pose = current_pose
+        self._publish_joint_positions(
+            self._joint_state_pub,
+            list(snapshot.positions),
+        )
         if not self._active or self._last_valid_joint_target is None:
             self._last_valid_joint_target = list(snapshot.positions)
         return current_pose
 
-    def _solve_joint_target(self, target_pose: np.ndarray) -> list[float]:
+    def _solve_joint_target(
+        self,
+        target_pose: np.ndarray,
+    ) -> tuple[list[float], bool]:
         if self._last_valid_joint_target is None:
             raise RuntimeError("valid joint feedback has not been initialized")
         try:
@@ -1154,9 +1322,9 @@ class SingleArmVelocityTeleop(Node):
                 f"{self._arm_name} QP 求解失败，保持上一组关节目标：{exc}",
                 throttle_duration_sec=1.0,
             )
-            return list(self._last_valid_joint_target)
+            return list(self._last_valid_joint_target), False
         self._last_valid_joint_target = list(result)
-        return list(result)
+        return list(result), True
 
     def _safe_stop(self, reset_active: bool) -> None:
         if not self._stop_sent:
@@ -1228,6 +1396,10 @@ class SingleArmVelocityTeleop(Node):
             return False
         self._last_joint_command_target = limited_target
         self._last_joint_command_velocity = limited_velocity
+        self._publish_joint_positions(
+            self._joint_target_pub,
+            limited_target,
+        )
         return True
 
     @staticmethod
@@ -1324,6 +1496,122 @@ class SingleArmVelocityTeleop(Node):
         self._cmd_vel_pub.publish(velocity_msg)
         self._target_clamped_pub.publish(clamped_msg)
 
+    def _build_act_control_sample(
+        self,
+        cycle: _ActCycleContext,
+    ) -> ActControlSample:
+        message = ActControlSample()
+        message.header.stamp = self.get_clock().now().to_msg()
+        message.header.frame_id = "rm_base"
+        message.control_seq = cycle.control_seq
+        message.control_monotonic_ns = cycle.control_monotonic_ns
+        message.feedback_monotonic_ns = cycle.feedback_monotonic_ns
+        message.action_monotonic_ns = cycle.action_monotonic_ns
+        message.feedback_age_ms = float(cycle.feedback_age_ms)
+        message.qp_duration_ms = float(cycle.qp_duration_ms)
+
+        q_actual = cycle.q_actual or [0.0] * 7
+        held_target = (
+            cycle.q_target
+            or self._last_successful_action_target
+            or q_actual
+        )
+        qp_target = cycle.q_qp_raw or held_target
+        limits = np.asarray(
+            self._ik_solver.joint_position_limits,
+            dtype=float,
+        )
+        if limits.shape != (7, 2) or not np.isfinite(limits).all():
+            raise ValueError("joint limits must have finite shape (7, 2)")
+        message.q_actual = [float(value) for value in q_actual]
+        message.q_qp_raw = [float(value) for value in qp_target]
+        message.q_target = [float(value) for value in held_target]
+        message.joint_lower_limits = limits[:, 0].tolist()
+        message.joint_upper_limits = limits[:, 1].tolist()
+
+        current_pose = cycle.current_pose
+        if current_pose is None:
+            current_pose = self._debug_pose_fallback()
+        if current_pose is None:
+            current_pose = np.eye(4)
+        raw_target_pose = cycle.raw_target_pose
+        if raw_target_pose is None:
+            raw_target_pose = current_pose
+        target_pose = cycle.target_pose
+        if target_pose is None:
+            target_pose = current_pose
+        message.tcp_current = self._pose_msg(
+            message.header.stamp,
+            current_pose,
+        ).pose
+        message.tcp_raw_target = self._pose_msg(
+            message.header.stamp,
+            raw_target_pose,
+        ).pose
+        message.tcp_target = self._pose_msg(
+            message.header.stamp,
+            target_pose,
+        ).pose
+        velocity = cycle.command_velocity or [0.0] * 6
+        if len(velocity) != 6:
+            raise ValueError("ACT command velocity must contain 6 values")
+        message.tcp_command_velocity.linear.x = float(velocity[0])
+        message.tcp_command_velocity.linear.y = float(velocity[1])
+        message.tcp_command_velocity.linear.z = float(velocity[2])
+        message.tcp_command_velocity.angular.x = float(velocity[3])
+        message.tcp_command_velocity.angular.y = float(velocity[4])
+        message.tcp_command_velocity.angular.z = float(velocity[5])
+
+        controller = self._last_msg
+        if controller is not None:
+            message.pico_pose = controller.pose
+            message.pico_grip = bool(controller.grip)
+            message.pico_trigger = float(controller.trigger)
+            message.pico_primary = bool(controller.primary)
+            message.pico_secondary = bool(controller.secondary)
+            message.pico_axis = [float(value) for value in controller.axis]
+
+        tool_target, tool_state, tool_pending, tool_failed = (
+            self._tool_state_snapshot()
+        )
+        message.gripper_target_open = tool_target
+        message.gripper_state_known = tool_state is not None
+        message.gripper_state_open = bool(tool_state)
+        message.gripper_command_pending = tool_pending
+        message.gripper_command_failed = tool_failed
+
+        message.teleop_active = bool(self._active)
+        message.feedback_valid = cycle.feedback_valid
+        message.action_valid = bool(
+            self._last_successful_action_target is not None
+            and cycle.feedback_valid
+            and not cycle.send_failed
+            and not cycle.control_fault
+        )
+        message.command_sent = cycle.command_sent
+        message.qp_attempted = cycle.qp_attempted
+        message.qp_success = cycle.qp_success
+        message.target_clamped = cycle.target_clamped
+        message.control_fault = bool(
+            cycle.control_fault or self._control_fault_latched
+        )
+        return message
+
+    def _publish_act_control_sample(
+        self,
+        cycle: _ActCycleContext,
+    ) -> None:
+        publisher = getattr(self, "_act_sample_pub", None)
+        if publisher is None:
+            return
+        try:
+            publisher.publish(self._build_act_control_sample(cycle))
+        except Exception as exc:
+            self.get_logger().warn(
+                f"{self._arm_name} ACT原子样本发布失败：{exc}",
+                throttle_duration_sec=1.0,
+            )
+
     @staticmethod
     def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped:
         transform = _make_transform(pose[:3, 3], pose[:3, :3])