ROSによるマニピュレータの制御（改定）

Ubuntu16.04+ROS(Kinetic)上でマニピュレータ制御を行う手法を解説する．

1. URDFによるマニピュレータのモデル作成方法

2. MoveIt!を利用した自作マニピュレータの制御

3. IKFastを利用した逆運動学ソルバとMoveit!の連携

4. Gazebo上でのマニピュレータの動力学シミュレーション（別ページへ）

1. URDFによるマニピュレータのモデル作成

URDFという定義でROS上のマニピュレータのリンク構成を定義する．

URDFについての詳しい定義はここを参照．

少ない自由度のマニピュレータの作成例はチュートリアルやROSの本でいろいろ紹介されているので

ここでは下記の図に示すような６自由度マニピュレータを構成することを考える．

6自由度マニピュレータの構成例と座標軸設定

ROSでURDFのモデルを下記の手順でパッケージ作成から行う．

パッケージ名はsixdofarmとする．

$ cd ~/catkin_ws/src

$ catkin_create_pkg sixdofarm urdf

$ cd sixdofarm

$ mkdir urdf

$ cd urdf

$ gedit sixdofarm.urdf

最後のコマンドで実行しているgeditでsixdofarm.urdfに下記の内容を記載する．

図ではmm単位での寸法を記載しているが，URDFではm単位での寸法を記載している．

また，リンクの質量，慣性モーメントの単位はkg, kgm^2で記載する必要があるが，

下記の例で示したモデルでは，根拠のない数値を与えているので，実際に利用したい時は

利用したいロボットのパラメータを与える必要がある．

sixdofarm.urdf

<?xml version="1.0" ?>

</material>

</material>

</material>

</joint>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</robot>

各リンクは，赤(red)，緑(green)，青(blue)の順番で色分けして描画している．

リンクは四角柱で描画しているが，リンク長がゼロのリンクでも関節が存在していることがわかるように

旋回関節は横向きの四角柱を関節取り付け位置に描画してる．

今回のこのような描画は，リンクごとの接触が起きないようにすることを全く考えていないため

あくまで，リンクの位置関係を認識するためだけに利用するものとする．

接触がないようにシミュレーションしたい時は

ROSチュートリアルページに紹介されているようなrrbotのモデルのように

四角柱の取り付け位置にオフセットを持たせるなどの工夫が必要．

もしくは，きちんとしたCADモデルを用いるなど．

モデルの確認

liburdfdomというツールで記載したURDFファイルが問題ないかどう確認する．

インストールは下記のコマンドで行う．

$ sudo apt-get install liburdfdom-tools

インストール後に下記のコマンドでurdfファイルの確認を行う．

実行場所は当然だが，~/catkin_ws/src/sixdofarm/urdfの下で実行しているものとする．

$ check_urdf sixdofarm.urdf

---------- Successfully Parsed XML ---------------

root Link: base has 1 child(ren)

child(1): link1

child(1): link2

child(1): link3

child(1): link4

child(1): link5

child(1): link6

child(1): link7

上記のような結果が出れば問題なく6関節のマニピュレータのモデルができている．

グラフ表示してどのような接続がされているか確認してみる．

$ urdf_to_graphiz sixdofarm.urdf

上記コマンドを打つと"sixdofarm.pdf"と"sixdofarm.gv"という２つのファイルが生成される．

pdfファイルを表示して確認する時は下記コマンドで表示できる．（ドキュメントビュワー(evince)がインストールされている時）

$ evince sixdofarm.pdf

*.gvファイルはpdfファイルを作成する時の元データと思えば良い．

テキストデータなので適当なエディタで内容を確認できる．

*.gvファイルからpdfファイルに変換したい時は下記コマンドで変換できる．

$ dot -Tpdf sixdofarm.gv -o sixdofarm.pdf

今回の例の"sixdofarm.pdf"の内容は下記のようになっているはず．

（クリックで拡大）

四角で囲まれているものはリンク，楕円で囲まれているのは関節となる．

リンクから伸びている矢印の横に記載されている"xyz" と "rpy" はリンク座標原点から

次の関節までの位置ベクトルを表していると考えればいい．

