Dobot MG400 を Gazebo で表示する【ROS2-Foxy】

はじめに
前提条件
メッシュ情報の変更方法
Gazebo で起動
URDF の修正について
URDF の修正
MG400 を Gazebo 上で表示する
おわりに

はじめに

前回は、MG400 の github から urdf.xacro をダウンロードし、xacro で URDF を作成しました。

今回は、作成した URDF のメッシュ情報を変更していきます。

前提条件

前提条件は、以下の通りです。

MG400 を Rviz2 上で表示できる。

メッシュ情報の変更方法

現在の mg400_training.urdf ではメッシュ情報を読み込めていないので、Gazebo 上に表示しようとするとエラーが出ます。

なので、mg400_training.urdf のメッシュ情報を以下のように変更します。

<mesh filename="package:///home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/base_link.dae"/>
↓
<mesh filename="/home/wsl-ubuntu/mg400_workspace/src/mg400_training/meshes/base_link.dae"/>

これを、全ての <mesh> タグで変更してください。

package:// は、~/.gazebo/models を指していますので、こちらにファイルをコピーしても問題ありません。

Gazebo で起動

<mesh> タグを変更したら、gazebo の launch ファイルを作成していきます。

cd ~/mg400_workspace/src/mg400_training
touch launch/gazebo.launch.py

今作成した gazebo.launch.py を以下のようにしてください。

import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess
from launch_ros.actions import Node

def generate_launch_description():
    pkg_box_car_description = get_package_share_directory("mg400_training")
    urdf_file = os.path.join(pkg_box_car_description, "urdf", "mg400_training.urdf")
    print(urdf_file)

    return LaunchDescription([

        ExecuteProcess(
            cmd=["gazebo", "-s", "libgazebo_ros_factory.so"],
            output="screen"
        ),

        Node(
            package="gazebo_ros",
            executable="spawn_entity.py",
            arguments=["-entity", "mg400_training", "-b", "-file", urdf_file],
        ),

        Node(
            package="robot_state_publisher",
            executable="robot_state_publisher",
            output="screen",
            arguments=[urdf_file],
        ),

    ])

ここまでできたら、あとは起動するだけです。

cd ~/mg400_workspace
colcon build --packages-select mg400_training
source ~/.bashrc
ros2 launch mg400_training gazebo.launch.py

上記を実行すると、以下のように gazebo が立ち上がります。

力のない mg400 が表示されたかと思います。

ここから、各関節に力の情報を入力していきます。

URDF の修正について

現状の URDF では、各 joint の接続情報はありますが、Gazebo のシミュレーション上で表示しようとすると、joint間の力学情報がないのでうまく表示できません。

この問題を解決するために、gazebo_ros2_control を使用します。
詳細な説明はこちらにあります。

前回の AGV では、libgazebo_ros_diff_drive.so を使用して、シミュレーションを実行しました。
似たようなことを、追記します。

URDF の修正

gazebo_ros2_control の追加

mg400_training.urdf の最終行の </ robot> の上の行に以下を追加してください。

<gazebo>
  <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
    <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
    <parameters>/home/wsl-ubuntu/mg400_workspace/src/mg400_training/config/jtc.yaml</parameters>
  </plugin>
</gazebo>

<ros2_control name="GazeboSystem" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>

    <joint name="mg400_j1">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>
    <joint name="mg400_j2_1">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">-1.57</param>   
    </joint>
    <joint name="mg400_j2_2">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>
    <joint name="mg400_j3_1">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">-1.57</param>   
    </joint>
    <joint name="mg400_j3_2">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>
    <joint name="mg400_j4_1">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>

    <joint name="mg400_j4_2">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>

    <joint name="mg400_j4_3">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>

    <joint name="mg400_j5_1">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>

    <joint name="mg400_j5">
      <command_interface name="position">
        <param name="min">-3.14</param>
        <param name="max">3.14</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
      <param name="initial_position">0.0</param>   
    </joint>

  </ros2_control>

gazebo_ros2_control を追加しました。
次は、<gazebo> タグの <parameters> にある yaml ファイルを作成します。

jtc.yaml の作成

cd ~/mg400_workspace/src/mg400_training
mkdir config
touch config/jtc.yaml

jtc.yaml は以下のように記載してください。

controller_manager:
  ros__parameters:
    update_rate: 100
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    joint_trajectory_controller:
      type: joint_trajectory_controller/JointTrajectoryController

joint_trajectory_controller:
  ros__parameters:
    joints:
      - mg400_j1
      - mg400_j2_1
      - mg400_j2_2
      - mg400_j3_1
      - mg400_j3_2
      - mg400_j4_1
      - mg400_j4_2
      - mg400_j4_3
      - mg400_j5_1
      - mg400_j5

    command_interfaces:
      - position

    state_interfaces:
      - position

    state_publish_rate: 50.0
    action_monitor_rate: 20.0

    allow_partial_joints_goal: false
    open_loop_control: true
    constraints:
      stopped_velocity_tolerance: 0.01
      goal_time: 0.0
      joint1:
        trajectory: 0.05
        goal: 0.03

詳細は後程説明します。

setup.py の編集

setup.py に先ほど作成した config を読み込むように指定します。

(os.path.join("share", package_name, "launch"), glob("launch/*")),
(os.path.join("share", package_name, "urdf"), glob("urdf/mg400*")),
(os.path.join("share", package_name, "urdf/common"), glob("urdf/common/*")),
(os.path.join("share", package_name, "meshes"), glob("meshes/*")),
↓
(os.path.join("share", package_name, "launch"), glob("launch/*")),
(os.path.join("share", package_name, "config"), glob("config/*")),
(os.path.join("share", package_name, "urdf"), glob("urdf/mg400*")),
(os.path.join("share", package_name, "urdf/common"), glob("urdf/common/*")),
(os.path.join("share", package_name, "meshes"), glob("meshes/*")),

MG400 を Gazebo 上で表示する

ここまで進めたら、MG400 を gazebo 上で表示してみましょう。

cd ~/mg400_workspace
colcon build --packages-select mg400_training
source ~/.bashrc
ros2 launch mg400_training gazebo.launch.py

以下のようにロボットが表示されれば成功です。

おわりに

今回はここまでとします。
ここまでで、MG400 だけでなく UR や Techman のロボットも同様に扱えるようになると思います。
様々なモデルでお試しください。

URDF に追記した部分と jtc.yaml に関しては次回、詳細を説明します。