GT2004: Modules/BehaviorControl/GT2004BehaviorControl/GT2004BasicBehaviors/GT2004EvolutionBasicBehaviors.h Source File

00001 /**
00002 * @file GT2004EvolutionBasicBehaviors.h
00003 *
00004 * Declaration of basic behaviors defined in evolution-basic-behaviors.xml.
00005 *
00006 * @author Uwe Düffert
00007 */
00008 
00009 #ifndef __GT2004EvolutionBasicBehaviors_h_
00010 #define __GT2004EvolutionBasicBehaviors_h_
00011 
00012 #include "../../BehaviorControl.h"
00013 #include "Tools/Xabsl2/Xabsl2Engine/Xabsl2Engine.h"
00014 #include "Tools/Xabsl2/Xabsl2Engine/Xabsl2BasicBehavior.h"
00015 #include "Tools/Evolution/Population.h"
00016 #include "Tools/Evolution/Parcour.h"
00017 #include "Tools/Math/PIDsmoothedValue.h"
00018 #include "Modules/WalkingEngine/GT2004ParameterSet.h"
00019 #include "Modules/WalkingEngine/InvKinWalkingParameterSets.h"
00020 
00021 /**
00022 * A BasicBehavior for measuring and calibrating an the current GT2004Parameters
00023 * @author Uwe Düffert
00024 */
00025 class GT2004BasicBehaviorMeasureGT2004Parameters : public Xabsl2BasicBehavior, public BehaviorControlInterfaces
00026 {
00027 public:
00028 /*
00029 * Constructor.
00030 * @param errorHandler Is invoked when errors occur
00031 * @param interfaces The paramters of the BehaviorControl module.
00032   */
00033   GT2004BasicBehaviorMeasureGT2004Parameters(const BehaviorControlInterfaces& interfaces,
00034     Xabsl2ErrorHandler& errorHandler,
00035     GT2004ParametersSet& gt2004ParametersSet,
00036     GT2004Parameters& gt2004ExtraParameters,
00037     Pose2D* gt2004ParametersCalibration,
00038     int& gt2004CurrentIndex,
00039     Pose2D& measurementRequest,
00040     int& evolutionMode):
00041   Xabsl2BasicBehavior("measure-gt2004-parameters", errorHandler),
00042     BehaviorControlInterfaces(interfaces),
00043     gt2004ParametersSet(gt2004ParametersSet),
00044     gt2004ExtraParameters(gt2004ExtraParameters),
00045     gt2004ParametersCalibration(gt2004ParametersCalibration),
00046     gt2004CurrentIndex(gt2004CurrentIndex),
00047     measurementRequest(measurementRequest),
00048     evolutionMode(evolutionMode),
00049     startFrameNumber(0),lastFrameNumber(0)
00050   {
00051   }
00052   /** Executes the basic behavior. */
00053   virtual void execute();
00054 private:
00055   /** robotPose frame number of first call of this beasic behavior in current measurement */
00056   unsigned long startFrameNumber;
00057   /** robotPose frame number of last call of this beasic behavior in current measurement */
00058   unsigned long lastFrameNumber;
00059   /** local variables to store measurement start position */
00060   double x0,y0,r0,t0;
00061 
00062   /** GT2004 Evolution stuff */
00063   GT2004ParametersSet& gt2004ParametersSet;
00064   GT2004Parameters& gt2004ExtraParameters;
00065   Pose2D* gt2004ParametersCalibration;
00066   int& gt2004CurrentIndex;
00067   Pose2D& measurementRequest;
00068   int& evolutionMode;
00069 };
00070 
00071 /**
00072 * A BasicBehavior for measuring and calibrating an the current GT2004Parameters while being partially blind (while turning)
00073 * @author Uwe Düffert
00074 */
00075 class GT2004BasicBehaviorMeasureGT2004ParametersBlind : public Xabsl2BasicBehavior, public BehaviorControlInterfaces
00076 {
00077 public:
00078 /*
00079 * Constructor.
00080 * @param errorHandler Is invoked when errors occur
00081 * @param interfaces The paramters of the BehaviorControl module.
00082   */
00083   GT2004BasicBehaviorMeasureGT2004ParametersBlind(const BehaviorControlInterfaces& interfaces,
00084     Xabsl2ErrorHandler& errorHandler,
00085     GT2004ParametersSet& gt2004ParametersSet,
00086     GT2004Parameters& gt2004ExtraParameters,
00087     Pose2D* gt2004ParametersCalibration,
00088     int& gt2004CurrentIndex,
00089     Pose2D& measurementRequest,
00090     int& evolutionMode):
00091   Xabsl2BasicBehavior("measure-gt2004-parameters-blind", errorHandler),
00092     BehaviorControlInterfaces(interfaces),
00093     gt2004ParametersSet(gt2004ParametersSet),
00094     gt2004ExtraParameters(gt2004ExtraParameters),
00095     gt2004ParametersCalibration(gt2004ParametersCalibration),
00096     gt2004CurrentIndex(gt2004CurrentIndex),
00097     measurementRequest(measurementRequest),
00098     evolutionMode(evolutionMode),
00099     startFrameNumber(0),lastFrameNumber(0),clusterFrames(0),goodFrames(0)
00100   {
00101   }
00102   /** Executes the basic behavior. */
00103   virtual void execute();
00104 private:
00105   /** robotPose frame number of first call of this beasic behavior in current measurement */
00106   unsigned long startFrameNumber;
00107   /** robotPose frame number of last call of this beasic behavior in current measurement */
00108   unsigned long lastFrameNumber;
00109   /** number of valid frames in current cluster*/
00110   unsigned long clusterFrames;
00111   /** local variables to store measurement start position */
00112   double x0,y0,r0,t0;
00113   double lastRot;
00114   double lastTime;
00115   double firstNullTime;
00116   double lastNullTime;
00117   double nullTime;
00118   Pose2D speed;
00119   int clusterNumber;
00120   int goodFrames;
00121   double dr,dx,dy;
00122 
00123   /** GT2004 Evolution stuff */
00124   GT2004ParametersSet& gt2004ParametersSet;
00125   GT2004Parameters& gt2004ExtraParameters;
00126   Pose2D* gt2004ParametersCalibration;
00127   int& gt2004CurrentIndex;
00128   Pose2D& measurementRequest;
00129   int& evolutionMode;
00130 };
00131 
00132 /**
00133 * A BasicBehavior for walking forward or backward following a certain parcour as well as
00134 * changing walking parameters according to the fitness measured in the last parcour walk
00135 * This Basic Behavior needs a walking engine that does not switch parameters according
00136 * to the MotionRequest (like InvKin:GT2003 and GT2004), but a non-switching one (like InvKin:FF)
00137 * @author Uwe Düffert
00138 */
00139 class GT2004BasicBehaviorEvolveOmniParameters : public Xabsl2BasicBehavior, public BehaviorControlInterfaces
00140 {
00141 public:
00142 /*
00143 * Constructor.
00144 * @param errorHandler Is invoked when errors occur
00145 * @param interfaces The paramters of the BehaviorControl module.
00146   */
00147   GT2004BasicBehaviorEvolveOmniParameters(const BehaviorControlInterfaces& interfaces,
00148     Xabsl2ErrorHandler& errorHandler,
00149     GT2004ParametersSet& gt2004ParametersSet,
00150     Pose2D* gt2004ParametersCalibration,
00151     int& gt2004CurrentIndex,
00152     Pose2D& measurementRequest,
00153     int& evolutionMode):
00154   Xabsl2BasicBehavior("evolve-walking-parameters", errorHandler),
00155     BehaviorControlInterfaces(interfaces),
00156     gt2004ParametersSet(gt2004ParametersSet),
00157     gt2004ParametersCalibration(gt2004ParametersCalibration),
00158     gt2004CurrentIndex(gt2004CurrentIndex),
00159     measurementRequest(measurementRequest),
00160     evolutionMode(evolutionMode),
00161     lastMode(0),
00162     motionRequestX(0.15, 0.25, 0.0, 0, -300, 300, 50),
00163     motionRequestY(0.15, 0.25, 0.0, 0, -300, 300, 50),
00164     motionRequestR(0.15, 0.25, 0.0, 0, -pi, pi, 0.5)
00165   {
00166     registerParameter("execution-mode", mode);
00167     //initialize Population
00168     delete(ftPopulation.individual[0]);
00169     delete(bPopulation.individual[0]);
00170     ERS7EvolveWalkingParameters ers7;
00171 
00172     InvKinWalkingParameters msh7backward("", 0, 88.5057, 79.127, 43.2139, 101.905, 77.6874, -49.6692, 9.76973, 28.5799, +0.0724927, -0.0922574, 1.20455, 0.843727, 1.44822, 34.8207, 38.8411, 100, 100, 1, 0.06, 61, 0.461105, 0.0361205, 0.0391534, 0.508829, 0.0532571, 0.0469326, 0, 0.5, 0.5, 0, 0, 0, 0.0337565);
00173     GT2004Parameters msh7(0, Pose2D(0, 0, 0), 77.5836, 80.2702, 50.3536, 105.174, 81.5432, -52.0932, 5, 24, -0.25, 0.016, 64, 0, 0.49, 0.06, 0.06, 0.49, 0.06, 0.06, 0, 0.5, 0.5, 0);
00174 
00175     ftPopulation.individual[0]= new GT2004Parameters(msh7); //ers7
00176     bPopulation.individual[0]= new GT2004Parameters(msh7backward); //ers7
00177     for (int i=1;i<10;i++)
00178     {
00179       ftPopulation.individual[i]->mutationOf(ftPopulation.individual[0], 0.3, 0.12, true);
00180       ftPopulation.individual[i]->index=ftPopulation.individual[0]->index;
00181       bPopulation.individual[i]->mutationOf(bPopulation.individual[0], 0.3, 0.12, true);
00182       bPopulation.individual[i]->index=bPopulation.individual[0]->index;
00183     }
00184     InTextFile inFTPopul("ft_pop.dat");
00185     if (inFTPopul.exists())
00186     {
00187       inFTPopul >> ftPopulation;
00188     }
00189     InTextFile inBPopul("b_pop.dat");
00190     if (inBPopul.exists())
00191     {
00192       inBPopul >> bPopulation;
00193     }
00194   }
00195 
00196   /**  Executes the basic behavior. */
00197   virtual void execute();
00198 
00199 private:
00200   /** a Polpulation of WalkingParameters */
00201   Population<GT2004Parameters,10> ftPopulation;
00202   Population<GT2004Parameters,10> bPopulation;
00203 
00204   /** specifies whether the robot should walk forward or backward or parameters should be changed */
00205   double mode;
00206   int lastMode;
00207 
00208   ForwardTurningParcour forwardTurningParcour;
00209   SimpleBackwardParcour simpleBackwardParcour;
00210   PIDsmoothedValue motionRequestX, motionRequestY, motionRequestR;
00211 
00212   /** GT2004 Evolution stuff */
00213   GT2004ParametersSet& gt2004ParametersSet;
00214   Pose2D* gt2004ParametersCalibration;
00215   int& gt2004CurrentIndex;
00216   Pose2D& measurementRequest;
00217   int& evolutionMode;
00218 };
00219 
00220 
00221 /**
00222 * A BasicBehavior for sending the current GT2004Parameters via PackageCognitionMotion to the
00223 * walking engine and choose the next Parameters to be optimized
00224 *
00225 * @author Uwe Düffert
00226 */
00227 class GT2004BasicBehaviorSendCurrentGT2004ParametersAndChooseNext : public Xabsl2BasicBehavior, public BehaviorControlInterfaces
00228 {
00229 public:
00230 /*
00231 * Constructor.
00232 * @param errorHandler Is invoked when errors occur
00233 * @param interfaces The paramters of the BehaviorControl module.
00234   */
00235   GT2004BasicBehaviorSendCurrentGT2004ParametersAndChooseNext(const BehaviorControlInterfaces& interfaces,
00236     Xabsl2ErrorHandler& errorHandler,
00237     GT2004ParametersSet& gt2004ParametersSet,
00238     Pose2D* gt2004ParametersCalibration,
00239     int& gt2004CurrentIndex,
00240     Pose2D& measurementRequest,
00241     int& evolutionMode):
00242   Xabsl2BasicBehavior("send-current-gt2004-parameters-and-choose-next", errorHandler),
00243     BehaviorControlInterfaces(interfaces),
00244     gt2004ParametersSet(gt2004ParametersSet),
00245     gt2004ParametersCalibration(gt2004ParametersCalibration),
00246     gt2004CurrentIndex(gt2004CurrentIndex),
00247     measurementRequest(measurementRequest),
00248     evolutionMode(evolutionMode)
00249   {
00250   }
00251 
00252   /**  Executes the basic behavior. */
00253   virtual void execute();
00254 
00255 private:
00256   /** GT2004 Evolution stuff */
00257   GT2004ParametersSet& gt2004ParametersSet;
00258   Pose2D* gt2004ParametersCalibration;
00259   int& gt2004CurrentIndex;
00260   Pose2D& measurementRequest;
00261   int& evolutionMode;
00262 };
00263 
00264 
00265 /**
00266 * A BasicBehavior for generating the next GT2004Parameters to be measured exactly
00267 *
00268 * @author Uwe Düffert
00269 */
00270 class GT2004BasicBehaviorNextGT2004ParametersToBeMeasured : public Xabsl2BasicBehavior, public BehaviorControlInterfaces
00271 {
00272 public:
00273 /*
00274 * Constructor.
00275 * @param errorHandler Is invoked when errors occur
00276 * @param interfaces The paramters of the BehaviorControl module.
00277   */
00278   GT2004BasicBehaviorNextGT2004ParametersToBeMeasured(const BehaviorControlInterfaces& interfaces,
00279     Xabsl2ErrorHandler& errorHandler,
00280     GT2004ParametersSet& gt2004ParametersSet,
00281     GT2004Parameters& gt2004ExtraParameters,
00282     Pose2D* gt2004ParametersCalibration,
00283     int& gt2004CurrentIndex,
00284     Pose2D& measurementRequest,
00285     int& evolutionMode):
00286   Xabsl2BasicBehavior("next-gt2004-parameters-to-measure", errorHandler),
00287     BehaviorControlInterfaces(interfaces),
00288     gt2004ParametersSet(gt2004ParametersSet),
00289     gt2004ExtraParameters(gt2004ExtraParameters),
00290     gt2004ParametersCalibration(gt2004ParametersCalibration),
00291     gt2004CurrentIndex(gt2004CurrentIndex),
00292     measurementRequest(measurementRequest),
00293     evolutionMode(evolutionMode),tableIndex(0),trials(0),maxSpeed(0),
00294     maxSpeedRatio(0),maxYRatio(0),maxRRatio(0)
00295   {
00296   }
00297 
00298   /**  Executes the basic behavior. */
00299   virtual void execute();
00300 
00301 private:
00302   /** GT2004 Evolution stuff */
00303   GT2004ParametersSet& gt2004ParametersSet;
00304   GT2004Parameters& gt2004ExtraParameters;
00305   Pose2D* gt2004ParametersCalibration;
00306   int& gt2004CurrentIndex;
00307   Pose2D& measurementRequest;
00308   int& evolutionMode;
00309 
00310   Pose2D requestTable[40];
00311   Pose2D measureTable[40];
00312   int tableIndex;
00313   int trials;
00314   double maxSpeed;
00315   double maxSpeedRatio;
00316   double maxYRatio;
00317   double maxRRatio;
00318 };
00319 
00320 /**
00321 * @class GT2004EvolutionBasicBehaviors
00322 *
00323 * Creates and registers simple basic behaviors
00324 */
00325 class GT2004EvolutionBasicBehaviors : public BehaviorControlInterfaces
00326 {
00327 public:
00328 /**
00329 * Constructor
00330   */
00331   GT2004EvolutionBasicBehaviors(const BehaviorControlInterfaces& interfaces,
00332     Xabsl2ErrorHandler& errorHandler,
00333     GT2004ParametersSet& gt2004ParametersSet,
00334     GT2004Parameters& gt2004ExtraParameters,
00335     Pose2D* gt2004ParametersCalibration,
00336     int& gt2004CurrentIndex,
00337     Pose2D& measurementRequest,
00338     int& evolutionMode)
00339     : BehaviorControlInterfaces(interfaces),
00340     errorHandler(errorHandler),
00341     basicBehaviorEvolveOmniParameters(interfaces,errorHandler, gt2004ParametersSet, gt2004ParametersCalibration, gt2004CurrentIndex, measurementRequest, evolutionMode),
00342     basicBehaviorMeasureGT2004Parameters(interfaces,errorHandler, gt2004ParametersSet, gt2004ExtraParameters, gt2004ParametersCalibration, gt2004CurrentIndex, measurementRequest, evolutionMode),
00343     basicBehaviorMeasureGT2004ParametersBlind(interfaces,errorHandler, gt2004ParametersSet, gt2004ExtraParameters, gt2004ParametersCalibration, gt2004CurrentIndex, measurementRequest, evolutionMode),
00344     basicBehaviorSendCurrentGT2004ParametersAndChooseNext(interfaces,errorHandler, gt2004ParametersSet, gt2004ParametersCalibration, gt2004CurrentIndex, measurementRequest, evolutionMode),
00345     basicBehaviorNextGT2004ParametersToBeMeasured(interfaces,errorHandler, gt2004ParametersSet, gt2004ExtraParameters, gt2004ParametersCalibration, gt2004CurrentIndex, measurementRequest, evolutionMode)
00346   {}
00347 
00348   /** Registers basic behaviors at the engine */
00349   void registerBasicBehaviors(Xabsl2Engine& engine);
00350 
00351 private:
00352   /** Is invoked when errors occurs */
00353   Xabsl2ErrorHandler& errorHandler;
00354 
00355   //!@name Basic Behaviors
00356   //!@{
00357   GT2004BasicBehaviorEvolveOmniParameters  basicBehaviorEvolveOmniParameters;
00358   GT2004BasicBehaviorMeasureGT2004Parameters  basicBehaviorMeasureGT2004Parameters;
00359   GT2004BasicBehaviorMeasureGT2004ParametersBlind  basicBehaviorMeasureGT2004ParametersBlind;
00360   GT2004BasicBehaviorSendCurrentGT2004ParametersAndChooseNext  basicBehaviorSendCurrentGT2004ParametersAndChooseNext;
00361   GT2004BasicBehaviorNextGT2004ParametersToBeMeasured  basicBehaviorNextGT2004ParametersToBeMeasured;
00362   //!@}
00363 };
00364 
00365 #endif // __GT2004EvolutionBasicBehaviors_h_
00366 
00367 /*
00368 * Change Log
00369 *
00370 * $Log: GT2004EvolutionBasicBehaviors.h,v $
00371 * Revision 1.4  2004/07/10 00:13:53  spranger
00372 * renaming for coderelease and preparations for gt2005
00373 *
00374 * Revision 1.3  2004/07/07 15:13:00  dueffert
00375 * evolution uses two populations now
00376 *
00377 * Revision 1.2  2004/06/14 16:54:56  juengel
00378 * Removed some WalkingEngineParameterSets.
00379 *
00380 * Revision 1.1  2004/05/29 18:18:19  dueffert
00381 * walk parameter evolution, measurement and calibration stuff ported to GT2004_WM
00382 *
00383 * Revision 1.1  2004/05/27 13:31:26  dueffert
00384 * walking evolution stuff separated
00385 *
00386 * Revision 1.23  2004/05/20 17:18:35  dueffert
00387 * automatic measurement (hopefully) finalized
00388 *
00389 * Revision 1.22  2004/05/19 07:59:44  dueffert
00390 * automatic walk speed measurement significantly improved
00391 *
00392 * Revision 1.21  2004/05/14 10:15:49  dueffert
00393 * exact measurement behavior with logging implemented
00394 *
00395 * Revision 1.20  2004/05/12 14:23:32  dueffert
00396 * measurement basic behavior added
00397 *
00398 * Revision 1.19  2004/04/29 15:15:01  dueffert
00399 * measurement is now frame instead of time based
00400 *
00401 * Revision 1.18  2004/04/16 14:56:36  dueffert
00402 * cleanup for Martins data flow graphics
00403 *
00404 * Revision 1.17  2004/03/24 13:44:49  dueffert
00405 * support for uniform noise mutation readded
00406 *
00407 * Revision 1.16  2004/03/19 16:37:27  dueffert
00408 * blind measurement works for the first time
00409 *
00410 * Revision 1.15  2004/03/12 17:11:38  dueffert
00411 * omni-evo uses parameters in package now; blind speed calculation improved
00412 *
00413 * Revision 1.14  2004/03/10 15:01:08  dueffert
00414 * first simple fast turn (blind) measurement implemented
00415 *
00416 * Revision 1.13  2004/03/10 10:02:22  dueffert
00417 * population size tweaked; statistics output added
00418 *
00419 * Revision 1.12  2004/03/09 10:28:10  dueffert
00420 * now using const interface too
00421 *
00422 * Revision 1.11  2004/03/09 08:46:30  dueffert
00423 * second measurement behavior added
00424 *
00425 * Revision 1.10  2004/03/08 01:06:49  roefer
00426 * Interfaces should be const
00427 *
00428 * Revision 1.9  2004/03/01 15:06:27  dueffert
00429 * measurement output beautified
00430 *
00431 * Revision 1.8  2004/02/23 16:48:11  dueffert
00432 * several improvements for measurement of walking
00433 *
00434 * Revision 1.7  2004/02/18 14:49:20  dueffert
00435 * behavior control can now change walking parameters
00436 *
00437 * Revision 1.6  2004/02/10 11:19:19  dueffert
00438 * simple basic behavior for evolution added, another improved, beautified
00439 *
00440 * Revision 1.5  2003/12/09 15:15:20  dueffert
00441 * WalkAccelerationRestrictor replaced by PIDsmoothedValue for evolving
00442 *
00443 * Revision 1.4  2003/11/19 13:49:39  dueffert
00444 * better positioning
00445 *
00446 * Revision 1.3  2003/10/31 08:32:49  dueffert
00447 * doxygen bugs fixed
00448 *
00449 * Revision 1.2  2003/10/28 13:27:22  dueffert
00450 * spelling and evolution logic improved
00451 *
00452 * Revision 1.1  2003/10/22 22:18:45  loetzsch
00453 * prepared the cloning of the GT2003BehaviorControl
00454 *
00455 * Revision 1.1  2003/10/06 13:39:31  cvsadm
00456 * Created GT2004 (M.J.)
00457 *
00458 * Revision 1.11  2003/09/30 10:51:13  dueffert
00459 * typos fixed
00460 *
00461 * Revision 1.10  2003/09/26 11:38:51  juengel
00462 * - sorted tools
00463 * - clean-up in DataTypes
00464 *
00465 * Revision 1.9  2003/08/09 14:57:06  dueffert
00466 * some evolution improvements
00467 *
00468 * Revision 1.8  2003/08/08 15:43:25  dueffert
00469 * some evolution implementation added
00470 *
00471 * Revision 1.7  2003/08/08 14:30:04  dueffert
00472 * some evolution implementation added
00473 *
00474 * Revision 1.6  2003/08/04 07:46:39  roefer
00475 * Challenge 2 modifications
00476 *
00477 * Revision 1.5  2003/07/30 14:52:03  dueffert
00478 * walk evolution agent added
00479 *
00480 * Revision 1.4  2003/07/09 11:44:26  jhoffman
00481 * obstacle avoidance simple behavior mode as used in challenge 3
00482 * known bug: sometimes stops in front of obstacle (but eventually the situation is resolved)
00483 *
00484 * Revision 1.3  2003/07/06 20:34:30  dueffert
00485 * GT2004BasicBehaviorGoToBallWithDirection added
00486 *
00487 * Revision 1.2  2003/07/04 16:24:24  jhoffman
00488 * updated obstacle avoider (simple behavior) to be used in challenge
00489 *
00490 * Revision 1.1.1.1  2003/07/02 09:40:23  cvsadm
00491 * created new repository for the competitions in Padova from the
00492 * tamara CVS (Tuesday 2:00 pm)
00493 *
00494 * removed unused solutions
00495 *
00496 * Revision 1.3  2003/06/04 00:43:45  loetzsch
00497 * put the GT2003WalkAccelerationRestrictor into two extra files in /Src/Tools
00498 *
00499 * Revision 1.2  2003/05/05 19:22:03  risler
00500 * added classes Simple/ContinuousBasicBehaviors for registering basic behaviors
00501 *
00502 * Revision 1.1  2003/05/03 15:14:02  loetzsch
00503 * GT2003BehaviorControl first draft
00504 *
00505 *
00506 */