Improve transition between spline and waypoint