WPGの開発

[Yusuke-Yamasaki-555]

参考書に準ずる。３次元線形倒立振子モデルで進めたい。

[Yusuke-Yamasaki-555]

まずは、理論の勉強を...

[Yusuke-Yamasaki-555]

息抜きに、静歩行を実装してみた。
静歩行issue
これにZMP観測とか、理論的なものを加えていく。
歩行パターンの決定も、この静歩行から辿れるだろう

[Yusuke-Yamasaki-555]

各関節の角速度を得るために、ヤコビ行列を実装した。合ってそうな値が得られたので、ひとまずOKということで。

以下、例。
""
ROBOTIS OP2の右脚の各関節角度[rad]。
{0, 0, -3.14/4, 3.14/2, -3.14/4, 0} (右手系)

この時、脚の末端を垂直に0.1[m/s]持ち上げたときの、各関節角速度。
0
0
-0.760633
1.52127
-0.760633
0

一応、このときのヤコビ行列とその逆行列。
ヤコビ行列
0 0 -0.131574 -0.0657871 0 0
0 0.131574 0 0 0 0
0 0 0 0.0657347 0 0
0 1 0 0 0 1
0 0 1 1 1 0
1 0 0 0 0 0

上記ヤコビ行列の逆行列
0 0 0 0 0 1
0 7.60027 0 0 0 0
-7.60027 0 -7.60633 0 0 0
0 0 15.2127 0 0 0
7.60027 -0 -7.60633 -0 1 -0
-0 -7.60027 -0 1 -0 -0

以下、例のイメージ図。

[Yusuke-Yamasaki-555]

これで、基準点（今回の場合は、重心と同位置）の位置・速度・角速度から、各関節の角度と角速度を求める手法が揃った（はず）。

角度 -> IK
角速度 -> ヤコビ行列を用いた計算（上記例では脚の末端の速度からだったが、その速度の符号を逆にしてやれば、胴体を基準にしたものになる（はず））

追記：
・遊脚：胴体の速度と、符号が同じ。
・支持脚：胴体の速度と、符号が逆。

[Yusuke-Yamasaki-555]

本格的な動歩行の実装に向けて、開発を勧めていく。

ROBOTIS OP2の重量・足裏から腰までの高さ。
・重量：3.0[kg]
・腰の高さ：219.5[mm]

これを元に、軌道エネルギーの式を立てて、歩行パラメータを構築する。そしたら、歩行パターン生成のための実装を進めていく。

実装方法は、はじめは動けば良いので、WebotsRobotHandlerに直接実装する。step()でリアルタイムに生成するのではなく、init()で歩行パターンを全て計算しておき、step()で順次パターンを読んで動かす方法を取る。

[Yusuke-Yamasaki-555]

動歩行の実装
#6
ができた。

なので、WPG nodeに移行・実装していく。

ついでに、リファクタリングとか整理をする。

[Yusuke-Yamasaki-555]

• ヤコビアンの関数、IK、FKを含めた、kinematicsライブラリを作成する。
  • ヤコビアンの関数の結果を、返り値で返すようにする。
  • kinematicsライブラリを作成し、ヤコビアンの関数を移行する。
    • kinematicsライブラリではなく、Jacobianライブラリに移行。kinematicsライブラリは全部を統合する形をとる。externを使うか？iostreamを参考に？
  • kinematicsライブラリにIKとFKを統合する。Jacobianも統合する。
• 動歩行の関数をWalking Pattern Generator nodeに移行して、node化する。
  • 動歩行の関数を整理する。
  • nodeに移行する。通信はPub/Subで。Serviceはめんどい。

[Yusuke-Yamasaki-555]

動歩行動作の生成をWPGへ移行できた。

ただ、問題が幾つか発生した。

• 1: LaunchするとWPGとWRH両方のnodeから、以下のWarningが出る。動作に支障は無い。同じPubを読んでいないのに。ggったら幾つか事例があったのでそこから解決していく。

Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.

• 2: データの受取Lossがある。Pub/SubでQoSも速度重視で設定しているから仕方がないと思う。あと、Pubを約10[ms]でしているが、コレをもっと正確に刻めるようになれば、ミスも減るだろう。
  • 全560回Pubをしている。Sub側の受取ミスは、30~40回ぐらい。5% ~ 7%ぐらいか。酷くて10%ぐらい。ミスするタイミングに偏りは見られず。
  • WPGに移行してから、ちょっとだけ歩行が乱れているように見える。データLossの影響だろう。
  • Lossを０にしたいので、Serviceの使用の検討。Lossを1%未満（1%に根拠は無い）に抑えられるならPub/Subでも良いかもだが。

[Yusuke-Yamasaki-555]

Pub/Subでの動歩行の動画。Lossのせいで乱れが大きい。
もちろん標準出力の原因も含まれる。

walking_no13_pub-sub.mp4

Service通信に移行するべき。

[Yusuke-Yamasaki-555]

WPG化は済んでいるが、pub/subのせいかdata lossがちょいある。service通信を検討するためにissueは閉じない。

QoSいじればなんとかなるか？これも検討

[Yusuke-Yamasaki-555]

あとアレだ。

動歩行の理論の部分を、もう一度筆記でまとめ直したほうが良いかも。で、どっかにちゃんとまとめて記録しておくべき。