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

第 52 章 接触力学与约束优化——摩擦锥 / 接触 Jacobian / Centroidal 深入 / 互补约束

难度: ⭐⭐~⭐⭐⭐ | 前置: Ch49 空间向量, Ch50 QP/NLP, Ch51 简化模型 | 学时: 20-25 小时

---

前置自测

在开始本章之前,请确认你能回答以下问题。如果任何一题感到陌生,请先回顾对应章节:

1. 空间力(Wrench)的 6 维表达 \(\boldsymbol{f} = [\boldsymbol{n}^T, \boldsymbol{f}^T]^T\) 中,力矩和力的排列顺序是什么?(→ Ch49)
2. QP 问题的标准形式 \(\min \frac{1}{2}x^T H x + g^T x\) s.t. \(Ax \le b\) 中,不等式约束如何编码"单侧接触"?(→ Ch50)
3. Centroidal Dynamics 的 6 维动量 \(\boldsymbol{h}_G = [k^T, l^T]^T\) 的物理含义是什么?(→ Ch51)
4. Pinocchio 中 getFrameJacobian() 返回的矩阵维度是什么?LOCAL_WORLD_ALIGNED 参考系的含义?(→ Ch47)
5. LIPM 假设角动量为零,这在什么情况下会严重失效?(→ Ch51)

---

本章目标

学完本章,你应能:

1. 深入理解接触力学三大铁律——法向非负、摩擦锥、互补性——并写出其数学形式
2. 掌握摩擦锥线性化的完整推导——内切/外切近似、误差界、wrench cone 扩展
3. 理解互补约束的三大处理路线——预定义模式/软化/Contact-Implicit TO——及各自的数学基础
4. 推导接触 Jacobian 的对偶关系——从运动空间到力空间的映射
5. 掌握 CCRBA 算法——O(N) 计算 CMM 的完整步骤
6. 理解角动量的守恒与可控性——飞行相位的约束与猫翻身悖论
7. 能把接触约束编码为 QP/NLP 约束——为 Ch53 WBC 和 Ch54 DDP 打基础

---

52.1 接触力的物理约束——三大铁律 ⭐⭐

动机:为什么需要约束接触力?

本节解决的问题:如果不约束接触力,优化器(QP/NLP)会给出物理上不可能的解——比如地面"吸"住机器人、足底在冰面上产生无穷大摩擦力、或者脚在空中仍然受到地面支撑力。

反面案例:假设你在 Ch50 的 QP 中忘记加摩擦锥约束,只加了牛顿-欧拉方程作为等式约束。求解器会怎么做?

场景: 四足机器人需要向右加速
QP目标: min ||τ||² (最小化关节扭矩)
等式约束: M*ddq + h = S^T*τ + J_c^T*λ (动力学)

无摩擦锥时的"最优解":
  λ_z = 0 (法向力为零,省力!)
  λ_x = 很大的值 (全靠水平摩擦力推动)
  → 物理上: 摩擦力远超法向力 × μ, 机器人会滑倒!

这就是为什么**每一个**足式机器人控制器都必须包含接触力约束。

历史背景

接触力学的数学框架可追溯到三位先驱:

年份	人物	贡献
1785	Coulomb	提出库仑摩擦定律 \(f_t \le \mu f_n\)
1933	Signorini	形式化单边接触问题(Signorini problem)
1963	Fichera	证明 Signorini 问题解的存在唯一性
1988	Moreau	非光滑力学框架,互补约束的现代形式
2013	Orin et al.	Centroidal 动力学与 CCRBA 算法

52.1.1 约束 1:法向非负(Signorini 条件) ⭐⭐

物理直觉:地面只能"推"机器人,不能"拉"。这是因为地面和足底之间没有粘性连接(不像胶带)。

数学形式:设接触点 \(\boldsymbol{p}_c\) 到地面的**有符号距离**为 \(\phi(\boldsymbol{p}_c)\)(正为离开地面,零为接触,负为穿透),接触力法向分量为 \(\lambda_c^n\),则:

\[\lambda_c^n \ge 0\]

这里 \(n\) 是接触面法向方向(指向机器人一侧)。对于水平地面,法向就是 \(z\) 轴,即 \(\lambda_c^z \ge 0\)。

完整 Signorini 条件(Hertz-Signorini-Moreau 条件):

Signorini 条件不仅要求法向力非负,还包含三个联立关系:

\[\boxed{\phi(\boldsymbol{p}_c) \ge 0, \quad \lambda_c^n \ge 0, \quad \phi(\boldsymbol{p}_c) \cdot \lambda_c^n = 0}\]

这三个条件的物理含义:

条件	含义	直觉
\(\phi \ge 0\)	不穿透	脚不能穿过地面
\(\lambda_c^n \ge 0\)	法向力非负	地面只推不拉
\(\phi \cdot \lambda_c^n = 0\)	互补性	离地时无力,有力时在地面上

几何解读:在 \((\phi, \lambda_c^n)\) 平面上,合法的 \((\phi, \lambda_c^n)\) 对只能落在两条射线上——正 \(\phi\) 轴(空中,无力)和正 \(\lambda_c^n\) 轴(接触,有力)。这构成一个"L 形"可行域。这好比电路中的理想二极管:电压(对应 \(\phi\))和电流(对应 \(\lambda_c^n\))不能同时为正——要么导通(接触,有力),要么截止(离地,无力)。不同之处在于,二极管的 V-I 关系是标量的,而接触力学还叠加了切向摩擦,使问题从 1D 扩展到 3D。

非平坦地形上的法向方向:

对于非平坦地形,法向 \(\boldsymbol{n}\) 不再是简单的 \([0, 0, 1]^T\)。设地形高度函数为 \(z = h(x, y)\),则:

\[\boldsymbol{n} = \frac{1}{\sqrt{1 + h_x^2 + h_y^2}} \begin{bmatrix} -h_x \\ -h_y \\ 1 \end{bmatrix}\]

其中 \(h_x = \partial h / \partial x\), \(h_y = \partial h / \partial y\)。接触力的法向分量变为:

\[\lambda_c^n = \boldsymbol{\lambda}_c \cdot \boldsymbol{n} \ge 0\]

⚠️ 编程陷阱:法向方向的坐标系

错误: 在倾斜地面上仍然用 \(\lambda_c^z \ge 0\) 作为约束。

症状: 机器人在斜坡上滑动,因为约束允许了沿斜面的"推力",但没有正确约束法向力。

根因: 法向非负约束必须在**接触面局部坐标系**中表达,而非世界系。

正确做法: 计算接触面法向 \(\boldsymbol{n}\),用 \(\boldsymbol{\lambda}_c \cdot \boldsymbol{n} \ge 0\) 代替 \(\lambda_c^z \ge 0\)。

52.1.2 约束 2:库仑摩擦锥(Coulomb Friction Cone) ⭐⭐

物理直觉:摩擦力不能无限大。当你推一个重物,摩擦力最多等于法向力乘以摩擦系数——超过这个极限,物体就滑动了。

数学形式:切向力(摩擦力)的模不能超过法向力乘以摩擦系数 \(\mu\):

\[\|\boldsymbol{\lambda}_c^t\|_2 = \sqrt{(\lambda_c^x)^2 + (\lambda_c^y)^2} \le \mu \lambda_c^n\]

其中 \(\boldsymbol{\lambda}_c^t\) 是切向(tangential)分量,\(\mu\) 是静摩擦系数。

典型摩擦系数:

接触对	\(\mu\) 范围	工程常用值
橡胶 — 干燥水泥	0.6 ~ 0.8	0.7
橡胶 — 湿水泥	0.4 ~ 0.5	0.45
橡胶 — 冰	0.05 ~ 0.15	0.1
金属 — 金属	0.3 ~ 0.6	0.4
橡胶 — 草地	0.3 ~ 0.5	0.4

几何含义:在 \((\lambda^x, \lambda^y, \lambda^z)\) 三维空间中,合法的接触力向量必须落在一个**圆锥体**内部:

• 顶点在原点 \((0, 0, 0)\)
• 轴线沿法向 \(z\) 方向
• 半顶角 \(\alpha = \arctan(\mu)\)

当 \(\mu = 0.7\) 时,\(\alpha \approx 35°\);当 \(\mu = 0.1\) 时,\(\alpha \approx 5.7°\)——冰面上的摩擦锥非常"尖",几乎只能产生竖直力。

本质洞察:Signorini 条件的三个联立关系 \(\phi \ge 0, \lambda^n \ge 0, \phi \cdot \lambda^n = 0\) 并非三条独立规则的简单叠加,而是**一个统一的物理原理的三个投影**:接触是一种**单边约束(unilateral constraint)**——它只能推、不能拉,且只在几何接触时才产生力。这三个条件中去掉任何一个,都会导致非物理解(穿透、拉力、或悬空力)。

为什么是 SOC(Second-Order Cone)?

摩擦锥约束 \(\sqrt{(\lambda^x)^2 + (\lambda^y)^2} \le \mu \lambda^z\) 在数学上是一个**二阶锥约束(SOC)**。SOC 是**凸**的,但不是线性的——这意味着:

• 如果问题只有 SOC 约束 + 二次目标 → SOCP(Second-Order Cone Program) → 可以用 Mosek/ECOS 求解
• 如果我们想用 QP 求解器(OSQP/qpOASES) → 必须把 SOC 线性化 → 这就是 52.2 节的内容

推导:为什么摩擦锥是凸的?

设 \(\boldsymbol{\lambda}_1, \boldsymbol{\lambda}_2\) 都在锥内,即 \(\|\boldsymbol{\lambda}_i^t\| \le \mu \lambda_i^n\), \(i = 1, 2\)。对任意 \(\alpha \in [0, 1]\):

\[\|\alpha \boldsymbol{\lambda}_1^t + (1-\alpha) \boldsymbol{\lambda}_2^t\| \le \alpha \|\boldsymbol{\lambda}_1^t\| + (1-\alpha) \|\boldsymbol{\lambda}_2^t\| \le \alpha \mu \lambda_1^n + (1-\alpha) \mu \lambda_2^n = \mu (\alpha \lambda_1^n + (1-\alpha) \lambda_2^n)\]

第一个不等式用了三角不等式,第二个用了各自的锥约束。因此凸组合仍在锥内,摩擦锥是凸集。\(\square\)

52.1.3 约束 3:互补性(Complementarity) ⭐⭐⭐

物理直觉:一只脚要么在空中(无接触力),要么在地面上(有接触力)——不可能"在空中却受到地面力",也不可能"在地面上却没有支撑力(除非起跳瞬间)"。

数学形式:

\[0 \le \phi(\boldsymbol{p}_c) \perp \lambda_c^n \ge 0\]

符号 \(\perp\) 读作"互补",表示 \(\phi \cdot \lambda_c^n = 0\)——两个非负量的乘积为零,意味着**至少一个为零**。

这是 LCP 的标准形式:线性互补问题(Linear Complementarity Problem)。如果 \(\phi\) 是 \(q\) 的线性函数(或线性化后),整个接触问题变成 LCP。

速度级别的互补条件:

在实际时间积分中,位置级别的互补条件 \(\phi \cdot \lambda^n = 0\) 不够用——我们还需要约束速度。速度级别的 Signorini 条件:

\[\text{接触状态:} \quad \phi = 0, \; \dot{\phi} = 0, \; \lambda^n \ge 0$$ $$\text{分离条件:} \quad \phi = 0, \; \dot{\phi} > 0 \implies \lambda^n = 0$$ $$\text{保持条件:} \quad \phi = 0, \; \dot{\phi} = 0, \; \ddot{\phi} \ge 0, \; \lambda^n \ge 0, \; \ddot{\phi} \cdot \lambda^n = 0\]

为什么互补约束难处理?

互补约束 \(\phi \cdot \lambda^n = 0\) 是一个**等式约束**,但它是**非光滑的**——在 \((\phi, \lambda^n) = (0, 0)\) 处(即接触刚好建立/断裂的瞬间)不可微。这相当于优化问题中遇到了一个"棱角"——梯度在棱角处不存在,就像绝对值函数 \(|x|\) 在 \(x = 0\) 处不可微一样,只是这里的棱角发生在二维空间中。这导致:

1. 标准梯度法(如 SQP)在这一点处 Jacobian 退化
2. 目标函数的梯度包含不连续跳变
3. 传统 QP 求解器无法直接处理

这就是 52.3 节三大路线要解决的核心难题。

💡 概念误区:互补性 vs 碰撞

错误理解: "互补约束就是碰撞检测"。

