CRBA 及稀疏分解路线¶

所属：刚体动力学专题 4-3 | 前置：10_空间向量代数（6D motion/force 向量、Plücker 变换 \({}^iX_j\)、motion/force 叉积 \(v\times\)/\(v\times^*\)、spatial inertia \(I\)）、20_Lagrange与Hamilton力学（\(M(q)\ddot{q}+C(q,\dot{q})\dot{q}+g(q)=\tau\) 的定义与 Christoffel 构造） 交叉引用：→ 50_动力学解析微分（dRNEA/dq 依赖本章 RNEA 结构）、→ 60_约束动力学（闭链扩展）、→ 05_运动控制/90_DDP

Plucker 变换记号约定：本章统一使用 \({}^iX_j\) 表示从坐标系 \(j\) 到坐标系 \(i\) 的 Plucker 变换。10_空间向量代数中出现的 \(X_{i,\lambda(i)}\) 与本章的 \({}^iX_{\lambda(i)}\) 含义相同。

前置自测¶

📋 前置自测（答不出 ≥ 2 题 → 先回 10_空间向量代数 / 20_Lagrange与Hamilton力学复习）

什么是 spatial velocity（6D motion vector）？它的前 3 维和后 3 维分别代表什么物理量？
Plücker 变换 \({}^iX_j\) 作用于 motion vector 和 force vector 时，变换矩阵的关系是什么？
写出 spatial Newton-Euler 方程 \(f = I \cdot a + v \times^* (I \cdot v)\) 中各项的物理含义。
Lagrangian 形式的运动方程 \(M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q) = \tau\) 中，M 的物理含义是什么？它有哪些数学性质（对称性、正定性）？
什么是"关节树的拓扑序编号"？为什么要保证 \(\lambda(i) < i\)？

本章目标¶

学完本章后，你能够：

从零手推 7-DoF 机械臂或浮基人形的 RNEA（逆动力学）全过程，理解每一步的物理含义
独立实现 RNEA/CRBA/ABA 的核心循环，并与 Pinocchio 的源码逐行对应
证明三大算法的复杂度（O(n)/O(n²)/O(n)）和正确性定理
选型决策：面对具体工程场景（MPC/WBC/仿真/状态估计），知道该用哪个算法、为什么
读懂 Pinocchio 的 rnea.hxx、crba.hxx、aba.hxx 源码结构和性能优化技巧

知识树总览¶

O(N) 动力学递推算法
├── 1. 历史与动机：从 O(n⁴) 到 O(n) 的四十年
├── 2. 关节树拓扑与编号约定
├── 3. RNEA：逆动力学 O(n) ⭐⭐
│   ├── 3.1 前向 pass：速度/加速度递推
│   ├── 3.2 反向 pass：力递推
│   ├── 3.3 完整伪代码与逐步推导
│   ├── 3.4 O(n) 复杂度严格证明
│   ├── 3.5 正确性定理证明
│   └── 3.6 特殊调用与工程用法
├── 4. CRBA：质量矩阵 M(q) 的 O(n²) 算法 ⭐⭐
│   ├── 4.1 复合刚体惯性的几何含义
│   ├── 4.2 完整伪代码与逐步推导
│   └── 4.3 复杂度分析与工程对比
├── 5. ABA：正动力学 O(n) ⭐⭐⭐
│   ├── 5.1 关节体惯性概念
│   ├── 5.2 三次遍历的完整推导
│   ├── 5.3 与 LDLᵀ 分解的等价性
│   └── 5.4 正确性定理与 Schur complement
├── 6. Minv 算法：直接计算 M⁻¹τ ⭐⭐⭐
├── 7. 三大算法统一视角 ⭐⭐
├── 8. 稀疏分解路线 ⭐⭐⭐
├── 9. 并行化：D&C ABA 与 GPU 批量 RNEA ⭐⭐⭐⭐
├── 10. Pinocchio 源码精读
└── 11. 典型例题：7-DOF 机械臂与浮基人形

1. 历史与动机：从 O(n⁴) 到 O(n) 的四十年 ⭐¶

1.1 为什么递推算法是动力学计算的核心¶

一个事实定义了这个专题的重要性：2008 年至今，几乎所有主流机器人软件栈的动力学内核都是 Featherstone 三大算法 RNEA/CRBA/ABA 的某种实现。 Pinocchio（INRIA）、RBDL（Felis）、Drake（TRI/MIT）、MuJoCo（DeepMind）、iDynTree（IIT）、RigidBodyDynamics.jl（JuliaRobotics）——所有这些库的内部循环结构，逐行都能映射到 Featherstone《Rigid Body Dynamics Algorithms》(Springer 2008) 第 5、6、7 章的三张伪代码表。掌握这三个算法，本质上就掌握了现代机器人动力学计算的全部核心。

1.2 历史进程：三个数量级的加速¶

早期的 Uicker-Kahn 方法（1965）天真地按定义写出 M(q) 再逐元素计算 Christoffel 符号，复杂度是 O(n⁴)，一个 30-DoF 的机器人需要约 81 万次浮点运算。这个方法的问题在于它把每个矩阵元素当作独立的计算任务，完全没有利用关节树的递推结构。

年代	方法	复杂度	作者	核心思想
1965	Uicker-Kahn	O(n⁴)	Uicker	逐元素计算 M 和 Christoffel 符号
1978	Hollerbach	O(n³)	Hollerbach	Lagrangian 递推公式
1980	RNEA	O(n)	Luh, Walker, Paul	Newton-Euler 前向+后向递推
1982	CRBA	O(n²)	Walker, Orin	复合刚体惯性后向累加
1983	ABA	O(n)	Featherstone	关节体惯性 + Schur 消元

从 O(n⁴) 到 O(n)，这是"三个数量级的加速"（Featherstone & Orin, ICRA 2000 综述）。 对 30-DoF 的人形机器人：O(n⁴) 需要 ≈ 810,000 FLOPs，O(n) 只需要 ≈ 6,600 FLOPs——差了 123 倍。

本质洞察：递推算法之所以能达到 O(n)，核心原因是**树状拓扑的局部性**——每个关节只需要和它的父/子关节通信。这和图像处理中卷积是局部操作、因此可以并行类似：树结构让全局问题分解为 n 个局部子问题的串联。如果机器人有闭链（如平行四边形机构），局部性被打破，算法就必须引入约束力 λ，回到 O(n) + O(m³) 的形式（m 是约束数）。

1.3 实时 MPC 对微秒级动力学的刚性需求¶

Crocoddyl 等现代优化控制框架每次 FDDP 迭代大约调用 N×ABA + N×RNEA + N×解析导数，其中 N 是 horizon 节点数（典型 60–115）。在 Atlas 级机器人（36-DoF）上，如果动力学每次调用 > 10 μs，整个 MPC 单次迭代就会超过 10 ms，完全无法满足 100 Hz 以上的实时闭环需求。Pinocchio 之所以能在 Atlas 上跑 ≈ 3 μs/ABA，正是因为它把 Featherstone 1983 年的 O(n) 递推结构打磨到了 Eigen+模板元编程的极致。

如果不这样做会怎样？ 假设我们用 O(n³) 的 Lagrangian 方法代替 ABA 做正动力学：36-DoF 需要 ≈ 46,656 FLOPs（n³/6 for Cholesky），相比 ABA 的 ≈ 7,920 FLOPs（220×36），慢了约 6 倍。在 MPC 的 100 个 horizon 节点上累积，就是 600 μs vs 100 μs 的差距——足以决定一个 100Hz MPC 能否实时。

1.4 本章的独特定位¶

本章是"最接近代码实现"的数学章节。 前面的空间向量代数教你几何工具，Lagrange-Hamilton 力学教你 M/C/g 的分析力学由来，而到了递推算法这一章，我们必须写出 for 循环、必须能和 Pinocchio 的 rnea.hxx 逐行对应、必须能证明某个 6×6 运算的次数。数学的优雅性让位于工程的精准性——但正是这种精准性，让你的代码能在 Atlas 上以 3 μs 完成 36 个关节的整套动力学。

跨领域类比：如果把 Lagrangian 动力学比作"用线性代数求解方程组 Ax=b"，那么 RNEA/CRBA/ABA 就是"利用 A 的稀疏结构做 LU/Cholesky 分解"——数学本质相同，但工程性能天壤之别。就像 MATLAB 里 A\b 对密集矩阵和稀疏矩阵的速度差异一样，递推算法利用了关节树诱导的稀疏性。

2. 关节树拓扑与编号约定 ⭐¶

2.1 核心概念¶

机器人模型在数学上是一棵**有向连通树**（Featherstone Ch.4）。每个节点代表一个刚体（link），每条边代表一个关节（joint）。关键定义：

Parent array \(\lambda(i)\)：关节 i 的父关节编号。基座（base body）取 \(\lambda = 0\)
拓扑序编号：关节从 1 到 n 编号，保证 \(\lambda(i) < i\)——即父节点编号一定小于子节点。这个约定使得"从根到叶"的遍历可以用 for i=1..n，"从叶到根"的遍历可以用 for i=n..1
运动子空间 \(S_i \in \mathbb{R}^{6 \times n_i}\)：描述关节 i 允许的自由运动方向
Revolute 关节：\(n_i = 1\)，\(S = [0\; 0\; 1\; 0\; 0\; 0]^T\)（绕 z 轴旋转）
Prismatic 关节：\(n_i = 1\)，\(S = [0\; 0\; 0\; 0\; 0\; 1]^T\)（沿 z 轴平移）
Spherical 关节：\(n_i = 3\)
Free joint（浮基）：\(n_i = 6\)

2.2 为什么拓扑序如此重要¶

如果不保证 \(\lambda(i) < i\) 会怎样？ 前向递推 \(v_i = {}^{i}X_{\lambda(i)}\, v_{\lambda(i)} + S_i \dot{q}_i\) 需要用到父节点的速度 \(v_{\lambda(i)}\)。如果父节点编号大于 \(i\)，那么在计算 \(v_i\) 时父节点的速度还没算出来——数据依赖被打破，算法不正确。拓扑序保证了**因果关系**：先算父节点，再算子节点。

这和计算图的拓扑排序完全一致——深度学习框架中的前向传播也必须按拓扑序执行 DAG 中的节点。

2.3 Pinocchio 的编号约定¶

Pinocchio 中，model.njoints = n+1，其中 joint 0 是 universe/world（虚拟世界节点），真正的第一个关节从 idx 1 开始。data.v[0] 永远是零向量（世界的速度为零）。

# Pinocchio 关键数据结构
model.parents      # parent array: parents[i] = λ(i)
model.nv           # 总自由度数 = Σ nᵢ
model.idx_v        # 关节 i 在 q̇ 向量中的起始索引
model.nvSubtree    # 以 i 为根的子树的总 DoF 数

3. RNEA：递推 Newton-Euler 逆动力学 O(n) ⭐⭐¶

3.1 动机与问题定义¶

RNEA 解决的问题：已知关节位置 \(q\)、速度 \(\dot{q}\)、加速度 \(\ddot{q}\)，求使机器人产生这些运动所需的关节力矩 \(\tau\)。

数学表达：

\[\tau = \text{RNEA}(q, \dot{q}, \ddot{q}) = M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q)\]

为什么需要 RNEA？ 在 MPC 和轨迹优化中，每个时间步都需要计算"给定轨迹所需的关节力矩"——这就是逆动力学。一个 FDDP 迭代中，N 个 horizon 节点每个都要调用一次 RNEA，N 通常是 60-115。如果 RNEA 不是 O(n)，整个 MPC 就无法实时。

如果直接用 Lagrangian 公式 \(\tau = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q)\) 呢？ 计算 M(q) 本身就是 O(n²)（CRBA），再做矩阵-向量乘 \(M \ddot{q}\) 又是 O(n²)——总共 O(n²)，比 RNEA 的 O(n) 慢。更关键的是，RNEA 根本不需要显式构造 M、C、g，它直接从物理定律（Newton-Euler 方程）出发，通过树上的递推一次性得到 \(\tau\)。

3.2 前向 pass：速度与加速度递推¶

核心思想：从基座（根节点）出发，沿着树向叶子传播，依次计算每个 link 的空间速度和空间加速度。

空间速度递推：

\[\boxed{v_i = {}^iX_{\lambda(i)} \, v_{\lambda(i)} + S_i \dot{q}_i}\]

物理含义：link i 的速度 = 父 link 的速度（变换到 link i 的坐标系）+ 关节 i 自身的运动贡献。

空间加速度递推：

\[\boxed{a_i = {}^iX_{\lambda(i)} \, a_{\lambda(i)} + S_i \ddot{q}_i + \underbrace{v_i \times (S_i \dot{q}_i)}_{\text{bias acceleration } c_i}}\]

物理含义：link i 的加速度 = 父 link 加速度的传递 + 关节加速度贡献 + Coriolis/离心加速度项 \(c_i\)。

本质洞察：bias acceleration \(c_i = v_i \times (S_i \dot{q}_i)\) 的本质是**旋转坐标系中的科氏力效应**。即使关节加速度 \(\ddot{q}_i = 0\)，只要 link 在旋转（\(v_i \neq 0\)）且关节在运动（\(\dot{q}_i \neq 0\)），link 上的点也会有加速度——这和地球上的科氏力让风偏转是同一个物理机制。

重力注入技巧（Featherstone Ch.5 §5.3）：

\[a_0 = -g_0\]

其中 \(g_0 = [0,0,0,0,0,9.81]^T\)（世界坐标系中的重力加速度，spatial 表示）。这个技巧的含义是：给基座一个"虚拟的向上加速度"，等效于让所有 link 感受到向下的重力。这样就不需要在后向 pass 中单独处理重力——它自然嵌入了每个 link 的加速度中。

如果不用这个技巧会怎样？ 你需要在后向 pass 的力递推中，对每个 link 额外加一项 \(m_i g\)（重力）。这增加了代码复杂度，而且在不同坐标系间变换重力方向容易出错。Featherstone 的 gravity-as-base-acceleration trick 把"分布在 n 个 link 上的重力"压缩成"基座初始化时的一次赋值"，极其优雅。

3.3 反向 pass：力递推¶

核心思想：从叶子节点出发，沿着树向根节点传播，依次计算每个 link 的净 spatial force，最终投影到关节运动方向得到力矩。

单个 link 的 Newton-Euler 方程：

\[f_i = I_i \, a_i + v_i \times^* (I_i \, v_i) - f_i^{ext}\]

其中： - \(I_i\) 是 link i 的 spatial inertia（6×6 对称正定矩阵） - \(I_i a_i\) 是"使 link i 以 \(a_i\) 加速所需的力" - \(v_i \times^* (I_i v_i)\) 是 velocity product force——包含了科氏力和离心力的贡献 - \(f_i^{ext}\) 是作用在 link i 上的外力

力的后向传播：

\[f_{\lambda(i)} \leftarrow f_{\lambda(i)} + {}^{\lambda(i)}X_i^* \, f_i\]

物理含义：link i 需要的力必须由父关节承担（达朗贝尔原理），所以向父节点回传。

关节力矩提取：

\[\tau_i = S_i^T \, f_i\]

物理含义：净 spatial force 在关节运动方向上的投影就是关节需要的力矩。

3.4 RNEA 完整伪代码与逐步推导¶

算法 RNEA(model, q, q̇, q̈) → τ      // 逆动力学：已知运动求力矩
输入:  q ∈ ℝⁿ (关节位置), q̇ ∈ ℝⁿ (关节速度), q̈ ∈ ℝⁿ (关节加速度)
输出:  τ ∈ ℝⁿ (关节力矩)
初始化: v₀ ← 0,  a₀ ← −g₀   // 重力作为基座的虚拟加速度

# ===== Pass 1: Forward（从根到叶，运动学传播）=====
for i = 1 .. n:
    # Step 1: 计算关节变换和运动子空间
    [ⁱXλ(i), Sᵢ] ← jcalc(jointType(i), qᵢ, q̇ᵢ)

    # Step 2: 关节速度贡献
    v_J ← Sᵢ · q̇ᵢ                        // 关节 i 产生的局部速度

    # Step 3: link i 的空间速度
    vᵢ ← ⁱXλ(i) · v_{λ(i)} + v_J         // 父 link 速度变换 + 关节贡献

    # Step 4: link i 的空间加速度（含 bias）
    cᵢ ← vᵢ × v_J                         // bias acceleration（科氏项）
    aᵢ ← ⁱXλ(i) · a_{λ(i)} + Sᵢ · q̈ᵢ + cᵢ

    # Step 5: link i 的 Newton-Euler 力（含 velocity product）
    fᵢ ← Iᵢ · aᵢ + vᵢ ×* (Iᵢ · vᵢ) − fᵢᵉˣᵗ

