コツがつかめてきた気がするぞ

やること

前回と前々回の記事では最小限の要素のURDFを書いてrvizやgazeboに表示させることを行いました。

今回は関節構造を持った少しロボットらしいURDFを書いていこうと思います。
ツールとして ブラウザでURDFの記述と表示結果を試せるmymodelrobot を使わせていただくので、今回はROSを立ち上げなくてもテストができます。

URDFの概念はシンプルなのですが、いざ記述するとなると手引きが少なく細かい点で試行錯誤をすることになります。ということで、つまづきポイントを中心に記事をまとめてみました。

当方も初心者のため、ドキュメントや専門書も読んでおりますが間違った記述をしている可能性が大いにあります。
その際はご指摘いただければと幸いです。

必要なもの

URDFを試せるブラウザツール mymodelrobot

参考

公式チュートリアル Create your own urdf file
ROS講座
大変参考になりました。ありがとうございます。

概念

URDFの記述方法について、公式サイトではツリー状という表現がされています。
イメージとしてはたとえばヒューマノイドなら腰を中心にしてそこから背骨や足や手を広がるように生やしていって人型にする感じです。
腰からつながる各関節の座標を、関節から関節へと順番にポンポンと相対座標で設定していきます。

概念は簡単なのですが、記述方法に少しクセがあるようなので、なるべくシンプルにわかりやすく説明していきたいと思います。

ブラウザツールを立ち上げてみる

mymodelrobotをクリックして立ち上げると、まず下記のような画面が現れると思います。

右側のビュー画面上でマウス操作をすると、視点を変更することができます。
また、右端のスライダーを操作すると、各関節を操作するともできます。（順運動学）
左側のコードを書き換えると、内容を変更することができます。
数値などを書き換えて下の**「Load robot URDF」**のボタンを押してみましょう。
記述にミスがなければ右側のビュー画面に結果が反映されます。
一度エラーを出した場合に、その後内容を正してもうまく反映されないことがあるかもしれません。その場合はブラウザのリロードボタンを押すとリセットされます。

また座標については右手座標系となります。上図の赤がx軸、緑がy軸、青がz軸で、それぞれ矢印の方向がプラスとなります。
数値を変更したり要素を削ったりしてURDFの挙動を確かめてみてください。

今回のゴール

今回はヒューマノイドの右上半身を作っていきます。
赤点が基準となる腰、青がひねり回転に対応する胸、緑から先が肩関節や肘関節になります。

下の方にコードを用意したのでさっそくコピペして実行してみてください。

関節を持つURDFスクリプトの書き方

URDFでは、空のリンクであるベースリンク（こう呼ぶのが流儀らしい）を起点として、「joint」を記述することでモデル成立させます。
またjointに付随する要素として「link」も設定していきます。URDFにおけるlinkはビジュアライズや衝突判定、質量や慣性計算などに使う要素を司りますが、今回はビジュアラアイズ部分のみを使います。

「joint」と「link」はペアになるように記述していきます。
新しいモデルを記述する際には、主役はあくまで「joint」であり、「link」はそれにぶら下がるおまけと考えると頭が整理しやすくなります。（理由は後述します）
とにかく、jointを次々と設定し、それにlinkをぶら下げていく方式で進めます。

ちなみにヒューマノイドの部位のネーミングルールについては、いろいろと一般的なものを調べて最大公約数的なものを下記にまとめてみました。
https://note.com/ninagawa123/n/n38234d690149

jointに対してlinkをどうぶら下げるか？

今回は描写や整理の都合上、「joint」の座標に対して、同じ座標に「link」のビジュアル用のローカル原点を設定しています。
腰から始まるツリー構造で考えたとき、肘関節とペアになっているのは上腕であるというイメージです。
上腕linkに固定された肘jointを回転させると、手首jointの位置が移動する（ので、手首に固定されている前腕linkも一緒に動く）というような感じなのですが…ちょっとわかりにくかったらすみません。

