An algorithm for replanning