目录
一、简介
大家好,欢迎关注遨博学院带来的系列技术分享文章(协作机器人ROS开发),今天我们来学习一下“自定义运动学求解器算法插件”。
14.自定义运动学求解器算法插件
二、环境版本
- 主机系统版本:Windwos10 64位
- 处理器型号:Intel-i7
- 虚拟机版本:VMware Workstation 16 Pro
- 虚拟机系统:Ubuntu 18.04.6 LTS
- ROS版本:Melodic
三、学习目标
四、知识储备
下面开始介绍本节内容的知识点:
1.MoveIt!插件机制
MoveIt!是ROS中一个重要的集成化开发平台,由一系列功能包组成,提供运动规划、操作控制、3D感知、运动学等功能模块,是ROS社区中使用度排名前三的功能包,目前已经支持AUBO-E5机械臂。
MoveIt!中集成了众多工具集软件,集成了各种库,包括:
运动规划(Motion Planning):MoveIt!只需提供机器人URDF模型,就可以调用几大运动规划库的规划算法(如OMPL,SBPL,CHMOP),自动生成机器人运动轨迹。
轨迹插值(Trajectory Processing):由于大多数规划器只能返回一系列路径点,MoveIt!可以根据机器人的控制参数(速度、加速度限制等)重新处理路径,生成一条带有时间戳、位置、速度、加速度信息的完整轨迹。
操作(Manipulation):根据识别的物体生成一系列动作抓取物体(pick-and-place)目前MoveIt!本身这部分功能还比较弱,不涉及任何反馈、动力学、re-grasp等操作问题。
3D感知(Perception):可以利用传感器采集的点云或深度图像,生成用于碰撞检测的OctoMap,OctoMap为八叉树形式表示点云,可以大大降低存储空间,同时,这些3D OctoMap也可以依据贝叶斯准则不断实时更新。这样,机器人就可以避开真实世界的障碍物了。
运动学(Kinematics):目前可以支持多种运动学求解器,如OpenRave的封闭解ikfast、Orocos的数值解KDL、考虑关节极限的数值解Trac_ik、用户自己定义的基于service的求解器。
碰撞检测(Collision Checking):碰撞检测是运动规划的一大难题,如果采用基于采样的规划算法,那么需要对每个采样点做有效性判断,这时候就需要进行碰撞检测。所以,运动规划需要提供一个高效的碰撞检测算法。FCL(Flexible Collision Library),可以非常快速地实现各种几何体(3D面片、OctoMap、基本几何体)的碰撞检测。
控制(Control):属于机器人控制接口,可以让MoveIt!发布机器人需要的控制指令。
交互(Interaction):MoveIt!给开发者提供了三种交互方式,直观的RViz图形界面、快速编程的Python、丰富的高级功能的C++。
如右图所示,MoveIt!中的众多功能都使用插件机制集成,并提供了丰富的接口。这样用户可以方便的覆写插件功能,而无需接触框架的核心。
2.MoveIt!运动学求解插件
运动学插件作为重要的功能模块,目前使用的主要有以下3个。
(1)KDL
KDL(Kinematics and Dynamics Library)是MoveIt!中的默认运动学插件,其有自己的优缺点,优点在于可求解封闭情况下逆运动,缺点在于速度慢,同时可能找不到解。
(2)TRAC-IK
TRAC-IK和KDL类似,也是一种基于数值解的运动学插件,但是在算法层面上进行了很多改进,求解效率高了很多。可以直接使用以下命令安装:
(3)IKFAST
IKFAST是一种基于解析算法的运动学插件,可以求解任意复杂运动链的运动学方程并保证每次求解的一致性,整体来说,IKFAST比较稳定快速,一般5us的速度可完成运算。
3.MoveIt!运动插件开发方法
MoveIt!并不提供算法源码,只是将算法按照约定的方法进行封装,算法是数学模型的代码实现,和MoveIt!本身并没有关系,如果需要集成功能算法到MoveIt!中,需要编写接口文件,如下图所示,满足MoveIt!的插件规范即可,然后向move_group进行注册,这样使用时即可通过yaml或launch文件进行直接的调用。
如图所示。
planning_interface是由moveit_core_package的核心库提供,提供一个标准的协议接口,方便用户集成其他算法。
PlannerConfigurationSettings用于配置参数。
PlanningContext用于规划算法的具体实现。
PlannerManager用于向MoveIt!注册插件,并告知MoveIt!此插件的作用。
PlannerManager实现时核心调用接口是Register,目的是进行注册。
PlanningContext实现时核心调用接口是solve,包含具体的运动规划算法。
4.自定义运动规划算法的实现
自定义一个LERPInterface类,主要内容是重构接口函数,完成自定义的运动规划算法,在类中重点是实现两个函数。
(1)solve函数,这是运动规划算法的主要调用接口,通过输入运动规划场景、运动规划的起始和终止位置,返回运动规划结果。
(2)interpolate函数,这是运动规划算法的具体实现函数,包含具体的算法。
自定义一个LERPPlanningContext类,这个类继承自MoveIt!提供的planning_interface::PlanningContext,实现了两个不同输入条件下的solve函数,terminate终止运动函数、clear清除缓存函数,如下列代码片段所示。
自定义一个LERPPlannerManager 类,这个类继承自MoveIt!提供的planning_interface::PlannerManager,其中,initialize实现了算法初始化、canServiceRequest实现该插件是否可以应答运动规划的请求、getDescription实现了获取运动规划器的字符串描述、getPlanningAlgorithms实现了获取运动规划算法的名称、getPlanningContext实现对避障状态配置。
如下列代码片段所示。
五、任务实施
接下来,我们来看下替换默认运动学求解器算法插件为自定义插件,控制虚拟机器人运动,验证算法的有效性的演示。
六、任务拓展
恢复默认配置。
完成更换自定义运动学求解器算法插件后,修改配置文件,将运动学插件更换为默认运动学求解器插件,分析相关配置文件的参数含义。
七、课堂小结
下面我们进行课堂小结:
MoveIt!的功能大部分都是以插件的形式提供,并提供了开发接口,运动学、碰撞检测、路径规划等功能都可以让用户将自己的算法移植到ROS中,完成对机器臂数学计算的验证。
八、课后练习
最后给大家布置两道课后习题。
1.在ROS中,分别更换运动学算法插件为KDL、TRAC-IK、IKFAST,完成相同路径的轨迹规划,对比AUBO-E5机械臂在不同运动学求解器下的路径效果。
2.按照例程编写一个运动学求解器算法插件,熟悉开发流程。
在接下来的ROS课程中,我们会讲解Intel RealSense深度相机ROS驱动开发,欢迎持续关注。