Real time trajectory generation and interpolation