约束动力学——从 DAE 到可微接触与浮基控制¶

所属：刚体动力学专题 4-6 | 前置：20_Lagrange与Hamilton力学（d'Alembert 原理、标准方程）、40_SE3几何力学（Lie 群上的动力学）、50_动力学解析微分（ABA 导数） 交叉引用：→ 20_微分几何与李群/30_李群基础与SO3_SE3（Frobenius 定理）、→ 70_辛结构与动量映射（SHAKE/RATTLE）、→ 05_运动控制/90_DDP

前置自测¶

📋 前置自测（答不出 ≥ 2 题 → 先回前置章节复习）

写出机器人标准动力学方程 $M(q)\ddot q + C(q,\dot q)\dot q + g(q) = \tau$ 中每一项的物理含义和维度。（→ 20_Lagrange与Hamilton力学）
什么是 Frobenius 定理？分布可积的等价条件是什么？（→ 30_SO3_SE3 §流形与分布）
解释 KKT 条件中互补松弛条件 $\lambda_i g_i(x) = 0$ 的物理含义。
什么是 DAE（微分代数方程）？它与 ODE 的本质区别是什么？
Pinocchio 中 getFrameJacobian 的三种 ReferenceFrame 分别是什么？何时用哪个？（→ 50_动力学解析微分）

本章目标¶

学完本章，你将能够： 1. 形式化**任意约束系统为 DAE 并识别其微分指数；运用 Baumgarte 稳定化消除数值漂移 2. **推导**从 d'Alembert 原理到完整 KKT 鞍点系统的每一步，理解 Lagrange 乘子的物理含义（约束力） 3. **建模**闭链机构（Stewart/Delta/Cassie 四杆膝）的 loop closure 约束并用 Pinocchio 3 求解 4. **理解**接触动力学的 LCP/NCP formulation 与三条可微化路线（软化/平滑化/子梯度） 5. **实现**浮基人形/四足的接触约束 QP-WBC，包括摩擦锥、ZMP、CWC 的完整约束集 6. **区分**五大仿真器（Pinocchio/MuJoCo/Drake/Dojo/Nimble）的约束模型差异，为具体应用选择合适工具 7. **理解**约束动力学与辛结构的关系（SHAKE/RATTLE），以及 Baumgarte 为何破坏辛性 8. **掌握**约束动力学解析导数（IFT on KKT）用于 DDP/MPC 的梯度传播 9. **运用 SymPy 符号计算验证 KKT 推导的每一步、Baumgarte 特征方程、摩擦锥线性化

核心哲学：约束动力学不是"给方程加一个条件"——它改变了系统的数学结构（ODE→DAE），引入了新的求解范式（KKT/LCP/NCP），要求新的数值工具（Baumgarte/GGL/Proximal），并创造了全新的应用可能（可微仿真、接触隐式 MPC）。掌握这一章，就掌握了从 Featherstone 1987 到 Carpentier 2025 的整条技术脉络。

面向工程师的行动清单： - 能用 Pinocchio 3 组装并求解约束动力学 - 知道 MuJoCo solref/solimp 参数的物理含义和调参方法 - 理解 WBC QP 的决策变量、约束、目标函数 - 知道何时用 proximal（秩亏）vs dense LDL（小系统）vs Schur（约束少）

向前承接：本章建立在 20_Lagrange与Hamilton力学（Euler-Lagrange 方程）、30_SO3_SE3（Frobenius 定理、Lie 群）、50_动力学解析微分（ABA 导数）之上。

向后指向：本章的 KKT 系统 + 解析导数直接喂给 MPC/DDP（运动控制方向）。可微接触是 sim-to-real RL 的入口（学习方向）。闭链建模直接应用于并联机器人（操作方向）。

与其他章节的连接矩阵：

方向	连接章节	连接方式
上游	20_Lagrange → d'Alembert 原理	KKT 推导起点
上游	30_SO3_SE3 → Frobenius 定理	约束可积性判据
上游	50_动力学微分 → ABA 导数	IFT on KKT 的基础
平行	70_辛结构 → SHAKE/RATTLE	约束系统的辛积分
下游	MPC/DDP → 约束最优控制	KKT 解析导数作为梯度
下游	RL → sim-to-real	可微接触作为可学习层
下游	并联机器人 → 闭链运动学	loop closure 建模

知识树¶

约束动力学（本章）
├── §1 约束分类学 ⭐⭐
│   ├── holonomic / nonholonomic（Frobenius 判据）
│   ├── bilateral / unilateral（Signorini 互补）
│   └── scleronomic / rheonomic
├── §2 DAE/ODE 约束形式化 ⭐⭐⭐
│   ├── index-3 → index-2 → index-1 降阶
│   ├── Petzold 定理：drift-off 效应
│   └── 与 ODE 积分器的兼容性
├── §3 Lagrange 乘子法与 KKT 系统 ⭐⭐
│   ├── d'Alembert → Lagrange 乘子定理
│   ├── 鞍点系统结构与 Schur 消元
│   └── λ 的物理含义：约束力
├── §4 Baumgarte 稳定化 ⭐⭐
│   ├── 完整推导：误差方程 → 临界阻尼
│   ├── 参数选择准则与步长约束
│   ├── 替代方案：GGL / Projection / SHAKE
│   └── MuJoCo solref 与 Baumgarte 映射
├── §5 闭链机构 ⭐⭐⭐
│   ├── loop closure 约束（SE(3) 恒等）
│   ├── cut-joint 方法与 spanning tree
│   └── O(N) 闭链求解（Pinocchio 3）
├── §6 接触动力学 ⭐⭐⭐⭐
│   ├── LCP / NCP formulation
│   ├── Signorini 条件与 Coulomb 摩擦锥
│   ├── 时步法：Stewart-Trinkle / Moreau 半隐
│   └── CWC / ZMP / Capture Point
├── §7 可微接触 ⭐⭐⭐⭐
│   ├── MuJoCo soft contact（Todorov 凸化）
│   ├── Complementarity smoothing（Dojo κ-smooth）
│   ├── LCP 子梯度（Nimble）
│   └── Pinocchio 3 proximal + 可微化
└── §8 浮基接触建模与 QP-WBC ⭐⭐⭐
    ├── 浮基方程 M q̈ + h = S⊤τ + J_c⊤λ
    ├── 人形/四足接触约束 QP
    └── HQP / TSID 分层控制

§1 约束分类学 ⭐⭐¶

1.1 动机：为什么需要约束分类¶

在 20_Lagrange与Hamilton力学中，我们推导了无约束机器人的运动方程 $M(q)\ddot q + C\dot q + g = \tau$。但现实中的机器人**不是在真空中自由漂浮的**——它有关节极限、有足端接触地面、有闭链联动机构、有夹持物体形成的闭合回路。每一种"限制"都是一个约束，而不同类型的约束需要完全不同的数学工具来处理。

本质洞察：约束不是"给方程加一个条件"那么简单。约束改变了系统的**数学结构**——从 ODE 变成 DAE，从光滑流变成非光滑混合系统，从凸优化变成互补问题。理解约束的分类，就是理解你面对的数学问题属于哪一类。

如果不做分类会怎样？ 把所有约束一视同仁地用同一种方法处理——比如都用 Baumgarte 稳定化——会导致：(1) 对 unilateral 约束（接触），Baumgarte 会在接触释放时产生非物理的"吸力"；(2) 对 nonholonomic 约束（轮子），试图积分约束方程得到位置层约束是不可能的（因为不可积）；(3) 对闭链约束，不区分约束维度会导致 KKT 矩阵尺寸错误。

1.2 Pfaffian 一般形式与 Frobenius 判据¶

设广义坐标 $q \in Q \subset \mathbb{R}^n$，约束以 Pfaffian 一般形式写：

\[A(q)\dot q + b(q,t) = 0, \quad A(q)\in\mathbb{R}^{m\times n}\]

这一形式统一了所有一阶约束。 若存在函数 $\varphi(q,t)$ 使该 Pfaffian 等价于 $\dot\varphi = 0$（即 $A\,dq + b\,dt$ 为某 exact 1-form），则约束为 holonomic（完整约束），可降维为 $\varphi(q,t) = 0$；否则为 nonholonomic（非完整约束）。

可积判据 = Frobenius 定理。回顾 30_SO3_SE3 中的 Frobenius 定理：对分布 $\Delta = \ker(A)$，

\[\Delta \text{ 可积} \iff \Delta \text{ involutive}: \forall X,Y\in\Gamma(\Delta),\ [X,Y]\in\Gamma(\Delta)\]

等价地，1-form 系统满足 $d\omega_i \wedge \omega_1 \wedge \cdots \wedge \omega_m = 0$。

类比：想象一个山丘的等高线。完整约束像"只能沿等高线走"——你的运动被限制在一个低维曲面上。非完整约束像"只能朝北或朝东走"——这不限制你能到达哪些位置（仍然可以到达整个平面），但限制了你的瞬时速度方向。

1.3 四元分类体系¶

分类维度	类型 A	类型 B	区分依据
时间依赖	Scleronomic（不含 t）	Rheonomic（含 t）	约束方程是否显含时间
等式/不等式	Bilateral（$\varphi = 0$）	Unilateral（$\varphi \geq 0$）	Signorini 互补 $\varphi \cdot \lambda = 0$
可积性	Holonomic	Nonholonomic	Frobenius 定理
理想性	理想约束（力 $\perp$ 虚位移）	非理想（含摩擦等）	约束力是否做虚功

1.4 经典例子详解¶

例 1：球铰约束 $r_A - r_B = 0$（3D holonomic bilateral scleronomic）。两刚体通过球铰连接，约束力是 3D 反力，方向任意。

例 2：足端非穿透 $z_\text{foot} \geq 0$（unilateral holonomic）。足在地面上方时约束不激活（$\lambda = 0$）；接触时 $z = 0$ 且 $\lambda \geq 0$（法向压力）。这是 Signorini 互补条件： $$z_\text{foot} \geq 0, \quad \lambda \geq 0, \quad z_\text{foot} \cdot \lambda = 0$$

例 3：独轮车约束 $\dot x \sin\theta - \dot y \cos\theta = 0$（nonholonomic）。验证不可积：写 Pfaffian 1-form $\omega = \sin\theta\,dx - \cos\theta\,dy$，计算 $$d\omega = \cos\theta\,d\theta \wedge dx + \sin\theta\,d\theta \wedge dy$$ $$d\omega \wedge \omega = \cos\theta\sin\theta\,d\theta\wedge dx\wedge dx - \cos^2\theta\,d\theta\wedge dx\wedge dy + \cdots$$ 化简得 $d\omega \wedge \omega \neq 0$，故**不可积**。其 Lie 括号 $[\partial/\partial v, \partial/\partial\omega]$ 生成新方向——这正是 Chow-Rashevsky 定理保证独轮车可达任意位置的原因。

1.5 约束分类对求解方法的影响¶

约束类型	数学工具	代表方法	典型机器人
Holonomic bilateral	DAE / KKT	Baumgarte / GGL / Proximal	闭链、球铰
Holonomic unilateral	LCP / NCP	Stewart-Trinkle / Moreau	足端接触
Nonholonomic	Pfaffian + 力层方程	d'Alembert-Lagrange	轮式、球滚动
Unilateral + friction	SOCP / CCP	MuJoCo Newton / Drake SAP	足式运动

⚠️ 常见陷阱¶

💡 概念误区：认为"nonholonomic = 不能到达某些位置" - 新手想法："约束限制了运动，所以 nonholonomic 约束限制了可达位置集" - 实际上：Chow-Rashevsky 定理告诉我们，bracket-generating 的非完整约束系统可以到达配置空间的任意点——约束限制的是**瞬时速度方向**，不是**可达位置** - 正确理解：独轮车不能侧向平移，但通过转弯可以到达任何 $(x, y, \theta)$ 组合

⚠️ 编程陷阱：把 unilateral 约束当 bilateral 处理 - 错误做法：对足端接触始终强制 $z = 0$ - 后果：机器人脚离地时出现非物理的"吸力"，被拉回地面 - 正确做法：必须用互补条件或 active-set 方法检测接触状态

§2 DAE/ODE 约束形式化 ⭐⭐⭐¶

2.1 动机：为什么直接积分约束系统会发散¶

回顾 20_Lagrange与Hamilton力学中的标准动力学方程。对于自由系统，$M\ddot q + C\dot q + g = \tau$ 是一个 ODE，任何标准积分器（Euler/RK4/...）都能正确处理。但加上约束 $\varphi(q) = 0$ 后，方程变成：

\[M(q)\ddot q + C(q,\dot q)\dot q + g(q) = \tau + J^\top(q)\lambda, \quad \varphi(q) = 0\]

这**不再是 ODE**。它包含代数约束 $\varphi(q) = 0$ 和未知的约束力 $\lambda$——这是一个**微分代数方程（DAE）**。如果你忽略约束方程、只积分动力学部分，然后"希望"约束自然满足，结果会是灾难性的。

反事实推理：如果直接用 RK4 积分上述方程（忽略约束），会发生什么？假设初始条件精确满足 $\varphi(q_0) = 0$，但由于数值截断误差，第一步就会产生 $\varphi(q_1) = O(h^5)$ 的微小违反。关键问题是：这个违反会衰减还是增长？

答案是：会以 $t^2$ 速度增长！ 这就是著名的 drift-off 效应。

2.2 微分指数的严格定义¶

定义（Hairer-Wanner 1996 §VII）：考虑半显式 DAE $$\dot x = f(x,y,t), \quad 0 = g(x,y,t)$$ 其**微分指数 $\nu$** 是把代数约束 $g$ 关于 $t$ 微分多少次才能显式解出代数变量 $y$ 的最小次数再加 1。

直觉：微分指数衡量的是"代数约束隐藏得有多深"。index-1 意味着对 $g = 0$ 微分一次就能看到 $y$；index-3 意味着需要微分三次。每多一阶，数值求解就困难得多。

2.3 约束力学的指数阶梯¶

对机器人约束 $\varphi(q) = 0$，根据写成的层级不同，微分指数不同：

层级	方程	微分指数	说明
位置层	$M\ddot q = \tau + J^\top\lambda,\quad \varphi(q) = 0$	3	原始形式，最病态
速度层	$M\ddot q = \tau + J^\top\lambda,\quad J(q)\dot q = 0$	2	对 $\varphi$ 微分一次
加速度层	$M\ddot q = \tau + J^\top\lambda,\quad J\ddot q + \dot J\dot q = 0$	1	对 $\varphi$ 微分两次
ODE（约化坐标）	$\ddot q = F(q,\dot q)$ on $T_qM$	0	完全消去约束

2.4 Drift-off 效应的数学证明¶

定理（Petzold 1982，Hairer-Wanner Vol II §VII.2）：对 index-3 DAE，标准 BDF 方法的阶数降低 $\nu - 1 = 2$ 阶；误差放大因子约为 $h^{-(\nu-1)}$。

简化证明：设 $m=1$, $M=I$, $J$ 为常数向量，忽略非保守力。令 $e(t) := \varphi(q(t))$ 为约束违反。真实解 $e \equiv 0$。

在 index-1 形式（加速度层），方程只强制 $\ddot e = J\ddot q + \dot J\dot q = 0$。但 $\dot e = J\dot q$ 和 $e = \varphi(q)$ 本身**没有被方程约束**。离散积分引入 $O(h^p)$ 局部截断误差 $\varepsilon$，此时：

\[\ddot e = \varepsilon(t) \implies \dot e(t) \sim \varepsilon \cdot t, \quad e(t) \sim \varepsilon \cdot t^2\]