ex: xyz:0 0 1 はz方向に1mの位置に次の関節があるという意味．

この図だと関節角の軸がどの方向にあるかわからない．

Rvizでのモデルの表示

結局上記までのツールでは関節格が思った場所にあるかわからないので

グラフィカルに確認するツールを使う．

下記コマンドでlaunchファイルを作成してrvizで確認する．

$ cd ~/catkin_ws/src/sixdofarm/

$ mkdir launch

$ cd launch

$ gedit sixdofarm.launch

下記に示す内容をsixdofarm.launchに記載する．

sixdofarm.launch

</launch>

作成したlaunchファイルを下記コマンドで実行するとRvizというグラフィカルツールで

作成した６自由度アームを表示することができる．

$ roslaunch sixdofarm sixdofarm.launch

$ rosrun rviz rviz

上記コマンドでsixdofarm.launchを実行した時に下図のようなウィンドウが現れる．

ここで６つの関節をスライドバーで調整することが可能となる．

このウィンドウは，先ほど記載したlaunchファイル内に記載した

joint_state_publisherのノードが提供するGUI画面であり

GUIにより入力された値を配信してくれる．

Rvizを立ち上げた時の表示は下記のようになる．

クリックで拡大

最初からこの画面になるわけではないので下記の手順で表示させること．

左のDisplaysの欄にある「Fixed Frame」を選んで「map」と書いてあるところを「base」に変更する．
その後「Add」ボタンを押して出てくるウィンドウ中の「By display type」タブ中にある「RobotModel」を選択して追加すると上記画面になる．

関節角をGUIのスライドバーで変化させることができるようになったが，これでは何もできないのでMoveItを使って自作した６自由度マニピュレータの逆運動学を解いてもらう手順を次の節で説明する．

逆運動学は良いからgazeboの連携を確認したいという場合は，２節，３節は飛ばして４節をみてください．

2. MoveItを利用した自作マニピュレータの制御

上記の第１節で作成したURDFの６自由度マニピュレータをMiveItで制御する方法を示す．

まず，下記のコマンドで必要なパッケージをインストールする．

$ sudo apt-get install ros-kinetic-moveit*

$ sudo apt-get install ros-kinetic-ros-control ros-kinetic-ros-controllers ros-kinetic-joint-state-controller ros-kinetic-effort-controllers ros-kinetic-position-controllers ros-kinetic-joint-trajectory-controller

インストールが終わったら早速下記のコマンドでMoveItのアシスタントツールを立ち上げる．

MoveItは導入が簡単になるようにGUIツールが用意されている．

$ roslaunch moveit_setup_assistant setup_assistant.launch

上記コマンドを打つと下記のような画面が現れる．

クリックで拡大

新規作成の場合は，左側のボタン「Create New MoveIt Configuration Package」を押す．

すると下記のような画面になるので，Load a URDF or COLLADA Robot Model の欄に

前節で作成した６自由度アームのurdfファイルを指定する．

クリックで拡大

ロードが成功すると下記のような画面になり，右側に６自由度アームの３Dモデルが表示される．

左のメニューに沿って設定していくとMoveItの設定が完了するようになっている．

まずはじめに，２番目にある「Self-Collisions」を選択してマニピュレータの自己干渉をチェックする．

真ん中にある「Regenerate Default Collision Matrix」というボタンを押すと，ロードしたurdfファイルのロボットの自己干渉をチェックしてくれる．

今回のロボットを自動チェックした結果は下記．

クリックして拡大

隣接したリンクの組み合わせが自己干渉してしまっているので「Disabled」にチェックが入っており，右列にその理由が記載されている．

自己干渉していないリンクを表示したい時は下にあるチェックボックスにマークを入れると表示される．

第１節でも述べたが，隣接したリンクの自己干渉も正確にチェックしたい時はモデルを作るときから考慮した方が良い．

Xacroなどを使ってCADモデルと連携させるなどの必要があるが，それらは，ROSの本のKobuki armのテキストを参考にしてください（やったことないので．．．そのうちやりたい）．

次に，「Virtual Joints」という項目があるが，仮想的な関節は今回取り扱わないのでスキップ．

