Finished testing the trajectory objective function. Numeric differentiation tests on both the gradient and the Hessian pass for some simple quadratic intermediate cost term potentials and intermediate derivative norm terms.