第 14 章 计算力矩控制与模型误差：把「抵消」做到底，再量出它的代价

本章解决一个问题：第 13 章的 PD+重力补偿已经把机械臂稳稳带到终点了，那为什么还要费劲算质量矩阵 M？答案分两半——模型准时它能好一个量级，模型不准时它坏得也最快。这两半都要用实测数字说话，本章把它们都量出来。

先修：第 10 章（crba 只填上三角、rnea、中立姿态重力项为 0）、第 11 章（aba、半隐式欧拉、误差与步长一次方成正比）、第 12 章（data 字段的生命周期）、第 13 章（按 M_ii 整定增益、dt 与带宽这对矛盾）。
本章新增的 C++ 知识：类里持有 const 引用成员；std::unique_ptr 成员会让拷贝构造被隐式删除（附 MSVC 报错原文）；用 std::function 做「控制律插件」，让三种控制律跑同一个仿真回路；Model 可以拷贝，于是「控制器以为的机器人」和「真实的机器人」第一次成为两个对象。
产出：tutorials/06_project/02_computed_torque_control.cpp。全书最后一个程序，也是第 4 篇的收官。

14.1 控制律只有两行，本质是「把机器人抵消掉」

第 10 章把逆动力学讲清楚了：给定运动，力矩是

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

PD 控制律的做法是：把想要的加速度藏进误差里。

τ = Kp⊙(q_des − q) + Kd⊙(q̇_des − q̇) （第 13 章还加了 + G(q)）

代回真实动力学，闭环变成 M·q̈ = Kp⊙e + Kd⊙ė + [G − b]。注意 M 留在误差方程左边：每个关节的响应速度由它自己的惯量决定，这就是第 13 章非要用 M_ii 去整定增益的原因。

计算力矩（computed torque / 逆动力学控制 / 反馈线性化）的做法更直接：既然等式左边是 M·q̈ + b，那就照着它把 τ 写出来。

τ = M(q)·[ q̈_des + Kd⊙(q̇_des − q̇) + Kp⊙(q_des − q) ] + b(q, q̇)

方括号里是「我想要的加速度」：前馈 q̈_des 加上一个把误差拉回去的二阶校正。把它代进真实动力学 M·q̈ + b = τ，如果控制器用的 M、b 和机器人身上那一份是同一个，两边直接对消：

M·q̈ = M·[ q̈_des + Kd⊙ė + Kp⊙e ] → ë + Kd⊙ė + Kp⊙e = 0

M 消失了。 六个强耦合、随姿态变化的关节，被抵消成六个独立的二阶线性系统，各自按你指定的 ω、ζ 收敛。第 13 章要靠 M_ii 归一化才勉强对齐的六种行为，在这里是结构性地变成一样的。

这就是全部原理。但请看清楚对消的那一步依赖什么：

ë + Kd⊙ė + Kp⊙e = (I − M_nom⁻¹M_true)·q̈_des + M_nom⁻¹·(b_nom − b_true)
└──────── 模型不准时，右边不再是 0 ────────┘

左边是你设计的优雅二阶系统，右边是你有多相信自己的模型。第一项乘在指令加速度上，第二项乘在 M_nom⁻¹ 上——两者都不是小量。所以计算力矩不是「更高级所以更好」，它只是把负担从「误差」转移到了「模型精度」上。这一句是全书最想传达的工程观点，【7】节会把它量成一个具体倍数。

三档控制律各自抵消了什么，一张表说清：

14.2 三个 API 拼出控制律，外加三条约定

本章只新增两个函数，其余都是前面的老朋友：

用它们组装控制律，就是四行：

nonLinearEffects(model, data, q, q_dot); Eigen::VectorXd b = data.nle; // 拷出来
crba(model, data, q); Eigen::MatrixXd M(data.M.selfadjointView<Eigen::Upper>());
Eigen::VectorXd ref = q_ddot_des + Kp.cwiseProduct(e) + Kd.cwiseProduct(e_dot);
return M * ref + b;

三条约定，每一条都有实测：

约定 1：crba 只写上三角，selfadjointView<Upper>() 不是可选的装饰。 第 10 章讲过这个规矩，本章量一下「忘了它」的代价：14.9 的探针用正式程序【4】里同一组 q/v/a 测试向量算了两遍——

正确 sym(M)*a+b = [ -15.6047 -2.31683 1.19205 -0.30946 -1.52252 -0.182474]
偷懒 M*a+b = [-15.6047 -2.476 4.83205 12.876 -4.47933 1.97194]
最大逐元素差 = 13.1855 N·m，正确力矩的模 = 15.8978 N·m

新建的 Data 里 M 的下三角是精确的 0（探针打出来就是 0.0000），所以「偷懒」等价于把所有耦合项整个丢掉。误差 13.19 N·m 和正确力矩 15.90 N·m 是同一量级——程序不崩、不报错，只是每个关节的力矩都差 80% 左右。探针还试了「先用 computeAllTerms 再用 crba」的顺序，下三角依然是 0：这两个算法都只往上三角写。所以这个坑很稳定，不需要担心它时有时无。

约定 2：data 是共享草稿纸，任何「待会儿还要用」的值必须当场拷进局部变量。 哪些函数会改写 nle？探针逐个调用、逐元素比快照：

调用 nle 差 g 差 M 差
————————————————————————–
crba(q1) 0.0000 0.0000 0.0000
rnea(q1,v1,0) 0.0000 0.0000 0.0000
nonLinearEffects(q1,v1) 0.0000 0.0000 0.0000
aba(q1,v1,tau) 0.0000 0.0000 0.0000
computeAllTerms(q1,v1) 4.44e-16 3.55e-15 8.88e-16
forwardKinematics(q2) 4.44e-16 3.55e-15 8.88e-16
nonLinearEffects(q2,v1) 37.5440 3.55e-15 8.88e-16
computeGeneralizedGravity(q2)37.5440 38.3297 8.88e-16

（差是相对最开始的快照算的，所以第 5、6 行的 4.44e-16 是 computeAllTerms 重算留下的浮点重组痕迹，forwardKinematics 那一行数字和上一行完全相同，说明它压根没碰这三样。）真正会重写 nle 的是 nonLinearEffects 和 computeAllTerms；crba、rnea、aba 都不碰它。

注意这条实测顺带纠正了一个容易写错的理由：控制律里那句 Eigen::VectorXd b_term = data_->nle; 并不是「因为下一步的 crba 会覆盖它」——crba 不覆盖。要防的是中间任何一次为了别的事重新算姿态。探针演示了这个后果：

正确（拷贝） tau = [-28.5786 17.0545 2.74605 11.2907 -2.14445 3.48708 ] N·m
错误（引用） tau = [8.96539 -13.267 1.29214 -2.00582 1.21614 0.268272] N·m
最大逐元素差 = 37.5440 N·m；力矩本身量级 35.4876 N·m（2-范数）

把 data.nle 的引用留到「换了个姿态重算」之后再用的话，得到的力矩里，b 是姿态 2 的、M 是姿态 1 的，误差比力矩本身还大。

约定 3：返回 Eigen 结果的函数，返回类型要显式写 Eigen::VectorXd。 第 02 章见过它崩、第 13 章见过它写进注释，本章的 computeTorque() 和三条 lambda 全都显式写了 -> Eigen::VectorXd。写成 auto 会返回引用局部临时量（M、b_term、ref）的表达式模板，函数一退就悬空。

顺带一句：本章没有用到 data.Minv。控制律只需要 M·ref，是一次乘法，不需要解方程；真要解 M⁻¹·x，请用第 10 章讲的 cholesky/solve，别自己 inverse()。

14.3 这一章的 C++ 结构，四个点

① 控制器持有的是引用成员 const Model& model_。 它必须在构造时就绑定，所以只能写在初始化列表里；绑上之后不能改绑，于是这个类不可赋值（operator= 被隐式删除）。这是「控制器相信哪个模型」这件事的表达方式：一个控制器一辈子只信一份模型。要换模型就再造一个。

② std::unique_ptr<Data> data_：控制器自带草稿纸。 为什么不用被控对象那份 data？因为仿真回路每一拍都要拿它跑 aba，而控制律每一拍要跑 nonLinearEffects + crba——共用一份就会互相踩（约定 2 的实测就是这个）。

代价是真实的：unique_ptr 只可移动、不可拷贝，于是整个类的拷贝构造被隐式删除。本章的临时试验文件里只写了一行 Controller b = a;，MSVC 的报错原文（14.9 有复现步骤）：

error C2280: “Controller::Controller(const Controller &)”: 尝试引用已删除的函数
编译器已在此处生成“Controller::Controller”
“Controller::Controller(const Controller &)”: 由于 数据成员 调用已删除或不可访问的函数
“std::unique_ptr<pinocchio::Data,…>::unique_ptr(const std::unique_ptr<…> &)”，
因此已隐式删除函数
“std::unique_ptr<pinocchio::Data,…>::unique_ptr(const std::unique_ptr<…> &)”: 已隐式删除函数

报错的最后一行指着 <memory> 里的 unique_ptr，而不是指着你的类——这是 C++ 里「一个成员拖垮整句话」的典型形态。想持有两份模型时，正确做法是造两个控制器对象，而不是拷贝一个再改它信的那个模型。

复现这份报错只要 25 行（整块存成 tutorials/06_project/99_probe_copy_ctrl.cpp，临时加一行 add_tutorial(99_probe_copy_ctrl) 再编译；_out/experiments/probe_copy_ctrl_MSVC.txt 是这次编译的报错原文）：

// 临时文件：故意编译不过，用来抓「控制器不可拷贝」的 MSVC 报错原文
#include <memory>
#include "pinocchio/multibody.hpp"
#include "pinocchio/multibody/sample-models.hpp"

using namespace pinocchio;

class Controller {
public:
Controller(const Model& model, const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd)
: model_(model), Kp_(Kp), Kd_(Kd) { data_ = std::make_unique<Data>(model); }
private:
const Model& model_;
std::unique_ptr<Data> data_;
Eigen::VectorXd Kp_, Kd_;
};

int main() {
Model model;
buildModels::manipulator(model, false);
Eigen::VectorXd k = Eigen::VectorXd::Ones(model.nv);
Controller a(model, k, k);
Controller b = a; // 这一行就是第 14 章坑 4 的复现点
(void)b;
return 0;
}

③ using TorqueLaw = std::function<VectorXd(const VectorXd&, const VectorXd&, double)>。 三个控制律塞进同一个 simulate() 里跑：这样「公平比较」是结构上保证的，而不是靠你复制粘贴三段几乎一样的循环。第 13 章的仿真回路只有一条控制律，不需要这层抽象；本章开始需要了。

④ Model 是可拷贝的：Model plant(model); 这是本章最重要的一个 C++ 事实，因为没有它就没有【7】的模型误差实验——「真实机器人」和「控制器以为的机器人」必须是两个对象。拷出来之后改 plant.inertias[i].mass() 就得到一个参数漂了的假机器人。

14.4 工程里加上这一章

根 CMakeLists.txt 里这一行在第 13 章就打开了，不用动：

add_subdirectory(tutorials/06_project)

tutorials/06_project/CMakeLists.txt 整份改成（也就是在上一章那两行的基础上加第三行）：

# 教程 06：综合实战
add_tutorial(01_trajectory_planning) # 关节空间轨迹规划 + PD 跟踪
add_tutorial(02_computed_torque_control) # 计算力矩（逆动力学）控制

改完先 cmake -S . -B build 重新配置一次再编译。

14.5 完整代码

存成 tutorials/06_project/02_computed_torque_control.cpp。整块复制即可编译：

