dtau 与代码生成流水线¶

所属：刚体动力学专题 4-5 | 前置：10_空间向量代数（Plucker 变换微分）、20_Lagrange与Hamilton力学（M/C/g 结构）、30_ON动力学递推算法（RNEA/ABA 递推结构）、40_SE3几何力学（ad/Ad 算子） 交叉引用：→ 05_运动控制/90_DDP（iLQR 需要 A_t, B_t）、→ 05_运动控制/120_MPC（OCS2/Crocoddyl 的实时性依赖解析导数）

前置自测¶

📋 前置自测（答不出 >= 2 题 → 先回前置专题复习）

RNEA 的前向递推计算什么？后向递推计算什么？写出核心递推公式。
ABA 与 RNEA 的关系是什么？ABA 求解的是什么问题（正/逆动力学）？
Featherstone 的 spatial cross 算子 v x_M 和 v x*_F 对应 Lie 群的什么？
对一个 n-DoF 机器人，有限差分计算 dtau/dq 需要多少次 RNEA 调用？
写出 DDP/iLQR 中 Riccati 反传需要的动力学 Jacobian A_t 和 B_t 的定义。

本章目标¶

学完本章后，你将能够： 1. 手推 Carpentier-Mansard RSS 2018 的 placement 微分恒等式，理解"∂X/∂q 作用在 motion/force 上 = 与 S 的空间叉乘" 2. 完整推导 ∂RNEA/∂q 和 ∂RNEA/∂v 的 O(Nd) 递推算法，理解稀疏性的来源 3. 从 ID-FD 互逆关系导出 ∂ABA/∂q, ∂ABA/∂v, ∂ABA/∂tau = M^{-1} 4. 掌握四条导数路线（解析、CodeGen、运行时 AD、有限差分）的精度-速度-适用场景对比 5. 能用 Pinocchio 调用 computeRNEADerivatives 和 computeABADerivatives，并与有限差分对比验证 6. 理解 CppADCodeGen 代码生成流水线在 OCS2 中的六步实现 7. 了解二阶导数 ∂^2 ID/∂q^2 在 exact DDP 中的作用

知识树¶

动力学解析微分
├── 根问题：MPC/DDP 需要 ∂f/∂(x,u)，怎么高效计算？
├── 主干 1：导数需求全景
│   ├── iLQR/DDP 的 Riccati 反传
│   ├── SQP/Direct Collocation
│   ├── 系统辨识 Fisher 矩阵
│   └── 可微仿真
├── 主干 2：四条导数路线
│   ├── 有限差分（O(Nn), 精度 10^{-7}）
│   ├── 运行时 AD（CppAD tape, 5-10x RNEA）
│   ├── CodeGen（CppADCodeGen → .so, 1-3x RNEA）
│   └── 解析微分（Carpentier-Mansard, O(Nd), 机器精度）
├── 主干 3：∂RNEA/∂q 的 O(N) 闭式
│   ├── Placement 微分恒等式（一招制胜）
│   ├── 前向导数 pass (Algorithm 2)
│   ├── 后向导数 pass (Algorithm 3)
│   └── 三个关键简化
├── 主干 4：∂ABA/∂(q,v,tau) 从 ID-FD 互逆导出
│   ├── ∂q_ddot/∂q = -M^{-1} ∂tau/∂q
│   ├── ∂q_ddot/∂tau = M^{-1} (AZA 算法)
│   └── computeMinverse 实现
├── 分支 A：与自动微分的对比
│   ├── CppAD 运行时 vs CodeGen
│   ├── CasADi SX/MX
│   └── JAX jacfwd/jacrev
├── 分支 B：二阶导数
│   ├── ∂^2 tau/∂q^2（Singh-Wensing 2022）
│   ├── exact DDP vs iLQR GN 近似
│   └── Pinocchio rnea-second-order-derivatives
├── 分支 C：代码生成流水线
│   ├── CppADCodeGen 六步
│   ├── OCS2 的 CppAdInterface
│   └── 嵌入式部署
└── 分支 D：在 MPC/DDP 中的应用
    ├── Crocoddyl 路线（解析）
    ├── OCS2 路线（CodeGen）
    └── acados + Pinocchio

5.1 为什么动力学导数是 MPC 的瓶颈 ⭐¶

动机¶

你已经实现了一个 DDP 求解器，对一个 7-DoF 机械臂做轨迹优化。代码跑起来了，但……太慢了。分析一下热点：90% 的时间花在计算动力学 Jacobian 上。

为什么？DDP 的 Riccati 反传需要在每个时间节点求 ∂f/∂x 和 ∂f/∂u。如果你用最简单的有限差分：对 7-DoF 系统，∂tau/∂q 是 7x7 矩阵，需要 8 次 RNEA 调用（中心差分需要 14 次）。乘以 N=50 个时间节点，再乘以 K=10 次 DDP 迭代——每次 MPC 更新需要 50 x 10 x 14 = 7000 次 RNEA。

对 36-DoF 人形机器人：每次有限差分需要 73 次 RNEA（中心差分），单次耗时 450 us。而 Crocoddyl 的解析路线只需 17 us——27 倍加速。这不是"好一点点"，这是实时可行 vs 完全不可行的差异。

本质洞察：Carpentier-Mansard RSS 2018 之所以是"过去十年刚体动力学领域最重要的工程论文之一"，不是因为它的数学特别深（所有推导用 Featherstone 的空间代数即可完成），而是因为它把"动力学导数"从 MPC 的计算瓶颈变成了副产品——让 whole-body NMPC 在真实人形机器人上第一次变得实时可行。

5.1.1 数字上的代价¶

一个典型的 whole-body MPC 控制回路（Crocoddyl/OCS2 在 ANYmal/Talos 上）：

参数	典型值	说明
时域节点数 N	50-100	每步 10-20ms，总时域 0.5-2s
iLQR 迭代数 K	5-20	每次 MPC 更新内
MPC 频率	100 Hz	每 10ms 更新一次
每秒导数评估次数	5000-20000	N x K x freq

对 36-DoF 人形： - 有限差分路线：20000 x 450 us = 9 秒/秒（CPU 负载 900%，不可能实时） - 解析路线：20000 x 17 us = 0.34 秒/秒（34% CPU，正好留足求解器时间）

5.1.2 导数的使用场景全景¶

动力学导数不只服务 MPC。它支撑：

轨迹优化（DDP/iLQR/SQP/Direct Collocation）：Bellman 二阶近似需要 ∂f/∂(x,u)
模型预测控制：每个 sample time 重复求解 OC 问题
系统辨识：Fisher 信息矩阵 J^T J，其中 J = ∂tau/∂theta，theta 是惯性参数
最优设计（co-design）：对连杆长度、电机选型的可微优化
灵敏度分析：扰动对闭环性能的一阶响应
Model-based RL：∂s'/∂(s,a) 直接喂 policy gradient
可微物理渲染：从视频反推物理参数
控制增益自整定：PD 增益的解析梯度

与 05_运动控制的交叉引用：在 90_DDP 中，Crocoddyl 的 DifferentialActionModelFreeFwdDynamics::calcDiff() 直接调用本章的 computeABADerivatives。在 120_MPC 中，OCS2 的 ocs2_pinocchio_interface 通过 CppADCodeGen 包装本章的解析/AD 导数。

5.1.3 为什么"直接 AD"远远不够¶

一个自然的想法：既然有 CppAD/JAX，为什么不直接 jax.jacfwd(dynamics)？

真实代价对比：

方法	7-DoF 耗时	36-DoF 耗时	首次开销	精度
有限差分	21 us	450 us	0	10^{-7}
CppAD 运行时	5-10 us	30-60 us	0	机器级
CppAD + CodeGen	1.5-3 us	8-15 us	数秒到数分钟	机器级
Pinocchio 解析	3.3 us	17 us	0	机器级

解析导数的独特优势：零编译开销 + 即时可用 + 机器精度 + 严格保零模式。

反事实推理：如果不保零模式会怎样？当 q_dot = q_ddot = 0 且无重力时，∂tau/∂q_dot 理论上严格为零矩阵。但有限差分会产生 ~10^{-7} 的稠密残差。DDP 的 Riccati 反传对这类"伪非零"极度敏感——它会误判 M 的稀疏结构，把本来应该是带状（O(Nd) 非零元素）的矩阵当作稠密（O(N^2) 非零元素）处理，QP 求解器速度直接下降一个数量级。

5.2 有限差分：原理与致命局限 ⭐¶

动机¶

有限差分是最简单的导数近似方法——对任何可以"输入数值、输出数值"的函数都能用。在验证解析导数正确性时它不可替代。但在实时控制中，它有致命的精度和速度缺陷。

5.2.1 前向差分¶

\[\frac{\partial f}{\partial x_j} \approx \frac{f(x + \varepsilon e_j) - f(x)}{\varepsilon}\]

误差由两部分组成： - 截断误差：~(epsilon/2) f''(x)（Taylor 展开余项） - 舍入误差：~epsilon_mach / epsilon（浮点减法的精度损失）

最优步长：$\varepsilon^* = \sqrt{\epsilon_{\mathrm{mach}}} \approx 10^{-8}$

此时总误差 ~10^{-8}（相对误差）。

5.2.2 中心差分¶

\[\frac{\partial f}{\partial x_j} \approx \frac{f(x + \varepsilon e_j) - f(x - \varepsilon e_j)}{2\varepsilon}\]

精度提升至 O(epsilon^2)： - 最优步长：$\varepsilon^* = \epsilon_{\mathrm{mach}}^{1/3} \approx 5 \times 10^{-6}$ - 总误差：~10^{-11}

但需要两倍的函数调用。

5.2.3 成本分析¶

对 f: R^n -> R^m（动力学 tau: R^{nv} -> R^{nv}）： - 前向差分：(n+1) 次 RNEA（或 ABA）调用 - 中心差分：2n 次调用

对 KUKA (n=7)：8 或 14 次 RNEA。对 Talos (n=36)：37 或 72 次 RNEA。

5.2.4 致命问题：破坏零模式¶

当系统静止（q_dot = q_ddot = 0）且无重力时：

\[\frac{\partial\tau}{\partial\dot q}\bigg|_{\dot q=0} = C(q, 0) = 0 \quad \text{(理论上精确为零)}\]

但有限差分会产生 ~10^{-7} 的稠密残差矩阵。

后果： - DDP 的 Riccati 反传把这些伪非零当真，误判稀疏结构 - QP 求解器（如 OSQP、qpOASES）因稠密矩阵大幅减速 - 控制器在静止平衡点附近出现微小但持续的抖动

解析导数则精确返回零矩阵——因为它直接从递推公式的代数结构计算，当输入为零时所有相关项自动消失。

5.2.5 有限差分的正确使用场景¶

虽然有限差分不适合实时控制，但它在以下场景不可替代：

单元测试：验证解析导数实现的正确性
原型开发：在推导解析公式之前快速验证算法可行性
复杂约束：某些非光滑约束（如摩擦锥切换）难以写出解析导数
调试：当解析导数结果可疑时，FD 是终极参考

# 典型用法：用 FD 验证解析导数
import pinocchio as pin
import numpy as np

def verify_rnea_derivatives(model, data, q, v, a, eps=1e-8):
    """用中心差分验证 computeRNEADerivatives"""
    dtau_dq_analytic, dtau_dv_analytic, _ = pin.computeRNEADerivatives(model, data, q, v, a)

    # 数值差分 dtau/dq
    dtau_dq_fd = np.zeros((model.nv, model.nv))
    for j in range(model.nv):
        # 正扰动
        v_eps = np.zeros(model.nv); v_eps[j] = eps
        q_plus = pin.integrate(model, q, v_eps)  # 注意：Lie 群积分！
        tau_plus = pin.rnea(model, data, q_plus, v, a)
        # 负扰动
        q_minus = pin.integrate(model, q, -v_eps)
        tau_minus = pin.rnea(model, data, q_minus, v, a)
        # 中心差分
        dtau_dq_fd[:, j] = (tau_plus - tau_minus) / (2 * eps)

    error = np.max(np.abs(dtau_dq_analytic - dtau_dq_fd))
    print(f"max |analytic - FD|: {error:.2e}")  # 应该 < 1e-6
    return error

⚠️ 编程陷阱：浮基时 FD 必须用 pin.integrate 而非 q + dq

对浮基模型，q 包含四元数。直接 q + epsilon * e_j 会破坏 |quaternion| = 1 约束，导致后续 RNEA 结果错误。正确做法是通过 Lie 群积分 pin.integrate(model, q, v_eps) 在切空间上做扰动。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：FD 步长选择

错误做法：硬编码 eps = 1e-6（或 1e-8）。

问题：最优步长取决于函数的二阶导数大小。对高刚度系统（如大惯量 + 强重力），f'' 很大，需要更小的 eps；对数值接近机器精度的量，eps 太小反而更差。

正确做法：对前向差分用 eps = sqrt(eps_mach) * max(|x_j|, 1.0)（缩放到变量量级）；或直接用中心差分获得额外精度。

练习¶

