まえがき

リンク系運動方程式における慣性行列の計算方法は幾つか知られていますが，代表的なのは「単位ベクトル法（unit vector method）」「合成剛体法（composite rigid body method）」の二つでしょう．これらはいずれもWalker and Orinにより示されたものです．

M. W. Walker and D. E. Orin, Efficient Dynamic Computer Simulation of Robotic Mechanisms, Transactions of the ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 104, pp. 205--211, 1982.

単位ベクトル法は分かりやすいので大抵のロボットの教科書で説明されています．一方，合成剛体法は単位ベクトル法よりも高速で実用的なのですが，説明している教科書はなぜかほとんどありません．筆者の手許の書籍では，次のFeatherstoneにしか書いてありませんでした．

R. Featherstone, Robot Dynamics Algorithms, Kluwer Academic Publishers, Boston/Dordrecht/Lancaster, 1987.

Featherstoneの空間ベクトル表現は表記を簡単にしますが，計算効率の良い実装をするには中身を知っている必要があります．このような事情から本記事を書くことにしました．剛体リンク系の運動方程式についてある程度知識を持っている読者を想定しています．

リンク系慣性行列に関する数学的準備

リンク系の運動方程式は，一般的に次の形で表せます．

\boldsymbol{H}(\boldsymbol{q})\ddot{\boldsymbol{q}}+\boldsymbol{b}(\boldsymbol{q},\dot{\boldsymbol{q}})=\boldsymbol{\tau}+\boldsymbol{J}(\boldsymbol{q})^{\mathrm{T}}\boldsymbol{f}

ただし，$\boldsymbol{q}$は一般化座標，$\boldsymbol{H}(\boldsymbol{q})$は慣性行列，$\boldsymbol{b}(\boldsymbol{q},\dot{\boldsymbol{q}})$はバイアス力（遠心力・コリオリ力・重力をまとめたもの），$\boldsymbol{\tau}$は一般化座標に対応する一般化力，$\boldsymbol{f}$は外力，$\boldsymbol{J}(\boldsymbol{q})$は外力の作用する接触点のヤコビ行列です．

リンク系はループは持たない開構造であるものとします．本記事の関心事項は慣性行列$\boldsymbol{H}(\boldsymbol{q})$のみなので，無重量空間でリンク系が速度を持たず（つまり$\boldsymbol{b}(\boldsymbol{q},\dot{\boldsymbol{q}})=\boldsymbol{0}$），外力も作用していない（$\boldsymbol{f}=\boldsymbol{0}$）世界での運動を考えれば十分です．

\boldsymbol{H}(\boldsymbol{q})\ddot{\boldsymbol{q}}=\boldsymbol{\tau}

さて，慣性行列の中身は次のようになっています．

\boldsymbol{H}(\boldsymbol{q})=\sum_{i=1}^{n}\left(m_{i}\boldsymbol{J}_{\mathrm{G}i}(\boldsymbol{q})^{\mathrm{T}}\boldsymbol{J}_{\mathrm{G}i}(\boldsymbol{q})+\boldsymbol{J}_{\mathrm{A}i}(\boldsymbol{q})^{\mathrm{T}}\boldsymbol{I}_{i}(\boldsymbol{q})\boldsymbol{J}_{\mathrm{A}i}(\boldsymbol{q})\right)

ただし，$n$は全リンク数，$m_{i}$は$i$番目リンクの質量，$\boldsymbol{I}_{i}(\boldsymbol{q})$は$i$番目リンクの重心まわり慣性テンソルで，$i$番目リンクの姿勢行列を$\boldsymbol{R}_{i}(\boldsymbol{q})$，リンク座標系における重心まわり慣性テンソル（定数行列）を${}^{i}\boldsymbol{I}_{i}$とそれぞれおけば，

\boldsymbol{I}_{i}(\boldsymbol{q})=\boldsymbol{R}_{i}(\boldsymbol{q}){}^{i}\boldsymbol{I}_{i}\boldsymbol{R}_{i}(\boldsymbol{q})^{\mathrm{T}}

です．また，$\boldsymbol{J}_{\mathrm{G}i}(\boldsymbol{q})$と$\boldsymbol{J}_{\mathrm{A}i}(\boldsymbol{q})$は$i$番目リンクは次式を満たす行列です．

