从 PICO 动捕到机器人动作重定向

在前一篇 GMR 工程梳理里（传送门），我主要总结的是"已有动作数据集如何重定向到机器人"。例如 AMASS中的 SMPL-X 动作，本身已经是一份结构化的人体运动数据；GMR 要做的是把这份人体动作送入机器人运动学模型，通过 IK 配置求解出目标机器人的 root 和关节轨迹。

这篇文章讨论的是另一个问题：如果我们不想只依赖公开动作数据集，而是希望自己录制动作，让机器人学习真实人体示教，应该怎么做？

本文打通的流程是：使用 PICO 和 tracker 完成人体动作采集，通过 XRoboToolkit 获取实时人体骨架，再把这份骨架保存成 raw 动作数据，最后接入 GMR 的重定向框架，生成指定机器人的动作文件。

整体链路可以概括为：

text 复制代码

真人动作
  -> PICO 头显 / 手柄 / 5 个 tracker
  -> PICO full-body tracking
  -> XRoboToolkit Unity Client
  -> XRoboToolkit PC Service
  -> Python 侧 xrobotoolkit_sdk
  -> XRobot raw skeleton npz
  -> GMR xrobot source
  -> 指定机器人 IK retargeting
  -> robot motion pkl
  -> 后续强化学习 / 模仿学习训练

需要强调的是，这里 PICO 并不是直接遥操作机器人。我们真正想要的是一条"动作数据生产Pipeline"：把真人表演固化成可保存、可回放、可反复重定向的数据，再生成机器人训练需要的动作轨迹。

一、背景：为什么选择 PICO，以及 XRoboToolkit 提供了什么

1. 从光学动捕到消费级 XR 动捕

传统机器人模仿学习中，如果要采集高质量人体动作，最经典的方案是光学动捕系统。光学 MOCAP 的优势很明显：精度高、延迟低、标定成熟，尤其适合实验室环境下的高质量动作采集。但它也有一些现实问题：

系统成本高；
场地和相机布置要求高；
标定和维护成本高；
对普通开发者和小团队并不友好；
采集流程不够轻量，难以快速反复试动作。

而 PICO 这类 XR 设备的优势，恰好在于它足够轻量。它本身是一款成熟的商业产品，已经迭代到 PICO 4 系列，并且有面向企业场景的版本。头显、手柄、全身体感 tracker、软件生态和调试工具都已经比较完整。对于机器人动作采集来说，它不是要完全替代高端光学动捕，而是提供了一条更低成本、更容易部署、更适合快速实验的数据采集路线。

在我们当前的使用方式里，PICO 配合 5 个 tracker，形成一套约 7 点追踪的身体观测输入。PICO 端的 full-body tracking 会进一步将这些观测融合为一套 24 关节人体骨架。也就是说，我们并不是直接使用 tracker 的原始点位，而是使用 PICO 追踪系统已经估计好的 body skeleton。

这点非常重要。因为 5 个 tracker 本身并不能覆盖人体所有关节，但 PICO 内部会利用人体模型、头显、手柄和 tracker 信息估计完整身体姿态。我们在 Python 侧拿到的，就是这套已经融合后的骨架结果。

2. XRoboToolkit 是什么

要从 PICO 里拿到这些实时追踪数据，需要一个数据通道。这里用到的是 XRoboToolkit。

XRoboToolkit 是 PICO / XR-Robotics 公开提供的 XR 机器人遥操作和数据采集工具链。它的核心仓库主要包括：

XRoboToolkit Unity Client: https://github.com/XR-Robotics/XRoboToolkit-Unity-Client
XRoboToolkit PC Service: https://github.com/XR-Robotics/XRoboToolkit-PC-Service

Unity Client 运行在 PICO 端，负责选择和发送头显、手柄、手部、PICO Motion Tracker 等追踪数据。PC Service 运行在电脑端，负责和头显建立连接，并通过 SDK 对外提供这些 XR 状态。

官方 Unity Client 的说明中也可以看到，它支持同步 head tracking、controller tracking、hand tracking，以及 PICO Motion Tracker 的 full body tracking 数据。对我们来说，最有价值的正是 full-body tracking 输出的人体骨架。

