ROS練習用のロボットを作る

　ロボット用ミドルウェアであるところのROSの勉強をしたいわけなんですが，ROS対応のロボットと言うと，こんなのとか，こんなのとか，安いのでもこんなのくらいは必要で，とてもちょろっと試してみるくらいの費用じゃできないんですよね．
　それでも僕は，ROSが使いたいんだ！！
　と言うわけで，なるべく最小限の費用でROS練習用のロボットを作ることにしました．

作ったロボット

　こんなかんじに動きます．
　今回はIntel Edisonで動かしました．

必要なもの

ハードウェア

• GWSサーボ Micro 2BB/MG ×2
• サーボブラケット ×2
• スマホ固定アーム
• USBカメラ
• 配線材料
• 5[V]くらい出るACアダプタ
• PWMが出せる制御ボード（今回はIntel Edison）
• 母艦PC（Ubuntuオススメ）

　スマホ固定アームにハンドドリルで穴をあけて，サーボブラケットを固定しました．同様に，USBカメラの背面にも穴をあけてブラケットに固定しています．ネジやナットは適当に身の回りにあったものを使用しました．

ソフトウェア

• ROS ロボット陽ミドルウェア
• Gazebo 物理シミュレータ
• libmraa Intel Edisonのインタフェース
• ros_gpio ROSの制御ボード用インタフェース

組立て

　フィーリングで．
　サーボモータと制御ボードの間の配線は，このあたりを参照のこと．

ソフトウェアの準備

　母艦PCとEdisonにROSをインストール．Edisonへのインストールは，ここを参照．母艦PCへは，こちらの Desktop-Full Install を行う．libmraaのインストールはこちら，ros_gpioは，ROSのsrcディレクトリで，

wstool set --git ros_gpio https://github.com/yoneken/ros_gpio.git
wstool update ros_gpio

ロボットの物理モデル作成

　ROSでは，ロボットの関節軸構成や物理モデルを記述するのに，Unified Robot Description Format (URDF)を使います．英語の説明を読み進めながら書きましょう．今回ターゲットとなるロボットの関節軸構造は，以下のようになっています．

データ構造

<robot name=" robot_name ">

　XML全体をくくり，一体のロボットを宣言するタグです． robot_name のところには，任意の名前を入れてください．

　ロボットのデータ構造は，リンクとジョイント（関節）の連なったチェーン構造で表現されます．

<link name=" link_name ">

　 link_name のところには，ロボット内で一意の名前を入れてください．

<joint name=" joint_name " type=" joint_type ">

　 joint_name のところには，ロボット内で一意の名前を入れてください． joint_type のところには，
- revolute : ヒンジ機構．上限・下限が必要．
- continuous : 無限回転機構
- prismatic : 直動機構．上限・下限が必要．
- fixed : 固定関節．
- floating : 自由関節．
- planar : 水平移動機構．
の各種タイプのうち，どれか一つを入れてください．

　このjoint要素の中で， parent ， child ， origin の要素を定義します．さらに必要に応じて， axis ， limit ， dynamics の要素も定義します．

<parent link=" link_name ">

　 link_name のところには，ジョイントの根元となるリンクの名前を入れてください．

<child link=" link_name ">

　 link_name のところには，ジョイントの先につながるリンクの名前を入れてください．

<origin xyz=" xyz_val " rpy=" rpy_val ">

　 xyz_val のところには，ジョイントの根元となるリンクの原点を基準とした，ジョイントのxyz距離[m]を入れてください．ロボットの座標系は，前方向がx軸，左手方向がy軸，垂直方向がz軸の右手系が標準です． rpy_val のところには，回転させる角度[rad]を入れてください．

例．
<origin xyz="0 0 0.088" rpy="0 0.5236 0">

<axis xyz=" xyz_val ">

　revoluteやcontinuousといった回転関節の場合は，回転軸の方向をaxisタグで定義します．方向は，右ネジの方向です．

<limit lower=" lower_lim " uppper=" upper_lim " effort=" effort_lim " velocity=" vel_lim ">

　revoluteやprismatic，といった上限・下限がある関節の場合は，lower_limとupper_limの値を適切に設定してください．これらに加えて，continuous関節の場合は，セーフティとしてeffort_limとvelocity_limの値も設定してください．計算方法はこちら．

　実際にチェーンを作成していく際は，まず，ロボット全体の中心となる base_link という名前のリンクを作成しましょう．このリンクのみ，必須のリンクになっています．任意のリンクをbase_linkに設定できますが，移動ロボットであれば重心があるリンク，アームロボットであれば環境と接続して動かないリンク，ヒューマノイドロボットであれば腰（胸）のリンクに設定すると良いでしょう．次に，base_linkにつながるジョイントを作成し，その次に作成したジョイントに接続するリンクを作成し・・というように，次々にリンクをつなげます．

