小鱼 ROS 2 新书上线!点击链接查看, 新书配套视频点击链接查看。
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
Moveit若不“骚”,为何机械臂老扭腰?小鱼带你解决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::planning_interface::MoveItErrorCode::SUCCESS); ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED"); ROS_INFO("Success with %ld points", my_plan.trajectory_.joint_trajectory.points.size()); move_group_interface.execute(my_plan); return success; } int main(int argc, char **argv) { ....... // 关节运动 joint_move(move_group_interface, {0, -M_PI /2, M_PI / 3, -M_PI / 3, M_PI / 2, 0}, joint_model_group); // 当前位置 geometry_msgs::PoseStamped current_pose = move_group_interface.getCurrentPose(); // 下降30cm后运动 current_pose.pose.position.z -= 0.3; cart_move(move_group_interface, current_pose.pose); }
运动效果如下:
接着是直接对z进行插值的轨迹运动代码。
/** * @brief 笛卡尔空间waypoints轨迹运动 * * @param move_group_interface * @param target_pose * @return true * @return false */ bool cart_move_with_waypoints(moveit::planning_interface::MoveGroupInterface &move_group_interface, const std::vector<geometry_msgs::Pose> waypoints) { moveit::planning_interface::MoveGroupInterface::Plan my_plan; moveit_msgs::RobotTrajectory trajectory; const double jump_threshold = 0.0; const double eef_step = 0.01; double fraction = move_group_interface.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0); move_group_interface.execute(trajectory); return true; } int main(int argc, char **argv) { ...... joint_move(move_group_interface, {0, -M_PI /2, M_PI / 3, -M_PI / 3, M_PI / 2, 0}, joint_model_group); geometry_msgs::PoseStamped current_pose = move_group_interface.getCurrentPose(); std::vector<geometry_msgs::Pose> waypoints; for(int i=0;i<30;i++) { current_pose.pose.position.z -= 0.01; waypoints.push_back(current_pose.pose); } cart_move_with_waypoints(move_group_interface, waypoints); }
运动效果如下:
四、总结
关于Moveit的运动规划,还有很多有意思的地方,小鱼将分几期慢慢和大家分享,敬请期待~
-
@小鱼 第一个不是普通的规划吗?第二个才是笛卡尔直线规划