bool BodyVerifier::fixRobotBody(RobotRepresentation &robot) {
bool changed = false;
while (true) {
// check velidity of body
int errorCode;
std::vector<std::pair<std::string, std::string> > offenders;
if (!BodyVerifier::verify(robot, errorCode, offenders)) {
// TODO treat other cases: Arduino constraints, missing core
if (errorCode == BodyVerifier::SELF_INTERSECTION) {
std::cerr << "Robot body has following intersection pairs:"
<< std::endl;
for (unsigned int i = 0; i < offenders.size(); ++i) {
// get robots IdPartMap: Volatile, so needs update
const RobotRepresentation::IdPartMap idPartMap =
robot.getBody();
std::cerr << offenders[i].first << " with "
<< offenders[i].second << std::endl;
// check if offending body part hasn't been removed yet
if (idPartMap.find(offenders[i].first) == idPartMap.end()
|| idPartMap.find(offenders[i].second)
== idPartMap.end()) {
continue;
}
// will remove body part with less descendants (i.e. this
// covers the case where one part descends from the other)
unsigned int numDesc[] =
{
idPartMap.find(offenders[i].first)->second.lock()->numDescendants(),
idPartMap.find(offenders[i].second)->second.lock()->numDescendants() };
std::cout << offenders[i].first << " has " << numDesc[0]
<< " descendants" << std::endl;
std::cout << offenders[i].second << " has " << numDesc[1]
<< " descendants" << std::endl;
if (numDesc[0] > numDesc[1]) {
robot.trimBodyAt(offenders[i].second);
std::cout << "Removing latter" << std::endl;
} else {
robot.trimBodyAt(offenders[i].first);
std::cout << "Removing former" << std::endl;
}
changed = true;
}
}
if (errorCode == BodyVerifier::INTERNAL_ERROR) {
std::cout << "Body verification failed due to internal error!"
<< std::endl;
return false;
}
} else {
break;
}
}
return changed;
}
bool NeatContainer::fillBrain(NEAT::Genome *genome,
boost::shared_ptr<RobotRepresentation> &robotRepresentation) {
NEAT::NeuralNetwork net;
genome->BuildPhenotype(net);
typedef std::map<std::string, boost::weak_ptr<NeuronRepresentation> >
NeuronMap;
std::map<std::string, std::vector<double> > neuronToPositionMap;
NeuronMap neuronMap;
// FIRST NEED TO CREATE PHYSICAL ROBOT REP TO DETERMINE POSITIONS
// parse robot message
robogenMessage::Robot robotMessage = robotRepresentation->serialize();
// parse robot
boost::shared_ptr<Robot> robot(new Robot);
// Initialize ODE
dInitODE();
dWorldID odeWorld = dWorldCreate();
dWorldSetGravity(odeWorld, 0, 0, 0);
dSpaceID odeSpace = dHashSpaceCreate(0);
if (!robot->init(odeWorld, odeSpace, robotMessage)) {
std::cout << "Problem when initializing robot in "
<< "NeatContainer::fillBrain!" << std::endl;
return false;
}
RobotRepresentation::IdPartMap body = robotRepresentation->getBody();
boost::shared_ptr<NeuralNetworkRepresentation> brain =
robotRepresentation->getBrain();
// For each body part, get its position then create an entry for every
// neuron by adding a 4th coordinate that is the neurons ioID.
for(RobotRepresentation::IdPartMap::iterator i = body.begin();
i != body.end(); i++) {
std::string id = i->first;
boost::shared_ptr<PartRepresentation> part = i->second.lock();
std::vector<boost::weak_ptr<NeuronRepresentation> > neurons =
brain->getBodyPartNeurons(part->getId());
osg::Vec3 pos = robot->getBodyPart(id)->getRootPosition();
for (unsigned int j = 0; j<neurons.size(); j++) {
std::vector<double> position;
position.push_back(pos.x() * 10.0); //roughly something in [-1,1]
position.push_back(pos.y() * 10.0);
//position.push_back(pos.z());
float io = neurons[j].lock()->getIoPair().second;
position.push_back((io/10.0));
neuronToPositionMap[neurons[j].lock()->getId()] = position;
neuronMap[neurons[j].lock()->getId()] = neurons[j];
}
}
// Now go through all neurons and query for weights and params with
// the obtained coordinates
for(NeuronMap::iterator i = neuronMap.begin(); i != neuronMap.end(); i++) {
std::vector<double> positionI = neuronToPositionMap[i->first];
boost::shared_ptr<NeuronRepresentation> neuronI = i->second.lock();
for(NeuronMap::iterator j = neuronMap.begin(); j != neuronMap.end();
j++) {
std::vector<double> positionJ = neuronToPositionMap[j->first];
boost::shared_ptr<NeuronRepresentation> neuronJ = j->second.lock();
if (brain->connectionExists(neuronI->getId(), neuronJ->getId())) {
// only set weights on existing connections
net.Flush();
std::vector<double> inputs;
for (unsigned int k = 0; k < positionI.size(); k++) {
inputs.push_back(positionI[k]);
}
for (unsigned int k = 0; k < positionJ.size(); k++) {
inputs.push_back(positionJ[k]);
}
inputs.push_back(1.0); //bias
net.Input(inputs);
for(int t=0; t<10; t++) {
net.Activate();
}
std::vector<double> outputs = net.Output();
if (outputs[0] < 0.5) {
// if first output is under threshold,
// connection "does not exist" according to genome so set
// weight to 0
brain->setWeight(neuronI->getIoPair(),
neuronJ->getIoPair(), 0.0);
} else {
// otherwise use the second output
// translate from [0,1] to [min, max]
double weight = outputs[1] * (evoConf_->maxBrainWeight -
evoConf_->minBrainWeight) +
evoConf_->minBrainWeight;
brain->setWeight(neuronI->getIoPair(),
neuronJ->getIoPair(), weight);
}
}
}
// now query for parameters
if (neuronI->getLayer() != NeuronRepresentation::INPUT) {
// input neurons don't have params
net.Flush();
std::vector<double> inputs;
for (unsigned int k = 0; k < positionI.size(); k++) {
inputs.push_back(positionI[k]);
}
for (unsigned int k = 0; k < positionI.size(); k++) {
inputs.push_back(0.0); // 0 for second set of coords
}
inputs.push_back(1.0); //bias
net.Input(inputs);
for(int t=0; t<10; t++) {
net.Activate();
}
std::vector<double> outputs = net.Output();
std::vector<double> params;
if(neuronI->getType() == NeuronRepresentation::SIGMOID ||
neuronI->getType() == NeuronRepresentation::CTRNN_SIGMOID){
// bias
params.push_back(outputs[2] * (evoConf_->maxBrainBias -
evoConf_->minBrainBias) + evoConf_->minBrainBias);
if(neuronI->getType() == NeuronRepresentation::CTRNN_SIGMOID) {
// tau
params.push_back(outputs[3] * (evoConf_->maxBrainTau -
evoConf_->minBrainTau) + evoConf_->minBrainTau);
}
} else if(neuronI->getType() == NeuronRepresentation::OSCILLATOR) {
// period
params.push_back( outputs[2] * (evoConf_->maxBrainPeriod -
evoConf_->minBrainPeriod) + evoConf_->minBrainPeriod);
// phase offset
params.push_back( outputs[3] * (evoConf_->maxBrainPhaseOffset -
evoConf_->minBrainPhaseOffset) +
evoConf_->minBrainPhaseOffset);
// amplitude
params.push_back( outputs[4] * (evoConf_->maxBrainAmplitude -
evoConf_->minBrainAmplitude) +
evoConf_->minBrainAmplitude);
} else {
std::cout << "INVALID TYPE ENCOUNTERED " << neuronI->getType()
<< std::endl;
}
neuronI->setParams(params);
}
}
return true;
}