「Planning Groups」の設定

初期画面は下記のような画面なので真ん中の下にある「Add Group」ボタンを押してグループを追加する．

「Add Group」ボタンを押すと次のような画面になるので「Group Name」を好みで入力して「Add Joints 」を押す．

人間型ロボットや多脚ロボットの場合は，脚や腕にグループを分けてわかりやすい名前をつけると良い．

ex: RArm:右腕，LArm:左腕

今回の例では，１本の腕なので「single」と名前をつけている．

クリックで拡大

「Add Joints」を押すと下記のような画面になるので，左側のリストに表示されている関節を全て右側に「＞」ボタンを押して右側のリストに追加する．追加後に「Save」ボタンを押す．

追加後の画面は下図のようになる．

次に，「Links」の内容を追加するために「Links 」を選択後に「Edit Selected」ボタンを押して次の画面に移る．

最後に「Chain」を選んで「Edit Selected」ボタンを押し設定画面へ移動する．

下図の通り，「Base Link」に"base"を登録し，「Top Link」に"link7"を登録する．

「Robot Poses」で基本姿勢を登録

すぐに動作を見たい場合はここはスキップしていい．

あとでアシスタントを立ち上げ直して追加することも可能です．

今回の例では，下図の通り，直立姿勢（initial_pose)と基本姿勢(standard_pose)を追加してみた．

initial_poseの例

standard_poseの例

２つのポーズの登録結果

次の「End Effectors」と「Passive Joints」いう項目は，今回のロボットでは取り付けていないのでスキップ．

「Configuration Files」で設定した内容をパッケージに出力

最後に，「Configuration Files」で設定内容をcatkinでコンパイルできるパッケージに出力する．

出力先は「Configuration Package Save Path」で設定する．

出力れいの名前付けのれいに沿って「ロボット名_moveit_config」とすると良い．

今回はロボット名はsixdofarmなので下記のようなパスを記載する．

/home/ユーザ名/catkin_ws/src/sixdofarm_moveit_config

「Generate Package」ボタンを押してパッケージを生成する．

「No end effectors have been added」，「No virtual joints have been added」と表示されるがそのまま「OK」ボタンを押して生成を開始する．

問題なければ「Exit Setup Assistant」ボタンの左側に「Configuration package generated successfully!」と出る．

最後にアシスタントを終了させておく．

以上で，MoveItを自作ロボットアームのモデルを利用可能にする準備が整った．

出力したパッケージをコンパイルするため下記のコマンドを実行．

$ cd ~/catkin_ws/

$ catkin_make

問題なくコンパイルできたら下記のコマンドで，Rviz上にマニピュレータを表示させる．

demo.launchというファイルが用意されているので必要な画面をすぐに表示可能です．

$ roslaunch sixdofarm_moveit_config demo.launch

コマンド入力後に下記のような画面が出れば成功．

クリックで拡大

６自由度アーム先端のボールのようなものをドラッグすると姿勢を変更可能．

腕の届く範囲で甲斐がない場合は表示されないのでドラッグする場所をいろいろ変えて確かめてみること．

下記は適当にドラッグしてみた例．

ボールの位置は手先というよりも，第７リンクの根元の旋回関節がある場所に表示されている．

先端の位置をドラッグしたいという時は，ダミー関節をつけるか，リンクを追加しておくなどのモデルの工夫が必要．

クリックで拡大

手先の位置姿勢をドラッグしただけで各関節の姿勢が求まっているのは，KDL(Kinematics Dynamics Library)のおかげです．

基本的に収束計算を行うことで解を求めているのである程度の時間がかかるので，もっと早く解を求めたいという場合は次の節のIKFastプラグインを作成する必要があります．

とりあえずドラッグして姿勢を求めてくれた姿勢は，現在の直立している姿勢からどのように変化させれば良いのかわからないので，その軌道を次の手順で生成する．

「Planning」のタブを選び，「Commands」の下にある「Plan」というボタンを押すだけで計画をしてくれる．

アニメーションでどのように関節角度が変化していくか視覚的に確認できる．

障害物も何もない空間でのプランニングなのであまりありがたみがわからないが

