MoveIt!入门教程-Move Group Python接口

MoveIt!入门教程-Move Group Python接口

说明

• MoveIt主要的用户接口功能通过MoveGroup类实现
• 这个类提供简易方式去实现大部分功能，比如：设置关节或目标姿态，创建行为规划，移动机器人，在环境中增加对象或给机器人增加或减少对象。

使用

1. 安装

• 使用Python接口前，要先导入moveit_commander等相关的Python包

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg

• 初始化moveit_commander和rospy

print "============ Starting tutorial setup"
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对象，这个接口应用与一组关节。这里使用左臂，能使用左臂来规划和执行动作。

group = moveit_commander.MoveGroupCommander("left_arm")

• 创建DisplayTrajectory发布器，可以得到轨迹在Rviz总实现可视化。

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

• 等待Rviz初始化

print "============ Waiting for RVIZ..."
rospy.sleep(10)
print "============ Starting tutorial "

2. 获得基本信息

• 打印参考系的名称

print "============ Reference frame: %s" % group.get_planning_frame()

• 打印这个组的末端执行器的连接名称

print "============ Reference frame: %s" % group.get_end_effector_link()

• 获得机器人的所有组

print "============ Robot Groups:"
print robot.get_group_names()

• 用于调式，打印机器人的状态

print "============ Printing robot state"
print robot.get_current_state()
print "============"

3. 规划姿态目标

• 为这个组规划动作，实现末端执行器到达期望的姿态目标。

print "============ Generating plan 1"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = -0.05
pose_target.position.z = 1.1
group.set_pose_target(pose_target)

• 调用规划器计算规划并在Rviz里显示，注意：这里只是规划，不是实际执行。

plan1 = group.plan()

print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(5)

• 你可以要求Rviz显示规划（或叫轨迹），但group.plan()方法会自动实现。

print "============ Visualizing plan1"
display_trajectory = moveit_msgs.msg.DisplayTrajectory()

display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory);

print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(5)

4. 移动到姿态目标

• 移动到一个姿势的目标是类似于上述步骤除了我们现在使用的go()功能。
• 请注意，我们已经设置了前面的姿势目标仍然是有效的，所以机器人将尝试移动到那个目标。
• 我们不会在本教程中使用该函数，因为它是一个阻塞函数，需要一个控制器是激活，执行后报告成功的轨迹。

# Uncomment below line when working with a real robot
# group.go(wait=True)

5. 规划到连接空间目标

• 设置连接空间目标并移动，首先清除姿态目标

group.clear_pose_targets()

• 取得组目前设置的连接值

group_variable_values = group.get_current_joint_values()
print "============ Joint values: ", group_variable_values

• 修改其中一个关节，规划移动到新的连接空间目标，可视化规划。

group_variable_values[0] = 1.0
group.set_joint_value_target(group_variable_values)

plan2 = group.plan()

print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(5)

6. 笛卡尔路径

• 你可以计划一个笛卡尔路径通过直接为末端执行器指定航点列表。

waypoints = []

# start with the current pose
waypoints.append(group.get_current_pose().pose)

# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x + 0.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z
waypoints.append(copy.deepcopy(wpose))

# second move down
wpose.position.z -= 0.10
waypoints.append(copy.deepcopy(wpose))

# third move to the side
wpose.position.y += 0.05
waypoints.append(copy.deepcopy(wpose))

• 我们希望笛卡尔路径被内插在一个分辨率为1厘米，这就是为何在笛卡尔转换指定0.01作为最大值。我们指定跳阈值为0，实际上是禁用它。（没理解好留原文）We want the cartesian path to be interpolated at a resolution of 1 cm which is why we will specify 0.01 as the max step in cartesian translation. We will specify the jump threshold as 0.0, effectively disabling it.

(plan3, fraction) = group.compute_cartesian_path(
                             waypoints,   # waypoints to follow
                             0.01,        # eef_step
                             0.0)         # jump_threshold

print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(5)

7. 添加/删除对象和附着/分离对象

• 首先，我们将定义碰撞对象消息。

collision_object = moveit_msgs.msg.CollisionObject()

• 完成后关闭

moveit_commander.roscpp_shutdown()

8. 完整代码

• 完整代码在这里

9. launch 文件

• 文件在这里

10. 在moveit_tutorials包直接运行代码

roslaunch moveit_tutorials move_group_python_interface_tutorial.launch

11. 期望的输出

• 在Rviz，我们应该能够看到下面的（会有一个延迟5-10秒之间每一步）：
  • 机器人移动左臂到它前面的姿态目标(plan1)
  • 重复上面的动作
  • 机器人移动左臂到连接目标。
  • 机器人沿着期望的笛卡尔路径运行