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

第47章：Pinocchio 深度精读——CRTP + Model-Data + 模板 Scalar¶

难度：⭐⭐⭐ | 建议用时：1.5 周 | 前置要求：Eigen 深度章节（02_C++基础与进阶/40_通用库剖析，表达式模板/对齐/SIMD）、李群与 manif（01_数学/20_微分几何与李群）、CRTP/设计模式章节（02_C++基础与进阶/10_C++语言核心）

前置自测¶

📋 答不出 >= 2 题 → 先回顾对应前置章节

CRTP 基础：写出 CRTP 的基本结构——基类模板 Base<Derived> 如何通过 static_cast<Derived*>(this) 在编译期派发到派生类？与虚函数相比，CRTP 在 vtable 查找和内联优化上有什么具体差异？（答不出 → 回顾 02_C++基础与进阶/10_C++语言核心中 CRTP 部分）
Eigen 模板参数：Eigen::Matrix<Scalar, Rows, Cols> 中的 Scalar 模板参数意味着什么？如果将 Scalar 从 double 替换为 Ceres::Jet<double, N>，矩阵的加法和乘法运算是否仍然正确？为什么？（答不出 → 回顾 Eigen 深度章节 + 01_数学/30_优化理论 Ceres 自动微分）
SE(3) 运算：给定两个齐次变换矩阵 $T_1, T_2 \in SE(3)$，$T_1 \cdot T_2$ 表示什么物理含义？manif 中如何用 compose() 完成这个运算？逆变换 $T^{-1}$ 的闭式表达式为什么不是 $T^T$？（答不出 → 回顾 01_数学/20_微分几何与李群群作用部分）
线程安全：const 引用为什么能避免数据竞争？如果两个线程同时读一个 const 对象、同时各自写一个独立对象，需要 mutex 吗？为什么？（答不出 → 回顾 02_C++基础与进阶/20_并发与系统编程实时约束部分）
动力学方程：写出拉格朗日形式的多刚体动力学方程 $M(q)\ddot{q} + C(q, \dot{q})\dot{q} + g(q) = \tau$，解释 $M(q)$ 的物理含义和数学性质（为什么是正定对称矩阵？）。（答不出 → 补充经典力学 / 机器人学导论基础）

本章目标¶

学完本章，你将能够：

定位 Pinocchio 在 INRIA 学派机器人 C++ 生态中的"基础设施"角色——理解为什么 Crocoddyl / TSID / OCS2 / Aligator / Pink 全部以它为动力学后端，以及这与 Sophus / manif 处理"孤立 SE(3)"的本质区别
精读 Pinocchio 的 CRTP 关节类型系统——从 JointModelBase<Derived> 出发，理解 12+ 种关节类型如何在编译期完成 calc() 派发，与 02_C++基础与进阶/10_C++语言核心讲过的 Sophus SO3Base 一个类型家族的 CRTP 进行规模对比
掌握 Model / Data 分离范式——理解为什么这个设计让 Pinocchio 天然多线程安全且零运行时 malloc，与 MuJoCo 的 mjModel / mjData 做精确字段映射
掌握模板化 Scalar 类型——理解同一份 rnea.hxx 如何服务 double / CppAD::AD<double> / CppAD::cg::CG<double> / 多精度浮点四种用途（本章上半部预览，47.4 详细展开）
能独立 加载 URDF，调用正运动学（FK）和 Jacobian 计算——为后续足式/40_CppAD与代码生成 CppAD 微分和足式/90_WBC分层优化与TSID TSID WBC 打好 API 基础

本章在课程中的位置：本章是足式方向（足式/30_Pinocchio深度精读）、机械臂方向（M01）、复合方向（复合/10_复合机器人全景引用）的**共享基础设施章节**。前面的 Eigen 深度章节让我们掌握了矩阵运算引擎，01_数学/20_微分几何与李群让我们掌握了单个 SE(3) 的李群运算，CRTP 章节让我们理解了 CRTP 的编译期多态原理。但这些都是"组件级"的知识。Pinocchio 把这些组件组装成了一个**完整的刚体动力学引擎**——它处理的不是一个变换矩阵，而是整棵关节树上 N 个串联变换的联合动力学。理解 Pinocchio 的设计，是理解整个 INRIA 学派规控生态（Crocoddyl、TSID、OCS2、Aligator）的前提。

47.1 Pinocchio 在机器人 C++ 生态中的位置 ⭐⭐¶

动机：从"一个 SE(3)"到"N 个串联 SE(3)" ⭐¶

前面的章节里，我们用 Sophus 和 manif 做了大量的 SE(3) 运算——一个旋转、一个位姿、相邻两帧的 delta。这些库解决的是"孤立的刚体变换"问题：给定两个坐标系之间的变换关系，如何组合、求逆、计算切向量、传播协方差。

但当你面对一个真实的机器人时，问题的规模发生了质变。考虑一个 12-DOF 的 Unitree Go2 四足机器人：

基座 (6-DOF 浮动) → 左前髋(Revolute) → 左前膝(Revolute) → 左前踝(Revolute) → 足端
                  → 右前髋(Revolute) → 右前膝(Revolute) → 右前踝(Revolute) → 足端
                  → 左后髋(Revolute) → 左后膝(Revolute) → 左后踝(Revolute) → 足端
                  → 右后髋(Revolute) → 右后膝(Revolute) → 右后踝(Revolute) → 足端

这是一棵**关节树**。每个关节有自己的关节变量 $q_i$、速度 $\dot{q}_i$、加速度 $\ddot{q}_i$，每个关节处有一个局部的 SE(3) 变换 $T_i(q_i)$。从基座到某条腿的足端，需要将这条链上所有局部变换**串联起来**：

\[T_{\text{foot}} = T_{\text{base}} \cdot T_{\text{hip}}(q_1) \cdot T_{\text{knee}}(q_2) \cdot T_{\text{ankle}}(q_3)\]

更重要的是，我们不仅需要位姿，还需要这条链的**速度传播**（用于雅可比计算）、力传播（用于逆动力学）、惯量聚合（用于惯量矩阵），以及这些量对 $q, \dot{q}, \ddot{q}$ 的**导数**（用于 MPC 优化）。

Sophus 和 manif 完全无法处理这个问题——它们不知道"关节"是什么、"关节树"是什么、"递归算法"是什么。它们只是 SE(3) 的计算器，不是动力学引擎。

Pinocchio 就是这个引擎。 Sophus 之于 Pinocchio，就像单个音符之于整首交响曲——Sophus 处理一个孤立的 SE(3) 变换，Pinocchio 则指挥整棵关节树上数十个 SE(3) 变换的协调运算，包括速度传播、力传递和惯量聚合。

核心算法：Featherstone 递归族 ⭐⭐¶

Pinocchio 实现了 Roy Featherstone《Rigid Body Dynamics Algorithms》（2008）中的经典递归算法。这些算法的共同特征是**利用关节树的拓扑结构，通过一次或两次遍历完成全局动力学量的计算**：

算法	全称	功能	输入 → 输出	复杂度	Go2 典型耗时
RNEA	Recursive Newton-Euler	逆动力学	$(q, v, a) \to \tau$	$O(N)$	~0.8 $\mu$s
ABA	Articulated-Body	正动力学	$(q, v, \tau) \to a$	$O(N)$	~1.2 $\mu$s
CRBA	Composite Rigid Body	惯量矩阵	$q \to M(q)$	$O(N^2)$	~0.6 $\mu$s
Cholesky	—	线性求解	$M, b \to M^{-1}b$	$O(N^3)$	可忽略（$N$ 小）

这里 $N$ 是关节数（不是机器人自由度 $n_v$，但对串联链两者通常相同量级）。RNEA 的 $O(N)$ 复杂度意味着即使关节数翻倍，计算时间也只是线性增长——这是递归利用树结构的直接结果，后续 47.6 节会从递推公式层面详细拆解。RNEA 的两遍递归好比**从树叶到树根的力传递**：第一遍（前向）从根到叶传播速度和加速度，就像从树干到树梢逐级传导晃动；第二遍（后向）从叶到根汇聚力和力矩，就像风吹树叶产生的力经过每根树枝逐级累加，最终汇聚到树干——每个关节只需处理自己那一段，所以总计算量与关节数成线性关系。

杀手级特性：解析导数 ⭐⭐⭐¶

Pinocchio 真正区别于其他动力学库（如 RBDL、Drake 的 MultibodyPlant）的杀手级特性是：它提供了上述算法的解析导数（analytical derivatives）。

这里要区分三种求导方式：

数值差分（Numerical Differentiation）：对每个变量做有限差分 $\partial f / \partial q_i \approx (f(q + \epsilon e_i) - f(q)) / \epsilon$。需要 $n_v$ 次额外的函数求值。对 12-DOF 系统，RNEA 的完整雅可比需要调用 RNEA 13 次。精度受 $\epsilon$ 选取影响，太大有截断误差，太小有舍入误差。

自动微分（Automatic Differentiation, AD）：通过 operator overloading（如 CppAD 的 AD<double> 类型）在前向计算的同时记录计算图，然后反向传播得到梯度。精度等于机器精度，但运行时有 tape 记录和回放的开销，通常比原始计算慢 3-10 倍。

解析导数（Analytical Derivatives）：根据 RNEA / ABA 的**数学递推结构**，手工推导出导数的闭式递推公式，然后直接实现为代码。这些公式发表在 Carpentier & Mansard 2018（RNEA 导数）和 Singh et al. 2022（ABA 导数）等论文中，实现在 Pinocchio 的 rnea-derivatives.hpp / aba-derivatives.hpp 等文件里。

性能数据（7-DOF 机械臂，Carpentier et al. 2019 基准测试）：

求导方法	RNEA 导数耗时	相对解析导数
数值差分	~15 $\mu$s	3x 慢
CppAD AD	~20 $\mu$s	4x 慢
Pinocchio 解析导数	~5 $\mu$s	基准

当 MPC 优化器每次迭代需要调用数百次动力学导数时，3-4 倍的性能差距意味着 MPC 实时性的成败。这就是为什么 Crocoddyl / Aligator 能做到微秒级的 backward pass——底层原因是 Pinocchio 的解析导数。

生态图：一个库撑起一个学派 ⭐¶

Pinocchio 不是一个孤立的库。它是 INRIA（法国国家信息与自动化研究所）Gepetto 团队的动力学内核，整个 INRIA 学派的规控生态都建立在它之上：

                     Pinocchio (动力学内核)
                           │
         ┌─────────────────┼─────────────────────┐
         │                 │                     │
      Crocoddyl          TSID                  OCS2
      (DDP 轨迹优化)     (WBC 逆动力学)         (腿足 MPC)
         │                 │                     │
         └─────────────────┼─────────────────────┘
                           │
                      Aligator (下一代,
                      ProxDDP 约束优化)

   Pink       ← 逆运动学（Python层，底层调用 Pinocchio FK/Jacobian）
   HPP-FCL / Coal  ← 可微碰撞检测（与 Pinocchio 深度集成）
   Stack-of-Tasks  ← 分层控制器家族
   ProxSuite       ← QP 求解器（OCS2/Aligator 的内层求解）

Pinocchio 3.x 最新进展（截至 2025 年）： - 椭球体关节（ellipsoid joints）：支持更灵活的关节约束建模 - mimic 关节 URDF 解析：正确处理 URDF 中的 <mimic> 标签（联动关节） - Coal 集成：HPP-FCL 的下一代碰撞检测库，支持可微碰撞距离计算 - CasADi 后端：除 CppAD 外，新增对 CasADi 符号框架的原生支持 - 改进的 Python 绑定：基于 eigenpy 3.x，NumPy 2.0 兼容

💡 一句话定位："Pinocchio 对机器人动力学计算的意义，等同于 Eigen 对数值线性代数的意义——它是基础设施层，不是应用层。你不直接用它写控制器，但每一个控制器都依赖它。"

本质洞察：Pinocchio 的价值不是"又一个动力学库"，而是它把**递归算法的数学优雅性**和**现代 C++ 的工程安全性**统一在了同一套代码中。RNEA 的 $O(N)$ 复杂度来自对树结构的递归利用，模板 Scalar 的泛型设计来自 C++ 的零开销抽象——两者缺一不可。理解 Pinocchio，本质上是理解"数学结构如何驱动软件架构"。

47.2 CRTP 关节类型系统 ⭐⭐⭐¶

动机：1kHz 控制循环中 vtable 开销不可接受 ⭐⭐¶

在 CRTP 章节（02_C++基础与进阶/10_C++语言核心）中我们讨论过 CRTP 和虚函数的取舍——架构层用虚函数，热路径用 CRTP。现在让我们用具体数字来看 Pinocchio 为什么必须选择 CRTP。

一个腿足 MPC 控制器的典型执行路径：

1kHz 主循环 → MPC 求解（每次迭代调用 RNEA 200+ 次）
                              ↓
           每次 RNEA → 遍历 N 个关节 → 对每个关节调用 calc() / motion() / force()

以 Go2 四足为例：$N = 13$（1 个 FreeFlyer 基座 + 12 个 Revolute 关节），每次 RNEA 调用 calc() 13 次。MPC 一次求解调用 RNEA 200 次 = calc() 被调用 2600 次。1kHz 意味着每秒执行 1000 次 MPC → calc() 每秒被调用 260 万次。

虚函数每次调用的额外开销是 2-5 ns（vtable 间接寻址 + 阻止内联 + 阻止 SIMD 向量化）。260 万次 $\times$ 3 ns = **约 8 ms / 秒**的纯多态开销。这在 1ms 的控制周期预算中占比不小，而且更严重的是，vtable 跳转破坏了指令流水线，阻止编译器将 calc() 的函数体内联到 RNEA 的循环中，进而阻止了循环级别的 SIMD 优化。

CRTP 消灭了这些开销——calc() 的派发在编译期完成，函数体被内联，编译器可以将整个关节运算循环向量化。

CRTP 层级结构：JointModelBase ⭐⭐⭐¶

Pinocchio 的关节类型系统的 CRTP 基类定义在 include/pinocchio/multibody/joint/joint-base.hpp 中（约 300 行）。核心结构如下：

// 基类——所有关节模型的公共接口
template <typename Derived>
struct JointModelBase
{
    // CRTP 核心：把 this 转型为派生类引用
    Derived& derived() { return *static_cast<Derived*>(this); }
    const Derived& derived() const
    { return *static_cast<const Derived*>(this); }

    // 所有算法通过 derived() 派发到具体关节类型
    template <typename ConfigVector>
    void calc(JointDataBase<...>& data,
              const Eigen::MatrixBase<ConfigVector>& q) const
    {
        derived().calc_impl(data, q);  // 编译期决定调用谁
    }

    // 关节的运动子空间维度（编译期常量或运行期值）
    int nq() const { return derived().nq_impl(); }
    int nv() const { return derived().nv_impl(); }
};

派生类列表——Pinocchio 3.x 支持 12+ 种关节类型，每一种都是 CRTP 派生：

template <typename Scalar, int Options = 0>
struct JointModelRevoluteTpl           // 旋转关节（绕固定轴，nq=1, nv=1）
    : JointModelBase<JointModelRevoluteTpl<Scalar, Options>> { ... };

template <typename Scalar, int Options = 0>
struct JointModelRevoluteUnalignedTpl  // 任意轴旋转关节
    : JointModelBase<JointModelRevoluteUnalignedTpl<Scalar, Options>> { ... };

template <typename Scalar, int Options = 0>
struct JointModelPrismaticTpl          // 平动关节（nq=1, nv=1）
    : JointModelBase<JointModelPrismaticTpl<Scalar, Options>> { ... };

template <typename Scalar, int Options = 0>
struct JointModelSphericalTpl          // 球关节（nq=4 四元数, nv=3）
    : JointModelBase<JointModelSphericalTpl<Scalar, Options>> { ... };

template <typename Scalar, int Options = 0>
struct JointModelFreeFlyerTpl          // 6-DOF 浮动基座（nq=7, nv=6）
    : JointModelBase<JointModelFreeFlyerTpl<Scalar, Options>> { ... };

template <typename Scalar, int Options = 0>
struct JointModelPlanarTpl             // 平面关节（nq=4, nv=3）
    : JointModelBase<JointModelPlanarTpl<Scalar, Options>> { ... };

template <typename Scalar, int Options = 0>
struct JointModelTranslationTpl        // 纯平动（nq=3, nv=3）
    : JointModelBase<JointModelTranslationTpl<Scalar, Options>> { ... };

template <typename Scalar, int Options = 0>
struct JointModelHelicalTpl            // 螺旋关节（旋转+平动耦合）
    : JointModelBase<JointModelHelicalTpl<Scalar, Options>> { ... };

// 以及 Composite（组合关节）、Mimic（镜像关节）、Universal（万向节）等

注意 nq 和 nv 的区别——这是 Pinocchio（和 MuJoCo）中一个关键概念：nq 是配置空间维度（用于存储关节位置），nv 是切空间维度（用于存储关节速度和加速度）。对于旋转关节两者都是 1，但对于球关节 nq=4（四元数）而 nv=3（角速度），对于浮动基座 nq=7（位置3 + 四元数4）而 nv=6（线速度3 + 角速度3）。这个差异来源于李群的流形结构——配置空间是流形，切空间是线性空间，两者维度不必相等。

与 Sophus CRTP 的规模对比 ⭐⭐¶

在 CRTP 章节中我们分析过 Sophus 的 CRTP：

Sophus:  SO3Base<Derived>  →  SO3<Scalar>
                           →  Map<SO3<Scalar>>
                           →  Map<const SO3<Scalar>>

这是**一个类型家族**的 CRTP——同一种数学对象（SO(3) 旋转）的不同存储形式（自有存储 vs 映射外部内存）共享运算代码。CRTP 在这里解决的是"如何让 SO3 和 Map<SO3> 共用 inverse()、log()、operator* 等运算而不用虚函数"。派生类总共 3 个。

Pinocchio 的 CRTP 是另一个量级：

Pinocchio:  JointModelBase<Derived>  →  JointModelRevoluteTpl
                                     →  JointModelRevoluteUnalignedTpl
                                     →  JointModelPrismaticTpl
                                     →  JointModelSphericalTpl
                                     →  JointModelFreeFlyerTpl
                                     →  JointModelPlanarTpl
                                     →  JointModelTranslationTpl
                                     →  JointModelHelicalTpl
                                     →  JointModelCompositeTpl
                                     →  JointModelMimicTpl
                                     →  JointModelUniversalTpl
                                     →  ... (12+ 种)

这是**不同物理对象**的 CRTP——十几种关节类型，每种有不同的 nq/nv、不同的 calc() 实现、不同的运动子空间矩阵 $S_i$。它们共享的是算法**接口**（calc、jacobian、motion、force），而不是实现。

calc() 的编译期派发 ⭐⭐⭐¶

当 RNEA 遍历关节树时，对每个关节调用 calc()。由于关节类型在编译期已知（通过 CRTP），编译器可以将 calc() 的实际实现内联到 RNEA 的递推循环中。以旋转关节为例：

// JointModelRevoluteTpl::calc_impl —— 旋转关节的核心计算
// 给定关节角 q_i，计算该关节的局部变换 M_i 和运动子空间 S_i
template <typename Scalar, int Options>
template <typename ConfigVector>
void JointModelRevoluteTpl<Scalar, Options>::calc_impl(
    JointDataRevoluteTpl<Scalar, Options>& data,
    const Eigen::MatrixBase<ConfigVector>& q) const
{
    const Scalar& q_i = q[idx_q()];     // 取出该关节对应的配置变量
    data.M.rotation(q_i);                // 用 q_i 构造绕轴的旋转矩阵
    data.S.setZero();                    // 运动子空间：只有一个分量非零
    data.S(axis_) = Scalar(1);
    // 整个函数只有几条赋值——编译器将其完全内联
}

