MoveIt教程8 - Move Group (Python接口）

类比于C++的MoveGroupInterface，MoveIt也提供了相应接口的Python封装。
可以编写.py程序控制机器人的常用基本操作：

设置关节角度或机器人姿态目标
运动规划
移动机器人
添加物体到环境里/从环境移除
将物体绑定到机器人上/从机器人上解绑

这些接口也是基于ROS的框架。

这里通过Panda机械臂的案例看一下怎么玩。

运行程序

启动panda机械臂MoveIt主程序

roslaunch panda_moveit_config demo.launch

在另一个终端rosrun运行python脚本

rosrun moveit_tutorials move_group_python_interface_tutorial.py

结果

教程中，对机械臂进行了以下几种控制：

1. 规划并运动到指定关节目标 (joint goal)
1. 规划一个到指定姿态目标（pose goal）的路径
1. 规划一个坐标系路径（Cartesian path）
1. 显示坐标系路径（Cartesian path）
1. 执行坐标系路径规划
1. 在末端执行器除添加一个立方体
1. 绑定立方体到机器人上
1. 在立方体绑定的情况下规划和执行一个坐标系路径（Cartesian path）
1. 解绑立方体
1. 移除立方体

代码分析

教程的完整代码https://github.com/ros-planning/moveit_tutorials/blob/kinetic-devel/doc/move_group_python_interface/scripts/move_group_python_interface_tutorial.py

下面我们分析一下代码块：

Setup

使用python接口时，我们通常需要加载moveit_commander，这是一个namespace package，里面会加载MoveGroupCommander, PlanningSceneInterface
以及RobotCommander

另外加载一些用到的ROS messages

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，创建一个node

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

创建一个RobotCommander的对象。RobotCommander是针对整个机器人的控制。

robot = moveit_commander.RobotCommander()

创建一个PlanningSceneInterface的对象。PlanningSceneInterface是用于和机器人环境的交互（如添加物体是会用到）

scene = moveit_commander.PlanningSceneInterface()

创建MoveGroupCommander的对象。MoveGroupCommander是针对一个规划组进行控制。这里通过设置group_name = panda_arm控制Panda机器人的手臂。

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

创建一个Publisher，发布的类型是DisplayTrajectory，用于rivz显示

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

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 ""

Planning to a Joint Goal

规划到一个关节目标

# 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()

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)

规划并执行

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()

Cartesian Paths

通过给一个指定的途经点waypoints的list来规划坐标系路径cartesian path

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

Displaying a Trajectory

在rviz中显示路径。调用group.plan()规划路径的时候回自动在rviz中显示。

也可以手动进行显示，创建一个 DisplayTrajectory
的消息对象，然后发布到'/move_group/display_planned_path'话题。

DisplayTrajectory消息主要有两个属性，起始点trajectory_start 和路径trajectory。

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);

Executing a Plan

执行计算出来的路径

group.execute(plan, wait=True)

Adding Objects to the Planning Scene

在场景中添加一个立方体, 设置位置在panda_leftfinger

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))

Ensuring Collision Updates Are Receieved

在添加/移除物体后，会发布一个更新碰撞物体的消息，这个消息在发布出去的时候可能会丢失，为了确保物体成功添加/移除，可以通过get_known_object_names()获取当前已知的物体来检查是否成功。

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

Attaching Objects to the Robot

绑定物体到机械臂。在用机械臂操纵物体的时候，为了防止MoveIt把某些link和物体的正常接触当做是碰撞而报，可以把这些link加入到touch_links，这样就会忽略这些link和物体的碰撞。

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

Detaching Objects from the Robot

解绑

scene.remove_attached_object(eef_link, name=box_name)

Removing Objects from the Planning Scene

移除

scene.remove_world_object(box_name)

注意：物体必须被解绑后才可以移除