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

JAX 在机械臂优化中的应用¶

本章定位：自动微分（AD）是将机械臂动力学接入现代优化框架（MPC、轨迹优化、参数辨识）的**桥梁技术**。没有 AD，你只能用有限差分求梯度——慢且不精确。有了 AD + 代码生成，你可以让 Pinocchio 的动力学"自动产出"微秒级的解析 Jacobian 和 Hessian。本章从 AD 三大范式入手，深入精读 CppAD / CppADCodeGen / CasADi 在机械臂优化中的具体应用，以 Pinocchio + CppADCodeGen 的"OCS2 范式"为核心主线，覆盖前向/反向 AD 选择、tape 机制、代码生成工作流、性能基准，直到用 AD 加速轨迹优化的完整实战。

与足式足式/40_CppAD与代码生成 CppAD 的关系：足式/40_CppAD与代码生成从 SLAM 工程师的视角介绍了 CppAD（对比 Ceres Jet），侧重于"运算符重载 AD 的基本原理"。本章从**机械臂优化的视角**出发，聚焦于三个足式/40_CppAD与代码生成未覆盖的核心问题：(1) CppADCodeGen 的代码生成工作流（足式/40_CppAD与代码生成未涉及），(2) CasADi 符号 AD 与 CppAD 的选型决策（足式/40_CppAD与代码生成只讲了 CppAD），(3) Pinocchio 的模板标量化如何与 AD 无缝结合（足式/40_CppAD与代码生成不涉及 Pinocchio）。

前置依赖：M01（Pinocchio 标量参数化 §4）、M05（QP/NLP 建模基础）、Ceres Solver 基础（Ceres Jet——最基础的 AD 心智模型）

下游章节：M08（轨迹优化——AD 驱动的碰撞/动力学代价函数）、M14（MoveIt2 集成——pick-ik 的 Pinocchio AD）

建议用时：1 周（AD 范式讲解 1 天 + CppAD 入门 1 天 + CppADCodeGen 实战 2 天 + CasADi/acados 2 天 + 性能基准 1 天）

本章知识导航¶

M06 自动微分与代码生成 知识体系
│
├── §1 为什么离不开 AD ⭐ ──── MPC 的梯度饥渴
│   ├── 三种求导方式对比 (解析/有限差分/AD)
│   └── Pinocchio 的"解析+AD"双轨策略
│
├── §2 AD 三大范式 ⭐⭐ ──── 原理深入
│   ├── 运算符重载 (CppAD/Ceres Jet)
│   ├── 源码变换 (Tapenade/Enzyme/JAX)
│   └── 符号计算 (CasADi)
│
├── §3 前向 vs 反向 AD ⭐⭐⭐ ──── 模式选择
│   └── m<n 用反向, n<m 用前向; RNEA 导数分析
│
├── §4 CppAD 实战 ⭐⭐ ──── tape 录制与回放
│   ├── Independent → 计算 → ADFun → Jacobian
│   └── Pinocchio ModelTpl<ADScalar> 无缝结合
│
├── §5 CppADCodeGen ⭐⭐⭐ ──── 代码生成工作流
│   ├── tape → C 源码 → .so → dlopen
│   └── OCS2 范式: 微秒级求导
│
├── §6 CasADi ⭐⭐ ──── 符号框架
│   ├── SX vs MX 选择
│   └── CasADi + acados 嵌入式 MPC
│
├── §7 梯度验证 ⭐⭐ ──── AD vs 有限差分交叉验证
│
└── §累积项目 ⭐⭐ ──── AD 增强的动力学模块

前置知识桥接¶

回顾 M01 标量参数化：Pinocchio 通过 ModelTpl<Scalar> 模板使得同一套算法代码可以用不同标量类型实例化——double 做数值计算，CppAD::AD<double> 做自动微分，casadi::SX 做符号计算。这种设计是 Pinocchio AD 能力的基础。

回顾 Ceres Jet：Ceres 的 Jet<double, N> 是最简单的前向 AD 实现——一个 dual number {value, gradient[N]}，通过运算符重载在前向计算中同时传播值和梯度。CppAD 是这个思想的工业级扩展。

预计阅读时间¶

模式	时间	适合人群
速查	30 分钟	只需了解 AD 范式选型
精读	5-6 小时	需要理解 CppAD tape 机制和代码生成流程
精读 + 实践	8-10 小时	需要动手实现 AD 驱动的 MPC 梯度计算

前置自测 ⭐¶

📋 答不出 >= 2 题 → 先回前置章节复习

编号	问题	答不出时回顾
1	Ceres Jet：`Jet<double, N>` 的内部结构是什么？它如何在一次前向计算中同时得到函数值和 N 维梯度？	Ceres Solver 基础章节
2	Pinocchio 标量参数化：`ModelTpl<CppAD::AD<double>>` 和 `ModelTpl<double>` 在 API 使用上有什么区别？为什么同一套 `rnea()` 代码能同时支持数值计算和自动微分？	M01 §4 标量参数化
3	链式法则：给定复合函数 \(h(x) = f(g(x))\)，写出 \(\frac{dh}{dx}\) 的链式法则展开。前向模式和反向模式分别如何计算？	微积分 / AD 基础
4	MPC 对梯度的需求：为什么 MPC 需要动力学函数对状态和控制的 Jacobian？如果用有限差分代替解析 Jacobian，7-DOF 机械臂需要多少次额外的动力学评估？	M01 §1, M02 §5.2
5	动态库加载：C++ 中 `dlopen()` / `dlsym()` 的作用是什么？为什么 CppADCodeGen 生成 `.so` 后可以在运行时动态加载？	C++ 基础（动态链接章节）

本章目标¶

学完本章后，你应该能够：

**区分**三种自动微分范式——运算符重载 (CppAD)、源码变换 (Tapenade/Enzyme)、符号计算 (CasADi)——知道各自的优势、劣势和适用场景
实现 CppAD 的 tape 录制/回放工作流：用 Independent() 声明自变量 → 正常计算 → 用 ADFun 封装 → Jacobian() 求导
执行 CppADCodeGen 的完整代码生成流程：录制 tape → 生成 C 源码 → 编译 .so → 运行时 dlopen 加载 → 微秒级求导
选择 CppAD vs CasADi 的最优路径：纯 C++ 栈用 CppAD，Python 原型 + 嵌入式部署用 CasADi + acados
**测量**并解释手写导数 vs AD vs 数值差分的性能差异，理解 AD 在 MPC 中的性能关键性

1. 为什么机械臂优化离不开自动微分 ⭐¶

1.1 MPC 的梯度饥渴 ⭐¶

回顾 M01 §1.1 和 M02 §5.2：MPC 在每个控制周期需要求解一个非线性优化问题。以 SQP (Sequential Quadratic Programming) 为例，每次迭代需要：

量	符号	维度 (7-DOF)	来源
动力学残差	\(f(x, u) - x_{\text{next}}\)	\(14 \times 1\)	RNEA / ABA
残差对状态的 Jacobian	\(\partial f / \partial x\)	\(14 \times 14\)	需要导数！
残差对控制的 Jacobian	\(\partial f / \partial u\)	\(14 \times 7\)	需要导数！
代价函数的 Hessian	\(\nabla^2_{xx} L\)	\(14 \times 14\)	需要二阶导数！

对于 20 步 MPC，每个周期需要 20 次上述计算。没有高效的导数计算，MPC 无法实时运行。

1.2 三种求导方式对比 ⭐¶

方法	实现难度	精度	7-DOF RNEA Jacobian 耗时	适用场景
Pinocchio 内置解析导数	低（调用 API）	机器精度	~4.5 \(\mu\)s	RNEA/ABA/CRBA 等经典算法
有限差分	低	\(O(\epsilon^2)\)	~50 \(\mu\)s (28次额外 RNEA)	原型验证
自动微分 (CppAD tape)	中	机器精度	~12 \(\mu\)s	任意自定义函数求导
AD + 代码生成	中	机器精度	~3 \(\mu\)s (CodeGen .so)	实时 MPC 部署

反事实推理：如果不用 AD 而坚持有限差分求导，会怎样？ - 7-DOF RNEA Jacobian 需要 \(2 \times 14 = 28\) 次额外 RNEA（中心差分，对 \(q\) 和 \(\dot{q}\) 各 7 维扰动） - 耗时：28 x 1.8 \(\mu\)s (Pinocchio RNEA) = 50.4 \(\mu\)s - 20 步 MPC 总导数耗时：50.4 x 20 = 1008 \(\mu\)s \(\approx\) 1 ms - 已经用完整个控制周期！而 Pinocchio 内置解析导数约为 4.5 x 20 = 90 \(\mu\)s；CppAD tape 回放约为 12 x 20 = 240 \(\mu\)s；CodeGen 预编译版本约为 3 x 20 = 60 \(\mu\)s - 更糟糕的是，有限差分精度只有 \(O(\epsilon^2) \approx 10^{-8}\)，而 AD 精度是 \(10^{-16}\)（机器精度）

1.3 Pinocchio 的"解析导数 + AD"双轨策略 ⭐¶

             Pinocchio 导数计算策略
             │
             ├── 内置解析导数（算法专用，调用成本低）
             │   ├── computeRNEADerivatives()  → ∂τ/∂q, ∂τ/∂v, ∂τ/∂a
             │   ├── computeABADerivatives()   → ∂q̈/∂q, ∂q̈/∂v, ∂q̈/∂τ
             │   └── computeForwardKinematicsDerivatives()
             │
             └── 通用 AD（任意函数可求导）
                 ├── ModelTpl<CppAD::AD<double>>   → 在 C++ 中
                 ├── ModelTpl<casadi::SX>           → 在 CasADi 中
                 └── CppADCodeGen → .so             → 代码生成最快

跨领域类比：Pinocchio 的双轨策略类似于数据库的"手写索引 + 自动索引"——对最常用的查询（RNEA/ABA）手动优化了导数（相当于手写索引），对任意查询提供通用的 AD 机制（相当于查询优化器自动选择索引）。

练习¶

⭐ 计算 7-DOF 机械臂使用有限差分求完整状态 Jacobian \(\partial \tau / \partial(q,\dot q)\)（14 维）的 RNEA 调用次数。如果只求 \(\partial \tau / \partial q\)，次数会减半吗？
⭐ 解释为什么 computeRNEADerivatives() 比 CppAD 版本更快——从"编译器可见性"的角度分析。
⭐⭐ 列举三个"Pinocchio 内置解析导数不够用、必须用通用 AD"的场景（提示：碰撞代价、自定义代价函数、参数辨识）。

2. 自动微分三大范式 ⭐⭐¶

2.1 范式总览 ⭐⭐¶

回顾 Ceres Solver 基础中学过的 Ceres Jet——那是**运算符重载 AD** 的一个简化版本。规控领域用的 AD 比这广得多，分为三大范式：

范式	代表工具	工作机制	优势	劣势
运算符重载	CppAD, Ceres Jet, Stan Math	用 AD 类型替换 `double`，重载运算符	透明，不修改代码	运行时 tape 开销
源码变换	Tapenade, Enzyme, JAX (XLA)	编译期分析代码生成导数代码	零运行时开销	对 C++ 模板支持有限
符号计算	CasADi, SymPy, Mathematica	用符号变量构建表达式图	可人工优化、可视化	需用符号类型重写代码

2.2 运算符重载 AD——CppAD 范式 ⭐⭐¶

CppAD 的核心思想：用 CppAD::AD<double> 类型替换 double，所有运算自动记录到 "tape"（计算图）。

#include <cppad/cppad.hpp>

using ADScalar = CppAD::AD<double>;

// Step 1: 声明自变量
std::vector<ADScalar> x = {1.0, 2.0, 3.0};
CppAD::Independent(x);  // 标记为自变量，开始录制 tape

// Step 2: 正常计算——tape 自动记录每一步
std::vector<ADScalar> y(1);
y[0] = sin(x[0]) * exp(x[1]) + x[2] * x[2];
// tape 记录了: sin → mul → exp → mul → add → ...

// Step 3: 封装为可求导对象
CppAD::ADFun<double> f(x, y);  // 结束录制

// Step 4: 在任意 x 处求值和求导
std::vector<double> x_val = {0.5, 1.0, 2.0};
std::vector<double> y_val = f.Forward(0, x_val);   // f(x_val)
std::vector<double> jac   = f.Jacobian(x_val);     // ∂f/∂x
std::vector<double> hess  = f.Hessian(x_val, 0);   // ∂²y[0]/∂x²

与 Ceres Jet 的关键区别（回顾足式/40_CppAD与代码生成）：

维度	Ceres Jet	CppAD
梯度维度	编译期固定 (`Jet<double, N>`)	运行时确定
微分模式	只做前向	前向 + 反向
可重用性	每次求导重新计算	tape 可多次回放
代码生成	无	有 (CppADCodeGen)
高阶导数	需手写嵌套	`AD<AD<double>>` 自动嵌套

对机器人规控的意义：Pinocchio 算法是**固定的**（Panda 动力学不会变），只有 \(q\), \(v\), \(\tau\) 变——CppAD 的"录制一次、无数次回放"模式**完美契合**。相比之下 Ceres Jet 每次求导都重算，对高频控制不够快。

