Ask Your Question

Revision history [back]

click to hide/show revision 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);