XR-RM75 双臂遥操作工作空间

本仓库是面向 Ubuntu 22.04 + ROS2 Humble + PICO 4 Ultra + 睿尔曼 RM75 的阶段一 XR 双臂遥操作项目。当前目标是先跑通一条低速、安全、可调试的闭环：

PICO/XR 双手柄 UDP JSON
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller 与 /xr/right_controller
  -> xr_rm_teleop/single_arm_velocity_teleop
  -> 左右 RM75 笛卡尔相对位姿透传控制
  -> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题

当前控制方式是“手柄相对位姿透传”遥操作：按住 grip 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP，经过工作空间限幅、目标低通、姿态低通和单帧步长限制后，通过 rm_movep_canfd 下发目标位姿。松开 grip、UDP 超时或节点退出时会请求机械臂慢停。

当前范围

已完成：

• PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
• 通过统一的 arm_debug.launch.py 支持左臂、右臂、双臂的 mock 调试和真机调试。
• RM75 真机连接适配，包含 rm_movep_canfd 位姿透传、安全速度/加速度配置、可选初始化点位移动。
• 真机模式下，点击对应手柄 trigger 可切换并保持对应夹爪开/关状态。
• Tkinter 启动面板 launcher_ui.py，用于现场快速启动、监控 topic、检查环境和清理进程。
• 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、grip、trigger 和 pose 诊断字段。

暂未完成：

• D405/D435 视频流、数据记录、相机标定和目标检测链路。
• 双臂碰撞模型、任务级状态机、自动采摘策略。
• PICO 端与 ROS 端的完整时间同步和状态回传。

项目结构

src/
├── README.md                       # 项目主文档
├── CODEX.md                        # Codex/Claude Code 项目工作流和安全规则
├── docs/
│   └── pico_udp_sender_ubuntu22_setup.md   # Ubuntu 22.04 下 PICO UDP Sender 配置教程
├── unity/
│   ├── XR_RM_PICO_UDP_Sender/      # PICO 4 Ultra UDP Sender Unity 工程
│   │   ├── Assets/Editor/          # Android/PICO 设置与 APK 构建菜单
│   │   ├── Assets/Scripts/         # UDP sender、配置面板、KeepAwake
│   │   ├── Assets/Resources/       # PICO 资源与 Roboto TMP 字体
│   │   ├── Packages/               # Unity package manifest
│   │   └── ProjectSettings/
│   └── PICO-Unity-Integration-SDK-release_3.4.0/
├── xr_rm_bringup/
│   ├── config/
│   │   ├── dual_arm_rm75.yaml      # 双臂配置：left_arm_teleop 与 right_arm_teleop
│   │   ├── left_arm_rm75.yaml      # 左臂单独调试配置
│   │   ├── right_arm_rm75.yaml     # 右臂单独调试配置
│   │   └── peripherals_rm75.yaml   # 左右臂末端外设配置
│   ├── launch/
│   │   └── arm_debug.launch.py     # 统一入口：arm:=left/right/both, use_mock:=true/false
│   └── tools/
│       ├── launcher_ui.py          # 图形化调试启动面板
│       └── realman_dual_arm_state_monitor.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
        └── fun_peripheral.py

single_arm_velocity_teleop 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 left_arm_teleop 和 right_arm_teleop。

环境准备

在工作空间根目录，也就是包含 src/ 的目录执行：

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

真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 Robotic_Arm 包。

如果希望 launcher_ui.py 从任意目录找到工作空间，可以设置：

export XR_RM_WS=/home/robot/WS_xr

使用 launcher_ui.py 调试

推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、source ROS2 与 install/setup.bash，并把每个命令放到独立终端中运行。

源码方式启动：

cd /home/robot/WS_xr
python3 src/xr_rm_bringup/tools/launcher_ui.py

构建后也可以通过 ROS2 入口启动：

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、controller 位置/频率监控。
• 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：同时查看 /xr/left_controller 和 /xr/right_controller。
• Open Target Velocity Monitor：同时查看 /xr_rm/left_rm75/cmd_vel 和 /xr_rm/right_rm75/cmd_vel；该话题表示目标位姿变化率，仅用于调试。
• Open ROS Topic/Node List Monitor：每秒刷新 ros2 topic list 和 ros2 node list。

Simulation 模式还提供 Open Controller Position Monitor 和 Open Controller Hz Monitor，用于快速看手柄位置字段和接收频率。

