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.
The user should not have to understand the dimensions of z, etc.  
It seems like it should be easy to add a set of methods to the TrajectoryOptimization class (which, I think, should probably be renamed to DirectTrajectoryOptimization?) for addCost, addFinalCost, addStateConstraint, addFinalStateConstraint, and for initial constraints, etc.  And that class should take the burden of keeping track of the indices... right?