（编程）用 Pinocchio 对 UR5 实现 verify_rnea_derivatives，扫描 eps in [10^{-12}, 10^{-2}]，画出相对误差曲线。验证最优步长约 5e-6（中心差分）。
（概念）解释为什么 4 阶复步长（complex-step）方法可以达到机器精度且无步长选择问题，但为什么 Pinocchio 不用它？（提示：SE(3) 上的复数化不平凡）

5.3 前向自动微分：Dual Numbers 与 CppAD ⭐⭐¶

动机¶

自动微分（AD）是介于符号微分和数值微分之间的"第三条路"——它在数值层面精确计算导数，不需要手推公式，也不受有限差分的步长问题困扰。理解 AD 是理解 CodeGen 和 Pinocchio 模板设计的前提。

5.3.1 Dual Numbers 的核心思想¶

扩充实数为对偶数 a + b epsilon，其中 epsilon^2 = 0（epsilon 不是无穷小，而是代数元素）。

任何解析函数 f 在对偶数上的求值： $$f(a + b\varepsilon) = f(a) + f'(a)\,b\,\varepsilon$$

证明：Taylor 展开 f(a + b epsilon) = f(a) + f'(a) b epsilon + f''(a)/2 (b epsilon)^2 + ... = f(a) + f'(a) b epsilon（因为 epsilon^2 = 0，所有高阶项消失）。

意义：只需把输入从 "a" 变为 "a + 1 epsilon"（设种子方向 b=1），函数输出的 epsilon 系数就精确等于 f'(a)。不需要步长选择，精度为机器级。

5.3.2 从 Dual Numbers 到 Jet¶

Ceres 的 Jet<double, N> 把 epsilon 从标量推广为 N 维向量——同时追踪 N 个方向导数：

\[x = a + \sum_{k=1}^N b_k\,\varepsilon_k, \quad \varepsilon_i\,\varepsilon_j = 0\ \forall i,j\]

这等价于一次前向传播同时计算 Jacobian 的 N 列。Eigen 的 AutoDiffScalar<VectorNd> 实现类似功能。

5.3.3 CppAD 的 Tape-Based AD¶

CppAD（Bradley Bell，COIN-OR 项目）使用 operator overloading 记录计算图（tape）：

#include <cppad/cppad.hpp>
using AD = CppAD::AD<double>;

std::vector<AD> x(n), y(m);
CppAD::Independent(x);          // 开始录制 tape
y = f(x);                        // 所有算子被重载，记录到 tape
CppAD::ADFun<double> fun(x, y);  // 停止录制

// 之后任意多次求值（不重新录制）
auto jac = fun.Jacobian(x_val);  // 回放 tape 求 Jacobian
auto hes = fun.Hessian(x_val, w);  // 求 Hessian

5.3.4 Pinocchio 的模板设计¶

Pinocchio 全代码模板化：ModelTpl<Scalar>、DataTpl<Scalar>、所有算法以 Scalar 为模板参数。这意味着一行代码就能启用 AD：

// 从 double 模型创建 AD 模型
auto ad_model = model.cast<CppAD::AD<double>>();
// 所有算法（RNEA, ABA, CRBA, FK, centroidal...）自动支持 AD 标量

这是模板元编程的威力：算法代码只写一次，通过 Scalar 类型参数化，编译器自动生成 double/AD/CasADi 等各种版本。

5.3.5 CppAD 运行时的性能开销¶

CppAD 在"运行时解释 tape"模式下的开销约为原生算法的 5-10 倍：

模型	RNEA (double)	RNEA (CppAD runtime)	倍率
KUKA 7-DoF	1.2 us	6-12 us	5-10x
Talos 36-DoF	5.5 us	30-55 us	5-10x

原因：CppAD 每次求值都要遍历 tape（一个操作节点的链表），有虚函数调用、分支预测失败、缓存不友好等开销。

结论：CppAD 运行时适合离线验证和原型开发，不适合实时控制（但 CodeGen 后就可以）。

跨领域类比：CppAD tape 就像 Python 的解释器——每次执行都要"读一行、翻译一行、执行一行"。CodeGen 则像把 Python 编译成 C——首次编译慢，但之后每次执行飞快。Pinocchio 解析导数则像手写的 C 代码——不需要编译流水线，直接就是最优的。

⚠️ 常见陷阱¶

⚠️ 编程陷阱：CppAD tape 污染

忘记 CppAD::Independent(x) 或在 tape 录制中修改非模板成员会导致 tape 记录错误。典型症状：Jacobian 某些列全为零或 NaN。

正确做法：在 Independent 和 ADFun 之间，只使用 AD 标量变量，不访问 double 类型的 model 成员。

⚠️ 编程陷阱：CasADi 不能包装已有 C++ 代码

CasADi 要求用自己的 SX/MX 类型从头构建表达式图。不能把 Pinocchio 的 double 代码直接套上 CasADi——必须用 pinocchio.casadi 桥接（Pinocchio 3）。

练习¶

（编程）实现一个 2D 对偶数类 Dual（存储 value 和 derivative），重载 +, *, sin, cos。用它计算 f(x) = sin(x)^2 + cos(x) 在 x=pi/3 处的导数，与解析结果对比。
（概念）解释为什么前向 AD 计算 Jacobian 一列的复杂度等于一次函数评估，而反向 AD 计算 Jacobian 一行的复杂度等于一次函数评估。对 n>m 的 f: R^n -> R^m，哪种更高效？

5.4 ∂RNEA/∂q 的 O(N) 闭式推导（Carpentier-Mansard RSS 2018）⭐⭐¶

动机¶

这是本章的技术核心。Carpentier-Mansard 在 2018 年 RSS 上发表的这篇论文，提出了 RNEA 一阶偏导的 O(Nd) 解析递推算法。它的核心 insight 令人优雅：placement 对关节角度的微分 = 被作用的 spatial 量与运动子空间 S 的空间叉乘——这一条恒等式把整个推导从不可能变为可能。

5.4.1 RNEA 回顾与记号¶

回顾 30_ON动力学递推算法中的 RNEA（Featherstone 记号）：

前向运动学 pass（i = 1..N，lambda(i) 为 i 的父关节）：

\[v_i = {}^iX_{\lambda(i)}\,v_{\lambda(i)} + S_i\dot q_i \tag{R1}$$ $$a_i = {}^iX_{\lambda(i)}\,a_{\lambda(i)} + S_i\ddot q_i + v_i \times (S_i\dot q_i) \tag{R2}$$ $$h_i = I_i\,v_i, \quad f_i = I_i\,a_i + v_i \times^* h_i - f_i^{\mathrm{ext}} \tag{R3}\]

后向力传递 pass（i = N..1）：

\[\tau_i = S_i^T f_i, \quad f_{\lambda(i)}\ \mathrel{+}=\ {}^{\lambda(i)}X^*_i\,f_i \tag{R4}\]

其中： - $v_i, a_i \in \mathbb{R}^6$：link i 的 spatial velocity / acceleration - $h_i, f_i \in \mathbb{R}^6$：link i 的 spatial momentum / force - $I_i \in \mathbb{R}^{6\times 6}$：link i 的 spatial inertia - $S_i$：关节运动子空间（revolute: 6x1 向量） - ${}^iX_{\lambda(i)}$：从 parent frame 到 child frame 的 Plucker transform - $\times, \times^*$：motion/force 空间叉算子（= ad, ad*）

5.4.2 核心代数技巧：Placement 微分恒等式¶

定理（Carpentier-Mansard 2018, Eq. 13-14）：

\[\frac{\partial\,{}^iX_{\lambda(i)}}{\partial q_i}\,v_{\lambda(i)} = \bigl({}^iX_{\lambda(i)}\,v_{\lambda(i)}\bigr) \times S_i \tag{Eq.13}\]

\[\frac{\partial\,{}^{\lambda(i)}X^*_i}{\partial q_i}\,f_i = {}^{\lambda(i)}X^*_i\,(S_i \times^* f_i) \tag{Eq.14}\]

物理直觉：关节 i 转动 dq_i 时，连杆 i 的 frame 发生微小旋转。这个旋转对任何 spatial quantity 的效应 = 将其与旋转方向（即 S_i）做一次空间叉乘。

本质洞察：这两条恒等式是整篇论文的"一招制胜"。它把一个看似需要 6x6 张量求导的问题（∂X/∂q 是一个 6x6 矩阵关于标量 q 的导数），压缩成一次熟悉的空间叉乘运算。整个 O(Nd) 复杂度由此而来。

证明 sketch（以 revolute joint 为例）：

设关节 i 的 placement 可分解为 ${}^iX_{\lambda(i)} = X_{\text{fixed}} \cdot X_{\text{joint}}(q_i)$，其中 $X_{\text{joint}}(q_i)$ 只包含绕旋转轴 s 的旋转。

对 $X_{\text{joint}}$ 求导：$dX_{\text{joint}}/dq_i$ 的作用等价于在 child frame 中做一个无穷小旋转 $\hat s$ —— 这正是 $S_i$ 的定义。因此：

\[\frac{\partial({}^iX_{\lambda(i)}\,m)}{\partial q_i} = ({}^iX_{\lambda(i)}\,m) \times S_i\]

（右边是"把 parent frame 的 motion 先变换到 child frame，再与 S_i 叉乘"。）

5.4.3 前向导数 Pass（Algorithm 2）¶

对 u in {q, q_dot} 作为微分变量：

Step 1：速度导数递推： $$\frac{\partial v_i}{\partial u} = {}^iX_{\lambda(i)}\,\frac{\partial v_{\lambda(i)}}{\partial u} + \frac{\partial v_J}{\partial u} + \text{placement derivative term}$$

具体展开（对 u = q_j）： - 若 j = i（自身关节）：利用 Eq.13 添加 $({}^iX_{\lambda(i)}\,v_{\lambda(i)}) \times S_i$ - 若 j != i 但是 i 的祖先：通过 ${}^iX_{\lambda(i)}$ 传播 - 若 j 既非 i 也非 i 的祖先：∂v_i/∂q_j = 0（稀疏性！）

Step 2：动量导数： $$\frac{\partial h_i}{\partial u} = I_i\,\frac{\partial v_i}{\partial u}$$

Step 3：加速度导数： $$\frac{\partial a_i}{\partial u} = {}^iX_{\lambda(i)}\,\frac{\partial a_{\lambda(i)}}{\partial u} + \text{(placement derivative)} + \frac{\partial v_i}{\partial u} \times v_J + v_i \times \frac{\partial v_J}{\partial u}$$

Step 4：力导数： $$\frac{\partial f_i}{\partial u} = I_i\,\frac{\partial a_i}{\partial u} + \frac{\partial v_i}{\partial u} \times^* h_i + v_i \times^* \frac{\partial h_i}{\partial u}$$

5.4.4 后向导数 Pass（Algorithm 3）¶

\[\frac{\partial\tau_i}{\partial u} = S_i^T\,\frac{\partial f_i}{\partial u} \tag{A3-1}\]

\[\frac{\partial f_{\lambda(i)}}{\partial u}\ \mathrel{+}=\ {}^{\lambda(i)}X^*_i\,\frac{\partial f_i}{\partial u} + \text{(dual placement derivative for } u=q_i\text{)} \tag{A3-3}\]

其中 dual placement derivative（对 u = q_i 时额外添加）：

\[{}^{\lambda(i)}X^*_i\,(S_i \times^* f_i)\]

5.4.5 三个关键简化¶

简化 1：∂tau/∂q_dot 比 ∂tau/∂q 简单得多

对 u = q_dot： - ∂X_J/∂q_dot = 0（placement 不依赖速度） - ∂v_J/∂q_dot_i = S_i（线性关系） - 加速度方程中的许多项消失

代数原因：velocity 对 q_dot 是线性的，所以二阶导数自动为零。

简化 2：∂tau/∂q_ddot = M(q)

由 Lagrange 方程 tau = M(q) q_ddot + C(q, q_dot) q_dot + g(q)： $$\frac{\partial\tau}{\partial\ddot q} = M(q)$$

这直接等于 CRBA 输出的质量矩阵——不需要额外推导。

简化 3：稀疏性（仅对 $\partial\tau/\partial\ddot q$ 严格成立）

⚠️ 常见误解：$\partial\tau_i/\partial q_j$ 并非"j 是 i 祖先时才非零"。

反例（2R 平面机械臂）：$\tau_1 = M_{11}\ddot q_1 + M_{12}\ddot q_2 + C_{112}\dot q_1\dot q_2 + C_{122}\dot q_2^2 + g_1(q_1, q_2)$。这里 $\partial\tau_1/\partial q_2 \neq 0$（通过重力项和 Coriolis 项），尽管关节 2 是关节 1 的后代而非祖先。

正确的稀疏性陈述：

$\partial\tau/\partial\ddot q = M(q)$：质量矩阵的稀疏性由树拓扑决定。对于分支树（如四足），$M_{ij} = 0$ 当 $i,j$ 不在同一根到叶路径上，这是真正的结构稀疏性
$\partial\tau/\partial q$ 和 $\partial\tau/\partial\dot q$：对串联链（如 KUKA）通常是稠密矩阵。对分支树有部分稀疏性——只有共享运动链路径的关节对之间才耦合
算法复杂度的来源：O(Nd) 复杂度不是因为结果矩阵稀疏，而是因为算法按列计算时，每列只需传播该关节到根的路径（深度 d）上的量