反事实推理：如果 Pinocchio 选择 Ceres Jet 而非 CppAD 作为 AD 后端，会怎样？ - Jet<double, N> 要求 N 在编译期固定——但 model.nv 是运行时确定的 - Jet 每次前向计算都重新求导——但 MPC 中动力学结构不变，tape 录制一次就够了 - Jet 不支持代码生成——无法做 OCS2 的 "录制→生成 C→编译→dlopen" 工作流 - 结论：Ceres Jet 适合 SLAM（固定维度、小规模），CppAD 适合规控（可变维度、需代码生成）

2.3 符号计算 AD——CasADi 范式 ⭐⭐¶

CasADi 用符号类型（SX/MX）构建表达式图，可以显式求导、化简、生成代码：

import casadi as ca

# Step 1: 定义符号变量
x = ca.SX.sym('x', 3)

# Step 2: 用符号变量构建表达式
y = ca.sin(x[0]) * ca.exp(x[1]) + x[2]**2

# Step 3: 求导（符号级别）
J = ca.jacobian(y, x)

# Step 4: 创建可调用函数
f = ca.Function('f', [x], [y, J])

# Step 5: 代码生成
f.generate('my_function.c')
# gcc -O3 -shared -o libmy_function.so my_function.c

SX vs MX：SX 在标量层面展开（小规模密集问题适用），MX 在矩阵层面保持结构（大规模稀疏问题适用）。

2.4 源码变换 AD——Enzyme 与 JAX ⭐⭐⭐¶

工具	语言	状态	机器人规控适用性
Enzyme	C/C++ (LLVM)	研究阶段，快速发展	高潜力——零运行时开销的 C++ AD
JAX	Python	生产就绪	GPU 训练优秀，实时 C++ 部署需额外步骤
JAX/Equinox	Python	生产就绪	神经网络+可微物理的统一框架
Tapenade	Fortran/C	成熟	不支持 C++

Enzyme（LLVM 级别的 AD）：Enzyme（Moses & Churavy, NeurIPS 2021, MIT）在 LLVM IR 层面进行源码变换——它不需要修改源代码或使用特殊类型，直接对编译后的 IR 进行自动微分。这意味着理论上可以对任意 C/C++ 代码（包括 Pinocchio 的模板代码）做零开销 AD。截至 2026 年，Enzyme 在简单 C 代码上已经非常成熟，但对 C++ 模板、虚函数、STL 容器的支持仍在积极开发中。一旦 Enzyme 对 C++ 模板的支持完善，它可能成为 CppAD 的替代者——因为它完全消除了 tape 回放开销和 AD 标量类型的额外内存。

JAX/Equinox 在机器人中的应用：JAX 的 jit + vmap + grad 三件套使得批量可微仿真成为可能。在机械臂领域的典型应用包括： - MuJoCo MJX：全栈可微仿真器，用 JAX 在 GPU 上并行数千个仿真实例 - Brax：Google 的 GPU 刚体物理引擎，用于 RL 训练 - Equinox（Kidger, 2021）：在 JAX 上构建的 PyTorch 风格神经网络库，适合将神经网络策略与可微物理结合 - diffrax：JAX 上的可微常微分方程求解器，用于连续时间最优控制

本质洞察：Enzyme 和 JAX 代表了 AD 的两个未来方向。Enzyme 追求"对现有 C++ 代码零侵入地求导"——适合已有大量 C++ 代码的生产环境。JAX 追求"GPU 上的大规模并行可微计算"——适合训练和数据驱动方法。两者不互斥，而是覆盖了不同的部署场景。

2.5 Pinocchio 生态为何选择 CppAD ⭐⭐¶

本质洞察：Pinocchio 选择 CppAD 而非 CasADi 的根本原因是"代码所有权"——Pinocchio 的动力学算法已经用 C++ 模板写好了，CppAD 只需换标量类型就能求导（零侵入）。而 CasADi 需要用 SX/MX 符号类型**重写**动力学建模代码——虽然 Pinocchio 也支持 CasADi 后端，但这意味着放弃了 Pinocchio 已有的 C++ 优化（CRTP 内联、SIMD 对齐等）。

练习¶

⭐ 用 CppAD 求解 \(f(x) = \sin(x_1) \cdot e^{x_2^2}\) 的梯度。与手写导数和有限差分的结果对比，验证精度。
⭐⭐ 用 CasADi Python 构建同一函数的符号表达式，用 jacobian() 求导，生成 C 代码并编译。比较 CasADi 生成代码和 CppAD tape 回放的执行速度。
⭐⭐ 解释为什么 JAX 在 RL 训练中广泛使用但在实时 C++ MPC 中不常见（提示：Python GIL、部署依赖、C++ 互操作性）。

3. 前向 AD vs 反向 AD 的选择策略 ⭐⭐⭐¶

3.1 数学基础 ⭐⭐⭐¶

给定函数 \(f: \mathbb{R}^n \to \mathbb{R}^m\)，计算 Jacobian \(J = \frac{\partial f}{\partial x} \in \mathbb{R}^{m \times n}\)：

前向模式 (Forward Mode)：每次计算 \(J\) 的一**列**，需要 \(n\) 次前向传播。

反向模式 (Reverse Mode)：每次计算 \(J\) 的一**行**，需要 \(m\) 次反向传播。

选择策略：

条件	推荐模式	原因
\(n < m\) (输入少输出多)	前向	\(n\) 次传播 < \(m\) 次传播
\(m < n\) (输出少输入多)	反向	\(m\) 次传播 < \(n\) 次传播
\(m = 1\) (标量输出)	反向	一次反向传播得到完整梯度

3.2 机械臂动力学中的选择 ⭐⭐¶

函数	输入维度 \(n\)	输出维度 \(m\)	推荐模式
RNEA: \(\tau = f(q, v, a)\)	21 (3x7)	7	反向模式 (m < n)
ABA: \(\ddot{q} = g(q, v, \tau)\)	21	7	反向模式
FK 位置: \(p = h(q)\)	7	3	反向模式
标量代价: \(L = c(q)\)	7	1	反向模式 (经典反向传播)

跨领域类比：前向 AD 和反向 AD 的关系，就像信号处理中的 FIR 和 IIR 滤波器——FIR（前向）结构简单但需要更多计算，IIR（反向）结构复杂但更高效。在深度学习中，反向传播 (backprop) 就是反向模式 AD——因为 loss 是标量（\(m=1\)），一次反向传播就能得到对所有参数的梯度。

CppAD 支持两种模式：

// 前向模式: 计算 J 的一列
std::vector<double> dx(n, 0.0);
dx[i] = 1.0;  // 对第 i 个输入求导
std::vector<double> dy = f.Forward(1, dx);
// dy = J[:, i]

// 反向模式: 计算 J 的一行
std::vector<double> w(m, 0.0);
w[j] = 1.0;  // 对第 j 个输出的导数加权
std::vector<double> dw = f.Reverse(1, w);
// dw = J[j, :]

3.3 Pinocchio 内置导数的特殊性 ⭐⭐⭐¶

computeRNEADerivatives() 使用的是**专门推导的解析导数**，不属于前向或反向 AD。Carpentier & Mansard (2018) 通过手工推导了 RNEA 递推过程的导数，复杂度仍为 \(O(N)\)，与 RNEA 本身相同。

这比通用 AD 更快的原因是：手写导数可以利用 RNEA 的特殊结构（空间向量的反对称性、递推的稀疏性），而通用 AD 把 RNEA 当作"黑盒"处理。

练习¶

⭐⭐ 对于 Franka Panda 的 RNEA (\(n=21, m=7\))，计算前向模式和反向模式分别需要多少次基本前向计算来得到完整 Jacobian。
⭐⭐ 用 CppAD 分别用前向模式和反向模式计算一个简单函数的梯度，验证结果一致。测量两种模式的耗时差异。
⭐⭐⭐ 解释为什么 computeRNEADerivatives() 比 CppAD 反向模式更快，尽管两者的算法复杂度理论上都是 \(O(N)\)。

4. CppAD 深度精读——tape 机制与 Pinocchio 集成 ⭐⭐¶

4.1 tape 的数据结构 ⭐⭐¶

CppAD 的 tape 本质上是一个**有向无环图 (DAG)**，记录了从输入到输出的所有运算步骤：

输入: x = [x0, x1]
计算: y = sin(x0) * x1 + x0^2

Tape DAG:
  x0 ──→ sin ──→ t1 ──→ mul ──→ t2 ──→ add ──→ y
  x1 ───────────────────→ mul
  x0 ──→ mul ──→ t3 ──────────→ add

tape 回放：前向回放计算函数值，反向回放计算梯度。tape 的大小与计算步骤数成正比——对于 7-DOF RNEA，tape 约有数千个运算节点。

4.2 Pinocchio + CppAD 集成 ⭐⭐¶

回顾 M01 §4.5，Pinocchio 的 ModelTpl<Scalar> 设计使得 CppAD 集成几乎透明：

#include <pinocchio/autodiff/cppad.hpp>
#include <pinocchio/algorithm/rnea.hpp>

// 1. 创建 AD 模型
typedef CppAD::AD<double> ADScalar;
typedef pinocchio::ModelTpl<ADScalar> ADModel;
typedef pinocchio::DataTpl<ADScalar> ADData;

pinocchio::Model model;
pinocchio::urdf::buildModel("panda.urdf", model);
ADModel ad_model = model.cast<ADScalar>();
ADData ad_data(ad_model);

// 2. 声明自变量并录制 tape
// CppAD::Independent 的自变量容器应使用 CppAD 支持的 SimpleVector。
// 工程上最稳妥的写法是 std::vector<ADScalar>，再用 Eigen::Map
// 把它映射成 Pinocchio 需要的 Eigen 向量视图。
std::vector<ADScalar> ad_x(model.nq);
for (int i = 0; i < model.nq; ++i)
    ad_x[i] = ADScalar(q_nominal[i]);
CppAD::Independent(ad_x);  // 开始录制

Eigen::Map<Eigen::Matrix<ADScalar, Eigen::Dynamic, 1>> ad_q(
    ad_x.data(), model.nq);

// 3. 调用 RNEA——tape 自动记录
Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_v =
    Eigen::VectorXd::Zero(model.nv).cast<ADScalar>();
Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> ad_a =
    Eigen::VectorXd::Random(model.nv).cast<ADScalar>();
pinocchio::rnea(ad_model, ad_data, ad_q, ad_v, ad_a);

// 4. 封装 AD 函数
std::vector<ADScalar> tau_out(model.nv);
for (int i = 0; i < model.nv; ++i)
    tau_out[i] = ad_data.tau[i];
CppAD::ADFun<double> rnea_ad(ad_x, tau_out);

// 5. 求 Jacobian: ∂τ/∂q (7x7 矩阵)
std::vector<double> q_nominal_vec(
    q_nominal.data(), q_nominal.data() + model.nq);
std::vector<double> jac = rnea_ad.Jacobian(q_nominal_vec);
// jac 展平为 49 维向量，reshape 为 7x7

⚠️ 编程陷阱：model.cast<ADScalar>() 会**深拷贝**整个模型

对于大型机器人（30+ 关节），此操作可能耗时数百微秒。应在初始化阶段完成一次，而非在控制循环中重复调用。

4.3 tape 的性能开销 ⭐⭐¶

CppAD tape 回放耗时通常是原始 double 计算的 3-10 倍：

计算	double	CppAD tape 回放	倍数
7-DOF RNEA	~1.8 \(\mu\)s	~8 \(\mu\)s	4.4x
7-DOF ABA	~2.5 \(\mu\)s	~12 \(\mu\)s	4.8x
7-DOF FK + Jacobian	~1.8 \(\mu\)s	~10 \(\mu\)s	5.6x

这个开销来自：(1) tape 遍历需要解释每个运算节点（类似虚拟机字节码），(2) tape 中的运算无法被编译器内联/向量化。这就是 CppADCodeGen 的价值所在。

练习¶

⭐⭐ 用 Pinocchio + CppAD 录制 Franka Panda RNEA 的 tape，计算 \(\partial \tau / \partial q\)。与 computeRNEADerivatives 的结果对比，验证误差 < \(10^{-10}\)。
⭐⭐ 测量 tape 录制和 tape 回放的分别耗时。录制只需一次，回放是重复的——理解这个区别对 MPC 的意义。
⭐⭐⭐ 结合 M04 的碰撞距离计算，实现"最近点 + point Jacobian"链式梯度，并用有限差分验证 \(\partial d_{\text{collision}} / \partial q\)。如果你的目标 Pinocchio/Coal 版本支持可微距离查询，再把同一测试作为 AD tape 的版本门控。

5. CppADCodeGen——从 tape 到优化的 C 代码 ⭐⭐⭐¶

5.1 为什么需要代码生成 ⭐⭐⭐¶

CppAD tape 回放的 3-10x 开销在 MPC 中不可接受。CppADCodeGen 的解决方案：将 tape 转换为优化后的 C 源码，编译为动态库，运行时加载。

CppAD tape (运行时解释)     CppADCodeGen (编译期优化)
    ┌─────────┐                 ┌──────────┐
    │ Op: sin  │                 │ C 源码    │
    │ Op: mul  │  ──CodeGen──→  │ (CSE/DCE  │  ──gcc -O3──→  .so
    │ Op: add  │                 │  优化后)  │
    │ ...      │                 └──────────┘
    └─────────┘                      │
    ~8 μs (回放)                     │ dlopen
                                     ▼
                                 ~2.5 μs (原生执行)

5.2 代码生成工作流 ⭐⭐⭐¶

#include <cppad/cg.hpp>

// Step 1: 用 CG 标量类型构建模型
using CGScalar = CppAD::cg::CG<double>;
using ADCGScalar = CppAD::AD<CGScalar>;