分屏监控依赖 x-terminal-emulator 指向 Terminator。若提示不支持，可安装并切换：

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，或分开运行：

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 \
  --pattern axis_sweep --seconds 60 --both-mode staggered

sample_udp_sender 默认使用 axis_sweep 成对扫轴轨迹，并在终端打印 XR +X/-X/+Y/-Y/+Z/-Z 标签。--hand both --both-mode staggered --seconds 60 会先左后右，适合肉眼确认左右臂方向；如果只想左右同时动，可用 --both-mode synchronized。需要检查末端姿态时可增加 --rotation-pattern rpy_steps --rotation-amplitude-deg 25。

观察：

ros2 topic echo /xr/left_controller
ros2 topic echo /xr/right_controller
ros2 topic echo /xr_rm/left_rm75/target_pose
ros2 topic echo /xr_rm/right_rm75/target_pose
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:=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 \
  left_robot_ip:=192.168.192.18 \
  right_robot_ip:=192.168.192.19

默认不会自动移动到初始化点。只有在确认安全区清空后，才显式打开：

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 是当前唯一的遥操作 launch 主入口，launcher_ui.py 中的 mock、单臂真机和双臂真机按钮都调用它。

常用参数：

• arm：left、right、both，默认 right。
• use_mock：true 不连接真机，false 连接 RM75。
• udp_host：UDP 监听地址，默认 0.0.0.0。
• udp_port：UDP 监听端口，默认 15000。
• udp_timer_hz：UDP receiver 轮询频率，默认 200.0。
• left_robot_ip：左臂 IP，默认 192.168.192.18。
• right_robot_ip：右臂 IP，默认 192.168.192.19。
• robot_port：RM75 TCP 端口，默认 8080。
• left_avoid_singularity / right_avoid_singularity：左右臂避奇异参数，默认左 0、右 1。
• avoid_singularity：非空时覆盖左右臂避奇异参数。
• frame_type：rm_movep_canfd 坐标系类型，默认 1。
• control_rate_hz：rm_movep_canfd 目标位姿发送频率，默认 90.0。
• follow：传给 rm_movep_canfd 的跟随标志，默认 false。
• configure_safety_limits：连接真机后是否配置速度/加速度安全参数，默认 true。
• enable_tool_control：是否在遥操作节点内启用末端工具控制 topic，默认 true。
• enable_trigger_gripper_control：是否允许用 trigger 点击切换对应夹爪状态，默认 true。
• trigger_close_threshold：trigger 点击判定阈值，默认 0.95。
• configure_peripheral_on_connect：遥操作节点连接真机后是否配置末端外设，默认 true；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
• move_to_initial_pose_on_connect：连接后是否执行 movej/movel 初始化，默认 false。

配置文件说明

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。

xr_rm_bringup/config/peripherals_rm75.yaml 保存末端工具坐标、负载和左右臂外设选择。当前配置为左臂 scissorgripper=2、右臂 scissorgripper=1，真机连接阶段会初始化外设，后续开合命令复用同一个 RealMan 连接。

重点控制参数：