正确理解: 互补约束描述的是**稳态接触关系**——"接触 ↔ 力"的逻辑耦合。碰撞(impact)是一个**瞬时事件**,需要额外的冲量模型(如恢复系数)。在足式机器人的 MPC/WBC 中,我们通常假设**无碰撞**(soft landing),只处理互补性。

🧠 思维陷阱:三大约束的独立性

陷阱: "法向非负 + 摩擦锥已经够了,互补性是多余的。"

反例: 如果没有互补性,优化器可以在脚离地 10cm 时仍然产生接触力——这在数学上满足 \(\lambda^z \ge 0\) 和摩擦锥,但物理上是胡说八道。互补性是连接**几何(距离)**和**力学(力)**的桥梁。

52.1.4 三大约束的统一数学框架

将三大约束合写为一个紧凑的数学形式:

\[\boxed{\begin{aligned} &\text{(Signorini)} \quad 0 \le \phi(\boldsymbol{p}_c) \perp \lambda_c^n \ge 0 \\ &\text{(Coulomb)} \quad \|\boldsymbol{\lambda}_c^t\|_2 \le \mu \lambda_c^n \\ &\text{(Maximum Dissipation)} \quad \dot{\boldsymbol{p}}_c^t \ne 0 \implies \boldsymbol{\lambda}_c^t = -\mu \lambda_c^n \frac{\dot{\boldsymbol{p}}_c^t}{\|\dot{\boldsymbol{p}}_c^t\|} \end{aligned}}\]

第三个条件是**最大耗散原则**(Maximum Dissipation Principle):如果接触点在滑动,摩擦力方向与滑动方向相反,且大小恰好等于 \(\mu \lambda_c^n\)。这是 Coulomb 摩擦的完整表述。

在足式机器人控制中的简化:大多数 MPC/WBC 假设**无滑动**(no-slip),即 \(\dot{\boldsymbol{p}}_c^t = 0\)。此时第三个条件自动满足,只需处理前两个。

52.1.5 补充约束：关节力矩限幅与足端力的耦合

除了上述三大接触力约束外，工程中还有两类实用约束值得注意：

足端力幅度约束：摆动腿不与地面接触，足端力严格为零（\(\mathbf{f}_i = \mathbf{0}\)）；支撑腿的法向力存在上限 \(f_{z,\max}\)，由电机物理极限决定。在 MPC 优化中，这些约束通过步态调度器提供的接触状态变量（\(c_i \in \{0, 1\}\)）来实现：\(c_i = 0\) 时强制 \(\mathbf{f}_i = \mathbf{0}\)。

关节力矩约束：即便优化器求出了满足摩擦锥的足端力，仍需确保这些力经雅可比转置 \(\boldsymbol{\tau} = J_c^T(\mathbf{q}) \cdot \mathbf{f}\) 映射到关节空间后不超过电机力矩极限：

\[|\tau_j| \le \tau_{j,\max}, \quad \forall j\]

由于 \(J_c(\mathbf{q})\) 含有正余弦函数，当关节角度 \(\mathbf{q}\) 也是决策变量时，力矩约束是非凸的。但在实时 MPC 的常见做法中，\(\mathbf{q}\) 由当前状态或参考轨迹确定（而非优化变量），此时 \(J_c\) 退化为常数矩阵，力矩约束变为关于足端力的线性约束——可直接纳入 QP 框架。

练习

练习 52.1.1 (⭐⭐): 对于一个四足机器人站在 \(30°\) 斜坡上,写出每只脚的法向非负约束和摩擦锥约束(用世界系坐标)。如果 \(\mu = 0.5\),机器人能否稳定站立?为什么?

练习 52.1.2 (⭐⭐⭐): 证明:当地面是平面时,Signorini 条件 \(\phi \ge 0, \lambda^n \ge 0, \phi \lambda^n = 0\) 可以等价写为 \(\lambda_c^n \in N_{C}(\phi)\),其中 \(N_C\) 是凸集 \(C = \{\phi \ge 0\}\) 的法锥(normal cone)。这个变分不等式观点为什么重要?

练习 52.1.3 (⭐⭐): 在 MuJoCo 中,接触模型使用"软接触"(spring-damper)。试分析:软接触是否满足严格的 Signorini 条件?为什么 MuJoCo 选择违反它?

---

52.2 摩擦锥线性化——从 SOC 到 QP 的关键一步 ⭐⭐

动机:为什么要线性化?

本节解决的问题:摩擦锥约束 \(\sqrt{(\lambda^x)^2 + (\lambda^y)^2} \le \mu \lambda^z\) 是二阶锥约束(SOC),标准 QP 求解器(OSQP, qpOASES)不支持 SOC。如何在保持问题凸性的前提下,把摩擦锥变成线性不等式?

反面案例:MIT Cheetah 3 的凸 MPC 必须在 1ms 内求解——如果用 SOCP 求解器(Mosek),耗时约 3-5ms,控制频率从 1000Hz 降到 200Hz。线性化后用 QP 求解器,耗时降到 0.8ms。

历史与现状

年份	方法	代表工作
2005	多面体近似(k=4)	早期 ZMP 步行控制器
2018	k=4 线性化 + 凸 MPC	MIT Cheetah 3 (Di Carlo et al.)
2020	SOC 直接求解	Grandia et al. (ANYmal, Mosek)
2023	混合:MPC 用线性化, WBC 用 SOC	OCS2 + 现代 WBC

52.2.1 多面体近似的数学推导 ⭐⭐

基本思想:用 \(k\) 条边的正多边形**内切/外切**于圆,把圆锥近似为多面体锥。这与计算机图形学中用多边形逼近曲面的思路一致——正如 3D 渲染用三角面片逼近球体(面数越多越圆),我们用线性半平面逼近圆锥(边数越多越精确)。区别在于,图形学的逼近误差影响视觉效果,而这里的逼近误差影响机器人是否会滑倒。

步骤 1:圆锥的截面是圆

在 \(\lambda^z = 1\) 平面上截取摩擦锥,得到半径为 \(\mu\) 的圆:

\[(\lambda^x)^2 + (\lambda^y)^2 \le \mu^2\]

步骤 2:用正 \(k\) 边形近似圆

在圆周上等距取 \(k\) 个点 \((\mu \cos\theta_i, \mu \sin\theta_i)\),其中 \(\theta_i = 2\pi i / k\)。

连接相邻点构成正 \(k\) 边形。每条边对应一个线性不等式。第 \(i\) 条边的**外法向**方向是 \((\cos\psi_i, \sin\psi_i)\),其中 \(\psi_i = \pi(2i+1)/k\)。

步骤 3:写出线性不等式

第 \(i\) 条不等式:接触力在第 \(i\) 个外法向方向上的投影不超过边到原点的距离:

\[\cos\theta_i \cdot \lambda^x + \sin\theta_i \cdot \lambda^y \le \mu_{\text{eff}} \cdot \lambda^z, \quad i = 0, 1, \dots, k-1\]

其中 \(\theta_i = 2\pi i / k\)。

关键: \(\mu_{\text{eff}}\) 的选取决定了是**内切**还是**外切**近似:

\[\boxed{\mu_{\text{eff}} = \begin{cases} \mu & \text{外切近似(outer approximation)} \\ \mu \cos(\pi/k) & \text{内切近似(inner approximation)} \end{cases}}\]

推导内切近似的 \(\mu_{\text{eff}}\):

正 \(k\) 边形**内切于**半径为 \(\mu\) 的圆时,多边形的**内切圆半径**为:

\[r_{\text{inner}} = \mu \cos(\pi / k)\]

当 \(k = 4\) 时,\(r_{\text{inner}} = \mu \cos(45°) = \mu / \sqrt{2} \approx 0.707\mu\),损失约 29% 的摩擦能力。

当 \(k = 8\) 时,\(r_{\text{inner}} = \mu \cos(22.5°) \approx 0.924\mu\),损失约 7.6%。

当 \(k = 16\) 时,\(r_{\text{inner}} = \mu \cos(11.25°) \approx 0.981\mu\),损失约 1.9%。

52.2.2 近似误差的定量分析 ⭐⭐

外切近似误差(多面体比圆锥大多少):

外切正 \(k\) 边形的面积与圆的面积之比:

\[\frac{A_{\text{outer}}}{A_{\text{circle}}} = \frac{k \mu^2 \tan(\pi/k)}{\pi \mu^2} = \frac{k \tan(\pi/k)}{\pi}\]

\(k\)	\(\tan(\pi/k)\)	面积比	超出百分比
4	1.000	1.273	27.3%
6	0.577	1.103	10.3%
8	0.414	1.055	5.5%
12	0.268	1.023	2.3%
16	0.199	1.013	1.3%

内切近似误差(多面体比圆锥小多少):

\[\frac{A_{\text{inner}}}{A_{\text{circle}}} = \frac{k \sin(2\pi/k)}{2\pi}\]

\(k\)	面积比	损失百分比
4	0.637	36.3%
6	0.827	17.3%
8	0.900	10.0%
12	0.955	4.5%
16	0.975	2.5%

工程选择指南:

场景	推荐 \(k\)	近似类型	理由
实时 MPC (1kHz)	4	外切	速度优先,接受 ~27% 过近似
标准 WBC (500Hz)	8	外切	平衡精度与速度
离线轨迹优化	16 或 SOC	内切/精确	精度优先
安全关键	8	内切	保守近似,不允许超出真实锥

💡 概念误区:内切 vs 外切的安全性

错误理解: "外切近似更安全,因为它包含了整个真实锥。"

正确理解: 恰恰相反!内切近似更安全。外切近似允许的力方向**超出**了真实摩擦锥——优化器可能找到一个"合法"解,但实际执行时机器人会滑倒。内切近似**保守**地限制了力方向——宁可少用摩擦力,也不冒滑倒风险。

MIT Cheetah 用外切的原因: 它的 MPC 代价函数已经倾向于产生接近竖直的力(加权矩阵使 \(\lambda^z\) 优先),实际解很少触碰锥边界。但如果你的控制器会频繁在锥边界操作(如极端地形),应切换到内切。

52.2.3 完整 C++ 实现与可视化 ⭐⭐

#include <Eigen/Dense>
#include <cmath>
#include <vector>

/**
 * @brief Generate polyhedral friction cone constraints
 * @param mu Friction coefficient
 * @param k Number of edges (4, 6, 8, 16, ...)
 * @param inner If true, use inner approximation (conservative)
 * @return A matrix (k x 3) such that A * [lx, ly, lz]^T <= 0
 *
 * Each row encodes: cos(theta_i)*lx + sin(theta_i)*ly - mu_eff*lz <= 0
 */
Eigen::MatrixXd frictionConeLinearization(double mu, int k = 8, 
                                           bool inner = false) {
  // Effective friction coefficient
  double mu_eff = inner ? mu * std::cos(M_PI / k) : mu;

  Eigen::MatrixXd A(k, 3);
  for (int i = 0; i < k; ++i) {
    double theta = 2.0 * M_PI * i / k;
    A(i, 0) = std::cos(theta);   // lambda^x coefficient
    A(i, 1) = std::sin(theta);   // lambda^y coefficient
    A(i, 2) = -mu_eff;           // lambda^z coefficient
  }
  return A;  // Constraint: A * [lx, ly, lz]^T <= 0
}

/**
 * @brief Compute approximation error metrics
 * @return {area_ratio, max_radial_error}
 */
std::pair<double, double> approximationError(int k, bool inner) {
  double area_ratio, max_error;
  if (inner) {
    // Inner polygon area / circle area
    area_ratio = k * std::sin(2.0 * M_PI / k) / (2.0 * M_PI);
    max_error = 1.0 - std::cos(M_PI / k);  // Relative radial shrinkage
  } else {
    // Outer polygon area / circle area
    area_ratio = k * std::tan(M_PI / k) / M_PI;
    max_error = 1.0 / std::cos(M_PI / k) - 1.0;  // Relative radial expansion
  }
  return {area_ratio, max_error};
}

/**
 * @brief For multiple contacts, stack friction cone constraints
 * @param mu Friction coefficients per contact
 * @param n_contacts Number of contacts
 * @param k Edges per cone
 * @return Block diagonal constraint matrix (n_contacts*k x n_contacts*3)
 */
Eigen::MatrixXd stackedFrictionCones(const std::vector<double>& mu,
                                      int n_contacts, int k = 8) {
  int total_rows = n_contacts * k;
  int total_cols = n_contacts * 3;
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(total_rows, total_cols);

  for (int c = 0; c < n_contacts; ++c) {
    A.block(c * k, c * 3, k, 3) = frictionConeLinearization(mu[c], k);
  }
  return A;
}

可视化代码(Python):

import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from mpl_toolkits.mplot3d.art3d import Poly3DCollection

