笛卡尔路径规划

上面的是机械臂通过笛卡尔空间运动，向下运动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);
}