机器人操作系统ROS（九）：MoveIt！与机械臂控制

MoveIt！简介
MoveIt！系统架构
MoveIt！编程与机械臂控制
- 关节空间规划
- 工作空间规划
- 笛卡尔运动规划
- 避障规划
Pick and Place示例

MoveIt！简介

MoveIt！是在2012年在PR2与多个机械臂功能包的基础上集成得到的ROS软件包，它为开发者提供了一个易于使用的集成化开发平台，由一系列移动操作的功能包组成，包括运动规划、操作控制、3D感知、运动学、控制与导航算法等，且提供友好的GUI，可以广泛应用于工业、商业、研发和其他领域。

使用MoveIt！实现机械臂控制的四个步骤：

组装：在控制之前需要有机器人，可以是真实的机械臂，也可以是仿真的机械臂，但都要创建完整的机器人URDF模型。
配置：使用MoveIt！控制机械臂之前，需要根据机器人的URDF模型，再使用Setup Assistant工具完成自碰撞矩阵、规划组、终端夹具等配置，配置完成后生成一个ROS功能包。
驱动：使用ArbotiX或者ros_control功能包中的控制器插件，实现对机械臂关节的驱动。插件的使用方法一般分为两步：首先创建插件的YAML配置文件，然后通过launch文件启动插件并加载配置参数。
控制：MoveIt！提供了C++、Python、rviz插件等接口，可以实现机器人关节空间和工作空间下的运动规划，规划过程中会综合考虑场景信息，并实现自主避障的优化控制。

关于MoveIt！安装与使用的更多内容可见教程。

MoveIt！系统架构

move_group是MoveIt！的核心节点，可以综合其他独立的功能组件作为用户提供ROS中的动作指令和服务。move_group本身并不具备丰富的功能，主要完成各功能包、插件的集成。它通过话题（Topic）的方式接收机器人发布的传感器信息、关节状态信息，以及机器人的TF坐标变换；它通过服务（Service）的方式运动规划器和运动学求解器通信；它还通过动作（Action）的方式向机器人控制器发送规划结果，并接受机器人执行情况的反馈；另外，还需要ROS参数服务器提供机器人的运动学参数，这些参数可根据机器人的URDF模型生成（SRDF和配置文件）。

MoveIt！编程与机械臂控制

MoveIt！的move_group提供了丰富的C++和Python的编程API，可以帮助我们完成更多运动控制的相关功能，接下来主要以Python API为例，介绍MoveIt！的编程方法。

关节空间规划

关节空间运动是以关节角度为控制量的机器人运动。机械臂关节空间的规划不需要考虑机器人终端的姿态。
使用如下命令实现MArm关节空间下的运动测试：

roslaunch marm_planning arm_planning.launch
rosrun marm_planning moveit_fk_demo.py

marm_planning/scripts/moveit_fk_demo.py的源码如下：

import rospy, sys
import moveit_commanderclass MoveItFkDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_fk_demo', anonymous=True)# 初始化需要使用move group控制的机械臂中的arm grouparm = moveit_commander.MoveGroupCommander('manipulator')# 设置机械臂运动的允许误差值arm.set_goal_joint_tolerance(0.001)# 设置允许的最大速度和加速度arm.set_max_acceleration_scaling_factor(0.5)arm.set_max_velocity_scaling_factor(0.5)# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()rospy.sleep(1)# 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度）joint_positions = [0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125]arm.set_joint_value_target(joint_positions)# 控制机械臂完成运动arm.go()rospy.sleep(1)# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()rospy.sleep(1)# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)if __name__ == "__main__":try:MoveItFkDemo()except rospy.ROSInterruptException:pass

工作空间规划

工作空间规划是通过机器人终端的三维坐标位置和姿态给定，在运动规划时使用逆向运动学求解各轴位置。
使用如下命令实现MArm工作空间下的运动测试：

roslaunch marm_planning arm_planning.launch
rosrun marm_planning moveit_ik_demo.py

marm_planning/scripts/moveit_ik_demo.py的源码如下：

