Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Getting final robot state from a moveit plan

I want to plan two consecutive motions on a move group. How can I do that without executing the first plan. Not that I cannot put the first goal as viapoint because I need to execute some other actions unrelated to the robot.

On the tutorials I also saw something like this:

   start_state.setFromIK(joint_model_group, start_pose2);
   move_group.setStartState(start_state);

But obviously this cannot guarantee continuous joint motion if there are mutiple IK solutions, I think.

My current code is this:

    moveit::planning_interface::MoveGroupInterface::Plan plan1;
    move_group.setPoseTarget(pose1);
    bool success = (move_group.plan(plan1) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (!success) {
        ROS_ERROR_STREAM("plan1 failed");
        return false;
    }

    moveit::planning_interface::MoveGroupInterface::Plan plan2;
    /*
    I want to get endState of plan1 here, something like:
    move_group.setStartState(plan1.getEndState());
    */
    move_group.setPoseTarget(pose2);
    bool success = (move_group.plan(plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
    if (!success) {
        ROS_ERROR_STREAM("plan2 failed");
        return false;
    }