Hello,

I'm following the tutorial of MoveIt to learning the MoveItCpp API in ROS Melodic. Before switching to MoveItCpp API, I followed the demo of of Move Group C++ Interface and implemented the Cartesian path, now I want to make an analog Caresian path by MoveItCpp API.

But I can't find way to plan the path after the successfull return of "computeCartesianPath" which will plan the path simultaneously in Move Group C++ Interface. I tried to iteratively setGoal and plan on each result state returned from "computeCartesianPath", but this looks ungraceful and probably time-cosuming. Is there a way, analog to Move Group C++ interface, to plan such Cartesian path?

Here are my code sippets:

// plan05
// 
// restore the start state
planningComponents->setStartState(*robotStartState);
// set the state to the one which satisfies the constraints of Cartesian
auto startState05 = *(moveitCppPtr->getCurrentState());
geometry_msgs::Pose startPose05;
if (dof > 6)
{
    // DOF 7
    startPose05.orientation.w = 1.0;
}
else
{
    // DOF 6
    startPose05.orientation = robotStartPose.orientation;
}
startPose05.position.x = paramStartPose05[0];
startPose05.position.y = paramStartPose05[1];
startPose05.position.z = paramStartPose05[2];
if (startState05.setFromIK(jointModelGroupPtr, startPose05, paramIkTimeout05))
{
    planningComponents->setStartState(startState05);

    // organize waypoints
    EigenSTL::vector_Isometry3d waypoints;
    Eigen::Isometry3d eigenPose;
    tf::poseMsgToEigen(startPose05, eigenPose);
    waypoints.push_back(eigenPose);

    geometry_msgs::Pose wayPose = startPose05;
    // up and back
    for (std::size_t i = 0; i < paramPoseIndex01.size(); ++i)
    {
        if (paramPoseIndex01[i] == "x")
        {
            wayPose.position.x += paramPoseDelta01[i];
        }
        else if (paramPoseIndex01[i] == "y")
        {
            wayPose.position.y += paramPoseDelta01[i];
        }
        else
        {
            wayPose.position.z += paramPoseDelta01[i];
        }
    }
    tf::poseMsgToEigen(wayPose, eigenPose);
    waypoints.push_back(eigenPose);
    // left
    for (std::size_t i = 0; i < paramPoseIndex02.size(); ++i)
    {
        if (paramPoseIndex02[i] == "x")
        {
            wayPose.position.x += paramPoseDelta02[i];
        }
        else if (paramPoseIndex02[i] == "y")
        {
            wayPose.position.y += paramPoseDelta02[i];
        }
        else
        {
            wayPose.position.z += paramPoseDelta02[i];
        }
    }
    tf::poseMsgToEigen(wayPose, eigenPose);
    waypoints.push_back(eigenPose);
    // down and right
    for (std::size_t i = 0; i < paramPoseIndex03.size(); ++i)
    {
        if (paramPoseIndex03[i] == "x")
        {
            wayPose.position.x += paramPoseDelta03[i];
        }
        else if (paramPoseIndex03[i] == "y")
        {
            wayPose.position.y += paramPoseDelta03[i];
        }
        else
        {
            wayPose.position.z += paramPoseDelta03[i];
        }
    }
    tf::poseMsgToEigen(wayPose, eigenPose);
    waypoints.push_back(eigenPose);

    std::vector<moveit::core::RobotStatePtr> resTraj;
    double fraction = startState05.computeCartesianPath(jointModelGroupPtr, resTraj, jointModelGroupPtr->getLinkModel(paramVisualLink), waypoints,
        true, paramEndEffectorStep, paramJumpThreshold);
    ROS_INFO_NAMED("demo", "visualizing plan 05 (Cartesian path) (%.2f%% achieved)", fraction * 100.0);

    visualTools.publishText(textPose, "Cartesian path", rvt::WHITE, rvt::XLARGE);
    for (std::size_t i = 0; i < waypoints.size(); ++i)
    {
        visualTools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
    }
    visualTools.trigger();

    for (auto it : resTraj)
    {
        planningComponents->setGoal(*it);
        auto planSolution05 = planningComponents->plan();
        // set the previous state to current
        planningComponents->setStartState(*it);

        std::this_thread::sleep_for(std::chrono::milliseconds(200));
    }
}
else
{
    visualTools.publishText(textPose, "failed to get the IK from pre-set start state", rvt::WHITE, rvt::XLARGE);
    visualTools.publishAxisLabeled(startPose05, "target_pose");
    visualTools.trigger();
}

// start the next plan
visualTools.deleteAllMarkers();
visualTools.prompt("Press 'next' to continue with plan 06");

iteratively setGoal and plan behavior:

Thanks for your insights!