229 lines
7.7 KiB
Markdown
Executable File
229 lines
7.7 KiB
Markdown
Executable File
# XR-RM75 双臂遥操作
|
||
|
||
基于 **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 / XRoboToolkit
|
||
-> UDP JSON
|
||
-> xr_rm_input/udp_controller_receiver
|
||
-> /xr/left_controller、/xr/right_controller
|
||
-> xr_rm_teleop/single_arm_velocity_teleop
|
||
-> 相对 TCP 目标 + Placo QP
|
||
-> Mock 或 RM75 rm_movej_canfd
|
||
-> joint_states / 调试话题
|
||
├── xr_rm_mujoco/dual_arm_simulator
|
||
└── ActControlSample + D455/D405
|
||
-> act_episode_recorder
|
||
-> episode_<编号>.hdf5
|
||
```
|
||
|
||
工作空间包含五个 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`。
|
||
|
||
## 环境与构建
|
||
|
||
在工作空间根目录执行：
|
||
|
||
```bash
|
||
cd /home/robot/WS_xr
|
||
source /opt/ros/humble/setup.bash
|
||
rosdep update
|
||
rosdep install --from-paths src -y --ignore-src
|
||
colcon build --symlink-install
|
||
source install/setup.bash
|
||
```
|
||
|
||
遥操作、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 覆盖这些版本。
|
||
|
||
真机模式另需睿尔曼 Python API2。ACT 采集需要 `h5py` 和 `pyrealsense2`，
|
||
三相机测试还需要 OpenCV。Mock 模式不依赖厂商 SDK。
|
||
|
||
## 快速开始
|
||
|
||
以下命令均在 `/home/robot/WS_xr` 执行，并先 source ROS2 与 `install/setup.bash`。
|
||
|
||
### Mock
|
||
|
||
```bash
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
|
||
```
|
||
|
||
另开终端发送模拟手柄数据：
|
||
|
||
```bash
|
||
ros2 run xr_rm_input sample_udp_sender \
|
||
--hand both --host 127.0.0.1 --port 15000 \
|
||
--pattern axis_sweep --seconds 30
|
||
```
|
||
|
||
单臂调试时将 `arm` 改为 `left` 或 `right`。推荐先分别完成左右单臂
|
||
Mock，再进入双臂或真机验证。
|
||
|
||
### MuJoCo
|
||
|
||
```bash
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||
arm:=both use_mock:=true use_mujoco:=true
|
||
```
|
||
|
||
MuJoCo 只订阅关节状态，不参与控制。真机显示可将 `use_mock` 改为 `false`，
|
||
但该命令会同时连接两台 RM75。
|
||
|
||
### PICO 输入
|
||
|
||
只保留一个 UDP 输入源，然后启动 XRoboToolkit bridge：
|
||
|
||
```bash
|
||
ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
|
||
--host 127.0.0.1 --port 15000 --hz 90
|
||
```
|
||
|
||
确认左右 topic 持续接收数据：
|
||
|
||
```bash
|
||
ros2 topic hz /xr/left_controller
|
||
ros2 topic hz /xr/right_controller
|
||
```
|
||
|
||
图形启动面板可运行 `python3 src/xr_rm_bringup/tools/launcher_ui.py`，提供
|
||
Simulation、MuJoCo、Real Hardware 和 Diagnostics 模式。
|
||
|
||
### 真机
|
||
|
||
确认对应 YAML 中 `move_to_initial_pose_on_connect: false`，再从单臂开始：
|
||
|
||
```bash
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false
|
||
```
|
||
|
||
单臂方向、限位、急停、超时停止和夹爪均验证后，才能启动双臂：
|
||
|
||
```bash
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false
|
||
```
|
||
|
||
按住 `grip` 控制对应机械臂；松开后停止。点击 `trigger` 切换对应夹爪开/关。
|
||
|
||
## RealSense 与 ACT 采集
|
||
|
||
### 三相机测试
|
||
|
||
连接两台 D405 和一台 D455 后执行：
|
||
|
||
```bash
|
||
/home/robot/miniconda3/envs/xr/bin/python \
|
||
src/xr_rm_bringup/tools/realsense_multi_camera_test.py
|
||
```
|
||
|
||
默认将序列号 `260322272273` 识别为左腕 D405，另一台 D405 为右腕，D455 为
|
||
全局相机。按 `S` 保存三路快照，按 `Q` 或 `Esc` 退出并打印链路汇总。
|
||
快照目录为 `src/xr_rm_bringup/test/camera_test_output/`。
|
||
|
||
### ACT episode
|
||
|
||
ACT 目前仅支持右臂真机。以下命令会连接并控制右侧 RM75：
|
||
|
||
```bash
|
||
ros2 launch xr_rm_bringup arm_debug.launch.py \
|
||
arm:=right use_mock:=false record_act:=true
|
||
```
|
||
|
||
`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
|
||
source /opt/ros/humble/setup.bash
|
||
colcon build --symlink-install
|
||
colcon test --event-handlers console_direct+
|
||
colcon test-result --verbose
|
||
```
|
||
|
||
涉及遥操作姿态控制时，额外运行：
|
||
|
||
```bash
|
||
pytest src/xr_rm_teleop/test/test_orientation_control.py
|
||
```
|
||
|
||
真机验证不属于自动测试。默认使用 `use_mock:=true`，未经现场安全确认不要连接或
|
||
移动机械臂。
|