平面３リンクマニピュレータ用の形状モデルのURDF記述

![Screenshot from 2021-02-09 16-45-04.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/663760/4406ef74-b066-c95b-99b3-70c0495ee08b.png)

#はじめに

ROSの３Dモデル記述形式URDFを使って、プチロボMS3Lを記述してみました（上掲）。「ロボット１日開発 初めてのROS&位置推定」という特集タイトルに惹かれて（騙されて、とまでは言いません）買った「トランジスタ技術」2020年９月号の第６章「自立移動ロボット用形状モデルの作り方」を参考にしました。マクロ言語xacroは使用していないせいか、結果、非常に長いXMLファイルになりました。ROSオフィシャルのチュートリアルではR2のモデリングのビデオもありますが、気が遠くなるような作業です。

#環境

- ubuntu 18.04 LTS
- ROS melodic
- VS Code

#XMLコード

- 注意点など

　1. ジョイントの位置やリンクの長さは、MS3Lを実測した概算数字をベースにしています。
　2. urdfの長さの単位はメートルですので、このモデリングの記述は、実寸の約10倍です。
　3. 先端部分には、ブラケットを縦に取り付けてフックのように使えるようにしました。
　　（こちらの動画に感銘をうけました。でも、プチロボのブラケットでは・・・。）
　　https://www.youtube.com/watch?v=gmQzhQwnh5k&feature=emb_logo

- マニュピュレータ用のurdfを書くときのちょっとしたコツ

1. 最初に、ジョイントありき。リンクより先に複数のジョイントの位置を骨格に決める。
2. リンクの長さは、ジョイント間の長さと同じにしてしまう。デザインに凝るとハマる。
3. リンクは、位置を指定せずにほっておくとリンクの中央にジョイントが配置される。そこでリンク長の半分ほどを`origin xyz`のZ軸の値で持ち上げる（Z軸方向に垂直にリンクを伸ばしている場合）とちょうどリンクの真下にジョイントが来る。 
4. まだ、xacroは使っていないが、便利そう。今後の課題。（前途多難の予想）

```simple_ms3l.urdf
<robot name="test_robot">
  <material name="red">
    <color rgba="1.0 0.0 0.0 2.0"/>
  </material>
  <material name="green">
    <color rgba="0.0 1.0 0.0 2.0"/>
  </material>
  <material name="blue">
    <color rgba="0.0 0.0 1.0 2.0"/>
  </material>
  <material name="white">
    <color rgba="1.0 1.0 1.0 2.0"/>
  </material>

  <link name="base_link"/>

  <joint name="base_joint" type="fixed">
    <parent link="base_link"/>
    <child  link="srv0_body"/>
  </joint>

  <link name="srv0_body">
    <visual>
      <geometry>
        <box size="0.25 0.15 0.3"/>
      </geometry>
      <origin xyz="0 0 0.15" rpy="0 0 0"/>
      <material name="green"/>
    </visual>
  </link>

  <joint name="joint1" type="revolute">
    <parent link="srv0_body"/>
    <child  link="srv2_body"/>
    <origin xyz="0 0 0.3" rpy="0 0 0"/>
    <limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
  </joint>
  <link name="srv2_body">
    <visual>
      <geometry>
        <box size="0.25 0.15 0.5"/>
      </geometry>
      <origin xyz="0 0 0.25" rpy="0 0 0"/>
      <material name="blue"/>
    </visual>
  </link>

  <joint name="joint2" type="revolute">
    <parent link="srv2_body"/>
    <child  link="srv3_body"/>
    <origin xyz="0 0 0.5" rpy="0 0 0"/>
    <limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
  </joint>
  <link name="srv3_body">
    <visual>
      <geometry>
        <box size="0.25 0.15 0.5"/>
      </geometry>
      <origin xyz="0 0 0.25" rpy="0 0 0"/>
      <material name="red"/>
    </visual>
  </link>
  
  <joint name="joint3" type="revolute">
    <parent link="srv3_body"/>
    <child  link="head_body"/>
    <origin xyz="0 0 0.5" rpy="0 0 0"/>
    <limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
  </joint>
  <link name="head_body">
    <visual>
      <geometry>
        <box size="0.25 0.15 0.2"/>
      </geometry>
      <origin xyz="0 0 0.1" rpy="0 0 0"/>
      <material name="white"/>
    </visual>
  </link>

  <joint name="head1_joint" type="fixed">
    <parent link="head_body"/>
    <child  link="head2_body"/>
    <origin xyz="0 -0.03 0.1" rpy="0 0 0"/>
    <limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
  </joint>
  <link name="head2_body">
    <visual>
      <geometry>
        <box size="0.15 0.4 0.01"/>
      </geometry>
      <origin xyz="0 0 0.1" rpy="0 0 0"/>
      <material name="white"/>
    </visual>
  </link>

  <joint name="head2_joint" type="fixed">
    <parent link="head2_body"/>
    <child  link="head3_body"/>
    <origin xyz="0 0.2 0.15" rpy="0 0 0"/>
  </joint>
  <link name="head3_body">
    <visual>
      <geometry>
        <box size="0.15 0.01 0.3"/>
      </geometry>
      <origin xyz="0 0 0.1" rpy="0 0 0"/>
      <material name="white"/>
    </visual>
  </link>
  <joint name="head3_joint" type="fixed">
    <parent link="head2_body"/>
    <child  link="head4_body"/>
    <origin xyz="0 -0.2 0.12" rpy="0 0 0"/>
  </joint>
  <link name="head4_body">
    <visual>
      <geometry>
        <box size="0.15 0.01 0.2"/>
      </geometry>
      <origin xyz="0 0 0.1" rpy="0 0 0"/>
      <material name="white"/>
    </visual>
  </link>
</robot>
```

#あとがき

1. 公式（？）のTutorialは、読んでも全然わからない（なんで、あんな葉っぱのような例を使って説明しているのか不思議でならない）から、他人の作ったものからノウハウを盗むほうが全然早いとおもう。（全くの個人としての感想です）
2. urdfの文法チェックツールは、正直なところこの程度の構造の場合は無用の長物。RVizで都度可視化して位置関係をチェックするほうが結局速い。時間はかかるが、トライアンドエラー風はわかりやすい。
3. マニピュレータの慣性モーメントってどうやって算出するんだろうか。プチロボのような小さな装置では影響はほぼないに等しいが、大きなものになったら、腕をブンブン回したら壊れてしまうだろうから相当に重要な数値のはずなのであるから、算出方法もきっと誰かが説明してくれているに違いない。