ROS入門 (28) - URDFとGazeboによるロボットモデルのシミュレーション

「URDF」と「Gazebo」によるロボットモデルのシミュレーションを行う手順をまとめました。

・Melodic

前回

1. urdf_sim_tutorial

今回は、公式の「urdf_sim_tutorial」を利用します。

GitHub - ros/urdf_sim_tutorial

2. urdf_sim_tutorialのセットアップ

「urdf_sim_tutorial」のセットアップ手順は、次のとおりです。

(1) ROS1のセットアップ。
「ROS入門 (8) - ROS1のパッケージの作成」と同様です。

(2) ワークスペースのセットアップ。
「ROS入門 (8) - ROS1のパッケージの作成」と同様です。

(3) 「~/catkin_ws/src」に「urdf_sim_tutorial」パッケージをクローン。
マウントしたPC側でクローンしました。

$ cd ~/catkin_ws/src
$ git clone git@github.com:ros/urdf_sim_tutorial.git

(4) 「~/catkin_ws」に移動してビルド。

$ cd ~/catkin_ws
$ catkin build
$ source ~/catkin_ws/devel/setup.bash

(5)「ros-melodic-joint-state-publisher-gui」のインストール。
「rviz」で利用します。

$ sudo apt update
$ sudo apt install ros-melodic-joint-state-publisher-gui

3. Gazeboの起動とURDFのロボットモデルの表示

「gazebo.launch」を使用して、Gazeboの起動とURDFのロボットモデルの表示を行います。

$ roslaunch urdf_sim_tutorial gazebo.launch

「gazebo.launch」の中身は、次のとおりです。

<launch>

  <!-- 引数 -->
  <arg name="paused" default="false"/>
  <arg name="use_sim_time" default="true"/>
  <arg name="gui" default="true"/>
  <arg name="headless" default="false"/>
  <arg name="debug" default="false"/>
  <arg name="model" default="$(find urdf_tutorial)/urdf/08-macroed.urdf.xacro"/>

  <!-- Gazeboの起動 -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="debug" value="$(arg debug)" />
    <arg name="gui" value="$(arg gui)" />
    <arg name="paused" value="$(arg paused)"/>
    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
    <arg name="headless" value="$(arg headless)"/>
  </include>

  <!-- URDFをパラメータに読み込み -->
  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />

  <!-- パラメータからURDFを読み取り、Gazeboでモデルを表示 -->
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
      args="-z 1.0 -unpause -urdf -model robot -param robot_description" respawn="false" output="screen" />

  <!-- robot_state_publisherの起動 -->
  <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
    <param name="publish_frequency" type="double" value="30.0" />
  </node>
</launch>

この状態では、ROSとGazeboはまだ対話できません。

4. ROSとGazeboの対話プラグインの追加

ROSとGazeboを対話させるには、プラグイン「gazebo_ros_control」をURDFに追加します。

(1) URDFの</ robot>タグ直前に、以下のコードを追加。

  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>/</robotNamespace>
    </plugin>
  </gazebo>

「~/catkin_ws/src/urdf_sim_tutorial/urdf/09-publishjoints.urdf.xacro」が追加済みのコードになります。

この状態では、ROSとGazeboはまだ対話してません。

5. コントローラの追加

Gazebo内で実行するROSコードを「コントローラ」と呼びます。
今回は、ロボットの関節状態をパブリッシュする「JointStateController」を追加します。

追加手順は、次のとおりです。

(1) コントローラの設定ファイルの準備。
・~/catkin_ws/src/urdf_sim_tutorial/config/joints.yaml

type: "joint_state_controller/JointStateController"
publish_rate: 50

(2) Gazeboとrvizの起動し、JointStateControllerを実行し、トピック「/joint_states」をパブリッシュ。

$ roslaunch urdf_sim_tutorial 09-joints.launch

「09-joints.launch」の中身は、次のとおりです。

<launch>
  <!-- 引数 -->
  <arg name="model" default="$(find urdf_sim_tutorial)/urdf/09-publishjoints.urdf.xacro"/>
  <arg name="rvizconfig" default="$(find urdf_tutorial)/rviz/urdf.rviz" />


  <!-- Gazeboの起動 -->
  <include file="$(find urdf_sim_tutorial)/launch/gazebo.launch">
    <arg name="model" value="$(arg model)" />
  </include>

  <!-- rvizの起動 -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" />

  <!-- JointStateControllerをパラメータに読み込み -->
  <rosparam command="load"
      file="$(find urdf_sim_tutorial)/config/joints.yaml"
      ns="r2d2_joint_state_controller" />

  <!-- JointStateControllerの実行 -->
  <node name="r2d2_controller_spawner" pkg="controller_manager" type="spawner"
      args="r2d2_joint_state_controller --shutdown-timeout 3"/>
</launch>

(3) トピック「/joint_states」が公開されていることを確認。
トピックは公開されていますが、情報（name、position、velocity、effort）が[]なのがわかります。

$ rostopic echo /joint_states
header: 
 seq: 1809
 stamp: 
   secs: 36
   nsecs: 550000000
 frame_id: ''
name: []
position: []
velocity: []
effort: []
---
    :

6. トランスミッションの追加

Gazeboの可動関節を制御するには、「トランスミッション」を使います。今回は、頭の関節にトランスミッションを追加します。

追加手順は、次のとおりです。

(1) URDFに以下のコードを追加。
このコードは基本的に定型文です。<joint>の名前は宣言済みの関節名を指定します。

  <transmission name="head_swivel_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="$head_swivel_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="head_swivel">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>

「~/catkin_ws/src/urdf_sim_tutorial/urdf/firsttransmission.urdf.xacro」が追加済みのコードになります。

