Refactor for better state/control abstraction

[dfridovi]

Create a unifying interface for states and controls instead of using raw VectorXds. Right now, we'll need states to provide access to positions and their derivatives.