模型	N (DoF)	d (深度)	$\partial\tau/\partial\ddot q$ 非零	稠密元素	$M$ 稀疏率
KUKA	7	7	28（全下三角+对角）	49	57%
ANYmal	18	4	~72	324	22%
Talos	36	6	~216	1296	17%
Atlas	30	6	~180	900	20%

注意：上表仅反映 $M(q)$ 的稀疏性。$\partial\tau/\partial q$ 对于 KUKA 这样的串联链是 7x7 稠密矩阵（无结构稀疏性可利用），O(Nd) 优势仅体现在分支树模型上。

5.4.6 伪代码汇总¶

// ===== 前向 pass =====
∂v_0 = 0;  ∂a_0 = 0

for i = 1..N:
    X = iX_λ(i);  S = S_i

    // 速度导数 (对 q)
    ∂v_i/∂q = X · ∂v_λ(i)/∂q
    ∂v_i/∂q[:, i] += (X · v_λ(i)) × S   // placement derivative (第 i 列)

    // 速度导数 (对 q_dot)
    ∂v_i/∂q̇ = X · ∂v_λ(i)/∂q̇
    ∂v_i/∂q̇[:, i] += S                   // 直接贡献

    // 加速度导数 (对 q)
    ∂a_i/∂q = X · ∂a_λ(i)/∂q + (X · a_λ(i)) × S [第 i 列]
              + ∂v_i/∂q × v_J + v_i × ∂v_J/∂q

    // 加速度导数 (对 q_dot)
    ∂a_i/∂q̇ = X · ∂a_λ(i)/∂q̇ + ∂v_i/∂q̇ × v_J

    // 动量和力导数
    ∂h_i/∂u = I_i · ∂v_i/∂u
    ∂f_i/∂u = I_i · ∂a_i/∂u + ∂v_i/∂u ×* h_i + v_i ×* ∂h_i/∂u

// ===== 后向 pass =====
for i = N..1:
    ∂τ_i/∂u = S_i^T · ∂f_i/∂u            // 填到 dtau_du 的第 i 行

    if λ(i) ≠ 0:
        ∂f_λ(i)/∂u += X*_λ(i)←i · ∂f_i/∂u
        // 对 u=q_i：额外加 dual placement term
        ∂f_λ(i)/∂q[:, i] += X*_λ(i)←i · (S_i ×* f_i)

5.4.7 与 Pinocchio 源码的对应¶

Pinocchio data 成员： - data.dVdq ↔ ∂v_i/∂q（6 x nv per joint） - data.dAdq ↔ ∂a_i/∂q - data.dAdv ↔ ∂a_i/∂q_dot - data.dFdq, data.dFdv, data.dFda ↔ 力导数

源码中的 helper 函数（motion.hpp）： - Motion::cross(Motion) → ad_xi eta（叉乘 motion） - Motion::cross(Force) → ad*_xi f（叉乘 force） - motionSet::motionAction(v, dV) → 对 6xN motion-set 逐列做 v x - motionSet::inertiaAction(I, dV) → I * dV 逐列

⚠️ 常见陷阱¶

⚠️ 编程陷阱：rnea_partial_da 只填上三角

computeRNEADerivatives 返回的第三个矩阵（∂tau/∂a = M）只填了上三角！使用前必须手动对称化：
Eigen::MatrixXd M = rnea_partial_da;
M.triangularView<Eigen::StrictlyLower>() =
    M.transpose().triangularView<Eigen::StrictlyLower>();
不做这步，M*x 的下三角部分全是垃圾值。

⚠️ 概念误区：认为 ∂tau/∂q 是稠密矩阵

新手想法：既然动力学是非线性的，∂tau/∂q 应该是一般的稠密矩阵。

实际上：由于运动树的因果结构，∂tau_i/∂q_j 只在 j 是 i 的祖先时非零。对串联臂，∂tau/∂q 是下三角的；对树状结构（如四足），它有更复杂但仍然稀疏的模式。利用这个稀疏性是 O(Nd) 复杂度的关键。

5.4.8 完整的 dtau/dq 矩阵的结构分析¶

为了深入理解稀疏性，让我们对不同机器人模型分析 dtau/dq 的非零模式。

串联臂（如 KUKA 7-DoF）：dtau/dq 是下三角矩阵（加对角线）。原因：在串联链中，关节 j 是关节 i 的祖先当且仅当 j <= i。因此 dtau_i/dq_j != 0 当且仅当 j <= i。

dtau/dq 非零模式（7-DoF 串联臂）：
  q1 q2 q3 q4 q5 q6 q7
tau1 [X  .  .  .  .  .  .]
tau2 [X  X  .  .  .  .  .]
tau3 [X  X  X  .  .  .  .]
tau4 [X  X  X  X  .  .  .]
tau5 [X  X  X  X  X  .  .]
tau6 [X  X  X  X  X  X  .]
tau7 [X  X  X  X  X  X  X]

总非零元素：1+2+3+4+5+6+7 = 28（vs 稠密的 49）。

四足（如 Solo12，4 条腿各 3 关节）：每条腿是独立的 3-DoF 串联链，加上 6-DoF 浮基。

dtau/dq 非零模式（浮基四足，简化）：
          base(6)  leg1(3)  leg2(3)  leg3(3)  leg4(3)
base(6)  [XXXXX    XXX      XXX      XXX      XXX    ]
leg1(3)  [XXXXX    XXX      ...      ...      ...    ]
leg2(3)  [XXXXX    ...      XXX      ...      ...    ]
leg3(3)  [XXXXX    ...      ...      XXX      ...    ]
leg4(3)  [XXXXX    ...      ...      ...      XXX    ]

基座行/列与所有关节耦合（因为基座是所有关节的祖先），但不同腿之间不直接耦合。

工程意义：QP 求解器（如 OSQP、qpOASES）可以利用这个稀疏模式加速求解。Pinocchio 的解析导数精确保留了这个模式（零元素严格为零），而有限差分会在零元素位置产生 ~10^{-7} 的残差，破坏稀疏性。

5.4.9 Lie 群切空间上的导数¶

对浮基模型，dtau/dq 实际上是在切空间上表达的——因为 q 的四元数部分有 4 个分量，但切空间只有 3 维。

Pinocchio 的 computeRNEADerivatives 返回的 dtau_dq 是 nv x nv 矩阵（不是 nv x nq），因为导数是对切空间变量 v（而非参数 q）求的。

这意味着：在计算"dtau/dq"时，实际做的是： $$\frac{\partial\tau}{\partial q}\bigg|_{\text{tangent}} = \lim_{h\to 0} \frac{\tau(\mathrm{Exp}(q, h\,e_j)) - \tau(q)}{h}$$

其中 Exp(q, delta) 是 Lie 群上的指数映射积分（Pinocchio 的 integrate）。

⚠️ 编程陷阱：用有限差分验证浮基导数时，必须用 pin.integrate(model, q, eps * e_j) 做扰动，不能用 q + eps * e_j。后者会破坏四元数的单位约束，导致 RNEA 返回错误结果，进而让 FD 验证失败——你会以为解析导数有 bug，但其实是 FD 本身的错误。

练习¶

（核心手推）对 2R 平面臂（两个 revolute 关节），手动执行 Algorithm 2-3 的每一步。具体写出 ∂v_1/∂q_1, ∂v_2/∂q_1, ∂v_2/∂q_2, ∂f_2/∂q_1, ∂tau_1/∂q_2 的表达式。
（编程）用 Pinocchio 对 KUKA iiwa 计算 dtau_dq，打印其稀疏模式（非零元素位置），验证下三角结构。
（概念）解释为什么对并联机构（如 Delta robot），Carpentier-Mansard 算法的优势减小（因为"祖先"关系更复杂）。
（进阶）对浮基 Solo12，计算 dtau_dq 并可视化其稀疏模式。解释为什么基座的前 6 行/列是稠密的，而不同腿之间是稀疏的。

5.5 ABA 解析导数：从 RNEA 导数 + M^{-1} 直接导出 ⭐⭐¶

动机¶

正向动力学（FD）求解 q_ddot = FD(q, v, tau)。DDP/iLQR 需要 ∂q_ddot/∂(q, v, tau)。直接对 ABA 的三趟递推求导会引入大量中间张量——Carpentier-Mansard 的妙招是利用 ID 和 FD 的互逆关系。

5.5.1 核心恒等式¶

逆动力学 ID 和正向动力学 FD 互为逆运算：

\[\mathrm{ID}(q, \dot q, \mathrm{FD}(q, \dot q, \tau)) = \tau\]

对 u in {q, q_dot} 求偏导（tau 不变）：

\[\frac{\partial\mathrm{ID}}{\partial u} + \frac{\partial\mathrm{ID}}{\partial\ddot q}\cdot\frac{\partial\mathrm{FD}}{\partial u} = 0\]

由于 $\frac{\partial\mathrm{ID}}{\partial\ddot q} = \frac{\partial\tau}{\partial\ddot q} = M(q)$（质量矩阵），得：

\[\frac{\partial\mathrm{ID}}{\partial u} + M\cdot\frac{\partial\ddot q}{\partial u} = 0\]

因此：

\[\boxed{\frac{\partial\ddot q}{\partial q} = -M^{-1}\frac{\partial\tau}{\partial q}, \quad \frac{\partial\ddot q}{\partial\dot q} = -M^{-1}\frac{\partial\tau}{\partial\dot q}, \quad \frac{\partial\ddot q}{\partial\tau} = M^{-1}}\]

意义：ABA 导数问题完全还原为 RNEA 导数 + 一次 M^{-1} 矩阵-向量乘（或矩阵-矩阵乘）。不需要直接微分 ABA 的复杂递推！

反事实推理：如果不用这个互逆技巧，而是直接对 ABA 三趟递推求导，会怎样？ABA 的中间量（如 articulated body inertia I^A_i, bias force p^A_i）本身就是 6x6 矩阵。对它们求导会产生 6x6xn 的三阶张量——存储和计算量都大幅增加，代码也极其复杂。互逆技巧的优雅在于：把一个"正向导数"问题转化为"逆向导数 + 线性求解"，后者的中间量已经在 §5.4 中计算好了。

5.5.2 高效 M^{-1}：AZA 算法¶

既然 ∂q_ddot/∂tau = M^{-1}，我们需要高效计算 M^{-1} 的所有列。

方法 1：CRBA + Cholesky 分解

\[M = L D L^T \quad \Rightarrow \quad M^{-1} = L^{-T} D^{-1} L^{-1}\]

Pinocchio 用 稀疏 UDU^T 分解（利用运动树的 elimination ordering），复杂度 O(Nd^2)。

方法 2：AZA 算法

ABA 在 tau = e_j（单位向量）、q_dot = 0、g = 0 的特殊输入下，输出恰好是 M^{-1} 的第 j 列：

\[\ddot q = \mathrm{ABA}(q, 0, e_j) = M^{-1} e_j = (M^{-1})_{:,j}\]

对所有 j = 1..n 做一次，得到完整的 M^{-1}。利用对称性，只需计算上（或下）三角——(n+1)n/2 列。

这个方法被 Singh-Russell-Wensing 2022 正式命名为 AZA（ABA-Zero-Algorithm）。

性能对比（Atlas 36-DoF）：

方法	耗时
CRBA + Cholesky	28.3 us
AZA	12.7 us

AZA 快约 2 倍——这是因为 ABA 的递推天然利用了树结构，而 Cholesky 分解需要额外的填入（fill-in）处理。

Pinocchio 的 computeMinverse(model, data, q) 即为 AZA 实现：

pin.computeMinverse(model, data, q)
# data.Minv: nv x nv 的 M^{-1} 矩阵（对称，存储上三角）

5.5.3 computeABADerivatives 的完整 API¶

// C++ API
pinocchio::computeABADerivatives(model, data, q, v, tau);
// 结果：
// data.ddq_dq  : nv x nv  (∂q_ddot/∂q)
// data.ddq_dv  : nv x nv  (∂q_ddot/∂v)
// data.Minv    : nv x nv  (∂q_ddot/∂tau = M^{-1})

# Python API
pin.computeABADerivatives(model, data, q, v, tau)
ddq_dq = data.ddq_dq  # ∂q̈/∂q
ddq_dv = data.ddq_dv  # ∂q̈/∂v̇
Minv   = data.Minv     # ∂q̈/∂τ = M⁻¹

内部流程： 1. 调用 computeRNEADerivatives 得到 ∂tau/∂q 和 ∂tau/∂v 2. 调用 computeMinverse（AZA）得到 M^{-1} 3. 计算 ∂q_ddot/∂q = -M^{-1} * ∂tau/∂q，∂q_ddot/∂v = -M^{-1} * ∂tau/∂v

5.5.4 在 iLQR/DDP 中的使用¶

DDP 的连续时间状态方程： $$\dot x = f(x, u) = \begin{bmatrix} v \\ \mathrm{FD}(q, v, \tau) \end{bmatrix} = \begin{bmatrix} v \\ \ddot q \end{bmatrix}$$

Jacobian： $$A = \frac{\partial f}{\partial x} = \begin{bmatrix} 0 & I \\ \partial\ddot q/\partial q & \partial\ddot q/\partial v \end{bmatrix}, \quad B = \frac{\partial f}{\partial u} = \begin{bmatrix} 0 \\ M^{-1} \end{bmatrix}$$