(2) Gazeboとrvizを実行。
トランスミッションが追加されたため、トピック「/joint_state」の情報（name、position、velocity、effort）が公開されます。

$ roslaunch urdf_sim_tutorial 09-joints.launch model:=urdf/10-firsttransmission.urdf.xacro

(3) 「rostopic」でトピック/joint_stateの確認。

$ rostopic echo /joint_states
header: 
 seq: 1386
 stamp: 
   secs: 27
   nsecs: 913000000
 frame_id: ''
name: [head_swivel]
position: [-4.208654580395432e-08]
velocity: [9.119072519434175e-08]
effort: [0.0]
---

7. 頭の関節の制御

頭の関節を制御する「JointPositionController」を追加します。

追加手順は、次のとおりです。

(1) コントローラの設定ファイルの準備。
・~/catkin_ws/src/urdf_sim_tutorial/config/head.yaml

type: "position_controllers/JointPositionController"
joint: head_swivel

(2) Gazeboとrvizを起動し、JointPositionControllerを実行し、トピック「/r2d2_head_controller/command」の値をサブスクライブ。

$ roslaunch urdf_sim_tutorial 10-head.launch

(3) トピック「/r2d2_head_controller/command」に値をパブリッシュ。
パブリッシュをするとすぐに頭の向きが変更されます。

$ rostopic pub /r2d2_head_controller/command std_msgs/Float64 "data: -0.707"

(4) URDFを以下のように変更。
パブリッシュをすると徐々に頭の向きが変更されるようになります。

  <joint name="head_swivel" type="continuous">
    <parent link="base_link"/>
    <child link="head"/>
    <axis xyz="0 0 1"/>
    <origin xyz="0 0 ${bodylen/2}"/>
    <limit effort="30" velocity="1.0"/>
  </joint>

「~/catkin_ws/src/urdf_sim_tutorial/urdf/11-limittransmission.urdf.xacro」が変更済みのコードになります。

(5) Gazeboとrvizを起動し、JointPositionControllerを実行。

$ roslaunch urdf_sim_tutorial 10-head.launch model:=urdf/11-limittransmission.urdf.xacro

(6) トピック「/r2d2_head_controller/command」に値をパブリッシュ。
徐々に頭の向きが変更されることを確認します。

$ rostopic pub /r2d2_head_controller/command std_msgs/Float64 "data: -0.707"

8. グリッパーの制御

グリッパーの各関節をグループ化して制御する「JointGroupPositionController」を追加します。

追加手順は、次のとおりです。

(1) コントローラの設定ファイルの準備。
・~/catkin_ws/src/urdf_sim_tutorial/config/gripper.yaml

type: "position_controllers/JointGroupPositionController"
joints:
  - gripper_extension
  - left_gripper_joint
  - right_gripper_joint

(2) Gazeboとrvizの起動し、JointGroupPositionControllerを実行し、トピック「/r2d2_gripper_controller/command」の値をサブスクライブ。

$ roslaunch urdf_sim_tutorial 12-gripper.launch

(3) トピック「/r2d2_gripper_controller/command」に値をパブリッシュ。
グリッパーが動くことを確認します。

$ rostopic pub /r2d2_gripper_controller/command std_msgs/Float64MultiArray "layout:
  dim:
  - label: ''
    size: 3
    stride: 1
  data_offset: 0
data: [0, 0.5, 0.5]"

$ rostopic pub /r2d2_gripper_controller/command std_msgs/Float64MultiArray "layout:
  dim:
  - label: ''
    size: 3
    stride: 1
  data_offset: 0
data: [-0.4, 0, 0]"

9. ホイールの制御

ロボットのホイールを制御する「DiffDriveController」を追加します。

追加手順は、次のとおりです。

(1) ホイールのトランスミッションを追加。

    <transmission name="${prefix}_${suffix}_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <actuator name="${prefix}_${suffix}_wheel_motor">
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
      <joint name="${prefix}_${suffix}_wheel_joint">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
    </transmission>

(2) ホイールのマテリアルを追加。

    <gazebo reference="${prefix}_${suffix}_wheel">
      <mu1 value="200.0"/>
      <mu2 value="100.0"/>
      <kp value="10000000.0" />
      <kd value="1.0" />
      <material>Gazebo/Grey</material>
    </gazebo>

(3) コントローラの設定ファイルの準備。
・~/catkin_ws/src/urdf_sim_tutorial/config/diffdrive.yaml

type: "diff_drive_controller/DiffDriveController"
publish_rate: 50

left_wheel: ['left_front_wheel_joint', 'left_back_wheel_joint']
right_wheel: ['right_front_wheel_joint', 'right_back_wheel_joint']

wheel_separation: 0.44

# ロボットのエンコーダ出力のオドメトリ共分散
pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]
twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03]

# robot_descriptionのトップレベルフレーム(リンク)
base_frame_id: base_link

# ロボットの速度と加速度の制限
linear:
  x:
    has_velocity_limits    : true
    max_velocity           : 0.2   # m/s
    has_acceleration_limits: true
    max_acceleration       : 0.6   # m/s^2
angular:
  z:
    has_velocity_limits    : true
    max_velocity           : 2.0   # rad/s
    has_acceleration_limits: true
    max_acceleration       : 6.0   # rad/s^2

(4) Gazeboとrvizの起動。

$ roslaunch urdf_sim_tutorial 13-diffdrive.launch

(5) rqt_robot_steeringの起動。
このGUIでR2D2のホイールを制御できます。

$ rosrun rqt_robot_steering rqt_robot_steering

10. 参考

urdf/Tutorials/Using a URDF in Gazebo - ROS Wiki

Unityではじめる ROS・人工知能 ロボットプログラミング実践入門