Pinocchio 浮动基专题 04：完整 C++ 模型与接口工程导读

不是再列一遍 API，而是读懂一个小工程

前面已经解释了状态约定和递归算法；这一篇把它们放回真实 C++ 调用关系中。目标是能够回答：模型在哪里定义，矩阵由谁分配，哪一次调用更新了哪块缓存，打印出的残差究竟在检验什么。

工程固定采用 Pinocchio v3.8.0，对应源码提交 655877b314baed68c7e2d4dd56b0a0200bb9f98e。模型是一个浮动基加两个绕 y 轴转动的关节，不需要 URDF、网格文件、ROS 节点或可视化服务器。这些外部系统不是不重要，而是在这个小实验里没有参与被检验的定义。

配套文件：

floating_base_demo.cpp：完整教学程序，负责建模、状态初始化、算法调用与误差检查。
CMakeLists.txt：明确版本、语言标准和链接目标。
Python 自检：同类模型的 Python 接口验证，便于对照。
环境与验证说明：以该文件记录的运行状态为准。

本篇按固定源码核对接口，不把“文件已生成”写成“已成功编译”。下面没有伪造终端输出；C++ 编译、数值执行、动态分配行为都需要在具备正确依赖的环境中分别验证。回看约定可读 01：模型与流形，算法对应关系见 02：动力学源码。

1. 先看工程边界，再运行命令

程序包含两类行为：模型构建与数值核对。它不是闭环控制器，也没有地面接触；输出了电机关节命令下的加速度，不代表机器人能站稳。

可把阅读路径压缩成一条数据链：

刚体参数和固定安装变换
    → Model / Joint / Frame
    → 建立 Data 和 q、v、a 缓冲
    → 构造同一个确定状态
    → RNEA / CRBA / ABA / 运动学 / 导数
    → 明确参考系的输出及残差

这里的命令应在已配置 Pinocchio 3.8.0 的受控环境或选定容器内执行，且当前目录是仓库根目录。它们不是要求在 Hexo 宿主机安装原生依赖：

cmake -S source/downloads/code/pinocchio_floating_base \
      -B build/pinocchio-floating-base \
      -DCMAKE_BUILD_TYPE=Release
cmake --build build/pinocchio-floating-base --parallel
./build/pinocchio-floating-base/floating_base_demo

这三步分别是配置、构建、执行。配置成功只说明 CMake 找到了必要目标与编译器；构建成功只说明代码能编译和链接；运行的数值检查通过，才说明这个固定模型满足这组断言。三者不能合并成一个模糊的“环境没问题”。

2. CMake 的关键不是手写一串 -I 和 -l

这个工程的核心约束是：

find_package(pinocchio 3.8.0 EXACT REQUIRED CONFIG)
add_executable(floating_base_demo floating_base_demo.cpp)
target_compile_features(floating_base_demo PRIVATE cxx_std_17)
target_link_libraries(floating_base_demo PRIVATE pinocchio::pinocchio)

完整文件还包含 cmake_minimum_required 与 project；上面只摘出与依赖合同直接相关的部分。EXACT 的作用是阻止自动拿另一个版本凑数，并不是承诺不同编译选项、架构或依赖 ABI 一定兼容。

使用导出的 pinocchio::pinocchio 目标，是让依赖提供必要的头文件目录、编译定义与链接依赖。不要看到缺一个符号就随手把系统中另一套 Pinocchio 的库目录加进来，造成头文件来自 A、二进制库来自 B。官方固定版本 README 给出了该 CMake 目标的使用方式。官方 CMake 用法

若 CMake 找不到包，应确认选定环境中确实存在 Pinocchio 的安装配置。必要时向 CMAKE_PREFIX_PATH 提供那个安装前缀，或将 pinocchio_DIR 指向实际的配置文件目录；路径取决于容器构建方式，不应在公共笔记里硬编码某个人的主目录。

3. 头文件决定你正在使用哪一层接口

源码开头优先包含 pinocchio/fwd.hpp，再包含具体模型与算法。官方 README 特别提醒这一顺序有助于避免不同 Boost variant 配置引发的问题。下面是一组与本实验对应的头文件：

#include <pinocchio/fwd.hpp>
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/algorithm/crba.hpp>
#include <pinocchio/algorithm/aba.hpp>
#include <pinocchio/algorithm/rnea-derivatives.hpp>

