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

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

＊注意：このページの例では，ノードの名前の先頭が数字になっています．推奨されていない名前のつけ方なので，「6dofarm」の名前を「sixdofarm」に変更することをお勧めします．

1. Gazebo上でのマニピュレータの動力学シミュレーション

１．１準備

まず，ros-indigo-desktop-fullをインストールしている事を前提としているので，下記コマンドでgazeboが立ち上がるはずなので確認しておく．

$ gazebo

初回起動の時は下記のようなエラーメッセージが何回も出てくるが，数分待つとgazeboのサーバからデータをダウンロードが完了するので次回起動時からエラーメッセージがなくなる．

Error [Node.cc:90] No namespace found

心配になるくらい待つことがあるが少し様子を見ること．

しばらく待つと真っ黒だった右側の画面に地面を表すグリッドが下記のように表示される．

Gazeboがインストールされていなければ下記コマンドでインストール．

$ sudo apt-get install gazebo2

次に，GazeboとRosをつなぐバッケージを下記コマンドでインストール．

$ sudo apt-get install ros-indigo-gazebo-ros-pkgs ros-indigo-gazebo-ros-control

$ sudo apt-get install ros-indigo-ros-control ros-indigo-ros-controllers

ros-gzeboをインストールすると下記のコマンドでgazeboを立ち上げることができる．

$ roslauch gazebo_ros empty_world.launch

通常は，下記のように引数をつけて実行する．

$ roslauch gazebo_ros empty_world.launch paused:=true use_sim_time:=false gui:=true throttled:=false headless:=false debug:=true

各引数については，下記のリンクを参考にすること．

gazeboとRosをつなぐ（ros launch 編）：（九州工業大学のブログ）

Tutorial: Using roslaunch to start Gazebo, world files and URDF models （英語のオリジナルページ）

gazebo_ros_pkg_install（gazebo_rosのインストール説明英語オリジナルページ）

リンク構造のロボットのデモンストレーションは下記のコマンドで試すことができる．

roslaunch gazebo_ros mud_world.launch

下図のような画面が立ち上がる．

上記の例のlanchファイル(mud_world.launch)は下記の場所にある．

$ roscd gazebo_ros/launch

$ pwd

/opt/ros/indigo/share/gazebo_ros/launch

mud_world.launchの内容は下記．

mud_world.launch

</include>

</launch>

自分用に利用したい時は，「world_name」が記載されている行のvalueを変更すればOK．

この例の場合は，「worlds/mud.world」を参照しているが，

このファイルは下記のコマンドで移動した先で確認できる．

