复合刚体 mj_crb
在编程套路中我们曾简单提过，整个仿真过程就是一个，求解正动力学计算加速度，然后通过数值积分更新系统状态， 再求解加速度更新状态，不断循环迭代的过程。正动力学是给定作用力计算刚体系统的加速度， 经典的方法有惯性矩阵法和传播算法。 本文要介绍的 mj_crb 函数就是一种惯性矩阵法的实现——复合刚体算法(Composite-Rigid-Body)。
本文中，我们先根据《刚体动力学》介绍惯性矩阵以及复合刚体算法， 再分析函数 mj_crb 的实现，最后讨论惯性矩阵的稀疏性。
1. 正动力学与惯性矩阵
令符号 \(\boldsymbol{q}, \dot{\boldsymbol{q}}, \ddot{\boldsymbol{q}}, \boldsymbol{\tau}\) 分别表示关节位置、速度、加速度和力，\(\boldsymbol{f}^x\) 表示外力， \(\boldsymbol{H}\) 和 \(\boldsymbol{C}\) 分别表示关节空间中的惯性矩阵和偏置力。那么关节的广义力向量 \(\boldsymbol{\tau}\) 和 加速度向量 \(\ddot{\boldsymbol{q}}\) 的关系可以写成运动方程: $$ \boldsymbol{\tau} = \boldsymbol{H}(\boldsymbol{q})\ddot{\boldsymbol{q}} + \boldsymbol{C}(\boldsymbol{q}, \dot{\boldsymbol{q}}, \boldsymbol{f}^x) $$ 这个方程中 \(\boldsymbol{H}\) 只与关节位置 \(\boldsymbol{q}\) 有关，是函数 mj_crb 求解的对象。 \(\boldsymbol{C}\) 则是关于 \(\boldsymbol{q}, \dot{\boldsymbol{q}}, \boldsymbol{f}^x\) 的函数， 可以通过加速度为 0 的递归牛顿欧拉算法求解。 在正动力学问题中，\(\boldsymbol{\tau}\) 是给定的，需要计算的是 \(\ddot{\boldsymbol{q}}\)。我们需要先后计算出系数矩阵 \(\boldsymbol{C}, \boldsymbol{H}\) 然后求解方程 \(\boldsymbol{H}\ddot{\boldsymbol{q}} = \boldsymbol{\tau} - \boldsymbol{C}\) 得到 \(\ddot{\boldsymbol{q}}\)。
对于一棵自由度为 \(n\) 的运动树，其中刚体的数量 \(N_B\) 和关节的数量 \(N_J\) 相等。 运动树的惯性矩阵 \(\boldsymbol{H}\) 则是一个对称的正定的 \(n\times n\) 的矩阵，通常将其划分成 \(N_B \times N_B\) 的矩阵块。 如果 \(n_i, n_j\) 分别是关节 \(i,j\) 的自由度，那么矩阵块 \(\boldsymbol{H}_{ij}\) 的尺寸是 \(n_i \times n_j\)。 如果树中的每个关节都只有一个自由度，那么 \(n = N_B\)，\(\boldsymbol{H}_{ij}\)就是一个\(1 \times 1\) 的矩阵。
物理意义上，\(\boldsymbol{H}_{ij}\)描述了关节 \(j\) 的加速度和关节 \(i\) 的力之间的关系。 根据《刚体动力学》第6章的内容，\(\boldsymbol{H}_{ij}\) 可以通过下式求出:
$$ \begin{equation}\label{equa_H} \boldsymbol{H}_{ij} = \boldsymbol{S}_i^T \left( \sum_{k \in v(i) \cap v(j)} \boldsymbol{I}_k \right) \boldsymbol{S}_j \end{equation} $$按照刚体系统建模的符号系统， 上式中，\(\boldsymbol{S}_i\) 是关节 \(i\) 的运动空间约束。 \(v(i)\)是以刚体 \(i\) 为根的子树中所有刚体的集合，或者说是关节 \(i\) 支撑的所有刚体。\(k \in v(i) \cap v(j)\) 表示关节 \(i\) 和关节 \(j\) 共同支撑的刚体集合。 我们用符号 \(\boldsymbol{I}_i^c\) 表示以刚体 \(i\) 为根节点的子树的惯性，把这棵子树整体看做是一个复合的刚体，\(\boldsymbol{I}_i^c\) 可以写成如下的形式: $$ \begin{equation}\label{equa_I} \boldsymbol{i}_i^c = \sum_{j \in v(i)} \boldsymbol{I}_j \qquad \Longrightarrow \qquad \boldsymbol{I}_i^c = \boldsymbol{I}_i + \sum_{j \in \mu(i)} \boldsymbol{I}_j^c \end{equation} $$ 其中 \(u(i)\) 表示刚体 \(i\) 的子节点。在上一节中，已经通过函数 mj_comPos 计算了各个刚体的惯性 \(\boldsymbol{I}_k\) 和各个关节的运动空间约束 \(\boldsymbol{S}_i\)，分别更新在 mjData 的 cinert 和 cdof 字段中。下面我们研究函数 mj_crb 完成系统惯性矩阵 \(H\) 的求解，结果将保存在 mjData 的 qM 字段中。
2. 函数 mj_crb
void mj_crb(const mjModel* m, mjData* d) {
mjtNum buf[6];
mjtNum* crb = d->crb;
int last_body = m->nbody - 1, nv = m->nv;
左侧是函数 mj_crb 的代码片段，该函数也是一个 MJAPI 函数，以模型 和数据对象作为输入。其中第三行获取了数据对象中的字段 crb，该字段用于保存各个运动子树的惯性 \(\boldsymbol{I}_i^c\)。 局部变量 last_body 记录的是最后一个刚体的编号，nv 获取的是系统的自由度。
下面的代码片段，是对上式(\(\ref{equa_I}\))的实现。对于叶子节点，其运动子树只有它本身，所以有 \(\boldsymbol{I}_i^c = \boldsymbol{I}_i\)。
在第5行中，首先将函数 mj_comPos中更新的 d->cinert 拷贝到 crb 中，对于叶子节点就已经求得其子树惯性。
然后在一个 for 循环中，从最后一个刚体开始向前遍历，将各个刚体的惯性累加到其父节点上，如此可以更新非叶子节点的运动子树的复合惯性 \(\boldsymbol{I}_i^c\)。
第7行对根节点的检查是必要的，因为编号为 0 的节点是 worldbody，它并不对应具体的刚体。
mju_copy(crb, d->cinert, 10*m->nbody);
for (int i = last_body; i > 0; i--) {
if (m->body_parentid[i] > 0)
mju_addTo(crb+10*m->body_parentid[i], crb+10*i, 10);
}
下面的代码片段，是在根据式(\(\ref{equa_H}\))计算各个惯性矩阵块 \(\boldsymbol{H}_{ij}\)。在代码片段中，我们省略了关于 simple body 和 armature inertia 的代码，因为它们与 CRB 算法关系不大。
第 10 行先将 d->qM 清零，在忽略 simple body He armature inertia 的代码情况下，第17行的 += 实际上与 = 没有什么差别。
在下面的双层 for 循环中，先计算了 \(\boldsymbol{I}_i^c \boldsymbol{S}_i\)，然后在内层循环中完成了 \(\boldsymbol{S}_j \left(\boldsymbol{I}_i^c \boldsymbol{S}_i\right)\)。
mju_zero(d->qM, m->nM);
for (int i = 0; i < nv; i++) {
// 省略关于 simple body 和 armature inertia 的代码
// precompute buf = crb_body_i * cdof_i
mju_mulInertVec(buf, crb+10*m->dof_bodyid[i], d->cdof+6*i);
for (int j = i; j >= 0; j = m->dof_parentid[j]) {
// M(i,j) += cdof_j * (crb_body_i * cdof_i)
d->qM[Madr_ij++] += mju_dot(d->cdof+6*j, buf, 6);
} } }
3. 惯性矩阵的稀疏性
仿真系统的惯性矩阵中第 \(i\) 行第 \(j\) 列的元素描述的是关节 \(j\) 的加速度于关节 \(i\) 的力之间的映射关系。根据下面的计算形式， 可以说只有当两个关节有共同支撑的刚体时，\(\boldsymbol{H}_{ij}\) 才不为 0。
 |