这三个矩阵正好是 computeABADerivatives 的输出！

Crocoddyl 中的对应（DifferentialActionModelFreeFwdDynamics::calcDiff()）：

// Crocoddyl 内部（简化）
pinocchio::computeABADerivatives(pin_model_, *pin_data_, q, v, tau);
// 组装 Fx = [0, I; ddq_dq, ddq_dv]
// 组装 Fu = [0; Minv * actuation_matrix]

练习¶

（手推）证明 ∂ID/∂q_ddot = M(q)。（提示：RNEA 可以写成 tau = M q_ddot + h(q, v)，其中 h 不依赖 q_ddot。）
（编程）对 UR5，用 computeABADerivatives 得到 ddq_dq，与 -(pin.computeMinverse(model, data, q) @ dtau_dq) 对比，验证一致性。
（概念）解释为什么 data.Minv 在浮基模型下是 (6+n) x (6+n)，而 B 矩阵（控制输入 Jacobian）是 (6+n) x n——前 6 行对应 unactuated 的浮基。

5.6 Pinocchio 源码精读 ⭐⭐¶

动机¶

调用 API 只是第一步。当你需要修改算法（如添加新的关节类型、集成到自定义框架）或调试异常行为时，必须能读懂源码。本节带你走一遍 Pinocchio 的 rnea-derivatives.hxx 核心逻辑。

5.6.1 关键文件路径¶

文件	作用
`include/pinocchio/algorithm/rnea-derivatives.hpp/.hxx`	`computeRNEADerivatives` 主实现
`include/pinocchio/algorithm/aba-derivatives.hpp/.hxx`	`computeABADerivatives`，复用 RNEA 导数 + AZA
`include/pinocchio/algorithm/kinematics-derivatives.hpp/.hxx`	FK 导数
`include/pinocchio/algorithm/rnea-second-order-derivatives.hpp/.hxx`	二阶导数
`include/pinocchio/algorithm/cholesky.hxx`	稀疏 UDU^T
`include/pinocchio/algorithm/constrained-dynamics-derivatives.hpp/.hxx`	约束动力学导数 (v3)
`include/pinocchio/autodiff/cppad.hpp`, `casadi.hpp`	AD 标量适配

5.6.2 核心 C++ API 签名¶

template<typename Scalar, int Options, template<typename,int> class JC,
         typename VQ, typename VV, typename VA,
         typename M1, typename M2, typename M3>
void computeRNEADerivatives(
    const ModelTpl<Scalar,Options,JC>& model,
    DataTpl<Scalar,Options,JC>&       data,
    const Eigen::MatrixBase<VQ>& q,     // nq x 1
    const Eigen::MatrixBase<VV>& v,     // nv x 1
    const Eigen::MatrixBase<VA>& a,     // nv x 1
    const Eigen::MatrixBase<M1>& rnea_partial_dq,   // ∂τ/∂q (nv x nv)
    const Eigen::MatrixBase<M2>& rnea_partial_dv,   // ∂τ/∂v̇ (nv x nv)
    const Eigen::MatrixBase<M3>& rnea_partial_da);  // ∂τ/∂a = M (上三角!)

5.6.3 源码级伪代码对应¶

rnea-derivatives.hxx 的前向 pass：

// 对每个关节 i（从根到叶）：
for(int i = 1; i < model.njoints; ++i) {
    // 1. 计算 FK 量（v_i, a_i 已在前一步 rnea 中算好）
    // 2. 速度导数传播
    //    data.dVdq[i] = X * data.dVdq[parent] + placement_term
    //    data.dVdv[i] = X * data.dVdv[parent] + S_i
    // 3. 加速度导数
    //    data.dAdq[i] = X * data.dAdq[parent] + ...
    // 4. 力导数
    //    data.dFdq[i] = I * data.dAdq[i] + cross_terms
}

后向 pass：

for(int i = model.njoints-1; i > 0; --i) {
    // 1. tau 导数：第 i 行 = S_i^T * data.dFdq[i]
    // 2. 传播到 parent：data.dFdq[parent] += X* * data.dFdq[i] + ...
}

5.6.4 Python 使用示例¶

import pinocchio as pin
import numpy as np

# 加载模型
model = pin.buildModelFromUrdf("ur5.urdf")
data = model.createData()

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

# 计算 RNEA 导数
dtau_dq, dtau_dv, dtau_da = pin.computeRNEADerivatives(model, data, q, v, a)
# dtau_da 是 M 矩阵（上三角），需要对称化
M = dtau_da.copy()
M[np.tril_indices(model.nv, -1)] = M.T[np.tril_indices(model.nv, -1)]

# 验证 dtau_da == M（来自 CRBA）
pin.crba(model, data, q)
M_crba = data.M.copy()
M_crba[np.tril_indices(model.nv, -1)] = M_crba.T[np.tril_indices(model.nv, -1)]
assert np.allclose(M, M_crba), "M from RNEA derivatives should equal CRBA"

# 计算 ABA 导数（FD 导数）
tau = pin.rnea(model, data, q, v, a)  # 先算对应的 tau
pin.computeABADerivatives(model, data, q, v, tau)
print(f"∂q̈/∂q shape: {data.ddq_dq.shape}")  # (nv, nv)
print(f"∂q̈/∂v shape: {data.ddq_dv.shape}")  # (nv, nv)
print(f"M^{-1} shape: {data.Minv.shape}")     # (nv, nv)

# 验证一致性：ddq_dq == -Minv @ dtau_dq
ddq_dq_check = -data.Minv @ dtau_dq
print(f"一致性误差: {np.max(np.abs(data.ddq_dq - ddq_dq_check)):.2e}")

5.6.5 CppAD 模板标量切换¶

#include "pinocchio/autodiff/cppad.hpp"
using ADScalar = CppAD::AD<double>;
using ADModel = pinocchio::ModelTpl<ADScalar>;

// 一行完成 double -> AD 切换
pinocchio::Model model;
pinocchio::buildModels::humanoidRandom(model);
ADModel ad_model = model.cast<ADScalar>();
ADModel::Data ad_data(ad_model);

// 录制 tape
Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_q = q0.cast<ADScalar>();
CppAD::Independent(ad_q);                       // tape start
auto ad_tau = pinocchio::rnea(ad_model, ad_data, ad_q, ad_v, ad_a);
CppAD::ADFun<double> f(ad_q, ad_tau);           // tape end

// 求 Jacobian（运行时回放）
auto jac = f.Jacobian(std::vector<double>(q0.data(), q0.data() + model.nq));

练习¶

（源码阅读）打开 Pinocchio 的 rnea-derivatives.hxx，找到前向 pass 中 "placement derivative" 对应的代码行。它调用了哪个 helper 函数？
（编程）用 CppAD + Pinocchio 对 UR5 录制 RNEA tape，求 Jacobian，与 computeRNEADerivatives 对比验证。

5.7 CppADCodeGen 代码生成流水线（OCS2 路线）⭐⭐¶

动机¶

OCS2（ETH leggedrobotics）的动力学导数路线是 Pinocchio + CppADCodeGen。这条路线的核心思想：用 CppAD 录制计算图（tape），然后把 tape "打印"成纯 C 代码，编译为共享库 (.so)，运行时通过函数指针调用——性能接近手写 C，无解释器开销。

5.7.1 核心六步¶

Step 1：构造 AD 版 Pinocchio 模型

using ad_scalar_t  = CppAD::cg::CG<double>;      // CodeGen 标量
using ad_scalar_ad = CppAD::AD<ad_scalar_t>;       // AD<CG<double>>
pinocchio::ModelTpl<ad_scalar_ad> modelAd = model.cast<ad_scalar_ad>();

注意：这里用的是 AD<CG<double>> 而非 AD<double>——前者不在运行时求值，而是记录一个可生成 C 代码的表达式图。

Step 2：定义 AD 闭包（用户 Lambda）

auto dynamics_func = [&](const ad_vector_t& x, ad_vector_t& y) {
    ad_vector_t q = x.head(model.nq);
    ad_vector_t v = x.segment(model.nq, model.nv);
    ad_vector_t tau = x.tail(model.nv);
    pinocchio::aba(modelAd, dataAd, q, v, tau);
    y = dataAd.ddq;  // 正向动力学输出
};

Step 3：录制 Tape

ad_vector_t x_ad(input_dim);
CppAD::Independent(x_ad);
ad_vector_t y_ad(output_dim);
dynamics_func(x_ad, y_ad);
CppAD::ADFun<ad_scalar_t> fun(x_ad, y_ad);

OCS2 用 CppAdInterface 封装了这个过程。

Step 4：生成 C 源代码

using namespace CppAD::cg;
ModelCSourceGen<double> csrc(fun, "robot_dynamics");
csrc.setCreateJacobian(true);
csrc.setCreateSparseJacobian(true);    // 只生成非零元素
ModelLibraryCSourceGen<double> lib(csrc);
DynamicModelLibraryProcessor<double> proc(lib, "robot_lib");

Step 5：GCC/Clang 编译为 .so

GccCompiler<double> compiler;
compiler.setSourcesFolder("/tmp/codegen_sources");
auto dynLib = proc.createDynamicLibrary(compiler, true);

OCS2 默认编译选项：-O3 -march=native -shared -fPIC。

Step 6：运行时 dlopen 调用

auto model_handle = dynLib->model("robot_dynamics");
std::vector<double> x_val = ...;
auto jac = model_handle->Jacobian(x_val);  // 函数指针调用，无解释器

5.7.2 生成代码的特征¶

CppADCodeGen 的图优化会： - 消除所有乘 0、加 0、乘 1 的冗余操作 - 稀疏 Jacobian 只生成非零项的计算代码 - 无循环、无分支、无堆分配——纯标量内联运算 - 可选 loop-detection 对重复模式做代码瘦身

典型生成的 C 代码片段：

// 自动生成的 robot_dynamics_sparse_jacobian.c（片段）
void sparse_jacobian(const double* x, double* jac) {
    double t0 = x[3] * x[7] + x[4] * x[8];  // 直接标量运算
    double t1 = sin(x[0]);
    double t2 = cos(x[0]);
    // ... 数百行无分支标量代码 ...
    jac[0] = t0 * t1 + t2;
    jac[1] = ...;
}

5.7.3 OCS2 的工程实践¶

OCS2 的 task.info 配置文件中：

[model_settings]
recompileLibrariesCppAd = true   ; 首次运行时编译
modelFolderCppAd = /tmp/ocs2     ; 缓存编译结果

典型工作流： 1. 首次运行：录制 tape + 生成 C + 编译 .so（耗时数秒到分钟） 2. 后续运行：直接 dlopen 缓存的 .so（耗时 ~ms） 3. 修改 URDF/cost/约束后：设 recompileLibrariesCppAd = true 触发重编译

⚠️ 编程陷阱：CodeGen 缓存失效

修改了 URDF 或 cost 函数却没重编译 .so → OCS2 会沿用旧的动力学/约束。症状：控制器行为与预期不符，但不报错。

正确做法：任何模型/cost 修改后，手动清空 modelFolderCppAd 或设 recompileLibrariesCppAd = true。

5.7.4 为什么 OCS2 选 CodeGen 而 Crocoddyl 选 Analytical¶

维度	OCS2 (CodeGen)	Crocoddyl (Analytical)
cost/约束复杂度	高（carrot, swing, ZMP, friction cone...）	中（标准动力学 cost）
用户自定义	只需写 Eigen + Pinocchio，AD 自动处理	需对每个新 cost 写解析导数
首次开销	数秒到分钟编译	零
运行时性能	与解析同级（甚至更快，因为全展开）	mus 级
Lie 群支持	通过 CppAD 间接支持	原生 SE(3) 闭式
调试便利性	较难（生成代码不可读）	较易（Python/C++ 镜像）

最佳实践：两路线可以共存——底层 RBD（RNEA/ABA/CRBA 的导数）走 Pinocchio 解析（mus 级、零编译），上层 cost/constraint（如 swing foot tracking、摩擦锥、self-collision）走 CppADCodeGen（用户友好、易扩展）。OCS2 的真实架构正是如此。

5.7.5 CasADi SX → C 代码的替代路线¶

CasADi 提供另一条符号 → C 代码的路线：

import casadi
import pinocchio.casadi as cpin

# CasADi 符号变量
q_sx = casadi.SX.sym('q', model.nq)
v_sx = casadi.SX.sym('v', model.nv)
tau_sx = casadi.SX.sym('tau', model.nv)

# Pinocchio CasADi 接口
cmodel = cpin.Model(model)
cdata = cmodel.createData()
cpin.aba(cmodel, cdata, q_sx, v_sx, tau_sx)

# 创建 CasADi 函数
ddq_func = casadi.Function('ddq', [q_sx, v_sx, tau_sx], [cdata.ddq])
ddq_dq_func = ddq_func.jacobian()

# 生成 C 代码
ddq_func.generate('robot_dynamics.c', {'with_header': True})

CasADi 的优势：与 IPOPT/SNOPT 的原生集成、MX 图对大型模型更高效。劣势：需要 Pinocchio 3 的 casadi 桥接，对老版本 Pinocchio 不可用。

