# XR-RM75 双臂遥操作工作空间 本仓库是一个面向 **Ubuntu 22.04 + ROS2 Humble** 的阶段一双臂遥操作项目。当前目标是先跑通 PICO/XR 手柄到双 RM75 的低速、安全、可调试闭环： ```text 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//current_pose、target_pose、cmd_vel 调试话题 ``` 当前控制方式是“手柄相对位移”遥操作：按住 `grip` 时锁定当前手柄位置和机械臂 TCP 位置，之后根据手柄相对位移生成目标 TCP，再用速度命令跟随。松开 `grip`、UDP 超时或节点退出时会发送零速度。 ## 当前范围 已完成： - PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。 - 左臂、右臂、双臂的 mock 调试和真机调试 launch。 - RM75 真机连接适配，包含速度透传初始化、安全速度/加速度配置、可选初始化点位移动。 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。 暂未完成： - 末端执行器与采摘夹爪完整 launch 集成。 - D405/D435 视频流、数据记录、相机标定和目标检测链路。 - 双臂碰撞模型、任务级状态机、自动采摘策略。 - PICO 端正式应用与 ROS 端的完整时间同步和状态回传。 ## 项目结构 ```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/` 的目录执行： ```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 ``` 真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。 如果希望 `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 ``` 构建后也可以通过 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。 - `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。若提示不支持，可安装并切换： ```bash sudo apt install terminator wmctrl xdotool sudo update-alternatives --config x-terminal-emulator ``` ## 推荐调试顺序 第一步：检查环境。 打开 `launcher_ui.py`，点击 `Check Env`。如果 `install/setup.bash` 缺失，先回工作空间根目录重新执行 `colcon build --symlink-install`。 第二步：跑 mock 闭环。 在 `Simulation` 模式运行 `One-Click Dual Mock Demo`，或分开运行： ```bash ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true ros2 run xr_rm_input sample_udp_sender --hand both --host 127.0.0.1 --port 15000 --seconds 10 ``` 观察： ```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:=false ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false ``` 第四步：双臂真机。 ```bash ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ left_robot_ip:=192.168.192.18 \ right_robot_ip:=192.168.192.19 ``` 默认不会自动移动到初始化点。只有在确认安全区清空后，才显式打开： ```bash ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \ move_to_initial_pose_on_connect:=true ``` ## Launch 入口说明 `arm_debug.launch.py` 是当前推荐主入口。 常用参数： - `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 { "hand": "right", "grip": true, "trigger": 0.2, "pos": [0.12, 1.05, -0.32], "quat": [0.0, 0.0, 0.0, 1.0] } ``` 左右手柄一起发送： ```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] } } } ``` 字段说明： - `hand`：`left` 或 `right`。 - `grip`：运动使能。`true` 时进入相对位移控制，`false` 时停止。 - `trigger`：扳机值，范围 `0.0-1.0`。当前控制主链路不使用它，`gripper_trigger_bridge.py` 可将其转换为夹爪力控比例。 - `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//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. 集成末端执行器 - 将 `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//cmd_vel` 是否有非零速度。 - 确认目标 TCP 没有被工作空间边界或圆柱半径限制夹住。 - 确认真机 SDK 连接成功，且 RM75 没有报警或急停。