joint-configuration.hpp 提供 neutral、integrate 等配置空间操作；frames.hpp 不是 URDF 解析器；rnea-derivatives.hpp 也不是普通 RNEA 的同义入口。按实际使用的层包含头文件，比依赖某个大头文件碰巧传递包含一切更容易检查。

官方 overview-simple.cpp 展示了 Model → Data → q/v/a → rnea 的基本形式，本专题是在它所体现的调用模式上加入浮动基与更严格的输出检查。

4. 逐关节建模：先连接，再附加刚体惯量

以下片段位于构建模型的函数中，已经包含上述头文件。这里显式创建模型和三个关节，名字与完整程序一致：

pinocchio::Model model;
const pinocchio::JointIndex root = model.addJoint(
    0, pinocchio::JointModelFreeFlyer(),
    pinocchio::SE3::Identity(), "root_joint");
const pinocchio::JointIndex shoulder = model.addJoint(
    root, pinocchio::JointModelRY(),
    pinocchio::SE3(Eigen::Matrix3d::Identity(),
                   Eigen::Vector3d(0.0, 0.0, 0.20)), "joint1");
const pinocchio::JointIndex elbow = model.addJoint(
    shoulder, pinocchio::JointModelRY(),
    pinocchio::SE3(Eigen::Matrix3d::Identity(),
                   Eigen::Vector3d(0.45, 0.0, 0.0)), "joint2");

addJoint 的父节点参数是 Joint ID；两个平移向量都是在父关节坐标系中定义的固定安装位置，不是世界坐标，也不是质心位置。JointModelRY 的转轴是该关节局部 y 轴。

浮动基根关节连到 universe，它的自由运动没有被六个电机代替。后面两个 RY 才是这个教学模型明确施加关节命令的对象。

addJoint 会登记类型、父节点、索引和安装变换，但新关节支承的惯量初始为零。只有连好关节还不足以得到一个物理有效的动力学模型；接下来必须附加刚体惯量。addJoint 契约

5. 惯量参数逐项读，不要只检查质量

完整示例使用的参数如下，转动惯量都关于各自质心，并用对应关节坐标轴表达：

支承关节	质量 kg	质心坐标 m	中心主惯量 kg·m²
root_joint	5.0	(0.02, -0.01, 0.03)	(0.30, 0.40, 0.50)
joint1	1.2	(0.20, 0, 0)	(0.012, 0.025, 0.030)
joint2	0.8	(0.15, 0, 0)	(0.008, 0.015, 0.020)

例如，下面是在上节已定义 model 与 shoulder 后，附加 joint1 刚体的片段：

Eigen::Matrix3d central_inertia = Eigen::Matrix3d::Zero();
central_inertia.diagonal() << 0.012, 0.025, 0.030;
const pinocchio::Inertia link_inertia(
    1.2, Eigen::Vector3d(0.20, 0.0, 0.0), central_inertia);
model.appendBodyToJoint(
    shoulder, link_inertia, pinocchio::SE3::Identity());

这里传单位安装变换，是因为质心和惯量已经用支承关节坐标系描述。把 0.20 米质心偏移再写进 appendBodyToJoint 的平移，会额外移动一次刚体。

三个中心主惯量应满足正性与三角不等式。这里只用对角中心惯量便于手算，不代表真实 URDF 的惯量一定对角。若 CAD 的主轴与关节轴不一致，需要先把中心惯量正确旋转到选定坐标轴，不能直接复制三个主惯量并假设主轴已经对齐。空间惯量块和重复平行轴错误见第 01 篇。

6. tool 是 Frame，不是第三个电机

在 elbow 所指的 joint2 上附加一个末端 Frame：

const pinocchio::FrameIndex tool = model.addFrame(pinocchio::Frame(
    "tool", elbow,
    pinocchio::SE3(Eigen::Matrix3d::Identity(),
                   Eigen::Vector3d(0.35, 0.03, 0.02)),
    pinocchio::OP_FRAME));

它增加一个可查询的坐标系，而不增加自由度。非零的侧向偏移让角速度对该点线速度的影响更容易在测试中显现；不是随意选个末端原点都能同样有效地暴露参考点错误。

