MoveIt控制机械臂的运动实现——机器人抓取系统基础系列（二）

文章目录

概要

MoveIt为开发者提供了针对机械臂的集成化开发平台，由一系列操作相关的功能包组成，包括运动规划、操作控制、3D感知、运动学、导航算法等。

MoveIt实现机械臂控制包含四个步骤，分别是组装机器人URDF模型；配置MoveIt规划功能包；驱动设置；控制与规划实现。

在UR机械臂的ROS驱动安装官方教程详解——机器人抓取系统基础系列（一）博文中，当完成UR机械臂的ROS驱动配置之后，实际上前三步都已经完成了，本文主要关注第四步，重点介绍MoveIt的规划类型及其实现。

1 用户接口和代码案例

MoveIt的核心节点move_group的用户接口如下图所示，包括C++，Python，Rviz交互接口等。其能够获取机器人的内部（如关节）和外部（如视觉）信息，来控制真实或者仿真中的机械臂运动。
MoveIt控制机械臂的运动实现——机器人抓取系统基础系列（二）-LMLPHP

这里先展示一个Python案例代码，先初步了解一下Moveit的控制流程。

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

该代码案例中，展示了基本的Python控制接口。包括创建规划组的控制对象，然后设置关节空间运动的目标位姿，最后完成机械臂的运动。

机械臂一共经历了三个状态，即从Home位到设定的joint_positions，再回到Home位姿。Home位姿位事先定义的位姿。

2 不同的规划类型

MoveIt的规划类型包括关节空间规划、工作空间规划和笛卡尔空间规划。下面分别阐述每种规划类型的定义，用法和相关的API。

2.1 关节空间规划

关节空间规划：这种规划方式直接在机械臂的关节角度上进行，通常用于精确控制机器人的每个关节。
在关节空间规划中，你可以指定一系列关节角度作为目标，MoveIt将计算出一个运动路径，使得机械臂能够从当前状态平滑地移动到目标状态。
关节空间规划通常用于需要精确控制关节角度的场景。

本文第1节中展示的案例代码就是关节空间运动规划的实现，关节空间运动规划的核心API如下：

arm = moveit_commander.MoveGroupCommander('manipulator') # 设置规划组的控制对象
arm.set_joint_value_target(joint_positions) # 设置关节空间的目标位姿
arm.go() # 控制机械臂完成运动

2.2 工作空间规划

工作空间规划：与关节空间规划不同，工作空间规划关注的是机械臂末端执行器在空间中的位置和姿态。
在这种规划中，你可以指定末端执行器需要达到的位姿（位置和姿态），MoveIt将根据逆运动学计算出相应的关节角度变化，以实现这一目标。
工作空间规划适用于那些更关注末端执行器位置而非具体关节角度的场景。

工作空间中运动控制的核心API使用流程如下：

arm = moveit_commander.MoveGroupCommander('manipulator') # 初始化需要使用move group 
end_effector_link = arm.get_end_effector_link() # 获取终端link的名称

reference_frame = 'base_link' 
arm.set_pose_reference_frame(reference_frame) # 设置目标位置所使用的参考坐标系

arm.set_start_state_to_current_state() # 设置机器臂当前的状态作为运动初始状态
arm.set_pose_target(target_pose, end_effector_link) # 设置机械臂终端运动的目标位姿

traj = arm.plan() # 规划运动路径
arm.execute(traj) # 按照规划的运动路径控制机械臂运动

arm.shift_pose_target(1, -0.05, end_effector_link) # 也可以对机械臂的单轴进行运动控制
arm.go()

首先需要创建规划组的控制对象；然后获取机器人终端link名称；其次设置目标位姿对应的参考坐标系、起始和终止位姿；最后进行规划并控制机器人运动。

2.3 笛卡尔空间规划

笛卡尔空间规划：笛卡尔空间规划是指在笛卡尔坐标系（即直角坐标系）中进行的运动规划。
这种规划方式允许你指定机械臂末端执行器在笛卡尔空间中的路径，例如直线或圆弧运动。
MoveIt可以使用这种规划来生成复杂的轨迹，使得机械臂能够沿着预定的路径移动，同时避开障碍物。

笛卡尔运动规划的核心API如下：

# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
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...")

笛卡尔路径规划的核心API——compute_cartesian_path共有四个参数：第一个参数是之前创建的路点列表；第二个参数是终端步进值；第三个参数是跳跃阈值；第四个参数用于设置运动过程中是否考虑避障。

3 MoveIt运行实操

准备工作1： 事先完成UR机械臂的ROS驱动配置工作，具体步骤参考博文UR机械臂的ROS驱动安装官方教程详解——机器人抓取系统基础系列（一）。

准备工作2： 可以《ROS机器人开发实践》书籍开源程序中的MoveIt代码为案例进行测试。事先下载本书的开源代码，下载地址为https://github.com/huchunxu/ros_exploring。其中，MoveIt规划案例功能包文件夹为ros_exploring-master\robot_marm\marm_planning。

Step1: 将下载到的MoveIt案例功能包（ros_exploring-master\robot_marm\marm_planning文件夹）复制到机器人的ROS工作空间中。ROS功能包相当于汽车的零部件可以直接拿来使用，不用做重复造轮子的事情。

Step2: 重新编译ROS工作空间；

$ catkin_make # 编译
$ source devel/setup.bash # 加载路径

Step3: 启动UR机械臂和相关软件；
参考博文UR机械臂的ROS驱动安装官方教程详解——机器人抓取系统基础系列（一）

# 同时启动机器人，MoveIt和Rviz
$ roslaunch ur_robot_driver ur5e_work_all.launch

Step4: MoveIt规划测试；

# 可以分别测试本文介绍的三种规划方法
$ rosrun marm_planning moveit_fk_demo.py # 关节空间规划
$ rosrun marm_planning moveit_ik_demo.py # 工作空间规划
$ rosrun marm_planning moveit_cartesian_demo.py _cartesian:=True # 笛卡尔空间规划

4 相关资料推荐

ROS MoveIt 控制机械臂速度的方法汇总https://dongyi.blog.csdn.net/article/details/139197592?spm=1001.2014.3001.5502
Moveit 1官网教程https://moveit.github.io/moveit_tutorials/
Moveit 1中文官网教程（个人翻译版）https://decyzy.github.io/moveit_tutorials/index.html
Moveit 2 (for ROS 2) 官网教程https://moveit.picknik.ai/main/index.html

小结

本文简单介绍了MoveIt的用户接口和规划类型，包括关节空间、工作空间、笛卡尔空间规划，然后基于UR机械臂实现MoveIt的规划控制。进一步地，如何基于视觉信息去控制机械臂运动将在后续系列文章中介绍。

EAI-Robotics