Working on trajectory generation