@@ -1,97 +1,44 @@
# XR-RM75 双臂遥操作工作空间
# XR-RM75 双臂遥操作
本仓库是面向 **Ubuntu 22.04 + ROS2 Humble + PICO 4 Ultra + 睿尔曼 RM75 ** 的阶段一 XR 双臂 遥操作项目。当前目标是先跑通一条低速、安全、可调试的闭环：
基于 **Ubuntu 22.04、 ROS2 Humble、 PICO 4 Ultra 和 睿尔曼 RM75 ** 的双臂 XR 遥操作
工作空间，支持单臂/双臂 Mock 与真机控制，以及 MuJoCo 运动学显示。
> [!WARNING]
> 真机命令会连接并控制机械臂。首次运行必须从 Mock 和单臂低速验证开始，确保急停
> 可用且工作区无人。当前项目没有双臂碰撞检测或避障。
## 当前能力
- PICO/XR 双手柄 UDP 输入，相对位姿目标与 Placo QP 七关节控制。
- 单臂/双臂 Mock 与真机、手柄/话题夹爪控制，以及只读 MuJoCo 双臂显示。
- 工作空间/圆柱限位、速度限制、指令超时和安全慢停。
- 统一 launch、Tkinter 启动面板、调试话题和 Mock 输入工具。
尚未完成：D405/D435 视频流、数据记录、相机标定、目标检测、双臂碰撞避障、任务级状态机，以及 PICO 与 ROS 的完整时间同步和状态回传。
## 系统架构
``` text
``` text
PICO/XR 双手柄 UDP JSON
PICO / XRoboToolkit
-> UDP JSON
-> xr_rm_input/udp_controller_receiver
-> xr_rm_input/udp_controller_receiver
-> /xr/left_controller 与 /xr/right_controller
-> /xr/left_controller、 /xr/right_controller
-> xr_rm_teleop/single_arm_velocity_teleop
-> xr_rm_teleop/single_arm_velocity_teleop
-> Placo QP 单步逆解
-> 相对 TCP 目标 + Placo QP
-> 左右 RM75 七关节角透传控制
-> Mock 或 RM75 rm_movej_canfd
-> /xr_rm/<arm_name>/ joint_states
-> joint_states / 调试话题
-> 可选 xr_rm_mujoco/dual_arm_simulator
-> 可选 xr_rm_mujoco/dual_arm_simulator
-> /xr_rm/<arm_name>/current_pose、raw_target_pose、target_pose、cmd_vel、target_clamped 调试话题
```
```
当前控制方式是“手柄相对位姿 + 单步 QP”遥操作：按住 `grip` 时锁定当前手柄位姿和机械臂 TCP 位姿，之后根据手柄相对位移和相对旋转生成目标 TCP。姿态目标使用旋转矩阵和 SO(3) 最短路径完成死区、滤波与限速，不经过 RPY。每个控制周期执行一次 Placo QP，并通过 `rm_movej_canfd` 下发 7 个关节目标。松开 `grip` 、UDP 或关节反馈超时、节点退出时会请求机械臂慢停。
工作空间包含五个 ROS2 包：`xr_rm_input` 负责手柄输入，`xr_rm_interfaces` 定义消息，
`xr_rm_teleop` 实现遥操作与真机适配，`xr_rm_bringup` 提供启动和配置，
`xr_rm_mujoco` 负责只读运动学显示。
## 当前范围
`single_arm_velocity_teleop` 每个实例只控制一台机械臂；双臂模式分别启动 `left_arm_teleop` 和 `right_arm_teleop` 。
已完成：
## 环境与构建
- PICO/XR 手柄 UDP 数据接收，并分发到左右手柄 ROS2 话题。
在工作空间根目录执行：
- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
- 使用现有双臂 URDF 的 MuJoCo 运动学显示，可由 Mock 或真机反馈同步双臂姿态。
- RM75 真机连接适配，包含关节反馈缓存、`rm_movej_canfd` 关节透传、安全速度/加速度配置、可选初始化点位移动。
- Placo 0.9.4 RM75 QP 逆解；收到首帧有效关节反馈后才启用，求解失败时保留上一组有效关节目标。
- 真机模式下，点击对应手柄 `trigger` 可切换并保持对应夹爪开/关状态。
- Tkinter 启动面板 `launcher_ui.py` ，用于现场快速启动、监控 topic、检查环境和清理进程。
- XRoboToolkit bridge 读取左右手柄 pose、Grip、Trigger、摇杆和主副按键。
暂未完成：
- D405/D435 视频流、数据记录、相机标定和目标检测链路。
- 双臂碰撞模型、任务级状态机、自动采摘策略。
- PICO 端与 ROS 端的完整时间同步和状态回传。
## 项目结构
``` text
src/
├── README.md # 项目主文档
├── AGENTS.md # Codex 项目工作流和安全规则
├── docs/superpowers/ # Superpowers 设计与实施计划
├── xr_rm_bringup/
│ ├── config/
│ │ ├── dual_arm_mujoco.yaml # MuJoCo 显示刷新参数
│ │ ├── 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 # 统一入口：单臂/双臂、Mock/真机、可选 MuJoCo
│ └── tools/
│ ├── launcher_ui.py # 图形化调试启动面板
│ └── realman_dual_arm_state_monitor.py
├── xr_rm_input/
│ ├── launch/
│ │ └── udp_receiver.launch.py # 低层 UDP 接收测试入口
│ ├── test/
│ │ └── test_controller_fields.py
│ └── xr_rm_input/
│ ├── udp_controller_receiver.py
│ ├── xrobotoolkit_to_udp_bridge.py
│ └── sample_udp_sender.py # 本机扫轴/正弦模拟手柄 UDP 数据
├── xr_rm_interfaces/
│ └── msg/
│ └── XrController.msg # 手柄状态与位姿
├── xr_rm_mujoco/
│ └── xr_rm_mujoco/
│ └── dual_arm_simulator.py # 双臂 URDF 运动学映射与 MuJoCo viewer
└── xr_rm_teleop/
├── models/
│ ├── rm75/ # 旧 RM75 模型资源（launch 不再选用）
│ ├── rm75_omnipicker/ # 旧单臂 OmniPicker 模型资源
│ └── dual_rm75/ # 当前左右臂统一使用的双 RM75 URDF 与网格
└── xr_rm_teleop/
├── placo_ik_solver.py # Placo 0.9.4 单步 QP 逆解
├── single_arm_velocity_teleop.py
├── realman_adapter.py
└── fun_peripheral.py
```
`single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop` 。
## Superpowers Git 约束
使用 Superpowers 执行任务时，只允许按 skill 工作流创建本地 Git 提交。
禁止执行 `git push` 、合并本地分支、合并 PR 或其他远程写操作。skill 如需
独立 worktree 或配套本地分支，可以创建，但不得将其合并到其他分支。
## 环境准备
在工作空间根目录，也就是包含 `src/` 的目录执行：
``` bash
``` bash
cd /home/robot/WS_xr
cd /home/robot/WS_xr
@@ -102,440 +49,125 @@ colcon build --symlink-install
source install/setup.bash
source install/setup.bash
```
```
真机模式还需要安装睿尔曼 Python API2。若未安装，mock 模式仍可正常使用；真机启动时会提示缺少 `Robotic_Arm` 包。
遥操作和 MuJoCo 节点固定使用 `/home/robot/miniconda3/envs/xr/bin/python` ，
其中固定 Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0；不要从
用户或系统 Python 覆盖这些版本。
遥操作和 MuJoCo 节点固定由 `/home/robot/miniconda3/envs/xr/bin/python` 启动，并复用其中的 Python 3.10、Placo 0.9.4、Pinocchio 3.7.0、NumPy 2.2.6 和 MuJoCo 3.10.0。 `ros2` 、 `colcon` 、pytest 和 `udp_controller_receiver` 仍使用系统 Python。禁止通过 `pip --user` 、 `sudo pip` 或系统安装升级 Placo、Pinocchio、EigenPy 和 NumPy 。
真机模式另需睿尔曼 Python API2； 。
只读检查 Placo 版本：
## 快速开始
以下命令均在 `/home/robot/WS_xr` 执行，并先 source ROS2 与 `install/setup.bash` 。
### Mock
``` bash
``` bash
/home/robot/miniconda3/envs/xr/bin/python -c \
ros2 launch xr_rm_bringup arm_debug.launch.py arm:= both use_mock:= true
"import importlib.metadata; print(importlib.metadata.version('placo'))"
```
```
输出必须为 `0.9.4` 。
另开终端发送模拟手柄数据：
同时检查 MuJoCo：
``` bash
``` bash
/home/robot/miniconda3/envs/xr/bin/python -c \
ros2 run xr_rm_input sample_udp_sender \
"import mujoco; print(mujoco.__version__)"
--hand both --host 127.0.0.1 --port 15000 \
```
当前验证版本为 `3.10.0` 。系统 pytest 会通过 `xr_rm_mujoco/test/conftest.py` 复用该固定 XR 环境中的 MuJoCo，因此新包可直接按 ROS2 标准方式测试：
``` bash
colcon test --packages-select xr_rm_mujoco --event-handlers console_direct+
colcon test-result --verbose
```
如果希望 `launcher_ui.py` 从任意目录找到工作空间，可以设置：
``` bash
export XR_RM_WS = /home/robot/WS_xr
```
## 使用 launcher_ui.py 调试
推荐现场调试优先使用图形化启动面板。它会自动进入工作空间、source ROS2 与 `install/setup.bash` ，并把每个命令放到独立终端中运行。
源码方式启动：
``` bash
cd /home/robot/WS_xr
python3 src/xr_rm_bringup/tools/launcher_ui.py
```
构建后也可以通过 ROS2 入口启动：
``` bash
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 run xr_rm_bringup launcher_ui
```
面板顶部的 `Mode` 分为四类：
- `Simulation` ：双臂 mock、XRoboToolkit bridge、双手 sample UDP 和 controller 频率监控。
- `MuJoCo` ：双臂 Mock/真机 MuJoCo launch、XRoboToolkit bridge 和 controller 频率监控；真机命令会连接两台 RM75。
- `Real Hardware` ：左右臂网络 ping、左臂/右臂/双臂真机 launch、XRoboToolkit bridge 和左右夹爪开合。
- `Diagnostics` ： `ros2 doctor --report` 、四个核心包的 `ros2 pkg prefix` 、controller 位置/频率监控。
常用按钮：
- `Run Selected` ：运行当前选中的命令。双击列表项也可以运行。
- `Check Env` ：检查 ROS2 Humble、工作空间 build、终端、四个核心 ROS 包、睿尔曼 API2。
- `Stop All` ：结束由本工作空间启动的 launch、sample sender、topic monitor、MuJoCo viewer、相关 ROS 节点和终端窗口。
`Stop All` 会保留现有 PC Service；点击启动器窗口 `X` 并确认退出时会额外停止 PC Service。两条清理路径都会停止 `dual_arm_simulator` ，关闭 MuJoCo viewer。
每个模式都会附带基础监控入口：
- `Open Controller Topic Monitor` ：同时查看 `/xr/left_controller` 和 `/xr/right_controller` 。
- `Open ROS Topic/Node List Monitor` ：每秒刷新 `ros2 topic list` 和 `ros2 node list` 。
`Simulation` 和 `MuJoCo` 模式还提供 `Open Controller Hz Monitor` ； `Diagnostics` 同时提供 controller 位置与频率监控。
分屏监控依赖 `x-terminal-emulator` 指向 Terminator。若提示不支持，可安装并切换：
``` bash
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 闭环。
分两个终端依次验证左臂：
``` bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:= left use_mock:= true
ros2 run xr_rm_input sample_udp_sender --hand left --host 127.0.0.1 --port 15000 \
--pattern axis_sweep --seconds 30
--pattern axis_sweep --seconds 30
```
```
停止左臂进程后，再分别验证右臂：
单臂调试时将 `arm` 改为 `left` 或 `right` 。推荐先分别完成左右单臂
Mock，再进入双臂或真机验证。
``` bash
### MuJoCo
ros2 launch xr_rm_bringup arm_debug.launch.py arm:= right use_mock:= true
ros2 run xr_rm_input sample_udp_sender --hand right --host 127.0.0.1 --port 15000 \
--pattern axis_sweep --seconds 30
```
`sample_udp_sender` 默认使用 `axis_sweep` 扫轴轨迹，并在终端打印 `XR +X/-X/+Y/-Y/+Z/-Z` 标签。需要检查末端姿态时可增加 `--rotation-pattern rpy_steps --rotation-amplitude-deg 25` 。
观察：
``` bash
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
```
第三步：单臂真机。
先只上一个臂，确认网络、方向、急停和限幅：
``` 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
```
所有 YAML 默认都不会执行 `movej(initial_joint_pose)` 。只有确认安全区清空后，才可在当前使用的
`left_arm_rm75.yaml` 、`right_arm_rm75.yaml` 或 `dual_arm_rm75.yaml` 中将
`move_to_initial_pose_on_connect` 改为 `true` 。
第四步：双臂真机。
``` bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:= both use_mock:= false
```
双臂默认不会自动移动到初始化点；机器人地址、控制参数和初始化移动开关均从
`dual_arm_rm75.yaml` 读取。
## Launch 入口说明
`arm_debug.launch.py` 是当前唯一的遥操作 launch 主入口，`launcher_ui.py` 中的 Simulation、MuJoCo 和 Real Hardware launch 命令都调用它。
常用参数：
- `arm` ： `left` 、`right` 、`both` ，默认 `right` 。
- `use_mock` ： `true` 不连接真机，`false` 连接 RM75。
- `use_mujoco` ： `true` 额外启动双臂 MuJoCo 显示，默认 `false` ，仅支持 `arm:=both` 。
- `udp_host` ： `0.0.0.0` 。
- `udp_port` ： `15000` 。
- `udp_timer_hz` ： `200.0` 。
机器人 IP/端口、控制频率、CANFD、限速、工具和初始化位姿等行为参数只由对应 YAML
配置，launch 不再提供同名覆盖项。
## MuJoCo 双臂仿真
无真机时，由 Mock 关节状态驱动 MuJoCo：
``` bash
``` bash
ros2 launch xr_rm_bringup arm_debug.launch.py \
ros2 launch xr_rm_bringup arm_debug.launch.py \
arm:= both use_mock:= true use_mujoco:= true
arm:= both use_mock:= true use_mujoco:= true
```
```
连接真机时，由两台 RM75 的实际关节反馈同步 MuJoCo。下面命令会连接真机，执行前必须完成真机安全检查：
MuJoCo 只订阅关节状态，不参与控制。真机显示可将 `use_mock` 改为 `false` ，
但该命令会同时连接两台 RM75。
### PICO 输入
只保留一个 UDP 输入源，然后启动 XRoboToolkit bridge：
``` bash
``` bash
ros2 launch xr_rm_bringup arm_debug.launch.py \
arm:= both use_mock:= false use_mujoco:= true
```
桌面 UI 的 `MuJoCo` 模式分别提供上述 Mock 和真机命令，并明确标记会连接真机的 `Dual Arm MuJoCo Real Hardware Launch` 。
MuJoCo 只订阅当前关节状态，不参与控制，也不向真机下发指令：
- `/xr_rm/left_rm75/joint_states` 、`/xr_rm/right_rm75/joint_states` ：当前适配器反馈；Mock 与真机模式均按控制周期约 `90 Hz` 发布。真机底层原始反馈周期仍为 `5 ms` （约 `200 Hz` ），由遥操作节点按 `90 Hz` 采样发布。
- `/xr_rm/left_rm75/joint_target` 、`/xr_rm/right_rm75/joint_target` ：经 QP 和现有限制处理后的目标关节角，仅用于调试，MuJoCo 不订阅。
- MuJoCo viewer 默认按 `dual_arm_mujoco.yaml` 中的 `60 Hz` 刷新。其初始姿态直接来自 `dual_arm_rm75.yaml` 的 `initial_joint_pose` ，不会在 MuJoCo YAML 中重复保存。
Mock 模式的遥操作目标仍经过 `dual_arm_rm75.yaml` 中的工作空间、圆柱、线速度、角速度、关节速度/加速度、超时和停止限制。左手 X、右手 A 分别立即复位对应 Mock 机械臂；Grip 保持按下时，下一控制周期会重新锚定并继续遥操作。真机复位完成后仍需松开 Grip 才能恢复遥操作，且 `move_to_initial_pose_on_connect` 保持为 `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` 。
`dual_arm_mujoco.yaml` 只保存 MuJoCo viewer 刷新频率；双臂初始关节角和遥操作限制继续统一读取 `dual_arm_rm75.yaml` 。
`xr_rm_bringup/config/peripherals_rm75.yaml` 保存真实控制器使用的末端工具坐标、负载和左右臂外设选择。左臂 `scissorgripper: 2` 是外设选择值，选择 `minisci` ， `0.165 m` ；右臂 `scissorgripper: 1` ，选择 `omnipic` ， `0.14 m` 。真机连接阶段仍会初始化外设，关节反馈、关节指令、慢停和开合命令复用该单臂节点的同一个 RealMan 连接。
Placo 使用 `xr_rm_teleop/models/dual_rm75/Dual_arm.urdf` 。左右 ROS 节点分别创建独立 solver：左臂从 `scissor_base_link` 到 `scissor_scissor_tcp` ，并 mask 右臂；右臂从 `omnipic_base_link` 到 `omnipic_OmniPic_tcp` ，并 mask 左臂。节点目标仍在各自局部基坐标系中，现有 PICO 映射不改为公共坐标系。
重点控制参数：
- `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` ：按 SO(3) 最短旋转角处理的目标 TCP 姿态滤波和死区。
- `max_orientation_speed` ：目标 TCP 姿态沿 SO(3) 最短路径的最大角速度，当前为 `0.5 rad/s` 。
- `workspace_min` / `workspace_max` ：笛卡尔工作空间边界。
- `cyl_radius_limit` ：基座圆柱半径限制。
- `xr_to_robot_matrix` ： `/xr/*_controller` Project 位移到 RM75 base 坐标的映射矩阵。
- `robot_ip` / `robot_port` ：
- `realtime_push_host_ip` ：连接机械臂 Wi-Fi 后本机实际 IPv4；可用
`ip -4 route get 192.168.192.19` 查看输出中的 `src` ，当前为 `192.168.192.148` 。
- `realtime_push_port` ： `8089` 、右臂 `8090` ，同机双臂不能重复。
- `realtime_push_cycle_ms` ：UDP 主动反馈周期，当前为厂商支持的 `5 ms` 。
- `follow` / `canfd_trajectory_mode` ： `rm_movej_canfd` 的高跟随和轨迹模式参数。
- 当前三份 YAML 默认均使用 `follow: false` 完成安全基线验证；确认关节加速度与反馈稳定后，再单独测试高跟随。
- `initial_joint_pose` ：mock 的初始关节反馈，以及显式开启初始化移动时的真机初始关节角。
当前 `/xr/*_controller` 的坐标处理：
- XRoboToolkit bridge 原样转发 SDK 的手柄位置和四元数，不额外转换坐标轴。
- receiver 默认按 `xyzw` 解析四元数，也可通过 `quat_order:=wxyz` 切换。
- 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]` 。
- 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]` 。
- 两侧局部 `-Y` 均指向机器人前方；局部 `+Y` 指向后方，后方工作空间仅保留 `0.10 m` 。
- 左臂局部 `+X/+Y/+Z` 分别指向下/后/左外侧；右臂分别指向上/后/右外侧。
如果某个机械臂方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
## 末端工具开合
真机 launch 默认会在遥操作节点内启用工具控制。左/右手柄 `trigger` 从低于阈值按到 `>= 0.95` 时，会切换一次对应夹爪开/关状态，并保持到下一次点击。`grip` 仍只控制机械臂运动，不影响夹爪 trigger 切换。
也可以用 Bool 话题手动控制开合，`true` 表示打开，`false` 表示闭合：
``` bash
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 的 `Real Hardware` 模式提供 `Left/Right Gripper Open/Close` 命令项；运行双臂真机 launch 时也可直接通过左右手柄 `trigger` 分别切换夹爪。
## UDP 数据格式
当前 XRoboToolkit bridge 每个周期发送一个双手柄 JSON 包：
``` json
{
"t" : 12.345 ,
"source_time" : 12.345 ,
"seq" : 42 ,
"frame_id" : "xr_world" ,
"controllers" : {
"left" : {
"hand" : "left" ,
"grip" : true ,
"trigger" : 0.0 ,
"axis" : [ 0.2 , -0.4 ] ,
"buttons" : {
"primary" : true ,
"secondary" : false
} ,
"pos" : [ -0.12 , 1.05 , 0.30 ] ,
"quat" : [ 0.0 , 0.0 , 0.0 , 1.0 ] ,
"pose_valid" : true ,
"pose_source" : "xrobotoolkit"
} ,
"right" : {
"hand" : "right" ,
"grip" : true ,
"trigger" : 1.0 ,
"axis" : [ -0.1 , 0.3 ] ,
"buttons" : {
"primary" : false ,
"secondary" : true
} ,
"pos" : [ 0.12 , 1.05 , 0.30 ] ,
"quat" : [ 0.0 , 0.0 , 0.0 , 1.0 ] ,
"pose_valid" : true ,
"pose_source" : "xrobotoolkit"
}
}
}
```
字段说明：
- `t` / `source_time` ：bridge 的 PC 单调时间，用于诊断发送周期。
- `seq` ：
- `frame_id` ：默认 `xr_world` ，会写入 `XrController.header.frame_id` 。
- `grip` ：运动使能。`true` 时进入相对位姿控制，`false` 时停止。
- `trigger` ：经过 bridge 滞回处理的 `0.0/1.0` 值；上升沿切换对应夹爪状态。
- `axis` ：摇杆 `[x, y]` ，每个分量限制在 `-1.0` 到 `1.0` 。
- `buttons.primary` ：左手 X 键或右手 A 键。
- `buttons.secondary` ：左手 Y 键或右手 B 键。
- `pos` ：手柄位置，长度 3。
- `quat` ：手柄姿态四元数，默认按 `xyzw` 解析。
- `pose_valid` ：姿态是否可信；`false` 时接收端强制 `grip=false` 。
- `pose_source` ：当前 bridge 使用 `xrobotoolkit` 。
`axis` 、`buttons.primary` 和 `buttons.secondary` 会进入 `XrController` ；旧 UDP
包缺少这些字段时分别回退为 `[0,0]` 、`false` 和 `false` 。
接收端发布的消息格式为：
``` text
std_msgs/Header header
string hand
bool grip
float32 trigger
bool primary
bool secondary
float32[2] axis
geometry_msgs/Pose pose
```
`udp_controller_receiver` 仍兼容调试用的单手柄包：可以直接发送带 `hand` 、`pos` 、
`quat` 的 JSON object，也可以用 `controllers` list、顶层 `left/right` 、
`pose.position` 、`position` 、`p` 、`q` 等常见字段。
## 官方 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：
``` bash
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 接收链路：
``` bash
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：
``` bash
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 \
ros2 run xr_rm_input xrobotoolkit_to_udp_bridge \
--host 127.0.0.1 --port 15000 --hz 90
--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。
确认左右 topic 持续接收数据：
验证手柄数据是否进入 ROS：
``` bash
``` bash
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/left_controller
ros2 topic hz /xr/right_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 数据已经进入当前遥操作输入层。
图形启动面板可运行 `python3 src/xr_rm_bringup/tools/launcher_ui.py` ，提供
Simulation、MuJoCo、Real Hardware 和 Diagnostics 模式。
## 真机安全验证
### 真机
第一次接真机时按这个顺序走 ： 确认对应 YAML 中 `move_to_initial_pose_on_connect: false` ，再从单臂开始 ：
1. 确认急停、网络、机械臂工作区和人员位置。
``` bash
2. ` launcher_ui.py` 中先 `Ping Left RM75` 或 `Ping Right RM75` 。
ros2 launch xr_rm_bringup arm_debug.launch.py arm: = left use_mock: = false
3. 确认对应 YAML 中 `move_to_initial_pose_on_connect: false` 后单臂启动。
ros2 launch xr_rm_bringup arm_debug.launch.py arm:= right use_mock:= 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. 左右臂都确认后，再运行 `Dual Arm RealMan Launch` 。
当前项目没有双臂碰撞检测/避障。双臂首次联调时，请让两个工作区在物理上分开，低速验证，不要让两臂末端互相靠近。
单臂方向、限位、急停、超时停止和夹爪均验证后，才能启动双臂：
## 后续优化路线
``` bash
ros2 launch xr_rm_bringup arm_debug.launch.py arm:= both use_mock:= false
```
为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
按住 `grip` 控制对应机械臂；松开后停止。点击 `trigger` 切换对应夹爪开/关。
左手 X、右手 A 会请求对应机械臂回到配置的初始位姿；真机使用前必须清空安全区。
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` 。
## Launch 参数
2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
3. 细化末端执行器：增加夹爪状态反馈、力控比例、安全上限和现场可视化提示。
4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
5. 从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。
## 常见问题
统一入口为 `xr_rm_bringup/launch/arm_debug.launch.py` ：
`launcher_ui.py` 提示找不到 `install/setup.bash` ： | 参数 | 默认值 | 说明 |
| --- | --- | --- |
| `arm` | `right` | `left` 、`right` 或 `both` |
| `use_mock` | `true` | `false` 会连接真机 |
| `use_mujoco` | `false` | 仅支持 `arm:=both` |
| `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 刷新频率 |
修改某一侧控制参数时，同时检查单臂和双臂 YAML 是否需要同步。必须保留工作空间/
圆柱限位、线速度与角速度限制、关节速度/加速度限制、指令超时和安全停止逻辑。
`configure_safety_limits` 不得默认关闭；
`move_to_initial_pose_on_connect` 必须保持默认 `false` 。
## 测试
在工作空间根目录执行：
``` bash
``` bash
cd /home/robot/WS_xr
source /opt/ros/humble/setup.bash
source /opt/ros/humble/setup.bash
colcon build --symlink-install
colcon build --symlink-install
source install/setup.bash
colcon test --event-handlers console_direct+
colcon test-result --verbose
```
```
真机模式提示缺少 `Robotic_Arm` ： 涉及遥操作姿态控制时，额外运行 ：
``` text
``` bash
未安装睿尔曼 Python API2。请安装厂商 SDK，或用 use_mock:=true 先跑模拟模式。
pytest src/xr_rm_teleop/test/test_orientation_control.py
```
```
Controller topic 没有数据：
真机验证不属于自动测试。默认使用 `use_mock:=true` ，未经现场安全确认不要连接或
移动机械臂。
- 确认 UDP 发送端目标 IP 是运行 ROS2 的主机 IP。
- 确认端口是 `15000` ，或 launch 与发送端端口一致。
- 用 `sample_udp_sender` 在本机验证接收链路。
- 确认 `xrobotoolkit_to_udp_bridge` 没有持续打印 SDK read failed；
读取失败时 bridge 会发送 `pose_valid=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 没有报警或急停。
YikaiFu-cart