Gazeboによるシュミレーションで物体をつかめるようにしたい

[y-masutani]

現状の問題点

このリポジトリで提供されているCRANE+のモデルを使って，Gazeboでシミュレーションをしていますが，机の上に置いた物体をグリッパで把持しようとしても物体が跳ね飛んでしまいます．

改善案

もし，Gazebo上でグリッパによる物体の把持の実績がありましたら，その具体的な設定やノウハウをリポジトリに追加していただけると嬉しいです．

代替案

その他

パラメータを色々変化させて試しているのですが，一向にうまく行きません．

[ShotaAk]

今のcrane_plusパッケージでは、gazebo上での物体把持に成功していません。

ハードウェアとコントローラの種類をpositionからeffortに変更する対策がありますが、
実機とgazeboで同じコードを実行できなくなる可能性もあるため、調査中です。

[y-masutani]

色々試してみると，跳ね飛ぶ場合もあるのですが，添付の画像のようにめり込む場合もあります．Contactsを表示するようにしていますが，木片とグリッパの間には球が表示されていません．そもそも接触の判定がうまくいっていないのかもしれません．このような症状は出ていませんか？

[ShotaAk]

現時点でのxacroファイルのパラメータでは物体のすり抜けを確認できてません。

念の為コリジョンモデルも確認しましたが、グリッパの先端まで衝突判定が存在してます。

[y-masutani]

そうですか，すり抜け（めり込み）はそちらでは再現しませんか…
そちらではグリッパでものをつかもうとすると，どうなりますか？
接触して跳ね飛びますか？それとも，接触は安定していて滑りますか？
グリッパが滑るという事例は他でも見かけますが，めり込んだり跳ね飛んだりという事例には出会っていません．
当方のGazeboの設定がなにか悪いのかもしれません．

[ShotaAk]

こちらで確認した現象としては以下のとおりです。

• グリッパが箱に接触する程度にグリッパ角度を閉じると、グリッパと箱が滑る
• グリッパをさらに閉じると、グリッパ上で箱が振動する
• グリッパの角度を閉じすぎると箱がはね跳ぶ

[y-masutani]

確認していただきありがとうございます．こちらの設定と何かが違うのでしょうね．
こちらのGazeboのバージョンは，Help→Aboutで
Gazebo multi-robot simulator, version 11.10.2
と表示されます．
table.worldでphysicsの設定はされていますか？

[y-masutani]

こちらでは，crane_plus_arm_controller/joint_trajectory, crane_plus_gripper_controller/joint_trajectory トピックへ，1時刻分だけのJointTrajectoryメッセージをパブリッシュして動かしています．MoveItは使っていません．
気になっているのは，関節のトルクが足りない振る舞いをすることです．重力に負けて定常偏差が生じます．

[y-masutani]

試しに重力の値を1/10にしてみました．

    <gravity>0.0 0.0 -0.981</gravity>

するといい感じです．めり込みはなくなりました．Contactsにチェックを入れると，グリッパと対象物体の間に青い球が表示されるようになりました．グリッパで挟むこともできています．持ち上げようとすると滑りますが…
この状況をどのように考えればいいのでしょうか？

[ShotaAk]

確認していただきありがとうございます．こちらの設定と何かが違うのでしょうね． こちらのGazeboのバージョンは，Help→Aboutで Gazebo multi-robot simulator, version 11.10.2 と表示されます． table.worldでphysicsの設定はされていますか？

こちらも同じくver11.10.2のGazeboを使用しています。

URDFやworldファイル等はデフォルトの状態です。
（箱を追加するため、table.worldのコメントを解除してます）

[ShotaAk]

こちらでは，crane_plus_arm_controller/joint_trajectory, crane_plus_gripper_controller/joint_trajectory トピックへ，1時刻分だけのJointTrajectoryメッセージをパブリッシュして動かしています．MoveItは使っていません． 気になっているのは，関節のトルクが足りない振る舞いをすることです．重力に負けて定常偏差が生じます．