对比虚函数版本——编译器在 RNEA 的循环中看到的是 joint_ptr->calc(data, q)，无法内联，必须在运行时查 vtable → 读函数指针 → 跳转。这三步操作本身只有几纳秒，但它阻止了编译器对循环整体的优化（内联、SIMD、指令重排序）。

CRTP 的"异构容器"难题与 Variant 解法 ⭐⭐⭐¶

纯 CRTP 有一个致命限制：不同 Derived 类型在编译期是**不同的类型**，无法放进同一个 std::vector。但一个机器人的关节链里每个关节的类型可能不同——Go2 的基座是 FreeFlyer，髋膝踝都是 Revolute——我们需要把这些不同类型按顺序存入同一个容器。

Pinocchio 的解法是 CRTP + boost::variant（C++17 环境下等价于 std::variant）：

// 将所有可能的关节类型注册到 variant 类型列表中
typedef boost::variant<
    JointModelRevoluteTpl<Scalar, Options>,
    JointModelPrismaticTpl<Scalar, Options>,
    JointModelSphericalTpl<Scalar, Options>,
    JointModelFreeFlyerTpl<Scalar, Options>,
    JointModelCompositeTpl<Scalar, Options>,
    // ... 共 12+ 种
> JointModelVariant;

// Model 类持有异构关节的 vector
struct ModelTpl {
    std::vector<JointModelVariant> joints;  // 每个元素可以是不同关节类型
    // ...
};

// 算法通过 visitor 模式分派到具体类型
struct CalcVisitor : boost::static_visitor<void> {
    template <typename JointModel>
    void operator()(const JointModel& jmodel) const {
        jmodel.calc(jdata, q);  // 此处 JointModel 是具体类型
    }
};

// RNEA 递推中对每个关节的调用
boost::apply_visitor(CalcVisitor{jdata, q}, model.joints[i]);

variant 的分派机制是**编译器生成的 switch-case**（根据 variant 内部的类型索引跳转），而不是 vtable 间接寻址。关键区别在于：switch-case 的每个 case 分支中，编译器**知道**具体类型是什么，因此可以将 calc() 的实现**内联**到 case 分支中。vtable 做不到这一点——编译器通过函数指针调用时无法内联。

性能对比（7-DOF 机械臂 RNEA，Carpentier et al. 2019）：

多态方式	RNEA 耗时	相对基准	内联可能性
虚函数继承	~2.5 $\mu$s	178%	不可能
CRTP + variant	~1.4 $\mu$s	100%	部分可能
纯 CRTP（假设可行）	~1.3 $\mu$s	93%	完全可能

⚠️ 陷阱：自定义关节类型需要修改 Pinocchio 的 variant 列表

如果你需要添加一种 Pinocchio 不支持的关节类型（例如柔性关节、腱驱动关节），仅仅写一个新的 JointModelMyCustom : JointModelBase<...> 是不够的。你还必须将它加入 JointModelVariant 的类型列表中，否则它无法被存入 Model::joints。这意味着你需要修改 Pinocchio 的头文件（joint-collection.hpp），然后重新编译 Pinocchio。这是 variant 方案相比虚函数继承的一个工程代价——虚函数继承允许在不修改框架代码的前提下添加新的派生类型。Pinocchio 团队正在通过 JointModelComposite 缓解这个问题——你可以将自定义关节表达为已有基本关节类型的组合，而不必注册新类型。

47.3 Model / Data 分离范式 ⭐⭐¶

动机——反面：如果不分离会怎样 ⭐⭐¶

假设我们不分离 Model 和 Data，而是把拓扑信息和计算缓冲都放在同一个结构体里：

// 反面设计：everything-in-one
struct RobotState {
    // 拓扑信息（不变）
    std::vector<JointType> joint_types;
    std::vector<SE3> joint_placements;
    std::vector<Inertia> link_inertias;

    // 计算缓冲（每次算法调用都会被覆写）
    std::vector<SE3> oMi;            // 每个关节在世界坐标系下的位姿
    Eigen::MatrixXd J;               // 雅可比矩阵
    Eigen::MatrixXd M;               // 惯量矩阵
    Eigen::VectorXd nle;             // 非线性效应项（科氏力+重力）
};

这个设计的问题：

问题 1：多线程时必须复制整个对象。 假设 MPC 线程和 WBC 线程都需要做动力学计算。由于 oMi、J、M 等缓冲是可变的，两个线程不能同时写同一个对象。要么加 mutex（串行化，浪费第二个核心），要么每个线程拷贝一份 RobotState。但这意味着**每个线程都持有了 joint_types、joint_placements、link_inertias 的副本**——这些数据从 URDF 加载后就再也不会改变，复制它们纯粹是内存浪费。对于一个 37-DOF 的人形机器人，Model 的常量数据占几百 KB，如果有 4 个线程各复制一份，就浪费了接近 1 MB——在嵌入式控制器上这不是小数目。

问题 2：无法判断哪些字段是陈旧的。 当你调用正运动学更新了 oMi，然后去读 J——J 的值是上次 computeJointJacobians 计算的结果，可能对应的是**上一个** $q$ 值。在 everything-in-one 设计中，没有任何机制告诉你"哪些缓冲已经过期"。你需要记住调用顺序的依赖关系，这是 bug 的温床。

问题 3：构造函数职责不清。 每次创建新线程，你不知道应该从哪里复制——是深拷贝整个对象？还是只拷贝计算缓冲？常量数据该共享还是复制？

理论：Model 持有不变量，Data 持有缓冲 ⭐⭐¶

Pinocchio 的解法非常干净：

pinocchio::Model——从 URDF / MJCF 解析后构造，此后**只读**： - 关节拓扑（父子关系、关节类型、关节在父 body 中的放置位姿 jointPlacements） - 惯性参数（每个 link 的质量 inertias、质心位置） - 关节限位（lowerPositionLimit、upperPositionLimit、velocityLimit、effortLimit） - 命名信息（关节名 names、frame 名 frames） - 全局维度常量（nq、nv、njoints、nframes、nbodies）

pinocchio::Data——根据 Model 的维度**一次性预分配**所有缓冲，此后每次算法调用只**覆写**： - oMi：每个关节在世界坐标系下的 SE(3) 位姿（std::vector<SE3>，长度 njoints） - v、a：每个关节的 6D 空间速度和加速度 - f：每个关节的 6D 空间力 - J：雅可比矩阵（$6 \times n_v$） - M：广义惯量矩阵（$n_v \times n_v$） - nle：非线性效应项（$n_v \times 1$） - ddq：广义加速度结果 - tau：广义力/力矩结果 - 以及 Cholesky 分解缓冲、RNEA 导数缓冲等——共 30+ 个预分配成员

Data 完整字段清单（按功能分组）：

理解 Data 的字段结构对调试和性能优化至关重要——不同的算法写入不同的字段，弄混了就会读到过期数据。下表按功能分组列出 Data 最重要的成员，标注了哪些算法会更新它们：

分组	字段名	类型	含义	由哪些算法填充
运动学	`oMi`	`std::vector<SE3>`	关节在世界坐标系下的位姿	`forwardKinematics`, `computeAllTerms`
	`oMf`	`std::vector<SE3>`	frame 在世界坐标系下的位姿	`updateFramePlacements`（须在 FK 之后调用）
	`liMi`	`std::vector<SE3>`	关节相对于父 body 的局部变换	`forwardKinematics`
	`v`	`std::vector<Motion>`	每个关节的 6D 空间速度	`forwardKinematics(model, data, q, v)`
	`a`	`std::vector<Motion>`	每个关节的 6D 空间加速度	`forwardKinematics(model, data, q, v, a)`
雅可比	`J`	`Matrix6x` (6 x nv)	完整雅可比矩阵（所有关节）	`computeJointJacobians`
	`dJ`	`Matrix6x` (6 x nv)	雅可比时间导数	`computeJointJacobiansTimeVariation`
动力学	`M`	`MatrixXd` (nv x nv)	广义惯量矩阵	`crba`, `computeAllTerms`
	`nle`	`VectorXd` (nv)	非线性效应项 $C\dot{q}+g$	`nonLinearEffects`, `computeAllTerms`
	`g`	`VectorXd` (nv)	重力项	`computeGeneralizedGravity`
	`tau`	`VectorXd` (nv)	RNEA 输出的关节力矩	`rnea`
	`ddq`	`VectorXd` (nv)	ABA 输出的关节加速度	`aba`
	`f`	`std::vector<Force>`	每个关节处的 6D 空间力	`rnea`（RNEA 反向递归中间量）
导数	`dtau_dq`	`MatrixXd` (nv x nv)	$\partial\tau/\partial q$	`computeRNEADerivatives`
	`dtau_dv`	`MatrixXd` (nv x nv)	$\partial\tau/\partial v$	`computeRNEADerivatives`
	`Minv`	`MatrixXd` (nv x nv)	$M^{-1}$（Cholesky 求逆）	`computeMinverse`
质心	`com[0]`	`Vector3d`	系统质心位置	`centerOfMass`
	`vcom[0]`	`Vector3d`	系统质心速度	`centerOfMass`（传入 v）
	`Jcom`	`Matrix3x` (3 x nv)	质心雅可比	`jacobianCenterOfMass`

⚠️ 陷阱：算法之间的依赖关系

这些字段之间存在隐式的依赖关系。例如 computeJointJacobians(model, data, q) 内部会先调用 forwardKinematics，但不会自动调用 updateFramePlacements。如果你接下来用 getFrameJacobian 查 frame 的雅可比，必须确保 updateFramePlacements 已被调用。Pinocchio 提供了一个便利函数 computeAllTerms(model, data, q, v)，一次调用填充 FK + Jacobian + M + nle + CoM 等多个字段，避免遗漏依赖——但它不含 RNEA 导数和 ABA。

computeAllTerms 的正确使用：

// computeAllTerms 是 WBC 中最常用的"一键计算"函数
// 它等价于依次调用：forwardKinematics + crba + nonLinearEffects
//                  + computeJointJacobians + centerOfMass
pinocchio::computeAllTerms(model, data, q, v);

// 调用后可直接读取：
// data.oMi      -> 关节位姿
// data.M        -> 惯量矩阵
// data.nle      -> 非线性效应项 (C*dq + g)
// data.J        -> 完整雅可比
// data.com[0]   -> 质心位置
// data.Jcom     -> 质心雅可比

// 但仍需手动调用：
pinocchio::updateFramePlacements(model, data);  // oMf
pinocchio::computeRNEADerivatives(model, data, q, v, a);  // 导数

如果你的控制循环既需要 FK、雅可比、惯量矩阵又需要质心，用 computeAllTerms 比分别调用节省约 30% 计算时间（因为中间变量只算一次）。但如果你只需要 FK 不需要惯量矩阵，单独调用 forwardKinematics 更快——computeAllTerms 会计算你可能不需要的 $M$ 和 $n_{le}$。

所有算法函数的签名都遵循同一模式：

// const Model& → 只读，不修改
// Data& → 可写，算法将结果写入 Data 的缓冲
pinocchio::forwardKinematics(const Model& model, Data& data,
                             const Eigen::VectorXd& q);

pinocchio::computeJointJacobians(const Model& model, Data& data,
                                 const Eigen::VectorXd& q);

pinocchio::rnea(const Model& model, Data& data,
                const Eigen::VectorXd& q,
                const Eigen::VectorXd& v,
                const Eigen::VectorXd& a);

pinocchio::crba(const Model& model, Data& data,
                const Eigen::VectorXd& q);

这个设计的思想本质是**函数式编程在 C++ 中的体现**：Model 是不可变数据（immutable），算法函数是纯函数（输入 Model + q/v/a → 副作用写入 Data），Data 是显式的副作用容器。这好比一个连锁餐厅的运营模式：菜谱（Model）全店统一、永远不改，每个厨房（线程）各自有一套锅碗瓢盆（Data），厨师按菜谱操作自己的工具，互不干扰。换一个更精确的工程类比：Model 和 Data 的关系好比**模具和工件**——模具（Model）定义了形状和尺寸，一旦设计定型就不再修改；工件（Data）是每次加工的产物，用同一套模具可以在不同的车床（线程）上同时生产不同的工件，彼此互不干扰，而且绝不会有人在加工过程中去改模具的形状。

本质洞察：Model/Data 分离表面上是"多线程优化"，但其更深层的价值在于**语义清晰性**——它强制工程师区分"机器人是什么"（拓扑、惯量、限位，不随时间变化）和"机器人此刻在做什么"（位姿、速度、力，每个控制周期都在变化）。这种区分消除了一整类 bug：你不可能意外地在算法执行过程中改变关节拓扑或惯量参数，因为 Model 是 const 的。

线程安全：N 线程共享 1 个 Model ⭐⭐⭐¶

Model / Data 分离的直接收益是**天然的多线程安全**：

// 构造：一次性加载 URDF
pinocchio::Model model;
pinocchio::urdf::buildModel("go2.urdf",
                            pinocchio::JointModelFreeFlyer(),  // 浮动基座
                            model);

// MPC 线程：自己的 Data
std::thread mpc_thread([&model]() {
    pinocchio::Data data_mpc(model);   // 根据 model 维度预分配
    while (running) {
        pinocchio::forwardKinematics(model, data_mpc, q_current);
        pinocchio::computeJointJacobians(model, data_mpc, q_current);
        // ... 用 data_mpc.oMi 和 data_mpc.J 做 MPC 计算
    }
});

// WBC 线程：自己的 Data
std::thread wbc_thread([&model]() {
    pinocchio::Data data_wbc(model);   // 独立的缓冲
    while (running) {
        pinocchio::rnea(model, data_wbc, q, v, a);
        pinocchio::crba(model, data_wbc, q);
        // ... 用 data_wbc.tau 和 data_wbc.M 做 WBC 计算
    }
});

// 可视化线程：又一个独立 Data
std::thread viz_thread([&model]() {
    pinocchio::Data data_viz(model);
    while (running) {
        pinocchio::forwardKinematics(model, data_viz, q_current);
        // ... 用 data_viz.oMi 渲染
    }
});

// 三个线程共享一个 const Model，各自有独立 Data
// 不需要任何 mutex、atomic 或 lock

对照并发编程章节（02_C++基础与进阶/20_并发与系统编程）中分析的 ORB-SLAM3——它有 5 个 mutex 保护 MapPoint、KeyFrame、关键帧队列等共享可变状态。根本原因是 SLAM 的地图是**动态增长**的（每帧都在添加/删除地图点），而腿足机器人的模型在 URDF 加载后是**静态不变**的。Model / Data 分离并不是万能的——它适用于"读多写少且常量数据和可变数据可以清晰划分"的场景。

与 MuJoCo mjModel / mjData 的映射 ⭐⭐¶

MuJoCo 独立发展出了几乎相同的 Model / Data 分离设计。两者的对应关系：

概念	Pinocchio	MuJoCo	说明
不可变模型	`pinocchio::Model`	`mjModel`	URDF/MJCF 解析后只读
可变缓冲	`pinocchio::Data`	`mjData`	算法写入，一次性预分配
配置空间维度	`model.nq`	`m->nq`	关节位置向量长度
速度空间维度	`model.nv`	`m->nv`	关节速度向量长度
关节数	`model.njoints`	`m->njnt`	—
body 质量	`model.inertias[i].mass()`	`m->body_mass[i]`	—
世界坐标系位姿	`data.oMi[i]` (SE3)	`d->xpos[i]` + `d->xmat[i]`	Pinocchio 用 SE3，MuJoCo 用分离的 pos+mat
雅可比矩阵	`data.J`	`mj_jac()` 写入用户缓冲	存储方式不同

关键差异：MuJoCo 的 mjModel 是纯 C 结构体（struct mjModel），所有成员都是裸指针 + 长度字段，面向最大性能和 GPU 兼容性。Pinocchio 的 Model 是 C++ 模板类（ModelTpl<Scalar, Options, JointCollection>），使用 std::vector<SE3>、Eigen::VectorXd 等 RAII 容器，面向类型安全和自动微分兼容性。两种设计哲学各有取舍——MuJoCo 追求仿真吞吐量（GPU 上百万环境并行），Pinocchio 追求算法可微性（解析导数 + AD 类型替换）。

与 Drake MultibodyPlant 和 RBDL 的对比 ⭐⭐¶

Pinocchio 并非唯一的 C++ 刚体动力学库。理解三者的定位差异能帮助你在不同项目中做出正确选型。

维度	Pinocchio	Drake MultibodyPlant	RBDL
开发团队	INRIA Gepetto（法国）	Toyota Research / MIT	Martin Felis（个人+社区）
设计定位	纯动力学引擎，极致性能 + AD	完整机器人仿真+规控框架	轻量级教学/原型动力学库
仿真能力	无仿真器（需搭配 MuJoCo/Gazebo）	内置多体仿真器 + 接触求解	无仿真器
AD 支持	CppAD / CppADCodeGen / CasADi（模板 Scalar）	`AutoDiffXd`（Drake 自研）	无原生 AD
解析导数	RNEA/ABA/CRBA 的闭式递推导数	无解析导数（依赖 AD）	无解析导数
闭环约束	v3.x 支持（ConstraintModel + Delassus）	完整支持（Loop Constraint）	有限支持（v2.6+）
Python 绑定	完整（eigenpy）	完整（pydrake）	有限
依赖大小	轻量（Eigen + Boost）	重量级（需 Bazel 构建，数 GB）	极轻量（几个头文件）
RNEA 性能（7-DOF）	~1.2 $\mu$s	~3-5 $\mu$s	~1.5 $\mu$s
上层控制框架	Crocoddyl, TSID, OCS2, Aligator	Drake 自带 MPC/轨迹优化	无

选型建议：

腿足 MPC/WBC（本课程的核心场景）：Pinocchio 是唯一选择——OCS2、Crocoddyl、TSID 都依赖它，且解析导数对 MPC 性能至关重要
机械臂操作 + 仿真一体化：Drake 更合适——它提供从仿真到规划到控制的完整管线，减少集成工作量
教学或快速原型验证：RBDL 最简单——几个头文件、零额外依赖，10 分钟即可跑通 RNEA
同时需要仿真 + 控制：Pinocchio + MuJoCo（仿真）或 Pinocchio + Gazebo（ROS 集成），而非 Drake 一体化方案——因为腿足社区的主流工具链围绕 Pinocchio 构建

反事实推理：如果 Pinocchio 不提供解析导数会怎样？OCS2 的 SQP-RTI 每次迭代需要完整的动力学 Jacobian。用数值差分，18-DOF 系统需要 37 次 RNEA 调用（中心差分）；用 CppAD，慢 3-5 倍。解析导数将这个代价压缩到 1 次递推，使 MPC 频率从 10-20 Hz 提升到 50-100 Hz——这就是为什么 OCS2 选择 Pinocchio 而非 Drake/RBDL 的决定性理由。

代码实战：从 URDF 到 FK ⭐¶

下面是一个完整的"加载 URDF → 创建 Model + Data → 计算正运动学"流程：

#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <iostream>

