Fixed minor bug in MoveWaypoints