Got simple 2-phase walking to work

Uses torque-squared cost function, and does a right-left foot map. Next
steps are to implement 4-phase walking, and then figure out how to get
CoT for the cost function.