Plan first and then act when stepping through waypoints