本章解决一个问题：给定机器人的姿态、速度、加速度，各关节需要输出多大力矩？这是动力学库存在的理由，也是第 13、14 章两套控制律的发动机。

先修：第 00 章（环境）、第 03 章（Model / Data 的分工）、第 07 章（正运动学）。
本章新增的 C++ 知识：Eigen 的「矩阵视图」（selfadjointView）——它不拷贝数据，只是换一种方式解读同一块内存。
产出：tutorials/04_dynamics/01_inverse_dynamics.cpp。里面藏着全书最重要的一个坑。

---

10.1 逆动力学到底在算什么

先把「逆」这个字讲清楚。机器人学里有一对方向相反的问题：

问题	已知	求解	Pinocchio 的函数
正动力学（forward dynamics）	姿态、速度、力矩	加速度 q̈ = M⁻¹(τ − b)	aba()（第 11 章）
逆动力学（inverse dynamics）	姿态、速度、加速度	力矩 τ = M q̈ + b	rnea()（本章）

两者由同一个方程联系：

τ = M(q)·q̈ + b(q, q̇)
  = M(q)·q̈ + C(q, q̇)·q̇ + G(q)

• M(q)：惯性矩阵 / 质量矩阵，nv × nv，对称正定。它是「多刚体系统」版的 m：把关节加速度翻译成关节力矩的系数矩阵。它随姿态变化，因为连杆之间的相对位置变了。
• G(q)：重力项——让机器人静止在姿态 q 上所需的力矩。注意它已经把重力方向算进去了，补偿时是 τ += G 而不是 τ -= G。本章输出里 rnea(q,0,0) 和 computeGeneralizedGravity(q) 逐位相等，就是这个符号约定的直接证据。
• C(q,q̇)·q̇：科氏力与离心力项。
• b(q,q̇) = C·q̇ + G：Pinocchio 里的 data.nle（non-linear effects，非线性效应）。

为什么逆动力学更常用：控制、轨迹优化、力控、人形机器人的 WBC，需要的都是「这个运动要多大力」，也就是逆问题。逆动力学有 O(n) 的 RNEA（Recursive Newton-Euler Algorithm，递归牛顿-欧拉）算法，6 自由度机械臂只需几微秒；正动力学要用到 M⁻¹，更贵。入门阶段九成的动力学调用是 rnea。

M 从哪来？它不是量出来的，而是用 CRBA（Composite Rigid Body Algorithm，复合刚体算法）算的——思路是「令第 j 个关节产生单位加速度、其余关节不动，需要多大力矩」，逐列求出来，第 j 列就是 M 的第 j 列。RNEA 沿链正推再反推，CRBA 逐列构造，两者算法路径完全不同——所以它们的结果应当相等，这本身就是一条可以自检的恒等式。本章 10.7 节就干这件事。

---

10.2 五个函数，先看「调用之前必须做什么」

函数	结果放在哪	输入	备注
rnea(model, data, q, q_dot, q_ddot)	返回值就是 tau	q(nq), v(nv), a(nv)	4.x 没有 data.tau 可读（那个字段属于约束系统）
computeGeneralizedGravity(model, data, q)	data.g	q(nq)	等价于 rnea(q, 0, 0)
nonLinearEffects(model, data, q, q_dot)	data.nle	q(nq), v(nv)	= 科氏/离心 + 重力
crba(model, data, q)	data.M（只填上三角）	q(nq)	完整质量矩阵
computeMinverse(model, data, q)	data.Minv（只填上三角）	q(nq)	M⁻¹，同样只填一半

三条通用规则，后面每一章都成立：

1. data 是草稿纸：任何一次调用都可能覆盖你上一次留下的结果。所以本章代码里凡是「待会儿还要用的中间量」，一律当场拷进局部变量（Eigen::VectorXd g_neutral = data.g;）。
2. 只填上三角 ≠ 算错了：M 对称，下半三角和上半三角一模一样，写两遍纯属浪费内存带宽。Pinocchio 只写一份，代价是逼调用方负责还原对称性。忘记还原是新手最常见的「不报错、但结果全错」。
3. 需要稠密矩阵时才做转换：data.M.selfadjointView<Eigen::Upper>() 是一个视图（不拷贝）；一旦你写 M * q_ddot，或者把它赋给 Eigen::MatrixXd，转换才真正发生。

---

10.3 工程里加上这一章

打开根 CMakeLists.txt，把第 00 章留下的那行注释去掉：

add_subdirectory(tutorials/01_basics)
add_subdirectory(tutorials/02_model)
add_subdirectory(tutorials/03_kinematics)
add_subdirectory(tutorials/04_dynamics)   # ← 本章

新建 tutorials/04_dynamics/CMakeLists.txt：

# 教程 04：动力学
add_tutorial(01_inverse_dynamics)      # 逆动力学 RNEA、重力项、质量矩阵

