実は、週末から次の週末まで、勤続20年のリフレッシュ休暇なんです！
娘の学校があるから、週末に一泊で大阪ＵＳＪまで旅行には行きましたが、後は自宅警備員（予定）です。

と言う理由で、先々週から取り組んでいる逆動力学部分を仕上げちゃいました！！
ちなみに、もちろんNEON化してあるので、91ステップ、23自由度の計算で13ms程度でした。
関節サーボのフィードフォワード用に使うので、１制御周期に１ステップ分計算すれば良いので
本当は計算速度とか、あまり関係ないのだけどね。１ステップ150μsくらいですか。

逆動力学のNEON化は、やはりできるだけ使うレジスタを減らすために、積和演算を多用しています。
計算順序も工夫して、だから、デバックも大変！！
コードを印刷してマーカーでチェックして、レジスタの運用もフロー書いて、
シミュレーションの途中の値をとって、エクセル上で一部再現計算して、
１ステップ分の関節トルクも、エクセルで静止時トルク＋慣性モーメントも考慮してオーダーチェックして。
ふぅ、疲れた・・・。まあ、大丈夫かな？（＾＾；；；

「ヒューマノイドロボット改定２版」にオープンソースのロボットシミュレータ「コレオノイド」のことが書かれていた。
Ｃ＋＋でコーディングされているらしいから、今度時間があるときに、自分の関節トルクシミュレータと計算結果のチェックをしてみたいな。
時間があったら・・・。

関節トルクまで計算できたから、やっぱり、次は、実機を動かしたいよね。
大量のシミュレーション計算と、サーボ角度指定を同時に「pthread」で処理する。