精读笔记

Problem Setting

《Constrained Articulated Body Dynamics Algorithms》（IEEE Transactions on Robotics / 2025）处理的是树形刚体系统的等式约束前向动力学：给定状态、力矩和运动约束，计算满足约束的加速度和约束力。表面上这是标准 KKT / Delassus 问题，但论文真正瞄准的是低复杂度和病态约束能否同时成立。

困难点在于两类结构冲突：刚体树结构天然支持 ABA 这类 O(n) 递推；但约束通过 Delassus 矩阵把远端 contact/end-effector 强耦合成 dense constraint-space problem，通常引入 m^3 factorization 或 m^2 d propagation。更糟的是，实际机器人中冗余接触、多个点约束同一刚体、奇异姿态都会让 J M^{-1} J^T 半正定，传统 Cholesky 路线直接失效。

以前方法大致卡在三处：软约束快但不解硬约束；PV/PV-early 低复杂度但对 singular constraints 不自然，常需 SVD 或 Tikhonov；proxLTL 鲁棒但仍在 joint-space/Delassus 路线上，复杂度和实际成本偏高。本文的关键矛盾是：能否保持 ABA 级别的树递推，同时获得 proximal solver 对奇异约束的鲁棒性。

Motivation

作者不是单纯想再写一个 constrained dynamics solver，而是抓住了已有路线的一个结构性缺口：低复杂度算法和鲁棒病态处理长期是分离的。ABA/LTL/PV 系列擅长利用树结构；proximal/KKT 系列擅长处理 rank deficiency；MuJoCo-style soft Gauss 擅长工程稳定性。但三者没有被统一到一个既简单又低复杂度的 formulation 中。

核心观察是：Tikhonov regularization 的问题不是数值上不够强，而是物理上改变了问题——它惩罚 constraint force magnitude，导致约束力被拉向 0，进而牺牲约束满足。PPA/ALM 则是另一种 damping：它正则化的是迭代子问题，不是最终物理解。这个区别是全文的出发点。

另一个动机是实现层面的：PV-early 虽然有 O(n+m)，但推导和实现复杂，且多 DoF joint 下 SVD 成本和适用性都不好。作者希望得到一种几乎只需在 ABA 中加入 link-local inertia/force augmentation 的 constrained solver。这是很实际的缺口：如果不能低成本进入 Pinocchio/Drake/MuJoCo 这类库，再好的低复杂度算法也很难被采用。

Core Idea

论文的核心不是“提出几个算法”，而是把 constrained dynamics 重新看成 proximal fixed-point 问题，并把每一步 proximal 子问题组织成可由刚体树递推求解的无约束动力学问题。具体说，等式约束 GPLC 的 dual Hessian 是 Delassus 矩阵；当它奇异时，直接求解 dual 不可靠。PPA 加上 1/μ I 让每步 dual problem 强凸/强凹，ALM 的等价形式则把这个过程转成 primal augmented inertia problem。

cABA 的本质变化在 maximal coordinates：约束残差二次项不再形成一个全局 J^T J，而是落在每个 constrained link 上，成为 H_i + μK_i^T K_i；dual 项成为 K_i^T(μk_i - λ_i) 的等效 spatial force。于是 ALM 的 primal minimization 和无约束 ABA 完全同构。换句话说，约束没有被“集中”到 constraint space，而是作为局部增广惯量和局部等效力注入树递推。

这带来的 inductive bias 很明确：利用机器人拓扑而不是约束空间 dense algebra。信息流沿 tree propagation，而不是先构造全局 Delassus 再 factorize。与 prior 的本质区别是，它不是在 Delassus solve 上做更快 linear algebra，而是重新排序了消元顺序：先把 dual/proximal 影响局部化，再让 ABA/Riccati recursion 消去 primal variables。

Method

1. Proximal / ALM formulation：解决 singular/redundant constraints 下 Delassus 不可逆的问题。它需要的不是给 Delassus 加 Tikhonov 后直接当解，而是把加 damping 的系统作为 proximal 子问题反复求解。核心变化是：每步良态，极限仍对应原硬约束问题；数值 infeasible 时也自然走向 least-squares residual。

