第 6 章抓取与放置¶

抓取是操作链路里唯一“规划器说了不算”的环节：MoveIt 判断的是碰撞与可达，而工件会不会被夹住、夹在哪里、松手后还在不在原地，由接触几何与摩擦决定。本章给出抓取候选的几何判据（可用一段脚本自己算）、夹爪与吸盘在软件侧的接口形状、以及一个桌面 Pick&Place 最小闭环——含实现里两处常见的顺序错误。以 ROS 2 Humble + MoveIt 2 2.5.10、样本臂 panda + 两指夹爪为例；mock 硬件下物理层缺席，力学结论只到判据级。

从感知到放置的接口形状：

flowchart LR
    percep["感知：位姿 / 点云"] --> cand["抓取候选生成"]
    cand --> judge["几何判据<br/>面心对置 / 力闭合"]
    judge --> tool["夹爪 / 吸盘接口"]
    tool --> pick["Pick & Place 闭环"]

1. 感知输出到抓取输入的接口形状¶

感知侧交出来的不是“一个抓取点”，而是一条可执行的约定：工件位姿（位置 + 朝向）在其标定参考系里给出，附带上位对结果的置信度与残余不确定度。抓取侧要消费的就是这些，外加模型自身的两件信息：末端几何（指尖形状与行程）与工件几何（CAD 或测量得到的形状）。

对位姿的来源与算法分工：手眼外参与相机标定归 camera-note 第 1 章，工件位姿解算（PnP 类）归其第 2 章，点云分割与拟合归其第 5 章；本章只接住结果，并问一个问题——这个位姿误差会让抓取失败吗。答案取决于抓取判据，因此判据要先立起来。

2. 抓取候选的几何判据¶

两指对置抓取的可行性可以拆成三条独立判据，全部只用到几何量，能在几十行脚本里算清。

跨度可行：两接触点间距离不超过夹爪最大张开，且不小于最小可夹尺寸。
对置（antipodal）与力闭合：两接触点的外法线大致相向，且两接触点连线落在两个摩擦锥的交集里。等价的数值条件是——连线与法线的夹角不超过摩擦锥半角 \(\arctan\mu\)。
深度与避让：指尖沿法线方向的接触长度足够（小于指尖有效长度、大于工件边缘余量），且夹爪本体不与工件或其他物体相交。

以 5 cm 立方体、单侧行程 0.04 m（最大张开 8 cm）、静摩擦系数 \(\mu=0.35\) 为例：

import math
import numpy as np

STROKE = 0.08            # 夹爪最大张开：单侧 0.04 m × 2
MU = 0.35                # 静摩擦系数（示例取值）

def evaluate(p1, p2, n1, mu=MU):
    """p1/p2 为两接触点，n1 为 p1 处外法线（对置面取 n2 = -n1）。"""
    d = p2 - p1
    span = float(np.linalg.norm(d))                        # 判据 1：跨度要 ≤ STROKE
    ang = math.degrees(math.acos(abs(float(np.dot(d, -n1))) / span))
    limit = math.degrees(math.atan(mu))                   # 摩擦锥半角：19.29°
    return span, ang, ang <= limit                        # 判据 2：力闭合

half = np.array([0.025, 0.0, 0.0])
n1 = np.array([-1.0, 0.0, 0.0])
## 面心对置：连线与法线共线 → 偏角 0°，力闭合成立
print(evaluate(-half, half, n1))
## 接触点横向各偏 10 mm（夹在面边缘上）
print(evaluate(np.array([-0.025, 0.010, 0.0]), np.array([0.025, -0.010, 0.0]), n1))

(0.05, 0.0, True)          # 面心对置：跨度 50 mm，偏角 0°，力闭合
(0.05385164807134504, 21.8014094863518, False)   # 偏 10 mm：偏角 21.8° > 19.29°，判据破坏

三条数字关系值得记住：工件边长 40 / 60 / 80 mm 的对置跨度都在 8 cm 行程内，100 mm 就超出——行程直接划定可抓工件的尺寸上限；接触点从面心横向偏移工件半宽的 40%（10 mm）就足以破坏力闭合，这说明抓取位姿的横向容差比很多人以为的要紧得多；摩擦系数从 0.5 降到 0.1，摩擦锥半角从 26.6° 收到 5.7°，同一“略微偏置”的抓取从可行变成不可行——油污、抛光面或脱模剂都会把这一容差直接吃掉。

