Skip to content →

Gazeboの「重力バグ」

coord.eです。忘れそうなので進捗より先にこっち書きます。ごめんなさい。

この記事の至るところにバグと書いていますが、あながちバグといったものではないのかもしれません。しかし望んだ動作が得られないことは確かなので、とりあえずバグと書いています。


タイトルに「Gazeboの」と書きました。正確には、このバグが起きる条件は

・GazeboのJoint::SetPosition関数を_preserveWorldVelocityをtrueにせずに呼んだとき

です。

これが実際いつ起きうるかというと、

・ROSとの連携で"position_controllers/PositionJointController"pid_gainsを指定しないで使用した場合

です。

Bulletを直接使うようになる前(Gazeboを使っていた時)、私達はこの地雷を見事に踏んでいました。

いろいろと調査したので、ここにまとめてみようと思います。

どのような問題なのか

http://answers.gazebosim.org/question/18684/mobile-arm-simulation-behaves-strangely-when-adding-ros_control-plugin/

上の質問・回答が一番わかり易いと思います。

つまりros_controlプラグインを使ったときに落下がおかしいよってことです。

詳しく言うと、Joint::SetPosition関数が今回の地雷なのですが、それを呼び出すのが"position_controllers/PositionJointController"です。(ここら)

上のページでは、ワークアラウンドとして次の2つが挙げられています。

  1. EffortJointInterface/VelocityJointInterfaceを使う
  2. /gazebo_ros_control/pid_gains/を設定する

実際どちらでも治ります。どちらでもSetPositionは呼ばれません。(1., 2.)

ですが1.はサーボを位置制御しているYamaXでは論外ですし2.もPID制御なしの純粋な位置制御をしたいときに困ります。

位置制御しながら解決する方法は?

ではどうしたら良いのでしょう。Gazebo8ぐらいまではどうしようもなかったのですが、Gazebo9からJoint::SetPosition_preserveWorldVelocity引数が追加されました。

その経緯は下のIssue, PRに書いてあります。

  1. https://bitbucket.org/osrf/gazebo/issues/2111/joint-setposition-alterate-the-simulation
  2. https://bitbucket.org/osrf/gazebo/pull-requests/2598/fix2111
  3. https://bitbucket.org/osrf/gazebo/pull-requests/2814

もともとそのままこの挙動を修正する予定だったようです(1. 2.)

ですが、どうも今までの挙動にも使いどころがあるらしく、設定を変えずに挙動が変わっては困るということで_preserveWorldVelocityフラグが導入されることになったようです。これをtrueにすると修正されると。(3.)

まあそこまではいいんですが、_preserveWorldVelocity、デフォルトがfalse

しかもこれをURDFやroslaunchなどから設定する術はなさそうです。(http://answers.gazebosim.org/question/18708/how-can-i-set-the-preserveworldvelocity-flag-in-gazebo-9-hopefully-through-roslaunch-and-urdf/)

つまり、自前でビルドしろと言うことですね★

ソースから修正する

そこで必要な修正をこのパッチに書きました。(Dockerコンテナ作成時にパッチを当ててビルドしています)

少しパッチから抜粋すると、

このようにデフォルト値をTrueにしています。

またどこかから明示的にfalseを投げてられていることがあったので、下のような修正も施しました。

人として最低な変更ですね。まあ動きゃいいんだよ精神です。

ということでこのパッチを使用する際は細心の注意を払ってください。何気に他のバグを誘発するようなことしてます。

 

治った…?

先日の記事にもあったこの動画です。

左が改造前、右が改造後です。

落下のおかしさはこれで治りました。めでたしめでたし。

ちょっと待てよ

はい。鋭い方はとっくにお気づきかと思いますが、gazebo_ros_control_preserveWorldVelocitytrueにしています。

じゃあだれがfalseを投げていたんだって話ですが、謎ですしもう動きはしましたし正直Gazeboのことはもう考えたくないので興味ある方いらっしゃいましたら探ってみてください。きっとさらなる闇が見えると思います(ごめんなさい)

参考文献

https://github.com/ros-simulation/gazebo_ros_pkgs/issues/612

https://github.com/ros-simulation/gazebo_ros_pkgs/issues/479

http://answers.gazebosim.org/question/18684/mobile-arm-simulation-behaves-strangely-when-adding-ros_control-plugin/

http://answers.gazebosim.org/question/17281/problems-with-model-simulation-behaves-like-there-is-moon-gravity-solved/

http://answers.gazebosim.org/question/15554/enabling-gazebo_ros_control-plugin-changes-physics-response-of-entire-body/

https://bitbucket.org/osrf/gazebo/issues/2111/joint-setposition-alterate-the-simulation

https://bitbucket.org/osrf/gazebo/pull-requests/2598/fix2111

preserveWorldVelocity関連

http://answers.gazebosim.org/question/18708/how-can-i-set-the-preserveworldvelocity-flag-in-gazebo-9-hopefully-through-roslaunch-and-urdf/
https://bitbucket.org/osrf/gazebo/pull-requests/2814

Published in DeepL2 YamaX シミュレーション ソフトウェア