第 11、12 章会往这个文件里再加两行。每次只加当前这章的那一行，改完立刻重新 cmake -S . -B build 一次再编译——add_tutorial() 找不到对应的 .cpp 时会在 configure 阶段就报一个很直的错，比攒三行再编译好查得多。

---

10.4 完整代码

存成 tutorials/04_dynamics/01_inverse_dynamics.cpp。整块复制即可编译：

// ============================================================================
// 教程 04-1: 逆动力学 (Inverse Dynamics / RNEA)
// ============================================================================
// 
// 【学习目标】
// 1. 理解逆动力学的物理意义：给定运动 → 求所需力矩
// 2. 会用 rnea() / computeGeneralizedGravity() / nonLinearEffects() / crba()
// 3. 用三个恒等式交叉验证自己算出来的东西是对的
// 4. 搞清 Pinocchio 4.x 的一个大坑：M 和 Minv 都只填了上三角
// 
// 【C++ 知识点】
// - const 引用作为输入参数
// - Eigen 的 selfadjointView / triangularView（矩阵"视图"，不拷贝数据）
// - 特征值分解 SelfAdjointEigenSolver
// 
// 【Pinocchio 知识点】
// - rnea(model, data, q, q_dot, q_ddot)：递归牛顿-欧拉，返回值就是 tau
// - computeGeneralizedGravity(model, data, q) → data.g
// - nonLinearEffects(model, data, q, q_dot)     → data.nle
// - crba(model, data, q)                        → data.M（只有上三角有效）
// - computeMinverse(model, data, q)             → data.Minv（只有上三角有效）
// ============================================================================

#include <iostream>
#include <iomanip>
#include <sstream>
#include <Eigen/Eigenvalues>        // SelfAdjointEigenSolver 在这个头文件里，multibody.hpp 不会帮你带上
#include "pinocchio/multibody.hpp"

#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/crba.hpp"          // crba
#include "pinocchio/algorithm/aba.hpp"           // computeMinverse 在这里
#include "pinocchio/multibody/sample-models.hpp"

using namespace pinocchio;

// 把"应该接近 0"的量用科学计数法打印出来。
// 如果跟着 std::fixed << setprecision(4) 一起打，1e-16 只会显示成 0.0000，
// 读者分不清这是真的精确到 0，还是根本没跑对。
std::string sci(double x) {
    std::ostringstream oss;
    oss << std::scientific << std::setprecision(2) << x;
    return oss.str();
}

// 打印一个向量
void printVector(const std::string& name, const Eigen::VectorXd& v) {
    std::cout << "  " << name << " = [";
    for (int i = 0; i < v.size(); ++i) {
        if (i > 0) std::cout << ", ";
        std::cout << std::fixed << std::setprecision(4) << v[i];
    }
    std::cout << "]" << std::endl;
}

