ROS1を使ってみる その4(URDFファイルの作成、Open Manipulator)
はじめに
今回はOpen ManipulatorのURDFファイルを作成してみました。
これまではROSですでに用意されていたOpen Manipulatorのモデルを利用していましたが、URDFファイルを作成できるようになれば、自分で設計したロボットをROSのシミュレーションに取り込むことができそうです。
独特の書き方があるようですが、まずはサンプルをいろいろ試してみました。エラーが出ることもしばしばあったので、ログを残しています。
▼以前の記事はこちら
関連情報
▼ROSのWikiのurdfに関するページはこちら。ここに書かれているコマンドは実行できませんでした。URDFファイルは試してみました。
https://wiki.ros.org/ja/urdf/Tutorials/Create%20your%20own%20urdf%20file
▼ROSについて検索しているとよく見かける、project_srsさんのQiitaのページ。パッケージの作成など、ROSでの操作はこちらが参考になりました。
https://qiita.com/srs/items/35bbaadd6c4be1e39bb9
https://qiita.com/srs/items/77f378230bf856a3625c
▼こちらの書籍にも、Wikiと同じ図が書かれていたりしました。
▼MATLAB & Simulinkのページもありました。ROSでの利用については書かれていませんが、URDFの概念は分かりやすかったです。
https://jp.mathworks.com/help/sm/ug/urdf-model-import.html
▼linkの要素はこちらにまとめられていました。
https://wiki.ros.org/urdf/XML/link
URDFファイルの作成
パッケージを作成してRVizで表示する
▼今回もWSL2のUbuntu 20.04で実行しています。
book_wsというワークスペースで作業しているので、ROSの環境を有効にしてurdfフォルダを作成し、移動しておきました。
cd ~/book_ws
source devel/setup.bash
cd src
catkin_create_pkg urdf_test std_msgs rospy roscpp tf
cd urdf_test
mkdir urdf
cd urdf
urdf_tutorialパッケージをインストールしました。私はROS Noeticの環境で実行しているので、Noeticに変更してインストールしました。
sudo apt-get install -y ros-noetic-urdf-tutorial
QiitaのページにあったURDFファイルを作成し、RVizで表示してみました。
sudo nano my_urdf.xml
を実行して、以下の内容を記述しました。
<robot name="test_robot">
<link name="base_link"/>
<joint name="body_joint" type="fixed">
<parent link="base_link"/>
<child link="body_link"/>
</joint>
<link name="body_link">
<visual>
<geometry>
<box size="0.3 0.3 0.2"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
</visual>
</link>
</robot>
以下のコマンドで構文チェックを行いました。
check_urdf my_urdf.xml
▼エラーは出ませんでした。
以下のコマンドを実行すると、RVizで表示されました。
roslaunch urdf_tutorial display.launch model:=my_urdf.xml
▼中央に立方体が表示されました。
linkとjointを追加する
WikiではURDFモデルについて順を追って解説されていました。
▼最初のモデルはこちら。Unreal Engineではロボットアームのジョイント関係を親子付けしていましたが、それに似てparentとchildが定義されています。
<robot name="test_robot">
<link name="link1" />
<link name="link2" />
<link name="link3" />
<link name="link4" />
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
</joint>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link3"/>
</joint>
<joint name="joint3" type="continuous">
<parent link="link3"/>
<child link="link4"/>
</joint>
</robot>
この時点ではまだRVizで表示することができませんでした。
▼さらに寸法や回転軸を追加したものがこちら。
<robot name="test_robot">
<link name="link1" />
<link name="link2" />
<link name="link3" />
<link name="link4" />
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="5 3 0" rpy="0 0 0" />
<axis xyz="-0.9 0.15 0" />
</joint>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link3"/>
<origin xyz="-2 5 0" rpy="0 0 1.57" />
<axis xyz="-0.707 0.707 0" />
</joint>
<joint name="joint3" type="continuous">
<parent link="link3"/>
<child link="link4"/>
<origin xyz="5 0 0" rpy="0 0 -1.57" />
<axis xyz="0.707 -0.707 0" />
</joint>
</robot>
▼chech_urdfではエラーは出ませんでした。
check_urdf my_urdf.xml
この状態でRVizで表示してみました。
roslaunch urdf_tutorial display.launch model:=my_urdf.xml
▼何も表示されていません。
▼左側にエラーが表示されていました。base_linkが無いようです。
先程の立方体を表示したときはbase_linkが含まれていたので、link1に親子付けすればいいんだろうなと思い、追加してみました。
▼base_linkを追加しました。
<robot name="test_robot">
<link name="base_link"/>
<link name="link1" />
<link name="link2" />
<link name="link3" />
<link name="link4" />
<joint name="base_link" type="continuous">
<parent link="base_link"/>
<child link="link1"/>
</joint>
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="5 3 0" rpy="0 0 0" />
<axis xyz="-0.9 0.15 0" />
</joint>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link3"/>
<origin xyz="-2 5 0" rpy="0 0 1.57" />
<axis xyz="-0.707 0.707 0" />
</joint>
<joint name="joint3" type="continuous">
<parent link="link3"/>
<child link="link4"/>
<origin xyz="5 0 0" rpy="0 0 -1.57" />
<axis xyz="0.707 -0.707 0" />
</joint>
</robot>
先程のエラーは表示されず、RVizで表示されました。
▼base_linkを動かすと、繋がっているのであろう線も動きました。
▼画像だと分かりにくいのですが、星座みたいになっています。
すべてのリンクに、赤い正方形を割り当ててみました。
▼geometryやmaterialを追加しています。
<robot name="test_robot">
<link name="base_link">
<visual>
<geometry>
<box size="0.3 0.3 0.2"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
</visual>
</link>
<link name="link1">
<visual>
<geometry>
<box size="0.3 0.3 0.2"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
</visual>
</link>
<link name="link2">
<visual>
<geometry>
<box size="0.3 0.3 0.2"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
</visual>
</link>
<link name="link3">
<visual>
<geometry>
<box size="0.3 0.3 0.2"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
</visual>
</link>
<link name="link4">
<visual>
<geometry>
<box size="0.3 0.3 0.2"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
</visual>
</link>
<joint name="base_link" type="fixed">
<parent link="base_link"/>
<child link="link1"/>
</joint>
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="5 3 0" rpy="0 0 0" />
<axis xyz="-0.9 0.15 0" />
</joint>
<joint name="joint2" type="continuous">
<parent link="link1"/>
<child link="link3"/>
<origin xyz="-2 5 0" rpy="0 0 1.57" />
<axis xyz="-0.707 0.707 0" />
</joint>
<joint name="joint3" type="continuous">
<parent link="link3"/>
<child link="link4"/>
<origin xyz="5 0 0" rpy="0 0 -1.57" />
<axis xyz="0.707 -0.707 0" />
</joint>
</robot>
▼それぞれのリンクに赤い立方体が配置されました。
形状や色の指定
Qiitaの記事にあった色付きのURDFファイルも取り込んでみました。
▼boxだけでなくcylinderやsphereが含まれています。
<robot name="test_robot">
<link name="base_link"/>
<joint name="body1_joint" type="fixed">
<parent link="base_link"/>
<child link="body1_link"/>
</joint>
<link name="body1_link">
<visual>
<geometry>
<box size="0.3 0.3 0.2"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="red">
<color rgba="1.0 0.0 0.0 2.0"/>
</material>
</visual>
</link>
<joint name="body2_joint" type="fixed">
<parent link="base_link"/>
<child link="body2_link"/>
<origin xyz="0.5 0 0" rpy="0 0 0"/>
</joint>
<link name="body2_link">
<visual>
<geometry>
<sphere radius="0.2"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="green">
<color rgba="0.0 1.0 0.0 2.0"/>
</material>
</visual>
</link>
<joint name="body3_joint" type="fixed">
<parent link="base_link"/>
<child link="body3_link"/>
<origin xyz="-0.5 0 0" rpy="0 0 0"/>
</joint>
<link name="body3_link">
<visual>
<geometry>
<cylinder length="0.3" radius="0.1"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="blue">
<color rgba="0.0 0.0 1.0 2.0"/>
</material>
</visual>
</link>
</robot>
読み込んでみると、以下のようなエラーが出ていました。
▼Unable to parse component [2.0] to a double(while parsing a color value)というエラーです。
▼色が反映されていません。
<color rgba="0.0 1.0 0.0 2.0"/>のように、数値が1を超えていたことが原因のようです。2.0だった部分を1.0に直すと、色付きで表示されました。
▼隣り合った部品は色分けすると良さそうですね。
多関節ロボットのモデルもあったので読み込んでみました。
▼先程と同じく、色の値が2.0の部分は1.0に直しています。materialの宣言は一度だけで使い回せるようですね。
<robot name="test_robot">
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
<link name="base_link"/>
<joint name="body1_joint" type="fixed">
<parent link="base_link"/>
<child link="body1_link"/>
</joint>
<link name="body1_link">
<visual>
<geometry>
<box size="0.1 0.1 0.5"/>
</geometry>
<origin xyz="0 0 0.25" rpy="0 0 0"/>
<material name="red"/>
</visual>
</link>
<joint name="body2_joint" type="revolute">
<parent link="body1_link"/>
<child link="body2_link"/>
<origin xyz="0.1 0 0.5" rpy="0 0 0"/>
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>
<link name="body2_link">
<visual>
<geometry>
<box size="0.1 0.1 0.4"/>
</geometry>
<origin xyz="0 0 0.2" rpy="0 0 0"/>
<material name="red"/>
</visual>
</link>
<joint name="body3_joint" type="revolute">
<parent link="body2_link"/>
<child link="body3_link"/>
<origin xyz="0.1 0 0.4" rpy="0 0 0"/>
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>
<link name="body3_link">
<visual>
<geometry>
<box size="0.1 0.1 0.4"/>
</geometry>
<origin xyz="0 0 0.2" rpy="0 0 0"/>
<material name="red"/>
</visual>
</link>
</robot>
▼問題なく表示できました。ジョイントの角度を変更すると動かすことができました。
Open ManipulatorのURDFファイルを作成する
Open Manipulator関連の情報について
Open Manipulatorのxacroファイルは存在するようですが、今回は練習がてらURDFで簡単なモデルを作成してみました。
▼xacroファイルはこちらのリポジトリにありました。
https://github.com/ROBOTIS-GIT/open_manipulator/tree/master/open_manipulator_description/urdf
▼CADのデータは以下のフォルダからダウンロードしました。
https://github.com/ROBOTIS-GIT/open_manipulator/tree/master/open_manipulator_description/meshes
▼Unreal EngineでOpen Manipulatorのモデルを作成したときも利用しました。
▼Open Manipulatorの寸法やパラメータは、以下のページに書かれています。
https://emanual.robotis.com/docs/en/platform/openmanipulator_x/specification/#hardware-specification
linkとjointを定義する
ジョイントが足りていないことに後で気づいたのですが、Open Manipulatorのようなモデルを作成してみました。
以下のコマンドでopen_manipulator.xmlファイルの作成と表示、削除を繰り返して調整しました。nanoエディタで作業するのはカーソル移動が大変なので、コード自体はVS Codeで書いています。
sudo nano open_manipulator.xml
roslaunch urdf_tutorial display.launch model:=open_manipulator.xml
rm -f open_manipulator.xml
以下のURDFファイルを作成しました。
<robot name="test_robot">
<material name="red">
<color rgba="1.0 0.0 0.0 1.0"/>
</material>
<link name="base_link"/>
<link name="link1">
<visual>
<geometry>
<box size="0.05 0.05 0.077"/>
</geometry>
<origin xyz="0 0 0.0385" rpy="0 0 0"/>
<material name="red"/>
</visual>
</link>
<link name="link2">
<visual>
<geometry>
<box size="0.05 0.05 0.128"/>
</geometry>
<origin xyz="0 0 0.0385" rpy="0 0 0"/>
<material name="red"/>
</visual>
</link>
<link name="link3">
<visual>
<geometry>
<box size="0.05 0.148 0.05"/>
</geometry>
<origin xyz="0 0.024 0" rpy="0 0 0"/>
<material name="red"/>
</visual>
</link>
<link name="link4">
<visual>
<geometry>
<box size="0.05 0.05 0.05"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 0"/>
<material name="red"/>
</visual>
</link>
<joint name="joint1" type="fixed">
<parent link="base_link"/>
<child link="link1"/>
</joint>
<joint name="joint2" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="0 0 0.077" rpy="0 0 0"/>
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>
<joint name="joint3" type="revolute">
<parent link="link2"/>
<child link="link3"/>
<origin xyz="0 0.024 0.128" rpy="0 0 0"/>
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>
<joint name="joint4" type="revolute">
<parent link="link3"/>
<child link="link4"/>
<origin xyz="0 0.148 0" rpy="0 0 0"/>
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>
</robot>
▼チェックは問題ありませんでした。親子付けが増えてくると、こちらの方が見やすいように思います。
check_urdf open_manipulator.xml
各リンクの形状は直方体にしましたが、後でCADのデータを割り当てるので特にこだわっていません。
▼以下のように表示されました。
▼Open Manipulatorで見たような姿勢になりました。
stlファイルを読み込めない問題
作成したURDFモデルにstlファイルを割り当ててみました。
調べているとCADのファイルパスを指定するのにpackage://~から始まっているものがあったのですが、これは相対パスでの指定になるようです。
▼こちらのページに少しだけ書かれていました。
▼こちらのページで議論されていました。
https://answers.gazebosim.org/question/1684/when-importing-a-urdf-model-how-are-package-uris-parsed
CADのデータはURDFファイルがある、パッケージのurdfフォルダの中に入れておきました。
▼WSL2のUbuntu環境には、エクスプローラーでアクセスできます。ドラッグ&ドロップで追加しました。
▼追加すると:Zone.Identifierが付いたものがあったので、rm -f ファイル名
で削除しました。
この後stlファイルのパスを指定したURDFファイルを作成して、RVizで表示してみたのですが、読み込むことができていませんでした。しかもエラーが出ていなかったので、対処に時間がかかりました。
これまでと同様に以下のコマンドを実行しました。
roslaunch urdf_tutorial display.launch model:=open_manipulator.xml
▼stlファイルを読み込めていません。
今回の問題の原因は、恐らくWSL2で実行しているからです。
▼以下のissueのページを参考に、LIBGL_ALWAYS_SOFTWARE=trueをつけてコマンドを実行してみました。
https://github.com/jsk-enshu/robot-programming/issues/440
LIBGL_ALWAYS_SOFTWARE=true roslaunch urdf_tutorial display.launch model:=open_manipulator.xml
これでstlファイルを読み込んで表示することができました。環境変数として実行しておいた方が良いのかもしれません。
▼以下のコマンドを予め実行しておけば、前の部分に付け加えなくても表示されました。頻繁に実行するようであれば、.bashrcに追記しておくと良さそうです。
export LIBGL_ALWAYS_SOFTWARE=true
▼scaleの単位が違うようで、巨大になって表示されていました。
▼scaleを調整しました。向きを調整する必要がありそうです。
stlファイルによってはASCIIかバイナリかという違いもあって、バイナリなら読み込めるようです。実際、Fusion 360でバイナリのstlファイルとしてエクスポートしないと読み込めないことがあったので注意が必要です。
▼こちらに書かれていました。
https://gist.github.com/KobayashiRui/4eb5caa16bb2bd1055b2b36a0758df88
stlファイルを読み込んでRVizで表示する
寸法図だけだと微妙にずれているところがあったので、微調整しながらURDFファイルを作成しました。
▼最終的なURDFファイルはこちら
<robot name="test_robot">
<link name="base_link"/>
<link name="link1">
<visual>
<geometry>
<mesh filename="package://urdf_test/urdf/chain_link1.stl" scale="0.001 0.001 0.001"/>
</geometry>
<origin xyz="0 -0.01 0" rpy="0 0 1.5708"/>
</visual>
</link>
<link name="link2">
<visual>
<geometry>
<mesh filename="package://urdf_test/urdf/chain_link2.stl" scale="0.001 0.001 0.001"/>
</geometry>
<origin xyz="0 0 0.035" rpy="0 0 1.5708"/>
</visual>
</link>
<link name="link3">
<visual>
<geometry>
<mesh filename="package://urdf_test/urdf/chain_link3.stl" scale="0.001 0.001 0.001"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 1.5708"/>
</visual>
</link>
<link name="link4">
<visual>
<geometry>
<mesh filename="package://urdf_test/urdf/chain_link4.stl" scale="0.001 0.001 0.001"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 1.5708"/>
</visual>
</link>
<link name="link5">
<visual>
<geometry>
<mesh filename="package://urdf_test/urdf/chain_link5.stl" scale="0.001 0.001 0.001"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 1.5708"/>
</visual>
</link>
<link name="grip_r">
<visual>
<geometry>
<mesh filename="package://urdf_test/urdf/chain_link_grip_r.stl" scale="0.001 0.001 0.001"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 1.5708"/>
</visual>
</link>
<link name="grip_l">
<visual>
<geometry>
<mesh filename="package://urdf_test/urdf/chain_link_grip_l.stl" scale="0.001 0.001 0.001"/>
</geometry>
<origin xyz="0 0 0" rpy="0 0 1.5708"/>
</visual>
</link>
<joint name="base" type="fixed">
<parent link="base_link"/>
<child link="link1"/>
</joint>
<joint name="joint1" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>
<joint name="joint2" type="revolute">
<parent link="link2"/>
<child link="link3"/>
<origin xyz="0 0 0.077" rpy="0 0 0"/>
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>
<joint name="joint3" type="revolute">
<parent link="link3"/>
<child link="link4"/>
<origin xyz="0 0.024 0.128" rpy="0 0 0"/>
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>
<joint name="joint4" type="revolute">
<parent link="link4"/>
<child link="link5"/>
<origin xyz="0 0.124 0" rpy="0 0 0"/>
<limit lower="-1.5" upper="1.5" effort="0" velocity="0"/>
</joint>
<joint name="joint_r" type="prismatic">
<parent link="link5"/>
<child link="grip_r"/>
<origin xyz="0 0.0875 0" rpy="0 0 0"/>
<limit lower="0.02" upper="0.038" effort="0" velocity="0.5"/>
</joint>
<joint name="joint_l" type="prismatic">
<parent link="link5"/>
<child link="grip_l"/>
<origin xyz="0 0.0875 0" rpy="0 0 0"/>
<limit lower="-0.038" upper="-0.02" effort="0" velocity="0.5"/>
</joint>
</robot>
先程stlファイルを読み込めたコマンドを実行しました。
LIBGL_ALWAYS_SOFTWARE=true roslaunch urdf_tutorial display.launch model:=open_manipulator.xml
▼以下のように表示することができました。
▼左側のShow Names、Show Axes、Show Arrowsをオフにすると、stlファイルだけ表示されました。
RViz上で操作してみました。
▼jointを操作することができました。
▼動画には表示されていませんが、小さいウィンドウのスライドバーでjointを操作できました。Randomizeを選択すると、ランダムな姿勢になります。
最後に
今回はinertiaやmassを設定していませんが、追加すれば設定できそうです。
XMLでの記述だったので、HTMLで画面を作成したときに似ていました。MoveItと連携できるなら、運動学の計算を行いたいなと思っています。
▼URDFファイルと違って、xacroファイルだとプログラマブルな書き方ができるそうです。パラメータの調整が大変だったので、xacroファイルの方が簡単になるかもしれません。
https://qiita.com/srs/items/43528d00ee789171367f
https://qiita.com/srs/items/9ac7f2f6e47732bb7535
趣味的ロボット研究所をもっと見る
購読すると最新の投稿がメールで送信されます。