第 5 章 任务级编排¶
一次规划请求解决的是“从这个位形到那个位姿”，而操作任务天然是分段的：接近、闭合、抬起、搬运、放下、松开。把这些段落串起来的机制就是任务级编排。本章讲 MoveIt Task Constructor（MTC，任务构造器）的 stage 模型——分解方式、解如何在 stage 之间传播、失败以什么形态暴露——并说明这套接口的形态边界：Python 绑定与 C++ 各自能承担到哪一步。以 ROS 2 Humble + MoveIt 2 2.5.10 + MTC 0.1.3、样本臂 panda 为例。
MTC 的 stage 组合与解传播方向：
flowchart LR
task["Task"] --> stages["Stage 组合<br/>Generator / Propagator / Connector"]
stages --> sol["Solutions<br/>解沿 stage 逐级传播"]
sol --> diag["失败计数与诊断"]1. 任务级分解的动机¶
单请求规划的边界在第 4 章已经清楚：目标是一组约束，起点是一个位形，中间由采样规划器搜索。操作任务里至少有三个东西塞不进这个模型：
- 目标本身是集合：抓取位姿由工件的对称性与夹爪方向决定，候选不止一个；选定哪个要等到知道“能不能到达”之后。
- 中间状态是任务语义的一部分：抓取后物体要从世界物体变成机器人的一部分（attach，第 3 章），此后所有碰撞查询都带着它。这不是一个位姿约束能表达的。
- 失败要在段落粒度上报：整条轨迹规划失败与“接近段可达但抬起段撞桌”是不同的处置——前者换预算重试，后者换抓取位姿。
MTC 的答案是把任务显式建成stage 图：每个 stage 只负责一小段语义，stage 之间通过解的接口（interface states）传递候选位形，规划发生在相邻 stage 的候选之间。抓取 = approach + grasp + lift + place 的分解正是这套模型的典型用法。
2. stage 的类型谱系与属性接口¶
MTC 的 stage 分三类：生成器（generator，产出候选状态而不规划）、连接器（connect，在两个接口状态之间规划轨迹）、容器（container，把若干 stage 组合成一个语义单元）。Python 绑定里可直接枚举（from moveit.task_constructor import core, stages）：
CurrentState FixedState GeneratePose GenerateGraspPose GeneratePlacePose
GenerateRandomPose ComputeIK Connect MoveTo MoveRelative ModifyPlanningScene
Pick Place SimpleGrasp SimpleUnGrasp FixCollisionObjects ← stage 类
Merger Alternatives Fallbacks SerialContainer ParallelContainerBase ← 容器类
PipelinePlanner JointInterpolationPlanner CartesianPath MultiPlanner ← 规划器类
PathLength DistanceToReference Clearance LinkMotion TrajectoryDuration ← 代价项
四类值得单独认：Pick / Place / SimpleGrasp 是把“接近 + 闭合 + attach”这种成段的语义打包好的容器；ModifyPlanningScene 提供 addObject / removeObject / attachObject / detachObject / allowCollisions，即在任务图内部修改场景（第 3 章的场景编辑在这里成为 stage 的一部分）；FixCollisionObjects 专门处理“物体与机器人初始就相交”的情形；末尾一列是代价项（cost term），挂在 stage 上参与解的排序。
每个 stage 的行为由属性表驱动，Python 侧属性名目直接可见：
ik = stages.ComputeIK("compute IK", generator) # 包裹一个位姿生成器
ik.group = "panda_arm" # 用组配置的 IK 求解器
ik.ik_frame = pose_in_hand # 目标位姿的度量点（末端上的偏置帧）
ik.max_ik_solutions = 4 # 限制每个位姿产出的关节解数量
ik.ignore_collisions = False # 候选解是否要过碰撞检查
props = ik.properties
props.configureInitFrom(core.Stage.PropertyInitializerSource.INTERFACE, ["target_pose"])
最后两行是 MTC 的核心机制：属性可以从父 stage（PARENT）或接口状态（INTERFACE）继承，而不是硬写。target_pose 从接口继承意味着这个 stage 不必知道自己的目标位姿——它由上游生成器产出的每个候选决定。这一层设计正是“解的传播”的实现方式：生成器产出候选，下游 stage 对每个候选分别计算，连接器负责把两侧候选两两连起来。
3. 解的传播与失败计数¶
stage 图的输出不是一条轨迹，而是每个 stage 的候选集与统计量。C++ demo（moveit_task_constructor_demo 的 pick & place，经 run.launch.py 提供参数）跑起来后，/statistics 话题（moveit_task_constructor_msgs/msg/TaskStatistics）给出的形态是：
每 stage 的 (solved 数, failed 数, 累计计算时间)：
id=2 (1, 0, 0.014 s) id=6 (74, 0, 5.418 s)
id=3 (1, 0, 0.014 s) id=7 (75, 22, 1.402 s)
id=4 (1, 0, 0.019 s) id=24 (402, 209, 9.835 s)
id=5 (0, 73, 3.353 s) id=25 (0, 531, 19.993 s)
三个可判读的点。其一，生成器与连接器的计数含义不同：id=2/3/4 各产出 1 个解（当前状态、场景修改一类），而 id=6/7 的几十个解是候选位姿与候选关节解；id=5 有 73 次失败 0 个解，说明它是尝试把两侧连起来却连不上的连接器。其二，失败计数集中在连接器（id=25 失败 531 次、累计 20 s），这是任务级规划的主要算力去向，也是排查的第一目标。其三，候选多不等于有解：这一轮里两侧各自有大量候选，但 /solution 上没有产出完整解。
完整解缺席的原因要从 move_group 那一侧读：连接阶段返回“计算出的路径含非法状态（37 个点中的第 8 个）”，并列出接触对 cube（物体）× panda_link5（机器人 link）。即生成器给出的抓取候选本身让手臂与工件相交，连接器在两侧候选之间找不到合法路径。处置顺序由此确定：先查候选（生成器的抓取位姿与 IK 帧偏置），再查连接（预算与路径约束），而不是一上来加大规划时间。
这套统计是 MTC 最有价值的可观测面：StageStatistics.solved / failed / total_compute_time 三个量把“任务在哪一步卡住、算力花在哪”变成可数的指标。第 10 章的端到端统计要按段拆解，用的就是它。
4. 接口形态的实际边界¶
MTC 的 Python 绑定（随 moveit-task-constructor-core 提供的 moveit.task_constructor）可以导入、枚举类型、构造 stage 对象并设置属性，但跑不完整条规划链路：task.loadRobotModel(node) 会失败，任务拿不到机器人模型，plan() 无从谈起。
原因在依赖侧而非 MTC 侧：这些绑定需要一个真实的 rclcpp 节点来订阅 /robot_description 与规划管线，而 Humble 的 rclcpp Python 扩展（rclcpp.cpython-310-*.so）只暴露 init / shutdown 与 Node，且 Node 除 name 之外没有可用成员。因此第 4 章与第 6 章的规划链路一律走 rclpy + 动作接口，MTC 的链路走 C++；Python 教程脚本（pickplace.py 一类）还额外依赖 moveit_commander，Humble 的二进制包里没有提供。
由此界定本章的取材范围：stage 谱系与属性名目取自 Python 绑定的直接枚举，可靠；stage 的组装顺序与容器嵌套语义取自 C++ demo 与其 Python 教程脚本的源码结构，行为数字以 C++ 侧统计为准；Pick / Place 容器的内部动作序列（接近距离、抬起方向、抓取后 attach 的挂点）在 C++ 侧由 setApproachMotion / setLiftMotion / setIKFrame 配置，Python 教程里同一组调用的签名一致，其具体执行结果属外部实现、不在本地断言。
5. 段落级的组合、回退与代价¶
stage 图不止是顺序串联。四类组合方式对应四种工程需求：
| 组合 | 语义 | 用途 |
|---|---|---|
Alternatives |
并列的多个候选，各产出解并参与排序 | 多个抓取位姿、多条入口路径 |
Fallbacks |
按顺序尝试，前面的失败才走后面的 | 先试笛卡尔直线抽出，失败退回采样规划 |
Merger |
两臂分别规划后再合并（MergeMode 控制合并策略） |
双臂或“先摆臂再动手”的分治 |
SerialContainer / ParallelContainerBase |
自定义段落结构 | 把带场景修改的子序列当作一个 stage |
代价项（PathLength、DistanceToReference、Clearance、LinkMotion、TrajectoryDuration）挂在 stage 上，通过 setCostTerm 生效。选择代价项就是在定义“什么算好解”：抓取侧常用 Clearance（离障碍越远越好）与 PathLength（省节拍），恢复侧常用 DistanceToReference（回到参考位形的偏差）。
回退与重规划在任务层也有分工。MTC 内部的重规划靠 Fallbacks 与生成器的候选顺序；跨任务的重规划（场景变了、物体掉了）则由外部决定——这也是与行为树的边界：MTC 负责“一个任务内部的分解与候选搜索”，行为树负责“跨任务的状态机与故障处置策略”（导航侧的行为树实践归 ROS2-note 第 10 章）。把 attach/detach 的时序写进行为树、把抓取位姿搜索写进 MTC，是常见的正确切分。
6. 编排与生成—测试式规划的关系¶
MTC 的结构是“先生成候选集合，再检验可达性”，属于生成—测试（generate-and-test）式规划：生成器廉价地产出大量候选，连接器用昂贵的规划逐个检验。这决定了两个实践结论。
第一，候选的数量与质量决定搜索代价。max_ik_solutions、angle_delta（抓取位姿的角度步长）这类参数直接决定候选规模；第 3 节里 id=25 的 531 次失败正是候选规模乘上连接代价的结果。收紧生成器（只产出符合夹爪几何与 clearance 的候选）通常比放宽规划器预算更有效。
第二，被检验的是“可达性”而不是“可抓取性”。连接器判断的只有碰撞与限位；抓取是否真的成立（力闭合、接触摩擦、指尖包络）不属于规划器，它的判据在第 6 章给出。把这两件事混在一起，会得到“规划成功但抓取失败”却无从归因的局面——这也是第 10 章端到端统计必须分段的原因。
算法层面的完备性、概率完备性等概念归 robotics-note 第 8、9 章；此处只需要工程结论：任务级编排改变的是候选的组织方式与失败的定位粒度，不改变单段规划的算法性质。
7. 要点¶
- 任务级编排解决的是单请求模型装不下的三件事：目标是候选集合、中间状态带任务语义（attach）、失败要在段落粒度定位。
- stage 分生成器 / 连接器 / 容器三类；属性可从父 stage 或接口状态继承（
PARENT/INTERFACE），继承机制就是解传播的实现方式。 - 统计量
StageStatistics的solved/failed/total_compute_time是首要可观测面：C++ demo 一轮里连接器失败 531 次、累计 20 s，而完整解为零——瓶颈在连接而不在生成。 - 无完整解时先读连接器的非法状态报告（本例为物体与
panda_link5接触），据此收紧抓取候选，而不是加大规划预算。 - 接口形态边界：MTC 的 Python 绑定可导入、可枚举、可构造 stage，但
loadRobotModel依赖的rclcppPython 节点只是 stub，完整链路需走 C++；行为数字以 C++ 侧为准。 - 组合方式对应四种需求：
Alternatives（并列候选）、Fallbacks（顺序回退）、Merger（分治合并）、容器（自定义段落）；代价项定义“什么算好解”。 - 生成—测试结构决定了：收紧候选比放宽预算有效；规划器只检验可达性，可抓取性另有色（第 6 章）。