add unity doc.

2026-05-30 16:04:37 +08:00
parent ce52895ec2
commit f4ee53d9c3
1538 changed files with 221228 additions and 2192 deletions
--- a/README.md
+++ b/README.md
@ -1,9 +1,9 @@
 # XR-RM75 双臂遥操作工作空间

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

 ```text
-PICO/XR 双手柄 UDP 数据
+PICO/XR 双手柄 UDP JSON
  -> xr_rm_input/udp_controller_receiver
  -> /xr/left_controller 与 /xr/right_controller
  -> xr_rm_teleop/single_arm_velocity_teleop
@ -21,41 +21,50 @@ PICO/XR 双手柄 UDP 数据
 - 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
 - RM75 真机连接适配，包含速度透传初始化、安全速度/加速度配置、可选初始化点位移动。
 - Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
+- 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip` 和 `trigger`。

 暂未完成：

- 末端执行器与采摘夹爪完整 launch 集成。
 - D405/D435 视频流、数据记录、相机标定和目标检测链路。
 - 双臂碰撞模型、任务级状态机、自动采摘策略。
- PICO 端正式应用与 ROS 端的完整时间同步和状态回传。
+- PICO 端与 ROS 端的完整时间同步和状态回传。

 ## 项目结构

 ```text
 src/
-├── README.md
+├── README.md                       # 项目主文档
+├── CODEX.md                        # Codex/Claude Code 项目工作流和安全规则
+├── docs/
+│   └── pico_udp_sender_ubuntu22_setup.md
+├── unity/
+│   ├── XR_RM_PICO_UDP_Sender/      # PICO 4 Ultra UDP Sender Unity 工程
+│   └── 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      # 右臂单独调试配置
+│   │   ├── 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
+│   │   └── arm_debug.launch.py     # 统一入口：arm:=left/right/both, use_mock:=true/false
 │   └── tools/
-│       └── launcher_ui.py           # 图形化调试启动面板
+│       ├── launcher_ui.py          # 图形化调试启动面板
+│       └── realman_dual_arm_state_monitor.py
 ├── xr_rm_input/
 │   ├── launch/
-│   │   └── udp_receiver.launch.py   # 低层 UDP 接收测试入口
+│   │   └── udp_receiver.launch.py  # 低层 UDP 接收测试入口
 │   └── xr_rm_input/
 │       ├── udp_controller_receiver.py
-│       └── sample_udp_sender.py     # 本机模拟手柄 UDP 数据
+│       └── sample_udp_sender.py    # 本机模拟手柄 UDP 数据
 ├── xr_rm_interfaces/
 │   └── msg/
-│       └── XrController.msg         # hand/grip/trigger/pose
+│       └── XrController.msg        # hand/grip/trigger/pose
 └── xr_rm_teleop/
    └── xr_rm_teleop/
        ├── single_arm_velocity_teleop.py
-        └── realman_adapter.py
+        ├── realman_adapter.py
+        └── fun_peripheral.py
 ```

 `single_arm_velocity_teleop` 这个名字保留是有意的：双臂模式不是一个大节点直接控制两台机械臂，而是启动两个相同的单臂控制节点，分别命名为 `left_arm_teleop` 和 `right_arm_teleop`。
@ -65,6 +74,7 @@ src/
 在工作空间根目录，也就是包含 `src/` 的目录执行：

 ```bash
+cd /home/robot/WS_xr
 source /opt/ros/humble/setup.bash
 rosdep update
 rosdep install --from-paths src -y --ignore-src
@ -87,6 +97,7 @@ export XR_RM_WS=/home/robot/WS_xr
 源码方式启动：

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

@ -104,7 +115,7 @@ ros2 run xr_rm_bringup launcher_ui
 - `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` 检查。
+- `Diagnostics`：`ros2 doctor --report` 和核心包的 `ros2 pkg prefix` 检查。

 常用按钮：

@ -114,9 +125,9 @@ ros2 run xr_rm_bringup launcher_ui

 每个模式都会附带三个监控入口：

- `Open Controller Topic Monitor`：用 Terminator 分屏同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
- `Open Cmd Vel Topic Monitor`：用 Terminator 分屏同时查看 `/xr_rm/left_rm75/cmd_vel` 和 `/xr_rm/right_rm75/cmd_vel`。
- `Open ROS Topic/Node List Monitor`：用 Terminator 分屏每秒刷新 `ros2 topic list` 和 `ros2 node list`。
+- `Open Controller Topic Monitor`：同时查看 `/xr/left_controller` 和 `/xr/right_controller`。
+- `Open Cmd Vel Topic 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`。

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

@ -201,6 +212,26 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \

 `xr_rm_bringup/config/peripherals_rm75.yaml` 保存末端工具坐标、负载和左右臂外设选择。当前配置为左臂 `scissorgripper=1`、右臂 `scissorgripper=2`，并且只在真机连接阶段初始化外设，不在主遥操作循环里控制开合。

+重点控制参数：
+
+- `controller_topic`：订阅的手柄话题。
+- `scale`：手柄位移到 TCP 位移的比例。
+- `kp_linear`：位置误差到速度命令的比例增益。
+- `max_linear_speed`：软件侧最大线速度。
+- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
+- `cyl_radius_limit`：基座圆柱半径限制。
+- `xr_to_robot_matrix`：PICO/OpenXR 位移到 RM75 base 坐标的映射矩阵。
+- `mock_initial_pose`：mock 模式初始 TCP 位姿。
+- `initial_joint_pose` / `initial_tcp_pose`：可选真机初始化点。
+
+当前坐标约定：
+
+- PICO/OpenXR：`+X` 向右，`+Y` 向上，`+Z` 向后。
+- 左臂映射：机器人位移增量 = `[-手柄y, 手柄z, -手柄x]`。
+- 右臂映射：机器人位移增量 = `[手柄y, 手柄z, 手柄x]`。
+
+如果某个方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
+
 ## 末端工具开合

 真机 launch 默认会在遥操作节点内启用工具控制。启动后可以用 Bool 话题控制开合，`true` 表示打开，`false` 表示闭合：
@ -215,26 +246,6 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa

 桌面 UI 的 `Left Arm`、`Right Arm` 和 `Dual Arm` 模式里也有对应的 Tool Open/Close 命令项。

-重点参数：
-
- `controller_topic`：订阅的手柄话题。
- `scale`：手柄位移到 TCP 位移的比例。
- `kp_linear`：位置误差到速度命令的比例增益。
- `max_linear_speed`：软件侧最大线速度，当前默认 `0.04`，真机初期保持低速。
- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
- `cyl_radius_limit`：基座圆柱半径限制。
- `xr_to_robot_matrix`：PICO/OpenXR 位移到 RM75 base 坐标的映射矩阵。
- `mock_initial_pose`：mock 模式初始 TCP 位姿。
- `initial_joint_pose` / `initial_tcp_pose`：可选真机初始化点。
-
-当前坐标约定：
-
- PICO/OpenXR：`+X` 向右，`+Y` 向上，`+Z` 向后。
- 左臂映射：机器人位移增量 = `[-手柄y, -手柄z, 手柄x]`。
- 右臂映射：机器人位移增量 = `[手柄y, -手柄z, -手柄x]`。
-
-如果某个方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
-
 ## UDP 数据格式

 单个手柄：
@ -244,7 +255,7 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
  "hand": "right",
  "grip": true,
  "trigger": 0.2,
-  "pos": [0.12, 1.05, -0.32],
+  "pos": [0.12, 1.05, 0.30],
  "quat": [0.0, 0.0, 0.0, 1.0]
 }
 ```