练习¶

（概念）解释 CodeGen 的 .so 为什么比 CppAD 运行时 tape 快——两者不都是"一次录制，多次求值"吗？（提示：tape 解释 vs 函数指针）
（工程）在 OCS2 的 ocs2_pinocchio_interface 中找到 CppAdInterface 的使用位置，列出它封装了上述六步中的哪几步。

5.8 四条路线的完整对比 ⭐⭐¶

5.8.1 性能基准数据¶

以下数据综合自 Carpentier-Mansard RSS 2018 Table I、Neunert-Giftthaler SIMPAR 2016、Pinocchio benchmarks。平台：Intel i7 @ 2.2 GHz, LLVM 9.0, 单核。

7-DoF 臂（KUKA / iiwa / Panda）：

路线	∂tau/∂(q,v) (us)	∂q_ddot/∂(q,v,tau) (us)	精度
RNEA baseline	1.20	—	—
ABA baseline	—	1.78	—
Pinocchio 解析	3.34	5.78	10^{-15}
CppADCodeGen .so	1.5-3.0	2-4	10^{-15}
CppAD 运行时	5-10	6-12	10^{-15}
有限差分	21.26	22.67	10^{-7}
RobCoGen C++	1.1-2	2-3	10^{-15}

36-DoF 人形（Atlas / Talos）：

路线	∂tau/∂(q,v) (us)	∂q_ddot/∂(q,v,tau) (us)	精度
RNEA/ABA baseline	5.51 / 9.81	—	—
Pinocchio 解析	16.72	45.20	10^{-15}
CppADCodeGen	8-15	25-40	10^{-15}
CppAD 运行时	30-60	50-80	10^{-15}
有限差分	452.46	470.14	10^{-7}
AZA (M^{-1} only)	—	12.70	10^{-15}

5.8.2 选型决策表¶

场景	推荐路线	理由
实时 MPC (100+ Hz)	Pinocchio 解析	零编译、mus 级、确定性
复杂 cost/约束	CppADCodeGen	用户只写 Eigen，AD 自动处理
快速原型	CppAD 运行时	零编译、精度高
单元测试/验证	有限差分	最简单、适用任何函数
嵌入式 (无 Pinocchio)	CodeGen .c	纯标量代码，无依赖
可微仿真训练	JAX AD (MJX)	GPU 并行 + vjp/jvp

5.8.3 关键观察¶

解析导数 ≈ 3x RNEA：稳定的常数因子，不随 DoF 变化
有限差分在大系统慢 27-45x：完全不可接受用于实时
CodeGen 可以比解析更快：因为全展开消除了循环/分支
解析严格保零模式：对 DDP 稀疏性至关重要

本质洞察：四条路线不是"越新越好"的线性关系，而是在精度-速度-灵活性-开发成本四维空间中的不同 trade-off。工程实践中最常见的"最优组合"是：底层 RBD 走 Pinocchio 解析，上层 cost/约束走 CodeGen。

5.9 二阶导数与 Exact DDP ⭐⭐⭐¶

动机¶

标准 iLQR 使用 Gauss-Newton (GN) 近似：$H \approx J^T J$，忽略了动力学的二阶项 $\partial^2 f/\partial x^2$。这在大多数情况下足够（因为最小二乘 cost 的 GN 近似在解附近是好的），但在某些场景下精确 Hessian 能显著加速收敛。

5.9.0 为什么 DDP 的收敛速度关乎实时性¶

在进入二阶导数的技术细节之前，理解"收敛速度"对 MPC 的实际影响很重要。

一个 real-time MPC 控制器（如 OCS2 在 ANYmal 上）的工作方式是：

每 10ms：
  1. 读取当前状态 x_0
  2. 用上一次的解作为 warm-start
  3. 做 K 次 iLQR/DDP 迭代
  4. 输出第一个时间步的控制 u_0*

关键约束：K 次迭代必须在 10ms 内完成。如果每次迭代需要 1ms，那最多做 10 次。

GN 近似（iLQR）的收敛速度是超线性的（asymptotically quadratic near solution）。但远离解时，它可能需要 20-50 次迭代才接近最优。

精确 Hessian（DDP）的收敛速度是严格二次的（even far from solution）。典型地只需要 5-10 次迭代。

对 humanoid 的大动作规划（如从蹲到站、前空翻恢复），初始猜测可能远离最优解——此时精确 Hessian 的收敛优势是决定性的。

反事实推理：如果我们只有 GN 近似且迭代预算有限（K=5），对大扰动恢复会怎样？GN 在前几次迭代的步长很保守（因为低估了曲率），K=5 时可能还没走出初始猜测的邻域。精确 Hessian 在第 2-3 次迭代就能给出合理的步长——差异直接体现为"机器人是否能从倾倒中恢复"。

5.9.1 iLQR 的 GN 近似 vs Exact DDP¶

iLQR（Gauss-Newton）： $$Q_{xx} \approx \ell_{xx} + f_x^T V_{xx}' f_x$$

忽略了 $V_x' \cdot f_{xx}$ 项（动力学的二阶张量与 value function 梯度的缩并）。

Exact DDP： $$Q_{xx} = \ell_{xx} + f_x^T V_{xx}' f_x + \sum_k (V_x')_k \cdot (f_{xx})_k$$

其中 $(f_{xx})_k = \partial^2 f_k/\partial x^2$ 是动力学第 k 个分量的 Hessian。

5.9.2 何时需要 Exact DDP¶

场景	GN 够用？	原因
正常跟踪	是	偏差小，GN 近似精确
大扰动恢复	否	远离解，GN 低估曲率
非最小二乘 cost	否	GN 只对
精度要求极高	否	GN 收敛速度是超线性，exact 是二次
硬约束 augmented Lagrangian	否	约束 Hessian 不可忽略

5.9.3 Singh-Russell-Wensing 2022 的解析二阶递推¶

Singh-Russell-Wensing (IROS 2022, arXiv:2203.01497) 给出了解析二阶 RNEA 导数的递推公式。核心思想：对 Alg. 2-3 再做一次关于 q 的微分，得到 $\partial^2\tau/\partial q_i \partial q_j$。

复杂度：O(Nd^2)——比一阶的 O(Nd) 多一个 d 因子（因为每个一阶导数列都需要再微分）。

Pinocchio 已实现：rnea-second-order-derivatives.hxx。

性能：对 7-DoF 臂约 30-50 us，36-DoF 约 200-500 us。比一阶慢 10-20 倍，但比 FD 计算二阶导数（需要 O(n^2) 次 RNEA）快得多。

5.9.4 在 SNOPT/IPOPT 中的应用¶

精确 Hessian 在 NLP 求解器中的价值： - IPOPT 的 exact Hessian mode 比 L-BFGS 近似收敛快 2-5 倍 - SNOPT 的 QP 子问题用精确 Hessian 时步长更大 - acados 的 RTI（Real-Time Iteration）scheme 中，精确 Hessian 减少外循环迭代数

反事实推理：如果我们只有 GN 近似，对 humanoid 的大动作规划（如从蹲到站）会怎样？GN 低估了动力学的非线性耦合，导致需要 20-50 次外循环迭代才收敛；精确 Hessian 只需要 5-10 次。对 real-time MPC 来说，迭代次数的差异直接决定了"能否在 10ms 窗口内收敛"。

练习¶

（概念）解释 GN 近似在什么条件下等于精确 Hessian？（提示：当 $V_x' = 0$ 时——即在最优解处。）
（进阶）写出 exact DDP 中 $Q_{xu}$ 的完整表达式（包含二阶项），解释为什么它比 $Q_{xx}$ 更容易计算。

5.10 与自动微分的详细对比 ⭐⭐¶

5.10.1 CppAD vs CasADi vs JAX¶

维度	CppAD	CasADi	JAX
语言	C++	C++/Python/MATLAB	Python
AD 模式	前向 + 反向	前向 + 反向	前向 + 反向 + vmap
图类型	Tape (操作序列)	SX (标量图) / MX (矩阵图)	XLA (编译图)
GPU	否	否	是 (TPU/GPU)
CodeGen	是 (CppADCodeGen)	是 (C/MATLAB/Python)	是 (XLA ahead-of-time)
Pinocchio 集成	原生模板	Pinocchio 3 casadi 桥	无直接（需 MJX 或手写 JAX 动力学）
典型用户	OCS2, Crocoddyl AD 验证	acados, 学术优化	MJX, Brax, RL

5.10.2 精度对比实验¶

import pinocchio as pin
import numpy as np

model = pin.buildModelFromUrdf("ur5.urdf")
data = model.createData()
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)
a = np.random.randn(model.nv)

# 1. 解析导数（ground truth）
dtau_dq_analytic, _, _ = pin.computeRNEADerivatives(model, data, q, v, a)

# 2. 前向 FD (eps = 1e-8)
dtau_dq_fd = np.zeros((model.nv, model.nv))
for j in range(model.nv):
    eps = 1e-8
    dq = np.zeros(model.nv); dq[j] = eps
    q_p = pin.integrate(model, q, dq)
    tau_p = pin.rnea(model, data, q_p, v, a)
    q_m = pin.integrate(model, q, -dq)
    tau_m = pin.rnea(model, data, q_m, v, a)
    dtau_dq_fd[:, j] = (tau_p - tau_m) / (2 * eps)

# 3. 对比
err_fd = np.max(np.abs(dtau_dq_analytic - dtau_dq_fd))
print(f"FD 最大误差: {err_fd:.2e}")  # 期望 ~1e-7 到 1e-6

# CppAD 和解析的差异应该是 ~1e-15（机器精度）

5.10.3 MJX-JAX 的特殊地位¶

MuJoCo 的 JAX 后端（MJX）值得特别讨论，因为它代表了一种与 Pinocchio 完全不同的哲学。

MJX 的核心优势： 1. GPU 批处理：可以同时对数千个状态计算导数（用于 RL 训练） 2. JAX 生态：与 Flax/Optax/JAX MD 等无缝集成 3. vjp/jvp 原生支持：不需要显式 Jacobian，直接 vector-Jacobian product

MJX 的限制： 1. 不利用树结构稀疏性：把动力学当作一般函数 AD，复杂度是 O(n^2) 而非 O(Nd) 2. MJX-Warp 后端不支持 AD（截至 2026 年 4 月）：需要 AD 必须用 JAX 后端 3. Solver 迭代数影响梯度质量：mjd_transitionFD 的准确性取决于 solver 是否收敛

典型用法：

import jax
import mujoco
from mujoco import mjx

# 加载 MuJoCo 模型
mj_model = mujoco.MjModel.from_xml_path("robot.xml")
mjx_model = mjx.put_model(mj_model)
mjx_data = mjx.put_data(mj_model, mj_data)

# 单步仿真函数
def step(data, ctrl):
    data = data.replace(ctrl=ctrl)
    data = mjx.step(mjx_model, data)
    return data

# JAX 自动微分
step_jac = jax.jacobian(lambda ctrl: step(mjx_data, ctrl).qpos)(ctrl)

# 批处理：同时对 1024 个状态求导
batched_step = jax.vmap(step)
batched_jac = jax.vmap(jax.jacobian(lambda c: step(d, c).qpos))

何时选 MJX vs Pinocchio：

场景	推荐	原因
Real-time MPC (单状态)	Pinocchio 解析	O(Nd) vs O(n^2)，CPU 更快
RL 训练 (批处理 1000+)	MJX-JAX	GPU 并行压倒一切
接触丰富的仿真	MuJoCo (原生)	接触模型更完善
系统辨识 / co-design	Pinocchio + CasADi	需要精确解析结构

5.10.4 在不同场景下的选择指南¶

你的项目需要什么？

需要实时控制？
├── 是 → 底层 RBD 走 Pinocchio 解析
│         ├── 上层 cost 简单？→ 解析也手推（Crocoddyl 路线）
│         └── 上层 cost 复杂？→ CppADCodeGen（OCS2 路线）
└── 否 → 
    ├── 需要 GPU 并行？→ JAX (MJX/Brax)
    ├── 需要与 NLP solver 集成？→ CasADi
    └── 需要快速原型？→ CppAD 运行时 或 Python FD

⚠️ 常见陷阱¶

⚠️ 思维陷阱：认为 "JAX 有自动微分所以不需要 Pinocchio"

新手想法：JAX 可以对任何函数自动求导，所以我直接用 JAX 写动力学然后 jacfwd 就行了。

实际上：JAX AD 是通用的，但不利用动力学的树状稀疏结构。对 36-DoF 人形，JAX 的 jacfwd 会把 RNEA 当作一般的 R^n -> R^n 函数处理，产生 O(n^2) 的中间量。而 Pinocchio 解析利用运动树结构达到 O(Nd)。在 CPU 上，Pinocchio 解析比 JAX AD 快 5-20 倍。

JAX 的真正优势在于 GPU 批处理——当你需要同时对 1000 个状态计算导数时（如 RL 训练），GPU 的并行性压倒一切。

5.11 代码生成 → 嵌入式部署 ⭐⭐⭐¶

动机¶

在实际机器人上，MPC 通常跑在嵌入式计算平台（如 Intel NUC、NVIDIA Jetson、甚至 MCU）。这些平台可能没有 Pinocchio 的完整依赖链。代码生成的终极目标是：把整个动力学 + 导数编译成无外部依赖的纯 C 代码，直接烧录到嵌入式平台。

