本章解决一个问题：一台机器人在 Pinocchio 里到底是被怎么「装」起来的，以及为什么每次调用算法都要同时递进去一个 Model 和一个 Data。这一章不涉及任何动力学公式，只把两件事量清楚：结构（关节树 + 坐标系树）和尺寸（nq/nv/njoints/nframes）。后面 11 章的每一行代码都建立在这两个表上。

先修：第 00 章（环境、build/bin）、第 01 章（值 / const 引用）、第 02 章（VectorXd 的区间与 segment）。
本章新增的 C++ 知识：位标志枚举（FrameType）为什么不能直接 cout；std::left/std::setw 排版；按索引访问 std::vector 成员。
产出：tutorials/02_model/01_create_model.cpp。这是全书第一个「你自己造出来的机器人」——只有 3 个关节，但它有完整的关节树、坐标系树，而且能算出末端位置。

---

3.1 一台机器人是一棵树，不是一串连杆

先建立物理图像。固定基座机械臂的拓扑是这样的：

世界(universe)
  └─ 关节1 绕 Z（底座回转）        jointPlacement = 原点
       └─ 关节2 绕 Y（肩）          jointPlacement = 沿父帧 Z 移 0.5 m
            └─ 关节3 绕 Y（肘）     jointPlacement = 沿父帧 Y 移 0.4 m
                 └─ end_effector    framePlacement = 沿父帧 Y 移 0.3 m

三个要点，全是本章程序逐条打印过的：

1. 关节之间是父子关系，每个关节只记「我在父亲的坐标系里哪儿」（model.jointPlacements[i]）。世界位姿不是你填的，是乘出来的：从 universe 一路把相对变换乘下来。这就是为什么改一个关节角会动到它下游所有连杆。
2. 坐标系（Frame）挂在关节上，不是挂在「连杆之间」。end_effector 的 parentJoint = 3，它自己不再产生自由度，只是一个「请替我算出这个点在世界里的位置」的请求标记。传感器帧、抓取点帧、相机帧都是这种「零自由度标记」。
3. 根上那位不是你的关节。索引 0 永远是 universe（世界），它没有质量、没有自由度，只是让「第一个真实关节的父节点」有个合法索引可填。这个 +1 偏移是本章最容易看错的数字，3.3 节用实测数据钉死它。

3.2 Model 存结构，Data 存中间量

这是 Pinocchio 和多数机器人库最不一样的设计，也是新手最容易写错的地方。

	Model	Data
装什么	关节树、坐标系树、惯性参数、名字、各种尺寸	每个算法的输入缓存和输出结果
什么时候变	建模型的时候；跑起来之后不再变	每次调用算法都被改写
谁分配内存	你 addJoint/addFrame 时	Data data(model) 按 Model 的尺寸一次性开好
典型成员	njoints nq nv joints inertias frames names	oMi oMf v a M g nle tau

为什么要把缓存拆出来？ 因为动力学算法是几十次调用/控制周期的事。如果每次 rnea 都自己 new 一堆 VectorXd(nv)、MatrixXd(nv,nv)，光分配内存就够把 1 kHz 控制循环打死。拆成 Model + Data 之后，内存在 Data data(model) 那一刻分配完，之后每次调用只是往同一块地上重写数字，零分配。

代价是两条纪律，第 10 章会专门做实验：

• 同一个 Data 是共享草稿纸。先 crba 再 rnea，data.M 不会被 rnea 清掉，但 data.g、data.tau 之类的字段会被覆盖。读结果的时机是「紧接着那次调用」，中间别插别的算法调用。
• Data 绑死在构造它的那个 Model 上。建完 Data 又往 Model 里加关节，Data 的数组尺寸不会自己长——轻则结果错，重则越界。正确做法是加完关节再 Data(model)，或者 data = Data(model)。

顺带把第 01 章那条纠正过的话再压一遍：Model 没有 pimpl，拷贝就是深拷贝（实测 sizeof(Model) = 912 字节，里面是一堆 std::vector）。所以「按值传 Model 很轻」是错的；本章往后所有函数签名一律 const Model& + Data&。

3.3 四个尺寸，外加那个 +1

先把名字对上，本章程序【模型验证】一节把这五个数全打了出来：

字段	含义	本章的 3 关节臂实测
nq	构形向量 q 的长度	3
nv	速度/力矩向量 v、tau 的长度	3
njoints	关节数组长度，含 universe	4
nvExtended	雅可比空间的维数（文档原话 Dimension of the jacobian space）	3
nbodies	BODY 型坐标系的个数，含根	1
nframes	坐标系数组长度	5

