Add detailed project prompt for XR-RM75 dual-arm teleoperation ROS2 project

2026-05-22 13:24:30 +08:00
parent de4cbdb6be
commit 340bd9138d
2 changed files with 673 additions and 79 deletions
--- a/PROMPT.md
+++ b/PROMPT.md
@ -0,0 +1,435 @@
+# Claude Code 生成提示词：XR-RM75 双臂遥操作 ROS2 项目
+
+下面这份提示词用于交给 Claude Code，让它在一个空的 ROS2 Humble 工作空间中生成一个与当前项目大致等价的代码框架。项目不是完整复刻论文中的 XRoboToolkit，而是参考其 OpenXR 数据约定、模块化分层和 grip 相对位移遥操作思想，落地一个阶段一可调试版本：PICO 4 Ultra 双手柄通过 UDP 控制左右 RM75 机械臂的笛卡尔速度。
+
+## 当前项目整体脉络
+
+项目根目录是 ROS2 workspace 的 `src/` 层，面向 Ubuntu 22.04 + ROS2 Humble。当前核心目标是跑通以下闭环：
+
+```text
+PICO 4 Ultra 双手柄位姿/按键
+  -> UDP JSON 数据包
+  -> xr_rm_input/udp_controller_receiver
+  -> /xr/left_controller 与 /xr/right_controller
+  -> xr_rm_teleop/single_arm_velocity_teleop
+  -> MockRealManAdapter 或 RealManAdapter
+  -> /xr_rm/<arm>/current_pose、target_pose、cmd_vel 调试话题
+  -> 真机模式下调用睿尔曼 RM75 Python API2 的 rm_movev_canfd
+```
+
+当前阶段已包含：
+
+- PICO/OpenXR 手柄 UDP 输入解析。
+- 左右手柄话题分发。
+- 左右 RM75 单臂速度遥操作节点。
+- 双臂 mock、双臂真机、单臂调试 launch。
+- acRealman 来源的 RM75 IP、工作空间、圆柱半径约束、初始化点位和部分安全配置。
+- Tkinter 启动面板，用于打开 mock、真机、诊断和话题监控命令。
+- 可选的 trigger 到夹爪力控比例桥接节点。
+
+当前阶段暂不实现：
+
+- XR 侧 Unity 应用。
+- 论文中的低延迟立体视频流。
+- QP IK、灵巧手 retarget、全身追踪、运动捕捉 tracker。
+- VLA 数据集记录和训练流程。
+- 双臂碰撞模型、自动采摘算法、移动底盘和升降柱。
+
+## 论文 XRoboToolkit 对本项目的启发
+
+论文《XRoboToolkit: A Cross-Platform Framework for Robot Teleoperation》的关键思想如下，生成代码时只吸收与当前阶段相关的部分：
+
+- XR 侧遵循 OpenXR 坐标约定：右手系，`+X` 向右，`+Y` 向上，`+Z` 向后；6DoF pose 使用位置 `[x, y, z]` 和四元数 `[qx, qy, qz, qw]`。
+- 实时追踪数据可统一放在一个 JSON 对象里传输，控制器字段包括 pose、grip、trigger、摇杆和按钮等。当前项目只需要 pose、grip、trigger、hand。
+- 控制器遥操作使用 grip 作为使能键。为了稳定直观，grip 首次按下时锁定手柄起点和机器人 TCP 起点，之后机器人末端跟随“手柄相对位移”，不是直接跟随绝对 pose。
+- XRoboToolkit 是模块化结构：XR 输入、PC 服务、机器人控制、视觉反馈、数据记录彼此解耦。当前项目也按 ROS2 package 拆分：interfaces、input、teleop、bringup。
+- 论文完整系统包含 QP IK、立体视觉、运动 tracker、灵巧手 retarget 和数据采集。当前项目的 RM75 阶段一只做笛卡尔速度遥操，后续扩展时再接这些模块。
+
+## 请 Claude Code 生成的项目
+
+请在一个 ROS2 Humble workspace 的 `src/` 目录中生成以下四个 package：
+
+```text
+xr_rm_interfaces/
+  CMakeLists.txt
+  package.xml
+  msg/XrController.msg
+
+xr_rm_input/
+  package.xml
+  setup.py
+  setup.cfg
+  resource/xr_rm_input
+  launch/udp_receiver.launch.py
+  xr_rm_input/__init__.py
+  xr_rm_input/udp_controller_receiver.py
+  xr_rm_input/sample_udp_sender.py
+
+xr_rm_teleop/
+  package.xml
+  setup.py
+  setup.cfg
+  resource/xr_rm_teleop
+  xr_rm_teleop/__init__.py
+  xr_rm_teleop/realman_adapter.py
+  xr_rm_teleop/single_arm_velocity_teleop.py
+  xr_rm_teleop/gripper_trigger_bridge.py
+
+xr_rm_bringup/
+  CMakeLists.txt
+  package.xml
+  config/dual_arm_rm75.yaml
+  config/left_arm_rm75.yaml
+  config/right_arm_rm75.yaml
+  launch/dual_arm_mock.launch.py
+  launch/dual_arm_realman.launch.py
+  launch/arm_debug.launch.py
+  tools/launcher_ui.py
+```
+
+同时生成根目录 `README.md`，说明构建、mock 验证、真机验证和话题检查方式。
+
+## 接口定义
+
+`xr_rm_interfaces/msg/XrController.msg`：
+
+```text
+std_msgs/Header header
+string hand
+bool grip
+float32 trigger
+geometry_msgs/Pose pose
+```
+
+`package.xml` 和 `CMakeLists.txt` 需要使用 `rosidl_default_generators` 生成该消息，并依赖 `std_msgs`、`geometry_msgs`。
+
+## xr_rm_input 需求
+
+实现 `UdpControllerReceiver` 节点，职责是把轻量级 XR UDP JSON 转成左右手柄 ROS2 消息。
+
+节点名：`udp_controller_receiver`
+
+参数：
+
+- `udp_host`，默认 `0.0.0.0`
+- `udp_port`，默认 `15000`
+- `topic`，兼容旧版单话题，默认 `/xr/right_controller`
+- `left_topic`，默认 `/xr/left_controller`
+- `right_topic`，默认空字符串，为空时使用 `topic`
+- `default_hand`，默认 `right`
+- `timer_hz`，默认 `100.0`
+- `quat_order`，默认 `xyzw`，可选 `wxyz`
+
+实现要点：
+
+- UDP socket 使用非阻塞模式，定时器里循环读取直到 `BlockingIOError`。
+- 支持单手柄 JSON：
+
+```json
+{
+  "hand": "right",
+  "grip": true,
+  "trigger": 0.2,
+  "pos": [0.12, 1.05, -0.32],
+  "quat": [0.0, 0.0, 0.0, 1.0]
+}
+```
+
+- 支持双手柄 JSON：
+
+```json
+{
+  "controllers": {
+    "left": {
+      "grip": true,
+      "trigger": 0.0,
+      "pos": [-0.12, 1.05, -0.32],
+      "quat": [0.0, 0.0, 0.0, 1.0]
+    },
+    "right": {
+      "grip": true,
+      "trigger": 0.4,
+      "pos": [0.12, 1.05, -0.32],
+      "quat": [0.0, 0.0, 0.0, 1.0]
+    }
+  }
+}
+```
+
+- 也支持数组、多种字段别名：`pos`/`position`/`p`、`quat`/`orientation`/`q`、`pose` 对象或 `[x,y,z,qx,qy,qz,qw]` 列表。
+- hand 归一化：`left`/`l`/`left_controller` 映射为 `left`，`right`/`r`/`right_controller` 映射为 `right`。
+- grip 支持 bool 和字符串布尔值。
+- trigger clamp 到 `[0, 1]`。
+- 异常包只 warn，不让节点崩溃。
+
+实现 `sample_udp_sender.py`：
+
+- 命令行参数：`--host`、`--port`、`--seconds`、`--hz`、`--amplitude`、`--hand left|right|both`
+- 按指定频率发送模拟手柄数据。
+- 发送期间 `grip=true`，结束时给每只手发送一次 `grip=false`。
+
+`udp_receiver.launch.py` 启动 `udp_controller_receiver`，暴露 `udp_host`、`udp_port`、`topic` 参数。
+
+## xr_rm_teleop 需求
+
+### realman_adapter.py
+
+实现三个对象：
+
+- `ArmPose` dataclass，字段 `x,y,z,rx,ry,rz`，提供 `xyz()`。
+- `MockRealManAdapter`：不连接真机，保存当前 pose，`send_cartesian_velocity()` 按控制周期简单积分速度，适合 mock 验证。
+- `RealManAdapter`：封装睿尔曼 Python API2。
+
+`RealManAdapter` 要点：
+
+- 延迟导入 `from Robotic_Arm.rm_robot_interface import RoboticArm, rm_thread_mode_e`，未安装时抛出清晰错误。
+- `connect()` 中创建机械臂连接，支持 IP 和端口。
+- 可选下发安全配置：避奇异、最大线速度、最大角速度、线加速度、角加速度、各关节最大速度和加速度。
+- 可选在连接后执行 `rm_movej(initial_joint_pose)` 和 `rm_movel(initial_tcp_pose)`，默认关闭。
+- 使用 `rm_set_movev_canfd_init(avoid_singularity, frame_type, dt_ms)` 初始化速度透传。
+- `get_current_pose()` 从不同 SDK 返回结构中递归解析常见字段：`pose`、`tool_pose`、`tcp_pose`、`current_pose`。
+- `send_cartesian_velocity(velocity, follow)` 调用 `rm_movev_canfd(velocity, follow, 0, 0)`。
+- `stop()` 发送零速度并尝试 slow stop。
+- `close()` 停止并删除机械臂连接。
+
+### single_arm_velocity_teleop.py
+
+实现 `SingleArmVelocityTeleop` 节点。
+
+节点名：`single_arm_velocity_teleop`
+
+核心参数：
+
+- `arm_name`，默认 `rm75`
+- `controller_topic`，默认 `/xr/right_controller`
+- `control_rate_hz`，默认 `50.0`
+- `command_timeout_sec`，默认 `0.12`
+- `scale`，默认 `1.0`
+- `kp_linear`，默认 `2.0`
+- `deadband_m`，默认 `0.002`
+- `low_pass_alpha`，默认 `0.35`
+- `max_linear_speed`，默认 `0.05`
+- `enable_position_axes`，默认 `[true, true, true]`
+- `workspace_min`、`workspace_max`
+- `cyl_radius_limit`
+- `low_z_threshold`
+- `low_z_min_radius`
+- `xr_to_robot_matrix`，长度 9
+- `use_mock`
+- `mock_initial_pose`
+- `robot_ip`、`robot_port`
+- `avoid_singularity`、`frame_type`、`follow`
+- `configure_safety_limits`
+- `max_line_speed`、`max_angular_speed`、`max_line_acc`、`max_angular_acc`
+- `joint_max_speed`、`joint_max_acc`
+- `move_to_initial_pose_on_connect`
+- `initial_joint_pose`、`initial_tcp_pose`、`init_move_speed`
+- `debug_topic_prefix`，默认 `/xr_rm`
+
+控制流程：
+
+1. 订阅 `controller_topic` 的 `XrController`。
+2. 每个控制 tick 检查是否收到消息，未收到或超时则 `_safe_stop()`。
+3. grip 未按下时停止输出，并清空 active 状态、手柄起点、机器人起点。
+4. grip 首次按下时只锁定当前手柄位置和当前 TCP 位置，不立即运动。
+5. grip 持续按下时：
+   - 计算 `controller_delta = controller_now - controller_start`
+   - 用 `xr_to_robot_matrix` 映射为机器人坐标增量
+   - `target = robot_start + scale * robot_delta`
+   - 先按 `workspace_min/max` 限幅，再按 `cyl_radius_limit` 限制基座圆柱半径，低高度区域使用 `low_z_min_radius`
+   - `error = target - robot_now`
+   - 对禁用轴清零
+   - 小于 `deadband_m` 输出零速度，否则 `velocity = kp_linear * error`
+   - 按向量范数限制 `max_linear_speed`
+   - 做一阶低通滤波
+   - 拼成 `[vx, vy, vz, 0, 0, 0]` 发送给 adapter
+6. 发布调试话题：
+   - `/xr_rm/<arm_name>/current_pose`，`PoseStamped`
+   - `/xr_rm/<arm_name>/target_pose`，`PoseStamped`
+   - `/xr_rm/<arm_name>/cmd_vel`，`TwistStamped`
+7. 所有异常、断连、松手、退出都要发送零速度。
+
+注意：当前阶段只控制 TCP 平移，不控制姿态。
+
+### gripper_trigger_bridge.py
+
+实现可选节点 `GripperTriggerBridge`：
+
+- 订阅某个 `/xr/*_controller`
+- 发布 `std_msgs/Float32` 到夹爪力控比例话题，如 `/omnipicker/right/force_ratio`
+- 参数包括 `require_grip`、`trigger_deadband`、`open_command`、`min_close_command`、`max_close_command`、`low_pass_alpha`
+- stale 或未按 grip 时发布开爪命令。
+
+## bringup 配置需求
+
+### RM75 默认参数
+
+沿用以下现场参数：
+
+- 左臂 IP：`192.168.192.18`
+- 右臂 IP：`192.168.192.19`
+- TCP 端口：`8080`
+- 左臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]`
+- 右臂工作空间：`x[-0.70, 0.50] y[-0.60, 0.40] z[0.10, 0.70]`
+- 左右臂圆柱半径限制：`[0.20, 0.60]`
+- `low_z_threshold=0.20`
+- `low_z_min_radius=0.21`
+- 保守速度：`max_linear_speed=0.04`
+- `scale=0.75`
+- `kp_linear=1.8`
+- `low_pass_alpha=0.35`
+
+### 坐标映射
+
+遵循论文中的 PICO/OpenXR 坐标：`+X` 向右，`+Y` 向上，`+Z` 向后。
+
+左臂 base 坐标：`+X` 向下，`+Z` 向右，前方工作区对应 `-Y`。
+
+左臂映射：
+
+```text
+robot_delta = [-controller_y, -controller_z, controller_x]
+xr_to_robot_matrix:
+[0.0, -1.0, 0.0,
+ 0.0,  0.0, -1.0,
+ 1.0,  0.0, 0.0]
+```
+
+右臂 base 坐标：`+X` 向上，`+Z` 向左，前方工作区对应 `-Y`。
+
+右臂映射：
+
+```text
+robot_delta = [controller_y, -controller_z, -controller_x]
+xr_to_robot_matrix:
+[0.0, 1.0, 0.0,
+ 0.0, 0.0, -1.0,
+ -1.0, 0.0, 0.0]
+```
+
+### 初始点位
+
+左臂：
+
+```text
+mock_initial_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
+initial_joint_pose: [-167.21, 28.48, 28.21, 61.35, -14.40, 84.49, -124.51]
+initial_tcp_pose: [-0.2562, -0.2765, 0.1489, -3.0190, -0.1010, 3.1400]
+```
+
+右臂：
+
+```text
+mock_initial_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
+initial_joint_pose: [-25.60, 34.09, -19.55, 71.59, 16.97, 80.98, 59.67]
+initial_tcp_pose: [0.2663, -0.2606, 0.1027, 3.0330, 0.0000, 1.0910]
+```
+
+真机 launch 默认不要自动移动到初始化点，只有显式设置 `move_to_initial_pose_on_connect:=true` 才执行。
+
+### launch 文件
+
+`dual_arm_mock.launch.py`：
+
+- 启动 `udp_controller_receiver`
+- 启动 `left_arm_teleop` 和 `right_arm_teleop`
+- 共用 `dual_arm_rm75.yaml`
+- 覆盖 `use_mock=true`
+- launch 参数：`udp_host`、`udp_port`、`robot_config`、`run_udp`、`run_left_arm`、`run_right_arm`
+
+`dual_arm_realman.launch.py`：
+
+- 和 mock 类似，但 `use_mock=false`
+- launch 参数包含 `left_robot_ip`、`right_robot_ip`、`robot_port`、`move_to_initial_pose_on_connect`
+
+`arm_debug.launch.py`：
+
+- 参数 `arm:=left|right|both`
+- 参数 `use_mock:=true|false`
+- 单臂时读取 `left_arm_rm75.yaml` 或 `right_arm_rm75.yaml`
+- 双臂时读取 `dual_arm_rm75.yaml`
+- 总是启动 `udp_controller_receiver`
+
+### 启动面板
+
+`tools/launcher_ui.py` 做一个简单 Tkinter 面板即可：
+
+- 自动寻找 workspace 根目录。
+- 每个终端启动前 source `/opt/ros/humble/setup.bash` 和 `install/setup.bash`。
+- 模式包括 Simulation、Left Arm、Right Arm、Dual Arm、Diagnostics。
+- 提供按钮运行 mock launch、真机 launch、sample UDP sender、topic list、node list、topic echo。
+- 提供清理按钮，能关闭由面板启动的终端或进程。
+
+## README 运行流程
+
+README 需要包含以下命令。
+
+构建：
+
+```bash
+source /opt/ros/humble/setup.bash
+rosdep update
+rosdep install --from-paths src -y --ignore-src
+colcon build --symlink-install
+source install/setup.bash
+```
+
+mock 验证：
+
+```bash
+ros2 launch xr_rm_bringup dual_arm_mock.launch.py
+ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
+```
+
+检查话题：
+
+```bash
+ros2 topic echo /xr/left_controller
+ros2 topic echo /xr/right_controller
+ros2 topic echo /xr_rm/left_rm75/cmd_vel
+ros2 topic echo /xr_rm/right_rm75/cmd_vel
+```
+
+单臂调试：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
+```
+
+真机启动：
+
+```bash
+ros2 launch xr_rm_bringup dual_arm_realman.launch.py \
+  left_robot_ip:=192.168.192.18 \
+  right_robot_ip:=192.168.192.19
+```
+
+安全验证说明：
+
+- 首次真机测试保持 `max_linear_speed <= 0.04`。
+- 先只上右臂，再上左臂。
+- 每次按住 grip 后分别检查上、前后、左右方向。
+- 方向反了只改对应臂 YAML 中 `xr_to_robot_matrix` 的符号。
+- 当前没有双臂碰撞模型，首次测试要让两臂工作区物理分离。
+
+## 代码风格和验收标准
+
+代码要求：
+
+- Python 使用类型标注，异常信息清晰。
+- ROS2 节点退出时必须安全停止机械臂。
+- mock 模式不得导入或依赖睿尔曼 SDK。
+- 真机 SDK 未安装时要提示使用 `use_mock:=true` 或安装厂商 API2。
+- 参数校验要覆盖控制频率、timeout、低通系数、工作空间上下界、圆柱半径。
+- 不要引入论文中当前阶段不需要的大型依赖，如 Unity、PlaCo、Pinocchio、MuJoCo、视频流模块。
+
+验收标准：
+
+- `colcon build --symlink-install` 能通过。
+- `ros2 launch xr_rm_bringup dual_arm_mock.launch.py` 能启动三个节点。
+- `sample_udp_sender --hand both` 后能看到左右 `/xr/*_controller` 和 `/xr_rm/*/cmd_vel`。
+- 松开 grip、UDP 超时、节点退出时都会发送零速度。
+- 修改 YAML 中 `xr_to_robot_matrix` 能改变对应臂方向映射。
+- 真机模式只在 `use_mock=false` 时尝试连接 RM75。
--- a/README.md
+++ b/README.md
@ -1,44 +1,71 @@
-# XR-RM75 双臂阶段一遥操作 ROS2 工作空间
+# XR-RM75 双臂遥操作工作空间

