Fix bug in InverseDynamicsFormulationAccForce: when adding a contact, the...

Fix bug in InverseDynamicsFormulationAccForce: when adding a contact, the number of equality constraints was incremented even if the motion constraints were relaxed to the cost function.

demo/demo_quadruped.py

 import pinocchio as se3
-from pinocchio import libpinocchio_pywrap as pin
+import pinocchio as pin
+#from pinocchio import libpinocchio_pywrap as pin
 import tsid
 import numpy as np
 from numpy import nan
@@ -112,6 +113,8 @@ sampleCom = trajCom.computeNext()
 q_ref = q[7:]
 trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
 
+print("Create QP solver with ", invdyn.nVar, " variables, ", invdyn.nEq, 
+      " equality and ", invdyn.nIn, " inequality constraints")
 solver = tsid.SolverHQuadProgFast("qp solver")
 solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
 

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

@@ -239,7 +239,8 @@ bool InverseDynamicsFormulationAccForce::addRigidContact(ContactBase & contact,
   cl->forceRegTask = std::make_shared<ConstraintEquality>(contact.name()+"_force_reg_task", forceRegConstr.rows(), m_v+m_k);
   m_hqpData[1].push_back(solvers::make_pair<double, std::shared_ptr<ConstraintBase> >(force_regularization_weight, cl->forceRegTask));
 
-  m_eq += motionConstr.rows();
+  if(motionPriorityLevel==0)
+    m_eq += motionConstr.rows();
   m_in += forceConstr.rows();
 
   return true;