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: