feat: Enhance single arm velocity teleoperation with zero velocity handling and new parameters

2026-05-26 13:51:15 +08:00
parent e57890cb11
commit f137e28ed7
9 changed files with 933 additions and 67 deletions
--- a/docs/PROMPT.md
+++ b/docs/PROMPT.md
@ -0,0 +1,411 @@
+# 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。
--- a/docs/pico4_ultra_udp_teleop_setup.md
+++ b/docs/pico4_ultra_udp_teleop_setup.md
@ -0,0 +1,490 @@
+# PICO 4 Ultra UDP 手柄接入与调试教程
+
+本文档用于把 PICO 4 Ultra 左右手柄接入当前 XR-RM75 ROS2 工作空间。目标是让 PICO 端发送符合本项目约定的 UDP JSON，ROS 端发布：
+
+```text
+/xr/left_controller
+/xr/right_controller
+```
+
+然后由 `single_arm_velocity_teleop` 把手柄相对位移转换成 RM75 TCP 运动。
+
+## 1. 当前项目约定
+
+当前链路如下：
+
+```text
+PICO 4 Ultra Unity 应用
+  -> UDP JSON, 默认端口 15000
+  -> xr_rm_input/udp_controller_receiver
+  -> /xr/left_controller 与 /xr/right_controller
+  -> xr_rm_teleop/single_arm_velocity_teleop
+  -> RM75 笛卡尔相对位移控制
+```
+
+ROS 端默认参数：
+
+| 项目 | 默认值 |
+| --- | --- |
+| UDP 监听地址 | `0.0.0.0` |
+| UDP 端口 | `15000` |
+| 左手柄话题 | `/xr/left_controller` |
+| 右手柄话题 | `/xr/right_controller` |
+| 四元数顺序 | `xyzw` |
+| 默认坐标 | PICO/OpenXR: `+X` 向右, `+Y` 向上, `+Z` 向后 |
+
+控制语义：
+
+- `grip=false`: 机械臂停止，退出相对位移遥操作。
+- `grip=true`: 第一帧锁定手柄起点和当前 TCP 起点，之后跟随手柄相对位移。
+- `trigger`: 当前主运动链路不使用，范围 `0.0-1.0`，预留给夹爪。
+- UDP 超过 `command_timeout_sec=0.12` 秒未更新时，机械臂停止。
+
+因此 PICO 端建议稳定发送 `60 Hz` 或 `90 Hz`。不要低于 `20 Hz`。
+
+## 2. UDP JSON 协议
+
+推荐 PICO 端每包同时发送左右手柄：
+
+```json
+{
+  "t": 12.345,
+  "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]
+    },
+    "right": {
+      "grip": true,
+      "trigger": 0.4,
+      "pos": [0.12, 1.05, 0.30],
+      "quat": [0.0, 0.0, 0.0, 1.0]
+    }
+  }
+}
+```
+
+也兼容单手柄包：
+
+```json
+{
+  "hand": "right",
+  "grip": true,
+  "trigger": 0.2,
+  "pos": [0.12, 1.05, 0.30],
+  "quat": [0.0, 0.0, 0.0, 1.0]
+}
+```
+
+字段要求：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `hand` | string | 单手柄包使用，`left` 或 `right` |
+| `controllers.left` | object | 左手柄数据 |
+| `controllers.right` | object | 右手柄数据 |
+| `grip` | bool | 运动使能 |
+| `trigger` | float | `0.0-1.0` |
+| `pos` | float[3] | 手柄位置 `[x, y, z]` |
+| `quat` | float[4] | 手柄姿态 `[qx, qy, qz, qw]` |
+| `frame_id` | string | 可选，默认 `xr_world` |
+
+## 3. PICO 端准备
+
+### 3.1 硬件与网络
+
+1. PICO 4 Ultra 和 Ubuntu ROS 主机连接到同一个局域网。
+2. 在 Ubuntu 上查看主机 IP：
+
+```bash
+hostname -I
+```
+
+假设输出里有 `192.168.1.42`，Unity 脚本里的 `host` 就填 `192.168.1.42`。
+
+3. 如果 Ubuntu 开了防火墙，放行 UDP 端口：
+
+```bash
+sudo ufw allow 15000/udp
+```
+
+4. PICO 上打开开发者模式和 USB 调试，方便 Unity 直接 Build And Run。
+
+### 3.2 Unity 工程
+
+推荐使用 Unity LTS 版本，并安装 Android Build Support、Android SDK/NDK、OpenJDK。
+
+工程设置建议：
+
+1. `File -> Build Settings -> Android -> Switch Platform`。
+2. `Player Settings -> Other Settings` 中设置包名，例如 `com.local.xr_rm_udp_sender`。
+3. `Player Settings -> Other Settings -> Internet Access` 设为 `Require`。UDP 发送需要 Android `INTERNET` 权限。
+4. 导入 PICO Unity Integration SDK。
+5. 按 PICO 官方输入映射文档，手柄输入通过 Unity XR Input System 的 `CommonUsages` 读取。
+
+如果使用 Unity OpenXR 输入路径，常用映射是：
+
+| 功能 | Unity XR `CommonUsages` | OpenXR 语义 |
+| --- | --- | --- |
+| 手柄位置 | `devicePosition` | `/input/grip/pose` position |
+| 手柄姿态 | `deviceRotation` | `/input/grip/pose` rotation |
+| 扳机按钮 | `triggerButton` | `/input/trigger/click` |
+| 扳机模拟量 | `trigger` | `/input/trigger/value` |
+| 抓握按钮 | `gripButton` | squeeze click/button |
+| 抓握模拟量 | `grip` | `/input/squeeze/value` |
+
+## 4. Unity 最小 UDP 发送脚本
+
+在 Unity 中创建 `PicoControllerUdpSender.cs`，挂到一个常驻 GameObject 上。把 Inspector 里的 `Host` 改成 Ubuntu ROS 主机 IP，`Port` 保持 `15000`。
+
+```csharp
+using System;
+using System.Net;
+using System.Net.Sockets;
+using System.Text;
+using UnityEngine;
+using UnityEngine.XR;
+
+public class PicoControllerUdpSender : MonoBehaviour
+{
+    [Header("ROS UDP Target")]
+    public string host = "192.168.1.42";
+    public int port = 15000;
+
+    [Header("Send")]
+    public float sendHz = 60.0f;
+    public bool convertUnityToProjectCoordinates = true;
+
+    private UdpClient client;
+    private IPEndPoint endPoint;
+    private float nextSendTime;
+    private readonly Packet packet = new Packet();
+
+    [Serializable]
+    private class Packet
+    {
+        public double t;
+        public string frame_id = "xr_world";
+        public Controllers controllers = new Controllers();
+    }
+
+    [Serializable]
+    private class Controllers
+    {
+        public ControllerPayload left = new ControllerPayload();
+        public ControllerPayload right = new ControllerPayload();
+    }
+
+    [Serializable]
+    private class ControllerPayload
+    {
+        public bool grip;
+        public float trigger;
+        public float[] pos = new float[] { 0.0f, 1.0f, 0.0f };
+        public float[] quat = new float[] { 0.0f, 0.0f, 0.0f, 1.0f };
+    }
+
+    private void OnEnable()
+    {
+        client = new UdpClient();
+        endPoint = new IPEndPoint(IPAddress.Parse(host), port);
+        nextSendTime = 0.0f;
+    }
+
+    private void OnDisable()
+    {
+        packet.controllers.left.grip = false;
+        packet.controllers.right.grip = false;
+        SendPacket();
+        client?.Close();
+        client = null;
+    }
+
+    private void Update()
+    {
+        if (Time.unscaledTime < nextSendTime)
+        {
+            return;
+        }
+
+        nextSendTime = Time.unscaledTime + 1.0f / Mathf.Max(sendHz, 1.0f);
+        packet.t = Time.realtimeSinceStartupAsDouble;
+
+        FillController(XRNode.LeftHand, packet.controllers.left);
+        FillController(XRNode.RightHand, packet.controllers.right);
+        SendPacket();
+    }
+
+    private void FillController(XRNode node, ControllerPayload payload)
+    {
+        InputDevice device = InputDevices.GetDeviceAtXRNode(node);
+        if (!device.isValid)
+        {
+            payload.grip = false;
+            payload.trigger = 0.0f;
+            return;
+        }
+
+        Vector3 position = Vector3.zero;
+        Quaternion rotation = Quaternion.identity;
+        float trigger = 0.0f;
+        float gripValue = 0.0f;
+        bool gripButton = false;
+
+        device.TryGetFeatureValue(CommonUsages.devicePosition, out position);
+        device.TryGetFeatureValue(CommonUsages.deviceRotation, out rotation);
+        device.TryGetFeatureValue(CommonUsages.trigger, out trigger);
+        device.TryGetFeatureValue(CommonUsages.grip, out gripValue);
+        device.TryGetFeatureValue(CommonUsages.gripButton, out gripButton);
+
+        payload.grip = gripButton || gripValue > 0.5f;
+        payload.trigger = Mathf.Clamp01(trigger);
+
+        if (convertUnityToProjectCoordinates)
+        {
+            // Unity scene coordinates are commonly +Z forward. This project expects
+            // OpenXR-style controller positions with +Z backward.
+            payload.pos[0] = position.x;
+            payload.pos[1] = position.y;
+            payload.pos[2] = -position.z;
+
+            payload.quat[0] = -rotation.x;
+            payload.quat[1] = -rotation.y;
+            payload.quat[2] = rotation.z;
+            payload.quat[3] = rotation.w;
+        }
+        else
+        {
+            payload.pos[0] = position.x;
+            payload.pos[1] = position.y;
+            payload.pos[2] = position.z;
+
+            payload.quat[0] = rotation.x;
+            payload.quat[1] = rotation.y;
+            payload.quat[2] = rotation.z;
+            payload.quat[3] = rotation.w;
+        }
+    }
+
+    private void SendPacket()
+    {
+        if (client == null || endPoint == null)
+        {
+            return;
+        }
+
+        string json = JsonUtility.ToJson(packet);
+        byte[] bytes = Encoding.UTF8.GetBytes(json);
+        client.Send(bytes, bytes.Length, endPoint);
+    }
+}
+```
+
+说明：
+
+- 如果 `/xr/left_controller` 和 `/xr/right_controller` 里 `pos.z` 与实际前后方向相反，优先切换 `convertUnityToProjectCoordinates` 后再验证。
+- 当前项目的主控制只用位置相对位移，姿态 `quat` 会发布出来，但暂不参与机械臂姿态控制。
+- `OnDisable()` 会补发一次 `grip=false`，避免退出 PICO 应用时机械臂保持 active 状态。
+
+## 5. ROS 端先做低层 UDP 验证
+
+在工作空间根目录 `/home/robot/WS_xr` 执行：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch xr_rm_input udp_receiver.launch.py udp_port:=15000
+```
+
+另开终端查看左右手柄：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 topic echo /xr/left_controller
+```
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 topic echo /xr/right_controller
+```
+
+再查看频率：
+
+```bash
+ros2 topic hz /xr/left_controller
+ros2 topic hz /xr/right_controller
+```
+
+期望现象：
+
+- PICO 应用启动后，两个 topic 都持续刷新。
+- 按下左手抓握键时，`/xr/left_controller` 的 `grip` 变成 `true`。
+- 按下右手抓握键时，`/xr/right_controller` 的 `grip` 变成 `true`。
+- 扳机从松开到按下时，`trigger` 从 `0.0` 附近变到 `1.0` 附近。
+- 平移手柄时，`pose.position` 连续变化。
+
+如果收不到包，先在 Ubuntu 上抓 UDP：
+
+```bash
+sudo tcpdump -ni any udp port 15000
+```
+
+能看到 UDP 但 ROS topic 没数据，说明 JSON 字段不符合协议。看 `udp_controller_receiver` 终端里的 `XR 数据包格式错误` 或 `XR 手柄字段错误`。
+
+## 6. 用 sample_udp_sender 排除 ROS 端问题
+
+在接 PICO 前，先确认 ROS 端链路是通的。
+
+终端 1：
+
+```bash
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true
+```
+
+终端 2：
+
+```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 --seconds 20
+```
+
+终端 3：
+
+```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
+```
+
+如果 sample sender 正常，而 PICO 不正常，问题在 PICO 端 IP、端口、权限、JSON 或坐标转换。
+
+## 7. PICO 端 mock 闭环调试流程
+
+先不要连接真机，使用 mock 模式验证完整控制链。
+
+1. 启动双臂 mock：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=true udp_port:=15000
+```
+
+2. 启动 PICO Unity 应用。
+3. 确认 `/xr/left_controller` 和 `/xr/right_controller` 正常刷新。
+4. 左手按住 `grip`，只移动左手一小段，观察：
+
+```bash
+ros2 topic echo /xr_rm/left_rm75/cmd_vel
+```
+
+5. 松开左手 `grip`，确认 `cmd_vel` 回到 0。
+6. 右手重复同样流程。
+7. 用 `ros2 topic hz` 确认频率稳定，建议 `50 Hz` 以上。
+
+## 8. 坐标方向检查
+
+当前配置使用的 PICO/OpenXR 位置坐标：
+
+```text
+X: 向右
+Y: 向上
+Z: 向后
+```
+
+双臂配置中的映射关系：
+
+```text
+左臂机器人位移增量 = [-手柄y, -手柄z,  手柄x]
+右臂机器人位移增量 = [ 手柄y, -手柄z, -手柄x]
+```
+
+建议现场按下面顺序验证：
+
+1. 只启动 `use_mock:=true`。
+2. 按住左手 `grip`，沿 PICO 的 `+X/-X`、`+Y/-Y`、`+Z/-Z` 每次只动一个轴。
+3. 记录 `/xr/left_controller.pose.position` 的变化方向。
+4. 记录 `/xr_rm/left_rm75/cmd_vel` 的方向。
+5. 右手重复。
+
+如果两个手柄在 ROS topic 里的某个轴都反了，优先检查 Unity 的坐标转换。
+
+如果 ROS topic 正确，但某一只机械臂运动方向不符合现场坐标，优先只改对应 YAML 的 `xr_to_robot_matrix`，不要同时改 Unity 坐标和机器人映射。
+
+## 9. 单臂真机小幅调试
+
+真机前检查：
+
+- 急停可用。
+- 机械臂工作区清空。
+- PICO topic 在 mock 下已经稳定。
+- `grip=false` 时 `/xr_rm/<arm>/cmd_vel` 为 0。
+- `move_to_initial_pose_on_connect` 保持 `false`。
+
+左臂：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false udp_port:=15000
+```
+
+右臂：
+
+```bash
+ros2 launch xr_rm_bringup arm_debug.launch.py arm:=right use_mock:=false udp_port:=15000
+```
+
+调试动作：
+
+1. 手柄保持静止。
+2. 按住 `grip`，第一帧只锁定起点，机械臂不应突然运动。
+3. 单轴移动手柄 `2-3 cm`。
+4. 松开 `grip`，确认机械臂停止。
+5. 每次只验证一个方向。
+
+## 10. 双臂真机调试
+
+只有在左右单臂都通过后，再启动双臂：
+
+```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 \
+  udp_port:=15000
+```
+
+双臂第一轮建议：
+
+1. 左右手 `grip` 都不按，确认双臂静止。
+2. 只按左手 `grip`，确认只有左臂响应。
+3. 只按右手 `grip`，确认只有右臂响应。
+4. 左右同时按住 `grip`，做小幅、慢速、单轴运动。
+
+## 11. 常见问题
+
+| 现象 | 排查 |
+| --- | --- |
+| PICO 启动后 ROS topic 没数据 | 检查 Unity `host` 是否是 Ubuntu IP；检查 PICO 和 Ubuntu 是否同网段；检查 `sudo tcpdump -ni any udp port 15000` |
+| tcpdump 有包但 topic 没数据 | JSON 字段不对；确认包含 `controllers.left/right` 或单包 `hand`；确认 `pos` 长度 3、`quat` 长度 4 |
+| topic 有数据但机械臂不动 | 检查 `grip` 是否为 `true`；检查 teleop 节点是否订阅对应话题；检查是否超过工作空间限幅 |
+| 按左手右臂动 | PICO 端 left/right 填反，或 Unity `XRNode.LeftHand/RightHand` 获取错误 |
+| 松开 grip 后仍有速度 | 确认 PICO 持续发送 `grip=false`，并检查 teleop 终端是否收到超时停止 |
+| 经常提示手柄数据超时 | 发送频率太低、网络丢包、PICO 应用后台暂停；提高 `sendHz`，保持应用前台运行 |
+| 前后方向反了 | 先切换 Unity 脚本里的 `convertUnityToProjectCoordinates`，再验证 topic 方向 |
+| 某一只臂方向反了 | 修改对应 YAML 的 `xr_to_robot_matrix` 符号 |
+| 抖动明显 | 降低 `scale` 或 `kp_linear`，提高 `deadband_m`，保持 `low_pass_alpha` 不要过大 |
+
+## 12. 参考入口
+
+- PICO Controller & HMD input mapping: https://developer-cn.picoxr.com/en/document/unity/input-mapping/
+- PICO Unity Integration SDK: https://github.com/Pico-Developer/PICO-Unity-Integration-SDK
+- PICO `PXR_Input` API: https://developer-cn.picoxr.com/en/reference/unity/client-api/PXR_Input/
+- Unity XR `InputDevice.TryGetFeatureValue`: https://docs.unity3d.com/ScriptReference/XR.InputDevice.TryGetFeatureValue.html
+- Unity XR `CommonUsages`: https://docs.unity3d.com/ScriptReference/XR.CommonUsages.html
+- Unity OpenXR input: https://docs.unity.cn/Packages/com.unity.xr.openxr%401.9/manual/input.html