Moveit!学习笔记 - Move Group（Python接口）

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

• 设置关节或姿势目
• 创建运动规划
• 移动机器人
• 向环境中添加对象
• 从robot上附加/分离对象

1.启动RViz和MoveGroup节点

启动 panda 机械臂 Moveit 主程序：

cd ws_moveit
roslaunch panda_moveit_config demo.launch

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

rosrun moveit_tutorials move_group_python_interface_tutorial.py

2.期望输出

在shell终端中按enter：

• robot计划并移动它的arm到关节目标。
• robot规划一条路径到达一个姿态目标。
• robot规划一条笛卡尔坐标路径。
• robot再次显示笛卡尔路径规划。
• robot执行笛卡尔路径规划。
• 一个方框出现在Panda末端执行器的位置。
• box更改颜色以表示它现在已被附加。
• robot计划并执行一个附加盒子的笛卡尔路径。
• box再次更改颜色以表示它现在已被分离。
• box消失。

3.代码理解

使用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对象，RobotCommander是针对整个机器人的控制：

    robot = moveit_commander.RobotCommander()

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

    scene = moveit_commander.PlanningSceneInterface()

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

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

3.2 Planning to a Joint Goal

规划到一个关节目标：

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

3.4 Cartesian Paths

通过指定末端执行器通过的路径点（waypoints）列表来直接规划笛卡尔路径：

    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

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

3.6 Executing a Plan

执行计算出来的路径:

    group.execute(plan, wait=True)

注意：robot当前的关节状态必须在RobotTrajectory的第一个路径点的一定范围内，否则execute()将失败。

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

3.8 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

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

3.10 Detaching Objects from the Robot

    scene.remove_attached_object(eef_link, name=box_name)

3.11 Removing Objects from the Planning Scene

    scene.remove_world_object(box_name)

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

4 完整代码

完整代码

【参考资源】
官网
相关博客