int main() {
    std::cout << "===== 教程 04-1: 逆动力学 =====" << std::endl;
    std::cout << std::endl;
    
    Model model;
    buildModels::manipulator(model, false);
    Data data(model);
    
    // ========================================================================
    // 1. 动力学方程长什么样
    // ========================================================================
    std::cout << "【1. 方程与约定】" << std::endl;
    std::cout << "  tau = M(q) * q_ddot + b(q, q_dot)" << std::endl;
    std::cout << "      = M(q) * q_ddot + C(q, q_dot) * q_dot + G(q)" << std::endl;
    std::cout << std::endl;
    std::cout << "    M(q)   - 惯性矩阵（质量矩阵），nv x nv，对称正定" << std::endl;
    std::cout << "    G(q)   - 重力项，data.g" << std::endl;
    std::cout << "    b(q,qd)- 非线性效应 = 科氏/离心 + 重力，data.nle" << std::endl;
    std::cout << "    tau    - 关节力矩（本模型 " << model.nv << " 维）" << std::endl;
    std::cout << std::endl;
    
    Eigen::VectorXd q = neutral(model);
    Eigen::VectorXd q_dot = Eigen::VectorXd::Zero(model.nv);
    Eigen::VectorXd q_ddot = Eigen::VectorXd::Zero(model.nv);
    q_dot[0] = 1.0;    // 关节1 以 1 rad/s 转动
    q_ddot[0] = 0.5;   // 关节1 以 0.5 rad/s^2 加速
    
    // ========================================================================
    // 2. 静态情形：只克服重力
    // ========================================================================
    
    std::cout << "【2. 重力矩 G(q)】" << std::endl;
    computeGeneralizedGravity(model, data, q);
    Eigen::VectorXd g_neutral = data.g;      // 拷出来，后面 data 会被别的计算覆盖
    printVector("中立姿态 q = [0,0,0,0,0,0] 的重力矩 tau_g", g_neutral);
    std::cout << "  全是 0：这个示例模型中立姿态下手臂竖直朝上，各关节转轴都过质心连线，" << std::endl;
    std::cout << "  重力臂为零。（真实机器人很少有这么「幸运」的姿态。）" << std::endl;
    
    // 换一个姿态再算
    Eigen::VectorXd q2 = neutral(model);
    q2[0] = PI<double>() / 4;
    q2[1] = -PI<double>() / 4;
    computeGeneralizedGravity(model, data, q2);
    Eigen::VectorXd g_q2 = data.g;           // 立刻拷出来：data 里的字段随时会被后续计算覆盖
    printVector("姿态 q2 = [45°, -45°, 0, 0, 0, 0] 的重力矩 tau_g", g_q2);
    std::cout << std::endl;
    
    // 静态 RNEA 应当与 computeGeneralizedGravity 完全一致
    Eigen::VectorXd tau_static = rnea(model, data, q2,
                                      Eigen::VectorXd::Zero(model.nv),
                                      Eigen::VectorXd::Zero(model.nv));
    printVector("同一姿态 rnea(q, 0, 0)", tau_static);
    std::cout << "  两者的差 = " << sci((tau_static - g_q2).norm())
              << "  （computeGeneralizedGravity 就是 rnea(q, 0, 0) 的特例；"
              << "本例实测恰好为 0，位级一致。别把这句话推广成「任何姿态都一定是 0」，"
              << "判断一致性要用阈值）" << std::endl;
    std::cout << std::endl;
    
    // ========================================================================
    // 3. 完整逆动力学 RNEA
    // ========================================================================
    
    std::cout << "【3. RNEA：给定运动求力矩】" << std::endl;
    Eigen::VectorXd tau_full = rnea(model, data, q, q_dot, q_ddot);
    printVector("tau = rnea(q, q_dot, q_ddot)", tau_full);
    std::cout << "  注意 rnea 的返回值就是 tau；4.x 里不要再去找 data.tau（那个字段属于约束系统）。" << std::endl;
    std::cout << std::endl;
    
    // ========================================================================
    // 4. 质量矩阵 M(q)：crba 与"上三角"大坑
    // ========================================================================
    
    std::cout << "【4. 质量矩阵 M(q)】" << std::endl;
    // crba 计算完整质量矩阵，但 Pinocchio 为了省时间只写 data.M 的上三角。
    // 直接把 data.M 当成稠密矩阵用，得到的是一个不对称的假矩阵。
    crba(model, data, q);
    Eigen::MatrixXd M_upper_raw = data.M;                       // 半成品
    Eigen::MatrixXd M = data.M.selfadjointView<Eigen::Upper>(); // 正确用法：镜像上三角
    
    std::cout << "  直接用 data.M:  ||M - M^T|| = "
              << (M_upper_raw - M_upper_raw.transpose()).norm()
              << "  ← 不为 0，说明这是个残缺矩阵" << std::endl;
    std::cout << "  用 selfadjointView<Upper>:  ||M - M^T|| = "
              << (M - M.transpose()).norm()
              << "  ← 这才是真正的 M" << std::endl;
    std::cout << std::endl;
    
    std::cout << "  M(q) = " << std::endl;
    std::cout << std::fixed << std::setprecision(4);
    for (int i = 0; i < M.rows(); ++i) {
        std::cout << "    [";
        for (int j = 0; j < M.cols(); ++j) {
            if (j > 0) std::cout << ", ";
            std::cout << M(i, j);
        }
        std::cout << "]" << std::endl;
    }
    
    // 对称正定是质量矩阵的物理要求，可以直接检查
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eig(M);
    std::cout << "  M 的特征值 = [" << eig.eigenvalues().transpose() << "]" << std::endl;
    std::cout << "  全部为正 => M 正定 => 模型的惯性参数是合法的" << std::endl;
    std::cout << std::endl;
    
    // ========================================================================
    // 5. 恒等式验证：tau = M * q_ddot + b(q, q_dot)
    // ========================================================================
    
    std::cout << "【5. 验证 tau = M*q_ddot + b(q,q_dot)】" << std::endl;
    nonLinearEffects(model, data, q, q_dot);
    Eigen::VectorXd b_term = data.nle;
    Eigen::VectorXd tau_reconstructed = M * q_ddot + b_term;
    
    printVector("rnea 给的 tau", tau_full);
    printVector("M*q_ddot + b 拼出来的 tau", tau_reconstructed);
    std::cout << "  两者之差 = " << sci((tau_full - tau_reconstructed).norm())
              << "  （两条完全不同的算法路径，结果一致才算可信）" << std::endl;
    std::cout << std::endl;
    
    // ========================================================================
    // 6. Minv：同样只存上三角，而且千万别再求逆
    // ========================================================================
    
    std::cout << "【6. 惯性矩阵的逆 Minv】" << std::endl;
    computeMinverse(model, data, q);
    Eigen::MatrixXd Minv = data.Minv.selfadjointView<Eigen::Upper>();
    
    std::cout << "  ||M * Minv - I|| = "
              << sci((M * Minv - Eigen::MatrixXd::Identity(model.nv, model.nv)).norm()) << std::endl;
    
    // 错误写法演示：把只填了上三角的 data.Minv 当成稠密矩阵去 .inverse()
    Eigen::MatrixXd Minv_wrong = data.Minv.inverse();
    std::cout << std::endl;
    std::cout << "  反面教材：data.Minv.inverse()（把残缺矩阵当稠密矩阵求逆）" << std::endl;
    std::cout << "    ||错误结果 - 正确 Minv|| = " << (Minv_wrong - Minv).norm() << std::endl;
    std::cout << "    用它解出来的加速度误差 = "
              << ((Minv_wrong - Minv) * tau_full).norm() << "  （量级完全不可接受）" << std::endl;
    std::cout << "  结论：要 M^(-1) * x，就写 data.Minv.selfadjointView<Upper>() * x，" << std::endl;
    std::cout << "        或者干脆用正动力学的 aba()（见教程 04-2）。" << std::endl;
    std::cout << std::endl;
    
    // ========================================================================
    // 7. 惯性参数
    // ========================================================================
    
    std::cout << "【7. 惯性参数：动力学计算的地基】" << std::endl;
    double total_mass = 0.0;
    std::cout << "  各关节/连杆的惯性参数（示例模型给的是人为设定的小数值）：" << std::endl;
    std::cout << std::fixed << std::setprecision(4);
    for (JointIndex i = 1; i < model.njoints; ++i) {
        const Inertia& I = model.inertias[i];
        total_mass += I.mass();
        std::cout << "    " << model.names[i] << ": mass=" << I.mass()
                  << " kg, 质心 lever=[" << I.lever().transpose() << "]"
                  << ", 转动惯量对角=[" << I.inertia().matrix().diagonal().transpose() << "]" << std::endl;
    }
    std::cout << "  总质量 = " << total_mass << " kg" << std::endl;
    std::cout << "  （model.inertias[0] 对应 universe，永远是零惯性）" << std::endl;
    std::cout << "  Inertia 的三个自由度：质量 mass()、质心相对关节原点的位置 lever()、" << std::endl;
    std::cout << "  绕质心的转动惯量 inertia()。注意 inertia() 返回的是 Symmetric3（对称矩阵的" << std::endl;
    std::cout << "  紧凑存储），要看普通 3x3 得再调 .matrix()。如果是从 CAD 里量出来的规则体，" << std::endl;
    std::cout << "  可以直接用现成的构造函数，不必自己算惯量矩阵：" << std::endl;
    Inertia box = Inertia::FromBox(2.0, 0.1, 0.2, 0.3);   // 质量 2kg，尺寸 0.1x0.2x0.3 m
    std::cout << "    Inertia::FromBox(2.0, 0.1, 0.2, 0.3) 的转动惯量对角 = ["
              << box.inertia().matrix().diagonal().transpose() << "]" << std::endl;
    std::cout << std::endl;
    
    // 演示：把某个连杆的质量放大 10 倍，看重力矩怎么变。
    // 这里要挑 lever 不为零的连杆（3 号 shoulder3_joint，质心在 0.5m 处）；
    // 如果改 1 号 shoulder1_joint，它的质心正好落在关节原点上，重力臂恒为零，
    // 质量再怎么变重力矩都不会动——这种「改了却没反应」的情况在调模型时很常见。
    JointIndex target = 3;
    model.inertias[target].mass() *= 10.0;
    computeGeneralizedGravity(model, data, q2);
    Eigen::VectorXd g_after = data.g;
    std::cout << "  把 " << model.names[target] << "（mass 1.0 -> 10.0 kg）的质量乘 10 后，" << std::endl;
    std::cout << "  q2 姿态的重力矩对比：" << std::endl;
    printVector("    tau_g(改前)", g_q2);
    printVector("    tau_g(改后)", g_after);
    std::cout << "    变化量就是关节 1、2 上的这对反向力矩：" << std::endl;
    printVector("    tau_g(改后) - tau_g(改前)", g_after - g_q2);
    std::cout << "  力矩确实变大了——这说明动力学结果完全由这些惯性参数决定，" << std::endl;
    std::cout << "  真实项目里必须用 CAD 导出或实测的惯量，不能相信默认值。" << std::endl;
    std::cout << std::endl;
    
    std::cout << "教程 04-1 完成！你已经掌握了逆动力学。" << std::endl;
    std::cout << "接下来学习正动力学和质心动力学。" << std::endl;
    
    return 0;
}

