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

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

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

１．１ 準備

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

$ gazebo

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

Error [Node.cc:90] No namespace found

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

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

[Err] [REST.cc:205] Error in REST request

このエラーが出ているときは，下記の場所のファイル内のアドレスを修正．

~/.ignition/fuel/config.yaml

この中の，「url: https://api.ignitionfuel.org」を

「url: https://api.ignitionrobotics.org」に修正する．

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

$ sudo apt-get install gazebo*

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

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

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

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

$ roslaunch gazebo_ros empty_world.launch

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

$ roslaunch gazebo_ros empty_world.launch paused:=true use_sim_time:=false gui:=true throttled:=false recording:=false debug:=true verbose:=true gui_required:=true

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

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/imelodic/share/gazebo_ros/launch

mud_world.launchの内容は下記．

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

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

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

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

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

$ ls | grep mud.world

mud.world

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

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

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

$ cd ~/.gazebo/models

$ ls

double_pendulum_with_base （サンプルを実行していた場合にdouble_pendulum_with_baseのディレクトリが見えるはず．中にモデルファイルがある．）

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

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

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

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

構成例

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

$ cd ~/catkin_ws/src

$ catkin_create_pkg sixdofarm_gazebo

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

$ cd ~/catkin_ws/src/sixdofarm_gazebo

$ mkdir launch

$ cd launch

$ gedit sixdofarm.launch

sixdofarm.launchの中身は下記．

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

$ cd ~/catkin_ws/src/sixdofarm_gazebo

$ mkdir worlds

$ cd worlds

$ gedit sixdofarm.world

sixdofarm.worldの中身は下記．

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

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

gas_stationは確認のためだけなので，必要がない人は削除する．（こっちのほうが軽いので推奨）

gas_stationを削除したファイルは下記．

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

