Gazebo/MoveIt のための 3D モデリング（１３）MoveIt の静モデルの作成
本シリーズ前回の記事 Gazebo/MoveIt のための 3D モデリング（１２）Gazebo の静モデルの作成 ではエクスポートしたメッシュデータファイルを Gazebo の静モデルとしてモデルファイルに組み込んで表示する方法を紹介しました．
今回はエクスポートしたメッシュデータファイルを MoveIt の静モデルとしてモデルファイルに組み込んで表示する方法を紹介します．
今回紹介するのは次の2通りの方法です．メッシュを MoveIt GUI で読み込んで障害物とする方法とロボットモデル作成につながる方法の URDF モデルの作成とそれを確認表示する方法です．
- MoveIt 空間内の動作計画に対する障害物として STL ファイルまたは DAE ファイルを読み込んで設置
- 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 の２つのファイルが含まれていますが，これはサンプルのためですのでどちらか１つのファイルだけでも 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 で，モデル構成要素の link や collision， geometry， mesh が記述されています．今回は静モデルですので link 要素は１つですがロボットのように関節が多いとリンク数にともなって 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 空間上に表示されます．
問題はありませんが気持ちの収まりが悪いようでしたらターミナルを追加で立ち上げて下記コマンドで例えば /world フレームから洗濯機モデルの /base_link フレームへの tf をパブリッシュすると警告が消えます．
$ rosrun tf2_ros static_transform_publisher 0 0 0 0 0 0 /world /base_link
今回の記事はここまでです．
本シリーズ次回の記事は洗濯機の URDF モデルにドアのヒンジなどの動く箇所を設定してより機械らしい（ロボットに近い）モデルにする様子を紹介する予定です．
Gazebo/MoveIt のための 3D モデリング（６）滑らかなサーフェス – 作成編（その１）
前回 「Gazebo/MoveIt のための 3D モデリング（５）滑らかなサーフェス – 知識編」 では 3D モデリングにおける滑らかなサーフェスとは何かについて曲率などの連続性や CAD やサーフェスモデラ内での形状表現のされ方について説明しました．
今回は前回の記事の知識を踏まえて滑らかなサーフェスの作成例について説明します．
滑らかなサーフェスは Gazebo や MoveIt のモデルを作成する場合においてはあまり重要性は高くない… と前回の記事を書いた時点では考えていましたが，今後，Gazebo や MoveIt を走らせる PC の CPU / GPU 性能が向上して細密メッシュモデルを使った精密なシミュレーションへの要求も段々と高くなるかもしれない，とも思いました．
洗濯機前面上部の操作ボタン
サーフェスはモデル内で大きいものから順に作成していった方が全体のバランスを確認しながら作業を進められて修正作業が必要になる量も少なくなるので良いと思います．
ただ，本記事ではまず，Rhinoceros に備わっているコマンド1つほどであまり制御点とか次数とか細かく意識しなくても作成できる曲率連続の滑らかなサーフェスの例として小さいサーフェスの洗濯機前面上部の操作ボタンのモデリングについて説明します．
四角いボタンのモデリング
洗濯機の前面視（ Rhinoceros の Right ビュー ）で四角いボタンの大きさを見ると大体 20mm 角でした．
ボタンを1つモデリングして，それを洗濯機前面のサーフェス上に8つコピーして配置します．
左の図は四角いボタンのソリッドモデルの作成過程をアニメーション化した画像です．
各手順を以下に順を追って説明します．
20mm の正方形の線を選択して「テーパで平面曲線を押し出し（ExtrudeCrvTapered）」 を実行し，ドラフト角度 15° の設定で 3mm 押し出します．
- テーパで平面曲線を押し出し
- メニュー: ソリッド(O) > 平面曲線を押し出し(X) > テーパ(T)
- コマンド: ExtrudeCrvTapered
四隅を「曲率連続」で丸めるために「エッジをブレンド（BlendEdge）」を実行して四隅のエッジを選択します．
デフォルトでは半径が 1mm となっていてモデリングしたい四隅の半径はもう少し大きいので洗濯機前面図と比較しながら寸法を調整します．
- エッジをブレンド
- メニュー: ソリッド(O) > エッジをフィレット(F) > エッジをブレンド(B)
- コマンド: BlendEdge
「エッジをブレンド」コマンド内の設定を次のようにして
- ハンドルを連動(L)=はい
- レールタイプ(R)=レール間の距離(I)
次にコマンド内設定で「全てを設定(T)」で半径（上記設定の場合はレール間の距離）を 8mm にしてコマンドを確定します．
「エッジをブレンド」コマンド内の「レールタイプ」の設定は「レール間の距離」にした場合が比較的綺麗なフィレットがかかるように思っているので設定しました．フィレットをかけるサーフェスの組み合わせによって他の設定の方が良い場合もあるので各モデリング対象にて適宜選択してください．
今度は指で押す面の周囲にブレンドエッジを作成します．「エッジをブレンド（BlendEdge）」コマンドを実行してコマンド内設定 「次の半径(R)」 を 2mm に設定してから該当するエッジをすべて選択して確定・実行します．
下の図は「エッジのブレンド」でフィレットを作成した四角いボタンの平均曲率解析とゼブラ解析の表示結果です．
これらのブレンドエッジは数ミリと小さいので問題にはあまりなりませんが，大きなサーフェスとしてブレンドエッジを作成する場合は応用的に次のリストの項目を調整編集すると良いでしょう．
- ブレンドエッジ関連で応用的な項目
- BlendEdge だとフィレット接続する方向は 5次-6点 で「曲率連続」になるが，長手方向は 3次-多点 になってしまいガタつく
- →「次数を変更（ChangeDegree）」で長手方向の次数を 7次 に変更してサーフェスマッチングを行い再接続する
- → 制御点が多すぎる場合は RebuildUV コマンドで U方向 のみ制御点を少なくしてリビルドして（この時点ではまだ 3次-多点 ），その後で「次数を変更（ChangeDegree）」で U方向を 7次 に変更してサーフェスマッチング（MatchSurf）
- テーパ押し出しの基となった正方形も各辺直線ではなく少し外に膨らむ緩い円弧にするとより滑らかな感じになる
四角いボタンのモデルができましたので洗濯機前面のサーフェス上にコピーして配置します．
ボタンの押される方向の軸と洗濯機前面のサーフェスの法線軸を合わせ，かつ上下辺を水平に配置したいので「配置（3点指定）・（Orient3Pt）」を利用してコピーします．「 配置（3点指定）」は下記リストの 3点 を移動元と移動先で指定して配置変換するものです．
- 移動原点
- 移動原点から第1の方向を決める方向上の点
- 移動原点と第1方向点で構成される軸回りの方向を決める点
四角いボタンの移動先とするために配置するサーフェス上の法線方向と接線方向の直線を各配置点で描画します．まず，YZ 平面上に四角いボタンを配置する中心線を描画して，洗濯機前面視（Rinoceros の Right ビュー）にてそれらをサーフェスに「投影（Project）」します．
- 投影
- メニュー: 曲線(C) > オブジェクトから曲線を作成(F) > 投影(P)
- コマンド: Project
「直線（Line）」を実行してコマンド内で「法線(N)」を指定，もしくはメニューから「サーフェス法線(U)」を実行して，サーフェスを選択後，サーフェス上にある投影した中心曲線の交点を選択して法線を描画します．
今回の「配置（3点指定）・（Orient3Pt）」ではスケーリングコピーはしないので長さは適当で大丈夫です．順次各点における法線を描画して合計8軸を準備します．
- サーフェス法線
- メニュー: 曲線(C) > 直線(L) > サーフェス法線(U)
- コマンド: Line → 法線(N) を選択
洗濯機前面の球面サーフェスに食い込む形で四角いボタンを配置したいのでコピー元となる点を 1mm ボタンの高さ方向へ移動しておいてから「配置（3点指定）・（Orient3Pt）」でコピー元，コピー先の各3点を指定してボタンをサーフェス上に配置します．
- 配置（3点指定）
- メニュー: 変形(T) > 配置(O) > 3点指定(3)
- コマンド: Orient3Pt
8つの四角いボタンを洗濯機前面のサーフェス上に配置してゴースト表示とレンダリング表示をしてそれぞれキャプチャしたものが次の2つの画像です．
丸いボタンのモデリング
丸いボタンの直径は大体 50mm ぐらいのようです．
丸いボタンのように軸回転形状のものは回転形状のプロファイル曲線を作成してそれを軸回りに回転してソリッドモデル（閉じたポリサーフェス）とするのが一番簡単だと思います．
丸いボタンモデルの作成自体は上記四角いボタンのように作成しやすい場所で作成して，軸回りの形状は軸対称で同じなのでそれを今度は「配置（2点指定）・（Orient）」でコピー移動して洗濯機上のサーフェスに配置します．
丸いボタンのモデリングと配置の作業手順をまとめると次のようになります．
- 回転中心軸を含む平面上に回転形状プロファイル曲線を描画
- 回転中心軸回りに「回転（Revolve）」にて1周分のサーフェスを作成
- 作成されたサーフェス群を「結合（Join）」してソリッド（閉じたポリサーフェス）にする
- 丸ボタンモデルのコピー元原点とコピー先の原点と方向（サーフェス法線方向）を描画
- 「配置（2点指定）・（Orient）」でコピー移動
回転方向は曲率一定になるので回転プロファイルの曲線さえ滑らかな曲線を作成すれば回転で作成するポリサーフェスも滑らかになります．そこで丸ボタンの回転プロファイルを次の図のように作成するのですが，ボタンの側面と指で押される面に相当する曲線間に滑らかなフィレット曲線を作成します．
ボタンの側面と指で押される面に相当する両曲線に接する円を描画して円の接する点でトリムするとその間の曲線の接続も比較的バランス良く繋がります．円との接点でトリムされた両曲線間に「接続（BlendCrv）」で曲線を作成します．
- 接続
- メニュー: 曲線(C) > 接続(U)
- コマンド: BlendCrv
「接続（BlendCrv）」内の設定で各接続点における接続条件を設定します．連続性を「曲率」もしくは「G3（曲率変化率）」を設定すると各設定に応じた滑らかな曲線が描画されます．各制御点は [ Shift ] キーを押しながらマウスでドラッグすると対象な点も同時に移動してくれるので便利です．曲線形状や曲率変化を見ながら制御点を調整して意図した形状で確定をします．
回転形状のプロファイル曲線が作成できたら「回転（Revolve）」 で曲線を回転したサーフェスを作成します．
- 回転
- メニュー: サーフェス(S) > 回転(V)
- コマンド: Revolve
1回転分のサーフェスを作成するにはコマンド内で「360度(F)」を指定します．
右の図（ワイヤーフレーム表示）のようなボタン形状のサーフェスが作成されます．
1回転分作成したサーフェスをすべて選択して「結合（Join・Ctrl-J）」してソリッドモデル（閉じたポリサーフェス）にします．
＜参考＞
Rhinoceros 上では回転形状プロファイル曲線群を先に「結合（Join）」してから「回転（Revolve）」しても同じ形状になるのですが，サーフェスのフィレットのような回り込みの大きい形状と一体化したサーフェスをメッシュ化するときにフィレット形状付近を飛ばして粗いメッシュが作成されてしまうことがあります．フィレット形状部分のサーフェスは一体化したものとせずに別体のものを作成した後「結合（Join）」した方がフィレットサーフェスのエッジがメッシュ境界に反映されるので適切な形状のメッシュ作成のためには良いでしょう．
丸いボタンのソリッドモデルが作成できたら「配置（2点指定）・（Orient）」で洗濯機前面サーフェス上の法線方向に合わせてコピー移動します．先述したように軸回りの形状は軸対称で同じなので位置と方向1つのみの指定の移動変換をします．
四角いボタンと同様に前面サーフェスに少し食い込ませたいので丸いボタンの背面から 1mm 入ったところをコピー原点としてからボタンを押す方向軸上の1点を選択して，コピー先の洗濯機前面サーフェス上の点と法線方向を指定してスケーリングしない設定でコピー移動します．
- 配置（2点指定）
- メニュー: 変形(T) > 配置(O) > 2点指定(2)
- コマンド: Orient
丸いボタンを洗濯機前面サーフェス上に配置したものを四角いボタンと併せて Perspective ビューにゴースト表示させたものが次の図です．
レンダリング表示にしてキャプチャした画像が次の図です．
洗濯機本体とボタン類のソリッドモデルをブーリアン演算して一体化するのは全ての作業の最後で良いので，他のモデリング作業の邪魔にならないようにとりあえず新しいレイヤー buttons（例）を追加してそこに移動しておきます．
今回の記事はここまでです．
本シリーズ次回の記事は引き続き滑らかなサーフェスの作例紹介で
「Gazebo/MoveIt のための 3D モデリング（７）滑らかなサーフェス – 作成編（その２）」
を予定しています．
Gazebo/MoveIt のための 3D モデリング（４）基本形状編 – その２
前回 「Gazebo/MoveIt のための 3D モデリング（３）基本形状編 – その１」 で Box 形状や球面で洗濯機の大きな面のモデリングを行いました．
今回はその続きで，洗濯機の背面や底部のモデリングを行います．
今回のゴールは左の図の状態です．
ボディ背部のモデリング
ボディ背面の突出形状部の最後部から 40mm の幅がありますので，洗濯機の主要形状部をその分トリムします．（前回 Box 形状の Scale1D を前後方向に行わずにガムボール移動などで World 座標系で X の正方向に 40mm 移動した場合はこの手順は不要）
洗濯機の側面視で最背部から垂直な直線を描画して，前方向に 40mm 移動させ，この直線を使ってボディをトリム（Trim）します．
Perspective ビューでトリムされたボディのエッジ分析（ShowEdges）をすると次の左の図のようになります．
このような開口部エッジが同一平面内にあって閉曲線になっている場合は平面で塞ぐ 「キャップ（Cap）」 を実行できます．
- キャップ
- メニュー: ソリッド(O) > キャップ(H)
- コマンド: Cap
「キャップ」の実行結果
同一平面内にある閉エッジ
次は背部に突出している形状を作成します．
背部の台形状の輪郭を描画します．上面視（Top ビュー）で座標 (-320,0) から水平の直線（Line）を後部に向かって描画して，その直線からオフセット（Offset）で オプション Both(B) で両サイドへのオフセット線をオフセット量 260[mm] = 520mm/2 で描画します．メインボディ最後部のエッジと 260mm オフセットした両直線の交点を中心に回転（Rotate）で 45°，-45° を指定して回転させて上面図画像の輪郭に合うことを確認します．
- 回転
- メニュー: 変形(T) > 回転(R)
- コマンド: Rotate
また直線（Line）を座標 (-360,0) から「両方向(B)」を指定して洗濯機の幅方向に描画します．
描画した３つの直線の台形での不要部分を互いにトリムします．
Perspective ビューに移って台形の開いている部分を直線で接続して結合（Join）して曲線を閉じます．閉じた曲線を洗濯機の高さ方向（Z方向）に +90mm ガムボールで移動させます．
背面部の基礎となる 3D 形状をソリッドの「垂直に押し出し」で作成します．とりあえず上面高さまで押し出しします．
- 垂直に押し出し
- メニュー: ソリッド(O) → 平面曲線を押し出し(X) → 直線(S)
- コマンド: ExtrudeCrv
洗濯機の側面視（Back ビュー）にて洗濯機背部形状の上端に相当する斜めの直線を描画して先程「押し出し」したソリッドを トリム（Trim） して キャップ（Cap） で閉じます．
洗濯機背面部上面でトリムしたモデル（キャップの前の段階）
三面図から読み取れる形状としてはここまでなのですが，洗濯機背部は角部や隅部に 「R形状」 や 「フィレット」 と言われる形状がつけられていることが多いです．
今回は三面図から 「R形状」 の寸法は読み取れないので，それを想像して寸法を決めて 「ロフトサーフェス（Loft）」 で作成します．
ロフトサーフェスは曲線と曲線の間にサーフェスを作ります．ロフトサーフェスの基となる曲線を 「フィレット(Fillet)」 で描画します．
- フィレット
- メニュー: 曲線(C) → フィレット(F)
- コマンド: Fillet
フィレットの半径は最後部の平面上の方に 40mm メインボディとつながる平面上の方に 80mm のフィレットをかけるとバランスの良さそうなロフトサーフェスになるかと思います．
- ロフト
- メニュー: サーフェス(S) > ロフト(L)
- コマンド: Loft
洗濯機背部上方のコーナー部にロフトでフィレットサーフェスが作成できたら ミラー（Mirror） でX軸対称に反転コピーします．そしてミラーリングして左右２つになったフィレットで洗濯機背部のソリッドモデルを トリム（Trim） します．
２つのフィレットとそれらでトリムされた背部ポリサーフェスを 結合（Join） すると１つのソリッド（閉じたポリサーフェス）になります．
洗濯機のメインのボディと背部の２つのソリッドモデルは互いに接し合っているので２つのソリッドの 「和の演算（BooleanUnion）」 を行って１つのソリッドモデルにします．
- 和の演算
- メニュー: ソリッド(O) > 和(N)
- コマンド: BooleanUnion
1つのソリッドになるので，くどいようですが ShowEdges で 「閉じたポリサーフェス（=ソリッド）」 であることを都度確認すると良いでしょう．
ボディ底部のモデリング
Gazebo や MoveIt のモデルとしては洗濯機底部はロボットとインタラクションすることはあまりないと思いますので大体の雰囲気をモデリングできれば十分です．
洗濯機の足部は三面図だけではなくカタログ画像からも少し形状が分かるので三面図と併せて参考にしてモデリングします．
カタログ画像や3面図から，足部はテーパのかかった円錐台形状であろうと思われます．
側面視や前面視からそれぞれの足の中心座標を推定し，下面と上面の直径はそれぞれ 50[mm] と 54[mm] ぐらいと当たりをつけて円を描画してロフトとキャップを組み合わせてソリッドモデルを作成します．
洗濯機底部の足以外のサーフェスのモデリングの大まかな様子は次の GIF アニメーションのような感じです．モデリングの履歴（ヒストリー）を使わないモデリングなので作成手順はやり易い順番で大丈夫です．またサーフェスの作成方法も１通りしかないのではなく，例えば 「ロフト （Loft）」 でフィレット形状を作成する代わりに 「サーフェス > フィレット（FilletSrf）」 や 「エッジをフィレット（FilletEdge）」 ，「回転（Revolve）」 を使ったりすることもできます．
これまで取り上げていない機能で利用したのは 「曲線を押し出し（ExtrudeCrv）」 と 「円柱（Cylinder）」 の機能です．
- 曲線を押し出し
- メニュー: サーフェス(S) > 曲線を押し出し(X) > 直線(S)
- コマンド: ExtrudeCrv
- 円柱
- メニュー: ソリッド(O) > 円柱(Y)
- コマンド: Cylinder
今回は洗濯機ボディの背部や底部のモデリング方法を紹介して，次のモデルとなり本記事のゴールに到着しました．
基本形状編はひとまず今回の記事までです．
本シリーズ次回の記事は 「Gazebo/MoveIt のための 3D モデリング（５） 滑らかなサーフェス – 知識編」 を予定しています．