---

10.5 编译并运行

cmake -S . -B build -G "Visual Studio 17 2022" -A x64
cmake --build build --config Release
build\bin\01_inverse_dynamics.exe

Git-Bash 里同样两条 cmake，运行改成 ./build/bin/01_inverse_dynamics.exe。

---

10.6 期望输出（实测快照）

Pinocchio 4.1.0 + Eigen 5.0.1 + MSVC v143 实测，退出码 0：

===== 教程 04-1: 逆动力学 =====

【1. 方程与约定】
  tau = M(q) * q_ddot + b(q, q_dot)
      = M(q) * q_ddot + C(q, q_dot) * q_dot + G(q)

    M(q)   - 惯性矩阵（质量矩阵），nv x nv，对称正定
    G(q)   - 重力项，data.g
    b(q,qd)- 非线性效应 = 科氏/离心 + 重力，data.nle
    tau    - 关节力矩（本模型 6 维）

【2. 重力矩 G(q)】
  中立姿态 q = [0,0,0,0,0,0] 的重力矩 tau_g = [0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
  全是 0：这个示例模型中立姿态下手臂竖直朝上，各关节转轴都过质心连线，
  重力臂为零。（真实机器人很少有这么「幸运」的姿态。）
  姿态 q2 = [45°, -45°, 0, 0, 0, 0] 的重力矩 tau_g = [-23.0535, 23.0535, 0.0000, 10.3005, -3.4684, 2.4525]

  同一姿态 rnea(q, 0, 0) = [-23.0535, 23.0535, 0.0000, 10.3005, -3.4684, 2.4525]
  两者的差 = 0.00e+00  （computeGeneralizedGravity 就是 rnea(q, 0, 0) 的特例；本例实测恰好为 0，位级一致。别把这句话推广成「任何姿态都一定是 0」，判断一致性要用阈值）

【3. RNEA：给定运动求力矩】
  tau = rnea(q, q_dot, q_ddot) = [6.0900, 0.0000, 0.0000, 0.0000, 1.1300, 0.0000]
  注意 rnea 的返回值就是 tau；4.x 里不要再去找 data.tau（那个字段属于约束系统）。

【4. 质量矩阵 M(q)】
  直接用 data.M:  ||M - M^T|| = 10.7941  ← 不为 0，说明这是个残缺矩阵
  用 selfadjointView<Upper>:  ||M - M^T|| = 0.0000  ← 这才是真正的 M

  M(q) = 
    [12.1800, 0.0000, 0.0000, 0.0000, 2.2600, 0.0000]
    [0.0000, 12.1700, 0.0000, 6.7100, 0.0000, 2.2500]
    [0.0000, 0.0000, 3.0100, 0.0000, 0.0000, 0.0000]
    [0.0000, 6.7100, 0.0000, 4.6100, 0.0000, 1.7500]
    [2.2600, 0.0000, 0.0000, 0.0000, 1.2600, 0.0000]
    [0.0000, 2.2500, 0.0000, 1.7500, 0.0000, 1.2500]
  M 的特征值 = [ 0.3392  0.8108  1.0793  3.0100 12.6292 16.6115]
  全部为正 => M 正定 => 模型的惯性参数是合法的

【5. 验证 tau = M*q_ddot + b(q,q_dot)】
  rnea 给的 tau = [6.0900, 0.0000, 0.0000, 0.0000, 1.1300, 0.0000]
  M*q_ddot + b 拼出来的 tau = [6.0900, 0.0000, 0.0000, 0.0000, 1.1300, 0.0000]
  两者之差 = 8.88e-16  （两条完全不同的算法路径，结果一致才算可信）

【6. 惯性矩阵的逆 Minv】
  ||M * Minv - I|| = 2.19e-15

  反面教材：data.Minv.inverse()（把残缺矩阵当稠密矩阵求逆）
    ||错误结果 - 正确 Minv|| = 9.3186
    用它解出来的加速度误差 = 50.7022  （量级完全不可接受）
  结论：要 M^(-1) * x，就写 data.Minv.selfadjointView<Upper>() * x，
        或者干脆用正动力学的 aba()（见教程 04-2）。

【7. 惯性参数：动力学计算的地基】
  各关节/连杆的惯性参数（示例模型给的是人为设定的小数值）：
    shoulder1_joint: mass=0.1000 kg, 质心 lever=[0.0000 0.0000 0.0000], 转动惯量对角=[0.0100 0.0100 0.0100]
    shoulder2_joint: mass=0.1000 kg, 质心 lever=[0.0000 0.0000 0.0000], 转动惯量对角=[0.0100 0.0100 0.0100]
    shoulder3_joint: mass=1.0000 kg, 质心 lever=[0.0000 0.0000 0.5000], 转动惯量对角=[1.0000 1.0000 1.0000]
    elbow_joint: mass=1.0000 kg, 质心 lever=[0.0000 0.0000 0.5000], 转动惯量对角=[1.0000 1.0000 1.0000]
    wrist1_joint: mass=0.1000 kg, 质心 lever=[0.0000 0.0000 0.0000], 转动惯量对角=[0.0100 0.0100 0.0100]
    wrist2_joint: mass=1.0000 kg, 质心 lever=[0.0000 0.0000 0.5000], 转动惯量对角=[1.0000 1.0000 1.0000]
  总质量 = 3.3000 kg
  （model.inertias[0] 对应 universe，永远是零惯性）
  Inertia 的三个自由度：质量 mass()、质心相对关节原点的位置 lever()、
  绕质心的转动惯量 inertia()。注意 inertia() 返回的是 Symmetric3（对称矩阵的
  紧凑存储），要看普通 3x3 得再调 .matrix()。如果是从 CAD 里量出来的规则体，
  可以直接用现成的构造函数，不必自己算惯量矩阵：
    Inertia::FromBox(2.0, 0.1, 0.2, 0.3) 的转动惯量对角 = [0.0217 0.0167 0.0083]

  把 shoulder3_joint（mass 1.0 -> 10.0 kg）的质量乘 10 后，
  q2 姿态的重力矩对比：
      tau_g(改前) = [-23.0535, 23.0535, 0.0000, 10.3005, -3.4684, 2.4525]
      tau_g(改后) = [-45.1260, 45.1260, 0.0000, 10.3005, -3.4684, 2.4525]
    变化量就是关节 1、2 上的这对反向力矩：
      tau_g(改后) - tau_g(改前) = [-22.0725, 22.0725, 0.0000, 0.0000, 0.0000, 0.0000]
  力矩确实变大了——这说明动力学结果完全由这些惯性参数决定，
  真实项目里必须用 CAD 导出或实测的惯量，不能相信默认值。

教程 04-1 完成！你已经掌握了逆动力学。
接下来学习正动力学和质心动力学。

---

10.7 逐段读懂这份输出

「2. 重力矩」：中立姿态的 G 恰好全是 0

中立姿态 q = [0,0,0,0,0,0] 的重力矩 tau_g = [0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]

这个 0 是示例模型的几何特殊性给的，不是普遍规律：buildModels::manipulator() 建出来的臂在中立姿态下沿世界 +Z 竖直朝上，每个连杆的质心都落在链条轴线上，重力臂为零。真实机器人几乎不会有这种「免费」的姿态，别把这个 0 当成「重力项没生效」的判据；反过来，如果你从 URDF 加载的机器人中立姿态重力矩全是 0，那大概率是模型装配错了。

换到 q2 = [45°, -45°, 0, 0, 0, 0] 就有值了：

tau_g = [-23.0535, 23.0535, 0.0000, 10.3005, -3.4684, 2.4525]

读一下这个向量的形状：第 1、2 项最大且互为相反数，第 3 项恰好是 0。第 3 关节是 RZ（绕手臂自身轴线转），而手臂的质心分布在这条轴上，重力对它的力臂恒为零——不管手臂怎么摆，这一项都是 0。这不是 bug，是几何。

rnea(q, 0, 0) 与 computeGeneralizedGravity(q) 差 = 0.00e+00

两条独立实现的路径，在静态情形下逐位相同（不是「接近」，是 0）。所以：

computeGeneralizedGravity 就是 rnea 在 v = a = 0 的特例。需要重力补偿时直接调它，语义最清楚，也省掉两个零向量的构造。

但别把这句话推广成「任何对比都一定是 0」。浮点计算里 1e-16 和 0 都显示为 0.0000，本书一律用科学计数法把这类残差单独打出来（代码里的 sci() 函数就是为这个存在的），并且判断一致性用阈值而不是 ==。

「4. 质量矩阵」：本章真正的坑

直接用 data.M:              ||M - M^T|| = 10.7941   ← 不为 0，说明这是个残缺矩阵
用 selfadjointView<Upper>:  ||M - M^T|| = 0.0000    ← 这才是真正的 M

crba 算完，data.M 里下半三角是没被写过的内存。你把 data.M 直接当稠密矩阵用，得到的是一个不对称的假矩阵：它对角线以下要么是全 0（新分配的），要么是上一次调用留下的残渣（data.M 被复用）。

• 对称性检查为什么有效：物理上 M 必须对称（能量守恒的直接推论）。所以 ||M − Mᵀ|| 是免费的正确性哨兵，一定要养成打印它的习惯。
• 正确写法：Eigen::MatrixXd M = data.M.selfadjointView<Eigen::Upper>();
• 更省时间的写法：不要物化，直接参与运算：(data.M.selfadjointView<Eigen::Upper>() * q_ddot)。Eigen 的视图是零成本的。

看 M 本身（中立姿态）：

[12.1800,  0.0000,  0.0000,  0.0000,  2.2600,  0.0000]
[ 0.0000, 12.1700,  0.0000,  6.7100,  0.0000,  2.2500]
[ 0.0000,  0.0000,  3.0100,  0.0000,  0.0000,  0.0000]
[ 0.0000,  6.7100,  0.0000,  4.6100,  0.0000,  1.7500]
[ 2.2600,  0.0000,  0.0000,  0.0000,  1.2600,  0.0000]
[ 0.0000,  2.2500,  0.0000,  1.7500,  0.0000,  1.2500]

四个可以直接读出来的物理事实：

1. 对角线跨了一个数量级：12.18（肩）到 1.25（腕）。总质量只有 3.3 kg，但肩关节要驱动整条手臂，等效惯性自然大。这个数字是第 13 章整定 PD 增益的直接输入——给腕关节和肩关节用同一个 Kp 是错的。
2. 第 3 行/列几乎全是 0（只有对角 3.01）：这就是上面说的 RZ 关节，它和别的关节完全解耦。手臂绕自身轴转，不影响任何其他关节的动量。
3. 非对角元很大：M(2,4) = 6.71、M(1,5) = 2.26。这些耦合项意味着「一个关节的加速度会在另一个关节上感量力矩」，是计算力矩控制必须算 M 而不是只算对角线的原因。
4. 特征值全为正：[0.3392 0.8108 1.0793 3.0100 12.6292 16.6115]。正定是质量矩阵的物理要求（任何方向的加速度都要消耗正的动能）。这是检查你模型惯性参数是否合法最快的一招——出现负特征值，几乎一定是 URDF 里的惯量矩阵或质量填错了。

「5. 恒等式」：8.88e-16

rnea 给的 tau          = [6.0900, 0.0000, 0.0000, 0.0000, 1.1300, 0.0000]
M*q_ddot + b 拼出来的   = [6.0900, 0.0000, 0.0000, 0.0000, 1.1300, 0.0000]
两者之差 = 8.88e-16

tau_full 来自 RNEA，M * q_ddot + b 来自 CRBA + nonLinearEffects——两条完全不同的算法路径。它们在 1e-15 量级上吻合，等于同时验证了三件事：crba 的上三角被你正确还原了、nle 的定义确实是「不含加速度项的那部分」、以及库本身没有版本兼容问题。

顺带读一下 tau 这个向量本身：[6.09, 0, 0, 0, 1.13, 0]。我们只给关节 1 加了速度（1 rad/s）和加速度（0.5 rad/s²），但耦合项让关节 5 也出现了 1.13 的力矩——这就是上面 M(1,5) = 2.26 那个非对角元的物理效果（2.26 × 0.5 = 1.13，可以手算对上）。

「6. 反面教材」：data.Minv.inverse() 能把机器人送进医院

||M * Minv - I||              = 2.19e-15      ← 正确用法
||Minv_wrong - Minv||         = 9.3186        ← 把残缺矩阵当稠密矩阵求逆
用它解出来的加速度误差         = 50.7022 rad/s²

Minv 同样只填上三角。你对它直接 .inverse()，Eigen 会老老实实把这个残缺矩阵当普通矩阵求逆，得到一个「看起来正常」的结果，然后你的正动力学就变成随机数生成器：本来该是几 rad/s² 的加速度，误差 50 rad/s²。

三句话记住这块：

• 想要 M⁻¹ x：写 data.Minv.selfadjointView<Eigen::Upper>() * x。
• 想要 q̈ = M⁻¹(τ − b)：直接用 aba(model, data, q, v, tau)（第 11 章），它内部走三角分解，比显式求逆又快又稳。
• 永远不要对 data.M 或 data.Minv 调 .inverse() / .determinant() / M - diag。

「7. 惯性参数」：动力学计算的地基

表里是示例模型的六组参数，注意三种形态：

关节	mass	lever	对角惯量
shoulder1/2_joint, wrist1_joint	0.1 kg	[0,0,0]	[0.01,0.01,0.01]
shoulder3/elbow_joint, wrist2_body	1.0 kg	[0,0,0.5]	[1,1,1]

这些数是为了让代码跑起来随手设的，不是任何真实机器人的参数（一个 1 kg 的连杆配 1 kg·m² 的转动惯量，等于一根几米长的棒子）。本书所有「加速度大得离谱」「力矩只要几牛米」的现象都源于此，不是库的 bug。

Inertia 只有三个自由度，代码里全用上了：

Inertia box = Inertia::FromBox(2.0, 0.1, 0.2, 0.3);   // 质量、x、y、z 边长

实测输出 转动惯量对角 = [0.0217 0.0167 0.0083]。可以手算验第一条：匀质长方体绕自身中心轴 I_xx = m(y²+z²)/12 = 2(0.04+0.09)/12 = 0.02167 ✓ ——这就是「用现成构造函数，别自己写惯量矩阵」的底气。

还要认一个类型陷阱：I.inertia() 返回的是 Symmetric3（只存 6 个数的紧凑对称存储），不是 3×3 矩阵，所以取对角要写 I.inertia().matrix().diagonal()，中间那个 .matrix() 不能省。

把 shoulder3 的质量乘 10，谁变了？

tau_g(改前) = [-23.0535, 23.0535, 0.0000, 10.3005, -3.4684, 2.4525]
tau_g(改后) = [-45.1260, 45.1260, 0.0000, 10.3005, -3.4684, 2.4525]
变化量      = [-22.0725, 22.0725, 0.0000, 0.0000, 0.0000, 0.0000]

只有肩关节 1、2 变了，肘关节 4 纹丝不动。这正符合受力分析：关节力矩要撑起的是**它自己和它下游（子树）**的质量；上臂（shoulder3 的连杆）挂在肩关节上，肘关节不需要撑它。所以改某个连杆的质量，只有它自己和它的祖先关节的重力矩会变。

顺带解释为什么程序里挑的是 target = 3 而不是 target = 1：

JointIndex target = 3;                    // shoulder3_joint，lever = [0,0,0.5]
model.inertias[target].mass() *= 10.0;

shoulder1_joint 的 lever 是 [0,0,0]——质心正好在关节原点上，重力臂恒为零。你把它的质量乘 10 甚至乘 1000，重力矩都不会动一个字。这种「明明改了却没反应」的情况，在调 URDF 时非常常见，看到它先查 lever，别怀疑库。

---

10.8 本章踩过的坑（按症状查）

坑 1：动力学结果错了，但编译器一声不响

症状：q̈ 或力矩看起来像随机数；或者「换个姿态结果就离谱」。
原因：把只填上三角的 data.M / data.Minv 当稠密对称矩阵用了。
修法：先用一行自检把它抓出来——

std::cout << (data.M - data.M.transpose()).norm() << std::endl;   // 不为 0 就是忘了还原对称
Eigen::MatrixXd M = data.M.selfadjointView<Eigen::Upper>();        // 正确写法

坑 2：error C2039: "selfadjointView": 不是 "Eigen::Matrix<...>" 的成员

原因：这个函数名没错，但某些老写法里是 M.selfadjointView<...>() 作用在 MatrixBase 上——当 data.M 通过 const 引用传入并被 Eigen 判定为「非方阵/表达式」时成员不可见。
修法：确认你在对方阵本身调用（data.M 是 MatrixXs，nv×nv），并且别把视图结果再套一层视图。

坑 3：M - v.asDiagonal() 编译不过：error C2678: 二进制"-"没有找到重载运算符

原因：asDiagonal() 返回的是乘积包装对象（只对 * 有意义），不是矩阵。
修法：先物化成矩阵再减——

Eigen::MatrixXd D = v.asDiagonal().toDenseMatrix();   // 或者
Eigen::MatrixXd M2 = M - v.asDiagonal().toDenseMatrix();

坑 4：SelfAdjointEigenSolver 报「不是模板」或「未声明的标识符」

原因：它不在 Eigen/Dense 里。
修法：#include <Eigen/Eigenvalues>。本章代码第 6 行就是这个。

坑 5：打印 1e-16 的残差时屏幕上是一片 0.0000

原因：std::fixed << std::setprecision(4) 是全局状态，一旦被前面某行设置，后面所有浮点输出都跟着变。
修法：所有需要科学计数法的数字走一个局部的 ostringstream（本章的 sci()），不要去动 std::cout 自己的格式状态。这条坑在第 13 章坑过我一整张表格。

坑 6：computeMinverse 找不到声明

原因：它在正动力学的头文件里，不在 crba.hpp 里。
修法：#include "pinocchio/algorithm/aba.hpp"。本章代码已经带上并加了注释。

---

10.9 练习（都能用本章程序改出来）

1. 把 q2 换成 [0, 90°, 0, 0, 0, 0]，预测 tau_g 的第 1 项是正是零，再跑。
   提示：肩 2 是 RY，转 90° 后手臂水平伸出，重力臂最大。
2. 只打印 M 的对角线，和第 13 章要用它整定增益——先记住这六个数：12.18 12.17 3.01 4.61 1.26 1.25。
3. 故意把 selfadjointView 那行删掉，让程序用残缺的 data.M 去拼 tau，看第 5 节的残差从 8.88e-16 变成多少。
   这是本章最有价值的三十秒：你会亲眼看到「不报错但错得离谱」长什么样。

本章要点

1. τ = M(q)q̈ + b(q,q̇)；rnea 给 τ，crba 给 M，nonLinearEffects 给 b，三者构成一条可以自检的环。
2. crba / computeMinverse 只填上三角。用 selfadjointView<Eigen::Upper>() 还原；对 data.M 做 .inverse() 是本书排名第一的错误用法。
3. 免费的正确性哨兵：||M − Mᵀ|| 应为 0，M 的特征值应全为正，rnea 与 M·q̈+b 的差应在 1e-15 量级。
4. 关节力矩只撑起「自己和子树」的重量；质心 lever 落在转轴上的关节，重力矩恒为 0。
5. 示例模型的惯性参数是人为的，M 对角线跨一个数量级这个事实，正是下一章整定控制增益的根据。

下一章把方向反过来：给了力矩，机器人怎么动？——aba()，以及「为什么用它而不是自己 Minv * tau」。