gaze_arm.urdf

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

  <link name="base_link">
  </link>

  <joint name="joint0" type="fixed">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.088" rpy="0 0.523594667 0" />
  </joint>

  <link name="link1">
  </link>

  <joint name="joint1" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0 0.086" rpy="0 0 0" />
    <axis xyz="0 1 0" />
    <limit lower="-1.57" upper="1.57" effort="0.5293" velocity="6.159937255" />
    <friction friction="0.01" />
    <dynamics damping="0.4" />
  </joint>

  <link name="link2">
  </link>

  <joint name="joint2" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 0 0.06" rpy="0 0 0" />
    <axis xyz="1 0 0" />
    <limit lower="-1.046666667" upper="1.046666667" effort="0.5293" velocity="6.159937255" />
    <dynamics damping="0.4" />
  </joint>

 <link name="link3">
  </link>

  <joint name="camera_joint" type="fixed">
    <parent link="link3"/>
    <child link="camera_link"/>
    <origin xyz="0.015 0 0.025" rpy="0 0 0" />
  </joint>

 <link name="camera_link">
  </link>

</robot>

　URDFを作成したら，

$ check_urdf gaze_arm.urdf

として，作成したURDFが正しいフォーマットになっているかどうかチェックできます．コマンドがインストールされていない場合は，

$ sudo apt-get install liburdfdom-dev

としてインストールしてください．

　また，

$ urdf_to_graphiz gaze_arm.urdf

とすることで，作成したURDFが見やすい形でPDFに出力されます．

　この時点で，

$ roslaunch urdf_tutorial display.launch model:= gaze_arm.urdf

とすると，rvizというROSのビジュアライザで，関節構造が3D表示されます．それぞれのジョイント部分の座標軸について，x軸，y軸，z軸がそれぞれ，赤，緑，青の棒で視覚化されます．

ビジュアルパラメータ

　ジョイントの座標軸表示だけだと面白くないので，視覚的にわかりやすいようリンクにビジュアルを設定しましょう．

<visual>

　この要素の中に， origin ， geometry ， material といった要素を定義します．

<origin xyz=" xyz_val " rpy=" rpy_val ">

　 xyz_val のところには，根元のジョイントの位置を原点を基準とした，xyz距離[m]を入れてください．base_linkの場合は，座標系の原点になります． rpy_val のところには，リンクを回転させる角度を入力してください．

<geometry>

　このタグの中には，

<box size="1 1 1" />
<cylinder radius="1" length="0.5"/>
<sphere radius="1"/>
<mesh filename="mesh.dae" scale="1."/>

といったタグで，形状を定義することができます．URDFで推奨されているメッシュのフォーマットは，Colladaの.dae形式です．STL形式も，一応サポートされています．

<material>

　このタグの中には，

<color rgba="0 0 .8 1" />
<texture filename="texture.png" />

といったタグで，表面の見栄えを定義することができます．

　先ほどのURDFにビジュアルパラメータを追加したURDFは，以下のようになります．

gaze_arm.urdf

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

  <link name="base_link">
    <visual>
      <origin xyz="0 0 0.044" rpy="0 0 0" />
      <geometry>
        <cylinder radius="0.015" length="0.088" />
      </geometry>
      <material name="blue">
        <color rgba="0 0 .8 1" />
      </material>
    </visual>
  </link>

  <joint name="joint0" type="fixed">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.088" rpy="0 0.523594667 0" />
  </joint>

  <link name="link1">
    <visual>
      <origin xyz="0 0 0.035" rpy="0 0 0" />
      <geometry>
        <cylinder radius="0.01" length="0.065" />
      </geometry>
      <material name="blue" />
    </visual>
  </link>

  <joint name="joint1" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0 0.086" rpy="0 0 0" />
    <axis xyz="0 1 0" />
    <limit lower="-1.57" upper="1.57" effort="0.5293" velocity="6.159937255" />
    <friction friction="0.01" />
    <dynamics damping="0.4" />
  </joint>

  <link name="link2">
    <visual>
      <origin xyz="0 0 0.005" rpy="0 0 0" />
      <geometry>
        <box size="0.015 0.045 0.05" />
      </geometry>
      <material name="green">
        <color rgba="0 .8 0 1" />
      </material>
    </visual>
  </link>

  <joint name="joint2" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 0 0.06" rpy="0 0 0" />
    <axis xyz="1 0 0" />
    <limit lower="-1.046666667" upper="1.046666667" effort="0.5293" velocity="6.159937255" />
    <dynamics damping="0.4" />
  </joint>

  <link name="link3">
    <visual>
      <origin xyz="0 0 -0.005" rpy="0 0 0" />
      <geometry>
        <box size="0.045 0.015 0.05" />
      </geometry>
      <material name="orange">
        <color rgba="1 .64 0 1" />
      </material>
    </visual>
  </link>

  <joint name="camera_joint" type="fixed">
    <parent link="link3"/>
    <child link="camera_link"/>
    <origin xyz="0.015 0 0.025" rpy="0 0 0" />
  </joint>

 <link name="camera_link">
    <visual>
      <origin xyz="0 0 0.0125" rpy="0 0 0" />
      <geometry>
        <box size="0.05 0.045 0.025" />
      </geometry>
      <material name="red">
        <color rgba=".8 0 0 1" />
      </material>
    </visual>
  </link>

