Started work on trajectory generation