また、記述の際はその主役であるjointに対して、"親となるjoint"と"子となるjoint"も設定していきます。こうすることで各要素が樹状の関係になっていきます。

jointの座標をどうやって決定するか？

これについては、手作業で書いていく際にはCADや実物で測定を行い、URDFに転記していくことになります。
原点となるベースリンクから順に、回転軸との差分をみていきます。
肩のロール＆ピッチや、肘のヨー＆ピッチなどは、その交点をジョイントの座標とするとよいかもしれません。あとでdaeやstlといった3Dモデルを重ねたり、質量の重心のオフセットなどを追記する際にも都合がよさそうです。
また腰ヨー軸や頭ヨー軸など、他の軸と交わらずジョイントのz座標を決めかねる場合があるかもしれません。その場合はサーボの中心をローカル原点とするといった基本ルールを決めておくと良いと思います。
全jointをサーボの中心で統一する流儀もあるかもしれませんが、どれが主流かはまだ調べきれていません。
今回の例では、交点ジョイントとサーボ中心ジョイントを使い分け、なおかつビジュアルの原点を0,0,0にしたときに、軸の関係がわかりやすいように調整しています。（elbowのz軸）

具体的にスクリプトを見ていきます。

<?xml version="1.0"?>
<robot name="Roid1">

  <!--base link 基準点-->
  <link name="base_link">
  </link>
  <!--ここでbase_to_waistのジョイント座標は(0,0,0) 原点0-->

  <!--base to waist 基準点からの腰の位置(赤点A)-->
  <joint name="base_to_waist" type="fixed">
    <parent link="base_link"/>
    <child link="waist"/>
    <origin xyz="0 0 0.277936"/><!--waist座標:前のジョイント座標 原点0 からの相対座標-->
  </joint>
  <link name="waist">
    <visual>
      <geometry>
        <box size="0.03 0.03 0.03"/>
      </geometry>
      <origin xyz="0 0 0"/><!--waist座標からの相対位置に赤点Aを描写-->
      <material name="red">
        <color rgba="1.0 0.0 0.0 1"/>
      </material>
    </visual>
  </link>

  <!--waist to chest 腰ヨー軸の設定と胸の位置(青点B)-->
  <joint name="waist_to_chest_cy" type="revolute">
    <parent link="waist"/>
    <child link="chest"/>
    <axis xyz="0 0 1"/>
    <origin xyz="0 0 0.065192"/><!--chest座標:waist座標からの相対座標-->
    <limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
  </joint>
  <link name="chest">
    <visual>
      <geometry>
        <box size="0.03 0.03 0.03"/>
      </geometry>
      <origin xyz="0 0 0"/><!--waist座標からの相対位置に青点Bを描写-->
      <material name="blue">
        <color rgba="0.1 0.1 1.0 1"/>
      </material>
    </visual>
  </link>

  <!--chest to shoulder_r 肩ピッチ軸(黄点C)-->
  <joint name="chest_to_shoulder_rr" type="revolute">
    <parent link="chest"/>
    <child link="shoulder_r"/>
    <axis xyz="0 1 0"/>
    <origin xyz="0.011221 -0.074375 0"/><!--shoulder座標:chest座標からの相対座標-->
    <limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
  </joint>
  <link name="shoulder_r">
    <visual>
      <geometry>
        <box size="0.03 0.03 0.03"/>
      </geometry>
      <origin xyz="0 0 0"/><!--shoulder座標からの相対位置に黄点Cを描写-->
      <material name="yellow">
        <color rgba="1.0 1.0 0.1 1"/>
      </material>
    </visual>
  </link>

  <!--shoulder to uppararm_r 肩ロール軸(緑点D)-->
  <joint name="shoulder_to_uppararm_rr" type="revolute">
    <parent link="shoulder_r"/>
    <child link="uppararm_r"/>
    <axis xyz="1 0 0"/>
    <origin xyz="0 0 0"/><!--uppararm座標:shoulder座標からの相対座標-->
    <limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
  </joint>
  <link name="uppararm_r">
    <visual>
      <geometry>
        <box size="0.01 0.05 0.05"/>
      </geometry>
      <origin xyz="0 0 0"/><!--uppararm座標からの相対位置に緑点Dを描写-->
      <material name="green">
        <color rgba="0.1 1.0 0.1 1"/>
      </material>
    </visual>
  </link>

  <!--uppararm to elbow_r 肘ヨー軸(紫点E)-->
  <joint name="uppararm_to_elbow_rp" type="revolute">
    <parent link="uppararm_r"/>
    <child link="elbow_r"/>
    <axis xyz="0 0 1"/>
    <origin xyz="0.011221 0 -0.054259"/><!--elbow座標:uppararm座標からの相対座標-->
    <limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
  </joint>
  <link name="elbow_r">
    <visual>
      <geometry>
        <box size="0.03 0.03 0.03"/>
      </geometry>
      <origin xyz="0 0 0"/><!--elbow座標からの相対位置に紫点Eを描写-->
      <material name="purple">
        <color rgba="1.0 0.1 1.0 1"/>
      </material>
    </visual>
  </link>

  <!--lowerarm to lowerarm_r-->
  <joint name="elbow_to_lowerarm_rp" type="revolute">
    <parent link="elbow_r"/>
    <child link="lowerarm_r"/>
    <axis xyz="0 1 0"/>
    <origin xyz="0.004698 0 -0.033252"/><!--lowerarm座標:elbow座標からの相対座標-->
    <limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
  </joint>
  <link name="lowerarm_r">
    <visual>
      <geometry>
        <box size="0.03 0.03 0.03"/>
      </geometry>
      <origin xyz="0 0 0"/><!--lowerarm座標からの相対位置に白点Fを描写-->
      <material name="white">
        <color rgba="0.8 0.8 0.8 1"/>
      </material>
    </visual>
    <visual>
      <geometry>
        <box size="0.03 0.01 0.04"/>
      </geometry>
      <origin xyz="0 0 -0.071011"/><!--白点Fからの相対位置にハンドとして白点Gを描写-->
    </visual>
  </link>

