《Pinocchio入门》第 12 章 质心动力学：`centerOfMass`、`h_com`，以及一张只记外力的账
本章解决一个问题：前三章算的都是「每个关节要出多大力、每个关节怎么动」，都是
nv维的账。这一章把它压缩成 6 个数——质心的平动 + 绕质心的动量矩。然后你会发现一件很反直觉的事：想让机器人「站得住」所需的那张账上，关节力矩一个都不出现，只有外力。本章用实测把这句话钉死：钉住一个姿态要花 33.63 N·m，而它对外界的动量账贡献是0.0000。先修：第 00 章（环境）、第 03 章（
Model/Data）、第 08 章（雅可比、有限差分自检）、第 09 章（Motion/Force的六维排布：线量在前）、第 10 章（M、b、上三角坑）、第 11 章（aba和那个四行仿真循环）。
本章新增的 C++ 知识：Eigen::JacobiSVD（奇异值、秩、伪逆 / 最小范数解）、有限差分步长h的 U 形误差曲线、以及「Eigen 引用包装不能进运算链」这个只在编译期出现一次的坑。
产出：tutorials/04_dynamics/03_centroidal_dynamics.cpp——全书第一份「整体账本」。附录 E 里列的下一步（动量控制、WBC、ZMP）全部以这 6 个数为入口。
12.1 整台机器人只有六个「全局」数
第 10、11 章的账都记在关节空间：rnea 给你 nv 个数（力矩），aba 给你 nv 个数（加速度）。这个空间很精确，但它回答不了一个很基本的问题：
「这台机器现在对外界总共要多少力？」
要回答它，得把 nv 个自由度合并。合并之后就只剩 6 个数：
h_com = ( m·v_com , L_com )
质心平动 绕质心的动量矩
6 不是随便挑的：质心是一个点，它的平动占 3 个分量；绕这个点的动量矩是一个旋量，占 3 个。合起来正好是一个六维空间量——和第 09 章那两个类（Motion / Force）同尺寸、同排布（线量在前，第 09 章【1】实测过）。所以 Pinocchio 把 h_com 存成 Force 类型的 data.hg。
然后是本张账的核心句子，动量定理：
d(h_com)/dt = 所有【外力】的旋量之和
外力在这个模型里只有两样：重力、以及固定基座给的那约束反力。关节力矩不在场——它是内力，只能在身体各部分之间搬运动量，改不了总量。这句话听起来像课本废话，本章程序【6】把它测成了两个数字：
| 情形 | 关节力矩 tau | d(h_com)/dt |
|---|---|---|
| A：把姿态钉住不动 | 模 33.6306 N·m（第 10 章的重力补偿值） | 六个分量全打印 0.0000 |
B：tau = 0 松手 | 0 | 线量 [-6.2970, -7.1258, -12.2672]，角量 [11.3207, -8.7952, 0.3870] |
A 行花了 33.63 N·m，账本上一个数都没动；B 行一分钱不花，账本反而在跳。 这张账只记外力，一寸不让。
和第 10 章对照着看，两套坐标的对应关系是：
| 关节空间（第 10、11 章） | 质心空间（本章） | |
|---|---|---|
| 动量 | M(q) q̇（nv 维） | h_com = Ag(q) q̇（6 维） |
| 方程 | M(q) q̈ + b(q, q̇) = tau | d(h_com)/dt = Ag(q) q̈ + dAg(q, q̇) q̇ = f_ext |
| 矩阵尺寸 | nv × nv | 6 × nv |
| 右边是什么 | 关节力矩（内力） | 只有外力 |
两行结构一模一样——「只依赖 q 的矩阵 × 加速度 + 一个二次项」——区别只在右边那张账收谁的钱。Ag 对速度是线性的这件事，程序【5】用 hg 与 Ag * v 逐位对比验过，差 2.74e-15；dhg = Ag*a + dAg*v 在【6】里差 1.13e-15。
为什么要学它：双足和四足的平衡判据（接触力、ZMP、capture point、动量反馈控制）都是从这 6 个数出发的；机械臂里它是「基座 / 螺钉要受多大劲」和碰撞瞬间冲量的算法。本章故意用固定基座机械臂演示，因为此时基座反力是这张账上唯一的未知外力，可以直接反解出来当自检——程序打印的静力平衡反力是 32.3730 N，而手算 总质量 × g = 3.300 × 9.81 = 32.373 N，一个数不差。
一句话记法：RNEA / ABA 数的是「每个关节怎么使劲」，质心动力学数的是「整台机器对外界总共要多少」。
12.2 六个 API，和它们各自往 data 里写什么
| 想算的东西 | 调用 | 结果在 | 尺寸 |
|---|---|---|---|
质心位置 r_com | centerOfMass(model, data, q) | data.com | std::vector<Vector3>，长度 njoints，下标 0 是全系统 |
质心速度 v_com | centerOfMass(model, data, q, v) | data.vcom | 同上 |
质心加速度 a_com | centerOfMass(model, data, q, v, a) | data.acom | 同上 |
| 质心雅可比 | jacobianCenterOfMass(model, data, q) | data.Jcom | 3 × nv（只有平动，没有姿态行） |
质心动量矩阵 Ag | computeCentroidalMap(model, data, q)（= ccrba(model, data, q, v)） | data.Ag | 6 × nv |
质心动量 h_com | computeCentroidalMomentum(model, data) | data.hg | Force（6 维） |
Ag 的变化率 dAg | dccrba(model, data, q, v) | data.dAg（顺手也填 data.Ag） | 6 × nv |
动量变化率 dh_com | computeCentroidalMomentumTimeVariation(model, data) | data.dhg | Force（6 维） |
读这张表要带三条约定，每一条后面都有坑：
约定 1：六维排布是「线量在前」。 data.hg.toVector() 的前三个分量是 m·v_com，后三个是绕质心的动量矩。这是第 09 章【1】量过的同一件事，本章程序里 printSpatial() 就按这个顺序打两行。
约定 2：data.com / vcom / acom 是数组，不是向量。 长度等于 model.njoints（本模型 7），com[0] 是全系统质心，com[i] 是「关节 i 往下的那棵子树」的质心。库默认就把整棵树都算出来（每个函数尾部都有 const bool computeSubtreeComs = true，见 algorithm/center-of-mass.hpp:104,138,176,199,253,279）。程序【1】里 com[0] = 1.4242 而 com[3] = 1.5161，两个数都对，是坑 3。
约定 3：带 q 的重载会自己做正运动学，不带 q 的两参数版本不会。 这不是猜测，是头文件里能读到的：impl::computeCentroidalMomentum(model, data, q, v) 的函数体就两行——forwardKinematics(model, data, q, v) 然后调两参数版本（src/algorithm/centroidal.hxx:429，动量变化率那条在 448）；而两参数版本 centroidal.hxx:18 / :63 的函数体里一个 forwardKinematics 都没有，它只从 data 里读上一次留下的状态。所以顺序必须是 先 FK 到需要的层级，再算动量。这是坑 1，程序【5】里有一行专门演示它。
还有半条约定，本章踩得最疼：Ag 不是 M。 本模型 nv = 6，所以 data.Ag 打印出来是 6 x 6，和质量矩阵长得一模一样，但一个是「动量 ← 速度」（6 × nv）、一个是「力矩 ← 加速度」（nv × nv）。程序【6】的自检 dhg = Ag*a + dAg*v 之所以能过，是因为它把 Ag 乘的是 a5（nv 维）、结果落在 6 维上；拿 Ag 去解 tau = M a + b 会算出一个六维向量冒充 nv 维力矩，编译器和库都不会拦你。
顺带一句 ccrba 和 computeCentroidalMap 的分工：ccrba 的文档原话是 “Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal momenta”（Ridig 是原文拼写错误），它要 v、顺手把 hg 和 data.M 也算了；只要 Ag 时用 computeCentroidalMap(model, data, q)。两个都会顺便算整机雅可比 data.J（文档 remark 里明说）。本章程序用的是后者。
12.3 工程里加上这一章
本章只往 tutorials/04_dynamics/CMakeLists.txt 里加一行——加完之后整份文件长这样（累积式，本章把它完整重抄一遍）：
# 教程 04：动力学
add_tutorial(01_inverse_dynamics) # 逆动力学 RNEA：运动 -> 力矩
add_tutorial(02_forward_dynamics) # 正动力学 ABA：力矩 -> 运动 + 数值积分仿真
add_tutorial(03_centroidal_dynamics) # 质心/动量相关量
这三行就是
04_dynamics子目录的最终形态，第 13 章换到tutorials/06_project去了，不会再动这份文件。改完记得重跑cmake -S . -B build（第 10 章 10.3 说过：add_tutorial()找不到对应.cpp时会在 configure 阶段就报错，比攒几行再编译好查）。
12.4 完整代码
存成 tutorials/04_dynamics/03_centroidal_dynamics.cpp。整块复制即可编译：
// ============================================================================
// 教程 04-3: 质心动力学 (Centroidal Dynamics)
// ============================================================================
//
// 【学习目标】
// 1. 会算质心位置、质心速度、质心动量 hg 和它的变化率 dhg
// 2. 理解 dhg 只由【外力】决定：关节力矩是内力，进不了这笔账
// 3. 每个解析结果都用「手工累加各刚体」或有限差分对账一次
// 4. 看懂质心雅可比为什么只有 3 行、又为什么会缺秩
//
// 【C++ 知识点】
// - 有限差分步长 h 的取舍：h 太大截断误差大，h 太小舍入误差大
// - JacobiSVD 求伪逆 / 最小范数解 / 零空间方向
// - 从 data 里取结果要先拷进局部变量（第 10 章 10.2 规则 1）
//
// 【Pinocchio 知识点】
// - centerOfMass(model, data, q[, v[, a]]) -> data.com / data.vcom / data.acom
// - jacobianCenterOfMass(model, data, q) -> 3 x nv（只有平动）
// - computeCentroidalMomentum(model, data) -> data.hg（读 data 里已算好的 v）
// - computeCentroidalMap / ccrba -> data.Ag（6 x nv，hg = Ag * v）
// - computeCentroidalMomentumTimeVariation -> data.dhg
// - dccrba -> data.dAg（dhg = Ag*a + dAg*v）
// ============================================================================
#include <iostream>
#include <iomanip>
#include <sstream>
#include <Eigen/SVD>
#include "pinocchio/multibody.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/center-of-mass.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/algorithm/joint-configuration.hpp" // integrate
#include "pinocchio/algorithm/aba.hpp" // aba：松手那一档要用
#include "pinocchio/algorithm/rnea.hpp"
#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();
}
// 固定 4 位小数打印一个 3 维向量
std::string vec3(const Eigen::Vector3d& v) {
std::ostringstream oss;
oss << std::fixed << std::setprecision(4) << "[" << v.x() << ", " << v.y()
<< ", " << v.z() << "]";
return oss.str();
}
// 把 6 维空间量按"线量 / 角量"两行打印（顺序和第 09 章的旋量一致：线量在前）
void printSpatial(const std::string& name, const Eigen::VectorXd& h) {
std::cout << " " << name << ":" << std::endl;
std::cout << " 线量部分 = " << vec3(h.head<3>()) << std::endl;
std::cout << " 角量部分 = " << vec3(h.tail<3>()) << std::endl;
}
int main() {
std::cout << "===== 教程 04-3: 质心动力学 =====" << std::endl;
std::cout << std::endl;
Model model;
buildModels::manipulator(model, false);
Data data(model);
Eigen::VectorXd q0 = neutral(model);
Eigen::VectorXd v0 = Eigen::VectorXd::Zero(model.nv);
// 一个「歪出来」的姿态：关节 1 转 45°、关节 2 转 -30°，其余不动。
// 故意让关节 3~6 全是 0，后面的缺秩讨论要用到这个「手臂是根直杆」的形状。
Eigen::VectorXd q1 = neutral(model);
q1[0] = PI<double>() / 4;
q1[1] = -PI<double>() / 6;
// ========================================================================
// 1. 质心动力学到底在算什么
// ========================================================================
std::cout << "【1. 为什么要盯质心】" << std::endl;
std::cout << " 关节空间有 nv = " << model.nv << " 维，但决定机器人「站不站得住」的" << std::endl;
std::cout << " 是整体的 6 个量：质心平动 + 绕质心的动量矩（合称质心动量 h_com）。" << std::endl;
std::cout << " 动量定理：d(h_com)/dt = 所有【外力】的旋量之和（重力、接触力、底座反力）。" << std::endl;
std::cout << " 关节力矩不在这张账上：它是内力，只能把动量在身体各部分之间来回搬运。" << std::endl;
std::cout << std::endl;
// 先手工把总质量和一阶矩累加出来——后面每一个质心量的分母都是它
double mass_total = 0.0, mom_z = 0.0;
std::cout << " 逐刚体累加（中立姿态，质心都在世界 z 轴上）：" << std::endl;
std::cout << " " << std::left << std::setw(20) << "刚体"
<< " 质量 质心 z m*z" << std::endl;
forwardKinematics(model, data, q0);
for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) { // i=0 是世界，质量恒为 0
const Inertia& Y = model.inertias[i];
// lever() 是质心在【刚体自身坐标系】里的偏移，act() 把它送到世界系
const Eigen::Vector3d com_world = data.oMi[i].act(Y.lever());
mass_total += Y.mass();
mom_z += Y.mass() * com_world.z();
std::cout << " " << std::left << std::setw(20) << model.names[i]
<< std::right << std::fixed << std::setprecision(3)
<< std::setw(8) << Y.mass() << std::setw(9) << com_world.z()
<< std::setw(9) << Y.mass() * com_world.z() << std::endl;
}
std::cout << " 总质量 = " << mass_total << " kg，一阶矩 sum(m_i*z_i) = "
<< mom_z << " kg*m" << std::endl;
std::cout << " 手算质心 z = " << mom_z << " / " << mass_total << " = "
<< std::setprecision(6) << mom_z / mass_total << " m" << std::endl;
// 中立姿态下 x/y 两个方向的质量分布是对称的（所有偏移都压在 z 轴上），
// 所以手算只需要给出 z 分量。
Eigen::Vector3d r_com_hand0(0.0, 0.0, mom_z / mass_total);
Eigen::Vector3d r_com0 = centerOfMass(model, data, q0); // 立刻拷出来
std::cout << " 库给的 = " << vec3(r_com0) << " m" << std::endl;
std::cout << " 差 = " << sci((r_com0 - r_com_hand0).norm())
<< " （质心定义就是质量加权平均，手工累加应当逐位相同）" << std::endl;
std::cout << " 注意 data.com 是个数组：长度 = " << data.com.size()
<< "，而 model.njoints = " << model.njoints
<< "，下标 0 才是全系统质心（12.9 坑 3）" << std::endl;
std::cout << " data.com[0] = " << vec3(data.com[0]) << " m（全系统）" << std::endl;
std::cout << " data.com[3] = " << vec3(data.com[3])
<< " m（关节 3 往下的子树）" << std::endl;
std::cout << std::endl;
// ========================================================================
// 2. 换个姿态再看质心
// ========================================================================
std::cout << "【2. 质心位置 r_com 随姿态走】" << std::endl;
Eigen::Vector3d r_com1 = centerOfMass(model, data, q1);
std::cout << " 中立姿态 r_com = " << vec3(r_com0) << " m，到基座的距离 = "
<< std::fixed << std::setprecision(4) << r_com0.norm() << std::endl;
std::cout << " 歪出姿态 r_com = " << vec3(r_com1) << " m，到基座的距离 = "
<< r_com1.norm() << std::endl;
std::cout << " 距离差 = " << sci((r_com1.norm() - r_com0.norm())) << std::endl;
std::cout << " （直杆绕穿过自己根部的轴转，质心只会换方向、不会换距离——" << std::endl;
std::cout << " 手臂弯起来才会改变这个距离，见【4】末尾的对照）" << std::endl;
// 通用版手算：把每个刚体的世界系质心按质量加权平均，任何姿态都成立
double m_acc = 0.0;
Eigen::Vector3d num_acc = Eigen::Vector3d::Zero();
forwardKinematics(model, data, q1);
for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) {
const Inertia& Y = model.inertias[i];
num_acc += Y.mass() * data.oMi[i].act(Y.lever());
m_acc += Y.mass();
}
Eigen::Vector3d r_com1_hand = num_acc / m_acc;
std::cout << " 歪出姿态逐刚体累加 = " << vec3(r_com1_hand) << " m，"
<< "与 centerOfMass 的差 = " << sci((r_com1 - r_com1_hand).norm()) << std::endl;
std::cout << std::endl;
// ========================================================================
// 3. 质心雅可比：v_com = J_com * q_dot
// ========================================================================
std::cout << "【3. 质心雅可比 J_com】" << std::endl;
// 注意它只有 3 行：质心是个点，只需要平动。第 08 章那个末端雅可比是 6 行。
Eigen::VectorXd v_demo = Eigen::VectorXd::Zero(model.nv);
v_demo[0] = 1.0; // 只让关节 1 以 1 rad/s 转
Eigen::MatrixXd J_com = jacobianCenterOfMass(model, data, q1);
std::cout << " J_com 尺寸 = " << J_com.rows() << " x " << J_com.cols() << std::endl;
std::cout << " J_com = " << std::endl << std::fixed << std::setprecision(4);
for (int i = 0; i < J_com.rows(); ++i) {
std::cout << " [";
for (int j = 0; j < J_com.cols(); ++j) {
if (j) std::cout << ", ";
std::cout << J_com(i, j);
}
std::cout << "]" << std::endl;
}
std::cout << " 第 3 列（关节 3）的模 = " << sci(J_com.col(2).norm())
<< " ← 整列为 0，缺秩的直接证据，【4】解释为什么" << std::endl;
Eigen::Vector3d v_com = J_com * v_demo;
std::cout << " J_com * q_dot = " << vec3(v_com) << " m/s" << std::endl;
// 库里的第二条路：centerOfMass 带 v 参数时顺手把 vcom 也算了
centerOfMass(model, data, q1, v_demo);
Eigen::Vector3d v_com_lib = data.vcom[0];
std::cout << " data.vcom[0] = " << vec3(v_com_lib) << " m/s，差 = "
<< sci((v_com - v_com_lib).norm()) << std::endl;
// 有限差分对账：真的把 q 沿 q_dot 推一小步，看质心移动多少
const double h = 1e-6;
Eigen::Vector3d r_plus = centerOfMass(model, data, integrate(model, q1, v_demo * h));
Eigen::Vector3d r_minus = centerOfMass(model, data, integrate(model, q1, -v_demo * h));
Eigen::Vector3d v_com_fd = (r_plus - r_minus) / (2.0 * h);
std::cout << " 中心差分(h=" << sci(h) << ") = " << vec3(v_com_fd) << " m/s，误差 = "
<< sci((v_com - v_com_fd).norm()) << std::endl;
std::cout << " （解析值到 1e-11 量级就停住了：再小的 h 会被减法舍入误差反噬，" << std::endl;
std::cout << " 本章 12.8 有个扫 h 的探针，把这条 U 形曲线实测出来）" << std::endl;
std::cout << std::endl;
// ========================================================================
// 4. 秩为什么缺：直杆姿态下质心只能在球面上动
// ========================================================================
std::cout << "【4. 秩缺在哪：径向方向动不了】" << std::endl;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(J_com, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Vector3d sigma = svd.singularValues();
std::cout << " 奇异值 = " << vec3(sigma) << "，秩 = "
<< svd.rank() << " / 3" << std::endl;
Eigen::Vector3d u_min = svd.matrixU().col(2); // 最小奇异值对应的左向量
std::cout << " 最小奇异值方向 u3 = " << vec3(u_min) << std::endl;
forwardKinematics(model, data, q1);
Eigen::Vector3d arm_axis = data.oMi[3].rotation().col(2); // 关节 3 的轴 = 直杆的指向
std::cout << " 手臂指向（关节 3 的轴）= " << vec3(arm_axis) << std::endl;
std::cout << " ||u3 x 手臂指向|| = " << sci(u_min.cross(arm_axis).norm())
<< " ← 两者平行：达不到的那个方向就是「沿手臂向外」的径向" << std::endl;
std::cout << " ||r_com x 手臂指向|| = " << sci(r_com1.cross(arm_axis).norm())
<< " ← 质心也确实落在这根杆的延长线上" << std::endl;
std::cout << " 直观读法：质心被锁在半径 " << r_com1.norm()
<< " m 的球面上，切向两个方向能动，法向一个方向动不了。" << std::endl;
Eigen::RowVectorXd radial = r_com1.normalized().transpose() * J_com;
std::cout << " 证据：把「沿手臂向外」的单位向量点乘 J_com 的每一列 = "
<< std::fixed << std::setprecision(4) << radial << std::endl;
std::cout << " 整列都是 " << sci(radial.norm()) << " 量级 —— "
<< "六个关节全都在切向上使劲，没有一个能拉长这根杆。" << std::endl;
Eigen::VectorXd q_bent = neutral(model);
q_bent[1] = 0.8; q_bent[3] = 0.6; // 把手臂弯起来
Eigen::MatrixXd J_bent = jacobianCenterOfMass(model, data, q_bent);
Eigen::JacobiSVD<Eigen::MatrixXd> svd_b(J_bent, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Vector3d sigma_b = svd_b.singularValues();
Eigen::Vector3d r_bent = centerOfMass(model, data, q_bent);
std::cout << " 对照：弯肘姿态 q = [0, 0.8, 0, 0.6, 0, 0]" << std::endl;
std::cout << " 奇异值 = " << vec3(sigma_b) << "，秩 = "
<< svd_b.rank() << " / 3，第 3 列的模 = " << sci(J_bent.col(2).norm()) << std::endl;
std::cout << " 质心到基座距离 = " << r_bent.norm() << " m（比 " << r_com0.norm()
<< " m 短：杆弯了，质心就往里收）" << std::endl;
std::cout << std::endl;
// ========================================================================
// 5. 质心动量 hg
// ========================================================================
std::cout << "【5. 质心动量 h_com = data.hg】" << std::endl;
// computeCentroidalMomentum 的两参数版本不接收 q/v：它读的是 data 里
// 上一次 forwardKinematics 留下的状态。所以顺序必须是 FK(q, v) -> ccm。
Eigen::VectorXd v5 = Eigen::VectorXd::Zero(model.nv);
v5[0] = 1.0;
forwardKinematics(model, data, q1, v5);
computeCentroidalMomentum(model, data);
Eigen::VectorXd h_vec = data.hg.toVector(); // 拷出来，后面还会被覆盖
printSpatial(" h_com（关节 1 以 1 rad/s 转动时）", h_vec);
// 坑演示：状态一个字没动，再调一次，结果当然一字不差
computeCentroidalMomentum(model, data);
std::cout << " 不做任何新计算、再调一次 computeCentroidalMomentum，差 = "
<< sci((data.hg.toVector() - h_vec).norm())
<< " ← 它只看 data 里的旧状态，不看你的 q 变量（12.9 坑 1）" << std::endl;
std::cout << " 线量部分应当等于 总质量 * 质心速度：" << std::endl;
std::cout << " m * v_com = " << vec3(mass_total * v_com) << std::endl;
std::cout << " 差 = " << sci((h_vec.head<3>() - mass_total * v_com).norm()) << std::endl;
// 第二条路：hg = Ag * v，Ag 是质心动量矩阵（6 x nv）
computeCentroidalMap(model, data, q1);
Eigen::MatrixXd Ag = data.Ag; // 拷出来
std::cout << " Ag 尺寸 = " << Ag.rows() << " x " << Ag.cols() << std::endl;
std::cout << " 与 Ag * v 的差 = " << sci((h_vec - Ag * v5).norm())
<< " ← 质心动量对速度是线性的，这就是那条线性映射" << std::endl;
// 手工累加：逐刚体把动量加起来，再和 hg 对账。
// 每个刚体贡献：线量 m*v_i，角量 I_i*w_i + m*(r_i - r_com) x v_i
std::cout << " 逐刚体累加 h_com（只打 x 分量：这里只有关节 1 在转，"
<< "而它的轴就是世界 x 轴，所以每个刚体的角速度都是 (1,0,0)）：" << std::endl;
std::cout << " " << std::left << std::setw(20) << "刚体"
<< std::right << " 质量 自转 x 轨道 x 小计 x" << std::endl;
forwardKinematics(model, data, q1, v5);
Eigen::Vector3d spin_total = Eigen::Vector3d::Zero();
Eigen::Vector3d orbit_total = Eigen::Vector3d::Zero();
Eigen::Vector3d lin_acc = Eigen::Vector3d::Zero();
for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) {
const Inertia& Y = model.inertias[i];
// act() 把刚体系旋量搬到世界系，同时把参考点也换成世界原点：
// 于是 .linear() 是「此刻位于世界原点那一点」的速度，
// 要求某一点 p 的速度就得用 v(p) = linear + omega x p（p 是世界坐标，不是偏移）
const Motion v_body_world = data.oMi[i].act(data.v[i]);
const Eigen::Vector3d omega = v_body_world.angular();
const Eigen::Vector3d com_world = data.oMi[i].act(Y.lever());
const Eigen::Vector3d v_point = v_body_world.linear() + omega.cross(com_world);
const Eigen::Matrix3d R = data.oMi[i].rotation();
const Eigen::Matrix3d I_com = Y.inertia(); // 绕自身质心的惯量，刚体系坐标
const Eigen::Vector3d spin = R * I_com * R.transpose() * omega;
const Eigen::Vector3d orbit = Y.mass() * (com_world - r_com1).cross(v_point);
spin_total += spin;
orbit_total += orbit;
lin_acc += Y.mass() * v_point;
std::cout << " " << std::left << std::setw(20) << model.names[i]
<< std::right << std::fixed << std::setprecision(4)
<< std::setw(9) << Y.mass() << std::setw(10) << spin.x()
<< std::setw(10) << orbit.x() << std::setw(10) << (spin + orbit).x() << std::endl;
}
const Eigen::Vector3d h_ang_acc = spin_total + orbit_total;
std::cout << " 自转项合计 = " << vec3(spin_total) << "，轨道项合计 = "
<< vec3(orbit_total) << std::endl;
std::cout << " 线量累加 = " << vec3(lin_acc) << "，hg 的线量 = " << vec3(h_vec.head<3>())
<< "，差 = " << sci((lin_acc - h_vec.head<3>()).norm()) << std::endl;
std::cout << " 角量累加 = " << vec3(h_ang_acc) << "，hg 的角量 = " << vec3(h_vec.tail<3>())
<< "，差 = " << sci((h_ang_acc - h_vec.tail<3>()).norm()) << std::endl;
std::cout << " 这三行就是 h_com 的定义：总线量 = 各刚体线量之和，"
<< "总角量 = 各刚体自转 + 绕质心公转的角动量之和。" << std::endl;
std::cout << std::endl;
// ========================================================================
// 6. 动量变化率 dhg：这张账上只有外力
// ========================================================================
std::cout << "【6. 动量变化率 d(h_com)/dt = data.dhg】" << std::endl;
// 这一层还要用到加速度，所以 forwardKinematics 必须算到 a 这一级。
Eigen::VectorXd a5 = Eigen::VectorXd::Zero(model.nv);
a5[0] = 0.5;
forwardKinematics(model, data, q1, v5, a5);
computeCentroidalMomentumTimeVariation(model, data);
Eigen::VectorXd dh_vec = data.dhg.toVector();
printSpatial(" dh_com", dh_vec);
std::cout << " 物理读法：线量部分 = 合外力，角量部分 = 绕质心的合外力矩。" << std::endl;
// 恒等式 dhg = Ag * a + dAg * v
dccrba(model, data, q1, v5);
Eigen::MatrixXd Ag2 = data.Ag, dAg = data.dAg;
Eigen::VectorXd dh_hand = Ag2 * a5 + dAg * v5;
std::cout << " 与 Ag*a + dAg*v 的差 = " << sci((dh_vec - dh_hand).norm())
<< " ← 和 M*q_ddot + b 是同一类分解，只是换了坐标" << std::endl;
// 线量部分的另一条路：合质量乘质心加速度
centerOfMass(model, data, q1, v5, a5);
Eigen::Vector3d a_com_lib = data.acom[0];
std::cout << " 线量部分 = m * a_com？ a_com = " << vec3(a_com_lib)
<< "，m*a_com = " << vec3(mass_total * a_com_lib) << std::endl;
std::cout << " 差 = " << sci((dh_vec.head<3>() - mass_total * a_com_lib).norm()) << std::endl;
// 外力只有重力和底座反力，所以底座反力可以直接反解出来
const Eigen::Vector3d mg(0.0, 0.0, mass_total * model.gravity.linear().z());
std::cout << " 动量定理：d(h_com)/dt = m*g + f_base => f_base = d(h_com)/dt - m*g" << std::endl;
std::cout << " m*g = " << vec3(mg) << " N" << std::endl;
// 情形 A：关节力矩把姿态钉住不动（v = 0, a = 0）
Eigen::VectorXd tau_hold = rnea(model, data, q1, v0, v0);
forwardKinematics(model, data, q1, v0, v0);
computeCentroidalMomentumTimeVariation(model, data);
Eigen::VectorXd dh_hold = data.dhg.toVector();
std::cout << " 情形 A 钉住不动：" << std::endl;
std::cout << " 钉住它需要的关节力矩 tau = " << std::fixed << std::setprecision(4)
<< tau_hold.transpose() << " N*m（这是第 10 章的重力补偿值）" << std::endl;
printSpatial(" dh_com", dh_hold);
std::cout << " => f_base = " << vec3(dh_hold.head<3>() - mg) << " N" << std::endl;
std::cout << " dh_com 是 0，但 tau 有 " << tau_hold.norm() << " N*m："
<< "力矩全花在内部，动量账本上看不见。" << std::endl;
// 情形 B：完全不发力矩（tau = 0），手臂在重力下甩下去
Eigen::VectorXd tau_zero = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd ddq_free = aba(model, data, q1, v0, tau_zero);
forwardKinematics(model, data, q1, v0, ddq_free);
computeCentroidalMomentumTimeVariation(model, data);
Eigen::VectorXd dh_free = data.dhg.toVector();
std::cout << " 情形 B 松手（tau = 0）：" << std::endl;
printSpatial(" dh_com", dh_free);
std::cout << " => f_base = " << vec3(dh_free.head<3>() - mg) << " N" << std::endl;
std::cout << " 既不等于 0 也不等于 -m*g：手臂一边下落一边拖底座，" << std::endl;
std::cout << " 底座反力变小了但没消失（质心也不是自由落体，它被底座拴着转）。" << std::endl;
// 自检：aba 与 rnea 互为反函数
Eigen::VectorXd tau_back = rnea(model, data, q1, v0, ddq_free);
std::cout << " 自检 ||rnea(q, v, aba(q, v, 0))|| = " << sci(tau_back.norm()) << std::endl;
std::cout << std::endl;
// ========================================================================
// 7. 想要某个质心速度，关节该怎么动
// ========================================================================
std::cout << "【7. 用伪逆解 q_dot：v_com_desired -> 关节速度】" << std::endl;
// J_com 是 3 x 6：未知数比方程多，解不唯一，取最小范数解 q_dot = J^+ * v_des
Eigen::Vector3d v_des(0.0, 0.0, 0.1);
std::cout << " 期望质心速度 = " << vec3(v_des) << " m/s（想竖直抬起来一点）" << std::endl;
Eigen::VectorXd q_dot_solved = svd.solve(v_des);
Eigen::Vector3d v_achieved = J_com * q_dot_solved;
std::cout << " 解出的关节速度 = [" << std::fixed << std::setprecision(4)
<< q_dot_solved.transpose() << "] rad/s" << std::endl;
std::cout << " 实际得到的质心速度 = " << vec3(v_achieved) << " m/s" << std::endl;
std::cout << " 残差 ||J_com*q_dot - v_des|| = " << sci((v_achieved - v_des).norm())
<< std::endl;
std::cout << " 残差不是 0：这个姿态秩只有 " << svd.rank()
<< "，径向那个方向怎么动关节都得不到，" << std::endl;
std::cout << " 伪逆只能在可达子空间里给一个「最接近」的解——这正是奇异性在质心层面的表现。" << std::endl;
std::cout << " 换弯肘姿态再试同一个期望：" << std::endl;
Eigen::VectorXd dq_bent = svd_b.solve(v_des);
std::cout << " 残差 = " << sci((J_bent * dq_bent - v_des).norm())
<< "，关节速度模 = " << dq_bent.norm() << " rad/s" << std::endl;
std::cout << std::endl;
// ========================================================================
// 8. 把质心账本挂到第 11 章的仿真循环上
// ========================================================================
std::cout << "【8. 松手 2 秒的质心账本（tau = 0，dt = 0.001 s）】" << std::endl;
Eigen::VectorXd q_snap = q1, v_snap = v0; // 记下 t = 0.400 s 那一拍，末尾对账要用
{
Eigen::VectorXd qs = q1, vs = v0;
const double dt = 0.001;
const int steps = 2000;
std::cout << " 时间(s) | 质心 z | 到基座距离 | ||h_com|| | 底座反力 z" << std::endl;
std::cout << " " << std::string(60, '-') << std::endl;
for (int step = 0; step <= steps; ++step) {
if (step % 400 == 0) {
forwardKinematics(model, data, qs, vs);
computeCentroidalMomentum(model, data);
Eigen::VectorXd hs = data.hg.toVector();
Eigen::VectorXd tau_now = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd ddq_now = aba(model, data, qs, vs, tau_now);
forwardKinematics(model, data, qs, vs, ddq_now);
computeCentroidalMomentumTimeVariation(model, data);
Eigen::VectorXd dhs = data.dhg.toVector();
Eigen::Vector3d rs = centerOfMass(model, data, qs);
std::cout << " " << std::fixed << std::setprecision(3) << std::setw(6)
<< step * dt << " | " << std::setw(8) << rs.z()
<< " | " << std::setw(10) << rs.norm()
<< " | " << std::setw(10) << hs.norm()
<< " | " << std::setw(10) << (dhs.head<3>() - mg).z() << std::endl;
if (step == 400) { q_snap = qs; v_snap = vs; }
}
if (step == steps) {
std::cout << " 末拍关节角 q = [" << std::fixed << std::setprecision(4)
<< qs.transpose() << "] rad" << std::endl;
break;
}
Eigen::VectorXd tau_now = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd ddq_now = aba(model, data, qs, vs, tau_now);
vs += ddq_now * dt;
qs = integrate(model, qs, vs * dt);
}
}
std::cout << " 读表：这不是「单调下落」，而是摆动。手臂是一根被底座拴着的复摆：" << std::endl;
std::cout << " 质心先掉到基座以下（t = 1.200 s 时 z = -1.323 m），再摆回 0.328 m；" << std::endl;
std::cout << " 底座反力在 11.4 ~ 71.9 N 之间来回，因为它等于 m*(a_com - g)，" << std::endl;
std::cout << " 而 a_com 每一拍都在换方向。只有第一行（t = 0）和【6】的情形 B 是同一瞬间。" << std::endl;
std::cout << " 另一列也在说话：到基座的距离从 1.424 缩到 1.181——" << std::endl;
std::cout << " 【4】里那个「距离被锁死」的前提已经不成立了，重力把关节 3~6 也带着动起来，" << std::endl;
std::cout << " 手臂不再是直杆。" << std::endl;
// 最后一项对账：dh_com 是不是真的等于 h_com 沿真实轨迹的时间导数
std::cout << " 拿 t = 0.400 s 那一拍做差分对账（解析 dh_com vs 往前推一拍再算 h_com）：" << std::endl;
Eigen::VectorXd ddq_snap = aba(model, data, q_snap, v_snap, tau_zero);
forwardKinematics(model, data, q_snap, v_snap, ddq_snap);
computeCentroidalMomentumTimeVariation(model, data);
Eigen::VectorXd dh_analytic = data.dhg.toVector();
forwardKinematics(model, data, q_snap, v_snap);
computeCentroidalMomentum(model, data);
Eigen::VectorXd h_now = data.hg.toVector();
for (int k = 0; k < 2; ++k) {
const double dt_fd = (k == 0) ? 1e-3 : 1e-4;
Eigen::VectorXd v_plus = v_snap + ddq_snap * dt_fd;
Eigen::VectorXd q_plus = integrate(model, q_snap, v_plus * dt_fd);
forwardKinematics(model, data, q_plus, v_plus);
computeCentroidalMomentum(model, data);
Eigen::VectorXd h_plus = data.hg.toVector();
std::cout << " 前推 dt = " << sci(dt_fd) << "，差 = "
<< sci(((h_plus - h_now) / dt_fd - dh_analytic).norm()) << std::endl;
}
std::cout << " 步长缩小 10 倍、误差也缩小约 10 倍——一阶欧拉的特征，第 11 章 11.2 说过。" << std::endl;
std::cout << std::endl;
std::cout << "教程 04-3 完成！你已经掌握了质心动力学。" << std::endl;
std::cout << "下一章开始综合实战：关节空间轨迹规划与 PD 控制。" << std::endl;
return 0;
}
12.5 编译并运行
cmake -S . -B build -G "Visual Studio 17 2022" -A x64
cmake --build build --config Release --target 03_centroidal_dynamics
build\bin\03_centroidal_dynamics.exe
Git-Bash 里同样两条 cmake，运行改成 ./build/bin/03_centroidal_dynamics.exe。
12.6 期望输出（实测快照）
Pinocchio 4.1.0 + Eigen 5.0.1 + MSVC v143 + /utf-8，Release 实测，退出码 0：
===== 教程 04-3: 质心动力学 =====
【1. 为什么要盯质心】
关节空间有 nv = 6 维，但决定机器人「站不站得住」的
是整体的 6 个量：质心平动 + 绕质心的动量矩（合称质心动量 h_com）。
动量定理：d(h_com)/dt = 所有【外力】的旋量之和（重力、接触力、底座反力）。
关节力矩不在这张账上：它是内力，只能把动量在身体各部分之间来回搬运。
逐刚体累加（中立姿态，质心都在世界 z 轴上）：
刚体 质量 质心 z m*z
shoulder1_joint 0.100 0.000 0.000
shoulder2_joint 0.100 0.000 0.000
shoulder3_joint 1.000 0.500 0.500
elbow_joint 1.000 1.500 1.500
wrist1_joint 0.100 2.000 0.200
wrist2_joint 1.000 2.500 2.500
总质量 = 3.300 kg，一阶矩 sum(m_i*z_i) = 4.700 kg*m
手算质心 z = 4.700 / 3.300 = 1.424242 m
库给的 = [0.0000, 0.0000, 1.4242] m
差 = 0.00e+00 （质心定义就是质量加权平均，手工累加应当逐位相同）
注意 data.com 是个数组：长度 = 7，而 model.njoints = 7，下标 0 才是全系统质心（12.9 坑 3）
data.com[0] = [0.0000, 0.0000, 1.4242] m（全系统）
data.com[3] = [0.0000, 0.0000, 1.5161] m（关节 3 往下的子树）
【2. 质心位置 r_com 随姿态走】
中立姿态 r_com = [0.0000, 0.0000, 1.4242] m，到基座的距离 = 1.4242
歪出姿态 r_com = [-0.7121, -0.8722, 0.8722] m，到基座的距离 = 1.4242
距离差 = 0.00e+00
（直杆绕穿过自己根部的轴转，质心只会换方向、不会换距离——
手臂弯起来才会改变这个距离，见【4】末尾的对照）
歪出姿态逐刚体累加 = [-0.7121, -0.8722, 0.8722] m，与 centerOfMass 的差 = 1.57e-16
【3. 质心雅可比 J_com】
J_com 尺寸 = 3 x 6
J_com =
[0.0000, 1.2334, 0.0000, 0.5511, 0.0000, 0.1312]
[-0.8722, -0.5035, 0.0000, -0.2250, -0.1071, -0.0536]
[-0.8722, 0.5035, 0.0000, 0.2250, -0.1071, 0.0536]
第 3 列（关节 3）的模 = 9.52e-17 ← 整列为 0，缺秩的直接证据，【4】解释为什么
J_com * q_dot = [0.0000, -0.8722, -0.8722] m/s
data.vcom[0] = [0.0000, -0.8722, -0.8722] m/s，差 = 1.57e-16
中心差分(h=1.00e-06) = [0.0000, -0.8722, -0.8722] m/s，误差 = 4.08e-11
（解析值到 1e-11 量级就停住了：再小的 h 会被减法舍入误差反噬，
本章 12.8 有个扫 h 的探针，把这条 U 形曲线实测出来）
【4. 秩缺在哪：径向方向动不了】
奇异值 = [1.5673, 1.2427, 0.0000]，秩 = 2 / 3
最小奇异值方向 u3 = [-0.5000, -0.6124, 0.6124]
手臂指向（关节 3 的轴）= [-0.5000, -0.6124, 0.6124]
||u3 x 手臂指向|| = 2.78e-16 ← 两者平行：达不到的那个方向就是「沿手臂向外」的径向
||r_com x 手臂指向|| = 0.00e+00 ← 质心也确实落在这根杆的延长线上
直观读法：质心被锁在半径 1.4242 m 的球面上，切向两个方向能动，法向一个方向动不了。
证据：把「沿手臂向外」的单位向量点乘 J_com 的每一列 = 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
整列都是 2.78e-17 量级 —— 六个关节全都在切向上使劲，没有一个能拉长这根杆。
对照：弯肘姿态 q = [0, 0.8, 0, 0.6, 0, 0]
奇异值 = [1.4978, 0.7641, 0.1943]，秩 = 3 / 3，第 3 列的模 = 3.59e-01
质心到基座距离 = 1.3614 m（比 1.4242 m 短：杆弯了，质心就往里收）
【5. 质心动量 h_com = data.hg】
h_com（关节 1 以 1 rad/s 转动时）:
线量部分 = [0.0000, -2.8782, -2.8782]
角量部分 = [4.8720, -0.7520, 0.7520]
不做任何新计算、再调一次 computeCentroidalMomentum，差 = 0.00e+00 ← 它只看 data 里的旧状态，不看你的 q 变量（12.9 坑 1）
线量部分应当等于 总质量 * 质心速度：
m * v_com = [0.0000, -2.8782, -2.8782]
差 = 6.28e-16
Ag 尺寸 = 6 x 6
与 Ag * v 的差 = 2.74e-15 ← 质心动量对速度是线性的，这就是那条线性映射
逐刚体累加 h_com（只打 x 分量：这里只有关节 1 在转，而它的轴就是世界 x 轴，所以每个刚体的角速度都是 (1,0,0)）：
刚体 质量 自转 x 轨道 x 小计 x
shoulder1_joint 0.1000 0.0100 0.0000 0.0100
shoulder2_joint 0.1000 0.0100 0.0000 0.0100
shoulder3_joint 1.0000 1.0000 -0.3466 0.6534
elbow_joint 1.0000 1.0000 0.0852 1.0852
wrist1_joint 0.1000 0.0100 0.0864 0.0964
wrist2_joint 1.0000 1.0000 2.0170 3.0170
自转项合计 = [3.0300, 0.0000, -0.0000]，轨道项合计 = [1.8420, -0.7520, 0.7520]
线量累加 = [0.0000, -2.8782, -2.8782]，hg 的线量 = [0.0000, -2.8782, -2.8782]，差 = 6.28e-16
角量累加 = [4.8720, -0.7520, 0.7520]，hg 的角量 = [4.8720, -0.7520, 0.7520]，差 = 2.71e-15
这三行就是 h_com 的定义：总线量 = 各刚体线量之和，总角量 = 各刚体自转 + 绕质心公转的角动量之和。
【6. 动量变化率 d(h_com)/dt = data.dhg】
dh_com:
线量部分 = [-0.0000, 1.4391, -4.3172]
角量部分 = [2.4360, -1.1280, -0.3760]
物理读法：线量部分 = 合外力，角量部分 = 绕质心的合外力矩。
与 Ag*a + dAg*v 的差 = 1.13e-15 ← 和 M*q_ddot + b 是同一类分解，只是换了坐标
线量部分 = m * a_com？ a_com = [-0.0000, 0.4361, -1.3083]，m*a_com = [-0.0000, 1.4391, -4.3172]
差 = 2.47e-32
动量定理：d(h_com)/dt = m*g + f_base => f_base = d(h_com)/dt - m*g
m*g = [0.0000, 0.0000, -32.3730] N
情形 A 钉住不动：
钉住它需要的关节力矩 tau = -28.2347 16.3013 0.0000 7.2836 -3.4684 1.7342 N*m（这是第 10 章的重力补偿值）
dh_com:
线量部分 = [0.0000, 0.0000, 0.0000]
角量部分 = [0.0000, 0.0000, 0.0000]
=> f_base = [0.0000, 0.0000, 32.3730] N
dh_com 是 0，但 tau 有 33.6306 N*m：力矩全花在内部，动量账本上看不见。
情形 B 松手（tau = 0）：
dh_com:
线量部分 = [-6.2970, -7.1258, -12.2672]
角量部分 = [11.3207, -8.7952, 0.3870]
=> f_base = [-6.2970, -7.1258, 20.1058] N
既不等于 0 也不等于 -m*g：手臂一边下落一边拖底座，
底座反力变小了但没消失（质心也不是自由落体，它被底座拴着转）。
自检 ||rnea(q, v, aba(q, v, 0))|| = 7.51e-15
【7. 用伪逆解 q_dot：v_com_desired -> 关节速度】
期望质心速度 = [0.0000, 0.0000, 0.1000] m/s（想竖直抬起来一点）
解出的关节速度 = [-0.0565 0.0205 0.0000 0.0092 -0.0069 0.0022] rad/s
实际得到的质心速度 = [0.0306, 0.0375, 0.0625] m/s
残差 ||J_com*q_dot - v_des|| = 6.12e-02
残差不是 0：这个姿态秩只有 2，径向那个方向怎么动关节都得不到，
伪逆只能在可达子空间里给一个「最接近」的解——这正是奇异性在质心层面的表现。
换弯肘姿态再试同一个期望：
残差 = 2.94e-17，关节速度模 = 0.2290 rad/s
【8. 松手 2 秒的质心账本（tau = 0，dt = 0.001 s）】
时间(s) | 质心 z | 到基座距离 | ||h_com|| | 底座反力 z
------------------------------------------------------------
0.000 | 0.872 | 1.424 | 0.000 | 20.106
0.400 | 0.531 | 1.416 | 8.988 | 11.417
0.800 | -0.674 | 1.336 | 19.901 | 42.245
1.200 | -1.323 | 1.374 | 24.113 | 71.875
1.600 | -0.389 | 1.356 | 16.808 | 23.730
2.000 | 0.328 | 1.181 | 7.860 | 14.015
末拍关节角 q = [ 4.7921 0.7381 0.7794 -0.4462 2.3881 -1.2880] rad
读表：这不是「单调下落」，而是摆动。手臂是一根被底座拴着的复摆：
质心先掉到基座以下（t = 1.200 s 时 z = -1.323 m），再摆回 0.328 m；
底座反力在 11.4 ~ 71.9 N 之间来回，因为它等于 m*(a_com - g)，
而 a_com 每一拍都在换方向。只有第一行（t = 0）和【6】的情形 B 是同一瞬间。
另一列也在说话：到基座的距离从 1.424 缩到 1.181——
【4】里那个「距离被锁死」的前提已经不成立了，重力把关节 3~6 也带着动起来，
手臂不再是直杆。
拿 t = 0.400 s 那一拍做差分对账（解析 dh_com vs 往前推一拍再算 h_com）：
前推 dt = 1.00e-03，差 = 1.37e-01
前推 dt = 1.00e-04，差 = 1.37e-02
步长缩小 10 倍、误差也缩小约 10 倍——一阶欧拉的特征，第 11 章 11.2 说过。
教程 04-3 完成！你已经掌握了质心动力学。
下一章开始综合实战：关节空间轨迹规划与 PD 控制。
12.7 逐段读懂这份输出
【1】质心就是质量加权平均——先把分母算出来
程序把六个刚体的质量、质心高度、m·z 列成表，然后打印
总质量 = 3.300 kg，一阶矩 sum(m_i*z_i) = 4.700 kg*m
手算质心 z = 4.700 / 3.300 = 1.424242 m
库给的 = [0.0000, 0.0000, 1.4242] m
差 = 0.00e+00
4.7 / 3.3 你拿计算器按也是 1.424242...。中立姿态下所有偏移都压在世界 z 轴上（第 05 章打印过这条链的 lever 全是 (0,0,*)），所以 x、y 两个方向的质量分布天生对称，手算只需要给 z 分量。差恰好是 0.00e+00——不是「小到看不见」，是逐位相同：这就是质心的定义，库和你的手算走的是同一条公式，没有任何会产生差别的环节。这个 3.300 和 4.700 后面还要反复出现：【5】里乘质心速度、【6】里乘重力、【8】里算质心加速度，用的都是它们。
同一张表里那两行 data.com[0] / data.com[3] 值得多看一眼：
data.com[0] = [0.0000, 0.0000, 1.4242] m（全系统）
data.com[3] = [0.0000, 0.0000, 1.5161] m（关节 3 往下的子树）
子树质心是「关节 3 以下那四块」的加权平均：质量 1.0 + 1.0 + 0.1 + 1.0 = 3.1，一阶矩 0.5 + 1.5 + 0.2 + 2.5 = 4.7，4.7 / 3.1 = 1.51613 ✓ 和打印的 1.5161 对上。分子居然也是 4.700：因为被排除的那两块（两个 0.1 kg 的肩块）质心高度是 0.000，对一阶矩的贡献本来就是零——它们只出现在分母里（3.300 - 0.200 = 3.100）。这不是巧合，是这条链的几何给的；换成 URDF 里的真实机械臂就不会这么巧了。
【2】换个姿态：质心只换方向，不换距离
程序把关节 1 转 45°、关节 2 转 −30°，其余保持 0，打印
中立姿态 r_com = [0.0000, 0.0000, 1.4242] m，到基座的距离 = 1.4242
歪出姿态 r_com = [-0.7121, -0.8722, 0.8722] m，到基座的距离 = 1.4242
距离差 = 0.00e+00
这两个数可以完全手推。 关节 3~6 都是 0，所以手臂是一根沿各自局部 ẑ 的直杆；姿态变换是 R_x(45°) · R_y(−30°)，杆的指向就是
R_y(−30°)·ẑ = (−0.5, 0, √3/2)
R_x(45°)·(−0.5, 0, √3/2) = (−0.5, −√2/2·√3/2, +√2/2·√3/2) = (−0.5, −√6/4, +√6/4)
= (−0.5000, −0.6124, +0.6124) ← 就是【4】打印的「手臂指向」
乘上长度 1.424242：1.424242 × 0.5 = 0.712121、1.424242 × 0.612372 = 0.872167 ⇒ r_com = (−0.7121, −0.8722, +0.8722) ✓ 和打印逐位相同。而距离当然不变：绕穿过原点的轴转是正交变换，不改变向量长度——距离差 = 0.00e+00 就是这个性质的测量结果。
这里要提前打一个招呼：这个「距离被锁死」是直杆给的礼物，不是质心的性质。【4】末尾弯肘之后距离就变了（1.3614），【8】摆动 2 秒之后变成 1.181。凡是「因为某个几何前提才成立」的结论，前提一破结论就失效——本章后面凡是这种性质的话都会写明前提。
程序接着用「通用版手算」（逐刚体把世界系质心加权平均，任何姿态都成立）再对一次，差 1.57e-16。
【3】J_com 只有 3 行，而且每一列都能手算
打印出来的 J_com 是 3 x 6：
[0.0000, 1.2334, 0.0000, 0.5511, 0.0000, 0.1312]
[-0.8722, -0.5035, 0.0000, -0.2250, -0.1071, -0.0536]
[-0.8722, 0.5035, 0.0000, 0.2250, -0.1071, 0.0536]
只有 3 行是因为质心是一个点，没有姿态。第 08 章那个末端雅可比是 6 行，因为它要同时管末端的位置和朝向；J_com 没有下半部分可用。
转轴关节的雅可比列有现成的手算公式：第 j 列 = ẑ_j × (r_com − p_j)（ẑ_j 是该关节轴在世界系的方向，p_j 是它的原点世界坐标）。逐列核对：
- 第 1 列（关节 1，RX，轴 = 世界
x̂，原点在世界原点）：
x̂ × (−0.7121, −0.8722, 0.8722) = (0, −0.8722, −0.8722)✓ 打印第一列正是它。 - 第 2 列（关节 2，RY）：
ẑ₂ = R_x(45°)·R_y(−30°)·ŷ = (0, √2/2, √2/2)，原点是0。
ẑ₂ × r_com = (0.7071×0.8722 + 0.7071×0.8722, −0.7071×0.7121, +0.7071×0.7121) = (1.2334, −0.5035, +0.5035)✓ 打印1.2334 / -0.5035 / 0.5035。 - 第 3 列整列为 0（打印「第 3 列（关节 3）的模 = 9.52e-17」）：关节 3 是 RZ，它的轴就是这根直杆的指向
a = (−0.5, −0.6124, 0.6124)，而它的原点也在世界原点（第 11 章【7】那张表里肩三联件的关节原点全在原点）。于是ẑ₃ × (r_com − 0) = a × (1.4242·a) = 0——叉乘两个平行向量必然为零。这一列的 0 和【4】的缺秩是同一件事的两面。
第 4~6 列要每个关节自己的世界原点，p_j 得沿 liMi 链推（第 07 章），本章不手推它们；公式已经给了，你拿【1】那张表的质量/高度数据加上第 05 章打印的关节原点，可以一列一列自己复算。
然后是同一个 v_com 的三条独立路线，程序把三条都打印出来：
J_com * q_dot = [0.0000, -0.8722, -0.8722] m/s
data.vcom[0] = [0.0000, -0.8722, -0.8722] m/s，差 = 1.57e-16
中心差分(h=1.00e-06) = [0.0000, -0.8722, -0.8722] m/s，误差 = 4.08e-11
第一条是「雅可比 × 速度」，第二条是库自己那条 centerOfMass(q, v) 的路（它写 data.vcom），第三条最笨也最硬：真的把 q 沿 q̇ 推 ±1e-6，看质心移动多少再除回来。前两条差到 1e-16，第三条停在 4.08e-11——这不是 bug，是差分法本身的天花板，12.8 有一个专门扫 h 的探针把这个上限实测出来（而且探针独立跑出的 h=1e-6 误差是 4.083e-11，和这里的 4.08e-11 是同一个数）。
顺便注意第三行用的是 integrate(model, q1, v_demo * h) 而不是 q1 + v_demo * h：第 11 章坑 1 讲过位形不是普通向量。这个模型六个关节全是转轴，nq == nv，两种写法逐位相同，所以这次用哪个都一样——但习惯要从这里开始养。
【4】秩缺在哪：质心被锁在一个球面上
奇异值 = [1.5673, 1.2427, 0.0000]，秩 = 2 / 3
最小奇异值方向 u3 = [-0.5000, -0.6124, 0.6124]
手臂指向（关节 3 的轴）= [-0.5000, -0.6124, 0.6124]
||u3 x 手臂指向|| = 2.78e-16 ← 两者平行
||r_com x 手臂指向|| = 0.00e+00 ← 质心也确实落在这根杆的延长线上
J_com 是 3 × 6，行不满秩，说明有三个平动方向里有一个是动不了的。SVD 把「动不了的那个方向」直接给了出来：最小奇异值对应的左向量 u3，实测和手臂指向平行（叉乘模 2.78e-16）。
物理读法：r_com × a = 0 说明质心落在这根直杆的延长线上，离基座 1.4242 m；而每一个关节能做的都只是「让杆转」或「让杆上的块沿杆摆」，没有任何一个属性能改变杆的长度（直杆 + 全零的关节 3~6）。于是质心只能在半径 1.4242 m 的球面上动：切向两个方向可以，法向（沿杆向外）那一个不可以。
程序把这句话验证到底：拿「沿手臂向外」的单位向量去点乘 J_com 的每一列，
证据：把「沿手臂向外」的单位向量点乘 J_com 的每一列 = 0.0000 0.0000 0.0000 0.0000 0.0000 0.0000
整列都是 2.78e-17 量级 —— 六个关节全都在切向上使劲，没有一个能拉长这根杆。
六个关节全都试过了，全在切向上。这就是「质心层面的奇异性」的几何形状——不是某个关节到极限，而是整体几何少了一个方向。
最后对照弯肘姿态 q = [0, 0.8, 0, 0.6, 0, 0]：
奇异值 = [1.4978, 0.7641, 0.1943]，秩 = 3 / 3，第 3 列的模 = 3.59e-01
质心到基座距离 = 1.3614 m（比 1.4242 m 短：杆弯了，质心就往里收）
三个奇异值全都非零、第三列从 9.52e-17 变成 0.359、距离从 1.4242 缩到 1.3614。「直杆」这个前提一撤，整节【4】的结论同时失效——这是本章最想让你练的直觉：结论要连着前提一起记。
【5】h_com：一个六维向量，可以用两种方式手算出来
h_com（关节 1 以 1 rad/s 转动时）:
线量部分 = [0.0000, -2.8782, -2.8782]
角量部分 = [4.8720, -0.7520, 0.7520]
线量部分是送分题：m · v_com = 3.300 × (0, −0.8722, −0.8722) = (0, −2.8782, −2.8782)（3.3 × 0.872167 = 2.87815）✓ 实测差 6.28e-16。
第二条路是 hg = Ag · v，Ag 尺寸打印为 6 x 6（因为 nv = 6，和 M 同形不同义，见 12.2 最后半条约定），差 2.74e-15。
角量部分才是这节的重点，程序逐刚体把每个块的两项加起来：
刚体 质量 自转 x 轨道 x 小计 x
shoulder1_joint 0.1000 0.0100 0.0000 0.0100
shoulder2_joint 0.1000 0.0100 0.0000 0.0100
shoulder3_joint 1.0000 1.0000 -0.3466 0.6534
elbow_joint 1.0000 1.0000 0.0852 1.0852
wrist1_joint 0.1000 0.0100 0.0864 0.0964
wrist2_joint 1.0000 1.0000 2.0170 3.0170
自转项合计 = [3.0300, 0.0000, -0.0000]，轨道项合计 = [1.8420, -0.7520, 0.7520]
自转项那一列为什么全是「质量 × 惯量系数」这么整齐：这个示例模型六个刚体的惯量张量都是各向同性的——三块 0.01·I₃、三块 1.0·I₃（第 05 章【7】逐个打印过，第 10 章又列了一次）。对 I = c·I₃ 有 R·(c I₃)·Rᵀ = c I₃，转到世界系根本不用转，于是一个刚体的自转角动量就是 c_i · ω。而关节 1 的轴正好是世界 x̂、转速 1 rad/s，所以六个刚体的 ω 全都是 (1,0,0)：
自转合计 = Σ c_i · ω = (0.01×3 + 1.0×3) · (1,0,0) = 3.03 · x̂ ✓ 打印 [3.0300, 0.0000, -0.0000]
顺带解释那个 -0.0000：z 分量的真值是 −0 或一个负号的小数（第 09 章 9.16【7】同款格式化现象），不是 bug。
轨道项可以整条手推，用的就是【1】那张表。记直杆指向的单位向量 a = (−0.5, −0.6124, 0.6124)，第 i 块离基座的距离是 d_i（就是【1】里的「质心 z」列：0, 0, 0.5, 1.5, 2.0, 2.5），质心距离 R = 1.424242，角速度 ω = x̂。第 i 块的速度 v_i = ω × r_i = d_i·(x̂ × a)，于是
L_orb = Σ m_i (r_i − R·a) × v_i = [Σ m_i d_i (d_i − R)] · (a × (x̂ × a))
两个因子各自都能算。前面那个：
Σ m_i d_i² − R·Σ m_i d_i = 9.15 − 1.424242 × 4.700 = 9.15 − 6.69394 = 2.45606
（9.15 = 0.25 + 2.25 + 0.4 + 6.25，全从【1】表里的 z 列平方乘质量来）
后面那个用二重叉乘公式 a × (x̂ × a) = x̂(a·a) − a(a·x̂) = x̂ + 0.5a = (0.75, −0.3062, +0.3062)。相乘：
2.45606 × (0.75, −0.3062, 0.3062) = (1.8420, −0.7520, +0.7520) ✓ 打印轨道项合计，逐位相同
再合上自转项：3.0300 + 1.8420 = 4.8720 ✓ 正是 hg 角量的 x 分量。整份 h_com 六个数到这里全部由【1】那张质量表推出来了，没有一个数需要背。
最后三行的对账分别是线量累加差 6.28e-16、角量累加差 2.71e-15。这个「先分自转/轨道再各自累加」的写法本章踩过一次参考点的坑（算出来是 2.03 而不是 2.7e-15），12.9 坑 4 有完整实录。
中间还有一行专门用来演坑 1：
不做任何新计算、再调一次 computeCentroidalMomentum，差 = 0.00e+00 ← 它只看 data 里的旧状态，不看你的 q 变量
【6】dh_com：这张账上只有外力，所以能反解基座反力
dh_com:
线量部分 = [-0.0000, 1.4391, -4.3172]
角量部分 = [2.4360, -1.1280, -0.3760]
三条自检同时过：和 Ag*a + dAg*v 差 1.13e-15（dccrba 顺手把 data.Ag 也填好了，所以这一行只用一个函数就能凑齐两个矩阵）；和 m · a_com 差 2.47e-32——比别的自检低十几个数量级，因为这两行不是两条独立算法，就是同一个量的两种写法（data.acom[0] 打印是 [-0.0000, 0.4361, -1.3083]，乘 3.3 就是上面那行线量）。
然后把外力拆开。这个模型的外力只有重力和基座约束反力，所以动量定理就是
d(h_com)/dt = m·g + f_base => f_base = d(h_com)/dt − m·g
m·g = [0.0000, 0.0000, -32.3730] N （3.300 × 9.81 = 32.373，手算）
情形 A：钉住不动。 程序先用 rnea(q, 0, 0) 算出钉住这个姿态要的关节力矩——就是第 10 章的重力补偿值：
tau = -28.2347 16.3013 0.0000 7.2836 -3.4684 1.7342 N*m
dh_com 的六个分量全是 0.0000
=> f_base = [0.0000, 0.0000, 32.3730] N
这一屏是本节的全部要点：tau 的模是 33.6306 N·m（√(28.2347² + 16.3013² + 7.2836² + 3.4684² + 1.7342²)，可以拿计算器复核），六个关节在拼命使劲；而 dh_com 全零，动量账本上什么都没发生。唯一的记录是基座反力 32.3730 N 向上——恰好等于全机重量，这就是静力平衡，一个数不多一个数不少。内力矩不进账，进了账的全是外力。
情形 B：tau = 0 松手。 用第 11 章的 aba 拿到自由下落的 q̈，再算 dh_com：
dh_com 线量 = [-6.2970, -7.1258, -12.2672]
=> f_base = [-6.2970, -7.1258, 20.1058] N
20.1058 = 32.3730 − 12.2672 ✓。它既不是 0（钉住时那样）也不是 −m·g（自由落体那样）：手臂一边往下甩一边拖着基座，基座还在承担大部分重量。把线量除以 3.300 就是质心加速度（手算自实测值）：
a_com = (−1.908, −2.159, −3.717) m/s²
z 方向只掉到重力加速度的 38%——质心不是自由落体，它被基座拴着。这也解释了为什么这行的角量部分非零（[11.3207, −8.7952, 0.3870]）：绕质心的合外力矩不为零，重力正在把这个身体拧着转。
最后一行是第 11 章那条反函数自检在这里再跑一次：||rnea(q, v, aba(q, v, 0))|| = 7.51e-15。
【7】想要某个质心速度，关节该怎么动：秩缺的时候伪逆不认账
J_com 是 3 × 6，未知数比方程多 ⇒ 解不唯一 ⇒ 取最小范数解 q̇ = J_com⁺ v_des（svd.solve() 给的就是这个）。程序发一个「竖直抬起质心 0.1 m/s」的指令：
期望质心速度 = [0.0000, 0.0000, 0.1000] m/s（想竖直抬起来一点）
解出的关节速度 = [-0.0565 0.0205 0.0000 0.0092 -0.0069 0.0022] rad/s
实际得到的质心速度 = [0.0306, 0.0375, 0.0625] m/s
残差 ||J_com*q_dot - v_des|| = 6.12e-02
残差 6.12e-02 是 61% 的指令没实现，而它完全可以手算。秩缺的那个方向是 u3 = (−0.5, −0.6124, 0.6124)，最小二乘的残差必然正好落在不可达方向上（这是投影定理，不是本模型的特性）：
残差的模 = |u3 · v_des| = 0.6124 × 0.1 = 0.06124 ✓ 实测 6.12e-02
实现的 = v_des − (u3·v_des)·u3
= (0,0,0.1) − 0.061237×(−0.5, −0.6124, 0.6124)
= (0.0306, 0.0375, 0.0625) ✓ 和打印逐位相同
于是竖直方向只实现了 0.0625 / 0.1 = 62.5%——而且这个数是精确的 5/8：u3 的 z 分量平方是 (√6/4)² = 3/8，所以 z 方向损失 3/8、留下 5/8。
程序里解出的六个关节速度中第 3 个恰好是 0.0000：关节 3 白给也没用，它那一整列都是 9.52e-17（【3】【4】），最小范数解当然不会用它。这是「奇异性在优化解里的表现」——不是报错，是那一列自动失效。
换满秩的弯肘姿态再发同一个指令，残差 2.94e-17、关节速度模 0.2290 rad/s：同一个目标，同一个函数，换个姿态就从「实现 62.5%」变成「全部实现」。
工程结论：质心/动量层面的控制命令不能对着奇异姿态发。第 08 章讲的是末端位姿的奇异性，这一章是它的整体版，判据更直观——看 J_com 最小的那个奇异值是不是 0。
【8】把账本挂到第 11 章的仿真循环上：一张会说话的表
tau = 0、dt = 0.001、跑 2 秒，每 0.4 秒打一行（质心高度、到基座的距离、动量模、基座反力的 z 分量）：
0.000 | 0.872 | 1.424 | 0.000 | 20.106
0.400 | 0.531 | 1.416 | 8.988 | 11.417
0.800 | -0.674 | 1.336 | 19.901 | 42.245
1.200 | -1.323 | 1.374 | 24.113 | 71.875
1.600 | -0.389 | 1.356 | 16.808 | 23.730
2.000 | 0.328 | 1.181 | 7.860 | 14.015
第一行就是【6】的情形 B（20.106 vs 20.1058，表里只印三位小数），这是全表唯一和前面逐位对应的一行。往后要读三件事：
- 这不是「单调下落」，是摆动。 质心
z从0.872掉到−1.323（已经掉到基座以下），又摆回+0.328。这根被基座拴住的直杆是一个复摆，不是自由落体。 - 基座反力在 11.4 ~ 71.9 N 之间来回，最大那一拍（
71.875，t = 1.200 s）比静力平衡时的32.37 N还大一倍多。反解一下那一拍的质心加速度：a_com,z = (71.875 − 32.373) / 3.300 = +11.97 m/s²（手算自实测表格）——向上、而且比重力加速度还大 1.2 倍。这是摆到最低点的向心加速度，物理上完全合理；如果你把它读成「基座受力变大了两倍多，机器人要飞起来」，那就对了，它确实正在被往上拽。 - 距离那一列（
1.424 → 1.181）在说【4】的前提没了。 末拍打印的关节角是q = [ 4.7921 0.7381 0.7794 -0.4462 2.3881 -1.2880 ] rad——六个关节全都在动，手臂早就不直了。「质心被锁在球面上」只在直杆期间近似成立，跑到第二秒已经不能用。
这段读表的本意不是让你记住这几个数，而是示范一件事：仿真表里任何一列都可以拿第 12 章的解析量去反推、去质疑。表末程序还做了一次「差分对账」，把 dh_com 和「往前推一拍再算 h_com」比：
前推 dt = 1.00e-03，差 = 1.37e-01
前推 dt = 1.00e-04，差 = 1.37e-02
步长缩 10 倍、误差缩 10 倍 ⇒ 一阶。这是第 11 章 11.2 那条欧拉精度的又一次独立测量，只是这次量的是动量而不是位形。注意别把它读成「dh_com 算错了」：错的是那一步前推用了欧拉，解析的 dh_com 是准的。
12.8 探针：把差分步长 h 扫一遍，看那条 U 形曲线
【3】里那句「解析值到 1e-11 量级就停住了」是本节的结论，但它是从一份输出里读出来的一个数，不是一个形状。这一节把形状测出来：h 从 1e+02 一路扫到 1e-14，同一个 v_com 分别用前向差分和中心差分去逼近，两个误差并排列出来。
探针只做这一件事。存成 tutorials/04_dynamics/99_probe_fd_step.cpp：
// 探针：有限差分的步长 h 扫一遍，看误差什么时候反过来咬人
// 编译：tutorials/04_dynamics/CMakeLists.txt 里临时加一行 add_tutorial(99_probe_fd_step)
// cmake -S . -B build && cmake --build build --config Release --target 99_probe_fd_step
#include <iostream>
#include <iomanip>
#include <sstream>
#include <cmath> // std::pow
#include "pinocchio/multibody.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/center-of-mass.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;
oss << std::scientific << std::setprecision(3) << x;
return oss.str();
}
int main() {
Model model;
buildModels::manipulator(model, false);
Data data(model);
Eigen::VectorXd q = neutral(model);
q[0] = PI<double>() / 4;
q[1] = -PI<double>() / 6;
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
v[0] = 1.0;
// 解析值：这一行就是第 12 章【3】里那个 4.08e-11 的参照物
const Eigen::MatrixXd J_com = jacobianCenterOfMass(model, data, q);
const Eigen::Vector3d v_analytic = J_com * v;
std::cout << " 解析 v_com = " << std::fixed << std::setprecision(10)
<< v_analytic.transpose() << " m/s" << std::endl;
std::cout << std::endl;
std::cout << " h | 前向差分误差 | 中心差分误差" << std::endl;
std::cout << " " << std::string(44, '-') << std::endl;
double best_fwd_h = 0.0, best_fwd_err = 1e300;
double best_ctr_h = 0.0, best_ctr_err = 1e300;
for (int e = 2; e >= -14; --e) {
const double h = std::pow(10.0, e);
// 前向差分：(f(q + h v) - f(q)) / h
const Eigen::Vector3d r0 = centerOfMass(model, data, q);
const Eigen::Vector3d r1 = centerOfMass(model, data, integrate(model, q, v * h));
const double err_fwd = ((r1 - r0) / h - v_analytic).norm();
// 中心差分：(f(q + h v) - f(q - h v)) / (2h)
const Eigen::Vector3d rm = centerOfMass(model, data, integrate(model, q, -v * h));
const double err_ctr = ((r1 - rm) / (2.0 * h) - v_analytic).norm();
std::cout << " " << std::setw(8) << sci(h) << " | " << std::setw(13)
<< sci(err_fwd) << " | " << std::setw(14) << sci(err_ctr) << std::endl;
if (err_fwd < best_fwd_err) { best_fwd_err = err_fwd; best_fwd_h = h; }
if (err_ctr < best_ctr_err) { best_ctr_err = err_ctr; best_ctr_h = h; }
}
std::cout << std::endl;
std::cout << " 前向差分最优点：h = " << sci(best_fwd_h) << "，误差 " << sci(best_fwd_err) << std::endl;
std::cout << " 中心差分最优点：h = " << sci(best_ctr_h) << "，误差 " << sci(best_ctr_err) << std::endl;
return 0;
}
在 tutorials/04_dynamics/CMakeLists.txt 里临时加一行 add_tutorial(99_probe_fd_step)，然后：
cmake -S . -B build
cmake --build build --config Release --target 99_probe_fd_step
build\bin\99_probe_fd_step.exe
实测输出（Release，退出码 0；解析 v_com 那一行用 10 位小数打印，是下面所有误差的参照物）：
解析 v_com = 0.0000000000 -0.8721668024 -0.8721668024 m/s
h | 前向差分误差 | 中心差分误差
--------------------------------------------
1.000e+02 | 1.240e+00 | 1.240e+00
1.000e+01 | 1.320e+00 | 1.301e+00
1.000e+00 | 5.998e-01 | 1.955e-01
1.000e-01 | 6.165e-02 | 2.055e-03
1.000e-02 | 6.167e-03 | 2.056e-05
1.000e-03 | 6.167e-04 | 2.056e-07
1.000e-04 | 6.167e-05 | 2.055e-09
1.000e-05 | 6.167e-06 | 2.800e-11
1.000e-06 | 6.166e-07 | 4.083e-11
1.000e-07 | 6.203e-08 | 8.328e-10
1.000e-08 | 2.653e-08 | 2.155e-08
1.000e-09 | 2.703e-08 | 2.703e-08
1.000e-10 | 2.703e-08 | 5.363e-07
1.000e-11 | 1.348e-05 | 1.096e-05
1.000e-12 | 8.999e-05 | 3.954e-05
1.000e-13 | 7.945e-04 | 1.027e-03
1.000e-14 | 7.903e-03 | 9.075e-04
前向差分最优点：h = 1.000e-08，误差 2.653e-08
中心差分最优点：h = 1.000e-05，误差 2.800e-11
五件事，按行号读：
- 最上面
h = 1e+02那行是「荒谬端」，但它荒谬得很说明问题。h = 100意味着让关节 1 转 100 弧度——早转了十几圈，差分商量到的根本不是切线。误差饱和在1.240e+00，而‖解析 v_com‖ = 0.8721668 × √2 = 1.2334：两个数一样。原因是当h → ∞时差分商(r₁ − r₀)/h → 0（分子有界：质心永远在半径1.4242的球面上，两点距离不超过2.848，所以差分商不超过2.848/h，h=100时是0.0285），于是误差退化成了「解析值本身的大小」。严格说这一行应当落在[1.205, 1.262]之间——实测1.240✓。 - 中段是教科书那一页。
h每缩小 10 倍，前向差分的误差缩小 10 倍（6.165e-02 → 6.167e-03 → 6.167e-04 → 6.167e-05 → 6.167e-06）：一阶。中心差分同样缩 10 倍时……不，它是缩小 100 倍（2.055e-03 → 2.056e-05 → 2.056e-07 → 2.055e-09）：二阶。截断误差O(h)与O(h²)的差别，这两列就是它的实物。 - 谷底之后两列一起往上翻。 前向在
1e-08见底（2.653e-08），到1e-11涨回1.348e-05（涨 500 倍）；中心在1e-05见底（2.800e-11），到1e-14涨回9.075e-04（涨三万倍）。翻脸的原因是同一个：h太小之后f(q+hv) − f(q)是两个几乎相同的数相减，有效位全丢了，分母却还在除h。误差是一条 U 形曲线，不是单调下降的。 - 谷底位置能推。 双精度机器精度
eps ≈ 2.2e-16，一阶格式的最优h ~ √eps ≈ 1.5e-08，二阶的h ~ eps^(1/3) ≈ 6.1e-06。实测谷底分别是1e-08和1e-05（这两个是实测，前面的 √eps 公式是教科书推算，两者量级吻合）。噪声地板也能核：1.2334 × 1.5e-08 ≈ 1.8e-08，和打印的2.703e-08同量级。 1e-09那一行两个误差一模一样（2.703e-08/2.703e-08）：舍入误差主导之后，「一阶还是二阶」已经没有意义了——两列在比谁的噪声底更低，而噪声底是一样的。这解释了一个常见困惑：「我用了中心差分为什么没变准」。
和本章正文的接口：程序【3】用的 h = 1e-6，中心差分，误差 4.08e-11；探针在同一个 h 上独立跑出 4.083e-11 ✓ 同一份代码抄了两遍都这么说。1e-6 在谷底右边一点点（谷底 1e-5）——教学代码里这个 1e-6 是好用的，但它不是最优的，你换成 1e-9 会立刻看到它变差三个数量级。第 08 章已经用有限差分自检过末端雅可比；以后任何一次差分，都值得像这样先扫一遍 h。
跑完记得把 add_tutorial(99_probe_fd_step) 那行删掉，回到 12.3 那份 CMakeLists.txt。
12.9 本章踩过的坑（按症状查）
坑 1：换了 q、v，data.hg 一个数都不变
症状：程序不报错，hg printed 出来和你上一轮一模一样，哪怕你已经把 q 改得面目全非。
原因：computeCentroidalMomentum(model, data) 这个两参数版本不接收 q / v，它只读 data 里上一次计算留下的状态。它的函数体里没有任何 forwardKinematics（src/algorithm/centroidal.hxx:18），而带 q 的四参数版本是「先 FK 再调它」（同文件 :429）。你改了局部变量 q，data 一无所知。
程序里的实录（【5】）：
不做任何新计算、再调一次 computeCentroidalMomentum，差 = 0.00e+00 ← 它只看 data 里的旧状态，不看你的 q 变量
修法：二选一，别混用。
// 路线 1：显式 FK 到需要的层级，再调两参数版本
forwardKinematics(model, data, q, v); // dhg 还要第三级：forwardKinematics(model, data, q, v, a)
computeCentroidalMomentum(model, data);
// 路线 2：直接用带 q/v 的重载，让库替你做 FK
computeCentroidalMomentum(model, data, q, v);
第 10、11 章的规则在这里同样成立并且更凶：每次换 q 都要重算，而 data 里的中间量随时会被覆盖——【5】里之所以先把 data.hg.toVector() 拷进 h_vec，就是因为后面还要 computeCentroidalMap 和 dccrba。
坑 2：J_com 当 6 行用、Ag 当 M 用——两个都不报错
症状：没有任何报错，结果全错；或者维度不匹配时炸在一个不相干的地方。
原因：本章一次出现三个「看着像」的东西。程序把它们的确切尺寸都打印了：
J_com 尺寸 = 3 x 6
Ag 尺寸 = 6 x 6
J_com是 3 × nv：质心是一个点，没有姿态，所以下半那三行不存在。拿它当第 08 章那个 6 行末端雅可比用，前 3 行你会得到正确答案，后 3 行不存在。Ag是 6 × nv：h = Ag · v，把nv维速度映射成 6 维动量。本模型nv = 6，所以它打印出来是6 x 6，和质量矩阵M同形不同义。M是nv × nv，把加速度映射成关节力矩（第 10 章）。
判别法：看乘的是什么、结果几维。Ag * v5（v5 是 nv 维）得到 6 维动量；M * a 得到 nv 维力矩。写 data.Ag * something 之前先问 something 是几维、结果的 6 个数是什么物理量。本模型 nv = 6 是最容易出事的巧合——换成第 05 章的 humanoid(model, true)（nv = 34），Ag 是 6 × 34、M 是 34 × 34，一眼就分开了。
坑 3：data.com[i] 不是第 i 个刚体的质心，data.com[0] 才是全系统的
症状：data.com[3] 打印出来 1.5161，和 centerOfMass(model, data, q) 返回的 1.4242 不一样，你以为其中一个算错了。
原因：data.com / vcom / acom 都是长度 njoints 的数组，程序把这行直接打印出来了：
注意 data.com 是个数组：长度 = 7，而 model.njoints = 7，下标 0 才是全系统质心
com[i] 是「关节 i 往下的那棵子树」的质量加权质心（com[3] = 4.700 / 3.100 = 1.5161，见 12.7【1】，两个数都对）。库默认把整棵树都算出来——每个函数的签名尾部都有 const bool computeSubtreeComs = true（algorithm/center-of-mass.hpp:104,138,176,199,253,279）。
修法：centerOfMass(...) 的返回值、data.com[0]，永远只要下标 0。要子树质心才用 com[i]（做部分动量、单块隔离体分析时它很有用，但那是另一件事）。另外别忘了 data.com 是 std::vector<Eigen::Vector3d>，data.com.size() 等于 njoints（程序打印的 7）而不是「刚体数 − 1」那种直觉：下标 0 是「整棵树 = 全系统」这一档，com[1] 到 com[6] 才对应六个关节各自的子树。
顺带一个同类：data.mass 也是同一个套路的数组，data.mass[0] 是总质量。12.9 坑 4 的探针里那条诊断行就是拿它乘出来的，实测和手算的 3.300 × 1.2334 = 4.070 逐位相同。
坑 4：oMi[i].act(data.v[i]) 的 .linear() 不是这个刚体质心的速度
症状：h_com 的角量怎么都对不上，差 2 量级；编译和运行都正常。这是本章最贵的坑，因为它是一个物理量的错，不是语法错。
原因：SE3::act(Motion) 做两件事——把旋量搬到世界系，并把参考点换成世界原点。所以搬完之后：
act(v).angular() = 这个刚体的角速度（和参考点无关，随便搬）
act(v).linear() = 「此刻正好和世界原点重合的那一点」的速度
act(v).linear() 不是 body 原点速度，也不是质心速度。要某一点 p 的速度，用第 09 章那条：
v(p) = act(v).linear() + omega.cross(p) // p 是该点的【世界坐标】，不是刚体系偏移
本章程序【5】就是这一行（03_centroidal_dynamics.cpp 里那句 v_point = v_body_world.linear() + omega.cross(com_world)）。最容易犯的两种错法是把 p 写成刚体系的 lever（漏了刚体原点自身的世界坐标），或者干脆把 .linear() 当质心速度用。
这两种错法各错多少，探针实测（存成 tutorials/04_dynamics/99_probe_actframe.cpp，临时加一行 add_tutorial(99_probe_actframe) 编译）：
// 探针：SE3::act(Motion) 到底把「参考点」搬到了哪里
// 编译：tutorials/04_dynamics/CMakeLists.txt 里临时加一行 add_tutorial(99_probe_actframe)
// cmake -S . -B build && cmake --build build --config Release --target 99_probe_actframe
//
// 第 12 章【5】里那段「逐刚体手工累加 h_com」曾经对不上，角量差 2 量级。
// 这里把三种写法并排放在一起，一次跑清：
// 正确：v(p) = act(v).linear() + omega x p （p 是该点的【世界坐标】）
// 错法A：直接把 act(v).linear() 当成质心速度
// 错法B：v = linear + omega x (R * lever)，漏掉刚体原点自身的世界坐标
// 判据：三种写法都同时累加「线量」和「角量」，和 data.hg 对账。
#include <iostream>
#include <iomanip>
#include <sstream>
#include "pinocchio/multibody.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/centroidal.hpp"
#include "pinocchio/multibody/sample-models.hpp"
using namespace pinocchio;
std::string sci(double x) {
std::ostringstream oss;
oss << std::scientific << std::setprecision(3) << x;
return oss.str();
}
std::string vec3(const Eigen::Vector3d& v) {
std::ostringstream oss;
oss << std::fixed << std::setprecision(4) << "[" << v.x() << ", " << v.y()
<< ", " << v.z() << "]";
return oss.str();
}
int main() {
Model model;
buildModels::manipulator(model, false);
Data data(model);
Eigen::VectorXd q = neutral(model);
q[0] = PI<double>() / 4;
q[1] = -PI<double>() / 6;
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
v[0] = 1.0;
forwardKinematics(model, data, q, v);
computeCentroidalMomentum(model, data);
const Eigen::Vector3d h_lin_ref = data.hg.linear();
const Eigen::Vector3d h_ang_ref = data.hg.angular();
const Eigen::Vector3d r_com = data.com[0];
std::cout << " 参照物 data.hg：线量 = " << vec3(h_lin_ref)
<< "，角量 = " << vec3(h_ang_ref) << std::endl;
std::cout << " 质心 r_com = " << vec3(r_com) << std::endl << std::endl;
Eigen::Vector3d lin_ok = Eigen::Vector3d::Zero(), ang_ok = Eigen::Vector3d::Zero();
Eigen::Vector3d lin_a = Eigen::Vector3d::Zero(), ang_a = Eigen::Vector3d::Zero();
Eigen::Vector3d lin_b = Eigen::Vector3d::Zero(), ang_b = Eigen::Vector3d::Zero();
for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i) {
const Inertia& Y = model.inertias[i];
const Motion v_world = data.oMi[i].act(data.v[i]);
const Eigen::Vector3d omega = v_world.angular();
const Eigen::Vector3d com_world = data.oMi[i].act(Y.lever());
const Eigen::Matrix3d R = data.oMi[i].rotation();
const Eigen::Matrix3d I_com = Y.inertia();
const Eigen::Vector3d spin = R * I_com * R.transpose() * omega;
// 正确
const Eigen::Vector3d v_ok = v_world.linear() + omega.cross(com_world);
lin_ok += Y.mass() * v_ok;
ang_ok += spin + Y.mass() * (com_world - r_com).cross(v_ok);
// 错法 A：把 .linear() 当成「这个刚体质心的速度」
lin_a += Y.mass() * v_world.linear();
ang_a += spin + Y.mass() * (com_world - r_com).cross(v_world.linear());
// 错法 B：忘了刚体原点本身的世界坐标，只把 lever 转到世界系去叉一次
// （等于 v = linear + omega x (R*lever)，比正确写法少了 omega x p_origin 这一项）
const Eigen::Vector3d v_b = v_world.linear() + omega.cross(R * Y.lever());
lin_b += Y.mass() * v_b;
ang_b += spin + Y.mass() * (com_world - r_com).cross(v_b);
}
std::cout << " 正确 v(p)=linear+omega x p ：线量差 "
<< sci((lin_ok - h_lin_ref).norm()) << "，角量差 "
<< sci((ang_ok - h_ang_ref).norm()) << std::endl;
std::cout << " 角量累加 = " << vec3(ang_ok) << std::endl;
std::cout << " 错法 A 直接用 .linear() ：线量差 "
<< sci((lin_a - h_lin_ref).norm()) << "，角量差 "
<< sci((ang_a - h_ang_ref).norm()) << std::endl;
std::cout << " 角量累加 = " << vec3(ang_a) << std::endl;
std::cout << " 错法 B omega x (R*lever) ：线量差 "
<< sci((lin_b - h_lin_ref).norm()) << "，角量差 "
<< sci((ang_b - h_ang_ref).norm()) << std::endl;
std::cout << " 角量累加 = " << vec3(ang_b) << std::endl;
// 错法 A 的误差为什么正好是这个数：所有刚体共用的那一个偏移
std::cout << std::endl;
std::cout << " 诊断：线量差 = m * |omega x r_com|？ "
<< sci((lin_a - h_lin_ref).norm()) << " vs "
<< sci((Eigen::Vector3d(1.0, 0.0, 0.0).cross(r_com) *
data.mass[0]).norm()) << std::endl;
std::cout << " （omega 是世界 x 轴，.linear() 给的是「原点处」的速度 = 0，"
<< "而质心处的速度是 omega x r_com，两者差一个叉乘——整个身体的线量差就是这个叉乘乘总质量）"
<< std::endl;
return 0;
}
实测（Release，退出码 0）：
参照物 data.hg：线量 = [0.0000, -2.8782, -2.8782]，角量 = [4.8720, -0.7520, 0.7520]
质心 r_com = [-0.7121, -0.8722, 0.8722]
正确 v(p)=linear+omega x p ：线量差 6.280e-16，角量差 2.706e-15
角量累加 = [4.8720, -0.7520, 0.7520]
错法 A 直接用 .linear() ：线量差 4.070e+00，角量差 2.127e+00
角量累加 = [3.0300, 0.0000, -0.0000]
错法 B omega x (R*lever) ：线量差 2.771e+00，角量差 2.029e+00
角量累加 = [3.1152, -0.0348, 0.0348]
诊断：线量差 = m * |omega x r_com|？ 4.070e+00 vs 4.070e+00
（omega 是世界 x 轴，.linear() 给的是「原点处」的速度 = 0，而质心处的速度是 omega x r_com，两者差一个叉乘——整个身体的线量差就是这个叉乘乘总质量）
这份输出的三个读法：
- 错法 A 的角量累加正好是
[3.0300, 0.0000, -0.0000]——那正是 12.7【5】里那个「自转项合计」！轨道角动量整块消失了，因为.linear()在这个姿态下恒为0：关节 1 的轴是世界x̂且穿过世界原点，所以每个刚体都在绕这条轴转，而轴上的点速度为零，世界原点正好在这条轴上。用「原点那一点的速度」去乘质心的位置偏移，自然什么都乘不出来。 - 先查线量，再查角量。 错法 A 的线量差
4.070e+00一眼就能对上号：‖m·v_com‖ = 3.300 × 1.2334 = 4.070✓ 探针最后一行就是这个诊断（用的还是data.mass[0]）。如果只盯角量那一行（差2.127），你很难猜出错在哪；线量那一行等于在告诉你「速度取错点了」。这是本章程序【5】同时打印线量累加和角量累加的原因。 - 错法 B 是「只漏了一项」的错：
omega × (R·lever)把偏移当成了世界坐标，少了刚体原点自身那一项，于是角量差2.029——和错法 A 的2.127只差一点点，但两条路的错因完全不同。「差值差不多」不能用来判断「错因相同」。
两个探针跑完都要删掉 add_tutorial 那一行。
坑 5：R * Y.inertia() * R.transpose() * omega 编译不过（C2678）
症状：一长串模板报错，指向你写得挺对的那一行。
原因：Inertia::inertia()（还有 lever()、mass() 的部分重载）返回的不是 Eigen::Matrix3d，而是 Eigen 的引用包装类型（expression wrapper）。它自己没法参与 A * B * C 这种运算链——operator* 找不到匹配的重载。MSVC 的原文（/utf-8 /W3，第 11 行是那句乘法）：
error C2678: 二进制“*”: 没有找到接受“const Eigen::Matrix3d”类型的左操作数的运算符(或没有可接受的转换)
error C2737: “spin”: 必须初始化 const 对象
第二条是连带伤害：第一条让初始化失败，于是 const 变量没被初始化。认症状要看第一条，第二条只是回声。
修法：先把引用包装落成一个真正的值，再进运算链。程序【5】就是这么写的：
const Eigen::Matrix3d R = data.oMi[i].rotation();
const Eigen::Matrix3d I_com = Y.inertia(); // ← 关键：先接住，别直接乘进链里
const Eigen::Vector3d spin = R * I_com * R.transpose() * omega;
第 02 章讲表达式模板时提过一次「返回 Eigen 表达式会悬空」，这里是它的编译期版本：那个是运行期崩，这个是编译期拦。被拦下来是运气好——lever() 单独传给一个 Eigen::Vector3d 形参（比如 act(Y.lever())）是完全合法的，本章程序里就那么用；只有把它塞进 A * B * C 才炸。规则很简单：从 Inertia / Motion / SE3 拿到的 Eigen 对象，先赋给一个具名的值类型，再参与运算。
坑 6：质心奇异不报错，只是残差归不了零
症状：svd.solve(v_des) 正常返回一个 nv 维向量，程序继续跑；但 J_com * q_dot 和 v_des 差 0.0612，或者动量控制在某个方向上「推不动」。
原因：这是几何限制，不是数值故障。【7】那一节的实测：秩 2 / 3，残差 6.12e-02，而且残差正好等于不可达方向上的损失 |u3 · v_des| = 0.06124（手推见 12.7【7】）——伪逆已经给出了可达子空间里最接近的解，剩下的那部分物理上就得不到。
判据：不要看返回值的模，要看【4】那两个东西：
奇异值 = [1.5673, 1.2427, 0.0000]，秩 = 2 / 3
最小奇异值贴 0 ⇒ 那个方向不可达。svd.rank() 用相对阈值自己判（本例给 2），但它给的是「判出来」的秩，不是物理上够不够：姿态一动，0.0000 可能变成 0.1943（【4】的弯肘对照），也可能是 1e-9 这种「秩判成 3 但解出来抖得厉害」的数。所以正式项目里要看的是奇异值本身的分布，不是 rank() 那个整数。
修法：加正则化（Jᵀ(JJᵀ + λI)⁻¹，λ 取 1e-6 级别起步）或者换姿态。别指望报错。
坑 7：把示例模型的 1.42 m 质心、2 秒摆幅当物理参考
本章出现的数都该这样读：
- 质心在
z = 1.4242 m（比一个成年人还高）：因为这条链被人为拉成了「肩在原点、往上立 2.5 m」，而且质量最大的两块（各1.0 kg）在z = 1.5和z = 2.5。 - 总质量
3.300 kg、静力平衡基座反力32.373 N：3.3 × 9.81，公式对，但真机不会有这么轻的手腕配重（wrist1只有 0.1 kg，见【1】那张表）。 - 【8】那张表里
71.875 N的峰值反力、11.97 m/s²的质心加速度：是一个 2.5 m 长的复摆自由甩 2 秒的结果，而积分用的是dt = 0.001的一阶欧拉（末段那条差分对账就是在给它量精度）。
第 05 章已经逐块打印过 buildModels::manipulator 的惯性参数，第 11 章坑 6 说过同一件事：这些数是人为设的，用来验证算法自洽，不是用来估算真机的受力。要把本章用到真实模型上，第 06 章的 URDF 加载换进来即可，API 一个都不用改——但那时【4】那个「整列为 0」的漂亮结论不会再有：真实机械臂各关节原点不重合、lever 不都沿 ẑ，缺秩只发生在真奇异位形上。
还有一个模型级的注意点：这条链的根部是固定关节（第 05 章：manipulator 没有浮动基座参数，六个关节全是转轴），所以「基座反力」是约束力，不是地面摩擦力。带浮动基座（humanoid(model, true)，第 05 章实测 nq = 35、nv = 34）时，账上要多一项：那 6 个基座自由度也是未知数，基座反力变成「浮动基座的运动方程的一部分」而不是可以随便反解的已知量。这是双足动力学和本章最大的分别。
12.10 练习（都能用本章程序改出来）
- 把【2】的「通用版手算」搬到弯肘姿态
q = [0, 0.8, 0, 0.6, 0, 0]上，和centerOfMass对账，再核对打印的质心到基座距离 = 1.3614。
这题考的是【2】那句话：加权平均那条路在任何姿态都能走通，而「质心都在 z 轴上」那种捷径只在零位姿态成立。程序里现成的循环换个q就行。 - 把【3】的
v_demo从关节 1 换成关节 4（v_demo[3] = 1.0）。 先回答三个问题再跑：J_com本身会不会变？（提示：它的签名里只有q。）第 3 列的模、奇异值、svd.rank()会不会变？J_com * q_dot会变成多少？不跑程序、只从书里印的那张 3×6 表就能读出来。
跑完对照。这题想练的是「雅可比只依赖位形、不依赖速度」这条铁律——以及【3】那张表你真的会读。
- 在【8】的循环里加一行质心加速度，逐拍验证
m·a_com = m·g + f_base：
先按 12.7【8】那条预测centerOfMass(model, data, qs, vs, ddq_now); // 拿 a_comt = 1.200 s那一拍的a_com,z（表里给的是71.875，m = 3.300，m·g是−32.373），再跑程序看打印对不对得上。
看的是：a_com,z会在摆动最低点超过g，而在t = 2.000 s那一拍（反力 14.015 N）它应当小于g。算一下是多大。 - 把 12.8 探针的差分对象从质心换成末端：用第 08 章的
computeFrameJacobian(model, data, q, FrameIndex, ReferenceFrame, J)拿 6 × nv 的末端雅可比，然后对末端位置做同样的h扫描。
问题：谷底还在1e-05/1e-08吗？如果挪了，是差分对象变「贵」了还是变「光滑」了？这题没有标准答案，重点是让你发现自己其实不知道h该怎么选——那正是 12.8 存在的理由。 - 换浮动基座模型：
buildModels::humanoid(model, true)（第 05 章实测nq = 35、nv = 34）。打印J_com的尺寸，先预测它的秩，再跑：- 尺寸：
3 × nv⇒? - 秩：前 6 列属于基座（浮动基座占 6 个
nv：3 个平动 + 3 个转动），基座平动可以直接把质心推到任何方向 ⇒ 秩应当是?
然后把本章程序的q1（6 维）换成neutral(model)，看【3】【4】【7】哪几节的结论还在、哪几节没了。
这题是给第 14 章之后的内容铺路：一旦基座能动，「基座反力」就不再是可随便反解的已知量（见坑 7 最后一段）。
- 尺寸：
本章要点
- 质心动力学是把
nv维合并成 6 个数：h_com = (m·v_com, L_com)，六维排布线量在前（第 09 章实测），存成Force类型的data.hg。它和第 10 章那条方程同构：h = Ag(q)·v、dh = Ag·a + dAg·v↔M·q̈ + b = τ，但右边只剩外力。 - 关节力矩进不了这张账（实测）：钉住姿态要花
‖tau‖ = 33.6306 N·m，而同一瞬间dh_com六个分量全打印0.0000；反过来tau = 0松手，dh_com线量是[-6.2970, -7.1258, -12.2672]。 - 动量定理可以直接当自检用：
f_base = dh_com − m·g。静力平衡时实测f_base = [0, 0, 32.3730] N，而手算总质量 × g = 3.300 × 9.81 = 32.373——一个数不差。松手那拍a_com,z = −12.2672 / 3.300 = −3.717 m/s²，只有g的 38%：质心不是自由落体，它被基座拴着。 J_com只有 3 行（质心没有姿态），每一列 =ẑ_j × (r_com − p_j)，本章手推了第 1、2、3 列并和打印逐位对上；第 3 列整列为 0（9.52e-17）因为关节 3 的轴正对着质心方向且穿过世界原点，叉乘平行向量必为零。- 秩缺有几何解释，且随前提消失：直杆姿态下质心被锁在半径
1.4242 m的球面上，奇异值[1.5673, 1.2427, 0.0000]、秩2/3；弯肘之后[1.4978, 0.7641, 0.1943]、秩3/3、距离也从1.4242变1.3614。结论要连着前提一起记。 h_com的六个数全能从【1】那张质量表推出来：线量3.300 × 0.8722 = 2.8782；角量的自转项因为六块惯量都是各向同性而等于Σc_i·ω = 3.0300，轨道项= [Σm·d² − R·Σm·d]·(a × (x̂ × a)) = 2.45606 × (0.75, −0.3062, 0.3062) = (1.8420, −0.7520, 0.7520)，合计4.8720✓ 实测差2.71e-15。伪逆的残差也可推：= |u3·v_des| = 0.06124✓ 实测6.12e-02，竖直方向恰好实现5/8。- 差分步长是 U 形曲线（12.8 实测）：前向差分一阶（
h/10 ⇒ 误差/10）、谷底h = 1e-08；中心差分二阶（h/10 ⇒ 误差/100）、谷底h = 1e-05、谷底误差2.800e-11；h = 1e-14时两列都涨回1e-03量级。谷底位置和√eps ≈ 1.5e-08/eps^(1/3) ≈ 6.1e-06的教科书公式量级吻合。 act(Motion)会换参考点（12.9 坑 4，探针实测）：.linear()是「此刻与世界原点重合那点」的速度，要哪一点就+ ω × p，p是世界坐标。写错的两种实测分别是角量差2.127和2.029——先查线量再查角量，线量那一行会把错因直接说出来。
下一章离开动力学，回到最朴素的问题：给定起点和终点，怎么在关节空间里画一条时间曲线，然后用一个 PD 把它跟住——顺便把第 11 章那个「重力 sag」用一个整定好的增益矩阵消掉。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
2
0- 0
已为社区贡献18条内容
所有评论(0)