Move Group Python接口

Move Group Python接口

最简单的MoveIt用户界面之一是通过基于Python的Move Group Interface。这些包装器为普通用户可能需要的大多数操作提供了功能，特别是设置关节或姿势目标，创建运动计划，移动机器人，将对象添加到环境中以及从机器人中附着/分离对象。

启动RViz和MoveGroup节点

roslaunch panda_moveit_config demo.launch

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

rosrun moveit_tutorials move_group_python_interface_tutorial.py

输出

在RViz中，我们应该能够看到以下内容：
要在每个步骤之间运行rosrun命令，先在终端中按下< enter >

1. 机器人计划并移动其手臂至联合目标。
2. 机器人计划到姿势目标的路径。
3. 机器人计划笛卡尔路径。
4. 机器人再次显示笛卡尔路径计划。
5. 机器人执行笛卡尔路径计划。
6. 熊猫末端执行器的位置出现一个框。
7. 该框将更改颜色以指示它现在已连接。
8. 机器人计划并执行附有盒子的笛卡尔路径。
9. 该框再次更改颜色以指示它现在已分离。
10. 该框消失。

整个代码

注意：完整的代码可以在GitHub教程仓库中看到。

要使用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 = moveit_commander.RobotCommander()

实例化一个PlanningSceneInterface对象。这为获取、设置和更新机器人对周围世界的内部理解提供了一个远程接口:

scene = moveit_commander.PlanningSceneInterface()

实例化MoveGroupCommander对象。该对象是计划组（一组关节）的接口。在本教程中，该组是熊猫机器人中的主要手臂关节，因此我们将组的名称设置为“ panda_arm”。如果使用其他机器人，请将此值更改为机器人手臂计划组的名称。该界面可用于计划和执行动作：

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

创建一个DisplayTrajectory ROS发布器，用于在Rviz中显示轨迹：

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

获取基本信息

＃我们可以获得该机器人的参考框架的名称：
planning_frame = move_group.get_planning_frame()
print "============ Planning frame: %s" % planning_frame
＃我们还可以打印该组的末端执行器链接的名称：
eef_link = move_group.get_end_effector_link()
print "============ End effector link: %s" % eef_link
＃我们可以得到机器人中所有组的列表:
group_names = robot.get_group_names()
print "============ Available Planning Groups:", robot.get_group_names()
＃有时，为了调试，打印的整个状态是有用的
＃机器人:
print "============ Printing robot state"
print robot.get_current_state()
print ""

设置关节目标

熊猫的初始配置是在一个奇点所以我们要做的第一件事是把它移动到一个更好的配置。

＃我们可以从组中获取关节值并调整一些值：
joint_goal = move_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

＃go命令可以使用关节值、姿态，如果你已经设置了姿态或关节目标也可以不使用参数
move_group.go(joint_goal, wait=True)

＃调用stop（）确保没有残留运动
move_group.stop()

规划姿态目标

我们可以为此组计划一个运动到最终执行者所需的姿势：

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

move_group.set_pose_target(pose_goal)

现在，我们规划器来计算计划并执行它。
plan = move_group.go(wait=True)

＃调用stop（）确保没有残留运动
move_group.stop()
＃计划好姿势后，最好清除目标。
＃clear_joint_value_targets()没有等价的函数
move_group.clear_pose_targets()

笛卡尔路径

你可以通过为末端执行器指定一个路径点列表来直接规划一个笛卡尔路径。如果在Python shell中以交互方式执行，则设置scale = 1.0。
waypoints = []

wpose = move_group.get_current_pose().pose
wpose.position.z -= scale * 0.1  ＃首先向上移动（z）
wpose.position.y += scale * 0.2  ＃和横向（y）
waypoints.append(copy.deepcopy(wpose))

wpose.position.x += scale * 0.1  ＃第二步:前进/后退(x) 
waypoints.append(copy.deepcopy(wpose))

wpose.position.y -= scale * 0.1  ＃三步横向移动(y)
waypoints.append(copy.deepcopy(wpose))

＃我们希望笛卡尔坐标路径以1厘米的分辨率内插
＃这就是为什么我们将0.01指定为笛卡尔坐标中的eef_step
＃我们将通过设置它为0.0来禁用跳转阈值，
＃忽略关节空间中不可行的跳跃检查，对于本教程。这就足够了

(plan, fraction) = move_group.compute_cartesian_path(
                                   waypoints,   ＃路径点
                                   0.01,        ＃插值步长
                                   0.0)         ＃跳跃阈值

＃注意：我们只是在计划，而不是要求move_group实际移动机器人：
return plan, fraction

显示轨迹

您可以让RViz为您可视化一个计划(即轨迹)。
但是group.plan()方法会自动执行这个操作，所以在这里不是很有用(它只是再次显示相同的轨迹):

一个显示轨迹msg有两个主要的字段，trajectory_start（起始轨迹）和trajectory （轨迹）。
我们使用当前的机器人状态来填充trajectory_start，以复制所有的AttachedCollisionObjects并将我们的计划添加到该轨迹。

display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan)
＃发布
display_trajectory_publisher.publish(display_trajectory);

执行计划

如果您希望机器人遵循已计算的计划，请使用执行：

move_group.execute(plan, wait=True)

注意：机器人的当前关节状态必须在RobotTrajectory中的第一个路径点的公差范围内，否则execute()将失败

向规划场景添加对象

首先，我们将在规划场景中左手手指的位置创建一个方框:

box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "panda_leftfinger"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.z = 0.07 ＃稍微高于末端执行器
box_name = "box"
scene.add_box(box_name, box_pose, size=(0.1, 0.1, 0.1))

确保收到碰撞更新

如果Python节点在发布冲突对象更新消息之前死亡，则消息可能丢失，框将不会出现。
为了确保进行了更新，我们要等到看到get_attached_objects()和get_known_object_names()列表中反映的更改。
就本教程而言，我们在规划场景中添加、删除、附加或分离对象后调用此函数。
然后等待更新完成或超时时间过了

start = rospy.get_time()
seconds = rospy.get_time()
while (seconds - start < timeout) and not rospy.is_shutdown():
  ＃测试盒子是否在附加的对象中
  attached_objects = scene.get_attached_objects([box_name])
  is_attached = len(attached_objects.keys()) > 0

  ＃测试盒子是否在场景中。
  ＃Note that attaching the box will remove it from known_objects
  is_known = box_name in scene.get_known_object_names()

  ＃测试我们是否处于预期状态
  if (box_is_attached == is_attached) and (box_is_known == is_known):
    return True

  ＃休眠以便我们为进程中的其他线程提供时间
  rospy.sleep(0.1)
  seconds = rospy.get_time()

＃如果我们退出while循环而不返回，那么我们就超时了
return False

将对象附加到机器人

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

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

从机器人上取下物体

我们还可以从规划场景中分离和移除对象：

scene.remove_attached_object(eef_link, name=box_name)

从规划场景中删除对象

我们可以把这个盒子从世界上拿走。

scene.remove_world_object(box_name)

注意：必须先分离对象，然后才能将其从世界上移除

启动文件

整个启动文件是这里 GitHub上。本教程中的所有代码都可以从moveit_tutorialsMoveIt安装程序中包含的程序包中运行 。