ROSを使用したCRANE+の動かし方　その8

こんにちは！長谷川です。
前回はROSのプログラムの書き方について説明しましたが、今回はCRANE+のURDFモデルの作り方について説明したいと思います。

URDFモデルとは

URDFとはUnified Robot Description Formatの略で、ロボットの形状や関節軸構成などのハードウェア情報を、XMLという形式で記録したものです。パッケージそのものの説明はこちらにあります。モデルの作り方についてのWikiはこちらにあるので、これをもとに説明を進めて行きたいと思います。

CRANE+のモデルについて

CRANE+の関節軸構成を以下のように分析し、名前付けします。

これに基づき、モデルを構成していきます。なお、座標系は右手系です。

パッケージ、ファイルの作成

解説は後回しにして、まずは実際にモデルを作ってみましょう。端末を起動して、

$ cd ~/catkin_ws/src
$ catkin_create_pkg crane_urdf std_msgs rospy roscpp sensor_msgs

と入力し、crane_urdfパッケージを作ります。この作業は今回のURDFモデル作成自体とは関係ありません。以前作成したturtlebot_armパッケージなどの上でモデルを作ることも可能ですが、次回紹介するプログラムを動かすためにはsensor_msgsに依存したパッケージを作らなければならないので、今回することにしました。
次に、

$ cd crane_urdf/src
$ vi crane.urdf

と入力してviを起動します。編集モードに入り、以下を貼り付けます。

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

  <link name="base_link">
    <visual>
      <origin xyz="0 0 0.0227" rpy="0 0 0" />
      <geometry>
	<box size="0.0499 0.032 0.0454" />
      </geometry>
      <material name="red">
	<color rgba=".8 0 0 1" />
      </material>
    </visual>
  </link>

  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child link="link1"/>
    <origin xyz="0.0145 0 0.0454" rpy="0 0 0" />
    <axis xyz="0 0 1" />
    <limit lower="-2.61" upper="2.61" effort="0.5" velocity="6.0" />
  </joint>

  <link name="link1">
    <visual>
      <origin xyz="0 0 0.013" rpy="0 0 0" />
      <geometry>
        <box size="0.024 0.0475 0.026" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 0 0.026" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>
  </link>

  <joint name="joint2" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0 0.026" rpy="0 0 0" />
    <axis xyz="0 1 0" />
    <limit lower="-1.75" upper="1.75" effort="0.5" velocity="6.0" />
  </joint>

  <link name="link2">
    <visual>
      <origin xyz="0 0 0" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 0 0.0415" rpy="0 0 0" />
      <geometry>
        <box size="0.024 0.0475 0.083" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 0 0.083" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>
  </link>

  <joint name="joint3" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 0 0.083" rpy="0 0 0" />
    <axis xyz="0 -1 0" />
    <limit lower="-2.55" upper="2.55" effort="0.5" velocity="6.0" />
  </joint>

  <link name="link3">
    <visual>
      <origin xyz="0 0 0" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 0 0.04675" rpy="0 0 0" />
      <geometry>
        <box size="0.024 0.0475 0.0935" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 0 0.0935" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>
  </link>

  <joint name="joint4" type="revolute">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="0 0 0.0935" rpy="0 0 0" />
    <axis xyz="0 -1 0" />
    <limit lower="-1.80" upper="1.80" effort="0.5" velocity="6.0" />
  </joint>

  <link name="link4">
    <visual>
      <origin xyz="0 0 0" rpy="1.57079632675 0 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 0 0.0227" rpy="0 0 0" />
      <geometry>
        <box size="0.024 0.0475 0.061" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 0.03295 0.065727241" rpy="-0.52359878 0 0" />
      <geometry>
        <box size="0.034 0.0014 0.019" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 0.0377 0.09665" rpy="0 0 0" />
      <geometry>
        <box size="0.034 0.0014 0.0454" />
      </geometry>
      <material name="red" />
    </visual>
  </link>

  <joint name="joint5" type="revolute">
    <parent link="link4"/>
    <child link="link5"/>
    <origin xyz="0 -0.0145 0.045" rpy="0 0 0" />
    <axis xyz="-1 0 0" />
    <limit lower="-0.69" upper="0.69" effort="0.5" velocity="6.0" />
  </joint>

  <link name="link5">
    <visual>
      <origin xyz="0 0 0" rpy="0 1.57079632675 0" />
      <geometry>
        <cylinder radius="0.012" length="0.0475" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 -0.013 0" rpy="0 0 0" />
      <geometry>
        <box size="0.0475 0.026 0.024" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 -0.0267 0.023" rpy="0 0 0" />
      <geometry>
        <box size="0.034 0.0014 0.070" />
      </geometry>
      <material name="red" />
    </visual>

    <visual>
      <origin xyz="0 -0.0166252378 0.07545" rpy="-0.52359878 0 0" />
      <geometry>
        <box size="0.034 0.0014 0.040299" />
      </geometry>
      <material name="red" />
    </visual>
  </link>

</robot>

これがURDFモデルを記述した内容になります。貼りつけたら、保存してviを終了してください。これでモデル作成は完了です。

モデルの確認

次に、作成したURDFファイルが正しいフォーマットになっているか確認してみましょう。まず、

sudo apt-get install liburdfdom-tools

と入力し、チェックのためのコマンドをインストールします。次に、

check_urdf crane.urdf

と入力すると、以下のように表示されました。

これで、正しいフォーマットになっていることが確かめられました。
そこで、

roslaunch urdf_tutorial display.launch model:=crane.urdf

と入力すると、rviz(ROSのビジュアライザ)が起動し、何やら表示されます。