\dot{\boldsymbol{p}}_{\mathrm{G}i}=\boldsymbol{J}_{\mathrm{G}i}(\boldsymbol{q})\dot{\boldsymbol{q}}
\\
\boldsymbol{\omega}_{i}=\boldsymbol{J}_{\mathrm{A}i}(\boldsymbol{q})\dot{\boldsymbol{q}}

ただし，$\boldsymbol{p}_{\mathrm{G}i}$は$i$番目リンクの重心位置，$\boldsymbol{\omega}_{i}$は$i$番目リンクの角速度です．重心を関心点とする基礎ヤコビ行列と呼んでも構いません．ちなみにこの式の通りに計算を実装したときの計算量は，単位ベクトル法とあまり変わりません．
式から明らかに，$\boldsymbol{H}(\boldsymbol{q})$は対称行列かつ正定値行列となります．

一般化座標と一般化力は，各リンクの関節変位$\boldsymbol{q}_{i}$と関節（駆動）力$\boldsymbol{\tau}_{i}$をまとめたものとして定められます．

\boldsymbol{q}=\left[\begin{array}{c}
\boldsymbol{q}_{0} \\
\boldsymbol{q}_{1} \\
\vdots \\
\boldsymbol{q}_{n}
\end{array}\right]
,\qquad
\boldsymbol{\tau}=\left[\begin{array}{c}
\boldsymbol{\tau}_{0} \\
\boldsymbol{\tau}_{1} \\
\vdots \\
\boldsymbol{\tau}_{n}
\end{array}\right]

これに対応して，慣性行列は次のように分解できます．

\boldsymbol{H}(\boldsymbol{q})=\left[\begin{array}{cccc}
\boldsymbol{H}_{11}(\boldsymbol{q}) & \boldsymbol{H}_{12}(\boldsymbol{q}) & \cdots & \boldsymbol{H}_{1n}(\boldsymbol{q}) \\
\boldsymbol{H}_{12}(\boldsymbol{q})^{\mathrm{T}} & \boldsymbol{H}_{22}(\boldsymbol{q}) & \cdots & \boldsymbol{H}_{2n}(\boldsymbol{q}) \\
\vdots & \vdots & \ddots & \vdots \\
\boldsymbol{H}_{1n}(\boldsymbol{q})^{\mathrm{T}} & \boldsymbol{H}_{2n}(\boldsymbol{q})^{\mathrm{T}}& \cdots & \boldsymbol{H}_{nn}(\boldsymbol{q}) \\
\end{array}\right]

つまり，

\left[\begin{array}{cccc}
\boldsymbol{H}_{11}(\boldsymbol{q}) & \boldsymbol{H}_{12}(\boldsymbol{q}) & \cdots & \boldsymbol{H}_{1n}(\boldsymbol{q}) \\
\boldsymbol{H}_{12}(\boldsymbol{q})^{\mathrm{T}} & \boldsymbol{H}_{22}(\boldsymbol{q}) & \cdots & \boldsymbol{H}_{2n}(\boldsymbol{q}) \\
\vdots & \vdots & \ddots & \vdots \\
\boldsymbol{H}_{1n}(\boldsymbol{q})^{\mathrm{T}} & \boldsymbol{H}_{2n}(\boldsymbol{q})^{\mathrm{T}}& \cdots & \boldsymbol{H}_{nn}(\boldsymbol{q}) \\
\end{array}\right]
\left[\begin{array}{c}
\ddot{\boldsymbol{q}}_{0} \\
\ddot{\boldsymbol{q}}_{1} \\
\vdots \\
\ddot{\boldsymbol{q}}_{n}
\end{array}\right]
=
\left[\begin{array}{c}
\boldsymbol{\tau}_{0} \\
\boldsymbol{\tau}_{1} \\
\vdots \\
\boldsymbol{\tau}_{n}
\end{array}\right]

と書けるということです．

合成剛体法

考え方

運動方程式において，仮想的に$i$番目関節以外の全ての関節の加速度がゼロである状況を考えましょう．

