ノートブック

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

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

f:id:primitivem:20170924215356p:plain

この記事ではロボットオペレーティングシステムのROSと、ロボットシミュレーターのGazeboを使いシンプルなロボットアームをステップバイステップで作ってゆきたいと思います。 この記事で解説するコード等は以下にあります。 https://github.com/embodmt/Gazebo_ROS/tree/master/simple_arm_1

Part1では関節が2つからなるアーム型のロボットモデルを単純にURDFで作成し、それをRVizで可視化して眺めるだけのものを作ります。

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

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

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

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

$ roscd simple_arm_1
$ mkdir urdf launch

ROSパッケージの作成及びcatkin workspaceについてはこの
http://wiki.ros.org/ROS/Tutorials/CreatingPackage 最も基本となる公式チュートリアルを参照 

そして作成したurdfディレクトリ内にURDFファイルを追加します。 Gazebo公式チュートリアルでのURDFファイルの取り扱いページ、 http://gazebosim.org/tutorials?tut=ros_urdf&cat=connect_ros
ROS公式のURDFチュートリアル http://wiki.ros.org/urdf/Tutorials を参考にリンク3つで関節2つからなるシンプルなアームを以下のように記述しました。

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

  <material name="orange">
    <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
  </material>

  <link name="world"/>

  <!--Base Link-->
  <link name="base">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.25"/>
      <geometry>
        <box size="0.07 0.07 0.5"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.25"/>
      <geometry>
        <box size="0.07 0.07 0.5"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0.25"/>
      <mass value="1"/>
      <inertia ixx="0.0212" ixy="0.0" ixz="0.0"
               iyy="0.0212" iyz="0.0"
               izz="0.000816"/>
      <!-- Ixx = (1/12)*M*(y^2+z^2)  Iyy = (1/12)*M*(x^2+z^2)  Izz = (1/12)*M*(x^2+y^2) 
            in case that moment of inertia of cube-->
    </inertial>
  </link>

  <!--Middle Link-->
  <link name="link1">
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.2"/>
      <geometry>
        <box size="0.07 0.07 0.5"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.2"/>
      <geometry>
        <box size="0.07 0.07 0.5"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0.2"/>
      <mass value="1"/>
      <inertia ixx="0.0212" ixy="0.0" ixz="0.0"
               iyy="0.0212" iyz="0.0"
               izz="0.000816"/>
    </inertial>
  </link>

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

  <!--World to Base Fixed-->
  <joint name="fixed" type="fixed">
    <parent link="world"/>
    <child link="base"/>
  </joint>

  <!--Base to Middle Revolute-->
  <joint name="joint1" type="revolute">
    <parent link="base"/>
    <child link="link1"/>
    <axis xyz="0 1 0"/>
    <origin xyz="0 0 0.45"/>
    <limit effort="1000" lower="-3.14" upper="3.14" velocity="0.5"/>
  </joint>

  <!--Middle to Top Revolute-->
  <joint name="joint2" type="revolute">
    〜〜 joint1と同様なので省略 〜〜
  </joint>

</robot>

ここではコードの見通しを良くするためにURDFファイルの内容を部分的に省略して貼り付けたが、完全なものは以下のリンクにあります。
https://github.com/embodmt/Gazebo_ROS/blob/master/simple_arm_1/urdf/simple_arm.urdf

次に先ほど作成したlaunchディレクトリに RVizを起動するための以下のようなlaunchファイルを追加します

<?xml version="1.0"?>
<launch>
  <arg name="model" default="$(find simple_arm_1)/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="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find simple_arm_1)/launch/simple_arm.rviz"/>
</launch>

このようにして2つのファイルをそれぞれのディレクトリに追加した後、以下のようにしてlaunchファイルを起動すると

$ roslaunch simple_arm_1 rviz.launch

このようにRVizが起動します。
f:id:primitivem:20170924214120p:plain このままでは座標の基準点が定まっていないためUI画面左上にある "Global Option"下の"Fixed Frame"のドロップダウンメニューから基準にしたいリンクあるいはROSのtfを選択し、次にUI画面左下付近にあるAddボタンを押して以下のようにRobotModelを選択することで f:id:primitivem:20170924214706p:plain 以下のように今回記述したロボットモデルをRViz上に表示することができました。 f:id:primitivem:20170924215153p:plain またlaunchファイルに記述されているように"robot state pubslisher"と"joint state publishe"のノードが立ち上がっているため、右の小さな別ウインドウのUIのスライドを操作することでRViz上のロボットモデルのジョイントを動かすこともできるよ。このスクリーンショットみたいに。 f:id:primitivem:20170924215356p:plain やったね!
このようにして操作したRVizの設定をGUI上のメニューから保存すると .rviz拡張子のついた設定ファイルがlaunchディレクトリに生成され次回以降のRVizの起動で今回のGUIの設定が保存された状態で起動することができます。

この記事で作成したROSパッケージのソースは以下にあります。
https://github.com/embodmt/Gazebo_ROS/tree/master/simple_arm_1
この記事で説明した手順を追わずにgithubのレポジトリから手元の環境にROSパッケージを構成したい場合は、普通に手元のcatkin workspace内にこのディレクトリをコピーしてcatkin makeすれば実行できます。