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.
If we want to ignore the type check for fast speed, then this method should be not public, change it to protected?