gazeboでhrp2が歩かない問題

[otsubo]

先週の研究会で少し話にあった内容なのですが,最近gazeboでhrp2が歩かないようになってた
問題を少し調べていて,start-jsk/rtmros_gazebo#217 このイシューの最後でご指摘していただいた通り,#419 このプルリクぐらいまで戻すと,問題なく歩きました.何が原因かはっきりわかっていないのですが,何かわかりましたら,またご報告いたします.

[mmurooka]

ホントですね．最新版だと歩いてすぐ転ぶけれど，#419 まで戻すと問題なく歩きます．

[mmurooka]

@k-okada, @YoheiKakiuchi , @snozawa

hrpsys起動時に，hrp2_hrpsys_config.py でstのパラメータをセットしていますが，
セットすると転んで，セットしなければ（デフォルトのままにすれば）歩けることがわかりました．
（rtmros_tutorialsのバージョンを戻して歩けていた理由は，
前のバージョンではstのidlが合わなくてstのパラメータをセットできなかったからでした．）

rtmros_common, rtmros_tutorials, rtmros_gazeboは最新で，
例えばhrp2jsknt(hrp2017)の場合は
https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py#L144
の一行ををコメントアウトすれば歩けます．

[mmurooka]

stのパラメータをセットして転ぶ
（origin/masterそのまま）
https://drive.google.com/a/jsk.imi.i.u-tokyo.ac.jp/file/d/0B5qanGnXorOuY2t3S3NYT2ZmbG8/view?usp=sharing

stのパラメータをセットしないで歩ける
（origin/masterからhttps://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py#L144 をコメントアウト）
https://drive.google.com/a/jsk.imi.i.u-tokyo.ac.jp/file/d/0B5qanGnXorOuUGFhVVNCT1FvMEE/view?usp=sharing

[snozawa]

@otsubo さん
ごめんなさい、こちら見落としてました。

@mmurooka さん
現象としては、

• #419まで戻しても、hrpsys_ros_bridge_tutorialsのpython設定が実機と同じ状態だと歩かない
• #419まで戻してかつpython設定を行わないと歩く

というのであってますか？

動いているときに
(send *ri* :get-st-algorithm)
を実行してみてもらえると助かります。

TPCCとかが帰ってくる場合、このSTの制御則であるいているということになります。
以前から、辛うじてgazeboであるけているのが確認されてるのはこのモードで、
今の実機とhrpsys-simulator系はTPCC以外の制御則であるいているので、
TPCC以外＋実機と同じパラメタで何とか歩かないか、というのがこれまでの経緯でした。
なので、TPCC以外＋実機と同じパラメタで歩けるとうれしい。

また、歩けない条件のときは、具体的にどのコマンドをうつとどうなりますか？
(send *ri* :start-st)
の時点でこける？発振する？
歩くまではなんとか持ちこたえてて歩くとダメ？

[mmurooka]

現象としては、
#419まで戻しても、hrpsys_ros_bridge_tutorialsのpython設定が実機と同じ状態だと歩かない
#419まで戻してかつpython設定を行わないと歩く
というのであってますか？

はい，だいたいそうなります．
#419 まで戻すとhrp2_hrpsys_config.pyでセットするstのパラメータが最新のhrpsysと異なるため（stのパラメータの中でemergency stopのマージンがスカラーかベクトルか異なる），その状態ではstの設定を変えることはできないです．

• #419まで戻して，hrp2_hrpsys_config.pyのみ最新にして実機と同じ設定にすると歩かない
• #419まで戻してかつpython設定を行わないと歩く（#419にした時点でstのパラメータは異なるのでそのままでもエラーでセットされない）

ことを確認しました．

動いているときに
(send ri :get-st-algorithm)
を実行してみてもらえると助かります。

tpccでした．このときの歩き方は少し試した限りではあんまり危なげではなさそうでした．

転び方は，
https://drive.google.com/file/d/0B5qanGnXorOuY2t3S3NYT2ZmbG8/view
の45秒あたりのように，start-stをした時点で後ろ向きや前向きに足首が回って転びます．

[snozawa]

#419まで戻して，hrp2_hrpsys_config.pyのみ最新にして実機と同じ設定にすると歩かない

これは、適切にpythonセッティングをしてパラメタがセットされるのを確認しても
あるかないということだよね？

#419 まで戻すとhrp2_hrpsys_config.pyでセットするstのパラメータが最新のhrpsysと異なるため（stのパラメータの中でemergency stopのマージンがスカラーかベクトルか異なる），その状態ではstの設定を変えることはできないです．

これがよくわからなかったんだけど、#419はrtmros_gazeboのPRであって、
hrpsysや、hrp2_hrpsys_config.pyといったものとリポジトリが違うので、
rtmros_gazeboを向かしにもどしてもhrpsysやhrp2_config_pyは変わらないので
こういう状況が起こらないと思うんだけど、違うのかな？

[mmurooka]

これは、適切にpythonセッティングをしてパラメタがセットされるのを確認しても
あるかないということだよね？

そうです．

これがよくわからなかったんだけど、#419はrtmros_gazeboのPRであって、
hrpsysや、hrp2_hrpsys_config.pyといったものとリポジトリが違うので、
rtmros_gazeboを向かしにもどしてもhrpsysやhrp2_config_pyは変わらないので
こういう状況が起こらないと思うんだけど、違うのかな？

rtmros_tutorialsのPRです．（hrpsys_gazebo_tutorialsはrtmros_tutorialsにあります．）
という理解でもう一度読み返していただければお分かりいただけるかと思います．

