Pinocchio C++ 初学者讲解手册

ai2work

362人浏览 · 2026-08-19 01:22:30

ai2work · 2026-08-19 01:22:30 发布

一份写给「刚学习 C++ 的你」的机器人学框架完全指南

适用项目：Pinocchio（Pinocchio 4.1.0 + Eigen 5.0.1，Windows / MSVC / CMake）

项目源码：
链接：https://pan.quark.cn/s/92197f938298?pwd=yMks
提取码：yMks

在这里插入图片描述

写在前面：这本手册想帮你做什么

你手边的这个项目，是一套循序渐进的 Pinocchio C++ 学习工程。它通过 6 组、共 13 个可以编译运行的小程序，带你从零开始掌握现代机器人学中最核心的计算框架之一 —— Pinocchio。

但我们也知道，对于刚接触 C++ 的同学来说，直接读这些代码会遇到两座大山：

C++ 语言本身：命名空间、引用、智能指针、模板、类……这些概念在代码里随处可见；
机器人学的专业概念：刚体、关节、自由度、位姿变换、雅可比矩阵、动力学……没有背景知识很难看懂代码在算什么。

这本手册的任务，就是把这两座大山一次性铲平。对于项目里的每一个程序，我们都会做到：

逐段讲解代码：每一行关键代码都告诉你「它是什么语法、为什么这样写」；
补充背景知识：涉及机器人学概念时，先用生活化的例子讲清楚，再看代码；
指出易错点：哪些地方容易编译报错、哪些 API 在不同版本间有变化、哪些是 Windows 平台的特殊问题。

建议的学习方式：

先读第 0 章把环境跑通、成功编译并运行第一个程序；如果你已经熟悉 C++ 工具链，可跳过第 0 章直接读第 1、2 章；
第 3、4 章是「C++ 与 Eigen 速成课」，可以通读一遍，遇到不懂的语法随时回来查；
从第 5 章开始，每一章对应项目中的一个教程目录。请务必一边读手册、一边运行对应的程序、一边看程序的输出，三者对照着学，效果最好；
每章末尾都有「动手练习」，强烈建议真的动手改代码、重新编译、观察输出变化。编程是练出来的，不是读出来的。

第 0 章学前准备（零基础）：软件安装、一键搭建、Visual Studio 使用、如何读报错、向量与矩阵数学前置
第 1 章认识项目：Pinocchio 是什么、机器人学基础概念、项目目录结构
第 2 章环境搭建与构建：conda、CMake、编译运行全流程，根 CMakeLists 逐行讲解
第 3 章 C++ 语言基础速成：从程序结构到类与智能指针，覆盖本项目用到的全部语法
第 4 章 Eigen 快速入门：Pinocchio 的数学基石 —— 向量、矩阵与运算
第 5 章教程 01 详解：Hello Pinocchio、智能指针、Eigen 初体验
第 6 章教程 02 详解：从零创建机器人模型、关节与坐标系、内置示例模型
第 7 章教程 03 详解：正向运动学、雅可比矩阵、坐标系操作与空间速度
第 8 章教程 04 详解：逆动力学（RNEA）、正动力学仿真（ABA）、质心动力学
第 9 章教程 05 详解：URDF 文件格式与模型加载
第 10 章教程 06 详解：综合实战 —— 轨迹规划与逆动力学控制
第 11 章辅助测试文件说明：test_*.cpp 的作用
附录 A：Pinocchio 常用 API 速查表
附录 B：常见编译/运行错误排查
附录 C：中英术语对照表
附录 D：进阶练习题
附录 E：延伸学习资料

第 0 章学前准备（面向绝对零基础）

这一章是给「还没装过编译器、没打开过 Visual Studio、没碰过命令行」的同学准备的。
如果你已经能熟练用 CMake 编译 C++ 项目、看得懂报错，可以直接跳到第 1 章。
但如果一看到「终端」「链接错误」「断点」就心里发虚，请务必从这里开始——把环境跑通，后面 90% 的"看不懂"都会消失。

0.1 你现在的起点，和这条学习路线的全貌

很多同学卡住，不是因为 Pinocchio 难，而是因为不知道从哪下手把代码跑起来。整个流程其实只有六步：

① 装软件  →  ② 跑 setup.bat  →  ③ 编译  →  ④ 运行 exe  →  ⑤ 读代码  →  ⑥ 改代码看输出
（一次性）    （一次性）         （改代码后常做）

每一步对应的"工具"和"你会看到什么"，先有个印象：

步骤	你用到的工具	这一步的目标
① 装软件	浏览器下载安装包	电脑上有了 C++ 编译器（MSVC）、构建工具（CMake）、包管理器（conda）
② 跑脚本	文件管理器双击 `setup.bat`	自动把 Pinocchio/Eigen 装进项目内的 `env/`，并把所有程序编译成 exe
③ 编译	CMake + MSVC（幕后由 setup.bat 或 VS 调用）	把 `.cpp` 变成 `.exe`
④ 运行	命令行或 Visual Studio	看到 `===== 教程 01-1 ... =====` 这类输出
⑤ 读代码	文本编辑器 / VS 的代码窗口	对照本手册，搞清楚每行在算什么
⑥ 改代码	文本编辑器 + 重新编译	改一个数、重新编、观察输出变化——这是真正学会的关键

本手册建议的循环：读一章手册 → 运行对应程序 → 改一两行再编译 → 看输出变了什么。遇到报错就翻到 0.7 节和附录 B 对照。

0.2 软件安装清单（一步一步来，别跳）

只需要 3 个软件。安装顺序无所谓，但路径有讲究（conda 必须装在「与 setup.bat 中 CONDA_ROOT 一致的位置」，否则要改脚本）。

0.2.1 Visual Studio 2022 Community（免费的，足够）

打开官网 https://visualstudio.microsoft.com/zh-hans/downloads/，下载 Visual Studio 2022 Community；
运行安装程序，在"工作负载"页勾选「使用 C++ 的桌面开发」（英文名 Desktop development with C++）；
在右侧"安装详细信息"里确认包含：
- MSVC v143 - VS 2022 C++ x64/x86 生成工具（编译器本体）
- Windows 10 或 11 SDK（系统头文件/库）
- C++ CMake 工具（VS 自带 CMake，免去另外装）；
点"安装"，装完重启一次电脑。

为什么是 VS 而不是记事本 + 命令行？VS 把"写代码、编译、运行、调试（设断点看变量）"集成在一个界面里，对初学者最友好。本项目根目录的 setup.bat 用的是命令行版 CMake，两者不冲突——你用哪种都行，0.4 节两套都讲。

0.2.2 Miniforge3（conda 的轻量开源版）

到 https://github.com/conda-forge/miniforge/releases 下载 Windows x86_64 安装包（文件名类似 Miniforge3-Windows-x86_64.exe）。如果 GitHub 慢，可用清华镜像 https://mirrors.tuna.tsinghua.edu.cn/github-release/conda-forge/miniforge/；
安装时安装路径务必与 setup.bat 里写死的 CONDA_ROOT 保持一致——项目默认约定为 Miniforge 装在指定目录下。装到别处也能用，但得回头改脚本第一行；
安装选项里"Add to PATH"勾不勾都行（本项目不依赖它，脚本自己拼路径）。

0.2.3 这三个工具分别干啥，一句话记住

工具	一句话角色	类比
MSVC	C++ 编译器，把 `.cpp` 文本翻译成 `.exe`	翻译官
CMake	构建系统生成器，读 `CMakeLists.txt` 决定"哪些源码编成哪些程序"	工程调度员
conda / Miniforge	包管理器，一键安装 Pinocchio 及其几十个依赖库	超市配送

你不需要手动下载 Eigen、urdfdom、Boost 这些——setup.bat 调用 conda 一次性全装好，放在项目内的 env/ 目录，不污染你电脑的其他环境。

0.3 一键搭建：运行 setup.bat

环境准备好后，这一行命令做完 ② 和 ③（安装 + 编译）：

方式一：双击。在文件管理器里双击项目根目录的 setup.bat，会弹出一个黑色命令行窗口自动执行。

方式二：命令行（推荐，能看到完整日志）。打开"开始菜单 → 终端"或 VS 的"开发者命令提示符"，进入项目目录：

cd D:\WorkSpace\cpp_WorkSpace\6Pinocchio
.\setup.bat

脚本会依次做这些事（对应脚本里的"第 X 步"）：

检查 setup.bat 中 CONDA_ROOT 指向的 Miniforge 安装目录是否存在；
创建 conda 环境 pinocchio_learn（Python 3.11），已存在就跳过；
conda install pinocchio -c conda-forge——这一步最慢，要下载 Eigen、urdfdom、Boost、coal 等几十个包，可能要 5~20 分钟，请耐心等进度条；
用 Python 验证安装；
进入 build/ 目录，调用 CMake 配置 + 编译（Release）。

期望看到的画面（节选真实输出）

============================================================
  Pinocchio C++ 学习项目 - 环境设置脚本
============================================================

[第一步] 创建 conda 环境 pinocchio_learn ...
环境 pinocchio_learn 已存在，跳过创建步骤        ← 第一次跑会显示"创建新环境..."

[第二步] 激活环境并安装 Pinocchio ...
Collecting package metadata ...                  ← 下载依赖，可能持续数分钟
Downloading and Extracting Packages ...
Preparing transaction: done
Verifying transaction: done
Executing transaction: done
Pinocchio 安装完成

[第三步] 验证安装 ...
Pinocchio version: 4.1.0                         ← 看到版本号即成功

[第五步] 构建项目 ...
-- Configuring done
-- Generating done
-- Build files have been written to: .../build

[第六步] 编译项目 ...
  01_hello_world.vcxproj -> ...\01_hello_world.exe
  02_smart_pointers.vcxproj -> ...\02_smart_pointers.exe
  ... （共十几个 exe 逐个生成）
============================================================
  ✓ 环境设置完成！
============================================================

看到 ✓ 环境设置完成！ 和一堆 xxx.exe 生成，就说明成功。如果某一步报红，先别慌，翻到 0.7 节和附录 B 对照。

网络问题怎么办？ 如果 conda install 卡在下载或报超时，多半是 conda-forge 默认源访问慢。可改用国内镜像：在 setup.bat 的 install pinocchio 之前加一行 conda config --add channels https://mirrors.tuna.tsinghua.edu.cn/anaconda/cloud/conda-forge（或用 mirrors.ustc.edu.cn）。详见 2.8 节。

0.4 第一次运行：期望看到什么

编译成功后，运行第一个程序。用命令行：

cd D:\WorkSpace\cpp_WorkSpace\6Pinocchio\build\bin\01_basics
01_hello_world.exe

期望输出（节选）：

===== 教程 01-1: Hello Pinocchio =====

【模型基本信息】
模型名称: manipulator
关节数量: 7
自由度 (DOF): 6
速度空间维度: 6
坐标系(Frame)数量: 13

【数据对象创建成功】
数据向量大小: 6

【关节列表】
  关节 0: FIXED (nq=0, nv=0)        ← 索引 0 是 universe（世界），固定不动
  关节 1: RZ (nq=1, nv=1)           ← 绕 Z 转
  关节 2: RY (nq=1, nv=1)           ← 绕 Y 转
  关节 3: RY (nq=1, nv=1)
  关节 4: RZ (nq=1, nv=1)
  关节 5: RY (nq=1, nv=1)
  关节 6: RZ (nq=1, nv=1)

【坐标系(Frame)列表】
  帧 0: universe
  帧 1: joint1_...
  ...（共 13 个）

关键验证点：自由度=6 且 关节数=7（多出来的 1 是 universe）。如果你看到的输出和这里对不上，要么模型版本不同，要么编译链接到了别的 Pinocchio——回到 0.3 重跑 setup.bat。

0.5 用 Visual Studio 图形界面构建（不想记命令行时）

如果你更习惯"点按钮"，全程用 VS 也完全可以：

打开工程：双击 build/PinocchioLearn.sln（CMake 配置后生成的解决方案文件）。若 build/ 不存在，先按 0.3 跑一次 setup.bat；
选择配置：顶部工具栏，左侧下拉选 Release，右侧选 x64（务必和 setup.bat 的 -A x64 一致）；
设启动项目：在右侧"解决方案资源管理器"里，右键 01_hello_world → “设为启动项目”（否则 F5 会跑别的程序）；
生成（编译）：菜单"生成 → 生成解决方案"（快捷键 F7）。底部"输出"窗口会滚动显示每个 .exe 的生成结果；
运行：Ctrl + F5（“不调试直接运行”）会在弹出的黑窗口里跑程序；F5 则是"调试运行"（遇到断点会停，见 0.6 节）；
改代码后重新编译：改完 .cpp 保存，再按 F7 即可，CMake 会自动重新配置、增量编译。

想加自己的程序？ 在 tutorials/某目录/ 里新建 my_test.cpp，然后打开该目录的 CMakeLists.txt，照葫芦画瓢加三行（参见 2.4 节的"子目录 CMakeLists 套路"），保存后在 VS 里"生成 → 生成解决方案"重新配置即可。

0.6 调试入门：断点、监视、单步（看懂"程序到底在干什么"）

初学者最容易有的困惑是：“代码我读了，但不知道运行时变量到底变成了啥。” 调试器就是干这个的。以 Visual Studio 为例：

操作	快捷键	作用
设/取消断点	F9	在某一行左边点一下（或按 F9），程序运行到这行会"暂停"
开始调试	F5	启动程序，遇到断点停住
单步跳过	F10	执行当前行，停在下一行（不进入函数内部）
单步进入	F11	执行当前行，若这行是函数调用，则进入函数内部
继续运行	F5（断点停住后再按）	从断点继续跑到下一个断点或结束
"监视"窗口	调试时菜单"调试 → 窗口 → 监视"	输入 `model.njoints` 这类表达式，实时看值

一个立刻能上手的小实验：

在 VS 里打开 tutorials/01_basics/01_hello_world.cpp；
在 buildModels::manipulator(model, false); 这一行按 F9 设断点；
按 F5 调试运行，程序会停在这一行（还没执行）；
再按 F10 执行这一行，然后看"局部变量/监视"窗口里 model.njoints 从 0 变成了 7；
继续 F10 往下走，观察 data.oMi 等结果在被 forwardKinematics 调用前后如何被填上数据。

调试的黄金法则：当你不确定"某行之后变量该是什么"，就在这行前后加断点，监视它。这比在脑子里空想高效十倍，也是你读 Pinocchio 源码、理解 data 被"算法函数填数据"模式的最快途径。

0.7 怎么读报错（每个初学者都会遇到，照着对号入座）

报错不可怕，可怕的是看不懂报错在说什么。C++ 的"失败"分三层，每层长什么样、怎么治，下面一次讲清。

A. 编译错误（compiler error）——“代码有语法/类型问题，压根没生成 exe”

典型长相（MSVC）：

01_hello_world.cpp(48): error C2065: “buildModels”: 未声明的标识符
01_hello_world.cpp(48): error C3861: “manipulator”: 找不到标识符

怎么读：文件名(行号): error 错误码: 中文/英文描述。括号里的 48 就是出问题的行——在 VS 里双击这行报错会自动跳过去。C2065 未声明的标识符 意思是"编译器不认识 buildModels 这个名字"，通常是：忘写 #include "pinocchio/multibody/sample-models.hpp"，或忘了 using namespace pinocchio;。

常见编译错误 TOP 3：

报错片段	大白话	治法
`error C2065: “xxx”: 未声明的标识符`	名字拼错 / 没 include / 没 using	检查拼写、补头文件、补 `using namespace`
`error C2146: 语法错误: 缺少“;”`	上一行少了分号	在报错行上一行末尾补 `;`
`error C2679: 二进制“<<”: 没有找到接受...类型的运算符`	你 `cout <<` 了一个它不知道怎么打印的类型	检查该类型是不是漏了 `.translation()` 之类的取值，或变量类型写错

B. 链接错误（linker error）——“代码语法没错，但找不到实现”

典型长相：

02_smart_pointers.obj : error LNK2019: 无法解析的外部符号
   "public: __cdecl pinocchio::Model::Model(void)" (...) ，
   该符号在函数 ... 中被引用
error LNK1120: 1 个无法解析的外部命令

大白话：编译器在头文件里看到了 Model::Model() 的"声明"（知道有这个函数），但链接阶段找不到它的"实现"（机器码），因为对应的 .lib 没被链接进来。LNK2019 无法解析的外部符号 几乎总是"某个依赖库没列进 target_link_libraries"。回到根 CMakeLists.txt 的 pinocchio_deps 列表，把缺失的 .lib 补上（详见附录 B 的 B4）。

C. 运行时错误（runtime error）——“编译链接都过了，一运行就崩”

常见两种：

缺少 dll：双击 exe 弹窗"由于找不到 xxx.dll，无法继续执行代码"。原因：env/Library/bin 下的动态库不在 PATH 里。治法：在运行前执行 set PATH=%PATH%;D:\WorkSpace\cpp_WorkSpace\6Pinocchio\env\Library\bin（cmd）或从该 bin 目录启动 exe；用 VS 的 Ctrl+F5 通常会自动带上。
异常 / 断言：程序打印 加载失败: ... 后退出（本项目用 try-catch 捕获并打印 e.what()），或显示"Debug Assertion Failed"。前者看它打印的原因（多半是 URDF 路径错、文件名拼错）；后者多是数组越界、data 未初始化等逻辑错误，靠 0.6 的断点调试定位。

错误排查决策树

程序没生成 exe？
  └─ 看报错里有没有 LNK → 是：缺 .lib（B 类）→ 补 CMakeLists 链接列表
  └─ 有 C2xxx/C1xxx 错误码 → 是：编译错（A 类）→ 按行号改代码
程序生成了但一运行就崩？
  └─ 弹窗说缺 dll → 加 PATH（C 类-缺库）
  └─ 打印"加载失败/异常" → 看 e.what() 信息（C 类-逻辑/路径）
  └─ 无任何输出直接退出 → 设断点 F5 调试，看哪行崩（C 类-逻辑）

记不住没关系——附录 B 把本项目可能遇到的 10 种错误按编号列好了，需要时直接查。

0.8 数学前置课：向量与矩阵（机器人学的语言）

如果有人说"我 C++ 还行，但看代码里的 Matrix3d、cross、solve 就头大"——问题大概率出在线性代数上，不是 C++ 上。机器人学的全部计算，本质上就是向量和矩阵的运算。这一节用最直白的话补上。

0.8.1 三种"数"：标量、向量、矩阵

标量（scalar）：一个单独的数，比如 关节角度 = 1.57（弧度）。
向量（vector）：一串有序的数，既有大小又有方向。比如"位置 (1, 2, 3)"、“速度 (0.5, 0, -9.8)”。机器人学里几乎一切物理量（位置、速度、力、力矩）都是向量。
矩阵（matrix）：一个按行和列排成的"数表"。比如 3×3 的旋转矩阵、6×n 的雅可比。矩阵的本职工作是线性变换：一个矩阵乘一个向量，等于"把这个向量旋转/缩放/映射成另一个向量"。

0.8.2 向量的两个基本操作

向量加法：  [1,2,3] + [4,5,6] = [5,7,9]       把两个"箭头"首尾相接
数乘：      2 × [1,2,3] = [2,4,6]             把箭头拉长 2 倍

几何意义：向量就是一个带方向的箭头。加法 = 两个箭头首尾相接，数乘 = 箭头变长变短。

0.8.3 矩阵 × 向量 = 变换（最核心的一句）

        ┌           ┐   ┌   ┐   ┌         ┐
        │ cosθ -sinθ │   │ 1 │   │   cosθ  │
Rz(θ) × │ sinθ  cosθ │ × │ 0 │ = │   sinθ  │     （绕 Z 轴转 θ 后，X 轴指向了 (cosθ,sinθ)）
        │   0     0  │   │ 0 │   │    0    │
        └           ┘   └   ┘   └         ┘

这就是机器人学里"姿态"的本质：旋转矩阵乘一个向量，等于把那个向量按旋转矩阵描述的姿态转一下。第 4 章你会反复见到 rotation * vector。

0.8.4 三个你必须认识的特殊运算

运算	写法	几何意思	本项目哪用到
转置	`M.transpose()`	把矩阵"沿对角线翻折"（行变列）	旋转矩阵的逆 = 它的转置
点积	`a.dot(b)`	两个向量"有多同向"，结果是标量	判断两向量夹角、投影
叉积	`a.cross(b)`	两个向量"夹出的垂直向量"，结果是向量	角速度 × 位置 = 线速度的一部分

点积和叉积的直觉：

点积大 → 两个箭头朝向接近；点积为 0 → 互相垂直（这就是"正交"）。

叉积的方向永远垂直于原来两个向量所在的平面，大小等于它们围成的平行四边形面积。机器人里"角速度叉乘位置得到线速度"正是这个关系。

0.8.5 为什么机器人学离不开这些

把前面几样串起来，就得到了机器人学的两条主线：