不过，PC Service 官方提供的主要是 C++ SDK。为了让 Python 脚本直接读取这些数据，我们还需要一层 Python binding，也就是 xrobotoolkit_sdk。这层 binding 的作用很单纯：把 C++ SDK 中的接口包装成 Python 模块，让我们可以在 GMR 侧直接调用：

python 复制代码

import xrobotoolkit_sdk as xrt

xrt.init()
body_poses = xrt.get_body_joints_pose()

因此，当前流程中真正需要的 XRoboToolkit 部分是：

text 复制代码

PICO Unity Client
  -> PC Service
  -> C++ SDK
  -> Python binding xrobotoolkit_sdk
  -> Python 中的人体骨架数据

对于这条动作采集到重定向的 pipeline 来说，XRoboToolkit 的角色很明确：把 PICO 的追踪结果稳定地送到 Python。

3. 哪些是 GMR 已有能力，哪些是我们新增的工作

为了说清楚这条链路，必须区分"GMR 原本已经有的能力"和"为了 PICO 动捕我们新增的能力"。

GMR 原本已经具备的核心能力包括：

GeneralMotionRetargeting 的 IK 重定向框架；
MuJoCo 机器人模型加载；
基于 mink.solve_ik() 的机器人 IK 求解；
SMPL-X / BVH 等人体数据源到机器人动作的基本范式；
RobotMotionViewer 等可视化工具；
机器人 motion pkl 的保存格式。

也就是说，GMR 已经解决了"给定一份人体骨架目标，如何求解机器人动作"的大问题。

我们这次新增或迁移变体的工作，主要集中在 PICO/XRobot 数据源上：

新增 XRobot 数据读取：从 xrobotoolkit_sdk 获取 PICO body tracking；
新增 raw skeleton 录制：把 PICO 输出的 24 关节骨架保存为 xrobot_raw_skeleton_v1；
新增 raw skeleton 回放：单独验证动捕源数据是否正确；
新增 XRobot 离线重定向：把 raw .npz 转成 robot .pkl；
新增或迁移 XRobot 到目标机器人的 IK 配置，例如 xrobot_to_target_robot.json；
对已有可视化/对比脚本做适配，使其能识别 XRobot raw .npz。

因此，这条 pipeline 的本质不是重写 GMR，而是给 GMR 增加了一种新的人体数据来源：src_human="xrobot"。

二、从 PICO 到 raw 动作数据：先把人体骨架录下来

1. 为什么 raw 数据是这条链路的核心

在动捕重定向里，最容易犯的错误是直接看机器人动作。如果机器人动得不对，马上开始调 IK 配置。但实际上，问题可能出在前面任何一层：PICO 校准、tracker 佩戴、PC Service 数据流、坐标系转换、关节命名、IK 映射等。

所以我们把流程拆成两段：

text 复制代码

第一段：PICO -> XRobot raw skeleton npz
第二段：XRobot raw skeleton npz -> robot motion pkl

第一段只回答一个问题：PICO 给出的人体骨架准不准？

第二段才回答另一个问题：机器人能不能模仿这份人体骨架？

这样拆开之后，调试就清楚很多。如果 raw skeleton 回放已经很准，说明 PICO、tracker、PC Service 和基础坐标转换基本没问题；如果机器人动作仍然不对，问题就应该集中到 GMR 的 IK 配置、机器人模型或后处理上。

更重要的是，raw skeleton 是可复现的数据。一旦录下来，后面无论怎么修改 xrobot_to_target_robot.json，都可以用同一份 raw .npz 反复转换，而不需要每次重新穿戴设备表演动作。

2. XRobot body skeleton 的数据结构

XRoboToolkit 输出的 full-body tracking 数据，在 Python 侧可以整理成 24 个关节。典型关节包括：

text 复制代码

Pelvis, Left_Hip, Right_Hip, Spine1, Left_Knee, Right_Knee,
Spine2, Left_Ankle, Right_Ankle, Spine3, Left_Foot, Right_Foot,
Neck, Left_Collar, Right_Collar, Head, Left_Shoulder, Right_Shoulder,
Left_Elbow, Right_Elbow, Left_Wrist, Right_Wrist, Left_Hand, Right_Hand