5.11.1 完整部署流水线¶

设计阶段（PC, 有完整环境）：
  URDF + cost/constraint → CppADCodeGen → .c 源文件 → 交叉编译 → .so

部署阶段（嵌入式平台，最小依赖）：
  MPC 循环：dlopen(.so) → 函数指针调用 → ddq, Jacobian

5.11.2 OCS2 在 NUC 上的实际部署¶

ETH leggedrobotics 团队在 ANYmal 上的部署： - 平台：Intel NUC 11 (i7-1165G7) - MPC 频率：200 Hz - 动力学：Pinocchio 解析 - 上层约束：CppADCodeGen .so - 总 CPU 占用：~30%

关键优化： 1. 首次运行在 NUC 上本地编译 .so（避免交叉编译的 ABI 问题） 2. 后续重启直接加载缓存的 .so 3. RNEA/ABA 导数用 Pinocchio 解析（无需 CodeGen） 4. 只对自定义 cost/constraint 用 CodeGen

5.11.3 CasADi 的替代部署路线¶

# acados + CasADi + Pinocchio 路线
import casadi
import pinocchio.casadi as cpin

# 构建符号动力学
cmodel = cpin.Model(model)
cdata = cmodel.createData()
q_sym = casadi.SX.sym('q', model.nq)
v_sym = casadi.SX.sym('v', model.nv)
tau_sym = casadi.SX.sym('tau', model.nv)
cpin.aba(cmodel, cdata, q_sym, v_sym, tau_sym)

# 导出给 acados
dynamics = casadi.Function('dynamics', [q_sym, v_sym, tau_sym], [cdata.ddq])
dynamics.generate('robot_dynamics.c')  # 纯 C 代码，无运行时依赖

# acados 读取生成的 C 代码作为 explicit/implicit ODE

acados 的优势：自带 C 代码求解器（RTI-SQP），整个 MPC 栈可以编译为单一可执行文件。

5.11.4 RobCoGen 的符号路线¶

RobCoGen（ETH/IIT，Frigerio-Buchli-Caldwell-Semini JOSER 2016）：

KINDSL (.kindsl) → Maxima 符号简化 → Java/Xtend 模板 → C++ (展开/内联)

所有零乘法在 Maxima 层消除
循环完全展开为标量运算
IROS 2019 benchmark：iiwa 1.1 us, HyQ 2.4 us, Atlas 5.9 us 正向动力学（全部最快）
缺点：每次修改模型都要重新运行整个工具链

练习¶

（工程）解释为什么交叉编译 CppADCodeGen 的 .so 可能遇到 ABI 兼容性问题（提示：编译器版本、glibc 版本、Eigen 对齐）。
（对比）列出 OCS2 路线 vs acados 路线在嵌入式部署上的 3 个关键差异。

5.12 惯性参数辨识的 Regressor ⭐⭐⭐¶

动机¶

系统辨识是动力学导数的另一个重要应用。Lagrange 方程对惯性参数 theta = (m_i, m_i c_i, I_i) 是线性的——这个性质使得回归分析成为可能。

5.12.1 线性回归形式¶

动力学方程可以写成： $$\tau = Y(q, \dot q, \ddot q)\,\theta$$

其中 Y in R^{nv x 10n} 是回归矩阵（regressor），theta in R^{10n} 是惯性参数向量（每个连杆 10 个参数：质量 1 + 质心 3 + 惯性张量上三角 6）。

为什么是线性的：回顾 RNEA 的力递推 $f_i = I_i a_i + v_i \times^* (I_i v_i) - f_i^{\text{ext}}$。空间惯性 $I_i$ 对 10 个参数 $(m_i, m_i c_i, I_i)$ 是线性的，而 $a_i, v_i$ 不依赖于惯性参数（它们只依赖于 q, v, a 和运动学结构）。因此 $f_i$ 对 $\theta$ 线性，进而 $\tau_i = S_i^T f_i$ 对 $\theta$ 也线性。

5.12.2 Pinocchio API¶

import pinocchio as pin

# 计算 regressor
Y = pin.computeJointTorqueRegressor(model, data, q, v, a)
# Y: nv x 10*njoints 矩阵

# 验证：tau = Y * theta
theta = model.inertias_to_vector()  # 提取当前模型的惯性参数
tau_check = Y @ theta
tau_rnea = pin.rnea(model, data, q, v, a)
print(f"一致性: {np.max(np.abs(tau_check - tau_rnea)):.2e}")  # ~1e-15

5.12.3 Base Parameter Reduction¶

10n 个参数中，很多是不可辨识的（对 tau 没有影响或影响退化）。Gautier-Khalil QR 分解给出基参数（base parameters），通常只有 ~4-6n 个。

持续激励条件（Persistent Excitation）：辨识精度要求 $\int_0^T Y^T Y\,dt \succ 0$（正定）。实践中需要设计特殊的激励轨迹（如 Fourier 级数轨迹）覆盖关节空间。

5.12.4 与动力学导数的联系¶

Fisher 信息矩阵 $\mathcal{I} = \sum_t Y_t^T \Sigma^{-1} Y_t$（Sigma 是噪声协方差）的计算和分析需要 Y 的精确值——这正是 computeJointTorqueRegressor 提供的。辨识精度的 Cramer-Rao 下界 = $\mathcal{I}^{-1}$ 的对角线。

5.13 运动学导数的解析计算 ⭐⭐¶

动机¶

除了动力学导数，运动学导数（如 Jacobian 时间导数 dJ/dt、运动学 Hessian d^2 FK/dq^2）在操作空间控制、TSID 和轨迹优化中同样重要。Pinocchio 提供了完整的运动学导数 API。

5.13.1 Pinocchio 运动学导数 API¶

// 一次性计算所有 FK 导数（填充 data 的多个成员）
pinocchio::computeForwardKinematicsDerivatives(model, data, q, v, a);

// 获取关节 i 的速度导数
// ref: LOCAL / WORLD / LOCAL_WORLD_ALIGNED
pinocchio::getJointVelocityDerivatives(model, data, joint_id, ref, dv_dq, dv_dv);
// dv_dq: 6 x nv (∂v_i/∂q)
// dv_dv: 6 x nv (∂v_i/∂v̇ = J_i, Jacobian!)

// 获取 Jacobian 时间导数 dJ/dt
pinocchio::getJointJacobianTimeVariation(model, data, joint_id, ref, dJ);
// dJ: 6 x nv 矩阵

// 获取运动学 Hessian（Pinocchio 3）
pinocchio::getJointKinematicHessian(model, data, joint_id, ref, hessian);
// hessian: 6 x nv x nv 三阶张量

5.13.2 dJ/dt 在操作空间控制中的用途¶

操作空间加速度控制： $$\ddot x = J(q)\,\ddot q + \dot J(q, \dot q)\,\dot q$$

要跟踪期望的末端加速度 $\ddot x_d$，需要求解： $$\ddot q = J^\dagger\,(\ddot x_d - \dot J\,\dot q) + (I - J^\dagger J)\,\ddot q_0$$

这里 dJ dot{q} 就是通过 getJointJacobianTimeVariation 得到的。

5.13.3 Reference frame 的选择¶

Frame	含义	典型用途
LOCAL	body frame of joint i	与 RNEA 导数一致
WORLD	固定世界系	任务空间控制
LOCAL_WORLD_ALIGNED	平移到 joint i 但方向对齐世界	TSID 任务

⚠️ 编程陷阱：getJointVelocityDerivatives 的 ref 必须与下游 cost/约束的 frame 一致。如果 TSID 的 TaskSE3 用 LOCAL_WORLD_ALIGNED 但你的 Jacobian 用 LOCAL，控制器会出现方向错误的力矩。

5.14 在 MPC/DDP 中的应用模式 ⭐⭐¶

5.14.1 Crocoddyl 路线（Mastalli ICRA 2020）¶

回顾 40_SE3几何力学中的 Centroidal dynamics：Crocoddyl 的 CostModelCentroidalMomentum 需要 A_G（来自 ccrba）的导数——这些导数通过本章的解析框架高效计算。

Pinocchio 解析导数 → DifferentialActionModelFreeFwdDynamics::calcDiff()
→ FDDP/BoxFDDP → 每 10ms 出一个最优轨迹

特点： - 纯 C++ 实现，Python 镜像 API 便于调试 - 直接调用 computeABADerivatives - Lie 群状态空间原生支持 - 适合 whole-body DDP（Talos 前空翻、ANYmal 跳跃）

代码片段（Crocoddyl Python）：

import crocoddyl

# 动力学模型自动使用 Pinocchio 解析导数
state = crocoddyl.StateMultibody(model)
actuation = crocoddyl.ActuationModelFull(state)
dmodel = crocoddyl.DifferentialActionModelFreeFwdDynamics(
    state, actuation, cost_model)

# imodel.calcDiff() 内部调用 pin.computeABADerivatives
imodel = crocoddyl.IntegratedActionModelEuler(dmodel, dt)

# DDP 求解
solver = crocoddyl.SolverFDDP(shooting_problem)
solver.solve(xs_init, us_init, maxiter=100)

5.12.2 OCS2 路线（Farshidian et al.）¶

Pinocchio 解析（底层 RBD） + CppADCodeGen（上层 cost/约束）
→ SLQ/MPC → 200 Hz on NUC

特点： - ROS 集成完善（ocs2_ros_interfaces） - 支持 switched systems（步态切换） - 双线程：MPC 线程 + policy 线程 - 适合足式机器人 locomotion

5.12.3 acados 路线¶

CasADi + Pinocchio.casadi → acados C 求解器 → RTI-SQP

特点： - 生成纯 C 求解器，无 Python 运行时依赖 - 支持 QP solver 选择：HPIPM, qpOASES, OSQP - 适合嵌入式部署和工业应用 - 对接 Matlab/Simulink

5.12.4 三路线对比¶

维度	Crocoddyl	OCS2	acados
底层语言	C++/Python	C++ (ROS)	C (Codegen)
动力学导数	Pinocchio 解析	解析 + CodeGen	CasADi
求解算法	FDDP/BoxFDDP	SLQ (modified iLQR)	RTI-SQP
适用场景	Locomotion/manipulation DDP	Legged MPC	通用 NMPC
Lie 群支持	原生	有限	无
ROS 集成	手动	完善	模板
嵌入式	需 Pinocchio 依赖	需 Pinocchio + CodeGen	纯 C 无依赖

5.15 Pinocchio computeMinverse 与 computeRNEADerivatives 源码分析 ⭐⭐⭐¶

5.13.1 computeMinverse 的 AZA 实现¶

Pinocchio 的 computeMinverse 实现了 AZA 算法：

核心思想：对每一列 j，执行一次"简化 ABA"——只有 tau = e_j（第 j 个单位向量），q_dot = 0，g = 0。

// 伪代码（简化自 pinocchio/algorithm/compute-all-terms.hxx）
void computeMinverse(const Model& model, Data& data, const VectorXd& q) {
    // Phase 1: 前向运动学（只需要 placement，不需要 v, a）
    for(int i = 1; i < model.njoints; ++i) {
        data.oMi[i] = data.oMi[parent] * data.jointPlacements[i] * joint.calc(q);
        data.liMi[i] = ...;  // 相邻关节间的变换
    }

    // Phase 2: 后向 pass（计算 articulated body inertia）
    for(int i = model.njoints-1; i > 0; --i) {
        // I^A_i = I_i + sum_{child c} X^T_c I^A_c X_c (LTDL update)
        // U_i = I^A_i * S_i
        // D_i = S_i^T * U_i (1x1 for revolute)
        // I^A_{parent} += I^A_i - U_i * D_i^{-1} * U_i^T
    }

    // Phase 3: 前向 pass（逐列填 M^{-1}）
    for(int j = 1; j <= model.nv; ++j) {
        // 对 tau = e_j 做一次简化前向 pass
        // a_i = ...（只有 j 的子树有非零加速度）
        // M^{-1}[i, j] = S_i^T * a_i（利用对称性只算上三角）
    }
}

5.13.2 computeRNEADerivatives 的关键优化¶

Pinocchio 源码中的几个关键优化技巧：

1. 列式计算：不存储完整的 6 x nv 中间矩阵，而是逐列处理（减少缓存 miss）。

2. 利用 S_i 的稀疏性：对 revolute joint，S_i 是 6x1 向量（只有一个非零分量）。叉乘 (X v) x S 退化为 motion 的一个分量提取 + 叉乘——大幅减少运算。

3. 避免重复计算：前向 pass 中的 X v_parent、X a_parent 在标准 RNEA 中已计算，直接复用。

4. Eigen 表达式模板：利用 Eigen 的延迟求值避免临时对象分配（但在 AD 场景下需要 .eval() 强制求值）。

5.13.3 性能剖析¶

用 perf 或 callgrind 分析 computeRNEADerivatives 的热点：

操作	占比
motion cross (`v x dV`)	~30%
inertia action (`I * dA`)	~25%
force cross (`v x* dH`)	~20%
placement action (`X * dV`)	~15%
其他（索引、分支）	~10%