# ===== Pass 2: Backward（从叶到根，力递推）=====
for i = n .. 1:
    # Step 6: 关节力矩 = 净力在运动方向的投影
    τᵢ ← Sᵢᵀ · fᵢ

    # Step 7: 向父节点传递力（达朗贝尔原理）
    if λ(i) ≠ 0:
        f_{λ(i)} ← f_{λ(i)} + λ(i)Xᵢᵀ · fᵢ    // force 变换 = motion 变换的转置

return τ

逐步推导说明：

为什么 Step 5 中有 \(v_i \times^* (I_i v_i)\) 这一项？回顾 spatial Newton-Euler 方程的推导（专题 4-1）：在旋转参考系中，Newton 第二定律变为

\[f = I \dot{v} = I(a + v \times v_{\text{relative}})\]

展开后得到 \(f = Ia + v \times^* (Iv)\)——其中 \(v \times^*\) 是 force 叉积算子（spatial force cross product），它把"旋转参考系中的惯性力"编码进来。对地面观察者来说，这些就是科氏力和离心力。

3.5 O(n) 复杂度严格证明¶

定理：RNEA 的计算复杂度是严格 O(n)，其中 n 是关节数量。

证明：

对每个关节 i，Pass 1 中的运算量： - jcalc：计算 6×6 Plücker 变换和运动子空间，固定开销（对 revolute 关节约 40 FLOPs） - ⁱXλ(i) · v_{λ(i)}：一次 6×6 矩阵-向量乘 = 36 FLOPs（但利用 Plücker 变换的稀疏结构可降到 ≈ 24 FLOPs） - vᵢ × v_J：一次 6D 叉积 = 12 FLOPs - ⁱXλ(i) · a_{λ(i)} + Sᵢ · q̈ᵢ + cᵢ：类似，≈ 30 FLOPs - Iᵢ · aᵢ：6×6 对称矩阵-向量乘 = 21 FLOPs（利用对称性） - vᵢ ×* (Iᵢ · vᵢ)：一次 force 叉积 = 12 FLOPs

Pass 1 每关节总计：\(k_1 \approx 100\) FLOPs

对每个关节 i，Pass 2 中的运算量： - Sᵢᵀ · fᵢ：对 revolute 关节，S 只有一个非零元素，= 1 FLOP - λ(i)Xᵢᵀ · fᵢ：一次 6×6 矩阵-向量乘 ≈ 24 FLOPs - 向量加法：6 FLOPs

Pass 2 每关节总计：\(k_2 \approx 30\) FLOPs

总运算数：\(\text{Total FLOPs} = n \cdot (k_1 + k_2) \approx 130n\)

因此 RNEA 是严格 O(n)，常数 ≈ 120-150 FLOPs/关节（Featherstone Table 5.2）。

与 O(n²) 方法的定量对比：

机器人	n	RNEA O(n) FLOPs	Lagrangian O(n²) FLOPs	加速比
Franka Panda	7	≈ 910	≈ 4,900	5.4×
ANYmal quadruped	18	≈ 2,340	≈ 32,400	13.8×
TALOS humanoid	38	≈ 4,940	≈ 144,400	29.2×

n 越大，O(n) 的优势越明显。

3.6 正确性定理证明 ⭐⭐⭐¶

定理（RNEA 正确性）：\(\text{RNEA}(q, \dot{q}, \ddot{q}) = M(q)\ddot{q} + C(q,\dot{q})\dot{q} + g(q)\)

证明思路（Featherstone Ch.5 §5.4）：

Step 1：加速度分离

观察 RNEA 输出对 \(\ddot{q}\) 的依赖关系。在 Pass 1 中：

\[a_i = {}^iX_{\lambda(i)} a_{\lambda(i)} + S_i \ddot{q}_i + c_i\]

由于 \(c_i = v_i \times (S_i \dot{q}_i)\) 不依赖于 \(\ddot{q}\)，我们可以把 \(a_i\) 写成关于 \(\ddot{q}\) 的仿射函数：

\[a_i = \underbrace{a_i^{(\ddot{q})}}_{\text{正比于 } \ddot{q}} + \underbrace{a_i^{(0)}}_{\text{不含 } \ddot{q}}\]

其中 \(a_i^{(\ddot{q})}\) 是"假设 \(c_i=0\), \(a_0=0\) 时，纯粹由 \(\ddot{q}\) 引起的加速度"。

Step 2：力的线性分解

由于 \(f_i = I_i a_i + v_i \times^* (I_i v_i)\)，力也可以分解为：

\[f_i = I_i a_i^{(\ddot{q})} + \underbrace{I_i a_i^{(0)} + v_i \times^* (I_i v_i)}_{\text{不含 } \ddot{q}}\]

Step 3：\(\ddot{q}\) 系数就是 M(q)

把 \(\tau_i = S_i^T f_i\) 中正比于 \(\ddot{q}\) 的部分提取出来。由加速度递推的线性性可知：

\[a_i^{(\ddot{q})} = \sum_{j=1}^{n} \Phi_{ij} S_j \ddot{q}_j\]

其中 \(\Phi_{ij}\) 是从 link j 到 link i 的 Plücker 变换链（仅当 j 是 i 的祖先时非零）。代入力的表达式并投影到 \(S_i^T\)，得到的 \(\ddot{q}\) 系数矩阵恰好就是 M(q) 的定义（Walker-Orin 1982 的 CRBA 公式）。

Step 4：零阶项分离 g(q)

令 \(\dot{q} = 0\), \(\ddot{q} = 0\)：此时 \(c_i = 0\), \(v_i = 0\), \(v_i \times^* (I_i v_i) = 0\)，仅剩 \(a_0 = -g_0\) 向叶子传播产生的力。RNEA 输出就是 \(g(q)\)。

Step 5：二次项就是 C(q,q̇)q̇

RNEA 输出中不含 \(\ddot{q}\) 且含 \(\dot{q}\) 的项 = Total - M(q)\(\ddot{q}\) - g(q) = \(C(q,\dot{q})\dot{q}\)。可以验证这些项对 \(\dot{q}\) 是二次的（\(c_i\) 对 \(\dot{q}\) 是一次，\(v_i \times^* I_i v_i\) 对 \(\dot{q}\) 是二次），与 Christoffel 符号给出的 C 矩阵一致。 \(\blacksquare\)

3.7 RNEA 的特殊调用（极其实用）⭐⭐¶

RNEA 的通用性在于：通过设置不同的输入，可以提取 Lagrangian 方程的各个部分：

调用方式	输出	Pinocchio API	用途
`RNEA(q, q̇, q̈)`	\(M\ddot{q} + C\dot{q} + g\)	`rnea(model, data, q, v, a)`	完整逆动力学
`RNEA(q, q̇, 0)`	\(C(q,\dot{q})\dot{q} + g(q)\)	`nonLinearEffects(model, data, q, v)`	非线性效应（重力补偿+科氏力补偿）
`RNEA(q, 0, 0)`	\(g(q)\)	`computeGeneralizedGravity(model, data, q)`	纯重力项
`RNEA(q, 0, eⱼ)`	M 的第 j 列	—	构造质量矩阵（n 次调用得 M）

第四种用法特别有启发性：跑 n 次 RNEA（每次 \(\ddot{q} = e_j\)，其中 \(e_j\) 是第 j 个单位向量），就能构出完整的 M(q)。这正是 Drake 的 CalcMassMatrixViaInverseDynamics 的实现思路。复杂度 O(n²)——和 CRBA 相同，但常数约大 2-3 倍。

3.8 常见陷阱¶

⚠️ 陷阱 1：bias acceleration 漏写

错误做法：aᵢ = ⁱXλ(i)a_{λ(i)} + Sᵢq̈ᵢ   // 漏掉了 cᵢ = vᵢ × (Sᵢq̇ᵢ)
现象：RNEA 对 q̇ = 0 时结果正确，但 q̇ ≠ 0 时结果错误
根本原因：cᵢ 是旋转参考系中的科氏加速度，即使关节加速度为零也存在
正确做法：aᵢ = ⁱXλ(i)a_{λ(i)} + Sᵢq̈ᵢ + vᵢ × (Sᵢq̇ᵢ)
自检方法：对比 RNEA(q,v,a) 与 crba(q)@a + nle(q,v)，差异应 < 1e-10

💡 概念误区：空间加速度 vs 经典加速度

新手想法："spatial acceleration 就是 link 质心的线加速度加角加速度"
实际上：spatial acceleration = d(spatial velocity)/dt，在 body-fixed 坐标系中
       具体为 a_spatial = [ω̇; v̇ − ω × v]，注意后三维不是质心加速度 r̈
       经典加速度 = [ω̇; r̈] = a_spatial + [0; ω × v]
为什么重要：基座初始化 a₀ = −g₀ 用的是 spatial acceleration 约定
            如果误用经典加速度约定，g(q) 的注入会多一个交叉项
参考：Featherstone Ch.2 §2.11，Stéphane Caron 的博文

🧠 思维陷阱：认为 RNEA 只能做逆动力学

新手想法："RNEA 是逆动力学算法，正动力学只能用 ABA"
实际上：RNEA 的输出包含了 M·q̈ + C·q̇ + g 整体信息
        通过特殊调用（§3.7），可以提取 g、C·q̇+g、M 的列
        甚至可以用 RNEA + CRBA + solve 做正动力学（Lagrangian 路线）
正确思维：RNEA 是一个"通用动力学计算引擎"，逆动力学只是它最直接的用法

3.9 练习题¶

（手推） 考虑一个 2-DOF 平面机械臂（两个 revolute 关节绕 z 轴），link 长度 \(L_1 = L_2 = 1\) m，质量 \(m_1 = m_2 = 1\) kg，质心在 link 中点。设 \(q = [\pi/4, \pi/3]^T\), \(\dot{q} = [1, 2]^T\), \(\ddot{q} = [0.5, -1]^T\)。手写 RNEA 的前向 pass 和后向 pass，列出每步的 \(v_i, a_i, f_i\)，最终得到 \(\tau\)。
（证明） 用数学归纳法证明：RNEA 的前向 pass 计算得到的 \(v_i\) 等于"关节 1 到关节 i 的速度贡献之和"，即 \(v_i = \sum_{j \in \text{path}(0,i)} {}^iX_j S_j \dot{q}_j\)。
（编程） 用 Pinocchio 加载 Franka Panda 模型，验证恒等式 rnea(q, v, 0) == nonLinearEffects(q, v)，对 1000 组随机输入计算最大绝对误差。

4. CRBA：计算质量矩阵 M(q) 的 O(n²) 算法 ⭐⭐¶

4.1 动机与问题定义¶

CRBA 解决的问题：给定关节位置 q，计算广义质量矩阵 \(M(q) \in \mathbb{R}^{n \times n}\)（对称正定）。

为什么需要显式的 M(q)？

在许多控制和估计算法中，我们需要 M(q) 本身而不仅仅是 \(M\ddot{q}\)： - WBC（全身控制）：QP 的约束中需要 \(M(q)\ddot{q} + h = J_c^T \lambda + S^T \tau\) - 阻抗控制：\(\tau = M(q) \ddot{q}_{des} + C\dot{q} + g - K_p e - K_d \dot{e}\) - 状态估计：动力学预测步需要 \(\ddot{q} = M^{-1}(\tau - h)\) - 操作空间控制：操作空间惯量 \(\Lambda = (J M^{-1} J^T)^{-1}\) 需要 M

RNEA 无法直接给出 M——它给出的是 \(M\ddot{q} + C\dot{q} + g\) 的整体值。虽然可以通过 n 次 RNEA 调用（\(\ddot{q} = e_j\)）逐列构造 M，但 CRBA 用更聪明的方法，一次后向遍历就填满了 M。

4.2 复合刚体惯性的几何含义 ⭐⭐¶

核心概念：把以 link i 为根的子树 \(T(i)\) 的所有 link"冻结"成一个刚体——这个复合刚体的总 spatial inertia 就是**复合刚体惯性** \(I_i^c\)。

\[\boxed{I_i^c = I_i + \sum_{j \in \text{children}(i)} {}^iX_j^* \, I_j^c \, {}^jX_i}\]

为什么要"冻结"子树？ 因为质量矩阵 \(M_{ij}\) 的物理含义是"只有关节 j 以单位加速度运动，其他关节都冻结时，关节 i 需要的力矩"。当子树内所有关节都冻结时，子树就是一个刚体，其惯性参数可以一次性算出来——不需要逐 link 累加。

跨领域类比：这就像计算一棵圣诞树各分枝的总重量——从叶子开始，把每个小分枝的重量加到它的母枝上，逐级累加到树干。每个节点存储的是"它以下全部重量的累计值"，这样从根到任何节点的总重量都可以在 O(1) 时间内查询到。

本质洞察：CRBA 的"复合刚体惯性"本质上是一种**自底向上的动态规划**——它利用了 M(q) 的一个深层结构性质：\(M_{ij}\) 只依赖于"关节 i 和关节 j 的最低公共祖先到两者之间路径上的 link 惯性"。把子树惯性累加后，这些路径上的信息被压缩成 6×6 矩阵，大大减少了重复计算。

4.3 CRBA 完整伪代码与推导¶

算法 CRBA(model, q) → M ∈ ℝⁿˣⁿ   (仅上三角)
输入:  q ∈ ℝⁿ (关节位置)
输出:  M ∈ ℝⁿˣⁿ (广义质量矩阵，对称正定，仅填充上三角)

# ===== Pass 1: Forward（运动学 + 初始化）=====
for i = 1 .. n:
    [ⁱXλ(i), Sᵢ] ← jcalc(jointType(i), qᵢ)
    Iᶜᵢ ← Iᵢ                    // 初始化：每个 link 的复合惯性 = 自身惯性

# ===== Pass 2: Backward（累加复合惯性 + 填充 M）=====
for i = n .. 1:
    # Step 1: 将子树惯性传给父节点
    if λ(i) ≠ 0:
        Iᶜ_{λ(i)} ← Iᶜ_{λ(i)} + ⁱXλ(i)ᵀ · Iᶜᵢ · ⁱXλ(i)    // Plücker 共轭变换

    # Step 2: 计算 F = Iᶜᵢ · Sᵢ （6 × nᵢ 矩阵）
    F ← Iᶜᵢ · Sᵢ

    # Step 3: 对角块 M[i,i] = Sᵢᵀ · F
    M[i, i] ← Sᵢᵀ · F             // nᵢ × nᵢ 子矩阵

    # Step 4: 沿祖先链填充交叉耦合块
    j ← i
    while λ(j) ≠ 0:
        F ← ⱼXλ(j)ᵀ · F           // 向祖先方向传播
        j ← λ(j)
        M[j, i] ← Sⱼᵀ · F         // nⱼ × nᵢ 子矩阵

return M

Step 4 的物理含义：沿祖先链传播 F 矩阵，计算"关节 i 的运动如何通过子树惯性耦合到祖先关节 j 上"。\(M_{ji}\) 表示"关节 i 加速时，关节 j 需要承受的反力"。

4.4 O(n²) 复杂度分析¶

定理：CRBA 的计算复杂度是 O(n²)（最坏情况下的链式拓扑）。

证明：

Pass 1：n 次 jcalc，每次 O(1)，总计 O(n)
Pass 2 外层循环：n 次
复合惯性回传 Iᶜ_{λ(i)} += ...：一次 6×6 双侧变换 ≈ 72 FLOPs
对角块计算：O(1)（对 revolute 关节，Sᵢ 是 6×1）
内层 while 循环：沿祖先链上传，最坏情况（链式拓扑）每次跑 O(n) 步

总复杂度：\(O(n) + \sum_{i=1}^{n} \text{depth}(i) = O(n \cdot \bar{d})\)

