getcapability.cc

The Little Green BATS is the first and so far only Dutch team in the 3D simulation league. We are a

#include "walkstraightto.ih"

Behavior::ConfidenceInterval WalkStraightTo::getCapability(rf<Behavior::State> s, rf<Behavior::Goal> g)
{
  Vector3D pos = g->getVector("Pos");
  pos.setZ(0);
  Vector3D straight(1, 0, 0);
  
  double angleToPos = pos.angle(straight) / M_PI * 180;
  
  //double minAngle = 10.0;
  //double maxAngle = 20.0;
  double angle = 10.0;

  /*if (pos.length() < 2){
  	angle = maxAngle - (pos.length() * ((maxAngle - minAngle) / 5));
  }*/

  //WorldModel& wm = WorldModel::getInstance();
  
  if (angleToPos > angle || angleToPos < -angle )//|| !wm.onMyFeet())
  {
  	_debugLevel3("Can't walk straight to position");
    return ConfidenceInterval(-1.0, 0.0);
  }
 	_debugLevel4("Capable");
  return ConfidenceInterval(0.9, 0.0);
}