运动学树与雅可比矩阵

机械臂末端在空间中的位置由链上所有关节共同决定。为了看清各关节的作用规律，我们先考察最简单的平面双连杆机构：其中第一关节转动时会带动整条后续运动链一起旋转，而第二关节转动时只改变第二杆相对于第一杆的局部夹角。将这种“父体基础运动”与“关节相对运动”的层次梳理清楚后，任意长度的串联链与带分支的树形机构都可按相同规律逐级建立。

设平面双连杆的基座固定于世界原点，机构在世界横轴与竖轴构成的 xz 平面内运动（竖轴 \boldsymbol e_z 向上）。两个转动关节的正转轴方向均取为世界负纵轴 -\boldsymbol e_y：当关节角为零时，两根连杆均竖直向下垂挂；正的关节转角使连杆向世界正横轴方向抬起。其中 q_1 为第一杆相对于竖直下方的转角，q_2 为第二杆相对于第一杆延长线的相对转角，因此两杆在世界系下的绝对转角分别为：

为便于本系列后续章节贯穿对比，两根连杆均取为均匀长方体，几何与物理参数固定如下：

每根连杆的质心位于各自杆段的中点，局部纵轴沿杆身指向负竖轴方向。由第四章式（4.6）的长方体主惯量公式，每根连杆关于自身质心的转动惯量矩阵为：

记基座铰链位于世界原点 O，两杆之间的连接铰链为 A，第二杆的末端点为 P。由平面三角几何关系，连接点 A 与末端点 P 的世界坐标分别为：

两根连杆各自的质心 C_1,C_2 分别位于对应杆段的中点：

在下方实验前，可先思考两个关节角的独立影响：若单独改变第一关节角 q_1，两根连杆会整体绕基座转动；若固定 q_1 而单独改变第二关节角 q_2，第二杆的绝对转角 q_1+q_2 会随之改变，但连接点 \boldsymbol x_A 的表达式中仅含有 q_1。

由于各点的世界坐标直接由关节角 (q_1,q_2) 通过三角函数生成，无论关节角如何变化，两段连杆的长度始终保持恒定，连接铰链也自动保持重合。当给定一条平滑的关节角时间轨迹 q(t) 时，只需在每一时刻代入同一套正运动学公式即可得到连续的机构运动。图中轨迹播放采用了如下周期为 T=8\,\mathrm s、相位为 \beta=2\pi t/T 的光滑基准轨迹（基准角记为 q_0）：

在任意时刻 t，机构的世界位姿由 q(t) 直接算出，而对该轨迹关于时间求导所得的 \dot q(t) 则直接给出后续小节计算速度所需的关节角速度。

除了用两个关节角描述双连杆外，另一种常见做法是分别独立记录两根刚体在平面内的质心坐标与绝对转角（共 6 个自由变量），再额外施加 4 个代数铰链约束方程将基座与两杆连接点锁死。这两种描述方式最终留下的独立运动自由度数完全相同，下方说明具体展开了这两套坐标表示的对应关系。

在双连杆的位置公式中，我们实际上完成了一个两步计算：先在父连杆上算出连接铰链的位置与姿态，再将子连杆相对于父连杆的局部偏置与旋转叠加进去。将这一“父子相对变换合成”写成通用的矩阵递推格式，即可直接推广到任意长度的串联机械臂以及具有多条分支的树形多体机构——树中每个刚体节点只需存储它相对于唯一父节点的局部几何关系，各刚体在世界系下的绝对位姿即可从根节点向叶节点按序累乘得出。

在运动学树中，以静止的世界系作为根节点（编号为 0），每个活动刚体 i 都通过一个关节连接到其唯一的父刚体 a=\operatorname{parent}(i)。设刚体 i 固连坐标系的原点在世界系中的位置为 \boldsymbol o_i，从刚体 i 局部系到世界系的旋转矩阵为 R_i；同时记子刚体 i 相对于父刚体 a 的局部旋转矩阵为 R_{ai}（将子系向量变为父系向量），从父系原点指向子系原点的相对位移在父系坐标轴下的分量为 \boldsymbol d_{ai}。由此可得从父刚体 a 到子刚体 i 的位姿正运动学递推公式：