2. cABA：解决 proxLTLs 虽鲁棒但仍依赖 joint-space LTL 的问题。作者把 ALM primal step 写在 maximal coordinates 中，约束项被吸收到 link inertia 和 spatial force，随后用 ABA 的 backward inertia propagation 和 forward rollout 求解。核心变化是 constrained forward dynamics 变成“带增广 link inertia 的 unconstrained ABA”。

3. reduced two-sweep：解决 ALM 多次迭代重复完整 ABA 的浪费。因为 μK_i^TK_i 和大部分 articulated inertia 在固定 contact set 下不变，迭代间主要变化是 λ 导致的 force correction。因此后续迭代只传播 Δf 并更新 constrained subtree acceleration。这个机制是 cABA 实际快的重要原因，但它依赖约束集合和拓扑在迭代内不变。

4. proxPV：解决传统 PV 对 constraint independence 的依赖。它基本是把 PV 产生的 dual function 放进 PPA，用 damped Delassus 做 proximal iteration。实质创新较小，更像是把 proximal 视角嫁接到 PV 上，收益是 robustness，复杂度仍有 m^2d 和 m^3。

5. cABA-OSIM：解决 damped Delassus inverse 计算中 m^3 factorization 的瓶颈。Woodbury identity 将 Λ_μ 写成 μI 减去一个基于 M_μ^{-1} 的 Delassus-like 项，因此可以调用低复杂度 OSIM propagation，而不是显式 factorize dense Λ_μ^{-1}。这是一个很干净的 linear algebra insight。

Key Insight / Why It Works

最关键的 insight 是：proximal damping 如果放在 dual fixed-point 层面，它不是物理 compliance；如果通过 ALM 转到 primal 层面，它又恰好等价于 link-level inertia augmentation。这个位置选择决定了方法既稳又不偏。Tikhonov 失败的原因是它把 constraint force 本身作为要压小的量；本文的 PPA 把当前 λ 与下一步 λ 的距离作为 proximal cost，最终不要求 λ 小，只要求 fixed point 满足原 KKT。

cABA 真正有效不是因为 ALM 新，也不是因为 ABA 新，而是二者在 maximal coordinates 中发生了结构对齐：ALM 的二次残差项 K_i^TK_i 是局部 6D spatial inertia 形式，正好能进入 Featherstone recursion。这是本文最核心贡献。它本质上是 better inductive bias / latent structure exploitation：把约束局部性和刚体树拓扑显式编码进求解过程。

proxPV 相比 cABA 更像是补全谱系：它说明 PV 也能 proximal 化，但没有改变 dense dual coupling 的基本瓶颈。proxLTLs 的竞争力则提醒一点：实际性能不只由渐近复杂度决定，vectorized sparse factorization 在现代 CPU 上很强。因此 cABA 的实际增益部分来自算法复杂度，部分来自避免 dense constraint factorization，但不是在所有机器人拓扑上压倒性。

cABA-OSIM 的贡献很实：它不是 scaling trick，而是利用 damped inverse 的特殊形式消掉 m^3 factorization。这里的“最优复杂度”判断基本成立，因为输出本身是 m×m 且必须遍历机器人结构；但它的适用对象是 damped Delassus inverse，而不是任意 undamped inverse。

如果要区分核心与辅助：核心是 proximal-ALM 与 maximal-coordinate ABA 的同构；reduced sweeps 是重要工程加速；proxPV 是理论统一和鲁棒性扩展；benchmark 和 Pinocchio 实现验证可用性但不是概念核心。

Relation To Prior Work

这篇论文处在 Featherstone ABA / Vereshchagin-PV / Gauss principle / proximal constrained dynamics 的交叉处。和 ABA 的关系是：cABA 几乎保留 ABA 的消元结构，只改变每个 link 的局部 inertia/force。和 PV 的关系是：PV 也是 maximal-coordinate DP，但显式保留并传播 constraint dual coupling；cABA 借 proximal formulation 让 dual 可以局部消去，从而避免 PV-early 那种复杂 early elimination 和 SVD。

和 proxLTL 的本质差异是坐标和消元顺序。proxLTL 在 joint-space 中先形成/使用 Delassus，再做 proximal dual iteration；cABA 在 maximal coordinates 中通过 ALM 先把约束变成局部 augmented dynamics，再用 tree recursion。两者数学上同属 proximal constrained dynamics，但计算图完全不同。

