System constraints

Replaces stateConstraint method logic with a list of constraint objects for each DrakeSystem.  These can handled more elegantly (preserving the structure of the constraints, and avoiding function handles) by the optimization classes.  

Taking another crack at finishing and merging this (just had a rough time bringing it back in line w/ master).  

I am not sure if we should seal these methods. The state constraints in DrakeSystem do not accept any 'data_ind', like the cached kinsol. I think we could overide these methods in the derived class, so that they can accept `data_ind` as a function input argument.