pinocchio::ModelTpl<ADCGScalar> model_cg =
    model.cast<ADCGScalar>();
pinocchio::DataTpl<ADCGScalar> data_cg(model_cg);

// Step 2: 录制 tape
std::vector<ADCGScalar> x(model.nq + model.nv + model.nv);
CppAD::Independent(x);
// ... 拆分 x 为 q, v, a 并调用 rnea ...
CppAD::ADFun<CGScalar> f(x, tau_out);

// Step 3: 从 tape 生成 C 源码
CppAD::cg::ModelCSourceGen<double> cgen(f, "rnea_panda");
cgen.setCreateJacobian(true);
CppAD::cg::ModelLibraryCSourceGen<double> libcgen(cgen);

// Step 4: 编译为 .so
CppAD::cg::DynamicModelLibraryProcessor<double> p(libcgen);
CppAD::cg::GccCompiler<double> compiler;
compiler.addCompileFlag("-O3");
compiler.addCompileFlag("-march=native");
auto dynamicLib = p.createDynamicLibrary(compiler);

// Step 5: 运行时加载并调用
auto rnea_model = dynamicLib->model("rnea_panda");
std::vector<double> x_val(21);  // [q, v, a]
std::vector<double> tau(7);
rnea_model->ForwardZero(x_val, tau);    // 求值
std::vector<double> jac(7 * 21);
rnea_model->Jacobian(x_val, jac);       // 求 Jacobian

5.3 代码生成的优化 ⭐⭐⭐¶

CppADCodeGen 自动应用以下优化：

优化	说明	典型效果
CSE (Common Subexpression Elimination)	消除重复计算	减少 30-50% 运算
DCE (Dead Code Elimination)	删除无用运算	减少 10-20% 代码
常量折叠	编译期计算已知常量	减少运行时运算
算术简化	`x * 1 → x`, `x + 0 → x`	微优化

生成的 C 代码是**无分支、无循环、纯算术**函数——编译器可以完全向量化。

5.4 OCS2 的工业实践 ⭐⭐⭐¶

OCS2 (leggedrobotics/ocs2) 是 ETH 开发的腿足 MPC 框架，其性能核心就是 Pinocchio + CppADCodeGen：

OCS2 MPC 启动流程:
  1. 加载 URDF → Pinocchio Model
  2. model.cast<CppAD::AD<CppAD::cg::CG<double>>>()
  3. 录制 RNEA/ABA/FK/碰撞 的 tape
  4. CppADCodeGen 生成 C 代码
  5. gcc -O3 编译为 derivatives.so
  6. 控制循环中 dlopen 加载，微秒级求导

本质洞察：CppADCodeGen 可能比手写导数**更快**——因为 CodeGen 生成的代码经过全局 CSE/DCE 优化，可能比人工推导的导数更高效。人类很难在数千行导数代码中发现所有公共子表达式。

⚠️ 概念误区：认为"手写导数一定比 AD 快"

实际上：手写导数的优势在于利用**算法结构**（如 RNEA 的递推稀疏性），CodeGen 的优势在于**表达式级别的优化**（CSE/DCE）。在某些 benchmark 中 CodeGen 更快。两者互补而非替代。

5.5 CppADCodeGen 的完整代码示例——从录制到运行 ⭐⭐⭐¶

下面给出一个完整可编译的 CppADCodeGen 工作流，将录制、生成、编译、加载四个阶段串联起来。这是理解 OCS2 内部机制的关键代码。

#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/autodiff/cppad.hpp>
#include <cppad/cg.hpp>
#include <iostream>
#include <chrono>

int main() {
    // ========== Phase 1: 加载 Pinocchio 模型 ==========
    pinocchio::Model model;
    pinocchio::urdf::buildModel("panda.urdf", model);
    std::cout << "DOF: " << model.nv << std::endl;

    // ========== Phase 2: 创建 CodeGen 标量类型的模型 ==========
    using CGD = CppAD::cg::CG<double>;
    using ADCGD = CppAD::AD<CGD>;

    pinocchio::ModelTpl<ADCGD> ad_model = model.cast<ADCGD>();
    pinocchio::DataTpl<ADCGD> ad_data(ad_model);

    // 输入向量: [q(7), v(7), a(7)] = 21 维
    const int input_dim = model.nq + model.nv + model.nv;
    const int output_dim = model.nv;  // tau(7)

    // ========== Phase 3: 录制 tape ==========
    std::vector<ADCGD> x(input_dim);
    for (int i = 0; i < input_dim; ++i) x[i] = ADCGD(0.0);
    CppAD::Independent(x);  // 开始录制

    // 拆分输入向量
    Eigen::Map<Eigen::Matrix<ADCGD, Eigen::Dynamic, 1>>
        ad_q(x.data(), model.nq);
    Eigen::Map<Eigen::Matrix<ADCGD, Eigen::Dynamic, 1>>
        ad_v(x.data() + model.nq, model.nv);
    Eigen::Map<Eigen::Matrix<ADCGD, Eigen::Dynamic, 1>>
        ad_a(x.data() + model.nq + model.nv, model.nv);

    // 调用 RNEA——tape 自动记录所有运算
    pinocchio::rnea(ad_model, ad_data, ad_q, ad_v, ad_a);

    // 提取输出
    std::vector<ADCGD> tau_out(output_dim);
    for (int i = 0; i < output_dim; ++i)
        tau_out[i] = ad_data.tau[i];

    CppAD::ADFun<CGD> rnea_fun(x, tau_out);  // 结束录制
    rnea_fun.optimize();  // 优化 tape 结构

    // ========== Phase 4: 生成 C 代码并编译 ==========
    CppAD::cg::ModelCSourceGen<double> cgen(rnea_fun, "rnea_7dof");
    cgen.setCreateForwardZero(true);   // 生成 f(x) 函数
    cgen.setCreateJacobian(true);      // 生成 J(x) 函数
    cgen.setCreateSparseJacobian(true); // 生成稀疏 Jacobian

    CppAD::cg::ModelLibraryCSourceGen<double> libcgen(cgen);
    CppAD::cg::DynamicModelLibraryProcessor<double> p(libcgen);

    CppAD::cg::GccCompiler<double> compiler;
    compiler.addCompileFlag("-O3");
    compiler.addCompileFlag("-march=native");
    compiler.addCompileFlag("-DNDEBUG");

    auto lib = p.createDynamicLibrary(compiler);
    // 此处 gcc 编译可能需要 1-10 秒（取决于复杂度）

    // ========== Phase 5: 运行时加载并调用 ==========
    auto rnea_model = lib->model("rnea_7dof");

    // 准备输入
    std::vector<double> x_val(input_dim, 0.0);
    // 设置 q 为随机值, v=0, a=0 (重力补偿)

    // 求值
    std::vector<double> tau_val(output_dim);
    rnea_model->ForwardZero(x_val, tau_val);

    // 求 Jacobian
    std::vector<double> jac_val(output_dim * input_dim);
    rnea_model->Jacobian(x_val, jac_val);
    // jac_val 展平为 7x21 矩阵: [dtau/dq | dtau/dv | dtau/da]

    // ========== Phase 6: 性能测量 ==========
    auto start = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 10000; ++i) {
        rnea_model->ForwardZero(x_val, tau_val);
    }
    auto end = std::chrono::high_resolution_clock::now();
    double fwd_us = std::chrono::duration<double, std::micro>(
        end - start).count() / 10000;

    start = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 10000; ++i) {
        rnea_model->Jacobian(x_val, jac_val);
    }
    end = std::chrono::high_resolution_clock::now();
    double jac_us = std::chrono::duration<double, std::micro>(
        end - start).count() / 10000;

    std::cout << "ForwardZero: " << fwd_us << " us" << std::endl;
    std::cout << "Jacobian:    " << jac_us << " us" << std::endl;

    return 0;
}

⚠️ 编程陷阱：CppADCodeGen 生成的 C 代码可能非常大

错误描述：对 30-DOF 人形机器人做完整动力学的代码生成，生成的 .c 文件可能超过 100 MB

现象：gcc -O3 编译时内存耗尽（需要 16+ GB RAM），或编译时间超过 30 分钟

根本原因：CSE 优化虽然减少了运算次数，但生成的中间变量名和赋值语句仍然很多

正确做法：(1) 降低优化级别 (-O1 代替 -O3)，(2) 将动力学分拆为多个小函数分别生成代码，(3) 使用 setMaxAssignmentsPerFunc() 限制单函数大小，(4) 增大编译机内存

5.6 CodeGen 编译流程详解——从生成到加载的每一步 ⭐⭐⭐¶

CppADCodeGen 的编译流程涉及多个阶段，每个阶段都有特定的配置选项和常见问题。以下是完整的流程分解：

┌─────────────────┐
│  CppAD tape      │  Phase 1: 录制计算图
│  (ADFun<CG<d>>)  │  耗时: ~5-50 ms (取决于模型复杂度)
└────────┬────────┘
         │ tape → DAG 分析
         ▼
┌─────────────────┐
│  C 源码生成       │  Phase 2: 表达式级优化 + C 代码输出
│  (CSE/DCE/常量折叠)│  耗时: ~100 ms - 10 s
│  输出: rnea.c     │  大小: 几 KB 到 100+ MB
└────────┬────────┘
         │ gcc / clang 编译
         ▼
┌─────────────────┐
│  共享库编译        │  Phase 3: C → 机器码
│  gcc -O3 -shared  │  耗时: 1 s - 30 min (!)
│  输出: rnea.so    │  大小: 几 KB 到 10+ MB
└────────┬────────┘
         │ dlopen / dlsym
         ▼
┌─────────────────┐
│  运行时加载        │  Phase 4: 动态链接
│  GenericModel     │  耗时: ~1 ms
│  ForwardZero()    │  调用耗时: ~2-3 μs
│  Jacobian()       │
└─────────────────┘

Phase 2 优化细节：

CppADCodeGen 的 C 代码生成器对 tape 中的 DAG 执行以下优化 pass：

优化 pass	示例	效果
CSE	`t1 = sin(q[0]); t5 = sin(q[0]);` → `t1 = sin(q[0]);`（复用 t1）	7-DOF RNEA 减少 ~40% 运算
DCE	删除计算了但从未被输出引用的中间变量	减少 ~15% 代码
常量折叠	`t = 9.81 * 0.5;` → `t = 4.905;`	消除运行时乘法
算术简化	`x * 1.0` → `x`; `x + 0.0` → （删除）	微优化

Phase 3 编译选项的选择：

CppAD::cg::GccCompiler<double> compiler;

// 选项 1: 最大优化（推荐用于部署）
compiler.addCompileFlag("-O3");          // 最高优化级别
compiler.addCompileFlag("-march=native"); // 利用本机 CPU 特性
compiler.addCompileFlag("-DNDEBUG");     // 禁用断言
compiler.addCompileFlag("-ffast-math");  // 允许浮点优化
// 编译耗时: ~10-30 s (7-DOF)，~5-30 min (30-DOF)
// 运行耗时: ~2.5 μs (7-DOF Jacobian)

// 选项 2: 快速编译（推荐用于开发迭代）
compiler.addCompileFlag("-O1");
compiler.addCompileFlag("-DNDEBUG");
// 编译耗时: ~2-5 s (7-DOF)
// 运行耗时: ~5 μs (7-DOF Jacobian) ← 比 -O3 慢 2x

// 选项 3: 超大模型（30+ DOF）
compiler.addCompileFlag("-O1");          // 不用 -O3 (内存会爆)
compiler.addCompileFlag("-pipe");        // 管道而非临时文件
// 如果仍然内存不足:
cgen.setMaxAssignmentsPerFunc(1000);     // 分拆为多个小函数

⚠️ 编程陷阱：-ffast-math 可能改变数值结果

错误描述：在 CodeGen 编译中使用 -ffast-math 后，导数值与 tape 版本不一致

现象：误差从 \(10^{-16}\) 增大到 \(10^{-12}\)，在大多数场景下可接受，但在精度敏感的应用（如参数辨识）中可能导致收敛问题

根本原因：-ffast-math 允许编译器重排浮点运算顺序（违反 IEEE 754 严格语义），例如 (a + b) + c 可能被优化为 a + (b + c)——浮点加法不满足结合律

正确做法：MPC 控制中通常可用（\(10^{-12}\) 精度绰绰有余）；参数辨识/标定中去掉此选项

Phase 4 运行时加载的 API：

// 加载生成的 .so 并获取模型接口
auto lib = std::make_unique<
    CppAD::cg::LinuxDynamicLib<double>>("./librnea_7dof.so");
auto model = lib->model("rnea_7dof");

// 查询模型信息
size_t n_in = model->Domain();    // 输入维度 (21)
size_t n_out = model->Range();    // 输出维度 (7)

// 前向求值
std::vector<double> x(n_in);     // [q, v, a]
std::vector<double> y(n_out);    // tau
model->ForwardZero(x, y);

// Jacobian 求值
std::vector<double> jac(n_out * n_in);  // 7 x 21
model->Jacobian(x, jac);
// jac[i * n_in + j] = ∂y[i] / ∂x[j]

// 稀疏 Jacobian（更高效）
std::vector<double> vals;
std::vector<size_t> rows, cols;
model->SparseJacobian(x, vals, rows, cols);
// 只返回非零元素及其行列索引

5.7 Sparse Jacobian——稀疏性的利用 ⭐⭐⭐¶

