正运动学 mj_kinematics
从 mj_step→ mj_forward→mj_forwardSkip→mj_fwdPosition→mj_kinematics 一路调用下去，就到了我们要讲的第一个完整的功能函数 mj_kinematics。 如其字面意思那样，这是一个正运动学的函数。所谓的正向运动学就是已知各个刚体的之间的相对位置关系， 以及各个关节的运动状态，推算各个刚体在世界坐标系下的位置和姿态。
在介绍机器人的数学物理基础的时候， 我们曾详细介绍过一个开链结构的正向运动学是如何解算的，有兴趣的读者再去翻翻看，这里不再赘述。 本文我们来分析 mj_kinematics 的实现源码，同时会编写一些例程练习使用 MuJoCo 的 API。
1. mj_kinematics
void mj_kinematics(const mjModel* m, mjData* d) {
mju_zero3(d->xpos);
mju_unit4(d->xquat);
mju_zero3(d->xipos);
mju_zero(d->xmat, 9);
mju_zero(d->ximat, 9);
d->xmat[0] = d->xmat[4] = d->xmat[8] = 1;
d->ximat[0] = d->ximat[4] = d->ximat[8] = 1;
正运动学的原理比较简单，需要根据机器人的结构为每个刚体建立一个坐标系，然后根据关节状态写出刚体之间的相对变换矩阵，最后从基座开始根据拓扑关系依次将这些相对变换矩阵串乘起来， 就可以得到各个刚体在世界坐标系下的位姿。
右侧是函数 mj_kinematics() 的代码片段。该函数的两个输入参数分别是模型 和数据对象。在该函数的一开始，先对 worldbody 的世界位姿进行了初始化。 按照刚体系统建模的套路，会对模型中的各个刚体按照拓扑关系从 0 开始编号。 在 MuJoCo 中编号 0 对应的是 worldbody 这个特殊的刚体。
mjData 的 xpos 是一个 mjtNum 类型的数组，依次记录着各个刚体的世界位置。类似的 xquat 和 xmat 分别是描述刚体姿态的四元数和旋转矩阵。xipos 和 ximat 分别是刚体质心的位置和旋转矩阵。 由于在 MoJoCo 中 worldbody 是所有拓扑关系树的根节点，它定义了世界坐标系，所以这里将其位置为零，四元数置为 \((1, 0, 0, 0)\)表示没有旋转，对应的旋转矩阵也是单位阵。
在接下来的代码片段中，调用了 mj_normalizeQuat 把 qpos 中所有的四元数都进行归一化处理。qpos 广义坐标系下各个关节的位置，对于球形关节和自由关节，则是描述姿态的四元数。 为了方便后续处理，这里需要对其进行归一化。第10,11行是对 mocap 对象的姿态进行归一化。通过 mocap 可以实现一些逆运动学的控制， 可以参考[这里]，我们也会在后文中详细分析这一功能。
void mj_normalizeQuat(const mjModel* m, mjtNum* qpos) {
// find quaternion fields and normalize
for (int i=0; i < m->njnt; i++) {
if (m->jnt_type[i] == mjJNT_BALL || m->jnt_type[i] == mjJNT_FREE) {
mju_normalize4(qpos+m->jnt_qposadr[i]+3*(m->jnt_type[i] == mjJNT_FREE));
}
}
}
mj_normalizeQuat(m, d->qpos);
for (int i=0; i < m->nmocap; i++)
mju_normalize4(d->mocap_quat+4*i);
然后就在一个 for 循环中遍历所有的刚体。其中局部变量 xpos 和 xquat 是两个用于临时记录刚体世界位姿的数组。 通过模型对象 m 中的字段 body_jntadr 和 body_jntnum 获取该刚体中的关节首地址和关节数量。字段 body_parentid 记录父节点的索引，body_pos 和 body_quat 记录相对父节点的位置和姿态。
for (int i=1; i < m->nbody; i++) {
mjtNum xpos[3], xquat[4];
int jntadr = m->body_jntadr[i]; // 刚体关节首地址
int jntnum = m->body_jntnum[i]; // 刚体关节数量
// 省略关于 free joint 和 mocap 相关的代码
int pid = m->body_parentid[i]; // 获取当前刚体的父节点
mjtNum *bodypos = m->body_pos+3*i; // 刚体相对父节点的位置向量的起始地址
mjtNum *bodyquat = m->body_quat+4*i; // 刚体相对父节点四元素的起始地址
if (pid) {
// 相对于父节点完成旋转和平移
mju_rotVecMat(xpos, bodypos, d->xmat+9*pid);
mju_addTo3(xpos, d->xpos+3*pid);
mju_mulQuat(xquat, d->xquat+4*pid, bodyquat);
} else {
// parent is the world
mju_copy3(xpos, bodypos);
mju_copy4(xquat, bodyquat);
}
如果一个刚体的父节点索引为 0 说明它直接与 worldbody 连接，那么它相对于 worldbody 的位姿就是其世界位姿。否则就在父节点的世界位姿基础上，再经过相对位姿的旋转和平移得到刚体的世界位姿。
这里调用 mju_rotVecMat 时指定了三个参数，分别是暂存刚体位置的局部变量 xpos，指向刚体相对父节点的位置向量 bodypos，
以及通过 d->mat 和 pid 查找的父节点的旋转矩阵的首地址。再组合上函数 mju_addTo3，有:
$$
\text{xpos} = \text{xmat}_{\text{pid}} * \text{bodypos} + \text{xpos}_{\text{pid}}
$$
类似的，刚体在世界坐标系下的姿态为 \(\text{xquat} = \text{xquat}_{\text{pid}} * \text{bodyquat}\)。在 MuJoCo 中关节是相对于子节点安装的， 也就是说刚体实际的世界位姿，是在上述的旋转平移之后，再附加上关节运动的结果。在下面的代码片段中，局部变量 xanchor 和 xaxis 分别用于记录各个关节的世界位置和关节轴向量。 由于一个刚体可以同时安装很多个关节，所以要通过一个 for 循环中遍历与指定刚体绑定的所有关节，依次把它们的作用串乘到刚体的世界位姿上。
mjtNum xanchor[3], xaxis[3];
for (int j=0; j < jntnum; j++) {
int jid = jntadr + j; // 获取关节编号
int qadr = m->jnt_qposadr[jid]; // 关节数值的首地址
mjtJoint jtype = m->jnt_type[jid]; // 关节类型
在 for 循环中，先通过 \(\text{xaxis} = \text{xquat} * \text{jnt_axis}_{\text{jid}}\) 和 \(\text{xanchor} = \text{xquat} * \text{jnt_pos}_{\text{jid}} + \text{xpos}\) 把关节轴和关节位置转换到世界坐标系下。
mju_rotVecQuat(xaxis, m->jnt_axis+3*jid, xquat);
mju_rotVecQuat(xanchor, m->jnt_pos+3*jid, xquat);
mju_addTo3(xanchor, xpos);
对于平移的关节，还需要沿关节轴的方向移动。40行中的 m->qpos0 是关节的初始位置，
d->qpos 则是关节当前的位置。函数 mju_addToScl3 的运算效果就是:
\(
\text{xpos} = \text{xpos} + (\text{qpos} - \text{qpos0}) * \text{xaxis}
\)
switch (jtype) {
case mjJNT_SLIDE:
mju_addToScl3(xpos, xaxis, d->qpos[qadr] - m->qpos0[qadr]);
break;
对于球形和转动关节，还需要用局部变量 qloc 记录关节转动产生的四元素。
case mjJNT_BALL:
case mjJNT_HINGE: {
mjtNum qloc[4];
if (jtype == mjJNT_BALL)
mju_copy4(qloc, d->qpos+qadr);
else
mju_axisAngle2Quat(qloc, m->jnt_axis+3*jid, d->qpos[qadr] - m->qpos0[qadr]);
然后将 qloc 应用到刚体姿态上。如果转轴并没有穿过刚体坐标系的原点，还需要把因为偏心作用额外产生的位移减去。
// apply rotation
mju_mulQuat(xquat, xquat, qloc);
// correct for off-center rotation
mjtNum vec[3];
mju_rotVecQuat(vec, m->jnt_pos+3*jid, xquat);
mju_sub3(xpos, xanchor, vec);
}
break;
MuJoCo 中没有实现其它类型的关节了。在遍历关节的 for 循环最后，还需要把关节的状态记录到 d->xanchor
和 d->xaxis 中。
default:
mjERROR("unknown joint type %d", jtype); // SHOULD NOT OCCUR
}
mju_copy3(d->xanchor+3*jid, xanchor);
mju_copy3(d->xaxis+3*jid, xaxis);
}
遍历完所有的关节之后，就可以将最后的位姿更新到刚体的世界坐标上了。
// assign xquat and xpos, construct xmat
mju_normalize4(xquat);
mju_copy4(d->xquat+4*i, xquat);
mju_copy3(d->xpos+3*i, xpos);
mju_quat2Mat(d->xmat+9*i, xquat);
}
// 省略惯性坐标(inertial)、可视化几何坐标(geoms) 和 sites 坐标的世界位姿同步
}
2. 一个6自由度机械臂的正运动学
为了简单验证 mj_kinematics 我们用长方体简单堆砌了一个 6 自由度的机械臂。如下图所示，该机械臂有 6 个转动关节。由于没有限幅，所以他们的值域是 \([-\pi, \pi]\)。 在 simulate(1) 中可以进行可视化，如果在 PAUSE 模式下，还可以通过右边栏直接修改关节值，看到机械臂的姿态。
同时，我们还提供了一个简单的程序，修改关节值， 然后调用 mj_kinematics 完成正运动学的推算，更新各个刚体的位姿。如下面的代码片段所示:
void PrintPos(const mjModel* m, mjData* d) {
for (int i = 0; i < m->nbody; i++) {
mjtNum * xpos = d->xpos + 3 * i;
printf("[%d]:\t%f\t%f\t%f\n",
i, xpos[0], xpos[1], xpos[2]);
}
}
std::cout << "-- d->qpos[0] = M_PI * 0.5" << std::endl;
d->qpos[0] = M_PI * 0.5;
mj_kinematics(m, d);
PrintPos(m, d);
我们可以通过如下的指令下载例程，并编译运行。
git clone https://github.com/gaoyichao/XiaoTuMujocoNote.git
cd XiaoTuMujocoNote/正运动学
mkdir build
cd build
cmake ..
make
cd ..
./build/test