IMPACT: 一种用于快速 Contact-Implicit 轨迹优化的 Implicit Active-Set 增广拉格朗日方法¶
论文阅读笔记 — 用于后续讨论的概览
1. 基础信息¶
- 题目: IMPACT — An Implicit Active-Set Augmented Lagrangian for Fast Contact-Implicit Trajectory Optimization
- 作者: Jiayun Li, Dejian Gong, Georgia Chalvatzaki（PEARL Lab, TU Darmstadt / Hessian.AI / Robotics Institute Germany）
- arXiv 编号: 2605.09127 (submitted 2026-05)
- 项目主页: https://jonaspflaume.github.io/impact_info/
- 关键词: contact-implicit trajectory optimization (CITO), MPCC, augmented Lagrangian, active-set, block coordinate descent, CI-MPC, dexterous manipulation
Figure 1：上图为 Allegro Hand 在仿真中实现对橡皮鸭的 in-hand reorientation；下图为 Panda 在真机上完成 Push-T 任务，红色曲线是物体原点轨迹。论文用同一算法把这两个差异巨大的任务（dexterous CI-MPC vs. long-horizon CITO）跑通。
2. 文章介绍¶
2.1 解决的领域和问题¶
CITO（Contact-Implicit Trajectory Optimization）是用统一连续优化框架同时推理"何时建立/断开接触"以及"接触状态下的连续动力学"的方法，被广泛应用于操控（push、in-hand reorient）和足式运动。它本质上是一类 MPCC（Mathematical Program with Complementarity Constraints）：用 \(0 \le G(x_t) \perp H(x_t) \ge 0\) 来表达"距离 vs 力"的互斥条件。
MPCC 的核心痛点：标准 NLP 求解器需要的 CQ（如 LICQ/MFCQ）在 complementarity 约束下系统性失效，KKT 条件失效，乘子可能 blow-up，初值敏感、数值脆弱。论文要解决的就是 — 在保留原始 nonsmooth complementarity 结构（不做 smoothing / relaxation）的前提下，提供一个数值稳健、有平稳点收敛保证、且工程上够快到能进 MPC loop 的求解器。
2.2 Motivation¶
主流处理 complementarity 的方法分为两类，各有缺陷：
- Smoothing / Relaxation（Scholtes、compliant contact）：拿到光滑梯度但牺牲了 rigid-contact 的物理精度，relaxation 收紧时退化与坏条件数会回来。
- Penalty（squared penalty、CRISP 等）：避免显式接触模式枚举，但大 penalty → stiff 子问题；启发式 continuation schedule → 收敛慢；用 IPOPT 解的话 wall-clock 仍然贵。
ADMM/C3 系列在多接触下扩展性好，但通常基于线性化动力学、产出任务空间命令、需要额外 tracking 层；难以直接处理 generalized coordinates 下的通用 MPCC。
作者提出第三条路：Safeguarded Augmented Lagrangian for MPCCs — 把等式与不等式约束扔给 AuLa（带 safeguard 的乘子/penalty 更新），但 complementarity 仍作为硬约束保留 在内层子问题里；内层用 BCD（X 块 Gauss-Newton；Y,Z 块 closed-form），借助这个 closed-form 步骤 隐式地 选择 contact branch。
2.3 之前工作的问题¶
| 类别 | 代表工作 | 缺陷 |
|---|---|---|
| Smooth/Compliant 接触模型 | Pang 2023, Suh 2022, Jin 2024 (cfree), Kurtz 2026 | 不严格满足 rigid 接触律；smoothing 程度强 → 控制振荡 / 偏差，弱 → 退化重新出现 |
| Scholtes Relaxation + NLP (IPOPT) | LCQPow, 经典 NLP | 需要 outer continuation \(t \downarrow 0\)；越接近 0 子问题越难；warm-start 收益有限 |
| Squared Penalty + NLP | CRISP, Onol 2020 | penalty 调度启发式；大 penalty 让子问题 ill-conditioned；CRISP 在 Push-T 上要 1243 次迭代 |
| ADMM / Consensus (C3 family) | Aydinoglu 2022/2024, Bui 2025 (C3+) | 多基于线性化动力学；输出任务空间命令需额外 tracking；针对 QPCC 而非 general MPCC |
| Vanilla AuLa（把 complementarity 也吸进 AL） | — | 互补乘子无界 → 经典的 multiplier blow-up |
2.4 论文解决方案（一句话）¶
用 safeguarded AuLa 处理光滑等式/不等式约束、把 complementarity 保留为内层硬约束，再用 BCD（X: Gauss-Newton，(Y,Z): 闭式选 cone 分支）求解 — 既继承了 AuLa 的平稳点收敛保证，又通过 closed-form 分支选择获得 implicit active-set 的快收敛。
2.5 与前序工作的关系¶
- 算法基础直接建立在最近 MPCC-AuLa 理论（Guo 2022, Jia 2023, De Marchi 2023, Kanzow 2022）— 论文的贡献是把这套理论落地到 TO/MPC，外加专门设计的内层 BCD 求解器。
- 在 CITO 测试中和 CRISP（Li 2025）正面竞争（CRISP 是同期 penalty + continuation 的强 baseline）。
- 在 CI-MPC 测试中和 cfree（Jin 2024，smoothing + IPOPT）对比。
- 论文方法上和 Ménager 2025（hal-05201780，AuLa+BCD for QPCC inverse dynamics）思路接近，但 IMPACT 更早提交，且面向 temporally coupled general MPCC 而非单步 QPCC。
3. 方法介绍¶
3.1 形式化：MPCC 与 vertical 重写¶
原始 MPCC 形式（generalized coordinates，sum-of-squares 目标）：
引入 slack \(Y, Z\)，把非线性塞到光滑等式里，complementarity 落到简单 slack 变量上：
为什么这样改？\(0 \le y \perp z \ge 0\) 是两条 axis-cone 的并集，几何极其简单，可以对每个 scalar pair 独立、闭式更新；而 \(G(x_t), H(x_t)\) 中的非线性现在变成了普通光滑等式 \(G(x_t) - y_t = 0\)，被 AuLa 自然处理。
3.2 Safeguarded AuLa Outer Loop¶
把 vertical 等式 \(\bar h_t := [h(x_t, x_{t+1});\, G(x_t)-y_t;\, H(x_t)-z_t]\) 和不等式 \(g_t\) 用 AuLa 吸收，complementarity 不进 AL，作为硬约束保留：
要点：
- 所有 penalty/multiplier 项都是平方型，叠到 \(r\) 上仍是 nonlinear least squares → 内层可以用 Gauss-Newton，免去 Hessian 计算。
- 乘子 \(\kappa, \mu\) 通过 clipping safeguard 到 \([\kappa_{\min},\kappa_{\max}]\)、\([0,\mu_{\max}]\)，避免 vanilla AuLa-for-MPCC 的乘子 blow-up。
- penalty 更新是经典 safeguarded 规则：合并 feasibility + KKT 残差不下降 \(\eta\) 倍就 \(\rho \gets \gamma \rho\)。
收敛定理（Theorem 1，重述自 Guo 2022 / Jia 2023）：在 safeguarded 乘子有界 + 内层残差 \(r_{\rm in}(w^k) \le \varepsilon_k \downarrow 0\) + MPCC regularity 的前提下，任何 feasible 累积点都是 MPCC 的一阶 stationary point（M-stationary）。
3.3 BCD Inner Solver¶
Figure 2：在二维 toy 问题上对比 Scholtes relaxation（左上）、squared penalty（右上）和 IMPACT。绿色是 complementarity 可行集（两条 axis 的并集），蓝色是目标函数等高线。IMPACT 把更新拆成两步：primal 更新（左下，受 slack 拖拽）+ slack 更新（右下，从 dual-shifted target 投影到 complementarity 集）。slack 像"弹簧锚点"把 primal 拉到合法 branch 上，而不是单纯做 feasibility projection。
Block 1 (X-update)：固定 \((Y,Z)\) 后内层目标是光滑 NLS，作者用 damped Gauss-Newton + Armijo backtracking 解。
Block 2 ((Y,Z)-update)：固定 \(X\) 后每个 scalar pair \((y_{t,i}, z_{t,i})\) 在 \(\{(y,0)| y\ge0\}\cup\{(0,z)|z\ge0\}\) 上做精确最小化：
- Case 1 \((z=0)\): \(y^\star = \max(0,\, G_i(x_t) + \tfrac{1}{\rho_{\bar h}}\kappa_{G,t,i})\)
- Case 2 \((y=0)\): \(z^\star = \max(0,\, H_i(x_t) + \tfrac{1}{\rho_{\bar h}}\kappa_{H,t,i})\)
比较两个 case 的目标函数值（其实就是两个 quadratic penalty 项），取小的那一支。这就是"implicit active set"的含义 — 每次 BCD sweep 都隐式地选择 contact / no-contact 分支，但不需要外层 mode enumeration 也不需要 homotopy schedule。
与 ADMM 投影（Bui 2025）的对比（论文 Remark）：ADMM 直接把 copy 变量投到可行集，纯 feasibility 驱动；IMPACT 的 slack update 是 dual-shifted projection — 把当前乘子/penalty 的影响注入 slack，像弹簧力一样把 primal 拽过来。
Theorem 2：在 \(\Phi^k\) 光滑+下有界、X-step 有 sufficient decrease、(Y,Z)-step 是精确最小化的前提下，BCD 在有限步内可把 \(r_{\rm in}\) 压到任意 \(\varepsilon_k\) 以下。这就给外层 AuLa 提供了所需的"attainability"接口。
实际实现里用的不是 \(r_{\rm in}\) 测试，而是 目标函数 stagnation: \(|\Phi^k(w^{(j)}) - \Phi^k(w^{(j+1)})| \le \tau_k\) 就停（"inexact/budgeted inner solve"）。
3.4 Implementation Details¶
- 硬件: AMD Ryzen 9 7950X3D，无 GPU，无显式多线程（不用 OpenMP/TBB）。
- 库: BLAS/LAPACK + Eigen sparse LDLT（线性求解）+ Eigen SIMD（slack 更新）+ CasADi C++（建模与符号微分，没用 CasADi CodeGen）。
- CITO 超参（典型）: \(\rho\) scaling factor 1.1–1.5，multiplier safeguard \(10^6\)，外层 tol \(10^{-5}\)，max outer iter 1000，max inner iter 10–50，Newton tol \(10^{-6}\)。
- Allegro CI-MPC 超参: 单 shooting，horizon \(N=4\)，control bound \(\pm 0.1\)，frame skip 50，contact Jacobian 只在初始状态算一次（与 cfree 一致），\(\rho_{\max}=10^3\)，\(\rho\) scale 5.0，max outer 10，max inner 5。
- MPC 控制频率: Allegro 任务 9.53 Hz（cfree 是 50.6 Hz），CITO Push Box 平均 0.15 s/solve。
4. 结果对比¶
4.1 CITO Benchmark（CRISP suite，全 0 初始化）¶
每个任务 50 个随机 goal/start-goal，统一终止准则：所有约束违反 \(< 10^{-5}\)。红色=最优，蓝色=次优。
| 任务 | Solver | Success | Track. Err. ↓ | Iters ↓ | Time (s) ↓ |
|---|---|---|---|---|---|
| Push Box | SR | 100.0% | 19.46±2.30 (红) | 164±12 (蓝) | 1.27±0.10 |
| PM | 98.0% | 58.00±5.11 | 66±13 (红) | 0.85±0.17 (蓝) | |
| CRISP | 100.0% | 51.90±4.08 | 641±358 | 2.52±1.34 | |
| IMPACT | 100.0% | 26.99±2.81 (蓝) | 219±49 | 0.15±0.03 (红) | |
| Push T | SR | 100.0% | 1.24±0.13 (红) | 202±11 (蓝) | 5.50±0.38 |
| PM | 90.0% | 21.99±2.57 | 117±14 (红) | 2.96±0.39 (蓝) | |
| CRISP | 100.0% | 8.03±0.71 (蓝) | 1243±407 | 25.73±6.92 | |
| IMPACT | 100.0% | 8.48±1.94 | 606±225 | 1.03±0.44 (红) | |
| Cart Transport | SR | 100.0% | 433.08±44.92 | 261±22 | 5.63±0.57 |
| PM | 100.0% | 408.65±42.32 | 87±14 (红) | 2.00±0.32 (蓝) | |
| CRISP | 100.0% | 235.10±21.70 (红) | 501±381 | 2.72±2.05 | |
| IMPACT | 100.0% | 381.89±32.35 (蓝) | 101±15 (蓝) | 0.08±0.01 (红) |
Figure 3：IMPACT 在三个 CITO 任务上的轨迹示例 — Push Box / Push T / Cart Transport。绿色虚线框是起点，橙色虚线框是终点，紫色箭头是接触力，绿色实线是物体原点轨迹，颜色从浅蓝渐变到深橙表示时间推进。Push T 的非凸性带来更复杂的 mode landscape。
速度结论: 相对 CRISP 速度提升 \(16.8\times / 25.0\times / 34.0\times\)（geomean \(24.3\times\)）；相对最快 baseline PM \(5.7\times / 2.9\times / 25.0\times\)（geomean \(7.4\times\)）；总 range \(2.9\times\)–\(70\times\)，geomean 13.8×。
质量结论: 速度领跑但 tracking error 不是最优。SR 在 Push Box / Push T 上质量明显更好（Push T 尤其差距大：1.24 vs 8.48）；CRISP 在 Cart Transport 上最好。IMPACT 是个 quality–speed trade-off — 适合 MPC 这种"快"比"完美轨迹"重要的场景。
4.2 Allegro Hand CI-MPC Benchmark（cfree, Jin 2024，17 objects）¶
Figure 4：17 个物体上的 in-hand reorientation 评测，对比 IMPACT、cfree(±0.1)、cfree(±0.2)。指标包括 success rate、control variance、smoothness、control effort。控制带宽扩大（±0.2）能让 cfree 维持成功率但显著恶化 control quality — 这是 smoothing-based 接触模型在闭环里振荡的特征。
| Metric | IMPACT | cfree(±0.1) | cfree(±0.2) |
|---|---|---|---|
| Success rate | 91.8%±4.1 | 91.2%±4.2 | 92.9%±3.9 |
| Control variance ↓ | 1.55±0.13 | 2.16±0.21 | 4.44±0.49 |
| Smoothness ↓ | 0.029±0.004 | 0.044±0.008 | 0.088±0.012 |
| Control effort ↓ | 3.0±1.0 | 4.8±2.0 | 8.2±4.8 |
| MPC steps to success | 66.5±10.9 | 71.4±13.5 | 57.8±14.6 |
| Control rate (Hz) | 9.53±0.36 | 50.6±0.86 | 54.0±1.30 |
结论: 成功率持平，控制质量全面更优（variance/smoothness/effort 都更低），但控制频率只有 cfree 的 1/5。论文把这定位成"我们没 smoothing 也能跑到 10 Hz"的工程可行性 demo，而不是"我们比 cfree 快"。
Stick 物体失败: IMPACT 在 stick 上明显比 cfree 差。论文归因于 LCP 模型对 slender geometry 高度敏感（频繁 mode switching）。
4.3 真机 Push-T（Panda，10 trials）¶
先离线求 full-horizon TO，机器人执行 reference；偏离则在线 replan。Sim-to-real 通过 tune 旋转 contact-radius 实现。10/10 成功。这是定性 demo 而非系统评测。
4.4 关键消融¶
论文未提供专门的 ablation 表（无"去掉 safeguard"、"换成 ADMM 投影"、"无 GN"等正式消融）— 这是评测部分最大的弱点之一（详见 §5.2）。
5. 引申问题 / 讨论¶
5.1 做得好的地方¶
- Vertical 重写让 nonlinearity 和 complementarity 解耦：把 \(G(x), H(x)\) 的非线性扔到光滑等式里，让 complementarity 只作用在 axis-aligned 的 slack 变量上 — 这是后续 closed-form (Y,Z) 更新得以成立的根基。等式约束多了 \(2|\text{pairs}|\) 维，但被 AuLa 吸收得很自然。
- Complementarity 留作硬约束、不进 AL：直接规避了 vanilla AuLa-for-MPCC 里乘子 unbounded 的病。这是 Guo 2022/Jia 2023 理论的精髓，IMPACT 把它工程落地。
- SOS 目标 + AuLa = Gauss-Newton 友好：所有 penalty / multiplier 项都是平方型，能直接叠进 \(r\) 形成 NLS，免去显式 Hessian，对 TO 这种高维稀疏问题至关重要。
- Closed-form (Y,Z) 是"branch picker"，但带 dual shift：和 ADMM 的纯 feasibility 投影相比，注入了乘子/penalty 的"弹簧力"，能 actively 把 primal 拽向当前 active mode，因此实际收敛速度比纯 projection 快得多（Figure 2 的可视化很有说服力）。
- C++ 实现里有大量工程心思：没用 GPU、没用多线程、没用 CasADi CodeGen，但通过 sparse LDLT + Eigen SIMD 把 BCD sweep 做得极快 — Push Box 一次 solve 仅 0.15 s。这个 vanilla CPU baseline 本身就是有价值的对照实验。
- 统一框架同时跑 long-horizon CITO 和 high-dim CI-MPC：同一个 solver 既能解 \(T=300\) 的 Cart Transport，又能在 10 Hz 跑 22-DOF Allegro hand 的多接触 MPC，框架的通用性确实展示了。
5.2 做得不够好的地方 / 值得质疑的地方¶
- Tracking quality 在 Push T 上明显输给 SR（8.48 vs 1.24，7× 差距）。论文把这归为 "quality-speed trade-off"，但这种 trade-off 在 MPC 里能否接受要看场景。如果下游需要高保真轨迹（e.g. 真机精细操控），IMPACT 直接拿"几何接触模型 + LCP"出来的解不一定够用。
- 没有真正的 ablation。论文宣称的速度提升来自至少 4 个独立来源：(a) AuLa 框架本身（vs penalty）；(b) closed-form (Y,Z) 更新（vs Newton 步）；(c) Gauss-Newton 内层（vs IPOPT 的 IPM）；(d) C++ vanilla 实现的高质量。论文没拆开任何一个。无法回答"如果把 CRISP 也用同样的 C++/Eigen 写一遍，差距还有多大？"或"换成 projected Newton 在 (Y,Z) 上是否更快？"
- CI-MPC 9.53 Hz vs cfree 50+ Hz 的鸿沟被淡化。论文重心放在"control quality 更好"上，但 5× 频率差在实际闭环稳定性里不是 cosmetic — 很多硬件平台需要 30+ Hz 才能稳定 in-hand 操控。论文应当承认这是个真实 gap，而不是用"我们没 smoothing"作为豁免。
- MPCC regularity 条件是定理 1 的核心前提，但论文未讨论它在典型接触问题里何时成立。M-stationarity 的"feasible accumulation point 存在"也只是假设。读者难以判断算法在真实任务里到底有多 robust，只能看实验。
- All-zero 初始化对其他 baseline 也是"标准"，但 IMPACT 是否能 warm-start 收益更大没测。MPC 默认 warm-start；如果 SR/CRISP 在 warm-start 下显著缩小 iter count，那 IMPACT 的 wall-clock 优势可能也会缩小。论文自己也承认 "the relative advantage of IMPACT may differ in such settings"。
- Allegro 实验在 stick 上失败，归因 "LCP + slender geometry sensitivity"，但这同时也暴露了 BCD 在 mode 频繁切换下的不稳定性。论文 Limitations 里也提到"transient regime before contact is firmly established, optimizer may switch between nearby contact sets"。换言之，implicit active-set 本身在硬切换时不一定收敛 — 没有 hysteresis / trust-region 兜底。
- 缺少与 C3 / ADMM 系列的直接对比。Related Work 里大段讨论 ADMM/C3+，并解释了"它们和我们不一样"，但 Push-T / Cart Transport 这类问题 C3+ 是有现成 baseline 的，理应跑一遍。否则读者只能信任 narrative。
- Allegro 任务依赖 "contact-encouraging cost"（来自 Jin 2024）。这是手工启发式，论文复用了它而没自己消除依赖 — Limitations 里承认这一点，但承认了不等于解决。意思是：speedup 一部分来自 baseline 本身已 tune 好的 cost。
- frozen Jacobian + 单 shooting 在 CI-MPC 里是个真实但被淡化的近似。短 horizon \(N=4\) 下勉强 work；如果 horizon 拉长（足式运动场景常需要），这个近似会显著降质，不知 IMPACT 是否还能保持优势。
- "\(2.9\times\)–\(70\times\) 提升"中的高端值（70×）很可能来自单一 unfavorable baseline-task 组合。Geomean 13.8× 是更 honest 的数字，但 abstract / conclusion 反复强调 70× 容易误导。
5.3 值得继续探讨的方向¶
- 第二阶 (Y,Z) 更新：当前是 closed-form scalar projection，但若引入 cross-block curvature（projected Newton over union of cones），收敛常数可能进一步降低 — 论文 commented-out 段提到过 BCD-derived Newton 方向作为 future work。
- Hysteresis / trust-region on mode updates 解决 stick 等 slender 物体的频繁切换问题（论文 Limitations 也指出）。
- Warm-start + receding horizon MPC 真机部署：当前真机只是离线 TO + tracking，让 IMPACT 直接作为 closed-loop CI-MPC 跑在 Allegro 真机上是自然下一步。
- 和 C3+ 在 planar pushing 上的正面对比：缺失这个就难以定位 IMPACT 在 ADMM 一族中的位置。
- Offline RL 提供 global guidance + IMPACT 做 local 精修：作者自己提到这个组合方向，能缓解对 contact-encouraging cost 的依赖。
- Conic（second-order cone）friction 模型：论文目前用 polyhedral friction → LCP；提到了 cone-complementarity 也 fit 这个 framework 但更难，验证一下扩展性。
- 真正的 ablation matrix：(a) AuLa vs Penalty in same C++ code；(b) closed-form vs projected Newton vs ADMM proj；(c) GN vs IPOPT inner；(d) C++ vs Python baseline — 能让"速度提升来自哪里"这个核心问题有答案。
- 多线程 / GPU 加速 (Y,Z) sweep：scalar pair 间天然 embarrassingly parallel，作者明确说"目前没多线程"，这是 free 的 future work。
- 平稳点之外的二阶充分条件：M-stationary 只是必要条件；能不能 detect 真正的局部最优（甚至 escape spurious stationarity）？
参考资源¶
- 论文 PDF: paper.pdf
- LaTeX 源码: source/
- 项目主页: https://jonaspflaume.github.io/impact_info/
- 关键相关工作:
- CRISP — Li et al., 2025（penalty-based CITO，主要 baseline）
- cfree — Jin et al., 2024（smoothing + IPOPT CI-MPC，Allegro benchmark 来源）
- C3 / C3+ — Aydinoglu 2022, Bui 2025（ADMM 系列，未对比）
- LCQPow — Hall 2025（penalty for QPCC）
- AuLa-for-MPCC 理论：Guo 2022, Jia 2023, De Marchi 2023, Kanzow 2022