-这个 `src/` 目录面向 **Ubuntu 22.04 + ROS2 Humble**，用于先跑通阶段一闭环：
+本仓库是一个面向 **Ubuntu 22.04 + ROS2 Humble** 的阶段一双臂遥操作项目。当前目标是先跑通 PICO/XR 手柄到双 RM75 的低速、安全、可调试闭环：

 ```text
-PICO 4 Ultra 双手柄 UDP 位姿/按键
+PICO/XR 双手柄 UDP 数据
+  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller 与 /xr/right_controller
-  -> 左右 RM75 相对位移速度遥操
-  -> PC 端查看 D405/D405/D435 视频，后续再接记录节点
+  -> xr_rm_teleop/single_arm_velocity_teleop
+  -> 左右 RM75 笛卡尔相对位移速度控制
+  -> /xr_rm/<arm_name>/current_pose、target_pose、cmd_vel 调试话题
 ```

-当前配置参考了最新 `acDual-arm-YikaiFuTest` 中的双臂 acRealman 参数，但本项目仍然采用 PICO 手柄相对位移遥操，不直接搬用戴盟的绝对 `PoseStamped` retarget 链路。
+当前控制方式是“手柄相对位移”遥操作：按住 `grip` 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，再用速度命令跟随。松开 `grip`、UDP 超时或节点退出时会发送零速度。

 ## 当前范围

