Trajectory Optimization

This is not really intended as "ready to accept," but I wanted to start the conversation on trajectory optimization. I added (what I hope is) a pretty flexible framework for direct methods for traj opt, along with some implementations for some first-order integration schemes and direct collocation. The code is far from being speed-optimized, and I think one of the important discussions should be on how to prevent repeated, unnecessary calculations, similar to what Hongkai has with kinsol for inverse kinematics. 

I also added a ConstraintManager class, which is just intended to be a general interface for combining multiple constraints into one. I think this would be a good approach to describe more complex constraints, like support polygons and complementarity constraints.
Agree that the dimensions bit is awkward. The user does not have to manage indices outside of the stacked `[h;x;u]` vector, time indices are automatically handled. Right now, I am requiring that, for constraints, the user specify the *time* index that the constraint applies to (1 for initial state, N for final, etc).

Ideally, a constraint could be added where the function is `[f,df] = fun(h,x,u)` instead of `[f,df] = fun(z)`. From talking with Hongkai, I think I can just do this, instead of using the ugly anonymous functions that I have.

Also, agree that the name should be DirectTrajectoryOptimization, will make that change.