在式（6.2）中，旋转矩阵通过右乘 R_{ai} 将子系方向先变到父系再变到世界系；而父子原点位移 \boldsymbol d_{ai} 由于是在父系坐标轴下度量，必须先左乘父体世界姿态 R_a 转为世界方向，才能与父原点世界位置 \boldsymbol o_a 相加。递推得到 (R_i,\boldsymbol o_i) 后，刚体 i 上任意局部坐标为 \boldsymbol p 的材料点在世界系中的位置即为：

在式（6.2）中，父子相对变换 (R_{ai},\boldsymbol d_{ai}) 由骨骼的固定安装偏置与关节变量 q_i 共同决定。若将子刚体局部坐标系的原点直接选在关节锚点处，记零位（q_i=0）时子系相对于父系的固定安装旋转为 R_0、父原点到关节锚点的固定父系位移为 \boldsymbol d_0，关节单位轴在子刚体局部系中的方向向量为 \boldsymbol n，则最常用的转动关节（hinge）与移动关节（slide）的父子相对变换分别为：

若两个刚体之间为无自由度的刚性固接（weld），则保持常数变换 R_{ai}=R_0,\ \boldsymbol d_{ai}=\boldsymbol d_0；若子刚体的局部原点并未选在关节锚点上，也只需将锚点相对于子系原点的固定偏移合并进上述变换即可。

以第 1 节的平面双连杆为例：第一杆局部系原点位于基座铰链 O，第二杆局部系原点位于连接铰链 A；两处的零位安装旋转均为单位矩阵 R_0=E，第一关节相对于世界原点的安装位移为零，第二关节相对于第一杆原点的父系安装位移则为 \boldsymbol d_0=-L_1\boldsymbol e_z。

位姿递推解决了各刚体在空间中“处于何处”的问题，而速度递推则进一步回答它们此刻“如何运动”。在树形机构中，子刚体的瞬时运动由两部分叠加而成：一部分是父刚体带动它整体牵连的基础运动，另一部分是当前关节允许子刚体相对于父刚体产生的局部相对运动。要把这两部分运动直接相加，必须先将它们统一到同一个参考点与同一组表达坐标轴之下。

第五章的式（5.2）已经给出了同一刚体速度场在更换参考点与坐标轴时的六维空间运动变换 X。只需将该变换中的旧系与新系分别取为父刚体局部系与子刚体局部系，再叠加当前关节的相对空间速度，即可建立树形速度递推。

在关节处，每个独立的广义速度分量都对应一个允许的六维空间运动方向；这些方向向量张成的线性子空间称为该关节的运动子空间（motion subspace），其基向量按列排成 6\times n_{v,i} 的运动子空间矩阵 S_i。当把参考点选在关节锚点、表达坐标轴取为子刚体局部系时，沿局部单位轴 \boldsymbol n 运动的转动关节与移动关节各自只有一列基向量：

其中 S_{\mathrm{hinge}} 乘以关节角速度 v_i 给出纯转动的相对空间速度，S_{\mathrm{slide}} 乘以关节线速度 v_i 则给出纯平移的相对空间速度；对于多自由度关节（如球关节或自由关节），矩阵 S_i 包含多列基向量，与该关节的广义速度列向量 v_i 相乘即得关节相对空间速度 S_iv_i。

设父刚体的空间速度 V_a 与子刚体的空间速度 V_i 分别以各自局部系原点为参考点、在各自局部坐标轴下表达。利用第五章式（5.2）的空间运动变换矩阵 X_i 将父刚体速度 V_a 变换到子刚体坐标系后，加上关节相对速度 S_iv_i，即得空间速度树形递推公式：

在式（6.3）中，X_iV_a 代表父刚体传来的牵连运动，S_iv_i 代表当前关节贡献的相对运动。递推求得子刚体的局部空间速度 V_i 后，刚体 i 上任意局部坐标为 \boldsymbol p 的材料点在世界系下的三维线速度即可由两点速度关系恢复：

空间加速度递推

