XR-RM75 双臂遥操作工作空间

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

PICO/XR 双手柄 UDP 数据
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller 与 /xr/right_controller
  -> xr_rm_teleop/single_arm_velocity_teleop
  -> 左右 RM75 笛卡尔相对位移速度控制
  -> /xr_rm/<arm_name>/current_pose、target_pose、cmd_vel 调试话题

当前控制方式是“手柄相对位移”遥操作：按住 grip 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，再用速度命令跟随。松开 grip、UDP 超时或节点退出时会发送零速度。

当前范围

已完成：

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

暂未完成：

• 末端执行器与采摘夹爪完整 launch 集成。
• D405/D435 视频流、数据记录、相机标定和目标检测链路。
• 双臂碰撞模型、任务级状态机、自动采摘策略。
• PICO 端正式应用与 ROS 端的完整时间同步和状态回传。

项目结构

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
│   └── 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

single_arm_velocity_teleop 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 left_arm_teleop 和 right_arm_teleop。

环境准备

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

source /opt/ros/humble/setup.bash
rosdep update
rosdep install --from-paths src -y --ignore-src
colcon build --symlink-install
source install/setup.bash

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

如果希望 launcher_ui.py 从任意目录找到工作空间，可以设置：

export XR_RM_WS=/home/robot/WS_xr

使用 launcher_ui.py 调试

推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、source ROS2 与 install/setup.bash，并把每个命令放到独立终端中运行。

源码方式启动：

python3 src/xr_rm_bringup/tools/launcher_ui.py

构建后也可以通过 ROS2 入口启动：

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。
• 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：用 Terminator 分屏同时查看 /xr/left_controller 和 /xr/right_controller。
• Open ROS Topic/Node List Monitor：用 Terminator 分屏每秒刷新 ros2 topic list 和 ros2 node list。

分屏监控依赖 x-terminal-emulator 指向 Terminator。若提示不支持，可安装并切换：

sudo apt install terminator wmctrl xdotool
sudo update-alternatives --config x-terminal-emulator

推荐调试顺序

第一步：检查环境。

打开 launcher_ui.py，点击 Check Env。如果 install/setup.bash 缺失，先回工作空间根目录重新执行 colcon build --symlink-install。

第二步：跑 mock 闭环。

在 Simulation 模式运行 One-Click Dual Mock Demo，或分开运行：

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

观察：

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

第三步：单臂真机。

先只上一个臂，确认网络、方向、急停和限幅：

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 \
  left_robot_ip:=192.168.192.18 \
  right_robot_ip:=192.168.192.19

默认不会自动移动到初始化点。只有在确认安全区清空后，才显式打开：

ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
  move_to_initial_pose_on_connect:=true

Launch 入口说明

arm_debug.launch.py 是当前唯一的遥操作 launch 主入口，launcher_ui.py 中的 mock、单臂真机和双臂真机按钮都调用它。

常用参数：

• arm：left、right、both，默认 right。
• use_mock：true 不连接真机，false 连接 RM75。
• udp_host：UDP 监听地址，默认 0.0.0.0。
• udp_port：UDP 监听端口，默认 15000。
• 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。

配置文件说明

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 数据格式

单个手柄：

{
  "hand": "right",
  "grip": true,
  "trigger": 0.2,
  "pos": [0.12, 1.05, -0.32],
  "quat": [0.0, 0.0, 0.0, 1.0]
}

左右手柄一起发送：

{
  "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]
    }
  }
}

字段说明：

• hand：left 或 right。
• grip：运动使能。true 时进入相对位移控制，false 时停止。
• trigger：扳机值，范围 0.0-1.0。当前控制主链路不使用它，预留给后续夹爪集成。
• pos：手柄位置，长度 3。
• quat：手柄姿态四元数，默认按 xyzw 解析。

真机安全验证

第一次接真机时按这个顺序走：

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. 左右臂都确认后，再进入双臂模式。

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

后续优化路线

为了达到“稳定可用的双臂 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. 集成末端执行器

• 新增夹爪控制节点或桥接节点，并纳入 arm_debug.launch.py 与 launcher_ui.py。
• 明确夹爪 topic、力控比例、开合方向和安全上限。
• 在 launcher_ui.py 中加入夹爪测试按钮和夹爪状态监控。
• 将 trigger 从“预留字段”变成稳定的夹爪控制输入。

4. 接入视觉和数据记录

• 加入 D405/D435 相机 launch、相机内外参和 TF。
• 统一保存手柄、机械臂、相机、夹爪状态，用 rosbag2 做实验记录。
• 增加一键启动“采集模式”：双臂遥操作 + 相机 + rosbag2。
• 从记录数据中提取标注样本，为后续检测/策略学习做准备。

5. 从遥操作走向半自动

• 先做目标检测和 3D 定位，只给操作者显示目标点，不直接控制机械臂。
• 再做单臂辅助：自动靠近目标，人工确认抓取。
• 最后做双臂任务分配：左/右臂根据目标位置、可达性和避碰约束自动选择。
• 加入任务级状态机：搜索、接近、抓取、放置、恢复、异常停止。

常见问题

launcher_ui.py 提示找不到 install/setup.bash：

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash

真机模式提示缺少 Robotic_Arm：

未安装睿尔曼 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 没有报警或急停。