Added nonlinear impedences on QP controller