// *
// * update energy level for each agents (ONLY active agents) (only in experimental setups featuring energy items)
// *
void MedeaAltruismReplayWorldObserver::updateAllAgentsEnergyLevel()
{
for ( int i = 0 ; i != gAgentCounter ; i++ ) // for each agent
{
MedeaAltruismAgentWorldModel *currentAgentWorldModel = dynamic_cast<MedeaAltruismAgentWorldModel*>(gWorld->getAgent(i)->getWorldModel());
// * check energy level. Becomes inactive if zero.
/*
if ( currentAgentWorldModel->getEnergyLevel() <= 0 )
{
currentAgentWorldModel->setDeltaEnergy(0);
currentAgentWorldModel->setActiveStatus(false);
currentAgentWorldModel->setWaitingAfterLifeSynchronization(true);
}*/
// * if active, check if agent harvests energy. (experimental setup dependant)
if ( currentAgentWorldModel->getActiveStatus() == true )
{
// * update agent energy (if needed) - agent should be on an active energy point location to get energy
Point2d posRobot(currentAgentWorldModel->_xReal,currentAgentWorldModel->_yReal);
if ( gEnergyMode && currentAgentWorldModel->getActiveStatus())
{
for(std::vector<EnergyPoint>::iterator it = gEnergyPoints.begin(); it != gEnergyPoints.end(); it++)
{
if( (getEuclidianDistance (posRobot,it->getPosition()) < gEnergyPointRadius) && (it->getActiveStatus()))
{
float loadingEnergy = currentAgentWorldModel->getEnergyHarvestingRate() * gEnergyPointValue;
//float loadingEnergy = std::max( 0.0 , std::min( double(gEnergyPointValue), MedeaAltruismSharedData::gEnergyMax - currentAgentWorldModel->getEnergyLevel() ) - fixedCost);
//float loadingEnergy = 5*(1.0/(2.0*sqrt(2.0*M_PI)))*gEnergyPointValue; // test?
//float loadingEnergy = 5*(1.0/(2.0*sqrt(2.0*M_PI)))*exp(-(pow((_key - it->getKey()),2.0)/(pow(2.0,2.0))))*gEnergyPointValue;
double energyMissing = MedeaAltruismSharedData::gEnergyMax-currentAgentWorldModel->getEnergyLevel();
double costMeasure = 0.0;
if ( currentAgentWorldModel->getEnergyLevel() + loadingEnergy < MedeaAltruismSharedData::gEnergyMax )
{
if (gEnergyPointValue > energyMissing)
{
costMeasure = energyMissing - loadingEnergy;
}
else
{
costMeasure = gEnergyPointValue - loadingEnergy;
}
}
gLogFile << gWorld->getIterations() << " : " << currentAgentWorldModel->_agentId << " c " << costMeasure << std::endl;
// update energy level
currentAgentWorldModel->setEnergyLevel(currentAgentWorldModel->getEnergyLevel() + loadingEnergy);
currentAgentWorldModel->setDeltaEnergy(currentAgentWorldModel->getDeltaEnergy() + loadingEnergy);
//saturate
if ( currentAgentWorldModel->getEnergyLevel() > MedeaAltruismSharedData::gEnergyMax ) // assume: need MedeaAltruismSharedData::gEvaluationTime to live full life
currentAgentWorldModel->setEnergyLevel(MedeaAltruismSharedData::gEnergyMax);
double loadingRatio = loadingEnergy/gEnergyPointValue;
double exponentialRespawn = exp(loadingRatio*exponentialFactor)*(MedeaAltruismSharedData::gHighestBoundRespawn/exp(exponentialFactor));
it->setRespawnLag(exponentialRespawn);
//it->setRespawnLag((loadingEnergy/gEnergyPointValue)*MedeaAltruismSharedData::gHighestBoundRespawn);
it->setActiveStatus(false);
}
}
}
}
// * update agent energy consumption -- if inactive, "revive" the agent (ie. it ran out of energy)
// decrease the energyLevel and deltaEnergyLevel
if (currentAgentWorldModel->getActiveStatus() == true )
{
if ( currentAgentWorldModel->getEnergyLevel() > 0.0 )
{
currentAgentWorldModel->setEnergyLevel(currentAgentWorldModel->getEnergyLevel()-1);
}
currentAgentWorldModel->setDeltaEnergy(currentAgentWorldModel->getDeltaEnergy()-1);
}
if ( currentAgentWorldModel->getEnergyLevel() <= 0.0 )
{
//gLogFile << gWorld->getIterations() << " : " << currentAgentWorldModel->_agentId << " die" << std::endl;
currentAgentWorldModel->resetActiveGenome();
currentAgentWorldModel->setMaturity(1);
currentAgentWorldModel->setEnergyLevel(MedeaAltruismSharedData::gEnergyRevive);
currentAgentWorldModel->setDeltaEnergy(0.0);
currentAgentWorldModel->setLifeTime(0);
currentAgentWorldModel->setActiveStatus(false);
currentAgentWorldModel->setWaitForGenome(false);
currentAgentWorldModel->_genomesList.empty();
}
}
}
// perform one controller update
// set motor value using motor format.
void MedeaSpPerceptronControlArchitecture::step()
{
_iteration++;
if ( _wm->getNewGenomeStatus() ) // check for new NN parameters
{
reset();
_wm->setNewGenomeStatus(false);
}
/* if ( _wm->_age < 0 ) // problem: _age is nowhere to be incremented
{
// ** security control (prior to a new behavior, get out of crash situation) -- random noise to avoid "STALL" status
_wm->_desiredTranslationalValue = ( ranf()*2.-1. ) * gMaxTranslationalSpeed ;
_wm->_desiredRotationalVelocity =( ranf()*2.-1. ) * gMaxRotationalSpeed ;
currentObserver->setKey( ( ranf()*2.-1. ) * MedeaSpSharedData::gMaxKeyRange);
return;
}
*/
_wm->_desiredTranslationalValue = 0.0;
_wm->_desiredRotationalVelocity = 0.0;
//We take the world observer which store now the energypoints vector!
/////////// NO MORE "gEnergyPoints"!!!!!!!
MedeaSpWorldObserver * wo = dynamic_cast<MedeaSpWorldObserver * >(_wm->_world->getWorldObserver());
std::vector< MedeaSpEnergyPoint*> * allEP=wo->getEnergyPoints();
if ( _wm->getActiveStatus() == true && !MedeaSpSharedData::gExperimentNoMovements)
{
double angleToClosestEnergyPoint = 0.0;
double shortestDistance = 0.0; //search the current active energy point
Point2d posRobot(_wm->_xReal,_wm->_yReal);
for(int i = 0; i<MedeaSpSharedData::gNbTypeResource ; i++)
{
//Could be better : trying to generalise for n type of energy point
std::vector<MedeaSpEnergyPoint*>::iterator closestPoint = allEP->begin();
//Look for the first point of the good type
while( (int)((*closestPoint)->getType()) != i ){
closestPoint++;
}
shortestDistance = getEuclidianDistance (posRobot,(*closestPoint)->getPosition());
for(std::vector<MedeaSpEnergyPoint*>::iterator it = allEP->begin(); it < allEP->end(); it++)//Can be optimised and starting with "closest point"would allow us not to check previous points again.j
{
if (((*it)->getType() == i))
{
double newDistance = getEuclidianDistance (posRobot,(*it)->getPosition());
if(newDistance < shortestDistance)
{
shortestDistance = newDistance;
closestPoint = it;
}
}
}
//compute the orientation of the active sun ( in degree between 0 and 360 )
//compute the orientation of the closest energy point ( in degree between 0 and 360 )
angleToClosestEnergyPoint = (atan2((*closestPoint)->getPosition().y-posRobot.y,(*closestPoint)->getPosition().x- posRobot.x)/M_PI)*180.0;
angleToClosestEnergyPoint += 360.0 ;
angleToClosestEnergyPoint = computeModulo(angleToClosestEnergyPoint,360.0);
if ( angleToClosestEnergyPoint > 180 ) // force btw -180 and 180
angleToClosestEnergyPoint -= 360.0;
//compute the angle between the actual orientation of the robot and the orientation of the closest energy point ( in degree between -180 and 180 )
double diffAngleToClosestEnergyPoint= angleToClosestEnergyPoint - _wm->_agentAbsoluteOrientation ;
if ( diffAngleToClosestEnergyPoint< -180.0 )
{
diffAngleToClosestEnergyPoint+= 360.0 ;
}
if ( diffAngleToClosestEnergyPoint> 180.0 )
{
diffAngleToClosestEnergyPoint-= 360.0 ;
}
//cast the diffAngle between -1 and 1
diffAngleToClosestEnergyPoint= diffAngleToClosestEnergyPoint/ 180.0 ;
_wm->setEnergyPointDirectionAngleValue(i,diffAngleToClosestEnergyPoint);
//cast the shortest distance between 0 and 1
if ( shortestDistance > gSensorRange )
shortestDistance = 1.0;
else
shortestDistance = shortestDistance / (double)gSensorRange;
_wm->setEnergyPointDistanceValue(i,shortestDistance);
// if ( gAgentIndexFocus == _wm->_agentId )
if ( gVerbose && gInspectAgent && gAgentIndexFocus == _wm->_agentId )
{
std::cout << "SunSensorValue: " << _wm->getEnergyPointDirectionAngleValue(i) << " , " << _wm->getEnergyPointDistanceValue(i) << std::endl;
}
}
std::vector<double> hiddenLayer;
hiddenLayer.resize(_nbHiddenNeurons);
for (int j= 0 ; j < _nbHiddenNeurons ; j++ )
{
hiddenLayer[j] = 0.0;
}
int geneToUse = 0;
// inputs to hidden Layer
// distance sensors
for ( int i = 0 ; i < _wm->_sensorCount ; i++ )
{
for (int j= 0 ; j < _nbHiddenNeurons ; j++ )
{
hiddenLayer[j] += (_wm->getSensorDistanceValue(i)/_wm->getSensorMaximumDistanceValue(i)) * _parameters[geneToUse] ; // !N - corrected BUG di0319 - use normalized sensor value in 0...1
geneToUse ++;
}
}
//floor sensor
for (int j= 0 ; j < _nbHiddenNeurons ; j++ )
{
if ( _wm->_floorSensor != 0 ) // binary detector -- either something, or nothing.
hiddenLayer[j] += 1.0 * _parameters[geneToUse];
//hiddenLayer[j] += _wm->_floorSensor/255.0 * _parameters[geneToUse];
geneToUse ++;
}
//direction of the closest energy point of each type
// gLogFile << "nress,"<<MedeaSpSharedData::gNbTypeResource<<std::endl;
for(int i=0 ; i < MedeaSpSharedData::gNbTypeResource ; i++)
{
for (int j= 0 ; j < _nbHiddenNeurons ; j++ )
{
hiddenLayer[j] += _wm->getEnergyPointDirectionAngleValue(i) * _parameters[geneToUse]; // ??? !N - should be angle. naming problem, code correct.
geneToUse ++;
}
}
//direction of the closest energy point
for(int i=0 ; i < MedeaSpSharedData::gNbTypeResource ; i++)
{
// std::cout<<"i:"<< i<<", energydistancevalue: "<<_wm->getEnergyPointDistanceValue(i)<<std::endl ;
for (int j= 0 ; j < _nbHiddenNeurons ; j++ )
{
hiddenLayer[j] += _wm->getEnergyPointDistanceValue(i) * _parameters[geneToUse]; // ??? !N - should be angle. naming problem, code correct.
geneToUse ++;
}
}
//energy level
for (int j= 0 ; j < _nbHiddenNeurons ; j++ )
{
hiddenLayer[j] += (_wm->getEnergyLevel()/MedeaSpSharedData::gEnergyMax) * _parameters[geneToUse]; // !N : added: energy value normalization btw 0 and 1.
geneToUse ++;
}
//bias
for (int j= 0 ; j < _nbHiddenNeurons ; j++ )
{
hiddenLayer[j] += 1.0 * _parameters[geneToUse];
geneToUse ++;
}
//activation function on hidden layer
// for (int j= 0 ; j < _nbHiddenNeurons ; j++ )
// {
// hiddenLayer[j] = tanh(hiddenLayer[j]);
// }
//hiddenLayer to output
_wm->_desiredTranslationalValue = 0;
_wm->_desiredRotationalVelocity = 0;
for (int j= 0 ; j < _nbHiddenNeurons ; j++ )
{
_wm->_desiredTranslationalValue += hiddenLayer[j] * _parameters[geneToUse] ;
geneToUse ++;
}
for (int j= 0 ; j < _nbHiddenNeurons ; j++ )
{
_wm->_desiredRotationalVelocity += hiddenLayer[j] * _parameters[geneToUse] ;
geneToUse ++;
}
_wm->_desiredTranslationalValue += 1.0 * _parameters[geneToUse] ;
geneToUse ++;
// gLogFile<<geneToUse<<" ability "<<_parameters[87]<< " vs "<<_parameters[geneToUse] <<" vs "<<_wm->getAbilityToForage()<<" size"<<_wm->_genome.size()<< std::endl;
_wm->_desiredRotationalVelocity += 1.0 * _parameters[geneToUse] ;
//activation function on output
_wm->_desiredTranslationalValue = tanh( _wm->_desiredTranslationalValue ) ; // !N note that tanh is optional for ANN outputs.
_wm->_desiredRotationalVelocity = tanh( _wm->_desiredRotationalVelocity );
// normalize to motor interval values
_wm->_desiredTranslationalValue = _wm->_desiredTranslationalValue * gMaxTranslationalSpeed;
_wm->_desiredRotationalVelocity = _wm->_desiredRotationalVelocity * gMaxRotationalSpeed;
}
}
// *
// * update energy level for each agents (ONLY active agents) (only in experimental setups featuring energy items)
// *
//TODO : randomize the order
void MedeaSpWorldObserver::updateAllAgentsEnergyLevel()
{
// --- Take from src/world.cpp
// * create an array that contains shuffled indexes. Used afterwards for randomly update agents.
// This is very important to avoid possible nasty effect from ordering such as "agents with low indexes moves first"
// outcome: among many iterations, the effect of ordering is reduced.
// As a results, roborobo is turn-based, with sotchastic updates within one turn
int shuffledIndex[gAgentCounter];
for ( int i = 0 ; i < gAgentCounter ; i++ )
shuffledIndex[i] = i;
for ( int i = 0 ; i < gAgentCounter-1 ; i++ ) // exchange randomly indexes with one other
{
int r = i + (rand() % (gAgentCounter-i)); // Random remaining position.
int tmp = shuffledIndex[i];
shuffledIndex[i] = shuffledIndex[r];
shuffledIndex[r] = tmp;
}
for ( int i = 0 ; i != gAgentCounter ; i++ ) // for each agent
{
MedeaSpAgentWorldModel *currentAgentWorldModel = dynamic_cast<MedeaSpAgentWorldModel*>(gWorld->getAgent(shuffledIndex[i])->getWorldModel());
// * check energy level. Becomes inactive if zero.
// * if active, check if agent harvests energy.
if(gWorld->getIterations() > MedeaSpSharedData::gNoEnergy){
if ( currentAgentWorldModel->getActiveStatus() == true )
{
// * update agent energy (if needed) - agent should be on an active energy point location to get energy
Point2d posRobot(currentAgentWorldModel->_xReal,currentAgentWorldModel->_yReal);
for(std::vector<MedeaSpEnergyPoint*>::iterator it = coopEnergyPoints->begin(); it<coopEnergyPoints->end(); it++)
{
if( (getEuclidianDistance (posRobot,(*it)->getPosition()) < gEnergyPointRadius) && (*it)->getActiveStatus())
{
double energyReachable =forageReward(currentAgentWorldModel->getAbilityToForage(),(*it)->getType());
if(gWorld->getIterations() > MedeaSpSharedData::gNoDenPenTime && energyReachable>0 )
{
(*it)->setQ_E((*it)->getQ_E()-1);
if((*it)->getQ_E()<=0 ) energyReachable=0;
}
double energyAdded;
// update energy level
if(currentAgentWorldModel->getEnergyLevel() + energyReachable > MedeaSpSharedData::gEnergyMax)
{
currentAgentWorldModel->setEnergyLevel(MedeaSpSharedData::gEnergyMax);
energyAdded=currentAgentWorldModel->getEnergyCounterOfOneRessource((*it)->getType()) + double(MedeaSpSharedData::gEnergyMax) - currentAgentWorldModel->getEnergyLevel();
}
else{
currentAgentWorldModel->setEnergyLevel(currentAgentWorldModel->getEnergyLevel() + energyReachable);
energyAdded=currentAgentWorldModel->getEnergyCounterOfOneRessource((*it)->getType()) + energyReachable ;
}
currentAgentWorldModel->setEnergyCounterOfOneRessource((*it)->getType(),energyAdded );
currentAgentWorldModel->setDeltaEnergy(currentAgentWorldModel->getDeltaEnergy() + energyReachable);
}
}
}
// * update agent energy consumption -- if inactive, "revive" the agent (ie. it ran out of energy)
// decrease the energyLevel and deltaEnergyLevel
if ( currentAgentWorldModel->getEnergyLevel() > 0.0 && currentAgentWorldModel->getActiveStatus() == true )
{
currentAgentWorldModel->setEnergyLevel(currentAgentWorldModel->getEnergyLevel()-1);
/***
* Allopatric test
* Deprecated
**/
bool allopatric = false;
if(allopatric){
if(currentAgentWorldModel->_xReal > 490 && currentAgentWorldModel->_xReal < 510){
int mountainCost=0;
mountainCost = 10000;//pow((1-(pow((currentAgentWorldModel->getEnergyLevel()-500),4)/pow(500,4))),1000)*40000;// pow((500-currentAgentWorldModel->getEnergyLevel())/100,100);
// currentAgentWorldModel->resetActiveGenome();
currentAgentWorldModel->_genomesList.clear();
currentAgentWorldModel->setWaitForGenome(true);
currentAgentWorldModel->setActiveStatus(false);
// currentAgentWorldModel->setRobotLED_status(false);
currentAgentWorldModel->setLifeTime(0);
if(currentAgentWorldModel->_xReal < 500) currentAgentWorldModel->_xReal=currentAgentWorldModel->_xReal - (rand() % 300);
else
currentAgentWorldModel->_xReal=currentAgentWorldModel->_xReal+ (rand() % 300);
// currentAgentWorldModel->setEnergyLevel(currentAgentWorldModel->getEnergyLevel() - mountainCost);
currentAgentWorldModel->_yReal=rand() % 200 + 10;
}
}
/***
* -------------
***/
}
currentAgentWorldModel->setDeltaEnergy(currentAgentWorldModel->getDeltaEnergy()-1);
// "revive" agent with empty genomes if ran out of energy.
if ( currentAgentWorldModel->getEnergyLevel() <= 0 && currentAgentWorldModel->getActiveStatus() == true)
{
currentAgentWorldModel->resetEnergyCounter();
currentAgentWorldModel->setMaturity(1);
if (currentAgentWorldModel->_agentId == gAgentIndexFocus && gVerbose) // debug
{
std::cout << "agent #" << gAgentIndexFocus << " is revived (energy was 0)." << std::endl;
}
currentAgentWorldModel->resetActiveGenome();//inutile?
currentAgentWorldModel->setEnergyLevel(MedeaSpSharedData::gEnergyRevive);
currentAgentWorldModel->setEnergyCounterOfOneRessource(currentAgentWorldModel->getEnergyCounter().size()-1,MedeaSpSharedData::gEnergyRevive);
//---The robot is Dead (ie not listening for a genome)
currentAgentWorldModel->setActiveStatus(false);
currentAgentWorldModel->setWaitForGenome(false);
currentAgentWorldModel->_genomesList.clear();
currentAgentWorldModel->_genomesList.empty();
}
}
}
}
// *
// * update energy level for each agents (ONLY active agents) (only in experimental setups featuring energy items)
// *
void MedeaAltruismReplayPartialEvoWorldObserver::updateAllAgentsEnergyLevel()
{
for ( int i = 0 ; i != gAgentCounter ; i++ ) // for each agent
{
MedeaAltruismReplayPartialEvoAgentWorldModel *currentAgentWorldModel = dynamic_cast<MedeaAltruismReplayPartialEvoAgentWorldModel*>(gWorld->getAgent(i)->getWorldModel());
// * check energy level. Becomes inactive if zero.
if ( currentAgentWorldModel->getEnergyLevel() <= 0 )
{
currentAgentWorldModel->setDeltaEnergy(0);
currentAgentWorldModel->setActiveStatus(false);
currentAgentWorldModel->setWaitingAfterLifeSynchronization(true);
}
// * if active, check if agent harvests energy. (experimental setup dependant)
if ( currentAgentWorldModel->getActiveStatus() == true )
{
// * update agent energy (if needed) - agent should be on an active energy point location to get energy
Point2d posRobot(currentAgentWorldModel->_xReal,currentAgentWorldModel->_yReal);
if ( gEnergyMode && currentAgentWorldModel->getActiveStatus())
{
for(std::vector<EnergyPoint>::iterator it = gEnergyPoints.begin(); it != gEnergyPoints.end(); it++)
{
if( (getEuclidianDistance (posRobot,it->getPosition()) < gEnergyPointRadius) && (it->getActiveStatus()))
{
float loadingEnergy = currentAgentWorldModel->getEnergyHarvestingRate() * gEnergyPointValue;
//float loadingEnergy = 5*(1.0/(2.0*sqrt(2.0*M_PI)))*gEnergyPointValue; // test?
//float loadingEnergy = 5*(1.0/(2.0*sqrt(2.0*M_PI)))*exp(-(pow((_key - it->getKey()),2.0)/(pow(2.0,2.0))))*gEnergyPointValue;
gLogFile << gWorld->getIterations() << " : " << currentAgentWorldModel->_agentId
<< " get " << loadingEnergy << " Before " << currentAgentWorldModel->getEnergyLevel() <<std::endl;
// update energy level
currentAgentWorldModel->setEnergyLevel(currentAgentWorldModel->getEnergyLevel() + loadingEnergy);
currentAgentWorldModel->setDeltaEnergy(currentAgentWorldModel->getDeltaEnergy() + loadingEnergy);
currentAgentWorldModel->increaseTotalEnergyHarvested(loadingEnergy);
//saturate
if ( currentAgentWorldModel->getEnergyLevel() > MedeaAltruismSharedData::gEnergyMax ) // assume: need MedeaAltruismSharedData::gEvaluationTime to live full life
currentAgentWorldModel->setEnergyLevel(MedeaAltruismSharedData::gEnergyMax);
it->setRespawnLag((loadingEnergy/gEnergyPointValue)*MedeaAltruismSharedData::gHighestBoundRespawn);
it->setActiveStatus(false);
}
}
}
}
// * update agent energy consumption -- if inactive, "revive" the agent (ie. it ran out of energy)
// decrease the energyLevel and deltaEnergyLevel
if (currentAgentWorldModel->getActiveStatus() == true )
{
if ( currentAgentWorldModel->getEnergyLevel() > 0.0 )
{
currentAgentWorldModel->setEnergyLevel(currentAgentWorldModel->getEnergyLevel()-1);
}
currentAgentWorldModel->setDeltaEnergy(currentAgentWorldModel->getDeltaEnergy()-1);
}
// "revive" agent with empty genomes if ran out of energy.
if ( currentAgentWorldModel->getEnergyLevel() <= 0.0 )
{
gLogFile << gWorld->getIterations() << " : " << currentAgentWorldModel->_agentId << " Restart" << std::endl;
// reformulate (check python script compatibility before): gLogFile << "Info(" << gWorld->getIterations() << ") : robot " << _wm->_agentId << " was revived (human intervention)" << std::endl;
if (currentAgentWorldModel->_agentId == gAgentIndexFocus && gVerbose) // debug
{
std::cout << "agent #" << gAgentIndexFocus << " is revived (energy was 0)." << std::endl;
}
currentAgentWorldModel->resetActiveGenome();
currentAgentWorldModel->setEnergyLevel(MedeaAltruismSharedData::gEnergyRevive); // !n : too few?
currentAgentWorldModel->setActiveStatus(false); // true: restart, false: no-restart
currentAgentWorldModel->setWaitingAfterLifeSynchronization(true);
currentAgentWorldModel->_genomesList.empty();
}
}
}
void MedeaAltUtilityBattleAgentObserver::step()
{
_iterationCount++;
if ( _wm->_agentId == 0 )
{
if ( MedeaAltUtilitySharedData::gExperimentNumber == 2 )
{
if (_wm->_agentId == gAgentIndexFocus ) // && gVerbose ) // debug
{
std::cout << std::endl << "#### Experiment no.2 starts now. ####" << std::endl;
}
// * remove energy points.
for(std::vector<EnergyPoint>::iterator it = gEnergyPoints.begin(); it != gEnergyPoints.end(); it++)
{
it->hide();
}
gEnergyPoints.clear();
// * setup new energy zone
Uint32 colorShown = 0xeab71fff;
Uint32 colorZone = 0xAAAAAAFF; // for floor sensor.
for (Sint16 xColor = MedeaAltUtilitySharedData::g_xStart_EnergyZone ; xColor < MedeaAltUtilitySharedData::g_xEnd_EnergyZone ; xColor++)
{
for (Sint16 yColor = Sint16 (MedeaAltUtilitySharedData::g_yStart_EnergyZone) ; yColor < Sint16 (MedeaAltUtilitySharedData::g_yEnd_EnergyZone) ; yColor ++)
{
pixelColor(gBackgroundImage, xColor, yColor, colorShown);
pixelColor(gZoneImage, xColor, yColor, colorZone);
}
}
}
}
// std::cout << "robot #" << _wm->_agentId << "\n" ;
MedeaAltUtilityPerceptronControlArchitecture* currentBehavior = dynamic_cast<MedeaAltUtilityPerceptronControlArchitecture*>(gWorld->getAgent(_wm->_agentId)->getBehavior());
if ( ! currentBehavior )
{
std::cerr << "Error from robot " << _wm->_agentId << " : the behavior architecture of this robot isn't a MedeaAltUtilityPerceptronControlArchitecture" << std::endl;
exit(1);
}
/*if (_wm->_agentId == 1)
{
std::cout << _key <<std::endl;
}*/
if ( gVerbose && gInspectAgent && gAgentIndexFocus == _wm->_agentId )
{
//std::cout << "target: " << _wm->getEnergyPointDirectionAngleValue() << std::endl;// " (" << _wm->_agentAbsoluteOrientation << "," << angleToClosestEnergyPoint << ")" << std::endl;
}
if ( MedeaAltUtilitySharedData::gExperimentNumber == 1 )
{
// * update the energy of the robot (if needed)
Point2d posRobot(_wm->_xReal,_wm->_yReal);
if ( gEnergyMode )
{
for(std::vector<EnergyPoint>::iterator it = gEnergyPoints.begin(); it != gEnergyPoints.end(); it++)
{
if( (getEuclidianDistance (posRobot,it->getPosition()) < gEnergyPointRadius) && (it->getActiveStatus()))
{
float loadingEnergy = gEnergyPointValue; // test?
//float loadingEnergy = 5*(1.0/(2.0*sqrt(2.0*M_PI)))*gEnergyPointValue; // test?
//float loadingEnergy = 5*(1.0/(2.0*sqrt(2.0*M_PI)))*exp(-(pow((_key - it->getKey()),2.0)/(pow(2.0,2.0))))*gEnergyPointValue;
// update energy level
_wm->setEnergyLevel(_wm->getEnergyLevel() + loadingEnergy);
_wm->setDeltaEnergy(_wm->getDeltaEnergy() + loadingEnergy);
//saturate
if ( _wm->getEnergyLevel() > MedeaAltUtilitySharedData::gEnergyMax ) // should be at least one lifetime
_wm->setEnergyLevel(MedeaAltUtilitySharedData::gEnergyMax);
if ( _wm->getDeltaEnergy() > MedeaAltUtilitySharedData::gEnergyMax )
_wm->setDeltaEnergy(MedeaAltUtilitySharedData::gEnergyMax);
gLogFile << "Info(" << gWorld->getIterations() << ") : " << _wm->_agentId
<< "(" << posRobot.x << "," << posRobot.y << ")"
<< " get an energy point at " << it->getPosition().x << "," << it->getPosition().y << " :: Value : " << loadingEnergy << std::endl;
it->setActiveStatus(false);
}
}
}
}
else
{
if ( MedeaAltUtilitySharedData::gExperimentNumber == 2 )
{
// * once per world update (TODO: move to worldobserver)
if (_wm->_agentId == 0 ) // debug
{
int agentsOnZone = 0;
for ( int i = 0 ; i != gAgentCounter ; i++ )
{
int x = (int)(gWorld->getAgent(i)->getWorldModel()->getXReal());
int y = (int)(gWorld->getAgent(i)->getWorldModel()->getYReal());
// std::cout << "x =" << x << " , y = " << y << std::endl;
if ( x >= MedeaAltUtilitySharedData::g_xStart_EnergyZone && y >= MedeaAltUtilitySharedData::g_yStart_EnergyZone && x <= MedeaAltUtilitySharedData::g_xEnd_EnergyZone && y <= MedeaAltUtilitySharedData::g_yEnd_EnergyZone )
agentsOnZone++;
}
// update MedeaAltUtilitySharedData::gZoneEnergy_harvestValue
//MedeaAltUtilitySharedData::gZoneEnergy_harvestValue = 10; // TODO :: TEMPORARY !!!!!!!!!!TEMPORARY !!!!!!!!!!TEMPORARY !!!!!!!!!!TEMPORARY !!!!!!!!!!TEMPORARY !!!!!!!!!!
if ( gVerbose )
std::cout << "There are " << agentsOnZone << " agents on the energy zone" << std::endl;
/**/
if ( agentsOnZone <= MedeaAltUtilitySharedData::gZoneEnergy_maxFullCapacity )
{
// best case
MedeaAltUtilitySharedData::gZoneEnergy_harvestValue = MedeaAltUtilitySharedData::gZoneEnergy_maxHarvestValue;
}
else
{
if ( agentsOnZone <= MedeaAltUtilitySharedData::gZoneEnergy_saturateCapacityLevel )
{
double energyValueSpan = MedeaAltUtilitySharedData::gZoneEnergy_maxHarvestValue - MedeaAltUtilitySharedData::gZoneEnergy_minHarvestValue;
int agentsOverheadCount = MedeaAltUtilitySharedData::gZoneEnergy_saturateCapacityLevel - MedeaAltUtilitySharedData::gZoneEnergy_maxFullCapacity;
double costPerAgents = energyValueSpan / (double)agentsOverheadCount;
MedeaAltUtilitySharedData::gZoneEnergy_harvestValue = MedeaAltUtilitySharedData::gZoneEnergy_maxHarvestValue - costPerAgents * ( agentsOnZone- MedeaAltUtilitySharedData::gZoneEnergy_maxFullCapacity ) ;
}
else
{
// worst case
MedeaAltUtilitySharedData::gZoneEnergy_harvestValue = MedeaAltUtilitySharedData::gZoneEnergy_minHarvestValue;
}
}
/**/
// debug : MedeaAltUtilitySharedData::gZoneEnergy_harvestValue = MedeaAltUtilitySharedData::gZoneEnergy_maxHarvestValue;
}
// * for each agent -- TODO: could be optimized by merging with previous block in the worldobserve
if ( _wm->_xReal >= MedeaAltUtilitySharedData::g_xStart_EnergyZone && _wm->_xReal <= MedeaAltUtilitySharedData::g_xEnd_EnergyZone && _wm->_yReal >= MedeaAltUtilitySharedData::g_yStart_EnergyZone && _wm->_yReal <= MedeaAltUtilitySharedData::g_yEnd_EnergyZone )
{
float loadingEnergy = MedeaAltUtilitySharedData::gZoneEnergy_harvestValue;
// update energy level
_wm->setEnergyLevel(_wm->getEnergyLevel() + loadingEnergy);
_wm->setDeltaEnergy(_wm->getDeltaEnergy() + loadingEnergy);
// saturate
if ( _wm->getEnergyLevel() > MedeaAltUtilitySharedData::gEnergyMax ) // assume: need MedeaAltUtilitySharedData::gEvaluationTime to live full life
_wm->setEnergyLevel(MedeaAltUtilitySharedData::gEnergyMax);
if ( _wm->getDeltaEnergy() > MedeaAltUtilitySharedData::gEnergyMax ) // assume: need MedeaAltUtilitySharedData::gEvaluationTime to live full life
_wm->setDeltaEnergy(MedeaAltUtilitySharedData::gEnergyMax);
Point2d posRobot(_wm->_xReal,_wm->_yReal);
gLogFile << "Info(" << gWorld->getIterations() << ") : " << _wm->_agentId
<< "(" << posRobot.x << "," << posRobot.y << ")"
<< " get an energy point at 0,0 :: Value : " << loadingEnergy << std::endl; // hack to comply with python log analyser
}
}
}
// * check energy level
if ( MedeaAltUtilitySharedData::gExperimentNumber == 1 || MedeaAltUtilitySharedData::gExperimentNumber == 2 )
{
if ( _wm->getEnergyLevel() <= 0 )
{
_wm->setDeltaEnergy(0); // must be set to zero to avoid broadcasting.
_wm->setActiveStatus(false);
}
}
// * broadcast the genome (current agent writes its genome to all neighbors
// broadcast only if agent is active (ie. not just revived) and deltaE>0.
if (
/*( _wm->getDeltaEnergy()>0.0 && _wm->getActiveStatus() == true )
||
( MedeaAltUtilitySharedData::gExperimentNumber == 0 && _wm->getActiveStatus() == true )*/
_wm->getActiveStatus() == true
)
{
for (int i = 0 ; i < gAgentCounter ; i++)
{
if ( ( i != _wm->_agentId ) && ( gRadioNetworkArray[_wm->_agentId][i] ) ) //&& (_wm->getEnergyLevel() > 0.0) ) --> always true as status is active
{
MedeaAltUtilityBattleAgentObserver* agentObserver = dynamic_cast<MedeaAltUtilityBattleAgentObserver*>(gWorld->getAgent(i)->getObserver());
if ( ! agentObserver )
{
std::cerr << "Error from robot " << _wm->_agentId << " : the observer of robot " << i << " isn't a MedeaAltUtilityBattleAgentObserver" << std::endl;
exit(1);
}
agentObserver->writeGenome(_currentGenome, _wm->_agentId);
}
}
}
// * handle genome renewal
//"restart" the robot in case it runs out of energy -- case: no synchronisation
/*
if ( ( _wm->getEnergyLevel() <= 0.0 ) && ( MedeaAltUtilitySharedData::gSynchronization == false ) ){
logStatus();
//resetActiveGenome();
_wm->setEnergyLevel( currentBehavior->getInitialEnergy());
_wm->setDeltaEnergy(0.0);
gLogFile << "Info(" << gWorld->getIterations() << ") : Human intervention on robot " << _wm->_agentId << " (Energy)" << std::endl;
_iterationCount = 0;
_wm->setActiveStatus(false); // !N.20100407 : inactive robot should get a new genome and move until it imports one from neighbors.
}
*/
// case: default for Medea, synchronised
if( _iterationCount >= MedeaAltUtilitySharedData::gEvaluationTime )
{
/**/
if (_wm->_agentId == gAgentIndexFocus && gVerbose) // debug
{
std::cout << "agent #" << gAgentIndexFocus << " is renewed" << std::endl;
std::cout << "agent #" << gAgentIndexFocus << " imported " << _genomesList.size() << " genomes. Energy is " << _wm->getEnergyLevel() << ". Status is " << _wm->getActiveStatus() << "." <<std::endl;
}
/**/
logStatus();
//"revive" the robot in case it runs out of energy
if ( MedeaAltUtilitySharedData::gExperimentNumber == 1 || MedeaAltUtilitySharedData::gExperimentNumber == 2 )
{
if ( _wm->getEnergyLevel() <= 0.0 )
{
gLogFile << "Info(" << gWorld->getIterations() << ") : Human intervention on robot " << _wm->_agentId << " (Energy)" << std::endl;
// reformulate (check python script compatibility before): gLogFile << "Info(" << gWorld->getIterations() << ") : robot " << _wm->_agentId << " was revived (human intervention)" << std::endl;
if (_wm->_agentId == gAgentIndexFocus && gVerbose) // debug
{
std::cout << "agent #" << gAgentIndexFocus << " is revived (energy was 0)." << std::endl;
}
logStatus();
//resetActiveGenome();
_wm->setEnergyLevel(MedeaAltUtilitySharedData::gEnergyRevive); // !n : too few?
_wm->setActiveStatus(false); // true: restart, false: no-restart
_genomesList.empty();
}
}
//else // uncomment if restart
// note: at this point, agent got energy, wether because it was revived or because of remaining energy.
// case: genome(s) imported, random pick.
if (_genomesList.size() > 0)
{
pickRandomGenome();
_wm->setActiveStatus(true); // !N.20100407 : revive takes imported genome if any
}
// case: no imported genome - wait for new genome.
else
{
gLogFile << "Info(" << gWorld->getIterations() << ") : robot nb." << _wm->_agentId
// << " is trying a whole new genome" << std::endl;
<< " is waiting for a new genome" << std::endl;
//resetActiveGenome(); // optional -- could be set to zeroes.
_wm->setActiveStatus(false); // !N.20100407 : inactive robot must import a genome from others.
}
//log the genome
gLogFile << "get active status" << std::endl ;
if ( _wm->getActiveStatus() == true )
{
gLogFile << "Info("<< gWorld->getIterations() <<") : robot nb."<< _wm->_agentId << " use genome :";
for(unsigned int i=0; i<_wm->_genome.size(); i++)
{
gLogFile << std::fixed << std::showpoint<< _wm->_genome[i] << " ";
}
gLogFile << std::endl;
}
//Re-initialize the main parameters
if ( MedeaAltUtilitySharedData::gExperimentNumber == 1 || MedeaAltUtilitySharedData::gExperimentNumber == 2 )
{
_wm->setDeltaEnergy(0.0); // !n : avant: 10.0
}
_iterationCount = 0;
_generationCount ++;
if ( _wm->_agentId == 0 )
{
if ( !gVerbose )
{
//std::cout << ".";
int activeCount = 0;
for ( int i = 0 ; i != gAgentCounter ; i++ )
{
if ( _wm->getActiveStatus() == true )
activeCount++;
}
std::cout << "[" << activeCount << "]";
}
}
}
}
void ChangeDetectionController::createOrganism(double &left, double &right, int desired_size, double speed) {
ChangeDetectionAgentWorldModel* worldModel = dynamic_cast<ChangeDetectionAgentWorldModel*> (_wm);
RobotAgentPtr agent = gWorld->getAgent(worldModel->_agentId);
// You are only eager to connect when your current organism is not large enough according to your standards.
if (agent->isPartOfOrganism()){
Organism *o = agent->getOrganism();
if (o->size() < desired_size){
agent->setConnectToOthers(Agent::POSITIVE);
}else{
agent->setConnectToOthers(Agent::NEUTRAL);
}
}else{
agent->setConnectToOthers(Agent::POSITIVE);
}
if (agent->getConnectToOthers() == Agent::NEGATIVE) {
avoidObstacles(left, right, speed);
return;
}
Point2d posRobot(worldModel->getPosition().x, worldModel->getPosition().y);
// double closestDistance = getMaximumDistance();
bool found = false;
Point2d closestPoint;
RobotAgentPtr closest;
vector<RobotAgentPtr> close = agent->getNearRobots();
// find robots that we can connect to
vector<RobotAgentPtr>::iterator it;
for (it = close.begin(); it != close.end(); it++) {
if ((*it)->getConnectToOthers() == Agent::POSITIVE) {
//Point2d closeRobot((*it)->getWorldModel()->_xReal, (*it)->getWorldModel()->_yReal);
//double distance = getEuclidianDistance(posRobot, closeRobot);
//if (distance < closestDistance) {
//found = true;
//closestDistance = distance;
//closestPoint = closeRobot;
//closest = (*it);
//}
}
}
// No robots that want to connect are close
if (!found) {
avoidObstacles(left, right, speed);
} else {
// steer towards the closest robot that wants to connect
double angle = (atan2(closestPoint.y - posRobot.y, closestPoint.x - posRobot.x) / M_PI) * 180.0;
double diffAngle = angle - _wm->_agentAbsoluteOrientation;
if (diffAngle < -180.0) {
diffAngle += 360.0;
}
if (diffAngle > 180.0) {
diffAngle -= 360.0;
}
followAngle(diffAngle, left, right);
left *= speed;
right *= speed;
}
}