由于父、子局部系的原点均固定在各自刚体的材料点上，第五章第 6 节已经证明：在刚体自身的局部坐标系中，空间加速度 A_i 直接等于局部空间速度分量列的时间导数 \dot V_i。对速度递推式（6.3）关于时间求导时，需要注意父子变换矩阵 X_i 本身会随关节运动而改变；当运动子空间基矩阵 S_i 在子刚体局部系下为常矩阵时，求导可得空间加速度树形递推公式：

上式右端最后一项 V_i\times(S_iv_i) 源自子刚体坐标系相对于父刚体坐标系的转动与平移。由于它是总速度 V_i 与关节相对速度 S_iv_i 的空间运动叉乘（关于速度的二次项），即使关节角加速度 \dot v_i 为零，该速度乘积项依然存在。

递推得到局部空间加速度 A_i 后，即可按第五章式（5.6）恢复刚体 i 上任意局部点 \boldsymbol p 的三维材料点实际加速度。记子系原点线速度为 \boldsymbol u=(V_i)_{\mathrm{lin}}、角速度为 \boldsymbol\omega=(V_i)_{\mathrm{ang}}、角加速度为 \boldsymbol\alpha=(A_i)_{\mathrm{ang}}（均在子系坐标轴下表达），先由 (A_i)_{\mathrm{lin}}+\boldsymbol\omega\times\boldsymbol u 恢复子系原点的实际线加速度，再叠加绕原点的切向与向心项并左乘 R_i 转到世界系：

这一公式先在子刚体局部系中完成材料点实际加速度的合成，最后一步旋转到世界坐标系；第 5 节我们还将通过对点速度雅可比矩阵求时间导数独立导出同一结果。

除了在各刚体局部系下递推外，像 MuJoCo 这样的引擎还会采用另一种等价的组织方式：将同一棵运动学树中所有刚体的空间运动量统一转换到世界坐标轴与同一个公共参考点 \boldsymbol c(t) 下（以上标 c 标记）。在公共参考系下，父子传递不再需要左乘变换矩阵 X_i，空间速度直接相加；尽管公共参考点 \boldsymbol c(t) 会随机构运动而移动，但普通时间导数的路径修正项与运动子空间导数中的路径项恰好相互抵消，使得固定局部轴关节的公共参考点空间加速度递推公式依然保持完全相同的简洁结构：

下方推导详细展示了移动公共参考点下路径速度修正项的相消过程，将局部系递推与 MuJoCo 源码中的公共系递推衔接起来。

在选定机构构型 q 后，我们还可以从“单自由度单位输入响应”的视角来组织速度计算：依次令第 j 个关节自由度以单位速率（v_j=1）运动、其余所有自由度保持静止（v_k=0,\ k\ne j），记录此时指定查询点 P 产生的瞬时速度，就得到了矩阵的第 j 列。由于任意广义速度向量 v 都是各单位基向量的线性组合，将该矩阵右乘 v 即可一次性算出所有关节同时运动时的查询点总速度，而将该矩阵转置还可在下一节直接把末端外力映射回各关节。

在给定构型 q 下，刚体上查询点 P 的世界线速度 \boldsymbol v_P 与所在刚体的世界角速度 \boldsymbol\omega 均为广义速度 v 的线性函数。将对应的线性映射矩阵分别记为点的线速度雅可比矩阵（translational Jacobian matrix）J_p(q) 与角速度雅可比矩阵（rotational Jacobian matrix）J_r(q)：

其中 J_p 与 J_r 均为 3\times n_v 矩阵，它们的第 j 列分别给出了仅第 j 个自由度取单位广义速度时查询点 P 的线速度与所在刚体的角速度；将两者上下拼接，即构成以查询点 P 为参考点的 6\times n_v 空间速度雅可比矩阵。

根据树形机构的连接关系，只有位于查询刚体祖先路径上的关节才会带动查询点运动。若祖先路径上的第 j 个自由度为转动关节，记其在世界系下的单位转轴方向为 \boldsymbol n_j、转轴上任意锚点位置为 \boldsymbol o_j，则单位角速度使查询点 P 绕该轴作瞬时定轴转动，对应的雅可比第 j 列为：