\left[\begin{array}{c}
\boldsymbol{H}_{1i}(\boldsymbol{q}) \\
\boldsymbol{H}_{2i}(\boldsymbol{q}) \\
\vdots \\
\boldsymbol{H}_{(i-1)i}(\boldsymbol{q}) \\
\boldsymbol{H}_{ii}(\boldsymbol{q}) \\
\boldsymbol{H}_{i(i+1)}(\boldsymbol{q})^{\mathrm{T}} \\
\vdots \\
\boldsymbol{H}_{in}(\boldsymbol{q})^{\mathrm{T}} \\
\end{array}\right]
\ddot{\boldsymbol{q}}_{i}
=
\left[\begin{array}{c}
\boldsymbol{\tau}_{0} \\
\boldsymbol{\tau}_{1} \\
\vdots \\
\boldsymbol{\tau}_{i-1} \\
\boldsymbol{\tau}_{i} \\
\boldsymbol{\tau}_{i+1} \\
\vdots \\
\boldsymbol{\tau}_{n}
\end{array}\right]

$i$番目関節よりも先側のリンク群は一体となって運動します．これを「$i$番目合成剛体」と呼ぶことにします．$i$番目関節より根元側のリンク群は静止したままなので，$i$番目合成剛体の運動に影響しません．$i$番目関節以外の関節には，関節運動を拘束するための力（内力）が発生することになります．

$i$番目リンク座標系に対する$i$番目合成剛体の質量，重心位置，慣性テンソルをそれぞれ$m_{i}^{\mathrm{C}}$，${}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}$，${}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}$とおきましょう．内力は全て打ち消し合うので，$i$番目関節に作用する6軸力${}^{i}\boldsymbol{f}_{i}$，${}^{i}\boldsymbol{n}_{i}$のみが$i$番目合成剛体の運動を作ります．これを記述する運動方程式は次式のようになります．

m_{i}^{\mathrm{C}}{}^{i}\hat{\boldsymbol{a}}_{\mathrm{G}i}^{\mathrm{C}}={}^{i}\boldsymbol{f}_{i}
\\
{}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}{}^{i}\hat{\boldsymbol{\alpha}}_{i}={}^{i}\boldsymbol{n}_{i}-{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times{}^{i}\boldsymbol{f}_{i}

ただし，

{}^{i}\hat{\boldsymbol{a}}_{\mathrm{G}i}^{\mathrm{C}}=\boldsymbol{R}_{i}^{\mathrm{T}}\ddot{\boldsymbol{p}}_{\mathrm{G}i}^{\mathrm{C}}
\\
{}^{i}\hat{\boldsymbol{\alpha}}_{i}=\boldsymbol{R}_{i}^{\mathrm{T}}\dot{\boldsymbol{\omega}}_{i}

とおきました．$\boldsymbol{\omega}_{i}$は$i$番目関節の運動により生じる合成剛体の角速度です．仮想的に速度がゼロの状況を考えているので，$\boldsymbol{\omega}_{i}=\boldsymbol{0}$であることにご注意下さい（このため遠心力・コリオリ力が生じません）．

ここで$i$番目リンク座標系原点位置を$\boldsymbol{p}_{i}$とおき，

{}^{i}\hat{\boldsymbol{a}}_{i}\overset{\mathrm{def}}{=}\boldsymbol{R}_{i}^{\mathrm{T}}\ddot{\boldsymbol{p}}_{i}

を定義すれば，

\ddot{\boldsymbol{p}}_{\mathrm{G}i}^{\mathrm{C}}=\ddot{\boldsymbol{p}}_{i}-\boldsymbol{R}_{i}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times{}^{i}\hat{\boldsymbol{\alpha}}_{i}
\quad\Leftrightarrow\quad
{}^{i}\hat{\boldsymbol{a}}_{\mathrm{G}i}^{\mathrm{C}}={}^{i}\hat{\boldsymbol{a}}_{i}-{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times{}^{i}\hat{\boldsymbol{\alpha}}_{i}

です（ここでも向心加速度・コリオリの加速度が生じません）．これを運動方程式に代入し，行列を使ってまとめれば，

\displaylines{
\left[\begin{array}{cc}
m_{i}^{\mathrm{C}}\boldsymbol{1} & -m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times \\
\boldsymbol{O} & {}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}
\end{array}\right]
\left[\begin{array}{c}
{}^{i}\hat{\boldsymbol{a}}_{i}
\\
{}^{i}\hat{\boldsymbol{\alpha}}_{i}
\end{array}\right]
=
\left[\begin{array}{cc}
\boldsymbol{1} & \boldsymbol{O} \\
-{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times & \boldsymbol{1}
\end{array}\right]
\left[\begin{array}{c}
{}^{i}\boldsymbol{f}_{i}
\\
{}^{i}\boldsymbol{n}_{i}
\end{array}\right]
\\
\Leftrightarrow\qquad
\left[\begin{array}{cc}
m_{i}^{\mathrm{C}}\boldsymbol{1} & -m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times \\
m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times & {}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}-m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times^{2}
\end{array}\right]
\left[\begin{array}{c}
{}^{i}\hat{\boldsymbol{a}}_{i}
\\
{}^{i}\hat{\boldsymbol{\alpha}}_{i}
\end{array}\right]
=
\left[\begin{array}{c}
{}^{i}\boldsymbol{f}_{i}
\\
{}^{i}\boldsymbol{n}_{i}
\end{array}\right]
}

となります．左辺の係数行列が合成剛体の慣性行列です．

根元側のリンク群は全て静止しているので，$i$番目関節の加速度は次の様に関節加速度のみで表せます．

\left[\begin{array}{c}
{}^{i}\hat{\boldsymbol{a}}_{i}
\\
{}^{i}\hat{\boldsymbol{\alpha}}_{i}
\end{array}\right]
=
\left[\begin{array}{c}
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{L}i}
\\
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{A}i}
\end{array}\right]
\ddot{\boldsymbol{q}}_{i}

