《Pinocchio入门》第 11 章 正动力学与仿真：`aba`、三档控制器，以及步长这件事
本章解决一个问题：给了力矩，机器人到底怎么动？把第 10 章那条方程反过来解，再用它搭一个能让时间流动起来的仿真循环——然后拿同一个循环、同一组增益，看「纯 PD」「PD + 重力补偿」「计算力矩」三档控制器差多少。
先修：第 00 章（环境）、第 03 章（
Model/Data的分工）、第 10 章（M、b、以及「data.M只填上三角」）。第 09 章不是必需的，但 11.7 里解释M的每个元素时会用到「刚体绕轴转动」那点直觉。
本章新增的 C++ 知识：把「控制律」当参数传（std::function+ lambda 的引用捕获）、定步长循环、std::setw/std::scientific混用时的格式残留。
产出：tutorials/04_dynamics/02_forward_dynamics.cpp——全书第一个让时间在程序里前进的程序。它也是第 13、14 章所有控制的底座。
11.1 把方程反过来解
第 10 章算的是「已知运动，求力矩」。本章把方向反过来：
| 已知 | 求解 | 函数 | |
|---|---|---|---|
| 逆动力学 | q、q̇、q̈ | τ = M q̈ + b | rnea()（第 10 章） |
| 正动力学 | q、q̇、τ | q̈ = M⁻¹(τ − b) | aba()（本章） |
方程只有一个：
M(q) · q̈ = τ − b(q, q̇)
给定力矩，加速度就是这根杠杆的另一头。仿真要的就是这个：每一拍问一次「现在的加速度是多少」，把速度推进一格，把位置推进一格，再问下一次。
为什么不自己写 Minv * (tau - b)？ 三条理由，从硬到软：
M⁻¹也是只填上三角的（第 10 章 10.2 那个坑）。data.Minv存的是上三角，computeMinverse之后直接对它做.inverse()，或者用.triangularView<Upper>()当成完整矩阵去乘，得到的都是残缺矩阵的逆。程序【3】把aba和Minv * (tau − b)两条路逐位对比，差是5.20e-15——这条对比只有在两条路都写对的时候才成立。- 贵。走「求逆」这条路要先
crba（把整个M攒出来，代价约 O(n²)），再求逆（O(n³)），最后乘。aba是 O(n)，它从头到尾没有显式出现过任何逆矩阵。六自由度机器人上这个差别是微秒级、看不出来；自由度上百的人形和带基座的四足就明显了。 - 你只要
q̈，不需要M⁻¹的全部。aba内部的两趟递推（从叶到根攒「铰接体惯性」，再从根到叶把力矩换成加速度）正是为这个问题定制的。这套算法叫 ABA（Articulated Body Algorithm），Featherstone 的经典结果，和 RNEA 是同一枚硬币的两面。
一句话记法：RNEA 是「运动 → 力矩」，ABA 是「力矩 → 运动」；仿真循环里每一次问「接下来怎么动」，都是 aba。
11.2 仿真循环只有四行，但每一行都有名字
整章的技术核心就是这四行（本章程序 simulate() 里的循环体）：
Eigen::VectorXd tau = controller(q, q_dot, t); // 1. 控制器：状态 -> 力矩
Eigen::VectorXd ddq = aba(model, data, q, q_dot, tau); // 2. 正动力学：力矩 -> 加速度
q_dot += ddq * dt; // 3. 推进速度
q = integrate(model, q, q_dot * dt); // 4. 推进位置（不要写 q += q_dot*dt）
四行各自的讲究：
- 第 3、4 行的顺序是有名字的。 先更新速度、再用新速度更新位置，这叫半隐式欧拉（symplectic / semi-implicit Euler）；反过来（先用旧速度推位置，再更新速度）就是显式欧拉。两者都是一阶精度，
dt足够小的时候看不出差别，但在「能量会不会一路漂走」这件事上表现完全不同。11.8 节有一个探针，把同一台倒摆用两种顺序各跑 4 秒，把机械能打印出来对账——实测差 8.8 倍。 - 第 4 行必须是
integrate(model, q, delta)，不能写q += delta。 因为q是位形（configuration），不是一个普通向量：带四元数的关节里，四元数必须待在单位球面上。这个模型的六个关节全是转轴关节，nq == nv == 6，两种写法逐位相同（实测差0.000000e+00），所以这个坑在本章程序里看不见；11.9 坑 1 用带自由基座的模型把它测出来了。 dt同时是仿真步长和控制周期。 这三档控制器每一拍都被调用一次，所以本章把两者当成一个数。真实机器人上这两个周期几乎必然不同（控制 1 kHz、仿真 500 Hz、或者反过来），差别留到第 14 章专门测。tau是从控制器里现算的，控制律里对data的写入会在下一拍被aba覆盖。 所以「待会儿还要用」的量必须当帧拷进局部变量——第 10 章 10.2 的规则 1 在这里第一次真的咬人（11.9 坑 4）。
11.3 工程里加上这一章
第 10 章已经打开了 tutorials/04_dynamics。本章只往这个子目录的 CMakeLists.txt 里加一行——加完之后整份文件长这样（累积式，本章把它完整重抄一遍）：
# 教程 04：动力学
add_tutorial(01_inverse_dynamics) # 逆动力学 RNEA、重力项、质量矩阵
add_tutorial(02_forward_dynamics) # 正动力学 ABA：力矩 -> 运动 + 数值积分仿真
第 12 章会再加一行
add_tutorial(03_centroidal_dynamics)。改完CMakeLists.txt记得重跑一次cmake -S . -B build（第 10 章 10.3 说过：add_tutorial()找不到对应.cpp时会在 configure 阶段就报错，比攒几行再编译好查）。
11.4 完整代码
存成 tutorials/04_dynamics/02_forward_dynamics.cpp。整块复制即可编译：
// ============================================================================
// 教程 04-2: 正动力学 (Forward Dynamics / ABA)
// ============================================================================
//
// 【学习目标】
// 1. 理解正动力学的物理意义：给定力矩 → 求加速度
// 2. 会用 aba() 一步算出关节加速度，不再手工求逆
// 3. 搭起一个最小可用的动力学仿真循环（积分 + 构形更新）
// 4. 对比 PD / PD+重力补偿 / 计算力矩 三种控制器的跟踪效果
//
// 【C++ 知识点】
// - Lambda 表达式与 std::function（把控制律当参数传）
// - 循环 + 数值积分
// - 引用捕获 [=] 与 [&] 的区别
//
// 【Pinocchio 知识点】
// - aba(model, data, q, q_dot, tau)：返回值就是关节加速度 ddq
// - integrate(model, q, delta)：在流形上把 q 推进 delta（四元数关节也能用）
// - nonLinearEffects / computeMinverse 与 aba 的等价关系
// ============================================================================
#include <iostream>
#include <iomanip>
#include <sstream>
#include <functional>
#include <algorithm>
#include "pinocchio/multibody.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/rnea.hpp" // nonLinearEffects
#include "pinocchio/algorithm/crba.hpp" // crba
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp" // integrate
#include "pinocchio/multibody/sample-models.hpp"
using namespace pinocchio;
// 用科学计数法打印"应该接近 0"的量，避免 fixed 格式把 1e-16 显示成 0.0000
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;
}
// ---------------------------------------------------------------------------
// 一次仿真：控制律与误差度量都由 lambda 提供
// ---------------------------------------------------------------------------
struct SimResult {
double final_error; // 最后一步的误差（稳态误差）
double last_exit_time; // 误差最后一次「在死区外」的时刻；在这之后就一直待在死区里
bool settled; // 结束时是否已稳定在死区内
double torque_peak; // 关节力矩峰值
double torque_peak_time; // 峰值出现在哪一拍（用来验证「峰值是不是第一拍」）
int torque_peak_joint;// 峰值出现在哪个关节
};
using TorqueLaw = std::function<Eigen::VectorXd(const Eigen::VectorXd&,
const Eigen::VectorXd&,
double)>;
using Metric = std::function<double(const Eigen::VectorXd&,
const Eigen::VectorXd&)>;
SimResult simulate(const Model& model, Data& data,
const Eigen::VectorXd& q_init,
const Eigen::VectorXd& q_dot_init,
double dt, int steps,
const TorqueLaw& controller,
const Metric& error_of,
double settle_tol) {
Eigen::VectorXd q = q_init;
Eigen::VectorXd q_dot = q_dot_init;
SimResult res{0.0, -1.0, false, 0.0, -1.0, -1};
for (int step = 0; step < steps; ++step) {
double t = step * dt;
// 1. 控制器根据当前状态给指令力矩
Eigen::VectorXd tau = controller(q, q_dot, t);
Eigen::Index jmax = 0;
double tau_max = tau.cwiseAbs().maxCoeff(&jmax);
if (tau_max > res.torque_peak) {
// 记下峰值出现在哪一拍、哪个关节：【7】节要拿它对账「第一拍的解析值」
res.torque_peak = tau_max;
res.torque_peak_time = t;
res.torque_peak_joint = (int)jmax;
}
// 2. 正动力学：tau -> 加速度（Pinocchio 内部用 ABA，O(n)）
Eigen::VectorXd ddq = aba(model, data, q, q_dot, tau);
// 3. 半隐式欧拉积分：先更新速度，再用新速度更新位置
q_dot += ddq * dt;
q = integrate(model, q, q_dot * dt); // 不要写 q += q_dot*dt
// 4. 记录误差与「稳定」判据
double err = error_of(q, q_dot);
res.final_error = err;
if (err >= settle_tol) {
// 还在死区外，稳定时刻只能往后推
res.last_exit_time = t + dt;
}
}
res.settled = (res.final_error < settle_tol);
return res;
}
int main() {
std::cout << "===== 教程 04-2: 正动力学与仿真 =====" << std::endl;
std::cout << std::endl;
Model model;
buildModels::manipulator(model, false);
Data data(model);
// ========================================================================
// 1. 正动力学在解什么方程
// ========================================================================
std::cout << "【1. 方程】" << std::endl;
std::cout << " M(q) * q_ddot = tau - b(q, q_dot)" << std::endl;
std::cout << " => q_ddot = M(q)^{-1} * (tau - b(q, q_dot)) = aba(model, data, q, q_dot, tau)" << std::endl;
std::cout << std::endl;
std::cout << " ABA（Articulated Body Algorithm，Featherstone）和 RNEA 是同一枚硬币的两面：" << std::endl;
std::cout << " RNEA: 运动 -> 力矩（逆动力学，教程 04-1）" << std::endl;
std::cout << " ABA : 力矩 -> 运动（正动力学，本教程）" << std::endl;
std::cout << " 两者都是 O(n)，但 ABA 的常数更大一些。仿真时应当直接调 aba()，" << std::endl;
std::cout << " 不要自己去 M.inverse() * (tau - nle)：既慢又容易踩上三角的坑。" << std::endl;
std::cout << std::endl;
// ========================================================================
// 2. 松手会怎样：纯重力下的自由下落
// ========================================================================
std::cout << "【2. 自由下落（tau = 0）】" << std::endl;
Eigen::VectorXd q_fall = neutral(model);
q_fall[1] = PI<double>() / 2; // 把关节2转 90°，让手臂躺平，重力才有办法矩
Eigen::VectorXd v_zero = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd tau_zero = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd ddq_fall = aba(model, data, q_fall, v_zero, tau_zero);
std::cout << " 姿态 q = [";
for (int i = 0; i < model.nq; ++i) { if (i) std::cout << ", "; std::cout << std::fixed << std::setprecision(3) << q_fall[i]; }
std::cout << "]" << std::endl;
printVector(" 松手瞬间的关节加速度 ddq", ddq_fall);
std::cout << " （中立姿态竖直向上时 ddq 会是全 0——那是个平衡位形；躺平后重力矩不再为零）" << std::endl;
std::cout << std::endl;
// ========================================================================
// 3. 自检：aba 与 Minv 的关系
// ========================================================================
std::cout << "【3. 自检：aba == Minv * (tau - b)？】" << std::endl;
Eigen::VectorXd tau_demo = Eigen::VectorXd::Zero(model.nv);
tau_demo[0] = 2.0; // 关节1 加 2 N·m
nonLinearEffects(model, data, q_fall, v_zero);
Eigen::VectorXd b_q = data.nle;
computeMinverse(model, data, q_fall);
Eigen::VectorXd ddq_manual =
data.Minv.selfadjointView<Eigen::Upper>() * (tau_demo - b_q);
Eigen::VectorXd ddq_aba = aba(model, data, q_fall, v_zero, tau_demo);
printVector(" aba 的结果", ddq_aba);
printVector(" 手工 Minv*(tau-b)", ddq_manual);
std::cout << " 差 = " << sci((ddq_aba - ddq_manual).norm())
<< " （机器精度量级即证明两条路径等价；" << std::endl;
std::cout << " 如果你在这里用了 .triangularView<Upper>() 或直接 .inverse()，这个差会变成 O(1)）" << std::endl;
std::cout << " 顺便注意上面这些加速度大得离谱（100 rad/s^2）：示例模型的肩关节只有" << std::endl;
std::cout << " 0.1 kg、转动惯量 0.01，2 N·m 就能把它甩飞。这是人为设定的惯性参数不真实，" << std::endl;
std::cout << " 不要拿这些数值当物理参考，本教程只关心算法是否自洽。" << std::endl;
std::cout << std::endl;
// ========================================================================
// 4. 恒力矩驱动：观察速度积分出来的运动
// ========================================================================
std::cout << "【4. 恒力矩 1 N·m 驱动关节1（2 秒）】" << std::endl;
double dt = 0.001;
int steps = 2000;
Eigen::VectorXd q0 = neutral(model);
Eigen::VectorXd q_dot0 = Eigen::VectorXd::Zero(model.nv);
{
Eigen::VectorXd q = q0, qd = q_dot0;
std::cout << " 时间(s) | 关节1角度 | 关节1角速度" << std::endl;
std::cout << " " << std::string(40, '-') << std::endl;
for (int step = 0; step <= steps; ++step) {
double t = step * dt;
if (step % 400 == 0) {
std::cout << " " << std::fixed << std::setprecision(3)
<< std::setw(6) << t << " | " << std::setw(10) << q[0]
<< " | " << std::setw(10) << qd[0] << std::endl;
}
if (step == steps) break;
Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv);
tau[0] = 1.0;
Eigen::VectorXd ddq = aba(model, data, q, qd, tau);
qd += ddq * dt;
q = integrate(model, q, qd * dt);
}
}
std::cout << " 关节1角度一路增大：没有反馈、没有重力平衡，恒力矩就是持续的角加速度来源。" << std::endl;
std::cout << std::endl;
// ========================================================================
// 5. 三种控制器对比
// ========================================================================
//
// 目标：把关节1从 0 带到 pi/2，同时其余关节保持 0（中立构形的偏置由 offset 给出）
// 用同一个仿真循环，只换控制律，比较跟踪误差：
// (a) 纯 PD
// (b) PD + 重力补偿（前馈 G(q)）
// (c) 计算力矩法（把 M*q_ddot 也算进前馈，等价于线性化）
// ========================================================================
std::cout << "【5. 三种控制器的跟踪效果】" << std::endl;
std::cout << " 目标位形是个阶跃：t=0 起就把关节1 的目标设成 pi/2，其余关节保持 0。" << std::endl;
std::cout << " 阶跃故意选得很难——它是检验「控制器有没有把重力吃掉」的最直接场景。" << std::endl;
Eigen::VectorXd q_des = neutral(model);
q_des[0] = PI<double>() / 2;
const double Kp = 200.0, Kd = 40.0;
// 误差度量：所有关节里最差的那个偏离目标多少
Metric track = [&](const Eigen::VectorXd& q, const Eigen::VectorXd&) {
return (q_des - q).cwiseAbs().maxCoeff();
};
// 注意：三个控制律都显式写了 -> Eigen::VectorXd，这是第 02 章那条「返回 Eigen 结果
// 不要靠推导」的规矩落地。实测：这里删掉它照样编译通过、输出逐字节相同（塞进
// std::function<Eigen::VectorXd(...)> 时会强制把表达式物化）；但同一个 lambda 如果
// 用 auto 接住，返回的就是引用局部量的表达式模板，Debug 下直接崩（第 02 章 2.7 量过）。
// 写成一种不需要例外的规矩，比记住哪几种情况侥幸没事划算。
// (a) 纯 PD
TorqueLaw pd_law = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& qd, double)
-> Eigen::VectorXd {
return Kp * (q_des - q) - Kd * qd;
};
// (b) PD + 重力补偿：把 G(q) 直接前馈进去，PD 只负责误差动态
TorqueLaw grav_law = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& qd, double)
-> Eigen::VectorXd {
computeGeneralizedGravity(model, data, q); // data.g = G(q)
return Kp * (q_des - q) - Kd * qd + data.g;
};
// (c) 计算力矩（反馈线性化）：tau = M*(q_ddot_des + Kd*e_dot + Kp*e) + b(q, q_dot)
// 目标位形是静止的，所以 q_ddot_des = 0；增益沿用 (a) 的 Kp/Kd，比较才公平。
TorqueLaw ct_law = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& qd, double)
-> Eigen::VectorXd {
Eigen::VectorXd e = q_des - q;
Eigen::VectorXd ed = -qd; // 目标速度为 0
Eigen::VectorXd vref = Kp * e + Kd * ed; // 期望加速度
nonLinearEffects(model, data, q, qd);
Eigen::VectorXd b = data.nle;
crba(model, data, q);
Eigen::MatrixXd M = data.M.selfadjointView<Eigen::Upper>();
return M * vref + b;
};
const double settle_tol = 0.1; // 把「误差落进 ±0.1 rad」当作整定判据
SimResult r_pd = simulate(model, data, q0, q_dot0, dt, steps, pd_law, track, settle_tol);
SimResult r_grav = simulate(model, data, q0, q_dot0, dt, steps, grav_law, track, settle_tol);
SimResult r_ct = simulate(model, data, q0, q_dot0, dt, steps, ct_law, track, settle_tol);
auto report = [&](const char* name, const SimResult& r) {
std::cout << " " << std::left << std::setw(22) << name
<< " 稳态误差=" << std::fixed << std::setprecision(6) << std::setw(10)
<< r.final_error;
if (r.settled) {
std::cout << " 死区±" << std::setprecision(1) << settle_tol << ": 于 "
<< std::setprecision(3) << r.last_exit_time << " s 后稳定";
} else {
std::cout << " 死区±" << std::setprecision(1) << settle_tol << ": 从未稳定";
}
std::cout << " 力矩峰值=" << std::setprecision(1) << r.torque_peak << " N·m"
<< "（关节 " << (r.torque_peak_joint + 1) << "，t="
<< std::setprecision(3) << r.torque_peak_time << " s）" << std::endl;
};
std::cout << " 控制器（Kp=" << std::setprecision(0) << Kp << ", Kd=" << Kd
<< std::setprecision(3) << ", dt=" << dt << " s, 时长 " << steps*dt << " s）:" << std::endl;
report("(a) 纯 PD", r_pd);
report("(b) PD + 重力补偿", r_grav);
report("(c) 计算力矩法", r_ct);
std::cout << std::endl;
std::cout << " 读表要点（一切以程序打印出来的数为准）：" << std::endl;
std::cout << " - 纯 PD 的稳态误差不会归零：停住时 tau 只剩 Kp*e，" << std::endl;
std::cout << " 要顶住重力矩就必须留着一份 e = tau_g / Kp，这就是重力 sag。" << std::endl;
std::cout << " 「从未稳定」不是说它没路过 ±0.1 的死区——它路过过，" << std::endl;
std::cout << " 只是又漂回死区外面去了。路过不等于稳定，这就是为什么要看稳态误差。" << std::endl;
std::cout << " - 把 G(q) 前馈进去以后，稳态误差变成原来的 " << std::setprecision(1)
<< (100.0 * r_grav.final_error / r_pd.final_error) << "%。" << std::endl;
std::cout << " 注意 (a)(b) 的力矩峰值相同：起始姿态是中立的，那一瞬间重力矩恰好为 0，" << std::endl;
std::cout << " 前馈项是随着手臂伸出去才逐渐起作用的。" << std::endl;
std::cout << " - 计算力矩法把 M 和 b 都抵消掉，闭环退化成纯粹的双积分器，" << std::endl;
std::cout << " 稳态误差只剩积分残差；代价是力矩峰值最大（阶跃指令要求瞬时加速度），" << std::endl;
std::cout << " 而且它完全依赖模型精度：模型一旦不准，这套前馈会反过来放大误差。" << std::endl;
std::cout << std::endl;
// ========================================================================
// 6. 步长对精度的影响
// ========================================================================
//
// 关键区分：PD 的残差主要来自重力 sag，跟步长关系不大；
// 计算力矩把模型项都抵消了，残差纯粹来自数值积分，所以对步长敏感。
// 两种控制器放在一起跑，就能看出这两种误差来源互不相干。
// ========================================================================
std::cout << "【6. 数值积分：步长不是随便取的】" << std::endl;
std::cout << " dt(s) | 纯PD 稳态误差 | 计算力矩 稳态误差" << std::endl;
std::cout << " " << std::string(52, '-') << std::endl;
for (double dt_try : {0.01, 0.005, 0.001}) {
int n = static_cast<int>(2.0 / dt_try);
SimResult a = simulate(model, data, q0, q_dot0, dt_try, n, pd_law, track, 0.1);
SimResult c = simulate(model, data, q0, q_dot0, dt_try, n, ct_law, track, 0.1);
std::cout << " " << std::fixed << std::setprecision(3) << std::setw(6) << dt_try
<< " | " << std::setprecision(6) << std::setw(13) << a.final_error
<< " | " << std::setw(15) << c.final_error << std::endl;
}
std::cout << " 纯 PD 那一列几乎不动（那是重力 sag，不是积分误差）；" << std::endl;
std::cout << " 计算力矩那一列随 dt 变小而变小——那才是欧拉积分的截断误差。" << std::endl;
std::cout << " 显式欧拉的能量会漂移；本例用半隐式欧拉，精度足够教学，" << std::endl;
std::cout << " 正式项目请换 RK4 或辛积分器，或者直接用现成的仿真器。" << std::endl;
std::cout << std::endl;
// ========================================================================
// 7. 把【5】的力矩峰值拆开算：第一拍只有 e0 和 M0 两个因子
// ========================================================================
//
// 起始状态是 q0 = 中立、q_dot0 = 0、t = 0，所以三条控制律在第一拍都能手算：
// e0 = q_des - q0 只有关节 1 非零，正好是 pi/2
// b0 = nonLinearEffects(q0, 0) —— 手臂竖直时重力对水平轴没有矩，整列该是 0
// (a) tau = Kp*e0 （Kd*q_dot0 = 0）
// (c) tau = M0*(Kp*e0) + b0 = Kp*(pi/2)*M0 的第一列
// 于是【5】里那两个峰值（314.2 与 3826.5）不再是「程序给的数」，而是能核对的乘积。
// ========================================================================
std::cout << "【7. 第一拍手算：峰值 = Kp*(pi/2) 乘上 M0 的一列】" << std::endl;
{
const Eigen::VectorXd v0 = Eigen::VectorXd::Zero(model.nv);
nonLinearEffects(model, data, q0, v0);
const Eigen::VectorXd b0 = data.nle;
crba(model, data, q0);
const Eigen::MatrixXd M0 = data.M.selfadjointView<Eigen::Upper>();
const Eigen::VectorXd e0 = q_des - q0;
printVector(" e0 = q_des - q0", e0);
printVector(" b0 = nle(q0, 0)", b0);
std::cout << " b0 的模 = " << sci(b0.norm())
<< "（手臂竖直，重力沿转轴方向，不产生绕水平轴的矩——这就是【2】说的平衡位形）" << std::endl;
printVector(" M0 的第一列（关节 1 那一列）", M0.col(0));
Eigen::Index mmax_idx = 0;
const double mmax = M0.col(0).cwiseAbs().maxCoeff(&mmax_idx);
std::cout << " 这一列里绝对值最大的元素 = " << std::fixed << std::setprecision(6)
<< mmax << "，出现在第 " << (mmax_idx + 1) << " 行" << std::endl;
// 把 M0(0,0) 一个刚体一个刚体累加出来。关节 1 是绕世界 x 轴的转副，
// q=0 时六个刚体的质心全都落在 z 轴上，到 x 轴的垂直距离就是 |z_c|，
// 于是每个刚体对 M(0,0) 的贡献 = m*z_c^2 + I_xx（I_xx 取质心处的转动惯量）。
std::cout << " 逐刚体累加 M(0,0)（关节 1 绕世界 x 轴；q=0 时质心都在 z 轴上）：" << std::endl;
std::cout << " " << std::left << std::setw(20) << "刚体"
<< " 质量 质心 z m*z^2 I_xx 小计" << std::endl;
forwardKinematics(model, data, q0);
double acc = 0.0;
for (int i = 1; i < (int)model.njoints; ++i) { // i=0 是世界，质量恒为 0
const Inertia& Y = model.inertias[i];
const Eigen::Vector3d com_world =
data.oMi[i].translation() + data.oMi[i].rotation() * Y.lever();
const double m = Y.mass(), z = com_world.z();
const double parallel = m * z * z;
const double spin = Y.inertia()(0, 0);
acc += parallel + spin;
std::cout << " " << std::left << std::setw(20) << model.names[i]
<< std::right << std::fixed << std::setprecision(3)
<< std::setw(7) << m << std::setw(9) << z
<< std::setw(9) << parallel << std::setw(9) << spin
<< std::setw(9) << (parallel + spin) << std::endl;
}
std::cout << " 累加 = " << std::setprecision(6) << acc
<< "，crba 给的 M(0,0) = " << M0(0, 0)
<< "，差 = " << sci(acc - M0(0, 0)) << std::endl;
const Eigen::VectorXd tau_a0 = pd_law(q0, v0, 0.0);
const Eigen::VectorXd tau_c0 = ct_law(q0, v0, 0.0);
printVector(" 第一拍 (a) 纯 PD", tau_a0);
std::cout << " Kp * pi/2 = " << std::setprecision(4) << Kp * PI<double>() / 2.0
<< " ← 就是【5】里 (a)(b) 的峰值（b 的重力前馈在这一拍恰好是 0，所以两行一样）" << std::endl;
printVector(" 第一拍 (c) 计算力矩", tau_c0);
const Eigen::VectorXd tau_c0_hand = Kp * (PI<double>() / 2.0) * M0.col(0) + b0;
std::cout << " 与手算 Kp*(pi/2)*M0.col(0) + b0 的差 = "
<< sci((tau_c0 - tau_c0_hand).norm()) << std::endl;
std::cout << " 它的最大值 = " << std::setprecision(4) << tau_c0.cwiseAbs().maxCoeff()
<< " ← 对比【5】里 (c) 打印的峰值；峰值时刻也标在【5】那一行的括号里" << std::endl;
}
// aba 与 rnea 是同一台机器的正反两向：把 aba 给的加速度喂回 rnea，应当还原输入的 tau
{
Eigen::VectorXd tau_rt = Eigen::VectorXd::Zero(model.nv);
tau_rt[1] = 1.5;
tau_rt[3] = -0.7;
const Eigen::VectorXd ddq_rt = aba(model, data, q_fall, v_zero, tau_rt);
const Eigen::VectorXd tau_back = rnea(model, data, q_fall, v_zero, ddq_rt);
std::cout << " aba/rnea 往返（在【2】那个躺平姿态、给关节 2 加 1.5、关节 4 加 -0.7）：" << std::endl;
printVector(" 喂进去的 tau", tau_rt);
printVector(" rnea(q, q_dot, aba(...)) 还原出的 tau", tau_back);
std::cout << " 差 = " << sci((tau_back - tau_rt).norm())
<< " ← 两条 O(n) 算法互检，比单看一条更可信" << std::endl;
}
std::cout << std::endl;
std::cout << "教程 04-2 完成！你已经掌握了正动力学与最小仿真循环。" << std::endl;
std::cout << "接下来学习质心动力学。" << std::endl;
return 0;
}
11.5 编译并运行
cmake -S . -B build -G "Visual Studio 17 2022" -A x64
cmake --build build --config Release --target 02_forward_dynamics
build\bin\02_forward_dynamics.exe
Git-Bash 里同样两条 cmake，运行改成 ./build/bin/02_forward_dynamics.exe。
11.6 期望输出（实测快照）
Pinocchio 4.1.0 + Eigen 5.0.1 + MSVC v143 + /utf-8，Release 实测，退出码 0：
===== 教程 04-2: 正动力学与仿真 =====
【1. 方程】
M(q) * q_ddot = tau - b(q, q_dot)
=> q_ddot = M(q)^{-1} * (tau - b(q, q_dot)) = aba(model, data, q, q_dot, tau)
ABA（Articulated Body Algorithm，Featherstone）和 RNEA 是同一枚硬币的两面：
RNEA: 运动 -> 力矩（逆动力学，教程 04-1）
ABA : 力矩 -> 运动（正动力学，本教程）
两者都是 O(n)，但 ABA 的常数更大一些。仿真时应当直接调 aba()，
不要自己去 M.inverse() * (tau - nle)：既慢又容易踩上三角的坑。
【2. 自由下落（tau = 0）】
姿态 q = [0.000, 1.571, 0.000, 0.000, 0.000, 0.000]
松手瞬间的关节加速度 ddq = [0.0000, 6.5002, 0.0000, -4.3550, 0.0000, -1.6794]
（中立姿态竖直向上时 ddq 会是全 0——那是个平衡位形；躺平后重力矩不再为零）
【3. 自检：aba == Minv * (tau - b)？】
aba 的结果 = [100.0000, 6.5002, -100.0000, -4.3550, -0.0000, -1.6794]
手工 Minv*(tau-b) = [100.0000, 6.5002, -100.0000, -4.3550, -0.0000, -1.6794]
差 = 5.20e-15 （机器精度量级即证明两条路径等价；
如果你在这里用了 .triangularView<Upper>() 或直接 .inverse()，这个差会变成 O(1)）
顺便注意上面这些加速度大得离谱（100 rad/s^2）：示例模型的肩关节只有
0.1 kg、转动惯量 0.01，2 N·m 就能把它甩飞。这是人为设定的惯性参数不真实，
不要拿这些数值当物理参考，本教程只关心算法是否自洽。
【4. 恒力矩 1 N·m 驱动关节1（2 秒）】
时间(s) | 关节1角度 | 关节1角速度
----------------------------------------
0.000 | 0.000 | 0.000
0.400 | 0.011 | 0.057
0.800 | 0.052 | 0.167
1.200 | 0.165 | 0.433
1.600 | 0.445 | 1.039
2.000 | 1.066 | 2.187
关节1角度一路增大：没有反馈、没有重力平衡，恒力矩就是持续的角加速度来源。
【5. 三种控制器的跟踪效果】
目标位形是个阶跃：t=0 起就把关节1 的目标设成 pi/2，其余关节保持 0。
阶跃故意选得很难——它是检验「控制器有没有把重力吃掉」的最直接场景。
控制器（Kp=200, Kd=40, dt=0.001 s, 时长 2.000 s）:
(a) 纯 PD 稳态误差=0.165947 死区±0.1: 从未稳定 力矩峰值=314.2 N·m（关节 1，t=0.000 s）
(b) PD + 重力补偿 稳态误差=0.056251 死区±0.1: 于 1.271 s 后稳定 力矩峰值=314.2 N·m（关节 1，t=0.000 s）
(c) 计算力矩法 稳态误差=0.000016 死区±0.1: 于 0.502 s 后稳定 力矩峰值=3826.5 N·m（关节 1，t=0.000 s）
读表要点（一切以程序打印出来的数为准）：
- 纯 PD 的稳态误差不会归零：停住时 tau 只剩 Kp*e，
要顶住重力矩就必须留着一份 e = tau_g / Kp，这就是重力 sag。
「从未稳定」不是说它没路过 ±0.1 的死区——它路过过，
只是又漂回死区外面去了。路过不等于稳定，这就是为什么要看稳态误差。
- 把 G(q) 前馈进去以后，稳态误差变成原来的 33.9%。
注意 (a)(b) 的力矩峰值相同：起始姿态是中立的，那一瞬间重力矩恰好为 0，
前馈项是随着手臂伸出去才逐渐起作用的。
- 计算力矩法把 M 和 b 都抵消掉，闭环退化成纯粹的双积分器，
稳态误差只剩积分残差；代价是力矩峰值最大（阶跃指令要求瞬时加速度），
而且它完全依赖模型精度：模型一旦不准，这套前馈会反过来放大误差。
【6. 数值积分：步长不是随便取的】
dt(s) | 纯PD 稳态误差 | 计算力矩 稳态误差
----------------------------------------------------
0.010 | 0.171650 | 0.000022
0.005 | 0.168487 | 0.000019
0.001 | 0.165947 | 0.000016
纯 PD 那一列几乎不动（那是重力 sag，不是积分误差）；
计算力矩那一列随 dt 变小而变小——那才是欧拉积分的截断误差。
显式欧拉的能量会漂移；本例用半隐式欧拉，精度足够教学，
正式项目请换 RK4 或辛积分器，或者直接用现成的仿真器。
【7. 第一拍手算：峰值 = Kp*(pi/2) 乘上 M0 的一列】
e0 = q_des - q0 = [1.5708, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
b0 = nle(q0, 0) = [0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
b0 的模 = 0.00e+00（手臂竖直，重力沿转轴方向，不产生绕水平轴的矩——这就是【2】说的平衡位形）
M0 的第一列（关节 1 那一列） = [12.1800, 0.0000, 0.0000, 0.0000, 2.2600, 0.0000]
这一列里绝对值最大的元素 = 12.180000，出现在第 1 行
逐刚体累加 M(0,0)（关节 1 绕世界 x 轴；q=0 时质心都在 z 轴上）：
刚体 质量 质心 z m*z^2 I_xx 小计
shoulder1_joint 0.100 0.000 0.000 0.010 0.010
shoulder2_joint 0.100 0.000 0.000 0.010 0.010
shoulder3_joint 1.000 0.500 0.250 1.000 1.250
elbow_joint 1.000 1.500 2.250 1.000 3.250
wrist1_joint 0.100 2.000 0.400 0.010 0.410
wrist2_joint 1.000 2.500 6.250 1.000 7.250
累加 = 12.180000，crba 给的 M(0,0) = 12.180000，差 = -1.78e-15
第一拍 (a) 纯 PD = [314.1593, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
Kp * pi/2 = 314.1593 ← 就是【5】里 (a)(b) 的峰值（b 的重力前馈在这一拍恰好是 0，所以两行一样）
第一拍 (c) 计算力矩 = [3826.4599, 0.0000, 0.0000, 0.0000, 709.9999, 0.0000]
与手算 Kp*(pi/2)*M0.col(0) + b0 的差 = 0.00e+00
它的最大值 = 3826.4599 ← 对比【5】里 (c) 打印的峰值；峰值时刻也标在【5】那一行的括号里
aba/rnea 往返（在【2】那个躺平姿态、给关节 2 加 1.5、关节 4 加 -0.7）：
喂进去的 tau = [0.0000, 1.5000, 0.0000, -0.7000, 0.0000, 0.0000]
rnea(q, q_dot, aba(...)) 还原出的 tau = [0.0000, 1.5000, 0.0000, -0.7000, 0.0000, 0.0000]
差 = 3.92e-15 ← 两条 O(n) 算法互检，比单看一条更可信
教程 04-2 完成！你已经掌握了正动力学与最小仿真循环。
接下来学习质心动力学。
11.7 逐段读懂这份输出
【2】松手：τ = 0 不等于 q̈ = 0
打印出来的自由下落加速度是
ddq = [0.0000, 6.5002, 0.0000, -4.3550, 0.0000, -1.6794]
三个 0 不是「没算出来」，是物理。 本章把关节 2（RY）转到 90°，于是整条手臂从「沿世界 ẑ 立着」变成「沿世界 x̂ 躺着」——所有连杆的质心都落在 x̂ 轴上。重力是 (0,0,-9.81)（model.gravity 的默认值，见 src/multibody/model.hxx:254,292,790），于是每个刚体受到的重力矩
r × (m·g) = (d·x̂) × (−m·g·ẑ) = +d·m·g·ŷ
合力矩的方向恒为 ŷ，只有转轴在 ŷ 上有分量的关节才感受得到。逐个核对第 1、3、5 号关节（这条链是 RX、RY、RZ、RY、RX、RY）：
| 关节 | 转轴在 q₂=90° 时指向 | 与 ŷ 的点积 | 打印的 ddq |
|---|---|---|---|
| 1（RX） | x̂ | 0 | 0.0000 ✓ |
| 2（RY） | ŷ | 1 | 6.5002 |
| 3（RZ） | Ry(90°)·ẑ = x̂ | 0 | 0.0000 ✓ |
| 4（RY） | ŷ | 1 | -4.3550 |
| 5（RX） | Ry(90°)·x̂ = −ẑ | 0 | 0.0000 ✓ |
| 6（RY） | ŷ | 1 | -1.6794 |
三个「该是 0」的位置和程序给出来的 0 一一对上。至于 2、4、6 三个数值为什么有负有正，那是六个自由度耦合在一起的结果，这种符号不需要背，也不需要猜——程序打印的就是答案；你要能判断的是「哪些位置必须是 0」，那才是可以手推的结构性结论。
顺带一句「中立姿态是平衡位形」：程序【7】里 b0 = nle(q0, 0) 打印出来六个 0.0000、模为 0.00e+00（q̇ = 0 时离心/科氏项本来就是 0，而竖直立着的链重力臂为零），和第 10 章【2】里中立姿态 tau_g 全零是同一条事实的两次测量。
【3】aba 与 Minv*(tau-b)：差 5.20e-15，以及那两个 ±100
两行逐位相同、差是机器精度量级——这条等价关系成立的前提是两边都把上三角还原对了。程序里手工那条路写的是 data.Minv.selfadjointView<Eigen::Upper>() * (tau_demo - b_q)；换成 .triangularView<Upper>() 或者 .inverse()，这个差立刻变成 O(1)，而且是「不报错的错」（第 10 章 10.7 已经演过一次）。
看这一行更有意思：
aba 的结果 = [100.0000, 6.5002, -100.0000, -4.3550, -0.0000, -1.6794]
输入只有关节 1 上的 2 N·m，结果关节 1 得 +100、关节 3 得 −100，而【2】里那三个非零数（6.5002、-4.3550、-1.6794）一个都没变。原因：q₂ = 90° 时关节 1 的轴（x̂）和关节 3 的轴（Ry(90°)·ẑ = x̂）重合在同一条线上，而且这两个关节的原点都在世界原点——【7】那张逐刚体表里 shoulder1/2/3 三个质心的 z 分别是 0.000 / 0.000 / 0.500，说明肩三联件的关节原点全在原点（sample-models.hxx:164-165：Marm 是沿局部 ẑ 平移 1，Id4 是不动，肩三段都用 Id4）。
于是这条链在 x̂ 方向上只剩两组刚体在「对拧」，把它们各自绕 x̂ 的惯量加起来（质心都在这条轴上 ⇒ 平移项为零，只剩自转项）：
A = {shoulder1, shoulder2} I_A = 0.01 + 0.01 = 0.02
B = {shoulder3, elbow, wrist1, wrist2} I_B = 1.0 + 1.0 + 0.01 + 1.0 = 3.01
关节 1 的角速度带着 A 转，关节 3 的角速度在 A 的基础上再叠加给 B，所以这一对自由度上的动能是
T = ½ I_A θ̇₁² + ½ I_B (θ̇₁ + θ̇₃)²
对应的 2×2 质量矩阵就是 [[I_A+I_B, I_B], [I_B, I_B]]。现在只有关节 1 有输入力矩、关节 3 是自由的（τ₃ = 0）：
第二行：I_B(α₁ + α₃) = 0 ⇒ α₃ = −α₁ ← B 的净角加速度为零，下游跟着它一起「不被拧」
第一行：(I_A+I_B)α₁ + I_B α₃ = 2 ⇒ I_A α₁ = 2 ⇒ α₁ = 2 / 0.02 = 100
+100 / −100 就是这么来的：2 N·m 全部花在 A 这两节壳层的对拧上，0.02 kg·m² 就是它唯一「撬得动」的惯量；B 被自由铰链保持静止。「一个自由度被两个关节重复表达」的账单，就体现为这一对反向数字。 顺便，I_B = 3.01 不是新数字——它就是第 10 章【4】打印的 M(2,2) = 3.0100（同一条轴、同一批刚体，见下面 11.10 练习 2 的说明）。
至于 100 rad/s² 这个离谱的数字：示例模型的肩关节只有 0.1 kg、转动惯量 0.01（见【7】那张表），2 N·m 就能把它甩飞。这是人为设定的惯性参数，不是物理参考值，本章只关心算法自不自洽。
再回头看那条「一个数都没变」：6.5002 / -4.3550 / -1.6794 在两行里逐位相同，说明在这个姿态下共轴那一对自由度是单独算的，不与其他三个互相牵连。原因是 A、B 两块的质心都落在这条 x̂ 轴上，绕它转不移动任何质心，也就改变不了任何关节的重力臂。这不是通用定理，是这个姿态给的巧合：把 q_fall[1] 从 PI<double>()/2 改成 PI<double>()/3（60°）重跑【2】和【3】，关节 3 的轴变成 (sin60°, 0, cos60°)，与关节 1 不再共线——预测：那三个数就不会再逐位相同，2 N·m 会洒到别的关节上去。这是本章最值得自己动手的三十秒，因为「结构性为零」这种判断只有在你能亲手打破它的时候才算真的懂了。
还有个细节：这一行第 5 个位置打印的是 -0.0000，而【2】那行是 0.0000。不是 bug，是负零被格式化出来了（第 09 章 9.16【7】同款），fixed 六位把它印成看得见的符号。
【4】恒力矩：q̈ 真的不是常数
表格打印的是关节 1 在恒定 1 N·m 下的角度和角速度：
0.400 | 0.011 | 0.057
0.800 | 0.052 | 0.167
1.200 | 0.165 | 0.433
1.600 | 0.445 | 1.039
2.000 | 1.066 | 2.187
拿相邻两行的角速度差除以 0.4 s，得到各段的平均角加速度：
| 区间(s) | Δq̇ | 平均 q̈(rad/s²) |
|---|---|---|
| 0 → 0.4 | 0.057 | 0.143 |
| 0.4 → 0.8 | 0.110 | 0.275 |
| 0.8 → 1.2 | 0.266 | 0.665 |
| 1.2 → 1.6 | 0.606 | 1.515 |
| 1.6 → 2.0 | 1.148 | 2.870 |
力矩恒定，加速度却一路涨到 20 倍。 这正是 q̈ = M(q)⁻¹(τ − b(q,q̇)) 的字面含义：τ 不变，q 一直在变，于是 M 和 b 都在变。
第一行确实接近匀加速（½ × 0.143 × 0.4² = 0.011，和打印的 0.011 一样），因为那一瞬间手臂还在中立位形、b = 0。后面几段为什么涨，可以用一条手推的公式估：只有关节 1 在动时，链绕世界 x̂ 倾倒，第 11.7【5】节会用到它
τ_g(q₁) = g · Σ mᵢdᵢ · sin(q₁) = 9.81 × 4.7 × sin(q₁) = 46.107 · sin(q₁) N·m
t = 2.0 s 那一拍打印的 q₁ = 1.066 rad，sin = 0.875，代入得 约 40 N·m——是那 1 N·m 恒指令的四十倍。拿它除以表中最后一段的平均加速度 2.87，得到「等效惯量」约 14，和【7】里中立位形的 M(0,0) = 12.18 是同一个量级。这条是推算，不是实测（姿态一变，M 和离心项也都在变，别指望两边相除正好相等），但它说明涨到 20 倍不是积分器在捣乱，而是重力接手了。
【5】三档控制器：一张表看清「重力是谁吃掉的」
增益 Kp=200, Kd=40，dt=0.001 s，跑 2 s，目标是从中立姿态直接把关节 1 阶跃到 π/2：
(a) 纯 PD 稳态误差=0.165947 死区±0.1: 从未稳定 力矩峰值=314.2 N·m（关节 1，t=0.000 s）
(b) PD + 重力补偿 稳态误差=0.056251 死区±0.1: 于 1.271 s 后稳定 力矩峰值=314.2 N·m（关节 1，t=0.000 s）
(c) 计算力矩法 稳态误差=0.000016 死区±0.1: 于 0.502 s 后稳定 力矩峰值=3826.5 N·m（关节 1，t=0.000 s）
先看误差。
-
(a) 的
0.165947就是重力 sag。停住时q̇ = 0，PD 只剩Kp·e，要顶住重力矩τ_g就必须留着e = τ_g / Kp。用手能估到同一个量级：b0 = 0说明这条链的质心全在z轴上，把【7】表里「质量 × 质心 z」乘起来是Σ mᵢdᵢ = 1.0×0.5 + 1.0×1.5 + 0.1×2.0 + 1.0×2.5 = 4.7 kg·m τ_g(躺平) = g × Σ mᵢdᵢ = 9.81 × 4.7 = 46.107 N·m e ≈ 46.107 / Kp = 46.107 / 200 = 0.2305 rad程序给
0.165947——同一量级，差在这只是一个估算：末拍手臂并没有停在π/2，其余关节也跟着下垂，力臂和不再是 4.7；而且 (a) 那一行「从未稳定」，末拍还在漂，q̇ ≠ 0时Kd·q̇也进了平衡式。这条估算的价值不在精度，而在它告诉你数量级是τ_g/Kp，以及为什么把Kp从 200 提到 800 只能把 sag 压到四分之一（而力矩峰值会变成 1256.6 N·m，见 11.9 坑 2）。 -
(b) 把
G(q)前馈进去以后，PD 不再需要「留一份误差来产生重力矩」，误差只剩动态残差。程序里那个百分比是自己算自己打印的：100 × 0.056251 / 0.165947 = 33.9%。 -
© 的
0.000016已经是积分残差了（模型项被前馈完全抵消，闭环退化成双积分器），比 (b) 小三个数量级。
再看力矩峰值——这是这张表最容易被忽略的一列。 括号里现在标了峰值出现的关节和时刻：三行全是「关节 1，t=0.000 s」，也就是说峰值都在第一拍。于是它俩能手算（这就是【7】那一节存在的原因）：
(a)(b) 峰值 = Kp × e₀ = 200 × π/2 = 314.1593 N·m ← 程序打印 314.1593
(c) 峰值 = Kp × (π/2) × M₀ 的第 1 个元素 = 314.1593 × 12.18 = 3826.46 N·m ← 程序打印 3826.4599
(a)(b) 的峰值为什么一样？因为起始姿态是平衡位形，那一拍 b0 = 0，重力前馈是零——前馈项要等手臂伸出去才逐渐起作用（这也是为什么两行的峰值都是 314.2，而不是 (b) 更大）。
© 的峰值最大，是因为阶跃指令要求第一拍就要产生 200 × π/2 的加速度，而把整个手臂（不只是关节 1 自己）加速那么多需要 M₀ 那一列。阶跃 + 计算力矩 = 力矩尖峰，这条不是 bug，是这套控制律的本性：它假设你给得起这个力矩。第 13 章换成有限加速度、可微的轨迹参考之后，这一列会掉下好几个数量级。
【6】步长：两列在说两种不同的误差
dt(s) | 纯PD 稳态误差 | 计算力矩 稳态误差
0.010 | 0.171650 | 0.000022
0.005 | 0.168487 | 0.000019
0.001 | 0.165947 | 0.000016
两列的性质不同：
- 左列从
0.171650到0.165947，只降 3.3%，而且它是「重力 sag」这个物理量，不是数值误差。dt减小到 0，它也不会归零——它收敛到τ_g/Kp那个平衡值。 - 右列从
0.000022到0.000016，随dt变小而变小，这是截断误差：模型项被前馈抵消之后，剩下的就只有积分器欠你的那部分。
但别拿这张表拟合收敛阶数，这是本章关于数值的一个诚实提醒：std::setprecision(6) 的打印让右列只剩下两位有效数字——末位的量化步进是 1e-6，占 1.6e-5 的 6%。想研究阶数，先把精度提到 10 位、时长固定，并且换一个不归零的度量（例如整条轨迹的积分平方误差）。第 10 章坑 5 说的「打印格式会吃掉你要看的数字」在这里换了个花样重现：上次是把 1e-16 印成 0.0000，这次是把 2.2e-5 印成只有两位有效数字。
顺便，程序【1】说「显式欧拉的能量会漂移；本例用半隐式欧拉」——这句话在 11.8 有实测数据撑着，测出来比想象得更直观。
【7】第一拍拆解：12.18 是程序自己加出来的
这一节把【5】的峰值从「程序给的数」变成「你能核对的乘积」。它先打印第一拍上三个因子的值：
e0 = q_des - q0 = [1.5708, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
b0 = nle(q0, 0) = [0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000]
M0 的第一列（关节 1 那一列） = [12.1800, 0.0000, 0.0000, 0.0000, 2.2600, 0.0000]
然后逐刚体把 M(0,0) 累加出来，这是本章最好用的一张表：
刚体 质量 质心 z m*z^2 I_xx 小计
shoulder1_joint 0.100 0.000 0.000 0.010 0.010
shoulder2_joint 0.100 0.000 0.000 0.010 0.010
shoulder3_joint 1.000 0.500 0.250 1.000 1.250
elbow_joint 1.000 1.500 2.250 1.000 3.250
wrist1_joint 0.100 2.000 0.400 0.010 0.410
wrist2_joint 1.000 2.500 6.250 1.000 7.250
累加 = 12.180000，crba 给的 M(0,0) = 12.180000，差 = -1.78e-15
用的公式就是「转动惯量对一条轴累加」：关节 1 绕世界 x̂ 转，q = 0 时六个刚体的质心全都落在 z 轴上，到 x̂ 轴的垂直距离就是 |z|，于是每个刚体贡献
m·z² + I_xx(绕质心)
第一项是「把这块质量搬到距离 z 处」的平移项（平行轴定理），第二项是它自己绕质心转的项。0.01 + 0.01 + 1.25 + 3.25 + 0.41 + 7.25 = 12.18，crba 给出来的也是 12.180000，差 -1.78e-15（这是累加顺序带来的舍入，不是算法分歧）。
同一条公式还能把这一列的第二个非零数 2.26 也算出来。 M(0,4) 是关节 1 与关节 5 的耦合项，两条轴都是 x̂，但分别在 z = 0 和 z = 2 上；只有同时挂在这两个关节下游的刚体才同时被两条轴撬动，也就是 wrist1 和 wrist2：
wrist1：质心恰好在关节 5 的原点上 → 对轴 5 的杠杆为零 → 只出自转项 0.01
wrist2：到轴 1 的距离 2.5、到轴 5 的距离 0.5 → 1.0×2.5×0.5 = 1.25，再加自转 1.0 → 2.25
合计 = 0.01 + 2.25 = 2.26 ✓
再拿第 10 章【4】打印的完整 M 对答案（那份输出里中立位形的质量矩阵是逐元素印出来的）：M(0,0)=12.18、M(0,4)=2.26，和这里一字节不差。顺手练两个：
M(2,2) = 3.01：关节 3 是 RZ，中立姿态下六个质心全在这条轴上，平移项一律为零，只剩下自转项1.0 + 1.0 + 0.01 + 1.0 = 3.01（关节 3 及其下游）。这就是「转轴穿过所有质心 ⇒ 这一项只是惯量之和」。M(4,4) = 1.26：轴在z=2上，所以只有下游两块，且距离要从关节 5 的原点量——wrist1距离 0（只出0.01），wrist2距离0.5（1.0×0.5² + 1.0 = 1.25），合计1.26✓。距离量错原点，是这个练习最常见的错法。
有了这张表，两个峰值就不再是神秘数字：
第一拍 (a) 纯 PD = [314.1593, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000] = 200 × π/2
第一拍 (c) 计算力矩 = [3826.4599, 0.0000, 0.0000, 0.0000, 709.9999, 0.0000] = 314.1593 × M0.col(0)
与手算 Kp*(pi/2)*M0.col(0) + b0 的差 = 0.00e+00
314.1593 × 12.18 = 3826.4599、314.1593 × 2.26 = 709.9999——打印出来的每一个都能拿计算器按出来（最后一位差在 π 的舍入）。
最后那段 aba/rnea 往返是把两条 O(n) 算法互扣：给关节 2 加 1.5、关节 4 加 −0.7，用 aba 换成加速度，再把这个加速度喂回 rnea 还原出力矩，差 3.92e-15。单向的恒等式只证明代码跑通了，双向的往返才证明两条路是同一条路。
11.8 一个必须亲手撞一次的实验：两种欧拉的能量对账
正文里那句「显式欧拉的能量会漂移，本例用半隐式」是抄来的道理。这一节把它测出来。
探针做的事只有一件：同一台机器人、同一个初始姿态、同一个 τ = 0，把循环里第 3、4 行的顺序交换两次，每 0.8 秒打印一次机械能。机械能 = 动能 + 重力势能：
forwardKinematics(model, data, q, v);
double E = computeKineticEnergy(model, data) + computePotentialEnergy(model, data, q);
（computeKineticEnergy 读的是 data 里已经算好的空间速度，所以调用前必须先 forwardKinematics。）
存成 tutorials/04_dynamics/99_probe_energy.cpp：
// 探针：同一个自由摆动，两种积分顺序的能量对账
// 编译：tutorials/04_dynamics/CMakeLists.txt 里临时加一行 add_tutorial(99_probe_energy)
// cmake -S . -B build && cmake --build build --config Release --target 99_probe_energy
#include <iostream>
#include <iomanip>
#include "pinocchio/multibody.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/kinematics.hpp" // forwardKinematics
#include "pinocchio/algorithm/energy.hpp" // computeKineticEnergy / computePotentialEnergy
#include "pinocchio/algorithm/joint-configuration.hpp" // integrate
#include "pinocchio/multibody/sample-models.hpp"
using namespace pinocchio;
// 机械能 = 动能 + 重力势能。kinetic 那一对函数读的是 data 里已经算好的空间速度，
// 所以调用前必须先 forwardKinematics(model, data, q, v)。
double energy(const Model& model, Data& data,
const Eigen::VectorXd& q, const Eigen::VectorXd& v) {
forwardKinematics(model, data, q, v);
return computeKineticEnergy(model, data) + computePotentialEnergy(model, data, q);
}
int main() {
Model model;
buildModels::manipulator(model, false);
Data data(model);
const double dt = 0.001;
const int steps = 4000; // 4 秒
Eigen::VectorXd q_final[2];
std::cout << std::fixed << std::setprecision(6);
std::cout << " 松手后自由摆动（tau = 0，无摩擦，机械能应当守恒）" << std::endl;
for (int order = 0; order < 2; ++order) {
Eigen::VectorXd q = neutral(model);
q[1] = PI<double>() / 3; // 倾斜 60 度：质心还在 z>0，初值能量非零
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd tau = Eigen::VectorXd::Zero(model.nv);
const double e0 = energy(model, data, q, v);
std::cout << (order == 0 ? " 显式欧拉：" : " 半隐式 ：")
<< " E(0) = " << std::setprecision(6) << e0 << " J" << std::endl;
for (int step = 1; step <= steps; ++step) {
const Eigen::VectorXd ddq = aba(model, data, q, v, tau);
if (order == 0) { // 显式：用「旧速度」推进位置，然后再改速度
q = integrate(model, q, v * dt);
v += ddq * dt;
} else { // 半隐式：先把速度推进，再用「新速度」推位置
v += ddq * dt;
q = integrate(model, q, v * dt);
}
if (step % 800 == 0 || step == steps) {
const double e = energy(model, data, q, v);
std::cout << " t=" << std::setw(5) << step * dt
<< " s E = " << std::setw(12) << e
<< " dE = " << std::setw(11) << (e - e0)
<< " J 相对漂移 = " << std::scientific << std::setprecision(3)
<< (e - e0) / std::abs(e0) << std::fixed << std::setprecision(6) << std::endl;
}
}
q_final[order] = q;
}
std::cout << " 两种顺序跑完 4 秒后的姿态差 = ";
std::cout << std::scientific << (q_final[1] - q_final[0]).cwiseAbs().maxCoeff() << std::endl;
return 0;
}
在 tutorials/04_dynamics/CMakeLists.txt 里加一行 add_tutorial(99_probe_energy)，然后：
cmake -S . -B build
cmake --build build --config Release --target 99_probe_energy
build\bin\99_probe_energy.exe
实测输出（Release，退出码 0）：
松手后自由摆动（tau = 0，无摩擦，机械能应当守恒）
显式欧拉： E(0) = 23.053500 J
t=0.800000 s E = 23.150222 dE = 0.096722 J 相对漂移 = 4.196e-03
t=1.600000 s E = 23.291660 dE = 0.238160 J 相对漂移 = 1.033e-02
t=2.400000 s E = 23.361202 dE = 0.307702 J 相对漂移 = 1.335e-02
t=3.200000 s E = 23.452553 dE = 0.399053 J 相对漂移 = 1.731e-02
t=4.000000 s E = 23.639385 dE = 0.585885 J 相对漂移 = 2.541e-02
半隐式 ： E(0) = 23.053500 J
t=0.800000 s E = 23.038767 dE = -0.014733 J 相对漂移 = -6.391e-04
t=1.600000 s E = 23.235438 dE = 0.181938 J 相对漂移 = 7.892e-03
t=2.400000 s E = 23.132035 dE = 0.078535 J 相对漂移 = 3.407e-03
t=3.200000 s E = 23.122561 dE = 0.069061 J 相对漂移 = 2.996e-03
t=4.000000 s E = 23.120382 dE = 0.066882 J 相对漂移 = 2.901e-03
两种顺序跑完 4 秒后的姿态差 = 3.408638e-01
读这份输出，三件事：
-
E(0) = 23.053500 J可以先手算验证。 探针把关节 2 转到 60°（不是 90°，理由见下），链还是直的，每个质心的高度变成dᵢ·cos60° = 0.5 dᵢ，于是E(0) = U = g · Σ mᵢ dᵢ · cos60° = 9.81 × 4.7 × 0.5 = 23.0535 JΣ mᵢdᵢ = 4.7来自【7】那张表的质量列和质心 z 列。如果初始能量恰好是 0，「相对漂移」这一列就变成除以一个零：探针最初版本用的是【2】的 90° 躺平姿态，那时所有质心都在z = 0平面上，E(0)打印出来是0.000000 J（六位fixed之下只能看见零，实际是2.8e-15量级的浮点残渣），于是同一份代码打印的「相对漂移」变成3.807e+13、1.626e+14这种荒唐数（完整记录在_out/experiments/probe_energy_drift_90deg.txt）。改成 60° 才有非零分母。这条本身就是个好提醒：任何「相对量」都要先确认分母不是零，而0.000000打印出来不代表它就是 0。 -
显式欧拉那一列只涨不落：
+0.0967 → +0.2382 → +0.3077 → +0.3991 → +0.5859 J，五步全正，单调增加，4 秒攒到初值的 2.5%。这就是「数值注入的能量」的典型形状——系统里没有摩擦，物理上没有任何东西能给它这么多能量，是积分器自己发的。半隐式那一列有负有正（−0.0147起步，中间+0.1819，收尾+0.0669），在初值附近来回摆，末拍比显式小 8.8 倍。 -
半隐式不是「守恒」，只是「不一路漂」。 别把结论说过头：
dE = 0.1819 J也不算小，六个自由度的耦合摆动本来就让「能量」和「周期」都不像一个单摆。真正的结论是最后那一行：同样的 4 秒，两种顺序跑完的姿态已经差了0.34 rad——比整个「把关节 1 从 0 带到 π/2」的目标的三分之一还多。步长和积分顺序不是精度上的装饰品，它们决定你仿真出来的是不是同一台机器。
正式版怎么办：本章这个「四行循环」是教学骨架。要长期跑，用现成的积分器（RK4、或者专门为力学系统设计的辛积分器），或者直接用带接触/碰撞处理的仿真器；dt 至少要跟着关节的最高固有频率走，不是随手取 0.001。
跑完记得把 add_tutorial(99_probe_energy) 那行删掉，回到 11.3 那份 CMakeLists.txt。
11.9 本章踩过的坑（按症状查）
坑 1：位置写成 q += q_dot*dt，四元数被越推越长
症状（两种，都会遇到）：
- 仿真跑着跑着，末端位置开始慢慢漂，但你查不到哪一步算错——因为每一步「看着都对」；
- 或者直接换一个模型就崩：
nq和nv不相等时，q + q_dot是尺寸不匹配的二元运算，Eigen 在 Debug 当场断言、Release 静默截断（固定/动态混算的完整机制见第 09 章坑 7）。
为什么本章程序里看不见：buildModels::manipulator 的六个关节全是转轴，nq == nv == 6，位形空间就是普通的 R⁶，integrate 退化成加法。探针第一件事就是把这个「侥幸」测出来：
manipulator: nq=6 nv=6
integrate(model,q,d) 与 q+d 的逐位最大差 = 0.000000e+00
一换成带自由基座的模型就露馅（第 05 章实测过 Humanoid(usingFF=true) 是 nq=35, nv=34；多出来的那一维就是四元数的第四个分量）：
humanoid(usingFF=true): nq=35 nv=34
关节 1 = JointModelFreeFlyer, 占 q 的 [0, 7), 占 v 的 [0, 6)
中立位形的四元数 (x,y,z,w) = 0.000000000000 0.000000000000 0.000000000000 1.000000000000 模 = 1.000000000000
integrate 后的四元数 = 0.000000000000 0.000000000000 0.049979169271 0.998750260395 模 = 1.000000000000
直接相加后的四元数 = 0.000000000000 0.000000000000 0.100000000000 1.000000000000 模 = 1.004987562112
sin(0.05) = 0.049979169271, cos(0.05) = 0.998750260395
给基座一个「绕 ẑ 转 0.1 rad」的增量，v 的后三个分量是 (0, 0, 0.1)。两种写法的差别一目了然：
integrate得到(0, 0, 0.049979169271, 0.998750260395)，模 1.000000000000，而且这两个数就是sin(0.05)和cos(0.05)——0.1 rad 的旋转要写成半角的四元数。程序把sin(0.05)/cos(0.05)单独打印出来，就是为了让你能一眼认出这个半角。- 直接相加得到
(0, 0, 0.1, 1)，模 1.004987562112（=√1.01）。角度错了，长度也错了，而且每一步都在错得更多。
修法：位形永远用 integrate(model, q, delta) 推进；反过来「两个位形相减」要用 difference(model, q1, q2)，不能写 q2 - q1（后者在四元数上同样不是你以为的东西）。不要用「算完再归一化」当补丁——归一化只修长度，修不了角度那半份错。
探针代码（存成 tutorials/04_dynamics/99_probe_integrate.cpp，加一行 add_tutorial(99_probe_integrate) 编译运行，跑完删掉那行）：
// 探针：为什么积分必须写 integrate(model, q, delta)，不能写 q += delta
// 编译：在 tutorials/04_dynamics/CMakeLists.txt 里临时加一行
// add_tutorial(99_probe_integrate)
// 然后 cmake -S . -B build && cmake --build build --config Release --target 99_probe_integrate
#include <iostream>
#include <iomanip>
#include <cmath>
#include "pinocchio/multibody.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp" // integrate
#include "pinocchio/multibody/sample-models.hpp"
using namespace pinocchio;
int main() {
// 1) 本章示例模型：nq == nv，两种写法应当逐位相同
Model manip;
buildModels::manipulator(manip, false);
Eigen::VectorXd q = neutral(manip);
Eigen::VectorXd d(manip.nv);
d << 0.05, -0.02, 0.03, 0.01, 0.0, -0.04;
const Eigen::VectorXd qi = integrate(manip, q, d);
const Eigen::VectorXd qp = q + d;
std::cout << "manipulator: nq=" << manip.nq << " nv=" << manip.nv << std::endl;
std::cout << " integrate(model,q,d) 与 q+d 的逐位最大差 = "
<< std::scientific << (qi - qp).cwiseAbs().maxCoeff() << std::endl;
// 2) 带 FreeFlyer 的模型：nq = nv + 1，多出来的那一维是四元数的第四个分量
Model ff;
buildModels::humanoid(ff, true);
std::cout << "humanoid(usingFF=true): nq=" << ff.nq << " nv=" << ff.nv << std::endl;
std::cout << " 关节 1 = " << ff.joints[1].shortname()
<< ", 占 q 的 [" << ff.idx_qs[1] << ", " << ff.idx_qs[1] + ff.nqs[1] << ")"
<< ", 占 v 的 [" << ff.idx_vs[1] << ", " << ff.idx_vs[1] + ff.nvs[1] << ")" << std::endl;
const Eigen::VectorXd qf = neutral(ff);
const int iq = ff.idx_qs[1];
const int iv = ff.idx_vs[1];
std::cout << std::fixed << std::setprecision(12);
std::cout << " 中立位形的四元数 (x,y,z,w) = "
<< qf.segment(iq + 3, 4).transpose()
<< " 模 = " << qf.segment(iq + 3, 4).norm() << std::endl;
// 给基座一个「绕世界 z 轴转 0.1 rad」的增量：v 的前 3 维是平移，后 3 维是角度增量
Eigen::VectorXd df = Eigen::VectorXd::Zero(ff.nv);
df.segment(iv + 3, 3) << 0.0, 0.0, 0.1;
const Eigen::VectorXd qi_ff = integrate(ff, qf, df);
Eigen::VectorXd qn_ff = qf;
qn_ff.segment(iv, 6) += df; // 新手最爱写的那一句（这里按前 6 维对齐，避免尺寸报错干扰观察）
std::cout << " integrate 后的四元数 = "
<< qi_ff.segment(iq + 3, 4).transpose()
<< " 模 = " << qi_ff.segment(iq + 3, 4).norm() << std::endl;
std::cout << " 直接相加后的四元数 = "
<< qn_ff.segment(iq + 3, 4).transpose()
<< " 模 = " << qn_ff.segment(iq + 3, 4).norm() << std::endl;
std::cout << " sin(0.05) = " << std::sin(0.05)
<< ", cos(0.05) = " << std::cos(0.05) << std::endl;
return 0;
}
坑 2：力矩峰值超上限几十倍，aba 一声不响
症状：仿真跑得很好，轨迹漂亮，dt 也够小。把力矩峰值和 URDF / 示例模型里的力矩上限放在一起看：
示例模型的力矩上限就是 10 N·m——src/multibody/sample-models.hxx:171 写着 static const Scalar vmax = 10., taumax = 10.;，:232-233 把它填进 lowerEffortLimit / upperEffortLimit。而【5】打印的峰值：
(a) 314.2 N·m → 31 倍上限
(b) 314.2 N·m → 31 倍
(c) 3826.5 N·m → 383 倍
aba 不看饱和：你给它多大的 τ 它就照单算多大的加速度，它不知道你的电机给得起多少。仿真器不会替你报错，直到你把这段代码放进真机——那时超限的指令被驱动器截断，你整条仿真对不上的轨迹就是这次的账单。
判据/修法：把峰值和 model.upperEffortLimit 逐关节比一次（本章程序打印了峰值、关节号和时刻，就是为了让这一步 5 秒能做完）。要么降增益、要么把阶跃换成有限加速度的轨迹参考（第 13 章），要么在控制律出口显式做饱和 tau = tau.cwiseMin(model.upperEffortLimit).cwiseMax(model.lowerEffortLimit)——注意：加了饱和之后，控制器的前馈假设（「我给的就是我算的」）就不再成立，计算力矩法的稳定性保证也随之失效，这一步要在仿真里如实建模才有意义。
坑 3：「稳态误差」其实是「末拍误差」
症状：看到 (a) 那一行「稳态误差=0.165947，死区±0.1: 从未稳定」，以为程序在自相矛盾——误差都算出来了，怎么还「从未稳定」？
simulate() 里 res.final_error = err; 每拍无条件覆盖，所以它是最后一拍的误差，不是「稳定之后的误差」。而 settled / last_exit_time 走的是另一套判据：只要误差还落在死区外，就把「稳定时刻」往后推（res.last_exit_time = t + dt;），推到最后一次离开死区的那一拍。
所以这一行的正确读法是：(a) 路过过 ±0.1 的死区，但又漂回去了，所以 2 s 结束时不算稳定；而 0.165947 只是 t=2.0 s 那一拍的瞬时误差。路过不等于稳定。这也是为什么本章要同时打印三个数（末拍误差、最后一次离开死区的时刻、是否稳定）——单个「稳态误差」在还在运动的系统上是一个会骗人的名字。
修法：给自己一条更硬的判据，例如「连续 N 拍误差都 < tol」才算稳定（本章为了代码短，用 settled + last_exit_time 两个字段凑出同样的信息），或者干脆把仿真跑到误差的变化率也接近 0 再取末拍值。
坑 4：控制律写进 data 的结果，下一拍就被 aba 覆盖
症状：数值「看着像上一拍的」，或者诡异得对不上任何一次计算。
data 是草稿纸（第 10 章 10.2 规则 1）。控制律 ct_law 里调了 nonLinearEffects 和 crba，于是 data.nle、data.M 都被写过；simulate() 下一拍的 aba 又会把 data 里大半个字段刷一遍。这里的隐蔽之处在于视图不是值：selfadjointView 返回的是指向 data.M 那块内存的视图，如果你写
crba(model, data, q);
auto M = data.M.selfadjointView<Eigen::Upper>(); // 视图，不拷贝
auto b = data.nle; // 拷贝（VectorXd 是值）
return M * vref + b; // 这一行里 M 还有效，侥幸没事
只要 M * vref 在任何一次别的 Pinocchio 调用之后才求值，乘到的就是被覆盖过的 data.M。本章【7】的写法是当场拷进值类型：
crba(model, data, q0);
const Eigen::MatrixXd M0 = data.M.selfadjointView<Eigen::Upper>(); // 这一次赋值才是真正的求值+拷贝
规矩：从 data 里取矩阵，要么当帧用完，要么赋给 Eigen::MatrixXd / Eigen::VectorXd 这类值类型。别用 auto 接视图。
坑 5：删掉 lambda 上那个 -> Eigen::VectorXd，居然也能跑
症状：你把三个控制律的显式返回类型删掉，程序编译通过、结果一字不差；于是你以为这条规矩是多余的——直到某一天同一份 lambda 被 auto 接住，Debug 直接崩。
本章实测过这件事（_out/experiments/probe_return_type.txt）：把 (a) 纯 PD 那个 lambda 的 -> Eigen::VectorXd 删掉，其余代码与主程序逐字节相同，结果是 编译通过（BUILD_RC=0）、运行 EXIT=0、输出与 _out/02_forward_dynamics.txt 逐字节相同。原因是 std::function<Eigen::VectorXd(...)> 的返回值会强制把 Eigen 表达式物化成 VectorXd。
所以这条规矩不是「不写就崩」，而是**「写了对什么都不知道的情况免疫」**：真正咬人的场景是第 02 章 2.7 亲手测过的那个——auto 推导出来的返回类型是引用局部量的表达式模板，同一份源码 Release 给出正确数字、Debug 连 main 都没走完，编译器全程沉默。
规矩：函数和 lambda 返回 Eigen 结果时，返回类型一律写死成 Eigen::VectorXd / Eigen::MatrixXd；写成一种不需要例外的形式，比记住哪几种情况侥幸没事划算。
坑 6：把示例模型的数字当物理参考
【3】里 2 N·m 产生 100 rad/s²，【5】里 3826.5 N·m 的峰值，【7】里 wrist2 一块 1 kg 的质量贡献了 M(0,0) 的六成（6.25 + 1.0 = 7.25，总共才 12.18）——这些数都只能用来验证算法，因为示例模型的惯性参数是人为设的（肩关节 0.1 kg、惯量 0.01·I₃，见 sample-models.hxx:166-169，第 10 章【7】已经把六个刚体的 mass / lever / 惯量对角 逐个打印过）。
判据：任何「这个力矩/加速度是不是太夸张了」的问题，先去看模型来源。URDF 里的惯量经常是 CAD 导出时的占位值，或者干脆只给了质量没给惯量——第 10 章 10.7 那句「动力学结果完全由这些惯性参数决定」在这里同样成立，而且正动力学会把它放大：M 偏小 ⇒ 同一份 τ 算出的 q̈ 偏大 ⇒ 仿真里的机器人比真机敏捷得多。
11.10 练习（都能用本章程序改出来）
- 把
Kp从 200 降到 50（Kd也按同比例降到 10）。 先按 11.7 的两条公式预测，再跑：- (a)(b) 的力矩峰值应当是
Kp × π/2 = 50 × 1.5708 = 78.54 N·m； - (a) 的重力 sag 应当从
0.166涨到接近 4 倍（e ≈ τ_g/Kp）。
跑完把你预测和实测的差距写下来——特别是 (a) 那一行的settled有没有变。
- (a)(b) 的力矩峰值应当是
- 用【7】那张逐刚体表算
M(1,1)（关节 2 那一列的对角元），和第 10 章【4】打印的M对答案。
提示：关节 2 的轴在q=0时也是ŷ，原点在(0,0,0)，所以它和关节 1 的轴互相垂直；从肩 2 往后的五个刚体都要算。第 10 章那张表里这一位是12.17——它和M(0,0)=12.18只差0.01，想想差在哪一块。 - 把【5】的阶跃目标改成匀速斜坡：
q_des[0] = min(π/2, t × π/4)（0.5 rad/s 的速度限）。三档控制器的力矩峰值各变成多少？哪一档掉得最狠？
这题是给第 13 章铺路的：峰值不是控制器的「脾气」，而是指令要求的加速度乘以M。 - 把 11.8 探针的
dt从0.001改成0.01，两种积分顺序各跑一次。 观察半隐式那一列是不是也不再「有负有正」，而是开始一路涨。
想看的结论：辛性质的救命效果只在dt远小于系统特征周期的时候成立，dt大到一定程度两种写法一起发散。 - 在
simulate()里加一行饱和（tau = tau.cwiseMin(model.upperEffortLimit)，上限是 10 N·m，见坑 2），重跑【5】。三行的误差、稳定时刻、峰值各变成什么？
这题没有标准答案，重点是你会亲眼看到「前馈假设被打破」之后计算力矩法的样子——第 14 章要把这个现象做成正式实验。
本章要点
- 正动力学就是
q̈ = M(q)⁻¹(τ − b(q,q̇))；用aba()，别自己求逆——Minv只填上三角（第 10 章坑），而aba是 O(n) 且不碰任何逆矩阵。本章自检两条：aba与Minv*(tau-b)差5.20e-15；aba→rnea往返差3.92e-15。 - 仿真循环四行：控制器 →
aba→ 推进速度 →integrate推进位置。第 3、4 行的顺序是半隐式欧拉；实测同一台倒摆跑 4 秒，显式欧拉的机械能单调涨到+0.5859 J（初值23.0535 J），半隐式在初值附近来回摆到+0.0669 J，而两种顺序跑完的姿态已经差0.34 rad。 - 位形不是向量：
q = integrate(model, q, delta)，反方向用difference。转轴模型上nq == nv，直接加侥幸逐位相同（实测差0.00e+00）；带四元数的模型上直接加把四元数模推成1.004987562112（√1.01），而integrate给的是半角(sin(0.05), cos(0.05))、模恒为 1。 M的每个元素都能手算：M(i,j) = Σ 刚体 [m·(到轴 i 的距离 × 到轴 j 的距离) + z_iᵀ I z_j]，只累加「同时挂在 i、j 下游」的块。本章程序把M(0,0)逐刚体累加成12.180000，crba也给12.180000，差-1.78e-15；M(0,4) = 0.01 + 2.25 = 2.26同理。- 三档控制器的差别全在第一拍和最后一拍看得清：稳态误差
0.166 / 0.0563 / 0.000016（(b) 是 (a) 的 33.9%），力矩峰值314.2 / 314.2 / 3826.5 N·m，而314.2 = Kp × π/2、3826.5 = 314.1593 × 12.18。阶跃 + 计算力矩 = 尖峰，这个模型里它已经是力矩上限（10 N·m）的 383 倍。 - 两种误差来源互不相干：纯 PD 那一列几乎不动（
0.1717 → 0.1659，降 3.3%），因为它是物理的重力 sagτ_g/Kp；计算力矩那一列随dt变小（2.2e-5 → 1.6e-5），因为它只剩截断误差。但setprecision(6)只给右列留了两位有效数字——别拿它拟合阶数。 data是草稿纸，视图不是值：从data取矩阵要么当帧用完，要么赋给值类型（const Eigen::MatrixXd M = data.M.selfadjointView<Eigen::Upper>();）。
下一章把「整条链的运动」压缩成一个量：质心 centerOfMass 与质心动量 computeCentroidalMomentum（data.hg），它们是后面双足和四足平衡控制的入口。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
0
0- 0
已为社区贡献18条内容
所有评论(0)