acRealman_xr_from_Yikai/docs/pico4_ultra_udp_teleop_setup.md

PICO 4 Ultra UDP 手柄接入与调试教程

本文档用于把 PICO 4 Ultra 左右手柄接入当前 XR-RM75 ROS2 工作空间。目标是让 PICO 端发送符合本项目约定的 UDP JSON，ROS 端发布：

/xr/left_controller
/xr/right_controller

然后由 single_arm_velocity_teleop 把手柄相对位移转换成 RM75 TCP 运动。

1. 当前项目约定

当前链路如下：

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 端每包同时发送左右手柄：

{
  "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]
    }
  }
}

也兼容单手柄包：

{
  "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：

hostname -I

假设输出里有 192.168.1.42，Unity 脚本里的 host 就填 192.168.1.42。

3. 如果 Ubuntu 开了防火墙，放行 UDP 端口：

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。

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 执行：

source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch xr_rm_input udp_receiver.launch.py udp_port:=15000

另开终端查看左右手柄：

source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 topic echo /xr/left_controller

source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 topic echo /xr/right_controller

再查看频率：

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：

sudo tcpdump -ni any udp port 15000

能看到 UDP 但 ROS topic 没数据，说明 JSON 字段不符合协议。看 udp_controller_receiver 终端里的 XR 数据包格式错误 或 XR 手柄字段错误。

6. 用 sample_udp_sender 排除 ROS 端问题

在接 PICO 前，先确认 ROS 端链路是通的。

终端 1：

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：

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：

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：

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，只移动左手一小段，观察：

ros2 topic echo /xr_rm/left_rm75/cmd_vel

5. 松开左手 grip，确认 cmd_vel 回到 0。
6. 右手重复同样流程。
7. 用 ros2 topic hz 确认频率稳定，建议 50 Hz 以上。

8. 坐标方向检查

当前配置使用的 PICO/OpenXR 位置坐标：

+X: 向右
+Y: 向上
+Z: 向后

双臂配置中的映射关系：

左臂机器人位移增量 = [-手柄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。

左臂：

ros2 launch xr_rm_bringup arm_debug.launch.py arm:=left use_mock:=false udp_port:=15000

右臂：

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. 双臂真机调试

只有在左右单臂都通过后，再启动双臂：

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