运动学：  末端位置 = R₁·R₂·R₃ × 初始位置          （一串旋转/平移矩阵乘出来）
          即  oM_ee = oM₁ × ₁M₂ × ₂M₃      （对应 data.oMi / data.oMf）

动力学：  τ = M·q̈ + C + G  →  变成"解 M·x = b"  （解线性方程组，见第 4 章 4.5）

一句话：运动学是"矩阵乘法链"，动力学是"解线性方程组"。看懂了这一节，第 4 章的 Eigen 类型（Vector3d、Matrix3d、VectorXd）就只是这些概念在代码里的名字；第 5~10 章的算法，也只是在反复组合这些运算。

0.8.6 给你的一分钟自测

合上手册，你能回答下面三句吗？（答案在括号里，先别看）

Vector3d(1,0,0) 绕 Z 轴转 90° 后变成什么向量？（(0,1,0)）
旋转矩阵乘一个向量，相当于做什么？（把这个向量按矩阵描述的姿态旋转/变换）
τ = M·q̈ + C + G 里，如果已知 τ 想求 q̈，本质上是在做什么？（解一个形如 M·x = b 的线性方程组）

都能答上来，数学前置就过关了，直接进第 1 章。

第 1 章认识项目

1.1 Pinocchio 是什么？

先打个比方。你拿起一部手机，它里面有一块「芯片」负责运算。在机器人研发领域，Pinocchio 就是那块「动力学芯片」。

Pinocchio（发音：皮诺曹）是一个开源的 C++ 刚体动力学库，由法国国家信息与自动化研究所（INRIA）等机构开发维护。它能在极短的时间内完成机器人控制所需的核心数学计算，而且速度在同类库中数一数二。它主要解决这几类问题：

运动学（Kinematics）：给定各关节的角度，机器人的手（末端执行器）在空间中的什么位置、朝向哪里？
动力学（Dynamics）：想让机器人按某种方式运动，每个关节电机需要输出多大力矩？反过来，给定力矩，机器人会怎么动？
雅可比与微分运动学：关节速度和末端速度之间如何换算？
质心动力学：整个机器人的质心在哪里、总动量是多少？这在人形机器人平衡控制中至关重要。

真实世界里，许多著名的机器人项目都使用 Pinocchio，比如波士顿动力风格的腿足机器人研究、欧洲空间局的机械臂仿真、各种人形机器人（如 Solo、ANYmal 的部分研究栈）等。学会它，你就拿到了一把进入现代机器人研究社区的钥匙。

为什么是 C++？
Pinocchio 也有 Python 接口，为什么本项目坚持用 C++？两个原因：其一，C++ 版本是「第一现场」，所有新特性首先在 C++ 中实现，性能也最高；其二，机器人控制器最终往往要跑在实时系统上，C++ 是行业标准。对于学习者来说，通过一个真实项目同时练 C++ 和机器人学，是一举两得。

1.2 在写代码之前：必须懂的机器人学概念

本项目代码里充满了 joint、frame、nq、SE3 这样的词。先花几分钟理解下面这些概念，后面的代码会好懂很多。

1.2.1 刚体（Rigid Body）与连杆（Link）

刚体是指形状和大小完全不会改变的物体 —— 你可以把它想象成一块绝对坚硬、掰不弯也压不扁的积木。机器人的每一根「杆子」、每一个「盒子」，在力学计算中都被当作刚体，称为连杆（link）。比如人的手臂：上臂是一根连杆，前臂是另一根连杆。

1.2.2 关节（Joint）

连杆之间靠关节连接。关节限制了连杆之间的相对运动方式。本项目涉及的主要关节类型：

类型	Pinocchio 类名	说明	生活类比
旋转关节（绕 X/Y/Z 轴）	`JointModelRX/RY/RZ`	只能绕固定轴转动，1 个自由度	门的合页、人的肘关节
平移关节（沿 X/Y/Z 轴）	`JointModelPX/PY/PZ`	只能沿固定轴滑动，1 个自由度	抽屉的滑轨、电梯
固定关节	`JointModelFixed`	不能动，0 个自由度	焊接死的连接
自由浮动	`JointModelFreeFlyer`	空间中完全自由，6 个自由度	空中飞行的无人机机体

1.2.3 自由度（DOF, Degree of Freedom）

自由度是描述「机器人能独立运动的方式有多少种」的数字。人的肩膀可以前后摆、左右摆、还能旋转，相当于 3 个自由度。本项目里的示例机械臂有 6 个旋转关节，所以是 6 自由度机械臂。

在 Pinocchio 里你会频繁见到两个容易混淆的量：

nq（configuration dimension，位形维数）：描述机器人「姿态」需要多少个数字。对纯旋转关节机器人，nq 就等于关节数。
nv（velocity dimension，速度维数）：描述机器人「速度」需要多少个数字。

对于只有旋转关节的机器人，nq == nv。但二者并非永远相等：例如四元数表示旋转需要 4 个数（nq 贡献 4），但角速度只有 3 个分量（nv 贡献 3）。先记住结论：本项目用到的模型里，nq 和 nv 相等，都等于可动关节数。

1.2.4 坐标系 / 帧（Frame）

要描述「手在哪里」，必须先规定「相对于谁、用哪套坐标轴来量」。这套「带原点、带三根坐标轴」的参照物就是坐标系，Pinocchio 里叫 Frame（帧）。

机器人身上会挂很多坐标系：世界坐标系（固定在地面上）、每个关节一个坐标系、末端执行器（手）一个坐标系、传感器安装点一个坐标系……Pinocchio 的核心工作之一，就是计算任意两个坐标系之间的相对位置和朝向。

1.2.5 位姿（Pose）与 SE3

一个刚体在空间中的状态由两部分决定：位置（在哪里，3 个数字）+ 姿态（朝向哪里，3 个数字），合起来叫位姿。

Pinocchio 用 SE3 类型表示位姿。名字来自数学上的「特殊欧几里得群 Special Euclidean group (3)」，你不需要深究群论，只需要知道：一个 SE3 = 一个 3×3 旋转矩阵 R + 一个 3 维平移向量 p，等价于下面的 4×4 齐次变换矩阵：

        ┌             ┐
        │  ┌   ┐      │
 SE3 =  │  │ R │  p   │     R：3×3 旋转矩阵（描述朝向）
        │  └   ┘      │     p：3×1 平移向量（描述位置）
        │  0 0 0   1  │
        └             ┘

两个 SE3 相乘表示「变换的叠加」，SE3 求逆表示「反过来的变换」。这些在教程 01-3 里会用代码实际演示。

1.2.6 运动学与动力学：一对「正反问题」

正向运动学（Forward Kinematics, FK）：已知关节角度 q → 求末端位姿。这是「正着算」，容易。
逆运动学（IK）：已知想要的末端位姿 → 反推关节角度。这是「倒着算」，通常有多解或无解，更难（本项目暂不涉及）。
逆动力学（Inverse Dynamics）：已知期望的运动（角度、速度、加速度）→ 求所需力矩 τ。用 RNEA 算法。
正动力学（Forward Dynamics）：已知施加的力矩 τ → 求机器人实际的加速度。用 ABA 算法。这是做仿真的基础。

机器人的动力学方程（你会在教程 04 反复见到它）：

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

读法：关节力矩 τ = 质量矩阵 M 乘关节加速度 q̈，加上科氏力/离心力项 C，再加上重力项 G。现在看不懂没关系，第 8 章会把每一项掰开揉碎讲清楚。

1.3 项目目录结构

6Pinocchio/
├── CMakeLists.txt            ← 整个工程的构建总入口（CMake 脚本）
├── setup.bat                 ← 一键环境搭建脚本（conda 安装 + 编译）
├── env/                      ← conda 环境本体（Pinocchio/Eigen 等库装在这里）
├── models/
│   └── simple_robot.urdf     ← 一个手写的 3 自由度机械臂 URDF 模型文件
├── tutorials/                ← 全部学习代码，按主题分 6 组
│   ├── 01_basics/            ← 教程 01：C++ 基础与 Pinocchio 入门（3 个程序）
│   ├── 02_model/             ← 教程 02：机器人模型创建（3 个程序）
│   ├── 03_kinematics/        ← 教程 03：运动学计算（3 个程序）
│   ├── 04_dynamics/          ← 教程 04：动力学计算（3 个程序）
│   ├── 05_urdf/              ← 教程 05：URDF 模型加载（1 个程序）
│   └── 06_project/           ← 教程 06：综合实战（2 个程序）
├── build/                    ← 编译产物目录（CMake 自动生成，可随时删除重建）
├── test_mingw.cpp 等          ← 几个用于摸索 API 的临时测试程序（第 11 章说明）
└── .conda_pkgs/              ← conda 安装包缓存，无需关心

学习路线（每个目录里程序的编号就是建议顺序）：

01_basics → 02_model → 03_kinematics → 04_dynamics → 05_urdf → 06_project
 (C++基础)  (建模型)     (运动学)        (动力学)      (读文件)   (综合实战)

1.4 本项目的技术栈一览

工具	版本	角色
Pinocchio	4.1.0	机器人动力学计算核心库
Eigen	5.0.1	线性代数库（向量/矩阵运算），Pinocchio 的地基
conda (miniforge)	—	包管理器，负责安装 Pinocchio 及其一堆依赖
CMake	≥ 3.22	构建系统：描述「哪些源码编译成哪些程序」
Visual Studio 2022 (MSVC)	—	C++ 编译器
C++ 标准	C++17	Pinocchio 的最低要求

给初学者的定心丸：你不需要预先安装 Pinocchio 或 Eigen。setup.bat 会用 conda 把它们连同几十个依赖库一起装进项目内的 env/ 目录，整个过程全自动。下一章我们一步步拆解它。

第 2 章环境搭建与构建

本章解决一个问题：如何把一堆 .cpp 源码变成可以双击运行的 .exe。对初学者来说，「构建」往往比写代码更让人困惑，所以我们把它彻底讲透。

2.1 先理解「构建」这件事

C++ 是编译型语言：你写的 .cpp 文本文件电脑是看不懂的，必须经过三步变成可执行程序：

预处理：把 #include 引入的头文件内容「粘贴」进你的代码；
编译：把 C++ 代码翻译成机器指令（.obj 目标文件）；
链接：把你的代码和第三方库（Pinocchio、Eigen、urdfdom……）的机器码拼在一起，产出 .exe。

手工执行这三步要敲几十行命令，而且项目一多就管理不过来。于是出现了 CMake：你写一份「说明书」（CMakeLists.txt），告诉它「哪些源码要编译成什么程序、依赖哪些库、头文件在哪」，CMake 负责生成真正干活的编译命令。

本项目的构建链路是：