// ============================================================================
// 教程 06-2: 综合实战 – 计算力矩（逆动力学）控制
// ============================================================================
//
// 【学习目标】
// 1. 把教程 04 的动力学 API 真正用起来：M(q)、b(q,q_dot) 组装计算力矩控制律
// 2. 看清计算力矩法的本质——用模型把机器人「抵消」成一个纯双积分器
// 3. 在同一份被控对象上公平比较三种控制律：PD / PD+重力 / 计算力矩
// 4. 亲手做一遍「模型误差实验」，理解为什么说计算力矩依赖模型精度
// 5. 用残差对账验证自己的实现没有写错（rnea 与 M*a+b 恒等）
//
// 【C++ 知识点】
// – 类里持有 const 引用成员 + std::unique_ptr 成员（控制器自带一份 Data）
// – std::function 作为「控制律」的插件接口；lambda 捕获轨迹对象
// – lambda 返回 Eigen 表达式必须显式写返回类型（04-2 的崩溃教训）
// – Model 可拷贝：用它造一个「真实机器人」和一个「控制器以为的机器人」
//
// 【Pinocchio 知识点】
// – crba(model, data, q) -> data.M：质量矩阵，只填上三角，要 selfadjointView<Upper>
// – nonLinearEffects(model, data, q, v) -> data.nle：科氏 + 重力项合计 b(q,v)
// – rnea(model, data, q, v, a)：同一项动力学的另一种算法，用来对账
// – aba(model, data, q, v, tau)：正动力学，充当被控对象
// – integrate(model, q, delta)：流形上的构形更新
// ============================================================================

#include <iostream>
#include <iomanip>
#include <sstream>
#include <cmath>
#include <string>
#include <vector>
#include <memory>
#include <functional>
#include <algorithm>
#include "pinocchio/multibody.hpp"

#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp" // rnea, nonLinearEffects
#include "pinocchio/algorithm/crba.hpp" // crba
#include "pinocchio/algorithm/aba.hpp" // aba
#include "pinocchio/algorithm/jacobian.hpp"
#include "pinocchio/algorithm/frames.hpp" // updateFramePlacements
#include "pinocchio/algorithm/joint-configuration.hpp" // integrate
#include "pinocchio/multibody/sample-models.hpp"

using namespace pinocchio;

// —————————————————————————
// 小工具：极小的数用科学计数法打印，否则全变成 0.0000 读者没法判断量级
// —————————————————————————
std::string sci(double x) {
std::ostringstream oss;
if (x != 0.0 && std::fabs(x) < 1e-3) {
oss << std::scientific << std::setprecision(2) << x;
} else {
oss << std::fixed << std::setprecision(4) << x;
}
return oss.str();
}

// 【踩过的坑】std::cout 的小数精度是「状态」，一旦某处写了 setprecision(2)，
// 后面所有浮点输出都跟着变成 2 位——第 6 节的 dt 列就被这样打印成了
// 0.01 / 0.01 / 0.00 / 0.00。凡是往表格里填数字，一律用下面这种
// 「局部ostringstream」格式化，别去动 cout 的全局状态。
std::string fmt(double x, int prec = 4) {
std::ostringstream oss;
oss << std::fixed << std::setprecision(prec) << x;
return oss.str();
}

void printVec(const std::string& name, const Eigen::VectorXd& v, int prec = 4) {
std::ostringstream oss;
oss << " " << name << " = [" << std::fixed << std::setprecision(prec);
for (int i = 0; i < v.size(); ++i) {
if (i > 0) oss << ", ";
oss << v[i];
}
oss << "]";
std::cout << oss.str() << std::endl; // 全程走局部流，不污染 cout 的格式状态
}

// ============================================================================
// 正弦轨迹：位置/速度/加速度都有解析式，正好是计算力矩法想要的 feedforward
// ============================================================================
//
// q_d(t) = offset + A * sin(w*t + phi)
// q_d_dot(t) = A*w * cos(w*t + phi)
// q_d_ddot(t)= -A*w^2 * sin(w*t + phi)
//
// 为什么用正弦而不是三次多项式（教程 06-1）？因为正弦永远在动：加速度一直
// 不为零，前馈项的价值能全程体现出来；多项式轨迹末尾会停住，只剩稳态保持。
// ============================================================================
class SineTrajectory {
public:
SineTrajectory(int nq)
: nq_(nq), amp_(Eigen::VectorXd::Zero(nq)),
freq_(Eigen::VectorXd::Zero(nq)), phase_(Eigen::VectorXd::Zero(nq)),
offset_(Eigen::VectorXd::Zero(nq)) {}

void setParameters(const Eigen::VectorXd& amp, const Eigen::VectorXd& freq,
const Eigen::VectorXd& phase, const Eigen::VectorXd& offset) {
amp_ = amp; freq_ = freq; phase_ = phase; offset_ = offset;
}

Eigen::VectorXd position(double t) const {
Eigen::VectorXd q(nq_);
for (int i = 0; i < nq_; ++i)
q[i] = offset_[i] + amp_[i] * std::sin(freq_[i] * t + phase_[i]);
return q;
}
Eigen::VectorXd velocity(double t) const {
Eigen::VectorXd v(nq_);
for (int i = 0; i < nq_; ++i)
v[i] = amp_[i] * freq_[i] * std::cos(freq_[i] * t + phase_[i]);
return v;
}
Eigen::VectorXd acceleration(double t) const {
Eigen::VectorXd a(nq_);
for (int i = 0; i < nq_; ++i)
a[i] = –amp_[i] * freq_[i] * freq_[i] * std::sin(freq_[i] * t + phase_[i]);
return a;
}

// 轨迹的「转速」上限，用它来判断采样率够不够
double maxOmega() const { return freq_.maxCoeff(); }

private:
int nq_;
Eigen::VectorXd amp_, freq_, phase_, offset_;
};

// ============================================================================
// 计算力矩控制器
// ============================================================================
//
// 控制律（教科书写法）：
// tau = M(q) * [ q_ddot_des + Kd*(q_dot_des – q_dot) + Kp*(q_des – q) ]
// + b(q, q_dot)
// 其中 b = C(q,q_dot)*q_dot + G(q)，Pinocchio 的 nonLinearEffects 一次给全。
//
// 代入真实动力学 M(q)q_ddot + b(q,q_dot) = tau，得到
// M*q_ddot = M*(q_ddot_des + Kd*e_dot + Kp*e)
// 若模型准确，M 消掉：
// e_ddot + Kd*e_dot + Kp*e = 0
// 每个关节都变成独立的二阶线性系统——这就是「反馈线性化 / 计算力矩」。
//
// 【一定要记住的代价】M 消不掉的那一刻（模型参数错、摩擦没建模、
// 关节力矩饱和），误差方程右边就多出一个扰动项，而且是乘在
// 一个很大的加速度指令上的。所以计算力矩不但不鲁棒，反而比 PD 更吃模型精度。
// 本教程第 7 节用实验把这句话验证出来。
// ============================================================================
class ComputedTorqueController {
public:
// 增益是向量：理由见教程 06-1 第 3 节，各关节等效惯量能差 10 倍。
ComputedTorqueController(const Model& model,
const Eigen::VectorXd& Kp,
const Eigen::VectorXd& Kd)
: model_(model), Kp_(Kp), Kd_(Kd) {
// Data 必须在 Model 建好之后构造，而且和这个 Model 一一绑定。
// 用 unique_ptr 持有：本类没有所有权转移需求，但 Data 尺寸不小，
// 放在堆上让控制器本身保持可拷贝语义清晰（构造函数里 make_unique）。
data_ = std::make_unique<Data>(model);
}

Eigen::VectorXd computeTorque(const Eigen::VectorXd& q,
const Eigen::VectorXd& q_dot,
const Eigen::VectorXd& q_des,
const Eigen::VectorXd& q_dot_des,
const Eigen::VectorXd& q_ddot_des) {
// 返回类型必须显式写 Eigen::VectorXd。写成 auto 会返回一个引用局部
// 临时量（M、b_term、q_ddot_computed）的表达式模板，函数一退就悬空，
// 这个坑在教程 04-2 里直接把程序跑崩过一次。
Eigen::VectorXd e = q_des – q;
Eigen::VectorXd e_dot = q_dot_des – q_dot;

// 1. 先把「想要的加速度」算出来（线性化后的外层闭环）
Eigen::VectorXd q_ddot_ref = q_ddot_des + Kp_.cwiseProduct(e)
+ Kd_.cwiseProduct(e_dot);

// 2. 再把它翻译成力矩：tau = M * q_ddot_ref + b
// b 用 nonLinearEffects 一次拿到（= 科氏项 + 重力项）。
// 当场拷进局部变量：data 是共享草稿纸。crba 本身不写 nle（14.9 探针逐个函数
// 量过），但 nonLinearEffects 和 computeAllTerms 会。只要中间为了别的事情
// 再算一次别的姿态，留着引用就会把两个姿态的量拼进同一条力矩里——
// 不崩、不报错，只是不对。
nonLinearEffects(model_, *data_, q, q_dot);
Eigen::VectorXd b_term = data_->nle;

crba(model_, *data_, q);
Eigen::MatrixXd M = Eigen::MatrixXd(data_->M.selfadjointView<Eigen::Upper>());

return M * q_ddot_ref + b_term;
}

// 只补偿重力、不动 M 和科氏项——用来做中间档对比
Eigen::VectorXd gravityCompensatedTorque(const Eigen::VectorXd& q,
const Eigen::VectorXd& q_dot,
const Eigen::VectorXd& q_des,
const Eigen::VectorXd& q_dot_des) {
Eigen::VectorXd tau = Kp_.cwiseProduct(q_des – q)
+ Kd_.cwiseProduct(q_dot_des – q_dot);
computeGeneralizedGravity(model_, *data_, q); // 结果写在 data_->g
tau += data_->g;
return tau;
}

// 纯 PD：连重力都不补
Eigen::VectorXd pdTorque(const Eigen::VectorXd& q,
const Eigen::VectorXd& q_dot,
const Eigen::VectorXd& q_des,
const Eigen::VectorXd& q_dot_des) const {
return Kp_.cwiseProduct(q_des – q) + Kd_.cwiseProduct(q_dot_des – q_dot);
}

const Eigen::VectorXd& Kp() const { return Kp_; }
const Eigen::VectorXd& Kd() const { return Kd_; }

private:
const Model& model_; // 控制器「相信」的模型
std::unique_ptr<Data> data_; // 它自己的计算缓存
Eigen::VectorXd Kp_, Kd_;
};

// —————————————————————————
// 一次仿真的返回值
// —————————————————————————
struct RunResult {
double e_max; // 全程最大关节误差（2-范数）
double e_rms; // 全程 RMS 误差
double e_ss; // 结束时刻误差
double tau_peak; // 力矩峰值（2-范数）
bool diverged;
Eigen::VectorXd q_final;
};

// 控制律的插件接口：输入 (q, q_dot, t)，输出 tau。
// 三个控制器都塞进同一个仿真回路，比较才公平。
using TorqueLaw = std::function<Eigen::VectorXd(const Eigen::VectorXd&,
const Eigen::VectorXd&,
double)>;