拡大すると、以下のようにロボットモデルができていることがわかります。

赤と緑と青の棒は、各関節のx,y,z軸を表しています。白い文字でlinkの名前も書かれていますね。ですが、これでは見にくいので、「Displays」欄内の「TF」に入っているチェックを外します。すると、以下のように、CRANE+を模したモデルができていることがわかります。

あまりCRANE+に似ていないモデルですみません・・・。色も赤一色ですし・・・。このモデルは、私がURDFファイルの書き方の練習用に作ったものなので、書きやすい直方体と円筒のみで構成され、なんとなく外形を似せている状態です。あくまで練習用なので、ご容赦ください。
端末にCtrl-Cを打ち込んででrvizを終了し、今度は

roslaunch urdf_tutorial display.launch model:=crane.urdf gui:=True

と入力してみてください。

今までのrvizの他に、Joint State Publisherなるウィンドウが表示されたと思います。ここに表示されたバーを動かすことで、モデルの各関節を動かすことができます。

URDFファイルのデータ構造

モデルの表記の仕方について、crane.urdfを見つつ学びましょう。

<robot name="crane">

</robot>

この二つに囲まれた部分で、一体のロボットが宣言されます。「crane」の所には他の名前を入れても機能します。

<link name="base_link">

</link>

リンク名「base_link」を定義したのち、この二つに囲まれた部分でリンクの状態を定義しています。リンク名はロボット内で唯一のものでなければなりません。なお、base_linkはロボット全体の中心となるリンクであり、必須のリンクとなっています。

<visual>

</visual>

この中で、リンク（この行ではbase_link）内の一つの物体についてのビジュアルを定義しています。

<origin xyz="0 0 0.0227" rpy="0 0 0" />

「xyz=”」の右には、根元のジョイントを原点とした時の、記述している物体の中心のxyz座標[m]を記入します。「rpy=”」の右には、物体を回転させる角度を記入します。

<geometry>

</geometry>

この中で、物体の形状を定義します。

<box size="0.0499 0.032 0.0454" />

記述している物体が、x方向に0.0499[m],y方向に0.032[m],z方向に0.0454[m]の厚みを持つ直方体であると定義しています。

<material name="red">

</material>

materialタグは、物体の表面の見栄えを定義するタグです。ここでは、この中で宣言された見栄えを「red」と定義することも同時に宣言しています。

<color rgba=".8 0 0 1" />

ここで、表面の色を定義しています。この数字だと赤色になるようです。

<joint name="joint1" type="revolute">

</joint>

ジョイント名「joint1」を定義したのち、この二つに囲まれた部分でジョイントの状態を定義しています。ジョイント名はロボット内で唯一のものでなければなりません。「type=”」の右には、ジョイントの種類を記入します。ジョイントの種類は以下のように定義されています。
revolute・・・ヒンジ機構
continuous・・・無限回転機構
prismatic・・・直動機構
fixed・・・固定関節
floating・・・自由関節
planar・・・水平移動機構
今回はrevolute、つまりヒンジ機構としています。

<parent link="base_link"/>

ジョイントの根元にあるリンク（親リンク）がbase_linkであると定義しています。

<child link="link1"/>

ジョイントの先にあるリンク（子リンク）がlink1であると定義しています。

<origin xyz="0.0145 0 0.0454" rpy="0 0 0" />

「xyz=”」の右には、親リンクの原点を基準とした、記述しているジョイントのxyz座標[m]を記入します。ここで、親リンクの原点とは、親リンクが最初のリンク（今回はbase_link）の場合はロボットモデル全体の原点、それ以外の場合は親リンクの根元のジョイントの位置を指します。「rpy=”」の右には、ジョイントを最初から回転させる角度を記入します。

<axis xyz="0 0 1" />

「xyz=”」の右には、回転関節の場合の回転軸の方向を記入します。回転軸の方向に進む右ねじが回る向きを正として、関節が回ることになります。

<limit lower="-2.61" upper="2.61" effort="0.5" velocity="6.0" />

ここでは、関節のリミットを設定しています。関節がrevoluteやprismaticの場合は、下限値にあたるlowerと、上限値にあたるupperを設定する必要があります。単位は、revoluteの場合は[rad]、prismaticの場合は[m]となっています。
effortとは、ジョイントが発揮できるトルク[Nm]または力[N]の絶対値の最大値のことです。今回は、0.5Nm以上のトルクを発揮できないよう制限されているということになります。
velocityとは、ジョイントの動く速度[m/s]または角速度[rad/s]の絶対値の最大値のことです。今回は、6.0rad/s以上の角速度で動かないよう制限されているということになります。

<material name="red" />

これは、先ほど定義した「red」という見栄えを物体の見栄えとすることを宣言しています。

<cylinder radius="0.012" length="0.0475" />

これは、記述している物体が半径0.012[m]、長さ0.0475[m]の円筒であると定義しています。ちなみに、

<sphere radius="1"/>

と書くと、半径1[m]の球となります。また、

<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae" scale="1."/>

などと、ファイルに記述された形状を反映させることも可能です。ファイル形式は、Colladaの.dae形式（推奨）と、.stl形式がサポートされています。

以上で、データ構造の基本は説明できたと思います。これ以上はこの記事では扱わないことにします。
その他の物理パラメータの設定方法や、拡張形式xacroの話については、チュートリアルの続きをご覧になってください。
また、シュミレータGazeboにモデルを読み込ませるためには、ファイルを変更しなければなりません。こちらをご覧になってください。

以上、CRANE+のURDFモデルの作り方について説明しました。
次回は、今回作ったモデルと実物のCRANE+の連動のさせ方について説明したいと思います。