Handle properly the QP failures when computing contact wrenches

[nunoguedelha]

The QP solver computing the contact wrenches may fail sometimes due to a badly conditioned matrix in the QP problem formulation, due to the mass matrix and/or Jacobians, which can occur when the robot has a singular joint configuration. This has been typically occurring after the robot falls down and sinks into the ground (expected behavior) until the feet are completely flipped over and the robot is upside down.

For better handling the QP failure, we could allow a given number of consecutive failures while using the last valid QP result, and stopping the simulation if that max number is reached.

[nunoguedelha]

@VenusPasandi @CarlottaSartore