Completed code to generate optimial trajectories with equality constraints