障害物をRviz上に表示して，その障害物を避けつつ目的とする姿勢に移動する計画もやってくれるらしい．．．（まだやっていないのでそのうち確かめます）

計画した軌道を実行する時は「Execute」ボタンを押して実行する．

計画と実行を同時に行う時は「Plan and Execute」ボタンを押す．

目的位置まで到達したらまた最初の位置から移動を始めるアニメーションを繰り返し行うので実行している気分にはならない．

繰り返しのアニメーションを見たくない時は下図のようにウィンドウの左側の「Displays」の

「MotionPlanning」の中の「Planned Path」の中にある「Loop Animation」のチェックを外すと

一回だけアニメーションして止まる．

また，移動した姿勢を等間隔で表示する機能を使いたい時は「Show Trail」チェックボックスにチェックを入れると

移動軌跡を表示してくれる．

クリックで拡大クリックして拡大

あくまで計画軌道の確認という位置付けなので，実機がない場合は，動力学シミュレータなどと連動して動作を確認する必要がある．２つ後の節のgazeboとの連携で説明を行う予定．

いろいろな目的位置までの計画を確かめてみたい時は「Query」というところの下にある「Select Goal State:」タブで<random valid>が選ばれた状態で「Update」ボタンを押すとランダムに目標姿勢を生成してくれる．

初期姿勢やよく使う姿勢として登録した「initial_pose」や「standard_pose」に移動したいという時は，

「Select Goal State:」タブが選ばれた状態で

<random valid>の項目をクリックして出てくるメニューの中の

「initial_pose」や「standard_pose」を選んで

「Update」ボタンを押すと目標姿勢が変更される．

次はIKFastという逆運動学ソルバをMoveItに利用する手順を説明する．

3. IKFastを利用した逆運動学ソルバとMoveItの連携

上記の第２節で作成したMoveItの逆運動学ソルバを高速に解を導くことのできるIKFastに変更する手順を示す．

IKFastという逆運動学の解を得るためにプラグインはOpenraveから生成する必要があります．

現在(2016/1/13)では，apt-get install openraveなんてコマンドですぐにインストールすることができないので，関連ソースを下記のページを参考にダウンロードしてからコンパイルする必要があります．

基本的に公式ページの説明通りなのだが，最新の説明になっていないのですぐにうまくいかない．

Creating a custom IKFast Plugin

３．１ OpenRaveのインストール

