2016年10月17日月曜日

ROSで始めるロボティクス(4) ー シミュレータ上でロボットを動かしてみる


ロボットシミュレータとしてGazeboを使用します。Gazeboはオープンソースで開発されたロボットアプリケーション開発のための動力学シミュレータです。ROSとも連携しており、モータのみならず、ToFセンサやカメラなどもシミュレーション出来ます。




Gazeboのインストール

まずは、Gazeboのインストールを行いましょう。インストールするバージョンはGazebo 2です。下記のコマンドでROS indigoに対応したバージョンのGazeboをインストールします。最新のGazeboではindigoでうまく動作しないため注意が必要です。

$ sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu trusty main" > /etc/apt/sources.list.d/gazebo-latest.list'
$ wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
$ sudo apt-get update
$ sudo apt-get install libsdformat1 libsdformat-dev gazebo2

URDFをGazebo対応にする

それでは前回作成した差動二輪型の移動ロボットの定義ファイルを、Gazebo対応できるようにしていきます。
まず、リンクに<inertial>動力学シミュレーションに使用する値を追加します。

   <inertial>
    <origin xyz="0 0 0"/>
    <mass value="0.500"/>
    <inertia ixx="0.0020833333" iyy="0.0070833333" izz="0.0083333333"/>
   </inertial>


<origin>は重心の位置、<mass>は重さ(単位はkg)。 
inertiaはMeshLabで算出できる他、手計算することも可能です。計算方法は今は割愛します。
タイヤを接続しているjointにトランスミッションを設定します。


<!-- ===============  Transmission =============== -->
<transmission name="left_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="left_wheel_joint">
  <hardwareInterface>VelocityJointInterface</hardwareInterface>
      </joint>
   <actuator name="left_wheel_motor">
  <hardwareInterface>VelocityJointInterface</hardwareInterface>
  <mechanicalReduction>30</mechanicalReduction>
      </actuator>
 </transmission>
 <transmission name="right_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="right_wheel_joint">
  <hardwareInterface>VelocityJointInterface</hardwareInterface>
      </joint>
   <actuator name="right_wheel_motor">
  <hardwareInterface>VelocityJointInterface</hardwareInterface>
  <mechanicalReduction>30</mechanicalReduction>
      </actuator>
</transmission>

transmissionではロボットの動力と変速に関する情報を定義していきます。
transmissionタグでロボットの動力を指定します。nameでtransmissinoに一意な名前をつけます。
type:タイプを指定します。ここでは一番シンプルなtransmission_interface/SimpleTransmissionを指定しています。
joint:transmissionに接続されているjointを指定します。
actuator:駆動装置に関する定義です。
次に、Gazebo固有の設定を追加していきます。ROSとGazeboを連携するプラグインをインストールします。

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


Gazeboによるシミュレーション時に使用する、物体の摩擦係数を設定しています。今回はおおよその値を設定しています。ポイントとしては、キャスターについてはころころと自由に転がる想定なので、摩擦係数を0に設定しています。

 <gazebo reference="base_link">
<selfCollide>true</selfCollide>
    <mu1 value="0.05" />
    <mu2 value="0.05" />
  </gazebo>

  <gazebo reference="left_wheel_link">
    <selfCollide>true</selfCollide>
    <mu1 value="0.8" />
    <mu2 value="0.8" />
  </gazebo>
<gazebo reference="right_wheel_link">
    <selfCollide>true</selfCollide>
    <mu1 value="0.8" />
    <mu2 value="0.8" />
  </gazebo>

  <gazebo reference="caster_link">
<selfCollide>true</selfCollide>
<!-- This caster is no frictional resistance. -->
    <mu1 value="0.0" />
    <mu2 value="0.0" />
  </gazebo>

まとめるとロボット定義ファイルは次のようになりました。

