タグアーカイブ urdf

著者:yamamoto.yosuke

Gazebo/MoveIt のための 3D モデリング(14)MoveIt の動モデルの作成

本シリーズ前回の記事 Gazebo/MoveIt のための 3D モデリング(13)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 ファイルに書き込んだものが次のようになります.

washing-machine_links.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_tutorialdisplay.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 も一緒に動いている様子が見られると思います.

今回の記事はここまでです.

著者:yamamoto.yosuke

Gazebo/MoveIt のための 3D モデリング(13)MoveIt の静モデルの作成

本シリーズ前回の記事 Gazebo/MoveIt のための 3D モデリング(12)Gazebo の静モデルの作成 ではエクスポートしたメッシュデータファイルを Gazebo の静モデルとしてモデルファイルに組み込んで表示する方法を紹介しました.

今回はエクスポートしたメッシュデータファイルを MoveIt の静モデルとしてモデルファイルに組み込んで表示する方法を紹介します.

今回紹介するのは次の2通りの方法です.メッシュを MoveIt GUI で読み込んで障害物とする方法とロボットモデル作成につながる方法の URDF モデルの作成とそれを確認表示する方法です.

  1. MoveIt 空間内の動作計画に対する障害物として STL ファイルまたは DAE ファイルを読み込んで設置
  2. URDF モデルの作成(→ ロボットリンクモデル作成へつながる)

MoveIt 内の障害物モデルとしてのメッシュ読込み

MoveIt には動作計画における障害物として STL ファイルや DAE ファイルをそのまま読み込んで MoveIt の動作計画空間内に配置する機能があります.

MoveIt の Motion Planning パネル内の Scene Objects タブを開いて, “Mesh from file” を選択します.

“Mesh from file” セレクタの右隣にあるプラスボタン [ + ] を押します.

(左図拡大は画像をクリック)

保存してある STL もしくは DAE ファイルを選択します.

ファイルを読み込むときに MoveIt の GUI インタフェースである RViz のメッセージウィンドウが開いて,ミリメートル単位で記述されているモデルをメートル単位に変換する旨の問いがなされるので [ Yes ] をクリックします.

読み込んだモデルをインタラクティブマーカや座標などを指定して意図した位置に設置し,左のチェックボックスをクリックすると設置リンクの選択を促されますので適宜選択して [ OK ] ボタンを押します.

次に [ Publish ] ボタンを押すと読み込んで設置したモデルが MoveIt 空間内で障害物として認識されます.

あとは [ Plan ] や [ Plan & Execute ] などで動作計画を実行するとその経路上に障害物があるとそれを避けたマニピュレーションの軌道が生成されます.

MoveIt モデル URDF ファイルの作成

MoveIt モデルの URDF ファイルの作成は下記リンク先の ROS Wiki に書かれています.
本記事ではそれらから MoveIt 静モデル作成に絞って説明します.

MoveIt モデルの作成にあたっては ROS パッケージを作成してその中にモデルの URDF ファイルを置くのが本記事の内容に続く応用も含めると一番簡便なのではないかと思います.

今回 ROS パッケージをつくるのが面倒なようでしたら下記リンク先リポジトリをクローンして利用してください.

3dmodeling-examples/
├── CMakeLists.txt
├── images
│   ├── front_view_win.png
│   ├── left_view_win.png
│   ├── top_view_win.png
│   ├── washing-machine_catalogue.pdf
│   └── washing-machine_catalogue.png
├── launch
│   ├── spawn-washingmachine.launch
│   └── world-washingmachine.launch
├── LICENSE
├── models
│   ├── gazebo_models
│   │   ├── washing-machine
│   │   │   ├── meshes
│   │   │   │   ├── base_link_blue-gray.stl
│   │   │   │   ├── base_link_dark-gray.stl
│   │   │   │   ├── base_link_gray-white.stl
│   │   │   │   ├── base_link_light-gray.stl
│   │   │   │   └── base_link.stl
│   │   │   ├── model.config
│   │   │   └── model.sdf
│   │   └── washing-machine-dae
│   │       ├── meshes
│   │       │   ├── base_link.dae
│   │       │   └── base_link.stl
│   │       ├── model.config
│   │       └── model.sdf
│   ├── urdf
│   │   ├── meshes
│   │   │   └── washing-machine
│   │   │       ├── base_link.dae
│   │   │       └── base_link.stl
│   │   └── washing-machine.urdf
│   └── washing-machine.3dm
├── package.xml
├── README.md
└── worlds
    └── washing-machine.world