优化方向：SIMD 向量化（Eigen 已做）、缓存局部性（列式 vs 行式）、避免分支（关节类型统一处理）。

练习¶

（源码阅读）打开 pinocchio/algorithm/aba-derivatives.hxx，找到它在哪里调用了 computeRNEADerivatives 和 computeMinverse。
（概念）解释为什么 AZA 对"树深 d 小"的模型（如四足，d=4）相比 Cholesky 优势最大。

本章小结¶

知识点	核心结论	难度	Pinocchio API
导数瓶颈	FD 占 MPC 90% 时间，解析加速 27x	⭐	—
有限差分	最优步长 sqrt(eps_mach)，精度 10^{-7}，破坏零模式	⭐	—
前向 AD	Dual number / CppAD tape，5-10x RNEA	⭐⭐	`cast<ADScalar>()`
Placement 微分恒等式	∂X/∂q_i 作用 = 与 S_i 叉乘	⭐⭐	内部
∂RNEA/∂q O(Nd)	前向+后向导数 pass	⭐⭐	`computeRNEADerivatives`
∂RNEA/∂v 简化	velocity 线性→二阶消失	⭐⭐	同上
∂tau/∂q_ddot = M	由 Lagrangian 直接读出	⭐⭐	同上（第三返回值）
ID-FD 互逆	∂q_ddot/∂u = -M^{-1} ∂tau/∂u	⭐⭐	`computeABADerivatives`
AZA (M^{-1})	比 Cholesky 快 2x	⭐⭐	`computeMinverse`
CppADCodeGen	6 步：tape→C→.so→dlopen	⭐⭐	OCS2 `CppAdInterface`
CasADi SX	符号图→C 代码	⭐⭐⭐	`pinocchio.casadi`
二阶导数	O(Nd^2)，exact DDP 收敛更快	⭐⭐⭐	`rnea-second-order-derivatives`
CodeGen 部署	纯 C 无依赖→嵌入式	⭐⭐⭐	—

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

项目：从零构建浮基四足控制器

本章新增模块： - 模块 4-5A：对 Solo12 调用 computeRNEADerivatives，用有限差分验证正确性 - 模块 4-5B：实现一个最简 iLQR（倒立摆），用 computeABADerivatives 提供的 A_t, B_t 做 Riccati 反传 - 模块 4-5C：对比解析 vs FD vs CppAD 的性能，画 benchmark 图

上一章（40_SE3几何力学）的模块：浮基建模 + centroidal dynamics 验证。下一章（60_约束动力学）将新增：带接触的动力学导数。

延伸阅读¶

资源	难度	内容
Carpentier-Mansard RSS 2018	⭐⭐	本章核心论文
Featherstone RBDA Ch.3,5,7	⭐⭐	空间代数 + RNEA/ABA（无导数）
Singh-Russell-Wensing IROS 2022	⭐⭐⭐	二阶解析导数
Griewank-Walther 2008	⭐⭐⭐	AD 圣经
Khalil-Dombre Ch.13-14	⭐⭐	系统辨识 regressor
Lynch-Park Modern Robotics Ch.8	⭐⭐	入门动力学
Carpentier PhD 2017 LAAS	⭐⭐⭐	Pinocchio 哲学源头
Suh et al. ICML 2022	⭐⭐⭐⭐	可微仿真 != 更好 PG
Chen et al. NeurIPS 2018 (Neural ODE)	⭐⭐⭐	伴随方程与 AD
Tedrake Underactuated notes	⭐⭐	交互式 Drake 教学

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
解析导数与 FD 不一致	FD 步长不对或浮基 q += dq 错误	1. 检查步长在 [1e-7, 1e-5] 2. 浮基用 `pin.integrate` 3. 打印差异最大的元素	本章 5.2
`rnea_partial_da` 乘向量结果错误	只填了上三角未对称化	添加 `M.triangularView<Lower>() = M.T.triangularView<Lower>()`	本章 5.6
OCS2 CodeGen 行为不符预期	缓存的 .so 过期	1. 检查 `recompileLibrariesCppAd` 设置 2. 手动删 `modelFolderCppAd` 3. 重启节点	本章 5.7
CppAD Jacobian 部分列为零	tape 录制时漏了 Independent	1. 确认 x_ad 维度 2. 确认 Independent 包含所有输入 3. 检查中间变量是否都是 AD 类型	本章 5.3
DDP 在平衡点附近抖动	FD 破坏零模式（5.2.4 讨论）	1. 切换到解析导数 2. 检查 C(q,0) 是否返回精确零 3. 打印 dtau_dv 在 v=0 时的范数	本章 5.2
M^{-1} 不对称	AZA 只填上三角未复制	对 data.Minv 做对称化处理	本章 5.5

附录 A：四条路线完整对照表¶

维度	有限差分	运行时 AD	CodeGen	解析
代表工具	Eigen NumericalDiff	CppAD, Sacado	CppADCodeGen, CasADi	Pinocchio
原理	(f(x+eps)-f(x))/eps	算子重载录 tape	tape→C→编译	手推递推公式
精度	10^{-7}-10^{-11}	机器精度	机器精度	机器精度
保零模式	否	是	是	是（严格）
复杂度	O(n * RNEA)	O(n * tape)	~1-3x RNEA	O(Nd)
编译开销	0	0	秒~分钟	0
代码侵入	低	低（换 Scalar）	中（需 pipeline）	高（需手推）
新约束扩展	即时	即时	即时（重编译）	需手推
7-DoF (us)	21	5-10	1.5-3	3.3
36-DoF (us)	450	30-60	8-15	17
适用场景	测试/原型	离线/验证	OCS2/工业	Crocoddyl/TSID

附录 B：可微仿真引擎综述（2020-2026）¶

引擎	语言	AD 策略	接触	GPU	状态
MuJoCo C	C	`mjd_transitionFD`	Convex MLCP	否	活跃
MJX-JAX	JAX	jacfwd/jacrev	CG solver	TPU/GPU	活跃
MJX-Warp	Warp	暂不支持 AD	更全	GPU	活跃 (3.3.5+)
Drake	C++	AutoDiffXd	SAP (凸软)	有限	活跃
Nimble	C++	LCP IFT	硬 LCP	部分	转生物力学
Dojo.jl	Julia	IP + IFT	NCP + SOC	否	活跃 v5
Pinocchio	C++	解析+CppAD/CasADi	无完整接触	CodeGen	活跃 v3
NVIDIA Warp	Python/CUDA	kernel reverse	自定义	GPU	活跃
DiffTaichi	Taichi	source transform	用户定义	GPU	维护

2024-2026 关键进展： 1. MJX 成为 RL 训练事实标准（Brax generalized deprecated） 2. MuJoCo Playground (RSS 2025) 一站式 sim-to-real 3. Pinocchio 3 + Crocoddyl 3：解析约束动力学导数 4. Suh et al. ICML 2022 证明"可微仿真 != 更好 PG"

附录 C：核心 GitHub 仓库¶

仓库	用途
stack-of-tasks/pinocchio	解析 RNEA/ABA 导数 + CppAD/CasADi
loco-3d/crocoddyl	FDDP/BoxFDDP，Pinocchio 后端
leggedrobotics/ocs2	SLQ/MPC + CppADCodeGen
coin-or/CppAD	经典 operator-overloading AD
joaoleal/CppADCodeGen	CppAD → C 源码生成
casadi/casadi	符号 AD + NLP 前端
google-deepmind/mujoco	MuJoCo 3 + MJX
RobotLocomotion/drake	AutoDiffXd + SAP
keenon/nimblephysics	LCP 解析梯度
dojo-sim/Dojo.jl	NCP + IP 可微接触
NVIDIA/warp	GPU 可微核
cdsousa/SymPyBotics	符号动力学教学
NVlabs/DiffRL	SHAC 官方实现

附录 D：视频与中文资源¶

英文视频： - Justin Carpentier "Pinocchio tutorial"（ICRA 2021 / IROS 2022 workshop） - Roy Featherstone RBDA 系列（YouTube） - MIT 6.832 Underactuated Robotics（Russ Tedrake） - CMU 16-745 Optimal Control and RL（Zac Manchester） - ETH Robot Dynamics（Marco Hutter）

中文： - 深蓝学院《机器人动力学》《最优控制与 MPC》 - B 站搜索：Pinocchio、RNEA、ABA、iLQR、DDP、自动微分 - 知乎：搜索"Carpentier"、"动力学导数"、"Crocoddyl 源码" - CSDN：Pinocchio 编译笔记、Crocoddyl 示例翻译

附录 E：学习时间预算¶

档位 3（实用 MPC 工程师，30-40h）： - 阅读：Carpentier-Mansard RSS 2018 全文 (4h) - 理论：手推 2R 臂的 ∂tau/∂q (8h) - 代码：Pinocchio Python demo + computeRNEADerivatives/computeABADerivatives (10h) - 工程：Crocoddyl pendulum DDP + UR5 arm (12h)

档位 4（深入型，60-80h）： - 档位 3 全部 + - 手推 Alg. 2-3 全部，对照 rnea-derivatives.hxx (10h) - CppAD 独立实现 + CppADCodeGen 生成 C (15h) - Singh-Wensing 二阶导数论文精读 (8h) - 自行实现 iLQR on Talos whole-body (10h)

附录 F：Placement 微分恒等式的严格证明 ⭐⭐⭐¶

本附录给出 Carpentier-Mansard Eq. (13) 和 Eq. (14) 的完整严格证明，补充正文 5.4.2 中的 sketch。

F.1 Eq. (13) 的证明¶

定理：对 revolute joint i，设 placement ${}^iX_{\lambda(i)}(q_i)$ 是关节角度 $q_i$ 的函数。则对任意 motion $m \in \mathbb{R}^6$：

\[\frac{\partial}{\partial q_i}\bigl({}^iX_{\lambda(i)}\,m\bigr) = \bigl({}^iX_{\lambda(i)}\,m\bigr) \times S_i\]

证明：

Step 1：分解 placement。关节 i 的 placement 可分解为： $${}^iX_{\lambda(i)} = X_{\text{tree}} \cdot X_J(q_i)$$

其中 $X_{\text{tree}}$ 是固定的 tree transform（不依赖 $q_i$），$X_J(q_i)$ 是关节变换。

Step 2：关节变换的微分。对 revolute joint： $$X_J(q_i) = \exp(\hat{s}\,q_i) \quad \Rightarrow \quad \frac{dX_J}{dq_i} = \hat{s}\,X_J = X_J\,\hat{s}$$

其中最后一个等式利用了 $\hat{s}$ 与 $\exp(\hat{s}\,q_i)$ 的交换性（因为它们在同一个一参子群上）。

但注意：在空间代数中，"乘以 hat{s}"等价于"做一次 motion cross"。具体地：

\[\frac{d}{dq_i}(X_J\,m) = \hat{s}\,X_J\,m = (X_J\,m) \times s\]

（最后一步利用了 spatial cross 的定义：v_hat m = m x v 对于 motion vectors。）

Step 3：合并。 $$\frac{\partial}{\partial q_i}\bigl(X_{\text{tree}}\,X_J\,m\bigr) = X_{\text{tree}}\,(X_J\,m) \times s = (X_{\text{tree}}\,X_J\,m) \times (X_{\text{tree}}\,s)$$

但 $X_{\text{tree}}$ 作用在 $s$ 上后，得到的正是 child frame 中的运动子空间 $S_i$。并且 $X_{\text{tree}} X_J = {}^iX_{\lambda(i)}$。因此：

\[\frac{\partial}{\partial q_i}\bigl({}^iX_{\lambda(i)}\,m\bigr) = \bigl({}^iX_{\lambda(i)}\,m\bigr) \times S_i \quad \blacksquare\]

F.2 Eq. (14) 的证明（对偶版本）¶

定理：对 force $f \in \mathbb{R}^6$：

\[\frac{\partial}{\partial q_i}\bigl({}^{\lambda(i)}X^*_i\,f\bigr) = {}^{\lambda(i)}X^*_i\,(S_i \times^* f)\]

证明：利用 dual transform $X^* = X^{-T}$ 和 force cross 的定义，以及 Eq. (13) 的对偶版本。

具体地：对任何 motion $m$，power 守恒给出： $$\langle {}^{\lambda(i)}X^*_i\,f,\ m \rangle = \langle f,\ {}^iX_{\lambda(i)}\,m \rangle$$

两侧对 $q_i$ 微分，左侧得到 $\langle \partial_{q_i}(X^*_i f), m \rangle + \langle X^*_i f, 0 \rangle$（m 不依赖 q_i）。右侧得到 $\langle f, (X_{\lambda(i)} m) \times S_i \rangle = \langle S_i \times^* f, X_{\lambda(i)} m \rangle = \langle X^*_i (S_i \times^* f), m \rangle$。

由 m 的任意性：$\partial_{q_i}(X^*_i f) = X^*_i (S_i \times^* f)$。$\blacksquare$

F.3 推广到其他关节类型¶

关节类型	$S_i$ 维度	$\partial S_i/\partial q_i$	简化程度
Revolute	6x1	0	最大（标量 cross）
Prismatic	6x1	0	同上
Spherical	6x3	0（for unit quaternion）	中等（3x cross）
Free-flyer	6x6 (= I)	0	S = I，部分项消失
Universal	6x2	非零（取决于参数化）	较少简化

