diff --git a/.gitignore b/.gitignore index b0ba6e5..1d93be6 100644 --- a/.gitignore +++ b/.gitignore @@ -63,6 +63,9 @@ target/ # PyCharm project files .idea +#kdev files +*.kdev4 + # Virtual env .venv diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index 1fff715..ef21039 100644 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -65,6 +65,9 @@ file(GLOB_RECURSE tol/plugin/fakebrain.cpp tol/plugin/helper.cpp tol/plugin/rlpower.cpp + tol/plugin/simple_split_brain.cpp + tol/plugin/extended_neural_network.cpp + tol/plugin/rlpowered_network.cpp ) add_library(tolworldcontrol SHARED ${TOL_WORLD_SRC}) diff --git a/cpp/tol/plugin/RobotController.cpp b/cpp/tol/plugin/RobotController.cpp index a16660b..ba4c692 100644 --- a/cpp/tol/plugin/RobotController.cpp +++ b/cpp/tol/plugin/RobotController.cpp @@ -7,6 +7,9 @@ #include "RobotController.h" #include "rlpower.h" +#include "simple_split_brain.h" +#include "extended_neural_network.h" +#include "rlpowered_network.h" #include @@ -17,7 +20,7 @@ RobotController::RobotController() {} RobotController::~RobotController() {} void RobotController::Load(::gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf) { - ::revolve::gazebo::RobotController::Load(_parent, _sdf); + ::revolve::gazebo::RobotController::Load(_parent, _sdf); std::cout << "ToL Robot loaded." << std::endl; } @@ -35,9 +38,11 @@ void RobotController::LoadBrain(sdf::ElementPtr sdf) std::cerr << "Brain does not define type, this is probably an error." << std::endl; return; } - if (brain->GetAttribute("algorithm")->GetAsString() == "rlpower") { - brain_.reset(new tol::RLPower(this->model->GetName(), brain, evaluator_, motors_, sensors_)); +// brain_.reset(new tol::RLPower(this->model->GetName(), brain,evaluator_, motors_, sensors_)); + brain_.reset(new tol::RLPowerNet(this->model->GetName(), brain,evaluator_, motors_, sensors_)); +// brain_. reset(new tol::ExtNN(this->model->GetName(), evaluator_, brain, motors_, sensors_)); +// brain_.reset(new tol::ExtendedNeuralNetwork(this->model->GetName(), evaluator_, brain, motors_, sensors_)); } else { std::cout << "Calling default ANN brain." << std::endl; revolve::gazebo::RobotController::LoadBrain(sdf); diff --git a/cpp/tol/plugin/RobotController.h b/cpp/tol/plugin/RobotController.h index 70a956a..4e47a88 100644 --- a/cpp/tol/plugin/RobotController.h +++ b/cpp/tol/plugin/RobotController.h @@ -12,6 +12,8 @@ #include "rlpower.h" #include "evaluator.h" +#include + namespace tol { class RobotController: public revolve::gazebo::RobotController { diff --git a/cpp/tol/plugin/extended_neural_network.cpp b/cpp/tol/plugin/extended_neural_network.cpp new file mode 100644 index 0000000..1733d06 --- /dev/null +++ b/cpp/tol/plugin/extended_neural_network.cpp @@ -0,0 +1,347 @@ +#include "extended_neural_network.h" +#include "helper.h" +#include "sensor.h" +#include "actuator.h" +#include "revolve/gazebo/motors/Motor.h" +#include "revolve/gazebo/sensors/Sensor.h" + + +namespace tol { + + ExtendedNeuralNetwork::ExtendedNeuralNetwork(std::string modelName, + tol::EvaluatorPtr evaluator, + sdf::ElementPtr node, + const std::vector &actuators, + const std::vector &sensors) : + revolve::brain::ExtendedNeuralNetwork( + modelName, + parseSDF(node, actuators, sensors), + evaluator, + Helper::createWrapper(actuators), + Helper::createWrapper(sensors)) { } + + ExtendedNeuralNetwork::~ExtendedNeuralNetwork() + { + + } + + + void ExtendedNeuralNetwork::update(const std::vector &actuators, + const std::vector &sensors, + double t, + double step) { +// std::cout << "yay" << std::endl; + revolve::brain::ExtendedNeuralNetwork::update( + Helper::createWrapper(actuators), + Helper::createWrapper(sensors), + t, step); + } + + +ExtendedNeuralNetwork::ExtNNConfig ExtendedNeuralNetwork::parseSDF(sdf::ElementPtr node, + const std::vector< revolve::gazebo::MotorPtr > & motors, + const std::vector< revolve::gazebo::SensorPtr > & sensors) { + ExtNNConfig ret; + + // Map neuron sdf elements to their id's + std::map neuronDescriptions; + + // List of all hidden neuron id's + std::vector hiddenIds; + + // Number of input neurons for mapping them to the input buffer + ret.numInputNeurons_ = 0; + + // Number of output neurons for mapping them to the output buffer + ret.numOutputNeurons_ = 0; + + // Number of hidden neurons + ret.numHiddenNeurons_ = 0; + + // Get the first sdf neuron element + auto neuron = node->HasElement("rv:neuron") ? node->GetElement("rv:neuron") : sdf::ElementPtr(); + + while (neuron) { + if (!neuron->HasAttribute("layer") || !neuron->HasAttribute("id")) { + std::cerr << "Missing required neuron attributes (id or layer). '" << std::endl; + throw std::runtime_error("Robot brain error"); + } + auto layer = neuron->GetAttribute("layer")->GetAsString(); + auto neuronId = neuron->GetAttribute("id")->GetAsString(); + + + // check if a neuron with this id has been already added + if (neuronDescriptions.count(neuronId)) { + std::cerr << "Duplicate neuron ID '" + << neuronId << "'" << std::endl; + throw std::runtime_error("Robot brain error"); + } + + // add this neuron to the id->sdf map + neuronDescriptions[neuronId] = neuron; + + // add the neuron id to the appropriate list of id's + if ("input" == layer) { + // inputIds.push_back(neuronId); + } + + else if ("output" == layer) { + // outputIds.push_back(neuronId); + } + + else if ("hidden" == layer) { + hiddenIds.push_back(neuronId); + } + + else { + std::cerr << "Unknown neuron layer '" << layer << "'." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + neuron = neuron->GetNextElement("rv:neuron"); + } + + + // Add output neurons for motors: + + // map of numbers of output neurons for each body part + std::map outputCountMap; + + for (auto it = motors.begin(); it != motors.end(); ++it) { + auto motor = *it; + auto partId = motor->partId(); + + if (!outputCountMap.count(partId)) { + outputCountMap[partId] = 0; + } + + for (unsigned int i = 0, l = motor->outputs(); i < l; ++i) { + std::stringstream neuronId; + neuronId << partId << "-out-" << outputCountMap[partId]; + outputCountMap[partId]++; + + auto neuronDescription = neuronDescriptions.find(neuronId.str()); + if (neuronDescription == neuronDescriptions.end()) { + std::cerr << "Required output neuron " << neuronId.str() << + " for motor could not be located" + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto newNeuron = neuronHelper(neuronDescription->second, ret); + ret.idToNeuron_[neuronId.str()] = newNeuron; + + } + } + // Add input neurons for sensors: + + // map of number of input neurons for each part: + + std::map inputCountMap; + + for (auto it = sensors.begin(); it != sensors.end(); ++it) { + auto sensor = *it; + auto partId = sensor->partId(); + + if (!inputCountMap.count(partId)) { + inputCountMap[partId] = 0; + } + + for (unsigned int i = 0, l = sensor->inputs(); i < l; ++i) { + std::stringstream neuronId; + neuronId << partId << "-in-" << inputCountMap[partId]; + inputCountMap[partId]++; + + auto neuronDescription = neuronDescriptions.find(neuronId.str()); + if (neuronDescription == neuronDescriptions.end()) { + std::cerr << "Required input neuron " << neuronId.str() << + " for sensor could not be located" + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto newNeuron = neuronHelper(neuronDescription->second,ret); + ret.idToNeuron_[neuronId.str()] = newNeuron; + } + } + + // initialize the array for sensor inputs: + ret.inputs_ = new double[ret.numInputNeurons_]; + ret.outputs_ = new double[ret.numOutputNeurons_]; + + + // Add hidden neurons: + for (auto it = hiddenIds.begin(); it != hiddenIds.end(); ++it) { + auto neuronDescription = neuronDescriptions.find(*it); + auto newNeuron = neuronHelper(neuronDescription->second,ret); + ret.idToNeuron_[*it] = newNeuron; + } + + + // Add connections: + auto connection = node->HasElement("rv:neural_connection") ? node->GetElement("rv:neural_connection") : sdf::ElementPtr(); + while (connection) { + if (!connection->HasAttribute("src") || !connection->HasAttribute("dst") + || !connection->HasAttribute("weight")) { + std::cerr << "Missing required connection attributes (`src`, `dst` or `weight`)." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto src = connection->GetAttribute("src")->GetAsString(); + auto dst = connection->GetAttribute("dst")->GetAsString(); + + + std::string dstSocketName; + if (connection->HasAttribute("socket")) { + dstSocketName = connection->GetAttribute("socket")->GetAsString(); + } + else { + dstSocketName = "None"; // this is the default socket name + } + + double weight; + connection->GetAttribute("weight")->Get(weight); + + // Use connection helper to set the weight + connectionHelper(src, dst, dstSocketName, weight, ret.idToNeuron_, ret); + + // Load the next connection + connection = connection->GetNextElement("rv:neural_connection"); + } + return ret; +} + +std::map ExtendedNeuralNetwork::parseSDFElement(sdf::ElementPtr elem) +{ + std::map params; + + auto subElem = elem->GetFirstElement(); + while (subElem) { + auto elName = subElem->GetName(); + double elValue = subElem->Get(); + params[elName] = elValue; + subElem = subElem->GetNextElement(); + } + + return params; +} + +void ExtendedNeuralNetwork::connectionHelper(const std::string &src, + const std::string &dst, + const std::string &socket, + double weight, + const std::map &idToNeuron, + ExtNNConfig &ret) +{ +// std::cout << "connection from " + src + " to " + dst + " was added with weight: " << weight << std::endl; + auto srcNeuron = idToNeuron.find(src); + if (srcNeuron == idToNeuron.end()) { + std::cerr << "Could not find source neuron '" << src << "'" << std::endl; + throw std::runtime_error("Robot brain error"); + } + auto dstNeuron = idToNeuron.find(dst); + if (dstNeuron == idToNeuron.end()) { + std::cerr << "Could not find destination neuron '" << dst << "'" << std::endl; + throw std::runtime_error("Robot brain error"); + } + + revolve::brain::NeuralConnectionPtr newConnection(new revolve::brain::NeuralConnection( + srcNeuron->second, + dstNeuron->second, + weight + )); + + // Add reference to this connection to the destination neuron + (dstNeuron->second)->AddIncomingConnection(socket, newConnection); + ret.connections_.push_back(newConnection); +} + + +revolve::brain::NeuronPtr ExtendedNeuralNetwork::neuronHelper(sdf::ElementPtr neuron, + ExtNNConfig &ret) +{ + if (!neuron->HasAttribute("type")) { + std::cerr << "Missing required `type` attribute for neuron." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + if (!neuron->HasAttribute("layer")) { + std::cerr << "Missing required `layer` attribute for neuron." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto type = neuron->GetAttribute("type")->GetAsString(); + auto layer = neuron->GetAttribute("layer")->GetAsString(); + auto id = neuron->GetAttribute("id")->GetAsString(); + + // map of parameter names and values + auto params = parseSDFElement(neuron); + + return addNeuron(id, type, layer, params, ret); +} + + + +revolve::brain::NeuronPtr ExtendedNeuralNetwork::addNeuron(const std::string &neuronId, + const std::string &neuronType, + const std::string &neuronLayer, // can be 'hidden', 'input' or 'output' + const std::map ¶ms, + ExtNNConfig &ret) +{ + revolve::brain::NeuronPtr newNeuron; +// std::cout << neuronType + " " + neuronId + " was added in"+ " "+ neuronLayer << std::endl; + if ("input" == neuronLayer) { + newNeuron.reset(new revolve::brain::InputNeuron(neuronId, params)); + + ret.inputNeurons_.push_back(newNeuron); + ret.inputPositionMap_[newNeuron] = ret.numInputNeurons_; + ret.numInputNeurons_++; + } + + else { + + if ("Sigmoid" == neuronType) { + newNeuron.reset(new revolve::brain::SigmoidNeuron(neuronId, params)); + } + else if ("Simple" == neuronType) { + newNeuron.reset(new revolve::brain::LinearNeuron(neuronId, params)); + } + else if ("Oscillator" == neuronType) { + newNeuron.reset(new revolve::brain::OscillatorNeuron(neuronId, params)); + } + else if ("V-Neuron" == neuronType) { + newNeuron.reset(new revolve::brain::VOscillator(neuronId, params)); + } + else if ("X-Neuron" == neuronType) { + newNeuron.reset(new revolve::brain::XOscillator(neuronId, params)); + } + else if ("Bias" == neuronType) { + newNeuron.reset(new revolve::brain::BiasNeuron(neuronId, params)); + } + else if ("Leaky" == neuronType) { + newNeuron.reset(new revolve::brain::LeakyIntegrator(neuronId, params)); + } + else if ("DifferentialCPG" == neuronType) { + newNeuron.reset(new revolve::brain::DifferentialCPG(neuronId, params)); + } + else { + std::cerr << "Unsupported neuron type `" << neuronType << '`' << std::endl; + throw std::runtime_error("Robot brain error"); + } + + if ("output" == neuronLayer) { + ret.outputNeurons_.push_back(newNeuron); + ret.outputPositionMap_[newNeuron] = ret.numOutputNeurons_; + ret.numOutputNeurons_++; + } + else { + ret.hiddenNeurons_.push_back(newNeuron); + ret.numHiddenNeurons_++; + } + } + + ret.allNeurons_.push_back(newNeuron); + return newNeuron; +} +} /* namespace tol */ + diff --git a/cpp/tol/plugin/extended_neural_network.h b/cpp/tol/plugin/extended_neural_network.h new file mode 100644 index 0000000..2256ec3 --- /dev/null +++ b/cpp/tol/plugin/extended_neural_network.h @@ -0,0 +1,111 @@ +#ifndef REVOLVE_GAZEBO_BRAIN_DIFFERENTIAL_NEAT_H +#define REVOLVE_GAZEBO_BRAIN_DIFFERENTIAL_NEAT_H + +#include "brain/extnn/extended_neural_network.h" +#include "evaluator.h" +#include "revolve/gazebo/brain/Brain.h" + + +#include +#include + +#include +#include + +namespace tol { + + class ExtendedNeuralNetwork : public revolve::gazebo::Brain, private revolve::brain::ExtendedNeuralNetwork { + + public: + /** + * Constructor for a neural network including neurons that are of a different type than the usual ones. + * @param modelName: name of the model + * @param evaluator: pointer to the evaluator that is used + * @param node: the sdf file containing the necessary information to build the network + * @param actuators: vector list of robot's actuators + * @param sensors: vector list of robot's sensors + * @return pointer to the neural network + */ + ExtendedNeuralNetwork(std::string modelName, + tol::EvaluatorPtr evaluator, + sdf::ElementPtr node, + const std::vector &actuators, + const std::vector &sensors); + + virtual ~ExtendedNeuralNetwork(); + + /** + * Method for updating sensors readings, actuators positions, ranked list of policies and generating new policy + * @param actuators: vector list of robot's actuators + * @param sensors: vector list of robot's sensors + * @param t: + * @param step: + */ + virtual void update(const std::vector &actuators, + const std::vector &sensors, + double t, + double step); + + /** + * Method to transform the sdf file to a configuration + * @param node: sdf file to be transformed + * @param motors: vector list of robot's motors + * @param sensors: vector list of robot's sensors + * @return configuration needed for the neural network + */ + static ExtNNConfig parseSDF(sdf::ElementPtr node, + const std::vector & motors, + const std::vector & sensors); + + /** + * Method to get the parameters of neurons from an sdf ElementPtr + * @param elem: sdf that includes the parameters + * @return string to parameter mapping + */ + static std::map parseSDFElement(sdf::ElementPtr elem); + + /** + * Helpermethod for getting the configuration + * adds a new connection to the configuration + * @param src: the beginning of the connetion + * @param dst: the end of the connection + * @param weight: weighting factor of the connection + * @param idToNeuron: mapping between the neurons and their ids + * @param ret: configuration for the network where the connection should be included + */ + static void connectionHelper(const std::string &src, + const std::string &dst, + const std::string &socket, + double weight, + const std::map &idToNeuron, + ExtNNConfig& ret); + /** + * Helpermethod for getting the configuration + * @param neuron: sdf for the neuron to be added + * @param ret: configuration for the network where the neuron should be added + */ + static revolve::brain::NeuronPtr neuronHelper(sdf::ElementPtr neuron, + ExtNNConfig& ret); + + /** + * Helpermethod for getting the configuration + * This function creates neurons and adds them to the configuration + * @param neuronId: id of the neuron + * @param neuronType: type of the neuron + * @param neuronLayer: layer the neuron should be added to, can be 'hidden', 'input' or 'output' + * @param params: parameters of the new neuron + */ + static revolve::brain::NeuronPtr addNeuron(const std::string &neuronId, + const std::string &neuronType, + const std::string &neuronLayer, + const std::map ¶ms, + ExtNNConfig& ret); + + + }; + +} /* namespace tol */ + +#endif //REVOLVE_GAZEBO_BRAIN_REINFORCEDLEARNING_H + + diff --git a/cpp/tol/plugin/helper.h b/cpp/tol/plugin/helper.h index 93ce423..41d9521 100644 --- a/cpp/tol/plugin/helper.h +++ b/cpp/tol/plugin/helper.h @@ -24,6 +24,7 @@ #include "sensor.h" #include +#include namespace tol { diff --git a/cpp/tol/plugin/rlpowered_network.cpp b/cpp/tol/plugin/rlpowered_network.cpp new file mode 100644 index 0000000..766d9ea --- /dev/null +++ b/cpp/tol/plugin/rlpowered_network.cpp @@ -0,0 +1,81 @@ +// +// Created by Milan Jelisavcic on 28/03/16. +// + +#include "rlpowered_network.h" +#include "simple_split_brain.h" +#include "helper.h" +#include "revolve/gazebo/motors/Motor.h" +#include "revolve/gazebo/sensors/Sensor.h" + +namespace tol { + + std::vector forController(std::vector> toConvert) { + return toConvert[0]; + } + std::vector> forLearner(std::vector toConvert) { + return std::vector>(1,toConvert); + } + + RLPowerNet::RLPowerNet(std::string modelName, + sdf::ElementPtr brain, + EvaluatorPtr evaluator, + std::vector &actuators, + std::vector &sensors) : + revolve::brain::ConvSplitBrain, std::vector>>(&forController, &forLearner) { + boost::shared_ptr swap1(new revolve::brain::ExtNNController(modelName, + ExtNN::parseSDF(brain, actuators, sensors), + evaluator, + Helper::createWrapper(actuators), + Helper::createWrapper(sensors))); + controller = boost::static_pointer_cast>>(swap1); + boost::shared_ptr swap2(new revolve::brain::RLPowerLearner(modelName, + parseSDF(brain), + 1)); + learner = boost::static_pointer_cast>>>(swap2); + evaluator_ = evaluator; + } + + RLPowerNet::~RLPowerNet() { } + + void RLPowerNet::update(const std::vector &actuators, + const std::vector &sensors, + double t, + double step) { + revolve::brain::ConvSplitBrain,std::vector>>::update( + Helper::createWrapper(actuators), + Helper::createWrapper(sensors), + t, step + ); + } + + revolve::brain::RLPowerLearner::Config RLPowerNet::parseSDF(sdf::ElementPtr brain) { + revolve::brain::RLPowerLearner::Config config; + + // Read out brain configuration attributes + config.algorithm_type = brain->HasAttribute("type") ? brain->GetAttribute("type")->GetAsString() : "A"; + + config.evaluation_rate = brain->HasAttribute("evaluation_rate") ? + std::stod(brain->GetAttribute("evaluation_rate")->GetAsString()) : + revolve::brain::RLPowerLearner::EVALUATION_RATE; + config.interpolation_spline_size = brain->HasAttribute("interpolation_spline_size") ? + std::stoul(brain->GetAttribute("interpolation_spline_size")->GetAsString()) : + revolve::brain::RLPowerLearner::INTERPOLATION_CACHE_SIZE; + config.max_evaluations = brain->HasAttribute("max_evaluations") ? + std::stoul(brain->GetAttribute("max_evaluations")->GetAsString()) : + revolve::brain::RLPowerLearner::MAX_EVALUATIONS; + config.max_ranked_policies = brain->HasAttribute("max_ranked_policies") ? + std::stoul(brain->GetAttribute("max_ranked_policies")->GetAsString()) : + revolve::brain::RLPowerLearner::MAX_RANKED_POLICIES; + config.noise_sigma = 10; + config.sigma_tau_correction = brain->HasAttribute("sigma_tau_correction") ? + std::stod(brain->GetAttribute("sigma_tau_correction")->GetAsString()) : + revolve::brain::RLPowerLearner::SIGMA_TAU_CORRECTION; + config.source_y_size = 6; + config.update_step = 0; + config.policy_load_path = ""; + + return config; + } + +} /* namespace tol */ diff --git a/cpp/tol/plugin/rlpowered_network.h b/cpp/tol/plugin/rlpowered_network.h new file mode 100644 index 0000000..c2480ec --- /dev/null +++ b/cpp/tol/plugin/rlpowered_network.h @@ -0,0 +1,61 @@ +// +// Created by Milan Jelisavcic on 28/03/16. +// + +#ifndef REVOLVE_GAZEBO_BRAIN_REINFORCEDLEARNING_NETWORK_H +#define REVOLVE_GAZEBO_BRAIN_REINFORCEDLEARNING_NETWORK_H + +#include "brain/split_cpg/converting_split_brain.h" +#include "brain/split_cpg/rlpower_learner.h" +#include "brain/split_cpg/extended_neural_network_controller.h" +#include "evaluator.h" +#include "revolve/gazebo/brain/Brain.h" + +#include +#include + +#include +#include + +namespace tol { + + class RLPowerNet : public revolve::gazebo::Brain, private revolve::brain::ConvSplitBrain,std::vector>> { + + public: + /** + * The RLPower constructor reads out configuration file, deretmines which algorithm type to apply and + * initialises new policy. + * @param modelName: name of a robot + * @param brain: configuration file + * @param evaluator: pointer to fitness evaluatior + * @param n_actuators: number of actuators + * @param n_sensors: number of sensors + * @return pointer to the RLPower class object + */ + RLPowerNet(std::string modelName, + sdf::ElementPtr brain, + tol::EvaluatorPtr evaluator, + std::vector &actuators, + std::vector &sensors); + + virtual ~RLPowerNet(); + + /** + * Method for updating sensors readings, actuators positions, ranked list of policies and generating new policy + * @param actuators: vector list of robot's actuators + * @param sensors: vector list of robot's sensors + * @param t: + * @param step: + */ + virtual void update(const std::vector &actuators, + const std::vector &sensors, + double t, + double step); + + static revolve::brain::RLPowerLearner::Config parseSDF(sdf::ElementPtr brain); + }; + +} /* namespace tol */ + +#endif //REVOLVE_GAZEBO_BRAIN_REINFORCEDLEARNING_NETWORK_H + diff --git a/cpp/tol/plugin/simple_split_brain.cpp b/cpp/tol/plugin/simple_split_brain.cpp new file mode 100644 index 0000000..30e17a2 --- /dev/null +++ b/cpp/tol/plugin/simple_split_brain.cpp @@ -0,0 +1,355 @@ +#include "simple_split_brain.h" +#include "helper.h" +#include "sensor.h" +#include "actuator.h" +#include "revolve/gazebo/motors/Motor.h" +#include "revolve/gazebo/sensors/Sensor.h" + + +namespace tol { + + ExtNN::ExtNN(std::string modelName, + tol::EvaluatorPtr evaluator, + sdf::ElementPtr node, + const std::vector &actuators, + const std::vector &sensors) : revolve::brain::SimpleSplitBrain>() { +// std::cout << "i get here \n"; + boost::shared_ptr swap1(new revolve::brain::ExtNNController(modelName, + parseSDF(node, actuators, sensors), + evaluator, + Helper::createWrapper(actuators), + Helper::createWrapper(sensors))); + controller = boost::static_pointer_cast>>(swap1); + boost::shared_ptr swap2(new revolve::brain::WeightVectorLearner()); + learner = boost::static_pointer_cast>>(swap2); + evaluator_ = evaluator; +// std::cout << "done" << std::endl; + } + + ExtNN::~ExtNN() + { + + } + + + void ExtNN::update(const std::vector &actuators, + const std::vector &sensors, + double t, + double step) { +// std::cout << "yay" << std::endl; + revolve::brain::SimpleSplitBrain>::update( + Helper::createWrapper(actuators), + Helper::createWrapper(sensors), + t, step + ); + } + + +revolve::brain::ExtNNController::ExtNNConfig ExtNN::parseSDF(sdf::ElementPtr node, + const std::vector< revolve::gazebo::MotorPtr > & motors, + const std::vector< revolve::gazebo::SensorPtr > & sensors) +{ + revolve::brain::ExtNNController::ExtNNConfig ret; + + // Map neuron sdf elements to their id's + std::map neuronDescriptions; + + // List of all hidden neuron id's + std::vector hiddenIds; + + // Number of input neurons for mapping them to the input buffer + ret.numInputNeurons_ = 0; + + // Number of output neurons for mapping them to the output buffer + ret.numOutputNeurons_ = 0; + + // Number of hidden neurons + ret.numHiddenNeurons_ = 0; + + // Get the first sdf neuron element + auto neuron = node->HasElement("rv:neuron") ? node->GetElement("rv:neuron") : sdf::ElementPtr(); + + while (neuron) { + if (!neuron->HasAttribute("layer") || !neuron->HasAttribute("id")) { + std::cerr << "Missing required neuron attributes (id or layer). '" << std::endl; + throw std::runtime_error("Robot brain error"); + } + auto layer = neuron->GetAttribute("layer")->GetAsString(); + auto neuronId = neuron->GetAttribute("id")->GetAsString(); + + + // check if a neuron with this id has been already added + if (neuronDescriptions.count(neuronId)) { + std::cerr << "Duplicate neuron ID '" + << neuronId << "'" << std::endl; + throw std::runtime_error("Robot brain error"); + } + + // add this neuron to the id->sdf map + neuronDescriptions[neuronId] = neuron; + + // add the neuron id to the appropriate list of id's + if ("input" == layer) { + // inputIds.push_back(neuronId); + } + + else if ("output" == layer) { + // outputIds.push_back(neuronId); + } + + else if ("hidden" == layer) { + hiddenIds.push_back(neuronId); + } + + else { + std::cerr << "Unknown neuron layer '" << layer << "'." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + neuron = neuron->GetNextElement("rv:neuron"); + } + + + // Add output neurons for motors: + + // map of numbers of output neurons for each body part + std::map outputCountMap; + + for (auto it = motors.begin(); it != motors.end(); ++it) { + auto motor = *it; + auto partId = motor->partId(); + + if (!outputCountMap.count(partId)) { + outputCountMap[partId] = 0; + } + + for (unsigned int i = 0, l = motor->outputs(); i < l; ++i) { + std::stringstream neuronId; + neuronId << partId << "-out-" << outputCountMap[partId]; + outputCountMap[partId]++; + + auto neuronDescription = neuronDescriptions.find(neuronId.str()); + if (neuronDescription == neuronDescriptions.end()) { + std::cerr << "Required output neuron " << neuronId.str() << + " for motor could not be located" + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto newNeuron = neuronHelper(neuronDescription->second, ret); + ret.idToNeuron_[neuronId.str()] = newNeuron; + + } + } + // Add input neurons for sensors: + + // map of number of input neurons for each part: + + std::map inputCountMap; + + for (auto it = sensors.begin(); it != sensors.end(); ++it) { + auto sensor = *it; + auto partId = sensor->partId(); + + if (!inputCountMap.count(partId)) { + inputCountMap[partId] = 0; + } + + for (unsigned int i = 0, l = sensor->inputs(); i < l; ++i) { + std::stringstream neuronId; + neuronId << partId << "-in-" << inputCountMap[partId]; + inputCountMap[partId]++; + + auto neuronDescription = neuronDescriptions.find(neuronId.str()); + if (neuronDescription == neuronDescriptions.end()) { + std::cerr << "Required input neuron " << neuronId.str() << + " for sensor could not be located" + << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto newNeuron = neuronHelper(neuronDescription->second,ret); + ret.idToNeuron_[neuronId.str()] = newNeuron; + } + } + + // initialize the array for sensor inputs: + ret.inputs_ = new double[ret.numInputNeurons_]; + ret.outputs_ = new double[ret.numOutputNeurons_]; + + + // Add hidden neurons: + for (auto it = hiddenIds.begin(); it != hiddenIds.end(); ++it) { + auto neuronDescription = neuronDescriptions.find(*it); + auto newNeuron = neuronHelper(neuronDescription->second,ret); + ret.idToNeuron_[*it] = newNeuron; + } + + + // Add connections: + auto connection = node->HasElement("rv:neural_connection") ? node->GetElement("rv:neural_connection") : sdf::ElementPtr(); + while (connection) { + if (!connection->HasAttribute("src") || !connection->HasAttribute("dst") + || !connection->HasAttribute("weight")) { + std::cerr << "Missing required connection attributes (`src`, `dst` or `weight`)." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto src = connection->GetAttribute("src")->GetAsString(); + auto dst = connection->GetAttribute("dst")->GetAsString(); + + + std::string dstSocketName; + if (connection->HasAttribute("socket")) { + dstSocketName = connection->GetAttribute("socket")->GetAsString(); + } + else { + dstSocketName = "None"; // this is the default socket name + } + + double weight; + connection->GetAttribute("weight")->Get(weight); + + // Use connection helper to set the weight + connectionHelper(src, dst, dstSocketName, weight, ret.idToNeuron_, ret); + + // Load the next connection + connection = connection->GetNextElement("rv:neural_connection"); + } + return ret; +} + +std::map ExtNN::parseSDFElement(sdf::ElementPtr elem) +{ + std::map params; + + auto subElem = elem->GetFirstElement(); + while (subElem) { + auto elName = subElem->GetName(); + double elValue = subElem->Get(); + params[elName] = elValue; + subElem = subElem->GetNextElement(); + } + + return params; +} + +void ExtNN::connectionHelper(const std::string &src, + const std::string &dst, + const std::string &socket, + double weight, + const std::map &idToNeuron, + revolve::brain::ExtNNController::ExtNNConfig &ret) +{ +// std::cout << "connection from " + src + " to " + dst + " was added with weight: " << weight << std::endl; + auto srcNeuron = idToNeuron.find(src); + if (srcNeuron == idToNeuron.end()) { + std::cerr << "Could not find source neuron '" << src << "'" << std::endl; + throw std::runtime_error("Robot brain error"); + } + auto dstNeuron = idToNeuron.find(dst); + if (dstNeuron == idToNeuron.end()) { + std::cerr << "Could not find destination neuron '" << dst << "'" << std::endl; + throw std::runtime_error("Robot brain error"); + } + + revolve::brain::NeuralConnectionPtr newConnection(new revolve::brain::NeuralConnection( + srcNeuron->second, + dstNeuron->second, + weight + )); + + // Add reference to this connection to the destination neuron + (dstNeuron->second)->AddIncomingConnection(socket, newConnection); + ret.connections_.push_back(newConnection); +} + + +revolve::brain::NeuronPtr ExtNN::neuronHelper(sdf::ElementPtr neuron, + revolve::brain::ExtNNController::ExtNNConfig &ret) +{ + if (!neuron->HasAttribute("type")) { + std::cerr << "Missing required `type` attribute for neuron." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + if (!neuron->HasAttribute("layer")) { + std::cerr << "Missing required `layer` attribute for neuron." << std::endl; + throw std::runtime_error("Robot brain error"); + } + + auto type = neuron->GetAttribute("type")->GetAsString(); + auto layer = neuron->GetAttribute("layer")->GetAsString(); + auto id = neuron->GetAttribute("id")->GetAsString(); + + // map of parameter names and values + auto params = parseSDFElement(neuron); + + return addNeuron(id, type, layer, params, ret); +} + + + +revolve::brain::NeuronPtr ExtNN::addNeuron(const std::string &neuronId, + const std::string &neuronType, + const std::string &neuronLayer, // can be 'hidden', 'input' or 'output' + const std::map ¶ms, + revolve::brain::ExtNNController::ExtNNConfig &ret) +{ + revolve::brain::NeuronPtr newNeuron; +// std::cout << neuronType + " " + neuronId + " was added in"+ " "+ neuronLayer << std::endl; + if ("input" == neuronLayer) { + newNeuron.reset(new revolve::brain::InputNeuron(neuronId, params)); + + ret.inputNeurons_.push_back(newNeuron); + ret.inputPositionMap_[newNeuron] = ret.numInputNeurons_; + ret.numInputNeurons_++; + } + + else { + + if ("Sigmoid" == neuronType) { + newNeuron.reset(new revolve::brain::SigmoidNeuron(neuronId, params)); + } + else if ("Simple" == neuronType) { + newNeuron.reset(new revolve::brain::LinearNeuron(neuronId, params)); + } + else if ("Oscillator" == neuronType) { + newNeuron.reset(new revolve::brain::OscillatorNeuron(neuronId, params)); + } + else if ("V-Neuron" == neuronType) { + newNeuron.reset(new revolve::brain::VOscillator(neuronId, params)); + } + else if ("X-Neuron" == neuronType) { + newNeuron.reset(new revolve::brain::XOscillator(neuronId, params)); + } + else if ("Bias" == neuronType) { + newNeuron.reset(new revolve::brain::BiasNeuron(neuronId, params)); + } + else if ("Leaky" == neuronType) { + newNeuron.reset(new revolve::brain::LeakyIntegrator(neuronId, params)); + } + else if ("DifferentialCPG" == neuronType) { + newNeuron.reset(new revolve::brain::DifferentialCPG(neuronId, params)); + } + else { + std::cerr << "Unsupported neuron type `" << neuronType << '`' << std::endl; + throw std::runtime_error("Robot brain error"); + } + + if ("output" == neuronLayer) { + ret.outputNeurons_.push_back(newNeuron); + ret.outputPositionMap_[newNeuron] = ret.numOutputNeurons_; + ret.numOutputNeurons_++; + } + else { + ret.hiddenNeurons_.push_back(newNeuron); + ret.numHiddenNeurons_++; + } + } + + ret.allNeurons_.push_back(newNeuron); + return newNeuron; +} +} /* namespace tol */ + diff --git a/cpp/tol/plugin/simple_split_brain.h b/cpp/tol/plugin/simple_split_brain.h new file mode 100644 index 0000000..7450157 --- /dev/null +++ b/cpp/tol/plugin/simple_split_brain.h @@ -0,0 +1,113 @@ +#ifndef REVOLVE_GAZEBO_BRAIN_DIFFERENTIAL_SPLIT_BRAIN_H_ +#define REVOLVE_GAZEBO_BRAIN_DIFFERENTIAL_SPLIT_BRAIN_H_ + +#include "brain/split_cpg/simple_split_brain.h" +#include "brain/split_cpg/extended_neural_network_controller.h" +#include "brain/split_cpg/weight_vector_learner.h" +#include "evaluator.h" +#include "revolve/gazebo/brain/Brain.h" + + +#include +#include + +#include +#include + +namespace tol { + + class ExtNN : public revolve::gazebo::Brain, private revolve::brain::SimpleSplitBrain> { + + public: + /** + * Constructor for a neural network including neurons that are of a different type than the usual ones. + * @param modelName: name of the model + * @param evaluator: pointer to the evaluator that is used + * @param node: the sdf file containing the necessary information to build the network + * @param actuators: vector list of robot's actuators + * @param sensors: vector list of robot's sensors + * @return pointer to the neural network + */ + ExtNN(std::string modelName, + tol::EvaluatorPtr evaluator, + sdf::ElementPtr node, + const std::vector &actuators, + const std::vector &sensors); + + virtual ~ExtNN(); + + /** + * Method for updating sensors readings, actuators positions, ranked list of policies and generating new policy + * @param actuators: vector list of robot's actuators + * @param sensors: vector list of robot's sensors + * @param t: + * @param step: + */ + virtual void update(const std::vector &actuators, + const std::vector &sensors, + double t, + double step); + + /** + * Method to transform the sdf file to a configuration + * @param node: sdf file to be transformed + * @param motors: vector list of robot's motors + * @param sensors: vector list of robot's sensors + * @return configuration needed for the neural network + */ + static revolve::brain::ExtNNController::ExtNNConfig parseSDF(sdf::ElementPtr node, + const std::vector & motors, + const std::vector & sensors); + + /** + * Method to get the parameters of neurons from an sdf ElementPtr + * @param elem: sdf that includes the parameters + * @return string to parameter mapping + */ + static std::map parseSDFElement(sdf::ElementPtr elem); + + /** + * Helpermethod for getting the configuration + * adds a new connection to the configuration + * @param src: the beginning of the connetion + * @param dst: the end of the connection + * @param weight: weighting factor of the connection + * @param idToNeuron: mapping between the neurons and their ids + * @param ret: configuration for the network where the connection should be included + */ + static void connectionHelper(const std::string &src, + const std::string &dst, + const std::string &socket, + double weight, + const std::map &idToNeuron, + revolve::brain::ExtNNController::ExtNNConfig &ret); + /** + * Helpermethod for getting the configuration + * @param neuron: sdf for the neuron to be added + * @param ret: configuration for the network where the neuron should be added + */ + static revolve::brain::NeuronPtr neuronHelper(sdf::ElementPtr neuron, + revolve::brain::ExtNNController::ExtNNConfig &ret); + + /** + * Helpermethod for getting the configuration + * This function creates neurons and adds them to the configuration + * @param neuronId: id of the neuron + * @param neuronType: type of the neuron + * @param neuronLayer: layer the neuron should be added to, can be 'hidden', 'input' or 'output' + * @param params: parameters of the new neuron + */ + static revolve::brain::NeuronPtr addNeuron(const std::string &neuronId, + const std::string &neuronType, + const std::string &neuronLayer, + const std::map ¶ms, + revolve::brain::ExtNNController::ExtNNConfig &ret); + + }; + + +} /* namespace tol */ + +#endif //REVOLVE_GAZEBO_BRARN_DIFFERENTIAL_SPLIT_BRAIN_H_ + + diff --git a/scripts/offline-evolve/run-experiments.sh b/scripts/offline-evolve/run-experiments.sh index bc7a171..adf1981 100755 --- a/scripts/offline-evolve/run-experiments.sh +++ b/scripts/offline-evolve/run-experiments.sh @@ -10,7 +10,7 @@ robot_list=( spider9 spider13 spider17 gecko7 gecko12 gecko17 snake5 snake7 snak config=rlpower.cfg gz_command=gzserver -load_controller= +#load_controller= manager=single_robot_manager.py no_experiments=10 output=output diff --git a/tol-revolve.kdev4 b/tol-revolve.kdev4 new file mode 100644 index 0000000..401c4b8 --- /dev/null +++ b/tol-revolve.kdev4 @@ -0,0 +1,3 @@ +[Project] +Manager=KDevCMakeManager +Name=tol-revolve diff --git a/tol/build/builder.py b/tol/build/builder.py index c48a35f..7797117 100644 --- a/tol/build/builder.py +++ b/tol/build/builder.py @@ -1,7 +1,7 @@ from revolve.build.sdf import RobotBuilder, BodyBuilder, NeuralNetBuilder, BasicBattery from revolve.angle.robogen.util import apply_surface_parameters from sdfbuilder import SDF -from ..spec import get_body_spec, get_brain_spec +from ..spec import get_body_spec, get_brain_spec, get_extended_brain_spec def get_builder(conf): @@ -10,7 +10,7 @@ def get_builder(conf): :return: """ body_spec = get_body_spec(conf) - brain_spec = get_brain_spec(conf) + brain_spec = get_extended_brain_spec(conf) return RobotBuilder(BodyBuilder(body_spec, conf), NeuralNetBuilder(brain_spec)) diff --git a/tol/spec/__init__.py b/tol/spec/__init__.py index aa043df..3f7a09a 100644 --- a/tol/spec/__init__.py +++ b/tol/spec/__init__.py @@ -1,5 +1,6 @@ from .body import get_body_spec, get_body_generator from .brain import get_brain_spec, get_brain_generator +from .extended_brain import get_extended_brain_spec from .robot import get_tree_generator __author__ = 'Elte Hupkes' diff --git a/tol/spec/extended_brain.py b/tol/spec/extended_brain.py new file mode 100644 index 0000000..4da2a91 --- /dev/null +++ b/tol/spec/extended_brain.py @@ -0,0 +1,118 @@ +from __future__ import absolute_import + +from revolve.generate import NeuralNetworkGenerator +from revolve.spec import default_neural_net, NeuralNetImplementation, NeuronSpec, ParamSpec + +from ..config import constants + + +def get_extended_brain_spec(conf): + """ + Returns the brain specification corresponding to the + given config. + :param conf: + :return: + """ + epsilon = conf.brain_mutation_epsilon + + return NeuralNetImplementation({ + "Input": NeuronSpec( + layers=["input"] + ), + + "Sigmoid": NeuronSpec( + params=[ + ParamSpec("bias", min_value=-1, max_value=1, default=0, epsilon=epsilon), + ParamSpec("gain", min_value=0, max_value=1, default=.5, epsilon=epsilon) + ], + layers=["output", "hidden"] + ), + + "Simple": NeuronSpec( + params=[ + ParamSpec("bias", min_value=-1, max_value=1, epsilon=epsilon), + ParamSpec("gain", min_value=0, max_value=1, default=.5, epsilon=epsilon) + ], + layers=["output", "hidden"] + ), + + # "Gain": NeuronSpec( + # params=[ + # ParamSpec("gain", min_value=0, max_value=1, default=.5, epsilon=epsilon) + # ], + # layers=["output", "hidden"] + # ), + + "Bias": NeuronSpec( + params=[ + ParamSpec("bias", min_value=-1.0, max_value=1.0, epsilon=epsilon), + ], + layers=["output", "hidden"] + ), + + "Oscillator": NeuronSpec( + params=[ + ParamSpec("period", min_value=0, max_value=10, epsilon=epsilon), + ParamSpec("phase_offset", min_value=0, max_value=3.14, epsilon=epsilon), + ParamSpec("amplitude", min_value=0, default=1, max_value=10000, epsilon=epsilon) + ], + layers=["output", "hidden"] + ), + + + # these neurons are for the nonlinear oscillator CPG model found in Ijspeert (2005): + "V-Neuron": NeuronSpec( + params=[ + ParamSpec("alpha", min_value = 0.05, max_value = 10.0, epsilon = epsilon), + ParamSpec("tau", min_value = 1.0, max_value = 50.0, epsilon = epsilon), + ParamSpec("energy", min_value = 0.0, max_value = 25.0, epsilon = epsilon) + ], + layers = ["output", "hidden"] + ), + + "X-Neuron": NeuronSpec( + params=[ + ParamSpec("tau", min_value = 0.01, max_value = 5.0, epsilon = epsilon), + ], + layers = ["output", "hidden"] + ), + + "DifferentialCPG": NeuronSpec( + params=[ + ParamSpec("bias", min_value = -1.0, max_value = 1.0, epsilon = epsilon), + ], + layers = ["output", "hidden"] + ), + + # # Leaky integrator + # "Leaky": NeuronSpec( + # params=[ + # ParamSpec("bias", min_value = 0.01, max_value = 10.0, epsilon = epsilon), + # ParamSpec("tau", min_value = 0.01, max_value = 10.0, epsilon = epsilon), + # ], + # layers = ["output", "hidden"] + # ), + + # "NL-Main": NeuronSpec( + # params=[ + # ParamSpec("a", min_value = 0.01, max_value = 10.0, epsilon = epsilon), + # ParamSpec("amplitude", min_value = 0.01, max_value = 10.0, epsilon = epsilon) + # ], + # layers = ["output", "hidden"] + # ) + + # "NL-Theta": NeuronSpec( + # params=[ + # ParamSpec("freq", min_value = 0.01, max_value = 10.0, epsilon = epsilon) + # ], + # layers = ["output", "hidden"] + # ) + + # "QuadNeuron": NeuronSpec( + # params=[], + # layers = ["output", "hidden"] + # ) + + }) + +