本記事執筆時のサンプルモデルパッケージは右に示すような構成になっていますので参考にしてみてください.

この中の MoveIt URDF モデルに関連するフォルダ・ファイルがハイライトされた部分です.

meshes フォルダに base_link.dae と base_link.stl の2つのファイルが含まれていますが,これはサンプルのためですのでどちらか1つのファイルだけでも URDF モデルは作成できます.

今回の MoveIt 静モデルのサンプル URDF ファイル washing-machine.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/base_link.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/base_link.dae" />
      </geometry>
    </visual>
  </link>
  
</robot>

URDF のデータ形式は XML で,モデル構成要素の linkcollisiongeometrymesh が記述されています.今回は静モデルですので link 要素は1つですがロボットのように関節が多いとリンク数にともなって link 要素およびそこに含まれる子要素も増えます.

ROS パッケージ化していることで ROS のファイルシステムが名前空間から package://3dmodeling-examples/... によって 3dmodeling-examples パッケージのディレクトリを自動解決できるようになっています.

collision メッシュの STL データは単位情報を持っていないので scale="0.001 0.001 0.001" で 0.001倍(=1/1000) 変換をして [mm] のデータを [m] に換算しています.

  • 注) 表示上コードの右の方に隠れていると思うので横スクロールで確認してください.

visual のメッシュは DAE ファイルを利用していて,DAE モデルは単位情報を持っていて読み込む側でスケール判断をするので scale は必要ありません.

URDF モデルファイルの RViz 表示

URDF モデルの確認には urdf_tutorial パッケージの display.launch を利用します.

urdf_tutorial は joint-state-publisher-gui パッケージをインストールすることで利用できるようになります.下記は ROS Melodic の場合のインストールコマンドですので他の ROS バージョンの場合は melodic の部分を noetic などに書き換えて実行してください.

$ sudo apt update
$ sudo apt install ros-melodic-joint-state-publisher-gui

ターミナルで ROS 環境の設定と urdf_tutorial の display.launch の起動を行います.

$ source ~/$PathToYourWorkspace/devel/setup.bash
$ roslaunch urdf_tutorial display.launch model:='$(find 3dmodeling-examples)/models/urdf/washing-machine.urdf'
  • 注)
    • $PathToYourWorkspace は各自の ROS ワークスペースへのパスを記述
    • Launch オプションの model:= には URDF ファイルパスを指定
      • 上記は 3dmodeling-examples パッケージ内のフォルダに washing-machine.urdf がある場合の例

正常に実行できると次の図のように洗濯機モデルが RViz 空間上に表示されます.

今回の単一リンクの正モデルの場合はリンク(フレーム)間の座標変換(tf)が無いので RViz 内の “Global Status: Warn / Fixed Frame No tf data. Actual error: Fixed Frame does not exist” と警告が出ますが問題はありません.
問題はありませんが気持ちの収まりが悪いようでしたらターミナルを追加で立ち上げて下記コマンドで例えば /world フレームから洗濯機モデルの /base_link フレームへの tf をパブリッシュすると警告が消えます.

$ rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 /world /base_link

今回の記事はここまでです.


本シリーズ次回の記事は洗濯機の URDF モデルにドアのヒンジなどの動く箇所を設定してより機械らしい(ロボットに近い)モデルにする様子を紹介する予定です.