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).  

Agree. We should not need to use cached kinsol so much for state constraints. The case I was thinking is for systems with the four-bar linkage (Atrias), but caching kinsol would not improve the speed by a lot. Since we use kinsol for at most twice in each constraint evaluation (one for each four bar).