- 已纳入：PICO 双手柄输入、左右 RM75 相对位移控制、左右臂独立/双臂调试 launch。
- 暂不纳入：末端执行器、AUBO 移动底盘、升降柱、XR 视频流、自动采摘算法。
+已完成：

-## 关键配置来源
+- PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
+- 左臂、右臂、双臂的 mock 调试和真机调试 launch。
+- RM75 真机连接适配，包含速度透传初始化、安全速度/加速度配置、可选初始化点位移动。
+- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。

-来自 `acDual-arm-YikaiFuTest/realman_pkg/core/arg_cfg.py`：
+暂未完成：

- 左臂 IP：`192.168.192.18`
- 右臂 IP：`192.168.192.19`
- 左右臂 TCP 端口：`8080`
- 左臂工作空间：`x[-0.50, 0.50] y[-0.60, -0.20] z[0.10, 0.50]`
- 右臂工作空间：`x[-0.70, 0.50] y[-0.60, 0.40] z[0.10, 0.70]`
- 圆柱半径限制：左右臂均为 `r[0.20, 0.60]`，低高度区域最小半径收紧到 `0.21`
- 左右臂初始化关节/末端点位、工作空间和 IP 沿用最新 acRealman 配置。
+- 末端执行器与采摘夹爪完整 launch 集成。
+- D405/D435 视频流、数据记录、相机标定和目标检测链路。
+- 双臂碰撞模型、任务级状态机、自动采摘策略。
+- PICO 端正式应用与 ROS 端的完整时间同步和状态回传。