若祖先路径上的第 j 个自由度为沿世界单位轴 \boldsymbol n_j 滑动的移动关节，则单位线速度使整个后续子树沿 \boldsymbol n_j 作纯平移，对应的雅可比第 j 列为：

至于不在查询刚体祖先路径上的所有其他关节自由度，它们的运动完全不会影响查询刚体的位姿，因此对应的雅可比列恒为零向量。

回到第 1 节的平面双连杆，考察末端点 P 在 xz 运动平面内的速度分量 (v_{P,x},v_{P,z})^{\mathsf T}。提取三维线速度雅可比 J_p 的第一行（横轴）与第三行（竖轴），可得双连杆末端的 2\times2 平面雅可比矩阵 J：

在下方实验中，默认构型即为 q_1=30^\circ,\ q_2=60^\circ，此时雅可比第一列为 (\sqrt3/2,3/2)^{\mathsf T}\,\mathrm m，第二列为 (0,1)^{\mathsf T}\,\mathrm m。可先分别高亮两个关节，对比各自的力臂向量 \boldsymbol x_P-\boldsymbol o_j 与正交的单位速度列方向；随后将广义速度设为 v=(1,-0.5)^{\mathsf T}\,\mathrm{rad/s}，观察末端竖向速度如何由两列贡献 1.5-0.5=1.0\,\mathrm{m/s} 线性合成。

当机构构型固定时，雅可比的各列向量随之固定，改变广义速度 v 只是改变各列向量的线性组合权重。而在图中切换到“力映射”模式时，同一列向量与末端外力 \boldsymbol f 作内积，立刻给出了该外力在对应关节上产生的广义力矩。下一节我们将利用虚功率相等原理把这一对偶内积正式写成雅可比转置映射。

前文建立的雅可比矩阵 J 将关节空间的广义速度 v 映射到了末端笛卡尔速度。反过来，当在机构末端或某个查询点 P 处施加外力时，各个关节电机需要提供多大的广义力矩才能与之平衡？这一反向映射无需重新推导力臂几何，只需令关节空间功率与末端笛卡尔功率在任意允许速度下恒相等，即可由式（6.4）直接导出。

设在刚体的查询点 P 处作用有世界外力 \boldsymbol f，且该刚体同时受到附加力偶矩 \boldsymbol\tau。对于任意允许的广义速度 v，外力与力偶在查询点处输入的瞬时机械功率 \mathcal P 为：

由广义力 \tau_q 的功率定义 \mathcal P=v^{\mathsf T}\tau_q，并利用 v 的任意性，即得将末端外力与力偶映射回关节空间的雅可比转置力映射公式：

在式（6.5）中，转动关节对应的广义力分量具有力矩量纲（\mathrm{N\,m}），移动关节对应的分量则具有力的量纲（\mathrm N）；对于不在查询刚体祖先链上的关节自由度，由于对应的雅可比列为零，外力在这些无关分支关节上产生的广义力也自然为零。

对于祖先链上的转动关节 j（记力臂为 \boldsymbol r=\boldsymbol x_P-\boldsymbol o_j），式（6.5）的第 j 行 (\tau_q)_j=(J_p)_j^{\mathsf T}\boldsymbol f+(J_r)_j^{\mathsf T}\boldsymbol\tau 在物理上应当恰好等于“点力关于关节锚点 \boldsymbol o_j 的力矩加上附加力偶矩后沿关节轴 \boldsymbol n_j 的投影”。下方推导利用向量混合积的轮换对称性验证了这一等价性，同时也解释了为何点力 \boldsymbol f 与力偶矩 \boldsymbol\tau 必须分别由 J_p^{\mathsf T} 与 J_r^{\mathsf T} 映射。

在机构运动过程中，雅可比矩阵 J_p(q) 的各列随构型 q(t) 不断变化。对点速度关系 \boldsymbol v_P=J_pv 两端关于时间求导，即可得到查询点 P 的材料点实际线加速度：

