ROS学习笔记（九）：MoveIt！与机械臂控制
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_commander
class MoveItFkDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_fk_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = 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)
# 关闭并退出moveit
moveit_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, Pose
class MoveItIkDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_ik_demo')
# 初始化需要使用move group控制的机械臂中的arm group
arm = 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_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.2593
target_pose.pose.position.y = 0.0636
target_pose.pose.position.z = 0.1787
target_pose.pose.orientation.x = 0.911822
target_pose.pose.orientation.y = -0.0269758
target_pose.pose.orientation.z = 0.285694
target_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)
# 控制机械臂终端向右移动5cm
arm.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()
# 关闭并退出moveit
moveit_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 deepcopy
class MoveItCartesianDemo:
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = 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).pose
print start_pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
waypoints.append(start_pose)
# 设置路点数据，并加入路点列表
wpose = deepcopy(start_pose)
wpose.position.z -= 0.2
waypoints.append(deepcopy(wpose))
wpose.position.x += 0.1
waypoints.append(deepcopy(wpose))
wpose.position.y += 0.1
waypoints.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)
# 关闭并退出moveit
moveit_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, Pose
class MoveItObstaclesDemo:
def __init__(self):
# 初始化move_group的API
moveit_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 group
arm = 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)
# 设置每次运动规划的时间限制：5s
arm.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_frame
table_pose.pose.position.x = 0.26
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
box1_pose = PoseStamped()
box1_pose.header.frame_id = reference_frame
box1_pose.pose.position.x = 0.21
box1_pose.pose.position.y = -0.1
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
box1_pose.pose.orientation.w = 1.0
scene.add_box(box1_id, box1_pose, box1_size)
box2_pose = PoseStamped()
box2_pose.header.frame_id = reference_frame
box2_pose.pose.position.x = 0.19
box2_pose.pose.position.y = 0.15
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
box2_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_frame
target_pose.pose.position.x = 0.2
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_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_frame
target_pose2.pose.position.x = 0.2
target_pose2.pose.position.y = -0.25
target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_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()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
# 设置场景物体的颜色
def setColor(self, name, r, g, b, a = 0.9):
# 初始化moveit颜色对象
color = ObjectColor()
# 设置颜色值
color.id = name
color.color.r = r
color.color.g = g
color.color.b = b
color.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.0
scene.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 += 1
rospy.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 = None
n_attempts = 0
# 生成放置姿态
places = self.make_places(place_pose)
# 重复尝试放置，直到成功或者最多尝试次数
while result != MoveItErrorCodes.SUCCESS and n _ attempts < max_place_attempts:
n _ attempts += 1
rospy.loginfo("Placeattempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.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 + x
place.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 应用。