nbodies 这个 1 值得停一下：模型明明有 3 个连杆，为什么是 1？因为它不是「连杆数」，而是「往关节上挂过几次惯性体」的计数——只有 model.appendBodyToJoint(...) 会让它 +1，初值 1 是那个根。本章用 addJoint 建骨架、惯性一律是零（addJoint 的文档原话：The inertia supported by the joint is set to Zero），所以它一直停在 1。对照第 00 章 01_hello_world 的输出：示例模型 buildModels::manipulator 是 njoints=7、nbodies=7，因为示例模型的每个关节都挂了一个惯性体。结论：njoints 数结构，nbodies 数惯性挂载，别混用。（惯性什么时候真正重要是第 05 章的主题。）

读表：你写了 3 次 addJoint，njoints 却是 4——因为 joints[0] 是 universe；nq、nv 是 3——因为 universe 不占自由度。这两套索引差 1，全库到处都是：

• 关节索引 1..njoints-1 对应 q 里的第 0..nv-1 段；
• model.names[i] 和 model.joints[i] 是同一套（含 universe）的索引，所以第 i 个真实关节叫 names[i+1]。第 13 章那张惯量表打印关节名用的就是 model.names[i + 1]，不是笔误。

joints[0] 还有个更阴的：它是个默认构造的占位关节。本章程序把它「自报家门」打印出来，实测：

[0] universe  (类型 JointModelRX, nq=0, nv=0; joints[i] 自报 nq=1)

nq=0 来自 model.nqs[0]（权威），自报 nq=1 来自 model.joints[0].nq()（占位对象自己说的）。两个数不一样，而且 joints[0] 说的错。 所以：尺寸一律问 model.nqs / model.nvs / model.idx_qs / model.idx_vs，别问 model.joints[i]。

3.4 q 是分段的，不是一个关节一个数

q 是 VectorXd(nq)，但它是每个关节占一段连续区间拼出来的：区间起点在 idx_qs[i]、长度在 nqs[i]；v 同理用 idx_vs / nvs。本章程序把这张布局表打出来，三个转动关节都是 1 长的小段，看起来平平无奇。

之所以现在就强调它，是因为 nq 和 nv 可以不相等。旋转关节用 1 个数表示角度，nq=nv=1；但自由飞行关节（连杆悬在空中 6 自由度）用 [x,y,z, 四元数 4 个] = 7 个数表示姿态，而速度只有 6 维（3 平移 + 3 旋转），于是 nq=7、nv=6。这条不对称会带来一串后果：

• q += v*dt 是错的（维度都对不上，四元数也不能这么加）。要用 integrate(model, q, delta) / difference(model, q0, q1)，第 11 章起全程在用。
• 任何「按 nv 分配、按 nq 索引」的写法都要重新检查。
• 雅可比的列数是 nv 不是 nq。

第 5 章会把浮动基座的示例模型真建出来，实测数字是 Humanoid(usingFF=true)：nq=35、nv=34，根部那个 JointModelFreeFlyer 自己 nq=7、nv=6——这张布局表立刻不再对称。本章你只需要记住两条：永远不要假设 nq == nv，也永远不要假设「关节 i 就占 q[i]」。

3.5 建一个关节要三步，第四步很多人忘

Pinocchio 4.x 的写法（本章程序逐个关节都走了这四步）：

Model model;                                     // 空模型，自带 universe
JointModelRZ joint1;                             // 关节「类型」对象，1 自由度绕 Z
JointIndex j1 = model.addJoint(0,                // 父关节索引（0 = universe）
                              joint1,
                              SE3::Identity(),   // 我在父帧里的位姿
                              "joint_1_base_rotation");
FrameIndex f1 = model.addJointFrame(j1);         // ← 第四步，4.x 必须显式调用

三个容易忽略的细节：

1. addJoint 返回的是索引，你后面全都要靠它，别丢弃返回值直接写死 1,2,3。名字写错、顺序改了，硬编码的索引就全错，而编译不会响。
2. jointPlacement 是相对父关节帧的，不是世界坐标。第二个关节填 (0,0,0.5) 意思是「沿我父亲（关节1）的 Z 轴走 0.5 m」——关节 1 一转，这个 0.5 m 的方向跟着转。想放世界坐标是你自己的事，没人替你算。
3. addJoint 不再顺手创建坐标系。这是 Pinocchio 4.x 相对旧教程/旧例子的一条行为变化：老代码里 addJoint 会附带生成一个同名关节帧，4.x 里你必须显式调 addJointFrame()。忘了的后果不是报错，而是后面 model.getFrameId("joint_1") 找不到帧（第 4 章会看到它静默返回 nframes）。本章输出里每行「索引=2, 其坐标系索引=2」就是在自证这一步做了。

3.6 Frame：五个参数，少一个就变味

Frame ee("end_effector",        // 名字
         joint3_idx,            // parentJoint：我挂在哪个关节上
         joint3_frame_idx,      // parentFrame：我的位姿相对哪个帧表达
         ee_placement,          // SE3，相对 parentFrame
         OP_FRAME);             // 类型
FrameIndex ee_idx = model.addFrame(ee);