</robot>

~~とにかく今回はlinkの位置をjointの位置に合わせて構成してみました。~~（←むしろこれしか方法がないようです。ゆきさんご指摘ありがとうございます。）
質量を持たせる場合にもビジュアルとは別のタグで重心を設定できるので、リンクのorigin座標についてはとくに気にせず使ってよさそうです。軸の中心で設定しておけば、stlやdaeといったビジュアル用の3Dモデルの位置を設定するときにも便利そうです。

とまどいポイント

物理でリンク構造を扱う場合、「リンク」といえば棒の部分で、「ジョイント」といえば回転や支点の軸の部分を指すと思います。
なので、joint同士の距離を決定するパラメータは、物理と同じようにlinkに含まれるものと無意識に考えてしまいました。
しかしURDFで登場するjointでは、jointから次のjointへの相対座標が方向と距離の両方の情報を持つことになり、パラメータ的にはlinkの成分も含んでいることになります。
そういう事情があるため、linkのタグの中で設定する数値が全体の位置にどう影響するのだろうと最初は頭が混乱してしまいました。
ジョイントをメインで記述を考え、リンクは後からぶら下げるというふうに頭を切り替えたところ、スッキリと理解できるようになりました。
gazeboなどでのシミュレーション時にはリンクに質量などの情報を入れるので、URDFでのlinkという表現には納得はしていますが、このあたりが理由でとっつきにくさを感じる人もいるかもしれません。
今回の記事が理解の一助になればと思います。

つづき

3Dモデルを表示することに挑戦していきたいと思います。

LINKの原点をどうするかについての記事