Files

YikaiFu-cart f137e28ed7 feat: Enhance single arm velocity teleoperation with zero velocity handling and new parameters

2026-05-26 13:51:15 +08:00

14 KiB

Raw Blame History

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。当前核心目标是跑通以下闭环：

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：

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：

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：

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

也支持数组、多种字段别名：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

控制流程：

订阅 controller_topic 的 XrController。
每个控制 tick 检查是否收到消息，未收到或超时则 _safe_stop()。
grip 未按下时停止输出，并清空 active 状态、手柄起点、机器人起点。
grip 首次按下时只锁定当前手柄位置和当前 TCP 位置，不立即运动。
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
发布调试话题：
- /xr_rm/<arm_name>/current_pose，PoseStamped
- /xr_rm/<arm_name>/target_pose，PoseStamped
- /xr_rm/<arm_name>/cmd_vel，TwistStamped
所有异常、断连、松手、退出都要发送零速度。

注意：当前阶段只控制 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。

左臂映射：

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。

右臂映射：

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]

初始点位

左臂：

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]

右臂：

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 需要包含以下命令。

构建：

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 验证：

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

检查话题：

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:=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:=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。

14 KiB Raw Blame History Unescape Escape