🎣
MoveIt2: 複数グループのインタラクティブマーカを表示させる
海洋ロボコンをやっている人です。
今回はMoveitで複数グループのインタラクティブマーカを表示、動作させる方法を備忘録としてまとめます。
イメージ↓↓↓
経緯は
- twitterで以下を発見 > 複数のmove_groupを使って遊びたい > 備忘録としてまとめとくか
という感じです。
さらっと読めると思うので、双腕ロボットをMoveitで作る方などのお力添えになれば幸いです。
ROS2-Moveit2で複数グループのインタラクティブマーカを表示させる
使用するモデルは、アイコンでも表示されているマンタ型ロボットで紹介していきます。
なお、ロボットのハード設計からソフト設計まで全て私が担当しており、所属研究室の教授からも承諾を得ています。
複数のmove_groupをもつようプログラムする
結論から述べると、
- 複数のmove_groupを持つグループを作成
- 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