MoveIt教程[4]：Move Group Python Interface

MoveIt最简单的用户接口是通过基于Python的Move Group接口。这些包装器为普通用户可能需要的大多数操作提供了功能，特别是设置关节或姿势目标、创建运动规划、移动机器人、向环境中添加对象和从robot上附加/分离对象。
快速看YouTube视频demo，了解Move Group Python接口的强大功能。
一.Start RViz and MoveGroup node
打开两个shell。在第一个shell中运行这个命令，并等待一切完成加载：

roslaunch panda_moveit_config demo.launch

现在使用rosrun在另一个shell中直接运行Python代码。注意，在某些情况下，可能需要使python脚本可执行：

rosrun moveit_tutorials move_group_python_interface_tutorial.py

二.Expected Output
在RViz中，应该能够看到以下内容：
在shell终端中按<enter>，在每个步骤之间运行rosrun命令：
1.robot计划并移动它的arm到关节目标。
2.robot规划一条路径到达一个姿态目标。
3.robot规划一条笛卡尔坐标路径。
4.robot再次显示笛卡尔路径规划。
5.robot执行笛卡尔路径规划。
6.一个方框出现在Panda末端执行器的位置。
7.box更改颜色以表示它现在已被附加。
8.robot计划并执行一个附加盒子的笛卡尔路径。
9.box再次更改颜色以表示它现在已被分离。
10.box消失。

三.The Entire Code
说明：全部代码参考文献[1]。
使用Python MoveIt接口，将导入moveit_commander名称空间。这个命名空间提供了MoveGroupCommander类、PlanningSceneInterface类和RobotCommander类。[详见下文]
也导入rospy和一些我们使用的信息：

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list

首先初始化moveit_commander和rospy节点：

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
                anonymous=True)

实例化RobotCommander对象。这个对象是robot的外部接口：

robot = moveit_commander.RobotCommander()

实例化一个PlanningSceneInterface对象。这个物体是robot周围世界的接口：

scene = moveit_commander.PlanningSceneInterface()

实例化MoveGroupCommander对象。这个对象是一组关节的接口。在本例中，组是Panda手臂中的关节，因此设置group_name = panda_arm。如果使用的是不同的机器人，应该将此值更改为robot手臂planning group的名称。这个界面可以用来规划和执行Panda的运动：

group_name = "panda_arm"
group = moveit_commander.MoveGroupCommander(group_name)

创建一个displayorbit publisher，稍后将其用于发布RViz可视化的轨迹：

display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                               moveit_msgs.msg.DisplayTrajectory,
                                               queue_size=20)

1.Getting Basic Information

# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print "============ Reference frame: %s" % planning_frame

# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print "============ End effector: %s" % eef_link

# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Robot Groups:", robot.get_group_names()

# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""

2.Planning to a Joint Goal
Panda的零配置是在一个奇点，所以要做的第一件事是把它移动到一个更好的配置。

# We can get the joint values from the group and adjust some of the values:
joint_goal = group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = -pi/4
joint_goal[2] = 0
joint_goal[3] = -pi/2
joint_goal[4] = 0
joint_goal[5] = pi/3
joint_goal[6] = 0

# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
group.go(joint_goal, wait=True)

# Calling ``stop()`` ensures that there is no residual movement
group.stop()

3.Planning to a Pose Goal
可以为这一组规划一个运动来达到末端执行器想要的姿势：

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)

现在，调用planner来计算计划并执行它。

plan = group.go(wait=True)
# Calling `stop()` ensures that there is no residual movement
group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
group.clear_pose_targets()

4.Cartesian Paths
可以通过指定末端执行器通过的路径点列表来直接规划笛卡尔路径：

waypoints = []

wpose = group.get_current_pose().pose
wpose.position.z -= scale * 0.1  # First move up (z)
wpose.position.y += scale * 0.2  # and sideways (y)
waypoints.append(copy.deepcopy(wpose))

wpose.position.x += scale * 0.1  # Second move forward/backwards in (x)
waypoints.append(copy.deepcopy(wpose))

wpose.position.y -= scale * 0.1  # Third move sideways (y)
waypoints.append(copy.deepcopy(wpose))

# We want the Cartesian path to be interpolated at a resolution of 1 cm
# which is why we will specify 0.01 as the eef_step in Cartesian
# translation.  We will disable the jump threshold by setting it to 0.0 disabling:
(plan, fraction) = group.compute_cartesian_path(
                                   waypoints,   # waypoints to follow
                                   0.01,        # eef_step
                                   0.0)         # jump_threshold

# Note: We are just planning, not asking move_group to actually move the robot yet:
return plan, fraction

5.Displaying a Trajectory
可以让RViz可视化一个规划[即轨迹]。但是group.plan()方法会自动执行这个操作，所以在这里不是很有用[它只是再次显示了相同的轨迹]：
一个DisplayTrajectory msg有两个主要字段，trajectory_start和trajectory。使用当前的robot状态来填充trajectory_start，以复制所有的AttachedCollisionObjects并将计划添加到轨迹中。

display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
# Publish
display_trajectory_publisher.publish(display_trajectory);

6.Executing a Plan
如果希望robot按照已经计算好的计划执行，请使用execute：

group.execute(plan, wait=True)

注意：robot当前的关节状态必须在RobotTrajectory的第一个路径点的一定范围内，否则execute()将失败。
7.Adding Objects to the Planning Scene
首先，在规划场景中左手手指的位置创建一个box：

box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "panda_leftfinger"
box_pose.pose.orientation.w = 1.0
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))

8.Ensuring Collision Updates Are Receieved
如果Python节点在发布collision对象更新消息之前死亡，则消息可能丢失，box将不会出现。为了确保进行了更新，要等到看到get_known_object_names()和get_known_object_names()列表中反映的更改。出于本教程的目的，在规划场景中添加、删除、附加或分离对象后调用此函数。然后等待更新完成或timeout秒已过。

start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
  # Test if the box is in attached objects
  attached_objects = scene.get_attached_objects([box_name])
  is_attached = len(attached_objects.keys()) > 0

  # Test if the box is in the scene.
  # Note that attaching the box will remove it from known_objects
  is_known = box_name in scene.get_known_object_names()

  # Test if we are in the expected state
  if (box_is_attached == is_attached) and (box_is_known == is_known):
    return True

  # Sleep so that we give other threads time on the processor
  rospy.sleep(0.1)
  seconds = rospy.get_time()

# If we exited the while loop without returning then we timed out
return False

9.Attaching Objects to the Robot
接下来，将把box固定在Panda的手腕上。操纵物体需要robot能够接触到它们，而不需要规划场景将接触报告为碰撞。通过向touch_links数组中添加链接名，可以告诉规划场景忽略这些链接和box之间的碰撞。对于Panda机器人，设置grasping_group=‘hand’。如果正在使用不同的robot，应该将此值更改为末端执行器组名称。

grasping_group = 'hand'
touch_links = robot.get_link_names(group=grasping_group)
scene.attach_box(eef_link, box_name, touch_links=touch_links)

10.Detaching Objects from the Robot
也可以从规划场景中分离和移除对象：

scene.remove_attached_object(eef_link, name=box_name)

11.Removing Objects from the Planning Scene
可以把这个box从世界上拿走。

scene.remove_world_object(box_name)

注意：在将对象从世界中移除之前，必须将其分离。

四.The Launch File
全部的launch文件参考文献[1]。本教程中的所有代码都可以从moveit_tutorials包[MoveIt的一部分]运行设置。

参考文献：
[1]move_group_python_interface_tutorial：https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py
[2]Move Group Python Interface：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html