Got walking to work well with torque squared cost function

Does not work with CoT cost function