ensure section trajectory starts at actual position

src/TimedPlanner.cpp

@@ -121,6 +121,7 @@ void TimedPlanner::doMotion() {
             ROS_WARN_STREAM("terminating current trajectory");
             return;
           }
+          group->getCurrentState()->copyJointGroupPositions("panda_arm", trajectory.joint_trajectory.points[0].positions);
           ROS_INFO_STREAM(
               "Moving to the next waypoint with speed factor " << motionSpeedFactor << " in mode " << waypoint.mode);
           TrajectoryUtil::applyMotionSpeedFactor(trajectory, motionSpeedFactor);