本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

TAMP¶

本章定位：在 D01 中我们建立了双臂的运动学框架——\(J_{\text{aug}}\)、\(J_{\text{rel}}\)、约束流形。但运动学只告诉我们"系统能怎么动"，规划则回答"系统应该怎么从 A 到 B"。双臂规划的核心挑战是：如何在 8 维弯曲约束流形上高效搜索无碰撞路径？本章从约束采样规划的三大方法出发，经过 TSR 约束表达、CBiRRT 算法、TAMP 任务级规划，到 MoveIt2 双臂配置实战，完整覆盖双臂规划的理论与工程。

适用范围：约束采样规划（Projection / Atlas / TangentBundle）对所有闭链系统通用——双臂、并联机构、可变形物体。TAMP 对多臂、多物体的长序列操作通用。

前置依赖：D01（协调运动学）、M07（OMPL 采样规划）、M04（碰撞检测）

下游章节：D03（协调力控——力相关的路径约束）、D09（MoveIt2 系统集成）、D10（综合实战）

建议用时：4 周（理论 2 周 + 代码实践 2 周）

前置自测 ⭐¶

📋 答不出 >= 2 题 → 先回前置章节复习

编号	问题	答不出时回顾
1	RRT 算法：写出 RRT 的核心循环（采样→最近邻→扩展→碰撞检查）。RRT 是概率完备的意味着什么？	M07 OMPL 采样规划
2	C-space 与工作空间：7-DOF 机械臂的 C-space 维度是多少？为什么在 C-space 而非工作空间中规划？	M07 OMPL 采样规划
3	约束流形：双 7-DOF 臂共持刚性物体时，约束流形的维度是多少？约束 Jacobian \(J_{\text{rel}}\) 的零空间物理含义是什么？	D01 约束流形
4	碰撞检测：FCL/Coal 中 `BVH` 和 `GJK` 分别做什么？窄相检测的时间复杂度与什么有关？	M04 碰撞检测
5	Newton 迭代：写出 Newton-Raphson 法的更新公式 \(x_{k+1} = x_k - f'(x_k)^{-1} f(x_k)\)。它的收敛性取决于什么？	数值方法基础

本章目标¶

学完本章后，你应该能够：

区分约束采样规划的三大方法——投影法（Projection）、Atlas 图谱法、切空间法（Tangent Bundle）——理解各自的原理、计算开销和适用场景
使用 TSR（Task Space Region）为双臂任务定义 6D 约束边界，能为"双臂抬箱保持水平"写出完整的 TSR Chain
实现基于 OMPL ConstrainedStateSpace 的双臂约束规划管线（Pinocchio 计算约束 + OMPL 规划 + MuJoCo 验证）
理解 TAMP 框架如何将"哪只手抓哪里"的任务级决策与运动规划联合求解
配置 MoveIt2 双臂规划组（both_arms group）并运行约束规划

本章知识导航¶

D2.1 核心挑战 ⭐
 │  └─ 维度灾难 + 约束零测集 + 臂间碰撞
 │
 ▼
D2.2 三大约束采样方法 ⭐⭐
 │  ├─ 投影法（Projection）── 简单但低效
 │  ├─ Atlas 图谱法 ── 高效但复杂
 │  └─ 切空间法（TangentBundle）── 折中
 │
 ▼
D2.3 TSR 约束表达 ⭐⭐ ──→ D2.4 CBiRRT 算法 ⭐⭐
 │                             │
 ▼                             ▼
D2.5 TAMP 任务级规划 ⭐⭐⭐    D2.6 臂间碰撞 ⭐⭐
                               │
                               ▼
                         D2.7 时间协调 ⭐⭐
                               │
                               ▼
                         D2.8 MoveIt2 双臂 ⭐
                               │
                               ▼
                         D2.9 前沿展望 ⭐⭐⭐⭐

前置知识桥接¶

回顾 D01（协调运动学）：闭链约束 \(h(q) = \log(T_{\text{rel,fixed}}^{-1} T_L^{-1} T_R)^{\vee} = 0\) 将 14 维 C-space 切割为 8 维约束流形 \(\mathcal{M}\)。约束 Jacobian \(J_h \approx J_{\text{rel}} = \bar{J}_R - \bar{J}_L\)，其零空间 \(\ker(J_{\text{rel}})\) 是约束流形的切空间（8 维）。在本章中，我们需要在这个 8 维弯曲流形上搜索无碰撞路径。

回顾 M07（OMPL 采样规划）：RRT 的核心循环是"采样 \(\to\) 最近邻 \(\to\) 扩展 \(\to\) 碰撞检查"。在自由 C-space 中，采样是均匀的，扩展是沿直线的。本章的关键改变是：采样必须在约束流形上，扩展必须沿流形的切方向——这就是约束采样规划的核心创新。

回顾 M04（碰撞检测）：FCL/Coal 提供 BVH 宽相过滤和 GJK/EPA 窄相距离计算。双臂系统的碰撞类型比单臂多一类——臂间碰撞（左臂和右臂之间的碰撞），这是 D2.6 的专题。

如果跳过本章会怎样¶

紧耦合任务：不理解约束规划，你无法为双臂共持物体找到从起点到目标的无碰撞路径——直线插值会破坏闭链约束，独立规划会产生内力冲突。
长序列操作：不理解 TAMP，你无法自动决定"先用哪只手抓、何时换手、是否需要 regrasp"——这些任务级决策需要符号推理和运动规划的联合求解。

预计阅读时间¶

模式	时间	建议
精读（含代码实践）	12-16 小时	完整阅读，实现 Projection 和 Atlas，跑 benchmark
速读（抓核心算法）	4-6 小时	重点读 D2.2（三方法对比）和 D2.4（CBiRRT）
速查	20-30 分钟	用知识导航定位，查 OMPL API 或 TSR 参数
复习	1-2 小时	读本章小结 + 常见误解 + 三方法对比表，做自测题

阅读路径建议：如果时间有限，按 D2.1 \(\to\) D2.2 \(\to\) D2.4（CBiRRT） \(\to\) D2.8（MoveIt2）的顺序读，跳过 D2.5（TAMP）和 D2.9（前沿）。TAMP 和前沿内容适合第二遍阅读或研究需要时查阅。

D2.1 双臂规划的核心挑战——为什么单臂方法不够 ⭐¶

动机：14 维 C-space 上的约束规划¶

回顾 M07：单臂 7-DOF 的运动规划在 7 维 C-space 中进行。RRT/PRM 等采样方法在 7 维空间中表现良好——采样效率足够，路径质量可接受。

但双臂将 C-space 提升到 14 维。维度的增加不是简单的"工作量翻倍"——采样规划的复杂度随维度**指数增长**（维度灾难）。更糟糕的是，闭链约束 \(h(q) = 0\) 使得可行空间从 14 维缩减为 8 维约束流形。在 14 维空间中均匀采样，命中 8 维约束流形（零测集）的概率为零。

反事实推理：如果直接用 RRT 在 14 维 ambient 空间中规划（不考虑约束），会怎样？采样点几乎不可能精确满足 \(\|h(q)\| = 0\)——即使降低容差到 \(\|h(q)\| < 0.01\)，在高维空间中命中率仍极低。如果强行用约束违反量作为"距离度量"做引导搜索，路径质量和收敛速度都不可接受。实验表明：在双 7-DOF 搬运任务中，无约束 RRT 需要 > 100 万次采样才能找到约束违反量 < 1mm 的路径，而约束感知的 CBiRRT 只需约 5000 次。

三大核心挑战 ⭐¶

挑战	原因	解决方法
维度灾难	14D C-space，采样效率指数下降	在 8D 约束流形上采样（Atlas/TangentBundle）
约束满足	均匀采样无法命中零测集	Newton 投影（Projection）或切空间采样（Atlas）
碰撞检测	臂间碰撞 + 自碰撞 + 环境碰撞	FCL/Coal 多对碰撞检查
时间协调	两臂到达同步	时间参数化 + 速度同步（D2.7）
任务级决策	哪只手抓哪里？需要 regrasp？	TAMP（D2.5）
路径平滑	采样路径含锯齿，约束下平滑困难	约束感知短路径平滑 + B-spline
多目标冲突	碰撞回避 vs 约束满足 vs 路径长度	task-priority 或加权目标

维度灾难的定量分析 ⭐¶

为了让读者真正理解维度灾难的严重程度，我们做一个具体的计算。

假设 C-space 是 \([0, 2\pi]^n\) 的超立方体，约束流形的"厚度"（即 \(\|h(q)\| < \epsilon\) 的区域在每个约束方向上的宽度）为 \(\epsilon\)。约束带的体积占比约为：

\[\text{ratio} \approx \left(\frac{\epsilon}{2\pi}\right)^k\]

其中 \(k\) 是约束维度。对于双 7-DOF 刚性共持：\(n = 14\)，\(k = 6\)，\(\epsilon = 0.001\) rad：

\[\text{ratio} \approx \left(\frac{0.001}{6.28}\right)^6 \approx 4.1 \times 10^{-23}\]

这意味着在 \(10^{23}\)（一百万亿亿）次均匀采样中，才期望有 1 次命中约束带。即使每次采样仅需 1 微秒，也需要约 \(3 \times 10^9\) 年——比宇宙年龄还长。

这就是为什么约束规划不能在 ambient 空间中均匀采样——必须直接在约束流形上采样（Atlas/TangentBundle），或者用投影法把每个采样点"拉"到流形上（Projection）。

从自由规划到约束规划的认知跳跃¶

自由空间规划（M07 RRT/PRM）：
  输入：q_start, q_goal, 碰撞检测器
  采样：在 C-space 中均匀采样
  连接：直线插值 + 碰撞检查
  输出：无碰撞路径

约束流形规划（本章 CBiRRT/AtlasRRT）：
  输入：q_start, q_goal, 碰撞检测器, 约束 h(q)=0
  采样：在约束流形上采样（投影/atlas/切空间）
  连接：流形上的测地线插值 + 碰撞检查
  输出：满足约束的无碰撞路径

关键区别：
  自由规划中，任何 q ∈ R^n 都是候选点
  约束规划中，只有 q ∈ M = {q : h(q)=0} 是候选点
  → 需要新的采样策略和连接策略

跨领域类比：约束流形规划 vs 球面上的路径搜索。想象在地球表面（2D 球面嵌入在 3D 空间中）找从北京到纽约的最短路。你不能沿直线穿过地球内部——必须沿球面的"大圆弧"行走。双臂约束规划也类似：你不能沿 \(\mathbb{R}^{14}\) 的直线连接两点——必须沿约束流形的"测地线"行走。投影法就是"先随便走，再投影回球面"；Atlas 法是"在球面上铺设切平面地图"。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"两只臂分别规划再合并就行"
   新手想法："左臂从 q_L,start 到 q_L,goal 规划，右臂同理，然后合并路径"
   实际上：独立规划完全忽略了闭链约束 J_rel · q̇ = 0。两条独立路径
          合并后，约束违反量 ‖h(q)‖ 可能达到厘米级，物理上意味着
          物体被挤压或拉伸——在刚性物体上这会导致极大的内力。
          即使是解耦任务，独立规划也忽略了臂间碰撞。
   正确做法：在联合 14D C-space 中做约束规划，或至少在合并后
            做约束修正（但修正代价可能比直接约束规划更高）。

练习¶

[估算] 在 14D 超立方体 \([0,1]^{14}\) 中均匀采样 \(N\) 个点，约束流形宽度为 \(\epsilon\)（即 \(\|h(q)\| < \epsilon\) 的区域体积占比）。当 \(\epsilon = 10^{-3}\) 时，需要多少个采样点才能期望有 1 个点落在约束带中？提示：这个数远大于你的想象——体积占比约 \(\epsilon^6 \sim 10^{-18}\)。
[思考] 如果约束不是等式 \(h(q) = 0\) 而是不等式 \(\|h(q)\| \leq \delta\)（弹性物体允许一定变形），规划问题会简单还是更难？
[计算] 双 6-DOF 臂（如双 ALOHA ViperX，总 12-DOF）刚性共持后有效 DOF = 12 - 6 = 6。约束流形维度 = 6，且零空间维度 = 0。与双 7-DOF 的情况（流形维度 8，零空间 2 维）相比，哪个在约束规划上更有挑战？提示：考虑零空间对碰撞回避的影响。

双臂规划的计算复杂度分层 ⭐¶

理解双臂规划的计算瓶颈有助于选择优化方向：

计算步骤	单次耗时	调用次数/规划	总占比
正运动学 FK	\(\sim 10\,\mu\text{s}\)	\(\sim 50000\)	\(\sim 10\%\)
Jacobian 计算	\(\sim 20\,\mu\text{s}\)	\(\sim 50000\)	\(\sim 20\%\)
Newton 投影 (3 步)	\(\sim 100\,\mu\text{s}\)	\(\sim 20000\)	\(\sim 40\%\)
碰撞检测	\(\sim 50\,\mu\text{s}\)	\(\sim 15000\)	\(\sim 15\%\)
最近邻搜索	\(\sim 5\,\mu\text{s}\)	\(\sim 20000\)	\(\sim 2\%\)
其他（内存分配等）	—	—	\(\sim 13\%\)

关键发现：Newton 投影占总计算量的约 40%。这解释了为什么 Atlas 法（大幅减少 Newton 调用次数）和向量化投影（加速单次 Newton 调用）是最有效的加速手段。碰撞检测占 15%——对于复杂环境（多个障碍物），碰撞检测的占比可能升至 30-40%。

D2.2 约束采样规划的三大方法 ⭐⭐¶

问题的统一形式¶

输入：起点 \(q_{\text{start}} \in \mathcal{M}\)，终点 \(q_{\text{goal}} \in \mathcal{M}\)，约束 \(h: \mathbb{R}^n \to \mathbb{R}^k\)，碰撞检测器 \(\texttt{CollisionFree}(q)\)

输出：路径 \(\pi: [0,1] \to \mathcal{M}\) 使得 \(\pi(0) = q_{\text{start}}\)，\(\pi(1) = q_{\text{goal}}\)，\(h(\pi(s)) = 0\)，\(\texttt{CollisionFree}(\pi(s))\) 对所有 \(s \in [0,1]\)。

前提条件：\(q_{\text{start}}\) 和 \(q_{\text{goal}}\) 必须精确在约束流形上（\(\|h(q_{\text{start}})\| < \epsilon\)，\(\|h(q_{\text{goal}})\| < \epsilon\)）。如果起点或终点不满足约束，需要先投影到流形上——这一步本身就可能失败（如目标位姿运动学不可达或物理上不可行）。

此外，起点和终点必须在约束流形的**同一连通分支**上——否则不存在约束满足路径。判断连通性是 NP-hard 问题，实践中通过规划尝试间接检测。当规划器在大量采样后仍未找到路径时，很可能两个构型处于不同的连通分支——此时需要考虑 regrasp（松开物体重新抓取到另一个构型）。

三种方法的核心区别在于**如何在约束流形 \(\mathcal{M}\) 上生成新的采样点**。三种方法共享同一个约束函数接口（OMPL 的 Constraint 基类），因此切换方法只需更换状态空间类型——这是 OMPL 约束规划框架的核心设计优势。下面分别介绍每种方法的原理、实现和性能特征。

方法 1：投影法（Projection） ⭐⭐¶

投影法是约束采样规划的"基线方法"——所有其他方法都与它对比来证明自身的优越性。

核心思想：在 ambient 空间 \(\mathbb{R}^n\) 中自由采样，然后用 Newton 迭代投影到 \(\mathcal{M}\)。这是三种方法中概念最简单、实现最直接的方案。

算法步骤：
  1. 在 R^n 中均匀采样 q_rand
  2. Newton 迭代投影到 M：
     repeat:
       q ← q - J_h⁺(q) · h(q)     (J_h⁺ 是 J_h 的伪逆)
     until ‖h(q)‖ < ε  or  达到最大迭代次数

  3. 如果收敛 → q_proj 是约束流形上的有效点
     如果不收敛 → 丢弃这个采样

  4. 碰撞检查 + 加入 RRT 树

Newton 投影的几何直觉：

        q_rand (R^n 中的自由点)
          │
          │ Newton 步: q ← q - J_h⁺ · h(q)
          ↓
        ·  ·  ·  ·  ·  ·
      ·                    ·
     ·    q_proj (流形上)    ·     ← 约束流形 M
      ·                    ·
        ·  ·  ·  ·  ·  ·

  每一步 Newton 迭代沿法方向（J_h^T 方向）移动，
  使 ‖h(q)‖ 减小。2-3 步通常足够收敛。

为什么用伪逆 \(J_h^+\) 而不是 \(J_h^{-1}\)？ 因为 \(J_h \in \mathbb{R}^{k \times n}\) 不是方阵（\(k < n\)），没有逆矩阵。当 \(J_h\) 满行秩时，伪逆 \(J_h^+ = J_h^T (J_h J_h^T)^{-1}\) 给出最小范数修正——即在满足约束的前提下，对 \(q\) 的改变量最小。若 \(J_h\) 行秩亏损（约束冗余或接近奇异），需用 SVD 截断伪逆或阻尼伪逆替代。

投影成功率分析：

投影法的效率瓶颈在于 Newton 投影的**成功率**（即收敛到流形的比例）。成功率取决于：

因素	影响	典型数值
约束维度 \(k\)	\(k\) 越大，约束越紧，成功率越低	\(k=6\)（双臂闭链）：成功率约 30-60%
流形曲率	曲率越大，Newton 投影越容易跳到错误分支	高曲率区域成功率可降至 10%
初始点到流形距离	距离越远，收敛越慢，成功率越低	距离 \(> 0.5\) rad 时成功率显著下降
\(J_h\) 条件数	条件数越大，投影越不稳定	\(\kappa > 10^4\) 时建议用阻尼伪逆

2024 年的加速突破：Kingston 等人 2024 年提出了**向量化投影**（Vectorized Projection），核心思想是利用 SIMD 指令并行投影多个构型——在 RRT 扩展步骤中，同时投影一条 edge 上的多个中间点，而非逐点串行投影。实验表明，在 AVX-512 支持的 CPU 上，向量化投影将双臂约束规划的总时间减少了 3-5 倍。这项工作的重要启示是：约束规划的计算瓶颈不在搜索策略，而在投影操作本身——优化投影的底层实现比改进搜索算法更有效。

优势： - 实现最简单——只需约束函数 \(h(q)\) 和 Jacobian \(J_h(q)\) - OMPL 直接提供 ProjectedStateSpace - 对低维约束（\(k\) 小）效率可接受 - 2024 年的向量化投影技术可进一步提升性能

劣势： - 采样效率低：在高维 ambient 空间采样，投影到低维流形，大量点被浪费 - Newton 不保证收敛：多解情况下可能投影到远离树的点 - 高曲率流形收敛慢：需要更多迭代或更小步长

OMPL 参数：

参数	默认值	说明
`tolerance`	\(10^{-3}\)	约束满足容差 \(\\|h(q)\\| < \text{tolerance}\)
`maxIterations`	50	Newton 最大迭代次数
`delta`	0.1	状态空间的步长

方法 2：Atlas 图谱法（Jaillet-Porta 2013） ⭐⭐⭐¶

核心思想：在流形 \(\mathcal{M}\) 上维护一组局部坐标图（chart），采样在 chart 内低维切空间中进行，然后映射回流形。

Atlas 法的关键创新：
  投影法在 n 维空间采样 → 效率 ~ (ε/L)^k (ε=约束带宽, L=空间范围)
  Atlas 法在 (n-k) 维切空间采样 → 效率接近在流形上均匀采样

  对于双 7-DOF (n=14, k=6, 流形维度=8)：
  投影法在 14D 采样 → 效率极低
  Atlas 法在 8D 采样 → 效率高得多

Atlas 的构造过程：

Step 1: 初始 chart C_0 在 q_start 处
  - 计算切空间基：T_{q_0}M = ker(J_h(q_0))
    工程上常用 D01 的同点相对 Jacobian J_rel = J̄_R - J̄_L 作为 J_h 的局部线性化
    通过 SVD: J_h = U Σ V^T → 切空间基 = V 的后 (n-k) 列
    （即约束 Jacobian 的零空间正交基）
  - C_0 = (q_0, basis_0, radius_0)

Step 2: 在 C_0 内采样
  - 生成低维随机向量 Δu ∈ R^{n-k}（8 维）
  - 映射到 ambient 空间：q_proj = q_0 + basis_0 · Δu
  - Newton 修正：q ← q - J_h⁺ h(q)（1-2 步即可，因为起点已接近流形）

Step 3: 如果 q 距 C_0 中心太远 → 创建新 chart C_1
  - 在 q 处计算新的切空间基（新的 SVD）
  - 检查相邻 chart 的法向夹角 < α_max（防止跨越高曲率区域）
  - 建立 chart 间的转移映射（transition map）

Step 4: RRT 在 atlas 上扩展
  - 选最近 chart → 在其内采样 → Newton 修正
  - 如果跨越 chart 边界 → 切换到相邻 chart

OMPL 中的 AtlasStateSpace：

// Atlas 法——只需替换状态空间类型，其余代码不变
auto css = std::make_shared<ompl::base::AtlasStateSpace>(
    ambient, constraint);

// Atlas 特有参数
css->setExploration(0.5);   // 探索 vs 利用平衡 (0=纯利用, 1=纯探索)
css->setRho(0.1);           // chart 半径
css->setAlpha(M_PI/6);      // 相邻 chart 最大法向夹角（30°）
css->setEpsilon(0.05);      // chart 内采样偏移量

// 其余代码与投影法完全相同——这就是 OMPL 统一 API 的优势
auto csi = std::make_shared<ompl::base::ConstrainedSpaceInformation>(css);
auto planner = std::make_shared<ompl::geometric::RRTConnect>(csi);

Atlas 法的存储开销分析：

每个 chart 存储：
  - 中心点 q_i ∈ R^14         → 14 × 8 = 112 bytes
  - 切空间基 ∈ R^{14×8}       → 14 × 8 × 8 = 896 bytes
  - 元数据（半径、相邻信息）   → ~100 bytes

  每个 chart ≈ 1.1 KB

  500 个 chart → ~550 KB（非常小）

  vs PRM 存 500 个 14D 节点 → 500 × 14 × 8 = 56 KB
  但 PRM 节点不满足约束！Atlas 同时提供约束满足 + 高效采样

方法 3：切空间法（Tangent Bundle） ⭐⭐¶

核心思想：AtlasRRT 的"惰性"版本
  - 不主动维护全局 atlas
  - 在每次扩展时临时构造切空间，使用后不保留

优势：内存更低（不存储 chart 集合）
劣势：
  - 同一区域重复计算切空间基（每次都做 SVD）
  - 覆盖度量不如 atlas 准确（没有全局 chart 信息）

OMPL: TangentBundleStateSpace
参数与 AtlasStateSpace 类似

TangentBundle 的工程选择依据：在大多数双臂规划场景中，TangentBundle 是最佳的折中选择——它在采样效率上接近 Atlas（在切空间采样而非 ambient 空间），但实现复杂度和内存开销接近 Projection。Kingston 等人 2019 年的 IMACS 论文（OMPL ConstrainedStateSpace 的理论蓝图）的实验表明：在双 7-DOF 搬运任务中，TangentBundle 的规划时间是 Projection 的 0.3-0.5 倍，是 Atlas 的 1.2-1.5 倍——性能接近 Atlas 但无需管理 chart 数据结构。

约束感知的路径插值：三种方法都需要一个共同的子程序——约束感知插值（constraint-aware interpolation）。在 RRT 的"连接"步骤中，不能直接用直线连接两个流形上的点（直线不在流形上），而需要沿流形的"近似测地线"插值：

约束感知插值算法：
  输入：流形上两点 q_a, q_b，步长 δ
  输出：流形上的路径 [q_a, q_1, q_2, ..., q_b]

  for t = δ, 2δ, ..., 1:
    q_interp = (1-t) · q_a + t · q_b    # 直线插值
    q_proj = Newton_Project(q_interp)     # 投影回流形
    if ‖h(q_proj)‖ > ε or Collision(q_proj):
      return FAILURE
    path.append(q_proj)

  return path

这个插值在步长 \(\delta\) 较大或流形曲率较高时可能失败（投影后的点偏离直线太远，可能跳到流形的其他分支）。工程上通常设 \(\delta \le 0.05\) rad 以保证插值质量。

三种方法的系统对比¶

维度	Projection	Atlas	TangentBundle
采样空间维度	\(n\)（14D）	\(n-k\)（8D）	\(n-k\)（8D）
采样效率	低	高	中
Newton 迭代	每次 3-10 步	每次 1-2 步	每次 1-2 步
实现复杂度	低	高	中
内存开销	低	高（chart 存储）	低
收敛保证	Newton 局部	chart 内保证	Newton 局部
高曲率流形	差	好（自适应 chart）	中
推荐场景	入门/debug/低维	高维/高曲率/工业级	一般场景/折中
OMPL 类	`ProjectedStateSpace`	`AtlasStateSpace`	`TangentBundleStateSpace`

选择决策树：

约束流形维度 ≤ 6？
├── 是 → 投影法足够（低维，Newton 效率可接受）
└── 否 → 流形曲率高？（如双臂操作空间有多个奇异点）
    ├── 是 → Atlas 法（自适应 chart 处理高曲率）
    └── 否 → TangentBundle（折中选择，内存低）

实际工程建议：先用 Projection 做原型验证，
确认可行后切换到 Atlas 优化性能——
OMPL 的统一 API 使切换只需改一行代码。

Benchmark 数据：三种方法在双 Franka 搬运任务中的实测对比 ⭐⭐¶

以下 benchmark 数据来自 Kingston et al. 2019 (IMACS) 的双 7-DOF 机械臂搬运任务（刚性共持 + 保持水平约束），提供了三种方法的定量对比：

指标	Projection	Atlas	TangentBundle
中位规划时间 (s)	12.4	3.2	4.8
成功率 (60s 超时)	78%	96%	92%
路径长度 (rad)	8.7	7.2	7.5
约束违反量 RMS	\(2.1 \times 10^{-4}\)	\(1.8 \times 10^{-4}\)	\(2.0 \times 10^{-4}\)
Newton 调用次数	48000	6200	9800
内存峰值 (MB)	12	45	15
碰撞检查次数	52000	18000	24000

关键观察： 1. Atlas 规划时间最短（3.2s vs 12.4s），但内存最高（45MB vs 12MB） 2. 三种方法的约束违反量相当（都在 \(10^{-4}\) 级别），说明数值精度主要取决于 Newton 容差设置，与采样方法无关 3. Newton 调用次数的巨大差异（48000 vs 6200）解释了时间差异——投影法将大量时间花在"投影失败→重新采样"的循环中 4. 碰撞检查次数与 Newton 调用次数强相关——每次成功投影后都需要碰撞检查

工程建议：如果你的任务需要 < 5s 的规划时间（如实时重规划），Atlas 或 TangentBundle 是必须的；如果 > 30s 可接受（如离线规划），Projection 足够且实现最简单。

本质洞察：三种约束采样方法的本质区别不是"算法不同"，而是"在什么空间采样"。投影法在 ambient 空间采样（高维，浪费），Atlas 在切空间采样（低维，高效），TangentBundle 是 Atlas 的无记忆版本。理解了"采样空间的维度决定效率"，三种方法的优劣就一目了然。

Pinocchio + OMPL 投影法完整实现¶

// ============================================================
// 投影法规划管线——ProjectedStateSpace + RRTConnect
// 完整可运行代码框架
// ============================================================

#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
#include <ompl/geometric/planners/rrt/RRTConnect.h>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/frames.hpp>

// --- Step 1: 加载模型 ---
pinocchio::Model model;
pinocchio::urdf::buildModel("dual_panda.urdf", model);
pinocchio::Data data(model);

const auto left_ee_id = model.getFrameId("panda_left_hand");
const auto right_ee_id = model.getFrameId("panda_right_hand");

// 计算初始相对位姿作为约束目标
Eigen::VectorXd q_init = pinocchio::neutral(model);
pinocchio::forwardKinematics(model, data, q_init);
pinocchio::updateFramePlacements(model, data);
pinocchio::SE3 T_rel_fixed = data.oMf[left_ee_id].inverse() * data.oMf[right_ee_id];

// --- Step 2: 创建 ambient 状态空间 ---
auto ambient = std::make_shared<ompl::base::RealVectorStateSpace>(model.nq);
ompl::base::RealVectorBounds bounds(model.nq);
for (int i = 0; i < model.nq; i++) {
    bounds.setLow(i, model.lowerPositionLimit[i]);
    bounds.setHigh(i, model.upperPositionLimit[i]);
}
ambient->setBounds(bounds);

// --- Step 3: 创建约束 ---
// 使用 D01 中定义的 DualArmConstraint。
// 注意：闭链 Jacobian 不能直接写成原始 J_R-J_L；必须先把左右 twist
// 通过 adjoint/平移项变换到同一表达系和同一参考点。
auto constraint = std::make_shared<DualArmConstraint>(
    model, data, left_ee_id, right_ee_id, T_rel_fixed);

// --- Step 4: 创建约束状态空间（投影法）---
auto css = std::make_shared<ompl::base::ProjectedStateSpace>(
    ambient, constraint);
auto csi = std::make_shared<ompl::base::ConstrainedSpaceInformation>(css);

// --- Step 5: 设置碰撞检测 ---
csi->setStateValidityChecker([&](const ompl::base::State* state) {
    // ConstrainedStateSpace::StateType 继承自 Eigen::Map<VectorXd>，可直接当 Eigen 向量使用
    const Eigen::Map<const Eigen::VectorXd>& q =
        *state->as<ompl::base::ConstrainedStateSpace::StateType>();
    // Pinocchio 碰撞检测
    return !pinocchio::computeCollisions(model, data,
        geom_model, geom_data, q, true);
});

// --- Step 6: 设置起点/终点 ---
ompl::base::ScopedState<> start(css), goal(css);
// ConstrainedStateSpace::StateType 继承自 Eigen::Map<VectorXd>，直接赋值即可
auto* sv = start->as<ompl::base::ConstrainedStateSpace::StateType>();
Eigen::Map<Eigen::VectorXd>(sv->getValues(), model.nq) = q_start;
css->enforceBounds(start.get());  // 只做状态边界裁剪，不会投影到约束流形 M

auto* gv = goal->as<ompl::base::ConstrainedStateSpace::StateType>();
Eigen::Map<Eigen::VectorXd>(gv->getValues(), model.nq) = q_goal;
css->enforceBounds(goal.get());   // 只做状态边界裁剪，不会修复闭链误差

// q_start/q_goal 若来自外部 IK 或示教，必须先验证满足闭链约束：
// ||constraint(q)|| < tol。若不满足，应使用 ProjectedStateSpace/constraint
// project 接口把状态投影到 M，或重新求解满足 T_L^{-1}T_R = T_rel_fixed 的 IK。

// --- Step 7: 规划 ---
auto pdef = std::make_shared<ompl::base::ProblemDefinition>(csi);
pdef->setStartAndGoalStates(start, goal);

auto planner = std::make_shared<ompl::geometric::RRTConnect>(csi);
planner->setRange(0.5);
planner->setProblemDefinition(pdef);
planner->setup();

auto status = planner->solve(30.0);  // 30s 超时

if (status) {
    auto path = pdef->getSolutionPath()->as<ompl::geometric::PathGeometric>();
    path->interpolate(50);  // 插值到 50 个点

    // 验证约束满足
    for (size_t i = 0; i < path->getStateCount(); i++) {
        const Eigen::Map<const Eigen::VectorXd>& q =
            *path->getState(i)->as<ompl::base::ConstrainedStateSpace::StateType>();
        Eigen::VectorXd h(6);
        constraint->function(q, h);
        std::cout << "Point " << i << ": constraint violation = "
                  << h.norm() << std::endl;
    }
}

// ============================================================
// ❌ 常见错误：起点不在约束流形上
// ============================================================
// 直接用 IK 解作为起点，但 IK 解不精确满足闭链约束
Eigen::VectorXd q_ik = solve_ik(target_L, target_R);
// ‖h(q_ik)‖ ≈ 0.005 > tolerance=0.001
// → OMPL 拒绝这个起点，规划直接失败

// ✅ 正确做法：先投影到约束流形
Eigen::VectorXd q = q_ik;
for (int iter = 0; iter < 50; iter++) {
    Eigen::VectorXd h(6);
    constraint->function(q, h);
    if (h.norm() < 1e-4) break;
    Eigen::MatrixXd J(6, model.nq);
    constraint->jacobian(q, J);
    // SVD 伪逆（最稳定）
    q -= J.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(h);
}

⚠️ 常见陷阱¶

⚠️ 编程陷阱：OMPL 约束规划中忘记设置碰撞检测
   错误做法：只设置了约束，没设置 StateValidityChecker
   现象：规划成功但路径穿越障碍物或两臂碰撞
   根本原因：OMPL 的约束只保证 h(q)=0，碰撞检查是独立的
   正确做法：必须同时设置约束（通过 ConstrainedStateSpace）
            和碰撞检测（通过 setStateValidityChecker）

💡 概念误区：认为"Atlas 法总是比投影法好"
   新手想法："Atlas 在低维切空间采样，效率一定更高"
   实际上：Atlas 法有初始化开销（第一个 chart 的 SVD）和维护成本
          （chart 间转移映射）。对于低维约束（k=1-2）或低曲率流形，
          投影法的 Newton 迭代很快收敛，总体时间可能比 Atlas 更短。
   正确思维：选择方法取决于 (n-k, 曲率, 规划次数) 的组合。
            一次性规划用投影法；反复规划同类问题用 Atlas（chart 可复用）。

练习¶

[实验] 在 OMPL 中用 ProjectedStateSpace + RRTConnect 运行 OMPL 自带的 ConstrainedPlanningImplicitChain demo。记录投影成功率和规划时间。
[对比] 在同一双臂问题上切换 Projected/Atlas/TangentBundle 三种方法。各跑 50 次，绘制规划时间的箱线图（box plot）。
[跨章综合] 回顾 M07 中 BIT* 的渐近最优性。将 BIT* 与约束投影法结合（即 Constrained BIT*），讨论：在约束流形上，"最优路径"的度量应该是 ambient 空间的欧几里得距离还是流形上的测地线距离？

D2.3 TSR——约束的工程化表达 ⭐⭐¶

动机：如何优雅地描述约束¶

在 D2.2 中，约束通过 \(h(q) = 0\) 定义。但工程中，我们通常不直接写约束函数——而是用更直觉的方式描述"末端允许的位姿范围"。

Berenson, Srinivasa 和 Kuffner 在 2011 年提出了 TSR（Task Space Region）——一种用 6D 边界框描述任务空间约束的通用表达方式。TSR 的优势是：工程师只需指定"末端位姿的哪些自由度被约束、约束范围是多少"，框架自动生成约束函数。

TSR 定义 ⭐⭐¶

一个 TSR 由三元组 \((T_{w0}, T_{we}, B_w)\) 组成：

\[T_{w0} \in SE(3): \quad \text{TSR 参考帧相对于世界系的位姿}\]

\[T_{we} \in SE(3): \quad \text{末端相对于 TSR 帧的"标称位姿"}\]

\[B_w \in \mathbb{R}^{6 \times 2}: \quad \text{6 维 bound box } [\text{lo}, \text{hi}]\]

\(B_w\) 的每一行约束一个任务空间维度（3 平移 + 3 旋转）：

B_w = ⎡ x_lo,  x_hi  ⎤   x 平移
      ⎢ y_lo,  y_hi  ⎥   y 平移
      ⎢ z_lo,  z_hi  ⎥   z 平移
      ⎢ rx_lo, rx_hi ⎥   roll 旋转
      ⎢ ry_lo, ry_hi ⎥   pitch 旋转
      ⎣ rz_lo, rz_hi ⎦   yaw 旋转

约定：
  - [lo, hi] = [-∞, +∞] → 该维度自由（不约束）
  - [0, 0] → 该维度固定（严格等式约束）
  - [-0.05, 0.05] → 该维度允许小范围偏差（±3°或±5cm）

TSR 的约束函数生成：

给定 TSR \((T_{w0}, T_{we}, B_w)\) 和末端位姿 \(T_{ee}\)，约束违反量为：

\[\delta = \log(T_{we}^{-1} \cdot T_{w0}^{-1} \cdot T_{ee})^{\vee} \in \mathbb{R}^6\]

对于每个维度 \(i\)：

\[h_i = \begin{cases} \delta_i - B_w^{lo}(i) & \text{if } \delta_i < B_w^{lo}(i) \\ \delta_i - B_w^{hi}(i) & \text{if } \delta_i > B_w^{hi}(i) \\ 0 & \text{if } B_w^{lo}(i) \leq \delta_i \leq B_w^{hi}(i) \end{cases}\]

TSR 使用示例¶

示例 1：双臂抬箱保持水平

T_w0 = 箱子当前位姿
T_we = I (末端与 TSR 帧重合)
B_w = ⎡ [-∞, +∞]     ⎤  x: 自由
      ⎢ [-∞, +∞]     ⎥  y: 自由
      ⎢ [-∞, +∞]     ⎥  z: 自由
      ⎢ [-0.05, 0.05] ⎥  roll:  ±3°（几乎水平）
      ⎢ [-0.05, 0.05] ⎥  pitch: ±3°
      ⎣ [-∞, +∞]     ⎦  yaw: 自由

物理含义：箱子可以在空间中自由移动和旋转 yaw，
         但 roll 和 pitch 必须接近零（保持水平）。
约束维度：2（roll 和 pitch 两个约束）

示例 2：倒水——壶嘴始终朝下

T_w0 = 杯子位姿
T_we = 壶嘴相对杯子的目标位姿
B_w = ⎡ [-0.02, 0.02] ⎤  x: 精确对准杯口（±2cm）
      ⎢ [-0.02, 0.02] ⎥  y: 精确对准杯口
      ⎢ [0.05, 0.20]  ⎥  z: 壶嘴在杯口上方 5-20cm
      ⎢ [-∞, +∞]      ⎥  roll: 自由
      ⎢ [1.0, 2.5]    ⎥  pitch: 倾斜倒水角度（57°-143°）
      ⎣ [-∞, +∞]      ⎦  yaw: 自由

约束维度：4（x, y, z, pitch 四个约束）

TSR Chain——双臂闭链约束的 TSR 表达¶

双臂共持物体形成闭链——这可以用一个 TSR Chain 描述：

TSR Chain = 左臂 TSR + 物体固定变换 + 右臂 TSR

具体构成：
  TSR_L: 左臂末端相对物体的约束
    T_w0 = 物体位姿（变量——物体在被搬运）
    T_we = T_{obj→left_grasp}（固定——抓取点不变）
    B_w = 全零（完全固定——不允许松手）

  T_{LR}: 两抓取点之间的固定变换（由物体几何决定）

  TSR_R: 右臂末端相对物体的约束
    T_w0 = 物体位姿
    T_we = T_{obj→right_grasp}
    B_w = 全零

组合约束：
  左手抓取固定 AND 物体变换固定 AND 右手抓取固定
  → 等价于 J_h · q̇ = 0（D01 中的闭链约束，局部可用同点 J_rel）

  但 TSR 可以叠加额外约束——如 "搬运途中保持水平"：
  TSR_level: 物体的 roll/pitch 约束
    B_w[3:5] = [-0.05, 0.05]

跨领域类比：TSR 之于约束规划，就像 SQL 之于数据库查询。你不需要手写底层的数据检索算法——只需要声明"我要什么"（约束的范围），框架自动生成高效的执行计划。TSR 让工程师从"写约束函数"的底层工作中解放出来，专注于"描述任务需求"。

TSR 到 OMPL 约束的转换¶

// ============================================================
// TSR 转换为 OMPL Constraint
// ============================================================
class TSRConstraint : public ompl::base::Constraint {
public:
    TSRConstraint(int n_dof,
                  const pinocchio::SE3& T_w0,
                  const pinocchio::SE3& T_we,
                  const Eigen::Matrix<double,6,2>& Bw,
                  std::function<pinocchio::SE3(const Eigen::VectorXd&)> fk)
        : ompl::base::Constraint(n_dof, countActiveDims(Bw))
        , T_w0_(T_w0), T_we_(T_we), Bw_(Bw), fk_(fk)
    {
        // 记录哪些维度被约束
        for (int i = 0; i < 6; i++) {
            constrained_dims_.push_back(
                !(Bw(i,0) == -1e10 && Bw(i,1) == 1e10));
        }
    }

    void function(const Eigen::Ref<const Eigen::VectorXd>& q,
                  Eigen::Ref<Eigen::VectorXd> out) const override {
        pinocchio::SE3 T_ee = fk_(q);
        Eigen::Matrix<double,6,1> delta =
            pinocchio::log6(T_we_.inverse() * T_w0_.inverse() * T_ee).toVector();

        int idx = 0;
        for (int i = 0; i < 6; i++) {
            if (constrained_dims_[i]) {
                if (delta(i) < Bw_(i,0))
                    out(idx) = delta(i) - Bw_(i,0);
                else if (delta(i) > Bw_(i,1))
                    out(idx) = delta(i) - Bw_(i,1);
                else
                    out(idx) = 0.0;
                idx++;
            }
        }
    }

private:
    static int countActiveDims(const Eigen::Matrix<double,6,2>& Bw) {
        int count = 0;
        for (int i = 0; i < 6; i++)
            if (!(Bw(i,0) == -1e10 && Bw(i,1) == 1e10)) count++;
        return count;
    }

    pinocchio::SE3 T_w0_, T_we_;
    Eigen::Matrix<double,6,2> Bw_;
    std::function<pinocchio::SE3(const Eigen::VectorXd&)> fk_;
    std::vector<bool> constrained_dims_;
};

TSR Chain 双臂约束规划——完整代码¶

将 TSR Chain 中的闭链约束 + 附加任务约束组合为一个 OMPL 多约束求解管线：

// ============================================================
// TSR Chain 约束规划——双臂共持+保持水平
// 组合两种约束：闭链约束 + 姿态约束
// ============================================================

class TSRChainConstraint : public ompl::base::Constraint {
public:
    TSRChainConstraint(
        const pinocchio::Model& model, pinocchio::Data& data,
        pinocchio::FrameIndex left_ee, pinocchio::FrameIndex right_ee,
        const pinocchio::SE3& T_rel_fixed,
        const Eigen::Matrix<double,6,2>& Bw_level)
        : ompl::base::Constraint(model.nq, 8)  // 6(闭链) + 2(roll,pitch)
        , model_(model), data_(data)
        , left_ee_(left_ee), right_ee_(right_ee)
        , T_rel_(T_rel_fixed), Bw_(Bw_level)
    {}

    void function(const Eigen::Ref<const Eigen::VectorXd>& q,
                  Eigen::Ref<Eigen::VectorXd> out) const override
    {
        pinocchio::forwardKinematics(model_, data_, q);
        pinocchio::updateFramePlacements(model_, data_);

        const auto& T_L = data_.oMf[left_ee_];
        const auto& T_R = data_.oMf[right_ee_];

        // 约束 1-6：闭链约束 h(q) = log(T_rel^{-1} T_L^{-1} T_R)
        pinocchio::SE3 T_err = T_rel_.inverse() * T_L.inverse() * T_R;
        out.head(6) = pinocchio::log6(T_err).toVector();

        // 约束 7-8：物体 roll/pitch 保持水平
        // 物体姿态 ≈ 左臂末端姿态（假设对称抓取）
        Eigen::Matrix3d R_obj = T_L.rotation();
        // 提取 roll, pitch（ZYX 欧拉角）
        double pitch = std::asin(-R_obj(2, 0));
        double roll  = std::atan2(R_obj(2, 1), R_obj(2, 2));

        // TSR bound 违反量
        out(6) = std::max(0.0, roll - Bw_(3, 1))
               + std::min(0.0, roll - Bw_(3, 0));
        out(7) = std::max(0.0, pitch - Bw_(4, 1))
               + std::min(0.0, pitch - Bw_(4, 0));
    }

    void jacobian(const Eigen::Ref<const Eigen::VectorXd>& q,
                  Eigen::Ref<Eigen::MatrixXd> J) const override
    {
        // 数值差分（开发阶段安全选择）
        const double eps = 1e-7;
        Eigen::VectorXd q_plus = q, h_plus(8), h_base(8);
        function(q, h_base);
        for (int i = 0; i < q.size(); i++) {
            q_plus(i) = q(i) + eps;
            function(q_plus, h_plus);
            J.col(i) = (h_plus - h_base) / eps;
            q_plus(i) = q(i);
        }
    }

private:
    const pinocchio::Model& model_;
    mutable pinocchio::Data data_;
    pinocchio::FrameIndex left_ee_, right_ee_;
    pinocchio::SE3 T_rel_;
    Eigen::Matrix<double,6,2> Bw_;
};

// --- 使用示例 ---
Eigen::Matrix<double,6,2> Bw_level;
Bw_level << -1e10, 1e10,   // x: 自由
            -1e10, 1e10,   // y: 自由
            -1e10, 1e10,   // z: 自由
            -0.05, 0.05,   // roll: ±3°
            -0.05, 0.05,   // pitch: ±3°
            -1e10, 1e10;   // yaw: 自由

auto tsr_chain = std::make_shared<TSRChainConstraint>(
    model, data, left_ee_id, right_ee_id, T_rel_fixed, Bw_level);

auto css = std::make_shared<ompl::base::ProjectedStateSpace>(
    ambient, tsr_chain);
// 后续与 D2.2 中的规划管线完全相同——OMPL 统一 API 的威力

⚠️ 常见陷阱¶

💡 概念误区：认为"TSR 的 6D bound 是在世界系中定义的"
   新手想法："B_w 的 x,y,z 限制的是世界系中的绝对位置"
   实际上：B_w 的约束是在 TSR 参考帧 T_w0 中定义的。如果 T_w0 是物体位姿，
          那么 B_w 限制的是"末端相对物体的偏差"——物体移动时，
          约束自动跟随物体。这使得 TSR 天然支持"保持抓取"类约束。
   正确理解：TSR 约束是相对于参考帧 T_w0 的，不是世界系绝对约束。

⚠️ 编程陷阱：TSR bound 的旋转部分使用欧拉角
   问题：TSR 原始定义中旋转 bound 使用 roll-pitch-yaw 欧拉角，
        但欧拉角在 pitch ≈ ±π/2 时有万向锁（gimbal lock）
   现象：约束在某些姿态下不连续，Newton 投影发散
   正确做法：对接近万向锁的姿态使用 axis-angle 或四元数约束

练习¶

[设计] 为"双臂抬箱旋转 90 度"写出完整的 TSR Chain。箱子需要保持水平（roll, pitch < 3 度），yaw 从 0 到 90 度连续变化。
[精读] 精读 Berenson 2011 论文的 Section III（TSR 定义）和 Section IV（CBiRRT2 算法）。为"在桌面上推动箱子沿直线移动 50cm"设计 TSR——提示：z 方向固定（桌面高度），roll/pitch 固定（箱子不翻倒），x 方向范围 [0, 0.5]。

D2.4 CBiRRT——约束双向 RRT ⭐⭐¶

动机：为什么需要双向搜索¶

单向 RRT 从起点向目标扩展，在高维空间中效率较低。双向 RRT（BiRRT/RRTConnect）从起点和终点同时扩展两棵树，当两树连接时找到路径。实验表明，RRTConnect 在大多数运动规划问题上比单向 RRT 快 5-50 倍。

CBiRRT（Constrained BiRRT）是 Berenson 等人 2009/2011 年将约束投影与 BiRRT 结合的算法——双臂约束规划的事实标准。

跨领域类比：CBiRRT 的双向搜索就像修建隧道时从山的两侧同时开挖——两支施工队在山体中间"会合"时，隧道就贯通了。单向 RRT 相当于只从一侧挖到另一侧，工程量几乎翻倍。约束投影则相当于"地质探测仪"——每挖一步都用仪器确认掘进方向仍在岩层允许的范围内，防止偏离设计隧道断面。

算法完整伪代码 ⭐⭐¶

输入: q_start ∈ M, q_goal ∈ M, 约束集 {C_i}, 碰撞检测器
输出: 满足约束的无碰撞路径，或 FAILURE

1. T_a ← {q_start}; T_b ← {q_goal}

2. for iter = 1 to max_iter:

   3. q_rand ← SampleAmbient()
      // 以概率 p_goal=0.05 直接采样 q_goal（目标偏向）

   4. q_near ← NearestNeighbor(T_a, q_rand)

   5. q_new ← Extend(q_near, q_rand, δ)
      // q_new = q_near + δ · (q_rand - q_near) / ‖q_rand - q_near‖

   6. (success, q_new) ← Project(q_new, {C_i})
      // Newton 投影到约束流形
      if not success: continue

   7. if not EdgeValid(q_near, q_new):
      // 约束感知的边碰撞检查
      continue

   8. T_a.Add(q_new, parent=q_near)

   9. // 尝试从 T_b 连接到 q_new
      q_connect ← NearestNeighbor(T_b, q_new)
      while true:
        q_step ← Extend(q_connect, q_new, δ)
        (ok, q_step) ← Project(q_step, {C_i})
        if not ok: break
        if not EdgeValid(q_connect, q_step): break
        T_b.Add(q_step, parent=q_connect)
        q_connect ← q_step
        if ‖q_connect - q_new‖ < ε:
          return ExtractPath(T_a, T_b, q_new, q_connect)

   10. Swap(T_a, T_b)  // 双向交替扩展

11. return FAILURE

Project 子程序详解：

function Project(q, {C_i}):
  for iter = 1 to max_project_iter:
    all_satisfied ← true
    for each constraint C_i:
      C_i.function(q, out_i)
      if ‖out_i‖ > ε:
        C_i.jacobian(q, J_i)
        // 伪逆 Newton 步
        Δq ← J_i.bdcSvd(ComputeThinU|ComputeThinV).solve(out_i)
        q ← q - Δq
        all_satisfied ← false
    if all_satisfied:
      return (true, q)
  return (false, q)  // 未收敛

EdgeValid——约束感知的边检查：

function EdgeValid(q_a, q_b):
  // 在 ambient 空间线性插值 + 约束投影 + 碰撞检查
  N ← ceil(‖q_b - q_a‖ / resolution)
  for i = 1 to N:
    s ← i / N
    q_interp ← (1-s) · q_a + s · q_b
    (ok, q_interp) ← Project(q_interp)  // 投影回流形
    if not ok: return false
    if not CollisionFree(q_interp): return false
  return true

反事实推理：如果 EdgeValid 不做约束投影（直接线性插值+碰撞检查），会怎样？中间点 \(q(s)\) 偏离约束流形——对刚性共持任务，这意味着物体在路径中间被"撕开"。虽然起点和终点都满足约束，但中间约束违反使路径物理不可行。在 MuJoCo 执行时，控制器需要施加极大力矩来补偿约束违反，导致力矩饱和或物体脱落。

Worked Example：CBiRRT 在双 Franka 搬运中的完整跟踪¶

以下逐步跟踪 CBiRRT 在"双 Franka 共持 30cm 杆从桌 A 搬到桌 B"中的前 5 次迭代，展示算法的关键决策过程。

场景参数：
  双 7-DOF Franka Panda，q ∈ R^14
  约束：h(q) = log(T_rel_fixed^{-1} · T_L^{-1} · T_R)^∨ = 0，k = 6 维
  约束流形维度：14 - 6 = 8
  δ = 0.3 rad（扩展步长）
  ε = 1e-4（约束容差）
  max_project_iter = 50

初始状态：
  q_start = [0, -0.785, 0, -2.356, 0, 1.571, 0.785,   (左臂)
             0,  0.785, 0, -2.356, 0, 1.571, 0.785]    (右臂)
  ‖h(q_start)‖ = 2.3e-15 (在流形上) ✓

  q_goal = (搬运到桌 B 后的构型)
  ‖h(q_goal)‖ = 1.8e-15 (在流形上) ✓

--- 迭代 1 ---
  T_a = {q_start}, T_b = {q_goal}

  1. SampleAmbient() → q_rand = [0.12, -0.53, ...] (14D 随机)
  2. NearestNeighbor(T_a, q_rand) → q_near = q_start（只有一个节点）
  3. Extend(q_start, q_rand, δ=0.3) → q_new_raw
     ‖q_rand - q_start‖ = 2.87
     方向 = (q_rand - q_start) / 2.87
     q_new_raw = q_start + 0.3 × 方向

  4. Project(q_new_raw):
     迭代 0: ‖h(q)‖ = 0.0342    → Δq = J_h⁺ · h = 0.018 rad
     迭代 1: ‖h(q)‖ = 0.00029   → Δq = 0.00015 rad
     迭代 2: ‖h(q)‖ = 2.1e-08   < ε ✓ → 收敛！(3 步 Newton)
     投影后: q_new ∈ M

  5. EdgeValid(q_start, q_new):
     3 个中间点，全部投影成功且无碰撞 ✓

  6. T_a.Add(q_new) → T_a = {q_start, q_new}

  7. 尝试从 T_b 连接:
     NearestNeighbor(T_b, q_new) → q_goal（距离 3.12 rad）
     Extend 一步 → Project 成功 → EdgeValid ✓ → 添加到 T_b
     ‖q_connect - q_new‖ = 2.82 > ε → 继续
     Extend 第二步 → Project 失败（初始点离流形太远）→ break

  8. Swap(T_a, T_b)

--- 迭代 2-4 ---
  (类似过程，两树交替扩展，各增加 2-3 个节点)

  关键事件（迭代 3）：
    投影失败！q_rand 落在高曲率区域（靠近协调奇异）
    J_h 条件数 = 2.8×10⁴ → Newton 步过大 → 50 次迭代未收敛
    → 丢弃此采样，继续下一次迭代

--- 迭代 5 ---
  T_a 有 6 个节点，T_b 有 4 个节点
  q_rand 恰好落在两树最近节点附近
  连接尝试：
    Extend → Project(2步) → EdgeValid ✓ 
    Extend → Project(2步) → EdgeValid ✓
    Extend → ‖q_connect - q_new‖ = 0.08 < δ → 再来一步
    → ‖q_connect - q_new‖ = 3.2e-05 < ε
    → 两树连接成功！

  ExtractPath: 10 个节点的路径
  路径简化后: 6 个节点
  约束验证: 所有中间点 ‖h(q)‖ < 1e-7 ✓
  碰撞验证: 全部无碰撞 ✓

  总采样次数: 47（含丢弃的失败投影 12 次）
  投影成功率: 35/47 = 74%
  总计算时间: 0.23 秒

从 worked example 中提取的关键教训：

Newton 投影通常 2-3 步收敛——但在高曲率区域可能失败。失败率约 25%，这是投影法效率低于 Atlas 法的主要原因
双向搜索大幅加速连接——单向 RRT 在此问题上平均需要 300+ 次采样，BiRRT 只需 ~50 次
目标偏向 (\(p_{\text{goal}} = 0.05\)) 的作用——迭代 5 中 q_rand 落在两树附近不是巧合，而是 5% 的概率直接采样 q_goal 附近

性能优化技巧¶

技巧	效果	实现代价
解析 Jacobian（Pinocchio）	Newton 投影快 5-10x	低——直接调用 API
目标偏向采样（\(p_{\text{goal}} = 0.05\)）	收敛快 2-5x	极低——一行代码
惰性碰撞检查（先检查节点再检查边）	减少 30-50% 碰撞检查	低
多分辨率边检查（先粗后细）	减少 40-60% 边检查	中
KD-tree 最近邻（vs 线性搜索）	最近邻快 100x（大树时）	中——OMPL 内置

⚠️ 常见陷阱¶

⚠️ 编程陷阱：CBiRRT 路径质量差（多余弯绕）
   现象：路径可行但不必要地绕远
   根本原因：RRT 类算法不保证最优性——路径质量取决于采样运气
   正确做法：规划后做路径简化（shortcutting）
     for trial = 1 to max_shortcut:
       i, j = random_pair(path)  // 随机选两个路径点
       if EdgeValid(path[i], path[j]):
         path.remove(i+1, ..., j-1)  // 删除中间点
   路径简化通常将路径长度减少 30-60%

💡 概念误区：认为"CBiRRT 是最优的"
   新手想法："CBiRRT 找到的路径就是最短的"
   实际上：CBiRRT 只保证概率完备性（如果解存在，给足够时间一定能找到），
          但不保证渐近最优性。对于更优路径，可以用 Informed RRT* 或
          BIT* 的约束版本——但收敛到最优需要更多采样时间。

练习¶

[实现] 在 OMPL 中用 ProjectedStateSpace + RRTConnect 实现双 Franka 共持 30cm 刚性杆的规划。测量使用解析 Jacobian（Pinocchio）vs 数值差分的规划时间差。
[优化] 实现约束感知的路径简化（shortcutting）：随机选取路径上两点，用约束感知 EdgeValid 尝试直连。统计 100 次 shortcut 后路径长度的缩减比例。

D2.5 TAMP——双臂的任务与运动联合规划 ⭐⭐⭐¶

动机：运动规划之上的任务级决策¶

D2.2-D2.4 的约束运动规划假设"抓取位姿已知"——即左手在哪里抓、右手在哪里抓、物体放在哪里，这些都是预先确定的。但在复杂双臂任务中，这些恰恰是需要自动决策的：

任务：将大箱子从桌 A 搬到桌 B（中间有障碍物）

需要决策的问题：
  1. 哪只手抓哪里？（抓取位姿选择）
  2. 需要换手吗？（regrasp 决策）
  3. 能否绕过障碍？还是需要先移开障碍？（任务顺序）
  4. 是双臂同时搬还是一臂递给另一臂？（策略选择）

这些是"任务级"决策，运动规划器无法回答——
需要 Task and Motion Planning (TAMP)

本质洞察：TAMP 的核心难点不是"任务规划难"或"运动规划难"——它们各自都有成熟的算法。真正的难点在于**两层之间的耦合**：任务规划器认为可行的动作序列（如"先放下 A 再拿起 B"），在运动层面可能不可行（"放下 A 的唯一可达位置恰好阻挡了拿起 B 的路径"）。反过来，运动规划器的约束（如碰撞、关节限位）又反向影响了任务层的可行动作集合。TAMP 的本质是在离散（任务）和连续（运动）两个空间之间寻找一致的解——这在数学上类似混合整数规划，是计算复杂度的根源。

TAMP 的双层结构 ⭐⭐⭐¶

┌─────────────────────────────────┐
│     符号层（Task Planning）       │
│  PDDL / 逻辑推理                │
│  决策：动作序列 + 抓取选择        │
│  输出：pick_L(obj, grasp_1)      │
│        handover(L→R)             │
│        place_R(obj, pose_B)      │
└──────────┬──────────────────────┘
           │ 可行性查询
           ↓
┌─────────────────────────────────┐
│    连续层（Motion Planning）     │
│  IK / 碰撞检测 / 路径规划        │
│  回答：这个动作可行吗？           │
│  输出：关节轨迹 或 "不可行"       │
└─────────────────────────────────┘

PDDLStream（Garrett 2020） ⭐⭐⭐¶

PDDLStream 是当前最广泛使用的 TAMP 框架。它的核心创新是将"几何可行性"作为**流（stream）**嵌入符号搜索——符号层不需要知道几何细节，只需知道"是否存在满足条件的连续参数"。

符号层（PDDL 动作定义）：
  (:action pick_left
   :parameters (?obj ?pose ?grasp ?q)
   :precondition (and (AtPose ?obj ?pose)
                      (HandEmpty left)
                      (Kin ?obj ?pose ?grasp ?q))
   :effect (and (Holding left ?obj ?grasp)
                (not (AtPose ?obj ?pose))
                (not (HandEmpty left))))

  (:action place_right
   :parameters (?obj ?pose ?grasp ?q)
   :precondition (and (Holding right ?obj ?grasp)
                      (Kin ?obj ?pose ?grasp ?q))
   :effect (and (AtPose ?obj ?pose)
                (HandEmpty right)
                (not (Holding right ?obj ?grasp))))

  (:action handover
   :parameters (?obj ?grasp_L ?grasp_R ?q_L ?q_R)
   :precondition (and (Holding left ?obj ?grasp_L)
                      (HandEmpty right)
                      (DualKin ?obj ?grasp_L ?grasp_R ?q_L ?q_R))
   :effect (and (Holding right ?obj ?grasp_R)
                (HandEmpty left)
                (not (Holding left ?obj ?grasp_L))))

流层（Stream 定义）：
  (:stream inverse-kinematics
   :inputs (?obj ?pose ?grasp)
   :domain (and (Grasp ?obj ?grasp) (Pose ?obj ?pose))
   :outputs (?q)
   :certified (Kin ?obj ?pose ?grasp ?q))

  (:stream sample-grasp
   :inputs (?obj)
   :domain (Graspable ?obj)
   :outputs (?grasp)
   :certified (Grasp ?obj ?grasp))

  (:stream motion-plan
   :inputs (?q_start ?q_goal)
   :domain (and (Conf ?q_start) (Conf ?q_goal))
   :outputs (?traj)
   :certified (Motion ?q_start ?q_goal ?traj))

Stream 的工作流：符号搜索器发现需要 (Kin box1 pose_A grasp_top ?q) → 调用 inverse-kinematics stream → stream 调用 IK 求解器 → 返回 q = [0.1, -0.5, ...] 或 "无解" → 符号搜索器根据结果继续或回溯。

双臂 TAMP 的特殊挑战¶

挑战 1：分支因子爆炸

单臂 TAMP：每个 pick 只有 1 只手可选
双臂 TAMP：每个 pick 有 3 种选择（左手/右手/双手）

  设每个物体有 G 个可行抓取，P 个放置位姿
  单臂分支因子：G × P
  双臂分支因子：3 × G × G × P  (两手 + 对称性)

  3 步任务的搜索空间：
    单臂：(GP)³
    双臂：(3G²P)³ = 27 G⁶ P³  ← 指数级更大

  剪枝启发式：
    - 物体尺寸 > 夹爪开口 → 排除单臂
    - 物体质量 > 单臂 payload × 0.8 → 排除单臂
    - 几何不可达 → 排除该手

挑战 2：Regrasp 与 Handover

Regrasp Graph：
  节点：(left_grasp, right_grasp, object_pose)
  边类型：
    ① transit: 一臂移动到新位姿（不持物）
    ② transfer: 一臂持物移动（另一臂跟随或独立）
    ③ regrasp: 放到桌上→重新抓取（通过桌面中转）
    ④ handover: 空中交接（双手同时接触物体）

  搜索方法：BFS/A* 在 regrasp graph 上
  每条边需 motion planning 验证可行性

  handover 的四阶段（力控细节见 D03）：
    1. Approach: 接收臂移到交接姿态
    2. Co-grasp: 双臂同持 (0.3-0.8s)
    3. Release: 给予臂力指数衰减 F(t) = F_0 · e^{-t/τ}, τ≈0.5s
    4. Retreat: 给予臂回撤到零位

LGP（Toussaint 2015）——优化视角的 TAMP¶

与 PDDLStream 的搜索方法不同，Toussaint 的 Logic-Geometric Programming 从**优化**角度解决 TAMP：

min_{骨架 σ, 轨迹 x(t)} Σ_k cost_k(x_k)
s.t. 每个 mode k 内动力学可行
     mode 切换满足预/后条件（如 grasp/release）
     碰撞约束
     闭链约束（双臂共持阶段）

骨架 σ = 符号动作序列 (pick_L, transfer, handover, place_R, ...)
轨迹 x(t) = 每个 mode 内的连续运动

优势：轨迹质量更高（优化 vs 搜索），自然处理连续代价
劣势：非凸优化可能陷入局部最优；组合爆炸更严重

维度	PDDLStream	LGP
核心方法	符号搜索 + 采样器	混合整数 NLP
路径质量	可行但非最优	局部最优
可扩展性	好（增量式搜索）	受限（NLP 规模）
实现难度	中（Python）	高（NLP 求解器）
典型求解时间	1-30 秒	10-300 秒
推荐场景	多物体/长序列	少步骤/质量优先

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"TAMP 只在复杂长序列任务中需要"
   新手想法："简单的搬运任务不需要 TAMP，直接规划就行"
   实际上：即使是"双臂搬箱子"这样看似简单的任务，
          TAMP 框架也有价值——它自动处理：
          (1) 抓取位姿选择（哪个抓取点无碰撞？）
          (2) 路径可行性（是否需要绕过障碍？）
          (3) 双臂分配（双臂搬还是单臂搬？）
          没有 TAMP，这些决策需要手动硬编码——脆弱且不通用。
   正确思维：TAMP 是通用框架，简单任务也受益于其自动化决策能力。

练习¶

[设计] 用 PDDLStream 的 PDDL 语法为以下场景定义动作和流：桌上有两个物体（一大一小），大物体需双臂搬到另一桌，小物体单臂搬到垃圾箱。定义 pick_left, pick_right, pick_both, place, handover 动作。
[思考] TAMP 的 mode 数量随任务步数指数增长。对于 5 步双臂任务，每步有 10 种抓取选择。如何设计剪枝启发式将搜索空间降低到可控范围？

D2.6 碰撞检测——臂间碰撞与自碰撞 ⭐⭐¶

动机：双臂碰撞的三重威胁¶

单臂碰撞检测考虑两类碰撞：自碰撞和环境碰撞。双臂增加了第三类：臂间碰撞。

碰撞类型及检查对象：

  ① 自碰撞（Self-collision）：
     左臂: C(7,2) - 相邻 = 21 - 6 = 15 对
     右臂: 同上 15 对
     总: 30 对

  ② 臂间碰撞（Inter-arm collision）：
     link_L1-link_R1, ..., link_L7-link_R7
     总: 7 × 7 = 49 对

  ③ 环境碰撞（Environment collision）：
     14 × N_obstacles 对

  总检查对数: 30 + 49 + 14N = 79 + 14N
  对比单臂: 15 + 7N → 双臂约 2.5-3 倍工作量

Pinocchio + Coal 碰撞管线¶

// ============================================================
// 双臂碰撞检测——Pinocchio + Coal (hpp-fcl) 实现
// ============================================================

#include <pinocchio/algorithm/geometry.hpp>

// 1. 加载碰撞几何
pinocchio::GeometryModel geom_model;
pinocchio::urdf::buildGeom(model, "dual_panda.urdf",
    pinocchio::COLLISION, geom_model);
pinocchio::GeometryData geom_data(geom_model);

// 2. 添加臂间碰撞对（Pinocchio 默认不添加跨子树的碰撞对）
for (auto left_id : left_arm_geom_ids) {
    for (auto right_id : right_arm_geom_ids) {
        geom_model.addCollisionPair(
            pinocchio::CollisionPair(left_id, right_id));
    }
}

// 3. 碰撞检查（快速——遇到第一个碰撞就返回）
bool has_collision = pinocchio::computeCollisions(
    model, data, geom_model, geom_data, q,
    true /* stopAtFirstCollision */);

// 4. 最近距离计算（用于碰撞回避梯度）
pinocchio::computeDistances(model, data, geom_model, geom_data, q);
double d_min = 1e10;
for (const auto& result : geom_data.distanceResults) {
    d_min = std::min(d_min, result.min_distance);
}
// d_min < 0 表示穿透；d_min < safety_margin 表示危险

MoveIt2 ACM 配置¶

# ============================================================
# SRDF 中的 Allowed Collision Matrix (ACM)
# 禁用永远不可能碰撞的对——减少检查量
# ============================================================

<!-- 同一臂相邻链接——机械上不可能碰撞 -->
<disable_collisions link1="panda_left_link0" link2="panda_left_link1" reason="Adjacent"/>

<!-- 两臂底座——距离太远永远不碰 -->
<disable_collisions link1="panda_left_link0" link2="panda_right_link0" reason="Never"/>

<!-- 注意：以下臂间碰撞对必须启用！ -->
<!-- panda_left_link4-7 vs panda_right_link4-7 -->
<!-- 这些末端段在双臂靠近时可能碰撞 -->

⚠️ 常见陷阱¶

⚠️ 编程陷阱：MoveIt Setup Assistant 错误禁用臂间碰撞
   现象：规划路径在执行时两臂末端碰撞
   根本原因：Setup Assistant 基于随机采样构建 ACM——如果采样未覆盖
            两臂交叉的构型，可能错误地将臂间碰撞标记为 "Never"
   正确做法：手动检查 ACM，确保所有 link_L{4-7} vs link_R{4-7}
            的碰撞对都启用（enabled）
   自检方法：在 RViz 中手动把两臂移到交叉位置，
            检查碰撞检测是否报警

练习¶

[计算] 对于双 Franka Panda + 10 个桌面物体，碰撞检测需要检查 79 + 14\(\times\)10 = 219 对。如果每对 GJK 检查耗时 1 微秒，1kHz 控制循环中碰撞检测占 0.22ms。这是否可接受？如果障碍物增加到 100 个呢？
[编程] 用 Pinocchio 碰撞管线随机采样 10000 个双 Franka 构型，统计臂间碰撞率。尝试不同的两臂底座间距（30cm、50cm、80cm），画碰撞率曲线。

D2.7 时间协调与轨迹同步 ⭐⭐¶

动机：路径有了，时间呢？¶

约束规划输出的是**几何路径**——没有时间信息。执行需要时间参数化。对于双臂，时间参数化不只是"速度平滑"的问题——还需要确保两臂同步到达关键构型。

同步策略对比 ⭐⭐¶

策略	方法	优势	劣势	适用
全局同步	\(s_L(t) = s_R(t) = s(t)\)	天然同步	慢臂拖慢快臂	紧耦合
关键点同步	在抓取/释放点强制同步	灵活	同步点可能停顿	松耦合
时变约束	\(h(q, t) = 0\) 随时间变化	无停顿	约束更复杂	高级

回顾 M10（时间参数化）：TOPP-RA 算法可以在满足速度/加速度/力矩约束的前提下找到时间最优的参数化。对于双臂，需要在 TOPP-RA 中同时施加两臂 14 个关节的约束。

同步/异步策略 Benchmark¶

以下是在 MuJoCo 双 Franka 仿真中，对"共持 30cm 杆从桌 A 搬到桌 B"任务的三种时间协调策略对比结果（100 次随机起点/终点，约束流形上的路径由 CBiRRT 生成）：

指标	全局同步	关键点同步	时变约束
平均执行时间 (s)	4.2 \(\pm\) 0.8	3.1 \(\pm\) 0.6	2.8 \(\pm\) 0.5
最大约束违反 (mm)	0.02 \(\pm\) 0.01	0.15 \(\pm\) 0.08	0.05 \(\pm\) 0.02
最大内力峰值 (N)	2.1 \(\pm\) 0.5	8.3 \(\pm\) 3.1	3.4 \(\pm\) 0.9
关节速度峰值 (rad/s)	1.2 \(\pm\) 0.2	1.8 \(\pm\) 0.3	1.5 \(\pm\) 0.3
停顿次数	0	2.3 \(\pm\) 0.8	0
实现复杂度	低	中	高
适用紧耦合度	⭐⭐⭐⭐⭐	⭐⭐⭐	⭐⭐⭐⭐

关键发现：

全局同步最安全但最慢——两臂共用一个 \(s(t)\)，约束违反极小，但快臂被慢臂拖慢
关键点同步最快但内力大——两臂在中间独立执行时可能出现瞬间不同步，内力峰值是全局同步的 4 倍
时变约束最平衡——但实现需要在 TOPP-RA 中嵌入约束检查回调，代码量增大 3 倍

工程建议：紧耦合刚性物体首选全局同步（安全第一）；柔性物体或松耦合任务可用关键点同步（效率优先）；对时间敏感的工业场景考虑时变约束。

双臂 DDP/轨迹优化完整设置¶

约束采样规划（CBiRRT）输出的路径通常不光滑且非时间最优。DDP（Differential Dynamic Programming）或 Crocoddyl 可以在约束流形上做局部轨迹优化：

# ============================================================
# 双臂 DDP 轨迹优化——Crocoddyl 设置框架
# 输入：CBiRRT 粗路径 → 输出：动力学可行的光滑轨迹
# ============================================================
import crocoddyl
import pinocchio as pin
import numpy as np

# 加载模型
model = pin.buildModelFromUrdf("dual_panda.urdf")
state = crocoddyl.StateMultibody(model)
actuation = crocoddyl.ActuationModelFull(state)

# 代价模型：每个时间步
running_costs = crocoddyl.CostModelSum(state)

# 代价 1：末端绝对位姿跟踪（由同一个物体参考派生左右末端参考）
frame_id_L = model.getFrameId("panda_left_hand")
frame_id_R = model.getFrameId("panda_right_hand")

T_obj_ref = pin.SE3.Identity()   # 由 CBiRRT 虚拟物体路径插值给出
T_grasp_L = pin.SE3.Identity()   # object -> left hand 的固定抓取变换
T_grasp_R = pin.SE3.Identity()   # object -> right hand 的固定抓取变换
x_ref_L = T_obj_ref * T_grasp_L
x_ref_R = T_obj_ref * T_grasp_R
cost_abs_L = crocoddyl.CostModelResidual(
    state,
    crocoddyl.ResidualModelFramePlacement(state, frame_id_L, x_ref_L))
cost_abs_R = crocoddyl.CostModelResidual(
    state,
    crocoddyl.ResidualModelFramePlacement(state, frame_id_R, x_ref_R))
running_costs.addCost("abs_L", cost_abs_L, 100.0)
running_costs.addCost("abs_R", cost_abs_R, 100.0)

# 代价 2：关节速度正则化（光滑性）
# ResidualModelState 默认会惩罚完整状态误差；这里把配置误差权重置零，
# 只保留速度分量。若 Crocoddyl 版本提供专用速度残差，也可直接替换为速度残差。
x_zero_vel = state.zero()
vel_weights = np.r_[np.zeros(model.nv), np.ones(model.nv)]
cost_vel = crocoddyl.CostModelResidual(
    state,
    crocoddyl.ActivationModelWeightedQuad(vel_weights),
    crocoddyl.ResidualModelState(state, x_zero_vel, actuation.nu))
running_costs.addCost("vel_reg", cost_vel, 0.01)

# 代价 3：控制力矩正则化（能效）
cost_ctrl = crocoddyl.CostModelResidual(
    state,
    crocoddyl.ResidualModelControl(state, actuation.nu))
running_costs.addCost("ctrl_reg", cost_ctrl, 0.001)

# 动力学约束作为 DAM
dam = crocoddyl.DifferentialActionModelFreeFwdDynamics(
    state, actuation, running_costs)

# 离散化
dt = 0.01  # 10 ms
T = 200    # 2 秒轨迹，200 个 knot 点
model_discrete = crocoddyl.IntegratedActionModelEuler(dam, dt)

# 终端代价（目标构型）
terminal_costs = crocoddyl.CostModelSum(state)
cost_goal = crocoddyl.CostModelResidual(
    state,
    crocoddyl.ResidualModelState(state, x_goal, actuation.nu))
terminal_costs.addCost("goal", cost_goal, 1000.0)
terminal_dam = crocoddyl.DifferentialActionModelFreeFwdDynamics(
    state, actuation, terminal_costs)
terminal_model = crocoddyl.IntegratedActionModelEuler(terminal_dam, 0.0)

# 组装 shooting problem
problem = crocoddyl.ShootingProblem(
    x0, [model_discrete] * T, terminal_model)

# DDP 求解
solver = crocoddyl.SolverDDP(problem)
solver.setCallbacks([crocoddyl.CallbackVerbose()])

# 用 CBiRRT 粗路径初始化（关键！否则 DDP 可能收敛到局部最优）
xs_init = interpolate_cbirrt_path(cbirrt_path, T + 1)
us_init = [np.zeros(actuation.nu)] * T
solver.solve(xs_init, us_init, maxiter=100)

注意：上面的 SolverDDP 教学示例没有把闭链约束作为硬约束求解，只是通过残差代价把 \(h(q)=0\) 软化。当前 Crocoddyl 已提供 ConstraintModelManager / ConstraintModelResidual 等约束建模接口，也有面向等式/不等式约束的求解器路线（如 SQP/augmented Lagrangian 家族）；工程上应按目标版本选择：

约束代价化：将 \(\|h(q)\|^2\) 作为高权重代价项——实现简单，但约束不保证精确满足
显式约束求解：用 Crocoddyl 的约束模型和支持约束的求解器处理 \(h(q)=0\)——更接近真实闭链需求
外层投影：在粗路径或前向 rollout 后投影回约束流形——适合作为初始化/修正步骤，而不是把 SolverDDP 本身说成硬约束求解器

⚠️ 常见陷阱¶

💡 概念误区：认为"路径相同意味着时间参数化也相同"
   新手想法："两臂走同样的路径，所以到达时间一样"
   实际上：同样的几何路径长度可能对应不同的关节空间路径长度
          （取决于各关节的移动量）。如果左臂关节 3 需要转 120°
          而右臂关节 3 只需转 30°，全局同步下左臂是瓶颈。
   正确做法：用 TOPP-RA 对联合 14D 路径做时间最优参数化，
            自动处理不同关节的约束差异。

练习¶

[实现] 对 CBiRRT 输出的双臂路径，用 TOPP-RA 做时间最优参数化。约束：每关节速度 < 2 rad/s，加速度 < 5 rad/s^2。

D2.8 MoveIt2 双臂配置实战 ⭐¶

Allowed Collision Matrix (ACM) 配置——双臂特有考虑 ⭐¶

双臂系统的 ACM 配置比单臂复杂得多。需要处理三类碰撞对：

碰撞对分类：
  Type A：同臂自碰撞（如 left_link3 vs left_link7）
    → 与单臂相同，在 SRDF 中 disable 始终无碰撞的对

  Type B：臂间碰撞（如 left_link5 vs right_link5）
    → 双臂独有！大多数臂间链接对需要启用碰撞检查
    → 仅 disable 物理上不可能碰撞的对（如 left_link0 vs right_link7）

  Type C：臂-环境碰撞（如 left_link4 vs table）
    → 与单臂相同

  实践建议：
    先用 MoveIt Setup Assistant 自动生成 ACM
    然后手动检查 Type B 对——自动生成可能过度 disable 臂间对

SRDF 配置 ⭐¶

<!-- dual_panda.srdf -->
<robot name="dual_panda">
  <!-- 单臂规划组 -->
  <group name="left_arm">
    <chain base_link="panda_left_link0" tip_link="panda_left_link8"/>
  </group>
  <group name="right_arm">
    <chain base_link="panda_right_link0" tip_link="panda_right_link8"/>
  </group>

  <!-- 双臂联合规划组——关键配置 -->
  <group name="both_arms">
    <group name="left_arm"/>
    <group name="right_arm"/>
  </group>

  <!-- 末端执行器 -->
  <end_effector name="left_hand" parent_link="panda_left_link8"
                group="left_gripper"/>
  <end_effector name="right_hand" parent_link="panda_right_link8"
                group="right_gripper"/>
</robot>

MoveIt2 C++ 双臂规划¶

#include <moveit/move_group_interface/move_group_interface.h>

// 创建双臂规划接口
auto both_arms = moveit::planning_interface::MoveGroupInterface(
    node, "both_arms");
both_arms.setPlanningTime(30.0);  // 14D 空间需要更多时间

// 设置两臂目标
geometry_msgs::msg::Pose left_target, right_target;
/* ... 设置目标位姿 ... */
both_arms.setPoseTarget(left_target, "panda_left_link8");
both_arms.setPoseTarget(right_target, "panda_right_link8");

// 添加路径约束（搬运途中保持箱子水平）
moveit_msgs::msg::OrientationConstraint oc;
oc.header.frame_id = "world";
oc.link_name = "panda_left_link8";
oc.orientation.w = 1.0;  // 水平
oc.absolute_x_axis_tolerance = 0.05;  // roll ±3°
oc.absolute_y_axis_tolerance = 0.05;  // pitch ±3°
oc.absolute_z_axis_tolerance = M_PI;  // yaw 自由
oc.weight = 1.0;

moveit_msgs::msg::Constraints constraints;
constraints.orientation_constraints.push_back(oc);
both_arms.setPathConstraints(constraints);

// 规划并执行
moveit::planning_interface::MoveGroupInterface::Plan plan;
auto code = both_arms.plan(plan);
if (code == moveit::core::MoveItErrorCode::SUCCESS) {
    both_arms.execute(plan);
}

MoveIt2 双臂 Planning Group 高级配置¶

多规划器分配：不同任务可以为同一 both_arms 组分配不同规划器，并设置自定义参数：

# ompl_planning.yaml — 高级双臂配置
both_arms:
  default_planner_config: RRTConnect
  planner_configs:
    - RRTConnect:
        range: 0.5          # 14D 需要较大步长
        type: geometric::RRTConnect
    - RRTstar:
        range: 0.3
        goal_bias: 0.1      # 14D 需要更强目标偏向
        type: geometric::RRTstar
    - BITstar:
        samples_per_batch: 200
        use_k_nearest: true
        type: geometric::BITstar

  # 约束规划特有参数
  projection_evaluator: joints(panda_left_joint1, panda_left_joint2,
                                panda_right_joint1, panda_right_joint2)
  longest_valid_segment_fraction: 0.005  # 0.5%——比单臂(1%)更严格

  # 超时设置（14D 约束规划需要更多时间）
  planning_time: 30.0

  # 闭链约束规划启用
  enforce_constrained_state_space: true
  constraint_approximations_path: /path/to/constraint_db/

约束近似数据库（预计算加速）：

MoveIt2 支持预计算约束近似（constraint approximation），对常见约束预先采样流形上的构型存入数据库，规划时从数据库中查询而非在线投影。对于双臂反复执行相同类型搬运任务，这可以将规划时间减少 5-10 倍：

// 生成约束近似数据库（离线执行一次）
auto constraint_sampler = moveit::core::constraint_samplers::
    ConstraintSamplerManager::selectDefaultSampler(
        planning_scene, "both_arms", constraints);

// 采样 10000 个约束满足的构型
for (int i = 0; i < 10000; i++) {
    moveit::core::RobotState state(robot_model);
    if (constraint_sampler->sample(state, 100)) {
        db.addState(state);
    }
}
db.saveToDisk("/path/to/constraint_db/dual_arm_level.db");

约束路径平滑 ⭐⭐¶

RRT/CBiRRT 输出的路径通常包含大量锯齿——这是采样规划的固有问题。路径平滑对于双臂系统尤为重要，因为锯齿路径中的急转弯会导致关节加速度峰值，进而引发共持物体上的振动和内力波动。

约束感知的短路径平滑（Constraint-Aware Shortcutting）：

算法：
  重复 N_iterations 次：
    1. 在路径上随机选两个点 q_a, q_b
    2. 尝试约束感知插值连接 q_a → q_b
    3. 如果成功且新子路径更短 → 替换原子路径

关键：第 2 步的插值必须用约束感知插值
      （直线插值 + Newton 投影），不能用普通直线

B-spline 约束平滑：

对路径做 B-spline 拟合后，control point 不在约束流形上。需要交替执行： 1. B-spline 拟合（优化光滑性） 2. 将 control point 投影回流形（保证约束满足） 3. 重复直到收敛

这种交替投影方法在双臂搬运任务中可以将路径长度减少 20-40%，同时将最大关节加速度降低 50-70%。

闭链约束规划的数值稳定性分析¶

Newton 投影是约束规划的核心子程序——其数值稳定性直接决定规划的成功率。以下分析三种数值问题及解决方案。

问题 1：Jacobian 病态

当约束 Jacobian \(J_h \in \mathbb{R}^{6 \times 14}\) 的条件数 \(\kappa(J_h) = \sigma_{\max} / \sigma_{\min}\) 很大时，Newton 步 \(\Delta q = J_h^+ h(q)\) 的数值误差被放大 \(\kappa\) 倍。对双臂闭链，\(J_h\) 通常由同点相对 Jacobian \(J_{\text{rel}}=\bar{J}_R-\bar{J}_L\) 近似得到；如果直接使用未平移参考点的 \(J_R-J_L\)，条件数和投影方向都会失真。

数值实验（双 Franka，1000 个随机构型）：
  κ(J_h) 分布：
    P(κ < 10)      = 62%  → Newton 2-3 步收敛
    P(10 < κ < 100) = 28%  → Newton 3-5 步收敛
    P(κ > 100)      = 8%   → 需要阻尼伪逆
    P(κ > 1000)     = 2%   → 投影可能失败

  解决方案：自适应正则化
    λ(κ) = max(λ_min, λ_0 · (κ / κ_thresh - 1))
    当 κ < κ_thresh: λ = λ_min（不正则化，保持精度）
    当 κ > κ_thresh: λ 线性增长（牺牲精度换稳定性）
    推荐：κ_thresh = 50, λ_0 = 0.01, λ_min = 1e-6

问题 2：步长过大导致越过流形

当 \(\|h(q)\|\) 较大时，Newton 步 \(\Delta q\) 也大——系统可能"越过"约束流形跳到另一侧，导致振荡不收敛。

  解决方案：带步长限制的 Newton
    if ‖Δq‖ > Δq_max:
        Δq ← Δq_max · Δq / ‖Δq‖  （截断到最大步长）
    推荐：Δq_max = 0.1 rad（约 6°）

  OMPL 实现：ProjectedStateSpace 的 delta 参数控制此限制

问题 3：log 映射的分支切割

约束函数中的 \(\log: SE(3) \to \mathfrak{se}(3)\) 在旋转角 \(\theta = \pi\) 时不连续（\(\pi\) 和 \(-\pi\) 跳变）。当物体旋转接近 180 度时，约束函数的梯度不连续，Newton 投影可能发散。

  检测：if ‖ω‖ > 0.9π (ω 是 log map 的旋转部分):
          print WARNING: 接近 log 分支切割

  解决方案：
    方案 A：使用四元数差 Δq_quat = q_1^{-1} ⊗ q_2 代替 log map
            四元数差在 θ = π 处连续（但在 θ = 2π 处不连续）
    方案 B：将路径分段，确保每段旋转角 < 150°
    方案 C：使用 Pinocchio 的 difference() 函数——
            它内部处理了分支切割

⚠️ 常见陷阱¶

⚠️ 编程陷阱：MoveIt2 双臂规划超时
   现象：plan() 返回失败或超时
   排查步骤：
     1. 增大规划时间：both_arms.setPlanningTime(60.0)
     2. 去掉路径约束，确认目标本身可达
     3. 用 RViz 可视化目标位姿，检查两臂是否碰撞
     4. 检查 SRDF 的 ACM 是否正确
     5. 尝试换规划器：both_arms.setPlannerId("RRTstar")

练习¶

[配置] 搭建双 Franka Panda 的 MoveIt2 配置包（moveit_setup_assistant）。配置 both_arms 组，在 RViz 中验证联合规划。
[对比] 比较 MoveIt2 默认规划器和自定义 OMPL 约束规划器在双臂搬运任务上的规划时间和路径质量。

D2.9 前沿展望——TAMP for Bimanual 与 Learning-based Motion Planning ⭐⭐⭐⭐¶

TAMP for Bimanual 的最新进展¶

D2.5 介绍了 PDDLStream 和 LGP 两种经典 TAMP 框架。近年来，TAMP 在双臂场景中的若干瓶颈正在被新方法突破：

LLM-guided TAMP：大语言模型（LLM）被用作 TAMP 的任务层"启发式搜索引擎"。Ding et al. 2024 (CoRL) 展示了用 GPT-4 生成 PDDL 动作序列的初始猜测，然后由运动规划器验证和修正。对于双臂任务，LLM 擅长处理自然语言描述的任务目标（如"把红色的杯子递给我，用左手拿住蓝色的碗"），将其转化为 TAMP 的形式化目标。但 LLM 对运动层约束（碰撞、关节限位、闭链约束）的理解仍然不足，容易生成运动不可行的方案。当前的工程实践是将 LLM 作为任务层的快速原型生成器，再用传统 TAMP 做可行性验证。

GCS-based Bimanual Planning：Cohn et al. 2024 (ICRA) 提出了基于图上凸集（Graphs of Convex Sets, GCS）的双臂规划方法。核心思想是将约束 C-space 分解为凸区域的图结构，在图上用混合整数凸优化搜索最优路径。与 CBiRRT 的随机搜索相比，GCS 提供了全局最优性保证（在凸分解的精度范围内），特别适合需要最优路径质量的装配任务。

GCS 的核心创新在于将约束流形的几何结构显式编码为图的拓扑结构——每个凸集对应流形的一个局部凸区域，图的边对应区域之间的可达关系。这使得路径搜索从随机采样转变为确定性优化，代价是需要预先计算凸分解（离线开销较大，但对重复任务可摊销）。

Analytic IK 加速：Cohn 等人还利用了 7-DOF 机械臂的**解析逆运动学**（analytic IK）来避免 Newton 投影。对于双 7-DOF 臂，每个末端位姿对应最多 16 个解析 IK 解（取决于肘关节角参数）。通过枚举 IK 解而非 Newton 投影来生成约束满足的构型，完全消除了投影失败的问题。实验表明，这种方法在双 Franka 搬运任务中将规划时间从 CBiRRT 的 \(\sim 10\text{s}\) 降低到 \(\sim 0.5\text{s}\)。

序列流形规划——多模式双臂任务 ⭐⭐⭐¶

许多真实的双臂任务涉及**约束模式的动态切换**（如 D01 中讨论的 handover 从解耦到紧耦合再到解耦）。Kingston 等人 2021 年在 RSS 上提出了**序列流形规划**（Sampling-Based Motion Planning on Sequenced Manifolds），将约束模式切换形式化为流形序列上的路径搜索。

传统方法：为每个模式独立规划，然后拼接
  模式 1 路径（解耦）+ 模式 2 路径（紧耦合）+ 模式 3 路径（解耦）
  问题：模式切换点的约束连续性无法保证

序列流形方法：在流形序列 M_1 → M_2 → M_3 上统一规划
  M_1 = R^14（无约束，解耦模式）
  M_2 = {q : h(q)=0}（闭链约束，紧耦合模式）
  M_3 = R^14（无约束，解耦模式）

  流形之间的切换条件：
    M_1 → M_2：两末端距离 < d_contact（接触建立）
    M_2 → M_3：内力 < f_release（释放条件）

  搜索在整个流形序列上统一进行，
  自动发现最优的切换时机和切换构型

这种方法与 D2.5 的 TAMP 互补——TAMP 决定"做什么"（任务级动作序列），序列流形规划解决"怎么做"（每个动作的运动和模式切换的连续性）。

折叠流形规划 ⭐⭐⭐⭐¶

2024 年 RSS 的最新工作（Motion Planning in Foliated Manifolds using Repetition）将约束流形推广到**叶层流形**（foliation）——当约束中包含一个连续参数 \(s\)（如抓取位姿沿物体表面连续变化）时，约束流形不是单一的 8 维流形，而是一族按 \(s\) 参数化的流形叶片。规划器可以在不同叶片之间跳跃（改变抓取位姿）来绕过障碍物——这为双臂的 regrasp 规划提供了优雅的数学框架。

Learning-based Bimanual Motion Planning¶

学习型运动规划是双臂领域的另一个活跃方向：

方法	核心思想	优势	局限
MPNet (Qureshi et al. 2020)	神经网络预测采样分布，引导 RRT	采样效率提升 10-100 倍	需要大量规划示例训练，泛化到新环境有限
CVAE + 约束投影	用条件 VAE 学习约束流形上的采样分布	直接在流形上采样，无需 Newton 投影	约束变化时需重新训练
Diffusion-based Planning	扩散模型生成约束满足的轨迹	多模态轨迹分布，适合双臂的多解性	约束满足精度依赖去噪步数
RL for TAMP	RL 学习任务层动作选择策略	无需手写启发式，可端到端优化	需要大量仿真交互，难以保证安全
序列流形规划	流形序列上的采样规划 (Kingston 2021)	自动管理约束切换的连续规划	需要预定义流形序列
物理信息约束规划	用物理先验引导采样 (Ni et al. 2024)	利用动力学可行性剪枝不可行样本	物理模型精度要求高

反事实推理：如果不用学习方法，双臂约束规划的效率瓶颈能否靠纯算法突破？实际上，约束流形上的采样规划在理论上就面临维度灾难——8 维约束流形在 14 维空间中是零测集，均匀采样的命中率随维度指数下降。学习方法的价值在于用数据驱动的方式"学会"约束流形的几何结构，从而将采样集中到高概率区域。但学习方法的泛化性和安全性保证仍是开放问题——在安全关键场景中，学习方法通常作为经典方法的加速器而非替代品。

经典规划与学习规划的融合策略 ⭐⭐⭐¶

当前最有前景的方向不是用学习完全替代经典规划，而是**学习为经典规划提供热启动**：

融合策略	学习负责	经典负责	优势
采样引导	学习约束流形的概率密度	RRT 搜索结构 + 碰撞检查	采样效率提升，安全保证不变
初始路径	生成粗糙但接近可行的路径	约束投影 + 碰撞修正 + 平滑	减少搜索空间，保持约束精度
启发式	学习 C-space 到目标的距离函数	A* / RRT* 搜索	减少探索浪费，保持最优性
chart 预测	预测 Atlas 的 chart 布局	Atlas 搜索 + Newton 修正	减少 chart 创建开销

本质洞察：学习方法和经典方法在双臂规划中不是竞争关系，而是分别解决问题的不同层面——学习解决"在哪里搜索"（降低搜索空间维度），经典解决"搜索到的结果是否正确"（保证约束满足和碰撞安全）。两者的融合是当前最具实用价值的研究方向。

本章常见误解汇总¶

误解	正确理解
双臂可以分别规划再合并	独立规划忽略闭链约束和臂间碰撞，合并后约束违反可达厘米级
投影法总是能收敛	Newton 投影在远离流形或 \(J_h\) 奇异时可能发散，需要步长控制和最大迭代数限制
Atlas 法一定比投影法快	Atlas 的 chart 维护开销在低维约束或简单流形中可能超过投影法的收益
TSR 只能表达 box 约束	TSR Chain 可以通过级联组合表达复杂的多末端约束
CBiRRT 保证找到最优路径	CBiRRT 是概率完备的但不是最优的，路径质量依赖后续平滑
TAMP 需要完美的领域模型	现代 TAMP（PDDLStream）使用惰性求值，只在需要时调用运动规划验证
约束流形上的距离等于 C-space 欧氏距离	流形上的测地线距离可能远大于欧氏距离（如绕过奇异点）
碰撞检查在约束规划中不变	约束投影可能将无碰撞点投影到碰撞区域，需要投影后再检查
14 维规划比 7 维慢 2 倍	采样规划复杂度随维度指数增长，14 维可能比 7 维慢 100-1000 倍
时间协调只需要速度缩放	关键点同步需要轨迹重参数化，不是简单的全局缩放

本章小结¶

知识点	核心要点	对应小节	难度
双臂规划挑战	14D C-space + 8D 约束流形 + 维度灾难	D2.1	⭐
投影法	ambient 采样 + Newton 投影，简单但低效	D2.2	⭐⭐
Atlas 法	切空间采样 + chart 管理，高效但复杂	D2.2	⭐⭐⭐
TangentBundle	Atlas 的惰性版本，折中选择	D2.2	⭐⭐
TSR	6D bound box 约束表达，工程友好	D2.3	⭐⭐
CBiRRT	约束双向 RRT + 投影，事实标准	D2.4	⭐⭐
TAMP/PDDLStream	符号+连续联合规划，处理任务级决策	D2.5	⭐⭐⭐
臂间碰撞	双臂独有碰撞类型，Coal/FCL 实现	D2.6	⭐⭐
时间协调	全局同步/关键点同步/时变约束	D2.7	⭐⭐
MoveIt2 双臂	SRDF both_arms 组 + 约束规划	D2.8	⭐

累积项目：本章新增模块¶

mini_dualarm/
├── src/
│   ├── dual_arm_constraint.cpp    # OMPL Constraint 子类 (NEW)
│   ├── tsr_constraint.cpp         # TSR → OMPL Constraint 转换 (NEW)
│   ├── constrained_planner.cpp    # Projection/Atlas 规划管线 (NEW)
│   └── collision_manager.cpp      # 臂间碰撞管理 (NEW)
├── config/
│   ├── dual_panda.srdf            # MoveIt2 SRDF (NEW)
│   └── ompl_planning.yaml         # OMPL 规划器参数 (NEW)
└── scripts/
    └── benchmark_planners.py      # 三种方法性能对比脚本 (NEW)

术语速查表¶

中文术语	英文术语	缩写	一句话定义
约束流形	Constraint Manifold	\(\mathcal{M}\)	满足 \(h(q)=0\) 的 C-space 子集，双臂刚性共持时为 8 维
投影法	Projection Method	Proj	ambient 采样 + Newton 投影到流形
图谱法	Atlas Method	Atlas	用切空间 chart 铺设流形的分段线性逼近
切空间束	Tangent Bundle	TB	Atlas 的惰性版本，按需创建 chart
任务空间区域	Task Space Region	TSR	用 6D bound box 表达末端约束的工程化描述
约束双向快速搜索树	Constrained Bi-directional RRT	CBiRRT	在约束流形上双向生长的 RRT，事实标准
任务与运动规划	Task and Motion Planning	TAMP	符号任务规划与连续运动规划的联合框架
规划域定义语言流	PDDLStream	—	Garrett 2020 提出的 TAMP 框架，流式惰性求值
臂间碰撞	Inter-arm Collision	—	双臂系统独有的左右臂之间的碰撞
约束感知插值	Constraint-aware Interpolation	—	沿流形而非直线连接两个构型
图上凸集	Graphs of Convex Sets	GCS	将 C-space 分解为凸区域图的优化规划方法

累积项目：本章新增模块（与上文合并）¶

延伸阅读¶

文献	类型	难度	推荐理由
Kingston, Moll, Kavraki 2018, "Sampling-Based Methods for Motion Planning with Constraints", ARCRAS	综述	⭐⭐⭐	约束规划最权威综述，系统比较 5 类方法
Berenson et al. 2011, "Task Space Regions", IJRR	论文	⭐⭐	TSR + CBiRRT 原始论文，工程化约束规划的奠基之作
Jaillet & Porta 2013, "AtlasRRT", IEEE T-RO	论文	⭐⭐⭐	Atlas 图谱法理论根基
Kingston et al. 2019, "IMACS", IJRR	论文	⭐⭐⭐	OMPL ConstrainedStateSpace 的蓝图
Garrett et al. 2021, "Integrated TAMP", ARCRAS	综述	⭐⭐⭐	TAMP 标准参考综述
Cohn et al. 2024, "Constrained Bimanual Planning with Analytic IK", ICRA	论文	⭐⭐⭐⭐	解析 IK + GCS 凸集图优化，最新进展
Kingston et al. 2021, "Sampling-Based Motion Planning on Sequenced Manifolds", RSS	论文	⭐⭐⭐	多约束模式切换的统一规划框架
Ni et al. 2024, "Physics-informed Neural Motion Planning on Constraint Manifolds", arXiv	论文	⭐⭐⭐⭐	用物理先验引导约束流形上的神经网络采样
Suomalainen et al. 2022, "A Survey of Robot Manipulation in Contact", RAS	综述	⭐⭐	接触约束下的操作规划综述，与双臂力控规划相关

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
Newton 投影发散	初始点离流形太远或 Jacobian 奇异	1. 减小步长 \(\delta\) 2. 增加 maxIterations 3. 检查 \(J_h\) 条件数 4. 用 SVD 伪逆替代正规方程	D2.2
Atlas chart 创建过多	流形曲率高或 \(\rho\) 太小	1. 增大 chart 半径 \(\rho\) 2. 检查是否存在协调奇异 3. 用 Projection 对比基线	D2.2
CBiRRT 长时间无解	狭窄通道或目标不可达	1. 放宽 TSR bound 2. 增大 max_iter 3. 检查 start/goal 在同一连通分支 4. 目标偏向采样	D2.4
路径中间约束违反	edge 检查未做约束投影	1. 开启约束感知插值 2. 减小 edge 检查分辨率 3. 打印中间点 \(\\|h(q)\\|\)	D2.4
MoveIt2 双臂规划超时	14D 高维 + 路径约束紧	1. 增大 planning_time 2. 去掉路径约束测试 3. 检查 ACM 4. 换规划器	D2.8
路径平滑后约束违反	平滑算法未考虑约束	1. 使用约束感知平滑 2. 平滑后重新投影 3. 检查中间点约束值	D2.4
两臂到达终点时间不同步	缺少时间协调层	1. 检查是否使用了时间参数化 2. 添加关键点同步约束 3. 速度缩放到较慢的臂	D2.7

API 速查表¶

API / 函数	库	说明
`ompl::base::ConstrainedStateSpace`	OMPL	约束状态空间基类，子类有 Projected/Atlas/TangentBundle
`ompl::base::ProjectedStateSpace`	OMPL	投影法状态空间，需提供 Constraint 子类
`ompl::base::AtlasStateSpace`	OMPL	Atlas 图谱法状态空间
`ompl::base::Constraint::function()`	OMPL	用户重写：计算约束值 \(h(q)\)
`ompl::base::Constraint::jacobian()`	OMPL	用户重写：计算约束 Jacobian \(J_h(q)\)
`ompl::geometric::CBiRRT2`	OMPL (custom)	约束双向 RRT，需自行实现或从 Berenson 代码移植
`moveit::planning_interface::MoveGroupInterface`	MoveIt2	规划接口，支持 `both_arms` 组
`moveit_msgs::msg::Constraints`	MoveIt2	路径约束消息类型
`pinocchio::computeJointJacobians`	Pinocchio	批量计算所有关节 Jacobian
`coal::CollisionManager`	Coal/HPP-FCL	宽相碰撞管理器，支持多对碰撞检查

研究实践建议¶

层次	建议	适用读者
入门级	在 OMPL 中用 ProjectedStateSpace 实现双 Franka 约束规划，对比有/无约束投影的路径质量	硕一新生
进阶级	实现 Atlas 状态空间并与 Projection 做 benchmark 对比（成功率、规划时间、路径长度）	硕士研究生
研究级	将 Diffusion Policy 生成的轨迹作为 CBiRRT 的初始猜测，评估学习引导的约束规划加速效果	博士研究生
前沿级	实现 GCS-based 双臂规划（Cohn et al. 2024），对比与 CBiRRT 的最优性和计算时间	博士高年级/博后

本章与后续章节的关系¶

后续章节	与 D02 的关系	从 D02 带走的关键知识
D03 双臂协调力控	D02 的路径 \(\to\) D03 的力控轨迹跟踪	约束满足路径作为力控参考轨迹
D09 系统集成	D02 的 OMPL 配置 \(\to\) D09 的 MoveIt2 管线	规划器参数、SRDF 配置、约束消息
D10 综合实战	D02 的所有方法 \(\to\) D10 的端到端系统	从 TAMP 到约束规划到执行的完整管线

版本信息速查¶

库 / 工具	本章使用版本	备注
OMPL	\(\ge\) 1.5.x	`ConstrainedStateSpace` 需要 1.5+，Atlas 需要 1.6+
MoveIt2	\(\ge\) Humble	双臂规划组需要 `both_arms` SRDF 配置
Pinocchio	\(\ge\) 2.6.x	约束 Jacobian 计算
Coal/HPP-FCL	\(\ge\) 2.4.x	`CollisionManager` 支持多对检查
Crocoddyl	\(\ge\) 2.0.x	轨迹优化，支持约束代价和约束模型
TOPP-RA	\(\ge\) 0.6.x	时间最优路径参数化

跨章综合练习¶

[综合 D01+D02] 使用 D01 中的 \(J_{\text{rel}}\) 零空间投影 \(P\) 和本章的 OMPL ProjectedStateSpace，为双 Franka Panda 实现一个完整的约束规划管线：（a）加载合并 URDF；（b）定义闭链约束（OMPL Constraint 子类）；（c）用 RRTConnect + Projection 规划；（d）验证路径上每个点的 \(\|h(q)\| < 10^{-3}\)。
[综合 D01+D02+M07] 对同一搬运任务（双臂共持箱子从桌面左侧移到右侧），分别用三种方法（Projection/Atlas/TangentBundle）规划 100 次，统计成功率、中位时间、路径长度。画出三种方法的 box plot 对比图。