ROS2, Gazebo Classic上で把持をする為の設定(Turtlebot Manipulation)

はじめに

Gazebo Classicによるシミュレーション環境内で、把持を行います。
Gazebo(旧Ignition Gazebo)ではそのままの設定で把持出来るという情報がありますが、Classicでは把持に関する機能は備わっていません。
本記事では、Gazebo Classic内で把持を行うための方法、及び使用したライブラリについてまとめます。
環境を以下に示します。

ROSの動作環境	使用したイメージ	Docker内OS	ROSバージョン
Dockerコンテナ内	osrf/ros:humble-desktop	Ubuntu 22.04	ROS2 Humble

実際に把持した様子は以下のようになります。

把持を行うまでの試行

今回把持に成功するまでに、複数の方法を試しました。今回試した方法を以下に示します。

• Gazebo(旧Ignition Gazebo)の使用
• 摩擦係数の設定
• アームと対象物の間に固定ジョイントを作成

今回試した方法

複数の方法を調べる中で、気になるライブラリを見つけました。

このライブラリでは、gazebo_grasp_pluginというものがあり、正に把持のためのGazeboプラグインを提供していました。今回はこのプラグインを使用することにしました。
リンクから飛ぶことが出来るリポジトリではROS2用は提供されていませんが、他の人が作成したものの、PRを送っていない(？)Humbleブランチがあります。

そのため今回は以下に示す、他の人の作成したブランチを使用します。

このブランチを使用しますが、ビルドツールの指定等いくつか編集すべき点があるので、変更点を示して、使い方をまとめます。
まず、cloneしてくる必要があるのでcloneします。

git clone -b humble https://github.com/kongoncharuk/gazebo-pkgs

元リポジトリから変更する点

プラグインをROS2で使用する際に、予め変更する点を示します。編集内容としては、ROS1のビルドツールを指定している部分をROS2用に変更します。
この項で編集するファイルは、以下の2つです。

• gazebo_grasp_plugin/CMakeLists.txt
• gazebo_grasp_plugin/package.xml

まず、gazebo_grasp_plugin/CMakeLists.txtの編集内容を示します。

62行目

install(TARGETS gazebo_grasp_fix gazebo_grasp_msgs
-   ARCHIVE DESTINATION lib/${PROJECT_NAME}
-   LIBRARY DESTINATION lib/${PROJECT_NAME}
-   RUNTIME DESTINATION bin/${PROJECT_NAME}
+   ARCHIVE DESTINATION lib
+   LIBRARY DESTINATION lib
+   RUNTIME DESTINATION bin 
)

76行目

install(DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}
-   DESTINATION include/${PROJECT_NAME}
+   DESTINATION include
  　FILES_MATCHING PATTERN "*.pb.h"
)

次に、gazebo_grasp_plugin/package.xmlの編集内容を示します。

<!-- The *_depend tags are used to specify dependencies -->
  <!-- Dependencies can be catkin packages or system dependencies -->
- <buildtool_depend>catkin</buildtool_depend>
+ <buildtool_depend>ament_cmake</buildtool_depend>
  <build_depend>gazebo_ros</build_depend>
  <build_depend>gazebo</build_depend>
  <build_depend>geometry_msgs</build_depend>
- <build_depend>roscpp</build_depend>
+ <build_depend>rclcpp</build_depend>
  <build_depend>std_msgs</build_depend>
  <build_depend>gazebo_version_helpers</build_depend>
  <run_depend>gazebo_ros</run_depend>
  <run_depend>gazebo</run_depend>
  <run_depend>geometry_msgs</run_depend>
- <run_depend>roscpp</run_depend>
+ <run_depend>rclcpp</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>gazebo_version_helpers</run_depend>

これらを編集して、把持に必要なパッケージだけビルドします。

colcon build --packages-select gazebo_grasp_plugin gazebo_grasp_plugin_ros gazebo_version_helpers

これで使うための準備は完了です。

使い方

このプラグインを使用するためには、ロボットの設定にプラグインを挿入することと、プラグインの呼び出しが必要です。
まず、プラグインを読めるようにパスを登録します。