机器人动力学的 Jacobian 可能具有部分稀疏结构——例如串行链中某些 \(\partial \tau_i / \partial q_j\) 项可能为零，但这取决于具体模型拓扑和状态。需注意：RNEA 的 \(\partial \tau / \partial q\) 一般并非严格下三角，因为科里奥利/离心项引入了全局耦合。具体的稀疏模式应通过运行时分析确认。CppADCodeGen 可以自动检测并利用这种稀疏性：

// 生成稀疏 Jacobian（只计算非零元素）
cgen.setCreateSparseJacobian(true);

// 运行时获取稀疏模式
auto model = lib->model("rnea_7dof");
auto sparsity = model->JacobianSparsitySet();
// sparsity 告诉你哪些 (i,j) 元素非零

// 稀疏求值（只计算非零元素，更快）
std::vector<double> sparse_jac;
std::vector<size_t> row, col;
model->SparseJacobian(x_val, sparse_jac, row, col);

对于 7-DOF 串行链，RNEA Jacobian \(\partial \tau / \partial q\) 一般是 \(7 \times 7\) 稠密矩阵（科里奥利和离心项会产生全局耦合）。其稀疏结构取决于具体模型和当前状态——某些项可能在特定构型下为零，但不能一般性地假设为下三角。建议通过 SparseJacobian 在运行时确定实际的稀疏模式，据此优化计算。

5.8 二阶导数——Hessian 的代码生成 ⭐⭐⭐⭐¶

轨迹优化中的 Newton 方法和 SQP 需要 Hessian（二阶导数）。CppADCodeGen 支持 Hessian 的代码生成：

cgen.setCreateHessian(true);

// 运行时调用
// 注意: Hessian 是 input_dim x input_dim (21x21)
std::vector<double> hess(input_dim * input_dim);
// w 是输出维度的权重向量（用于加权 Hessian）
std::vector<double> w(output_dim, 1.0);
rnea_model->Hessian(x_val, w, hess);

💡 思维陷阱：认为"二阶导数一定比一阶导数慢很多"

新手想法："Hessian 是 \(n \times n\) 矩阵，计算量应该是 Jacobian 的 \(n\) 倍"

实际上：CppADCodeGen 的 Hessian 代码生成利用了对称性和稀疏性。对于 7-DOF RNEA，Hessian 只比 Jacobian 慢 2-3 倍（而非 7 倍），因为很多二阶偏导数为零或相等。

正确理解：当你在 tape 中设置 setCreateHessian(true) 时，CodeGen 会分析整个计算图的二阶结构，只生成非零、非重复的二阶偏导数代码。

练习¶

⭐⭐ 为 Franka Panda 的 RNEA 生成 CppADCodeGen 的 .so 文件。测量 ForwardZero 和 Jacobian 的耗时。
⭐⭐⭐ 查看生成的 C 代码，估算运算次数。找到 CSE 优化消除的重复计算。
⭐⭐⭐ 精读 OCS2 的 ocs2_pinocchio_interface/ 源码，标注完整的"模型→cast→录制→代码生成→dlopen"路径。
⭐⭐ 生成稀疏 Jacobian，打印稀疏模式，并用有限差分抽查几个非零/近零元素。不要预设 \(\partial \tau_1 / \partial q_7 = 0\)：对串行机械臂，基座侧力矩常会受远端关节姿态影响，实际稀疏结构应以目标模型和状态为准。对比稀疏 vs 密集 Jacobian 的计算耗时。

6. CasADi 在机械臂优化中的应用 ⭐⭐¶

6.1 CasADi vs CppAD 选型 ⭐⭐¶

维度	CppAD + CppADCodeGen	CasADi
建模语言	C++ (Pinocchio 原生)	Python (SX/MX) 或 C++
与 Pinocchio 集成	原生（标量模板）	`pinocchio-casadi` 包
代码生成目标	导数函数 .so	NLP 函数 .so
交互式调试	困难（C++ 编译）	容易（Python/Jupyter）
部署依赖	无（纯 C .so）	CasADi C 运行时

选型决策：

你的工作流是什么？
│
├── 纯 C++ 栈，Pinocchio 动力学
│   └── CppAD + CppADCodeGen（OCS2 范式）
│
├── Python 原型 + C++ 部署
│   └── CasADi + Ipopt 或 CasADi + acados
│
├── 嵌入式实时 MPC（STM32 级）
│   └── acados（生成自包含 C 代码）
│
└── 快速原型/学术论文
    └── CasADi Python（最短开发周期）

6.2 Pinocchio + CasADi 集成 ⭐⭐¶

import pinocchio as pin
import pinocchio.casadi as cpin
import casadi as ca
import numpy as np

# 1. 加载并转换模型
model = pin.buildModelFromUrdf("panda.urdf")
cmodel = cpin.Model(model)
cdata = cmodel.createData()

# 2. 定义符号变量
q = ca.SX.sym("q", model.nq)
v = ca.SX.sym("v", model.nv)
a = ca.SX.sym("a", model.nv)

# 3. 符号 RNEA + 求导
cpin.rnea(cmodel, cdata, q, v, a)
tau = cdata.tau
dtau_dq = ca.jacobian(tau, q)   # 7x7 符号 Jacobian
dtau_dv = ca.jacobian(tau, v)   # 7x7 符号 Jacobian

# 4. 创建可调用函数
rnea_fn = ca.Function("rnea",
    [q, v, a],          # 输入
    [tau, dtau_dq, dtau_dv],  # 输出: 函数值 + 两个 Jacobian
    ["q", "v", "a"],    # 输入名称
    ["tau", "dtau_dq", "dtau_dv"]  # 输出名称
)

# 5. 数值评估
q_val = np.zeros(7)
v_val = np.zeros(7)
a_val = np.random.randn(7)
tau_val, J_q, J_v = rnea_fn(q_val, v_val, a_val)

# 6. 代码生成（可选）
opts = {"with_header": True}
rnea_fn.generate('rnea_casadi.c', opts)
# gcc -O3 -shared -fPIC -o librnea_casadi.so rnea_casadi.c

# 7. 验证与 Pinocchio double 版本一致
data = model.createData()
pin.rnea(model, data, q_val, v_val, a_val)
print("Pinocchio tau:", data.tau)
print("CasADi tau:   ", np.array(tau_val).flatten())
# 应完全一致（误差 < 1e-14）

CasADi 的独特优势——交互式探索：

与 CppAD 不同，CasADi 在 Python/Jupyter 中可以**交互式**查看符号表达式结构：

# 查看 tau 的符号表达式大小
print(f"tau 表达式包含 {tau.n_dep()} 个运算节点")
print(f"dtau_dq 表达式包含 {dtau_dq.n_dep()} 个运算节点")

# 可视化表达式图（小规模时有用）
# ca.cse(tau)  # 手动触发 CSE

# 打印特定元素的符号表达式
print("tau[0] =", tau[0])  # 看第一个关节力矩的公式

这种交互式能力在调试和验证阶段非常有价值——你可以看到"CasADi 理解的动力学长什么样"，确认它没有遗漏任何项。

6.3 CasADi 的 SX vs MX——何时用哪个 ⭐⭐¶

CasADi 有两种符号类型，选错会导致严重的性能问题：

维度	SX	MX
粒度	标量级（每个 \(a_{ij}\) 是独立符号）	矩阵级（整个 \(A\) 是一个符号节点）
表达式大小	可能爆炸（\(n^2\) 个标量）	紧凑（1 个矩阵节点）
适用规模	小（\(n < 20\)）	大（\(n > 20\)）
代码生成质量	最优（标量级 CSE）	良好（矩阵级优化）
Pinocchio 兼容	是（`pinocchio-casadi` 用 SX）	受限

⚠️ 编程陷阱：对 30-DOF 人形机器人用 SX 做完整动力学

现象：Python 进程内存暴涨到 16+ GB，CasADi 构建符号表达式耗时数分钟

根本原因：SX 将 \(30 \times 30\) 惯量矩阵的 900 个元素都展开为独立的符号表达式树，每个树包含数百个节点

正确做法：对 DOF > 15 的系统，改用 MX 或改用 CppAD（CppAD 不做符号展开，tape 大小与计算步骤成正比而非与矩阵元素数成正比）

6.4 acados——MPC 代码生成集大成者 ⭐⭐⭐¶

acados (acados/acados) 生成**完整的 MPC 求解器栈**：

CasADi Python 建模
  ↓ acados Python API
acados 代码生成
  ↓
完全自包含 C 代码:
  ├── SQP-RTI 外层
  ├── HPIPM QP 求解器
  ├── BLASFEO 线性代数
  └── 动力学 + 代价函数
  ↓
可部署到 STM32 Cortex-M7

SQP-RTI (Real-Time Iteration)：每个控制周期只执行一步 SQP 迭代，依靠 MPC 滚动时域的 warm-start。

典型用户：openpilot (comma.ai) 的量产自动驾驶辅助系统。

6.5 CasADi vs CppAD 实战对比——同一任务两种实现 ⭐⭐⭐¶

理论上的选型表需要实战验证。下面我们用同一个任务——Franka Panda 的 RNEA Jacobian 计算——分别用 CppAD 和 CasADi 实现，从开发体验、性能、代码量三个维度做直接对比。

任务：计算 \(\partial \tau / \partial q \in \mathbb{R}^{7 \times 7}\)，其中 \(\tau = \text{RNEA}(q, 0, 0)\)（零速零加速的重力补偿力矩对关节角的 Jacobian）。

CppAD 实现 (C++)：

// cppad_rnea_jacobian.cpp — 约 40 行核心代码
#include <pinocchio/autodiff/cppad.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include <chrono>

int main() {
    // 加载模型
    pinocchio::Model model;
    pinocchio::urdf::buildModel("panda.urdf", model);

    // 创建 AD 模型
    using ADScalar = CppAD::AD<double>;
    auto ad_model = model.cast<ADScalar>();
    pinocchio::DataTpl<ADScalar> ad_data(ad_model);

    // 录制 tape
    std::vector<ADScalar> x(model.nq);
    for (int i = 0; i < model.nq; ++i) x[i] = ADScalar(0.0);
    CppAD::Independent(x);

    Eigen::Map<Eigen::Matrix<ADScalar, -1, 1>>
        ad_q(x.data(), model.nq);
    auto ad_v = Eigen::VectorXd::Zero(model.nv).cast<ADScalar>();
    auto ad_a = Eigen::VectorXd::Zero(model.nv).cast<ADScalar>();

    pinocchio::rnea(ad_model, ad_data, ad_q, ad_v, ad_a);

    std::vector<ADScalar> tau_out(model.nv);
    for (int i = 0; i < model.nv; ++i)
        tau_out[i] = ad_data.tau[i];
    CppAD::ADFun<double> rnea_ad(x, tau_out);

    // 求 Jacobian
    Eigen::VectorXd q0 = Eigen::VectorXd::Zero(model.nq);
    auto jac = rnea_ad.Jacobian(
        std::vector<double>(q0.data(), q0.data() + model.nq));
    // jac 是 49 维向量 (7x7 展平)

    // 性能测量
    auto t0 = std::chrono::high_resolution_clock::now();
    for (int i = 0; i < 10000; ++i)
        rnea_ad.Jacobian(
            std::vector<double>(q0.data(), q0.data() + model.nq));
    auto t1 = std::chrono::high_resolution_clock::now();
    double us = std::chrono::duration<double, std::micro>(
        t1 - t0).count() / 10000;
    std::cout << "CppAD tape Jacobian: " << us << " us" << std::endl;
    // 典型输出: ~12 us
}

CasADi 实现 (Python)：

# casadi_rnea_jacobian.py — 约 25 行核心代码
import pinocchio as pin
import pinocchio.casadi as cpin
import casadi as ca
import numpy as np
import time

# 加载模型
model = pin.buildModelFromUrdf("panda.urdf")
cmodel = cpin.Model(model)
cdata = cmodel.createData()

# 定义符号变量
q = ca.SX.sym("q", model.nq)
v = ca.SX.zeros(model.nv)
a = ca.SX.zeros(model.nv)

# 符号 RNEA
cpin.rnea(cmodel, cdata, q, v, a)
tau = cdata.tau

# 求 Jacobian（符号级）
J = ca.jacobian(tau, q)

# 创建可调用函数
rnea_jac_fn = ca.Function("rnea_jac", [q], [tau, J])

# 数值验证
q0 = np.zeros(model.nq)
tau_val, J_val = rnea_jac_fn(q0)
print(f"tau = {np.array(tau_val).flatten()}")
print(f"J shape = {np.array(J_val).shape}")

# 性能测量
t0 = time.perf_counter()
for _ in range(10000):
    rnea_jac_fn(q0)
t1 = time.perf_counter()
us = (t1 - t0) / 10000 * 1e6
print(f"CasADi Python Jacobian: {us:.1f} us")
# 典型输出: ~35 us (Python 调用开销)

# 代码生成（消除 Python 开销）
rnea_jac_fn.generate("rnea_casadi.c")
# gcc -O3 -shared -fPIC -o librnea_casadi.so rnea_casadi.c
# 代码生成版: ~4 us (与 CppADCodeGen 可比)

三维对比总结：

