ROS講座38 gazebo modelを作成する

# 環境
この記事は以下の環境で動いています。

| 項目 | 値 |
|:-:|:-:|
| CPU | Core i5-8250U |
| Ubuntu | 20.04 |
| ROS | Noetic |
| Gazebo | 11.9.0 |

インストールについては[ROS講座02 インストール](https://qiita.com/srs/items/e0e0a9dc3f94c2d3348e)を参照してください。
またこの記事のプログラムはgithubにアップロードされています。[ROS講座11 gitリポジトリ](https://qiita.com/srs/items/25a56a6abda41dcf396e)を参照してください。

# 概要
前回はroslaunchをするときにworldファイルを指定することで自由にgazenoシミュレーター上でのworldを作ることができるということを解説しました。しかし前回の方法では`~/.gazebo/model`にある、gazeboがもともと持っているmodelしかworldファイルで指定できません。今回は自作のmodelを作成してworldファイルで使用する方法を説明します。

# gazebo modelの作成

box1という名前のmodelを作ります。model.sdfとmodel.configの2つのファイルが必要です。それぞれが同じモデル名のディレクトリの中にある必要があります。

## model.sdf
```xml:gazebo1_lecture/models/simple_box/model.sdf
```
要素としてはほとんどurdfと同じです。一辺が1mの立方体になります。

## model.config

```xml:sim3_lecture/models/box1/model.config
<?xml version='1.0'?>
<sdf version="1.4">
  <model name="simple_box">
    <pose>0 0 0.5 0 0 0</pose>
      <link name="link">
      <inertial>
        <mass>1.0</mass>
        <ixx>0.083</ixx>
        <ixy>0.0</ixy>
        <ixz>0.0</ixz>
        <iyy>0.083</iyy>
        <iyz>0.0</iyz>
        <izz>0.083</izz>
      </inertial>
      <collision name="collision">
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
        <material>
          <script>
            <uri>file://media/materials/scripts/gazebo.material</uri>
            <name>Gazebo/Red</name>
          </script>
        </material>
      </visual>
    </link>
  </model>
</sdf>
```

* 最初の`sun`と`ground_plane`ではそれぞれ照明と床モデルを取り込んでいます。
* `<model name="box1">`~`</model>`までが箱のモデルの記述です。`<inertial>`、`<collision>`、`<visual>`の3つの主な要素を記述します。
    * `<inertial>`は重さについての要素です。重量とイナーシャ（後述）を記述します。物理演算に使われる値です。
        * `<math>`は重さでkg単位で書きます。
        * `<inertia>`は慣性モーメントで、回転方向の慣性に相当します。6パラメーターで記述しますが、計算が難しいので後述
    * `<collision>`は衝突検出に使われる値です。例えば地面の上に箱があるのは地面と箱の表面で衝突があるからです。これがないと無限に下に落ちてしまいます。
    * `<visual>`はGUIでの表示に使われる値です。基本は`<collision>`と同じ形にします。

# モデルの使い方

## worldファイル
```xml:gazebo1_lecture/worlds/box.world
<?xml version="1.0" ?>
<sdf version="1.5">
  <world name="default">
    <include>
      <uri>model://sun</uri>
    </include>

    <include>
      <uri>model://ground_plane</uri>
    </include>

    <include>
      <uri>model://simple_box</uri>
    </include>
  </world>
</sdf>
```

`<uri>model://box1</uri>`の部分で指定しているモデルが前回と変わりました。

# 実行
上記の状態で実行しただけだとmodelにパスが通っていないためにモデルが出現しません（もしくはgazebo自体が立ち上がらない）。そのためにパスを通す必要があるのですが3通りの方法があります。

## コマンドラインでパスを通して実行
上の例では`gazebo1_lecture/sdf/simple_box/`以下にモデルファイルを置きましたが、gazeboにパスが通っていないので実行してもgazeboがモデルファイルを見つけることができません。環境変数の`$GAZEBO_MODEL_PATH`にディレクトリを追加するとgazeboが見つけることができます。よって以下のようにすると実行できます。

```shell:モデルをpathに追加してgazeboを実行
roscd gazebo1_lecture/models/
export GAZEBO_MODEL_PATH=`pwd`:$GAZEBO_MODEL_PATH
roscd sim3_lecture/worlds/
gazebo box.world 
```

同様にパスを指定してからならroslaunchも実行できます。

```shell:モデルをpathに追加してgazeboを実行
roscd gazebo1_lecture/models/
export GAZEBO_MODEL_PATH=`pwd`:$GAZEBO_MODEL_PATH
roscd sim3_lecture/worlds/
roslaunch gazebo_ros empty_world.launch world_name:=`pwd`/box.world 
```

どちらの方法でも以下のような画面が現れます。
![sim_model.png](https://qiita-image-store.s3.amazonaws.com/0/254442/a0ff5d95-8b1f-0b68-be95-609fc0ea10ad.png)

しかしこの方法はスマートでないので避けたいところです。

## package.xmlに記述を加えてroslaunchで実行
 
rosを使う場合の正攻法です。
packge.xmlに記述を加えると上記の環境変数の設定相当の処理を行ってくれます。
`<exec_depend>gazebo_ros</exec_depend>`は実行依存の記述で、`<gazebo_ros gazebo_model_path="${prefix}/models" />`はexportと同様のことができる記述です。ここでの`${prefix}`はこのファイル（package.xml）があるパスです。パスはmodelフォルダがあるディレクトリを直接示さないといけません。例えば`<gazebo_ros gazebo_model_path="${prefix}"/>`と書くと動きません。

```xml:sim3_lecture/package.xmlに追加
<package>
  (中略)
  <exec_depend>gazebo_ros</exec_depend>
  <export>
    <gazebo_ros gazebo_media_path="${prefix}/worlds" />
    <gazebo_ros gazebo_model_path="${prefix}/models" /> 
  </export>
</package>
```

gazebo_media_pathはworldファイルへパスを通していて、gazebo_model_pathはgazeboモデルのファイルにパスを通しています。この記述をすれば環境変数の設定をすることなく以下のコマンドで実行できます。
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。


```shell:launchで実行
roslaunch gazebo_ros empty_world.launch world_name:=box.world 
```

# イナーシャについて

イナーシャは慣性モーメントとも呼ばれ、回転の運動において「重さ」に相当するものです。重さ（mass）は1次元ですが、イナーシャは回転方向も考慮した値で6次元の値になります。行列で書かれ

```math
\begin{pmatrix}
I_xx & I_xy & I_xz\\
I_yx & I_yy & I_yz\\
I_zx & I_zy & I_zz
\end{pmatrix}
```

となります。これだと9次元あるように見えますが、$I_xy=I_yx, I_yz=I_zy, I_zx=I_xz$となるので6次元です。6次元の中でも対角要素$(I_xx,I_yy,I_zz)$と非対角要素$(I_xy,I_yz,I_xz)$があり、軸の方向をうまくとると非対角要素は0となることが知られています。正確な値はCADなどでないと出すのが難しいのですが、基本的な図形なら[Webサイト](https://rivi-manufacturing.com/calculation-tool/1601/)で計算もできます。
大体の値でよいな

```math
I_{xx} = I_{yy} = I_{zz} = \frac{MD^2}{10}\\
I_{xy} = I_{yz} = I_{zx} = 0\\
(D:長さ[m]、M:重さ[kg])
```

とすると程よい値になります。小さい部品をたくさん組み合わせるロボットのようなものでは、この程度のざっくりした値でどうにかなります。逆にこのざっくりした値からかけ離れた値が入っているとシミュレーションが期待した動作をしなくなったり、計算のエラーでモデルが破城したり、モデルが吹き飛んだり、fixedで固定しているjointでうまく固定できない（拘束条件が満たせない）ということがあります。

# 参考
[gazeno tutorial: modelの作り方](http://gazebosim.org/tutorials?tut=build_model)
[ROSパッケージのディレクトリにgazeboのpathを通す](https://answers.ros.org/question/212113/problems-viewing-collada-files-in-gazebo-ros/)
[envタグでgazebo_modelにパスを通す](http://olfash.hateblo.jp/entry/fetch_experiment)

# 目次ページへのリンク
[ROS講座の目次へのリンク](https://qiita.com/srs/items/5f44440afea0eb616b4a)