export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:(cloneしたディレクトリのパス)/build/gazebo_grasp_plugin

主に、以下のファイルを編集します。

• ロボットのURDFファイル(~.urdf)
• Worldの設定ファイル(~.world, ~.sdf)

これらのファイルの設定を順に示します。

ロボットのURDFファイルの変更

今回は、turtlebot3 manipulationを使用しています。ロボットを定義しているURDFファイル内に、プラグインを挿入する必要があります。
まず、ロボットを定義するURDFファイルの最後に以下の項目を追加します。

<robot>
...
    <gazebo>
    <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
        <arm>
            <arm_name>name-of-arm</arm_name>
            <palm_link> hand_link_name  </palm_link>
            <gripper_link> finger_index_link_1 </gripper_link>
            <gripper_link> finger_index_link_2 </gripper_link>
        </arm>
        <forces_angle_tolerance>100</forces_angle_tolerance>
        <update_rate>4</update_rate>
        <grip_count_threshold>4</grip_count_threshold>
        <max_grip_count>8</max_grip_count>
        <release_tolerance>0.005</release_tolerance>
        <disable_collisions_on_attach>false</disable_collisions_on_attach>
        <contact_topic>__default_topic__</contact_topic>
    </plugin>osrf/ros:humble-desktop
    </gazebo>

</robot>

これを追加したあと、<arm>で囲まれている部分をいくつかの項目を使用するロボットに合わせて書き換えます。
turtlebotの場合の例も示します。

項目名	内容	turtlebotの場合
arm_name	アームの名前	arm
palm_link	グリッパーの根本部分のリンク	link5
gripper_link	グリッパー自体のリンク	gripper_left_link, gripper_right_link

これらの名前は、SRDF, URDFで設定している名前を入力します。
arm_nameは、SRDFで定義されているアームのグループの名前を入力します。
palm_link, gripper_linkはUDRFで定義しているリンクの名前を設定します。

Worldの設定ファイルの変更

gazeboで周辺環境を設定するためのWorldファイルを編集します。
以下の項目を追加します。

<sdf version="1.5">
  <world name="default">
...
+ <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so"/>
  </world>
</sdf>

各パラメータの設定

ロボットのURDFファイル内に挿入したプラグインにはいくつかパラメータがあります。<arm>以下のパラメータについてまとめます。基本的に、私の場合は最後のパラメータを除いて、ほぼそのままの状態で使用することが出来ました。

このプラグインは、更新レートごとに把持しているかしていないのかを判断し、把持しているなら判断用の変数に1加算、していないなら1減算します。
加算と減算を繰り返した結果、変数の値が閾値を超えた場合「把持状態」に変化しています。
また、最大値を設定して、判断用の変数が増え続けないようになっています。

項目名	内容
forces_angle_tolerance	把持したとみなす、接触同士の角度
update_rate	把持されているか否かを判断するレート
grip_count_threshold	把持状態に変更する為の閾値
max_grip_count	値の最大値
release_tolerance	物体と離れていると判断する距離
disable_collisions_on_attach	把持している最中の物体の衝突判定
contact_topic	接触情報のトピック名

私の場合、最後のパラメータはgz topic -lで名前を確認して、gz topic -e {トピック名}で接触したときに更新されているか確認して入力しました。turtlebotの時は、/gazebo/default/turtlebot3_manipulation_system/contactsでした。

私は、最初はここの設定をいくら変えても把持が出来ませんでした。
色々と確認した結果、原因は非常に単純で、把持する対象が重すぎてグリッパーが物体と拮抗して、動かなかったことでした。どうしてもうまく行かない場合は、環境の設定も確認してください。

まとめ

今回は、Gazebo Classic内で把持を行えるようにプラグインを使用しました。

この調査をしている途中に、Ignition GazeboがGazeboに名前を変更するなどいろいろな動きがあって困惑しました...
これからは、今後のためにGazebo(旧Ignition Gazebo)も調査したいと考えています。