int main()
{
    // 1. 构造 Model：从 URDF 文件解析
    pinocchio::Model model;
    pinocchio::urdf::buildModel(
        "go2_description/urdf/go2.urdf",
        pinocchio::JointModelFreeFlyer(),   // 指定基座关节类型
        model);

    std::cout << "模型名称: " << model.name << "\n";
    std::cout << "nq = " << model.nq       // 配置空间维度（Go2: 7+12=19）
              << ", nv = " << model.nv      // 速度空间维度（Go2: 6+12=18）
              << ", njoints = " << model.njoints << "\n";

    // 2. 构造 Data：根据 Model 维度一次性预分配所有缓冲
    pinocchio::Data data(model);
    // 此时 data.oMi 已分配但值未初始化——不要在调用算法前读取！

    // 3. 生成随机合法配置（考虑关节限位和四元数归一化）
    Eigen::VectorXd q = pinocchio::randomConfiguration(model);

    // 4. 计算正运动学：填充 data.oMi（每个关节在世界坐标系下的 SE(3) 位姿）
    pinocchio::forwardKinematics(model, data, q);

    // 5. 读取结果
    for (int i = 0; i < model.njoints; ++i) {
        std::cout << "Joint " << model.names[i]
                  << " 位姿:\n" << data.oMi[i] << "\n";
    }

    // 也可以通过 frame 名称查找特定末端的位姿
    // 注意：需要先调用 updateFramePlacements
    pinocchio::updateFramePlacements(model, data);
    auto frame_id = model.getFrameId("FL_foot");  // 左前足端
    std::cout << "左前足端位姿:\n" << data.oMf[frame_id] << "\n";

    return 0;
}

⚠️ 陷阱：forwardKinematics 未调用前 oMi 是未初始化数据

刚用 pinocchio::Data data(model) 构造 Data 时，data.oMi 的内存已经分配，但值是**未初始化的**（或者是默认构造的单位变换，取决于 Pinocchio 版本）。如果你在调用 forwardKinematics 之前就读取 data.oMi[i]，得到的是**毫无意义的旧数据**。同样，data.J 在调用 computeJointJacobians 之前是脏数据，data.M 在调用 crba 之前是脏数据。Pinocchio 的 Data 不会自动标记"哪些缓冲已更新"——你必须记住算法调用的依赖顺序。这是 Model / Data 分离设计的一个工程代价：灵活性和性能换来了更高的用户责任。

常见 bug 场景：在 MPC 循环中，上一帧调用过 forwardKinematics，这一帧改了 $q$ 但忘记重新调用，然后直接读 data.oMi——读到的是**上一帧的位姿结果**，对应的是旧的 $q$ 值。程序不会报错，但控制器会表现出一帧的延迟，在高速运动时可能导致不稳定。

⚠️ 陷阱：buildModel 的基座关节类型必须显式指定

对于固定基座的机械臂，可以直接调用 pinocchio::urdf::buildModel("robot.urdf", model)——此时基座被视为固定在世界坐标系上。但对于腿足机器人（浮动基座），你**必须**传入 pinocchio::JointModelFreeFlyer() 作为第二个参数。如果遗漏了这个参数，Pinocchio 会把基座当作固定的，model.nq 和 model.nv 会少 7 和 6，所有后续的动力学计算都会得到错误的结果——因为浮动基座的 6 个自由度被忽略了。URDF 文件本身**不包含**基座关节类型的信息（URDF 规范假设基座固定），所以 Pinocchio 无法自动推断。

练习 ⭐⭐¶

练习 47.3.1（⭐⭐）：加载你的机器人 URDF（如果没有，用 Pinocchio 自带的 example-robot-data 中的 solo12 或 talos），打印 model.nq、model.nv、model.njoints、model.nframes。然后遍历 model.joints，对每个关节打印其名称和 nq_impl() / nv_impl() 值。验证所有关节的 nq 之和等于 model.nq，所有关节的 nv 之和等于 model.nv。

练习 47.3.2（⭐⭐⭐）：用两个线程模拟 MPC + WBC 的并行计算场景。主线程加载 URDF 构造 Model，然后启动两个线程：一个反复调用 forwardKinematics + computeJointJacobians，另一个反复调用 rnea + crba。两个线程各自持有独立的 Data，共享同一个 const Model。运行 10 秒，验证没有 crash、没有 data race（可用 ThreadSanitizer 编译检测）。然后尝试故意让两个线程共享同一个 Data 对象（不加锁），观察 ThreadSanitizer 报出的数据竞争警告。

47.3.6 空间代数原语——SE3 / Motion / Force / Inertia ⭐⭐⭐¶

动机：为什么不能用"普通的向量和矩阵"做动力学？ ⭐⭐¶

到目前为止我们讲了 Pinocchio 的架构（CRTP、Model/Data），但还没有触及它**计算时操纵的基本数据类型**。在 47.5 节我们马上会写出 RNEA 的递推公式，里面充斥着 ${}^i X_{\lambda(i)}$、$v_i \times S_i \dot q_i$、$I_i a_i$ 这样的符号。如果你不知道这些符号在 Pinocchio 里对应**哪个 C++/Python 类型、用哪个方法计算**，那些公式就只是黑板上的数学，无法落到代码。这一节补上这个缺口——它是连接 01_数学/20_微分几何与李群（孤立 SE(3)）和 05_运动控制/10_足式/50_空间向量代数与旋量（空间向量理论）与 Pinocchio 工程实现的桥梁。

回顾 01_数学/20_微分几何与李群：我们在那里用 Sophus/manif 处理过单个 SE(3) 变换的 compose、inverse、log。但那套工具处理的是"位姿"这一种几何对象。刚体动力学需要的对象更多——除了位姿，还有 6 维速度（线速度 + 角速度打包成一个对象）、6 维力（力 + 力矩打包）、6×6 空间惯量。考虑一个最朴素的问题：如何把一个刚体在 frame $A$ 下测得的速度，转换到 frame $B$ 下表达？

如果用"普通"的方式，你会把线速度 $v$ 和角速度 $\omega$ 分开存成两个 Vector3d，然后手写转换公式：

\[\omega_B = {}^B R_A\, \omega_A, \qquad v_B = {}^B R_A\, v_A + {}^B p_A \times ({}^B R_A\, \omega_A)\]

注意线速度的转换里**混入了角速度**（因为参考点变了，平移速度要加上 $\omega \times r$ 的牵连项）。这个耦合极易写错——少一项叉乘、符号反了，程序不报错但结果错。而且这只是速度；力、加速度、惯量各有各的转换规则，全部手写就是一场灾难。

反事实推理：如果 Pinocchio 把 6D 速度拆成两个独立的 Vector3d 来传播会怎样？RNEA 的正向递归 $v_i = {}^i X_{\lambda(i)} v_{\lambda(i)} + S_i \dot q_i$ 就要拆成"角速度递推 + 线速度递推（含耦合项）"两段代码，且每个关节类型的 $S_i$ 都要分别处理线/角分量。代码量翻倍、出错点翻倍。把线/角打包成一个 6D 对象、把坐标变换打包成一个 6×6 伴随算子，才能让递推公式写成一行——这就是空间代数（spatial algebra）存在的根本理由。

历史：Featherstone 的空间向量 ⭐¶

这套"把线量和角量打包成 6 维对象"的代数体系由 Roy Featherstone 在 1980 年代系统化，写进了《Rigid Body Dynamics Algorithms》（2008）。它的核心洞察是：刚体的瞬时运动（twist）天然是 6 维的——李代数 $\mathfrak{se}(3)$ 就是 6 维向量空间；刚体受的力（wrench）也是 6 维的——它是 $\mathfrak{se}(3)$ 的对偶空间 $\mathfrak{se}(3)^*$。一旦接受"6 维是自然维度"，所有动力学量的坐标变换都统一为 6×6 矩阵乘法，所有交叉耦合都统一为 6 维叉乘算子。Pinocchio 把 Featherstone 的数学对象一一映射为 C++ 类型，这是它能用几十行模板代码实现整个动力学的根基。05_运动控制/10_足式/50_空间向量代数与旋量专门讲这套理论的数学推导，本节只讲它在 Pinocchio 里的**类型与 API 落地**。

四个核心类型 ⭐⭐⭐¶

Pinocchio 的空间代数由四个带 Scalar 模板参数的类型构成，它们就是 47.4 节模板层级里"层级 1"的成员：

类型	数学对象	维度	物理含义	李群/代数
`SE3` (`SE3Tpl<Scalar>`)	刚体位姿 ${}^A M_B$	$R \in SO(3)$ + $p \in \mathbb{R}^3$	坐标系变换	群 $SE(3)$
`Motion` (`MotionTpl<Scalar>`)	空间速度 twist	$(v, \omega) \in \mathbb{R}^6$	线速度 + 角速度	代数 $\mathfrak{se}(3)$
`Force` (`ForceTpl<Scalar>`)	空间力 wrench	$(f, \tau) \in \mathbb{R}^6$	力 + 力矩	对偶 $\mathfrak{se}(3)^*$
`Inertia` (`InertiaTpl<Scalar>`)	空间惯量	$6 \times 6$ 对称正定	质量 + 质心 + 转动惯量	映射 $\mathfrak{se}(3) \to \mathfrak{se}(3)^*$

注意一个容易混淆的约定：Pinocchio 的 6D 向量布局是 [线性; 角度]——Motion 的前 3 维是线速度 $v$、后 3 维是角速度 $\omega$；Force 的前 3 维是力 $f$、后 3 维是力矩 $\tau$。这与某些教材（Featherstone 原书把角量放前面）相反，是初学者读 Pinocchio 源码时最常踩的坑。

import pinocchio as pin
import numpy as np

# ---- SE3：刚体位姿 ----
R = pin.utils.rotate('z', np.pi / 4)        # 绕 z 轴转 45° 的旋转矩阵
p = np.array([1.0, 2.0, 3.0])
M = pin.SE3(R, p)                            # 由旋转 + 平移构造
print(M.rotation)                           # 3x3 旋转部分
print(M.translation)                        # 3x1 平移部分
print(M.homogeneous)                        # 4x4 齐次矩阵
print(M.action)                             # 6x6 伴随变换矩阵 Ad(M)（作用于 Motion）
print(M.actionInverse)                      # 6x6 Ad(M^{-1})

M_inv = M.inverse()                         # SE(3) 求逆（解析公式，非矩阵求逆）
M_id  = pin.SE3.Identity()                  # 单位变换
M_rand = pin.SE3.Random()                   # 随机合法位姿

# ---- Motion：6D 空间速度 ----
v = pin.Motion(np.array([0.1, 0.2, 0.3,     # 线速度 (前 3 维)
                         0.4, 0.5, 0.6]))   # 角速度 (后 3 维)
print(v.linear)                             # 线速度部分 (Vector3)
print(v.angular)                            # 角速度部分 (Vector3)
print(v.vector)                             # 完整 6D 向量
v0 = pin.Motion.Zero()

# ---- Force：6D 空间力 ----
f = pin.Force(np.array([1.0, 0.0, 0.0,      # 力 (前 3 维)
                        0.0, 0.0, 0.5]))    # 力矩 (后 3 维)
print(f.linear)                             # 力部分
print(f.angular)                            # 力矩部分

# ---- Inertia：6D 空间惯量 ----
mass   = 2.5                                 # 质量 (kg)
lever  = np.array([0.0, 0.0, 0.1])          # 质心相对 frame 原点的位置
I_c    = np.diag([0.01, 0.01, 0.02])        # 质心处的 3x3 转动惯量
Y = pin.Inertia(mass, lever, I_c)           # 构造 6x6 空间惯量
print(Y.mass)                               # 标量质量
print(Y.lever)                              # 质心位置
print(Y.matrix())                           # 展开成 6x6 矩阵

核心运算 1：`act` / `actInv`——坐标系变换 ⭐⭐⭐¶

SE3 最重要的能力是把 Motion、Force、Inertia 从一个坐标系搬到另一个坐标系。这正是 RNEA 递推中 ${}^i X_{\lambda(i)} v_{\lambda(i)}$ 这一项干的事——把父关节的速度变换到子关节坐标系。Pinocchio 用 act（正向作用）和 actInv（逆向作用）封装了 47 章前文反复出现的"伴随变换"：

A_M_B = pin.SE3.Random()        # frame A 到 frame B 的变换
v_B   = pin.Motion.Random()     # 在 B 下表达的速度

# act：把 B 下的量变换到 A 下表达。等价于 Ad(A_M_B) @ v_B
v_A = A_M_B.act(v_B)            # Motion 的伴随变换
# 数值上等价于：
v_A_check = pin.Motion(A_M_B.action @ v_B.vector)
assert np.allclose(v_A.vector, v_A_check.vector)

# actInv：反向变换，等价于 Ad(A_M_B^{-1}) @ v_A
v_B_back = A_M_B.actInv(v_A)
assert np.allclose(v_B_back.vector, v_B.vector)   # act 与 actInv 互逆

# act 对 Force 用的是对偶伴随 Ad^{-T}（力和速度的变换互为对偶）
f_B = pin.Force.Random()
f_A = A_M_B.act(f_B)            # Force 的（对偶）伴随变换

# act 对 Inertia：Y_A = A_M_B.act(Y_B)，对应 Ad^{-T} Y Ad^{-1}
Y_B = pin.Inertia.Random()
Y_A = A_M_B.act(Y_B)           # 空间惯量的坐标变换（CRBA 后向递推的核心操作）

本质洞察：act 之所以能对 Motion、Force、Inertia 三种对象**同名重载**，是因为它们在数学上都是"被 $SE(3)$ 作用的对象"，只是作用方式不同——速度用伴随 $\mathrm{Ad}$，力用对偶伴随 $\mathrm{Ad}^{-T}$，惯量用合同变换 $\mathrm{Ad}^{-T} Y\, \mathrm{Ad}^{-1}$。Pinocchio 把"群作用"这个抽象概念落实为一个统一的 act 接口，让 RNEA/CRBA 的递推代码读起来像数学公式本身。这是"数学结构驱动 API 设计"的又一个范例——与 47.1 节末尾的本质洞察呼应。

核心运算 2：`cross`——空间叉乘 ⭐⭐⭐¶

RNEA 正向递归里的 $v_i \times S_i \dot q_i$、反向递归里的 $v_i \times^* I_i v_i$ 用的是**空间叉乘**。空间向量有两种叉乘：

$v \times m$（Motion 叉乘 Motion，记作 $\mathrm{ad}_v$）——出现在加速度递推的科氏项
$v \times^* f$（Motion 叉乘 Force，记作 $\mathrm{ad}_v^*$）——出现在力递归的陀螺项

v = pin.Motion.Random()
m = pin.Motion.Random()
f = pin.Force.Random()

vxm = v.cross(m)        # Motion × Motion → Motion（即 ad_v(m)）
vxf = v.cross(f)        # Motion × Force  → Force （即 ad_v^*(f)）

# 物理意义：v.cross(m) 的角部分是 ω_v × ω_m，
# 线部分是 v_v × ω_m + ω_v × v_m（旋量叉乘的耦合结构）

这个 cross 就是把"空间速度的李括号"封装成一次方法调用。没有它，你得手写 4 个 Vector3d.cross 再组装——这正是 47.5 节 RNEA 递推能写得如此紧凑的底层支撑。

核心运算 3：`Inertia` 作用于 `Motion`——牛顿-欧拉方程 ⭐⭐¶

空间惯量 $I_i$ 乘以空间加速度 $a_i$ 得到空间力——这就是打包成 6 维形式的**牛顿-欧拉方程** $f = I a$（牛顿第二定律 $f = m\dot v$ 和欧拉方程 $\tau = I\dot\omega + \omega\times I\omega$ 的统一）：

Y = pin.Inertia.Random()
a = pin.Motion.Random()
f = Y * a               # Inertia × Motion → Force，即空间牛顿-欧拉方程

# 这正是 RNEA 反向递归 f_i = I_i a_i + (惯性力项) 的第一项

小结：空间代数如何"撑起"整个算法层 ⭐⭐¶

把这一节的四个类型和三类运算放在一起看，就能理解 47.1 节那句"数学结构驱动软件架构"的本质洞察具体落在哪里。RNEA、ABA、CRBA、CCRBA 这些算法的递推公式，逐项都能翻译成空间代数的方法调用：

递推公式中的符号	空间代数 API	出现在
${}^i X_{\lambda(i)}\, v_{\lambda(i)}$（父速度变到子系）	`iMli.actInv(v_parent)`	RNEA/ABA 正向递归
$v_i \times S_i \dot q_i$（科氏耦合）	`v.cross(S_dq)`	RNEA 加速度递归
$I_i a_i$（牛顿-欧拉）	`Y * a`	RNEA 力递归
$v_i \times^* (I_i v_i)$（陀螺项）	`v.cross(Y * v)`	RNEA 力递归
${}^{\lambda(i)} X_i^*\, f_i$（子力汇聚到父）	`iMli.act(f_child)`	RNEA 反向递归
$I_i^c = I_i + \sum {}^iX_j^* I_j^c {}^jX_i$（复合惯量）	`Y_i + iMlj.act(Y_j)`	CRBA 后向递归

本质洞察：Pinocchio 的算法层之所以能用区区几十行模板代码实现，是因为它把"6 维打包 + 群作用 + 李括号"这三件事固化成了 Motion/Force/Inertia 上的 act/actInv/cross/operator*。一旦这层代数原语就位，RNEA 的两趟递归几乎就是把 Featherstone 书里的公式**逐行誊抄**成 C++——没有任何手写的 $3\times3$ 块拼接、没有任何线/角分量的显式拆分。这就是"好的抽象消灭样板代码"的教科书案例：数学家发现 6 维是自然维度（数学结构），工程师把 6 维对象做成一等公民类型（软件架构），于是算法实现退化为公式的直译。回头看 47.2 的 CRTP 和 47.3 的 Model/Data，它们解决的是"如何高效组织和派发"；本节的空间代数解决的是"用什么数据类型计算"——前者是骨架，后者是血肉，合起来才是完整的 Pinocchio。

与单个 SE(3)（Sophus/manif）的边界 ⭐⭐¶

回到本节开头的桥接：Sophus/manif 和 Pinocchio 的空间代数**像在哪、不像在哪**？

像：两者都实现了 $SE(3)$ 的 compose/inverse，都有 6 维李代数（manif 的 SE3Tangent 类比于 Pinocchio 的 Motion），都提供 log/exp。
不像（边界）：① manif 的核心是**位姿估计**（协方差传播、雅可比 $J_l/J_r$），它的 Tangent 默认 $[\text{平移}; \text{旋转}]$ 布局且强调"在流形上做梯度优化"；Pinocchio 的核心是**动力学**，Motion/Force 是物理速度/力，强调 Inertia 作用和空间叉乘——manif 没有 Inertia 和空间叉乘 $\mathrm{ad}$。② Pinocchio 的 6D 布局是 $[\text{线性}; \text{角度}]$，manif 的 SE3Tangent 是 $[\rho; \theta]$（平移 $\rho$ 在前、旋转 $\theta$ 在后）——两者顺序看似都是"平移在前"，但 manif 的旋转部分是李代数 $\theta$（用于 exp），Pinocchio 的角部分是物理角速度 $\omega$，不要把 Motion.vector 直接喂给 manif 的 exp。③ 不要用 Pinocchio 的 Motion 做位姿估计的不确定性传播，也不要用 manif 的 Tangent 做 RNEA——它们服务不同问题。

⚠️ 概念误区：把 Motion 当成"位姿的微小增量 $\log(T)$"

错误描述：看到 Motion 是 6 维、SE3 是位姿，初学者以为 Motion = log6(SE3)，把速度和"位姿增量"混为一谈。现象/后果：在做数值积分时写出 M_next = pin.exp6(v)（把速度当增量直接指数映射），得到的位姿在时间尺度上完全错误——量纲都不对（速度是 1/s，增量是无量纲）。根本原因：Motion（$\mathfrak{se}(3)$ 的元素）确实和 $\log(T)$ 同属一个 6 维空间，但物理含义不同——前者是"每秒变化多少"，后者是"一共变化多少"。二者差一个时间因子 $\mathrm{d}t$。正确做法：积分位姿时用 M_next = M * pin.exp6(v * dt)（右乘 body-fixed 增量，且乘上 $\mathrm{d}t$）；Pinocchio 在配置空间层面提供 pin.integrate(model, q, v*dt) 自动处理每个关节的流形结构，优先用它而非手写 exp6。

