Fix bug in python bindings of InverseDynamicsFormulationAccForce. The methods...

Fix bug in python bindings of InverseDynamicsFormulationAccForce. The methods getContactForces and checkContact were assuming that the contact force vectors were always 12d. This was the case a while ago, but now TSID supports also 3d contact points, so this assumption does no longer hold in general. To fix the bug I have introduced a new method that allows the user to get the contact force vector without knowing its size in advance.

bindings/python/formulations/formulation.hpp

@@ -124,13 +124,13 @@ namespace tsid
         return self.getContactForces(sol);
       }
       static bool checkContact(T& self, const std::string & name,  const solvers::HQPOutput & sol){
-          Eigen::VectorXd f(12);
-          return self.getContactForces(name, sol, f);
+          tsid::math::Vector f = self.getContactForces(name, sol);
+          if(f.size()>0)
+              return true;
+          return false;
       }
       static Eigen::VectorXd getContactForce (T & self, const std::string & name, const solvers::HQPOutput & sol){
-          Eigen::VectorXd f(12);
-          self.getContactForces(name, sol, f);
-          return f;
+          return self.getContactForces(name, sol);
       }
 
 

include/tsid/formulations/inverse-dynamics-formulation-acc-force.hpp

@@ -133,6 +133,7 @@ namespace tsid
     const Vector & getActuatorForces(const HQPOutput & sol);
     const Vector & getAccelerations(const HQPOutput & sol);
     const Vector & getContactForces(const HQPOutput & sol);
+    Vector getContactForces(const std::string & name, const HQPOutput & sol);
     bool getContactForces(const std::string & name,
                           const HQPOutput & sol,
                           RefVector f);

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

@@ -455,6 +455,21 @@ const Vector & InverseDynamicsFormulationAccForce::getContactForces(const HQPOut
   return m_f;
 }
 
+Vector InverseDynamicsFormulationAccForce::getContactForces(const std::string & name,
+                                                            const HQPOutput & sol)
+{
+  decodeSolution(sol);
+  for(std::vector<ContactLevel*>::iterator it=m_contacts.begin(); it!=m_contacts.end(); it++)
+  {
+    if((*it)->contact.name()==name)
+    {
+      const int k = (*it)->contact.n_force();
+      return m_f.segment((*it)->index, k);
+    }
+  }
+  return Vector::Zero(0);
+}
+
 bool InverseDynamicsFormulationAccForce::getContactForces(const std::string & name,
                                                           const HQPOutput & sol,
                                                           RefVector f)