parentJoint 和 parentFrame 是两件事：前者说「我属于哪个关节的运动子树」（决定我跟着谁动），后者说「我的位姿数值是从哪个帧量起的」。填错 parentFrame，位姿就从一个错误的原点累加。

Pinocchio 还留了一个少写 parentFrame 的四参数版本，它会把 parentFrame 直接写成 0（universe）。于是你以为「末端在肘前 0.3 m」，模型理解成「末端在世界原点前 0.3 m」。不报错、不崩、数字就是错。本章用的是五参数版本，输出里那行 父坐标系=3 就是证据。

坐标系类型是个位标志枚举 FrameType：

类型	用途
JOINT	关节自己的帧，addJointFrame 生成的就是它
FIXED_JOINT	universe / 固定关节帧
BODY	连杆质心帧，动力学内部用
OP_FRAME	你关心的操作点：末端、抓取点
SENSOR	传感器安装点

因为是位标志，cout << frame.type 打出来的是一个数字，而且 getFrameId 的默认类型过滤是「这几种或起来」——所以本章程序自带一个 frameTypeName() 把类型翻成人话。这张表后面第 4 章、第 7 章都要用。

3.7 算出来的位姿在 data.oMi 和 data.oMf，而且不是同一个函数填的

这是本章最重要的一条 API 纪律，程序末尾做了现场对照实验：

forwardKinematics(model, data, q);          // 填 data.oMi（每个关节的世界位姿）
// 此时读 data.oMf[ee_idx]  ->  [0, 0, 0]，不报错
updateFramePlacements(model, data);         // 才把 data.oMf 算出来
// 再读                        ->  [0, 0.7, 0.5]

data.oMf[ee_idx].translation() 在漏掉第二步时不会给你 NaN、不会越界、不会抛异常，它就是静静地躺在初始的单位变换上。这是全书最贵的一类 bug：控制律拿到一个永远不变的末端位置，PD 看着「误差怎么不收敛」，你把增益越调越大，最后机床抖起来——而代码从头到尾没有一行报错。

第 07 章会把这条链讲全（forwardKinematics 的 JointLevel/BodyLevel 选项、oMi 与 oMf 的对应关系）。本章先把它变成肌肉记忆：读 oMf 之前必须 updateFramePlacements。

3.8 工程里加上这一章

打开根 CMakeLists.txt，把第 00 章留下的那行注释去掉：

add_subdirectory(tutorials/01_basics)
add_subdirectory(tutorials/02_model)   # ← 本章
# add_subdirectory(tutorials/03_kinematics)

新建 tutorials/02_model/CMakeLists.txt：

# 教程 02：亲手搭建机器人模型
add_tutorial(01_create_model)          # Model/Data 分工、关节树、坐标系

第 04、05 章会往这个文件里再加两行。改完先重新 cmake -S . -B build ... 再编译（理由见第 01 章 1.3 节）。

---

3.9 完整代码

存成 tutorials/02_model/01_create_model.cpp。整块复制即可编译：

// ============================================================================
// 教程 02-1: 从零创建机器人模型
// ============================================================================
// 
// 【学习目标】
// 1. 理解 Pinocchio 中机器人模型的层次结构：Model → Joint → Frame
// 2. 学习如何编程式地构建一个简单的机器人模型
// 3. 掌握 JointModel 类型（旋转关节、平移关节等）
// 
// 【C++ 知识点】
// - 构造函数初始化列表
// - 成员函数和方法调用
// - 枚举类型（enum class）
// - 引用（&）作为函数参数
// 
// 【Pinocchio 知识点】
// - pinocchio::Model::addJoint()：向模型添加关节
// - pinocchio::JointModelRZ / JointModelRY：关节类型
// - pinocchio::Frame：连杆上的坐标系
// - pinocchio::Model::addFrame()：向模型添加坐标系
// ============================================================================

#include <iostream>
#include <iomanip>
#include <string>
#include "pinocchio/multibody.hpp"
#include "pinocchio/multibody/joint.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/frames.hpp"            // updateFramePlacements
#include "pinocchio/algorithm/joint-configuration.hpp"

using namespace pinocchio;

// FrameType 是一个位标志枚举，直接 cout 只会打印出数字，
// 这里给初学者一个可读的名字。
const char * frameTypeName(FrameType type) {
    switch (type) {
        case OP_FRAME:     return "OP_FRAME(用户定义的操作帧)";
        case JOINT:        return "JOINT(关节帧)";
        case FIXED_JOINT:  return "FIXED_JOINT(固定关节帧)";
        case BODY:         return "BODY(连杆帧)";
        case SENSOR:       return "SENSOR(传感器帧)";
        default:           return "复合/未知类型";
    }
}

