2019年2月3日日曜日

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


背景

ROSとはロボットを動かすのに便利なフレームワークです。
uArmSwiftProとは、UFACTORYが作ったロボットアームです。

前回uArmSwiftProをMoveit!で動かしてみました。
その際y軸方向に動かせない問題があると分かったので、解決に取り組んでみました。
ロボットはまだ動かせていませんが、Moveit!上のモデルは動かせるようになったので、備忘録を兼ねて取り組んだ内容を共有します。

使ったもの

ROS + Moveit!でuArmSwiftProを動かせる環境

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

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

uArmSwiftPro + グリッパ

グリッパを付けたuArmSwiftProです。
これ以後SwiftProと呼びます。


グリッパを回転できるモデルをxacroに割り当てる

SwiftProのモデルを定義しているxacroファイルを見てみると、Gripper.STLが使われていることが分かりました。


このGripper.STLを確認してみたところ、アームの先端とグリッパが固定されたモデルになっていて、グリッパを回転できるようになっていませんでした。


Gripper.STLを分割できたら良かったのですが、FreeCADでGripper.STLからFreeCAD形式のモデルを生成しようとしても12時間以上かけても処理が終わらなかったので、自分で簡素なモデルを作って割り当てることにしました。
(Gripper.STLを上下に分割する方法をご存じでしたら、やり方を教えていただくか、分割していただけると嬉しいです。)


Link8.STLとGripperPart.STLという名前で上記のモデルを保存し、SwiftProのモデルを定義しているxacroファイルを下記のように変更しました。
(Link10とJoint10の値は、rvizでモデルを確認しながら調整しました。)
~/ws_moveit/src/RosForSwiftAndSwiftPro/swiftpro/urdf/pro_model.xacro
<link name="Link8">
    .. 
            <mesh filename="package://swiftpro/urdf/pro_links/Link8.STL"/>
    ..
</link>


<link name="Link10">
    <inertial>
        <origin xyz="0.027288 0.001085 -0.007344" rpy="0 0 0"/>
        <mass value="0.012"/>
        <inertia ixx="0.000002184" ixy="0.000000020" ixz="-0.000000207"
                 iyy="0.000002992" iyz="0.000000371" izz="0.000001832"/>
    </inertial>
    <visual>
        <geometry>
            <mesh filename="package://swiftpro/urdf/pro_links/GripperPart.STL"/>
        </geometry>
        <origin xyz = "-0.22201 0 -0.24651" rpy = "0 0 0" />
        <material name = "">
            <color rgba = "0.3 0.3 0.3 1" />
        </material>
    </visual>
</link>

<joint name="Joint10" type="revolute">
    <axis xyz="0 0 1"/>
    <limit effort = "1000.0" lower = "-3" upper = "3" velocity = "0" />
    <parent link="Link8"/>
    <child link="Link10"/>
    <origin xyz= "0.05 0 0" rpy = " 0 0 0" />
</joint>

これにより、グリッパを回転できる状態のモデルができました。


モデルを割り当ててMoveit!で動かせるように設定を変更する

xacroを書き換えたことにより回転グリッパをできそうなモデルができましたが、 rvizで表示すると変更した部分が赤くなり、Moveit!のPlan and Executeを実行してもエラーが出てしまいます。



重複を排除したエラーの内容はこちらです。
[ WARN] [1549161294.726096594]: RRTConnect: Skipping invalid start state (invalid state)
[ERROR] [1549161294.726654714]: RRTConnect: Motion planning start tree could not be initialized!
[ WARN] [1549161294.726849684]: ParallelPlan::solve(): Unable to find solution by any of the threads in 0.001220 seconds

赤くならないように、Plan and Executeでエラーがでないように、グリッパを回転できるように、設定ファイルを書き換えました。
moveit setup configを通して変更できたら良いのだと思いますが、自分の環境だと既存の設定がうまく読み込めなかったので、試行錯誤しながら設定ファイルの記述を書き換えました。
「ここはおかしい」という突っ込みがありましたら、コメントをいただけると嬉しいです。
~/ws_moveit/src/RosForSwiftAndSwiftPro/pro_moveit_config/config/fake_controllers.yaml
-      - Joint8
+      - Joint10
~/ws_moveit/src/RosForSwiftAndSwiftPro/pro_moveit_config/config/swiftpro.srdf
    <group name="arm">
+        <link name="Link10" />
+        <joint name="Joint10" />
    </group>

    <group_state name="home" group="arm"> 
+        <joint name="Joint10" value="0" />
     </group_state>

+    <disable_collisions link1="Link8" link2="Link10" reason="Adjacent" />

上記の変更を行った後、Moveit!のためにrvizを起動し直すと、赤くなっていないモデルが表示されると思います。

動作確認

Moveit!のためにSwiftProのプログラムを動かします。


y軸方向に動かします。


動かせました。
嬉しい。

PlanningタブのPlan and Executeを押して、ロボットを動かしてみます。


初期位置です。


右回転させます。
なぜか右回転と共にアーム先端の位置が下がりました。


左回転させます。
少し左を向くと停止しました。
右回転だと下がったので、左回転だと位置を上げようとして、z方向の上限に達して停止したのかもしれません。


rviz上のモデルはy軸方向に動いてくれましたが、Plan and Executeを実行すると期待しない位置にアームが動きました。
rviz上のモデルと実際のロボットの座標変換が不完全なようなので、それに関する修正が必要そうです。

まとめ

まだSwiftProを期待通りに動かせてはいないものの、rviz上でy軸方向に動かせるようになりました。

今回行った修正はgithubで公開しています。
Add Link8 and GripperPart as Link10

何かの参考になれば嬉しいです。

0 件のコメント :