📂 项目地址
GitHub：Xbotics-Embodied-AI-club/Xbotics-Embodied-AI-Handbook/tree/main
第 6 讲已经完成手眼标定，并通过坐标变换得到目标物体在机器人基座坐标系中的位姿。机器人已经能够回答“物体在哪里”，接下来似乎只要让机械臂去抓就可以了。
但真正执行时，问题才刚刚开始。机械臂按计划到达 A 区，夹爪闭合、抬升、移动，最后在 B 区打开。控制台没有报错，整段轨迹也执行完了。可回头一看，方块还留在 A 区——夹爪从一开始就是空的。
这是机器人操作里很典型的一类失败：动作执行完了，任务却没有完成。 本讲就在第 6 讲的位姿结果上继续向前：把物体位姿转换成夹爪动作，决定怎样接近、什么时候闭合、如何确认物体已经离桌，以及失败后该改什么。
我们用两个难度递增的任务贯穿全讲：先把红色方块从 A 区搬到 B 区，跑通最小闭环；再从侧面抓起竖直瓶子并放入收纳盒，处理滑落、盒沿碰撞和姿态约束。
1 坐标有了，动作还没有
物体位姿是一个空间坐标，抓取却是一段与环境发生接触的过程。把方块中心直接发送给机械臂，夹爪可能从错误方向撞上去，也可能在尚未对准时闭合。即使末端顺利到位，程序仍不知道两指之间有没有物体。
一套可用的操作流程至少要做四件事：生成一组有安全间隔的动作位姿；按顺序执行；在抓取、抬升和放置后检查物理结果；失败时停止原流程并选择恢复动作。
1.1 方块是基础关，瓶子是升级关
方块形状规则、重心稳定，适合从正上方抓取。瓶子沿用同一套流程，却把接近方向改成水平侧向，并把放置目标从平面区域换成有边界的收纳盒。程序骨架可以不变，但抓取姿态、夹爪宽度、放置高度和成功条件必须随任务变化。
1.2 为什么要把任务拆成几个状态
如果把抓取、抬升和搬运命令一次性发完，方块没有夹住，机械臂也会照样前往 B 区。状态机在动作之间插入检查点：预抓取状态只判断是否到位，夹爪状态只负责闭合，抬升验证状态再判断物体有没有随夹爪离桌。检查不通过，后续搬运便不会发生。
动作命令负责“做什么”，检测条件负责“做成没有”，状态机负责“下一步去哪”。
2 从物体位姿到夹爪该去哪里
先看一个最直接的问题：第 6 讲输出的是物体位姿，但机械臂控制的是夹爪位姿。两者不是同一个东西。
物体位姿把物体的参考点放进机器人基座坐标系；夹爪位姿则规定末端执行器的位置和朝向。方块中心可能在实体内部，瓶子参考点也不一定落在适合夹持的瓶身位置。因此，中间必须加一层“抓取关系”：夹爪相对于物体应该放在哪里、朝向哪里、张开多大。
2.1 先认识输入：物体位姿说了什么
用 object_pose_base 表示目标物体在机器人基座坐标系中的位姿。它由三维位置和四元数姿态组成：
这组数值只告诉我们物体在哪里。夹爪真正要执行的抓取，还需要确定抓取点、接近方向、夹爪朝向和开口宽度。
2.2 用一个抓取候选描述“怎么夹”
抓取候选可以理解为一副虚拟夹爪：把它摆到物体附近，就能直观看出真实夹爪应该以什么位置和方向靠近。
参考 GraspNet 的六维抓取格式，本讲把一次抓取写成以下几个量：
图中的 U 形框只是抓取姿态的可视化。真正执行时，必须让 SO-101 的真实夹爪与它重合，再由真实夹爪接触物体。
2.3 方块竖直抓，瓶子水平抓
方块任务采用顶部抓取。夹爪位于方块正上方，接近方向竖直向下，两指夹住方块相对的两个侧面。抓取点优先选在几何中心附近，因为这里离重心近，抬升时不容易旋转。
瓶子保持竖直，夹爪从侧面水平接近。两指夹住瓶身中部，既不能太低——否则下侧指尖容易碰桌面；也不能太高——否则可能夹到瓶肩或瓶盖，接触不稳定。
2.4 五个位姿，五个不同的安全问题
一次抓取—放置任务使用五个关键位姿：
预抓取位姿：先停在物体附近，但不接触； 抓取位姿：真实夹爪包围物体并准备闭合； 抬升位姿：夹住后先离开桌面，验证物体是否跟随； 放置位姿：物体已经得到桌面或容器支撑，但夹爪还没有松开； 退出位姿：夹爪打开后离开目标区域。
预抓取位姿：给接近动作留出一段安全距离
先定义三个量： 是抓取位置， 是从预抓取位置指向抓取位置的单位接近向量， 是预抓取距离。预抓取位置 为：
方块的 竖直向下，所以预抓取位姿在方块正上方；瓶子的 水平指向瓶身，所以预抓取位姿在瓶子侧面。
抬升位姿：先证明“抓住了”，再开始搬运
先定义 为抬升高度， 为基座坐标系中竖直向上的单位向量。抬升位置为：
方块和瓶子虽然抓取方向不同，但抬升都沿竖直方向进行。原因很简单：先把物体从桌面和周围障碍物中“拔出来”，再做大范围水平搬运，碰撞风险更低。
放置位姿：到位时先别松手
放置位姿不是“夹爪已经离开”的位置。此时方块应当已经落在 B 区桌面上，瓶子应当已经进入收纳盒并获得底部支撑，但夹爪仍然保持闭合。确认物体受到支撑后，夹爪才打开，然后移动到退出位姿。
2.5 先做几何初筛，再谈机械臂能不能到
抓取候选生成以后，先排除一眼就不合理的方案。这里不求逆运动学，也不规划完整轨迹，只检查物体、夹爪和场景之间的几何关系。
首先看夹爪宽度。定义 为物体在夹持方向上的宽度， 和 分别为夹爪能够稳定夹持的最小、最大宽度。候选至少应满足：
其次看接触位置。方块不能只夹到边角，瓶子应避开瓶盖、瓶肩和容易变形的位置。最后检查桌面和目标区域的几何间隙。这里考虑桌面高度，不是因为机械臂“放在桌子上”，而是因为夹爪指尖、腕部和被抓物体在接近时都可能碰到桌面。瓶子入盒时，还要确认夹爪能够通过盒口，并且释放后有退出空间。
几何初筛通过，只说明这个抓取“看起来合理”。机械臂是否真的够得到、关节有没有解、整段路径会不会撞，还要交给下一节。
3 五个位姿怎么串成一次不会乱跑的任务
五个位姿只是五个目标。如果程序把它们依次塞给机械臂，却不检查中间结果，任务仍然很危险。
状态机解决的正是这个问题。它把连续动作拆成一个个状态，每个状态只做一件事，并明确写出四样东西：输入、执行动作、完成条件、失败条件。
状态机不是动作播放列表。它是“做一步、看结果、再决定下一步”的执行规则。
3.1 主流程：正常情况下机器人怎么走
主流程可以压缩成五个阶段：
任务初始化：读取物体位姿和放置目标，生成动作位姿； 抓取执行：到预抓取位姿，低速接近，闭合夹爪； 抓取验证：抬升并确认物体随夹爪离开桌面； 放置执行：搬运、下降、释放； 结果记录：检测放置结果，退出并保存日志。
任何阶段失败，都进入同一个失败处理入口。失败处理先记录“在哪个状态失败、看到了什么证据”，再决定是重新感知、重新生成位姿、调整参数，还是安全退出。
3.2 到预抓取位姿：大动作先在安全区完成
预抓取位姿允许机械臂从当前姿态进行较大范围运动。此时夹爪还没有进入物体附近，因此可以使用正常速度的轨迹控制或运动规划。
到位判断不能只看“命令发出去了”。先定义位置误差 、姿态误差 ，以及各自允许上限 、 。只有同时满足
状态机才进入低速接近。否则继续等待控制器收敛；超过超时时间，则记录“预抓取到位失败”。
3.3 低速接近：越靠近物体，动作越要保守
从预抓取到抓取位姿的距离通常不长，却是最容易撞到物体和桌面的阶段。这里建议使用短直线轨迹或分段小步移动，并降低速度和加速度。
低速接近的完成条件有两个：末端到达抓取位姿；物体仍在允许位置内。若方块被夹爪推离 A 区中心，或瓶子在接近过程中倾倒，即使末端到位，也不能进入夹爪闭合状态。
这里可以做一个很直接的对照实验：固定其他参数，只改变接近速度，再记录每档速度下的抓取成功率、物体位移和碰撞次数。结果通常很直观——速度提高以后，时间可能只省下一点，撞飞物体和错过闭合时机的概率却会明显上升。
3.4 接触以后，每一步都要等物理世界给出回答
从夹爪碰到物体开始，程序不能再只看控制器是否返回成功。正确的顺序是：闭合夹爪，短距离抬升，确认物体跟随；搬到目标上方，低速下降，确认物体获得支撑；最后打开夹爪，等待物体稳定，再退出观察区域。
这几个停顿看似拖慢了流程，实际是在降低返工成本。空抓若在抬升阶段就被发现，机器人不必带着空夹爪走完整段搬运轨迹；瓶子若在盒沿处受到阻挡，也不应继续下降或强行释放。具体采用哪些信号判断，将在第 4 节展开。
3.5 执行前校验：看起来合理，还要真的能到
第 2 节的初筛只看几何关系。执行前校验要面对具体机械臂，回答三个更实际的问题：
目标是否处于 SO-101 的工作空间； 每个末端位姿是否存在满足关节限位的逆运动学解； 从当前姿态到目标姿态的整条轨迹是否碰桌面、盒子或机械臂自身。
只有完整动作序列都通过，状态机才进入预抓取。若某个关键位姿无解，不能只删掉那一步继续执行，而应回到动作生成，调整姿态、偏移或中间点。
3.6 代码里怎么写状态机
程序中建议把任务逻辑和设备接口分开。状态机只决定“当前处于哪个状态、下一步去哪”；机器人后端负责“怎样把末端移动到目标”；检测模块负责“结果是否成立”。
每个状态可以拆成三个函数：
on_enter() | ||
on_update() | ||
on_exit() |
状态函数统一返回四种结果：
RUNNING | ||
SUCCESS | ||
RETRYABLE_FAILURE | ||
FATAL_FAILURE |
完成条件最好要求连续若干周期成立，而不是某一帧刚好满足就转移。这种“去抖”可以减少视觉抖动和电流瞬时尖峰带来的误判。
4 机械臂动完了，不等于任务成功
最常见的误判是：程序执行到了最后一行，于是把任务记成成功。真实情况可能完全不同——夹爪闭合了，但中间没有物体；机械臂抬起来了，但方块还留在桌面；夹爪在 B 区打开了，但方块滚出了边界。
成功检测必须跟着任务阶段走。抓取后看有没有形成夹持，抬升后看物体是否离桌，释放后看物体是否稳定留在目标区域。
4.1 夹爪到底夹到东西没有
最容易获得的信号是夹爪宽度。若夹爪闭合后还保留一段与物体宽度相符的开口，说明两指之间可能有物体；若夹爪几乎完全合拢，更像是空抓。
但宽度不能单独定案。夹爪可能卡在机构上，也可能碰到桌面或其他物体。电机电流、力矩或末端力可以提供第二类证据：接触发生后，负载通常会升高并保持一段时间。视觉则回答第三个问题——夹在两指之间的到底是不是目标物体。
入门实验推荐使用“夹爪宽度 + 抬升后视觉”组合。前者给出即时提示，后者用物体是否离桌完成最终确认。
4.2 物体有没有跟着夹爪离开桌面
抬升检测是最直观的抓取验证。抓取前保存物体初始位置，抬升过程中持续估计物体高度和它与夹爪的相对位置。
方块任务中，只要方块底面离开桌面，并连续若干帧保持在夹爪下方，就可以确认抬升成功。瓶子任务还要看瓶身是否下滑或旋转。若瓶子虽然没有完全掉落，但相对夹爪持续下移，也应判为“夹持不稳定”，而不是等到真正掉落才处理。
4.3 物体是不是稳定留在目标区域
放置检测发生在夹爪打开并退出以后。先退出再看，能够减少夹爪遮挡。
方块任务不能只检查中心点是否落入 B 区。更稳妥的做法是检查方块投影有足够比例位于 B 区内，并等待几帧确认它没有继续滑动。瓶子任务则检查瓶体主体是否进入收纳盒、是否卡在盒沿、是否滚出；若任务要求竖直放置，还要增加姿态条件。
放置成功包含三个条件：
物体进入目标区域； 物体与夹爪已经分离； 等待稳定时间后，物体没有继续滚动或倾倒。
4.4 日志要能回答“最早哪里不对”
只保存 success=False，对调试几乎没有帮助。看一段典型的失败时间线：夹爪闭合后保留了合理开口，初步检测认为可能夹到物体；末端随后抬高，但视觉中的物体高度没有变化。最早出现矛盾的是抬升验证，因此这次失败应先检查抓取点和夹爪接触，而不是去调 B 区放置位置。
日志至少要把这条推理链还原出来：五个动作位姿、状态时间戳、末端与夹爪反馈、物体观测、阶段检测结果，以及第一次失败时的代码和关键帧。任务编号和统一时间基准把这些信息对齐，失败分析才不会停留在猜测上。
5 失败不是一句“没抓起来”
机器人失败后，最没有用的结论就是“再试一次”。如果位姿、速度、夹爪参数和场景都没变，下一次很可能重复同样的失败。
更有效的做法是先找最早出现异常的状态。后面看到的现象往往只是前面问题的结果。方块在抬升时掉落，原因可能是抓取点偏离重心；瓶子在收纳盒外倾倒，也可能是搬运途中已经下滑，而不是放置位姿本身错误。
5.1 先按阶段缩小排查范围
排查时先看事实，再写原因。例如“夹爪闭合后宽度接近零，方块仍在 A 区”是可观察事实；“抓取点不准”是推断。两者要分开记录，避免把猜测当结论。
5.2 恢复不是回到起点，而是改掉导致失败的条件
目标被推走，先重新感知；抓取方向不合理，重新生成候选；接近撞到桌面，提高抓取点或改变姿态；夹持不足，调整闭合终点、抓取点或抬升加速度；规划无解，换候选或增加中间点。
恢复动作本身也要有顺序：停止当前运动，判断是否需要保持夹持，把机械臂移到不会扩大风险的位置，再重新感知和规划。目标已经被推走以后，不能沿原轨迹快速重试。
5.3 三次重试不能是三次复制粘贴
重试必须设置上限，并记录每一次到底改了什么。
第一次失败：重新感知，排除偶发定位误差； 第二次失败：调整抓取偏移、预抓取距离、接近速度或夹爪参数； 第三次失败：换抓取方向或采用更保守的策略； 仍然失败，或出现急停、通信中断、持续过载：安全退出。
安全退出不等于直接断电。程序应停止当前动作，根据物体是否仍被夹持决定保持还是释放，再将机械臂移动到允许的安全位姿，最后保存日志和报警信息。
5.4 失败样本是下一轮改进最值钱的数据
一次失败样本应保存“任务输入—动作目标—执行过程—失败判定—恢复结果”。至少包括图像、深度、物体位姿、动作位姿、关节与末端轨迹、夹爪命令和反馈，以及失败前后视频。
失败标签建议分成五项：失败阶段、直接现象、推定原因、恢复动作、恢复结果。后续统计时，才能回答哪些失败最常见、哪种参数修改最有效、哪些任务应该补采数据。
6 实验：把整套抓取—放置流程跑起来
实验按三层推进：先用 Mock 检查状态流和日志，再用 ManiSkill 检查控制与碰撞，最后才接入 SO-101。每切换一层，只替换机器人后端，不改任务配置、检测逻辑和状态机。这样出现问题时，可以判断错误来自上层流程，还是来自仿真或硬件接口。
6.1 代码怎么拆：状态机不直接碰硬件
真机实验由 SO-101 机械臂、二指夹爪、相机、控制计算机和急停装置组成。机械臂基座固定在桌面，桌面作为工作环境中的碰撞平面；相机提供目标物体位姿和放置区域观测；控制程序负责动作目标生成、状态转移和结果记录。
无硬件实验采用两级替代方案。教学模拟后端不依赖机器人软件，可直接验证任务配置、状态机、检测、恢复和日志是否正确。ManiSkill 后端进一步提供动力学、碰撞、相机观测和视频录制，用于观察动作在仿真环境中的实际结果。
项目按职责拆分为若干 Python 模块。状态机只调用统一的后端接口，不直接操作串口、仿真器或规划器。这样可以在不修改任务逻辑的前提下切换运行平台，也便于单独测试每个模块。
代码清单 7-1 推荐的项目目录结构
robot_pick_place/
├── models.py # 位姿、任务、观测、状态和失败代码
├── config.py # 方块与瓶子任务参数
├── pose_generator.py # 生成五个动作位姿
├── checker.py # 抓取、抬升和放置检测
├── recovery.py # 根据失败类型调整下一次任务参数
├── state_machine.py # 抓取—放置状态机
├── task_logger.py # JSONL 任务日志
├── run_demo.py # 命令行入口
└── backends/
├── base.py # 统一后端接口
├── mock.py # 无外部依赖的教学模拟后端
├── so101_adapter.py # SO-101 真机适配器
└── maniskill_adapter.py # ManiSkill 仿真适配器
6.2 先统一数据结构和任务参数
第一步不是写状态机，而是先统一模块之间传递的数据。完整定义见 models.py。正文先看状态机运行时传递的三类对象，任务参数 TaskConfig 随后单独对照：
@dataclass(frozen=True)
class ActionTargets:
pre_grasp: Pose
grasp: Pose
lift: Pose
place: Pose
retreat: Pose
@dataclass(frozen=True)
class Observation:
timestamp: float
eef_pose: Pose
gripper_width: float
object_pose: Pose | None
object_visible: bool
object_attached: bool
collision: bool = False
@dataclass(frozen=True)
class MotionResult:
success: bool
reason: str = ""
这里有一条清晰的数据流：TaskConfig 描述“要完成什么任务”，ActionTargets 给出“五步分别去哪里”，Observation 返回“机器人和物体现在怎样”，MotionResult 则说明“一条运动命令是否执行成功”。状态机只依赖这些统一对象，因此不需要知道底层连接的是 Mock、ManiSkill 还是真机。
配置文件完整内容见 config.py。TaskConfig 字段很多，对照两个任务时先看接近方向、抓取偏移、预抓取距离和夹爪宽度：
def cube_task() -> TaskConfig:
return TaskConfig(
approach_direction=(0.0, 0.0, -1.0),
grasp_offset=(0.0, 0.0, 0.055),
pre_grasp_distance=0.12,
gripper_open_width=0.075,
gripper_close_width=0.035,
# 其余位置、速度和检测阈值见完整源码
)
def bottle_task() -> TaskConfig:
return TaskConfig(
approach_direction=(-1.0, 0.0, 0.0),
grasp_offset=(0.065, 0.0, 0.03),
pre_grasp_distance=0.11,
gripper_open_width=0.095,
gripper_close_width=0.058,
# 其余位置、速度和检测阈值见完整源码
)
方块的接近方向是竖直向下，瓶子的接近方向是水平方向；grasp_offset 又把夹爪参考点放到各自合适的夹持位置。状态机没有改变，改变的是输入给它的任务配置。
代码中的四元数和距离仅作为教学示例。真机实验必须依据 SO-101 的末端坐标系、夹爪安装方向和标定结果重新确认，不能复制未验证的姿态数值。
6.3 把物体位姿变成动作位姿
动作位姿生成函数先将接近方向归一化，再根据目标物体位姿、抓取偏移和抓取姿态得到抓取位姿。预抓取位姿沿接近方向反向偏移；抬升位姿在抓取位姿基础上提高；退出位姿位于目标区域上方。
代码将 place_pose 解释为物体期望到达的位置，因此生成机械臂放置位姿时仍需保留抓取偏移。这样，方块中心最终落在 B 区目标点，瓶身中心最终落在收纳盒内部，而不是把末端参考点误当成物体中心。
完整实现见 pose_generator.py。核心不是辅助函数 _normalize()，而是五个位姿如何由两个参考位姿派生：
def generate_targets(task: TaskConfig) -> ActionTargets:
ax, ay, az = _normalize(task.approach_direction)
ox, oy, oz = task.grasp_offset
qx, qy, qz, qw = task.grasp_quaternion
# 物体位姿 + 抓取偏移 = 夹爪真正要到达的抓取位姿
grasp = Pose(
task.object_pose.x + ox,
task.object_pose.y + oy,
task.object_pose.z + oz,
qx,
qy,
qz,
qw,
)
# 沿接近方向反向退开，得到安全的预抓取位姿
pre_grasp = grasp.shifted(
-ax * task.pre_grasp_distance,
-ay * task.pre_grasp_distance,
-az * task.pre_grasp_distance,
)
lift = grasp.shifted(0.0, 0.0, task.lift_height)
# place_pose 描述物体目标，因此放置末端仍要保留 grasp_offset
place = Pose(
task.place_pose.x + ox,
task.place_pose.y + oy,
task.place_pose.z + oz,
qx,
qy,
qz,
qw,
)
retreat = place.shifted(0.0, 0.0, task.retreat_height)
return ActionTargets(
pre_grasp=pre_grasp,
grasp=grasp,
lift=lift,
place=place,
retreat=retreat,
)
这段代码最容易误解的是 place：task.place_pose 表示物体最终应在的位置，不是夹爪末端的位置，因此生成放置目标时仍然要加上 grasp_offset。否则程序会把夹爪参考点送到 B 区中心，而物体中心会产生同样大小的偏差。
运行前检查 打印五个位姿并在 RViz、Open3D 或仿真窗口中可视化。若抓取位姿方向、预抓取偏移或放置高度明显错误，应在动作执行前修正。
6.4 先用 Mock 后端把逻辑跑通
RobotBackend 定义状态机需要的最小能力：建立连接、重置任务、读取观测、移动末端、控制夹爪和停止运动。真机与仿真只要实现这些方法，便可以复用同一套状态机。
MockBackend 是可运行的教学模拟后端。它直接更新末端位姿，并在夹爪到达物体附近且执行闭合时建立“物体附着”关系；夹爪打开时释放物体。该后端不模拟真实动力学，其用途是检查状态机顺序、数据流、检测逻辑和日志格式。
统一接口的完整定义在 backends/base.py，Mock 实现在 backends/mock.py。状态机实际只要求后端提供下面七项能力：
class RobotBackend(ABC):
@abstractmethod
def connect(self) -> None:
...
def reset_task(self, task: TaskConfig) -> None:
...
def get_observation(self) -> Observation:
...
def move_pose(self, target: Pose, speed: float,
timeout: float = 8.0) -> MotionResult:
...
def set_gripper(self, width: float, force: float = 0.5,
timeout: float = 3.0) -> MotionResult:
...
def stop(self) -> None:
...
def disconnect(self) -> None:
...
Mock 后端最关键的不是连接和复位样板代码，而是它怎样用一个简化的“附着关系”模拟抓住物体：
def set_gripper(self, width: float, force: float = 0.5,
timeout: float = 3.0) -> MotionResult:
self.gripper_width = width
closing = width <= self.task.gripper_close_width + 0.01
opening = width >= self.task.gripper_open_width - 0.01
if closing and self.eef_pose.distance_to(self.object_pose) <= 0.11:
self.object_attached = True
self.object_from_eef = (
self.object_pose.x - self.eef_pose.x,
self.object_pose.y - self.eef_pose.y,
self.object_pose.z - self.eef_pose.z,
)
elif opening and self.object_attached:
self.object_attached = False
self.object_pose = replace(self.object_pose, z=self.task.place_pose.z)
return MotionResult(True)
闭合时，只有夹爪足够接近物体才建立 object_attached=True；之后末端移动，物体按照相对偏移一起移动；张开时解除附着。它能验证状态顺序和检测逻辑，但没有接触力、摩擦、碰撞和惯性，因而不能证明真实抓取会成功。
6.5 检测和恢复：失败后必须改点东西
检测函数只读取统一 Observation，不依赖具体运行平台。抓取检测结合夹爪宽度、物体与末端距离以及仿真附着状态；抬升检测比较物体相对于初始位置的高度变化；放置检测确认物体已经脱离夹爪，并位于目标位置允许范围内。
恢复函数根据失败代码修改下一次任务参数。运动或碰撞失败时增加预抓取距离并降低接近速度；抓取或抬升失败时微调抓取偏移、夹爪闭合宽度和接近速度；放置失败时调整放置高度和退出高度。此处调整量是教学示例，真机实验应依据日志和安全范围逐步确定。
完整检测逻辑见 checker.py。三个函数分别回答“夹住了吗、抬起来了吗、放到目标了吗”：
def grasp_success(obs: Observation, task: TaskConfig) -> bool:
ifnot obs.object_visible or obs.object_pose isNone:
returnFalse
width_is_plausible = (
task.gripper_close_width - 0.015
<= obs.gripper_width
<= task.object_width + 0.03
)
object_is_near = obs.eef_pose.distance_to(obs.object_pose) <= 0.12
return obs.object_attached or (width_is_plausible and object_is_near)
def lift_success(
obs: Observation,
initial_object_pose: Pose,
task: TaskConfig,
) -> bool:
if obs.object_pose isNone:
returnFalse
height_gain = obs.object_pose.z - initial_object_pose.z
object_is_near = obs.eef_pose.distance_to(obs.object_pose) <= 0.14
return height_gain >= task.lift_threshold and object_is_near
def place_success(obs: Observation, task: TaskConfig) -> bool:
if obs.object_pose isNoneor obs.object_attached:
returnFalse
xy_error = hypot(
obs.object_pose.x - task.place_pose.x,
obs.object_pose.y - task.place_pose.y,
)
return xy_error <= task.place_xy_tolerance
这三个判据有意使用不同证据。抓取阶段看夹爪开口与物体距离，抬升阶段看物体高度增量，放置阶段看物体是否脱离夹爪并进入目标区域。这样即使“运动命令执行成功”，任务仍可能在物理检测上失败。
恢复逻辑见 recovery.py。以抓取或抬升失败为例，下一次尝试会同时改变抓取高度、夹爪闭合宽度和接近速度：
def adjust_task(task: TaskConfig, failure: FailureCode) -> TaskConfig:
if failure in {FailureCode.GRASP_FAILED, FailureCode.LIFT_FAILED}:
gx, gy, gz = task.grasp_offset
return replace(
task,
grasp_offset=(gx, gy, gz + 0.005),
gripper_close_width=max(0.0, task.gripper_close_width - 0.003),
slow_speed=max(0.03, task.slow_speed * 0.85),
)
return task
这里真正重要的是“重试必须改变条件”，而不是记住示例中的 0.005 或 0.003。这些调整量只用于教学；真机应根据失败证据、平台限位和低速试验逐步确定。
6.6 状态机：把所有模块串起来
完整状态机见 state_machine.py。run_attempt() 负责一次尝试，每个状态都遵循“进入—执行—检查—失败即返回”的模式：
def run_attempt(self) -> AttemptResult:
targets = generate_targets(self.task)
initial_object_pose = self.task.object_pose
self._enter(State.APPROACH)
result = self.backend.move_pose(targets.grasp, self.task.slow_speed)
ifnot result.success:
return self._fail(FailureCode.MOTION_FAILED, result.reason)
self._enter(State.CLOSE_GRIPPER)
result = self.backend.set_gripper(self.task.gripper_close_width)
ifnot result.success:
return self._fail(FailureCode.GRASP_FAILED, result.reason)
self._enter(State.VERIFY_GRASP)
ifnot grasp_success(self.backend.get_observation(), self.task):
return self._fail(FailureCode.GRASP_FAILED, "grasp check failed")
self._enter(State.LIFT)
result = self.backend.move_pose(targets.lift, self.task.slow_speed)
obs = self.backend.get_observation()
ifnot result.success ornot lift_success(obs, initial_object_pose, self.task):
return self._fail(FailureCode.LIFT_FAILED, "lift check failed")
# 搬运、下降、释放和放置检测使用相同模式，见完整源码。
跨尝试的恢复由 run() 管理。失败后先停止，再判断是否达到重试上限；仍可恢复时才修改任务参数：
def run(self) -> bool:
current_task = self.task
for retry_index in range(current_task.max_retries + 1):
self.task = current_task
self.backend.reset_task(current_task)
result = self.run_attempt()
if result.success:
returnTrue
self.backend.stop()
if retry_index >= current_task.max_retries:
self._enter(State.SAFE_EXIT, reason=result.reason)
returnFalse
self._enter(State.RECOVER, failure=result.failure.name)
current_task = adjust_task(current_task, result.failure, retry_index + 1)
returnFalse
这样，单次状态流与重试策略彼此独立，恢复逻辑不会散落在每一个动作状态中。
6.7 运行、日志和结果查看
TaskLogger 为每次任务创建独立目录，并以 JSON Lines 格式保存事件。每一行是一个完整 JSON 对象，包含时间、状态、事件、统一观测以及失败代码、重试次数等附加字段。JSON Lines 便于逐行追加，即使程序异常中止，也能保留已经写入的记录。
完整实现见 task_logger.py。日志器的核心是把一次状态事件和当时的观测组成一条记录，然后立即追加到文件：
def log(self, state: State, event: str,
observation: Observation | None = None, **extra: Any) -> None:
record = {
"time": time(),
"state": state.name,
"event": event,
**extra,
}
if observation is not None:
record["observation"] = asdict(observation)
with self.events_path.open("a", encoding="utf-8") as file:
file.write(json.dumps(record, ensure_ascii=False) + "\n")
采用 JSONL 而不是任务结束后一次性写入大 JSON，是为了让每次状态转换都立即落盘。即使程序中途退出，已经发生的状态和失败证据仍然能够保留。
命令行入口读取任务名称，创建后端和日志器，并在 try/finally 结构中保证程序结束时断开连接。教学模拟后端没有外部依赖，可以作为项目的第一项自动化测试。
入口的完整实现见 run_demo.py。它只负责组装任务、后端、日志器和状态机：
def main() -> int:
args = build_parser().parse_args()
task = get_task(args.task)
backend = MockBackend()
logger = TaskLogger(args.output, task.name)
backend.connect()
try:
machine = PickPlaceStateMachine(backend, task, logger)
success = machine.run()
finally:
backend.disconnect()
print(f"task={task.name}, success={success}")
print(f"log_dir={logger.task_dir}")
return0if success else1
入口层没有抓取细节：切换任务只改变 TaskConfig，切换平台只替换 backend，状态机本身保持不变。
在教学模拟后端运行两个任务：
# 在包含 robot_pick_place/ 的目录中执行
python -m robot_pick_place.run_demo --task cube
python -m robot_pick_place.run_demo --task bottle
# 典型输出
task=cube_a_to_b, success=True
log_dir=runs/<时间戳>_cube_a_to_b
task=bottle_to_bin, success=True
log_dir=runs/<时间戳>_bottle_to_bin
配套代码已分别运行方块和瓶子配置。两个任务均依次进入 13 个执行状态并生成 events.jsonl。教学模拟通过只能证明程序逻辑闭环正确，不能替代碰撞检查、动力学验证和真机安全测试。
6.8 有真机：接入 SO-101
SO-101 真机适配器接收一个已经配置和标定的机器人对象。该对象负责硬件连接、读取电机与相机观测以及发送关节目标。适配器另外注入三个与本项目相关的函数：plan_pose() 将末端目标位姿转换为一系列关节动作；read_eef_pose() 根据关节状态计算实际末端位姿；detect_object() 从相机观测中获得目标物体位姿。
LeRobot 的机器人接口采用 connect()、get_observation() 和 send_action() 组织硬件读写。适配器将这些底层方法转换为状态机所需的 move_pose()、set_gripper() 和 get_observation()。运动规划可以由 MoveIt 2、配套的逆运动学与插值模块，或其他经过验证的规划器完成。
完整适配器见 backends/so101_adapter.py。正文只看最关键的 move_pose()：它把状态机给出的末端位姿交给规划器，再逐条发送规划出的关节动作。
def move_pose(self, target: Pose, speed: float,
timeout: float = 8.0) -> MotionResult:
deadline = monotonic() + timeout
try:
joint_actions = self.plan_pose(target, speed)
except Exception as exc:
return MotionResult(False, f"planning failed: {exc}")
for action in joint_actions:
if monotonic() > deadline:
self.stop()
return MotionResult(False, "motion timeout")
self.robot.send_action(action)
sleep(0.02)
return MotionResult(True)
这段代码刻意把 plan_pose() 作为注入函数，而不是在适配器内写死。原因是逆运动学、URDF、关节顺序和碰撞规划都依赖具体机械臂配置。适配器负责统一接口，规划器负责把末端目标转换为安全、可执行的关节轨迹。
另外两条真机路径同样重要：get_observation() 把电机与相机数据转换成统一 Observation；set_gripper() 把以米表示的夹爪目标转换成硬件实际命令。它们的完整边界检查应保留在源码中，不需要在正文逐行展开。
表 7-1 SO-101 真机运行前的安全检查
| 真机检查项目 | 执行要求 |
|---|---|
真机安全原则 AI 生成或修改的真机代码不得直接执行。必须检查关节限位、夹爪方向、速度、规划结果、碰撞模型、超时和急停逻辑，并先进行无物体低速试运行。
6.9 没有真机：接入仿真环境
没有真机时，可以直接使用预装好的云端环境。ManiSkill 通过 Gymnasium 接口创建任务环境，reset() 返回初始观测，step() 接收控制器动作并返回新观测、奖励、终止标志和附加信息。单环境 CPU 实验可使用 CPUGymWrapper 将批量张量转换为常规的非批量 NumPy 接口；RecordEpisode 则负责保存视频和轨迹。
ManiSkill 的动作维度和语义由所选控制器决定。示例使用关节增量控制创建环境，随后由 pose_to_actions() 将状态机给出的绝对动作位姿转换为一段控制器动作。若改用末端增量位姿控制，则应按照环境 action_space 的顺序构造平移、旋转和夹爪动作。运行前必须打印 observation_space 和 action_space，不能根据其他机器人或其他控制器的维度直接复制动作向量。
适配器完整实现见 backends/maniskill_adapter.py。接入时最关键的是先固定环境、观测模式和控制模式，再检查空间定义：
import gymnasium as gym
import mani_skill.envs
from mani_skill.utils.wrappers.gymnasium import CPUGymWrapper
from mani_skill.utils.wrappers.record import RecordEpisode
env = gym.make(
"PickCube-v1",
num_envs=1,
obs_mode="state_dict",
control_mode="pd_joint_delta_pos",
render_mode="rgb_array",
)
env = CPUGymWrapper(env)
env = RecordEpisode(
env,
output_dir="runs/maniskill_video",
save_trajectory=True,
save_video=True,
video_fps=30,
)
obs, info = env.reset(seed=0)
print(env.observation_space)
print(env.action_space)
这里没有展开 pose_to_actions()，因为它必须与当前环境的 control_mode 配套：在 pd_joint_delta_pos 下，它要输出关节增量；换成末端位姿控制器后，动作的维度和语义都会变化。因此，打印 observation_space 和 action_space 不是调试附属步骤，而是编写转换函数之前必须完成的接口确认。
表 7-2 真机、仿真与教学模拟代码的主要差异
| 差异项目 | SO-101 真机 | ManiSkill 仿真 | 教学模拟后端 |
|---|---|---|---|
6.10 实验步骤：从空跑到真机
实验按六个检查点推进，每一步都要留下可验证的输出：
Mock 空跑：分别运行 cube和bottle，确认状态机到达DONE，并生成events.jsonl。位姿可视化：画出五个动作位姿。方向、高度或偏移不合理时，在这一阶段修正，不要等真机碰撞后再查。 仿真闭环：在 ManiSkill 中检查控制器动作、碰撞、物体是否随夹爪运动，以及视频和轨迹能否保存。 真机空载：只执行预抓取、抬升和放置位姿，低速确认逆运动学、轨迹、桌面间隙和急停路径。 逐级加任务：先运行方块，再运行瓶子；每次只增加一种新约束，不同时修改多组参数。 制造一次可恢复失败：在仿真中改变接近偏移或夹爪宽度，检查失败代码、参数调整和第二次尝试是否都写入日志。真机不得通过故意碰撞完成这一项。
6.11 怎么看结果
实验结果需要同时保留空间、时间和任务三个层面的证据。空间层面显示目标物体位姿、五个动作位姿和机械臂末端轨迹；时间层面显示状态机进入与退出时刻、夹爪命令和检测结果；任务层面统计成功率、耗时、重试次数和失败类型。
表 7-3 第 6 节实验的可视化与分析输出
| 输出文件或图表 | 必须包含的内容 | 用于回答的问题 |
|---|---|---|
日志分析的完整脚本见 analyze_runs.py。正文只看单次任务如何从事件流提取四个核心指标：
def summarize_task(task_dir: Path) -> dict[str, object]:
events_path = task_dir / "events.jsonl"
records = [
json.loads(line)
for line in events_path.read_text(encoding="utf-8").splitlines()
if line.strip()
]
success = any(record.get("event") == "success"for record in records)
failures = [record for record in records if record.get("event") == "failure"]
states = [record["state"] for record in records if record.get("event") == "enter"]
return {
"task_dir": task_dir.name,
"success": success,
"failure_count": len(failures),
"last_failure": failures[-1].get("code", "") if failures else"",
"state_count": len(states),
}
数据处理顺序是：逐行读取事件 → 判断是否出现 success → 收集全部 failure → 统计进入过的状态。批量遍历目录和写入 CSV 只是外围流程，留在完整源码中即可。把这条最小链路跑通后，再增加总耗时、各状态耗时和失败类型占比。
6.12 用一次失败日志做复盘
假设瓶子最终倒在收纳盒外。不要从最后一帧开始猜，而是沿状态时间线向前找：如果瓶子在 VERIFY_LIFT 时已经相对夹爪下滑，盒外倾倒只是后果，第一修改项应是夹持位置、闭合宽度或抬升速度；只有瓶子在搬运阶段始终稳定、进入 MOVE_PLACE 后才碰到盒沿，才应该调整放置位姿和运动路径。
一次合格的复盘写四行就够：最早异常状态是什么；当时直接观察到了什么；哪些证据支持原因判断；下一轮只改哪个条件。把事实和推断分开，通常比列出十种“可能原因”更有用。
6.13 让 AI 帮忙分析，但别把判断交出去
AI 适合帮助梳理概念、检查代码和提出可能原因，但它看不到真实机械臂当时的全部状态，也不能替代实验判断。一次完整的 AI 分析记录，应保留原始问题、回答摘要、采纳了什么、哪些地方仍需验证，以及验证后的最终结论。凡是涉及真机参数的建议，都要回到文档、代码和低速实验中逐项确认。
可以交给 AI 辅助分析的问题
| 类型 | 可以这样问 |
|---|---|
pose_to_actions() 和 gripper_action() 应如何修改？ | |
6.14 Vibe Coding：让 AI 补完日志分析，再逐项核对
这一轮 Vibe Coding 从扩展 analyze_runs.py 开始。先把 events.jsonl 的字段示例和期望输出交给 AI，让它生成一个结构清楚的初稿；随后在教学模拟日志上实际运行，并逐项核对统计结果。只要有一项数字与原始日志对不上，就回到字段定义和计算逻辑继续检查。
一个可直接使用的 Prompt
请基于下面的 JSONL 任务日志，编写一个适合初学者的 Python 分析脚本。
输入：
1. runs/ 下多个任务目录中的 events.jsonl；
2. 每行包含 time、state、event、observation 和可选 failure code；
3. 状态进入事件为 event="enter"，成功事件为 event="success"。
输出：
1. 每次任务是否成功、总耗时、重试次数；
2. 每个状态的持续时间；
3. 各失败代码的出现次数；
4. summary.csv；
5. 一张状态耗时柱状图和一张失败类型统计图。
要求：
- 使用 pathlib、json、csv 和 matplotlib；
- 缺少字段时给出清晰提示，不直接崩溃；
- 函数拆分清楚，包含类型标注；
- 不使用复杂框架；
- 解释每一段代码的作用和假设。
初稿跑起来以后，还可以继续追问：日志中断或最后一行不完整时如何处理；同一状态多次进入时如何计算耗时；如何区分一次任务中的多次重试；如何将方块与瓶子分组统计；如何把失败关键帧路径写入报告。
验证边界 AI 可以生成代码初稿，但每一段代码都要能够解释，并通过实际运行核对结果。只要修改涉及真机控制，就必须重新检查限位、速度、超时、夹爪方向和急停路径。
6.15 最后怎么判断这次实验真的跑通
一段成功视频只能说明某一次运行完成了任务；一份代码也只能说明程序写出来了。要判断这次实验是否真正跑通，还要看状态机是否按顺序推进、检测是否给出可靠证据、失败后是否发生了有依据的调整，以及日志能否把整个过程还原出来。
表 7-4 完整实验结果的评价参考（满分 100 分）
| 检查项 | 看到什么才算完成 | 参考分值 |
|---|---|---|
summary.csv、关键帧和任务日志 | ||
运行完两个任务后，最后再做一次检查：代码是否真的执行到了正确状态，物体是否真正进入目标区域，失败时参数是否发生了有依据的变化。完成这些检查以后，整条规则操作闭环才算跑通。
7 收一下：从一个坐标，到一次完整任务
开篇那次空抓之所以会一路运行到 B 区，是因为程序只检查了命令是否执行，没有检查物体是否跟随。现在，五个动作位姿给出路径，状态机在关键位置停下来，夹爪与视觉提供物理证据，失败代码则把流程送回正确的恢复入口。
方块搬运和瓶子入盒表面上是两个不同的任务，它们在底层其实是同一套“生成—执行—检测—恢复”闭环。只要其中一环缺少可靠证据，程序就不应宣布任务成功。
到这里，第 7 讲也为传统模块化机器人操作画上了句号。从前面的感知、位姿估计，到本讲的抓取、放置、结果检测和失败恢复，我们搭建的是一条典型的模块化流水线：
相机图像 → 目标位姿 → 动作规划 → 状态机执行 → 成功检测与失败恢复
在这套系统中，每个模块的输入、输出和判断规则都由人明确设计。机器人为什么进入某个状态、为什么执行某个动作、又为什么失败，都可以沿着程序逐步检查。这正是传统方法的优势：结构清晰、行为可解释，也便于定位和处理异常。
但它的局限同样明显。环境、物体和任务一旦发生变化，工程人员往往需要重新调整阈值、修改规则，甚至增加新的状态。面对难以穷举的真实场景，我们自然会提出另一个问题：
能不能不再逐条编写规则，而是让机器人直接从任务经验中学习“看到什么，就应该做什么”？
第 8 讲开始，课程会换一种方法回答这个问题：从感知、规划和控制分段设计的模块化 pipeline，走向直接学习 observation 到 action 映射的端到端 policy。规则不会从系统里消失，但机器人的核心行为将不再只靠工程师逐条写出，而要从任务数据中学习。
参考资料
MoveIt 2 Documentation：https://moveit.picknik.ai/ MoveIt 2 Tutorials：https://moveit.picknik.ai/main/doc/tutorials/tutorials.html ManiSkill Documentation：https://maniskill.readthedocs.io/ robosuite Documentation：https://robosuite.ai/docs/ MuJoCo Menagerie：https://github.com/google-deepmind/mujoco_menagerie Isaac Lab Documentation：https://isaac-sim.github.io/IsaacLab/ AnyGrasp SDK：https://github.com/graspnet/anygrasp_sdk GraspNet Baseline：https://github.com/graspnet/graspnet-baseline Fang H S, Wang C, Gou M, Lu C. GraspNet-1Billion. CVPR, 2020：https://graspnet.net/
往期回顾
-END-
点击下方卡片，关注【Xbotics具身智能实验室】公众号
你想要的这里都有~~
Ask Me Anything｜提问箱
对文章有疑惑，或想聊更深？欢迎把你的问题丢给我们：技术方案、实操踩坑、课程与资料、项目合作、职业发展，都可以问。
怎么问：在评论区留言，或私信公众号
我们会做什么：每周集中整理高质量问题并公开回复，重点问题邀请作者或嘉宾深度解答；典型问题会加入知识库并持续更新。
提问小提示：尽量说明「你的目标—当前做法—期望产出」，附上必要信息（硬件/软件版本、数据规模等），能更快获得有用答案。
一起把问题变成知识，推动社区进步 🚀