import rospy, sys
import moveit_commander
from geometry_msgs.msg import PoseStamped, Poseclass MoveItIkDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_ik_demo')# 初始化需要使用move group控制的机械臂中的arm grouparm = moveit_commander.MoveGroupCommander('manipulator')# 获取终端link的名称end_effector_link = arm.get_end_effector_link()# 设置目标位置所使用的参考坐标系reference_frame = 'base_link'arm.set_pose_reference_frame(reference_frame)# 当运动规划失败后，允许重新规划arm.allow_replanning(True)# 设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.set_goal_position_tolerance(0.001)arm.set_goal_orientation_tolerance(0.001)# 设置允许的最大速度和加速度arm.set_max_acceleration_scaling_factor(0.5)arm.set_max_velocity_scaling_factor(0.5)# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()rospy.sleep(1)# 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，# 姿态使用四元数描述，基于base_link坐标系target_pose = PoseStamped()target_pose.header.frame_id = reference_frametarget_pose.header.stamp = rospy.Time.now()     target_pose.pose.position.x = 0.2593target_pose.pose.position.y = 0.0636target_pose.pose.position.z = 0.1787target_pose.pose.orientation.x = 0.911822target_pose.pose.orientation.y = -0.0269758target_pose.pose.orientation.z = 0.285694target_pose.pose.orientation.w = -0.293653# 设置机器臂当前的状态作为运动初始状态arm.set_start_state_to_current_state()# 设置机械臂终端运动的目标位姿arm.set_pose_target(target_pose, end_effector_link)# 规划运动路径traj = arm.plan()# 按照规划的运动路径控制机械臂运动arm.execute(traj)rospy.sleep(1)# 控制机械臂终端向右移动5cmarm.shift_pose_target(1, -0.05, end_effector_link)arm.go()rospy.sleep(1)# 控制机械臂终端反向旋转90度arm.shift_pose_target(3, -1.57, end_effector_link)arm.go()rospy.sleep(1)# 控制机械臂回到初始化位置arm.set_named_target('home')arm.go()# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)if __name__ == "__main__":MoveItIkDemo()

笛卡尔运动规划

工作空间的运动规划并没有对机器人终端轨迹有任何约束，目标位姿给定后，可以通过运动学反解获得关节空间下的各轴位置（即角度），接下来的规划和运动仍然在关节空间下完成。笛卡尔运动规划不仅考虑机械臂的起始位姿，还考虑了运动过程中的位姿约束。
使用如下命令实现MArm笛卡尔运动规划测试：

roslaunch marm_planning arm_planning_with_trail.launch
# 带路径约束的运动规划
rosrun marm_planning moveit_cartesian_demo.py _cartesian:=True
# 不带路径约束的运动规划
rosrun marm_planning moveit_cartesian_demo.py _cartesian:=False