即**速度层漂移线性增长、位置层漂移二次增长**。这就是"drift-off effect"。

类比：想象一条铁轨（约束流形）。火车（数值解）在铁轨上行驶，但每步都有微小的侧向力（截断误差）。如果没有"拉回铁轨"的机制，火车就会越来越偏离——这就是 drift-off。Baumgarte 稳定化相当于给铁轨加了弹簧和阻尼器。

2.5 Index reduction 的策略¶

策略 1：微分到 index-1。把约束对时间微分两次，得到加速度层方程。优点：可用标准 ODE 积分器；缺点：丢失位置和速度层信息，产生 drift-off。

策略 2：Overdetermined DAE（OD-DAE）。同时保留所有三层方程： $$M\ddot q = \tau + J^\top\lambda, \quad \varphi = 0, \quad J\dot q = 0, \quad J\ddot q + \dot J\dot q = 0$$ 使用特殊的 OD-DAE 求解器（如 DASSL 的变体）。

策略 3：约化坐标。选取满足约束的最小坐标 $z \in \mathbb{R}^{n-m}$，使得 $q = \phi(z)$ 自动满足 $\varphi(\phi(z)) \equiv 0$。优点：变回纯 ODE；缺点：$\phi$ 的构造可能困难或有奇异性。

策略 4：稳定化方法（Baumgarte/GGL/Projection）。保持 index-1 形式但加入稳定化项，抑制 drift-off。这是工程实践中最常用的方法。

本质洞察：Index reduction 不是"数学 trick"——它反映了一个深刻的物理事实：约束力 $\lambda$ 本质上是加速度层的量（它平衡的是力），但约束条件 $\varphi = 0$ 是位置层的量。两者之间差了两阶微分，这个"阶差"就是 index-3 的来源。

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"用更高阶的积分器就能解决 drift-off" - 新手想法："RK4 阶太低，用 RK8 或 DOP853 就行了" - 实际上：Petzold 定理告诉我们，对 index-3 DAE，**任何**标准 ODE 积分器都会产生 drift-off，只是速率不同。高阶方法只是让初始偏离更小（$O(h^p)$ 而非 $O(h)$），但增长趋势不变 - 正确做法：必须使用约束稳定化或专用 DAE 求解器

练习¶

手推：对单摆（$\varphi(q) = x^2 + y^2 - l^2 = 0$），写出其 index-3 DAE、index-2 DAE（速度层）、index-1 DAE（加速度层）的具体形式。
用 Python 实现：用显式 Euler 积分 index-1 形式的单摆，画出约束违反 $\varphi(q(t))$ 随时间的增长曲线，验证 $O(t^2)$ 增长。
比较：对同一问题分别用 RK4 和 Störmer-Verlet，哪个的 drift 更慢？为什么？（提示：回顾 70_辛结构与动量映射中辛积分器的性质）

§3 Lagrange 乘子法的完整推导与 KKT 系统 ⭐⭐¶

3.1 动机：约束力从哪里来¶

在 20_Lagrange与Hamilton力学中，我们用虚功原理推导了自由系统的 Euler-Lagrange 方程。核心思想是：对于无约束系统，虚位移 $\delta q$ 可以是任意方向的，因此可以从 $\sum (F_i - m_i\ddot r_i) \cdot \delta r_i = 0$（对任意 $\delta r$）推出每个分量方程独立为零。

但有约束时情况完全不同。虚位移不再是任意的——它必须满足约束的线性化 $J(q)\delta q = 0$。这意味着我们只能从 d'Alembert 原理推出"广义力在约束切空间方向为零"，而无法推出每个分量独立为零。约束法线方向的力——约束力——是自由的、由约束面自动提供的。

3.2 从 d'Alembert 原理到 KKT 的每一步¶

Step 1：d'Alembert-Lagrange 原理

\[\sum_i (F_i - m_i \ddot r_i) \cdot \delta r_i = 0, \quad \forall \delta r_i \text{ 满足 } J(q)\delta q = 0\]

用广义坐标重写，利用 $T = \frac{1}{2}\dot q^\top M(q)\dot q$ 和 $V = V(q)$：

\[\left(\frac{d}{dt}\frac{\partial L}{\partial \dot q} - \frac{\partial L}{\partial q} - \tau\right) \cdot \delta q = 0, \quad \forall \delta q \in \ker J(q)\]

Step 2：Lagrange 乘子定理的引入

关键数学工具：若 $Q^\top \xi = 0$ 对所有 $\xi \in \ker J$ 成立，则 $\exists\,\lambda \in \mathbb{R}^m$ 使得 $Q = J^\top \lambda$。

这是线性代数中 $(\ker J)^\perp = \text{row}(J) = \text{col}(J^\top)$ 的直接推论。物理含义：如果一个力向量对所有满足约束的虚位移做功为零，那么这个力必然属于约束 Jacobian 的行空间——即它是一个"约束力"。

Step 3：得到 Euler-Lagrange + 约束力

\[\frac{d}{dt}\frac{\partial L}{\partial \dot q} - \frac{\partial L}{\partial q} = \tau + J^\top(q)\lambda \tag{★}\]

代入机器人标准形 $T = \frac{1}{2}\dot q^\top M\dot q$, $V = V(q)$：

\[M(q)\ddot q + C(q,\dot q)\dot q + g(q) = \tau + J^\top(q)\lambda\]

Step 4：约束方程的两次微分

位置层约束：$\varphi(q) = 0$

第一次微分：$\frac{d\varphi}{dt} = \frac{\partial\varphi}{\partial q}\dot q = J(q)\dot q = 0$

第二次微分：$\frac{d}{dt}(J\dot q) = J\ddot q + \dot J\dot q = 0$

Step 5：联立得 KKT 系统

\[\boxed{\begin{bmatrix} M(q) & -J^\top(q) \\ J(q) & 0\end{bmatrix}\begin{bmatrix}\ddot q \\ \lambda\end{bmatrix} = \begin{bmatrix}\tau - C\dot q - g \\ -\dot J\,\dot q\end{bmatrix}}\]

3.3 KKT 系统的结构分析¶

鞍点结构。上述系统是一个**鞍点问题**（saddle-point system）。左上块 $M \succ 0$ 正定，右上/左下块 $\pm J^\top$ 耦合了动力学和约束。整体矩阵是**不定的**（既有正特征值又有负特征值），但在 LICQ 条件下唯一可解。

LICQ 条件（Linear Independence Constraint Qualification）：$J(q)$ 行满秩，即约束是线性无关的。违反 LICQ 时（冗余约束或奇异构型），$(JM^{-1}J^\top)$ 奇异，需要正则化处理。

Schur 消元（闭形式解）。从动力学方程（KKT 第一行 $M\ddot q - J^\top\lambda = \tau - C\dot q - g$）解出 $\ddot q$： $$\ddot q = M^{-1}(\tau - C\dot q - g + J^\top\lambda)$$

代入约束方程 $J\ddot q + \dot J\dot q = 0$（KKT 第二行）： $$J M^{-1}(\tau - C\dot q - g + J^\top\lambda) + \dot J\dot q = 0$$ $$J M^{-1} J^\top \lambda = -J M^{-1}(\tau - C\dot q - g) - \dot J\dot q$$

定义 Delassus 算子（操作空间惯量）： $$\Lambda(q) := (JM^{-1}J^\top)^{-1}$$

则： $$\boxed{\lambda = -\Lambda \left[JM^{-1}(\tau - C\dot q - g) + \dot J\dot q\right]}$$ $$\ddot q = M^{-1}(\tau - C\dot q - g + J^\top\lambda)$$

物理意义验证：当 $\tau = C\dot q + g$（关节力恰好补偿偏置力）且 $\dot J\dot q = 0$ 时，$\lambda = 0$——不需要额外的约束力来维持约束。当有外部力矩试图违反约束时，$\lambda$ 产生补偿力将系统"拉回"约束流形。

本质洞察：Delassus 算子 $\Lambda = (JM^{-1}J^\top)^{-1}$ 是 Khatib (1987) 操作空间动力学中的**操作空间惯量矩阵**。它描述了"从约束空间看过去，系统有多'重'"。当 $\Lambda$ 大时，约束力也大——直觉上就是"这个方向系统很重，需要很大的力才能维持约束"。

3.4 J 的几何含义¶

约束 Jacobian $J(q) = \partial\varphi/\partial q$ 有深刻的几何含义：

行空间 $\text{row}(J) = \text{col}(J^\top)$：约束力的方向。$J^\top\lambda$ 只能产生 $J$ 行空间方向的力
零空间 $\ker(J)$：约束容许运动空间，即约束流形的切空间 $T_q\mathcal{M}$
M-内积正交分解：$\mathbb{R}^n = \ker(J) \oplus_M \text{range}(M^{-1}J^\top)$

这意味着广义加速度可以分解为两个正交分量：约束切向运动**和**约束法向约束力响应。

3.5 $\lambda$ 的物理含义¶

约束类型	$\lambda$ 物理含义	维度
球铰 (point-to-point)	3D 反力	3
转动铰 (revolute)	5D wrench（除旋转轴外）	5
足端点接触	3D 接触力 $(f_x, f_y, f_z)$	3
足端面接触	6D wrench（力 + 力矩）	6
闭链内力	cut joint 处的 wrench	5 或 6

反事实推理：如果 $\lambda$ 不存在（即没有约束力），系统会怎样？以球铰为例：两个刚体在铰接点的加速度不再一致，它们会穿透彼此或分离。$\lambda$ 正是"阻止这一切发生的力"——它恰好大到使约束满足，不多也不少。这就是 Gauss 最小约束原理的精神。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：混淆 Jacobian 的转置方向 - 错误做法：写成 $M\ddot q + h = \tau + J\lambda$（缺少转置） - 后果：力的方向错误，约束力不在正确空间 - 正确做法：约束力永远是 $J^\top\lambda$（Jacobian 的**转置**乘乘子）

💡 概念误区：认为"LICQ 违反 = 系统有问题" - 新手想法："J 不满秩说明模型有 bug" - 实际上：冗余约束在四足机器人中是常态（4 足 × 3D = 12 约束，只需 6 来控制质心） - 正确处理：使用 proximal 正则化 $(JM^{-1}J^\top + \mu I)^{-1}$ 或最小二乘（伪逆）

练习¶

对 2D 单摆（$\varphi = x^2 + y^2 - l^2$），写出完整的 KKT 系统，求出 $\lambda$ 的物理含义（绳/杆的张力/压力）。
Schur 消元实践：对 3-DOF 平面臂加一个末端固定约束（2D），用 SymPy 实现 Schur 消元求 $\lambda$。
证明：在 M-内积下，$\ker(J)$ 和 $\text{range}(M^{-1}J^\top)$ 是正交互补的。

§4 Baumgarte 稳定化 ⭐⭐¶

4.1 动机：为什么 index-1 形式不够¶

§2 中我们知道，直接积分 index-1 形式（加速度层）会产生 drift-off。工程中需要一个**简单、廉价、可调**的方法来抑制这种漂移。Baumgarte (1972) 提出了一个优雅的思路：把约束视为一个被微扰的动力系统，加入负反馈使其稳定。

历史背景：Joachim Baumgarte 于 1972 年在 Computer Methods in Applied Mechanics and Engineering 创刊号上发表此方法。虽然简单，但 50 年后它仍然是工程实践（MuJoCo、Pinocchio、Drake）的核心组件之一。

4.2 完整推导¶

核心思想：定义约束违反 $e(t) := \varphi(q(t))$。在精确解上 $e \equiv 0$。由于数值误差，实际上 $e \neq 0$。我们希望设计一个"控制律"使 $e \to 0$。

Step 1：当前 index-1 方程的问题

index-1 形式只强制 $\ddot e = 0$（加速度层）。这意味着即使 $e(0) \neq 0$ 或 $\dot e(0) \neq 0$，也不会被修正——误差被"冻结"或线性增长。

Step 2：添加稳定化反馈

把加速度层方程 $J\ddot q + \dot J\dot q = 0$ 替换为：

\[J\ddot q + \dot J\dot q + 2\alpha\,J\dot q + \beta^2\,\varphi(q) = 0\]

新增的两项 $2\alpha J\dot q$ 和 $\beta^2\varphi$ 分别是**阻尼项**和**弹性项**，把约束违反当作一个弹簧-阻尼系统来稳定。

Step 3：误差方程分析

由 $e = \varphi(q)$, $\dot e = J\dot q$, $\ddot e = J\ddot q + \dot J\dot q$，代入得：

\[\ddot e + 2\alpha\,\dot e + \beta^2\,e = 0\]

这是一个**二阶线性 ODE**！特征方程：

\[s^2 + 2\alpha s + \beta^2 = 0\]

根：$s_{1,2} = -\alpha \pm \sqrt{\alpha^2 - \beta^2}$

Step 4：临界阻尼条件

条件	根的性质	行为
$\alpha < \beta$（欠阻尼）	复根	振荡衰减
$\alpha > \beta$（过阻尼）	双实根	最慢模 $s \approx -\beta^2/(2\alpha)$，越大越慢
$\alpha = \beta$（临界阻尼）	双重根 $s = -\alpha$	无振荡最快衰减

临界阻尼 $\alpha = \beta$ 时：

\[e(t) = (c_1 + c_2\,t)\,e^{-\alpha t}\]

这是无振荡条件下最快的衰减方式。

4.3 参数选择准则¶

准则 1（时间常数）：设希望约束违反在 $T_\text{settle}$ 时间内衰减到 $1\%$，则需要 $e^{-\alpha T_\text{settle}} \approx 0.01$，得 $\alpha \approx 4.6 / T_\text{settle}$。例如控制周期 1ms 要求 10ms 收敛 → $\alpha \approx 460$ Hz。

准则 2（积分器稳定性约束）：显式 Euler 的稳定域要求 $\alpha h \leq 1$（Flores EUCOMES 2008）。若步长 $h = 0.001$ s，则 $\alpha \leq 1000$。

准则 3（经验范围）：

应用场景	步长 $h$	推荐 $\alpha = \beta$	理由
腿足 MPC (1kHz)	1 ms	100–500 Hz	快速修正但不超稳定域
MuJoCo 仿真 (2kHz)	0.5 ms	200–1000 Hz	MuJoCo 隐式可承受更高
慢速工业臂 (100Hz)	10 ms	10–50 Hz	步长大需保守

4.4 Baumgarte 的缺陷¶

尽管 Baumgarte 方法简单有效，它有若干固有缺陷：

破坏辛结构：反馈项 $2\alpha J\dot q + \beta^2\varphi$ 不来自于任何 Hamiltonian，因此添加后系统不再保能量。回顾 70_辛结构与动量映射：辛积分器的优势在于保能量漂移有界——Baumgarte 破坏了这一点。
参数依赖步长：不存在对所有步长"最优"的 $(\alpha, \beta)$ 组合（Ascher-Chin-Reich 1994）。换步长就得换参数。
J 奇异时失效：当 $J$ 在奇异构型退化时，$\beta^2\varphi$ 项修正方向可能不对——因为 $\varphi$ 的梯度方向（即 $J$）本身就退化了。
漂移仅被抑制而非消除：稳态误差为 $e_\text{ss} = F/(α^2\beta^2)$，其中 $F$ 是驱动误差的"力"。永远不会精确回到约束面。

4.5 替代方案¶

方案 1：Coordinate Projection（Eich 1993）

