Meta Quest 3 VR Teleop 设置

在编写任何代码之前，请确认 Meta Quest 3 和控制 PC 可以通过同一本地网络相互访问。 系统使用 UDP — 两个设备必须共享同一子网，并且 UDP 端口 8888 和 8889 必须在 PC 防火墙中打开。

要求清单：

• 启用手部追踪的 Meta Quest 3 耳机（设置 → 运动追踪 → 手部追踪）
• Wi-Fi 6 接入点 — 强烈建议将 UDP 传输延迟保持在 10 毫秒以下
• 控制运行 Linux (Ubuntu 22.04+) 或使用 Python 3.10+ 的 macOS 的 PC
• 对于 AgileX Piper：USB 转 CAN 适配器（例如 Kvaser 或 PEAK）、连接到手臂的 CAN 电缆
• PC 防火墙上的 UDP 端口 8888 和 8889 打开入站

验证连接：

# On the control PC — find your IP address
ip addr show   # Linux
ipconfig getifaddr en0   # macOS (Wi-Fi)

# Quick UDP listener test (run on PC, send any packet from Quest)
python3 -c “导入套接字; s=socket.socket(socket.AF_INET,socket.SOCK_DGRAM); s.bind(('0.0.0.0',8888)); print('监听...'); print(s.recvfrom(256))"

Quest 3 上运行的 Unity 应用程序是使用 Unity 2022.3 LTS 或更高版本构建的，使用 XR Hands 包进行手部跟踪。 三个脚本处理远程操作端：

• VRHandPoseSender.cs — 从 XR Hands 子系统读取手势，序列化为 4​​​​​​​​​​5 字节二进制数据包，通过 UDP 发送
• VRGripperController.cs — 将捏力映射到标准化夹具值 [0, 1]
• VR远程操作管理器.cs — 生命周期管理、连接状态 UI、自动重新连接

你做 不是 需要重新编译Unity应用程序才能在机械臂之间切换。 将以下字段公开为序列化的 Inspector 字段，并从 Unity 编辑器或通过配置文件调整它们：

Python 服务器运行三个并发线程： 接收线程 读取原始 UDP 数据报， 安全线 验证数据包并将其放入队列，以及 机器人控制线程 清空队列并调用机器人控制器。 这种分离可确保缓慢的机器人 SDK 调用不会阻塞 UDP 接收。

安装依赖项：

pip install python-can piper_sdk

# Activate the CAN interface (run once per boot, requires root or dialout group)
bash can_activate.sh can0 1000000

# Verify the interface is UP
ifconfig can0

运行远程操作服务器：

python3 teleoperation_main.py

服务器绑定到 0.0.0.0:8888 （右手）和可选 0.0.0.0:8889 （左手）。 当数据包到达时 valid 标志设置后，控制线程调用 robot.set_pose() 和 robot.set_gripper()。 按 Ctrl-C 触发紧急停止并彻底关闭。

显示所有三个线程如何交互的简化服务器结构：

# teleoperation_main.py (simplified structure)
进口 socket, struct, queue, threading, signal, time
从 piper_controller 进口 PiperController   # swap this to change arms

HOST = "0.0.0.0"
RIGHT_PORT    = 8888
LEFT_PORT     = 8889
QUEUE_MAXSIZE = 3   # drop stale frames if robot is slow
CONTROL_HZ    = 30  # robot command rate

pose_queue     = queue.Queue(maxsize=QUEUE_MAXSIZE)
shutdown_event = threading.Event()

定义 udp_接收器(port: int):
    """线程 1 — 接收原始 UDP 数据报。"""
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.bind((HOST, port))
    sock.settimeout(1.0)
    虽然没有 shutdown_event.is_set():
        尝试:
            data, _ = sock.recvfrom(256)
            pose = parse_packet(data)
            如果 pose 和 pose[“有效的”]:
                尝试:
                    pose_queue.put_nowait(pose)
                除了 queue.Full:
                    pose_queue.get_nowait()   # drop oldest frame
                    pose_queue.put_nowait(pose)
        除了 socket.timeout:
            继续
    sock.close()

定义 机器人控制循环(robot: PiperController):
    """线程 3 — CONTROL_HZ 处的排出队列和命令机器人。"""
    period = 1.0 / CONTROL_HZ
    last_pose = None
    虽然没有 shutdown_event.is_set():
        t0 = time.monotonic()
        尝试:
            pose = pose_queue.get(timeout=0.1)
            如果 pose[“停止”]:
                robot.emergency_stop()
                继续
            如果 pose[“有效的”]:
                last_pose = pose
                x, y, z = transform_position(pose[“位置”])
                roll, pitch, yaw = quat_to_euler(pose[“旋转”])
                robot.set_pose(x, y, z, roll, pitch, yaw)
                robot.set_gripper(pose[“抓手”])
            # tracking lost — hold last known position (do not send zero)
        除了 queue.Empty:
            经过
        time.sleep(max(0, period - (time.monotonic() - t0)))

这 piper_controller.py 模块封装了 AgileX piper_sdk Python 库。 它实现了预期的五方法接口 teleoperation_main.py。 关键设计决策：

• 通过 USB 进行 CAN： Piper 臂通过 CAN 总线进行通信。 SDK 采用 CAN 接口名称（can0）并要求在连接之前激活接口。
• 从机模式： 呼唤 MasterSlaveConfig(0xFC, 0, 0, 0) 将手臂置于从属模式，以便它接受流位置命令。
• 工作空间夹紧： 在每次 SDK 调用之前，位置都被硬钳位到文件顶部的常量 — 这是固件之前的最后一道软件防御线。
• SDK单位： 位置以微米（整数）为单位，方向以毫度为单位 - 在传递到之前将浮点毫米/度值乘以 1000 EndEffectorCtrl.