<?xml version="1.0"?>
<robot name="my_robo" >
  <link name="base_link">

    <visual>
      <geometry>
        <box size="0.400 0.200 0.100"/>
      </geometry>
    </visual>

    <collision>
      <geometry>
        <box size="0.400 0.200 0.100"/>
      </geometry>
    </collision>

   <inertial>
    <origin xyz="0 0 0"/>
    <mass value="0.500"/>
    <inertia ixx="0.0020833333" ixy="0" ixz="0" iyy="0.0070833333" iyz="0" izz="0.0083333333"/>
   </inertial>
  </link>

  <link name="left_wheel_link">
    <visual>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black"/>
    </visual>

    <collision>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
    </collision>

   <inertial>
    <origin xyz="0 0 0"/>
    <mass value="0.500"/>
    <inertia ixx="0.0013541667" ixy="0" ixz="0" iyy="0.0013541667" iyz="0" izz="0.0025"/>
   </inertial>

  </link>

  <joint name="left_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="left_wheel_link"/>
    <origin rpy="-1.5707 0 0" xyz="-0.100 0.130 0"/>
    <axis xyz="0 0 1"/>
  </joint>

  <link name="right_wheel_link">
    <visual>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <material name="black"/>
    </visual>

    <collision>
      <geometry>
        <cylinder radius="0.1" length="0.05"/>
      </geometry>
    </collision>

   <inertial>
    <origin xyz="0 0 0"/>
    <mass value="0.500"/>
    <inertia ixx="0.0013541667" ixy="0" ixz="0" iyy="0.0013541667" iyz="0" izz="0.0025"/>
   </inertial>

  </link>

  <joint name="right_wheel_joint" type="continuous">
    <parent link="base_link"/>
    <child link="right_wheel_link"/>
    <origin rpy="1.5707 0 0" xyz="-0.100 -0.130 0"/>
    <axis xyz="0 0 -1"/>
  </joint>

  <link name="caster_link">
    <visual>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>

    <collision>
      <geometry>
        <sphere radius="0.05"/>
      </geometry>
    </collision>
    
   <inertial>
    <origin xyz="0 0 0"/>
    <mass value="0.500"/>
    <inertia ixx="0.00025" ixy="0" ixz="0" iyy="0.00025"  iyz="0" izz="0.00025"/>
   </inertial>

  </link>
  
  <joint name="caster_joint" type="fixed">
   <parent link="base_link"/>
   <child link="caster_link"/>
   <origin xyz="0.150 0 -0.050"/>
 </joint>
 
 <link name="base_footprint"/>
  <joint name="base_link_joint" type="fixed">
   <parent link="base_footprint"/>
   <child link="base_link"/>
   <origin xyz="0 0 0.100"/>
 </joint>
  
  <!-- ===============  Transmission =============== -->
 <transmission name="left_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="left_wheel_joint">
  <hardwareInterface>VelocityJointInterface</hardwareInterface>
      </joint>
   <actuator name="left_wheel_motor">
  <hardwareInterface>VelocityJointInterface</hardwareInterface>
  <mechanicalReduction>30</mechanicalReduction>
      </actuator>
 </transmission>
 <transmission name="right_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="right_wheel_joint">
  <hardwareInterface>VelocityJointInterface</hardwareInterface>
      </joint>
   <actuator name="right_wheel_motor">
  <hardwareInterface>VelocityJointInterface</hardwareInterface>
  <mechanicalReduction>30</mechanicalReduction>
      </actuator>
 </transmission>

  <!-- =============== Gazebo =============== -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <robotNamespace>my_robo</robotNamespace>
    </plugin>
  </gazebo>
  
 <gazebo reference="base_link">
   <selfCollide>true</selfCollide>
    <mu1 value="0.05" />
    <mu2 value="0.05" />
  </gazebo>

 <gazebo reference="left_wheel_link">
   <selfCollide>true</selfCollide>
    <mu1 value="0.8" />
    <mu2 value="0.8" />
  </gazebo>

 <gazebo reference="right_wheel_link">
   <selfCollide>true</selfCollide>
    <mu1 value="0.8" />
    <mu2 value="0.8" />
  </gazebo>

 <gazebo reference="caster_link">
   <selfCollide>true</selfCollide>
     <!-- This caster is no frictional resistance. -->
    <mu1 value="0.0" />
    <mu2 value="0.0" />
  </gazebo>

</robot>

Gazeboのros連携するプラグインをインストールします。

$ sudo apt-get install -y ros-indigo-gazebo-ros-control
$ sudo apt-get install -y ros-indigo-ros-control ros-indigo-ros-controllers

Gazeboが使えるように設定していきましょう。

$ cs
$ cd my_robo/my_robo_description
$ mkdir config
$ cd config

まずはコントローラーを準備します。コントローラーは通常、ロボットの用途に合わせて準備しますが、今回は、差動2輪ロボット用として、DiffDriveControllerというコントローラーが既に用意されていますのでこちらを利用しましょう。
これを使うことで、ロボットの2輪をシミュレーションすることが出来ます。これを利用します。

コントローラーの設定ファイルを準備します。
controller.yamlというファイルを準備して次のように入力します。


$ gedit controller.yaml

