![]() | 1 | initial version |
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);