工业上的抓取生成（GraspGen、Contact-GraspNet 等）输出的正是满足这类判据的候选集合（外加网络学到的形状先验）。其可用性与推理链路不在本章断言，公开评估口径（FetchBench、GraspNet-1Billion 一类基准）同理只作名目引用。

判据与规划的关系要摆正：判据筛掉的是“物理上不可能”的候选，规划器判的是“在不撞的前提下能不能到达”。两件事都要过，顺序上先判据后规划——用昂贵的规划去筛本已被几何否证的候选是常见浪费。

3. 末端执行器的软件接口¶

夹爪与吸盘在软件侧的差别不在运动学而在接口通路：

末端	控制通路	反馈	规划侧表达
电动夹爪	动作接口（`/panda_hand_controller/gripper_cmd`，`control_msgs/action/GripperCommand`）	位置、是否停止（`reached_goal` / `stalled`）、力矩（有传感器时）	SRDF 的 `hand` 组 + 被动关节 + 具名组态 `open` / `close`
气动夹爪	数字输出（阀）经 IO，不走关节接口	到位开关量、真空压力开关	通常是“虚拟”二态，或按位置建模
吸盘	真空阀（IO）+ 真空度反馈	压力阈值判定吸附是否成立	抓取点为吸附面法向单点，无跨度判据

panda 的样本栈里，panda_hand_controller 是 position_controllers/GripperActionController，动作语义是“走到目标位置、带最大力矩上限”。开合通路的指令-返回形态（mock 硬件）：

指令位置 → (result.position, reached_goal, stalled, effort, 耗时, /joint_states 两指)
  0.040 → (0.0395, True, False, 20.0, 0.052 s, [0.04, 0.04])
  0.028 → (0.0280, True, False, 20.0, 0.051 s, [0.028, 0.028])
  0.020 → (0.0280, True, False, 20.0, 0.053 s, [0.02, 0.02])
  0.060 → (0.0600, True, False, 20.0, 0.051 s, [0.06, 0.06])

这四条返回里藏着三条要防的事。

镜像在硬件侧完成：两指读数始终相等，因为 finger2 由 ros2_control 的 mimic 参数复制 finger1 的指令（第 2 章）。控制侧只发一个值。

result.position 不是终点判据：指令 0.020 时 result.position 报回 0.028，而 /joint_states 里两指都已经是 0.02——结果体里的位置是完成时刻的反馈快照，存在一拍滞后。判断夹爪到位应以 joint_states 为准。

mock 层判不出“夹住了”：reached_goal 在四个指令下全为 True、effort 恒等于请求里的 max_effort。这一层没有接触力学，闭合到位不等于夹稳；真实硬件上 stalled（走不动而提前停止）才是“碰到工件”的信号，且需要与力矩/压力阈值一起判。这就是把抓取结论写进仿真路线时要标判据级的原因。

越限指令照样执行：0.06 m 超出 URDF 里 0.04 的限位，mock 照样到位且报告成功。而规划侧的场景状态用的是模型限位，两者一旦不一致，下一次规划的起点就是模型外状态——这类“看起来没事”的偏差是后续神秘失败的常见来源。控制通路不替模型限位兜底。

4. 抓取与放置的最小闭环¶

把上面几块串成一条可复现的链路。每一段单独判定，序列共 11 段：

段	通路	判据
open	夹爪动作	目标 0.04，读 `joint_states`
approach	采样规划 + 执行（位姿目标，工具朝下）	返回码成功、终点误差
insert	`/compute_cartesian_path` 直线下降 + 执行	`fraction > 0.99` 且执行成功
close	夹爪动作	闭合位置 ≥ 工件半宽
attach	`/apply_planning_scene`	`robot_state.attached_collision_objects` 写入成功
lift	直线上升 + 执行	`fraction`、执行返回码
transport	采样规划 + 执行（放置点上方）	返回码
place_down	直线下降 + 执行	`fraction`、放置到位误差
open2	夹爪动作（先张爪）	目标 0.04
detach	`/apply_planning_scene`（`operation=REMOVE`）	物体转为世界物体
retract	直线上升 + 执行	`fraction`、执行返回码

抓取几何在这里有两条硬关系。下降深度由末端几何定出：指尖中心在 panda_hand 下方 0.1034 m，预抓取位姿取工件中心上方 0.21 m，则插入段的位移是 0.21 − 0.1034 = 0.1066 m，落下后指尖正好跨在工件中心高度上。闭合位置必须不小于工件半宽——这一点在实做里踩到了：把闭合位置写成 0.02 而工件半宽 0.025，手指几何上插进工件体内，detach 之后该物体作为世界物体留在原地，于是抬起段的 fraction 直接掉下来（3 轮里插入段只过 1 轮、抬起段 2/3 通过）。同一条序列把闭合位置改成“半宽 + 3 mm”后，三轮全部通过：

