no stop during replanning | new action set | refactored collision check for whole trajectory