def visualize_friction_cones(mu=0.7, k_values=[4, 6, 8, 16]):
    """Visualize exact cone vs polyhedral approximations."""
    fig = plt.figure(figsize=(16, 4))

    for idx, k in enumerate(k_values):
        ax = fig.add_subplot(1, len(k_values), idx + 1, projection='3d')

        # Exact cone
        theta = np.linspace(0, 2 * np.pi, 100)
        z = np.linspace(0, 1, 50)
        T, Z = np.meshgrid(theta, z)
        X = mu * Z * np.cos(T)
        Y = mu * Z * np.sin(T)
        ax.plot_surface(X, Y, Z, alpha=0.15, color='blue')

        # Polyhedral cone (outer)
        angles = np.linspace(0, 2 * np.pi, k + 1)
        for i in range(k):
            verts = [[0, 0, 0],
                     [mu * np.cos(angles[i]), mu * np.sin(angles[i]), 1],
                     [mu * np.cos(angles[i+1]), mu * np.sin(angles[i+1]), 1]]
            ax.add_collection3d(Poly3DCollection(
                [verts], alpha=0.3, color='red', edgecolor='red'))

        # Error annotation
        err = (1.0 / np.cos(np.pi / k) - 1.0) * 100
        ax.set_title(f'k={k}, error={err:.1f}%')
        ax.set_xlabel('lx'); ax.set_ylabel('ly'); ax.set_zlabel('lz')

    plt.suptitle('Friction Cone Linearization Comparison')
    plt.tight_layout()
    plt.savefig('friction_cone_comparison.png', dpi=150)
    plt.show()

52.2.4 Wrench Cone:从 3D 点接触到 6D 面接触 ⭐⭐⭐

问题:3D 点接触模型假设接触发生在一个几何点上。但实际上,足底是一个有面积的矩形/圆形区域。面接触除了产生力,还会产生**力矩**(绕法向的摩擦力矩)。

6D Contact Wrench(接触旋量):

对于一个矩形面接触(尺寸 \(2l_x \times 2l_y\)),接触 wrench \(\boldsymbol{w}_c = [\boldsymbol{n}_c^T, \boldsymbol{f}_c^T]^T \in \mathbb{R}^6\) 必须满足:

\[\boldsymbol{w}_c \in \mathcal{W}_c = \{U \boldsymbol{w}_c \le 0\}\]

其中 \(U\) 是一个约束矩阵,它同时编码了:

1. 法向力非负:\(f_c^z \ge 0\)
2. 摩擦锥:\(\sqrt{(f_c^x)^2 + (f_c^y)^2} \le \mu f_c^z\)(线性化后)
3. 绕法向的扭矩约束:\(|n_c^z| \le \gamma f_c^z\),其中 \(\gamma\) 是"扭转摩擦系数"
4. CoP(Center of Pressure)约束:\(|n_c^x| \le l_y f_c^z\) 且 \(|n_c^y| \le l_x f_c^z\)

关键区别: 力和力矩不是独立的。如 Stephane Caron 所指出:"if we fix the resultant force, it will affect the shape of the moment cone." 因此 6D wrench cone 不是 3D 力锥和 3D 力矩锥的简单笛卡尔积——它们之间有耦合约束。

\[U = \begin{bmatrix} 0 & 0 & 0 & 0 & 0 & -1 \\ 0 & 0 & 0 & 1 & 0 & -\mu \\ 0 & 0 & 0 & -1 & 0 & -\mu \\ 0 & 0 & 0 & 0 & 1 & -\mu \\ 0 & 0 & 0 & 0 & -1 & -\mu \\ 1 & 0 & 0 & 0 & 0 & -l_y \\ -1 & 0 & 0 & 0 & 0 & -l_y \\ 0 & 1 & 0 & 0 & 0 & -l_x \\ 0 & -1 & 0 & 0 & 0 & -l_x \\ 0 & 0 & 1 & 0 & 0 & -\gamma \\ 0 & 0 & -1 & 0 & 0 & -\gamma \\ \end{bmatrix}\]

对于人形机器人的扁平足底,6D wrench cone 比 3D 点接触更准确——它能捕捉"脚尖翘起"(CoP 移到足底边缘)这类失稳模式。

TSID 中的 6D 接触:

// TSID: 6D contact for flat foot (humanoid)
tsid::contacts::Contact6d contact_RF(
    "contact_RF", robot, "right_foot_frame",
    contact_normal,    // [0, 0, 1]
    mu,                // friction coefficient
    lx, ly,            // half-dimensions of foot
    fMin, fMax,        // min/max normal force
    w_forceReg         // force regularization weight
);

⚠️ 编程陷阱:k=4 的方向选择

错误: 用 \(\theta_i = 0, \pi/2, \pi, 3\pi/2\)(与 x-y 轴对齐)。

问题: 当 \(k = 4\) 且方向对齐坐标轴时,多面体退化为**方形锥**(box cone)。约束变成 \(|\lambda^x| \le \mu \lambda^z\) 且 \(|\lambda^y| \le \mu \lambda^z\),这过度放松了对角线方向——对角方向上允许 \(\sqrt{2}\mu \lambda^z\) 的摩擦力。

解决: 旋转 45 度,使用 \(\theta_i = \pi/4, 3\pi/4, 5\pi/4, 7\pi/4\),或直接用 \(k = 8\)。

🧠 思维陷阱:线性化精度过度追求

陷阱: "必须用 k=16 才能保证安全。"

现实: 摩擦系数 \(\mu\) 本身的测量误差通常在 10-20%。用 k=4 的近似误差(~27%)与 \(\mu\) 的不确定性是同一量级。在 \(\mu\) 测不准的情况下,追求 k=16 是伪精度。更好的策略是:用 k=4 配合保守的 \(\mu\) 估计(比如真实 \(\mu = 0.7\) 时,控制器用 \(\mu = 0.5\))。

练习

练习 52.2.1 (⭐⭐): 用 Python 画出 \(k = 4, 8, 16\) 的内切和外切多面体锥截面(在 \(\lambda^z = 1\) 平面上),与精确圆对比。标注每种情况的面积误差百分比。

练习 52.2.2 (⭐⭐⭐): 对于一个人形机器人的扁平足底(\(l_x = 0.1\text{m}\), \(l_y = 0.05\text{m}\), \(\mu = 0.6\)),构造完整的 6D wrench cone 约束矩阵 \(U\)。用 Python 验证:当 CoP 刚好在足底边缘时(\(n_c^x = l_y f_c^z\)),约束是否恰好取等号?

练习 52.2.3 (⭐⭐): 在 OCS2 的 FrictionConeConstraint 中,它使用的是内切还是外切近似?\(k\) 的值是多少?阅读源码后回答。

---

52.3 互补约束的处理——三大路线 ⭐⭐⭐

动机:为什么互补约束是足式控制的核心难题?

本节解决的问题:互补约束 \(0 \le \phi \perp \lambda^n \ge 0\) 是非光滑的,传统优化求解器无法直接处理。如何让优化器"知道"何时该有接触力、何时不该有?

反面案例:如果把互补约束直接放进 NLP:

minimize  cost(x, u, lambda)
subject to:
  dynamics: M*ddq + h = S^T*tau + J_c^T*lambda
  complementarity: phi(p_c) * lambda_n = 0
  bounds: phi >= 0, lambda_n >= 0

Ipopt 会怎么做?在 \(\phi = 0, \lambda^n = 0\) 附近,约束 Jacobian \([\lambda^n, \phi]\) 的秩不足(LICQ 违反)——Ipopt 要么不收敛,要么收敛到非物理解。

52.3.1 路线 1:预定义接触模式(Mode-Scheduled) ⭐⭐

核心思想:步态(gait)是预先设计的——控制工程师**告诉**求解器每条腿在每个时刻是"接触"还是"摆动"。

数学实现:

设时域 \([0, T]\) 被分成 \(M\) 个模式段 \([t_{m-1}, t_m]\),每段有一个**接触模式** \(\sigma_m \in \{0, 1\}^{n_c}\),其中 \(n_c\) 是接触点数(四足 = 4)。

在第 \(m\) 段,对第 \(i\) 个接触点:

\[\sigma_m^{(i)} = 1 \implies \begin{cases} \phi_i = 0 & \text{(脚在地面)} \\ \lambda_i^n \ge 0 & \text{(有支撑力)} \\ \dot{\boldsymbol{p}}_{c,i}^t = 0 & \text{(无滑动)} \end{cases}\]

\[\sigma_m^{(i)} = 0 \implies \begin{cases} \phi_i \ge 0 & \text{(脚可能离地)} \\ \lambda_i^n = 0 & \text{(无接触力)} \\ \text{swing trajectory} & \text{(摆腿轨迹)} \end{cases}\]

互补性自动满足:在每个模式段内,\(\phi_i\) 和 \(\lambda_i^n\) 中恰好一个被固定为零,乘积自然为零。

OCS2 中的实现:

// OCS2: ModeSchedule defines contact sequence
// Example: trot gait for quadruped
ModeSchedule modeSchedule;
modeSchedule.eventTimes = {0.2, 0.4, 0.6, 0.8};  // Phase transitions
modeSchedule.modeSequence = {
  9,   // 0b1001: LF + RH in contact (trot phase 1)
  6,   // 0b0110: RF + LH in contact (trot phase 2)
  9,   // LF + RH again
  6,   // RF + LH again
  9    // LF + RH
};
// Each mode number encodes a bit mask: 
//   bit 0 = LF, bit 1 = RF, bit 2 = LH, bit 3 = RH

优缺点分析:

维度	评价
实时性	每段内是凸 QP/NLP,可实时求解
简单性	实现简单,工业界主流
适应性	接触模式不是优化变量,不能自适应地形
新颖步态	不能发现新步态(如用手支撑)
鲁棒性	依赖步态调度器的正确性

52.3.2 路线 2:软化互补约束(Relaxation) ⭐⭐⭐

核心思想:用光滑函数**近似**非光滑的互补条件,使问题变成标准 NLP。

方法 A:松弛(Relaxation)

\[\phi \cdot \lambda^n \le \epsilon, \quad \epsilon > 0\]

允许 \(\phi\) 和 \(\lambda^n\) 同时为小正数。在优化过程中逐步减小 \(\epsilon \to 0\)。

方法 B:对数障碍(Log-Barrier)

\[\min_{\phi, \lambda^n} \; \text{cost} - \kappa \left( \ln(\phi) + \ln(\lambda^n) \right) + \frac{1}{2\rho} (\phi \cdot \lambda^n)^2\]

障碍项 \(-\kappa \ln(\cdot)\) 保证 \(\phi > 0, \lambda^n > 0\);惩罚项 \((\phi \cdot \lambda^n)^2 / (2\rho)\) 推动乘积趋向零。逐步减小 \(\kappa\) 和 \(\rho\)。

方法 C:Fischer-Burmeister (FB) 函数

定义 NCP 函数:

\[\Phi_{\text{FB}}(a, b) = \sqrt{a^2 + b^2} - a - b\]

关键性质:\(\Phi_{\text{FB}}(a, b) = 0 \iff a \ge 0, b \ge 0, ab = 0\)

证明:

• 若 \(a > 0, b = 0\):\(\sqrt{a^2} - a - 0 = |a| - a = 0\) (因为 \(a > 0\))
• 若 \(a = 0, b > 0\):\(\sqrt{b^2} - 0 - b = |b| - b = 0\) (因为 \(b > 0\))
• 若 \(a > 0, b > 0\):\(\sqrt{a^2 + b^2} < a + b\)(因为 \((a+b)^2 = a^2 + 2ab + b^2 > a^2 + b^2\)),所以 \(\Phi_{\text{FB}} < 0 \ne 0\)。
• 若 \(a < 0\) 或 \(b < 0\):\(\sqrt{a^2+b^2} \ge |a| > a\)(若 \(a < 0\)),所以 \(\Phi_{\text{FB}} > 0 \ne 0\)。

因此 \(\Phi_{\text{FB}} = 0\) 精确等价于互补条件。\(\square\)

把互补约束替换为:

\[\Phi_{\text{FB}}(\phi, \lambda^n) = 0\]

这是一个**等式约束**,且在 \(a > 0\) 或 \(b > 0\) 时可微。不可微点只在 \((0, 0)\),可以用**正则化**处理:

\[\Phi_{\text{FB}}^{\epsilon}(a, b) = \sqrt{a^2 + b^2 + \epsilon^2} - a - b\]

当 \(\epsilon > 0\) 时,\(\Phi_{\text{FB}}^{\epsilon}\) 处处可微,且 \(\epsilon \to 0\) 时逼近原始 FB 函数。

