got trajectory generation working