#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);}