実行コマンド例を添付していただけると、こちらでも確認できます。

また、MoveItを使用しない&軌道も考慮しない場合は、TrajectoryControllerではなく
position_controllers/JointGroupPositionControllerを使用することをおすすめします。
こちらは目標角度をdouble型の配列として指示できます。

https://github.com/ros-controls/ros2_controllers/tree/master/position_controllers

[y-masutani]

実行コマンド例を添付していただけると、こちらでも確認できます。

crane_plusのパッケージを利用するlaunchファイルとPythonのノードを作っています．
https://github.com/AI-Robot-Book/chapter6/tree/master/crane_plus_commander

crane_plusのパッケージは既に使えるようになっているという前提で，以下のようにしていただくと，こちらで試しているのと同じ状態を確認できると思います．

cd ~/ワークスペース/src
git clone https://github.com/AI-Robot-Book/chapter2.git
git clone https://github.com/AI-Robot-Book/chapter6.git
cd ~/ワークスペース
colcon build --packages-select airobot_interfaces crane_plus_commander

端末1

cd ~/ワークスペース
source install/setup.bash
ros2 launch crane_plus_commander crane_plus_gazebo_no_moveit.launch.py

端末2

cd ~/ワークスペース
source install/setup.bash
ros2 run crane_plus_commander commander2

[y-masutani]

また、MoveItを使用しない&軌道も考慮しない場合は、TrajectoryControllerではなく
position_controllers/JointGroupPositionControllerを使用することをおすすめします。
こちらは目標角度をdouble型の配列として指示できます。

情報ありがとうございます．
crane_plusのパッケージをそのまま利用するというこを重視していたため，他の可能性を探ることを怠っていました．
JointGroupPositionController も試してみます．

[ShotaAk]

実行環境の情報提供ありがとうございます

本件に関連しますがgazeboに代わるシミュレータの、Ignition gazeboでの動作も調査中です。

こちらではURDFを書き換えることなく箱を持ち上げられたため、gazeboよりシミュレーションがしやすいかもしれません。
動作確認でき次第、本リポジトリにコードをpushする予定です。

ignition.mp4

[ShotaAk]

提供していただいたパッケージは正常に動作しました。
挙動に違いは見られませんでした。
箱を掴もうとすると物体が跳ねます。

-2022-06-06_18.46.18.mp4

[y-masutani]

本件に関連しますがgazeboに代わるシミュレータの、Ignition gazeboでの動作も調査中です。

こちらではURDFを書き換えることなく箱を持ち上げられたため、gazeboよりシミュレーションがしやすいかもしれません。
動作確認でき次第、本リポジトリにコードをpushする予定です。

そうなんですね！！
動力学のエンジンが違うのですか？
俄然，Ignition gazeboに興味を持ちました．試したくなりました．
インストールするのは，Gazebo Fortressのビルド済みのパッケージで大丈夫でしょうか？
https://gazebosim.org/docs/fortress/install_ubuntu
従来のGazeboと共存できますか？
モデルの互換性はどれぐらいあるのでしょうか？プラグインは同じでいけますか？

すいません．crane_plusのパッケージと直接関係ないことを書いてしまいました．
良い情報源や事例があれば教えてください．

[y-masutani]

提供していただいたパッケージは正常に動作しました。
挙動に違いは見られませんでした。
箱を掴もうとすると物体が跳ねます。

確認ありがとうございます．動画も拝見しました．こちらの挙動とずいぶん違います．
特別なことはしていないつもりなんですが，Gazeboやcrane_plusのインストールや設定をやり直してみます．

[ShotaAk]

本issueの題目から離れてしまうので、ignitionの話はほどほどにします。

ROSとの連携が重要なので、どのバージョンが適切なのかは調査中です。（Foxyだと Citadel?）
crane_plusでの対応について、もう少々おまちください。
https://github.com/gazebosim/ros_gz

