Pose to PoseのためのMoveGroupAction

（この記事はROSアドベントカレンダー Advent Calendar 2018の22日目です）

MoveGroupActionについて

Motion PlanningのためのROSプラグイン，MoveItのAction Interfaceです.
MoveItのSystem Architectureは以下のようになっています．

この中で，表題の通りチュートリアルでも触れられていないMoveGroup Actionについて解説していきます．
Actionを使うとご存知の通り，動作が中断してもFeedbackが帰ってくるのでデバッグ等に役立ちます．

前提

• ros-kinetic-moveitがインストール済み
• 使用したいロボットのモデル等設定ファイルがセットアップされている
  • MoveIt! Setup Assistantまで終わっていれば大丈夫です．
• 以降のコードは上のTutorialと同じ環境で実行できます．

では実装しながら解説していきましょう．

Move Group Python Interfaceでの実装

まずMove Group Python Interfaceでの実装を見てみましょう Tutorial

pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = 0.4
pose_goal.position.y = 0.1
pose_goal.position.z = 0.4
group.set_pose_target(pose_goal)

Pose()にエンドエフェクタの座標・姿勢を指定してset_pose_target()に渡すだけです．簡単ですね．

MoveGroup Actionでの実装

必要なモジュールのimport

import rospy

from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import Pose
import moveit_commander
from moveit_msgs.msg import MoveGroupAction, MoveGroupGoal, MoveItErrorCodes
from moveit_msgs.msg import Constraints, JointConstraint, PositionConstraint, OrientationConstraint, BoundingVolume

import actionlib

from shape_msgs.msg import SolidPrimitive

Tutorialでは使わなかったモジュールがいくつかありますが，それらに関しては実装の中で解説していきます．

Action Clientの定義

rospy.init_node('move_group_client')
action_topic = '/move_group'
client = actionlib.SimpleActionClient(action_topic, MoveGroupAction)

NodeとTopicを指定しAction Clientを作ります．

必要なロボットのパラメータ取得

move_group = 'panda_arm'
group = moveit_commander.MoveGroupCommander(move_group)

planning_frame = group.get_planning_frame()
print('planning_frame:', planning_frame)
# ('planning_frame:', '/panda_link0')

eef_link = group.get_end_effector_link()
print('eef_link:', eef_link)
# ('eef_link:', 'panda_link8')

tolerance = 0.01

tolerance以外の設定はTutorialと同じです．toleranceは目標座標・姿勢の許容誤差を設定しています．

Goalの設定

まずTutorialと同じように目標座標・姿勢を指定します．

pose_goal = Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = 0.4
pose_goal.position.y = 0.1
pose_goal.position.z = 0.4 

次にGoalを作成します．

action_goal = MoveGroupGoal()
action_goal.request.group_name = move_group

まずMoveGroupGoal()のインスタンスを作成し，request.group_nameに動かしたいロボットのグループの名前を設定します．
MoveGroupGoal()は実際のプランの内容を設定するrequestとプランニング時のオプションを設定するplanning_optionsを持っています．

次は目標座標・姿勢を設定していきましょう．

goal_constraint = Constraints()
goal_constraint.position_constraints.append(PositionConstraint())
goal_constraint.position_constraints[0].header.frame_id = planning_frame
goal_constraint.position_constraints[0].link_name = eef_link

MoveGroup Actionは目標姿勢をgoal_constraintで表現します．
goal_constraintの下には座標に関する拘束position_constraintsと角度に関する拘束orientation_constraintsがあり，目標座標・姿勢はこの２つで表されます．

solid_primitive = SolidPrimitive()
solid_primitive.dimensions = [tolerance * tolerance]
solid_primitive.type = solid_primitive.SPHERE

bounding_volume = BoundingVolume()
bounding_volume.primitives.append(solid_primitive)
bounding_volume.primitive_poses.append(pose_goal)
goal_constraint.position_constraints[0].constraint_region = bounding_volume

まず目標座標についての設定です．
PositionConstraint().constraint_regionは目標位置を中心としtoleranceを半径とした球体で位置と許容誤差を表しています．

goal_constraint.orientation_constraints.append(OrientationConstraint())
goal_constraint.orientation_constraints[0].header.frame_id = planning_frame
goal_constraint.orientation_constraints[0].link_name = eef_link
goal_constraint.orientation_constraints[0].orientation = pose_goal.orientation
goal_constraint.orientation_constraints[0].absolute_x_axis_tolerance = tolerance
goal_constraint.orientation_constraints[0].absolute_y_axis_tolerance = tolerance
goal_constraint.orientation_constraints[0].absolute_z_axis_tolerance = tolerance

次に姿勢に関して設定をしていきます．こちらは簡単で，同じようにplanning_frameとeef_linkを設定した後orientation_constraint.orientationにpose_goal.orientation，orientation_constraint.absolute_(x or y or z)_axis_toleranceにtoleranceを設定します．

action_goal.request.goal_constraints.append(goal_constraint)
action_goal.request.num_planning_attempts = 5
action_goal.request.allowed_planning_time = 5
action_goal.planning_options.replan = True

action_goal.planning_options.plan_only = False

最後に，プランニングの設定を行います．
それぞれ

• action_goal.request.num_planning_attempts：プランの計算が行われる回数

• action_goal.request.allowed_planning_time：プランニングに使われる最大時間（秒）

• action_goal.planning_options.replan：プランニングが失敗した時に再開するかどうか

• action_goal.planning_options.plan_only：プランニングのみをするかどうか
  を設定しています．
  ここで設定したもの以外にもリファレンス(moveit_msgs/MotionPlanRequest Message, moveit_msgs/PlanningOptions Message)を見ると様々な設定があります．

あとは作成したaction_goalを送信します．

client.send_goal(action_goal)

まとめ

MoveGroup ActionはMove Group Python Interfaceに比べて複雑ですが使えると他のアプリケーションと組み合わせる時など使いやすくなります．また，今回紹介した実装から発展させてさらに複雑な動作を作ることもできます．

最後に，この記事が数ヶ月前の筆者のように悩める人の一助となれば幸いです．