三种软化方法对比:

方法	光滑性	收敛速度	精度	实现复杂度
松弛	光滑	慢(线性)	\(O(\epsilon)\)	低
Log-Barrier	光滑	中等	\(O(\kappa)\)	中
Fischer-Burmeister	半光滑	快(超线性)	精确	高

⚠️ 编程陷阱:松弛参数的调度

错误: 一开始就设 \(\epsilon = 10^{-8}\)。

症状: 优化器不收敛,NaN 爆炸。

根因: \(\epsilon\) 太小时问题的条件数极差。

正确做法: 从 \(\epsilon = 0.1\) 开始,每次优化收敛后减半,直到 \(\epsilon < 10^{-4}\)。典型的 continuation schedule: \(\epsilon \in \{0.1, 0.05, 0.01, 0.005, 0.001\}\)。

💡 概念误区:Fischer-Burmeister 的光滑性

错误理解: "FB 函数是光滑的,可以直接放进 Ipopt。"

正确理解: 原始 FB 函数在 \((0,0)\) 处不可微(半光滑)。需要用正则化版本 \(\Phi_{\text{FB}}^{\epsilon}\),或者用专门的半光滑牛顿法。Ipopt 需要二阶导数,而 FB 在 \((0,0)\) 附近的 Hessian 不存在。

52.3.3 路线 3:Contact-Implicit Trajectory Optimization (CITO) ⭐⭐⭐⭐

核心思想:把接触位置、时刻和力**全部作为决策变量**,让优化器自主发现最优接触策略。

标准 CITO 公式(Posa et al. 2014):

\[\boxed{\begin{aligned} \min_{x_{0:N}, u_{0:N-1}, \lambda_{0:N}} \quad & \sum_{k=0}^{N-1} l(x_k, u_k) + l_f(x_N) \\ \text{s.t.} \quad & x_{k+1} = f(x_k, u_k, \lambda_k) & \text{(dynamics)} \\ & 0 \le \phi(x_k) \perp \lambda_k^n \ge 0 & \text{(normal complementarity)} \\ & \|\boldsymbol{\lambda}_k^t\| \le \mu \lambda_k^n & \text{(friction cone)} \\ & 0 \le (\mu \lambda_k^n - \|\boldsymbol{\lambda}_k^t\|) \perp \|\dot{\boldsymbol{p}}_c^t\| \ge 0 & \text{(sliding complementarity)} \end{aligned}}\]

最后一条是**滑动互补**:如果摩擦力没有饱和(在锥内部),则接触点不滑动。

最新进展(2024-2026):

论文	核心创新	关键指标
Pang et al. 2023, T-RO	局部光滑化 + quasi-dynamic 模型	首次全局 CITO for manipulation
Kurtz et al. 2024, IJRR	Inverse dynamics CITO	实时 CITO-MPC > 100Hz
STOCS 2025, T-RO	无穷规划框架,动态实例化接触点	高保真几何 3D manipulation
ci-SCvx 2026, arXiv	GPU 加速连续时间逐次凸化	比 SCP SOTA 快 10x+

CITO 的根本困难:

1. 组合爆炸: \(n_c\) 个接触点,\(N\) 个时间步,最多 \(2^{n_c \cdot N}\) 种可能的接触模式序列
2. 非凸非光滑: 互补约束导致可行域非凸
3. 局部极小值: 不同的接触策略对应不同的局部最优
4. 计算代价: 典型 CITO 需要分钟到小时级求解

玩具示例:2D 弹跳球:

// Simplified CITO: 2D ball bouncing on ground
// Decision variables: [x(t), z(t), lambda_n(t)] for t = 0..N
// Dynamics: m*ddz = lambda_n - m*g, m*ddx = 0
// Complementarity: 0 <= z _|_ lambda_n >= 0 (relaxed: z*lambda_n <= eps)

#include <ifopt/problem.h>
#include <ifopt/ipopt_solver.h>

class BouncingBallVariables : public ifopt::VariableSet {
public:
  static constexpr int N = 50;
  static constexpr double dt = 0.02;

  // Variables: z_0..z_N, lambda_0..lambda_N
  BouncingBallVariables() : VariableSet(2 * (N + 1), "ball_vars") {
    // Initialize: free fall trajectory
    for (int k = 0; k <= N; ++k) {
      double t = k * dt;
      x_(k) = 1.0 - 0.5 * 9.81 * t * t;  // z position
      x_(N + 1 + k) = 0.0;                 // lambda_n = 0
    }
  }

  VectorXd GetValues() const override { return x_; }
  void SetVariables(const VectorXd& x) override { x_ = x; }
  VecBound GetBounds() const override {
    VecBound bounds(GetRows());
    for (int k = 0; k <= N; ++k) {
      bounds[k] = ifopt::Bounds(0.0, ifopt::inf);     // z >= 0
      bounds[N+1+k] = ifopt::Bounds(0.0, ifopt::inf); // lambda >= 0
    }
    return bounds;
  }
private:
  VectorXd x_ = VectorXd::Zero(2 * (N + 1));
};

class ComplementarityConstraint : public ifopt::ConstraintSet {
public:
  static constexpr double eps = 1e-3;  // Relaxation
  static constexpr int N = 50;

  ComplementarityConstraint() : ConstraintSet(N + 1, "compl") {}

  VectorXd GetValues() const override {
    auto vars = GetVariables()->GetComponent("ball_vars")->GetValues();
    VectorXd g(N + 1);
    for (int k = 0; k <= N; ++k) {
      g(k) = vars(k) * vars(N + 1 + k);  // z_k * lambda_k
    }
    return g;
  }

  VecBound GetBounds() const override {
    VecBound b(N + 1);
    for (int k = 0; k <= N; ++k)
      b[k] = ifopt::Bounds(-ifopt::inf, eps);  // z*lambda <= eps
    return b;
  }

  void FillJacobianBlock(std::string var_set, 
                          Jacobian& jac) const override {
    if (var_set == "ball_vars") {
      auto vars = GetVariables()->GetComponent("ball_vars")->GetValues();
      for (int k = 0; k <= N; ++k) {
        jac.coeffRef(k, k) = vars(N + 1 + k);       // d(z*l)/dz = l
        jac.coeffRef(k, N + 1 + k) = vars(k);        // d(z*l)/dl = z
      }
    }
  }
};

🧠 思维陷阱:CITO 能替代步态设计吗?

陷阱: "有了 CITO,就不需要人工设计步态了。"

现实: 目前(2026 年)CITO 在腿足机器人上仍然是**离线工具**——用于发现新步态或验证概念。实时控制仍然依赖预定义步态 + MPC/WBC。原因:CITO 的求解时间(秒级)远超实时要求(毫秒级)。Kurtz et al. 2024 首次展示了 100Hz CITO-MPC,但仅限于操作任务(自由度低)。ci-SCvx(2026)在 GPU 上实现了快速 CITO,但离腿足实时部署仍有距离。

练习

练习 52.3.1 (⭐⭐⭐): 实现正则化 Fischer-Burmeister 函数 \(\Phi_{\text{FB}}^{\epsilon}(a, b)\),计算其梯度 \(\partial \Phi / \partial a\) 和 \(\partial \Phi / \partial b\)。画出 \(\epsilon = 0.1, 0.01, 0.001\) 时的函数值热力图(以 \(a, b\) 为轴)。

练习 52.3.2 (⭐⭐⭐⭐): 用 Ifopt + Ipopt 实现上述 2D 弹跳球 CITO。初始条件 \(z(0) = 1\), \(\dot{z}(0) = 0\)。优化器能否自动发现"自由落体 → 接触地面 → 产生支撑力 → 停住"的序列?

练习 52.3.3 (⭐⭐): 比较三种软化方法(松弛/log-barrier/FB)在以下指标上的差异:收敛迭代次数、最终互补性违反量 \(\max_k \phi_k \cdot \lambda_k^n\)、总计算时间。用一个简单的 1D 弹跳球例子。

---

52.4 接触 Jacobian 深入——从速度到广义力 ⭐⭐

前三节解决了"接触力应满足什么约束"以及"如何在优化中处理这些约束"。但还有一个关键环节尚未打通:接触力作用在足端,而动力学方程写在关节空间——两者之间的桥梁就是接触 Jacobian。

动机:为什么接触 Jacobian 是力传递的核心?

本节解决的问题:接触力 \(\boldsymbol{\lambda}_c\) 作用在足端,但动力学方程是在关节空间写的。如何把足端力"翻译"成关节空间的广义力?反过来,如何从关节速度得到足端速度?

反面案例:忘记接触 Jacobian 的后果——

场景: 你手动在动力学方程中直接加了 lambda_z 到基座z方向
错误: M*ddq + h = S^T*tau + [0,0,lambda_z,0,...,0]^T

正确: M*ddq + h = S^T*tau + J_c^T * lambda_c
       J_c^T 把力从接触空间映射到关节空间
       它不仅影响基座,还影响接触链上的所有关节!

52.4.1 接触 Jacobian 的推导 ⭐⭐

运动学链:接触点 \(\boldsymbol{p}_c\) 是广义坐标 \(q\) 的函数:

\[\boldsymbol{p}_c = \boldsymbol{p}_c(q) \in \mathbb{R}^3\]

对时间求导(链式法则):

\[\dot{\boldsymbol{p}}_c = \frac{\partial \boldsymbol{p}_c}{\partial q} \dot{q} = \boldsymbol{J}_c(q) \dot{q}\]

其中 \(\boldsymbol{J}_c(q) \in \mathbb{R}^{3 \times n_v}\) 就是接触 Jacobian(线速度部分)。

完整 6D Jacobian:如果我们关心接触点的角速度(例如面接触),则用 6D Jacobian:

\[\begin{bmatrix} \boldsymbol{\omega}_c \\ \dot{\boldsymbol{p}}_c \end{bmatrix} = \boldsymbol{J}_c^{6D}(q) \dot{q}, \quad \boldsymbol{J}_c^{6D} \in \mathbb{R}^{6 \times n_v}\]

对偶关系(Virtual Work Principle):

由虚功原理,接触力 \(\boldsymbol{\lambda}_c\) 在虚位移 \(\delta \boldsymbol{p}_c\) 上做的虚功等于对应广义力在虚广义位移 \(\delta q\) 上做的虚功:

\[\boldsymbol{\lambda}_c^T \delta \boldsymbol{p}_c = \boldsymbol{\lambda}_c^T \boldsymbol{J}_c \delta q = (\boldsymbol{J}_c^T \boldsymbol{\lambda}_c)^T \delta q\]

因此,接触力在广义坐标空间中等价于:

\[\boxed{\boldsymbol{\tau}_{\text{contact}} = \boldsymbol{J}_c^T \boldsymbol{\lambda}_c}\]

这就是动力学方程 \(M \ddot{q} + h = S^T \tau + \boldsymbol{J}_c^T \boldsymbol{\lambda}_c\) 中接触力项的来源。

本质洞察:\(J_c\) 和 \(J_c^T\) 构成一对**对偶映射**——\(J_c\) 把关节速度"翻译"成接触点速度(运动方向),而 \(J_c^T\) 把接触力"翻译"成关节广义力(力方向)。这不是巧合,而是**虚功原理的必然结果**:能量守恒要求运动和力的传递使用同一个线性映射的转置。这个对偶关系贯穿整个机器人学——从末端执行器力控到 WBC 的约束力分配,都依赖于此。

52.4.2 接触 Jacobian 的时间导数与漂移项 ⭐⭐⭐

为什么需要 \(\dot{\boldsymbol{J}}_c\)?

在约束接触(脚固定在地面)的情况下,加速度级约束为:

\[\ddot{\boldsymbol{p}}_c = \boldsymbol{J}_c \ddot{q} + \dot{\boldsymbol{J}}_c \dot{q} = 0\]

\(\dot{\boldsymbol{J}}_c \dot{q}\) 称为**漂移项(drift term / bias acceleration)**。如果忽略它,约束加速度 \(\ddot{\boldsymbol{p}}_c \ne 0\),接触点会逐渐漂离地面(constraint drift)。

Pinocchio 的漂移计算:

// Method 1: Use getFrameClassicalAcceleration (includes drift)
pinocchio::forwardKinematics(model, data, q, v, 
                              Eigen::VectorXd::Zero(model.nv));
pinocchio::updateFramePlacements(model, data);

auto drift = pinocchio::getFrameClassicalAcceleration(
    model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED);
// drift.linear() = dJ * dq (when joint accelerations are zero)

