2019年2月9日土曜日

ROS Moveit!でuArmSwiftProをグリッパの回転を含めて動かせるようにしてみた


背景

ROSとはロボット制御のプログラムを提供してくれるフレームワークです。
Moveit!とはROSで提供される動作制御プログラムです。
uArmSwiftProとはUFACTORYが販売しているロボットアームです。

前々回と前回の記事でuArmSwiftProをMoveit!で動かそうとしたものの、期待通りに動かず、修正を試みていました。

前々回: uArmSwiftProをROS MelodicのMoveit!で動かしてみた
前回: ROS rviz上のMoveit!向けuArmSwiftProをy軸方向に移動できるようにしてみた

試行錯誤の末ようやくグリッパの回転を含めてMoveit!で制御できるようになったので、備忘録を兼ねてやったことを共有します。

使ったもの

Moveit!が動く状態のROS

環境構築方法は前々回の記事を参考にしてください。

uArmSwiftProをROS MelodicのMoveit!で動かしてみた

前回の記事の修正を行ったuArmSwiftPro向けのファイル

修正内容は前回の記事を参考にしてください。

ROS rviz上のMoveit!向けuArmSwiftProをy軸方向に移動できるようにしてみた

uArmSwiftPro

グリッパ付きのものを使いました。


Arduino IDE

シリアル通信の内容確認に利用しました。

Arduino - Software

グリッパの角度をrvizに表示

Moveit!をグリッパに対応させる前に、swiftproのdisplay.launchでグリッパの角度を表示できるようにします。

uArmSwiftProからシリアル通信で送られてくる情報を見てみます。
Arduino IDEで/dev/ttyACM0を115200bpsで開きます。


/swiftpro/src/swiftpro_read_node.cppのmain文に書いてあるように、「M2019\r\n」と「M2120 V0.05\r\n」を入力すると情報を発信し始めます。


アームを動かして値の変化を見てみると、グリッパの角度が4番目の値として送られていることが分かりました。


この4番目の角度はswiftpro/src/swiftpro_read_node.cppで既にSwiftproState_topicに渡されていたため、その情報をJoint10に反映する処理をswiftpro/src/swiftpro_rviz_node.cppに追加しました。
変更内容はgithubのdiffを見てください。

Handle Joint10 with using motor_angle4 on swiftpro_rviz_node

displayの動作確認

下記のようなコマンドを実行して、プログラムのリビルド、再読込、displayの動作確認を行います。
cd ~/ws_moveit/
catkin build
source devel/setup.bash
roslaunch swiftpro pro_display.launch

ロボットの現在の位置情報が、グリッパの回転角も含めて、大体同じ位置にrviz上に表示されるようになりました。





Moveit!でグリッパの角度を制御

具体的な変更点はgithubのdiffをご覧ください。

Support angle4th on Moveit! for swiftpro

変更内容を説明します。
pro_moveit_config/config/joint_limits.yaml
pro_moveit_config/launch/moveit.rviz
Moveit!でJoint10の値を扱えるようにしました。
(見様見真似で変更した内容なので、別の良い書き方があるかもしれません。)

swiftpro/src/swiftpro_moveit_node.cpp
angle4thという書き込み用にtopicが定義されていたので、それに対してrvizで動かしたモデルのアームの角度を送信するようにしました。
また、Moveit!のJointStateで送られてくる角度が期待通りの順番になっていなかったので、nameで管理されている関節名を見ながら、期待する値を処理できるようにしました。
この順番が違うために、前回期待通りに動かなかったようです。

swiftpro/src/swiftpro_write_node.cpp
書き込み用のtopicと分かるように、それに関する関数とtopicを「write_」で始まる名前に変更しました。

Moveit!の動作確認

下記のようなコマンドを実行して、プログラムのリビルド、再読込、Moveit!での動作確認を行います。
(ターミナルを2つ使います。)
cd ~/ws_moveit
catkin build
source devel/setup.bash
roslaunch swiftpro pro_control.launch
cd ~/ws_moveit
catkin build # 別のターミナルでcatkin buildを実行済みだったら必要ないです。
source devel/setup.bash
roslaunch pro_moveit_config demo.launch

グリッパの角度は維持したまま、アームの位置を右回転下方向に移動させてみます。


移動前。


移動後。


アームの先端はぎこちない動きをするものの、Moveit!で指定した大体の位置に移動してくれました。

今度はy軸方向だけ変化させ、アームを左に振らせてみます。


移動前


移動後


ぎこちないながらも大体指定した位置に移動してくれました。
前回は左に振らせようとするとフリーズしていたので、今回の修正によってその不具合も直せたようです。

グリッパの角度を制御するサーボは根本に付いているサーボより性能が劣るためか、あまり細かい動きができないようで、ぎこちない動きをしていました。
また、個体差なのか、値変換時の定数が良くないのか、グリッパは本体と平湖を維持するように支持しているものの、アームを左右に降ると若干角度がずれてしまうようでした。

上記の変更を行ったファイルを使う

下記のようなコマンドで本家のファイルをこの記事の変更が適用されたファイルに置き換えれば、前回と今回の修正を適用した状態でuArmSwiftProを制御できます。
cd ~/ws_moveit/src/
sudo rm -r RosForSwiftAndSwiftPro
git clone https://github.com/asukiaaa/RosForSwiftAndSwiftPro.git
cd ../
catkin build
source ~/.bashrc

本家のリポジトリに対してプルリクエストはしていますが、記事を書いている時点ではまだ受理されていません。
それが受理されるまでは、アームを回転させたい場合、この記事の内容を反映するか上記のような操作で変更が適用されたポジトリを利用するのが良いと思います。

まとめ

グリッパのサーボの動きがぎこちなく、誤差は残るものの、Moveit!でuArmSwiftProグリッパの角度を制御できるようになりました。

この記事が何かの参考になれば嬉しいです。

0 件のコメント :