Ejemplo n.º 1
0
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);
}
Ejemplo n.º 2
0
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();
}