// Method 2: Explicit computation of dJ
pinocchio::computeJointJacobiansTimeVariation(model, data, q, v);
Eigen::MatrixXd dJ(6, model.nv);
dJ.setZero();
pinocchio::getFrameJacobianTimeVariation(
    model, data, frame_id, pinocchio::LOCAL_WORLD_ALIGNED, dJ);
Eigen::VectorXd drift_term = dJ * v;  // 6x1 bias acceleration

Baumgarte 稳定化:

即使正确计算了漂移项,数值积分仍可能导致微小的约束漂移。Baumgarte 稳定化在加速度约束中加入位置和速度修正:

\[\boldsymbol{J}_c \ddot{q} + \dot{\boldsymbol{J}}_c \dot{q} + 2\alpha \dot{\boldsymbol{p}}_c + \beta^2 (\boldsymbol{p}_c - \boldsymbol{p}_c^{\text{ref}}) = 0\]

其中 \(\alpha, \beta > 0\) 是稳定化增益。典型值:\(\alpha = \beta = 50\)。

⚠️ 编程陷阱:忘记漂移项

错误: 在 WBC 中写接触约束为 \(J_c \ddot{q} = 0\)。

症状: 仿真中脚缓慢滑动,数秒后偏移数厘米。

根因: 缺少 \(\dot{J}_c \dot{q}\) 项。正确的约束是 \(J_c \ddot{q} = -\dot{J}_c \dot{q}\)。

进一步: 即使加了漂移项,仍需 Baumgarte 稳定化来抑制数值积分累积误差。

💡 概念误区:Crocoddyl 的隐式约束处理

问题: Crocoddyl 的 DifferentialActionModelContactFwdDynamics 看起来没有显式的 \(J_c \ddot{q} = -\dot{J}_c \dot{q}\) 约束,它是怎么处理的?

答案: Crocoddyl 调用 pinocchio::forwardDynamics(),该函数内部求解 KKT 系统: $\(\begin{bmatrix} M & J_c^T \\ J_c & 0 \end{bmatrix} \begin{bmatrix} \ddot{q} \\ -\lambda \end{bmatrix} = \begin{bmatrix} \tau - h \\ -\dot{J}_c \dot{q} \end{bmatrix}\)$ 约束 \(J_c \ddot{q} = -\dot{J}_c \dot{q}\) 被**隐式**嵌入在 KKT 系统中。a0(drift acceleration)就是 \(-\dot{J}_c \dot{q}\)。添加 JMinvJt_damping 可以在 \(J_c\) 不满秩时保证矩阵可逆。

52.4.3 参考系选择:LOCAL vs WORLD vs LOCAL_WORLD_ALIGNED ⭐⭐

Pinocchio 提供三种参考系选项,它们对接触 Jacobian 的计算至关重要:

参考系	数学定义	Jacobian 结构	摩擦锥自然性
LOCAL	速度在 frame 自身坐标系下表达	稀疏(只含关节链)	摩擦锥方向随 frame 旋转
WORLD	速度在世界系下表达,参考点在世界原点	包含额外的叉乘项	但有远距离叉乘误差
LOCAL_WORLD_ALIGNED (LWA)	速度在世界系朝向下表达,参考点在 frame 处	兼顾稀疏性和直觉性	推荐用于接触力学

为什么 LWA 最适合接触力学?

摩擦锥 \(\|\lambda^{xy}\| \le \mu \lambda^z\) 假设 \(z\) 轴是接触面法向。在世界系中(水平地面),法向就是 \(z\) 轴——直接匹配。如果用 LOCAL 参考系,法向方向随 frame 旋转,每次都要旋转摩擦锥约束矩阵,增加复杂性且容易出错。

代码示例:对比三种参考系:

// Compare Jacobians in different reference frames
Eigen::MatrixXd J_local(6, model.nv), J_world(6, model.nv), J_lwa(6, model.nv);
J_local.setZero(); J_world.setZero(); J_lwa.setZero();

pinocchio::computeJointJacobians(model, data, q);
pinocchio::updateFramePlacements(model, data);

pinocchio::getFrameJacobian(model, data, foot_frame_id,
                             pinocchio::LOCAL, J_local);
pinocchio::getFrameJacobian(model, data, foot_frame_id,
                             pinocchio::WORLD, J_world);
pinocchio::getFrameJacobian(model, data, foot_frame_id,
                             pinocchio::LOCAL_WORLD_ALIGNED, J_lwa);

// Relationship: J_lwa = [R_wf, 0; 0, R_wf] * J_local
// where R_wf is the frame orientation in world frame
// J_world involves additional cross-product terms due to 
// the reference point being at the world origin

🧠 思维陷阱:WORLD 参考系的"远距离"问题

陷阱: "WORLD 参考系最自然,应该总是用它。"

问题: WORLD 参考系的参考点在世界原点。当接触点远离原点时,角速度分量会引入大量叉乘项 \(\omega \times r\),导致 Jacobian 数值条件差,且 \(J^T \lambda\) 的物理含义不直观。

推荐: 使用 LOCAL_WORLD_ALIGNED——参考点在接触 frame 处(避免远距离叉乘),方向用世界系(摩擦锥自然表达)。

练习

练习 52.4.1 (⭐⭐): 对一个 3-DOF 平面机械臂(基座固定,2 个旋转关节 + 末端),手动推导接触 Jacobian \(J_c(q)\)。验证 \(J_c^T \lambda\) 给出的关节力矩与你用力矩平衡直接计算的结果一致。

练习 52.4.2 (⭐⭐⭐): 用 Pinocchio 计算 Go2 四足机器人前左脚的 Jacobian,分别用三种参考系。验证关系:\(J_{\text{LWA}} = \text{diag}(R_{wf}, R_{wf}) \cdot J_{\text{LOCAL}}\)。

练习 52.4.3 (⭐⭐): 解释为什么 Crocoddyl 的 DifferentialActionModelContactFwdDynamics 构造时需要 JMinvJt_damping 参数。当 \(J_c\) 满秩时,该参数设为 0 会有问题吗?

---

52.5 Centroidal Momentum Matrix (CMM) 与 CCRBA ⭐⭐⭐

动机:为什么需要 CMM?

本节解决的问题:Centroidal Dynamics(\(\dot{h}_G = \sum \text{external wrenches}\))中的 \(h_G\)——系统在质心处的 6 维动量——如何从关节状态 \((q, \dot{q})\) 高效计算?朴素方法需要遍历所有 link 逐个累加,复杂度 O(N^2)。CMM 给出一个线性映射,且 CCRBA 算法实现了 O(N) 计算。

反面案例:如果你用朴素方法计算 Centroidal Momentum:

# WRONG: O(N^2) brute force
h_G = np.zeros(6)
for link in robot.links:
    # Get link CoM velocity in world frame - O(N) per link
    v_link = get_link_com_velocity(link, q, dq)
    # Get link spatial inertia
    I_link = link.spatial_inertia
    # Transform to centroidal frame and accumulate
    h_G += transform_to_com(I_link @ v_link, com)

# Total: O(N) per link x N links = O(N^2)
# For a humanoid with 30+ links, this is wasteful

52.5.1 CMM 的数学定义 ⭐⭐

Centroidal Momentum \(\boldsymbol{h}_G \in \mathbb{R}^6\) 是系统所有 link 在质心 \(G\) 处的空间动量之和:

\[\boldsymbol{h}_G = \begin{bmatrix} \boldsymbol{k}_G \\ \boldsymbol{l}_G \end{bmatrix} = \sum_{i=1}^{N_b} {}^G\boldsymbol{X}_i^* \boldsymbol{I}_i \boldsymbol{v}_i\]

其中: - \(\boldsymbol{k}_G \in \mathbb{R}^3\):绕质心的角动量 - \(\boldsymbol{l}_G \in \mathbb{R}^3\):线动量(\(= m \dot{\boldsymbol{p}}_G\)) - \({}^G\boldsymbol{X}_i^*\):从 link \(i\) 到质心 \(G\) 的空间力变换 - \(\boldsymbol{I}_i\):link \(i\) 的空间惯量 - \(\boldsymbol{v}_i\):link \(i\) 的空间速度

CMM 的线性性:由于 \(\boldsymbol{v}_i\) 是 \(\dot{q}\) 的线性函数(通过运动学链),整个 \(\boldsymbol{h}_G\) 也是 \(\dot{q}\) 的线性函数:

\[\boxed{\boldsymbol{h}_G = \boldsymbol{A}_G(q) \dot{q}}\]

\(\boldsymbol{A}_G(q) \in \mathbb{R}^{6 \times n_v}\) 就是 Centroidal Momentum Matrix (CMM)。它依赖构型 \(q\),但不依赖速度 \(\dot{q}\)。

52.5.2 CMM 的分块结构与物理含义 ⭐⭐

将 CMM 按基座和关节分块:

\[\boldsymbol{A}_G = \begin{bmatrix} \boldsymbol{A}_{G,\text{base}}^{6 \times 6} & \boldsymbol{A}_{G,\text{joints}}^{6 \times (n_v - 6)} \end{bmatrix}\]

进一步分解基座部分:

\[\boldsymbol{A}_{G,\text{base}} = \begin{bmatrix} \boldsymbol{A}_{G,\omega\omega} & \boldsymbol{A}_{G,\omega v} \\ \boldsymbol{A}_{G,v\omega} & \boldsymbol{A}_{G,vv} \end{bmatrix}\]

关键性质 1: 线动量子块

线动量 \(\boldsymbol{l}_G = m \dot{\boldsymbol{p}}_G\)。由于质心速度对基座线速度的偏导数是 \(m \boldsymbol{I}_3\)(总质量乘以单位阵),CMM 的对应子块**总是** \(m \boldsymbol{I}_3\)——与构型 \(q\) 无关:

\[\frac{\partial \boldsymbol{l}_G}{\partial \dot{\boldsymbol{p}}_{\text{base}}} = m \boldsymbol{I}_3\]

关键性质 2: Centroidal 惯量矩阵 \(\boldsymbol{I}_G\) (CCRBI)

\[\boldsymbol{I}_G = \boldsymbol{A}_G M^{-1} \boldsymbol{A}_G^T \in \mathbb{R}^{6 \times 6}\]

\(\boldsymbol{I}_G\) 称为 Centroidal Composite Rigid Body Inertia (CCRBI)——它描述了"如果整个机器人被焊成一个刚体,该刚体在质心处的空间惯量"。CCRBI 随构型 \(q\) 变化(因为 link 之间的相对位置改变了)。

关键性质 3: CMM 与全身质量矩阵 \(M\) 的关系

\[\boldsymbol{A}_G(q) = \sum_{i=1}^{N_b} {}^G\boldsymbol{X}_i^* \boldsymbol{I}_i \boldsymbol{J}_i(q)\]

其中 \(\boldsymbol{J}_i\) 是 link \(i\) 质心的空间 Jacobian。注意 \(A_G\) 的维度是 \(6 \times n_v\)(从关节空间到 6D 动量空间),而 \(M\) 的维度是 \(n_v \times n_v\)(关节空间到关节空间)。

52.5.3 CCRBA 算法详解 ⭐⭐⭐

CCRBA (Centroidal Composite Rigid Body Algorithm) 是 Orin et al. 2013 提出的 O(N) 算法。它的核心思想是**从叶子到根递推复合惯量,然后从根到叶子计算 CMM 的列**。这好比计算一棵企业组织树的"总薪资":先从基层员工向上逐级汇总(叶到根),得到每个部门的总人力成本;再从总部向下分配预算(根到叶)。CCRBA 用同样的递推逻辑汇总空间惯量,避免了逐个 link 独立计算带来的 O(N^2) 重复遍历。

算法步骤(伪代码):

Algorithm: CCRBA (Orin et al. 2013)
Input:  model (kinematic tree), q (configuration), v (velocity)
Output: A_G (CMM), h_G (centroidal momentum), I_G (CCRBI)

// Phase 1: Forward Kinematics (root to leaves)
for i = 1 to N_b do
    Compute joint transform X_J(q_i)
    X_i = X_J(q_i) * X_T(i)         // Frame transform for link i
    v_i = X_i * v_{parent(i)} + S_i * dq_i  // Spatial velocity
end

// Phase 2: Composite Inertia (leaves to root) - same as CRBA
for i = N_b downto 1 do
    I_c(i) = I(i)                     // Start with own inertia
    for each child j of i do
        I_c(i) += X_j^* * I_c(j) * X_j^{-1}  // Accumulate children
    end
end

// Phase 3: CoM and Centroidal Transform
p_G = (1/m) * sum_i m_i * p_i        // Center of mass
X_G = spatial_transform(R=I, p=p_G)  // Transform to centroidal frame