-`dual_arm_rm75.yaml` 里的左右 `xr_to_robot_matrix` 依据论文中的 OpenXR/PICO 手柄坐标和现场双臂 base 坐标图设置：
+## 项目结构

- PICO/OpenXR：`+X` 向右，`+Y` 向上，`+Z` 向后。
- 左臂：`+X` 向下，`+Z` 向右，前方工作区对应 `-Y`。
- 右臂：`+X` 向上，`+Z` 向左，前方工作区对应 `-Y`。
+```text
+src/
+├── README.md
+├── xr_rm_bringup/
+│   ├── config/
+│   │   ├── dual_arm_rm75.yaml       # 双臂配置：left_arm_teleop 与 right_arm_teleop
+│   │   ├── left_arm_rm75.yaml       # 左臂单独调试配置
+│   │   └── right_arm_rm75.yaml      # 右臂单独调试配置
+│   ├── launch/
+│   │   ├── arm_debug.launch.py      # 推荐主入口：arm:=left/right/both, use_mock:=true/false
+│   │   ├── dual_arm_mock.launch.py  # 双臂 mock 快速入口
+│   │   └── dual_arm_realman.launch.py
+│   └── tools/
+│       └── launcher_ui.py           # 图形化调试启动面板
+├── xr_rm_input/
+│   ├── launch/
+│   │   └── udp_receiver.launch.py   # 低层 UDP 接收测试入口
+│   └── xr_rm_input/
+│       ├── udp_controller_receiver.py
+│       └── sample_udp_sender.py     # 本机模拟手柄 UDP 数据
+├── xr_rm_interfaces/
+│   └── msg/
+│       └── XrController.msg         # hand/grip/trigger/pose
+└── xr_rm_teleop/
+    └── xr_rm_teleop/
+        ├── single_arm_velocity_teleop.py
+        ├── realman_adapter.py
+        └── gripper_trigger_bridge.py
+```