每个关节在每一帧包含两个核心量：

text 复制代码

position: [x, y, z]
rotation: [qx, qy, qz, qw]

XRoboToolkit 原始姿态四元数通常以 xyzw 形式从 SDK 返回，而 GMR 内部使用 wxyz。因此在 Python 整理数据时，需要统一四元数顺序。

最终每一帧会变成 GMR 熟悉的结构：

python 复制代码

{
    "Pelvis": (position, quaternion_wxyz),
    "Left_Hip": (position, quaternion_wxyz),
    "Right_Hip": (position, quaternion_wxyz),
    ...
}

这和 SMPL-X 前向计算之后得到的人体关节字典，在形式上是一致的。区别只是：SMPL-X 是由人体模型参数计算出来的，XRobot 是由 PICO 实时追踪系统估计出来的。

3. 坐标系转换：参考 GMR 中已有的 G1 样本

PICO / Unity 和 MuJoCo / GMR 的坐标系并不天然一致。XRobot 数据不能直接送入 GMR，否则会出现左右方向、身体朝向、四肢旋转都不对的问题。

这个部分我们没有从零拍脑袋设计，而是参考了 GMR 工程中已经配置好的 xrobot_to_g1.json 样本。G1 是 GMR 已经支持的 XRobot 重定向目标，它相当于提供了一个可用的坐标系参照：XRobot 数据应该怎样从 Unity/PICO 的表达迁移到 GMR 的人体目标表达，再怎样进入 MuJoCo 机器人坐标系。

在 XRobot 数据进入 Python 后，会先进行 Unity 到右手坐标系的变换。位置变换使用的矩阵为：

text 复制代码

[[1, 0,  0],
 [0, 0, -1],
 [0, 1,  0]]

旋转也必须做对应变换，不能只变位置。否则骨架点的位置可能看起来差不多，但各关节局部坐标系会错，进入 IK 后就会表现为手臂方向错误、脚掌方向异常、躯干旋转不自然等问题。

在迁移到 target_robot 时，我们沿着 G1 样本的思路处理 XRobot 坐标系，然后重新建立 XRobot 关节到 target_robot MuJoCo body 的映射。也就是说，坐标系处理参考的是 GMR 已有 XRobot->G1 的成功样本；机器人匹配关系和偏移，则需要针对target_robot 单独配置。

4. raw skeleton 文件保存什么

我们最终把 PICO 输出的人体骨架保存成 .npz，格式命名为：

text 复制代码

xrobot_raw_skeleton_v1

它主要包含：

python 复制代码

{
    "format": "xrobot_raw_skeleton_v1",
    "fps": 30.0 or 50.0,
    "joint_names": [...],
    "positions": (T, 24, 3),
    "rotations": (T, 24, 4),
    "valid": (T, 24),
    "timestamps": (T,)
}

其中：

positions 保存每帧每个关节的三维位置；
rotations 保存每帧每个关节的四元数，统一为 wxyz；
valid 标记关节在该帧是否有效；
fps 是录制帧率；
timestamps 保留来自设备或控制器侧的时间信息。

这份 raw .npz 是整条 pipeline 中最重要的中间数据。它不是机器人动作，也不是训练文件，而是一份"人体动作源数据"。它的价值在于可以独立回放、独立检查，并且可以被多次重定向到不同机器人。

5. raw skeleton 的质量判断

判断动捕质量时，应该先看 raw skeleton，而不是看机器人。

如果 raw skeleton 本身已经和 PICO 数字人的动作一致，说明人体追踪源数据是可信的。此时如果机器人姿态不对，重点就应该转向 IK 配置。

如果 raw skeleton 本身就有明显问题，例如左右脚错位、骨盆漂移、腿长异常、身体姿态不连续，那么优先检查：

tracker 是否正确绑定；
PICO 端 full-body tracking 是否开启；
tracker calibration 是否准确；
PC Service 是否稳定接收 body data；
坐标系转换是否正确。

这个判断顺序很重要。它可以避免把动捕源数据的问题误判成机器人重定向的问题。

三、从 raw skeleton 到机器人重定向