ただし，${}^{i}\hat{\boldsymbol{Z}}_{\mathrm{L}i}$および${}^{i}\hat{\boldsymbol{Z}}_{\mathrm{A}i}$はそれぞれ$i$番目関節軸方向を表す行列（関節運動が作る並進運動および回転運動の単位方向ベクトルを集めたもの）です．たとえば

\begin{cases}
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{L}i}=\left[\begin{array}{c}
 0 \\ 0 \\ 1
\end{array}\right] \\
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{A}i}=\left[\begin{array}{c}
 0 \\ 0 \\ 0
\end{array}\right]
\end{cases}
\quad(\mbox{直動関節})
,\qquad
\begin{cases}
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{L}i}=\left[\begin{array}{c}
 0 \\ 0 \\ 0
\end{array}\right] \\
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{A}i}=\left[\begin{array}{c}
 0 \\ 0 \\ 1
\end{array}\right]
\end{cases}
\quad(\mbox{回転関節})

となります．

また，$i$番目関節よりも根元側にある$j$番目関節には，${}^{i}\boldsymbol{f}_{i}$，${}^{i}\boldsymbol{n}_{i}$を等価変換した6軸力が作用します．

\left[\begin{array}{c}
{}^{j}\boldsymbol{f}_{j}
\\
{}^{j}\boldsymbol{n}_{j}
\end{array}\right]
=
\left[\begin{array}{cc}
{}^{j}\boldsymbol{R}_{i} & \boldsymbol{O}
\\
{}^{j}\boldsymbol{p}_{i}\times{}^{j}\boldsymbol{R}_{i} & {}^{j}\boldsymbol{R}_{i}
\end{array}\right]
\left[\begin{array}{c}
{}^{i}\boldsymbol{f}_{i}
\\
{}^{i}\boldsymbol{n}_{i}
\end{array}\right]

関節駆動力は，この関節に作用する6軸力の関節軸方向成分です．

\boldsymbol{\tau}_{j}=\left[\begin{array}{cc}
{}^{j}\hat{\boldsymbol{Z}}_{\mathrm{L}j}^{\mathrm{T}}
&
{}^{j}\hat{\boldsymbol{Z}}_{\mathrm{A}j}^{\mathrm{T}}
\end{array}\right]
\left[\begin{array}{c}
{}^{j}\boldsymbol{f}_{j}
\\
{}^{j}\boldsymbol{n}_{j}
\end{array}\right]

以上をまとめると，次式を得ます．