-真机上仍需低速确认方向。
+`single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。

-## 构建
+## 环境准备

-在包含 `src/` 的工作空间根目录执行：
+在工作空间根目录，也就是包含 `src/` 的目录执行：

 ```bash
 source /opt/ros/humble/setup.bash
@ -48,43 +75,74 @@ colcon build --symlink-install
 source install/setup.bash
 ```

-## UI 启动面板
+真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。

-项目内置了一个类似 acRealman 的 Tkinter 启动面板，包含仿真、左臂真机、右臂真机、双臂真机、sample UDP 发送、常用 topic 检查和清理按钮。
+如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：

-从工作空间根目录直接运行源码脚本：
+```bash
+export XR_RM_WS=/home/robot/WS_xr
+```
+
+## 使用 launcher_ui.py 调试
+
+推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、source ROS2 与 `install/setup.bash`，并把每个命令放到独立终端中运行。
+
+源码方式启动：

 ```bash
 python3 src/xr_rm_bringup/tools/launcher_ui.py
 ```

-构建并 `source install/setup.bash` 后，也可以通过 ROS2 入口运行：
+构建后也可以通过 ROS2 入口启动：

 ```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
 ros2 run xr_rm_bringup launcher_ui
 ```

-## 优先验证：双臂 mock
+面板顶部的 `Mode` 分为五类：

-mock 模式不会连接真实 RM75，适合先验证 UDP 输入、左右手柄路由和相对位移控制。
+- `Simulation`：左臂 mock、右臂 mock、双臂 mock、sample UDP 发送、one-click mock demo。
+- `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` 检查。

-终端 1：
+常用按钮：
+
+- `Run Selected`：运行当前选中的命令。双击列表项也可以运行。
+- `Check Env`：检查 ROS2 Humble、工作空间 build、终端、核心 ROS 包、睿尔曼 API2。
+- `Stop All`：结束由本工作空间启动的 launch、sample sender、topic monitor、相关 ROS 节点和终端窗口。
+
+每个模式都会附带两个监控入口：
+
+- `Open Controller Topic Monitor`：用 Terminator 分屏同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
+- `Open ROS Topic/Node List Monitor`：用 Terminator 分屏每秒刷新 `ros2 topic list` 和 `ros2 node list`。
+
+分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：

 ```bash
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 launch xr_rm_bringup dual_arm_mock.launch.py
+sudo apt install terminator wmctrl xdotool
+sudo update-alternatives --config x-terminal-emulator
 ```

-终端 2：
+## 推荐调试顺序
+
+第一步：检查环境。
+
+打开 `launcher_ui.py`，点击 `Check Env`。如果 `install/setup.bash` 缺失，先回工作空间根目录重新执行 `colcon build --symlink-install`。
+
+第二步：跑 mock 闭环。
+
+在 `Simulation` 模式运行 `One-Click Dual Mock Demo`，或分开运行：

 ```bash
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
+ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 --seconds 10
 ```

-检查话题：
+观察：

 ```bash
 ros2 topic echo /xr/left_controller
