Added nonlinear proportional gains on CoM