初めまして、
先にGithubにIssueを立ててしまったのですが、その後にdiscouseの方が活発に議論が行われているとのことを知ったため、こちらでも質問させていただきます。
calcMassMatrix関数を用いた慣性行列計算において、rotorInertiaが2回足されているように思います。まず、
の中でやっている逆動力学演算A_G * delta_i + b_G
(梶田先生教科書第2版6.40式)
で、rotorInertiaを含んだ形でA_G * delta_i + b_G
を計算しています。その後、
で、A_G * delta_i + b_G
のi成分にさらにrotorInertiaを足しています。
順動力学計算コード
あたりを眺めると、慣性行列にrotorInertiaを1回足す方が順動力学計算結果と整合性が取れるように思うのですが、いかがでしょうか?