int main() {
    std::cout << "===== 教程 02-1: 从零创建机器人模型 =====" << std::endl;
    std::cout << std::endl;
    
    // ========================================================================
    // 创建一个简单的 3 自由度机械臂
    // 结构：基座(固定) → 关节1(绕Z) → 连杆1 → 关节2(绕Y) → 连杆2 → 关节3(绕Y) → 末端
    // ========================================================================
    
    Model model;
    
    // ------------------------------------------------------------------------
    // 1. 关于固定基座
    //    Pinocchio 中，关节索引 0 是"宇宙"(universe)关节，代表世界坐标系
    //    对于固定基座机器人，我们直接从索引 0 开始添加关节
    //    不需要添加额外的"固定关节"
    // ------------------------------------------------------------------------
    std::cout << "【说明】固定基座机器人直接从 universe 关节(索引0)开始" << std::endl;
    std::cout << std::endl;
    
    // ------------------------------------------------------------------------
    // 2. 添加关节 1：绕 Z 轴旋转（底座旋转）
    //    JointModelRZ 是绕 Z 轴的 1-DOF 旋转关节
    //    addJoint 参数（新 API）：
    //      - 父关节索引
    //      - 关节模型对象（JointModel）
    //      - 关节在父坐标系下的变换（SE3）
    //      - 关节名称
    // ------------------------------------------------------------------------
    
    JointModelRZ joint1;  // 绕 Z 轴旋转
    
    JointIndex joint1_idx = model.addJoint(
        JointIndex(0),        // 父关节：universe（世界坐标系）
        joint1,               // 关节模型
        SE3::Identity(),      // 关节位于原点，无偏移
        "joint_1_base_rotation"
    );
    
    // ------------------------------------------------------------------------
    // 2.5 给关节补一个坐标系：addJointFrame
    //     Pinocchio 4.x 里 addJoint() 只登记关节本身，不会再顺手创建坐标系。
    //     想让别人（以及你自己写的 getFrameId）能找到"关节1 的坐标系"，
    //     必须显式调用 addJointFrame()。返回值就是这个新坐标系的索引。
    //     下面每个关节都补一个，稍后末端坐标系要挂在关节3 的坐标系上。
    // ------------------------------------------------------------------------
    FrameIndex joint1_frame_idx = model.addJointFrame(joint1_idx);
    
    std::cout << "添加关节1 (绕Z), 索引=" << joint1_idx 
              << ", 其坐标系索引=" << joint1_frame_idx << std::endl;
    
    // ------------------------------------------------------------------------
    // 3. 添加关节 2：绕 Y 轴旋转（肩关节）
    //    关节位置：在关节1上方，偏移一段距离
    //    SE3 构造方式：SE3(旋转矩阵, 平移向量)
    //    使用 Eigen::Matrix3d::Identity() 表示无旋转
    //    使用 Eigen::Vector3d 表示平移
    // ------------------------------------------------------------------------
    
    JointModelRY joint2;  // 绕 Y 轴旋转
    
    // 关节相对于父关节的变换：沿 Z 轴偏移 0.5 米
    SE3 joint2_placement(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0.0, 0.0, 0.5));
    
    JointIndex joint2_idx = model.addJoint(
        joint1_idx,           // 父关节：关节1
        joint2,               // 关节模型
        joint2_placement,     // 沿 Z 偏移 0.5m
        "joint_2_shoulder"
    );
    
    FrameIndex joint2_frame_idx = model.addJointFrame(joint2_idx);
    
    std::cout << "添加关节2 (绕Y), 索引=" << joint2_idx 
              << ", 其坐标系索引=" << joint2_frame_idx << std::endl;
    
    // ------------------------------------------------------------------------
    // 4. 添加关节 3：绕 Y 轴旋转（肘关节）
    //    偏移位置：沿 Y 轴方向延伸
    // ------------------------------------------------------------------------
    
    JointModelRY joint3;
    
    // 关节相对于父关节的变换：沿 Y 轴偏移 0.4 米
    SE3 joint3_placement(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0.0, 0.4, 0.0));
    
    JointIndex joint3_idx = model.addJoint(
        joint2_idx,
        joint3,
        joint3_placement,
        "joint_3_elbow"
    );
    
    FrameIndex joint3_frame_idx = model.addJointFrame(joint3_idx);
    
    std::cout << "添加关节3 (绕Y), 索引=" << joint3_idx 
              << ", 其坐标系索引=" << joint3_frame_idx << std::endl;
    std::cout << std::endl;
    
    // ========================================================================
    // 添加坐标系（Frame）
    // ========================================================================
    // Frame 附加在关节上，用于表示连杆上的特定位置
    // 例如：末端执行器、传感器安装点等
    //
    // Frame 构造参数（5 参数版本）：
    //   - 名称
    //   - 父关节索引
    //   - 父坐标系索引：新坐标系的位姿是相对它表达的
    //   - 位姿变换（SE3）
    //   - 坐标系类型（OP_FRAME = 操作帧）
    //
    // 另有一个少写"父坐标系"的 4 参数版本，它会把父坐标系写成 0（universe），
    // 于是位姿就被当成相对世界原点来表达，几乎总是不是你想要的。
    // 这里用 5 参数版本，把末端挂在关节3 的坐标系上。
    // ------------------------------------------------------------------------
    
    // 末端执行器坐标系（附加在关节3上）
    SE3 ee_placement(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0.0, 0.3, 0.0));  // 沿 Y 轴偏移
    
    Frame ee_frame(
        "end_effector",       // 帧名称
        joint3_idx,           // 父关节
        joint3_frame_idx,     // 父坐标系：关节3 自己的坐标系
        ee_placement,         // 位姿（相对父坐标系）
        OP_FRAME              // 帧类型：操作帧
    );
    
    FrameIndex ee_frame_idx = model.addFrame(ee_frame);
    
    std::cout << "添加末端坐标系, 索引=" << ee_frame_idx << std::endl;
    std::cout << std::endl;
    
    // ========================================================================
    // 验证模型
    // ========================================================================
    
    std::cout << "【模型验证】" << std::endl;
    std::cout << "  关节总数 (njoints): " << model.njoints << std::endl;
    std::cout << "  连杆总数 (nbodies): " << model.nbodies << std::endl;
    std::cout << "  自由度数 (nq): " << model.nq << std::endl;
    std::cout << "  速度维数 (nv): " << model.nv << std::endl;
    std::cout << "  雅可比维数 (nvExtended): " << model.nvExtended << std::endl;
    std::cout << "  坐标系总数 (nframes): " << model.nframes << std::endl;
    std::cout << std::endl;
    
    // 注意 njoints = 3 个真实关节 + 1 个 universe = 4。
    // 索引 0 那位不是你的关节：Pinocchio 用一个占位关节表示"世界"，
    // 它的 shortname() 会报到第一个真实关节的类型上去，别被误导。
    // 关节的名字在 model.names 里（addJoint 的最后一个参数就是它）。
    std::cout << "  关节清单:" << std::endl;
    for (JointIndex i = 0; i < model.njoints; ++i) {
        std::cout << "    [" << i << "] " << model.names[i]
                  << "  (类型 " << model.joints[i].shortname()
                  << ", nq=" << model.nqs[i]
                  << ", nv=" << model.nvs[i]
                  << "; joints[i] 自报 nq=" << model.joints[i].nq() << ")" << std::endl;
    }
    std::cout << std::endl;
    
    // q 和 v 不是一一对应的「每个关节一个数」：每个关节占一段连续区间，
    // 区间起点在 idx_qs/idx_vs 里，长度在 nqs/nvs 里，也就是说
    // 「关节 i 占 q 的 [idx_qs[i], idx_qs[i]+nqs[i])」。下面顺手把两个数组
    // 的长度也打出来，让读者能自己核对它们是按关节索引的。
    // 对旋转关节 nq=nv，这张表看不出名堂；第 5 章的自由飞行关节
    // nq=7（3 个平移 + 4 个四元数分量）而 nv=6，表就会立刻不对称。
    std::cout << "  每个关节在 q / v 里占的区间 (idx_qs.size() = "
              << model.idx_qs.size() << ", nqs.size() = " << model.nqs.size()
              << "):" << std::endl;
    for (JointIndex i = 0; i < model.njoints; ++i) {
        std::cout << "    [" << i << "] " << std::left << std::setw(24)
                  << model.names[i]
                  << " q: [" << model.idx_qs[i] << ", "
                  << model.idx_qs[i] + model.nqs[i] << ")  v: ["
                  << model.idx_vs[i] << ", " << model.idx_vs[i] + model.nvs[i]
                  << ")" << std::endl;
    }
    std::cout << std::endl;
    std::cout << "  坐标系清单:" << std::endl;
    for (FrameIndex i = 0; i < model.nframes; ++i) {
        const Frame & frame = model.frames[i];
        std::cout << "    [" << i << "] " << frame.name
                  << "  父关节=" << frame.parentJoint
                  << "  父坐标系=" << frame.parentFrame
                  << "  类型=" << frameTypeName(frame.type) << std::endl;
    }
    std::cout << std::endl;
    
    // ========================================================================
    // 创建数据对象并验证模型可以计算
    // ========================================================================
    
    Data data(model);
    
    // 设置一个中立姿态
    Eigen::VectorXd q_neutral = neutral(model);
    
    std::cout << "【中立姿态 q】" << std::endl;
    for (int i = 0; i < model.nq; ++i) {
        std::cout << "  q[" << i << "] = " << q_neutral[i] << std::endl;
    }
    std::cout << std::endl;
    
    // 验证：正向运动学
    forwardKinematics(model, data, q_neutral);
    
    std::cout << "【正向运动学验证】" << std::endl;
    std::cout << "  各关节的世界位姿 (data.oMi):" << std::endl;
    for (JointIndex i = 1; i < model.njoints; ++i) {  // 0 是 universe，跳过
        Eigen::Vector3d pos = data.oMi[i].translation();
        std::cout << "    " << model.names[i] << ": [" 
                  << pos.x() << ", " << pos.y() << ", " << pos.z() << "]" << std::endl;
    }
    std::cout << std::endl;
    
    // 末端坐标系的位姿存在 data.oMf 里，但它不是 forwardKinematics 顺手算好的：
    // 必须再调用一次 updateFramePlacements。先读一次，看看"漏掉这一步"长什么样：
    // 不会报错、不会是 NaN，就是一开始那个单位变换， translation() 给你 [0,0,0]。
    std::cout << "  还没调用 updateFramePlacements 时, data.oMf["
              << ee_frame_idx << "] 的平移 = ["
              << data.oMf[ee_frame_idx].translation().transpose()
              << "]   <-- 静默错误，程序不会提醒你" << std::endl;
    
    updateFramePlacements(model, data);
    
    std::cout << "  调用 updateFramePlacements 之后:" << std::endl;
    std::cout << "  末端执行器 end_effector 的世界位姿 (data.oMf[" << ee_frame_idx << "]):" << std::endl;
    Eigen::Vector3d ee_pos = data.oMf[ee_frame_idx].translation();
    std::cout << "    [" << ee_pos.x() << ", " << ee_pos.y() << ", " << ee_pos.z() << "]"
              << std::endl;
    std::cout << "  验算：末端相对关节3 的偏移 ["
              << ee_placement.translation().x() << ", "
              << ee_placement.translation().y() << ", "
              << ee_placement.translation().z() << "]，关节3 自己的位姿 ["
              << data.oMi[joint3_idx].translation().x() << ", "
              << data.oMi[joint3_idx].translation().y() << ", "
              << data.oMi[joint3_idx].translation().z() << "]，相加 = ["
              << (data.oMi[joint3_idx].translation() + ee_placement.translation()).x() << ", "
              << (data.oMi[joint3_idx].translation() + ee_placement.translation()).y() << ", "
              << (data.oMi[joint3_idx].translation() + ee_placement.translation()).z() << "]"
              << std::endl;
    std::cout << "        与上面 oMf 的数字一致（中立姿态没有旋转，平移可以直接相加）。"
              << std::endl;
    std::cout << std::endl;
    
    std::cout << "模型创建成功！" << std::endl;
    std::cout << "接下来请运行教程 02-2 学习关节和坐标系的详细操作。" << std::endl;
    
    return 0;
}

