ROS講座49 オムニホイールのシミュレーション1（オムニホイールのモデリング）

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

| 項目 | 値 |
|:-:|:-:|
| 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)を参照してください。

# 概要
オムニホイールによる全方向移動機構をgazenoでシミュレーションで行います。今回はオムニホイールのモデリングを行います。

## オムニホイールとは
オムニホイールとはフレームの中に垂直に「バレル」という小さなタイヤが入っているものです。タイヤの本来の回転軸方向と垂直な方向にも動けます。

![Triple_Rotacaster_commercial_industrial_omni_wheel.jpg](https://qiita-image-store.s3.amazonaws.com/0/254442/53c0e1ce-49b2-fdeb-3dff-17fa1b841b8e.jpeg)
[wikipediaより](https://en.wikipedia.org/wiki/Omni_wheel)

## 全方向移動機構とは
普通のタイヤを使った差動2輪機構では前後・回転方向のみで横方向への移動はきませんが、全方向移動機構はこれに横方向の移動も合わせた水平面上での全方向の移動が可能です。これを実現する方式の1つとしてオムニホイール3つを使った方法があります。

## シミュレーションの方法
バレルが多いために簡易的にバレルの無視してタイヤの横方向の滑りをなくす方法もあるらしいのですが、今回は愚直にバレルまですべてシミュレーションします。

# オムニホイールのモデリング
[Nexusの38mmオムニホイール](https://www.vstone.co.jp/robotshop/index.php?main_page=product_info&products_id=4568)を対象にします。

![nexus.jpg](https://qiita-image-store.s3.amazonaws.com/0/254442/d82f8b42-8cb4-8c47-749b-8a1c4f8f60b5.jpeg)


[ロボットショップより](https://www.vstone.co.jp/robotshop/index.php?main_page=product_info&products_id=4568)
 
これをもとにモデルを作ります。作りやすさのために直径は38mmに変えました。
今回は全てFusion360を使いました。機械系のCADと考えると機能不足も少しありますが、初心者でも使いやすく、3Dプリンタやレーザーカッターといったラピットプロトタイピングとは相性が良いCADです。
以下のCADで形を作った後にROSで使う方法については[ROS講座34](https://qiita.com/eri_aka/items/d247c998929a8be9da21)を参照してください。

## バレルのモデリング
バレルの外側のカーブはΦ38mmの曲線になるようにします。実測したところ一番太いところでΦ10mmでした。1つのオムニホイール当たり円の1/8以上をカバーしなければなりません。そのためにはバレルの長さは14.5mm以上必要です。実測したところ大体16mmでした。

![omni_barrel.png](https://qiita-image-store.s3.amazonaws.com/0/254442/ef513248-9b35-6475-982e-89a273989338.png)

立体化すると以下のような感じです。本当は軸が通る穴等もっと作りこむところはありますが、こんな感じで

![omni_barrel_3d.png](https://qiita-image-store.s3.amazonaws.com/0/254442/f18ac84a-7957-8201-a537-74f5803ec7dc.png)

## フレームのモデリング
フレームは8つのバレルを固定する部品です。計るのが大変なので大体です。

![omni_frame.png](https://qiita-image-store.s3.amazonaws.com/0/254442/d2bfe98f-154a-79c5-2599-fbb21f7e9b73.png)

これを3D化すると以下のようになります。

![omni_frame_3d.png](https://qiita-image-store.s3.amazonaws.com/0/254442/e0dbf7ec-db8d-264b-5022-967c70064015.png)

# ソースファイル
## オムニホイールの記述のURDF(xacro)

* まずバレルを記述するxacroを作ります。そのあとにオムニホイールを記述するxacroを作りその中でフレームと8つのバレルを呼び出します。
* バレルには衝突判定（collision）を付けますが、フレームには付けませんでした。
* バレルの回転は自由回転で、モーターの軸の回転はros_controlで制御します。
* モーターの軸は、モータのフレームの穴に「ピッタリ」嵌まっています。そのために両方に衝突判定（colisionn）を付けると物理演算用の剛体同士の摩擦で動かなくなります。そのために軸には衝突判定を付けませんでした。
* バレルの衝突に関する物理演算用の弾性と粘性を設定します(`<kp value="5000" />`と`<kd value="10" />`)。これによってgazeboに入れたときにオムニホイールが回転してバレルが地面に接触したときの振動が抑えられます。

```xml:sim2_lecture/xacro/odm_omni_wheel_set1.xacro
<robot xmlns:xacro="http://ros.org/wiki/xacro">  
  <xacro:macro name="barrel" params="prefix parent dir">
    <joint name="${prefix}_joint" type="continuous"><!-- continuous -->
      <parent link="${parent}"/>
      <child link="${prefix}_link"/>
      <origin rpy="${pi/2} 0 ${dir}" xyz="${0.014*cos(dir)} ${0.014*sin(dir)} 0.006"/>
      <axis xyz="0 0 1"/>
      <dynamics damping="0.001"/>
    </joint>
    <transmission name="${prefix}_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${prefix}_joint">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="${prefix}_motor">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>
    <link name="${prefix}_link">
      <visual>
        <geometry>
          <mesh filename="package://sim2_lecture/stl/omni_barrel.stl" />
        </geometry>
        <material name="black"/>
      </visual>
      <collision>
        <geometry>
          <mesh filename="package://sim2_lecture/stl/omni_barrel.stl" />
        </geometry>
      </collision>
      <inertial>
        <mass value="0.002"/>
        <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
      </inertial>  
    </link>
    <gazebo reference="${prefix}_link">
      <mu1 value="2.5" />
      <mu2 value="2.5" />
      <kp value="5000" />
      <kd value="10" />
      <material>Gazebo/Black</material>
    </gazebo>

    <joint name="${prefix}_mark_joint" type="fixed">
      <parent link="${prefix}_link"/>
      <child link="${prefix}_mark_link"/>
      <origin rpy="0 0 0" xyz="0.005 0 0"/>
    </joint>
    <link name="${prefix}_mark_link">
      <visual>
        <geometry>
          <box size="0.005 0.005 0.005"/>
        </geometry>
        <material name="white"/>
      </visual>
    </link>
    <gazebo reference="${prefix}_mark_link">
      <material>Gazebo/White</material>
    </gazebo>
  </xacro:macro>

  <xacro:macro name="motor" params="prefix parent">
    <joint name="${prefix}_joint" type="fixed">
      <parent link="${parent}"/>
      <child  link="${prefix}_body_link"/>
    </joint>
    <link name="${prefix}_body_link">
      <visual>
        <geometry>
          <mesh filename="package://sim2_lecture/stl/ga25_body.stl" />
        </geometry>
        <material name="gray"/>
      </visual>
      <collision>
        <origin rpy="0 ${radians(90)} 0" xyz="-0.025 0 0"/>
        <geometry>
          <cylinder radius="0.012" length="0.05" />
        </geometry>
      </collision>
      <inertial>
        <mass value="0.150"/>
        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
      </inertial>  
    </link>
    <gazebo reference="${prefix}_body_link">
      <material>Gazebo/Gray</material>
    </gazebo>

    <joint name="${prefix}_shaft_joint" type="continuous">
      <parent link="${prefix}_body_link"/>
      <child  link="${prefix}_shaft_link"/>
      <origin rpy="0 0 0" xyz="0.012 0 0"/>
      <axis xyz="1 0 0"/>
      <limit velocity="40.0" effort="3.0" />
      <dynamics damping="0.01" friction="0.0"/>
    </joint>
    <transmission name="${prefix}_shaft_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="${prefix}_shaft_joint">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      </joint>
      <actuator name="${prefix}_shaft_motor">
        <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>

    <link name="${prefix}_shaft_link">
      <visual>
        <geometry>
          <mesh filename="package://sim2_lecture/stl/ga25_shaft.stl" />
        </geometry>
        <material name="gray"/>
      </visual>
      <inertial>
        <mass value="0.010"/>
        <inertia ixx="0.0000005" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
      </inertial>  
    </link>
    <gazebo reference="${prefix}_shaft_link">
      <material>Gazebo/Gray</material>
    </gazebo>
  </xacro:macro>

  <xacro:macro name="omni_wheel_set" params="prefix parent">
    <joint name="${prefix}_base_joint" type="fixed">
      <parent link="${parent}"/>
      <child link="${prefix}_base_link"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </joint>
    <link name="${prefix}_base_link" />
    
    <xacro:motor prefix="${prefix}_motor" parent="${prefix}_base_link" />

    <joint name="${prefix}_housind_joint" type="fixed">
      <parent link="${prefix}_motor_shaft_link"/>
      <child  link="${prefix}_housing_link"/>
      <origin xyz="0.012 0 0" rpy="0 ${radians(90)} 0"/>
    </joint>
    <link name="${prefix}_housing_link">
      <visual>
        <geometry>
          <mesh filename="package://sim2_lecture/stl/omni_housing.stl" />
        </geometry>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <material name="gray"/>
      </visual>
      <inertial>
        <mass value="0.025"/>
        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
      </inertial>  
    </link>
    <gazebo reference="${prefix}_housing_link">
      <material>Gazebo/Gray</material>
    </gazebo>

    <joint name="${prefix}_housing_s_joint" type="fixed">
      <parent link="${prefix}_housing_link"/>
      <child link="${prefix}_housing_s_link"/>
      <origin rpy="0 ${pi} ${pi/4}" xyz="0 0 0"/>
    </joint>
    <link name="${prefix}_housing_s_link">
      <inertial>
        <mass value="0.025"/>
        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
      </inertial>  
    </link>

    <xacro:barrel prefix="${prefix}_barrel_1" parent="${prefix}_housing_link" dir="0"/>
    <xacro:barrel prefix="${prefix}_barrel_2" parent="${prefix}_housing_link" dir="${pi/2}"/>
    <xacro:barrel prefix="${prefix}_barrel_3" parent="${prefix}_housing_link" dir="${pi}"/>
    <xacro:barrel prefix="${prefix}_barrel_4" parent="${prefix}_housing_link" dir="${-pi/2}"/>
    <xacro:barrel prefix="${prefix}_barrel_5" parent="${prefix}_housing_s_link" dir="0"/>
    <xacro:barrel prefix="${prefix}_barrel_6" parent="${prefix}_housing_s_link" dir="${pi/2}"/>
    <xacro:barrel prefix="${prefix}_barrel_7" parent="${prefix}_housing_s_link" dir="${pi}"/>
    <xacro:barrel prefix="${prefix}_barrel_8" parent="${prefix}_housing_s_link" dir="${-pi/2}"/>
  </xacro:macro>
</robot>
```

# 実行
各ターミナルごとに実行前に`source ~/catkin_ws/devel/setup.bash`を実行する必要があります。

```shell
roscd sim2_lecture/xacro
roslaunch urdf_tutorial display.launch model:=odm_single_wheel_only.xacro
```

![wheel_single.png](https://qiita-image-store.s3.amazonaws.com/0/254442/6d4c3af2-b54a-6fdf-4ba4-16b7deabe379.png)
※tfの表示などの設定は変えています。

バレルについている「白い箱」はバレルが回転しているかを確認するためのもので衝突判定や重さはありません。

# 補足

## 自由回転する軸について
オムニホイールの「バレル」は特に動力はなく自由回転する軸です。これを実現するにはurdfでその軸をeffort_interfaceで定義します。これの初期目標値は0なのでつまり力のかからない自由回転する軸になります。

## イナーシャのパラメーターについて
実は上記のurdfでのオムニホイールの各パーツのイナーシャは実際よりもだいぶ大きい値になっています。これはミスではなくわざと行っています。
どうやらgazeboはオムニホイールのような細かな物理現象を解くのに向いているわけではないみたいで（細かい物理演算を再現するにはシミュレーションステップを細かくする必要がある？）、設定によっては振動したりパーツが滑ったりはじけたりしまいます。今回は最終的にうまくシミュレーションできたパラメーターを使用しています。

# 参考
[wikipedia OmniWheel](https://en.wikipedia.org/wiki/Omni_wheel)
[omni planner](https://answers.ros.org/question/212889/gazebo-planar-move-plugin-for-omni-directional-wheel/)
[gazeboの物理特性の一覧](http://gazebosim.org/tutorials/?tut=ros_urdf#%3Cgazebo%3EElementsForLinks)

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