第 6 章 从 URDF 读进一个真实机器人：`＜limit＞` 活着，基座的质量「不在账上」
本章解决一个问题：前三章的模型全是你自己在 C++ 里
addJoint拼出来的，或者用buildModels::manipulator一行调用出来的。真实项目不是这样——真实项目给你的是一个.urdf文件。本章把一份 4 连杆 3 关节的 URDF 读进 Pinocchio，然后逐项对账：XML 里写的每一个数字，最后落在Model的哪个字段。对账的收获比想象大：你会发现<origin>根本不在frame.placement里，<limit>一个都没丢，而 URDF 里明明写着 18.5 kg，computeTotalMass却告诉你 8.5 kg——基座被当成世界的一部分，不是机器人的一部分。先修：第 03 章（
Model/Data分工、njoints那个 +1、idx_q/idx_v）、第 04 章（关节类型表、FrameType位标志、getFrameId找不到时的静默失败）、第 05 章（Inertia的三个访问器、手算复核的习惯）。
本章新增的 C++ 知识：std::ifstream探文件是否存在；std::string的find_last_of/rfind截路径；用raw string literal承载 XML（练习 5）。
产出：models/simple_robot.urdf+tutorials/05_urdf/01_load_urdf.cpp。这份 URDF 后面第 07～09 章、第 11～14 章都还在用，别删。
6.1 这份 URDF 长什么样
四个连杆、三个转动关节，一根竖直的三自由度臂：
base_link (10 kg, 方块 0.2×0.2×0.1)
│ joint1_base_rotation revolute, axis=(0,0,1), origin xyz=(0,0,0.05)
link1 (5 kg, 圆柱 r=0.05 l=0.5, 质心在 z=0.25)
│ joint2_shoulder revolute, axis=(0,1,0), origin xyz=(0,0,0.5)
link2 (3 kg, 圆柱 r=0.04 l=0.4, 质心在 z=0.2)
│ joint3_elbow revolute, axis=(0,1,0), origin xyz=(0,0,0.4)
end_effector (0.5 kg, 球 r=0.03, 质心在 z=0.05)
完整文件在 6.11 节，自己存成 models/simple_robot.urdf。它是给教学用的：结构是真的（串联臂、Z 转 + Y 转 + Y 转），惯性参数是随手写的（6.7 节会把手算结果和文件里的数字并排摆出来，四个连杆没一个对得上）。
URDF 的标签去哪了：一张总表
| URDF 里写的 | 落到 Model/Data 的哪里 | 本章实测的位置 |
|---|---|---|
<robot name> | model.name | 【2】节打印 simple_3dof_arm |
<link name> | 一个 BODY 帧；若有 <inertial>，还贡献一份惯性 | 【3b】【4】 |
<joint name, type> | 一个关节 + 一个 JOINT 帧；model.names[i] | 【3】 |
<joint><origin xyz rpy> | model.jointPlacements[i] | 【3c】，不在 frames[i].placement |
<joint><axis> | 决定关节的 C++ 类型（RX/RY/RZ 或 Unaligned） | 【3】+ 6.4 节 |
<joint><limit lower upper> | lowerPositionLimit / upperPositionLimit | 【3c】 |
<limit velocity effort> | upperVelocityLimit / upperEffortLimit | 【3c】 |
<link><inertial><mass> | model.inertias[j].mass()，j 见 6.7 节 | 【4】 |
<inertial><origin> | model.inertias[j].lever() | 【4】 |
<inertial><inertia ixx...> | model.inertias[j].inertia().matrix() | — |
没写的 <dynamics>/摩擦/转子 | damping / friction / rotorInertia 全 0 | 【3c】 |
这张表就是本章的全部主线：程序【3b】【3c】【4】三节存在的意义，是把这 11 行逐行打印出来给你看。网上讲 URDF 加载的教程几乎都停在「buildModel 成功，nq 是几」，而真正会咬人的是下面这些细节。
6.2 维度对账：njoints=4 但只有 3 个关节
加载完打印：
名称: simple_3dof_arm
关节数: 4
自由度: 3
速度维数: 3
坐标系数: 8
第 03 章那条规则在 URDF 模型上一字不差：njoints = 真关节数 + 1，多的那个是索引 0 的 universe。所以【3】节的循环从 JointIndex i = 1 开始，只打印出 3 行。
nq = nv = 3：三个关节各 1 自由度，且都是转动关节，坐标和速度同维。记住这个等式，它在 6.7 节会被拿来当对照——本节的 URDF 是「nq==nv 的幸运情况」，浮动基座的人形就不是（第 05 章 5.3 节）。
nframes = 8 = 4 个 BODY 帧 + 3 个 JOINT 帧 + 1 个 universe（FIXED_JOINT）帧，【3b】节的位标志统计会当场把这 8 个分完。
6.3 <origin> 的去向：为什么帧列表里的平移全是 0
先看在【3b】节打印的帧表（8 行，placement 平移 那一列全是 [0 0 0]）：
Frame 0: universe 父关节=0, placement 平移=[0 0 0], 类型位=4
Frame 1: base_link 父关节=0, placement 平移=[0 0 0], 类型位=8
Frame 2: joint1_base_rotation 父关节=1, placement 平移=[0 0 0], 类型位=2
Frame 3: link1 父关节=1, placement 平移=[0 0 0], 类型位=8
...
如果你的第一反应是「URDF 里明明写了 xyz="0 0 0.5"，怎么读进来全变 0 了，是不是解析失败」——这个反应本身就是本章要拆掉的坑。
两点解释：
Frame::placement是「相对自己父关节的关节帧」的位姿，不是世界坐标。对串联臂来说，link 的 BODY 帧就贴在它自己那个关节的原点上（因为 URDF 的<link>没有额外的帧偏移要表达），所以相对平移是 0。第 04 章【2】节讲过这个作用域，这里是它在 URDF 上的具体表现。- URDF 的
<origin>存在另一个容器里：model.jointPlacements[i]。【3c】节把它打印出来，三个数字一个不少：
jointPlacements[1] joint1_base_rotation 平移=[0 0 0.05]
jointPlacements[2] joint2_shoulder 平移=[0 0 0.5]
jointPlacements[3] joint3_elbow 平移=[0 0 0.4]
顺带把 0.05、0.5、0.4 加起来 = 0.95，这就是零位时末端的高度（6.6 节会用到）。
源码上的对应关系一眼能看懂：解析器给每个关节建关节帧时，把父帧位姿和 URDF 的 <origin> 复合起来交给 model.addJoint(...)，位置参数就是 frame.placement * placement（env/Library/include/pinocchio/src/parsers/urdf/model.hxx:604 起的那几个 return model.addJoint(frame.parentJoint, TypeX/TypeY/TypeZ(), frame.placement * placement, ...)）。而 BODY 帧是用 addBodyFrame(...) 建的（src/multibody/model.hxx:1502），传的是刚建好的关节帧 id，所以相对它平移为 0。
规矩：想知道某个关节在父坐标系里偏了多少，读 model.jointPlacements[i]；想知道某个帧在自己关节帧里偏了多少，读 model.frames[f].placement。两个都不是世界坐标——世界坐标只有算过运动学之后才存在（data.oMi、data.oMf）。
6.4 <axis> 决定的是 C++ 类型，不是运行期参数
【3】节每个关节印两行名字：
关节 1: 名称=joint1_base_rotation, 类型=JointModelRZ (完整类型名: struct pinocchio::JointModelRevoluteTpl<double,0,2>), nq=1, nv=1
关节 2: 名称=joint2_shoulder, 类型=JointModelRY (完整类型名: struct pinocchio::JointModelRevoluteTpl<double,0,1>), nq=1, nv=1
关节 3: 名称=joint3_elbow, 类型=JointModelRY (完整类型名: struct pinocchio::JointModelRevoluteTpl<double,0,1>), nq=1, nv=1
URDF 的 <axis xyz="0 0 1"> 没有变成一个 Vector3 存在模型里，而是在解析那一刻就选定了模板实参：RevoluteTpl<double,0,2>，最后的 2 就是 Z 轴；1 是 Y。这跟第 05 章 5.1 节看到的完全同一族类型。
分派发生在 extractCartesianAxis（src/parsers/urdf/model.hxx:604）：把 <axis> 归到 AXIS_X / AXIS_Y / AXIS_Z / AXIS_UNALIGNED 四档，前两档命中就用 JointModelRX/RY/RZ，否则退化成 JointModelRevoluteUnaligned（多一个 axis 字段，Lie group 也不同）。你写 axis="0.707 0.707 0" 的那一刻，选的是另一个类。
为什么值得在意：轴向关节有解析公式，unaligned 走通用路径，两者在 nq/nv 上一样的（都 1/1），但类型名不一样、type().name() 不一样，一些按类型分派的算法（比如某些 Jacobian 特化路径）会走不同分支。所以「类型名」不是装饰，是诊断信息——本章程序把它打出来就是这个原因。
另外 URDF 的 type 属性（revolute / continuous / prismatic / floating / planar / spherical / fixed）也各自映射到不同类：case REVOLUTE 在 src/parsers/urdf/model.hxx:315，case CONTINUOUS 在 325 行（用 JointModelRUB*，unbounded），FLOATING 用 JointModelFreeFlyer（第 05 章那位 nq=7/nv=6 的主角）。练习 3 让你亲手改一个看差别。
6.5 <limit> 一个都没丢，没写的都是 0
【3c】节后半段：
位置限位 lowerPositionLimit = [-3.14 -2.5 -2 ]
位置限位 upperPositionLimit = [3.14 2.5 2 ]
速度限位 upperVelocityLimit = [2 1.5 1 ] rad/s
力矩限位 upperEffortLimit = [100 80 50 ] N*m
damping = [0 0 0]
friction = [0 0 0]
rotorInertia = [0 0 0]
对一下 URDF：joint1 lower=-3.14 upper=3.14 effort=100 velocity=2.0，joint2 -2.5/2.5/80/1.5，joint3 -2/2/50/1.0。三列数字整整齐齐排成三个 VectorXd，顺序就是关节 1/2/3。
两个要记的点：
- 字段名在 4.1 里换了。这六个向量的现名是
lowerEffortLimit/upperEffortLimit、lowerVelocityLimit/upperVelocityLimit（src/multibody/model.hxx:181-197）；老的effortLimit、velocityLimit还在，但被标了PINOCCHIO_DEPRECATED（181-198 行那两段注释原话是 “Deprecated in favor of lowerEffortLimit and upperEffortLimit”）。网上 3.x 的model.effortLimit在你机器上编译会出弃用警告——能过，但别学。 - 这些向量的长度是
nv，不是njoints。本章三个关节各 1 自由度，所以第 0/1/2 格正好对上关节 1/2/3；一旦有浮动基座或 mimic，这个巧合就没了，要按第 03 章的jointVelocitySelector取段。别拿下标硬猜。
<limit> 之外的东西（<dynamics damping friction>、<safety_controller>）这份 URDF 没写，于是 damping/friction/rotorInertia 全 0。0 不是「没读到」的标记，而是「读到空、按零处理」。第 14 章做控制器仿真时，这三个 0 会直接决定你的电机模型有多天真——到时候回来改这份 URDF 就行。
6.6 末端位姿：手算复核，一位小数都不差
【4】节给 q = [π/4, −π/3, π/6]，程序输出：
位置: [-0.244949 -0.244949 0.75 ]
旋转矩阵:
[0.6124 -0.7071 -0.3536]
[0.6124 0.7071 -0.3536]
[0.5000 0.0000 0.8660]
这组数字可以完全用手算复现，这是本书反复要求的好习惯（第 02 章起就一直在做）。
先建三个 <origin> 叠出来的链：joint1 在世界 (0,0,0.05)；joint2 相对 joint1 偏 (0,0,0.5)；joint3 相对 joint2 偏 (0,0,0.4)；end_effector 的 BODY 帧贴在 joint3 原点（6.3 节已经实测过它的相对平移是 0）。
- 第一段 0.5：它只被 joint1 的 Z 轴转动过。绕 Z 转不改变
z，也不改变x=y=0。所以 joint2 在(0, 0, 0.05+0.5) = (0,0,0.55)。 - 第二段 0.4：它被 joint2 的 Y 轴转了
−60°。绕 Y 转把(0,0,1)变成(sin(−60°), 0, cos(−60°)) = (−0.866, 0, 0.5)，乘 0.4 得(−0.3464, 0, 0.2)。 - joint3 的 30° 只转末端姿态：BODY 帧贴在 joint3 原点上，没有杠杆臂，所以位置不动。这一步很多人算错，就是忘了「末端帧就挂在肘关节原点」。
于是 joint3（=末端）在 joint1 的局部系里是 (−0.3464, 0, 0.55+0.2=0.75)。再套 joint1 的 Rz(45°)：x' = 0.7071·(−0.3464) − 0.7071·0 = −0.2449，y' = 0.7071·(−0.3464) + 0.7071·0 = −0.2449。
手算 = [-0.2449, -0.2449, 0.75]，和程序 [−0.244949 −0.244949 0.75] 完全一致。
姿态同理：末端帧朝向 = Rz(π/4)·Ry(−π/3)·Ry(π/6) = Rz(45°)·Ry(−30°)。
Rz(45°) = [ .7071 -.7071 0 ] Ry(-30°) = [ .866 0 -.5 ]
[ .7071 .7071 0 ] [ 0 1 0 ]
[ 0 0 1 ] [ .5 0 .866]
行向量做乘法：第一行 = 0.7071·(.866,0,−.5) + (−0.7071)·(0,1,0) = (0.6124, −0.7071, −0.3536)；第二行 = (0.6124, 0.7071, −0.3536)；第三行 = (0.5, 0, 0.866)。三行和程序打的一字不差。
注意 Ry(−30°) 的写法：[[cos,0,sin],[0,1,0],[−sin,0,cos]]，代 θ=−30° 后 (0,0,1) 方向的元素是 sin(−30°)=−0.5，(1,0) 元素是 −sin(−30°)=+0.5。转动手感和矩阵元素差一个符号是这类矩阵最常见的自欺，所以本章把两个矩阵都摊开写。
顺带一条第 03 章就立过的规矩：updateFramePlacements(model, data) 必须调，否则 data.oMf[ee_idx] 还是单位变换，你会读到 [0 0 0]。本章程序里那行 updateFramePlacements 的注释就是这句话。
6.7 质量对账：URDF 写了 18.5 kg，模型只认 8.5 kg
这是本章最值得读的一节。【4】节打印的惯性表：
inertias[0] universe mass=10.000 kg lever=[0.000 0.000 0.000]
inertias[1] joint1_base_rotation mass=5.000 kg lever=[0.000 0.000 0.250]
inertias[2] joint2_shoulder mass=3.000 kg lever=[0.000 0.000 0.200]
inertias[3] joint3_elbow mass=0.500 kg lever=[0.000 0.000 0.050]
从 i=1 求和 = 8.500 kg；从 i=0 求和 = 18.500 kg
URDF 里四个 <mass value> 是 10 / 5 / 3 / 0.5，和是 18.5。inertias[0] 那一格就是 base_link 的 10 kg——名字那列印的是「这一格属于哪个关节」（model.names[0] 是 universe），不是「这一格是虚构的」。
机制：两条完全不同的挂载路径
inertias 这个向量的下标是关节索引，而 URDF 的根连杆没有父关节可挂，于是解析器给它俩走了不同的路：
-
根连杆（base_link）：
addRootJoint在没有root_joint参数时，只加一个 BODY 帧，并且把惯性塞进这个 Frame 对象（src/parsers/urdf/model.hxx:133-134）：model.addFrame( Frame(body_name, parent_frame.parentJoint, 0, parent_frame.placement, BODY, Y));而
Model::addFrame的第二个形参默认是append_inertia = true（src/multibody/model.hxx:697），函数体里做的事是（src/multibody/model.hxx:1580-1583）：frames.push_back(frame); if (append_inertia) inertias[frame.parentJoint] += frame.placement.act(frame.inertia);base_link 的
parentJoint是 0，于是 10 kg 落在inertias[0]。 -
子连杆（link1/link2/end_effector）：走解析器的
appendBodyToJoint助手（src/parsers/urdf/model.hxx:422-433），里面调的是Model::appendBodyToJoint（src/multibody/model.hxx:1479-1485）：const Inertia & iYf = Y.se3Action(body_placement); inertias[joint_index] += iYf; nbodies++;之后它另外用
addBodyFrame建 BODY 帧——那个 Frame 对象不带惯性（addBodyFrame内部调addFrame(Frame(body_name, parentJoint, parentFrame, body_placement, BODY))，见src/multibody/model.hxx:1502，五参构造的惯性默认零），所以不会被addFrame再加一遍。这就是为什么inertias[1]是 5.000 kg 而不是 10.000 kg：每条连杆的质量只进账一次。
还有一条你会在换真实 URDF 时撞上的：解析器在挂之前有一句 if (!Y.isZero(Scalar(0)))（src/parsers/urdf/model.hxx:429）。没有 <inertial> 的连杆不会报错，只是不贡献质量——它照样生成 BODY 帧，照样能在运动学里被引用，动力学上却是零质量零惯量。
聚合口径也统统从 1 开始
computeTotalMass 的函数体（src/algorithm/center-of-mass.hxx:17-25）就五行：
Scalar m = Scalar(0);
for (JointIndex i = 1; i < (JointIndex)(model.njoints); ++i)
{
m += model.inertias[i].mass();
}
return m;
从 1 开始。同一份文件里的 computeSubtreeMasses（37-48 行）也从 1 开始，centerOfMass 用的就是这套累加。所以：
Pinocchio 口径的总质量 computeTotalMass = 8.500 kg
手算质心（含 i=0 的 10 kg 基座） = [-0.027 -0.027 0.208 ] m
手算质心（从 i=1 加，即 computeTotalMass 的口径） = [-0.059 -0.059 0.453 ] m
centerOfMass 给的 = [-0.059 -0.059 0.453 ] m
centerOfMass 与「从 i=1」那版之差 = 0.00e+00 m
结论：对固定基座的 URDF，centerOfMass 算的是「会动的那部分」的质心，基座被当成世界的一部分。这不是 bug，是自洽的口径（基座不动，它对广义重力矩的贡献落在 universe 那格，而 universe 没有速度维度）。但它有两个实际后果，你早晚要碰上：
- 拿
computeTotalMass报「整机重量」会少算基座。给甲方报参数、选轴承、算地面反力时，这是实打实的错。要整机质量就自己从 0 加。 ‖g‖之类的量也跟着小：第 10 章的重力矩里，joint1 那条链撑的是「上面所有会动的质量」，基座自己那 10 kg 由固定结构承担，不进力矩账。这符合直觉——底座螺栓承受的静载和关节电机承受的力矩本来就是两张清单。
顺手做个诚实性检查：lever 那三列 (0.250, 0.200, 0.050) 正好是 URDF 里三个 <inertial><origin xyz> 的 z，第 4 格是 0，对应 base_link 的 <origin xyz="0 0 0">。对上了，说明 <inertial><origin> → lever 这条映射也是通的。
再对一次账：这份 URDF 的惯量数字是编的
【4】节没打印惯量矩阵（第 05 章 5.4 节已经教过怎么读），这里直接手算给你看——用标准公式复核，四个连杆没有一个和文件里的数字相符：
| 连杆 | 形状/质量 | 手算的轴向惯量 | 手算的横向惯量 | 文件里写的 izz / ixx |
|---|---|---|---|---|
| link1 | 圆柱 r=0.05, l=0.5, m=5 | ½mr² = 0.00625 | m(3r²+l²)/12 = 0.1073 | 0.001 / 0.01 |
| link2 | 圆柱 r=0.04, l=0.4, m=3 | 0.0024 | 0.0412 | 0.0005 / 0.005 |
| end_effector | 球 r=0.03, m=0.5 | 2/5·mr² = 0.00018 | 同左 | 0.0001 / 0.0001 |
| base_link | 方 0.2×0.2×0.1, m=10 | m(x²+y²)/12 = 0.0667 | m(y²+z²)/12 = 0.0417 | 0.1 / 0.1 |
link1 的横向惯量差了 10.7 倍，基座反而被写大了 2.4 倍。但这些数字全都合法——对角元都是正数、也都满足三角不等式（0.01+0.001 > 0.01），Pinocchio 不会抱怨一个字。
所以：合法性检查拦不住错的惯性参数。第 05 章 5.4 节说示例模型的参数「够用来验证算法、不够用来选电机」，这条对任何你自己写的 URDF 同样成立。做控制仿真之前，先拿这张表的手算方式复核一遍自己的 <inertial>。
6.8 帧的类型位与 nbodies
【3b】节把 8 个帧的类型位打出来了：universe 是 4，三个关节帧是 2，四个连杆帧是 8。对照枚举（env/Library/include/pinocchio/src/multibody/frame.hxx:29-36）：
enum FrameType
{
OP_FRAME = 0x1 << 0, // 1 用户运行时自己加的帧
JOINT = 0x1 << 1, // 2 关节帧（关节的子帧）
FIXED_JOINT= 0x1 << 2, // 4 固定关节的帧
BODY = 0x1 << 3, // 8 连杆帧：挂碰撞/惯性/视觉属性
SENSOR = 0x1 << 4 // 16 传感器帧
};
第 04 章教的位标志统计在这里再走一遍，程序打出 JOINT=3 BODY=4 FIXED_JOINT=1 其他=0 （合计 8，应等于 nframes=8）。universe 是 FIXED_JOINT 而不是 BODY，这是很多人第一次读帧表会愣住的地方——它是根，类型上被当作「一个不可动的关节」。
model.nbodies 打出 4。这个数不是 BODY 帧的个数（那是 4，巧合相同），而是「Model 构造时 nbodies=1（src/multibody/model.hxx:270），之后每调一次 Model::appendBodyToJoint 才 nbodies++（同文件 1484 行）」。所以 4 = 1（根）+ 3（三个子连杆）。如果 base_link 的 <inertial> 被删掉，走的是 addFrame 那条路，nbodies 照样是 4，但 inertias[0].mass() 会变 0——这两个数是两套账，别混（练习 4 就是量这个）。
「巧合相同」这个词是认真的：加一个 type="fixed" 的连杆，两个数当场分家。第 07 章练习 1 会在 end_effector 后面挂一段 tool0（偏移 0 0 0.1），改完再跑这个程序，【3b】那张表会多两行（tool_mount 类型位=4、tool0 类型位=8，两者 placement 平移都是 [0 0 0.1]），而【4】节的 inertias[3] 从 mass=0.500 lever=[0 0 0.05] 变成 mass=0.700 lever=[0 0 0.070]——0.2 kg 被并进了父关节那一格，nbodies 一个都不加。
6.9 4.1 里真实存在的 URDF 入口，以及 3.x 的坑
【5】节把这份安装里能编过的写法列了一遍，来源是把 env/Library/include/pinocchio/parsers/urdf.hpp 打开看声明，而不是背网贴：
urdf::buildModel(filename, model, verbose = false, mimic = false)——后两个有默认值。mimic决定要不要把<mimic>标签解析成仿制关节；默认false，也就是说你在 URDF 里写了<mimic>，模型也不会自动绑上（对比第 05 章 5.2 节：示例模型那边是显式传true）。urdf::buildModel(filename, rootJoint, model, ...)——显式指定根关节，用来从整机上截一段链。urdf::buildModelFromXML(xml_string, model, ...)——从内存字符串加载，练习 5 用它。urdf::buildGeom(model, filename, type, geomModel, package_dirs, meshLoader)——package_dirs只出现在几何这一层，也就是解析package://引用的 mesh 才需要它。buildModelsFromUrdf在这份安装里 grep 不到（整个env/Library/include/pinocchio目录）。它是 3.x 时代「一次返回 Model + 碰撞 + 视觉」的封装，网上教程出现频率极高，你照抄就报C3861 找不到标识符。同理，buildModel(filename, package_dirs, model)这个 3.x 重载这里也没有。
【6】节是 parsers/ 目录实测清单：urdf.hpp mjcf.hpp sdf.hpp srdf.hpp graph.hpp utils.hpp config.hpp。「头文件存在」不等于「能用」：parsers/sdf.hpp 第一行就 #include <sdf/sdf.hh>，而本机 env/Library/include 下没有 sdf/ 目录（libsdformat 没装），所以 SDF 这条路在这台机器上是断的。网上「Pinocchio 支持 URDF/SDF/MJCF」的清单式回答，到了你的机器上要重新验证一遍——判据永远是自己编译一次。
6.10 工程里加上这一章
两处改动。
一、打开根 CMakeLists.txt 里这一行的注释（第 00 章给的是注释状态）：
# add_subdirectory(tutorials/05_urdf) -> add_subdirectory(tutorials/05_urdf)
二、新建 tutorials/05_urdf/CMakeLists.txt，整份内容：
# 教程 05：加载 URDF 模型（需要 models/simple_robot.urdf）
add_tutorial(01_load_urdf)
三、新建 models/simple_robot.urdf（6.11 节的完整文件）。注意：CMake 只把 MODEL_DIR 定义成「源码目录下的 models/」，并不会把文件拷进 build/。所以 URDF 必须放在项目根的 models/ 里，和 CMakeLists.txt 同级；放错位置的话程序【1】节就会打印它自己那句 ❌ 错误：无法打开 URDF 文件！。
改完 CMakeLists 要重新配置一次（子目录列表变了，CMake 要生成新的 vcxproj）。
6.11 URDF 文件
存成 models/simple_robot.urdf（注意：是项目根的 models/，不是 build/models/）：
<?xml version="1.0"?>
<!--
============================================================================
简单 3 自由度机械臂 URDF 模型
============================================================================
这是一个用于 Pinocchio 学习的简化机器人模型
结构：基座 -> 关节1(绕Z) -> 连杆1 -> 关节2(绕Y) -> 连杆2 -> 关节3(绕Y) -> 末端
URDF (Unified Robot Description Format) 是机器人描述的标准 XML 格式
============================================================================
-->
<robot name="simple_3dof_arm">
<!-- ==================== 基座 (Base) ==================== -->
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.2 0.2 0.1"/>
</geometry>
<material name="grey">
<color rgba="0.5 0.5 0.5 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.2 0.2 0.1"/>
</geometry>
</collision>
<!-- 惯性参数：质量 10kg，近似为立方体 -->
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="10.0"/>
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
</inertial>
</link>
<!-- ==================== 连杆 1 (Link 1) ==================== -->
<link name="link1">
<visual>
<origin xyz="0 0 0.25" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.05" length="0.5"/>
</geometry>
<material name="blue">
<color rgba="0.0 0.0 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.25" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.05" length="0.5"/>
</geometry>
</collision>
<!-- 质量 5kg，质心在中心 -->
<inertial>
<origin xyz="0 0 0.25" rpy="0 0 0"/>
<mass value="5.0"/>
<inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.001"/>
</inertial>
</link>
<!-- ==================== 连杆 2 (Link 2) ==================== -->
<link name="link2">
<visual>
<origin xyz="0 0 0.2" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.04" length="0.4"/>
</geometry>
<material name="green">
<color rgba="0.0 1.0 0.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.2" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.04" length="0.4"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0.2" rpy="0 0 0"/>
<mass value="3.0"/>
<inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.0005"/>
</inertial>
</link>
<!-- ==================== 末端执行器 (End Effector) ==================== -->
<link name="end_effector">
<visual>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<geometry>
<sphere radius="0.03"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0.05" rpy="0 0 0"/>
<mass value="0.5"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
</link>
<!-- ==================== 关节 1: 基座旋转 (绕 Z 轴) ==================== -->
<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"/>
<limit lower="-3.14" upper="3.14" effort="100" velocity="2.0"/>
</joint>
<!-- ==================== 关节 2: 肩关节 (绕 Y 轴) ==================== -->
<joint name="joint2_shoulder" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-2.5" upper="2.5" effort="80" velocity="1.5"/>
</joint>
<!-- ==================== 关节 3: 肘关节 (绕 Y 轴) ==================== -->
<joint name="joint3_elbow" type="revolute">
<parent link="link2"/>
<child link="end_effector"/>
<origin xyz="0 0 0.4" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit lower="-2.0" upper="2.0" effort="50" velocity="1.0"/>
</joint>
</robot>
6.12 完整代码
存成 tutorials/05_urdf/01_load_urdf.cpp。整块复制即可编译：
// ============================================================================
// 教程 05-1: 从 URDF 文件读进一个真实机器人
// ============================================================================
//
// 【学习目标】
// 1. 用 pinocchio::urdf::buildModel 把 models/simple_robot.urdf 读成 Model
// 2. 看清 URDF 的 link/joint 变成 Pinocchio 的 Frame/Joint 的对应关系
// 3. 量出从 URDF 读到的惯性参数（<inertial> 直接进 model.inertias）并和质心对账
// 4. 学会「这台机器上到底装了哪些解析器」的判据：看安装目录，不背清单
//
// 【C++ 知识点】
// - std::ifstream 检查文件是否存在
// - try/catch 包住解析调用，打印 e.what()
// - 预处理宏 MODEL_DIR（由 CMake 传进来的绝对路径）
//
// 【Pinocchio 4.1 知识点】
// - urdf::buildModel(filename, model, verbose = false, mimic = false)
// - urdf::buildModelFromXML / urdf::buildGeom（package_dirs 只在 buildGeom 这一层）
// - buildModelsFromUrdf 在 4.1.0 这份安装里不存在
// - centerOfMass(model, data, q) 在 algorithm/center-of-mass.hpp
// ============================================================================
#include <iostream>
#include <iomanip>
#include <string>
#include <fstream>
#include "pinocchio/multibody.hpp"
#include "pinocchio/parsers/urdf.hpp"
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/frames.hpp" // updateFramePlacements
#include "pinocchio/algorithm/center-of-mass.hpp" // centerOfMass
using namespace pinocchio;
using namespace Eigen;
int main() {
std::cout << "===== 教程 05-1: 从 URDF 读进一个真实机器人 =====" << std::endl;
std::cout << std::endl;
// ========================================================================
// 1. URDF 格式简介
// ========================================================================
//
// URDF (Unified Robot Description Format) 是描述机器人结构的标准格式
// 它使用 XML 语法，包含：
// - link：连杆（刚体）
// - joint：关节（连接连杆的运动副）
// - robot：机器人根元素
//
// 基本结构：
// <robot name="...">
// <link name="..."> ... </link>
// <joint name="..." type="..."> ... </joint>
// </robot>
// ========================================================================
std::cout << "【1. URDF 文件位置】" << std::endl;
// MODEL_DIR 在 CMakeLists.txt 中定义（见第 06 章 6.2 节）
std::string urdf_path = std::string(MODEL_DIR) + "/simple_robot.urdf";
// 期望输出要和书上的基线逐字节对账，而 MODEL_DIR 里有每个人的盘符，
// 所以这里只印「从 models 那一层开始」的末段，不印整条绝对路径。
const size_t sep = urdf_path.find_last_of("/\\");
const size_t tail = urdf_path.rfind("models", sep);
std::cout << " URDF 路径末段: "
<< (tail == std::string::npos ? urdf_path.substr(sep + 1)
: urdf_path.substr(tail))
<< std::endl;
std::cout << " （完整路径是 MODEL_DIR 宏 + 文件名；换台机器只是前面的盘符不同）" << std::endl;
// 检查文件是否存在
std::ifstream file(urdf_path);
if (!file.is_open()) {
std::cerr << " ❌ 错误：无法打开 URDF 文件！" << std::endl;
std::cerr << " 请确保 " << urdf_path << " 存在" << std::endl;
return 1;
}
std::cout << " ✓ 文件存在" << std::endl;
std::cout << std::endl;
// ========================================================================
// 2. 基本 URDF 加载
// ========================================================================
std::cout << "【2. 基本 URDF 加载】" << std::endl;
try {
// 方式1：构建模型
// Pinocchio 4.x API: buildModel(filename, model) 将结果写入 model
Model model;
pinocchio::urdf::buildModel(urdf_path, model);
std::cout << " ✓ URDF 加载成功！" << std::endl;
std::cout << " 模型信息：" << std::endl;
std::cout << " 名称: " << model.name << std::endl;
std::cout << " 关节数: " << model.njoints << std::endl;
std::cout << " 自由度: " << model.nq << std::endl;
std::cout << " 速度维数: " << model.nv << std::endl;
std::cout << " 坐标系数: " << model.nframes << std::endl;
std::cout << std::endl;
// ========================================================================
// 3. 查看 URDF 模型结构
// ========================================================================
std::cout << "【3. 模型关节结构】" << std::endl;
for (JointIndex i = 1; i < model.njoints; ++i) {
const auto& joint = model.joints[i];
std::cout << " 关节 " << i << ": 名称=" << model.names[i]
<< ", 类型=" << joint.shortname()
<< " (完整类型名: " << joint.type().name() << ")"
<< ", nq=" << joint.nq() << ", nv=" << joint.nv() << std::endl;
}
std::cout << std::endl;
// 查看 Frame
std::cout << "【3b. 坐标系结构】" << std::endl;
std::cout << " URDF 的每个 link 生成一个 BODY 帧，每个 joint 生成一个 JOINT/FIXED_JOINT 帧。" << std::endl;
std::cout << " 类型位是位标志（env/Library/include/pinocchio/src/multibody/frame.hxx）:" << std::endl;
std::cout << " OP_FRAME=1 JOINT=2 FIXED_JOINT=4 BODY=8 SENSOR=16" << std::endl;
for (FrameIndex i = 0; i < model.nframes; ++i) {
const auto& frame = model.frames[i];
std::cout << " Frame " << i << ": " << std::left << std::setw(24) << frame.name
<< " 父关节=" << frame.parentJoint
<< ", placement 平移=[" << frame.placement.translation().transpose() << "]"
<< ", 类型位=" << frame.type << std::endl;
}
// 第 04 章教的位标志统计，这里再走一遍：URDF 模型的类型分布和示例模型不一样
int n_joint = 0, n_fixed = 0, n_body = 0, n_other = 0;
for (FrameIndex i = 0; i < model.nframes; ++i) {
const int t = model.frames[i].type;
if (t & JOINT) ++n_joint;
else if (t & FIXED_JOINT) ++n_fixed;
else if (t & BODY) ++n_body;
else ++n_other;
}
std::cout << " 按类型统计：JOINT=" << n_joint << " BODY=" << n_body
<< " FIXED_JOINT=" << n_fixed << " 其他=" << n_other
<< " （合计 " << (n_joint + n_body + n_fixed + n_other)
<< "，应等于 nframes=" << model.nframes << "）" << std::endl;
std::cout << " model.nbodies = " << model.nbodies
<< "（Model 构造时 nbodies 就是 1，此后只有 appendBodyToJoint 会加 1；"
<< "根连杆 base_link 走的是 addFrame，不参与这个计数）" << std::endl;
std::cout << std::endl;
// ========================================================================
// 3c. URDF 的 <origin> 和 <limit> 分别落到哪里
// ========================================================================
std::cout << "【3c. <origin> 与 <limit> 的落点】" << std::endl;
std::cout << " 每个关节的 jointPlacements（URDF 的 <origin> 就在这，不在 frames[i].placement）：" << std::endl;
for (JointIndex i = 1; i < model.njoints; ++i) {
std::cout << " jointPlacements[" << i << "] " << std::left << std::setw(24)
<< model.names[i]
<< "平移=[" << model.jointPlacements[i].translation().transpose() << "]"
<< std::endl;
}
// 这三个向量的长度是 nv；本章三个关节各 1 自由度，所以第 0/1/2 格正好对应关节 1/2/3
std::cout << " 位置限位 lowerPositionLimit = [" << model.lowerPositionLimit.transpose() << "]" << std::endl;
std::cout << " 位置限位 upperPositionLimit = [" << model.upperPositionLimit.transpose() << "]" << std::endl;
std::cout << " 速度限位 upperVelocityLimit = [" << model.upperVelocityLimit.transpose() << "] rad/s" << std::endl;
std::cout << " 力矩限位 upperEffortLimit = [" << model.upperEffortLimit.transpose() << "] N*m" << std::endl;
std::cout << " URDF 没写的三个参数（damping / friction / rotorInertia）：" << std::endl;
std::cout << " damping = [" << model.damping.transpose() << "]" << std::endl;
std::cout << " friction = [" << model.friction.transpose() << "]" << std::endl;
std::cout << " rotorInertia = [" << model.rotorInertia.transpose() << "]" << std::endl;
std::cout << std::endl;
// ========================================================================
// 4. 正向运动学计算
// ========================================================================
std::cout << "【4. 正向运动学验证】" << std::endl;
Data data(model);
Eigen::VectorXd q = neutral(model);
// 设置一个姿态
if (model.nq >= 2) {
q[0] = PI<double>() / 4;
if (model.nq >= 3) {
q[1] = -PI<double>() / 3;
q[2] = PI<double>() / 6;
}
}
std::cout << " q = [" << q.transpose() << "] rad" << std::endl;
forwardKinematics(model, data, q);
updateFramePlacements(model, data); // ← 少了这行，下面读到的 oMf 是 [0,0,0]
// 输出末端位姿
FrameIndex ee_idx = model.getFrameId("end_effector");
if (ee_idx < model.nframes) {
SE3 ee_pose = data.oMf[ee_idx];
Eigen::Vector3d ee_pos = ee_pose.translation();
std::cout << " 末端执行器 " << model.frames[ee_idx].name << " 位姿:" << std::endl;
std::cout << " 位置: [" << ee_pos.transpose() << "] m" << std::endl;
std::cout << " 旋转矩阵:" << std::endl << std::fixed << std::setprecision(4);
for (int r = 0; r < 3; ++r) {
std::cout << " [" << ee_pose.rotation().row(r) << "]" << std::endl;
}
} else {
std::cerr << " ❌ 找不到名为 end_effector 的坐标系！" << std::endl;
std::cerr << " 用 existFrame(\"...\") 或遍历 model.frames 确认名字。" << std::endl;
}
// 总质量与质心：URDF 里的 <inertial> 标签会直接进到 model.inertias
std::cout << " 每个关节挂的惯性（inertias 的下标和 joints 一一对应）：" << std::endl;
double total_all = 0.0, total_from_1 = 0.0;
for (JointIndex i = 0; i < model.njoints; ++i) {
const double mi = model.inertias[i].mass();
total_all += mi;
if (i >= 1) total_from_1 += mi;
std::cout << " inertias[" << i << "] " << std::left << std::setw(22)
<< model.names[i] << " mass=" << std::fixed << std::setprecision(3)
<< mi << " kg lever=[" << model.inertias[i].lever().transpose()
<< "]" << std::endl;
}
std::cout << " 从 i=1 求和 = " << total_from_1 << " kg；从 i=0 求和 = " << total_all
<< " kg" << std::endl;
std::cout << " 差的那 10 kg 是 base_link：URDF 的根连杆被挂在 inertias[0]（universe 那格）上。" << std::endl;
std::cout << " URDF 里四个 <mass> 之和 10+5+3+0.5 = 18.5，只有「连 i=0 一起加」才对得上。" << std::endl;
// 质心的两种口径，用同一段代码各算一遍，看 Pinocchio 用的是哪一种
std::cout << " 逐连杆的世界质心（oMi[i].act(lever_i)）：" << std::endl;
Eigen::Vector3d com_all = Eigen::Vector3d::Zero(), com_from1 = Eigen::Vector3d::Zero();
double m_all = 0.0, m_from1 = 0.0;
for (JointIndex i = 0; i < model.njoints; ++i) {
const double mi = model.inertias[i].mass();
const Eigen::Vector3d p = data.oMi[i].act(model.inertias[i].lever());
std::cout << " i=" << i << " m=" << std::fixed << std::setprecision(3) << mi
<< " 世界质心=[" << p.transpose() << "]" << std::endl;
if (mi <= 0.0) continue;
com_all.noalias() += mi * p;
m_all += mi;
if (i >= 1) {
com_from1.noalias() += mi * p;
m_from1 += mi;
}
}
com_all /= m_all;
com_from1 /= m_from1;
Eigen::Vector3d com = centerOfMass(model, data, q);
std::cout << " Pinocchio 口径的总质量 computeTotalMass = "
<< computeTotalMass(model) << " kg" << std::endl;
std::cout << " 手算质心（含 i=0 的 10 kg 基座） = [" << com_all.transpose() << "] m" << std::endl;
std::cout << " 手算质心（从 i=1 加，即 computeTotalMass 的口径） = ["
<< com_from1.transpose() << "] m" << std::endl;
std::cout << " centerOfMass 给的 = [" << com.transpose() << "] m" << std::endl;
std::cout << " centerOfMass 与「从 i=1」那版之差 = " << std::scientific
<< std::setprecision(2) << (com - com_from1).norm()
<< " m（fixed 三位小数时显示 0.000，这里换成科学计数法看真实量级）" << std::endl;
std::cout << std::fixed;
std::cout << " => 固定基座 URDF 的根连杆惯性落在 inertias[0]，而聚合质量从 1 开始加，" << std::endl;
std::cout << " 所以 centerOfMass 算的是「会动的那部分」的质心。基座被当成世界的一部分。" << std::endl;
std::cout << std::endl;
} catch (const std::exception& e) {
std::cerr << " ❌ 加载失败: " << e.what() << std::endl;
std::cerr << " 可能的原因：" << std::endl;
std::cerr << " 1. URDF 文件格式错误" << std::endl;
std::cerr << " 2. 依赖的 mesh 文件找不到" << std::endl;
std::cerr << " 3. URDF 中使用了不支持的特性" << std::endl;
return 1;
}
// ========================================================================
// 5. 其他 URDF 加载方式
// ========================================================================
std::cout << "【5. 4.1 里真正存在的 URDF 入口】" << std::endl;
std::cout << " (a) urdf::buildModel(filename, model, verbose = false, mimic = false)" << std::endl;
std::cout << " 后两个参数有默认值；mimic 决定是否把 <mimic> 标签解析成仿制关节，" << std::endl;
std::cout << " 默认 false —— 也就是说你的 URDF 里写了 mimic，模型也不会自动绑上。" << std::endl;
std::cout << " (b) urdf::buildModel(filename, rootJoint, model, ...)" << std::endl;
std::cout << " 指定根的关节，用来从一个完整机器人上截一段链（第 07 章会说什么时候要）" << std::endl;
std::cout << " (c) urdf::buildModelFromXML(xml_string, model, ...)" << std::endl;
std::cout << " 从内存里的 XML 字符串加载" << std::endl;
std::cout << " (d) urdf::buildGeom(model, filename, type, geomModel, package_dirs, meshLoader)" << std::endl;
std::cout << " package_dirs 只出现在几何/网格这一层：解析 package:// 引用的 mesh 才需要它。" << std::endl;
std::cout << " 4.1 的 buildModel 本身没有 package_dirs 参数——网上大量 3.x 写法" << std::endl;
std::cout << " buildModel(filename, package_dirs, model) 在这里编不过。" << std::endl;
std::cout << " 另外：buildModelsFromUrdf 在这份安装里 grep 不到（env/Library/include/pinocchio 全目录），" << std::endl;
std::cout << " 那是 3.x 的「一次返回 Model+碰撞+视觉」的封装，别照抄。" << std::endl;
std::cout << std::endl;
// ========================================================================
// 6. 这台机器上装了哪些解析器（用 ls 判，不要背清单）
// ========================================================================
std::cout << "【6. 本机 pinocchio/parsers/ 目录里有什么】" << std::endl;
std::cout << " urdf.hpp mjcf.hpp sdf.hpp srdf.hpp graph.hpp utils.hpp config.hpp" << std::endl;
std::cout << " 注意「头文件存在」不等于「能用」：" << std::endl;
std::cout << " parsers/sdf.hpp 第一行就 #include <sdf/sdf.hh>，而本机 env/Library/include 下面" << std::endl;
std::cout << " 没有 sdf/ 这个目录（libsdformat 没装），所以 SDF 这条路在这台机器上是断的。" << std::endl;
std::cout << " 判据永远是：自己编译一次。本章只用 URDF。" << std::endl;
std::cout << std::endl;
std::cout << "教程 05-1 完成！你已经会把 URDF 读进 Pinocchio，并且知道总质量该从哪个下标加起。" << std::endl;
std::cout << "接下来第 07 章开始算运动学：同一个 URDF、同一串 q，末端到底在哪。" << std::endl;
return 0;
}
6.13 编译并运行
cmake -S . -B build -G "Visual Studio 17 2022" -A x64
cmake --build build --config Release
build\bin\01_load_urdf.exe
Git-Bash 里同样两条 cmake，运行改成 ./build/bin/01_load_urdf.exe。
一定从 build/bin 里跑这个 exe，别把它单独拷到别的目录——Pinocchio 的运行时 DLL 是构建后复制到 build/bin 的，exe 一旦离开这个目录就找不到它们（6.16 节坑 1 有实测的报错原文）。
6.14 期望输出（实测快照）
Pinocchio 4.1.0 + Eigen 5.0.1 + MSVC v143 + /utf-8，Release 实测，退出码 0：
===== 教程 05-1: 从 URDF 读进一个真实机器人 =====
【1. URDF 文件位置】
URDF 路径末段: models/simple_robot.urdf
（完整路径是 MODEL_DIR 宏 + 文件名；换台机器只是前面的盘符不同）
✓ 文件存在
【2. 基本 URDF 加载】
✓ URDF 加载成功！
模型信息：
名称: simple_3dof_arm
关节数: 4
自由度: 3
速度维数: 3
坐标系数: 8
【3. 模型关节结构】
关节 1: 名称=joint1_base_rotation, 类型=JointModelRZ (完整类型名: struct pinocchio::JointModelRevoluteTpl<double,0,2>), nq=1, nv=1
关节 2: 名称=joint2_shoulder, 类型=JointModelRY (完整类型名: struct pinocchio::JointModelRevoluteTpl<double,0,1>), nq=1, nv=1
关节 3: 名称=joint3_elbow, 类型=JointModelRY (完整类型名: struct pinocchio::JointModelRevoluteTpl<double,0,1>), nq=1, nv=1
【3b. 坐标系结构】
URDF 的每个 link 生成一个 BODY 帧，每个 joint 生成一个 JOINT/FIXED_JOINT 帧。
类型位是位标志（env/Library/include/pinocchio/src/multibody/frame.hxx）:
OP_FRAME=1 JOINT=2 FIXED_JOINT=4 BODY=8 SENSOR=16
Frame 0: universe 父关节=0, placement 平移=[0 0 0], 类型位=4
Frame 1: base_link 父关节=0, placement 平移=[0 0 0], 类型位=8
Frame 2: joint1_base_rotation 父关节=1, placement 平移=[0 0 0], 类型位=2
Frame 3: link1 父关节=1, placement 平移=[0 0 0], 类型位=8
Frame 4: joint2_shoulder 父关节=2, placement 平移=[0 0 0], 类型位=2
Frame 5: link2 父关节=2, placement 平移=[0 0 0], 类型位=8
Frame 6: joint3_elbow 父关节=3, placement 平移=[0 0 0], 类型位=2
Frame 7: end_effector 父关节=3, placement 平移=[0 0 0], 类型位=8
按类型统计：JOINT=3 BODY=4 FIXED_JOINT=1 其他=0 （合计 8，应等于 nframes=8）
model.nbodies = 4（Model 构造时 nbodies 就是 1，此后只有 appendBodyToJoint 会加 1；根连杆 base_link 走的是 addFrame，不参与这个计数）
【3c. <origin> 与 <limit> 的落点】
每个关节的 jointPlacements（URDF 的 <origin> 就在这，不在 frames[i].placement）：
jointPlacements[1] joint1_base_rotation 平移=[0 0 0.05]
jointPlacements[2] joint2_shoulder 平移=[0 0 0.5]
jointPlacements[3] joint3_elbow 平移=[0 0 0.4]
位置限位 lowerPositionLimit = [-3.14 -2.5 -2 ]
位置限位 upperPositionLimit = [3.14 2.5 2 ]
速度限位 upperVelocityLimit = [2 1.5 1 ] rad/s
力矩限位 upperEffortLimit = [100 80 50 ] N*m
URDF 没写的三个参数（damping / friction / rotorInertia）：
damping = [0 0 0]
friction = [0 0 0]
rotorInertia = [0 0 0]
【4. 正向运动学验证】
q = [0.785398 -1.0472 0.523599] rad
末端执行器 end_effector 位姿:
位置: [-0.244949 -0.244949 0.75 ] m
旋转矩阵:
[0.6124 -0.7071 -0.3536]
[0.6124 0.7071 -0.3536]
[0.5000 0.0000 0.8660]
每个关节挂的惯性（inertias 的下标和 joints 一一对应）：
inertias[0] universe mass=10.000 kg lever=[0.000 0.000 0.000]
inertias[1] joint1_base_rotation mass=5.000 kg lever=[0.000 0.000 0.250]
inertias[2] joint2_shoulder mass=3.000 kg lever=[0.000 0.000 0.200]
inertias[3] joint3_elbow mass=0.500 kg lever=[0.000 0.000 0.050]
从 i=1 求和 = 8.500 kg；从 i=0 求和 = 18.500 kg
差的那 10 kg 是 base_link：URDF 的根连杆被挂在 inertias[0]（universe 那格）上。
URDF 里四个 <mass> 之和 10+5+3+0.5 = 18.5，只有「连 i=0 一起加」才对得上。
逐连杆的世界质心（oMi[i].act(lever_i)）：
i=0 m=10.000 世界质心=[0.000 0.000 0.000]
i=1 m=5.000 世界质心=[0.000 0.000 0.300]
i=2 m=3.000 世界质心=[-0.122 -0.122 0.650 ]
i=3 m=0.500 世界质心=[-0.263 -0.263 0.793 ]
Pinocchio 口径的总质量 computeTotalMass = 8.500 kg
手算质心（含 i=0 的 10 kg 基座） = [-0.027 -0.027 0.208 ] m
手算质心（从 i=1 加，即 computeTotalMass 的口径） = [-0.059 -0.059 0.453 ] m
centerOfMass 给的 = [-0.059 -0.059 0.453 ] m
centerOfMass 与「从 i=1」那版之差 = 0.00e+00 m（fixed 三位小数时显示 0.000，这里换成科学计数法看真实量级）
=> 固定基座 URDF 的根连杆惯性落在 inertias[0]，而聚合质量从 1 开始加，
所以 centerOfMass 算的是「会动的那部分」的质心。基座被当成世界的一部分。
【5. 4.1 里真正存在的 URDF 入口】
(a) urdf::buildModel(filename, model, verbose = false, mimic = false)
后两个参数有默认值；mimic 决定是否把 <mimic> 标签解析成仿制关节，
默认 false —— 也就是说你的 URDF 里写了 mimic，模型也不会自动绑上。
(b) urdf::buildModel(filename, rootJoint, model, ...)
指定根的关节，用来从一个完整机器人上截一段链（第 07 章会说什么时候要）
(c) urdf::buildModelFromXML(xml_string, model, ...)
从内存里的 XML 字符串加载
(d) urdf::buildGeom(model, filename, type, geomModel, package_dirs, meshLoader)
package_dirs 只出现在几何/网格这一层：解析 package:// 引用的 mesh 才需要它。
4.1 的 buildModel 本身没有 package_dirs 参数——网上大量 3.x 写法
buildModel(filename, package_dirs, model) 在这里编不过。
另外：buildModelsFromUrdf 在这份安装里 grep 不到（env/Library/include/pinocchio 全目录），
那是 3.x 的「一次返回 Model+碰撞+视觉」的封装，别照抄。
【6. 本机 pinocchio/parsers/ 目录里有什么】
urdf.hpp mjcf.hpp sdf.hpp srdf.hpp graph.hpp utils.hpp config.hpp
注意「头文件存在」不等于「能用」：
parsers/sdf.hpp 第一行就 #include <sdf/sdf.hh>，而本机 env/Library/include 下面
没有 sdf/ 这个目录（libsdformat 没装），所以 SDF 这条路在这台机器上是断的。
判据永远是：自己编译一次。本章只用 URDF。
教程 05-1 完成！你已经会把 URDF 读进 Pinocchio，并且知道总质量该从哪个下标加起。
接下来第 07 章开始算运动学：同一个 URDF、同一串 q，末端到底在哪。
6.15 逐段读懂这份输出
6.3～6.9 节已经把大部分段落讲完了，这里补没讲到的部分。
【1】为什么打印「路径末段」而不是完整路径
URDF 路径末段: models/simple_robot.urdf
（完整路径是 MODEL_DIR 宏 + 文件名；换台机器只是前面的盘符不同）
程序里算路径的那行是：
std::string urdf_path = std::string(MODEL_DIR) + "/simple_robot.urdf";
MODEL_DIR 是 CMake 用 target_compile_definitions 注进来的绝对路径（第 00 章根 CMakeLists.txt 里那句 MODEL_DIR="${CMAKE_SOURCE_DIR}/models"）。本章程序故意不把它的完整值打印出来：本书所有期望输出都是逐字节对比的基线，把 D:/WorkSpace/... 这种机器相关的路径印进输出，你换台机器就会看到「其余部分一模一样，只有这一行不同」。所以打印的是截出来的末段（find_last_of("/\\") + rfind("models", sep)），它在任何机器上都是 models/simple_robot.urdf。
std::ifstream file(urdf_path); if (!file.is_open()) 那两行是本章唯一主动做的失败检查：文件不存在时程序打印自己的错误并 return 1，而不是把异常留到 buildModel 里再抛。
【2】四个数字，四个含义
name / njoints / nq / nv / nframes 全部和 URDF 对得上，njoints 多 1（6.2 节）。这里补一个程序没打印但你马上想知道的：model.parents、model.idx_qs、model.nqs、model.idx_vs、model.nvs。第 03 章这些是你自己建模型时手动传的参数，现在解析器替你填了——练习 2 让你把它们打出来和 URDF 对照。
【3】shortname() 和 type().name() 的关系
同一行打印两个名字，是因为它们回答不同的问题：前者给人看（JointModelRZ），后者给类型系统和日志看（struct pinocchio::JointModelRevoluteTpl<double,0,2>）。第 04 章「三个名字只有一个是你起的」那条在这里同样成立：joint1_base_rotation 是你起的，另两个是 Pinocchio 给的。
【3b】帧表的 父关节 列有个反直觉的地方
Frame 1: base_link 父关节=0
Frame 2: joint1_base_rotation 父关节=1
Frame 3: link1 父关节=1
关节 1 的 JOINT 帧，parentJoint 是 1（它自己），不是父关节 0。因为 JOINT 帧的定义就是「贴在这个关节自己的帧上」（addJointFrame 设的）。所以读帧表时 父关节 那一列要读成「这个帧挂在哪个关节的坐标系上」，而不是「这个帧的父节点是谁」。父子关系在 model.parents 里（练习 2）。
【4】逐连杆的世界质心那四行
i=0 m=10.000 世界质心=[0.000 0.000 0.000]
i=1 m=5.000 世界质心=[0.000 0.000 0.300]
i=2 m=3.000 世界质心=[-0.122 -0.122 0.650 ]
i=3 m=0.500 世界质心=[-0.263 -0.263 0.793 ]
这四行是 data.oMi[i].act(model.inertias[i].lever())。第一行可以当场验：joint1 的帧在世界 (0,0,0.05)，只被 Rz(45°) 转过，lever=(0,0,0.25) 绕 Z 转不变，所以 (0,0,0.3)，正好是 0.05+0.25 ✓。第二、三行同理，只是要带上 Y 轴转角产生的水平分量。
然后两个「手算质心」的差别只在分母：含基座除以 18.5，不含基座除以 8.5，centerOfMass 给的和后者一样（差 0.00e+00）。那一行特意用科学计数法，是因为 std::fixed + 三位小数会把 1e-17 也印成 0.000——你看不出这是「真的 0」还是「1e-16 的数值残渣」。第 02 章讲 det() 给 e-16 残渣时说过同一件事。
【5】【6】这两节没有任何 API 调用
它们只是打印清单，但清单的内容是把这份安装的 parsers/ 目录列一遍、把 sdf.hpp 的 include 查一遍得出的（6.9 节）。写教程的人容易犯、读者更容易照单全收的错误，就是把「Pinocchio 支持 URDF/SDF/MJCF」当成自己机器上的事实。这两节的用处是让你养一个动作：换库版本或换机器，先把目录列出来再写代码。
6.16 本章踩过的坑（按症状查）
坑 1：error while loading shared libraries: boost_serialization.dll
症状：配置、编译全成功，命令行运行却立刻退出，Git-Bash 给的退出码是 127：
01_load_urdf.exe: error while loading shared libraries: boost_serialization.dll: cannot open shared object file: No such file or directory
（报错行的前缀是 exe 自己的完整路径，各机器不同。）
真相：这是运行时找不到 DLL，和编译无关——buildModel 那行都还没执行。根 CMakeLists.txt 在构建后会把 Pinocchio/boost 的 DLL 复制到 build/bin，exe 也在那里；把 exe 单独拷走（拷到桌面、拷到别的盘）就必然这样断。
解法三选一：① 就在 build/bin 里跑；② 用 run.bat（它把 conda 环境的 Library\bin 加进 PATH 再跑）；③ 自己把 env/Library/bin 加进 PATH。别去找「是不是没装 boost」——第 00 章的 conda 环境装好了，是路径问题。
坑 2：帧表里平移全是 [0 0 0]，以为 URDF 没读进去
本章最容易被误判成 bug 的地方，6.3 节整节在讲它。判据只有一条：去看 model.jointPlacements[i]，那里有 0.05 / 0.5 / 0.4。frames[i].placement 是「帧相对自己父关节帧」的位姿，串联臂的 BODY 帧就贴在关节原点，所以是 0——这不是解析失败。
坑 3：computeTotalMass 报的质量比 URDF 里的 <mass> 之和少
不是少 0.1，是少整整 10 kg（本例）。机制在 6.7 节：根连杆的惯性落在 inertias[0]，而所有聚合量从 i=1 开始加。要整机质量就自己从 0 加，别改口径去将就函数名。
坑 4：照抄 3.x 的 buildModel(filename, package_dirs, model) 或 buildModelsFromUrdf
症状：error C3861: 找不到标识符（后者），或重载不匹配的 C2664（前者）。4.1 的 buildModel 签名是 (filename, model, verbose=false, mimic=false)，package_dirs 只在 buildGeom(...) 里。判据是把 env/Library/include/pinocchio/parsers/urdf.hpp 打开数重载，一分钟出结果——别背清单。
坑 5：URDF 里写了 <mimic>，模型却毫无反应
症状：nq 等于关节数，mimic 关节照样有自己的 q 格子。真相：buildModel(..., mimic = false) 的默认值就是不解析仿制关节，必须在调用时显式传 true。这跟第 05 章 5.2 节 manipulator(model, true) 那个 true 是两回事——同一个字面含义在不同函数签名里位置不同，编译器不会提醒你。
坑 6：连杆没有 <inertial>，程序照样跑通
这条不报任何错，只在你算动力学时咬人：解析器有一句 if (!Y.isZero(Scalar(0)))（src/parsers/urdf/model.hxx:429），零惯量直接不挂。于是那一格 inertias[i].mass() 是 0，nbodies 也不加。运动学、Jacobian 全都正常，rnea/crba 给你一套「这根连杆没有质量」的错结果。从 CAD 导出的 URDF 十有八九缺 <inertial>，加载完第一件事就是像【4】节那样把每格质量打出来核对。
坑 7：把 upperPositionLimit[i] 的下标 i 当关节索引
本章碰巧对（三个关节各 1 自由度）。这些向量长度是 nv，而 nv 对关节索引的映射在有浮动基座（nv=6）或球形关节时会整体错位。永远用 jmodel.jointVelocitySelector(...) / jointConfigSelector(...) 取段（第 03 章），别靠「第 i 个关节就是第 i 格」。
坑 8：getFrameId("end-effector") 拼错不报错
第 04 章的老坑在 URDF 上更容易犯：帧名来自 XML，手打进 C++，一个下划线就错。4.1 的 getFrameId 找不到时返回 model.nframes（越界索引），所以本章程序写成：
FrameIndex ee_idx = model.getFrameId("end_effector");
if (ee_idx < model.nframes) { ... } else { std::cerr << "❌ 找不到名为 end_effector 的坐标系！" ... }
更稳的写法是先用 model.existFrame("end_effector") 探一次。
坑 9：parsers/sdf.hpp 存在，就以为能用 SDF
本机实测：env/Library/include/pinocchio/parsers/ 里有 sdf.hpp，但它第一行 #include <sdf/sdf.hh>，而 env/Library/include 下没有 sdf/（libsdformat 没装）。头文件在 ≠ 能编译能跑。判据是自己编译一次，本章只用 URDF。
6.17 练习（都能用本章程序改出来）
-
换姿态再手算一次。把
q改成[0, π/2, 0]：先在纸上算末端位置（joint2 在(0,0,0.55)，第二段 0.4 绕 Y 转 90° 后变水平），再跑程序对。然后改成[0, π/2, π/2]，看 joint3 的转动是否改变末端位置（回忆 6.6 节那句「BODY 帧贴在关节原点，没有杠杆臂」）。 -
把树结构打出来。加一段循环打印
model.parents[i]、model.idx_qs[i]、model.nqs[i]、model.idx_vs[i]、model.nvs[i]（i从 1 到njoints）。和 URDF 的<parent link>/<child link>对照，确认解析器建出来的链就是你写的那条；再确认idx_qs是0,1,2（本例每关节占 1 格）。本例nq==nv==3，所以idx_qs和idx_vs两列数字一样——这是巧合，第 05 章的人形就不是。 -
改
type看类型怎么变。把joint2_shoulder的type="revolute"改成continuous，重跑【3】节。记录：类型名变成什么、nq/nv变不变、upperPositionLimit[1]还是不是2.5。然后回答：continuous 关节的位置限位还有意义吗？ -
验证
inertias[0]和nbodies是两套账。把base_link的整个<inertial>块删掉重跑，看三个数怎么变：computeTotalMass（应该仍是 8.500）、从 i=0 求和（应从 18.500 变 8.500）、model.nbodies（应仍是 4）。跑之前先写下预测，预测错了才说明 6.7 节读进去了。 -
用
buildModelFromXML加载同一份 URDF。C++11 的 raw string 正好装 XML：const std::string xml = R"urdf( <robot name="from_string"> <link name="base_link"><inertial><mass value="10"/><inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/></inertial></link> <!-- 把剩下的 link 和三个 joint 都粘进来 --> </robot> )urdf"; Model m2; pinocchio::urdf::buildModelFromXML(xml, m2);只打印
m2.name、m2.njoints、m2.nq、m2.nframes四个数，和文件版并排比。一样，就说明「文件路径」这件事对解析器无关紧要——它只是个 XML 容器。（不必粘全，粘到 joint2 就够看出规律。）
本章要点
<origin>进model.jointPlacements[i]，不进frames[f].placement。串联臂的 BODY 帧贴在关节原点，帧表平移列全是 0 属于正常。njoints的 +1 规则在 URDF 模型上同样成立：njoints=4而<joint>只有 3 个，索引 0 是 universe，model.names[0]就是universe。<axis>选的是 C++ 类型：extractCartesianAxis（src/parsers/urdf/model.hxx:604）命中 X/Y/Z 就用JointModelRX/RY/RZ（模板最后一个参数是轴号 0/1/2），否则退化成JointModelRevoluteUnaligned。类型名是诊断信息，不是装饰。<limit>一个都没丢：落在lowerPositionLimit/upperPositionLimit/lower|upperVelocityLimit/lower|upperEffortLimit；4.1 已把effortLimit/velocityLimit标为弃用。这些向量长度是nv，取段要用 selector。- 固定基座 URDF 的根连杆惯性在
inertias[0]：走addFrame(Frame(..., BODY, Y))+append_inertia=true（src/multibody/model.hxx:1580-1583）这条路，而不是appendBodyToJoint。所以它不加nbodies，也不被任何聚合量统计到。 computeTotalMass从i=1开始加（src/algorithm/center-of-mass.hxx:17-25），computeSubtreeMasses同口径。本例 URDF 写 18.5 kg，函数给 8.5 kg；centerOfMass算的是「会动的那部分」的质心。- 子连杆的质量只进账一次：
Model::appendBodyToJoint用+=写inertias[joint]，随后addBodyFrame建的 BODY 帧惯性默认零，不会重复计入（src/multibody/model.hxx:1479-1485、1502）。 - 没有
<inertial>不报错：if (!Y.isZero(0))（src/parsers/urdf/model.hxx:429）直接跳过挂载。运动学一切正常，动力学质量全是 0。加载完第一件事是打印每格质量。 mimic默认false，URDF 里写了<mimic>也不会自动绑；package_dirs只在buildGeom那一层；buildModelsFromUrdf在这份 4.1 安装里不存在。- 帧的类型位是位标志：
OP_FRAME=1 JOINT=2 FIXED_JOINT=4 BODY=8 SENSOR=16（src/multibody/frame.hxx:29-36）；universe 的类型是 FIXED_JOINT。 - 期望输出里别印绝对路径：本书逐字节比对基线，
MODEL_DIR的完整值换机器就变，所以【1】节只印末段。
下一章（第 07 章）开始正运动学：同一份 URDF、同一串 q，把末端位姿用两条不同的路算出来（关节级 data.oMi 与帧级 data.oMf），并解释为什么少调一个 update... 函数不会报错、只会给你一个 [0 0 0]。
DAMO开发者矩阵，由阿里巴巴达摩院和中国互联网协会联合发起，致力于探讨最前沿的技术趋势与应用成果，搭建高质量的交流与分享平台，推动技术创新与产业应用链接，围绕“人工智能与新型计算”构建开放共享的开发者生态。
更多推荐
0
0- 0
已为社区贡献16条内容
所有评论(0)