// Phase 4: CMM Columns (root to leaves)
I_G = X_G^* * I_c(root) * X_G^{-1}  // CCRBI
for i = 1 to N_b do
    // Column of A_G corresponding to joint i
    A_G(:, joint_i) = X_G^* * I_c(i) * S_i
end

// Phase 5: Centroidal Momentum
h_G = A_G * v

复杂度分析:

步骤	操作	复杂度
Phase 1	正向运动学	O(N)
Phase 2	复合惯量	O(N)
Phase 3	质心计算	O(N)
Phase 4	CMM 列	O(N)
Phase 5	矩阵-向量乘法	O(N * nv)
总计		O(N * nv)

对比朴素方法的 O(N^2 * nv),当 N = 30(nv ~ 36)时加速约 30 倍。

Pinocchio 调用:

#include <pinocchio/algorithm/centroidal.hpp>

// Compute CMM, centroidal momentum, and CCRBI
pinocchio::ccrba(model, data, q, v);

// Results:
Eigen::MatrixXd A_G = data.Ag;   // CMM: 6 x nv
Eigen::Matrix<double, 6, 1> h_G = data.hg;  // Centroidal momentum
Eigen::Vector3d com = data.com[0]; // Center of mass position
Eigen::Matrix<double, 6, 6> I_G = data.Ig;  // CCRBI: 6 x 6

// Verify: h_G should equal A_G * v
assert((h_G - A_G * v).norm() < 1e-10);

// Also available: time derivative of CMM
pinocchio::dccrba(model, data, q, v);
Eigen::MatrixXd dA_G = data.dAg;  // dA_G/dt: 6 x nv
// hdot_G = dA_G * v + A_G * a  (where a = ddq)

⚠️ 编程陷阱:ccrba() vs computeCentroidalMomentum()

错误: 混用 ccrba() 和 computeCentroidalMomentum()。

区别: ccrba() 计算 CMM 矩阵 \(A_G\) + 动量 \(h_G\);computeCentroidalMomentum() 只计算 \(h_G\)(不构造 \(A_G\)),更快但无法用于 WBC 约束公式。如果你需要 \(A_G\)(比如在 MPC 中约束角动量率),必须调用 ccrba()。

💡 概念误区:CMM 与质量矩阵 M 的混淆

错误理解: "CMM 就是质量矩阵 M 的前 6 行。"

正确理解: \(A_G\) 把 \(\dot{q}\) 映射到质心处的 6D 动量;\(M\) 把 \(\ddot{q}\) 映射到关节空间的广义力。两者维度不同(\(A_G\): \(6 \times n_v\), \(M\): \(n_v \times n_v\))。\(A_G\) 涉及到质心的坐标变换,不是简单地截取 \(M\) 的某些行。

🧠 思维陷阱:CCRBI 是常数?

陷阱: "系统的空间惯量不随构型变化。"

反例: 当四足机器人蜷缩四肢时,绕质心的转动惯量远小于四肢伸展时——这就是 CCRBI \(I_G\) 依赖 \(q\) 的原因。花样滑冰运动员收臂加速旋转就是这个原理。

练习

练习 52.5.1 (⭐⭐): 用 Pinocchio 对 Go2 计算 ccrba()。验证 \(A_G\) 的线动量子块确实接近 \(m I_3\)(\(m\) 是总质量)。精确等于还是近似等于?为什么?

练习 52.5.2 (⭐⭐⭐): 计算 \(I_G\)(CCRBI)在不同构型下的变化。当四条腿完全伸直 vs 完全蜷缩时,\(I_G\) 的角动量子块(左上 \(3 \times 3\))差异有多大?物理解释是什么?

练习 52.5.3 (⭐⭐⭐): 验证 Centroidal 动力学恒等式:\(\dot{h}_G = \sum_c {}^G X_c^* \lambda_c + m [0^T, g^T]^T\)。用 Pinocchio 的 ABA(前向动力学)计算 \(\ddot{q}\),再用 \(dA_G \dot{q} + A_G \ddot{q}\) 验证左边等于右边。

---

52.6 角动量的特殊地位——守恒与可控性 ⭐⭐

上一节展示了如何通过 CMM 高效计算系统的 6 维 Centroidal 动量。其中线动量 \(l_G = m\dot{p}_G\) 的行为相对直观,但角动量 \(k_G\) 蕴含着更加微妙且对控制至关重要的物理性质,值得专门展开讨论。

动机:为什么角动量值得单独讨论?

本节解决的问题:线动量 \(\boldsymbol{l}_G = m \dot{\boldsymbol{p}}_G\) 的规律简单——外力之和改变线动量。但角动量 \(\boldsymbol{k}_G\) 有微妙得多的性质:它在飞行相位守恒,在接触相位只能通过"力的力臂"改变。理解这些性质对跳跃、翻滚、步行摆臂都至关重要。

52.6.1 角动量动力学 ⭐⭐

线动量与角动量的演化方程:

\[\dot{\boldsymbol{l}}_G = m \boldsymbol{g} + \sum_c \boldsymbol{f}_c\]

\[\dot{\boldsymbol{k}}_G = \sum_c (\boldsymbol{p}_c - \boldsymbol{p}_G) \times \boldsymbol{f}_c\]

关键区别:

特征	线动量 \(\boldsymbol{l}_G\)	角动量 \(\boldsymbol{k}_G\)
外力影响	所有外力直接叠加	只有相对质心有"力臂"的力才产生力矩
重力影响	\(m\boldsymbol{g}\) 直接改变	零! 重力过质心,力臂为零
无接触时	\(\dot{l}_G = mg\) (自由落体)	\(\dot{k}_G = 0\) (守恒!)
单点接触	可自由改变(改变力大小)	力矩方向受限(只能绕接触点旋转)

52.6.2 飞行相位的角动量守恒 ⭐⭐

定理:当机器人完全离开地面(飞行相位)时,重力绕质心的力矩为零,因此:

\[\dot{\boldsymbol{k}}_G = 0 \implies \boldsymbol{k}_G = \text{const}\]

推论 1:跳跃中不能"转身"

如果起跳时 \(k_G^z = 0\)(无偏航角动量),那么整个飞行阶段 \(k_G^z = 0\)——机器人不可能在空中改变偏航角。

这意味着:如果跳跃目标需要落地时朝向不同方向,必须在**起跳前**就注入足够的角动量。

推论 2:翻跟斗需要精确的起跳角动量

后空翻(backflip)需要起跳时提供精确的 \(k_G^y\)(绕俯仰轴),使得在飞行时间 \(T\) 内恰好旋转 \(2\pi\):

\[k_G^y = I_{yy} \cdot \frac{2\pi}{T}\]

\(T\) 由起跳竖直速度决定(\(T = 2v_z / g\)),而 \(I_{yy}\) 在飞行中可以通过蜷缩/伸展改变——这就是体操运动员的技巧。

52.6.3 猫翻身悖论:零角动量下的姿态改变 ⭐⭐⭐

悖论:猫从倒置位置自由落下,能在空中翻转 180 度平稳落地。但飞行中角动量守恒,初始 \(k_G = 0\),怎么可能旋转?

解答:猫**不是作为刚体旋转**,而是通过**关节内部运动**改变姿态:

1. 前半身蜷缩(减小转动惯量),后半身伸展(增大转动惯量)
2. 前半身快速旋转 \(\Delta\theta_1\)(角动量 \(I_1 \dot{\theta}_1\))
3. 后半身反向慢速旋转 \(\Delta\theta_2\)(角动量 \(-I_2 \dot{\theta}_2\))
4. 由于 \(I_1 < I_2\) 但 \(I_1 \dot{\theta}_1 = I_2 \dot{\theta}_2\),所以 \(\Delta\theta_1 > \Delta\theta_2\)
5. 重复多次,净旋转逐步累积

数学表达:

\[k_G = I_{\text{front}} \dot{\theta}_{\text{front}} + I_{\text{rear}} \dot{\theta}_{\text{rear}} = 0\]

\[\implies \dot{\theta}_{\text{rear}} = -\frac{I_{\text{front}}}{I_{\text{rear}}} \dot{\theta}_{\text{front}}\]

由于 \(I_{\text{front}} \ne I_{\text{rear}}\)(不对称蜷缩),\(|\dot{\theta}_{\text{front}}| \ne |\dot{\theta}_{\text{rear}}|\),净旋转角非零。

这是 Centroidal 模型的盲区:Centroidal 模型只看总角动量 \(k_G = 0\),无法捕捉内部角动量重分配。需要全身动力学模型。

52.6.4 工程应用:摆臂稳定化与 Flywheel 模型 ⭐⭐

人形机器人行走中的角动量问题:

行走时腿的摆动产生角动量:左腿前摆产生正向角动量;右腿前摆也产生正向角动量。如果不补偿,躯干会逐渐向后旋转。

解决方案 1:摆臂(Arm Swing)

手臂反向摆动产生反向角动量,抵消腿的角动量:

\[k_G^y \approx k_{\text{legs}}^y + k_{\text{arms}}^y = 0\]

解决方案 2:Flywheel 模型

在 LIPM 的基础上增加一个"飞轮"——躯干的角动量自由度:

\[\dot{k}_G^y = \sum_c (\boldsymbol{p}_c - \boldsymbol{p}_G) \times \boldsymbol{f}_c \cdot \hat{y}\]

Flywheel 模型保留了 LIPM 的简单性,同时能捕捉角动量的变化——这对快速行走和跑步至关重要。

角动量在 MPC 中的约束形式:

// In OCS2/WBC: constrain angular momentum rate
// h_dot_angular = sum_c (p_c - p_G) x f_c
// Often soft-constrained: minimize ||h_dot_angular||^2
// This encourages "zero angular momentum rate" = smooth motion

// Hard constraint version (bound total angular momentum):
// |k_G| <= k_max
// This prevents excessive spinning

// Centroidal MPC cost term:
Eigen::Matrix<double, 6, 6> Q_centroidal;
Q_centroidal.setZero();
Q_centroidal.topLeftCorner<3, 3>() = w_angular * Eigen::Matrix3d::Identity();
// Weight angular momentum tracking (usually toward zero)

💡 概念误区:LIPM 假设 \(k_G = 0\) 的限制

LIPM 假设角动量恒为零:\(k_G = 0\)。

成立条件: 慢速行走(< 0.5 m/s),双足支撑时间长,腿的摆动角动量小。

严重失效条件: 快速行走(> 1 m/s)、跑步、跳跃、在不平地形上行走。此时必须升级到 Centroidal 模型(保留角动量自由度)或 Flywheel 模型。

🧠 思维陷阱:角动量"越小越好"

陷阱: 在 MPC 中强制 \(k_G = 0\)。

反例: 跳跃前需要累积角动量;在扰动恢复时,适量角动量帮助保持平衡(摆臂效应)。正确做法:软约束 \(\min \|k_G\|^2\),允许必要时偏离零。

练习

练习 52.6.1 (⭐⭐): 一个四足机器人从 0.5m 高度自由落体。起跳时 \(k_G = 0\)。落地前身体绕俯仰轴偏转了 5 度。这是否违反角动量守恒?解释原因(提示:考虑空气阻力和内部关节运动)。

练习 52.6.2 (⭐⭐⭐): 用 Pinocchio 仿真:一个简化的"猫"模型(2-body planar robot,通过关节连接)。初始角动量为零,通过控制关节角度实现 180 度翻转。提示:交替改变两个 body 的惯量(通过改变关节角)。

练习 52.6.3 (⭐⭐): 解释为什么人形机器人 Atlas (Boston Dynamics) 在跑步时摆臂幅度很大,而四足机器人 Spot 不需要摆臂。从角动量的角度分析。

---

52.7 非线性摩擦锥 vs 线性化——精度与性能的工程权衡 ⭐⭐⭐

动机:线性化够好了吗?

本节解决的问题:52.2 节展示了如何线性化摩擦锥。但在实际 WBC/MPC 中,线性化带来多大误差?什么时候必须用精确 SOC 锥?

52.7.1 系统性精度对比 ⭐⭐

实验设置:四足机器人 WBC 力分配问题,四脚接触,\(\mu = 0.6\),目标:用最小力抗重力。

约束类型	求解器	求解时间	最大摩擦违反	力分配均匀度	约束条数
线性锥 \(k=4\)	OSQP	0.3 ms	7.6% 可能超出	95.2%	4x4=16
线性锥 \(k=4\) (内切)	OSQP	0.3 ms	0% (保守)	93.1%	16
线性锥 \(k=8\)	OSQP	0.5 ms	1.1%	97.8%	4x8=32
线性锥 \(k=16\)	OSQP	0.8 ms	0.15%	99.2%	4x16=64
精确 SOC	Mosek	2.5 ms	0%	100%	4 (SOC)
精确 SOC	ECOS	3.2 ms	0%	100%	4 (SOC)
精确 SOC	ProxQP	1.8 ms	0%	99.9%	4 (SOC)