练习 ⭐⭐¶

练习 47.3.6.1（⭐⭐）：用 pin.SE3.Random() 生成 A_M_B，用 pin.Motion.Random() 生成 v_B。分别用 A_M_B.act(v_B) 和手写公式 $\omega_A = R\,\omega_B,\ v_A = R\,v_B + p \times (R\,\omega_B)$ 计算 v_A，验证两者一致。这道题让你确信 act 内部就是那个"容易写错的耦合公式"——以后放心用 act 而不必手写。

练习 47.3.6.2（⭐⭐⭐）：验证空间叉乘与伴随的关系：对随机 Motion v 和 Motion m，验证 v.cross(m).vector 等于 $6\times6$ 矩阵 $\mathrm{ad}_v$ 乘以 m.vector，其中 $\mathrm{ad}_v = \begin{bmatrix}\hat\omega & \hat v\\ 0 & \hat\omega\end{bmatrix}$（注意 Pinocchio 的 [线性;角度] 布局下这个块结构的具体形式）。提示：用 pin.skew() 构造 $3\times3$ 反对称矩阵。

47.3.5 承上启下：从设计哲学到核心算法实战¶

上半部分解决了"Pinocchio 为什么要这样设计"的问题——CRTP 消灭虚函数开销，Model/Data 分离实现零锁多线程。但设计哲学只是地基，接下来我们要在这个地基上建房子：从模板化 Scalar 类型的设计哲学到 FK/RNEA/ABA/Jacobian 的完整 Python 代码，再到 v3.x 新增的约束动力学与碰撞接口。

47.4 模板化 Scalar 类型——一份代码四种用途 ⭐⭐⭐¶

动机：为什么同一个 RNEA 要"变身"四次？ ⭐⭐¶

回顾 47.3：Pinocchio 所有核心类型都带 Scalar 模板参数。但上一节只讲了"它是什么"，没有讲**"为什么非得这么做"**。答案藏在下游框架的需求里。

考虑一个腿足 MPC 系统的完整计算流水线：

[实时控制器]  需要 rnea(q,v,a) → τ       ← 用 double，追求极速
[Crocoddyl]   需要 rnea 的梯度 ∂τ/∂q     ← 用 CppAD::AD<double>，tape-based AD
[OCS2]         需要导出独立 C 代码部署到嵌入式 ← 用 CppAD::cg::CG<double>，代码生成
[CasADi 求解]  需要符号表达式做 NLP          ← 用 casadi::SX，符号计算

如果没有模板化 Scalar，你需要**维护四份几乎相同的 RNEA 实现**，每份只有标量运算细节不同。任何一处算法修正（比如 Featherstone 递归中的一个符号错误）都必须同步到四份代码。这在工业代码库中是灾难性的维护负担。

Pinocchio 的回答是：RNEA 只写一次，通过 Scalar 模板参数实例化出四个版本，由编译器保证它们的逻辑完全一致。

`ModelTpl<Scalar>` 的模板实例化机制 ⭐⭐⭐¶

所有核心类型的模板层级如下：

// 层级 1：空间代数类型随 Scalar 变化
SE3Tpl<Scalar>          // 刚体位姿
MotionTpl<Scalar>       // 6D twist
ForceTpl<Scalar>        // 6D wrench
InertiaTpl<Scalar>      // 6D 空间惯量

// 层级 2：关节类型随 Scalar 变化
JointModelRevoluteTpl<Scalar, Options>
JointDataRevoluteTpl<Scalar, Options>

// 层级 3：整个 Model/Data 随 Scalar 变化
ModelTpl<Scalar, Options, JointCollectionTpl>
DataTpl<Scalar, Options, JointCollectionTpl>

// 层级 4：算法函数是 Scalar-generic 的模板函数
template <typename Scalar, int Options, template<...> class JC>
void rnea(const ModelTpl<Scalar,Options,JC>& model,
          DataTpl<Scalar,Options,JC>& data,
          const Eigen::Matrix<Scalar,Dynamic,1>& q,
          const Eigen::Matrix<Scalar,Dynamic,1>& v,
          const Eigen::Matrix<Scalar,Dynamic,1>& a);

当你写 pinocchio::rnea(model_double, data_double, q, v, a) 时，编译器推导 Scalar = double，实例化出纯数值版本。当你写 pinocchio::rnea(model_ad, data_ad, q_ad, v_ad, a_ad) 时，编译器推导 Scalar = CppAD::AD<double>，实例化出自动微分版本。算法源码完全相同，只是标量运算被"替换"了。

实战：同一调用 double vs AD 类型 ⭐⭐¶

import pinocchio as pin
import numpy as np

# ---- double 版本：纯数值计算 ----
model = pin.buildSampleModelHumanoidRandom()
data  = model.createData()
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)
a = np.random.randn(model.nv)

tau = pin.rnea(model, data, q, v, a)
print(f"tau (double): {tau[:4]}...")  # 数值向量

// ---- CppAD 版本（C++ 侧）：同一行调用，得到梯度 ----
typedef CppAD::AD<double> ADScalar;
pinocchio::ModelTpl<ADScalar> model_ad = model.cast<ADScalar>();
pinocchio::DataTpl<ADScalar>  data_ad(model_ad);
Eigen::Matrix<ADScalar, Eigen::Dynamic, 1> q_ad = q.cast<ADScalar>();

CppAD::Independent(q_ad);                                // 开始录制 tape
pinocchio::rnea(model_ad, data_ad, q_ad, v_ad, a_ad);    // 同一行 rnea()
CppAD::ADFun<double> f(q_ad, data_ad.tau);               // 构建函数对象
Eigen::MatrixXd dtau_dq = f.Jacobian(q_val);             // ∂τ/∂q 矩阵

# ---- 解析导数（推荐的高性能路线）----
pin.computeRNEADerivatives(model, data, q, v, a)
dtau_dq = data.dtau_dq   # 解析 ∂τ/∂q
dtau_dv = data.dtau_dv   # 解析 ∂τ/∂v
dtau_da = data.M          # ∂τ/∂a = M(q)，即广义惯量矩阵

这里有一个关键区分：Pinocchio 同时提供**两种梯度路线**。路线一是 Scalar = AD 的自动微分，通用但较慢。路线二是 computeRNEADerivatives() 等**手推闭式解析导数**，是 Pinocchio 团队根据 Featherstone 算法的数学结构直接推导出来的，速度比 AD 快 3-5 倍。Crocoddyl 和 Aligator 主要走路线二。

为什么下游框架需要不同的 Scalar？ ⭐⭐¶

下游框架	需要的 Scalar	原因
实时控制器（1kHz WBC）	`double`	只需数值结果，追求最低延迟
Crocoddyl（DDP 轨迹优化）	`double` + 解析导数	DDP 的 backward pass 需要 ∂τ/∂q, ∂τ/∂v
OCS2（嵌入式 MPC 部署）	`CppAD::cg::CG<double>`	代码生成：导出一个不依赖 Pinocchio 的纯 C 文件，可部署到 ARM 裸机
CasADi 用户	`casadi::SX`	在 CasADi 的符号框架中构建 NLP，用 IPOPT 求解

OCS2 的代码生成路线值得单独说明：它用 CppAD::cg::CG<double> 实例化 rnea()，将整个递归算法"展开"为一个无循环、无分支的纯算术表达式序列，然后编译为高度优化的 C 代码。这个生成的代码**不需要 Pinocchio 头文件**，可以直接在 ARM Cortex-M 等裸机上运行。这是"模板 Scalar"设计最极端也最精彩的应用。

与 Ceres Jet 的对比（01_数学/30_优化理论） ⭐⭐¶

01_数学/30_优化理论中讲的 Ceres 用 Jet<double, N> 做前向自动微分——把每个 double 扩展为 "值 + N 维偏导" 的双数。Pinocchio 的 Scalar = CppAD::AD<double> 做的是**反向自动微分（tape-based）**——先录制一遍前向计算的"磁带"，然后反向回放求梯度。

关键差异：

特性	Ceres `Jet<double,N>`	Pinocchio + `CppAD::AD<double>`
微分方向	前向（forward mode）	反向（reverse mode）
梯度复杂度	O(N) per 输出，N = 自变量个数	O(M) per 输入，M = 输出个数
适合场景	输出维度 >> 输入维度（稀疏残差）	输入维度 >> 输出维度（动力学梯度）
二阶导	需要嵌套 Jet（`Jet<Jet<...>>`）	tape 原生支持二阶导

腿足机器人的 RNEA 输入是 (q, v, a) 共 3N 维，输出 tau 是 N 维——输入远大于输出，所以**反向 AD 更合适**。这也是 Pinocchio 选择 CppAD 而非 Ceres Jet 的根本原因。但 Pinocchio 的设计并不排斥 Jet——如果你把 Scalar 设为 ceres::Jet<double,N>，代码一样能编译通过，只是性能特征不同。这种"Scalar 无关"的泛型设计赋予了下游用户完全的选择自由。

⚠️ 陷阱：AD 类型的性能代价

CppAD::AD<double> 的每次标量运算都要在"tape"上记录一个操作节点。对于 12-DOF 的 RNEA，double 版本耗时约 1.5 μs，AD 版本耗时约 50-150 μs——慢 30-100 倍。因此在实时控制循环中**绝不能用 AD 类型做在线计算**。正确做法是：离线用 AD 求一次梯度（或用代码生成导出 C 代码），在线只用 double 版本和预计算的解析导数。

更隐蔽的问题：CppAD::AD<double> 的 tape 消耗大量内存。一个 37-DOF 人形机器人的 RNEA tape 可能占用 10-50 MB。如果在循环中反复创建 tape 而不释放，内存会在几秒内耗尽。正确模式是创建一次 CppAD::ADFun，然后反复调用其 .Jacobian() 方法。

47.5 核心算法实战——FK / RNEA / ABA / Jacobian ⭐⭐¶

完整流程：加载 URDF → 正运动学 → 打印末端位姿 ⭐¶

import pinocchio as pin
import numpy as np
from pathlib import Path

# ---------- 1. 加载 URDF ----------
# 使用 example-robot-data 提供的 Panda 模型
from example_robot_data import load as erd_load
robot = erd_load("panda")
model, data = robot.model, robot.model.createData()

print(f"模型名称: {model.name}")
print(f"关节数 njoints: {model.njoints}")   # 含 universe 根关节
print(f"配置空间维度 nq: {model.nq}")        # 7（Panda 是 7-DOF）
print(f"速度空间维度 nv: {model.nv}")        # 7

# ---------- 2. 正运动学（FK）----------
q = pin.neutral(model)                      # 零位构型
pin.forwardKinematics(model, data, q)       # 计算所有关节位姿
pin.updateFramePlacements(model, data)      # 更新 frame 位姿

# 获取末端执行器的位姿
ee_frame_id = model.getFrameId("panda_hand")
ee_pose = data.oMf[ee_frame_id]             # SE3 对象
print(f"末端位置: {ee_pose.translation}")
print(f"末端旋转:\n{ee_pose.rotation}")

这段代码的关键调用链是 buildModelFromUrdf() -> createData() -> forwardKinematics() -> updateFramePlacements()。注意 forwardKinematics() 只更新**关节**位姿（存在 data.oMi 中，i 代表 joint index），并不更新**frame** 位姿（存在 data.oMf 中）。frame 包括 link 上的附加参考点（如传感器安装位置、末端执行器 TCP），需要额外调用 updateFramePlacements()。忘记这一步是初学者最常见的错误之一——data.oMf 全是上一次调用的过期数据，但程序不会报错，只会得到错误的位姿。

RNEA：给定 (q, v, a)，计算 tau ⭐⭐¶

# ---------- 3. 逆动力学（RNEA）----------
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)
a = np.random.randn(model.nv)

tau = pin.rnea(model, data, q, v, a)
print(f"所需扭矩 tau: {tau}")

# 特殊用法 1：重力补偿项（令 v=0, a=0，只剩重力在各关节产生的扭矩）
tau_gravity = pin.rnea(model, data, q,
                       np.zeros(model.nv), np.zeros(model.nv))
# 等价于：
tau_gravity_check = pin.computeGeneralizedGravity(model, data, q)
assert np.allclose(tau_gravity, tau_gravity_check, atol=1e-14)

# 特殊用法 2：科氏力 + 离心力（令 a=0，减去重力项）
tau_nle = pin.rnea(model, data, q, v, np.zeros(model.nv))
tau_coriolis = tau_nle - tau_gravity  # 纯科氏/离心力项

RNEA 是 Pinocchio 中**调用频率最高**的算法。在 Crocoddyl 的一次 DDP 求解中（horizon=100, iterations=50），RNEA 被调用约 5000 次。这就是为什么它必须快到微秒级。

ABA：给定 (q, v, tau)，计算加速度 ⭐⭐¶

# ---------- 4. 正动力学（ABA）----------
tau_input = np.random.randn(model.nv)

a_result = pin.aba(model, data, q, v, tau_input)
print(f"关节加速度: {a_result}")

# 验证互逆性：ABA 和 RNEA 互为逆运算
tau_check = pin.rnea(model, data, q, v, a_result)
error = np.linalg.norm(tau_check - tau_input)
print(f"RNEA(q, v, ABA(q, v, tau)) ≈ tau?  误差={error:.2e}")
# 误差应在 1e-12 量级（浮点精度极限）

ABA 比 RNEA 慢约 2 倍（三趟递归 vs 两趟），但仍然是 O(N) 复杂度。在仿真中（给定扭矩求加速度），ABA 是核心；在控制中（给定期望运动求扭矩），RNEA 是核心。两者的互逆关系 RNEA(q, v, ABA(q, v, tau)) = tau 是最好的正确性验证手段。

Jacobian：关节雅可比矩阵 ⭐⭐¶

# ---------- 5. 雅可比矩阵 ----------
q = pin.neutral(model)
pin.computeJointJacobians(model, data, q)
pin.updateFramePlacements(model, data)

# 获取末端 frame 的雅可比（6 x nv 矩阵）
J_local = pin.getFrameJacobian(model, data, ee_frame_id, pin.LOCAL)
J_world = pin.getFrameJacobian(model, data, ee_frame_id, pin.WORLD)
J_lwa   = pin.getFrameJacobian(model, data, ee_frame_id,
                                pin.LOCAL_WORLD_ALIGNED)

print(f"J_local shape: {J_local.shape}")    # (6, 7)
print(f"J_world shape: {J_world.shape}")    # (6, 7)
print(f"J_lwa   shape: {J_lwa.shape}")      # (6, 7)

Frame vs Joint 雅可比：三种参考坐标系 ⭐⭐⭐¶

这是初学者最常犯错的地方。Pinocchio 提供三种参考坐标系（ReferenceFrame 枚举）：

枚举值	含义	速度关系
`LOCAL`	表达在末端 frame 自身坐标系（body-fixed）	${}^B v = J_{\text{local}} \cdot \dot{q}$
`WORLD`	表达在世界坐标系原点	${}^W v = J_{\text{world}} \cdot \dot{q}$
`LOCAL_WORLD_ALIGNED`	表达在末端位置处，但朝向与世界坐标系对齐的坐标系	${}^{W,p} v = J_{\text{lwa}} \cdot \dot{q}$

三者之间的数学关系：$J_{\text{world}} = {}^W X_B \cdot J_{\text{local}}$，其中 ${}^W X_B$ 是 6x6 伴随变换矩阵。

完整的坐标系验证实战——理解三种坐标系最好的方式是数值验证它们之间的关系：

import pinocchio as pin
import numpy as np

# 加载模型并计算 FK
robot = erd_load("panda")
model, data = robot.model, robot.model.createData()
q = pin.randomConfiguration(model)
pin.forwardKinematics(model, data, q)
pin.computeJointJacobians(model, data, q)
pin.updateFramePlacements(model, data)

ee_id = model.getFrameId("panda_hand")

# 获取三种参考坐标系下的 Jacobian
J_local = pin.getFrameJacobian(model, data, ee_id, pin.LOCAL)
J_world = pin.getFrameJacobian(model, data, ee_id, pin.WORLD)
J_lwa   = pin.getFrameJacobian(model, data, ee_id, pin.LOCAL_WORLD_ALIGNED)

# 验证 1: J_world = Ad(oMf) @ J_local
# Ad 是 6x6 伴随变换矩阵
oMf = data.oMf[ee_id]
Ad = np.zeros((6, 6))
Ad[:3, :3] = oMf.rotation
Ad[3:, 3:] = oMf.rotation
Ad[:3, 3:] = pin.skew(oMf.translation) @ oMf.rotation
J_world_check = Ad @ J_local
print(f"J_world 验证误差: {np.linalg.norm(J_world - J_world_check):.2e}")

# 验证 2: J_lwa 只旋转坐标轴，不引入耦合
# J_lwa 的线速度部分直接是末端在世界系下的平移速度
# 而 J_world 的线速度部分包含 ω × p 的耦合（因为参考点在世界原点）
R = oMf.rotation
J_lwa_lin_check = R @ J_local[:3, :]
print(f"J_lwa 线速度验证误差: {np.linalg.norm(J_lwa[:3,:] - J_lwa_lin_check):.2e}")

何时选用哪种坐标系——决策指南：

场景	推荐坐标系	原因
IK/WBC 任务误差在世界坐标系下定义	`LOCAL_WORLD_ALIGNED`	最直观，线速度直接对应世界系平移
SE(3) log 映射误差（body-fixed）	`LOCAL`	`log6(T_err)` 是 body-fixed 量，雅可比必须匹配
力控/阻抗控制	`LOCAL` 或 `LOCAL_WORLD_ALIGNED`	取决于力的参考坐标系
质心动量控制	`WORLD`	角动量相对世界坐标系原点定义
Crocoddyl / OCS2	框架内部约定	检查框架文档，不要自行假设

LOCAL_WORLD_ALIGNED 是**最推荐的默认选择**。它在末端位置处建立一个与世界坐标系平行的坐标系，使得雅可比矩阵的线速度部分直接对应世界坐标系下的平移速度，角速度部分直接对应世界坐标系下的旋转速度，同时避免了 WORLD 模式中因末端远离世界原点而产生的非直觉线速度-角速度耦合项。

⚠️ 陷阱：Jacobian 参考坐标系选错导致 IK / WBC 静默失败

在逆运动学和全身控制中，任务误差通常在世界坐标系下定义（如"末端移到 [0.5, 0, 0.3]"）。如果你用 LOCAL 雅可比来做 dq = J_pinv @ e_world，关节增量方向会完全错误——因为误差坐标系和雅可比坐标系不匹配。程序不会报错，但机器人会往奇怪的方向运动，IK 永远不收敛。

规则：误差在哪个坐标系下定义，就用那个坐标系的雅可比。大多数情况下 LOCAL_WORLD_ALIGNED 是最安全的选择。如果你用 SE(3) 对数映射 pin.log6(T_err) 计算误差，那个误差是 body-fixed 的，此时应搭配 LOCAL 雅可比。

RNEA 递推公式拆解——理解"为什么 O(N)" ⭐⭐⭐¶

前面多次提到 RNEA 是 O(N) 复杂度，但还没有解释**为什么**。理解递推公式有助于你在调试时"读懂" Pinocchio 内部在做什么，也是理解足式/40_CppAD与代码生成 CppAD tape 机制的前提。

RNEA 由两趟递归组成——正向传播速度/加速度，反向传播力/力矩：

正向递归（从基座到末端，i = 1, ..., N）：

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