@ -257,13 +268,13 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
    "left": {
      "grip": true,
      "trigger": 0.0,
-      "pos": [-0.12, 1.05, -0.32],
+      "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.32],
+      "pos": [0.12, 1.05, 0.30],
      "quat": [0.0, 0.0, 0.0, 1.0]
    }
  }
@ -278,6 +289,8 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
 - `pos`：手柄位置，长度 3。
 - `quat`：手柄姿态四元数，默认按 `xyzw` 解析。

+PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并向 ROS2 主机发送 UDP 的详细步骤见 [docs/pico_udp_sender_ubuntu22_setup.md](docs/pico_udp_sender_ubuntu22_setup.md)。
+
 ## 真机安全验证

 第一次接真机时按这个顺序走：
@ -294,42 +307,13 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa

 ## 后续优化路线

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

-### 1. 先把遥操作闭环做稳
-
- 用真实 PICO 数据替换 `sample_udp_sender`，固定 UDP JSON 协议和坐标系。
- 增加手柄数据频率、延迟、丢包统计话题。
- 记录 `/xr/*_controller`、`/xr_rm/*/cmd_vel`、`/xr_rm/*/current_pose`，用于回放和复盘。
- 完成 `xr_to_robot_matrix` 的现场标定流程，把标定结果固化进 YAML。
-
-### 2. 提升真机安全性
-
- 加入启动前安全检查节点：网络可达、SDK 可用、初始姿态在工作区内。
- 增加硬件急停状态、软件急停 topic 和 UI 上的 Stop 状态提示。
- 给左右臂增加更细的工作区限制，尤其是双臂中间区域的互斥边界。
- 在速度命令外再加一层限幅和加速度限制，降低手柄抖动带来的突变。
-
-### 3. 集成末端执行器
-
- 新增夹爪控制节点或桥接节点，并纳入 `arm_debug.launch.py` 与 `launcher_ui.py`。
- 明确夹爪 topic、力控比例、开合方向和安全上限。
- 在 `launcher_ui.py` 中加入夹爪测试按钮和夹爪状态监控。
- 将 `trigger` 从“预留字段”变成稳定的夹爪控制输入。
-
-### 4. 接入视觉和数据记录
-
- 加入 D405/D435 相机 launch、相机内外参和 TF。
- 统一保存手柄、机械臂、相机、夹爪状态，用 rosbag2 做实验记录。
- 增加一键启动“采集模式”：双臂遥操作 + 相机 + rosbag2。
- 从记录数据中提取标注样本，为后续检测/策略学习做准备。
-
-### 5. 从遥操作走向半自动
-
- 先做目标检测和 3D 定位，只给操作者显示目标点，不直接控制机械臂。
- 再做单臂辅助：自动靠近目标，人工确认抓取。
- 最后做双臂任务分配：左/右臂根据目标位置、可达性和避碰约束自动选择。
- 加入任务级状态机：搜索、接近、抓取、放置、恢复、异常停止。
+1. 稳定 PICO 数据链路：固定 UDP JSON 协议和坐标系，增加频率、延迟、丢包统计，记录 `/xr/*_controller`、`/xr_rm/*/cmd_vel`、`/xr_rm/*/current_pose`。
+2. 提升真机安全性：增加启动前安全检查、软件急停 topic、UI Stop 状态提示、双臂中间区域互斥边界和速度/加速度限幅。
+3. 集成末端执行器：明确夹爪 topic、力控比例、开合方向和安全上限，把 `trigger` 从预留字段变成稳定夹爪输入。
+4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
+5. 从遥操作走向半自动：先做目标检测和 3D 定位提示，再做单臂辅助，最后做双臂任务分配和任务级状态机。

 ## 常见问题