@ -93,48 +151,79 @@ ros2 topic echo /xr_rm/left_rm75/cmd_vel
 ros2 topic echo /xr_rm/right_rm75/cmd_vel
 ```

-也可以使用统一调试入口分别验证：
+第三步：单臂真机。
+
+先只上一个臂，确认网络、方向、急停和限幅：

 ```bash
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=true
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=true
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
+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
 ```

-## 连接双 RM75
-
-先安装睿尔曼 Python API2，并确认两台控制器网络可达。
+第四步：双臂真机。

 ```bash
-source /opt/ros/humble/setup.bash
-source install/setup.bash
-ros2 launch xr_rm_bringup dual_arm_realman.launch.py \
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
  left_robot_ip:=192.168.192.18 \
  right_robot_ip:=192.168.192.19
 ```

-更推荐用统一调试入口按阶段放开：
+默认不会自动移动到初始化点。只有在确认安全区清空后，才显式打开：

 ```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
-
-# 双臂一起调试
-ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
+  move_to_initial_pose_on_connect:=true
 ```

-真机 launch 默认不会让机械臂自动移动到 acRealman 初始化点。若现场确认安全区已清空，可显式开启：
+## Launch 入口说明

-```bash
-ros2 launch xr_rm_bringup dual_arm_realman.launch.py move_to_initial_pose_on_connect:=true
-```
+`arm_debug.launch.py` 是当前推荐主入口。

-## UDP JSON 格式
+常用参数：

-单个手柄数据：
+- `arm`：`left`、`right`、`both`，默认 `right`。
+- `use_mock`：`true` 不连接真机，`false` 连接 RM75。
+- `udp_host`：UDP 监听地址，默认 `0.0.0.0`。
+- `udp_port`：UDP 监听端口，默认 `15000`。
+- `left_robot_ip`：左臂 IP，默认 `192.168.192.18`。
+- `right_robot_ip`：右臂 IP，默认 `192.168.192.19`。
+- `robot_port`：RM75 TCP 端口，默认 `8080`。
+- `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。
+
+`dual_arm_mock.launch.py` 和 `dual_arm_realman.launch.py` 是双臂快速入口，适合脚本化启动。它们也支持 `run_udp`、`run_left_arm`、`run_right_arm` 等开关，用于只启动部分节点。
+
+## 配置文件说明
+
+`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`。
+
+重点参数：
+
+- `controller_topic`：订阅的手柄话题。
+- `scale`：手柄位移到 TCP 位移的比例。
+- `kp_linear`：位置误差到速度命令的比例增益。
+- `max_linear_speed`：软件侧最大线速度，当前默认 `0.04`，真机初期保持低速。
+- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
+- `cyl_radius_limit`：基座圆柱半径限制。
+- `xr_to_robot_matrix`：PICO/OpenXR 位移到 RM75 base 坐标的映射矩阵。
+- `mock_initial_pose`：mock 模式初始 TCP 位姿。
+- `initial_joint_pose` / `initial_tcp_pose`：可选真机初始化点。
+
+当前坐标约定：
+
+- PICO/OpenXR：`+X` 向右，`+Y` 向上，`+Z` 向后。
+- 左臂映射：机器人位移增量 = `[-手柄y, -手柄z, 手柄x]`。
+- 右臂映射：机器人位移增量 = `[手柄y, -手柄z, -手柄x]`。
+
+如果某个方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
+
+## UDP 数据格式
+
+单个手柄：

 ```json
 {
@ -146,7 +235,7 @@ ros2 launch xr_rm_bringup dual_arm_realman.launch.py move_to_initial_pose_on_con
 }
 ```

