GT2004: Modules/BehaviorControl/GT2004BehaviorControl/GT2004Symbols/EvolutionSymbols.cpp Source File

00001 /** 
00002 * @file EvolutionSymbols.cpp
00003 *
00004 * Implementation of class EvolutionSymbols.
00005 *
00006 * @author Martin Lötzsch
00007 */
00008 
00009 #include "EvolutionSymbols.h"
00010 #include "Tools/FieldDimensions.h"
00011 #include "Tools/Math/Pose2D.h"
00012 
00013 EvolutionSymbols::EvolutionSymbols(const BehaviorControlInterfaces& interfaces,
00014                                  GT2004ParametersSet& gt2004ParametersSet,
00015                                  Pose2D* gt2004ParametersCalibration,
00016                                  int& gt2004CurrentIndex,
00017                                  Pose2D& measurementRequest,
00018                                  int& evolutionMode):
00019 BehaviorControlInterfaces(interfaces),
00020 gt2004ParametersSet(gt2004ParametersSet),
00021 gt2004ParametersCalibration(gt2004ParametersCalibration),
00022 gt2004CurrentIndex(gt2004CurrentIndex),
00023 measurementRequest(measurementRequest),
00024 evolutionMode(evolutionMode),
00025 timeWhenBallWasStartedToCatch(0), timeUntilBallWasCaught(0),
00026 role(BehaviorTeamMessage::goalie),
00027 estimatedTimeToReachBall(0.0),
00028 timeOfLastGoodSelfLocalization(SystemCall::getCurrentSystemTime())
00029 {
00030 }
00031 
00032 
00033 void EvolutionSymbols::registerSymbols(Xabsl2Engine& engine)
00034 {
00035   // "duration-of-bad-self-localization"
00036   engine.registerDecimalInputSymbol("duration-of-bad-self-localization", &durationOfBadSelfLocalization);
00037   
00038   // "current-gt2004-parameters-start-position.*"
00039   engine.registerDecimalInputSymbol("current-gt2004-parameters-start-position.x",  &currentGT2004ParametersStartPosition.x);
00040   engine.registerDecimalInputSymbol("current-gt2004-parameters-start-position.y",  &currentGT2004ParametersStartPosition.y);
00041   engine.registerDecimalInputSymbol("current-gt2004-parameters-start-position.angle",  &currentGT2004ParametersStartDirection);
00042   // "current-gt2004-parameters-measure-blind"
00043   engine.registerBooleanInputSymbol("current-gt2004-parameters-measure-blind",
00044     this,(bool(Xabsl2FunctionProvider::*)())&EvolutionSymbols::getCurrentGT2004ParametersMeasureBlind);
00045   // "quality-of-current-gt2004-parameters"
00046   engine.registerDecimalInputSymbol("quality-of-current-gt2004-parameters",this,
00047     (double (Xabsl2FunctionProvider::*)())&EvolutionSymbols::getQualityOfCurrentGT2004Parameters);
00048 }
00049 
00050 void EvolutionSymbols::update()
00051 {
00052   if (robotPose.getValidity()>0.5)
00053   {
00054     timeOfLastGoodSelfLocalization=SystemCall::getCurrentSystemTime();
00055   }
00056   durationOfBadSelfLocalization=0.001*SystemCall::getTimeSince(timeOfLastGoodSelfLocalization);
00057   
00058   //calculate how much we will walk in N s (see MotionRequestResult.nb).
00059   //we use a mixture of theory and last measurement:
00060   Pose2D motReq=measurementRequest;
00061   if (gt2004ParametersCalibration[gt2004CurrentIndex].translation.abs()!=0)
00062   {
00063     motReq.translation += gt2004ParametersCalibration[gt2004CurrentIndex].translation;
00064     motReq.translation /= 2;
00065     motReq.rotation += gt2004ParametersCalibration[gt2004CurrentIndex].rotation;
00066     motReq.rotation /= 2;
00067   }
00068   double N=4.75;
00069   Pose2D afterNs;
00070   Pose2D afterN2s;
00071   afterNs.rotation=motReq.rotation*N;
00072   afterN2s.rotation=afterNs.rotation/2;
00073   if (fabs(afterNs.rotation)<0.0001)
00074   {
00075     afterNs.translation=motReq.translation*N;
00076     afterN2s.translation=afterNs.translation/2;
00077   }
00078   else
00079   {
00080     double dx=motReq.translation.x;
00081     double dy=motReq.translation.y;
00082     double dr=motReq.rotation;
00083     afterNs.translation.x=(dy*(cos(dr*N)-1)+dx*sin(dr*N))/dr;
00084     afterNs.translation.y=(dx*(1-cos(dr*N))+dy*sin(dr*N))/dr;
00085     afterN2s.translation.x=(dy*(cos(dr*N/2)-1)+dx*sin(dr*N/2))/dr;
00086     afterN2s.translation.y=(dx*(1-cos(dr*N/2))+dy*sin(dr*N/2))/dr;
00087   }
00088   //cool heuristics with afterNs for start point...
00089   if (fabs(afterNs.rotation)>2.8)
00090   {
00091     currentGT2004ParametersStartPosition=Vector2<double>(-1400,0);
00092     //currentGT2004ParametersStartDirection=afterNs.rotation<0?80:-80;
00093     currentGT2004ParametersStartDirection=afterNs.rotation<0?80:-80;
00094     if (fabs(motReq.rotation)>1.5)
00095     {
00096       currentGT2004ParametersStartDirection=0;
00097     }
00098   }
00099   else if (fabs(afterNs.rotation)<1.0)
00100   {
00101     if (afterNs.translation.x>=fabs(afterNs.translation.y))
00102     {
00103       Pose2D start=Pose2D(0,-700,0).minusDiff(afterNs);
00104       currentGT2004ParametersStartPosition=start.translation;
00105       currentGT2004ParametersStartDirection=start.rotation*180/pi;
00106       //currentGT2004ParametersStartDirection += (atan2(-start.translation.y,-start.translation.x)-start.rotation)/2.9*180/pi;
00107       if (currentGT2004ParametersStartPosition.y>1350)
00108       {
00109         currentGT2004ParametersStartPosition.y=1350;
00110         currentGT2004ParametersStartDirection+=15;
00111       }
00112       else if (currentGT2004ParametersStartPosition.y<-1350)
00113       {
00114         currentGT2004ParametersStartPosition.y=-1350;
00115         currentGT2004ParametersStartDirection-=15;
00116       }
00117     }
00118     else if (afterNs.translation.x<=-fabs(afterNs.translation.y))
00119     {
00120       currentGT2004ParametersStartPosition=Vector2<double>(-700,0);
00121       currentGT2004ParametersStartDirection=0;
00122       if (afterNs.translation.y>1350)
00123       {
00124         currentGT2004ParametersStartDirection=20;
00125       }
00126       else if (afterNs.translation.y<-1350)
00127       {
00128         currentGT2004ParametersStartDirection=20;
00129       }
00130     }
00131     else
00132     {
00133       Pose2D start=Pose2D(0,-1500,0).minusDiff(afterN2s);
00134       currentGT2004ParametersStartPosition=start.translation;
00135       currentGT2004ParametersStartDirection=start.rotation*180/pi;
00136     }
00137   }
00138   else
00139   {
00140     double rat=fabs(GT2004ParametersSet::getRatio(motReq));
00141     if ((afterNs.translation.x>=fabs(afterNs.translation.y))&&(rat<0.2))
00142     {
00143       Pose2D start=Pose2D(afterNs.rotation/2.1,-900,0).minusDiff(afterNs);
00144       currentGT2004ParametersStartPosition=start.translation;
00145       currentGT2004ParametersStartDirection=start.rotation*180/pi;
00146     }
00147     else if ((afterNs.translation.x<=-fabs(afterNs.translation.y))&&(rat<0.2))
00148     {
00149       currentGT2004ParametersStartPosition=Vector2<double>(-800,0);
00150       currentGT2004ParametersStartDirection=-afterNs.rotation/2*180/pi;
00151     }
00152     else
00153     {
00154       Pose2D start=Pose2D(0,-1500,0).minusDiff(afterN2s);
00155       currentGT2004ParametersStartPosition=start.translation;
00156       currentGT2004ParametersStartDirection=start.rotation*180/pi;
00157     }
00158   }
00159 }
00160 
00161 bool EvolutionSymbols::getCurrentGT2004ParametersMeasureBlind()
00162 {
00163   return (gt2004ParametersCalibration[gt2004CurrentIndex].translation.abs()==0)?
00164     fabs(measurementRequest.rotation)>0.55:
00165   fabs(measurementRequest.rotation+gt2004ParametersCalibration[gt2004CurrentIndex].rotation)>1.1;
00166 }
00167 
00168 double EvolutionSymbols::getQualityOfCurrentGT2004Parameters()
00169 {
00170   if (gt2004CurrentIndex>=0)
00171   {
00172     Pose2D fiction=measurementRequest;
00173     Pose2D reality=gt2004ParametersCalibration[gt2004CurrentIndex];
00174     //2do: more usefull implementation
00175     return 20000.0/(200+(fiction-reality).translation.abs());
00176   }
00177   else
00178   {
00179     return 0;
00180   }
00181 }
00182 
00183 /*
00184 * Change Log
00185 * 
00186 * $Log: EvolutionSymbols.cpp,v $
00187 * Revision 1.2  2004/07/10 00:13:50  spranger
00188 * renaming for coderelease and preparations for gt2005
00189 *
00190 * Revision 1.1  2004/05/29 18:18:20  dueffert
00191 * walk parameter evolution, measurement and calibration stuff ported to GT2004_WM
00192 *
00193 * Revision 1.1  2004/05/27 13:31:27  dueffert
00194 * walking evolution stuff separated
00195 *
00196 * Revision 1.22  2004/05/23 13:30:31  dueffert
00197 * some start positions corrected
00198 *
00199 * Revision 1.21  2004/05/20 17:18:51  dueffert
00200 * automatic measurement (hopefully) finalized
00201 *
00202 * Revision 1.20  2004/05/19 08:00:27  dueffert
00203 * automatic walk speed measurement significantly improved
00204 *
00205 * Revision 1.19  2004/05/17 11:16:51  dueffert
00206 * blind measurement improved
00207 *
00208 * Revision 1.18  2004/04/19 10:34:55  dueffert
00209 * parameters tuned to allow measurement oft *fast* walking
00210 *
00211 * Revision 1.17  2004/04/16 14:56:37  dueffert
00212 * cleanup for Martins data flow graphics
00213 *
00214 * Revision 1.16  2004/03/25 17:40:15  loetzsch
00215 * adaptations to changes in the game controller and the led request
00216 *
00217 * Revision 1.15  2004/03/19 09:21:23  dueffert
00218 * ability to measure freely choosen motion request added
00219 *
00220 * Revision 1.14  2004/03/15 12:37:54  dueffert
00221 * start position improved
00222 *
00223 * Revision 1.13  2004/03/10 15:00:00  dueffert
00224 * copy'n'past bug fixed; starting position improved
00225 *
00226 * Revision 1.12  2004/03/10 10:00:52  dueffert
00227 * copy'n'paste bug fixed; start position tweaking
00228 *
00229 * Revision 1.11  2004/03/09 08:45:16  dueffert
00230 * second measurement behavior added; calculation of tsart position improved
00231 *
00232 * Revision 1.10  2004/03/08 01:06:59  roefer
00233 * Interfaces should be const
00234 *
00235 * Revision 1.9  2004/03/03 08:37:51  dueffert
00236 * measurement start position is now calculated instead of predefined; file beautified
00237 *
00238 * Revision 1.8  2004/03/01 15:00:47  dueffert
00239 * small start position improvements
00240 *
00241 * Revision 1.7  2004/02/29 13:42:58  dueffert
00242 * UDParametersSet symmetries handled
00243 *
00244 * Revision 1.6  2004/02/27 16:43:21  dueffert
00245 * small improvements
00246 *
00247 * Revision 1.5  2004/02/23 16:48:11  dueffert
00248 * several improvements for measurement of walking
00249 *
00250 * Revision 1.4  2004/02/18 14:49:20  dueffert
00251 * behavior control can now change walking parameters
00252 *
00253 * Revision 1.3  2004/02/10 11:13:18  dueffert
00254 * symbol for evolution added
00255 *
00256 * Revision 1.2  2003/12/06 17:45:33  loetzsch
00257 * replaced Player::playerRole (goalie, defender, striker1, striker2)
00258 * by Player::playerNumber (one, two, three, four)
00259 *
00260 * Revision 1.1  2003/10/06 13:39:29  cvsadm
00261 * Created GT2004 (M.J.)
00262 *
00263 * Revision 1.11  2003/07/07 21:15:39  risler
00264 * search-ball.x added
00265 * search-for-ball works without wlan
00266 *
00267 * Revision 1.10  2003/07/07 15:05:57  risler
00268 * no wlan fallback :
00269 * 5 seconds after wlan failure
00270 * striker1 goes slightly into own half as well
00271 *
00272 * Revision 1.9  2003/07/06 21:58:25  risler
00273 * role change : orientation influence reduced
00274 *
00275 * Revision 1.8  2003/07/05 21:27:19  risler
00276 * bugfix: no wlan fallback, time for out of role striker increased
00277 *
00278 * Revision 1.7  2003/07/05 10:15:58  risler
00279 * no wlan fallback for role change improved
00280 *
00281 * Revision 1.6  2003/07/04 16:30:34  loetzsch
00282 * bonus for the current Striker in computeRole()
00283 *
00284 * Revision 1.5  2003/07/03 16:08:17  loetzsch
00285 * renamed "robot-is-not-stuck-but-obstacles-are-close" to "obstacles-are-close"
00286 *
00287 * Revision 1.4  2003/07/03 11:04:42  juengel
00288 * obstaclesAreClose is calculated
00289 *
00290 * Revision 1.3  2003/07/03 10:15:22  loetzsch
00291 * added symbol "not-stuck-but..."
00292 *
00293 * Revision 1.2  2003/07/02 19:15:44  loetzsch
00294 * ::computeRole()  does not use BehaviorTeamMessage::whoIsOnline() anymore
00295 *
00296 * Revision 1.1.1.1  2003/07/02 09:40:23  cvsadm
00297 * created new repository for the competitions in Padova from the 
00298 * tamara CVS (Tuesday 2:00 pm)
00299 *
00300 * removed unused solutions
00301 *
00302 * Revision 1.22  2003/06/26 16:20:29  loetzsch
00303 * some role assignment experiments
00304 *
00305 * Revision 1.21  2003/06/23 19:13:21  loetzsch
00306 * shifted the position of  "the-striker-is-playing-near-the-opponent-goal" for 20cm to the opponent goal
00307 *
00308 * Revision 1.20  2003/06/22 11:48:54  juengel
00309 * new robot is stuck condition: > 20 percent of range is closer than 300mm
00310 *
00311 * Revision 1.19  2003/06/21 15:26:46  juengel
00312 * obstaclesModel.getPercentageOfLowDistanceObstaclesInRange is used to calculate robot-is-stuck
00313 *
00314 * Revision 1.18  2003/06/20 20:15:24  juengel
00315 * Renamed some methods in ObstaclesModel.
00316 *
00317 * Revision 1.17  2003/06/20 13:58:31  juengel
00318 * Added calculation of robot is stuck.
00319 *
00320 * Revision 1.16  2003/06/19 21:31:45  juengel
00321 * Added robot-is-stuck.
00322 *
00323 * Revision 1.15  2003/06/19 10:17:17  risler
00324 * symbol another-teammate-is-preparing-a-kick added
00325 *
00326 * Revision 1.14  2003/06/17 19:54:59  risler
00327 * ball caught symbols moved to strategy
00328 *
00329 * Revision 1.13  2003/06/02 14:10:20  loetzsch
00330 * added some hysteresises for the supporter position options
00331 *
00332 * Revision 1.12  2003/05/31 14:38:48  loetzsch
00333 * changed the symbols for the intercept
00334 *
00335 * Revision 1.11  2003/05/30 11:14:49  dueffert
00336 * temporary bugfix
00337 *
00338 * Revision 1.10  2003/05/28 17:48:35  loetzsch
00339 * better positioning for the supporters
00340 *
00341 * Revision 1.9  2003/05/27 16:52:53  loetzsch
00342 * first passing experiments
00343 *
00344 * Revision 1.8  2003/05/26 13:45:02  loetzsch
00345 * some improvements
00346 *
00347 * Revision 1.7  2003/05/25 22:37:02  loetzsch
00348 * finished the game state options of GT2003
00349 *
00350 * Revision 1.6  2003/05/14 09:24:49  risler
00351 * current role bonus hyteresis no longer in estimateTimeToReachBall
00352 * added current role bonus for offensive supporter
00353 *
00354 * Revision 1.5  2003/05/08 14:25:30  risler
00355 * some bugfixes
00356 *
00357 * Revision 1.4  2003/05/08 13:20:37  loetzsch
00358 * some cleanup
00359 *
00360 * Revision 1.3  2003/05/08 10:20:35  dueffert
00361 * bugs fixed
00362 *
00363 * Revision 1.2  2003/05/08 00:22:46  risler
00364 * added strategy symbols
00365 *
00366 * Revision 1.1  2003/05/06 16:28:19  loetzsch
00367 * added class StrategySymbols
00368 *
00369 */
00370