ノートブック

古き良きスタイルのノートブックです。

シンプルなロボットアーム Part 2

f:id:primitivem:20170926172406p:plain

ロボットオペレーティングシステムのROSと、ロボットシミュレーターのGazeboを使いシンプルなロボットアームをステップバイステップで作ってゆきたいと思います。
シンプルなロボットアーム Part1 に続いてこの記事では5つの関節をもつシンプルなロボットアームをURDFで記述しそれをGazebo上にスポーンさせ、それとは別にロボットを制御するROSノードを起動しシミュレーション上でアームを動かしてみます。

この記事で解説するコード等は以下にあります。 https://github.com/embodmt/Gazebo_ROS/tree/master/simple_arm_2

以下の内容は Ubuntu 16.04 と ROS kinetic の環境で実行したものです。

まずはパッケージを作ります。任意のcatkin workspace内のsrcディレクトリに移動して

$ cd src
$ catkin_create_pkg simple_arm_2
$ cd ..
$ catkin_make
$ source devel/setup.bash

これでパッケージを作成・ビルドして、つぎに以下のように各種ディレクトリを作ります。

$ roscd simple_arm_2
$ mkdir urdf launch config scripts

こうして作った各種ディレクトリに必要なファイルを追加してゆきます。 urdfディレクトリにsimple_arm.urdfを、launchディレクトリにgazebo.launchとcontrol.launchを、configディレクトリにcontrol.yamlを、 そしてscriptsディレクトにjoint_control.pyをそれそれ追加してゆきます。以下で順番に見てゆきましょう。

simple_arm.urdf

ロボットモデルのURDFファイルはPart1の記事のときとほぼ同じだがリンクとジョイントの記述の他にtransmissionというGazeboシミュレーションのための記述が新たに加わりました。 詳しくはGazeboの公式チュートリアルのGazeboとROSの連帯に関することが書かれたこのページ等を参照
http://gazebosim.org/tutorials?tut=ros_control&cat=connect_ros
以下のコードは見通しを良くするため重複する記述は割愛して概要だけをペーストしています。
simple_arm.urdf

<?xml version="1.0"?>
<robot name="simple_arm">

  <link name="world"/>

  <!-- all inertia param below set like Izz = (1/12)*(x^2+y^2), that approximately as box
  -->

  <link name="base">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.005"/>
      <geometry>
        <cylinder length="0.01" radius="0.02"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.005"/>
      <geometry>
        <cylinder length="0.01" radius="0.02"/>
      </geometry>
      <material name="white">
        <color rgba="1 1 1 0.5"/>
      </material>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0.005"/>
      <mass value="1.0"/>
      <inertia ixx="0.00014" ixy="0.0" ixz="0.0"
               iyy="0.00014" iyz="0.0"
               izz="0.000266"/>
    </inertial>
  </link>

  <link name="link1">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.01"/>
      <geometry>
        <box size="0.02 0.02 0.03"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.01"/>
      <geometry>
        <box size="0.02 0.02 0.03"/>
      </geometry>
      <material name="white">
        <color rgba="1 1 1 0.5"/>
      </material>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="1.0"/>
      <inertia ixx="0.000108" ixy="0.0" ixz="0.0" 
               iyy="0.000108" iyz="0.0" 
               izz="0.000066"/>
    </inertial>
  </link>

  <link name="link2">
    〜〜 link1と同様なので省略 〜〜
  </link>

  <link name="link3">
    〜〜 link1と同様なので省略 〜〜
  </link>

  <link name="link4">
    〜〜 link1と同様なので省略 〜〜
  </link>

  <link name="link5">
    〜〜 link1と同様なので省略 〜〜
  </link>

  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base"/>
  </joint>

  <joint name="joint1" type="revolute">
    <parent link="base"/>
    <child link="link1"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0.01"/>
    <limit effort="10000" lower="-3.14" upper="3.14" velocity="0.5"/>
  </joint>

  <joint name="joint2" type="revolute">
    〜〜 joint1と同様なので省略 〜〜
  </joint>

  <joint name="joint3" type="revolute">
    〜〜 joint1と同様なので省略 〜〜
  </joint>

  <joint name="joint4" type="revolute">
    〜〜 joint1と同様なので省略 〜〜
  </joint>

  <joint name="joint5" type="revolute">
    〜〜 joint1と同様なので省略 〜〜
  </joint>


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

  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint1">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="tran2">
    〜〜 tran1と同様なので省略 〜〜
  </transmission>
  <transmission name="tran3">
    〜〜 tran1と同様なので省略 〜〜
  </transmission>
  <transmission name="tran4">
    〜〜 tran1と同様なので省略 〜〜
  </transmission>
  <transmission name="tran5">
    〜〜 tran1と同様なので省略 〜〜
  </transmission>