式中第一项 J_p\dot v 由关节广义加速度 \dot v 线性产生，第二项 \dot J_pv 则源于各关节转轴方向与力臂向量在运动过程中的旋转（包含了向心加速度与科氏加速度）；即使所有关节均以恒定速率运动（\dot v=\boldsymbol0），速度二次项 \dot J_pv 依然会使查询点产生非零的实际加速度。

在平面双连杆中，2\times2 雅可比矩阵 J 的两列分别代表两个关节独立运动时所能产生的末端速度方向。只要这两列向量线性无关，它们的线性组合就能张满整个二维运动平面；而一旦两列向量在某个特定构型下变为共线（线性相关），所有关节速度组合就只能产生沿同一条直线的末端速度，垂直于该直线的方向则丧失了一阶瞬时运动能力。这种使雅可比矩阵降秩的特殊构型称为运动学奇异构型（kinematic singularity）。

计算平面双连杆雅可比矩阵 J 的行列式，可以直接找出奇异构型出现的条件：

为了直观度量机构在不同方向上的运动灵活性，可在关节速度空间中取单位圆盘 \|v\|\le1\,\mathrm{rad/s}，观察它在雅可比线性映射 J 下生成的末端速度集合——即可操作性速度椭圆（manipulability velocity ellipse）。当输入关节速度沿单位圆周旋转时，末端速度矢量的轨迹可参数化为：

随着机构逐渐接近共线奇异构型，速度椭圆的短轴会迅速缩短，意味着要在短轴方向产生有限的末端速度需要极大的关节角速度；当恰好到达共线奇异构型（q_2=0 或 \pi）时，短轴彻底坍缩为零，整个速度椭圆退化为一条一维线段。

速度椭圆的两根正交主轴方向与半轴长度可由对称半正定矩阵 K=JJ^{\mathsf T} 的特征分解求得：K 的单位特征向量 \boldsymbol u_j 给出了末端速度椭圆的主轴方向，而对应特征值的算术平方根 \sigma_j=\sqrt{\lambda_j} 则给出了沿该主轴方向的速度放大比例（即半轴长度）。由于此处两个关节同为转动关节，采用统一的角速度单位 \mathrm{rad/s}，可以直接用欧氏单位圆盘公平比较各个输入方向。

不仅如此，输出椭圆的每一根主轴方向 \boldsymbol u_j 在输入关节速度圆中也对应着一个确定的单位关节速度方向 \boldsymbol w_j：对于非零半轴 \sigma_j>0，只需取 \boldsymbol w_j=J^{\mathsf T}\boldsymbol u_j/\sigma_j，即可满足 \|\boldsymbol w_j\|=1 且 J\boldsymbol w_j=\sigma_j\boldsymbol u_j。这组放大系数 \sigma_j 即为雅可比矩阵 J 的奇异值（singular values），而 \boldsymbol u_j 与 \boldsymbol w_j 则分别是对应的左、右奇异向量。当短轴奇异值 \sigma_2\to0 时，沿右奇异向量 \boldsymbol w_2 方向的关节速度组合虽然仍在以单位速率转动关节，但在末端产生的一阶瞬时速度却趋于零。

在下方实验中，可先开启主方向显示，验证输入圆中的两条正交右奇异向量 \boldsymbol w_1,\boldsymbol w_2 如何精确对应输出椭圆的长轴与短轴（J\boldsymbol w_j=\sigma_j\boldsymbol u_j）。随后将第二关节角 q_2 逐渐拖向 0^\circ，观察椭圆短轴如何收缩至零，而改变第一关节角 q_1 则只使整个速度椭圆随连杆一起刚性旋转而不改变其主半轴长度。

可以看到，当两杆共线时，短轴对应的输入方向 \boldsymbol w_2 进入了雅可比矩阵 J 的零空间（J\boldsymbol w_2=\boldsymbol0），此时两个关节按照 \boldsymbol w_2 的比例反向协调转动只会让两杆发生局部屈伸摆动，而末端的一阶瞬时速度为零。值得注意的是，末端雅可比 J 降秩只代表末端点位置映射在某个笛卡尔方向上失去了瞬时速度能力，机构自身的两个关节自由度依然独立存在；在第七、八章引入计入全部刚体平动与转动动能的广义质量矩阵 M(q) 时，只要各连杆具有非零转动惯量，M(q) 在奇异构型下依然保持严格正定。

