Modules/BehaviorControl/CommonXabsl2Symbols/RobotPoseSymbols.cpp

Go to the documentation of this file.

/** 
* @file RobotPoseSymbols.cpp
*
* Implementation of class RobotPoseSymbols.
*
* @author Martin Lötzsch
*/

#include "RobotPoseSymbols.h"
#include "Platform/SystemCall.h"
#include "Tools/Math/Geometry.h"
#include "Tools/FieldDimensions.h"
#include "Tools/Debugging/DebugDrawings.h"

RobotPoseSymbols::RobotPoseSymbols(const BehaviorControlInterfaces& interfaces)
: BehaviorControlInterfaces(interfaces)
{
}


void RobotPoseSymbols::registerSymbols(Xabsl2Engine& engine)
{
  engine.registerDecimalInputSymbol("robot-pose.x",&robotPose.translation.x);
  engine.registerDecimalInputSymbol("robot-pose.y",&robotPose.translation.y);
  engine.registerDecimalInputSymbol("robot-pose.angle",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getAngle);
  engine.registerDecimalInputSymbol("robot-pose.validity",&robotPose.getValidity());
  
  engine.registerDecimalInputSymbol("robot-pose.distance-to-own-goal",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getDistanceToOwnGoal);
  engine.registerDecimalInputSymbol("robot-pose.distance-to-own-penalty-area",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getDistanceToOwnPenaltyArea);
  engine.registerDecimalInputSymbol("robot-pose.distance-to-opponent-goal",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getDistanceToOpponentGoal);


  engine.registerDecimalInputSymbol("defensive-supporter.robot-pose.y",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getDefensiveSupporterRobotPoseY);
 
  engine.registerDecimalInputSymbol("striker.robot-pose.y",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getStrikerRobotPoseY);

  engine.registerDecimalInputSymbol("robot-pose.distance-to-border",&robotPose.distanceToBorder);

  engine.registerDecimalInputSymbol("robot-pose.angle-to-border",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getAngleToBorder);

  
  engine.registerDecimalInputSymbol("robot-pose.goalie-defend-pos-x",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendPosX);
  engine.registerDecimalInputSymbol("robot-pose.goalie-defend-pos-y",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendPosY);
  engine.registerDecimalInputSymbol("robot-pose.goalie-defend-angle",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendAngle);
  engine.registerDecimalInputSymbol("robot-pose.goalie-defend-step-pos-x",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendStepPosX);
  engine.registerDecimalInputSymbol("robot-pose.goalie-defend-step-pos-y",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendStepPosY);
  engine.registerDecimalInputSymbol("robot-pose.goalie-defend-step-angle",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendStepAngle);
  engine.registerDecimalInputFunction("robot-pose.update-goalie-defend-position",this,
    (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::updateGoalieDefendPosition);
  engine.registerDecimalInputFunctionParameter("robot-pose.update-goalie-defend-position",
    "robot-pose.update-goalie-defend-position.ball-pos-x",RobotPoseSymbols::ballPosX);
  engine.registerDecimalInputFunctionParameter("robot-pose.update-goalie-defend-position",
    "robot-pose.update-goalie-defend-position.ball-pos-y",RobotPoseSymbols::ballPosY);
}

void RobotPoseSymbols::update()
{
}

double RobotPoseSymbols::getAngle()
{
  return toDegrees(robotPose.rotation);
}

double RobotPoseSymbols::getAngleToBorder()
{
  return toDegrees(robotPose.angleToBorder);
}


double RobotPoseSymbols::getDistanceToOwnGoal()
{
  return Geometry::distanceTo(robotPose,
    Vector2<double>(xPosOwnGroundline,yPosCenterGoal));
}

double RobotPoseSymbols::getDistanceToOwnPenaltyArea()
{
  return FieldDimensions::distanceToOwnPenaltyArea(robotPose.translation);
}

double RobotPoseSymbols::getDistanceToOpponentGoal()
{
  return Geometry::distanceTo(robotPose,
    Vector2<double>(xPosOpponentGroundline,yPosCenterGoal));
}

double RobotPoseSymbols::getDefensiveSupporterRobotPoseY()
{
  for (int i=0; i<3;i++)
  {
    if (teamMessageCollection[i].isActual())
    {
      if (teamMessageCollection[i].behaviorTeamMessage.dynamicRole 
        == BehaviorTeamMessage::defensiveSupporter)
      {
        return teamMessageCollection[i].robotPose.translation.y;
      }
    }
  }
  
  return -1;
}

double RobotPoseSymbols::getStrikerRobotPoseY()
{
  for (int i=0; i<3;i++)
  {
    if (teamMessageCollection[i].isActual())
    {
      if (teamMessageCollection[i].behaviorTeamMessage.dynamicRole 
        == BehaviorTeamMessage::striker)
      {
        return teamMessageCollection[i].robotPose.translation.y;
      }
    }
  }
  
  return -1;
}

double RobotPoseSymbols::getGoalieDefendAngle()
{
  return RobotPoseSymbols::goalieDefendAngle;
}

double RobotPoseSymbols::getGoalieDefendPosX()
{
  return RobotPoseSymbols::goalieDefendPositionX;
}

double RobotPoseSymbols::getGoalieDefendPosY()
{
  return RobotPoseSymbols::goalieDefendPositionY;
}

double RobotPoseSymbols::getGoalieDefendStepPosX()
{
  return RobotPoseSymbols::goalieDefendStepPosX;
}

double RobotPoseSymbols::getGoalieDefendStepPosY()
{
  return RobotPoseSymbols::goalieDefendStepPosY;
}

double RobotPoseSymbols::getGoalieDefendStepAngle()
{
  return RobotPoseSymbols::goalieDefendStepAngle;
}

double RobotPoseSymbols::updateGoalieDefendPosition()
{
  RobotPoseSymbols::goalieDefendRadiusMax = 800;
  const RobotDimensions& rD = getRobotConfiguration().getRobotDimensions();
  const double degree90 = fromDegrees(90);
  const double degree180 = fromDegrees(180);
  double alpha = 0;
  double beta = 0;
  double gamma = 0;
  double teta = 0;
  double r1 = 0;
  double r2 = 0;
  double r = 0;
  double distX = -xPosOwnGroundline + RobotPoseSymbols::ballPosX;
  double halfSW = rD.shoulderWidth / 2;

  //toDegrees(double angle);
  //fromDegrees(double degrees);
  
  if( RobotPoseSymbols::ballPosY < 0.0 ) {
    
    alpha = atan2( RobotPoseSymbols::ballPosY , distX );
    gamma = atan2( distX , (yPosLeftPenaltyArea - RobotPoseSymbols::ballPosY) );
    if( RobotPoseSymbols::ballPosY > yPosRightPenaltyArea )
      beta = atan2( distX , (yPosLeftPenaltyArea + RobotPoseSymbols::ballPosY) );
    else if( RobotPoseSymbols::ballPosY < yPosRightPenaltyArea)
      beta = atan2( (yPosRightPenaltyArea - RobotPoseSymbols::ballPosY) , distX ) + degree90;
    else
      beta = degree90;
    teta = (degree180 - gamma - beta) / 2;
    
    r1 = halfSW / tan( teta );
    r = distX / sin( degree90 + alpha );
    r2 = r - r1;
    
  } else if( RobotPoseSymbols::ballPosY > 0.0 ){
    
    alpha = atan( RobotPoseSymbols::ballPosY / distX );
    beta = atan( distX / (yPosLeftPenaltyArea + RobotPoseSymbols::ballPosY) );
    if( RobotPoseSymbols::ballPosY < yPosLeftPenaltyArea )
      gamma = atan2( distX , (yPosLeftPenaltyArea - RobotPoseSymbols::ballPosY) );
    else if( RobotPoseSymbols::ballPosY > yPosLeftPenaltyArea)
      gamma = atan2( (yPosRightPenaltyArea + RobotPoseSymbols::ballPosY) , distX ) + degree90;
    else
      gamma = degree90;
    teta = (degree180 - gamma - beta) / 2;
    
    r1 = halfSW / tan( teta );
    r = distX / sin( degree90 - alpha );
    r2 = r - r1;
    
  } else if( RobotPoseSymbols::ballPosY == 0.0 ) {
    
    alpha = 0.0;
    teta = atan2( yPosLeftPenaltyArea , distX );
    r1 = halfSW / tan( teta );
  }
  
  /*if(toDegrees(alpha) < -70)
    RobotPoseSymbols::computeGoalieDefendMinPos(-degree90, rD);
  else if(toDegrees(alpha) > 70)
    RobotPoseSymbols::computeGoalieDefendMinPos(degree90, rD);
  else*/
    RobotPoseSymbols::computeGoalieDefendMinPos(alpha, rD);
  
  RobotPoseSymbols::goalieDefendRadius = r2;
  
  if( r2 < RobotPoseSymbols::goalieDefendRadiusMin ) {
    
    RobotPoseSymbols::goalieDefendPositionX = RobotPoseSymbols::goalieDefendMinPosX;
    RobotPoseSymbols::goalieDefendPositionY = RobotPoseSymbols::goalieDefendMinPosY;
    
  } else if( r2 > RobotPoseSymbols::goalieDefendRadiusMax ) {
    
    if( RobotPoseSymbols::ballPosY != 0.0 ) {
      RobotPoseSymbols::goalieDefendPositionX = xPosOwnGroundline + cos( alpha ) * RobotPoseSymbols::goalieDefendRadiusMax;
      RobotPoseSymbols::goalieDefendPositionY = sin( alpha ) * RobotPoseSymbols::goalieDefendRadiusMax;
    } else {
      RobotPoseSymbols::goalieDefendPositionX = xPosOwnGroundline + RobotPoseSymbols::goalieDefendRadiusMax;
      RobotPoseSymbols::goalieDefendPositionY = 0.0;
    }
    
  } else {
    
    if( RobotPoseSymbols::ballPosY != 0.0 ) {
      RobotPoseSymbols::goalieDefendPositionX = xPosOwnGroundline + cos( alpha ) * r2;
      RobotPoseSymbols::goalieDefendPositionY = sin( alpha ) * r2;
    } else {
      RobotPoseSymbols::goalieDefendMinPosX = xPosOwnGroundline + distX - r1;
      RobotPoseSymbols::goalieDefendMinPosY = 0.0;
    }
  }

  RobotPoseSymbols::goalieDefendAngle = toDegrees(alpha);
  //RobotPoseSymbols::goalieDebugAngleAlpha = RobotPoseSymbols::goalieDefendAngle;
  if(RobotPoseSymbols::goalieDefendAngle < -70)
    RobotPoseSymbols::goalieDefendAngle = -90;
  else if(RobotPoseSymbols::goalieDefendAngle > 70)
    RobotPoseSymbols::goalieDefendAngle = 90;

  double diffX = fabs( RobotPoseSymbols::goalieDefendPositionX - robotPose.translation.x );
  double diffY = fabs( RobotPoseSymbols::goalieDefendPositionY - robotPose.translation.y );
  double angle = toDegrees(robotPose.rotation);
  double diffA = fabs( angle - RobotPoseSymbols::goalieDefendAngle );

  double ratio = 0;
  if( diffX > 100 ) {

    ratio = 100 / diffX;
    if( RobotPoseSymbols::goalieDefendPositionX < robotPose.translation.x )
      RobotPoseSymbols::goalieDefendStepPosX = robotPose.translation.x - 100;
    else
      RobotPoseSymbols::goalieDefendStepPosX = robotPose.translation.x + 100;

  } else {
    ratio = 1.0;
    RobotPoseSymbols::goalieDefendStepPosX = RobotPoseSymbols::goalieDefendPositionX;
  }

  if( diffY > 100 ) {

    ratio += 100 / diffY;
    if( RobotPoseSymbols::goalieDefendPositionY < robotPose.translation.y )
      RobotPoseSymbols::goalieDefendStepPosY = robotPose.translation.y - 100;
    else
      RobotPoseSymbols::goalieDefendStepPosY = robotPose.translation.y + 100;

  } else {
    ratio += 1.0;
    RobotPoseSymbols::goalieDefendStepPosY = RobotPoseSymbols::goalieDefendPositionY;
  }

  if( RobotPoseSymbols::goalieDefendAngle < angle ) 
    RobotPoseSymbols::goalieDefendStepAngle = angle - ratio / 2 * diffA;
  else
    RobotPoseSymbols::goalieDefendStepAngle = angle + ratio / 2 * diffA;
  

  return RobotPoseSymbols::goalieDefendPositionX;
}

void RobotPoseSymbols::computeGoalieDefendMinPos(const double& alpha, const RobotDimensions& rD)
{
  const double degree90 = fromDegrees(90);
  const double degree45 = fromDegrees(45);
  double halfSW = rD.shoulderWidth / 2;
  
  if(alpha == 0.0) {
    RobotPoseSymbols::goalieDefendRadiusMin = halfSW + rD.bodyWidth;
    RobotPoseSymbols::goalieDefendMinPosX = xPosOwnGroundline + RobotPoseSymbols::goalieDefendRadiusMin;
    RobotPoseSymbols::goalieDefendMinPosY = 0.0;
  } else if(alpha < -degree45) {
    RobotPoseSymbols::goalieDefendRadiusMin = cos( degree90 + alpha) * yPosLeftPenaltyArea;
    RobotPoseSymbols::goalieDefendMinPosX = xPosOwnGroundline + sin( degree90 + alpha) * yPosLeftPenaltyArea + halfSW;
    RobotPoseSymbols::goalieDefendMinPosY = yPosRightPenaltyArea;
  } else if(alpha > degree45) {
    RobotPoseSymbols::goalieDefendRadiusMin = cos( degree90 - alpha) * yPosLeftPenaltyArea;
    RobotPoseSymbols::goalieDefendMinPosX = xPosOwnGroundline + sin( degree90 - alpha) * yPosLeftPenaltyArea + halfSW;
    RobotPoseSymbols::goalieDefendMinPosY = yPosLeftPenaltyArea;
  } else {
    double r0 = halfSW + rD.bodyWidth;
    RobotPoseSymbols::goalieDefendRadiusMin = r0 / cos( alpha );
    RobotPoseSymbols::goalieDefendMinPosX = xPosOwnGroundline + r0;
    RobotPoseSymbols::goalieDefendMinPosY = sin( alpha ) * r0;
  }
}

/*
* Change Log
* 
* $Log: RobotPoseSymbols.cpp,v $
* Revision 1.7  2004/06/22 18:48:56  juengel
* kickAngles clean up
*
* Revision 1.6  2004/06/16 23:09:17  juengel
* Added angle-to-center-of-field.
*
* Revision 1.5  2004/06/16 17:07:33  cesarz
* Moved body PSD calculations
*
* Revision 1.4  2004/06/02 17:18:23  spranger
* MotionRequest cleanup
*
* Revision 1.3  2004/05/26 18:56:41  loetzsch
* clean up in the behavior control interfaces
*
* Revision 1.2  2004/05/22 20:41:41  juengel
* Renamed ballP_osition to ballModel.
*
* Revision 1.1.1.1  2004/05/22 17:17:01  cvsadm
* created new repository GT2004_WM
*
* Revision 1.18  2004/05/04 16:24:25  juengel
* angleToPointBehindOpponentGoal  is not vision-based any more
*
* Revision 1.17  2004/05/04 10:48:58  loetzsch
* replaced all enums
* xxxBehaviorControlTimeAfterWhichCommunicatedBallsAreAccepted
* by
* behaviorControlTimeAfterWhichCommunicatedBallsAreAccepted
* (this mechanism was neither fully implemented nor used)
*
* Revision 1.16  2004/04/08 15:33:01  wachter
* GT04 checkin of Microsoft-Hellounds
*
* Revision 1.15  2004/04/05 17:56:47  loetzsch
* merged the local German Open CVS of the aibo team humboldt with the tamara CVS
*
* Revision 1.2  2004/04/04 01:16:13  jumped
* Added angle-to-point-behind-opponent-goal.
*
* Revision 1.1.1.1  2004/03/31 11:16:42  loetzsch
* created ATH repository for german open 2004
*
* Revision 1.14  2004/03/27 16:16:37  juengel
* Added distanceToBorder and angleToBorder.
*
* Revision 1.13  2004/03/22 20:22:35  kerdels
* adjusted threshold
*
* Revision 1.12  2004/03/17 16:31:20  kerdels
* added boolean input symbol "robot-pose.something-in-front-of-chest" utilizing the chest distance sensor
*
* Revision 1.11  2004/03/08 00:58:57  roefer
* Interfaces should be const
*
* Revision 1.10  2004/03/01 17:14:02  kerdels
* added robot-pose.distance-to-opponent-goal,
* moved robot-pose.angle-to-teammate1-3 to MSH2004StrategySymbols as fieldpos.angle-to-teammate1-3,
* added fieldpos.distance-to-teammate1-3,
* added DTT-Options newKickToGoal, newKickToClear, newKickToTeammate1-3,
* added DTT-OptionClass newKickToTeammate,
* added kickToPosRating function in DefaultOptionRating
*
* Revision 1.9  2004/02/28 12:50:15  rempe
* some changes
*
* Revision 1.8  2004/02/27 10:45:17  spranger
* moved get-Angle-to-Teammate function and parameter to ATH2004StrategySymbols
*
* Revision 1.7  2004/02/25 13:49:12  spranger
* removed OUTPUTS
*
* Revision 1.6  2004/02/25 13:45:23  spranger
* added function getAngleToTeammate and Xabsl input-function get-angle-to-teammate
*
* Revision 1.5  2004/02/19 17:26:43  kerdels
* modified kickAngle to Teammates like suggested by Walter
*
* Revision 1.4  2004/02/16 00:54:47  rempe
* symbols for new goalie
*
* Revision 1.3  2004/01/27 14:28:14  lohmann
* getPenaltyPointAngle added (for bananaCross)
*
* Revision 1.2  2004/01/07 23:17:01  kerdels
* added symbols:
* robot-pose.angle-to-teammate1
* robot-pose.angle-to-teammate2
* robot-pose.angle-to-teammate3
*
* Revision 1.1  2003/10/22 22:18:44  loetzsch
* prepared the cloning of the GT2003BehaviorControl
*
* Revision 1.1  2003/10/06 13:39:29  cvsadm
* Created GT2004 (M.J.)
*
* Revision 1.9  2003/09/01 10:20:11  juengel
* DebugDrawings clean-up 2
* DebugImages clean-up
* MessageIDs clean-up
* Stopwatch clean-up
*
* Revision 1.8  2003/07/07 15:12:55  juengel
* Changed bestAngleAwayFromOwnGoal near borders
*
* Revision 1.7  2003/07/06 02:14:12  juengel
* goalKickAngle is as bestAngleAwayFromOwnGoal if robot looks to own goal.
*
* Revision 1.6  2003/07/06 00:18:31  juengel
* changed condition for goalKickAngle
*
* Revision 1.5  2003/07/05 23:57:09  juengel
* Added best-angle-away-from-own-goal and best-angle-away-from-own-goal-no-obstacles.
*
* Revision 1.4  2003/07/04 12:25:28  juengel
* Added bestAngleToOpponentGoalNoObstacles.
*
* Revision 1.3  2003/07/03 23:11:20  juengel
* active passing
*
* Revision 1.2  2003/07/03 10:50:07  juengel
* Added Drawing kickAnglesRadar.
*
* Revision 1.1.1.1  2003/07/02 09:40:23  cvsadm
* created new repository for the competitions in Padova from the 
* tamara CVS (Tuesday 2:00 pm)
*
* removed unused solutions
*
* Revision 1.17  2003/06/27 15:10:53  juengel
* goal-kick-angle uses getAngleOfLargeGapInRange2.
*
* Revision 1.16  2003/06/26 12:36:26  juengel
* goal-kick-angle improved.
*
* Revision 1.15  2003/06/24 09:38:01  risler
* bug fix: goal kick angle should be relative
*
* Revision 1.14  2003/06/22 10:55:01  juengel
* Fixed bug in calculation of bestAngleToOpponentGoal.
*
* Revision 1.13  2003/06/20 21:43:52  juengel
* New version of best angle to opponent goal.
*
* Revision 1.12  2003/06/20 20:15:24  juengel
* Renamed some methods in ObstaclesModel.
*
* Revision 1.11  2003/06/19 19:51:07  juengel
* Obstacles are used to determine bestAngleToOpponentGoal.
*
* Revision 1.10  2003/06/19 12:11:17  juengel
* Changed calculation of goal-kick-angle.
*
* Revision 1.9  2003/06/18 18:31:49  juengel
* goal-kick-angle is calculated
*
* Revision 1.8  2003/06/18 13:47:36  loetzsch
* added input symbol "goalie.kick-when-stuck"
*
* Revision 1.7  2003/06/18 09:12:47  loetzsch
* added symbol "striker.robot-pose.y"
*
* Revision 1.6  2003/06/17 18:28:45  thomas
* added: goalie cont-behaviours, return-state, etc.
*
* Revision 1.5  2003/06/16 20:01:23  loetzsch
* added "defensive-supporter.robot-pose.y"
*
* Revision 1.4  2003/06/05 18:10:02  juengel
* Added angle-to-next-free-teammate.
*
* Revision 1.3  2003/05/26 13:07:00  juengel
* Moved angleToFreePartOfGoal from specialPercept to obstaclesPercept.
*
* Revision 1.2  2003/05/26 08:30:12  juengel
* Moved angleToFreePartOfGoal from specialPercept to obstaclesPercept.
*
* Revision 1.1  2003/05/06 16:03:05  loetzsch
* added class RobotPoseSymbols
*
* Revision 1.1  2003/05/04 11:41:40  loetzsch
* added class RobotPoseSymbols
*
*/

---