对 revolute 和 prismatic，$\partial S_i/\partial q_i = 0$ 是关键简化——Alg. 2-3 中大量项因此消失。对 spherical 和 universal joint，需要额外处理 $\partial S/\partial q$ 项。

附录 G：2R 平面臂的完整解析微分验证 ⭐⭐¶

本附录对最简单的非平凡模型（2R 平面臂）完整手推 dtau/dq，并与 Carpentier-Mansard 算法的结果逐项对比。

G.1 模型参数¶

两个 revolute joint，关节角 q = (q_1, q_2)
连杆长度 l_1, l_2；质量 m_1, m_2
质心到关节距离 l_{c1}, l_{c2}
绕质心转动惯量 I_1, I_2

G.2 Lagrangian 方法直接推导¶

质量矩阵 M(q)：

\[M_{11} = I_1 + I_2 + m_1 l_{c1}^2 + m_2(l_1^2 + l_{c2}^2 + 2l_1 l_{c2}\cos q_2)$$ $$M_{12} = M_{21} = I_2 + m_2 l_{c2}^2 + m_2 l_1 l_{c2}\cos q_2$$ $$M_{22} = I_2 + m_2 l_{c2}^2\]

动力学方程 tau = M(q) q_ddot + C(q, q_dot) q_dot + g(q)：

\[\frac{\partial\tau_1}{\partial q_2} = \frac{\partial M_{11}}{\partial q_2}\ddot q_1 + \frac{\partial M_{12}}{\partial q_2}\ddot q_2 + \frac{\partial C_1}{\partial q_2}\dot q + \frac{\partial g_1}{\partial q_2}\]

计算各项： $$\frac{\partial M_{11}}{\partial q_2} = -2m_2 l_1 l_{c2}\sin q_2$$ $$\frac{\partial M_{12}}{\partial q_2} = -m_2 l_1 l_{c2}\sin q_2$$

G.3 Carpentier-Mansard 算法推导¶

用 Alg. 2-3 的 spatial vector 方法推导同样的 dtau_1/dq_2：

前向 pass： - $\partial v_2/\partial q_2$：包含 placement derivative term $(X_{2 \leftarrow 1} v_1) \times S_2$ - $\partial a_2/\partial q_2$：包含 $(X_{2 \leftarrow 1} a_1) \times S_2 + ...$

后向 pass： - $\partial f_2/\partial q_2 = I_2 \partial a_2/\partial q_2 + ...$ - $\partial \tau_1/\partial q_2 = S_1^T (\partial f_1/\partial q_2) = S_1^T (X^*_{1 \leftarrow 2} \partial f_2/\partial q_2 + X^*_{1 \leftarrow 2} (S_2 \times^* f_2))$

展开所有 spatial vector 运算后，与 G.2 的结果完全一致——验证了算法的正确性。

G.4 数值验证¶

import pinocchio as pin
import numpy as np

# 构造 2R 臂（用 Pinocchio 的 model builder）
model = pin.Model()
# 添加 joint 1 (revolute around z)
joint_id1 = model.addJoint(0, pin.JointModelRZ(), pin.SE3.Identity(), "joint1")
model.appendBodyToJoint(joint_id1,
    pin.Inertia(m1, np.array([lc1, 0, 0]), np.diag([0, 0, I1])),
    pin.SE3.Identity())
# 添加 joint 2
placement2 = pin.SE3(np.eye(3), np.array([l1, 0, 0]))
joint_id2 = model.addJoint(joint_id1, pin.JointModelRZ(), placement2, "joint2")
model.appendBodyToJoint(joint_id2,
    pin.Inertia(m2, np.array([lc2, 0, 0]), np.diag([0, 0, I2])),
    pin.SE3.Identity())

data = model.createData()
q = np.array([0.5, 0.3])
v = np.array([1.0, -0.5])
a = np.array([0.1, 0.2])

# 解析导数
dtau_dq, _, _ = pin.computeRNEADerivatives(model, data, q, v, a)
print(f"dtau_1/dq_2 (analytic) = {dtau_dq[0, 1]:.6f}")

# 手推公式
dtau1_dq2_manual = -2*m2*l1*lc2*np.sin(q[1]) * a[0] \
                    - m2*l1*lc2*np.sin(q[1]) * a[1] + ...
print(f"dtau_1/dq_2 (manual)   = {dtau1_dq2_manual:.6f}")

附录 H：反向自动微分与 Pontryagin 伴随 ⭐⭐⭐⭐¶

H.1 伴随方程¶

对终端代价优化问题 $\min \Phi(x_T)$，s.t. $\dot x = f(x, u(\theta))$：

\[\dot\mu = -\left(\frac{\partial f}{\partial x}\right)^T \mu, \quad \mu(T) = \nabla\Phi(x_T)\]

\[\frac{d\Phi}{d\theta} = -\int_0^T \mu^T \frac{\partial f}{\partial\theta}\,dt\]

关键洞察：伴随方程就是 Pontryagin 最大值原理中的协态方程，也是 reverse-mode AD 在连续时间下的精确对应。

H.2 与 Neural ODE 的联系¶

Chen et al. (NeurIPS 2018) 的 Neural ODE 把 (x, mu, dL/d theta) 组成增广 ODE 反向积分，实现 O(1) 内存的梯度计算——代价是 2x 的计算时间（前向 + 反向各一次积分）。

H.3 Checkpointing Trade-off¶

策略	内存	计算
Full tape	O(T)	O(T)
Adjoint ODE	O(1)	2T
Checkpointing	O(sqrt(T))	O(T log T)

Griewank-Walther 2008 证明了 checkpointing 的最优性。

H.4 在可微仿真中的应用¶

对一个 N 步离散仿真： $$x_{k+1} = f(x_k, u_k), \quad k = 0, ..., N-1$$ $$L = \sum_k l(x_k, u_k) + \Phi(x_N)$$

反向传播（reverse-mode AD）： $$\lambda_N = \nabla\Phi(x_N)$$ $$\lambda_k = \nabla_x l(x_k, u_k) + \left(\frac{\partial f}{\partial x_k}\right)^T \lambda_{k+1}$$ $$\frac{\partial L}{\partial u_k} = \nabla_u l(x_k, u_k) + \left(\frac{\partial f}{\partial u_k}\right)^T \lambda_{k+1}$$

这里的 $\partial f/\partial x_k$ 和 $\partial f/\partial u_k$ 正是本章的 computeABADerivatives 提供的 A_t 和 B_t！

本质洞察：DDP/iLQR 的 Riccati 反传和 reverse-mode AD 的反向传播在数学上是同一个东西——它们都是 Pontryagin 协态方程的离散形式。区别在于：DDP 同时求解了最优控制律（通过 Q_{uu}^{-1} Q_{ux}），而纯 AD 只提供梯度。

附录 I：隐函数定理在约束动力学导数中的应用 ⭐⭐⭐¶

I.1 约束动力学的 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}\]

其中 h = C q_dot + g，J_c 是接触 Jacobian，lambda 是接触力。

I.2 对参数 theta 应用 IFT¶

定义 $F(\ddot q, \lambda; \theta) = 0$（KKT 条件）。由隐函数定理：

\[\frac{\partial(\ddot q, \lambda)}{\partial\theta} = -\left[\frac{\partial F}{\partial(\ddot q, \lambda)}\right]^{-1} \frac{\partial F}{\partial\theta}\]

其中 $\partial F/\partial(\ddot q, \lambda)$ 就是 KKT 矩阵——它在前向仿真中已经分解过了！

因此约束动力学的导数几乎"免费"：只需要一次已分解的 KKT 矩阵的回代（back-substitution）。

I.3 Pinocchio 3 的实现¶

// Pinocchio 3 约束动力学导数
pinocchio::computeConstraintDynamicsDerivatives(
    model, data, contact_models, contact_datas);
// 输出：data.ddq_partial_dq, data.ddq_partial_dv, data.dlambda_partial_dq, ...

I.4 与可微仿真的连接¶

Amos-Kolter 的 OptNet (ICML 2017) 和 de Avila Belbute-Peres (NeurIPS 2018) 把 QP/LCP 作为可微层，其核心数学工具正是 IFT：

\[\frac{\partial z^*}{\partial\theta} = -\left[\frac{\partial \mathrm{KKT}}{\partial z}\right]^{-1} \frac{\partial \mathrm{KKT}}{\partial\theta}\]

Nimble 和 Dojo 的硬接触可微仿真也基于同一原理。

附录 J：跨章综合练习¶

练习 J.1（综合 4-3 + 4-5 + 控制方向 90_DDP）¶

题目：对 KUKA iiwa 实现一个最简 iLQR： 1. 用 pin.computeABADerivatives 提供 A_t, B_t 2. 用标准 Riccati 反传求最优增益 3. 跟踪一个关节空间轨迹（从初始位形到目标位形） 4. 对比：用有限差分 vs 解析导数，控制效果和计算时间

目标：完整体验"动力学导数 -> DDP -> 控制"的流水线。

练习 J.2（综合 4-4 + 4-5 + 控制方向 120_MPC）¶

题目：对 Solo12 四足： 1. 用 centroidal dynamics 作为简化模型 2. 用 ccrba/dccrba + 本章的导数 API 线性化 3. 实现一个 centroidal MPC（只优化接触力，不优化关节力矩） 4. 在站立平衡下测试推力恢复

目标：理解 centroidal MPC 如何利用本章的导数技术。

练习 J.3（综合 4-5 + CodeGen + 嵌入式）¶

题目： 1. 用 CasADi + Pinocchio.casadi 构建 UR5 的正向动力学符号表达式 2. 生成 C 代码（dynamics.generate('robot.c')） 3. 编译为独立可执行文件（无 Python/Pinocchio 依赖） 4. 对比生成代码与 Pinocchio 解析的性能

目标：体验代码生成 -> 嵌入式部署的完整流水线。

附录 K：与后续专题的桥梁¶

→ 60_约束动力学（接触导数）：Pinocchio 3 的 constrainedDynamicsDerivatives 把本章的无约束导数扩展到带接触约束的情况。核心数学工具是隐函数定理（附录 I），加上 KKT 矩阵的 Schur 补分解。

→ 05_运动控制/90_DDP：iLQR/DDP 的 Riccati 反传直接消费本章的 A_t = d f/d x 和 B_t = d f/d u。Crocoddyl 的 DifferentialActionModelFreeFwdDynamics::calcDiff() 就是本章 computeABADerivatives 的包装。FDDP（Mastalli 2020）和 BoxFDDP 在 constraint handling 上扩展了标准 DDP，但底层导数计算不变。

→ 05_运动控制/120_MPC：acados 的 RTI (Real-Time Iteration) scheme 每步只做一次 QP 迭代——这对导数精度的要求更高（因为没有多次迭代来纠正导数误差）。本章的解析导数（机器精度）直接保证了 RTI 的收敛性。OCS2 的双线程架构（MPC 线程 + policy 线程）中，本章的 computeABADerivatives 是 MPC 线程的热点函数。

→ 系统辨识：本章 5.12 节的 regressor 矩阵 Y 直接服务于在线/离线动力学参数辨识。结合激励轨迹设计（最大化 Fisher 信息矩阵 det(Y^T Y)），可以在真实机器人上精确标定所有惯性参数。

→ 可微仿真与 RL：MJX-JAX 的 RL 训练中，SHAC (Short Horizon Actor-Critic) 需要 truncated-horizon policy gradient，其中的 "d env/d action" 正是本章讨论的正向动力学导数 d q_ddot/d tau = M^{-1}。

附录 L：性能优化进阶技巧 ⭐⭐⭐¶

L.1 SIMD 向量化¶

Pinocchio 通过 Eigen 的 SIMD 支持（SSE/AVX）自动向量化矩阵运算。关键优化点：

6x6 spatial inertia 乘法：一次 AVX-256 指令处理 4 个 double
6xN motion-set 的逐列 cross：可用 SIMD scatter/gather
Cholesky 分解的 BLAS-3 内核

K.2 缓存友好的数据布局¶

computeRNEADerivatives 的内存访问模式：

前向 pass：顺序访问 data.dVdq[1..N]（缓存友好）
后向 pass：逆序访问 data.dFdq[N..1]（缓存友好，因为是连续内存）
关键：dtau_dq 矩阵是按列（Fortran order）还是按行（C order）存储？Eigen 默认列优先，Pinocchio 顺应之。

K.3 多线程并行¶

对不同的时间节点的导数计算可以并行化（它们之间无依赖）：

#pragma omp parallel for
for(int t = 0; t < N; ++t) {
    pinocchio::computeABADerivatives(model, data_pool[t], q[t], v[t], tau[t]);
}

但对单个时间节点的导数计算，Alg. 2-3 的前向/后向 pass 是串行的（树结构依赖）。

K.4 模型简化¶

简化	效果	适用场景
固定不动的关节	减小 nv，M 矩阵更小	只用上半身的 humanoid
合并刚性连接	减少 RNEA pass 的连杆数	简化后的 URDF
只算上三角	减半矩阵操作	M, M^{-1}, dtau_da
利用对称性	镜像四足的左右腿共享	ANYmal 等对称模型