Revision history [back]

I found the solution after some while. It is not as clean as I would like it to be but here it is.

robot_state::RobotState start_state(*move_group.getCurrentState());

moveit::planning_interface::MoveGroupInterface::Plan plan2;
robot_state::RobotState state(start_state);
const std::vector<double> joints = plan1.trajectory_.joint_trajectory.points.back().positions;
state.setJointGroupPositions(move_group_name, joints);
move_group.setStartState(state);
move_group.setPoseTarget(pose2);