marm_planning/scripts/moveit_cartesian_demo.py的源码如下：

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopyclass MoveItCartesianDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_cartesian_demo', anonymous=True)# 初始化需要使用move group控制的机械臂中的arm grouparm = MoveGroupCommander('manipulator')# 当运动规划失败后，允许重新规划arm.allow_replanning(True)# 设置目标位置所使用的参考坐标系arm.set_pose_reference_frame('base_link')# 设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.set_goal_position_tolerance(0.001)arm.set_goal_orientation_tolerance(0.001)# 设置允许的最大速度和加速度arm.set_max_acceleration_scaling_factor(0.5)arm.set_max_velocity_scaling_factor(0.5)# 获取终端link的名称end_effector_link = arm.get_end_effector_link()# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()rospy.sleep(1)# 获取当前位姿数据最为机械臂运动的起始位姿start_pose = arm.get_current_pose(end_effector_link).poseprint start_pose# 初始化路点列表waypoints = []# 将初始位姿加入路点列表waypoints.append(start_pose)# 设置路点数据，并加入路点列表wpose = deepcopy(start_pose)wpose.position.z -= 0.2waypoints.append(deepcopy(wpose))wpose.position.x += 0.1waypoints.append(deepcopy(wpose))wpose.position.y += 0.1waypoints.append(deepcopy(wpose))fraction = 0.0   #路径规划覆盖率maxtries = 100   #最大尝试规划次数attempts = 0     #已经尝试规划次数# 设置机器臂当前的状态作为运动初始状态arm.set_start_state_to_current_state()# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点while fraction < 1.0 and attempts < maxtries:(plan, fraction) = arm.compute_cartesian_path (waypoints,   # waypoint poses，路点列表0.01,        # eef_step，终端步进值0.0,         # jump_threshold，跳跃阈值True)        # avoid_collisions，避障规划# 尝试次数累加attempts += 1# 打印运动规划进程if attempts % 10 == 0:rospy.loginfo("Still trying after " + str(attempts) + " attempts...")# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动if fraction == 1.0:rospy.loginfo("Path computed successfully. Moving the arm.")arm.execute(plan)rospy.loginfo("Path execution complete.")# 如果路径规划失败，则打印失败信息else:rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.")  rospy.sleep(1)# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()rospy.sleep(1)# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)if __name__ == "__main__":try:MoveItCartesianDemo()except rospy.ROSInterruptException:pass

避障规划

机器人在工作空间内运动规划过程中存在障碍物时，需要进行避障规划。

使用如下命令实现MArm避障规划测试：

roslaunch marm_planning arm_planning.launch
rosrun marm_planning moveit_obstacles_demo.py

marm_planning/scripts/moveit_obstacles_demo.py的源码如下：

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander, PlanningSceneInterface
from moveit_msgs.msg import  PlanningScene, ObjectColor
from geometry_msgs.msg import PoseStamped, Poseclass MoveItObstaclesDemo:def __init__(self):# 初始化move_group的APImoveit_commander.roscpp_initialize(sys.argv)# 初始化ROS节点rospy.init_node('moveit_obstacles_demo')# 初始化场景对象scene = PlanningSceneInterface()# 创建一个发布场景变化信息的发布者self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)# 创建一个存储物体颜色的字典对象self.colors = dict()# 等待场景准备就绪rospy.sleep(1)# 初始化需要使用move group控制的机械臂中的arm grouparm = MoveGroupCommander('arm')# 获取终端link的名称end_effector_link = arm.get_end_effector_link()# 设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.set_goal_position_tolerance(0.01)arm.set_goal_orientation_tolerance(0.05)# 当运动规划失败后，允许重新规划arm.allow_replanning(True)# 设置目标位置所使用的参考坐标系reference_frame = 'base_link'arm.set_pose_reference_frame(reference_frame)# 设置每次运动规划的时间限制：5sarm.set_planning_time(5)# 设置场景物体的名称table_id = 'table'box1_id = 'box1'box2_id = 'box2'# 移除场景中之前运行残留的物体scene.remove_world_object(table_id)scene.remove_world_object(box1_id)scene.remove_world_object(box2_id)    rospy.sleep(1)# 控制机械臂先回到初始化位置arm.set_named_target('home')arm.go()rospy.sleep(2)# 设置桌面的高度table_ground = 0.25# 设置table、box1和box2的三维尺寸table_size = [0.2, 0.7, 0.01]box1_size = [0.1, 0.05, 0.05]box2_size = [0.05, 0.05, 0.15]# 将三个物体加入场景当中table_pose = PoseStamped()table_pose.header.frame_id = reference_frametable_pose.pose.position.x = 0.26table_pose.pose.position.y = 0.0table_pose.pose.position.z = table_ground + table_size[2] / 2.0table_pose.pose.orientation.w = 1.0scene.add_box(table_id, table_pose, table_size)box1_pose = PoseStamped()box1_pose.header.frame_id = reference_framebox1_pose.pose.position.x = 0.21box1_pose.pose.position.y = -0.1box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0box1_pose.pose.orientation.w = 1.0   scene.add_box(box1_id, box1_pose, box1_size)box2_pose = PoseStamped()box2_pose.header.frame_id = reference_framebox2_pose.pose.position.x = 0.19box2_pose.pose.position.y = 0.15box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0box2_pose.pose.orientation.w = 1.0   scene.add_box(box2_id, box2_pose, box2_size)# 将桌子设置成红色，两个box设置成橙色self.setColor(table_id, 0.8, 0, 0, 1.0)self.setColor(box1_id, 0.8, 0.4, 0, 1.0)self.setColor(box2_id, 0.8, 0.4, 0, 1.0)# 将场景中的颜色设置发布self.sendColors()    # 设置机械臂的运动目标位置，位于桌面之上两个盒子之间target_pose = PoseStamped()target_pose.header.frame_id = reference_frametarget_pose.pose.position.x = 0.2target_pose.pose.position.y = 0.0target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05target_pose.pose.orientation.w = 1.0# 控制机械臂运动到目标位置arm.set_pose_target(target_pose, end_effector_link)arm.go()rospy.sleep(2)# 设置机械臂的运动目标位置，进行避障规划target_pose2 = PoseStamped()target_pose2.header.frame_id = reference_frametarget_pose2.pose.position.x = 0.2target_pose2.pose.position.y = -0.25target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05target_pose2.pose.orientation.w = 1.0# 控制机械臂运动到目标位置arm.set_pose_target(target_pose2, end_effector_link)arm.go()rospy.sleep(2)# 控制机械臂回到初始化位置arm.set_named_target('home')arm.go()# 关闭并退出moveitmoveit_commander.roscpp_shutdown()moveit_commander.os._exit(0)# 设置场景物体的颜色def setColor(self, name, r, g, b, a = 0.9):# 初始化moveit颜色对象color = ObjectColor()# 设置颜色值color.id = namecolor.color.r = rcolor.color.g = gcolor.color.b = bcolor.color.a = a# 更新颜色字典self.colors[name] = color# 将颜色设置发送并应用到moveit场景当中def sendColors(self):# 初始化规划场景对象p = PlanningScene()# 需要设置规划场景是否有差异     p.is_diff = True# 从颜色字典中取出颜色设置for color in self.colors.values():p.object_colors.append(color)# 发布场景物体颜色设置self.scene_pub.publish(p)if __name__ == "__main__":try:MoveItObstaclesDemo()except KeyboardInterrupt:raise

