Restore CoM control

CoM control now seems to work fine, apart from joints 0 and 2 of the
torso, which seems to be swapped in the CoM Jacobian.