-也支持一次发送左右手柄：
+左右手柄一起发送：

 ```json
 {
@ -167,23 +256,93 @@ ros2 launch xr_rm_bringup dual_arm_realman.launch.py move_to_initial_pose_on_con
 }
 ```

-`grip` 是机械臂运动使能：按下时锁定当前手柄和 TCP 起点，之后只跟随相对位移；松开、超时或节点退出都会发送零速度。
+字段说明：

-## 真机前方向验证
+- `hand`：`left` 或 `right`。
+- `grip`：运动使能。`true` 时进入相对位移控制，`false` 时停止。
+- `trigger`：扳机值，范围 `0.0-1.0`。当前控制主链路不使用它，`gripper_trigger_bridge.py` 可将其转换为夹爪力控比例。
+- `pos`：手柄位置，长度 3。
+- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。

-第一轮保持 `max_linear_speed <= 0.04`，先只上右臂：
+## 真机安全验证

-1. 按住右手 `grip`，手柄缓慢上移，确认 TCP 方向。
-2. 手柄缓慢前推/后拉，确认 TCP 方向。
-3. 手柄缓慢左移/右移，确认 TCP 方向。
-4. 若任一方向相反，只改 `dual_arm_rm75.yaml` 中对应臂的 `xr_to_robot_matrix` 符号。
-5. 右臂确认后再开放左臂。
+第一次接真机时按这个顺序走：