• controller_topic：订阅的手柄话题。
• scale：手柄位移到 TCP 位移的比例。
• target_filter_alpha / target_filter_alpha_fast：目标 TCP 低通滤波系数，快速移动时自动使用更大的系数。
• target_filter_fast_threshold_m：进入快速滤波区间的目标变化阈值。
• max_linear_speed：目标位姿单帧步长限制对应的最大线速度。
• enable_orientation_control：是否把手柄相对旋转映射到 TCP 姿态。
• orientation_filter_alpha / orientation_deadband_rad：目标 TCP 姿态低通和死区。
• max_orientation_speed：目标姿态单帧步长限制对应的最大角速度。
• workspace_min / workspace_max：笛卡尔工作空间边界。
• cyl_radius_limit：基座圆柱半径限制。
• xr_to_robot_matrix：/xr/*_controller Project 位移到 RM75 base 坐标的映射矩阵。
• current_pose_poll_hz：低频读取真机当前 TCP 的频率；控制中不再每帧阻塞读取状态。
• mock_initial_pose：mock 模式初始 TCP 位姿。
• initial_joint_pose / initial_tcp_pose：可选真机初始化点。

当前 /xr/*_controller 的 Project 坐标约定：

• Project：+X 向右，+Y 向上，+Z 向后。
• Unity APK 的 Project (+Z back) 会把 PXR pxr_predict 原始坐标转换为 project.x=native.z、project.y=native.y、project.z=-native.x。
• Source raw 模式保留原始 pose source 坐标，只用于现场对照。
• 左臂映射：机器人位移增量 = [-手柄y, 手柄z, -手柄x]。
• 右臂映射：机器人位移增量 = [手柄y, 手柄z, 手柄x]。

如果 /xr/*_controller.pose.position 已符合 Project 坐标，但某个机械臂方向相反，只改对应臂的 xr_to_robot_matrix 符号，不要同时改多个控制参数。

末端工具开合

真机 launch 默认会在遥操作节点内启用工具控制。左/右手柄 trigger 从低于阈值按到 >= 0.95 时，会切换一次对应夹爪开/关状态，并保持到下一次点击。grip 仍只控制机械臂运动，不影响夹爪 trigger 切换。

也可以用 Bool 话题手动控制开合，true 表示打开，false 表示闭合：

ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
ros2 topic pub --once /xr_rm/left_rm75/tool_enable std_msgs/msg/Bool "{data: false}"

ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: true}"
ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: false}"

桌面 UI 的 Left Arm 和 Right Arm 模式里也有对应的 Tool Open/Close 命令项；Dual Arm 真机模式下可直接通过左右手柄 trigger 分别切换夹爪。

UDP 数据格式

当前 Unity APK 每个周期发送一个双手柄 JSON 包：

{
  "t": 12.345,
  "source_time": 12.345,
  "seq": 42,
  "frame_id": "xr_world",
  "controllers": {
    "left": {
      "grip": true,
      "trigger": 0.0,
      "pos": [-0.12, 1.05, 0.30],
      "quat": [0.0, 0.0, 0.0, 1.0],
      "pose_valid": true,
      "pose_source": "pxr_predict",
      "tracking_state": 3,
      "controller_status": 2,
      "grip_value": 1.0,
      "axis": [0.0, 0.0],
      "buttons": {
        "grip": true,
        "primary": false,
        "secondary": false,
        "menu": false,
        "axis_click": false
      }
    },
    "right": {
      "grip": true,
      "trigger": 0.4,
      "pos": [0.12, 1.05, 0.30],
      "quat": [0.0, 0.0, 0.0, 1.0],
      "pose_valid": true,
      "pose_source": "unity_xr",
      "tracking_state": 3,
      "controller_status": -1,
      "grip_value": 0.8,
      "axis": [0.0, 0.0],
      "buttons": {
        "grip": true,
        "primary": false,
        "secondary": false,
        "menu": false,
        "axis_click": false
      }
    }
  }
}

字段说明：

• t / source_time：Unity 端 Time.realtimeSinceStartupAsDouble，用于后续延迟分析。
• seq：Unity 端递增包序号，用于后续丢包分析。
• frame_id：默认 xr_world，会写入 XrController.header.frame_id。
• grip：运动使能。true 时进入相对位姿控制，false 时停止。
• trigger：扳机值，范围 0.0-1.0。真机模式下跨过 0.95 的上升沿会切换对应夹爪开/关状态。
• pos：手柄位置，长度 3。
• quat：手柄姿态四元数，默认按 xyzw 解析；遥操作节点会用 grip 锁定后的相对旋转控制 TCP 姿态。
• pose_valid：姿态是否可信。ROS 接收端看到 false 会强制 grip=false。
• pose_source：pxr_predict、unity_xr、xrobotoolkit 或 none，用于判断姿态来自 PICO 预测接口、Unity XR fallback 还是官方 XRoboToolkit SDK bridge。
• tracking_state / controller_status：Unity/PICO 侧追踪诊断值，只用于日志和排查。
• grip_value、axis、buttons：PICO 端输入诊断字段，当前不会写入 XrController 消息。

udp_controller_receiver 仍兼容调试用的单手柄包：可以直接发送带 hand、pos、quat 的 JSON object，也可以用 controllers list、顶层 left/right、pose.position、position、p、q 等常见字段。四元数默认按 xyzw 解析，也可通过 quat_order:=wxyz 切换。

PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并向 ROS2 主机发送 UDP 的详细步骤见 docs/pico_udp_sender_ubuntu22_setup.md。

官方 XRoboToolkit bridge

如果使用官方 XRoboToolkit APK 和 PC-Service，可以用 xrobotoolkit_to_udp_bridge 从本机 ROS Python 环境中的 xrobotoolkit_sdk 读取左右手柄数据，再转换成当前 udp_controller_receiver 支持的 UDP JSON。

正式运行时不要同时启动官方 PXREAClientUnity / RobotLinuxDemo 可视化窗口。/opt/apps/roboticsservice/run3D.sh 会启动这个可视化 demo，适合单独确认 PICO 与 PC-Service 已连接；bridge 遥操作链路中只需要 PC-Service。

运行前只保留一个 UDP 输入源。先清掉重复 bridge、sample sender 和官方 Unity 可视化 demo，再保留或启动 PC-Service：

pkill -f '[x]robotoolkit_to_udp_bridge'
pkill -f '[s]ample_udp_sender'
pkill -f '[R]obotLinuxDemo.x86_64'
pkill -f '[P]XREAClientUnity'
pgrep -af RoboticsServiceProcess || /opt/apps/roboticsservice/runService.sh

启动 ROS mock 接收链路：

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true

另开终端启动 bridge：

cd /home/robot/WS_xr
source ~/.bashrc
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
  --host 127.0.0.1 --port 15000 --hz 90

bridge 默认对 grip/trigger 做轻量滞回：grip 按下阈值 0.90、松开阈值 0.75；trigger 按下阈值 0.95、松开阈值 0.75。启动日志会打印 PID、UDP endpoint 和阈值，便于确认当前只运行了一个 bridge。

验证手柄数据是否进入 ROS：

ps -ef | grep -E 'xrobotoolkit_to_udp_bridge|sample_udp_sender|RobotLinuxDemo|PXREAClientUnity' | grep -v grep
ros2 topic hz /xr/left_controller
ros2 topic hz /xr/right_controller
ros2 topic echo /xr/left_controller --field pose.position
ros2 topic echo /xr/right_controller --field pose.position
ros2 topic echo /xr/left_controller --field grip
ros2 topic echo /xr/right_controller --field grip
ros2 topic echo /xr/right_controller --field trigger

/xr/left_controller 和 /xr/right_controller 持续刷新、位置随手柄移动变化、grip 随握持键切换，即表示官方 XRoboToolkit 数据已经进入当前遥操作输入层。

真机安全验证

第一次接真机时按这个顺序走：

1. 确认急停、网络、机械臂工作区和人员位置。
2. launcher_ui.py 中先 Ping Left RM75 或 Ping Right RM75。
3. 单臂启动，move_to_initial_pose_on_connect:=false。
4. 手握急停，按住 grip 后只做小幅单轴移动。
5. 逐个确认上/下、前/后、左/右方向。
6. 小角度转动手柄，确认 /xr_rm/<arm>/target_pose 姿态和 /xr_rm/<arm>/cmd_vel.twist.angular 变化符合预期。
7. 点击对应 trigger，确认每次点击都会切换对应夹爪状态，松开 trigger 后状态保持且左右不串臂。
8. 确认松开 grip 后机械臂慢停，/xr_rm/<arm>/cmd_vel 回到零；trigger 仍只影响夹爪，不影响机械臂运动门控。
9. 左右臂都确认后，再进入双臂模式。

当前项目没有双臂碰撞检测。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。

后续优化路线

为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：

1. 稳定 PICO 数据链路：利用 seq、source_time、pose_valid 做频率、延迟、丢包和追踪状态统计，记录 /xr/*_controller、/xr_rm/*/raw_target_pose、/xr_rm/*/target_pose、/xr_rm/*/target_clamped、/xr_rm/*/current_pose。
2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
3. 细化末端执行器：增加夹爪状态反馈、力控比例、安全上限和现场可视化提示。
4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
5. 从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。

常见问题

launcher_ui.py 提示找不到 install/setup.bash：

cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
colcon build --symlink-install
source install/setup.bash

真机模式提示缺少 Robotic_Arm：

未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。

Controller topic 没有数据：

• 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
• 确认端口是 15000，或 launch 与发送端端口一致。
• 用 sample_udp_sender 在本机验证接收链路。
• 如果 Unity HUD 显示某个手柄 invalid none，ROS 侧会把该手柄 grip 强制置为 false。

机械臂不动：

• 确认 grip=true。
• 确认 udp_controller_receiver 终端没有持续 pose_valid=false 日志；该字段不会写入 XrController 消息，但会让接收端强制停止。
• 确认 /xr_rm/<arm>/raw_target_pose 与 /xr_rm/<arm>/target_pose 是否在变化。
• 确认 /xr_rm/<arm>/target_clamped 是否持续为 true，如果是，目标 TCP 可能被工作空间、圆柱半径或单帧步长限制夹住。
• 确认真机 SDK 连接成功，且 RM75 没有报警或急停。