Moved code to fill the trajectory goals to own functions.