// plant 是「真实的机器人」，可以和控制器相信的 model 不是同一个对象。
RunResult simulate(const Model& plant, Data& plant_data,
const SineTrajectory& traj,
const TorqueLaw& law,
double dt, double total_time,
const Eigen::VectorXd& q_start,
const Eigen::VectorXd& q_dot_start,
FrameIndex ee_idx, bool verbose, int print_every) {
const int steps = static_cast<int>(std::round(total_time / dt));

Eigen::VectorXd q = q_start;
Eigen::VectorXd q_dot = q_dot_start; // 必须等于轨迹在 t=0 的速度，见第 2 节

RunResult res{0.0, 0.0, 0.0, 0.0, false, Eigen::VectorXd::Zero(plant.nq)};
double err_sum_sq = 0.0;

for (int step = 0; step <= steps; ++step) {
double t = step * dt;
Eigen::VectorXd q_des = traj.position(t);

Eigen::VectorXd tau = law(q, q_dot, t);

// 被控对象：正动力学
Eigen::VectorXd q_ddot = aba(plant, plant_data, q, q_dot, tau);

q_dot += q_ddot * dt;
q = integrate(plant, q, q_dot * dt);

if (!q.array().isFinite().all() || !q_dot.array().isFinite().all()) {
res.diverged = true;
break;
}

double err = (q_des – q).norm();
res.e_max = std::max(res.e_max, err);
res.e_ss = err;
err_sum_sq += err * err;
res.tau_peak = std::max(res.tau_peak, tau.norm());

if (verbose && (step % print_every == 0 || step == steps)) {
std::cout << " " << std::setw(6) << fmt(t, 2)
<< " | " << std::setw(11) << fmt(err, 4)
<< " | " << std::setw(11) << fmt(tau.norm(), 2)
<< " | " << (step == steps ? "结束" : "跟踪中") << std::endl;
}
}

res.e_rms = std::sqrt(err_sum_sq / (steps + 1));
res.q_final = q;
return res;
}

int main() {
std::cout << "===== 教程 06-2: 计算力矩（逆动力学）控制 =====" << std::endl;
std::cout << std::endl;

// ========================================================================
// 1. 模型：控制器用的名义模型
// ========================================================================

Model model;
buildModels::manipulator(model, false);
Data data(model);
FrameIndex ee_idx = model.getFrameId("effector_body");

std::cout << "【1. 系统配置】" << std::endl;
std::cout << " 名义模型 nq = " << model.nq << ", nv = " << model.nv
<< ", 末端坐标系索引 = " << ee_idx << std::endl;

crba(model, data, neutral(model));
Eigen::MatrixXd M0 = Eigen::MatrixXd(data.M.selfadjointView<Eigen::Upper>());
Eigen::VectorXd M_diag = M0.diagonal();
std::cout << " diag(M(neutral)) = [" << M_diag.transpose() << "] kg·m²" << std::endl;
std::cout << std::endl;

// ========================================================================
// 2. 轨迹
// ========================================================================

const double total_time = 6.0;
const double dt = 0.002;

SineTrajectory traj(model.nq);
Eigen::VectorXd amp(model.nq), freq(model.nq),
phase(model.nq), offset(model.nq);
for (int i = 0; i < model.nq; ++i) {
amp[i] = 0.3;
freq[i] = 1.0 + 0.3 * i; // 1.0 ~ 2.5 rad/s，越靠外的关节转得越快
phase[i] = 0.0; // 关键：sin(0)=0，轨迹起点正好落在中立位形上
offset[i] = 0.0;
}
traj.setParameters(amp, freq, phase, offset);

std::cout << "【2. 参考轨迹】" << std::endl;
std::cout << " q_d(t) = offset + A*sin(omega*t + phi)，"
<< "A = " << amp[0] << " rad" << std::endl;
std::cout << " omega = [" << freq.transpose() << "] rad/s" << std::endl;
std::cout << " 时长 = " << total_time << " s, 步长 dt = " << dt << " s" << std::endl;
std::cout << " 轨迹里每个关节都在持续加速，所以前馈项不会闲着。" << std::endl;
std::cout << " 【为什么 phi 必须是 0】仿真初值一定要「落在轨迹上」：" << std::endl;
std::cout << " q(0)=q_d(0) 且 q_dot(0)=q_dot_d(0)，本程序就是这么设的。" << std::endl;
std::cout << " 第一版把 phi 写成 0.2*i：t=0 时参考轨迹最大那一格已经在" << std::endl;
std::cout << " 0.25 rad，六个关节合起来是 0.39 rad 的跳变。三个控制器的 e_max" << std::endl;
std::cout << " 当场被压成同一个数 0.3930，看起来「彼此没区别」；再把初速度也归零，" << std::endl;
std::cout << " 起步这一帧的力矩峰值涨到实测 1745 N·m，之后连模型误差实验都" << std::endl;
std::cout << " 测不出东西。两组对照的完整表格在 14.9 的探针里，改一行重跑量的。" << std::endl;
std::cout << " 做对比实验之前，先确认你的指标没有被 t=0 的边界条件污染。" << std::endl;
std::cout << std::endl;

// ========================================================================
// 3. 增益整定
// ========================================================================

const double omega_des = 6.0; // 闭环带宽 rad/s
const double zeta_des = 1.0; // 临界阻尼
Eigen::VectorXd Kp = M_diag * (omega_des * omega_des);
Eigen::VectorXd Kd = M_diag * (2.0 * zeta_des * omega_des);

ComputedTorqueController controller(model, Kp, Kd);

std::cout << "【3. 增益】" << std::endl;
std::cout << " omega = " << omega_des << " rad/s, zeta = " << zeta_des << std::endl;
printVec("Kp = M_ii*omega^2", controller.Kp(), 1);
printVec("Kd = 2*zeta*omega*M_ii", controller.Kd(), 1);
std::cout << " omega*dt = " << fmt(omega_des * dt, 3)
<< " （离散化余量，判据见教程 06-1 第 8 节）" << std::endl;
std::cout << std::endl;

// ========================================================================
// 4. 先对账：M*a + b 是不是真的等于 rnea
// ========================================================================
//
// 计算力矩的实现全靠 M 和 b。这两样是从两个不同的算法里拿的，
// 必须先证明它们和 rnea 说的是同一件事，否则后面所有的误差分析都建
// 在沙子上。恒等式：rnea(q, v, a) == M(q)*a + b(q, v)。
// ========================================================================

std::cout << "【4. 实现自检：rnea 与 M*a + b 是否恒等】" << std::endl;
Eigen::VectorXd q_test = neutral(model);
q_test[0] = 0.7; q_test[1] = –0.4; q_test[2] = 0.3; q_test[3] = 0.2;
Eigen::VectorXd v_test = Eigen::VectorXd::Zero(model.nv);
v_test << 1.1, –0.6, 0.4, 0.9, –0.2, 0.3;
Eigen::VectorXd a_test = Eigen::VectorXd::Zero(model.nv);
a_test << 2.0, –1.5, 0.7, 1.3, –0.4, 0.6;

Eigen::VectorXd tau_rnea = rnea(model, data, q_test, v_test, a_test);
nonLinearEffects(model, data, q_test, v_test);
Eigen::VectorXd b_test = data.nle;
crba(model, data, q_test);
Eigen::MatrixXd M_test = Eigen::MatrixXd(data.M.selfadjointView<Eigen::Upper>());
Eigen::VectorXd tau_mb = M_test * a_test + b_test;

std::cout << " rnea = [" << tau_rnea.transpose() << "] N·m" << std::endl;
std::cout << " M*a + b = [" << tau_mb.transpose() << "] N·m" << std::endl;
std::cout << " 最大逐元素差 = " << sci((tau_rnea – tau_mb).cwiseAbs().maxCoeff())
<< " N·m（浮点重组误差，应远小于力矩本身量级）" << std::endl;
std::cout << std::endl;

// ========================================================================
// 5. 三种控制律同台对比（模型完全准确）
// ========================================================================

std::cout << "【5. 三种控制律对比（被控对象 = 名义模型，无模型误差）】" << std::endl;
std::cout << " 列: t=时刻(s), e=关节误差(rad), tau=力矩 2-范数(N*m)" << std::endl;
std::cout << " " << std::setw(6) << "t(s)" << " | " << std::setw(11) << "e(rad)"
<< " | " << std::setw(11) << "tau(N*m)" << " | state" << std::endl;
std::cout << " " << std::string(52, '-') << std::endl;

Eigen::VectorXd q_start = neutral(model);
Eigen::VectorXd q_dot_start = traj.velocity(0.0); // 初速度也放在轨迹上

// 三条 lambda 都显式写了 -> Eigen::VectorXd，理由见控制器注释
TorqueLaw pd_law = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot,
double t) -> Eigen::VectorXd {
return controller.pdTorque(q, q_dot, traj.position(t), traj.velocity(t));
};
TorqueLaw pdg_law = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot,
double t) -> Eigen::VectorXd {
return controller.gravityCompensatedTorque(q, q_dot,
traj.position(t), traj.velocity(t));
};
TorqueLaw ct_law = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot,
double t) -> Eigen::VectorXd {
return controller.computeTorque(q, q_dot, traj.position(t),
traj.velocity(t), traj.acceleration(t));
};

RunResult r_pd = simulate(model, data, traj, pd_law, dt, total_time,
q_start, q_dot_start, ee_idx, false, 500);
RunResult r_pdg = simulate(model, data, traj, pdg_law, dt, total_time,
q_start, q_dot_start, ee_idx, false, 500);
RunResult r_ct = simulate(model, data, traj, ct_law, dt, total_time,
q_start, q_dot_start, ee_idx, true, 500);
std::cout << std::endl;

// 末端误差：把每条控制律的终点状态单独算一次
auto eeError = [&](const Eigen::VectorXd& q_final) {
forwardKinematics(model, data, q_final);
updateFramePlacements(model, data);
Eigen::Vector3d p_act = data.oMf[ee_idx].translation();

Eigen::VectorXd q_ref = traj.position(total_time);
forwardKinematics(model, data, q_ref);
updateFramePlacements(model, data);
return (p_act – data.oMf[ee_idx].translation()).norm();
};

std::cout << " " << std::left << std::setw(24) << " law"
<< std::setw(12) << "e_max(rad)" << std::setw(12) << "e_rms(rad)"
<< std::setw(12) << "e_ss(rad)" << std::setw(12) << "ee_err(m)"
<< "tau_peak(N*m)" << std::endl;
std::cout << " " << std::string(86, '-') << std::endl;
auto row = [&](const std::string& name, const RunResult& r) {
if (r.diverged) {
std::cout << " " << std::left << std::setw(24) << name << "OVER" << std::endl;
return;
}
std::cout << " " << std::left << std::setw(24) << name
<< std::setw(12) << sci(r.e_max)
<< std::setw(12) << sci(r.e_rms)
<< std::setw(12) << sci(r.e_ss)
<< std::setw(12) << sci(eeError(r.q_final))
<< std::setw(14) << sci(r.tau_peak) << std::endl;
};
row("(a) PD", r_pd);
row("(b) PD + gravity", r_pdg);
row("(c) computed torque", r_ct);
std::cout << " 图例：e_max=全程最大, e_rms=均方根, e_ss=结束时刻, ee_err=末端位置误差" << std::endl;
std::cout << std::endl;
std::cout << " 三条控制律为什么这样排（以实测数为准）：" << std::endl;
std::cout << " (a) 重力没补：参考一伸出去就撞上重力 sag，e_rms 最大。" << std::endl;
std::cout << " (b) 补了重力：sag 没了，但科氏项和 M 的动态部分仍要靠误差去顶，" << std::endl;
std::cout << " e_rms 大约是 (a) 的一半。" << std::endl;
std::cout << " (c) 计算力矩：M 和 b 都被抵消，剩下的只是积分残差，e_rms 比 (b)" << std::endl;
std::cout << " 低 " << fmt(r_pdg.e_rms / r_ct.e_rms, 1) << " 倍——这就是前馈的价值。" << std::endl;
std::cout << " 注意力矩峰值反而是 (c) 最小（" << fmt(r_ct.tau_peak, 1) << " vs "
<< fmt(r_pd.tau_peak, 1) << " N·m）：" << std::endl;
std::cout << " PD 的力矩全靠误差撑，起步那一瞬间误差最大、力矩也最猛；计算力矩" << std::endl;
std::cout << " 提前知道要多少力矩（前馈），力矩曲线基本就是 rnea 的解析解，平滑且" << std::endl;
std::cout << " 更小。很多教材说「计算力矩力矩更大」，那是和阶跃指令（04-2 第 5 节）" << std::endl;
std::cout << " 比的结果；跟得上一台连续运动的机器人时，它反而更省力。" << std::endl;
std::cout << std::endl;