\[a_i = {}^i X_{\lambda(i)} \, a_{\lambda(i)} + S_i \, \ddot{q}_i + v_i \times S_i \, \dot{q}_i\]

其中 $\lambda(i)$ 是关节 $i$ 的父关节，${}^i X_{\lambda(i)}$ 是从父体到子体的 6D 空间变换，$S_i$ 是关节运动子空间矩阵（对旋转关节是 $[0,0,0,0,0,1]^T$ 或其旋转轴方向），$v_i \times$ 是空间速度的叉积算子。

直觉：正向递归在做"速度和加速度的逐级传递"——基座的运动通过关节树一级一级传到末端，每一级加上该关节自身的运动贡献。

反向递归（从末端到基座，i = N, ..., 1）：

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

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

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

其中 $I_i$ 是 body $i$ 的 6D 空间惯量，$v_i \times^*$ 是空间力的叉积算子，$f_i^{\text{ext}}$ 是外力。

直觉：反向递归在做"力的逐级聚合"——先用 Newton-Euler 方程算出每个 body 需要多大的力才能产生要求的加速度，然后从末端往回传，把所有力聚合到每个关节处，最后投影到关节运动方向得到关节力矩。

为什么是 O(N)：每个关节只被访问两次（正向一次、反向一次），每次的计算量是常数（6x6 矩阵运算）。总计算量 = 2N 次常数操作 = O(N)。相比之下，直接组装 $M(q)\ddot{q} + h(q,\dot{q}) = \tau$ 再求解，组装 $M$ 需要 O(N^2)，求解线性系统需要 O(N^3)——在高自由度系统上递归方法的优势会越来越明显。

跨领域类比：RNEA 的递推结构与 SLAM 中 Bayes 树的消元过程有深层相似性。SLAM 的因子图消元也是"沿树的正向传播信息、反向聚合信息"，利用稀疏结构将 O(N^3) 降到 O(N)。两者的共同本质是：树结构允许自底向上/自顶向下的局部计算替代全局矩阵运算。

性能基准 ⭐⭐¶

以下数据测量于 Intel i7-12700H（单核），Pinocchio 3.1 + Eigen 3.4（AVX2 开启）：

算法	Panda 7-DOF	Go2 18-DOF	Talos 37-DOF
`forwardKinematics`	0.5 μs	0.9 μs	1.8 μs
`rnea`	1.2 μs	2.0 μs	3.5 μs
`aba`	2.5 μs	4.0 μs	7.0 μs
`computeRNEADerivatives`	4.5 μs	8.0 μs	16 μs
`computeJointJacobians`	1.0 μs	1.8 μs	3.5 μs
`crba`（惯量矩阵 M(q)）	0.8 μs	1.5 μs	5.0 μs

即使是 37-DOF 人形，完整的逆动力学 + 梯度计算也只需约 20 μs。在 1kHz 控制循环的 1000 μs 预算中，Pinocchio 的动力学计算仅占 2%，为上层控制器（QP 求解、轨迹优化）留出了充足的计算余量。

⚠️ 陷阱：首次调用延迟与预热

Python 绑定首次调用 rnea() 时会触发动态链接和缓存初始化，耗时可达 100-500 μs。做性能测试时，务必先"预热"几百次调用，再用 timeit 统计稳态性能。C++ 侧没有这个问题，但 Data 的首次构造涉及内存分配，同样应排除在计时之外。

CRBA 递推拆解——惯量矩阵 $M(q)$ 是怎么算出来的 ⭐⭐⭐¶

47.5 前文拆解了 RNEA 的两趟递归，但 crba（计算惯量矩阵 $M(q)$）的内部机制还没讲。理解 CRBA 对调试 WBC 至关重要——QP 的 KKT 矩阵里 $M$ 出现在最显眼的位置，一旦 $M$ 算错（最常见是忘了补全下三角，见后文陷阱），整个控制器就崩。

Composite Rigid Body Algorithm（复合刚体算法） 的核心思想是：惯量矩阵的元素 $M_{ij}$ 表示"关节 $j$ 的单位加速度在关节 $i$ 处引起的广义力"。直接按定义算需要 $O(N^2)$ 次 RNEA（每个关节单独施加单位加速度），但 CRBA 利用了一个关键观察——关节 $i$ 处感受到的惯量，是它的整个子树（所有后代 body）刚化为一个复合刚体后的惯量。

CRBA 的递推结构是"自叶向根聚合复合惯量 + 沿支撑路径填表"：

第一步：后向递归聚合复合刚体惯量（$i = N, \dots, 1$）

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

其中 $I_i^c$ 是以关节 $i$ 为根的子树"刚化"后的复合空间惯量（composite rigid body inertia）。Pinocchio 把这个量存在 data.Ycrb[i] 中——Ycrb 即 "Y composite rigid body" 的缩写，是一个 std::vector<Inertia>。这一步用的正是 47.3.6 讲的 Inertia 的 act（坐标变换）和加法。

第二步：用复合惯量填充 $M$ 的元素

\[F = I_i^c \, S_i \quad(\text{关节 } i \text{ 运动子空间承受的复合力})$$ $$M_{ii} = S_i^T F\]

然后沿关节 $i$ 到根的**支撑路径**（support path）向上回填：对路径上每个祖先 $j$，

\[F \leftarrow {}^{\lambda(j)} X_j^* \, F, \qquad M_{ij} = M_{ji} = S_j^T F\]

为什么是 $O(N^2)$ 而非 $O(N^3)$：第一步后向递归是 $O(N)$。第二步对每个关节 $i$ 沿支撑路径回填，路径长度最坏 $O(N)$，共 $N$ 个关节，故 $O(N^2)$。对串联链（如机械臂）支撑路径就是整条链，是 $O(N^2)$ 的最坏情况；但对分叉的树（如四足，每条腿只有 3 节），路径很短，实际接近 $O(N)$。这就是为什么 47.5 性能表里 Go2（18-DOF 但分 4 叉）的 crba 只要 1.5 μs，而 Talos（37-DOF 长链多）要 5.0 μs——CRBA 的实际开销强烈依赖树的**形状**而非单纯自由度数。

跨领域类比：CRBA 的"子树刚化"思想与有限元里的**子结构凝聚（substructuring / Guyan reduction）**异曲同工——把一个复杂子结构的内部自由度凝聚掉，只保留它对边界的等效刚度/质量。CRBA 把子树的所有关节"凝聚"成根关节感受到的一个等效复合惯量 $I_i^c$。相似之处仅在于"局部聚合成等效量"；不同之处是 FEM 凝聚的是刚度矩阵、CRBA 聚合的是空间惯量，且 CRBA 利用了树拓扑使聚合严格 $O(N)$。

⚠️ 编程陷阱：crba 只填充 $M$ 的上三角

错误描述：调用 pin.crba(model, data, q) 后直接把 data.M 当完整对称矩阵用（如 np.linalg.solve(data.M, b)）。现象/后果：求解结果错误。data.M 的下三角全是 0（或上一次调用的脏数据），它根本不是对称矩阵。诡异的是小自由度下有时"看起来差不多对"，掩盖了 bug。根本原因：出于性能，CRBA 利用 $M$ 的对称性**只计算并写入上三角**，下三角留空。这是 Pinocchio（和大多数高性能动力学库）的刻意约定，文档有写但极易被忽略。正确做法：用前补全下三角——M = data.M; M = M + M.T - np.diag(M.diagonal())（前文 Worked Example 第 6 步正是这么做的）。或者，如果只需要解 $M x = b$，用 Pinocchio 的 Cholesky 接口 pin.cholesky.decompose(model, data, q) + pin.cholesky.solve(...)，它内部正确处理对称性，比"补全 + 通用求解"更快。

47.5.5 质心动力学与 CCRBA——SRBD MPC 的地基 ⭐⭐⭐¶

动机：腿足 MPC 为什么盯着"质心动量"？ ⭐⭐¶

前面所有算法都在关节空间（$q, v, \tau$）里打转。但腿足运动控制有一个独特视角——质心动量（centroidal momentum）。原因来自一条物理铁律：机器人受的外力只有重力（已知）和地面接触力。由牛顿-欧拉定律，整个系统的**线动量变化率 = 合外力**、绕质心的角动量变化率 = 合外力矩。也就是说，不管机器人内部 18 个关节怎么动，它的质心动量只能被脚底的接触力改变。这把"高维全身运动"和"低维质心行为"解耦开——单刚体动力学 MPC（SRBD MPC，见 05_运动控制/10_足式/70_SRBD_MPC 与凸MPC）正是建立在这个解耦之上。

本质洞察：质心动量是腿足控制的"沙漏腰"——上面是几十个关节的高维运动，下面是几个接触力的低维输入，中间被 6 维质心动量 $h_G = (\text{线动量}, \text{角动量})$ 卡住。MPC 在这个 6 维瓶颈上做优化，复杂度与关节数无关，这是腿足 MPC 能实时的关键。CRBA 给的是关节空间惯量 $M$，CCRBA 给的是质心空间的"惯量映射"——两者是同一棵树在不同空间的投影。

回顾 02_机器人本体/复合动力学（复合/20）讲过的复合刚体思想：把多个刚体聚合成等效刚体。质心动力学把这个思想推到极致——把**整个机器人**聚合成一个绕质心的 6×6 复合惯量。

质心动量矩阵 $A_g$ 与 CCRBA ⭐⭐⭐¶

质心动量 $h_G \in \mathbb{R}^6$（前 3 维线动量、后 3 维绕质心角动量）与关节速度的关系是线性的：

\[h_G = A_g(q)\, \dot q\]

矩阵 $A_g(q) \in \mathbb{R}^{6 \times n_v}$ 称为**质心动量矩阵（Centroidal Momentum Matrix, CMM）。计算它的算法叫 **CCRBA（Centroidal Composite Rigid Body Algorithm）——它是 CRBA 的"质心版"，由 Orin & Goswami（2008）、Wensing & Orin 提出，Pinocchio 用 ccrba 实现：

import pinocchio as pin
import numpy as np
from example_robot_data import load as erd_load

robot = erd_load("go2")
model, data = robot.model, robot.model.createData()
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv) * 0.1

# ---- ccrba：计算质心动量矩阵 Ag、质心动量 hg、质心复合惯量 Ig ----
Ag = pin.ccrba(model, data, q, v)   # 返回 6 x nv 的 CMM，并填充 data
# 等价地，data 里也有：
#   data.Ag  -> 质心动量矩阵 (6, nv)
#   data.hg  -> 质心动量 (Force 对象, hg = Ag @ v)
#   data.Ig  -> 质心复合刚体惯量 (6x6 Inertia，整机绕质心的等效惯量)

print(f"Ag shape: {Ag.shape}")            # (6, 18)
print(f"质心动量 hg: {data.hg.vector}")    # 6D：前3线动量，后3角动量
print(f"线动量 = 总质量 × 质心速度? ")
m_total = pin.computeTotalMass(model)
pin.centerOfMass(model, data, q, v)
print(np.allclose(data.hg.linear, m_total * data.vcom[0]))  # True

# ---- 验证 hg = Ag @ v ----
hg_check = Ag @ v
print(np.allclose(data.hg.vector, hg_check))   # True

data.hg 的**线动量部分恒等于"总质量 × 质心速度"**（这是上面断言为 True 的原因）——这是质心动量定义的直接结果，也是一个绝佳的自检：如果你的 data.hg.linear 不等于 $m\, v_{\text{com}}$，说明 ccrba 的输入 $q, v$ 与 centerOfMass 不一致。

质心动量的时间导数与 $\dot A_g$ ⭐⭐⭐¶

MPC 不仅要质心动量，还要它的**变化率**（因为动力学约束是 $\dot h_G = \sum (\text{接触力旋量}) + m g$）。链式法则给出：

\[\dot h_G = A_g(q)\, \ddot q + \dot A_g(q, \dot q)\, \dot q\]

Pinocchio 用两个函数提供这些量：

# 方式 1：computeCentroidalMomentum——只要 hg（动量本身）
pin.computeCentroidalMomentum(model, data, q, v)
hg = data.hg                      # Force 对象

# 方式 2：computeCentroidalMomentumTimeVariation——同时要 hg 和 dhg
pin.computeCentroidalMomentumTimeVariation(model, data, q, v, a)
hg  = data.hg                     # 质心动量
dhg = data.dhg                    # 质心动量变化率 (Force 对象)

# 方式 3：dccrba——要矩阵 Ag 和它的时间导数 dAg（MPC 线性化需要）
Ag  = pin.ccrba(model, data, q, v)
dAg = pin.dccrba(model, data, q, v)   # 6 x nv，CMM 的时间导数 data.dAg
# 验证链式法则：dhg = Ag @ a + dAg @ v
pin.computeCentroidalMomentumTimeVariation(model, data, q, v, a)
dhg_check = Ag @ a + dAg @ v
print(np.allclose(data.dhg.vector, dhg_check))   # True

⚠️ 思维陷阱：把质心角动量误当作"绕世界原点的角动量"

错误描述：以为 data.hg 的角动量部分是机器人绕世界坐标系原点的角动量。现象/后果：在写质心 MPC 的角动量约束时用错了力臂参考点，得到的姿态控制律有一个随质心位置漂移的系统误差，机器人走着走着姿态就偏。根本原因：centroidal 的字面意思就是"绕质心的"——$h_G$ 的角动量是**绕瞬时质心**计算的，不是绕世界原点。质心在运动，所以这个参考点是时变的。Pinocchio 内部用 data.oMc（世界系到质心系、姿态与世界对齐）处理这个时变参考。正确做法：明确 $h_G$ 是 "centroidal" 量——表达在一个原点位于质心、姿态与世界系对齐的坐标系。接触力旋量要换算到同一个质心参考系再代入 $\dot h_G = \sum \text{wrench}$。SRBD MPC 推导（足式/70）会专门处理这个参考系换算。

与 `computeAllTerms` 的关系 ⭐⭐¶

注意一个工程细节：computeAllTerms（47.3 介绍的"一键计算"）会顺带填充 data.Ag 和 data.hg（它内部调用了 ccrba 的计算路径），但**不计算 data.dAg**（时间导数较贵，按需才算）。所以 WBC 里如果只需要质心动量本身，computeAllTerms 已经够；如果是质心 MPC 需要线性化矩阵 $\dot A_g$，必须额外调 dccrba。这与 47.3 节"computeAllTerms 不含 RNEA 导数"的取舍逻辑一致——贵的导数类计算一律按需。

理论-工程桥接：从 `data.hg` 到 SRBD MPC 的状态方程 ⭐⭐⭐¶

把上面的 API 接到下游控制器，才能看出它们的分量。05_运动控制/10_足式/70_SRBD_MPC 与凸MPC 的单刚体 MPC 用一个 6 维（或 13 维含姿态）质心状态描述整机：质心位置 $c$、线动量（或质心速度 $\dot c$）、绕质心角动量 $k_G$、基座姿态 $\theta$。它的连续时间状态方程正是把上面公式逐项落地：

\[\dot c = \frac{1}{m}\, h_{G,\text{lin}}, \qquad \dot k_G = h_{G,\text{ang}} \text{ 的变化率} = \sum_{i=1}^{n_c} \left[ (p_i - c) \times f_i \right] , \qquad \dot h_{G,\text{lin}} = \sum_{i=1}^{n_c} f_i + m g\]

其中 $f_i$ 是第 $i$ 个接触点的地面反力、$p_i$ 是接触点位置、$n_c$ 是接触数。对照这组方程，Pinocchio 的三个量各就各位：

MPC 需要的量	Pinocchio 提供	调用
当前质心位置 $c$	`data.com[0]`	`centerOfMass(model, data, q)`
当前质心动量 $h_G$（线 + 角）	`data.hg`	`computeCentroidalMomentum` / `ccrba`
整机绕质心转动惯量 $\bar I_G$（角动量约束用）	`data.Ig`（6×6 的右下 3×3 块）	`ccrba`
全身运动到质心动量的映射 $A_g$	`data.Ag`	`ccrba`

理论-工程桥接的要害：SRBD MPC 把机器人**近似**成"一个绕质心的刚体 + 无质量的腿"，于是只需要 data.Ig 这一个 6×6 惯量就能写出角动量动力学——这正是 SRBD 名字（Single Rigid Body Dynamics）的由来。但真实机器人腿有质量、摆动时角动量会变，所以高保真的质心 MPC（centroidal MPC）改用完整的 $A_g(q)$ 而非常数 $\bar I_G$：用 data.Ag @ ddq 表达 $\dot h_G$ 的运动学部分。从 data.Ig 升级到 data.Ag 的这一步，就是从 SRBD MPC 走向全质心 MPC 的分水岭——前者快但近似、后者准但需要每步重算 $A_g, \dot A_g$。

反事实推理：如果 SRBD MPC 不用 Pinocchio 的 ccrba 而是手工维护一个常数惯量 $\bar I_G$（许多早期 MIT Cheetah 风格控制器就这么做）会怎样？在原地踏步、缓慢行走时近似很好，省掉了每步 ccrba 的开销；但在大幅摆腿、跳跃、空翻时，腿的角动量贡献不可忽略，常数 $\bar I_G$ 会让 MPC 预测的姿态严重失真。这就是为什么动态动作（如 ANYmal/Go2 的跳跃）倾向用 data.Ag 的全质心模型，而准静态步态可以用常数惯量近似——用哪个量，取决于动作的剧烈程度，这是一个典型的"模型保真度 vs 计算成本"工程权衡。

练习 ⭐⭐¶

练习 47.5.5.1（⭐⭐）：对 Go2 模型，构造一个"自由落体"测试——令 $q$ 为任意构型、$v$ 任意、所有接触力为零、只有重力。用 computeCentroidalMomentumTimeVariation 计算 data.dhg，验证它的线动量变化率等于 $m_{\text{total}} \cdot g$（$g = [0,0,-9.81]$）、角动量变化率约为 0（无外力矩）。这道题验证"质心动量只被外力改变"的物理铁律。

练习 47.5.5.2（⭐⭐⭐）：验证质心动量矩阵 $A_g$ 与关节空间惯量 $M$ 的关系。已知 $A_g = {}^{c}X_0^* \, P \, M$ 形式的投影关系（$P$ 取前 6 行的浮动基座块）。更简单的数值验证：对浮动基座机器人，$A_g$ 的前 6 列（对应基座 6 个自由度）应当等于整机复合惯量 data.Ig 经坐标变换后的结果。用 ccrba 同时拿到 Ag 和 data.Ig，验证 Ag[:, :6] 与 data.Ig.matrix()（适当变换后）一致。

提示：这道题的深层意义是理解"质心动量矩阵的浮动基座块就是整机复合惯量"——这正是 SRBD 近似的数学根据。做完后回看 47.5.5 开头的"沙漏腰"本质洞察，你会更清楚为什么 6 维质心量能概括整机：因为 $A_g$ 的前 6 列已经把全身惯量压缩进了一个 6×6 的复合惯量 data.Ig。

47.6 约束动力学（v3.x 新增） ⭐⭐⭐⭐¶

Pinocchio 2.x 时代只能处理**开链机器人**（树形关节拓扑、无闭环约束）。但真实机器人大量存在闭链结构：

平行四连杆膝关节：Go2、ANYmal C 的膝关节并非简单旋转关节，而是平行四连杆驱动——两根连杆形成闭环
人形机器人腰部平行连杆：Talos、Atlas 的躯干用多连杆并联机构提高刚度
Mimic 关节：一个关节的运动严格跟随另一个（如 Robotiq 2F-85 抓手的欠驱动手指，v3.3+ 原生支持）

Pinocchio 3.x（2024 年起逐步发布）引入了 ConstraintModelTpl 体系来处理这些约束。

约束建模接口 ⭐⭐⭐¶

import pinocchio as pin

