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

SIG_GPRealSpeedFitnessFunction.cpp

00001 #include "SIGEL_GP/SIG_GPRealSpeedFitnessFunction.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_GPRealSpeedFitnessFunction::SIG_GPRealSpeedFitnessFunction( 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                              "RealSpeedFitnessFunction" )
00018   { };
00019 
00020   SIG_GPRealSpeedFitnessFunction::~SIG_GPRealSpeedFitnessFunction()
00021   { };
00022 
00023   double SIG_GPRealSpeedFitnessFunction::evalFitness()
00024   {
00025     double const recordingRate = 0.5;
00026 
00027     int const recordingFrequency = int( recordingRate / simparameter.getStepSize() );
00028 
00029     SIGEL_GP::SIG_GPFullDataRecorder recorder( recordingFrequency );
00030 
00031     SIGEL_Simulation::SIG_Simulation *simulation = new SIGEL_Simulation::SIG_Simulation( rob,
00032                                                                                          environment,
00033                                                                                          program,
00034                                                                                          simparameter,
00035                                                                                          recorder );
00036 
00037     double fitness = 0;
00038 
00039     try
00040       {
00041         simulation->start();
00042       }
00043     catch (SIGEL_Tools::SIG_Exception &e)
00044       { };
00045 
00046     // delete simulation;
00047 
00048     DL_vector *endPosition = new DL_vector();
00049     *endPosition = recorder.endPosition;
00050 
00051     DL_matrix *endRotation = new DL_matrix();
00052     *endRotation = recorder.endRotation;
00053 
00054     recorder.positions.append( endPosition );
00055     recorder.rotations.append( endRotation );
00056 
00057     double totalDistance = 0;
00058 
00059     DL_vector *actPosition = recorder.positions.first();
00060     DL_matrix *actRotation = recorder.rotations.first();
00061 
00062     DL_vector lastRealPosition = normalizeRobotPosition( *actPosition,
00063                                                          *actRotation );
00064 
00065     while (actPosition)
00066       {
00067         DL_vector actRealPosition = normalizeRobotPosition( *actPosition,
00068                                                             *actRotation );
00069 
00070         if (isValid( actRealPosition.x ) && isValid( actRealPosition.y ) && isValid( actRealPosition.z ))
00071           {
00072             DL_vector localDistanceVector = actRealPosition;
00073             localDistanceVector.minusis( &lastRealPosition );
00074 
00075             double localDistance = localDistanceVector.norm();
00076 
00077 #ifdef SIG_DEBUG
00078             SIGEL_Tools::SIG_IO::cerr << "localDistance: "
00079                                       << localDistance
00080                                       << "\n";
00081 #endif
00082 
00083             totalDistance += localDistance;
00084 
00085             lastRealPosition = actRealPosition;
00086           }
00087         else
00088           {
00089             totalDistance = 0;
00090             break;
00091           };
00092 
00093         actPosition = recorder.positions.next();
00094         actRotation = recorder.rotations.next();
00095       };
00096 
00097     int simulatedSeconds = QTime().secsTo( simparameter.getTimeToSimulate() );
00098 
00099     fitness = totalDistance / simulatedSeconds;
00100 
00101     return fitness;
00102   };
00103 
00104 }

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