Improvements on moveToWaypoint()