上面的构造形式直接指定支承 Joint 和固定安装位姿。大型工程可以另外登记 Joint Frame、Body Frame 和语义父 Frame；这些名字与 Frame 组织不改变本例的两条运动关节边。不要把增加命名 Frame 误读成增加独立动力学坐标。Frame 构造与含义

7. 索引打印是最便宜的一次正确性检查

按这个建模顺序，预期索引关系为：

关节	idx_q / nq	idx_v / nv
root_joint	0 / 7	0 / 6
joint1	7 / 1	6 / 1
joint2	8 / 1	7 / 1

整机 nq=9、nv=8。表格是模型定义导出的预期，不是终端运行截图。完整程序打印索引，就是为了让你将定义与运行时对象对应起来。

for (pinocchio::JointIndex jid = 1; jid < model.njoints; ++jid) {
  const auto & joint = model.joints[jid];
  std::cout << model.names[jid]
            << " idx_q=" << joint.idx_q() << " nq=" << joint.nq()
            << " idx_v=" << joint.idx_v() << " nv=" << joint.nv()
            << '\n';
}

C++ 中这些是方法调用；不要照搬 Python 中 joint.idx_q 的属性语法。通用控制接口应按 idx_v() 提取广义速度与力，按 idx_q() 提取配置，两者不能共用一个切片。

8. 预分配：先明确工作区，再谈实时性

在模型结构固定后创建 Data。下面的分工刻意清楚，便于保存不同算法的结果：

pinocchio::Data mass_data(model), bias_data(model), inverse_data(model);
pinocchio::Data forward_data(model), frame_data(model), derivative_data(model);
Eigen::VectorXd q = pinocchio::neutral(model);
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);
Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv);
Eigen::MatrixXd M(model.nv, model.nv);
Eigen::VectorXd tau(model.nv), h(model.nv), recovered_a(model.nv);

示例使用确定的配置、速度和加速度，方便不同语言或容器间重现；初始配置从 neutral 出发，再采用合法的流形增量。不要为了制造“非零测试”把四元数四项任意填满而不归一化。

这段代码只说明创建工作区的位置，不承诺后续所有 Eigen 运算零分配。.eval()、分解器尺寸变化、临时 MatrixXd、字符串、流输出都可能分配内存。若未来进入高频控制循环，应把检查、日志和计算分层，使用针对实际构建模式的动态分配检测，而不是仅凭用了 Data 就标记为实时安全。

9. 先复制结果，再调用会改写缓存的算法

在上节变量已创建的上下文中，可以写出清楚的动力学核对链：

pinocchio::crba(model, mass_data, q);
M = mass_data.M.selfadjointView<Eigen::Upper>();
tau = pinocchio::rnea(model, inverse_data, q, v, a);
h = pinocchio::nonLinearEffects(model, bias_data, q, v);
recovered_a = pinocchio::aba(model, forward_data, q, v, tau);

这里 tau、h 和 recovered_a 是已经定义的 VectorXd，赋值会得到独立存储中的数值。反过来，const auto & tau = rnea(...) 可能绑定到 Data 内部向量；如果随后复用同一 Data，就不能把这个引用视为历史快照。

还要区分语言层：v3.8.0 C++ CRBA 核心保证上三角，因此这里从 selfadjointView<Upper> 恢复完整矩阵；Python 包装层已经执行对称填充。不能把 Python 返回矩阵的表象原封不动地当成 C++ 缓存合同。CRBA 核心契约、Python CRBA 包装

本例的残差分别核对：

$r_{\mathrm{ID}}=\tau-Ma-h,\qquad r_{\mathrm{FD}}=\operatorname{ABA}(q,v,\tau)-a.$

质量矩阵最小特征值还提供一个退化检查，但“特征值为正”不是完整的 URDF 审计，也没有检查接触可行性。这些数值应该连同单位、范数类型与阈值一起读，不能只看屏幕上是否出现零。

10. frame 的 Jv 检查为什么要保持同一个 q

下面是一种方便审计的调用顺序。tool、q、v 和 frame_data 都来自前文：

pinocchio::forwardKinematics(model, frame_data, q, v);
pinocchio::computeJointJacobians(model, frame_data, q);
pinocchio::updateFramePlacements(model, frame_data);

pinocchio::Data::Matrix6x J(6, model.nv);
J.setZero();
pinocchio::getFrameJacobian(
    model, frame_data, tool, pinocchio::LOCAL_WORLD_ALIGNED, J);