1. 接入 GMR 的方式：新增 `src_human="xrobot"`

当 raw skeleton 已经可靠之后，后面的工作就回到了 GMR 熟悉的范式：给定人体目标，求机器人 IK。

区别在于，SMPL-X 流程中的人体目标来自 SMPL-X body model 前向计算，而 PICO 流程中的人体目标来自 XRobot raw skeleton。为了让 GMR 统一处理，我们把 XRobot 也注册成一种人体数据源：

python 复制代码

retargeter = GeneralMotionRetargeting(
    src_human="xrobot",
    tgt_robot="target_robot",
    actual_human_height=actual_human_height,
)

这样，GMR 会加载对应的 IK 配置，例如：

text 复制代码

general_motion_retargeting/ik_configs/xrobot_to_target_robot.json

从这一层开始，流程和 SMPL-X 重定向非常相似：缩放人体骨架、应用偏移、设置 IK target、调用 mink.solve_ik()，最后得到机器人 qpos。

因此，robot motion pkl 的结构和前文 SMPL-X 流程是同一种产物。这里不再重复展开，仍然可以理解为：

text 复制代码

root_pos + root_rot + dof_pos + fps

后续训练接口也不需要关心动作来自 SMPL-X 还是 PICO，只要它已经变成目标机器人的 motion pkl 即可。

2. 不能把 24 个关节全部无脑送进 IK

XRobot raw .npz 里有 24 个关节，但 IK 配置并不一定使用全部关节。

例如对 target_robot 来说，我们真正关心的通常是：

Pelvis / Spine3：身体主干；
Left_Hip / Right_Hip：髋部参考；
Left_Knee / Right_Knee：膝部参考；
Left_Foot / Right_Foot：脚部落点和方向参考；
Left_Shoulder / Right_Shoulder：肩部参考；
Left_Elbow / Right_Elbow：肘部参考；
Left_Wrist / Right_Wrist：手腕参考。

这些关节会被写入 xrobot_to_target_robot.json，并定义各自的权重、偏移和旋转修正。没有进入配置表的关节，不应该随便传入 GMR 的匹配任务。否则不仅没有意义，还可能因为缺少 scale/offset 定义导致错误。

因此，离线转换时的正确做法是：

text 复制代码

读取 xrobot_to_target_robot.json
  -> 提取实际参与 IK 的人体关节名
  -> 从 raw skeleton 每帧中取出这些关节
  -> 构造 human_data
  -> 调用 retargeter.retarget(human_data)

这也是我们为 XRobot raw 数据新增离线转换脚本的核心逻辑。

3. `xrobot_to_target_robot.json` 才是重定向效果的核心

最初机器人动作异常时，raw skeleton 已经很准确，但机器人姿态仍然明显不对。这说明问题不在 PICO，而在 IK 配置。

这里需要特别强调：不能简单把 smplx_to_target_robot.json 改成 XRobot 关节名就直接使用。SMPL-X、BVH、XRobot 虽然都描述人体，但它们的关节坐标系、旋转语义和骨架定义并不相同。

xrobot_to_target_robot.json 需要独立处理：

XRobot 关节到 target_robot MuJoCo body 的对应关系；
每个匹配点的位置权重；
每个匹配点的旋转权重；
人体点位到机器人 body 的位置偏移；
XRobot 坐标系到机器人目标坐标系的旋转偏移；
各关节在人体尺度和机器人尺度之间的缩放关系。

这份配置决定了"机器人应该用哪个 body 去追踪人体哪个关节"，也决定了"追位置更重要，还是追方向更重要"。

实际调试中，一旦坐标系和旋转偏移修正到位，机器人动作会从非常怪异变成比较自然。这也说明 GMR 的 IK 框架本身是可用的，关键是给它正确的人体目标和正确的匹配表。

4. 脚踝和脚掌是最容易踩坑的部分

PICO tracker 绑在脚踝附近，但这并不等于它能精确追踪机器人意义上的脚踝关节方向，更不等于能完全还原脚掌姿态。