// ========================================================================
// 6. 计算力矩的收敛质量：把 dt 压小看残差会不会跟着掉
// ========================================================================

std::cout << "【6. 计算力矩的残差来自哪里（dt 扫描）】" << std::endl;
std::cout << " dt(s) | e_rms(rad) | e_ss(rad) | 状态" << std::endl;
std::cout << " " << std::string(50, '-') << std::endl;
double first_rms = 0.0, last_rms = 0.0;
int row_i = 0;
for (double d : {0.01, 0.005, 0.002, 0.001}) {
RunResult r = simulate(model, data, traj, ct_law, d, total_time,
q_start, q_dot_start, ee_idx, false, 1000000);
if (row_i == 0) first_rms = r.e_rms;
last_rms = r.e_rms;
++row_i;
std::cout << " " << std::left << std::setw(11) << fmt(d, 4)
<< std::setw(14) << sci(r.e_rms)
<< std::setw(13) << sci(r.e_ss)
<< (r.diverged ? "OVER（发散）" : "ok") << std::endl;
}
std::cout << " 实测：dt 从 " << fmt(0.01, 3) << " 降到 " << fmt(0.001, 3)
<< "（缩小 10 倍），e_rms 从 " << sci(first_rms) << " 降到 " << sci(last_rms)
<< "（缩小 " << fmt(first_rms / last_rms, 1) << " 倍）。" << std::endl;
std::cout << " 误差和 dt 一次方成正比——这正是半隐式欧拉「一阶方法」的定义。" << std::endl;
std::cout << " 也就是说 (c) 剩下的误差完全来自积分离散化，不是控制律不行：" << std::endl;
std::cout << " 想再压一个量级，要么把 dt 再除以 10（算力换精度），" << std::endl;
std::cout << " 要么换更高阶的积分器（RK4 / 辛积分器）。" << std::endl;
std::cout << std::endl;

// ========================================================================
// 7. 模型误差实验：计算力矩的真正软肋
// ========================================================================
//
// 第 5 节里模型是完美准确的，这在真实项目里不存在。这里造一个「真实
// 机器人」：把名义模型整份拷贝出来，再扰动它的连杆质量。控制器仍然
// 使用未扰动的 model_——这才是工程上「模型 vs 实物」的差距。
// ========================================================================

std::cout << "【7. 模型误差实验：把真实机器人的质量乘一个系数】" << std::endl;
std::cout << " 控制器相信的模型不变（仍是名义参数），被控对象的连杆质量 x scale。" << std::endl;
std::cout << " Model 是可拷贝的。只改质量这类【数值】参数时，旧 Data 配新 Model 照样" << std::endl;
std::cout << " 算得对（14.9 探针实测差 0.00e+00）；但 Data 里每个缓存的长度都是从" << std::endl;
std::cout << " Model 抄来的，改【结构】（关节数、基座类型）就全错位，" << std::endl;
std::cout << " 所以规矩照旧：拷出 Model 就立刻重建 Data。" << std::endl;
std::cout << " 列: e_rms 是全程均方根误差(rad)，最后两列是相对 scale=1.0 基线的放大倍数" << std::endl;
std::cout << " " << std::left << std::setw(8) << "scale"
<< std::setw(14) << "e_rms(PD+G)" << std::setw(14) << "e_rms(CT)"
<< std::setw(12) << "PD+G x" << "CT x" << std::endl;
std::cout << " " << std::string(62, '-') << std::endl;

double base_pdg = 0.0, base_ct = 0.0;
double last_ratio_pdg = 0.0, last_ratio_ct = 0.0;
for (double scale : {1.0, 0.8, 1.2, 1.5, 2.0, 3.0}) {
Model plant(model);
Data plant_data(plant);
for (JointIndex i = 1; i < plant.njoints; ++i) {
plant.inertias[i].mass() *= scale;
}
// 改完立刻生效吗？是的：inertias 是 Model 的普通成员，
// crba/rnea/aba 每次都现读现算，没有缓存需要失效。

RunResult b = simulate(plant, plant_data, traj, pdg_law, dt, total_time,
q_start, q_dot_start, ee_idx, false, 1000000);
RunResult c = simulate(plant, plant_data, traj, ct_law, dt, total_time,
q_start, q_dot_start, ee_idx, false, 1000000);
if (scale == 1.0) { base_pdg = b.e_rms; base_ct = c.e_rms; }

double rb = b.e_rms / base_pdg;
double rc = c.e_rms / base_ct;
last_ratio_pdg = rb; last_ratio_ct = rc;

std::cout << " " << std::left << std::setw(8) << fmt(scale, 1)
<< std::setw(14) << (b.diverged ? "OVER" : sci(b.e_rms))
<< std::setw(14) << (c.diverged ? "OVER" : sci(c.e_rms))
<< std::setw(12) << fmt(rb, 2) << fmt(rc, 2) << std::endl;
}
std::cout << " 读表（以最后实测倍数为准）：质量翻到 3 倍时，PD+重力只放大 "
<< fmt(last_ratio_pdg, 2) << " 倍，计算力矩放大 " << fmt(last_ratio_ct, 2)
<< " 倍。" << std::endl;
std::cout << " 原因就在第 5 节那行「抵消」。把 tau = M_nom*(q_ddot_des + Kd*e_dot + Kp*e) + b_nom" << std::endl;
std::cout << " 代进真实动力学 M_true*q_ddot + b_true = tau，误差不再是零，而是" << std::endl;
std::cout << " e_ddot + Kd*e_dot + Kp*e = (I – M_nom^{-1} M_true) * q_ddot_des" << std::endl;
std::cout << " + M_nom^{-1} (b_nom – b_true)" << std::endl;
std::cout << " 右边第一项和「指令加速度」成正比：本例的指令加速度是 A*omega^2，" << std::endl;
std::cout << " 最大的关节有 0.3*2.5^2 = 1.875 rad/s^2，模型差 3 倍就等于给它塞了个" << std::endl;
std::cout << " 持续扰动。而 PD+重力根本不指望抵消，模型偏差只是让它多顶一点力矩，" << std::endl;
std::cout << " 误差随扰动近似线性增长——所以它的放大倍数（" << fmt(last_ratio_pdg, 2)
<< "）远低于 CT（" << fmt(last_ratio_ct, 2) << "）。" << std::endl;
std::cout << " 结论：" << std::endl;
std::cout << " – 计算力矩在「模型准」时精度最高，在「模型错」时劣化也最快；" << std::endl;
std::cout << " – 工程上很少裸用它：先做参数辨识，或者加鲁棒项（滑模、自适应），" << std::endl;
std::cout << " 或者退一步只用「重力补偿 + PD」，把精度需求交给更高的增益。" << std::endl;
std::cout << std::endl;

// ========================================================================
// 8. 收尾
// ========================================================================

std::cout << "【8. 你已经做到的事】" << std::endl;
std::cout << " 1. 用 crba + nonLinearEffects 组装出计算力矩控制律，并和 rnea 对过账" << std::endl;
std::cout << " 2. 用同一个仿真回路公平比较 PD / PD+重力 / 计算力矩" << std::endl;
std::cout << " 3. 用拷贝 + 扰动模型的方式，量出了「模型误差 -> 跟踪误差」的传导关系" << std::endl;
std::cout << " 这条链路（URDF/模型 -> 运动学 -> 动力学 -> 轨迹 -> 控制）就是机器人" << std::endl;
std::cout << " 软件栈的最小可用版本，把它彻底吃透再去看 WBC、MPC 会轻松很多。" << std::endl;
std::cout << std::endl;
std::cout << "【继续学习的方向】" << std::endl;
std::cout << " 1. 把 buildModels::manipulator 换成教程 05 加载的真实 URDF 模型" << std::endl;
std::cout << " 2. 在力矩上加饱和限制，观察计算力矩在饱和下的表现" << std::endl;
std::cout << " 3. 用 data.M 做加权，试试 QP 形式的任务空间控制（教程里没讲）" << std::endl;
std::cout << " 4. 把这条控制回路接到实时循环里，量一量单次 computeTorque 的耗时" << std::endl;

return 0;
}

14.6 编译并运行

cmake -S . -B build -G "Visual Studio 17 2022" -A x64
cmake –build build –config Release
build\\bin\\02_computed_torque_control.exe

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

Release 版实测 0.148 秒跑完（这个程序内部做了 3 + 4 + 12 = 19 次完整仿真：【5】的三种控制律各 1 次、【6】的 4 个 dt、【7】的 6 个 scale × 2 条控制律，每次 3001 步）。和第 13 章一样，这个数字是给你吃定心丸用的：本章和 14.9 的所有结论都是「改一行、重跑一遍」得到的，0.15 秒的成本意味着你可以随便试。

14.7 期望输出（实测快照）

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

===== 教程 06-2: 计算力矩（逆动力学）控制 =====

【1. 系统配置】
名义模型 nq = 6, nv = 6, 末端坐标系索引 = 15
diag(M(neutral)) = [12.18 12.17 3.01 4.61 1.26 1.25] kg·m²

【2. 参考轨迹】
q_d(t) = offset + A*sin(omega*t + phi)，A = 0.3 rad
omega = [ 1 1.3 1.6 1.9 2.2 2.5] rad/s
时长 = 6 s, 步长 dt = 0.002 s
轨迹里每个关节都在持续加速，所以前馈项不会闲着。
【为什么 phi 必须是 0】仿真初值一定要「落在轨迹上」：
q(0)=q_d(0) 且 q_dot(0)=q_dot_d(0)，本程序就是这么设的。
第一版把 phi 写成 0.2*i：t=0 时参考轨迹最大那一格已经在
0.25 rad，六个关节合起来是 0.39 rad 的跳变。三个控制器的 e_max
当场被压成同一个数 0.3930，看起来「彼此没区别」；再把初速度也归零，
起步这一帧的力矩峰值涨到实测 1745 N·m，之后连模型误差实验都
测不出东西。两组对照的完整表格在 14.9 的探针里，改一行重跑量的。
做对比实验之前，先确认你的指标没有被 t=0 的边界条件污染。

【3. 增益】
omega = 6 rad/s, zeta = 1
Kp = M_ii*omega^2 = [438.5, 438.1, 108.4, 166.0, 45.4, 45.0]
Kd = 2*zeta*omega*M_ii = [146.2, 146.0, 36.1, 55.3, 15.1, 15.0]
omega*dt = 0.012 （离散化余量，判据见教程 06-1 第 8 节）

【4. 实现自检：rnea 与 M*a + b 是否恒等】
rnea = [ -15.6047 -2.31683 1.19205 -0.30946 -1.52252 -0.182474] N·m
M*a + b = [ -15.6047 -2.31683 1.19205 -0.30946 -1.52252 -0.182474] N·m
最大逐元素差 = 7.11e-15 N·m（浮点重组误差，应远小于力矩本身量级）

【5. 三种控制律对比（被控对象 = 名义模型，无模型误差）】
列: t=时刻(s), e=关节误差(rad), tau=力矩 2-范数(N*m)
t(s) | e(rad) | tau(N*m) | state
—————————————————-
0.00 | 0.0027 | 4.13 | 跟踪中
1.00 | 0.0022 | 41.19 | 跟踪中
2.00 | 0.0019 | 15.27 | 跟踪中
3.00 | 0.0021 | 18.46 | 跟踪中
4.00 | 0.0023 | 12.83 | 跟踪中
5.00 | 0.0025 | 20.88 | 跟踪中
6.00 | 0.0024 | 11.89 | 结束

