Real time path planning, trajectory generation