const pinocchio::Motion velocity = pinocchio::getFrameVelocity(
    model, frame_data, tool, pinocchio::LOCAL_WORLD_ALIGNED);
Eigen::Matrix<double, 6, 1> residual = J * v - velocity.toVector();

先做带 v 的 FK，是为了给关节运动缓存正确的速度。随后关节雅可比调用使用同一个 q；它不负责把任意旧速度同步到另一个状态。传出式 C++ 雅可比应按接口要求准备好尺寸并清零；不能依赖不属于当前关节支撑链的列碰巧是零。

完整示例分别使用 LOCAL、LOCAL_WORLD_ALIGNED、WORLD，每次左右两边参考系一致。LWA 和 WORLD 的轴相同，参考点不同。改变一边的参考系，测试应能失败；在世界原点、单位姿态、零角速度这种过于特殊的状态下，错误可能被掩盖。Frame 雅可比实现

11. 配置导数：一个函数名，背后有不同的空间

完整示例还比较 computeRNEADerivatives 的配置导数和局部有限差分。先创建 nv 维扰动 delta，再生成 integrate(model, q, ±delta)，保持 v 和 a 的数值坐标不变，然后比较两次 RNEA 的差。

这检验的是 nv × nv 的切空间配置导数；不是给九维配置数组的每个存储数字加一个 epsilon。若对四元数的四项直接逐个扰动，既改变了问题定义，也可能离开单位四元数约束。

配置导数数值与有限差分的大小还取决于步长。把步长减半后误差不再减小，可能说明已进入舍入误差主导区域，不应该立刻下结论说解析导数错误。官方 C++ 导数测试同样采用流形积分构造配置扰动。rnea-derivatives.cpp 测试

矩阵名字 dtau_dq 容易让人忘记这里的 q 是流形变量。详细数学约定回到 01 的右扰动解释；接口自检的基本思路见 03：数值技巧。

12. 按失败阶段排查，不要反复重装

失败阶段	常见原因	首先核对
CMake 找不到包	当前环境没有开发安装，或搜索前缀错误	所选容器、实际配置文件位置、请求版本
版本不满足 EXACT	找到了另一版 Pinocchio	CMake 解析到的安装前缀，不先删除版本约束
缺少头文件 / 类型	include 不完整、用了别版接口	固定 tag 的声明与实际编译 include 路径
链接未定义符号	链接目标遗漏、头库混用、ABI 不一致	`pinocchio::pinocchio` 及工具链架构
运行时尺寸断言	nq/nv 混用、模型改变后 Data 未重建	每关节索引、输入形状、工作区创建位置
质量矩阵异常	空惯量、重复平行轴、错误的对称恢复	每个刚体参数与 CRBA 上三角合同
只有 Jv 检查失败	参考点不一致或速度缓存过期	q/v/Frame ID/ReferenceFrame 是否成套
只有导数检查失败	系数扰动代替流形扰动、步长或别名问题	integrate、固定 v/a、结果复制、尺度

Release 模式可能关闭某些断言，所以“没有断言报错”不是合法输入的证明。遇到数值问题时先保留状态和错误结果，不要只改阈值直到程序退出码变成零。

13. 如果以后接入实际机器人，需要替换哪些部分

可以先保留工作区管理、同一状态的算法核对、残差与版本记录，只替换模型构造来源。换成 URDF 后，必须重新确认根关节类型、传感器 Frame、每个执行器对应的 idx_v、惯量单位与坐标系。

再往后才是状态估计、控制周期、外力、接触和通信。控制器输出的电机力矩通过执行器映射进入广义力；无驱动的浮动基六维不能被当成六个普通力矩命令填进去。动力学 API 只回答模型问题，不替你决定控制器、执行器或接触环境的物理可实现性。

建议给每次真正运行保存：源码提交、Pinocchio 版本、C++ 编译器、CPU 架构、构建类型、模型参数、输入 q/v/a、各项残差与退出码。这样将来遇到笔记中的问题，才可以区分“理论约定理解错了”“模型变了”和“软件环境变了”。

继续读 05：接触约束与质心动力学，把无接触的 API 核对扩展到约束方程；或者进入 06：导数与控制线性化。完整顺序见专题入口。