試行錯誤な日々: 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を起動し直すと、赤くなっていないモデルが表示されると思います。