ARGoS Example
mpga_phototaxis_loop_functions.cpp
<- back
#include "mpga_phototaxis_loop_functions.h"
/****************************************/
/****************************************/
CMPGAPhototaxisLoopFunctions::CMPGAPhototaxisLoopFunctions() :
m_vecInitSetup(5),
m_pcFootBot(NULL),
m_pcController(NULL),
m_pfControllerParams(new Real[GENOME_SIZE]),
m_pcRNG(NULL) {}
/****************************************/
/****************************************/
CMPGAPhototaxisLoopFunctions::~CMPGAPhototaxisLoopFunctions() {
delete[] m_pfControllerParams;
}
/****************************************/
/****************************************/
void CMPGAPhototaxisLoopFunctions::Init(TConfigurationNode& t_node) {
/*
* Create the random number generator
*/
m_pcRNG = CRandom::CreateRNG("argos");
/*
* Create the foot-bot and get a reference to its controller
*/
m_pcFootBot = new CFootBotEntity(
"fb", // entity id
"fnn" // controller id as set in the XML
);
AddEntity(*m_pcFootBot);
m_pcController = &dynamic_cast<CFootBotNNController&>(m_pcFootBot->GetControllableEntity().GetController());
/*
* Create the initial setup for each trial
* The robot is placed 4.5 meters away from the light
* (which is in the origin) at angles
* { PI/12, 2*PI/12, 3*PI/12, 4*PI/12, 5*PI/12 }
* wrt to the world reference.
* Also, the rotation of the robot is chosen at random
* from a uniform distribution.
*/
CRadians cOrient;
for(size_t i = 0; i < 5; ++i) {
/* Set position */
m_vecInitSetup[i].Position.FromSphericalCoords(
4.5f, // distance from origin
CRadians::PI_OVER_TWO, // angle with Z axis
static_cast<Real>(i+1) * CRadians::PI / 12.0f // rotation around Z
);
/* Set orientation */
cOrient = m_pcRNG->Uniform(CRadians::UNSIGNED_RANGE);
m_vecInitSetup[i].Orientation.FromEulerAngles(
cOrient, // rotation around Z
CRadians::ZERO, // rotation around Y
CRadians::ZERO // rotation around X
);
}
/*
* Process trial information, if any
*/
try {
UInt32 unTrial;
GetNodeAttribute(t_node, "trial", unTrial);
SetTrial(unTrial);
Reset();
}
catch(CARGoSException& ex) {}
}
/****************************************/
/****************************************/
void CMPGAPhototaxisLoopFunctions::Reset() {
/*
* Move robot to the initial position corresponding to the current trial
*/
if(!MoveEntity(
m_pcFootBot->GetEmbodiedEntity(), // move the body of the robot
m_vecInitSetup[GetTrial()].Position, // to this position
m_vecInitSetup[GetTrial()].Orientation, // with this orientation
false // this is not a check, leave the robot there
)) {
LOGERR << "Can't move robot in <"
<< m_vecInitSetup[GetTrial()].Position
<< ">, <"
<< m_vecInitSetup[GetTrial()].Orientation
<< ">"
<< std::endl;
}
}
/****************************************/
/****************************************/
void CMPGAPhototaxisLoopFunctions::ConfigureFromGenome(const Real* pf_genome) {
/* Copy the genes into the NN parameter buffer */
for(size_t i = 0; i < GENOME_SIZE; ++i) {
m_pfControllerParams[i] = pf_genome[i];
}
/* Set the NN parameters */
m_pcController->GetPerceptron().SetOnlineParameters(GENOME_SIZE, m_pfControllerParams);
}
/****************************************/
/****************************************/
Real CMPGAPhototaxisLoopFunctions::Score() {
/* The performance is simply the distance of the robot to the origin */
return m_pcFootBot->GetEmbodiedEntity().GetOriginAnchor().Position.Length();
}
/****************************************/
/****************************************/
REGISTER_LOOP_FUNCTIONS(CMPGAPhototaxisLoopFunctions, "mpga_phototaxis_loop_functions")
<- back