Main Page   Namespace List   Class Hierarchy   Compound List   File List   Namespace Members   Compound Members  

SIG_GPNiceWalkingFitnessFunction.cpp

00001 #include "SIGEL_GP/SIG_GPNiceWalkingFitnessFunction.h"
00002 
00003 #include "SIGEL_GP/SIG_GPFullDataRecorder.h"
00004 #include "SIGEL_Simulation/SIG_Simulation.h"
00005 
00006 namespace SIGEL_GP
00007 {
00008 
00009   SIG_GPNiceWalkingFitnessFunction::SIG_GPNiceWalkingFitnessFunction( SIGEL_Program::SIG_Program &program,
00010                                                                   SIGEL_Robot::SIG_Robot &robot,
00011                                                                   SIGEL_Environment::SIG_Environment &environment,
00012                                                                   SIGEL_Simulation::SIG_SimulationParameters & simulationParameters )
00013     : SIG_GPFitnessFunction( program,
00014                              robot,
00015                              environment,
00016                              simulationParameters,
00017                              "NiceWalkingFitnessFunction" )
00018   { };
00019 
00020   SIG_GPNiceWalkingFitnessFunction::~SIG_GPNiceWalkingFitnessFunction()
00021   { };
00022 
00023   double SIG_GPNiceWalkingFitnessFunction::evalFitness()
00024   {
00025     double const toleranceBandWidth = 0.5;
00026 
00027     SIGEL_GP::SIG_GPFullDataRecorder recorder( 1 );
00028 
00029     SIGEL_Simulation::SIG_Simulation *simulation = new SIGEL_Simulation::SIG_Simulation( rob,
00030                                                                                          environment,
00031                                                                                          program,
00032                                                                                          simparameter,
00033                                                                                          recorder );
00034 
00035     double fitness = 0;
00036 
00037     try
00038       {
00039         simulation->start();
00040       }
00041     catch (SIGEL_Tools::SIG_Exception &e)
00042       { };
00043 
00044     // delete simulation;
00045 
00046     DL_vector realStartPosition = normalizeRobotPosition( *recorder.positions.first(),
00047                                                           *recorder.rotations.first() );
00048 
00049     DL_vector realEndPosition = normalizeRobotPosition( recorder.endPosition,
00050                                                         recorder.endRotation );
00051 
00052     double const optimalHeight = realStartPosition.y;
00053 
00054     double const minimalHeight = optimalHeight - toleranceBandWidth;
00055     double const maximalHeight = optimalHeight + toleranceBandWidth;
00056 
00057     DL_vector distanceVector = realStartPosition;
00058     distanceVector.minusis( &realEndPosition );
00059 
00060     double distance = distanceVector.norm();
00061 
00062     int simulatedSeconds = QTime().secsTo( simparameter.getTimeToSimulate() );
00063 
00064     fitness = distance / simulatedSeconds;
00065 
00066     DL_vector *endPosition = new DL_vector();
00067     *endPosition = recorder.endPosition;
00068 
00069     DL_matrix *endRotation = new DL_matrix();
00070     *endRotation = recorder.endRotation;
00071 
00072     recorder.positions.append( endPosition );
00073     recorder.rotations.append( endRotation );
00074 
00075     DL_vector *actPosition = recorder.positions.first();
00076     DL_matrix *actRotation = recorder.rotations.first();
00077 
00078     while (actPosition)
00079       {
00080         DL_vector actRealPosition = normalizeRobotPosition( *actPosition,
00081                                                             *actRotation );
00082 
00083         if ( !(isValid( actRealPosition.x ) && isValid( actRealPosition.y ) && isValid( actRealPosition.z )) )
00084           {
00085             fitness = 0;
00086             break;
00087           };
00088 
00089         if ((actRealPosition.y < minimalHeight) || (actRealPosition.y > maximalHeight))
00090           {
00091             fitness = 0;
00092             break;
00093           };
00094 
00095         actPosition = recorder.positions.next();
00096         actRotation = recorder.rotations.next();
00097       };
00098 
00099     return fitness;
00100   };
00101 
00102 }

Generated at Mon Sep 3 01:32:23 2001 for PG 368 - SIGEL by doxygen1.2.3 written by Dimitri van Heesch, © 1997-2000