law e_max(rad) e_rms(rad) e_ss(rad) ee_err(m) tau_peak(N*m)
————————————————————————————–
(a) PD 0.2619 0.1304 0.0878 0.0906 53.1390
(b) PD + gravity 0.1306 0.0621 0.0357 0.0243 47.7904
(c) computed torque 0.0028 0.0022 0.0024 0.0011 42.0588
图例：e_max=全程最大, e_rms=均方根, e_ss=结束时刻, ee_err=末端位置误差

三条控制律为什么这样排（以实测数为准）：
(a) 重力没补：参考一伸出去就撞上重力 sag，e_rms 最大。
(b) 补了重力：sag 没了，但科氏项和 M 的动态部分仍要靠误差去顶，
e_rms 大约是 (a) 的一半。
(c) 计算力矩：M 和 b 都被抵消，剩下的只是积分残差，e_rms 比 (b)
低 27.9 倍——这就是前馈的价值。
注意力矩峰值反而是 (c) 最小（42.1 vs 53.1 N·m）：
PD 的力矩全靠误差撑，起步那一瞬间误差最大、力矩也最猛；计算力矩
提前知道要多少力矩（前馈），力矩曲线基本就是 rnea 的解析解，平滑且
更小。很多教材说「计算力矩力矩更大」，那是和阶跃指令（04-2 第 5 节）
比的结果；跟得上一台连续运动的机器人时，它反而更省力。

【6. 计算力矩的残差来自哪里（dt 扫描）】
dt(s) | e_rms(rad) | e_ss(rad) | 状态
————————————————–
0.0100 0.0111 0.0122 ok
0.0050 0.0056 0.0061 ok
0.0020 0.0022 0.0024 ok
0.0010 0.0011 0.0012 ok
实测：dt 从 0.010 降到 0.001（缩小 10 倍），e_rms 从 0.0111 降到 0.0011（缩小 10.0 倍）。
误差和 dt 一次方成正比——这正是半隐式欧拉「一阶方法」的定义。
也就是说 (c) 剩下的误差完全来自积分离散化，不是控制律不行：
想再压一个量级，要么把 dt 再除以 10（算力换精度），
要么换更高阶的积分器（RK4 / 辛积分器）。

【7. 模型误差实验：把真实机器人的质量乘一个系数】
控制器相信的模型不变（仍是名义参数），被控对象的连杆质量 x scale。
Model 是可拷贝的。只改质量这类【数值】参数时，旧 Data 配新 Model 照样
算得对（14.9 探针实测差 0.00e+00）；但 Data 里每个缓存的长度都是从
Model 抄来的，改【结构】（关节数、基座类型）就全错位，
所以规矩照旧：拷出 Model 就立刻重建 Data。
列: e_rms 是全程均方根误差(rad)，最后两列是相对 scale=1.0 基线的放大倍数
scale e_rms(PD+G) e_rms(CT) PD+G x CT x
————————————————————–
1.0 0.0621 0.0022 1.00 1.00
0.8 0.0475 0.0100 0.76 4.51
1.2 0.0791 0.0093 1.27 4.15
1.5 0.1080 0.0241 1.74 10.82
2.0 0.1639 0.0509 2.64 22.84
3.0 0.3021 0.1119 4.86 50.21
读表（以最后实测倍数为准）：质量翻到 3 倍时，PD+重力只放大 4.86 倍，计算力矩放大 50.21 倍。
原因就在第 5 节那行「抵消」。把 tau = M_nom*(q_ddot_des + Kd*e_dot + Kp*e) + b_nom
代进真实动力学 M_true*q_ddot + b_true = tau，误差不再是零，而是
e_ddot + Kd*e_dot + Kp*e = (I – M_nom^{-1} M_true) * q_ddot_des
+ M_nom^{-1} (b_nom – b_true)
右边第一项和「指令加速度」成正比：本例的指令加速度是 A*omega^2，
最大的关节有 0.3*2.5^2 = 1.875 rad/s^2，模型差 3 倍就等于给它塞了个
持续扰动。而 PD+重力根本不指望抵消，模型偏差只是让它多顶一点力矩，
误差随扰动近似线性增长——所以它的放大倍数（4.86）远低于 CT（50.21）。
结论：
– 计算力矩在「模型准」时精度最高，在「模型错」时劣化也最快；
– 工程上很少裸用它：先做参数辨识，或者加鲁棒项（滑模、自适应），
或者退一步只用「重力补偿 + PD」，把精度需求交给更高的增益。

【8. 你已经做到的事】
1. 用 crba + nonLinearEffects 组装出计算力矩控制律，并和 rnea 对过账
2. 用同一个仿真回路公平比较 PD / PD+重力 / 计算力矩
3. 用拷贝 + 扰动模型的方式，量出了「模型误差 -> 跟踪误差」的传导关系
这条链路（URDF/模型 -> 运动学 -> 动力学 -> 轨迹 -> 控制）就是机器人
软件栈的最小可用版本，把它彻底吃透再去看 WBC、MPC 会轻松很多。

【继续学习的方向】
1. 把 buildModels::manipulator 换成教程 05 加载的真实 URDF 模型
2. 在力矩上加饱和限制，观察计算力矩在饱和下的表现
3. 用 data.M 做加权，试试 QP 形式的任务空间控制（教程里没讲）
4. 把这条控制回路接到实时循环里，量一量单次 computeTorque 的耗时

14.8 逐段读懂这份输出

【1】diag(M(neutral)) = [12.18 12.17 3.01 4.61 1.26 1.25]

这六个数就是本章全部故事的物理底座。第 05 章量过示例模型的参数：质量 (0.1, 0.1, 1, 1, 0.1, 1) kg，中立姿态下各体质心高度 0, 0, 0.5, 1.5, 2.0, 2.5 m，每个体都是各向同性惯性（小关节 0.01·I₃，大关节 1.0·I₃）。

手算 M_11：关节 1 绕世界 z 轴转，转动惯量 = 各体质心到轴距离的平方乘质量，加上各体绕自己质心的惯量。中立姿态所有质心都在 z 轴上，距离就是它们的 z 值：

Σ m_i·z_i² = 1·0.5² + 1·1.5² + 0.1·2.0² + 1·2.5² = 0.25 + 2.25 + 0.4 + 6.25 = 9.15
Σ c_i = 3×0.01 + 3×1.0 = 3.03
M_11 = 9.15 + 3.03 = 12.18 ← 和程序打印的一模一样

第 12 章表格里那两个累加值（Σm·z = 4.700、Σm·z² = 9.15、Σc = 3.03）在这里第二次派上用场。这就是 M 的物理内容：它不是什么抽象矩阵，就是「转到这个关节时，整条手臂的惯量有多大」。

再验最后一个：M_66 只有关节 6 往后的那一段——wrist2 一个 1 kg 的体，离轴 0.5 m：1·0.5² + 1.0 = 1.25 ✓ 正是打印值。M_55 多带上坐在轴上的 wrist1（0.1 kg，力臂 0，惯量 0.01）：0.25 + 0 + 1.0 + 0.01 = 1.26 ✓。

首尾之比 12.18 / 1.25 = 9.7：肩关节和腕关节的等效惯量差近 10 倍。第 13 章 13.3 就靠这个数说服你别用标量增益，本章再靠它解释 14.1 那句「M 消不掉」。

【3】增益：Kp = [438.5, 438.1, 108.4, 166.0, 45.4, 45.0]

Kp_i = M_ii·ω²，ω = 6 → ω² = 36。逐项对：12.18×36 = 438.5、3.01×36 = 108.4、1.25×36 = 45.0 ✓。Kd_i = 2ζω·M_ii = 12·M_ii：12.18×12 = 146.2 ✓。

注意这六个 Kp 相差 9.7 倍，但它们买到的 ω 是同一个 6 rad/s——这正是「按惯量整定」的含义，也是 14.1 里 M 对消之后「六个独立二阶系统」在数值上的样子。

omega*dt = 0.012：第 13 章的判据（ω·dt 要远小于 1，≳0.2 就开始出问题）在这里有约 80 倍余量。这一行不是装饰，14.9 实验二要把带宽推到 24 rad/s，那时 ω·dt = 0.048，余量只剩 20 倍。

【4】最大逐元素差 = 7.11e-15 N·m

这是本章的实现自检：rnea(q,v,a) 和 M(q)·a + b(q,v) 是同一定律的两条不同算法路径（前者是一次递推，后者是把 M、b 分开算再乘回去）。它们必须恒等。

量级核对：打印的力矩是 [-15.6047, -2.31683, …]，14.9 的探针顺手算了它的模 ‖τ‖ = 15.8978 N·m，于是相对偏差 = 7.11e-15 / 15.8978 = 4.5e-16，正好是 double 机器精度 2.2e-16 的两倍——这就是纯粹的浮点重组误差。如果这里给你 1e-3 而不是 1e-15，那不是你「精度不够」，是你把 M 的下三角忘了一半（约定 1 的 13.1855 就是这个量级的错）。

顺带：这一行的测试向量 q_test/v_test/a_test 和 14.9 探针里用的是同一组值，所以【4】打印的 M*a + b 向量和探针「正确 sym(M)*a+b」那行逐字相同。这种跨程序逐字相同，是比自己重新推导更快的验算方式。

【5】三种控制律同台

先看带 verbose 的那张表（只有 © 打印过程）：

0.00 | 0.0027 | 4.13 | 跟踪中
1.00 | 0.0022 | 41.19 | 跟踪中

t = 0 那一行的 4.13 N·m 值得单独说，因为它是「前馈到底买到了什么」的最小例子。t = 0 时：q = q_des = neutral（phi = 0 保证的），所以 e = 0、ė = 0；q̈_des(0) = −A·ω²·sin(0) = 0。方括号里整项为零 → τ = b(q, q̇)。而 b = C·q̇ + G，第 10 章实测过中立姿态的 G 恰好为 0，所以这 4.13 N·m 全是科氏项。探针把这一段单独量了出来：

q_dot(0) = [ 0.3 0.39 0.48 0.57 0.66 0.75] rad/s
b(q0, v0)（= 科氏 + 重力）= [ -3.8183 0.678528 0.775944 0.505728 -1.03766 0.3312] N·m
||b|| = 4.1333 N·m ← 应当就是表里那一格 4.13
其中重力项 G(q0) 的模 = 0.0000 N·m（中立姿态，第 10 章量过它恰好为 0）

0.3 = A·ω₁ = 0.3×1.0、0.75 = 0.3×2.5 ✓，每个关节的初速度就是 A·ω_i。

这 4.13 N·m 是 PD 拿不到的：误差为零时 PD 输出零力矩，而机器人此时正需要 4.13 N·m 来维持科氏平衡。PD 只能靠「先做错一点」来产生纠正力矩——这就是前馈的本质：它不需要先犯错。

再看汇总三行：

