Zenn
🎣

MoveIt2: 複数グループのインタラクティブマーカを表示させる

2022/10/08に公開
ROS2
ロボット
Gazebo
tech

海洋ロボコンをやっている人です。
今回はMoveitで複数グループのインタラクティブマーカを表示、動作させる方法を備忘録としてまとめます。
イメージ↓↓↓

経緯は

  • twitterで以下を発見 > 複数のmove_groupを使って遊びたい > 備忘録としてまとめとくか

という感じです。

さらっと読めると思うので、双腕ロボットをMoveitで作る方などのお力添えになれば幸いです。


https://www.youtube.com/watch?v=0DxQdT6U4pM

ROS2-Moveit2で複数グループのインタラクティブマーカを表示させる

使用するモデルは、アイコンでも表示されているマンタ型ロボットで紹介していきます。
なお、ロボットのハード設計からソフト設計まで全て私が担当しており、所属研究室の教授からも承諾を得ています。

複数のmove_groupをもつようプログラムする

結論から述べると、

  1. 複数のmove_groupを持つグループを作成
  2. end_effectorタグを正しく設定（重要)

することで、SRDFファイルとしては以下を追加する形です。

manta_v2.srdf
    <group name="Manta">
        <group name="Rtop" />
        <group name="Rmid" />        
        <group name="Rbtm" />
        <group name="Ltop" />
        <group name="Lmid" />        
        <group name="Lbtm" />    
    </group>

    <group_state group="Manta" name="Init">
        <joint name="rev_link_Rtop_roll1" value="0"/>
        <joint name="rev_link_Rtop_pitch" value="0"/>
        <joint name="rev_link_Rtop_roll2" value="0"/>
        <joint name="rev_link_Rmid_roll1" value="0"/>
        <joint name="rev_link_Rmid_pitch" value="0"/>
        <joint name="rev_link_Rmid_roll2" value="0"/>
        <joint name="rev_link_Rbtm_roll1" value="0"/>
        <joint name="rev_link_Rbtm_pitch" value="0"/>
        <joint name="rev_link_Rbtm_roll2" value="0"/>
        <joint name="rev_link_Ltop_roll1" value="0"/>
        <joint name="rev_link_Ltop_pitch" value="0"/>
        <joint name="rev_link_Ltop_roll2" value="0"/>
        <joint name="rev_link_Lmid_roll1" value="0"/>
        <joint name="rev_link_Lmid_pitch" value="0"/>
        <joint name="rev_link_Lmid_roll2" value="0"/>
        <joint name="rev_link_Lbtm_roll1" value="0"/>
        <joint name="rev_link_Lbtm_pitch" value="0"/>
        <joint name="rev_link_Lbtm_roll2" value="0"/>
    </group_state>

    <group_state group="Manta" name="Up">
        <joint name="rev_link_Rtop_roll1" value="-0.52"/>
        <joint name="rev_link_Rtop_pitch" value="0.0"/>
        <joint name="rev_link_Rtop_roll2" value="0.70"/>
        <joint name="rev_link_Rmid_roll1" value="-0.52"/>
        <joint name="rev_link_Rmid_pitch" value="0.0"/>
        <joint name="rev_link_Rmid_roll2" value="0.70"/>
        <joint name="rev_link_Rbtm_roll1" value="-0.52"/>
        <joint name="rev_link_Rbtm_pitch" value="0.0"/>
        <joint name="rev_link_Rbtm_roll2" value="0.70"/>
        <joint name="rev_link_Ltop_roll1" value="0.52"/>
        <joint name="rev_link_Ltop_pitch" value="-0.0"/>
        <joint name="rev_link_Ltop_roll2" value="-0.70"/>
        <joint name="rev_link_Lmid_roll1" value="0.52"/>
        <joint name="rev_link_Lmid_pitch" value="-0.0"/>
        <joint name="rev_link_Lmid_roll2" value="-0.70"/>
        <joint name="rev_link_Lbtm_roll1" value="0.52"/>
        <joint name="rev_link_Lbtm_pitch" value="-0.0"/>
        <joint name="rev_link_Lbtm_roll2" value="-0.70"/>
    </group_state>

    <end_effector name="rtop_eef" parent_link="link_Rtop_end_1" group="Rtop" />
    <end_effector name="rmid_eef" parent_link="link_Rmid_end_1" group="Rmid" />
    <end_effector name="rbtm_eef" parent_link="link_Rbtm_end_1" group="Rbtm" />
    <end_effector name="ltop_eef" parent_link="link_Ltop_end_1" group="Ltop" />
    <end_effector name="lmid_eef" parent_link="link_Lmid_end_1" group="Lmid" />
    <end_effector name="lbtm_eef" parent_link="link_Lbtm_end_1" group="Lbtm" />

自分のロボットには、エンドエフェクタ用のリンクはURDFに設定してあるものの、エンドエフェクタにハンドなどがついてないので、SRDF内にend_effectorの記述をしてませんでした。

ただ、これだと以下の記事からもわかるように、複数のインタラクティブマーカが表示されない要因となっているようです。

dual arm configuration problems

そのため、必ずエンドエフェクタの設定を追加するようにしてください。


あとは、追加したmove_groupをros2_controlで動かせれるようにxxxxx_ros_controllers.yamlも追記します。

manta_v2_ros_controllers.yaml
controller_manager:
  ros__parameters:
    update_rate: 100  # Hz

+    manta_controller:
+      type: joint_trajectory_controller/JointTrajectoryController

    rtop_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

+manta_controller:
+  ros__parameters:
+    joints:
+      - rev_link_Rtop_roll1
+      - rev_link_Rtop_pitch
+      - rev_link_Rtop_roll2
+      - rev_link_Rmid_roll1
+      - rev_link_Rmid_pitch
+      - rev_link_Rmid_roll2
+      - rev_link_Rbtm_roll1
+      - rev_link_Rbtm_pitch
+      - rev_link_Rbtm_roll2
+      - rev_link_Ltop_roll1
+      - rev_link_Ltop_pitch
+      - rev_link_Ltop_roll2
+      - rev_link_Lmid_roll1
+      - rev_link_Lmid_pitch
+      - rev_link_Lmid_roll2
+      - rev_link_Lbtm_roll1
+      - rev_link_Lbtm_pitch
+      - rev_link_Lbtm_roll2
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity

rtop_controller:
  ros__parameters:
    joints:
      - rev_link_Rtop_roll1
      - rev_link_Rtop_pitch
      - rev_link_Rtop_roll2
    command_interfaces:
      - position
    state_interfaces:
      - position
      - velocity


ローンチファイルにもcontroller managerでスポーンするコントローラーを追加します。

moveit_demo.launch.py
    load_controllers = []
    for controller in [
        "rtop_controller",
        "rmid_controller",
        "rbtm_controller",
        "ltop_controller",
        "lmid_controller",
        "lbtm_controller",
+        "manta_controller",
        "joint_state_broadcaster",
    ]:
        load_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner.py {}".format(controller)],
                shell=True,
                output="screen",
            )
        ]

Rviz2, Gazeboで動作確認する

buildを通したら、gazebo_ros2_controlのlaunchとMoveitのlaunchを実行し、複数のインタラクティブマーカで遊べるか確認してみましょう。

First Shell

ros2 launch manta_v2_gazebo_ros2_control move_group_controller.launch.py

Second Shell

ros2 launch manta_v2_moveit_config moveit_demo.launch.py


複数のマーカを動かして、moveitでプランニングできていればOKですね。

以上。

Discussion