关键发现:

1. \(k=4\) 外切的 7.6% 误差意味着:在摩擦极限附近操作时(如冰面),可能超出真实摩擦锥,导致滑倒
2. \(k=8\) 是实践中的甜蜜点:1.1% 误差 vs 0.5ms(足够 2kHz WBC)
3. ProxQP 的 SOC 求解比 Mosek 快——Simple Robotics 团队优化了 proximal point 迭代

52.7.2 计算复杂度分析 ⭐⭐⭐

QP 的约束数与求解时间的关系:

对于 \(n_c\) 个接触点,每个用 \(k\) 条边近似,加上法向非负约束:

• 总不等式约束数:\(n_c \times (k + 1)\)
• 四足(\(n_c = 4\)),\(k = 8\):\(4 \times 9 = 36\) 个不等式
• 人形(\(n_c = 2\),6D wrench),\(k = 8\) + CoP + 扭矩:\(2 \times 16 \approx 32\) 个不等式

QP 求解时间通常与约束数**超线性**增长(对于 active-set 方法约 \(O(m^2)\),对于 interior-point 约 \(O(m^{1.5})\))。

SOC 求解的替代路径——ProxSuite:

ProxSuite 是 Simple Robotics 团队开发的 proximal point 优化库,可以直接处理 SOC 约束:

#include <proxsuite/proxqp/dense/dense.hpp>
using namespace proxsuite::proxqp;

// Setup QP with friction cone constraints
// ProxQP efficiently handles box + equality + inequality constraints
// For direct SOC support, use the conic interface or reformulate

dense::QP<double> qp(n_var, n_eq, n_ineq);
qp.init(H, g, A_eq, b_eq, A_ineq, b_ineq, l_box, u_box);
qp.settings.eps_abs = 1e-6;
qp.settings.max_iter = 100;
qp.solve();

Eigen::VectorXd x_opt = qp.results.x;
// Contact forces extracted from x_opt

⚠️ 编程陷阱:QP warm-start 与接触模式切换

场景: MPC 每步 warm-start 上一步的解。

陷阱: 当接触模式切换(脚从接触变为摆动),active set 完全改变。如果直接 warm-start,OSQP 可能需要更多迭代(旧 active set 完全错误)。

解决: 在模式切换时禁用 warm-start,或用"冷启动 + 更多最大迭代"策略。OCS2 的 SwitchedModelReferenceManager 在模式切换时自动处理这个问题。

练习

练习 52.7.1 (⭐⭐): 对同一个力分配问题,分别用 \(k = 4, 8, 16\) 和精确 SOC 求解。画出四条腿力向量在摩擦锥截面上的位置。哪种情况的力最接近锥边界?

练习 52.7.2 (⭐⭐⭐): 用 ProxQP 实现一个简单的 WBC QP,比较 OSQP (\(k=8\) 线性化) vs ProxQP 的求解时间和解精度。在 100 个随机构型上统计。

---

52.8 软接触 vs 刚体接触——工程选择与仿真实践 ⭐⭐⭐

动机:为什么有两种接触模型?

本节解决的问题:刚体接触(LCP/互补性)假设接触面无变形,接触力瞬间产生。软接触(弹簧-阻尼/体积弹性)假设接触面有弹性变形。两者各有优劣——为什么工业控制器用刚体,而仿真器(MuJoCo)用软接触?

52.8.1 刚体接触模型 ⭐⭐

数学表达:

\[\text{LCP: } \quad w = M^{-1}(J_c^T \lambda + \tau_{\text{ext}}) + \text{bias}, \quad 0 \le w \perp \lambda \ge 0\]

其中 \(w\) 是接触点的加速度(或速度,取决于时间步格式)。

特点: - 接触力是**约束力**,不是显式函数——由互补条件隐式确定 - 数学上精确(真正的不穿透,不滑移) - 但需要求解 LCP/QP,每一步都是一个优化问题

52.8.2 软接触模型 ⭐⭐

弹簧-阻尼(Spring-Damper)模型:

\[\lambda_c^n = \max\!\Big(0,\; k \cdot (-\phi) - d \cdot \dot{\phi}\Big) \cdot \mathbb{1}[\phi \le 0]\]

• \(k\)：接触刚度(N/m)
• \(d\)：接触阻尼(Ns/m)
• \(\phi\)：有符号距离（\(\phi < 0\) 表示穿透，\(-\phi\) 即穿透深度 \(\delta\)）
• 外层 \(\max(0, \cdot)\) 防止阻尼项在闭合阶段（\(\dot{\phi} < 0\)）使法向力变为负值（拉力），保证 \(\lambda_c^n \ge 0\)

Hertz 接触模型(更符合物理):

\[\lambda_c^n = k_H |\phi|^{3/2} - d_H |\phi|^{3/2} \dot{\phi}\]

\(3/2\) 次方来自弹性力学——球体压入平面的接触力与变形量的 \(3/2\) 次方成正比(Hertz 1882)。

Drake Hydroelastic 模型(2019+):

Drake 的 Hydroelastic Contact 更进一步——不是点接触,而是**体积压力场接触**:

1. 每个碰撞体被赋予一个**压力场**(pressure field):刚度从表面到内部线性增加
2. 两个物体的压力场在接触区域**叠加**
3. 接触面积、压力分布、合力/合力矩通过积分计算

\[\boldsymbol{f}_c = \int_{\Omega_{\text{contact}}} p(\boldsymbol{x}) \boldsymbol{n}(\boldsymbol{x}) \, dA\]

优势: 自然处理面接触、滚动摩擦、CoP 偏移。

劣势: 需要 mesh 离散化,计算量大,参数(弹性模量)难校准。

MuJoCo 的软接触:

MuJoCo 使用一种**凸近似**的软接触模型——将接触问题转化为一个优化问题,通过 PGS (Projected Gauss-Seidel) 或 Newton 方法求解。核心参数:

<!-- MuJoCo contact parameters -->
<option>
  <flag contact="enable"/>
</option>
<default>
  <geom condim="3" friction="0.7 0.005 0.0001"
        solimp="0.9 0.95 0.001 0.5 2"
        solref="0.02 1"/>
  <!-- solref: [timeconst, dampratio] controls contact dynamics -->
  <!-- solimp: [dmin, dmax, width, midpoint, power] controls impedance -->
</default>

52.8.3 两种模型的详细对比 ⭐⭐

维度	刚体接触(LCP)	软接触(Spring-Damper/Hydroelastic)
物理精度	精确的不穿透	允许微小穿透
数学光滑性	非光滑(互补约束)	光滑(连续力函数)
计算方法	LCP/QP 求解	ODE 积分
时间步	可以用大步长	需要小步长(刚性 ODE)
参数需求	只需 \(\mu\)	需要 \(k, d\)(难校准)
面接触	需要显式建模	自然支持(Hydroelastic)
控制器适配	控制器也用 LCP 思维	控制器和仿真不匹配
可微性	不可微(需特殊处理)	天然可微(适合 RL)

为什么工业控制器用刚体模型?

1. Sim-to-Real 一致性: 控制器中的约束(摩擦锥 QP)与刚体接触模型匹配
2. 参数简单: 只需 \(\mu\),不需要弹性模量/阻尼比
3. 性能: 不需要小时间步

为什么仿真器(MuJoCo/Isaac)倾向软接触?

1. 数值稳定性: 软接触力是连续函数,ODE 积分更稳定
2. GPU 并行: 软接触不需要求解全局 QP,适合 GPU 大规模并行
3. RL 训练: RL 需要可微的仿真器(用于策略梯度),软接触天然可微

🧠 思维陷阱:仿真器选择对控制算法的影响

陷阱: "仿真器无所谓,只要能仿真就行。"

现实: 不同仿真器的接触模型差异会导致 sim-to-real gap。在 MuJoCo 中训练的 RL 策略,如果迁移到 Drake 刚体仿真中验证,行为可能完全不同。2024 年的研究(Humanoid-Gym)发现 MuJoCo 的 dynamics 比 Isaac Gym 更接近真实环境,因此推荐"Isaac Gym 训练 + MuJoCo Sim2Sim 验证"流程。

练习

练习 52.8.1 (⭐⭐): 在 MuJoCo 中,调节 solref 参数(接触时间常数)从 0.002 到 0.2。观察:穿透深度如何变化?接触力的振荡如何变化?找到"足够硬但不振荡"的甜蜜点。

练习 52.8.2 (⭐⭐⭐): 如果你要研究"足底压力分布对平衡的影响",应该用刚体接触还是 Hydroelastic?用技术论据说明理由(提示:刚体点接触无法表达压力分布)。

---

52.9 接触力学在 RL 仿真中的角色 ⭐⭐⭐

动机:RL 时代为什么仍需理解接触力学?

本节解决的问题:强化学习(RL)通过试错学习控制策略,似乎不需要显式建模接触力。但仿真器中的接触模型决定了 RL 策略的质量和 sim-to-real 迁移性能。

52.9.1 三大仿真器的接触模型对比 ⭐⭐

特性	MuJoCo (3.x)	Drake (Hydroelastic)	Isaac Gym/Sim
接触模型	软接触(凸近似)	Hydroelastic + 刚体	GPU 软接触
求解方法	PGS / Newton	LCP + 压力场积分	并行 PBD/TGS
可微性	MuJoCo MJX (JAX)	部分可微	Warp/Torch
并行度	MJX: TPU ~M FPS	单线程为主	4096 env, ~90k FPS
物理精度	中(可调)	高	低-中
Sim2Real 难度	中	低(高保真)	高(需域随机化)

2024-2025 年的实践趋势:

训练阶段: Isaac Gym (4096 并行环境) --> 策略
验证阶段: MuJoCo (高精度) --> Sim2Sim 验证
部署准备: 域随机化 + 接触参数扫描

为什么不全用 Drake?

Drake 的 Hydroelastic 接触在每步需要 mesh 求交和压力场积分,单环境仿真速度约 100-500 Hz(远低于 RL 需求的 10000+ Hz)。且 GPU 加速有限。

52.9.2 接触参数对 RL 策略的影响 ⭐⭐⭐

域随机化(Domain Randomization)中的接触参数:

# Isaac Gym: contact parameter randomization
randomize_config = {
    "friction_range": [0.3, 1.2],      # Friction coefficient
    "restitution_range": [0.0, 0.3],   # Bounce coefficient  
    "contact_stiffness": [1e3, 1e5],   # Contact spring stiffness
    "contact_damping": [10, 1000],     # Contact damping
}

研究发现(2024): - 摩擦系数随机化范围过大 → 策略过于保守,不利用摩擦 - 接触刚度过低 → 策略学会"踩进"地面(利用穿透获得额外支撑) - 最佳实践:先在真实机器人上测量 \(\mu\) 范围,再在该范围附近随机化

⚠️ 编程陷阱:RL 策略利用仿真器 bug

症状: 策略在仿真中表现完美,但 sim-to-real 完全失败。

常见原因: 策略学会了利用仿真器的非物理特性,如: - 穿透地面获得额外推力(接触刚度太低) - 在空中仍受到摩擦力(接触检测有延迟) - 利用非对称的接触求解器(PGS 的求解顺序影响结果)

验证方法: 在另一个仿真器(如 MuJoCo -> Drake)中验证策略。如果跨仿真器表现差异大,说明策略可能利用了仿真器特异性。

💡 概念误区:RL 不需要理解物理

错误理解: "RL 策略自动学会物理,不需要工程师理解接触力学。"

正确理解: RL 工程师需要理解接触力学来**设计 reward**、选择仿真器参数、诊断 sim-to-real gap。比如:如果 reward 惩罚足底力太大,策略可能学会用"滑步"代替"踏步"——这需要理解摩擦锥才能诊断。

练习

练习 52.9.1 (⭐⭐): 在 MuJoCo 中用同一个策略(如 PPO 训练的四足行走),分别在 \(\mu = 0.3\) 和 \(\mu = 0.8\) 下测试。策略的步态有何变化?这说明策略是否"理解"了摩擦物理?

练习 52.9.2 (⭐⭐⭐): 设计一个实验:比较 Isaac Gym 和 MuJoCo 中训练的同一个策略在真实机器人上的表现。需要控制哪些变量?预期哪个仿真器训练的策略迁移更好?

---

52.10 本章小结

核心知识点总结表