Install OpenRave on Ubuntu14.04(クリックして参考ページへ）

とりあえず上記ページへ行けばわかりますが消えても問題ないように必要なパッケージインストールを下記に記す．

$ sudo apt-get install cmake g++ git qt4-dev-tools zlib-bin $ sudo apt-get install ipython python-dev python-h5py python-numpy python-scipy python-sympy

$ sudo apt-get install libassimp-dev libavcodec-dev libavformat-dev libavformat-dev libboost-all-dev libboost-date-time-dev libbullet-dev libfaac-dev libglew-dev libgsm1-dev liblapack-dev libmpfr-dev libode-dev libogg-dev libopenscenegraph-dev libpcre3-dev libpcrecpp0 libqhull-dev libqt4-dev libsoqt-dev-common libsoqt4-dev libswscale-dev libswscale-dev libvorbis-dev libx264-dev libxml2-dev libxvidcore-dev

$ sudo add-apt-repository ppa:openrave/release $ sudo sh -c 'echo "deb-src http://ppa.launchpad.net/openrave/release/ubuntu `lsb_release -cs` main" >> /etc/apt/sources.list.d/openrave-release-`lsb_release -cs`.list' $ sudo apt-get install collada-dom-dev

上記のコマンドを打てばソースのコンパイル環境が整うので下記のコマンドでファイルを取得．もしくはサイトに行って最新版をダウンロード．

$ cd ~

$ git clone --branch latest_stable https://github.com/rdiankov/openrave.git

$ cd openrave

$ cmake CMakeFIles.txt

$ make

$ sudo make install

コンパイルからインストールまで滞りなく終わったら，下記の内容を".bashrc"に追加しておく．

$ cd ~

$ gedit .bashrc

.bashrcの追記内容

-----ここから-------------

export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$(openrave-config --python-dir)/openravepy/_openravepy_

export PYTHONPATH=$PYTHONPATH:$(openrave-config --python-dir)

source `openrave-config --share-dir`/openrave.bash

-----ここまで--------------

OpenRaveのサンプルを動かして問題ないかどうか下記のコマンドで確認．

$ openrave.py --example hanoi

ハノイの塔のサンプル実行例．

上記のようなウィンドウが出てハノイの塔のデモンストレーション動作が確認できればインストール成功．

3.2 OpenRave用のモデルデータCollada Fileの作成

下記コマンドで，前節までで使ってきたurdf形式の６自由度マニピュレータのモデルをOpenRaveで利用できるモデル形式へ変換する．

$ rosrun collada_urdf urdf_to_collada <myrobot_name>.urdf <myrobot_name>.dae

<myrobot_name>は前節で作った６自由度マニピュレータであれば「sixdofarm」と記載する．

前節で作ったロボットのモデルのある場所まで移動してから変換コマンドを実行する．

$ cd ~/catkin_ws/src/sixdofarm/urdf/

$ rosrun collada_urdf urdf_to_collada sixdofarm.urdf sixdofarm.dae

「Document successfully written to sixdofarm.dae」と表示されれば成功．

生成されたcollada形式のファイルを下記のコマンドで確認する．

$ openrave-robot.py sixdofarm.dae --info links

name index parents

-------------------

base 0

link1 1 base

link2 2 link1

link3 3 link2

link4 4 link3

link5 5 link4

link6 6 link5

link7 7 link6

-------------------

name index parents

openrave-robot.pyのパスが通っていない場合は下記のコマンドで実行．

$ /usr/local/bin/openrave-robot.py sixdofarm.dae --info links

リンクの接続が表示されたが，これでは分かりにくいのでOpenRaveで表示してみる．

$ openrave sixdofarm.dae

下記のような画面が出ればOK.

変換された.daeファイルは関節部分の接触している場所のリンクが切り取られた形状をしている．

最初のモデルが悪いためにこのようになってしまっているが一応動くのでこのまま進めます．．．

3.3 IKSolverのファイル生成

MoveItの説明というよりもOpenRaveの説明になってしまいますが，IKSolverのファイルをOpenRaveのコマンドで生成します．

前節で生成したcollara形式のモデルを利用して逆運動学計算のC＋＋ファイルを生成することができます．

下記コマンドで逆運動学計算のプログラムを生成．

$ cd ~/catkin_ws/src/sixdofarm/urdf/

$ python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=sixdofarm.dae --iktype=transform6d --baselink=1 --eelink=7 --savefile=./sixdofarm.cpp

1行目は，自分のロボットの.daeファイルがあるところに移動しているだけ．

２行目で逆運動学のファイル生成．「-robot=」で自分のロボットの*.daeファイルを指定．「-baselink=」でベースリンクの番号を指定．「-eelink=」で先端のリンク番号を指定．「-savefile=」で出力ファイル名を指定．

各リンクの番号は3.2節で確認したリンク情報のindexの番号を使う．

sixdofarm.cppというファイルが生成されるまで以外と時間がかかるので待つ．

これを単体で使うことももちろん可能ですが．．．今は省略．

次に，この出力ファイルを使ってプラグインを生成します．

3.3 プラグイン作成

まず下記コマンドでOpenRaveのバージョンを確かめておく．（確かめなくてもいいけど）

$ python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --version

2016年1月8日確認の最新バージョンは「0x10000048」．１０進数でいうと６２番目．

あるバージョンまでは１０進数だったので，１６進数で表示された場合は，プラグイン作成ツールの修正が必要になる．

Allow hex version number of ikFast as used since 2014-10-08（ここ参考）

MoveItとIKfastを連携するためのツールが10進数で管理していた時のままなので，上記リンク先と同じように

/opt/ros/kinetic/lib/moveit_ikfast/create_ikfast_moveit_plugin.py

というmoveitのフルインストール時にインストールされたpythonファイルの中の

solver_version = int(line_search.group(1))

を見つけ出して下記のように修正する．

solver_version = int(line_search.group(1),0)

修正してプラグイン作成ツールを実行すると．．．下記のようないい加減なバージョンが出てくるが動くのでそのまま使うことにする．（ここまでの説明で実行したコマンドで出てくるメッセージではありません）

found source code generated by IKFast version 268435528

次に，プラグイン作成のためのパッケージを生成する．

$ cd ~/catkin_ws/src

$ catkin_create_pkg sixdofarm_ikfast_single_plugin

パッケージのネームのルールは公式ページと同じ通りにしておいた．

<myrobot_name>_ikfast_<planning_group_name>_plugin

という名前がいいらしい．<myrobot_name>は自分のロボットの名前というよりも「~/catkin_ws/src」の中に生成しておいた，自分のロボットモデルのパッケージメイと考えたほうが良い（かも）．

<planning_group_name>はMoveItのパッケージをアシスタントツールで作った時に起動生成する組み合わせとして登録しておいたグループの名前を書く．今回は「single」というグループを作ってあったのでその名前を使う．

上記の入れ物だけ作ったらとりあえずコンパイル．

$ cd ~/catkin_ws

$ catkin_make

次に，この節で最初に修正したpythonスクリプトを使ってプラグインのソースコードを生成．

$ rosrun moveit_ikfast create_ikfast_moveit_plugin.py sixdofarm single sixdofarm_ikfast_single_plugin ~/catkin_ws/src/sixdofarm/urdf/sixdofarm.cpp

上記コマンドを実行して成功した場合は最後に下記のメッセージが出る．

Created update plugin script at ~/catkin_ws/src/sixdofarm_ikfast_single_plugin/update_ikfast_plugin.sh

ちなみに，本家HPに書いてあるコマンドは下記

rosrun moveit_ikfast create_ikfast_moveit_plugin.py <myrobot_name> <planning_group_name> <moveit_ik_plugin_pkg> <ikfast_output_path>

<myrobot_name>：自分のロボットのモデルが管理されているパッケージの名前と考えたほうがいい（かも）

<planning_group_name>：MoveItで計算する逆運動学のグループ名（パッケージ生成時に指定したグループ名）

<moveit_ik_plugin_pkg>：本節で空のパッケージを作った時の名前

<ikfast_output_path>：前節で出力したファイルへのフルパスを書く必要がある．

無事ソースが生成できたのでもう一回コンパイル．

$ cd ~/catkin_ws

$ catkin_make

上記のコンパイルが終わると，プラグインを組み込む作業をする必要が残っているように感じるが，勝手にIKFastに置き換えられている．

どこが置き換えられているかというと，第２節で確認していたパッケージ（sixdofarm_moveit_config)の設定ファイルを見るとわかる．