---

3.10 编译并运行

cmake -S . -B build -G "Visual Studio 17 2022" -A x64
cmake --build build --config Release
build\bin\01_create_model.exe

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

---

3.11 期望输出（实测快照）

Pinocchio 4.1.0 + Eigen 5.0.1 + MSVC v143 + /utf-8，Release 实测，退出码 0：

===== 教程 02-1: 从零创建机器人模型 =====

【说明】固定基座机器人直接从 universe 关节(索引0)开始

添加关节1 (绕Z), 索引=1, 其坐标系索引=1
添加关节2 (绕Y), 索引=2, 其坐标系索引=2
添加关节3 (绕Y), 索引=3, 其坐标系索引=3

添加末端坐标系, 索引=4

【模型验证】
  关节总数 (njoints): 4
  连杆总数 (nbodies): 1
  自由度数 (nq): 3
  速度维数 (nv): 3
  雅可比维数 (nvExtended): 3
  坐标系总数 (nframes): 5

  关节清单:
    [0] universe  (类型 JointModelRX, nq=0, nv=0; joints[i] 自报 nq=1)
    [1] joint_1_base_rotation  (类型 JointModelRZ, nq=1, nv=1; joints[i] 自报 nq=1)
    [2] joint_2_shoulder  (类型 JointModelRY, nq=1, nv=1; joints[i] 自报 nq=1)
    [3] joint_3_elbow  (类型 JointModelRY, nq=1, nv=1; joints[i] 自报 nq=1)

  每个关节在 q / v 里占的区间 (idx_qs.size() = 4, nqs.size() = 4):
    [0] universe                 q: [0, 0)  v: [0, 0)
    [1] joint_1_base_rotation    q: [0, 1)  v: [0, 1)
    [2] joint_2_shoulder         q: [1, 2)  v: [1, 2)
    [3] joint_3_elbow            q: [2, 3)  v: [2, 3)

  坐标系清单:
    [0] universe  父关节=0  父坐标系=0  类型=FIXED_JOINT(固定关节帧)
    [1] joint_1_base_rotation  父关节=1  父坐标系=0  类型=JOINT(关节帧)
    [2] joint_2_shoulder  父关节=2  父坐标系=1  类型=JOINT(关节帧)
    [3] joint_3_elbow  父关节=3  父坐标系=2  类型=JOINT(关节帧)
    [4] end_effector  父关节=3  父坐标系=3  类型=OP_FRAME(用户定义的操作帧)

