This should be checked. Probably we should keep the velocities of the first waypoint.