[y-masutani]

確認ありがとうございます．動画も拝見しました．こちらの挙動とずいぶん違います．
特別なことはしていないつもりなんですが，Gazeboやcrane_plusのインストールや設定をやり直してみます．

再度試してみました．

対象としているwood_cube_5cmの位置が，table.world内でコメントアウトしているアームの土台に近い

      <pose>0 -0.1 1.0 0 0 0</pose>

の場合ですと，そちらで試していただいたビデオと似たような挙動でした．Contactsの青い球がしっかり表示され，めり込みも生じません，持ち上がりませんが，把持したような状態にできます．

一方，私は今まで，wood_cube_5cmの位置を

      <pose>0.25 0.0 1.015 0 0 0</pose>

にしていました，グリッパを水平にして腕を伸ばし，対象を横から把持するという想定です．この場合は，Contactsの青い球が表示されなかったり，明らかなめり込みが生じます．また，把持と関係あるのかわかりませんが，アームが重力に負けて目標の姿勢を維持できません．

恐れ入りますが，このような対象位置の場合に，そちらでどうなるか試してみていただけないでしょうか．
よろしくお願いいたします．

[ShotaAk]

状況確認できました。
おそらく下記の画像のようにリンク同士が離れたような挙動をしていると思います。

この問題はgazebo_ros2_controlでも取り上げられており、今現在も解決してません。:cry:

ros-controls/gazebo_ros2_control#73

[y-masutani]

はい．関節がはずれる挙動はしばしば遭遇します．
そのことは，接触判定がうまくいかないことや，重力に負けてしまうことと関係しているのでしょうか？

[ShotaAk]

そのことは，接触判定がうまくいかないことや，重力に負けてしまうことと関係しているのでしょうか？

原因まではわかりませんが、関連していると思われます。
私の環境でも、腕を伸ばしたときは手先が箱をすり抜ける現象が再現しました。

[y-masutani]

私の環境でも、腕を伸ばしたときは手先が箱をすり抜ける現象が再現しました。

確認ありがとうございます．こちらの環境の問題ではないことがわかり安心しました．

crane_plus_ignition パッケージをまだ試していませんが，Ignitionにすると関節がはずれたり，手先が箱にめり込む問題は解消されますか？

[ShotaAk]

Ignitionにすると関節がはずれたり，手先が箱にめり込む問題は解消されますか？

上記の症状はignitionでは発生していません。

support_ignitionブランチに切り替えるとお試しいただけます。

#38
にてレビュー中ですので、masterへのマージはもう少々お待ちください。

[ShotaAk]

crane_plus_ignitionをmasterブランチへマージしました。
crane_plus_gazeboの代わりに、crane_plus_ignitionを使用することを推奨します。

実行手順は下記READMEに記載しています。
https://github.com/rt-net/crane_plus/tree/1.0.0/crane_plus_ignition

[y-masutani]

応答が遅くなり，申し訳ありません．
crane_plus_ignitionの動作確認ができました．関節の不可解な現象はなくなり，特に設定を変更することなく，グリッパで物体を把持して持ち上げることができるようになりました．しかし，めり込みはかなり生じています．この状態でそのままにしておくと，Ignition Gazeboが落ちてしまいます．

[y-masutani]

細かいことですが，

• https://github.com/rt-net/crane_plus/blob/master/crane_plus_ignition/README.md にtypoがあります．
  crane_plus_ignitionw → crane_plus_ignition
• Gazeboの場合と同じように，起動時のカメラのポースを適切に設定できないでしょうか？
• Gazeboの場合に比べて画角がかなり大きいですね．

[ShotaAk]

ご報告ありがとうございます。
それぞれ、Ignitionに関することであり、本issueの内容から外れるため、別途issueを立てます。

[ShotaAk]

シミュレーションをIgnitionに移行したため、本issueをクローズします。