Pick and Place示例

此节演示了一个简单的机器人pick and place应用，这个应用就是让机器人用夹爪将工作空间内的某个物体夹起来，然后将该物体放到目标位置。类似的功能在机器人的实际生产应用中使用非常广泛，例如码垛、搬运、挑拣等工作。

在这个应用例程中，假设已知物体的位置，MoveIt！需要控制机器人去抓取物体并放置到指定位置。

使用如下命令实现pick and place例程：

roslaunch marm_planning arm_planning.launch
rosrun marm_planning moveit_pick_and_place_demo.py

创建抓取的目标物体
创建一个用来抓取的目标物体，并将其设置颜色，代码如下：

target_size = [0.02,0.01,0.12]
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.32
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[2] + target_size[2]/2.0
target_pose.pose.orientation.w =1.0scene.add_box(target_id, target_pose, target_size)self.setColor(target_id, 0.9, 0.9, 0, 1.0)

设置目标物体的放置位置
创建一个place，并准备放置目标物体的位置，代码如下：

place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 0.32
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = table_ground + table_size[2] + target_size[2]/2.0
place_pose.pose.orientation.w = 1.0

生成抓取姿态
生成抓取姿态的代码如下：

grasp_pose = target_pose
grasps = self.make_grasps(grasp_pose, [target_id])
for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose)rospy.sleep(0.2)

在抓去之前，需要确认目标位姿是否存在正确的抓取姿态。在pick时，目标物体的抓取位置就是物体摆放的位置，然后使用make_grasps()函数生成抓取姿态的列表，并将抓取姿态的消息发布，显示在rviz中，代码如下：

