TAMP_T1 任务与运动规划基础 (Task and Motion Planning Foundations)¶
难度: ⭐ ~ ⭐⭐⭐⭐ (逐级递进) 前置知识: A* 搜索、一阶逻辑基本概念、RRT/PRM 概念 核心参考: Garrett et al., Annual Review of Control, Robotics, and Autonomous Systems, 2021
0. 前置自测¶
开始本章之前，请完成下面三道自测题。如果有两道以上答不出来，建议先回顾相应基础章节。
| # | 问题 | 期望掌握程度 |
|---|---|---|
| Q1 | 请用一句话描述 A* 算法与 Dijkstra 算法的区别，并写出 A* 的评价函数 \(f(n) = g(n) + h(n)\) 中各项含义。 | 能写出公式并解释 |
| Q2 | 一阶逻辑中，谓词 (predicate)、量词 (quantifier)、合取 (conjunction) 的含义分别是什么？请举例。 | 能读懂简单逻辑表达式 |
| Q3 | RRT 和 PRM 算法分别属于什么类型的采样规划方法？它们在查询次数上有什么区别？ | 能对比两者特性 |
参考答案 (点击展开)
**Q1**: A* 在 Dijkstra 的基础上加入了启发式函数 $h(n)$（对目标距离的估计），使搜索更有方向性。$g(n)$ 是从起点到节点 $n$ 的实际代价，$h(n)$ 是从 $n$ 到目标的估计代价，$f(n)$ 是总估计代价。当 $h(n)$ 可容许 (admissible) 时，A* 保证找到最优路径。 **Q2**: 谓词是描述对象性质或关系的函数，如 $\text{On}(A, B)$ 表示"A 在 B 上面"；量词包括全称量词 $\forall$ ("对所有") 和存在量词 $\exists$ ("存在某个")；合取 $\wedge$ 表示逻辑"与"，如 $P \wedge Q$ 意为"P 且 Q"。 **Q3**: PRM (Probabilistic Roadmap) 是**多查询 (multi-query)** 方法，离线建图后可反复查询；RRT (Rapidly-exploring Random Tree) 是**单查询 (single-query)** 方法，每次查询从头构建。PRM 适合需要反复查询同一环境的场景，RRT 适合一次性查询或环境频繁变化的场景。1. 本章目标¶
学完本章后，你应当能够：
- 解释为什么单独的任务规划或单独的运动规划都无法解决实际机器人操作问题
- 编写一个完整的 PDDL domain + problem 文件，并用规划器求解
- 描述 TAMP 的核心挑战：无限分支因子、几何可行性、耦合约束
- 比较 PDDLStream、LGP、FFRob 三种主流 TAMP 框架的设计哲学
- 理解 Foundation Models 在 TAMP 中的作用与局限性
- 设计一个简单的 pick-and-place TAMP 系统的整体架构
2. 为什么需要 TAMP ⭐¶
2.1 一个具体的机器人任务¶
想象这样一个场景：你有一个带机械臂的移动机器人，需要整理一张桌子。桌子上有： - 一个红色杯子，需要放到架子上层 - 一本书，需要归到书柜里 - 桌面需要擦干净
你作为人类，可以毫不费力地规划出步骤：先拿杯子放到架子上，再拿书放到书柜，最后拿抹布擦桌子。但对机器人来说，这需要同时回答两个层面的问题：
| 层面 | 问题 | 难点 |
|---|---|---|
| 任务层 (Task) | 先做什么？后做什么？ | 组合爆炸：3个物体的操作排列 = \(3! = 6\) 种，加上中间步骤更多 |
| 运动层 (Motion) | 怎么移过去？怎么抓？从哪个角度放？ | 连续空间：每个抓取姿态有 6 自由度，无穷多种选择 |
关键问题：这两个层面不是独立的。
2.2 单独用运动规划会怎样？¶
假设你有一个完美的 RRT 规划器，能帮机械臂避开所有障碍物。但是：
RRT/PRM 解决的是："已知起点和终点，在有障碍的构型空间中找一条可行路径。" 它回答不了"起点和终点应该是什么"这个问题。
类比：运动规划像 GPS 导航——你告诉它目的地，它帮你规划路线。但它不能帮你决定今天先去超市还是先去银行。
2.3 单独用任务规划会怎样？¶
假设你有一个完美的 PDDL 规划器（后面会详细介绍），它能推理出动作序列：
Plan:
1. pick(cup, table)
2. place(cup, shelf_top)
3. pick(book, table)
4. place(book, bookcase)
5. pick(cloth, drawer)
6. wipe(table)
看起来很完美？问题是：
- 第 2 步
place(cup, shelf_top)—— 架子上层太高，机械臂够不到 - 第 1 步
pick(cup, table)—— 杯子被书挡在角落里，机械臂伸不进去 - 任务规划器完全不知道这些物理约束，因为它的世界模型只有离散的符号
任务规划器的世界观：
- 状态 = {On(cup, table), On(book, table), Clear(shelf_top), ...}
- 它不知道 shelf_top 有多高
- 它不知道 cup 和 book 的具体位置
- 它不知道机械臂的工作空间有多大
2.4 天真的结合方式：先规划任务，再检查运动¶
最直觉的想法：先用任务规划器生成一个动作序列，再用运动规划器逐步检查每步是否可行。
朴素 Plan-then-Check:
1. 任务规划器输出: [pick(cup), place(cup, shelf), pick(book), ...]
2. 运动规划器检查 pick(cup): OK
3. 运动规划器检查 place(cup, shelf): FAIL! 够不到!
4. 回到第1步，加约束重新规划...
5. 新计划: [pick(cup), place(cup, counter), pick(book), ...]
6. 运动规划器检查 place(cup, counter): OK
7. 运动规划器检查 pick(book): FAIL! book 换了位置后被挡住了!
8. 回到第1步...无限循环
这种方法有一个致命缺陷：指数级回溯。假设任务计划有 \(n\) 步，每步有 \(k\) 种可能失败的原因，最坏情况下需要尝试 \(O(k^n)\) 种组合。
陷阱框
朴素 Plan-then-Check 的根本问题：任务规划器和运动规划器之间没有信息流动。任务规划器不知道"为什么失败"，所以无法针对性地修正计划。这不是简单地增加重试次数能解决的。
2.5 Gap：符号世界 vs 几何世界¶
TAMP 的核心挑战可以归结为一个根本性的鸿沟：
符号世界 (Symbolic/Discrete) 几何世界 (Geometric/Continuous)
┌─────────────────────┐ ┌─────────────────────────┐
│ On(cup, table) │ │ cup.pose = (0.3, 0.2, 0.1) │
│ Clear(shelf) │ ← GAP → │ shelf.height = 1.2m │
│ HandEmpty(robot) │ │ arm.workspace = ... │
│ ... │ │ collision_mesh = ... │
│ 有限个命题 │ │ 无穷维连续空间 │
└─────────────────────┘ └─────────────────────────┘
| 属性 | 符号世界 | 几何世界 |
|---|---|---|
| 状态表示 | 命题集合 \(\{P_1, P_2, ...\}\) | 连续向量 \(\mathbf{q} \in \mathbb{R}^d\) |
| 分支因子 | 有限 (动作数 \(\times\) 对象数) | 无限 (连续参数) |
| 可行性判断 | 逻辑推理 | 碰撞检测 + 逆运动学 |
| 搜索方法 | 图搜索 (BFS, A*) | 采样/优化 (RRT, CHOMP) |
| 典型工具 | PDDL 规划器 | MoveIt, OMPL |
TAMP 的使命就是架起这座桥。
自测检查点 2¶
用自己的话解释：为什么不能简单地"先任务规划再运动规划"？请举一个具体的失败案例。
3. 经典任务规划 ⭐⭐¶
3.1 为什么要单独学任务规划？¶
任务规划是 TAMP 的"上层引擎"。要理解 TAMP 如何工作，必须先理解任务规划器能做什么、不能做什么。本节将从最基础的 STRIPS 表示讲起，到 PDDL 语言，再到启发式搜索。
3.2 STRIPS 表示¶
STRIPS (Stanford Research Institute Problem Solver, 1971) 是最经典的动作表示形式，今天的 PDDL 仍然基于它。
核心思想：世界状态用一组命题 (propositions) 描述，每个动作通过前提条件 (preconditions) 和效果 (effects) 来定义。
形式化定义：
\[
\text{Action} = \langle \text{name}(\text{params}), \; \text{Pre}, \; \text{Eff}^+, \; \text{Eff}^- \rangle
\]
其中： - \(\text{Pre}\)：前提条件——执行动作前必须为真的命题集合 - \(\text{Eff}^+\)：正效果 (add list)——执行动作后变为真的命题 - \(\text{Eff}^-\)：负效果 (delete list)——执行动作后变为假的命题
示例：pick 动作
\[
\text{pick}(obj, loc): \begin{cases}
\text{Pre} = \{\text{On}(obj, loc), \; \text{HandEmpty}, \; \text{Clear}(obj)\} \\
\text{Eff}^+ = \{\text{Holding}(obj)\} \\
\text{Eff}^- = \{\text{On}(obj, loc), \; \text{HandEmpty}, \; \text{Clear}(obj)\}
\end{cases}
\]
直觉理解：要拿起一个物体，需要（1）它在某个位置上，（2）手是空的，（3）物体上面没有东西。拿起之后，手里有了物体（正效果），但物体不在原来的位置了、手也不空了（负效果）。
3.3 PDDL 语言¶
PDDL (Planning Domain Definition Language) 是 1998 年为国际规划竞赛 (IPC) 设计的标准化语言。它将规划问题分为两个文件：
| 文件 | 内容 | 类比 |
|---|---|---|
| Domain 文件 | 类型、谓词、动作模板 | 游戏规则 |
| Problem 文件 | 具体对象、初始状态、目标 | 具体一局的设置 |
3.3.1 完整 PDDL 示例：Pick-and-Place Domain¶
Domain 文件 (pick-place-domain.pddl)：
(define (domain pick-place)
(:requirements :strips :typing)
;; 类型定义
(:types
object ;; 可操作的物体
location ;; 位置（桌子、架子等）
robot ;; 机器人
)
;; 谓词定义：描述世界状态的"词汇表"
(:predicates
(on ?obj - object ?loc - location) ;; 物体在某位置上
(holding ?r - robot ?obj - object) ;; 机器人正在抓着某物体
(hand-empty ?r - robot) ;; 机器人手是空的
(clear ?obj - object) ;; 物体上面没有东西
(reachable ?r - robot ?loc - location) ;; 机器人能够到某位置
)
;; 动作1: 拿起物体
(:action pick
:parameters (?r - robot ?obj - object ?loc - location)
:precondition (and
(on ?obj ?loc)
(hand-empty ?r)
(clear ?obj)
(reachable ?r ?loc)
)
:effect (and
(holding ?r ?obj)
(not (on ?obj ?loc))
(not (hand-empty ?r))
)
)
;; 动作2: 放下物体
(:action place
:parameters (?r - robot ?obj - object ?loc - location)
:precondition (and
(holding ?r ?obj)
(reachable ?r ?loc)
)
:effect (and
(on ?obj ?loc)
(hand-empty ?r)
(not (holding ?r ?obj))
)
)
;; 动作3: 移动到某位置附近
(:action move-to
:parameters (?r - robot ?from - location ?to - location)
:precondition (and
(reachable ?r ?from)
)
:effect (and
(reachable ?r ?to)
(not (reachable ?r ?from))
)
)
)
Problem 文件 (pick-place-problem.pddl)：
(define (problem tidy-table)
(:domain pick-place)
;; 具体对象
(:objects
arm - robot
cup book - object
table shelf bookcase - location
)
;; 初始状态
(:init
(on cup table)
(on book table)
(clear cup)
(clear book)
(hand-empty arm)
(reachable arm table)
)
;; 目标状态
(:goal (and
(on cup shelf)
(on book bookcase)
))
)
3.3.2 PDDL 语法要点表¶
| 关键字 | 含义 | 示例 |
|---|---|---|
:requirements |
声明使用的 PDDL 特性 | :strips :typing :equality |
:types |
定义对象类型 | object location robot |
:predicates |
定义谓词（状态变量） | (on ?obj - object ?loc - location) |
:action |
定义动作模板 | 见上面完整示例 |
:parameters |
动作的参数列表 | (?r - robot ?obj - object) |
:precondition |
前提条件（逻辑表达式） | (and (holding ?r ?obj) ...) |
:effect |
效果（正效果 + (not ...) 负效果） |
(and (on ?obj ?loc) (not (holding ?r ?obj))) |
?变量名 |
变量，由规划器实例化 | ?obj, ?loc |
3.4 启发式搜索：FF 规划器¶
任务规划本质上是在状态空间中搜索。状态空间可以非常大——如果有 \(n\) 个命题，状态空间大小为 \(2^n\)。因此需要启发式函数来引导搜索。
FF (Fast-Forward) 规划器 使用一种巧妙的启发式——松弛计划 (relaxed plan)：
- 松弛：忽略所有动作的负效果 (delete list)。也就是说，命题一旦为真就永远为真
- 求解松弛问题：由于删除效果被忽略，松弛问题可以在多项式时间内求解
- 启发式值：松弛计划的长度（动作数量）作为启发式估计
\[
h_{FF}(s) = |\text{RelaxedPlan}(s, G)|
\]
为什么这个启发式有效？ - 松弛问题去掉了负效果（减少了约束），其最优代价 \(h^+\) 不超过原问题最优代价（可容许）。但 \(h_{FF}\) 不是松弛最优解——它只是贪心抽取的一个松弛计划，其长度可能高于 \(h^+\)（反例：贪心抽取策略未找到最短松弛计划），因此 \(h_{FF}\) 不一定可容许 - 实践中 \(h_{FF}\) 效果非常好 - FF 使用的是 enforced hill climbing + BFS 的搜索策略
松弛示例：
原始 pick(cup, table):
Pre: {On(cup,table), HandEmpty, Clear(cup)}
Eff+: {Holding(cup)}
Eff-: {On(cup,table), HandEmpty, Clear(cup)} ← 松弛时忽略这行
松弛后：拿起杯子后，杯子同时在桌上和在手里。
这不物理，但让搜索变得容易。
3.5 代码实践：用 pyperplan 运行 PDDL¶
pyperplan 是一个轻量级的 Python PDDL 规划器，非常适合教学使用。
"""
用 pyperplan 求解 PDDL 规划问题
安装: pip install pyperplan
"""
import pyperplan
# 方法1: 命令行调用
# pyperplan pick-place-domain.pddl pick-place-problem.pddl
# 方法2: Python API
from pyperplan.planner import (
search_plan, SEARCHES, HEURISTICS,
_parse, _ground
)
def solve_pddl(domain_file: str, problem_file: str):
"""求解一个 PDDL 规划问题并打印结果"""
# 1. 解析 PDDL 文件
problem = _parse(domain_file, problem_file)
# 2. grounding: 将带变量的动作模板实例化为具体动作
task = _ground(problem)
# 3. 搜索求解 (使用 A* + FF 启发式)
heuristic_class = HEURISTICS['hff'] # FF 启发式
heuristic = heuristic_class(task)
search_fn = SEARCHES['astar']
solution = search_fn(task, heuristic)
if solution is None:
print("No solution found!")
return None
print("=== Solution Plan ===")
for i, action in enumerate(solution):
print(f" Step {i+1}: {action}")
print(f"Total steps: {len(solution)}")
return solution
# 使用示例
if __name__ == "__main__":
solve_pddl("pick-place-domain.pddl", "pick-place-problem.pddl")
# 预期输出类似:
# Step 1: (pick arm cup table)
# Step 2: (move-to arm table shelf)
# Step 3: (place arm cup shelf)
# Step 4: (move-to arm shelf table)
# Step 5: (pick arm book table)
# Step 6: (move-to arm table bookcase)
# Step 7: (place arm book bookcase)
陷阱框
PDDL 的根本局限：PDDL 无法表达数值约束（距离、角度、力）和几何约束（碰撞、可达性）。上面的 domain 中
(reachable ?r ?loc)是一个布尔谓词，它说的是"能/不能到达"，但不编码"为什么不能到达"或"需要怎样的运动"。这正是 TAMP 要弥补的缺口。
3.6 ADL 扩展：超越 STRIPS ⭐⭐¶
STRIPS 的表达能力有限——它只支持最基本的命题逻辑。在实际机器人任务中，我们常常需要更丰富的表达能力。ADL (Action Description Language) 是 STRIPS 的重要扩展，PDDL 通过 :requirements 声明来启用这些特性。
3.6.1 条件效果 (Conditional Effects)¶
动机：在 STRIPS 中，一个动作的效果是固定的。但现实中，同一个动作在不同条件下可能产生不同的结果。例如，"擦桌子"的效果取决于桌上是否有水渍——如果有水渍就变干净，如果没有则无变化。
语法：使用 (when <条件> <效果>) 表达条件效果。
(define (domain kitchen-adl)
(:requirements :strips :typing :conditional-effects)
(:types object location robot surface-type)
(:predicates
(on ?obj - object ?loc - location)
(holding ?r - robot ?obj - object)
(hand-empty ?r - robot)
(dirty ?loc - location)
(wet ?obj - object)
(is-sponge ?obj - object)
(fragile ?obj - object)
(broken ?obj - object)
)
;; pick 动作：如果物体是易碎的且没有小心抓取，可能会破损
;; 用条件效果表达：抓取易碎物体时标记为"需要小心"
(:action pick
:parameters (?r - robot ?obj - object ?loc - location)
:precondition (and
(on ?obj ?loc)
(hand-empty ?r)
(not (broken ?obj))
)
:effect (and
(holding ?r ?obj)
(not (on ?obj ?loc))
(not (hand-empty ?r))
;; 条件效果：如果物体是湿的，抓取后手也变湿
(when (wet ?obj) (wet ?r))
)
)
;; wipe 动作：擦拭效果取决于是否用海绵
(:action wipe
:parameters (?r - robot ?tool - object ?loc - location)
:precondition (and
(holding ?r ?tool)
(is-sponge ?tool)
)
:effect (and
;; 条件效果：只有脏的表面才会被清洁
(when (dirty ?loc) (not (dirty ?loc)))
;; 擦拭后海绵变湿
(wet ?tool)
)
)
)
为什么条件效果重要？ 如果没有条件效果，你必须为每种条件组合写一个单独的动作。例如 pick-fragile、pick-normal、pick-wet 等，动作数量会组合爆炸。条件效果让一个动作模板就能覆盖所有情况。
3.6.2 量化前提 (Quantified Preconditions)¶
动机：STRIPS 中的前提条件只能列举具体的命题。但有时我们需要表达"所有物体都满足某条件"或"存在某个物体满足某条件"。
语法：使用 (forall ...) 和 (exists ...) 量词。
(define (domain kitchen-quantified)
(:requirements :strips :typing :universal-preconditions :existential-preconditions)
(:types object location robot)
(:predicates
(on ?obj - object ?loc - location)
(holding ?r - robot ?obj - object)
(hand-empty ?r - robot)
(clean ?loc - location)
(at-robot ?r - robot ?loc - location)
)
;; 清洁表面：前提是表面上没有任何物体
;; 用全称量词表达 "对所有物体 ?x，?x 都不在 ?loc 上"
(:action clean-surface
:parameters (?r - robot ?loc - location)
:precondition (and
(at-robot ?r ?loc)
(hand-empty ?r)
;; forall: 桌面上不能有任何物体
(forall (?x - object) (not (on ?x ?loc)))
)
:effect (clean ?loc)
)
;; 紧急撤离：存在某个位置是安全的
(:action evacuate
:parameters (?r - robot ?from - location)
:precondition (and
(at-robot ?r ?from)
;; exists: 存在至少一个安全位置
(exists (?safe - location) (clean ?safe))
)
:effect (and
;; 移动到某个安全位置 (简化处理)
(not (at-robot ?r ?from))
)
)
)
grounding 代价：量化前提在 grounding 阶段会被展开为具体实例。如果有 \(n\) 个物体，(forall (?x - object) ...) 会被展开为 \(n\) 个命题的合取。这意味着物体数量增加时，grounding 后的问题规模会快速增长。这是 PDDL 可扩展性的一个根本瓶颈。
3.6.3 时态 PDDL (Temporal PDDL) 与 Durative Actions ⭐⭐⭐¶
动机：基本 PDDL 假设所有动作是瞬时的和串行的——一个动作完成后才能开始下一个。但在真实机器人操作中，很多动作有持续时间，而且多个动作可以并行执行。例如：左手拿杯子的同时，右手可以拿书。
Durative Action 语法（PDDL 2.1 引入）：
(define (domain kitchen-temporal)
(:requirements :strips :typing :durative-actions)
(:types object location robot arm)
(:predicates
(on ?obj - object ?loc - location)
(holding ?a - arm ?obj - object)
(arm-empty ?a - arm)
(reachable ?a - arm ?loc - location)
(moving ?a - arm)
)
;; 持续性动作：pick 需要 3 秒
(:durative-action pick
:parameters (?a - arm ?obj - object ?loc - location)
:duration (= ?duration 3) ;; 持续 3 个时间单位
:condition (and
;; at start: 动作开始时必须满足
(at start (on ?obj ?loc))
(at start (arm-empty ?a))
(at start (reachable ?a ?loc))
;; over all: 整个执行期间必须满足
(over all (reachable ?a ?loc))
)
:effect (and
;; at start: 动作开始时立即生效
(at start (not (arm-empty ?a)))
(at start (moving ?a))
;; at end: 动作结束时生效
(at end (holding ?a ?obj))
(at end (not (on ?obj ?loc)))
(at end (not (moving ?a)))
)
)
;; 持续性动作：place 需要 2 秒
(:durative-action place
:parameters (?a - arm ?obj - object ?loc - location)
:duration (= ?duration 2)
:condition (and
(at start (holding ?a ?obj))
(at start (reachable ?a ?loc))
(over all (reachable ?a ?loc))
)
:effect (and
(at start (not (holding ?a ?obj)))
(at start (moving ?a))
(at end (on ?obj ?loc))
(at end (arm-empty ?a))
(at end (not (moving ?a)))
)
)
;; 移动需要 5 秒
(:durative-action move-to
:parameters (?a - arm ?from - location ?to - location)
:duration (= ?duration 5)
:condition (and
(at start (reachable ?a ?from))
(over all (not (moving ?a))) ;; 移动时不能同时操作
)
:effect (and
(at start (not (reachable ?a ?from)))
(at end (reachable ?a ?to))
)
)
)
时态 PDDL 的关键概念：
| 时间条件/效果 | 含义 | 使用场景 |
|---|---|---|
at start |
动作起始瞬间 | 资源锁定（手不再空闲） |
at end |
动作结束瞬间 | 结果生效（物体到达目标） |
over all |
整个执行期间 | 不变量（手臂必须保持可达） |
?duration |
动作持续时间 | 可以是常数或表达式 |
为什么时态 PDDL 与 TAMP 密切相关？ 在 TAMP 中，运动规划器返回的轨迹有确定的执行时间。如果任务规划器能考虑时间约束，就可以发现并行执行的机会，从而缩短整体任务完成时间。例如，双臂机器人可以左右手同时工作——时态 PDDL 能自然地表达这种并行性。
3.6.4 数值流变量 (Numeric Fluents) ⭐⭐⭐¶
动机：标准 PDDL 只有布尔谓词——要么真要么假。但机器人操作经常涉及数值量：电池电量、物体重量、距离等。数值流变量 (numeric fluents) 是 PDDL 2.1 引入的重要扩展。
(define (domain kitchen-numeric)
(:requirements :strips :typing :numeric-fluents)
(:types object location robot)
(:predicates
(on ?obj - object ?loc - location)
(holding ?r - robot ?obj - object)
(hand-empty ?r - robot)
(at-robot ?r - robot ?loc - location)
)
;; 数值函数定义
(:functions
(weight ?obj - object) ;; 物体重量 (kg)
(capacity ?r - robot) ;; 机器人最大负载 (kg)
(battery-level ?r - robot) ;; 电池电量 (%)
(distance ?from - location ?to - location) ;; 位置间距离 (m)
(total-energy-used) ;; 累计能耗
)
;; pick 动作：检查重量约束，消耗能量
(:action pick
:parameters (?r - robot ?obj - object ?loc - location)
:precondition (and
(on ?obj ?loc)
(hand-empty ?r)
(at-robot ?r ?loc)
;; 数值前提：物体重量不超过机器人负载能力
(<= (weight ?obj) (capacity ?r))
;; 数值前提：电池电量足够
(>= (battery-level ?r) 5)
)
:effect (and
(holding ?r ?obj)
(not (on ?obj ?loc))
(not (hand-empty ?r))
;; 数值效果：抓取消耗能量，正比于物体重量
(decrease (battery-level ?r) (* 2 (weight ?obj)))
(increase (total-energy-used) (* 2 (weight ?obj)))
)
)
;; move 动作：消耗与距离成正比的能量
(:action move-to
:parameters (?r - robot ?from - location ?to - location)
:precondition (and
(at-robot ?r ?from)
(>= (battery-level ?r)
(* 3 (distance ?from ?to))) ;; 足够的电量
)
:effect (and
(at-robot ?r ?to)
(not (at-robot ?r ?from))
(decrease (battery-level ?r) (* 3 (distance ?from ?to)))
(increase (total-energy-used) (* 3 (distance ?from ?to)))
)
)
)
对应的 Problem 文件：
(define (problem kitchen-task-numeric)
(:domain kitchen-numeric)
(:objects
panda - robot
cup plate bowl - object
table counter shelf - location
)
(:init
(on cup table) (on plate table) (on bowl counter)
(hand-empty panda) (at-robot panda table)
;; 数值初始化
(= (weight cup) 0.3)
(= (weight plate) 0.5)
(= (weight bowl) 0.8)
(= (capacity panda) 2.0)
(= (battery-level panda) 100)
(= (distance table counter) 1.5)
(= (distance table shelf) 2.0)
(= (distance counter shelf) 1.0)
(= (total-energy-used) 0)
)
(:goal (and (on cup shelf) (on plate shelf) (on bowl shelf)))
;; 优化目标：最小化总能耗
(:metric minimize (total-energy-used))
)
数值流变量与 TAMP 的联系：数值流变量提供了在符号层面编码部分几何信息（距离、重量）的途径。虽然它仍无法完整表达 3D 几何约束，但相比纯布尔 PDDL 已经前进了一大步。在实际的 TAMP 系统中，数值流变量常用于对运动规划代价的粗略估计，帮助任务规划器选择更合理的动作序列。
陷阱框
ADL 特性的计算代价：PDDL 的高级特性（条件效果、量化前提、数值流变量）虽然增强了表达能力，但每一种都会显著增加规划器的计算负担。条件效果使状态转移计算变复杂，量化前提使 grounding 规模膨胀，数值流变量使搜索空间从有限变为无限。因此在实际 TAMP 系统中，应当谨慎选择需要的特性——不要因为"可以用"就全部启用，而是根据问题需求选择最小必要子集。
时态规划器稀缺：能处理 durative actions 的规划器远少于处理基本 STRIPS 的规划器。OPTIC、POPF 等时态规划器的鲁棒性和效率都不如 FF、FastDownward 等经典规划器。如果你的任务不需要并行执行，不要无谓地使用时态 PDDL。
练习 3¶
- (⭐) 修改上面的 PDDL domain，增加一个
wipe动作，前提是桌面上没有任何物体且手里拿着抹布 - (⭐⭐) 添加一个新物体
cloth（抹布）在抽屉里 (drawer)，修改目标使得桌面最终被擦干净。写出完整的 problem 文件 - (⭐⭐) 如果有 5 个物体需要移动到 5 个不同位置，PDDL 规划器的 grounding 阶段会生成多少个具体的
pick动作实例？(提示：考虑参数组合) - (⭐⭐⭐) 写一个完整的 PDDL domain 和 problem 文件，实现经典的 Blocksworld 问题：有 4 个方块 A、B、C、D，初始状态为 A 在 B 上、C 在 D 上、B 和 D 在桌面上；目标状态为 A-B-C-D 从上到下堆叠。你需要定义
pick-up（从桌面拿起）、put-down（放到桌面）、stack（堆叠到另一个方块上）、unstack（从方块上取下）四个动作 - (⭐⭐⭐) 使用 ADL 条件效果重写 Blocksworld domain，将四个动作合并为两个通用动作
grab和release，利用条件效果处理"从桌面拿"和"从方块上拿"的区别
4. 采样运动规划回顾 ⭐⭐¶
4.1 构型空间 (Configuration Space, C-space)¶
运动规划的第一个核心概念是构型空间。
定义：构型 (configuration) \(\mathbf{q}\) 是完全描述机器人所有关节状态的向量。对于一个 \(n\) 自由度的机器人：
\[
\mathbf{q} = (q_1, q_2, \ldots, q_n) \in \mathcal{C} \subseteq \mathbb{R}^n
\]
| 机器人类型 | 构型空间维度 | 构型示例 |
|---|---|---|
| 平面移动机器人 | 3 (\(x, y, \theta\)) | \((1.5, 2.0, 0.3)\) |
| 6-DOF 机械臂 | 6 (\(q_1, \ldots, q_6\)) | \((\pi/4, -\pi/3, \ldots)\) |
| 7-DOF 冗余臂 (如 Panda) | 7 | 更灵活，有自运动 |
| 人形机器人 | 30+ | 高维规划非常困难 |
构型空间被划分为： - 自由空间 \(\mathcal{C}_{free}\)：不与障碍物碰撞的构型集合 - 障碍空间 \(\mathcal{C}_{obs}\)：与障碍物碰撞的构型集合
\[
\mathcal{C} = \mathcal{C}_{free} \cup \mathcal{C}_{obs}, \quad \mathcal{C}_{free} \cap \mathcal{C}_{obs} = \emptyset
\]
运动规划问题：在 \(\mathcal{C}_{free}\) 中找一条从 \(\mathbf{q}_{start}\) 到 \(\mathbf{q}_{goal}\) 的连续路径。
4.2 PRM：多查询方法¶
PRM (Probabilistic Roadmap) 分两个阶段：
PRM 算法:
┌─────────────────────────────┐
│ 阶段 1: 离线建图 (Learning) │
│ 1. 在 C_free 中随机采样 N 个点│
│ 2. 对每个点，连接 k 个最近邻 │
│ 3. 检查连线是否在 C_free 中 │
│ 4. 构建路图 (Roadmap) G │
├─────────────────────────────┤
│ 阶段 2: 在线查询 (Query) │
│ 1. 将 q_start, q_goal 连入 G │
│ 2. 在 G 上用 A*/Dijkstra 搜索│
└─────────────────────────────┘
优势：路图建好后，新查询只需图搜索，非常快。
劣势：建图需要大量预计算；环境变化后需要重建。
4.3 RRT/RRT*：单查询方法¶
RRT (Rapidly-exploring Random Tree) 通过增量式地扩展一棵树来探索 C-space：
RRT 算法:
Input: q_start, q_goal, C_free
T.init(q_start)
for i = 1 to N:
q_rand ← RandomSample(C)
q_near ← NearestNode(T, q_rand)
q_new ← Steer(q_near, q_rand, step_size)
if CollisionFree(q_near, q_new):
T.add_node(q_new)
T.add_edge(q_near, q_new)
if Distance(q_new, q_goal) < threshold:
return ExtractPath(T, q_start, q_new)
return FAILURE
RRT 的关键属性： - 概率完备 (probabilistically complete)：如果解存在，随着采样数趋于无穷，找到解的概率趋于 1 - 不保证最优：找到的路径通常不是最短的
RRT*：在 RRT 基础上增加了 rewire 操作，保证渐近最优 (asymptotically optimal)：
\[
\lim_{n \to \infty} P(\text{cost}(\text{path}_n) = \text{cost}^*) = 1
\]
4.3.1 RRT-Connect：双向搜索加速 ⭐⭐¶
在 TAMP 的内层运动规划中，RRT-Connect (Kuffner & LaValle, 2000) 是最常用的变体，因为它同时从起点和终点生长两棵树，在实践中比单向 RRT 快一个数量级。
C++ 伪代码：
/**
* RRT-Connect: 双向 RRT 算法
*
* 核心思想：两棵树交替扩展，每次不是只走一步 (Extend)，
* 而是贪心地走到底 (Connect)，大幅加速两树汇合。
*/
struct Node {
Eigen::VectorXd config; // 构型 (关节角度)
int parent_idx; // 父节点索引
};
enum ExtendResult { REACHED, ADVANCED, TRAPPED };
class RRTConnect {
public:
// 核心数据结构：两棵树
std::vector<Node> tree_start_; // 从起点生长的树
std::vector<Node> tree_goal_; // 从终点生长的树
/**
* Extend: 向 q_target 方向扩展一步
* 返回: REACHED (到达目标), ADVANCED (前进了一步), TRAPPED (碰撞)
*/
ExtendResult Extend(std::vector<Node>& tree,
const Eigen::VectorXd& q_target) {
// 1. 找到树中距离 q_target 最近的节点
int nearest_idx = FindNearest(tree, q_target);
const Eigen::VectorXd& q_near = tree[nearest_idx].config;
// 2. 从 q_near 向 q_target 方向走一步 (步长限制)
Eigen::VectorXd direction = q_target - q_near;
double dist = direction.norm();
Eigen::VectorXd q_new;
if (dist <= step_size_) {
q_new = q_target; // 已经足够近，直接到达
} else {
q_new = q_near + step_size_ * direction.normalized();
}
// 3. 碰撞检查：q_near 到 q_new 的路径段
if (!IsCollisionFree(q_near, q_new)) {
return TRAPPED; // 碰撞，无法扩展
}
// 4. 加入新节点
tree.push_back({q_new, nearest_idx});
if ((q_new - q_target).norm() < tolerance_) {
return REACHED; // 到达目标
}
return ADVANCED; // 前进了一步但未到达
}
/**
* Connect: 贪心地反复 Extend 直到到达或被困
* 这是 RRT-Connect 的关键——不是只走一步，而是尽可能走到底
*/
ExtendResult Connect(std::vector<Node>& tree,
const Eigen::VectorXd& q_target) {
ExtendResult result = ADVANCED;
while (result == ADVANCED) {
result = Extend(tree, q_target);
}
return result; // REACHED 或 TRAPPED
}
/**
* Plan: 主规划循环
*/
std::vector<Eigen::VectorXd> Plan(
const Eigen::VectorXd& q_start,
const Eigen::VectorXd& q_goal,
int max_iterations = 5000) {
tree_start_.clear();
tree_goal_.clear();
tree_start_.push_back({q_start, -1});
tree_goal_.push_back({q_goal, -1});
// 用指针交替引用两棵树
auto* tree_a = &tree_start_;
auto* tree_b = &tree_goal_;
for (int i = 0; i < max_iterations; ++i) {
// 1. 随机采样
Eigen::VectorXd q_rand = RandomSample();
// 2. 树 A 向随机点 Extend 一步
if (Extend(*tree_a, q_rand) != TRAPPED) {
// 3. 取树 A 最新加入的节点
Eigen::VectorXd q_new = tree_a->back().config;
// 4. 树 B 向 q_new 贪心 Connect
// (关键区别: Extend vs Connect)
if (Connect(*tree_b, q_new) == REACHED) {
// 两棵树连接成功！提取完整路径
return ExtractPath(*tree_a, *tree_b);
}
}
// 5. 交换 A 和 B，下次迭代反过来
std::swap(tree_a, tree_b);
}
return {}; // 规划失败
}
private:
double step_size_ = 0.1; // 步长
double tolerance_ = 0.01; // 到达容差
};
为什么 RRT-Connect 比单向 RRT 快？
| 特性 | 单向 RRT | RRT-Connect |
|---|---|---|
| 树的数量 | 1 棵 | 2 棵（起点 + 终点） |
| 扩展策略 | Extend (走一步) | Extend + Connect (走到底) |
| 连接方式 | 到达阈值判断 | 两树主动靠近 |
| 平均迭代数 | \(O(1/p)\) | \(O(1/\sqrt{p})\)，其中 \(p\) 是问题的"可连通概率" |
双向搜索的直觉：两棵树从两端向中间生长，两边同时缩小搜索范围。如果把 C-space 想象成一片森林，单向 RRT 是从一端走到另一端，RRT-Connect 是两个人从两端向中间走——他们碰面的概率高得多。
4.3.2 OMPL 代码示例：7-DOF 机械臂规划 ⭐⭐¶
OMPL (Open Motion Planning Library) 是运动规划领域的标准开源库，MoveIt 的后端就是 OMPL。以下展示如何用 OMPL 的 C++ API 为 7-DOF 机械臂（如 Franka Panda）进行运动规划。
/**
* OMPL 运动规划示例：7-DOF 机械臂
*
* 编译: g++ -std=c++17 ompl_7dof.cpp -lompl -lboost_serialization -o plan
* 依赖: sudo apt install libompl-dev
*/
#include <ompl/base/SpaceInformation.h>
#include <ompl/base/spaces/RealVectorStateSpace.h>
#include <ompl/geometric/SimpleSetup.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <ompl/geometric/planners/rrt/RRTstar.h>
#include <ompl/geometric/planners/prm/PRM.h>
#include <iostream>
namespace ob = ompl::base;
namespace og = ompl::geometric;
/**
* 碰撞检查回调函数
* 实际应用中，这里会调用 FCL/Bullet 等碰撞检测库
* 教学目的下使用简化版本
*/
bool isStateValid(const ob::State* state) {
// 获取状态 (7个关节角度)
const auto* realState =
state->as<ob::RealVectorStateSpace::StateType>();
// 简化碰撞检查：避开关节极限附近的区域
// 实际应用中应使用正运动学 + 碰撞几何体
for (int i = 0; i < 7; ++i) {
double q = realState->values[i];
// 模拟自碰撞区域 (简化)
if (i >= 2 && std::abs(q) > 2.5) {
return false; // 碰撞
}
}
return true; // 无碰撞
}
int main() {
// ====== 1. 定义构型空间 (C-space) ======
// Panda 有 7 个旋转关节
auto space = std::make_shared<ob::RealVectorStateSpace>(7);
// 设置关节范围 (Panda 的实际关节限位)
ob::RealVectorBounds bounds(7);
// 各关节限位 (弧度)
double lower[] = {-2.8973, -1.7628, -2.8973, -3.0718,
-2.8973, -0.0175, -2.8973};
double upper[] = { 2.8973, 1.7628, 2.8973, -0.0698,
2.8973, 3.7525, 2.8973};
for (int i = 0; i < 7; ++i) {
bounds.setLow(i, lower[i]);
bounds.setHigh(i, upper[i]);
}
space->setBounds(bounds);
// ====== 2. 创建 SimpleSetup ======
og::SimpleSetup ss(space);
// 设置碰撞检查函数
ss.setStateValidityChecker(isStateValid);
// 设置碰撞检查分辨率 (路径上的检查密度)
// 值越小检查越密 (更安全但更慢)
ss.getSpaceInformation()->setStateValidityCheckingResolution(0.01);
// ====== 3. 设置起点和终点 ======
ob::ScopedState<> start(space);
ob::ScopedState<> goal(space);
// 起始构型：home 位置
start[0] = 0.0; start[1] = -0.785; start[2] = 0.0;
start[3] = -2.356; start[4] = 0.0; start[5] = 1.571;
start[6] = 0.785;
// 目标构型：抓取姿态 (由 IK 求解器给出)
goal[0] = 0.5; goal[1] = 0.3; goal[2] = -0.5;
goal[3] = -1.8; goal[4] = 0.2; goal[5] = 2.0;
goal[6] = 1.2;
ss.setStartAndGoalStates(start, goal);
// ====== 4. 选择规划器 ======
// 方式 A: RRT-Connect (TAMP 最常用)
auto planner = std::make_shared<og::RRTConnect>(
ss.getSpaceInformation());
planner->setRange(0.5); // 步长
// // 方式 B: RRT* (如果需要渐近最优)
// auto planner = std::make_shared<og::RRTstar>(
// ss.getSpaceInformation());
// // 方式 C: PRM (如果需要多次查询)
// auto planner = std::make_shared<og::PRM>(
// ss.getSpaceInformation());
ss.setPlanner(planner);
// ====== 5. 求解 ======
ob::PlannerStatus solved = ss.solve(5.0); // 最多5秒
if (solved) {
std::cout << "Found solution!" << std::endl;
// 路径简化 (去除冗余的中间点)
ss.simplifySolution();
// 输出路径
og::PathGeometric& path = ss.getSolutionPath();
path.interpolate(50); // 插值到 50 个路径点
std::cout << "Path has " << path.getStateCount()
<< " states" << std::endl;
// 打印前3个和最后1个路径点
for (size_t i = 0; i < std::min((size_t)3,
path.getStateCount()); ++i) {
const auto* s = path.getState(i)
->as<ob::RealVectorStateSpace::StateType>();
std::cout << " q[" << i << "] = (";
for (int j = 0; j < 7; ++j) {
std::cout << s->values[j];
if (j < 6) std::cout << ", ";
}
std::cout << ")" << std::endl;
}
std::cout << " ..." << std::endl;
} else {
std::cout << "No solution found!" << std::endl;
}
return 0;
}
OMPL 在 TAMP 中的角色：在 PDDLStream 等 TAMP 框架中，每当需要检查一个离散动作（如 pick(cup)）的运动可行性时，底层就会调用 OMPL 的 RRT-Connect 来规划从当前构型到目标抓取构型的无碰撞路径。一个典型的 TAMP 问题可能调用运动规划器 50-200 次，因此运动规划器的速度直接决定了整个 TAMP 系统的性能。
4.3.3 RRT* 的渐近最优性证明概述 ⭐⭐⭐⭐¶
定理 (Karaman & Frazzoli, 2011)：RRT 不是渐近最优的，而 RRT* 是渐近最优的。
为什么 RRT 不是渐近最优的？ 直觉解释：
RRT 中每个新节点只连接到最近邻，一旦连接就不再修改。随着采样数增加，树的拓扑结构被早期的随机连接所"锁定"——后来的样本无法改善已有路径。形式化地说，Karaman 和 Frazzoli 证明了：
\[
P\left(\lim_{n \to \infty} c_n^{RRT} = c^* \right) = 0
\]
也就是说，即使采样到无穷多个点，RRT 找到的路径代价几乎不可能收敛到最优值 \(c^*\)。
RRT* 的两个关键改进：
-
近邻搜索半径：使用随采样数增长的半径 \(r_n = \gamma \left(\frac{\log n}{n}\right)^{1/d}\)（而不是固定的最近邻），其中 \(d\) 是 C-space 维度，\(\gamma\) 是与空间有关的常数
-
Rewire 操作：每加入一个新节点 \(q_{new}\)，检查 \(r_n\) 范围内的所有节点，如果通过 \(q_{new}\) 中转能降低某节点到起点的代价，就重新连接（rewire）
渐近最优性的证明思路：
\[
\text{Step 1: } \text{令 } \sigma^* \text{ 为最优路径，cost}(\sigma^*) = c^*
\]
\[
\text{Step 2: } \text{将 } \sigma^* \text{ 用 } \delta\text{-细管覆盖 (}\delta\text{-tube)}
\]
\[
\text{Step 3: } \text{当 } n \to \infty \text{，管中每个小球内至少有一个样本 (覆盖性)}
\]
\[
\text{Step 4: } \text{rewire 保证这些样本被最优连接}
\]
\[
\text{Step 5: } \text{因此 RRT* 的路径 cost} \to c^* \quad \square
\]
关键的技术步骤是证明半径 \(r_n\) 的选择足以保证"好的连接"。如果 \(r_n\) 太小，rewire 范围不够；如果太大，计算代价太高。\(\gamma (\log n / n)^{1/d}\) 恰好是平衡点——它保证每个球内的期望样本数是 \(\Theta(\log n)\)，既足以找到好的连接，又不会让每个节点的近邻搜索过于昂贵。
实践意义：在 TAMP 中，RRT* 通常不直接使用（太慢），但其变体 Informed RRT* 和 BIT* 在需要高质量轨迹时很有价值。例如在 LGP 的 Layer 3 中，如果 KOMO 优化陷入局部最优，可以用 Informed RRT* 作为替代方案。
陷阱框
RRT 的"概率完备"不等于"一定能找到"。概率完备意味着随着迭代数趋于无穷，找到解的概率趋于 1。但在有限时间内，RRT 可能找不到解——尤其是在窄通道 (narrow passage) 问题中。TAMP 系统必须为运动规划器设置超时时间，并在超时后能够有意义地回退（例如尝试不同的抓取姿态或不同的动作顺序）。
C-space 维度诅咒：RRT 的采样效率随维度指数下降。对于 7-DOF 机械臂（7维 C-space），RRT 通常在 1 秒内完成；但对于双臂系统（14维）或人形机器人（30+维），可能需要数十秒甚至无法在合理时间内找到解。这也是为什么 TAMP 研究者越来越关注轨迹优化方法（CHOMP、TrajOpt、KOMO）——它们利用梯度信息，在高维空间中比采样方法更有效率。
4.4 轨迹优化：CHOMP 与 TrajOpt¶
采样方法找到的路径通常不够平滑。轨迹优化方法将运动规划转化为优化问题：
\[
\min_{\xi} \underbrace{f_{smooth}(\xi)}_{\text{平滑性}} + \lambda \underbrace{f_{obs}(\xi)}_{\text{避障代价}}
\]
其中 \(\xi = (\mathbf{q}_0, \mathbf{q}_1, \ldots, \mathbf{q}_T)\) 是离散化的轨迹。
CHOMP (Covariant Hamiltonian Optimization for Motion Planning)： - 平滑性代价：\(f_{smooth} = \sum_t \|\mathbf{q}_{t+1} - \mathbf{q}_t\|^2\)（动力学能量） - 避障代价：使用 signed distance field - 优化方法：协变梯度下降
TrajOpt： - 使用 sequential convex optimization (SCO) - 碰撞约束用凸松弛处理 - 实践中比 CHOMP 更快收敛
4.5 运动规划与 TAMP 的关系¶
在 TAMP 框架中，运动规划扮演的是"内层求解器" (inner solver) 的角色：
TAMP 调用关系:
┌────────────────────────────┐
│ 任务规划器 (外层) │
│ "先 pick(cup)，再 place" │
│ │ │
│ ▼ │
│ ┌──────────────────┐ │
│ │ 运动规划器 (内层) │ │
│ │ "pick(cup) 的运动 │ │
│ │ 是否物理可行？" │ │
│ └──────────────────┘ │
│ │ │
│ 可行 → 继续下一步 │
│ 不可行 → 反馈给外层 │
└────────────────────────────┘
每当任务规划器提出一个离散动作（如 pick(cup, table)），运动规划器需要：
1. 为该动作生成连续参数（抓取姿态、接近轨迹等）
2. 检查该运动是否无碰撞
3. 如果不可行，将信息反馈给任务规划器
练习 4¶
- (⭐) 一个 6-DOF 机械臂的 C-space 是几维的？如果每个关节的范围是 \([-\pi, \pi]\)，C-space 的体积是多少？
- (⭐⭐) 为什么 RRT 适合作为 TAMP 的内层运动规划器，而 PRM 通常不太适合？(提示：考虑环境变化)
- (⭐⭐) 在 pick-and-place 任务中，机器人需要为"抓取杯子"生成运动轨迹。列举至少 3 个需要运动规划器解决的子问题。
5. TAMP 的核心挑战 ⭐⭐¶
5.1 无限分支因子 (Infinite Branching Factor)¶
在经典任务规划中，分支因子是有限的——每个状态下可执行的动作数量是有限的。但在 TAMP 中：
离散部分：选择什么动作 + 操作什么物体 → 有限选择
\[
\text{pick}(\text{cup}, \text{table}), \quad \text{pick}(\text{book}, \text{table}), \quad \ldots
\]
连续部分：每个动作需要连续参数 → 无限选择
\[
\text{pick}(\text{cup}, \text{table}, \underbrace{\text{grasp\_pose}}_{\in SE(3)}, \underbrace{\text{approach\_traj}}_{\in \mathcal{C}^T})
\]
一个抓取姿态 \(\mathbf{g} \in SE(3)\) 有 6 个自由度（3 平移 + 3 旋转），即无穷多种可能。
\[
\text{分支因子} = \underbrace{|\mathcal{A}| \cdot |\mathcal{O}|^{k}}_{\text{有限，离散}} \times \underbrace{|\mathbb{R}^d|}_{\text{无限，连续}} = \infty
\]
5.2 几何可行性 (Geometric Feasibility)¶
符号上可行的计划不一定物理上可行：
| 符号状态 | 符号判断 | 几何现实 |
|---|---|---|
pick(cup, table) |
Pre 满足 → 可执行 | 杯子被其他物体包围，无法接近 |
place(cup, shelf_top) |
Pre 满足 → 可执行 | 架子太高，超出机械臂工作空间 |
move-to(arm, A, B) |
Pre 满足 → 可执行 | A 到 B 之间有不可移动的障碍物 |
形式化地说，对于每个符号动作 \(a\)，存在一个几何可行性函数：
\[
\text{Feasible}(a, \mathbf{q}, \mathcal{W}) = \begin{cases}
\text{True} & \text{if } \exists \text{ 运动轨迹使 } a \text{ 在世界 } \mathcal{W} \text{ 中可执行} \\
\text{False} & \text{otherwise}
\end{cases}
\]
计算这个函数本身就需要调用运动规划器，而运动规划器的每次调用都有计算代价。
5.3 耦合约束 (Coupled Constraints)¶
物体之间的约束可能跨越多个动作步骤：
场景: 杯子A在盘子B上面，盘子B在桌子上
目标: 把盘子B放到架子上
简单思路: pick(B), place(B, shelf)
问题: 拿B时A会掉下来！
正确方案:
1. pick(A)
2. place(A, temp_location) ← A 放哪里？temp_location 是连续变量！
3. pick(B)
4. place(B, shelf)
更复杂的情况——阻挡问题 (blocking)：
场景: 要从架子上取物体C，但物体D挡在前面
任务规划器看到: On(C, shelf), On(D, shelf), goal: Holding(C)
任务规划器的方案: pick(C, shelf)
运动规划器: 失败！D 挡住了 C！
正确方案:
1. pick(D, shelf)
2. place(D, ???) ← 放哪里才不会挡住后续操作？
3. pick(C, shelf)
关键洞察：临时放置位置的选择是一个连续决策，它影响后续所有动作的可行性。这就是"耦合"的含义——当前的连续决策与未来的离散/连续决策相互影响。
5.4 形式化：混合状态空间¶
TAMP 的状态空间是混合的 (hybrid)，同时包含离散和连续分量：
\[
\mathcal{S} = \mathcal{S}_{discrete} \times \mathcal{S}_{continuous}
\]
其中： - \(\mathcal{S}_{discrete}\)：符号状态，如 \(\{On(cup, table), HandEmpty, \ldots\}\) - \(\mathcal{S}_{continuous}\)：几何状态，如所有物体的位姿 + 机器人构型
一个 TAMP 动作是一个混合动作：
\[
a = (a_{sym}, \boldsymbol{\theta})
\]
其中 \(a_{sym}\) 是符号动作（如 pick），\(\boldsymbol{\theta} \in \mathbb{R}^d\) 是连续参数（如抓取姿态、放置位置）。
TAMP 问题的形式化定义：
\[
\text{Find } \pi = [(a_1^{sym}, \boldsymbol{\theta}_1), (a_2^{sym}, \boldsymbol{\theta}_2), \ldots, (a_n^{sym}, \boldsymbol{\theta}_n)]
\]
\[
\text{s.t. } \forall i: \text{Pre}_{sym}(a_i^{sym}, s_i) \wedge \text{Feasible}(a_i^{sym}, \boldsymbol{\theta}_i, s_i)
\]
\[
s_{i+1} = \text{Transition}(s_i, a_i^{sym}, \boldsymbol{\theta}_i)
\]
\[
s_{n+1} \models G
\]
自测检查点 5¶
请解释"无限分支因子"的含义，并说明它为什么使得标准的图搜索算法（如 BFS、A*）无法直接用于 TAMP。
6. TAMP 算法分类框架 ⭐⭐⭐¶
6.1 Garrett et al. 2021 的分类法¶
Garrett 等人在 2021 年的 Annual Review 综述中提出了一个非常清晰的分类框架，用两个维度来组织 TAMP 算法：
维度 1：连续空间的处理策略
| 策略 | 思路 | 代表方法 |
|---|---|---|
| 采样 (Sampling) | 对连续参数进行随机采样，生成有限候选集 | PDDLStream, FFRob |
| 优化 (Optimization) | 将连续参数作为优化变量，求解 NLP | LGP, KOMO |
维度 2：任务与运动的集成策略
| 策略 | 思路 | 优缺点 |
|---|---|---|
| 后验修复 (Post-hoc) | 先完成任务规划，再检查/修复运动可行性 | 简单但低效，前面分析过的"Plan-then-Check" |
| 交错 (Interleaved) | 任务规划和运动规划交替进行，运动结果反馈给任务 | 较好的平衡，PDDLStream |
| 联合 (Joint) | 在一个统一的空间中同时搜索离散和连续解 | 最紧密耦合，LGP |
6.2 分类矩阵¶
│ 后验修复 │ 交错 │ 联合
──────────────┼────────────────┼───────────────┼────────────────
采样 │ 朴素 │ PDDLStream │
(Sampling) │ Plan-Check │ FFRob │
──────────────┼────────────────┼───────────────┼────────────────
优化 │ │ │ LGP (Toussaint)
(Optim.) │ TrajOpt+STRIPS │ │ KOMO
──────────────┼────────────────┼───────────────┼────────────────
6.3 三种代表方法对比¶
| 特性 | PDDLStream | LGP | FFRob |
|---|---|---|---|
| 作者/团队 | Garrett (MIT) | Toussaint (TU Berlin) | Garrett (MIT, 早期) |
| 连续空间策略 | 采样 (Streams) | 优化 (NLP) | 采样 |
| 集成策略 | 交错 | 联合 | 交错 |
| 核心思想 | PDDL + 条件生成器 | 整个问题 = 一个数学规划 | 前向搜索 + 几何推理 |
| 任务规划器 | 标准 PDDL 规划器 | 骨架搜索 | 启发式搜索 |
| 运动规划器 | 外部调用 (如 BiRRT) | KOMO 轨迹优化 | 外部调用 |
| 优势 | 通用性强，易扩展 | 全局优化，轨迹质量高 | 理论保证好 |
| 劣势 | 采样效率依赖生成器质量 | 非凸问题可能陷入局部最优 | 扩展性有限 |
| 适用场景 | 通用操作任务 | 需要高质量轨迹的任务 | 理论研究 |
6.4 集成策略的直觉理解¶
后验修复 (Post-hoc):
任务规划 ──────→ 完整符号计划 ──────→ 运动验证 ──→ 可行？
│
Yes: 完成 │ No: 回到起点
↓
重新规划
交错 (Interleaved):
任务规划 ──→ 一步 ──→ 运动检查 ──→ 可行？
│
Yes: 下一步 │ No: 换个连续参数 / 换个动作
↓
局部回溯
联合 (Joint):
┌──────────────────────────────────┐
│ 统一搜索/优化空间 │
│ 同时决定: 做什么 + 怎么做 │
│ 离散变量和连续变量一起优化 │
└──────────────────────────────────┘
练习 6¶
- (⭐⭐) 为什么"后验修复"策略在实践中效率低下？请用一个具体的例子说明
- (⭐⭐⭐) 如果一个任务有 10 步，每步有 100 种可能的连续参数值，后验修复最坏需要检查多少种组合？交错策略能如何减少这个数字？
- (⭐⭐) 采样策略和优化策略各自在什么类型的 C-space 中表现更好？(提示：考虑窄通道问题)
7. PDDLStream 框架 ⭐⭐⭐¶
7.1 核心思想¶
PDDLStream (Garrett et al., 2020) 的核心创新是在 PDDL 的基础上增加了 Streams——连续参数的条件生成器。
类比： - PDDL 动作定义了"做什么"的规则 - Streams 定义了"连续参数从哪里来"的规则
传统 PDDL:
pick(robot, cup, table)
所有参数都是离散的、预定义的
PDDLStream:
pick(robot, cup, table, grasp_pose, approach_traj)
grasp_pose 和 approach_traj 由 Stream 生成
7.2 Stream 的定义¶
一个 Stream 是一个条件生成器 (conditional generator)：
\[
\text{Stream} = \langle \text{inputs}, \; \text{domain}, \; \text{outputs}, \; \text{certified}, \; \text{gen\_fn} \rangle
\]
| 组件 | 含义 | 示例 |
|---|---|---|
inputs |
输入参数 | 物体 ?obj，表面 ?surf |
domain |
输入必须满足的条件 | (Graspable ?obj) |
outputs |
输出参数（连续值） | 抓取姿态 ?grasp |
certified |
输出保证满足的条件 | (GraspPose ?obj ?grasp) |
gen_fn |
实际的生成函数（Python 代码） | 采样一个抓取姿态 |
示例：抓取姿态生成器
# PDDLStream 中的 Stream 定义
from pddlstream.language.stream import StreamInfo
# Stream 声明 (PDDL-like 语法)
"""
(:stream sample-grasp
:inputs (?obj)
:domain (Graspable ?obj)
:outputs (?grasp)
:certified (GraspPose ?obj ?grasp)
)
"""
def grasp_generator(obj):
"""
为给定物体生成抓取姿态的生成器函数。
每次调用 next() 返回一个新的抓取姿态。
"""
# 获取物体的几何信息
mesh = get_mesh(obj)
# 基于物体形状采样抓取姿态
while True:
# 在物体表面均匀采样接触点
contact_points = sample_contact_points(mesh, n=2)
# 计算夹爪姿态
grasp_pose = compute_grasp_from_contacts(contact_points)
# 检查力封闭 (force closure)
if is_force_closure(contact_points, mesh):
yield (grasp_pose,) # 返回元组
示例：逆运动学求解器
"""
(:stream inverse-kinematics
:inputs (?robot ?obj ?pose ?grasp)
:domain (and (KinBody ?robot) (GraspPose ?obj ?grasp) (AtPose ?obj ?pose))
:outputs (?conf ?traj)
:certified (and (Kin ?robot ?obj ?pose ?grasp ?conf ?traj)
(Conf ?robot ?conf)
(Traj ?robot ?traj))
)
"""
def ik_generator(robot, obj, pose, grasp):
"""
给定物体位姿和抓取姿态，计算机器人构型和接近轨迹。
"""
# 计算末端执行器目标位姿
ee_pose = multiply_poses(pose, invert_pose(grasp))
while True:
# 求解逆运动学 (可能有多解)
conf = solve_ik(robot, ee_pose, randomize=True)
if conf is not None and not check_collision(robot, conf):
# 规划接近轨迹
traj = plan_approach(robot, conf)
if traj is not None:
yield (conf, traj)
7.3 PDDLStream 的算法变体¶
PDDLStream 提供了三种主要的求解算法：
| 算法 | 策略 | 适用场景 |
|---|---|---|
| Incremental | 逐步增加 Stream 采样数量 | 简单问题，Stream 成功率高 |
| Focused | 只在需要时调用 Stream | 复杂问题，Stream 代价高 |
| Adaptive | 根据历史成功率调整采样 | 通用场景 |
Incremental 算法流程：
Incremental 算法:
max_samples = 1
while True:
1. 对每个 Stream 生成 max_samples 个样本
2. 将样本作为"事实"加入 PDDL 问题
3. 调用标准 PDDL 规划器求解
4. if 找到解:
验证解的可行性
if 可行: return 解
5. max_samples += 1 (增加采样密度)
Focused 算法的改进：
Focused 算法不盲目增加采样，而是分析规划失败的原因：
Focused 算法:
while True:
1. 用"乐观"假设调用 PDDL 规划器
(假设所有 Stream 都能成功)
2. 得到一个"骨架" (skeleton)
3. 按骨架需求，有针对性地调用 Stream
4. if 所有 Stream 成功: return 解
5. if 某个 Stream 失败:
将失败信息反馈给 PDDL 规划器
搜索替代骨架
7.4 完整的 PDDLStream Pick-and-Place 示例¶
"""
PDDLStream pick-and-place 示例 (简化版)
展示核心概念，非完整可运行代码
"""
from pddlstream.language.generator import from_gen_fn
from pddlstream.algorithms.focused import solve_focused
# ========== 1. 定义 Domain (PDDL 部分) ==========
DOMAIN_PDDL = """
(define (domain pick-place-stream)
(:predicates
(Conf ?q)
(Pose ?obj ?p)
(Grasp ?obj ?g)
(Kin ?obj ?p ?g ?q ?t)
(FreeTraj ?t)
(AtConf ?q)
(AtPose ?obj ?p)
(Holding ?obj)
(HandEmpty)
(Unsafe ?t ?obj ?p)
)
(:action pick
:parameters (?obj ?p ?g ?q ?t)
:precondition (and (Kin ?obj ?p ?g ?q ?t)
(AtPose ?obj ?p)
(HandEmpty)
(AtConf ?q))
:effect (and (Holding ?obj)
(not (AtPose ?obj ?p))
(not (HandEmpty)))
)
(:action place
:parameters (?obj ?p ?g ?q ?t)
:precondition (and (Kin ?obj ?p ?g ?q ?t)
(Holding ?obj)
(AtConf ?q))
:effect (and (AtPose ?obj ?p)
(HandEmpty)
(not (Holding ?obj)))
)
)
"""
# ========== 2. 定义 Streams ==========
STREAM_PDDL = """
(define (stream pick-place-stream)
(:stream sample-grasp
:inputs (?obj)
:domain (Graspable ?obj)
:outputs (?g)
:certified (Grasp ?obj ?g)
)
(:stream sample-pose
:inputs (?obj ?surface)
:domain (Stackable ?obj ?surface)
:outputs (?p)
:certified (and (Pose ?obj ?p) (Supported ?obj ?p ?surface))
)
(:stream inverse-kinematics
:inputs (?obj ?p ?g)
:domain (and (Pose ?obj ?p) (Grasp ?obj ?g))
:outputs (?q ?t)
:certified (and (Kin ?obj ?p ?g ?q ?t) (Conf ?q))
)
)
"""
# ========== 3. 定义生成器函数 ==========
def get_stream_map(robot, fixed_objects):
"""返回 Stream 名称到生成器函数的映射"""
def sample_grasp_fn(obj):
"""采样物体的抓取姿态"""
grasps = compute_grasp_candidates(obj)
for g in grasps:
yield (g,)
def sample_pose_fn(obj, surface):
"""采样物体在表面上的稳定放置姿态"""
for _ in range(100):
p = sample_placement(obj, surface)
if p is not None:
yield (p,)
def ik_fn(obj, pose, grasp):
"""逆运动学求解"""
ee_pose = compute_ee_target(pose, grasp)
conf = inverse_kinematics(robot, ee_pose)
if conf is not None:
traj = plan_motion(robot, conf, fixed_objects)
if traj is not None:
yield (conf, traj)
return {
'sample-grasp': from_gen_fn(sample_grasp_fn),
'sample-pose': from_gen_fn(sample_pose_fn),
'inverse-kinematics': from_gen_fn(ik_fn),
}
# ========== 4. 定义问题并求解 ==========
def create_problem(robot, objects, surfaces):
"""创建 PDDLStream 问题实例"""
init = [
('HandEmpty',),
('AtConf', get_current_conf(robot)),
('Conf', get_current_conf(robot)),
]
# 添加物体的当前位姿
for obj in objects:
init.append(('AtPose', obj, get_pose(obj)))
init.append(('Pose', obj, get_pose(obj)))
init.append(('Graspable', obj))
# 目标: 把 cup 放到 shelf 上
goal = ('and',
('AtPose', 'cup', 'shelf_pose'),
('HandEmpty',))
return DOMAIN_PDDL, STREAM_PDDL, init, goal
# 求解
solution = solve_focused(
DOMAIN_PDDL, STREAM_PDDL, init, goal,
stream_map=get_stream_map(robot, fixed),
max_time=60
)
if solution is not None:
plan, cost, evaluations = solution
print("Found plan:")
for action in plan:
print(f" {action.name}: {action.args}")
陷阱框
Stream 的质量决定 PDDLStream 的性能。如果抓取姿态生成器只能生成很少的有效姿态，或者生成的姿态分布不好（总是偏向一侧），整个系统的效率会急剧下降。设计好的 Stream 生成器是 PDDLStream 实践中最重要的工程工作。
7.5 完整 PDDLStream 示例：多物体厨房场景 ⭐⭐⭐¶
前面的 PDDLStream 示例较为简化。下面给出一个更接近实际应用的完整厨房场景，包含 3 个以上物体、多个存储位置（架子/桌子/抽屉），以及完整的 Stream 定义。
场景描述：
厨房场景:
┌─────────────────────────────────────────┐
│ │
│ ┌─────┐ ┌──────┐ ┌─────────┐ │
│ │shelf │ │counter│ │ drawer │ │
│ │ top │ │ │ │(closed) │ │
│ │ │ │ mug │ │ bowl │ │
│ │shelf │ │ plate │ │ │ │
│ │ mid │ │ │ └─────────┘ │
│ │ │ └──────┘ │
│ └─────┘ ┌───────┐ │
│ │ table │ │
│ ┌─────┐ │ spoon │ │
│ │robot│ │ │ │
│ │panda│ └───────┘ │
│ └─────┘ │
└─────────────────────────────────────────┘
物体: mug (杯子), plate (盘子), spoon (勺子), bowl (碗)
目标: mug → shelf_top, plate → shelf_mid,
spoon → drawer, bowl → counter
约束: 抽屉需要先打开才能放东西进去
完整 Domain 定义：
KITCHEN_DOMAIN = """
(define (domain kitchen-tamp)
(:predicates
;; 物体与位置关系
(Obj ?obj)
(Surface ?surf)
(Drawer ?d)
(Robot ?r)
;; 几何谓词 (由 Streams 认证)
(Pose ?obj ?pose)
(Grasp ?obj ?grasp)
(Conf ?q)
(Traj ?traj)
(Kin ?obj ?pose ?grasp ?q ?traj)
(FreeMotion ?q1 ?q2 ?traj)
(Supported ?obj ?pose ?surf)
(InsideDrawer ?obj ?pose ?d)
;; 状态谓词
(AtPose ?obj ?pose)
(AtConf ?q)
(Holding ?obj)
(HandEmpty)
(DrawerOpen ?d)
(DrawerClosed ?d)
;; 碰撞检查
(UnsafeTraj ?traj ?obj ?pose)
(UnsafeApproach ?traj ?obj ?pose)
)
;; ======== 动作定义 ========
;; 自由空间移动 (不持物)
(:action move-free
:parameters (?q1 ?q2 ?traj)
:precondition (and
(FreeMotion ?q1 ?q2 ?traj)
(AtConf ?q1)
(HandEmpty)
;; 轨迹不与任何物体碰撞
(forall (?obj ?pose)
(imply (and (Obj ?obj) (AtPose ?obj ?pose))
(not (UnsafeTraj ?traj ?obj ?pose))))
)
:effect (and
(AtConf ?q2)
(not (AtConf ?q1)))
)
;; 抓取物体
(:action pick
:parameters (?obj ?pose ?grasp ?q ?traj)
:precondition (and
(Kin ?obj ?pose ?grasp ?q ?traj)
(AtPose ?obj ?pose)
(AtConf ?q)
(HandEmpty)
;; 接近轨迹不碰撞
(forall (?obj2 ?pose2)
(imply (and (Obj ?obj2) (AtPose ?obj2 ?pose2)
(not (= ?obj ?obj2)))
(not (UnsafeApproach ?traj ?obj2 ?pose2))))
)
:effect (and
(Holding ?obj)
(not (AtPose ?obj ?pose))
(not (HandEmpty)))
)
;; 放置物体到表面
(:action place-on-surface
:parameters (?obj ?pose ?grasp ?q ?traj ?surf)
:precondition (and
(Kin ?obj ?pose ?grasp ?q ?traj)
(Holding ?obj)
(AtConf ?q)
(Supported ?obj ?pose ?surf)
;; 放置轨迹不碰撞
(forall (?obj2 ?pose2)
(imply (and (Obj ?obj2) (AtPose ?obj2 ?pose2))
(not (UnsafeApproach ?traj ?obj2 ?pose2))))
)
:effect (and
(AtPose ?obj ?pose)
(HandEmpty)
(not (Holding ?obj)))
)
;; 放置物体到抽屉内
(:action place-in-drawer
:parameters (?obj ?pose ?grasp ?q ?traj ?d)
:precondition (and
(Kin ?obj ?pose ?grasp ?q ?traj)
(Holding ?obj)
(AtConf ?q)
(InsideDrawer ?obj ?pose ?d)
(DrawerOpen ?d) ;; 抽屉必须是打开的
)
:effect (and
(AtPose ?obj ?pose)
(HandEmpty)
(not (Holding ?obj)))
)
;; 打开抽屉
(:action open-drawer
:parameters (?d ?q)
:precondition (and
(Drawer ?d)
(DrawerClosed ?d)
(HandEmpty)
(AtConf ?q)
)
:effect (and
(DrawerOpen ?d)
(not (DrawerClosed ?d)))
)
)
"""
完整 Stream 定义：
KITCHEN_STREAMS = """
(define (stream kitchen-tamp)
;; 采样抓取姿态
(:stream sample-grasp
:inputs (?obj)
:domain (Obj ?obj)
:outputs (?grasp)
:certified (Grasp ?obj ?grasp)
)
;; 采样表面上的放置位姿
(:stream sample-place-pose
:inputs (?obj ?surf)
:domain (and (Obj ?obj) (Surface ?surf))
:outputs (?pose)
:certified (and (Pose ?obj ?pose)
(Supported ?obj ?pose ?surf))
)
;; 采样抽屉内的放置位姿
(:stream sample-drawer-pose
:inputs (?obj ?d)
:domain (and (Obj ?obj) (Drawer ?d))
:outputs (?pose)
:certified (and (Pose ?obj ?pose)
(InsideDrawer ?obj ?pose ?d))
)
;; 逆运动学 + 接近轨迹
(:stream inverse-kinematics
:inputs (?obj ?pose ?grasp)
:domain (and (Pose ?obj ?pose) (Grasp ?obj ?grasp))
:outputs (?q ?traj)
:certified (and (Kin ?obj ?pose ?grasp ?q ?traj)
(Conf ?q) (Traj ?traj))
)
;; 自由空间运动规划
(:stream plan-free-motion
:inputs (?q1 ?q2)
:domain (and (Conf ?q1) (Conf ?q2))
:outputs (?traj)
:certified (and (FreeMotion ?q1 ?q2 ?traj)
(Traj ?traj))
)
;; 碰撞检查 (test stream, 不生成新值)
(:stream check-traj-collision
:inputs (?traj ?obj ?pose)
:domain (and (Traj ?traj) (Obj ?obj) (Pose ?obj ?pose))
:certified (UnsafeTraj ?traj ?obj ?pose)
)
)
"""
生成器函数实现：
import numpy as np
from itertools import count
def create_kitchen_stream_map(robot, scene):
"""
为厨房场景创建所有 Stream 生成器
Args:
robot: 机器人模型 (含运动学、碰撞模型)
scene: 场景描述 (物体网格、表面几何等)
"""
def sample_grasp_gen(obj_name):
"""
抓取姿态生成器
策略: 对称物体用解析法, 不规则物体用采样法
"""
obj = scene.get_object(obj_name)
mesh = obj.collision_mesh
for attempt in count():
if attempt > 200:
return # 放弃
if obj.is_cylindrical():
# 圆柱形物体 (如杯子): 360度等分采样
angle = np.random.uniform(0, 2 * np.pi)
# 侧面抓取: 夹爪垂直于圆柱轴线
grasp_tf = np.eye(4)
grasp_tf[:3, :3] = rotation_z(angle) @ rotation_x(np.pi/2)
grasp_tf[:3, 3] = [obj.radius * np.cos(angle),
obj.radius * np.sin(angle),
obj.height / 2]
else:
# 一般物体: 基于对跖点 (antipodal) 采样
p1, p2, n1, n2 = sample_antipodal_contacts(mesh)
grasp_tf = compute_grasp_frame(p1, p2, n1, n2,
gripper_width=0.08)
# 力封闭检查
if check_force_closure(grasp_tf, mesh, friction_coeff=0.5):
yield (grasp_tf,)
def sample_place_pose_gen(obj_name, surface_name):
"""
表面上稳定放置位姿生成器
需要检查: (1) 物体在表面范围内
(2) 不与已有物体碰撞
(3) 重力下稳定
"""
surface = scene.get_surface(surface_name)
obj = scene.get_object(obj_name)
for attempt in count():
if attempt > 100:
return
# 在表面范围内随机采样 (x, y)
x = np.random.uniform(surface.x_min + obj.radius,
surface.x_max - obj.radius)
y = np.random.uniform(surface.y_min + obj.radius,
surface.y_max - obj.radius)
z = surface.height + obj.support_height
# 随机旋转 (绕 z 轴)
yaw = np.random.uniform(0, 2 * np.pi)
pose = create_pose([x, y, z], [0, 0, yaw])
# 检查与已有物体的碰撞
if not scene.check_placement_collision(obj_name, pose):
# 检查稳定性 (重心在支撑面内)
if check_static_stability(obj, pose, surface):
yield (pose,)
def ik_gen(obj_name, pose, grasp):
"""
逆运动学生成器
输入: 物体位姿 + 抓取姿态 → 机器人关节角度 + 接近轨迹
"""
# 计算末端执行器目标位姿
ee_target = pose @ np.linalg.inv(grasp)
for attempt in count():
if attempt > 50:
return
# 求解 IK (随机初始化以获得不同解)
q_sol = robot.solve_ik(
ee_target,
seed=robot.random_config(),
max_iters=100,
tolerance=1e-4
)
if q_sol is None:
continue
# 碰撞检查
if robot.check_self_collision(q_sol):
continue
if scene.check_robot_collision(robot, q_sol):
continue
# 规划接近轨迹 (从预抓取位姿到抓取位姿)
pre_grasp_pose = ee_target @ retreat_offset(distance=0.1)
q_pre = robot.solve_ik(pre_grasp_pose, seed=q_sol)
if q_pre is not None:
approach_traj = interpolate_joint_space(
q_pre, q_sol, n_steps=20)
# 检查整条接近轨迹
if all(not scene.check_robot_collision(robot, q)
for q in approach_traj):
yield (q_sol, approach_traj)
def free_motion_gen(q_start, q_goal):
"""
自由空间运动规划生成器
内部调用 RRT-Connect (通过 OMPL)
"""
path = robot.plan_motion(
q_start, q_goal,
scene.get_obstacles(),
planner='RRTConnect',
timeout=5.0
)
if path is not None:
yield (path,)
return {
'sample-grasp': from_gen_fn(sample_grasp_gen),
'sample-place-pose': from_gen_fn(sample_place_pose_gen),
'sample-drawer-pose': from_gen_fn(sample_drawer_pose_gen),
'inverse-kinematics': from_gen_fn(ik_gen),
'plan-free-motion': from_gen_fn(free_motion_gen),
'check-traj-collision': from_test_fn(check_collision_test),
}
问题定义与求解：
def create_kitchen_problem(robot, scene):
"""创建完整的厨房整理问题"""
q_init = robot.get_current_config()
init_facts = [
('HandEmpty',),
('AtConf', q_init),
('Conf', q_init),
('Robot', 'panda'),
# 物体
('Obj', 'mug'), ('Obj', 'plate'),
('Obj', 'spoon'), ('Obj', 'bowl'),
# 表面
('Surface', 'shelf_top'), ('Surface', 'shelf_mid'),
('Surface', 'counter'), ('Surface', 'table'),
# 抽屉
('Drawer', 'drawer_1'),
('DrawerClosed', 'drawer_1'),
# 初始位姿
('AtPose', 'mug', scene.get_pose('mug')),
('Pose', 'mug', scene.get_pose('mug')),
('Supported', 'mug', scene.get_pose('mug'), 'counter'),
('AtPose', 'plate', scene.get_pose('plate')),
('Pose', 'plate', scene.get_pose('plate')),
('Supported', 'plate', scene.get_pose('plate'), 'counter'),
('AtPose', 'spoon', scene.get_pose('spoon')),
('Pose', 'spoon', scene.get_pose('spoon')),
('Supported', 'spoon', scene.get_pose('spoon'), 'table'),
('AtPose', 'bowl', scene.get_pose('bowl')),
('Pose', 'bowl', scene.get_pose('bowl')),
('InsideDrawer', 'bowl', scene.get_pose('bowl'), 'drawer_1'),
]
goal = ('and',
('exists', ('?p1',), ('and', ('Supported', 'mug', '?p1', 'shelf_top'),
('AtPose', 'mug', '?p1'))),
('exists', ('?p2',), ('and', ('Supported', 'plate', '?p2', 'shelf_mid'),
('AtPose', 'plate', '?p2'))),
('exists', ('?p3',), ('and', ('InsideDrawer', 'spoon', '?p3', 'drawer_1'),
('AtPose', 'spoon', '?p3'))),
('exists', ('?p4',), ('and', ('Supported', 'bowl', '?p4', 'counter'),
('AtPose', 'bowl', '?p4'))),
)
stream_map = create_kitchen_stream_map(robot, scene)
solution = solve_focused(
KITCHEN_DOMAIN, KITCHEN_STREAMS,
init_facts, goal,
stream_map=stream_map,
max_time=120,
verbose=True
)
if solution:
plan, cost, evals = solution
print(f"Plan found! {len(plan)} actions, "
f"{evals} stream evaluations")
for i, action in enumerate(plan):
print(f" {i+1}. {action.name}({action.args})")
# 预期输出类似:
# 1. open-drawer(drawer_1, q_drawer)
# 2. move-free(q_drawer, q_bowl)
# 3. pick(bowl, pose_bowl, grasp_bowl, q_pick_bowl, traj1)
# 4. place-on-surface(bowl, pose_new, grasp_bowl, q3, traj2, counter)
# 5. move-free(q3, q_mug)
# 6. pick(mug, pose_mug, grasp_mug, q4, traj3)
# 7. place-on-surface(mug, pose_shelf, grasp_mug, q5, traj4, shelf_top)
# ... (续完所有物体)
else:
print("No solution found within time limit!")
陷阱框
PDDLStream 的 forall 碰撞检查是性能瓶颈。在上面的 domain 中，每个
pick和place动作都包含forall碰撞检查——这意味着每当考虑一个动作时，必须检查该动作的轨迹是否与场景中每一个物体碰撞。物体数量 \(n\) 越多，碰撞检查次数 \(O(n)\) 越多，而每次碰撞检查本身又涉及数百次几何计算。在有 10+ 物体的场景中，这很容易成为性能瓶颈。Stream 的 lazy 评估很重要。在 Focused 算法中，Stream 只在"骨架需要"时才被调用（lazy evaluation）。如果改用 Incremental 算法盲目预生成所有样本，4 个物体 x 100 个抓取采样 x 50 个放置采样 = 20000 次 Stream 评估，其中大部分根本不会被用到。选择正确的算法变体对性能至关重要。
练习 7¶
- (⭐⭐) 在上面的 PDDLStream 示例中，为什么
inverse-kinematicsStream 的输入包含?pose和?grasp两个参数？如果只有其中一个，能否计算出机器人构型？ - (⭐⭐⭐) Incremental 算法和 Focused 算法在什么场景下性能差异最大？(提示：考虑 Stream 成功率很低的情况)
- (⭐⭐⭐) 如果要处理一个物体堆叠在另一个物体上的情况，需要在 Stream 定义中加入什么样的碰撞检查？
8. Logic-Geometric Programming (LGP) ⭐⭐⭐⭐¶
8.1 Toussaint 的优化视角¶
Marc Toussaint 在 IJCAI 2015 提出了一种与 PDDLStream 截然不同的 TAMP 方法。他的核心思想是：
整个 TAMP 问题可以表述为一个数学规划问题 (Mathematical Program)，同时包含离散决策变量和连续优化变量。
传统方法把 TAMP 分解为"任务规划 + 运动规划"两个子问题。LGP 的哲学是不分解，而是用统一的数学框架来表达。
形式化定义：
\[
\min_{\mathbf{a}, \mathbf{x}} \sum_{t=0}^{T} c_t(\mathbf{x}_t, \mathbf{a}_t)
\]
\[
\text{s.t.} \quad \mathbf{a} \in \{0, 1\}^{|\mathcal{A}| \times T} \quad \text{(离散: 选择动作序列)}
\]
\[
\mathbf{x}_t \in \mathbb{R}^d \quad \text{(连续: 机器人+物体的轨迹)}
\]
\[
g_t(\mathbf{x}_t, \mathbf{a}_t) \leq 0 \quad \text{(约束: 运动学、碰撞、物理)}
\]
\[
h_t(\mathbf{x}_t, \mathbf{a}_t) = 0 \quad \text{(等式约束: 抓取、放置)}
\]
这是一个混合整数非线性规划 (Mixed-Integer NLP, MINLP) 问题——一般来说极其难解。LGP 的贡献在于设计了一个高效的分层求解策略。
8.2 三层逼近 (Three-Level Approximation)¶
LGP 使用三个逐步精细的层次来逼近完整解：
Layer 1: 骨架搜索 (Skeleton Search)
↓ 粗糙但快速
Layer 2: 关键帧优化 (Keyframe Optimization)
↓ 中等精度
Layer 3: 完整轨迹优化 (Full Path Optimization)
↓ 精细但昂贵
最终解
Layer 1：骨架搜索
在纯逻辑层面搜索动作序列（骨架）。类似于传统任务规划，但使用更紧凑的表示。
\[
\text{skeleton} = (a_1^{sym}, a_2^{sym}, \ldots, a_n^{sym})
\]
例如：[pick(cup), move, place(cup, shelf), pick(book), move, place(book, bookcase)]
Layer 2：关键帧优化
固定骨架，只优化每个动作的关键构型（起始/结束构型）。这是一个有限维 NLP：
\[
\min_{\mathbf{q}_1, \mathbf{q}_2, \ldots, \mathbf{q}_n} \sum_{i} \|\mathbf{q}_{i+1} - \mathbf{q}_i\|^2
\]
\[
\text{s.t.} \quad \text{IK}(\mathbf{q}_i, \text{target}_i) = 0 \quad \forall i \in \text{manipulation steps}
\]
\[
\text{collision}(\mathbf{q}_i) \leq 0 \quad \forall i
\]
如果关键帧优化失败（例如某个构型不可达），则回到 Layer 1 搜索新的骨架。
Layer 3：完整轨迹优化
固定关键帧，优化它们之间的完整连续轨迹。使用 KOMO 求解器。
8.3 KOMO 轨迹优化¶
KOMO (K-Order Markov Optimization) 是 LGP 使用的轨迹优化方法。
核心思想：将轨迹约束表达为 \(k\) 阶马尔可夫约束——每个时刻的约束只依赖前 \(k\) 个时刻的状态。
\[
\min_{\mathbf{x}_{0:T}} \sum_{t=0}^{T} \sum_{k} \phi_k(\mathbf{x}_{t-k:t})^T \phi_k(\mathbf{x}_{t-k:t})
\]
| \(k\) 阶 | 约束类型 | 示例 |
|---|---|---|
| \(k=0\) | 位置约束 | 末端执行器在目标位置 |
| \(k=1\) | 速度约束 | 路径平滑性、碰撞避免 |
| \(k=2\) | 加速度约束 | 动力学可行性、力矩限制 |
KOMO 将上述问题转化为一个稀疏结构的 NLP，使用 Augmented Lagrangian 方法求解：
\[
\mathcal{L}(\mathbf{x}, \boldsymbol{\lambda}, \boldsymbol{\mu}) = f(\mathbf{x}) + \boldsymbol{\lambda}^T \mathbf{h}(\mathbf{x}) + \frac{\mu}{2} \|\mathbf{h}(\mathbf{x})\|^2 + \boldsymbol{\nu}^T \max(0, \mathbf{g}(\mathbf{x}))
\]
稀疏结构的优势：由于马尔可夫性，约束的 Jacobian 矩阵是带状的（banded），可以用 \(O(T)\) 而非 \(O(T^3)\) 的时间求解。
8.4 PDDLStream vs LGP：哲学对比¶
| 维度 | PDDLStream | LGP |
|---|---|---|
| 世界观 | 离散为主，连续为辅 | 连续为主，离散为辅 |
| 连续空间 | 采样：随机生成候选值 | 优化：求解最优值 |
| 信息流 | Stream 生成 → PDDL 验证 | 统一目标函数梯度流 |
| 优势 | 通用性、模块化 | 全局优化、轨迹质量 |
| 劣势 | 采样效率不稳定 | 非凸问题的局部最优 |
| 对窄通道 | 需要足够多的样本 | 梯度可能无法穿越 |
| 对长horizon | PDDL 规划器处理得好 | NLP 变量数爆炸 |
| 实现复杂度 | 中等（需要好的 Streams） | 高（需要实现 KOMO） |
| 代表性成果 | MIT 多个 demo | RAI/KOMO 框架 |
直觉类比：
- PDDLStream 像试错法：生成很多候选方案，逐个检验，找到一个可行的就行
- LGP 像解方程：把所有约束写成方程组，让数学求解器找最优解
两种方法各有适用场景，没有绝对的优劣之分。
陷阱框
LGP 对初始化敏感。由于底层是非凸优化，不好的初始猜测会导致收敛到局部最优或根本不收敛。实践中通常需要用 Layer 1 和 Layer 2 的结果来为 Layer 3 提供好的初始化。此外，接触模式（contact mode）的切换是非光滑的，标准 NLP 求解器处理起来有困难。
8.5 KOMO Augmented Lagrangian 求解推导 ⭐⭐⭐⭐¶
KOMO 的核心优化问题包含等式约束 \(h(\mathbf{x}) = 0\)（运动学、抓取等）和不等式约束 \(g(\mathbf{x}) \leq 0\)（碰撞、关节极限等）。直接求解这个约束优化问题很困难，KOMO 使用 Augmented Lagrangian Method (ALM) 将其转化为一系列无约束问题。
Step 1: 原始约束优化问题
\[
\min_{\mathbf{x}} f(\mathbf{x}) \quad \text{s.t.} \quad h_i(\mathbf{x}) = 0, \; i = 1, \ldots, p; \quad g_j(\mathbf{x}) \leq 0, \; j = 1, \ldots, m
\]
其中 \(f(\mathbf{x}) = \sum_t \|\mathbf{q}_{t+1} - \mathbf{q}_t\|^2\)（轨迹平滑代价）。
Step 2: 构造 Augmented Lagrangian
为什么不直接用 Lagrangian \(L = f + \boldsymbol{\lambda}^T \mathbf{h}\)？因为 Lagrangian 的极值点是鞍点而非极小点——它对 \(\mathbf{x}\) 是极小，对 \(\boldsymbol{\lambda}\) 是极大。直接用梯度下降法会在鞍点附近振荡。
Augmented Lagrangian 加入二次罚项来"凸化"问题：
\[
\mathcal{L}_A(\mathbf{x}, \boldsymbol{\lambda}, \boldsymbol{\nu}, \mu) = f(\mathbf{x}) + \sum_{i=1}^{p} \left[ \lambda_i h_i(\mathbf{x}) + \frac{\mu}{2} h_i(\mathbf{x})^2 \right] + \sum_{j=1}^{m} \frac{1}{2\mu} \left[ \max(0, \nu_j + \mu \, g_j(\mathbf{x}))^2 - \nu_j^2 \right]
\]
解读各项：
| 项 | 来源 | 作用 |
|---|---|---|
| \(f(\mathbf{x})\) | 目标函数 | 轨迹平滑性 |
| \(\lambda_i h_i(\mathbf{x})\) | Lagrange 乘子项 | 将等式约束的违反转化为代价 |
| \(\frac{\mu}{2} h_i^2\) | 二次罚项 | 让问题在 \(\mathbf{x}\) 方向"更凸"，改善收敛性 |
| \(\max(0, \nu_j + \mu g_j)^2\) | 不等式约束项 | 只在约束被违反时产生罚，否则不影响目标 |
Step 3: 迭代求解
ALM 的核心是外层-内层迭代：
Augmented Lagrangian 算法:
初始化: λ ← 0, ν ← 0, μ ← 1.0, x ← x_init
for k = 1, 2, ..., K:
// 内层: 固定乘子, 对 x 做无约束优化
x^{k+1} ← argmin_x L_A(x, λ^k, ν^k, μ^k)
// 使用 Newton 法或 Gauss-Newton 法, 利用 KOMO 的稀疏结构
// 外层: 更新乘子
for i = 1..p:
λ_i^{k+1} ← λ_i^k + μ^k · h_i(x^{k+1})
for j = 1..m:
ν_j^{k+1} ← max(0, ν_j^k + μ^k · g_j(x^{k+1}))
// 检查收敛
if max(|h(x)|, |max(0,g(x))|) < ε:
return x^{k+1} // 收敛
// 增大罚系数 (如果约束违反没有充分减小)
if constraint_violation 没有减少到上轮的 1/4:
μ^{k+1} ← 10 · μ^k
else:
μ^{k+1} ← μ^k
为什么 ALM 优于纯罚方法 (Penalty Method)？
纯罚方法使用 \(\min_x f(x) + \frac{\mu}{2} \|h(x)\|^2\)，需要 \(\mu \to \infty\) 才能满足约束，但 \(\mu\) 太大会导致 Hessian 矩阵条件数爆炸，Newton 步的数值精度急剧下降。ALM 通过 Lagrange 乘子 \(\lambda\) 来"记住"约束信息，使得 \(\mu\) 不需要太大就能满足约束——这是 ALM 的核心优势。
Step 4: KOMO 的稀疏结构利用
KOMO 中，\(\mathbf{x} = (\mathbf{q}_0, \mathbf{q}_1, \ldots, \mathbf{q}_T)\) 是一条完整轨迹。由于约束都是 \(k\)-阶马尔可夫的（每个约束只涉及 \(k+1\) 个相邻时间步），Hessian 矩阵 \(\nabla^2 \mathcal{L}_A\) 具有带状结构：
\[
\nabla^2 \mathcal{L}_A = \begin{bmatrix}
H_{00} & H_{01} & & & \\
H_{10} & H_{11} & H_{12} & & \\
& H_{21} & H_{22} & H_{23} & \\
& & \ddots & \ddots & \ddots \\
& & & H_{T-1,T} & H_{TT}
\end{bmatrix}
\]
这个带状矩阵可以用 band Cholesky 分解 在 \(O(T \cdot d^2)\) 时间内求解（\(d\) 是单步构型维度），而不是一般密集矩阵的 \(O(T^3 \cdot d^3)\)。这使得 KOMO 能高效处理长时间跨度的轨迹。
8.6 SQP 方法对比 ⭐⭐⭐⭐¶
除了 Augmented Lagrangian，另一种常用的约束优化方法是 Sequential Quadratic Programming (SQP)。TrajOpt 就使用 SQP（严格说是 Sequential Convex Programming, SCP）。两种方法的对比：
| 特性 | Augmented Lagrangian (KOMO) | SQP/SCP (TrajOpt) |
|---|---|---|
| 核心思想 | 将约束优化转化为无约束序列 | 将非凸问题转化为凸子问题序列 |
| 内层求解 | 无约束 Newton 法 | 二次规划 (QP) 或 凸优化 |
| 约束处理 | 罚项 + 乘子更新 | 线性化约束 + 信赖域 |
| 碰撞约束 | 二次罚 | 凸松弛 (signed distance 线性化) |
| 收敛速度 | 超线性 (乘子更新后) | 超线性 (在可行域附近) |
| 对非凸性的鲁棒性 | 依赖初始化 | 信赖域提供一定鲁棒性 |
| 实现复杂度 | 中等 | 较高 (需要 QP 求解器) |
| 开源实现 | RAI/KOMO | TrajOpt (trajopt_ros) |
何时选 ALM？ 当约束较多且结构化（如 KOMO 的马尔可夫结构）时，ALM 可以利用稀疏性，效率很高。
何时选 SQP？ 当约束较少但非常非线性（如复杂的碰撞约束几何体），SQP 的信赖域机制对局部非凸性更鲁棒。
8.7 RAI 代码示例 ⭐⭐⭐¶
RAI (Robotic AI) 框架 是 Toussaint 团队开发的 KOMO/LGP 实现。以下展示如何用 RAI 的 Python 接口定义和求解一个简单的 pick-and-place 轨迹优化问题。
"""
RAI/KOMO 轨迹优化示例
安装: pip install robotic
文档: https://marctoussaint.github.io/robotic/
"""
import robotic as ry
import numpy as np
# ====== 1. 创建场景配置 ======
C = ry.Config()
# 加载 Panda 机器人
C.addFile(ry.raiPath('scenarios/pandaSingle.g'))
# 添加桌面物体
C.addFrame('table') \
.setPosition([0.5, 0, 0.3]) \
.setShape(ry.ST.box, size=[0.6, 0.4, 0.02, 0.005]) \
.setColor([0.5, 0.5, 0.5]) \
.setContact(1)
C.addFrame('cup', 'table') \
.setRelativePosition([0.0, 0.1, 0.06]) \
.setShape(ry.ST.cylinder, size=[0.04, 0.04, 0.08, 0.03]) \
.setColor([1, 0, 0]) \
.setMass(0.2) \
.setContact(1)
C.addFrame('target') \
.setPosition([0.5, -0.2, 0.5]) \
.setShape(ry.ST.marker, size=[0.05])
# ====== 2. 定义 KOMO 轨迹优化问题 ======
# 参数: 2 个阶段 (pick + place), 每阶段 20 步, 时间步长 0.1
komo = ry.KOMO(C, phases=2, slicesPerPhase=20, kOrder=2)
# 平滑性代价 (加速度最小化)
komo.addControlObjective([], order=2, scale=1.0)
# ------ Phase 1: 抓取 (t=1.0) ------
# 在 t=1.0 时刻, 末端执行器到达杯子位置
komo.addObjective(
times=[1.0],
feature=ry.FS.positionDiff,
frames=['l_gripper', 'cup'],
type=ry.OT.eq, # 等式约束
scale=[1e1] # 约束权重
)
# 抓取时接近方向: 从上方垂直抓取
komo.addObjective(
times=[1.0],
feature=ry.FS.vectorZ,
frames=['l_gripper'],
type=ry.OT.eq,
target=[0, 0, -1], # z轴朝下
scale=[1e1]
)
# 抓取前的速度为零 (稳定抓取)
komo.addObjective(
times=[1.0],
feature=ry.FS.qItself,
type=ry.OT.eq,
order=1, # 一阶 = 速度
scale=[1e1]
)
# ------ Phase 2: 放置 (t=2.0) ------
komo.addObjective(
times=[2.0],
feature=ry.FS.positionDiff,
frames=['cup', 'target'],
type=ry.OT.eq,
scale=[1e1]
)
# 放置时速度为零
komo.addObjective(
times=[2.0],
feature=ry.FS.qItself,
type=ry.OT.eq,
order=1,
scale=[1e1]
)
# ------ 全程碰撞避免 ------
komo.addObjective(
times=[], # 空 = 所有时间步
feature=ry.FS.accumulatedCollisions,
type=ry.OT.ineq, # 不等式约束: ≤ 0
scale=[1e0]
)
# ====== 3. 求解 ======
# 使用 Augmented Lagrangian 求解
solver = ry.NLP_Solver(komo.nlp(), verbose=2)
solver.setOptions(
stopTolerance=1e-3,
maxStep=0.2,
damping=1e-2
)
ret = solver.solve()
print(f"Solver status: {ret}")
print(f"Constraint violations: eq={komo.getReport(False)}")
# ====== 4. 可视化结果 ======
# 获取优化后的轨迹
path = komo.getPath()
print(f"Trajectory shape: {path.shape}") # (40, 7) = 40步 x 7关节
# 在 RAI 可视化器中回放
for t in range(path.shape[0]):
C.setJointState(path[t])
C.view()
ry.wait(0.05)
RAI 代码的关键理解：
| 代码元素 | 含义 | 对应数学 |
|---|---|---|
kOrder=2 |
约束最高阶数为 2 | Hessian 的带宽 |
ry.FS.positionDiff |
位置差特征 | \(\phi(\mathbf{q}) = p_{ee}(\mathbf{q}) - p_{target}\) |
ry.OT.eq |
等式约束 | \(h(\mathbf{x}) = 0\) |
ry.OT.ineq |
不等式约束 | \(g(\mathbf{x}) \leq 0\) |
order=1 |
一阶特征（速度） | \(\frac{d\phi}{dt} \approx \phi(\mathbf{q}_{t}) - \phi(\mathbf{q}_{t-1})\) |
scale |
约束/代价权重 | \(\mathcal{L}\) 中各项的系数 |
陷阱框
KOMO 的 scale 参数极其敏感。scale 过小则约束被忽视（轨迹穿透障碍物），scale 过大则优化问题病态（数值精度丢失，求解器不收敛）。在实践中，不同类型的约束需要不同量级的 scale——位置约束通常用 \(10^1\)，碰撞约束用 \(10^0\)，速度约束用 \(10^1\)。调试 KOMO 问题时，第一件事就是检查约束违反量（
komo.getReport()），如果某个约束违反量远大于其他，说明 scale 设置不平衡。RAI 的 phases 概念容易混淆。phases 不是时间步数，而是"逻辑阶段"数。每个 phase 内部有
slicesPerPhase个时间步。例如phases=2, slicesPerPhase=20意味着总共 40 个时间步，phase 1 对应 \(t \in [0, 1]\)，phase 2 对应 \(t \in [1, 2]\)。动作发生在 phase 边界上（\(t = 1.0, 2.0, \ldots\)）。
练习 8¶
- (⭐⭐⭐) 在 LGP 的三层逼近中，Layer 2 (关键帧优化) 的变量维度是多少？如果机器人有 7 个自由度，动作序列有 10 步，变量总数大约是多少？
- (⭐⭐⭐) 为什么 KOMO 的马尔可夫结构对计算效率至关重要？如果没有马尔可夫性，求解复杂度会变成什么？
- (⭐⭐⭐⭐) 设计一个混合方法：用 PDDLStream 做骨架搜索，用 KOMO 做轨迹优化。这样做的优势和挑战是什么？
9. Foundation Models + TAMP ⭐⭐⭐¶
9.1 LLM 进入机器人规划¶
2022-2025 年间，大语言模型 (LLM) 开始被引入机器人规划领域。核心动机是：
- 经典 TAMP 需要手动编写 PDDL domain，费时费力
- LLM 具有大量常识知识（知道杯子要放在架子上而不是地上）
- LLM 能够理解自然语言指令
9.2 SayCan：LLM 概率 x 可行性概率¶
SayCan (Ahn et al., CoRL 2022) 是 Google 提出的开创性工作。
核心公式：
\[
a^* = \arg\max_{a \in \mathcal{A}} \underbrace{P_{\text{LLM}}(a \mid \text{instruction}, \text{history})}_{\text{"说"的概率：语言合理性}} \times \underbrace{P_{\text{affordance}}(a \mid \text{state})}_{\text{"做"的概率：物理可行性}}
\]
| 组件 | 来源 | 作用 |
|---|---|---|
| \(P_{\text{LLM}}\) | 大语言模型 (PaLM) | 评估动作在语义上是否合理 |
| \(P_{\text{affordance}}\) | 每个技能的 value function | 评估动作在物理上是否可行 |
SayCan 的工作流程：
用户指令: "帮我拿一杯水"
Step 1: LLM 给每个候选动作打分 (语言合理性)
"走到厨房" → P_LLM = 0.7
"拿起杯子" → P_LLM = 0.1 (还没到厨房)
"打开冰箱" → P_LLM = 0.05
Step 2: Affordance model 给每个候选动作打分 (物理可行性)
"走到厨房" → P_aff = 0.9 (路径无障碍)
"拿起杯子" → P_aff = 0.0 (杯子不在附近)
"打开冰箱" → P_aff = 0.1 (冰箱在附近但不是当前目标)
Step 3: 相乘选择最优动作
"走到厨房" → 0.7 × 0.9 = 0.63 ← 最高！
"拿起杯子" → 0.1 × 0.0 = 0.00
"打开冰箱" → 0.05 × 0.1 = 0.005
执行 "走到厨房"，然后重复...
9.3 Code as Policies¶
另一种思路是让 LLM 直接生成控制代码而不是选择预定义动作。
# 用户指令: "把所有红色物体放到盒子里"
# LLM 生成的代码:
def task():
red_objects = detect_objects(color="red")
box = detect_objects(type="box")[0]
for obj in red_objects:
grasp_pose = compute_grasp(obj)
pick(obj, grasp_pose)
place_pose = compute_placement(obj, box)
place(obj, place_pose)
优势：灵活性极高，能处理之前未见过的任务组合。
风险：LLM 生成的代码可能有逻辑错误、物理不合理的操作、安全隐患。
9.4 对比表：经典 TAMP vs LLM 规划¶
| 维度 | 经典 TAMP | LLM 规划 |
|---|---|---|
| 知识来源 | 手写 PDDL domain | 预训练语料中的常识 |
| 输入 | 形式化问题描述 | 自然语言指令 |
| 完备性 | 有理论保证 | 无保证 |
| 最优性 | 可优化明确目标 | 目标隐含在自然语言中 |
| 泛化性 | 限于定义的 domain | 对新任务有一定泛化 |
| 几何推理 | 通过运动规划器 | 几乎没有 (纯语言) |
| 安全性 | 可验证 | 难以验证 |
| 长horizon | 强 (规划器搜索) | 弱 (容易累积错误) |
| 开发成本 | 高 (写 PDDL + Streams) | 低 (写 prompt) |
| 典型错误 | 搜索超时 | 幻觉、物理不合理 |
9.5 2025 共识：LLM 作为启发式引导¶
经过 2022-2025 年的大量实践，机器人学界形成了一些共识：
- LLM 不能替代经典 TAMP：LLM 缺乏精确的几何推理能力，生成的计划需要验证
- LLM 是优秀的启发式：可以用 LLM 来引导搜索方向，减少搜索空间
- 混合架构是趋势：LLM 提供高层策略 → 经典 TAMP 验证和细化 → 运动规划器执行
2025 主流架构:
┌────────────────────────────────────────┐
│ LLM Layer (高层策略/启发式) │
│ "先清理桌面，再擦桌子" │
│ │ │
│ ▼ │
│ ┌──────────────────────┐ │
│ │ TAMP Layer (验证+细化) │ │
│ │ PDDL + Streams │ │
│ │ 检查可行性 │ │
│ │ │ │ │
│ │ ▼ │ │
│ │ ┌─────────────┐ │ │
│ │ │ Motion Plan │ │ │
│ │ │ RRT / KOMO │ │ │
│ │ └─────────────┘ │ │
│ └──────────────────────┘ │
│ │ │
│ ▼ │
│ 机器人执行 │
└────────────────────────────────────────┘
陷阱框
不要直接把 LLM 的输出当作机器人动作指令。 LLM 可能说"把杯子放到架子上"，但完全不考虑架子高度是否在机械臂工作空间内。所有 LLM 输出必须经过物理可行性验证。在安全关键场景中（如手术机器人），LLM 只能作为辅助建议，不能作为最终决策。
9.6 可微分 TAMP (Differentiable TAMP) ⭐⭐⭐⭐¶
2023-2025 年的一个重要研究方向是将 TAMP 的各个组件变为可微分的，从而可以用梯度优化端到端地训练和求解。
9.6.1 STAMP：变分推断视角¶
STAMP (Scalable Task and Motion Planning) 将 TAMP 问题形式化为变分推断 (Variational Inference) 问题。
核心思想：将任务-运动规划看作概率推断——给定目标 \(G\)，推断最可能的动作序列和连续参数。
形式化地：
\[
p(\mathbf{a}, \boldsymbol{\theta} \mid G) \propto p(G \mid \mathbf{a}, \boldsymbol{\theta}) \cdot p(\boldsymbol{\theta} \mid \mathbf{a}) \cdot p(\mathbf{a})
\]
其中： - \(p(\mathbf{a})\) 是动作序列的先验（来自任务规划器或 LLM） - \(p(\boldsymbol{\theta} \mid \mathbf{a})\) 是给定动作的连续参数先验（如均匀采样） - \(p(G \mid \mathbf{a}, \boldsymbol{\theta})\) 是目标的似然（计划是否达成目标）
变分推断的关键：直接计算后验 \(p(\mathbf{a}, \boldsymbol{\theta} \mid G)\) 是 intractable 的（因为需要对所有可能的 \(\mathbf{a}, \boldsymbol{\theta}\) 求和/积分）。STAMP 引入变分分布 \(q_\phi(\mathbf{a}, \boldsymbol{\theta})\) 来近似后验，通过最小化 KL 散度：
\[
\min_\phi \; \text{KL}\left[ q_\phi(\mathbf{a}, \boldsymbol{\theta}) \; \| \; p(\mathbf{a}, \boldsymbol{\theta} \mid G) \right]
\]
等价于最大化 ELBO (Evidence Lower Bound)：
\[
\mathcal{L}(\phi) = \mathbb{E}_{q_\phi} \left[ \log p(G \mid \mathbf{a}, \boldsymbol{\theta}) \right] - \text{KL}\left[ q_\phi \| p(\mathbf{a}) p(\boldsymbol{\theta} \mid \mathbf{a}) \right]
\]
与经典 TAMP 的联系：
| TAMP 概念 | 变分推断对应 |
|---|---|
| 任务规划搜索 | 离散变量 \(\mathbf{a}\) 的后验推断 |
| 运动规划/采样 | 连续变量 \(\boldsymbol{\theta}\) 的后验推断 |
| 约束满足 | 似然函数 \(p(G \mid \mathbf{a}, \boldsymbol{\theta})\) |
| 搜索启发式 | 先验 \(p(\mathbf{a})\) |
| PDDLStream 的 Focused | 类似于结构化变分分布 |
优势：变分推断框架允许用梯度方法联合优化离散和连续变量（通过 Gumbel-Softmax 松弛离散变量），避免了搜索+采样的组合爆炸。
9.6.2 cuTAMP：GPU 并行加速 ⭐⭐⭐⭐¶
传统 TAMP 的瓶颈之一是运动规划器的串行调用——每次碰撞检查、每次 IK 求解都在 CPU 上串行执行。cuTAMP 利用 GPU 的大规模并行能力来加速 TAMP 的关键组件。
GPU 并行化架构：
┌────────────────────────────────────────────────────┐
│ cuTAMP 架构 │
│ │
│ CPU (任务层) │
│ ┌───────────────────────────┐ │
│ │ Task Planner / Skeleton │ │
│ │ 搜索 (串行, 逻辑推理) │ │
│ └────────────┬──────────────┘ │
│ │ 批量请求 │
│ ▼ │
│ GPU (运动层) │
│ ┌───────────────────────────────────────────┐ │
│ │ ┌─────────┐ ┌──────────┐ ┌──────────┐ │ │
│ │ │ 批量 IK │ │ 批量碰撞 │ │ 批量轨迹 │ │ │
│ │ │ 1024组 │ │ 检查 │ │ 优化 │ │ │
│ │ │ 并行求解 │ │ 4096对 │ │ 256条 │ │ │
│ │ │ │ │ 并行检查 │ │ 并行优化 │ │ │
│ │ └─────────┘ └──────────┘ └──────────┘ │ │
│ └───────────────────────────────────────────┘ │
│ │
│ 加速比: 碰撞检查 ~100x, IK ~50x, 整体 TAMP ~10x │
└────────────────────────────────────────────────────┘
关键技术：
| 组件 | CPU 实现 | GPU 实现 | 加速比 |
|---|---|---|---|
| 碰撞检查 | FCL (串行) | cuCollision (并行 BVH) | 50-200x |
| 逆运动学 | 单次 Newton | 批量 Newton (不同初始化) | 30-100x |
| 采样 | 单线程 random() |
cuRAND 并行采样 | 100x+ |
| 轨迹优化 | 单条 KOMO | 批量 KOMO (不同骨架) | 10-50x |
cuTAMP 的直觉：传统 TAMP 是"一个人试钥匙"——拿一把钥匙试锁，不对就换一把。cuTAMP 是"一千个人同时试不同的钥匙"——GPU 上同时评估大量候选方案，找到可行方案的速度大幅提升。
9.7 VLM 引导的 TAMP ⭐⭐⭐¶
视觉-语言模型 (Vision-Language Model, VLM) 如 GPT-4V、Gemini 是 2024-2025 年的重要进展。与纯文本 LLM 相比，VLM 能直接"看"场景图像，为 TAMP 提供更丰富的环境理解。
VLM-TAMP 架构：
┌──────────────────────────────────────────────────┐
│ VLM-TAMP Pipeline │
│ │
│ ┌──────────┐ ┌──────────────────────┐ │
│ │ 相机图像 │──→│ VLM (GPT-4V/Gemini) │ │
│ │ + 深度图 │ │ · 物体识别与定位 │ │
│ └──────────┘ │ · 空间关系推理 │ │
│ │ · 抓取可行性评估 │ │
│ │ · 动作序列建议 │ │
│ └─────────┬────────────┘ │
│ │ │
│ ▼ │
│ ┌────────────────────────────────────────┐ │
│ │ PDDL 问题生成器 (VLM 输出 → PDDL) │ │
│ │ · 自动生成 objects 列表 │ │
│ │ · 推断 init 状态 (空间关系) │ │
│ │ · 将自然语言目标转为 PDDL goal │ │
│ └────────────────┬───────────────────────┘ │
│ │ │
│ ▼ │
│ ┌──────────────────────────┐ │
│ │ PDDLStream / LGP 求解 │ │
│ │ (标准 TAMP pipeline) │ │
│ └──────────────────────────┘ │
└──────────────────────────────────────────────────┘
VLM 在 TAMP 中的三个应用层次：
| 层次 | 作用 | 示例 | 信任度 |
|---|---|---|---|
| 感知层 | 将图像转为符号状态 | "图中杯子在盘子上方" → On(cup, plate) |
高 (可验证) |
| 启发式层 | 建议动作优先级 | "先移开遮挡物" → 搜索优先级 | 中 (需验证) |
| 规划层 | 直接生成完整计划 | "1. pick cup, 2. place shelf..." | 低 (常有错) |
最佳实践：VLM 在感知层最可靠——将场景图像转化为结构化的 PDDL 状态描述。在启发式层次也有价值——例如用 VLM 判断"哪些物体可能挡住目标物体"来引导搜索。但在规划层直接生成动作序列则不可靠，应始终由经典 TAMP 求解器进行验证。
陷阱框
VLM 的空间推理能力被高估了。虽然 VLM 能识别物体并描述"杯子在盘子左边"，但对精确的几何关系（距离、角度、可达性）推理能力很弱。例如，VLM 可能无法判断两个物体之间 5cm 的间隙是否足够机械手通过。因此 VLM 输出的空间关系只能作为粗粒度的启发式，不能替代精确的碰撞检测和运动规划。
VLM 的幻觉 (hallucination) 在机器人场景中尤其危险。如果 VLM 将不存在的物体"识别"出来并加入 PDDL 状态，或者遗漏了存在的障碍物，后果可能是机器人碰撞损坏。必须有独立的感知验证机制（如点云分割）来 cross-check VLM 的输出。
练习 9¶
- (⭐⭐) SayCan 中的 \(P_{\text{affordance}}\) 是如何训练的？为什么需要为每个技能单独训练？
- (⭐⭐⭐) 设计一个实验来比较 LLM 规划和经典 TAMP 在以下场景的表现：10 个物体需要按特定顺序堆叠成塔。你预测哪种方法表现更好？为什么？
- (⭐⭐⭐) 如果要用 LLM 来加速 PDDLStream 的搜索，你会在哪个环节引入 LLM？(提示：骨架搜索、Stream 采样优先级、启发式函数)
10. 本章小结¶
知识图谱¶
TAMP
┌────┴─────┐
任务规划 运动规划
┌───┴───┐ ┌────┴────┐
STRIPS PDDL PRM/RRT TrajOpt
│ │ │ │
└───┬───┘ └────┬────┘
│ │
┌───┴────────────────┴───┐
│ 核心挑战 │
│ · 无限分支因子 │
│ · 几何可行性 │
│ · 耦合约束 │
└───────────┬────────────┘
│
┌─────────┼──────────┐
│ │ │
PDDLStream LGP LLM+TAMP
(采样+交错) (优化+联合) (启发式)
核心要点回顾¶
| 节 | 核心要点 | 一句话总结 |
|---|---|---|
| 2 | 为什么需要 TAMP | 符号世界和几何世界必须桥接，否则规划要么无意义要么不可行 |
| 3 | 经典任务规划 | PDDL 用命题逻辑描述世界，FF 用松弛计划做启发式搜索 |
| 4 | 运动规划回顾 | C-space、PRM、RRT、TrajOpt 是 TAMP 的内层求解器 |
| 5 | TAMP 核心挑战 | 无限分支因子 + 几何可行性 + 耦合约束 = 混合状态空间 |
| 6 | 分类框架 | 连续空间(采样/优化) x 集成策略(后验/交错/联合) |
| 7 | PDDLStream | PDDL + Streams，采样+交错的代表 |
| 8 | LGP | 整个 TAMP = MINLP，优化+联合的代表 |
| 9 | FM + TAMP | LLM 提供常识和启发式，但不能替代经典 TAMP 的可行性保证 |
延伸阅读¶
| 优先级 | 文献 | 内容 |
|---|---|---|
| 必读 | Garrett et al., Integrated Task and Motion Planning, Annual Review 2021 | TAMP 最全面的综述 |
| 必读 | Garrett et al., PDDLStream, ICAPS 2020 | PDDLStream 框架原论文 |
| 推荐 | Toussaint, Logic-Geometric Programming, IJCAI 2015 | LGP 原论文 |
| 推荐 | LaValle, Planning Algorithms, 2006 | 运动规划的经典教材(免费在线) |
| 推荐 | Ahn et al., Do As I Can, Not As I Say: Grounding Language in Robotic Affordances, CoRL 2022 | SayCan 原论文 |
| 进阶 | Liang et al., Code as Policies, 2023 | LLM 生成控制代码 |
| 进阶 | Toussaint et al., Sequence-of-Constraints MPC, RSS 2022 | KOMO 的最新进展 |
11. 累积项目：简单 TAMP 系统 ⭐⭐⭐¶
项目目标¶
用 pyperplan + PyBullet 实现一个简单的 pick-and-place TAMP 系统。这个项目将贯穿后续章节，逐步扩展功能。
系统架构¶
┌─────────────────────────────────────────────────────┐
│ Mini-TAMP System │
│ │
│ ┌──────────────┐ ┌──────────────┐ │
│ │ PDDL │ │ PyBullet │ │
│ │ Domain + │ │ Simulation │ │
│ │ Problem │ │ Environment │ │
│ └──────┬───────┘ └──────┬───────┘ │
│ │ │ │
│ ▼ ▼ │
│ ┌──────────────┐ ┌──────────────┐ │
│ │ pyperplan │ │ Motion │ │
│ │ (任务规划) │ │ Planner │ │
│ └──────┬───────┘ │ (BiRRT) │ │
│ │ └──────┬───────┘ │
│ ▼ │ │
│ ┌──────────────────────────┴───┐ │
│ │ TAMP Coordinator │ │
│ │ (任务-运动协调器) │ │
│ └──────────────────────────────┘ │
└─────────────────────────────────────────────────────┘
第 1 阶段代码框架 (本章)¶
"""
Mini-TAMP 系统 -- 第 1 阶段: 基础框架
文件: mini_tamp.py
"""
import pybullet as p
import pybullet_data
import numpy as np
from pathlib import Path
# ========== 1. PyBullet 仿真环境 ==========
class TableTopEnv:
"""桌面操作仿真环境"""
def __init__(self, gui=True):
self.physics_client = p.connect(p.GUI if gui else p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setGravity(0, 0, -9.81)
# 加载场景
self.plane_id = p.loadURDF("plane.urdf")
self.table_id = p.loadURDF(
"table/table.urdf",
basePosition=[0.5, 0, 0],
useFixedBase=True
)
# 加载机械臂 (Panda)
self.robot_id = p.loadURDF(
"franka_panda/panda.urdf",
basePosition=[0, 0, 0],
useFixedBase=True
)
self.objects = {} # name -> pybullet_id
self.ee_link = 11 # Panda 末端执行器链接索引
def add_object(self, name: str, shape: str, position: list,
color: list = [1, 0, 0, 1]):
"""添加一个简单物体到场景中"""
if shape == "cube":
col_id = p.createCollisionShape(p.GEOM_BOX,
halfExtents=[0.025]*3)
vis_id = p.createVisualShape(p.GEOM_BOX,
halfExtents=[0.025]*3,
rgbaColor=color)
elif shape == "cylinder":
col_id = p.createCollisionShape(p.GEOM_CYLINDER,
radius=0.02, height=0.06)
vis_id = p.createVisualShape(p.GEOM_CYLINDER,
radius=0.02, length=0.06,
rgbaColor=color)
body_id = p.createMultiBody(
baseMass=0.1,
baseCollisionShapeIndex=col_id,
baseVisualShapeIndex=vis_id,
basePosition=position
)
self.objects[name] = body_id
return body_id
def get_object_pose(self, name: str) -> tuple:
"""获取物体位姿"""
pos, orn = p.getBasePositionAndOrientation(self.objects[name])
return np.array(pos), np.array(orn)
def get_robot_conf(self) -> np.ndarray:
"""获取机器人当前关节角度"""
joint_states = p.getJointStates(
self.robot_id,
range(7) # Panda 有 7 个关节
)
return np.array([s[0] for s in joint_states])
# ========== 2. 任务规划接口 ==========
class TaskPlanner:
"""任务规划器 (封装 pyperplan)"""
def __init__(self, domain_file: str, problem_file: str):
self.domain_file = domain_file
self.problem_file = problem_file
def solve(self) -> list:
"""求解任务规划问题，返回动作列表"""
from pyperplan.planner import _parse, _ground, SEARCHES, HEURISTICS
problem = _parse(self.domain_file, self.problem_file)
task = _ground(problem)
heuristic = HEURISTICS['hff'](task)
solution = SEARCHES['astar'](task, heuristic)
if solution is None:
return []
return list(solution)
# ========== 3. 运动规划接口 ==========
class MotionPlanner:
"""简单的运动规划器 (BiRRT)"""
def __init__(self, env: TableTopEnv):
self.env = env
self.robot_id = env.robot_id
self.num_joints = 7
def solve_ik(self, target_pos: np.ndarray,
target_orn: np.ndarray = None) -> np.ndarray:
"""逆运动学求解"""
if target_orn is None:
target_orn = p.getQuaternionFromEuler([np.pi, 0, 0])
joint_angles = p.calculateInverseKinematics(
self.robot_id,
self.env.ee_link,
target_pos,
target_orn,
maxNumIterations=100,
residualThreshold=1e-4
)
return np.array(joint_angles[:self.num_joints])
def plan_motion(self, start_conf: np.ndarray,
goal_conf: np.ndarray,
max_iters: int = 1000) -> list:
"""
BiRRT 运动规划 (简化版)
返回: 关节角度轨迹列表，或 None (规划失败)
"""
# 简化版: 线性插值 + 碰撞检查
# 完整版应使用 BiRRT
n_steps = 50
path = []
for i in range(n_steps + 1):
alpha = i / n_steps
conf = (1 - alpha) * start_conf + alpha * goal_conf
if self._check_collision(conf):
return None # 碰撞，规划失败
path.append(conf)
return path
def _check_collision(self, conf: np.ndarray) -> bool:
"""检查给定构型是否碰撞"""
# 设置机器人构型
for i in range(self.num_joints):
p.resetJointState(self.robot_id, i, conf[i])
# 检查碰撞
contacts = p.getContactPoints(self.robot_id)
# 过滤掉自碰撞
for c in contacts:
if c[2] != self.robot_id: # 与非自身碰撞
return True
return False
# ========== 4. TAMP 协调器 ==========
class TAMPCoordinator:
"""
简单的 TAMP 协调器
策略: Plan-then-Check with retry
(后续章节会升级为更高级的策略)
"""
def __init__(self, env: TableTopEnv,
task_planner: TaskPlanner,
motion_planner: MotionPlanner):
self.env = env
self.task_planner = task_planner
self.motion_planner = motion_planner
def solve(self, max_retries: int = 5) -> list:
"""
求解 TAMP 问题
返回: [(符号动作, 运动轨迹), ...] 或 None
"""
# Step 1: 任务规划
task_plan = self.task_planner.solve()
if not task_plan:
print("Task planning failed!")
return None
print(f"Task plan found: {len(task_plan)} steps")
for i, action in enumerate(task_plan):
print(f" Step {i+1}: {action}")
# Step 2: 逐步检查运动可行性
full_plan = []
current_conf = self.env.get_robot_conf()
for action in task_plan:
# 解析符号动作，获取目标位姿
target_pos = self._action_to_target(action)
if target_pos is None:
print(f" Cannot parse action: {action}")
return None
# 逆运动学
goal_conf = self.motion_planner.solve_ik(target_pos)
if goal_conf is None:
print(f" IK failed for action: {action}")
return None
# 运动规划
trajectory = self.motion_planner.plan_motion(
current_conf, goal_conf
)
if trajectory is None:
print(f" Motion planning failed for: {action}")
return None # 简单版: 直接失败
full_plan.append((action, trajectory))
current_conf = goal_conf
print(f"\nFull TAMP solution found!")
return full_plan
def _action_to_target(self, action) -> np.ndarray:
"""
将符号动作转换为目标末端执行器位置
(简化版: 硬编码位置映射)
"""
action_str = str(action)
# 解析动作名和参数
# 实际应用中应基于物体当前位姿计算
if "pick" in action_str:
obj_name = action_str.split()[1] # 简化解析
if obj_name in self.env.objects:
pos, _ = self.env.get_object_pose(obj_name)
return pos + np.array([0, 0, 0.05]) # 偏移
elif "place" in action_str:
# 返回放置目标位置
pass
return None
# ========== 5. 主程序 ==========
def main():
"""运行 Mini-TAMP 系统"""
# 创建环境
env = TableTopEnv(gui=True)
env.add_object("cup", "cylinder", [0.5, 0.1, 0.65],
color=[1, 0, 0, 1])
env.add_object("book", "cube", [0.5, -0.1, 0.65],
color=[0, 0, 1, 1])
# 创建规划器
task_planner = TaskPlanner(
"pick-place-domain.pddl",
"pick-place-problem.pddl"
)
motion_planner = MotionPlanner(env)
# 创建 TAMP 协调器
coordinator = TAMPCoordinator(env, task_planner, motion_planner)
# 求解
solution = coordinator.solve()
if solution:
print("\n=== Executing plan ===")
for action, trajectory in solution:
print(f"Executing: {action}")
for conf in trajectory:
for i in range(7):
p.resetJointState(env.robot_id, i, conf[i])
p.stepSimulation()
input("Press Enter to exit...")
p.disconnect()
if __name__ == "__main__":
main()
项目里程碑¶
| 阶段 | 章节 | 扩展内容 |
|---|---|---|
| 阶段 1 (本章) | TAMP_T1 | 基础框架：pyperplan + PyBullet + 简单协调器 |
| 阶段 2 | TAMP_T2 | 升级为 PDDLStream 式的 Stream 生成器 |
| 阶段 3 | TAMP_T3 | 加入碰撞检测和 BiRRT 运动规划 |
| 阶段 4 | TAMP_T4 | 加入 LLM 启发式和更复杂的场景 |
项目练习¶
- (⭐) 运行上面的代码，观察 PyBullet 中的场景。修改物体位置，看机械臂能否到达
- (⭐⭐) 实现
_action_to_target方法的完整版本，根据实际的物体位姿和目标位置计算末端执行器的目标 - (⭐⭐⭐) 将
plan_motion从线性插值替换为真正的 BiRRT 算法。提示：你需要实现随机采样、最近邻搜索、碰撞检查三个组件 - (⭐⭐⭐) 加入一个"阻挡检测"功能：当物体 A 挡住了物体 B 的抓取路径时，自动生成"先移开 A"的子计划
12. 最终自测¶
完成本章后，请用以下问题检验你的理解：
| # | 问题 | 答案位置 |
|---|---|---|
| 1 | TAMP 的全称是什么？它解决的核心问题是什么？ | 第 2 节 |
| 2 | PDDL 的 domain 文件和 problem 文件分别定义什么？ | 第 3 节 |
| 3 | FF 规划器的松弛计划启发式是如何工作的？ | 第 3.4 节 |
| 4 | C-space 中的自由空间和障碍空间分别是什么？ | 第 4.1 节 |
| 5 | 列举 TAMP 的三个核心挑战 | 第 5 节 |
| 6 | PDDLStream 中的 Stream 是什么？举一个例子 | 第 7.2 节 |
| 7 | LGP 的三层逼近策略是哪三层？ | 第 8.2 节 |
| 8 | SayCan 的核心公式中两个概率分别代表什么？ | 第 9.2 节 |
| 9 | 为什么 LLM 不能完全替代经典 TAMP？ | 第 9.5 节 |
| 10 | 画出一个简单 TAMP 系统的架构图 | 第 11 节 |
下一章预告: TAMP_T2 将深入探讨 PDDLStream 的高级特性，包括 Certified/Optimistic Streams、多种搜索策略的对比实验，以及如何处理不确定性下的 TAMP (Belief-Space TAMP)。