维度	CppAD (C++)	CasADi (Python)	CasADi (CodeGen)
核心代码行数	~40 行	~25 行	~30 行(含编译步骤)
开发周期	~2 小时(需 C++ 编译调试)	~30 分钟(Python 交互式)	~1 小时(含编译)
tape/符号构建耗时	~5 ms	~200 ms	~200 ms + 编译
单次 Jacobian 耗时	~12 \(\mu\)s (tape)	~35 \(\mu\)s (Python)	~4 \(\mu\)s (CodeGen)
CodeGen 后耗时	~3 \(\mu\)s (.so)	—	~4 \(\mu\)s (.so)
调试体验	困难(模板错误)	优秀(交互式)	中等
部署依赖	无(纯 C .so)	Python + CasADi	CasADi C 运行时

本质洞察：CasADi Python 版本虽然运行时慢（Python 解释器开销），但它的**开发效率是 CppAD 的 4 倍**——不需要编译等待、可以交互式检查中间结果、错误信息更友好。这就是为什么学术研究首选 CasADi 做原型，部署时再切换到 CppADCodeGen。

正确的工作流是：CasADi Python 原型验证 → CppADCodeGen C++ 部署。两步之间通过 Pinocchio 的标量参数化桥接——同一个 RNEA 算法，SX 类型用于 CasADi，CG<double> 类型用于 CppADCodeGen。

6.6 acados 代码生成的完整工作流 ⭐⭐⭐¶

acados 将 CasADi 建模和 MPC 求解器代码生成整合为一个无缝管线。以下是一个 2-DOF 机械臂 MPC 的完整 acados 工作流，展示从建模到嵌入式部署的每一步：

# acados_arm_mpc.py — 完整 MPC 代码生成示例
from acados_template import (
    AcadosOcp, AcadosOcpSolver, AcadosModel)
import casadi as ca
import numpy as np

# ===== 1. 定义动力学模型 =====
def arm_dynamics():
    """2-DOF 机械臂连续时间动力学"""
    # 状态: [q1, q2, dq1, dq2]
    q = ca.SX.sym('q', 2)
    dq = ca.SX.sym('dq', 2)
    x = ca.vertcat(q, dq)
    u = ca.SX.sym('u', 2)  # 关节力矩

    # 简化的 2-DOF 动力学 (M * ddq + C * dq + g = tau)
    m1, m2, l1, l2, g = 1.0, 0.5, 0.5, 0.3, 9.81
    M = ca.SX(2, 2)
    M[0,0] = (m1 + m2) * l1**2 + m2 * l2**2 \
           + 2*m2*l1*l2*ca.cos(q[1])
    M[0,1] = m2*l2**2 + m2*l1*l2*ca.cos(q[1])
    M[1,0] = M[0,1]
    M[1,1] = m2*l2**2

    C = ca.SX(2, 2)
    h = m2*l1*l2*ca.sin(q[1])
    C[0,0] = -h*dq[1]; C[0,1] = -h*(dq[0]+dq[1])
    C[1,0] = h*dq[0];  C[1,1] = 0

    gvec = ca.SX(2, 1)
    gvec[0] = (m1+m2)*g*l1*ca.sin(q[0]) \
            + m2*g*l2*ca.sin(q[0]+q[1])
    gvec[1] = m2*g*l2*ca.sin(q[0]+q[1])

    ddq = ca.solve(M, u - C@dq - gvec)
    xdot = ca.vertcat(dq, ddq)

    model = AcadosModel()
    model.name = 'arm_2dof'
    model.x = x; model.u = u; model.f_expl_expr = xdot
    return model

# ===== 2. 配置 OCP =====
ocp = AcadosOcp()
ocp.model = arm_dynamics()
N = 20; Tf = 1.0  # 20 步, 1 秒时域
ocp.dims.N = N

# 代价函数
Q = np.diag([10, 10, 1, 1])  # 状态权重
R = np.diag([0.01, 0.01])    # 控制权重
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.W = np.block([[Q, np.zeros((4,2))],
                        [np.zeros((2,4)), R]])
ocp.cost.Vx = np.vstack([np.eye(4), np.zeros((2, 4))])  # (ny, nx) = (6, 4)
ocp.cost.Vu = np.vstack([np.zeros((4,2)), np.eye(2)])
ocp.cost.yref = np.zeros(6)

# 约束
ocp.constraints.lbu = np.array([-50, -20])  # 力矩下限
ocp.constraints.ubu = np.array([50, 20])    # 力矩上限
ocp.constraints.idxbu = np.array([0, 1])
ocp.constraints.x0 = np.array([0, 0, 0, 0])

# 求解器配置
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'  # 实时迭代
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.tf = Tf

# ===== 3. 代码生成 + 编译 =====
solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
# 此步骤:
#   1. CasADi 生成动力学函数 C 代码
#   2. acados 生成完整 SQP-RTI 求解器 C 代码
#   3. CMake 编译为共享库
#   4. Python wrapper 通过 ctypes 加载

# ===== 4. 在线求解 =====
x0 = np.array([0.5, -0.3, 0, 0])
x_ref = np.array([1.0, 0.5, 0, 0])  # 目标配置

solver.set(0, 'lbx', x0)
solver.set(0, 'ubx', x0)
for k in range(N):
    solver.set(k, 'yref', np.concatenate([x_ref, np.zeros(2)]))

status = solver.solve()
u_opt = solver.get(0, 'u')
solve_time_ms = solver.get_stats('time_tot') * 1000
print(f"求解耗时: {solve_time_ms:.2f} ms")
# 典型输出: ~0.3 ms (SQP-RTI 单次迭代)

⚠️ 编程陷阱：acados 的 SQP-RTI 只做一步迭代

错误描述：新手期望 solver.solve() 返回最优解，但 SQP-RTI 每次只做一步 SQP 迭代

现象：第一次调用返回的解"不够好"，但控制效果随时间改善

根本原因：SQP-RTI (Real-Time Iteration) 的设计哲学是"一个控制周期做一步 SQP 迭代"。虽然每步不是最优的，但通过 MPC 的滚动时域和 warm-start，解质量在数个周期后收敛。这种策略用最优性换取了确定性的计算时间。

正确理解：SQP-RTI 的总收敛速度 = 控制频率 / SQP 收敛步数。如果控制跑在 500 Hz 而 SQP 需要 3 步收敛，等效"优化频率"是 500/3 \(\approx\) 167 Hz——对大多数机械臂任务足够了。

练习¶

⭐⭐ 用 CasADi Python 定义一个 2-DOF 机械臂简单 MPC（10 步时域）。用 Ipopt 求解。
⭐⭐⭐ 用 acados 为同一 MPC 生成 C 代码。对比 acados 和 CasADi+Ipopt 的求解速度。
⭐⭐ （跨章综合题）画出 Ceres Jet (SLAM) → CppAD (规控基础) → CppADCodeGen (实时 MPC) 的层次关系。解释每一层解决了上一层的什么局限。
⭐⭐⭐ 用 CasADi 和 CppAD 分别实现 Panda RNEA 的 Jacobian 计算。用有限差分验证两者的一致性（误差应 < \(10^{-10}\)）。对比开发时间和运行时间。

7. 性能基准——手写 vs AD vs 数值差分 ⭐⭐⭐¶

7.1 完整性能对比 ⭐⭐¶

以 Franka Panda 7-DOF 为例，计算 RNEA 对状态 \((q,\dot q)\) 的完整 Jacobian：

\[\left[\frac{\partial \tau}{\partial q}\ \frac{\partial \tau}{\partial \dot q}\right] \in \mathbb{R}^{7 \times 14}\]

方法	耗时	精度	代码行数	维护成本
`computeRNEADerivatives` (Pinocchio 内置解析导数)	~4.5 \(\mu\)s	\(10^{-16}\)	0 (内置)	无
CppADCodeGen .so	~3 \(\mu\)s	\(10^{-16}\)	~50 行设置	低
CppAD tape 回放	~12 \(\mu\)s	\(10^{-16}\)	~30 行设置	低
CasADi SX (C 代码生成)	~4 \(\mu\)s	\(10^{-16}\)	~20 行 Python	低
有限差分 (中心)	~50 \(\mu\)s	\(10^{-8}\)	~5 行	无
有限差分 (单侧)	~25 \(\mu\)s	\(10^{-4}\)	~3 行	无

7.2 在 MPC 中的实际影响 ⭐⭐¶

20 步 MPC，每步需要 RNEA + Jacobian：

方法	每步总耗时	20 步总耗时	占 1ms 预算
Pinocchio RNEA + 内置解析导数	6.3 \(\mu\)s	126 \(\mu\)s	12.6%
Pinocchio RNEA + CodeGen	4.8 \(\mu\)s	96 \(\mu\)s	9.6%
Pinocchio RNEA + tape	13.8 \(\mu\)s	276 \(\mu\)s	27.6%
Pinocchio RNEA + 有限差分	51.8 \(\mu\)s	1036 \(\mu\)s	超时！

本质洞察：在 MPC 中，导数计算的选择直接决定了你能跑多少步预测时域、能跑多快的控制频率。CppADCodeGen 让 7-DOF 机械臂的 100 Hz MPC 成为可能。如果用有限差分，只能做 10 Hz MPC 或减少预测步数——两者都会降低控制质量。

7.3 高自由度下的性能缩放 ⭐⭐⭐¶

DOF	手写导数	CodeGen	tape	有限差分
6	~3.8 \(\mu\)s	~2.5 \(\mu\)s	~10 \(\mu\)s	~40 \(\mu\)s
7	~4.5 \(\mu\)s	~3.0 \(\mu\)s	~12 \(\mu\)s	~50 \(\mu\)s
12	~9 \(\mu\)s	~6 \(\mu\)s	~25 \(\mu\)s	~100 \(\mu\)s
30	~20 \(\mu\)s	~15 \(\mu\)s	~60 \(\mu\)s	~600 \(\mu\)s

有限差分缩放最差——需要 \(2N\) 次额外 RNEA 调用，\(N\) 越大开销越大。AD 的缩放更好，导数计算复杂度与前向计算成固定倍数关系。

7.4 端到端 MPC 性能剖析——导数在全栈中的占比 ⭐⭐⭐¶

单纯的导数 benchmark 只是局部视角。在实际 MPC 系统中，导数计算只是众多计算环节之一。以下是 OCS2 风格的 7-DOF MPC 全栈性能剖析：

OCS2 MPC 单周期计算分解（20 步预测时域，7-DOF Panda）：

计算环节	CppADCodeGen	CppAD tape	有限差分
动力学评估 (RNEA x 20)	36 \(\mu\)s	36 \(\mu\)s	36 \(\mu\)s
动力学导数 (Jacobian x 20)	60 \(\mu\)s	240 \(\mu\)s	1000 \(\mu\)s
代价函数 + 梯度	15 \(\mu\)s	30 \(\mu\)s	100 \(\mu\)s
碰撞代价 + 梯度 (x 20)	40 \(\mu\)s	80 \(\mu\)s	300 \(\mu\)s
QP 构建 (Riccati 分解)	80 \(\mu\)s	80 \(\mu\)s	80 \(\mu\)s
QP 求解 (HPIPM)	120 \(\mu\)s	120 \(\mu\)s	120 \(\mu\)s
线搜索 + 更新	30 \(\mu\)s	30 \(\mu\)s	30 \(\mu\)s
单次 SQP 迭代总计	381 \(\mu\)s	616 \(\mu\)s	1666 \(\mu\)s
可行 SQP-RTI 频率	2.6 kHz	1.6 kHz	600 Hz

关键发现：

导数计算占总耗时的 30%（CodeGen）到 84%（有限差分）
CodeGen 使得导数计算**不再是瓶颈**——QP 求解 (120 \(\mu\)s) 反而成为最大单项
有限差分方案中，导数计算 (1400 \(\mu\)s) 占总耗时的 84%，是系统瓶颈

本质洞察：AD + CodeGen 的真正价值不仅是"导数更快"，而是**改变了系统瓶颈的位置**。用有限差分时，优化精力应该花在减少差分次数上；用 CodeGen 时，优化精力应该花在 QP 求解器选择和 warm-start 策略上。不同的导数方法导致不同的系统优化方向。

有限差分的 epsilon 选择——截断误差与舍入误差的权衡：

有限差分精度取决于步长 \(\epsilon\) 的选择。理论上：

\[\text{中心差分误差} = \underbrace{\frac{\epsilon^2}{6} f'''(\xi)}_{\text{截断误差}} + \underbrace{\frac{2 \epsilon_{\text{mach}}}{\epsilon} |f(x)|}_{\text{舍入误差}}\]

截断误差随 \(\epsilon\) 增大而增大，舍入误差随 \(\epsilon\) 增大而减小。最优 \(\epsilon\) 在两者平衡处：

\[\epsilon^* \approx \left(\frac{3 \epsilon_{\text{mach}} |f(x)|}{|f'''(\xi)|}\right)^{1/3} \approx 10^{-5} \text{ 到 } 10^{-6}\]

（对于 double 精度，\(\epsilon_{\text{mach}} \approx 2.2 \times 10^{-16}\)。）

实测验证——RNEA Jacobian 的差分精度 vs epsilon：

epsilon      中心差分误差 (vs 解析导数)    单侧差分误差
1e-2         3.2e-4                        5.1e-2
1e-4         3.1e-8                        4.8e-4
1e-6         4.5e-12                       5.2e-6
1e-8         2.1e-8 ← 开始恶化             3.3e-8
1e-10        1.8e-6 ← 严重恶化             1.5e-6
1e-12        2.3e-4 ← 完全不可用            8.7e-5

中心差分的最优 epsilon 在 \(10^{-6}\) 附近，单侧差分在 \(10^{-7}\) 附近。选错 epsilon 可能导致数个量级的精度损失——这是有限差分最大的工程风险。

⚠️ 思维陷阱：认为"epsilon 越小越好"

新手想法："机器精度是 \(10^{-16}\)，所以 epsilon 设成 \(10^{-15}\) 最准"

实际上：\(\epsilon = 10^{-15}\) 时，\(f(x + \epsilon) - f(x)\) 的有效数字不足 1 位——分子的两个值几乎相同，相减后只剩舍入噪声。结果的误差反而比 \(\epsilon = 10^{-6}\) 大 8 个量级。

正确做法：中心差分用 \(\epsilon \in [10^{-7}, 10^{-5}]\)，或更好地——用 AD 彻底消除这个问题。

练习¶

⭐⭐ 在你的机器上运行四种方法的 benchmark，绘制柱状图。
⭐⭐⭐ 对于 30-DOF 人形机器人（如 Talos），估算使用不同导数方法时 MPC 的可行控制频率。
⭐⭐ 测量有限差分在不同 \(\epsilon\) 值下的精度。画出"\(\epsilon\) vs 误差"曲线，找到最优 \(\epsilon\)（截断误差 vs 舍入误差的权衡）。
⭐⭐⭐ 实现上述全栈 MPC 性能剖析：对 Pinocchio RNEA + 导数 + QP 构建 + QP 求解分别计时，验证导数占比数据。

8. 实战：用 AD 加速轨迹优化 ⭐⭐¶

8.1 问题设定 ⭐⭐¶

考虑一个简单的轨迹优化问题：给定起点 \(q_0\) 和终点 \(q_T\)，找到平滑且无碰撞的轨迹 \(q_{0:T}\)：

\[\min_{q_{1:T-1}} \sum_{t=0}^{T-1} \left[ \underbrace{\|q_{t+1} - q_t\|^2}_{\text{平滑性}} + \underbrace{w_c \cdot c_{\text{coll}}(q_t)}_{\text{碰撞代价}} + \underbrace{w_d \cdot \|\tau(q_t)\|^2}_{\text{力矩最小化}} \right]\]

每一项的梯度来源： - 平滑性：手写即可（简单二次） - 碰撞代价：M04 的碰撞距离梯度 + FK Jacobian - 力矩最小化：RNEA 导数 \(\partial \tau / \partial q\)（本章核心）

8.2 用 CppAD 实现自动求导的代价函数 ⭐⭐¶

void trajectoryOptCostAndGrad(
    const pinocchio::Model& model,
    pinocchio::Data& data,
    const std::vector<Eigen::VectorXd>& q_traj,
    double w_c, double w_d,
    double& total_cost,
    std::vector<Eigen::VectorXd>& grad
) {
    total_cost = 0.0;
    grad.resize(q_traj.size(),
                Eigen::VectorXd::Zero(model.nv));

    for (size_t t = 0; t < q_traj.size() - 1; ++t) {
        // 1. 平滑性代价（手写梯度即可）
        Eigen::VectorXd dq = q_traj[t+1] - q_traj[t];
        total_cost += dq.squaredNorm();
        grad[t]   -= 2.0 * dq;
        grad[t+1] += 2.0 * dq;

        // 2. 力矩最小化代价（用 Pinocchio 解析导数）
        Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
        Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
        pinocchio::rnea(model, data, q_traj[t], v, a);
        Eigen::VectorXd tau = data.tau;

        pinocchio::computeRNEADerivatives(model, data,
            q_traj[t], v, a);

        total_cost += w_d * tau.squaredNorm();
        grad[t] += w_d * 2.0 * data.dtau_dq.transpose() * tau;

        // 3. 碰撞代价（结合 M04 碰撞距离梯度）
        // ... 使用 Pinocchio + Coal ...
    }
}

8.3 碰撞代价的梯度：链式法则优先，AD 版本门控 ⭐⭐⭐¶

碰撞代价是轨迹优化中最容易写错梯度的部分，因为碰撞距离到关节角的映射同时经过 FK、几何放置、最近点计算和代价函数。工程上不要默认把 pinocchio::computeDistances(ad_model, ...) 当成所有 Pinocchio/Coal 版本都可编译、可录制的 CppAD 路径；更稳妥的默认实现是"最近点 + point Jacobian + 链式法则"，再用有限差分做回归测试。

回顾 M04 §7.2，碰撞距离对关节角的梯度通过链式法则计算：

\[\frac{\partial d_{\text{coll}}}{\partial q} = \frac{\partial d_{\text{geom}}}{\partial T_A} \frac{\partial T_A}{\partial q} + \frac{\partial d_{\text{geom}}}{\partial T_B} \frac{\partial T_B}{\partial q}\]

默认实现按下面的链式法则展开。设 Coal 返回最近点 \(p_A, p_B\) 和距离法向 \(n = (p_A - p_B) / \|p_A - p_B\|\)，则局部不发生最近特征切换时：

\[ \frac{\partial d}{\partial q} = n^\top \left(J_{p_A}(q) - J_{p_B}(q)\right) \]

其中 \(J_{p_A}, J_{p_B}\) 是两个最近点在世界系的 3D point Jacobian。碰撞代价若为 \(c(d)\)，最终梯度是：

\[ \frac{\partial c}{\partial q} = \frac{\partial c}{\partial d}\frac{\partial d}{\partial q} \]

// 伪代码：默认推荐路径，不依赖 AD 几何 tape。
pinocchio::forwardKinematics(model, data, q);
pinocchio::updateGeometryPlacements(model, data, geom_model, geom_data, q);
pinocchio::computeDistances(model, data, geom_model, geom_data, q);

for (const auto& active_pair : active_collision_pairs) {
    // 1. 从 distanceResults 读取距离、最近点和对应几何体。
    double d = active_pair.distance;
    Eigen::Vector3d pA_world = active_pair.nearest_point_A_world;
    Eigen::Vector3d pB_world = active_pair.nearest_point_B_world;
    Eigen::Vector3d n = (pA_world - pB_world).normalized();

    // 2. 计算最近点的 point Jacobian。
    //    具体实现需要把 pA/pB 映射到各自 parent joint 或 frame 的局部坐标，
    //    再用 Pinocchio 的 point Jacobian / frame Jacobian 组合得到 3 x nv。
    Eigen::MatrixXd JpA = pointJacobianWorld(model, data, geomA, pA_world);
    Eigen::MatrixXd JpB = pointJacobianWorld(model, data, geomB, pB_world);

    // 3. 距离梯度和代价梯度。
    Eigen::RowVectorXd dd_dq = n.transpose() * (JpA - JpB);
    grad += dc_dd(d, d_safe, d_act) * dd_dq.transpose();
}

在目标版本明确支持几何标量模板、最近点结果和分支表达都可录制时，才把碰撞代价放进 CppAD tape；这应写成版本门控路径，并保留有限差分回归测试：

// 伪代码：只有目标 Pinocchio/Coal 版本验证通过后才启用。
if constexpr (SupportsCppADCollisionDistance<TargetPinocchio, TargetCoal>) {
    // record_fk_and_collision_cost_tape();
    // compare_ad_gradient_with_finite_difference(q_samples);
} else {
    // use_nearest_points_and_point_jacobians();
    // compare_chain_gradient_with_finite_difference(q_samples);
}

关键风险：碰撞距离是分段光滑函数。最近点可能从"顶点-面"切换到"边-边"，或从一个碰撞对切换到另一个碰撞对；这些特征切换处距离连续但梯度不连续。无论使用手写链式梯度、CppAD 还是 CodeGen，都必须用有限差分在随机样本和边界样本上做回归测试，并在优化器中接受次梯度/激活集切换带来的非光滑性。

8.4 CppADCodeGen 预编译版本 ⭐⭐⭐¶

对于需要反复求解的轨迹优化（如 MPC 的滚动优化），可以用 CppADCodeGen 预编译动力学、平滑代价和其它已验证可录制的光滑代价。碰撞代价只有在上一节的版本门控和有限差分回归测试通过后，才适合进入同一个 CodeGen 路径；否则应继续使用最近点链式梯度。

// 预编译：在程序启动时执行一次
// 将已验证可录制的代价函数通过 CppADCodeGen 生成 .so
// 运行时 dlopen 加载

// 优化循环中：
for (int iter = 0; iter < max_iter; ++iter) {
    // 微秒级求值 + 求导
    cost_fn->ForwardZero(q_flat, cost_val);
    cost_fn->Jacobian(q_flat, grad_val);

    // L-BFGS 更新
    q_flat -= alpha * H_inv * grad_val;
}

8.5 完整轨迹优化算法框架 ⭐⭐¶

将以上组件组合为一个完整的轨迹优化器：

输入: q_0 (起点), q_T (终点), 障碍物列表
输出: 优化后的轨迹 q_{0:T}

1. 初始化: 线性插值 q_t = q_0 + t/T * (q_T - q_0)

2. 预编译 AD 函数:
   - RNEA 导数 .so (CppADCodeGen)
   - 光滑代价 .so (CppADCodeGen)
   - 碰撞代价默认用最近点链式梯度；只有版本门控通过时才 CodeGen

3. 迭代优化:
   for iter = 1 to max_iter:
     total_cost = 0
     total_grad = zeros(T * nv)

     for t = 0 to T-1:
       # 平滑性代价（手写梯度）
       cost_smooth, grad_smooth = smooth_cost(q_t, q_{t+1})

      # 碰撞代价（最近点链式梯度，或版本门控后的 AD 梯度）
       cost_coll, grad_coll = collision_cost_fn(q_t)

       # 力矩代价（Pinocchio 解析导数）
       cost_torque, grad_torque = torque_cost(q_t)

       total_cost += cost_smooth + w_c * cost_coll
                   + w_d * cost_torque
       total_grad[t] += grad_smooth + w_c * grad_coll
                      + w_d * grad_torque

     # 更新轨迹
     q_{0:T} -= alpha * total_grad
     # (保持 q_0 和 q_T 不变)

     if converged(total_cost): break

4. 输出: 优化后的 q_{0:T}

这个框架混合使用了三种导数来源：手写（平滑性）、Pinocchio 内置（力矩）、CppAD/CodeGen（碰撞）——在实际工程中，混合策略通常比纯 AD 更高效。

练习¶

⭐⭐ 实现一个简单的 2-DOF 平面机械臂轨迹优化：用 CppAD 对"力矩最小化"代价函数自动求导，用梯度下降优化 10 步轨迹。
⭐⭐⭐ 将 M04 的碰撞代价集成到轨迹优化中。验证优化后的轨迹确实绕过了障碍物。
⭐⭐⭐ 对比三种实现方式的总优化耗时：(a) 全手写梯度 (b) CppAD tape (c) CppADCodeGen 预编译。
⭐⭐ 在优化过程中打印每次迭代的三项代价（平滑/碰撞/力矩），观察收敛行为。调整权重 \(w_c\) 和 \(w_d\) 对轨迹形状的影响。

9. JAX 在机械臂中的新兴应用 ⭐⭐¶

9.1 JAX 的定位 ⭐⭐¶

JAX 不属于"Pinocchio + CppAD"的 C++ 主线，但在以下场景越来越重要：

场景	JAX 的角色
MuJoCo MJX	全栈可微仿真（GPU 加速）
RL 训练	可微物理 + 策略梯度
并行优化	`vmap` + `jit` 批量轨迹优化

9.2 何时选 JAX ⭐⭐¶

你的场景？
│
├── GPU 训练 RL + 可微仿真 → JAX (MuJoCo MJX)
├── 实时 C++ MPC 部署 → CppAD + CppADCodeGen
├── Python 原型快速验证 → CasADi 或 JAX
└── 嵌入式部署 → CasADi + acados

练习¶

⭐⭐ 用 JAX 实现一个 2-DOF FK 并自动求导。与 CasADi Python 版本的开发体验对比。
⭐⭐ 解释为什么 MuJoCo MJX 选择 JAX 而非 CppAD 作为可微仿真后端。

10. AD 调试与验证方法论 ⭐⭐¶

10.1 AD 导数的系统化验证 ⭐⭐¶

AD 最大的风险不是"算错"（AD 在数学上是精确的），而是**实现错误**——比如自变量声明遗漏、tape 录制范围错误、输入向量拼接顺序错误。以下是系统化的验证方法：

方法 1：有限差分对比（golden standard）

// 用有限差分验证 AD Jacobian
void verifyJacobian(
    CppAD::ADFun<double>& ad_fun,
    const std::vector<double>& x,
    double epsilon = 1e-6, double tol = 1e-5) {

    // AD Jacobian
    auto jac_ad = ad_fun.Jacobian(x);

    // 有限差分 Jacobian
    size_t n = x.size();
    size_t m = ad_fun.Range();
    std::vector<double> jac_fd(m * n);

    for (size_t j = 0; j < n; ++j) {
        auto x_plus = x, x_minus = x;
        x_plus[j] += epsilon;
        x_minus[j] -= epsilon;
        auto y_plus = ad_fun.Forward(0, x_plus);
        auto y_minus = ad_fun.Forward(0, x_minus);
        for (size_t i = 0; i < m; ++i) {
            jac_fd[i * n + j] =
                (y_plus[i] - y_minus[i]) / (2 * epsilon);
        }
    }

    // 逐元素对比
    double max_err = 0;
    for (size_t k = 0; k < m * n; ++k) {
        double err = std::abs(jac_ad[k] - jac_fd[k]);
        double rel_err = err / (1 + std::abs(jac_ad[k]));
        max_err = std::max(max_err, rel_err);
        if (rel_err > tol) {
            size_t i = k / n, j = k % n;
            std::cerr << "Jacobian 不匹配: J["
                << i << "," << j << "] AD=" << jac_ad[k]
                << " FD=" << jac_fd[k]
                << " err=" << rel_err << std::endl;
        }
    }
    std::cout << "最大相对误差: " << max_err << std::endl;
}

方法 2：Pinocchio 解析导数交叉验证

对于 RNEA 和 ABA 导数，Pinocchio 有手写的解析导数可以作为参考：

// 三方交叉验证: 解析 vs AD vs 有限差分
auto dtau_dq_analytic = data.dtau_dq;     // 解析导数
auto dtau_dq_ad = rnea_ad.Jacobian(x);    // AD 导数
auto dtau_dq_fd = finite_diff(rnea, x);   // 有限差分

// 三者两两比较
double err_analytic_ad = (dtau_dq_analytic - dtau_dq_ad).norm();
double err_analytic_fd = (dtau_dq_analytic - dtau_dq_fd).norm();
double err_ad_fd = (dtau_dq_ad - dtau_dq_fd).norm();

// 预期: err_analytic_ad ≈ 1e-16 (两者都是精确的)
//        err_analytic_fd ≈ 1e-8  (有限差分截断误差)
//        err_ad_fd ≈ 1e-8        (有限差分截断误差)

💡 概念误区：认为"AD 导数和解析导数不一致说明 AD 有 bug"

新手想法："AD 和解析导数应该完全一样（误差 < 1e-16），如果误差是 1e-10 就说明有问题"

实际上：误差 \(10^{-10}\) 到 \(10^{-14}\) 范围内的差异通常来自**运算顺序不同**。同一数学公式用不同的代码实现（AD 展开 vs 手写解析式），浮点运算顺序可能不同，累积的舍入误差略有差异。这不是 bug，而是浮点算术的正常行为。

真正的 bug 信号：误差 > \(10^{-5}\)，或在某些 \(q\) 值下误差突然从 \(10^{-14}\) 跳到 \(10^{-3}\)——这通常意味着 Independent() 声明遗漏了某个变量。

本章常见误解汇总¶

误解	正确理解	相关章节
"AD 和数值差分差不多"	AD 精度是机器精度 \(10^{-16}\)，数值差分只有 \(O(\epsilon^2) \approx 10^{-8}\)	§1.2
"有限差分够用"	7-DOF Jacobian 需 28 次额外 RNEA，比 AD 慢 10 倍；精度差异在 MPC 中导致 QP 收敛变慢	§1.2
"CppAD 和 CasADi 功能等价"	CppAD 是 C++ 运算符重载 AD，CasADi 是符号计算框架——前者适合纯 C++ 栈，后者适合 Python 原型+代码生成	§6
"tape 录制开销很大"	录制只需一次，回放可重复使用；CodeGen 后更是零 tape 开销	§4, §5
"AD 导数和解析导数应该完全相同"	\(10^{-10}\) 到 \(10^{-14}\) 的差异来自浮点运算顺序不同，属正常行为	§7
"SX 总是比 MX 好"	SX 标量展开在高 DOF 系统上会内存爆炸；\(\geq 20\) DOF 应用 MX	§6.1
"反向 AD 总是比前向 AD 快"	只有当输出维度 \(m\) 远小于输入维度 \(n\) 时反向才有优势；\(m \approx n\) 时两者接近	§3
"代码生成的 .so 不需要重新编译"	当机器人模型（URDF）改变时，.so 必须重新生成——它是特定模型的编译产物	§5.2

累积项目：AD 增强的动力学模块 ⭐⭐¶

本章新增模块¶

mini-manip/
├── src/
│   ├── load_panda.cpp              ← M01
│   ├── dynamics_benchmark.cpp      ← M02
│   ├── collision_checker.cpp       ← M04
│   └── ad_derivatives.cpp          ← M06 新增
├── include/
│   ├── dynamics_interface.hpp      ← M02
│   ├── collision_checker.hpp       ← M04
│   └── ad_derivatives.hpp          ← M06 新增
├── codegen/
│   └── rnea_panda.so               ← M06 新增: CppADCodeGen 生成
└── CMakeLists.txt                  ← M06 更新: 添加 CppAD 依赖

练习¶

⭐ 实现 ADDerivatives 类：封装 CppAD tape 录制和 Jacobian 计算。
⭐⭐ 添加 CppADCodeGen 预编译功能：构建时生成 rnea_panda.so，运行时 dlopen 加载。
⭐⭐⭐ （跨章综合题）结合 M01 的 FK、M04 的碰撞距离、M06 的梯度验证流程，实现完整的"碰撞代价函数 + 梯度"模块；默认使用最近点链式梯度，AD 路径必须通过有限差分回归测试后再启用。

本章小结¶

知识点	核心要点	难度
AD 三大范式	运算符重载(CppAD) / 源码变换(Enzyme/JAX) / 符号(CasADi)	⭐⭐
前向 vs 反向 AD	m<n 用反向，n<m 用前向；RNEA 导数用反向更优	⭐⭐⭐
CppAD tape 机制	Independent→计算→ADFun→Jacobian；tape 回放 3-10x 开销	⭐⭐
CppADCodeGen	tape→C代码→.so→dlopen；消除 tape 回放开销	⭐⭐⭐
CasADi vs CppAD	纯C++用CppAD，Python原型用CasADi，嵌入式用acados	⭐⭐
性能基准	CodeGen(~3\(\mu\)s) < Pinocchio 内置解析导数(~4.5\(\mu\)s) < CppAD tape(~12\(\mu\)s) < 有限差分(~50\(\mu\)s)	⭐⭐⭐
OCS2 范式	Pinocchio + CppADCodeGen = 实时 MPC 的性能核心	⭐⭐⭐

延伸阅读¶

资源	难度	说明
Griewank & Walther (2008) "Evaluating Derivatives"	⭐⭐⭐	AD 理论教科书
Carpentier & Mansard (2018) "Analytical Derivatives of Rigid Body Dynamics"	⭐⭐⭐	Pinocchio 解析导数论文
CppAD 官方文档 (`coin-or.github.io/CppAD/doc/`)	⭐⭐	CppAD tutorial
CppADCodeGen Wiki (`github.com/joaoleal/CppADCodeGen/wiki`)	⭐⭐	代码生成教程
CasADi 用户指南 (`web.casadi.org`)	⭐⭐	CasADi 官方教程
Verschueren et al. (2022) "acados—modular framework for fast embedded optimal control"	⭐⭐⭐	acados 论文
OCS2 文档 (`leggedrobotics.github.io/ocs2/`)	⭐⭐⭐	Pinocchio + CppADCodeGen 工业实践
Pinocchio `autodiff/cppad.hpp` 源码	⭐⭐⭐	标量模板化 + AD 的工业级写法
Moses & Churavy (2021) "Instead of Rewriting Foreign Code for Machine Learning, Automatically Synthesize Fast Gradients" (Enzyme)	⭐⭐⭐⭐	Enzyme 论文——编译器级 AD 的理论基础
Howell et al. (2022) "Dojo: A Differentiable Physics Engine for Robotics"	⭐⭐⭐	Julia 可微仿真——与 MJX 类似但用 Julia
Freeman et al. (2021) "Brax: A Differentiable Physics Engine for Large Scale Rigid Body Simulation"	⭐⭐⭐	JAX 可微仿真——理解 MJX 的前身
Pinocchio 3.x CasADi 集成文档	⭐⭐	`ModelTpl<casadi::SX/MX>` 的完整 API

跨章综合练习 ⭐⭐⭐¶

题目：综合 M01（Pinocchio 动力学）+ M05（QP 建模）+ M06（自动微分），实现一个 AD 驱动的 MPC 梯度计算管线：

M01 复用：用 Pinocchio 加载 Franka Panda URDF，实现 RNEA 动力学函数 \(\tau = f(q, \dot{q}, \ddot{q})\)
M06 核心：用三种方法计算 RNEA Jacobian \(\partial \tau / \partial q\)：
(a) Pinocchio 内置 computeRNEADerivatives
(b) CppAD tape 录制 + Jacobian() 回放
(c) CppADCodeGen 生成 .so + dlopen 加载
精度验证：在 100 个随机配置下比较三种方法的输出，确认误差 \(< 10^{-10}\)
性能对比：测量三种方法的耗时（10000 次平均），绘制柱状图
M05 集成：将 CppADCodeGen 生成的 Jacobian 送入 ProxQP，组装一个简单的 IK QP，验证端到端工作

评分标准： - 三种方法的数值输出应在 \(10^{-14}\) 以内一致 - 性能排序应为 CodeGen < 解析导数 < tape 回放 - 端到端 IK QP 能在 50 \(\mu\)s 内求解

进阶挑战： - 添加碰撞代价函数（需要 AD 求导，Pinocchio 内置解析导数不覆盖） - 用 CasADi Python 实现同样的管线，比较开发效率和运行性能 - 为 CppADCodeGen 添加 Hessian 生成（二阶导数），用于 SQP 的牛顿步

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
CppAD 录制 tape 时 segfault	`model.cast<ADScalar>()` 在多线程中并发调用	1. 确保 cast 在单线程中完成 2. 检查 Eigen 对齐 3. 检查 AD 变量初始化	§4.2
CppADCodeGen 生成的 .so 与 tape 结果不一致	输入向量顺序/维度定义不同	1. 打印 tape 输入输出维度 2. 检查 [q,v,a] 拼接顺序 3. 用简单函数先验证	§5.2
CppADCodeGen 编译失败（内存不足）	生成的 C 代码太大	1. 降低优化级别 (-O1) 2. 分拆为多个小函数 3. 增大编译机内存	§5.2
CasADi SX 表达式爆炸	对高 DOF 系统用 SX 展开标量运算	1. 改用 MX 保持矩阵结构 2. 减少符号变量维度 3. 部分计算用数值替代	§6.1
有限差分梯度方向错误	epsilon 选择不当	1. 扫描 epsilon 找最优值 (1e-7 到 1e-5) 2. 用 AD 结果验证 3. 关键维度手动检查	§7.3
AD 导数与 Pinocchio 解析导数不一致	q/v/a 中某个被当作常量而非变量	1. 检查 Independent() 声明的变量集 2. 确认 v 和 a 是否也需要设为自变量 3. 打印 tape 的输入维度	§4.2

术语速查表¶

术语	英文	一句话定义
AD	Automatic Differentiation	在数值计算中同时传播函数值和导数值的技术
前向模式	Forward Mode AD	从输入到输出传播方向导数，一次传播计算一个输入维度的导数
反向模式	Reverse Mode AD	从输出到输入反向传播伴随量，一次传播计算一个输出维度对所有输入的导数
tape	Computation Tape	CppAD 的计算记录——记录前向计算中的每一步运算
CodeGen	Code Generation	CppADCodeGen 将 tape 编译为 C 源码再编译为 .so 动态库
dual number	对偶数	前向 AD 的基本数据结构 \((v, \dot{v})\)，同时携带值和导数
adjoint	伴随变量	反向 AD 中从输出反向传播的量，\(\bar{x} = \partial L / \partial x\)
SX	CasADi Symbolic Scalar	CasADi 的标量符号表达式类型，完全展开计算图
MX	CasADi Symbolic Matrix	CasADi 的矩阵符号表达式类型，保持矩阵运算结构
OCS2	Optimal Control for Switched Systems	Pinocchio + CppADCodeGen 的实时 MPC 框架

符号表¶

符号	含义	首次出现
\(\partial f / \partial x\)	函数 \(f\) 对变量 \(x\) 的 Jacobian 矩阵	§1.1
\(\nabla^2_{xx} L\)	Lagrangian 对状态的 Hessian	§1.1
\(n\)	输入维度（自变量个数）	§3
\(m\)	输出维度（因变量个数）	§3
\(\dot{v}\)	前向 AD 的方向导数	§3
\(\bar{v}\)	反向 AD 的伴随变量	§3
\(\epsilon\)	有限差分的扰动量	§1.2, §7

AD 范式选型的工程决策树 ⭐⭐¶

在实际项目中，选择 AD 工具不应基于技术偏好，而应基于项目约束。以下是一个系统化的决策流程：

你的项目技术栈是什么？
│
├── 纯 C++ (ROS2 / 嵌入式)
│   │
│   ├── 需要微秒级导数？(实时 MPC)
│   │   └── Pinocchio + CppADCodeGen → .so → dlopen
│   │       (OCS2 范式，性能最优)
│   │
│   ├── 不需要极致性能？(离线优化)
│   │   └── Pinocchio + CppAD tape → 直接 Jacobian()
│   │       (简单，无需编译 .so)
│   │
│   └── 需要符号简化或人工优化？
│       └── CasADi C++ API → 生成 C 代码 → 编译
│
├── Python 原型 + C 部署
│   │
│   ├── 控制/MPC？
│   │   └── CasADi Python → acados → 生成 C 代码
│   │       (SQP-RTI + HPIPM，可部署到 Cortex-M7)
│   │
│   └── RL / 仿真？
│       └── MuJoCo MJX + JAX → GPU AD
│           (批量并行仿真 + 自动微分)
│
└── 研究/实验 (不关心部署)
    └── CasADi Python Opti() API
        (开发效率最高，交互式调试)

关键考量因素：

因素	CppAD (+ CodeGen)	CasADi	JAX (MJX)
部署目标	嵌入式 C/C++	嵌入式 C (via acados)	GPU (Python)
学习曲线	中（需理解 tape）	低（Python 友好）	低-中
与 Pinocchio 集成	原生	原生 (v3.x)	不直接
性能上限	极高 (.so 预编译)	高 (C 代码生成)	极高 (GPU 并行)
适合批量	否	有限	是 (vmap/pmap)
高阶导数	是 (`AD<AD<double>>`)	有限	是 (jax.hessian)

JAX/MJX 在机械臂 AD 中的新角色 ⭐⭐⭐¶

2024-2025 年，MuJoCo MJX（MuJoCo 的 JAX 后端）为机械臂优化引入了一条新的 AD 路线：

import jax
import jax.numpy as jnp
from mujoco import mjx

# 加载模型到 GPU
model = mujoco.MjModel.from_xml_path("panda.xml")
mjx_model = mjx.put_model(model)
mjx_data = mjx.put_data(model, mujoco.MjData(model))

# 定义可微仿真步
@jax.jit
def sim_step(data, ctrl):
    data = data.replace(ctrl=ctrl)
    return mjx.step(mjx_model, data)

# 自动求导: d(state_next)/d(ctrl)
grad_fn = jax.jacfwd(sim_step, argnums=1)
# 一次调用获得完整 Jacobian，在 GPU 上并行

MJX AD 的独特优势： 1. GPU 并行：jax.vmap 可以同时对 1000 个不同初始状态求导，吞吐量比 CppAD 高 100 倍 2. 仿真可微：不仅动力学可微，整个仿真步（包括接触）都可微——这对 sim-to-real 中的系统辨识至关重要 3. XLA 优化：JAX 的 XLA 编译器会自动做 CSE (Common Subexpression Elimination)、循环展开等优化

MJX AD 的局限： 1. 精度：MJX 使用 float32 而非 float64，对需要高精度的 MPC 可能不够 2. 延迟：JAX 的 JIT 编译首次调用很慢（数秒），不适合需要快速冷启动的场景 3. 部署：生成的计算图绑定在 GPU 上，无法部署到嵌入式平台

本质洞察：CppAD/CasADi 和 JAX/MJX 解决的是**不同维度的问题**。前者追求"一次求导的极致速度"（微秒级，适合实时控制循环），后者追求"大批量求导的吞吐量"（GPU 并行，适合 RL 训练和参数辨识）。这不是替代关系，而是互补关系——训练阶段用 MJX + JAX，部署阶段用 Pinocchio + CppADCodeGen。

Enzyme——编译器级 AD 的未来方向 ⭐⭐⭐⭐¶

Enzyme 是 MIT 开发的 LLVM 插件，在编译器层面实现 AD——直接在 LLVM IR 上做源码变换，无需修改用户代码。

Enzyme 与 CppAD 的根本区别：

维度	CppAD (运算符重载)	Enzyme (源码变换)
修改用户代码	需要（替换 double 为 AD 类型）	不需要
运行时开销	有（tape 录制/回放）	零（编译期完成）
适用语言	C++ only	C/C++/Fortran/Rust/Julia
模板支持	原生	有限（需 LLVM 能 inline 展开模板）
成熟度	生产级	实验级（2026 年）

为什么 Enzyme 在机器人领域尚未流行？

Pinocchio 的 CRTP 模板层层嵌套，Enzyme 需要先将所有模板展开为 LLVM IR 才能求导。当前 Enzyme 对深层模板代码的支持仍不完善——可能在 JointModelVariant 的 std::variant 访问处失败。一旦 Enzyme 成熟到能处理 Pinocchio 级别的模板代码，它有潜力完全取代 CppAD——因为零运行时开销意味着不需要 CodeGen 就能达到预编译 .so 的性能。

反事实推理：如果 Enzyme 在 2026 年能完美支持 Pinocchio 代码，会发生什么？ - CppADCodeGen 的 "录制 tape → 生成 C → 编译 .so" 三步流程可以简化为 "加 -fplugin=enzyme 编译选项" 一步 - OCS2 不再需要在构建时预编译 .so，简化部署流程 - 但 CasADi 的符号化能力（可视化表达式、手动简化）不会被取代——它们的价值不在速度，在于可解释性

本章与后续章节的关系¶

后续章节	关系	本章铺垫的知识点
M08 轨迹优化	AD 驱动的碰撞/动力学代价函数梯度	CppAD tape + CodeGen 工作流
M14 MoveIt2 集成	pick-ik 的 Pinocchio AD	ModelTpl 的使用方式
足式/CppAD	共享 CppAD 基础，本章是机械臂视角的补充	CppADCodeGen (足式未覆盖)

研究实践建议¶

入门级¶

用 CasADi Python 做简单函数的符号微分，直观感受 AD 与数值差分的精度差异
完成 §7 的三方交叉验证实验（解析 vs AD vs 有限差分）

进阶级¶

用 CppADCodeGen 为你的机器人模型生成 RNEA 导数的 .so 文件，测量性能提升
比较 CasADi SX 和 MX 在不同 DOF 下的内存占用和表达式构建时间

研究级¶

关注 Enzyme (LLVM-based 源码变换 AD) 的发展——它可能改变"运算符重载 vs 源码变换"的性能格局
评估 JAX 在 MuJoCo MJX 中的 GPU AD 能力与 CppAD CPU AD 的性能比较

API 速查表¶

CppAD 核心 API¶

API	功能	签名摘要
`CppAD::Independent(x)`	声明自变量，开始 tape 录制	`x`: `vector<AD<double>>&`
`CppAD::ADFun<double> f(x, y)`	封装 tape 为可求导函数	`x`: 输入, `y`: 输出
`f.Jacobian(x0)`	计算完整 Jacobian	`x0`: `vector<double>`, 返回展平的 Jacobian
`f.Forward(0, x0)`	前向零阶（函数值）	返回 `y = f(x0)`
`f.Reverse(1, w)`	反向一阶（伴随）	`w`: 权重向量，返回 \(w^T \cdot J\)
`f.optimize()`	优化 tape（CSE 等）	减少 tape 大小，加速回放

CppADCodeGen 核心流程¶

// 1. 录制 tape (同 CppAD)
CppAD::ADFun<double> f(x, y);

// 2. 创建代码生成模型
CppAD::cg::ModelCSourceGen<double> cgen(f, "my_func");
cgen.setCreateJacobian(true);

// 3. 生成 C 源码
CppAD::cg::ModelLibraryCSourceGen<double> libgen(cgen);
CppAD::cg::DynamicModelLibraryProcessor<double> p(libgen);

// 4. 编译为 .so
CppAD::cg::GccCompiler<double> compiler;
auto lib = p.createDynamicLibrary(compiler);
auto model = lib->model("my_func");

// 5. 运行时调用
auto jac = model->Jacobian(x0);  // 微秒级！

CasADi Python 核心 API¶

API	功能	示例
`ca.SX.sym('x', n)`	创建 SX 符号变量	`x = ca.SX.sym('x', 7)`
`ca.MX.sym('x', n)`	创建 MX 符号变量	`x = ca.MX.sym('x', 7)`
`ca.jacobian(y, x)`	符号 Jacobian	`J = ca.jacobian(tau, q)`
`ca.Function('f', [x], [y])`	封装为可调用函数	`f = ca.Function('f', [q], [tau])`
`f.generate('f.c')`	生成 C 代码	可用 gcc 编译为 .so
`ca.Opti()`	创建优化问题	`opti = ca.Opti()`

AD 性能基准——四种方法的量化对比 ⭐⭐⭐¶

以下是在 Intel i7-12700K / GCC 13.2 / -O3 -march=native 环境下，计算 Franka Panda RNEA 完整 Jacobian \(\partial \tau / \partial (q, \dot{q}, \ddot{q}) \in \mathbb{R}^{7 \times 21}\) 的性能对比：

方法	耗时	精度	适用场景	开发复杂度
Pinocchio 解析导数	4.5 \(\mu\)s	\(10^{-16}\)	RNEA/ABA/FK 专用	低（API 调用）
CppADCodeGen .so	3.0 \(\mu\)s	\(10^{-16}\)	任意函数（预编译后）	中（录制+编译流程）
CppAD tape 回放	12 \(\mu\)s	\(10^{-16}\)	任意函数（灵活但慢）	中
有限差分（中心）	50 \(\mu\)s	\(10^{-8}\)	原型验证、AD 结果交叉检查	低
CasADi SX + CodeGen	4.0 \(\mu\)s	\(10^{-16}\)	Python 建模 + C 部署	中
JAX (MJX)	~8 \(\mu\)s (GPU)	\(10^{-7}\) (fp32)	批量求导（RL 训练）	低

关键观察：

CodeGen 最快：CppADCodeGen 的 .so 比 Pinocchio 解析导数还快 30%——因为 CodeGen 生成的代码是针对特定机器人模型的"定制版"，而 Pinocchio 的解析导数仍有一定的通用性开销
tape 回放的 3-10x 开销：CppAD tape 回放比 CodeGen 慢 4 倍，但比有限差分快 4 倍——这是"灵活性 vs 性能"的折中
有限差分的精度瓶颈：不仅慢，而且 \(10^{-8}\) 的精度在 MPC 的 QP 子问题中会导致更多迭代（参见 M05）

不同 DOF 下的 AD 方法缩放：

DOF    解析导数    CodeGen    tape回放    有限差分
  6     3.8 us     2.5 us     10 us      36 us  (12次额外RNEA)
  7     4.5 us     3.0 us     12 us      50 us  (14次额外RNEA)
 12     8.2 us     5.5 us     22 us      96 us  (24次额外RNEA)
 30    22  us      15  us     60 us     270 us  (60次额外RNEA)

所有 AD 方法的缩放都是 \(O(c \cdot n)\)（线性），但常数 \(c\) 差异决定了在实时系统中的可用性。对于 30-DOF 人形机器人的 MPC，只有 CodeGen 和解析导数能在 1 ms 预算内完成 20 步导数计算。

AD 调试的最佳实践 ⭐⭐¶

AD 代码的调试比普通数值代码更困难，因为 AD 类型（CppAD::AD<double>）不能直接用 std::cout 打印中间值（会影响 tape 录制）。以下是经验证的调试方法：

方法 1：分层验证

验证策略：从简单到复杂，逐层确认正确性

Step 1: 单函数验证
  - 对 f(x) = x^2 求导，确认 f'(x) = 2x
  - 这验证 AD 基础设施正确（Independent/ADFun/Jacobian）

Step 2: Pinocchio 单算法验证
  - 对 FK 求导，与 Pinocchio computeJointJacobians 对比
  - 这验证 ModelTpl<ADScalar> 的 cast 正确

Step 3: 复合函数验证
  - 对碰撞代价 f(q) = sigmoid(d(FK(q))) 求导
  - 与有限差分做交叉验证
  - 如果 Step 1-2 正确但 Step 3 错误，问题在 f 的 sigmoid 部分

Step 4: 全 MPC 验证
  - 在 MPC 闭环中运行 AD 导数
  - 如果 MPC 不收敛但 Step 1-3 正确，问题在 QP warm-start 或约束组装

方法 2：梯度检查器

// gradient_checker.hpp — 通用梯度检查工具
template <typename Func>
bool check_gradient(Func f, const Eigen::VectorXd& x,
                    const Eigen::MatrixXd& J_ad,
                    double eps = 1e-7, double tol = 1e-4) {
    int m = J_ad.rows(), n = J_ad.cols();
    Eigen::MatrixXd J_fd(m, n);
    for (int j = 0; j < n; ++j) {
        Eigen::VectorXd x_plus = x, x_minus = x;
        x_plus[j] += eps;
        x_minus[j] -= eps;
        // 中心差分
        J_fd.col(j) = (f(x_plus) - f(x_minus)) / (2 * eps);
    }
    double max_err = (J_ad - J_fd).cwiseAbs().maxCoeff();
    if (max_err > tol) {
        // 找到误差最大的元素
        int row, col;
        (J_ad - J_fd).cwiseAbs().maxCoeff(&row, &col);
        std::cerr << "梯度检查失败: 元素 [" << row << "," << col
                  << "] AD=" << J_ad(row, col)
                  << " FD=" << J_fd(row, col)
                  << " err=" << max_err << std::endl;
        return false;
    }
    return true;
}

跨领域类比：AD 调试中的"分层验证"策略与深度学习中的"梯度检查"完全同构。PyTorch 的 torch.autograd.gradcheck 做的就是"用有限差分验证自动微分的正确性"。机器人领域的 AD 调试可以直接借鉴深度学习社区的经验。区别在于：深度学习中梯度检查是一次性验证（训练前确认模型定义正确），机器人 MPC 中梯度检查可能需要在特定失败配置上做针对性验证（某个关节角组合触发了奇异点）。

版本信息速查¶

工具	当前版本 (截至 2026.06)	安装方式	许可证
CppAD	2024.x	源码 / `conda install cppad`	EPL-2.0 / GPL-3.0
CppADCodeGen	2.5.x	源码编译	EPL-1.0
CasADi	3.6.x	`pip install casadi`	LGPL-3.0
Enzyme	0.0.x (实验性)	LLVM 插件	Apache-2.0
JAX	0.4.x	`pip install jax`	Apache-2.0
acados	0.3.x	源码编译	BSD-2-Clause
OCS2	1.0.x	源码编译 (catkin/colcon)	BSD-3-Clause