bool ConstraintChecker::HasSupportPolygon(const Robot& robot,const Stance& stance,const Vector3& gravity,int numFCEdges) { vector<ContactPoint> cps(NumContactPoints(stance)); int k=0; for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) { const Hold& h=i->second; for(size_t j=0;j<h.contacts.size();j++,k++) cps[k]=h.contacts[j]; } Vector3 com = robot.GetCOM(); vector<Vector3> f; return TestCOMEquilibrium(cps,gravity,numFCEdges,com,f); }
bool ConstraintChecker::HasSupportPolygon_Robust(const Robot& robot,const Stance& stance,const Vector3& gravity,Real robustnessFactor,int numFCEdges) { vector<ContactPoint> cps(NumContactPoints(stance)); int k=0; for(Stance::const_iterator i=stance.begin();i!=stance.end();i++) { const Hold& h=i->second; for(size_t j=0;j<h.contacts.size();j++,k++) cps[k]=h.contacts[j]; } Vector3 com = robot.GetCOM(); EquilibriumTester eq; eq.Setup(cps,gravity,numFCEdges,com); eq.SetRobustnessFactor(robustnessFactor); return eq.TestCurrent(); }