串联双连杆只有唯一的一条父子传递链，而人形机器人、四足机器人或带灵巧手的机械臂则是具有多条分支的树形机构。在一般运动学树中，某个关节自由度能否影响指定刚体的运动完全由树的拓扑祖先关系决定：只有位于查询刚体自身及其向根节点回溯的祖先路径上的关节，才会改变该刚体的位姿与速度。

因此，在程序中组装某个查询点 P 的雅可比矩阵时，无需遍历树上的全部关节，只需从查询点所属的刚体出发，沿着父指针 parent 一路向根节点回溯：每经过一个可动关节，就填入该关节对应的雅可比列；若经过的是刚性固接节点（weld），则不产生新列，但继续沿着其父指针向上回溯，其余所有未经过的旁支关节列直接保持为零。

在下方实验中，可先选中某一条分支上的刚体作为查询对象，尝试调节另一条旁支上的关节角：可以看到查询点的位置与雅可比矩阵完全保持不变，旁支自由度对应的雅可比列恒为零；而当调节两条分支的公共祖先关节（躯干或基座关节）时，两条分支则会同时随公共祖先一起运动。

由此可见，运动学树的祖先拓扑决定了雅可比矩阵的稀疏非零结构（哪些列可能非零），而各祖先关节当前的转轴方向与到查询点的力臂则进一步决定了这些非零列的具体数值。中间的刚性固接节点虽然不引入新的速度列，但它作为结构桥梁把下方的子孙刚体与上方的祖先关节连接在一起；在第八章计算多体逆动力学时，子树上的空间受力同样需要穿过这些固接节点一路向根部累加回传。

总结起来，运动学树上的计算天然分为两个遍历方向：递推位姿、空间速度与空间加速度时，按从根到叶（父节点先于子节点）的拓扑序向下传播；而组装查询点雅可比或汇总子树受力时，则按从叶到根（子节点回溯父节点）的逆序向上归并。

在前面的讨论中，转动关节与移动关节都只用 1 个标量坐标记录位置，并对应 1 个标量速度变量，因此位置数组与速度数组的长度相等。然而当机构中包含三维球关节（ball joint）或六自由度浮动基座（free joint）时，为了彻底消除万向节死锁，引擎通常使用 4 个分量的单位四元数来存储三维姿态，而瞬时角速度依然只有 3 个独立分量。下表列出了 MuJoCo 中四类基本关节的配置维数 n_q、速度维数 n_v 及其速度分量的物理含义：

在 MuJoCo 的数据结构中，qpos 数组（总长度为 n_q）按序保存所有关节的配置变量，qvel 数组（总长度为 n_v）则保存各独立运动方向上的广义速度。需要注意接口顺序上的一个细节：自由关节在 qvel 中的 6 个分量按“前 3 项为世界系线速度、后 3 项为体坐标系角速度”排列，而在内部转换为六维空间速度向量时，仍统一按本系列“角分量在前、线分量在后”的顺序组织。

由于 n_q 与 n_v 不再相等，广义配置 q 构成一个受单位四元数模长约束的流形，其在当前构型 q 处的局部允许微小运动构成一个 n_v 维的切空间（tangent space）。记切空间中的无穷小运动增量为 \delta\xi\in\mathbb R^{n_v}（转动分量单位为 \mathrm{rad}，平移分量单位为 \mathrm m），则配置数组 q 的一阶增量与时间导数 \dot q 可通过一个 n_q\times n_v 的配置导数映射矩阵 N(q) 统一写出：

在式（6.6）中，块对角矩阵 N(q)\in\mathbb R^{n_q\times n_v} 将 n_v 维独立广义速度 v 线性映射为 n_q 维配置数组的时间导数 \dot q。