【中立姿态 q】
  q[0] = 0
  q[1] = 0
  q[2] = 0

【正向运动学验证】
  各关节的世界位姿 (data.oMi):
    joint_1_base_rotation: [0, 0, 0]
    joint_2_shoulder: [0, 0, 0.5]
    joint_3_elbow: [0, 0.4, 0.5]

  还没调用 updateFramePlacements 时, data.oMf[4] 的平移 = [0 0 0]   <-- 静默错误，程序不会提醒你
  调用 updateFramePlacements 之后:
  末端执行器 end_effector 的世界位姿 (data.oMf[4]):
    [0, 0.7, 0.5]
  验算：末端相对关节3 的偏移 [0, 0.3, 0]，关节3 自己的位姿 [0, 0.4, 0.5]，相加 = [0, 0.7, 0.5]
        与上面 oMf 的数字一致（中立姿态没有旋转，平移可以直接相加）。

模型创建成功！
接下来请运行教程 02-2 学习关节和坐标系的详细操作。

---

3.12 逐段读懂这份输出

【说明】那一行：为什么没有「固定关节」

程序开头打印「固定基座机器人直接从 universe 关节(索引0)开始」。很多人第一次建模型会去找 JointModelFixed 想加一个「底座关节」——不需要。固定基座 = 第一个真实关节的父索引填 0。底座本身不是关节，它只是世界。

