Added Actuator^2 cost function, seems to work

Things seem to work in single stance. Still does not like to optimize
power, but it at least will converge now.