Double Stance Trajectory now works

I fixed a few small bugs in the automatically generated code (still have
a transpose error in the actuation).

The major change here was that I break the dynamical state into two
parts, so that only the states that are variable are passed as decision
variables. (IE. the position of the feel is not passed through GPOPS,
but rather passed as a constant).