# piper_controller.py
从 piper_sdk 进口 C_PiperInterface
进口 math, logging

logger = logging.getLogger(__name__)

# Piper workspace limits (millimetres) — keep 50 mm inside physical limits
X_MIN, X_MAX = -400, 400
Y_MIN, Y_MAX = -400, 400
Z_MIN, Z_MAX =  50,  700

# Maximum joint speed (0–100 %; keep conservative for teleop)
SPEED_PERCENT = 25


班级 派珀控制器:
    """XArmController 的直接替代品 — 相同的五种方法接口。"""

    定义 __初始化__(self, can_interface: str = “可以0”):
        self.can_interface = can_interface
        self._piper: C_PiperInterface | None = None
        self.connected = False

    定义 连接(self) -> bool:
        """打开CAN端口并启用从机模式。"""
        尝试:
            self._piper = C_PiperInterface(
                can_name=self.can_interface,
                judge_flag=False,     # allow 3rd-party CAN adapters
                can_auto_init=True,
                dh_is_offset=1,      # firmware >= V1.6-3
            )
            self._piper.ConnectPort()
            self._piper.MasterSlaveConfig(0xFC, 0, 0, 0)  # enter slave mode
            self.connected = True
            logger.info(“Piper 在 %s 上已连接”, self.can_interface)
            返回 True
        除了 Exception 作为 e:
            logger.error(“Piper 连接失败：%s”, e)
            返回 False

    定义 断开(self):
        """禁用伺服并关闭端口。"""
        如果 self._piper:
            尝试:
                self._piper.DisableArm(7)   # disable all joints
            除了 Exception:
                经过
        self.connected = False

    定义 设置姿势(self, x: float, y: float, z: float,
                 roll: float, pitch: float, yaw: float):
        """将末端执行器移动到 (x,y,z) 毫米，方向为（滚动、俯仰、偏航）度数。"""
        如果不 self.connected:
            返回
        x = max(X_MIN, min(X_MAX, x))
        y = max(Y_MIN, min(Y_MAX, y))
        z = max(Z_MIN, min(Z_MAX, z))
        尝试:
            self._piper.EndEffectorCtrl(
                整数(x * 1000), 整数(y * 1000), 整数(z * 1000),
                整数(roll  * 1000),
                整数(pitch * 1000),
                整数(yaw   * 1000),
                SPEED_PERCENT,
            )
        除了 Exception 作为 e:
            logger.warning(“设置姿势错误：%s”, e)

    定义 设置夹具(self, value: float):
        """设置夹具打开度。0.0 = 全闭，1.0 = 全开。"""
        如果不 self.connected:
            返回
        GRIPPER_MAX_UM = 70_000   # 70 mm max opening in µm
        target_um = 整数(value * GRIPPER_MAX_UM)
        尝试:
            self._piper.GripperCtrl(target_um, SPEED_PERCENT, 0x01, 0)
        除了 Exception 作为 e:
            logger.warning(“set_gripper 错误：%s”, e)

    定义 紧急停止(self):
        """立即禁用所有关节。可以安全地从任何线程调用。"""
        如果 self._piper:
            尝试:
                self._piper.DisableArm(7)
            除了 Exception:
                经过

试运行检查表（关闭电源）：

• 启动 Python 服务器并连接 Quest 3。 观看 transform_position 输出打印到终端。
• 将您的手放在预期工作空间的中心。 确认打印的 XYZ 值接近机器人的起始位置（Piper 约为 0、0、300 毫米）。
• 将手移至每个工作区边缘。 确认值保持在限制范围内并且 Z 轴上永远不会为负值。
• 按 Quest 上的菜单按钮触发软件急停。 确认 emergency_stop() 被调用并且循环停止。

第一场直播：

• 开始于 SPEED_PERCENT = 20 — 这大约是 40°/s 的最大关节速度。
• 启用伺服电源。 缓慢移动，第一分钟停留在手臂的原始位置附近。
• 验证手部运动和机器人运动方向是否相同。 如果手腕向后旋转，请调整 rotationOffset 在 Unity 检查器中 Y 上±90°。
• 确认方向映射正确后，逐渐扩大运动范围。
• 始终保持物理紧急停止装置（电源继电器）触手可及。

实施了两个软件急停路径：

• 任务 3 菜单按钮： 设置每个后续 UDP 数据包中标志字节的位 1。 Python 服务器调用 robot.emergency_stop() 立即地。
• Ctrl-C（信号）： 信号处理程序设置关闭事件，这会导致控制循环调用 emergency_stop() 并干净地退出。

控制器交换模式适用于任何具有 Python SDK 的手臂。 唯一的要求是您的控制器类使用以下签名实现这五个方法：

添加新手臂的步骤：

1. 写 myarm_controller.py 使用arm的SDK实现上述五种方法。 将工作区边界和速度限制硬编码为模块级常量。
2. 隔离单元测试：调用 connect()， 然后 set_pose 距离家 5 厘米的安全位置。 验证手臂移动并返回预期位置。
3. 交换导入 teleoperation_main.py： 代替 from piper_controller import PiperController 使用您的新控制器。 无需其他更改。
4. 校准 Unity Inspector 参数（positionOffset, rotationOffset, scaleFactor）用于新手臂的工作空间。
5. 在任何操作员会话之前验证急停行为、跟踪丢失保持和工作空间夹具。