每步积分后，解投影问题： $$\min_{\tilde q} \|\tilde q - q\|_M^2 \quad \text{s.t.} \quad \varphi(\tilde q) = 0$$

这相当于用 Newton 法把 $q$ 拉回约束流形。严格消除漂移，但每步需要额外的 Newton 迭代（通常 1-2 步收敛）。

方案 2：GGL Formulation（Gear-Leimkuhler-Gupta 1985）

引入第二 Lagrange 乘子 $\eta$，同时强制位置和速度约束： $$\dot q = v + J^\top\eta, \quad M\dot v = f(q,v) + J^\top\lambda, \quad \varphi(q) = 0, \quad J\dot q = 0$$

drift-off 阶数降为零。代价：系统维度增加。

方案 3：SHAKE/RATTLE（辛 DAE 积分器）

回顾 70_辛结构与动量映射中的辛积分器。SHAKE（Ryckaert-Ciccotti-Berendsen 1977）和 RATTLE（Andersen 1983）是**约束系统的辛积分器**：

SHAKE：在 Verlet 步后解非线性方程把位置拉回约束面
RATTLE：SHAKE 的速度层扩展，同时满足位置和速度约束

辛 + 约束满足 的双重保证使其成为分子动力学（GROMACS、LAMMPS）的标准方法。

方案 4：Post-stabilization（Ascher-Chin-Petzold-Reich 1995）

与积分器独立的正交投影稳定器。先用任何积分器前进一步，再做一步投影。优点：不修改积分器本身，可叠加使用。

4.6 MuJoCo 的 soft constraint 与 Baumgarte 的精确对应¶

MuJoCo 的约束模型（Todorov ICRA 2014）不直接用 Baumgarte，而是解一个凸优化问题：

\[\min_x \frac{1}{2}\|x - a_\text{free}\|_M^2 + \frac{1}{2}\|y - a^*\|_{R^{-1}}^2 \quad \text{s.t.} \quad y = Jx + a_0 \in \mathcal{K}^*\]

其中 $a^* = -b\,v - k\,r$（$r$ = constraint residual, $v = J\dot q$），$R = (1-d)/d \cdot A^{-1}$。

参数映射： $$k \leftrightarrow \beta^2, \quad b \leftrightarrow 2\alpha, \quad \texttt{solref} = (1/\beta,\; \alpha/\beta)$$

当 damp_ratio = 1 即 $\alpha = \beta$（临界阻尼 Baumgarte）。区别在于：MuJoCo 不直接加约束力（硬约束 $R = 0$），而用 $R > 0$ 构成软凸优化；极限 $R \to 0$ 回到硬约束 Baumgarte。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：MuJoCo solref 参数混淆 - 错误做法：设 solref = (α, β) - 实际上：solref = (timeconst, dampratio) = $(1/\beta, \alpha/\beta)$ - 正确映射：$\beta = 1/\texttt{solref[0]}$, $\alpha = \texttt{solref[1]} \cdot \beta$

🧠 思维陷阱：认为"Baumgarte 越强（α越大）越好" - 新手想法："α=10000 应该收敛最快" - 实际上：当 $\alpha h > 1$ 时显式积分器会**发散**。α 太大 → 约束违反反而爆炸 - 正确做法：$\alpha \leq 1/(h \cdot \text{安全系数})$，安全系数取 2-3

练习¶

对单摆 + Baumgarte 稳定化，用 Python 画出 $(\alpha, \beta)$ 参数平面上"稳定收敛区域"的边界。
比较 Baumgarte (α=β=100) 与 Coordinate Projection 在 1000 步积分后的约束违反。
解释为什么 MuJoCo 选择 solref = (0.02, 1) 作为默认值（即 β=50Hz, 临界阻尼）。

§5 闭链机构 ⭐⭐⭐¶

5.1 动机：树形模型的局限¶

URDF（和 Featherstone 的 O(N) 算法）假设机器人是**树形结构**——从根到每个末端只有一条路径。但许多实际机器人有**闭合运动链**：

Stewart-Gough 平台：6-UPS 并联机构，6 条腿连接固定基和运动平台
Delta 机器人：3 个平行四边形 + 1 个平台 loop
Cassie/Digit（Agility Robotics）：每腿一个四杆联动（shin-achilles-heelspring）
人形四杆膝关节：各膝 1 个 loop

这些机构的 URDF 表示必须"砍掉一个关节变树 + 加回闭环约束"。不处理闭链，ABA 就会**忽略闭环耦合力**，计算出错误的动力学。

类比：闭链约束像是桥梁中的桁架结构。如果你只计算一侧（树形），忽略另一侧的支撑力，桥就会"塌陷"。cut-joint 方法相当于"切开桁架但记住切口处有内力"。

5.2 建模流程（Featherstone 2008 Ch.8）¶

Step 1：识别 kinematic loop

通过图论分析：关节数 - 刚体数 + 1 = loop 数（Euler 公式的推广）。或用 Grübler 公式： $$\text{DOF} = 6(N-1-j) + \sum_{i=1}^j f_i$$ 其中 $N$ = 刚体数（含地面），$j$ = 关节数，$f_i$ = 第 $i$ 个关节的自由度。

Step 2：选择 cut joint

每个 loop 选一个关节切开，使剩余结构成为 spanning tree。选择原则： - 优先切高自由度关节（减少约束维度） - 避免切到驱动关节（保持输入完整性）

Step 3：构造 loop-closure 约束

对于被切断的关节，约束是"两端的帧必须重合"。设切断关节连接刚体 A 和刚体 B：

\[C_i(q) = {}^0M_{A_i}^{-1} \cdot {}^0M_{B_i} = I_{4\times4}\]

即从世界看，经由 spanning tree 的两条不同路径到达 cut joint 的两端，得到的 SE(3) 变换必须相等。

Step 4：线性化得 Pfaffian 约束

\[K_i(q)\,\dot q = 0\]

其中 $K_i$ 是 loop closure 约束的 Jacobian。

Step 5：用 tree forward dynamics + 约束力求解

\[K\,M^{-1}\,K^\top\,\lambda = -K\,\hat a - \dot K\,\dot q$$ $$\ddot q = \hat a + M^{-1}\,K^\top\,\lambda\]

其中 $\hat a$ 是由 ABA 计算的"无约束加速度"（树形系统的自由加速度）。

5.3 约束维度分析¶

切断关节类型	约束维度	说明
3D ball joint	3	位置必须重合
Revolute (1-DOF)	5	6D rigid - 1D rotation
Fixed (rigid)	6	完全刚连
Prismatic (1-DOF)	5	6D rigid - 1D translation

5.4 实际机器人的闭链结构¶

Cassie/Digit 四杆联动膝

Cassie 每腿有一个四杆联动结构：shin（胫骨）、achilles（跟腱连杆）、heelspring（足跟弹簧）。在 URDF 中，这被建模为： - Spanning tree：hip → thigh → shin → foot - Cut joint：achilles 的下端与 foot 的连接点 - 约束：5D revolute loop closure

整机 KKT 矩阵尺寸：Cassie 有 n=20 个 tree 关节自由度 + 2 × 5 = 10 维闭链约束，KKT 矩阵为 (20+10) × (20+10) = 30 × 30。

Stewart-Gough 6-UPS 平台

6 条腿，每条 UPS (Universal-Prismatic-Spherical)
平台 6-DOF，但仅 1 个驱动轴（棱柱关节）每腿
切开 5 条腿的球铰 → 15D 约束 + 5 条腿的万向节 → 总约束 = 6×5 = 30? 不！
正确分析：树 = 固基 + 一条腿 + 平台（6 DOF）；其余 5 条腿各贡献 5 或 6 约束
最终 DOF = 6

5.5 Pinocchio 3 闭链 API¶

#include <pinocchio/algorithm/constrained-dynamics.hpp>
using namespace pinocchio;

// 构造闭链约束模型
RigidConstraintModel cm(
    CONTACT_6D,          // 约束类型：完全刚连
    model,               // 机器人模型
    joint1_id,           // cut joint 一端的关节 ID
    joint1_placement,    // 约束参考帧在 joint1 中的位姿
    joint2_id,           // cut joint 另一端的关节 ID
    joint2_placement,    // 约束参考帧在 joint2 中的位姿
    ReferenceFrame::LOCAL  // 参考坐标系
);

// 初始化约束动力学（预分配稀疏 Cholesky）
RigidConstraintModelVector cms{cm};
RigidConstraintDataVector cds{RigidConstraintData(cm)};
ProximalSettings settings(1e-12, 1e-10, 10);  // mu, accuracy, max_iter
initConstraintDynamics(model, data, cms);

// 求解
constraintDynamics(model, data, q, v, tau, cms, cds, settings);
// 结果：data.ddq, data.lambda_c

官方例子：examples/simulation-closed-kinematic-chains.py；单元测试 closed-loop-dynamics.cpp。

5.6 O(N) 闭链求解策略¶

Featherstone Ch.8 的核心思想：先用 O(N) 的 ABA 求解 tree 部分，再用 Schur 补解闭链约束力。

总复杂度：$O(N) + O(m^3)$，其中 $N$ 是 tree 自由度、$m$ 是约束维度。由于 $m$ 通常远小于 $N$，这比密集 KKT 的 $O((N+m)^3)$ 快得多。

Pinocchio 3 的 constrainedABA 实现了这一策略，性能数据： - Atlas (n=36, 12 约束)：5-7 μs - 密集 KKT：~30 μs - 加速比：4-6×

⚠️ 常见陷阱¶

⚠️ 编程陷阱：URDF 无法表示闭链 - 错误做法：试图在 URDF 中用 mimic 关节模拟闭链 - 后果：mimic 只是运动学耦合，不产生约束力 - 正确做法：使用 MuJoCo XML（支持 loop closure）或 Pinocchio 手工构造 RigidConstraintModel

练习¶

画出 Cassie 单腿的 spanning tree 和 cut joint 位置。计算 Grübler 公式验证 DOF。
对简单的平行四边形机构（4 杆），写出 loop closure 约束的 SE(3) 形式。
估算 Cassie 整机（2 腿各 1 四杆 + 2 足接触 × 3D）的 KKT 矩阵尺寸和 Schur 方法 FLOP。

§6 接触动力学 ⭐⭐⭐⭐¶

6.1 动机：为什么接触比一般约束难得多¶

闭链约束是 bilateral（始终激活）的。但足端接触是 unilateral（可能激活也可能不激活）+ 有摩擦 的。这带来了三个根本困难：

互补性（complementarity）：$z \geq 0$, $\lambda \geq 0$, $z \cdot \lambda = 0$——不是连续优化，而是组合问题
摩擦锥（friction cone）：$\|f_t\| \leq \mu f_n$——非线性不等式约束
事件检测（event detection）：接触建立/释放/滑动/粘着的切换——混合系统

本质洞察：接触动力学之所以困难，不是因为方程复杂，而是因为它**本质上不是一个连续问题**。解的存在性和唯一性在一般情况下都不保证（Painlevé 悖论，1895）。所有"求解接触"的方法都是某种**近似**——区别只在于近似的方式和代价。

6.2 Signorini 条件（法向接触）¶

设 $\phi_n$ 为法向间距（gap function），$\lambda_n$ 为法向接触力：

\[\phi_n \geq 0, \quad \lambda_n \geq 0, \quad \phi_n \cdot \lambda_n = 0\]

三个条件的含义： - $\phi_n \geq 0$：不穿透（物体不能进入地面） - $\lambda_n \geq 0$：法向力只能是压力（地面不能"拉住"脚） - $\phi_n \cdot \lambda_n = 0$：互补性——要么不接触（$\phi_n > 0, \lambda_n = 0$），要么接触（$\phi_n = 0, \lambda_n > 0$），不能同时穿透又有力

6.3 Coulomb 摩擦锥¶

接触力 $f = (f_t, f_n)$（切向 + 法向）必须满足：

\[\|f_t\| \leq \mu\,f_n\]

其中 $\mu$ 是摩擦系数。几何上，接触力被限制在一个**冰激凌锥**内。

线性化近似：工程中常用 k-边形锥（k=4,8,16）近似圆锥：

\[|f_{t,x}| + |f_{t,y}| \leq \mu f_n \quad \text{(4-pyramid)}\]

或更精确的 8-polygon：选 k 个方向 $d_i = (\cos\theta_i, \sin\theta_i)$：

\[d_i^\top f_t \leq \mu f_n, \quad i = 1,...,k\]

6.4 LCP/NCP Formulation¶

线性互补问题（LCP）：

\[0 \leq \lambda \perp (A\lambda + b) \geq 0\]

即找 $\lambda$ 使得 $\lambda \geq 0$, $A\lambda + b \geq 0$, 且 $\lambda_i(A\lambda + b)_i = 0$ 对所有 $i$。

接触动力学的 LCP 形式：在加速度层，法向加速度 $a_n = J_n\ddot q + \dot J_n\dot q$，代入动力学得：

\[a_n = J_n M^{-1} J_n^\top \lambda_n + J_n M^{-1}(\tau - h) + \dot J_n \dot q\]

令 $A = J_n M^{-1} J_n^\top$（Delassus 算子），$b = J_n M^{-1}(\tau - h) + \dot J_n \dot q$，Signorini 条件变为：

\[0 \leq \lambda_n \perp (A\lambda_n + b) \geq 0\]

非线性互补问题（NCP）：加入 Coulomb 摩擦后，切向方程不再线性。用 maximum dissipation principle：

\[f_t = -\mu f_n \frac{v_t}{\|v_t\|} \quad \text{(sliding)}, \quad \|f_t\| \leq \mu f_n \quad \text{(sticking)}\]

这给出 NCP 或 Cone Complementarity Problem (CCP)。

6.5 Stewart-Trinkle 时步法¶

核心思想（Stewart-Trinkle 1996）：不在加速度层求解（连续时间），而是在**冲量层**（离散时间步）求解。设步长 $h$：

\[M(v^+ - v^-) = h\,f_\text{ext} + J_c^\top p_c\]

其中 $p_c = h\,\lambda$ 是接触冲量。配合离散 Signorini 条件：

\[0 \leq p_n \perp \phi_n^+ \geq 0\]

优势： 1. 避免了加速度层 LCP 的病态性（Painlevé 悖论在冲量层消失） 2. 自然处理碰撞（impact）和持续接触统一框架 3. 存在性和唯一性保证更强

6.6 Moreau 半隐格式¶

Moreau 扫过过程（sweeping process）：把接触视为 measure differential inclusion：

\[M\,dv \in -h(q,v)\,dt + N_K(v)\,dt\]

其中 $N_K(v)$ 是容许速度集 $K$ 在 $v$ 处的法锥。

Moreau 半隐格式： 1. 预测：$\tilde v = v_k + h\,M^{-1}f_\text{ext}(q_k, v_k)$ 2. 修正：$v_{k+1} = \text{proj}_{K(q_k)}^M(\tilde v)$（M-范数投影到容许集） 3. 更新：$q_{k+1} = q_k + h\,v_{k+1}$

**Dojo 仿真器**正是基于 Moreau 思路的 variational integrator + NCP 求解。

6.6.1 从 Stewart-Trinkle 到工程实践的完整链路¶

**时步法的工程实现细节**远比数学描述复杂。以下是一个完整的接触求解流程：

Phase 1：碰撞检测（Collision Detection） 1. 宽相（Broad Phase）：AABB/BVH 树快速剔除不可能碰撞的刚体对。MuJoCo 用 engine_collision.c 中的 mj_collision；Drake 用 FCL/Coal。 2. 窄相（Narrow Phase）：GJK/EPA 算法求精确接触点、法向、穿透深度。Pinocchio 使用 Coal 库（HPP-FCL 后继）。 3. 输出：接触点集 $\{(p_i, n_i, \phi_i)\}$——位置、法向、间距。