右侧是我们从刚体动力学算法中搬来的图，图中描述了五种不同拓扑结构的运动树。 其中空心点表示树的根节点，对应到 mujoco 中就是 worldbody。实心点表示刚体，连线则是关节。这五棵运动树都有四个关节，所以他们的惯性矩阵就是 \(4 \times 4\) 的。 灰色方块表示矩阵中非零的元素。比如图 (a) 中的四个关节各自支撑了一个刚体，不存在两个关节在运动树的同一个分支上，所以得到的就是一个对角矩阵。这是最稀疏的情况。 图 (b) 则是最稠密的情况，这棵树实际是一条链，所有的关节都在一个分支上，它们都共同支撑了最后一个刚体，所以任意两个关节之间都有 \(\boldsymbol{H}_{ij}\)。
得益于惯性矩阵的这种稀疏特性，mujoco 可以用一些特殊的数据结构只保存矩阵中的那些非零元素。仿真系统中的运动树越多，节省的空间也越多。 当然实现起来的代码就没那么好理解。
auto qm = Eigen::MatrixXd::Zero(m->nv, m->nv);
for (int i=0; i < m->nv; i++) {
int Madr_ij = m->dof_Madr[i];
for (int j=i; j >= 0; j = m->dof_parentid[j]) {
qm(i, j) = d->qM[Madr_ij++];
qm(j, i) = qm(i, j);
} }
std::cout << qm << std::endl;
惯性矩阵被保存在数据对象的 mjData.qM 字段中。mjModel.nM 记录了非零元素数量, mjModel.dof_Madr对应惯性矩阵中的每一行，
记录着各行中非零元素在mjData.qM中的位置索引。
左侧是我们参照函数 mj_crb，实现的一段例程，
它将稀疏存储的惯性矩阵转换成 Eigen 的稠密矩阵。
其外层的 for 循环中我们逐行的遍历惯性矩阵 mjDtata.qM，我们先从 mjModel.dof_Madr 中查询出第 \(i\) 行的非零元素起始索引，记录在变量 Madr_ij 中。
这个索引值实际对应的是对角线上的第 \(i\) 行第 \(i\) 列的元素。
上面代码片段中的内层 for 循环用于遍历第\(i\)行中的非零元素。如果两个关节共同支撑了同一个刚体，那么这两个关节在运动树上一定出现在同一个分支上，相互之间是祖先/后代的关系。
所以 mjData.qM 中依次记录了关节 \(i\) 的各个祖先关节的惯性关系，我们可以通过 mjModel.dof_parentid 逐级向上查找到祖先关节的索引 \(j\)。
由于惯性矩阵是对称的，所以第5,6行中通过交换角标的方式填充 Eigen 的稠密矩阵。