Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Modules/BehaviorControl/CommonXabsl2Symbols/RobotPoseSymbols.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file RobotPoseSymbols.cpp
    00003 *
    00004 * Implementation of class RobotPoseSymbols.
    00005 *
    00006 * @author Martin Lötzsch
    00007 */
    00008
    00009 #include "RobotPoseSymbols.h"
    00010 #include "Platform/SystemCall.h"
    00011 #include "Tools/Math/Geometry.h"
    00012 #include "Tools/FieldDimensions.h"
    00013 #include "Tools/Debugging/DebugDrawings.h"
    00014
    00015 RobotPoseSymbols::RobotPoseSymbols(const BehaviorControlInterfaces& interfaces)
    00016 : BehaviorControlInterfaces(interfaces)
    00017 {
    00018 }
    00019
    00020
    00021 void RobotPoseSymbols::registerSymbols(Xabsl2Engine& engine)
    00022 {
    00023   engine.registerDecimalInputSymbol("robot-pose.x",&robotPose.translation.x);
    00024   engine.registerDecimalInputSymbol("robot-pose.y",&robotPose.translation.y);
    00025   engine.registerDecimalInputSymbol("robot-pose.angle",this,
    00026     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getAngle);
    00027   engine.registerDecimalInputSymbol("robot-pose.validity",&robotPose.getValidity());
    00028   engine.registerDecimalInputSymbol("robot-pose.position-variance",&robotPose.getPositionVariance());
    00029   engine.registerDecimalInputSymbol("robot-pose.position-standard-deviation",this,
    00030     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getPositionStandardDeviation);
    00031
    00032   engine.registerDecimalInputSymbol("robot-pose.distance-to-own-goal",this,
    00033     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getDistanceToOwnGoal);
    00034   engine.registerDecimalInputSymbol("robot-pose.distance-to-own-penalty-area",this,
    00035     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getDistanceToOwnPenaltyArea);
    00036   engine.registerDecimalInputSymbol("robot-pose.distance-to-opponent-goal",this,
    00037     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getDistanceToOpponentGoal);
    00038
    00039   engine.registerDecimalInputSymbol("robot-pose.speed-factor-from-standard-deviation",this,
    00040     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::calculateSpeedFactorFromRobotPositionStandardDeviation);
    00041
    00042   engine.registerDecimalInputSymbol("defensive-supporter.robot-pose.y",this,
    00043     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getDefensiveSupporterRobotPoseY);
    00044
    00045   engine.registerDecimalInputSymbol("striker.robot-pose.y",this,
    00046     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getStrikerRobotPoseY);
    00047
    00048   engine.registerDecimalInputSymbol("robot-pose.distance-to-border",this,
    00049     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getDistanceToBorder);
    00050
    00051   engine.registerDecimalInputSymbol("robot-pose.angle-to-border",this,
    00052     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getAngleToBorder);
    00053
    00054
    00055   engine.registerDecimalInputSymbol("robot-pose.goalie-defend-pos-x",this,
    00056     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendPosX);
    00057   engine.registerDecimalInputSymbol("robot-pose.goalie-defend-pos-y",this,
    00058     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendPosY);
    00059   engine.registerDecimalInputSymbol("robot-pose.goalie-defend-angle",this,
    00060     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendAngle);
    00061   engine.registerDecimalInputSymbol("robot-pose.goalie-defend-step-pos-x",this,
    00062     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendStepPosX);
    00063   engine.registerDecimalInputSymbol("robot-pose.goalie-defend-step-pos-y",this,
    00064     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendStepPosY);
    00065   engine.registerDecimalInputSymbol("robot-pose.goalie-defend-step-angle",this,
    00066     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::getGoalieDefendStepAngle);
    00067   engine.registerDecimalInputFunction("robot-pose.update-goalie-defend-position",this,
    00068     (double (Xabsl2FunctionProvider::*)())&RobotPoseSymbols::updateGoalieDefendPosition);
    00069   engine.registerDecimalInputFunctionParameter("robot-pose.update-goalie-defend-position",
    00070     "robot-pose.update-goalie-defend-position.ball-pos-x",RobotPoseSymbols::ballPosX);
    00071   engine.registerDecimalInputFunctionParameter("robot-pose.update-goalie-defend-position",
    00072     "robot-pose.update-goalie-defend-position.ball-pos-y",RobotPoseSymbols::ballPosY);
    00073 }
    00074
    00075 void RobotPoseSymbols::update()
    00076 {
    00077 }
    00078
    00079 double RobotPoseSymbols::getPositionStandardDeviation()
    00080 {
    00081   return robotPose.getPositionStandardDeviation();
    00082 }
    00083
    00084 double RobotPoseSymbols::getAngle()
    00085 {
    00086   return toDegrees(robotPose.rotation);
    00087 }
    00088
    00089 double RobotPoseSymbols::getAngleToBorder()
    00090 {
    00091   return toDegrees(robotPose.angleToBorder);
    00092 }
    00093
    00094 double RobotPoseSymbols::getDistanceToBorder()
    00095 {
    00096   return FieldDimensions::distanceToBorder(robotPose.getPose().translation);
    00097 }
    00098
    00099
    00100 double RobotPoseSymbols::getDistanceToOwnGoal()
    00101 {
    00102   return Geometry::distanceTo(robotPose,
    00103     Vector2<double>(xPosOwnGroundline,yPosCenterGoal));
    00104 }
    00105
    00106 double RobotPoseSymbols::getDistanceToOwnPenaltyArea()
    00107 {
    00108   return FieldDimensions::distanceToOwnPenaltyArea(robotPose.translation);
    00109 }
    00110
    00111 double RobotPoseSymbols::getDistanceToOpponentGoal()
    00112 {
    00113   return Geometry::distanceTo(robotPose,
    00114     Vector2<double>(xPosOpponentGroundline,yPosCenterGoal));
    00115 }
    00116
    00117 double RobotPoseSymbols::getDefensiveSupporterRobotPoseY()
    00118 {
    00119   for (int i=0; i<3;i++)
    00120   {
    00121     if (teamMessageCollection[i].isActual())
    00122     {
    00123       if (teamMessageCollection[i].behaviorTeamMessage.dynamicRole
    00124         == BehaviorTeamMessage::defensiveSupporter)
    00125       {
    00126         return teamMessageCollection[i].robotPose.translation.y;
    00127       }
    00128     }
    00129   }
    00130
    00131   return -1;
    00132 }
    00133
    00134 double RobotPoseSymbols::getStrikerRobotPoseY()
    00135 {
    00136   for (int i=0; i<3;i++)
    00137   {
    00138     if (teamMessageCollection[i].isActual())
    00139     {
    00140       if (teamMessageCollection[i].behaviorTeamMessage.dynamicRole
    00141         == BehaviorTeamMessage::striker)
    00142       {
    00143         return teamMessageCollection[i].robotPose.translation.y;
    00144       }
    00145     }
    00146   }
    00147
    00148   return -1;
    00149 }
    00150
    00151 double RobotPoseSymbols::getGoalieDefendAngle()
    00152 {
    00153   return RobotPoseSymbols::goalieDefendAngle;
    00154 }
    00155
    00156 double RobotPoseSymbols::getGoalieDefendPosX()
    00157 {
    00158   return RobotPoseSymbols::goalieDefendPositionX;
    00159 }
    00160
    00161 double RobotPoseSymbols::getGoalieDefendPosY()
    00162 {
    00163   return RobotPoseSymbols::goalieDefendPositionY;
    00164 }
    00165
    00166 double RobotPoseSymbols::getGoalieDefendStepPosX()
    00167 {
    00168   return RobotPoseSymbols::goalieDefendStepPosX;
    00169 }
    00170
    00171 double RobotPoseSymbols::getGoalieDefendStepPosY()
    00172 {
    00173   return RobotPoseSymbols::goalieDefendStepPosY;
    00174 }
    00175
    00176 double RobotPoseSymbols::getGoalieDefendStepAngle()
    00177 {
    00178   return RobotPoseSymbols::goalieDefendStepAngle;
    00179 }
    00180
    00181 double RobotPoseSymbols::updateGoalieDefendPosition()
    00182 {
    00183   RobotPoseSymbols::goalieDefendRadiusMax = 800;
    00184   const RobotDimensions& rD = getRobotConfiguration().getRobotDimensions();
    00185   const double degree90 = fromDegrees(90);
    00186   const double degree180 = fromDegrees(180);
    00187   double alpha = 0;
    00188   double beta = 0;
    00189   double gamma = 0;
    00190   double teta = 0;
    00191   double r1 = 0;
    00192   double r2 = 0;
    00193   double r = 0;
    00194   double distX = -xPosOwnGroundline + RobotPoseSymbols::ballPosX;
    00195   double halfSW = rD.shoulderWidth / 2;
    00196
    00197   //toDegrees(double angle);
    00198   //fromDegrees(double degrees);
    00199
    00200   if( RobotPoseSymbols::ballPosY < 0.0 ) {
    00201
    00202     alpha = atan2( RobotPoseSymbols::ballPosY , distX );
    00203     gamma = atan2( distX , (yPosLeftPenaltyArea - RobotPoseSymbols::ballPosY) );
    00204     if( RobotPoseSymbols::ballPosY > yPosRightPenaltyArea )
    00205       beta = atan2( distX , (yPosLeftPenaltyArea + RobotPoseSymbols::ballPosY) );
    00206     else if( RobotPoseSymbols::ballPosY < yPosRightPenaltyArea)
    00207       beta = atan2( (yPosRightPenaltyArea - RobotPoseSymbols::ballPosY) , distX ) + degree90;
    00208     else
    00209       beta = degree90;
    00210     teta = (degree180 - gamma - beta) / 2;
    00211
    00212     r1 = halfSW / tan( teta );
    00213     r = distX / sin( degree90 + alpha );
    00214     r2 = r - r1;
    00215
    00216   } else if( RobotPoseSymbols::ballPosY > 0.0 ){
    00217
    00218     alpha = atan( RobotPoseSymbols::ballPosY / distX );
    00219     beta = atan( distX / (yPosLeftPenaltyArea + RobotPoseSymbols::ballPosY) );
    00220     if( RobotPoseSymbols::ballPosY < yPosLeftPenaltyArea )
    00221       gamma = atan2( distX , (yPosLeftPenaltyArea - RobotPoseSymbols::ballPosY) );
    00222     else if( RobotPoseSymbols::ballPosY > yPosLeftPenaltyArea)
    00223       gamma = atan2( (yPosRightPenaltyArea + RobotPoseSymbols::ballPosY) , distX ) + degree90;
    00224     else
    00225       gamma = degree90;
    00226     teta = (degree180 - gamma - beta) / 2;
    00227
    00228     r1 = halfSW / tan( teta );
    00229     r = distX / sin( degree90 - alpha );
    00230     r2 = r - r1;
    00231
    00232   } else if( RobotPoseSymbols::ballPosY == 0.0 ) {
    00233
    00234     alpha = 0.0;
    00235     teta = atan2( yPosLeftPenaltyArea , distX );
    00236     r1 = halfSW / tan( teta );
    00237   }
    00238
    00239   /*if(toDegrees(alpha) < -70)
    00240     RobotPoseSymbols::computeGoalieDefendMinPos(-degree90, rD);
    00241   else if(toDegrees(alpha) > 70)
    00242     RobotPoseSymbols::computeGoalieDefendMinPos(degree90, rD);
    00243   else*/
    00244     RobotPoseSymbols::computeGoalieDefendMinPos(alpha, rD);
    00245
    00246   RobotPoseSymbols::goalieDefendRadius = r2;
    00247
    00248   if( r2 < RobotPoseSymbols::goalieDefendRadiusMin ) {
    00249
    00250     RobotPoseSymbols::goalieDefendPositionX = RobotPoseSymbols::goalieDefendMinPosX;
    00251     RobotPoseSymbols::goalieDefendPositionY = RobotPoseSymbols::goalieDefendMinPosY;
    00252
    00253   } else if( r2 > RobotPoseSymbols::goalieDefendRadiusMax ) {
    00254
    00255     if( RobotPoseSymbols::ballPosY != 0.0 ) {
    00256       RobotPoseSymbols::goalieDefendPositionX = xPosOwnGroundline + cos( alpha ) * RobotPoseSymbols::goalieDefendRadiusMax;
    00257       RobotPoseSymbols::goalieDefendPositionY = sin( alpha ) * RobotPoseSymbols::goalieDefendRadiusMax;
    00258     } else {
    00259       RobotPoseSymbols::goalieDefendPositionX = xPosOwnGroundline + RobotPoseSymbols::goalieDefendRadiusMax;
    00260       RobotPoseSymbols::goalieDefendPositionY = 0.0;
    00261     }
    00262
    00263   } else {
    00264
    00265     if( RobotPoseSymbols::ballPosY != 0.0 ) {
    00266       RobotPoseSymbols::goalieDefendPositionX = xPosOwnGroundline + cos( alpha ) * r2;
    00267       RobotPoseSymbols::goalieDefendPositionY = sin( alpha ) * r2;
    00268     } else {
    00269       RobotPoseSymbols::goalieDefendMinPosX = xPosOwnGroundline + distX - r1;
    00270       RobotPoseSymbols::goalieDefendMinPosY = 0.0;
    00271     }
    00272   }
    00273
    00274   RobotPoseSymbols::goalieDefendAngle = toDegrees(alpha);
    00275   //RobotPoseSymbols::goalieDebugAngleAlpha = RobotPoseSymbols::goalieDefendAngle;
    00276   if(RobotPoseSymbols::goalieDefendAngle < -70)
    00277     RobotPoseSymbols::goalieDefendAngle = -90;
    00278   else if(RobotPoseSymbols::goalieDefendAngle > 70)
    00279     RobotPoseSymbols::goalieDefendAngle = 90;
    00280
    00281   double diffX = fabs( RobotPoseSymbols::goalieDefendPositionX - robotPose.translation.x );
    00282   double diffY = fabs( RobotPoseSymbols::goalieDefendPositionY - robotPose.translation.y );
    00283   double angle = toDegrees(robotPose.rotation);
    00284   double diffA = fabs( angle - RobotPoseSymbols::goalieDefendAngle );
    00285
    00286   double ratio = 0;
    00287   if( diffX > 100 ) {
    00288
    00289     ratio = 100 / diffX;
    00290     if( RobotPoseSymbols::goalieDefendPositionX < robotPose.translation.x )
    00291       RobotPoseSymbols::goalieDefendStepPosX = robotPose.translation.x - 100;
    00292     else
    00293       RobotPoseSymbols::goalieDefendStepPosX = robotPose.translation.x + 100;
    00294
    00295   } else {
    00296     ratio = 1.0;
    00297     RobotPoseSymbols::goalieDefendStepPosX = RobotPoseSymbols::goalieDefendPositionX;
    00298   }
    00299
    00300   if( diffY > 100 ) {
    00301
    00302     ratio += 100 / diffY;
    00303     if( RobotPoseSymbols::goalieDefendPositionY < robotPose.translation.y )
    00304       RobotPoseSymbols::goalieDefendStepPosY = robotPose.translation.y - 100;
    00305     else
    00306       RobotPoseSymbols::goalieDefendStepPosY = robotPose.translation.y + 100;
    00307
    00308   } else {
    00309     ratio += 1.0;
    00310     RobotPoseSymbols::goalieDefendStepPosY = RobotPoseSymbols::goalieDefendPositionY;
    00311   }
    00312
    00313   if( RobotPoseSymbols::goalieDefendAngle < angle )
    00314     RobotPoseSymbols::goalieDefendStepAngle = angle - ratio / 2 * diffA;
    00315   else
    00316     RobotPoseSymbols::goalieDefendStepAngle = angle + ratio / 2 * diffA;
    00317
    00318
    00319   return RobotPoseSymbols::goalieDefendPositionX;
    00320 }
    00321
    00322 double RobotPoseSymbols::calculateSpeedFactorFromRobotPositionStandardDeviation()
    00323 {
    00324   double speedFactor, maxStandardDeviation = 500;
    00325   speedFactor = (maxStandardDeviation*1.3 - robotPose.getPositionStandardDeviation())/maxStandardDeviation;
    00326   if(speedFactor > 1.0) speedFactor = 1.0;
    00327   if(speedFactor < 0.1) speedFactor = 0.1;
    00328   return speedFactor;
    00329 }
    00330
    00331 void RobotPoseSymbols::computeGoalieDefendMinPos(const double& alpha, const RobotDimensions& rD)
    00332 {
    00333   const double degree90 = fromDegrees(90);
    00334   const double degree45 = fromDegrees(45);
    00335   double halfSW = rD.shoulderWidth / 2;
    00336
    00337   if(alpha == 0.0) {
    00338     RobotPoseSymbols::goalieDefendRadiusMin = halfSW + rD.bodyWidth;
    00339     RobotPoseSymbols::goalieDefendMinPosX = xPosOwnGroundline + RobotPoseSymbols::goalieDefendRadiusMin;
    00340     RobotPoseSymbols::goalieDefendMinPosY = 0.0;
    00341   } else if(alpha < -degree45) {
    00342     RobotPoseSymbols::goalieDefendRadiusMin = cos( degree90 + alpha) * yPosLeftPenaltyArea;
    00343     RobotPoseSymbols::goalieDefendMinPosX = xPosOwnGroundline + sin( degree90 + alpha) * yPosLeftPenaltyArea + halfSW;
    00344     RobotPoseSymbols::goalieDefendMinPosY = yPosRightPenaltyArea;
    00345   } else if(alpha > degree45) {
    00346     RobotPoseSymbols::goalieDefendRadiusMin = cos( degree90 - alpha) * yPosLeftPenaltyArea;
    00347     RobotPoseSymbols::goalieDefendMinPosX = xPosOwnGroundline + sin( degree90 - alpha) * yPosLeftPenaltyArea + halfSW;
    00348     RobotPoseSymbols::goalieDefendMinPosY = yPosLeftPenaltyArea;
    00349   } else {
    00350     double r0 = halfSW + rD.bodyWidth;
    00351     RobotPoseSymbols::goalieDefendRadiusMin = r0 / cos( alpha );
    00352     RobotPoseSymbols::goalieDefendMinPosX = xPosOwnGroundline + r0;
    00353     RobotPoseSymbols::goalieDefendMinPosY = sin( alpha ) * r0;
    00354   }
    00355 }
    00356