本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。
D02 双臂协调规划——闭链约束 / TSR / CBiRRT / Atlas-RRT / 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_armsgroup）并运行约束规划
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） |
从自由规划到约束规划的认知跳跃¶
自由空间规划（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\)（弹性物体允许一定变形），规划问题会简单还是更难？
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]\)。
三种方法的核心区别在于**如何在约束流形 \(\mathcal{M}\) 上生成新的采样点**。
方法 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 截断伪逆或阻尼伪逆替代。
优势：
- 实现最简单——只需约束函数 \(h(q)\) 和 Jacobian \(J_h(q)\)
- OMPL 直接提供 ProjectedStateSpace
- 对低维约束（\(k\) 小）效率可接受
劣势： - 采样效率低：在高维 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 类似
三种方法的系统对比¶
| 维度 | 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 使切换只需改一行代码。
本质洞察：三种约束采样方法的本质区别不是"算法不同"，而是"在什么空间采样"。投影法在 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 自带的ConstrainedPlanningImplicitChaindemo。记录投影成功率和规划时间。 -
[对比] 在同一双臂问题上切换 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 结合的算法——双臂约束规划的事实标准。
算法完整伪代码¶
输入: 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 的双层结构¶
┌─────────────────────────────────┐
│ 符号层（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×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 ± 0.8 | 3.1 ± 0.6 | 2.8 ± 0.5 |
| 最大约束违反 (mm) | 0.02 ± 0.01 | 0.15 ± 0.08 | 0.05 ± 0.02 |
| 最大内力峰值 (N) | 2.1 ± 0.5 | 8.3 ± 3.1 | 3.4 ± 0.9 |
| 关节速度峰值 (rad/s) | 1.2 ± 0.2 | 1.8 ± 0.3 | 1.5 ± 0.3 |
| 停顿次数 | 0 | 2.3 ± 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 双臂配置实战 ⭐¶
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");
闭链约束规划的数值稳定性分析¶
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 约束规划器在双臂搬运任务上的规划时间和路径质量。
本章小结¶
| 知识点 | 核心要点 | 对应小节 | 难度 |
|---|---|---|---|
| 双臂规划挑战 | 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)
延伸阅读¶
| 文献 | 类型 | 难度 | 推荐理由 |
|---|---|---|---|
| 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 凸集图优化，最新进展 |
故障排查手册¶
| 症状 | 可能原因 | 排查步骤 | 相关章节 |
|---|---|---|---|
| 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 |