（浮动基座是另一回事：buildModels::humanoid 之类用自由飞行关节当根，第 4 章和第 5 章会看到 njoints、nq 怎么跟着变。）

【模型验证】：五个尺寸 + 两张清单

  关节总数 (njoints): 4
  连杆总数 (nbodies): 1
  自由度数 (nq): 3
  速度维数 (nv): 3
  雅可比维数 (nvExtended): 3
  坐标系总数 (nframes): 5

3 个真实关节 → njoints=4（+1 是 universe）；nq=nv=3（转动关节一对一）；nframes=5 = 4 个关节帧（含 universe 帧）+ 1 个 end_effector。自己数一遍再对上这几个数，比记住任何一条规则都有用：njoints 是关节数+1，nframes 是「你显式加过的帧」的总数。nbodies=1 的原因见 3.3 节那段（本章没挂任何惯性体）。

关节清单里那行 [0] universe (类型 JointModelRX, nq=0, nv=0; joints[i] 自报 nq=1) 是 3.3 节说的占位对象：model.nqs[0] 给 0，model.joints[0].nq() 给 1。信前者。

布局表这一行 idx_qs.size() = 4, nqs.size() = 4 顺手回答了一个问题：这两个数组和关节索引一一对应，没有额外的哨兵项。区间写法 [起点, 起点+长度)：universe 是 [0, 0)（零长度，不占格子），关节 1 占 q[0]、关节 2 占 q[1]、关节 3 占 q[2]。

坐标系清单里三列要连着看：end_effector 的 父关节=3（跟着关节 3 动）、父坐标系=3（位姿从关节 3 的帧量起）。四个关节帧的索引和关节索引同号（addJointFrame 紧跟 addJoint 长出来的），但这是本章代码结构造成的巧合，不是 Pinocchio 的保证——练习 5 让你亲手验证。

【中立姿态 q】：q = [0, 0, 0]

neutral(model) 返回「最自然的姿态」：转动关节给 0，四元数给单位四元数 (0,0,0,1)。对全旋转关节的模型，它就是一串 0。

注意输出用的是 q_neutral[i]，默认格式化，所以是 0 而不是 0.0000——Eigen 只有在流里被 std::fixed/setprecision 影响后才补小数位。期望输出对账时，格式的差别也是差别。

【正向运动学验证】：[0, 0, 0] / [0, 0, 0.5] / [0, 0.4, 0.5]

三个关节的世界位置，正好是 3.1 节那棵树在中立姿态（无旋转）下乘出来的结果：关节 2 = 沿 Z 上移 0.5；关节 3 = 关节 2 再沿 Y 前移 0.4。

把 q 改成 [π/2, 0, 0] 重跑一次（练习 2），你会看到关节 2 的 x/y 变了、z 不变——这正是「jointPlacement 相对父帧」的直观证据。

最后那组对照：[0, 0, 0] → [0, 0.7, 0.5]

这就是 3.7 节的实验。输出里「还没调用 updateFramePlacements 时 … [0, 0, 0]」和「之后 … [0, 0.7, 0.5]」挨在一起，中间只隔一行函数调用。0.7 = 0.4 + 0.3（关节 3 的 Y 坐标 + 末端相对它的 Y 偏移），手算能对上；程序末尾那行验算就是这个意思。

---

3.13 本章踩过的坑（按症状查）

坑 1：njoints 比预期多 1，或者关节名从 names[1] 开始

universe 占索引 0。不是 bug，是约定。要遍历真实关节，写 for (JointIndex i = 1; i < model.njoints; ++i)（本章程序就是这么写的）；要按 q 的布局遍历，用 idx_qs/nqs。