-当前双臂是两个独立控制节点，没有跨臂碰撞模型；首次测试请让两臂工作区物理分离。
+1. 确认急停、网络、机械臂工作区和人员位置。
+2. `launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75`。
+3. 单臂启动，`move_to_initial_pose_on_connect:=false`。
+4. 手握急停，按住 `grip` 后只做小幅单轴移动。
+5. 逐个确认上/下、前/后、左/右方向。
+6. 确认松开 `grip` 后 `/xr_rm/<arm>/cmd_vel` 归零。
+7. 左右臂都确认后，再进入双臂模式。

-## 从 acRealman 迁入的优化
+当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。

- 右臂工作空间改为 acRealman 独立右臂边界，不再沿用左臂窄范围。
- 速度遥操目标在笛卡尔边界外增加基座圆柱半径限制，减少低位近基座不可达目标。
- 真机连接后会下发 acRealman 同款 RM75 硬件速度、加速度和避奇异配置。
- 保留 acRealman 的左右臂 `movej`/`movel` 初始化点位，可通过 launch 参数显式启用。
+## 后续优化路线
+
+为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进。
+
+### 1. 先把遥操作闭环做稳
+
+- 用真实 PICO 数据替换 `sample_udp_sender`，固定 UDP JSON 协议和坐标系。
+- 增加手柄数据频率、延迟、丢包统计话题。
+- 记录 `/xr/*_controller`、`/xr_rm/*/cmd_vel`、`/xr_rm/*/current_pose`，用于回放和复盘。
+- 完成 `xr_to_robot_matrix` 的现场标定流程，把标定结果固化进 YAML。
+
+### 2. 提升真机安全性
+
+- 加入启动前安全检查节点：网络可达、SDK 可用、初始姿态在工作区内。
+- 增加硬件急停状态、软件急停 topic 和 UI 上的 Stop 状态提示。
+- 给左右臂增加更细的工作区限制，尤其是双臂中间区域的互斥边界。
+- 在速度命令外再加一层限幅和加速度限制，降低手柄抖动带来的突变。
+
+### 3. 集成末端执行器
+
+- 将 `gripper_trigger_bridge.py` 纳入 bringup launch。
+- 明确夹爪 topic、力控比例、开合方向和安全上限。
+- 在 `launcher_ui.py` 中加入夹爪测试按钮和夹爪状态监控。
+- 将 `trigger` 从“预留字段”变成稳定的夹爪控制输入。
+
+### 4. 接入视觉和数据记录
+
+- 加入 D405/D435 相机 launch、相机内外参和 TF。
+- 统一保存手柄、机械臂、相机、夹爪状态，用 rosbag2 做实验记录。
+- 增加一键启动“采集模式”：双臂遥操作 + 相机 + 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
+```
+
+真机模式提示缺少 `Robotic_Arm`：
+
+```text
+未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。
+```
+
+Controller topic 没有数据：
+
+- 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
+- 确认端口是 `15000`，或 launch 与发送端端口一致。
+- 用 `sample_udp_sender` 在本机验证接收链路。
+
+机械臂不动：
+
+- 确认 `grip=true`。
+- 确认 `/xr_rm/<arm>/cmd_vel` 是否有非零速度。
+- 确认目标 TCP 没有被工作空间边界或圆柱半径限制夹住。
+- 确认真机 SDK 连接成功，且 RM75 没有报警或急停。