机械臂与LLM：关于具身智能Agent的一些尝试
前言
最近在做某个嵌入开发的外壳建模设计时，发现建模时需要测量各种零件和螺丝孔位等位置和尺寸的精确信息实在是麻烦，想着要是能有一个设备可以接入到 Agent 让 AI 借助外部工具完成这种繁琐的工作再结合目前的 blender mcp 进行自动化建模就好了。然后就发现了一个开源的机械臂项目（SO-101），其机械臂零件（除了舵机）都可以进行 3D 打印，而我手上正好有一台 3D 打印机（拓竹的 A1 mini），所以就想弄一个来验证自己的想法，毕竟具身智能是未来 AI 的下一个突破点（纯软件层面的 AI 模型可以说是趋近完美了，再加上目前如火如荼进行的 Harness 工程迭代，后续 AI 模型在软件工程方面取代人类已经可以预见；而具身智能则是 AI 介入物理世界的工具，打通具身智能的泛化应用则标志着 AI 在接管真实世界上具备了工程可行性）。
我的想法就是把机械臂接入到 Agent 中，由 Agent 来控制机械臂的运行，看看能不能借助 LLM 来实现具身智能的某种泛化能力（目前具身智能相关的专用模型——即世界模型的泛化能力有限，且我觉得收集数据进行模型训练是一个很枯燥的过程😂）。
SO-101
这是一个由 hugging face 开源的机械臂项目，所有部件都提供了官方的 3D 打印文件，然后也提供了完整的物料购买清单（包括淘宝和海外平台），可以说是把饭喂你嘴里了：
我就是照着官方给的淘宝链接买的各种物料，最后也是一遍组装通过（我个人算是嵌入开发的小白，完全没有相关背景知识）；不难发现，这套机械臂的大部分成本来自 STS3215 这款舵机，均价接近 100，不算便宜，但跟工业级机械臂相比肯定是便宜很多。当然，官网也提供了完整的组装和调试的教程——SO-101 · Hugging Face。
leader 臂与 follower 臂
follower 臂和 leader 臂结构上略有不同，它们可以成对使用，也可以单独使用；成对使用的场景大多是由人操作 follower 臂，然后将舵机姿态传递到 leader 臂上，即 leader 臂负责复现 follower 臂的姿态，最常见的用途就是使用 follower 臂来操控 leader 臂完成各种动作进行数据收集，然后进行模型训练或微调。
而作为单个机械臂进行使用时，则可以通过舵机控制板来控制各个机械臂舵机的角度进而完成各种动作。
我就是只使用了 leader 臂，因为我不需要收集数据进行模型训练，而是直接使用 Agent 基于运动学控制机械臂的动作。
3D 打印
如果你手里正好有拓竹的打印机，建议使用 maker world 社区的模型进行打印（值得注意的是里面 follower 臂和 leader 臂的特有零件盘貌似标反了，而且有盘零件包含一些官方测试零件，就是数字 0 和 1 的那种）：
我是使用 PETG 耗材进行打印的，各个部件和舵机之间卡得刚刚好，不确定使用 PLA 打印的零件收缩率如何。
组装
虽说官网就有组装视频，但是我觉得下面这个 B 站上的组装视频更加详细：
组装完就是这样；需要注意的是每个部件靠近舵机的地方都有一个豁口，用于收纳舵机的连线，建议组装后都把舵机连线整理到豁口里面。
舵机校准
校准也是同理，可以参考下面的 B 站视频：
机械臂控制
同样地，SO101 机械臂可以使用 lerobot 提供的 api 进行控制（lerobot.motors.feetech 模块，因为 STS3215 舵机的厂商就是 feetech），但是这种控制是基于 USB 直连舵机控制板进行串口通信的。
无线通信控制
如果要使用无线通信的方式来控制机械臂，则可以给舵机控制板连上一个 esp32 开发板（这里我使用的开发板是 esp32ch340c，其实如果只是单纯为了无线通信转发完全可以使用 esp32c3 SuperMini 这款超迷你的板子），然后 PC 通过 BLE（或 WiFi）与 esp32 开发板进行通信，esp32 开发板则通过 UART 协议与舵机控制进行通信，大致通信过程如下图所示：
不过需要注意的是，连接 esp32 开发板到舵机控制板的 RX/TX 的杜邦线很容易插反，如果发现 PC 端发送数据正常但是舵机一点反应都没有最好试试交换一下 RX/TX 的线🙈。跟 lerobot.motors.feetech 模块类似，使用 BLE 进行通信时，也需要遵循 Feetech STS3215 的自定义串口协议，协议具体内容如下：
应用层：Feetech STS3215 串口协议
UART 上跑的是 Feetech Protocol 0（与 Dynamixel v1 类似），波特率 1 000 000 bps，8N1。SO-101 从臂 6 个舵机 ID 与模型号如下（与 LeRobot 配置一致）：
| 关节 | 舵机 ID | 型号 | Model Number |
|---|---|---|---|
| shoulder_pan | 1 | sts3215 | 777 |
| shoulder_lift | 2 | sts3215 | 777 |
| elbow_flex | 3 | sts3215 | 777 |
| wrist_flex | 4 | sts3215 | 777 |
| wrist_roll | 5 | sts3215 | 777 |
| gripper | 6 | sts3215 | 777 |
1. 指令帧格式（Protocol 0）
主机（PC/ESP32 透传后表现为 PC）发往舵机的写指令帧：
┌──────┬──────┬────┬────────┬─────────────┬──────────┬──────────┐
│ 0xFF │ 0xFF │ ID │ Length │ Instruction │ Param... │ Checksum │
└──────┴──────┴────┴────────┴─────────────┴──────────┴──────────┘
| 字段 | 说明 |
|---|---|
0xFF 0xFF |
帧头（固定） |
ID |
舵机 ID（1–253），0xFE 为广播 ID |
Length |
后续字节数 = len(Instruction + Params) + 1（含校验和） |
Instruction |
指令码，如 0x01 PING、0x02 READ、0x03 WRITE |
Param... |
寄存器地址、长度、数据等 |
Checksum |
~(ID + Length + Instruction + Params...) & 0xFF |
舵机响应帧结构类似，第 5 字节为 Error 而非 Instruction，其后跟返回数据。
2. 常用指令码
| 指令 | 值 | 用途 |
|---|---|---|
| PING | 0x01 |
探测舵机是否在线，返回 Model Number |
| READ | 0x02 |
读寄存器（如 Present_Position） |
| WRITE | 0x03 |
写寄存器（如 Goal_Position） |
| REG_WRITE | 0x04 |
延迟写入，配合 ACTION |
| ACTION | 0x05 |
触发 REG_WRITE |
| SYNC_READ | 0x82 |
同步读多个舵机 |
| SYNC_WRITE | 0x83 |
同步写多个舵机 |
LeRobot 的 FeetechMotorsBus.sync_read("Present_Position") / sync_write("Goal_Position", ...) 在底层会组好上述包，经 BlePortHandler.writePort() 发出。
3. 示例：单舵机 PING（ID=1）
主机发送（6 字节，经 BLE → UART 原样透传）：
FF FF 01 02 01 FB
| 字节 | 含义 |
|---|---|
FF FF |
帧头 |
01 |
舵机 ID |
02 |
Length |
01 |
Instruction: PING |
FB |
Checksum |
舵机响应（7 字节，经 UART → BLE 原样透传）：
FF FF 01 02 00 09 F0
| 字节 | 含义 |
|---|---|
FF FF |
帧头 |
01 |
舵机 ID |
02 |
Length |
00 |
Error |
09 |
Model Number 低字节（777 = 0x0309） |
F0 |
Checksum |
连接握手时，LeRobot 会对 ID 1–6 逐个 PING，并校验 Model Number 是否为 777。
4. 示例：广播 PING（扫描总线上所有舵机）
发送: FF FF FE 02 01 FE
ID=0xFE (广播)
总线上每个在线舵机会依次回复，固件会把多段 UART 数据连续 Notify 回 PC，由 scservo_sdk 在接收缓冲中解析。
5. 一次完整「读位置」的透传路径
以 sync_read("Present_Position") 为例：
FeetechMotorsBus组 SYNC_READ 包（约 20–40 字节）BlePortHandler.writePort()→ bleak Write(NUS_RX)- ESP32 onWrite → Serial2 → 舵机板
- 各舵机响应 → Serial2 RX → ESP32 bleBridgePoll → Notify(NUS_TX)
BlePortHandler收到 Notify → 填入 RX 环形缓冲- scservo_sdk
readPort()取字节 → 解析各 ID 的 Present_Position RealBackend将角度转为弧度返回 viewer
整条链路上 字节内容 与 USB 串口直连时一致；差异仅在延迟与超时（BLE 通常增加 50–200 ms 往返）。
如图所示，直接在舵机控制板上连接一个 esp32 开发板即可实现无线通信
Xbox 手柄控制
由于我没有使用配套的 leader 臂，所以如果想要手动调整机械臂的姿态（开启舵机扭矩的情况下）是很困难的，不过可以实际上也可以用键盘或者 Xbox 这类游戏手柄来发送控制命令达到某种程度的姿态控制。由于我手里正好有 Xbox 手柄，所以我选择用手柄进行控制。
一开始我是想要 pygame 的 gamepad 模块进行对接实现，后面发现效果并不好，而且 Windows 平台自带 Xinput 库（dll 文件），直接使用 Xinput 来识别手柄操作非常方便。我给 Xbox 手柄的按键简单设计了一套机械臂的交互，主要还是操控机械臂的末端，然后基于 IK 求解进行运动：
仿真环境
尽管我们可以通过直连舵机控制板或者无线通信来直接控制真实的机械臂，但有时候在快速验证算法和流程时仿真环境比较方便；这里我把仿真环境和真实机械臂当做不同的执行后端，使用完全相同的 interface，因此可以快速切换执行目标。
我这里使用了 MuJoCo（python 包）这个仿真框架，仿真渲染需要使用的各部件 3D 模型文件可以直接在 SO101官方github项目 中找到，主要文件如下：
然后基于这些模型和官方 github 中提供的 URDF定义文件 生成一份简洁的 MJCF（MuJoCo xml）文件：
1 |
|
基于这份 MJCF 文件运行得到的仿真效果如下：
其中 GUI 面板左右两侧的功能都是 MuJoCo viewer 自带的，可以自行查看或设置各关节的姿态数据；不过后面我发现 SO101 官方 github 项目中本身就有一个机械臂的 MJCF 定义文件：
对比上面 AI 写的 MJCF 文件，两者的区别如下：
不难看出 AI 写的 MJCF 文件在物理参数方面算是比较简化的，但由于我使用仿真环境的目的并不是进行严格的物理交互，而是实时反映机械臂关节状态以及在虚拟环境中模拟机械臂姿态，因此这套简化配置也完全够用了；如果你需要进行严格的物理仿真交互那么建议还是参考官方的 MJCF 文件中的参数设置。
Agent 搭建
有了机械臂（包括仿真环境）的硬件能力，接下来要做的就是把这些硬件能力接入到 Agent 中，让其可以直接控制机械臂。
机械臂 mcp 工具
由于真实机械臂和仿真环境中的机械臂是共用相同 interface 的不同执行后端，所以在 mcp 服务中 Agent 不需要过多关心具体执行后端的差异，使用上层的同一套工具即可：
动画
为了让 Agent 可以自行设计类似游戏中的骨骼动画，我让 AI 设计了一套简易的基于关键帧的机械臂动画执行引擎，也提供了相应的动画工具：
同时在前端对话工具显示 UI 中我加上了 replay 机制，方便进行动作的回放用于测试：
在我的规划中，实际上可以把 Agent 设计得不错的动画加入到一个专门的动画库中，后续可以基于向量查询匹配合适的动画快速复用。
状态机
为了让 Agent 能够设计更复杂的机械臂操作逻辑，我让 AI 设计了一套简洁够用的有限状态机（FSM）系统，跟上面的动画类似，Agent 可以利用工具提交和控制状态机；简单来说就是把节点分成几个类型：
trigger：触发器，用于触发状态机的开始action：执行机械臂动作logic：纯逻辑计算，可以获取计算结果branch：决定分支走向
此外，为了支持后续更复杂的逻辑，在底层设计了状态机可以嵌套，即状态机可以作为一个节点在其它状态机中使用，且状态机本身可以循环执行；具体的实现细节如下：
SO-101 状态机 (FSM) 模块架构文档
概述
FSM 模块位于 custom/SO101/sim/fsm/，为 SO-101 机械臂提供了一套声明式的有限状态机引擎。LLM Agent 通过 MCP 工具 submit_state_machine 提交 JSON 配置即可编排复杂的机械臂动作序列，无需编写代码。
设计目标
- 声明式编排：通过 JSON 描述节点图，降低 LLM 生成可执行方案的门槛
- 实时反馈：执行过程中通过 WebSocket 向前端推送生命周期事件，可视化状态流转
- 可扩展：新增动作/触发器/逻辑只需注册 handler 函数
- 安全约束：IK 预检 + 关节限位 + 超时保护，防止不可达目标导致阻塞
模块结构
custom/SO101/sim/fsm/
├── __init__.py # 包声明
├── schema.py # Pydantic 数据模型：节点类型定义 & FSM 配置验证
├── engine.py # 异步执行引擎：FSMRunner
├── nodes.py # 内置 handler 实现（trigger / action / logic）
└── ik.py # 正/逆运动学求解器（纯 numpy 实现）
架构全景
┌─────────────────────────────────────────────────────┐
│ LLM Agent (Claude / GPT) │
│ ↕ MCP tool: submit_state_machine(config) │
├─────────────────────────────────────────────────────┤
│ mcp_server.py │
│ · 验证 JSON → FSMConfig (schema.py) │
│ · 创建 FSMRunner 并 asyncio.create_task(run()) │
├─────────────────────────────────────────────────────┤
│ engine.py — FSMRunner │
│ · 管理当前节点、循环计数、共享 context │
│ · 解析模板 {{path}} → 动态参数 │
│ · 调度 handler → 获取 outcome → 路由 transition │
│ · 通过 on_event 回调广播生命周期事件 │
├─────────────────────────────────────────────────────┤
│ nodes.py — Handler Registry │
│ · TRIGGER_HANDLERS: wait_start, focus │
│ · ACTION_HANDLERS: move_to_position, gripper … │
│ · LOGIC_HANDLERS: random_int, get_object_pos … │
├─────────────────────────────────────────────────────┤
│ ik.py — 运动学层 │
│ · forward_kinematics_rad() — 正运动学 │
│ · solve_ik() — 阻尼最小二乘 IK + 随机重启 │
│ · _workspace_precheck() — 可达性快速预检 │
├─────────────────────────────────────────────────────┤
│ RobotBackend (sim / real) │
│ · get_joint_positions / set_joint_targets │
│ AnimationPlayer │
│ · 关键帧插值、smoothstep 缓动 │
└─────────────────────────────────────────────────────┘
核心组件详解
1. schema.py — 数据模型
使用 Pydantic v2 定义 JSON 配置的类型系统，由 FSMConfig.model_validate() 在提交时一次性校验。
节点类型
| 类型 | 类 | 职责 | 允许的 transitions |
|---|---|---|---|
trigger |
TriggerNode |
等待环境条件满足后触发 | satisfied, timeout |
action |
ActionNode |
执行机械臂动作 | success, failure, timeout |
logic |
LogicNode |
计算/检测，结果写入 context | success, failure, timeout |
branch |
BranchNode |
读取 context 值并条件路由 | 由 cases 定义 |
终结符
$done— 状态机成功完成$fail— 状态机失败终止
FSMConfig 顶层字段
class FSMConfig(BaseModel):
id: str # 唯一标识
name: str # 显示名称
loop: bool = False # 是否在 $done 后重新开始
max_loop_count: int | None = None # 最大循环次数（None=无限）
initial: str # 起始节点 ID
context: dict[str, Any] = {} # 初始共享上下文
nodes: dict[str, FSMNode] # 节点图
图完整性验证
_validate_graph 模型验证器确保：
initial指向一个存在的节点- 所有 transition 目标要么是已定义的节点 ID，要么是终结符
2. engine.py — FSMRunner
异步执行引擎，核心循环：
start → enter_node → execute_handler → get_outcome
→ resolve_transition → (terminal? / next_node / loop)
关键特性
模板解析 — 参数中的 {{context.path}} 会在执行前被替换为 context 中的实际值：
{
"action": { "name": "move_to_position", "params": {
"x": "{{object_pos.x}}",
"y": "{{object_pos.y}}",
"z": "{{object_pos.z}}"
}}
}
支持：
- 完整替换（保留原始类型，如数值）
- 部分替换（字符串内嵌入）
- 嵌套 dict/list 递归解析
- 点号路径（如
result.angle_rad）
Branch 节点评估 — 支持运算符：\==, !=, <, >, <=, >=, in, between
超时保护 — 每个节点可配 timeout_ms，超时返回 "timeout" outcome
循环控制 — 当 loop=true 且到达 $done 时，重置 current_node 为 initial，直到达到 max_loop_count
生命周期事件
FSMRunner 通过 on_event 回调推送以下事件（经 WebSocket 到达前端）：
| 事件 | 时机 | 数据 |
|---|---|---|
fsm_started |
状态机开始 | config, current_node |
fsm_node_enter |
进入节点 | node_id, node_type, label |
fsm_node_exit |
离开节点 | node_id, outcome, result |
fsm_loop |
开始新一轮循环 | loop_count |
fsm_completed |
状态机结束 | status (done/fail/stopped/error), loop_count |
3. nodes.py — Handler 注册表
所有 handler 共享统一签名：
async def handler(
params: dict, # 节点配置中的参数（已模板解析）
context: dict, # 共享上下文（可读取）
backend: RobotBackend, # 机器人硬件/仿真后端
animation_player: AnimationPlayer, # 动画播放器
) -> tuple[str, Any] # (outcome, result_value)
已注册的 Trigger Handlers
| 名称 | 参数 | 行为 |
|---|---|---|
wait_start |
interval_s (默认 1.0) |
等待指定秒数后触发 satisfied |
focus |
target, threshold (0.4), poll_interval_s (0.3), text_queries |
轮询检测引擎直到目标物体出现。返回坐标信息 |
已注册的 Action Handlers
| 名称 | 参数 | 行为 |
|---|---|---|
move_to_position |
x, y, z (mm), duration_s (0.5), speed (1.0) |
IK 求解 → 关键帧动画驱动末端到目标位置 |
open_gripper |
— | 夹爪全开 (1.745 rad) |
close_gripper |
— | 夹爪全闭 (-0.175 rad) |
rock |
— | 石头手势（全闭） |
scissors |
— | 剪刀手势（半开） |
paper |
— | 布手势（全开） |
wait |
duration_ms (1000) |
等待指定毫秒 |
已注册的 Logic Handlers
| 名称 | 参数 | 返回值 | 行为 |
|---|---|---|---|
get_gripper_angle |
— | {angle_rad} |
读取当前夹爪角度 |
random_int |
min, max |
{value} |
生成 [min, max] 随机整数 |
get_object_position |
target, threshold, timeout_s, poll_interval_s |
{x, y, z, frame, ...} |
检测目标物体并返回 3D 坐标 |
4. ik.py — 运动学求解
正运动学 (FK)
forward_kinematics_rad(q_rad) — 输入 5 关节弧度值，返回 4×4 齐次变换矩阵（末端位姿）。
基于 SO-101 URDF 中提取的关节坐标系参数，逐级叠加旋转变换。
逆运动学 (IK)
solve_ik(current_joints_rad, target_xyz_mm) — 阻尼最小二乘法（DLS）+ 随机重启策略：
- 工作空间预检 — 快速排除明显不可达目标：
- 总距离 > 560mm
- 水平距离 > 500mm
- 过近（< 100mm 且高度 < 250mm）
- 从当前构型出发求解 — 优先保持连续性
- 随机重启 (默认 8 次) — 从关节限位内随机初始化，避免局部极小
- FK 验证 — 解算后用 FK 反算验证误差 < 10mm
关节限位
shoulder_pan: [-110°, +110°]
shoulder_lift: [-100°, +100°]
elbow_flex: [-97°, +97°]
wrist_flex: [-95°, +95°]
wrist_roll: [-157°, +163°]
配置示例
简单拾取动作
{
"id": "pick_cup",
"name": "拾取杯子",
"loop": false,
"initial": "detect",
"context": {},
"nodes": {
"detect": {
"type": "logic",
"label": "检测杯子位置",
"logic": { "name": "get_object_position", "params": { "target": "cup" } },
"output_key": "cup_pos",
"transitions": { "success": "move_above", "failure": "$fail" }
},
"move_above": {
"type": "action",
"label": "移动到杯子上方",
"action": { "name": "move_to_position", "params": {
"x": "{{cup_pos.x}}",
"y": "{{cup_pos.y}}",
"z": 200
}},
"transitions": { "success": "open", "failure": "$fail" }
},
"open": {
"type": "action",
"label": "张开夹爪",
"action": { "name": "open_gripper", "params": {} },
"transitions": { "success": "descend", "failure": "$fail" }
},
"descend": {
"type": "action",
"label": "下降到抓取高度",
"action": { "name": "move_to_position", "params": {
"x": "{{cup_pos.x}}",
"y": "{{cup_pos.y}}",
"z": "{{cup_pos.z}}"
}},
"transitions": { "success": "grasp", "failure": "$fail" }
},
"grasp": {
"type": "action",
"label": "夹取",
"action": { "name": "close_gripper", "params": {} },
"transitions": { "success": "lift", "failure": "$fail" }
},
"lift": {
"type": "action",
"label": "抬起",
"action": { "name": "move_to_position", "params": {
"x": "{{cup_pos.x}}",
"y": "{{cup_pos.y}}",
"z": 250
}},
"transitions": { "success": "$done", "failure": "$fail" }
}
}
}
石头剪刀布循环
{
"id": "rps_game",
"name": "石头剪刀布",
"loop": true,
"max_loop_count": 5,
"initial": "wait",
"context": {},
"nodes": {
"wait": {
"type": "trigger",
"label": "等待开始",
"trigger": { "name": "wait_start", "params": { "interval_s": 2.0 } },
"transitions": { "satisfied": "random" }
},
"random": {
"type": "logic",
"label": "随机选择",
"logic": { "name": "random_int", "params": { "min": 1, "max": 3 } },
"output_key": "choice",
"transitions": { "success": "decide", "failure": "$fail" }
},
"decide": {
"type": "branch",
"label": "分支",
"branch": {
"source": "choice.value",
"operator": "==",
"cases": { "rock": 1, "scissors": 2, "paper": 3 },
"default": "rock"
},
"transitions": { "rock": "do_rock", "scissors": "do_scissors", "paper": "do_paper" }
},
"do_rock": {
"type": "action",
"label": "石头",
"action": { "name": "rock", "params": {} },
"transitions": { "success": "$done" }
},
"do_scissors": {
"type": "action",
"label": "剪刀",
"action": { "name": "scissors", "params": {} },
"transitions": { "success": "$done" }
},
"do_paper": {
"type": "action",
"label": "布",
"action": { "name": "paper", "params": {} },
"transitions": { "success": "$done" }
}
}
}
扩展指南
添加新的 Action Handler
- 在
nodes.py中实现异步函数，遵循统一签名：
async def _action_my_action(
params: dict,
context: dict,
backend: RobotBackend,
animation_player: AnimationPlayer,
) -> tuple[str, Any]:
# 实现逻辑...
return ("success", None) # 或 ("failure", {"error": "..."})
- 注册到对应的 handler 字典：
ACTION_HANDLERS["my_action"] = _action_my_action
添加新的 Trigger/Logic Handler
同上，分别注册到 TRIGGER_HANDLERS / LOGIC_HANDLERS。
Trigger 返回 ("satisfied", data) 或由超时触发 "timeout"。
Logic 返回 ("success", result_dict)，result 会被写入 context[output_key]。
数据流
Agent 提交 JSON config
│
▼
FSMConfig.model_validate() ← 类型 + 图完整性校验
│
▼
FSMRunner.run() — asyncio Task
│
├─→ _execute_node()
│ ├── _resolve_params() ← {{模板}} → context 值
│ ├── handler() ← 实际执行
│ └── timeout 保护
│
├─→ LogicNode: context[output_key] = result
│
├─→ BranchNode: _evaluate_branch() → outcome
│
└─→ transitions[outcome] → next_node | $done | $fail
│
▼
on_event() → WebSocket → 前端可视化
安全与容错
- IK 预检：
_workspace_precheck()在求解前快速排除不可达目标 - 超时机制：每个节点可配
timeout_ms，防止 handler 无限阻塞 - 异常捕获：handler 内部异常统一映射为
"failure"outcome - 单实例约束：同一时间仅允许一个 FSMRunner 运行，新提交自动停止旧实例
- 关节限位：IK 求解过程中持续 clip 到关节限位范围内
- 外部停止：
stop()方法设置标志位，主循环在下一次迭代退出
在前端交互上，跟动画类似，状态机也支持 replay 方便调试，且同时渲染状态机并高亮当前步骤。
跟动画库类似，我之前也设想过类似的状态机库用于快速复用已有的状态机。
世界（环境）感知
为了让 Agent 能够感知到机械臂所在真实物理世界空间，主要是识别到空间中的物体语义及该物体的坐标（机械臂基座坐标系），为此我设想基于 RGBD 相机 + 零样本对象检测模型进行环境的感知。
零样本对象检测（Zero-Shot Object Detection）模型
顾名思义，这种模型不需要给出预先的标签样本，直接输入图像和标签文本即可检测当前图像中是否存在标签文本所表示的物体，具体定义可以查看下面 hugging face 的说明：
这里我选择的模型是 grounding-dino-tiny，无他，因为这款模型的参数量很小（0.2B）且下载量名列前茅，我的 PC 显卡可以带得动（只需要几 G 的显存）；
RGBD 相机与手眼标定
RGBD 相机就是可以获取画面中每个像素点的深度值的相机，即集成了深度传感器的相机；这里我买了一款很便宜的 RGBD 相机——Astra pro（全新的才 100 多），规格参数如下：
从价格和规格参数不难看出，这个 RGBD 的精度比较一般，主要就是用来测试想法的可行性。我使用 RGBD 相机主要就是想把相机空间中的物体转换到机械臂基座空间中，然后基于相机空间的深度值计算物体到夹爪的大致距离（矢量），便于夹爪运动到目标物体位置。
手眼标定
想要把相机空间坐标转换为机械臂基座空间坐标，手眼标定就是不可避免的的步骤；手眼标定有两种情况：
- 眼在手外：即相机不在机械臂上
- 眼在手上：相机固定在机械臂末端上（相机与末端关节的相对位置保持不变）
具体定义和标定算法可以参考下面这篇文章：
而我使用的就是眼在手外的标定，因为使用的 RGBD 相机不算轻便，且 SO101 机械臂的末端负载能力有限，实测末端固定一个 iPad mini（300g 左右）的重量都会在某些姿态中导致舵机负载过重；
这是我自行设计的 iPadmini 末端关节固定装置，其可以卡住机械臂末端关节基座部分，然后把 iPadmini 装上去
装上 iPadmini 后就可以显示棋盘图片，然后调整机械臂姿态就可以收集数据进行标定
然而我试过很多次标定，也无法得到一个误差较小的标定结果，基本上误差都超过 200mm 了，这种结果完全无法正常进行坐标系转换：
1 | 2026-08-02 17:54:37,403 [INFO] custom.SO101.calibration.capture_store: 从 samples.json 加载 15 组采样 (含角点) |
误差来源
后面仔细捋了一下，毕竟从机械臂打印和组装开始就充满了各种误差，然后叠加后续标定流程中的各种误差就更加显著了：
- 零件尺寸偏差：3D 打印材料存在收缩率
- 零件组装：零件与舵机组装时的相对位置偏差
- 舵机中位校准：中位姿态不准（纯手动摆的位置，基本上就是一个大概的中位姿态）
- 末端关节与标定板相对位置偏移，标定过程标定板位置发生移动（因为没有完全固定住）
- 相机参数：内外参，相机精度
- 棋盘格子尺寸测量精度误差：因为基于 iPadmini 显示的棋盘图片，其格子尺寸是未知的，一般尺子测量误差不是很低
标定误差改进的可能性
- 使用小巧的高精度近距深度相机（Realsense D405/Astro gemini 305）进行眼在手上标定
- 自行设计固定相机基座到相机位置的零件，使得其相对距离在一个固定值（误差范围很小）
VLA 模型与 WAM
因为上面对于环境感知的思路实际上是我凭直觉拍脑袋想出来😂，而实际在具身智能业界，目前用于感知环境而输出动作的模型主要是 VLA 和 WAM：
- VLA（Vision Language Action）：顾名思义，基于 Vision（相机画面）+Language（自然语言指令）作为输入，得到输出——Action，动作执行。
- World Model：世界模型，通常模型内部对于外部真实世界有一个建模（状态表示），可以通过当前 Vision 和动作预测动作在世界中的具体影响（未来状态的预测）。
- WAM（World Action Model）：VLA 和世界模型的结合，通过预测不同动作达到的未来状态来选择符合目标状态的动作。
不过世界模型目前存在很多技术路线，也正在快速迭代发展之中，想要了解目前的技术脉络可以看一下这个 B 站视频：
后话：具身智能的小脑
尽管这次对于机械臂与 Agent 结合的简单尝试在手眼标定这一步惨烈失败了🙈，但也了解到很多具身智能相关的知识和现状，于是我发现对于具身智能来说可能最需要解决的问题就是“小脑”——一个泛化能力很强，且具备高精度低延迟动作输出的模型。
仔细想想人类在做出动作时，大脑压根不会想这个动作要控制哪些关节，这些关节的姿态（旋转角度）是什么。不过对于机器人，最终要控制动作确实需要输出这些关节的姿态数据，而且要想跟人类表现一样，这些输出结果还要非常的低延迟。
至于扮演大脑的复杂决策功能，实际上目前 LLM 就可以做到，不过后续也可能直接把 LLM 的能力融合进这个充当小脑的动作模型中形成一个统一的模型。
