optimze unity project for xr_rm
This commit is contained in:
98
README.md
98
README.md
@ -21,7 +21,7 @@ PICO/XR 双手柄 UDP JSON
|
||||
- 通过统一的 `arm_debug.launch.py` 支持左臂、右臂、双臂的 mock 调试和真机调试。
|
||||
- RM75 真机连接适配，包含 `rm_movep_canfd` 位姿透传、安全速度/加速度配置、可选初始化点位移动。
|
||||
- Tkinter 启动面板 `launcher_ui.py`，用于现场快速启动、监控 topic、检查环境和清理进程。
|
||||
- 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip` 和 `trigger`。
|
||||
- 自定义 PICO 4 Ultra UDP Sender Unity 工程，负责发送左右手柄 pose、`grip`、`trigger` 和 pose 诊断字段。
|
||||
|
||||
暂未完成：
|
||||
|
||||
@ -36,9 +36,14 @@ src/
|
||||
├── README.md # 项目主文档
|
||||
├── CODEX.md # Codex/Claude Code 项目工作流和安全规则
|
||||
├── docs/
|
||||
│ └── pico_udp_sender_ubuntu22_setup.md
|
||||
│ └── 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/
|
||||
@ -56,7 +61,7 @@ src/
|
||||
│ │ └── 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
|
||||
@ -111,7 +116,7 @@ ros2 run xr_rm_bringup launcher_ui
|
||||
|
||||
面板顶部的 `Mode` 分为五类：
|
||||
|
||||
- `Simulation`：左臂 mock、右臂 mock、双臂 mock、sample UDP 发送、one-click mock demo。
|
||||
- `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。
|
||||
@ -123,12 +128,14 @@ ros2 run xr_rm_bringup launcher_ui
|
||||
- `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。若提示不支持，可安装并切换：
|
||||
|
||||
```bash
|
||||
@ -148,9 +155,12 @@ sudo update-alternatives --config x-terminal-emulator
|
||||
|
||||
```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 --seconds 10
|
||||
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`。
|
||||
|
||||
观察：
|
||||
|
||||
```bash
|
||||
@ -200,8 +210,12 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
|
||||
- `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`。
|
||||
- `configure_peripheral_on_connect`：遥操作节点连接真机后是否配置末端外设，默认 `true`；工具控制会复用同一个 RealMan 连接，避免两个进程同时抢占同一机械臂。
|
||||
- `move_to_initial_pose_on_connect`：连接后是否执行 `movej`/`movel` 初始化，默认 `false`。
|
||||
@ -226,18 +240,20 @@ ros2 launch xr_rm_bringup arm_debug.launch.py arm:=both use_mock:=false \
|
||||
- `max_linear_speed`：目标位姿单帧步长限制对应的最大线速度。
|
||||
- `workspace_min` / `workspace_max`：笛卡尔工作空间边界。
|
||||
- `cyl_radius_limit`：基座圆柱半径限制。
|
||||
- `xr_to_robot_matrix`：PICO/OpenXR 位移到 RM75 base 坐标的映射矩阵。
|
||||
- `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 坐标约定：
|
||||
|
||||
- PICO/OpenXR：`+X` 向右，`+Y` 向上，`+Z` 向后。
|
||||
- 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_to_robot_matrix` 符号，不要同时改多个控制参数。
|
||||
如果 `/xr/*_controller.pose.position` 已符合 Project 坐标，但某个机械臂方向相反，只改对应臂的 `xr_to_robot_matrix` 符号，不要同时改多个控制参数。
|
||||
|
||||
## 末端工具开合
|
||||
|
||||
@ -255,34 +271,52 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
|
||||
|
||||
## UDP 数据格式
|
||||
|
||||
单个手柄：
|
||||
|
||||
```json
|
||||
{
|
||||
"hand": "right",
|
||||
"grip": true,
|
||||
"trigger": 0.2,
|
||||
"pos": [0.12, 1.05, 0.30],
|
||||
"quat": [0.0, 0.0, 0.0, 1.0]
|
||||
}
|
||||
```
|
||||
|
||||
左右手柄一起发送：
|
||||
当前 Unity APK 每个周期发送一个双手柄 JSON 包：
|
||||
|
||||
```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]
|
||||
"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]
|
||||
"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
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -290,11 +324,19 @@ ros2 topic pub --once /xr_rm/right_rm75/tool_enable std_msgs/msg/Bool "{data: fa
|
||||
|
||||
字段说明：
|
||||
|
||||
- `hand`：`left` 或 `right`。
|
||||
- `t` / `source_time`：Unity 端 `Time.realtimeSinceStartupAsDouble`，用于后续延迟分析。
|
||||
- `seq`：Unity 端递增包序号，用于后续丢包分析。
|
||||
- `frame_id`：默认 `xr_world`，会写入 `XrController.header.frame_id`。
|
||||
- `grip`：运动使能。`true` 时进入相对位移控制，`false` 时停止。
|
||||
- `trigger`：扳机值，范围 `0.0-1.0`。当前控制主链路不使用它，预留给后续夹爪集成。
|
||||
- `pos`：手柄位置，长度 3。
|
||||
- `quat`：手柄姿态四元数，默认按 `xyzw` 解析。
|
||||
- `pose_valid`：姿态是否可信。ROS 接收端看到 `false` 会强制 `grip=false`。
|
||||
- `pose_source`：`pxr_predict`、`unity_xr` 或 `none`，用于判断姿态来自 PICO 预测接口还是 Unity XR fallback。
|
||||
- `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](docs/pico_udp_sender_ubuntu22_setup.md)。
|
||||
|
||||
@ -316,7 +358,7 @@ PICO 4 Ultra 在 Ubuntu 22.04 下配置 Unity、构建 APK、安装到头显并
|
||||
|
||||
为了达到“稳定可用的双臂 XR 遥操作/采摘平台”，建议按下面顺序推进：
|
||||
|
||||
1. 稳定 PICO 数据链路：固定 UDP JSON 协议和坐标系，增加频率、延迟、丢包统计，记录 `/xr/*_controller`、`/xr_rm/*/raw_target_pose`、`/xr_rm/*/target_pose`、`/xr_rm/*/target_clamped`、`/xr_rm/*/current_pose`。
|
||||
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. 集成末端执行器：明确夹爪 topic、力控比例、开合方向和安全上限，把 `trigger` 从预留字段变成稳定夹爪输入。
|
||||
4. 接入视觉和数据记录：加入 D405/D435 相机 launch、TF、内外参和 rosbag2 实验记录。
|
||||
@ -344,10 +386,12 @@ 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 没有报警或急停。
|
||||
|
||||
Reference in New Issue
Block a user