Fix removeRigidContact

[EtienneAr]

Hello,

I noticed an error while using rigid contact, the number of equalities is decremented no matter what when removeRigidContact is called.

tsid/src/formulations/inverse-dynamics-formulation-acc-force.cpp

Line 559 in 6e0293b

m_eq -= (*it)->motionConstraint->rows();

However, is the motion task has been added with a priority > 0, it shouldn't affect the number of equalities

tsid/src/formulations/inverse-dynamics-formulation-acc-force.cpp

Line 82 in 6e0293b

if (priorityLevel == 0) m_eq += c.rows();

So nEq was underflowing in our walking scenario where we constantly break and make contact.

Here is a small fix

[andreadelprete]

Thanks for your contribution @EtienneAr . The code looks good to me. I wait to see if somebody else has something to say, otherwise I'll move on with a merge.