https://123dok.net/document/yngnoe1z-a-two-stage-suboptimal-approximation-for-variable-compliance-and-torque-control.html