Phase 2：约束 Jacobian 构造 每个接触点 $i$ 贡献： - 法向 Jacobian $J_{n,i} = n_i^\top J_i(q)$（1 维） - 切向 Jacobian $J_{t,i} = D_i^\top J_i(q)$（2 维，$D_i$ 是切平面基）

整体 $J_c = [J_{n,1}; J_{t,1}; ...; J_{n,k}; J_{t,k}]$。

Phase 3：冲量求解 组装时步 LCP/CCP/QP 并求解（Newton/CG/PGS 之一）。

Phase 4：速度更新与位置积分 $v^+ = v^- + M^{-1}J_c^\top p_c$，$q^+ = q + hv^+$。

反事实推理：如果跳过 Phase 1 的宽相，直接做窄相，会怎样？ 100 个物体的场景有 $\binom{100}{2} = 4950$ 个潜在碰撞对——全部做 GJK 需要 ~5ms。加了 AABB 剪枝后通常只剩 <50 对需要精确检测——时间降到 <0.5ms。

Phase 2 中 ReferenceFrame 的选择对摩擦锥的影响：

回顾 §8.2 中 Pinocchio 的三种 ReferenceFrame。对摩擦锥 $\|f_t\| \leq \mu f_n$，必须在**接触帧**中定义——法向 $n$ 在接触帧中是 $[0,0,1]$。因此：

ReferenceFrame	摩擦锥定义	适用场景
`LOCAL`	最直接，$n = e_3$	摩擦锥约束
`LOCAL_WORLD_ALIGNED`	需变换 $n$	Cartesian 误差
`WORLD`	含额外叉积项，不推荐	极少使用

接触模型的数值特性对比：

模型	穿透深度	能量守恒	求解代价	梯度可用
硬 LCP	0（精确）	好（冲量守恒）	高（NP-hard 一般）	否
MuJoCo soft	$O(\text{solref})$	中（softness 耗散）	低（凸 QP）	是（MJX）
Dojo κ-smooth	$O(\kappa)$	好（variational）	中（IPM）	是（IFT）
Drake SAP	小（SAP 修正）	好	中（Newton）	是

接触事件类型的完整分类：

事件	物理	数学条件	处理方式
Make（建立接触）	物体到达地面	$\phi \to 0^+$, $\dot\phi < 0$	激活约束行
Break（释放接触）	物体离开地面	$\lambda_n \to 0^+$	去激活约束行
Stick→Slip	静摩擦被突破	$\\|f_t\\| \to \mu f_n$	约束变不等式
Slip→Stick	滑动减速到零	$\\|v_t\\| \to 0$	不等式变等式
Impact（碰撞）	速度不连续	$v^+ \neq v^-$	冲量层求解

6.7 Contact Wrench Cone (CWC) 与 ZMP/Capture Point¶

ZMP（Zero Moment Point）（Vukobratović-Stepanenko 1972）：地面上合地反力矩水平分量为零的点。投影公式：

\[p_\text{ZMP} = \frac{(c - p) \times mg + \dot L}{(mg + m\ddot c) \cdot n}\]

Capture Point（Pratt 2006）：LIPM 下 $p_\text{CP} = c + \dot c/\omega$，$\omega = \sqrt{g/z_c}$。

反事实推理：如果不区分 COP/ZMP/Capture Point，直接用 ZMP 做步态规划会怎样？ - 在单足支撑+平坦地面：COP = ZMP，没问题 - 在双足支撑或不平整地面：COP 和 ZMP 分离，用 ZMP 可能导致不稳定的足力分配 - 在跑步（飞行相）：ZMP 在支撑面外是允许的（因为有惯性力），但 COP 始终在接触面内

⚠️ 常见陷阱¶

💡 概念误区：认为"摩擦锥线性化 4 边就够" - 新手想法："4-pyramid 近似 = 方形摩擦锥，够用了" - 实际上：4-pyramid 在对角方向（45°）允许的摩擦力比圆锥少 $1 - 1/\sqrt{2} \approx 29\%$ - 对大 μ（如橡胶足：μ=0.8）影响显著；建议至少 8-polygon 或直接用 SOCP

⚠️ 编程陷阱：接触维度不一致 - 错误做法：humanoid 足用 3D point contact - 后果：无法约束 yaw 扭矩，机器人会原地打转 - 正确做法：humanoid 足用 6D surface contact，点接触仅用于球面足（如 ANYmal 原始版本）

练习¶

对 2D 弹跳球（单接触点），写出完整的 LCP 形式并手动求解一步。
比较 4-pyramid 和 8-polygon 摩擦锥近似在 μ=0.5 时的最大切向力差异。
推导 Capture Point 公式：从 LIPM 动力学 $\ddot c = \omega^2(c - p)$ 出发，找到使 $c \to p$ 的初始 CoM 位置条件。

§7 可微接触 ⭐⭐⭐⭐¶

7.1 动机：为什么需要可微接触¶

2020-2025 年，机器人学的一大趋势是**端到端可微**：从感知到控制到仿真，整条 pipeline 都可以反向传播梯度。但接触动力学是这条链上最大的"断点"——LCP 的互补条件 $\lambda \cdot \phi = 0$ 在切换点（接触建立/释放）处**不可微**。

如果接触不可微会怎样？ Policy gradient 通过 simulator 的梯度为零或不存在；Model-based RL 无法"学到"接触瞬间该怎么做；Trajectory Optimization 在接触切换时 NLP 梯度断裂。

三条可微化路线提供了不同的解决方案，各有代价：

7.2 路线 A：MuJoCo 软化（Softening）¶

核心思想：放弃严格互补性，用**软约束**（$R > 0$ 正则化）使问题变成凸 QP/SOCP：

\[\min_{x,y} \frac{1}{2}x^\top M x - x^\top\tau_\text{bias} + \frac{1}{2}\|y - a^*\|_R^2 \quad \text{s.t.} \quad y = Jx + c,\; y \in \mathcal{K}^*\]

其中 $R > 0$ 是"软化矩阵"，由 solref/solimp 参数决定。

优点：凸（保证唯一解）、解析可逆、梯度良好定义。MJX (JAX) 原生支持 jax.grad。

缺点：物理不精确——允许轻微穿透（penetration）和人工滑移（artificial slip）。对 sim-to-real 可能引入 gap。

参数映射： - solref = (timeconst, dampratio) → Baumgarte $(1/\beta, \alpha/\beta)$ - solimp = (d₀, d₁, width, midpoint, power) → 非线性阻抗参数

7.3 路线 B：Dojo κ-平滑化（Smoothing）¶

核心思想（Howell-LeCleac'h et al., arXiv:2203.00806）：保持硬接触物理，但用**内点法参数 κ** 平滑互补条件：

\[s \circ \lambda = \kappa\,\mathbf{e}, \quad \lambda, s \in \mathcal{Q}\]

当 $\kappa \to 0$ 时回到精确互补；$\kappa > 0$ 时是平滑近似。

关键设计变量：κ 不是"越小越好"，而是**用户可调的精度-梯度质量权衡**： - 大 κ：平滑、梯度信息丰富（对 RL/TO 有用），但物理不够精确 - 小 κ：精确物理，但梯度在切换点附近可能很陡

梯度计算：对 KKT 残差 $\Phi(z, \lambda, s; \kappa) = 0$ 应用隐函数定理： $$\frac{\partial z^*}{\partial \theta} = -\left(\frac{\partial\Phi}{\partial z}\right)^{-1} \frac{\partial\Phi}{\partial\theta}$$

7.4 路线 C：Nimble 解析子梯度¶

核心思想（Werling-Omens-Lee-Exarchos-Liu, RSS 2021）：保留 hard LCP，不做任何平滑化。当 active set 确定时（即已知哪些接触激活、哪些滑动），LCP 退化为一个线性方程组——对它求导是平凡的。

关键 insight：在 active set 不切换的区间内，接触力是 $\theta$（参数）的解析函数；在切换点使用**子梯度**（选择左导数或右导数之一）。

优缺点： - 优点：物理完全精确、无近似 - 缺点：切换点处梯度可能不连续，需要后续插值或 bundle 方法

7.5 路线 D：随机平滑化（Suh et al., 2022）¶

核心思想：不修改 simulator，而是对**输入参数加随机扰动**，取期望：

\[\nabla_\theta \mathbb{E}_\epsilon[L(\text{sim}(\theta + \epsilon))] \approx \frac{1}{N}\sum_{i=1}^N L(\text{sim}(\theta + \epsilon_i)) \cdot \nabla_\theta \log p(\epsilon_i)\]

这是 REINFORCE/score function estimator 在物理仿真中的应用。不需要 simulator 可微，只需要能采样。

与 Natural Policy Gradient 的联系：回顾 70_辛结构与动量映射 §3.9——Fisher 信息矩阵在策略空间上定义的度量。随机平滑化本质上是在参数空间上做"平滑"，与 NPG 的 KL 散度正则化异曲同工。

7.6 Pinocchio 3 proximal + 可微化¶

Pinocchio 3 的 proximal 方法（Carpentier-Budhiraja-Mansard RSS 2021）统一了约束动力学和可微化：

\[\begin{bmatrix} M & -J^\top \\ J & \mu I\end{bmatrix}\begin{bmatrix}\ddot q^{k+1} \\ \lambda^{k+1}\end{bmatrix} = \begin{bmatrix}S^\top\tau - h \\ -\gamma - \mu\lambda^k\end{bmatrix}\]

迭代 $\lambda^{(k+1)} = \lambda^{(k)} + \frac{1}{\mu}(J\ddot q^{(k+1)} + \gamma)$（Augmented Lagrangian 形式）。

为什么 proximal 方法天然支持可微化？ 因为正则化后的 KKT 矩阵 $(JM^{-1}J^\top + \mu I)$ 始终可逆（即使 J 秩亏），IFT 永远成立。梯度就是：

\[\frac{\partial(\ddot q, \lambda)}{\partial\theta} = -\begin{bmatrix} M & -J^\top \\ J & \mu I\end{bmatrix}^{-1} \frac{\partial F}{\partial\theta}\]

7.7 四条路线对比¶

维度	MuJoCo 软化	Dojo κ-smooth	Nimble 子梯度	随机平滑
物理精度	中（有穿透/滑移）	高（κ→0 精确）	高（精确 LCP）	依赖 base sim
梯度质量	好（凸解析）	好（可调 κ）	切换点不连续	高方差
求解代价	低（凸 QP）	中（IPM）	中（稀疏线性）	高（N 次采样）
实现复杂度	低（MJX 原生）	中（Julia）	中（C++）	低（wrapper）
适用场景	GPU RL 大批量	MPC/TO	精确物理需求	任意不可微 sim

⚠️ 常见陷阱¶

🧠 思维陷阱：认为"可微仿真 = 更好的仿真" - 新手想法："Dojo 比 MuJoCo 好因为它可微" - 实际上：可微化有代价（平滑化降低精度，子梯度有噪声）。对于不需要梯度的任务（如 RL 用 reward shaping），MuJoCo 的速度优势可能更重要 - 正确思维：可微仿真是**工具**，不是**目标**。根据任务选择方法

练习¶

在 MuJoCo 中，设置 solref = (0.02, 1) 和 solref = (0.002, 1)，比较接触穿透深度。解释为什么更小的 timeconst 导致更小的穿透。
解释为什么 Dojo 的 κ 参数和 Pinocchio 的 μ 参数在数学上等价（都是互补条件的正则化）。
对一个简单的 2D 弹跳球，分别用 MuJoCo（软化）和精确 LCP 仿真 100 次弹跳，比较能量漂移。

§8 浮基接触建模与 QP-WBC ⭐⭐⭐¶

8.1 动机：从理论到腿足控制¶

前面 7 节建立了约束动力学的完整数学框架。本节把一切组合起来，展示**人形/四足机器人**的完整接触建模与控制流程。这是 WBC（Whole-Body Control）和 MPC（Model Predictive Control）的数学基础。

8.2 浮基方程的完整形式¶

设 $q \in SE(3) \times \mathbb{R}^n$，速度维度 $n_v = n + 6$：

\[M(q)\ddot q + C(q,\dot q)\dot q + g(q) = S^\top\tau + J_c^\top\lambda\]

其中 $S = [0_{n \times 6} \quad I_n] \in \mathbb{R}^{n \times n_v}$ 是选择矩阵，剔除浮基 6 行（浮基无电机）。

为什么前 6 行特殊？ 浮基的 6 个自由度（3 平移 + 3 旋转）没有对应的执行器。机器人只能通过**接触力 $J_c^\top\lambda$** 间接控制这 6 个自由度。这就是"欠驱动性"的来源。

8.3 前 6 行 = 质心动力学¶

动力学方程的前 6 行恰好是质心动力学（Orin-Goswami-Lee 2013）：

\[\dot h_G = \begin{bmatrix} m\ddot c \\ \dot L_G \end{bmatrix} = \begin{bmatrix} \sum_i f_i - mg \\ \sum_i (p_i - c) \times f_i + \tau_i \end{bmatrix}\]

质心动量矩阵 $h_G = A_G(q)\,v$，其中 $A_G \in \mathbb{R}^{6 \times n_v}$ 是整机惯量矩阵前 6 行经 CoM 坐标变换得到。

8.4 WBC QP 的完整形式¶

决策变量：$(\ddot q,\, \tau,\, \lambda)$

目标函数： $$\min_{\ddot q, \tau, \lambda} \sum_k w_k \|J_k\ddot q + \dot J_k\dot q - \ddot x_k^\text{des}\|^2$$

约束： 1. 动力学方程（等式）：$M\ddot q + C\dot q + g = S^\top\tau + J_c^\top\lambda$ 2. 接触不滑动（等式）：$J_c\ddot q + \dot J_c\dot q = 0$（或加 Baumgarte 项） 3. 摩擦锥（不等式）：$\lambda \in \mathcal{FC}(\mu)$ 4. 扭矩限制（不等式）：$\tau_\min \leq \tau \leq \tau_\max$ 5. 关节限制（不等式）：$q_\min \leq q + \dot q\,dt + \frac{1}{2}\ddot q\,dt^2 \leq q_\max$

消元简化：由动力学方程解出 $\tau$：

\[\tau = (SS^\top)^{-1}S(M\ddot q + C\dot q + g - J_c^\top\lambda)\]

代入后决策变量简化为 $(\ddot q, \lambda)$，约束数减少。

8.5 分层 QP（HQP）¶

严格优先级的 WBC 通过分层 QP 实现：

优先级	任务	约束形式
P0（最高）	动力学可行性、接触约束	等式约束（硬）
P1	质心平衡（CoM 位置/速度）	最小化误差
P2	末端跟踪（手/脚 SE(3)）	最小化误差
P3	姿态保持（关节角默认位）	最小化误差

实现方式 1：级联 QP。先解 P0+P1 的 QP，得到 P1 的最优值；然后在 P2 的 QP 中加约束"P1 不变坏"。

实现方式 2：零空间投影。P1 在 P0 零空间中优化；P2 在 P0∩P1 零空间中优化。

实现方式 3：TSID 软权重。用大权重近似硬优先级：$w_1 \gg w_2 \gg w_3$。简单但不严格。

8.6 TSID 库接口（C++）¶

#include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
using namespace tsid;