\left[\begin{array}{cc}
{}^{j}\hat{\boldsymbol{Z}}_{\mathrm{L}j}^{\mathrm{T}}
&
{}^{j}\hat{\boldsymbol{Z}}_{\mathrm{A}j}^{\mathrm{T}}
\end{array}\right]
\left[\begin{array}{cc}
{}^{j}\boldsymbol{R}_{i} & \boldsymbol{O}
\\
{}^{j}\boldsymbol{p}_{i}\times{}^{j}\boldsymbol{R}_{i} & {}^{j}\boldsymbol{R}_{i}
\end{array}\right]
\left[\begin{array}{cc}
m_{i}^{\mathrm{C}}\boldsymbol{1} & -m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times \\
m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times & {}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}-m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times^{2}
\end{array}\right]
\left[\begin{array}{c}
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{L}i}
\\
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{A}i}
\end{array}\right]
\ddot{\boldsymbol{q}}_{i}
=
\boldsymbol{\tau}_{j}

すなわち

\boldsymbol{H}_{ji}=
\left[\begin{array}{cc}
{}^{j}\hat{\boldsymbol{Z}}_{\mathrm{L}j}^{\mathrm{T}}
&
{}^{j}\hat{\boldsymbol{Z}}_{\mathrm{A}j}^{\mathrm{T}}
\end{array}\right]
\left[\begin{array}{cc}
{}^{j}\boldsymbol{R}_{i} & \boldsymbol{O}
\\
{}^{j}\boldsymbol{p}_{i}\times{}^{j}\boldsymbol{R}_{i} & {}^{j}\boldsymbol{R}_{i}
\end{array}\right]
\left[\begin{array}{cc}
m_{i}^{\mathrm{C}}\boldsymbol{1} & -m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times \\
m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times & {}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}-m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times^{2}
\end{array}\right]
\left[\begin{array}{c}
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{L}i}
\\
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{A}i}
\end{array}\right]

です．ただし，$j$番目リンクが$i$番目関節よりも根元側にあることを前提としています．$j$番目リンクが$i$番目リンクの属する枝と異なる枝にある場合はゼロ行列となります．アルゴリズム上は，

\displaylines{
\left[\begin{array}{c}
{}^{i}\hat{\boldsymbol{f}}_{i}
\\
{}^{i}\hat{\boldsymbol{n}}_{i}
\end{array}\right]
=
\left[\begin{array}{cc}
m_{i}^{\mathrm{C}}\boldsymbol{1} & -m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times \\
m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times & {}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}-m_{i}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}\times^{2}
\end{array}\right]
\left[\begin{array}{c}
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{L}i}
\\
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{A}i}
\end{array}\right]
\\
\boldsymbol{H}_{ii}=
\left[\begin{array}{cc}
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{L}i}^{\mathrm{T}} &
{}^{i}\hat{\boldsymbol{Z}}_{\mathrm{A}i}^{\mathrm{T}}
\end{array}\right]
\left[\begin{array}{c}
{}^{i}\hat{\boldsymbol{f}}_{i}
\\
{}^{i}\hat{\boldsymbol{n}}_{i}
\end{array}\right]
}

を先に計算しておき，$i$番目リンクから親側にリンクをたどりながら，

\displaylines{
{}^{i}\boldsymbol{S}_{j}=
\left[\begin{array}{cc}
{}^{i}\boldsymbol{R}_{j} & {}^{i}\boldsymbol{p}_{j}\times{}^{i}\boldsymbol{R}_{j}
\\
\boldsymbol{O} & {}^{i}\boldsymbol{R}_{j}
\end{array}\right]
\left[\begin{array}{c}
{}^{j}\hat{\boldsymbol{Z}}_{\mathrm{L}j}
\\
{}^{j}\hat{\boldsymbol{Z}}_{\mathrm{A}j}
\end{array}\right]
\\
\boldsymbol{H}_{ji}=
{}^{i}\boldsymbol{S}_{j}^{\mathrm{T}}
\left[\begin{array}{c}
{}^{i}\hat{\boldsymbol{f}}_{i}
\\
{}^{i}\hat{\boldsymbol{n}}_{i}
\end{array}\right]
\\
\boldsymbol{H}_{ij}=\boldsymbol{H}_{ji}^{\mathrm{T}}
}

を求めていくのが良いでしょう．

合成剛体の質量特性計算アルゴリズム

$i$番目リンク座標系に対する$i$番目合成剛体の質量$m_{i}^{\mathrm{C}}$，重心位置${}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}$，慣性テンソル${}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}$が計算できれば情報が揃います．これらは再帰的に計算することが出来ます．

まず，$i$番目リンクが開リンク系の末端ならば，