• (a)→(b)：e_rms 从 0.1304 到 0.0621，正好一半（程序打印「大约是 (a) 的一半」，0.0621/0.1304 = 0.476 ✓）。抵消重力就值这一倍。
• (b)→©：e_rms 再降 27.9 倍。这个数字要解释一句：程序打印的 27.9 是两个未舍入的原值相除，而你拿表里印出来的 0.0621 / 0.0022 会算出 28.2。差在这一列只印了四位小数。书里印比值时用原值、你手算时用印值，对不上是正常的——这是全书所有表格里都会遇到的小摩擦，处理方式就是别把最后一位当真理。
• e_ss（结束时刻误差）在 © 上比 e_rms 略大（0.0024 vs 0.0022）：说明 © 的残差是跟随性质的，不是「没收敛」——它全程都在 0.002 上下，见 verbose 表里 t = 1…6 全在 0.0019~0.0027 之间。
• 力矩峰值反而是 © 最小（42.06 < 47.79 < 53.14）。很多教材给人的印象是「计算力矩费力气」，那是拿它跟阶跃指令比的结果（第 11 章【5】那种）。跟得上一台连续运动的机器人时，它的力矩曲线基本就是逆动力学的解析解，平滑且更小；PD 的力矩全靠误差撑，起步那一下最猛。
• 末端误差和关节误差的量级关系：ee_err / e_ss 分别是 1.03、0.68、0.46（单位 m/rad）。手臂全展 2.5 m、腕到末端 0.5 m，所以这三个比值落在物理允许的区间内 ✓ —— 这是检查「我是不是把末端误差算成了关节误差」的快速数量级判据。

【6】dt 扫描：残差来自积分，不来自控制律

0.0100 0.0111 0.0122 ok
0.0050 0.0056 0.0061 ok
0.0020 0.0022 0.0024 ok
0.0010 0.0011 0.0012 ok

dt 每减半，e_rms 也几乎减半（0.0111→0.0056→0.0022→0.0011）；程序把首尾一比，打印「缩小 10.0 倍」。误差 ∝ dt¹ 就是「一阶方法」的定义，第 11 章用自由落体和能量漂移量过同一件事，这里在闭环里又量到一次：© 抵消得再干净，也抵不过半隐式欧拉的截断误差。想再要一个量级，只能 dt 再除以 10（算力换精度）或者换 RK4/辛积分器。

第三行 0.0022 和【5】表里 © 的 e_rms 是同一个数（同一设置跑的同一件事）——这种「同一量在不同段落重复出现」是自查转录错误的好机会。

【7】模型误差实验：本章的核心

先看表格本身（e_rms，控制律用同一份名义模型，只有被控对象的质量被乘了 scale）：

读法一（大纲里那句话的出处）：质量翻到 3 倍时，PD+重力的误差是自己的 4.86 倍，计算力矩是自己的 50.21 倍。「计算力矩吃模型精度」这件事被量出来了。

读法二（更重要，而且容易被读法一带跑）：把倍数换成绝对值再看一遍——scale=3 时计算力矩是 0.1119 rad，PD+重力是 0.3021 rad，计算力矩仍然是三者里最准的，只是优势从 28 倍缩到 2.70 倍（0.3021/0.1119）。

所以那张「50 倍 vs 4.9 倍」的表说的是相对劣化，不是「模型不准就别用计算力矩」。程序末尾给的结论是「模型准时精度最高、模型错时劣化最快」，这两句同时成立，不矛盾——如果你在笔记里只抄了一句「计算力矩怕模型误差」，那你抄丢了信息。

读法三：它真的近似线性吗？ 拿表里的数算 (e_rms − e_rms(scale=1)) / |scale − 1|：

（这些比值是用上面表格里的打印值手算的，不是程序输出的。）两条都近似常数 → 误差随模型偏差近似线性增长 ✓，程序说的「近似线性」对 PD+重力成立，对计算力矩也成立。但注意计算力矩的斜率（0.036~0.055）比 PD+重力的（0.073~0.120）更小：绝对意义上它每单位模型误差多付的代价其实更少，它「放大 50 倍」全靠基线低 28 倍。斜率缓慢上行（0.036→0.055）也是诚实的结果：mass() *= scale 只放大质量，不放大各体绕自身质心的惯量，所以 M_true 并不是 scale·M_nom，线性只是近似。

读法四：那个 1.875 rad/s² 是哪个 ω？ 程序末尾推的是 A·ω² = 0.3×2.5² = 1.875。这里的 ω 是轨迹自己的频率 omega = [1, 1.3, 1.6, 1.9, 2.2, 2.5] rad/s（【2】那一行），不是设计带宽 omega_des = 6（【3】那一行）。两个 ω 都叫 omega，是本章最容易读错的地方：

• 轨迹频率 ω 决定 q̈_des，也就是扰动的强度（模型误差通过它进账本）；
• 设计带宽 ω_des 决定 Kp = M·ω_des²，也就是你压住扰动的手劲。

dist/Kp = 1.875·(模型相对差) / (M·ω_des²/M) —— 分子里的 ω 属于轨迹、分母里的属于带宽，二者互不相干。14.9 实验二就把带宽单独扫一遍，量出这条 1/ω_des² 的标度律。

【8】收尾

04-2 第 5 节 这类写法指的是教程编号（tutorials/04_dynamics/02_forward_dynamics），也就是本书第 11 章。全书的坑和引用都用「章的中文编号 + 程序编号」两种说法混着标，速查见附录 B。

14.9 动手对照实验：三组实测

下面三组都是改代码重跑得到的，不是推演。复现代码在 _out/experiments/，各自独立可编译（临时往 tutorials/06_project/CMakeLists.txt 加一行 add_tutorial(99_probe_*)，cmake -S . -B build && cmake –build build –config Release –target 99_probe_*）。

实验一：初值不在轨迹上，指标会被 t = 0 那一帧吃掉

正式程序【2】讲了「第一版把 phi 写成 0.2*i」这个故事。探针把 phi 和「初速度取不取轨迹值」两个开关各设两档，四种组合全跑：

// 探针：第 14 章两组对照实验的实测来源
// A. 初值不在轨迹上：把 phi 从 0 改回 0.2*i，看 t=0 那一帧吃掉多少指标
// B. 带宽 x 质量系数二维表：离散化残差和模型误差残差各自的标度律
// 编译：tutorials/06_project/CMakeLists.txt 里临时加一行 add_tutorial(99_probe_ct_experiments)
// cmake -S . -B build && cmake –build build –config Release –target 99_probe_ct_experiments
#include <iostream>
#include <iomanip>
#include <sstream>
#include <cmath>
#include <string>
#include <vector>
#include <memory>
#include <functional>
#include <algorithm>
#include "pinocchio/multibody.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp" // rnea, nonLinearEffects
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp" // integrate
#include "pinocchio/multibody/sample-models.hpp"

using namespace pinocchio;

std::string sci(double x) {
std::ostringstream oss;
if (x != 0.0 && std::fabs(x) < 1e-3) {
oss << std::scientific << std::setprecision(2) << x;
} else {
oss << std::fixed << std::setprecision(4) << x;
}
return oss.str();
}

std::string fmt(double x, int prec = 4) {
std::ostringstream oss;
oss << std::fixed << std::setprecision(prec) << x;
return oss.str();
}

class SineTrajectory {
public:
SineTrajectory(int nq)
: nq_(nq), amp_(Eigen::VectorXd::Zero(nq)), freq_(Eigen::VectorXd::Zero(nq)),
phase_(Eigen::VectorXd::Zero(nq)), offset_(Eigen::VectorXd::Zero(nq)) {}

void setParameters(const Eigen::VectorXd& amp, const Eigen::VectorXd& freq,
const Eigen::VectorXd& phase, const Eigen::VectorXd& offset) {
amp_ = amp; freq_ = freq; phase_ = phase; offset_ = offset;
}
Eigen::VectorXd position(double t) const {
Eigen::VectorXd q(nq_);
for (int i = 0; i < nq_; ++i)
q[i] = offset_[i] + amp_[i] * std::sin(freq_[i] * t + phase_[i]);
return q;
}
Eigen::VectorXd velocity(double t) const {
Eigen::VectorXd v(nq_);
for (int i = 0; i < nq_; ++i)
v[i] = amp_[i] * freq_[i] * std::cos(freq_[i] * t + phase_[i]);
return v;
}
Eigen::VectorXd acceleration(double t) const {
Eigen::VectorXd a(nq_);
for (int i = 0; i < nq_; ++i)
a[i] = –amp_[i] * freq_[i] * freq_[i] * std::sin(freq_[i] * t + phase_[i]);
return a;
}

private:
int nq_;
Eigen::VectorXd amp_, freq_, phase_, offset_;
};

struct RunResult {
double e_max, e_rms, e_ss, tau_peak;
bool diverged;
Eigen::VectorXd q_final;
};

using TorqueLaw = std::function<Eigen::VectorXd(const Eigen::VectorXd&,
const Eigen::VectorXd&, double)>;

RunResult simulate(const Model& plant, Data& plant_data, const SineTrajectory& traj,
const TorqueLaw& law, double dt, double total_time,
const Eigen::VectorXd& q_start, const Eigen::VectorXd& q_dot_start) {
const int steps = static_cast<int>(std::round(total_time / dt));
Eigen::VectorXd q = q_start, q_dot = q_dot_start;
RunResult res{0.0, 0.0, 0.0, 0.0, false, Eigen::VectorXd::Zero(plant.nq)};
double err_sum_sq = 0.0;

for (int step = 0; step <= steps; ++step) {
double t = step * dt;
Eigen::VectorXd tau = law(q, q_dot, t);
Eigen::VectorXd q_ddot = aba(plant, plant_data, q, q_dot, tau);
q_dot += q_ddot * dt;
q = integrate(plant, q, q_dot * dt);

if (!q.array().isFinite().all() || !q_dot.array().isFinite().all()) {
res.diverged = true;
break;
}
double err = (traj.position(t) – q).norm();
res.e_max = std::max(res.e_max, err);
res.e_ss = err;
err_sum_sq += err * err;
res.tau_peak = std::max(res.tau_peak, tau.norm());
}
res.e_rms = std::sqrt(err_sum_sq / (steps + 1));
res.q_final = q;
return res;
}

// 控制器：相信的永远是未扰动的 nominal model
class Controller {
public:
Controller(const Model& model, const Eigen::VectorXd& Kp, const Eigen::VectorXd& Kd)
: model_(model), Kp_(Kp), Kd_(Kd), data_(new Data(model)) {}

Eigen::VectorXd computeTorque(const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot,
const Eigen::VectorXd& q_des, const Eigen::VectorXd& q_dot_des,
const Eigen::VectorXd& q_ddot_des) {
Eigen::VectorXd e = q_des – q, e_dot = q_dot_des – q_dot;
Eigen::VectorXd ref = q_ddot_des + Kp_.cwiseProduct(e) + Kd_.cwiseProduct(e_dot);
nonLinearEffects(model_, *data_, q, q_dot);
Eigen::VectorXd b_term = data_->nle;
crba(model_, *data_, q);
Eigen::MatrixXd M = Eigen::MatrixXd(data_->M.selfadjointView<Eigen::Upper>());
return M * ref + b_term;
}
Eigen::VectorXd pdgTorque(const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot,
const Eigen::VectorXd& q_des, const Eigen::VectorXd& q_dot_des) {
Eigen::VectorXd tau = Kp_.cwiseProduct(q_des – q) + Kd_.cwiseProduct(q_dot_des – q_dot);
computeGeneralizedGravity(model_, *data_, q);
tau += data_->g;
return tau;
}
Eigen::VectorXd pdTorque(const Eigen::VectorXd& q, const Eigen::VectorXd& q_dot,
const Eigen::VectorXd& q_des, const Eigen::VectorXd& q_dot_des) const {
return Kp_.cwiseProduct(q_des – q) + Kd_.cwiseProduct(q_dot_des – q_dot);
}

private:
const Model& model_;
Eigen::VectorXd Kp_, Kd_;
std::unique_ptr<Data> data_;
};

