质量质心 mj_comPos
函数 mj_fwdPosition 完成了上一节介绍的正运动学推算，更新了各个刚体和关节的世界位姿之后， 就需要调用函数 mj_comPos 来考虑它们的质量了。本文要分析的这个函数，主要完成了三件事情: ① 更新各运动子树的质心和质量 ② 更新各个刚体关于运动子树质心坐标系的惯性张量 ③ 更新在世界坐标系下，各个关节相对于质心的平移和旋转运动。
关于质心和惯性张量的内容，有兴趣的读者可以参考机器人控制的数学物理基础系列文章。 本文重点在分析函数 mj_comPos 的实现代码。
1. 计算质心
void mj_comPos(const mjModel* m, mjData* d) {
mjtNum offset[3], axis[3];
mj_markStack(d);
mjtNum* mass_subtree = mj_stackAllocNum(d, m->nbody);
// clear subtree
mju_zero(mass_subtree, m->nbody);
mju_zero(d->subtree_com, m->nbody*3);
我们在分析动力学问题的时候，通常找一个参考点来描述刚体总体的运动状态，然后再讨论刚体上各个质点相对于该参考点的运动。 质心就是这样一个常用的参考点，它的质量等于刚体上各个质点的质量之和， 它的位置矢量则是各个质点位置矢量的加权平均，即:
$$ m_c = \sum_{i = 1}^n m_i \qquad \boldsymbol{r_c} = \cfrac{\sum_{i = 1}^n m_i \boldsymbol{r_i}}{\sum_{i = 1}^n m_i} $$上面右侧是该函数的代码片段，与其它 MJAPI 函数一样有两个输入参数，分别是模型 和数据对象。第二行中定义了两个局部的三维数组，offset 用于临时保存各个刚体和关节相对于质心的位置矢量， axis 用于临时记录关节的运动轴。第3行标记使用了数据对象 d 的数据栈空间， 接着从栈空间中分配了一段内存，用指针 mass_subtree 标记，用于记录各个刚体对应的运动子树的质量。数据对象 d 中的字段 subtree_com 记录了各个运动子树的质心坐标。
在下面的 for 循环中，mj_comPos 从最后一个刚体开始，倒着向前遍历所有的刚体。根据刚体编码套路， 在一个运动树中，父节点的编码一定小于子节点。所以这里倒序遍历，就会先计算叶子节点构成的运动子树，它们只有一个节点。 下面 for 循环中的 9 行直接按照质点的定义，将刚体的质量乘上世界坐标更新到 subtre_com 上。第 10 行则用于累积质量。
for (int i = m->nbody-1; i >= 0; i--) {
mju_addToScl3(d->subtree_com+3*i, d->xipos+3*i, m->body_mass[i]);
mass_subtree[i] += m->body_mass[i];
其实在这个 for 循环中，经历了上面两个的操作，就已经求得以 i 为根节点的运动子树的质量和质心位矢的分子部分。
对于那些非叶子节点的刚体，其运动子树除了自身之外，还包含由他派生出去的所有子节点。
所以我们在更新子节点的时候，还需要将以该节点为根的子树的质量和位矢分子累加到其父节点上，如下面的代码片段所示。
这样当遍历到父节点的时候，其对应的 subtree_com 和 mass_subtree 就已经累加了所有的子节点的质心，再通过上面两行的操作累加上自身的质量和位矢分子即可。
i = 0 时对应的是 worldbody 它没有父节点，所以不需要该操作了。
if (i) {
int j = m->body_parentid[i];
mju_addTo3(d->subtree_com+3*j, d->subtree_com+3*i);
mass_subtree[j] += mass_subtree[i];
}
按照质心的定义，位矢分子还需要除以运动子树的总质量得到的才是质心位矢。在下面的代码片段中，讨论了运动子树的总质量，如果质量很小，那么有除零的风险， 并且得到的质心位矢的数值误差可能很大，所以这里直接赋予了对应刚体的世界坐标。
if (mass_subtree[i] < mjMINVAL)
mju_copy3(d->subtree_com+3*i, d->xipos+3*i);
else
mju_scl3(d->subtree_com+3*i, d->subtree_com+3*i, 1.0/mjMAX(mjMINVAL, mass_subtree[i]));
}
2. 计算惯性张量
惯性张量是一个 \(3 \times 3\) 的对称矩阵 \(\boldsymbol{I}\)，描述的是刚体转动时的惯性特征。 在接下来的for循环中，先计算了各个刚体质心与其运动子树质心的偏移矢量，通过函数 mju_inertCom 更新数据对象 d 的字段 cinert。 对于每个刚体，字段 cinert 都有 10 个元素。其中前六个对应惯性张量矩阵中的3个转动惯量和3个惯性积，剩下 3 个对应着刚体质心与运动子树质心的平移，最后一个元素记录刚体自身的质量。
mju_zero(d->cinert, 10);
for (int i = 1; i < m->nbody; i++) {
mju_sub3(offset, d->xipos+3*i, d->subtree_com+3*m->body_rootid[i]);
mju_inertCom(d->cinert+10*i, m->body_inertia+3*i, d->ximat+9*i, offset, m->body_mass[i]);
}
下面是函数 mju_inertCom 的代码片段，它有五个参数。形参 res 传入的是字段 cinert 中对应刚体的首地址，inert 传入的是刚体绕三个惯量主轴的转动惯量，mat 则是刚体的姿态矩阵， dif 是刚刚计算的质心的偏移矢量，mass 是刚体的质量。
void mju_inertCom(mjtNum res[10], const mjtNum inert[3], const mjtNum mat[9], const mjtNum dif[3], mjtNum mass) {
// tmp = diag(inert) * mat' (mat is local-to-global rotation)
mjtNum tmp[9] = {mat[0]*inert[0], mat[3]*inert[0], mat[6]*inert[0],
mat[1]*inert[1], mat[4]*inert[1], mat[7]*inert[1],
mat[2]*inert[2], mat[5]*inert[2], mat[8]*inert[2]};
// res_rot = mat * diag(inert) * mat'
res[0] = mat[0]*tmp[0] + mat[1]*tmp[3] + mat[2]*tmp[6];
res[1] = mat[3]*tmp[1] + mat[4]*tmp[4] + mat[5]*tmp[7];
res[2] = mat[6]*tmp[2] + mat[7]*tmp[5] + mat[8]*tmp[8];
res[3] = mat[0]*tmp[1] + mat[1]*tmp[4] + mat[2]*tmp[7];
res[4] = mat[0]*tmp[2] + mat[1]*tmp[5] + mat[2]*tmp[8];
res[5] = mat[3]*tmp[2] + mat[4]*tmp[5] + mat[5]*tmp[8];
根据惯性张量的坐标变换规则，假设 \(\boldsymbol{I}_b\) 是在刚体的局部坐标系下求得的惯性张量，也就是这里通过 inert 构成的对角矩阵。 经过旋转矩阵 \(R_{b}^0\) 转换到世界坐标系下的惯性张量 \(\boldsymbol{I}_0\)，有变换关系:
$$ \boldsymbol{I}_0 = {R_{b}^0} \boldsymbol{I}_b {\boldsymbol{R}_b^0}^T $$ 左侧的代码片段实现的就是这个关系。它先在一个局部数组 tmp 中完成 \(\boldsymbol{I}_b {\boldsymbol{R}_b^0}^T\) 的计算。然后计算转动惯量， \(J_x = \text{res}[0]\), \(J_y = \text{res}[1]\), \(J_z = \text{res}[2]\),以及惯性积\(J_{xy} = -\text{res}[3]\), \(J_{zx} = -\text{res}[4]\), \(J_{yz} = -\text{res}[5]\)。
res[0] += mass*(dif[1]*dif[1] + dif[2]*dif[2]);
res[1] += mass*(dif[0]*dif[0] + dif[2]*dif[2]);
res[2] += mass*(dif[0]*dif[0] + dif[1]*dif[1]);
res[3] -= mass*dif[0]*dif[1];
res[4] -= mass*dif[0]*dif[2];
res[5] -= mass*dif[1]*dif[2];
除了旋转之外，刚体原点相对于运动子树质心还存在一个 dif 的平移量。应用平行轴定理，有: $$ \boldsymbol{I}_k = \boldsymbol{I}_0 + m (\boldsymbol{r}^T\boldsymbol{r} \boldsymbol{E}_{3\times 3} - \boldsymbol{r}\boldsymbol{r}^T) $$ 上式中，\(\boldsymbol{r}^T\boldsymbol{r}\boldsymbol{E}_{3\times3}\) 得到的是一个 \(3 \times 3\) 的对角矩阵，对角元素都是 $$ \text{dif}[0] * \text{dif}[0] + \text{dif}[1] * \text{dif}[1] * \text{dif}[2] * \text{dif}[2] $$
该函数剩下几行没啥营养，就不展开了。
res[6] = mass*dif[0];
res[7] = mass*dif[1];
res[8] = mass*dif[2];
// res_mass = mass
res[9] = mass;
}
3. 相对质心的运动
接下来的代码把各个关节的运动转换成，世界坐标系下关于运动子树质心的运动。 在接下来的 for 循环中，如下面的代码片段所示，遍历所有的关节。用局部变量 da 记录对应关节自由度数据的起始索引，bi 则是绑定刚体 id。 并将运动子树质心指向关节的偏移矢量记录到 offset 中。
for (int j=0; j < m->njnt; j++) {
int da = 6*m->jnt_dofadr[j];
int bi = m->jnt_bodyid[j];
mju_sub3(offset, d->subtree_com+3*m->body_rootid[bi], d->xanchor+3*j);
在数据对象 d 中有字段 cdof 记录各个自由度关于质心的运动关系，每个自由度有 6 个数，前三个描述相对质心的转动，后三个描述相对质心的平移。 我们先来看只有一个自由度的平动关节和旋转关节，它们都调用函数 mju_dofCom 完成自由度的映射。
下面右侧的代码片段就是函数 mju_dofCom 的所有内容，它检查输入的参数 offset 是否为 0 来区分是旋转运动还是平移运动。 对于平动关节，只有平移没有旋转，所以将对应的 cdof 前三个元素置为 0，后三个元素保持平移轴。 对于旋转关节，由于旋转运动对于刚体上的每个点而言都是一样的，所以直接将前刚体的转轴拷贝到 cdof 的前三个元素上。而参考点的不同，刚体上点的线速度都是不一样的， 存在关系 \(\boldsymbol{v} = \boldsymbol{\omega} \times \boldsymbol{r}\)。所以这里用转轴与偏移矢量的叉积来表示对应自由度的平移运动。
switch ((mjtJoint) m->jnt_type[j]) {
case mjJNT_SLIDE:
mju_dofCom(d->cdof+da, d->xaxis+3*j, 0);
break;
case mjJNT_HINGE:
mju_dofCom(d->cdof+da, d->xaxis+3*j, offset);
break;
if (offset) {
mju_copy3(res, axis);
mju_cross(res+3, axis, offset);
} else {
mju_zero3(res);
mju_copy3(res+3, axis);
}
对于有6个自由度的自由关节和3个自由度的球关节而言，需要逐个处理它们的自由度。我们先看在下面的代码片段中 44-51 行的代码，这里针对球关节， 遍历了所有的三个旋转轴，先从旋转矩阵中依次提取三个旋转自由度的转轴，再通过 mju_dofCom 完成自由度的映射。 对于自由关节，它的旋转自由度的处理方式与球关节一样。下面片段中关于 mjJNT_FREE 的讨论，是在处理它的平移自由度。
case mjJNT_FREE:
mju_zero(d->cdof+da, 18);
for (int i=0; i < 3; i++)
d->cdof[da+3+7*i] = 1;
// rotation components: same as ball
skip = 18;
mjFALLTHROUGH;
case mjJNT_BALL:
for (int i=0; i < 3; i++) {
axis[0] = d->xmat[9*bi+i+0];
axis[1] = d->xmat[9*bi+i+3];
axis[2] = d->xmat[9*bi+i+6];
mju_dofCom(d->cdof+da+skip+6*i, axis, offset);
}
break;