// 创建 formulation（决策变量 = (q̈, λ)）
auto formulation = std::make_shared<InverseDynamicsFormulationAccForce>(
    "tsid", robot, false);

// 添加接触
auto contact_RF = std::make_shared<Contact6d>("contact_RF", robot,
    "RF_frame", contact_normal, mu, fMin, fMax);
formulation->addRigidContact(contact_RF, 1e-5);

// 添加任务（带优先级权重）
auto task_com = std::make_shared<TaskComEquality>("task_com", robot);
formulation->addMotionTask(task_com, w_com, priority_level);

// 每控制周期求解
auto HQPData = formulation->computeProblemData(t, q, v);
auto sol = solver.solve(HQPData);
auto tau = formulation->getActuatorForces(sol);

8.7 接触力分配的冗余问题¶

问题：四足机器人 4 足 × 6D = 24 维接触力 $\lambda$，质心方程仅 6 维——存在 18 维冗余。

物理含义：有无穷多种接触力分配方式都能产生相同的质心加速度。需要额外准则选择"最优"分配。

常见优化准则：

准则	目标函数	物理含义
最小力范数	$\min\\|\lambda\\|_2^2$	均匀分配，避免单足过载
最小能耗	$\min\lambda^\top R\lambda$	R 含关节-力映射
最大摩擦余量	$\max\min_i(\mu f_{n,i} - \\|f_{t,i}\\|)$	避免滑移
CoP 居中	$\min\\|p_\text{CoP} - p_\text{center}\\|^2$	增加稳定裕度

QP 求解器选择：

求解器	类型	适用规模	速度	Pinocchio/TSID 支持
EiQuadProg	Active Set	小-中	~50 μs	TSID 默认
ProxQP	Proximal ALM	中-大	~30 μs	ProxSuite
OSQP	ADMM	大	~100 μs	稀疏友好
qpOASES	Active Set	小-中	~40 μs	经典选择
HPIPM	Riccati	结构化	~10 μs	OCS2 后端

8.8 角动量调节¶

**角动量任务**在 humanoid 控制中至关重要。考虑单腿跳跃：如果不主动调节角动量，着陆时身体会旋转。

CMM 方程：$\dot L_G = \sum_i (p_i - c) \times f_i + \tau_i$

WBC 中的角动量任务： $$\min \|A_\omega\ddot q + \dot A_\omega\dot q - \dot L_\text{des}\|^2$$

其中 $A_\omega$ 是 CMM 的角动量行（下 3 行）。TSID 提供 TaskAngularMomentumEquality 类。

如果不控制角动量会怎样？ - 跳跃后空中翻转（like a cat without righting reflex） - 着陆时力矩不平衡导致摔倒 - 跑步时上半身摆动过大

8.9 与 MPC 的级联架构¶

典型腿足控制栈：

步态规划 (5-10 Hz)
    ↓ 接触序列 + 落脚点
MPC (30-100 Hz)
    ↓ 期望 CoM 轨迹 + 足力
WBC/TSID QP (200-1000 Hz)
    ↓ 关节扭矩 τ
底层电机驱动

MPC 给出"宏观目标"（在哪里踩、质心怎么动），WBC 把它变成满足所有约束的"微观执行"（具体每个关节出多少力矩）。约束动力学是 WBC 层的数学语言。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：浮基 S 矩阵方向搞反 - Pinocchio 约定：q 的前 7 个元素是 xyz+quaternion，v 的前 6 个是 linear+angular velocity - 某些库（如 Drake）约定相反（angular 在前） - 不看源码就写 S 必错

⚠️ 编程陷阱：J_c 选错 ReferenceFrame - LOCAL：Jacobian 表达于接触帧。摩擦锥在此帧定义 - LOCAL_WORLD_ALIGNED：原点在接触点，轴与世界平行。适合 Cartesian 误差 - 混用是 Pinocchio 用户最高频 bug

练习¶

对一个简单的 2D 三连杆浮基机器人（1 足接触），写出完整的 WBC QP（目标 + 约束），明确矩阵维度。
跨章综合题：结合 §3 的 KKT 推导 + §6 的摩擦锥 + §8 的 WBC 框架，用 Python/Pinocchio 为 SOLO 四足（12 关节）组装一个站立平衡的 QP。验证：松开一只脚后另外三只脚的接触力如何重新分配。
解释 MPC → WBC 级联中，为什么 MPC 频率可以低（30Hz）而 WBC 必须高（1kHz）。

本章小结¶

知识点	核心内容	关键公式/方法	应用场景
约束分类	holonomic/nonholonomic, bilateral/unilateral	Frobenius 定理	建模第一步
DAE index	index-3 drift-off	Petzold 定理	选择积分器
KKT 系统	鞍点结构 + Schur 消元	$[M,-J^\top;J,0][q̈;λ]=..$	所有约束求解
Baumgarte	临界阻尼稳定化	$α=β$, $αh≤1$	数值稳定
闭链	cut-joint + loop closure	constrainedABA	并联机构
接触	LCP/NCP + 摩擦锥	Signorini + Coulomb	腿足控制
可微接触	软化/平滑/子梯度	MuJoCo/Dojo/Nimble	RL/TO
WBC	浮基 QP + HQP	$\min\\|J\ddot q-\ddot x\\|^2$	实时控制

§9 约束动力学在不同仿真器中的统一视角 ⭐⭐⭐¶

9.1 五大仿真器的约束处理对比¶

理解不同仿真器的约束处理策略，是选择工具和调试 sim-to-real gap 的关键。以下从**约束模型、求解器、可微性、接触表示**四个维度对比主流工具。

维度	Pinocchio 3	MuJoCo 3	Drake	Dojo	Nimble
约束模型	硬约束 + proximal	软约束（凸化）	SAP（半解析）	硬 NCP + κ-smooth	硬 LCP
求解器	Sparse Cholesky + ALM	Newton/CG/PGS	Newton + line-search	Primal-dual IPM	Pivoting
时间步进	连续 + 用户积分器	离散时步（内置）	离散时步	Variational integrator	连续 + Euler
可微性	IFT on KKT	MJX native jvp	SAP 解析梯度	κ-smooth IFT	LCP 子梯度
接触表示	RigidConstraint	geom pair + solref	Hydroelastic / point	最大坐标 NCP	capsule/mesh
典型用途	控制/TO/WBC	RL/仿真/MPC	规划/分析	研究/TO	研究/可微
语言	C++/Python	C/Python/JAX	C++/Python	Julia	C++/Python
许可	BSD-2	Apache-2	BSD-3	MIT	MIT

9.2 统一数学框架¶

尽管接口不同，所有仿真器在数学层面求解的都是**同一类问题**的不同近似：

\[\min_v \frac{1}{2}(v - v_\text{free})^\top M (v - v_\text{free}) \quad \text{s.t.} \quad \text{contact constraints}\]

区别在于"contact constraints"的表示： - Pinocchio：$J_c v = 0$（硬等式，通过 proximal 松弛） - MuJoCo：$y \in \mathcal{K}^*$（锥约束 + 软化 $R > 0$） - Drake SAP：$\sum_i \ell_i(J_i v)$（penalty 正则化 + Newton） - Dojo：$s \circ \lambda = \kappa\mathbf{e}$（互补条件平滑化） - Nimble：$0 \leq \lambda \perp (Av + b) \geq 0$（严格 LCP）

9.3 选择建议¶

应用场景	推荐工具	理由
实时腿足 WBC/MPC	Pinocchio 3 + ProxQP	最快约束动力学 (~8μs)
GPU 大规模 RL	MuJoCo / MJX	原生 GPU 并行
精确物理分析	Drake	Hydroelastic + 严格收敛
可微 TO/MPC 研究	Dojo / Pinocchio 3	κ-smooth / IFT 梯度
毕业论文验证	MuJoCo + Pinocchio	社区大、文档全

§10 历史演进脉络：约束动力学 300 年 ⭐⭐¶

10.1 经典时期（1743-1972）¶

年份	里程碑	贡献者	核心思想
1743	d'Alembert 原理	d'Alembert	虚功原理→受约束运动
1788	《分析力学》	Lagrange	Lagrange 乘子法
1829	最小约束原理	Gauss	加速度最小范数→凸优化根源
1879	可积判据	Frobenius	分布可积⟺involutive
1895	Painlevé 悖论	Painlevé	加速度层 LCP 可能无解
1897	非完整约化	Chaplygin	循环坐标约化含曲率
1939	控制性定理	Chow	非完整系统的可达性
1972	约束稳定化	Baumgarte	加反馈抑制漂移

10.2 计算时期（1982-2008）¶

年份	里程碑	贡献者	核心思想
1982	DAE≠ODE	Petzold	Index 理论与 drift-off
1985	GGL formulation	Gear-Leimkuhler-Gupta	双乘子消除漂移
1987	操作空间	Khatib	Delassus 算子/Schur 消元
1992	无乘子公式	Udwadia-Kalaba	伪逆显式解
1996	时步法	Stewart-Trinkle	冲量层 LCP
1996	DAE 专著	Hairer-Wanner	数值 DAE 理论完成
2006	凸接触	Anitescu	Coulomb→凸 QP/CCP
2008	RBDA 教材	Featherstone	O(N) 闭链算法

10.3 可微化时期（2014-2025）¶

年份	里程碑	贡献者/团队	核心思想
2014	MuJoCo 凸模型	Todorov	软约束+解析可逆
2021	Pinocchio 3 proximal	Carpentier-Budhiraja-Mansard	ALM+稀疏 Cholesky
2021	Nimble	Werling et al. (Stanford)	LCP 解析子梯度
2022	Dojo	Howell-LeCleac'h et al. (MIT)	κ-smooth NCP
2024	CI-MPC	Le Cleac'h et al.	接触隐式实时 MPC
2024	接触比较分析	Le Lidec-Jallet-Carpentier	系统评估 6 仿真器
2024	constrainedABA	Sathya-Carpentier	O(N) 约束前向动力学
2025	ProxDDP/Aligator	Jallet-Carpentier	PHR+DDP 实时 MPC

10.4 开放问题¶

多接触可微性：active-set 切换点的梯度质量仍不理想——需要更好的平滑化或子梯度策略
大规模并行：GPU 上的约束求解（MJX island decomposition）尚在早期
柔体+接触：FEM 柔体与刚体接触的统一求解框架（Drake Hydroelastic 是初步尝试）
长时接触：接触持续数秒时的能量漂移问题——variational integrator + contact 是前沿方向
Sim-to-Real gap：接触模型参数（μ, solref, solimp）的自动辨识

§11 核心公式速查表 ⭐¶

━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
约束层级         方程                          Index
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━
位置层          φ(q) = 0                       3
速度层          J(q)q̇ = 0                     2
加速度层         Jq̈ + J̇q̇ = 0                 1
Baumgarte       Jq̈ + J̇q̇ + 2αJq̇ + β²φ = 0   1(stab)
GGL             φ=0, Jv=0, q̇=v+J⊤η, Mv̇=f+J⊤λ  2(stab)
━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━━

KKT 主系统：
⎡M   −J⊤⎤⎡q̈⎤   ⎡τ − Cq̇ − g⎤
⎣J    0 ⎦⎣λ⎦ = ⎣   −J̇q̇    ⎦

Proximal KKT（Pinocchio 3）：
⎡M   −J⊤⎤⎡q̈⎤   ⎡ τ − h    ⎤
⎣J   μI ⎦⎣λ⎦ = ⎣−γ − μλᵏ  ⎦

Schur 消元：
λ = (JM⁻¹J⊤)⁻¹ · (JM⁻¹(τ−Cq̇−g) + J̇q̇)
q̈ = M⁻¹(τ − Cq̇ − g + J⊤λ)

Baumgarte 临界阻尼：
ë + 2αė + β²e = 0  →  α = β

浮基接触方程：
Mq̈ + Cq̇ + g = S⊤τ + J_c⊤λ,  S = [0 I_n]

质心动量：h_G = A_G(q)v

ZMP：p_ZMP = ((c−p)×mg + L̇)/((mg+mc̈)·n)

Capture Point (LIPM)：p_CP = c + ċ/ω, ω = √(g/z_c)

DCM：ξ = c + ċ/ω,  ξ̇ = ω(ξ − p_ZMP)

Udwadia-Kalaba：q̈ = f + M^(−½)(AM^(−½))⁺(b − Af)

IFT on KKT：∂(q̈,λ)/∂θ = −[KKT]⁻¹ ∂F/∂θ

CWC (矩形足 2X×2Y)：
  ‖f_t‖ ≤ μf_z,  |τ_x| ≤ Yf_z,  |τ_y| ≤ Xf_z

Signorini 互补：φ ≥ 0, λ ≥ 0, φ·λ = 0

Coulomb 摩擦锥：‖f_t‖ ≤ μf_n

§12 无约束 vs 约束动力学全维度对照 ⭐¶

维度	无约束（专题 4-2）	约束（本章）
方程	Mq̈ + h = τ	Mq̈ + h = S⊤τ + J⊤λ, Jq̈+J̇q̇=0
决策变量	q̈	(q̈, λ)
主算法	ABA O(n), CRBA O(n²)	constrainedABA, Schur, proximal
数值稳定	标准 ODE 积分器	Baumgarte / GGL / projection
数学结构	ODE on ℝ²ⁿ	DAE index-3 on 约束流形
奇异性	M ≻ 0 保证	J 可能秩亏 → proximal/pseudo-inverse
典型机器人	固定基机械臂	腿足、闭链、抓取
代表库	Pinocchio RNEA/ABA	Pinocchio constraintDynamics, MuJoCo
可微化	直接 IFT on ODE	IFT on KKT, active set 需子梯度
控制策略	计算力矩法	WBC/TSID QP + MPC
核心文献	Featherstone 2008 Ch.6-7	Featherstone Ch.8 + Carpentier 2021

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

项目：从零构建四足站立控制器

本章新增：约束动力学层

Ch1  加载 URDF → 关节空间表示
Ch2  正运动学 → 足端位置
Ch3  逆运动学 → 关节角目标
Ch4  PD 控制 → 关节力矩
Ch5  步态生成 → 接触序列
Ch6  约束动力学（本章新增）：
     ├── 浮基方程组装 (M, C, g, S)
     ├── 接触 Jacobian 构造 (J_c, LOCAL/LWA)
     ├── 摩擦锥线性化 (8-polygon)
     ├── WBC QP 组装与求解 (OSQP/ProxQP)
     └── 站立平衡验证

§13 约束动力学与控制的深层联系 ⭐⭐⭐¶

13.1 约束动力学作为 WBC 的语言¶

WBC（Whole-Body Control） 不仅仅"使用"约束动力学——它**就是**约束动力学的优化求解。以下是这种对应关系的完整展开：

WBC 概念	约束动力学概念	数学表达
任务空间加速度	KKT 解的投影	$\ddot x = J\ddot q + \dot J\dot q$
接触力分配	Lagrange 乘子的选择	$\lambda \in \arg\min\\|\lambda\\|$ s.t. EoM
零空间运动	ker(J) 中的自由度	$\ddot q_\text{null} \in \ker(J_\text{task})$
优先级	嵌套 KKT	先解高优先→在其零空间解低优先
摩擦锥	不等式约束	$\lambda \in \mathcal{FC}(\mu)$
力矩限制	决策变量界约束	$\tau_\min \leq \tau \leq \tau_\max$

13.2 从 Gauss 原理看 WBC 的最优性¶

WBC QP：$\min_{\ddot q} \|J\ddot q + \dot J\dot q - \ddot x^\text{des}\|^2$ s.t. EoM + constraints