</robot>

　check_urdfで，URDFが壊れていないことを確認しましょう．また，

$ roslaunch urdf_tutorial display.launch model:= gaze_arm.urdf

として，rvizで見栄えを確認してください．

物理パラメータ

　リンクには干渉と慣性のパラメータを，ジョイントには摩擦とダンピング抵抗の値を定義することができます．

<collision>

　link要素の中に記述します．この要素の中に， origin ， geometry といった要素を定義します．これらの要素は，visual要素で指定したものと同様に定義します．

<inertial>

　link要素の中に記述します．この要素の中に， origin ， mass ， inertia といった要素を定義します．定義方法は，記述例を参照してください．

<inertial>

　joint要素の中に記述します．この要素の中に， damping ， friction といった要素を定義します．定義方法は，こちらを参照してください．

　物理パラメータを追記したURDFは以下のようになります．

gaze_arm.urdf

<?xml version="1.0"?>
<robot name="gaze_arm">
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0.044" rpy="0 0 0" />
      <geometry>
        <cylinder radius="0.015" length="0.088" />
      </geometry>
      <material name="blue">
        <color rgba="0 0 .8 1" />
      </material>
    </visual>

    <collision>
      <origin xyz="0 0 0.044" rpy="0 0 0" />
      <geometry>
        <cylinder radius="0.015" length="0.088" />
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0.044" rpy="0 0 0" />
      <mass value="0.03" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
                         iyy="1.0" iyz="0.0"
                                   izz="1.0" />
    </inertial>
  </link>

  <joint name="joint0" type="fixed">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0 0 0.088" rpy="0 0.523594667 0" />
    <axis xyz="0 1 0" />
  </joint>

  <link name="link1">
    <visual>
      <origin xyz="0 0 0.035" rpy="0 0 0" />
      <geometry>
        <cylinder radius="0.01" length="0.065" />
      </geometry>
      <material name="blue" />
    </visual>

    <collision>
      <origin xyz="0 0 0.035" rpy="0 0 0" />
      <geometry>
        <cylinder radius="0.01" length="0.065" />
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0.035" rpy="0 0 0" />
      <mass value="0.03" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
                         iyy="1.0" iyz="0.0"
                                   izz="1.0" />
    </inertial>
  </link>

  <joint name="joint1" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0 0.086" rpy="0 0 0" />
    <axis xyz="0 1 0" />
    <limit lower="-1.57" upper="1.57" effort="0.5293" velocity="6.159937255" />
    <dynamics damping="0.4" friction="0.1" />
  </joint>

  <link name="link2">
    <visual>
      <origin xyz="0 0 0.005" rpy="0 0 0" />
      <geometry>
        <box size="0.015 0.045 0.05" />
      </geometry>
      <material name="green">
        <color rgba="0 .8 0 1" />
      </material>
    </visual>

    <collision>
      <origin xyz="0 0 0.005" rpy="0 0 0" />
      <geometry>
        <box size="0.015 0.045 0.035" />
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0.005" />
      <mass value="0.04" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
                         iyy="1.0" iyz="0.0"
                                   izz="1.0" />
    </inertial>
  </link>

  <joint name="joint2" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 0 0.06" rpy="0 0 0" />
    <axis xyz="1 0 0" />
    <limit lower="-1.046666667" upper="1.046666667" effort="0.5293" velocity="6.159937255" />
    <dynamics damping="0.4" friction="0.1" />
  </joint>

  <link name="link3">
    <visual>
      <origin xyz="0 0 -0.005" rpy="0 0 0" />
      <geometry>
        <box size="0.045 0.015 0.05" />
      </geometry>
      <material name="orange">
        <color rgba="1 .64 0 1" />
      </material>
    </visual>

    <collision>
      <origin xyz="0 0 -0.005" rpy="0 0 0" />
      <geometry>
        <box size="0.045 0.015 0.05" />
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 -0.005" rpy="0 0 0" />
      <mass value="0.04" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
                         iyy="1.0" iyz="0.0"
                                   izz="1.0" />
    </inertial>
  </link>


  <joint name="camera_joint" type="fixed">
    <parent link="link3"/>
    <child link="camera_link"/>
    <origin xyz="0.015 0 0.025" rpy="0 0 0" />
  </joint>

  <link name="camera_link">
    <visual>
      <origin xyz="0 0 0.0125" rpy="0 0 0" />
      <geometry>
        <box size="0.05 0.045 0.025" />
      </geometry>
      <material name="red">
        <color rgba=".8 0 0 1" />
      </material>
    </visual>

    <collision>
      <origin xyz="0 0 0.0125" rpy="0 0 0" />
      <geometry>
        <box size="0.05 0.045 0.025" />
      </geometry>
    </collision>

    <inertial>
      <origin xyz="0 0 0.0125" rpy="0 0 0" />
      <mass value="0.04" />
      <inertia ixx="1.0" ixy="0.0" ixz="0.0"
                         iyy="1.0" iyz="0.0"
                                   izz="1.0" />
    </inertial>
  </link>

