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
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 }