\displaylines{
m_{i}^{\mathrm{C}}=m_{i}
\\
{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}={}^{i}\boldsymbol{p}_{\mathrm{G}i}
\\
{}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}={}^{i}\boldsymbol{I}_{i}
}

です．これらから根元側にさかのぼりながら，

\displaylines{
m_{i}^{\mathrm{C}}=m_{i}+\sum_{j\in\mathcal{I}_{\mathrm{C}i}}m_{j}^{\mathrm{C}}
\\
{}^{i}\boldsymbol{p}_{\mathrm{G}j}^{\mathrm{C}}={}^{i}\boldsymbol{p}_{j}+{}^{i}\boldsymbol{R}_{j}{}^{j}\boldsymbol{p}_{\mathrm{G}j}^{\mathrm{C}}
\\
{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}=
\left(
m_{i}{}^{i}\boldsymbol{p}_{\mathrm{G}i}+
\sum_{j\in\mathcal{I}_{\mathrm{C}i}}m_{j}^{\mathrm{C}}{}^{i}\boldsymbol{p}_{\mathrm{G}j}^{\mathrm{C}}
\right)\Big/
m_{i}^{\mathrm{C}}
 \\
{}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}={}^{i}\boldsymbol{I}_{i}-m_{i}({}^{i}\boldsymbol{p}_{\mathrm{G}i}-{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}})\times^{2}+
\sum_{j\in\mathcal{I}_{\mathrm{C}i}}\left\{
{}^{i}\boldsymbol{R}_{j}{}^{j}\boldsymbol{I}_{j}^{\mathrm{C}}{}^{i}\boldsymbol{R}_{j}^{\mathrm{T}}+
m_{j}^{\mathrm{C}}
({}^{i}\boldsymbol{p}_{\mathrm{G}j}^{\mathrm{C}}-{}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}})\times^{2}
\right\}
}

と計算していけば良いです．ただし，$\mathcal{I}_{\mathrm{C}i}$は$i$番目リンクの子リンクインデックス群です．これらのうち$m_{i}^{\mathrm{C}}$は運動学情報を用いませんので，最初に一度だけ計算して求めておけば良いことにご注意下さい．

RoKiでの実装

RoKiでは合成剛体法を次の二つの関数で実装しています．

/* compute a cell of the inertia matrix of a kinematic chain based on the composite rigid body method. */
static void _rkChainLinkInertiaMatCRB(rkLink *link, zVec6D wi[], zVec6D si[], zMat hij, zMat inertia)
{
  rkLink *lp;
  zFrame3D f;
  int i, j;

  /* diagonal block */
  rkJointCRBWrench( rkLinkJoint(link), rkLinkCRB(link), wi );
  rkJointCRBXform( rkLinkJoint(link), ZFRAME3DIDENT, si );
  zMatSetSizeNC( hij, rkLinkJointSize(link), rkLinkJointSize(link) );
  for( i=0; i<rkLinkJointSize(link); i++ )
    for( j=0; j<rkLinkJointSize(link); j++ )
      zMatElemNC(hij,i,j) = zVec6DInnerProd( &si[i], &wi[j] );
  zMatPutNC( inertia, rkLinkJointIDOffset(link), rkLinkJointIDOffset(link), hij );
  /* non-diagonal block */
  for( lp=rkLinkParent(link); lp; lp=rkLinkParent(lp) ){
    _zFrame3DXform( rkLinkWldFrame(link), rkLinkWldFrame(lp), &f );
    rkJointCRBXform( rkLinkJoint(lp), &f, si );
    zMatSetSizeNC( hij, rkLinkJointSize(lp), rkLinkJointSize(link) );
    for( i=0; i<rkLinkJointSize(lp); i++ )
      for( j=0; j<rkLinkJointSize(link); j++ )
        zMatElemNC(hij,i,j) = zVec6DInnerProd( &si[i], &wi[j] );
    zMatPutNC(  inertia, rkLinkJointIDOffset(lp), rkLinkJointIDOffset(link), hij );
    zMatTPutNC( inertia, rkLinkJointIDOffset(link), rkLinkJointIDOffset(lp), hij );
  }

  if( rkLinkChild(link) )
    _rkChainLinkInertiaMatCRB( rkLinkChild(link), wi, si, hij, inertia );
  if( rkLinkSibl(link) )
    _rkChainLinkInertiaMatCRB( rkLinkSibl(link), wi, si, hij, inertia );
}

