- fix bug in formulation of inequality constraints
- set force normal direction to Z axis by default
- set friction coefficients and normal force max/min in initialization
- if solver doesn’t find a solution stop the controller
- add active set as output parameter of QP solver