源码 .cpp ──(CMake 配置)──> Visual Studio 工程 ──(MSVC 编译+链接)──> build/bin/*.exe
                 ↑                                      ↑
        依赖库的位置由 CMakeLists.txt            库文件 .lib / 头文件 .h
        指向 conda 环境 env/                     全部来自 env/Library/

2.2 setup.bat 逐段讲解

setup.bat 是 Windows 批处理脚本（.bat = batch，双击即可运行）。它完成了「从零到编译成功」的全部步骤。我们逐段看：

@echo off
chcp 65001 >nul

@echo off：关闭命令回显，运行时不打印每条命令本身，只显示命令的输出；
chcp 65001：把控制台字符编码切换为 UTF-8，这样脚本里的中文和 ✓✗ 符号才能正常显示。

set CONDA_ROOT=D:\software\miniforge3
set CONDA=%CONDA_ROOT%\condabin\conda.bat

if not exist "%CONDA%" (
    echo ❌ 找不到 conda！请确认 miniforge 安装在 D:\software\miniforge3
    pause
    exit /b 1
)

set 是定义变量。这里假定你把 miniforge（一个轻量级的 conda 发行版）装在 setup.bat 中 CONDA_ROOT 所指向的目录。如果你的安装路径不同，改第一行（即 CONDA_ROOT 变量）即可。if not exist 检查 conda.bat 是否存在，不在就报错退出 —— 这是脚本的「自检」逻辑。

"%CONDA%" env list | findstr /C:"pinocchio_learn" >nul
if %errorlevel%==0 (
    echo 环境 pinocchio_learn 已存在，跳过创建步骤
) else (
    call "%CONDA%" create -n pinocchio_learn python=3.11 -y
)

先列出所有 conda 环境，用 findstr 查找是否已存在名为 pinocchio_learn 的环境，存在就跳过，不存在就创建。conda create -n 环境名 python=3.11 -y 的意思是：创建一个名叫 pinocchio_learn 的新环境，里面装 Python 3.11，-y 表示所有确认都自动选「是」。

什么是 conda 环境？ 把它理解成一个「独立的软件盒子」。每个盒子里可以装不同版本的库而互不干扰。本项目把所有依赖装进项目内的 env/ 盒子（根 CMakeLists 的注释用的是 conda create -p ./env，即按路径建环境），避免污染你电脑上的其他软件。

call "%CONDA%" activate pinocchio_learn
"%CONDA%" install pinocchio -c conda-forge -y

激活环境后安装 Pinocchio。-c conda-forge 表示从 conda-forge 这个软件源下载 —— Pinocchio 官方推荐这个源。这一步会自动带上所有依赖：Eigen、urdfdom（URDF 解析器）、Boost、coal（碰撞检测）、eigenpy 等几十个包。这一步耗时最长（可能几分钟到十几分钟），请耐心等待。

python -c "import pinocchio; print('Pinocchio version:', pinocchio.__version__)"

用 Python 快速验证安装成功。即使 Python 绑定验证失败，C++ 库也可能已经装好，所以脚本没有在这里直接退出。

"D:\Environment\CMake\bin\cmake.exe" .. -G "Visual Studio 17 2022" -A x64 -DCMAKE_PREFIX_PATH="%ENV_PREFIX%"
"D:\Environment\CMake\bin\cmake.exe" --build . --config Release

这两行是构建的核心，值得单独展开讲，见下一节。

2.3 CMake 命令详解

CMake 的工作方式分为「配置」和「构建」两个阶段，必须在 build/ 目录里执行（这叫做 out-of-source build，把生成物和源码分开，随时可以删掉 build 重来而不伤源码）。

第一阶段：配置（生成工程文件）

cmake .. -G "Visual Studio 17 2022" -A x64 -DCONDA_ENV="...\6Pinocchio\env"

逐个参数解释：

..：CMakeLists.txt 在上一级目录（项目根）；
-G "Visual Studio 17 2022"：-G = generator（生成器），告诉 CMake 生成 VS2022 能打开的工程文件。"Visual Studio 17 2022" 是 VS2022 的固定代号（注意 17 ≠ 2022，这是历史编号）；
-A x64：architecture，编译 64 位程序；
-DCONDA_ENV=...：-D名字=值 表示给 CMake 传入一个变量。这里把 conda 环境路径传给工程，后面 CMakeLists.txt 里的 ${CONDA_ENV} 就会取到这个值。

第二阶段：构建（真正编译）

cmake --build . --config Release

--build .：构建当前目录的工程；
--config Release：使用 Release（发布）配置，编译器会做优化、不带调试信息，运行更快。另一种是 Debug，便于调试但运行慢。

编译成功后，所有可执行程序集中在：

build/bin/01_basics/01_hello_world.exe
build/bin/01_basics/02_smart_pointers.exe
build/bin/01_basics/03_eigen_intro.exe
build/bin/02_model/...
（以此类推，每个教程一个子目录）

在命令行运行即可：

cd build\bin\01_basics
01_hello_world.exe

小贴士：你也可以直接用 Visual Studio 打开 build/PinocchioLearn.sln，在图形界面里选择目标、按 F5 调试运行，还能单步跟踪代码 —— 对学习很有帮助。

2.4 根 CMakeLists.txt 逐行讲解

这是整个工程的「总说明书」。打开 CMakeLists.txt，对照下面的讲解阅读：

cmake_minimum_required(VERSION 3.22)
project(PinocchioLearn LANGUAGES CXX)

第一行声明「本脚本要求 CMake 至少 3.22 版」；第二行声明工程名为 PinocchioLearn，语言为 C++（CXX 是 C++ 在 CMake 里的代号）。

set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)

指定使用 C++17 标准。Pinocchio 的最低要求就是 C++17。EXTENSIONS OFF 表示不启用编译器私有的语言扩展，保证代码的可移植性。

if(MSVC)
    add_compile_definitions(_CRT_SECURE_NO_WARNINGS)
    add_compile_definitions(NOMINMAX)
    add_compile_definitions(_WIN32_WINNT=0x0A00)
    add_compile_definitions(BOOST_MPL_LIMIT_LIST_SIZE=40)
    add_compile_options(/bigobj /utf-8 /Zc:__cplusplus)
endif()

这一段是 MSVC（微软编译器）专属配置，每一条都有来头，也是初学者编译报错时的常见关键词：

_CRT_SECURE_NO_WARNINGS：关掉 MSVC 对 strcpy 之类函数的一堆「不安全」警告；
NOMINMAX：非常重要。Windows 头文件会用宏定义 min/max，会跟标准库的 std::min 冲突导致诡异的编译错误，这个宏禁止 Windows 定义它们；
_WIN32_WINNT=0x0A00：声明目标 Windows 版本为 Win10（0x0A00）；
BOOST_MPL_LIMIT_LIST_SIZE=40：Boost 模板元编程的限制参数。Pinocchio 大量使用模板，不设置这个会报「list size exceeds limit」错误；
/bigobj：允许生成「大目标文件」。Pinocchio 模板展开后代码量巨大，不加这个会报 C1128: number of sections exceeded object file format limit —— 这是 Pinocchio + MSVC 的经典错误；
/utf-8：源文件按 UTF-8 解读。本项目源码里有大量中文注释，不加会出现乱码或报错；
/Zc:__cplusplus：让 __cplusplus 宏正确报告 C++ 标准版本（MSVC 的历史包袱）。

if(NOT DEFINED CONDA_ENV)
    message(FATAL_ERROR "请指定 conda 环境路径：-DCONDA_ENV=<conda环境路径>")
endif()

set(ENV_PREFIX "${CONDA_ENV}")
set(ENV_INCLUDE "${ENV_PREFIX}/Library/include")
set(ENV_LIB "${ENV_PREFIX}/Library/lib")
set(ENV_BIN "${ENV_PREFIX}/Library/bin")

检查用户是否传入了 CONDA_ENV，没传就直接报错并提示用法（FATAL_ERROR = 致命错误，立即停止）。然后基于环境路径派生出三个关键目录：头文件目录（include）、库文件目录（lib）、运行时 DLL 目录（bin）。

conda 环境的目录结构（Windows 版）：env/Library/include 下放所有头文件（如 pinocchio/、eigen3/），env/Library/lib 下放 .lib 链接库，env/Library/bin 下放 .dll 运行时库。记住这个结构，排查「找不到头文件/库」的问题就靠它。

add_library(pinocchio_deps INTERFACE)

创建一个名为 pinocchio_deps 的 INTERFACE 库。它不产生任何编译产物，纯粹是一个「打包好的依赖清单」：后面任何程序只要链接它，就自动获得正确的头文件目录和库文件列表。这是 CMake 里复用依赖配置的惯用手法。

target_include_directories(pinocchio_deps INTERFACE
    ${ENV_INCLUDE}
    ${ENV_INCLUDE}/eigen3
)

告诉编译器「去这两个目录找头文件」。为什么要单独列 eigen3 子目录？因为 Eigen 的头文件惯例是 #include <Eigen/Core>，而 conda 里 Eigen 装在 include/eigen3/Eigen/ 下，所以要把 include/eigen3 也加进搜索路径。

target_link_libraries(pinocchio_deps INTERFACE
    ${ENV_LIB}/pinocchio_default.lib
    ${ENV_LIB}/pinocchio_parsers.lib
    ...
    ${ENV_LIB}/boost_filesystem.lib
    ${ENV_LIB}/fmt.lib
    ${ENV_LIB}/tinyxml2.lib
    ...
)

这是全文件最长的一段：手工列出要链接的所有库。正常的 CMake 项目会写 find_package(pinocchio REQUIRED) 让 CMake 自动找到依赖，但 Pinocchio 的自动配置会拖出一条几十层深的依赖链，在 Windows 上经常出错。所以本项目干脆手动列全 —— 顺序大致是：

Pinocchio 核心库：pinocchio_default（核心算法）、pinocchio_parsers（URDF 等解析器）、pinocchio_collision（碰撞）、pinocchio_casadi、pinocchio_extra、pinocchio_visualizers；
Pinocchio 的直接依赖：coal（碰撞几何）、eigenpy、casadi；
URDF/SDF 解析链：urdfdom_model、urdfdom_sensor、urdfdom_world、sdformat、tinyxml2（XML 解析器）；
通用依赖：assimp（网格加载）、qhull（凸包计算）、十几个 boost_* 库、fmt/spdlog（格式化与日志）、zlib（压缩）。

链接的顺序与缺失：如果链接时报「无法解析的外部符号 unresolved external symbol」，通常就是这里少列了某个 .lib。排查方法：看错误信息里的符号名属于哪个库，把它补进列表。

add_subdirectory(tutorials/01_basics)
add_subdirectory(tutorials/02_model)
...（共 6 行）

把 6 个教程目录纳入构建。每个子目录里有自己的 CMakeLists.txt，负责定义该目录下的可执行程序。

子目录 CMakeLists 的套路（以 01_basics/CMakeLists.txt 为例）：

add_executable(01_hello_world 01_hello_world.cpp)     # 源码 → 可执行程序
target_link_libraries(01_hello_world PRIVATE pinocchio_deps)  # 链接依赖清单
set_target_properties(01_hello_world PROPERTIES
    RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin/01_basics"  # exe 输出位置
)

三行一个程序，非常规整。教程 05 的 CMakeLists 多了一行特别的：

target_compile_definitions(01_load_urdf PRIVATE
    MODEL_DIR="${CMAKE_SOURCE_DIR}/models"
)

它定义了一个编译期常量 MODEL_DIR，值被替换成 models 目录的绝对路径。这样代码里写 std::string(MODEL_DIR) + "/simple_robot.urdf" 就能找到模型文件，不用关心程序在哪里运行。CMAKE_SOURCE_DIR 是 CMake 内置变量，代表工程根目录。

2.5 动手：从零构建一遍

把流程亲手走一遍（假设 conda 环境已随 setup.bat 建好）：

cd D:\WorkSpace\cpp_WorkSpace\6Pinocchio
mkdir build
cd build
cmake .. -G "Visual Studio 17 2022" -A x64 -DCONDA_ENV="D:\WorkSpace\cpp_WorkSpace\6Pinocchio\env"
cmake --build . --config Release

配置阶段成功的标志是最后输出 -- Generating done 和 -- Build files have been written to: .../build；构建阶段成功的标志是若干 xxx.vcxproj -> ...\xxx.exe。然后运行：

bin\01_basics\01_hello_world.exe

看到 ===== 教程 01-1: Hello Pinocchio ===== 就大功告成。下一章我们开始讲解它输出的每一行背后是什么。

2.6 改了代码之后怎么办？

只需要重新执行构建阶段（配置不用重跑，CMake 会自动检测 CMakeLists 的变化）：

cd build
cmake --build . --config Release

增量编译只重编改过的文件，通常几秒钟。这也是为什么教程鼓励你「改一行、编一次、跑一次」。

2.7 编译产物都去哪了：build 目录结构详解

很多初学者编译完一脸懵：“exe 到底生成在哪？” 这一节把 build/ 目录摊开讲清楚。

CMake 采用 out-of-source build（源码和生成物分离）：所有编译产物都在 build/ 里，源码目录保持干净。编译成功后，build/ 大致长这样：

build/
├── PinocchioLearn.sln          ← Visual Studio 解决方案（双击即可用 VS 打开）
├── CMakeCache.txt              ← CMake 的配置缓存（记录了 CONDA_ENV 等变量）
├── CMakeFiles/                ← CMake 内部文件，不用管
├── bin/                        ← ★ 所有 exe 都在这
│   ├── 01_basics/
│   │   ├── 01_hello_world.exe
│   │   ├── 02_smart_pointers.exe
│   │   └── 03_eigen_intro.exe
│   ├── 02_model/  ...
│   ├── 03_kinematics/  ...
│   └── ... （每个教程一个子目录）
└── (各 .vcxproj 工程文件散落在对应子目录)

关键事实：

exe 全部集中在 build/bin/ 下，按教程分目录。运行程序就 cd 到对应的 bin/xx/ 目录执行；
每个 .exe 旁边通常还会有一份 .pdb（调试符号文件，F5 调试时用，可忽略）；
build/ 可以整个删掉然后重跑 cmake ..（配置）+ cmake --build .（编译）从头来过，源码不受影响——所以"编译乱了就删 build 重来"是安全的万能招；
运行时缺 dll 时，把 env/Library/bin 加进 PATH 后，从 build/bin/xx/ 启动 exe 即可（见 0.7 节 C 类错误）。

2.8 安装/网络问题对策（conda 下载慢或失败）

setup.bat 里 conda install pinocchio 这步最容易出问题，几乎都是网络。两种对策：

对策一：换国内镜像源（推荐）。在 setup.bat 调用 install 之前，加一行配置清华源：

call "%CONDA%" config --add channels https://mirrors.tuna.tsinghua.edu.cn/anaconda/cloud/conda-forge
call "%CONDA%" config --set channel_priority flexible
"%CONDA%" install pinocchio -c conda-forge -y

如果用的是中科大源，把上面网址换成 https://mirrors.ustc.edu.cn/anaconda/cloud/conda-forge。

对策二：确认版本与平台。Pinocchio 在 conda-forge 上提供的是 x86_64（64 位） 包，且本项目 CMake 用 -A x64。务必保证：

你的 Windows 是 64 位；
VS 安装时选了 x64 生成工具；
CMake 配置用的是 -A x64（和 setup.bat 一致）。

对策三：手动分步装。如果一键脚本中途失败，可手动在终端执行，便于看清是哪一步卡住：

D:\software\miniforge3\condabin\conda.bat create -n pinocchio_learn python=3.11 -y
D:\software\miniforge3\condabin\conda.bat activate pinocchio_learn
conda install pinocchio -c conda-forge -y

装好后再单独跑 CMake 配置与编译（cmake .. -G "Visual Studio 17 2022" -A x64 -DCONDA_ENV="...env" + cmake --build . --config Release）即可。

验证安装的"金标准"：装完后执行 python -c "import pinocchio; print(pinocchio.__version__)"，能打印 4.1.0 一类版本号，就说明 C++ 库一定也装好了（Python 绑定和 C++ 库来自同一个包）。

第 3 章 C++ 语言基础速成

这一章不是完整的 C++ 教科书，而是本项目所用到的全部 C++ 语法的最小集合。每个知识点都配有项目中的真实例子。建议通读一遍（约 40 分钟），之后读代码时遇到想不起来的语法，再回来按目录查阅。

3.1 一个 C++ 程序的骨架

先看项目里最简单的程序 test_mingw.cpp：

#include <iostream>

int main() {
    std::cout << "Hello World" << std::endl;
    return 0;
}

逐行拆解：

#include <iostream>：预处理指令。把标准库的「输入输出流」头文件引入进来，std::cout（控制台输出）就定义在里面。尖括号 <...> 表示系统/标准库头文件，双引号 "..." 表示项目自己的头文件（比如项目里写的 #include "pinocchio/multibody.hpp"）。
int main() { ... }：main 函数，每个 C++ 程序有且只有一个，程序从这里开始执行。返回值类型是 int（整数），return 0; 表示「程序正常结束」（非 0 通常表示出错）。
std::cout << "Hello World" << std::endl;：向控制台输出。<< 是「流插入运算符」，可以连续串接多个要输出的内容。std::endl 输出一个换行并刷新缓冲区。

.h 和 .hpp 有什么区别？ 都是 C++ 头文件。.hpp 只是社区约定，通常表示「纯 C++ 头文件」。Pinocchio 的头文件全部是 .hpp。

3.2 变量与基本数据类型

int steps = 100;            // 整数
double dt = 0.01;           // 双精度浮点数（小数），机器人学里的默认数值类型
double t = step * dt;       // 整数和浮点数混合运算，结果自动提升为浮点
bool flag = true;           // 布尔值：true / false
char c = 'A';               // 单个字符（本项目很少用）

命名规则：字母、数字、下划线组成，不能以数字开头。本项目遵循常见的命名风格：变量和函数用小写加下划线（q_dot、joint1_idx），类型名用大写开头（Model、Data）。

const 修饰符：表示「这个值不能再改」。

const double PI_2 = 3.14159265 / 2;
// PI_2 = 3.0;  // 编译错误！const 变量不可修改

你会在函数参数里大量见到它（3.7 节细讲）。

类型转换：不同整数类型之间转换用 static_cast<目标类型>(值)，这是 C++ 推荐的显式转换写法：

int steps = static_cast<int>(duration / dt);   // double 转 int（小数部分被截断）
JointIndex last_joint = JointIndex(model.njoints - 1);  // 函数式写法，等价于 static_cast

3.3 运算符与表达式

a + b, a - b, a * b, a / b    // 四则运算。注意：两个整数相除结果还是整数！5/2 == 2
a % b                          // 取余数：step % 20 == 0 表示 step 能被 20 整除
++i / i++                      // 自增 1（循环里最常见）
a == b, a != b, a < b, a >= b  // 比较，结果是 bool
a && b, a || b, !a             // 逻辑与、或、非
x += y                         // 等价于 x = x + y（还有 -=、*=、/=）
a ? b : c                      // 三目运算：a 为真取 b，否则取 c

项目中的真实用例：

if (step % 20 == 0) { ... }                      // 每 20 步输出一次
std::cout << (data_ptr ? "有效" : "空")          // 指针非空判断 + 三目运算
overall_max = std::max(overall_max, max_errors[i]);  // std::max 取较大值

3.4 控制流：if、for、while、switch

if / else：

if (ee_idx < model.nframes) {
    // 找到了坐标系，执行这里的代码
} else if (some_other_condition) {
    // ...
} else {
    // 以上都不满足时执行
}

for 循环（三种形态在项目里都会出现）：

// 形态1：经典 for，用索引遍历
for (int i = 0; i < model.nq; ++i) {
    std::cout << q[i];
}

// 形态2：范围 for（range-based for），C++11 引入，直接取元素不用索引
for (const auto& joint : model.joints) {
    std::cout << joint.shortname();
}

// 形态3：遍历索引类型（JointIndex 本质是整数类型）
for (JointIndex i = 0; i < model.njoints; ++i) { ... }

范围 for 里的 const auto& 先按下不表，3.9 节讲完引用和 auto 后你会完全明白。目前记住：遍历容器又不想修改元素时，就写 for (const auto& 元素 : 容器)。

while：本项目没有直接使用，但你要认识它 —— while(条件) { 循环体 }，条件为真就一直转。仿真循环理论上都可以用 while 写。

switch（出现在教程 02-2，用于根据 Frame 类型分支）：

switch (frame.type) {
    case OP_FRAME:    std::cout << "操作帧"; break;
    case JOINT:       std::cout << "关节帧"; break;
    case BODY:        std::cout << "连杆帧"; break;
    default:          std::cout << "其他";   break;
}

switch 适合「一个变量对多个固定取值」的分支。每个 case 后的 break 不能忘，否则会「穿透」到下一个分支。

3.5 函数

函数是「一段起了名字、可以反复调用的代码」。项目里的典型函数：

//  返回类型  函数名(参数列表)
void printModelInfo(Model model, const std::string& label) {
    std::cout << "[" << label << "] 关节数=" << model.njoints << std::endl;
}

返回类型：void 表示不返回任何东西；其他函数如 double position(double t) 返回一个 double；
参数：调用时传入。参数可以有默认值（本项目未用到，但标准库大量使用）；
return：返回结果并结束函数。

函数重载：同名函数可以有多个版本，只要参数列表不同。Pinocchio 的 forwardKinematics 就是典型：

forwardKinematics(model, data, q);              // 3 参数版：只算位置
forwardKinematics(model, data, q, q_dot);       // 4 参数版：位置 + 速度
forwardKinematics(model, data, q, q_dot, q_ddot); // 5 参数版：位置 + 速度 + 加速度

编译器根据你传入的参数个数和类型自动选择对应版本 —— 这就是重载。

std:: 前缀的函数：C++ 标准库函数都住在 std 命名空间里。项目常用的：

std::sin(x), std::cos(x)     // 三角函数（弧度制），需要 #include <cmath>
std::max(a, b)               // 较大值，需要 #include <algorithm>
std::string(50, '-')         // 构造 50 个 '-' 组成的字符串（用来画分隔线）

3.6 容器：std::vector 与 std::string

std::vector：可以自动变长的数组

#include <vector>

std::vector<double> max_errors;          // 空的 double 列表
max_errors.push_back(0.5);               // 末尾追加一个元素
max_errors.size();                       // 元素个数（返回 size_t，一种无符号整数）
max_errors[i];                           // 按索引访问（从 0 开始）

std::vector<std::shared_ptr<Model>> models;   // 元素是智能指针的 vector（教程 01-2）

教程 06-1 还展示了两个进阶用法：

trajectories_.reserve(nq_);      // 预先分配 nq_ 个元素的内存，避免反复扩容
trajectories_.emplace_back(q0[i], qf[i], 0.0, 0.0, T);  // 原地构造一个元素

emplace_back 和 push_back 的区别：push_back 是「先造好一个对象再放进去」，emplace_back 是「把构造参数传进去，直接在容器里造」。对于复杂对象，后者效率更高。初学阶段二者混用问题不大，认识即可。

std::string：字符串

#include <string>

std::string urdf_path = std::string(MODEL_DIR) + "/simple_robot.urdf";  // 拼接
std::cout << model.name;     // Pinocchio 里的 name 就是 std::string，可直接输出

+ 可以拼接字符串；std::string("abc") 是把 C 风格字符串 "abc" 包装成 string 对象。

3.7 引用（Reference）：C++ 最容易被误解的概念

先看一个现象。下面两种写法，哪种「更高效」？

void f1(std::vector<double> v);      // 按值传递：把整个 vector 复制一份
void f2(const std::vector<double>& v); // 按 const 引用传递：不复制，只给原名起个别名

答案是 f2。引用（&）就是给已存在的变量起别名，不产生拷贝。理解引用是理解 C++ 性能的关键。

int a = 10;
int& ref = a;     // ref 是 a 的别名（注意 int& 中的 &）
ref = 20;         // 通过别名修改，a 也变成 20

项目中的三大应用场景：

场景一：函数参数用 const T&，只读且免拷贝 —— 这是 C++ 最常见的参数写法：

void printSE3(const std::string& name, const SE3& transform);
//            ^^^^^^^^^^^^^^^^^^       ^^^^^^^^^^
//            字符串不拷贝              SE3 对象不拷贝，且函数内不能修改它

场景二：函数参数用 T&（不带 const），让函数能「写回」结果 —— Pinocchio 的核心模式：

void buildModels::manipulator(Model& model, bool mimic);
// 调用：
Model model;                              // 先准备一个空模型
buildModels::manipulator(model, false);   // 函数往 model 里「填东西」
// 调用结束后，model 已经被填满了

这就是 Pinocchio 大量 API 的风格：调用者创建结果对象并传进去，函数负责填充。

场景三：引用已有对象的成员，避免拷贝：

const SE3& pose = data.oMi[i];    // pose 是 data.oMi[i] 的别名，零拷贝读取
const auto& joint = model.joints[i];  // auto& 自动推导类型的引用

引用 vs 指针：引用必须初始化、永远指向最初的对象、语法上像普通变量；指针可以为空、可以改指向、要用 * 和 -> 访问。现代 C++ 的准则：优先用引用，只有「可能为空」或「需要改指向」时才用指针。

3.8 指针与智能指针

原始指针（了解即可）

指针存储的是「对象的内存地址」。

Model m;
Model* p = &m;          // & 取地址：p 指向 m
std::cout << p->njoints; // -> 通过指针访问成员

裸指针最大的问题是「谁负责释放内存」容易搞不清。现代 C++ 的答案是：用智能指针，忘掉手动 new/delete。

std::shared_ptr：共享所有权

#include <memory>

auto model_ptr1 = std::make_shared<Model>();   // 在堆上创建 Model，由智能指针管理
auto model_ptr2 = model_ptr1;   // 两个指针共享同一对象，内部「引用计数」变为 2
model_ptr1.use_count();         // 查询有几个指针共享（返回 2）
model_ptr1->njoints;            // 像普通指针一样用 -> 访问成员
*model_ptr1;                    // * 解引用，得到对象本身
model_ptr2.reset();             // ptr2 退出共享，计数减 1；计数归零时对象自动销毁
model_ptr1.get();               // 取出裸指针（只读借用，切勿 delete）

核心机制是引用计数：每个 shared_ptr 都记得「现在有几个兄弟在共享这个对象」，最后一个离开的人负责关灯（释放内存）。教程 01-2 用代码完整演示了这个过程。

std::unique_ptr：独占所有权

auto data_ptr = std::make_unique<Data>(model1);  // 创建并独占
// auto data_ptr2 = data_ptr;     // 编译错误！unique_ptr 不允许拷贝
auto data_ptr2 = std::move(data_ptr);  // 只能「搬家」（移动），原指针变为空
if (data_ptr) { ... }             // 移动后 data_ptr 为空，条件为假

unique_ptr 没有引用计数的开销，更轻量。当你明确「这个对象只有一个主人」时用它 —— 教程 06-2 的控制器就持有 std::unique_ptr<Data>。

什么时候该用哪个？

本项目的实际结论很简单：Pinocchio 的 Model/Data 都是值语义的对象，直接 Model model; 创建在栈上就好，大多数时候根本不需要智能指针。智能指针主要用于：需要把对象存进容器、需要跨越函数边界传递所有权、或在类里管理生命周期时。

3.9 auto 与范围 for 的完整真相

auto 让编译器自动推导变量类型。Pinocchio 的类型名经常长得吓人（模板套模板），auto 能救命：

auto model_ptr = std::make_shared<Model>();   // 推导为 std::shared_ptr<Model>
const auto& joint = model.joints[i];          // 推导为关节类型的 const 引用

组合记忆卡片：

写法	含义	用途
`auto x = ...`	推导类型，拷贝一份	轻量对象
`const auto& x = ...`	推导类型，只读引用，不拷贝	读大对象，最常用
`auto& x = ...`	推导类型，可写引用	需要修改原对象

3.10 类（class）：把数据和操作打包

教程 06 是「类的实战课」。先看一个最小例子 —— 三次多项式轨迹规划器：

class CubicPolynomial {
public:                                    // 公开区：外部可以访问
    CubicPolynomial(double q0, double qf, double v0, double vf, double T)
        : q0_(q0), qf_(qf), v0_(v0), vf_(vf), T_(T) {   // 构造函数 + 初始化列表
        a0_ = q0;
        a1_ = v0;
        a2_ = (3.0 * (qf - q0) - (2.0 * v0 + vf) * T) / (T * T);
        a3_ = (2.0 * (q0 - qf) + (v0 + vf) * T) / (T * T * T);
    }

    double position(double t) const {      // const 成员函数：承诺不修改对象
        return a0_ + a1_ * t + a2_ * t * t + a3_ * t * t * t;
    }

private:                                   // 私有区：只有类自己能访问
    double q0_, qf_, v0_, vf_, T_;         // 成员变量（后缀 _ 是命名习惯）
    double a0_, a1_, a2_, a3_;
};

// 使用：
CubicPolynomial traj(0.0, 1.0, 0.0, 0.0, 3.0);  // 创建对象（自动调用构造函数）
double q = traj.position(1.5);                   // 调用成员函数

要点逐个解释：

构造函数：与类同名的特殊函数，创建对象时自动执行，负责初始化。本例根据起止条件算出多项式系数。
初始化列表 : q0_(q0), qf_(qf), ...：在对象「出生的一瞬间」直接把成员变量初始化好，比在函数体里赋值效率更高，是 C++ 的惯用写法。q0_(q0) 读作「用参数 q0 初始化成员 q0_」。
public / private：封装。外部代码只能看到 public 接口，内部细节（系数怎么算的）藏在 private 里，随便改都不影响使用者。
const 成员函数：position(double t) const 末尾的 const 承诺「这个函数不会修改对象」。凡是「只查询不修改」的函数都应标 const —— 这样 const 对象/引用也能调用它。
成员变量命名：q0_ 这样的下划线后缀用来区分成员变量和局部变量，避免 q0 = q0 这种让人困惑的代码。

教程 06-2 的 InverseDynamicsController 还演示了在类里持有资源：

class InverseDynamicsController {
public:
    InverseDynamicsController(const Model& model, double Kp, double Kd)
        : model_(model), Kp_(Kp), Kd_(Kd) {
        data_ = std::make_unique<Data>(model);   // 控制器拥有自己的 Data
    }
private:
    const Model& model_;      // 引用外部模型（借用，不拥有）
    std::unique_ptr<Data> data_;  // 独占一个计算数据对象
    double Kp_, Kd_;
};

注意两种「持有」的区别：model_ 是引用 —— 模型由外部创建，控制器只是借用（所以要求控制器的寿命不能超过模型）；data_ 是 unique_ptr —— 控制器自己创建、自己销毁。

struct 和 class 的区别：几乎只有默认访问权限不同（struct 默认 public，class 默认 private）。Pinocchio 里的 Motion、Force、SE3 等数据类型本质上都是 struct。你在项目里看到 struct 不用怕，当它是「公开的类」即可。

3.11 模板（Template）：类型也能当参数

Eigen::VectorXd、std::vector<double> 里的 <...> 就是模板参数。模板让「同一份代码适配多种类型」。项目里你自己写模板的地方只有一处（教程 01-3 的打印辅助函数）：

template<typename VectorType>                    // 声明：VectorType 是「类型占位符」
void printVector(const std::string& name, const VectorType& v) {
    for (Eigen::Index i = 0; i < v.size(); ++i) {
        std::cout << v(i);                       // 只要能调 .size() 和 (i) 的类型都行
    }
}

printVector("位置", position);    // VectorType = Eigen::Vector3d
printVector("q", q_neutral);      // VectorType = Eigen::VectorXd

调用时编译器自动「实例化」出对应类型的版本。理解到这一层，足够读懂本项目 95% 的代码。剩下的模板深水区（Pinocchio 内部大量使用 CRTP 模板元编程）你不需要掌握，只需要接受一个事实：这正是 Pinocchio 编译慢、报错长、需要 /bigobj 的根本原因。

顺带认识两个会出现在报错和代码里的模板相关写法：

std::vector<std::shared_ptr<Model>> models;  // 模板里套模板
data.Minv.template triangularView<Eigen::Upper>();  // 「模板成员函数」的显式写法，
                                                    // 在依赖模板参数的对象上调用
                                                    // 模板成员时必须加 template 关键字

3.12 命名空间（namespace）

命名空间是「类型的姓氏」，防止不同库的类型重名。pinocchio::Model 表示「pinocchio 家的 Model」。

using namespace pinocchio;   // 之后写 Model 就等同于 pinocchio::Model
using namespace Eigen;       // 同理，Eigen 家的类型也可以省写前缀

也可以在函数内部局部使用（教程 test_new_api.cpp 就是这么做的）：

int main() {
    using namespace pinocchio;   // 只在这个函数内生效
    Model model;
    ...
}

为什么注释里说「大型项目中要谨慎使用」？ 因为 using namespace 会把整个名字空间引入当前作用域，容易引发命名冲突。在 .cpp 文件的实现代码里用没问题；在头文件里则应避免。

3.13 异常处理（try / catch）

当程序遇到无法继续的错误（文件打不开、URDF 格式错误），C++ 的机制是「抛出异常」。教程 05 用它包裹 URDF 加载：

try {
    Model model;
    pinocchio::urdf::buildModel(urdf_path, model);   // 可能抛异常的代码
    // ...正常使用 model...
} catch (const std::exception& e) {                  // 捕获所有标准异常
    std::cerr << "加载失败: " << e.what() << std::endl;  // e.what() 是错误描述
    return 1;                                        // 以错误码退出
}

std::cerr 是「错误输出流」，和 std::cout 用法相同，语义上用于报错信息。有了 try-catch，程序不会「闪退」，而是优雅地告诉你哪里出了问题。

3.14 std::function 与类型别名（using）

教程 06-2 出现了：

using TrajectoryFunc = std::function<Eigen::VectorXd(double)>;

using A = B; 是类型别名：给类型 B 起个简称 A。读作「TrajectoryFunc 就是『接收 double、返回 VectorXd 的函数』类型」。
std::function<返回值(参数)> 是一个「万能函数容器」，可以装普通函数、lambda 表达式等任何可调用对象。

顺带一提lambda 表达式（匿名小函数，C++11 引入），语法是 [捕获](参数) { 函数体 }：

auto square = [](double x) { return x * x; };   // 定义一个立即使用的函数对象
square(3.0);   // 9.0

本项目源码里没有直接写 lambda，但它是现代 C++ 的标配，且 06-2 的知识点列表提到了它，这里先混个脸熟。

3.15 预处理指令补充：#define

#define _USE_MATH_DEFINES   // 必须写在 #include <cmath> 之前

#define 名字值 是文本替换宏。_USE_MATH_DEFINES 是 MSVC 的特殊开关：定义它之后，<cmath> 才会提供 M_PI（圆周率）这类常量。注意 Pinocchio 4.x 自己提供了 PI<double>()，两种写法在项目中都出现了，推荐用后者。

另外 CMake 的 target_compile_definitions(MODEL_DIR="...") 本质上也是给编译器加一个全局 #define，所以代码里能直接用 MODEL_DIR 这个名字。

3.16 输出格式化：让打印更整齐

#include <iomanip>

std::cout << std::fixed << std::setprecision(4);  // 固定小数点显示，保留 4 位
std::cout << std::left << std::setw(30) << name;  // 左对齐，占 30 字符宽

这些是「流操纵器」，插入到 << 链条里改变后续输出的格式。教程代码用它们让仿真数据的表格整齐对齐。

3.17 内存模型：栈（stack）与堆（heap）——理解指针/引用的地基

前面讲了引用、指针、智能指针，但"为什么需要它们"的答案，藏在 C++ 的内存模型里。这部分不懂，后面 shared_ptr/unique_ptr 永远像黑魔法。花五分钟看完，绝对值。

C++ 程序运行时，内存主要分两大块：

┌───────────────────────────────────────────────┐
│  栈 (stack)          │  堆 (heap)              │
│  - 自动管理           │  - 手动/智能指针管理     │
│  - 函数里的局部变量    │  - 用 new / make_shared  │
│    都在这             │    申请的大块对象        │
│  - 函数结束自动销毁    │  - 生命周期由你/智能指针 │
│  - 速度快、容量小      │    决定，容量大          │
└───────────────────────────────────────────────┘

栈上的变量（最常见）：

void f() {
    Model model;          // model 在"栈"上，函数 f 结束时自动销毁
    int n = model.njoints; // n 也在栈上
}                          // 离开 f，model 和 n 自动消失

栈变量的特点：出生在函数开始、死亡在函数结束，你不用操心释放。绝大多数 Pinocchio 代码（Model model; Data data(model);）就是这样用的——简单、安全、快。

堆上的变量（需要"活过"函数，或很大/要共享时）：

Model* p = new Model();   // 在"堆"上造一个 Model，p 只是个指向它的指针
// ... 用 p->njoints ...
delete p;                  // 必须手动 delete，否则内存泄漏

裸 new/delete 很容易忘写 delete 或重复 delete 导致崩溃。所以现代 C++ 用智能指针接管堆内存（见 3.8 节）：std::make_shared<Model>() 在堆上创建，shared_ptr 用引用计数自动释放，unique_ptr 独占并在退出作用域时释放。

为什么这关系到引用和指针？ 一句话总结：

你想要什么	用什么	原因
函数内临时用、用完就扔	栈上对象 + `const T&` 只读传参	快、自动回收
让函数"填"出一个结果	栈上对象 + `T&` 引用传参（如 `buildModels::manipulator(model, ...)`）	不拷贝、函数直接改原对象
对象要跨函数长期存在、或被多人共享	堆上对象 + `shared_ptr`	生命周期可控、引用计数自动释放
对象只属于某一个主人（如控制器持有 Data）	堆上对象 + `unique_ptr`	独占、零引用计数开销

记忆锚点：本项目里 Model model; Data data(model); 创立的对象都在栈上，全自动管理——所以"大多数时候根本不需要智能指针"（3.8 节原话）。智能指针只在"要把对象塞进 vector、要跨函数传所有权、要在类里管生命周期"时才出场。理解了栈/堆，你就知道它们为什么这么设计。

栈溢出警告（进阶了解）：栈容量有限（通常几 MB）。如果你在函数里写 double huge[10'000'000]; 这种巨型数组，会"栈溢出"崩溃。大数组/大矩阵要用 Eigen::VectorXd（内部在堆上分配）或 std::vector（堆上）。这也是为什么关节向量用 VectorXd 而不是固定大小数组。

3.18 本章 C++ 速成课小结与自查清单

读完第 3 章，你应该能凭记忆回答下面的问题。答不上来的回去看对应小节。

#	自查问题	对应小节
1	`int main()` 是什么？程序从哪开始、怎么算"正常结束"？	3.1
2	`double` 和 `int` 相除结果是什么类型？`5/2` 等于几？	3.2、3.3
3	`for (const auto& x : container)` 每个词分别什么意思？	3.4、3.9
4	函数重载是什么？`forwardKinematics` 为什么能写 3/4/5 个参数版本？	3.5
5	`std::vector` 的 `push_back` 和 `emplace_back` 区别？	3.6
6	引用 `&` 和指针 `*` 最关键的三个区别是什么？	3.7
7	`shared_ptr` 和 `unique_ptr` 分别什么时候用？`.get()` 取到裸指针后能 `delete` 吗？	3.8
8	`auto`、`const auto&`、`auto&` 三者的取舍？	3.9
9	构造函数初始化列表 `: q0_(q0)` 为什么比在函数体里赋值好？	3.10
10	`template<typename T>` 是干嘛的？为什么 Pinocchio 编译慢？	3.11
11	栈和堆有什么区别？为什么本项目大多用栈上对象？	3.17

给纯新手的最后一句：第 3 章不需要一次背熟。把它当"字典"——读教程代码时遇到不认识的语法（const auto&、emplace_back、->），回来查对应小节即可。真正记住它们的是"改代码—编译—看报错"这个循环，而不是反复读。

第 4 章 Eigen 快速入门：Pinocchio 的数学基石

Pinocchio 里一切数值计算 —— 位置、旋转、力矩、雅可比 —— 都建立在 Eigen 这个线性代数库之上。Eigen 的特点是纯头文件：它没有 .lib 库文件，全部实现写在头文件里（所以你在 CMakeLists 里找不到 eigen.lib，只需要配 include 路径）。

本章覆盖项目用到的全部 Eigen 操作。学完后，教程 01-3 的代码你将毫无障碍。

4.1 Eigen 的核心类型一览

类型	含义	在本项目里表示什么
`Eigen::Vector3d`	3 维向量（double）	位置、速度、力、角速度
`Eigen::Matrix3d`	3×3 矩阵	旋转矩阵、惯性张量
`Eigen::Matrix4d`	4×4 矩阵	齐次变换矩阵
`Eigen::VectorXd`	动态大小向量	关节角度 q、速度 v、力矩 τ
`Eigen::MatrixXd`	动态大小矩阵	雅可比矩阵、质量矩阵

命名规律：Vector/Matrix + 大小 + 字母。3 表示固定大小 3，X 表示动态大小，d 表示 double。于是 Vector3f 就是 float 版的 3 维向量 —— 举一反三。

4.2 Vector3d：三维向量的创建与运算

// 四种创建方式
Eigen::Vector3d position{1.0, 2.0, 3.0};              // 列表初始化（项目首选）
Eigen::Vector3d v2(0.5, 0.0, -9.8);                   // 构造函数形式
Eigen::Vector3d zero = Eigen::Vector3d::Zero();       // 静态方法造零向量
Eigen::Vector3d ones = Eigen::Vector3d::Ones();       // 全 1
Eigen::Vector3d ex   = Eigen::Vector3d::UnitX();      // (1,0,0)，同理 UnitY/UnitZ

// 访问元素：两种方式等价
position.x();  position.y();  position.z();
position(0);   position(1);   position(2);
position[0];   // 也可以

// 运算（和普通数学一样直观）
Eigen::Vector3d c = position + v2;        // 加法
Eigen::Vector3d d = position * 2.0;       // 数乘
double len = position.norm();             // 长度（模）
double dp  = position.dot(v2);            // 点积（结果是标量）
Eigen::Vector3d cp = position.cross(v2);  // 叉积（结果是向量）

为什么向量要单独造一个库？ 因为机器人学里 3 维向量运算是绝对高频操作。Eigen 对这些运算做了极致优化（SIMD 指令、编译期展开），写起来又和数学公式几乎一一对应。

4.3 Matrix3d：矩阵与旋转

Matrix3d I = Matrix3d::Identity();   // 单位矩阵

// 用「逗号初始化器」逐行填矩阵 —— Eigen 的招牌语法：
double angle = PI<double>() / 2.0;   // 90°
Matrix3d rotation;
rotation << std::cos(angle), -std::sin(angle), 0,   // 第 1 行
            std::sin(angle),  std::cos(angle), 0,   // 第 2 行
            0,                0,               1;   // 第 3 行

<< 在 Eigen 里被重载成「按行优先顺序填元素」的语法，从左上角开始，每行从左到右。上面这个矩阵正是绕 Z 轴旋转 90° 的旋转矩阵：

Rz(θ) = [ cosθ  -sinθ   0 ]
        [ sinθ   cosθ   0 ]
        [   0      0    1 ]

旋转矩阵怎么用？矩阵乘向量 = 把向量旋转到新朝向：

Eigen::Vector3d rotated = rotation * Eigen::Vector3d{1.0, 0.0, 0.0};
// (1,0,0) 绕 Z 轴转 90° 后变成 (0,1,0) —— 自己画个坐标系验证一下！

其他常用操作：

m.transpose();      // 转置
m.inverse();        // 逆矩阵（旋转矩阵的逆恰好等于转置）
m * n;              // 矩阵乘法
m(i, j);            // 访问第 i 行第 j 列元素（都从 0 数）
m.rows(); m.cols(); // 行数、列数

旋转矩阵的直观理解：把坐标系的三根轴（X、Y、Z）旋转后，新的三根轴方向作为列向量排成的矩阵，就是旋转矩阵。它是「坐标系姿态」的完整描述。

另一种构造旋转的方式（教程 03-3 用到）：

Eigen::Matrix3d R = (Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ()))
                        .toRotationMatrix();
// AngleAxisd：绕「指定轴」转「指定角度」，最直观的旋转描述

4.4 VectorXd / MatrixXd：运行时才知道大小的向量矩阵

机器人自由度各不相同 —— 机械臂 6 个关节，人形机器人 30+ 个。所以关节向量的长度必须运行时决定：

// 构造时指定大小
Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv);    // n 维零向量
Eigen::VectorXd q = Eigen::VectorXd::Random(model.nq);  // n 维随机向量（各分量 -1~1）

// 访问与修改
q[0] = 0.5;      q(i) 等价写法
q.size();        // 长度

// 整体运算
q += q_dot * dt;            // 向量加法 + 数乘，一行完成半隐式欧拉积分
double n = q.norm();        // 范数（长度）
double m = q.cwiseAbs().maxCoeff();  // 各分量取绝对值后的最大值

cwise 前缀表示「逐元素（component-wise）运算」：cwiseAbs() 逐元素绝对值、cwiseProduct() 逐元素相乘（区别于点积和矩阵乘）。

块操作（block）：从大矩阵里切出一块，雅可比分析的核心工具：

Eigen::MatrixXd J = ...;                        // 6×n 的雅可比
J.block(0, 0, 3, J.cols());   // 从(0,0)开始，3 行 n 列 → 角速度部分
J.block(3, 0, 3, J.cols());   // 从(3,0)开始，3 行 n 列 → 线速度部分

block(起始行, 起始列, 行数, 列数)。类似地，向量用 segment(起始, 长度) 切片：

Eigen::VectorXd big(6);
big << a, b;                    // 拼接两个 3 维向量成 6 维
big.segment(0, 3);              // 前 3 个元素
big.segment(3, 3);              // 后 3 个元素

4.5 解线性方程组：不要求逆

动力学里反复出现 M · q̈ = b（已知 M 和 b，求 q̈）。初学者直觉是 q̈ = M⁻¹·b，但数值计算中显式求逆既慢又不稳定。Eigen 提供更专业的「求解器」：

Eigen::VectorXd x = M.fullPivLu().solve(b);   // 用 LU 分解解 M x = b

fullPivLu() 是选主元 LU 分解，适合通用方阵；.solve(b) 直接给出解。项目里两种写法都有（教学目的），你要都认识。

对于超定/欠定方程（雅可比求伪逆），用 SVD：

Eigen::JacobiSVD<Eigen::MatrixXd> svd(J_com, Eigen::ComputeThinU | Eigen::ComputeThinV);
svd.setThreshold(1e-6);                        // 小于该阈值的奇异值视为 0（数值稳定）
Eigen::VectorXd q_dot = svd.solve(v_desired);  // 最小范数解（伪逆效果）

这正是教程 04-3「想让质心以某速度运动，反推关节速度」所用到的数学工具。

4.6 SE3：Pinocchio 的位姿类型（Eigen 之上的抽象）

SE3 不是 Eigen 的类型，而是 Pinocchio 定义的，但内部由 Eigen 的 Matrix3d + Vector3d 构成：

// 创建：SE3(旋转矩阵, 平移向量)
SE3 T(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1.0, 0.0, 0.0));
SE3 id = SE3::Identity();                    // 恒等变换：不旋转、不平移

// 读取
T.translation();          // 平移部分 p（Vector3d）
T.rotation();             // 旋转部分 R（Matrix3d）
T.toHomogeneousMatrix();  // 转成 4×4 齐次矩阵（打印观察用）

// 运算
SE3 T3 = T1 * T2;         // 变换复合：先 T2 后 T1
SE3 Ti = T.inverse();     // 逆变换
Eigen::Vector3d p2 = T.actOnEigenObject(p);  // 把变换施加到一个点上

记忆卡片：

oMi[i]：origin（世界系）到关节 i 的变换（存在 Data 里）；
oMf[i]：世界系到坐标系（frame）i 的变换；
iMf、jointPlacements 等类似命名，都是「A到B的SE3变换记作 AMB」的套路。

4.7 打印 Eigen 对象的小技巧

std::cout << com.transpose() << std::endl;

.transpose() 把列向量转成行向量，打印出来就是一行数字，这是项目测试代码里常用的快速打印方式。正式场合（要控制小数位数）则用手写循环 + std::setprecision，教程里两种都有。

第 5 章教程 01 详解：C++ 基础与 Pinocchio 入门

对应目录：tutorials/01_basics/，共 3 个程序。
运行前置：已按第 2 章完成编译。

5.1 程序 01_hello_world：你的第一个 Pinocchio 程序

它做了什么

创建一个内置的 6 自由度示例机械臂模型，打印它的基本信息（关节数、自由度数等），再遍历列出所有关节和坐标系。这是理解 Pinocchio「Model / Data」二元结构的第一课。

代码逐段讲解

头部：包含与命名空间

#include <iostream>       // 标准输入输出流（C++ 的"打印"工具）
#include <memory>         // 智能指针工具
#include <string>         // 字符串类型

#include "pinocchio/multibody.hpp"             // Model 和 Data 类
#include "pinocchio/multibody/sample-models.hpp" // 示例模型生成器

using namespace pinocchio;

pinocchio/multibody.hpp 是 Pinocchio 最核心的头文件，Model、Data 都在里面。第二行的 sample-models.hpp 提供「造示例机器人」的函数。

创建模型

Model model;                          // ① 声明一个空模型对象
buildModels::manipulator(model, false); // ② 往里面填入 6 自由度机械臂

这是 Pinocchio 4.x 的典型 API 风格（回忆 3.7 节：引用参数当输出）。逐词解读第二行：

buildModels 是一个命名空间（「造模型」工具箱）；
manipulator 是其中的函数：造一个机械臂；
第一个参数 model 是输出参数，函数把造好的结构填进去；
第二个参数 false 表示不使用 mimic（仿制）关节 —— 即每个关节都独立控制。

版本注意：如果你在网上看到旧教程写 buildSampleModelManipulator(model)，那是 Pinocchio 3.x 的写法，4.x 已改为 buildModels::manipulator(model, ...)。项目根目录的 test_pinocchio.cpp 用的就是旧 API，编译它会报错 —— 这正好是「版本差异」的活教材（第 11 章详述）。

读取模型信息

std::cout << "模型名称: " << model.name << std::endl;
std::cout << "关节数量: " << model.njoints << std::endl;
std::cout << "自由度 (DOF): " << model.nq << std::endl;
std::cout << "速度空间维度: " << model.nv << std::endl;
std::cout << "坐标系(Frame)数量: " << model.nframes << std::endl;

model.name、model.njoints 等是 Model 的公开成员变量，直接用 . 读取。运行后你会看到大致如下的输出：

模型名称: manipulator
关节数量: 7
自由度 (DOF): 6
速度空间维度: 6
坐标系(Frame)数量: 13

咦，6 个关节的机械臂为什么 njoints = 7？ 因为索引 0 是一个特殊的「universe（宇宙）」关节，代表固定的世界坐标系，它没有自由度。于是：njoints = 可动关节数 + 1。这个 +1 是初学者最容易踩的坑，请刻在脑子里。

创建 Data 对象

Data data(model);   // 用 model 构造一个配套的 Data

到这里必须讲清楚 Pinocchio 最重要的设计 —— Model 与 Data 分离：

┌───────────────────── Model（静态，只读）─────────────────────┐
│  机器人长什么样：关节类型、数量、连杆长度、质量、惯性……       │
│  特点：创建一次，永不改变；可被多个 Data 共享（多线程友好）   │
└──────────────────────────────────────────────────────────────┘
                            │ 构造时绑定
┌───────────────────── Data（动态，可写）──────────────────────┐
│  计算的工作区：当前位姿 oMi/oMf、雅可比 J、力矩 tau、质心……   │
│  特点：每次算法调用都会更新它；用多少建多少                   │
└──────────────────────────────────────────────────────────────┘

类比：Model 是乐谱（固定不变），Data 是演奏时的草稿纸（每次演奏都在上面写写画画）。所有算法函数的签名都是 算法(model, data, 输入...)：从 model 读结构，把结果写进 data。

遍历关节与坐标系

for (JointIndex i = 0; i < model.njoints; ++i) {
    const auto& joint = model.joints[i];      // model.joints 是关节数组
    std::cout << "  关节 " << i << ": "
              << joint.shortname()            // 关节类型简称，如 "RZ"
              << " (nq=" << joint.nq()        // 该关节贡献的配置维数
              << ", nv=" << joint.nv() << ")" << std::endl;
}

JointIndex / FrameIndex 只是整数类型的别名（语义化命名，让代码自我解释：这个索引是用来索引关节的）。joint.shortname() 是成员函数调用（注意带括号，与成员变量 model.njoints 不带括号形成对比）。

本节小结

概念	一句话
Model	机器人的静态蓝图
Data	与 Model 绑定的计算工作区
universe 关节	索引 0 的固定世界关节，所以 njoints 比可动关节多 1
nq / nv	位形维数 / 速度维数

5.2 程序 02_smart_pointers：智能指针与值语义

这个程序的主线其实有两个：一是演示 C++ 智能指针（语法课），二是告诉你一个反直觉的事实：Pinocchio 的 Model 拷贝极其便宜。

关键代码讲解

Model 的值语义

void printModelInfo(Model model, const std::string& label) { ... }
//                  ^^^^^^^^^^^ 按值传递 —— 会拷贝一份 Model

Model model1;
buildModels::manipulator(model1, false);
printModelInfo(model1, "model1");     // 传值拷贝
Model model2 = model1;                // 再拷贝一次

按我们在 3.7 节的知识，「按值传递会复制整个对象」，而 Model 里装着那么多关节数据，拷贝不应该很贵吗？源码注释给出的答案是：Pinocchio 4.x 的 Model 内部采用了 pimpl 惯用法（pointer to implementation：对象里只存一个指向真正数据的指针），所以拷贝 Model 只是拷贝了一个「共享句柄」，开销极低。

对初学者的实际建议：虽然拷贝便宜，但传大对象时首选仍是 const Model&，这是 C++ 社区的良好习惯，也永远不会错。

shared_ptr 三连

auto model_ptr1 = std::make_shared<Model>();   // 造对象 + 包装成 shared_ptr
buildModels::manipulator(*model_ptr1, false);  // *解引用 取出对象本身传给 API

auto model_ptr2 = model_ptr1;   // 共享，引用计数 → 2
auto model_ptr3 = model_ptr1;   // 再共享，引用计数 → 3

model_ptr1.use_count();         // == 3
model_ptr2.reset();             // ptr2 放手，计数 → 2，对象仍活着

*model_ptr1 的用法值得注意：很多 Pinocchio API 要的是「对象本身」而不是指针，所以要先解引用。而 model_ptr1->njoints 则是「通过指针访问成员」。两个符号的分工：* 取对象，-> 取成员。

unique_ptr 与 std::move

auto data_ptr = std::make_unique<Data>(model1);   // 独占的 Data
auto data_ptr2 = std::move(data_ptr);             // 所有权搬家
std::cout << (data_ptr ? "有效" : "空");          // 打印"空"

std::move 读作「搬家」而非「移动数据」：数据原地不动，只是换了主人。搬家后原指针变成空（nullptr），空指针在布尔判断中为假。

容器存智能指针

std::vector<std::shared_ptr<Model>> models;
for (int i = 0; i < 3; ++i) {
    auto m = std::make_shared<Model>();
    buildModels::manipulator(*m, false);
    models.push_back(m);     // 三个智能指针共享各自对象
}

为什么容器里放 shared_ptr 而不是直接放 Model？这是通用技巧：容器扩容搬移元素时，搬的只是轻量指针，且元素可以被容器外的代码共享。

.get()：取出裸指针

Model* raw_ptr = model_ptr3.get();   // 只读借用
raw_ptr->njoints;                    // 正常访问
// 千万不要 delete raw_ptr —— 内存归 shared_ptr 管！

5.3 程序 03_eigen_intro：Eigen 初体验

这个程序是第 4 章知识的「实战版」，代码本身按注释分为 5 节，这里补充几个代码之外的理解要点。

printVector：第一个模板函数

template<typename VectorType>
void printVector(const std::string& name, const VectorType& v) {
    for (Eigen::Index i = 0; i < v.size(); ++i) {
        std::cout << std::fixed << std::setprecision(4) << v(i);
    }
}

它同时被 Vector3d 和 VectorXd 调用 —— 模板的威力第一次显形。Eigen::Index 是 Eigen 的索引类型（本质是长整型）。

旋转矩阵验证小实验

程序输出 Rz(90°) * [1,0,0] 的结果是 [0, 1, 0]（可能有 1e-17 级别的数值噪声，这是浮点数的正常现象，不是 bug）。建议你改一下代码：把 90° 改成 45°、把旋转轴换成绕 X 轴，观察输出变化，建立「矩阵 × 向量 = 旋转」的直觉。

SE3 四则运算

SE3 translate(Eigen::Matrix3d::Identity(), Eigen::Vector3d(1.0, 0.0, 0.0));
SE3 combined = translate * identity_transform;   // 复合
SE3 inverse = combined.inverse();                // 求逆
SE3 should_be_identity = combined * inverse;     // T * T⁻¹ = I

打印结果会显示最后一行确实是单位矩阵。这个验证模式（做完运算再验证数学性质）在机器人代码调试中非常常用。

PI 常量的两种写法

double angle = PI<double>() / 2.0;   // Pinocchio 4.x 推荐写法

PI<double>() 是一个模板函数调用：PI<标量类型>() 返回该类型的 π。旧代码常见的 M_PI 需要先 #define _USE_MATH_DEFINES 且只在 MSVC 下要操心，而 Pinocchio 的 PI<double>() 跨平台一致。

5.4 动手练习

在 01_hello_world.cpp 里添加一行，打印 model.nbodies（连杆数量），重新编译运行，猜猜它和关节数是什么关系；
修改 03_eigen_intro.cpp：构造绕 X 轴旋转 30° 的矩阵，把 [0,1,0] 旋转后打印，手算验证结果；
在 02_smart_pointers.cpp 中，把 std::vector<std::shared_ptr<Model>> 改成 std::vector<Model>，思考为什么它同样能工作（提示：Model 拷贝便宜）。

第 6 章教程 02 详解：机器人模型创建

对应目录：tutorials/02_model/，共 3 个程序。
本章你将学会：亲手从零搭建一个机器人模型，并彻底搞懂 Joint 与 Frame 的关系。

6.1 程序 01_create_model：从零搭建 3 自由度机械臂

我们要搭的机器人长什么样

 universe（关节0，世界坐标系，固定不动）
    │
    ├── joint_1_base_rotation ── 绕 Z 轴旋转（底座左右转头），位于原点
    │         │
    │         │ 沿父坐标系 Z 轴向上 0.5 m
    │         ▼
    ├── joint_2_shoulder ── 绕 Y 轴旋转（肩关节，点头/抬头）
    │         │
    │         │ 沿父坐标系 Y 轴延伸 0.4 m（大臂长度）
    │         ▼
    ├── joint_3_elbow ── 绕 Y 轴旋转（肘关节）
    │         │
    │         │ 沿 Y 轴再延伸 0.3 m（小臂长度）
    │         ▼
    └── end_effector（末端坐标系，机械手的安装点）

这个「一根链条从上到下」的结构叫串联机械臂，是工业机器人的经典形态。

核心 API：addJoint

JointModelRZ joint1;                       // ① 造一个关节类型对象：绕 Z 轴旋转

JointIndex joint1_idx = model.addJoint(
    JointIndex(0),        // ② 父关节索引：挂在 universe（世界）下
    joint1,               // ③ 关节模型
    SE3::Identity(),      // ④ 关节相对于父关节的位姿（这里是原点，无偏移）
    "joint_1_base_rotation"  // ⑤ 关节名称
);

addJoint 返回新关节的索引（JointIndex），后面挂子关节时要用它当「父关节」。理解这 5 个参数，就理解了 Pinocchio 建模的核心思想：

机器人 = 一棵树。每个关节声明「我爸爸是谁（父关节索引）+ 我在爸爸坐标系里的位置（SE3）+ 我是什么类型（绕什么轴动）」。

第二个关节演示了「带偏移的挂载」：

JointModelRY joint2;   // 绕 Y 轴旋转

SE3 joint2_placement(Eigen::Matrix3d::Identity(),   // 朝向：与父系一致（不额外旋转）
                     Eigen::Vector3d(0.0, 0.0, 0.5)); // 位置：沿父系 Z 轴向上 0.5 米

JointIndex joint2_idx = model.addJoint(
    joint1_idx,            // 父关节：关节1
    joint2,
    joint2_placement,      // 「我在关节1上方 0.5 米处」
    "joint_2_shoulder"
);

SE3 placement 的物理意义要重点消化：它描述的是「子关节坐标系的原点和朝向，在父关节坐标系里的数值」。关节 2 的 placement 说的是「从关节 1 出发，沿它的 Z 轴走 0.5 米，就到了我这儿」。连杆的「长度」正是靠这些 placement 表达出来的 —— Pinocchio 里并没有单独的「连杆」对象，连杆被隐式地表示为两个关节之间的这段变换。

第三个关节同理：挂在关节 2 下，沿 Y 轴偏移 0.4 米（大臂）。

核心 API：addFrame（附加坐标系）

光有关节还不够 —— 「手尖」这个点并不会动（它不是关节），但我们又必须知道它的位置。解决办法：在关节 3 上附加一个 Frame：

SE3 ee_placement(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0.0, 0.3, 0.0));

Frame ee_frame(
    "end_effector",       // 帧名称：之后可以用名字找到它
    joint3_idx,           // 父关节：挂在关节3上
    ee_placement,         // 相对关节3的位姿：沿 Y 轴 0.3 米
    OP_FRAME              // 帧类型：操作帧（OP_FRAME）
);

FrameIndex ee_frame_idx = model.addFrame(ee_frame);

Frame 类型速览（教程 02-2 会用到 switch 判断它们）：

类型	含义	何时出现
`OP_FRAME`	操作帧，用户自定义的操作点	手动 addFrame 时（比如末端手尖）
`JOINT`	关节自带的帧	每个 addJoint 自动生成
`BODY`	连杆帧	URDF 的 link 会生成
`SENSOR`	传感器帧	URDF 里声明了传感器时
`FIXED_JOINT`	固定关节帧	固定关节生成

Joint 与 Frame 的关系总结：每加一个关节，系统自动多一个同名 JOINT 帧；而手尖、传感器这类「只关心位置、不提供自由度」的点，用 addFrame 挂到最近的关节上。之后所有「某点在哪」的查询都统一走 Frame 接口。

验证：正向运动学跑一遍

Data data(model);
Eigen::VectorXd q_neutral = neutral(model);   // 中立姿态：所有关节角度为 0
forwardKinematics(model, data, q_neutral);    // 计算每个关节的世界位姿

for (JointIndex i = 0; i < model.njoints; ++i) {
    SE3 joint_pose = data.oMi[i];             // 关节 i 在世界系下的位姿
    Eigen::Vector3d pos = joint_pose.translation();
    ...
}

neutral(model) 是自由函数（不属于任何类），返回机器人的「零位姿态」向量。对所有角度为 0 的机器人做正向运动学，各关节位置应该正好等于我们设置的 placement 累加结果：关节 1 在 (0,0,0)，关节 2 在 (0,0,0.5)，关节 3 在 (0,0.4,0.5)。用这个规律可以立刻检验模型搭得对不对。

data.oMi[i] 的命名规则：o=origin（世界系），Mi=到关节 i 的变换。读 Pinocchio 源码和文档时，这套「oM*、M」命名会反复出现。

6.2 程序 02_joints_and_frames：把关节和坐标系看个底朝天

这个程序是「解剖课」：加载示例机械臂后，把每个关节、每个 Frame 的全部属性打印出来。

读取关节的树结构

JointIndex parent_id = model.parents[i];          // 关节 i 的父关节索引
const SE3& placement = model.jointPlacements[i];  // 关节 i 相对父关节的位姿

model.parents 是一个数组：parents[i] 告诉你关节 i 的爸爸是谁。有了 parents 数组，就能从任意关节一路向上追溯到 universe。这就是「机器人是一棵树」在数据结构上的体现。

Frame 的完整属性

const auto& frame = model.frames[i];
frame.name;           // 名称
frame.type;           // 类型（OP_FRAME / JOINT / BODY ...）
frame.parentJoint;    // 它挂在哪个关节上
frame.placement;      // 相对父关节的位姿

按名字查找 Frame

FrameIndex ee_idx = model.getFrameId("arm_tool_link");
if (ee_idx < model.nframes) {   // 判断是否找到
    ...
}

getFrameId(name) 按名称查索引。示例机械臂的末端帧叫 arm_tool_link，基座帧叫 arm_base_link。判断「找没找到」的惯用写法就是检查返回值是否小于 nframes（找不到会返回一个越界的大值）。

JointModel vs JointData

Data data(model);
data.joints.size();             // JointData 的个数 == 关节个数
data.joints[1].joint_q().size(); // 关节1当前配置占用的维数

与 Model/Data 的分工完全对应：model.joints[i]（JointModel）描述「关节是什么类型、绕哪根轴」，永远不变；data.joints[i]（JointData）存放「这个关节当前的角度、速度算到什么程度了」，随计算更新。

6.3 程序 03_sample_models：Pinocchio 4.x 的内置模型

两个可用的示例模型

Model manipulator;
buildModels::manipulator(manipulator, false);  // 6 自由度机械臂

Model humanoid;
buildModels::humanoid(humanoid, true);         // 仿人机器人（参数 true：启用 mimic 关节）

模型	自由度	结构	适用场景
manipulator	nq = 6	基座 + 6 旋转关节 + 末端	机械臂控制入门（本项目主力）
humanoid	nq ≈ 37	躯干 + 双臂 + 双腿	仿人/全身控制研究

mimic（仿制）关节：一个关节的运动强制复制另一个关节（比如机械手的对称手指用一个电机驱动）。humanoid(model, true) 的第二个参数就是开关。

版本差异警示牌（重要）：Pinocchio 3.x 时代还有 buildSampleModelFreeFlyer()、buildSampleModelQuadruped() 等函数，4.x 已全部移除。需要浮动基座或四足模型时，改用 URDF 加载（教程 05）。网上大量旧教程还在用这些函数，复制它们的代码必然编译失败 —— 这是本项目代码里特意用注释反复强调的。

创建 Data 的两种等价写法

Data data1(manipulator);            // 写法1：直接构造
auto data2 = humanoid.createData(); // 写法2：模型成员函数（官方推荐，语义更清晰）

随机姿态的规范生成方式

Eigen::VectorXd q = Eigen::VectorXd::Random(manipulator.nq);  // 各分量 [-1,1] 随机
q *= PI<double>();                                            // 缩放到 [-π, π]

VectorXd::Random(n) 生成 n 维随机向量，各分量均匀分布在 [-1, 1]。乘 π 后恰好覆盖常见的关节角度范围。测试代码里经常这样「造一个随便的姿态」来验证算法。

6.4 本章知识地图

建模三板斧：
  addJoint(父索引, 关节类型, 相对位姿SE3, 名字)   → 添加可动关节，返回索引
  addFrame(Frame(名字, 父关节, 相对位姿, 类型))   → 添加操作点/传感器点
  neutral(model) / forwardKinematics(...)        → 验证模型搭得对不对

查询三件套：
  model.parents[i]          关节的父关节
  model.jointPlacements[i]  关节相对父关节的位姿
  model.getFrameId("名字")  按名字找 Frame

6.5 动手练习

修改 01_create_model.cpp：给机械臂加第 4 个关节（绕 X 轴旋转的腕关节），挂在关节 3 下、沿 Y 轴偏移 0.3 米处。编译运行，确认 njoints 和 nq 各增加了多少；
打印每个关节的 model.parents[i]，画出示例机械臂的关节树，验证 universe 是根；
思考题：为什么关节 2 的 placement 是沿 Z 轴偏移 0.5，而关节 3 是沿 Y 轴偏移 0.4？（提示：结合关节 1 是绕 Z 旋转、关节 2 是绕 Y 旋转，想想每个连杆的「伸展方向」。）

第 7 章教程 03 详解：运动学计算

对应目录：tutorials/03_kinematics/，共 3 个程序。
运动学回答的问题是：「给定关节角度/速度，机器人各部位在哪里、以什么速度运动？」

7.1 程序 01_forward_kinematics：正向运动学

数学本质（30 秒版）

机器人是一条运动链。每个关节 i 相对父关节的位姿 = 固定安装位姿 × 由当前角度决定的旋转。从世界系出发，把这条链上的变换一路乘过去，就得到任意关节的世界位姿：

世界→关节3  =  (世界→关节1) × (关节1→关节2) × (关节2→关节3)
   oM3     =      oM1       ×      1M2      ×      2M3

forwardKinematics(model, data, q) 做的就是这件事，结果写进 data.oMi[]（每个关节）和 data.oMf[]（每个 Frame）。

代码讲解

标准开场（后续所有教程都长这样，请形成肌肉记忆）：

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

三参数正向运动学

Eigen::VectorXd q1 = neutral(model);      // 关节角度向量
forwardKinematics(model, data, q1);       // 计算！

调用之后，立即可以从 data 里读结果 —— 注意这个「调用算法 → 从 data 读结果」的模式贯穿 Pinocchio 全部 API。

读取关节位姿

for (JointIndex i = 1; i < model.njoints; ++i) {   // 从 1 开始，跳过 universe
    const SE3& pose = data.oMi[i];
    Eigen::Vector3d pos = pose.translation();
    std::cout << pos.x() << ", " << pos.y() << ", " << pos.z();
}

循环从 1 开始是有意的：关节 0 是 universe，位姿永远是单位阵，没有信息量。

改变角度、重新计算

Eigen::VectorXd q2 = neutral(model);
q2[0] = M_PI / 4;    // 关节1（绕Z）转 45°
q2[1] = -M_PI / 3;   // 关节2（绕Y）转 -60°
q2[2] = M_PI / 6;    // 关节3 转 30°
forwardKinematics(model, data, q2);   // 同一个 data，重新算一遍即可

q[i] 用 [] 或 () 访问都行。Data 对象是可重复使用的：换一个 q 再调一次，旧结果被覆盖。

读取末端 Frame 的完整位姿

FrameIndex ee_idx = model.getFrameId("arm_tool_link");   // 按名字找末端
if (ee_idx < model.nframes) {
    const SE3& ee_pose = data.oMf[ee_idx];     // 末端在世界系下的位姿
    printSE3("", ee_pose);                     // 打印 4×4 矩阵
    Eigen::Vector3d ee_pos = ee_pose.translation();  // 只取位置
}

注意区分：data.oMi[关节] 和 data.oMf[坐标系]。查「手尖在哪」用 oMf + getFrameId，这是最实用的组合。

两个坐标系之间的相对变换

SE3 base_to_ee = data.oMf[base_idx].inverse() * data.oMf[ee_idx];

这一行值得玩味。oMf[base].inverse() 是「世界→基座」的逆，即「基座→世界」；再右乘「世界→末端」，世界系在中间抵消，得到基座→末端的变换。变换链的「消去律」是 SE3 代数最常用到的技巧：

(基座→世界) × (世界→末端) = 基座→末端
   oMb⁻¹    ×    oMee    =   bMee

有了 bMee，其 translation 就是「末端相对基座的位置」—— 机械臂控制中比世界坐标更有意义的量。

printSE3 辅助函数

Eigen::Matrix4d mat = transform.toHomogeneousMatrix();  // SE3 → 4×4 矩阵
// 然后双重循环逐元素打印

toHomogeneousMatrix() 是观察 SE3 内容最直观的方式：左上 3×3 是旋转，右边一列是平移。

7.2 程序 02_jacobian：雅可比矩阵

雅可比是什么？为什么重要？

对正向运动学求「导数」，就得到雅可比。它回答：如果每个关节以某个速度转动，末端会以什么空间速度运动？

v_ee = J(q) · q̇
        ↑        ↑
  6×n 雅可比   n 维关节速度

v_ee 是 6 维空间速度 = [角速度 ω (3维); 线速度 v (3维)]

注意 Pinocchio 的约定：角速度在前 3 行，线速度在后 3 行。这个顺序和有些教材（线速度在前）相反，看输出时别搞反。

雅可比的用途：速度映射（本教程）、逆运动学迭代求解、力映射（静力学里 τ = Jᵀf）、奇异性分析……它是运动学通往控制与力学的桥梁。

代码讲解

计算与提取

Eigen::VectorXd q = neutral(model);
forwardKinematics(model, data, q);

computeJointJacobians(model, data, q);   // 一次算出全部关节的雅可比，存入 data.J
const auto& J = data.J;                  // 6×n 矩阵

data.J 是末端关节的雅可比（6 行 n 列）。想取任意关节的雅可比，用：

Eigen::MatrixXd J_world = getJointJacobian(model, data, ee_joint, WORLD);
//                                                    ^关节索引  ^参考坐标系

前提是已经调过 computeJointJacobians。第三个参数是参考系选择，下面细说。

矩阵块操作实战

J.block(0, 0, 3, J.cols());   // 前 3 行：角速度映射
J.block(3, 0, 3, J.cols());   // 后 3 行：线速度映射

复习 4.4 节的 block 语法。观察中立姿态下的输出，你会发现很多 0 和 ±1 —— 这是因为零位时各关节轴与世界轴平行/垂直，雅可比结构特别简洁。

速度映射演示

Eigen::VectorXd q_dot = Eigen::VectorXd::Zero(model.nv);
q_dot[0] = 1.0;   // 关节1 以 1 rad/s 旋转
q_dot[1] = 0.5;   // 关节2 以 0.5 rad/s 旋转

Eigen::VectorXd v_ee = data.J * q_dot;   // 一次矩阵乘法，得到 6 维末端速度

参考坐标系 ReferenceFrame

getJointJacobian(model, data, joint, WORLD);              // 世界系表达
getJointJacobian(model, data, joint, LOCAL);              // 关节自身坐标系表达
getJointJacobian(model, data, joint, LOCAL_WORLD_ALIGNED); // 原点在关节、轴与世界平行

同一个物理速度，用不同坐标系的轴来记录数字就不同 —— 就像「向北 3 m/s」和「向船头方向 3 m/s」是同一速度的两种描述。WORLD 最直观；LOCAL 常用于控制律设计；LOCAL_WORLD_ALIGNED 兼顾两者，做位置控制时常用。

奇异性分析

Eigen::FullPivLU<Eigen::MatrixXd> lu(J_copy);
int rank = lu.rank();    // 矩阵的秩
if (rank < std::min(J_copy.rows(), J_copy.cols())) {
    std::cout << "机器人处于奇异位形附近！";
}

奇异位形：某些姿态下雅可比降秩，机器人瞬时失去某个方向的运动能力。生活例子：手臂完全伸直时，手无法沿手臂方向再向外移动（只能靠弯曲手肘）。秩 deficient 的位形就是奇异位形，控制中要尽量避免（此时求逆会数值爆炸）。

MSVC 避坑：为什么不用 computeFrameJacobian？

注释里写明：computeFrameJacobian 在 MSVC 下有 Boost Fusion 模板问题，所以项目统一改用 computeJointJacobians + getJointJacobian 组合。记住这个替代方案，以后你在 Windows 上遇到奇怪的模板编译错误，先想想是不是这类问题。

7.3 程序 03_frame_operations：空间速度与坐标系操作

Motion：空间速度类型

Pinocchio 把「线速度 + 角速度」打包成 Motion 类型：

const Motion& spatial_velocity = data.v[i];   // 关节 i 的空间速度（FK 带速度后填充）
spatial_velocity.linear();    // 线速度部分（Vector3d）
spatial_velocity.angular();   // 角速度部分（Vector3d）

对称地，Force 类型打包「力 + 力矩」，Inertia 打包「质量 + 质心 + 惯性张量」。Pinocchio 的空间代数就是用这三类对象做运算。

带速度的正向运动学

forwardKinematics(model, data, q);            // 先算位姿
forwardKinematics(model, data, q, q_dot);     // 再算速度（4 参数重载）

MSVC 避坑：5 参数版本 forwardKinematics(model, data, q, q_dot, q_ddot) 在 MSVC 下可能触发 Boost Fusion 模板错误。项目的对策是拆成两步调用。如果你以后需要加速度信息，先试 5 参数版，编不过就拆。

坐标系速度的参考系选择

FrameIndex ee_idx = model.getFrameId("effector_body");
Motion v_world = getFrameVelocity(model, data, ee_idx, ReferenceFrame::WORLD);
Motion v_local = getFrameVelocity(model, data, ee_idx, ReferenceFrame::LOCAL);

两者描述的是同一个物理速度。WORLD 版本用世界轴分解，LOCAL 版本用末端自身坐标轴分解。做「沿工具自身方向前进」这类控制时用 LOCAL，做「在世界中画轨迹」时用 WORLD。

SE3 对点的作用：actOnEigenObject

Eigen::Vector3d relative = base_pose.inverse().actOnEigenObject(ee_pose.translation());

T.actOnEigenObject(p) = 把变换 T 施加到点 p（旋转再加平移），返回 Eigen 向量。这是「把一个世界坐标点换算到另一坐标系下」的标准写法。

Force 类型初探

Force gravity_force;
gravity_force.linear(Eigen::Vector3d(0, 0, -9.8));   // 力分量
gravity_force.angular(Eigen::Vector3d::Zero());      // 力矩分量

注意 Force 的 linear/angular 既是读（不带参数）也是写（带参数），这种写法在 Pinocchio 里很常见。

变换复合与验证

SE3 combined = T1 * R90 * T2;      // 三个变换依次复合（从右往左作用）
Eigen::Vector3d transformed = combined.actOnEigenObject(point);

复合顺序要特别注意：T1 * R90 * T2 作用在点上时，是先 T2、再 R90、最后 T1。矩阵乘法从右往左读，和函数复合 f(g(x)) 一个道理。

用雅可比预测速度并交叉验证

程序最后做了一件漂亮的事：

Eigen::MatrixXd J_v = J_ee.block(3, 0, 3, J_ee.cols());  // 取线速度部分（后3行）
Eigen::Vector3d predicted_v = J_v * q_dot;               // 雅可比预测的末端线速度

Motion actual_vel = getFrameVelocity(model, data, ee_idx, ReferenceFrame::WORLD);
// 比较 predicted_v 和 actual_vel.linear()

两条独立的路径（雅可比乘法 vs 直接速度运动学）算出同一个物理量，结果一致 —— 这叫交叉验证，是检验你对 API 理解是否正确的最佳方法，也是工程调试的基本功。

7.4 本章知识地图

forwardKinematics(model, data, q)           位置：data.oMi / data.oMf
forwardKinematics(model, data, q, q_dot)    速度：data.v[i] / getFrameVelocity
computeJointJacobians(model, data, q)       雅可比：data.J（末端，角速度在前）
getJointJacobian(model, data, i, ref)       任意关节雅可比，可选参考系
getFrameVelocity(model, data, f, ref)       任意 Frame 速度，可选参考系

SE3 代数：  A⁻¹×B = 相对变换      T.actOnEigenObject(p) = 变换一个点
Motion = {linear, angular}   Force = {linear, angular}

7.5 动手练习

让关节 1 以 2 rad/s 转动，分别用 data.J * q_dot 和 getFrameVelocity 计算末端线速度，验证两者一致；
找到机械臂的一个奇异位形（提示：试试让肩肘两关节轴共线），打印该姿态下雅可比的秩；
把 base_to_ee = oMf[base].inverse() * oMf[ee] 中的顺序反过来乘，观察结果有什么不对，用变换链的读法解释原因。

第 8 章教程 04 详解：动力学计算

对应目录：tutorials/04_dynamics/，共 3 个程序。
如果说运动学研究「机器人怎么动」，动力学研究的是「为什么会这样动 / 要怎样才动得起来」—— 力与运动的关系。

8.0 先建立直觉：牛顿第二定律的机器人版

单个质点：F = m·a（力 = 质量 × 加速度）。

机器人是几十个刚体用关节串起来的系统，同样的思想推广后变成：

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

τ（tau，n 维向量）：每个关节电机要输出的力矩 —— 这是我们要算的目标；
M(q)：质量矩阵（惯性矩阵），n×n，描述「加速各关节有多费劲」，随姿态变化；
q̈：关节加速度向量（q 上两点 = 二阶导）；
C(q, q̇)：科氏力与离心力项 —— 挥动手臂时感觉到的「甩出去」的力；
G(q)：重力项 —— 举着手臂不动也要使劲，就是因为 G。

逆动力学：已知 q、q̇、q̈（想要的运动）→ 算 τ（需要多大力矩）。
正动力学：已知 τ（实际施加的力矩）→ 算 q̈（机器人实际怎么加速）。

两者互为反问题，分别对应「控制」和「仿真」两大应用。

8.1 程序 01_inverse_dynamics：逆动力学（RNEA）

重力矩：最简单的逆动力学

机器人静止时（q̇ = 0、q̈ = 0），动力学方程退化为 τ = G(q) —— 电机只需对抗重力。程序先算这个：

Eigen::VectorXd q = neutral(model);
computeGeneralizedGravity(model, data, q);   // 计算重力项
Eigen::VectorXd tau_gravity = data.g;        // 结果在 data.g

观察输出你会发现：机械臂竖直时，绕竖直轴（关节 1）的重力矩是 0（重力不产生绕竖直轴的力矩），而肩、肘关节的力矩不为 0。这个「哪些关节受力、哪些不受力」的判断，是检验动力学直觉的好练习。

术语说明：「广义重力」中的「广义」指它是投影到关节空间的重力效应（每个关节一个等效力矩），而不是世界空间里的力。

完整逆动力学：τ = M·q̈ + b

项目里计算逆动力学采用了「分块组装」的方式（教学上更透明）：

// 第 1 步：非线性效应 b(q, q̇) = 重力 + 科氏力 + 离心力
nonLinearEffects(model, data, q3, q_dot3);
Eigen::VectorXd b_term = data.nle;            // nle = non-linear effects

// 第 2 步：质量矩阵 M(q)
computeMinverse(model, data, q3);             // 注意：算的是 M 的逆！
Eigen::MatrixXd M = data.Minv.inverse();      // 再求逆得到 M 本身

// 第 3 步：组装
Eigen::VectorXd tau_motion = M * q_ddot3 + b_term;

两个新 API 需要记住：

nonLinearEffects(model, data, q, q_dot) → 结果在 data.nle。当 q_dot = 0 时，nle 恰好等于重力矩 data.g；
computeMinverse(model, data, q) → 结果在 data.Minv，它是 M⁻¹（质量矩阵的逆），不是 M！名字里的 “Minverse” 就是 M-inverse 的意思。想要 M 就得再 .inverse() 一次（教学代码这么写没问题，生产代码应尽量避免显式求逆）。

为什么 Pinocchio 偏爱 M⁻¹？ 因为正动力学（仿真）的核心运算是 q̈ = M⁻¹(τ - b)，直接算 M⁻¹ 比算 M 再求逆高效得多。Pinocchio 的 ABA 算法正是沿这个思路设计的。

验证：拆解再重组

程序第 6 节做了一次漂亮的自洽性检查：

tau_inertia = M_q * q_ddot3;                    // 纯惯性部分
tau_reconstructed = tau_inertia + tau_gravity;  // 惯性 + 重力
// 与 tau_motion（一次算出的完整力矩）对比

因为测试时 q̇ = 0（科氏力为零），所以完整力矩应当恰好等于「惯性 + 重力」。两组数字在小数点后若干位内一致 —— 说明我们对公式的理解和 API 的用法都没错。

惯性参数：质量、质心、惯性张量

const auto& inertia1 = model.inertias[1];
inertia1.mass();      // 连杆质量 (kg)
inertia1.lever();     // 质心位置（相对连杆坐标系）

model.inertias[i] 是第 i 个连杆的 Inertia 对象，打包了动力学三要素。做真实机器人项目时，这些参数要从 CAD 软件或实测获得 —— 参数不准，算出的力矩就不准，控制效果自然差。

关于 rnea() 函数

Pinocchio 最经典的逆动力学接口是：

Eigen::VectorXd tau = rnea(model, data, q, q_dot, q_ddot);  // RNEA 一步到位

RNEA（Recursive Newton-Euler Algorithm，递归牛顿-欧拉算法）从末端向基座递归传递力与力矩，复杂度与关节数成正比（O(n)），是教科书级的高效算法。本项目的教程代码为了教学透明拆成了 nle + Minverse 两步，但你在真实项目和 test_new_api.cpp 里都会见到直接调用 rnea 的写法。

8.2 程序 02_forward_dynamics：正动力学与仿真

核心公式

正动力学就是把方程反着解：

q̈ = M(q)⁻¹ · (τ - b(q, q̇))

代码写法：

nonLinearEffects(model, data, q, q_dot);          // b
computeMinverse(model, data, q);                  // M⁻¹（在 data.Minv）
Eigen::VectorXd q_ddot = data.Minv.fullPivLu().solve(tau - data.nle);

data.Minv.fullPivLu().solve(x) 等价于 Minv · x，这里写成 solve 形式是沿用 Eigen 的规范写法（Minv 对称正定时也可用更快的分解，学习阶段不必纠结）。

关于 aba() 函数：Pinocchio 的正动力学标准接口是 q_ddot = aba(model, data, q, q_dot, tau)（ABA = Articulated Body Algorithm，铰接体算法，同样 O(n)）。项目在 MSVC 下遇到其三参数版本的 Boost Fusion 模板问题，所以教程改用「nle + Minv」的手动组合实现相同效果。功能等价，只是少了些性能优化。

仿真循环：半隐式欧拉积分

有了 q̈，如何「让时间流动起来」？数值积分：

double dt = 0.01;    // 时间步长 10 ms
for (int step = 0; step <= steps; ++step) {
    // ① 用当前状态算加速度
    q_ddot = f(q, q_dot, tau);
    // ② 先更新速度
    q_dot += q_ddot * dt;
    // ③ 再用【新】速度更新位置
    q += q_dot * dt;
}

这叫半隐式欧拉法（semi-implicit Euler）：先用旧位置算出的加速度更新速度，再用刚更新的速度去更新位置。比「两步都用旧值」的显式欧拉稳定得多，是物理仿真和游戏引擎的标配。dt 越小越精确但越慢，0.001~0.01 s 是常见范围。

程序先演示「零力矩自由运动」：τ = 0 时机器人在重力下自然甩动，输出的角度/速度表格会显示关节 1 开始加速 —— 重力在暗中做功。

PD 控制：第一个反馈控制器

Eigen::VectorXd error = q_desired - q;                 // 位置误差
Eigen::VectorXd tau_pd = Kp * error - Kd * q_dot;      // 控制律

PD 控制器是工业界最常用的基础控制器：

P（比例）项 Kp·误差：偏得越多，纠正力矩越大 —— 像弹簧把关节往目标拉；
D（微分）项 -Kd·速度：动得越快，阻尼力矩越大 —— 像减震器防止冲过头来回振荡。

仿真循环变成：

每一步：读当前状态 → PD 算出 τ → 正动力学算出 q̈ → 积分更新状态 → 下一步

这就构成了一个闭环仿真系统：控制器和机器人模型互相喂数据。观察输出表格：误差随时间收敛到 0 附近，说明控制成功。Kp 太小收敛慢，Kd 太小会振荡 —— 动手调一调这两个数，你会对「控制参数整定」有第一手感觉。

8.3 程序 03_centroidal_dynamics：质心动力学

为什么要看质心？

对人形/四足机器人，单个关节的细节有时不重要，重要的是「整体」：整个机器人的质心（质量集中点）在哪、总动量是多少 —— 因为机器人会不会摔倒，取决于质心的运动。

质心动力学的基本定律（牛顿定律对整机的推广）：

d(线动量)/dt = 外力之和        （外力：重力、地面接触力……）
d(角动量)/dt = 外力矩之和

计算质心位置

Eigen::Vector3d com = centerOfMass(model, data, q);   // 一步得到质心坐标

改变姿态后再算一次，质心就移动了 —— 人弯腰时质心前移，可能移出脚掌支撑面就会摔倒，正是这个道理。

质心动量与动量变化率

forwardKinematics(model, data, q2);
forwardKinematics(model, data, q2, q_dot2);   // 先算位姿和速度
computeCentroidalMomentum(model, data);       // 结果在 data.hg
const Force& h_com = data.hg;                 // 质心动量（打包成 Force 类型）

data.hg 是 Force 类型，这里复用它的「6 维」结构：linear 部分存线动量、angular 部分存角动量。

computeCentroidalMomentumTimeVariation(model, data);  // 动量变化率 → data.dhg

版本注意：3.x 的 computeCentroidalDynamics 在 4.x 中已被移除，替代者就是上面的 computeCentroidalMomentumTimeVariation。调用前必须先完成带速度的正向运动学，这也是注释里「先 forwardKinematics 两步、再算动量」序列的由来。

质心雅可比与伪逆控制

Eigen::MatrixXd J_com = jacobianCenterOfMass(model, data, q2);   // 3×n
// 关系：v_com = J_com · q̇

「想让质心以速度 v_des 运动，各关节该怎么动？」—— 解方程 J_com · q̇ = v_des。通常关节数 n > 3，方程有无穷多解，程序用 SVD 分解求最小范数解（伪逆）：

Eigen::JacobiSVD<Eigen::MatrixXd> svd(J_com, Eigen::ComputeThinU | Eigen::ComputeThinV);
svd.setThreshold(1e-6);
Eigen::VectorXd q_dot_desired = svd.solve(v_com_desired);

setThreshold 把极小的奇异值当 0 处理，避免数值爆炸（回忆 7.2 节的奇异性讨论 —— 这里是它的实际应用）。最后程序用 J_com * q_dot_desired 反算验证确实得到了期望质心速度。

应用前景：人形机器人的行走（ZMP/捕获点理论）、跳跃、抗推扰平衡，核心都是对质心动力学的规划与控制。

8.4 本章知识地图

逆动力学（运动→力矩，用于控制）
  rnea(model, data, q, v, a)                    经典一步式
  computeGeneralizedGravity → data.g            仅重力项
  nonLinearEffects → data.nle                   重力+科氏+离心
  computeMinverse → data.Minv                   质量矩阵的逆 M⁻¹

正动力学（力矩→运动，用于仿真）
  aba(model, data, q, v, tau)                   经典一步式
  q̈ = Minv.solve(τ − nle)                       本项目的 MSVC 兼容写法
  半隐式欧拉：v += q̈·dt;  q += v·dt

质心动力学
  centerOfMass(model, data, q)                  质心位置
  computeCentroidalMomentum → data.hg           质心动量
  computeCentroidalMomentumTimeVariation → data.dhg
  jacobianCenterOfMass → J_com                  3×n，v_com = J_com·q̇

8.5 动手练习

在逆动力学程序中，把姿态换成「大臂水平伸直」，观察肩关节重力矩显著增大 —— 用杠杆原理（力 × 力臂）解释；
修改 PD 仿真：把 Kd 从 2.0 改成 0.2，观察关节角度是否出现振荡/过冲；再改回并加大 Kp，体会两个增益的作用；
把仿真步长 dt 从 0.01 改成 0.1，观察数值积分是否还稳定（提示：可能会发散），理解「步长与稳定性」的关系；
思考：为什么质心雅可比只有 3 行，而末端雅可比有 6 行？

第 9 章教程 05 详解：URDF 模型加载

对应目录：tutorials/05_urdf/ + models/simple_robot.urdf。
前面几章的机器人都是代码「造」出来的。真实项目中，机器人模型通常来自设计部门给的模型文件 —— 最常见的格式就是 URDF。

9.1 URDF 是什么？

URDF（Unified Robot Description Format，统一机器人描述格式） 是 ROS 生态事实上的标准机器人描述格式，本质是一个 XML 文件。它描述三件事：

连杆（link）：有哪些刚体，各自的质量、惯性、外观形状；
关节（joint）：连杆之间怎么连接，绕哪根轴、能转多少；
（可选）视觉与碰撞几何：用什么形状渲染/做碰撞检测。

打开 models/simple_robot.urdf，它描述的正是一个 3 自由度机械臂：基座 → 大臂 → 小臂 → 末端球。

9.2 逐段读懂 simple_robot.urdf

根元素与连杆

<robot name="simple_3dof_arm">

  <link name="base_link">
    <visual>      <!-- 外观：怎么画给"人"看 -->
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <box size="0.2 0.2 0.1"/>        <!-- 0.2×0.2×0.1 米的盒子 -->
      </geometry>
      <material name="grey">
        <color rgba="0.5 0.5 0.5 1.0"/>
      </material>
    </visual>
    <collision>   <!-- 碰撞体：怎么算"撞没撞"，常比 visual 简化 -->
      <geometry><box size="0.2 0.2 0.1"/></geometry>
    </collision>
    <inertial>    <!-- 惯性参数：动力学计算的依据 ★ -->
      <mass value="10.0"/>               <!-- 质量 10 kg -->
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
  </link>

每个 link 的三段各有分工：visual 只管显示，collision 只管碰撞检测，inertial 才是动力学的灵魂 —— 质量 + 惯性张量（ixx~izz 六个分量构成 3×3 对称矩阵）。做控制/仿真时如果 URDF 里没有 inertial，动力学计算会出错或退化。

origin xyz="..." rpy="..." 表示该几何相对连杆坐标系原点的位置和姿态，rpy = roll-pitch-yaw 三个欧拉角（弧度）。

连杆的尺寸链：link1 是长 0.5 m 的圆柱（大臂），几何中心放在 z=0.25 处；link2 长 0.4 m（小臂）；end_effector 是半径 0.03 m 的小球（手尖）。质量依次是 5 kg、3 kg、0.5 kg —— 越靠手越轻，符合真实设计。

关节

  <joint name="joint1_base_rotation" type="revolute">
    <parent link="base_link"/>            <!-- 父连杆 -->
    <child link="link1"/>                 <!-- 子连杆 -->
    <origin xyz="0 0 0.05" rpy="0 0 0"/>  <!-- 关节位置：基座顶面 -->
    <axis xyz="0 0 1"/>                   <!-- 旋转轴：Z 轴 -->
    <limit lower="-3.14" upper="3.14" effort="100" velocity="2.0"/>
  </joint>

type="revolute" = 旋转关节（对应的还有 prismatic 平移、fixed 固定、continuous 无限旋转）。关节五要素：父、子、位置（origin）、轴（axis）、限位（limit：角度范围 lower/upper、最大力矩 effort、最大转速 velocity）。这些 limit 在做轨迹规划和电机选型时必须遵守。

对照第 6 章的手动建模：URDF 的 joint 对应 addJoint，origin+axis 对应 placement + 关节类型，link 的惯量对应 model.inertias。两条路殊途同归。

9.3 C++ 加载代码讲解

路径与文件检查

std::string urdf_path = std::string(MODEL_DIR) + "/simple_robot.urdf";
std::ifstream file(urdf_path);       // 尝试打开文件
if (!file.is_open()) {
    std::cerr << "无法打开 URDF 文件！" << std::endl;
    return 1;
}

MODEL_DIR 是 CMake 注入的编译期常量（见 2.4 节）。std::ifstream 是文件输入流，is_open() 检查是否打开成功 —— 这是「先检查再使用」的防御式编程习惯。

加载（try-catch 包裹）

try {
    Model model;
    pinocchio::urdf::buildModel(urdf_path, model);   // 核心一行：文件 → Model
    std::cout << "名称: " << model.name;             // simple_3dof_arm
    ...
} catch (const std::exception& e) {
    std::cerr << "加载失败: " << e.what() << std::endl;
    return 1;
}

pinocchio::urdf::buildModel(文件名, 输出模型) —— 解析 XML、构建关节树、读入惯量，一气呵成。加载后得到的 Model 与代码建模的 Model 没有任何区别，第 5~8 章学的全部算法直接可用。这就是「模型与算法解耦」的好处。

程序接着打印关节结构、Frame 列表，并做一次正向运动学验证（把关节 1 转 45°、关节 2 转 -60°、关节 3 转 30°，查末端位置）—— 和第 7 章一模一样的套路。

其他加载方式（注释中提到，认识即可）

urdf::buildModelFromXML(xml_string, ...);     // 从字符串而非文件加载
urdf::buildModelsFromUrdf(filename, ...);     // 同时得到碰撞/视觉几何模型
urdf::buildModel(filename, package_dirs, ...); // 指定 package:// 资源搜索路径

Pinocchio 还原生支持 SDF（Gazebo 仿真格式）与 MJCF（MuJoCo 格式）等，接口类似。

9.4 动手练习

用记事本打开 simple_robot.urdf，把 link1 的质量从 5 kg 改成 50 kg，重新编译运行教程 04-1 的类似逻辑（或直接写几行加载代码算重力矩），观察肩关节重力矩的变化；
给 URDF 添加第 4 个关节和连杆（腕关节，绕 Z 轴），加载后打印 nq 验证；
把 joint2 的 <axis xyz="0 1 0"/> 改成 <axis xyz="1 0 0"/>，思考机械臂的运动平面会发生什么变化。

第 10 章教程 06 详解：综合实战

对应目录：tutorials/06_project/，共 2 个程序。
终于到了把全部知识串起来的时刻：让机器人真正地"执行任务" —— 规划一条轨迹，再用控制器驱动机器人沿轨迹运动。

10.1 一个完整的「机器人执行任务」系统长什么样

┌────────────┐   q_des(t), q̇_des(t)   ┌────────────┐   τ    ┌────────────┐
│ 轨迹规划器  │ ──────────────────────→ │  控制器     │ ─────→ │ 机器人      │
│(想去哪里)   │                         │(怎么使劲)   │        │(动力学模型) │
└────────────┘                         └────────────┘        └─────┬──────┘
       ↑                                                           │
       └──────────────── q, q̇（当前状态反馈）←────────────────────┘

教程 06-1 实现「PD 控制」，06-2 升级为「逆动力学控制（计算力矩控制）」，并对比两者性能。

10.2 程序 01_trajectory_planning：三次多项式轨迹 + PD 控制

三次多项式轨迹：为什么、是什么

任务：让每个关节在 T 秒内从 q0 平稳运动到 qf，起止速度都为 0（避免「咣」地一下起步/刹停）。

数学上找一条曲线 q(t)，满足四个边界条件：q(0)=q0、q(T)=qf、q̇(0)=0、q̇(T)=0。四个条件恰好确定一个三次多项式的四个系数：

q(t) = a0 + a1·t + a2·t² + a3·t³

系数公式（程序构造函数里那两行）：

a0 = q0
a1 = v0
a2 = (3(qf−q0) − (2v0+vf)·T) / T²
a3 = (2(q0−qf) + (v0+vf)·T) / T³

对 t 求导得到速度、二阶导得到加速度 —— 三个 getter（position/velocity/acceleration）就是干这个的。三次多项式保证了位置和速度连续（加速度在起止点会有跳变，想要更平滑可以用五次多项式，这是练习题）。

三个类的设计（C++ 综合复习）

class CubicPolynomial { ... };    // 单关节轨迹：输入边界条件，输出任意时刻的 q/q̇/q̈
class PDController { ... };       // 控制器：输入当前与期望状态，输出力矩 τ
class TrajectoryPlanner { ... };  // 多关节规划器：内部持有 n 个 CubicPolynomial

TrajectoryPlanner 演示了组合（composition）：一个类持有另一个类的 vector。构造时的关键代码：

trajectories_.reserve(nq_);                    // 预分配内存
for (int i = 0; i < nq_; ++i) {
    trajectories_.emplace_back(q0[i], qf[i], 0.0, 0.0, T);  // 原地构造
}

reserve + emplace_back 是「容器装重型对象」的最佳实践组合（3.6 节讲过，这里是实战）。

主仿真循环：控制系统的"心跳"

for (int step = 0; step <= steps; ++step) {
    double t = step * dt;

    // ① 规划器给出 t 时刻的期望状态
    Eigen::VectorXd q_des = planner.getPosition(t);
    Eigen::VectorXd q_dot_des = planner.getVelocity(t);

    // ② 控制器根据误差算力矩
    Eigen::VectorXd tau = controller.computeTorque(q, q_dot, q_des, q_dot_des);

    // ③ 机器人动力学响应（正动力学）
    nonLinearEffects(model, data, q, q_dot);
    computeMinverse(model, data, q);
    Eigen::VectorXd q_ddot = data.Minv.fullPivLu().solve(tau - data.nle);

    // ④ 积分，时间前进
    q_dot += q_ddot * dt;
    q += q_dot * dt;

    // ⑤ 记录误差指标
    double error_norm = (q_des - q).norm();
}

①→⑤ 每 10 ms 执行一次，共 3 秒 = 300 步。输出的表格里「误差范数」应该从大变小，最终「状态：完成」时接近 0。

Kp=50、Kd=10 是怎么选的？ 经验法则：Kd ≈ 2·√Kp 附近能得到近似「临界阻尼」（最快收敛且不振荡）。√50≈7，乘 2 约 14，取 10 略欠阻尼，收敛快但有轻微过冲 —— 你可以改成 14 对比一下。

结果验证

程序最后用正向运动学检查末端位置、打印最终关节角度与目标之差。「仿真跑完了不算完，用独立方法验证终态」是工程素养。

10.3 程序 02_whole_body_control：逆动力学控制（计算力矩法）

PD 控制的痛点

纯 PD 把机器人当成「一堆独立弹簧」，完全无视重力、惯量耦合。结果：静止时稳态误差、高速跟踪时明显滞后。06-2 的对比实验里，纯 PD 的最大跟踪误差比 ID+PD 大一个数量级 —— 运行程序，亲眼看看。

计算力矩控制律

核心思想：既然我们有精确的动力学模型，就用它把非线性的机器人"抵消"掉，变成纯粹的线性系统，然后再用 PD 收尾。

τ = M(q)·( q̈_des + Kp·e + Kd·ė ) + b(q, q̇)
         └───────┬───────┘   └──┬──┘
           前馈：喂期望加速度   反馈：纠误差

代入真实动力学 M·q̈ = τ − b，两边一消，误差方程变成：

ë + Kd·ė + Kp·e = 0

这是一个标准的二阶线性系统 —— 只要 Kp、Kd 取正，误差必然指数收敛。这就是「反馈线性化」的魅力。

类的实现细节

class InverseDynamicsController {
public:
    InverseDynamicsController(const Model& model, double Kp, double Kd)
        : model_(model), Kp_(Kp), Kd_(Kd) {
        data_ = std::make_unique<Data>(model);   // 控制器自带计算工作区
    }

    Eigen::VectorXd computeTorque(...) {
        Eigen::VectorXd e = q_des - q;
        Eigen::VectorXd e_dot = q_dot_des - q_dot;
        Eigen::VectorXd q_ddot_computed = q_ddot_des + Kp_ * e + Kd_ * e_dot;

        nonLinearEffects(model_, *data_, q, q_dot);      // b
        Eigen::VectorXd b_term = data_->nle;
        computeMinverse(model_, *data_, q);
        Eigen::MatrixXd M = data_->Minv.inverse();       // M
        return M * q_ddot_computed + b_term;             // 控制律
    }

private:
    const Model& model_;             // 借用外部模型（引用成员，构造时必须初始化！）
    std::unique_ptr<Data> data_;     // 独占自己的 Data
    double Kp_, Kd_;
};

两个 C++ 细节值得注意：

引用成员 const Model& model_ 必须在初始化列表里初始化（model_(model)），不能拖到构造函数体内赋值 —— 因为引用一经"出生"就必须绑定对象；
*data_ 解引用：算法函数要 Data 对象，而成员是 unique_ptr，所以传 *data_。

正弦轨迹生成器

class TrajectoryGenerator {
public:
    using TrajectoryFunc = std::function<Eigen::VectorXd(double)>;  // 类型别名
    // position: q_i(t) = offset + A·sin(ω·t + φ)
    // velocity: q̇_i(t) = A·ω·cos(ω·t + φ)        ← sin 的导数
    // acceleration: q̈_i(t) = −A·ω²·sin(ω·t + φ)  ← 再求导
};

正弦轨迹是控制测试的标准输入：它永远在动（不像点到点轨迹会停下来），最能暴露跟踪误差。每个关节用不同频率（1.0 + i*0.3），避免整臂做"整齐划一"的特殊运动。

正动力学里的一个精妙写法

Eigen::VectorXd q_ddot = data.Minv.template triangularView<Eigen::Upper>()
                                   * (tau - data.nle);

data.Minv 存储时只填充了上三角部分（对称矩阵存一半省内存），triangularView<Upper>() 显式地「只按上三角矩阵来乘」，既快又避免读到未填充的下三角。.template 关键字是因为调用者是依赖模板的对象，编译器需要这个提示才能正确解析（3.11 节埋的伏笔在这里回收）。

你也会看到 06-1 里用 fullPivLu().solve() 的写法 —— 那种写法把 Minv 当普通稠密矩阵处理，更通用但略慢且可能引入微小不对称。两种都正确，了解差异即可。

性能分析与对比

double max_err = error.cwiseAbs().maxCoeff();   // 各关节绝对误差的最大值
double avg_err = error.norm() / model.nq;       // 平均误差

程序把每步误差存进 std::vector，结束后统计全程最大/平均误差，并给出评级（<0.01 优秀，<0.05 良好）。最后用完全相同的增益跑一遍纯 PD，对比两者的最大误差并算出「改善百分比」。运行一次，把这个数字记下来 —— 它会让你真正理解「基于模型的控制」的价值。

10.4 两个程序教会你的完整工作流

1. 建模：buildModels::manipulator / urdf::buildModel
2. 规划：给定任务 → 生成 q_des(t), q̇_des(t), q̈_des(t)
3. 控制：误差 + 模型 → τ（PD 或 计算力矩）
4. 仿真：正动力学 + 积分 → 状态演化
5. 评估：误差范数、能耗、收敛时间
6. 调优：改增益/轨迹参数 → 回到 3

这六步循环，就是机器人控制研发的日常。你已经完整走通了一遍。

10.5 动手练习

把 06-1 的三次多项式升级为五次多项式（增加起止加速度为 0 的边界条件，共 6 个条件），对比关节加速度曲线的平滑度（提示：系数公式可查「quintic polynomial trajectory」）；
在 06-2 中把 Kp 从 100 降到 10，观察最大跟踪误差如何变化，理解前馈与反馈的分工；
给 PD 控制器加一个重力前馈：tau = PD输出 + data.g（先算 computeGeneralizedGravity），看看纯 PD 的误差能否逼近 ID+PD 的效果；
综合挑战：把 06-1 的模型换成教程 05 的 URDF 模型（3 自由度），调整 q_goal 的维数，让"自己的机器人"跑通整套流程。

第 11 章辅助测试文件说明

项目根目录下有几个 test_*.cpp，它们不属于教程，而是作者摸索 Pinocchio 4.x API 时的「草稿纸」。了解它们的用途，也能帮你理解「版本差异」这个真实世界的问题。

文件	作用	说明
`test_mingw.cpp`	Hello World 级别的编译器测试	只打印一句话。用来验证 MinGW 工具链能不能正常工作，与 Pinocchio 无关
`test_mingw_pinocchio.cpp`	最小 Pinocchio 冒烟测试	用新 API（`buildModels::manipulator`）创建模型并打印信息，验证「编译器 + Pinocchio」组合可用
`test_pinocchio.cpp`	旧 API 验证	用的是 3.x 写法 `buildSampleModelManipulator(model)`，在当前 4.1.0 环境下会编译失败。它被保留下来作为「新旧 API 对比」的活标本
`test_new_api.cpp`	新 API 全家桶测试	依次验证 FK、雅可比、`rnea`、`aba`、质心等主要接口，全部使用 4.x 风格，可视为一份「API 可用性清单」
`test_mingw.exe`	test_mingw.cpp 的编译产物	已编译好的可执行文件

学习启示：接触一个新库时，先写一个 10 行以内的最小测试程序验证环境，再逐步加功能 —— 比一上来就写大程序、出错后无从排查要聪明得多。这几个文件正是这种工作方法的示范。

附录 A：Pinocchio 常用 API 速查表

均按本项目使用的 Pinocchio 4.1.0 风格整理。m = Model，d = Data。

建模与加载

API	说明
`buildModels::manipulator(m, mimic)`	内置 6 自由度机械臂
`buildModels::humanoid(m, mimic)`	内置仿人机器人
`m.addJoint(parent_idx, joint, placement, name)`	添加关节，返回索引
`m.addFrame(Frame(name, parentJoint, placement, type))`	添加坐标系
`pinocchio::urdf::buildModel(path, m)`	从 URDF 文件加载模型
`m.getFrameId("name")`	按名字查 Frame 索引

模型属性

属性	说明
`m.nq / m.nv`	位形维数 / 速度维数
`m.njoints / m.nframes / m.nbodies`	关节 / 坐标系 / 连杆数量
`m.parents[i]`	关节 i 的父关节索引
`m.jointPlacements[i]`	关节 i 相对父关节的 SE3
`m.inertias[i]`	连杆 i 的惯性参数（mass / lever / inertia）
`m.joints[i].shortname() / .nq() / .nv()`	关节类型与维数

运动学

API	输出位置	说明
`neutral(m)`	返回值	中立位形向量
`forwardKinematics(m, d, q)`	`d.oMi`, `d.oMf`	位姿
`forwardKinematics(m, d, q, v)`	`d.v[i]`	+速度
`getFrameVelocity(m, d, f, ref)`	返回值（Motion）	Frame 速度，ref ∈ {WORLD, LOCAL, LOCAL_WORLD_ALIGNED}
`computeJointJacobians(m, d, q)`	`d.J`	全部关节雅可比（末端 6×n，角速度在前）
`getJointJacobian(m, d, i, ref)`	返回值	指定关节的雅可比
`jacobianCenterOfMass(m, d, q)`	返回值（3×n）	质心雅可比

动力学

API	输出位置	说明
`rnea(m, d, q, v, a)`	返回值 τ	逆动力学（RNEA）
`aba(m, d, q, v, tau)`	返回值 q̈	正动力学（ABA）
`computeGeneralizedGravity(m, d, q)`	`d.g`	重力项
`nonLinearEffects(m, d, q, v)`	`d.nle`	重力+科氏+离心
`computeMinverse(m, d, q)`	`d.Minv`	质量矩阵的逆 M⁻¹
`centerOfMass(m, d, q)`	返回值	质心位置
`computeCentroidalMomentum(m, d)`	`d.hg`	质心动量（先算 FK 带速度）
`computeCentroidalMomentumTimeVariation(m, d)`	`d.dhg`	质心动量变化率

常用类型

类型	内容
`SE3`	位姿：rotation() + translation()，支持 `*`、`inverse()`、`actOnEigenObject(p)`
`Motion`	空间速度：linear() + angular()
`Force`	空间力：linear()（力）+ angular()（力矩）
`Inertia`	惯性：mass() + lever()（质心）+ inertia()（张量）
`JointModelRZ/RY/RX/PX/PY/PZ`	单自由度关节类型

Pinocchio 3.x → 4.x 常见变化

3.x	4.x
`buildSampleModelManipulator(m)`	`buildModels::manipulator(m, false)`
`buildSampleModelHumanoid(m)`	`buildModels::humanoid(m, true)`
`buildSampleModelFreeFlyer/Quadruped`	已移除，改用 URDF
`data.q`	`data.q_in`
`M_PI`（配合宏）	`PI<double>()`
`computeCentroidalDynamics`	`computeCentroidalMomentumTimeVariation`

附录 B：常见编译 / 运行错误排查

B1. C1128: number of sections exceeded object file format limit
原因：Pinocchio 模板展开产物太大。解决：确认 CMakeLists 里有 add_compile_options(/bigobj)（本项目已配置）。

B2. error C2059/C2589 或提示 min/max 相关的奇怪错误
原因：Windows 头文件的 min/max 宏污染。解决：add_compile_definitions(NOMINMAX)（已配置）；或在包含顺序上确保先定义后包含。

B3. Boost 报 BOOST_MPL_LIMIT_LIST_SIZE 超限
解决：确认定义了 BOOST_MPL_LIMIT_LIST_SIZE=40（已配置）。

B4. unresolved external symbol ...（链接错误）
原因：某个依赖库没链接。解决：看符号属于哪个库，补进根 CMakeLists 的 target_link_libraries(pinocchio_deps INTERFACE ...) 列表。常见漏网之鱼：boost 系列、urdfdom 系列。

B5. 运行时提示缺少 .dll
原因：程序找不到 env/Library/bin 下的动态库。解决：运行前把该目录加入 PATH，或从该目录启动 exe。

B6. 调用 computeFrameJacobian 或 5 参数 forwardKinematics 时出现一大串 Boost Fusion 模板错误
这是 MSVC 平台的已知问题。解决：按本项目的替代方案 —— 用 computeJointJacobians + getJointJacobian 组合，以及分两步调用 forwardKinematics。

B7. buildSampleModelManipulator 未定义
原因：你在用 4.x 库但抄了 3.x 代码。解决：查附录 A 的「3.x→4.x 变化表」。

B8. 中文输出乱码
原因：源文件编码或控制台编码不是 UTF-8。解决：确认 /utf-8 编译选项；控制台执行 chcp 65001。

B9. CMake 报「请指定 conda 环境路径」
解决：配置命令加上 -DCONDA_ENV="...\6Pinocchio\env"。

B10. 程序输出全是 0 或者位姿不对
排查顺序：① 是不是忘了调用 forwardKinematics（data 里不会自动有结果）；② 索引是否从 1 开始跳过了 universe；③ q 的维数是否等于 model.nq。

附录 C：中英术语对照表

英文	中文	备注
Rigid Body / Link	刚体 / 连杆	机器人的"骨头"
Joint	关节	连杆间的可动连接
revolute / prismatic joint	旋转 / 平移关节	URDF type 值
Degree of Freedom (DOF)	自由度	独立运动的数目
Configuration q	位形（关节角度向量）	nq 维
Forward Kinematics (FK)	正向运动学	q → 位姿
Jacobian	雅可比矩阵	关节速度 ↔ 空间速度
Singularity	奇异位形	雅可比降秩的姿态
Inverse Dynamics	逆动力学	运动 → 力矩
Forward Dynamics	正动力学	力矩 → 加速度
RNEA	递归牛顿-欧拉算法	逆动力学算法
ABA	铰接体算法	正动力学算法
Mass Matrix M(q)	质量（惯性）矩阵	加速的"阻力"
Coriolis / Centrifugal	科氏力 / 离心力	运动耦合效应
Gravity Compensation	重力补偿	用 G(q) 抵消重力
Center of Mass (CoM)	质心	整机质量集中点
Centroidal Momentum	质心动量	线动量+角动量
Torque τ	关节力矩	电机输出的"劲儿"
Frame	坐标系 / 帧	带原点和三轴的参照物
Pose / SE3	位姿 / 刚体变换	位置 + 朝向
Homogeneous Matrix	齐次变换矩阵	SE3 的 4×4 表示
End Effector	末端执行器	机械臂的"手"
URDF	统一机器人描述格式	XML 模型文件
Inertia Tensor	惯性张量	刚体转动惯量描述
PD Controller	比例-微分控制器	最基础的反馈控制
Computed Torque Control	计算力矩控制	逆动力学控制
Numerical Integration	数值积分	仿真中推进时间
Semi-implicit Euler	半隐式欧拉法	稳定的积分格式
Pseudo-inverse	伪逆	非方阵方程的最小二乘解

附录 D：进阶练习题（学完全部教程后）

D1（运动学）：实现一个逆运动学的数值解法 —— 给定目标末端位置，用雅可比伪逆迭代：q ← q + α·J⁺·(p_target − p_current)，从随机初始姿态出发，观察是否收敛。

D2（动力学）：给仿真加入「关节限位」：当 q 接近 URDF 里的 lower/upper 时施加一个排斥力矩，观察机器人被"弹回"。

D3（控制）：把 06-2 的正弦轨迹换成教程 06-1 的点到点轨迹，让逆动力学控制器完成「停顿-运动-停顿」的循环任务，并统计每次到位的整定时间。

D4（建模）：从零搭建一个 2 自由度平面双连杆机械臂（两个 JointModelRY，运动限制在 XZ 平面），手工推导它的正向运动学公式，与 Pinocchio 的输出对比。

D5（综合）：加载 URDF 模型，实现「重力补偿 + PD」控制器，并故意把模型质量参数改错 50%，观察控制性能的退化 —— 体会「模型精度对基于模型的控制有多重要」。

D6（工具链）：给项目新增一个教程目录 07_exercises，包含你自己的练习程序。要求：正确编写 CMakeLists（模仿现有目录的三行套路），能一键编译。

附录 E：延伸学习资料

官方资源

Pinocchio 官方文档：https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/ （C++ API 全集）
Pinocchio 官方教程（Python 版，概念通用）：https://github.com/stack-of-tasks/pinocchio/tree/master/examples
Eigen 官方手册：https://eigen.tuxfamily.org/dox/ （重点看 Quick Reference 页）

机器人学理论配套

《机器人学导论》（Craig 著）：运动学、雅可比、动力学的经典教材，与本项目章节一一对应；
《Modern Robotics》（Lynch & Park）：使用空间代数（与 Pinocchio 同一套数学语言），有免费 PDF 和视频课；
《Robot Dynamics Lecture Notes》（ETH Zurich, Marco Hutter）：现代视角的动力学讲义，人形/腿足导向。

下一步可以探索的 Pinocchio 模块

碰撞检测（coal/hpp-fcl 集成）
接触与约束动力学（ConstraintDynamics）
自动微分与 CasADi 集成（pinocchio_casadi）
Python 绑定（pinocchio-python，适合快速原型验证）

附录 F：练习题参考答案（含可运行代码思路）

本附录给各章「动手练习」和附录 D 进阶题提供思路 + 关键代码。你可以直接把代码片段粘进对应教程的 .cpp 里（替换或追加），重新编译运行来核对结果。所有代码基于本项目的 Pinocchio 4.1.0 API。

写答案的目的不是让你"抄完就完"，而是对照你的解法查漏补缺——最好先自己写一版，再来看这里。

F.1 第 5 章练习

练习 1（打印连杆数）：在 01_hello_world.cpp 打印 model.nbodies 那一行附近加：

std::cout << "连杆数量 (nbodies): " << model.nbodies << std::endl;

关系：nbodies 是连杆（body）总数，含 universe 对应的世界 body。对本示例机械臂，通常 nbodies == njoints（每个关节挂一个 body，universe 也占一个）。运行后核对数字是否印证这个关系。

练习 2（绕 X 轴转 30°）：在 03_eigen_intro.cpp 的旋转实验处改：

double angle = PI<double>() / 6.0;   // 30°
Eigen::Matrix3d Rx = (Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitX()))
                         .toRotationMatrix();
Eigen::Vector3d r = Rx * Eigen::Vector3d(0.0, 1.0, 0.0);
std::cout << "Rx(30°) * [0,1,0] = " << r.transpose() << std::endl;

手算验证：绕 X 轴转 θ 的矩阵为

Rx = [1   0      0  ]
     [0  cosθ  -sinθ]
     [0  sinθ   cosθ]

Rx·[0,1,0]ᵀ = [0, cosθ, sinθ]ᵀ ≈ [0, 0.8660, 0.5000]。程序输出应一致（末位可能有 1e-16 噪声）。

练习 3（vector<Model> 为何能工作）：把 02_smart_pointers.cpp 里的 std::vector<std::shared_ptr<Model>> 改成 std::vector<Model>，程序照样能编译运行。原因见 5.2 节：Pinocchio 4.x 的 Model 内部用 pimpl（只存一个共享句柄），拷贝极廉价，所以值语义容器也毫无压力。结论：拷贝便宜 ≠ 一定要拷贝，传 const Model& 仍是首选习惯。

F.2 第 6 章练习

练习 1（加第 4 个腕关节）：在 01_create_model.cpp 关节 3 之后追加：

JointModelRZ joint4;   // 绕 X? 题目要求绕 X——用 JointModelRX
// 题目说"绕 X 轴旋转的腕关节"，故用 RX：
JointModelRX joint4;
SE3 joint4_placement(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0.0, 0.3, 0.0));
JointIndex joint4_idx = model.addJoint(
    joint3_idx, joint4, joint4_placement, "joint_4_wrist");

结果：njoints 增加 1（由 4 变 5，含 universe），nq/nv 各增加 1（由 3 变 4）。别忘了顺手给腕关节也 addFrame 一个末端，并重新跑 forwardKinematics 验证。

练习 2（打印关节树）：在 02_joints_and_frames.cpp 加：

for (JointIndex i = 0; i < model.njoints; ++i)
    std::cout << "关节 " << i << " 的父 = " << model.parents[i] << std::endl;

你会看到 parents[1]=0, parents[2]=1, parents[3]=2 …，从任意关节一路 parents[parents[...]] 回溯，最终都到 0（universe）——这就是"机器人是一棵树，universe 是根"的实证。

练习 3（思考题）：关节 2 的 placement 沿 Z 偏移 0.5，是因为关节 1 是绕 Z 旋转的"底座"，它把世界系 Z 轴当作自己的伸展方向，所以大臂沿 Z 向上长；关节 3 沿 Y 偏移 0.4，是因为关节 2 是绕 Y 旋转的"肩"，其局部 Y 轴成了大臂方向，所以小臂沿 Y 伸展。一句话：placement 的偏移方向，永远写在"父关节"的局部坐标轴里。

F.3 第 7 章练习

练习 1（两种方法算末端线速度，验证一致）：在 02_jacobian.cpp 里：

Eigen::VectorXd q_dot = Eigen::VectorXd::Zero(model.nv);
q_dot[0] = 2.0;                       // 关节1 以 2 rad/s 转
forwardKinematics(model, data, q, q_dot);
computeJointJacobians(model, data, q);
Eigen::Vector3d pred_lin = data.J.block(3, 0, 3, data.J.cols()) * q_dot; // 后3行=线速度

FrameIndex ee = model.getFrameId("arm_tool_link");
if (ee < model.nframes) {
    Motion act = getFrameVelocity(model, data, ee, ReferenceFrame::WORLD);
    std::cout << "预测线速度: " << pred_lin.transpose() << std::endl;
    std::cout << "实际线速度: " << act.linear().transpose() << std::endl;
}

两组数字应基本相等（差值在 1e-10 量级）。不一致就说明你对"雅可比后 3 行是线速度"的理解有误——回看 7.2 节。

练习 2（找奇异位形）：让肩、肘两关节轴共线（把相关角度设为 0 看 rank），代码：

Eigen::VectorXd q = neutral(model);
// 尝试若干姿态，这里示范"大臂小臂完全伸直"的一种：
q.setZero();
forwardKinematics(model, data, q);
computeJointJacobians(model, data, q);
Eigen::MatrixXd Jc = data.J;   // 6×n
Eigen::FullPivLU<Eigen::MatrixXd> lu(Jc);
std::cout << "雅可比秩 = " << lu.rank()
          << " (满秩应是 " << std::min(Jc.rows(), Jc.cols()) << ")" << std::endl;

若 rank < min(行,列)，即为奇异/近奇异。多试几个 q 体会 rank 变化。

练习 3（反过来乘会怎样）：把
SE3 base_to_ee = data.oMf[base_idx].inverse() * data.oMf[ee_idx];
改成
SE3 ee_to_base = data.oMf[ee_idx].inverse() * data.oMf[base_idx];
得到的是 ee→base（末端到基座）的变换，恰好是原来的逆。按"消去律"读法：(ee→world) × (world→base) = ee→base。顺序一反，物理含义就反了——这正是 SE3 乘法不可交换（矩阵乘法不交换）的体现。

F.4 第 8 章练习

练习 1（大臂水平→肩重力矩变大）：在 01_inverse_dynamics.cpp：

Eigen::VectorXd q = neutral(model);
q[1] = PI<double>() / 2.0;          // 肩关节转 90°，大臂甩到水平
computeGeneralizedGravity(model, data, q);
std::cout << "重力矩 tau = " << data.g.transpose() << std::endl;

对比竖直姿态（q=neutral）下 data.g[1] 的值：水平时力臂最长，肩力矩显著增大——这正是杠杆原理 力矩 = 重力 × 水平力臂。

练习 2 / 3（PD 增益与步长）：直接改 02_forward_dynamics.cpp 里的 Kd、Kp、dt 三个常量，重新编译运行，观察输出表格里角度/误差的收敛或发散。预期：Kd 太小→过冲振荡；Kp 太大→高频振荡；dt 从 0.01 改到 0.1→半隐式欧拉也可能失稳发散。

练习 4（质心雅可比为什么只有 3 行）：质心是一个"点"，只有线速度（3 维），没有旋转；末端是"刚体"，空间速度含角速度 + 线速度（6 维）。所以 jacobianCenterOfMass 返回 3×n，而 data.J 是 6×n。

F.5 第 9 章练习（方向性提示）

把 models/simple_robot.urdf 里 link1 的 <mass value="5.0"/> 改成 50.0，保存后用 pinocchio::urdf::buildModel 重新加载，跑一遍 computeGeneralizedGravity 看肩关节重力矩变化——质量越大、力臂不变时重力矩成比例增大。
照第 6 章 F.2.1 的写法，在 URDF 末尾加一个 <link> + <joint type="revolute" axis="0 0 1">，重新加载后打印 model.nq 验证多了 1 个自由度。
<axis xyz="1 0 0"/> 让关节绕 X 轴转，机械臂的运动平面会从 YZ 平面变成 YZ? 实际是绕 X 旋转 → 运动发生在 YZ 平面。对比原绕 Y 旋转（运动在 XZ 平面）的差异。

F.6 第 10 章练习（方向性提示）

五次多项式：在 CubicPolynomial 基础上增加起止加速度为 0 两个条件（共 6 个），解 6 元一次方程组得 6 个系数 a0~a5，position(t) 改为 a0+a1 t+a2 t²+a3 t³+a4 t⁴+a5 t⁵。对比三次多项式，加速度曲线在起止点不再跳变，更平滑。
Kp 降到 10：逆动力学控制器的跟踪误差会明显变大（前馈项不变，但反馈增益弱了，对模型误差更敏感）。
重力前馈：在 PD 控制器里先 computeGeneralizedGravity(model, data, q); 得 data.g，输出 tau = Kp*e - Kd*q_dot + data.g;，纯 PD 的稳态误差会被大幅抵消，逼近 ID+PD。
换 URDF 模型：把 01_trajectory_planning.cpp 的 buildModels::manipulator 换成 urdf::buildModel(urdf_path, model)，q_goal 维数改成 model.nq（URDF 是 3 维），其余控制/仿真循环原样复用。

F.7 附录 D 进阶题思路

D1（数值逆运动学）——雅可比伪逆迭代：

Eigen::VectorXd q = Eigen::VectorXd::Random(model.nq) * PI<double>();
for (int iter = 0; iter < 100; ++iter) {
    forwardKinematics(model, data, q);
    FrameIndex ee = model.getFrameId("arm_tool_link");
    Eigen::Vector3d p = data.oMf[ee].translation();          // 当前末端位置
    Eigen::Vector3d e = p_target - p;                        // 位置误差
    if (e.norm() < 1e-4) break;
    computeJointJacobians(model, data, q);
    Eigen::MatrixXd J = data.J.block(3, 0, 3, data.J.cols()); // 仅用线速度部分
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
    svd.setThreshold(1e-6);
    Eigen::VectorXd dq = svd.solve(e);                       // 伪逆：dq = J⁺·e
    q += 0.5 * dq;                                           // α=0.5 步长，防过冲
}

观察：从随机初始姿态出发，末端逐渐逼近 p_target；若目标不可达或陷入奇异，迭代会停滞——这正是 IK 的真实难点。

D2（关节限位排斥力矩）：在仿真循环算完 tau 后加：

for (int i = 0; i < model.nq; ++i) {
    double lo = jnt_lower[i], hi = jnt_upper[i];   // 从 URDF limit 读取
    if (q[i] < lo + 0.05) tau[i] += K_rep * (lo + 0.05 - q[i]);   // 贴近下界→往里推
    if (q[i] > hi - 0.05) tau[i] -= K_rep * (q[i] - hi + 0.05);
}

机器人会被"弹回"限位内。

D3（06-2 用点到点轨迹）：把 TrajectoryGenerator 的正弦改成教程 06-1 的 CubicPolynomial（起止速度为 0），让逆动力学控制器完成"停顿-运动-停顿"，统计每次到位的整定时间（误差首次小于阈值后的稳定时间）。

D4（2-DOF 平面双连杆，手工 FK）：建两个 JointModelRY，限制在 XZ 平面。手工推导：

设连杆长 L1, L2，关节角 θ1, θ2（均绕 Y，在 XZ 平面）：
末端 X = L1·sinθ1 + L2·sin(θ1+θ2)
末端 Z = L1·cosθ1 + L2·cos(θ1+θ2)

用 forwardKinematics 算出的 data.oMf[ee].translation() 与之对比，应完全吻合——这是检验你"运动学=变换链乘法"理解的最硬核方法。

D5（重力补偿+PD，故意改错质量）：实现 tau = Kp·e - Kd·q_dot + data.g 的控制器，先把模型质量参数设对跑一遍记误差，再把 model.inertias[i].mass() 人为改错 50%（用 model.inertias[i].mass() = ... 不可直接写，需用 model.inertias[i] 的可变接口或重建模型），重跑看误差退化——你会直观感受到"基于模型的控制有多依赖模型精度"。

D6（新增教程目录）：在 tutorials/ 下建 07_exercises/，里面放你的 .cpp 和一份三行式 CMakeLists.txt：

add_executable(07_my_test 07_my_test.cpp)
target_link_libraries(07_my_test PRIVATE pinocchio_deps)
set_target_properties(07_my_test PROPERTIES
    RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/bin/07_exercises")

并在根 CMakeLists.txt 末尾加一行 add_subdirectory(tutorials/07_exercises)，重跑 CMake 配置即可一键编译。

结语

走到这里，你已经完成了从「C++ 初学者」到「能用 Pinocchio 做机器人建模、运动学、动力学计算与控制仿真」的完整旅程。回头看，这个项目其实只教了一件事的六个侧面：

Model + Data + 算法函数，读模型、填数据、算结果。

所有的运动学、动力学、控制，都是这个模式在不同算法上的展开。掌握了这个模式，再去读 Pinocchio 的官方示例、论文配套代码，你会发现它们全都"似曾相识"。

机器人学的世界很大 —— 运动规划、状态估计、强化学习、视觉伺服……而动力学计算永远是其中绕不开的地基。祝你在这条路上走得开心。

DAMO开发者矩阵

DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。

更多推荐

电力挂轨巡检机器人：变电站与输电线路智能巡检方案

DAMO开发者矩阵

锚定具身智能、通用AI、低空经济三大国家战略赛道——2027赛逸展与“十五五”规划同频共振

DAMO开发者矩阵

**935亿元的具身智能融资，掏出来的不是未来，是一道洗牌的分水岭**

DAMO开发者矩阵

所有评论(0)

查看更多评论

ai2work

@m0_64703412

已为社区贡献3条内容

Pinocchio C++ 初学者讲解手册

ai2work

写在前面：这本手册想帮你做什么

目录

第 0 章 学前准备（面向绝对零基础）

0.1 你现在的起点，和这条学习路线的全貌

0.2 软件安装清单（一步一步来，别跳）

0.2.1 Visual Studio 2022 Community（免费的，足够）

0.2.2 Miniforge3（conda 的轻量开源版）

0.2.3 这三个工具分别干啥，一句话记住

0.3 一键搭建：运行 setup.bat

期望看到的画面（节选真实输出）

0.4 第一次运行：期望看到什么

0.5 用 Visual Studio 图形界面构建（不想记命令行时）

0.6 调试入门：断点、监视、单步（看懂"程序到底在干什么"）

0.7 怎么读报错（每个初学者都会遇到，照着对号入座）

A. 编译错误（compiler error）——“代码有语法/类型问题，压根没生成 exe”

B. 链接错误（linker error）——“代码语法没错，但找不到实现”

C. 运行时错误（runtime error）——“编译链接都过了，一运行就崩”

错误排查决策树

0.8 数学前置课：向量与矩阵（机器人学的语言）

0.8.1 三种"数"：标量、向量、矩阵

0.8.2 向量的两个基本操作

0.8.3 矩阵 × 向量 = 变换（最核心的一句）

0.8.4 三个你必须认识的特殊运算

0.8.5 为什么机器人学离不开这些

0.8.6 给你的一分钟自测

第 1 章 认识项目

1.1 Pinocchio 是什么？

1.2 在写代码之前：必须懂的机器人学概念

1.2.1 刚体（Rigid Body）与连杆（Link）

1.2.2 关节（Joint）

1.2.3 自由度（DOF, Degree of Freedom）

1.2.4 坐标系 / 帧（Frame）

1.2.5 位姿（Pose）与 SE3

1.2.6 运动学与动力学：一对「正反问题」

1.3 项目目录结构

1.4 本项目的技术栈一览

第 2 章 环境搭建与构建

2.1 先理解「构建」这件事

2.2 setup.bat 逐段讲解

2.3 CMake 命令详解

2.4 根 CMakeLists.txt 逐行讲解

2.5 动手：从零构建一遍

2.6 改了代码之后怎么办？

2.7 编译产物都去哪了：build 目录结构详解

2.8 安装/网络问题对策（conda 下载慢或失败）

第 3 章 C++ 语言基础速成

3.1 一个 C++ 程序的骨架

3.2 变量与基本数据类型

3.3 运算符与表达式

3.4 控制流：if、for、while、switch

3.5 函数

3.6 容器：std::vector 与 std::string

std::vector：可以自动变长的数组

std::string：字符串

3.7 引用（Reference）：C++ 最容易被误解的概念

3.8 指针与智能指针

原始指针（了解即可）

std::shared_ptr：共享所有权

std::unique_ptr：独占所有权

什么时候该用哪个？

3.9 auto 与范围 for 的完整真相

3.10 类（class）：把数据和操作打包

3.11 模板（Template）：类型也能当参数

3.12 命名空间（namespace）

3.13 异常处理（try / catch）

3.14 std::function 与类型别名（using）

3.15 预处理指令补充：#define

3.16 输出格式化：让打印更整齐

3.17 内存模型：栈（stack）与堆（heap）——理解指针/引用的地基

3.18 本章 C++ 速成课小结与自查清单

第 4 章 Eigen 快速入门：Pinocchio 的数学基石

第 4 章 Eigen 快速入门：Pinocchio 的数学基石

4.1 Eigen 的核心类型一览

4.2 Vector3d：三维向量的创建与运算

4.3 Matrix3d：矩阵与旋转

4.4 VectorXd / MatrixXd：运行时才知道大小的向量矩阵

4.5 解线性方程组：不要求逆

4.6 SE3：Pinocchio 的位姿类型（Eigen 之上的抽象）

第 0 章学前准备（面向绝对零基础）

第 1 章认识项目

第 2 章环境搭建与构建

第 5 章教程 01 详解：C++ 基础与 Pinocchio 入门

第 6 章教程 02 详解：机器人模型创建

第 7 章教程 03 详解：运动学计算

第 8 章教程 04 详解：动力学计算

第 9 章教程 05 详解：URDF 模型加载

第 10 章教程 06 详解：综合实战