</robot>

このコードの完全版はこちら https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_2/urdf/simple_arm.urdf

launchファイル

次にGazeboを起動するlaunchファイルを以下のようにして書きます。
gazebo.launch

<launch>
  <!-- set param -->
  <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"/>

  <!-- launch gazebo -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="physics" value="bullet" />
    <arg name="world_name" value="$(find simple_arm_2)/launch/sim.world"/>
    <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>

  <!-- spawn model -->
  <arg name="model" default="$(find simple_arm_2)/urdf/simple_arm.urdf"/>
  <param name="robot_description" textfile="$(arg model)"/>
  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model" respawn="false" output="screen"
  args="-urdf -model simple_arm -param robot_description"/>

  <!-- launch control -->
  <include file="$(find simple_arm_2)/launch/control.launch" />

</launch>

Gazeboの起動とモデルのスポーンとROSと連帯してシミュレーション上のモデルと相互作用するための仮想的なコントローラーを同時に起動するlaunchファイルになっています。 ちなみにコードの場所ははこちら https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_2/launch/gazebo.launch
このlaunchファイルの最後の行でGazeboとROSとをつなぐ部分のlaunchファイルを呼び出しており、それを別のlaunchファイルとして以下のように書きました。
control.launch

<launch>

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

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/simple_arm" args="joint1_position_controller joint2_position_controller
    joint3_position_controller joint4_position_controller joint5_position_controller
    joint_state_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">
    <remap from="/joint_states" to="/simple_arm/joint_states" />
  </node>

</launch>

このlaunchファイルもlaunchディレクトリに追加します。コードの場所はここです
https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_2/launch/control.launch

control.yaml

次にロボットのコントローラーに関する設定ファイルも必要で、configディレクトリに以下のものを追加します。 control.yaml

simple_arm:
  # Publish all joint states -----------------------------------
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

  # Position Controllers ---------------------------------------
  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint1
    pid: {p: 6.0, i: 0.6, d: 0.06}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint2
    pid: {p: 5.0, i: 0.5, d: 1.0}
  joint3_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint3
    pid: {p: 4.0, i: 0.4, d: 1.0}
  joint4_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint4
    pid: {p: 3.0, i: 0.3, d: 0.03}
  joint5_position_controller:
    type: effort_controllers/JointPositionController
    joint: joint5
    pid: {p: 1.0, i: 0.1, d: 0.01} 

このコードの場所はここです。 https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_2/config/control.yaml
ここでロボットの関節の回転を与えるアクチュエーターに関する設定もしており、任意のPIDパラメータを設定しています。 PIDパラメータの設定にはコツがいるようですが、今回私の場合はアームの付け根にある関節 joint1 から順に大きい値を設定し、 ミュレーション上で動かしながら微妙に調整してゆきました。しかしPID調整については今回のROSパッケージ作成にとって主題ではないためそこそこ動けばいい程度の調整です。

このようにして simple_arm.urdf、gazebo.launch、control.launch、control.yamlの4つのファイルが揃ったところでgazeboのシミュレーション上に ロボットを登場させROSで制御することが可能になりますが、さらにロボットを制御するROSノードをpythonで書きたいと思います。
ちなみにgazeboのシミュレーション上のロボットをターミナルからコマンド等で制御するやりかたはこのGazebo公式チュートリアル https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_2/config/control.yamlhttp://gazebosim.org/tutorials?tut=ros_comm&cat=connect_ros などに載っています。

