optmotiongenにおけるbsplineを用いたdynamicな動作について

[MiraiHattori]

No description provided.

[MiraiHattori]

間違えて内容のないissueを送ってしまいましたが，追記という形で立てます．

@mmurooka さん，有識者の方々
optmotiongenでjaxonを用いてステップ，歩行及び走行を試そうとしていました．
ロボットに2歩のステップを踏ませるように制約を与えたのですが，
ロボットの重心軌道z方向がジャンプするような軌道を取り，計算がうまくいかなかったので質問させていただきたいです．
https://drive.google.com/file/d/1nhIIejdv_Mhr4Bhfg0ny-qtdNLJYFrJa/view?usp=sharing

こちらは，重心軌道はフリーフォールであることが想定されていて，離地と着地のタイミングを調整する必要があるということでしょうか．
また，重心がz方向に動くことによるzmp変化は考慮されているのでしょうか．
また，manualディレクトリにあるマニュアルによると運動方程式にしたがって計算されているようなのですが，
ロボットのダイナミクスはどの程度考慮されているのでしょうか．

ソースコードは，optmotiongenのサンプル
https://github.com/jsk-ros-pkg/jsk_control/blob/master/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-bspline-dynamic.gif
をjaxon用に改変し用いました．

貼った動画を撮影してからソースコードを改変してしまったのですが，一応改変したプログラムの場所は
https://github.com/future731/optmotiongen_sample/tree/master/euslisp
のjaxonディレクトリの下にあります．

参考: hrp2ではうまくいっているようにも見えます．
1歩
https://drive.google.com/file/d/17ix--JBksxt9lZzTDCNejALLXHGNa6-p/view?usp=sharing
https://github.com/future731/optmotiongen_sample/blob/master/euslisp/hrp2/hrp2-one-step.l

2歩
https://drive.google.com/file/d/19w8pLM8EHTq3otF8hpy7F_4lTuwcSRh_/view?usp=sharing
https://github.com/future731/optmotiongen_sample/blob/master/euslisp/hrp2/hrp2-two-step.l

[mmurooka]

参考: hrp2ではうまくいっているようにも見えます．

このプログラムで歩くような動作ちゃんと出るんですね，書いた本人も意外でした．
HRP2でできているプログラムをJAXONで試して上手く行かなかったということ？
それなら，本質的な誤りはなくて，チューニングの問題だと思います．

Bsplineを用いた動的動作は，optmotiongenの中でも一番アドバンストな内容であまり動作を確認できていないです．問題設定によってそれっぽい動作が出る時と全然変な動作しか出ない時があります．もともと非凸な最適化を解いているので，局所解しか得られないのは仕方ないけれど，まだ改善の余地はあるはず，という段階のプログラムです．中身を完全に理解して，誤りを見つけて直したり，最適化の収束性が向上するように，設計変数の初期値を変えたり，目的関数の正則化項を変えたり，というチューニングをやらないとお手軽に使えるようにはなっていないです．
（optmotiongenの他の機能はもっと手軽に使えると思いますが動的な動作は作れないですね．）

こちらは，重心軌道はフリーフォールであることが想定されていて，離地と着地のタイミングを調整する必要があるということでしょうか．

重心軌道にフリーフォール等の想定はないです．
指定したタスクと運動方程式を満たす軌道が自動的に生成されます．
（もちろん力を受けなければ，運動方程式通りに重力加速度で下に加速します．）

また，重心がz方向に動くことによるzmp変化は考慮されているのでしょうか．

重心の運動方程式が考慮されているので，重心がz方向に動くことによるzmp変化も考慮されていることになります．

また，manualディレクトリにあるマニュアルによると運動方程式にしたがって計算されているようなのですが，
ロボットのダイナミクスはどの程度考慮されているのでしょうか．

manualに書いてある式の通りです．重心の運動方程式が考慮されています．角運動量は考慮されていなかったと思います．

[MiraiHattori]

返信ありがとうございます．
角運動量のタスクとして， https://github.com/jsk-ros-pkg/jsk_control/blob/master/eus_qp/optmotiongen/manual/manual.pdf
の 78ページと83ページを見たのですが，角運動量のノルムが0になるように最適化されているということで正しそうでしょうか．

[mmurooka]

角運動量のノルムが0になるように最適化されているということで正しそうでしょうか．

はい．
ただ，式(4.319)の後に\bm{A}_{\theta}と\bm{A}_{\phi}（= Centroidal momentum matrix）をゼロとすると仮定しているので，関節速度に応じた各リンクの運動で生じる角運動量は無視されていることになります．つまり，最適化のコンフィギュレーションに含まれている角運動量(:ang-moment-control-vectorから得られる角運動量軌道）はゼロになるように最適化されていますが，実際のロボットの運動の角運動量はゼロになっているとは限らないです．これが実際にどのくらい問題になるのかは分かりませんが，当時の自分の中では要修正ポイントとして残っていました．実装にはCentroidal momentum matrixの関節角度による微分が必要になって，結構大変なはずです．

[MiraiHattori]

返信ありがとうございます．

Centroidal momentum matrixの関節角度微分を実装するのは計算量的にもだいぶ大きそうですね．

先ほど，hrp2でうまくいった歩行っぽい動作がjaxonで動かない件が解決しました．設定ファイルのlistの書き方を間違えていたようです．

ソースコードは，optmotiongenのサンプル
https://github.com/jsk-ros-pkg/jsk_control/blob/master/eus_qp/optmotiongen/doc/images/sample-sqp-optimization-bspline-dynamic.gif
をjaxon用に改変し用いました．

(list (A) (B))と書くべきところを((list A) (list B))と書いていたのを直したところ，hrp2と同じように歩くモーションを生成できました．choreonoidや実機ではまだ試していないですが，それっぽいモーションができていそうです．