几何解释：这就是 Gauss 最小约束原理的推广！ - Gauss (1829)：在约束面的切空间中，选离自由加速度最近的点 - WBC (2014)：在约束面的切空间中，选离期望任务加速度最近的点

区别仅在于"距离"的度量——Gauss 用 M-范数，WBC 用任务权重 W。

本质洞察：WBC 不是"发明了一种新控制方法"——它是 Gauss 1829 年思想在多任务、有不等式约束场景下的现代推广。理解这个历史联系，就不会把 WBC 当作 black-box 优化，而是能从物理第一性原理理解它的行为。

13.3 约束动力学与 MPC 的关系¶

MPC 求解的是**带约束动力学的最优控制问题**：

\[\min_{u_{0:N-1}} \sum_{k=0}^{N-1} l(x_k, u_k) + l_f(x_N)$$ $$\text{s.t.} \quad x_{k+1} = f_\text{constrained}(x_k, u_k), \quad h(x_k, u_k) \leq 0\]

其中 $f_\text{constrained}$ 就是**约束动力学正向积分**——即本章 KKT 系统的求解结果。

约束动力学在 MPC 中的角色： 1. 状态传播：给定 $(q_k, v_k, \tau_k)$，用约束动力学计算 $(q_{k+1}, v_{k+1})$ 2. 灵敏度计算：$\partial x_{k+1}/\partial u_k$，$\partial x_{k+1}/\partial x_k$——就是 §21 的 IFT on KKT 3. 约束满足：MPC 的约束（摩擦锥、关节限制）来自约束动力学的物理限制

没有高效的约束动力学，就没有实时 MPC。 Pinocchio 3 的 constrainedABA (~8μs) 使得 1kHz WBC 和 100Hz MPC 在同一处理器上成为可能。

13.4 Sim-to-Real Gap 的约束动力学根源¶

Sim-to-real gap 中，**接触模型参数不匹配**是最大的 gap 来源。具体地：

参数	仿真	实际	影响
摩擦系数 μ	0.7（猜测）	0.4-1.0（变化）	滑动/不滑动判断错误
接触刚度	MuJoCo solref	材料弹性	穿透深度/弹跳行为
阻尼	MuJoCo solimp	材料内耗	碰撞恢复系数
接触几何	简化凸包	真实形状	接触点位置/法向

Domain Randomization（DR）通过随机化这些参数来桥接 gap——本质上是在"约束模型参数"空间上做鲁棒优化。

§14 Python 验证实验 ⭐⭐¶

14.1 实验 1：Baumgarte 稳定化参数扫描¶

import numpy as np
import matplotlib.pyplot as plt

def simulate_pendulum_baumgarte(alpha, beta, h=0.001, T=5.0):
    """单摆 + Baumgarte 稳定化, 返回约束违反历史"""
    L = 1.0; g = 9.81; m = 1.0
    q = np.array([1.0, 0.0])
    v = np.array([0.0, -1.0])
    violations = []
    for _ in range(int(T/h)):
        phi = q[0]**2 + q[1]**2 - L**2
        J = 2*q
        Jdot_v = 2*np.dot(v, v)
        gamma = -Jdot_v - 2*alpha*np.dot(J, v) - beta**2*phi
        f = np.array([0, -m*g])
        JMinvJT = np.dot(J, J) / m
        lam = (np.dot(J, f/m) - gamma) / JMinvJT
        a = (f + lam*J) / m
        v = v + h*a
        q = q + h*v
        violations.append(abs(phi))
    return np.array(violations)

# 对比三种阻尼条件
fig, axes = plt.subplots(1, 3, figsize=(15, 4))
configs = [(10, 10, "临界阻尼"), (5, 10, "欠阻尼"), (50, 10, "过阻尼")]
for ax, (a, b, title) in zip(axes, configs):
    v = simulate_pendulum_baumgarte(a, b)
    ax.semilogy(v); ax.set_title(f"{title} (α={a}, β={b})")
plt.tight_layout(); plt.savefig("baumgarte_comparison.png")

14.2 实验 2：验证 Drift-off O(t^2) 增长¶

def simulate_no_stabilization(h=0.001, T=2.0):
    """无稳定化, 验证位置约束 O(t^2) 漂移"""
    L = 1.0; g = 9.81; m = 1.0
    q = np.array([1.0, 0.001])  # 微小初始违反
    v = np.array([0.0, -1.0])
    violations = []
    for _ in range(int(T/h)):
        J = 2*q; Jdot_v = 2*np.dot(v, v)
        gamma = -Jdot_v  # 无 Baumgarte 项
        f = np.array([0, -m*g])
        JMinvJT = np.dot(J, J) / m
        lam = (np.dot(J, f/m) - gamma) / JMinvJT
        a = (f + lam*J) / m
        v = v + h*a; q = q + h*v
        violations.append(q[0]**2 + q[1]**2 - L**2)
    return np.array(violations)

v = simulate_no_stabilization()
t = np.arange(len(v))*0.001
plt.loglog(t[10:], np.abs(v[10:]), label="实际漂移")
plt.loglog(t[10:], 1e-5*t[10:]**2, '--', label="O(t^2)")
plt.legend(); plt.title("验证 Petzold drift-off effect")

14.3 实验 3：Schur vs Dense KKT 性能对比¶

import time
from scipy.linalg import cho_factor, cho_solve

def benchmark_solvers(n=30, m=12, trials=5000):
    """对比密集 LDL vs Schur 方法"""
    np.random.seed(42)
    A = np.random.randn(n, n)
    M = A @ A.T + np.eye(n)  # 正定
    J = np.random.randn(m, n)
    rhs_q = np.random.randn(n)
    rhs_c = np.random.randn(m)

    # Dense: 组装 KKT 整体求解
    KKT = np.block([[M, -J.T], [J, np.zeros((m,m))]])
    rhs = np.concatenate([rhs_q, rhs_c])
    t0 = time.perf_counter()
    for _ in range(trials):
        x = np.linalg.solve(KKT, rhs)
    t_dense = (time.perf_counter()-t0)/trials*1e6

    # Schur: 先解 lambda 再解 ddq
    c = cho_factor(M)
    t0 = time.perf_counter()
    for _ in range(trials):
        Minv_J = cho_solve(c, J.T)
        S = J @ Minv_J
        lam = np.linalg.solve(S, J@cho_solve(c,rhs_q) - rhs_c)
        ddq = cho_solve(c, rhs_q + J.T @ lam)
    t_schur = (time.perf_counter()-t0)/trials*1e6

    print(f"n={n}, m={m}: Dense={t_dense:.0f}us, Schur={t_schur:.0f}us")

benchmark_solvers(30, 12)  # humanoid 模型
benchmark_solvers(50, 24)  # 四足全接触

§15 约束动力学的符号计算验证 ⭐⭐¶

15.1 SymPy 验证 KKT 推导¶

符号计算工具（参考 00_项目导航/符号计算工具指南.md）在约束动力学中的典型应用：

import sympy as sp

# 定义符号
q1, q2, dq1, dq2, ddq1, ddq2 = sp.symbols('q1 q2 dq1 dq2 ddq1 ddq2')
lam = sp.Symbol('lambda')
m, g, L = sp.symbols('m g L', positive=True)

# 单摆约束: phi = q1^2 + q2^2 - L^2 = 0
phi = q1**2 + q2**2 - L**2

# 约束 Jacobian
J = sp.Matrix([phi]).jacobian(sp.Matrix([q1, q2]))
print(f"J = {J}")  # [2*q1, 2*q2]

# J_dot * q_dot
q = sp.Matrix([q1, q2])
dq = sp.Matrix([dq1, dq2])
Jdot = J.jacobian(q) * dq  # ∂J/∂q * q̇ 
# 对向量化: J_dot_dq = 2*(dq1^2 + dq2^2)
Jdot_dq = sp.diff(J * dq, q1)*dq1 + sp.diff(J * dq, q2)*dq2
print(f"J_dot * dq = {sp.simplify(Jdot_dq)}")

# KKT 系统验证
M = m * sp.eye(2)
f_ext = sp.Matrix([0, -m*g])
tau = sp.Matrix([0, 0])  # 无驱动

# 组装 KKT
KKT_matrix = sp.BlockMatrix([
    [M, -J.T],
    [J, sp.zeros(1, 1)]
])
KKT_rhs = sp.Matrix([f_ext[0], f_ext[1], -Jdot_dq[0]])

# 求解
sol = sp.linsolve((sp.Matrix(KKT_matrix), KKT_rhs), ddq1, ddq2, lam)
print(f"λ (约束力/绳张力) = {sp.simplify(list(sol)[0][2])}")

15.2 Baumgarte 误差方程的符号验证¶

# 验证 Baumgarte 特征方程
alpha, beta, s = sp.symbols('alpha beta s')
char_eq = s**2 + 2*alpha*s + beta**2
roots = sp.solve(char_eq, s)
print(f"特征根: {roots}")
# 输出: [-alpha + sqrt(alpha^2 - beta^2), -alpha - sqrt(alpha^2 - beta^2)]

# 临界阻尼条件
critical = sp.solve(alpha**2 - beta**2, alpha)
print(f"临界阻尼: alpha = {critical}")
# 输出: [-beta, beta], 取正值 alpha = beta

符号计算的价值：在推导 KKT 系统变体（如含 Baumgarte 项、含摩擦锥线性化）时，手推容易出错。用 SymPy 验证每一步的符号正确性，特别是 $\dot J\dot q$ 的展开（容易漏项）。

§16 常见陷阱完整清单（15 条） ⭐¶

直接积分 index-3 DAE → 二次漂移，必须降 index + 稳定化
Baumgarte 过阻尼 → 未考虑步长约束 $\alpha h \leq 1$
J_c ReferenceFrame 混用 → 最高频 Pinocchio bug
LICQ 未检查 → KKT 奇异，需 proximal 正则化
浮基 S 矩阵方向 → 不同库约定不同，必查源码
CoP 当 ZMP → 多接触时 COP ≠ ZMP
接触维度不一致 → humanoid 足应 6D 非 3D
摩擦锥线性化过粗 → 4-pyramid 对大 μ 误差 >29%
MuJoCo solref 混淆 → solref = (timeconst, dampratio) ≠ (α, β)
ProximalSettings.mu 不当 → 过小不稳，过大伪滑移
Active-set 切换 → 梯度不连续需子梯度处理
URDF 无闭链 → 必须 SDF 或手工 RigidConstraintModel
忽略 J_dot 中的速度项 → $\dot J$ 不可省略
接触检测频率不足 → 快速运动穿过薄障碍物
活跃集外的导数无效 → IFT 仅在不切换区间成立

延伸阅读¶

资源	类型	难度	说明
Featherstone 2008 RBDA Ch.8	教材	⭐⭐⭐⭐	闭链最权威
Hairer-Wanner 1996 Solving ODEs II Ch.VII	教材	⭐⭐⭐⭐⭐	DAE 理论圣经
Carpentier-Budhiraja-Mansard RSS 2021	论文	⭐⭐⭐⭐	Pinocchio 3 proximal
Howell et al. 2022 "Dojo" (arXiv:2203.00806)	论文	⭐⭐⭐⭐	κ-smooth 可微接触
Todorov ICRA 2014	论文	⭐⭐⭐	MuJoCo 约束模型
Brogliato 2016 Nonsmooth Mech. 3e	教材	⭐⭐⭐⭐⭐	非光滑力学全书
Wensing et al. TRO 2023	综述	⭐⭐⭐	优化控制综述
Jallet-Carpentier TRO 2025 (ProxDDP)	论文	⭐⭐⭐⭐	最新 DDP+约束
Le Cleac'h et al. TRO 2024 (CI-MPC)	论文	⭐⭐⭐⭐	接触隐式 MPC
Bloch 2015 Nonholonomic Mech.	教材	⭐⭐⭐⭐	非完整力学

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
约束违反 $\varphi$ 随时间二次增长	使用了 index-1 形式但无稳定化	1.检查是否加了 Baumgarte 项 2.打印 $\varphi$ 和 $J\dot q$ 3.加稳定化或换 projection	§2, §4
Baumgarte 后约束违反振荡	欠阻尼（$\alpha < \beta$）	1.检查 $\alpha/\beta$ 比值 2.设为临界阻尼 $\alpha = \beta$ 3.验证 $\alpha h \leq 1$	§4
KKT 求解返回 NaN/Inf	LICQ 违反（J 秩亏）	1.打印 $\sigma_\min(J)$ 2.检查是否在奇异构型 3.加 proximal $\mu I$	§3
WBC QP 无可行解	摩擦锥/力矩限制过紧	1.松弛摩擦系数 2.检查 $\tau$ 限制是否合理 3.打印各约束余量	§8
闭链机器人动力学异常	闭链约束未激活	1.检查 `RigidConstraintModel` 是否正确构造 2.确认 joint_id 和 placement 3.验证整机 DOF	§5
接触力在切换时跳变	active set 不连续切换	1.打印接触力历史 2.加 Baumgarte 平滑过渡 3.检查步态时序	§6
MuJoCo 仿真穿透地面	solref 参数过松	1.减小 `solref[0]`（timeconst） 2.检查 `solimp` 3.增大迭代次数	§7

§17 KKT 求解器路线详解 ⭐⭐⭐¶

17.1 动机：选择正确的求解器¶

KKT 系统 $[M, -J^\top; J, 0][\ddot q; \lambda] = [...]$ 是约束动力学的核心方程。不同的求解策略在**速度、精度、鲁棒性**方面有天壤之别。选错求解器可能导致：实时系统超时（密集方法对大系统）、精度不足（迭代方法过早终止）、奇异构型崩溃（无正则化）。

17.2 方法对比¶

方法	算法	复杂度	代表实现	适用场景
密集 LDL⊤	Bunch-Kaufman 分解	$O((n_v+m)^3)$	Eigen LDLT	小型系统 (<50 DOF)
Range-space Schur	解 $JM^{-1}J^\top\lambda = ...$	$O(n^2 + m^3)$	Khatib 1987	中型，约束少
Null-space Schur	投影到 $\ker(J)$	$O(n^2 + m^3)$	Mistry-Righetti 2012	冗余系统
Proximal (Pinocchio 3)	稀疏 Cholesky + ALM	$O(n_v + m)$	Carpentier RSS 2021	大型，秩亏鲁棒
Newton (MuJoCo)	精确 Hessian + line-search	2-5 迭代/步	Todorov 2014	通用，中等规模
CG (MuJoCo 3)	Conjugate Gradient	稀疏友好	MuJoCo 3 默认	大规模稀疏
PGS	Projected Gauss-Seidel	10 sweeps	经典 LCP	实时低精度
SAP (Drake)	Semi-Analytic Primal Newton	全局收敛	Castro et al. 2021	Drake 默认

17.3 Pinocchio 3 Proximal 方法详解¶

正则化 KKT 系统：

\[\begin{bmatrix} M & -J^\top \\ J & \mu I\end{bmatrix}\begin{bmatrix}\ddot q^{k+1} \\ \lambda^{k+1}\end{bmatrix} = \begin{bmatrix}S^\top\tau - h \\ -\gamma - \mu\lambda^k\end{bmatrix}\]

为什么加 $\mu I$？ 1. 使左下块从 $[J, 0]$ 变成 $[J, \mu I]$，整体矩阵从不定变为正定（当 $\mu$ 足够大） 2. 即使 $J$ 秩亏（LICQ 违反），$(JM^{-1}J^\top + \mu I)$ 仍可逆 3. $\mu \to 0$ 时收敛到精确解；$\mu > 0$ 给出最小二乘解