my_robo:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

  diff_drive_controller:
    type        : "diff_drive_controller/DiffDriveController"
    left_wheel  : 'left_wheel_joint'
    right_wheel : 'right_wheel_joint'
    publish_rate: 50.0               # default: 50
    pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
    twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]

    # Wheel separation and diameter. These are both optional.
    # diff_drive_controller will attempt to read either one or both from the
    # URDF if not specified as a parameter
    wheel_separation : 0.26
    wheel_radius : 0.1

    # Wheel separation and radius multipliers
    wheel_separation_multiplier: 1.0 # default: 1.0
    wheel_radius_multiplier    : 1.0 # default: 1.0

    # Velocity commands timeout [s], default 0.5
    cmd_vel_timeout: 1.0
    
    # Base frame_id
    base_frame_id: base_footprint #default: base_link
    
    # Velocity and acceleration limits
    # Whenever a min_* is unspecified, default to -max_*
    linear:
      x:
        has_velocity_limits    : true
        max_velocity           : 0.55  # m/s
        min_velocity           : -0.55 # m/s
        has_acceleration_limits: true
        max_acceleration       : 0.25  # m/s^2
        min_acceleration       : -0.25 # m/s^2
    angular:
      z:
        has_velocity_limits    : true
        max_velocity           : 0.3  # rad/s
        min_velocity           : -0.3
        has_acceleration_limits: true
        max_acceleration       : 0.3  # rad/s^2
        min_acceleration       : -0.3

このコントローラーを起動するファイルcontrol.launchを作成します。


$ cs
$ cd my_robo/my_robo_description/launch
$ gedit control.launch

<launch>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find my_robo_description)/config/controller.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager"
  type="spawner" ns="my_robo" output="screen" 
  args="joint_state_controller
     diff_drive_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher"
  type="robot_state_publisher"
  respawn="false" output="screen" ns="/my_robo">
  </node>

</launch>

$ cs
$ cd my_robo/my_robo_description/launch
$ gedit gazebo.launch

<launch>
  <arg name="model" default="$(find my_robo_description)/my_robo.urdf"/>
  
  <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"/>
  
  <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>

  <param name="robot_description" command="$(find xacro)/xacro.py $(arg model)"/>
  
  <include file="$(find my_robo_description)/launch/control.launch"/>
  
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
    args="-urdf -model my_robo -param robot_description"/>


</launch>

実行します。

roslaunch my_robo_description gazebo.launch

Gazeboの画面が表示されます。



前進させてみましょう。別のターミナルを開き

rostopic pub /my_robo/diff_drive_controller/cmd_vel geometry_msgs/Twist -- '[2.0,0.0,0.0]' '[0.0,0.0,0.0]'

前進すれば成功です!
GazeboのコントローラーがTwistメッセージを受け取ると、エミュレータ上で車輪が回転し、前進を行いました。では先程のrostopicについて説明しましょう。

GazeboのコントローラーNodeが起動しています。このノードは

/my_robo/diff_drive_controller/cmd_vel

というTopicを「Subscribe(購読)」していて、Topicが来るのを待ち続けています。

そこへ、Topicを送信するのが「rostopic pub」というコマンドになります。

rostopic pub [Topic名] [メッセージ型名] -- [メッセージの内容]

rostopicには他にもコマンドがあります。現在登録されているTopicが表示するには
rostopic listを実行します。

$ rostopic list
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/my_robo/diff_drive_controller/cmd_vel
/my_robo/diff_drive_controller/odom
/my_robo/joint_states
/rosout
/rosout_agg
/tf
/tf_static

Topicが購読している型はrostopic typeで調べることが出来ます。

$ rostopic type /my_robo/diff_drive_controller/cmd_vel
geometry_msgs/Twist

geometry_msgs/Twistというメッセージ型だとわかります。
ではこのメッセージの構成を調べてみます。メッセージを調べるにはrosmsg showコマンドを使います。

$ rosmsg show geometry_msgs/Twist
geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

このメッセージは直線方向と回転方向の速度を指定するためのメッセージです。
先ほどrostopic pubコマンドで、

geotry_msgs/Twist -- '[2.0,0.0,0.0]' '[0.0,0.0,0.0]'

としました。これはlinear(直線方向)でxの方行に2m/sで進むという指示です。
angularは回転です。z軸に回転を与えると方向転換します。例えば
[2.0,0.0,0.0]' '[0.0,0.0,2.0]
とすれば回転しながら進みます。
すぐに停止したのは、このメッセージは1秒間しか有効でないためです。
ロボットを継続的に直進させたければ、1秒以内に再び直進命令をsubscribeします。


次回は、Gazeboを使いロボットシミュレータにToFセンサーを追加したいと思います。





参考文献:
http://gazebosim.org/tutorials?tut=inertia&cat=
http://gazebosim.org/tutorials/?tut=ros_urdf
http://forestofazumino.web.fc2.com/ros/ros_urdf_xacro.html
140 180 Gazebo , ROS , URDF , ロボット

記載されている会社名、および商品名等は、各社の商標または登録商標です。

0 コメント:

コメントを投稿

Related Posts Plugin for WordPress, Blogger...