URDFファイルを作る

URDFはUnified Robot Description Formatの略で、
XMLフォーマットのロボットモデルの記述です。
ROSオリジナルでしょうか？

ROSのurdfパッケージではこれをC++から読み込むことができます。

概要
http://www.ros.org/wiki/urdf

詳細
http://www.ros.org/wiki/urdf/XML

チュートリアル
http://www.ros.org/wiki/urdf/Tutorials/Create%20your%20own%20urdf%20file

そしてURDFから運動学用のモデルKDL (Kinematics and Dynamics Library) treeを作るようです。
http://www.ros.org/wiki/kdl_parser
URDFは運動学のモデルよりは広い概念のようです。

また、このファイルにシミュレータのデータやセンサ取り付け位置なども統合されています。


ではチュートリアルからやりましょう。

以下のような構造のロボットがいたとします。

するとURDFでは以下のようなXMLで記述します。
これをmy_urdf.xmlとして保存しましょう。

<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />

  <joint name="joint1" type="continuous">
    <parent link="world"/>
    <child link="link1"/>
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link1"/>
    <child link="link3"/>
  </joint>

  <joint name="joint4" type="continuous">
    <parent link="link3"/>
    <child link="link4"/>
  </joint>
</robot>

world -> link1にjoint1という仮想的なジョイントを作っています。
それ以外は普通にジョイントがリンク（剛体）２つをつなぐ構造で、
図とマッチしていることがわかります。


ではこのmy_urdf.xmlをツールを使ってパースできるかチェックしてみましょう。

$ rosmake urdf
  $ rosrun urdf check_urdf my_urdf.xml

すべて正しければ以下のように表示されます。

robot name is: test_robot
  ---------- Successfully Parsed XML ---------------
  root Link: world has 1 children
      child(1):  link1
          child(1):  link2
          child(2):  link3
              child(1):  link4

次にこれに空間を加えていきます。

図をみるとリンクの根元に緑色の矢印（参照フレーム）がついています。そしてそのフレームはジョイントと一致しています。

なので、このツリー構造の空間はリンクの参照フレームから子供のジョイントまでのオフセットを与えることで記述できます。

<origin>を使って、位置のオフセットをxyz、姿勢のオフセットをrpyで表すと以下のように書くことができます。

<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />

  <joint name="joint1" type="continuous">
    <parent link="world"/>
    <child link="link1"/>
    <origin xyz="5 0 0" rpy="0 0 3.14" />
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="5 2 0" rpy="0 0 3.14" />
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link1"/>
    <child link="link3"/>
    <origin xyz="5 0.5 0" rpy="0 0 0" />
  </joint>

  <joint name="joint4" type="continuous">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="5 0 0" rpy="0 0 3.14" />
  </joint>
</robot>

ではもう一度チェックしてみましょう。

$ rosrun urdf check_urdf my_urdf.xml

結果は同じですね。では次にいきましょう。

次はジョイントの回転を決めます。

これでロボットの運動学モデルが完成です。<axis>を使ってジョイントの記述をします。

ローカルフレームでの回転軸を指定します。

ローカルとは子リンクの参照フレームの座標系における値となりますので、

joint1では、

<axis xyz="0 1 0" />

となります。

すると全部はだいたいこんな感じになります。

<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />

  <joint name="joint1" type="continuous">
    <parent link="world"/>
    <child link="link1"/>
    <origin xyz="5 0 0" rpy="0 0 3.14" />
    <axis xyz="0 1 0" />
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="5 2 0" rpy="0 0 3.14" />
    <axis xyz="0.707 0.707 0" />
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link1"/>
    <child link="link3"/>
    <origin xyz="5 0.5 0" rpy="0 0 0" />
    <axis xyz="0.9 0.15 0" />
  </joint>

  <joint name="joint4" type="continuous">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="5 0 0" rpy="0 0 3.14" />
    <axis xyz="0.707 0.707 0" />
  </joint>
</robot>

ではチェックして、

$ rosrun urdf check_urdf my_urdf.xml

これで可視化しましょう。

$ rosrun urdf urdf_to_graphiz my_urdf.xml

こんなんでました。

では次にこれをプログラムから読み込みましょう。