坑 2：model.joints[0].nq() 返回 1，模型明明只有 0 自由度

占位关节的默认构造类型是 JointModelRX（1 自由度）。问尺寸问 model.nqs[i] / model.nvs[i]，别问 joints[i]。这条在第 5 章解剖示例模型时还会再撞一次。

坑 3：data.oMf 全是 [0, 0, 0]，程序一点意见都没有

漏了 updateFramePlacements(model, data)。见 3.7。修完之后如果 oMi 也是 0，那是更前面漏了 forwardKinematics。

坑 4：getFrameId("joint_1_base_rotation") 找不到帧 / 返回一个很大的数

addJoint 之后没调 addJointFrame。返回值是 model.nframes（越界哨兵），不是 -1、不是异常。第 4 章专门讲这个函数怎么安全地用。

坑 5：末端位置怎么算都是同一个错值，差一个固定平移

Frame 用了四参数版本，parentFrame 被默默填成 0（universe）。改成五参数版本，把位姿挂到你真正想挂的那个帧上。打印 frame.parentFrame 一秒定位。

坑 6：改完模型，Data 的数组还是老尺寸

Data 是 Model 尺寸的快照。加完关节/帧重新构造 Data。症状通常是越界崩溃，或者结果对不上但尺寸看着「小了一截」。

坑 7：q += v * dt 编译过了但姿态坏了

转动关节上它看着没事（nq==nv，四则运算恰好成立）；一旦模型里有自由飞行关节，q 的长度是 nq、v 是 nv，+= 直接把 Eigen 断言打出来说「尺寸不匹配」。一律 q = integrate(model, q, v*dt)（第 11 章全程这么写）。

坑 8：std::cout << frame.type 打出来是 51

FrameType 是位标志。要么用本章的 frameTypeName()，要么按位测 frame.type & OP_FRAME。别把它当普通 enum 去 == 一个组合值。

坑 9：void f(Model m) ——按值传模型

第 01 章实测过：Model 没有 pimpl，拷贝是深拷贝一打 std::vector。控制循环里一次都不该出现。签名写 const Model& model, Data& data。

---

3.14 练习（都能用本章程序改出来）

1. 在关节 3 之后再加一个绕 Y 的关节 4（jointPlacement 沿 Y 移 0.2 m）并补 addJointFrame，然后加一个新的 OP_FRAME 末端。预测 njoints/nq/nv/nframes 各变成多少，再跑一遍对答案。
2. 把 q_neutral 换成 Eigen::VectorXd q(3); q << PI<double>()/2, 0, 0;，跑一次。核对关节 2 的世界位置：它应该从 [0,0,0.5] 变成 [?, ?, ?]，用「绕 Z 转 90° 把 +Y 方向变成 -x 方向」手算一遍。
3. 故意把 Frame 的五参数版本改成四参数版本（去掉 joint3_frame_idx），看输出里 父坐标系= 变成几、末端位置变成多少。这个实验值得亲手做，因为它不报错。
4. 故意注释掉 updateFramePlacements(model, data); 那一行，确认程序照常跑完、退出码照常是 0，只有末端位置退化成 [0, 0, 0]。体会一下「静默错误」在真机上意味着什么。
5. 把 model.names[i] 换成 model.frames[i].name，把两张清单合并成一张按索引排的表，检查你的机器人里「关节 i」和「坐标系 i」是否同号（本章是同号的：addJointFrame 紧跟 addJoint，索引一一长出来）。第 4 章的例子会打破这个巧合。

---

本章要点

• Model = 结构 + 参数（基本不变），Data = 算法的输入输出缓存（每次调用都被改写）。Data data(model) 一次性开好内存，这就是 Pinocchio 能做到微秒级动力学的原因。结果要在「紧接那次调用之后」读。
• 索引 0 永远是 universe：njoints、nbodies 都含它，q/v 不含它。真实关节从 1 数起，model.names[i+1] 才是第 i 个关节的名字。
• joints[0] 是默认构造的占位对象，joints[0].nq() 会谎报 1；尺寸一律问 nqs/nvs/idx_qs/idx_vs（实测）。
• q 是按关节分段的，nq 与 nv 可以不相等（自由飞行关节 7 vs 6）。别写 q += v*dt，用 integrate。
• 4.x 建关节是四步：addJoint(父索引, 关节模型, 相对父帧的 SE3, 名字) → addJointFrame(关节索引)。第三步忘了，后面 getFrameId 静默失败。
• Frame 用五参数构造：parentJoint 决定跟谁动，parentFrame 决定位姿从哪量。四参数版本把 parentFrame 塞成 0，不报错但结果错。
• forwardKinematics 只填 oMi；读 oMf 之前必须 updateFramePlacements（本章实测 [0,0,0] → [0,0.7,0.5]）。