链式机器人（退化为一条直线）：\(\bar{d} = O(n)\)，总复杂度 O(n²)
平衡二叉树（人形机器人躯干分叉）：\(\bar{d} = O(\log n)\)，实际 O(n log n)

CRBA vs "n 次 RNEA" 常数对比：

方法	理论复杂度	每关节常数	Atlas (36-DoF) 实测
CRBA	O(n²)	≈ 7n	≈ 3.5 μs
n × RNEA	O(n²)	≈ 25n	≈ 9 μs

CRBA 快约 3 倍，因为它共享了复合惯性的计算，而 n 次 RNEA 每次都要重新做前向 pass。

4.5 CRBA 的 Worked Example：理解复合惯性的累加¶

考虑一个简单的 2-link 平面臂来具体理解 CRBA 的执行过程：

Setup：Link 1 质量 \(m_1 = 3\) kg, Link 2 质量 \(m_2 = 2\) kg，两个 revolute 关节。

Pass 2 执行（从 i=2 开始倒序）：

i = 2： - \(I_2^c = I_2\)（叶子节点，复合惯性就是自身惯性） - \(F_2 = I_2^c \cdot S_2\)（6×1 向量——\(I_2^c\) 在关节 2 方向的投影） - \(M_{22} = S_2^T F_2 = S_2^T I_2^c S_2\)

对 revolute 关节，\(S_2 = [0,0,1,0,0,0]^T\)，所以 \(M_{22}\) 就是 \(I_2^c\) 的 (3,3) 元素——即 link 2 绕其关节轴的转动惯量。

将 \(I_2^c\) 传给父节点：\(I_1^c \leftarrow I_1^c + {}^1X_2^* I_2^c {}^2X_1\)

这一步的物理含义：link 2 的惯性（表示在 link 2 坐标系中）被 Plücker 变换到 link 1 坐标系，然后加到 link 1 的惯性上。变换后的 \(I_2^c\)（在 link 1 坐标系中）相当于"把 link 2 冻结后，从 link 1 的视角看到的额外惯性"。

沿祖先链填充：\(F_2' = {}^1X_2^T F_2\)（将 F 变换到 link 1 坐标系） \(M_{12} = S_1^T F_2'\)

\(M_{12}\) 的物理含义：关节 2 加速时对关节 1 产生的反力矩——即关节 2 的运动通过子树惯性耦合到关节 1。

i = 1： - 此时 \(I_1^c\) 已经包含了 link 2 的贡献（上一步累加过了） - \(F_1 = I_1^c \cdot S_1\) - \(M_{11} = S_1^T I_1^c S_1\)

\(M_{11}\) 比 link 1 自身的转动惯量大——因为 \(I_1^c\) 包含了"冻结 link 2 后整个子树"的惯性。这就是为什么根关节的有效惯量总是最大的。

验证：最终得到的 2×2 矩阵 M 应该与 Lagrangian 方法（从动能 \(T = \frac{1}{2}\dot{q}^T M \dot{q}\) 推导）得到的结果完全一致。

关键观察：

复合惯性 \(I_1^c\) 包含了 link 1 和 link 2 的总惯性——就像"把子树冻结成一个大刚体"
\(M_{22} < M_{11}\)——因为关节 2 只"看到"link 2 的惯性，而关节 1 "看到"整个臂的惯性
\(M_{12} = M_{21}\)——对称性来自能量守恒（动能 \(T\) 对 \(\dot{q}\) 的 Hessian 必须对称）

4.6 为什么 CRBA 只需要 O(n²) 而不是 O(n³)¶

一个自然的疑问是：M 有 n² 个元素，计算每个似乎至少需要 O(1)，所以 O(n²) 已经是下界了？

是的，但 CRBA 的巧妙之处在于它**不独立地计算每个 \(M_{ij}\)**。关键观察：

复合惯性 \(I_i^c\) 只计算一次（后向累加），代价 O(n)
对角块 \(M_{ii} = S_i^T I_i^c S_i\) 只需 O(1)（S 是 6×1 或 6×3）
交叉块 \(M_{ji}\) 通过沿祖先链传播 F 矩阵得到，每个 \(M_{ji}\) 花 O(1)

总的 \(M_{ji}\) 对数 = \(\sum_i \text{depth}(i)\)，对链式拓扑是 \(\sum_i i = O(n^2)\)，因此 CRBA 的 O(n²) 来自"填充 M 上三角的总非零元素数"——它已经是 information-theoretic 下界了。

对比暴力方法：直接按定义 \(M_{ij} = \sum_k \frac{\partial T}{\partial \dot{q}_i \partial \dot{q}_j}\) 计算，每个 \(M_{ij}\) 涉及对所有 link 求导，单个元素 O(n)，总计 O(n³)——CRBA 通过"共享子树惯性"把 n³ 降到了 n²。

4.7 常见陷阱¶

⚠️ 编程陷阱：CRBA 只返回 M 上三角

错误做法：tau = data.M @ a  // 直接矩阵-向量乘
现象：结果错误（下三角全是 0 或垃圾值）
根本原因：Pinocchio 的 crba 只填充 data.M 的上三角部分（节省一半计算量）
正确做法：
    # Python
    M = data.M
    M = M + M.T - np.diag(M.diagonal())  # 对称化
    # 或者：
    tau = M @ a  # 使用 scipy.linalg.solve 的 assume_a='pos'
    # C++
    data.M.selfadjointView<Eigen::Upper>()  // 告诉 Eigen M 是对称的

💡 概念误区：认为 CRBA 和 RNEA 计算重复

新手想法："CRBA 计算 M(q)，RNEA 计算 M(q)q̈+C+g，是不是算了两遍 M？"
实际上：RNEA 从不显式构造 M。它直接通过递推得到 τ = M q̈ + C q̇ + g 的值，
        但这个"值"中的 M 是隐式的——你无法从 RNEA 的输出反推出 M 本身。
        CRBA 的价值在于显式输出 M 的每个元素，供需要 M 本身的算法使用。
正确理解：RNEA 给出"乘积的结果"，CRBA 给出"矩阵本身"——功能互补，不冲突。

4.8 练习题¶

（手推） 对 2-DOF 平面臂，手动执行 CRBA 的后向 pass：计算 \(I_2^c = I_2\)，\(I_1^c = I_1 + {}^1X_2^* I_2^c {}^2X_1\)，然后填充 \(M_{22} = S_2^T I_2^c S_2\)，\(M_{12} = S_1^T ({}^1X_2^T I_2^c S_2)\)，\(M_{11} = S_1^T I_1^c S_1\)。与 Lagrangian 方法（直接对动能取偏导）得到的 M 对比。
（编程） 用 Pinocchio 对 ANYmal (18-DoF) 验证：CRBA 输出的 M 是对称正定的（检查 \(M = M^T\)，所有特征值 > 0）。画出 M 的稀疏结构图（plt.spy(M)），观察哪些位置是零或近零——这反映了关节树的拓扑。

5. ABA：关节体正动力学 O(n) ⭐⭐⭐¶

5.1 动机与问题定义¶

ABA 解决的问题：已知关节位置 \(q\)、速度 \(\dot{q}\)、力矩 \(\tau\)，求关节加速度 \(\ddot{q}\)。

数学表达：

\[\ddot{q} = \text{ABA}(q, \dot{q}, \tau) = M(q)^{-1}(\tau - C(q,\dot{q})\dot{q} - g(q))\]

为什么需要专门的正动力学算法？ 仿真器的核心循环就是正动力学：给定当前状态 \((q, \dot{q})\) 和控制输入 \(\tau\)，计算加速度 \(\ddot{q}\)，然后积分得到下一时刻的状态。如果用"CRBA + Cholesky + solve"的 Lagrangian 路线，复杂度是 O(n²) + O(n³/3) + O(n²)。ABA 把这一切压缩到 O(n)。

历史脉络：1983 年 Featherstone 在 IJRR 上发表 ABA，这是机器人动力学算法的里程碑时刻。在此之前，正动力学只能通过"先算 M 再求逆"的间接方法完成。Featherstone 的洞察是：不需要先构造 M 再求逆——可以在树上的递推过程中直接"消元"。

5.2 关节体惯性（Articulated-Body Inertia）的概念 ⭐⭐⭐¶

定义：对 link i 以及以它为根的子树 \(T(i)\)，假设所有子关节都是"自由关节"（只受约束力，不受外部驱动力矩），则存在唯一的 6×6 对称正定矩阵 \(I_i^A\) 和 6D 偏置力 \(p_i^A\)，使得：

\[f_i = I_i^A \, a_i + p_i^A\]

即：以子树全体为一个"铰接体"，其"等效惯量"就是 \(I_i^A\)，其"等效偏置力"就是 \(p_i^A\)。

对比复合刚体惯性 \(I_i^c\)：

特征	复合刚体惯性 \(I_i^c\)	关节体惯性 \(I_i^A\)
假设	子树关节冻结（q̈ = 0, q̇ = 0）	子树关节自由（τ = 0）
物理含义	"冻结子树"的总惯量	"铰接子树"的等效惯量
数学性质	总是 ≥ \(I_i\)（惯性只增不减）	总是 ≤ \(I_i^c\)（自由关节"吸收"了部分惯性）
用于	CRBA（计算 M）	ABA（计算正动力学）

跨领域类比：想象你推一辆购物车。如果车轮被锁死（冻结），你感受到的是整辆车的惯性（\(I^c\)）。如果车轮可以自由转动（铰接），你推车时感受到的"等效惯性"会更小——因为车轮的转动"吸收"了部分推力，而不会把所有惯性传递给你的手。\(I^A\) 就是这个"考虑了关节自由度后的等效惯性"。

本质洞察：关节体惯性 \(I^A\) 的本质是一个 Schur complement。它把"关节自由度方向上可以被力矩吸收的惯性"减去了，只保留了"约束方向上不可避免地传递给父节点的惯性"。\(I^A = I^c - (\text{关节方向可吸收的部分})\)。

5.3 \(I^A\) 的递推公式——Schur Complement 的核心¶

Featherstone 的关键洞察：\(I^A\) 和 \(p^A\) 可以从**叶到根递推**，在每一层消去子关节的约束力：

\[\boxed{I_i^A = I_i + \sum_{j \in \text{children}(i)} {}^iX_j^* \Big(I_j^A - I_j^A S_j (S_j^T I_j^A S_j)^{-1} S_j^T I_j^A\Big) {}^jX_i}\]

记 \(U_j = I_j^A S_j\), \(D_j = S_j^T U_j\)（对 revolute 关节，D 是标量），则括号内简化为：

\[I_j^A - U_j D_j^{-1} U_j^T\]

这就是经典的 Schur complement（Schur 补）！

Schur complement 的几何含义：考虑一个二次型 \(x^T A x\) 被分块为：

\[\begin{bmatrix} x_1 \\ x_2 \end{bmatrix}^T \begin{bmatrix} A_{11} & A_{12} \\ A_{21} & A_{22} \end{bmatrix} \begin{bmatrix} x_1 \\ x_2 \end{bmatrix}\]

如果 \(x_2\) 是"自由的"（可以选择最优值最小化二次型），那么对 \(x_2\) 求最优后，剩余的 \(x_1\) 的有效二次型矩阵就是 \(A_{11} - A_{12} A_{22}^{-1} A_{21}\)——这就是 Schur complement。

在 ABA 中：\(x_2\) 对应"关节加速度 \(\ddot{q}_j\)"（自由变量），\(x_1\) 对应"父节点加速度 \(a_{\lambda(j)}\)"（传递给父节点的力）。"消去 \(x_2\)"就是"让关节自由运动，看父节点感受到多少等效惯性"。

5.4 ABA 完整伪代码（三次遍历）¶

算法 ABA(model, q, q̇, τ) → q̈    (正动力学：已知力矩求加速度)
输入:  q ∈ ℝⁿ, q̇ ∈ ℝⁿ, τ ∈ ℝⁿ
输出:  q̈ ∈ ℝⁿ

# ===== Pass 1: Forward（运动学 + 初始化）=====
v₀ ← 0
for i = 1 .. n:
    [ⁱXλ(i), Sᵢ] ← jcalc(jointType(i), qᵢ, q̇ᵢ)
    vᵢ ← ⁱXλ(i) · v_{λ(i)} + Sᵢ q̇ᵢ        // 速度递推
    cᵢ ← vᵢ × (Sᵢ q̇ᵢ)                      // bias acceleration
    Iᴬᵢ ← Iᵢ                                // 初始化关节体惯性
    pᴬᵢ ← vᵢ ×* (Iᵢ vᵢ) − fᵢᵉˣᵗ           // 初始化偏置力

# ===== Pass 2: Backward（从叶到根，Schur 消元）=====
for i = n .. 1:
    Uᵢ ← Iᴬᵢ · Sᵢ                           // 6 × nᵢ 矩阵
    Dᵢ ← Sᵢᵀ · Uᵢ                           // nᵢ × nᵢ 矩阵（关节有效惯量）
    uᵢ ← τᵢ − Sᵢᵀ · pᴬᵢ                    // 残余力矩

    if λ(i) ≠ 0:
        # Schur complement：消去关节 i 的自由度
        Iᴬ_reduced ← Iᴬᵢ − Uᵢ · Dᵢ⁻¹ · Uᵢᵀ    // 关键！去掉"可被关节吸收"的惯性
        Iᴬ_{λ(i)} ← Iᴬ_{λ(i)} + ⁱXλ(i)ᵀ · Iᴬ_reduced · ⁱXλ(i)

        # 偏置力传播
        pᴬ_{λ(i)} ← pᴬ_{λ(i)} + ⁱXλ(i)ᵀ · (pᴬᵢ + Iᴬᵢ · cᵢ + Uᵢ · Dᵢ⁻¹ · uᵢ)