int main() {
Model model;
buildModels::manipulator(model, false);
Data data(model);

crba(model, data, neutral(model));
Eigen::VectorXd M_diag =
Eigen::MatrixXd(data.M.selfadjointView<Eigen::Upper>()).diagonal();

const double total_time = 6.0, dt = 0.002;
Eigen::VectorXd amp(model.nq), freq(model.nq), offset = Eigen::VectorXd::Zero(model.nq);
for (int i = 0; i < model.nq; ++i) {
amp[i] = 0.3;
freq[i] = 1.0 + 0.3 * i;
}

// ========================================================================
// A. 初值污染：phi 与 q_dot(0) 两个来源
// ========================================================================
std::cout << "【A. 初值不在轨迹上时，指标被 t=0 那一帧吃掉多少】" << std::endl;
std::cout << " " << std::left << std::setw(26) << " 情形"
<< std::setw(10) << "跳变(rad)"
<< std::setw(12) << "e_max PD" << std::setw(12) << "e_max PD+G"
<< std::setw(12) << "e_max CT" << std::setw(13) << "tau_pk PD"
<< std::setw(13) << "tau_pk CT" << "e_rms CT" << std::endl;
std::cout << " " << std::string(98, '-') << std::endl;

for (double phi_coef : {0.0, 0.2}) {
for (int vmode = 0; vmode < 2; ++vmode) {
Eigen::VectorXd phase(model.nq);
for (int i = 0; i < model.nq; ++i) phase[i] = phi_coef * i;

SineTrajectory traj(model.nq);
traj.setParameters(amp, freq, phase, offset);

Eigen::VectorXd q_start = neutral(model);
Eigen::VectorXd q_dot_start = (vmode == 0) ? traj.velocity(0.0)
: Eigen::VectorXd::Zero(model.nv);
double jump = (traj.position(0.0) – q_start).norm();

Controller ctl(model, M_diag * 36.0, M_diag * 12.0);
TorqueLaw pd = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& v,
double t) -> Eigen::VectorXd {
return ctl.pdTorque(q, v, traj.position(t), traj.velocity(t));
};
TorqueLaw pdg = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& v,
double t) -> Eigen::VectorXd {
return ctl.pdgTorque(q, v, traj.position(t), traj.velocity(t));
};
TorqueLaw ct = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& v,
double t) -> Eigen::VectorXd {
return ctl.computeTorque(q, v, traj.position(t), traj.velocity(t),
traj.acceleration(t));
};

RunResult a = simulate(model, data, traj, pd, dt, total_time, q_start, q_dot_start);
RunResult b = simulate(model, data, traj, pdg, dt, total_time, q_start, q_dot_start);
RunResult c = simulate(model, data, traj, ct, dt, total_time, q_start, q_dot_start);

std::ostringstream label;
label << "phi=" << fmt(phi_coef, 1) << "*i, "
<< (vmode == 0 ? "v0=轨迹" : "v0=0");
std::cout << " " << std::left << std::setw(26) << label.str()
<< std::setw(10) << fmt(jump, 4)
<< std::setw(12) << fmt(a.e_max, 4)
<< std::setw(12) << fmt(b.e_max, 4)
<< std::setw(12) << fmt(c.e_max, 4)
<< std::setw(13) << fmt(a.tau_peak, 1)
<< std::setw(13) << fmt(c.tau_peak, 1)
<< fmt(c.e_rms, 4) << std::endl;
}
}
std::cout << " 第一行就是本章正式程序的设置（跳变 0，误差最小的那一档）。" << std::endl;
std::cout << std::endl;

// ========================================================================
// B. 带宽 x 质量系数：两种残差各自的标度
// ========================================================================
std::cout << "【B. omega_des x 被控对象质量系数（控制律 = 计算力矩）】" << std::endl;
std::cout << " 每格是 e_rms(rad)；最右两列是力矩峰值(N*m)" << std::endl;
std::cout << " " << std::left << std::setw(8) << "omega"
<< std::setw(10) << "omega*dt"
<< std::setw(12) << "scale=1.0" << std::setw(12) << "scale=1.5"
<< std::setw(12) << "scale=2.0" << std::setw(12) << "scale=3.0"
<< std::setw(14) << "tau_pk(1.0)" << "tau_pk(3.0)" << std::endl;
std::cout << " " << std::string(92, '-') << std::endl;

Eigen::VectorXd phase0 = Eigen::VectorXd::Zero(model.nq);
SineTrajectory traj(model.nq);
traj.setParameters(amp, freq, phase0, offset);
Eigen::VectorXd q_start = neutral(model);
Eigen::VectorXd q_dot_start = traj.velocity(0.0);

for (double omega : {3.0, 6.0, 12.0, 24.0}) {
Controller ctl(model, M_diag * (omega * omega), M_diag * (2.0 * omega));
TorqueLaw ct = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& v,
double t) -> Eigen::VectorXd {
return ctl.computeTorque(q, v, traj.position(t), traj.velocity(t),
traj.acceleration(t));
};
std::cout << " " << std::left << std::setw(8) << fmt(omega, 0)
<< std::setw(10) << fmt(omega * dt, 3);
double peak1 = 0.0, peak3 = 0.0;
for (double scale : {1.0, 1.5, 2.0, 3.0}) {
Model plant(model);
Data plant_data(plant);
for (JointIndex i = 1; i < plant.njoints; ++i)
plant.inertias[i].mass() *= scale;
RunResult r = simulate(plant, plant_data, traj, ct, dt, total_time,
q_start, q_dot_start);
if (scale == 1.0) peak1 = r.tau_peak;
if (scale == 3.0) peak3 = r.tau_peak;
std::cout << std::setw(12) << (r.diverged ? "OVER" : sci(r.e_rms));
}
std::cout << std::setw(14) << fmt(peak1, 1) << fmt(peak3, 1) << std::endl;
}
std::cout << std::endl;

std::cout << " 标度检验（CT，dt=0.002）：括号里的倍数 = 本格 e_rms / omega=6 那一格" << std::endl;
const int NOMEGA = 4;
const double omegas[NOMEGA] = {3.0, 6.0, 12.0, 24.0};
for (double scale : {1.0, 3.0}) {
double e_val[NOMEGA];
for (int k = 0; k < NOMEGA; ++k) {
const double omega = omegas[k];
Controller ctl(model, M_diag * (omega * omega), M_diag * (2.0 * omega));
TorqueLaw ct = [&](const Eigen::VectorXd& q, const Eigen::VectorXd& v,
double t) -> Eigen::VectorXd {
return ctl.computeTorque(q, v, traj.position(t), traj.velocity(t),
traj.acceleration(t));
};
Model plant(model);
Data plant_data(plant);
for (JointIndex i = 1; i < plant.njoints; ++i)
plant.inertias[i].mass() *= scale;
RunResult r = simulate(plant, plant_data, traj, ct, dt, total_time,
q_start, q_dot_start);
e_val[k] = r.diverged ? –1.0 : r.e_rms;
}
std::cout << " scale=" << fmt(scale, 1) << " :";
for (int k = 0; k < NOMEGA; ++k) {
std::cout << " " << fmt(omegas[k], 0) << "->" << sci(e_val[k])
<< " (x" << fmt(e_val[k] / e_val[1], 2) << ")";
}
std::cout << std::endl;
}
return 0;
}

实测（Release，0.271 秒跑完整个探针）：

情形 跳变(rad)e_max PD e_max PD+G e_max CT tau_pk PD tau_pk CT e_rms CT
————————————————————————————————–
phi=0.0*i, v0=轨迹 0.0000 0.2619 0.1306 0.0028 53.1 42.1 0.0022
phi=0.0*i, v0=0 0.0000 0.2626 0.1471 0.0484 81.8 1236.3 0.0108
phi=0.2*i, v0=轨迹 0.3949 0.3930 0.3930 0.3930 58.4 625.7 0.0703
phi=0.2*i, v0=0 0.3949 0.4288 0.4289 0.4006 113.9 1745.5 0.0766

四条要看的：

实验二：带宽 × 模型误差，两种残差各有各的标度

同一个程序里，把设计带宽 omega_des 扫 3/6/12/24，把被控对象质量系数扫 1.0/1.5/2.0/3.0：

omega omega*dt scale=1.0 scale=1.5 scale=2.0 scale=3.0 tau_pk(1.0) tau_pk(3.0)
——————————————————————————————–
3 0.006 0.0026 0.0824 0.1852 0.4526 42.1 122.6
6 0.012 0.0022 0.0241 0.0509 0.1119 42.1 115.1
12 0.024 0.0020 0.0067 0.0133 0.0273 42.1 112.4
24 0.048 0.0019 0.0026 0.0040 0.0072 42.1 111.5

第二行 omega=6 那一整行，和正式程序【7】表的 scale 列（0.0022 / 0.0241 / 0.0509 / 0.1119）逐格相同 ✓。

看 scale=1.0 那一列：0.0026 → 0.0022 → 0.0020 → 0.0019。带宽从 3 拉到 24（8 倍），误差只动了 1.15 倍。模型准的时候，提带宽几乎不改善精度——因为剩下的残差根本不是控制律产生的，是【6】那个 ∝ dt 的积分截断误差。

看 scale=3.0 那一列：0.4526 → 0.1119 → 0.0273 → 0.0072。带宽每翻一倍，误差除以约 4（探针把倍数直接打出来：x4.05, x1.00, x0.24, x0.06）。这就是 1/ω_des² 标度律，也正是 14.2 那条推论 e ≈ dist/Kp、Kp = M·ω_des² 的直接后果：模型误差可以用带宽去压，而且压得很利索。

再验一次那个不变量：把每格乘上 omega²（手算自上面的表）：

scale=3.0 时 e_rms × omega²： 0.4526×9 = 4.07 0.1119×36 = 4.03
0.0273×144 = 3.93 0.0072×576 = 4.15

四格全在 4.0 附近（±3%）。e·Kp ≈ 常数 说的就是「力矩要把扰动顶住」这件事和带宽无关 ✓。同一算法用在 scale=1.0 列上：0.023、0.079、0.288、1.094，完全不是常数 → 那一列的主导机制不是模型扰动，是离散化。同一个表里两列、两个机制，用一个乘法检验就分开了。

最后看最右两列：tau_pk(1.0) 四行全是 42.1（模型准时，计算力矩的力矩就是逆动力学解析解，和增益无关——这是这一列本该有的样子，也是个很好的自检）；tau_pk(3.0) 是 122.6 / 115.1 / 112.4 / 111.5，基本不动甚至微降。这看着反直觉（误差降 60 倍、力矩不涨），但和上面那条不变量正好自洽：Kp·e ≈ dist 不随带宽变，所以力矩当然也不变。

那提带宽是不是万能药？ 在这张表的范围内，实测支持它：模型差 3 倍时，把带宽从 6 提到 24，e_rms 从 0.1119 降到 0.0072（比模型准确时的 PD+重力还低两个量级），力矩峰值不涨。但有三条代价表里看不到，必须说清楚：

实验三：data 这块草稿纸，和 M 的下三角

前两个实验动的是控制律，这一个动的是「怎么读 API」。它把 14.2 三条约定的实测全部量了出来：谁改写 nle/g/M、留着引用跨姿态用会错多少、忘写 selfadjointView 差多少，以及【7】里那句「Data 要不要跟着 Model 重建」的真相。

// 探针：data 这块草稿纸上，谁改了谁
// 1. nle / g / M 分别被哪些算法改写（逐个调用后对比快照）
// 2. 「不拷出来」的后果：拿引用跨一次重算去用力矩，错多少
// 3. 只改惯性数值时，用旧 Model 建的 Data 配新 Model 算得对不对
// 4. Data 里哪些东西的长度是由 Model 决定的（结构一改就错位）
// 编译：tutorials/06_project/CMakeLists.txt 里临时加一行 add_tutorial(99_probe_data_sharing)
// cmake -S . -B build && cmake –build build –config Release –target 99_probe_data_sharing
#include <iostream>
#include <iomanip>
#include <sstream>
#include <cmath>
#include <string>
#include "pinocchio/multibody.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp" // rnea, nonLinearEffects, computeGeneralizedGravity
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/multibody/sample-models.hpp"

