Moveit若不“骚”，为何机械臂老扭腰？小鱼带你解决Moveit规划绕路问题？

最新推荐文章于 2024-10-19 10:10:17 发布

原创

最新推荐文章于 2024-10-19 10:10:17 发布 · 997 阅读

17 ·

CC 4.0 BY-SA版权

文章标签：

#Moveit #规划

你好，我是每天掉很多头发的小鱼，当然掉头发的原因不是扭腰，而是对机器人爱的深沉。
请添加图片描述
今天说的问题曾今有不少小伙伴问过小鱼，当时小鱼也没有仔细研究，最近这两天因为一个机械臂相关的单子（欢迎砸单），让小鱼不得不又整了一下，今天这篇文章就把解决方案分享给大家。

一、看问题

我们先看下问题，小二上图！

上面的是机械臂通过笛卡尔空间运动，向下运动30cm，虽然最终的flan坐标是成功到达目标点了，但这个腰扭的让人心跳加速，气血上涌。

二、找原因

为什么路径和姿态那么奇怪，小鱼一开始以为是姿态问题，毕竟用的求解器是kdl，后来换了ikfast头也不正常，于是问题就来到了路径的头上，moveit默认的路径搜索使用的是ompl开源库，http://ompl.kavrakilab.org/。

OMPL集成了很多路径搜索算法，比如常见的RRT、RRTStar、PRM、EST、SBL、 KPIECE、SyCLOP等，虽然都可以进行路径搜索，但并不能保证最终搜索出来的路径是我们想要的那种，比如这里小鱼让机械臂向下移30cm，结果机械臂饶了一圈。

小鱼将ompl支持的算法常用的都测试了下，可能因为参数调的不够好，最终效果都不太满意，关于如何调整moveit所使用的ompl算法，小鱼再搞一篇文章来讲。

回到我们现在的问题，我们该如何解决？

三、kill它

Moveit除了支持直接运动到某一个点，还支持指定轨迹的运动，给定多个过路点，然后调用moveit的接口计算出一个路径。这里小鱼给的解决方案就是我们来生成路点来进行运动。

两点之间，直线最短。两点之间插值的算法有很多，简单起见，我们就采用直线插补，因为这里小鱼是从当前点向下运动30cm,所以姿态是不存在变化的，姿态的变化也有相应的插值算法，比如球面线性插值（Spherical linear interpolation，通常简称Slerp）。

让我们看看前后的Moveit代码对比：

首先是普通的笛卡尔空间运动

/**
 * @brief 笛卡尔空间运动
 *
 * @param move_group_interface
 * @param target_pose
 * @return true
 * @return false
 */
bool cart_move(moveit::planning_interface::MoveGroupInterface &move_group_interface, geometry_msgs::Pose &target_pose)
{
   
   
    move_group_interface.setPoseTarget(target_pose);
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group_interface.plan(my_plan) == moveit