对于单自由度转动关节、移动关节以及自由关节的三维位置分量，对应的对角块就是单位阵（即 \dot q_i=v_i）。而对于存储为单位四元数 \mathsf r=[s,\boldsymbol\rho] 的姿态块，其广义速度为局部体坐标系角速度 \boldsymbol\omega_b\in\mathbb R^3；由第三章式（3.6）的四元数右乘微分公式，可得对应的 4\times3 姿态导数映射块 N_R(\mathsf r)：

从几何上看，单位四元数模长恒为 1（\mathsf r^{\mathsf T}\mathsf r=1）要求其任意允许的一阶变化 \dot{\mathsf r} 必须与当前四元数 \mathsf r 正交（\mathsf r^{\mathsf T}\dot{\mathsf r}=0）。矩阵 N_R(\mathsf r) 的 3 列不仅严格正交于 \mathsf r，而且彼此正交、满足 N_R^{\mathsf T}N_R=\tfrac14E；因此 3 个独立的体角速度分量恰好无冗余地张成了四维单位球面在 \mathsf r 处的整个三维切空间。

在父子位姿合成中，球关节存储的四元数 \mathsf r 描述的是子刚体相对于模型零位姿态的局部旋转：若父刚体世界姿态为 R_a、父子零位安装旋转为 R_0，则子刚体的世界姿态为 R_i=R_aR_0R(\mathsf r)；而自由关节存储的四元数则直接代表该刚体在世界系下的姿态 R(\mathsf r)。由于两者的角速度变量 \boldsymbol\omega_b 均定义在子刚体自身的局部坐标系下，它们在离散时间步 h 内的有限姿态更新统一采用第三章的指数映射右乘公式（记步内转角为 \theta=h\|\boldsymbol\omega_b\|）：

当 \|\boldsymbol\omega_b\|=0 时，右侧增量直接取单位四元数 [1,\boldsymbol0]；对于自由关节，其世界位置同时按 \boldsymbol x^+=\boldsymbol x+h\boldsymbol v_W 线性更新。可以看到，尽管位置配置数组 q 含有 n_q 个浮点数，但系统的速度雅可比矩阵（3\times n_v）、广义力向量（n_v\times1）以及后续章节的广义质量矩阵（n_v\times n_v）全部定义在 n_v 维独立切空间之上。

若将刚体上某查询点的世界位置写为配置向量 q 的函数 \boldsymbol x=f(q)，则根据式（6.6）的复合函数链式求导法则，对配置分量的形式偏导矩阵 \partial f/\partial q\in\mathbb R^{3\times n_q} 右乘切空间映射矩阵 N(q)\in\mathbb R^{n_q\times n_v}，即得到作用于广义速度 v 的物理线速度雅可比 J_p=(\partial f/\partial q)N(q)\in\mathbb R^{3\times n_v}。这一链式关系将四元数的四维数组存储、三维切空间速度映射与广义力投影有机统一了起来。

在下方实验中，给定常值局部体角速度 \boldsymbol\omega_b 后，可观察刚体及其局部坐标轴在三维空间中的连续旋转，同时右侧面板实时显示单位四元数 4 个分量的协同演化；切换至“自由关节”模式后，姿态四元数仍按同一右乘公式更新，而另外 3 个位置坐标则按世界线速度匀速平移。

从图中可以直观看到，四元数的 4 个存储分量受 3 个局部角速度分量支配并始终保持在单位球面上。因此，在利用有限差分数值校验含球关节或自由关节的雅可比矩阵时，不能直接对四元数的 4 个数组分量单独加减扰动，而应当沿着 3 个独立的局部角速度方向施加正负指数映射微元旋转，再对世界点位置作中心差分。下方计算展示了这一切空间有限差分的标准做法。

以下源码分析对应 MuJoCo 3.10.0。在正运动学位姿递推函数 mj_kinematics1 中，引擎分两步计算子刚体的世界位姿：先合成模型 XML 定义的父子零位固定安装偏置（bodypos 与 bodyquat），再按序叠加该刚体上各个关节的相对位移与旋转。当父节点为活动刚体（编号为 pid）时，合成固定安装变换的核心语句为：

