Gazebo/MoveIt のための 3D モデリング（１４）MoveIt の動モデルの作成
本シリーズ前回の記事 Gazebo/MoveIt のための 3D モデリング（１３）MoveIt の静モデルの作成 では CAD などからエクスポートしたメッシュデータファイルを MoveIt の静モデルとしてモデルファイルに組み込んで表示する方法を紹介しました．
今回は洗濯機の URDF (Unified Robot Description Format) モデルにドアのヒンジなどの動く箇所を設定して，より機械らしい（ロボットに近い）モデルにする様子を紹介します．
各リンクモデルメッシュのエクスポート
前回の MoveIt の静モデル作成においては洗濯機全体として1つのメッシュデータファイル（ DAE もしくは STL ）をエクスポートして利用しました．
MoveIt のリンクモデル作成では各リンクに対応したメッシュをそれぞれエクスポートしてそれぞれのリンクのメッシュファイルとして利用します．
今回の洗濯機モデルでは次の3つのリンク構成にします．
- 洗濯機本体: main-body
- 洗濯槽の扉: door
- 洗剤投入トレイ: tray
今回は「洗濯機本体（main-body）」は元の洗濯機全体の座標系そのままとするので配置の変更はしません．
「洗濯槽の扉（door）」と「洗剤投入トレイ（tray）」の形状データを各リンクの座標系原点に配置します．
元々の洗濯機全体の座標系で配置されたオブジェクトを残しつつ，別途各リンクのエクスポート用にリンク座標系の原点にオブジェクトを配置してメッシュデータとしてエクスポートします．
Rhinoceros では右の図のようにオブジェクトを含む既存のレイヤを右クリックするとメニューに「レイヤとオブジェクトを複製」ができるのでこの機能で複製した先のレイヤで作業すると良いでしょう．
各リンク座標系基準の配置用レイヤでそれぞれの各リンクは次のように配置しました．
- 洗濯機本体: main-body
- 位置: 変更なし
- 角度: 変更なし
- 洗濯槽の扉: door
- 位置: 開閉ヒンジ回転軸の中心が座標原点
- 角度: 開閉ヒンジ回転軸を Z軸 に一致
- 洗剤投入トレイ: tray
- 位置: 最後下部エッジ中心が座標原点
- 角度: 変更なし
各リンク座標基準に配置したオブジェクトを選択して「選択オブジェクトをエクスポート」コマンドから DAE (Collada) か STL 形式でエクスポートします．
Rhinoceros から DAE (Collada) をエクスポートする場合はエクスポートオプションにて
「ジオメトリのみを保存」のみにチェック
を入れてファイルを書き出します．
この「ジオメトリのみを保存」でも色や単位情報も保存されます．
今回は表示（visual）用に色付きの DAE ファイルとしてエクスポートし，干渉チェック（collision）用にデータ量を少なくするため粗目の設定で STL ファイルをエクスポートしました．
- 表示 visual 用 DAE ファイル
- main-body.dae
- door.dae
- tray.dae
- 干渉チェック collision 用 STL ファイル
- main-body.stl
- door.stl
- tray.stl
リンク機構を含む URDF モデルファイルの作成
DAE や STL のメッシュデータのエクスポートが終わったらリンク機構を含む URDF モデルファイルを作成します．
ファイル配置
3dmodeling-examples/models/urdf/ ├── meshes │ └── washing-machine │ ├── base_link.dae │ ├── base_link.stl │ ├── door.dae │ ├── door.stl │ ├── main-body.dae │ ├── main-body.stl │ ├── tray.dae │ └── tray.stl ├── washing-machine_links.urdf └── washing-machine.urdf
前回作成してメッシュファイルを配置したフォルダ
3dmodeling-examples/models/urdf/meshes
内にエクスポートしたメッシュファイルを配置します．
そしてフォルダ 3dmodeling-examples/models/urdf/ にファイル washing-machine_links.urdf をテキストファイルとして作成してそこにリンク機構を含む URDF モデルを作り込みます．前回作成したファイル washing-machine.urdf を複製してファイルの名前を変更しても良いです．
リンクの相対姿勢・可動域の確認
URDF データで 「洗濯機本体（main-body）」 と 「洗濯槽の扉（door）」 および 「洗剤投入トレイ（tray）」 それぞれの相対的な姿勢の関係とそれぞれの可動域を 「関節（joint）」 として定義しますので， CAD ( Rhinoceros など ) 上で相対姿勢および可動域の測定を行います．
その際，各リンクオブジェクトについて DAE や STL ファイルへのエクスポート用に原点へ移動した配置ではなく元々の洗濯機全体内での配置で調べるということに注意してください．
まずは 「洗濯機本体（main-body）」 と並進的な相対位置関係にある 「洗剤投入トレイ（tray）」 の座標の確認と設定可能な可動域を調べます．
- 洗濯機本体（main-body） → 洗剤投入トレイ（tray）
- 相対位置: 洗濯機本体原点から ( 0.0 , -190.00 , 956.00 ) [mm] の位置
- 相対角度: ゼロ
- 可動域: 前方へ 200 [mm] とした
次に 「洗濯機本体（main-body）」 と 「洗濯槽の扉（door）」 の相対座標・角度やの確認と設定可能な可動域を調べます．
扉は傾いて洗濯機本体に取り付けられているのでその角度とヒンジ回りの可動域も調べます．Rhinoceros では角度表示がラジアンでもできるのでそれを利用します．
- 洗濯機本体（main-body） → 洗濯槽の扉（door）
- 相対位置: 洗濯機本体原点から ( 306.58 , -258.00 , 675.82 ) [mm] の位置
- 相対角度: 洗濯槽の扉（door）原点から Y軸と平行な軸 回りに 0.1396 [rad] (= 8.0 [deg] )
- 可動域: ヒンジを軸に閉じた状態から 1.8326 [rad] (= 105 [deg] ) 開くとした
URDF データの作成
リンクモデルに必要なメッシュファイルと情報が揃いましたので URDF ファイルに書き込んだものが次のようになります．
<?xml version="1.0" ?>
<robot name="washing-machine">
<link name="base_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://3dmodeling-examples/models/urdf/meshes/washing-machine/main-body.stl" scale="0.001 0.001 0.001" />
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://3dmodeling-examples/models/urdf/meshes/washing-machine/main-body.dae" />
</geometry>
</visual>
</link>
<link name="door">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://3dmodeling-examples/models/urdf/meshes/washing-machine/door.stl" scale="0.001 0.001 0.001" />
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://3dmodeling-examples/models/urdf/meshes/washing-machine/door.dae" />
</geometry>
</visual>
</link>
<link name="tray">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://3dmodeling-examples/models/urdf/meshes/washing-machine/tray.stl" scale="0.001 0.001 0.001" />
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://3dmodeling-examples/models/urdf/meshes/washing-machine/tray.dae" />
</geometry>
</visual>
</link>
<joint name="joint1" type="revolute">
<parent link="base_link"/>
<child link="door"/>
<origin xyz="0.30658 -0.258 0.67582" rpy="0 -0.1396 0"/>
<axis xyz="0 0 1" />
<limit effort="30" velocity="1.0" lower="-1.8326" upper="0.0" />
</joint>
<joint name="joint2" type="prismatic">
<parent link="base_link"/>
<child link="tray"/>
<origin xyz="0.0 -0.190 0.956" rpy="0 0 0"/>
<axis xyz="1 0 0" />
<limit effort="30" velocity="1.0" lower="0.0" upper="0.200" />
</joint>
</robot>
washing-machine_links.urdf 内のそれぞれの要素について説明します．
- 5〜48行
<link>要素: リンクの定義 3つ （ base_link, door, tray ） <collision>要素にメッシュに STL ファイルを使用し単位変換 [mm] → [m]<visual>要素に DAE メッシュファイルを使用<origin>はリンク内のメッシュの配置なので今回は全てゼロ
- 50〜56行
<joint>要素: joint1 typeで関節形式revolve（=回転）を設定<parent>要素で関節を介する親リンクbase_linkを指定<child>要素で関節を介する子リンクdoorを指定<origin>要素で親子リンク間の相対座標- 位置の単位はメートル [m]
- 姿勢角の正負に注意（座標軸に対して右ねじの法則）
<axis>要素でrevolve関節の回転軸のリンク座標系での方向を設定<limit>要素effort: 最大トルク – 今回はとりあえずの値velocity: 最大角速度 – 今回はとりあえずの値lower: 可動域下限 – 回転関節なので下限角度で単位はラジアン [rad]upper: 可動域上限 – 回転関節なので上限角度で単位はラジアン [rad]
- 58〜64行
<joint>要素: joint2 typeで関節形式prismatic（=並進）を設定<parent>要素で関節を介する親リンクbase_linkを指定<child>要素で関節を介する子リンクtrayを指定<origin>要素で親子リンク間の相対座標<axis>要素でprismatic関節のリンク座標系での移動方向を設定<limit>要素effort: 最大力 – 今回はとりあえずの値velocity: 最大速度 – 今回はとりあえずの値lower: 可動域下限 – 並進関節なので下限位置で単位はメートル [m]upper: 可動域上限 – 並進関節なので上限位置で単位はメートル [m]
下記リンク先の ROS Wiki に URDF ファイルの作成方法のチュートリアルがありますので参考にしてください．
URDF モデルの確認
urdf_tutorial の display.launch で URDF モデル washing-machine_links.urdf の確認をします．（下記コマンド横スクロールで末尾まで表示）
$ roslaunch urdf_tutorial display.launch model:='$(find 3dmodeling-examples)/models/urdf/washing-machine_links.urdf'
URDF で <joint> 要素を定義して joint_state_publisher ウィンドウ内のスライドバーも有効になっているので関節を動かしてみます．
動画では表示メッシュの動きとともに TF も一緒に動いている様子が見られると思います．
今回の記事はここまでです．