model = pin.buildModelFromUrdf("robot_with_closed_loop.urdf")
data  = model.createData()

# 定义一个环路闭合约束：joint_A 和 joint_B 的末端应重合
constraint = pin.RigidConstraintModel(
    pin.ContactType.CONTACT_6D,     # 6D 约束（位置+姿态完全锁定）
    model,
    joint_A_id, placement_A,        # 约束在关节 A 上的附着位姿
    joint_B_id, placement_B         # 约束在关节 B 上的附着位姿
)
constraint_data = constraint.createData()

Delassus 算子与约束求解 ⭐⭐⭐⭐¶

约束动力学的核心方程组是：

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

\[J_c\,\ddot{q} + \dot{J}_c\,\dot{q} = 0\]

其中 $\lambda$ 是约束力（拉格朗日乘子），$J_c$ 是约束雅可比矩阵。将第一式中的 $\ddot{q}$ 用 $M^{-1}$ 消去后代入第二式，得到 Delassus 方程：

\[G\,\lambda = b, \quad G = J_c\,M^{-1}\,J_c^T\]

矩阵 $G$ 称为 Delassus 算子——它描述了约束空间中的"等效惯量"。Pinocchio 3.x 提供了 computeDelassusMatrix() 的高效实现，利用树结构稀疏性避免显式构造 $M^{-1}$（直接用 Cholesky 分解的回代实现）。

对于包含不等式约束的接触问题（足端接触的法向力 $\lambda_n \geq 0$、摩擦锥 $\|\lambda_t\| \leq \mu\,\lambda_n$），约束动力学退化为一个 QP。Pinocchio 团队推荐使用他们同时开发的 ProxQP 作为后端：

import proxsuite

# 构建接触 QP：min 0.5 x'Hx + g'x, s.t. Ax=b, Cl<=Cu
qp = proxsuite.proxqp.dense.QP(n, n_eq, n_ineq)
qp.init(H, g, A, b, C, l, u)        # H 来自 Delassus, C 编码摩擦锥
qp.solve()
lambda_opt = qp.results.x            # 最优约束力/接触力

与下游控制框架的衔接 ⭐⭐¶

约束动力学是 足式/90_WBC分层优化与TSID（WBC 全身控制） 和**接触隐式轨迹优化**的基础设施：

TSID / WBC：将接触力 $\lambda$ 作为决策变量，与 $\ddot{q}$ 和 $\tau$ 一起在 QP 中联合求解——Delassus 算子出现在 QP 的 KKT 矩阵中
Aligator（ProxDDP）：在 DDP 框架中用近端算子直接处理约束，不需要罚函数松弛
接触隐式优化：将接触的建立与断开作为优化问题的一部分，让求解器自动发现最优接触时序——这需要对 Delassus 算子求导，Pinocchio 3.x 的解析导数支持正是为此准备

ProximalSolver：近端正则化约束求解器 ⭐⭐⭐⭐¶

Pinocchio 3.x 引入的另一个重要组件是 ProximalSolver——一种基于近端算子（proximal operator）的约束动力学求解器。传统 KKT 方法直接求解鞍点问题 $(M, J_c^T; J_c, 0)$，当约束近似冗余（$J_c$ 接近行秩亏）时数值条件极差。ProximalSolver 通过在 Delassus 算子上加正则项 $\mu I$ 来改善条件数：

\[G_\mu = J_c M^{-1} J_c^T + \mu I\]

迭代格式为：

\[\lambda^{k+1} = \text{prox}_{\mathcal{C}} \left( G_\mu^{-1} (b + \mu \lambda^k) \right)\]

其中 $\text{prox}_{\mathcal{C}}$ 是约束集合 $\mathcal{C}$ 上的投影算子（对于摩擦锥约束，投影到二阶锥上）。

为什么近端方法优于直接 KKT 求解：

特性	直接 KKT	ProximalSolver
数值稳定性	约束冗余时条件数 $\to \infty$	$\mu$ 正则化保证有界
不等式约束	需要 active set 或 interior point	投影算子自然处理
摩擦锥	需要线性化（多面体近似）	直接投影到 SOC 锥
收敛速度	一步精确（但可能不存在解）	线性收敛（$\mu$ 小时快）
与 Aligator 的关系	—	Aligator/ProxDDP 内部使用同一套近端框架

# ProximalSolver 的使用示意（Pinocchio 3.x）
import pinocchio as pin

model = ...  # 带闭环约束的模型
data  = model.createData()

# 构造约束动力学求解器
prox_settings = pin.ProximalSettings(
    mu=1e-8,           # 近端正则化系数（越小越精确，但收敛越慢）
    accuracy=1e-10,    # 收敛精度
    max_iter=10        # 最大迭代次数
)

# 约束正动力学：给定 (q, v, tau)，求 (a, lambda)
pin.constraintDynamics(
    model, data, q, v, tau,
    contact_models, contact_datas,
    prox_settings
)
# 结果：data.ddq = 加速度, contact_datas[i].contact_force = 接触力

本质洞察：ProximalSolver 的核心思想与 ADMM（交替方向乘子法）同源——都是通过"分裂+正则化"将难以直接求解的约束优化问题转化为一系列易解的子问题。$\mu$ 参数的角色是"弹性系数"：$\mu = 0$ 是刚性约束（精确但可能不可解），$\mu > 0$ 是弹性约束（始终有解但引入微小违反）。这个思想在后续 Aligator 的 ProxDDP 中被进一步发展为轨迹优化中的约束处理方案。

Worked Example：完整的正逆动力学闭环验证 ⭐⭐¶

以下代码演示从 URDF 加载到正逆动力学互逆验证的完整流程，可作为项目模板直接使用：

import pinocchio as pin
import numpy as np
from example_robot_data import load as erd_load

# ===== 1. 加载模型 =====
robot = erd_load("go2")
model = robot.model
data  = model.createData()

print(f"模型: {model.name}")
print(f"nq={model.nq}, nv={model.nv}, njoints={model.njoints}")
print(f"nframes={model.nframes}, nbodies={model.nbodies}")

# ===== 2. 随机构型 =====
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv) * 0.1
a = np.random.randn(model.nv) * 0.1

# ===== 3. 逆动力学 (RNEA): (q,v,a) -> tau =====
tau = pin.rnea(model, data, q, v, a)

# ===== 4. 正动力学 (ABA): (q,v,tau) -> a_check =====
a_check = pin.aba(model, data, q, v, tau)

# ===== 5. 互逆性验证 =====
err = np.linalg.norm(a - a_check)
print(f"RNEA-ABA 互逆性误差: {err:.2e}")  # 应 < 1e-12
assert err < 1e-10, "正逆动力学不一致！"

# ===== 6. 惯量矩阵验证：M*a + nle = tau =====
pin.crba(model, data, q)
M = data.M.copy()
M = M + M.T - np.diag(M.diagonal())  # CRBA 只填上三角

nle = pin.nonLinearEffects(model, data, q, v)
tau_check = M @ a + nle
err2 = np.linalg.norm(tau - tau_check)
print(f"M*a + nle = tau 验证误差: {err2:.2e}")  # 应 < 1e-10

# ===== 7. computeAllTerms 一键计算 =====
pin.computeAllTerms(model, data, q, v)
pin.updateFramePlacements(model, data)

# 现在 data.M, data.nle, data.oMi, data.J, data.com 全部可用
print(f"质心位置: {data.com[0]}")
print(f"质心雅可比 shape: {data.Jcom.shape}")  # (3, nv)

# ===== 8. 四个足端位置和雅可比 =====
foot_names = ["FL_foot", "FR_foot", "RL_foot", "RR_foot"]
for name in foot_names:
    fid = model.getFrameId(name)
    pos = data.oMf[fid].translation
    J_foot = pin.getFrameJacobian(model, data, fid, pin.LOCAL_WORLD_ALIGNED)
    print(f"{name}: pos={pos}, J shape={J_foot.shape}")

这个 worked example 覆盖了控制栈中最常用的 Pinocchio API 调用链。在后续足式/90_WBC分层优化与TSID WBC 和足式/110_OCS2完整栈与双线程MPC OCS2 中，你会反复看到 computeAllTerms + getFrameJacobian 的组合——它们提供 QP 约束矩阵所需的所有信息。

Worked Example：RNEA 解析导数 vs 数值差分——完整对比流程 ⭐⭐⭐¶

这个 worked example 展示如何获取 RNEA 导数并用数值差分验证其正确性。理解这个流程是使用足式/40_CppAD与代码生成 CppADCodeGen 和足式/110_OCS2完整栈与双线程MPC OCS2 MPC 的前提。

import pinocchio as pin
import numpy as np
import time

robot = erd_load("panda")
model, data = robot.model, robot.model.createData()
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)
a = np.random.randn(model.nv)

# ===== 方法 1：Pinocchio 解析导数 =====
t0 = time.perf_counter()
for _ in range(10000):
    pin.computeRNEADerivatives(model, data, q, v, a)
t_analytical = (time.perf_counter() - t0) / 10000 * 1e6
dtau_dq_analytical = data.dtau_dq.copy()
dtau_dv_analytical = data.dtau_dv.copy()
# dtau_da = M（惯量矩阵），因为 tau = M*a + nle，所以 ∂tau/∂a = M

# ===== 方法 2：数值差分（中心差分）=====
eps = 1e-8
dtau_dq_numerical = np.zeros((model.nv, model.nv))

t0 = time.perf_counter()
for i in range(model.nv):
    q_plus = pin.integrate(model, q, eps * np.eye(model.nv)[i])
    q_minus = pin.integrate(model, q, -eps * np.eye(model.nv)[i])
    tau_plus = pin.rnea(model, data, q_plus, v, a)
    tau_minus = pin.rnea(model, data, q_minus, v, a)
    dtau_dq_numerical[:, i] = (tau_plus - tau_minus) / (2 * eps)
t_numerical = (time.perf_counter() - t0) / 10000 * 1e6 * 10000 / model.nv

# ===== 对比 =====
err = np.linalg.norm(dtau_dq_analytical - dtau_dq_numerical, ord='fro')
print(f"解析 vs 数值差分误差: {err:.2e}")       # 应 < 1e-4
print(f"解析导数耗时: {t_analytical:.1f} us")   # ~5 us
print(f"数值差分耗时: ~{2*model.nv*t_analytical/5:.0f} us")  # ~20 us

注意数值差分中使用 pin.integrate() 而非 q + dq——这对于包含四元数的配置空间是必要的（直接加法会破坏四元数的单位范数约束，导致 Pinocchio 内部出错）。

47.7 Pinocchio + Coal 碰撞接口 ⭐⭐⭐¶

Pinocchio 通过 Coal（原名 HPP-FCL，2024 年更名为 Coal）提供碰撞检测和最近距离计算。Coal 支持的几何基元包括球体、胶囊、圆柱、凸多面体、BVH 三角网格等。

跨领域类比：Coal 在 Pinocchio 生态中的角色类似于 OpenCV 在视觉 SLAM 生态中的角色——它是一个底层几何计算引擎，本身不关心机器人学，但被上层框架（Pinocchio / Crocoddyl）封装后成为了不可或缺的基础设施。正如 ORB-SLAM 不自己实现特征匹配而是调用 OpenCV，Crocoddyl 不自己实现碰撞检测而是通过 Pinocchio 调用 Coal。

为什么碰撞检测对腿足机器人重要 ⭐⭐¶

在腿足 MPC（特别是足式/230_Perceptive_MPC Perceptive MPC）中，碰撞检测有三个核心用途：

摆动腿碰撞回避：摆动阶段的腿不能撞到地面障碍物。MPC 需要"摆动轨迹与地面的最近距离"作为约束
躯体碰撞检测：低矮障碍物可能碰到机器人躯干。需要"躯干几何体与高程图的距离"
自碰撞检测：人形机器人的双臂和腿可能互相碰撞。需要"机器人自身不同 link 之间的距离"

Coal 提供了这三种场景都需要的底层计算——给定两个几何体在空间中的位姿，计算它们的最近距离和最近点对。

碰撞检测与距离计算 ⭐⭐⭐¶

import pinocchio as pin

# 加载带碰撞几何的 URDF（第三个返回值是 visual model，此处忽略）
model, collision_model, visual_model = pin.buildModelsFromUrdf(
    "panda.urdf", package_dirs=["/path/to/meshes"]
)
data           = model.createData()
collision_data = collision_model.createData()

q = pin.randomConfiguration(model)

# 更新几何体位置（必须先调用 FK）
pin.forwardKinematics(model, data, q)
pin.updateGeometryPlacements(model, data, collision_model, collision_data, q)

# 碰撞检测：任意一对几何体碰撞即返回 True
is_colliding = pin.computeCollisions(
    model, data, collision_model, collision_data, q, True  # True = 遇到首个碰撞即停
)

# 距离计算：返回所有碰撞对的最近距离
pin.computeDistances(model, data, collision_model, collision_data, q)
for k, dr in enumerate(collision_data.distanceResults):
    if dr.min_distance < 0.05:
        print(f"碰撞对 {k}: 最近距离={dr.min_distance:.4f} m, "
              f"最近点A={dr.nearest_points[0]}, 最近点B={dr.nearest_points[1]}")

碰撞梯度与轨迹优化集成 ⭐⭐⭐¶

仅知道"是否碰撞"对轨迹优化不够用——优化器需要**距离对关节角的梯度** $\partial d / \partial q$，才能将碰撞规避表示为可微约束。Coal 从 v3.0 起支持最近距离的解析导数，Pinocchio 将其封装为：

# 注意：pin.computeDistanceDerivatives() 并非 Pinocchio 的实际 API。
# 碰撞距离梯度需要手动计算：先通过 computeDistances() 获取最近点对,
# 再利用最近点法向量与 Pinocchio 的 frame Jacobian 组合得到 ∂d/∂q。
# 具体步骤（概念伪代码）：
pin.computeDistances(model, data, collision_model, collision_data, q)
pin.computeJointJacobians(model, data, q)
for k, dr in enumerate(collision_data.distanceResults):
    # 法向量 n = (p2 - p1) / ||p2 - p1||
    n = (dr.nearest_points[1] - dr.nearest_points[0])
    n /= np.linalg.norm(n)
    # 获取对应 frame 的 Jacobian，投影到法向量方向得到 ∂d/∂q
    # dd_dq = n^T @ J_p1 - n^T @ J_p2

碰撞距离梯度的数学推导：

设两个几何体 A 和 B 分别附着在关节 $j_A$ 和 $j_B$ 上，它们的最近距离为 $d(q) = \|p_B(q) - p_A(q)\|$，最近点分别为 $p_A, p_B$。对 $d$ 关于 $q$ 求导：

\[\frac{\partial d}{\partial q} = \hat{n}^T \left( \frac{\partial p_B}{\partial q} - \frac{\partial p_A}{\partial q} \right) = \hat{n}^T \left( J_B(q) - J_A(q) \right)\]

其中 $\hat{n} = (p_B - p_A) / \|p_B - p_A\|$ 是单位法向量，$J_A, J_B$ 是对应最近点的位置 Jacobian（仅取线速度部分的前 3 行）。

这个梯度的物理含义很直观：距离的变化率等于两个最近点"相互远离的速度"在法向量方向的投影。如果关节运动使两个点沿法向量方向相互靠近，$\partial d/\partial q < 0$（距离减小）；反之 $\partial d/\partial q > 0$（距离增大）。轨迹优化器利用这个梯度信息"推"关节远离碰撞——这正是足式/230_Perceptive_MPC Perceptive MPC 中摆动腿碰撞回避约束的数学基础。

碰撞对的配置与性能优化 ⭐⭐¶

默认情况下，Pinocchio 会检测所有几何体对之间的碰撞——但对于 N 个几何体，这意味着 $O(N^2)$ 次检测，大部分是不必要的（如同一条腿上相邻 link 之间不可能碰撞）。

# 禁用不需要检测的碰撞对（同一条腿的相邻 link）
collision_model.addCollisionPair(
    pin.CollisionPair(geom_id_A, geom_id_B))  # 显式添加需要检测的对

# 或从 SRDF 文件加载碰撞过滤规则
pin.removeCollisionPairs(model, collision_model,
                         "robot.srdf",
                         verbose=True)
# SRDF 文件定义了"哪些几何体对永远不会碰撞"——通常由 MoveIt! Setup Assistant 生成

配置合理的碰撞对可以将检测时间从数百 $\mu$s 降到几十 $\mu$s——对于 1 kHz 控制循环中的碰撞约束检查，这个优化是必要的。

⚠️ 陷阱：碰撞几何 vs 视觉几何

URDF 中通常定义了两套几何体：<collision> 用于碰撞检测（简化几何，如胶囊体），<visual> 用于可视化（精细 mesh）。pin.buildModelsFromUrdf 会同时加载两者。碰撞检测**必须使用 collision_model**——如果你误用了 visual_model 做碰撞检测，精细 mesh 的 BVH 查询会慢 10-100 倍，而且对 MPC 的碰撞回避约束来说精度完全没有必要。

这使得**碰撞规避约束可以直接嵌入基于梯度的轨迹优化器**（Crocoddyl、IPOPT），而不需要采样式规划器（RRT/PRM）的离散碰撞查询。对于腿足机器人的全身运动规划（如人形机器人穿越狭窄空间），这是必需能力。

本章小结¶

本章下半部分（47.4-47.7）从四个维度展开了 Pinocchio 的工程实战面：

维度	核心内容	解决的问题
模板 Scalar	一份算法代码，四种标量类型实例化	消灭代码重复；统一数值/AD/代码生成/符号计算
核心算法	FK/RNEA/ABA/Jacobian 完整 Python 代码	建立 URDF -> Model -> Data -> 算法的完整心智模型
约束动力学	ConstraintModel + Delassus + ProxQP	闭链、接触、mimic 关节——从教科书走向真实机器人
碰撞接口	Coal 碰撞/距离查询 + 解析梯度	碰撞规避嵌入梯度优化，替代纯采样式规划

上半章（47.1-47.3）讲的是 Pinocchio 的**设计哲学**（CRTP、Model-Data 分离、variant 异构容器），本下半章讲的是**如何用它做事**。两者结合才能既知其所以然、又用得顺手。

练习¶

练习 47-A：加载 Panda URDF 并验证 RNEA 退化形式 ⭐⭐¶

任务：用 Pinocchio Python 绑定加载 Franka Panda 的 URDF（从 example-robot-data 获取）。在 5 个随机构型下分别计算：

tau_rnea = pin.rnea(model, data, q, np.zeros(nv), np.zeros(nv))
tau_grav = pin.computeGeneralizedGravity(model, data, q)

验证两者之差的范数小于 1e-14。

意义：理解 RNEA 在 v=0、a=0 时退化为纯重力项计算。这个退化形式在重力补偿控制器中直接使用——它就是"让机器人在任意构型下保持静止所需的关节扭矩"。

练习 47-B：FK -> IK 往返验证 ⭐⭐⭐¶

任务：

取一个随机关节角 q_target，用 FK 计算末端位姿 T_target
从另一个初始构型 q_init 出发，用阻尼最小二乘 IK 迭代：

for i in range(200):
    pin.forwardKinematics(model, data, q)
    pin.updateFramePlacements(model, data)
    T_current = data.oMf[ee_id]
    e = pin.log6(T_current.inverse() * T_target).vector  # body-fixed 误差
    if np.linalg.norm(e) < 1e-6:
        break
    J = pin.computeFrameJacobian(model, data, q, ee_id, pin.LOCAL)
    dq = J.T @ np.linalg.solve(J @ J.T + 1e-6 * np.eye(6), e)
    q = pin.integrate(model, q, dq)

验证最终位置误差 < 1mm，姿态误差 < 0.01 rad