# ===== Pass 3: Forward（从根到叶，回代求 q̈）=====
a₀ ← −g₀                                     // 注入重力
for i = 1 .. n:
    a'ᵢ ← ⁱXλ(i) · a_{λ(i)} + cᵢ            // 父加速度传递 + bias
    q̈ᵢ ← Dᵢ⁻¹ · (uᵢ − Uᵢᵀ · a'ᵢ)          // 关节加速度回代
    aᵢ ← a'ᵢ + Sᵢ · q̈ᵢ                      // 完整 link 加速度

return q̈

5.5 ABA 的 O(n) 复杂度证明¶

定理：ABA 的计算复杂度是严格 O(n)。

证明：

三个 pass 都是沿树的**单次遍历**（没有嵌套循环）：

Pass 1：每关节的运算量（与 RNEA Pass 1 相同）≈ 60 FLOPs - Plücker 变换：≈ 24 FLOPs - 叉积 \(c_i\)：≈ 12 FLOPs - 初始化 \(I^A, p^A\)：≈ 24 FLOPs

Pass 2：每关节的运算量 ≈ 100 FLOPs - \(U_i = I^A_i S_i\)：对 revolute 关节，6×6 矩阵选一列 = 6 FLOPs - \(D_i = S_i^T U_i\)：对 revolute，内积 = 6 FLOPs（D 是标量） - \(I^A_{reduced} = I^A_i - U_i D_i^{-1} U_i^T\)：秩 1 更新 = 21 FLOPs - Plücker 双侧变换：≈ 72 FLOPs

Pass 3：每关节的运算量 ≈ 40 FLOPs - Plücker 变换：≈ 24 FLOPs - \(q̈_i = D_i^{-1}(u_i - U_i^T a'_i)\)：≈ 7 FLOPs - \(a_i = a'_i + S_i q̈_i\)：≈ 6 FLOPs

总运算数：\(\text{Total} = n \cdot (60 + 100 + 40) \approx 200n\) FLOPs

常数约 200-220 FLOPs/关节，严格 O(n)。

与 CRBA+Cholesky 路线的对比：

n (DoF)	ABA O(n)	CRBA O(n²) + Cholesky O(n³/3)	ABA 加速比
7	1,540	343 + 114 + 49 ≈ 506	0.3× (ABA 更慢!)
12	2,640	1,008 + 576 + 288 ≈ 1,872	0.7×
18	3,960	2,268 + 1,944 + 972 ≈ 5,184	1.3×
36	7,920	9,072 + 15,552 + 7,776 ≈ 32,400	4.1×

经验规则：n ≲ 12 时 CRBA+Cholesky 更快（因为 BLAS/SIMD 对密集小矩阵极度优化）；n ≳ 12 时 ABA 开始占优势；n = 36 时 ABA 快 4 倍以上。

5.6 ABA 与 LDLᵀ 分解的等价性 ⭐⭐⭐¶

定理（Featherstone Ch.7 §7.3, Jain Ch.7）：ABA 的三次遍历在代数上等价于对质量矩阵 \(M(q)\) 做**块 LDLᵀ 分解**，其中块结构由关节树诱导。

等价关系： - Pass 2（Backward）= LDLᵀ 的**因子化步骤**（从最后一行开始消元） - Pass 3（Forward）= LDLᵀ 的**前向/后向替换**（求解 \(M \ddot{q} = \tau - h\)） - \(D_i\) = LDLᵀ 中的对角块 D 的第 i 个元素 - \(U_i\) = L 因子中的列块

为什么这很深刻？ 它揭示了 ABA 不是一个"天外飞仙"的巧妙算法，而是一个**自然的稀疏线性代数操作**——它就是在利用 M(q) 的树状稀疏结构做高效分解。如果你把 M(q) 的稀疏模式画出来，它的 Cholesky 因子 L 的非零结构恰好就是"每个节点与其所有祖先"——这是由关节树的拓扑决定的。

这个视角的威力在于： 1. 它解释了为什么 ABA 是 O(n)——因为树状拓扑的 LDLᵀ 就是 O(n) 2. 它指明了推广方向——闭链约束打破了树状结构，需要 fill-in 处理 3. 它连接了动力学和 SLAM——iSAM2 的 Bayes tree 也是做 elimination tree 上的稀疏因子化

5.7 ABA 正确性定理¶

定理：\(\text{ABA}(q, \dot{q}, \tau) = M(q)^{-1}(\tau - C(q,\dot{q})\dot{q} - g(q))\)

证明思路：

Pass 1 与 RNEA 的 Pass 1 本质相同——计算 \(v_i\), \(c_i\)。偏置力 \(p_i^A = v_i \times^* I_i v_i - f_i^{ext}\) 是"假设 \(\ddot{q}=0\) 时每个 link 会施加的 spatial force"。
Pass 2 的核心是逐关节 Schur complement 消元。数学上等价于：对线性方程组

\[M(q) \ddot{q} = \tau - C(q,\dot{q})\dot{q} - g(q)\]

从最后一个关节开始，逐个消去 \(\ddot{q}_n, \ddot{q}_{n-1}, \ldots\)。消去 \(\ddot{q}_i\) 后，方程组"缩小"了一维，\(I^A_{\lambda(i)}\) 记录了消元后的等效惯性。

Pass 3 是回代——从根向叶逐个求解 \(\ddot{q}_i\)，这对应 LDLᵀ 分解后的前向/后向替换。
结合正确性：由构造，ABA 的输出满足 \(M \ddot{q} + h = S^T \tau + J_c^T \lambda\)（无约束时 \(\lambda = 0\)），即 \(\ddot{q} = M^{-1}(\tau - h)\)。 \(\blacksquare\)

5.8 常见陷阱¶

⚠️ 编程陷阱：Iᴬ 的正定性丢失

现象：ABA 输出 NaN 或极大值
根本原因：Schur complement Iᴬ − U D⁻¹ Uᵀ 在浮点下可能失去对称正定性
          尤其当 Dᵢ 很小时（关节有效惯量接近零），数值误差被放大
修复方法：
  1. 加 armature（电机反射惯量）：model.armature[i] > 0 增大 Dᵢ
  2. 显式对称化：Iᴬ = (Iᴬ + Iᴬ.T) / 2
  3. 使用 double precision（single 在 36+ DoF 时容易崩）
Pinocchio 做法：jdata.StU().diagonal() += model.armature (line 365 of aba.hxx)

💡 概念误区：认为 ABA 和 CRBA+solve 给出不同结果

新手想法："ABA 是近似算法，可能不如直接求解 M⁻¹(τ-h) 精确"
实际上：ABA 在精确算术下给出完全相同的结果——它们在代数上等价
        浮点精度差异通常在 1e-12 量级（double），不影响工程使用
        唯一例外：极端病态模型（大质量比 > 10⁶）时 ABA 可能比 CRBA+LDLᵀ 更不稳定
        因为 ABA 的 Schur complement 累积了更多浮点误差
验证方法：ddq_aba vs np.linalg.solve(M, tau - nle)，差异应 < 1e-10

5.9 练习题¶

（手推） 对 2-DOF 平面臂，手动执行 ABA 的三个 pass。从 link 2 开始计算 \(I_2^A = I_2\)，做 Schur complement 得到传给 link 1 的等效惯性，然后 Pass 3 回代得到 \(\ddot{q}_1, \ddot{q}_2\)。验证结果与 \(M^{-1}(\tau - C\dot{q} - g)\) 一致。
（证明） 证明关节体惯性满足 \(I_i^A \preceq I_i^c\)（矩阵不等式，半正定序）。提示：\(I^A\) 是 \(I^c\) 减去一个半正定矩阵（Schur complement 项）。
（编程） 对 TALOS (38-DoF)，分别用 ABA 和 CRBA+Cholesky+solve 计算正动力学，对 10000 组随机输入比较：(a) 结果差异的最大范数 (b) 计算时间的均值和标准差。

5.10 ABA 的 Worked Example：2-DOF 平面臂完整手推 ⭐⭐¶

为了让 ABA 的三次遍历变得具体，我们在一个极简模型上逐步执行全部计算。

模型设定：2-DOF 平面臂，两个 revolute 关节绕 z 轴（2D 简化，但使用完整 6D spatial vector 框架）。

Link 1：长度 \(L_1 = 1\) m，质量 \(m_1 = 2\) kg，质心在中点，转动惯量 \(I_{zz,1} = m_1 L_1^2/12\)
Link 2：长度 \(L_2 = 0.8\) m，质量 \(m_2 = 1\) kg，质心在中点，转动惯量 \(I_{zz,2} = m_2 L_2^2/12\)
初始状态：\(q = [0, 0]^T\)（两臂水平），\(\dot{q} = [1, 2]^T\)，\(\tau = [5, 3]^T\)
重力：\(g = 9.81\) m/s² 向下

Pass 1：Forward（初始化）

\(v_0 = 0\)（世界静止）

关节 1（\(i = 1\)）： - \(S_1 = [0, 0, 1, 0, 0, 0]^T\)（绕 z 旋转） - \(v_1 = {}^1X_0 \cdot v_0 + S_1 \dot{q}_1 = S_1 \cdot 1 = [0, 0, 1, 0, 0, 0]^T\) - \(c_1 = v_1 \times (S_1 \dot{q}_1) = [0,0,1,0,0,0]^T \times [0,0,1,0,0,0]^T = 0\)（同方向叉积为零） - \(I_1^A \leftarrow I_1\)（link 1 的 6×6 spatial inertia） - \(p_1^A \leftarrow v_1 \times^* (I_1 v_1) - f_1^{ext}\)

关节 2（\(i = 2\)）： - \(v_2 = {}^2X_1 \cdot v_1 + S_2 \dot{q}_2\) - \(c_2 = v_2 \times (S_2 \dot{q}_2)\)（非零！因为 \(v_2\) 的线速度部分非零） - \(I_2^A \leftarrow I_2\) - \(p_2^A \leftarrow v_2 \times^* (I_2 v_2)\)

Pass 2：Backward（Schur 消元）

从 \(i = 2\) 开始： - \(U_2 = I_2^A S_2\)：取 \(I_2^A\) 的第 3 列（因为 \(S_2 = e_3\)） - \(D_2 = S_2^T U_2 = I_2^A[3,3]\)：是标量（关节 2 的有效转动惯量） - \(u_2 = \tau_2 - S_2^T p_2^A = 3 - p_2^A[3]\)

Schur complement：\(I_2^{A,\text{reduced}} = I_2^A - U_2 D_2^{-1} U_2^T\)

这是一个秩 1 更新！对 revolute 关节，\(U_2\) 是 6×1 向量，\(D_2\) 是标量，所以 \(U_2 D_2^{-1} U_2^T\) 是一个秩 1 矩阵。

将 \(I_2^{A,\text{reduced}}\) 通过 Plücker 变换加到 \(I_1^A\)：

\[I_1^A \leftarrow I_1^A + {}^1X_2^* \cdot I_2^{A,\text{reduced}} \cdot {}^2X_1\]

然后对 \(i = 1\)： - \(U_1 = I_1^A S_1\) - \(D_1 = S_1^T U_1\)（这就是"关节 1 感受到的总有效惯量"——包含了子树的贡献） - \(u_1 = \tau_1 - S_1^T p_1^A\)

Pass 3：Forward（回代）

\(a_0 = -g_0 = [0, 0, 0, 0, 0, 9.81]^T\)（重力注入）

关节 1： - \(a_1' = {}^1X_0 \cdot a_0 + c_1\) - \(\ddot{q}_1 = D_1^{-1}(u_1 - U_1^T a_1')\) - \(a_1 = a_1' + S_1 \ddot{q}_1\)

关节 2： - \(a_2' = {}^2X_1 \cdot a_1 + c_2\) - \(\ddot{q}_2 = D_2^{-1}(u_2 - U_2^T a_2')\) - \(a_2 = a_2' + S_2 \ddot{q}_2\)

关键观察：

Pass 2 中，\(D_1\) 包含了从 link 2 传上来的惯性贡献——它不仅仅是 link 1 自身的惯量，还包括"通过关节 2 连接的子树惯量"（但被 Schur complement 减去了"关节 2 能吸收的部分"）
Pass 3 中，\(\ddot{q}_1\) 先被确定（因为根节点最先知道完整的加速度信息），然后 \(\ddot{q}_2\) 基于 \(a_1\)（包含了 \(\ddot{q}_1\) 的影响）计算——这就是"从根到叶"的回代
整个过程没有构造 M(q)，也没有求逆——直接从 \(\tau\) 得到了 \(\ddot{q}\)

5.11 ABA 中 velocity product force 的深入理解¶

Pass 1 中的 \(p_i^A = v_i \times^* (I_i v_i) - f_i^{ext}\) 值得深入讨论。

\(v_i \times^* (I_i v_i)\) 被称为 velocity product force（有时也叫 bias force）。它的物理含义是：即使所有加速度为零（\(\ddot{q} = 0\), \(a = 0\)），旋转的刚体由于离心力和科氏力效应，仍然会产生净 spatial force。

具体展开（对 link i 在其 body frame 中）：

\[v_i \times^* (I_i v_i) = \begin{bmatrix} \omega_i \\ v_{o,i} \end{bmatrix} \times^* \begin{bmatrix} I_{\omega} \omega_i + m_i \hat{c} v_{o,i} \\ m_i v_{o,i} - m_i \hat{c} \omega_i \end{bmatrix}\]

其中 \(\hat{c}\) 是质心相对于 body frame 原点的叉积矩阵。

反事实推理：如果忽略 velocity product force（\(p_i^A = -f_i^{ext}\)），ABA 的结果等价于假设"没有科氏力和离心力"。对高速旋转的关节（如工业机器人快速运动），忽略这些项会导致 10-50% 的加速度误差——足以让仿真在几百毫秒内发散。

6. Minv 算法：直接计算 M⁻¹ ⭐⭐⭐¶

6.1 问题定义与动机¶

有些场景需要**完整的 \(M^{-1}\) 矩阵**（而不仅仅是 \(M^{-1}v\) 的值）： - 某些 inverse-dynamics MPC 需要 \(M^{-1}\) 作为投影算子 - 操作空间动力学：\(\Lambda = (J M^{-1} J^T)^{-1}\) 需要 \(M^{-1}\) - 接触力求解中的 Delassus 矩阵 \(G = J M^{-1} J^T\)

6.2 两条路线¶

路线 1：CRBA + Cholesky + 显式求逆

1. crba(model, data, q)           → data.M (上三角)        O(n²)
2. cholesky::decompose(model, data) → data.U, data.D      O(n² ~ n·depth)
3. cholesky::computeMinv(model, data) → data.Minv         O(n²)
总计：O(n²)（对树形拓扑）

路线 2：Minv 直接算法（Featherstone 2010/2014, Carpentier 2018 变体）

在 ABA 的递推结构上直接生成 \(M^{-1}\) 的所有元素，不经过 M 的构造。Pinocchio API：computeMinverse(model, data, q) → data.Minv。

6.3 Minv 直接算法的核心思想¶

Minv 直接算法（Carpentier 2018 变体）的关键洞察是：\(M^{-1}\) 的第 j 列就是 \(M^{-1} e_j\)，而 \(M^{-1} e_j = \text{ABA}(q, 0, e_j)\)——即对"只有关节 j 有单位力矩"的情况做正动力学。

但如果简单地调 n 次 ABA，复杂度是 O(n²)（n 次 O(n)）。Minv 直接算法更聪明：它注意到 n 次 ABA 共享**同一个 Pass 2**（因为 \(I^A\) 和 \(p^A\) 不依赖 \(\tau\)），只需 n 次不同的 Pass 3。

算法 Minv(model, q) → M⁻¹ ∈ ℝⁿˣⁿ
# Pass 1 + Pass 2：与 ABA 相同，计算 Iᴬ, pᴬ, U, D（只做一次）
ABA_Pass1_Pass2(model, q)

# 对每一列 j，只做 Pass 3（回代）
for j = 1 .. n:
    a₀ ← 0  （不注入重力——因为我们只要 M⁻¹ 而非完整正动力学）
    u ← eⱼ  （只有关节 j 有单位力矩）
    for i = 1 .. n:
        q̈ᵢ = Dᵢ⁻¹(uᵢ - Uᵢᵀ a'ᵢ)
        aᵢ = a'ᵢ + Sᵢ q̈ᵢ
    M⁻¹[:, j] ← q̈

return M⁻¹

复杂度：Pass 1+2 = O(n)，n 次 Pass 3 = n × O(n) = O(n²)。总计 O(n²)。

与 CRBA+Cholesky+Invert 的对比： - CRBA+Cholesky+Invert：O(n²) + O(n²) + O(n²) = O(n²)，但常数更大 - Minv 直接：O(n) + O(n²) = O(n²)，常数更小（共享了 ABA 的 Pass 1+2）

6.4 何时用哪种¶

需求	最佳选择	复杂度
只需 \(M^{-1} v\)（一个向量）	`ABA(q, 0, v)`	O(n)
需要完整 \(M^{-1}\) 矩阵	`computeMinverse`	O(n²)
需要 M 和 \(M^{-1}\) 都有	CRBA + cholesky + computeMinv	O(n²)
需要 M 但不需要 \(M^{-1}\)	直接 CRBA	O(n²)

反事实推理：如果不知道 ABA(q, 0, v) = \(M^{-1}v\) 这个技巧，你可能会先用 CRBA 构造 M，再做 Cholesky 分解求解 Mv = b——对单个向量 v 这是 O(n²) + O(n²)，而 ABA 只需 O(n)。对 MPC 中 100 个 horizon 节点，这是 100×O(n) vs 100×O(n²) 的差距。

7. 三大算法统一视角：树递推的不同面目 ⭐⭐¶

7.1 核心统一：消息传递¶

RNEA、CRBA、ABA 看似功能不同，但本质上都是**同一棵关节树上的消息传递算法**：

算法	功能	消息传递方向	传递的"消息"
RNEA	逆动力学 τ	Forward + Backward	速度/加速度 → 力/力矩
CRBA	质量矩阵 M	Backward only	复合惯性 \(I^c\)
ABA	正动力学 q̈	Forward + Backward + Forward	速度 → 关节体惯性 \(I^A\) → 加速度

跨领域类比：这三个算法与概率图模型（Bayesian Networks）上的推断算法惊人地类似：

动力学算法	概率推断类比	共同结构
RNEA（前向+后向）	Belief Propagation（Sum-Product）	树上的两次消息传递
CRBA（纯后向累加）	Variable Elimination（从叶到根消元）	自底向上动态规划
ABA（前向+后向+前向）	Gaussian Elimination on Bayes Net	LDLᵀ 因子化 + 回代

本质洞察：三大算法的统一本质是**利用树状图的稀疏结构做线性代数**。RNEA 是"矩阵-向量乘"的 O(n) 实现（利用 M 的隐式因子化），CRBA 是"显式构造 M"，ABA 是"直接求解 \(Mx = b\) 而不显式构造 M"。它们的深层统一源自 M(q) 的稀疏 Cholesky 因子恰好是关节树的 elimination tree。

7.2 SOA（Spatial Operator Algebra）视角¶

Jain 2011 的 SOA 框架给出了最优雅的统一：

\[M = H \Phi M_0 \Phi^T H^T\]

其中： - \(\Phi\)：树上的传播算子（对应 Plücker 变换链） - \(H\)：关节选择矩阵（对应 S） - \(M_0\)：块对角的 link 惯性矩阵

在这个框架下： - RNEA = 计算 \(M \ddot{q} + h\)（通过 \(\Phi\) 的前向/后向应用） - CRBA = 显式构造 \(H \Phi M_0 \Phi^T H^T\) - ABA = 利用 \(M\) 的 \(LDL^T\) 因子化求解 \(M\ddot{q} = \tau - h\)

7.3 从 Lagrangian 到递推：为什么 Newton-Euler 更适合计算¶

Lagrangian 方法和 Newton-Euler 方法从物理上是完全等价的——它们给出相同的运动方程。但从**计算效率**角度看，Newton-Euler（递推）方法有本质优势：

比较维度	Lagrangian	Newton-Euler (递推)
思考方式	能量（标量）→ 变分 → EoM	力/力矩（向量）→ 牛顿定律 → 递推
计算的"粒度"	整个系统的能量表达式	每个 link 的局部力平衡
符号复杂度	随 n 指数增长（Christoffel 符号）	固定：每 link 一组 6D 方程
利用拓扑	困难（M 是稠密矩阵）	自然（树递推直接利用稀疏性）
最优算法	O(n²) (CRBA) / O(n³) (直接)	O(n) (RNEA/ABA)

核心差异的根源：Lagrangian 方法的输出是一个 n×n 的矩阵方程（M, C, g），它把"局部的关节间耦合"编码成了"全局的矩阵元素"。即使 M(q) 的很多元素是零（远距离关节不耦合），构造矩阵本身也需要 O(n²)。Newton-Euler 递推从不构造这个矩阵——它直接在树上传递"力消息"，每个 link 只和邻居通信。

跨领域类比：这就像 PageRank 算法——你可以先构造完整的 web 图邻接矩阵（O(n²) 空间），然后做矩阵-向量乘；或者你可以直接在图上做消息传递（每个节点只看邻居，O(边数)）。后者就是 Power Iteration 的稀疏实现，等价于前者但快得多。RNEA 之于 Lagrangian 就是这个关系。

7.4 算法选型的工程决策树¶

                    你需要什么？
                        │
         ┌──────────────┼──────────────┐
         │              │              │
    τ = f(q,q̇,q̈)    M(q) 本身      q̈ = f(q,q̇,τ)
         │              │              │
      RNEA O(n)       CRBA O(n²)      │
                                       │
                              ┌────────┼────────┐
                              │        │        │
                          n ≤ 12   12<n<36    n ≥ 36
                              │        │        │
                    CRBA+dense     ABA O(n)   ABA O(n)
                    Cholesky                   (绝对最优)

完整选型决策表：

场景	首选算法	备选	理由
逆动力学（已知 q,q̇,q̈ 求 τ）	RNEA	—	O(n) 唯一正确选择
正动力学仿真 n < 12	CRBA + 密集 LDLᵀ	ABA	密集 Cholesky 在小 n 有 SIMD 优势
正动力学仿真 n ≥ 12	ABA	CRBA + 稀疏 Cholesky	ABA O(n) 常数更优
需要 M 矩阵（WBC/阻抗控制）	CRBA	n×RNEA	CRBA 常数约 3× 更小
需要 M⁻¹ 整矩阵	`computeMinverse`	CRBA+cholesky	Minv 直接算法 O(n²)
只需 M⁻¹ 乘一个向量	ABA(q,0,v)	—	O(n) 最优
MPC 内环（需导数）	解析微分	数值差分	解析快 5-10×
浮基+接触（人形/四足）	稀疏 LDLᵀ	ProxQP	MuJoCo island 算法
超高 DoF（>100）	D&C ABA	—	O(log N) 并行
批量环境（RL 训练）	GPU 数据并行	CPU 多线程	4096+ 环境时 GPU 饱和

7.5 computeAllTerms 的正确使用¶

控制器中经常一次性需要多个量（M, nle, Jacobian, CoM...）。如果分别调用各自的 API：

# ❌ 低效做法：前向运动学被重复计算 4 次！
pin.crba(model, data, q)                    # 内含一次前向运动学
pin.nonLinearEffects(model, data, q, v)     # 又做一次前向运动学
pin.computeJointJacobians(model, data, q)   # 又一次！
pin.centerOfMass(model, data, q, v)         # 又一次！

# ✅ 高效做法：computeAllTerms 共享前向运动学
pin.computeAllTerms(model, data, q, v)
# 一次调用，输出：data.M, data.nle, data.J, data.com, data.Ag, ...

computeAllTerms 在一次前向运动学遍历中顺便计算所有派生量，后向遍历累加 CRBA 和 nle，节省约 30-50% 计算量。

但要注意：computeAllTerms 不输出独立的 C(q,q̇) 矩阵（只输出 C·q̇+g 整体）。如果你的控制器需要 C 矩阵本身（例如 passivity-based control），要单独调 computeCoriolisMatrix。

7.6 练习题¶

（分析） 为什么 CRBA 在 n=7 时比 ABA 更快？从 BLAS Level 2/3 的 cache 行为角度解释。
（设计） 你正在设计一个 WBC 控制器，需要 M(q)、\(C\dot{q}+g\)、关节 Jacobian J。你会如何组合 Pinocchio 的 API 调用来最小化总计算时间？

8. 稀疏分解路线 ⭐⭐⭐¶

8.1 为什么讨论稀疏性¶

当机器人模型从串联臂（7 DoF）扩展到人形（36+ DoF）或多机器人系统（100+ DoF），\(M(q)\) 的维度快速增长，但**它的非零结构由关节树诱导，具有高度稀疏性**。利用这种稀疏性可以把 Cholesky 分解从 O(n³) 降到 O(n²) 甚至更低。

8.2 M(q) 的稀疏结构——Branch-Induced Sparsity¶

定理（Featherstone IJRR 2005）：\(M(q)\) 的元素 \(M_{ij}\) 非零当且仅当关节 i 和关节 j 在关节树中具有祖先-后代关系。

推论：\(M(q)\) 的非零结构恰好是关节树的"可达性矩阵"——如果 i 是 j 的祖先（或反之），则 \(M_{ij} \neq 0\)；否则 \(M_{ij} = 0\)。

对不同拓扑的稀疏性：

拓扑	M 的带宽	Cholesky 填充	因子化复杂度
串联链	全满（上三角全非零）	无 fill-in	O(n²)
平衡二叉树（人形双腿双臂）	每行 ≈ O(log n) 非零	O(n log n)	O(n log² n)
星形（8 条腿共享躯干）	只有躯干行全满	O(n)	O(n) + O(k²)（k=躯干 DoF）

8.3 稀疏 LDLᵀ 分解¶

Pinocchio 的 cholesky::decompose 实现了 Featherstone IJRR 2005 的稀疏 LDLᵀ：

for j = n-1 downto 0:
    D[j] = M[j,j] - (已计算的贡献)
    for i in ancestors(j):        // 只遍历祖先链！
        U[i,j] = (M[i,j] - ...) / D[j]

关键：内层循环只沿祖先链上爬——这是稀疏性的代码体现。对链式拓扑，祖先链长度 O(n)，总复杂度 O(n²)；对平衡树，祖先链长度 O(log n)，总复杂度 O(n log n)。

8.4 Elimination Tree 与因子图的关系 ⭐⭐⭐⭐¶

深层联系：Featherstone 的稀疏 Cholesky 与 SLAM 中 iSAM2 的 Bayes Tree 共享同一个数学结构——Elimination Tree。

动力学	SLAM	共同抽象
关节树	因子图	图结构
M(q) 的稀疏模式	信息矩阵的稀疏模式	稀疏矩阵
CRBA 的祖先链遍历	Variable Elimination 的 clique tree	Elimination tree
Featherstone sparse LDLᵀ	iSAM2 的 Bayes tree update	增量因子化

这不是巧合——两者都是在**树结构诱导的稀疏线性系统**上做高效因子化。理解了动力学的稀疏 Cholesky，你就自动理解了 SLAM 后端优化的核心数据结构。

8.5 稀疏 vs 密集的经验交叉点¶

根据 Sathya, Carpentier et al. (RSS 2021) 的 benchmark：

机器人	DoF	密集 Cholesky (μs)	稀疏 LDLᵀ (μs)	稀疏加速
UR5	6	0.3	0.35	×0.86（稀疏更慢！）
ANYmal A1	18	1.2	0.84	×1.4
TALOS	38	5.8	2.9	×2.0
22 humanoids	800+	不可行	45 (with island)	∞

经验规则：n ≤ 12 用密集（SIMD/cache 优势）；n ≥ 20 用稀疏；12-20 视拓扑而定。

8.6 稀疏 LDLᵀ 的具体实现（Featherstone IJRR 2005）¶

Pinocchio 的 cholesky::decompose 实现细节：

// 简化版稀疏 LDLᵀ（对应 Featherstone IJRR 2005 Algorithm 1）
void decompose(const Model& model, Data& data) {
    const int nv = model.nv;

    for (int j = nv - 1; j >= 0; --j) {
        // D[j] = M[j,j] - Σ_{k>j, k∈subtree(j)} U[k,j]² · D[k]
        data.D[j] = data.M(j, j);

        // 只遍历 j 的子树内的行（稀疏性！）
        const int nvt = data.nvSubtree_fromRow[j] - 1;
        for (int k = j + 1; k <= j + nvt; ++k) {
            data.D[j] -= data.U(k, j) * data.U(k, j) * data.D[k];
        }

        data.Dinv[j] = 1.0 / data.D[j];

        // 沿祖先链填充 U 的非零元素
        int i = model.parents_fromRow[j];
        while (i >= 0) {
            data.U(i, j) = data.M(i, j);
            for (int k = j + 1; k <= j + nvt; ++k) {
                data.U(i, j) -= data.U(i, k) * data.U(k, j) * data.D[k];
            }
            data.U(i, j) *= data.Dinv[j];
            i = model.parents_fromRow[i];  // 向上跳到下一个祖先
        }
    }
}

关键设计决策：

外层循环从 nv-1 到 0：子树后于父节点处理（保证依赖已就绪）
内层 while 只沿祖先链：这是稀疏性的代码体现——U 矩阵只在"节点-祖先"位置非零
nvSubtree_fromRow：记录每个 DOF 行对应的子树大小，限制内层循环范围
parents_fromRow：从 DOF 索引到父 DOF 的映射（对多 DOF 关节需要特殊处理）

8.7 LDLᵀ 求解（前向+后向替换）¶

一旦有了 \(M = U D U^T\) 的因子化，解 \(Mx = b\) 分三步：

1. 前向替换：解 U^T y = b
   for i = 0 .. nv-1:
       y[i] = b[i] - Σ_{j: parent(j)=i} U[j,i] · y[j]

2. 对角缩放：z = D⁻¹ y
   for i = 0 .. nv-1:
       z[i] = Dinv[i] · y[i]

3. 后向替换：解 U x = z
   for i = nv-1 .. 0:
       x[i] = z[i]
       j = parent(i)
       while j >= 0:
           x[j] -= U[i,j] · x[i]
           j = parent(j)

每步复杂度与因子化相同：O(n·depth)。

8.8 MuJoCo 的独特做法¶

MuJoCo 对所有矩阵都用稀疏 LDLᵀ，并配合 3.0 版本的 contact island algorithm：

检测场景中的独立接触"岛"（互不影响的连通分量）
每个岛独立做约束求解
多岛并行执行

这就是 MuJoCo 在"22 个人形同时接触"场景能给出 3× 加速（DeepMind 官方 benchmark）的原因。

9. 并行化：D&C ABA 与 GPU 批量 RNEA ⭐⭐⭐⭐¶

9.1 为什么需要并行化¶

标准 RNEA/ABA 是**串行算法**——前向 pass 的第 i 步依赖第 i-1 步的输出。在 O(n) 已经是最优串行复杂度的情况下，进一步加速只能靠并行。

两个主要应用场景： 1. 超大多体系统（>100 DoF）：分子动力学、柔性机器人 2. 批量并行（强化学习）：同一模型在 10000+ 不同状态上做动力学计算

9.2 Divide-and-Conquer ABA (O(log N))¶

Featherstone 在 IJRR 1999 ("A Divide-and-Conquer Articulated-Body Algorithm for Parallel O(log N) Calculation of Rigid-Body Dynamics") 提出了 D&C ABA：

核心思想：把 n 关节的树从中间切开，形成两个 n/2 的子树。每个子树独立计算自己的关节体惯性 \(I^A\)，然后在连接点处做一次 6×6 的 Schur complement 合并。

DCA(subtree T):
    if |T| == 1:
        return I^A_leaf, p^A_leaf       // 叶子直接计算
    else:
        T_left, T_right = split(T)      // 从中间切开
        (I^A_L, p^A_L) = DCA(T_left)    // 递归（可并行!）
        (I^A_R, p^A_R) = DCA(T_right)   // 递归（可并行!）
        return merge(I^A_L, p^A_L, I^A_R, p^A_R)  // 合并（6×6 操作）

复杂度：递归深度 \(O(\log n)\)，每层做 O(1) 个 6×6 操作。给定 O(n) 个处理器，总并行时间 O(log n)。

实际应用：主要在分子动力学（1000+ 链节）和 GPU 仿真（如 Isaac Gym 内部）中使用。对常规 36-DoF 机器人，O(n) 已经够快（3 μs），D&C 的 overhead 不划算。

9.3 GPU 批量 RNEA/ABA¶

强化学习训练时，需要对同一个机器人模型在 10000+ 不同状态上并行计算动力学。此时的并行策略是**数据并行**而非算法并行：

// 每个 CUDA thread 处理一个环境实例
__global__ void batch_rnea(states[N_envs], taus[N_envs]) {
    int env_id = blockIdx.x * blockDim.x + threadIdx.x;
    taus[env_id] = rnea(model, states[env_id].q, states[env_id].v, states[env_id].a);
}

MuJoCo 的 mjx（JAX 加速版）和 NVIDIA Isaac Gym 都采用这种策略。在 A100 GPU 上，批量 RNEA 可以达到 10M+ 次/秒。

9.4 D&C ABA 的合并操作详解¶

当两个子树 \(T_L\) 和 \(T_R\) 在连接关节处合并时，需要做什么？

假设连接关节为 joint k，将 \(T_R\) 连接到 \(T_L\) 的 link k 上。\(T_R\) 已经计算出了从其根 link 看到的关节体惯性 \(I_{R}^A\) 和偏置力 \(p_{R}^A\)。合并步骤：

将 \(I_R^A\) 通过 Plücker 变换到 link k 的坐标系
做 Schur complement 消去 joint k 的自由度
将结果加到 \(T_L\) 对 link k 的惯性上

这个合并操作本身是 O(1)（常数个 6×6 矩阵运算），因此不影响并行深度。

D&C ABA 的工程限制：

限制因素	说明	影响
通信开销	子树间合并需要传递 6×6 矩阵（288 bytes）	在 GPU 上 warp 间通信有延迟
负载均衡	人形机器人的树不是平衡的（腿比臂长）	某些处理器闲置等待
适用阈值	n > 50 才有加速效果	常规机器人太小，overhead 超过收益
数值稳定性	合并操作中 Schur complement 的累积误差	需要双精度

9.5 GPU 批量动力学的工程实践¶

MuJoCo MJX (JAX 后端)：

MuJoCo 3.0 提供了 mjx 子库，将核心动力学算法用 JAX 重写，支持 GPU/TPU 加速：

import mujoco
from mujoco import mjx
import jax

# 加载模型并创建 mjx 数据
model = mujoco.MjModel.from_xml_path("humanoid.xml")
mjx_model = mjx.put_model(model)

# 批量创建 4096 个环境
batch_size = 4096
keys = jax.random.split(jax.random.PRNGKey(0), batch_size)

# vmap 自动批量化
batched_step = jax.vmap(mjx.step, in_axes=(None, 0))

# 在 GPU 上并行执行 4096 个仿真步
# 单次调用完成所有环境的动力学计算
mjx_data_batch = batched_step(mjx_model, mjx_data_batch)

NVIDIA Isaac Gym / Isaac Lab：

Isaac Lab 使用 CUDA 实现的批量动力学，在 RTX 4090 上： - 4096 个 ANYmal (18-DoF) 环境：约 1.2 ms/step（含碰撞检测） - 等效 3.4M 次 RNEA/秒 - 相比 CPU 串行：约 200× 加速

9.6 符号化代码生成——另一种"并行"加速 ⭐⭐⭐¶

对固定拓扑的机器人（如 Franka Panda 永远是 7-DOF revolute），可以在编译期把 RNEA/ABA 的 for 循环完全展开，消除所有"乘以零"的运算：

工具链： - RobCoGen（ETH RSL）：从 URDF 生成特化的 C++ 代码，每个关节的 S 矩阵在编译期已知 - CppADCodeGen + Pinocchio：先用 CppAD 录制 RNEA 的计算图，再用 CodeGen 生成无循环的 C 代码 - SymPyBotics：Python 符号计算 + C 代码生成

性能对比（7-DOF Panda）：

方法	RNEA 耗时	相对通用实现
Pinocchio (通用)	1.0 μs	1.0×
RobCoGen (特化)	0.4 μs	2.5× 更快
CppADCodeGen	0.5 μs	2.0× 更快

特化代码消除了：运行时的 joint type 分发、零元素乘法、不需要的坐标分量。代价是：只适用于固定拓扑，重构模型需要重新生成代码。

反事实推理：如果你的 MPC 需要在 1 kHz 下跑 100 个 horizon 节点的 ABA+导数，每次需要约 100×(ABA + RNEA derivatives) ≈ 100×(5+17) = 2200 μs。使用通用 Pinocchio 可能刚好在 2.2 ms 临界线上。如果用 RobCoGen 特化代码，降到 ≈ 900 μs，给碰撞检测和通信留出充足余量。这就是为什么 ETH RSL 在实际四足上优先使用 RobCoGen。

9.7 练习题¶

（思考） D&C ABA 的 O(log n) 复杂度假设有 O(n) 个处理器。如果只有 p 个处理器（p < n），实际复杂度是什么？（提示：Brent's theorem: \(T_p \leq T_1/p + T_\infty\)）
（编程） 用 JAX 的 vmap 对 MuJoCo MJX 做批量正动力学：对 batch_size = [1, 16, 64, 256, 1024, 4096] 分别测量 step 耗时，画出 throughput (env/s) vs batch_size 曲线，找到 GPU 饱和点。
（分析） 为什么 D&C ABA 对常规 36-DoF 机器人不实用？从以下角度分析：(a) O(n) 常数对比 (b) 通信 overhead (c) 现有硬件的并行粒度。

10. Pinocchio 源码精读 ⭐⭐¶

10.1 架构概览¶

Pinocchio 的设计哲学：编译期多态 + 模板元编程 = 零虚函数开销。

核心抽象是 CRTP Visitor Pattern：每个算法（RNEA/CRBA/ABA）被实现为一个 JointUnaryVisitorBase 的 CRTP 派生类，在编译期静态分发到各种 joint type（revolute/prismatic/spherical/free/...）。这避免了虚函数调用的 vtable 查找开销——在 1000Hz 的控制循环中，每个虚函数调用多花 2-3 ns，累积到 36 个关节 × 3 个 pass = 108 次 × 3 ns = 324 ns，约占 ABA 总耗时的 10%。

// Pinocchio 的 CRTP visitor 模式（简化）
template<typename Derived>
struct JointUnaryVisitorBase {
    template<typename JointModel>
    static void algo(const JointModel& jmodel, JointData& jdata, ...) {
        Derived::algo(jmodel, jdata, ...);  // 编译期静态分发
    }
};

struct RneaForwardStep : JointUnaryVisitorBase<RneaForwardStep> {
    // 每种 joint type 都有特化版本
    template<typename JointModel>
    static void algo(const JointModel& jmodel, JointData& jdata, ...) {
        // RNEA 前向 pass 的一步
    }
};

10.2 rnea.hxx 精读¶

文件：include/pinocchio/algorithm/rnea.hxx（约 750+ 行）

RneaForwardStep::algo() 关键片段（对应 Pass 1）：

// jcalc: 计算关节变换和运动子空间
jmodel.calc(jdata.derived(), q.derived(), v.derived());

// ⁱXλ(i)：link placement × joint transform
data.liMi[i] = model.jointPlacements[i] * jdata.M();

// v_J = Sᵢ q̇ᵢ（关节产生的局部速度）
data.v[i] = jdata.v();

// vᵢ = ⁱXλ(i) · v_{λ} + v_J（速度递推）
if (parent > 0)
    data.v[i] += data.liMi[i].actInv(data.v[parent]);

// aᵢ = ⁱXλ(i) · a_{λ} + Sᵢq̈ᵢ + cᵢ（加速度递推）
data.a_gf[i]  = jdata.c() + (data.v[i] ^ jdata.v());   // cᵢ = vᵢ × v_J
data.a_gf[i] += jdata.S() * jmodel.jointVelocitySelector(a);
data.a_gf[i] += data.liMi[i].actInv(data.a_gf[parent]);

// fᵢ = Iᵢaᵢ + vᵢ×*Iᵢvᵢ（Newton-Euler 力）
data.f[i] = model.inertias[i] * data.a_gf[i]
           + model.inertias[i].vxiv(data.v[i]);

RneaBackwardStep::algo() 关键片段（对应 Pass 2）：

// τᵢ = Sᵢᵀ fᵢ（关节力矩）
jmodel.jointVelocitySelector(data.tau) = jdata.S().transpose() * data.f[i];

// f_{λ} += λXᵢ fᵢ（力的后向传播）
if (parent > 0)
    data.f[parent] += data.liMi[i].act(data.f[i]);

Pinocchio 变量 ↔ Featherstone 符号对照：

Pinocchio	Featherstone	含义
`data.liMi[i]`	\({}^iX_{\lambda(i)}\)	Plücker motion 变换
`data.v[i]`	\(v_i\)	link i 的 spatial velocity
`data.a_gf[i]`	\(a_i\)（含重力）	link i 的 spatial acceleration
`data.f[i]`	\(f_i\)	link i 的 net spatial force
`data.tau`	\(\tau\)	关节力矩向量
`jdata.v()`	\(S_i \dot{q}_i\)	关节产生的局部速度
`jdata.c()`	部分 \(c_i\)	bias acceleration 的 \(\dot{S}\dot{q}\) 部分
`data.liMi[i].actInv()`	\({}^iX_{\lambda(i)} \cdot\)	motion 前向变换
`data.liMi[i].act()`	\({}^{\lambda(i)}X_i^* \cdot\)	force 后向变换（共轭转置）

10.3 aba.hxx 精读¶

文件：include/pinocchio/algorithm/aba.hxx（约 900+ 行）

ABA 有三个 visitor：AbaForwardStep1（Pass 1）、AbaBackwardStep（Pass 2）、AbaForwardStep2（Pass 3）。

AbaBackwardStep::algo() 是最复杂的部分（Schur 消元）：

// U = Iᴬ · S（关节体惯性在关节方向上的投影）
jdata.U().noalias() = data.Yaba[i] * jdata.S();

// D = Sᵀ U（关节有效惯量）
jdata.StU().noalias() = jdata.S().transpose() * jdata.U();

// 加 armature 提升条件数
jdata.StU().diagonal() += jmodel.jointVelocitySelector(model.armature);

// D⁻¹
internal::PerformStYSInversion<Scalar>::run(jdata.StU(), jdata.Dinv());

// u = τ − Sᵀ p（残余力矩）
jmodel.jointVelocitySelector(data.u) -= jdata.S().transpose() * data.f[i];

if (parent > 0) {
    // Schur complement: Iᴬ_reduced = Iᴬ − U D⁻¹ Uᵀ
    Inertia::Matrix6 Ia = data.Yaba[i];
    Ia.noalias() -= jdata.UDinv() * jdata.U().transpose();

    // 传给父节点
    data.Yaba[parent] += data.liMi[i].act(Ia);
    data.f[parent]    += data.liMi[i].act(pa);    // pa = p + Ia·c + U·D⁻¹·u
}

关键实现细节：

data.Yaba[i] 是完整的 6×6 一般矩阵（不是 Inertia 类的 10 参数压缩），因为 Schur complement 后不再是纯 spatial inertia 形式
model.armature 被加到 D 的对角线上——这是防止 \(D_i\) 过小导致数值不稳定的工程手段
jdata.UDinv() = \(U_i D_i^{-1}\) 被缓存，避免 Pass 3 中重复计算

10.4 crba.hxx 精读¶

文件：include/pinocchio/algorithm/crba.hxx（约 400+ 行）

CrbaBackwardStep::algo() 关键片段：

// F = Iᶜ · S（复合惯性在关节方向上的投影）
ColsBlock jF = jmodel.jointCols(data.Fcrb[i]);
jF = data.Ycrb[i] * jdata.S();

// M[i,i] = Sᵀ Iᶜ S（对角块）
data.M.block(jmodel.idx_v(), jmodel.idx_v(), jmodel.nv(), jmodel.nv())
    = jdata.S().transpose() * jF;

// 复合惯性向父节点传递：Iᶜ_{λ} += λXᵢ · Iᶜᵢ
if (parent > 0) {
    data.Ycrb[parent] += data.liMi[i].act(data.Ycrb[i]);
    // 同时传播 F 矩阵，用于填充祖先链上的交叉耦合块
}

关键变量： - data.Ycrb[i]：\(I_i^c\)（6×6 spatial inertia，使用 Inertia 类的压缩表示） - data.Fcrb：Plücker 变换后的 F 矩阵列缓冲，用于祖先链填充 - data.M：最终质量矩阵（仅上三角填充）

10.5 RBDL / Drake / MuJoCo 对照¶

特征	Pinocchio	RBDL	Drake	MuJoCo
语言	C++17（模板元）	C++11	C++17	C (性能核心)
多态	CRTP 静态分发	直接 for 循环	模板+AutoDiff	无多态（固定 joint type）
性能	★★★★★	★★★☆	★★★★	★★★★★
AutoDiff	CppAD/CasADi 后端	无	内建三标量	有限差分
接口	`rnea(model,data,q,v,a)`	`InverseDynamics(model,Q,QDot,QDDot,Tau)`	`plant.CalcInverseDynamics(context,...)`	`mj_rne(m,d,flg_acc,result)`

11. 典型例题 ⭐⭐¶

11.1 例题 1：7-DOF Franka Panda 的 RNEA 验证¶

目标：用 Pinocchio 验证 RNEA 的各种恒等式。

import numpy as np
import pinocchio as pin

# 加载 Franka Panda 模型
model = pin.buildSampleModelManipulator()  # 6-DOF 示例
data = model.createData()

# 随机生成状态
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)
a = np.random.randn(model.nv)

# 验证恒等式 1：rnea(q,v,0) == nonLinearEffects(q,v)
tau1 = pin.rnea(model, data, q, v, np.zeros(model.nv))
nle = pin.nonLinearEffects(model, data, q, v)
print(f"恒等式1 误差: {np.max(np.abs(tau1 - nle)):.2e}")  # 应 < 1e-14

# 验证恒等式 2：crba(q) @ a + nle(q,v) == rnea(q,v,a)
pin.crba(model, data, q)
M = data.M.copy()
M = M + M.T - np.diag(M.diagonal())  # 对称化！关键步骤
tau2 = M @ a + nle
tau3 = pin.rnea(model, data, q, v, a)
print(f"恒等式2 误差: {np.max(np.abs(tau2 - tau3)):.2e}")  # 应 < 1e-12

# 验证恒等式 3：aba(q,v,tau) == solve(M, tau - nle)
tau_input = np.random.randn(model.nv)
ddq_aba = pin.aba(model, data, q, v, tau_input)
ddq_lagrangian = np.linalg.solve(M, tau_input - nle)
print(f"恒等式3 误差: {np.max(np.abs(ddq_aba - ddq_lagrangian)):.2e}")  # 应 < 1e-10

11.2 例题 2：浮基人形机器人的 ABA¶

场景：36-DOF 人形机器人自由落体（τ = 0，无接触）

import pinocchio as pin
from pinocchio.robot_wrapper import RobotWrapper

# 加载 TALOS 模型（浮基人形）
robot = RobotWrapper.BuildFromURDF("path/to/talos.urdf", "path/to/meshes")
model = robot.model
data = robot.data

# 初始状态：站立构型，零速度
q0 = robot.q0  # 默认站立构型（含浮基 7 维：pos + quaternion）
v0 = np.zeros(model.nv)  # 零速度

# 零力矩（自由落体）
tau = np.zeros(model.nv)

# ABA 计算加速度
ddq = pin.aba(model, data, q0, v0, tau)

# 验证：浮基的加速度应该 ≈ [0,0,0, 0,0,-9.81]
# （前 3 维角加速度为零，后 3 维线加速度为重力加速度）
print(f"Base angular acceleration: {ddq[:3]}")   # 应接近 [0,0,0]
print(f"Base linear acceleration: {ddq[3:6]}")   # 应接近 [0,0,-9.81]

# 注意：关节加速度不为零！因为没有力矩抵消重力
print(f"Joint accelerations (first 6): {ddq[6:12]}")  # 非零，取决于构型

关键理解：浮基机器人的 ABA 返回的 \(\ddot{q}\) 前 6 维是基座的广义加速度（在 body-fixed frame 中），后 n-6 维是关节加速度。自由落体时基座获得 \(-g\) 的加速度（坐标系约定），但关节也会加速——因为没有力矩抵消重力对各 link 的影响。

11.3 例题 3：RNEA 手算与 Pinocchio 验证（渐隐示例法）¶

Phase 1：完整 Worked Example

对 3-DOF 平面臂（三个 revolute 关节，link 长度 L=1m，质量 m=1kg，质心在中点），在 \(q = [0, \pi/4, -\pi/4]^T\), \(\dot{q} = [1, 0, 0]^T\), \(\ddot{q} = [0, 0, 0]^T\) 下执行 RNEA。

因为 \(\ddot{q} = 0\)，RNEA 的输出就是 \(C(q,\dot{q})\dot{q} + g(q)\)。

Pass 1（前向）：

关节 1：\(q = [0, 0]\)（平面简化为 4D spatial vector [ω_z, v_x, v_y, 0]） - \(S_1 = [1, 0, 0, 0]^T\)（绕 z 轴） - \(v_1 = S_1 \dot{q}_1 = [1, 0, 0, 0]^T\) - \(c_1 = v_1 \times (S_1 \dot{q}_1) = 0\)（同方向叉积为零） - \(a_1 = {}^1X_0 \cdot a_0 + 0 + 0 = {}^1X_0 \cdot (-g_0)\)

关节 2：\(q_2 = \pi/4\) - \(v_2 = {}^2X_1 v_1 + S_2 \cdot 0 = {}^2X_1 v_1\)（因为 \(\dot{q}_2 = 0\)） - 注意：虽然 \(\dot{q}_2 = 0\)，但 \(v_2 \neq 0\)（因为 link 1 在转动，link 2 跟着动） - \(c_2 = v_2 \times (S_2 \cdot 0) = 0\)

关节 3：类似

Pass 2（后向）——只有重力产生力矩（因为 \(\dot{q}_2 = \dot{q}_3 = 0\)，无 Coriolis）

Phase 2：填空版（读者自行完成标记为 _____ 的部分）

对同一个 3-DOF 臂，改为 \(\dot{q} = [1, 2, -1]^T\)（有 Coriolis 效应）：

\(v_2 = {}^2X_1 v_1 + S_2 \dot{q}_2 = \_\_\_\_\_ + [1, 0, 0, 0]^T \cdot 2\)
\(c_2 = v_2 \times (S_2 \dot{q}_2) = \_\_\_\_\_\)（提示：这里非零！）
\(f_2 = I_2 a_2 + v_2 \times^* I_2 v_2 = \_\_\_\_\_\)
\(\tau_2 = S_2^T f_2 = \_\_\_\_\_\)

Phase 3：只给框架

对 7-DOF Panda 机械臂，用 Pinocchio 实现以下验证框架：

# 框架：验证 RNEA 的特殊调用恒等式
import pinocchio as pin
import numpy as np

model = pin.buildSampleModelManipulator()
data = model.createData()

N_samples = 1000
max_err_nle, max_err_grav = 0.0, 0.0

for _ in range(N_samples):
    q = pin.randomConfiguration(model)
    v = np.random.randn(model.nv)
    a = np.random.randn(model.nv)

    # 恒等式 1: rnea(q, v, 0) == nonLinearEffects(q, v)
    tau_rnea_v0 = pin.rnea(model, data, q, v, np.zeros(model.nv))
    nle = pin.nonLinearEffects(model, data, q, v)
    max_err_nle = max(max_err_nle, np.max(np.abs(tau_rnea_v0 - nle)))

    # 恒等式 2: rnea(q, 0, 0) == computeGeneralizedGravity(q)
    tau_rnea_00 = pin.rnea(model, data, q, np.zeros(model.nv), np.zeros(model.nv))
    g_vec = pin.computeGeneralizedGravity(model, data, q)
    max_err_grav = max(max_err_grav, np.max(np.abs(tau_rnea_00 - g_vec)))

print(f"Max |rnea(q,v,0) - nle|:  {max_err_nle:.2e}")
print(f"Max |rnea(q,0,0) - g(q)|: {max_err_grav:.2e}")
assert max_err_nle < 1e-10 and max_err_grav < 1e-10

11.4 例题 4：性能 Benchmark¶

import time
import pinocchio as pin
import numpy as np

# 对三种规模的机器人做性能测试
robots = {
    "Panda (7-DoF)": pin.buildSampleModelManipulator(),
    # "ANYmal (18-DoF)": load_anymal(),
    # "TALOS (38-DoF)": load_talos(),
}

for name, model in robots.items():
    data = model.createData()
    N_trials = 10000

    # RNEA 性能
    t0 = time.perf_counter_ns()
    for _ in range(N_trials):
        q = pin.randomConfiguration(model)
        v = np.random.randn(model.nv)
        a = np.random.randn(model.nv)
        pin.rnea(model, data, q, v, a)
    t_rnea = (time.perf_counter_ns() - t0) / N_trials / 1000  # μs

    # CRBA 性能
    t0 = time.perf_counter_ns()
    for _ in range(N_trials):
        q = pin.randomConfiguration(model)
        pin.crba(model, data, q)
    t_crba = (time.perf_counter_ns() - t0) / N_trials / 1000

    # ABA 性能
    t0 = time.perf_counter_ns()
    for _ in range(N_trials):
        q = pin.randomConfiguration(model)
        v = np.random.randn(model.nv)
        tau = np.random.randn(model.nv)
        pin.aba(model, data, q, v, tau)
    t_aba = (time.perf_counter_ns() - t0) / N_trials / 1000

    print(f"{name}: RNEA={t_rnea:.1f}μs, CRBA={t_crba:.1f}μs, ABA={t_aba:.1f}μs")

12. 数值稳定性与实现细节 ⭐⭐¶

12.1 浮点精度问题分类¶

递推算法的数值误差来源有四类：

误差来源	影响	严重程度	缓解方法
累积舍入误差	长链递推中每步的舍入误差累积	中等	使用 double（不是 float）
Iᴬ 正定性丢失	Schur complement 后微小负特征值	严重	armature + 对称化
小角度奇异	\(\sin(\theta)/\theta\) 在 \(\theta \to 0\) 时	低	Taylor 展开到足够阶
大质量比	\(m_{\max}/m_{\min} > 10^6\) 时条件数恶化	严重	重新参数化/正则化

12.2 Armature 的物理含义与工程作用¶

model.armature[i] 表示关节 i 的电机转子转动惯量反射到关节输出端的值：

\[I_{\text{armature}} = I_{\text{rotor}} \times r^2\]

其中 \(r\) 是传动比（减速比）。例如减速比 100:1 的电机，转子惯量 \(10^{-5}\) kg·m² 反射后变成 \(10^{-5} \times 100^2 = 0.1\) kg·m²。

在 ABA 中的作用：armature 被加到 \(D_i = S_i^T I_i^A S_i + I_{\text{armature},i}\)。这等效于在关节方向增加了一个"虚拟惯量"，使 \(D_i\) 远离零，改善了 \(D_i^{-1}\) 的条件数。

如果不加 armature 会怎样？ 对细长轻质 link（如末端夹爪），\(I_i^A\) 在 S 方向的投影可能非常小（因为 link 本身几乎没有绕关节轴的转动惯量）。此时 \(D_i \approx 0\)，\(D_i^{-1} \to \infty\)，ABA 输出爆炸。加上 armature 后 \(D_i \geq I_{\text{armature}} > 0\)，保证了数值稳定。

12.3 外力坐标系——最常见的工程错误¶

不同库对外力的坐标系约定不同，混用必然得到错误结果：

库	外力坐标系	API
Pinocchio	Link-local（body frame）	`fext[i]` 是 `pinocchio::Force` 类型
Drake	World（global frame）	`SpatialForce` 默认世界系
MuJoCo	World	`xfrc_applied` 在世界系

坐标系转换：

# 从世界系力转换到 Pinocchio 的 link-local 系
Force_world = pin.Force(np.array([0, 0, 0, 10, 0, 0]))  # 世界系 10N 沿 x

# 转换到 link i 的局部坐标系
oMi = data.oMi[i]  # link i 相对世界的位姿
Force_local = oMi.actInv(Force_world)  # 变换到 body frame

# 现在可以安全地传给 Pinocchio 的 rnea
fext = [pin.Force.Zero()] * model.njoints
fext[i] = Force_local
tau = pin.rnea(model, data, q, v, a, fext)

12.4 浮基机器人的特殊注意事项¶

浮基机器人（四足、人形）的 RNEA 返回值有特殊含义：

tau = pin.rnea(model, data, q, v, a)  # 形状为 (nv,)
# tau[:6]  = 基座需要的 "虚拟外力"（不是驱动力矩！基座不可驱动）
# tau[6:]  = 关节驱动力矩

# 物理含义：tau[:6] 就是"若要实现当前运动，需要有人给基座施加多少 wrench"
# 在自由运动中 tau[:6] 必须为零（无外力支撑基座）
# 在接触控制中 tau[:6] = 总接触力的合力

常见错误：把 tau[:6] 当作"基座电机力矩"去执行——基座没有电机！这 6 维代表的是约束方程 \(S^T \tau + J_c^T \lambda\) 中的 \(J_c^T \lambda\) 部分。

12.5 数值验证方法清单¶

验证方法	检测内容	容差（double）
`rnea(q,v,a) == crba(q)@a + nle(q,v)`	RNEA/CRBA 一致性	1e-12
`aba(q,v,tau) == solve(M, tau-nle)`	ABA/Lagrangian 一致性	1e-10
`M == M.T`	M 对称性	1e-14
`eigvals(M) > 0`	M 正定性	所有特征值 > 1e-10
`rnea(q,0,0) == computeGravity(q)`	重力项一致性	1e-14
`sum(tau[:6]) for free-fall == 0`	浮基自由落体一致性	1e-10

本章小结¶

算法	功能	输入 → 输出	复杂度	核心思想	Pinocchio API
RNEA	逆动力学	(q,q̇,q̈) → τ	O(n)	Newton-Euler 前向+后向递推	`rnea()`
CRBA	质量矩阵	q → M(q)	O(n²)	复合刚体惯性后向累加	`crba()`
ABA	正动力学	(q,q̇,τ) → q̈	O(n)	关节体惯性 Schur 消元	`aba()`
Minv	逆质量矩阵	q → M⁻¹	O(n²)	ABA 结构生成 M⁻¹	`computeMinverse()`
稀疏 LDLᵀ	M 的因子化	M → L,D	O(n·depth)	树状稀疏性利用	`cholesky::decompose()`
D&C ABA	并行正动力学	(q,q̇,τ) → q̈	O(log n)	分治 + 并行合并	无标准实现

三大算法的本质关系：

\[\underbrace{\text{RNEA}}_{\text{计算 } M\ddot{q}+h} \xrightarrow{\text{分离 } \ddot{q} \text{ 系数}} \underbrace{\text{CRBA}}_{\text{构造 } M} \xrightarrow{\text{求解 } M^{-1}} \underbrace{\text{ABA}}_{\text{直接得 } \ddot{q}}\]

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

项目：从零构建动力学计算库

本章新增： - rnea(model, q, v, a) → 逆动力学函数 - crba(model, q) → 质量矩阵计算 - aba(model, q, v, tau) → 正动力学函数 - 恒等式验证脚本（3 组恒等式 × 1000 随机测试） - 性能 benchmark 脚本

前置（已有）模块： - Ch1：关节树数据结构、parent array、jcalc - Ch2：正向运动学、Plücker 变换

后续（待加）模块： - Ch4（专题 4-5）：解析微分 computeRNEADerivatives - Ch5（专题 4-6）：约束动力学 constraintDynamics

延伸阅读¶

#	资源	难度	评价
1	Featherstone, Rigid Body Dynamics Algorithms (2008) Ch.5-7	⭐⭐	本章绝对主教材
2	Luh, Walker, Paul, "On-Line Computational Scheme" (ASME 1980)	⭐⭐	RNEA 原始论文
3	Featherstone, "The Calculation of Robot Dynamics Using ABIs" (IJRR 1983)	⭐⭐⭐	ABA 原始论文
4	Carpentier et al., "The Pinocchio C++ Library" (IEEE SII 2019)	⭐⭐	现代实现参考
5	Jain, Robot and Multibody Dynamics (2011) Ch.5-7	⭐⭐⭐	SOA 统一框架
6	Featherstone, "Efficient Factorization of JSIM" (IJRR 2005)	⭐⭐⭐	稀疏 Cholesky
7	Featherstone, "D&C ABA" (IJRR 1999)	⭐⭐⭐⭐	并行 O(log N)
8	Carpentier & Mansard, "Analytical Derivatives" (RSS 2018)	⭐⭐⭐	解析微分（衔接下章）
9	Sathya et al., "Proximal Sparse Constrained Dynamics" (RSS 2021)	⭐⭐⭐⭐	稀疏约束动力学
10	Lynch & Park, Modern Robotics Ch.8 (中文版 2020)	⭐	Newton-Euler 入门
11	Stéphane Caron, "Forward Dynamics" (博客)	⭐⭐	Pinocchio 代码对比
12	Neuman et al., "Benchmarking Robot Dynamics" (IROS 2019)	⭐⭐	三大库性能对比

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关小节
RNEA 对 q̇≠0 结果错误	漏掉 bias acceleration \(c_i\)	1. 检查 \(v_i \times (S_i \dot{q}_i)\) 项 2. 对比 q̇=0 结果（应正确） 3. 打印每个 link 的 \(c_i\)	§3.4
`data.M @ a` 结果错误	CRBA 只填充上三角	1. 打印 `data.M` 的下三角是否为零 2. 使用 `selfadjointView<Upper>()` 3. 显式对称化	§4.5
ABA 输出 NaN	\(D_i\) 接近零导致 \(D_i^{-1}\) 爆炸	1. 检查 `model.armature` 是否为零 2. 加 armature > 1e-6 3. 检查惯性参数合理性	§5.8
RNEA 与 CRBA+ABA 结果不一致	坐标系约定不匹配	1. 检查 fext 是否在 link-local frame 2. 检查 gravity 方向 3. 确认浮基 convention	§3.8
正动力学仿真发散	积分方法不匹配浮基	1. 对浮基不能用简单欧拉法 2. 使用 `pin.integrate(model, q, v*dt)` 3. 检查 quaternion 归一化	§10

自测题目（按档位标注）¶

（档位 3 核心） 手写一个 3-joint 串联平面机械臂的 RNEA 前向+后向全过程。列出每步的 \(v_i, a_i, f_i\) 以及最终 \(\tau\)。（约 4 小时）
（档位 3） 用归纳法证明 RNEA 的 O(n) 复杂度：每个关节的计算量是常数 k，总运算 = k·n。给出 k 的上界。
（档位 3） 证明 RNEA(q, q̇, q̈) = M(q)q̈ + C(q,q̇)q̇ + g(q)。
（档位 3 编程） 安装 Pinocchio，加载 Franka Panda，验证恒等式（容差 1e-10）：
rnea(q, v, 0) == nonLinearEffects(q, v)
crba(q) @ a + nonLinearEffects(q, v) == rnea(q, v, a)
aba(q, v, tau) == np.linalg.solve(crba(q), tau - nonLinearEffects(q, v))
（档位 3 编程） 对 Panda (7-DoF)、ANYmal (18-DoF)、TALOS (38-DoF) 各跑 10000 次随机输入，统计 RNEA/CRBA/ABA 耗时，验证 O(n)/O(n²) 的经验斜率。
（档位 3+） 手推 2-joint 平面臂的 articulated-body inertia \(I^A\)。验证 \(S_1^T I_1^A S_1\) 与 M(q) 的关系。
（档位 4） 用 George-Liu 稀疏 Cholesky 理论证明：JSIM M(q) 的稀疏 Cholesky 因子 L 的非零结构恰好是"每个节点与其所有祖先"。
（档位 3 编程） 实现一个简化版 RNEA（仅支持 revolute 关节），用 Python/NumPy 写不超过 100 行代码，并与 Pinocchio 的输出对比验证。
（档位 3+） 对 Franka Panda (7-DoF)，分别使用三种方法计算正动力学：
方法 A：aba(q, v, tau)
方法 B：crba(q) → Eigen LDLT.solve(tau - nle)
方法 C：crba(q) → Pinocchio cholesky::decompose + solve 统计 10000 次随机输入的耗时均值和 95 百分位值。验证在 7-DoF 上方法 B 可能最快。
（档位 4 研究级） 阅读 Featherstone IJRR 1999 的 D&C ABA 论文。对一个 32-link 链式机器人，画出 D&C 递归树，标注每层的工作量和通信量。计算在 16 个处理器上的理论加速比（使用 Brent's theorem）。

跨章综合题¶

（需要综合空间向量代数 + Lagrange力学 + 本章递推算法）

设计一个完整的验证流程：

用空间向量代数的工具，手动构造一个 3-DOF 平面臂的 Plücker 变换 \({}^iX_{\lambda(i)}\)
用 Lagrange 方法，从动能 \(T = \frac{1}{2}\dot{q}^T M(q) \dot{q}\) 出发推导 M(q) 的解析表达式
用本章的 CRBA 伪代码，从 spatial inertia 出发递推计算 M(q)
验证两种方法给出相同的 M(q)
用 RNEA 计算 \(\tau = M\ddot{q} + C\dot{q} + g\)，与直接从 Lagrangian 推导的 EoM 对比

这道题将三章的知识串联起来，验证"Lagrangian → Newton-Euler → 递推"三条路径的一致性。

附录 A：性能基准数据（Pinocchio SII 2019 + IROS 2019）¶

机器人	DoF (+floating)	RNEA (μs)	ABA (μs)	CRBA (μs)	最快库
Franka/KUKA iiwa	7	~1.0	~1.0	~1.0	RobCoGen (ABA 1.1), Pinocchio
HyQ / quadruped	12+6 = 18	~1.5	~2.4	~1.8	Pinocchio (RNEA float-base)
HRP-2 / Talos	32+6 ≈ 38	~3.0	~3.5	~3.5	Pinocchio
Atlas (DRC 28-DoF)	30+6 = 36	3.5	5.9	~4.5	Pinocchio

测试平台：Intel Core i7 @ 2.4 GHz，100k 次随机输入均值。

附录 B：Pinocchio 完整 API 表¶

数学任务	API	头文件	返回字段
τ = M q̈ + C q̇ + g (RNEA)	`rnea(model, data, q, v, a)`	`algorithm/rnea.hpp`	`data.tau`
C q̇ + g	`nonLinearEffects(model, data, q, v)`	同上	`data.nle`
g(q)	`computeGeneralizedGravity(model, data, q)`	同上	`data.g`
M(q) 上三角	`crba(model, data, q)`	`algorithm/crba.hpp`	`data.M`
q̈ = M⁻¹(τ − h)	`aba(model, data, q, v, tau)`	`algorithm/aba.hpp`	`data.ddq`
M⁻¹(q)	`computeMinverse(model, data, q)`	同上	`data.Minv`
全部常用量	`computeAllTerms(model, data, q, v)`	`compute-all-terms.hpp`	多字段
M 的 LDLᵀ 分解	`cholesky::decompose(model, data)`	`algorithm/cholesky.hpp`	`data.U/D/Dinv`
C(q,q̇) 矩阵	`computeCoriolisMatrix(model, data, q, v)`	`algorithm/rnea.hpp`	`data.C`

附录 C：RBDL / Drake / MuJoCo API 映射¶

数学任务	RBDL	Drake	MuJoCo
RNEA	`InverseDynamics`	`plant.CalcInverseDynamics`	`mj_rne`
CRBA	`CompositeRigidBodyAlgorithm`	`plant.CalcMassMatrix`	`mj_crb`
ABA	`ForwardDynamics`	内部 `CalcForwardDynamics`	`mj_forward`
M 分解	内嵌于 `ForwardDynamicsLagrangian`	内部	`mj_factorM`
解 Mx=y	无独立 API	内部	`mj_solveM`
C·q̇ + g	无独立 API	`CalcBiasTerm`	`d->qfrc_bias`

附录 D：常见陷阱完整清单¶

bias acceleration 漏写：vᵢ × (Sᵢq̇ᵢ) 对 revolute joint 不为零
CRBA 只返回上三角：下三角是 0，直接 M@a 会错
ABA 的 Iᴬ 正定性：加 armature 提升 Dᵢ 条件数
spatial vs 经典加速度：base 初始化 a₀ = −g₀ 是 spatial 约定
编号约定：Pinocchio joint 0 = universe，真关节从 1 开始
浮基 RNEA 返回值：前 6 维是 base wrench（不是驱动力）
computeAllTerms ≠ crba + rnea：不输出独立 C 矩阵
MuJoCo qfrc_bias：= C·q̇+g，不是纯 Coriolis
外力坐标系：Pinocchio 要 link-local，Drake 要 world
aba 返回 ddq：不做积分，浮基 q 的积分需要 Lie group 方法

附录 E：核心教材与视频资源¶

教材： - Featherstone, Rigid Body Dynamics Algorithms (Springer 2008) — 本章主教材 - Jain, Robot and Multibody Dynamics (Springer 2011) — SOA 路径 - Lynch & Park, Modern Robotics Ch.8 (中文版 2020) — 入门

视频： - MIT 6.832 Underactuated Robotics (Russ Tedrake) — Ch.23 Multi-Body Dynamics - Modern Robotics Ch.8 YouTube (Northwestern) — Newton-Euler 视角 - ETH RSL Robot Dynamics (Marco Hutter) — 浮基视角

博客： - Stéphane Caron "Spatial vector algebra cheat sheet" — 公式速查 - Stéphane Caron "Forward dynamics" — Pinocchio 代码对比 - 知乎《Featherstone 刚体动力学算法》系列 — 中文读书笔记

代码仓库： - stack-of-tasks/pinocchio — 本章源码精读对象 - google-deepmind/mujoco — MuJoCo 动力学实现 - RobotLocomotion/drake — Drake multibody - rbdl/rbdl — RBDL (Martin Felis) - royfeatherstone.org/spatial/v2/ — MATLAB 教学代码

附录 F：学习时间预算¶

档位 3（核心路径，35-45 小时）¶

周次	任务	时间	产出
第 1 周	§2 拓扑 + §3.1-3.3 RNEA 前向/后向	8h	能手推 RNEA
第 2 周	§3.4-3.7 RNEA 证明 + 特殊调用 + 源码精读	8h	rnea.hxx 看懂
第 3 周	§4 CRBA 完整 + 源码精读	6h	crba.hxx 看懂
第 4-5 周	§5 ABA 完整（最难）+ 源码精读	14h	aba.hxx 三个 visitor 通读
第 5-6 周	§6-8 Minv + 统一视角 + 稀疏	6h	理解稀疏 vs 密集
第 6 周	§11 例题 + Benchmark	4h	性能测试完成

档位 4（进阶扩展，额外 20-30 小时）¶

周次	任务	时间
第 7 周	§9 D&C ABA + GPU 批量	6h
第 8 周	闭链约束 (Featherstone Ch.8)	6h
第 9 周	Elimination tree + 因子图类比	6h
第 10 周	符号化 RNEA/ABA + 代码生成	6h

结语：从"微分方程"到"树上的消息传递"¶

学完本章，你对"机器人动力学"的认识会从"一堆微分方程"升级为"一棵关节树上的消息传递算法"——RNEA 是前向+后向两次消息传递，CRBA 是后向累加子树惯量，ABA 是带 Schur 消元的三次传递。

更深层地，你会看见它们都是同一个 elimination tree 上的稀疏矩阵分解——这个视角在专题 4-5（解析微分）、专题 4-6（约束动力学）、第五批 SLAM（Bayes Tree）中都会反复出现。

最关键的收获：你第一次把"数学"和"微秒级实时代码"真正对上了。Featherstone Table 7.1 的第 14 行和 Pinocchio aba.hxx 的第 180 行是同一个东西——前者在纸上，后者在 Atlas 上 3 μs 执行。这种"数学与实现的双向确认"是成为机器人博士和资深工程师的分水岭。

与后续专题的桥梁¶

向后衔接：从动力学到控制¶

学完本章的三大算法后，你拥有了现代机器人控制的"动力学引擎"。以下是后续章节如何复用本章成果的具体映射：

→ 专题 4-5《解析微分与自动微分》

Pinocchio computeRNEADerivatives/computeABADerivatives（Carpentier-Mansard RSS 2018）在 RNEA/ABA 的递推结构之上直接生成 ∂τ/∂q, ∂τ/∂q̇, ∂q̈/∂τ 等偏导，复杂度保持 O(n²)。

核心思想：既然 RNEA 的每一步都是可微的（矩阵-向量乘、叉积都有解析导数），那么对整个递推链应用链式法则就能得到端到端的导数。这比数值差分（有限差分法）快 5-10 倍，且精度更高（无截断误差）。

本章提供的基础：递推结构的完整理解——知道"哪些中间变量依赖哪些输入"是写解析导数的前提。

→ 专题 4-6《约束动力学与接触》

接触约束 λ 如何嵌入 RNEA/ABA：

\[M(q)\ddot{q} + h = S^T \tau + J_c^T \lambda\]

Featherstone Ch.8 的方法：在 ABA 的递推中额外处理 loop constraint（闭链约束），得到 "constrained ABA"。Sathya-Carpentier RSS 2021 的 ProxSuite 方法：把约束转化为近端算子（proximal operator），在稀疏 LDLᵀ 的框架内迭代求解。

本章提供的基础：稀疏 LDLᵀ 的实现和理解——约束动力学就是在此基础上加入 KKT 系统的求解。

→ 第三批《MPC 与最优控制》

Crocoddyl 的 FDDP 算法每次迭代的计算链：

for t = 0 .. N-1:
    q̈_t = ABA(q_t, v_t, τ_t)                    # 正动力学（积分）
    ∂q̈/∂q, ∂q̈/∂v, ∂q̈/∂τ = ABA_derivatives(...)  # 导数（线性化）
    τ_ID = RNEA(q_t, v_t, q̈_des)                 # 逆动力学（初始猜测）

一个典型的 100 节点 MPC 单次迭代 = 100×ABA + 100×ABA_derivatives + 100×RNEA ≈ 100×(5+17+3) = 2500 μs on Atlas。本章的算法直接决定了这个数字能否满足实时约束。

→ 第五批《SLAM 与状态估计》

回顾本章 §8.4 的 elimination tree 讨论。Bayes tree（iSAM2 的核心数据结构）与 joint tree（Featherstone sparse Cholesky 的依据）共享同一个数学结构：

SLAM：\(\Lambda x = b\)（信息矩阵 × 状态 = 信息向量），\(\Lambda\) 的稀疏模式由因子图拓扑决定
动力学：\(M \ddot{q} = \tau - h\)（质量矩阵 × 加速度 = 广义力），\(M\) 的稀疏模式由关节树拓扑决定

两者的 elimination tree 决定了 Cholesky 因子的 fill-in 模式——理解其一就理解了其二。这是机器人学中"动力学"和"估计"看似不同领域却共享深层数学结构的绝佳例证。

向前回顾：本章如何利用前置知识¶

专题 4-1 的 Plücker 变换：本章的 \({}^iX_{\lambda(i)}\) 就是 4-1 中定义的 6×6 运动变换矩阵。actInv 对应 motion transform，act 对应 force transform（共轭转置）。如果 4-1 没学好，本章的每一个公式都看不懂。
专题 4-2 的 Lagrangian 方程：本章证明 RNEA 等价于 \(M\ddot{q}+C\dot{q}+g = \tau\)，这个等价性的右侧就是 4-2 中从 Lagrangian 推导出的标准形式。理解了 4-2 中 M/C/g 的定义，才能理解 RNEA 正确性证明中"系数分离"的逻辑。
第一批的 Cholesky 分解：本章 §8 的稀疏 LDLᵀ 是第一批中通用 Cholesky 分解的特化——特化到"非零结构由树拓扑决定"的情况。如果不理解 Cholesky 的基本原理（\(A = LL^T\), 前向/后向替换），就无法理解 ABA 与 LDLᵀ 的等价性。

练习¶

练习 1：RNEA 手算（基础）¶

对 2-DOF 平面机械臂（连杆 1 长 \(l_1 = 0.5\) m, 质量 \(m_1 = 2\) kg; 连杆 2 长 \(l_2 = 0.3\) m, 质量 \(m_2 = 1\) kg; 质心在连杆中点），在构型 \(q = (\pi/4, \pi/6)\), \(\dot q = (1, 2)\) rad/s, \(\ddot q = (0, 0)\)（无加速度）下：

(a) 前向递推（关节 1 → 2）： - 计算连杆变换 \({}^iX_{\lambda(i)}\) （用 Plücker 变换矩阵） - 递推计算空间速度 \(v_i = {}^iX_{\lambda(i)} v_{\lambda(i)} + S_i \dot q_i\) - 递推计算空间加速度 \(a_i = {}^iX_{\lambda(i)} a_{\lambda(i)} + S_i \ddot q_i + v_i \times S_i \dot q_i\)（注意初始条件 \(a_0 = (0; 0; 0; 0; 9.81; 0)^T\) 用于包含重力）

(b) 反向递推（关节 2 → 1）： - 计算每个连杆上的力 \(f_i = \mathcal{I}_i a_i + v_i \times^* (\mathcal{I}_i v_i)\) - 累加子连杆力 \(f_i \leftarrow f_i + {}^iX_{\lambda(i+1)}^{-*} f_{i+1}\) - 投影到关节轴 \(\tau_i = S_i^T f_i\)

(c) 用 Pinocchio 验证：pin.rnea(model, data, q, dq, ddq) 的输出应与手算一致。

练习 2：CRBA vs 数值微分验证（编程）¶

(a) 加载一个 7-DOF 机械臂模型（如 KUKA iiwa），用 pin.crba(model, data, q) 计算 \(M(q)\)。

(b) 用 RNEA 的"列提取"方法构造 \(M\)：对 \(j = 1, \ldots, 7\)，令 \(\ddot q = e_j\)（第 j 个单位向量），\(\dot q = 0\)，\(g = 0\)，调用 pin.rnea(model, data, q, 0, e_j)，得到 \(M\) 的第 \(j\) 列。

(c) 验证两种方法的结果一致（\(\|M_{\text{CRBA}} - M_{\text{RNEA}}\|_F < \epsilon\)）。

(d) 性能对比：用 %timeit 测量 100,000 次调用的总时间。对于 7-DOF 臂，CRBA（\(O(N^2)\)）应该比 N 次 RNEA（\(O(N) \times N = O(N^2)\)）更快。为什么？（提示：CRBA 避免了重复的前向递推。）

练习 3：ABA 正向动力学与积分（中等）¶

(a) 用 ABA 算法 pin.aba(model, data, q, dq, tau) 计算给定 \(\tau\) 下的 \(\ddot q\)。

(b) 验证 ABA 的正确性：\(\ddot q_{\text{ABA}}\) 应满足 \(M \ddot q_{\text{ABA}} = \tau - C\dot q - g\)。用 CRBA 和 RNEA 分别计算 \(M\) 和 \((C\dot q + g)\)，检查 \(M^{-1}(\tau - C\dot q - g)\) 与 ABA 输出是否一致。

(c) 实现简单的 Euler 积分器：\(q_{k+1} = q_k + \dot q_k \cdot dt\), \(\dot q_{k+1} = \dot q_k + \ddot q_k \cdot dt\)，仿真自由落体（\(\tau = 0\)）1 秒。

(d) 观察能量守恒：计算每步的总能量 \(E = \frac{1}{2}\dot q^T M \dot q + V(q)\)。Euler 积分是否保能量？与 Runge-Kutta 4 对比。这引出后续章节辛积分器的动机。

练习 4：稀疏性与树拓扑的关系（理论 + 编程）¶

(a) 对串联 7-DOF 臂，画出质量矩阵 \(M\) 的非零结构（应为全稠密下三角 + 对称）。解释为什么串联链没有结构稀疏性。

(b) 对四足机器人（如 ANYmal，18-DOF：体 6-DOF + 4 × 3-DOF 腿），画出 \(M\) 的预期非零结构。哪些 \(M_{ij}\) 必然为零？（提示：不同腿的关节之间只通过 base 耦合。）

(c) 用 Pinocchio 加载两个模型，计算 \(M\) 并绘制 plt.spy(M) 热图。与你的预测对比。

(d) 讨论：对于人形机器人（30+ DOF），ABA 的 \(O(N)\) 相比 CRBA 的 \(O(N^2)\) 优势有多大？在什么 DoF 数量下 ABA 开始显著优于 \(M^{-1}\) 的 \(O(N^3)\) 方法？

练习 5：跨章综合——RNEA 与 Euler-Poincare 的等价性（进阶）¶

本题连接 O(N) 递推算法（本章）、空间向量代数（第 10 章）和 SE(3) 几何力学（第 40 章）。

(a) 写出单刚体的 RNEA（只有一个"连杆"，无关节）：前向递推给出 \(v = \xi\)（body twist），\(a = \dot\xi\)。反向递推给出 \(f = \mathcal{I}\dot\xi + \xi \times^* (\mathcal{I}\xi)\)。

(b) 将 RNEA 的结果 \(f = \mathcal{I}\dot\xi + \xi \times^* (\mathcal{I}\xi) = f_{\text{ext}}\) 与 Euler-Poincare 方程 \(\mathcal{I}\dot\xi + \xi \times^* (\mathcal{I}\xi) = f_{\text{ext}}\) 对比——两者完全一致！（注意这里 \(\xi \times^*\) 是 Featherstone 的 force cross = \(-\mathrm{ad}^T\)。）

(c) 对多连杆系统，RNEA 的反向递推 \(f_i = \mathcal{I}_i a_i + v_i \times^* h_i - f_i^{\text{ext}}\)，\(f_{\lambda(i)} \leftarrow f_{\lambda(i)} + {}^{\lambda(i)}X_i^{-*} f_i\) 可以看作"多体 Euler-Poincare 方程在运动树上的递推求解"。请解释： - 前向递推 = 计算每个连杆的 body twist（运动学约束在 SE(3)\(^N\) 上的左平凡化） - 反向递推 = 对偶变量（力）从叶子向根传播（Euler-Poincare 方程的"回代"）

(d) 这个等价性意味着什么？如果你理解了 Euler-Poincare，就理解了 RNEA 的每一步"为什么这样做"。反过来，RNEA 给出了 EP 方程的一个 \(O(N)\) 数值求解算法。