本页面主要介绍我在具身智能真机上的一些工程经验，内容相对偏工程。如果你不想看工程内容，仅读一句话概述即可。

一句话概述

我具有多种机械臂的搭建和操作经验，包括Franka、类双臂Aloha（松灵Piper和方舟无限）、智元精灵G1。我具有完整的机械臂平台基础设施搭建经历，包括基于SDK/ROS再封装控制接口、数据采集搭建、模型（远程服务器）测试接口（基于H200服务器http远程推理）、真机RECAP强化学习流程复现（$\pi_{0.6}^*$官方开源前复现成功），以及AnyGrasp传统机械臂控制方案部署（除相机标定外全部工作）。

SDK/ROS接口再封装

机械臂硬件提供的控制SDK/ROS往往兼容大量的接口，以方便用户能够完全控制机械臂的行为。例如，官方会同时提供机械臂状态（error code等）、MIT模式、位置控制模式等各种接口封装CAN总线数据，因此SDK中存在数十甚至上百个接口函数。

然而，这些接口在开发具身智能模型开发时大多数用不到。通常，软件和模型开发者期望的接口是类Gym接口，仅需提供reset、step等必须接口即可，这能大大帮助快速验证模型方案。因此，进行接口的再封装（例如封装到类Gym接口）是必要的。

我在Franka、松灵Piper、智元G1机械臂上进行过完整的接口开发。

Franka：由于Franka机械臂极易发生加速度故障（例如快速加速减速），我基于facebook Polymetis封装控制机械臂，并自行接入Realsense相机图像读取接口，完成封装
Piper：由于采购的是Cobot Magic双臂整机，官方提供完整的封装。后续我主要做了两方面的改进：1. 添加关键点功能（通过一个脚踏版触发一个按键，记录下任务中间关键点，例如子任务切换、错误恢复等）。2. 重新封装，支持Human-in-Loop推理（双向动作同步），详细内容将在中RECAP节介绍
Agibot：原始Agibot对底盘、升降机、关节、末端EE控制等做了大量的适配和封装，但需要使用以太网使用三方控制机进行调试开发。我主要实现了三方控制机（4090主机）上的Quest遥操作（未使用官方售卖版本和apk，独立使用Unity+Python实现）、数据采集pipeline（适配Quest遥操）及相关关键帧实时标注、模型远程推理（局域网远程H200服务器http推理）

更多详细信息将在下面介绍

数据采集

这里主要介绍我在智元机器人上的采集经验。（如果你希望获取apk或技术帮助，请邮件联系，视情况有偿或无偿提供）

智元官方VR遥操售价2w￥，远高于市场价（Pica/Quest单价仅3k-5k￥）。由于成本和预算审核要求，我用4天时间自行实现并测试了智元的VR遥操作（基于Quest），控制包收发频率稳定达到50Hz。我选择VR遥操的原因是，搭建gello或外骨骼方案需要购买大量电机，成本在1k+/台，随机械臂数量线性增长。而单个VR可以用来遥操大量的机械臂（随同时使用人数增长），对于学术团队更具有性价比。

Quest端

主要负责读取quest本身的按键、姿态信息，打包成数据包发送给控制主机（局域网UDP通讯）。出于敏捷开发考虑，Quest侧仅需实现网络连接和原始数据读取、发送逻辑即可，无需在Quest端实现复杂的转换逻辑。主要使用Unity中的Quest官方SDK开发（国内Unity版本不支持，需要使用国际版Unity），大约0.5-1天即可完成测试完毕。

Python端

python端使用socket + struct解出Quest数据包后，通过坐标转换进行相对位移、旋转控制。使用子线程收包、惰性解包、numpy、scipy，配合ROS，即可实现稳定50Hz的控制，因此无需特殊优化。

坐标转换

这是开发中最大的难点，由于Quest是一个左手系坐标，而智元G1的左右手不是平移不变的，因此开发比较复杂。智元SDK中提供的控制接口直接在底座ROS默认坐标系中进行控制（这点和Franka、Piper不一样）。

对于位置（xyz），仅需要左乘一个矩阵即可，通过持续打印收到的Quest数据包可以轻松获取Quest的坐标系，按照映射很容易构建出转换

对于旋转（四元数），使用scipy转换为旋转矩阵后，需要左乘一个矩阵，再右乘一个矩阵。具体来说，左乘矩阵和位置转换完全一样，右乘矩阵和SDK规约有关，没有很好的经验公式，推荐记录多组同姿态的Quest-机器人的四元数（例如单位四元数下另一方的四元数）算右乘矩阵。

相对位置

直接使用绝对坐标和四元数非常危险，一方面坐标数值很难线性映射（和头戴位置有关，每人每天都不一样），另一方面Quest很难摆出正前正右等姿态。因此，强烈推荐相对位移和相对旋转控制。

具体来说，将Quest的旋转矩阵转换到机械臂坐标系中，构造相对旋转结果（以下所有矩阵均在ROS坐标系中）

\[R_{ctrl} = R_{curr\_ctrl} \cdot R_{quest\_init}^{T} \cdot R_{robot\_init}\]

然后将$R_{ctrl}$转回四元数控制

其它Tips

同时触发双手复位时，强制关闭遥操，防止发生意外
操控腰部升降机或俯仰时，禁用机械臂控制，并且强烈建议完成后，触发刷新初始姿态防止发生意外
左手handtrigger（中指）：按下后切换遥操Enable/Disable，终端会有绿色/红色字体提示当前状态。每次Enable，会重置相对位移的起点，可在Disable下安全地调整手姿态保持舒适
右手handtrigger：触发一个断点，可用于标记任务关键点，便于后续统一标注，通过ros topic发布

远程http测试

由于采购经费和网络安全限制，我没有合适的推理卡为每台机械臂独立供应。为了更好地加速开发迭代，我参考$\pi_0$团队方案，实现了基于http的远程推理。在使用局域网的情况下，可以稳定保证20ms数据传输延迟（模型推理延迟另计）。

主要技术栈包括FastAPI、Uvicorn、Msgpack。仅供个人使用，不适配多人随机并发问题，不做量化和自动路由。

真机RECAP复现

$\pi_{0.6}^*$将RECAP算法应用到了VLA模型上，但它直到2026年5月尚未开源，因此我自行复现了算法。

使用松灵Cobot Magic四臂（遥操、运行各2臂）。但官方从硬件层面合并了主从臂的CAN信号，因此我对它做了硬件和ROS改进，适配双向同步。

新增两个CAN总线，将合并的信号重新分拆
重新ROS控制，将原本遥操时硬件信号同步改为软件函数同步
新增Human-in-Loop推理功能，通过踏板触发特定按键，切换模式。推理时主臂跟随从臂，介入时从臂跟随主臂

AnyGrasp部署

在Franka和Piper单臂上部署过Anygrasp模型，以类Yolo检测的形式进行抓取推理。

部署的核心难点是坐标标定和坐标变换，其中我负责坐标变换部分。具体来说，需要对旋转变换额外右乘一个$R_{adjust}$矩阵，将旋转从基座坐标系变到法兰盘坐标系（根据SDK的要求）