joint_control.py

シミュレーション上のロボットアームを制御するためのROSノードをPythonで以下のように書きます。 joint_control.py

import rospy
from std_msgs.msg import String
from std_msgs.msg import Float64
from sensor_msgs.msg import JointState
import random,math
import numpy as np

def joint_control():
    pub1 = rospy.Publisher('/simple_arm/joint1_position_controller/command', Float64, queue_size=10)
    pub2 = rospy.Publisher('/simple_arm/joint2_position_controller/command', Float64, queue_size=10)
    pub3 = rospy.Publisher('/simple_arm/joint3_position_controller/command', Float64, queue_size=10)
    pub4 = rospy.Publisher('/simple_arm/joint4_position_controller/command', Float64, queue_size=10)
    pub5 = rospy.Publisher('/simple_arm/joint5_position_controller/command', Float64, queue_size=10)
    rospy.init_node('joint_control', anonymous=True)
    
    rate = rospy.Rate(0.5)
    while not rospy.is_shutdown():
        #v = rospy.get_time()
        #rospy.loginfo(v)
        
        joint_pos = [random.uniform(-2.0,2.0), random.uniform(-1.0,1.0), random.uniform(-1.0,1.0), random.uniform(-1.0,1.0), random.uniform(-1.0,1.0)]

        pub1.publish(joint_pos[0])
        pub2.publish(joint_pos[1])
        pub3.publish(joint_pos[2])
        pub4.publish(joint_pos[3])
        pub5.publish(joint_pos[4])
        rospy.loginfo("joint pose %s", joint_pos)
        rate.sleep()

if __name__ == '__main__':
    try:
        joint_control()   
    except rospy.ROSInterruptException:
        pass

このコードの場所はここです。 https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_2/scripts/joint_control.py
このファイルをscriptsディレクトリに追加します。

Gazeboを起動

これで準備は完了したので、いよいよ起動してみましょう。以下のようにしてGazeboを起動して

$ roslaunch simple_arm_2 gazebo.launch

別のターミナルからロボット制御用のpythonで書いたROSノードを以下のようにして起動します。

$ rosrun simple_arm_2 joint_control.py

するとGazebo上に登場したロボットがくねくねとランダムに関節角度を変えて動くのが観察できます。

さらに別の起動のやり方として、先ほど作ったgazebo.launchとjoint_control.pyのノードとRVizの3つを同時に起動する 以下のようなlaunchファイルを書いてみます。
gazebo_rviz.launch

<launch>
  
  <!-- gazebo -->
  <include file="$(find simple_arm_2)/launch/gazebo.launch"/>


  <!-- rviz -->
  <arg name="model" default="$(find simple_arm_2)/urdf/simple_arm.urdf" />
  <arg name="gui" default="True"/>
  <param name="robot_description" textfile="$(arg model)"/>
  <param name="use_gui" value="$(arg gui)"/>
  
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find simple_arm_2)/launch/urdf.rviz"/>
  

  <!-- control node python script -->  
  <node name="joint_control" pkg="simple_arm_2" type="joint_control.py" />

</launch>

このコードの場所はここです。https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_2/launch/gazebo_rviz.launch

そしてこれを起動してみると

$ roslaunch simple_arm_2 gazebo_rviz.launch

GazeboとRVizが同時に立ち上がり、両方のUI画面にロボットが現れ動く様子が観察できました。 f:id:primitivem:20170926172342p:plainf:id:primitivem:20170926172358p:plainf:id:primitivem:20170926172406p:plain こんな感じでくねくね動くよ。やったね!

この記事で作成したROSパッケージのソースは以下にあります。
https://github.com/embodmt/Gazebo_ROS/tree/master/simple_arm_2
普通に手元のcatkin workspace内にこのディレクトリをコピーしてcatkin makeすれば依存ライブラリ等も必要なく、簡単に実行できます。