Commit 5f7453c5 authored by student's avatar student
Browse files

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.
parent f4f96456
......@@ -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);
}
......
......@@ -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);
......
......@@ -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)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment