本文档属于 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 加速轨迹优化的完整实战。

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

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

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

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

前置自测 ⭐¶

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

编号	问题	答不出时回顾
1	Ceres Jet：`Jet<double, N>` 的内部结构是什么？它如何在一次前向计算中同时得到函数值和 N 维梯度？	v8 Ch17 Ceres Jet
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` 后可以在运行时动态加载？	v8 Ch18 动态链接

本章目标¶

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

**区分**三种自动微分范式——运算符重载 (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 μs	RNEA/ABA/CRBA 等经典算法
有限差分	低	\(O(\epsilon^2)\)	~50 μs (28次额外 RNEA)	原型验证
自动微分 (CppAD tape)	中	机器精度	~12 μs	任意自定义函数求导
AD + 代码生成	中	机器精度	~3 μs (CodeGen .so)	实时 MPC 部署

反事实推理：如果不用 AD 而坚持有限差分求导，会怎样？ - 7-DOF RNEA Jacobian 需要 \(2 \times 14 = 28\) 次额外 RNEA（中心差分，对 \(q\) 和 \(\dot{q}\) 各 7 维扰动） - 耗时：28 x 1.8 μs (Pinocchio RNEA) = 50.4 μs - 20 步 MPC 总导数耗时：50.4 x 20 = 1008 μs ≈ 1 ms - 已经用完整个控制周期！而 Pinocchio 内置解析导数约为 4.5 x 20 = 90 μs；CppAD tape 回放约为 12 x 20 = 240 μs；CodeGen 预编译版本约为 3 x 20 = 60 μ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 范式总览¶

你在 v8 Ch17 学过 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 的关键区别（回顾 Ch48）：

维度	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)	研究原型	高潜力但模板支持不足
JAX	Python	生产就绪	GPU 训练优秀，实时 C++ 部署需额外步骤
Tapenade	Fortran/C	成熟	不支持 C++

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 μs	~8 μs	4.4x
7-DOF ABA	~2.5 μs	~12 μs	4.8x
7-DOF FK + Jacobian	~1.8 μs	~10 μ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 μs (tape)	~35 μs (Python)	~4 μs (CodeGen)
CodeGen 后耗时	~3 μs (.so)	—	~4 μ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 ≈ 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 μs	\(10^{-16}\)	0 (内置)	无
CppADCodeGen .so	~3 μs	\(10^{-16}\)	~50 行设置	低
CppAD tape 回放	~12 μs	\(10^{-16}\)	~30 行设置	低
CasADi SX (C 代码生成)	~4 μs	\(10^{-16}\)	~20 行 Python	低
有限差分 (中心)	~50 μs	\(10^{-8}\)	~5 行	无
有限差分 (单侧)	~25 μs	\(10^{-4}\)	~3 行	无

7.2 在 MPC 中的实际影响¶

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

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

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

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

DOF	手写导数	CodeGen	tape	有限差分
6	~3.8 μs	~2.5 μs	~10 μs	~40 μs
7	~4.5 μs	~3.0 μs	~12 μs	~50 μs
12	~9 μs	~6 μs	~25 μs	~100 μs
30	~20 μs	~15 μs	~60 μs	~600 μ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 μs	36 μs	36 μs
动力学导数 (Jacobian x 20)	60 μs	240 μs	1000 μs
代价函数 + 梯度	15 μs	30 μs	100 μs
碰撞代价 + 梯度 (x 20)	40 μs	80 μs	300 μs
QP 构建 (Riccati 分解)	80 μs	80 μs	80 μs
QP 求解 (HPIPM)	120 μs	120 μs	120 μs
线搜索 + 更新	30 μs	30 μs	30 μs
单次 SQP 迭代总计	381 μs	616 μs	1666 μs
可行 SQP-RTI 频率	2.6 kHz	1.6 kHz	600 Hz

关键发现：

导数计算占总耗时的 30%（CodeGen）到 84%（有限差分）
CodeGen 使得导数计算**不再是瓶颈**——QP 求解 (120 μs) 反而成为最大单项
有限差分方案中，导数计算 (1400 μ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 增强的动力学模块 ⭐⭐¶

本章新增模块¶

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μs) < Pinocchio 内置解析导数(~4.5μs) < CppAD tape(~12μs) < 有限差分(~50μ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 的工业级写法

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
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