/* compute the inertia matrix of a kinematic chain based on the composite rigid body method. */
bool rkChainInertiaMatCRB(rkChain *chain, zMat inertia)
{
  zMatStruct hij;
  zVec6D wi[6], si[6];
  double _e[36];

  rkChainUpdateCRB( chain );
  zMatBufNC(&hij) = _e;
  _rkChainLinkInertiaMatCRB( rkChainRoot(chain), wi, si, &hij, inertia );
  return true;
}

rkJointCRBWrench()は${}^{i}\hat{\boldsymbol{f}}_{i}$および${}^{i}\hat{\boldsymbol{n}}_{i}$，rkJointCRBXform()は${}^{i}\boldsymbol{S}_{j}$を，それぞれ求める関数です．これらは仮想関数で，実体は各関節クラスのメンバ関数として個別に実装することで高速化しています．関節クラスには固定関節（fixed），回転関節（revolute），直動関節（prismatic），円筒関節（cylindrical），自在継手（ユニバーサル関節 Hooke），球面関節（spherical），浮遊関節（float）を用意してあります．

$m_{i}^{\mathrm{C}}$の再帰的計算はrkLinkUpdateCRBMass()，${}^{i}\boldsymbol{p}_{\mathrm{G}i}^{\mathrm{C}}$および${}^{i}\boldsymbol{I}_{i}^{\mathrm{C}}$の再帰的計算はrkLinkUpdateCRB()でそれぞれ実装しています．

/* update mass of the composite rigit body of a link. */
double rkLinkUpdateCRBMass(rkLink *link)
{
  rkLink *l;

  if( !rkLinkChild(link) ) return rkMPMass( rkLinkCRB(link) );
  rkMPSetMass( rkLinkCRB(link), rkLinkMass(link) );
  for( l=rkLinkChild(link); l; l=rkLinkSibl(l) )
    rkMPMass( rkLinkCRB(link) ) += rkLinkUpdateCRBMass( l );
  return rkMPMass(rkLinkCRB(link));
}

/* update the composite rigit body of a link. */
rkMP *rkLinkUpdateCRB(rkLink *link)
{
  rkLink *l;
  zMat3D tmpi;
  zVec3D tmpr;

  if( !rkLinkChild(link) ) return rkLinkCRB(link);
  /* composite COM */
  _zVec3DMul( rkLinkCOM(link), rkLinkMass(link), rkMPCOM(rkLinkCRB(link)) );
  for( l=rkLinkChild(link); l; l=rkLinkSibl(l) ){
    rkLinkUpdateCRB( l );
    _zXform3D( rkLinkAdjFrame(l), rkMPCOM(rkLinkCRB(l)), &tmpr );
    _zVec3DCatDRC( rkMPCOM(rkLinkCRB(link)), rkMPMass(rkLinkCRB(l)), &tmpr );
  }
  zVec3DDivDRC( rkMPCOM(rkLinkCRB(link)), rkMPMass(rkLinkCRB(link)) );
  /* composite inertia */
  _zVec3DSub( rkLinkCOM(link), rkMPCOM(rkLinkCRB(link)), &tmpr );
  rkMPShiftInertia( rkLinkMP(link), &tmpr, rkMPInertia(rkLinkCRB(link)) );
  for( l=rkLinkChild(link); l; l=rkLinkSibl(l) ){
    zRotMat3D( rkLinkAdjAtt(l), rkMPInertia(rkLinkCRB(l)), &tmpi );
    _zXform3D( rkLinkAdjFrame(l), rkMPCOM(rkLinkCRB(l)), &tmpr );
    _zVec3DSubDRC( &tmpr, rkMPCOM(rkLinkCRB(link)) );
    zMat3DCatVec3DDoubleOuterProdDRC( &tmpi, -rkMPMass(rkLinkCRB(l)), &tmpr );
    zMat3DAddDRC( rkMPInertia(rkLinkCRB(link)), &tmpi );
  }
  return rkLinkCRB(link);
}

慣性行列の計算は，

rkChainInertiaMatCRB()（合成剛体法）
rkChainInertiaMatUV()（単位ベクトル法）
rkChainInertiaMatMJ()（ヤコビ行列を用いて直接的に求める方法）

を実装しています．後者二つの計算時間は合成剛体法の倍程度かかります．