using namespace pinocchio;

std::string sci(double x) {
std::ostringstream oss;
if (x != 0.0 && std::fabs(x) < 1e-3) {
oss << std::scientific << std::setprecision(2) << x;
} else {
oss << std::fixed << std::setprecision(4) << x;
}
return oss.str();
}

// 逐元素最大差：VectorXd 与 MatrixXd 各来一个
double maxDiff(const Eigen::VectorXd& a, const Eigen::VectorXd& b) {
return (a – b).cwiseAbs().maxCoeff();
}
double maxDiff(const Eigen::MatrixXd& a, const Eigen::MatrixXd& b) {
return (a – b).cwiseAbs().maxCoeff();
}

int main() {
Model model;
buildModels::manipulator(model, false);
Data data(model);
const int nv = model.nv;

Eigen::VectorXd q1 = neutral(model), q2 = neutral(model);
q1[0] = 0.7; q1[1] = –0.4; q1[2] = 0.3; q1[3] = 0.2;
q2[0] = –0.2; q2[1] = 0.9; q2[2] = 0.1; q2[3] = –0.5;
Eigen::VectorXd v1 = Eigen::VectorXd::Zero(nv);
v1 << 1.1, –0.6, 0.4, 0.9, –0.2, 0.3;
Eigen::VectorXd tau = Eigen::VectorXd::Zero(nv);
tau << 5.0, –3.0, 2.0, 1.0, –0.5, 0.2;

std::cout << "【1. 谁改写 data.nle / data.g / data.M】" << std::endl;
std::cout << " 基线：先算一次 nonLinearEffects(q1,v1) 与 computeGeneralizedGravity(q1)，" << std::endl;
std::cout << " 再把 nle/g/M 拷出来当快照，往下逐个调算法、比差。" << std::endl;
std::cout << " " << std::left << std::setw(30) << " 调用"
<< std::setw(14) << "nle 差" << std::setw(14) << "g 差"
<< "M 差" << std::endl;
std::cout << " " << std::string(74, '-') << std::endl;

nonLinearEffects(model, data, q1, v1);
Eigen::VectorXd nle0 = data.nle;
computeGeneralizedGravity(model, data, q1);
Eigen::VectorXd g0 = data.g;
crba(model, data, q1);
Eigen::MatrixXd M0(data.M);

auto report = [&](const std::string& name) {
std::cout << " " << std::left << std::setw(30) << (" " + name)
<< std::setw(14) << sci(maxDiff(data.nle, nle0))
<< std::setw(14) << sci(maxDiff(data.g, g0))
<< std::setw(14) << sci(maxDiff(Eigen::MatrixXd(data.M), M0))
<< std::endl;
};

crba(model, data, q1); report("crba(q1)");
rnea(model, data, q1, v1, Eigen::VectorXd::Zero(nv)); report("rnea(q1,v1,0)");
nonLinearEffects(model, data, q1, v1); report("nonLinearEffects(q1,v1)");
aba(model, data, q1, v1, tau); report("aba(q1,v1,tau)");
computeAllTerms(model, data, q1, v1); report("computeAllTerms(q1,v1)");
forwardKinematics(model, data, q2); report("forwardKinematics(q2)");
nonLinearEffects(model, data, q2, v1); report("nonLinearEffects(q2,v1)");
computeGeneralizedGravity(model, data, q2); report("computeGeneralizedGravity(q2)");

// ========================================================================
std::cout << std::endl;
std::cout << "【2. 不拷出来会怎样：把 nle 的引用留到重算之后再用】" << std::endl;
// 正确写法：算完 b 立刻拷出来，再做后面的计算
nonLinearEffects(model, data, q1, v1);
Eigen::VectorXd b_copy = data.nle;
nonLinearEffects(model, data, q2, v1); // 中间插了一次「换个姿态重算」
crba(model, data, q1);
Eigen::MatrixXd M(data.M.selfadjointView<Eigen::Upper>());
Eigen::VectorXd a_ref = Eigen::VectorXd::Ones(nv) * 0.5;
Eigen::VectorXd tau_ok = M * a_ref + b_copy; // 用拷贝：q1 的 b
Eigen::VectorXd tau_bad = M * a_ref + data.nle; // 用引用：已经是 q2 的 b
std::cout << " 正确（拷贝） tau = [" << tau_ok.transpose() << "] N·m" << std::endl;
std::cout << " 错误（引用） tau = [" << tau_bad.transpose() << "] N·m" << std::endl;
std::cout << " 最大逐元素差 = " << sci(maxDiff(tau_ok, tau_bad))
<< " N·m；力矩本身量级 " << sci(tau_ok.norm()) << " N·m（2-范数）" << std::endl;
std::cout << " 程序不会崩，也不会抱怨：它只是把两个不同姿态的量拼在了一起。" << std::endl;

// ========================================================================
std::cout << std::endl;
std::cout << "【3. 只改惯性数值时，旧 Model 建的 Data 配新 Model 算得对不对】" << std::endl;
Model plant(model);
for (JointIndex i = 1; i < plant.njoints; ++i) plant.inertias[i].mass() *= 3.0;
Data data_new(plant); // 规矩写法
Data data_old(model); // 「忘了重建」写法
Eigen::VectorXd q = q1, v = v1;
Eigen::VectorXd ddq_new = aba(plant, data_new, q, v, tau);
Eigen::VectorXd ddq_old = aba(plant, data_old, q, v, tau);
std::cout << " 重建 Data q_ddot = [" << ddq_new.transpose() << "] rad/s^2" << std::endl;
std::cout << " 复用旧 Data q_ddot = [" << ddq_old.transpose() << "] rad/s^2" << std::endl;
std::cout << " 最大逐元素差 = " << sci(maxDiff(ddq_new, ddq_old)) << std::endl;
crba(plant, data_new, q);
Eigen::MatrixXd M_new(data_new.M.selfadjointView<Eigen::Upper>());
crba(plant, data_old, q);
Eigen::MatrixXd M_old(data_old.M.selfadjointView<Eigen::Upper>());
std::cout << " 同样条件下 crba 的 M 差 = " << sci(maxDiff(M_new, M_old)) << std::endl;

// ========================================================================
std::cout << std::endl;
std::cout << "【4. Data 的尺寸全都是从 Model 抄来的（所以改结构必须重建）】" << std::endl;
Model human;
buildModels::humanoid(human, true);
Data data_human(human);
std::cout << " 机械臂 model: nq = " << model.nq << ", nv = " << model.nv
<< ", njoints = " << model.njoints << std::endl;
std::cout << " 人 形 model: nq = " << human.nq << ", nv = " << human.nv
<< ", njoints = " << human.njoints << std::endl;
std::cout << " 对应的 data.M: " << data.M.rows() << "x" << data.M.cols()
<< " 与 " << data_human.M.rows() << "x" << data_human.M.cols() << std::endl;
std::cout << " data.nle 长度: " << data.nle.size() << " 与 "
<< data_human.nle.size() << std::endl;
std::cout << " data.com 长度: " << data.com.size() << " 与 "
<< data_human.com.size() << "（= njoints，第 12 章 12.9 坑 3）" << std::endl;
std::cout << " 结论：改质量/惯性这类【数值】，Data 不必重建也算得对（见【3】的 0）；" << std::endl;
std::cout << " 改关节数、基座类型这类【结构】，上面这些长度立刻对不上，" << std::endl;
std::cout << " 旧 Data 配新 Model 属于未定义行为。所以规矩照旧：拷出 Model 就重建 Data。" << std::endl;
return 0;
}

它的关键输出已经贴在 14.2 的约定 1、约定 2 里（nle 改写表、37.5440 N·m 的引用错误）。这里补两条没贴的：

重建 Data q_ddot = [6.36759 -5.49916 3.1038 10.6886 -4.59756 -3.29019] rad/s^2
复用旧 Data q_ddot = [6.36759 -5.49916 3.1038 10.6886 -4.59756 -3.29019] rad/s^2
最大逐元素差 = 0.0000
同样条件下 crba 的 M 差 = 0.0000

plant 只被改了 inertias[i].mass()（数值），所以用旧 Model 建的 Data 去跑新 plant，结果逐位相同、误差精确为 0。因此【7】里那句「Data 必须按新模型重建」不是被实测支持的物理结论，而是一条工程规矩：Data 里每个缓存的长度都是从 Model 抄的（同一个探针打了出来——机械臂 nv=6 → M 是 6×6、nle 长 6、com 长 7；人形 nq=35/nv=34/njoints=30 → M 34×34、nle 34、com 30），改结构（关节数、基座类型）时旧 Data 一定错位，而「改结构」在你换 URDF 时天天发生。所以规矩照旧，只是要知道它防的是哪一种情况。

第三支探针量的是【5】表第一行那个 4.13 N·m，以及「忘了 selfadjointView」的具体代价（14.2 约定 1、约定 2 引用的数字都出自这里）：

// 探针：第 14 章正文里两处「想当然」的实测核对
// 1. 【5】表第一行 t=0 那个 4.13 N·m 到底是什么：科氏项还是重力项
// 2. crba 的 M 忘了对称化就拿去组力矩，差多少；下三角到底是 0 还是旧值
// 编译：tutorials/06_project/CMakeLists.txt 里临时加一行 add_tutorial(99_probe_ct_terms)
// cmake -S . -B build && cmake –build build –config Release –target 99_probe_ct_terms
#include <iostream>
#include <iomanip>
#include <sstream>
#include <cmath>
#include <string>
#include "pinocchio/multibody.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/rnea.hpp"
#include "pinocchio/algorithm/crba.hpp"
#include "pinocchio/algorithm/aba.hpp"
#include "pinocchio/algorithm/compute-all-terms.hpp"
#include "pinocchio/multibody/sample-models.hpp"

using namespace pinocchio;

std::string sci(double x) {
std::ostringstream oss;
if (x != 0.0 && std::fabs(x) < 1e-3) {
oss << std::scientific << std::setprecision(2) << x;
} else {
oss << std::fixed << std::setprecision(4) << x;
}
return oss.str();
}

double maxAbsOffDiagLower(const Eigen::MatrixXd& M) {
double worst = 0.0;
for (int r = 1; r < M.rows(); ++r)
for (int c = 0; c < r; ++c)
worst = std::max(worst, std::fabs(M(r, c)));
return worst;
}

int main() {
Model model;
buildModels::manipulator(model, false);

const int nq = model.nq;
Eigen::VectorXd amp(nq), freq(nq);
for (int i = 0; i < nq; ++i) {
amp[i] = 0.3;
freq[i] = 1.0 + 0.3 * i;
}

// ========================================================================
// 1. t = 0 那一格的力矩是谁
// ========================================================================
std::cout << "【1. 正式程序【5】表第一行 t=0 的 tau = 4.13 N·m 的来历】" << std::endl;
std::cout << " t=0 时：q = neutral（所以 e = 0）；phi = 0（所以 q_d(0)=q(0)）；" << std::endl;
std::cout << " q_ddot_des(0) = -A*omega^2*sin(0) = 0；q_dot 却已经在轨迹上、非零。" << std::endl;
std::cout << " 于是控制律里的 ref = q_ddot_des + Kp*e + Kd*e_dot = 0，tau 只剩 b(q, q_dot)。" << std::endl;

Eigen::VectorXd q0 = neutral(model);
Eigen::VectorXd v0 = Eigen::VectorXd::Zero(model.nv);
for (int i = 0; i < nq; ++i) v0[i] = amp[i] * freq[i] * std::cos(0.0); // 轨迹在 t=0 的速度
std::cout << " q_dot(0) = [" << v0.transpose() << "] rad/s" << std::endl;