ALM 迭代更新： $$\lambda^{(k+1)} = \lambda^{(k)} + \frac{1}{\mu}(J\ddot q^{(k+1)} + \gamma)$$

典型 3-5 次迭代收敛。ProximalSettings.mu 取值： - 正常情况：$10^{-12}$ ~ $10^{-8}$ - 秩亏时（膝盖伸直等）：增到 $10^{-4}$ - 太大：引入伪滑移；太小：数值不稳

17.4 性能基准数据¶

Pinocchio 3（Sathya-Carpentier TRO 2024，Intel i7-7700K 单线程）： - Atlas (n=36, 12 约束)：constrainedABA 5-7 μs；naive dense KKT ~30 μs - Talos (n_v=38, 2×6D 足)：proximal sparse ~8 μs - constrained derivatives：~25 μs（比 finite difference 快 50-100×）

MuJoCo（Todorov 2014）：27-DOF humanoid + 10 接触：forward ~0.1 ms

Drake SAP：Cassie full body + 4 contacts：~0.2 ms

17.5 Gauss 原理与 Udwadia-Kalaba 方程¶

Gauss 最小约束原理（1829）：受理想约束的真实加速度在允许加速度集中最小化"Gauss 函数"：

\[Z(\ddot q) = \frac{1}{2}(\ddot q - f)^\top M(\ddot q - f), \quad f = M^{-1}(\tau - h)\]

即在 M-范数下，约束加速度是"离自由加速度最近的"。

Udwadia-Kalaba 方程（1992）：给定线性约束 $A\ddot q = b$，无需 Lagrange 乘子的显式解：

\[\boxed{\ddot q = f + M^{-1/2}(AM^{-1/2})^+(b - Af)}\]

其中 $(·)^+$ 是 Moore-Penrose 伪逆。

推导：令 $x = M^{1/2}(\ddot q - f)$，约束变为 $AM^{-1/2}x = b - Af$。Gauss 最小化 $\|x\|^2$ subject to 线性约束，最小范数解为 $x = (AM^{-1/2})^+(b - Af)$。代回得 Udwadia-Kalaba 方程。

本质洞察：Gauss 原理是凸接触模型（MuJoCo、Anitescu、Dojo）的**物理合法性源头**。它们都以某种形式解 Gauss 函数的最小化——只是约束集不同（等式约束 vs 锥约束 vs 互补约束）。

与 Pinocchio proximal 的桥梁：$\mu \to 0$ 时 $(JM^{-1}J^\top + \mu I)^{-1} \to (JM^{-1}J^\top)^+$，即正则化伪逆收敛到 Udwadia-Kalaba 的伪逆。

17.6 IFT on KKT：约束动力学的解析导数¶

设 KKT 残差 $F(z, \theta) = 0$，$z = (\ddot q, \lambda)$，$\theta \in \{q, v, \tau\}$：

\[F(z,\theta) = \begin{bmatrix}M\ddot q + h - S^\top\tau - J^\top\lambda \\ J\ddot q + \gamma\end{bmatrix} = 0\]

隐函数定理： $$\frac{\partial z}{\partial\theta} = -\underbrace{\begin{bmatrix} M & -J^\top \\ J & 0\end{bmatrix}^{-1}}_{\text{已在 forward 分解过}} \frac{\partial F}{\partial\theta}$$

核心 insight：KKT 分解既用于 forward dynamics 也用于 derivative computation，每个方向的代价仅为一次回代 $O((n_v+m)^2)$。Pinocchio 3 的 computeConstraintDynamicsDerivatives 实现了这一点。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：ProximalSettings.mu 取值不当 - 过小（$10^{-15}$）：数值不稳定，除法接近 0/0 - 过大（$10^{-2}$）：引入明显伪滑移，约束不精确 - 正确范围：正常 $10^{-10}$ ~ $10^{-8}$，秩亏时 $10^{-4}$

§18 KKT 求解的数值稳定性深入分析 ⭐⭐⭐¶

18.1 鞍点系统的条件数¶

KKT 系统 $\begin{bmatrix}M & -J^\top\\J & 0\end{bmatrix}$ 的条件数取决于 $M$ 和 $J$ 的谱性质。

最坏情况：当 $J$ 的最小奇异值 $\sigma_\min(J) \to 0$（接近奇异构型），条件数 $\kappa \to \infty$。此时 direct solve（LDL⊤）的数值误差可能大到使结果无意义。

正则化的效果：Proximal 方法加 $\mu I$ 后，条件数变为： $$\kappa \leq \frac{\|M\| + \|J\|^2/\mu}{\lambda_\min(M) + \mu}$$

当 $\sigma_\min(J)$ 很小时，条件数从 $O(1/\sigma_\min^2)$ 降为 $O(1/\mu)$。

18.2 Schur 补 vs 直接法的数值对比¶

**Range-space Schur 方法**的瓶颈在 $JM^{-1}J^\top$ 的求逆。当 $M$ 对角占优（如用 lumped inertia 近似）时，$M^{-1}$ 计算精确，Schur 方法稳定。

**Null-space Schur 方法**先解零空间投影 $N = I - J^\dagger J$，再在零空间内解降维 ODE。适用于约束数 $m$ 接近自由度 $n$ 的情况（如高度约束的并联机构）。

类比：Range-space 像"先解约束力再算运动"；Null-space 像"先确定可行运动方向再在其中优化"。前者适合约束少（$m \ll n$）的情况，后者适合约束多（$m \approx n$）的情况。

18.3 迭代求解器（CG/MINRES）在大规模场景中的应用¶

当 DOF 超过 100（如 humanoid 全身 + 多接触点），直接法的 $O(n^3)$ 可能不够快。MuJoCo 3 的默认求解器 CG（Conjugate Gradient）利用了 KKT 矩阵的稀疏性：

优势： - 不显式构造 KKT 矩阵，只需矩阵-向量乘（$O(n)$ for sparse） - 可自然并行化（GPU 友好） - 收敛到 $\epsilon$ 精度只需 $O(\sqrt{\kappa}\log(1/\epsilon))$ 次迭代

劣势： - 收敛速度依赖条件数——接触多时可能需要预条件 - 不保证精确解——只是近似

§19 Vakonomic vs Nonholonomic 力学 ⭐⭐⭐⭐¶

19.1 两种约束处理范式¶

对于非完整约束 $A(q)\dot q = 0$，存在两种截然不同的动力学方程推导方式：

Nonholonomic（d'Alembert 范式，物理正确）：虚位移在约束分布内取值（$\delta q \in \ker A$），但**变分路径不必满足约束**。得： $$\frac{d}{dt}\frac{\partial L}{\partial\dot q} - \frac{\partial L}{\partial q} = A^\top\lambda$$

Vakonomic（Kozlov 范式，数学等价形式）：变分限制在满足约束的路径上（$A\dot q = 0$ 对变分也成立）。对 $\tilde L = L - \lambda^\top A\dot q$ 做无约束 Euler-Lagrange。

关键区别：两者**一般不等价**。只有当约束是 holonomic 时，两者给出相同方程。对 nonholonomic 约束，d'Alembert 是物理正确的（实验可验证），Vakonomic 是数学上有用的（与最优控制等价）。

19.2 Chaplygin 约化¶

设定：配置空间分为 $(r^\alpha, s^a)$，Lagrangian 和约束都不依赖 $s^a$（循环坐标）。约束形式：$\dot s^a = -A^a_\alpha(r)\dot r^\alpha$。

代回 $L$ 得 reduced Lagrangian $L_c(r, \dot r)$，但方程含**曲率项**（Ehresmann 连接曲率 $B^a_{\alpha\beta}$）：

\[\frac{d}{dt}\frac{\partial L_c}{\partial\dot r^\alpha} - \frac{\partial L_c}{\partial r^\alpha} = -\frac{\partial L}{\partial\dot s^a}B^a_{\alpha\beta}\dot r^\beta\]

这揭示了"cyclic 但无动量守恒"的非完整特有现象。

19.3 控制性：Chow-Rashevsky 定理¶

定理：设 $M$ 连通，分布 $\Delta = \text{span}\{g_1, ..., g_m\}$ 是 bracket-generating（$\text{Lie}(g_1,...,g_m)(q) = T_qM$），则任两点可由水平曲线连接。

与 Frobenius 的对偶关系： - Frobenius $\Rightarrow$ 可积 $\Rightarrow$ holonomic $\Rightarrow$ 运动被限制在子流形上 - Chow $\Rightarrow$ 不可积 $\Rightarrow$ nonholonomic $\Rightarrow$ 可控到全空间

机器人应用：独轮车 2 维控制输入可到达 3 维配置空间的任意点——因为 $[\partial/\partial v, \partial/\partial\omega]$ 生成了第三个方向。

§20 时步法深入：Stewart-Trinkle 与 Moreau 格式 ⭐⭐⭐⭐¶

20.1 为什么加速度层 LCP 不够¶

加速度层 LCP 有两个根本缺陷： 1. Painlevé 悖论（1895）：某些构型下加速度层 LCP 无解或多解 2. 碰撞处理：加速度层无法处理速度不连续（impact）

时步法绕过这些问题：在**冲量层**（离散时间步）求解，自然统一了持续接触和碰撞。

20.2 Stewart-Trinkle 时步法¶

离散动力学： $$M(v^+ - v^-) = h\,f_\text{ext} + J_n^\top p_n + J_t^\top p_t$$

其中 $p_n, p_t$ 是法向和切向接触冲量。

离散 Signorini： $$0 \leq p_n \perp \phi_n^+ \geq 0$$

离散 Coulomb（最大耗散原则）： $$p_t \in -\mu p_n \cdot \partial\|v_t^+\|$$

整体形成一个 Mixed LCP 或 CCP（Cone Complementarity Problem）。

20.3 Moreau 半隐格式¶

\[\tilde v = v_k + h\,M^{-1}f(q_k, v_k)$$ $$v_{k+1} = \text{proj}_{K(q_k)}^M(\tilde v)$$ $$q_{k+1} = q_k + h\,v_{k+1}\]

其中 $K$ 是速度容许集（由接触法向和摩擦锥定义），$\text{proj}^M$ 是 M-范数下的投影。

优势： - 无需事件检测（自动处理 make/break/slide/stick 切换） - 一步一解（不需要多阶段积分） - 存在性保证（凸投影总有解）

20.4 Anitescu 凸化¶

核心思想（Anitescu 2006）：把 Coulomb 时间步问题凸松弛为严格凸 QP/CCP：

\[\min_{v^+} \frac{1}{2}(v^+ - \tilde v)^\top M(v^+ - \tilde v) \quad \text{s.t.} \quad \Phi_i(v^+) \in \mathcal{FC}_i^*\]

slipping 时精确
sticking 时有小 artificial slip（$h \to 0$ 时消失）
MuJoCo、Dojo、Drake、Chrono 的共同凸根基

20.5 收敛性分析¶

Stewart (2000) 证明了：时步法的离散解序列在 $h \to 0$ 时收敛到 measure differential inclusion（MDI） 的解：

\[M\,dv + h(q,v)\,dt \in N_{K(q)}(v)\,dt\]

这给出了时步法的数学合法性——它不是"数值 hack"，而是有严格数学基础的方法。

§21 接触动力学的解析导数 ⭐⭐⭐⭐¶

21.1 动机：可微仿真需要梯度¶

可微仿真（differentiable simulation）是 2020-2025 年的热门方向。目标：给定参数 $\theta$（如物理参数、初始条件、控制输入），计算仿真输出 $y = \text{sim}(\theta)$ 对 $\theta$ 的导数 $\partial y/\partial\theta$。

21.2 IFT 方法¶

当 active set 确定时（已知哪些接触激活），KKT 系统是光滑方程组 $F(z, \theta) = 0$。由隐函数定理：

\[\frac{\partial z}{\partial\theta} = -\left(\frac{\partial F}{\partial z}\right)^{-1}\frac{\partial F}{\partial\theta}\]

计算流程： 1. Forward：解 KKT 得 $(z^*, \lambda^*)$，同时获得 KKT 分解 2. Backward：对每个参数方向 $\theta_i$，计算 $\partial F/\partial\theta_i$，回代 KKT 分解

复杂度：forward $O((n+m)^3)$ 一次 + backward $O((n+m)^2)$ 每方向。

21.3 Active-set 切换的处理¶

切换点（接触建立/释放/stick→slip）处 $\partial F/\partial z$ 不连续。三种处理方式：

子梯度（Nimble）：选左导或右导之一
平滑化（Dojo）：κ-smooth 使 KKT 处处可微
事件检测 + 分段 IFT：检测切换时刻，各段内用 IFT

21.4 Pinocchio 3 实现¶

// 求解约束动力学
constraintDynamics(model, data, q, v, tau, cms, cds, settings);

// 计算解析导数（复用 KKT 分解）
computeConstraintDynamicsDerivatives(model, data, cms, cds, settings);

// 获取结果
MatrixXd ddq_dq = data.ddq_dq;   // ∂q̈/∂q
MatrixXd ddq_dv = data.ddq_dv;   // ∂q̈/∂v  
MatrixXd ddq_dtau = data.ddq_dtau; // ∂q̈/∂τ
MatrixXd dlambda_dq = data.dlambda_dq; // ∂λ/∂q

性能：Talos (n_v=38)：constrained derivatives ~25 μs，比 finite difference 快 50-100×。

§22 多体接触图与 Island Decomposition ⭐⭐⭐¶

22.1 动机：大规模场景的并行化¶

当场景中有多个不相互接触的物体组时，它们的约束 Jacobian 是**块对角的**——各组可以独立求解。MuJoCo 3.0 (2023-12) 引入了 island decomposition：

检测 scene 中互不交互的子组件（island）
每个 island 的约束 Jacobian 独立
并行求解各 island

22.2 性能提升¶

基准（MuJoCo 3.0 release notes）：22 个 humanoid 场景，island decomposition 带来 3× 加速。

API：mjtEnableBit::mjENBL_ISLAND；mjData.solver 变为 mjNISLAND × mjNSOLVER 矩阵。

22.3 对 RL 训练的意义¶

Isaac Lab / MuJoCo Playground 的大规模并行 RL 训练（数千个环境同时运行）正是利用了 island 分解。每个训练环境是一个独立的 island，GPU 上可高效批量处理。

§23 非完整约束的控制（Chained Form）⭐⭐⭐¶

23.1 典型非完整系统¶

差速驱动（TurtleBot）： $$\dot x = v\cos\theta, \quad \dot y = v\sin\theta, \quad \dot\theta = \omega$$

约束 $\dot x\sin\theta - \dot y\cos\theta = 0$，输入 $(v, \omega)$。

Ackermann 转向（自动驾驶）： $$\dot x = v\cos\theta, \quad \dot y = v\sin\theta, \quad \dot\theta = \frac{v}{L}\tan\phi$$

约束更复杂，涉及最小转弯半径。

23.2 Chained Form 标准化¶

Murray-Sastry 1993 证明了大类非完整系统可变换为 chained form： $$\dot z_1 = u_1, \quad \dot z_2 = u_2, \quad \dot z_3 = z_2 u_1, \quad \dot z_4 = z_3 u_1, \quad \cdots$$

在此标准形下，路径规划和反馈控制有系统方法（如 sinusoidal inputs、polynomial planning）。

23.3 与腿足机器人的联系¶

虽然腿足机器人的接触约束是 holonomic 的（位置层），但在**步态切换**时，可用非完整思想： - 单支撑相：swing leg 自由 - 双支撑相：整机被约束为"quasi-static crawl"

