00001
00002
00003
00004
00005
00006
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
00198
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
00240
00241
00242
00243
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
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