mji_mulMatVec3(xpos, d->xmat+9*pid, bodypos);
mji_addTo3(xpos, d->xpos+3*pid);
mji_mulQuat(xquat, d->xquat+4*pid, bodyquat);

这段代码直接对应式（6.2）：父系下的局部安装位移 bodypos 先左乘父刚体的世界旋转矩阵 xmat 转为世界方向，再加上父刚体的世界位置 xpos；姿态四元数则由父刚体的世界四元数 xquat 左乘局部安装四元数 bodyquat。在此基础上，若该刚体带有转动关节，程序继续在右侧乘入绕局部关节轴的旋转四元数；若关节锚点与子刚体原点不重合（偏心锚点），则通过 xpos = xanchor - rotated(jnt_pos) 修正子刚体原点位置，确保旋转严格绕关节锚点进行。

进入空间运动量计算阶段后，mj_comPos 将每个关节自由度的运动子空间基向量统一变换到所属运动学树的公共质心参考点并存入 cdof，随后 mj_comVel 沿树从根到叶累加各自由度的空间速度贡献。而当用户或约束模块调用 mj_jac 计算某个空间查询点的三维雅可比矩阵时，程序从查询刚体对应的最后一个自由度出发，沿着 dof_parentid 指针一路向根回溯祖先自由度，并将 cdof 从公共质心参考点换算到查询点：

mji_cross(tmp, cdof, offset);
jacp[i+0*nv] = cdof[3] + tmp[0];
jacp[i+1*nv] = cdof[4] + tmp[1];
jacp[i+2*nv] = cdof[5] + tmp[2];
i = m->dof_parentid[i];

代码中 offset 是从树公共参考点指向查询点的世界位移向量；将 cdof 的前 3 项（世界单位角速度列）与 offset 叉乘，再加上后 3 项（公共参考点处的单位线速度列），即由两点速度关系还原出该自由度在查询点处产生的线速度雅可比列，而未在回溯循环中访问到的旁支自由度列则保持初始的零值。

在广义配置积分与差分方面，mj_integratePosInd 对单自由度转动和移动关节直接执行标量加法 q_i^+=q_i+hv_i，对球关节调用 mju_quatIntegrate 执行四元数指数映射右乘更新，对自由关节则先更新三维世界位置再调用同一四元数积分函数更新姿态。反过来，用于计算两构型间切空间速度差分的 mj_differentiatePos 则对四元数计算局部相对旋转的对数映射，返回 3 个局部体角速度分量；这与 MuJoCo 官方计算文档中关于位置维数 nq 与速度维数 nv 的说明完全对应。

配套教学脚本 kinematics.py 实现了固定基座运动学树的位姿递推、空间速度递推、几何雅可比组装以及雅可比转置力投影。模型中每个刚体节点包含一个转动、移动或固接关节，且子系原点直接取在关节锚点处；平面双连杆的参数读取自 共享模型数据，并将在后续第七、八章的动力学计算中继续沿用：

from kinematics import *

model = two_link_model()
q = model_data()["default_angles"]
v = [1.0, -0.5]
state = forward(model, q, v)
P = [0, 0, -1]  # second-link endpoint in its local frame
J = geometric_jacobian(model, state, 2, P)
print(world_point(state, 2, P))
print(jacobian_velocity(J, v))  # angular first, then linear
print(point_velocity(state, 2, P))

可在仓库根目录或按相对路径运行配套校验脚本，核对递推速度、雅可比速度、切空间有限差分与虚功率恒等式：

python3 examples/check_kinematics.py

from kinematics import integrate_configuration
from quaternion import from_rotvec, to_matrix
from rotation import apply

r = from_rotvec([0.3, -0.4, 0.2])
p, eps = [0.2, -0.3, 0.5], 1e-6
rp = integrate_configuration(["ball"], r, [1, 0, 0], eps)
rm = integrate_configuration(["ball"], r, [1, 0, 0], -eps)
xp, xm = apply(to_matrix(rp), p), apply(to_matrix(rm), p)
column = [(a-b)/(2*eps) for a, b in zip(xp, xm)]
expected = apply(to_matrix(r), [0, -p[2], p[1]])
print(column, expected)