Part IV · 第 14 章运动规划 · 14.4Part IV · Ch. 14 Motion Planning · 14.4

任务与运动规划 (TAMP)Task & Motion Planning

“把杯子放进托盘”听起来是一步，可如果杯子前面挡着一个盒子、托盘里还剩不下多少地方，机器人就得先决定做哪些事、按什么顺序（任务规划），再为每一步选出具体的抓取、摆放位置和无碰撞轨迹（运动规划），而这两层互相牵制。本节先讲符号规划（STRIPS / PDDL 在谓词上的搜索），再看几何怎样让纯符号的计划失效，然后亲手运行采样器、可行性检查、回溯，以及“规划—绑定—学习”的完整循环，最后用逻辑-几何规划（LGP）把连续参数放进一个优化问题里联合求解。 "Put the cup in the tray" sounds like one step, but if a box stands in front of the cup and the tray is nearly full, the robot must decide which actions to take and in what order (task planning) and also choose a concrete grasp, placement and collision-free trajectory for each one (motion planning), and the two levels constrain each other. This lesson starts with symbolic planning (STRIPS / PDDL search over predicates), shows how geometry breaks purely symbolic plans, lets you run samplers, feasibility checks, backtracking and the full plan-bind-learn loop yourself, and ends with Logic-Geometric Programming (LGP), which solves for the continuous parameters jointly in one optimization problem.

约 40 分钟~40 min 前置：14.1 图搜索与 A* · 14.2 采样式规划 · 14.3 轨迹优化 · 7.5 逆运动学Prereqs: 14.1 Graph search & A* · 14.2 Sampling-based planning · 14.3 Trajectory optimization · 7.5 Inverse kinematics 6 个交互6 interactives

1为什么机器人需要它Why robots need this

14.1 到 14.3 节回答的都是同一个问题：已知起点构型和终点构型，找一条不碰撞的路径。可是让机器人“收拾桌子”“装洗碗机”“把零件装进夹具”时，终点构型本身并不知道。机器人要先想清楚做什么：先拿哪个物体、要不要先把挡路的东西挪开、东西放到哪儿；还要想清楚怎么做：从哪一侧抓、放在托盘的哪个位置、手臂沿什么轨迹过去。前者是离散的选择（动作的种类和顺序），后者是连续的选择（抓取位姿、摆放位姿、关节构型、轨迹）。

这两层不能分开解决。只看符号，“拿起杯子，放进托盘”是一个完美的两步计划；可杯子前面如果有个盒子挡住了夹爪的来路，第一步在几何上就做不到。反过来，只看几何，运动规划器根本不知道“先挪盒子”这种动作该不该做。任务与运动规划（Task and Motion Planning，TAMP）就是把两层放到同一个搜索里：符号层提出动作序列，几何层为每个动作找具体参数并检查可行性，失败的信息再反馈给符号层。

本节的路线：先用积木世界讲清楚符号规划（第 2 节 FIG 1），再看几何怎样让它失效（FIG 2）；第 3 节运行一个完整的 TAMP 循环（FIG 3）；第 4 节拆开循环里的两个零件：采样器（FIG 4）和回溯（FIG 5）；第 5 节换一个视角，把连续参数交给优化（LGP，FIG 6）。

Lessons 14.1 to 14.3 all answer the same question: given a start configuration and a goal configuration, find a collision-free path. But when a robot is asked to "clear the table", "load the dishwasher" or "put the part into the fixture", the goal configuration itself is unknown. The robot must first work out what to do: which object to take first, whether to move something out of the way, where to put things; and also how to do it: which side to grasp from, where in the tray to place, along which trajectory to move the arm. The first is a discrete choice (the kinds and order of actions), the second a continuous one (grasp poses, placement poses, joint configurations, trajectories).

The two cannot be solved separately. Looking only at symbols, "pick up the cup, put it in the tray" is a perfect two-step plan; but if a box in front of the cup blocks the gripper's way in, the first step is geometrically impossible. Conversely, looking only at geometry, a motion planner has no idea whether an action like "move the box first" is worth taking. Task and Motion Planning (TAMP) puts both levels into one search: the symbolic level proposes action sequences, the geometric level looks for concrete parameters for each action and checks feasibility, and what it learns from failures goes back to the symbolic level.

The route of this lesson: symbolic planning made clear in the blocks world (Section 2, FIG 1), then how geometry breaks it (FIG 2); in Section 3 a complete TAMP loop (FIG 3); Section 4 takes two parts of the loop apart: samplers (FIG 4) and backtracking (FIG 5); Section 5 changes the point of view and hands the continuous parameters to optimization (LGP, FIG 6).

2直觉：符号规划，以及它看不见的东西Intuition: symbolic planning, and what it cannot see

世界 = 一组为真的命题The world as a set of true statements

经典的符号规划（STRIPS，1971）把世界描述成一组为真的谓词：on(C,A)（C 在 A 上）、ontable(A)、clear(B)（B 上面没东西）、handempty（手是空的）。没写出来的命题就当作假。每个动作有三张表：前提（执行前必须为真的命题）、添加（执行后变真的）、删除（执行后变假的）。例如 unstack(C,A) 的前提是 on(C,A)、clear(C)、handempty，它添加 holding(C) 和 clear(A)，删除 on(C,A)、clear(C)、handempty。

这样一来，规划就是 14.1 节的图搜索：节点是状态（命题集合），边是可执行的动作，目标是包含所有目标命题的任一状态。BFS 给出最短计划，A* 用启发式少展开一些状态。下图是经典的积木世界，左边是当前状态，右边是目标。你可以点下面的动作按钮自己动手（只有前提满足的动作才会出现），也可以让 BFS 或 A* 来规划。

Classical symbolic planning (STRIPS, 1971) describes the world as a set of true predicates: on(C,A) (C is on A), ontable(A), clear(B) (nothing on B), handempty. Anything not listed is taken to be false. Every action has three lists: preconditions (statements that must hold before it runs), add (statements that become true) and delete (statements that become false). For example unstack(C,A) requires on(C,A), clear(C) and handempty; it adds holding(C) and clear(A) and deletes on(C,A), clear(C) and handempty.

Planning is then the graph search of Lesson 14.1: nodes are states (sets of statements), edges are applicable actions, and the goal is any state containing all goal statements. BFS returns a shortest plan; A* uses a heuristic to expand fewer states. Below is the classic blocks world, the current state on the left and the goal on the right. Click the action buttons to act yourself (only actions whose preconditions hold are offered), or let BFS or A* plan.

FIG 1积木世界里的 STRIPS：状态是命题集合，动作改写命题STRIPS in the blocks world: states are sets of statements, actions rewrite them

左：当前状态（侧视，灰色横线是桌面，右上角是夹爪）；右：目标状态，目标满足时标题旁出现 ✓。下方第一行是当前状态里所有为真的命题，属于目标的命题带绿框；第二行是目标命题，未满足的带红色虚线框。第三行是上一个动作的三张表：蓝框为前提，绿框为添加，红色删除线为删除。“BFS / A*”从当前状态规划；“展开”是从队列里取出并扩展的状态个数，“可达”是从初始状态出发能到达的全部状态数。A* 的启发式是“还没满足的目标命题个数”。 Left: the current state (side view; the plain grey line is the table, the gripper sits top right); right: the goal state, with a ✓ by its title once the goal holds. Below, the first row lists every true statement of the current state, those belonging to the goal framed in green; the second row lists the goal statements, unmet ones with a dashed red frame. The third row shows the three lists of the last action: blue frames are preconditions, green frames the add list, red strike-through the delete list. "BFS / A*" plan from the current state; "expanded" counts states taken off the queue and expanded, "reachable" is the number of all states reachable from the initial one. The A* heuristic is "the number of goal statements not yet true".

试一试Try this

