フルボディIKのデモプログラムを作りました

2020/04/24ik typescript

デモ : https://gyojir.github.io/fullbody-ik-demo/
ソースコード : https://github.com/gyojir/fullbody-ik-demo/

WEB上で動作とソースコードを確認できる、ヤコビアンを用いたフルボディIKの実装が見当たらなかったので作ることにしました。

IKとは逆運動学のことでInverse Kinematicsの略です。手先位置・手先角度などの目的の拘束条件を満たすような関節角度(関節変数)を計算によって求めます。フルボディIKでは、肩から手先のように部分的にではなく全身に対して計算を行います。

ヤコビアンというのは簡単に言うと、ある関節を曲げた時に手先や足先がどう動くかというのをひとまとめ(行列)にしたもので、それを使うとIKの計算ができます。

なるべく理論が分かりやすいようにソースコードを書いたつもりなので、フルボディIKを実装したい人や理論を理解したい人が動作を確認しながらソースコードを読んで参考にしてもらえるとうれしいです。(理論的には一部テキトーな部分はありますが…)

機能

回転ジョイント
可動ルート(スライダジョイント)
位置拘束(速度ヤコビアン)
向き拘束(角速度ヤコビアン)
参照姿勢追随(バイアス付き最小ノルム解 or 姿勢速度ヤコビアン)
球面関節可動範囲
優先度付きタスク

これらができれば後は大体応用が利くと思います。

参考になったもの

理論 & 実装

ヤコビアンを用いた逆運動学 - 向井研究室

https://mukai-lab.org/content/JacobianInverseKinematics.pdf (URLが変わったようです)
https://mukai-lab.org/library/ik-legacy/

分かりやすいヤコビアンIKの理論と、シンプルなIKの実装が載っています。自分は初めにこのページで理論を読み、コードをコピペして色々試しました。

他の参考資料の実装では速度ヤコビアンの計算に外積が使われていることが多いのですが、この資料の実装ではヤコビアンの定義通りに座標変換に対して偏微分を行っています。個人的に外積の方は直観的で、偏微分の方は理論的なイメージです。一応、自分の実装では偏微分バージョンのヤコビアン計算と外積バージョンのヤコビアン計算の2種類の関数が書いてあります。 (ただ偏微分の方は遅いので使っていません)

津田順平, フルボディIKエンジンの作り方, CEDEC2011, 2011.

https://cedil.cesa.or.jp/cedil_sessions/view/613

ある程度理論が分かってくるとかなり参考になる資料です。フルボディIKについてとても分かりやすく全般的にまとめられています。今回の実装はほとんどこの資料に沿って作りました。
CEDiLに登録すれば見れます。

今回は重心制御は実装しませんでした。

UE4 向けの Fullbody IK プラグイン（β版）を公開しました - ほげたつブログ

http://hogetatu.hatenablog.com/entry/2017/08/31/180316

実装と参考資料が参考になりました。

三谷純他共著，Computer Graphics Gems JP 2012，ボーンデジタル，2012.

フルボディIKではないですが、IKの理論と実装が載っています。 [^ヤコビアンを用いた逆運動学 - 向井研究室]と同じ向井智彦さんが書いているので、大体同じことが書いてあります。

理論

吉川恒夫, ロボット制御基礎論, コロナ社, 1988.

[^CEDECの資料]でも参考資料として挙げられている資料です。ヤコビアンに関しては、基本的な理論や特異姿勢に関する話が書いてあります。ロボット分野の本なのでDHパラメータなどが出てきます。 (自分の実装ではDHパラメータは使っていません)

この本ではヤコビアン計算について外積を使って説明しています。この”外積を使ったヤコビアン計算”について、直観的には理解できていたのですが、直接触れられている資料があまり見つからなかったので助かりました。

山根, 中村, ヒューマンフィギアの全身運動生成のための協応構造化インタフェース, 日本ロボット学会誌,vol.20. no.3, pp.335-343. 2002.

[^CEDECの資料]でも挙げられている↓の資料
Yamane,K., Synergetic Choreography of Human Figures. Simulating and Generating Motions of Human Figures(Springer Tracts in Advanced Robotics). (2004) 107-119
と大体同じ内容の日本語論文です。(Synergetic Choreography… の方はWEBで見れなかったのでNDL ONLINEで取り寄せました)

球面関節の範囲制限や、優先度付きタスクについての詳細な解説が載っています。

【解説】一般逆行列

https://www.slideshare.net/wosugi/ss-79624897

擬似逆行列の導出・解説が載っています。とても分かりやすいです。

フルボディIKを勘違いしていた

実は自分、初めはかなり大きな勘違いをしていまして、単純なアーム型のIKは実装できてもフルボディIKの理論がさっぱり分かりませんでした。勘違いというのは

「アーム型ではルートから手先の範囲で偏微分したんだから、今度は左足から右手にかけて偏微分すればいいんだ！あれ？途中にルートがあるから単純な座標変換ができないぞ？？左ひざから手先へのベクトルと回転軸で外積すればいいのか？？？」

という感じで、全く見当違いのことを考えていました。しかし理解してみれば全然違って、

