/**
* This function sets up connections between robots who are close enough to
* each other and have their connection ring set to 1.
*/
void RobotAgent::setUpConnections() {
if (!gUseOrganisms) return;
int nAgents = _wm->_world->getNbOfAgent();
int id = _wm->_agentId;
if (this->isPartOfOrganism()) {
if (this->getConnectToOthers() == Agent::NEGATIVE) {
this->letGoOfOrganism();
}/* else if (this->getConnectToOthers() == Agent::NEUTRAL) {
std::vector<RobotAgentPtr>::iterator it;
for (it = this->connected->begin(); it != this->connected->end(); it++) {
RobotAgentPtr other = (*it);
if (other->getConnectToOthers() == Agent::NEUTRAL) {
it = this->connected->erase(it);
other->removeNeighbour(gWorld->getAgent(this->_wm->_agentId));
break;
}
}
}*/
if (this->connected->empty()) {
this->letGoOfOrganism();
}
}
for (int i = 0; i < nAgents; i++) {
RobotAgentPtr other = _wm->_world->getAgent(i);
RobotAgentWorldModel *otherWM = (RobotAgentWorldModel*) other->getWorldModel();
int otherId = otherWM->_agentId;
if (otherId != id) {
// If they're out of bounds, ignore.
// This happens at the start of the simulation sometimes and can lead
// to false positives
if (this->isOutOfBounds() || other->isOutOfBounds()) {
return;
}
double x1, y1, x2, y2;
x1 = this->_wm->_xReal;
y1 = this->_wm->_yReal;
x2 = other->_wm->_xReal;
y2 = other->_wm->_yReal;
// Are they within range?
if (SDL_CollideBoundingCircle(gAgentMaskImage, x1, y1, gAgentMaskImage, x2, y2, gConnectionGap)) {
// connect to other robot
// ---- Mark: but only connect in case you are willing to do so.
if ((this->getConnectToOthers() == Agent::POSITIVE) && (other->getConnectToOthers() == Agent::POSITIVE)) {
// || other->getConnectToOthers() == Agent::NEUTRAL)) {
this->connectToRobot(other);
}
// else if (this->getConnectToOthers() == Agent::NEUTRAL && other->getConnectToOthers() == Agent::POSITIVE) {
// this->connectToRobot(other);
// }
}
}
}
}
void DynamicController::leaveOrganism(double &left, double &right) {
DynamicAgentWorldModel* worldModel = dynamic_cast<DynamicAgentWorldModel*> (_wm);
RobotAgentPtr agent = gWorld->getAgent(worldModel->_agentId);
if (agent->isPartOfOrganism()){
// Note: implicit knowledge here: leaving means no longer wanting to connect until create organism is called again.
agent->setConnectToOthers(Agent::NEGATIVE);
agent->letGoOfOrganism();
}
}
void DynamicController::setLED() {
DynamicAgentWorldModel* worldModel = dynamic_cast<DynamicAgentWorldModel*> (_wm);
RobotAgentPtr agent = gWorld->getAgent(worldModel->_agentId);
worldModel->setRobotLED_colorValues(0, 255, 0);
if(agent->isPartOfOrganism() && agent->getOrganism()->leader == agent){
worldModel->setRobotLED_status(true);
}else{
worldModel->setRobotLED_status(false);
}
}
void ParcoursWorldInterface::rePlaceRobot(RobotAgentPtr agent) {
agent->unregisterAgent();
do {
int x = (int) 20 + (ranf() * (double) (gSpawnWidth));
int y = (int) 20 + (ranf() * (double) (gSpawnHeight));
agent->setCoordReal(x, y);
agent->setCoord(x, y);
Point2d pos(x,y);
agent->oldPosition = pos;
}while (agent->isCollision());
agent->registerAgent();
}
vector<double> DynamicController::getSensorValues() {
DynamicAgentWorldModel* worldModel = dynamic_cast<DynamicAgentWorldModel*> (_wm);
/* Inputs:
* Distance Sensors x5
* Energy Sensors x3
* Energy Value Sensor x3
* organism size
* energy level
* 1 bias node
*/
vector<double> sensors;
for (int i = 0; i < 5; i++) {
sensors.push_back( _wm->getDefaultSensors()->getSensorDistanceValue(i) / _wm->getDefaultSensors()->getSensorMaximumDistanceValue(i));
}
for(int i=1;i<4;i++){
sensors.push_back(worldModel->getTypedEnergySensor()->getSensorValue(i) / worldModel->getTypedEnergySensor()->getSensorMaximumDistanceValue(i));
sensors.push_back(worldModel->getTypedEnergySensor()->getSensorTypeValue(i) / DynamicSharedData::NUM_WEIGHTS);
}
// Organism Size
RobotAgentPtr agent = gWorld->getAgent(worldModel->_agentId);
if (agent->getOrganism()) {
sensors.push_back(agent->getOrganism()->size() / gWorld->getNbOfAgent());
} else {
sensors.push_back(0.0);
}
// Energy Level
sensors.push_back(worldModel->getEnergyLevel() / DynamicSharedData::MAX_ENERGY);
// bias node
sensors.push_back(1.0);
/**
* Make sure the inputs are correctly normalised
*/
for(int i=0;i< sensors.size(); i++){
assert(sensors[i] <= 1.0 && sensors[i] >= -1.0);
}
return sensors;
}
bool Organism::contains(RobotAgentPtr robot) {
std::vector<RobotAgentPtr>::iterator it;
for (it = this->agents.begin(); it != this->agents.end(); it++) {
if ((*it)->getWorldModel()->_agentId == robot->getWorldModel()->_agentId) {
return true;
}
}
return false;
}
// Just gather all nearby genomes. Duplicates will be handled elsewhere.
void MONEEControlArchitecture::gatherGenomes(std::vector<Genome*> & genePool, int commDistSquared) {
genePool.clear();
std::vector<RobotAgentPtr>::iterator itAgent;
if (commDistSquared < 0) {
for (int i=0;i<gWorld->getNbOfAgent(); i++) {
RobotAgentPtr agent = gWorld->getAgent(i);
MONEEControlArchitecture* controller = static_cast<MONEEControlArchitecture*>(agent->getBehavior());
genePool.push_back(controller->getGenome());
}
} else {
for (int i=0;i<gWorld->getNbOfAgent(); i++) {
RobotAgentPtr agent = gWorld->getAgent(i);
if (isRadioConnection(agent->getWorldModel()->_agentId,_wm->_agentId)) {
MONEEControlArchitecture* controller = static_cast<MONEEControlArchitecture*>(agent->getBehavior());
genePool.push_back(controller->getGenome());
}
}
}
}
void Organism::removeRobot(RobotAgentPtr robot) {
std::vector<RobotAgentPtr>::iterator it;
for (it = this->agents.begin(); it != this->agents.end(); it++) {
if ((*it)->getWorldModel()->_agentId == robot->getWorldModel()->_agentId) {
this->agents.erase(it);
break;
}
}
robot->setOrganism(NULL);
// Remove the organism if there is less than 1 robot left in it
if (this->agents.size() < 1) {
this->leader = RobotAgentPtr();
this->setInactive();
}else{
// Create another leader in case the leader has just been removed. We'll just take the first one in the agents vector.
if (this->leader == robot){
this->leader = agents.at(0);
}
}
}
vector<double> ChangeDetectionController::getSensorValues() {
ChangeDetectionAgentWorldModel* worldModel = dynamic_cast<ChangeDetectionAgentWorldModel*> (_wm);
vector<double> sensors;
//for (int i = 0; i < worldModel->_sensorCount; i++) {
//sensors.push_back(worldModel->getSensorDistanceValue(i) / worldModel->getSensorMaximumDistanceValue(i));
//sensors.push_back(worldModel->getDitchDistanceValue(i) / worldModel->getSensorMaximumDistanceValue(i));
//}
RobotAgentPtr agent = gWorld->getAgent(worldModel->_agentId);
if (agent->getOrganism()) {
sensors.push_back(agent->getOrganism()->size() / gWorld->getNbOfAgent());
} else {
sensors.push_back(0.0);
}
sensors.push_back(0.0); // always move right
sensors.push_back((gEnvironmentImage->w - worldModel->getPosition().x) / gEnvironmentImage->w); // distance to goal
return sensors;
}
/**
* Check whether the organism is still connected
*/
std::vector<Organism*> Organism::checkIntegrity() {
std::vector<Organism*> orgs = std::vector<Organism*>();
if (!isActive()) {
return orgs;
}
std::vector<RobotAgentPtr> not_visited = std::vector<RobotAgentPtr>(this->agents);
std::vector<RobotAgentPtr> loners = std::vector<RobotAgentPtr>();
std::vector<RobotAgentPtr> connected = std::vector<RobotAgentPtr>();
std::stack<RobotAgentPtr> s = std::stack<RobotAgentPtr>();
// Tree visiting algorithm using a stack
// All nodes of the "tree" should be visited, otherwise
// the organism is falling apart
// Repeat until all robots in the (ex)organism are accounted for
while (!not_visited.empty()) {
s.push(not_visited.front());
while (!s.empty()) {
RobotAgentPtr a = s.top();
s.pop();
// This robot has been visited
std::vector<RobotAgentPtr>::iterator it;
it = std::find(not_visited.begin(), not_visited.end(), a);
if (it != not_visited.end()) {
not_visited.erase(it);
}
// so it is connected to the current organism
connected.push_back(a);
// Add the neighbours of this robot to the stack
std::vector<RobotAgentPtr>::iterator it2;
for (it2 = a->getConnected()->begin(); it2 != a->getConnected()->end(); it2++) {
if (std::find(connected.begin(), connected.end(), (*it2)) == connected.end()) {
s.push((*it2));
}
}
}
if (connected.size() == this->agents.size()) {
// organism intact.
return orgs;
} else if (connected.size() > 1) {
// a connected part of the organism exist
Organism *o = new Organism();
std::vector<RobotAgentPtr>::iterator it;
for (it = connected.begin(); it != connected.end(); it++) {
o->addRobot((*it));
(*it)->setOrganism(o);
}
orgs.push_back(o);
} else if (connected.size() == 1) {
// this robot is not connected to any other
connected.front()->letGoOfOrganism();
loners.push_back(connected.front());
}
connected.clear();
}
std::vector<RobotAgentPtr>::iterator it2;
for (it2 = loners.begin(); it2 != loners.end(); it2++) {
(*it2)->setOrganism(NULL);
}
this->setInactive();
return orgs;
}
void DynamicController::step(double &left, double &right) {
DynamicAgentWorldModel* worldModel = dynamic_cast<DynamicAgentWorldModel*> (_wm);
RobotAgentPtr agent = gWorld->getAgent(worldModel->_agentId);
if(worldModel->getEnergyLevel() <= 0){
return;
}
setLED();
agent->setConnectToOthers(Agent::NEUTRAL);
if(agent->isPartOfOrganism()){
worldModel->organismTime++;
}else{
worldModel->swarmTime++;
}
// retrieve the values of the distance sensors
vector<double> inputs = this->getSensorValues();
if (inputs.size() == 0) {
cerr << "ERR: robot did not return any distance sensor data - can't build neural net" << endl;
return;
}
// Outputs are the following:
// Node 1: Form Organism
// Node 2: Leave Organism
// Node 3: Halt
// Node 4: Avoid obstacle
// Node 5: Move to direction
// Node 6: Desired organism size - for node 1
// Node 7: Desired direction - for node 5
// Node 8: Desired speed - for node 5
vector<double> outputs = vector<double> (8, 0);
// create the neural net if it didn't exist already
if (neuralNet == NULL) {
neuralNet = new SimplePerceptron(inputs.size(), outputs.size());
neuralNet->setActivationFunction(&ActivationFunctionTanh::apply);
// load the weights
neuralNet->loadParameters(&weights.front(), weights.size());
}
// set the sensor values as input neuron values
neuralNet->step(&inputs.front());
// and calculate the output
neuralNet->getOutputValues(&outputs.front());
vector<double> choices;
// first 5 elements are strategies
for (int i = 0; i < 5; i++) {
choices.push_back(outputs[i]);
}
// the first 5 are interpreted as a choice for a strategy. Highest wins.
int choice = distance(choices.begin(), max_element(choices.begin(), choices.end()));
// Obtain the numbers for speed, direction, and size of the organism.
int org_size = (int) MAX_ORG_SIZE * ((outputs[5]+1)/2);
double direction = MAX_ANGLE * outputs[6];
double speed = MAX_SPEED * ((outputs[7]+1)/2);
// Do some logging
if (gWorld->getIterations() % gOrganismSampleFrequency == 0) {
DynamicSharedData::outputLogFile << worldModel->_agentId << "," << gWorld->getIterations() << ", ";
for (int i = 0; i < 8; i++) {
DynamicSharedData::outputLogFile << outputs[i] << ((i < 7) ? ":" : "");
}
DynamicSharedData::outputLogFile << "," << choice << "," << org_size << "," << direction << "," << speed << std::endl;
}
/** For Test Purposes */
// org_size = 10;
// speed = 1;
// direction = 0;
// if(!agent->isPartOfOrganism() || agent->getOrganism()->size() < 3){
// choice = FORM;
// }else{
// choice = MOVE;
// }
//std::cout << "Robot: " << worldModel->_agentId << " is ";
switch (choice) {
case FORM:
//std::cout << "forming organism of size " << org_size << " with speed " << speed;
createOrganism(left, right, org_size, speed);
break;
case LEAVE:
//std::cout << "leaving organism";
leaveOrganism(left, right);
break;
case HALT:
//std::cout << "halting";
//halt(left, right);
move(left, right, direction, speed);
break;
case AVOID:
//std::cout << "avoiding obstacles with speed " << speed;
avoidObstacles(left, right, speed);
break;
case MOVE:
//std::cout << "moving with speed " << speed << " in the direction " << direction;
move(left, right, direction, speed);
break;
}
//std::cout << std::endl;
}
void DynamicController::createOrganism(double &left, double &right, int desired_size, double speed) {
DynamicAgentWorldModel* worldModel = dynamic_cast<DynamicAgentWorldModel*> (_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();
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()->getPosition();
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;
}
}
void RobotAgent::connectToRobot(RobotAgentPtr other) {
if (!gUseOrganisms) return;
if (this->isPartOfOrganism() && other->isPartOfOrganism()
&& this->getOrganism()->getId() == other->getOrganism()->getId()) {
// the same organism, do nothing.
} else if (other->isPartOfOrganism() && this->isPartOfOrganism()
&& other->getOrganism()->getId() != this->getOrganism()->getId()) {
// two different organisms
Organism* o1 = this->getOrganism();
Organism* o2 = other->getOrganism();
this->addNeighbour(other);
other->addNeighbour(gWorld->getAgent(this->_wm->_agentId));
// merge smaller into larger
if (o1->size() >= o2->size()) {
gLogFile << "Merging organism " << o2->getId() << " into " << o1->getId() << std::endl;
//other->clickOnto(this, o2);
o2->mergeInto(o1);
o2->setInactive();
//Organism::remove(o2);
//delete o2;
} else {
gLogFile << "Merging organism " << o1->getId() << " into " << o2->getId() << std::endl;
//this->clickOnto(other, o1);
o1->mergeInto(o2);
o1->setInactive();
//Organism::remove(o1);
//delete o1;
}
} else if (other->isPartOfOrganism()) {
Organism* organism = other->getOrganism();
organism->addRobot(gWorld->getAgent(this->_wm->_agentId));
this->setOrganism(organism);
this->addNeighbour(other);
other->addNeighbour(gWorld->getAgent(this->_wm->_agentId));
//this->clickOnto(other);
gLogFile << "Adding agent " << this->_wm->_agentId << " to organism: " << organism->getId() << std::endl;
} else if (this->isPartOfOrganism()) {
Organism* organism = this->getOrganism();
organism->addRobot(other);
other->setOrganism(organism);
this->addNeighbour(other);
other->addNeighbour(gWorld->getAgent(this->_wm->_agentId));
//other->clickOnto(this);
gLogFile << "Adding agent " << other->_wm->_agentId << " to organism: " << organism->getId() << std::endl;
} else {
Organism* organism = new Organism();
gLogFile << "Creating new organism: " << organism->getId() << " robots: " << this->_wm->_agentId << ", " << other->_wm->_agentId << std::endl;
organism->addRobot(gWorld->getAgent(this->_wm->_agentId));
organism->addRobot(other);
this->setOrganism(organism);
other->setOrganism(organism);
this->addNeighbour(other);
other->addNeighbour(gWorld->getAgent(this->_wm->_agentId));
Organism::add(organism);
//this->clickTogether(other);
}
}
