Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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;

}