× 左足から右手にかけて偏微分
→ そもそも左ひざの回転は手先とは直接関係ないので、ルートから手先の範囲でヤコビアン計算して、それ以外は0でいい

ということでした。

「それだとフルボディIKにならないじゃないか！！」

昔の自分はまだ納得しないでしょう。しかし、フルボディIKで重要なのはそこではなくて拘束条件の方なのです。一つ一つの拘束条件に関するヤコビアンは、フルボディになっても変わりません。

つまり、
ヤコビアンを用いたフルボディIKというのは、足先位置や手先位置などの複数の拘束条件に関するヤコビアンを1つの行列に押し込むことで、複数の拘束条件を同時に満たす関節変数の値を求めること
なのです。

重要なのは、ヤコビ行列の一行(もしくは列)が、1つの拘束条件に関する変位(例:手先のx座標, 首のオイラー角Y, etc…)に対応していて、互いの行は全く関係ないこと(順番も!)です。拘束条件を増やしたければ単純にヤコビ行列の行を増やせばよくて、あとは擬似逆行列を求めてしまえば、拘束条件を満たすようないい感じの関節変数の値を取り出せてしまうわけです。拘束条件をあたかもモジュールのように簡単に付け外しできてしまうのです！

このポイントさえ理解できれば、結構やりたい放題できてしまうことが分かります。

冗長変数は面白い

ヤコビアンIKについて調べていると、冗長変数を使ったトリックがいくつも出てきます。これがなかなか面白くて、冗長変数を使うと拘束条件を満たしたまま結果をいじくりまわすことができるのです。これは冗長項がヤコビアンの零空間ベクトルであるために為せる業です。
これは他でも結構解説されていますが、一応冗長変数について自分なりの解釈を書いておこうと思います。ただ数学にあまり自信がないので間違いがあるかもしれません…

ヤコビアンの零空間ベクトルというのはどういうことかというと、まず求めたいのは

\dot{p} = J \dot{\theta}

のようなヤコビアンに対して $\dot{p}$ を与えた時の $\dot{\theta}$ です。 $m<n$ としたとき、通常は擬似逆行列を用いて $\dot{\theta}$ のノルムを最小にするような解を求めます。

ここで、 $J \bm{x} = \vec{0}$ となるようなベクトル $\bm{x}$ を考え( $J$ の零空間ベクトル)、 $\dot{\theta} = \dot{\theta_0} + \bm{x}$ に分解して、 $\theta_0$ のノルムが最小になるように解 $\dot{\theta}$ を求めることにすると

\dot{p} = J(\dot{\theta_0} + \bm{x})

\dot{p} = J\dot{\theta_0} + J\bm{x}

\dot{p} = J\dot{\theta_0}

となり、結局 $\bm{x}$ の項は無くなるので、普通に最小ノルム解を求めればよく

J^{\#} = J^T(JJ^T)^{-1}

\dot{\theta_0} = J^{\#} \dot{p}

\dot{\theta} = J^{\#} \dot{p} + \bm{x}

となります。つまり、 $J \bm{x} = \vec{0}$ を満たす $\bm{x}$ を使いさえすれば $\dot{\theta}$ の値を自由に制御することができるということです。では $J \bm{x} = \vec{0}$ を満たす $\bm{x}$ というのはどんなものかというと、以下の通りです。

\bm{x} = (I - J^{\#} J) \bm{z}

左から $J$ をかけると0になることが分かると思います。ここで、 $\bm{z}$ は冗長変数と呼ばれる任意のベクトルです。ということで、

\dot{\theta} = J^{\#} \dot{p} + (I - J^{\#} J) \bm{z}

これがいわゆる一般化擬似逆行列解となります。

冗長変数の使い道として最もシンプルなのは、[^Computer Graphics Gems JP 2012]で説明されているような関節角度の可動範囲制限です。冗長変数に以下のように範囲を超えた場合に戻すような値を入れることで範囲制限を実現できます。

\bm{z}_i = \begin{cases} \theta_{i min} - \theta_i &\text{if } \theta_i < \theta_{imin} \\ \theta_{i max} - \theta_i &\text{if } \theta_i > \theta_{imax} \end{cases}

冗長変数の使い道のさらに進んだ例としては、[^CEDECの資料]や[^ヒューマンフィギアの…]で説明されている、優先度付きタスクがあります。低優先度の拘束条件を冗長変数に押し込めることで、上で説明した可動範囲制限よりも表現力の高い制御を行うことができます。

冗長変数は活用していきましょう！

ImGuiは素晴らしい

デモプログラムではThree.jsを用いましたが、デバッグのGUIとしてはThree.jsに標準搭載されているdat.GUIではなくimgui-jsを使いました。 ImGuiはまさしくプロトタイピングに適したもので、非常に簡単にデバッグGUIを作ることができます。動的にUIの項目を増やしたりといったdat.GUIではやりにくいことも簡単にできますので、オススメです。

ただ、npmでインストールしたimgui-jsはnode.js環境では使いにくかったため、forkしたものをES2015にビルドしなおして使っています。

終わりに

もしやる気があればシンプルなIKからフルボディIKへのステップバイステップの実装と解説記事を書こうと思っています。

機能