def make_grasps(self, initial_pose_stamped, allowed_touch_objects):# 初始化抓取姿态对象 g = Grasp()# 创建夹爪张开、闭合的姿态 g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN)g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED)# 设置期望的夹爪靠近、撤离目标的参数g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 0.0, 0.0])g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, -1.0, 1.0])# 设置抓取姿态g.grasp_pose = initial_pose_stamped # 需要尝试改变姿态的数据列表pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3]yaw_vals = [0]# 抓取姿态的列表grasps = []# 改变姿态，生成抓取动作 for y in yaw_vals:for p in pitch_vals:# 欧拉角到四元数的转换q = quaternion _ from _ euler (0, p , y )# 设置抓取的姿态g.grasp_pose.pose.orientation.x = q[0]g.grasp_pose.pose.orientation.y = q[1]g.grasp_pose.pose.orientation.z = q[2]g.grasp_pose.pose.orientation.w = q[3]# 设置抓取的唯一 id 号g.id = str(len(grasps))# 设置允许接触的物体g.allowed_touch_objects = allowed_touch_objects# 将本次规划的抓取放入抓取列表中grasps.append(deepcopy(g))# 返回抓取列表return grasps

make_grasps()函数通过pitch角度的变化得到不同的抓取姿态。
pick
设置pick时的代码如下：

while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:n_attempts += 1rospy.loginfo("Pick attempt: " + str(n_attempts))result = arm.pick(target_id, grasps)rospy.sleep(0.2)

接下来机器人就可以尝试 pick 目标物体了。针对不同的抓取姿态，如果无法求解运动学逆解或者规划轨迹会发生碰撞，pick 的运动规划就会出错，因此例程设置重新尝试规划的次数。如果规划成功，则 pick()会控制机器人按照规划轨迹运动。

place
如果 pick 阶段的运动控制没有问题，那我们的工作已经完成了一半。接下来的任务是 place ，将目标物体放置到指定位置，代码如下：

if result == MoveItErrorCodes.SUCCESS:result = Nonen_attempts = 0# 生成放置姿态 places = self.make_places(place_pose)# 重复尝试放置，直到成功或者最多尝试次数while result != MoveItErrorCodes.SUCCESS and n _ attempts < max_place_attempts:n _ attempts += 1rospy.loginfo("Placeattempt: " + str(n_attempts))for place in places:result = arm.place(target_id, place)if result == MoveItErrorCodes.SUCCESS:breakrospy.sleep(0.2)

place 与 pick 的原理是一致的，同样需要使用 make_places() 生成一个可能的夹爪放置姿态列表。然后根据这些可能的放置姿态尝试规划 place 操作的轨迹，规划成功后就可以控制机器人运动了，代码如下：

def make_places(self, init_pose)# 初始化放置抓取物体的位置place = PoseStamped()# 设置放置抓取物体的位置place = init_pose# 定义 x 方向上用于尝试放置物体的偏移参数x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.0151]# 定义 y 方向上用于尝试放置物体的偏移参数y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]pitch_vals = [0]# 定义用于尝试放置物体的偏航角参数yaw_vals = [0]# 定义放置物体的姿态列表places = []# 生成每个角度和偏移方向上的抓取姿态for y in yaw_vals:for p in pitch_vals:for y in y_vals:for x in x_vals:place.pose.position.x = init_pose.pose.position.x + xplace.pose.position.y = init_pose.pose.position.y + y# 欧拉角到四元数的转换q = quaternion_from_euler(0, p, y)# 欧拉角到四元数的转换place.pose.orientation.x = q[0] place.pose.orientation.y = q[1] place.pose.orientation.z = q[2] place.pose.orientation.w = q[3]# 将该放置姿态加入列表places.append(deepcopy(place))# 返回放置物体的姿态列表return places

make_places() 和 make_grasps() 的实现原理相同，都是通过设定的方向偏移、旋转列表生成多个可能的终端姿态

通过以上例程，我们就实现了一个简单的 pick and place 应用。