在“Sussman 异常”里只用动作按钮（不用规划器的“执行”）把积木摆成目标。最少要 6 个动作；先把 A 放到 B 上的人会发现，接下来为了把 B 放到 C 上又得把 A 拿下来。In "Sussman anomaly", reach the goal using only the action buttons (not the planner's "Execute"). The minimum is 6 actions; whoever puts A on B first will find that A has to come off again so that B can go onto C.
切到“四块积木”，在初始状态分别按“BFS”和“A*”：两者都给出 8 步的计划，但 BFS 展开了 86 个状态（可达状态共 125 个），A* 只展开 32 个。Switch to "four blocks" and, from the initial state, press "BFS" and then "A*": both return an 8-action plan, but BFS expands 86 states (of 125 reachable) and A* only 32.

符号层擅长什么What the symbolic level is good at

“Sussman 异常”说明了为什么要搜索整个动作序列，而不是一个子目标一个子目标地完成：目标 on(A,B) 和 on(B,C) 互相干扰，先完成任何一个都得在之后拆掉它。符号层用几十个状态就理清了这种顺序上的依赖，它不需要知道积木在哪个坐标、有多大。这既是它的力量，也是它的盲区。

The "Sussman anomaly" shows why one must search over whole action sequences instead of achieving one subgoal after another: the goals on(A,B) and on(B,C) interfere, and whichever is achieved first has to be undone later. The symbolic level sorts out such ordering dependencies with a few dozen states, and it never needs to know where the blocks are or how big they are. That is both its strength and its blind spot.

几何让符号计划失效Geometry breaks symbolic plans

换一个更像真实机器人的场景：俯视一个三面有墙、只能从前面伸手进去的货架。目标是把琥珀色的物体 T 放进右边的托盘。符号模型里只有 on(T,shelf)、handempty 这类命题，pick(T) 的前提都满足，所以符号规划器满怀信心地给出两步计划 pick(T) → place(T,tray)。可是夹爪要从前面开口一路伸到 T 那里，它扫过的区域（下图的长条）如果碰到蓝色的 B，这一步在几何上就不可行。同样，托盘里的青色杯子 M 放在角落时 T 还放得下，放在中间时四周剩下的空隙都比 T 窄，place(T,tray) 找不到任何位置。

这两种失败符号层都看不见：它的命题里没有“谁挡着谁”，也没有“托盘还剩多少空间”。你当然可以把这些写成命题，但它们取决于连续的位置和尺寸，事先把所有可能的 blocks(x,y) 都算出来，等于把几何问题提前解了一遍。

Switch to a scene more like a real robot's: a top view of a shelf with walls on three sides that can only be reached from the front. The goal is to put the amber object T into the tray on the right. The symbolic model contains statements like on(T,shelf) and handempty; the preconditions of pick(T) all hold, so the symbolic planner confidently returns the two-step plan pick(T) → place(T,tray). But the gripper has to reach from the open front all the way to T, and if the region it sweeps (the long strip in the figure) touches the blue object B, that step is geometrically impossible. Likewise, with the cyan cup M in a corner of the tray T still fits, but with M in the middle every remaining gap is narrower than T and place(T,tray) finds no position at all.

The symbolic level sees neither failure: its statements say nothing about "who blocks whom" or "how much room is left in the tray". You could of course write these as statements too, but they depend on continuous positions and sizes; computing every possible blocks(x,y) in advance amounts to solving the geometric problem beforehand.

FIG 2符号上完美的计划，几何上可能做不到A symbolically perfect plan can be geometrically impossible

俯视图，单位 m。货架内部 60 × 40 cm，左、右、后三面是墙（粗线），只能从下方的开口伸进去。拖动 T、B（限制在货架里）和托盘中的杯子 M（限制在托盘里）。长条是夹爪抓 T 时扫过的区域：张开的两指从开口一直伸到 T 的中线，宽度是 T 的宽度加两侧各 2 cm 的手指；它与别的物体相交时变成红色并标出挡路的物体，否则是绿色。托盘里的绿色区域是 T 的中心可以放的位置（T 要完全在托盘内，并与 M 保持 1 cm 间隙）；没有这样的位置时托盘标成红色。托盘 20 × 20 cm，T 与 M 都是 8 × 8 cm。 Top view, in metres. The shelf interior is 60 × 40 cm with walls (thick lines) on the left, right and back; it can only be entered through the opening at the bottom. Drag T and B (kept inside the shelf) and the cup M (kept inside the tray). The strip is the region the gripper sweeps when grasping T: the two open fingers travel from the opening up to T's centre line, and the strip is as wide as T plus a 2 cm finger on each side; when it intersects another object it turns red and marks the blocking object, otherwise it is green. The green area in the tray shows where T's centre may go (T entirely inside the tray, with a 1 cm gap to M); when there is no such place the tray is marked red. The tray is 20 × 20 cm, T and M are both 8 × 8 cm.

挪开 B（或挪动 T），让夹爪的通道变成绿色：符号计划没有变，变的只是几何。Move B out of the way (or move T) until the gripper's strip turns green: the symbolic plan did not change, only the geometry did.
把杯子 M 拖到托盘中间，让 T 再也放不进去。M 的中心落在托盘正中约 6 × 6 cm 的方块里时，四周的空隙都只剩不到 9 cm（T 的 8 cm 加 1 cm 间隙），T 放不进去。Drag the cup M to the middle of the tray until T no longer fits. Once M's centre lies in a roughly 6 × 6 cm square at the tray's centre, every gap around it is narrower than 9 cm (T's 8 cm plus the 1 cm gap), so T cannot fit.

3动手：规划—绑定—学习的循环Play: the plan-bind-learn loop

TAMP 最常见的结构是一个循环，下面这个版本接近 Srivastava 等人（2014）的“接口层”和 PDDLStream 的做法：

规划：符号规划器（这里是 BFS）在当前已知的命题下找最短的动作序列，叫作计划骨架（plan skeleton）。骨架里的动作还缺连续参数：从哪儿抓、放在哪儿。
绑定：按顺序为骨架里每个动作找连续参数，并在几何上检查。pick 检查夹爪通道是否畅通；place 在目标区域里随机采样摆放位置，最多 40 次，取第一个不碰撞的。
学习：如果某一步失败，分析原因并写成新的命题，例如通道被 B 挡住就加入 blocks(B,T)，托盘因为 M 放不下就加入“M 还在托盘里时 T 放不进托盘”；带着新命题回到第 1 步。

新命题让符号规划器“看见”了它原本看不见的几何事实：pick(T) 多了一个前提“没有还在货架上的物体挡着 T”，于是最短计划变成先挪 B。下图从默认场景开始，按“单步”看循环一步步走，按“执行”看机器人照着找到的计划动起来。运行前可以拖动 T、B、C 和托盘里的 M 改变场景。

The most common TAMP structure is a loop; the version below is close to the "interface layer" of Srivastava et al. (2014) and to what PDDLStream does:

Plan: a symbolic planner (BFS here) finds the shortest action sequence given the statements known so far: the plan skeleton. The actions in a skeleton still lack their continuous parameters: where to grasp, where to put.
Bind: go through the skeleton in order, find continuous parameters for each action and check them geometrically. pick checks whether the gripper's way in is clear; place samples placements at random in the target region, at most 40 times, and takes the first collision-free one.
Learn: if a step fails, diagnose why and write it as a new statement: a way in blocked by B adds blocks(B,T), a tray too full because of M adds "T does not fit in the tray while M is still in it"; then go back to step 1 with the new statements.

The new statements let the symbolic planner "see" geometric facts it was blind to: pick(T) gains the precondition "no object still on the shelf blocks T", so the shortest plan now moves B first. The figure starts from the default scene; press "Step" to walk through the loop, and "Execute" to watch the robot carry out the plan it found. Before running you can drag T, B, C and the cup M in the tray to change the scene.

FIG 3完整的 TAMP 循环：符号规划、几何绑定、从失败中学习The complete TAMP loop: symbolic planning, geometric binding, learning from failure

俯视图。左边是放挪开物体用的边桌，中间是只能从前面伸进去的货架，右边是托盘（里面有杯子 M）。目标只有一条：in(T,tray)。右侧（窄屏时在下方）的日志按迭代列出：符号规划器给出的骨架（BFS 展开的状态数）、每个动作的绑定结果（✓ 或 ✗）、学到的新命题（紫色）。单步时画面显示当前正在检查的几何状态：失败的通道画成红色，挡路的物体加红框；成功的通道画成绿色。每次绑定都从真实的初始场景重新开始模拟。摆放采样用固定随机种子，所以同一个场景每次结果相同。 Top view. On the left a side table for objects that get moved away, in the middle the shelf that can only be entered from the front, on the right the tray (holding the cup M). There is one goal: in(T,tray). The log on the right (below, on narrow screens) lists, per iteration: the skeleton returned by the symbolic planner (with the number of states BFS expanded), the binding result of each action (✓ or ✗) and the newly learned statements (purple). While stepping, the picture shows the geometric state being checked: a failed way in is drawn red with red frames around the blocking objects, a successful one green. Every binding attempt simulates again from the true initial scene. Placement sampling uses a fixed random seed, so the same scene always gives the same result.

在默认场景里一直按“单步”（或按“运行”）直到找到计划：第一个骨架因为 B 挡路失败，学到 blocks(B,T) 之后第二个骨架（4 个动作）成功。再按“执行”看一遍。In the default scene keep pressing "Step" (or "Run") until a plan is found: the first skeleton fails because B is in the way, and after learning blocks(B,T) the second skeleton (4 actions) succeeds. Then press "Execute" and watch.
改变场景，使最终计划需要 6 个动作。两种办法：把 C 拖到 B 正前方（于是 C 挡 B、B 挡 T），或者把 M 拖到托盘中间（于是要先把 M 挪走）。看日志里循环用了几次迭代、学到了什么。Change the scene so that the final plan needs 6 actions. Two ways: drag C right in front of B (so C blocks B and B blocks T), or drag M to the middle of the tray (so M must be moved out first). Read in the log how many iterations the loop needed and what it learned.

核心洞见Key insight

符号层负责“组合”：在离散的动作序列里搜索，几十个状态就能排好顺序。几何层负责“诚实”：它知道真实的尺寸和位置，能否定看似可行的步骤。两层之间的接口是失败时返回什么：只说“失败了”，符号层下次多半提出同一个骨架；说清楚“B 挡着 T”，符号层就能把它变成新的前提，绕开同一类失败。

The symbolic level handles the combinatorics: it searches over discrete action sequences and settles the order with a few dozen states. The geometric level keeps it honest: it knows the true sizes and positions and can veto steps that only look feasible. The interface between them is what comes back after a failure: a bare "failed" usually gets the same skeleton proposed again; "B blocks T" can be turned into a new precondition that steers around the whole class of failures.

4循环里的两个零件：采样器与回溯Two parts inside the loop: samplers and backtracking

采样器：产生连续参数，再检查可行性Samplers: propose continuous values, then check them

FIG 3 里的 pick 只做了一次通道检查，真实的机械臂要回答的问题多得多：从哪个方向抓？那个抓取位姿手臂够得着吗（逆运动学）？够得着的话，手臂和手指会不会撞到旁边的东西（碰撞检测）？TAMP 系统把这些拆成两类零件：采样器（生成候选的抓取方向、摆放位置）和检查器（逆运动学、碰撞检测、稳定性）。PDDLStream 把这样一对“生成 + 认证”叫作一个 stream：输入物体，输出抓取，并认证“这是 T 的一个有效抓取”这条命题。

下图是一个俯视的平面 3R 机械臂（7.5 节两连杆逆解再加一个腕关节）。夹爪可以从任意方向 $\varphi$ 去抓罐子。采样器在 $[0,2\pi)$ 上均匀随机地取 40 个方向，每个方向依次检查：腕部点在两连杆的工作空间里吗（不在就是琥珀色“够不着”）？两种肘部解里有没有一个不碰撞（都碰就是红色“碰撞”）？剩下的是绿色“可行”。

The pick of FIG 3 made a single check of the way in; a real arm must answer far more: from which direction to grasp? Can the arm reach that grasp pose (inverse kinematics)? If so, do the arm and fingers hit anything nearby (collision checking)? TAMP systems split this into two kinds of parts: samplers (generating candidate grasp directions, placements) and checkers (inverse kinematics, collision checking, stability). PDDLStream calls such a "generate + certify" pair a stream: given an object it outputs grasps and certifies the statement "this is a valid grasp of T".

The figure shows a planar 3R arm seen from above (the two-link inverse solution of Lesson 7.5 plus a wrist joint). The gripper may grasp the can from any direction $\varphi$. The sampler draws 40 directions uniformly at random from $[0,2\pi)$ and checks each in turn: is the wrist point inside the two-link workspace (if not: amber, "out of reach")? Is at least one of the two elbow solutions collision-free (if both collide: red, "collision")? The rest are green, "feasible".

FIG 4抓取采样器：逆运动学与碰撞检测筛掉大部分候选A grasp sampler: inverse kinematics and collision checks reject most candidates

俯视图，单位 m。机械臂基座在原点，连杆长 36 cm、30 cm，腕到罐心 10 cm；罐子半径 3.5 cm。可拖动罐子和两个障碍物（障碍物不能压到罐子上）。罐子周围一圈小点是 40 个采样方向的结果：绿 = 可行，琥珀 = 够不着，红 = 碰撞；每个点画在夹爪伸过来的那一侧。青色虚线圆是两连杆能到达的最远距离 66 cm：腕部点必须在圆内。滑块选一个方向 $\varphi$，画出对应的手臂（可行时画成蓝色，碰撞时画成红色，够不着时只画出腕部目标 ✕）。“密集估计”用 720 个等间距方向估计各类所占的比例 $p$。 Top view, in metres. The arm base is at the origin, the links are 36 cm and 30 cm long, and the wrist is 10 cm from the can's centre; the can's radius is 3.5 cm. Drag the can and the two obstacles (obstacles cannot be pushed onto the can). The ring of small dots around the can shows the 40 sampled directions: green = feasible, amber = out of reach, red = collision; each dot sits on the side the gripper comes from. The cyan dashed circle is the farthest the two links reach, 66 cm: the wrist point must lie inside it. The slider picks one direction $\varphi$ and draws the arm for it (blue when feasible, red when colliding, only the wrist target ✕ when out of reach). The "dense estimate" uses 720 evenly spaced directions to estimate the share $p$ of each class.

转动滑块 $\varphi$，找一个手臂够得着、但会撞到障碍物的方向（手臂变红）。逆运动学有解不等于这个抓取能用。Turn the slider $\varphi$ to a direction the arm can reach but where it hits an obstacle (the arm turns red). An inverse-kinematics solution does not make a grasp usable.
拖动障碍物把罐子围起来，让可行比例 $p$ 降到 25% 以下但不为 0：采样器平均要试 $1/p\ge4$ 次才碰到一个可行抓取。（例如把下方的窄障碍放到罐子和基座之间。）Drag the obstacles to hem the can in until the feasible share $p$ drops below 25% but not to 0: on average the sampler now needs $1/p\ge4$ tries to hit one feasible grasp. (For example, put the narrow obstacle between the can and the base.)

回溯：前面的连续选择会卡死后面的步骤Backtracking: earlier continuous choices can doom later steps

有了采样器，最简单的绑定方法是贪心：按顺序给每个动作取第一个可行的参数，不再回头。这在动作彼此无关时没问题，可 TAMP 里的动作几乎总是相关的：第一个盒子放在哪儿，决定了第二、第三个盒子还剩多少地方。下图把三个宽 12、20、15 cm 的盒子依次放到一段隔板上，隔板长度 = 盒宽之和 + 余量。每个盒子最多采样 $K$ 次。回溯（深度优先搜索）在某一层用完 $K$ 次也找不到位置时，退回上一层换一个位置再试。

两种采样器：拒绝采样在整段隔板上均匀取位置，撞到已有的盒子就作废；自由空间采样只在还空着、放得下的位置里取，从不作废。注意第二种已经是“完美”的单步采样器，但贪心时它仍然会失败：前两个盒子落在中间，把剩下的空间切成两段，每段都比第三个盒子窄。

With samplers in hand, the simplest binding method is greedy: give each action, in order, the first feasible parameter value and never look back. That is fine when the actions are independent, but in TAMP they almost never are: where the first box goes decides how much room is left for the second and third. In the figure three boxes, 12, 20 and 15 cm wide, are placed one after another on a shelf board whose length is the sum of their widths plus some slack. Each box gets at most $K$ samples. Backtracking (depth-first search) returns to the previous level and tries another position there whenever a level runs out of its $K$ samples.

Two samplers: rejection sampling draws positions uniformly along the whole board and discards any that hit a box already placed; free-space sampling draws only among positions that are still free and wide enough, so it never discards. The second is already a "perfect" one-step sampler, yet greedy binding still fails with it: when the first two boxes land in the middle, they cut the remaining space into two pieces that are each narrower than the third box.

FIG 5贪心与回溯：在连续参数上的深度优先搜索Greedy versus backtracking: depth-first search over continuous parameters

上：侧视的隔板（灰色横线），已放下的盒子按层编号；刚被拒绝的采样画成红色虚线框。下：搜索树，横轴是采样的先后顺序，第 $k$ 行是第 $k$ 个盒子的采样，线连到它的父节点（上一个盒子的位置）。红 ✕ = 碰撞被拒，琥珀 = 当前路径上，绿 = 最终成功的路径，灰点 = 被回溯放弃的位置。统计读数在当前设置下对两种方法各跑 400 个随机种子。 Top: the shelf board in side view (plain grey line) with the boxes placed so far, numbered by level; the sample just rejected is drawn as a dashed red frame. Bottom: the search tree, with sampling order along the horizontal axis; row $k$ holds the samples for box $k$, each joined to its parent (the position of the previous box). Red ✕ = rejected by collision, amber = on the current path, green = the final successful path, grey dots = positions abandoned by backtracking. The statistics readout runs 400 random seeds at the current settings for each method.

用默认设置选“贪心”并运行：失败（换种子直到失败也行）。不换种子，改选“回溯”再运行：同一组随机数下，退回去换掉前面盒子的位置之后成功了。With the default settings choose "greedy" and run: it fails (if it does not, change the seed until it does). Keep the seed, choose "backtracking" and run again: with the same random numbers, going back and changing an earlier box's position leads to success.
保持自由空间采样，把余量降到 5 cm 或更小：统计里贪心的成功率跌到 40% 以下，而回溯的成功率仍是贪心的两倍以上。Keep free-space sampling and lower the slack to 5 cm or less: in the statistics greedy's success rate falls below 40%, while backtracking's stays more than twice as high.

一个想法，几种看法One idea, several views

约束满足：固定一个骨架之后，找参数就是一个约束满足问题（CSP），变量是各个动作的连续参数，约束是逆运动学、碰撞、区域；回溯是 CSP 的标准解法。搜索：FIG 5 下半部分就是一棵搜索树，和 FIG 1 的状态图一样，只是分支来自采样而不是有限的动作表。概率：若第 $k$ 步一次采样成功的概率是 $p_k$，平均要试 $1/p_k$ 次（几何分布，4.9 节的拒绝采样）；但各步的 $p_k$ 依赖前面的选择，所以“各步相乘”只在各步独立时成立。

Constraint satisfaction: once a skeleton is fixed, finding parameters is a constraint satisfaction problem (CSP) whose variables are the actions' continuous parameters and whose constraints are inverse kinematics, collision and regions; backtracking is the textbook CSP method. Search: the lower half of FIG 5 is a search tree just like the state graph of FIG 1, except that branches come from sampling rather than from a finite action list. Probability: if one sample at step $k$ succeeds with probability $p_k$, it takes $1/p_k$ tries on average (a geometric distribution; rejection sampling from Lesson 4.9); but each $p_k$ depends on the earlier choices, so "multiply the steps" is only valid when the steps are independent.

5另一个视角：用优化代替采样（逻辑-几何规划）Another view: optimization instead of sampling (Logic-Geometric Programming)

采样加回溯是“先猜、再检查、不行就退”。Toussaint（2015）提出的逻辑-几何规划（LGP）换了个思路：骨架一旦定下来，所有连续参数（各个关键时刻的构型、抓取、摆放）就组成一个非线性规划，可以一次联合求解，让后面动作的约束直接影响前面的选择。外层仍是对骨架的符号搜索，内层是 14.3 节那样的优化。

下面的侧视例子：一根 30 cm 的棒子平放在桌上，要插进左边架子上的槽里，至少插进 $d_{\min}$。夹爪必须留在槽外，所以只能抓棒子靠外的那一端。夹爪从左上方出发，贪心地选“离我最近”的抓取点，就会抓到左端，而插槽要求抓右边，插入那一步无解。联合优化同时看见两步的约束，直接选出可以插入的抓取。如果棒子右端被一根横梁盖住，直接插入的骨架就不可行，LGP 只能换一个更长的骨架：先放到右边空地上，换个位置重新抓，再插入。

为了能把整个可行集画出来，这里只用“关键帧”层次：变量是各个关键时刻夹爪的位置，代价是相邻关键帧之间距离的平方和（起点 → 抓取 → … → 回到起点），约束都是线性的，用增广拉格朗日法（6.7 节）求解。完整的 LGP 还会在关键帧之间优化整条轨迹。

Sampling plus backtracking means "guess, check, step back if it fails". The Logic-Geometric Programming (LGP) of Toussaint (2015) takes another approach: once a skeleton is fixed, all its continuous parameters (the configurations at the key moments, the grasps, the placements) form one nonlinear program that can be solved jointly, so the constraints of later actions act directly on earlier choices. The outer level is still a symbolic search over skeletons; the inner level is optimization as in Lesson 14.3.

The side-view example below: a 30 cm rod lies on the table and must be inserted into the slot of the rack on the left, at least $d_{\min}$ deep. The gripper must stay outside the slot, so it can only hold the rod near its outer end. The gripper starts at the top left; choosing greedily the grasp "closest to me" picks the rod's left end, but insertion needs a grasp on the right, and the insertion step has no solution. Joint optimization sees the constraints of both steps at once and picks a grasp that allows insertion directly. If a crossbar covers the right end of the rod, the direct skeleton is infeasible and LGP has to move to a longer skeleton: put the rod down on the free area on the right, grasp it again elsewhere, then insert.

So that the whole feasible set can be drawn, only the "keyframe" level is used here: the variables are the gripper positions at the key moments, the cost is the sum of squared distances between consecutive keyframes (start → grasp → … → back to the start), all constraints are linear, and the augmented Lagrangian method (Lesson 6.7) solves it. A full LGP would also optimize the whole trajectory between keyframes.

FIG 6LGP：在骨架之内联合优化关键帧，在骨架之间做符号搜索LGP: optimize the keyframes jointly within a skeleton, search symbolically across skeletons

左：侧视场景，单位 m。左边是带槽的架子（开口在 x = 0.20 m，槽深 32 cm），中间是桌上的棒子，带斜线的横梁盖住它右端的一部分，右边虚线括号是可以临时放下棒子的空地；$\mathsf S$ 是夹爪的起点和终点。算出的解里，各关键帧的夹爪画成琥珀色并编号，青色虚线是关键帧之间的连线，琥珀色轮廓是棒子在各关键帧的位置。右：直接插入骨架的两个变量平面，横轴是抓取点相对棒子中心的偏移 $s$，纵轴是插入时棒子中心的位置 $p$；绿色多边形是可行集，蓝色椭圆是代价等高线，琥珀色折线是增广拉格朗日法每次外层迭代后的位置，紫色竖虚线是贪心选的抓取 $s$。 Left: the side-view scene, in metres. On the left the rack with the slot (opening at x = 0.20 m, 32 cm deep), in the middle the rod on the table, with a hatched crossbar covering part of its right end; the dashed bracket on the right is free space where the rod can be put down for a while; $\mathsf S$ is where the gripper starts and ends. In a solution, the gripper at each keyframe is drawn in amber and numbered, cyan dashed lines join consecutive keyframes, and amber outlines show the rod at each keyframe. Right: the plane of the two variables of the direct-insertion skeleton, the grasp offset $s$ from the rod's centre horizontally and the rod's centre position $p$ at insertion vertically; the green polygon is the feasible set, blue ellipses are cost contours, the amber polyline is where the augmented Lagrangian method stands after each outer iteration, and the purple dashed vertical line is the grasp $s$ chosen greedily.

默认设置下先按“贪心逐步”：抓在 $s=-13$ cm（离起点最近的一端），插入那一步无解。再按“联合优化”：增广拉格朗日法收敛到 $s=7$ cm、$p=17$ cm，约束违反小于 $10^{-6}$ m。With the defaults press "Greedy, step by step" first: it grasps at $s=-13$ cm (the end nearest the start) and the insertion step has no solution. Then press "Joint optimization": the augmented Lagrangian method converges to $s=7$ cm, $p=17$ cm with a constraint violation below $10^{-6}$ m.
把横梁覆盖比例加到 0.25 或以上，再按“LGP 搜索”：直接插入的骨架不可行（可行集消失，约束违反停在一个正数上，f = 0.25 时是 5 mm），搜索转向 4 个动作的“放下再抓”骨架并找到解。Raise the crossbar's coverage to 0.25 or more and press "LGP search": the direct-insertion skeleton is infeasible (the feasible set vanishes, the violation stalls at a positive value, 5 mm at f = 0.25) and the search moves on to the 4-action "put down and regrasp" skeleton and solves it.

采样还是优化？Sampling or optimization?

采样器把每个约束当成黑盒，只要能检查就能用，组合起来也简单；代价是参数之间的耦合只能靠回溯慢慢发现。优化把整个骨架的约束一起交给求解器，耦合直接体现在梯度里，解也更平滑；代价是需要可微的约束和好的初值，而且非凸问题上求解器失败并不能证明骨架不可行。FIG 6 能准确判断“不可行”，只是因为这里的约束全是线性的。

Samplers treat every constraint as a black box: anything that can be checked can be used, and combining them is easy; the price is that couplings between parameters are only discovered slowly, through backtracking. Optimization hands the constraints of the whole skeleton to a solver at once, so couplings show up directly in the gradients and the solutions are smoother; the price is the need for differentiable constraints and a good initial guess, and on a nonconvex problem a solver failure does not prove the skeleton infeasible. FIG 6 can declare "infeasible" with certainty only because all its constraints are linear.

6再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，图中对应的元素会被高亮：FIG 1 的前提 / 添加 / 删除命题，FIG 2 的夹爪通道和托盘可行区，FIG 4 的够不着与碰撞的采样点，FIG 3 日志里的骨架，FIG 5 已放下的盒子位置，FIG 6 的关键帧和可行集。Tip: hover the highlighted symbols in the formulas and the matching elements light up: the precondition / add / delete statements in FIG 1, the gripper strip and the tray's feasible area in FIG 2, the out-of-reach and colliding samples in FIG 4, the skeletons in the FIG 3 log, the placed box positions in FIG 5, and the keyframes and feasible set in FIG 6.

STRIPS 与 PDDLSTRIPS and PDDL

给定一个有限的命题集合 $\mathcal P$，状态 $s\subseteq\mathcal P$ 是为真的命题（没列出的为假，即“封闭世界假设”）。动作 $a$ 由三个集合描述，可执行条件与转移函数是

Given a finite set of statements $\mathcal P$, a state $s\subseteq\mathcal P$ is the set of statements that are true (anything not listed is false: the "closed-world assumption"). An action $a$ is described by three sets; when it applies and what it does are

$$ a \text{ applicable in } s \iff \hl{pre}{\mathrm{pre}(a)}\subseteq s,\qquad \gamma(s,a)=\big(s\setminus\hl{del}{\mathrm{del}(a)}\big)\cup\hl{add}{\mathrm{add}(a)} $$

计划是一串动作 $a_1,\dots,a_n$，从初始状态 $s_0$ 依次可执行，并且最终状态包含目标 $G$：$G\subseteq\gamma(\cdots\gamma(s_0,a_1)\cdots,a_n)$。实际中动作写成带参数的模式，规划器把参数换成具体物体得到“落地”动作。PDDL（规划领域定义语言，1998）是描述它们的标准语言，FIG 1 的 stack 写出来是：

A plan is a sequence of actions $a_1,\dots,a_n$ that is applicable in turn from the initial state $s_0$ and ends in a state containing the goal $G$: $G\subseteq\gamma(\cdots\gamma(s_0,a_1)\cdots,a_n)$. In practice actions are written as parameterized schemas, and the planner substitutes concrete objects to obtain "ground" actions. PDDL (the Planning Domain Definition Language, 1998) is the standard language for them; the stack of FIG 1 reads:

(:action stack
  :parameters (?x ?y)
  :precondition (and (holding ?x) (clear ?y))
  :effect (and (on ?x ?y) (clear ?x) (handempty)
               (not (holding ?x)) (not (clear ?y))))

启发式。FIG 1 的 A* 用 $h(s)=|G\setminus s|$。当目标只包含 on 命题时它是可采纳的：只有 stack 能让一个 on 命题变真，而每个 stack 只添加一个 on，所以还差几个目标命题，至少就还要几步。每一步最多让 $h$ 变化 1，所以它也是一致的（14.1 节的两个条件）。实用的规划器（FF、Fast Downward）用更强的启发式，例如忽略删除表的“松弛计划”长度：信息量大得多，但不再可采纳。

规模。命题个数随物体数多项式增长，状态数却是指数增长：3 块积木（含手的状态）有 22 个可达状态，4 块有 125 个。判定一般 STRIPS 问题有没有计划是 PSPACE 完全的（Bylander 1994）。所以 TAMP 里的符号层要尽量小：只放和任务有关的物体与动作。

Heuristics. The A* of FIG 1 uses $h(s)=|G\setminus s|$. When the goal contains only on statements it is admissible: only stack makes an on statement true, and each stack adds just one, so at least as many steps remain as goal statements are missing. Each step changes $h$ by at most 1, so it is also consistent (the two conditions of Lesson 14.1). Practical planners (FF, Fast Downward) use stronger heuristics, for example the length of a "relaxed plan" that ignores delete lists: far more informative, but no longer admissible.

Size. The number of statements grows polynomially with the number of objects, the number of states exponentially: 3 blocks (counting the hand) give 22 reachable states, 4 blocks give 125. Deciding whether a general STRIPS problem has a plan is PSPACE-complete (Bylander 1994). That is why the symbolic level of a TAMP system should be kept small: only the objects and actions relevant to the task.

混合问题：骨架与绑定The hybrid problem: skeletons and bindings

TAMP 的状态既有离散部分 $s$（命题），也有连续部分 $x$：机器人构型 $\mathbf q\in\mathcal C$（7.2 节）和各物体的位姿。动作带连续参数，例如 $\mathrm{pick}(o,g,\mathbf q)$ 里的抓取 $g$（夹爪相对物体的位姿）和构型 $\mathbf q$，$\mathrm{place}(o,p,g,\mathbf q)$ 里的摆放位姿 $p$。一个计划骨架是一串只确定了离散部分的动作，绑定就是为它找连续参数：

A TAMP state has a discrete part $s$ (statements) and a continuous part $x$: the robot configuration $\mathbf q\in\mathcal C$ (Lesson 7.2) and the poses of the objects. Actions carry continuous parameters, for example the grasp $g$ (the gripper's pose relative to the object) and configuration $\mathbf q$ in $\mathrm{pick}(o,g,\mathbf q)$, and the placement pose $p$ in $\mathrm{place}(o,p,g,\mathbf q)$. A plan skeleton is a sequence of actions whose discrete part alone is fixed, and binding it means finding the continuous parameters:

$$ \hl{skel}{\pi=\big(a_1(\boldsymbol\theta_1),\dots,a_K(\boldsymbol\theta_K)\big)},\qquad \text{find } \hl{theta}{\boldsymbol\theta_{1:K}}\ \text{ such that }\ \bigwedge_{k=1}^{K}\Big[\hl{kin}{\mathrm{Kin}(\mathbf q_k,o_k,p_k,g_k)}\wedge\hl{coll}{\mathrm{CFree}(\boldsymbol\tau_k,x_{k-1})}\wedge\hl{place}{p_k\in\mathcal R_k}\Big] $$

$\mathrm{Kin}$ 要求 $\mathbf q_k$ 的正运动学把夹爪送到 $p_k\cdot g_k$（物体位姿乘以抓取，FIG 4 的“够得着”）；$\mathrm{CFree}$ 要求第 $k$ 段轨迹 $\boldsymbol\tau_k$ 在当时的场景 $x_{k-1}$ 中不碰撞（FIG 2、FIG 4 的碰撞检查，轨迹本身来自 14.2 节的 RRT 或 14.3 节的优化）；$\mathcal R_k$ 是允许的摆放区域（FIG 2 的托盘）。关键在于同一个参数出现在多个约束里：抓取 $g$ 在 pick 时选定，却要在 place 时满足约束（FIG 6）；第一个盒子的位置 $p_1$ 会进入后面所有盒子的 $\mathrm{CFree}$（FIG 5）。这正是贪心会失败、需要回溯或联合优化的原因。

完整的循环（FIG 3）可以写成：

$\mathrm{Kin}$ demands that the forward kinematics of $\mathbf q_k$ bring the gripper to $p_k\cdot g_k$ (object pose times grasp: the "reachable" of FIG 4); $\mathrm{CFree}$ demands that the $k$-th trajectory $\boldsymbol\tau_k$ be collision-free in the scene $x_{k-1}$ of that moment (the collision checks of FIG 2 and FIG 4; the trajectory itself comes from an RRT of Lesson 14.2 or an optimizer of Lesson 14.3); $\mathcal R_k$ is the allowed placement region (the tray of FIG 2). The crux is that one parameter appears in several constraints: the grasp $g$ is chosen at pick but must satisfy constraints at place (FIG 6); the position $p_1$ of the first box enters the $\mathrm{CFree}$ of every later box (FIG 5). That is exactly why greedy binding fails and backtracking or joint optimization is needed.

The complete loop (FIG 3) can be written as:

F ← ∅                                  # learned statements
loop:
    π ← TaskPlan(s0, G, F)             # shortest skeleton consistent with F
    if π = none: return FAIL
    θ, why ← Bind(π, budget)           # samplers + checks + backtracking
    if θ ≠ none: return (π, θ)
    F ← F ∪ Diagnose(why)              # e.g. blocks(B,T), noroom(T,tray | M)
    budget ← grow(budget)              # keep revisiting old skeletons with more samples

最后一行很重要：采样有限次失败并不能证明骨架不可行（也许只是没采到），所以完备的算法要不断回到旧骨架、给更多样本。若每个采样器在可行集上有正的概率（可行集有非零体积），这样的算法是概率完备的：有解时，找到解的概率随时间趋于 1，和 14.2 节的 RRT 一样。

PDDLStream（Garrett、Lozano-Pérez、Kaelbling，2020）把采样器写进 PDDL：一个 stream 声明输入、输出和它认证的命题，例如（由论文中的例子简化而来）

The last line matters: failing after finitely many samples does not prove a skeleton infeasible (the samples may simply have missed), so a complete algorithm must keep returning to old skeletons with more samples. If every sampler puts positive probability on its feasible set (and the feasible set has nonzero volume), such an algorithm is probabilistically complete: when a solution exists, the probability of finding it tends to 1 over time, just as for the RRT of Lesson 14.2.

PDDLStream (Garrett, Lozano-Pérez and Kaelbling, 2020) writes samplers into PDDL: a stream declares its inputs, outputs and the statements it certifies, for example (simplified from the examples in the paper)

(:stream sample-grasp
  :inputs (?o)  :domain (Graspable ?o)
  :outputs (?g) :certified (Grasp ?o ?g))
(:stream inverse-kinematics
  :inputs (?o ?p ?g) :domain (and (Pose ?o ?p) (Grasp ?o ?g))
  :outputs (?q)      :certified (Kin ?o ?p ?g ?q))

它的“聚焦”算法先假设每个 stream 都能产生一个乐观的占位值，让符号规划器在这些占位值上规划，得到一个骨架；然后只调用这个骨架用到的 stream 去产生真实的值。被求值过的 stream 实例“层级”会升高，下一轮乐观规划不能再依赖同样的求值，规划器就转向别的选择；允许的层级随时间增大，所以每个实例以后还会被再次求值，不会被永久排除。这和 FIG 3 的循环是同一个思想：符号规划器决定先算哪些几何量，几何计算的结果再回流成命题。

Its "focused" algorithm first assumes every stream can produce an optimistic placeholder value and lets the symbolic planner plan with these placeholders to get a skeleton; only then does it call the streams that skeleton uses to produce real values. Each stream instance it evaluates has its "level" raised, so the next optimistic plan cannot rely on the same evaluations and the planner turns to other choices; the allowed level grows over time, so every instance gets evaluated again later and nothing is ruled out for good. It is the same idea as the loop of FIG 3: the symbolic planner decides which geometric quantities to compute first, and the results of the geometric computation flow back as statements.

逻辑-几何规划Logic-Geometric Programming

LGP 把整个问题写成一个对骨架和轨迹的联合优化（Toussaint 2015）：离散状态 $s_k$ 决定在每段时间里哪些约束生效（例如“物体固定在夹爪上”），切换时刻的约束 $h_{\text{switch}}$ 描述动作本身（例如“在 $t_k$ 时夹爪到达抓取位姿”）：

LGP writes the whole problem as a joint optimization over skeletons and trajectories (Toussaint 2015): the discrete state $s_k$ decides which constraints are active during each phase (for example "the object is attached to the gripper"), and the switch constraints $h_{\text{switch}}$ at the switching times describe the actions themselves (for example "at $t_k$ the gripper reaches the grasp pose"):

$$ \min_{K,\;a_{1:K},\;x(t)}\ \int_0^T c\big(x,\dot x,\ddot x\big)\,dt\quad\text{s.t.}\quad \begin{cases} h_{\text{path}}(x(t),s_{k(t)})=0,\ \ g_{\text{path}}(x(t),s_{k(t)})\le0\\ \hl{kf}{h_{\text{switch}}\big(x(t_k),a_k\big)=0}\\ s_k=\mathrm{succ}(s_{k-1},a_k),\quad G\subseteq s_K\end{cases} $$

外层对骨架 $a_{1:K}$ 做树搜索，内层对每个骨架解一个非线性规划。完整轨迹的 NLP 很贵，所以 LGP 用一串由粗到细的“界”：先只解最终位姿，再解关键帧序列，最后才解整条轨迹；粗一级不可行的骨架直接剪掉。FIG 6 就是关键帧这一级：变量 $\mathbf z$（直接插入时 $\mathbf z=(s,p)$），关键帧 $\hl{kf}{\mathbf x_k}=C_k\mathbf z+\mathbf d_k$ 是 $\mathbf z$ 的仿射函数，

The outer level runs a tree search over skeletons $a_{1:K}$; the inner level solves one nonlinear program per skeleton. The NLP over full trajectories is expensive, so LGP uses a sequence of coarse-to-fine "bounds": first only the final pose, then the sequence of keyframes, and only then the whole trajectory; a skeleton infeasible at a coarser level is pruned at once. FIG 6 is the keyframe level: the variables are $\mathbf z$ (for direct insertion $\mathbf z=(s,p)$) and the keyframes $\hl{kf}{\mathbf x_k}=C_k\mathbf z+\mathbf d_k$ are affine in $\mathbf z$,

$$ \min_{\mathbf z\in\hl{feas}{\mathcal F_\pi}}\ f(\mathbf z)=\sum_{k=1}^{K+1}\big\|\mathbf x_k-\mathbf x_{k-1}\big\|^2,\qquad \hl{feas}{\mathcal F_\pi}=\{\mathbf z:\ A\mathbf z\le\mathbf b\},\qquad \mathbf x_0=\mathbf x_{K+1}=\mathsf S $$

直接插入骨架的约束是：抓取点在棒子上且不在横梁下 $s_{\min}\le s\le s_{\max}-fL$；插入深度 $x_o-(p-L/2)\ge d_{\min}$；棒子不顶穿槽底；夹爪留在槽外 $p+s\ge x_o+0.04$。最后一条把 $s$ 和 $p$ 耦合起来，正是它让贪心失败。增广拉格朗日法交替做两件事（6.7 节）：

The constraints of the direct-insertion skeleton: the grasp lies on the rod and not under the crossbar, $s_{\min}\le s\le s_{\max}-fL$; the insertion depth $x_o-(p-L/2)\ge d_{\min}$; the rod does not push through the slot's back wall; the gripper stays outside the slot, $p+s\ge x_o+0.04$. The last one couples $s$ and $p$, and it is the one that makes greedy fail. The augmented Lagrangian method alternates two things (Lesson 6.7):

$$ \mathbf z\leftarrow\arg\min_{\mathbf z}\ f(\mathbf z)+\frac{\rho}{2}\sum_i\max\!\Big(0,\ \mathbf a_i^\top\mathbf z-b_i+\frac{\lambda_i}{\rho}\Big)^2,\qquad \lambda_i\leftarrow\max\big(0,\ \lambda_i+\rho\,(\mathbf a_i^\top\mathbf z-b_i)\big) $$

内层问题是二次函数加单侧二次罚项，用几步牛顿法就能精确求解；外层把乘子 $\lambda_i$ 推向 KKT 条件里的值，$\rho$ 每次乘 3。FIG 6 的默认情况 6 次外层迭代后违反量就低于 $10^{-9}$ m。可行集为空时乘子和 $\rho$ 不断增长、违反量却停在一个正数上，这就是“这个骨架不可行”的信号。

The inner problem is a quadratic plus one-sided quadratic penalties, solved exactly by a few Newton steps; the outer step pushes the multipliers $\lambda_i$ towards their KKT values, and $\rho$ is multiplied by 3 each time. In FIG 6's default case the violation is below $10^{-9}$ m after 6 outer iterations. When the feasible set is empty, the multipliers and $\rho$ keep growing while the violation stalls at a positive value: the sign that "this skeleton is infeasible".

向下精化与抽象的选择Downward refinement and the choice of abstraction

如果每个符号上可行的骨架在几何上都能绑定（“向下精化性质”），TAMP 就退化成先规划、后填参数。现实里这几乎从不成立（FIG 2），于是所有 TAMP 方法都在回答同一个问题：怎样让几何的失败尽早、尽量具体地告诉符号层。符号抽象太粗，循环要转很多圈；太细（例如把格子化的位置都写成命题），符号搜索本身就爆炸。

If every symbolically feasible skeleton could be bound geometrically (the "downward refinement property"), TAMP would collapse into planning first and filling in parameters afterwards. In reality this almost never holds (FIG 2), so every TAMP method answers the same question: how to tell the symbolic level about geometric failures as early and as specifically as possible. An abstraction too coarse makes the loop go round many times; one too fine (writing discretized positions as statements, say) makes the symbolic search itself explode.

7机器人中的它In robotics

真实系统里In real systems

研究系统：PDDLStream 是开源的 Python 库，用 Fast Downward 做符号搜索，自带 PyBullet 中的抓取、摆放、做饭与清洁等示例；Kaelbling 和 Lozano-Pérez 的 HPN（Hierarchical Planning in the Now，2011）在分层抽象上规划，只把眼前的一步细化到几何，执行中不断重规划；Dantam 等人（2016）把 TAMP 写成增量的约束求解（SMT）；Toussaint 组的 LGP 用 KOMO 优化器解关键帧和轨迹，用在工具使用等需要接触和物理的任务上。 工业与 ROS：MoveIt Task Constructor 把一个任务写成固定结构的“阶段”管线（生成抓取、逆运动学、在阶段之间用运动规划器连接），在每个阶段的多个候选解之间搜索：这相当于骨架由人写死、只做绑定的 TAMP。ROS 2 的 PlanSys2 用 PDDL 规划、用行为树执行动作。大多数已部署的系统仍然是手写的任务结构（状态机或行为树）加运动规划器，完整的 TAMP 主要用于研究和高度多变的任务。 大模型：近年有工作让语言模型提出高层计划（SayCan 把 LLM 对每个技能的打分与学到的“这个技能此刻能否成功”的估计相乘，Code as Policies 让 LLM 直接写机器人程序），或者让 LLM 把自然语言翻译成 PDDL 再交给经典规划器（LLM+P）；几何可行性仍要由本节这样的检查来保证。这部分在 Part VII 展开。

Research systems: PDDLStream is an open-source Python library that uses Fast Downward for the symbolic search and ships PyBullet examples of picking, placing, cooking and cleaning; the HPN (Hierarchical Planning in the Now, 2011) of Kaelbling and Lozano-Pérez plans over a hierarchy of abstractions, refines only the next step down to geometry and replans continually during execution; Dantam et al. (2016) cast TAMP as incremental constraint solving (SMT); Toussaint's group solves LGP keyframes and trajectories with the KOMO optimizer and has applied it to tool use and other tasks that need contact and physics. Industry and ROS: MoveIt Task Constructor writes a task as a pipeline of "stages" with a fixed structure (generate grasps, inverse kinematics, motion planners connecting the stages) and searches among the candidate solutions of each stage: a TAMP whose skeleton is written by hand and which only does binding. PlanSys2 in ROS 2 plans with PDDL and executes actions with behaviour trees. Most deployed systems still use a hand-written task structure (a state machine or behaviour tree) plus a motion planner; full TAMP is mainly used in research and for highly variable tasks. Large models: recent work lets language models propose high-level plans (SayCan multiplies the LLM's score for each skill by a learned estimate of whether that skill can succeed right now, Code as Policies has the LLM write robot programs directly), or has an LLM translate natural language into PDDL for a classical planner (LLM+P); geometric feasibility must still be guaranteed by checks like those in this lesson. Part VII covers this.

和前面各节的关系。TAMP 是一个“调用者”：符号层的搜索就是 14.1 节的 BFS / A*，只是节点变成了命题集合；动作之间的轨迹由 14.2 节的 RRT / PRM 或 14.3 节的轨迹优化来算；抓取候选可以来自 9.3 节的力封闭评估；可达性来自 7.5 节的逆运动学。一次 TAMP 求解里，运动规划器和碰撞检测往往被调用成千上万次，所以它们的速度直接决定 TAMP 的速度；PDDLStream 这类方法花很多心思在“少调用、晚调用”上。

How it relates to the earlier lessons. TAMP is a "caller": the symbolic search is the BFS / A* of Lesson 14.1, with sets of statements as nodes; trajectories between actions come from the RRT / PRM of Lesson 14.2 or the trajectory optimization of Lesson 14.3; grasp candidates can come from the force-closure evaluation of Lesson 9.3; reachability comes from the inverse kinematics of Lesson 7.5. In one TAMP solve, motion planners and collision checkers are often called thousands of times, so their speed directly sets TAMP's speed; methods like PDDLStream put much effort into calling them as rarely and as late as possible.

8常见误区Common pitfalls

误区 1：相信没有经过几何检查的符号计划Pitfall 1: trusting a symbolic plan that was never checked geometrically

FIG 2 的两步计划在符号上无懈可击，在几何上却做不到。符号模型只是抽象，抽象丢掉的东西（遮挡、空间、可达性、稳定性）随时可能让某一步失败。计划的每一步都要绑定到具体参数并检查，执行时还要准备好重规划，因为感知到的几何本身就有误差。

The two-step plan of FIG 2 is flawless symbolically and impossible geometrically. A symbolic model is an abstraction, and whatever it drops (occlusion, space, reachability, stability) can make any step fail. Every step must be bound to concrete parameters and checked, and execution must be ready to replan, because the perceived geometry itself is uncertain.

误区 2：贪心地接受第一个可行的样本Pitfall 2: greedily accepting the first feasible sample

FIG 5：即使单步采样器是完美的，贪心绑定在余量小时也会大概率失败，因为前面的连续选择决定了后面还剩什么。要么回溯（并给每层一个预算，避免在一个死胡同里无限采样），要么像 LGP 那样把整个骨架的约束一起优化，要么设计“为后面留余地”的采样器（例如靠边摆放），但这类启发式会牺牲完备性。

FIG 5: even with a perfect one-step sampler, greedy binding fails with high probability when the slack is small, because earlier continuous choices decide what is left later. Either backtrack (with a budget per level, so as not to sample forever in a dead end), or optimize the constraints of the whole skeleton together as LGP does, or design samplers that "leave room for later" (packing against an edge, say), though such heuristics give up completeness.

误区 3：失败时只返回“失败”Pitfall 3: reporting a failure as just "failed"

如果几何层只说“这个骨架不行”，符号层能做的只有把这个骨架拉黑，然后尝试下一个，而下一个往往因为同样的原因失败（比如换个地方放 T，但 B 依然挡着）。FIG 3 能两次迭代就成功，是因为失败被诊断成了具体的命题 blocks(B,T)，一次排除了一整类骨架。诊断太具体也不好：把“B 在 (0.31, 0.15) 时挡着 T”写成命题，B 稍微一动它就没用了。

If the geometric level only says "this skeleton does not work", all the symbolic level can do is blacklist it and try the next, which often fails for the same reason (placing T somewhere else while B still blocks it). FIG 3 succeeds in two iterations because the failure was diagnosed as the concrete statement blocks(B,T), ruling out a whole class of skeletons at once. Too specific a diagnosis is no good either: a statement like "B blocks T while B is at (0.31, 0.15)" becomes useless as soon as B moves a little.

误区 4：采样器只在“好看”的值里取Pitfall 4: samplers that only produce "nice" values

为了快，人们常把抓取限制在四个正方向、把摆放限制在区域中心。这在空旷场景里很有效，可 FIG 4 里被围住的罐子可能只剩一个窄窄的可行方向区间，四个正方向恰好全落在外面。采样器必须对整个可行集有正的概率，算法才可能概率完备；好的做法是“先试启发式的值，再退回到随机采样”，并且用约束感知的采样（例如先采抓取再解逆运动学，而不是在关节空间盲采）提高 $p$。

For speed, people often restrict grasps to four axis directions and placements to the centre of a region. That works well in open scenes, but the hemmed-in can of FIG 4 may have only one narrow interval of feasible directions, with all four axis directions outside it. A sampler must put positive probability on the whole feasible set for the algorithm to be probabilistically complete; good practice is "try the heuristic values first, then fall back to random sampling", and to raise $p$ with constraint-aware sampling (sample the grasp first and solve inverse kinematics, instead of sampling joint space blindly).

误区 5：把所有物体和动作都塞进符号层Pitfall 5: putting every object and action into the symbolic level

状态数随物体数指数增长（FIG 1：3 块积木 22 个状态，4 块 125 个），厨房里几十个物体时符号搜索本身就会撑爆，而每个多余的骨架还要触发昂贵的几何检查。实际系统只把和目标相关的物体放进规划问题（挡路的物体通过失败反馈才被加进来，正如 FIG 3 里的 B 和 C），并用分层规划把长任务拆开（HPN）。

The number of states grows exponentially with the number of objects (FIG 1: 22 states for 3 blocks, 125 for 4); with dozens of objects in a kitchen the symbolic search alone blows up, and each superfluous skeleton also triggers expensive geometric checks. Real systems put only the objects relevant to the goal into the planning problem (blocking objects enter through failure feedback, just like B and C in FIG 3), and split long tasks with hierarchical planning (HPN).

9小练习Check yourself

1. 状态 $s=\{$on(A,B), ontable(B), clear(A), handempty$\}$。执行 unstack(A,B) 之后的状态是？1. The state is $s=\{$on(A,B), ontable(B), clear(A), handempty$\}$. What is the state after unstack(A,B)?

$\gamma(s,a)=(s\setminus\mathrm{del})\cup\mathrm{add}$：删除 on(A,B)、clear(A)、handempty，添加 holding(A)、clear(B)，ontable(B) 不受影响，得到选项 C。选项 A 忘了删除 clear(A)（被拿着的积木不算“上面空着”）也忘了添加 clear(B)；选项 B 没删除 on(A,B)；选项 D 是再执行一次 putdown(A) 之后的状态。$\gamma(s,a)=(s\setminus\mathrm{del})\cup\mathrm{add}$: delete on(A,B), clear(A) and handempty, add holding(A) and clear(B); ontable(B) is untouched, giving option C. Option A forgets to delete clear(A) (a held block does not count as clear) and to add clear(B); option B does not delete on(A,B); option D is the state after a further putdown(A).

2. 在 FIG 2 的默认场景里，符号规划器给出 pick(T) → place(T,tray)，而夹爪通道被 B 挡住。最有用的做法是？2. In FIG 2's default scene the symbolic planner returns pick(T) → place(T,tray), while B blocks the gripper's way in. What is the most useful response?

挡路是场景的事实，不是采样运气：只要 B 不动，pick(T) 的任何参数都不行（排除 A）。只拉黑骨架，下一个骨架（比如把 T 放到边桌）同样要先 pick(T)，还会失败（排除 C）。运动规划器只在一个固定的“模式”里找路径，它不会自己产生“先挪 B”这种离散决策（排除 D）。具体的失败命题让符号层一次绕开整类失败，这正是 FIG 3 的做法。The blockage is a fact of the scene, not bad luck in sampling: while B stays put, no parameters of pick(T) work (ruling out A). Merely blacklisting the skeleton leads to the next one (say, putting T on the side table), which also begins with pick(T) and fails again (ruling out C). A motion planner searches for a path within one fixed "mode"; it does not generate discrete decisions like "move B first" by itself (ruling out D). A concrete failure statement lets the symbolic level avoid the whole class of failures at once, which is what FIG 3 does.

3. FIG 5 里自由空间采样器从不产生碰撞的位置，为什么贪心绑定仍然会失败？3. In FIG 5 the free-space sampler never produces a colliding position. Why does greedy binding still fail?

自由空间采样器的每个样本都可行，所以 $K$ 再大，贪心也只会用第一个样本（排除 B）；隔板长度 = 盒宽之和 + 余量，总是放得下（排除 C）。失败来自参数之间的耦合：第三个盒子的可行集依赖前两个盒子的位置，可能已经是空集。回溯回到前面换位置，所以能成功。Every sample of the free-space sampler is feasible, so however large $K$ is, greedy uses only the first one (ruling out B); the board length is the sum of the widths plus the slack, so the boxes always fit in principle (ruling out C). The failure comes from coupling between parameters: the third box's feasible set depends on the first two positions and may already be empty. Backtracking goes back and changes them, which is why it succeeds.

4. 目标是 $G=\{$on(A,B), on(B,C)$\}$。为什么 $h(s)=|G\setminus s|$ 是可采纳的启发式？4. The goal is $G=\{$on(A,B), on(B,C)$\}$. Why is $h(s)=|G\setminus s|$ an admissible heuristic?

可采纳 = 从不高估剩余代价。每个缺失的 on 目标至少需要一个不同的 stack，所以 $h$ 是下界。它远小于真实代价：Sussman 初始状态 $h=2$，而最优计划要 6 步（排除 B）。积木世界当然有删除表（排除 C）；BFS 不用启发式（排除 D）。Admissible means never overestimating the remaining cost. Each missing on goal needs at least one distinct stack, so $h$ is a lower bound. It is far below the true cost: in the Sussman initial state $h=2$ while the optimal plan takes 6 steps (ruling out B). The blocks world certainly has delete lists (ruling out C); BFS uses no heuristic (ruling out D).

5. FIG 6 默认设置下贪心失败、联合优化成功，原因是？5. With FIG 6's defaults, greedy fails and joint optimization succeeds. Why?

约束 $p+s\ge x_o+0.04$ 把两个动作的参数耦合起来。贪心先按“离起点最近”选 $s=-13$ cm，此后不存在满足插入深度的 $p$；联合优化在可行集 $\mathcal F_\pi$ 上取最小，落在 $s=7$ cm、$p=17$ cm 的角点上。默认设置下这个骨架是可行的（排除 D），两者的代价函数相同（排除 B）。The constraint $p+s\ge x_o+0.04$ couples the parameters of the two actions. Greedy first picks $s=-13$ cm as "nearest to the start", after which no $p$ meets the insertion depth; joint optimization minimizes over the feasible set $\mathcal F_\pi$ and ends at the corner $s=7$ cm, $p=17$ cm. At the defaults this skeleton is feasible (ruling out D), and both use the same cost (ruling out B).

10延伸阅读Further reading

R. E. Fikes & N. J. Nilsson, STRIPS: A New Approach to the Application of Theorem Proving to Problem Solving, Artificial Intelligence 2(3–4), 1971 — 前提 / 添加 / 删除表的出处where precondition / add / delete lists come from
M. Ghallab, D. Nau & P. Traverso, Automated Planning: Theory and Practice, Morgan Kaufmann, 2004 — 经典规划、PDDL、启发式搜索的教科书the textbook on classical planning, PDDL and heuristic search
J. Hoffmann & B. Nebel, The FF Planning System: Fast Plan Generation Through Heuristic Search, JAIR 14, 2001; M. Helmert, The Fast Downward Planning System, JAIR 26, 2006 — 松弛计划启发式与现代符号规划器relaxed-plan heuristics and modern symbolic planners
L. P. Kaelbling & T. Lozano-Pérez, Hierarchical Task and Motion Planning in the Now, ICRA 2011 — 分层、边执行边细化的 TAMPhierarchical TAMP that refines while executing
S. Srivastava, E. Fang, L. Riano, R. Chitnis, S. Russell & P. Abbeel, Combined Task and Motion Planning Through an Extensible Planner-Independent Interface Layer, ICRA 2014 — 把几何失败翻译成命题反馈给任意符号规划器translating geometric failures into statements for any symbolic planner
M. Toussaint, Logic-Geometric Programming: An Optimization-Based Approach to Combined Task and Motion Planning, IJCAI 2015 — LGP 的原始论文the original LGP paper
N. T. Dantam, Z. K. Kingston, S. Chaudhuri & L. E. Kavraki, Incremental Task and Motion Planning: A Constraint-Based Approach, RSS 2016 — 用增量 SMT 求解做 TAMPTAMP through incremental SMT solving
C. R. Garrett, T. Lozano-Pérez & L. P. Kaelbling, PDDLStream: Integrating Symbolic Planners and Blackbox Samplers via Optimistic Adaptive Planning, ICAPS 2020 — stream、乐观规划与聚焦算法streams, optimistic planning and the focused algorithm
C. R. Garrett, R. Chitnis, R. Holladay, B. Kim, T. Silver, L. P. Kaelbling & T. Lozano-Pérez, Integrated Task and Motion Planning, Annual Review of Control, Robotics, and Autonomous Systems 4, 2021 — 全面的综述，从这里入门a comprehensive survey; start here