和 MuJoCo / Drake 的 relaxed formulation 的关系很有意思：MuJoCo 的 soft Gauss equality constraint 可以看成 proxLTLs 的第一步且 λ=0；PV-soft 可以看成 cABA 的第一步。这个观点把“软约束 simulator”和“硬约束 proximal solver”放在同一连续谱上：多做几步 ALM/PPA 就从 compliance 走向 rigid constraint。

哪些是已有思想重组？PPA、ALM、ABA、PV、Woodbury 都不是新东西。实质创新在于把它们放在正确的坐标系统和消元顺序里，使 constrained ABA 以非常简单的 link-local modification 出现，并给出 damped Delassus inverse 的低复杂度构造。

Dataset / Evaluation

评价覆盖了机械臂、四足、人形、带手复杂人形，以及人工 chain / balanced binary tree。这个覆盖对算法 claim 是合适的：它同时测试 n scaling、m scaling、tree depth、branching topology、floating-base branching 等影响复杂度的关键因素。没有真机实验不是问题，因为论文主张是动力学内核算法，不是控制策略。

benchmark 主要验证三件事：operation count 符合复杂度分析；C++ timing 在实际库中仍有优势；proximal iteration 在冗余/奇异约束下通常少量步数收敛。尤其是把 operation count 和 CPU timing 分开是必要的，因为 LTL 在 Pinocchio 中高度向量化，会掩盖理论复杂度差异。

但 evaluation 也有边界。它没有验证在完整 contact simulator 中的长期能量行为、接触切换稳定性、摩擦不等式、impact handling，也没有系统研究 μ scheduling。冗余约束实验支持 proximal robustness，但还不足以说明真实复杂接触场景下可以直接替代现有 solver。cABA 对小机器人或 LTL 友好拓扑的优势也并不稳定，论文自己数据已显示 proxLTLs 在一些重约束场景非常接近。

Limitation

第一，问题范围限制明显：论文主要是树形结构、等式运动约束、前向动力学内核。闭链、摩擦接触、不等式 complementarity、contact mode switching 都不是本文真正解决的对象。未来扩展方向被提到，但文中没有证明这些性质会保留。

第二，PPA/ALM 的迭代次数被经验性当作常数。大多数 benchmark 中几步收敛，但这依赖 μ、约束尺度、Baumgarte 项、数值 infeasibility 和初值。文中固定 μ 的实验已经显示少量 case 需要更多迭代，说明最坏情况下 O(n+m) 只是 fixed-iteration 视角，而不是严格整体复杂度。

第三，cABA 的增益上限受硬件和实现影响。递推算法天然 sequential，不容易像 LTL/Cholesky 那样吃满 SIMD / BLAS。论文中实际 timing 的优势小于 operation count 优势，说明一部分理论收益在现代 CPU 上会被实现因素抵消。

第四，cABA-OSIM 的最优性只针对 damped Delassus inverse。虽然实际 contact/control 中 damping 常常合理，但如果任务确实需要 undamped inverse 或精确 rank-revealing 信息，这个算法并不直接解决。

第五，文中未充分说明在动态变化 contact set 下 reduced sweeps 和缓存项的维护成本。如果每个仿真步 contact topology 大幅改变，完整三扫和数据结构更新可能削弱工程收益。

Takeaway

• 1. 最值得记住的是：硬约束动力学中的 damping 应该放在 proximal iteration 上，而不是作为 force penalty 直接改变物理解。
• 这个 distinction 对 simulator 设计很重要。
• 2. maximal coordinates 不是只带来冗余变量；在这里它让约束项保持 link-local，从而和 ABA 的 spatial recursion 对齐。
• 这个 insight 可迁移到 differentiable dynamics、contact derivatives、结构化 QP solver。

一句话总结

这篇论文把 proximal / ALM 约束处理与 ABA 的树递推结构精确对齐，给出了首个简单、线性复杂度且能处理奇异等式约束的 constrained ABA，并把软约束 simulator、PV 系列和 proximal KKT solver 统一到同一算法谱系中。