知识点	核心公式/概念	工程应用	难度
Signorini 条件	\(\phi \ge 0, \lambda^n \ge 0, \phi \cdot \lambda^n = 0\)	所有接触约束的基础	⭐⭐
库仑摩擦锥	\(\|\lambda^t\| \le \mu \lambda^n\) (SOC)	WBC/MPC 的核心约束	⭐⭐
摩擦锥线性化	\(k\)-边多面体近似,误差 \(\sim 1-\cos(\pi/k)\)	MIT Cheetah 凸 MPC	⭐⭐
Wrench Cone	6D 力/力矩联合约束	人形机器人面接触	⭐⭐⭐
Fischer-Burmeister	\(\Phi(a,b) = \sqrt{a^2+b^2} - a - b = 0\)	互补约束光滑化	⭐⭐⭐
Contact-Implicit TO	接触时刻/位置作为决策变量	离线步态发现	⭐⭐⭐⭐
接触 Jacobian 对偶	\(\tau_c = J_c^T \lambda_c\)	力从接触空间到关节空间	⭐⭐
漂移项	\(J_c \ddot{q} + \dot{J}_c \dot{q} = 0\)	约束保持(防止足滑)	⭐⭐
CCRBA	O(N) 计算 CMM: \(h_G = A_G \dot{q}\)	Centroidal MPC/WBC	⭐⭐⭐
角动量守恒	飞行相位 \(\dot{k}_G = 0\)	跳跃/翻滚规划	⭐⭐
猫翻身悖论	零角动量下通过惯量不对称旋转	理解全身动力学 vs Centroidal	⭐⭐⭐
软 vs 刚体接触	弹簧阻尼 vs LCP	仿真器选择	⭐⭐

三大路线对比总结

维度	预定义模式	软化互补	Contact-Implicit
实时性	ms 级	秒级	分钟级
适应性	固定步态	需初始猜测	自动发现
工业成熟度	OCS2/legged_control	研究代码	前沿研究
数学难度	低	中	高
代表	MIT Cheetah, ANYmal	Ipopt + NLP	Posa 2014, ci-SCvx

---

累积项目:四足站立控制器——接触力约束模块

项目目标

在前面章节的"四足站立控制器"基础上,添加**接触力约束模块**,使得 QP 求解器产生物理上合法的接触力。

模块接口

/**
 * @brief Contact force constraint module for quadruped standing controller
 * 
 * This module adds friction cone and unilateral contact constraints
 * to the WBC QP formulation from Ch50.
 */
class ContactForceConstraintModule {
public:
  struct Config {
    double mu = 0.6;                // Friction coefficient
    int k = 8;                      // Polygon edges for linearization
    bool use_inner = false;         // Inner approximation (conservative)
    double f_z_min = 10.0;          // Minimum normal force (N)
    double f_z_max = 500.0;         // Maximum normal force (N)
  };

  /**
   * @brief Add friction cone constraints for active contacts
   * @param contact_flags Which feet are in contact (4-element bool vector)
   * @param A_ineq Reference to inequality constraint matrix
   * @param b_ineq Reference to inequality bound vector
   * @param offset Starting row in constraint matrices
   * @return Number of constraints added
   */
  int addFrictionConeConstraints(
      const std::array<bool, 4>& contact_flags,
      Eigen::MatrixXd& A_ineq,
      Eigen::VectorXd& b_ineq,
      int offset);

  /**
   * @brief Zero force constraints for swing legs
   * @return Number of equality constraints added
   */
  int addZeroForceConstraints(
      const std::array<bool, 4>& contact_flags,
      Eigen::MatrixXd& A_eq,
      Eigen::VectorXd& b_eq,
      int offset);

private:
  Config config_;
  Eigen::MatrixXd cone_A_;  // Precomputed cone constraint matrix
};

实现要点

int ContactForceConstraintModule::addFrictionConeConstraints(
    const std::array<bool, 4>& contact_flags,
    Eigen::MatrixXd& A_ineq,
    Eigen::VectorXd& b_ineq,
    int offset) {

  int n_added = 0;

  for (int leg = 0; leg < 4; ++leg) {
    if (!contact_flags[leg]) continue;  // Skip swing legs

    // Decision variable indices for this leg's contact force
    int col_start = leg * 3;  // lx, ly, lz for this leg

    // Add k friction cone edges
    for (int i = 0; i < config_.k; ++i) {
      double theta = 2.0 * M_PI * i / config_.k;
      double mu_eff = config_.use_inner ? 
          config_.mu * std::cos(M_PI / config_.k) : config_.mu;

      int row = offset + n_added;
      A_ineq(row, col_start)     = std::cos(theta);   // lx
      A_ineq(row, col_start + 1) = std::sin(theta);   // ly
      A_ineq(row, col_start + 2) = -mu_eff;           // -mu*lz
      b_ineq(row) = 0.0;  // cos*lx + sin*ly - mu*lz <= 0
      n_added++;
    }

    // Add normal force bounds: f_z_min <= lz <= f_z_max
    // lz >= f_z_min  -->  -lz <= -f_z_min
    A_ineq(offset + n_added, col_start + 2) = -1.0;
    b_ineq(offset + n_added) = -config_.f_z_min;
    n_added++;

    // lz <= f_z_max
    A_ineq(offset + n_added, col_start + 2) = 1.0;
    b_ineq(offset + n_added) = config_.f_z_max;
    n_added++;
  }

  return n_added;
}

验证方法

// Test: Standing with symmetric weight distribution
// Expected: each leg bears m*g/4 vertically
// Friction force components should be ~0

void testSymmetricStanding() {
  double m = 12.0;  // Go2 mass (kg)
  double g = 9.81;

  // Solve QP with friction cone constraints
  auto result = solveStandingQP(
      /*contact_flags=*/{true, true, true, true},
      /*mu=*/0.6, /*k=*/8);

  for (int leg = 0; leg < 4; ++leg) {
    double fz = result.forces[leg].z();
    double ft = result.forces[leg].head<2>().norm();

    // Normal force approx mg/4
    EXPECT_NEAR(fz, m * g / 4.0, 1.0);
    // Tangential force approx 0 (symmetric standing)
    EXPECT_NEAR(ft, 0.0, 0.1);
    // Friction cone satisfied
    EXPECT_LE(ft, 0.6 * fz + 1e-6);
  }
}

---

项目精读指南

项目	文件路径	精读重点	难度
OCS2	ocs2_legged_robot/src/constraint/FrictionConeConstraint.cpp	摩擦锥约束:SOC 形式,augmented Lagrangian 处理	⭐⭐
OCS2	ocs2_centroidal_model/src/CentroidalModelInfo.cpp	Centroidal 模型信息提取	⭐⭐
Pinocchio	include/pinocchio/algorithm/centroidal.hpp	CCRBA 实现	⭐⭐⭐
Pinocchio	include/pinocchio/algorithm/frames.hpp	接触 Jacobian API	⭐⭐
Crocoddyl	include/crocoddyl/multibody/actions/contact-fwddyn.hxx	KKT 系统求解接触动力学	⭐⭐⭐
Crocoddyl	include/crocoddyl/multibody/friction-cone.hpp	摩擦锥类实现	⭐⭐
TSID	include/tsid/contacts/contact-6d.hpp	6D wrench 接触(人形)	⭐⭐⭐
TSID	include/tsid/contacts/contact-point.hpp	3D 点接触(足式)	⭐⭐
Drake	multibody/plant/contact_model.h	Hydroelastic 接触模型	⭐⭐⭐⭐

---

延伸阅读

必读经典 ⭐⭐

1. Di Carlo J., Wensing P. M., Katz B., Bledt G., Kim S. (2018) "Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control" — IROS. 凸 MPC 的奠基论文,摩擦锥线性化的工程典范。

2. Orin D. E., Goswami A., Lee S.-H. (2013) "Centroidal dynamics of a humanoid robot" — Autonomous Robots. CMM 与 CCRBA 算法的完整数学推导。

3. Stewart D., Trinkle J. (1996) "An implicit time-stepping scheme for rigid body dynamics with inelastic collisions and Coulomb friction" — Int. J. Numer. Meth. Eng. LCP 接触时间积分的经典论文。

Contact-Implicit TO ⭐⭐⭐

1. Posa M., Cantu C., Tedrake R. (2014) "A direct method for trajectory optimization of rigid bodies through contact" — IJRR. CITO 奠基论文。

2. Manchester Z., Kuindersma S. (2020) "Variational contact-implicit trajectory optimization" — ISRR. CITO 的变分形式。

3. Pang T. et al. (2023) "Global Planning for Contact-Rich Manipulation via Local Smoothing of Quasi-Dynamic Contact Models" — T-RO. 最新 CITO for manipulation。

4. Kurtz V., Castro A. M., Onol A. O., Lin H. (2024) "Inverse Dynamics Trajectory Optimization for Contact-Implicit MPC" — IJRR. 首次实现 100Hz CITO-MPC。

5. ci-SCvx (2026) "GPU-Accelerated Continuous-Time Successive Convexification for Contact-Implicit Legged Locomotion" — arXiv. GPU 加速 CITO,比 SOTA 快 10x。

软接触与仿真 ⭐⭐⭐

1. Masterjohn J. et al. (2022) "Velocity Level Approximation of Pressure Field Contact Patches" — RA-L. Drake Hydroelastic 的理论基础。

2. Wensing P. M., Orin D. E. (2016) "Improved Computation of the Humanoid Centroidal Dynamics and Application for Whole-Body Control" — 改进的 CCRBA 在 WBC 中的应用。

前沿方向 ⭐⭐⭐⭐

1. ContactBench (Simple Robotics, 2024) — 接触模型与求解器的系统性 benchmark,比较了 PGS/Lemke/proximal 等方法。

2. Proximal methods for contact (RSS 2024) — 将 proximal point 方法应用于接触逆动力学,扩展 MuJoCo 的方法到刚性和柔性接触。

开放研究问题

• 在线摩擦估计: 真实 \(\mu\) 随地面类型、湿度、磨损变化。能否用触觉传感器 + 在线估计实时更新 \(\mu\)?
• 滑移显式建模: 当前 WBC 假设不滑。在光滑地面上如何把滑移写入控制器?
• 接触丰富操作 + 运动联合: 双臂操作(多接触)+ 双足行走(切换接触)的统一数学框架?
• 可微接触仿真: 如何让 LCP/互补约束变得可微,以便端到端优化?Drake 的 time-stepping 方案和 MuJoCo MJX 是两个方向。

---

与其他章节的衔接

向前承接: - Ch49 空间向量代数 → 52.4 接触 Jacobian 的空间力变换 - Ch50 QP/NLP 建模 → 52.2 摩擦锥变成 QP 线性不等式 - Ch51 LIPM/Centroidal → 52.5 CMM 是 Centroidal 动力学的计算基础

向后指向: - 52.2 摩擦锥约束 → Ch53 WBC 的关键不等式约束 - 52.3 互补约束 → Ch54 DDP 接触模型(Crocoddyl contact-fwddyn) - 52.4 接触 Jacobian → Ch55 OCS2 FrictionConeConstraint 的实现基础 - 52.5 CCRBA → Ch55 OCS2 Centroidal MPC 的在线计算 - 52.6 角动量 → Ch53 WBC 角动量跟踪任务 - 52.9 RL 仿真 → Ch63 腿足 RL 训练栈

---

预计学习时间

1 周(20-25 小时):

模块	时间	重点
52.1-52.2 接触约束 + 摩擦锥	5-6 h	推导 + 代码实现
52.3 互补约束三路线	4-5 h	Fischer-Burmeister + CITO 概念
52.4-52.5 Jacobian + CCRBA	4-5 h	Pinocchio API + 数值验证
52.6 角动量	2-3 h	物理直觉 + 猫翻身
52.7-52.9 工程对比	3-4 h	仿真器选择 + RL 接触
练习 + 源码阅读	4-5 h	OCS2 + Crocoddyl 精读

---

学完后的能力检验清单

• 能写出 Signorini 条件的三个联立关系,并解释每一条的物理含义
• 能推导摩擦锥线性化的内切/外切误差公式
• 能实现 \(k=4,8\) 的摩擦锥约束矩阵并集成到 QP 中
• 能识别三种互补约束处理路线的适用场景
• 能解释 Fischer-Burmeister 函数为什么能替代互补约束
• 能用 Pinocchio 计算接触 Jacobian 并解释三种参考系的区别
• 能调用 pinocchio::ccrba() 并验证 CMM 的分块性质
• 能解释飞行相位角动量守恒与猫翻身的表面矛盾
• 能在 OCS2 源码中找到 FrictionConeConstraint 的实现细节
• 能对比 MuJoCo/Drake/Isaac 的接触模型差异及各自适用场景

---