</robot>

$ roslaunch urdf_tutorial display.launch model:= gaze_arm.urdf gui:=True

ここで上記のように，rvizの起動時にguiパラメータにTrueをわたしてやると，GUIで関節をぐりぐり動かすことができるようになります．

Gazebo用パラメータ

　さてさて，Gazeboで物理シミュレーションを行うためには，まだいくつかのパラメータを追加する必要があります．

　まずやらなければいけないのが，Gazebo用の特別なリンクの定義です．

<link name="world">

　worldという名前のリンクを，robot要素の最初に記述してください．

<joint name="world_joint" type="fixed">

　次に，world_jointという名前のリンクで，worldリンクとbase_linkの間を接続してください．今回はロボットアームで根元が地面に固定されているのでリンクタイプをfixedにしましたが，台車型の場合はplanar，その他の場合はfloatingなど，ロボットのタイプに合わせてリンクタイプを選択してください．

<transmission name=" transmission_name ">

　稼働関節のシミュレーションを行うため，transmissionの要素を追加します．この要素は稼働関節ごとに一つ作成し，それぞれの関節がどのようなタイプのアクチュエータで駆動されているのかを定義します．詳しくはこちらを参照してください．

<gazebo reference=" link_name ">

　 link_name で指定したリンクにGazebo用のパラメータを追加します．主に行うのは，見栄えの材質の設定と，センサーの設定になります．記述例を参照してください．

<gazebo>

　ロボットとGazeboを接続するプラグインを設定します．必ず必要です．

　以上の要素を追加したURDFがこちらになります．

　ROSのソースディレクトリで

$ wstool set --git gaze_arm https://github.com/yoneken/gaze_arm.git
$ wstool update gaze_arm

として，

$ catkin_make_isolated --install --pkg gaze_arm

でビルドしてください．その後，

$roslaunch gaze_arm test_urdf.launch

で，Gazeboが起動します．お疲れさまでした！これでロボットを物理シミュレータでテストすることができるようになりました♪

xacro

　前章までで，やりたいことは全てできるようになりました．
　この章は，URDFの記述を少しだけ楽にする，拡張書式の話なので，そこまで重要ではありません．既に集中力が限界であれば，また気が向いたときに読んでもらえばOKです．
　xacroというのは，URDFの記述に

• 定数
• 数式
• マクロ

を導入することで，URDFの記述を少しだけ楽にすることができます．最終的に出力されるのはURDFなので，URDFの書き方さえマスターしていれば，xacroは無視して使う必要はないです．

xacro書式の宣言

　robot要素に，

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="firefighter">

のように，xmlns:xacroの項目を入れることで，xacro書式が有効にななります．

定数

<xacro:property name="pi" value="3.1415926535897931" />

　上記のようにpiという定数を宣言した場合，URDFのなかで， ${pi} のように記述して，その定数を使用することができるようになります．

数式

　xacro書式では， ${} で囲まれた中では， ${pi/6.} といったように，数式による記述が可能となります．

マクロ

<xacro:macro name="default_inertial" params="mass origin_xyz">

　上記のように，引数付きのマクロを利用することができるようになります．詳細については，こちらを．具体的な利用方法については，先ほどのURDFをxacro形式で書き直したものを参照してください．

まとめ

• ROS勉強用のロボットを安価に作った．
• ロボットの関節軸構成は，URDFというフォーマットで記述できる．
• xacroというURDFの拡張書式がある．