Revision history [back]

The following works. (reference)

#include<moveit/robot_trajectory/robot_trajectory.h>
#include<moveit/robot_state/robot_state.h>   

 group.setStartState(*group.getCurrentState());
 group.setPoseTarget(target_pose);
 success = group.plan(plan); 

 moveit::core::RobotModelConstPtr robot_model=group.getRobotModel();
 robot_trajectory::RobotTrajectory trajectory(robot_model, planning_group);
 trajectory.setRobotTrajectoryMsg(*(group.getCurrentState()),plan.trajectory_);

 group.setStartState(*trajectory.getLastWayPointPtr());
 group.setPoseTarget(target_pose2);
 success2 = group.plan(plan2); 

 if(success and success2){
    success = group.execute(plan);
    if(success)
     success2 = group.execute(plan2);
 }