PICO 数字人看起来脚很稳定，是因为 PICO 内部的人体模型会做约束和补全。但我们在 GMR 中拿到的是 skeleton 结果，再把它映射到机器人脚部 IK，如果过度相信局部旋转，就可能让机器人踝关节出现内扣、上翻、扭转等问题。

所以脚部目标需要谨慎设计：

对脚部位置，可以相对更信任；
对脚部旋转，要结合实际效果设置权重；
对机器人踝关节，可以根据需要做 clamp 或 fixed 后处理；
对脚掌偏移，要结合机器人 toe/ankle link 的实际位置调。

这个问题不是 PICO 独有。只要用稀疏 tracker 推断完整人体骨架，再重定向到机器人，脚部通常都是最敏感的位置之一。

5. 最终形成的数据闭环

最终得到的 robot motion pkl 和 SMPL-X 重定向得到的 pkl 在下游看来是同类数据。也就是说，训练系统不需要知道动作来自 AMASS、BVH，还是来自 PICO。只要它已经被转换成目标机器人的 root 和关节轨迹，就可以进入同一套训练流程。

完整闭环可以概括为：

text 复制代码

PICO 真人动作采集
  -> XRobot raw skeleton npz
  -> XRobot 到目标机器人的 GMR IK 配置
  -> robot motion pkl
  -> 训练格式转换
  -> 强化学习 / 模仿学习
  -> 机器人学会真人动作

这就是这条 pipeline 的意义：PICO 不只是一个实时交互设备，而是成为了我们生产机器人动作数据集的入口。相比只使用公开数据集，这让我们可以根据任务需求自己设计动作、自己录制动作，并把动作接入已有的 GMR 和训练框架。

四、总结：这条链路解决了什么

这次打通的核心，不是某一个脚本，而是把三件事连接了起来：

使用 PICO 和 XRoboToolkit 低成本采集真人动作；
将 PICO 输出的人体追踪结果保存为可复现的 XRobot raw skeleton；
基于 GMR 的通用 IK 框架，把这份 raw skeleton 重定向成指定机器人的动作。

和光学动捕相比，PICO 方案的绝对精度不一定占优，但它有部署简单、成本低、商业产品成熟、可快速反复采集的优势。对于机器人学习中的动作原型验证和示教数据生成，这些优势非常实际。

和普通 SMPL-X 重定向相比，PICO/XRobot 流程最大的变化在输入端：人体动作不再来自公开数据集，而是来自我们自己的真人表演。GMR 原有的机器人 IK 框架仍然发挥核心作用，只是新增了 xrobot 这一类人体数据源，以及面向目标机器人的独立 IK 配置。

最终，当机器人真的通过训练学会这份 PICO 录制的动作时，就说明这条链路已经完成闭环：

text 复制代码

真人动作 -> PICO 动捕 -> raw skeleton -> 机器人重定向 -> 训练 -> 机器人动作技能

这也是我认为它最有价值的地方：它把"我做一个动作"变成了"我可以生产一份机器人可学习的动作数据"。

后续，我们将继续探索有关实时遥操的工程实现，例如TWIST2等文章都有相关的工作。

从 PICO 动捕到机器人动作重定向

一、背景：为什么选择 PICO，以及 XRoboToolkit 提供了什么

1. 从光学动捕到消费级 XR 动捕

2. XRoboToolkit 是什么

3. 哪些是 GMR 已有能力，哪些是我们新增的工作

二、从 PICO 到 raw 动作数据：先把人体骨架录下来

1. 为什么 raw 数据是这条链路的核心

2. XRobot body skeleton 的数据结构

3. 坐标系转换：参考 GMR 中已有的 G1 样本

4. raw skeleton 文件保存什么

5. raw skeleton 的质量判断

三、从 raw skeleton 到机器人重定向

1. 接入 GMR 的方式：新增 src_human="xrobot"

2. 不能把 24 个关节全部无脑送进 IK

3. xrobot_to_target_robot.json 才是重定向效果的核心

4. 脚踝和脚掌是最容易踩坑的部分

5. 最终形成的数据闭环

四、总结：这条链路解决了什么

1. 接入 GMR 的方式：新增 `src_human="xrobot"`

3. `xrobot_to_target_robot.json` 才是重定向效果的核心