$ env |grep GAZEBO_RESOURCE_PATH (とgazeboの本家HPでは書いてあるが，apt-get install ros-indigo-desktop-fullでインストールした場合はパスが設定されていない）

$ cd /usr/share/gazebo-2.2/worlds

$ ls | grep mud.world

mud.world

mud.worlの中身は下記の通り．

sun, ground_plane, double_pendulum_woth_baseの３つのモデルを参照しているが，このモデルはもともとインストールされているのではなく，存在していない場合は，サーバから自動的にダウンロードしてくる．

mud.world

<sdf version="1.4"> <world name="default"> <include> <uri>model://sun</uri> </include> <include> <uri>model://ground_plane</uri> </include> <include> <uri>model://double_pendulum_with_base</uri> <name>pendulum_thick_mud</name> <pose>-2.0 0 0 0 0 0</pose> </include> ... </world> </sdf>

ダウンロードしてきたモデルは，自分のホームディレクトリ直下にある「.gazebo/model」というディレクトリに保存されている．

$ cd ~/.gazebo/models

$ ls

double_pendulum_with_base ground_plane sun （この３つのディレクトリが見えるはず．中にモデルファイルがある）

１．２ Gazebo ROSパッケージの作成

Gazebo ROSのページの紹介のHPの通り，下記のようなディレクトリ構成でパッケージを作成する．

「MYROBOT_description」は，自分のロボットモデルの記述があるディレクトリ．

「MYROBOT_gazebo」は，GazeboとROSをつなぐlaunchファイルとworldファイルを管理するディレクトリ

構成例

../catkin_ws/src /MYROBOT_description package.xml CMakeLists.txt /urdf MYROBOT.urdf /meshes mesh1.dae mesh2.dae ... /materials /cad /MYROBOT_gazebo /launch MYROBOT.launch /worlds MYROBOT.world /models world_object1.dae world_object2.stl world_object3.urdf /materials /plugins

早速６自由度アームのgazeboパッケージを作成してみる．

$ cd ~/catkin_ws/src

$ catkin_create_pkg 6dofarm_gazebo

生成したパッケージにlaunchフォルダとファイルを作成する．

$ cd ~/catkin_ws/src/6dofarm_gazebo

$ mkdir launch

$ cd launch

$ gedit 6dofarm.launch

6dofarm.launchの中身は下記．

6dofarm.launch

</include>

</launch>

launchファイル中にworlds/6dofarm.worldを記載したが存在していないので下記コマンドで生成する．

$ cd ~/catkin_ws/src/6dofarm_gazebo

$ mkdir worlds

$ cd worlds

$ gedit 6dofarm.world

6dofarm.worldの中身は下記．

6dofarm.world

<?xml version="1.0" ?><sdf version="1.4"> <world name="default"> <include> <uri>model://ground_plane</uri> </include> <include> <uri>model://sun</uri> </include> <include> <uri>model://gas_station</uri> <name>gas_station</name> <pose>-2.0 7.0 0 0 0 0</pose> </include> </world></sdf>

gas_station（ガソリンステーションのモデル）があるが．．．

Gazebo launchのページのチュートリアルと同じものをコピーしたので入っているだけ．

gas_stationは確認のためだけなので，必要がない人は削除する．

ここで，下記のようにlaunchファイルを実行するとgazeboが立ち上がりモデルが表示される．

$ roslaunch 6dofarm_gazebo 6dofarm.launch

実行後に下記のようなメッセージが出て，ダウンロードを開始するのでしばらく待つ．

Warning [ModelDatabase.cc:334] Getting models from[http://gazebosim.org/models/]. This may take a few seconds.

サーバ接続エラーが起きたら何回か立ち上げ直して試してみること．

立ち上がると下記のような画面が出る．

この状態では，ロボットは全く表示されていないのでロボットを表示させる手順に移る．

ロボットのモデルが既にできている場合は，ROS service CallのRobot Spawn Methodを利用可能．

Spawn（スポーン）は生成する意味だと思っていい（かもしれない）．

一般的なコマンドは下記の通り，

$ rosrun gazebo_ros spawn_model -file `rospack find MYROBOT_description`/urdf/MYROBOT.urdf -urdf -x 0 -y 0 -z 1 -model MYROBOT

MYROBOTは自分のロボットの名前．MYROBOT_descriptionは，前ページで6dofarmというパッケージを作っていたのであればそのまま利用できる．

よって，今回の６自由度アームをgazeboに表示させるために下記のコマンドを打つ．

（下記コマンドを打つ前に，先ほどの「roslaunch 6dofarm_gazebo 6dofarm.launch」でガソリンスタンドがgazeboに表示されている状態にしておくこと．）

$ rosrun gazebo_ros spawn_model -file `rospack find 6dofarm`/urdf/6dofarm.urdf -urdf -x 0 -y 0 -z 1 -model 6dofarm

上記コマンドを打つと，下記のように真っ白なロボットが降ってくる．．．で，倒れる．

これでは使い物にならないし，いちいち２個のウィンドウを使って環境表示とロボットの生成なんて手順をしなくてはいけないのでlaunchファイルを下記のように書き換えて対応する．

6dofarm.launch

</include>

</launch>

付け足した部分は，前回作成したロボットをlaunchする時の部分を抜き出してコピーしただけ．

最後に，spawnするために１行足せば，launchファイルを実行するだけでgazeboシミュレータ上に6dofarmのモデルを表示することができる．

$ roslaunch 6dofarm_gazebo 6dofarm.launch

修正しlaunchファイルでは上記コマンドだけで下記のようなgazeboシミュレータがすぐに立ち上がる．

ここまでできたが．．．

rvizで色付けした内容は反映されておらず，環境にも固定されていないために，すぐに倒れてしまっている．

移動ロボットならこれで良いが，固定のマニピュレータを考えている場合は，world座標系と固定する必要がある．

次の節で，urdfのファイルを固定する方法やGazebo上でも利用できるパラメータを設定する修正を行う．

1.3 URDFのロボットのGazeboパラメータ設定

1.3.1 world座標への固定

gazebo公式のTutorial: Using a URDF in Gazeboを参考に設定していく．

gazeboの世界に固定したい場合は，urdfファイルに一つworldリンクを追加するだけで良い．今回のマニピュレータでは，一番根本のリンクはbaseと成っているので，その一つ前にworldリンクを追加し，baseと固定してあげれば良い．

具体的には，下記の記述の通りリンクを追加すれば良い．

<joint name="world2base" type="fixed"> ここの名前(joint name)は適当．typeはfixedで固定しておく．

<parent link="world"/> 親をworldする．

<child link="base"/> 接続先の子供を自分のロボットの本体リンク（今回はbase）にする

</joint>

修正した6dofarm.urdfを下記に示す．

ちなみに，わかっていると思いますが，このファイルは，6dofarm_gazeboのパッケージの中のlaunchディレクトリ内の6dofarm.launchで指定した場所にあるurdfファイルです．

6dofarm.urdf (worldリンク追加）

<?xml version="1.0" ?>

</material>

</material>

</material>

</joint>

</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>

まず，これでgazeboの世界に固定されているか確認してみましょう！

$ roslaunch 6dofarm_gazebo 6dofarm.launch

上記コマンドで立ち上げると下記のようにgazeboの画面が立ち上がり，放置して待っていてもロボットが倒れないことを確認できます．

放置で倒れないと言っても本当に床に固定されているかわからないので，「ボール」や「円筒」などをぶつけてみてもベースリンクが倒れないか確認してみる．

ロボットアームの関節は折れ曲がって倒れているが，worldリンクと固定したbaseリンクは床面から離れていないことが確認出来る．

次に，gazebo上のロボットアームに色付けすることを考える．

1.3.2 gazeboの色付けと関節角トルクの設定

gazeboで各リンクに色指定する方法と関節のトルク設定をまとめて紹介する．

詳しくは，gazebo公式ページ：Tutorial: Using a URDF in Gazeboを参考にすること．

urdfファイルの各リンクの記述の最後に次の記述を追加することで色やトルク入力を受け付けるように設定が可能となる．

下記はリンク６の例．

<gazebo reference="link6"> urdfファイルのリンクを指定する（ここではlink6)

<material>Gazebo/Green</material> gazeboで表示する色を指定する

</gazebo>

<transmission name="trans6"> 関節トルクを発生させるトランスミッションの名前を指定する．(link6に対応してtrans6と命名）

<type>transmission_interface/SimpleTransmission</type> トランスミッションのインタフェースの種類を指定する

<joint name="joint6"> トランスミッションを割り当てるurdfの関節名を指定する

<hardwareInterface>EffortJointInterface</hardwareInterface> gazebo上のハードウェアとしてトルク(Effort)発生機を指定

</joint>

<actuator name="motor6"> アクチュエータの名前を指定する(link6に対応してmotor6と命名）

<hardwareInterface>EffortJointInterface</hardwareInterface> アクチュエータをトルク発生機に割り当てる

<mechanicalReduction>1</mechanicalReduction> 減速比

</actuator>

</transmission>

<robotNamespace>/6dofarm</robotNamespace>

</plugin>

</gazebo>

色の指定は<material>Gazebo/Green</material>で指定するだけでよい．

なお，Greenの指定は，urdfのファイルで記載してある<material name="green">で指定した定義を参照している．

よって，今回の6dofarmの例では，Red, Green, Blueをrviz用に指定してあるのでそのまま同じ色をgazeboにも利用する．

また，urdfファイルの最後にgazebo_ros連携の下記プラグインを記述する．（厳密には一番最後でなくて良い．<robot>・・・</robot>の範囲中に記載しておけばOKの筈．）

<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"> プラグインを指定

<robotNamespace>/6dofarm</robotNamespace> 自分のロボットの名前を記載．urdfの最初の方に記載している名前と一致させること

</plugin>

</gazebo>

上記の項目を記載して修正の終わったurdfファイルを下記に示す．

6dofarm.urdf 修正２

<?xml version="1.0" ?>

</material>

</material>

</material>

</joint>

</joint>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

<material>Gazebo/Red</material>

</gazebo>

<type>transmission_interface/SimpleTransmission</type>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</joint>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</actuator>

</transmission>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

<material>Gazebo/Green</material>

</gazebo>

<type>transmission_interface/SimpleTransmission</type>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</joint>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</actuator>

</transmission>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

<material>Gazebo/Blue</material>

</gazebo>

<type>transmission_interface/SimpleTransmission</type>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</joint>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</actuator>

</transmission>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

<material>Gazebo/Red</material>

</gazebo>

<type>transmission_interface/SimpleTransmission</type>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</joint>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</actuator>

</transmission>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

<material>Gazebo/Green</material>

</gazebo>

<type>transmission_interface/SimpleTransmission</type>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</joint>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</actuator>

</transmission>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

</joint>

<material>Gazebo/Blue</material>

</gazebo>

<type>transmission_interface/SimpleTransmission</type>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</joint>

<hardwareInterface>EffortJointInterface</hardwareInterface>

</actuator>

</transmission>

</geometry>

</collision>

</geometry>

</visual>

</inertial>

</link>

<material>Gazebo/Red</material>

</gazebo>

<robotNamespace>/6dofarm</robotNamespace>

</plugin>

</gazebo>

</robot>

上記ファイルでgazebo上にロボットを表示させて，色付けされたか確認する．

トルクの指令はまだできない．

$ roslaunch 6dofarm_gazebo 6dofarm.launch

上記コマンドで立ち上げると下記のようにgazeboの画面が立ち上がり，ロボットに色付けされていることを確認できます．

次の節で，各関節を動かすためのノード作成について述べる．

1.3.3 ros_controlを使った関節角の制御

gazebo 公式のTutorial: ROS Controlを参考に設定する．

まず，ros_controlを使ったノードを作成する．

$ cd ~/catkin_ws/src/

$ catkin_create_pkg 6dofarm_control

$ cd 6dofarm_control

$ mkdir launch //launchディレクトリ作成

$ mkdir config //configディレクトリ作成

$ cd config

$ gedit 6dofarm_control.yaml //configディレクトリ内にロボットの設定ファイル（.yaml)を作成

名前のルールは，"ロボット名_control"としてこれまでと同じように統一しておくと良い．

configディレクトリ内に設定ファイル（ロボット名_control.yaml）を下記のように記載する．

6dofarm_control.yaml

6dofarm:

# Publish all joint states -----------------------------------

joint_state_controller:

type: joint_state_controller/JointStateController

publish_rate: 50

# Position Controllers ---------------------------------------

joint1_position_controller:

type: effort_controllers/JointPositionController

joint: joint1

pid: {p: 100.0, i: 0.01, d: 10.0}

joint2_position_controller:

type: effort_controllers/JointPositionController

joint: joint2

pid: {p: 100.0, i: 0.01, d: 10.0}

joint3_position_controller:

type: effort_controllers/JointPositionController

joint: joint3

pid: {p: 100.0, i: 0.01, d: 10.0}

joint4_position_controller:

type: effort_controllers/JointPositionController

joint: joint4

pid: {p: 100.0, i: 0.01, d: 10.0}

joint5_position_controller:

type: effort_controllers/JointPositionController

joint: joint5

pid: {p: 100.0, i: 0.01, d: 10.0}

joint6_position_controller:

type: effort_controllers/JointPositionController

joint: joint6

pid: {p: 100.0, i: 0.01, d: 10.0}

pidという項目で，PIDのゲインを設定してあるが，公式ページと同じ値を入れてあり現在はいい加減な数字です．

publish_rateで制御周期を設定している．50hzよりも早くしたい時は数字を大きくすればよい．

次に，launchディレクトリに移動して，launchファイルを記載する．

例に漏れず名前は，ロボット名_control/launch/ロボット名_control.launch にする．

$ cd ~/catkin_ws/src/6dofarm_control/launch

$ gedit 6dofarm_control.launch //launchディレクトリ内にロボットのlaunchファイルを作成

6dofarm_control.launchの内容は下記の通り．

6dofarm_control.launch

<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"

output="screen" ns="/6dofarm" args="joint1_position_controller joint2_position_controller joint3_position_controller joint4_position_controller joint5_position_controller joint6_position_controller joint_state_controller"/>

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"

respawn="false" output="screen">

</node>

</launch>

あとは実行して確かめるだけ．

下記の手順で，gazeboを立ち上げてからcontrollerを立ち上げる．

$ roslaunch 6dofarm_gazebo 6dofarm.launch 端末１つ目で立ち上げる

$ roslaunch 6dofarm_control 6dofarm_control.launch 端末２つ目で立ち上げる

２つのノードを立ち上げた後に，コマンドラインでcontrollerに目的角度を指定してみる．

$ rostopic pub -1 /6dofarm/joint2_position_controller/command std_msgs/Float64 "data: 1.5"

上記のコマンドは関節２に対して1.5radに動けという指令．

*コマンド記述の注意点！！"data:"と"1.5"の間には"スペース"が必ず必要です！

これがなかったためにワーニングだけメッセージが出て通信できなくなって，どこが悪いのかわからず時間を潰してしまったことがありました．．．

下記の写真が上記コマンドを実行した時の様子．

PIDのゲインがいい加減なので，プラプラして動いている．

とりあえず，gazebo上の関節角に指令を与えられる状態になったのでPIDゲインを調整してみる．

1.3.3 rqtとdynamic reconfigureを使ったPID制御ゲインの調整

PIDのゲインは決め方が色々ありますが，ここではビジュアル的に見てフィードバックゲインを決定する方法で調整します．

まず，指令を連続的に変化させるためにrqtを利用する．

$ roslaunch 6dofarm_gazebo 6dofarm.launch 端末１つ目で立ち上げる

$ roslaunch 6dofarm_control 6dofarm_control.launch 端末２つ目で立ち上げる

$ rqt 端末３つ目で立ち上げる

rqtまっさらの画面（一度も利用したことがない時）が立ち上がる．

上の段にあるメニューの「Plugins」→「Topics」→「Message Publisher」を選択し追加する．

Message Publisherの追加

追加後の画面は下記の通り．

「Topic」の横にあるタブから自分がコントロールしたい関節を選ぶ．

関節１の場合は「/6dofarm/joint1_position_controller/command」

typeも送信したい型をタブから選択するだけで自分で書く必要がない．

「＋」ボタンで追加をする．

リストの表に入っている項目の「rate」の数字を変えると更新周波数を変更できる．

今回の例は「100」に設定．

「expression」の項目に数式を書くと更新周波数に従って更新される．

gazeboのチュートリアルページで紹介されている下記の式の通り記載してみる．

sin(i/rate*speed)*diff + offset

rate:100 [hz]（周波数を100にしてあるので）

speed: 5

diff: 1.7 [rad]

offset: 0 [rad]

→sin(i/100*5)*1.7 + 0

上記の設定をした状態は写真を参考．

ここまで設定したら，最後にリストの横のチェックボックスにチェックを入れると設定した情報を送信し始める．

Gazeboの画面を見て確認しても良いが，わかりにくいので下図の通りrqtにモニターを追加する．

「Plugins」->「Visualization」->「Plot」で追加．

追加後の画面は下図．

プロットしたいトピックを記載して追加ボタンを押す．

今回の例では，次の３つを追加する．

・/6dofarm/joint1_position_controller/command/data （指令値）

・/6dofarm/joint1_position_controller/state/error （指令値と現在位置の差）

・/6dofarm/joint1_position_controller/state/process_value (gazebo上の関節角）

なお，トピックのリストが微妙にきちんと出てこないので手打ちをしないといけないので面倒．．．

上記の画面のように２つのデータがプロットされることがわかる．

画面の横軸の表示範囲を調整したいときは下記の通りFigure Option を開いて，X軸の表示幅をmin, maxに記載するだけで良い．

Figure Option windowで横軸の表示範囲を調整

PIDのゲインを調整するツール「dynamic reconfigure」をrqtに追加する．

追加後に，joint1のPIDのゲインを調整して，所望する誤差の範囲に収まるように調整する．

PIDのゲイン調整の方法は詳しくは述べないが，実機のロボットがある場合は，その性能を無視して理想的なサーボゲインを調整することには意味がありません．用途の応じて調整してください．

1.4 Moveit!とGazeboの接続

Rosの本で紹介されていたKobuki armの動作は，Moveitで軌道生成されたあとに，「Execute」ボタンを押しただけで実行されている様子が紹介されている．

実際に同じアームを持っていないと試すことができないので，作成した６自由度ロボットアームとGazebo上で再現したモデルへMoveitから指令ができるようにするための接続方法について解説する．

Moveit!からGazeboへの関節角の指令は，joint_trajectory_controllerを用いる．

リンク先のページ（http://wiki.ros.org/joint_trajectory_controller）「Controller configuration examples」に記載されている通り，コントローラを下記のように記載する．

$ cd ~/catkin_ws/src/6dofarm_moveit_config/config

$ gedit trajectory_control.yaml

trajectory_control.yamlの内容は，前節の設定に合わせて各関節のコントローラはeffort_controllersに指定しておく．

trajectory_control.yaml

6dofarm:

joint_state_controller:

type: joint_state_controller/JointStateController

publish_rate: 50

6dofarm_joint_controller:

type: "effort_controllers/JointTrajectoryController"

joints:

- joint1

- joint2

- joint3

- joint4

- joint5

- joint6

gains:

joint1: {p: 100.0, i: 0.01, d: 10.0}

joint2: {p: 100.0, i: 0.01, d: 10.0}

joint3: {p: 100.0, i: 0.01, d: 10.0}

joint4: {p: 100.0, i: 0.01, d: 10.0}

joint5: {p: 100.0, i: 0.01, d: 10.0}

joint6: {p: 100.0, i: 0.01, d: 10.0}

１行目は名前空間，２行目は，任意のコントローラの名前（今回は，6dofarm_joint_controller とした），３行目にコントローラの種類．

type: "effort_controllers/JointTrajectoryController"

と書くことで，トルク制御のコントローラをjoint_trajectory_controllerの型に当てはめることができる．

その下にある，joints:も今回設定したurdf内に記載したgazeboの定義に合わせて名前を列挙していけば良い．

最後に，gains:で各関節の制御ゲインを設定しておく．

注意すべき点は，各行の先頭の位置をきちんと合わせておくこと．実行時にうまく読み込めなかったり，思ってもいないパラメータに設定されてしまわないように注意すること．

別のページからこのページにたどり着いた人は，コントローラの型をURDFファイルに書き込んだコントローラに合わせることを忘れないようにしてください．．

position_controllersを設定している人は，

type: "position_controllers/JointTrajectoryController"

velocity_controllersを設定している人は

type: "velocity_controllers/JointTrajectoryController"

Moveitとの接続は，上記の設定ファイルができればもう終わったようなものです．

（わからなかった時は，いろいろノードを立ち上げたりコントローラを変更したりとごちゃごちゃになってわかりにくかったのですが，とても簡単にできることが後でわかりました．．．）

~~前ページのmoveitを扱ったフォルダ内のlaunchフォルダにあるdemo.launchファイルを修正して，gazeboに接続できるようにします．~~

demo.launchを流用していたらはまったので，作り直します．．．

$ cd ~/catkin_ws/src/6dofarm_moveit_config/launch

$ gedit 6dofarm_moveit.launch

gazeboのlaunchファイルをいちいち別窓で立ち上げるのが面倒なので，moveitを立ち上げるlaunchファイルの先頭に6dofarm_gazebo/launch/6dofarm.launchをインクルードしてしまいます．

それ以外はほとんどdemo.launchと同じ内容です．

6dofarm_moveit.launch

</include>

</include>

</include>

</launch>

move_group.launchは，moveitのツールでノードを作った時にlaunchフォルダに自動的に生成されるlaunchファイルです．

（2016年6月17日更新）ここで，次の6dofarm_moveit_controller.launch.xmlを修正する前に，

move_group.launchの修正をしておく．修正はリンク先の記載で確認．

move_group.launchの中身を見ると，"trajectory_execution.launch.xml" の記載があり，

trajectory_execution.launch.xmlの最後にincludeの指定がされている

"6dofarm_moveit_controller_manager.launch.xml"は，launchフォルダにあるにもかかわらず中身が空っぽです．

下記のように，6dofarm_moveit_controller.launch.xmlを記載します．(ここの文言誤植修正2017/07/20）

$ cd ~/catkin_ws/src/6dofarm_moveit_config/launch

$ gedit 6dofarm_moveit_controller_manager.launch.xml (ここの誤植修正2017/07/20）

6dofarm_moveit_controller_manager.launch.xml

<!--

-->

</launch>

さらに，この中で指定している"controllers.yaml"もconfigフォルダにないのでファイルを作成する．

$ cd ~/catkin_ws/src/6dofarm_moveit_config/config

$ gedit controllers.yaml

controllers.yaml

controller_manager_ns: controller_manager

controller_list:

- name: 6dofarm/6dofarm_joint_controller

action_ns: follow_joint_trajectory

type: FollowJointTrajectory

default: true

joints:

- joint1

- joint2

- joint3

- joint4

- joint5

- joint6

上記まで設定すれば動作確認をする準備ができました．

早速下記のコマンドで実行してみる．

$ roslaunch 6dofarm_moveit_config 6dofarm_moveit.launch

このコマンド一つで，gazeboの立ち上げとmoveitを選択した状態のrvizの画面を立ち上げてくれる．

下記に，立ち上げ後のgazeboとrvizの画面を示す．

rvizの画面（これまでの画像と同じはず．アップした画面が異なっているのは．．．このページを書いている時に違う環境でやっているため画面がスクリーンに入らなかったためです．）

gazeboの画面（こちらも同様．同じ環境でやっている人は同じ画面が出るはず）

次に，rviz上のMoveitの動作プランが実行ボタンを押すことでgazebo上のロボットへ送信されることを確認してみる．

目標位置を設定した状態の例

Moveitの説明ですでに解説した通り，移動先の場所をて先のボールをドラッグして指定するか，「Select Goal State」でランダムに設定するなどして目標位置を決定する．

目標位置への移動プランが計算された状態

目標位置までの動作計画をしてもらうために「Plan」ボタンを押してMoveitに計画してもらう．

次に，計画された軌道をgazebo上のロボットへ指示するために，「Execute」ボタンを押す．

Gazeboの画面を確認すると，Rviz上で表示されたマニピュレータの計画軌道通り目的位置まで動作する様子が確認出来る．

Gazebo上の画面：目標位置に到達していることが確認できる

これでうまくいったことになるのだが，Rvizの画面を確認すると，「Execute」ボタンを押した後も，現在位置？を示していると考えられる「Scene Robot」の位置が更新されていない．

->(2016年6月17日更新部分を修正するとScene Robotも動くようになります！）

Scene Robotの「Show Robot Visual」のチェックを外すと消すことができるが．．．

では，Gazebo上のロボットの現在位置はRviz上に反映されていないのか？というと，「Add」ボタンを押して「Robot Model」を追加すると，Gazebo上のロボットの現在位置がRviz上で確認できることがわかる．

Robot StateによりRobotModelのTFが更新されているために情報を確認できる．

RobotModelを追加

RobotModelがGazebo上の姿勢と一致していることが確認できる

それでは，Scene Robotの状態を変更するにはどうすれば良いか？というと，Joint State PublisherのGUIウィンドウを使うと修正することができる．

Joint State Publisherの情報はScene Robotに反映される．

ということで，joint state publisherのguiで関節角を指定すると，Robot Stateを変更できます．

~~どうしてもnode立ち上げのパラメータだけで，Scene Robot に反映させる設定がわかりませんでした．これでは使いにくいので見逃しているだけだと思います．~~

~~誰か知っていたら教えてください！~~．（下の2016年6月17日更新で解決）

自分でnode書いてrobot stateで受け取った値をjoint stateに渡すならすぐできると思いますが．．．

2016年6月17日更新

上記までの説明で，変更していなかったmove_group.launchを修正することで

joint_statesの情報が反映されるようになります．

参考ページはここ．

moveitのソース自体(move_group.cpp)がデフォルトでprivate変数でjoint_statesが割り当てられているらしく，

変更できるようなものになっていないのが原因．．．

それを回避するためにわざわざプラグインを作っている人がいるくらい．

直して欲しいものです．．．

変更箇所は下記の通り．

move_group.launchの中にある

のargs="$(arg command_args)"の部分を

args="joint_states:=/6dofarm/joint_states $(arg command_args)"

と記述を追加するだけ．

下記の場所にあるのでエディタで修正しましょう！

$ cd ~/catkin_ws/src/6dofarm_moveit_config/launch

$ gedit move_group.launch

上記の記述例は，6dofarmの名前空間に合わせていますので，自分が使っているノードの名前空間に置き換えて利用してください．

move_groupのノードに/6dofarm/joint_statesの情報が繋がるようになります．

デフォルトでは/joint_statesのみを受け付けるようになっている．

他の方法としては，プラグインを自分で書いたり，

robot_statesを無理やりjoint_statesに変更したりする方法もありますが，

下記のようにトピックの名前の修正をコマンドラインで実行する方法でも実行できます．

$ rostopic echo /6dofarm/joint_states | rostopic pub /joint_states sensor_msgs/JointState

move_group.launchの中身を変えていけばこんなコマンドは実行する必要はありません．

何か同じような名前空間の問題で困った時に試しに使ってみる程度か？

なお，上記のmove_group.launchの修正を行った場合のrqt_graphの表示は下記の通りとなる．

/6dofarm/joint_statesがmove_groupに接続されていることが確認できる．

/6dofarm/joint_statesがmove_groupに接続されると

Scene Robotの表示が更新されるのだが，

Excuteボタンで実行した移動先から

次の目標位置へロボットアームの軌道計画を行う場合は，

Start Stateを現在位置に更新する必要がある．

更新方法は簡単で，下記画像にあるように

「Planning」タブの中の「Query」の「Select Start State」をクリックして

<current>の選択をした後に「Update」ボタンを押すだけで良い．

これでGazebo上の現在位置から次の目標位置へ軌道計画が行える状態になった．

rvizで確認してjoint_statesがmove_groupに接続されていても，SceneRobotに姿勢が反映されない時は，

rvizの左メニューにあるMotionPlanningのPlanning Scene Topicが

launchファイルの設定次第でデフォルトの値が

/planning_sceneとなっている場合があるので

/move_group/monitored_planning_sceneに変更すると，

gazeboの姿勢が反映されるようになるはずです．

1.5 今後の予定

自分でノードを作って，moveitの関数を使ってアームに指令を行うための方法を解説予定．次は．．．障害物を置いて障害物を回避する動作など．．．これはおそらく，ノードを作る時に障害物を指定できるのでそこで試せると考えています．あくまで予定．

Page updated

Google Sites

Report abuse

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

1. Gazebo上でのマニピュレータの動力学シミュレーション

１．１ 準備

通常は，下記のように引数をつけて実行する．

$ roslauch gazebo_ros empty_world.launch paused:=true use_sim_time:=false gui:=true throttled:=false headless:=false debug:=true

各引数については，下記のリンクを参考にすること．

gazeboとRosをつなぐ（ros launch 編）：（九州工業大学のブログ）

リンク構造のロボットのデモンストレーションは下記のコマンドで試すことができる．

1.3 URDFのロボットのGazeboパラメータ設定

1.3.1 world座標への固定

1.3.2 gazeboの色付けと関節角トルクの設定

1.3.3 ros_controlを使った関節角の制御

1.3.3 rqtとdynamic reconfigureを使ったPID制御ゲインの調整

1.4 Moveit!とGazeboの接続

1.5 今後の予定

１．１準備