412 lines
14 KiB
Markdown
412 lines
14 KiB
Markdown
# 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 单臂速度遥操作节点。
|
||
- 通过统一的 `arm_debug.launch.py` 支持双臂 mock、双臂真机和单臂调试。
|
||
- acRealman 来源的 RM75 IP、工作空间、圆柱半径约束、初始化点位和部分安全配置。
|
||
- Tkinter 启动面板，用于打开 mock、真机、诊断和话题监控命令。
|
||
|
||
当前阶段暂不实现：
|
||
|
||
- XR 侧 Unity 应用。
|
||
- 夹爪 trigger 桥接和末端执行器 launch 集成。
|
||
- 论文中的低延迟立体视频流。
|
||
- 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_bringup/
|
||
CMakeLists.txt
|
||
package.xml
|
||
config/dual_arm_rm75.yaml
|
||
config/left_arm_rm75.yaml
|
||
config/right_arm_rm75.yaml
|
||
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 平移，不控制姿态。
|
||
|
||
## 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 文件
|
||
|
||
`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`
|
||
- 这是唯一的遥操作 launch 主入口，UI 和命令行调试都优先使用它。
|
||
|
||
### 启动面板
|
||
|
||
`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 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
|
||
```
|
||
|
||
检查话题：
|
||
|
||
```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 arm_debug.launch.py arm:=both use_mock:=false \
|
||
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 arm_debug.launch.py arm:=both use_mock:=true` 能启动三个节点。
|
||
- `sample_udp_sender --hand both` 后能看到左右 `/xr/*_controller` 和 `/xr_rm/*/cmd_vel`。
|
||
- 松开 grip、UDP 超时、节点退出时都会发送零速度。
|
||
- 修改 YAML 中 `xr_to_robot_matrix` 能改变对应臂方向映射。
|
||
- 真机模式只在 `use_mock=false` 时尝试连接 RM75。
|