关键点：必须用 pin.integrate() 而非 q += dq。配置空间可能包含四元数（自由浮动基座），直接加法会破坏单位范数约束。此外注意 log6() 返回的是 body-fixed 误差，所以雅可比要选 LOCAL——这正是 47.5 中 Jacobian 参考坐标系规则的直接应用。

练习 47-C：RNEA double vs 解析导数性能对比 ⭐⭐⭐¶

任务：对 Panda 7-DOF 模型，用 timeit 测量 10000 次调用的平均耗时：

pin.rnea(model, data, q, v, a)——纯逆动力学
pin.computeRNEADerivatives(model, data, q, v, a)——解析导数
手写数值差分：对 q 的每一维做 $\pm\epsilon$ 扰动，调用 $2 \times \text{nv} + 1$ 次 RNEA

记录三者耗时比，并验证解析导数与数值差分的结果误差 < 1e-6。

预期结果：解析导数约为纯 RNEA 的 4-5 倍耗时；数值差分约为 15-20 倍。解析导数快的原因：它在一次正向/反向递归中"顺便"收集梯度信息，复用了所有中间变量；数值差分必须对每个自变量独立完整地重跑一遍 RNEA，无法复用任何中间结果。

练习 47-D：画 CRTP 继承关系图 ⭐⭐¶

任务：阅读以下 Pinocchio 头文件，用 Mermaid 或 PlantUML 画出完整的继承关系图：

文件	要提取的信息
`joint-base.hpp`	`JointModelBase<Derived>` 基类接口
`joint-revolute.hpp`	`JointModelRevoluteTpl`（1-DOF 旋转）
`joint-prismatic.hpp`	`JointModelPrismaticTpl`（1-DOF 平移）
`joint-free-flyer.hpp`	`JointModelFreeFlyerTpl`（6-DOF 浮动基座）
`joint-spherical.hpp`	`JointModelSphericalTpl`（3-DOF 球关节）
`joint-collection.hpp`	`JointCollectionDefaultTpl`（variant 类型列表）

图中须标注三项信息：(a) 每个关节类型的自由度（nq / nv）；(b) CRTP derived() 的调用方向（基类 -> 派生类）；(c) variant 包含了哪些具体类型。完成后对照 47.2-47.3 的文字描述做自检——这张图就是上半章全部 CRTP + variant 内容的可视化总结。

累积项目：从零构建四足站立控制器——本章新增 URDF 加载 + FK/ID 计算模块¶

本课程的足式方向设置了一个贯穿多章的**累积项目**：从零构建一个能在仿真中让四足机器人（Unitree Go2）稳定站立的控制器。每一章在前一章的基础上添加新模块，最终在足式/110_OCS2完整栈与双线程MPC 集成为完整的 MPC + WBC 控制栈。

本章（足式/30_Pinocchio深度精读）是累积项目的起点，负责搭建动力学计算的基础设施层。具体交付物：

阶段 1：URDF 加载与模型检查 - 用 pinocchio::urdf::buildModel 加载 Go2 URDF（浮动基座模式），验证 model.nq=19、model.nv=18、model.njoints=14 - 封装为 RobotModel 类，持有 pinocchio::Model（只读）和一个工厂方法 createData() 用于创建线程独立的 Data

阶段 2：正运动学模块 - 实现 computeFootPositions(model, data, q) 函数，一次调用返回四个足端在世界坐标系下的位置（LOCAL_WORLD_ALIGNED 参考系） - 实现 computeFootJacobians(model, data, q) 函数，返回四个足端的 $6 \times 18$ 雅可比矩阵

阶段 3：逆动力学模块 - 实现 computeGravityCompensation(model, data, q) 函数，调用 RNEA（v=0, a=0）计算重力补偿扭矩 - 实现 computeInverseDynamics(model, data, q, v, a) 函数，封装完整 RNEA 调用 - 编写单元测试：验证 RNEA 退化形式（练习 47-A）、ABA-RNEA 互逆性

阶段 4：多线程验证 - 创建两个线程（模拟 MPC + WBC），各自持有独立 Data，共享同一个 const Model，循环调用 FK + RNEA，用 ThreadSanitizer 验证无数据竞争

后续章节的扩展路线：足式/40_CppAD与代码生成将为这个项目添加 CppADCodeGen 动力学导数预编译模块 $\to$ 足式/90_WBC分层优化与TSID 添加 WBC QP 求解层 $\to$ 足式/110_OCS2完整栈与双线程MPC 添加 OCS2 MPC 外层 $\to$ 最终在 MuJoCo 仿真中实现稳定站立 + 小幅推扰恢复。

🔧 故障排查手册¶

现象	可能原因	排查方法
FK 结果全为单位变换，足端位置集中在原点附近	`forwardKinematics()` 未调用或调用后未执行 `updateFramePlacements()`，`data.oMf` 中仍是未初始化/过期数据	在读取 `data.oMf` 前打印 `data.oMi[1]`，确认非单位阵；确保调用顺序为 `forwardKinematics` → `updateFramePlacements` → 读取
浮动基座模型的 `model.nq` 比预期少 7	`buildModel()` 时未传入 `JointModelFreeFlyer()` 参数，Pinocchio 将基座视为固定	检查 `buildModel` 调用是否传入第二参数；对照 URDF 确认 `model.nq = 7 + n_joints`
RNEA 输出力矩全为零或量级异常	输入的 $q$ 向量维度与 `model.nq` 不匹配，或四元数部分未归一化	用 `pinocchio::normalize(model, q)` 归一化后重新调用；打印 `q.size()` 与 `model.nq` 对照
IK 迭代不收敛，误差不下降	Jacobian 参考坐标系（LOCAL / WORLD / LOCAL_WORLD_ALIGNED）与误差坐标系不匹配	若误差用 `log6(T_err)` 计算（body-fixed），雅可比须用 `LOCAL`；若误差在世界坐标系下定义，用 `LOCAL_WORLD_ALIGNED`
多线程调用 Pinocchio 时出现数据竞争或结果不稳定	多个线程共享了同一个 `Data` 对象	确保每个线程持有独立的 `pinocchio::Data` 实例，仅共享 `const Model`；用 ThreadSanitizer 编译验证
约束动力学求解发散（ddq 爆炸或 NaN）	ProximalSolver 的 $\mu$ 太小或约束近乎冗余	增大 $\mu$（如 1e-6 → 1e-4）；检查约束雅可比 $J_c$ 的条件数；确认约束不矛盾（如两个约束要求同一关节去不同位置）
`computeAllTerms` 后 `data.oMf` 仍为旧值	`computeAllTerms` 不调用 `updateFramePlacements`	在 `computeAllTerms` 之后手动调用 `updateFramePlacements(model, data)`
碰撞检测漏检（明显相交的几何体返回 is_colliding=false）	URDF 中碰撞几何体路径错误（mesh 未加载），或 `updateGeometryPlacements` 未在 FK 后调用	检查 `collision_model.ngeoms` 是否 > 0；确认 FK → `updateGeometryPlacements` → `computeCollisions` 的调用顺序

Pinocchio API 速查手册¶

以下按使用场景组织最常用的 API 调用模式。这不是 API 文档的替代品，而是根据腿足控制栈的实际需求总结的"食谱"。

场景 A：WBC 控制循环（1 kHz） ⭐⭐¶

# 每个控制周期需要的完整计算
pin.computeAllTerms(model, data, q, v)   # M, nle, FK, J, CoM
pin.updateFramePlacements(model, data)   # oMf（frame 位姿）

# 提取 WBC 需要的量
M = data.M                              # 惯量矩阵
nle = data.nle                           # C*dq + g
for foot in foot_ids:
    J = pin.getFrameJacobian(model, data, foot, pin.LOCAL_WORLD_ALIGNED)
    pos = data.oMf[foot].translation

场景 B：MPC 轨迹优化（需要导数） ⭐⭐⭐¶

# MPC 需要动力学导数
pin.computeRNEADerivatives(model, data, q, v, a)
dtau_dq = data.dtau_dq   # ∂τ/∂q
dtau_dv = data.dtau_dv   # ∂τ/∂v
# Pinocchio 也提供 M 的 Cholesky 分解用于快速求解 M^{-1} b
pin.computeMinverse(model, data, q)
Minv = data.Minv          # M^{-1}

场景 C：质心动力学（SRBD MPC） ⭐⭐⭐¶

# 单刚体动力学 MPC 需要质心和质心 Jacobian
pin.centerOfMass(model, data, q, v)      # 填充 com, vcom
pin.jacobianCenterOfMass(model, data, q)  # 填充 Jcom
com = data.com[0]        # 质心位置 (3,)
vcom = data.vcom[0]      # 质心速度 (3,)
Jcom = data.Jcom          # 质心 Jacobian (3, nv)
total_mass = pin.computeTotalMass(model)

场景 D：Model 自省（调试用） ⭐¶

# 遍历所有关节
for i in range(model.njoints):
    print(f"Joint {i}: {model.names[i]}, "
          f"parent={model.parents[i]}, "
          f"nq={model.joints[i].nq()}, nv={model.joints[i].nv()}")

# 遍历所有 frame
for i in range(model.nframes):
    frame = model.frames[i]
    print(f"Frame {i}: {frame.name}, "
          f"type={frame.type}, "
          f"parent_joint={frame.parentJoint}")

# 检查关节限位
print(f"q_lower: {model.lowerPositionLimit}")
print(f"q_upper: {model.upperPositionLimit}")
print(f"v_max:   {model.velocityLimit}")
print(f"tau_max: {model.effortLimit}")

Pinocchio 2.x vs 3.x 迁移要点 ⭐⭐¶

如果你使用的是 Pinocchio 3.x（推荐），以下是相对于 2.x 的主要变化：

功能	Pinocchio 2.x	Pinocchio 3.x
加载示例模型	`EXAMPLE_ROBOT_DATA_MODEL_DIR` 环境变量	`from example_robot_data import load`
碰撞库名称	HPP-FCL (`hppfcl`)	Coal (`coal`)
约束动力学	不支持	`ConstraintModelTpl` + `constraintDynamics()`
Mimic 关节	手工处理	v3.3+ 原生支持
ProximalSolver	不存在	`ProximalSettings` + 近端约束求解
命名空间	`pinocchio::`	`pinocchio::` (不变)
Python 包名	`import pinocchio`	`import pinocchio` (不变)

47.7.5 Frames 体系深入——Joint 之外的"参考点"系统 ⭐⭐⭐¶

动机：为什么 Joint 不够，还需要 Frame？ ⭐⭐¶

47.5 节用 getFrameJacobian 取足端雅可比、用 model.getFrameId("FL_foot") 找足端，并在多处陷阱里反复提醒"forwardKinematics 只更新 data.oMi（关节），要读 data.oMf（frame）必须先 updateFramePlacements"。但我们一直没有正面回答：Frame 到底是什么？它和 Joint 是什么关系？ 这个缺口在 WBC 里会致命——任务空间几乎全部定义在 frame 上（足端、手端、IMU、相机），搞不清 frame 体系，连"末端在哪"都取不对。

考虑一个具体问题：Go2 的足端 FL_foot 在哪个 Joint 上？答案是它**不在任何 Joint 上**——足端是小腿 link 末端的一个固定点，没有自由度。如果 Pinocchio 只有 Joint 概念，你就没法引用这个点。再想机械臂的 TCP（工具中心点）、IMU 安装位置、相机光心——它们全都是刚体上的**固定参考点**，不是关节。

反事实推理：如果 Pinocchio 不提供 Frame，只能用 Joint 来表达末端会怎样？你要么被迫在 URDF 里给每个关心的点加一个"假关节"（nq=0 的固定关节，污染关节树、虚增 njoints），要么每次都手动维护"从某个 Joint 到目标点的固定偏移 SE3"并自己做坐标变换——后者正是初学者的常见做法，但它把本应由库管理的拓扑信息散落到用户代码里，极易出错。Frame 系统就是把"刚体上的命名参考点"提升为一等公民，让 getFrameJacobian/getFrameVelocity 直接对它工作。

Frame 的数据结构与五种类型 ⭐⭐⭐¶

一个 Frame 是"挂在某个 Joint 上、相对该 Joint 有固定偏移的命名参考点"。它的字段（已与官方 API 核实）：

字段	类型	含义
`name`	`str`	frame 名称（如 `"FL_foot"`、`"panda_hand"`）
`parentJoint`	`int`	支撑该 frame 的关节索引——frame 刚性附着在这个关节的 body 上
`parentFrame`	`int`	父 frame 索引（仅用于记录 frame 树的层级，计算时不用它）
`placement`	`SE3`	frame 相对 `parentJoint` 局部坐标系的固定偏移 ${}^{\text{joint}} M_{\text{frame}}$
`type`	`FrameType`	frame 类型（见下表）
`inertia`	`Inertia`	附加在该 frame 上的惯量（如末端负载）

⚠️ 概念误区：parentFrame vs parentJoint

错误描述：以为 frame 的位姿是"沿 parentFrame 链一级级算出来的"，于是去操心 frame 树的层级。现象/后果：在自定义 frame、或调试 oMf 时把精力浪费在 parentFrame 上，却找不到位姿算错的真正原因。根本原因：Pinocchio 计算 frame 世界位姿用的是 data.oMf[f] = data.oMi[parentJoint] * frame.placement——只依赖 parentJoint 和 placement，完全不经过 parentFrame。parentFrame 仅仅是为了记录"这个 frame 是从哪个 frame 派生来的"这一族谱信息，对运动学计算无影响。官方文档明确：parentJoint 才是你该关心的量。正确做法：定位 frame 位姿问题时，检查 frame.parentJoint（对不对）和 frame.placement（偏移对不对），不要管 parentFrame。

FrameType 枚举有五个值，对应 frame 的不同来源：

`FrameType`	来源	典型例子
`JOINT`	与某个真实关节同位的 frame	每个关节自动生成一个同名 frame
`FIXED_JOINT`	URDF 中被 Pinocchio "吸收"的固定关节	URDF 里 `type="fixed"` 的关节不会成为 Joint，而是降级为 FIXED_JOINT 类型的 frame
`BODY`	link（连杆）的参考 frame	URDF 中每个 `<link>` 对应一个 BODY frame
`OP_FRAME`	操作 frame（Operational Frame），用户定义的虚拟点	手动 `addFrame` 加的 TCP、足端控制点、传感器位置
`SENSOR`	传感器 frame	IMU、力/力矩传感器的安装位置

本质洞察：URDF 里的"固定关节"在 Pinocchio 里**消失**了——它不占 nq/nv，被合并进父 body 的惯量，只留下一个 FIXED_JOINT 类型的 frame 记录它原来的位置。这是 Pinocchio 的一个关键优化：固定关节没有自由度，保留它只会让关节树虚胖、让 RNEA 多遍历无意义的节点。把它降级为 frame，既保留了"这个位置可被引用"的能力，又不付出动力学计算的代价。这解释了一个常见困惑——"我 URDF 里明明有 20 个关节，为什么 model.njoints 只有 14？"：因为 6 个是固定关节，被吸收成 frame 了。

自定义 Frame：`addFrame` ⭐⭐¶

实践中你常需要给机器人加一个 URDF 里没有的控制点——比如在足端再往下偏移 2cm 到接触面、或在末端工具尖端定义 TCP：

import pinocchio as pin
import numpy as np

robot = erd_load("panda")
model, data = robot.model, robot.model.createData()

# 在 panda_hand 末端往前偏移 0.1m 处定义一个 TCP（工具中心点）
hand_frame_id = model.getFrameId("panda_hand")
hand_frame    = model.frames[hand_frame_id]

# 新 frame 挂在与 panda_hand 相同的 parentJoint 上，
# placement 是 panda_hand 的偏移再叠加 0.1m 的 z 向偏移
tcp_placement = hand_frame.placement * pin.SE3(np.eye(3), np.array([0, 0, 0.1]))
tcp_frame = pin.Frame("tcp",
                      hand_frame.parentJoint,   # 同一个支撑关节
                      hand_frame_id,             # parentFrame（仅记录）
                      tcp_placement,             # 相对 parentJoint 的固定偏移
                      pin.FrameType.OP_FRAME)    # 操作 frame
tcp_id = model.addFrame(tcp_frame)

# 注意：addFrame 改了 model，必须重新 createData（Data 的 oMf 长度变了）
data = model.createData()

q = pin.neutral(model)
pin.forwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
print(f"TCP 位姿:\n{data.oMf[tcp_id]}")

⚠️ 编程陷阱：addFrame 之后必须重建 Data

错误描述：调用 model.addFrame(...) 后继续用旧的 data 对象。现象/后果：访问新 frame 的 data.oMf[new_id] 越界崩溃，或读到垃圾——因为旧 data.oMf 是按旧 model.nframes 预分配的，没有新 frame 的槽位。根本原因：Data 在构造时根据 Model 的维度**一次性预分配**所有缓冲（47.3 的核心设计）。addFrame 增加了 model.nframes，旧 Data 的 oMf 数组长度跟不上。正确做法：任何修改 Model 结构的操作（addFrame、addJoint、appendModel）之后，必须 data = model.createData() 重建。这也是为什么 Model 的结构修改应当**全部在初始化阶段完成**、运行时绝不碰 Model——与 47.3 "Model 加载后只读"的原则一脉相承。

Frame 的速度与加速度——spatial vs classical ⭐⭐⭐¶

47.5 只讲了 frame 的**位姿**（oMf）和**雅可比**（getFrameJacobian）。但接触控制、阻抗控制还需要 frame 的**速度和加速度**。Pinocchio 提供（均已与官方 API 核实，返回 Motion 对象）：

# 必须先 forwardKinematics（带 v、a 才能取速度、加速度）
q = pin.randomConfiguration(model)
v = np.random.randn(model.nv)
a = np.random.randn(model.nv)
pin.forwardKinematics(model, data, q, v, a)   # 同时传 q,v,a
pin.updateFramePlacements(model, data)

fid = model.getFrameId("panda_hand")

# 空间速度（6D twist），可选参考坐标系
vel = pin.getFrameVelocity(model, data, fid, pin.LOCAL_WORLD_ALIGNED)
print(vel.linear, vel.angular)

# 空间加速度（spatial acceleration）——注意这不是"看得见的"加速度
acc_spatial = pin.getFrameAcceleration(model, data, fid, pin.LOCAL_WORLD_ALIGNED)

# 经典加速度（classical acceleration）——这才是质点真实的 d²p/dt²
acc_classic = pin.getFrameClassicalAcceleration(model, data, fid, pin.LOCAL_WORLD_ALIGNED)

这里藏着一个**极其重要、极其容易错**的区分：空间加速度（spatial acceleration） 与 经典加速度（classical acceleration） 不是一回事。

空间加速度是空间速度的纯时间导数 $\dot{V} = (\dot v, \dot\omega)$（在运动旋量意义下）。但一个固连在刚体上的物理点 $p$，它"看得见的"线加速度 $\ddot p$ 还要加上一个**向心/科氏耦合项** $\omega \times v$：

\[a_{\text{classical, linear}} = a_{\text{spatial, linear}} + \omega \times v_{\text{linear}}\]

角加速度部分两者相同（$\dot\omega$）。

⚠️ 思维陷阱：用空间加速度当物理加速度做接触/碰撞约束

