ZhengLiu-cart/acRealman_xr_from_Yikai
1
0
Fork 0
forked from YikaiFu-cart/acRealman_xr
Code Pull Requests Activity

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

Browse Source
This commit is contained in:
YikaiFu-cart
2026-05-22 13:24:30 +08:00
parent de4cbdb6be
commit 340bd9138d
2 changed files with 673 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files

317
README.md
View File

@ -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 没有报警或急停。