(gas_stationを削除したほうを利用している人は，床面だけ表示される）

$ roslaunch sixdofarm_gazebo sixdofarm.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は，前ページでsixdofarmというパッケージを作っていたのであればそのまま利用できる．

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

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

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

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

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

(2021年7月24日修正：joint_state_publisherのsouce_listを/sixdofarm/joint_statesに指定するように修正）

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

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

$ roslaunch sixdofarm_gazebo sixdofarm.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と固定してあげれば良い．

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

<link name="world"/>

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

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

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

</joint>

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

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

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

$ roslaunch sixdofarm_gazebo sixdofarm.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>hardware_interface/EffortJointInterface</hardwareInterface> gazebo上のハードウェアとしてトルク(Effort)発生機を指定

</joint>

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

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

<mechanicalReduction>1</mechanicalReduction> 減速比

</actuator>

</transmission>

<gazebo>

<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">

<robotNamespace>/sixdofarm</robotNamespace>

</plugin>

</gazebo>

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

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

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

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

<gazebo>

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

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

</plugin>

</gazebo>

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

sixdofarm.urdf 修正２

<?xml version="1.0" ?>

<robot name="sixdofarm">

<material name="red">

<color rgba="1.0 0.0 0.0 1.0"/>

</material>

<material name="green">

<color rgba="0.0 1.0 0.0 1.0"/>

</material>

<material name="blue">

<color rgba="0.0 0.0 1.0 1.0"/>

</material>

<link name="world"/>

<joint name="world2base" type="fixed">

<parent link="world"/>

<child link="base"/>

</joint>

<link name="base"/>

<joint name="fixed" type="fixed">

<parent link="base"/>

<child link="link1"/>

</joint>

<!-- Link 1 -->

<link name="link1">

<collision>

<origin rpy="0 0 0" xyz="0 0 0.075"/>

<geometry>

<box size="0.05 0.05 0.15"/>

</geometry>

</collision>

<visual>

<origin rpy="0 0 0" xyz="0 0 0.075"/>

<geometry>

<box size="0.05 0.05 0.15"/>

</geometry>

<material name="red"/>

</visual>

<inertial>

<origin rpy="0 0 0" xyz="0 0 0.075"/>

<mass value="1"/>

<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>

</inertial>

</link>

<joint name="joint1" type="revolute">

<parent link="link1"/>

<child link="link2"/>

<origin rpy="0 0 0" xyz="0 0 0.15"/>

<axis xyz="0 0 1"/>

<limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/>

</joint>

<gazebo reference="link1">

<material>Gazebo/Red</material>

</gazebo>

<transmission name="trans1">

<type>transmission_interface/SimpleTransmission</type>

<joint name="joint1">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

</joint>

<actuator name="motor1">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

<mechanicalReduction>1</mechanicalReduction>

</actuator>

</transmission>

<!-- Link 2 -->

<link name="link2">

<collision>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<geometry>

<box size="0.05 0.1 0.05"/>

</geometry>

</collision>

<visual>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<geometry>

<box size="0.05 0.1 0.05"/>

</geometry>

<material name="green"/>

</visual>

<inertial>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<mass value="1"/>

<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>

</inertial>

</link>

<joint name="joint2" type="revolute">

<parent link="link2"/>

<child link="link3"/>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<axis xyz="0 1 0"/>

<limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/>

</joint>

<gazebo reference="link2">

<material>Gazebo/Green</material>

</gazebo>

<transmission name="trans1">

<type>transmission_interface/SimpleTransmission</type>

<joint name="joint2">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

</joint>

<actuator name="motor2">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

<mechanicalReduction>1</mechanicalReduction>

</actuator>

</transmission>

<!-- Link 3 -->

<link name="link3">

<collision>

<origin rpy="0 0 0" xyz="0 0 0.0525"/>

<geometry>

<box size="0.05 0.05 0.105"/>

</geometry>

</collision>

<visual>

<origin rpy="0 0 0" xyz="0 0 0.0525"/>

<geometry>

<box size="0.05 0.05 0.105"/>

</geometry>

<material name="blue"/>

</visual>

<inertial>

<origin rpy="0 0 0" xyz="0 0 0.0525"/>

<mass value="1"/>

<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>

</inertial>

</link>

<joint name="joint3" type="revolute">

<parent link="link3"/>

<child link="link4"/>

<origin rpy="0 0 0" xyz="0 0 0.105"/>

<axis xyz="0 0 1"/>

<limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/>

</joint>

<gazebo reference="link3">

<material>Gazebo/Blue</material>

</gazebo>

<transmission name="trans3">

<type>transmission_interface/SimpleTransmission</type>

<joint name="joint3">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

</joint>

<actuator name="motor3">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

<mechanicalReduction>1</mechanicalReduction>

</actuator>

</transmission>

<!-- Link 4 -->

<link name="link4">

<collision>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<geometry>

<box size="0.05 0.1 0.05"/>

</geometry>

</collision>

<visual>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<geometry>

<box size="0.05 0.1 0.05"/>

</geometry>

<material name="red"/>

</visual>

<inertial>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<mass value="1"/>

<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>

</inertial>

</link>

<joint name="joint4" type="revolute">

<parent link="link4"/>

<child link="link5"/>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<axis xyz="0 1 0"/>

<limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/>

</joint>

<gazebo reference="link4">

<material>Gazebo/Red</material>

</gazebo>

<transmission name="trans4">

<type>transmission_interface/SimpleTransmission</type>

<joint name="joint4">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

</joint>

<actuator name="motor4">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

<mechanicalReduction>1</mechanicalReduction>

</actuator>

</transmission>

<!-- Link 5 -->

<link name="link5">

<collision>

<origin rpy="0 0 0" xyz="0 0 0.0525"/>

<geometry>

<box size="0.05 0.05 0.105"/>

</geometry>

</collision>

<visual>

<origin rpy="0 0 0" xyz="0 0 0.0525"/>

<geometry>

<box size="0.05 0.05 0.105"/>

</geometry>

<material name="green"/>

</visual>

<inertial>

<origin rpy="0 0 0" xyz="0 0 0.0525"/>

<mass value="1"/>

<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>

</inertial>

</link>

<joint name="joint5" type="revolute">

<parent link="link5"/>

<child link="link6"/>

<origin rpy="0 0 0" xyz="0 0 0.105"/>

<axis xyz="0 1 0"/>

<limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/>

</joint>

<gazebo reference="link5">

<material>Gazebo/Green</material>

</gazebo>

<transmission name="trans5">

<type>transmission_interface/SimpleTransmission</type>

<joint name="joint5">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

</joint>

<actuator name="motor5">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

<mechanicalReduction>1</mechanicalReduction>

</actuator>

</transmission>

<!-- Link 6 -->

<link name="link6">

<collision>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<geometry>

<box size="0.05 0.1 0.05"/>

</geometry>

</collision>

<visual>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<geometry>

<box size="0.05 0.1 0.05"/>

</geometry>

<material name="blue"/>

</visual>

<inertial>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<mass value="1"/>

<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>

</inertial>

</link>

<joint name="joint6" type="revolute">

<parent link="link6"/>

<child link="link7"/>

<origin rpy="0 0 0" xyz="0 0 0.0"/>

<axis xyz="0 0 1"/>

<limit effort="30" lower="-2.617" upper="2.617" velocity="1.571"/>

</joint>

<gazebo reference="link6">

<material>Gazebo/Blue</material>

</gazebo>

<transmission name="trans6">

<type>transmission_interface/SimpleTransmission</type>

<joint name="joint6">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

</joint>

<actuator name="motor6">

<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>

<mechanicalReduction>1</mechanicalReduction>

</actuator>

</transmission>

<!-- Link 7 (end effector)-->

<link name="link7">

<collision>

<origin rpy="0 0 0" xyz="0 0 0.05"/>

<geometry>

<box size="0.05 0.05 0.1"/>

</geometry>

</collision>

<visual>

<origin rpy="0 0 0" xyz="0 0 0.05"/>

<geometry>

<box size="0.05 0.05 0.1"/>

</geometry>

<material name="red"/>

</visual>

<inertial>

<origin rpy="0 0 0" xyz="0 0 0.05"/>

<mass value="1"/>

<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>

</inertial>

</link>

<gazebo reference="link7">

<material>Gazebo/Red</material>

</gazebo>

<gazebo>

<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">

<robotNamespace>/sixdofarm</robotNamespace>

</plugin>

</gazebo>

</robot>

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

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

$ roslaunch sixdofarm_gazebo sixdofarm.launch

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

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

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

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

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

$ cd ~/catkin_ws/src/

$ catkin_create_pkg sixdofarm_control

$ cd sixdofarm_control

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

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

$ cd config

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

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

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

sixdofarm_control.yaml

sixdofarm:

# 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/sixdofarm_control/launch

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

sixdofarm_control.launchの内容は下記の通り．

sixdofarm_control.launch

<launch>

<!-- Load joint controller configurations from YAML file to parameter server -->

<rosparam file="$(find sixdofarm_control)/config/sixdofarm_control.yaml" command="load"/>

<!-- load the controllers -->

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

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

<!-- convert joint states to TF transforms for rviz, etc -->

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

respawn="false" output="screen">

<remap from="/joint_states" to="/sixdofarm/joint_states" />

</node>

</launch>

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

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

$ roslaunch sixdofarm_gazebo sixdofarm.launch 端末１つ目で立ち上げる

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

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

$ rostopic pub -1 /sixdofarm/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 sixdofarm_gazebo sixdofarm.launch 端末１つ目で立ち上げる

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

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

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

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

Message Publisherの追加

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

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

関節１の場合は「/sixdofarm/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」で追加．

追加後の画面は下図．

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

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

・/sixdofarm/joint1_position_controller/command/data （指令値）

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

・/sixdofarm/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/sixdofarm_moveit_config/config

$ gedit trajectory_control.yaml

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

trajectory_control.yaml

sixdofarm:

joint_state_controller:

type: "joint_state_controller/JointStateController"

publish_rate: 50

sixdofarm_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}

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

type: "effort_controllers/JointTrajectoryController"

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

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

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

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

position_controllersを設定している人は，

type: "position_controllers/JointTrajectoryController"

velocity_controllersを設定している人は

type: "velocity_controllers/JointTrajectoryController"

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

launchファイルを最後に作ります．

$ cd ~/catkin_ws/src/sixdofarm_moveit_config/launch

$ gedit sixdofarm_moveit.launch

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

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

sixdofarm_moveit.launch

<launch>

<!-- gazeboo -->

<include file="$(find sixdofarm_gazebo)/launch/sixdofarm.launch" />

<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->

<include file="$(find sixdofarm_moveit_config)/launch/planning_context.launch">

<arg name="load_robot_description" value="true"/>

</include>

<!-- controller -->

<rosparam file="$(find sixdofarm_moveit_config)/config/trajectory_control.yaml" command="load"/>

<node name="sixdofarm_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/sixdofarm" args="sixdofarm_joint_controller joint_state_controller"/>

<!-- MoveIt -->

<include file="$(find sixdofarm_moveit_config)/launch/move_group.launch">

<arg name="publish_monitored_planning_scene" value="true" />

<arg name="allow_trajectory_execution" value="true"/>

<arg name="fake_execution" value="false"/>

<arg name="info" value="true"/>

<arg name="debug" value="false"/>

</include>

<!-- Rviz and load the default config to see the state of the move_group node -->

<include file="$(find sixdofarm_moveit_config)/launch/moveit_rviz.launch">

<arg name="rviz_config" value="$(find sixdofarm_moveit_config)/launch/moveit.rviz"/>

<arg name="debug" value="false"/>

</include>

</launch>

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

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

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

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

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

$ cd ~/catkin_ws/src/sixdofarm_moveit_config/launch

$ gedit sixdofarm_moveit_controller_manager.launch.xml

sixdofarm_moveit_controller_manager.launch.xml

<launch>

<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->

<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />

<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

<!--

<arg name="controller_manager_name" default="moveit_simple_controller_manager" />

<param name="controller_manager_name" value="$(arg controller_manager_name)" />

-->

<arg name="use_controller_manager" default="true" />

<param name="use_controller_manager" value="$(arg use_controller_manager)" />

<!-- load controller_list -->

<rosparam file="$(find sixdofarm_moveit_config)/config/controllers.yaml"/>

</launch>

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

$ cd ~/catkin_ws/src/sixdofarm_moveit_config/config

$ gedit controllers.yaml

controllers.yaml

controller_manager_ns: controller_manager

controller_list:

- name: sixdofarm/sixdofarm_joint_controller

action_ns: follow_joint_trajectory

type: FollowJointTrajectory

default: true

joints:

- joint1

- joint2

- joint3

- joint4

- joint5

- joint6

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

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

$ roslaunch sixdofarm_moveit_config sixdofarm_moveit.launch

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

（2021年7月24日追記）

上記のコマンドで立ち上がらなかったときは，下記を確認．

RLException: unused args [execution_type] for include of [/home/melodic/catkin_ws/src/sixdofarm_moveit_config/launch/sixdofarm_moveit_controller_manager.launch.xml]

The traceback for the exception was written to the log file

というエラーだったら，sixdofarm_moveit_controller_manager.launch.xmlとmove_group.launchにある

「execution_type」が記載されている行を削除．

sixdofarm_moveit_controller_manager.launch.xmlは下記のようになっていたので（画面をはみ出ている部分はコピペで確認してください．

<include file="$(find sixdofarm_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml">

<arg name="execution_type" value="$(arg execution_type)" />

</include>

下記のように修正（行末が見えないですが，includeのタグを閉じるように修正してください）

<include file="$(find sixdofarm_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml"/>

move_groupは2行あったのでそこを削除

<!-- move_group settings -->と書いてあるところの4行目にある下記の設定を削除

<arg name="execution_type" default="interpolate"/> <!-- set to 'last point' to skip intermediate trajectory in fake execution -->

<!-- Trajectory Execution Functionality -->と書いているところの5行したにある下記の設定を削除

<arg name="execution_type" value="$(arg execution_type)" />

あとは，sixdofarm_moveit_controller_manager.launch.xmlが自動生成を最近してros_controllers.yamlにcontrollerを割り当てている時があるので，勝手に修正されていないかチェック．

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

rvizの画面

gazeboの画面

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

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

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

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

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

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

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

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

2021年7月24日時点の修正では，rqt_graphでNodeの接続を確認すると/joint_state_publishierが/sixdofarm/joint_stateを/joint_stateに変換してから/move_groupがサブスクライブしている．．．以前はこんなことはなかったのだが．．．修正方法を知っていれば教えて下さい．．．

と思ってっていたのだが，以前のkineticで問題があったのと同じ方法にすれば直りました．melodicではこの問題は解決していたはずなのですが，再発しています．．．なぜか．

やり方は，

move_group.launchの下記の行を

<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">

下記のとおり修正するだけ，

<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="joint_states:=/sixdofarm/joint_states $(arg command_args)">

そうすると，/sixdofarm/joint_statesから直接move_groupがjoint_statesの値を読み取ります．

下記が修正後のrqt_graphで確認した例．

1.5 今後の予定

障害物を置いて障害物を回避する動作を解説する．