Initial work on Lumped Actuation biped model

Most of the code is written, but there is a bug somewhere. It should be
something simple - one of the matricies that is getting passed to gpops
has the wrong dimensions.