[snozawa]

rtmros_tutorialsのPRです．（hrpsys_gazebo_tutorialsはrtmros_tutorialsにあります．）
という理解でもう一度読み返していただければお分かりいただけるかと思います．

なるほど、わかりました。

[iory]

具体的にgazeboを使って歩くことを試してみました．
https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py#L82
https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py#L181
https://github.com/start-jsk/rtmros_tutorials/blob/master/hrpsys_ros_bridge_tutorials/src/hrpsys_ros_bridge_tutorials/hrp2_hrpsys_config.py#L278
の
stp.st_algorithm=OpenHRP.StabilizerService.EEFMQPCOPを
stp.st_algorithm=OpenHRP.StabilizerService.TTPCに変更後，gazeboを立ち上げる

roslaunch hrpsys_gazebo_tutorials gazebo_hrp2jsknts_no_controllers.launch
rtmlaunch hrpsys_gazebo_tutorials hrp2jsknts_hrpsys_bringup.launch

roseusで

roseus
(load "package://hrpsys_ros_bridge_tutorials/euslisp/hrp2jsknts-interface.l")
(hrp2jsknts-init)
(setq *robot* *hrp2jsknts*)
(send *ri* :start-st)
(send *ri* :angle-vector (send *robot* :angle-vector) 4000)
(send *ri* :start-auto-balancer)
(send *ri* :go-pos 1 0 0)

で歩きます．start-stの前にreset-poseにすると，何故かHRP2が回転してしまいますので，
順番が大事です．

[snozawa]

これ何目標でやってますか？
なんでも良いのでgazeboでgo-posくらいができる動画がとれればよいのと、普通にあるけるようにするのと、
普通に実機でやっているデモなどがうごいて透過的に使えるようにするのとで、色々違ってくるかなと思います。
それで、TPCCは実機では使われてなくて、また実機のEEFMほげほげのような制御モードとけっこう違いがあります。
後者２つもしくは一番最後の項目くらいを狙うなら、
「歩きました」＝「実機と同じ制御構成、（ほぼ）同じパラメタで歩きました」を調べたら良いと思います。
（後者２つが目標なら、実機いろいろ違うものを作って、動きます、というのは避けたいところ）

#503 (comment)
をはじめみたときは、kinematicsモードみたいなのを摘要する前に戻したら
いいかんじになるのかと思ったけど、後々きいてそうじゃないらしい、というのが分かりました。

start-stの前にreset-poseにすると，何故かHRP2が回転してしまいますので，順番が大事です．

これはなんでだろうね。
前より状況が悪化してそうですね。
パラメタが何があって、何を調整してみてるかを共有すると良さそうですね。

[snozawa]

あと、動画もあるとわかりやすそうですね。

[iory]

これ何目標でやってますか？

動作獲得の学習のために，simulator上で試行ができるといいと思っているので，
そこが目標です．プラス，他のデモなどもgazebo上で再現できるようになるといいです．
なので，後者２つが目標です．

なので，

「歩きました」＝「実機と同じ制御構成、（ほぼ）同じパラメタで歩きました」を調べたら良いと思います。

をやっていくのかと思います．

これはなんでだろうね。
前より状況が悪化してそうですね。

reset-pose -> st 回転する
reset-pose -> st -> auto-balancer 回転しない
https://drive.google.com/open?id=0B_NiLAzvehC9TjkxcXMwemt6bmc
ということがわかりました．

ttpcで，st -> auto-balancer -> gopos
https://drive.google.com/open?id=0B_NiLAzvehC9bl9aOG12X1BZeGM

EEFMQPCOP -> auto-balancer -> gopos
https://drive.google.com/open?id=0B_NiLAzvehC9NS1zYWhFY19pZW8

[snozawa]

  <world name="default">
    <physics type="ode">
      <gravity>0 0 -9.80665</gravity>
      <max_step_size>0.001</max_step_size>
      <real_time_factor>1</real_time_factor>
      <real_time_update_rate>1000</real_time_update_rate>
      <ode>
        <solver>
          <type>world</type>
        </solver>
        <constraints>
          <cfm>1e-7</cfm>
          <erp>1e-2</erp>
        </constraints>
      </ode>
    </physics>
  </world>

ODEのquickをworldにするのを試したことがないので、
上記を適宜empty.worldかなにかにうつしてためせたりするかな？
http://answers.gazebosim.org/question/232/gazebo-physics-solver-type-quick-vs-world/

[snozawa]

動作獲得の学習のために，simulator上で試行ができるといいと思っているので，そこが目標です．

これいいですね、ぜひやりましょう。

あと先ほど口頭で伝えましたが
start-auto-balancer->start-stの純で起動してみてください

[iory]

ODEのquickをworldにするのを試したことがないので、
上記を適宜empty.worldかなにかにうつしてためせたりするかな？
start-auto-balancer->start-stの純で起動してみてください

quick -> worldに変更してみましたが，
EEFMQPCOPの挙動は変わらず，auto-balancerを入れても倒れず，
次にstを入れた時点で倒れました．

@YoheiKakiuchi さんから聞いたことをメモとして書きますと，
choreonoidだと足裏と地面の間にブッシュ要素があって，
それを入れないと歩かないということでした．
あとは，inertiaのリンク間の差が0.001 ~ 1000倍以下でないと，
加速度がかかった時に大きな力がかかり，その部分が吹っ飛び，それにつられて体全体も，
飛んでしまうということでした．
ですが，このinertiaのチェックは行っているとのことなので，
あとは他のパラメータの調整になるのかなと思います．