trial0 O=(0.472, 0.031) 端到端=True 到位误差=(2.96 mm, 3.80°) 全 11 段 OK
trial1 O=(0.548, -0.018) 端到端=True 到位误差=(4.03 mm, 2.98°) 全 11 段 OK
trial2 O=(0.481, 0.045) 端到端=True 到位误差=(3.46 mm, 2.43°) 全 11 段 OK

第二处顺序错误同样关键：先张爪，再 detach，最后才 retract。若在夹爪仍闭合时就脱开，物体作为世界物体出现在手指之间，抬起即碰撞。放置后物体位置由 detach 请求里的位姿显式给定——在 mock 层这是唯一的“放置”定义方式，也意味着这条链路验的是几何与时序，不验工件是否真的留在桌上。第 10 章的端到端统计沿用了这套分段判据。

5. 放置精度与运动学约束¶

放置到位误差的中位是 3.5 mm、最大 4.0 mm（place_down 段之后，工具朝下、位置容差请求 5 mm）。三个来源决定了这个数字：

请求侧容差：位姿目标本身是区域（第 4 章的 PositionConstraint + OrientationConstraint），采样规划器只需落到区域内，不保证落在区域中心。
重定时与执行插值：轨迹在 100 Hz 的位置跟随下插值（第 1 章、第 7 章），末点保持误差取决于控制器。
手眼与基座变换：目标位姿来自感知，落在 panda_link0 还是 world 上取决于虚拟关节与手眼外参（第 2 章、camera-note 第 1 章）。

判定“放置精度够不够”应当在放下的那一刻测，而不是在整条任务结束后测——回退段的误差与放置无关，混在一起会把链路问题误读成精度问题。

运动学奇异是另一类上限。工具朝下、在桌面上方一定高度内横向伸手时，腕部三轴接近共线（腕部奇异），零空间里微小的位姿误差会被放大成大关节角变化；判据是可操作度（Yoshikawa 判据 \(w=\sqrt{\det(JJ^{\top})}\)，定义与推导见 robotics-note 第 4 章），工程做法是在抓取候选生成阶段就把低可操作度区域排除，而不是等规划失败。姿态表示与四元数约定带来的坑归 control-note 第 7 章。

6. 要点¶

感知侧交出的是“位姿 + 置信度”，抓取侧要立刻问两句：几何上抓得住吗（判据），碰撞上到得了吗（规划）；顺序先判据后规划。
抓取三判据都能自己算：跨度受行程上限约束（panda 样本 8 cm）、力闭合要求连线偏角不超过摩擦锥半角 \(\arctan\mu\)、深度与避让要求指尖形状匹配。
容差比直觉更紧：接触点横向偏移工件半宽的 40%，5 cm 立方的对置抓取就从可行变成破坏力闭合；\(\mu\) 从 0.5 降到 0.1 会把容许偏角从 26.6° 收到 5.7°。
夹爪的软件接口是动作 + 位置语义：镜像由 ros2_control 的 mimic 完成，到位判据取 joint_states 而不是结果体；reached_goal 在无物理层恒为真，“夹住了”这一层必须靠真实力矩/压力反馈判。
控制通路不兜底模型限位：越限指令照样执行并报告成功，之后所有规划的起点都会与模型不一致。
闭环的时序有两条硬规定：下降深度按指尖偏置（样本 0.1034 m）算，闭合位置不小于工件半宽；放置段必须“张爪 → 脱开 → 回退”。
放置精度在放下的那一刻测；奇异位形用可操作度在候选生成阶段排除，而不是等规划失败。

参考文献与延伸阅读¶

Mahler, J., Liang, J., Niyaz, S., et al. Dex-Net 2.0: Deep Learning to Plan Robust Grasps with Synthetic Point Clouds and Analytic Grasp Metrics. RSS, 2017. arXiv
Sundermeyer, M., Mousavian, A., Triebel, R., & Fox, D. Contact-GraspNet: Efficient 6-DoF Grasp Generation in Cluttered Scenes. ICRA, 2021. arXiv
MoveIt. 项目门户与文档入口. 文档

第 6 章 抓取与放置¶