ROS/MoveIt!によるマニピュレータの制御

Ubuntu18.04+ROS(Melodic)上でマニピュレータ制御を行う手法の解説

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で記載する必要があるが，

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

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

各リンクは，赤(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に記載する．

melodicではjoint-state-publisherでwarningが出るのでjoint-state-publisher-guiを入れておく．

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

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

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

$ roslaunch sixdofarm sixdofarm.launch

$ rosrun rviz rviz

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

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

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

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

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

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

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

1. 左のDisplaysの欄にある「Fixed Frame」を選んで「map」と書いてあるところを「base」に変更する．

2. その後「Add」ボタンを押して出てくるウィンドウ中の「By display type」タブ中にある「RobotModel」を選択して追加すると上記画面になる．

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

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

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

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

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

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

$ sudo apt-get install ros-melodic-ros-control ros-melodic-ros-controllers ros-melodic-joint-state-controller ros-melodic-effort-controllers ros-melodic-position-controllers ros-melodic-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」を選択してマニピュレータの自己干渉をチェックする．

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

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

クリックして拡大

マトリックス表示で見やすくなった．

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

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

SetupAssistant2.0からはmatrix表示が採用されており，どのリンクとのペアが鑑賞しているのかわかりやすくなっている．

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

次に，「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)と基本姿勢(pose1)を追加してみた．

initialの例

直立姿勢（initial)が登録されているのが確認できる

基本姿勢（pose1)の登録

２つのポーズの登録結果

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

「ROS Control」で関節のコントローラの設定

「Ros Control」で関節のコントローラを選択する．

今回は，関節を位置制御するためにposition controllerを選択します．

まず，「Auto Add Follow JointsTrajectory Controllers For Each Planning Group」のボタンを押すと，下の表にPlanning Groupsで設定したグループが追加されます．

singleと名前をつけたグループには，single_controllerという名前でコントローラが自動的に割り当てられています．

single_controllerを選択してから「Edit_Selected」を押して，コントローラを変更します．

「Controller Type」を変更するために，メニューをクリックして，一覧の中から＠effort_controllers/JointPositionController」を選択する．

「Simulation」でGazeboのモデル生成

「Simulation」でGazeboのモデルが生成できるようになった．

「Generate URDF」をクリックすると，ウィンドウ内にGazeboに対応したURDFを出力してくれる．

これを，コピーして，自分でURDFを書き換える必要がある．

後ほどの節でGazeboの説明をするが，ここで自動生成したファイルを使っても良い．

「3D Perception」Depth Cameraなどのデータを取り扱うときのデータ形式の設定

「３D Perception」でセンサの設定ができる．今回は使わないので「None」のままでOK.

「PointCloud」と「Depth Map」が選択可能．

「Author Information」作成者の入力

「Author Information」で製作者の名前とメールアドレスを入力．無記名だと生成させてくれない．

「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)のおかげです．

ほかにもいろいろな，逆運動学のソルバーがあるのでkdlを選んだところで，別のライブラリを選んでお好みのものを使ってください．

IKFastが早いのですが，これは依然と導入方法が違うので別の機会に説明します．

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

「initial」や「pose1」を選んで

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

4. Gazebo上での動力学シミュレーション

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

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