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