Adds trajectory optimization script.  Working for relatively simple trajectories.