Gazebo + ROS で自分だけのロボットをつくる 4.URDFファイルをつくる

# はじめに
今回は、手順4.についてです。


0. [完成までの手順](http://qiita.com/RyodoTanaka/items/c3014fd6d0f06d12814f)
1. [好きなCADソフトを使ってSTLファイルを作る](http://qiita.com/RyodoTanaka/items/cad2cf100dd916d56717)
2. [好きなソフトを使ってCOLLADAファイルを作る](http://qiita.com/RyodoTanaka/items/b1ebd48da81669db7409)
3. [URDFファイルを作る](http://qiita.com/RyodoTanaka/items/174e82f06b10f9885265) **←今ココ**
4. [GazeboとROSの連携](http://qiita.com/RyodoTanaka/items/6fa7e45f98b55376a95b)
5. [ros_controlについて](http://qiita.com/RyodoTanaka/items/e7feb74d0e29a8598f48)
6. [実際にlaunchしてみる](http://qiita.com/RyodoTanaka/items/af834ed767589a02e27a)

尚、本記事に出てくるロボット（２種類）のソースコードは、GitHubにて公開しております。

- 独立二輪型ロボット：http://github.com/CIR-KIT/fourth_robot_pkg

また、本記事で対象にしているROSバージョンはIndigoです。

# URDFってなに？
URDFとは、Unified Robot Description Formatの略であり、XML記法で記述するロボットモデルのフォーマットです。
何言ってんだ？という方は、ロボットを記述するためのプログラム言語だと思っておいてください。
URDFについては、ちょっとググれば先人の素晴らしい知恵がたくさんヒットすると思います。
そこで、本記事では、URDFのメリット・デメリットをまとめておきたいと思います。
### URDFのメリット

- 開リンク機構の記述が可能
- リンクの相対位置関係の記述が可能
- 独立したロボットの幾何学的要素と運動要素の定義が可能
- 記述が比較的楽
- Xacroという便利ツール（マクロ記述）有

### URDFのデメリット

- 閉リンク機構の記述が不可能（不対応）
- リンクの絶対位置関係の記述が不可能 
- 複数のロボットの幾何学的要素と運動要素の定義が不可能
- 照明や標高地図などの環境の定義が不可能

こんなところだろうと思います。

#### URDF最大の問題点
さて、**URDFは閉リンク機構に対応していない**ということですが、それじゃあ困りますよね...orz
シミュレータである**Gazeboでは、SDFというフォーマットを用いてURDFの問題点を解決している**ようですが、
**ROS側はURDFしか対応していない**ので、結局閉リンクのロボットの設計は一旦置いておいたほうが良さそうです...orz
しかもこれ、[結構前から問題視されている](https://twitter.com/k_yone/status/555738435638394881)ようなので、そろそろ解決しなきゃいけませんよね...!（誰がするんだろう？...orz）
[URDF2.0とか言うのをつくる動き](http://sachinchitta.github.io/urdf2/)もあるみたいです。(実際のコードは未だ見つかかりませんでした...orz リンク先はそのためのアイデアだけが書いてあるようです)

### Gazebo以外のロボットシミュレータ
Gazebo以外にも、閉リンクに対応しているロボットシミュレータに[OpenHRP3](http://fkanehiro.github.io/openhrp3-doc/jp/abstract.html)というのがあります。
このシミュレータでは、[Choreonoid](http://choreonoid.org/ja/)というツールを用いるのですが、
これもROSと同様、[閉リンクへの対応はあまりされていないらしい](https://twitter.com/k_yone/status/555738435638394881)です。
つまり、2016/03/29時点では、閉リンクに対応した有効なツールがすぐには見つからない・使えない状況にあると言えます。

### 気を取り直して...
閉リンクどうのこうのという話題は、ある程度作りたいロボットが固まっている方になら通ずる話と思いますが、
特に想定していない方は一旦忘れていただくのが賢明だと思います。
というのも、簡単な移動台車（二輪）とか、簡単なロボットハンド（パラレルメカニズムを除く）を作るだけならURDFで問題ありません。

# URDFを書こう
URDFの詳しい書き方については、公式のwikiをご覧いただくか、参考2〜5をご覧頂くのが良いと思います。
先人達の素晴らしい記事がたくさんありますので、是非そちらをご覧ください。
本記事では、あえて必要最低限の要素と方針だけを記すことで、情報を整理する役割を果たそうと思います。

## ロボットモデルの要素
URDFで考えるロボットモデルでは、関節部分をJoint、関節により接合されている剛体をLinkと定義しています。
これは、
![公式wikiの図](http://wiki.ros.org/ja/urdf/Tutorials/Create%20your%20own%20urdf%20file?action=AttachFile&do=get&target=link.png)
この図をご覧頂ければお分かりいただけると思います。
すべてのLinkとJointは木構造で記述することが条件になっています。
つまり、複数の親を持ってはならないということです。それさえ守ればOKです。
URDFでは、このLinkとJointにそれぞれ条件を追加することができます。
### LinkとJointの要素
一言に要素と言っても、必須要素と、オプション要素があります。
LinkとJointの必須要素は、以下の通りです。

- **Link**
  - **name**：リンクの名前。
- **Joint**
  - **name**：ジョイントの名前。
  - **type**：ジョイントの種類。以下の種類があります。  
     - **revolute**:軸方向の回転するjointタイプ。上限、下限値有り。
     - **continuous**：revoluteの上限、下限値無しのタイプ。
     - **prismatic**：軸方向にスライドするjointタイプ。上限、下限値有り。
     - **fixed**：固定されたjointタイプ。つまり、０自由度。
     - **floating**：６自由度を持つjointタイプ。一番自由度が高いjointタイプ。
     - **planar**：軸の上下前後方向の動きを持つjointタイプ。

続いて、オプション要素は以下の通りです。ただし種類が多いので、全部ではなく、よく使うものを記しています。

- **Link**
  - **visual**：見た目要素。コ コに前回までに作っJointたCOLLADA or STLを指定したりします。（デフォルト形状もあり）
  - **collision**：衝突要素。ココにも、COLLADA or STLを指定します。（こちらもデフォルト形状あり）
  - **inertia**：質量特性要素。質量、イナーシャ情報を指定します。
- **Joint**  
  - **parent**：親リンクの名前を指定。
  - **child**：子リンクの名前を指定。
  - **origin**：親リンクと子リンクの位置姿勢関係を記述。ただし、回転に関してはroll->pitch->yawの順に変換が行われることに注意。 
  - **axis**：jointが回転やスライドするタイプである場合、どの軸に対してそれを行うのかを指定する。
  - **limit**：jointに上限、下限などの制限がある場合、その制限について指定する。

この他の要素は、Linkについては参考8を、Jointは参考9をご覧いただくのが最もわかりやすいと思います。
因みに、Joint要素にオプションとして、**mimic**要素を取り付けると、対象とするJointの状態が他のJointの状況に合わせて動くようになるので、これを用いれば閉リンク機構を記述することも一応可能です。
ただし、閉リンク構造を記述するためにあるものではなさそうなのでなんとも言えません...

## 最も簡単な例
というわけで、最も簡単なURDFファイルを作ります。
ただし、ココで言う簡単というのは、必要最低限の要素しか書かないという意味です。
作成するモデルは、ロボットモデルの要素での説明に用いた図のモデルです。

```xml:minimum_robot.urdf
<?xml version="1.0"?>
<robot name="minimum_robot">

<link name="Link1"/>

<joint name="Joint1" type="fixed">
  <parent link="Link1"/>
  <child link="Link2"/>
</joint>

<link name="Link2"/>

<joint name="Joint2" type="fixed">
  <parent link="Link1"/>
  <child link="Link3"/>
</joint>

<link name="Link3"/>

<joint name="Joint3" type="fixed">
  <parent link="Link3"/>
  <child link="Link4"/>
</joint>

<link name="Link4"/>
</robot>
```

もちろん、これだけじゃあ実際のロボットは出来上がりませんが、
最低限の要素を理解しておくことは重要だと思うので、紹介しておきました。

## 最も簡単なCOLLADAを用いた例
続いて、COLLADAファイルを用いたURDFの最も簡単な例を紹介したいと思います。
COLLADAファイルは主に見た目要素を記述するときに重要なので、
**visual**要素を追加したものをご紹介します。

```xml:minimum_visual_robot
<robot name="minimum_visual_robot">

<link name="base_footprint"/>
<joint name="base_link_joint" type="fixed">
  <origin xyz="0 0 0.5"/>
  <parent link="base_footprint"/>
  <child link="base_link"/>
</joint>

<link name="base_link">
   <visual>
    <geometry>
      <mesh filename="package://minimum_visual_robot/meshes/DAE/base/base_link.dae"/>
    </geometry>
  </visual>
</link>

</robot>
```

こんなかんじです。
上の例では、**minimum_visual_robot_description**パッケージが存在すると仮定したときに、
パッケージのディレクトリからのパスを指定して実際のCOLLADAファイルを指定しています。
**collision**要素を追加するときも同様にしてファイルを指定すれば大丈夫です。
尚、**collision**要素で指定するファイルはSTL形式であることが多いようです。
また、STLはバイナリ形式でないとダメなので注意してください。
パッケージの作り方については後述します。

## Gazeboと連携するには
Gazeboと連携するためには、ここまでにご紹介した要素だけでは足りません。
**ros_control**というものの理解が必要になってくるので、この辺りについては、他の記事でご紹介します。
今回の記事では、URDFの基礎を理解することが大事です。

# 実際にパッケージを作ってみよう
というわけで、URDFの構造が大体理解できれば後は書いてみるだけなので、練習あるのみです。
しかし、どこにurdfファイルを置いたらいいのか、COLLADAファイルはどこに置けば良いのか、
なんの指針もなく困ると思いますので、PR2のソースを参考にしたファイル構造をまずご紹介したいと思います。
もちろん、自分の好きな構造でも構いませんが、後々例に出すロボットでは、この構造に沿って作ってますので、
参考にしていただけるとありがたいです。

### パッケージを作る
- 1. catkin workspace を作る（わからなければ、[Wiki](http://wiki.ros.org/ROS/Tutorials/InstallingandConfiguringROSEnvironment)へどうぞ）
- 2. パッケージを作る
   ただし、<>で囲まれているところは、任意の名前と読み替えてください。（<>で囲む必要はありません）

```bash
$ cd <catkin_ws>/src
$ catkin_create_pkg <robot_name>_description
```

- 3. 各種ディレクトリを作る

```bash
$ cd <catkin_ws>/src/<robot_name>_description
$ mkdir mesh
$ mkdir urdf
$ mkdir robots
$ mkdir launch
```

ここまでで、ファイル構造は以下のようになったはずです。

```
test_robot_description
    ├── CMakeLists.txt
    ├── launch
    ├── meshes
    ├── package.xml
    ├── robots
    └── urdf
```
各ディレクトリの説明は以下の通りです。

```
test_robot_description
    ├── CMakeLists.txt
    ├── launch           <- launchファイルを格納
    ├── meshes           <- COLLADA or STLファイルを格納
    ├── package.xml
    ├── robots           <- Xacroを使う際に、各マクロを組み合わせたXacroファイルを格納
    └── urdf             <- URDF or Xacroファイルを格納
```
ここで述べているXacroについては後述しますので、気にしなくて結構です。
つまり、URDFだけ使ってロボットを記述するなら、robotsディレクトリは必要ありません。

### launchファイルをつくる
というわけで、Rvizを用いて作成したURDFを見てみたいと思います。
launchファイルは以下のとおりです。

```xml:display.launch
<launch>
	<arg name="model" />
	<arg name="gui" default="False" />
	<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="state_publisher" />
	<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz" required="true" />
</launch>
```

はい。もうお分かりいただけたと思いますが、**urdf_tutorial**パッケージの**display.launch**そのものです。
一応、コードをお見せしておくほうが、中で何やってるのかわかりやすいかと思ったのでご紹介しました。
実際にlaunchするには、

```bash
$ roslaunch urdf_tutorial display.launch model:='$(find <robot_name>_description)/urdf/robot_name.urdf'
```

以上のコマンドで行います。**\<robot_name\>**の部分は、ご自身のロボット名を当てはめてください。

# Xacroについて
ここまでに、URDFの書き方についてご紹介しました。
ただ、実際に書かれた方ならお分かりかと思いますが、非常に長くなって大変です。
重複している部分も多かったと思います。
このような問題を解決刷るためにXacroというURDFのマクロ記述記法があります。
Xacroにはいろいろな機能がありますので、簡単にご紹介します。

- **Property Block**：定数を定義することができます。
- **Math expressions**：算術記号などを使えるようになります。四則演算、定数同士の計算が可能で、結果を代入することが可能です。
- **Conditional Blocks**：if else 文が使用可能になります。
- **Rospack commands**：roslaunch でも使用されているものです。find や　arg コマンドが使用できるようになります。
- **Macros**：マクロが記述できるようになります。引数をとることができ、マクロの中で使用することができます。
    - **Default parameters**：マクロに指定する引数にデフォルト値を持たせることができます。
- **Including other xacro files**：他のXacroファイルをIncludeできるようになります。
- **YAML support**：YAML形式でプロパティを記述刷ることができます。

以上が代表的な機能です。これ以外にもあるようですが、僕はあまりつかったことがありません。
さらなる情報は参考10をご覧ください。

### Xacroのファイル構成
ということで、Xacroの例をご紹介します。
これまでにご紹介してきた独立二輪型ロボットのXacroです。
複数のXacroファイルを組み合わせて作成してあるので、本記事では、ファイル構成をご紹介します。

```
fourth_robot_description
├── CMakeLists.txt
├── launch
│   └── fourth_robot_description.launch   <- display用のlaunchファイル
├── meshes                                <- CADデータを格納
│   ├── DAE                               <- COLLADAファイルを格納
│   └── STL                               <- STLファイルを格納
├── package.xml
├── robots
│   └── fourth_robot.urdf.xacro           <- ロボットモデルのXacroファイル
└── urdf
    ├── common.xacro                      <- 円周率など、共通定義の記述用Xacro
    ├── base                              <- 胴体部分のXacroファイルを格納
    │   ├── base.gazebo.xacro             <- 胴体部分のGazeboに関する記述用Xacro  
    │   └── base.urdf.xacro               <- 胴体部分の基本的な記述用Xacro
    ├── sensors                           <- センサ部分のXacroファイルを格納
    │   ├── gim30                         ┓ 
    │   │   └── gim30.urdf.xacro        　┃
    │   ├── gps                           ┃
    │   │   └── gps.urdf.xacro            ┣━ 各種センサのXacro
    │   └── lrf                           ┃
    │       ├── lrf.gazebo.xacro          ┃
    │       └── lrf.urdf.xacro            ┛
    └── wheel                             <- 車輪部分のXacroファイルを格納
        ├── wheel.gazebo.xacro            <- 車輪部分のGazeboに関する記述用Xacro
        ├── wheel.transmission.xacro      <- 車輪部分のTransmissionに関する記述用Xacro
        └── wheel.urdf.xacro              <- 車輪部分の基本的な記述用Xacro
```

といった感じです。
一見、複雑に見えるかと思いますが、URDFを全部書くより、効率的で何より整理するのが楽になります。
もちろん、Xacroを使わなくても結構ですが、独立二輪型ロボットを記述するだけでも上記の通りなので、
Xacroをマスターすることを強くおすすめします。

### Xacroのデバッグ
Xacroのデバッグは、一言で言うと面倒くさいです。
というのも、エラーコードも読みづらく（関係ないPythonコード部分のエラーが多い）、慣れるまでは非常に苦労します。
ROSでXacroが使用されるときは、URDFに展開してから使用されるので、
Xacroのデバッグでは、正しくURDFに展開できるかどうかを確認します。
XacroをURDFに展開するには、以下のコマンドを使います。

```bash
$ source <catkin_ws>/devel/setup.bash
$ rosrun xacro xacro.py <robot_name>.xacro -> <output>.urdf
```

大事なことは、\<robot_name\>.xacroが格納されているパッケージにパスが通っていることです。
パスが通っていないと、コードはあっているのにエラーが発生します。これ、初歩的なミスですがよくハマるのでご注意ください。
いずれにせよ、うまく行けば何事もなく\<output\>.urdfにコードが展開されます。
失敗すれば、エラーメッセージが出ますがその大半はPythonコードのエラーメッセージであり、
重要なエラーメッセージは、最終行にどのXacroファイルでエラーが起きたかという情報がありますので、そちらを確認してみてください。

### Xacroをlaunchする
というわけで、エラーも乗り越え、無事にXacroが完成したら、launchしてみましょう。
先ほどURDFで行ったlaunchではうまく行きません。というのも、XacroファイルをURDFに展開するよう記述しなければならないからです。
というわけで、launchファイルをご紹介します。

```xml:display_xacro.launch
<launch>
  <!-- arguments -->
  <arg name="model"/>
  <arg name="gui" default="true" />

  <!-- prameters -->
  <param name="robot_description" command="$(find xacro)/xacro.py '$(arg model)'"/>
  <param name="use_gui" value="$(arg gui)"/>

  <!-- nodes -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
</launch>
```

以上です。お分かりとは思いますが、実際にlaunchする際は引数**model**を指定する必要があります。

### Xacroを使った例
この記事で一貫してご紹介している独立二輪型ロボットのソースをGitHubにて公開しておりますので、よろしければご覧ください。
http://github.com/CIR-KIT/fourth_robot_pkg
launch方法は、以下の通りです。

```bash
$ source <catkin_ws>/devel/setup.bash
$ roslaunch fourth_robot_description fourth_robot_description.launch
```

うまく行けば、以下のようにRviz上で、モデルを確認できるようになります。
![Screenshot from 2016-03-31 15:57:09.png](https://qiita-image-store.s3.amazonaws.com/0/110699/5075d9db-787b-4037-a107-319cb324274e.png)

# まとめ
というわけで、今回はURDFとXacroの書き方についてご紹介しました。
ここまでくると、ROS上で自分のロボットを目視できるようになるのでテンションが上がりますね！
次回はGazeboと連携するために必要な要素についてご紹介します。


# 参考
1. http://wiki.ros.org/urdf
2. http://qiita.com/MoriKen/items/613635b90f3a98042dc5
3. http://qiita.com/yoneken/items/ed2e5edf3aa4e0d8d2e3
4. http://ros-robot.blogspot.jp/2010/01/urdf.html
5. http://fkanehiro.github.io/openhrp3-doc/jp/abstract.html
6. http://wiki.ros.org/ja/urdf/Tutorials/Create%20your%20own%20urdf%20file
7. http://sachinchitta.github.io/urdf2/
8. http://wiki.ros.org/urdf/XML/link
9. http://wiki.ros.org/urdf/XML/joint
10. http://wiki.ros.org/xacro

# **動作環境**
| | |
| :---: | :---: |
| **PC** | Lenovo ThinkPad X240 |
| **Prosessor** |  Intel Core i7-4600U (2.10GHz, 4MB, 1600MHz) |
| **RAM** | PC3-12800 DDR3L (8GB) |
| **OS** | Ubuntu 14.04 LTS 64bit |
| **Kernel** | 3.13.0-44-generic |
| **ROS** | Indigo |

| | |
| :---: | :---: |
| **PC** | Desktop |
| **Prosessor** |  Intel® Core™ i5-4460 CPU @ 3.20GHz × 4  |
| **RAM** | DDR3 (24GB) |
| **OS** | Ubuntu 14.04 LTS 64bit |
| **Kernel** | 3.13.0-83-generic |
| **ROS** | Indigo |