Pose to PoseのためのMoveGroupAction

（この記事は[ROSアドベントカレンダー Advent Calendar 2018](https://qiita.com/advent-calendar/2018/ros)の22日目です）

# MoveGroupActionについて
Motion PlanningのためのROSプラグイン，[MoveIt](http://moveit.ros.org/)のAction Interfaceです.
MoveItのSystem Architectureは以下のようになっています．
![SA](https://moveit.ros.org/assets/images/robots_old/Overview.0012.jpg)

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

# 前提
* ros-kinetic-moveitがインストール済み
* 使用したいロボットのモデル等設定ファイルがセットアップされている
    * [MoveIt! Setup Assistant](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/setup_assistant/setup_assistant_tutorial.html)まで終わっていれば大丈夫です．
* 以降のコードは上のTutorialと同じ環境で実行できます．

では実装しながら解説していきましょう．
# Move Group Python Interfaceでの実装
まずMove Group Python Interfaceでの実装を見てみましょう [Tutorial](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-pose-goal)

```python
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 
```python
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](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html)では使わなかったモジュールがいくつかありますが，それらに関しては実装の中で解説していきます．

## Action Clientの定義
```python
rospy.init_node('move_group_client')
action_topic = '/move_group'
client = actionlib.SimpleActionClient(action_topic, MoveGroupAction)
```
NodeとTopicを指定しAction Clientを作ります．

## 必要なロボットのパラメータ取得
```python
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](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html)と同じです．`tolerance`は目標座標・姿勢の許容誤差を設定しています．

## Goalの設定
まず[Tutorial](http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-pose-goal)と同じように目標座標・姿勢を指定します．

```python
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を作成します．

```python

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

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

```python
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`があり，目標座標・姿勢はこの２つで表されます．

```python
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`を半径とした球体で位置と許容誤差を表しています．

```python
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`を設定します．

```python
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](http://docs.ros.org/melodic/api/moveit_msgs/html/msg/MotionPlanRequest.html),  [moveit_msgs/PlanningOptions Message](http://docs.ros.org/melodic/api/moveit_msgs/html/msg/PlanningOptions.html))を見ると様々な設定があります．

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

```python
client.send_goal(action_goal)
```

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

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