当前位置：首页 > news >正文

笛卡尔路径规划

news 2025/5/17 12:18:08

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

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

OMPL集成了很多路径搜索算法，比如常见的RRT、RRTStar、PRM、EST、SBL、 KPIECE、SyCLOP

等，虽然都可以进行路径搜索，但并不能保证最终搜索出来的路径是我们想要的那种，比如这里小鱼让机械臂向下移30cm，结果机械臂饶了一圈。

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

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

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);
}