这种"切换约束"的框架在 hybrid zero dynamics（HZD） 中得到了系统化。

23.4 与 WBC 的衔接¶

非完整约束在 WBC 框架中的处理方式与完整约束有本质区别：

约束类型	WBC 中的处理	约束行数	乘子物理含义
完整 bilateral	$J\ddot q + \dot J\dot q = 0$	位置+速度+加速度	约束力
完整 unilateral	$J\ddot q + \dot J\dot q \geq 0$ + 互补	同上但条件激活	接触力
非完整	$A(q)\dot q = 0$ 仅速度层	仅速度层	约束力方向

关键区别：非完整约束**不能对时间微分**来得到加速度层约束——因为 $\dot A\dot q + A\ddot q = 0$ 包含的约束力方向不正确（它不来自虚功原理的正确应用）。

正确的做法是用 d'Alembert-Lagrange 方程直接处理 Pfaffian 约束，或在 WBC 中把非完整约束作为**速度层约束**处理（某些 QP 求解器支持）。

§24 完整推导示例：Cassie 四杆联动的约束动力学 ⭐⭐⭐¶

24.1 系统描述¶

Cassie（Agility Robotics）每腿有一个四杆联动结构： - Thigh：髋关节到膝关节 - Shin：膝关节到踝关节 - Achilles：平行于 shin 的连杆 - Heelspring：足跟弹簧

四杆联动使得膝关节弯曲时脚保持水平（自动适应地形）。

24.2 Spanning tree + Cut joint¶

选择切开 achilles 的下端与 foot 的连接点（球铰/revolute）： - Spanning tree：hip → thigh → shin → foot → (heelspring → ...) - Cut joint：achilles 下端 ↔ foot 上端 - 约束维度：5D revolute loop closure（6D rigid - 1D 旋转自由度）

24.3 约束方程¶

设 achilles 下端在世界坐标系中的位姿为 ${}^0T_{A}(q)$，foot 上端为 ${}^0T_{B}(q)$：

\[C(q) = {}^0T_A^{-1}(q) \cdot {}^0T_B(q) = I\]

取 5 维约束（去掉允许的旋转轴方向）： $$\varphi(q) = \text{extract\_5D}(C(q) - I) = 0 \in \mathbb{R}^5$$

24.4 Pinocchio 实现代码¶

#include <pinocchio/algorithm/constrained-dynamics.hpp>
#include <pinocchio/parsers/urdf.hpp>

// 加载 Cassie URDF (tree 部分)
pinocchio::Model model;
pinocchio::urdf::buildModel("cassie_tree.urdf", model);
pinocchio::Data data(model);

// 手动添加四杆联动闭链约束
// 每腿一个 5D revolute loop closure
pinocchio::RigidConstraintModel cm_left(
    pinocchio::CONTACT_6D,  // 先用 6D，后续可约化
    model,
    model.getJointId("left_achilles_bottom"),
    pinocchio::SE3::Identity(),  // achilles 下端帧
    model.getJointId("left_foot_top"),
    pinocchio::SE3::Identity(),  // foot 上端帧
    pinocchio::ReferenceFrame::LOCAL
);

pinocchio::RigidConstraintModel cm_right(/* 类似 */);

// 添加足端接触约束 (面接触 6D)
pinocchio::RigidConstraintModel cm_lf_contact(
    pinocchio::CONTACT_6D,
    model,
    model.getJointId("left_foot"),
    pinocchio::SE3::Identity(),
    0,  // 世界帧
    pinocchio::SE3(Eigen::Matrix3d::Identity(),
                    Eigen::Vector3d(0, 0, 0)),
    pinocchio::ReferenceFrame::LOCAL
);

// 组装所有约束
pinocchio::RigidConstraintModelVector cms{
    cm_left, cm_right, cm_lf_contact, cm_rf_contact};
pinocchio::RigidConstraintDataVector cds;
for (auto& cm : cms) cds.push_back(
    pinocchio::RigidConstraintData(cm));

// 设置 proximal 参数
pinocchio::ProximalSettings settings;
settings.mu = 1e-8;             // 正则化
settings.absolute_accuracy = 1e-10;
settings.max_iter = 10;

// 初始化并求解
pinocchio::initConstraintDynamics(model, data, cms);
pinocchio::constraintDynamics(
    model, data, q, v, tau, cms, cds, settings);

// 结果
Eigen::VectorXd ddq = data.ddq;       // 广义加速度
Eigen::VectorXd lambda = data.lambda_c; // 约束力

// 解析导数（for MPC/DDP）
pinocchio::computeConstraintDynamicsDerivatives(
    model, data, cms, cds, settings);
// data.ddq_dq, data.ddq_dv, data.ddq_dtau 已填充

24.5 整机 KKT 尺寸¶

Cassie 整机： - Tree DOF：$n = 20$（2 腿各 5 tree joints + 浮基 6 DOF + 2 手臂 2 DOF） - 闭链约束：$2 \times 5 = 10$ 维 - 足接触（双支撑）：$2 \times 6 = 12$ 维（面接触） - 总约束：$m = 22$ - KKT 矩阵尺寸：$(20 + 22) \times (20 + 22) = 42 \times 42$

用 Pinocchio 3 constrainedABA：~10 μs（远快于密集 $O(42^3)$）。

跨章交叉引用索引¶

引用目标	本章使用位置	引用方式
30_SO3_SE3 → Frobenius 定理	§1.2 约束可积判据	直接使用定理判断 holonomic
20_Lagrange → d'Alembert 原理	§3.2 KKT 推导起点	从虚功原理出发
70_辛结构 → 辛积分器	§4.4 Baumgarte 破坏辛性	解释为何 SHAKE 优于 Baumgarte
50_动力学微分 → 解析导数	§21 IFT on KKT	KKT 分解复用于梯度计算
10_变分法 → Euler-Lagrange	§3.2 Lagrange 乘子引入	受约束变分问题
70_辛结构 → Lie-Poisson 约化	§19 Chaplygin 约化	非完整约化的几何根源

层级	方程	微分指数	说明
位置层	\(M\ddot q = \tau + J^\top\lambda,\quad \varphi(q) = 0\)	3	原始形式，最病态
速度层	\(M\ddot q = \tau + J^\top\lambda,\quad J(q)\dot q = 0\)	2	对 \(\varphi\) 微分一次
加速度层	\(M\ddot q = \tau + J^\top\lambda,\quad J\ddot q + \dot J\dot q = 0\)	1	对 \(\varphi\) 微分两次
ODE（约化坐标）	\(\ddot q = F(q,\dot q)\) on \(T_qM\)	0	完全消去约束

条件	根的性质	行为
\(\alpha < \beta\)（欠阻尼）	复根	振荡衰减
\(\alpha > \beta\)（过阻尼）	双实根	最慢模 \(s \approx -\beta^2/(2\alpha)\)，越大越慢
\(\alpha = \beta\)（临界阻尼）	双重根 \(s = -\alpha\)	无振荡最快衰减

ReferenceFrame	摩擦锥定义	适用场景
`LOCAL`	最直接，\(n = e_3\)	摩擦锥约束
`LOCAL_WORLD_ALIGNED`	需变换 \(n\)	Cartesian 误差
`WORLD`	含额外叉积项，不推荐	极少使用

事件	物理	数学条件	处理方式
Make（建立接触）	物体到达地面	\(\phi \to 0^+\), \(\dot\phi < 0\)	激活约束行
Break（释放接触）	物体离开地面	\(\lambda_n \to 0^+\)	去激活约束行
Stick→Slip	静摩擦被突破	\(\\|f_t\\| \to \mu f_n\)	约束变不等式
Slip→Stick	滑动减速到零	\(\\|v_t\\| \to 0\)	不等式变等式
Impact（碰撞）	速度不连续	\(v^+ \neq v^-\)	冲量层求解

准则	目标函数	物理含义
最小力范数	\(\min\\|\lambda\\|_2^2\)	均匀分配，避免单足过载
最小能耗	\(\min\lambda^\top R\lambda\)	R 含关节-力映射
最大摩擦余量	\(\max\min_i(\mu f_{n,i} - \\|f_{t,i}\\|)\)	避免滑移
CoP 居中	\(\min\\|p_\text{CoP} - p_\text{center}\\|^2\)	增加稳定裕度

WBC 概念	约束动力学概念	数学表达
任务空间加速度	KKT 解的投影	\(\ddot x = J\ddot q + \dot J\dot q\)
接触力分配	Lagrange 乘子的选择	\(\lambda \in \arg\min\\|\lambda\\|\) s.t. EoM
零空间运动	ker(J) 中的自由度	\(\ddot q_\text{null} \in \ker(J_\text{task})\)
优先级	嵌套 KKT	先解高优先→在其零空间解低优先
摩擦锥	不等式约束	\(\lambda \in \mathcal{FC}(\mu)\)
力矩限制	决策变量界约束	\(\tau_\min \leq \tau \leq \tau_\max\)

症状	可能原因	排查步骤	相关章节
约束违反 \(\varphi\) 随时间二次增长	使用了 index-1 形式但无稳定化	1.检查是否加了 Baumgarte 项 2.打印 \(\varphi\) 和 \(J\dot q\) 3.加稳定化或换 projection	§2, §4
Baumgarte 后约束违反振荡	欠阻尼（\(\alpha < \beta\)）	1.检查 \(\alpha/\beta\) 比值 2.设为临界阻尼 \(\alpha = \beta\) 3.验证 \(\alpha h \leq 1\)	§4
KKT 求解返回 NaN/Inf	LICQ 违反（J 秩亏）	1.打印 \(\sigma_\min(J)\) 2.检查是否在奇异构型 3.加 proximal \(\mu I\)	§3
WBC QP 无可行解	摩擦锥/力矩限制过紧	1.松弛摩擦系数 2.检查 \(\tau\) 限制是否合理 3.打印各约束余量	§8
闭链机器人动力学异常	闭链约束未激活	1.检查 `RigidConstraintModel` 是否正确构造 2.确认 joint_id 和 placement 3.验证整机 DOF	§5
接触力在切换时跳变	active set 不连续切换	1.打印接触力历史 2.加 Baumgarte 平滑过渡 3.检查步态时序	§6
MuJoCo 仿真穿透地面	solref 参数过松	1.减小 `solref[0]`（timeconst） 2.检查 `solimp` 3.增大迭代次数	§7

方法	算法	复杂度	代表实现	适用场景
密集 LDL⊤	Bunch-Kaufman 分解	\(O((n_v+m)^3)\)	Eigen LDLT	小型系统 (<50 DOF)
Range-space Schur	解 \(JM^{-1}J^\top\lambda = ...\)	\(O(n^2 + m^3)\)	Khatib 1987	中型，约束少
Null-space Schur	投影到 \(\ker(J)\)	\(O(n^2 + m^3)\)	Mistry-Righetti 2012	冗余系统
Proximal (Pinocchio 3)	稀疏 Cholesky + ALM	\(O(n_v + m)\)	Carpentier RSS 2021	大型，秩亏鲁棒
Newton (MuJoCo)	精确 Hessian + line-search	2-5 迭代/步	Todorov 2014	通用，中等规模
CG (MuJoCo 3)	Conjugate Gradient	稀疏友好	MuJoCo 3 默认	大规模稀疏
PGS	Projected Gauss-Seidel	10 sweeps	经典 LCP	实时低精度
SAP (Drake)	Semi-Analytic Primal Newton	全局收敛	Castro et al. 2021	Drake 默认

约束类型	WBC 中的处理	约束行数	乘子物理含义
完整 bilateral	\(J\ddot q + \dot J\dot q = 0\)	位置+速度+加速度	约束力
完整 unilateral	\(J\ddot q + \dot J\dot q \geq 0\) + 互补	同上但条件激活	接触力
非完整	\(A(q)\dot q = 0\) 仅速度层	仅速度层	约束力方向

约束动力学——从 DAE 到可微接触与浮基控制¶

前置自测¶

本章目标¶

知识树¶

§1 约束分类学 ⭐⭐¶

1.1 动机：为什么需要约束分类¶

1.2 Pfaffian 一般形式与 Frobenius 判据¶

1.3 四元分类体系¶

1.4 经典例子详解¶

1.5 约束分类对求解方法的影响¶

⚠️ 常见陷阱¶

§2 DAE/ODE 约束形式化 ⭐⭐⭐¶

2.1 动机：为什么直接积分约束系统会发散¶

2.2 微分指数的严格定义¶

2.3 约束力学的指数阶梯¶

2.4 Drift-off 效应的数学证明¶

2.5 Index reduction 的策略¶

⚠️ 常见陷阱¶

练习¶

§3 Lagrange 乘子法的完整推导与 KKT 系统 ⭐⭐¶

3.1 动机：约束力从哪里来¶

3.2 从 d'Alembert 原理到 KKT 的每一步¶

3.3 KKT 系统的结构分析¶

3.4 J 的几何含义¶

3.5 \(\lambda\) 的物理含义¶

⚠️ 常见陷阱¶

练习¶

§4 Baumgarte 稳定化 ⭐⭐¶

4.1 动机：为什么 index-1 形式不够¶

4.2 完整推导¶

4.3 参数选择准则¶

4.4 Baumgarte 的缺陷¶

4.5 替代方案¶

4.6 MuJoCo 的 soft constraint 与 Baumgarte 的精确对应¶

⚠️ 常见陷阱¶

练习¶

§5 闭链机构 ⭐⭐⭐¶

5.1 动机：树形模型的局限¶

5.2 建模流程（Featherstone 2008 Ch.8）¶

5.3 约束维度分析¶

5.4 实际机器人的闭链结构¶

5.5 Pinocchio 3 闭链 API¶

5.6 O(N) 闭链求解策略¶

⚠️ 常见陷阱¶

练习¶

§6 接触动力学 ⭐⭐⭐⭐¶

6.1 动机：为什么接触比一般约束难得多¶

6.2 Signorini 条件（法向接触）¶

6.3 Coulomb 摩擦锥¶

6.4 LCP/NCP Formulation¶

6.5 Stewart-Trinkle 时步法¶

6.6 Moreau 半隐格式¶

6.6.1 从 Stewart-Trinkle 到工程实践的完整链路¶

6.7 Contact Wrench Cone (CWC) 与 ZMP/Capture Point¶

⚠️ 常见陷阱¶

练习¶

§7 可微接触 ⭐⭐⭐⭐¶

7.1 动机：为什么需要可微接触¶

7.2 路线 A：MuJoCo 软化（Softening）¶

7.3 路线 B：Dojo κ-平滑化（Smoothing）¶

7.4 路线 C：Nimble 解析子梯度¶

7.5 路线 D：随机平滑化（Suh et al., 2022）¶

7.6 Pinocchio 3 proximal + 可微化¶

7.7 四条路线对比¶

⚠️ 常见陷阱¶

练习¶

§8 浮基接触建模与 QP-WBC ⭐⭐⭐¶

8.1 动机：从理论到腿足控制¶

8.2 浮基方程的完整形式¶

8.3 前 6 行 = 质心动力学¶

8.4 WBC QP 的完整形式¶

8.5 分层 QP（HQP）¶

8.6 TSID 库接口（C++）¶

8.7 接触力分配的冗余问题¶

8.8 角动量调节¶

8.9 与 MPC 的级联架构¶

⚠️ 常见陷阱¶

练习¶

本章小结¶

§9 约束动力学在不同仿真器中的统一视角 ⭐⭐⭐¶