错误描述：在写"足端落地瞬间垂直加速度为零"或"末端跟踪期望笛卡尔加速度"的约束时，直接用 getFrameAcceleration（空间加速度）。现象/后果：当 frame 同时有较大平移速度和角速度时（如快速摆动腿），少了 $\omega\times v$ 项，约束方程有系统偏差——足端落地有冲击、笛卡尔跟踪有稳态误差，且速度越大误差越大，低速测试时还发现不了。根本原因：空间加速度 $\dot V$ 是旋量的导数，**不等于**刚体上某点的真实二阶位移导数 $\ddot p$。两者差一个 $\omega \times v$ 的向心项。任务空间控制律里的"加速度"几乎总是指经典加速度（$\ddot p$），因为期望轨迹是用笛卡尔位置二阶导描述的。正确做法：任务空间（笛卡尔）加速度约束一律用 getFrameClassicalAcceleration。只有当你在空间向量（旋量）框架内做纯代数推导、且全程保持旋量一致性时，才用 getFrameAcceleration。这个区分也是 WBC（足式/90）里任务加速度 $\ddot x = J\ddot q + \dot J \dot q$ 中 $\dot J\dot q$ 项（drift 项）必须用经典约定的根源。

为什么 Frame 是 WBC 的"任务接口" ⭐⭐¶

把本节接到下游：全身控制（足式/90_WBC分层优化与TSID）的每一个任务——足端跟踪、质心调节、躯干姿态、自碰撞规避——都要先回答"任务量定义在机器人的哪个点上"。答案几乎总是一个 frame。WBC 的任务雅可比就是 getFrameJacobian、任务速度就是 getFrameVelocity、任务的 drift 项 $\dot J\dot q$ 就藏在 getFrameClassicalAcceleration 里。

本质洞察：Joint 是机器人的"驱动接口"（你能直接命令的自由度），Frame 是机器人的"任务接口"（你想控制的物理点）。控制的本质就是在这两个接口之间架桥——用关节空间的输入（$\tau$ 作用在 Joint 上）实现任务空间的目标（位姿/速度定义在 Frame 上），而连接二者的正是 frame 雅可比 $J = \partial(\text{frame 运动})/\partial(\text{关节运动})$。理解了"Joint 驱动、Frame 任务、Jacobian 架桥"这个三角，后续 WBC 的 QP 结构（决策变量是关节量、约束/代价定义在 frame 量上）就一目了然了。

练习 ⭐⭐¶

练习 47.7.5.1（⭐⭐）：遍历 Go2 模型的所有 frame（for f in model.frames），统计每种 FrameType 的数量。验证 BODY 类型 frame 的数量等于 model.nbodies、JOINT 类型 frame 数量与 model.njoints 的关系。然后找出所有 OP_FRAME 和 SENSOR 类型的 frame，理解它们分别对应机器人的什么物理部件。

练习 47.7.5.2（⭐⭐⭐）：数值验证 spatial 与 classical 加速度的关系。取一个有较大速度的随机状态 $(q, v, a)$，用 getFrameVelocity 取 $\omega, v_{\text{lin}}$，用 getFrameAcceleration 取空间加速度，手动加上 $\omega \times v_{\text{lin}}$，验证结果等于 getFrameClassicalAcceleration 的线性部分。这道题让你彻底记住"任务空间用 classical"——以后写 WBC 不会再用错。

练习 47.7.5.3（⭐⭐，跨节综合）：综合 47.5 和本节——给 Panda 加一个 TCP frame（addFrame），重建 Data，然后同时取 TCP 的 getFrameJacobian（LOCAL_WORLD_ALIGNED）和 getFrameVelocity，验证 J @ v 等于 getFrameVelocity 返回的 6D twist。这把"雅可比是速度到关节速度的线性映射"这个定义在自定义 frame 上闭环验证一遍。

47.8 Pinocchio 3.x 新特性与动力学库生态更新（2024-2026） ⭐⭐⭐¶

Pinocchio 3.x 新 API 亮点 ⭐⭐¶

Pinocchio 3.x 系列（3.0-3.4，截至 2026 年初）在 2.x 的基础上引入了多项面向工程和研究的重要更新：

MeshCat 可视化集成：Pinocchio 3.x 通过 pinocchio.visualize.MeshcatVisualizer 提供了开箱即用的 3D 可视化。相比旧版的 Gepetto-viewer（需要安装 CORBA 通信栈），MeshCat 基于 WebSocket + Three.js，在浏览器中渲染，安装零依赖、远程可访问。对于在服务器上运行 MPC 实验的场景，MeshCat 的浏览器渲染特别方便——你不需要 X11 转发。

# Pinocchio 3.x MeshCat 可视化示例
from pinocchio.visualize import MeshcatVisualizer

viz = MeshcatVisualizer(model, collision_model, visual_model)
viz.initViewer(open=True)  # 自动打开浏览器
viz.loadViewerModel()
viz.display(q)  # 显示当前位形

Coal 碰撞梯度：Coal（HPP-FCL 的继任者）从 3.0 版本起支持最近距离的解析导数 $\partial d / \partial q$。这一特性对轨迹优化至关重要——Crocoddyl/Aligator 可以将碰撞避免作为光滑约束（而非硬碰撞检测），基于梯度信息高效推开轨迹。在 Pinocchio 3.x 中，computeDistances() 返回的 DistanceResult 对象包含了最近点对的位置信息，结合 Pinocchio 的解析 Jacobian 即可链式求导得到碰撞距离对关节角的梯度。

CasADi 后端：除 CppAD 外，Pinocchio 3.x 新增了对 CasADi 符号框架的原生支持。CasADi 的优势在于它与 IPOPT 等 NLP 求解器有开箱即用的集成，适合非线性轨迹优化问题。实例化方式为 pinocchio.ModelTpl[casadi.SX]，tape 录制和求导全部由 CasADi 代理。

与 Drake / MuJoCo Python 绑定的对比更新（2025） ⭐⭐⭐¶

2024-2025 年，Drake 和 MuJoCo 的 Python 生态发生了显著变化，与 Pinocchio 的竞争格局值得重新审视：

维度	Pinocchio 3.x	Drake (2025)	MuJoCo 3.x + `mujoco` Python
核心定位	纯动力学引擎（解析导数优先）	全栈仿真+控制框架	物理仿真引擎（接触求解优先）
解析导数	RNEA/ABA/CRBA 全套解析导数	部分解析导数（MultibodyPlant 的质量矩阵和偏置力）	不提供解析导数（依赖 MJX 的 JAX AD）
Python 绑定质量	eigenpy 3.x，NumPy 2.0 兼容	pydrake，深度集成但安装复杂	`mujoco` 包，pip install 即用
接触求解	3.x ProximalSolver（刚性接触）	基于 SAP/TAMSI 的离散接触	Newton/PGS 求解器（业界最成熟）
GPU 支持	无	无	MJX（JAX 后端，GPU 批量仿真）
RL 训练适配	不直接适配（需要配合 gym 包装）	不直接适配	MuJoCo Playground / Gymnasium 直接集成
典型用户	OCS2、Crocoddyl、TSID、Aligator	抓取规划、hydroelastic 接触	RL 训练、MPC 验证、MJX 可微优化

本质洞察：Pinocchio 和 MuJoCo/Drake 之间不是简单的"谁更好"关系。Pinocchio 的优势在于解析导数的极致性能——这对 MPC 实时性至关重要；MuJoCo 的优势在于接触仿真的精度和 GPU 并行的规模；Drake 的优势在于全栈集成（从建模到控制到可视化的一站式体验）。工程实践中，三者常常组合使用：用 Pinocchio 做 MPC 内部的动力学求值，用 MuJoCo 做仿真验证和 RL 训练，用 Drake 做抓取规划和接触丰富的操作任务。

延伸阅读¶

必读经典 ⭐¶

资料	类型	难度	说明
Featherstone, Rigid Body Dynamics Algorithms, Springer 2008	教材	⭐⭐⭐	递归动力学算法的经典教材。RNEA/ABA/CRBA 的完整推导和伪代码均出自本书，是理解 Pinocchio 算法层的必读文献
Carpentier & Mansard, "Analytical Derivatives of Rigid Body Dynamics Algorithms", RSS 2018	论文	⭐⭐⭐⭐	Pinocchio 解析导数的理论基础。推导了 RNEA 导数的闭式递推公式，性能比 AD 快 3-5 倍。理解本文是深入 Crocoddyl/Aligator backward pass 的前提
Pinocchio 官方 Tutorial（gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/）	文档	⭐⭐	官方 API 文档和入门教程。Python 和 C++ 示例覆盖 FK/RNEA/Jacobian 等核心功能，适合边查边用

进阶与前沿 ⭐⭐⭐¶

资料	类型	难度	说明
Pinocchio GitHub 仓库（github.com/stack-of-tasks/pinocchio）	源码	⭐⭐	阅读 `include/pinocchio/algorithm/rnea.hxx` 可直接看到模板化 RNEA 的实现；`joint-base.hpp` 和 `joint-collection.hpp` 是理解 CRTP + variant 的最佳入口
Carpentier J., et al. (2019) "The Pinocchio C++ library -- A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives", SII	论文	⭐⭐⭐	Pinocchio 的系统级论文，包含完整的性能基准测试和设计决策分析
Jallet W., Bambade A., Mansard N., Carpentier J. (2024) "PROXDDP: Proximal Constrained Trajectory Optimization", RSS	论文	⭐⭐⭐⭐	Aligator 求解器的理论基础，与 Pinocchio 3.x 的 ProximalSolver 共享数学框架
Singh S., Russell R., Wensing P. (2022) "Efficient Analytical Derivatives of Rigid-Body Dynamics using Spatial Vector Algebra", RA-L	论文	⭐⭐⭐⭐	ABA 导数的解析递推公式，补全了 RNEA 导数之外的理论空白
Coal (HPP-FCL) GitHub（github.com/coal-library/coal）	源码	⭐⭐	Pinocchio 的碰撞检测后端，v3.0 起支持最近距离的解析导数

求导方法	RNEA 导数耗时	相对解析导数
数值差分	~15 \(\mu\)s	3x 慢
CppAD AD	~20 \(\mu\)s	4x 慢
Pinocchio 解析导数	~5 \(\mu\)s	基准

多态方式	RNEA 耗时	相对基准	内联可能性
虚函数继承	~2.5 \(\mu\)s	178%	不可能
CRTP + variant	~1.4 \(\mu\)s	100%	部分可能
纯 CRTP（假设可行）	~1.3 \(\mu\)s	93%	完全可能

类型	数学对象	维度	物理含义	李群/代数
`SE3` (`SE3Tpl<Scalar>`)	刚体位姿 \({}^A M_B\)	\(R \in SO(3)\) + \(p \in \mathbb{R}^3\)	坐标系变换	群 \(SE(3)\)
`Motion` (`MotionTpl<Scalar>`)	空间速度 twist	\((v, \omega) \in \mathbb{R}^6\)	线速度 + 角速度	代数 \(\mathfrak{se}(3)\)
`Force` (`ForceTpl<Scalar>`)	空间力 wrench	\((f, \tau) \in \mathbb{R}^6\)	力 + 力矩	对偶 \(\mathfrak{se}(3)^*\)
`Inertia` (`InertiaTpl<Scalar>`)	空间惯量	\(6 \times 6\) 对称正定	质量 + 质心 + 转动惯量	映射 \(\mathfrak{se}(3) \to \mathfrak{se}(3)^*\)

递推公式中的符号	空间代数 API	出现在
\({}^i X_{\lambda(i)}\, v_{\lambda(i)}\)（父速度变到子系）	`iMli.actInv(v_parent)`	RNEA/ABA 正向递归
\(v_i \times S_i \dot q_i\)（科氏耦合）	`v.cross(S_dq)`	RNEA 加速度递归
\(I_i a_i\)（牛顿-欧拉）	`Y * a`	RNEA 力递归
\(v_i \times^* (I_i v_i)\)（陀螺项）	`v.cross(Y * v)`	RNEA 力递归
\({}^{\lambda(i)} X_i^*\, f_i\)（子力汇聚到父）	`iMli.act(f_child)`	RNEA 反向递归
\(I_i^c = I_i + \sum {}^iX_j^* I_j^c {}^jX_i\)（复合惯量）	`Y_i + iMlj.act(Y_j)`	CRBA 后向递归

枚举值	含义	速度关系
`LOCAL`	表达在末端 frame 自身坐标系（body-fixed）	\({}^B v = J_{\text{local}} \cdot \dot{q}\)
`WORLD`	表达在世界坐标系原点	\({}^W v = J_{\text{world}} \cdot \dot{q}\)
`LOCAL_WORLD_ALIGNED`	表达在末端位置处，但朝向与世界坐标系对齐的坐标系	\({}^{W,p} v = J_{\text{lwa}} \cdot \dot{q}\)

MPC 需要的量	Pinocchio 提供	调用
当前质心位置 \(c\)	`data.com[0]`	`centerOfMass(model, data, q)`
当前质心动量 \(h_G\)（线 + 角）	`data.hg`	`computeCentroidalMomentum` / `ccrba`
整机绕质心转动惯量 \(\bar I_G\)（角动量约束用）	`data.Ig`（6×6 的右下 3×3 块）	`ccrba`
全身运动到质心动量的映射 \(A_g\)	`data.Ag`	`ccrba`

特性	直接 KKT	ProximalSolver
数值稳定性	约束冗余时条件数 \(\to \infty\)	\(\mu\) 正则化保证有界
不等式约束	需要 active set 或 interior point	投影算子自然处理
摩擦锥	需要线性化（多面体近似）	直接投影到 SOC 锥
收敛速度	一步精确（但可能不存在解）	线性收敛（\(\mu\) 小时快）
与 Aligator 的关系	—	Aligator/ProxDDP 内部使用同一套近端框架

第47章：Pinocchio 深度精读——CRTP + Model-Data + 模板 Scalar¶

前置自测¶

本章目标¶

47.1 Pinocchio 在机器人 C++ 生态中的位置 ⭐⭐¶

动机：从"一个 SE(3)"到"N 个串联 SE(3)" ⭐¶

核心算法：Featherstone 递归族 ⭐⭐¶

杀手级特性：解析导数 ⭐⭐⭐¶

生态图：一个库撑起一个学派 ⭐¶

47.2 CRTP 关节类型系统 ⭐⭐⭐¶

动机：1kHz 控制循环中 vtable 开销不可接受 ⭐⭐¶

CRTP 层级结构：JointModelBase ⭐⭐⭐¶

与 Sophus CRTP 的规模对比 ⭐⭐¶

calc() 的编译期派发 ⭐⭐⭐¶

CRTP 的"异构容器"难题与 Variant 解法 ⭐⭐⭐¶

47.3 Model / Data 分离范式 ⭐⭐¶

动机——反面：如果不分离会怎样 ⭐⭐¶

理论：Model 持有不变量，Data 持有缓冲 ⭐⭐¶

线程安全：N 线程共享 1 个 Model ⭐⭐⭐¶

与 MuJoCo mjModel / mjData 的映射 ⭐⭐¶

与 Drake MultibodyPlant 和 RBDL 的对比 ⭐⭐¶

代码实战：从 URDF 到 FK ⭐¶

练习 ⭐⭐¶

47.3.6 空间代数原语——SE3 / Motion / Force / Inertia ⭐⭐⭐¶

动机：为什么不能用"普通的向量和矩阵"做动力学？ ⭐⭐¶

历史：Featherstone 的空间向量 ⭐¶

四个核心类型 ⭐⭐⭐¶

核心运算 1：act / actInv——坐标系变换 ⭐⭐⭐¶

核心运算 2：cross——空间叉乘 ⭐⭐⭐¶

核心运算 3：Inertia 作用于 Motion——牛顿-欧拉方程 ⭐⭐¶

小结：空间代数如何"撑起"整个算法层 ⭐⭐¶

与单个 SE(3)（Sophus/manif）的边界 ⭐⭐¶

练习 ⭐⭐¶

47.3.5 承上启下：从设计哲学到核心算法实战¶

47.4 模板化 Scalar 类型——一份代码四种用途 ⭐⭐⭐¶

动机：为什么同一个 RNEA 要"变身"四次？ ⭐⭐¶

ModelTpl<Scalar> 的模板实例化机制 ⭐⭐⭐¶

实战：同一调用 double vs AD 类型 ⭐⭐¶

为什么下游框架需要不同的 Scalar？ ⭐⭐¶

与 Ceres Jet 的对比（01_数学/30_优化理论） ⭐⭐¶

47.5 核心算法实战——FK / RNEA / ABA / Jacobian ⭐⭐¶

完整流程：加载 URDF → 正运动学 → 打印末端位姿 ⭐¶

RNEA：给定 (q, v, a)，计算 tau ⭐⭐¶

ABA：给定 (q, v, tau)，计算加速度 ⭐⭐¶

Jacobian：关节雅可比矩阵 ⭐⭐¶

Frame vs Joint 雅可比：三种参考坐标系 ⭐⭐⭐¶

RNEA 递推公式拆解——理解"为什么 O(N)" ⭐⭐⭐¶

性能基准 ⭐⭐¶

CRBA 递推拆解——惯量矩阵 \(M(q)\) 是怎么算出来的 ⭐⭐⭐¶

47.5.5 质心动力学与 CCRBA——SRBD MPC 的地基 ⭐⭐⭐¶

动机：腿足 MPC 为什么盯着"质心动量"？ ⭐⭐¶

质心动量矩阵 \(A_g\) 与 CCRBA ⭐⭐⭐¶

质心动量的时间导数与 \(\dot A_g\) ⭐⭐⭐¶

与 computeAllTerms 的关系 ⭐⭐¶

理论-工程桥接：从 data.hg 到 SRBD MPC 的状态方程 ⭐⭐⭐¶

练习 ⭐⭐¶

47.6 约束动力学（v3.x 新增） ⭐⭐⭐⭐¶

约束建模接口 ⭐⭐⭐¶

Delassus 算子与约束求解 ⭐⭐⭐⭐¶

与下游控制框架的衔接 ⭐⭐¶

ProximalSolver：近端正则化约束求解器 ⭐⭐⭐⭐¶

Worked Example：完整的正逆动力学闭环验证 ⭐⭐¶

Worked Example：RNEA 解析导数 vs 数值差分——完整对比流程 ⭐⭐⭐¶

47.7 Pinocchio + Coal 碰撞接口 ⭐⭐⭐¶

为什么碰撞检测对腿足机器人重要 ⭐⭐¶

碰撞检测与距离计算 ⭐⭐⭐¶

碰撞梯度与轨迹优化集成 ⭐⭐⭐¶

碰撞对的配置与性能优化 ⭐⭐¶

本章小结¶

练习¶

练习 47-A：加载 Panda URDF 并验证 RNEA 退化形式 ⭐⭐¶

练习 47-B：FK -> IK 往返验证 ⭐⭐⭐¶

练习 47-C：RNEA double vs 解析导数性能对比 ⭐⭐⭐¶

练习 47-D：画 CRTP 继承关系图 ⭐⭐¶

累积项目：从零构建四足站立控制器——本章新增 URDF 加载 + FK/ID 计算模块¶

🔧 故障排查手册¶

Pinocchio API 速查手册¶

场景 A：WBC 控制循环（1 kHz） ⭐⭐¶

场景 B：MPC 轨迹优化（需要导数） ⭐⭐⭐¶

场景 C：质心动力学（SRBD MPC） ⭐⭐⭐¶

场景 D：Model 自省（调试用） ⭐¶

核心运算 1：`act` / `actInv`——坐标系变换 ⭐⭐⭐¶

核心运算 2：`cross`——空间叉乘 ⭐⭐⭐¶

核心运算 3：`Inertia` 作用于 `Motion`——牛顿-欧拉方程 ⭐⭐¶

`ModelTpl<Scalar>` 的模板实例化机制 ⭐⭐⭐¶

与 `computeAllTerms` 的关系 ⭐⭐¶

理论-工程桥接：从 `data.hg` 到 SRBD MPC 的状态方程 ⭐⭐⭐¶

自定义 Frame：`addFrame` ⭐⭐¶