$ cd ~/catkin_ws/src/sixdofarm_moveit_config/config

$ gedit kinematics.yaml

下記のように２行目のkinematics_solverがIKFastKinematicsPluginに置き換えられている．

---一部抜粋------------------

single:

kinematics_solver: sixdofarm_single_kinematics/IKFastKinematicsPlugin

----------------------

もともと何が指定されいたか見ていないからわからないかもしれませんが，kdlの計算に戻したい時は

上記の２行目を下記のように修正すればもとどおりになります．

kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin

---

ということで，動作確認のために，下記のコマンドで実際に動くかどうか確認．

$ roslaunch sixdofarm_moveit_config demo.launch

実行すると前節と同じようにrvizが立ち上がりますので，手先のリンクをドラッグして逆運動学計算の速さを体験してもらいます．気のせいかもしれませんが動きが軽くなったような気がします．．．

どのくらい早くなったか計測したい人は，自分でやってください．（投げやりですみません）

ここまで終わったので，あとは実機につなげたいところですが，物がないのでgazeboと連携して動かしてみたいと思います．

4. 次の予定

６自由度マニピュレータを動力学シミュレーションする手順を示す．逆運動学は必要ないということがあるので，２，３節の内容を実行していなくてもGazeboと連携できることを示した後に，逆運動学でのモーションの実行例を示す．

（Gazeboによるマニピュレータのシミュレーション（改訂版）へ移動）

Page updated

Google Sites

Report abuse