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/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