Working commit on traj opt.