diff --git a/.dockerignore b/.dockerignore new file mode 100644 index 0000000000..21607ddf7c --- /dev/null +++ b/.dockerignore @@ -0,0 +1,13 @@ +.git/ +/.hgignore +/.hgreview + +/build/ +/cmake-build-*/ +/.venv/ + +__pycache__/ + +.idea/ +.vscode/ +/experiments/*/data/ diff --git a/.gitignore b/.gitignore index 2d71a28844..cb41228637 100644 --- a/.gitignore +++ b/.gitignore @@ -10,7 +10,8 @@ __pycache__/ .Python env/ /build/ -/cmake-build-*/ +cmake-build-*/ +cmake-multineat-*/ /output*/ develop-eggs/ dist/ @@ -75,4 +76,9 @@ Pipfile.lock # Revolve related *.sdf -*.urdf \ No newline at end of file +*.urdf +src/ + +# OS crap +.DS_Store +.directory diff --git a/.gitmodules b/.gitmodules index c8afd881e1..7530606eb1 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "thirdparty/limbo"] path = thirdparty/limbo url = https://github.com/resibots/limbo.git +[submodule "thirdparty/MultiNEAT"] + path = thirdparty/MultiNEAT + url = https://github.com/ci-group/MultiNEAT.git diff --git a/Dockerfile b/Dockerfile index bc5005e53b..f5503fe710 100644 --- a/Dockerfile +++ b/Dockerfile @@ -11,9 +11,12 @@ RUN apt-get update && apt-get upgrade -y && apt-get install -y \ libcairo2-dev \ graphviz \ libeigen3-dev \ - libnlopt-dev && \ + libnlopt-dev \ + libboost-python-dev \ + libboost-numpy-dev &&\ apt-get clean && \ rm -rf /var/lib/apt/lists/* ADD . /revolve +RUN /revolve/docker/build_install_multineat.sh RUN /revolve/docker/build_revolve.sh diff --git a/cpprevolve/CMakeLists.txt b/cpprevolve/CMakeLists.txt index d39c8565d7..3d1e872370 100644 --- a/cpprevolve/CMakeLists.txt +++ b/cpprevolve/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required (VERSION 3.7.0) # Project name project (Revolve) -set (CMAKE_CXX_STANDARD 11) +set (CMAKE_CXX_STANDARD 17) # Include cmake subdirectories add_subdirectory(revolve/brains) diff --git a/cpprevolve/revolve/brains/CMakeLists.txt b/cpprevolve/revolve/brains/CMakeLists.txt index 8b7f614ff7..8f62bda78b 100644 --- a/cpprevolve/revolve/brains/CMakeLists.txt +++ b/cpprevolve/revolve/brains/CMakeLists.txt @@ -1,12 +1,12 @@ -file(GLOB_RECURSE - CONTROLLER_SRCS - controller/*.cpp - controller/actuators/*.cpp - controller/sensors/*.cpp +set (CMAKE_CXX_STANDARD 14) + +set(CONTROLLER_SRCS + controller/sensors/AngleToTargetDetector.cpp + controller/DifferentialCPG.cpp ) -file(GLOB_RECURSE - LEARNER_SRCS - learner/*.cpp + +set(LEARNER_SRCS + learner/BayesianOptimizer.cpp ) # PKG-CONFIG @@ -17,6 +17,13 @@ find_package(Boost REQUIRED COMPONENTS system) # Find Eigen3 - A lightweight C++ template library for vector and matrix math find_package(Eigen3 REQUIRED) +find_package(MultiNEAT REQUIRED) + +# These dependencies are required for the AngleToTargetDetector Sensor +find_package(OpenCV REQUIRED) +#find_package(raspicam REQUIRED) #only on the raspberry side + +find_package(MultiNEAT REQUIRED) # Find NLOpt - Non Linear Optimization pkg_check_modules(NLOpt REQUIRED nlopt>=2.4) @@ -30,16 +37,29 @@ add_library(revolve-learners SHARED ${LEARNER_SRCS}) target_include_directories(revolve-controllers PUBLIC ${EIGEN3_INCLUDE_DIR} - PUBLIC ${Boost_INCLUDE_DIRS}) + PUBLIC ${Boost_INCLUDE_DIRS} + PUBLIC ${OpenCV_INCLUDE_DIRS} +) target_include_directories(revolve-learners + PUBLIC ${EIGEN3_INCLUDE_DIR} PUBLIC ${Boost_INCLUDE_DIRS} PUBLIC ${LIMBO_DIR}/src PUBLIC ${NLOpt_INCLUDE_DIRS}) -target_include_directories(revolve-learners - PUBLIC ${NLOpt_LIBRARY_DIRS}) +target_compile_definitions(revolve-learners + PUBLIC USE_NLOPT=1 +) + +target_link_libraries(revolve-controllers + PUBLIC MultiNEAT::MultiNEAT + ${OpenCV_LIBS} + ) + +target_link_libraries(revolve-learners + revolve-controllers + MultiNEAT::MultiNEAT) install(TARGETS revolve-controllers revolve-learners RUNTIME DESTINATION bin - LIBRARY DESTINATION lib) \ No newline at end of file + LIBRARY DESTINATION lib) diff --git a/cpprevolve/revolve/brains/controller/Controller.h b/cpprevolve/revolve/brains/controller/Controller.h index 1512af0d3a..19187a1dcb 100644 --- a/cpprevolve/revolve/brains/controller/Controller.h +++ b/cpprevolve/revolve/brains/controller/Controller.h @@ -12,22 +12,36 @@ namespace revolve { +class DifferentialCPG; class Controller { public: + enum ControllerType { + NONE = 0, + NEURAL_NETWORK, + SPLINES, + DIFFERENTIAL_CPG, + RANDOM, + // add new controller types here + } const controller_type; + /// \brief Constructor - explicit Controller() {} + explicit Controller(ControllerType controller_type) + : controller_type(controller_type) + {} /// \brief Deconstructor virtual ~Controller() {} virtual void update( - const std::vector< std::unique_ptr< Actuator > > &_actuators, - const std::vector< std::unique_ptr< Sensor > > &_sensors, + const std::vector> &_actuators, + const std::vector> &_sensors, const double _time, const double _step ) = 0; + + virtual DifferentialCPG* into_DifferentialCPG() { return nullptr; } }; } diff --git a/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp b/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp index bba9ce77dc..bb37579b57 100644 --- a/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp +++ b/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp @@ -22,39 +22,79 @@ #include "DifferentialCPG.h" // STL macros +#include #include +#include +#include +#include +#include #include -#include #include #include -#include +#include + #include #include -#include +#include // Project headers #include "actuators/Actuator.h" - #include "sensors/Sensor.h" -// TODO: Resolve odd behaviour at the end of the validation procedure -// This behaviour is not present if you directly load a trained controller +// define this if you want to debug the weights of the CPG network +//#define DifferentialCPG_PRINT_INFO // Define namespaces using namespace revolve; /** - * Constructor for DifferentialCPG class. + * Constructor for DifferentialCPG class without cppn config. * * @param _model * @param robot_config */ DifferentialCPG::DifferentialCPG( - const DifferentialCPG::ControllerParams ¶ms, - const std::vector< std::unique_ptr< Actuator > > &actuators) - : next_state(nullptr) - , n_motors(actuators.size()) - , output(new double[actuators.size()]) + const DifferentialCPG::ControllerParams ¶ms, + const std::vector> &actuators, + std::shared_ptr angle_to_target_sensor) + : Controller(ControllerType::DIFFERENTIAL_CPG), next_state(nullptr), n_motors(actuators.size()), output(new double[actuators.size()]), angle_to_target_sensor(std::move(angle_to_target_sensor)), connection_weights(actuators.size(), 0) +{ + this->init_params_and_connections(params, actuators); + // Save weights for brain + assert(params.weights.size() == n_weights); + connection_weights.resize(n_weights, 0); + for (size_t j = 0; j < n_weights; j++) + { + connection_weights.at(j) = params.weights.at(j); + } + + // Set ODE matrix at initialization + set_ode_matrix(); + + std::cout << "Brain has been loaded." << std::endl; +} + +/** + * Constructor for DifferentialCPG class that loads weights from CPPN. + * + * @param params + * @param actuators + * @param config_cppn_genome + */ +DifferentialCPG::DifferentialCPG( + const DifferentialCPG::ControllerParams ¶ms, + const std::vector> &actuators, + const NEAT::Genome &gen, + std::shared_ptr angle_to_target_sensor) + : Controller(ControllerType::DIFFERENTIAL_CPG), next_state(nullptr), n_motors(actuators.size()), output(new double[actuators.size()]), angle_to_target_sensor(std::move(angle_to_target_sensor)), connection_weights(actuators.size(), 0) +{ + this->init_params_and_connections(params, actuators); + + this->load_genome_to_controller(gen); + std::cout << "DifferentialCPG brain with CPPN configuration has been loaded." << std::endl; +} + +void DifferentialCPG::init_params_and_connections(const ControllerParams ¶ms, const std::vector> &actuators) { // Controller parameters this->reset_neuron_random = params.reset_neuron_random; @@ -62,80 +102,115 @@ DifferentialCPG::DifferentialCPG( this->range_lb = -params.range_ub; this->range_ub = params.range_ub; this->use_frame_of_reference = params.use_frame_of_reference; - this->signal_factor_all_ = params.signal_factor_all; - this->signal_factor_mid = params.signal_factor_mid; - this->signal_factor_left_right = params.signal_factor_left_right; + this->output_signal_factor = params.output_signal_factor; this->abs_output_bound = params.abs_output_bound; + this->connection_weights = params.weights; + + if (use_frame_of_reference) + { + std::cout << "using frame of reference" << std::endl; + } + else + { + std::cout << "NOT using frame of reference" << std::endl; + } + + if (use_frame_of_reference and not angle_to_target_sensor) + { + std::clog << "WARNING!: use_frame_of_reference is activated but no angle_to_target_sensor camera is configured. " + "Disabling the use of the frame of reference" + << std::endl; + use_frame_of_reference = false; + } - size_t j=0; - for (const std::unique_ptr &actuator: actuators) + size_t j = 0; + for (const std::shared_ptr &actuator : actuators) { // Pass coordinates - auto coord_x = actuator->coordinate_x(); - auto coord_y = actuator->coordinate_y(); - this->motor_coordinates[{coord_x, coord_y}] = j; + int coord_x = actuator->coordinate_x(); + int coord_y = actuator->coordinate_y(); + int coord_z = actuator->coordinate_z(); + this->motor_coordinates[{coord_x, coord_y, coord_z}] = j; // Set frame of reference int frame_of_reference = 0; // We are a left neuron - if (coord_x < 0) + if (coord_y < 0) { frame_of_reference = -1; } // We are a right neuron - else if (coord_x > 0) + else if (coord_y > 0) { frame_of_reference = 1; } // Save neurons: bias/gain/state. Make sure initial states are of different sign. - this->neurons[{coord_x, coord_y, 1}] = {0.f, 0.f, this->init_neuron_state, frame_of_reference}; //Neuron A - this->neurons[{coord_x, coord_y, -1}] = {0.f, 0.f, -this->init_neuron_state, frame_of_reference}; // Neuron B + neurons.emplace_back(Neuron{coord_x, coord_y, coord_z, 1, 0.f, 0.f, this->init_neuron_state, frame_of_reference}); + neurons.emplace_back(Neuron{coord_x, coord_y, coord_z, -1, 0.f, 0.f, -this->init_neuron_state, frame_of_reference}); + + // These connections don't have to be made explicit + //this->connections[{coord_x, coord_y, coord_z, 1, coord_x, coord_y, coord_z, -1}] = j; + //this->connections[{coord_x, coord_y, coord_z, -1, coord_x, coord_y, coord_z, 1}] = j; j++; } // Add connections between neighbouring neurons - int i = 0; - for (const std::unique_ptr &actuator: actuators) + size_t i = j; + for (const std::shared_ptr &actuator : actuators) { // Get name and x,y-coordinates of all neurons. - double x = actuator->coordinate_x(); - double y = actuator->coordinate_y(); + const double x = actuator->coordinate_x(); + const double y = actuator->coordinate_y(); + const double z = actuator->coordinate_z(); // Continue to next iteration in case there is already a connection between the 1 and -1 neuron. // These checks feel a bit redundant. // if A->B connection exists. - if (this->connections.count({x, y, 1, x, y, -1}) > 0) + if (this->connections.count({x, y, z, 1, x, y, z, -1}) > 0) { continue; } // if B->A connection exists: - if (this->connections.count({x, y, -1, x, y, 1}) > 0) + if (this->connections.count({x, y, z, -1, x, y, z, 1}) > 0) { continue; } // Loop over all positions. We call it neighbours, but we still need to check if they are a neighbour. - for (const std::unique_ptr &neighbour: actuators) + for (const std::shared_ptr &neighbour : actuators) { + if (neighbour.get() == actuator.get()) + { + continue; + } + // Get information of this neuron (that we call neighbour). - double near_x = neighbour->coordinate_x(); - double near_y = neighbour->coordinate_y(); + const double near_x = neighbour->coordinate_x(); + const double near_y = neighbour->coordinate_y(); + const double near_z = neighbour->coordinate_z(); // If there is a node that is a Moore neighbour, we set it to be a neighbour for their A-nodes. // Thus the connections list only contains connections to the A-neighbourhood, and not the // A->B and B->A for some node (which makes sense). - double dist_x = std::fabs(x - near_x); - double dist_y = std::fabs(y - near_y); + const double dist_x = std::fabs(x - near_x); + const double dist_y = std::fabs(y - near_y); + const double dist_z = std::fabs(z - near_z); // TODO: Verify for non-spiders - if (std::fabs(dist_x + dist_y - 2) < 0.01) + if (dist_x + dist_y + dist_z < 2.01) { - if(std::get<0>(this->connections[{x, y, 1, near_x, near_y, 1}]) != 1 or - std::get<0>(this->connections[{near_x, near_y, 1, x, y, 1}]) != 1) + if (this->connections.count({x, y, z, 1, near_x, near_y, near_z, 1}) == 0 and + this->connections.count({near_x, near_y, near_z, 1, x, y, z, 1}) == 0) { - this->connections[{x, y, 1, near_x, near_y, 1}] = std::make_tuple(1, i); - this->connections[{near_x, near_y, 1, x, y, 1}] = std::make_tuple(1, i); +#ifdef DifferentialCPG_PRINT_INFO + std::cout << "Creating connnection [" + << x << ';' << y << ';' << z << ';' << 1 << '|' + << near_x << ';' << near_y << ';' << near_z << ';' << 1 + << "] to connection_weights[" << i << ']' << std::endl; +#endif + this->connections[{x, y, z, 1, near_x, near_y, near_z, 1}] = i; + //this->connections[{near_x, near_y, near_z, 1, x, y, z, 1}] = i; i++; } } @@ -144,22 +219,9 @@ DifferentialCPG::DifferentialCPG( // Initialise array of neuron states for Update() method this->next_state = new double[this->neurons.size()]; - this->n_weights = (int)(this->connections.size()/2) + this->n_motors; - - // Loading Brain - // Save weights for brain - assert(params.weights.size() == this->n_weights); - this->sample.resize(this->n_weights); - for(size_t j = 0; j < this->n_weights; j++) - { - this->sample(j) = params.weights.at(j); - } - - // Set ODE matrix at initialization - this->set_ode_matrix(); - - std::cout << "Brain has been loaded." << std::endl; + // the size is: external connection weights + internal CPG weights + this->n_weights = (int)(this->connections.size()) + this->n_motors; } /** @@ -171,6 +233,79 @@ DifferentialCPG::~DifferentialCPG() delete[] this->output; } +void DifferentialCPG::set_connection_weights(std::vector weights) +{ + this->connection_weights = weights; + this->set_ode_matrix(); +} + +void DifferentialCPG::load_genome_to_controller(const NEAT::Genome &genome) +{ + // build the NN according to the genome + NEAT::NeuralNetwork net; + genome.BuildPhenotype(net); + unsigned int net_depth = net.CalculateNetworkDepth(); + + // get weights for each connection + // assuming that connections are distinct for each direction + connection_weights.resize(n_weights, 0); + std::vector inputs(8); + + for (const std::pair, size_t> &motor : motor_coordinates) + { + size_t k = motor.second; + + // convert tuple to vector + std::tie(inputs[0], inputs[1], inputs[2]) = motor.first; + inputs[3] = 1; + std::tie(inputs[4], inputs[5], inputs[6]) = motor.first; + inputs[7] = -1; + inputs[8] = 1; + + net.Flush(); + net.Input(inputs); + for (int i = 0; i < net_depth; i++) + net.Activate(); + double weight = net.Output()[0]; +#ifdef DifferentialCPG_PRINT_INFO + std::cout << "Creating weight [" + << inputs[0] << ';' << inputs[1] << ';' << inputs[2] << ';' << inputs[3] << '|' + << inputs[4] << ';' << inputs[5] << ';' << inputs[6] << ';' << inputs[7] + << "] to connection_weights[" << k << "]\t-> " << weight << std::endl; +#endif + this->connection_weights.at(k) = weight; // order of weights corresponds to order of connections. + } + + for (const std::pair, int> &con : connections) + { + int k = con.second; + // convert tuple to vector + std::tie(inputs[0], inputs[1], inputs[2], inputs[3], inputs[4], inputs[5], inputs[6], inputs[7]) = con.first; + inputs[8] = 1; + + net.Flush(); + net.Input(inputs); + for (int i = 0; i < net_depth; i++) + net.Activate(); + double weight = net.Output()[0]; +#ifdef DifferentialCPG_PRINT_INFO + std::cout << "Creating weight [" + << inputs[0] << ';' << inputs[1] << ';' << inputs[2] << ';' << inputs[3] << '|' + << inputs[4] << ';' << inputs[5] << ';' << inputs[6] << ';' << inputs[7] + << "] to connection_weights[" << k << "]\t-> " << weight << std::endl; +#endif + this->connection_weights.at(k) = weight; // order of weights corresponds to order of connections. + } + + // Set ODE matrix at initialization + this->set_ode_matrix(); +} + +std::vector DifferentialCPG::get_connection_weights() +{ + return this->connection_weights; +} + /** * Callback function that defines the movement of the robot * @@ -180,16 +315,16 @@ DifferentialCPG::~DifferentialCPG() * @param _step */ void DifferentialCPG::update( - const std::vector< std::unique_ptr < Actuator > > &actuators, - const std::vector< std::unique_ptr < Sensor > > &sensors, - const double time, - const double step) + const std::vector> &actuators, + const std::vector> &sensors, + const double time, + const double step) { // Send new signals to the motors this->step(time, step); unsigned int p = 0; - for (const auto &actuator: actuators) + for (const auto &actuator : actuators) { actuator->write(this->output + p, step); p += actuator->n_outputs(); @@ -200,44 +335,49 @@ void DifferentialCPG::update( * Make matrix of weights A as defined in dx/dt = Ax. * Element (i,j) specifies weight from neuron i to neuron j in the system of ODEs */ -void DifferentialCPG::set_ode_matrix(){ +void DifferentialCPG::set_ode_matrix() +{ // Initiate new matrix std::vector> matrix; + matrix.reserve(this->neurons.size()); // Fill with zeroes - for(size_t i =0; i neurons.size(); i++) + for (size_t i = 0; i < this->neurons.size(); i++) { // Initialize row in matrix with zeros - std::vector< double > row; - for (size_t j = 0; j < this->neurons.size(); j++) - { - row.push_back(0); - } - matrix.push_back(row); + const std::vector row(this->neurons.size(), 0); + matrix.emplace_back(row); } - // Process A<->B connections + // Process A<->A connections int index = 0; - for(size_t i =0; i neurons.size(); i++) + for (const Neuron &neuron : neurons) { - // Get correct index - int c = 0; - if (i%2 == 0){ - c = i + 1; - } - else{ - c = i - 1; + int x = neuron.x; + int y = neuron.y; + int z = neuron.z; + if (neuron.w < 0) + { + continue; } - - // Add a/b connection weight - index = (int)(i/2); - auto w = this->sample(index) * - (this->range_ub - this->range_lb) + this->range_lb; - matrix[i][c] = w; - matrix[c][i] = -w; + size_t k = motor_coordinates.at({x, y, z}); +#ifdef DifferentialCPG_PRINT_INFO + std::cout << "Setting connection [" + << x << ';' << y << ';' << z << ';' << 1 << '|' + << x << ';' << y << ';' << z << ';' << -1 + << "] to connection_weights[" << k << "]\t-> " << this->connection_weights.at(k) << std::endl; +#endif + auto weight = this->connection_weights.at(k) * + (this->range_ub - this->range_lb) + + this->range_lb; + size_t i = index; + size_t c = index + 1; + matrix.at(i).at(c) = weight; + matrix.at(c).at(i) = -weight; + index += 2; } - // A<->A connections + // A<->B connections index++; int k = 0; std::vector connections_seen; @@ -245,22 +385,24 @@ void DifferentialCPG::set_ode_matrix(){ for (auto const &connection : this->connections) { // Get connection information - int x1, y1, z1, x2, y2, z2; - std::tie(x1, y1, z1, x2, y2, z2) = connection.first; + int x1, y1, z1, w1, x2, y2, z2, w2; + std::tie(x1, y1, z1, w1, x2, y2, z2, w2) = connection.first; // Find location of the two neurons in this->neurons list int l1 = -1; int l2 = -1; int c = 0; - for(auto const &neuron : this->neurons) + for (const Neuron &neuron : this->neurons) { - int x, y, z; - std::tie(x, y, z) = neuron.first; - if (x == x1 and y == y1 and z == z1) + int x = neuron.x; + int y = neuron.y; + int z = neuron.z; + int w = neuron.w; + if (x == x1 and y == y1 and z == z1 and w == w1) { l1 = c; } - else if (x == x2 and y == y2 and z == z2) + else if (x == x2 and y == y2 and z == z2 and w == w2) { l2 = c; } @@ -269,8 +411,9 @@ void DifferentialCPG::set_ode_matrix(){ } // Add connection to seen connections - if(l1 > l2) + if (l1 > l2) { + // swap l1 and l2 int l1_old = l1; l1 = l2; l2 = l1_old; @@ -279,18 +422,30 @@ void DifferentialCPG::set_ode_matrix(){ // if not in list, add to list auto connections_list = std::find(connections_seen.begin(), connections_seen.end(), connection_string); - if(connections_list == connections_seen.end()) + if (connections_list == connections_seen.end()) { connections_seen.push_back(connection_string); } - // else continue to next iteration - else{ + else // else continue to next iteration + { + // actually, we should never encounter this, every connection should appear only once + std::cerr << "Should not see the same connection appearing twice: " << connection_string << std::endl; + throw std::runtime_error("Should not see the same connection appearing twice"); continue; } + const int sample_index = connections[{x1, y1, z1, w1, x2, y2, z2, w2}]; +#ifdef DifferentialCPG_PRINT_INFO + std::cout << "Setting connection [" + << x1 << ';' << y1 << ';' << z1 << ';' << w1 << '|' + << x2 << ';' << y2 << ';' << z2 << ';' << w2 + << "] to connection_weights[" << sample_index << "]\t-> " << this->connection_weights.at(sample_index) << std::endl; +#endif + // Get weight - auto w = this->sample(index + k) * - (this->range_ub - this->range_lb) + this->range_lb; + const auto w = this->connection_weights.at(sample_index) * + (this->range_ub - this->range_lb) + + this->range_lb; // Set connection in weight matrix matrix[l1][l2] = w; @@ -298,58 +453,54 @@ void DifferentialCPG::set_ode_matrix(){ k++; } +#ifdef DifferentialCPG_PRINT_INFO + std::cout << "DifferentialCPG: added " << connections_seen.size() << " connections" << std::endl; +#endif + // Update matrix - this->ode_matrix = matrix; + this->ode_matrix = std::move(matrix); // Reset neuron state this->reset_neuron_state(); -} +#ifdef DifferentialCPG_PRINT_INFO + std::cout << " Matrix " << std::endl; + for (const auto &row : ode_matrix) + { + + std::cout << "| "; + for (double value : row) + { + std::cout << std::setw(5) << std::setprecision(2) << value << ' '; + } + std::cout << '|' << std::endl; + } +#endif +} /** * Set states back to original value (that is on the unit circle) */ -void DifferentialCPG::reset_neuron_state(){ - int c = 0; - for(auto const &neuron : this->neurons) +void DifferentialCPG::reset_neuron_state() +{ + for (Neuron &neuron : this->neurons) { - // Get neuron properties - int x, y, z, frame_of_reference; - double bias ,gain ,state; - std::tie(x, y, z) = neuron.first; - std::tie(bias, gain, state, frame_of_reference) = neuron.second; - - if (z == -1) + neuron.bias = 0.0f; + neuron.gain = 0.0f; + if (this->reset_neuron_random) + { + neuron.state = ((double)rand() / (RAND_MAX)) * 2 * this->init_neuron_state - this->init_neuron_state; + } + else if (neuron.w == -1) { // Neuron B - if (this->reset_neuron_random) - { - this->neurons[{x, y, z}] = {0.f, - 0.f, - ((double) rand() / (RAND_MAX))*2*this->init_neuron_state - this->init_neuron_state, - frame_of_reference}; - } - else - { - this->neurons[{x, y, z}] = {0.f, 0.f, -this->init_neuron_state, frame_of_reference}; - } + neuron.state = -this->init_neuron_state; } else { // Neuron A - if (this->reset_neuron_random) - { - this->neurons[{x, y, z}] = {0.f, - 0.f, - ((double) rand() / (RAND_MAX))*2*this->init_neuron_state - this->init_neuron_state, - frame_of_reference}; - } - else - { - this->neurons[{x, y, z}] = {0.f, 0.f, +this->init_neuron_state, frame_of_reference}; - } + neuron.state = this->init_neuron_state; } - c++; } } @@ -360,65 +511,95 @@ void DifferentialCPG::reset_neuron_state(){ * @param _output */ void DifferentialCPG::step( - const double time, - const double dt) + const double time, + const double dt) { int neuron_count = 0; - for (const auto &neuron : this->neurons) + for (const Neuron &neuron : this->neurons) { - // Neuron.second accesses the second 3-tuple of a neuron, containing the bias/gain/state. - double recipient_bias, recipient_gain, recipient_state; - int frame_of_reference; - std::tie(recipient_bias, recipient_gain, recipient_state, frame_of_reference) = neuron.second; + double recipient_state = neuron.state; // Save for ODE this->next_state[neuron_count] = recipient_state; neuron_count++; } - // Copy values from next_state into x for ODEINT - state_type x(this->neurons.size()); + // Copy values from next_state into x_state for ODEINT + state_type x_state(this->neurons.size()); for (size_t i = 0; i < this->neurons.size(); i++) { - x[i] = this->next_state[i]; + x_state[i] = this->next_state[i]; } // Perform one step stepper.do_step( - [this](const state_type &x, state_type &dxdt, double t) + [this](const state_type &x, state_type &dxdt, double t) + { + for (size_t i = 0; i < this->neurons.size(); i++) { - for(size_t i = 0; i < this->neurons.size(); i++) + dxdt[i] = 0; + for (size_t j = 0; j < this->neurons.size(); j++) { - dxdt[i] = 0; - for(size_t j = 0; j < this->neurons.size(); j++) - { - dxdt[i] += x[j]*this->ode_matrix[j][i]; - } + dxdt[i] += x[j] * this->ode_matrix[j][i]; } - }, - x, - time, - dt); + } + }, + x_state, + time, + dt); // Copy values into nextstate for (size_t i = 0; i < this->neurons.size(); i++) { - this->next_state[i] = x[i]; + this->next_state[i] = x_state[i]; + } + + // // Load the angle value from the sensor + // double angle_difference = this->angle_to_goal - move_angle; + // if (angle_difference > M_PI) + // angle_difference -= 2 * M_PI; + // else if (angle_difference < -M_PI) + // angle_difference += 2 * M_PI; + // this->angle_diff = angle_difference; + + // Factor with which robot slows down to allow for steering + double angle_difference = 0.0; + double slow_down_factor = 1.0; + if (use_frame_of_reference) + { + angle_difference = angle_to_target_sensor->detect_angle(); + + // Pick smallest angle between target and sensor + if (angle_difference > M_PI) + { + // std::cout << "Angle [" << angle_difference << "] is greater then pi" << std::endl; + angle_difference -= 2 * M_PI; + } + else if (angle_difference < -M_PI) + { + // std::cout << "Angle [" << angle_difference << "] is smaller then pi" << std::endl; + angle_difference += 2 * M_PI; + } + // Power determines strength of penalty, the higher the value the stronger the penalty + const double frame_of_reference_slower_power = 7.0; + slow_down_factor = std::pow( + (M_PI - std::abs(angle_difference)) / M_PI, frame_of_reference_slower_power); + // std::cout << "Angle difference is [" << angle_difference << "], robot is slowed down by factor [" << slow_down_factor << "]" << std::endl; } // Loop over all neurons to actually update their states. Note that this is a new outer for loop - auto i = 0; auto j = 0; - for (auto &neuron : this->neurons) + auto i = 0; + auto j = 0; + for (Neuron &neuron : this->neurons) { // Get bias gain and state for this neuron. Note that we don't take the coordinates. // However, they are implicit as their order did not change. - double bias, gain, state; - int frame_of_reference; - std::tie(bias, gain, state, frame_of_reference) = neuron.second; - double x, y, z; - std::tie(x, y, z) = neuron.first; - neuron.second = {bias, gain, this->next_state[i], frame_of_reference}; - j = this->motor_coordinates[{x,y}]; + int x = neuron.x; + int y = neuron.y; + int z = neuron.z; + int frame_of_reference = neuron.frame; + neuron.state = this->next_state[i]; + j = this->motor_coordinates[{x, y, z}]; // Should be one, as output should be based on +1 neurons, which are the A neurons if (i % 2 == 1) { @@ -426,32 +607,30 @@ void DifferentialCPG::step( // f(a) = (w_ao*a - bias)*gain // Apply saturation formula - auto x = this->next_state[i]; + auto x_input = this->next_state[i]; + + double output_j = this->output_function(x_input); + // std::cout << "output_j before slowing down = [" << output_j << "]" << std::endl; // Use frame of reference - if(use_frame_of_reference) + if (use_frame_of_reference and frame_of_reference != 0) { - - if (std::abs(frame_of_reference) == 1) - { - this->output[j] = this->signal_factor_left_right*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0*x/this->abs_output_bound)) -1); - } - else if (frame_of_reference == 0) - { - this->output[j] = this->signal_factor_mid*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0*x/this->abs_output_bound)) -1); - } - else + // std::cout << "Frame of reference is [" << frame_of_reference << "]" << std::endl; + if ((frame_of_reference == 1 and angle_difference < 0) or + (frame_of_reference == -1 and angle_difference > 0)) //TODO >= / <= ? { - std::clog << "WARNING: frame_of_reference not in {-1,0,1}." << std::endl; + output_j *= slow_down_factor; + // std::cout << "Slow down " << x <<','<< y <<','<< z << " with factor " << slow_down_factor << std::endl; + // std::cout << "New output_j equals [" << output_j << "]" << std::endl; } - - } - // Don't use frame of reference - else - { - this->output[j] = this->signal_factor_all_*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0*x/this->abs_output_bound)) -1); } + this->output[j] = (output_j + 1.0) / 2.0; } i++; } } + +double DifferentialCPG::output_function(double input) const +{ + return this->output_signal_factor * this->abs_output_bound * ((2.0) / (1.0 + std::pow(2.718, -2.0 * input / this->abs_output_bound)) - 1); +} diff --git a/cpprevolve/revolve/brains/controller/DifferentialCPG.h b/cpprevolve/revolve/brains/controller/DifferentialCPG.h index d223f45d9a..c539f25859 100644 --- a/cpprevolve/revolve/brains/controller/DifferentialCPG.h +++ b/cpprevolve/revolve/brains/controller/DifferentialCPG.h @@ -8,9 +8,12 @@ #include "Controller.h" #include "actuators/Actuator.h" #include "sensors/Sensor.h" +#include "sensors/AngleToTargetDetector.h" + #include +#include #include -#include +#include typedef std::vector< double > state_type; @@ -25,41 +28,61 @@ class DifferentialCPG bool use_frame_of_reference; double init_neuron_state; double range_ub; - double signal_factor_all; - double signal_factor_mid; - double signal_factor_left_right; + double output_signal_factor; double abs_output_bound; std::vector< double > weights; + /// can be null, indicating that there is no map + std::unique_ptr>> connection_map; }; /// \brief Constructor - /// \param[in] _modelName Name of the robot - /// \param[in] _node The brain node - /// \param[in] _motors Reference to a motor list, it be reordered - /// \param[in] _sensors Reference to a sensor list, it might be reordered + /// \param[in] params Parameters for the controller + /// \param[in] _actuators Reference to a actuator list + DifferentialCPG( + const DifferentialCPG::ControllerParams ¶ms, + const std::vector> &_actuators, + std::shared_ptr angle_to_target_sensor = nullptr); + + /// \brief Constructor for Controller with config CPPN + /// \param[in] params Parameters for the controller + /// \param[in] _actuators Reference to a actuator list + /// \param[in] config_cppn_genome Reference to the genome for configuring the weights in CPG DifferentialCPG( - const ControllerParams ¶ms, - const std::vector< std::unique_ptr < Actuator > > &_actuators); + const DifferentialCPG::ControllerParams ¶ms, + const std::vector> &_actuators, + const NEAT::Genome &config_cppn_genome, + std::shared_ptr angle_to_target_sensor = nullptr); /// \brief Destructor virtual ~DifferentialCPG(); /// \brief The default update method for the controller - /// \param[in] _motors Motor list + /// \param[in] _actuators Actuator list /// \param[in] _sensors Sensor list /// \param[in] _time Current world time /// \param[in] _step Current time step virtual void update( - const std::vector< std::unique_ptr < Actuator > > &actuators, - const std::vector< std::unique_ptr < Sensor > > &sensors, - const double _time, - const double _step) override; + const std::vector> &actuators, + const std::vector> &sensors, + double _time, + double _step) override; + + /// \brief Set the connection weights of the Controller and make sure the matrix is set appropriately + /// \param[in] The weights to be set + void set_connection_weights(std::vector weights); + + void load_genome_to_controller(const NEAT::Genome &genome); + + DifferentialCPG* into_DifferentialCPG() override { return this; }; + + /// \brief Return the weights of the connections + std::vector get_connection_weights(); protected: - void step( - const double time, - const double step); + void step(double time, double step); + + void init_params_and_connections(const ControllerParams ¶ms, const std::vector> &actuators); void set_ode_matrix(); @@ -67,25 +90,32 @@ class DifferentialCPG /// \brief Function that resets neuron state void reset_neuron_state(); + /// \brief function that transforms the value of the CPG A-neurons and returns the correct output for the actuators + double output_function(double input) const; + public: - std::map< std::tuple< double, double >, size_t > motor_coordinates; + std::map< std::tuple< int, int, int >, size_t > motor_coordinates; protected: /// \brief Register of motor IDs and their x,y-coordinates // std::map< std::string, std::tuple< int, int > > positions; - /// \brief Register of individual neurons in x,y,z-coordinates - /// \details x,y-coordinates define position of a robot's module and - // z-coordinate define A or B neuron (z=1 or -1 respectively). Stored + struct Neuron { + int x, y, z, w; + double bias, gain, state; + int frame; + }; + + /// \brief Register of individual neurons in x,y,z,w-coordinates + /// \details x,y,z-coordinates define position of a robot's module and + // w-coordinate define A or B neuron (w=1 or -1 respectively). Stored // values are a bias, gain, state and frame of reference of each neuron. - std::map< std::tuple< int, int, int >, std::tuple< double, double, double, int > > - neurons; + std::vector< Neuron > neurons; /// \brief Register of connections between neighnouring neurons - /// \details Coordinate set of two neurons (x1, y1, z1) and (x2, y2, z2) - // define a connection. The second tuple contains 1: the connection value and - // 2: the weight index corresponding to this connection. - std::map< std::tuple< int, int, int, int, int, int >, std::tuple > + /// \details Coordinate set of two neurons (x1, y1, z1, w1) and (x2, y2, z2, w2) + /// define a connection. The value is the weight index corresponding to this connection. + std::map< std::tuple< int, int, int, int, int, int, int, int>, int > connections; /// \brief Runge-Kutta 45 stepper @@ -94,6 +124,9 @@ class DifferentialCPG /// \brief Used for ODE-int std::vector> ode_matrix; + /// \brief Angle sensor holder + std::shared_ptr<::revolve::AngleToTargetDetector> angle_to_target_sensor; + private: /// \brief Used to determine the next state array double *next_state; @@ -107,20 +140,14 @@ class DifferentialCPG /// \brief Limbo optimizes in [0,1] double range_ub; - /// \brief Loaded sample - Eigen::VectorXd sample; + /// \brief Loaded weights + std::vector connection_weights; /// \brief The number of weights to optimize size_t n_weights; /// \brief Factor to multiply output signal with - double signal_factor_all_; - - /// \brief Factor to multiply output signal with - double signal_factor_mid; - - /// \brief Factor to multiply output signal with - double signal_factor_left_right; + double output_signal_factor; /// \brief When reset a neuron state,do it randomly: bool reset_neuron_random; @@ -135,6 +162,7 @@ class DifferentialCPG bool use_frame_of_reference; double abs_output_bound; + }; } diff --git a/cpprevolve/revolve/brains/controller/DifferentialCPG2.cpp b/cpprevolve/revolve/brains/controller/DifferentialCPG2.cpp new file mode 100644 index 0000000000..2d59a24141 --- /dev/null +++ b/cpprevolve/revolve/brains/controller/DifferentialCPG2.cpp @@ -0,0 +1,111 @@ +#include "DifferentialCPG2.hpp" + +#include +#include + +using namespace revolve; + +DifferentialCPG2::DifferentialCPG2(DifferentialCPG2::Parameters const ¶meters, std::vector> const &actuators) +{ + create_neurons(parameters, actuators); + create_ode_matrix(parameters, actuators); +} + +void DifferentialCPG2::create_neurons(Parameters const ¶meters, std::vector> const &actuators) +{ + // create two neurons per actuator + // save index of first neuron as output neuron for an actuator + for (size_t i = 0; i < actuators.size(); ++i) + { + actuator_neurons.push_back(neuron_state.size()); + neuron_state.push_back(parameters.initial_neuron_state); + neuron_state.push_back(parameters.initial_neuron_state); + } +} + +void DifferentialCPG2::create_ode_matrix(Parameters const ¶meters, std::vector> const &actuators) +{ + // reserve space for matrix + ode_matrix.reserve(actuators.size()); + for (size_t i = 0; i < actuators.size(); ++i) + { + ode_matrix.emplace_back(std::vector(actuators.size(), 0)); + } + + size_t external_weight_index = actuators.size(); + + for (size_t i = 0; i < actuators.size(); ++i) + { + // internal weights + ode_matrix[i * 2][i * 2 + 1] = parameters.weights[i]; + ode_matrix[i * 2 + 1][i * 2] = -parameters.weights[i]; + + // external weights + double const x1 = actuators[i]->coordinate_x(); + double const y1 = actuators[i]->coordinate_y(); + double const z1 = actuators[i]->coordinate_z(); + + for (size_t j = i + 1; j < actuators.size(); ++i) + { + double const x2 = actuators[j]->coordinate_x(); + double const y2 = actuators[j]->coordinate_y(); + double const z2 = actuators[j]->coordinate_z(); + + double const dist_x = std::fabs(x1 - x2); + double const dist_y = std::fabs(y1 - y2); + double const dist_z = std::fabs(z1 - z2); + + // connect moore neighbours + if (dist_x < 1.01 && dist_y < 1.01 && dist_z < 1.01) + { + if (parameters.weights.size() - 1 < external_weight_index) + { + std::cerr << "Not enough weights provided for DifferentialCPG2 controller. Expected at least " << external_weight_index + 1 << ". Throwing error..\n"; + throw std::runtime_error(std::string("Not enough weights provided for DifferentialCPG2 controller. Expected at least ") + std::to_string(external_weight_index + 1) + "."); + } + ode_matrix[i * 2][j * 2] = parameters.weights[external_weight_index]; + ode_matrix[j * 2][i * 2] = -parameters.weights[external_weight_index]; + external_weight_index += 1; + } + } + } + + if (parameters.weights.size() != external_weight_index) + { + std::cerr << "Too many weights provided for DifferentialCPG2 controller. Expected exactly " << external_weight_index << ". Throwing error..\n"; + throw std::runtime_error(std::string("Too many weights provided for DifferentialCPG2 controller. Expected exactly ") + std::to_string(external_weight_index) + "."); + } +} + +void DifferentialCPG2::update( + std::vector> const &actuators, + std::vector> const &sensors, + double const time, + double const dt) +{ + integrate_neural_network(time, dt); + update_actuators(actuators, dt); +} + +void DifferentialCPG2::integrate_neural_network(double const time, double const dt) +{ + ode_solver.do_step( + [this](std::vector const &state, std::vector &dxdt, double) + { + for (size_t i = 0; i < state.size(); ++i) + { + dxdt[i] = state[i] * std::accumulate(ode_matrix[i].begin(), ode_matrix[i].end(), 0, std::plus()); + } + }, + neuron_state, + time, + dt); +} + +void DifferentialCPG2::update_actuators(std::vector> const &actuators, double const dt) +{ + for (size_t i = 0; i < actuators.size(); ++i) + { + actuators[i]->write(&neuron_state[actuator_neurons[i]], dt); + } +} diff --git a/cpprevolve/revolve/brains/controller/DifferentialCPG2.hpp b/cpprevolve/revolve/brains/controller/DifferentialCPG2.hpp new file mode 100644 index 0000000000..7c9955389c --- /dev/null +++ b/cpprevolve/revolve/brains/controller/DifferentialCPG2.hpp @@ -0,0 +1,52 @@ +#ifndef REVOLVE_DIFFERENTIAL_CPG_2_HPP +#define REVOLVE_DIFFERENTIAL_CPG_2_HPP + +#include "Controller.h" + +#include + +#include "actuators/Actuator.h" +#include + +namespace revolve +{ + + class DifferentialCPG2 : public Controller + { + public: + struct Parameters + { + double initial_neuron_state; + double output_signal_gain; + double abs_output_bound; + std::vector weights; + }; + + DifferentialCPG2(Parameters const ¶meters, std::vector> const &actuators); + + void update( + std::vector> const &actuators, + std::vector> const &sensors, + double const time, + double const dt) override; + + private: + // indices of neurons corresponding to actuators + std::vector actuator_neurons; + + std::vector neuron_state; + + std::vector> ode_matrix; + + // neural network ode solver + boost::numeric::odeint::runge_kutta4> ode_solver; + + void create_neurons(Parameters const ¶meters, std::vector> const &actuators); + void create_ode_matrix(Parameters const ¶meters, std::vector> const &actuators); + void integrate_neural_network(double const time, double const dt); + void update_actuators(std::vector> const &actuators, double const dt); + }; + +} + +#endif // REVOLVE_DIFFERENTIAL_CPG_2_HPP diff --git a/cpprevolve/revolve/brains/controller/FixedAngleController.h b/cpprevolve/revolve/brains/controller/FixedAngleController.h new file mode 100644 index 0000000000..47a5e5cd90 --- /dev/null +++ b/cpprevolve/revolve/brains/controller/FixedAngleController.h @@ -0,0 +1,40 @@ +// +// Created by Matteo De Carlo on 11/09/2019. +// + +#ifndef REVOLVE_FIXEDANGLECONTROLLER_H +#define REVOLVE_FIXEDANGLECONTROLLER_H + +#include "Controller.h" + +namespace revolve +{ + +class FixedAngleController: public Controller +{ +public: + explicit FixedAngleController(double angle) + : Controller(ControllerType::NONE) + , angle(angle) + {} + + void update(const std::vector > &_actuators, + const std::vector > &_sensors, + const double _time, + const double _step) override + { + std::vector output(1, angle); + for (auto &actuator: _actuators) { + output.resize(actuator->n_outputs(), angle); + actuator->write(output.data(), _step); + } + } + +private: + double angle; +}; + +} + + +#endif //REVOLVE_FIXEDANGLECONTROLLER_H diff --git a/cpprevolve/revolve/brains/controller/actuators/Actuator.h b/cpprevolve/revolve/brains/controller/actuators/Actuator.h index e8bf932cec..a961aa4888 100644 --- a/cpprevolve/revolve/brains/controller/actuators/Actuator.h +++ b/cpprevolve/revolve/brains/controller/actuators/Actuator.h @@ -24,13 +24,21 @@ class Actuator inline double coordinate_y() const { return std::get<1>(this->coordinates); } inline double coordinate_z() const { return std::get<2>(this->coordinates); } + enum StateType { + POSITION, + VELOCITY, + TORQUE + }; + + virtual double Current_State( StateType type ) = 0; + virtual void write(const double *output, double step) = 0; inline unsigned int n_outputs() const {return this->_n_outputs;} -private: +protected: const unsigned int _n_outputs; - const std::tuple coordinates; + std::tuple coordinates; }; } diff --git a/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.cpp b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.cpp new file mode 100644 index 0000000000..6a6126aeef --- /dev/null +++ b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.cpp @@ -0,0 +1,366 @@ +// +// Created by matteo on 2/28/20. +// + +#include "AngleToTargetDetector.h" +#include +#include +#include +#include + +revolve::AngleToTargetDetector::AngleToTargetDetector(const unsigned int shrink_factor, const bool show_image) + : Sensor(1), show_image(show_image), shrink_factor(shrink_factor) + // , angle(std::atan(img.cols/img.rows) * 180 / M_PI) + , + angle(NAN) +{ +} + +void revolve::AngleToTargetDetector::read(double *input) +{ + input[0] = detect_angle(); +} + +float revolve::AngleToTargetDetector::detect_angle() +{ + get_image(raw_image); + unsigned int image_cols = raw_image.cols / shrink_factor; + unsigned int image_rows = raw_image.rows / shrink_factor; + cv::resize(raw_image, image, cv::Size(image_cols, image_rows)); + + cv::medianBlur(image, image_blur, 5); + cv::cvtColor(image_blur, image_hsv, cv::COLOR_BGR2HSV); + + //green + const int gLowH1 = 35, gHighH1 = 40, gLowH2 = 41, gHighH2 = 59, gLowS1 = 140, gLowS2 = 69, gHighS = 255, gLowV = 104, gHighV = 255; + //blue + const int bLowH = 99, bHighH = 121, bLowS = 120, bHighS = 255, bLowV = 57, bHighV = 211; + + //detecting Blue + cv::inRange(image_hsv, cv::Scalar(bLowH, bLowS, bLowV), cv::Scalar(bHighH, bHighS, bHighV), image_blue); + //detecting Green + cv::inRange(image_hsv, cv::Scalar(gLowH1, gLowS1, gLowV), cv::Scalar(gHighH1, gHighS, gHighV), image_green1); + cv::inRange(image_hsv, cv::Scalar(gLowH2, gLowS2, gLowV), cv::Scalar(gHighH2, gHighS, gHighV), image_green2); + cv::add(image_green1, image_green2, image_green); + + std::vector> contours_blue, contours_green; + ; //contours_red, contours_yellow; + + cv::findContours(image_blue, contours_blue, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + cv::findContours(image_green, contours_green, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + //cv::findContours(image_red, contours_red, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + //cv::findContours(image_yellow, contours_yellow, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_NONE); + + std::vector rect_coord, rect_coord_blue, rect_coord_green; //rect_coord_red, rect_coord_yellow; + + // blue contours + for (const std::vector &contours_blue_line : contours_blue) + { + double image_blue_area_buf = cv::contourArea(contours_blue_line); + + if (image_blue_area_buf > 5) + { + cv::Rect bounding_rect = cv::boundingRect(contours_blue_line); + rect_coord_blue.emplace_back(bounding_rect); + } + } + + // green contours + for (const std::vector &contours_green_line : contours_green) + { + double image_blue_area_buf = cv::contourArea(contours_green_line); + + if (image_blue_area_buf > 5) + { + cv::Rect bounding_rect = cv::boundingRect(contours_green_line); + rect_coord_green.emplace_back(bounding_rect); + } + } + + //// red contours + //for (const std::vector &contours_red_line : contours_red) { + // double image_blue_area_buf = cv::contourArea(contours_red_line); + // + // if (image_blue_area_buf > 5) { + // cv::Rect bounding_rect = cv::boundingRect(contours_red_line); + // rect_coord_red.emplace_back(bounding_rect); + // } + //} + // + //// yellow contours + //for (const std::vector &contours_yellow_line : contours_yellow) { + // double image_blue_area_buf = cv::contourArea(contours_yellow_line); + // + // if (image_blue_area_buf > 5) { + // cv::Rect bounding_rect = cv::boundingRect(contours_yellow_line); + // rect_coord_yellow.emplace_back(bounding_rect); + // } + //} + + rect_coord.reserve(rect_coord_blue.size() + rect_coord_green.size()); // preallocate memory + // + rect_coord_red.size() + rect_coord_yellow.size() + rect_coord.insert(rect_coord.end(), rect_coord_blue.begin(), rect_coord_blue.end()); + rect_coord.insert(rect_coord.end(), rect_coord_green.begin(), rect_coord_green.end()); + //rect_coord.insert( rect_coord.end(), rect_coord_red.begin(), rect_coord_red.end() ); + //rect_coord.insert( rect_coord.end(), rect_coord_yellow.begin(), rect_coord_yellow.end() ); + + // ----- MAGIC GONGJIN CODE HERE ---------------------------------------------- + unsigned int num = rect_coord.size(); + int distanceBox[num][num], distanceBoxSum[num], numBox[num], minDistanceBox[num], min2DistanceBox[num], rectBoxHeight = 0, rectBoxHeightMax = 0; + for (int i = 0; i < num; i++) //calculating the suitable(medium) value of height + { + if (rect_coord[i].height > rectBoxHeightMax) + { + rectBoxHeight = rectBoxHeightMax; // set this value as the height of box + rectBoxHeightMax = rect_coord[i].height; + } + else if (rect_coord[i].height > rectBoxHeight) + rectBoxHeight = rect_coord[i].height; + } + + for (int j = 0; j < num; j++) //calculating the value of minimum and the second minimum distance for each box + { + minDistanceBox[j] = 800; + min2DistanceBox[j] = 800; + for (int x = 0; x < num; x++) + { + if (j != x) + { + distanceBox[j][x] = std::min( + std::abs(rect_coord[j].tl().x - rect_coord[x].br().x), + std::abs(rect_coord[j].br().x - rect_coord[x].tl().x)); + + if (distanceBox[j][x] < minDistanceBox[j]) + { + min2DistanceBox[j] = minDistanceBox[j]; //the second minimum distance + minDistanceBox[j] = distanceBox[j][x]; //the minimun distance + } + else if (distanceBox[j][x] < min2DistanceBox[j]) + { + min2DistanceBox[j] = distanceBox[j][x]; + } + } + } + distanceBoxSum[j] = minDistanceBox[j] + min2DistanceBox[j]; + } + + for (int i = 0; i < num; i++) //sequence from minimum distance to maximum distance + { + numBox[i] = 0; + for (int j = 0; j < num; j++) + { + if (i != j) // get the Box[i] sequence + { + if (distanceBoxSum[i] > distanceBoxSum[j]) + numBox[i] += 1; //numBox[i] = numBox[i] +1, save the number + if (distanceBoxSum[i] == distanceBoxSum[j]) + { + if (minDistanceBox[i] >= minDistanceBox[j]) //always have the same distance between two points each other + numBox[i] += 1; // + } + } + } + } + //-------------difine the ROIs of robot------------ + int lastnum = num, robNum, minRectCoorX[num], minRectCoorY[num], maxRectCoorX[num], maxRectCoorY[num]; + for (robNum = 0; lastnum >= 2 && robNum < num; robNum++) + { + int minNumBox = 100; + for (int k = 0; k < num; k++) //get the minNumBox between the rest + { + minNumBox = std::min(numBox[k], minNumBox); + } + for (int i = 0; i < num; i++) //get the coordination of rectangle of robot from boxes + { + if (numBox[i] == minNumBox) //find the minimum one between the rest (usually it is 0 when 1 robot) + { + lastnum--; + if (num > 2) //when robot only have 2 boxes at least, just combine the two boxes + numBox[i] = 100; //make it not included in the rest + minRectCoorX[robNum] = rect_coord[i].tl().x; + minRectCoorY[robNum] = rect_coord[i].tl().y; + maxRectCoorX[robNum] = rect_coord[i].br().x; + maxRectCoorY[robNum] = rect_coord[i].br().y; + int bufnum = 0, jBox[50] = {0}; + for (int j = 0; j < num; j++) //calculating the coordination of rectangle incluing boxes belong to the distance area + { + //-------------the first threshold condition------------------- + if (j != i && numBox[j] != 100 && distanceBox[i][j] < 4.3 * rectBoxHeight) //3.4, 3.5, 4.5, 4.3 justify if the box belong to the same robot by distance of boxeswith the center box + { + jBox[bufnum] = j; + lastnum--; + bufnum++; //the number of boxes that match the threshold of (distanceBox[i][j] < 3.4 * rectBoxHeight) + } + //----calculating the max distance between boxes after the first threshold condition, preparing for next-------- + if (j == num - 1 && bufnum >= 1) //bufnum >= 1 (it have two candidate at least) + { + int maxBoxDisOut[num], max_in_out[num][num], maxBoxDisOutNum[num]; + for (int buf = 0; buf < bufnum; buf++) //calculating the max distance between boxes in jBox[bufnum] + { + maxBoxDisOut[jBox[buf]] = 0; + int rectCoor_tl_br, rectCoor_br_tl; + if (bufnum == 1) // one other box and one center box + { + rectCoor_tl_br = std::abs(rect_coord[i].tl().x - rect_coord[jBox[0]].br().x); //calculating the inside or outside distance between the same boxes + rectCoor_br_tl = std::abs(rect_coord[i].br().x - rect_coord[jBox[0]].tl().x); //calculating the inside or outside distance between the same boxes + maxBoxDisOut[jBox[0]] = std::min(rectCoor_tl_br, rectCoor_br_tl); //max, min + } + else + { + for (int buff = 0; buff < bufnum; buff++) + { + rectCoor_tl_br = std::abs(rect_coord[jBox[buf]].tl().x - rect_coord[jBox[buff]].br().x); //calculating the inside or outside distance between the same boxes + rectCoor_br_tl = std::abs(rect_coord[jBox[buf]].br().x - rect_coord[jBox[buff]].tl().x); //calculating the inside or outside distance between the same boxes + max_in_out[jBox[buf]][jBox[buff]] = std::min(rectCoor_tl_br, rectCoor_br_tl); //max,min + if (max_in_out[jBox[buf]][jBox[buff]] > maxBoxDisOut[jBox[buf]]) + { + maxBoxDisOut[jBox[buf]] = max_in_out[jBox[buf]][jBox[buff]]; + maxBoxDisOutNum[buf] = jBox[buff]; + } + } + } + } + //bufnum >1 guarantte the robot have center box and two other box (bufnum=2) at least, or not go to compare center box and another one box + if (bufnum >= 2) + { + int delNum = 0; + for (int bufff = 0; bufff < bufnum; bufff++) //compare the max distance (robot size from left to right) of boxes in jBox[bufnum] + { + if (maxBoxDisOut[jBox[bufff]] < 6.2 * rectBoxHeight) //if > the length of robot, delete far one, get the near one as rectangle + { + minRectCoorX[robNum] = std::min(rect_coord[jBox[bufff]].tl().x, minRectCoorX[robNum]); + minRectCoorY[robNum] = std::min(rect_coord[jBox[bufff]].tl().y, minRectCoorY[robNum]); + maxRectCoorX[robNum] = std::max(rect_coord[jBox[bufff]].br().x, maxRectCoorX[robNum]); + maxRectCoorY[robNum] = std::max(rect_coord[jBox[bufff]].br().y, maxRectCoorY[robNum]); + numBox[jBox[bufff]] = 100; //set a constant not zero and more than all of the numBox + } + //TODO this else if is doing exactly the same code as above, remove it + else if (distanceBox[i][jBox[bufff]] < distanceBox[i][maxBoxDisOutNum[bufff]]) //always have two boxes match this condition at the same time, choice one of them + { + minRectCoorX[robNum] = std::min(rect_coord[jBox[bufff]].tl().x, minRectCoorX[robNum]); + minRectCoorY[robNum] = std::min(rect_coord[jBox[bufff]].tl().y, minRectCoorY[robNum]); + maxRectCoorX[robNum] = std::max(rect_coord[jBox[bufff]].br().x, maxRectCoorX[robNum]); + maxRectCoorY[robNum] = std::max(rect_coord[jBox[bufff]].br().y, maxRectCoorY[robNum]); + numBox[jBox[bufff]] = 100; //set a constant not zero and more than all of the numBox + } + else + { + minRectCoorX[robNum] = std::min(rect_coord[maxBoxDisOutNum[bufff]].tl().x, minRectCoorX[robNum]); + minRectCoorY[robNum] = std::min(rect_coord[maxBoxDisOutNum[bufff]].tl().y, minRectCoorY[robNum]); + maxRectCoorX[robNum] = std::max(rect_coord[maxBoxDisOutNum[bufff]].br().x, maxRectCoorX[robNum]); + maxRectCoorY[robNum] = std::max(rect_coord[maxBoxDisOutNum[bufff]].br().y, maxRectCoorY[robNum]); + numBox[maxBoxDisOutNum[bufff]] = 100; + delNum++; + } + } + lastnum = lastnum + delNum; //plus for the cancelled more one + bufnum = bufnum - delNum; + } + else //compare center box and another one box, when bufnum = 1 + { + if (maxBoxDisOut[jBox[0]] < 6.2 * rectBoxHeight) //the length of robot 9.4 + { + minRectCoorX[robNum] = std::min(rect_coord[jBox[0]].tl().x, minRectCoorX[robNum]); + minRectCoorY[robNum] = std::min(rect_coord[jBox[0]].tl().y, minRectCoorY[robNum]); + maxRectCoorX[robNum] = std::max(rect_coord[jBox[0]].br().x, maxRectCoorX[robNum]); + maxRectCoorY[robNum] = std::max(rect_coord[jBox[0]].br().y, maxRectCoorY[robNum]); + numBox[jBox[0]] = 100; //set a constant not zero and more than all of the numBox + } + else //just one center to rest + { + robNum--; + } + } + } + } + } + } + } + + // calculate the angle + if (std::isnan(angle) and robNum == 0) + { + // init first angle + angle = atan(image.cols / static_cast(image.rows)) * 180.0 / M_PI; + } + else + { + for (int i = 0; i < robNum; i++) + { + const int robCenterCoorX = 2 * (minRectCoorX[i] + maxRectCoorX[i]); + const int robCenterCoorY = 2 * (minRectCoorY[i] + maxRectCoorY[i]); + char textRobCenterCoor[64], textDistance[64]; + + if (show_image) + { + cv::rectangle(raw_image, cv::Point(shrink_factor * minRectCoorX[i], shrink_factor * minRectCoorY[i]), cv::Point(shrink_factor * maxRectCoorX[i], shrink_factor * maxRectCoorY[i]), cv::Scalar(0, 255, 0), 1); + cv::circle(raw_image, cv::Point(robCenterCoorX, robCenterCoorY), 3, cv::Scalar(0, 255, 0), 4); + + std::snprintf(textRobCenterCoor, sizeof(textRobCenterCoor), "(%d,%d)", robCenterCoorX, robCenterCoorY); + cv::putText(raw_image, textRobCenterCoor, cv::Point(robCenterCoorX + 10, robCenterCoorY + 3), + cv::FONT_HERSHEY_DUPLEX, 0.4, cv::Scalar(0, 255, 0), 1); + } + + const int leftLine = raw_image.cols / 2; + const int rightLine = raw_image.cols / 2; + if (robCenterCoorX < leftLine) + { + double distance = robCenterCoorX - leftLine; + angle = std::atan(distance / robCenterCoorY) * 180.0 / M_PI; + if (show_image) + { + std::snprintf(textDistance, sizeof(textDistance), "L:%f Angle: %f", distance, angle); + cv::putText(raw_image, textDistance, cv::Point(0.0 * raw_image.cols, 15), cv::FONT_HERSHEY_DUPLEX, 0.5, + cv::Scalar(0, 255, 0), 1); + } + } + + if (robCenterCoorX > rightLine) + { + double distance = robCenterCoorX - rightLine; + // TODO do not convert to degrees + angle = std::atan(distance / robCenterCoorY) * 180.0 / M_PI; + if (show_image) + { + std::snprintf(textDistance, sizeof(textDistance), "R:%f Angle: %f", distance, angle); + cv::putText(raw_image, textDistance, cv::Point(0.5 * raw_image.cols, 15), cv::FONT_HERSHEY_DUPLEX, 0.5, + cv::Scalar(0, 255, 0), 1); + } + } + + if (show_image) + { + cv::line(raw_image, cv::Point(shrink_factor * minRectCoorX[i], shrink_factor * minRectCoorY[i]), + cv::Point(shrink_factor * maxRectCoorX[i], shrink_factor * maxRectCoorY[i]), cv::Scalar(0, 255, 0), 1); + cv::line(raw_image, cv::Point(shrink_factor * minRectCoorX[i], shrink_factor * maxRectCoorY[i]), + cv::Point(shrink_factor * maxRectCoorX[i], shrink_factor * minRectCoorY[i]), cv::Scalar(0, 255, 0), 1); + cv::line(raw_image, cv::Point(leftLine, 0), cv::Point(leftLine, raw_image.rows), cv::Scalar(0, 255, 0), 1); + cv::line(raw_image, cv::Point(rightLine, 0), cv::Point(rightLine, raw_image.rows), cv::Scalar(0, 255, 0), 1); + } + } + } + + if (robNum == 0 and show_image) // no robots in the field of view + { + // show image if no robot is detected + char textDistance[64]; + float text_pos; + if (angle < 0) + text_pos = 0.0; + else + text_pos = 0.5; + std::snprintf(textDistance, sizeof(textDistance), "Angle: %f", angle); + std::snprintf(textDistance, sizeof(textDistance), "Angle: %f", angle); + cv::putText(raw_image, textDistance, cv::Point(text_pos * raw_image.cols, 15), cv::FONT_HERSHEY_DUPLEX, 0.5, + cv::Scalar(255, 0, 0), 1); + } + + assert(not std::isnan(angle)); + + if (show_image) + { + cv::imshow("revolve-controller", raw_image); + cv::waitKey(5); + } + return angle; +} diff --git a/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.h b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.h new file mode 100644 index 0000000000..7536901f5d --- /dev/null +++ b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetDetector.h @@ -0,0 +1,35 @@ +// +// Created by matteo on 2/28/20. +// + +#ifndef REVOLVE_ANGLETOTARGETDETECTOR_H +#define REVOLVE_ANGLETOTARGETDETECTOR_H + +#include "Sensor.h" +#include + +namespace revolve { + +class AngleToTargetDetector : public Sensor { +public: + explicit AngleToTargetDetector(unsigned int shrink_factor = 4, bool show_image = false); + virtual ~AngleToTargetDetector() = default; + + void read(double *input) override; + virtual float detect_angle(); + +protected: + virtual void get_image(cv::Mat &image) = 0; + +protected: + const bool show_image; + const unsigned int shrink_factor; + double angle; + cv::Mat raw_image, image; + cv::Mat image_blur, image_hsv, image_blue, image_green1, image_green2, image_green; +}; + +} + + +#endif //REVOLVE_ANGLETOTARGETDETECTOR_H diff --git a/cpprevolve/revolve/brains/controller/sensors/AngleToTargetSensor.h b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetSensor.h new file mode 100644 index 0000000000..5059c6eac0 --- /dev/null +++ b/cpprevolve/revolve/brains/controller/sensors/AngleToTargetSensor.h @@ -0,0 +1,29 @@ +// +// Created by Matteo De Carlo on 25/02/2020. +// + +#ifndef REVOLVE_ANGLETOTARGETSENSOR_H +#define REVOLVE_ANGLETOTARGETSENSOR_H + +#include "Sensor.h" + +namespace revolve +{ + +class AngleToTargetSensor : public Sensor { +public: + explicit AngleToTargetSensor() + : Sensor(1) + {} + + virtual double angle_to_target() = 0; + + void read(double *input) override + { + *input = angle_to_target(); + } +}; + +} + +#endif //REVOLVE_ANGLETOTARGETSENSOR_H diff --git a/cpprevolve/revolve/brains/controller/sensors/Sensor.h b/cpprevolve/revolve/brains/controller/sensors/Sensor.h index 496e54bf5f..f2678ac3e1 100644 --- a/cpprevolve/revolve/brains/controller/sensors/Sensor.h +++ b/cpprevolve/revolve/brains/controller/sensors/Sensor.h @@ -14,9 +14,16 @@ class Sensor : _n_inputs(n_inputs) {} + virtual ~Sensor() = default; + /// \brief Read the value of the sensor into the /// \param[in] _input: array. /// \brief[in,out] _input Input value to write on + /// + /// Reads the current value of this sensor into the given + /// network output array. This should fill the number of input neurons + /// the sensor specifies to have, i.e. if the sensor specifies 2 input + /// neurons it should fill `input[0]` and `input[1]` virtual void read(double *input) = 0; inline unsigned int n_inputs() const {return this->_n_inputs;} diff --git a/cpprevolve/revolve/gazebo/CMakeLists.txt b/cpprevolve/revolve/gazebo/CMakeLists.txt index 944178757e..d0e3983762 100644 --- a/cpprevolve/revolve/gazebo/CMakeLists.txt +++ b/cpprevolve/revolve/gazebo/CMakeLists.txt @@ -18,7 +18,8 @@ add_definitions(-pedantic -Wno-long-long -Wall -Wextra -Wformat=2 -Wswitch-enum -Wuninitialized -Wswitch-default -Winit-self -Wfloat-equal -fPIC ) -set (CMAKE_CXX_STANDARD 11) +# need c++17 for libSimpleAmqpClient >= 7.something +set (CMAKE_CXX_STANDARD 17) # Debug Flags set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -O0 -ggdb3 -DDEBUG") @@ -45,7 +46,7 @@ endif () find_package(PkgConfig REQUIRED) # Find Boost -find_package(Boost REQUIRED COMPONENTS system) +find_package(Boost REQUIRED COMPONENTS system filesystem) include_directories(${Boost_INCLUDE_DIRS}) # Find Eigen3 - A lightweight C++ template library for vector and matrix math @@ -112,6 +113,13 @@ include_directories( ${GAZEBO_PROTO_INCLUDE} ) +# Find OpenCV +find_package( OpenCV REQUIRED ) + +#pkg_check_modules(JSONCPP REQUIRED jsoncpp>1.7) +#pkg_check_modules(AMQP REQUIRED libSimpleAmqpClient>2.6) +pkg_check_modules(PQXX REQUIRED libpqxx>6.4) + # Add Gazebo C++ flags (this includes c++11) list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") @@ -200,6 +208,8 @@ add_library( target_link_libraries( revolve-gazebo + revolve-controllers + revolve-learners ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${GSL_LIBRARIES} @@ -238,16 +248,6 @@ target_link_libraries( ${GAZEBO_LIBRARIES} ) -# Create Torus World plugin -add_library(TorusWorldPlugin SHARED - plugin/TorusWorld.cpp - plugin/register_torus_world_plugin.cpp -) -target_link_libraries(TorusWorldPlugin - revolve-gazebo - ${GAZEBO_LIBRARIES} -) - # Create Robot plugin add_library( RobotControlPlugin SHARED diff --git a/cpprevolve/revolve/gazebo/Types.h b/cpprevolve/revolve/gazebo/Types.h index d8b9944e7c..b5b8127e96 100644 --- a/cpprevolve/revolve/gazebo/Types.h +++ b/cpprevolve/revolve/gazebo/Types.h @@ -22,6 +22,9 @@ #include #include +#include +#include + namespace revolve { @@ -41,9 +44,9 @@ namespace revolve typedef std::shared_ptr< Brain > BrainPtr; - typedef std::shared_ptr< Motor > MotorPtr; + typedef std::shared_ptr< revolve::Actuator > MotorPtr; - typedef std::shared_ptr< VirtualSensor > SensorPtr; + typedef std::shared_ptr< revolve::Sensor > SensorPtr; typedef std::shared_ptr< MotorFactory > MotorFactoryPtr; diff --git a/cpprevolve/revolve/gazebo/brains/Brains.h b/cpprevolve/revolve/gazebo/brains/Brains.h index 0ef202b4df..c2bec687a1 100644 --- a/cpprevolve/revolve/gazebo/brains/Brains.h +++ b/cpprevolve/revolve/gazebo/brains/Brains.h @@ -25,5 +25,7 @@ #include #include #include +#include +#include #endif // REVOLVE_GAZEBO_BRAINS_BRAINS_H_ diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp index 88d4a69bc1..7d5e069af4 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp @@ -17,1174 +17,67 @@ * Author: Milan Jelisavcic & Maarten van Hooft * Date: December 29, 2018 * + * Cleaned up by andi on 06-10-19. + * */ -// STL macros -#include -#include -#include -#include -#include -#include -#include -#include - -// Other libraries -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// Project headers -#include "../motors/Motor.h" - -#include "../sensors/Sensor.h" - #include "DifferentialCPG.h" -#include "DifferentialCPG_BO.h" - -// TODO: Resolve odd behaviour at the end of the validation procedure -// This behaviour is not present if you directly load a trained controller - -// Define namespaces namespace gz = gazebo; using namespace revolve::gazebo; -// Copied from the limbo tutorial the BO implementation is based on -using Mean_t = limbo::mean::Data; -using Init_t = limbo::init::LHS; -using Kernel_t = limbo::kernel::MaternFiveHalves; -using GP_t = limbo::model::GP; - -/** - * Constructor for DifferentialCPG class. - * - * @param _model - * @param robot_config - */ -DifferentialCPG::DifferentialCPG( - const ::gazebo::physics::ModelPtr &_model, - const sdf::ElementPtr robot_config, - const std::vector< revolve::gazebo::MotorPtr > &_motors, - const std::vector< revolve::gazebo::SensorPtr > &_sensors) - : next_state(nullptr) - , input(new double[_sensors.size()]) - , output(new double[_motors.size()]) +DifferentialCPG::DifferentialCPG(const sdf::ElementPtr brain_sdf, + const std::vector &_motors, + std::shared_ptr<::revolve::AngleToTargetDetector> angle_to_target_sensor) + : revolve::DifferentialCPG(load_params_from_sdf(brain_sdf), _motors, std::move(angle_to_target_sensor)) { - - this->learner = robot_config->GetElement("rv:brain")->GetElement("rv:learner"); - - // Check for brain - if (not robot_config->HasElement("rv:brain")) - { - throw std::runtime_error("DifferentialCPG brain did not receive brain"); - } - auto brain = robot_config->GetElement("rv:brain"); - - // Check for learner - if (not brain->HasElement("rv:learner")) - { - throw std::runtime_error("DifferentialCPG brain did not receive learner"); - } - auto learner = brain->GetElement("rv:learner"); - - // Check for controller - if (not brain->HasElement("rv:controller")) - { - throw std::runtime_error("DifferentialCPG brain did not receive controller"); - } - auto controller = brain->GetElement("rv:controller"); - - // Check for actuators - if (not brain->HasElement("rv:actuators")) - { - throw std::runtime_error("DifferentialCPG brain did not receive actuators"); - } - auto actuators = brain->GetElement("rv:actuators"); - - // Controller parameters - this->reset_neuron_state_bool = std::stoi(controller->GetAttribute("reset_neuron_state_bool")->GetAsString()); - this->reset_neuron_random = std::stoi(controller->GetAttribute("reset_neuron_random")->GetAsString()); - this->init_neuron_state = std::stod(controller->GetAttribute("init_neuron_state")->GetAsString()); - this->range_lb = -std::stod(controller->GetAttribute("range_ub")->GetAsString()); - this->range_ub = std::stod(controller->GetAttribute("range_ub")->GetAsString()); - this->use_frame_of_reference = std::stoi(controller->GetAttribute("use_frame_of_reference")->GetAsString()); - this->signal_factor_all_ = std::stod(controller->GetAttribute("signal_factor_all")->GetAsString()); - this->signal_factor_mid = std::stod(controller->GetAttribute("signal_factor_mid")->GetAsString()); - this->signal_factor_left_right = std::stod(controller->GetAttribute("signal_factor_left_right")->GetAsString()); - - // Limbo BO Learner parameters - this->kernel_noise_ = std::stod(learner->GetAttribute("kernel_noise")->GetAsString()); - this->kernel_optimize_noise_ = std::stoi(learner->GetAttribute("kernel_optimize_noise")->GetAsString()); - this->kernel_sigma_sq_ = std::stod(learner->GetAttribute("kernel_sigma_sq")->GetAsString()); - this->kernel_l_ = std::stod(learner->GetAttribute("kernel_l")->GetAsString()); - this->kernel_squared_exp_ard_k_ = std::stoi(learner->GetAttribute("kernel_squared_exp_ard_k")->GetAsString()); - this->acqui_gpucb_delta_ = std::stod(learner->GetAttribute("acqui_gpucb_delta")->GetAsString()); - this->acqui_ucb_alpha_ = std::stod(learner->GetAttribute("acqui_ucb_alpha")->GetAsString()); - this->acqui_ei_jitter_ = std::stod(learner->GetAttribute("acqui_ei_jitter")->GetAsString()); - - // Non-limbo BO learner para - this->n_init_samples = std::stoi(learner->GetAttribute("n_init_samples")->GetAsString()); - this->n_learning_iterations = std::stoi(learner->GetAttribute("n_learning_iterations")->GetAsString()); - this->n_cooldown_iterations = std::stoi(learner->GetAttribute("n_cooldown_iterations")->GetAsString()); - this->init_method = learner->GetAttribute("init_method")->GetAsString(); - - // Meta parameters - this->startup_time = std::stoi(controller->GetAttribute("startup_time")->GetAsString()); - this->reset_robot_position = std::stoi(controller->GetAttribute("reset_robot_position")->GetAsString()); - this->run_analytics = std::stoi(controller->GetAttribute("run_analytics")->GetAsString()); - this->load_brain = controller->GetAttribute("load_brain")->GetAsString(); - this->evaluation_rate = std::stoi(learner->GetAttribute("evaluation_rate")->GetAsString()); - this->abs_output_bound = std::stoi(learner->GetAttribute("abs_output_bound")->GetAsString()); - this->verbose = std::stoi(controller->GetAttribute("verbose")->GetAsString()); - - // Create transport node - this->node_.reset(new gz::transport::Node()); - this->node_->Init(); - - // Get Robot - this->robot = _model; - this->n_motors = _motors.size(); - auto name = _model->GetName(); - - if(this->verbose) - { - std::cout << robot_config->GetDescription() << std::endl; - } - auto motor = actuators->HasElement("rv:servomotor") - ? actuators->GetElement("rv:servomotor") - : sdf::ElementPtr(); - auto j = 0; - while(motor) - { - if (not motor->HasAttribute("coordinates")) - { - std::cerr << "Missing required motor coordinates" << std::endl; - throw std::runtime_error("Robot brain error"); - } - - // Split string and get coordinates - auto coordinate_string = motor->GetAttribute("coordinates")->GetAsString(); - std::vector coordinates; - boost::split(coordinates, coordinate_string, boost::is_any_of(";")); - - // Check if we have exactly 2 coordinates - if (not coordinates.size() == 2) - { - throw std::runtime_error("Coordinates are not exactly of length two "); - } - - // Check if the coordinates are integers - try - { - for(auto coord : coordinates) - { - std::stoi(coord); - } - } - catch(std::invalid_argument e1) - { - std::cout << "Invalid argument: Cannot cast coordinates to integers " << std::endl; - }; - - // Pass coordinates - auto coord_x = std::stoi(coordinates[0]); - auto coord_y = std::stoi(coordinates[1]); - if (this->verbose) - { - std::cout << "coord_x,coord_y = " << coord_x << "," << coord_y << std::endl; - } - auto motor_id = motor->GetAttribute("part_id")->GetAsString(); - this->positions[motor_id] = {coord_x, coord_y}; - this->motor_coordinates[{coord_x, coord_y}] = j; - - // Set frame of reference - int frame_of_reference = 0; - // We are a left neuron - if (coord_x < 0) - { - frame_of_reference = -1; - } - // We are a right neuron - else if (coord_x > 0) - { - frame_of_reference = 1; - } - - // Save neurons: bias/gain/state. Make sure initial states are of different sign. - this->neurons[{coord_x, coord_y, 1}] = {0.f, 0.f, this->init_neuron_state, frame_of_reference}; //Neuron A - this->neurons[{coord_x, coord_y, -1}] = {0.f, 0.f, -this->init_neuron_state, frame_of_reference}; // Neuron B - - // TODO: Add check for duplicate coordinates - motor = motor->GetNextElement("rv:servomotor"); - j++; - } - - // Add connections between neighbouring neurons - int i = 0; - for (const auto &position : this->positions) - { - // Get name and x,y-coordinates of all neurons. - auto name = position.first; - int x, y; std::tie(x, y) = position.second; - - // Continue to next iteration in case there is already a connection between the 1 and -1 neuron. - // These checks feel a bit redundant. - // if A->B connection exists. - if (this->connections.count({x, y, 1, x, y, -1})) - { - continue; - } - // if B->A connection exists: - if (this->connections.count({x, y, -1, x, y, 1})) - { - continue; - } - - // Loop over all positions. We call it neighbours, but we still need to check if they are a neighbour. - for (const auto &neighbour : this->positions) - { - // Get information of this neuron (that we call neighbour). - int near_x, near_y; std::tie(near_x, near_y) = neighbour.second; - - // If there is a node that is a Moore neighbour, we set it to be a neighbour for their A-nodes. - // Thus the connections list only contains connections to the A-neighbourhood, and not the - // A->B and B->A for some node (which makes sense). - int dist_x = std::abs(x - near_x); - int dist_y = std::abs(y - near_y); - - // TODO: Verify for non-spiders - if (dist_x + dist_y == 2) - { - if(std::get<0>(this->connections[{x, y, 1, near_x, near_y, 1}]) != 1 or - std::get<0>(this->connections[{near_x, near_y, 1, x, y, 1}]) != 1) - { - if(this->verbose) - { - std::cout << "New connection at index " << i << ": " << x << ", " << y << ", " << near_x << ", " << near_y << std::endl; - } - this->connections[{x, y, 1, near_x, near_y, 1}] = std::make_tuple(1, i); - this->connections[{near_x, near_y, 1, x, y, 1}] = std::make_tuple(1, i); - i++; - } - } - } - } - - // Create directory for output. - this->directory_name = controller->GetAttribute("output_directory")->GetAsString(); - if(this->directory_name.empty()) - { - this->directory_name = "output/cpg_bo/"; - this->directory_name += std::to_string(time(0)) + "/"; - } - - std::system(("mkdir -p " + this->directory_name).c_str()); - - // Initialise array of neuron states for Update() method - this->next_state = new double[this->neurons.size()]; - this->n_weights = (int)(this->connections.size()/2) + this->n_motors; - - // Check if we want to load a pre-trained brain - if(!this->load_brain.empty()) - { - // Get line - if(this->verbose) - { - std::cout << "I will load the following brain:" << std::endl; - } - std::ifstream brain_file(this->load_brain); - std::string line; - std::getline(brain_file, line); - - // Get weights in line - std::vector weights; - boost::split(weights, line, boost::is_any_of(",")); - - // Save weights for brain - Eigen::VectorXd loaded_brain(this->n_weights); - for(size_t j = 0; j < this->n_weights; j++) - { - loaded_brain(j) = std::stod(weights.at(j)); - if(this->verbose) - { - std::cout << loaded_brain(j) << ","; - } - } - if(this->verbose) - { - std::cout << std::endl; - } - - // Close brain - brain_file.close(); - - // Save these weights - this->samples.push_back(loaded_brain); - - // Set ODE matrix at initialization - this->set_ode_matrix(); - - // Go directly into cooldown phase: Note we do require that best_sample is filled. Check this - this->current_iteration = this->n_init_samples + this->n_learning_iterations; - - if(this->verbose) - { - std::cout << std::endl << "Brain has been loaded." << std::endl; - } - } - else - { - if (this->verbose) - { - std::cout << "Don't load existing brain" << std::endl; - } - - // Initialize BO - this->bo_init_sampling(); - } - - // Initiate the cpp Evaluator - this->evaluator.reset(new Evaluator(this->evaluation_rate)); - this->evaluator->directory_name = this->directory_name; } -/** - * Destructor - */ -DifferentialCPG::~DifferentialCPG() +DifferentialCPG::DifferentialCPG(const sdf::ElementPtr brain_sdf, + const std::vector &_motors, + const NEAT::Genome &genome, + std::shared_ptr<::revolve::AngleToTargetDetector> angle_to_target_sensor) + : revolve::DifferentialCPG(load_params_from_sdf(brain_sdf), _motors, genome, std::move(angle_to_target_sensor)) { - delete[] this->next_state; - delete[] this->input; - delete[] this->output; } -/** - * Dummy function for limbo - */ -struct DifferentialCPG::evaluation_function{ - // Number of input dimension (samples.size()) - BO_PARAM(size_t, dim_in, 18); - - // number of dimensions of the fitness - BO_PARAM(size_t, dim_out, 1); - - Eigen::VectorXd operator()(const Eigen::VectorXd &x) const { - return limbo::tools::make_vector(0); - }; -}; - -/** - * Performs the initial random sampling for BO - */ -void DifferentialCPG::bo_init_sampling(){ - if(this->verbose) - { - // We only want to optimize the weights for now. - std::cout << "Number of weights = connections/2 + n_motors are " - << this->connections.size()/2 - << " + " - << this->n_motors - << std::endl; - - // Information purposes - std::cout << std::endl << "Sample method: " << this->init_method << ". Initial " - "samples are: " << std::endl; - } - - // Random sampling - if(this->init_method == "RS") - { - for (size_t i = 0; i < this->n_init_samples; i++) - { - // Working variable to hold a random number for each weight to be optimized - Eigen::VectorXd init_sample(this->n_weights); - - // For all weights - for (size_t j = 0; j < this->n_weights; j++) - { - // Generate a random number in [0, 1]. Transform later - double f = ((double) rand() / (RAND_MAX)); - - // Append f to vector - init_sample(j) = f; - } - - // Save vector in samples. - this->samples.push_back(init_sample); - } - } - // Latin Hypercube Sampling - else if(this->init_method == "LHS") - { - // Working variable - double my_range = 1.f/this->n_init_samples; - - // If we have n dimensions, create n such vectors that we will permute - std::vector> all_dimensions; - - // Fill vectors - for (size_t i=0; i < this->n_weights; i++) - { - std::vector one_dimension; - - // Prepare for vector permutation - for (size_t j = 0; j < this->n_init_samples; j++) - { - one_dimension.push_back(j); - } - - // Vector permutation - std::random_shuffle(one_dimension.begin(), one_dimension.end() ); - - // Save permuted vector - all_dimensions.push_back(one_dimension); - } - - // For all samples - for (size_t i = 0; i < this->n_init_samples; i++) - { - // Initialize Eigen::VectorXd here. - Eigen::VectorXd init_sample(this->n_weights); - - // For all dimensions - for (size_t j = 0; j < this->n_weights; j++) - { - // Take a LHS - init_sample(j) = all_dimensions.at(j).at(i)*my_range + ((double) rand() / (RAND_MAX))*my_range; - } - - // Append sample to samples - this->samples.push_back(init_sample); - } - } - else - { - std::cout << "Please provide a choice of init_method in {LHS, RS}" << std::endl; - } - - // Print samples - if(this->verbose) - { - for(auto init_sample :this->samples) - { - for (int h = 0; h < init_sample.size(); h++) - { - std::cout << init_sample(h) << ", "; - } - std::cout << std::endl; - } - } -} - -/** - * Function that obtains the current fitness by calling the evaluator and stores it - */ -void DifferentialCPG::save_fitness(){ - // Get fitness - double fitness = this->evaluator->Fitness(); - - // Save sample if it is the best seen so far - if(fitness >this->best_fitness) - { - this->best_fitness = fitness; - this->best_sample = this->samples.back(); - } - - if (this->verbose) - { - std::cout << "Iteration number " << this->current_iteration << " has fitness " << - fitness << ". Best fitness: " << this->best_fitness << std::endl; - } - - // Limbo requires fitness value to be of type Eigen::VectorXd - Eigen::VectorXd observation = Eigen::VectorXd(1); - observation(0) = fitness; - - // Save fitness to std::vector. This fitness corresponds to the solution of the previous iteration - this->observations.push_back(observation); - - // Write fitness to file - std::ofstream fitness_file; - fitness_file.open(this->directory_name + "fitnesses.txt", std::ios::app); - fitness_file << fitness << std::endl; - fitness_file.close(); -} - - - -/** - * Struct that holds the parameters on which BO is called. This is required - * by limbo. - */ -struct DifferentialCPG::Params{ - - struct bayes_opt_boptimizer : public limbo::defaults::bayes_opt_boptimizer { - }; - - // depending on which internal optimizer we use, we need to import different parameters -#ifdef USE_NLOPT - struct opt_nloptnograd : public limbo::defaults::opt_nloptnograd { - }; -#elif defined(USE_LIBCMAES) - struct opt_cmaes : public lm::defaults::opt_cmaes { - }; -#else -#error(NO SOLVER IS DEFINED) -#endif - struct kernel : public limbo::defaults::kernel { - BO_PARAM(double, noise, 0.001); - BO_PARAM(bool, optimize_noise, false); - }; - - struct bayes_opt_bobase : public limbo::defaults::bayes_opt_bobase { - // set stats_enabled to prevent creating all the directories - BO_PARAM(bool, stats_enabled, false); - BO_PARAM(bool, bounded, true); - }; - - // 1 Iteration as we will perform limbo step by steop - struct stop_maxiterations : public limbo::defaults::stop_maxiterations { - BO_PARAM(int, iterations, 1); - }; - - struct kernel_exp : public limbo::defaults::kernel_exp { - /// @ingroup kernel_defaults - BO_PARAM(double, sigma_sq, 0.1); - BO_PARAM(double, l, 0.1); // the width of the kernel. Note that it assumes equally sized ranges over dimensions - }; - - struct kernel_squared_exp_ard : public limbo::defaults::kernel_squared_exp_ard { - /// @ingroup kernel_defaults - BO_PARAM(int, k, 3); // k number of columns used to compute M - /// @ingroup kernel_defaults - BO_PARAM(double, sigma_sq, 0.1); //brochu2010tutorial p.9 without sigma_sq - }; - - struct kernel_maternfivehalves : public limbo::defaults::kernel_maternfivehalves - { - BO_DYN_PARAM(double, sigma_sq); //brochu2010tutorial p.9 without sigma_sq - BO_DYN_PARAM(double, l); //characteristic length scale - }; - - struct acqui_gpucb : public limbo::defaults::acqui_gpucb { - //UCB(x) = \mu(x) + \kappa \sigma(x). - BO_PARAM(double, delta, 0.1 );//acqui_gpucb_delta_); // default delta = 0.1, delta in (0,1) convergence guaranteed - }; - - struct acqui_ei : public limbo::defaults::acqui_ei{ - BO_PARAM(double, jitter, 0.5); - }; - - // This is just a placeholder to be able to use limbo with revolve - struct init_lhs : public limbo::defaults::init_lhs{ - BO_PARAM(int, samples, 0); - }; - - struct acqui_ucb : public limbo::defaults::acqui_ucb { - //constexpr double ra = acqui_ucb_alpha_; - //UCB(x) = \mu(x) + \alpha \sigma(x). high alpha have high exploration - //iterations is high, alpha can be low for high accuracy in enough iterations. - // In contrast, the lsow iterations should have high alpha for high - // searching in limited iterations, which guarantee to optimal. - // BO_PARAM(double, alpha, transform_double(acqui_ucb_alpha_)); // default alpha = 0.5 - BO_DYN_PARAM(double, alpha); // default alpha = 0.5 - - }; -}; - -BO_DECLARE_DYN_PARAM(double, DifferentialCPG::Params::acqui_ucb, alpha); -BO_DECLARE_DYN_PARAM(double, DifferentialCPG::Params::kernel_maternfivehalves, sigma_sq); -BO_DECLARE_DYN_PARAM(double, DifferentialCPG::Params::kernel_maternfivehalves, l); - -/** - * Wrapper function that makes calls to limbo to solve the current BO - * iteration and returns the best sample - */ -void DifferentialCPG::bo_step(){ - Params::acqui_ucb::set_alpha(this->acqui_ucb_alpha_); - Params::kernel_maternfivehalves::set_l(this->kernel_l_); - Params::kernel_maternfivehalves::set_sigma_sq(this->kernel_sigma_sq_); - - // Save all parameters once - if (this->current_iteration == 0) - { - // Save parameters - this->save_parameters(); - } - Eigen::VectorXd x; - - // In case we are done with the initial random sampling. - if (this->current_iteration >= this->n_init_samples) - { - // std::cout << "Acquisition function: " << this->acquisition_function << std::endl; - if(true) - { - - // Specify bayesian optimizer. TODO: Make attribute and initialize at bo_init - limbo::bayes_opt::BOptimizer, - limbo::modelfun, - limbo::acquifun>> boptimizer; - - // Optimize. Pass dummy evaluation function and observations . - boptimizer.optimize(DifferentialCPG::evaluation_function(), - this->samples, - this->observations); - x = boptimizer.last_sample(); - - // Write parametesr to verify thread-stability after the run - std::ofstream dyn_parameters_file; - dyn_parameters_file.open(this->directory_name + "dynamic_parameters.txt", std::ios::app); - dyn_parameters_file << Params::acqui_ucb::alpha() << ","; - dyn_parameters_file << Params::kernel_maternfivehalves::sigma_sq() << ","; - dyn_parameters_file << Params::kernel_maternfivehalves::l() << std::endl; - dyn_parameters_file.close(); - - - } - // else if(this->acquisition_function == "GP_UCB") - // { - // // Specify bayesian optimizer. TODO: Make attribute and initialize at bo_init - // limbo::bayes_opt::BOptimizer, - // limbo::modelfun, - // limbo::acquifun>> boptimizer; - // - // // Optimize. Pass dummy evaluation function and observations . - // boptimizer.optimize(DifferentialCPG::evaluation_function(), - // this->samples, - // this->observations); - // x = boptimizer.last_sample(); - // } - // else if(this->acquisition_function == "EI") - // { - // // Specify bayesian optimizer. TODO: Make attribute and initialize at bo_init - // limbo::bayes_opt::BOptimizer, - // limbo::modelfun, - // limbo::acquifun>> boptimizer; - // - // // Optimize. Pass dummy evaluation function and observations . - // boptimizer.optimize(DifferentialCPG::evaluation_function(), - // this->samples, - // this->observations); - // x = boptimizer.last_sample(); - // } - else - { - std::cout << "Specify correct acquisition function: {EI, UCB, GP_UCB}" << std::endl; - } - - // Save this x_hat_star - this->samples.push_back(x); - } -} - -/** - * Callback function that defines the movement of the robot - * - * @param _motors - * @param _sensors - * @param _time - * @param _step - */ -void DifferentialCPG::Update( - const std::vector< revolve::gazebo::MotorPtr > &_motors, - const std::vector< revolve::gazebo::SensorPtr > &_sensors, - const double _time, - const double _step) -{ - // Prevent two threads from accessing the same resource at the same time - boost::mutex::scoped_lock lock(this->networkMutex_); - - // Read sensor data and feed the neural network - unsigned int p = 0; - for (const auto &sensor : _sensors) - { - sensor->Read(this->input + p); - p += sensor->Inputs(); - } - - this->evaluator->Update(this->robot->WorldPose(), _time, _step); - - // Only start recording the fitness after the startup time each iteration - double elapsed_evaluation_time = _time - this->start_time; - if((std::fmod(elapsed_evaluation_time, (int)this->evaluation_rate) >= this->startup_time) & this->start_fitness_recording) - { - // Update position -// this->evaluator->Update(this->robot->WorldPose(), _time, _step); - this->start_fitness_recording = false; - } - // Evaluate policy on certain time limit, or if we just started - if ((elapsed_evaluation_time > this->evaluation_rate) or ((_time - _step) < 0.001)) - { - // Update position -// this->evaluator->Update(this->robot->WorldPose(), _time, _step); - this->start_fitness_recording = true; - - // Get and save fitness (but not at start) - if(not (_time - _step < 0.001 )) - { - this->save_fitness(); - } - - // Reset robot if opted to do - if(this->reset_robot_position) - { - //this->robot->Reset(); - this->robot->ResetPhysicsStates(); - auto start_pose = ::ignition::math::Pose3d(); - start_pose.Set(0.0, 0.0, 0.05, 0.0, 0.0, 0.0); - this->robot->SetWorldPose(start_pose); - this->robot->Update(); - } - - // Reset neuron state if opted to do - if(this->reset_neuron_state_bool) - { - this->reset_neuron_state(); - } - - // If we are still learning - if(this->current_iteration < this->n_init_samples + this->n_learning_iterations) - { - if(this->verbose) - { - if (this->current_iteration < this->n_init_samples) - { - std::cout << std::endl << "Evaluating initial random sample" << std::endl; - } - else - { - std::cout << std::endl << "I am learning " << std::endl; - } - } - // Get new sample (weights) and add sample - this->bo_step(); - - // Set new weights - this->set_ode_matrix(); - - // Update position -// this->evaluator->Update(this->robot->WorldPose(), _time, _step); - } - // If we are finished learning but are cooling down - reset once - else if((this->current_iteration >= (this->n_init_samples + - this->n_learning_iterations)) - and (this->current_iteration < (this->n_init_samples + - this->n_learning_iterations + - this->n_cooldown_iterations - 1))) - { - if(this->verbose) - { - std::cout << std::endl << "I am cooling down " << std::endl; - } - - // Update robot position -// this->evaluator->Update(this->robot->WorldPose(), _time, _step); - - // Use best sample in next iteration - this->samples.push_back(this->best_sample); - - // Set ODE matrix - this->set_ode_matrix(); - } - // Else we don't want to update anything, but construct plots from this run once. - else - { -// // Create plots -// if(this->run_analytics) -// { -// // Construct plots -// this->get_analytics(); -// } - - // Exit - if(this->verbose) - { - std::cout << std::endl << "I am finished " << std::endl; - } - std::exit(0); - } - - // Evaluation policy here - this->start_time = _time; - this->evaluator->Reset(); - this->current_iteration += 1; - } - - // Send new signals to the motors - this->step(_time, this->output); - p = 0; - for (const auto &motor: _motors) - { - motor->Update(this->output + p, _step); - p += motor->Outputs(); - } -} - -/** - * Make matrix of weights A as defined in dx/dt = Ax. - * Element (i,j) specifies weight from neuron i to neuron j in the system of ODEs - */ -void DifferentialCPG::set_ode_matrix(){ - // Initiate new matrix - std::vector> matrix; - - // Fill with zeroes - for(size_t i =0; i neurons.size(); i++) - { - // Initialize row in matrix with zeros - std::vector< double > row; - for (size_t j = 0; j < this->neurons.size(); j++) - { - row.push_back(0); - } - matrix.push_back(row); - } - - // Process A<->B connections - int index = 0; - for(size_t i =0; i neurons.size(); i++) - { - // Get correct index - int c = 0; - if (i%2 == 0){ - c = i + 1; - } - else{ - c = i - 1; - } - - // Add a/b connection weight - index = (int)(i/2); - auto w = this->samples.at(this->current_iteration)(index) * - (this->range_ub - this->range_lb) + this->range_lb; - matrix[i][c] = w; - matrix[c][i] = -w; - } - - // A<->A connections - index++; - int k = 0; - std::vector connections_seen; - - for (auto const &connection : this->connections) - { - // Get connection information - int x1, y1, z1, x2, y2, z2; - std::tie(x1, y1, z1, x2, y2, z2) = connection.first; - - // Find location of the two neurons in this->neurons list - int l1, l2; - int c = 0; - for(auto const &neuron : this->neurons) - { - int x, y, z; - std::tie(x, y, z) = neuron.first; - if (x == x1 and y == y1 and z == z1) - { - l1 = c; - } - else if (x == x2 and y == y2 and z == z2) - { - l2 = c; - } - // Update counter - c++; - } - - // Add connection to seen connections - if(l1 > l2) - { - int l1_old = l1; - l1 = l2; - l2 = l1_old; - } - std::string connection_string = std::to_string(l1) + "-" + std::to_string(l2); - - // if not in list, add to list - auto connections_list = std::find(connections_seen.begin(), connections_seen.end(), connection_string); - if(connections_list == connections_seen.end()) - { - connections_seen.push_back(connection_string); - } - // else continue to next iteration - else{ - continue; - } - - // Get weight - auto w = this->samples.at(this->current_iteration)(index + k) * - (this->range_ub - this->range_lb) + this->range_lb; - - // Set connection in weight matrix - matrix[l1][l2] = w; - matrix[l2][l1] = -w; - k++; - } - - // Update matrix - this->ode_matrix = matrix; - - // Reset neuron state - this->reset_neuron_state(); - - // Save this sample to file - std::ofstream samples_file; - samples_file.open(this->directory_name + "samples.txt", std::ios::app); - auto sample = this->samples.at(this->current_iteration); - for(size_t j = 0; j < this->n_weights; j++) - { - samples_file << sample(j) << ", "; - } - samples_file << std::endl; - samples_file.close(); -} - - -/** - * Set states back to original value (that is on the unit circle) - */ -void DifferentialCPG::reset_neuron_state(){ - int c = 0; - for(auto const &neuron : this->neurons) - { - // Get neuron properties - int x, y, z, frame_of_reference; - double bias ,gain ,state; - std::tie(x, y, z) = neuron.first; - std::tie(bias, gain, state, frame_of_reference) = neuron.second; - - if (z == -1) - { - // Neuron B - if (this->reset_neuron_random) - { - this->neurons[{x, y, z}] = {0.f, - 0.f, - ((double) rand() / (RAND_MAX))*2*this->init_neuron_state - this->init_neuron_state, - frame_of_reference}; - } - else - { - this->neurons[{x, y, z}] = {0.f, 0.f, -this->init_neuron_state, frame_of_reference}; - } - } - else - { - // Neuron A - if (this->reset_neuron_random) - { - this->neurons[{x, y, z}] = {0.f, - 0.f, - ((double) rand() / (RAND_MAX))*2*this->init_neuron_state - this->init_neuron_state, - frame_of_reference}; - } - else - { - this->neurons[{x, y, z}] = {0.f, 0.f, +this->init_neuron_state, frame_of_reference}; - } - } - c++; - } -} - -/** - * Step function that is called from within Update() - * - * @param _time - * @param _output - */ -void DifferentialCPG::step( - const double _time, - double *_output) +revolve::DifferentialCPG::ControllerParams DifferentialCPG::load_params_from_sdf(sdf::ElementPtr brain_sdf) { - int neuron_count = 0; - for (const auto &neuron : this->neurons) - { - // Neuron.second accesses the second 3-tuple of a neuron, containing the bias/gain/state. - double recipient_bias, recipient_gain, recipient_state; - int frame_of_reference; - std::tie(recipient_bias, recipient_gain, recipient_state, frame_of_reference) = neuron.second; - - // Save for ODE - this->next_state[neuron_count] = recipient_state; - neuron_count++; - } - - // Copy values from next_state into x for ODEINT - state_type x(this->neurons.size()); - for (size_t i = 0; i < this->neurons.size(); i++) - { - x[i] = this->next_state[i]; - } - - // Stepper. The result is saved in x. Begin time t, time step dt - double dt = (_time - this->previous_time); - this->previous_time = _time; - - // Perform one step - stepper.do_step( - [this](const state_type &x, state_type &dxdt, double t) - { - for(size_t i = 0; i < this->neurons.size(); i++) - { - dxdt[i] = 0; - for(size_t j = 0; j < this->neurons.size(); j++) - { - dxdt[i] += x[j]*this->ode_matrix[j][i]; - } - } - }, - x, - _time, - dt); - - // Copy values into nextstate - for (size_t i = 0; i < this->neurons.size(); i++) - { - this->next_state[i] = x[i]; - } - - // Loop over all neurons to actually update their states. Note that this is a new outer for loop - auto i = 0; auto j = 0; - for (auto &neuron : this->neurons) - { - // Get bias gain and state for this neuron. Note that we don't take the coordinates. - // However, they are implicit as their order did not change. - double bias, gain, state; - int frame_of_reference; - std::tie(bias, gain, state, frame_of_reference) = neuron.second; - double x, y, z; - std::tie(x, y, z) = neuron.first; - neuron.second = {bias, gain, this->next_state[i], frame_of_reference}; - j = this->motor_coordinates[{x,y}]; - // Should be one, as output should be based on +1 neurons, which are the A neurons - if (i % 2 == 1) - { - // TODO: Add Milan's function here as soon as things are working a bit - // f(a) = (w_ao*a - bias)*gain - - // Apply saturation formula - auto x = this->next_state[i]; - - // Use frame of reference - if(use_frame_of_reference) - { - - if (std::abs(frame_of_reference) == 1) + // Get all params from the sdf + // TODO: Add exception handling + sdf::ElementPtr controller_sdf = brain_sdf->GetElement("rv:controller"); + if (controller_sdf == nullptr) + throw std::runtime_error("Controller element not found when reading CPG parameters"); + + revolve::DifferentialCPG::ControllerParams params; + // params.reset_neuron_random = + (controller_sdf->GetAttribute("reset_neuron_random")->Get(params.reset_neuron_random)); + // params.use_frame_of_reference = + (controller_sdf->GetAttribute("use_frame_of_reference")->Get(params.use_frame_of_reference)); + std::clog << "USE_FRAME_OF_REFERENCE: " << controller_sdf->GetAttribute("use_frame_of_reference")->GetAsString() << std::endl; + params.init_neuron_state = stod(controller_sdf->GetAttribute("init_neuron_state")->GetAsString()); + params.range_ub = stod(controller_sdf->GetAttribute("range_ub")->GetAsString()); + params.output_signal_factor = stod(controller_sdf->GetAttribute("output_signal_factor")->GetAsString()); + params.abs_output_bound = stod(controller_sdf->GetAttribute("abs_output_bound")->GetAsString()); + + // Get the weights from the sdf: + // If loading with CPPN, the weights attribute does not exist + if (controller_sdf->HasAttribute("weights")) + { + std::string sdf_weights = controller_sdf->GetAttribute("weights")->GetAsString(); + std::string delimiter = ";"; + + size_t pos = 0; + std::string token; + while ((pos = sdf_weights.find(delimiter)) != std::string::npos) { - this->output[j] = this->signal_factor_left_right*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0*x/this->abs_output_bound)) -1); + token = sdf_weights.substr(0, pos); + params.weights.push_back(stod(token)); + sdf_weights.erase(0, pos + delimiter.length()); } - else if (frame_of_reference == 0) - { - this->output[j] = this->signal_factor_mid*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0*x/this->abs_output_bound)) -1); - } - else - { - std::cout << "WARNING: frame_of_reference not in {-1,0,1}." << std::endl; - } - - } - // Don't use frame of reference - else{ - this->output[j] = this->signal_factor_all_*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0*x/this->abs_output_bound)) -1); - } + // push the last element that does not end with the delimiter + params.weights.push_back(stod(sdf_weights)); } - i++; - } - - // Comment to save disk space -// // Write state to file -// std::ofstream state_file; -// state_file.open(this->directory_name + "states.txt", std::ios::app); -// for(size_t i = 0; i < this->neurons.size(); i++) -// { -// state_file << this->next_state[i] << ","; -// } -// state_file << std::endl; -// state_file.close(); -// -// // Write signal to file -// std::ofstream signal_file; -// signal_file.open(this->directory_name + "signal.txt", std::ios::app); -// for(size_t i = 0; i < this->n_motors; i++) -// { -// signal_file << this->output[i] << ","; -// } -// signal_file << std::endl; -// signal_file.close(); -} - - -/** - * Save the parameters used in this run to a file. - */ -void DifferentialCPG::save_parameters(){ - // Write parameters to file - std::ofstream parameters_file; - parameters_file.open(this->directory_name + "parameters.txt"); - - // Various parameters - parameters_file << "Dimensions: " << this->n_weights << std::endl; - parameters_file << "n_init_samples: " << this->n_init_samples << std::endl; - parameters_file << "n_learning_iterations: " << this->n_learning_iterations << std::endl; - parameters_file << "n_cooldown_iterations: " << this->n_cooldown_iterations << std::endl; - parameters_file << "evaluation_rate: " << this->evaluation_rate << std::endl; - parameters_file << "abs_output_bound: " << this->abs_output_bound << std::endl; - parameters_file << "signal_factor_all: " << this->signal_factor_all_ << std::endl; - parameters_file << "range_lb: " << this->range_lb << std::endl; - parameters_file << "range_ub: " << this->range_ub << std::endl; - parameters_file << "run_analytics: " << this->run_analytics << std::endl; - parameters_file << "load_brain: " << this->load_brain << std::endl; - parameters_file << "reset_robot_position: " << this->reset_robot_position << std::endl; - parameters_file << "reset_neuron_state_bool: " << this->reset_neuron_state_bool << std::endl; - parameters_file << "reset_neuron_random: " << this->reset_neuron_random << std::endl; - parameters_file << "initial state value: " << this->init_neuron_state << std::endl; - - // BO hyper-parameters - parameters_file << std::endl << "Initialization method used: " << this->init_method << std::endl; - parameters_file << "Acqui. function used: " << this->acquisition_function << std::endl; - parameters_file << "EI jitter: " <directory_name - + " " - + std::to_string((int)this->n_init_samples) - + " " - + std::to_string((int)this->n_cooldown_iterations); - // Execute python command - std::system(std::string("python3 " + plot_command).c_str()); + return params; } diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.h b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.h index 2595f83621..bb4417fa41 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.h +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.h @@ -1,275 +1,44 @@ -/* - * Copyright (C) 2015-2018 Vrije Universiteit Amsterdam - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - * Description: TODO: - * Author: Milan Jelisavcic - * Date: December 29, 2018 - * - */ +// +// Created by andi on 20-09-19. +// -#ifndef REVOLVE_DIFFERENTIALCPG_H_ -#define REVOLVE_DIFFERENTIALCPG_H_ +#pragma once -// Standard libraries -#include -#include - -// External libraries -#include -#include - -// Project headers -#include "Evaluator.h" +#include +#include #include "Brain.h" -/// These numbers are quite arbitrary. It used to be in:13 out:8 for the -/// Arduino, but I upped them both to 20 to accommodate other scenarios. -/// Should really be enforced in the Python code, this implementation should -/// not be the limit. -#define MAX_INPUT_NEURONS 20 -#define MAX_OUTPUT_NEURONS 20 - -/// Arbitrary value -#define MAX_HIDDEN_NEURONS 30 - -/// Convenience -#define MAX_NON_INPUT_NEURONS (MAX_HIDDEN_NEURONS + MAX_OUTPUT_NEURONS) - -/// (bias, tau, gain) or (phase offset, period, gain) -#define MAX_NEURON_PARAMS 3 - -typedef std::vector< double > state_type; - namespace revolve { - namespace gazebo { - class DifferentialCPG - : public Brain + namespace gazebo { - /// \brief Constructor - /// \param[in] _modelName Name of the robot - /// \param[in] _node The brain node - /// \param[in] _motors Reference to a motor list, it be reordered - /// \param[in] _sensors Reference to a sensor list, it might be reordered - public: - DifferentialCPG( - const ::gazebo::physics::ModelPtr &_model, - const sdf::ElementPtr robot_config, - const std::vector< MotorPtr > &_motors, - const std::vector< SensorPtr > &_sensors); - - public: void set_ode_matrix(); - - /// \brief Destructor - public: virtual ~DifferentialCPG(); - - /// \brief The default update method for the controller - /// \param[in] _motors Motor list - /// \param[in] _sensors Sensor list - /// \param[in] _time Current world time - /// \param[in] _step Current time step - public: - virtual void Update( - const std::vector< MotorPtr > &_motors, - const std::vector< SensorPtr > &_sensors, - const double _time, - const double _step); - - protected: - void step( - const double _time, - double *_output); - - /// \brief Register of motor IDs and their x,y-coordinates - protected: std::map< std::string, std::tuple< int, int > > positions; - - public: std::map< std::tuple< int, int>, int> motor_coordinates; - - - /// \brief Register of individual neurons in x,y,z-coordinates - /// \details x,y-coordinates define position of a robot's module and - // z-coordinate define A or B neuron (z=1 or -1 respectively). Stored - // values are a bias, gain, state and frame of reference of each neuron. - protected: - std::map< std::tuple< int, int, int >, std::tuple< double, double, double, int > > - neurons; - - /// \brief Register of connections between neighnouring neurons - /// \details Coordinate set of two neurons (x1, y1, z1) and (x2, y2, z2) - // define a connection. The second tuple contains 1: the connection value and - // 2: the weight index corresponding to this connection. - protected: - std::map< std::tuple< int, int, int, int, int, int >, std::tuple > - connections; - - /// \brief Runge-Kutta 45 stepper - protected: boost::numeric::odeint::runge_kutta4< state_type > stepper; - - /// \brief Pointer to access parameters - private: sdf::ElementPtr learner; - - /// \brief Used to determine the next state array - private: double *next_state; - - /// \brief Used for ODE-int - protected: std::vector> ode_matrix; - protected: state_type x; - - /// \brief One input state for each input neuron - private: double *input; - - /// \brief Used to determine the output to the motors array - private: double *output; - - /// \brief Location where to save output - private: std::string directory_name; - - /// \brief Name of the robot - private: ::gazebo::physics::ModelPtr robot; - - /// \brief Init BO loop - public: void bo_init_sampling(); - - /// \brief Main BO loop - public: void bo_step(); - - /// \brief evaluation rate - private: double evaluation_rate; - - /// \brief Get fitness - private: void save_fitness(); - - /// \brief Pointer to the fitness evaluator - protected: EvaluatorPtr evaluator; - - /// \brief Holder for BO parameters - public: struct Params; - - /// \brief Save parameters - private: void save_parameters(); - - /// \brief Best fitness seen so far - private: double best_fitness = -10.0; - - /// \brief Sample corresponding to best fitness - private: Eigen::VectorXd best_sample; - - /// \brief Starting time - private: double start_time; - - /// \brief BO attributes - private: size_t current_iteration = 0; - - /// \brief Max number of iterations learning is allowed - private: size_t n_learning_iterations; - - /// \brief Number of initial samples - private: size_t n_init_samples; - - /// \brief Cool down period - private: size_t n_cooldown_iterations; - - /// \brief Limbo optimizes in [0,1] - private: double range_lb; - - /// \brief Limbo optimizes in [0,1] - private: double range_ub; - - /// \brief How to take initial random samples - private: std::string init_method; - - /// \brief All fitnesses seen so far. Called observations in limbo context - private: std::vector< Eigen::VectorXd > observations; - - /// \brief All samples seen so far. - private: std::vector< Eigen::VectorXd > samples; - - /// \brief The number of weights to optimize - private: size_t n_weights; - - /// \brief Dummy evaluation funtion to reduce changes to be made on the limbo package - public: struct evaluation_function; - - /// \brief Reset the robot to starting position each iteration. - private: bool reset_robot_position; - - /// \brief Reset neuron state at each iteration (also during validation) - private: bool reset_neuron_state_bool; - - /// \brief Factor to multiply output signal with - private: double signal_factor_all_; - - /// \brief Factor to multiply output signal with - private: double signal_factor_mid; - - /// \brief Factor to multiply output signal with - private: double signal_factor_left_right; - - /// \brief Function that resets neuron state - private: void reset_neuron_state(); - - /// \brief When reset a neuron state,do it randomly: - private: bool reset_neuron_random; - - /// \brief Boolean to enable/disable constructing plots - private: bool run_analytics; - - /// \brief Automatically generate plots - public: void get_analytics(); - - /// \brief Show output (1) or not (0) - public: int verbose; - - /// \brief Time to skip for fitness evaluation during training - public: int startup_time; - - /// \brief Helper for startup time - private: bool start_fitness_recording = true; - - /// \brief absolute bound on motor signal value - public: double abs_output_bound; - - /// \brief Holds the number of motors in the robot - private: size_t n_motors; - - /// \brief Helper for numerical integrator - private: double previous_time = 0; - - /// \brief Initial neuron state - private: double init_neuron_state; - - /// \brief Holder for loading a brain - private: std::string load_brain = ""; - - /// \brief Specifies the acquisition function used - public: std::string acquisition_function; - - /// \brief Use frame of reference {-1,0,1} version or not - private: bool use_frame_of_reference; - - // BO Learner parameters - private: double kernel_noise_; - private: bool kernel_optimize_noise_; - public: double kernel_sigma_sq_; - public: double kernel_l_; - private: int kernel_squared_exp_ard_k_; - private: double acqui_gpucb_delta_ ; - public: double acqui_ucb_alpha_; - private: double acqui_ei_jitter_; - }; - } + /// \brief connection between gazebo and revolve CPG + /// \details gets the sdf - model data and passes them to revolve + class DifferentialCPG: public revolve::DifferentialCPG + { + public: + /// \brief Constructor + /// \param[in] brain_sdf ElementPtr containing the "brain" - tag of the model sdf + /// \param[in] _motors vector list of motors + /// \details Extracts controller parameters + /// from brain_sdf and calls revolve::DifferentialCPG's contructor. + explicit DifferentialCPG(const sdf::ElementPtr brain_sdf, + const std::vector< MotorPtr > &_motors, + std::shared_ptr angle_to_target_sensor = nullptr); + + protected: + explicit DifferentialCPG(const sdf::ElementPtr brain_sdf, + const std::vector &_motors, + const NEAT::Genome &genome, + std::shared_ptr angle_to_target_sensor = nullptr); + + /// \brief extracts CPG controller parameters from brain_sdf + /// \param[in] brain_sdf ElementPtr containing the "brain" - tag of the model sdf + /// \return parameters of the CPG controller + /// \details get the strings of the controller parameters and convert them to the + /// appropriate datatype. Store them in a revolve::DifferentialCPG::ControllerParams + /// struct and return them. + static revolve::DifferentialCPG::ControllerParams load_params_from_sdf(sdf::ElementPtr brain_sdf); + }; + } } - -#endif //REVOLVE_DIFFERENTIALCPG_H_ diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.cpp new file mode 100644 index 0000000000..20cfb68f1b --- /dev/null +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.cpp @@ -0,0 +1,62 @@ +// +// Created by andi on 06-10-19. +// + +#include "DifferentialCPGClean.h" + +using namespace revolve::gazebo; + +DifferentialCPGClean::DifferentialCPGClean(const sdf::ElementPtr brain_sdf, + const std::vector &_motors, + std::shared_ptr angle_to_target_sensor) + : revolve::DifferentialCPG(load_params_from_sdf(brain_sdf), _motors, angle_to_target_sensor) +{ +} + +DifferentialCPGClean::DifferentialCPGClean(const sdf::ElementPtr brain_sdf, + const std::vector &_motors, + const NEAT::Genome &genome, + std::shared_ptr angle_to_target_sensor) + : revolve::DifferentialCPG(load_params_from_sdf(brain_sdf), _motors, genome, angle_to_target_sensor) +{ +} + +revolve::DifferentialCPG::ControllerParams DifferentialCPGClean::load_params_from_sdf(sdf::ElementPtr brain_sdf) +{ + // Get all params from the sdf + // TODO: Add exception handling + sdf::ElementPtr controller_sdf = brain_sdf->GetElement("rv:controller"); + std::clog << "USE_FRAME_OF_REFERENCE: " << controller_sdf->GetAttribute("use_frame_of_reference")->GetAsString() << std::endl; + revolve::DifferentialCPG::ControllerParams params; + // params.reset_neuron_random = (controller_sdf->GetAttribute("reset_neuron_random")->Get(params.reset_neuron_random)); + // params.use_frame_of_reference = (controller_sdf->GetAttribute("use_frame_of_reference")->Get(params.use_frame_of_reference)); + params.use_frame_of_reference = true; + params.init_neuron_state = stod(controller_sdf->GetAttribute("init_neuron_state")->GetAsString()); + params.range_ub = stod(controller_sdf->GetAttribute("range_ub")->GetAsString()); + params.output_signal_factor = stod(controller_sdf->GetAttribute("output_signal_factor")->GetAsString()); + params.abs_output_bound = stod(controller_sdf->GetAttribute("abs_output_bound")->GetAsString()); + + // Get the weights from the sdf: + // If loading with CPPN, the weights attribute does not exist + if (controller_sdf->HasAttribute("weights")) + { + std::string sdf_weights = controller_sdf->GetAttribute("weights")->GetAsString(); + std::string delimiter = ";"; + + if (!sdf_weights.empty()) + { + size_t pos = 0; + std::string token; + while ((pos = sdf_weights.find(delimiter)) != std::string::npos) + { + token = sdf_weights.substr(0, pos); + params.weights.push_back(stod(token)); + sdf_weights.erase(0, pos + delimiter.length()); + } + // push the last element that does not end with the delimiter + params.weights.push_back(stod(sdf_weights)); + } + } + + return params; +} diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.h b/cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.h new file mode 100644 index 0000000000..f559320150 --- /dev/null +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.h @@ -0,0 +1,47 @@ +// +// Created by andi on 20-09-19. +// + +#ifndef REVOLVE_DIFFERENTIALCPGCLEAN_H +#define REVOLVE_DIFFERENTIALCPGCLEAN_H + +#include +#include +#include "Brain.h" + +namespace revolve +{ + namespace gazebo + { + /// \brief connection between gazebo and revolve CPG + /// \details gets the sdf - model data and passes them to revolve + class DifferentialCPGClean: public revolve::DifferentialCPG + { + public: + /// \brief Constructor + /// \param[in] brain_sdf ElementPtr containing the "brain" - tag of the model sdf + /// \param[in] _motors vector list of motors + /// \details Extracts controller parameters + /// from brain_sdf and calls revolve::DifferentialCPG's contructor. + explicit DifferentialCPGClean(const sdf::ElementPtr brain_sdf, + const std::vector< MotorPtr > &_motors, + std::shared_ptr angle_to_target_sensor = nullptr); + + protected: + explicit DifferentialCPGClean(const sdf::ElementPtr brain_sdf, + const std::vector &_motors, + const NEAT::Genome &genome, + std::shared_ptr angle_to_target_sensor = nullptr); + + /// \brief extracts CPG controller parameters from brain_sdf + /// \param[in] brain_sdf ElementPtr containing the "brain" - tag of the model sdf + /// \return parameters of the CPG controller + /// \details get the strings of the controller parameters and convert them to the + /// appropriate datatype. Store them in a revolve::DifferentialCPG::ControllerParams + /// struct and return them. + static revolve::DifferentialCPG::ControllerParams load_params_from_sdf(sdf::ElementPtr brain_sdf); + }; + } +} + +#endif //REVOLVE_DIFFERENTIALCPGCLEAN_H diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPG_BO.h b/cpprevolve/revolve/gazebo/brains/DifferentialCPG_BO.h index c859415630..f149e374ce 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG_BO.h +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG_BO.h @@ -19,22 +19,26 @@ #include #include -namespace limbo { - namespace defaults { - struct bayes_opt_boptimizer { +namespace limbo +{ + namespace defaults + { + struct bayes_opt_boptimizer + { BO_PARAM(int, hp_period, -1); }; } BOOST_PARAMETER_TEMPLATE_KEYWORD(acquiopt) - namespace bayes_opt { + namespace bayes_opt + { using boptimizer_signature = boost::parameter::parameters, - boost::parameter::optional, - boost::parameter::optional, - boost::parameter::optional, - boost::parameter::optional, - boost::parameter::optional>; + boost::parameter::optional, + boost::parameter::optional, + boost::parameter::optional, + boost::parameter::optional, + boost::parameter::optional>; // clang-format off /** @@ -66,10 +70,12 @@ namespace limbo { class A5 = boost::parameter::void_, class A6 = boost::parameter::void_> // clang-format on - class BOptimizer : public BoBase { + class BOptimizer : public BoBase + { public: // defaults - struct defaults { + struct defaults + { using acquiopt_t = opt::NLOptNoGrad; }; @@ -84,7 +90,7 @@ namespace limbo { /// The main function (run the Bayesian optimization algorithm) template - void optimize(const StateFunction& sfun, std::vector all_samples, std::vector all_observations, const AggregatorFunction& afun = AggregatorFunction(), bool reset = true) + void optimize(const StateFunction &sfun, std::vector all_samples, std::vector all_observations, const AggregatorFunction &afun = AggregatorFunction(), bool reset = true) { this->_init(sfun, afun, reset); //reset @@ -92,10 +98,12 @@ namespace limbo { this->_samples = all_samples; this->_observations = all_observations; - if (!this->_observations.empty()) { + if (!this->_observations.empty()) + { _model.compute(this->_samples, this->_observations); } - else { + else + { std::cout << "OBSERVATION SET IS EMPTY \n"; _model = model_t(StateFunction::dim_in(), StateFunction::dim_out()); } @@ -104,14 +112,16 @@ namespace limbo { struct timeval timeStart, timeEnd; double timeDiff; - while (!this->_stop(*this, afun)) { + while (!this->_stop(*this, afun)) + { - gettimeofday(&timeStart,NULL); + gettimeofday(&timeStart, NULL); acquisition_function_t acqui(_model, this->_current_iteration); auto acqui_optimization = - [&](const Eigen::VectorXd& x, bool g) { return acqui(x, afun, g); }; + [&](const Eigen::VectorXd &x, bool g) + { return acqui(x, afun, g); }; Eigen::VectorXd starting_point = tools::random_vector(StateFunction::dim_in(), Params::bayes_opt_bobase::bounded()); // new samples are from the acquisition optimizer @@ -124,18 +134,16 @@ namespace limbo { _model.add_sample(this->_samples.back(), this->_observations.back()); - if (Params::bayes_opt_boptimizer::hp_period() > 0 - && (this->_current_iteration + 1) % Params::bayes_opt_boptimizer::hp_period() == 0) + if (Params::bayes_opt_boptimizer::hp_period() > 0 && (this->_current_iteration + 1) % Params::bayes_opt_boptimizer::hp_period() == 0) _model.optimize_hyperparams(); this->_current_iteration++; this->_total_iterations++; - gettimeofday(&timeEnd,NULL); + gettimeofday(&timeEnd, NULL); - timeDiff = 1000000 * (timeEnd.tv_sec - timeStart.tv_sec) - + timeEnd.tv_usec - timeStart.tv_usec; //tv_sec: value of second, tv_usec: value of microsecond - timeDiff/=1000; + timeDiff = 1000000 * (timeEnd.tv_sec - timeStart.tv_sec) + timeEnd.tv_usec - timeStart.tv_usec; //tv_sec: value of second, tv_usec: value of microsecond + timeDiff /= 1000; std::ofstream ctime; ctime.open("../ctime.txt", std::ios::app); @@ -145,7 +153,7 @@ namespace limbo { /// return the best observation so far (i.e. max(f(x))) template - const Eigen::VectorXd& best_observation(const AggregatorFunction& afun = AggregatorFunction()) const + const Eigen::VectorXd &best_observation(const AggregatorFunction &afun = AggregatorFunction()) const { auto rewards = std::vector(this->_observations.size()); std::transform(this->_observations.begin(), this->_observations.end(), rewards.begin(), afun); @@ -155,7 +163,7 @@ namespace limbo { /// return the best sample so far (i.e. the argmax(f(x))) template - const Eigen::VectorXd& best_sample(const AggregatorFunction& afun = AggregatorFunction()) const + const Eigen::VectorXd &best_sample(const AggregatorFunction &afun = AggregatorFunction()) const { auto rewards = std::vector(this->_observations.size()); std::transform(this->_observations.begin(), this->_observations.end(), rewards.begin(), afun); @@ -164,17 +172,19 @@ namespace limbo { } /// Return a reference to the last sample. Used for implementation with revolve - const Eigen::VectorXd& last_sample(){ + const Eigen::VectorXd &last_sample() + { return this->_samples.back(); } - const model_t& model() const { return _model; } + const model_t &model() const { return _model; } protected: model_t _model; }; - namespace _default_hp { + namespace _default_hp + { template using model_t = model::GPOpt; template @@ -184,10 +194,10 @@ namespace limbo { /// A shortcut for a BOptimizer with UCB + GPOpt /// The acquisition function and the model CANNOT be tuned (use BOptimizer for this) template + class A1 = boost::parameter::void_, + class A2 = boost::parameter::void_, + class A3 = boost::parameter::void_, + class A4 = boost::parameter::void_> using BOptimizerHPOpt = BOptimizer>, acquifun<_default_hp::acqui_t>, A1, A2, A3, A4>; } } diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.cpp new file mode 100644 index 0000000000..858afaea77 --- /dev/null +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.cpp @@ -0,0 +1,55 @@ +// +// Created by andi on 11-10-19. +// + +#include +#include "Brain.h" + +#include "DifferentialCPPNCPG.h" +#include "DifferentialCPGClean.h" + +using namespace revolve::gazebo; + +bool string_replace(std::string& str, const std::string& from, const std::string& to) +{ + size_t start_pos = str.find(from); + int substitutions = 0; + while (start_pos != std::string::npos) + { + str.replace(start_pos, from.length(), to); + substitutions++; + start_pos = str.find(from); + } + return substitutions > 0; +} + + +DifferentialCPPNCPG::DifferentialCPPNCPG(const sdf::ElementPtr brain_sdf, + const std::vector &motors, + std::shared_ptr angle_to_target_sensor) + : DifferentialCPGClean( + brain_sdf, + motors, + DifferentialCPPNCPG::load_cppn_genome_from_sdf(brain_sdf), + angle_to_target_sensor) +{} + +/// \brief extracts CPPN genome from brain_sdf +/// \param[in] brain_sdf ElementPtr containing the "brain" - tag of the model sdf +/// \return the NEAT genome +/// \details Loads genome from SDF as string and deserializes it into type type NEAT::Genome +NEAT::Genome DifferentialCPPNCPG::load_cppn_genome_from_sdf(const sdf::ElementPtr brain_sdf) +{ + const sdf::ElementPtr genome_sdf = brain_sdf->GetElement("rv:controller")->GetElement("rv:genome"); + const std::string genome_type = genome_sdf->GetAttribute("type")->GetAsString(); + if (genome_type != "CPPN") + { + throw std::runtime_error("unexpected GENOME type"); + } + std::string genome_string = genome_sdf->GetValue()->GetAsString(); + string_replace(genome_string, "inf", std::to_string(std::numeric_limits::max())); + NEAT::Genome genome = NEAT::Genome(); + genome.Deserialize(genome_string); + + return genome; +} diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.h b/cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.h new file mode 100644 index 0000000000..aa20e9cf87 --- /dev/null +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.h @@ -0,0 +1,37 @@ +// +// Created by andi on 11-10-19. +// + +#ifndef REVOLVE_DIFFERENTIALCPPNCPG_H +#define REVOLVE_DIFFERENTIALCPPNCPG_H + +#include +#include "DifferentialCPGClean.h" +#include "Brain.h" + + +namespace revolve { +namespace gazebo { + +/// \brief connection between gazebo and revolve CPG with config CPPN +/// \details gets the sdf - model data and passes them to revolve +class DifferentialCPPNCPG : public DifferentialCPGClean +{ +public: + /// \brief Constructor + /// \param[in] brain_sdf ElementPtr containing the "brain" - tag of the model sdf + /// \param[in] _motors vector list of motors + /// \details Extracts controller parameters and Genome + /// from brain_sdf and calls revolve::DifferentialCPG's contructor. + explicit DifferentialCPPNCPG(const sdf::ElementPtr brain_sdf, + const std::vector< MotorPtr > &_motors, + std::shared_ptr angle_to_target_sensor = nullptr); + +protected: + static NEAT::Genome load_cppn_genome_from_sdf(const sdf::ElementPtr brain_sdf); +}; +} +} + + +#endif //REVOLVE_DIFFERENTIALCPPNCPG_H diff --git a/cpprevolve/revolve/gazebo/brains/FixedAngleController.h b/cpprevolve/revolve/gazebo/brains/FixedAngleController.h new file mode 100644 index 0000000000..168035e0ed --- /dev/null +++ b/cpprevolve/revolve/gazebo/brains/FixedAngleController.h @@ -0,0 +1,33 @@ +// +// Created by Matteo De Carlo on 11/09/2019. +// + +#ifndef GAZEBO_REVOLVE_FIXEDANGLECONTROLLER_H +#define GAZEBO_REVOLVE_FIXEDANGLECONTROLLER_H + + +#include +#include +#include +#include +#include "Brain.h" + +namespace revolve +{ +namespace gazebo +{ + +class FixedAngleController: public revolve::FixedAngleController +{ +public: + explicit FixedAngleController(double angle) + : revolve::FixedAngleController(angle) + {} + +}; + +} +} + + +#endif //GAZEBO_REVOLVE_FIXEDANGLECONTROLLER_H diff --git a/cpprevolve/revolve/gazebo/brains/NeuralNetwork.cpp b/cpprevolve/revolve/gazebo/brains/NeuralNetwork.cpp index 94fd84f372..c30df9b21b 100644 --- a/cpprevolve/revolve/gazebo/brains/NeuralNetwork.cpp +++ b/cpprevolve/revolve/gazebo/brains/NeuralNetwork.cpp @@ -54,7 +54,8 @@ NeuralNetwork::NeuralNetwork( const sdf::ElementPtr &_settings, const std::vector< MotorPtr > &_motors, const std::vector< SensorPtr > &_sensors) - : flipState_(false) + : Controller(ControllerType::NEURAL_NETWORK) + , flipState_(false) , nInputs_(0) , nOutputs_(0) , nHidden_(0) @@ -164,7 +165,8 @@ NeuralNetwork::NeuralNetwork( unsigned int outputsIndex = 0; for (const auto &motor : _motors) { - std::string partId = motor->PartId(); + Motor *gz_motor = reinterpret_cast(motor.get()); + std::string partId = gz_motor->PartId(); auto details = neuronPartIdMap.find(partId); if (details == neuronPartIdMap.end()) { @@ -176,7 +178,7 @@ NeuralNetwork::NeuralNetwork( const auto &neuron_list = details->second; auto neuron_iter = neuron_list.cbegin(); - for (unsigned int i = 0, l = motor->Outputs(); i < l; ++i) + for (unsigned int i = 0, l = motor->n_outputs(); i < l; ++i) { while (not ((*neuron_iter)->GetAttribute("layer")->GetAsString() == "output")) { @@ -204,7 +206,8 @@ NeuralNetwork::NeuralNetwork( unsigned int inputsIndex = 0; for (const auto &sensor : _sensors) { - auto partId = sensor->PartId(); + Sensor * gz_sensor = reinterpret_cast(sensor.get()); + auto partId = gz_sensor->PartId(); auto details = neuronPartIdMap.find(partId); if (details == neuronPartIdMap.end()) { @@ -215,7 +218,7 @@ NeuralNetwork::NeuralNetwork( const auto &neuron_list = details->second; auto neuron_iter = neuron_list.cbegin(); - for (unsigned int i = 0, l = sensor->Inputs(); i < l; ++i) + for (unsigned int i = 0, l = sensor->n_inputs(); i < l; ++i) { while (not ((*neuron_iter)->GetAttribute("layer")->GetAsString() == "input")) { @@ -393,7 +396,7 @@ void NeuralNetwork::Step(const double _time) } ///////////////////////////////////////////////// -void NeuralNetwork::Update( +void NeuralNetwork::update( const std::vector< MotorPtr > &_motors, const std::vector< SensorPtr > &_sensors, const double _time, @@ -405,8 +408,8 @@ void NeuralNetwork::Update( unsigned int p = 0; for (const auto &sensor : _sensors) { - sensor->Read(this->input_.data()+p); - p += sensor->Inputs(); + sensor->read(this->input_.data()+p); + p += sensor->n_inputs(); } this->Step(_time); @@ -419,8 +422,8 @@ void NeuralNetwork::Update( p = 0; for (const auto &motor: _motors) { - motor->Update(output.data()+p, _step); - p += motor->Outputs(); + motor->write(output.data()+p, _step); + p += motor->n_outputs(); } } diff --git a/cpprevolve/revolve/gazebo/brains/NeuralNetwork.h b/cpprevolve/revolve/gazebo/brains/NeuralNetwork.h index a1147f6663..cf98dcb526 100644 --- a/cpprevolve/revolve/gazebo/brains/NeuralNetwork.h +++ b/cpprevolve/revolve/gazebo/brains/NeuralNetwork.h @@ -19,14 +19,14 @@ * */ -#ifndef REVOLVE_GAZEBO_BRAIN_NEURALNETWORK_H_ -#define REVOLVE_GAZEBO_BRAIN_NEURALNETWORK_H_ +#pragma once #include #include #include #include +#include #include @@ -53,8 +53,7 @@ namespace revolve SUPG }; - class NeuralNetwork - : public Brain + class NeuralNetwork : public ::revolve::Controller { /// \brief Constructor /// \param[in] _modelName Name of the robot @@ -75,11 +74,11 @@ namespace revolve /// \param[in] _sensors Sensor list /// \param[in] _time Current world time /// \param[in] _step Current time step - public: virtual void Update( + public: void update( const std::vector< MotorPtr > &_motors, const std::vector< SensorPtr > &_sensors, - const double _time, - const double _step); + double _time, + double _step) override; /// \brief Steps the neural network protected: void Step(const double _time); @@ -153,5 +152,3 @@ namespace revolve }; } /* namespace gazebo */ } /* namespace revolve */ - -#endif /* REVOLVE_GAZEBO_BRAIN_NEURALNETWORK_H_ */ diff --git a/cpprevolve/revolve/gazebo/brains/RLPower.cpp b/cpprevolve/revolve/gazebo/brains/RLPower.cpp index 527ced5c72..2904faa43b 100644 --- a/cpprevolve/revolve/gazebo/brains/RLPower.cpp +++ b/cpprevolve/revolve/gazebo/brains/RLPower.cpp @@ -41,16 +41,13 @@ RLPower::RLPower( const ::gazebo::physics::ModelPtr &_model, const sdf::ElementPtr &_settings, const std::vector< MotorPtr > &_motors, - const std::vector< SensorPtr > &_sensors) - : generationCounter_(0) + const std::vector< SensorPtr > &/*_sensors*/) + : Controller(ControllerType::SPLINES) + , generationCounter_(0) , cycleStartTime_(-1) - , startTime_(-1) , evaluationRate_(30.0) // default + , startTime_(-1) { - // Create transport node - this->node_.reset(new gz::transport::Node()); - this->node_->Init(); - auto learner_settings = _settings->GetElement("rv:learner"); this->robot_ = _model; @@ -82,7 +79,7 @@ RLPower::RLPower( RLPower::~RLPower() = default; ///////////////////////////////////////////////// -void RLPower::Update( +void RLPower::update( const std::vector< MotorPtr > &_motors, const std::vector< SensorPtr > &/* _sensors */, double _time, @@ -114,8 +111,8 @@ void RLPower::Update( auto p = 0; for (const auto &motor: _motors) { - motor->Update(&output[p], _step); - p += motor->Outputs(); + motor->write(&output[p], _step); + p += motor->n_outputs(); } auto currPosition = this->robot_->WorldPose(); diff --git a/cpprevolve/revolve/gazebo/brains/RLPower.h b/cpprevolve/revolve/gazebo/brains/RLPower.h index 2e49c0e9c7..c14e5bf195 100644 --- a/cpprevolve/revolve/gazebo/brains/RLPower.h +++ b/cpprevolve/revolve/gazebo/brains/RLPower.h @@ -30,6 +30,7 @@ #include #include +#include #include #include "Evaluator.h" @@ -39,8 +40,7 @@ namespace revolve { namespace gazebo { - class RLPower - : public Brain + class RLPower : public ::revolve::Controller { typedef const std::shared_ptr ConstModifyPolicyPtr; @@ -82,7 +82,7 @@ namespace revolve /// \param[in] _sensors: vector list of robot's sensors /// \param[in] _time: /// \param[in] _step: - public: void Update( + public: void update( const std::vector< MotorPtr > &_motors, const std::vector< SensorPtr > &_sensors, double _time, diff --git a/cpprevolve/revolve/gazebo/brains/ThymioBrain.cpp b/cpprevolve/revolve/gazebo/brains/ThymioBrain.cpp index ff3c93b8ad..2dac088fbd 100644 --- a/cpprevolve/revolve/gazebo/brains/ThymioBrain.cpp +++ b/cpprevolve/revolve/gazebo/brains/ThymioBrain.cpp @@ -35,6 +35,7 @@ ThymioBrain::ThymioBrain( sdf::ElementPtr /* _node */, std::vector< MotorPtr > &/* _motors */, std::vector< SensorPtr > &/* _sensors */) + : Brain() { std::cout << "Hello!" << std::endl; this->robot_ = _model; @@ -66,7 +67,7 @@ void ThymioBrain::Update( auto p = 0; for (const auto &motor: _motors) { - motor->Update(&output[p], _step); - p += motor->Outputs(); + motor->write(&output[p], _step); + p += motor->n_outputs(); } } diff --git a/cpprevolve/revolve/gazebo/brains/ThymioBrain.h b/cpprevolve/revolve/gazebo/brains/ThymioBrain.h index b270b0bb71..eaf300ed95 100644 --- a/cpprevolve/revolve/gazebo/brains/ThymioBrain.h +++ b/cpprevolve/revolve/gazebo/brains/ThymioBrain.h @@ -30,7 +30,7 @@ namespace revolve namespace gazebo { class ThymioBrain - : public Brain + : public ::revolve::gazebo::Brain { /// \brief The RLPower constructor reads out configuration file, /// deretmines which algorithm type to apply and initialises new policy. diff --git a/cpprevolve/revolve/gazebo/motors/ActuatorWrapper.h b/cpprevolve/revolve/gazebo/motors/ActuatorWrapper.h new file mode 100644 index 0000000000..aa80b3762e --- /dev/null +++ b/cpprevolve/revolve/gazebo/motors/ActuatorWrapper.h @@ -0,0 +1,40 @@ +// +// Created by Matteo De Carlo on 11/09/2019. +// + +#ifndef REVOLVE_ACTUATORWRAPPER_H +#define REVOLVE_ACTUATORWRAPPER_H + + +#include +#include "Motor.h" + +namespace revolve +{ +namespace gazebo +{ + +class ActuatorWrapper: public revolve::Actuator +{ +public: + explicit ActuatorWrapper(Motor *wrapped_actuator, double x, double y, double z) + : revolve::Actuator(wrapped_actuator->Outputs(), x, y, z) + , wrapped_actuator(wrapped_actuator) + { + assert(wrapped_actuator); + } + + void write(const double *output, double step) override + { + wrapped_actuator->write(output, step); + } + +private: + Motor *wrapped_actuator; +}; + +} +} + + +#endif //REVOLVE_ACTUATORWRAPPER_H diff --git a/cpprevolve/revolve/gazebo/motors/JointMotor.cpp b/cpprevolve/revolve/gazebo/motors/JointMotor.cpp index dfbf9694dc..16268ed429 100644 --- a/cpprevolve/revolve/gazebo/motors/JointMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/JointMotor.cpp @@ -31,8 +31,9 @@ JointMotor::JointMotor( const std::string &_partId, const std::string &_motorId, sdf::ElementPtr _motor, - const unsigned int _outputs) - : Motor(_model, _partId, _motorId, _outputs) + const unsigned int _outputs, + const std::string &_coordinates) + : Motor(_model, _partId, _motorId, _outputs, _coordinates) { if (not _motor->HasAttribute("joint")) { diff --git a/cpprevolve/revolve/gazebo/motors/JointMotor.h b/cpprevolve/revolve/gazebo/motors/JointMotor.h index de780c4d8e..251cf85e14 100644 --- a/cpprevolve/revolve/gazebo/motors/JointMotor.h +++ b/cpprevolve/revolve/gazebo/motors/JointMotor.h @@ -42,7 +42,8 @@ namespace revolve const std::string &_partId, const std::string &_motorId, sdf::ElementPtr _motor, - const unsigned int _outputs); + const unsigned int _outputs, + const std::string &_coordinates); /// \brief Destructor public: virtual ~JointMotor(); @@ -52,6 +53,9 @@ namespace revolve /// \brief Scoped name of the controlled joint protected: std::string jointName_; + + + public: void write(const double *output, double step){throw std::logic_error("write() not implemented");} }; } /* namespace gazebo */ } /* namespace revolve */ diff --git a/cpprevolve/revolve/gazebo/motors/Motor.cpp b/cpprevolve/revolve/gazebo/motors/Motor.cpp index 59b95f48b6..13b79f9a6e 100644 --- a/cpprevolve/revolve/gazebo/motors/Motor.cpp +++ b/cpprevolve/revolve/gazebo/motors/Motor.cpp @@ -20,22 +20,36 @@ #include #include +#include namespace gz = gazebo; using namespace revolve::gazebo; ///////////////////////////////////////////////// -Motor::Motor( +Motor::Motor ( ::gazebo::physics::ModelPtr _model, const std::string &_partId, const std::string &_motorId, - const unsigned int outputNeurons) - : model_(_model) + const unsigned int outputNeurons, + const std::string &_coordinates) + : revolve::Actuator(outputNeurons, 0, 0, 0) + , model_(std::move(_model)) , partId_(_partId) , motorId_(_motorId) - , outputs_(outputNeurons) { + // TODO conversion crashes if coordinates string is empty + std::vector coordinates; + double d_coordinates[3] = {0,0,0}; + + boost::split(coordinates, _coordinates, boost::is_any_of(";")); + + for(int i=0; icoordinates = std::tuple{d_coordinates[0], d_coordinates[1], d_coordinates[2]}; } ///////////////////////////////////////////////// @@ -50,7 +64,7 @@ std::string Motor::PartId() ///////////////////////////////////////////////// unsigned int Motor::Outputs() { - return this->outputs_; + return this->n_outputs(); } ///////////////////////////////////////////////// diff --git a/cpprevolve/revolve/gazebo/motors/Motor.h b/cpprevolve/revolve/gazebo/motors/Motor.h index 3441f61beb..6bc084f687 100644 --- a/cpprevolve/revolve/gazebo/motors/Motor.h +++ b/cpprevolve/revolve/gazebo/motors/Motor.h @@ -26,23 +26,25 @@ #include #include +#include namespace revolve { namespace gazebo { - class Motor + class Motor : public ::revolve::Actuator { /// \brief Constructor /// \brief[in] _model Model identifier /// \brief[in] _partId Module identifier /// \brief[in] _motorId Motor identifier /// \brief[in] _outputs Number of motor outputs - public: Motor( + public: Motor ( ::gazebo::physics::ModelPtr _model, const std::string &_partId, const std::string &_motorId, - const unsigned int _outputs); + unsigned int _outputs, + const std::string &_coordinates); /// \brief Destructor public: virtual ~Motor(); @@ -54,7 +56,7 @@ namespace revolve /// array of values, out of which the motor should read the first `n` /// values if it specifies `n` outputs. /// \param[in] step Actuation time in seconds - public: virtual void Update( + public: void write( const double *_output, double _step) = 0; @@ -81,9 +83,6 @@ namespace revolve /// \brief Robot-wide unique motor ID protected: std::string motorId_; - - /// \brief Number of output neurons that should be connected to the motor. - protected: unsigned int outputs_; }; } /* namespace gazebo */ } /* namespace tol_robogen */ diff --git a/cpprevolve/revolve/gazebo/motors/MotorFactory.cpp b/cpprevolve/revolve/gazebo/motors/MotorFactory.cpp index 0aee478484..78a586b088 100644 --- a/cpprevolve/revolve/gazebo/motors/MotorFactory.cpp +++ b/cpprevolve/revolve/gazebo/motors/MotorFactory.cpp @@ -40,16 +40,18 @@ MotorPtr MotorFactory::Motor( sdf::ElementPtr _motorSdf, const std::string &_type, const std::string &_partId, - const std::string &_motorId) + const std::string &_motorId, + const std::string &_coordinates) { + MotorPtr motor; if ("position" == _type) { - motor.reset(new PositionMotor(this->model_, _partId, _motorId, _motorSdf)); + motor.reset(new PositionMotor(this->model_, _partId, _motorId, _motorSdf, _coordinates)); } else if ("velocity" == _type) { - motor.reset(new VelocityMotor(this->model_, _partId, _motorId, _motorSdf)); + motor.reset(new VelocityMotor(this->model_, _partId, _motorId, _motorSdf, _coordinates)); } return motor; @@ -58,23 +60,23 @@ MotorPtr MotorFactory::Motor( ///////////////////////////////////////////////// MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf) { + auto coordinates = _motorSdf->GetAttribute("coordinates"); auto typeParam = _motorSdf->GetAttribute("type"); auto partIdParam = _motorSdf->GetAttribute("part_id"); // auto partNameParam = _motorSdf->GetAttribute("part_name"); auto idParam = _motorSdf->GetAttribute("id"); - if (not typeParam or not partIdParam or not idParam) - { - std::cerr << "Motor is missing required attributes (`id`, `type` or " - "`part_id`)." << std::endl; - throw std::runtime_error("Motor error"); - } + if (coordinates == nullptr) throw std::runtime_error("Motor coordinates not found"); + if (typeParam == nullptr) throw std::runtime_error("Motor typeParam not found"); + if (partIdParam == nullptr) throw std::runtime_error("Motor partIdParam not found"); + if (idParam == nullptr) throw std::runtime_error("Motor idParam not found"); // auto partName = partNameParam->GetAsString(); auto partId = partIdParam->GetAsString(); auto type = typeParam->GetAsString(); auto id = idParam->GetAsString(); - MotorPtr motor = this->Motor(_motorSdf, type, partId, id); + auto coord = coordinates->GetAsString(); + MotorPtr motor = this->Motor(_motorSdf, type, partId, id, coord); if (not motor) { diff --git a/cpprevolve/revolve/gazebo/motors/MotorFactory.h b/cpprevolve/revolve/gazebo/motors/MotorFactory.h index f9863d7151..82abdc66b1 100644 --- a/cpprevolve/revolve/gazebo/motors/MotorFactory.h +++ b/cpprevolve/revolve/gazebo/motors/MotorFactory.h @@ -26,6 +26,7 @@ #include #include +#include namespace revolve { @@ -47,11 +48,12 @@ namespace revolve /// ID and type. This is the convenience wrapper over `create` that has /// required attributes already checked, usually you should override /// this when adding new motor types. - public: virtual MotorPtr Motor( + public: virtual std::shared_ptr Motor( sdf::ElementPtr _motorSdf, const std::string &_type, const std::string &_partId, - const std::string &_motorId); + const std::string &_motorId, + const std::string &_coordinates); /// \brief Creates a motor for the given model for the given SDF element. public: virtual MotorPtr Create(sdf::ElementPtr _motorSdf); diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp index 63a33724fe..744cf67d5b 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -18,7 +18,7 @@ #include #include - +#include #include "PositionMotor.h" namespace gz = gazebo; @@ -27,44 +27,45 @@ using namespace revolve::gazebo; ///////////////////////////////////////////////// PositionMotor::PositionMotor( - gz::physics::ModelPtr _model, - const std::string &_partId, - const std::string &_motorId, - const sdf::ElementPtr _motor) - : JointMotor(std::move(_model), _partId, _motorId, _motor, 1) - , positionTarget_(0) - , noise_(0) + gz::physics::ModelPtr _model, + const std::string &_partId, + const std::string &_motorId, + const sdf::ElementPtr _motor, + const std::string &_coordinates) + : JointMotor(std::move(_model), _partId, _motorId, _motor, 1, _coordinates) + , positionTarget_(0) + , noise_(0) { - // Retrieve upper / lower limit from joint set in parent constructor - // Truncate ranges to [-pi, pi] - this->upperLimit_ = std::fmin(M_PI, this->joint_->UpperLimit(0)); - this->lowerLimit_ = std::fmax(-M_PI, this->joint_->LowerLimit(0)); - this->fullRange_ = ((this->upperLimit_ - this->lowerLimit_ + 1e-12) >= - (2 * M_PI)); - - if (_motor->HasElement("rv:pid")) - { - auto pidElem = _motor->GetElement("rv:pid"); - this->pid_ = Motor::CreatePid(pidElem); - } - - auto noise = _motor->GetAttribute("noise"); - if (noise) - { - noise->Get(this->noise_); - } - - // I've asked this question at the Gazebo forums: - // http://answers.gazebosim.org/question/9071/joint-target-velocity-with-maximum-force/ - // Until it is answered I'm resorting to calling ODE functions directly - // to get this to work. This will result in some deprecation warnings. - // It has the added benefit of not requiring the world update - // connection though. - // updateConnection_ = gz::event::Events::ConnectWorldUpdateBegin(boost::bind( - // &PositionMotor::OnUpdate, this, _1)); - - auto maxEffort = joint_->GetEffortLimit(0); - joint_->SetParam("fmax", 0, maxEffort); + // Retrieve upper / lower limit from joint set in parent constructor + // Truncate ranges to [-pi, pi] + this->upperLimit_ = std::fmin(M_PI, this->joint_->UpperLimit(0)); + this->lowerLimit_ = std::fmax(-M_PI, this->joint_->LowerLimit(0)); + this->fullRange_ = ((this->upperLimit_ - this->lowerLimit_ + 1e-12) >= + (2 * M_PI)); + + if (_motor->HasElement("rv:pid")) + { + auto pidElem = _motor->GetElement("rv:pid"); + this->pid_ = Motor::CreatePid(pidElem); + } + + auto noise = _motor->GetAttribute("noise"); + if (noise) + { + noise->Get(this->noise_); + } + + // I've asked this question at the Gazebo forums: + // http://answers.gazebosim.org/question/9071/joint-target-velocity-with-maximum-force/ + // Until it is answered I'm resorting to calling ODE functions directly + // to get this to work. This will result in some deprecation warnings. + // It has the added benefit of not requiring the world update + // connection though. + // updateConnection_ = gz::event::Events::ConnectWorldUpdateBegin(boost::bind( + // &PositionMotor::OnUpdate, this, _1)); + + auto maxEffort = joint_->GetEffortLimit(0); + joint_->SetParam("fmax", 0, maxEffort); } ///////////////////////////////////////////////// @@ -75,60 +76,86 @@ PositionMotor::~PositionMotor() = default; // DoUpdate(info.simTime); // } +double PositionMotor::Current_State( Actuator::StateType type) +{ + if (type==0) + { + return this->joint_->Position(0); + } + else if (type == 1) + { + return this->joint_->GetVelocity(0); + } + else if (type == 2) + { + return this->joint_->GetForce(0); + } +} + ///////////////////////////////////////////////// -void PositionMotor::Update( - const double *outputs, - double /*step*/) +void PositionMotor::write( + const double *outputs, + double /*step*/) { - // Just one network output, which is the first - auto output = outputs[0]; - - // Motor noise in range +/- noiseLevel * actualValue - output += ((2 * ignition::math::Rand::DblUniform() * this->noise_) - - this->noise_) * - output; - - // Truncate output to [0, 1] - // Note: Don't actually target the full joint range, this way a low update - // rate won't mess with the joint constraints as much leading to a more - // stable system. - output = std::fmin(std::fmax(1e-5, output), 0.99999); - this->positionTarget_ = this->lowerLimit_ + - (output * (this->upperLimit_ - this->lowerLimit_)); - - // Perform the actual motor update - this->DoUpdate(this->joint_->GetWorld()->SimTime()); + // Just one network output, which is the first + auto output = outputs[0]; + + // Motor noise in range +/- noiseLevel * actualValue + output += ((2 * ignition::math::Rand::DblUniform() * this->noise_) - + this->noise_) * + output; + + // Truncate output to [0, 1] + // Note: Don't actually target the full joint range, this way a low update + // rate won't mess with the joint constraints as much leading to a more + // stable system. + output = std::fmin(std::fmax(1e-5, output), 0.99999); +// this->positionTarget_ = this->lowerLimit_ + +// (output * (this->upperLimit_ - this->lowerLimit_)); + + this->positionTarget_ = output*2-1;//2*5.235988-5.235988; + // Perform the actual motor update + this->DoUpdate(this->joint_->GetWorld()->SimTime()); } ///////////////////////////////////////////////// void PositionMotor::DoUpdate(const ::gazebo::common::Time &_simTime) { - auto stepTime = _simTime - this->prevUpdateTime_; - if (stepTime <= 0) - { - // Only respond to positive step times - return; - } - - this->prevUpdateTime_ = _simTime; - auto position = this->joint_->Position(0); - - // TODO Make sure normalized angle lies within possible range - // I get the feeling we might be moving motors outside their - // allowed range. Also something to be aware of when setting - // the direction. - - if (this->fullRange_ and std::fabs(position - positionTarget_) > M_PI) - { - // Both the position and the position target will be in the range - // [-pi, pi]. For a full range of motion joint, using an angle +- 2 PI - // might result in a much shorter required movement. In this case we - // best correct the current position to something outside the range. - position += (position > 0 ? -2 * M_PI : 2 * M_PI); - } - - auto error = position - this->positionTarget_; - auto cmd = this->pid_.Update(error, stepTime); - - this->joint_->SetParam("vel", 0, cmd); + auto stepTime = _simTime - this->prevUpdateTime_; + if (stepTime <= 0) + { + // Only respond to positive step times + return; + } + + this->prevUpdateTime_ = _simTime; + auto position = this->joint_->Position(0); + + // TODO Make sure normalized angle lies within possible range + // I get the feeling we might be moving motors outside their + // allowed range. Also something to be aware of when setting + // the direction. + + if (this->fullRange_ and std::fabs(position - positionTarget_) > M_PI) + { + // Both the position and the position target will be in the range + // [-pi, pi]. For a full range of motion joint, using an angle +- 2 PI + // might result in a much shorter required movement. In this case we + // best correct the current position to something outside the range. + position += (position > 0 ? -2 * M_PI : 2 * M_PI); + } + const double mean = 0.0; + const double stddev = 0.05; + std::default_random_engine generator; + auto dist = std::bind(std::normal_distribution{mean, stddev}, + std::mt19937(std::random_device{}())); +// std::normal_distribution dist(mean, stddev); // + auto error = (position - this->positionTarget_); + auto cmd = this->pid_.Update(error, stepTime)/stepTime.Double(); +// auto cmd = this->positionTarget_;//##################################### + auto velLimit = joint_->GetVelocityLimit(0); + cmd = std::fmax(-velLimit,std::fmin(velLimit,cmd)); + + double pert = dist()*velLimit; + this->joint_->SetParam("vel", 0, cmd); } diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.h b/cpprevolve/revolve/gazebo/motors/PositionMotor.h index 7f07c831a5..4fdb0ab95a 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.h +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.h @@ -44,16 +44,19 @@ namespace revolve ::gazebo::physics::ModelPtr _model, const std::string &_partId, const std::string &_motorId, - const sdf::ElementPtr _motor); + const sdf::ElementPtr _motor, + const std::string &_coordinates); /// \brief Destructor public: virtual ~PositionMotor() override; /// \brief - public: virtual void Update( + public: virtual void write( const double *_outputs, double _step) override; + public: virtual double Current_State( Actuator::StateType type ) override ; + /// \brief World update event function // protected: void OnUpdate(const ::gazebo::common::UpdateInfo info); diff --git a/cpprevolve/revolve/gazebo/motors/VelocityMotor.cpp b/cpprevolve/revolve/gazebo/motors/VelocityMotor.cpp index 9a45596621..0d286925a3 100644 --- a/cpprevolve/revolve/gazebo/motors/VelocityMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/VelocityMotor.cpp @@ -30,8 +30,9 @@ VelocityMotor::VelocityMotor( ::gazebo::physics::ModelPtr _model, const std::string &_partId, const std::string &_motorId, - sdf::ElementPtr _motor) - : JointMotor(_model, _partId, _motorId, _motor, 1) + sdf::ElementPtr _motor, + const std::string &_coordinates) + : JointMotor(_model, _partId, _motorId, _motor, 1, _coordinates) , velocityTarget_(0) , noise_(0) { @@ -66,7 +67,19 @@ VelocityMotor::~VelocityMotor() { } -void VelocityMotor::Update( +double VelocityMotor::Current_State( Actuator::StateType type) { + if (type == 0) { + return this->joint_->Position(0); + } else if (type == 1) { + return this->joint_->GetVelocity(0); + } + else if (type == 2) + { + return this->joint_->GetForce(0); + } +} + +void VelocityMotor::write( const double *outputs, double /*step*/) { @@ -89,4 +102,12 @@ void VelocityMotor::DoUpdate(const ::gazebo::common::Time &/*simTime*/) // I'm caving for now and am setting ODE parameters directly. // See https://tinyurl.com/y7he7y8l this->joint_->SetParam("vel", 0, this->velocityTarget_); +// this->pid_.S + this->model_->GetJointController()->SetVelocityPID( + this->joint_->GetScopedName(),this->pid_); + + this->model_->GetJointController()->SetVelocityTarget( + this->joint_->GetScopedName(),this->velocityTarget_); + + } diff --git a/cpprevolve/revolve/gazebo/motors/VelocityMotor.h b/cpprevolve/revolve/gazebo/motors/VelocityMotor.h index 3a978c4922..ee4ad03cea 100644 --- a/cpprevolve/revolve/gazebo/motors/VelocityMotor.h +++ b/cpprevolve/revolve/gazebo/motors/VelocityMotor.h @@ -26,67 +26,67 @@ #include -namespace revolve -{ - namespace gazebo - { - class VelocityMotor - : public JointMotor - { - public: - /// \brief Constructor - /// \param model The model the motor is contained in - /// \param The joint driven by the motor - /// \param partId The part ID the motor belongs to - /// \param motorId Whether the motor is velocity driven (the - /// alternative is position driven) - /// \param motor The derivative gain of the motor's PID controller - public: VelocityMotor( +namespace revolve { +namespace gazebo { +class VelocityMotor : public JointMotor { +public: + /// \brief Constructor + /// \param model The model the motor is contained in + /// \param The joint driven by the motor + /// \param partId The part ID the motor belongs to + /// \param motorId Whether the motor is velocity driven (the + /// alternative is position driven) + /// \param motor The derivative gain of the motor's PID controller + VelocityMotor( ::gazebo::physics::ModelPtr _model, const std::string &_partId, const std::string &_motorId, - sdf::ElementPtr _motor); + sdf::ElementPtr _motor, + const std::string &_coordinates); - /// \brief Destructor - public: virtual ~VelocityMotor(); + /// \brief Destructor + ~VelocityMotor() override; - /// \brief The velocity motor update action takes an output between 0 - /// and 1 and converts it to a velocity target between the minimum and - /// maximum velocity set by the motor. - /// \param[in,out] outputs - /// \param[in] step - virtual void Update( - const double *outputs, - double step); + /// \brief The velocity motor update action takes an output between 0 + /// and 1 and converts it to a velocity target between the minimum and + /// maximum velocity set by the motor. + /// \param[in,out] outputs + /// \param[in] step + void write( + const double *outputs, + double step) override; - /// \brief World update event function -// protected: void OnUpdate(const ::gazebo::common::UpdateInfo info); + double Current_State(Actuator::StateType type) override; - /// \brief Perform the actual update given the step size - protected: void DoUpdate(const ::gazebo::common::Time &simTime); +protected: +/// \brief World update event function +// void OnUpdate(const ::gazebo::common::UpdateInfo info); - /// \brief Store update event pointer -// protected: ::gazebo::event::ConnectionPtr updateConnection_; + /// \brief Perform the actual update given the step size + void DoUpdate(const ::gazebo::common::Time &simTime); - /// \brief Last update time, used to determine update step time - protected: ::gazebo::common::Time prevUpdateTime_; + /// \brief Store update event pointer +// ::gazebo::event::ConnectionPtr updateConnection_; - /// \brief The current velocity target - protected: double velocityTarget_; + /// \brief Last update time, used to determine update step time + ::gazebo::common::Time prevUpdateTime_; - /// \brief Velocity limits - protected: double minVelocity_; + /// \brief The current velocity target + double velocityTarget_; - /// \brief - protected: double maxVelocity_; + /// \brief Velocity limits + double minVelocity_; - /// \brief Motor noise - protected: double noise_; + /// \brief + double maxVelocity_; - /// \brief PID for this velocity motor - protected: ::gazebo::common::PID pid_; - }; - } // namespace gazebo + /// \brief Motor noise + double noise_; + + /// \brief PID for this velocity motor + ::gazebo::common::PID pid_; +}; +} // namespace gazebo } // namespace revolve #endif // REVOLVE_VELOCITYMOTOR_H diff --git a/cpprevolve/revolve/gazebo/msgs/robot_states.proto b/cpprevolve/revolve/gazebo/msgs/robot_states.proto index 60f77dce0f..e85cde33b1 100644 --- a/cpprevolve/revolve/gazebo/msgs/robot_states.proto +++ b/cpprevolve/revolve/gazebo/msgs/robot_states.proto @@ -2,12 +2,21 @@ syntax = "proto2"; package revolve.msgs; import "time.proto"; import "pose.proto"; +import "vector3d.proto"; + +message Orientation { + optional gazebo.msgs.Vector3d vec_forward = 1; + optional gazebo.msgs.Vector3d vec_left = 2; + optional gazebo.msgs.Vector3d vec_back = 3; + optional gazebo.msgs.Vector3d vec_right = 4; +} message RobotState { required uint32 id = 1; required string name = 2; required gazebo.msgs.Pose pose = 3; optional bool dead = 4; + optional Orientation orientation_vecs = 5; } message RobotStates { diff --git a/cpprevolve/revolve/gazebo/plugin/BodyAnalyzer.cpp b/cpprevolve/revolve/gazebo/plugin/BodyAnalyzer.cpp index 41ea335fcc..0a83b29689 100644 --- a/cpprevolve/revolve/gazebo/plugin/BodyAnalyzer.cpp +++ b/cpprevolve/revolve/gazebo/plugin/BodyAnalyzer.cpp @@ -184,6 +184,14 @@ void BodyAnalyzer::OnContacts(ConstContactsPtr &msg) // My suggested fixes are present in the gazebo6-revolve branch auto bbox = model->BoundingBox(); auto box = response.mutable_boundingbox(); + + for (const auto &link: model->GetLinks()) + { + ignition::math::Vector3d pos = link->WorldPose().Pos(); + bbox.Min().Min(pos); + bbox.Max().Max(pos); + } + gz::msgs::Set(box->mutable_min(), bbox.Min()); gz::msgs::Set(box->mutable_max(), bbox.Max()); diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index a02aac322c..5966630a0b 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -17,13 +17,15 @@ * */ -#include +#include #include #include #include +#include #include +#include #include "RobotController.h" @@ -53,56 +55,65 @@ void RobotController::Load( ::gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf) { - // Store the pointer to the model / world - this->model_ = _parent; - this->world_ = _parent->GetWorld(); - this->initTime_ = this->world_->SimTime().Double(); - - // Create transport node - this->node_.reset(new gz::transport::Node()); - this->node_->Init(); - - // Subscribe to robot battery state updater - this->batterySetSub_ = this->node_->Subscribe( - "~/battery_level/request", - &RobotController::UpdateBattery, - this); - this->batterySetPub_ = this->node_->Advertise< gz::msgs::Response >( - "~/battery_level/response"); - - if (not _sdf->HasElement("rv:robot_config")) + try { - std::cerr - << "No `rv:robot_config` element found, controller not initialized." - << std::endl; - return; - } + // Store the pointer to the model / world + this->model_ = _parent; + this->world_ = _parent->GetWorld(); + this->initTime_ = this->world_->SimTime().Double(); + + // Create transport node + this->node_.reset(new gz::transport::Node()); + this->node_->Init(); + + // Subscribe to robot battery state updater + this->batterySetSub_ = this->node_->Subscribe( + "~/battery_level/request", + &RobotController::UpdateBattery, + this); + this->batterySetPub_ = this->node_->Advertise( + "~/battery_level/response"); + + if (not _sdf->HasElement("rv:robot_config")) + { + std::cerr + << "No `rv:robot_config` element found, controller not initialized." + << std::endl; + return; + } - auto robotConfiguration = _sdf->GetElement("rv:robot_config"); + auto robotConfiguration = _sdf->GetElement("rv:robot_config"); - if (robotConfiguration->HasElement("rv:update_rate")) - { - auto updateRate = robotConfiguration->GetElement("rv:update_rate")->Get< double >(); - this->actuationTime_ = 1.0 / updateRate; - } + if (robotConfiguration->HasElement("rv:update_rate")) + { + auto updateRate = robotConfiguration->GetElement("rv:update_rate")->Get(); + this->actuationTime_ = 1.0 / updateRate; + } - // Load motors - this->motorFactory_ = this->MotorFactory(_parent); - this->LoadActuators(robotConfiguration); + // Load motors + this->motorFactory_ = this->MotorFactory(_parent); + this->LoadActuators(robotConfiguration); - // Load sensors - this->sensorFactory_ = this->SensorFactory(_parent); - this->LoadSensors(robotConfiguration); + // Load sensors + this->sensorFactory_ = this->SensorFactory(_parent); + this->LoadSensors(robotConfiguration); - // Load brain, this needs to be done after the motors and sensors so they - // can potentially be reordered. - this->LoadBrain(robotConfiguration); + // Load brain, this needs to be done after the motors and sensors so they + // can potentially be reordered. + this->LoadBrain(robotConfiguration); - // Call the battery loader - this->LoadBattery(robotConfiguration); + // Call the battery loader + this->LoadBattery(robotConfiguration); - // Call startup function which decides on actuation - this->Startup(_parent, _sdf); + // Call startup function which decides on actuation + this->Startup(_parent, _sdf); + } + catch (const std::exception &e) + { + std::cerr << "Error Loading the Robot Controller, exception: " << std::endl + << e.what() << std::endl; + throw; + } } ///////////////////////////////////////////////// @@ -136,8 +147,7 @@ void RobotController::UpdateBattery(ConstRequestPtr &_request) ///////////////////////////////////////////////// void RobotController::LoadActuators(const sdf::ElementPtr _sdf) { - if (not _sdf->HasElement("rv:brain") - or not _sdf->GetElement("rv:brain")->HasElement("rv:actuators")) + if (not _sdf->HasElement("rv:brain") or not _sdf->GetElement("rv:brain")->HasElement("rv:actuators")) { return; } @@ -159,8 +169,7 @@ void RobotController::LoadActuators(const sdf::ElementPtr _sdf) ///////////////////////////////////////////////// void RobotController::LoadSensors(const sdf::ElementPtr _sdf) { - if (not _sdf->HasElement("rv:brain") - or not _sdf->GetElement("rv:brain")->HasElement("rv:sensors")) + if (not _sdf->HasElement("rv:brain") or not _sdf->GetElement("rv:brain")->HasElement("rv:sensors")) { return; } @@ -200,29 +209,77 @@ void RobotController::LoadBrain(const sdf::ElementPtr _sdf) return; } - auto brain = _sdf->GetElement("rv:brain"); - auto controller = brain->GetElement("rv:controller")->GetAttribute("type")->GetAsString(); - auto learner = brain->GetElement("rv:learner")->GetAttribute("type")->GetAsString(); - std::cout << "Loading controller " << controller << " and learner " << learner << std::endl; + auto brain_sdf = _sdf->GetElement("rv:brain"); + auto controller_type = brain_sdf->GetElement("rv:controller")->GetAttribute("type")->GetAsString(); + auto learner = brain_sdf->GetElement("rv:learner")->GetAttribute("type")->GetAsString(); + std::cout << "Loading controller " << controller_type << " and learner " << learner << std::endl; - if ("offline" == learner and "ann" == controller) + if ("offline" == learner and "ann" == controller_type) { - brain_.reset(new NeuralNetwork(this->model_, brain, motors_, sensors_)); + brain_.reset(new NeuralNetwork(this->model_, brain_sdf, motors_, sensors_)); } - else if ("rlpower" == learner and "spline" == controller) + else if ("rlpower" == learner and "spline" == controller_type) { - if (not motors_.empty()) { - brain_.reset(new RLPower(this->model_, brain, motors_, sensors_)); + if (not motors_.empty()) + { + brain_.reset(new RLPower(this->model_, brain_sdf, motors_, sensors_)); } } - else if ("bo" == learner and "cpg" == controller) + else if ("bo" == learner and "cpg" == controller_type) + { + //WARNING! not doing BO any more + brain_.reset(new DifferentialCPG(_sdf, motors_)); + } + else if ("target" == learner and "cpg" == controller_type) + { + std::shared_ptr fake_target_sensor( + new GZAngleToTargetDetector(this->model_, ignition::math::Vector3d(0, 10, 0))); + brain_.reset(new DifferentialCPGClean(brain_sdf, motors_, fake_target_sensor)); + } + else if ("offline" == learner && "cpg-target" == controller_type) + { + std::vector target_vec; + std::string target_str = brain_sdf->GetElement("rv:controller")->GetAttribute("target")->GetAsString(); + std::string delimiter = ";"; + size_t pos = 0; + std::string token; + std::cout << "The generated target equals (" << target_str << ")" << std::endl; + while ((pos = target_str.find(delimiter)) != std::string::npos) + { + token = target_str.substr(0, pos); + target_vec.push_back(stod(token)); + target_str.erase(0, pos + delimiter.length()); + } + // push the last element that does not end with the delimiter + target_vec.push_back(stod(target_str)); + + ignition::math::Vector3d target(target_vec[0], target_vec[1], target_vec[2]); + + std::shared_ptr fake_target_sensor( + new GZAngleToTargetDetector(this->model_, target)); + brain_.reset(new DifferentialCPGClean(brain_sdf, motors_, fake_target_sensor)); + } + else if ("offline" == learner and "cpg" == controller_type) + { + brain_.reset(new DifferentialCPGClean(brain_sdf, motors_)); + } + else if ("offline" == learner and "cppn-cpg" == controller_type) + { + brain_.reset(new DifferentialCPPNCPG(brain_sdf, motors_)); + } + else if ("offline" == learner and "fixed-angle" == controller_type) { - brain_.reset(new DifferentialCPG(this->model_, _sdf, motors_, sensors_)); + double angle = std::stod( + brain_sdf->GetElement("rv:controller")->GetAttribute("angle")->GetAsString()); + brain_.reset(new FixedAngleController(angle)); } else { - throw std::runtime_error("Robot brain is not defined."); + std::ostringstream message; + message << "Robot brain is not defined. (learner='" << learner << "', controller='" << controller_type << "')"; + throw std::runtime_error(message.str()); } + std::cout << "Loaded controller " << controller_type << " and learner " << learner << std::endl; } ///////////////////////////////////////////////// @@ -254,7 +311,7 @@ void RobotController::DoUpdate(const ::gazebo::common::UpdateInfo _info) auto currentTime = _info.simTime.Double() - initTime_; if (brain_) - brain_->Update(motors_, sensors_, currentTime, actuationTime_); + brain_->update(motors_, sensors_, currentTime, actuationTime_); } ///////////////////////////////////////////////// @@ -274,7 +331,7 @@ double RobotController::BatteryLevel() return 0.0; } - return batteryElem_->GetElement("rv:level")->Get< double >(); + return batteryElem_->GetElement("rv:level")->Get(); } ///////////////////////////////////////////////// diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.h b/cpprevolve/revolve/gazebo/plugin/RobotController.h index 6fe8e7bd69..edb1c317ca 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.h +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.h @@ -29,9 +29,13 @@ #include #include +#include "revolve/brains/controller/sensors/Sensor.h" +#include "revolve/brains/controller/actuators/Actuator.h" namespace revolve { + class Controller; + namespace gazebo { class RobotController @@ -114,7 +118,7 @@ namespace revolve protected: SensorFactoryPtr sensorFactory_; /// \brief Brain controlling this model - protected: BrainPtr brain_; + protected: std::unique_ptr<::revolve::Controller> brain_; /// \brief Actuation time, in seconds protected: double actuationTime_; diff --git a/cpprevolve/revolve/gazebo/plugin/TorusWorld.cpp b/cpprevolve/revolve/gazebo/plugin/TorusWorld.cpp deleted file mode 100644 index aea9cd773c..0000000000 --- a/cpprevolve/revolve/gazebo/plugin/TorusWorld.cpp +++ /dev/null @@ -1,27 +0,0 @@ -// -// Created by matteo on 6/19/19. -// - -#include "TorusWorld.h" - -using namespace revolve::gazebo; - -TorusWorld::TorusWorld() -{ - -} - -TorusWorld::~TorusWorld() -{ - -} - -void TorusWorld::Load(::gazebo::physics::WorldPtr _parent, sdf::ElementPtr _sdf) -{ - -} - -void TorusWorld::OnUpdate(const ::gazebo::common::UpdateInfo &_info) -{ - -} diff --git a/cpprevolve/revolve/gazebo/plugin/TorusWorld.h b/cpprevolve/revolve/gazebo/plugin/TorusWorld.h deleted file mode 100644 index 8556a58732..0000000000 --- a/cpprevolve/revolve/gazebo/plugin/TorusWorld.h +++ /dev/null @@ -1,39 +0,0 @@ -// -// Created by matteo on 6/19/19. -// - -#ifndef REVOLVE_TORUSWORLD_H -#define REVOLVE_TORUSWORLD_H - -#include -#include -#include -#include - - -namespace revolve { -namespace gazebo { - -class TorusWorld : public ::gazebo::WorldPlugin { -public: - TorusWorld(); - ~TorusWorld() override; - - virtual void Load( - ::gazebo::physics::WorldPtr _parent, - sdf::ElementPtr _sdf) override; - - virtual void OnUpdate(const ::gazebo::common::UpdateInfo &_info); - -private: - - // Pointer to the update event connection - ::gazebo::event::ConnectionPtr onBeginUpdateConnection; - ::gazebo::event::ConnectionPtr onEndUpdateConnection; -}; - -} -} - - -#endif //REVOLVE_TORUSWORLD_H diff --git a/cpprevolve/revolve/gazebo/plugin/WorldController.cpp b/cpprevolve/revolve/gazebo/plugin/WorldController.cpp index ea491681d7..18fe1f0718 100644 --- a/cpprevolve/revolve/gazebo/plugin/WorldController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/WorldController.cpp @@ -21,8 +21,9 @@ #include "WorldController.h" -namespace gz = gazebo; +//#define USE_MARKERS +namespace gz = gazebo; using namespace revolve::gazebo; ///////////////////////////////////////////////// @@ -122,8 +123,91 @@ void WorldController::Reset() this->lastRobotStatesUpdateTime_ = 0; //this->world_->SimTime().Double(); } + +enum Orientation { + FORWARD = 0, + LEFT = 1, + BACK = 2, + RIGHT = 3, +}; + +ignition::math::Pose3d generateMarkerPose(const Orientation index, const ignition::math::Pose3d &robotPose) +{ + static const std::array markerOffset { + ignition::math::Vector3d { 0 , -1, 0}, // Forward + ignition::math::Vector3d { 1 , 0, 0}, // Left + ignition::math::Vector3d { 0 , 1, 0}, // Backwards + ignition::math::Vector3d {-1 , 0, 0}, // Right + }; + + assert(index < 4); + return ignition::math::Pose3d( + robotPose.CoordPositionAdd(markerOffset[index]), + ignition::math::Quaterniond::Identity + ); + +} + + +void fillMessages(const Orientation orientation, + const ignition::math::Pose3d &worldPose, + ignition::msgs::Marker &markerMsg, + ::revolve::msgs::Orientation* orientationVecs, + bool materials = false) +{ + ignition::math::Pose3d markerPose = generateMarkerPose(orientation, worldPose); + ignition::math::Vector3d orientation_vec = markerPose.Pos() - worldPose.Pos(); + + switch (orientation) { + case FORWARD: + gz::msgs::Set(orientationVecs->mutable_vec_forward(), orientation_vec); + break; + case LEFT: + gz::msgs::Set(orientationVecs->mutable_vec_left(), orientation_vec); + break; + case BACK: + gz::msgs::Set(orientationVecs->mutable_vec_back(), orientation_vec); + break; + case RIGHT: + gz::msgs::Set(orientationVecs->mutable_vec_right(), orientation_vec); + break; + default: + assert(false); + } + +#ifdef USE_MARKERS + // absolute position + //ignition::msgs::Set(markerMsg.mutable_pose(), markerPose); + // relative vector + ignition::msgs::Set(markerMsg.mutable_pose(), + ignition::math::Pose3d(orientation_vec, ignition::math::Quaterniond::Identity)); + + if (materials) { + ignition::msgs::Material *matMsg = markerMsg.mutable_material(); + switch (orientation) { + case FORWARD: + matMsg->mutable_script()->set_name("Gazebo/BlueLaser"); + break; + case LEFT: + matMsg->mutable_script()->set_name("Gazebo/Red"); + break; + case BACK: + matMsg->mutable_script()->set_name("Gazebo/Yellow"); + break; + case RIGHT: + matMsg->mutable_script()->set_name("Gazebo/Green"); + break; + default: + assert(false); + } + } +#endif +} + + ///////////////////////////////////////////////// -void WorldController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) { +void WorldController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) +{ if (not this->robotStatesPubFreq_) { return; } @@ -136,9 +220,17 @@ void WorldController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) { msgs::RobotStates msg; gz::msgs::Set(msg.mutable_time(), _info.simTime); + // MARKER MESSAGE + ignition::msgs::Marker markerMsg; +#ifdef USE_MARKERS + ::ignition::transport::Node ignNode; + markerMsg.set_ns("revolve"); + markerMsg.set_action(ignition::msgs::Marker::ADD_MODIFY); +#endif + { boost::recursive_mutex::scoped_lock lock_physics(*this->world_->Physics()->GetPhysicsUpdateMutex()); - for (const auto &model : this->world_->Models()) { + for (const boost::shared_ptr &model : this->world_->Models()) { if (model->IsStatic()) { // Ignore static models such as the ground and obstacles continue; @@ -150,9 +242,61 @@ void WorldController::OnBeginUpdate(const ::gazebo::common::UpdateInfo &_info) { stateMsg->set_id(model->GetId()); auto poseMsg = stateMsg->mutable_pose(); - auto relativePose = model->RelativePose(); + // relative pose and world pose are the same because the robot is relative to the world directly + //auto relativePose = model->RelativePose(); + auto worldPose = model->WorldPose(); + ::revolve::msgs::Orientation* orientationVecs = stateMsg->mutable_orientation_vecs(); + + // UPDATE/GENERATE MARKERS + + std::map>::iterator model_markers = markers_.find(model.get()); + if (model_markers == markers_.end()) { + std::array new_marker_ids { + markers_ids_, + markers_ids_+1, + markers_ids_+2, + markers_ids_+3 + }; + markers_ids_ += 4; +#ifdef USE_MARKERS + markerMsg.set_type(ignition::msgs::Marker::SPHERE); + + const double MARKER_SCALE = 0.05; + ignition::msgs::Set(markerMsg.mutable_scale(), + ignition::math::Vector3d(MARKER_SCALE,MARKER_SCALE,MARKER_SCALE)); +#endif + + int i = 0; + for (uint64_t marker_id : new_marker_ids) { + Orientation orientation = Orientation(i); + fillMessages(orientation, worldPose, markerMsg, orientationVecs, true); +#ifdef USE_MARKERS + markerMsg.set_id(marker_id); + assert(ignNode.Request("/marker", markerMsg)); +#endif + i++; + } + + bool success; + std::tie(model_markers, success) = markers_.emplace( + model.get(), + new_marker_ids + ); + assert(success); + } else { + int i = 0; + for (uint64_t marker_id : model_markers->second) { + Orientation orientation = Orientation(i); + fillMessages(orientation, worldPose, markerMsg, orientationVecs); +#ifdef USE_MARKERS + markerMsg.set_id(marker_id); + assert(ignNode.Request("/marker", markerMsg)); +#endif + i++; + } + } - gz::msgs::Set(poseMsg, relativePose); + gz::msgs::Set(poseMsg, worldPose); // Death sentence check const std::string name = model->GetName(); @@ -331,11 +475,15 @@ void WorldController::HandleRequest(ConstRequestPtr &request) { boost::mutex::scoped_lock lock(this->insertMutex_); - this->insertMap_[name] = std::make_tuple(request->id(), robotSDF.ToString(), true); + this->world_->InsertModelString(robotSDF.ToString()); + bool operation_pending = false; + this->insertMap_[name] = std::make_tuple( + request->id(), + robotSDF.ToString(), + operation_pending); } //TODO insert here, it's better - //this->world_->InsertModelString(robotSDF.ToString()); // Don't leak memory // https://bitbucket.org/osrf/sdformat/issues/104/memory-leak-in-element diff --git a/cpprevolve/revolve/gazebo/plugin/WorldController.h b/cpprevolve/revolve/gazebo/plugin/WorldController.h index 359badbd96..207159795c 100644 --- a/cpprevolve/revolve/gazebo/plugin/WorldController.h +++ b/cpprevolve/revolve/gazebo/plugin/WorldController.h @@ -128,6 +128,9 @@ class WorldController: public ::gazebo::WorldPlugin // boost::mutex world_insert_remove_mutex; ::gazebo::physics::Model_V models_to_remove; + + std::map<::gazebo::physics::Model*, std::array > markers_; + uint64_t markers_ids_ = 0; }; } // namespace gazebo diff --git a/cpprevolve/revolve/gazebo/plugin/register_torus_world_plugin.cpp b/cpprevolve/revolve/gazebo/plugin/register_torus_world_plugin.cpp deleted file mode 100644 index 71aaf3cc3f..0000000000 --- a/cpprevolve/revolve/gazebo/plugin/register_torus_world_plugin.cpp +++ /dev/null @@ -1,9 +0,0 @@ -// -// Created by matteo on 6/19/19. -// - -#include -#include "TorusWorld.h" - -using namespace gazebo; -GZ_REGISTER_WORLD_PLUGIN(revolve::gazebo::TorusWorld) diff --git a/cpprevolve/revolve/gazebo/sensors/BatterySensor.cpp b/cpprevolve/revolve/gazebo/sensors/BatterySensor.cpp deleted file mode 100644 index 9ce50c6ffc..0000000000 --- a/cpprevolve/revolve/gazebo/sensors/BatterySensor.cpp +++ /dev/null @@ -1,63 +0,0 @@ -/* -* Copyright (C) 2017 Vrije Universiteit Amsterdam -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -* -* Author: Elte Hupkes -* -*/ - -#include - -#include "BatterySensor.h" - -namespace gz = gazebo; - -using namespace revolve::gazebo; - -///////////////////////////////////////////////// -BatterySensor::BatterySensor( - ::gazebo::physics::ModelPtr _model, - const std::string &_partId, - const std::string &_sensorId) - : VirtualSensor(_model, _partId, _sensorId, 1) -{ - // Find the revolve plugin to get the battery data - auto modelSdf = _model->GetSDF(); - if (modelSdf->HasElement("plugin")) - { - auto pluginSdf = modelSdf->GetElement("plugin"); - while (pluginSdf) - { - if (pluginSdf->HasElement("rv:robot_config")) - { - // Found revolve plugin - auto settingsSdf = pluginSdf->GetElement("rv:robot_config"); - if (settingsSdf->HasElement("rv:battery")) - { - this->battery_ = settingsSdf->GetElement("rv:battery"); - } - - break; - } - pluginSdf = pluginSdf->GetNextElement("plugin"); - } - } -} - -///////////////////////////////////////////////// -void BatterySensor::Read(double *_input) -{ - _input[0] = this->battery_ and - (this->battery_->HasElement("rv:level") ? - this->battery_->GetElement("rv:level")->Get< double >() : 0.0); -} diff --git a/cpprevolve/revolve/gazebo/sensors/BatterySensor.h b/cpprevolve/revolve/gazebo/sensors/BatterySensor.h deleted file mode 100644 index 3be6509d80..0000000000 --- a/cpprevolve/revolve/gazebo/sensors/BatterySensor.h +++ /dev/null @@ -1,53 +0,0 @@ -/* -* Copyright (C) 2017 Vrije Universiteit Amsterdam -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -* -* Description: Sensor that reads the battery state out of the model SDF -* Author: Elte Hupkes -* -*/ - -#ifndef REVOLVE_BATTERYSENSOR_H -#define REVOLVE_BATTERYSENSOR_H - -#include - -#include "VirtualSensor.h" - -namespace revolve -{ - namespace gazebo - { - class BatterySensor - : public VirtualSensor - { - /// \brief Constructor - /// \brief[in] _model Model identifier - /// \brief[in] _partId Module identifier - /// \brief[in] _sensorId Sensor identifier - public: BatterySensor( - ::gazebo::physics::ModelPtr _model, - const std::string &_partId, - const std::string &_sensorId); - - /// \brief Reads the battery value - /// \param[in,out] _input: Input parameter of the sensor - public: virtual void Read(double *_input); - - /// \brief SDF battery - protected: sdf::ElementPtr battery_; - }; - } -} - -#endif // REVOLVE_BATTERYSENSOR_H diff --git a/cpprevolve/revolve/gazebo/sensors/GZAngleToTargetDetector.cpp b/cpprevolve/revolve/gazebo/sensors/GZAngleToTargetDetector.cpp new file mode 100644 index 0000000000..6adfbf4f68 --- /dev/null +++ b/cpprevolve/revolve/gazebo/sensors/GZAngleToTargetDetector.cpp @@ -0,0 +1,34 @@ +#include "GZAngleToTargetDetector.h" +#include +#include +#include + +using namespace revolve::gazebo; + +GZAngleToTargetDetector::GZAngleToTargetDetector(::gazebo::physics::ModelPtr robot, const ignition::math::Vector3d &target) + : robot(std::move(robot)), target(target) +{ + std::cout << "GZAngleToTargetDetector::GZAngleToTargetDetector(" << target << ")" << std::endl; +} + +float GZAngleToTargetDetector::detect_angle() +{ + // Get Robot orientation + const ignition::math::Pose3d robotPose = robot->WorldPose(); // gives position of robot wrt world, Pose3d has position AND orientation + const ignition::math::Vector3d forward{0, 1, 0}; // Forward vector + const ignition::math::Pose3d forward_pose( + robotPose.CoordPositionAdd(forward), + ignition::math::Quaterniond::Identity); // needs to be calculated every single time (no static const) + + const ignition::math::Vector3d orientation_vec = (forward_pose.Pos() - robotPose.Pos()).Normalized(); + const ignition::math::Vector3d robot_to_target_vec = (this->target - robotPose.Pos()).Normalized(); + + // calculate angle from target, flattened onto the x,y plane + // explained here: + // https://code-examples.net/en/q/d6a4f5 + const double dot = orientation_vec[0] * robot_to_target_vec[0] + orientation_vec[1] * robot_to_target_vec[1]; + const double det = orientation_vec[0] * robot_to_target_vec[1] - orientation_vec[1] * robot_to_target_vec[0]; + const double angle = std::atan2(det, dot); + + return angle; +} diff --git a/cpprevolve/revolve/gazebo/sensors/GZAngleToTargetDetector.h b/cpprevolve/revolve/gazebo/sensors/GZAngleToTargetDetector.h new file mode 100644 index 0000000000..3fecf4cfdd --- /dev/null +++ b/cpprevolve/revolve/gazebo/sensors/GZAngleToTargetDetector.h @@ -0,0 +1,27 @@ +#pragma once + +#include +#include "revolve/brains/controller/sensors/AngleToTargetDetector.h" + +namespace revolve { +namespace gazebo { + +class GZAngleToTargetDetector : public revolve::AngleToTargetDetector { +public: + explicit GZAngleToTargetDetector(::gazebo::physics::ModelPtr robot, const ignition::math::Vector3d& target); + ~GZAngleToTargetDetector() override = default; + + float detect_angle() override; + +protected: + void get_image(cv::Mat &image) override { + throw std::runtime_error("Should never call this"); + } + +protected: + const ::gazebo::physics::ModelPtr robot; + const ignition::math::Vector3d target; +}; + +} +} \ No newline at end of file diff --git a/cpprevolve/revolve/gazebo/sensors/ImuSensor.cpp b/cpprevolve/revolve/gazebo/sensors/ImuSensor.cpp index 8272485994..005fc937a8 100644 --- a/cpprevolve/revolve/gazebo/sensors/ImuSensor.cpp +++ b/cpprevolve/revolve/gazebo/sensors/ImuSensor.cpp @@ -19,6 +19,7 @@ #include #include +#include #include "ImuSensor.h" @@ -70,7 +71,7 @@ void ImuSensor::OnUpdate() } ///////////////////////////////////////////////// -void ImuSensor::Read(double *_input) +void ImuSensor::read(double *_input) { // Copy our values to the input array memcpy(_input, this->lastValues_, sizeof(this->lastValues_)); diff --git a/cpprevolve/revolve/gazebo/sensors/ImuSensor.h b/cpprevolve/revolve/gazebo/sensors/ImuSensor.h index 8ff8f825b5..99679960f6 100644 --- a/cpprevolve/revolve/gazebo/sensors/ImuSensor.h +++ b/cpprevolve/revolve/gazebo/sensors/ImuSensor.h @@ -22,6 +22,7 @@ #define REVOLVE_GAZEBO_SENSORS_IMUSENSOR_H_ #include +#include #include "Sensor.h" @@ -49,7 +50,7 @@ namespace revolve /// \brief Read the value of this IMU sensor into the /// \param[in] _input: array. /// \brief[in,out] _input Input value to write on - public: virtual void Read(double *_input); + public: void read(double *_input) override; /// \brief Called when the IMU sensor is updated public: void OnUpdate(); diff --git a/cpprevolve/revolve/gazebo/sensors/LightSensor.cpp b/cpprevolve/revolve/gazebo/sensors/LightSensor.cpp index a1cbb3f854..75fe294190 100644 --- a/cpprevolve/revolve/gazebo/sensors/LightSensor.cpp +++ b/cpprevolve/revolve/gazebo/sensors/LightSensor.cpp @@ -20,6 +20,7 @@ #include #include +#include namespace gz = gazebo; @@ -57,9 +58,6 @@ LightSensor::LightSensor( std::bind(&LightSensor::OnUpdate, this)); } -///////////////////////////////////////////////// -LightSensor::~LightSensor() = default; - ///////////////////////////////////////////////// void LightSensor::OnUpdate() { @@ -89,7 +87,7 @@ void LightSensor::OnUpdate() /// read method - although I have to check whether this is even possible. In /// any case that would force the sensor update here on the "driver" thread, /// which might be detrimental to performance. -void LightSensor::Read(double *_input) +void LightSensor::read(double *_input) { _input[0] = this->lastValue_; } diff --git a/cpprevolve/revolve/gazebo/sensors/LightSensor.h b/cpprevolve/revolve/gazebo/sensors/LightSensor.h index 86a6bf452c..e56984f445 100644 --- a/cpprevolve/revolve/gazebo/sensors/LightSensor.h +++ b/cpprevolve/revolve/gazebo/sensors/LightSensor.h @@ -24,6 +24,7 @@ #include #include +#include namespace revolve { @@ -43,12 +44,9 @@ namespace revolve std::string _partId, std::string _sensorId); - /// \brief Destructor - public: virtual ~LightSensor(); - /// \brief Returns a float intensity between 0 and 1 /// \brief[in,out] _input Input value to write on - public: virtual void Read(double *_input); + public: void read(double *_input) override; /// \brief Called when the camera sensor is updated public: void OnUpdate(); diff --git a/cpprevolve/revolve/gazebo/sensors/PointIntensitySensor.cpp b/cpprevolve/revolve/gazebo/sensors/PointIntensitySensor.cpp deleted file mode 100644 index 2d269dc554..0000000000 --- a/cpprevolve/revolve/gazebo/sensors/PointIntensitySensor.cpp +++ /dev/null @@ -1,84 +0,0 @@ -/* -* Copyright (C) 2017 Vrije Universiteit Amsterdam -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -* -* Author: Elte Hupkes -* -*/ - -#include - -#include "PointIntensitySensor.h" - -namespace gz = gazebo; - -using namespace revolve::gazebo; - -///////////////////////////////////////////////// -PointIntensitySensor::PointIntensitySensor( - sdf::ElementPtr _sensor, - ::gazebo::physics::ModelPtr _model, - const std::string &_partId, - const std::string &_sensorId) - : VirtualSensor(_model, _partId, _sensorId, 1) - , maxInput_(1) - , r_(1) -{ - if (not _sensor->HasElement("rv:point_intensity_sensor")) - { - std::cerr << "PointIntensitySensor missing " - "`rv:point_intensity_sensor` element." << std::endl; - throw std::runtime_error("Robot brain error."); - } - - auto configElem = _sensor->GetElement("rv:point_intensity_sensor"); - - if (not configElem->HasElement("rv:point")) - { - std::cerr << "PointIntensitySensor missing `rv:point` element." - << std::endl; - } - - auto pointElem = configElem->GetElement("rv:point"); - this->point_ = pointElem->Get< ignition::math::Vector3d >(); - - if (configElem->HasElement("rv:function")) - { - auto funcElem = configElem->GetElement("rv:function"); - - if (funcElem->HasAttribute("r")) - { - funcElem->GetAttribute("r")->Get(this->r_); - } - - if (funcElem->HasAttribute("i_max")) - { - funcElem->GetAttribute("i_max")->Get(this->maxInput_); - } - } -} - -///////////////////////////////////////////////// -void PointIntensitySensor::Read(double *_input) -{ - auto distance = this->model_->WorldPose().Pos().Distance(this->point_); - - if (distance < this->r_) - { - _input[0] = this->maxInput_; - } - else - { - _input[0] = this->maxInput_ * std::pow(this->r_ / distance, 2); - } -} diff --git a/cpprevolve/revolve/gazebo/sensors/PointIntensitySensor.h b/cpprevolve/revolve/gazebo/sensors/PointIntensitySensor.h deleted file mode 100644 index ca028530e8..0000000000 --- a/cpprevolve/revolve/gazebo/sensors/PointIntensitySensor.h +++ /dev/null @@ -1,75 +0,0 @@ -/* -* Copyright (C) 2017 Vrije Universiteit Amsterdam -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -* -* Description: Sensor that measures the intensity this robot feels of a -* radiating point somewhere in the distance. This is -* parameterised by a couple of values. Say the distance of the -* center of mass of the robot to the intensity source is `d`, we -* then have parameters I+, `r` and `a` such that for the measured -* intensity I it holds that: -* -* I = I+ if d <= r -* I = I+ * (r/d)^2 otherwise -* -* This corresponds to a quadratic decrease with `r` - intensity -* is maximal at `r`, 1/4 at 2r, 1/9 at 3r, etc. -* Author: Elte Hupkes -* -*/ - -#ifndef REVOLVE_POINTINTENSITYSENSOR_H -#define REVOLVE_POINTINTENSITYSENSOR_H - -#include - -#include "VirtualSensor.h" - -namespace revolve -{ - namespace gazebo - { - class PointIntensitySensor - : public VirtualSensor - { - /// \brief Constructor - /// \brief[in] _model Model identifier - /// \brief[in] _sensor Sensor identifier - /// \brief[in] _partId Module identifier - /// \brief[in] _sensorId Sensor identifier - public: PointIntensitySensor( - sdf::ElementPtr _sensor, - ::gazebo::physics::ModelPtr _model, - const std::string &_partId, - const std::string &_sensorId); - - /// \brief Reads the battery value - /// \brief[in,out] _input Input value to write on - public: virtual void Read(double *_input); - - /// \brief The point to which proximity should be returned - protected: ::ignition::math::Vector3d point_; - - /// \brief The value of the input neuron of this sensor is calculated - /// from the distance with the function: - /// `a / (distance**b)` - /// Where it will be made sure that the output is between 0 and a. - protected: double maxInput_; - - /// \brief Radius distance - protected: double r_; - }; - } -} - -#endif // REVOLVE_POINTINTENSITYSENSOR_H diff --git a/cpprevolve/revolve/gazebo/sensors/Sensor.cpp b/cpprevolve/revolve/gazebo/sensors/Sensor.cpp index babd1443d2..f4d284b53d 100644 --- a/cpprevolve/revolve/gazebo/sensors/Sensor.cpp +++ b/cpprevolve/revolve/gazebo/sensors/Sensor.cpp @@ -20,6 +20,8 @@ #include #include +#include +#include namespace gz = gazebo; @@ -32,7 +34,10 @@ Sensor::Sensor( std::string _partId, std::string _sensorId, unsigned int _inputs) - : VirtualSensor(_model, _partId, _sensorId, _inputs) + : ::revolve::Sensor(_inputs) + , partId_(_partId) + , sensorId_(_sensorId) + , sensor_(nullptr) { if (not _sensor->HasAttribute("sensor") or not _sensor->HasAttribute("link")) { @@ -62,12 +67,3 @@ Sensor::Sensor( throw std::runtime_error("Sensor error"); } } - -///////////////////////////////////////////////// -Sensor::~Sensor() = default; - -///////////////////////////////////////////////// -::gazebo::sensors::SensorPtr Sensor::GzSensor() -{ - return sensor_; -} diff --git a/cpprevolve/revolve/gazebo/sensors/Sensor.h b/cpprevolve/revolve/gazebo/sensors/Sensor.h index ba062030da..5bb3529599 100644 --- a/cpprevolve/revolve/gazebo/sensors/Sensor.h +++ b/cpprevolve/revolve/gazebo/sensors/Sensor.h @@ -21,15 +21,17 @@ #define REVOLVE_GAZEBO_SENSORS_SENSOR_H_ #include +#include +#include +#include -#include +#include namespace revolve { namespace gazebo { - class Sensor - : public VirtualSensor + class Sensor: public ::revolve::Sensor { /// \brief Constructor /// \brief[in] _model Model identifier @@ -44,11 +46,23 @@ namespace revolve std::string _sensorId, unsigned int _inputs); - /// \brief Destructor - public: virtual ~Sensor(); + /// \return The part ID + public: const std::string& PartId() const { return partId_; }; - /// \return The attached Gazebo sensor - public: ::gazebo::sensors::SensorPtr GzSensor(); + /// \return The ID of the sensor + public: const std::string& SensorId() const { return sensorId_; }; + + /// \return The attached Gazebo sensor + public: const ::gazebo::sensors::SensorPtr& GzSensor() const { return sensor_; }; + + /// \brief The model this sensor is part of + protected: ::gazebo::physics::ModelPtr model_; + + /// \brief ID of the body part the motor belongs to + protected: std::string partId_; + + /// \brief ID of the sensor + protected: std::string sensorId_; /// \brief The actual sensor object this sensor is receiving input from protected: ::gazebo::sensors::SensorPtr sensor_; diff --git a/cpprevolve/revolve/gazebo/sensors/SensorFactory.cpp b/cpprevolve/revolve/gazebo/sensors/SensorFactory.cpp index 187d8c040d..9e80a40dc1 100644 --- a/cpprevolve/revolve/gazebo/sensors/SensorFactory.cpp +++ b/cpprevolve/revolve/gazebo/sensors/SensorFactory.cpp @@ -55,18 +55,6 @@ SensorPtr SensorFactory::Sensor( { sensor.reset(new TouchSensor(this->model_, _sensorSdf, _partId, _sensorId)); } - else if ("basic_battery" == _type) - { - sensor.reset(new BatterySensor(this->model_, _partId, _sensorId)); - } - else if ("point_intensity" == _type) - { - sensor.reset(new PointIntensitySensor( - _sensorSdf, - this->model_, - _partId, - _sensorId)); - } else { std::clog << "Sensor type \"" << _type << "\" not recognized. Ignoring sensor" << std::endl; diff --git a/cpprevolve/revolve/gazebo/sensors/SensorFactory.h b/cpprevolve/revolve/gazebo/sensors/SensorFactory.h index c6c05df167..aadecc3696 100644 --- a/cpprevolve/revolve/gazebo/sensors/SensorFactory.h +++ b/cpprevolve/revolve/gazebo/sensors/SensorFactory.h @@ -22,10 +22,12 @@ #define REVOLVE_GAZEBO_SENSORS_SENSORFACTORY_H_ #include +#include #include #include +#include namespace revolve { diff --git a/cpprevolve/revolve/gazebo/sensors/Sensors.h b/cpprevolve/revolve/gazebo/sensors/Sensors.h index 4c7ab5e32c..df45a1f39b 100644 --- a/cpprevolve/revolve/gazebo/sensors/Sensors.h +++ b/cpprevolve/revolve/gazebo/sensors/Sensors.h @@ -22,8 +22,6 @@ // Includes all sensor types for convenience #include -#include -#include #include #include diff --git a/cpprevolve/revolve/gazebo/sensors/TouchSensor.cpp b/cpprevolve/revolve/gazebo/sensors/TouchSensor.cpp index 2d7d80887f..0679f72101 100644 --- a/cpprevolve/revolve/gazebo/sensors/TouchSensor.cpp +++ b/cpprevolve/revolve/gazebo/sensors/TouchSensor.cpp @@ -21,6 +21,7 @@ #include #include +#include namespace gz = gazebo; @@ -64,7 +65,7 @@ void TouchSensor::OnUpdate() } ///////////////////////////////////////////////// -void TouchSensor::Read(double *_input) +void TouchSensor::read(double *_input) { _input[0] = this->lastValue_ ? 1 : 0; } diff --git a/cpprevolve/revolve/gazebo/sensors/TouchSensor.h b/cpprevolve/revolve/gazebo/sensors/TouchSensor.h index b41e91fb6e..ef85697996 100644 --- a/cpprevolve/revolve/gazebo/sensors/TouchSensor.h +++ b/cpprevolve/revolve/gazebo/sensors/TouchSensor.h @@ -24,6 +24,7 @@ #include #include +#include namespace revolve { @@ -49,7 +50,7 @@ namespace revolve /// \brief The touch sensor is boolean; it is either touching something /// or it is not. Since the NN works with floats, we return 0.0 or 1.0. /// \brief[in,out] _input Input value to write on - public: virtual void Read(double *_input); + public: void read(double *_input) override; /// \brief Called when the camera sensor is updated public: void OnUpdate(); diff --git a/cpprevolve/revolve/gazebo/sensors/VirtualSensor.cpp b/cpprevolve/revolve/gazebo/sensors/VirtualSensor.cpp deleted file mode 100644 index 6739a0775f..0000000000 --- a/cpprevolve/revolve/gazebo/sensors/VirtualSensor.cpp +++ /dev/null @@ -1,58 +0,0 @@ -/* -* Copyright (C) 2017 Vrije Universiteit Amsterdam -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -* -* Description: TODO: -* Author: Elte Hupkes -* -*/ - -#include - -#include - -using namespace revolve::gazebo; - -///////////////////////////////////////////////// -VirtualSensor::VirtualSensor( - ::gazebo::physics::ModelPtr _model, - const std::string _partId, - const std::string _sensorId, - const unsigned int _inputs) - : model_(_model) - , partId_(_partId) - , sensorId_(_sensorId) - , inputs_(_inputs) -{ -} - -///////////////////////////////////////////////// -VirtualSensor::~VirtualSensor() = default; - -///////////////////////////////////////////////// -unsigned int VirtualSensor::Inputs() -{ - return this->inputs_; -} - -///////////////////////////////////////////////// -std::string VirtualSensor::PartId() -{ - return this->partId_; -} - -///////////////////////////////////////////////// -std::string VirtualSensor::SensorId() -{ - return this->sensorId_; -} diff --git a/cpprevolve/revolve/gazebo/sensors/VirtualSensor.h b/cpprevolve/revolve/gazebo/sensors/VirtualSensor.h deleted file mode 100644 index d5a9c38ede..0000000000 --- a/cpprevolve/revolve/gazebo/sensors/VirtualSensor.h +++ /dev/null @@ -1,84 +0,0 @@ -/* -* Copyright (C) 2017 Vrije Universiteit Amsterdam -* -* Licensed under the Apache License, Version 2.0 (the "License"); -* You may obtain a copy of the License at -* -* http://www.apache.org/licenses/LICENSE-2.0 -* -* Unless required by applicable law or agreed to in writing, software -* distributed under the License is distributed on an "AS IS" BASIS, -* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -* See the License for the specific language governing permissions and -* limitations under the License. -* -* Description: The `VirtualSensor` is the base class for all Sensors, it -* defines the sensor interface but is not necessarily connected -* to anything concrete in the simulation. -* Author: Elte Hupkes -* -*/ - -#ifndef REVOLVE_GAZEBO_SENSORS_VIRTUALSENSOR_H_ -#define REVOLVE_GAZEBO_SENSORS_VIRTUALSENSOR_H_ - -#include - -#include -#include - -#include - -namespace revolve -{ - namespace gazebo - { - class VirtualSensor - { - /// \brief Constructor - /// \brief[in] _model Model identifier - /// \brief[in] _sensor Sensor identifier - /// \brief[in] _partId Module identifier - /// \brief[in] _sensorId Sensor identifier - /// \param[in] _inputs Number of inputs a sensor has - public: VirtualSensor( - ::gazebo::physics::ModelPtr _model, - const std::string _partId, - const std::string _sensorId, - const unsigned int _inputs); - - /// \brief Destructor - public: virtual ~VirtualSensor(); - - /// \brief Reads the current value of this sensor into the given - /// network output array. This should fill the number of input neurons - /// the sensor specifies to have, i.e. if the sensor specifies 2 input - /// neurons it should fill `input[0]` and `input[1]` - /// \brief[in,out] _input Input value to write on - public: virtual void Read(double *_input) = 0; - - /// \return The part ID - public: std::string PartId(); - - /// \return The ID of the sensor - public: std::string SensorId(); - - /// \return Number of inputs this sensor generates - public: unsigned int Inputs(); - - /// \brief The model this sensor is part of - protected: ::gazebo::physics::ModelPtr model_; - - /// \brief ID of the body part the motor belongs to - protected: std::string partId_; - - /// \brief ID of the sensor - protected: std::string sensorId_; - - /// \brief Number of inputs this sensor generates - protected: unsigned int inputs_; - }; - } /* namespace gazebo */ -} /* namespace revolve */ - -#endif /* REVOLVE_GAZEBO_SENSORS_VIRTUALSENSOR_H_ */ diff --git a/cpprevolve/revolve/raspberry/RaspController.cpp b/cpprevolve/revolve/raspberry/RaspController.cpp index 1a424b2fc0..0ccc7d5ec3 100644 --- a/cpprevolve/revolve/raspberry/RaspController.cpp +++ b/cpprevolve/revolve/raspberry/RaspController.cpp @@ -61,6 +61,39 @@ void RaspController::set_new_controller(const YAML::Node &conf) params.weights.emplace_back(weight.as()); } + if (conf["reset_neuron_random"].IsDefined()) { + params.reset_neuron_random = conf["reset_neuron_random"].as(); + std::cout << "Setting reset_neuron_random to: " << params.reset_neuron_random << std::endl; + } + if (conf["use_frame_of_reference"].IsDefined()) { + params.use_frame_of_reference = conf["use_frame_of_reference"].as(); + std::cout << "Setting use_frame_of_reference to: " << params.use_frame_of_reference << std::endl; + } + if (conf["init_neuron_state"].IsDefined()) { + params.init_neuron_state = conf["init_neuron_state"].as(); + std::cout << "Setting init_neuron_state to: " << params.init_neuron_state << std::endl; + } + if (conf["range_ub"].IsDefined()) { + params.range_ub = conf["range_ub"].as(); + std::cout << "Setting range_ub to: " << params.range_ub << std::endl; + } + if (conf["signal_factor_all"].IsDefined()) { + params.signal_factor_all = conf["signal_factor_all"].as(); + std::cout << "Setting signal factor all to: " << params.signal_factor_all << std::endl; + } + if (conf["signal_factor_mid"].IsDefined()) { + params.signal_factor_mid = conf["signal_factor_mid"].as(); + std::cout << "Setting signal_factor_mid to: " << params.signal_factor_mid << std::endl; + } + if (conf["signal_factor_left_right"].IsDefined()) { + params.signal_factor_all = conf["signal_factor_left_right"].as(); + std::cout << "Setting signal_factor_left_right to: " << params.signal_factor_left_right << std::endl; + } + if (conf["abs_output_bound"].IsDefined()) { + params.abs_output_bound = conf["abs_output_bound"].as(); + std::cout << "Setting abs_output_bound to: " << params.abs_output_bound << std::endl; + } + revolve_controller.reset( new DifferentialCPG(params,this->actuators) ); diff --git a/docker/build_install_multineat.sh b/docker/build_install_multineat.sh new file mode 100755 index 0000000000..8e3df8109a --- /dev/null +++ b/docker/build_install_multineat.sh @@ -0,0 +1,12 @@ +#!/bin/bash +set -e + +# Build Revolve +cd /revolve/thirdparty/MultiNEAT + +mkdir -p build && cd build +cmake .. -DCMAKE_BUILD_TYPE="Release" +make -j4 +make -j4 install +cd .. +rm -r build diff --git a/docker/build_revolve.sh b/docker/build_revolve.sh index 953e270d87..c25dc0b848 100755 --- a/docker/build_revolve.sh +++ b/docker/build_revolve.sh @@ -13,4 +13,5 @@ make -j4 # Install the Python dependencies cd /revolve +pip3 install scikit-build pip3 install -r requirements.txt diff --git a/experiments/examples/manager_pop.py b/experiments/examples/manager_pop.py index ed42adced8..c0c1fb5189 100755 --- a/experiments/examples/manager_pop.py +++ b/experiments/examples/manager_pop.py @@ -2,20 +2,25 @@ import asyncio from pyrevolve import parser +from pyrevolve.custom_logging.logger import logger from pyrevolve.evolution import fitness +from pyrevolve.evolution.population.population import Population +from pyrevolve.evolution.population.population_config import PopulationConfig +from pyrevolve.evolution.population.population_management import ( + steady_state_population_management, +) from pyrevolve.evolution.selection import multiple_selection, tournament_selection -from pyrevolve.evolution.population import Population, PopulationConfig -from pyrevolve.evolution.pop_management.steady_state import steady_state_population_management from pyrevolve.experiment_management import ExperimentManagement -from pyrevolve.genotype.plasticoding.crossover.crossover import CrossoverConfig -from pyrevolve.genotype.plasticoding.crossover.standard_crossover import standard_crossover -from pyrevolve.genotype.plasticoding.initialization import random_initialization -from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig -from pyrevolve.genotype.plasticoding.mutation.standard_mutation import standard_mutation -from pyrevolve.genotype.plasticoding.plasticoding import PlasticodingConfig +from pyrevolve.genotype.direct_tree.direct_tree_config import DirectTreeGenotypeConfig +from pyrevolve.genotype.direct_tree.direct_tree_crossover import ( + crossover_list as direct_tree_crossover_list, +) +from pyrevolve.genotype.direct_tree.direct_tree_genotype import DirectTreeGenotype +from pyrevolve.genotype.direct_tree.direct_tree_mutation import ( + mutate as direct_tree_mutate, +) from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue -from pyrevolve.custom_logging.logger import logger async def run(): @@ -28,32 +33,49 @@ async def run(): population_size = 100 offspring_size = 50 - genotype_conf = PlasticodingConfig( - max_structural_modules=100, - ) - - mutation_conf = MutationConfig( - mutation_prob=0.8, - genotype_conf=genotype_conf, - ) - - crossover_conf = CrossoverConfig( - crossover_prob=0.8, + genotype_conf: DirectTreeGenotypeConfig = DirectTreeGenotypeConfig( + max_parts=50, + min_parts=10, + max_oscillation=5, + init_n_parts_mu=10, + init_n_parts_sigma=4, + init_prob_no_child=0.1, + init_prob_child_block=0.4, + init_prob_child_active_joint=0.5, + mutation_p_duplicate_subtree=0.2, + mutation_p_delete_subtree=0.2, + mutation_p_generate_subtree=0.2, + mutation_p_swap_subtree=0.2, + mutation_p_mutate_oscillators=0.5, + mutation_p_mutate_oscillator=0.5, + mutate_oscillator_amplitude_sigma=0.3, + mutate_oscillator_period_sigma=0.3, + mutate_oscillator_phase_sigma=0.3, ) - # experiment params # + mutation_conf = genotype_conf + crossover_conf = genotype_conf # Parse command line / file input arguments settings = parser.parse_args() experiment_management = ExperimentManagement(settings) - do_recovery = settings.recovery_enabled and not experiment_management.experiment_is_new() + do_recovery = ( + settings.recovery_enabled and not experiment_management.experiment_is_new() + ) - logger.info('Activated run '+settings.run+' of experiment '+settings.experiment_name) + logger.info( + "Activated run " + settings.run + " of experiment " + settings.experiment_name + ) if do_recovery: - gen_num, has_offspring, next_robot_id = experiment_management.read_recovery_state(population_size, offspring_size) - - if gen_num == num_generations-1: - logger.info('Experiment is already complete.') + ( + gen_num, + has_offspring, + next_robot_id, + _, + ) = experiment_management.read_recovery_state(population_size, offspring_size) + + if gen_num == num_generations - 1: + logger.info("Experiment is already complete.") return else: gen_num = 0 @@ -61,18 +83,27 @@ async def run(): population_conf = PopulationConfig( population_size=population_size, - genotype_constructor=random_initialization, + genotype_constructor=lambda conf, _id: DirectTreeGenotype( + conf, _id, random_init=True + ), genotype_conf=genotype_conf, fitness_function=fitness.displacement_velocity, - mutation_operator=standard_mutation, + mutation_operator=lambda genotype, gen_conf: direct_tree_mutate( + genotype, gen_conf, in_place=False + ), mutation_conf=mutation_conf, - crossover_operator=standard_crossover, + crossover_operator=lambda parents, gen_conf, _cross_conf: direct_tree_crossover_list( + [p.genotype for p in parents], gen_conf + ), crossover_conf=crossover_conf, selection=lambda individuals: tournament_selection(individuals, 2), - parent_selection=lambda individuals: multiple_selection(individuals, 2, tournament_selection), + parent_selection=lambda individuals: multiple_selection( + individuals, 2, tournament_selection + ), population_management=steady_state_population_management, population_management_selector=tournament_selection, evaluation_time=settings.evaluation_time, + grace_time=settings.grace_time, offspring_size=offspring_size, experiment_name=settings.experiment_name, experiment_management=experiment_management, @@ -84,36 +115,46 @@ async def run(): simulator_queue = SimulatorQueue(n_cores, settings, settings.port_start) await simulator_queue.start() - analyzer_queue = AnalyzerQueue(1, settings, settings.port_start+n_cores) + analyzer_queue = AnalyzerQueue(1, settings, settings.port_start + n_cores) await analyzer_queue.start() - population = Population(population_conf, simulator_queue, analyzer_queue, next_robot_id) + population = Population( + population_conf, simulator_queue, analyzer_queue, next_robot_id + ) if do_recovery: # loading a previous state of the experiment - await population.load_snapshot(gen_num) + population.load_snapshot(gen_num) if gen_num >= 0: - logger.info('Recovered snapshot '+str(gen_num)+', pop with ' + str(len(population.individuals))+' individuals') + logger.info( + "Recovered snapshot " + + str(gen_num) + + ", pop with " + + str(len(population.individuals)) + + " individuals" + ) if has_offspring: - individuals = await population.load_offspring(gen_num, population_size, offspring_size, next_robot_id) + individuals = population.load_offspring( + gen_num, population_size, offspring_size, next_robot_id + ) gen_num += 1 - logger.info('Recovered unfinished offspring '+str(gen_num)) + logger.info("Recovered unfinished offspring " + str(gen_num)) if gen_num == 0: - await population.init_pop(individuals) + await population.initialize(individuals) else: - population = await population.next_gen(gen_num, individuals) + population = await population.next_generation(gen_num, individuals) experiment_management.export_snapshots(population.individuals, gen_num) else: # starting a new experiment experiment_management.create_exp_folders() - await population.init_pop() + await population.initialize() experiment_management.export_snapshots(population.individuals, gen_num) - while gen_num < num_generations-1: + while gen_num < num_generations - 1: gen_num += 1 - population = await population.next_gen(gen_num) + population = await population.next_generation(gen_num) experiment_management.export_snapshots(population.individuals, gen_num) # output result after completing all generations... diff --git a/experiments/examples/manager_population_cppn.py b/experiments/examples/manager_population_cppn.py new file mode 100755 index 0000000000..eb941d9c07 --- /dev/null +++ b/experiments/examples/manager_population_cppn.py @@ -0,0 +1,278 @@ +#!/usr/bin/env python3 +import math +from dataclasses import dataclass + +import multineat +from pyrevolve import parser +from pyrevolve.custom_logging.logger import logger +from pyrevolve.evolution.fitness import follow_line as fitness_follow_line +from pyrevolve.evolution.population.population import Population +from pyrevolve.evolution.population.population_config import PopulationConfig +from pyrevolve.evolution.population.population_management import ( + steady_state_population_management, +) +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +from pyrevolve.experiment_management import ExperimentManagement +from pyrevolve.genotype.bodybrain_composition.config import ( + Config as BodybrainCompositionConfig, +) +from pyrevolve.genotype.bodybrain_composition.crossover import ( + crossover as bodybrain_composition_crossover, +) +from pyrevolve.genotype.bodybrain_composition.genotype import ( + Genotype as BodybrainCompositionGenotype, +) +from pyrevolve.genotype.bodybrain_composition.mutation import ( + mutate as bodybrain_composition_mutate, +) +from pyrevolve.genotype.cppnneat.body.config import Config as CppnneatBodyConfig +from pyrevolve.genotype.cppnneat.body.crossover import ( + crossover as cppnneat_body_crossover, +) +from pyrevolve.genotype.cppnneat.body.develop import develop as cppnneat_body_develop +from pyrevolve.genotype.cppnneat.body.genotype import Genotype as CppnneatBodyGenotype +from pyrevolve.genotype.cppnneat.body.mutation import mutate as cppnneat_body_mutate +from pyrevolve.genotype.cppnneat.brain.config import Config as CppnneatBrainConfig +from pyrevolve.genotype.cppnneat.brain.crossover import ( + crossover as cppnneat_brain_crossover, +) +from pyrevolve.genotype.cppnneat.brain.develop import develop as cppnneat_brain_develop +from pyrevolve.genotype.cppnneat.brain.genotype import Genotype as CppnneatBrainGenotype +from pyrevolve.genotype.cppnneat.brain.mutation import mutate as cppnneat_brain_mutate +from pyrevolve.genotype.cppnneat.config import get_default_multineat_params +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue + + +@dataclass +class GenotypeConstructorConfig: + body_n_start_mutations: int + brain_n_start_mutations: int + bodybrain_composition_config: BodybrainCompositionConfig + body_multineat_params: multineat.Parameters + brain_multineat_params: multineat.Parameters + body_cppn_output_activation_type: multineat.ActivationFunction + brain_cppn_output_activation_type: multineat.ActivationFunction + + +def create_random_genotype( + config: GenotypeConstructorConfig, id: int +) -> BodybrainCompositionGenotype: + return BodybrainCompositionGenotype[CppnneatBodyGenotype, CppnneatBrainGenotype]( + id, + config.bodybrain_composition_config, + CppnneatBodyGenotype.random( + config.body_multineat_params, + config.body_cppn_output_activation_type, + config.body_n_start_mutations, + config.bodybrain_composition_config.body_develop_config.innov_db, + config.bodybrain_composition_config.body_develop_config.rng, + ), + CppnneatBrainGenotype.random( + config.brain_multineat_params, + config.brain_cppn_output_activation_type, + config.brain_n_start_mutations, + config.bodybrain_composition_config.brain_develop_config.innov_db, + config.bodybrain_composition_config.brain_develop_config.rng, + ), + ) + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 100 + population_size = 10 + offspring_size = 5 + + target_distance = 10 + + body_n_start_mutations: int = 10 + brain_n_start_mutations: int = 10 + + # body multineat settings + multineat_params_body = get_default_multineat_params() + + # brain multineat settings + multineat_params_brain = get_default_multineat_params() + + # multineat rng + rng = multineat.RNG() + rng.TimeSeed() + + # multineat innovation databases + innov_db_body = multineat.InnovationDatabase() + innov_db_brain = multineat.InnovationDatabase() + + # config for body + body_config = CppnneatBodyConfig( + multineat_params=multineat_params_body, + innov_db=innov_db_body, + rng=rng, + mate_average=False, + interspecies_crossover=True, + ) + + # config for brain + brain_config = CppnneatBrainConfig( + multineat_params=multineat_params_brain, + innov_db=innov_db_brain, + rng=rng, + mate_average=False, + interspecies_crossover=True, + abs_output_bound=1.0, + use_frame_of_reference=True, + output_signal_factor=1.0, + range_ub=1.0, + init_neuron_state=math.sqrt(2) / 2.0, + reset_neuron_random=False, + ) + + # bodybrain composition genotype config + bodybrain_composition_config = BodybrainCompositionConfig( + body_crossover=cppnneat_body_crossover, + brain_crossover=cppnneat_brain_crossover, + body_crossover_config=body_config, + brain_crossover_config=brain_config, + body_mutate=cppnneat_body_mutate, + brain_mutate=cppnneat_brain_mutate, + body_mutate_config=body_config, + brain_mutate_config=brain_config, + body_develop=cppnneat_body_develop, + brain_develop=cppnneat_brain_develop, + body_develop_config=body_config, + brain_develop_config=brain_config, + ) + + # genotype constructor config. Used by `create_random_genotype` in this file. + genotype_constructor_config = GenotypeConstructorConfig( + body_n_start_mutations, + brain_n_start_mutations, + bodybrain_composition_config, + multineat_params_body, + multineat_params_brain, + body_cppn_output_activation_type=multineat.ActivationFunction.TANH, + brain_cppn_output_activation_type=multineat.ActivationFunction.TANH, + ) + + # parse command line arguments + settings = parser.parse_args() + + # create object that provides functionality + # to access the correct experiment directories, + # export/import things, recovery info etc. + experiment_management = ExperimentManagement(settings) + + # settings for the evolutionary process + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=create_random_genotype, + genotype_conf=genotype_constructor_config, + fitness_function=fitness_follow_line, + mutation_operator=bodybrain_composition_mutate, + mutation_conf=bodybrain_composition_config, + crossover_operator=bodybrain_composition_crossover, + crossover_conf=bodybrain_composition_config, + selection=lambda individuals: tournament_selection(individuals, 2), + parent_selection=lambda individuals: multiple_selection( + individuals, 2, tournament_selection + ), + population_management=steady_state_population_management, + population_management_selector=tournament_selection, + evaluation_time=settings.evaluation_time, + offspring_size=offspring_size, + experiment_name=settings.experiment_name, + experiment_management=experiment_management, + # target_distance=target_distance, + ) + + # check if recovery is required + do_recovery = ( + settings.recovery_enabled and not experiment_management.experiment_is_new() + ) + + # print some info about the experiment and recovery + logger.info( + "Activated run " + settings.run + " of experiment " + settings.experiment_name + ) + if settings.recovery_enabled: + if experiment_management.experiment_is_new(): + logger.info("This is a new experiment. No recovery performed.") + else: + logger.info("Recovering proviously stopped run") + + # set gen_num and next_robot_id to starting value, + # or get them from recovery state + # gen_num will be -1 if nothing has been done yet + if do_recovery: + ( + gen_num, + has_offspring, + next_robot_id, + _, + ) = experiment_management.read_recovery_state(population_size, offspring_size) + else: + gen_num = 0 + next_robot_id = 1 + + # maybe experiment is done already? + if gen_num == num_generations - 1: + logger.info("Experiment is already complete.") + return + + # setup simulator_quque and analyzer_queue based on number of cores + n_cores = settings.n_cores + + simulator_queue = SimulatorQueue(n_cores, settings, settings.port_start) + await simulator_queue.start() + + analyzer_queue = AnalyzerQueue(1, settings, settings.port_start + n_cores) + await analyzer_queue.start() + + # create start population + population = Population( + population_conf, simulator_queue, analyzer_queue, next_robot_id + ) + + # Recover if required + if do_recovery: + # loading a previous state of the experiment + population.load_snapshot( + gen_num + ) # I think this breaks when gen_num == -1 --Aart + if gen_num >= 0: + logger.info( + "Recovered snapshot " + + str(gen_num) + + ", pop with " + + str(len(population.individuals)) + + " individuals" + ) + if has_offspring: + individuals = population.load_offspring( + gen_num, population_size, offspring_size, next_robot_id + ) + gen_num += 1 + logger.info("Recovered unfinished offspring " + str(gen_num)) + + if gen_num == 0: + await population.initialize(individuals) + else: + population = await population.next_generation(gen_num, individuals) + + experiment_management.export_snapshots(population.individuals, gen_num) + else: + # starting a new experiment + experiment_management.create_exp_folders() + await population.initialize() + experiment_management.export_snapshots(population.individuals, gen_num) + + # our evolutionary loop + # gen_num can still be -1. + while gen_num < num_generations - 1: + gen_num += 1 + population = await population.next_generation(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) diff --git a/experiments/examples/one_robot.py b/experiments/examples/one_robot.py new file mode 100644 index 0000000000..d16c6a1dd6 --- /dev/null +++ b/experiments/examples/one_robot.py @@ -0,0 +1,53 @@ +import asyncio +import logging +import sys +import os + +from pyrevolve import parser +from pyrevolve.custom_logging import logger +from pyrevolve.revolve_bot import RevolveBot +from pyrevolve.SDF.math import Vector3 +from pyrevolve.tol.manage import World +from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor +from pyrevolve.evolution import fitness + + +async def run(): + # Parse command line / file input arguments + settings = parser.parse_args() + + # Start Simulator + if settings.simulator_cmd != 'debug': + simulator_supervisor = DynamicSimSupervisor( + world_file=settings.world, + simulator_cmd=settings.simulator_cmd, + simulator_args=["--verbose"], + plugins_dir_path=os.path.join('.', 'build', 'lib'), + models_dir_path=os.path.join('.', 'models'), + simulator_name='gazebo' + ) + await simulator_supervisor.launch_simulator(port=settings.port_start) + await asyncio.sleep(0.1) + + # Connect to the simulator and pause + connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) + await asyncio.sleep(1) + + # init finished + robot_file_path = 'experiments/examples/yaml/spider.yaml' + robot = RevolveBot() + robot.load_file(robot_file_path, conf_type='yaml') + robot.update_substrate() + robot.save_file(f'{robot_file_path}.sdf', conf_type='sdf') + + # insert robot + await connection.pause(False) + robot_manager = await connection.insert_robot(robot, Vector3(0, 0, 2.0), life_timeout=None) + await asyncio.sleep(1.0) + + # restart simulation + await connection.pause(False) + + # Start the main life loop + while True: + await asyncio.sleep(1.0) diff --git a/experiments/examples/run-experiments b/experiments/examples/run-experiments index fcbcc5e2b3..7f10183580 100755 --- a/experiments/examples/run-experiments +++ b/experiments/examples/run-experiments @@ -1,9 +1,7 @@ #!/bin/bash -while true - do - - for i in {1..10}; do - ./revolve.py --experiment-name default_experiment_$i --run $i --manager experiments/examples/manager_pop.py --n-cores 5; - sleep 5s - done -done \ No newline at end of file +while true; do + for i in {1..10}; do + ./revolve.py --experiment-name default_experiment_$i --run $i --manager experiments/examples/manager_pop.py --n-cores 5; + sleep 5s + done +done diff --git a/experiments/examples/tutorial3.py b/experiments/examples/tutorial3.py index e7c4196263..4c678ef9c4 100755 --- a/experiments/examples/tutorial3.py +++ b/experiments/examples/tutorial3.py @@ -57,4 +57,3 @@ async def run(): status = 'dead' if robot_manager.dead else 'alive' print(f"Robot fitness ({status}) is: {fitness.displacement(robot_manager, robot)} \n") await asyncio.sleep(1.0) - diff --git a/experiments/examples/yaml/spider.yaml b/experiments/examples/yaml/spider.yaml index c150f8efbe..5ca194386b 100644 --- a/experiments/examples/yaml/spider.yaml +++ b/experiments/examples/yaml/spider.yaml @@ -145,4 +145,17 @@ body: blue: 0.72 orientation : 0 brain: - type: rlpower-splines + type: cpg + learner: + type : none + controller: + reset_neuron_random : false; + use_frame_of_reference : false; + init_neuron_state : 0.707; + range_ub : 1.0; + signal_factor_all : 4.0; + signal_factor_mid : 2.5; + signal_factor_left_right : 2.5; + abs_output_bound : 1.0; + weights: [0.482167, 0.560357, 0.753772, 0.221536, 0.44513, 0.667353, 0.580933, 0.246228, 0.111797, + 0.110425, 0.667353, 0.519204, 0.11134, 0.667353, 0.70439, 0.000228624, 0.444673, 0.287837] diff --git a/experiments/examples/yaml/spider9.yaml b/experiments/examples/yaml/spider9.yaml index d52c547299..7b4268861a 100644 --- a/experiments/examples/yaml/spider9.yaml +++ b/experiments/examples/yaml/spider9.yaml @@ -145,4 +145,15 @@ body: blue: 0.72 orientation : 0 brain: - type: bo-cpg + type: cppn-cpg + learner: + type: none + controller: + reset_neuron_random: false; + use_frame_of_reference: false; + init_neuron_state: 0.707; + range_ub: 1.0; + signal_factor_all: 4.0; + signal_factor_mid: 2.5; + signal_factor_left_right: 2.5; + abs_output_bound: 1.0; diff --git a/experiments/hardware/generate_robot_config.py b/experiments/hardware/generate_robot_config.py new file mode 100755 index 0000000000..8103dec9a8 --- /dev/null +++ b/experiments/hardware/generate_robot_config.py @@ -0,0 +1,69 @@ +""" +This script loads a robot.yaml file and creates the corresponding SDF and robot_config.yaml for the robot + +This is useful to create the hardware configuration for the robot, starting from the yaml description of the robot. +""" + +import os +import sys +import yaml +from collections import OrderedDict +import xml.etree.ElementTree + +# Add `..` folder in search path +current_dir = os.path.dirname(os.path.abspath(__file__)) +newpath = os.path.join(current_dir, '..', '..') +sys.path.append(newpath) + +from pyrevolve import revolve_bot + + +async def run(): + # Load a robot from yaml + robot = revolve_bot.RevolveBot() + robot.load_file("experiments/examples/yaml/spider.yaml") + robot.update_substrate() + + # Generate and Save the SDF file of the robot + # robot.save_file('robot.sdf.xml', conf_type='sdf') + robot_sdf = xml.etree.ElementTree.fromstring(robot.to_sdf()) + + # Generate the robot_config.yaml + ns = {'rv': 'https://github.com/ci-group/revolve'} + + sdf_model = robot_sdf.find('model') + sdf_plugin = sdf_model.find('plugin') + sdf_robot_config = sdf_plugin.find('rv:robot_config', ns) + sdf_brain = sdf_robot_config.find('rv:brain', ns) + sdf_actuators = sdf_brain.find('rv:actuators', ns) + + servos = [] + for actuator in sdf_actuators: + coordinates = [float(v) for v in actuator.attrib['coordinates'].split(';')] + servos.append({ + 'pin': -1, + 'name': actuator.attrib['part_name'], + 'coordinates': coordinates, + 'inverse': False, + }) + + raspberry_yaml_conf = OrderedDict() + raspberry_yaml_conf['robot_name'] = sdf_model.attrib['name'] + raspberry_yaml_conf['robot_id'] = 1 + raspberry_yaml_conf['robot_address'] = { + # ip: "192.168.1.12" + # port: 8888 + } + raspberry_yaml_conf['servos'] = servos + raspberry_yaml_conf['rgb_pins'] = [15, 14, 4] + raspberry_yaml_conf['controller'] = { + 'type': "differential-cpg", + # spider weights + 'weights': [], + } + + with open('robot_conf.yaml', 'w') as robot_file: + robot_file.write(yaml.dump(raspberry_yaml_conf)) + + print('generated file "robot_conf.yaml", you still need to insert the correct pin numbers and check if some of the ' + 'servos need to be Inverted') diff --git a/experiments/hardware/visualize_robot_in_simulation.py b/experiments/hardware/visualize_robot_in_simulation.py new file mode 100644 index 0000000000..d21642d468 --- /dev/null +++ b/experiments/hardware/visualize_robot_in_simulation.py @@ -0,0 +1,61 @@ +""" +This script loads a robot.yaml file in simulation and sets all of the servos to -1 + +This is useful to create the hardware configuration for the robot, to set the inversion of the servos correctly. +""" +import asyncio +import os +import sys + +# Add `..` folder in search path +current_dir = os.path.dirname(os.path.abspath(__file__)) +newpath = os.path.join(current_dir, '..', '..') +sys.path.append(newpath) + +from pyrevolve.SDF.math import Vector3 +from pyrevolve.revolve_bot.brain.fixed_angle import FixedAngleBrain +from pyrevolve import revolve_bot, parser +from pyrevolve.revolve_bot import RevolveBot +from pyrevolve.tol.manage import World +from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor + + +async def run(): + robot_file_path = "experiments/examples/yaml/spider.yaml" + + # Parse command line / file input arguments + settings = parser.parse_args() + + # Start Simulator + if settings.simulator_cmd != 'debug': + simulator_supervisor = DynamicSimSupervisor( + world_file=settings.world, + simulator_cmd=settings.simulator_cmd, + simulator_args=["--verbose"], + plugins_dir_path=os.path.join('.', 'build', 'lib'), + models_dir_path=os.path.join('.', 'models'), + simulator_name='gazebo' + ) + await simulator_supervisor.launch_simulator(port=settings.port_start) + await asyncio.sleep(0.1) + + # Connect to the simulator and pause + connection = await World.create(settings, world_address=('127.0.0.1', settings.port_start)) + await asyncio.sleep(1) + + # init finished + + robot = RevolveBot(_id=settings.test_robot) + robot.load_file(robot_file_path, conf_type='yaml') + robot._brain = FixedAngleBrain(-1) + + await connection.pause(True) + robot_manager = await connection.insert_robot(robot, Vector3(0, 0, 0.25), life_timeout=None) + await asyncio.sleep(1.0) + + # Start the main life loop + while True: + try: + await asyncio.sleep(1.0) + except InterruptedError: + break diff --git a/experiments/unmanaged/create_charts.py b/experiments/unmanaged/create_charts.py index ea6b23a835..87ad60cf1a 100755 --- a/experiments/unmanaged/create_charts.py +++ b/experiments/unmanaged/create_charts.py @@ -6,11 +6,14 @@ import matplotlib import matplotlib.pyplot as plt import numpy as np +from scipy.ndimage import gaussian_filter from pyrevolve.SDF.math import Vector3 from pyrevolve.revolve_bot import RevolveBot +Inf = float('Inf') + def parse_vec3(source: str): # example source: Vector3(2.394427e+00, 3.195821e-01, 2.244915e-02) assert (source[:8] == 'Vector3(') @@ -153,7 +156,8 @@ def draw_chart(folder_name: str, ax): else: raise RuntimeError("WAT?") - return robot_points_new, robot_points_new_pop, \ + return data, \ + robot_points_new, robot_points_new_pop, \ robot_points_mate, robot_points_mate_pop, \ robot_points_death, robot_points_death_pop, \ robot_speed @@ -180,6 +184,74 @@ def calculate_min_max_len(data): return _min, _max, _len +def robot_density(robots, label='start_pos', sigma=1.0): + positions = [] + # sizes: + min_x = -10 + max_x = 10 + min_y = -10 + max_y = 20 + for key, robot in robots.items(): + pos = parse_vec3(robot.life[label]) + positions.append(pos) + x = pos[0] + y = pos[1] + + if min_x > x: + min_x = x + elif x > max_x: + max_x = x + + if min_y > y: + min_y = y + elif y > max_y: + max_y = y + + min_x = int(min_x) + max_x = int(max_x + 3.5) + min_y = int(min_y) + max_y = int(max_y + 3.5) + + # creating meshgrid + delta = 1 + x = np.arange(min_x, max_x, delta) + y = np.arange(min_y, max_y, delta) + X, Y = np.meshgrid(x, y) + + # densities calculation: + Z = np.zeros_like(X, dtype=int) + for pos in positions: + if not (min_x < pos[0] < max_x-1): + print(f"WARNING, X = {pos[0]}") + continue + if not (min_y < pos[1] < max_y-1): + print(f"WARNING, Y = {pos[1]}") + continue + x_round = round(pos[0] + 0.5) + y_round = round(pos[1] + 0.5) + + x = int(x_round - min_x) + y = int(y_round - min_y) + #TODO multiply for delta != 1 + + try: + Z[x][y] += 1 + except IndexError: + print(f"DIOCANE Z[{x}][{y}] += 1") + + return X, Y, gaussian_filter(Z, sigma=sigma) + # return X, Y, Z + + +def countour_plot(robots, label='start_pos', sigma=1.0): + X, Y, Z = robot_density(robots, label, sigma=sigma) + + fig, ax = plt.subplots() + CS = ax.contour(X, Y, Z) + ax.clabel(CS, inline=1, fontsize=10) + ax.set_title(f'{label} - sigma={sigma}') + + if __name__ == '__main__': folder_name = sys.argv[1] live_update = False @@ -201,7 +273,7 @@ def calculate_min_max_len(data): if live_update: plt.ion() - robot_points_new, robot_points_new_pop, \ + data, robot_points_new, robot_points_new_pop, \ robot_points_mate, robot_points_mate_pop, \ robot_points_death, robot_points_death_pop, \ robot_speed = draw_chart(folder_name, ax) @@ -218,6 +290,13 @@ def calculate_min_max_len(data): if save_png: plt.savefig(os.path.join(folder_name, 'population-speed.png'), bbox_inches='tight') + countour_plot(data['robots'], 'start_pos', sigma=0.6) + if save_png: + plt.savefig(os.path.join(folder_name, 'population-start_pos-contour.png'), bbox_inches='tight') + countour_plot(data['robots'], 'last_pos', sigma=0.6) + if save_png: + plt.savefig(os.path.join(folder_name, 'population-last_pos-contour.png'), bbox_inches='tight') + if not live_update and not save_png: plt.show() elif not save_png: @@ -234,7 +313,7 @@ def update_data(dataset, points, points_pop): return min(X), max(X), min(Y), max(Y) while True: - robot_points_new, robot_points_new_pop, \ + data, robot_points_new, robot_points_new_pop, \ robot_points_mate, robot_points_mate_pop, \ robot_points_death, robot_points_death_pop, \ robot_speed = draw_chart(folder_name, ax) diff --git a/pyrevolve/SDF/revolve_bot_sdf_builder.py b/pyrevolve/SDF/revolve_bot_sdf_builder.py index a577f5f02a..9457df269f 100644 --- a/pyrevolve/SDF/revolve_bot_sdf_builder.py +++ b/pyrevolve/SDF/revolve_bot_sdf_builder.py @@ -11,9 +11,11 @@ def revolve_bot_to_sdf(robot, robot_pose, nice_format, self_collide=True): sdf_root = ElementTree.Element('sdf', {'version': '1.6'}) - assert (robot.id is not None) + robot_id = robot.id + assert (robot_id is not None) + robot_id = f"robot_{robot_id}" model = ElementTree.SubElement(sdf_root, 'model', { - 'name': str(robot.id) + 'name': str(robot_id) }) pose_elem = SDF.Pose(robot_pose) @@ -150,13 +152,13 @@ def _module_to_sdf(module, parent_link, parent_slot: BoxSlot, parent_collision, joint = module.to_sdf('{}'.format(slot_chain), parent_link, child_link) # parent_slot = parent_module.boxslot(parent_slot) - module_slot = module.boxslot_frame(Orientation.SOUTH) + module_slot = module.boxslot_frame(Orientation.BACK) _sdf_attach_module(module_slot, module.orientation, visual_frame, collisions_frame[0], parent_slot, parent_collision) - parent_slot = module.boxslot_frame(Orientation.NORTH) - module_slot = module.boxslot_servo(Orientation.SOUTH) + parent_slot = module.boxslot_frame(Orientation.FORWARD) + module_slot = module.boxslot_servo(Orientation.BACK) _sdf_attach_module(module_slot, None, visual_servo, collisions_servo[0], parent_slot, collisions_frame[0]) @@ -201,7 +203,7 @@ def _module_to_sdf(module, parent_link, parent_slot: BoxSlot, parent_collision, else: visual, collision, sensor = module.to_sdf(slot_chain, my_link) - module_slot = module.boxslot(Orientation.SOUTH) + module_slot = module.boxslot(Orientation.BACK) _sdf_attach_module(module_slot, module.orientation, visual, collision, parent_slot, parent_collision) diff --git a/pyrevolve/angle/evolve.py b/pyrevolve/angle/evolve.py index 9e6059e347..52c8daae22 100644 --- a/pyrevolve/angle/evolve.py +++ b/pyrevolve/angle/evolve.py @@ -5,9 +5,7 @@ import math import random -from .representation import Tree, Node - -from ..spec.msgs import BodyPart, Neuron, Robot +from .representation import Tree, Node, Neuron, Robot, BodyPart from ..util import decide diff --git a/pyrevolve/angle/manage/robotmanager.py b/pyrevolve/angle/manage/robotmanager.py index 2daedcd74e..93e14e7c7d 100644 --- a/pyrevolve/angle/manage/robotmanager.py +++ b/pyrevolve/angle/manage/robotmanager.py @@ -5,6 +5,7 @@ from collections import deque from pyrevolve.SDF.math import Vector3, Quaternion +from pyrevolve.revolve_bot.revolve_module import Orientation from pyrevolve.util import Time import math import os @@ -51,11 +52,12 @@ def __init__( self._dt = deque(maxlen=speed_window) self._positions = deque(maxlen=speed_window) self._orientations = deque(maxlen=speed_window) + self._orientation_vecs = deque(maxlen=speed_window) self._contacts = deque(maxlen=speed_window) self._seconds = deque(maxlen=speed_window) self._times = deque(maxlen=speed_window) - self._dist = 0 + self._dist = 0. self._time = 0 self._idx = 0 self._count = 0 @@ -70,7 +72,7 @@ def __init__( @property def name(self): - return str(self.robot.id) + return str(f'robot_{self.robot.id}') def update_state(self, world, time, state, poses_file): """ @@ -96,6 +98,17 @@ def update_state(self, world, time, state, poses_file): euler = qua.get_rpy() euler = np.array([euler[0], euler[1], euler[2]]) # roll / pitch / yaw + vec_forward = state.orientation_vecs.vec_forward + vec_left = state.orientation_vecs.vec_left + vec_back = state.orientation_vecs.vec_back + vec_right = state.orientation_vecs.vec_right + orientation_vecs = { + Orientation.FORWARD: Vector3(vec_forward.x, vec_forward.y, vec_forward.z), + Orientation.LEFT: Vector3(vec_left.x, vec_left.y, vec_left.z), + Orientation.BACK: Vector3(vec_back.x, vec_back.y, vec_back.z), + Orientation.RIGHT: Vector3(vec_right.x, vec_right.y, vec_right.z), + } + age = world.age() if self.starting_time is None: @@ -143,6 +156,7 @@ def update_state(self, world, time, state, poses_file): self._ds.append(ds) self._dt.append(dt) self._orientations.append(euler) + self._orientation_vecs.append(orientation_vecs) self._seconds.append(age.sec) def update_contacts(self, world, module_contacts): diff --git a/pyrevolve/angle/manage/world.py b/pyrevolve/angle/manage/world.py index 31c8fd9a9b..9b2e1fa863 100644 --- a/pyrevolve/angle/manage/world.py +++ b/pyrevolve/angle/manage/world.py @@ -1,5 +1,4 @@ -from __future__ import absolute_import -from __future__ import print_function +from __future__ import annotations import csv import os @@ -13,16 +12,20 @@ from pygazebo.msg import gz_string_pb2 from pygazebo.msg.contacts_pb2 import Contacts +from .robotmanager import RobotManager from pyrevolve.SDF.math import Vector3 from pyrevolve.spec.msgs import BoundingBox from pyrevolve.spec.msgs import ModelInserted from pyrevolve.spec.msgs import RobotStates -from .robotmanager import RobotManager -from ...gazebo import manage -from ...gazebo import RequestHandler -from ...util import multi_future -from ...util import Time -from ...custom_logging.logger import logger +from pyrevolve.gazebo import manage, RequestHandler +from pyrevolve.util import multi_future, Time +from pyrevolve.custom_logging.logger import logger + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Optional, Union + from pyrevolve.SDF import Pose + from pyrevolve.revolve_bot import RevolveBot class WorldManager(manage.WorldManager): @@ -409,9 +412,9 @@ async def analyze_tree(self, tree): async def insert_robot( self, - revolve_bot, - pose=Vector3(0, 0, 0.05), - life_timeout=None, + revolve_bot: RevolveBot, + pose: Union[Pose, Vector3] = Vector3(0, 0, 0.05), + life_timeout: Optional[float] = None, ): """ Inserts a robot into the world. This consists of two steps: @@ -436,9 +439,20 @@ async def insert_robot( # if the ID is digit, when removing the robot, the simulation will try to remove random stuff from the # environment and give weird crash errors - assert(not str(revolve_bot.id).isdigit()) sdf_bot = revolve_bot.to_sdf(pose) + + # assert the robot id is not bad + # assert not str(revolve_bot.id).isdigit() + import xml.dom.minidom + reparsed = xml.dom.minidom.parseString(sdf_bot) + for model in reparsed.documentElement.getElementsByTagName('model'): + robot_name = model.getAttribute('name') + if str(robot_name).isdigit(): + error_message = f'Inserting robot with invalid name: {robot_name}' + logger.critical(error_message) + raise RuntimeError(error_message) + logger.info("Inserting robot {}.".format(robot_name)) # if self.output_directory: # robot_file_path = os.path.join( diff --git a/pyrevolve/angle/representation.py b/pyrevolve/angle/representation.py index 15e31f7e1b..65b938f8b3 100644 --- a/pyrevolve/angle/representation.py +++ b/pyrevolve/angle/representation.py @@ -1,9 +1,57 @@ from __future__ import absolute_import import copy +from typing import List -from ..spec.msgs import Robot -from ..spec.msgs import BodyPart + +class Robot(object): + def __init__(self): + self.id: int = None + self.body: BodyPart = BodyPart() + self.brain: NeuralNetwork = NeuralNetwork() + + +class Neuron(object): + def __init__(self): + self.id: str = "" + self.layer: str = "" + self.type: str = "" + self.partId: str = "" + self.param = {} + + def CopyFrom(self, other): + self.id = other.id + self.layer = other.layer + self.type = other.type + self.partId = other.partId + self.param = other.param + + +class NeuralNetwork(object): + + def __init__(self): + self.neuron: List[Neuron] = [] + self.connection: List[NeuralConnection] = [] + + +class BodyPart(object): + def __init__(self): + self.id = None + self.type = None + self.orientation = None + self.child = None + self.x = None + self.y = None + self.param = {} + + def CopyFrom(self, other): + self.id = other.id + self.type = other.type + self.orientation = other.orientation + self.x = other.x + self.y = other.y + self.child = other.child + self.params = other.params def _create_subtree(body_part, brain, body_spec): @@ -23,6 +71,22 @@ def _create_subtree(body_part, brain, body_spec): return node +def _create_body_subtree(body_part, body_spec): + """ + :param body_part: + :param brain: + :param body_spec: + :return: + """ + # Gather neurons for this part + node = Node(body_part, [], body_spec) + for conn in body_part.child: + subtree = _create_body_subtree(conn.part, body_spec) + node.set_connection(conn.src_slot, conn.dst_slot, subtree) + + return node + + class Tree(object): """ A tree to represent a robot that can be used for evolution. @@ -43,6 +107,7 @@ def __init__(self, root): # Maps node IDs to nodes for looked up nodes self._nodes = {} + self._explore_tree(self.root) def to_protobot(self, robot_id=0): """ @@ -80,7 +145,7 @@ def from_body_brain(body, brain, body_spec): # Generate neuron map, make sure every neuron is assigned to a part neuron_map = {} for neuron in brain.neuron: - if not neuron.HasField("partId"): + if neuron.partId is None: raise Exception("Neuron {} not associated with any " "part.".format(neuron.id)) @@ -111,6 +176,39 @@ def from_body_brain(body, brain, body_spec): return tree + @staticmethod + def from_body(body, body_spec): + """ + Creates a tree from a body and a brain. Every neuron will need + to have an assigned part ID in order for this to work. + + :param body: + :type body: Body + :param brain: + :type brain: NeuralNetwork + :type body_spec: BodyImplementation + :param body_spec: + :return: + """ + # Generate neuron map, make sure every neuron is assigned to a part + neuron_map = {} + + # Create the tree without neural net connections + root = _create_body_subtree( + body_part=body.root, + body_spec=body_spec + ) + tree = Tree(root) + + return tree + + def _explore_tree(self, root): + key = root.part.id + if key not in self._nodes: + self._nodes[key] = root + for child in root.child_connections(): + self._explore_tree(child.node) + def get_node(self, node_id): """ Returns the node with the given ID from this @@ -178,12 +276,12 @@ def __init__(self, part, neurons, body_spec): self.part.x = part.x self.part.y = part.y - if part.HasField("label"): - self.part.label = part.label + #if part.HasField("label"): + # self.part.label = part.label - for param in part.param: - new_param = self.part.param.add() - new_param.CopyFrom(param) + #for param in part.param: + # new_param = self.part.param.add() + # new_param.CopyFrom(param) # Maps slot ids to other nodes self.connections = {} @@ -198,6 +296,7 @@ def __init__(self, part, neurons, body_spec): inputs = sum(1 for n in neurons if n.layer == "input") outputs = sum(1 for n in neurons if n.layer == "output") if inputs != self.spec.inputs or outputs != self.spec.outputs: + print(inputs, outputs, self.spec.inputs, self.spec.outputs) raise Exception("Part input / output mismatch.") # Performance caches @@ -205,6 +304,9 @@ def __init__(self, part, neurons, body_spec): self._len = -1 self._io = None + def __repr__(self): + return " " + str(self.part.id) + " " + str(self.part.type) + " " + str(self.part.orientation) + @property def id(self): """ diff --git a/pyrevolve/angle/robogen/body_parts/__init__.py b/pyrevolve/angle/robogen/body_parts/__init__.py index dbe1b7ef18..13cefb35ef 100644 --- a/pyrevolve/angle/robogen/body_parts/__init__.py +++ b/pyrevolve/angle/robogen/body_parts/__init__.py @@ -1,17 +1,17 @@ from __future__ import absolute_import -from .active_hinge import ActiveHinge -from .core_component import CoreComponent -from .hinge import Hinge -from .light_sensor import LightSensor -from .touch_sensor import TouchSensor -from .fixed_brick import FixedBrick -from .parametric_bar_joint import ParametricBarJoint -from .wheel import * -from .cardan import * -from .active_cardan import * -from .active_rotator import * -from .active_wheel import * -from .active_wheg import * +#from .active_hinge import ActiveHinge +#from .core_component import CoreComponent +#from .hinge import Hinge +#from .light_sensor import LightSensor +#from .touch_sensor import TouchSensor +#from .fixed_brick import FixedBrick +#from .parametric_bar_joint import ParametricBarJoint +#from .wheel import * +#from .cardan import * +#from .active_cardan import * +#from .active_rotator import * +#from .active_wheel import * +#from .active_wheg import * __author__ = 'Elte Hupkes' diff --git a/pyrevolve/angle/robogen/config.py b/pyrevolve/angle/robogen/config.py index 9a22683020..24b3fd73bb 100644 --- a/pyrevolve/angle/robogen/config.py +++ b/pyrevolve/angle/robogen/config.py @@ -7,6 +7,8 @@ def __init__(self, max_parts, max_inputs, max_outputs, + initial_parts_mu, + initial_parts_sigma, body_mutation_epsilon=0.05, enforce_planarity=True, disable_sensors=False, @@ -23,3 +25,5 @@ def __init__(self, self.enable_wheel_parts = enable_wheel_parts self.max_parts = max_parts self.min_parts = min_parts + self.initial_parts_mu = initial_parts_mu + self.initial_parts_sigma = initial_parts_sigma diff --git a/pyrevolve/angle/robogen/spec/body.py b/pyrevolve/angle/robogen/spec/body.py index ae23841935..abdd07d475 100644 --- a/pyrevolve/angle/robogen/spec/body.py +++ b/pyrevolve/angle/robogen/spec/body.py @@ -2,9 +2,10 @@ import random -from ....generate import FixedOrientationBodyGenerator +from .. import Config +from ....revolve_bot.revolve_module import CoreModule, ActiveHingeModule, BrickModule, TouchSensorModule +from ....revolve_bot.body import FixedOrientationBodyGenerator from ....spec import BodyImplementation, PartSpec, ParamSpec -from ..body_parts import * # A utility function to generate color property parameters. Note that color # parameters do not mutate. @@ -28,128 +29,34 @@ def get_body_spec(conf): """ parts = { "Core": PartSpec( - body_part=CoreComponent, + body_part=CoreModule, arity=4, inputs=0 if conf.disable_sensors else 6, params=color_params ), "FixedBrick": PartSpec( - body_part=FixedBrick, + body_part=BrickModule, arity=4, params=color_params ), "ActiveHinge": PartSpec( - body_part=ActiveHinge, + body_part=ActiveHingeModule, arity=2, outputs=1, params=color_params ), - "Hinge": PartSpec( - body_part=Hinge, - arity=2, - params=color_params - ), - "ParametricBarJoint": PartSpec( - body_part=ParametricBarJoint, - arity=2, - params=[ParamSpec( - "connection_length", - default=50, - min_value=20, - max_value=100, - epsilon=conf.body_mutation_epsilon - ), ParamSpec( - "alpha", - default=0, - min_value=-0.5 * math.pi, - max_value=0.5 * math.pi, - epsilon=conf.body_mutation_epsilon - ), ParamSpec( - "beta", - default=0, - min_value=0, - max_value=0 if conf.enforce_planarity else math.pi, - epsilon=conf.body_mutation_epsilon - )] + color_params - ) - } - if conf.enable_wheel_parts: - parts.update({ - "Wheel": PartSpec( - body_part=Wheel, - arity=1, - params=color_params + [ - ParamSpec( - "radius", - min_value=40, - max_value=80, - default=60, - epsilon=conf.body_mutation_epsilon) - ] - ), - "ActiveWheel": PartSpec( - body_part=ActiveWheel, - arity=1, - outputs=1, - params=color_params + [ - ParamSpec( - "radius", - min_value=40, - max_value=80, - default=60, - epsilon=conf.body_mutation_epsilon) - ] - ), - "Cardan": PartSpec( - body_part=Cardan, - arity=2, - params=color_params - ), - "ActiveCardan": PartSpec( - body_part=ActiveCardan, - arity=2, - outputs=2, - params=color_params - ), - "ActiveRotator": PartSpec( - body_part=ActiveRotator, - arity=2, - outputs=1, - params=color_params - ), - "ActiveWheg": PartSpec( - body_part=ActiveWheg, - arity=2, - outputs=1, - params=color_params + [ - ParamSpec( - "radius", - min_value=40, - max_value=80, - default=60, - epsilon=conf.body_mutation_epsilon) - ] - ) - }) + } if not conf.disable_sensors: if conf.enable_touch_sensor: parts['TouchSensor'] = PartSpec( - body_part=TouchSensor, + body_part=TouchSensorModule, arity=1, inputs=2, params=color_params ) - if conf.enable_light_sensor: - parts['LightSensor'] = PartSpec( - body_part=LightSensor, - arity=1, - inputs=1, - params=color_params - ) - return BodyImplementation(parts) @@ -158,7 +65,7 @@ class BodyGenerator(FixedOrientationBodyGenerator): Body generator for ToL with some additions """ - def __init__(self, conf): + def __init__(self, conf: Config): """ """ body_spec = get_body_spec(conf) diff --git a/pyrevolve/angle/robogen/spec/robot.py b/pyrevolve/angle/robogen/spec/robot.py index a901cb440e..7b1b868705 100644 --- a/pyrevolve/angle/robogen/spec/robot.py +++ b/pyrevolve/angle/robogen/spec/robot.py @@ -1,6 +1,6 @@ from __future__ import absolute_import -from ... import TreeGenerator +from ... import TreeGenerator, Tree class RobogenTreeGenerator(TreeGenerator): @@ -15,7 +15,7 @@ def __init__(self, body_gen, brain_gen, conf): self.conf = conf super(RobogenTreeGenerator, self).__init__(body_gen, brain_gen) - def generate_tree(self): + def generate_tree(self) -> Tree: """ Overrides `generate_tree` to force robot planarity. Robots without output neurons are also discarded because we can be diff --git a/pyrevolve/config.py b/pyrevolve/config.py index 2f22e1bd56..c1ddaf574f 100644 --- a/pyrevolve/config.py +++ b/pyrevolve/config.py @@ -72,6 +72,13 @@ def str_to_address(v): help="Determine which manager to use. Defaults to no manager." ) +parser.add_argument( + '--fitness', + default="displacement_velocity", + type=str, + help="Determine which manager to use. Defaults to no manager." +) + parser.add_argument( '--experiment-name', default='default_experiment', type=str, @@ -91,6 +98,18 @@ def str_to_address(v): "Loads a yaml robot." ) +parser.add_argument( + '--record', + default=False, type=bool, + help="When running with --test-robot argument, records the video of the test run" +) + +parser.add_argument( + '--plot-test-robot', + default=False, type=bool, + help="When testing a robot, plot the data instead of printing it to the terminal. Default False." +) + parser.add_argument( '--test-robot-collision', default=None, type=str, @@ -135,12 +154,20 @@ def str_to_address(v): parser.add_argument( '--evaluation-time', default=30, type=float, - help="In offline evolution, this determines the length of the experiment run." + help="In offline evolution, this determines the length of the evaluation time. " + "A single run time will be evaluation-time + grace-time" # For old_online_fitness: # "The size of the `speed window` for each robot, i.e. the number of " # "past (simulation) seconds over which its speed is evaluated." ) +parser.add_argument( + '--grace-time', + default=0, type=float, + help="In offline evolution, this determines how much time before the start of an evaluation." + # For old_online_fitness it's useless. +) + parser.add_argument( '--recovery-enabled', default=True, type=str_to_bool, diff --git a/pyrevolve/data_analisys/visualize_robot.py b/pyrevolve/data_analisys/visualize_robot.py index aada5c5bab..b95c286f17 100644 --- a/pyrevolve/data_analisys/visualize_robot.py +++ b/pyrevolve/data_analisys/visualize_robot.py @@ -2,16 +2,115 @@ import logging import sys import os +import math +import numpy as np +from typing import List from pyrevolve import parser from pyrevolve.custom_logging import logger from pyrevolve.revolve_bot import RevolveBot from pyrevolve.SDF.math import Vector3 +from pyrevolve.revolve_bot.revolve_module import Orientation from pyrevolve.tol.manage import World from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor from pyrevolve.evolution import fitness +def rotation(robot_manager, _robot, factor_orien_ds: float = 0.0): + # TODO move to measurements? + orientations: float = 0.0 + delta_orientations: float = 0.0 + + assert len(robot_manager._orientations) == len(robot_manager._positions) + + fitnesses = [0.] + choices = ['None'] + i = 0 + + for i in range(1, len(robot_manager._orientations)): + rot_i_1 = robot_manager._orientations[i - 1] + rot_i = robot_manager._orientations[i] + + angle_i: float = rot_i[2] # roll / pitch / yaw + angle_i_1: float = rot_i_1[2] # roll / pitch / yaw + pi_2: float = math.pi / 2.0 + + if angle_i_1 > pi_2 and angle_i < - pi_2: # rotating left + choice = 'A' + delta_orientations = (2.0 * math.pi + angle_i - angle_i_1) #% (math.pi * 2.0) + elif (angle_i_1 < - pi_2) and (angle_i > pi_2): + choice = 'B' + delta_orientations = - (2.0 * math.pi - angle_i + angle_i_1) #% (math.pi * 2.0) + else: + choice = 'C' + delta_orientations = angle_i - angle_i_1 + #print(f"{choice} {i}\t{delta_orientations:2.0f}\t= {angle_i:2.0f} - {angle_i_1:2.0f}") + i += 1 + orientations += delta_orientations + fitnesses.append(orientations) + choices.append(choice) + + fitnesses = np.array(fitnesses) + fitnesses -= factor_orien_ds * robot_manager._dist + return (fitnesses, choices) + + +def panoramic_rotation(robot_manager, robot: RevolveBot, vertical_angle_limit: float = math.pi/4): + total_angle = 0.0 + total_angles = [0.] + vertical_limit = math.sin(vertical_angle_limit) + + # decide which orientation to choose, [0] is correct because the "grace time" values are discarded by the deques + if len(robot_manager._orientation_vecs) == 0: + return total_angles + + # Chose orientation base on the + chosen_orientation = None + min_z = 1.0 + for orientation, vec in robot_manager._orientation_vecs[0].items(): + z = abs(vec.z) + if z < min_z: + chosen_orientation = orientation + min_z = z + print(f"Chosen orientation for robot {robot.id} is {chosen_orientation}") + + vec_list = [vecs[chosen_orientation] for vecs in robot_manager._orientation_vecs] + + for i in range(1, len(robot_manager._orientation_vecs)): + # from: https://code-examples.net/en/q/d6a4f5 + # more info: https://en.wikipedia.org/wiki/Atan2 + # Just like the dot product is proportional to the cosine of the angle, + # the determinant is proportional to its sine. So you can compute the angle like this: + # + # dot = x1*x2 + y1*y2 # dot product between [x1, y1] and [x2, y2] + # det = x1*y2 - y1*x2 # determinant + # angle = atan2(det, dot) # atan2(y, x) or atan2(sin, cos) + # + # The function atan2(y,x) (from "2-argument arctangent") is defined as the angle in the Euclidean plane, + # given in radians, between the positive x axis and the ray to the point (x, y) ≠ (0, 0). + + # u = prev vector + # v = curr vector + u: Vector3 = vec_list[i-1] + v: Vector3 = vec_list[i] + + # if vector is too vertical, fail the fitness + # (assuming these are unit vectors) + if abs(u.z) > vertical_limit: + while len(total_angles) < len(robot_manager._orientations): + total_angles.append(total_angles[-1]) + return total_angles + + dot = u.x*v.x + u.y*v.y # dot product between [x1, y1] and [x2, y2] + det = u.x*v.y - u.y*v.x # determinant + delta = math.atan2(det, dot) # atan2(y, x) or atan2(sin, cos) + + total_angle += delta + total_angles.append(total_angle) + + return total_angles + + async def test_robot_run(robot_file_path: str): log = logger.create_logger('experiment', handlers=[ logging.StreamHandler(sys.stdout), @@ -23,10 +122,17 @@ async def test_robot_run(robot_file_path: str): # Parse command line / file input arguments settings = parser.parse_args() + world: str = settings.world + if settings.record: + # world = "plane.recording.world" + _world = world.split('.') # type: List[str] + _world.insert(-1, 'recording') + world = '.'.join(_world) + # Start Simulator if settings.simulator_cmd != 'debug': simulator_supervisor = DynamicSimSupervisor( - world_file=settings.world, + world_file=world, simulator_cmd=settings.simulator_cmd, simulator_args=["--verbose"], plugins_dir_path=os.path.join('.', 'build', 'lib'), @@ -44,18 +150,89 @@ async def test_robot_run(robot_file_path: str): robot = RevolveBot(_id=settings.test_robot) robot.load_file(robot_file_path, conf_type='yaml') + robot.update_substrate() robot.save_file(f'{robot_file_path}.sdf', conf_type='sdf') await connection.pause(True) robot_manager = await connection.insert_robot(robot, Vector3(0, 0, 0.25), life_timeout=None) await asyncio.sleep(1.0) - # Start the main life loop - while True: - # Print robot fitness every second - status = 'dead' if robot_manager.dead else 'alive' - print(f"Robot fitness ({status}) is \n" - f" OLD: {fitness.online_old_revolve(robot_manager)}\n" - f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n" - f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}") - await asyncio.sleep(1.0) + if settings.plot_test_robot: + import matplotlib.pyplot as plt + import matplotlib + gui_env = ['TKAgg', 'GTK3Agg', 'Qt5Agg', 'Qt4Agg', 'WXAgg'] + for gui in gui_env: + try: + print("testing", gui) + matplotlib.use(gui, warn=False, force=True) + from matplotlib import pyplot as plt + break + except Exception as e: + print(e) + continue + print("Using:", matplotlib.get_backend()) + plt.ion() + fig, ax1 = plt.subplots(1, 1) + SIZE = 300 + + line10, = ax1.plot([0 for i in range(SIZE)], [0 for i in range(SIZE)], '-', label='x') + line11, = ax1.plot([0 for i in range(SIZE)], [0 for i in range(SIZE)], '-', label='y') + line12, = ax1.plot([0 for i in range(SIZE)], [0 for i in range(SIZE)], '-', label='z') + line13, = ax1.plot([0 for i in range(SIZE)], [0 for i in range(SIZE)], '-', label='fitness') + # line20, = ax2.plot([0 for i in range(SIZE)], [0 for i in range(SIZE)], '-', label='x') + # line21, = ax2.plot([0 for i in range(SIZE)], [0 for i in range(SIZE)], '-', label='y') + # line22, = ax2.plot([0 for i in range(SIZE)], [0 for i in range(SIZE)], '-', label='z') + # line23, = ax2.plot([0 for i in range(SIZE)], [0 for i in range(SIZE)], '-', label='fitness') + ax1.legend() + # ax2.legend() + fig.show() + EPS = 0.1 + + def update(line, ax, x, y): + # print(f'x={x}') + # print(f'y={y}') + # line.set_data(x, y) + line.set_ydata(y) + line.set_xdata(x) + miny = min(min(y)-EPS, ax.get_ylim()[0]) + maxy = max(max(y)+EPS, ax.get_ylim()[1]) + ax.set_xlim(min(x)-EPS, max(x)+EPS) + ax.set_ylim(miny, maxy) + + while True: + await asyncio.sleep(0.1) + times = [float(t) for t in robot_manager._times] + steps = [i for i in range(len(times))] + vecs = [vec[Orientation.FORWARD] for vec in robot_manager._orientation_vecs] + xs = [float(v.x) for v in vecs] + ys = [float(v.y) for v in vecs] + zs = [float(v.z) for v in vecs] + # fitnesses, _ = rotation(robot_manager, robot) + fitnesses = panoramic_rotation(robot_manager, robot) + #fitness.panoramic_rotation(robot_manager, robot) + if len(times) < 2: + continue + assert len(times) == len(xs) + update(line10, ax1, times, xs) + update(line11, ax1, times, ys) + update(line12, ax1, times, zs) + update(line13, ax1, times, fitnesses) + # update(line20, ax2, steps, xs) + # update(line21, ax2, steps, ys) + # update(line22, ax2, steps, zs) + # update(line23, ax2, steps, fitnesses) + fig.canvas.draw() + fig.canvas.flush_events() + + else: + # Start the main life loop + while True: + # Print robot fitness every second + if not settings.record: + status = 'dead' if robot_manager.dead else 'alive' + print(f"Robot fitness ({status}) is \n" + f" OLD: {fitness.online_old_revolve(robot_manager)}\n" + f" DISPLAC: {fitness.displacement(robot_manager, robot)}\n" + f" DIS_VEL: {fitness.displacement_velocity(robot_manager, robot)}") + + await asyncio.sleep(1.0) diff --git a/pyrevolve/evolution/fitness.py b/pyrevolve/evolution/fitness.py index 95be1c03cf..4b64632171 100644 --- a/pyrevolve/evolution/fitness.py +++ b/pyrevolve/evolution/fitness.py @@ -1,6 +1,23 @@ +from __future__ import annotations + +import math import random as py_random +import sys +from typing import TYPE_CHECKING, Tuple + +from pyrevolve.custom_logging.logger import logger +from pyrevolve.revolve_bot.revolve_module import Orientation +from pyrevolve.SDF.math import Vector3 from pyrevolve.tol.manage import measures +if TYPE_CHECKING: + from pyrevolve.angle import RobotManager + from pyrevolve.revolve_bot import RevolveBot + + +def _distance_flat_plane(pos1: Vector3, pos2: Vector3): + return math.sqrt((pos1.x - pos2.x) ** 2 + (pos1.y - pos2.y) ** 2) + def stupid(_robot_manager, robot): return 1.0 @@ -45,15 +62,16 @@ def online_old_revolve(robot_manager): fitness_limit = 1.0 age = robot_manager.age() - if age < (0.25 * robot_manager.conf.evaluation_time) \ - or age < warmup_time: + if age < (0.25 * robot_manager.conf.evaluation_time) or age < warmup_time: # We want at least some data return 0.0 d = 1.0 - (fitness_size_discount * robot_manager.size) - v = d * (d_fac * measures.displacement_velocity(robot_manager) - + v_fac * measures.velocity(robot_manager) - + s_fac * robot_manager.size) + v = d * ( + d_fac * measures.displacement_velocity(robot_manager) + + v_fac * measures.velocity(robot_manager) + + s_fac * robot_manager.size + ) return v if v <= fitness_limit else 0.0 @@ -64,7 +82,7 @@ def displacement_velocity_hill(robot_manager, robot): elif _displacement_velocity_hill == 0: _displacement_velocity_hill = -0.1 # temp elif - # elif _displacement_velocity_hill > 0: + # elif _displacement_velocity_hill > 0: # _displacement_velocity_hill *= _displacement_velocity_hill return _displacement_velocity_hill @@ -81,3 +99,184 @@ def floor_is_lava(robot_manager, robot): fitness = _displacement_velocity_hill * _contacts return fitness + + +def rotation( + robot_manager: RobotManager, _robot: RevolveBot, factor_orien_ds: float = 0.0 +): + # TODO move to measurements? + orientations: float = 0.0 + delta_orientations: float = 0.0 + + assert len(robot_manager._orientations) == len(robot_manager._positions) + + for i in range(1, len(robot_manager._orientations)): + rot_i_1 = robot_manager._orientations[i - 1] + rot_i = robot_manager._orientations[i] + + angle_i: float = rot_i[2] # roll / pitch / yaw + angle_i_1: float = rot_i_1[2] # roll / pitch / yaw + pi_2: float = math.pi / 2.0 + + if angle_i_1 > pi_2 and angle_i < -pi_2: # rotating left + delta_orientations = 2.0 * math.pi + angle_i - angle_i_1 + elif (angle_i_1 < -pi_2) and (angle_i > pi_2): + delta_orientations = -(2.0 * math.pi - angle_i + angle_i_1) + else: + delta_orientations = angle_i - angle_i_1 + orientations += delta_orientations + + fitness_value: float = orientations - factor_orien_ds * robot_manager._dist + return fitness_value + + +def panoramic_rotation( + robot_manager, robot: RevolveBot, vertical_angle_limit: float = math.pi / 4 +): + """ + This fitness evolves robots that take a panoramic scan of their surroundings. + If the chosen forward vector ever points too much upwards or downwards (limit defined by `vertical_angle_limit`), + the fitness is reported only up to the point of "failure". + + In this fitness, I'm assuming any "grace time" is not present in the data and the first data points + in the robot_manager queues are the starting evaluation points. + :param robot_manager: Behavioural data of the robot + :param robot: Robot object + :param vertical_angle_limit: vertical limit that if passed will invalidate any subsequent step of the robot. + :return: fitness value + """ + total_angle = 0.0 + vertical_limit = math.sin(vertical_angle_limit) + + # decide which orientation to choose, [0] is correct because the "grace time" values are discarded by the deques + if len(robot_manager._orientation_vecs) == 0: + return total_angle + + # Chose orientation base on the + chosen_orientation = None + min_z = 1.0 + for orientation, vec in robot_manager._orientation_vecs[0].items(): + z = abs(vec.z) + if z < min_z: + chosen_orientation = orientation + min_z = z + logger.info(f"Chosen orientation for robot {robot.id} is {chosen_orientation}") + + vec_list = [vecs[chosen_orientation] for vecs in robot_manager._orientation_vecs] + + for i in range(1, len(robot_manager._orientation_vecs)): + # from: https://code-examples.net/en/q/d6a4f5 + # more info: https://en.wikipedia.org/wiki/Atan2 + # Just like the dot product is proportional to the cosine of the angle, + # the determinant is proportional to its sine. So you can compute the angle like this: + # + # dot = x1*x2 + y1*y2 # dot product between [x1, y1] and [x2, y2] + # det = x1*y2 - y1*x2 # determinant + # angle = atan2(det, dot) # atan2(y, x) or atan2(sin, cos) + # + # The function atan2(y,x) (from "2-argument arctangent") is defined as the angle in the Euclidean plane, + # given in radians, between the positive x axis and the ray to the point (x, y) ≠ (0, 0). + + # u = prev vector + # v = curr vector + u: Vector3 = vec_list[i - 1] + v: Vector3 = vec_list[i] + + # if vector is too vertical, fail the fitness + # (assuming these are unit vectors) + if abs(u.z) > vertical_limit: + return total_angle + + dot = u.x * v.x + u.y * v.y # dot product between [x1, y1] and [x2, y2] + det = u.x * v.y - u.y * v.x # determinant + delta = math.atan2(det, dot) # atan2(y, x) or atan2(sin, cos) + + total_angle += delta + + return total_angle + + +def follow_line(robot_manager: RobotManager, robot: RevolveBot) -> float: + """ + As per Emiel's master's research. + + Fitness is determined by the formula: + + F = e3 * (e1 / (delta + 1) - penalty_factor * e2) + + Where e1 is the distance travelled in the right direction, + e2 is the distance of the final position p1 from the ideal + trajectory starting at starting position p0 and following + the target direction. e3 is distance in right direction divided by + length of traveled path(curved) + infinitesimal constant to never divide + by zero. + delta is angle between optimal direction and traveled direction. + """ + penalty_factor = 0.01 + + epsilon: float = sys.float_info.epsilon + + # length of traveled path(over the complete curve) + path_length = measures.path_length(robot_manager) # L + + # robot position, Vector3(pos.x, pos.y, pos.z) + pos_0 = robot_manager._positions[0] # start + pos_1 = robot_manager._positions[-1] # end + + # robot displacement + displacement: Tuple[float, float] = (pos_1[0] - pos_0[0], pos_1[1] - pos_0[1]) + displacement_length = math.sqrt(displacement[0] ** 2 + displacement[1] ** 2) + if displacement_length > 0: + displacement_normalized = ( + displacement[0] / displacement_length, + displacement[1] / displacement_length, + ) + else: + displacement_normalized = (0, 0) + + # steal target from brain + # is already normalized + target = robot._brain.target + target_length = math.sqrt(target[0] ** 2 + target[1] ** 2) + target_normalized = (target[0] / target_length, target[1] / target_length) + + # angle between target and actual direction + delta = math.acos( + min( # bound to account for small float errors. acos crashes on 1.0000000001 + 1.0, + max( + -1, + target_normalized[0] * displacement_normalized[0] + + target_normalized[1] * displacement_normalized[1], + ), + ) + ) + + # projection of displacement on target line + dist_in_right_direction: float = ( + displacement[0] * target_normalized[0] + displacement[1] * target_normalized[1] + ) + + # distance from displacement to target line + dist_to_optimal_line: float = math.sqrt( + (dist_in_right_direction * target_normalized[0] - displacement[0]) ** 2 + + (dist_in_right_direction * target_normalized[1] - displacement[1]) ** 2 + ) + + logger.info( + f"target: {target}, displacement: {displacement}, dist_in_right_direction: {dist_in_right_direction}, dist_to_optimal_line: {dist_to_optimal_line}, delta: {delta}, path_length: {path_length}" + ) + + # filter out passive blocks + if dist_in_right_direction < 0.01: + fitness = 0 + logger.info(f"Did not pass fitness test, fitness = {fitness}") + else: + fitness = (dist_in_right_direction / (epsilon + path_length)) * ( + dist_in_right_direction / (delta + 1) + - penalty_factor * dist_to_optimal_line + ) + + logger.info(f"Fitness = {fitness}") + + return fitness diff --git a/pyrevolve/evolution/individual.py b/pyrevolve/evolution/individual.py index c2448b3423..259863175e 100644 --- a/pyrevolve/evolution/individual.py +++ b/pyrevolve/evolution/individual.py @@ -1,56 +1,88 @@ -# (G,P) +from __future__ import annotations +import os + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Optional, List, Union + from pyrevolve.revolve_bot import RevolveBot + from pyrevolve.genotype import Genotype + from pyrevolve.evolution.speciation.species import Species class Individual: - def __init__(self, genotype, phenotype=None): + def __init__(self, genotype: Genotype, phenotype: Optional[Union[RevolveBot, List[RevolveBot]]] = None): """ Creates an Individual object with the given genotype and optionally the phenotype. :param genotype: genotype of the individual :param phenotype (optional): phenotype of the individual """ - self.genotype = genotype - self.phenotype = phenotype - self.fitness = None - self.parents = None - self.failed_eval_attempt_count = 0 + self.genotype: Genotype = genotype + self.phenotype: Union[RevolveBot, List[RevolveBot]] = phenotype + self.fitness: Optional[float] = None + self.parents: Optional[List[Individual]] = None + self.objectives = [] - def develop(self): + def develop(self) -> None: """ Develops genotype into a intermediate phenotype - """ if self.phenotype is None: self.phenotype = self.genotype.develop() @property - def id(self): + def id(self) -> int: _id = None if self.phenotype is not None: - _id = self.phenotype.id + if isinstance(self.phenotype, list): + _id = self.phenotype[0].id + else: + _id = self.phenotype.id elif self.genotype.id is not None: _id = self.genotype.id return _id - def export_genotype(self, folder): - self.genotype.export_genotype(f'{folder}/genotypes/genotype_{self.phenotype.id}.txt') + def export_genotype(self, folder: str) -> None: + self.genotype.export_genotype(os.path.join(folder, f'genotype_{self.id}.txt')) - def export_phenotype(self, folder): - if self.phenotype is not None: - self.phenotype.save_file(f'{folder}/phenotypes/{self.phenotype.id}.yaml', conf_type='yaml') + def export_phenotype(self, folder: str) -> None: + if self.phenotype is None: + self.develop() + if isinstance(self.phenotype, list): + for i, alternative in enumerate(self.phenotype): + alternative.save_file(os.path.join(folder, f'phenotype_{alternative.id}_{i}.yaml'), conf_type='yaml') + else: + self.phenotype.save_file(os.path.join(folder, f'phenotype_{self.phenotype.id}.yaml'), conf_type='yaml') + + def export_phylogenetic_info(self, folder: str) -> None: + """ + Export phylogenetic information + (parents and other possibly other information to build a phylogenetic tree) + :param folder: folder where to save the information + """ + if self.parents is not None: + parents_ids: List[str] = [str(p.id) for p in self.parents] + parents_ids_str = ",".join(parents_ids) + else: + parents_ids_str = 'None' + + filename = os.path.join(folder, f'parents_{self.id}.yaml') + with open(filename, 'w') as file: + file.write(f'parents:{parents_ids_str}') - def export_fitness(self, folder): + def export_fitness_single_file(self, folder: str) -> None: """ It's saving the fitness into a file. The fitness can be a floating point number or None :param folder: folder where to save the fitness """ - with open(f'{folder}/fitness_{self.id}.txt', 'w') as f: + with open(os.path.join(folder, f'fitness_{self.id}.txt'), 'w') as f: f.write(str(self.fitness)) - def export(self, folder): + def export(self, folder: str) -> None: self.export_genotype(folder) + self.export_phylogenetic_info(folder) self.export_phenotype(folder) self.export_fitness(folder) - def __repr__(self): + def __repr__(self) -> str: return f'Individual_{self.id}({self.fitness})' diff --git a/pyrevolve/evolution/pop_management/generational.py b/pyrevolve/evolution/pop_management/generational.py deleted file mode 100644 index 56aba792e2..0000000000 --- a/pyrevolve/evolution/pop_management/generational.py +++ /dev/null @@ -1,3 +0,0 @@ -def generational_population_management(old_individuals, new_individuals): - assert (len(old_individuals) == len(new_individuals)) - return new_individuals diff --git a/pyrevolve/evolution/population.py b/pyrevolve/evolution/population.py deleted file mode 100644 index 45ad959032..0000000000 --- a/pyrevolve/evolution/population.py +++ /dev/null @@ -1,269 +0,0 @@ -# [(G,P), (G,P), (G,P), (G,P), (G,P)] - -from pyrevolve.evolution.individual import Individual -from pyrevolve.SDF.math import Vector3 -from pyrevolve.tol.manage import measures -from ..custom_logging.logger import logger -import time -import asyncio -import os - - -class PopulationConfig: - def __init__(self, - population_size: int, - genotype_constructor, - genotype_conf, - fitness_function, - mutation_operator, - mutation_conf, - crossover_operator, - crossover_conf, - selection, - parent_selection, - population_management, - population_management_selector, - evaluation_time, - experiment_name, - experiment_management, - offspring_size=None, - next_robot_id=1): - """ - Creates a PopulationConfig object that sets the particular configuration for the population - - :param population_size: size of the population - :param genotype_constructor: class of the genotype used - :param genotype_conf: configuration for genotype constructor - :param fitness_function: function that takes in a `RobotManager` as a parameter and produces a fitness for the robot - :param mutation_operator: operator to be used in mutation - :param mutation_conf: configuration for mutation operator - :param crossover_operator: operator to be used in crossover - :param selection: selection type - :param parent_selection: selection type during parent selection - :param population_management: type of population management ie. steady state or generational - :param evaluation_time: duration of an experiment - :param experiment_name: name for the folder of the current experiment - :param experiment_management: object with methods for managing the current experiment - :param offspring_size (optional): size of offspring (for steady state) - """ - self.population_size = population_size - self.genotype_constructor = genotype_constructor - self.genotype_conf = genotype_conf - self.fitness_function = fitness_function - self.mutation_operator = mutation_operator - self.mutation_conf = mutation_conf - self.crossover_operator = crossover_operator - self.crossover_conf = crossover_conf - self.parent_selection = parent_selection - self.selection = selection - self.population_management = population_management - self.population_management_selector = population_management_selector - self.evaluation_time = evaluation_time - self.experiment_name = experiment_name - self.experiment_management = experiment_management - self.offspring_size = offspring_size - self.next_robot_id = next_robot_id - - -class Population: - def __init__(self, conf: PopulationConfig, simulator_queue, analyzer_queue=None, next_robot_id=1): - """ - Creates a Population object that initialises the - individuals in the population with an empty list - and stores the configuration of the system to the - conf variable. - - :param conf: configuration of the system - :param simulator_queue: connection to the simulator queue - :param analyzer_queue: connection to the analyzer simulator queue - :param next_robot_id: (sequential) id of the next individual to be created - """ - self.conf = conf - self.individuals = [] - self.analyzer_queue = analyzer_queue - self.simulator_queue = simulator_queue - self.next_robot_id = next_robot_id - - def _new_individual(self, genotype): - individual = Individual(genotype) - individual.develop() - self.conf.experiment_management.export_genotype(individual) - self.conf.experiment_management.export_phenotype(individual) - self.conf.experiment_management.export_phenotype_images(os.path.join('data_fullevolution', 'phenotype_images'), individual) - individual.phenotype.measure_phenotype() - individual.phenotype.export_phenotype_measurements(self.conf.experiment_management.data_folder) - - return individual - - async def load_individual(self, id): - data_path = self.conf.experiment_management.data_folder - genotype = self.conf.genotype_constructor(self.conf.genotype_conf, id) - genotype.load_genotype(os.path.join(data_path, 'genotypes', f'genotype_{id}.txt')) - - individual = Individual(genotype) - individual.develop() - individual.phenotype.measure_phenotype() - - with open(os.path.join(data_path, 'fitness', f'fitness_{id}.txt')) as f: - data = f.readlines()[0] - individual.fitness = None if data == 'None' else float(data) - - with open(os.path.join(data_path, 'descriptors', f'behavior_desc_{id}.txt')) as f: - lines = f.readlines() - if lines[0] == 'None': - individual.phenotype._behavioural_measurements = None - else: - individual.phenotype._behavioural_measurements = measures.BehaviouralMeasurements() - for line in lines: - if line.split(' ')[0] == 'velocity': - individual.phenotype._behavioural_measurements.velocity = float(line.split(' ')[1]) - #if line.split(' ')[0] == 'displacement': - # individual.phenotype._behavioural_measurements.displacement = float(line.split(' ')[1]) - if line.split(' ')[0] == 'displacement_velocity': - individual.phenotype._behavioural_measurements.displacement_velocity = float(line.split(' ')[1]) - if line.split(' ')[0] == 'displacement_velocity_hill': - individual.phenotype._behavioural_measurements.displacement_velocity_hill = float(line.split(' ')[1]) - if line.split(' ')[0] == 'head_balance': - individual.phenotype._behavioural_measurements.head_balance = float(line.split(' ')[1]) - if line.split(' ')[0] == 'contacts': - individual.phenotype._behavioural_measurements.contacts = float(line.split(' ')[1]) - - return individual - - async def load_snapshot(self, gen_num): - """ - Recovers all genotypes and fitnesses of robots in the lastest selected population - :param gen_num: number of the generation snapshot to recover - """ - data_path = self.conf.experiment_management.experiment_folder - for r, d, f in os.walk(data_path +'/selectedpop_'+str(gen_num)): - for file in f: - if 'body' in file: - id = file.split('.')[0].split('_')[-2]+'_'+file.split('.')[0].split('_')[-1] - self.individuals.append(await self.load_individual(id)) - - async def load_offspring(self, last_snapshot, population_size, offspring_size, next_robot_id): - """ - Recovers the part of an unfinished offspring - :param - :return: - """ - individuals = [] - # number of robots expected until the latest snapshot - if last_snapshot == -1: - n_robots = 0 - else: - n_robots = population_size + last_snapshot * offspring_size - - for robot_id in range(n_robots+1, next_robot_id): - individuals.append(await self.load_individual('robot_'+str(robot_id))) - - self.next_robot_id = next_robot_id - return individuals - - async def init_pop(self, recovered_individuals=[]): - """ - Populates the population (individuals list) with Individual objects that contains their respective genotype. - """ - for i in range(self.conf.population_size-len(recovered_individuals)): - individual = self._new_individual(self.conf.genotype_constructor(self.conf.genotype_conf, self.next_robot_id)) - self.individuals.append(individual) - self.next_robot_id += 1 - - await self.evaluate(self.individuals, 0) - self.individuals = recovered_individuals + self.individuals - - async def next_gen(self, gen_num, recovered_individuals=[]): - """ - Creates next generation of the population through selection, mutation, crossover - - :param gen_num: generation number - :param individuals: recovered offspring - :return: new population - """ - - new_individuals = [] - - for _i in range(self.conf.offspring_size-len(recovered_individuals)): - # Selection operator (based on fitness) - # Crossover - if self.conf.crossover_operator is not None: - parents = self.conf.parent_selection(self.individuals) - child_genotype = self.conf.crossover_operator(parents, self.conf.genotype_conf, self.conf.crossover_conf) - child = Individual(child_genotype) - else: - child = self.conf.selection(self.individuals) - - child.genotype.id = self.next_robot_id - self.next_robot_id += 1 - - # Mutation operator - child_genotype = self.conf.mutation_operator(child.genotype, self.conf.mutation_conf) - # Insert individual in new population - individual = self._new_individual(child_genotype) - - new_individuals.append(individual) - - # evaluate new individuals - await self.evaluate(new_individuals, gen_num) - - new_individuals = recovered_individuals + new_individuals - - # create next population - if self.conf.population_management_selector is not None: - new_individuals = self.conf.population_management(self.individuals, new_individuals, - self.conf.population_management_selector) - else: - new_individuals = self.conf.population_management(self.individuals, new_individuals) - new_population = Population(self.conf, self.simulator_queue, self.analyzer_queue, self.next_robot_id) - new_population.individuals = new_individuals - logger.info(f'Population selected in gen {gen_num} with {len(new_population.individuals)} individuals...') - - return new_population - - async def evaluate(self, new_individuals, gen_num, type_simulation = 'evolve'): - """ - Evaluates each individual in the new gen population - - :param new_individuals: newly created population after an evolution iteration - :param gen_num: generation number - """ - # Parse command line / file input arguments - # await self.simulator_connection.pause(True) - robot_futures = [] - for individual in new_individuals: - logger.info(f'Evaluating individual (gen {gen_num}) {individual.genotype.id} ...') - robot_futures.append(asyncio.ensure_future(self.evaluate_single_robot(individual))) - - await asyncio.sleep(1) - - for i, future in enumerate(robot_futures): - individual = new_individuals[i] - logger.info(f'Evaluation of Individual {individual.phenotype.id}') - individual.fitness, individual.phenotype._behavioural_measurements = await future - - if individual.phenotype._behavioural_measurements is None: - assert (individual.fitness is None) - - if type_simulation == 'evolve': - self.conf.experiment_management.export_behavior_measures(individual.phenotype.id, individual.phenotype._behavioural_measurements) - - logger.info(f'Individual {individual.phenotype.id} has a fitness of {individual.fitness}') - if type_simulation == 'evolve': - self.conf.experiment_management.export_fitness(individual) - - async def evaluate_single_robot(self, individual): - """ - :param individual: individual - :return: Returns future of the evaluation, future returns (fitness, [behavioural] measurements) - """ - if individual.phenotype is None: - individual.develop() - - if self.analyzer_queue is not None: - collisions, _bounding_box = await self.analyzer_queue.test_robot(individual, self.conf) - if collisions > 0: - logger.info(f"discarding robot {individual} because there are {collisions} self collisions") - return None, None - - return await self.simulator_queue.test_robot(individual, self.conf) diff --git a/pyrevolve/evolution/pop_management/__init__.py b/pyrevolve/evolution/population/__init__.py similarity index 100% rename from pyrevolve/evolution/pop_management/__init__.py rename to pyrevolve/evolution/population/__init__.py diff --git a/pyrevolve/evolution/population/population.py b/pyrevolve/evolution/population/population.py new file mode 100644 index 0000000000..008b88fbab --- /dev/null +++ b/pyrevolve/evolution/population/population.py @@ -0,0 +1,393 @@ +from __future__ import annotations + +import asyncio +import math +import os +import re +from typing import TYPE_CHECKING + +from pyrevolve.custom_logging.logger import logger +from pyrevolve.evolution import fitness +from pyrevolve.evolution.individual import Individual +from pyrevolve.evolution.population.population_config import PopulationConfig +from pyrevolve.revolve_bot.brain import BrainCPGTarget +from pyrevolve.revolve_bot.revolve_bot import RevolveBot +from pyrevolve.tol.manage.measures import BehaviouralMeasurements + +if TYPE_CHECKING: + from typing import Callable, List, Optional + + from pyrevolve.tol.manage.robotmanager import RobotManager + from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue, SimulatorQueue + + +MULTI_DEV_BODY_PNG_REGEX = re.compile("body_(\\d+)_(\\d+)\\.png") +BODY_PNG_REGEX = re.compile("body_(\\d+)\\.png") + + +class Population: + """ + Population class + + It handles the list of individuals: evaluations, mutations and crossovers. + It is the central component for robot evolution in this framework. + """ + + def __init__( + self, + config: PopulationConfig, + simulator_queue: SimulatorQueue, + analyzer_queue: Optional[AnalyzerQueue] = None, + next_robot_id: int = 1, + ): + """ + Creates a Population object that initialises the + individuals in the population with an empty list + and stores the configuration of the system to the + conf variable. + + :param config: configuration of the system + :param simulator_queue: connection to the simulator queue + :param analyzer_queue: connection to the analyzer simulator queue + :param next_robot_id: (sequential) id of the next individual to be created + """ + self.config = config + self.individuals = [] + self.analyzer_queue = analyzer_queue + self.simulator_queue = simulator_queue + self.next_robot_id = next_robot_id + + def _new_individual(self, genotype, parents: Optional[List[Individual]] = None): + individual = Individual(genotype) + individual.develop() + if isinstance(individual.phenotype, list): + for alternative in individual.phenotype: + alternative.update_substrate() + alternative.measure_phenotype() + alternative.export_phenotype_measurements( + self.config.experiment_management.data_folder + ) + else: + individual.phenotype.update_substrate() + individual.phenotype.measure_phenotype() + individual.phenotype.export_phenotype_measurements( + self.config.experiment_management.data_folder + ) + if parents is not None: + individual.parents = parents + + self.config.experiment_management.export_genotype(individual) + self.config.experiment_management.export_phenotype(individual) + self.config.experiment_management.export_phenotype_images(individual) + + return individual + + def load_snapshot(self, gen_num: int, multi_development=True) -> None: + """ + Recovers all genotypes and fitnesses of robots in the lastest selected population + :param gen_num: number of the generation snapshot to recover + :param multi_development: if multiple developments are created by the same individual + """ + data_path = self.config.experiment_management.generation_folder(gen_num) + for r, d, f in os.walk(data_path): + for filename in f: + if "body" in filename: + if multi_development: + _id = MULTI_DEV_BODY_PNG_REGEX.search(filename) + if int(_id.group(2)) != 0: + continue + else: + _id = BODY_PNG_REGEX.search(filename) + assert _id is not None + _id = int(_id.group(1)) + self.individuals.append( + self.config.experiment_management.load_individual( + _id, self.config + ) + ) + + def load_offspring( + self, + last_snapshot: int, + population_size: int, + offspring_size: int, + next_robot_id: int, + ) -> List[Individual]: + """ + Recovers the part of an unfinished offspring + :param last_snapshot: number of robots expected until the latest snapshot + :param population_size: Population size + :param offspring_size: Offspring size (steady state) + :param next_robot_id: TODO + :return: the list of recovered individuals + """ + individuals = [] + # number of robots expected until the latest snapshot + if last_snapshot == -1: + n_robots = 0 + else: + n_robots = population_size + last_snapshot * offspring_size + + for robot_id in range(n_robots + 1, next_robot_id): + # TODO refactor filename + individuals.append( + self.config.experiment_management.load_individual(robot_id, self.config) + ) + + self.next_robot_id = next_robot_id + return individuals + + async def initialize( + self, recovered_individuals: Optional[List[Individual]] = None + ) -> None: + """ + Populates the population (individuals list) with Individual objects that contains their respective genotype. + """ + recovered_individuals = ( + [] if recovered_individuals is None else recovered_individuals + ) + + for i in range(self.config.population_size - len(recovered_individuals)): + new_genotype = self.config.genotype_constructor( + self.config.genotype_conf, self.next_robot_id + ) + individual = self._new_individual(new_genotype) + self.individuals.append(individual) + self.next_robot_id += 1 + + await self.evaluate(self.individuals, 0) + self.individuals = recovered_individuals + self.individuals + + async def next_generation( + self, gen_num: int, recovered_individuals: Optional[List[Individual]] = None + ) -> Population: + """ + Creates next generation of the population through selection, mutation, crossover + + :param gen_num: generation number + :param recovered_individuals: recovered offspring + :return: new population + """ + recovered_individuals = ( + [] if recovered_individuals is None else recovered_individuals + ) + + new_individuals = [] + + for _i in range(self.config.offspring_size - len(recovered_individuals)): + # Selection operator (based on fitness) + # Crossover + parents: Optional[List[Individual]] = None + if self.config.crossover_operator is not None: + parents = self.config.parent_selection(self.individuals) + child_genotype = self.config.crossover_operator( + parents, self.config.genotype_conf, self.config.crossover_conf + ) + # temporary individual that will be used for mutation + child = Individual(child_genotype) + child.parents = parents + else: + # fake child + child = self.config.selection(self.individuals) + parents = [child] + + child.genotype.id = self.next_robot_id + self.next_robot_id += 1 + + # Mutation operator + child_genotype = self.config.mutation_operator( + child.genotype, self.config.mutation_conf + ) + # Insert individual in new population + individual = self._new_individual(child_genotype, parents) + + new_individuals.append(individual) + + # evaluate new individuals + await self.evaluate(new_individuals, gen_num) + + new_individuals = recovered_individuals + new_individuals + + # create next population + if self.config.population_management_selector is not None: + new_individuals = self.config.population_management( + self.individuals, + new_individuals, + self.config.population_management_selector, + ) + else: + new_individuals = self.config.population_management( + self.individuals, new_individuals + ) + new_population = Population( + self.config, self.simulator_queue, self.analyzer_queue, self.next_robot_id + ) + new_population.individuals = new_individuals + logger.info( + f"Population selected in gen {gen_num} with {len(new_population.individuals)} individuals..." + ) + + return new_population + + async def _evaluate_objectives( + self, new_individuals: List[Individual], gen_num: int + ) -> None: + """ + Evaluates each individual in the new gen population for each objective + :param new_individuals: newly created population after an evolution iteration + :param gen_num: generation number + """ + assert isinstance(self.config.objective_functions, list) is True + assert self.config.fitness_function is None + + robot_futures = [] + for individual in new_individuals: + individual.develop() + individual.objectives = [ + -math.inf for _ in range(len(self.config.objective_functions)) + ] + + assert len(individual.phenotype) == len(self.config.objective_functions) + for objective, robot in enumerate(individual.phenotype): + logger.info( + f"Evaluating individual (gen {gen_num} - objective {objective}) {robot.id}" + ) + objective_fun = self.config.objective_functions[objective] + future = asyncio.ensure_future( + self.evaluate_single_robot( + individual=individual, + fitness_fun=objective_fun, + phenotype=robot, + ) + ) + robot_futures.append((individual, robot, objective, future)) + + await asyncio.sleep(1) + + for individual, robot, objective, future in robot_futures: + assert objective < len(self.config.objective_functions) + + logger.info(f"Evaluation of Individual (objective {objective}) {robot.id}") + fitness, robot._behavioural_measurements = await future + individual.objectives[objective] = fitness + logger.info( + f"Individual {individual.id} in objective {objective} has a fitness of {fitness}" + ) + + if robot._behavioural_measurements is None: + assert fitness is None + + self.config.experiment_management.export_behavior_measures( + robot.id, robot._behavioural_measurements, objective + ) + + for individual, robot, objective, _ in robot_futures: + self.config.experiment_management.export_objectives(individual) + + async def evaluate( + self, new_individuals: List[Individual], gen_num: int, type_simulation="evolve" + ) -> None: + """ + Evaluates each individual in the new gen population + + :param new_individuals: newly created population after an evolution iteration + :param gen_num: generation number + TODO remove `type_simulation`, I have no idea what that is for, but I have a strong feeling it should not be here. + """ + if callable(self.config.fitness_function): + await self._evaluate_single_fitness( + new_individuals, gen_num, type_simulation + ) + elif isinstance(self.config.objective_functions, list): + await self._evaluate_objectives(new_individuals, gen_num) + else: + raise RuntimeError("Fitness function not configured correctly") + + async def _evaluate_single_fitness( + self, new_individuals: List[Individual], gen_num: int, type_simulation="evolve" + ) -> None: + # Parse command line / file input arguments + # await self.simulator_connection.pause(True) + robot_futures = [] + for individual in new_individuals: + logger.info(f"Evaluating individual (gen {gen_num}) {individual.id} ...") + assert callable(self.config.fitness_function) + robot_futures.append( + asyncio.ensure_future( + self.evaluate_single_robot( + individual=individual, fitness_fun=self.config.fitness_function + ) + ) + ) + + await asyncio.sleep(1) + + for i, future in enumerate(robot_futures): + individual = new_individuals[i] + logger.info(f"Evaluation of Individual {individual.phenotype.id}") + ( + individual.fitness, + individual.phenotype._behavioural_measurements, + ) = await future + + if individual.phenotype._behavioural_measurements is None: + assert individual.fitness is None + + if type_simulation == "evolve": + self.config.experiment_management.export_behavior_measures( + individual.phenotype.id, + individual.phenotype._behavioural_measurements, + None, + ) + + logger.info( + f"Individual {individual.phenotype.id} has a fitness of {individual.fitness}" + ) + if type_simulation == "evolve": + self.config.experiment_management.export_fitness(individual) + + async def evaluate_single_robot( + self, + individual: Individual, + fitness_fun: Callable[[RobotManager, RevolveBot], float], + phenotype: Optional[RevolveBot] = None, + ) -> (float, BehaviouralMeasurements): + """ + :param individual: individual + :param fitness_fun: fitness function + :param phenotype: robot phenotype to test [optional] + :return: Returns future of the evaluation, future returns (fitness, [behavioural] measurements) + """ + if phenotype is None: + if individual.phenotype is None: + individual.develop() + phenotype = individual.phenotype + + assert isinstance(phenotype, RevolveBot) + + # set target + assert isinstance(phenotype._brain, BrainCPGTarget) + phenotype._brain.target = (0.0, 10.0, 0.0) + + if self.analyzer_queue is not None: + collisions, bounding_box = await self.analyzer_queue.test_robot( + individual, phenotype, self.config, fitness_fun + ) + if collisions > 0: + logger.info( + f"discarding robot {individual} because there are {collisions} self collisions" + ) + return None, None + else: + phenotype.simulation_boundaries = bounding_box + + if self.simulator_queue is not None: + return await self.simulator_queue.test_robot( + individual, phenotype, self.config, fitness_fun + ) + else: + print("MOCKING SIMULATION") + return await self._mock_simulation() + + async def _mock_simulation(self): + return fitness.random(None, None), BehaviouralMeasurements() diff --git a/pyrevolve/evolution/population/population_config.py b/pyrevolve/evolution/population/population_config.py new file mode 100644 index 0000000000..d9612e09ef --- /dev/null +++ b/pyrevolve/evolution/population/population_config.py @@ -0,0 +1,85 @@ +from __future__ import annotations + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Callable, Optional, List + from pyrevolve.evolution.individual import Individual + from pyrevolve.genotype import Genotype + from pyrevolve.revolve_bot import RevolveBot + from pyrevolve.tol.manage.robotmanager import RobotManager + + +class PopulationConfig: + def __init__(self, + population_size: int, + genotype_constructor: Callable[[object, int], Genotype], + genotype_conf: object, + fitness_function: Optional[Callable[[RobotManager, RevolveBot], float]], + mutation_operator: Callable[[Genotype, object], Genotype], + mutation_conf: object, + crossover_operator: Callable[[List[Individual], object, object], Genotype], + crossover_conf: object, + selection: Callable[[List[Individual]], Individual], + parent_selection: Callable[[List[Individual]], List[Individual]], + population_management: Callable[ + [List[Individual], List[Individual], Callable[[List[Individual]], Individual]], + List[Individual] + ], + population_management_selector: Optional[Callable[[List[Individual]], Individual]], + evaluation_time: float, + experiment_name: str, + experiment_management, + offspring_size: Optional[int] = None, + grace_time: float = 0.0, + objective_functions: Optional[List[Callable[[RobotManager, RevolveBot], float]]] = None): + """ + Creates a PopulationConfig object that sets the particular configuration for the population + + :param population_size: size of the population + :param genotype_constructor: class of the genotype used. + First parameter is the config of the genome. + Second is the id of the genome + :param genotype_conf: configuration for genotype constructor + :param fitness_function: function that takes in a `RobotManager` as a parameter and produces a fitness for + the robot. Set to `None` if you want to use `objective_functions` instead. + :param objective_functions: list of functions that takes in a `RobotManager` as a parameter and produces a + fitness for the robot. This parameter is to be instead of the `fitness_function` when using an algorithm + that uses multiple objective optimization, like NSGAII. + :param mutation_operator: operator to be used in mutation + :param mutation_conf: configuration for mutation operator + :param crossover_operator: operator to be used in crossover. + First parameter is the list of parents (usually 2). + Second parameter is the Genotype Conf + Third parameter is Crossover Conf + :param selection: selection type + :param parent_selection: selection type during parent selection + :param population_management: type of population management ie. steady state or generational + :param evaluation_time: duration of an evaluation (experiment_time = grace_time + evaluation_time) + :param experiment_name: name for the folder of the current experiment + :param experiment_management: object with methods for managing the current experiment + :param offspring_size (optional): size of offspring (for steady state) + :param grace_time: time to wait before starting the evaluation (experiment_time = grace_time + evaluation_time), default to 0.0 + """ + # Test if at least one of them is set + assert fitness_function is not None or objective_functions is not None + # Test if not both of them are set at the same time + assert fitness_function is None or objective_functions is None + + self.population_size = population_size + self.genotype_constructor = genotype_constructor + self.genotype_conf = genotype_conf + self.fitness_function = fitness_function + self.mutation_operator = mutation_operator + self.mutation_conf = mutation_conf + self.crossover_operator = crossover_operator + self.crossover_conf = crossover_conf + self.parent_selection = parent_selection + self.selection = selection + self.population_management = population_management + self.population_management_selector = population_management_selector + self.evaluation_time = evaluation_time + self.grace_time = grace_time + self.experiment_name = experiment_name + self.experiment_management = experiment_management + self.offspring_size = offspring_size + self.objective_functions = objective_functions diff --git a/pyrevolve/evolution/pop_management/steady_state.py b/pyrevolve/evolution/population/population_management.py similarity index 53% rename from pyrevolve/evolution/pop_management/steady_state.py rename to pyrevolve/evolution/population/population_management.py index 1f675664c7..efa4241706 100644 --- a/pyrevolve/evolution/pop_management/steady_state.py +++ b/pyrevolve/evolution/population/population_management.py @@ -2,7 +2,14 @@ def steady_state_population_management(old_individuals, new_individuals, selector): + # Steady state population + # total size of population is the sum of all the species individuals. pop_size = len(old_individuals) selection_pool = old_individuals + new_individuals return multiple_selection(selection_pool, pop_size, selector) + + +def generational_population_management(old_individuals, new_individuals): + assert len(old_individuals) == len(new_individuals) + return new_individuals diff --git a/pyrevolve/evolution/selection.py b/pyrevolve/evolution/selection.py index 9b3e06cb8c..ed264e6c83 100644 --- a/pyrevolve/evolution/selection.py +++ b/pyrevolve/evolution/selection.py @@ -1,5 +1,12 @@ +from __future__ import annotations from random import randint +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import List, Callable + from pyrevolve.evolution.individual import Individual + + _neg_inf = -float('Inf') @@ -9,9 +16,10 @@ def _compare_maj_fitness(indiv_1, indiv_2): return fit_1 > fit_2 -def tournament_selection(population, k=2): +def tournament_selection(population: List[Individual], k=2) -> Individual: """ Perform tournament selection and return best individual + :param population: list of individuals where to select from :param k: amount of individuals to participate in tournament """ best_individual = None @@ -22,14 +30,22 @@ def tournament_selection(population, k=2): return best_individual -def multiple_selection(population, selection_size, selection_function): +def multiple_selection(population: List[Individual], + selection_size: int, + selection_function: Callable[[List[Individual]], Individual] + ) -> List[Individual]: """ - Perform selection on population of distinct group, can be used in the form parent selection or survival selection - :param population: parent selection in population - :param selection_size: amount of indivuals to select + Perform selection on population of distinct group, it can be used in the + form parent selection or survival selection. + It never selects the same individual more than once + :param population: list of individuals where to select from + :param selection_size: amount of individuals to select :param selection_function: """ - assert (len(population) >= selection_size) + if len(population) < selection_size: + print("selection problem: population " + str(len(population)) + " - selection size " + str(selection_size)) + assert (len(population) >= selection_size) + selected_individuals = [] for _ in range(selection_size): new_individual = False @@ -39,3 +55,22 @@ def multiple_selection(population, selection_size, selection_function): selected_individuals.append(selected_individual) new_individual = True return selected_individuals + + +def multiple_selection_with_duplicates(population: List[Individual], + selection_size: int, + selection_function: Callable[[List[Individual]], Individual] + ) -> List[Individual]: + """ + Perform selection on population of distinct group, it can be used in the + form parent selection or survival selection. + It can select the same individual more than once + :param population: list of individuals where to select from + :param selection_size: amount of individuals to select + :param selection_function: + """ + selected_individuals = [] + for _ in range(selection_size): + selected_individual = selection_function(population) + selected_individuals.append(selected_individual) + return selected_individuals diff --git a/pyrevolve/evolution/speciation/__init__.py b/pyrevolve/evolution/speciation/__init__.py new file mode 100644 index 0000000000..41b1fa72fa --- /dev/null +++ b/pyrevolve/evolution/speciation/__init__.py @@ -0,0 +1,5 @@ +""" +A population class that supports speciation, inspired from HyperNEAT + +If you use this class, make sure you don't use negative fitnesses, as they are not supported +""" diff --git a/pyrevolve/evolution/speciation/age.py b/pyrevolve/evolution/speciation/age.py new file mode 100644 index 0000000000..f599f1eebc --- /dev/null +++ b/pyrevolve/evolution/speciation/age.py @@ -0,0 +1,65 @@ +from __future__ import annotations + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Dict + + +class Age: + + def __init__(self): + # Age of the species (in generations) + self._generations: int = 0 + # Age of the species (in evaluations) + self._evaluations: int = 0 + + # generational counter. + self._no_improvements: int = 0 + + def __eq__(self, other): + if not isinstance(other, Age): + # don't attempt to compare against unrelated types + return NotImplemented + + return self._generations == other._generations \ + and self._evaluations == other._evaluations \ + and self._no_improvements == other._no_improvements + + # GETTERS + def generations(self): + return self._generations + + def evaluations(self): + return self._evaluations + + def no_improvements(self): + return self._no_improvements + + # Age + def increase_evaluations(self) -> None: + self._evaluations += 1 + + def increase_generations(self) -> None: + self._generations += 1 + + def increase_no_improvement(self) -> None: + self._no_improvements += 1 + + def reset_generations(self) -> None: + self._generations = 0 + self._no_improvements = 0 + + def serialize(self) -> Dict[str, int]: + return { + 'generations': self._generations, + 'evaluations': self._evaluations, + 'no_improvements': self._no_improvements, + } + + @staticmethod + def Deserialize(obj: Dict) -> Age: + age = Age() + age._generations = obj['generations'] + age._evaluations = obj['evaluations'] + age._no_improvements = obj['no_improvements'] + return age diff --git a/pyrevolve/evolution/speciation/genus.py b/pyrevolve/evolution/speciation/genus.py new file mode 100644 index 0000000000..fc62d6b0d6 --- /dev/null +++ b/pyrevolve/evolution/speciation/genus.py @@ -0,0 +1,274 @@ +from __future__ import annotations + +from pyrevolve.custom_logging.logger import logger +from .species import Species +from .species_collection import SpeciesCollection, count_individuals + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from .population_speciated_config import PopulationSpeciatedConfig + from pyrevolve.evolution.individual import Individual + from typing import List, Callable, Iterator, Coroutine, Optional + + +class Genus: + """ + Collection of species + """ + + def __init__(self, + config: PopulationSpeciatedConfig, + species_collection: Optional[SpeciesCollection] = None, + next_species_id: int = 1): + """ + Creates the genus object. + :param config: Population speciated config. + :param species_collection: Managers the list of species. + """ + #TODO refactor config (is it part of species, species collection, or genus? + self.config: PopulationSpeciatedConfig = config + + self.species_collection: SpeciesCollection = SpeciesCollection() \ + if species_collection is None else species_collection + + self._next_species_id: int = next_species_id + + def __len__(self): + return len(self.species_collection) + + def iter_individuals(self) -> Iterator[Individual]: + """ + Returns all individuals from the species. + :return: an iterator of `individual` for all individuals of the species + """ + for species in self.species_collection: + for individual, _ in species.iter_individuals(): + yield individual + + # TODO refactor + def speciate(self, individuals: List[Individual]) -> None: + """ + Creates the species. It takes a list of individuals and splits them into multiple species, grouping the + compatible individuals together. + + :param individuals: + :return: + """ + assert len(individuals) > 0 + + # clear out the species list + self.species_collection.clear() + + # NOTE: we are comparing the new generation's genomes to the representatives from the previous generation! + # Any new species that is created is assigned a representative from the new generation. + for individual in individuals: + # Iterate through each species and check if compatible. If compatible, then add the species. + # If not compatible, create a new species. + for species in self.species_collection: + if species.is_compatible(individual, self.config): + species.append(individual) + break + else: + self.species_collection.add_species(Species([individual], self._next_species_id)) + self._next_species_id += 1 + + self.species_collection.cleanup() + + async def next_generation(self, + recovered_individuals: List[Individual], + generate_individual_function: Callable[[List[Individual]], Individual], + evaluate_individuals_function: Callable[[List[Individual]], Coroutine]) -> Genus: + """ + Creates the genus for the next generation + + :param recovered_individuals: TODO implement recovery + :param generate_individual_function: The function that generates a new individual. + :param evaluate_individuals_function: Function to evaluate new individuals + :return: The Genus of the next generation + """ + assert len(recovered_individuals) == 0 + + # update species stagnation and stuff + self.species_collection.update() + + # update adjusted fitnesses + self.species_collection.adjust_fitness(self.config) + + # calculate offspring amount + offspring_amounts = self._count_offsprings(self.config.offspring_size) + + # clone species: + new_species_collection = SpeciesCollection() + need_evaluation: List[Individual] = [] + orphans: List[Individual] = [] + old_species_individuals: List[List[Individual]] = [[] for _ in range(len(self.species_collection))] + + ################################################################## + # GENERATE NEW INDIVIDUALS + for species_index, species in enumerate(self.species_collection): + + # Get the individuals from the individual with adjusted fitness tuple list. + old_species_individuals[species_index] = [individual for individual, _ in species.iter_individuals()] + + new_individuals = [] + + for _ in range(offspring_amounts[species_index]): + new_individual = generate_individual_function(old_species_individuals[species_index]) + need_evaluation.append(new_individual) + + # if the new individual is compatible with the species, otherwise create new. + if species.is_compatible(new_individual, self.config): + new_individuals.append(new_individual) + else: + orphans.append(new_individual) + + new_species_collection.add_species(species.create_species(new_individuals)) + + ################################################################## + # MANAGE ORPHANS, POSSIBLY CREATE NEW SPECIES + # recheck if other species can adopt the orphan individuals. + list_of_new_species = [] + for orphan in orphans: + for species in new_species_collection: + if species.is_compatible(orphan, self.config): + species.append(orphan) + break + else: + new_species = Species([orphan], self._next_species_id) + new_species_collection.add_species(new_species) + list_of_new_species.append(new_species) + # add an entry for new species which does not have a previous iteration. + self._next_species_id += 1 + + # Do a recount on the number of offspring per species. + new_species_size = sum(map(lambda species: len(species), list_of_new_species)) + offspring_amounts = self._count_offsprings(self.config.population_size - new_species_size) + assert sum(offspring_amounts) == self.config.population_size - new_species_size + + ################################################################## + # EVALUATE NEW INDIVIDUALS + # TODO avoid recovered individuals here [partial recovery] + await evaluate_individuals_function(need_evaluation) + + ################################################################## + # POPULATION MANAGEMENT + # Update the species population, based on the population management algorithm. + for species_index, new_species in enumerate(new_species_collection): + new_species_individuals = [individual for individual, _ in new_species.iter_individuals()] + + if species_index >= len(old_species_individuals): + # Finished. The new species keep the entire population. + break + + # create next population ## Same as population.next_gen + new_individuals = self.config.population_management(new_species_individuals, + old_species_individuals[species_index], + offspring_amounts[species_index], + self.config.population_management_selector) + new_species.set_individuals(new_individuals) + + ################################################################## + # ASSERT SECTION + # check species IDs [complicated assert] + species_id = set() + for species in new_species_collection: + if species.id in species_id: + raise RuntimeError(f"Species ({species.id}) present twice") + species_id.add(species.id) + + new_species_collection.cleanup() + + # Assert species list size and number of individuals + n_individuals = count_individuals(new_species_collection) + if n_individuals != self.config.population_size: + raise RuntimeError(f'count_individuals(new_species_collection) = {n_individuals} != ' + f'{self.config.population_size} = population_size') + + ################################################################## + # Create the next GENUS + new_genus = Genus(self.config, new_species_collection, self._next_species_id) + + return new_genus + + # TODO testing + # TODO separate these functions to a different class, and pass on the species collection. + def _count_offsprings(self, number_of_individuals: int) -> List[int]: + """ + Calculates the number of offspring allocated for each individual. + The total of allocated individuals will be `number_of_individuals` + + :param number_of_individuals: Total number of individuals to generate. + :return: a list of integers representing the number of allocated individuals for each species. + The index of this list correspond to the same index in self._species_list. + """ + assert number_of_individuals > 0 + + average_adjusted_fitness: float = self._calculate_average_fitness(number_of_individuals) + + species_offspring_amount: List[int] = self._calculate_population_size(average_adjusted_fitness) + + missing_offspring = number_of_individuals - sum(species_offspring_amount) + + species_offspring_amount = self._correct_population_size(species_offspring_amount, missing_offspring) + sum_species_offspring_amount = sum(species_offspring_amount) + + if sum_species_offspring_amount != number_of_individuals: + raise RuntimeError(f'generated sum_species_offspring_amount ({sum_species_offspring_amount}) ' + f'does not equal number_of_individuals ({number_of_individuals}).\n' + f'species_offspring_amount: {species_offspring_amount}') + + return species_offspring_amount + + def _calculate_average_fitness(self, number_of_individuals: int) -> float: + # Calculate the total adjusted fitness + total_adjusted_fitness: float = 0.0 + for species in self.species_collection: + for _, adjusted_fitness in species.iter_individuals(): + total_adjusted_fitness += adjusted_fitness + + # Calculate the average adjusted fitness + assert total_adjusted_fitness > 0.0 + average_adjusted_fitness = total_adjusted_fitness / float(number_of_individuals) + + return average_adjusted_fitness + + def _calculate_population_size(self, average_adjusted_fitness: float) -> List[int]: + species_offspring_amount: List[int] = [] + + for species in self.species_collection: + offspring_amount: float = 0.0 + for individual, adjusted_fitness in species.iter_individuals(): + offspring_amount += adjusted_fitness / average_adjusted_fitness + # sometimes offspring amount could become `numpy.float64` which will break the code becuause it cannot + # be used in ranges. Forcing conversion to integers here fixes that issue. + species_offspring_amount.append(int(round(offspring_amount))) + + return species_offspring_amount + + def _correct_population_size(self, species_offspring_amount: List[int], missing_offspring: int) -> List[int]: + if missing_offspring > 0: # positive have lacking individuals + # take best species and + species_offspring_amount[self.species_collection.get_best()[0]] += missing_offspring + + elif missing_offspring < 0: # negative have excess individuals + # remove missing number of individuals + excess_offspring = -missing_offspring + excluded_id_list = set() + + while excess_offspring > 0: + worst_species_index, species = self.species_collection.get_worst(1, excluded_id_list) + current_amount = species_offspring_amount[worst_species_index] + + if current_amount > excess_offspring: + current_amount -= excess_offspring + excess_offspring = 0 + else: + excess_offspring -= current_amount + current_amount = 0 + + species_offspring_amount[worst_species_index] = current_amount + excluded_id_list.add(species.id) + + assert excess_offspring == 0 + + return species_offspring_amount diff --git a/pyrevolve/evolution/speciation/population_speciated.py b/pyrevolve/evolution/speciation/population_speciated.py new file mode 100644 index 0000000000..cfafd8ea85 --- /dev/null +++ b/pyrevolve/evolution/speciation/population_speciated.py @@ -0,0 +1,130 @@ +from __future__ import annotations + +import os + +from pyrevolve.evolution.population.population import Population +from pyrevolve.evolution.individual import Individual +from pyrevolve.evolution.speciation.species_collection import count_individuals +from pyrevolve.custom_logging.logger import logger +from .genus import Genus +from .species import Species + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Optional, List, Dict + from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue, SimulatorQueue + from pyrevolve.evolution.speciation.population_speciated_config import PopulationSpeciatedConfig + + +class PopulationSpeciated(Population): + def __init__(self, + config: PopulationSpeciatedConfig, + simulator_queue: SimulatorQueue, + analyzer_queue: Optional[AnalyzerQueue] = None, + next_robot_id: int = 1, + next_species_id: int = 1): + # TODO analyzer + super().__init__(config, simulator_queue, analyzer_queue, next_robot_id) + self.individuals = None # TODO Crash when we should use it + + # Genus contains the collection of different species. + self.genus = Genus(config, next_species_id=next_species_id) + + async def initialize(self, recovered_individuals: Optional[List[Individual]] = None) -> None: + """ + Populates the population (individuals list) with Individual objects that contains their respective genotype. + """ + assert recovered_individuals is None + individuals = [] + + recovered_individuals = [] if recovered_individuals is None else recovered_individuals + + for i in range(self.config.population_size - len(recovered_individuals)): + individual = self._new_individual( + self.config.genotype_constructor(self.config.genotype_conf, self.next_robot_id)) + individuals.append(individual) + self.next_robot_id += 1 + + await self.evaluate(individuals, 0) + individuals = recovered_individuals + individuals + + self.genus.speciate(individuals) + + async def next_generation(self, + gen_num: int, + recovered_individuals: Optional[List[Individual]] = None) -> PopulationSpeciated: + """ + Creates next generation of the population through selection, mutation, crossover + + :param gen_num: generation number + :param recovered_individuals: recovered offspring + :return: new population + """ + # TODO recovery + assert recovered_individuals is None + recovered_individuals = [] if recovered_individuals is None else recovered_individuals + + # TODO create number of individuals based on the number of recovered individuals. + new_genus = await self.genus.next_generation( + recovered_individuals, + self._generate_individual, + lambda individuals: self.evaluate(individuals, gen_num) + ) + + # append recovered individuals ## Same as population.next_gen + # new_individuals = recovered_individuals + new_individuals + + new_population = PopulationSpeciated(self.config, self.simulator_queue, self.analyzer_queue, self.next_robot_id) + new_population.genus = new_genus + logger.info(f'Population selected in gen {gen_num} ' + f'with {len(new_population.genus.species_collection)} species ' + f'and {count_individuals(new_population.genus.species_collection)} individuals.') + + return new_population + + def _generate_individual(self, individuals: List[Individual]) -> Individual: + # Selection operator (based on fitness) + # Crossover + if self.config.crossover_operator is not None and len(individuals) > 1: + # TODO The if above may break if the parent_selection needs more than 2 different individuals as input. + parents = self.config.parent_selection(individuals) + child_genotype = self.config.crossover_operator(parents, self.config.genotype_conf, self.config.crossover_conf) + child = Individual(child_genotype) + else: + child = self.config.selection(individuals) + parents = [child] + + child.genotype.id = self.next_robot_id + self.next_robot_id += 1 + + # Mutation operator + child_genotype = self.config.mutation_operator(child.genotype, self.config.mutation_conf) + + # Create new individual + return self._new_individual(child_genotype, parents) + + def load_snapshot(self, gen_num: int) -> None: + """ + Recovers all genotypes and fitness of the robots in the `gen_num` generation + :param gen_num: number of the generation snapshot to recover + """ + assert gen_num >= 0 + loaded_individuals: Dict[int, Individual] = {} + + def load_individual_fn(_id: int) -> Individual: + return self.config.experiment_management.load_individual(str(_id), self.config) + + data_path = self.config.experiment_management.generation_folder(gen_num) + for file in os.scandir(data_path): + file: os.DirEntry + if not file.name.startswith('species_'): + # skip irrelevant files + continue + if file.is_file() and file.name.endswith('.yaml'): + species = Species.Deserialize(file.path, loaded_individuals, load_individual_fn) + self.genus.species_collection.add_species(species) + + n_loaded_individuals = count_individuals(self.genus.species_collection) + if n_loaded_individuals != self.config.population_size: + raise RuntimeError(f'The loaded population ({n_loaded_individuals}) ' + f'does not match the population size ({self.config.population_size})') diff --git a/pyrevolve/evolution/speciation/population_speciated_config.py b/pyrevolve/evolution/speciation/population_speciated_config.py new file mode 100644 index 0000000000..d9a25947db --- /dev/null +++ b/pyrevolve/evolution/speciation/population_speciated_config.py @@ -0,0 +1,105 @@ +from __future__ import annotations +from ..population.population_config import PopulationConfig + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Callable, Optional, List + from pyrevolve.evolution.individual import Individual + from pyrevolve.genotype import Genotype + from pyrevolve.revolve_bot import RevolveBot + from pyrevolve.tol.manage.robotmanager import RobotManager + + +class PopulationSpeciatedConfig(PopulationConfig): + + # TODO reorder arguments + def __init__(self, + population_size: int, + genotype_constructor: Callable[[object, int], Genotype], + genotype_conf: object, + fitness_function: Callable[[RobotManager, RevolveBot], float], + mutation_operator: Callable[[Genotype, object], Genotype], + mutation_conf: object, + crossover_operator: Callable[[List[Individual], object, object], Genotype], + crossover_conf: object, + selection: Callable[[List[Individual]], Individual], + parent_selection: Callable[[List[Individual]], List[Individual]], + population_management: Callable[ + [List[Individual], List[Individual], Callable[[List[Individual]], Individual]], + List[Individual] + ], + population_management_selector: Callable[[List[Individual]], Individual], + evaluation_time: float, + experiment_name: str, + experiment_management, + are_individuals_compatible_fn: Callable[[Individual, Individual], bool], + young_age_threshold: int = 5, + young_age_fitness_boost: float = 1.1, + old_age_threshold: int = 30, + old_age_fitness_penalty: float = 0.5, + species_max_stagnation: int = 50, + offspring_size: Optional[int] = None, + grace_time: float = 0.0, + objective_functions: Optional[List[Callable[[RobotManager, RevolveBot], float]]] = None): + """ + Creates a PopulationSpeciatedConfig object that sets the particular configuration for the population with species + + :param population_size: size of the population + :param genotype_constructor: class of the genotype used. + First parameter is the config of the genome. + Second is the id of the genome + :param genotype_conf: configuration for genotype constructor + :param fitness_function: function that takes in a `RobotManager` as a parameter and produces a fitness for + the robot. Set to `None` if you want to use `objective_functions` instead. + :param objective_functions: list of functions that takes in a `RobotManager` as a parameter and produces a + fitness for the robot. This parameter is to be instead of the `fitness_function` when using an algorithm + that uses multiple objective optimization, like NSGAII. + :param mutation_operator: operator to be used in mutation + :param mutation_conf: configuration for mutation operator + :param crossover_operator: operator to be used in crossover. + First parameter is the list of parents (usually 2). + Second parameter is the Genotype Conf + Third parameter is Crossover Conf + :param selection: selection type + :param parent_selection: selection type during parent selection + :param population_management: type of population management ie. steady state or generational + :param evaluation_time: duration of an evaluation (experiment time = grace_time + evaluation_time) + :param experiment_name: name for the folder of the current experiment + :param experiment_management: object with methods for managing the current experiment + :param are_individuals_compatible_fn: function that determines if two individuals are compatible + :param young_age_threshold: define until what age (excluded) species are still young + and need to be protected (with a fitness boost) + :param young_age_fitness_boost: Fitness multiplier for young species. + Make sure it is > 1.0 to avoid confusion + :param old_age_threshold: define from what age (excluded) species start to be considered old + and need to be penalized (the best species is forcefully kept young - age 0) + :param old_age_fitness_penalty: Fitness multiplier for old species. + Make sure it is < 1.0 to avoid confusion. + :param species_max_stagnation: maximum number of iterations without improvement of the species. + :param offspring_size (optional): size of offspring (for steady state) + :param grace_time: time to wait before starting the evaluation (experiment time = grace_time + evaluation_time), default to 0.0 + """ + super().__init__(population_size, + genotype_constructor, + genotype_conf, + fitness_function, + mutation_operator, + mutation_conf, + crossover_operator, + crossover_conf, + selection, + parent_selection, + population_management, + population_management_selector, + evaluation_time, + experiment_name, + experiment_management, + offspring_size, + grace_time, + objective_functions) + self.are_individuals_compatible = are_individuals_compatible_fn # type: Callable[[Individual, Individual], bool] + self.young_age_threshold = young_age_threshold + self.young_age_fitness_boost = young_age_fitness_boost + self.old_age_threshold = old_age_threshold + self.old_age_fitness_penalty = old_age_fitness_penalty + self.species_max_stagnation = species_max_stagnation diff --git a/pyrevolve/evolution/speciation/population_speciated_management.py b/pyrevolve/evolution/speciation/population_speciated_management.py new file mode 100644 index 0000000000..a440506f89 --- /dev/null +++ b/pyrevolve/evolution/speciation/population_speciated_management.py @@ -0,0 +1,17 @@ +from pyrevolve.evolution.selection import multiple_selection, multiple_selection_with_duplicates + + +def steady_state_speciated_population_management(old_individuals, new_individuals, number_of_individuals, selector): + # Stead state population + # total size of population is the sum of all the species individuals. + # TODO old function: need parameter for ... + selection_pool = old_individuals + new_individuals + + return multiple_selection_with_duplicates(selection_pool, number_of_individuals, selector) + + +def generational_population_speciated_management(old_individuals, new_individuals, number_of_individuals, selector): + # Note (old_individuals, number_of_individuals, and selector) are not used, + # but for the interface to be similar to steady state speciated. + return new_individuals + diff --git a/pyrevolve/evolution/speciation/species.py b/pyrevolve/evolution/speciation/species.py new file mode 100644 index 0000000000..3199a52c76 --- /dev/null +++ b/pyrevolve/evolution/speciation/species.py @@ -0,0 +1,237 @@ +from __future__ import annotations +import math +import os +import yaml + +from pyrevolve.evolution.speciation.age import Age + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import List, Optional, Dict, Iterator, Callable + from pyrevolve.evolution.individual import Individual + from .population_speciated_config import PopulationSpeciatedConfig + + +class Species: + """ + Collection of individuals that share the same Species + I.e. they have compatible genomes and are considered similar individuals/solutions. + A crossover between two individuals of the same species is supposed to have a good fitness. + """ + + def __init__(self, individuals: List[Individual], species_id: int, age: Age = None, best_fitness: float = 0.0): + + # list of individuals and adjusted fitnesses + # TODO _adjusted_fitness name to split off from regular individuals + self._individuals: List[(Individual, Optional[float])] = [(individual, None) for individual in individuals] + + # ID of the species + self._id: int = species_id + + self.age: Age = Age() if age is None else age + + # Fitness + self._last_best_fitness: float = best_fitness # TODO -Inf |-float('Inf')| + + def __eq__(self, other): + if not isinstance(other, Species): + # don't attempt to compare against unrelated types + return NotImplemented + + if self._id != other._id or self.age != other.age: + return False + + for (individual1, fit1), (individual2, fit2) in zip(self._individuals, other._individuals): + if individual1 != individual2 or fit1 != fit2: + return False + return True + + # TODO refactor population_config + def is_compatible(self, candidate: Individual, population_config: PopulationSpeciatedConfig) -> bool: + """ + Tests if the candidate individual is compatible with this Species + :param candidate: candidate individual to test against the current species + :param population_config: config where to pick the `are genomes compatible` function + :return: if the candidate individual is compatible or not + """ + if self.empty(): + return False + return population_config.are_individuals_compatible(candidate, self._representative()) + + # TODO duplicate code with species collection best/worst function + def get_best_fitness(self) -> float: + """ + Finds the best fitness for individuals in the species. If the species is empty, it returns negative infinity. + :return: the best fitness in the species. + """ + if self.empty(): + return -math.inf + return self.get_best_individual().fitness + + def get_best_individual(self) -> Individual: + """ + :return: the best individual of the species + """ + # TODO cache? + # all the individuals should have fitness defined + return max(self._individuals, + key=lambda individual: individual[0].fitness if individual[0].fitness is not None else -math.inf)[0] + + # TODO refactor population_config + def adjust_fitness(self, is_best_species: bool, population_config: PopulationSpeciatedConfig) -> None: + """ + This method performs fitness sharing. It computes the adjusted fitness of the individuals. + It also boosts the fitness of the young and penalizes old species + + :param is_best_species: True if this is the best species. + Fitness adjustment has a different behaviour if the species is the best one. + :param population_config: collection of configuration parameters + :type population_config PopulationSpeciatedConfig + """ + assert not self.empty() + + n_individuals = len(self._individuals) + + for individual_index, (individual, _) in enumerate(self._individuals): + fitness = individual.fitness + if fitness is None: + fitness = 0.0 + assert fitness >= 0.0 # TODO can we make this work with negative fitnesses? + + fitness = self._adjusted_fitness(fitness, is_best_species, population_config) + + # Compute the adjusted fitness for this member + self._individuals[individual_index] = (individual, fitness / n_individuals) + + def _adjusted_fitness(self, + fitness: float, + is_best_species: bool, + population_config: PopulationSpeciatedConfig) -> float: + """ + Generates the adjusted fitness (not normalized) for the individual. + It's based on its current fitness, the status of the species and the Configuration of the experiment. + + It also updates the self._last_best_fitness. + + :param fitness: real fitness of the individual + :param is_best_species: is `self` the best species in the population? + :param population_config: experimental configuration + :return: the adjusted fitness for the individual (not normalized) + """ + # set small fitness if it is absent. + if fitness == 0.0: + fitness = 0.0001 + + # update the best fitness and stagnation counter + if fitness >= self._last_best_fitness: + self._last_best_fitness = fitness + self.age._no_improvements = 0 + + # TODO refactor + # boost the fitness up to some young age + number_of_generations = self.age.generations() + if number_of_generations < population_config.young_age_threshold: + fitness *= population_config.young_age_fitness_boost + + # penalty for old species + if number_of_generations > population_config.old_age_threshold: + fitness *= population_config.old_age_fitness_penalty + + # EXTREME penalty if this species is stagnating for too long time + # one exception if this is the best species found so far + if not is_best_species and self.age.no_improvements() > population_config.species_max_stagnation: + fitness *= 0.0000001 + + return fitness + + def create_species(self, new_individuals: List[Individual]) -> Species: + """ + Clone the current species with a new list of individuals. + This function is necessary to produce the new generation. + + Updating the age of the species should have already been happened before this. + This function will not update the age. + + :param new_individuals: list of individuals that the cloned species should have + :return: The cloned Species + """ + return Species(new_individuals, self._id, self.age, self._last_best_fitness) + + # ID + @property + def id(self) -> int: + return self._id + + def _representative(self) -> Individual: + """ + Returns a reference to the representative individual. + It crashes if the species is empty. + :return: the representative individual + """ + # TODO study differences in selecting the representative individual. + return self._individuals[0][0] + # return self.get_best_individual() + + # Individuals + def append(self, individual: Individual) -> None: + self._individuals.append((individual, None)) + + def empty(self) -> bool: + return len(self._individuals) == 0 + + def __len__(self): + return len(self._individuals) + + def iter_individuals(self) -> Iterator[(Individual, float)]: + """ + :return: an iterator of (individual, adjusted_fitness) for all individuals of the species + """ + return iter(self._individuals) + + def serialize(self, filename: str) -> None: + """ + Saving the Species to file in yaml formats. + :param filename: file where to save the species + """ + data = { + 'id': self.id, + 'individuals_ids': [int(individual.id) for individual, _ in self._individuals], + 'age': self.age.serialize() + } + + with open(filename, 'w') as file: + yaml.dump(data, file) + + @staticmethod + def Deserialize(filename: str, + loaded_individuals: Dict[int, Individual], + load_individual_fn: Callable[[int], Individual] = None) -> Species: + """ + Alternative constructor that loads the species from file + :param filename: path to the species file + :param loaded_individuals: dictionary of all individuals ever created, to get a reference of them into + inside loaded_individuals list + :param load_individual_fn: optional function to load up individuals from disk + :return: loaded Species + """ + assert os.path.isfile(filename) + with open(filename, 'r') as file: + data = yaml.load(file, Loader=yaml.SafeLoader) + + def read_or_load(_id: int) -> Individual: + if load_individual_fn is not None and _id not in loaded_individuals: + loaded_individuals[_id] = load_individual_fn(_id) + return loaded_individuals[_id] + + individuals = [read_or_load(_id) for _id in data['individuals_ids']] + + species = Species( + individuals=individuals, + species_id=data['id'], + ) + species.age = Age.Deserialize(data['age']) + + return species + + def set_individuals(self, new_individuals: List[Individual]): + self._individuals = [(individual, None) for individual in new_individuals] diff --git a/pyrevolve/evolution/speciation/species_collection.py b/pyrevolve/evolution/speciation/species_collection.py new file mode 100644 index 0000000000..52eb3e9521 --- /dev/null +++ b/pyrevolve/evolution/speciation/species_collection.py @@ -0,0 +1,214 @@ +from __future__ import annotations +from collections.abc import Iterable, Iterator +import math +import numpy +from pyrevolve.evolution.speciation.species import Species +from pyrevolve.evolution.individual import Individual + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import List, Optional, Set + from .population_speciated_config import PopulationSpeciatedConfig + from pyrevolve.evolution.individual import Individual + + +""" +Based on https://refactoring.guru/design-patterns/iterator/python/example +To create an iterator in Python, there are two abstract classes from the built- +in `collections` module - Iterable,Iterator. +method in the iterator. +""" + + +class SpeciesCollection(Iterable): + """ + Concrete Collections provide one or several methods for retrieving fresh + iterator instances, compatible with the collection class. + """ + + def __init__(self, collection: List[Species] = None) -> None: + self._collection: List[Species] \ + = collection if collection is not None else [] + + # CACHING ELEMENTS + # best and worst are a tuple of the index (0) and Individual (1) + # TODO make elements better accessible. + self._best: (int, Species) = (0, None) + #self._worst: (int, Species) = (0, None) + + self._cache_needs_updating: bool = True + + def __iter__(self) -> SpeciesIterator: + """ + The __iter__() method returns the iterator object itself, by default we + return the iterator in ascending order. + """ + return SpeciesIterator(self._collection) + + def set_individuals(self, species_index: int, new_individuals: List[Individual]) -> None: + self._collection[species_index].set_individuals(new_individuals) + self._cache_needs_updating = True + + def _update_cache(self) -> None: + assert len(self._collection) > 0 + + # BEST + species_best_fitness = [species.get_best_individual().fitness for species in self._collection] + species_best_fitness = map(lambda f: -math.inf if f is None else f, species_best_fitness) + index = int(numpy.argmax(species_best_fitness)) + self._best = (index, self._collection[index]) + + # cannot calculate WORST cache, because + # there are 2 different version of the worst individual. + # Which one should be cached? + + self._cache_needs_updating = False + + def get_best(self) -> (int, Species): + """ + :return: the index of the best species and the species + """ + assert len(self._collection) > 0 + + if self._cache_needs_updating: + self._update_cache() + return self._best + + def get_worst(self, + minimal_size: int, + exclude_id_list: Optional[Set[int]] = None) -> (int, Species): + """ + Finds the worst species (based on the best fitness of that species) + Crashes if there are no species with at least `minimal_size` individuals + + :param minimal_size: Species with less individuals than this will not be considered + :param exclude_id_list: Species in this list will be ignored + :return: the index and a reference to the worst species + """ + assert len(self._collection) > 0 + + worst_species_index, worst_species = self._calculate_worst_fitness(minimal_size, exclude_id_list) + + assert worst_species_index != -1 + assert worst_species is not None + + return worst_species_index, worst_species + + def _calculate_worst_fitness(self, + minimal_size: int, + exclude_id_list: Optional[Set[int]]) -> (int, Species): + worst_species_index = -1 + worst_species_fitness = math.inf + worst_species = None + + for i, species in enumerate(self._collection): + + if exclude_id_list is not None \ + and species.id in exclude_id_list: + continue + + if len(species) < minimal_size: + continue + # TODO remove - this is never used since the function is only called in count_offsprings. + # species_fitness = -math.inf + + species_fitness = species.get_best_fitness() + species_fitness = -math.inf if species_fitness is None else species_fitness + if species_fitness < worst_species_fitness: + worst_species_fitness = species_fitness + worst_species = species + worst_species_index = i + + return worst_species_index, worst_species + + def add_species(self, item: Species): + self._collection.append(item) + self._cache_needs_updating = True + + def __len__(self): + """ + Get the length of the species + :return: number of species + """ + return len(self._collection) + + def cleanup(self) -> None: + """ + Remove all empty species (cleanup routine for every case..) + """ + new_collection = [] + for species in self._collection: + if not species.empty(): + new_collection.append(species) + + self._collection = new_collection + + def clear(self) -> None: + self._collection.clear() + + def adjust_fitness(self, config: PopulationSpeciatedConfig) -> None: + """ + Computes the adjusted fitness for all species + """ + for species in self._collection: + species.adjust_fitness(species is self.get_best()[1], config) + + def update(self) -> None: + """ + Updates the best_species, increases age for all species + """ + # the old best species will be None at the first iteration + _, old_best_species = self.get_best() + + for species in self._collection: + # Reset the species and update its age + species.age.increase_generations() + species.age.increase_no_improvement() + + # This prevents the previous best species from sudden death + # If the best species happened to be another one, reset the old + # species age so it still will have a chance of survival and improvement + # if it grows old and stagnates again, it is no longer the best one + # so it will die off anyway. + if old_best_species is not None: + old_best_species.age.reset_generations() + + +def count_individuals(species_collection: SpeciesCollection) -> int: + """ + Counts the number of individuals in the species_list. + :param species_collection: collection of species + :return: the total number of individuals + """ + # count the total number of individuals inside every species in the species_list + number_of_individuals = 0 + + for species in species_collection: + number_of_individuals += len(species) + + return number_of_individuals + + +class SpeciesIterator(Iterator): + """ + Concrete Iterators implement various traversal algorithms. These classes + store the current traversal position at all times. + """ + + def __init__(self, collection: List[Species]) -> None: + self._collection = collection + # `_position` stores the current traversal position. + self._position = 0 + + def __next__(self): + """ + The __next__() method must return the next item in the sequence. On + reaching the end, and in subsequent calls, it must raise StopIteration. + """ + try: + value = self._collection[self._position] + self._position += 1 + except IndexError: + raise StopIteration() + + return value diff --git a/pyrevolve/experiment_management.py b/pyrevolve/experiment_management.py index af9f38207c..f3263e3582 100644 --- a/pyrevolve/experiment_management.py +++ b/pyrevolve/experiment_management.py @@ -1,123 +1,475 @@ +from __future__ import annotations + import os import shutil -import numpy as np +import yaml + from pyrevolve.custom_logging.logger import logger -import sys +from pyrevolve.evolution.individual import Individual +from pyrevolve.tol.manage import measures +from pyrevolve.revolve_bot.revolve_bot import RevolveBot + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import List, AnyStr, Optional + from pyrevolve.tol.manage.measures import BehaviouralMeasurements + from pyrevolve.evolution.speciation.genus import Genus + from pyrevolve.evolution.speciation.species import Species + from pyrevolve.evolution.population.population_config import PopulationConfig class ExperimentManagement: + EXPERIMENT_FOLDER = 'data' + DATA_FOLDER = 'data_fullevolution' + GENERATIONS_FOLDER = 'generations' # ids of robots in the name of all types of files are always phenotype ids, and the standard for id is 'robot_ID' def __init__(self, settings): self.settings = settings - manager_folder = os.path.dirname(self.settings.manager) - self._experiment_folder = os.path.join(manager_folder, 'data', self.settings.experiment_name, self.settings.run) - self._data_folder = os.path.join(self._experiment_folder, 'data_fullevolution') + manager_folder: str = os.path.dirname(self.settings.manager) + self._experiment_folder: str = os.path.join(manager_folder, self.EXPERIMENT_FOLDER, self.settings.experiment_name, self.settings.run) + self._data_folder: str = os.path.join(self._experiment_folder, self.DATA_FOLDER) + self._genotype_folder: str = os.path.join(self.data_folder, 'genotypes') + self._phylogenetic_folder: str = os.path.join(self.data_folder, 'phylogeny') + self._phenotype_folder: str = os.path.join(self.data_folder, 'phenotypes') + self._phenotype_images_folder: str = os.path.join(self._phenotype_folder, 'images') + self._fitness_file_path: str = os.path.join(self.data_folder, 'fitness.csv') + self._fitnesses_saved = set() + self._descriptor_folder: str = os.path.join(self.data_folder, 'descriptors') + self._behavioural_desc_folder: str = os.path.join(self._descriptor_folder, 'behavioural') + self._failed_robots_folder: str = os.path.join(self.data_folder, 'failed_eval_robots') + self._generations_folder: str = os.path.join(self.experiment_folder, self.GENERATIONS_FOLDER) - def create_exp_folders(self): + #TODO refactoring + def create_exp_folders(self) -> None: + """ + Creates all necessary folders for the data to be saved. + WARNING: It deletes the current experiment folder if there is one. + """ if os.path.exists(self.experiment_folder): shutil.rmtree(self.experiment_folder) os.makedirs(self.experiment_folder) os.mkdir(self.data_folder) - os.mkdir(os.path.join(self.data_folder, 'genotypes')) - os.mkdir(os.path.join(self.data_folder, 'phenotypes')) - os.mkdir(os.path.join(self.data_folder, 'descriptors')) - os.mkdir(os.path.join(self.data_folder, 'fitness')) - os.mkdir(os.path.join(self.data_folder, 'phenotype_images')) - os.mkdir(os.path.join(self.data_folder, 'failed_eval_robots')) + os.mkdir(self._genotype_folder) + os.mkdir(self._phylogenetic_folder) + os.mkdir(self._phenotype_folder) + os.mkdir(self._descriptor_folder) + os.mkdir(self._behavioural_desc_folder) + os.mkdir(self._phenotype_images_folder) + os.mkdir(self._failed_robots_folder) + os.mkdir(self._generations_folder) @property - def experiment_folder(self): + def experiment_folder(self) -> str: + """ + Main folder of the experiment, all experimental data should be contained inside here. + + The format of the folder is going to be: + {manager_folder}/data/{experiment_name}/{run_number} + """ return self._experiment_folder @property - def data_folder(self): + def data_folder(self) -> str: + """ + Folder that should contain all of the data for the all the individuals. + It does not contain any data regarding generations. + """ return self._data_folder - def export_genotype(self, individual): + def generation_folder(self, gen_num: int): + return os.path.join(self._generations_folder, f'generation_{gen_num}') + + def export_genotype(self, individual: Individual) -> None: + """ + Export the genotype to file in the `self._genotype_folder` folder + :param individual: individual to export + """ if self.settings.recovery_enabled: - individual.export_genotype(self.data_folder) + individual.export_genotype(self._genotype_folder) + individual.export_phylogenetic_info(self._phylogenetic_folder) - def export_phenotype(self, individual): + def export_phenotype(self, individual: Individual) -> None: + """ + Export the phenotype (yaml only) to file in the `self._phenotype_folder` folder + :param individual: individual to export + """ if self.settings.export_phenotype: - individual.export_phenotype(self.data_folder) + individual.export_phenotype(self._phenotype_folder) - def export_fitnesses(self, individuals): - folder = self.data_folder + def export_fitnesses(self, individuals: List[Individual]) -> None: + """ + Export the fitnesses of all the individuals in the list + :param individuals: list of individuals which fitness need exporting + """ for individual in individuals: - individual.export_fitness(folder) + # TODO this is very inefficient if the elements are already in the set, but at least if works + self.export_fitness(individual) + + def export_fitness(self, individual: Individual) -> None: + """ + Exports fitness to the fitness file. If the individual fitness is already present, the value is overridden + :param individual: individual which fitness needs "saving" + """ + fitness_line = f'{individual.id},{individual.fitness}\n' - def export_fitness(self, individual): - folder = os.path.join(self.data_folder, 'fitness') - individual.export_fitness(folder) + if individual.id in self._fitnesses_saved: + # search and overwrite + logger.warning(f'Individual({individual.id}) fitness is going to be overwritten, ' + f'normally is not be expected') + str_individual_id = str(individual.id) + with open(self._fitness_file_path, 'r') as fitness_file: + lines = fitness_file.readlines() + with open(self._fitness_file_path, 'w') as fitness_file: + found = False + for line in lines: + _file_id, _file_fitness = line.split(',') + if _file_id == str_individual_id: + logger.warning(f'Individual({individual.id}) fitness overwritten, ' + f'the old value was {_file_fitness}') + fitness_file.write(fitness_line) + found = True + else: + fitness_file.write(line) - def export_behavior_measures(self, _id, measures): - filename = os.path.join(self.data_folder, 'descriptors', f'behavior_desc_{_id}.txt') - with open(filename, "w") as f: + if not found: + logger.error(f"fitness of individual_{str_individual_id} should have been in fitness.csv file but " + f"was not found, appending at the end") + self._fitnesses_saved.remove(individual.id) + self.export_fitness(individual) + else: + # append at the end + with open(self._fitness_file_path, 'a') as fitness_file: + fitness_file.write(fitness_line) + self._fitnesses_saved.add(individual.id) + + def export_objectives(self, individual: Individual) -> None: + """ + Exports fitness to the fitness file. If the individual fitness is already present, the value is overridden + :param individual: individual which fitness needs "saving" + """ + objectives_string = ','.join([str(o) for o in individual.objectives]) + fitness_line = f'{individual.id},{objectives_string}\n' + + if individual.id in self._fitnesses_saved: + # search and overwrite + logger.warning(f'Individual({individual.id}) fitness is going to be overwritten, ' + f'normally is not be expected') + str_individual_id = str(individual.id) + with open(self._fitness_file_path, 'r') as fitness_file: + lines = fitness_file.readlines() + with open(self._fitness_file_path, 'w') as fitness_file: + found = False + for line in lines: + splitted_line = line.split(',') + _file_id = splitted_line[0] + _file_fitness = splitted_line[1:] + + if _file_id == str_individual_id: + logger.warning(f'Individual({individual.id}) fitness overwritten, ' + f'the old value was {_file_fitness}') + fitness_file.write(fitness_line) + found = True + else: + fitness_file.write(line) + + if not found: + logger.error(f"fitness of individual_{str_individual_id} should have been in fitness.csv file but " + f"was not found, appending at the end") + self._fitnesses_saved.remove(individual.id) + self.export_fitness(individual) + else: + # append at the end + with open(self._fitness_file_path, 'a') as fitness_file: + fitness_file.write(fitness_line) + self._fitnesses_saved.add(individual.id) + + def export_behavior_measures(self, _id: int, measures: BehaviouralMeasurements, extra_info: Optional[AnyStr] = None) -> None: + """ + Exports behavioural measurements of an individual in the correct folder + :param _id: id of the individual + :param measures: Behavioral measurements of the individual + """ + if extra_info is not None: + filename = f'behavior_desc_{_id}_{extra_info}.txt' + else: + filename = f'behavior_desc_{_id}.txt' + filepath = os.path.join(self._behavioural_desc_folder, filename) + with open(filepath, 'w') as f: if measures is None: f.write(str(None)) else: for key, val in measures.items(): - f.write(f"{key} {val}\n") + f.write(f'{key} {val}\n') + + def export_phenotype_ids(self, identifiers: List[int], dirpath : str): + with open(dirpath + '/identifiers.txt', 'w') as f: + for identifier in identifiers: + f.write("%s\n" % identifier) + + def export_phenotype_images(self, individual: Individual, dirpath: Optional[str] = None, rename_if_present=False) -> None: + + phenotypes = individual.phenotype if isinstance(individual.phenotype, list) else [individual.phenotype] + + dirpath = dirpath if dirpath is not None \ + else self._phenotype_images_folder - def export_phenotype_images(self, dirpath, individual): - individual.phenotype.render_body(os.path.join(self.experiment_folder, dirpath, f'body_{individual.phenotype.id}.png')) - individual.phenotype.render_brain(os.path.join(self.experiment_folder, dirpath, f'brain_{individual.phenotype.id}.png')) + for i, phenotype in enumerate(phenotypes): + try: + # -- Body image -- + body_filename_part = os.path.join(dirpath, f'body_{phenotype.id}_{i}') + if rename_if_present and os.path.exists(f'{body_filename_part}.png'): + counter = 1 + while os.path.exists(f'{body_filename_part}_{counter}.png'): + counter += 1 + os.symlink(f'body_{phenotype.id}_{i}.png', + f'{body_filename_part}_{counter}.png', + target_is_directory=False) + else: + # Write file + phenotype.render_body(f'{body_filename_part}.png') - def export_failed_eval_robot(self, individual): - individual.genotype.export_genotype(os.path.join(self.data_folder, 'failed_eval_robots', f'genotype_{individual.phenotype.id}.txt')) - individual.phenotype.save_file(os.path.join(self.data_folder, 'failed_eval_robots', f'phenotype_{individual.phenotype.id}.yaml')) - individual.phenotype.save_file(os.path.join(self.data_folder, 'failed_eval_robots', f'phenotype_{individual.phenotype.id}.sdf'), conf_type='sdf') + # -- Brain image -- + brain_filename_part = os.path.join(dirpath, f'brain_{phenotype.id}_{i}') + if rename_if_present and os.path.exists(f'{brain_filename_part}.png'): + counter = 1 + while os.path.exists(f'{brain_filename_part}_{counter}.png'): + counter += 1 + os.symlink(f'brain_{phenotype.id}_{i}.png', + f'{brain_filename_part}_{counter}.png', + target_is_directory=False) + else: + # Write file + phenotype.render_brain(os.path.join(dirpath, f'{brain_filename_part}.png')) + except Exception as e: + logger.warning(f'Error rendering phenotype images: {e}') - def export_snapshots(self, individuals, gen_num): + def export_failed_eval_robot(self, individual: Individual) -> None: + """ + Exports genotype and phenotype of a failed individual in the "failed individuals" folder + :param individual: Individual that failed + """ + individual.genotype.export_genotype(os.path.join(self._failed_robots_folder, f'genotype_{individual.phenotype.id}.txt')) + individual.phenotype.save_file(os.path.join(self._failed_robots_folder, f'phenotype_{individual.phenotype.id}.yaml')) + individual.phenotype.save_file(os.path.join(self._failed_robots_folder, f'phenotype_{individual.phenotype.id}.sdf'), conf_type='sdf') + + #TODO refactoring + def export_snapshots(self, individuals: List[Individual], gen_num: int) -> None: if self.settings.recovery_enabled: - path = os.path.join(self.experiment_folder, f'selectedpop_{gen_num}') + path = os.path.join(self._generations_folder, f'generation_{gen_num}') if os.path.exists(path): shutil.rmtree(path) os.mkdir(path) + + ids = [] for ind in individuals: - self.export_phenotype_images(f'selectedpop_{str(gen_num)}', ind) - logger.info(f'Exported snapshot {str(gen_num)} with {str(len(individuals))} individuals') + self.export_phenotype_images(ind, path) + ids.append(ind.id) + self.export_phenotype_ids(ids, path) + logger.info(f'Exported snapshot {gen_num} with {len(individuals)} individuals') - def experiment_is_new(self): + def export_snapshots_species(self, genus: Genus, gen_num: int) -> None: + if self.settings.recovery_enabled: + path = os.path.join(self._generations_folder, f'generation_{gen_num}') + if os.path.exists(path): + shutil.rmtree(path) + os.mkdir(path) + for species in genus.species_collection: + species_on_disk = os.path.join(path, f'species_{species.id}.yaml') + species_folder = os.path.join(path, f'species_{species.id}') + os.mkdir(species_folder) + species.serialize(species_on_disk) + for individual, _ in species.iter_individuals(): + self.export_phenotype_images(individual, species_folder, rename_if_present=True) + + def experiment_is_new(self) -> bool: + """ + Tests if the experiment is new or there is already some data that can be recovered + :return: False if there is already some data that could be recovered + """ if not os.path.exists(self.experiment_folder): return True - path, dirs, files = next(os.walk(os.path.join(self.data_folder, 'fitness'))) - if len(files) == 0: + fitness_file = self._fitness_file_path + if not os.path.isfile(fitness_file): + return True + if os.stat(fitness_file).st_size == 0: return True + + return False + + def read_recovery_state(self, + population_size: int, + offspring_size: int, + species=False, + n_developments: int = 1) -> (int, bool, int): + """ + Read the saved data to determine how many generations have been completed and + if the last generation has partially started evaluating. + + It also resets and reloads the `self._fitnesses_saved` set + + It assumes that, if the N generation is successfully completed, + also the 0..N-1 generation are successfully completed. + + :param population_size: how many individuals should each generation have + :param offspring_size: how many offspring to expect for each generation + :param species: if the data we are about to read is from a speciated population + :param n_developments: number of developments for individuals, default 1 + :return: (last complete generation), (the next generation already has some data), (next robot id) + """ + + # the latest complete snapshot + last_complete_generation = -1 + last_species_id = -1 + + if not species: + for folder in os.scandir(self._generations_folder): + if folder.is_dir() and folder.name.startswith('generation_'): + # Normal population + n_exported_files = 0 + for file in os.scandir(folder.path): + if file.is_file() and file.name.endswith('.png'): + n_exported_files += 1 + + if n_exported_files/n_developments == population_size: + generation_n = int(folder.name.split('_')[1]) + if generation_n > last_complete_generation: + last_complete_generation = generation_n else: - return False + # Species! + for folder in os.scandir(self._generations_folder): + if folder.is_dir() and folder.name.startswith('generation_'): + n_exported_genomes = 0 + for species_on_disk in os.scandir(folder.path): + species_on_disk: os.DirEntry + if not species_on_disk.is_file(): + continue + with open(species_on_disk.path) as file: + species = yaml.load(file, Loader=yaml.SafeLoader) + n_exported_genomes += len(species['individuals_ids']) + species_id = species['id'] - def read_recovery_state(self, population_size, offspring_size): - snapshots = [] + if species_id > last_species_id: + last_species_id = species_id - for r, d, f in os.walk(self.experiment_folder): - for dir in d: - if 'selectedpop' in dir: - exported_files = len([name for name in os.listdir(os.path.join(self.experiment_folder, dir)) if os.path.isfile(os.path.join(self.experiment_folder, dir, name))]) - if exported_files == (population_size * 2): # body and brain files - snapshots.append(int(dir.split('_')[1])) + if n_exported_genomes == population_size: + generation_n = int(folder.name[len('generation_'):]) + if generation_n > last_complete_generation: + last_complete_generation = generation_n - if len(snapshots) > 0: - # the latest complete snapshot - last_snapshot = np.sort(snapshots)[-1] + if last_complete_generation > 0: # number of robots expected until the snapshot - n_robots = population_size + last_snapshot * offspring_size + expected_n_robots: int = population_size + last_complete_generation * offspring_size else: - last_snapshot = -1 - n_robots = 0 + expected_n_robots = 0 - robot_ids = [] - for r, d, f in os.walk(os.path.join(self.data_folder, 'fitness')): - for file in f: - robot_ids.append(int(file.split('.')[0].split('_')[-1])) - last_id = np.sort(robot_ids)[-1] + # reset the fitnesses read + self._fitnesses_saved = set() + + last_id_with_fitness = -1 + with open(self._fitness_file_path, 'r') as fitness_file: + for line in fitness_file: + line_split = line.split(',') + individual_id = line_split[0] + _fitness = line_split[1:] + individual_id = int(individual_id) + self._fitnesses_saved.add(individual_id) + if individual_id > last_id_with_fitness: + last_id_with_fitness = individual_id # if there are more robots to recover than the number expected in this snapshot - if last_id > n_robots: + if last_id_with_fitness > expected_n_robots: # then recover also this partial offspring has_offspring = True else: has_offspring = False - return last_snapshot, has_offspring, last_id+1 + # last complete generation, the next generation already has some data, next robot id + #TODO return also last species ID + return last_complete_generation, has_offspring, last_id_with_fitness+1, last_species_id+1, + + def load_individual(self, + _id: int, + config: PopulationConfig, + fitness: Optional[str] = None) -> Individual: + """ + Loads an individual from disk + :param _id: id of the robot to load + :param config: population config, needed to know which genome to load + :param fitness: optionally pass the fitness already in, to speed up the loading process. + Pass it the fitness as a 'None' instead of None or it will read the file anyway. + :return: the Individual loaded from the file system + """ + + genotype = config.genotype_constructor(config.genotype_conf, _id) + genotype.load_genotype( + os.path.join(self._genotype_folder, f'genotype_{_id}.txt')) + + individual = Individual(genotype) + individual.develop() + if isinstance(individual.phenotype, RevolveBot): + phenotypes = [individual.phenotype] + elif isinstance(individual.phenotype, list): + phenotypes = individual.phenotype + else: + raise RuntimeError(f"individual.phenotype is of wrong type: {type(individual.phenotype)}") + + for phenotype in phenotypes: + phenotype.update_substrate() + phenotype.measure_phenotype() + + # load fitness + if fitness is None: + with open(self._fitness_file_path, 'r') as fitness_file: + for line in fitness_file: + line_split = line.split(',') + line_id = line_split[0] + line_fitness = line_split[1:] # type List[str] + if line_id == _id: + objectives = [None if line_fitness_v.startswith('None') else float(line_fitness_v) for line_fitness_v in line_fitness] + if len(line_fitness) == 1: + fitness = objectives[0] + objectives = None + break + else: + fitness = None + objectives = None + else: + fitness = float(fitness) + + individual.fitness = None if fitness is None else fitness + individual.objectives = None if objectives is None else objectives + + for i, phenotype in enumerate(phenotypes): + # if there are phenotype alternatives, load with _{i} id postfix + full_id = str(phenotype.id) if len(phenotypes) == 1 else f"{phenotype.id}_{i}" + with open(os.path.join(self._behavioural_desc_folder, f'behavior_desc_{full_id}.txt')) as f: + lines = f.readlines() + if lines[0] == 'None': + phenotype._behavioural_measurements = None + else: + phenotype._behavioural_measurements = measures.BehaviouralMeasurements() + for line in lines: + line_split = line.split(' ') + line_0 = line_split[0] + line_1 = line_split[1] + if line_0 == 'velocity': + phenotype._behavioural_measurements.velocity = \ + float(line_1) if line_1 != 'None\n' else None + # if line_0 == 'displacement': + # phenotype._behavioural_measurements.displacement = \ + # float(line_1) if line_1 != 'None\n' else None + elif line_0 == 'displacement_velocity': + phenotype._behavioural_measurements.displacement_velocity = \ + float(line_1) if line_1 != 'None\n' else None + elif line_0 == 'displacement_velocity_hill': + phenotype._behavioural_measurements.displacement_velocity_hill = \ + float(line_1) if line_1 != 'None\n' else None + elif line_0 == 'head_balance': + phenotype._behavioural_measurements.head_balance = \ + float(line_1) if line_1 != 'None\n' else None + elif line_0 == 'contacts': + phenotype._behavioural_measurements.contacts = \ + float(line_1) if line_1 != 'None\n' else None + + return individual diff --git a/pyrevolve/gazebo/manage/world.py b/pyrevolve/gazebo/manage/world.py index c5452dac59..f25a95b026 100644 --- a/pyrevolve/gazebo/manage/world.py +++ b/pyrevolve/gazebo/manage/world.py @@ -1,15 +1,17 @@ #!/usr/bin/env python3 -from __future__ import absolute_import +from __future__ import absolute_import, annotations -# Global / system import time - +from pygazebo import Publisher, Manager as PyGazeboManager from pygazebo.msg import world_control_pb2 -# Revolve -from ..connect import connect, RequestHandler -import logging -from ...custom_logging.logger import logger +from pyrevolve.gazebo.connect import connect, RequestHandler +from pyrevolve.custom_logging.logger import logger + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Optional, Any + # Construct a message base from the time. This should make # it unique enough for consecutive use when the script @@ -29,34 +31,35 @@ class WorldManager(object): def __init__( self, _private=None, - world_address=None, + world_address: (str, int) = None, ): """ - - :param _private: - :return: + This constructor is private because a constructor cannot be async. Use create instead. + :param _private: to make the constructor private + :param world_address: simulator address """ if _private is not self._PRIVATE: raise ValueError("The manager cannot be directly constructed," "rather the `create` coroutine should be used.") - self.world_address = world_address - - self.manager = None - self.world_control = None + self.world_address: (str, int) = world_address - self.request_handler = None + # These are later initialized in `self._init()` + # Calling the create function instead of the constructor directly ensures + # that these values are correctly initialized + self.manager: PyGazeboManager = None + self.world_control: Publisher = None + self.request_handler: RequestHandler = None @classmethod async def create( cls, - world_address=("127.0.0.1", 11345), - ): + world_address: (str, int) = ("127.0.0.1", 11345), + ) -> WorldManager: """ - - :param analyzer_address: - :param world_address: - :return: + Substitute async constructor. It creates and initializes the connection to the simulator. + :param world_address: address to connect to + :return: The WorldManager object initialized and connected """ self = cls( _private=cls._PRIVATE, @@ -69,24 +72,23 @@ async def disconnect(self): await self.manager.stop() await self.request_handler.stop() - async def _init(self): + async def _init(self) -> None: """ Initializes connections for the world manager - :return: """ if self.manager is not None: return # Initialize the manager connections as well as the general request # handler - self.manager = await connect(self.world_address[0], self.world_address[1]) + self.manager: PyGazeboManager = await connect(self.world_address[0], self.world_address[1]) - self.world_control = await self.manager.advertise( + self.world_control: Publisher = await self.manager.advertise( topic_name='/gazebo/default/world_control', msg_type='gazebo.msgs.WorldControl' ) - self.request_handler = await RequestHandler.create( + self.request_handler: RequestHandler = await RequestHandler.create( manager=self.manager, msg_id_base=MSG_BASE ) @@ -94,43 +96,36 @@ async def _init(self): # Wait for connections await self.world_control.wait_for_listener() - async def pause(self, pause=True): + async def pause(self, pause: bool = True) -> None: """ Pause / unpause the world :param pause: :return: Future for the published message """ - if pause: - logger.info("Pausing the world.") - else: - logger.info("Resuming the world.") - msg = world_control_pb2.WorldControl() msg.pause = pause - future = await self.world_control.publish(msg) - return future + await self.world_control.publish(msg) async def reset( self, - rall=False, - time_only=True, - model_only=False - ): + rall: bool = False, + time_only: bool = True, + model_only: bool = False + ) -> None: """ Reset the world. Defaults to time only, since that appears to be the only thing that is working by default anyway. - :param model_only: - :param time_only: - :param rall: + :param rall: reset all + :param model_only: resets only the models + :param time_only: resets only the time """ - logger.info("Resetting the world state.") msg = world_control_pb2.WorldControl() msg.reset.all = rall msg.reset.model_only = model_only msg.reset.time_only = time_only await self.world_control.publish(msg) - async def insert_model(self, sdf, timeout=None): + async def insert_model(self, sdf, timeout: Optional[float] = None) -> Any: """ Insert a model wrapped in an SDF tag into the world. Make sure it has a unique name, as it will be literally inserted into the @@ -144,7 +139,7 @@ async def insert_model(self, sdf, timeout=None): :type sdf: SDF :param timeout: Life span of the model :type timeout: float|None - :return: + :return: the message response """ return await self.request_handler.do_gazebo_request( request="insert_sdf", @@ -154,8 +149,9 @@ async def insert_model(self, sdf, timeout=None): async def delete_model( self, - name, req="entity_delete" - ): + name: str, + req: str = "entity_delete" + ) -> Any: """ Deletes the model with the given name from the world. :param name: @@ -163,7 +159,7 @@ async def delete_model( delete a robot, I suggest using `delete_robot` rather than `entity_delete` because this attempts to prevent some issues with segmentation faults occurring from deleting sensors. - :return: + :return: the message response """ return await self.request_handler.do_gazebo_request( request=req, diff --git a/pyrevolve/genotype/bodybrain_composition/config.py b/pyrevolve/genotype/bodybrain_composition/config.py new file mode 100644 index 0000000000..a3ea51f3fd --- /dev/null +++ b/pyrevolve/genotype/bodybrain_composition/config.py @@ -0,0 +1,58 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import Any, Callable, Generic, List, TypeVar + +from pyrevolve.revolve_bot.brain import Brain +from pyrevolve.revolve_bot.revolve_module import CoreModule + +from .sub_genotype import SubGenotype + +_body_genotype = TypeVar("_body_genotype", bound=SubGenotype) +_body_crossover_config = TypeVar("_body_crossover_config") +_body_mutate_config = TypeVar("_body_mutate_config") +_body_develop_config = TypeVar("_body_develop_config") +_brain_genotype = TypeVar("_brain_genotype", bound=SubGenotype) +_brain_crossover_config = TypeVar("_brain_crossover_config") +_brain_mutate_config = TypeVar("_brain_mutate_config") +_brain_develop_config = TypeVar("_brain_develop_config") + + +@dataclass(frozen=True) +class Config( + Generic[ + _body_genotype, + _body_crossover_config, + _body_mutate_config, + _body_develop_config, + _brain_genotype, + _brain_crossover_config, + _brain_mutate_config, + _brain_develop_config, + ] +): + body_crossover: Callable[ + [ + List[_body_genotype], + _body_crossover_config, + ], + _body_genotype, + ] + body_crossover_config: _body_crossover_config + body_mutate: Callable[[_body_genotype, _body_mutate_config], _body_genotype] + body_mutate_config: _body_mutate_config + body_develop: Callable[[_body_genotype, _brain_develop_config], CoreModule] + body_develop_config: _body_develop_config + + brain_crossover: Callable[ + [ + List[_brain_genotype], + _brain_crossover_config, + ], + _brain_genotype, + ] + brain_crossover_config: _brain_crossover_config + brain_mutate: Callable[[_brain_genotype, _brain_mutate_config], _brain_genotype] + brain_mutate_config: _brain_mutate_config + brain_develop: Callable[[_brain_genotype, _brain_develop_config, CoreModule], Brain] + brain_develop_config: _brain_develop_config diff --git a/pyrevolve/genotype/bodybrain_composition/crossover.py b/pyrevolve/genotype/bodybrain_composition/crossover.py new file mode 100644 index 0000000000..e50273c363 --- /dev/null +++ b/pyrevolve/genotype/bodybrain_composition/crossover.py @@ -0,0 +1,46 @@ +from typing import List + +from pyrevolve.evolution.individual import Individual +from typeguard import typechecked + +from .config import Config +from .genotype import Genotype + + +@typechecked +def crossover(parents: List[Individual], _, config: Config) -> Genotype: + assert len(parents) >= 1 + + body_genotype_type = type(parents[0].genotype.body_genotype) + brain_genotype_type = type(parents[0].genotype.brain_genotype) + + # assert all parents' body genotypes are of the same type + # same for brain genotypes + assert all( + [ + type(parent.genotype.body_genotype) == body_genotype_type + for parent in parents + ] + ) + assert all( + [ + type(parent.genotype.brain_genotype) == brain_genotype_type + for parent in parents + ] + ) + + body_child = config.body_crossover( + [parent.genotype.body_genotype for parent in parents], + config.body_crossover_config, + ) + assert type(body_child) == body_genotype_type + + brain_child = config.brain_crossover( + [parent.genotype.brain_genotype for parent in parents], + config.brain_crossover_config, + ) + assert type(brain_child) == brain_genotype_type + + return Genotype( + 0xDEADBEEF, config, body_child, brain_child + ) # id is placeholder. expected to be set by framework later diff --git a/pyrevolve/genotype/bodybrain_composition/genotype.py b/pyrevolve/genotype/bodybrain_composition/genotype.py new file mode 100644 index 0000000000..f368797cc6 --- /dev/null +++ b/pyrevolve/genotype/bodybrain_composition/genotype.py @@ -0,0 +1,76 @@ +from __future__ import annotations + +import json +from typing import Any, Dict, Generic, TypeVar + +from pyrevolve.genotype import Genotype as RevolveGenotype +from pyrevolve.revolve_bot import RevolveBot +from typeguard import typechecked + +from .config import Config +from .sub_genotype import SubGenotype + +_body_type = TypeVar("_body_type", bound=SubGenotype) +_brain_type = TypeVar("_brain_type", bound=SubGenotype) + + +class Genotype(Generic[_body_type, _brain_type], RevolveGenotype): + id: int + _config: Config + _body_genotype: _body_type + _brain_genotype: _brain_type + + @typechecked + def __init__( + self, + robot_id: int, + config: Config, + body_genotype: _body_type, + brain_genotype: _brain_type, + ): + self.id = robot_id + self._config = config + self._body_genotype = body_genotype + self._brain_genotype = brain_genotype + + @typechecked + def develop(self) -> RevolveBot: + phenotype = RevolveBot(self.id) + phenotype._body = self._config.body_develop( + self._body_genotype, self._config.body_develop_config + ) + phenotype._brain = self._config.brain_develop( + self._brain_genotype, self._config.brain_develop_config, phenotype._body + ) + # TODO is this update_substrate required? + phenotype.update_substrate() + return phenotype + + @typechecked + def export_genotype(self, filepath: str): + body = self._body_genotype.serialize_to_dict() + brain = self._brain_genotype.serialize_to_dict() + asjson = json.dumps({"body": body, "brain": brain}) + file = open(filepath, "w+") + file.write(asjson) + file.close() + + @typechecked + def load_genotype(self, filepath: str) -> None: + file = open(filepath) + data = json.loads(file.read()) + + assert isinstance(data, Dict) + assert "body" in data + assert "brain" in data + + self._body_genotype.deserialize_from_dict(data["body"]) + self._brain_genotype.deserialize_from_dict(data["brain"]) + + @property + def body_genotype(self) -> _body_type: + return self._body_genotype + + @property + def brain_genotype(self) -> _brain_type: + return self._brain_genotype diff --git a/pyrevolve/genotype/bodybrain_composition/mutation.py b/pyrevolve/genotype/bodybrain_composition/mutation.py new file mode 100644 index 0000000000..fb952b630d --- /dev/null +++ b/pyrevolve/genotype/bodybrain_composition/mutation.py @@ -0,0 +1,19 @@ +from typeguard import typechecked + +from .config import Config +from .genotype import Genotype + + +@typechecked +def mutate(genotype: Genotype, config: Config) -> Genotype: + mutated_body = config.body_mutate(genotype.body_genotype, config.body_mutate_config) + assert type(mutated_body) == type(genotype.body_genotype) + + mutated_brain = config.brain_mutate( + genotype.brain_genotype, config.brain_mutate_config + ) + assert type(mutated_brain) == type(genotype.brain_genotype) + + return Genotype( + genotype.id, config, mutated_body, mutated_brain + ) # id must be the same as input id diff --git a/pyrevolve/genotype/bodybrain_composition/sub_genotype.py b/pyrevolve/genotype/bodybrain_composition/sub_genotype.py new file mode 100644 index 0000000000..6e5651e82c --- /dev/null +++ b/pyrevolve/genotype/bodybrain_composition/sub_genotype.py @@ -0,0 +1,12 @@ +from abc import ABC, abstractmethod +from typing import Any, Dict, Generic, TypeVar + + +class SubGenotype(ABC): + @abstractmethod + def serialize_to_dict() -> Dict[Any, Any]: + pass + + @abstractmethod + def deserialize_from_dict(self, serialized: Dict[Any, Any]): + pass diff --git a/pyrevolve/genotype/cppnneat/body/config.py b/pyrevolve/genotype/cppnneat/body/config.py new file mode 100644 index 0000000000..8a72a6e55b --- /dev/null +++ b/pyrevolve/genotype/cppnneat/body/config.py @@ -0,0 +1,8 @@ +from dataclasses import dataclass + +from ..config import Config as CppnneatConfig + + +@dataclass +class Config(CppnneatConfig): + pass diff --git a/pyrevolve/genotype/cppnneat/body/crossover.py b/pyrevolve/genotype/cppnneat/body/crossover.py new file mode 100644 index 0000000000..e8ebbb3f4a --- /dev/null +++ b/pyrevolve/genotype/cppnneat/body/crossover.py @@ -0,0 +1,13 @@ +from typing import List + +from typeguard import typechecked + +from ..crossover import crossover as cppnneat_crossover +from .config import Config +from .genotype import Genotype + + +@typechecked +def crossover(parents: List[Genotype], config: Config) -> Genotype: + crossover_parent = cppnneat_crossover(parents, config) + return Genotype(crossover_parent._multineat_genome) diff --git a/pyrevolve/genotype/cppnneat/body/develop.py b/pyrevolve/genotype/cppnneat/body/develop.py new file mode 100644 index 0000000000..c733347443 --- /dev/null +++ b/pyrevolve/genotype/cppnneat/body/develop.py @@ -0,0 +1,208 @@ +from dataclasses import dataclass +from queue import Queue +from typing import Any, Optional, Set, Tuple + +import multineat +from pyrevolve.revolve_bot.revolve_module import ( + ActiveHingeModule, + BrickModule, + CoreModule, + RevolveModule, +) +from typeguard import typechecked + +from .config import Config +from .genotype import Genotype + +# TODO use numpy for vector arithmetic + + +@dataclass +class _Module: + position: Tuple[int, int, int] + forward: Tuple[int, int, int] + up: Tuple[int, int, int] + chain_length: int + module_reference: CoreModule + + +@typechecked +def develop(genotype: Genotype, config: Config) -> CoreModule: + max_parts = 10 + + body_net = multineat.NeuralNetwork() + genotype.multineat_genome.BuildPhenotype(body_net) + + to_explore: Queue[RevolveModule] = Queue() + grid: Set[Tuple(int, int, int)] = set() + + core_module = CoreModule() + core_module.id = "core" + core_module.rgb = [1, 1, 0] + core_module.orientation = 0 + + to_explore.put( + _Module((0, 0, 0), (0, -1, 0), (0, 0, 1), 0, core_module) + ) # forward is always slot 1 + grid.add((0, 0, 0)) + part_count = 1 + + while not to_explore.empty(): + module: _Module = to_explore.get() + + child_index_range: range + if type(module.module_reference) == CoreModule: + child_index_range = range(0, 4) + elif type(module.module_reference) == BrickModule: + child_index_range = range(1, 4) + elif type(module.module_reference) == ActiveHingeModule: + child_index_range = range(1, 2) + else: # Should actually never arrive here but just checking module type to be sure + raise RuntimeError + + for child_index in child_index_range: + if part_count < max_parts: + child = _add_child(body_net, module, child_index, grid) + if child != None: + to_explore.put(child) + part_count += 1 + + return core_module + + +# get module type, orientation +def _evaluate_cppg( + body_net: multineat.NeuralNetwork, + position: Tuple[int, int, int], + chain_length: int, +) -> Tuple[Any, int]: + body_net.Input( + [1.0, position[0], position[1], position[2], chain_length] + ) # 1.0 is the bias input + body_net.Activate() + outputs = body_net.Output() + + # get module type from output probabilities + type_probs = [outputs[0], outputs[1], outputs[2]] + types = [None, BrickModule, ActiveHingeModule] + module_type = types[type_probs.index(min(type_probs))] + + # get rotation from output probabilities + rotation_probs = [outputs[3], outputs[4]] + rotation = rotation_probs.index(min(rotation_probs)) + + return (module_type, rotation) + + +def _add_child( + body_net: multineat.NeuralNetwork, + module: _Module, + child_index: int, + grid: Set[Tuple[int, int, int]], +) -> Optional[_Module]: + forward = _get_new_forward(module.forward, module.up, child_index) + position = _add(module.position, forward) + chain_length = module.chain_length + 1 + + # if grid cell is occupied, don't make a child + # else, set cell as occupied + if position in grid: + return None + else: + grid.add(position) + + child_type, orientation = _evaluate_cppg(body_net, position, chain_length) + if child_type == None: + return None + + child = child_type() + module.module_reference.children[child_index] = child + child.id = module.module_reference.id + "_" + str(child_index) + child.orientation = orientation * 90 + + # coloring + if child_type == BrickModule: + child.rgb = [1, 0, 0] + elif child_type == ActiveHingeModule: + child.rgb = [0, 1, 0] + else: # Should actually never arrive here but just checking module type to be sure + raise RuntimeError + + up = _get_new_up(module.up, forward, orientation) + return _Module( + position, + forward, + up, + chain_length, + child, + ) + + # TODO check for self collision? + # revolve_bot -> update_subtrate throws on collision (raise for intersection True) + + +def _get_new_forward( + parent_forward: Tuple[int, int, int], parent_up: Tuple[int, int, int], slot: int +) -> Tuple[int, int, int]: + rotation: int + if slot == 0: + rotation = 2 + elif slot == 1: + rotation = 0 + elif slot == 2: + rotation = 3 + else: + rotation = 1 + + return _rotate(parent_forward, parent_up, rotation) + + +def _get_new_up( + parent_up: Tuple[int, int, int], new_forward: Tuple[int, int, int], orientation: int +) -> Tuple[int, int, int]: + return _rotate(parent_up, new_forward, orientation) + + +def _add(a: Tuple[int, int, int], b: Tuple[int, int, int]) -> Tuple[int, int, int]: + return tuple(map(sum, zip(a, b))) + + +def _timesscalar(a: Tuple[int, int, int], scalar: int) -> Tuple[int, int, int]: + return (a[0] * scalar, a[1] * scalar, a[2] * scalar) + + +def _cross(a: Tuple[int, int, int], b: Tuple[int, int, int]) -> Tuple[int, int, int]: + return ( + a[1] * b[2] - a[2] * b[1], + a[2] * b[0] - a[0] * b[2], + a[0] * b[1] - a[1] * b[0], + ) + + +def _dot(a: Tuple[int, int, int], b: Tuple[int, int, int]) -> int: + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + + +# rotates a around b. angle from [0,1,2,3]. 90 degrees each +def _rotate( + a: Tuple[int, int, int], b: Tuple[int, int, int], angle: int +) -> Tuple[int, int, int]: + cosangle: int + sinangle: int + if angle == 0: + cosangle = 1 + sinangle = 0 + elif angle == 1: + cosangle = 0 + sinangle = 1 + elif angle == 2: + cosangle = -1 + sinangle = 0 + else: + cosangle = 0 + sinangle = -1 + + return _add( + _add(_timesscalar(a, cosangle), _timesscalar(_cross(b, a), sinangle)), + _timesscalar(b, _dot(b, a) * (1 - cosangle)), + ) diff --git a/pyrevolve/genotype/cppnneat/body/genotype.py b/pyrevolve/genotype/cppnneat/body/genotype.py new file mode 100644 index 0000000000..b930891c19 --- /dev/null +++ b/pyrevolve/genotype/cppnneat/body/genotype.py @@ -0,0 +1,30 @@ +from __future__ import annotations + +import multineat +from typeguard import typechecked + +from ..genotype import Genotype as CppnneatGenotype + + +class Genotype(CppnneatGenotype): + @staticmethod + @typechecked + def random( + multineat_params: multineat.Parameters, + output_activation_type: multineat.ActivationFunction, + n_start_mutations: int, + innov_db: multineat.InnovationDatabase, + rng: multineat.RNG, + ) -> Genotype: + n_inputs = 4 + 1 # bias(always 1), pos_x, pos_y, pos_z, chain_length + n_outputs = 5 # empty, brick, activehinge, rot0, rot90 + random_parent = super(Genotype, Genotype).random( + n_inputs, + n_outputs, + multineat_params, + output_activation_type, + n_start_mutations, + innov_db, + rng, + ) + return Genotype(random_parent._multineat_genome) diff --git a/pyrevolve/genotype/cppnneat/body/mutation.py b/pyrevolve/genotype/cppnneat/body/mutation.py new file mode 100644 index 0000000000..8ab7dab4c1 --- /dev/null +++ b/pyrevolve/genotype/cppnneat/body/mutation.py @@ -0,0 +1,11 @@ +from typeguard import typechecked + +from ..mutation import mutate as cppnneat_mutate +from .config import Config +from .genotype import Genotype + + +@typechecked +def mutate(genotype: Genotype, config: Config) -> Genotype: + parent_mutate = cppnneat_mutate(genotype, config) + return Genotype(parent_mutate._multineat_genome) diff --git a/pyrevolve/genotype/cppnneat/brain/config.py b/pyrevolve/genotype/cppnneat/brain/config.py new file mode 100644 index 0000000000..62a56fd916 --- /dev/null +++ b/pyrevolve/genotype/cppnneat/brain/config.py @@ -0,0 +1,13 @@ +from dataclasses import dataclass + +from ..config import Config as CppnneatConfig + + +@dataclass +class Config(CppnneatConfig): + abs_output_bound: float # clamp actuator position between this and -1 this. if you don't use, use 1.0 + use_frame_of_reference: bool # enable gongjin steering + output_signal_factor: float # actuator gain. if you don't know, use 1.0 + range_ub: float # scale weights to be between this and -this. if you don't know, use 1.0 + init_neuron_state: float # initial value of neurons. if you don't know, use 1/2*sqrt(2) + reset_neuron_random: bool # ignore init neuron state and use random value diff --git a/pyrevolve/genotype/cppnneat/brain/crossover.py b/pyrevolve/genotype/cppnneat/brain/crossover.py new file mode 100644 index 0000000000..e8ebbb3f4a --- /dev/null +++ b/pyrevolve/genotype/cppnneat/brain/crossover.py @@ -0,0 +1,13 @@ +from typing import List + +from typeguard import typechecked + +from ..crossover import crossover as cppnneat_crossover +from .config import Config +from .genotype import Genotype + + +@typechecked +def crossover(parents: List[Genotype], config: Config) -> Genotype: + crossover_parent = cppnneat_crossover(parents, config) + return Genotype(crossover_parent._multineat_genome) diff --git a/pyrevolve/genotype/cppnneat/brain/develop.py b/pyrevolve/genotype/cppnneat/brain/develop.py new file mode 100644 index 0000000000..4b5f6537e5 --- /dev/null +++ b/pyrevolve/genotype/cppnneat/brain/develop.py @@ -0,0 +1,81 @@ +from xml.etree import ElementTree + +import multineat +from pyrevolve.revolve_bot.brain import Brain +from pyrevolve.revolve_bot.brain.cpg_target import BrainCPGTarget +from pyrevolve.revolve_bot.revolve_bot import RevolveBot +from pyrevolve.revolve_bot.revolve_module import CoreModule +from typeguard import typechecked + +from .config import Config +from .genotype import Genotype + + +@typechecked +def develop(genotype: Genotype, config: Config, body: CoreModule) -> Brain: + brain = BrainCPGTarget() + brain.abs_output_bound = config.abs_output_bound + brain.use_frame_of_reference = config.use_frame_of_reference + brain.output_signal_factor = config.output_signal_factor + brain.range_ub = config.range_ub + brain.init_neuron_state = config.init_neuron_state + brain.reset_neuron_random = config.reset_neuron_random + + # Convert to sdf so we can extract things like position and order of actuators exactly like they would be read by the plugin + bot = RevolveBot("dummy") + bot._body = body + bot._brain = BrainCPGTarget() # dummy + bot.update_substrate() + sdf = bot.to_sdf() + root = ElementTree.fromstring(sdf) + namespaces = {"rv": "https://github.com/ci-group/revolve"} + actuators = root.findall( + "model/plugin[@name='robot_controller']/rv:robot_config/rv:brain/rv:actuators/rv:servomotor", + namespaces, + ) + + # calculate weights from actuators + brain.weights = [] + + brain_net = multineat.NeuralNetwork() + genotype.multineat_genome.BuildPhenotype(brain_net) + + parsecoords = lambda coordsstr: list(map(lambda x: float(x), coordsstr.split(";"))) + + # internal weights + for actuator in actuators: + coords = parsecoords(actuator.attrib["coordinates"]) + brain_net.Input( + [1.0, coords[0], coords[1], coords[2], coords[0], coords[1], coords[2]] + ) # 1.0 is the bias input + brain_net.Activate() + weight = brain_net.Output()[0] + brain.weights.append(weight) + + # external weights + for i, actuator in enumerate(actuators[:-1]): + for neighbour in actuators[i + 1 :]: + leftcoords = parsecoords(actuator.attrib["coordinates"]) + rightcoords = parsecoords(neighbour.attrib["coordinates"]) + if ( + abs(leftcoords[0] - rightcoords[0]) + + abs(leftcoords[1] - rightcoords[1]) + + abs(leftcoords[2] - rightcoords[2]) + < 2.01 + ): + brain_net.Input( + [ + 1.0, + leftcoords[0], + leftcoords[1], + leftcoords[2], + rightcoords[0], + rightcoords[1], + rightcoords[2], + ] + ) + brain_net.Activate() + weight = brain_net.Output()[0] + brain.weights.append(weight) + + return brain diff --git a/pyrevolve/genotype/cppnneat/brain/genotype.py b/pyrevolve/genotype/cppnneat/brain/genotype.py new file mode 100644 index 0000000000..265ed983d3 --- /dev/null +++ b/pyrevolve/genotype/cppnneat/brain/genotype.py @@ -0,0 +1,30 @@ +from __future__ import annotations + +import multineat +from typeguard import typechecked + +from ..genotype import Genotype as CppnneatGenotype + + +class Genotype(CppnneatGenotype): + @staticmethod + @typechecked + def random( + multineat_params: multineat.Parameters, + output_activation_type: multineat.ActivationFunction, + n_start_mutations: int, + innov_db: multineat.InnovationDatabase, + rng: multineat.RNG, + ) -> Genotype: + n_inputs = 6 + 1 # bias(always 1), x1, y1, z1, x2, y2, z2 + n_outputs = 1 # weight + random_parent = super(Genotype, Genotype).random( + n_inputs, + n_outputs, + multineat_params, + output_activation_type, + n_start_mutations, + innov_db, + rng, + ) + return Genotype(random_parent._multineat_genome) diff --git a/pyrevolve/genotype/cppnneat/brain/mutation.py b/pyrevolve/genotype/cppnneat/brain/mutation.py new file mode 100644 index 0000000000..8ab7dab4c1 --- /dev/null +++ b/pyrevolve/genotype/cppnneat/brain/mutation.py @@ -0,0 +1,11 @@ +from typeguard import typechecked + +from ..mutation import mutate as cppnneat_mutate +from .config import Config +from .genotype import Genotype + + +@typechecked +def mutate(genotype: Genotype, config: Config) -> Genotype: + parent_mutate = cppnneat_mutate(genotype, config) + return Genotype(parent_mutate._multineat_genome) diff --git a/pyrevolve/genotype/cppnneat/config.py b/pyrevolve/genotype/cppnneat/config.py new file mode 100644 index 0000000000..67ba8c563b --- /dev/null +++ b/pyrevolve/genotype/cppnneat/config.py @@ -0,0 +1,57 @@ +from __future__ import annotations + +from dataclasses import dataclass + +import multineat + + +def get_default_multineat_params() -> multineat.Params: + multineat_params = multineat.Parameters() + + multineat_params.MutateRemLinkProb = 0.02 + multineat_params.RecurrentProb = 0.0 + multineat_params.OverallMutationRate = 0.15 + multineat_params.MutateAddLinkProb = 0.08 + multineat_params.MutateAddNeuronProb = 0.01 + multineat_params.MutateWeightsProb = 0.90 + multineat_params.MaxWeight = 8.0 + multineat_params.WeightMutationMaxPower = 0.2 + multineat_params.WeightReplacementMaxPower = 1.0 + multineat_params.MutateActivationAProb = 0.0 + multineat_params.ActivationAMutationMaxPower = 0.5 + multineat_params.MinActivationA = 0.05 + multineat_params.MaxActivationA = 6.0 + + multineat_params.MutateNeuronActivationTypeProb = 0.03 + + multineat_params.ActivationFunction_SignedSigmoid_Prob = 0.0 + multineat_params.ActivationFunction_UnsignedSigmoid_Prob = 0.0 + multineat_params.ActivationFunction_Tanh_Prob = 1.0 + multineat_params.ActivationFunction_TanhCubic_Prob = 0.0 + multineat_params.ActivationFunction_SignedStep_Prob = 1.0 + multineat_params.ActivationFunction_UnsignedStep_Prob = 0.0 + multineat_params.ActivationFunction_SignedGauss_Prob = 1.0 + multineat_params.ActivationFunction_UnsignedGauss_Prob = 0.0 + multineat_params.ActivationFunction_Abs_Prob = 0.0 + multineat_params.ActivationFunction_SignedSine_Prob = 1.0 + multineat_params.ActivationFunction_UnsignedSine_Prob = 0.0 + multineat_params.ActivationFunction_Linear_Prob = 1.0 + + multineat_params.MutateNeuronTraitsProb = 0.0 + multineat_params.MutateLinkTraitsProb = 0.0 + + multineat_params.AllowLoops = False + + return multineat_params + + +@dataclass +class Config: + innov_db: multineat.InnovationDatabase + rng: multineat.RNG + # when two parents have the same connection, + # take the average of the two instead of picking one + mate_average: bool + # if you don't know what this is, make it True + interspecies_crossover: bool + multineat_params: multineat.Parameters diff --git a/pyrevolve/genotype/cppnneat/crossover.py b/pyrevolve/genotype/cppnneat/crossover.py new file mode 100644 index 0000000000..54b60a9b7d --- /dev/null +++ b/pyrevolve/genotype/cppnneat/crossover.py @@ -0,0 +1,18 @@ +from typing import List + +from typeguard import typechecked + +from .config import Config +from .genotype import Genotype + + +@typechecked +def crossover(parents: List[Genotype], config: Config) -> Genotype: + assert len(parents) == 2 + return parents[0].mate( + parents[1], + config.multineat_params, + config.rng, + config.mate_average, + config.interspecies_crossover, + ) diff --git a/pyrevolve/genotype/cppnneat/genotype.py b/pyrevolve/genotype/cppnneat/genotype.py new file mode 100644 index 0000000000..cc761b0ca8 --- /dev/null +++ b/pyrevolve/genotype/cppnneat/genotype.py @@ -0,0 +1,98 @@ +from __future__ import annotations + +from typing import Any, Dict + +import multineat +from typeguard import typechecked + +from ..bodybrain_composition.sub_genotype import ( + SubGenotype as BodybrainCompositionSubGenotype, +) + + +class Genotype(BodybrainCompositionSubGenotype): + _multineat_genome: multineat.Genomemultineat_genome + + @typechecked + def __init__(self, multineat_genome: multineat.Genome): + self._multineat_genome = multineat_genome + + @staticmethod + @typechecked + def random( + n_inputs: int, + n_outputs: int, + multineat_params: multineat.Parameters, + output_activation_type: multineat.ActivationFunction, + n_start_mutations: int, + innov_db: multineat.InnovationDatabase, + rng: multineat.RNG, + ) -> Genotype: + multineat_genome = multineat.Genome( + 0, # ID + n_inputs, + 0, # n_hidden + n_outputs, + False, # FS_NEAT + output_activation_type, + multineat.ActivationFunction.TANH, # hidden activation type + 0, # seed_type + multineat_params, + 0, # number of hidden layers + ) + + genome = Genotype(multineat_genome) + for _ in range(n_start_mutations): + genome.mutate(multineat_params, innov_db, rng) + + return genome + + @property + @typechecked + def multineat_genome(self) -> multineat.Genome: + return self._multineat_genome + + @typechecked + def mutate( + self, + multineat_params: multineat.Parameters, + innov_db: multineat.InnovationDatabase, + rng: multineat.RNG, + ) -> None: + self.multineat_genome.Mutate( + False, + multineat.SearchMode.COMPLEXIFYING, + innov_db, + multineat_params, + rng, + ) + + @typechecked + def mate( + self, + partner: Genotype, + multineat_params: multineat.Parameters, + rng: multineat.RNG, + mate_average: bool, + interspecies_crossover: bool, + ) -> Genotype: + child_multineat_genome = self.multineat_genome.Mate( + partner.multineat_genome, + mate_average, + interspecies_crossover, + rng, + multineat_params, + ) + return Genotype(child_multineat_genome) + + @typechecked + def clone(self) -> Genotype: + return Genotype(multineat.Genome(self.multineat_genome)) + + @typechecked + def serialize_to_dict(self) -> Dict[str, Any]: + return {"multineat_genome": self._multineat_genome.Serialize()} + + @typechecked + def deserialize_from_dict(self, serialized: Dict[str, Any]): + self._multineat_genome.Deserialize(serialized["multineat_genome"]) diff --git a/pyrevolve/genotype/cppnneat/mutation.py b/pyrevolve/genotype/cppnneat/mutation.py new file mode 100644 index 0000000000..a621b22f7c --- /dev/null +++ b/pyrevolve/genotype/cppnneat/mutation.py @@ -0,0 +1,11 @@ +from typeguard import typechecked + +from .config import Config +from .genotype import Genotype + + +@typechecked +def mutate(genotype: Genotype, config: Config) -> Genotype: + copy = genotype.clone() + copy.mutate(config.multineat_params, config.innov_db, config.rng) + return copy diff --git a/pyrevolve/genotype/direct_tree/__init__.py b/pyrevolve/genotype/direct_tree/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/pyrevolve/genotype/direct_tree/compound_mutation.py b/pyrevolve/genotype/direct_tree/compound_mutation.py new file mode 100644 index 0000000000..8c2bda2bf1 --- /dev/null +++ b/pyrevolve/genotype/direct_tree/compound_mutation.py @@ -0,0 +1,33 @@ +from typing import Callable + +from pyrevolve.genotype.direct_tree.direct_tree_config import DirectTreeMutationConfig +from pyrevolve.genotype.direct_tree.direct_tree_genotype import DirectTreeGenotype +from pyrevolve.genotype.direct_tree.direct_tree_neat_genotype import DirectTreeNEATGenotype +from pyrevolve.genotype.neat_brain_genome import NeatBrainGenomeConfig, NeatBrainGenome + + +class DirectTreeNEATMutationConfig: + def __init__(self, + tree_mutation_conf: DirectTreeMutationConfig, + neat_conf: NeatBrainGenomeConfig): + self.tree_mutation = tree_mutation_conf + self.neat = neat_conf + + +def composite_mutation(genotype: DirectTreeNEATGenotype, + body_mutation: Callable[[DirectTreeGenotype], DirectTreeGenotype], + brain_mutation: Callable[[NeatBrainGenome], NeatBrainGenome]) -> DirectTreeNEATGenotype: + new_genome = genotype.clone() + new_genome._body_genome = body_mutation(new_genome._body_genome) + for i in range(len(new_genome._brain_genomes)): + new_genome._brain_genomes[i] = brain_mutation(new_genome._brain_genomes[i]) + return new_genome + + +def standard_mutation(genotype: DirectTreeNEATGenotype, + mutation_conf: DirectTreeNEATMutationConfig) -> DirectTreeNEATGenotype: + return composite_mutation( + genotype, + lambda g: direct_tree_mutation(g, mutation_conf.tree_mutation), + lambda g: neat_mutation(g, mutation_conf.neat), + ) \ No newline at end of file diff --git a/pyrevolve/genotype/direct_tree/direct_tree_config.py b/pyrevolve/genotype/direct_tree/direct_tree_config.py new file mode 100644 index 0000000000..a60a502ed3 --- /dev/null +++ b/pyrevolve/genotype/direct_tree/direct_tree_config.py @@ -0,0 +1,85 @@ + +class DirectTreeGenotypeConfig(object): + def __init__(self, + max_parts: int = 50, + min_parts: int = 10, + max_oscillation: float = 5, + init_n_parts_mu: float = 10, + init_n_parts_sigma: float = 4, + init_prob_no_child: float = 0.1, + init_prob_child_active_joint: float = 0.4, + init_prob_child_block: float = 0.5, + mutation_p_duplicate_subtree: float = 0.05, + mutation_p_delete_subtree: float = 0.05, + mutation_p_generate_subtree: float = 0.05, + mutation_p_swap_subtree: float = 0.05, + mutation_p_mutate_oscillators: float = 0.05, + mutation_p_mutate_oscillator: float = 0.05, + mutate_oscillator_amplitude_sigma: float = 0.1, + mutate_oscillator_period_sigma: float = 0.1, + mutate_oscillator_phase_sigma: float = 0.1, + ): + self.max_parts: int = max_parts + self.min_parts: int = min_parts + self.max_oscillation: float = max_oscillation + + self.init: RandomGenerateConfig = RandomGenerateConfig( + n_parts_mu=init_n_parts_mu, + n_parts_sigma=init_n_parts_sigma, + prob_no_child=init_prob_no_child, + prob_child_active_joint=init_prob_child_active_joint, + prob_child_block=init_prob_child_block, + ) + + self.mutation: DirectTreeMutationConfig = DirectTreeMutationConfig( + p_duplicate_subtree=mutation_p_duplicate_subtree, + p_delete_subtree=mutation_p_delete_subtree, + p_generate_subtree=mutation_p_generate_subtree, + p_swap_subtree=mutation_p_swap_subtree, + p_mutate_oscillators=mutation_p_mutate_oscillators, + p_mutate_oscillator=mutation_p_mutate_oscillator, + mutate_oscillator_amplitude_sigma=mutate_oscillator_amplitude_sigma, + mutate_oscillator_period_sigma=mutate_oscillator_period_sigma, + mutate_oscillator_phase_sigma=mutate_oscillator_phase_sigma, + ) + + +class RandomGenerateConfig: + def __init__(self, + n_parts_mu: float, + n_parts_sigma: float, + prob_no_child: float, + prob_child_active_joint: float, + prob_child_block: float, + ): + self.n_parts_mu: float = n_parts_mu + self.n_parts_sigma: float = n_parts_sigma + self.prob_no_child: float = prob_no_child + self.prob_child_active_joint: float = prob_child_active_joint + self.prob_child_block: float = prob_child_block + + +class DirectTreeMutationConfig: + def __init__(self, + p_delete_subtree, + p_generate_subtree, + p_swap_subtree, + p_duplicate_subtree, + p_mutate_oscillators, + p_mutate_oscillator, + mutate_oscillator_amplitude_sigma, + mutate_oscillator_period_sigma, + mutate_oscillator_phase_sigma, + ): + self.p_generate_subtree: float = p_generate_subtree + self.p_duplicate_subtree: float = p_duplicate_subtree + self.p_delete_subtree: float = p_delete_subtree + self.p_swap_subtree: float = p_swap_subtree + # global probability if to mutate oscillators at all + self.p_mutate_oscillators: float = p_mutate_oscillators + # probability tested for each single oscillator + self.p_mutate_oscillator: float = p_mutate_oscillator + # how much variance to mutate each of the oscillator parameters + self.mutate_oscillator_amplitude_sigma: float = mutate_oscillator_amplitude_sigma + self.mutate_oscillator_period_sigma: float = mutate_oscillator_period_sigma + self.mutate_oscillator_phase_sigma: float = mutate_oscillator_phase_sigma diff --git a/pyrevolve/genotype/direct_tree/direct_tree_crossover.py b/pyrevolve/genotype/direct_tree/direct_tree_crossover.py new file mode 100644 index 0000000000..33b20ac4f1 --- /dev/null +++ b/pyrevolve/genotype/direct_tree/direct_tree_crossover.py @@ -0,0 +1,85 @@ +import random +import sys +from typing import List, Tuple + +from pyrevolve.genotype.direct_tree.direct_tree_config import DirectTreeGenotypeConfig +from pyrevolve.genotype.direct_tree.direct_tree_utils import recursive_iterate_modules, subtree_size, duplicate_subtree +from pyrevolve.genotype.direct_tree.direct_tree_genotype import DirectTreeGenotype +from pyrevolve.revolve_bot import RevolveModule +from pyrevolve.revolve_bot.revolve_module import Orientation, CoreModule + + +def crossover_list(parents: List[DirectTreeGenotype], conf: DirectTreeGenotypeConfig): + assert len(parents) == 2 + return crossover(parents[0], parents[1], conf, None) + + +def crossover(parent_a: DirectTreeGenotype, + parent_b: DirectTreeGenotype, + conf: DirectTreeGenotypeConfig, + new_id: int = -1) \ + -> DirectTreeGenotype: + """ + Performs actual crossover between two robot trees, parent_a and parent_b. This + works as follows: + - Robot `a` is copied + - A random node `q` is picked from this copied robot. This may + be anything but the root node. + - We generate a list of nodes from robot b which, when replacing `q`, + would not violate the robot's specifications. If this list is empty, + crossover is not performed. + - We pick a random node `r` from this list + :return: New genotype + """ + parent_a_size = subtree_size(parent_a.representation) + genotype_child = parent_a.clone() + empty_slot_list_a: List[Tuple[RevolveModule, int, RevolveModule]] = [] # parent, slot, child + for _, _, module, _ in recursive_iterate_modules(genotype_child.representation): + for slot, child in module.iter_children(): + # allow back connection only for core block, not others + _slot = Orientation(slot) + if _slot is Orientation.BACK and not isinstance(child, CoreModule): + continue + empty_slot_list_a.append((module, slot, child)) + + module_list_b: List[Tuple[RevolveModule, int]] = [] + for module_parent, _, module, _ in recursive_iterate_modules(parent_b.representation): + if module_parent is None: + continue + module_size = subtree_size(module) + module_list_b.append((module, module_size)) + + crossover_point_found = False + n_tries = 100 + while not crossover_point_found and n_tries > 0: + n_tries -= 1 + module_parent_a, module_parent_a_slot, module_a = random.choice(empty_slot_list_a) + module_a_size = subtree_size(module_a) + + def compatible(module_b: RevolveModule, module_b_size: int) -> bool: + new_size = parent_a_size - module_a_size + module_b_size + return conf.min_parts <= new_size <= conf.max_parts + + unrelated_module_list = [e for e in module_list_b if compatible(*e)] + if not unrelated_module_list: + continue + + module_b, _ = random.choice(unrelated_module_list) + + module_parent_a.children[module_parent_a_slot] = duplicate_subtree(module_b) + crossover_point_found = True + + if not crossover_point_found: + print(f'Crossover between genomes {parent_a.id} and {parent_b.id} was not successful after 100 trials,' + f' returning a clone of {parent_a.id} unchanged', file=sys.stderr) + else: + print(f'Removing {module_a_size} modules and adding {subtree_size(module_b)} modules') + + genotype_child.id = new_id + + module_ids = set() + for _, _, module, _ in recursive_iterate_modules(genotype_child.representation, include_none_child=False): + assert module.id not in module_ids + module_ids.add(module.id) + + return genotype_child diff --git a/pyrevolve/genotype/direct_tree/direct_tree_genotype.py b/pyrevolve/genotype/direct_tree/direct_tree_genotype.py new file mode 100644 index 0000000000..6c8f834b9c --- /dev/null +++ b/pyrevolve/genotype/direct_tree/direct_tree_genotype.py @@ -0,0 +1,135 @@ +from __future__ import annotations +from typing import Optional + +from pyrevolve.genotype.direct_tree.direct_tree_utils import duplicate_subtree +from pyrevolve.genotype import Genotype +from pyrevolve.genotype.direct_tree.direct_tree_config import DirectTreeGenotypeConfig + +from pyrevolve.genotype.direct_tree import direct_tree_random_generator +from pyrevolve.revolve_bot import RevolveBot +from pyrevolve.revolve_bot.revolve_module import ActiveHingeModule, RevolveModule +from pyrevolve.revolve_bot.brain import BrainNN, brain_nn +from pyrevolve.revolve_bot.revolve_module import CoreModule + + +class DirectTreeGenotype(Genotype): + + def __init__(self, conf: DirectTreeGenotypeConfig, robot_id: Optional[int], random_init=True): + """ + :param conf: configurations for l-system + :param robot_id: unique id of the robot + :type conf: PlasticodingConfig + """ + self.conf: DirectTreeGenotypeConfig = conf + assert robot_id is None or str(robot_id).isdigit() + self.id: int = int(robot_id) if robot_id is not None else -1 + + if random_init: + assert robot_id is not None + assert conf is not None + self.representation: CoreModule = CoreModule() + self.random_initialization() + else: + self.representation = None + + # Auxiliary variables + self.valid: bool = False + + self.phenotype: Optional[RevolveBot] = None + + def clone(self): + # Cannot use deep clone for this genome, because it is bugged sometimes + _id = self.id if self.id >= 0 else None + + other = DirectTreeGenotype(self.conf, _id, random_init=False) + other.valid = self.valid + other.representation = duplicate_subtree(self.representation) + + other.phenotype = None + return other + + def load_genotype(self, genotype_filename: str) -> None: + revolvebot: RevolveBot = RevolveBot() + revolvebot.load_file(genotype_filename, conf_type='yaml') + self.id = revolvebot.id + self.representation = revolvebot._body + + # load brain params into the modules + brain = revolvebot._brain + assert isinstance(brain, BrainNN) + + module_map = {} + for module in revolvebot.iter_all_elements(): + assert module.id not in module_map + module_map[module.id] = module + + for node_id, oscillator in brain.params.items(): + node = brain.nodes[node_id] + module_map[node.part_id].oscillator_amplitude = oscillator.amplitude + module_map[node.part_id].oscillator_period = oscillator.period + module_map[node.part_id].oscillator_phase = oscillator.phase_offset + + for module in revolvebot.iter_all_elements(): + assert module_map[module.id] == module + + return + + def export_genotype(self, filepath: str) -> None: + self.develop() + self.phenotype.save_file(filepath, conf_type='yaml') + + def develop(self) -> RevolveBot: + if self.phenotype is None: + self.phenotype: RevolveBot = RevolveBot(self.id) + self.phenotype._body: CoreModule = self.representation + self.phenotype._brain = self._develop_brain(self.representation) + return self.phenotype + + def _develop_brain(self, core: CoreModule): + brain = BrainNN() + + for module in self.phenotype.iter_all_elements(): + if isinstance(module, ActiveHingeModule): + node = brain_nn.Node() + node.id = f'node_{module.id}' + node.part_id = module.id + + node.layer = 'output' + node.type = 'Oscillator' + + params = brain_nn.Params() + params.period = module.oscillator_period + params.phase_offset = module.oscillator_phase + params.amplitude = module.oscillator_amplitude + node.params = params + + brain.params[node.id] = params + brain.nodes[node.id] = node + + # add imu sensor stuff or the brain fail to load + for p in range(1, 7): + _id = 'IMU_' + str(p) + node = brain_nn.Node() + node.layer = 'input' + node.type = 'Input' + node.part_id = core.id + node.id = _id + brain.nodes[_id] = node + + return brain + + def random_initialization(self): + self.representation = direct_tree_random_generator.generate_tree(self.representation, + max_parts=self.conf.max_parts, + n_parts_mu=self.conf.init.n_parts_mu, + n_parts_sigma=self.conf.init.n_parts_sigma, + config=self.conf) + return self + + def mutate(self): + from pyrevolve.genotype.direct_tree.direct_tree_mutation import mutate + return mutate(self, self.conf, in_place=False) + + def crossover(self, other: DirectTreeGenotype, new_id: int): + from pyrevolve.genotype.direct_tree.direct_tree_crossover import crossover + return crossover(self, other, self.conf, new_id) diff --git a/pyrevolve/genotype/direct_tree/direct_tree_mutation.py b/pyrevolve/genotype/direct_tree/direct_tree_mutation.py new file mode 100644 index 0000000000..701dc71da6 --- /dev/null +++ b/pyrevolve/genotype/direct_tree/direct_tree_mutation.py @@ -0,0 +1,278 @@ +import math +import random +from typing import Callable, List, Any, Optional, Iterable, Tuple, Union, Type + +from pyrevolve.genotype.direct_tree.direct_tree_random_generator import generate_new_module +from pyrevolve.genotype.direct_tree.direct_tree_utils import subtree_size, recursive_iterate_modules, duplicate_subtree +from pyrevolve.genotype.direct_tree.compound_mutation import DirectTreeNEATMutationConfig +from pyrevolve.genotype.direct_tree.direct_tree_config import DirectTreeMutationConfig, DirectTreeGenotypeConfig +from pyrevolve.genotype.direct_tree.direct_tree_neat_genotype import DirectTreeNEATGenotype +from pyrevolve.util import decide +from pyrevolve.genotype.direct_tree.direct_tree_genotype import DirectTreeGenotype +from pyrevolve.revolve_bot import RevolveBot +from pyrevolve.revolve_bot.revolve_module import CoreModule, RevolveModule, Orientation, ActiveHingeModule, BrickModule + + +def mutate(genotype: DirectTreeGenotype, + genotype_conf: DirectTreeGenotypeConfig, + in_place=False): + """ + Mutates the robot tree. This performs the following operations: + - Body parameters are mutated + - Brain parameters are mutated + - A subtree might be removed + - A subtree might be duplicated + - Two subtrees might be swapped + - Subtrees are duplicated at random + - Body parts are added at random + :param genotype: + :param genotype_conf: + :param in_place: + :return: mutated version of the genome + """ + if not in_place: + genotype = genotype.clone() + tree: CoreModule = genotype.representation + + revolvebot = RevolveBot(genotype.id) + revolvebot._body = tree + + # TODO change colors + + # delete_random_subtree + if decide(genotype_conf.mutation.p_delete_subtree): + r, n = delete_random_subtree(tree, genotype_conf) + if r is not None: + print(f"DELETED {n} ELEMENTS") + + # generate random new module + if decide(genotype_conf.mutation.p_generate_subtree): + r = generate_random_new_module(tree, genotype_conf) + if r is not None: + print(f"GENERATED NEW SUBTREE of size {subtree_size(r)}") + + # TODO random rotate modules + + # duplicate random subtree + if decide(genotype_conf.mutation.p_duplicate_subtree): + if duplicate_random_subtree(tree, genotype_conf): + print("DUPLICATED") + + # swap random subtree + if decide(genotype_conf.mutation.p_swap_subtree): + if swap_random_subtree(tree): + print("SWAPPED") + + # mutate oscillators + if decide(genotype_conf.mutation.p_mutate_oscillators): + mutate_oscillators(tree, genotype_conf) + + module_ids = set() + for _, _, module, _ in recursive_iterate_modules(genotype.representation, include_none_child=False): + assert module.id not in module_ids + module_ids.add(module.id) + + return genotype + + +def delete_random_subtree(root: RevolveModule, + genotype_conf: DirectTreeGenotypeConfig) \ + -> (Optional[RevolveModule], int): + """ + Deletes a subtree at random, assuming this is possible within + the boundaries of the robot specification. + :param root: Root node of the tree + :param genotype_conf: + :return: The removed subtree (or None if no subtree was removed) and the size of the subtree (the amount of modules removed) + """ + robot_size = subtree_size(root) + max_remove_list = robot_size - genotype_conf.min_parts + + module_list = [] + for parent, parent_slot, module, depth in recursive_iterate_modules(root): + if parent is None: + continue + _subtree_size = subtree_size(module) + # This line of code above it's slow, because it's recalculated for each subtree. + # But I don't care at the moment. You can speed it up if you want. + if _subtree_size > max_remove_list: + continue + module_list.append((parent, module, _subtree_size)) + + if not module_list: + return None, 0 + + parent, subtree_root, _subtree_size = random.choice(module_list) + for i, module in parent.iter_children(): + if module == subtree_root: + parent.children[i] = None + break + else: + # break was not reached, module not found about children + raise RuntimeError("Subtree not found in the parent module!") + return subtree_root, _subtree_size + + +def generate_random_new_module(root: RevolveModule, + genotype_conf: DirectTreeGenotypeConfig) \ + -> Optional[RevolveModule]: + """ + Generates a new random module at a random position. It fails if the robot is already too big. + Could generate an invalid robot. + :param root: root of the robot tree + :param genotype_conf: genotype configuration + :return: reference to the new module, None if no insertion was possible + """ + robotsize: int = subtree_size(root) + if genotype_conf.max_parts == robotsize: + return None + + empty_slot_list: List[Tuple[RevolveModule, int]] = [] + for parent, parent_slot, module, depth in recursive_iterate_modules(root): + # Create empty slot list + for slot, child in module.iter_children(): + # allow back connection only for core block, not others + _slot = Orientation(slot) + if _slot is Orientation.BACK and not isinstance(child, CoreModule): + continue + if child is None: + empty_slot_list.append((module, slot)) + + if not empty_slot_list: + return None + + # choose random empty slot to where the duplication is created + target_parent, target_empty_slot = random.choice(empty_slot_list) # type: (RevolveModule, int) + + possible_children_probs: List[float] = [ + 0, + genotype_conf.init.prob_child_block, + genotype_conf.init.prob_child_active_joint, + ] + + new_module = generate_new_module(target_parent, target_empty_slot, possible_children_probs, genotype_conf) + + if new_module is None: + # randomly chose to close this slot, not enabled + return None + + target_parent.children[target_empty_slot] = new_module + + +def duplicate_random_subtree(root: RevolveModule, conf: DirectTreeGenotypeConfig) -> bool: + """ + Picks a random subtree that can be duplicated within the robot + boundaries, copies it and attaches it to a random free slot. + :param root: root of the robot tree + :param conf: direct tree genotype configuration + :return: True if duplication happened + """ + robotsize = subtree_size(root) + max_add_size = conf.max_parts - robotsize + + # Create a list of subtrees that is not larger than max_add_size + module_list: List[Tuple[RevolveModule, RevolveModule, int]] = [] + empty_slot_list: List[Tuple[RevolveModule, int]] = [] + for parent, parent_slot, module, depth in recursive_iterate_modules(root): + # Create empty slot list + for slot, child in module.iter_children(): + # allow back connection only for core block, not others + _slot = Orientation(slot) + if _slot is Orientation.BACK and not isinstance(child, CoreModule): + continue + if child is None: + empty_slot_list.append((module, slot)) + + if parent is None: + continue + + _subtree_size = subtree_size(module) + # This line of code above it's slow, because it's recalculated for each subtree. + # But I don't care at the moment. You can speed it up if you want. + if _subtree_size > max_add_size: + continue + # Create possible source subtree list + module_list.append((parent, module, _subtree_size)) + + if not module_list: + return False + if not empty_slot_list: + return False + + # choose random tree to duplicate + parent, subtree_root, _subtree_size = random.choice(module_list) + # choose random empty slot to where the duplication is created + target_parent, target_empty_slot = random.choice(empty_slot_list) + + # deep copy the source subtree + subtree_root = duplicate_subtree(subtree_root) + # and attach it + target_parent.children[target_empty_slot] = subtree_root + + return True + + +def swap_random_subtree(root: RevolveModule) -> bool: + """ + Picks to random subtrees (which are not parents / children of each + other) and swaps them. + :param root: root of the robot tree + :return: True if swapping happened + """ + module_list: List[Tuple[RevolveModule, int, RevolveModule]] = [] + for parent, parent_slot, module, depth in recursive_iterate_modules(root, include_none_child=True): + if parent is None: + continue + module_list.append((parent, parent_slot, module)) + + parent_a, parent_a_slot, a = random.choice(module_list) + a_module_set = set() + for _, _, module, _ in recursive_iterate_modules(a): + a_module_set.add(module) + + unrelated_module_list = [e for e in module_list if e[2] not in a_module_set] + if not unrelated_module_list: + return False + + parent_b, parent_b_slot, b = random.choice(unrelated_module_list) + + parent_b.children[parent_b_slot] = a + parent_a.children[parent_a_slot] = b + + return True + + +def mutate_oscillators(root: RevolveModule, conf: DirectTreeGenotypeConfig) -> None: + """ + Mutates oscillation + :param root: root of the robot tree + :param conf: genotype config for mutation probabilities + """ + + for _, _, module, _ in recursive_iterate_modules(root): + if isinstance(module, ActiveHingeModule): + if decide(conf.mutation.p_mutate_oscillator): + module.oscillator_amplitude += random.gauss(0, conf.mutation.mutate_oscillator_amplitude_sigma) + module.oscillator_period += random.gauss(0, conf.mutation.mutate_oscillator_period_sigma) + module.oscillator_phase += random.gauss(0, conf.mutation.mutate_oscillator_phase_sigma) + + # amplitude is clamped between 0 and 1 + module.oscillator_amplitude = clamp(module.oscillator_amplitude, 0, 1) + # phase and period are periodically repeating every max_oscillation, + # so we bound the value between [0,conf.max_oscillation] for convenience + module.oscillator_phase = module.oscillator_phase % conf.max_oscillation + module.oscillator_period = module.oscillator_period % conf.max_oscillation + + +def clamp(value: Union[float, int], + minvalue: Union[float, int], + maxvalue: Union[float, int]) \ + -> Union[float, int]: + """ + Clamps the value to a minimum and maximum + :param value: source value + :param minvalue: min possible value + :param maxvalue: max possible value + :return: clamped value + """ + return min(max(minvalue, value), maxvalue) diff --git a/pyrevolve/genotype/direct_tree/direct_tree_neat_genotype.py b/pyrevolve/genotype/direct_tree/direct_tree_neat_genotype.py new file mode 100644 index 0000000000..df337d1ee1 --- /dev/null +++ b/pyrevolve/genotype/direct_tree/direct_tree_neat_genotype.py @@ -0,0 +1,121 @@ +from __future__ import annotations + +from .direct_tree_genotype import DirectTreeGenotype +from .. import Genotype + +from typing import TYPE_CHECKING + +from ..neat_brain_genome import NeatBrainGenome, NeatBrainGenomeConfig +from ...angle.robogen import Config + +if TYPE_CHECKING: + from typing import Optional, List, Union + from pyrevolve.revolve_bot import RevolveBot + + +class DirectTreeNEATGenotypeConfig: + def __init__(self, + tree_conf: Config, + cppn_conf: NeatBrainGenomeConfig, + number_of_brains: int = 1): + assert number_of_brains > 0 + self.body: Config = tree_conf + self.brain: NeatBrainGenomeConfig = cppn_conf + self.number_of_brains: int = number_of_brains + + +class DirectTreeNEATGenotype(Genotype): + def __init__(self, conf: Optional[DirectTreeNEATGenotypeConfig] = None, robot_id: Optional[int] = None): + + self._id: int = robot_id + + self._brain_genomes = [] + if conf is None: + self._body_genome = None + else: + assert robot_id is not None + self._body_genome: DirectTreeGenotype = DirectTreeGenotype(conf.body, robot_id).random_initialization() + for _ in range(conf.number_of_brains): + self._brain_genomes.append(NeatBrainGenome(conf.brain, robot_id)) + + @property + def id(self) -> int: + return self._id + + def is_brain_compatible(self, + other: DirectTreeNEATGenotype, + conf: DirectTreeNEATGenotypeConfig) -> bool: + """ + Test if all brains are compatible + :param other: other genome to test against + :param conf: Genome Configuration object + :return: true if brains are compatible + """ + if not isinstance(other, DirectTreeNEATGenotype): + return False + if len(self._brain_genomes) != len(other._brain_genomes): + return False + for self_brain, other_brain in zip(self._brain_genomes, other._brain_genomes): + if not self_brain.is_compatible(other_brain, conf.brain): + return False + + return True + + @id.setter + def id(self, value) -> None: + self._id = value + self._body_genome.id = value + for brain_genome in self._brain_genomes: + # WARNING! multiple genomes with the same id? + brain_genome.id = value + + def export_genotype(self, file_path: str) -> None: + """ + Connects to plasticoding expor_genotype function + :param file_path: file to save the genotype file to + """ + with open(file_path, 'w+') as f: + # the first element is the number of brain genomes + f.write(f'{len(self._brain_genomes)}\n') + # write the body genome + self._body_genome._export_genotype_open_file(f) + # write the brain genomes + for brain_genome in self._brain_genomes: + brain_genome._export_genotype_open_file(f) + + def load_genotype(self, file_path: str) -> None: + with open(file_path) as f: + lines = f.readlines() + # remove first element - it's the number of brain genomes + number_of_brain_genomes = int(lines.pop(0)) + # read the body genome + self._body_genome._load_genotype_from(lines[:-number_of_brain_genomes]) + # read the brain genomes + for brain_i in range(number_of_brain_genomes): + i = -number_of_brain_genomes + brain_i + self._brain_genomes[brain_i]._load_genotype_from(lines[i].strip()) + + def clone(self) -> DirectTreeNEATGenotype: + clone = DirectTreeNEATGenotype() + clone._body_genome = self._body_genome.clone() + clone._brain_genomes = [] + for brain_genome in self._brain_genomes: + clone._brain_genomes.append(brain_genome.clone()) + return clone + + def develop(self) -> Union[RevolveBot, List[RevolveBot]]: + """ + Develops the genome into a (series of) revolve_bot (proto-phenotype) + :return: one/many RevolveBot instance(s) + """ + phenotypes = [] + for i, brain_genome in enumerate(self._brain_genomes): + phenotype: RevolveBot = self._body_genome.develop() + phenotype._id += i * 10000000 + phenotype._brain = brain_genome.develop() + phenotypes.append(phenotype) + + if len(phenotypes) == 1: + return phenotypes[0] + else: + return phenotypes diff --git a/pyrevolve/genotype/direct_tree/direct_tree_random_generator.py b/pyrevolve/genotype/direct_tree/direct_tree_random_generator.py new file mode 100644 index 0000000000..ded97368b7 --- /dev/null +++ b/pyrevolve/genotype/direct_tree/direct_tree_random_generator.py @@ -0,0 +1,157 @@ +import random +import math +import queue +import uuid +from typing import Tuple, List, Optional, Type + +from pyrevolve.genotype.direct_tree.direct_tree_utils import recursive_iterate_modules +from pyrevolve.genotype.direct_tree.direct_tree_config import DirectTreeGenotypeConfig +from pyrevolve.revolve_bot import RevolveBot +from pyrevolve.revolve_bot.revolve_module import RevolveModule, CoreModule, BrickModule, ActiveHingeModule, Orientation + + +possible_orientation: List[float] = [0, 90, 180, 270] + +module_short_name_conversion = { + BrickModule: 'B', + ActiveHingeModule: 'J', + CoreModule: 'C', +} + +possible_children: List[Optional[Type[RevolveModule]]] = [ + None, + BrickModule, + ActiveHingeModule +] + + +def generate_tree(core: CoreModule, + max_parts: int, + n_parts_mu: float, + n_parts_sigma: float, + config: DirectTreeGenotypeConfig) \ + -> CoreModule: + """ + Generate + :param core: Core module of the tree + :param max_parts: max number of blocks to generate + :param n_parts_mu: target size of the tree (gauss mu) + :param n_parts_sigma: variation of the target size of the tree (gauss sigma) + :param config: genotype configuration + :return: the updated core + """ + robot = RevolveBot() + robot._body = core + + count_parts = 1 + _max_parts: int = math.floor( + random.gauss(n_parts_mu, n_parts_sigma) + 0.5 + ) + max_parts = min(max_parts, _max_parts) + + + core_color: Tuple[float, float, float] = ( + random.uniform(0, 1), + random.uniform(0, 1), + random.uniform(0, 1) + ) + + core.id = 'C' + core.rgb = core_color + + # Breadth First Search + # difference by discovered and not labeled is done by doing the update_substrate. + # it's a slow solution, but it reuses code that we already now as working. + + slot_queue = queue.Queue() # infinite FIFO queue + + def append_new_empty_slots(module: RevolveModule): + for slot_i, _child in module.iter_children(): + # slot is (i, null_ref) + slot_queue.put((module, slot_i)) + + append_new_empty_slots(core) + + possible_children_probs: List[float] = [ + config.init.prob_no_child, + config.init.prob_child_block, + config.init.prob_child_active_joint, + ] + + while count_parts <= max_parts and not slot_queue.empty(): + # position and reference + parent, chosen_slot \ + = slot_queue.get_nowait() # type: (RevolveModule, int) + + new_child_module: RevolveModule = generate_new_module(parent, chosen_slot, possible_children_probs, config) + if new_child_module is None: + continue + + try: + # add it to parent + parent.children[chosen_slot] = new_child_module + robot.update_substrate(raise_for_intersections=True) + except RevolveBot.ItersectionCollisionException: + # could not add module, the slot was already occupied + # (meaning this position was already occupied by a previous node) + parent.children[chosen_slot] = None + continue + + # add new module's slots to the queue + append_new_empty_slots(new_child_module) + count_parts += 1 + + module_ids = set() + for _, _, module, _ in recursive_iterate_modules(core, include_none_child=False): + assert module.id not in module_ids + module_ids.add(module.id) + + return core + + +def generate_new_module(parent: RevolveModule, + parent_free_slot: int, + possible_children_probs: List[float], + config: DirectTreeGenotypeConfig) \ + -> Optional[RevolveModule]: + """ + Generates new random block + :param parent: only for new module id + :param parent_free_slot: only for new module id + :param possible_children_probs: probabilites on how to select the new module, list of 3 floats. + :param config: genotype configuration + :return: returns new module, or None if the probability to not select any new module was extracted + """ + module_color: Tuple[float, float, float] = ( + random.uniform(0, 1), + random.uniform(0, 1), + random.uniform(0, 1) + ) + + new_child_module_class = random.choices(possible_children, + weights=possible_children_probs, + k=1)[0] + if new_child_module_class is None: + # randomly chose to close this slot + return None + + new_child_module: RevolveModule = new_child_module_class() + + # random rotation + new_child_module.orientation = random.choice(possible_orientation) + + # generate module ID + short_mod_type = module_short_name_conversion[new_child_module_class] + # new_child_module.id = f'{parent.id}{Orientation(parent_free_slot).short_repr()}_{short_mod_type}' + new_child_module.id = str(uuid.uuid1()) + new_child_module.rgb = module_color + + # if is active, add oscillator parameters + if isinstance(new_child_module, ActiveHingeModule): + new_child_module.oscillator_period = random.uniform(0, config.max_oscillation) + # makes no sense to shift it more than the max oscillation period + new_child_module.oscillator_phase = random.uniform(0, config.max_oscillation) + # output is capped between (0,1) excluded + new_child_module.oscillator_amplitude = random.uniform(0, 1) + + return new_child_module diff --git a/pyrevolve/genotype/direct_tree/direct_tree_utils.py b/pyrevolve/genotype/direct_tree/direct_tree_utils.py new file mode 100644 index 0000000000..fa5bfdfa12 --- /dev/null +++ b/pyrevolve/genotype/direct_tree/direct_tree_utils.py @@ -0,0 +1,75 @@ +import copy +import uuid +from collections import deque +from typing import Optional, Iterable, Tuple + +from pyrevolve.revolve_bot import RevolveModule + + +def recursive_iterate_modules(module: RevolveModule, + parent: Optional[RevolveModule] = None, + parent_slot: Optional[int] = None, + depth: int = 1, + include_none_child: bool = False) \ + -> Iterable[Tuple[Optional[RevolveModule], Optional[int], RevolveModule, int]]: + """ + Iterate all modules, depth search first, yielding parent, parent slot, module and depth, starting from root_depth=1. + Uses recursion. + :param module: starting module to expand + :param parent: for internal recursiveness, parent module. leave default + :param parent_slot: for internal recursiveness, parent module slot. leave default + :param depth: for internal recursiveness, depth of the module passed in. leave default + :param include_none_child: if to include also None modules (consider empty as leaves) + :return: iterator for all modules with (parent, parent_slot, module, depth) + """ + if module is not None: + for slot, child in module.iter_children(): + if include_none_child or child is not None: + for _next in recursive_iterate_modules(child, module, slot, depth+1): + yield _next + yield parent, parent_slot, module, depth + + +def subtree_size(module: RevolveModule) -> int: + """ + Calculates the size of the subtree starting from the module + :param module: root of the subtree + :return: how many modules the subtree has + """ + count = 0 + if module is not None: + for _ in bfs_iterate_modules(root=module): + count += 1 + return count + + +def bfs_iterate_modules(root: RevolveModule) \ + -> Iterable[Tuple[Optional[RevolveModule], RevolveModule]]: + """ + Iterates throw all modules breath first, yielding parent and current module + :param root: root tree to iterate + :return: iterator for all modules with respective parent in the form: `(Parent,Module)` + """ + assert root is not None + to_process = deque([(None, root)]) + while len(to_process) > 0: + r: (Optional[RevolveModule], RevolveModule) = to_process.popleft() + parent, elem = r + for _i, child in elem.iter_children(): + if child is not None: + to_process.append((elem, child)) + yield parent, elem + + +def duplicate_subtree(root: RevolveModule) -> RevolveModule: + """ + Creates a duplicate of the subtree given as input + :param root: root of the source subtree + :return: new duplicated subtree + """ + assert root is not None + dup_root = copy.deepcopy(root) + # generate new ids for duplicated items + for _, _, module, _ in recursive_iterate_modules(dup_root, include_none_child=False): + module.id = str(uuid.uuid1()) + return dup_root diff --git a/pyrevolve/genotype/genotype.py b/pyrevolve/genotype/genotype.py index 4317fad3f4..6ed56d3e4f 100644 --- a/pyrevolve/genotype/genotype.py +++ b/pyrevolve/genotype/genotype.py @@ -1,9 +1,12 @@ +import copy + + class Genotype: def clone(self): """ Create an returns deep copy of the genotype """ - raise NotImplementedError("Method must be implemented by genome") + return copy.deepcopy(self) def develop(self): """ @@ -14,6 +17,12 @@ def develop(self): """ raise NotImplementedError("Method must be implemented by genome") + def load_genotype(self, file_path: str): + raise NotImplementedError("Method must be implemented by genome") + + def export_genotype(self, file_path: str): + raise NotImplementedError("Method must be implemented by genome") + class GenotypeConfig: def __init__(self, diff --git a/pyrevolve/genotype/lsystem_neat/__init__.py b/pyrevolve/genotype/lsystem_neat/__init__.py new file mode 100644 index 0000000000..3c726e0af6 --- /dev/null +++ b/pyrevolve/genotype/lsystem_neat/__init__.py @@ -0,0 +1 @@ +from .lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig diff --git a/pyrevolve/genotype/lsystem_neat/crossover.py b/pyrevolve/genotype/lsystem_neat/crossover.py new file mode 100644 index 0000000000..795913f5a0 --- /dev/null +++ b/pyrevolve/genotype/lsystem_neat/crossover.py @@ -0,0 +1,48 @@ +from pyrevolve.genotype.plasticoding.plasticoding import Plasticoding, Alphabet +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype +from pyrevolve.genotype.neat_brain_genome.crossover import NEATCrossoverConf +from pyrevolve.genotype.neat_brain_genome.crossover import standard_crossover as NEATBrainCrossover +from pyrevolve.genotype.plasticoding.crossover.standard_crossover import generate_child_genotype as PlasticodingCrossover + +import random + + +class CrossoverConfig: + def __init__(self, + crossover_prob): + """ + Creates a Crossover object that sets the configuration for the crossover operator + + :param crossover_prob: crossover probability + """ + self.crossover_prob = crossover_prob + + +def standard_crossover(parents, lsystem_conf, crossover_conf): + """ + Creates an child (individual) through crossover with two parents + + :param parents: Parents type Individual + :param lsystem_conf: LSystemCPGHyperNEATGenotypeConfig type with config for NEAT and Plasticoding + :param crossover_conf: CrossoverConfig for lsystem crossover type + :return: brain and body crossover (Only body right now) + """ + assert len(parents) == 2 + + parents_body_genotype = [p.genotype._body_genome for p in parents] + parents_brain_genotypes = [pair for pair in zip(parents[0].genotype._brain_genomes, parents[1].genotype._brain_genomes)] + + child_genotype = LSystemCPGHyperNEATGenotype() + Neatconf = NEATCrossoverConf() + + new_body = PlasticodingCrossover(parents_body_genotype, lsystem_conf.plasticoding, crossover_conf) + new_brain = [] + for g1, g2 in parents_brain_genotypes: + new_brain.append(NEATBrainCrossover([g1, g2], Neatconf, crossover_conf, lsystem_conf)) + + child_genotype._body_genome = new_body + child_genotype._brain_genomes = new_brain + + return child_genotype + + diff --git a/pyrevolve/genotype/lsystem_neat/lsystem_neat_genotype.py b/pyrevolve/genotype/lsystem_neat/lsystem_neat_genotype.py new file mode 100644 index 0000000000..1146676d46 --- /dev/null +++ b/pyrevolve/genotype/lsystem_neat/lsystem_neat_genotype.py @@ -0,0 +1,121 @@ +from __future__ import annotations + +from .. import Genotype +from pyrevolve.genotype.plasticoding.plasticoding import Alphabet +from pyrevolve.genotype.plasticoding.initialization import random_initialization +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenome, NeatBrainGenomeConfig + +from typing import TYPE_CHECKING + +if TYPE_CHECKING: + from typing import Optional, List, Union + from pyrevolve.genotype.plasticoding.plasticoding import Plasticoding, PlasticodingConfig + from pyrevolve.revolve_bot import RevolveBot + + +class LSystemCPGHyperNEATGenotypeConfig: + def __init__(self, + plasticonfig_conf: PlasticodingConfig, + neat_conf: NeatBrainGenomeConfig, + number_of_brains: int = 1): + assert number_of_brains > 0 + self.plasticoding = plasticonfig_conf + self.number_of_brains: int = number_of_brains + self.neat: NeatBrainGenomeConfig = neat_conf + + +class LSystemCPGHyperNEATGenotype(Genotype): + def __init__(self, conf: Optional[LSystemCPGHyperNEATGenotypeConfig] = None, robot_id: Optional[int] = None): + + self._id: int = robot_id + + self._brain_genomes = [] + if conf is None: + self._body_genome = None + else: + assert robot_id is not None + self._body_genome: Plasticoding = random_initialization(conf.plasticoding, robot_id) + for _ in range(conf.number_of_brains): + self._brain_genomes.append(NeatBrainGenome(conf.neat, robot_id)) + + @property + def id(self) -> int: + return self._id + + def is_brain_compatible(self, + other: LSystemCPGHyperNEATGenotype, + conf: LSystemCPGHyperNEATGenotypeConfig) -> bool: + """ + Test if all brains are compatible + :param other: other genome to test against + :param conf: Genome Configuration object + :return: true if brains are compatible + """ + if not isinstance(other, LSystemCPGHyperNEATGenotype): + return False + if len(self._brain_genomes) != len(other._brain_genomes): + return False + for self_brain, other_brain in zip(self._brain_genomes, other._brain_genomes): + if not self_brain.is_compatible(other_brain, conf.neat): + return False + + return True + + @id.setter + def id(self, value) -> None: + self._id = value + self._body_genome.id = value + for brain_genome in self._brain_genomes: + # WARNING! multiple genomes with the same id? + brain_genome.id = value + + def export_genotype(self, file_path: str) -> None: + """ + Connects to plasticoding expor_genotype function + :param file_path: file to save the genotype file to + """ + with open(file_path, 'w+') as f: + # the first element is the number of brain genomes + f.write(f'{len(self._brain_genomes)}\n') + # write the body genome + self._body_genome._export_genotype_open_file(f) + # write the brain genomes + for brain_genome in self._brain_genomes: + brain_genome._export_genotype_open_file(f) + + def load_genotype(self, file_path: str) -> None: + with open(file_path) as f: + lines = f.readlines() + # remove first element - it's the number of brain genomes + number_of_brain_genomes = int(lines.pop(0)) + # read the body genome + self._body_genome._load_genotype_from(lines[:-number_of_brain_genomes]) + # read the brain genomes + for brain_i in range(number_of_brain_genomes): + i = -number_of_brain_genomes + brain_i + self._brain_genomes[brain_i]._load_genotype_from(lines[i].strip()) + + def clone(self) -> LSystemCPGHyperNEATGenotype: + clone = LSystemCPGHyperNEATGenotype() + clone._body_genome = self._body_genome.clone() + clone._brain_genomes = [] + for brain_genome in self._brain_genomes: + clone._brain_genomes.append(brain_genome.clone()) + return clone + + def develop(self) -> Union[RevolveBot, List[RevolveBot]]: + """ + Develops the genome into a (series of) revolve_bot (proto-phenotype) + :return: one/many RevolveBot instance(s) + """ + phenotypes = [] + for i,brain_genome in enumerate(self._brain_genomes): + phenotype: RevolveBot = self._body_genome.develop() + phenotype._id += i*10000000 + phenotype._brain = brain_genome.develop() + phenotypes.append(phenotype) + + if len(phenotypes) == 1: + return phenotypes[0] + else: + return phenotypes diff --git a/pyrevolve/genotype/lsystem_neat/mutation.py b/pyrevolve/genotype/lsystem_neat/mutation.py new file mode 100644 index 0000000000..4dc8139d49 --- /dev/null +++ b/pyrevolve/genotype/lsystem_neat/mutation.py @@ -0,0 +1,40 @@ +from __future__ import annotations + +from ..plasticoding.mutation.standard_mutation import standard_mutation as plasticondig_mutation +from ..neat_brain_genome.mutation import mutation_complexify as neat_mutation +from ..plasticoding.mutation.mutation import MutationConfig as PlasticodingMutationConf +from ..neat_brain_genome import NeatBrainGenomeConfig + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Callable + from ..plasticoding import Plasticoding + from . import LSystemCPGHyperNEATGenotype + from ..neat_brain_genome import NeatBrainGenome + + +class LSystemNeatMutationConf: + def __init__(self, + plasticoding_mutation_conf: PlasticodingMutationConf, + neat_conf: NeatBrainGenomeConfig): + self.plasticoding = plasticoding_mutation_conf + self.neat = neat_conf + + +def composite_mutation(genotype: LSystemCPGHyperNEATGenotype, + body_mutation: Callable[[Plasticoding], Plasticoding], + brain_mutation: Callable[[NeatBrainGenome], NeatBrainGenome]) -> LSystemCPGHyperNEATGenotype: + new_genome = genotype.clone() + new_genome._body_genome = body_mutation(new_genome._body_genome) + for i in range(len(new_genome._brain_genomes)): + new_genome._brain_genomes[i] = brain_mutation(new_genome._brain_genomes[i]) + return new_genome + + +def standard_mutation(genotype: LSystemCPGHyperNEATGenotype, + mutation_conf: LSystemNeatMutationConf) -> LSystemCPGHyperNEATGenotype: + return composite_mutation( + genotype, + lambda g: plasticondig_mutation(g, mutation_conf.plasticoding), + lambda g: neat_mutation(g, mutation_conf.neat), + ) diff --git a/pyrevolve/genotype/neat_brain_genome/__init__.py b/pyrevolve/genotype/neat_brain_genome/__init__.py new file mode 100644 index 0000000000..4e67f3b2ba --- /dev/null +++ b/pyrevolve/genotype/neat_brain_genome/__init__.py @@ -0,0 +1 @@ +from .neat_brain_genome import NeatBrainGenome, NeatBrainGenomeConfig diff --git a/pyrevolve/genotype/neat_brain_genome/crossover.py b/pyrevolve/genotype/neat_brain_genome/crossover.py new file mode 100644 index 0000000000..08acbd40a0 --- /dev/null +++ b/pyrevolve/genotype/neat_brain_genome/crossover.py @@ -0,0 +1,39 @@ +import random + +from pyrevolve.genotype.neat_brain_genome import NeatBrainGenome + + +class NEATCrossoverConf: + def __init__(self): + self.mate_average = True + self.interspecies_crossover = True + self.speciation = True + + +def standard_crossover(parents, NeatCrossoverConf, crossover_conf, lsystem_conf): + """ + Creates an child (genotype) through crossover with two parents + + :param parents: parents brain genome to be used for crossover + :param NeatCrossoverConf: NEAT genotype configuration object + :param crossover_conf: CrossoverConfig for lsystem + :return: genotype result of the crossover + """ + assert len(parents) == 2 + + crossover_attempt = random.uniform(0.0, 1.0) + if crossover_attempt > crossover_conf.crossover_prob: + new_genotype = parents[0]._neat_genome + else: + mother = parents[0]._neat_genome + father = parents[1]._neat_genome + new_genotype = mother.Mate(father, + NeatCrossoverConf.mate_average, + NeatCrossoverConf.interspecies_crossover, + lsystem_conf.neat.rng, + lsystem_conf.neat.multineat_params + ) + child_genome = NeatBrainGenome() + child_genome._brain_type = parents[0]._brain_type + child_genome._neat_genome = new_genotype + return child_genome diff --git a/pyrevolve/genotype/neat_brain_genome/mutation.py b/pyrevolve/genotype/neat_brain_genome/mutation.py new file mode 100644 index 0000000000..c3be62b335 --- /dev/null +++ b/pyrevolve/genotype/neat_brain_genome/mutation.py @@ -0,0 +1,21 @@ +import multineat + + +def _mutation(genotype, baby_is_clone: bool, search_mode: multineat.SearchMode, genotype_conf): + new_genotype = genotype.clone() + new_genotype._neat_genome.Mutate( + baby_is_clone, + search_mode, + genotype_conf.innov_db, + genotype_conf.multineat_params, + genotype_conf.rng + ) + return new_genotype + + +def mutation_complexify(genotype, genotype_conf): + return _mutation(genotype, False, multineat.SearchMode.COMPLEXIFYING, genotype_conf) + + +def mutation_simplify(genotype, genotype_conf): + return _mutation(genotype, False, multineat.SearchMode.SIMPLIFYING, genotype_conf) diff --git a/pyrevolve/genotype/neat_brain_genome/neat_brain_genome.py b/pyrevolve/genotype/neat_brain_genome/neat_brain_genome.py new file mode 100644 index 0000000000..984403ed35 --- /dev/null +++ b/pyrevolve/genotype/neat_brain_genome/neat_brain_genome.py @@ -0,0 +1,246 @@ +from __future__ import annotations + +import enum +import multineat +import re +import sys + +from pyrevolve.genotype import Genotype +from pyrevolve.revolve_bot.brain import BrainCPG, BrainCPPNCPG + + +class BrainType(enum.Enum): + NN = 0 + CPPN = 1 + CPG = 2 # HyperNEAT -> CPG + + +class NeatBrainGenomeConfig: + def __init__(self, brain_type: BrainType = BrainType.CPG, random_seed=None): + self._brain_type = brain_type + self.innov_db = multineat.InnovationDatabase() + # TODO self.innov_db.Init_with_genome(a) + self.rng = multineat.RNG() + if random_seed is None: + self.rng.TimeSeed() + else: + self.rng.Seed(random_seed) + + # normal NEAT section + self.n_inputs = 1 + self.n_outputs = 1 + + # generate multineat params object + self.multineat_params = self._generate_multineat_params(brain_type) + + # CPG parameters + self.reset_neuron_random = False + self.use_frame_of_reference = False + self.init_neuron_state = 0.707 + self.range_ub = 1.0 + self.signal_factor_all = 4.0 + self.signal_factor_mid = 2.5 + self.signal_factor_left_right = 2.5 + self.abs_output_bound = 1.0 + + @property + def brain_type(self): + return self._brain_type + + @brain_type.setter + def brain_type(self, brain_type: BrainType): + self._brain_type = brain_type + self._regenerate_multineat_params() + + def _regenerate_multineat_params(self): + self.multineat_params = self._generate_multineat_params(self._brain_type) + + @staticmethod + def _generate_multineat_params(brain_type: BrainType): + params = multineat.Parameters() + + if brain_type is BrainType.CPG: + params.MutateRemLinkProb = 0.02 + params.RecurrentProb = 0.0 + params.OverallMutationRate = 0.15 + params.MutateAddLinkProb = 0.08 + params.MutateAddNeuronProb = 0.01 + params.MutateWeightsProb = 0.90 + params.MaxWeight = 8.0 + params.WeightMutationMaxPower = 0.2 + params.WeightReplacementMaxPower = 1.0 + + params.MutateActivationAProb = 0.0 + params.ActivationAMutationMaxPower = 0.5 + params.MinActivationA = 0.05 + params.MaxActivationA = 6.0 + + params.MutateNeuronActivationTypeProb = 0.03 + + params.ActivationFunction_SignedSigmoid_Prob = 0.0 + params.ActivationFunction_UnsignedSigmoid_Prob = 0.0 + params.ActivationFunction_Tanh_Prob = 1.0 + params.ActivationFunction_TanhCubic_Prob = 0.0 + params.ActivationFunction_SignedStep_Prob = 1.0 + params.ActivationFunction_UnsignedStep_Prob = 0.0 + params.ActivationFunction_SignedGauss_Prob = 1.0 + params.ActivationFunction_UnsignedGauss_Prob = 0.0 + params.ActivationFunction_Abs_Prob = 0.0 + params.ActivationFunction_SignedSine_Prob = 1.0 + params.ActivationFunction_UnsignedSine_Prob = 0.0 + params.ActivationFunction_Linear_Prob = 1.0 + + params.MutateNeuronTraitsProb = 0.0 + params.MutateLinkTraitsProb = 0.0 + + params.AllowLoops = False + elif brain_type is BrainType.NN: + params.RecurrentProb = 0.0 + params.OverallMutationRate = 1.0 + + params.ArchiveEnforcement = False + + params.MutateWeightsProb = 0.05 + + params.WeightMutationMaxPower = 0.5 + params.WeightReplacementMaxPower = 8.0 + params.MutateWeightsSevereProb = 0.0 + params.WeightMutationRate = 0.25 + params.WeightReplacementRate = 0.9 + + params.MaxWeight = 8.0 + + params.MutateAddNeuronProb = 0.001 + params.MutateAddLinkProb = 0.3 + params.MutateRemLinkProb = 0.0 + + params.MinActivationA = 4.9 + params.MaxActivationA = 4.9 + + params.ActivationFunction_SignedSigmoid_Prob = 0.0 + params.ActivationFunction_UnsignedSigmoid_Prob = 1.0 + params.ActivationFunction_Tanh_Prob = 0.0 + params.ActivationFunction_SignedStep_Prob = 0.0 + + params.MutateNeuronTraitsProb = 0.0 + params.MutateLinkTraitsProb = 0.0 + + params.AllowLoops = True + params.AllowClones = True + + else: + raise NotImplementedError(f"{brain_type} not supported") + + return params + + def is_brain_cppn(self): + return self.brain_type is BrainType.CPPN or self.brain_type is BrainType.CPG + + +class NeatBrainGenome(Genotype): + def __init__(self, conf: NeatBrainGenomeConfig = None, robot_id=None): # Change + if conf is None: + self._brain_type = None + self._neat_genome = None + return + + # self.id = int(robot_id) + self._brain_type = conf.brain_type + is_cppn = conf.is_brain_cppn() + + if is_cppn: + + # if HyperNEAT + if conf.brain_type is BrainType.CPG: + # calculate number of inputs + n_coordinates = 4 + conf.n_inputs = n_coordinates * 2 + + # calculate number of outputs + conf.n_outputs = 1 # connection weight + # conf.n_outputs += 1 # connection to output weight - unused now + + self._neat_genome = multineat.Genome( + 0, # ID + conf.n_inputs, + 0, # n_hidden + conf.n_outputs, + False, # FS_NEAT + multineat.ActivationFunction.TANH, # output activation type + multineat.ActivationFunction.TANH, # hidden activation type + 0, # seed_type + conf.multineat_params, + 0, # number of hidden layers + ) + else: + self._neat_genome = multineat.Genome( + 0, # ID + conf.n_inputs, + 0, # n_hidden + conf.n_outputs, + False, # FS_NEAT + multineat.ActivationFunction.UNSIGNED_SIGMOID, # output activation type + multineat.ActivationFunction.UNSIGNED_SIGMOID, # hidden activation type + 0, # seed_type + conf.multineat_params, + 0, # number of hidden layers + ) + + if type(robot_id) is int: + self.id = robot_id + else: + self.id = int(re.search(r'\d+', str(robot_id))[0]) + self.phenotype = None + + def load_genotype(self, file_path: str): + with open(file_path) as f: + lines = f.readlines() + self._load_genotype_from(lines[0]) + + def _load_genotype_from(self, text): + text = text.strip() + self._neat_genome.Deserialize(text.replace('inf', str(sys.float_info.max)).strip('\n')) + + def export_genotype(self, file_path: str): + with open(file_path, 'w+') as file: + self._export_genotype_open_file(file) + + def _export_genotype_open_file(self, file): + text = self._neat_genome.Serialize() + file.write(text + '\n') + + # override + def clone(self): + clone = NeatBrainGenome() + clone._brain_type = self._brain_type # the conf must not be deep copied + clone._neat_genome = multineat.Genome(self._neat_genome) + return clone + + @property + def id(self): + return str(self._neat_genome.GetID()) + + @id.setter + def id(self, value: int): + self._neat_genome.SetID(int(value)) + + def develop(self): + if self._brain_type is BrainType.CPG: + # basically, HyperNEAT + brain = BrainCPPNCPG(self._neat_genome) + brain.reset_neuron_random = False + brain.use_frame_of_reference = False + brain.init_neuron_state = 0.707 + brain.range_ub = 1.0 + brain.signal_factor_all = 4.0 + brain.signal_factor_mid = 2.5 + brain.signal_factor_left_right = 2.5 + brain.abs_output_bound = 1.0 + else: + raise NotImplementedError(f"{self._brain_type} brain not implemented yet") + + return brain + + def is_compatible(self, other: NeatBrainGenome, conf: NeatBrainGenomeConfig) -> bool: + return isinstance(other, NeatBrainGenome) \ + and self._neat_genome.IsCompatibleWith(other._neat_genome, conf.multineat_params) diff --git a/pyrevolve/genotype/plasticoding/__init__.py b/pyrevolve/genotype/plasticoding/__init__.py index dfb6ef9691..17c5df4506 100644 --- a/pyrevolve/genotype/plasticoding/__init__.py +++ b/pyrevolve/genotype/plasticoding/__init__.py @@ -1 +1,42 @@ -from .plasticoding import Plasticoding \ No newline at end of file +from .alphabet import Alphabet +from .initialization import random_initialization +from .plasticoding import Plasticoding + + +class PlasticodingConfig: + def __init__(self, + initialization_genome=random_initialization, + e_max_groups=3, + oscillator_param_min=1, + oscillator_param_max=10, + weight_param_min=-1, + weight_param_max=1, + weight_min=-1, + weight_max=1, + axiom_w=Alphabet.CORE_COMPONENT, + i_iterations=3, + max_structural_modules=100, + robot_id=0, + allow_vertical_brick=True, + use_movement_commands=True, + use_rotation_commands=True, + use_movement_stack=True, + allow_joint_joint_attachment=True, + ): + self.allow_joint_joint_attachment = allow_joint_joint_attachment + self.initialization_genome = initialization_genome + self.e_max_groups = e_max_groups + self.oscillator_param_min = oscillator_param_min + self.oscillator_param_max = oscillator_param_max + self.weight_param_min = weight_param_min + self.weight_param_max = weight_param_max + self.weight_min = weight_min + self.weight_max = weight_max + self.axiom_w = axiom_w + self.i_iterations = i_iterations + self.max_structural_modules = max_structural_modules + self.robot_id = robot_id + self.allow_vertical_brick = allow_vertical_brick + self.use_movement_commands = use_movement_commands + self.use_rotation_commands = use_rotation_commands + self.use_movement_stack = use_movement_stack diff --git a/pyrevolve/genotype/plasticoding/alphabet.py b/pyrevolve/genotype/plasticoding/alphabet.py new file mode 100644 index 0000000000..17e4e43d64 --- /dev/null +++ b/pyrevolve/genotype/plasticoding/alphabet.py @@ -0,0 +1,128 @@ +from __future__ import annotations +from collections.abc import Iterable +from enum import Enum +from pyrevolve.custom_logging.logger import logger + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import List, Union + +INDEX_SYMBOL = 0 +INDEX_PARAMS = 1 + + +class Alphabet(Enum): + # Modules + CORE_COMPONENT = 'C' + JOINT_HORIZONTAL = 'AJ1' + JOINT_VERTICAL = 'AJ2' + BLOCK = 'B' + BLOCK_VERTICAL = 'BV' + SENSOR = 'ST' + + # MorphologyMountingCommands + ADD_RIGHT = 'addr' + ADD_FRONT = 'addf' + ADD_LEFT = 'addl' + + # MorphologyMovingCommands + MOVE_RIGHT = 'mover' + MOVE_FRONT = 'movef' + MOVE_LEFT = 'movel' + MOVE_BACK = 'moveb' + ROTATE_90 = 'rotate90' + ROTATE_N90 = 'rotaten90' + PUSH_MOV_STACK = '(' + POP_MOV_STACK = ')' + + # ControllerChangingCommands + ADD_EDGE = 'brainedge' + MUTATE_EDGE = 'brainperturb' + LOOP = 'brainloop' + MUTATE_AMP = 'brainampperturb' + MUTATE_PER = 'brainperperturb' + MUTATE_OFF = 'brainoffperturb' + + # ControllerMovingCommands + MOVE_REF_S = 'brainmoveFTS' + MOVE_REF_O = 'brainmoveTTS' + + @staticmethod + def modules(allow_vertical_brick: bool) -> List[Alphabet]: + # this function MUST return the core always as the first element + modules = [ + Alphabet.CORE_COMPONENT, + Alphabet.JOINT_HORIZONTAL, + Alphabet.JOINT_VERTICAL, + Alphabet.BLOCK, + Alphabet.SENSOR, + ] + if allow_vertical_brick: + modules.append(Alphabet.BLOCK_VERTICAL) + return modules + + def is_vertical_module(self) -> bool: + return self is Alphabet.JOINT_VERTICAL \ + or self is Alphabet.BLOCK_VERTICAL + + def is_joint(self) -> bool: + return self is Alphabet.JOINT_VERTICAL \ + or self is Alphabet.JOINT_HORIZONTAL + + @staticmethod + def morphology_mounting_commands() -> List[Alphabet]: + return [ + Alphabet.ADD_RIGHT, + Alphabet.ADD_FRONT, + Alphabet.ADD_LEFT, + ] + + @staticmethod + def morphology_moving_commands(use_movement_commands: bool, + use_rotation_commands: bool, + use_movement_stack: bool) -> List[Alphabet]: + commands = [] + if use_movement_commands: + commands.append(Alphabet.MOVE_RIGHT) + commands.append(Alphabet.MOVE_FRONT) + commands.append(Alphabet.MOVE_LEFT) + commands.append(Alphabet.MOVE_BACK) + if use_rotation_commands: + commands.append(Alphabet.ROTATE_90) + commands.append(Alphabet.ROTATE_N90) + if use_movement_stack: + commands.append(Alphabet.PUSH_MOV_STACK) + commands.append(Alphabet.POP_MOV_STACK) + + if len(commands) == 0: + logger.warning("NO MOVEMENT COMMAND IS CONFIGURED - this could be a problem") + + return commands + + @staticmethod + def controller_changing_commands() -> List[Alphabet]: + return [ + Alphabet.ADD_EDGE, + Alphabet.MUTATE_EDGE, + Alphabet.LOOP, + Alphabet.MUTATE_AMP, + Alphabet.MUTATE_PER, + Alphabet.MUTATE_OFF, + ] + + @staticmethod + def controller_moving_commands() -> List[Alphabet]: + return [ + Alphabet.MOVE_REF_S, + Alphabet.MOVE_REF_O, + ] + + @staticmethod + def wordify(letters: Union[Alphabet, List] + ) -> Union[(Alphabet, List), List[(Alphabet, List)]]: + if isinstance(letters, Iterable): + return [(a, []) for a in letters] + elif isinstance(letters, Alphabet): + return (letters, []) + else: + raise RuntimeError(f'Cannot wordify element of type {type(letters)}') diff --git a/pyrevolve/genotype/plasticoding/crossover/standard_crossover.py b/pyrevolve/genotype/plasticoding/crossover/standard_crossover.py index d95ad6286c..b03f070593 100644 --- a/pyrevolve/genotype/plasticoding/crossover/standard_crossover.py +++ b/pyrevolve/genotype/plasticoding/crossover/standard_crossover.py @@ -1,7 +1,7 @@ -from pyrevolve.genotype.plasticoding.plasticoding import Plasticoding, Alphabet, PlasticodingConfig -from pyrevolve.evolution.individual import Individual import random -from ....custom_logging.logger import genotype_logger + +from pyrevolve.genotype.plasticoding.plasticoding import Plasticoding, Alphabet +from pyrevolve.custom_logging.logger import genotype_logger def generate_child_genotype(parent_genotypes, genotype_conf, crossover_conf): @@ -17,12 +17,12 @@ def generate_child_genotype(parent_genotypes, genotype_conf, crossover_conf): if crossover_attempt > crossover_conf.crossover_prob: grammar = parent_genotypes[0].grammar else: - for letter in Alphabet.modules(): + for letter in Alphabet.wordify(Alphabet.modules(genotype_conf.allow_vertical_brick)): parent = random.randint(0, 1) # gets the production rule for the respective letter grammar[letter[0]] = parent_genotypes[parent].grammar[letter[0]] - genotype = Plasticoding(genotype_conf, 'tmp') + genotype = Plasticoding(genotype_conf, None) genotype.grammar = grammar return genotype.clone() @@ -31,10 +31,10 @@ def standard_crossover(parent_individuals, genotype_conf, crossover_conf): """ Creates an child (individual) through crossover with two parents - :param parent_genotypes: genotypes of the parents to be used for crossover + :param parent_individuals: parent individuals to be used for crossover :return: genotype result of the crossover """ - parent_genotypes = [p.genotype for p in parent_individuals] + parent_genotypes = [p.representation for p in parent_individuals] new_genotype = generate_child_genotype(parent_genotypes, genotype_conf, crossover_conf) #TODO what if you have more than 2 parents? fix log genotype_logger.info( diff --git a/pyrevolve/genotype/plasticoding/decoder.py b/pyrevolve/genotype/plasticoding/decoder.py new file mode 100644 index 0000000000..9e56bef44e --- /dev/null +++ b/pyrevolve/genotype/plasticoding/decoder.py @@ -0,0 +1,541 @@ +from pyrevolve.genotype.plasticoding.alphabet import Alphabet, INDEX_SYMBOL, INDEX_PARAMS +from pyrevolve.revolve_bot import RevolveBot +from pyrevolve.revolve_bot.brain import BrainNN +from pyrevolve.revolve_bot.brain.brain_nn import Connection, Node, Params +from pyrevolve.revolve_bot.revolve_module import Orientation, CoreModule, BrickModule, ActiveHingeModule, \ + TouchSensorModule + + +# Early Development +class GrammarExpander: + def __init__(self, genotype): + """ + Plasticoding late development decoder + :param genotype: Plasticoding genotype object + :type genotype: Plasticoding + """ + self._genotype = genotype + self._grammar = genotype.grammar + self._conf = genotype.conf + + def expand_grammar(self, n_iterations: int, axiom=Alphabet.CORE_COMPONENT): + """ + Expands the grammar in a command sentence wrapped inside a PlasticodingDecoder + :param n_iterations: number of iterations of the grammar expansion + :param axiom: starting axiom of the grammar + :type axiom: Alphabet + :return: sentence wrapped inside a PlasticodingDecoder ready to be decoded + :rtype PlasticodingDecoder + """ + developed_sentence = [(axiom, [])] + + for i in range(0, n_iterations): + + position = 0 + for aux_index in range(0, len(developed_sentence)): + + symbol = developed_sentence[position][INDEX_SYMBOL] + # TODO check if is present in production rules instead, don't assume production rules and modules are + # the same + if symbol in Alphabet.modules(self._conf.allow_vertical_brick): + # removes symbol + developed_sentence.pop(position) + # replaces by its production rule + ii = 0 + for ii in range(0, len(self._grammar[symbol])): + developed_sentence.insert(position + ii, self._grammar[symbol][ii]) + position = position + ii + 1 + else: + position = position + 1 + # logger.info('Robot ' + str(self.id) + ' was early-developed.') + return PlasticodingDecoder(self._genotype.id, self._conf, developed_sentence) + + +# Late Development +class PlasticodingDecoder: + + class Cursor: + def __init__(self, current_module, orientation): + self.module = current_module + self.orientation = orientation # degrees + self.history = [current_module] + + def copy(self): + # deepcopy is an overkill here and probably even armful + copy = PlasticodingDecoder.Cursor(self.module, self.orientation) + copy.history = self.history.copy() + return copy + + class CursorStack: + def __init__(self, starting_module, orientation=0): + self._stack = [ + PlasticodingDecoder.Cursor(starting_module, orientation) + ] + + def pop(self): + if len(self._stack) > 1: + self._stack.pop() + + def push(self): + self._stack.append( + self.current.copy() + ) + + @property + def current(self): + return self._stack[-1] + + @property + def history(self): + return self.current.history + + def save_history(self): + self.current.history.append(self.current.module) + + def pop_history(self): + assert(len(self.current.history) > 0) + self.current.module = self.current.history[-1] + self.current.history.pop() + + def empty(self): + return len(self._stack) == 0 + + def __init__(self, id_, conf, command_sentence): + """ + Plasticoding late development decoder + :param id_: the id of the genome + :type id_: int|str + :param conf: Plasticoding configuration object + :type conf: PlasticodingConfig + :param command_sentence: result of the early developmental stage + """ + self._id = id_ + self._conf = conf + self._command_sentence = command_sentence + self.body = RevolveBot() + self.brain = BrainNN() + + self.stack = None + self.morph_mounting_container = None + self.quantity_modules = 1 + self.quantity_nodes = 0 + self.inputs_stack = [] + self.outputs_stack = [] + self.edges = {} + + def decode_sentence(self): + self.body._id = self._id + + core_module = CoreModule() + self.body._body = core_module + core_module.id = str(self.quantity_modules) + core_module.info = { + 'orientation': Orientation.FORWARD, + 'new_module_type': Alphabet.CORE_COMPONENT + } + core_module.rgb = [1, 1, 0] + self.stack = PlasticodingDecoder.CursorStack(core_module, orientation=0) + + for word in self._command_sentence[1:]: + symbol = word[INDEX_SYMBOL] + + if symbol in Alphabet.morphology_mounting_commands(): + self.morph_mounting_container = symbol + + elif symbol in Alphabet.modules(self._conf.allow_vertical_brick): + if self.morph_mounting_container is not None \ + and symbol is not Alphabet.CORE_COMPONENT: + + if type(self.stack.current.module) == CoreModule \ + or type(self.stack.current.module) == BrickModule: + slot = self.get_slot(self.morph_mounting_container).value + elif type(self.stack.current.module) == ActiveHingeModule: + slot = Orientation.FORWARD.value + else: + raise RuntimeError( + f'Mounting reference {type(self.stack.current.module)} does not support a mount') + + if self.quantity_modules < self._conf.max_structural_modules: + self.new_module(slot, symbol, word, self._conf.allow_joint_joint_attachment) + + elif symbol in Alphabet.morphology_moving_commands(self._conf.use_movement_commands, + self._conf.use_rotation_commands, + self._conf.use_movement_stack): + self.move_in_body(word) + + elif symbol in Alphabet.controller_changing_commands(): + self.decode_brain_changing(word) + + elif symbol in Alphabet.controller_moving_commands(): + self.decode_brain_moving(word) + + else: + raise RuntimeError(f'Unrecognized symbol: {symbol}') + + self.add_imu_nodes() + + self.body._brain = self.brain + return self.body + + @staticmethod + def get_slot(morph_mounting_container): + slot = None + if morph_mounting_container == Alphabet.ADD_FRONT: + slot = Orientation.FORWARD + elif morph_mounting_container == Alphabet.ADD_LEFT: + slot = Orientation.LEFT + elif morph_mounting_container == Alphabet.ADD_RIGHT: + slot = Orientation.RIGHT + return slot + + def get_angle(self, new_module_type, parent): + if self._conf.use_rotation_commands: + pending_rotation = self.stack.current.orientation + parent_orientation = parent.orientation if parent.orientation is not None else 0 + return parent_orientation + pending_rotation + else: + angle = 0 + vertical_new_module = new_module_type.is_vertical_module() + vertical_parent = parent.info['new_module_type'].is_vertical_module() + if vertical_new_module and not vertical_parent: + angle = 90 + elif vertical_parent and not vertical_new_module: + angle = 270 + return angle + + @staticmethod + def get_color(new_module_type): + if new_module_type == Alphabet.BLOCK: + rgb = [0.0, 0.0, 1.0] + elif new_module_type == Alphabet.BLOCK_VERTICAL: + rgb = [0.5, 0.5, 1.0] + elif new_module_type == Alphabet.JOINT_HORIZONTAL: + rgb = [1.0, 0.08, 0.58] + elif new_module_type == Alphabet.JOINT_VERTICAL: + rgb = [0.7, 0.0, 0.0] + elif new_module_type == Alphabet.SENSOR: + rgb = [0.7, 0.7, 0.7] + else: + rgb = [1.0, 1.0, 1.0] + return rgb + + def move_in_body(self, word): + symbol = word[INDEX_SYMBOL] + if symbol == Alphabet.PUSH_MOV_STACK: + self.stack.push() + + elif symbol == Alphabet.POP_MOV_STACK: + self.stack.pop() + + elif symbol == Alphabet.MOVE_BACK: + if len(self.stack.history) > 0: + self.stack.pop_history() + + elif symbol == Alphabet.MOVE_FRONT: + if self.stack.current.module.children[Orientation.FORWARD.value] is not None \ + and type(self.stack.current.module.children[Orientation.FORWARD.value]) is not TouchSensorModule: + self.stack.save_history() + self.stack.current.module = self.stack.current.module.children[Orientation.FORWARD.value] + + elif symbol == Alphabet.MOVE_RIGHT or symbol == Alphabet.MOVE_LEFT: + + if symbol == Alphabet.MOVE_LEFT and type(self.stack.current.module) is not ActiveHingeModule \ + and self.stack.current.module.children[Orientation.LEFT.value] is not None \ + and type(self.stack.current.module.children[Orientation.LEFT.value]) is not TouchSensorModule: + self.stack.save_history() + self.stack.current.module = self.stack.current.module.children[Orientation.LEFT.value] + + elif symbol == Alphabet.MOVE_RIGHT and type(self.stack.current.module) is not ActiveHingeModule \ + and self.stack.current.module.children[Orientation.RIGHT.value] is not None \ + and type(self.stack.current.module.children[Orientation.RIGHT.value]) is not TouchSensorModule: + self.stack.save_history() + self.stack.current.module = self.stack.current.module.children[Orientation.RIGHT.value] + + if type(self.stack.current.module) is ActiveHingeModule \ + and self.stack.current.module.children[Orientation.FORWARD.value] is not None: + self.stack.save_history() + self.stack.current.module = self.stack.current.module.children[Orientation.FORWARD.value] + + elif symbol is Alphabet.ROTATE_90 or symbol is Alphabet.ROTATE_N90: + self.stack.current.orientation += 90 if symbol is Alphabet.ROTATE_90 else -90 + + else: + raise RuntimeError(f'movement command not recognized {symbol}') + + def new_module(self, slot, new_module_type: Alphabet, word, allow_joint_joint_attachment: bool): + mount = False + if self.stack.current.module.children[slot] is None: + mount = True + if new_module_type == Alphabet.SENSOR \ + and type(self.stack.current.module) is ActiveHingeModule: + mount = False + elif not allow_joint_joint_attachment \ + and type(self.stack.current.module) is ActiveHingeModule \ + and new_module_type.is_joint(): + mount = False + + if type(self.stack.current.module) is CoreModule \ + and self.stack.current.module.children[1] is not None \ + and self.stack.current.module.children[2] is not None \ + and self.stack.current.module.children[3] is not None \ + and self.stack.current.module.children[0] is None: + slot = 0 + mount = True + + if not mount: + return + + if new_module_type == Alphabet.BLOCK \ + or new_module_type == Alphabet.BLOCK_VERTICAL: + module = BrickModule() + elif new_module_type == Alphabet.JOINT_VERTICAL \ + or new_module_type == Alphabet.JOINT_HORIZONTAL: + module = ActiveHingeModule() + elif new_module_type == Alphabet.SENSOR: + module = TouchSensorModule() + else: + raise NotImplementedError(f'{new_module_type}') + + module.info = {'new_module_type': new_module_type} + module.orientation = self.get_angle(new_module_type, self.stack.current.module) + module.rgb = self.get_color(new_module_type) + + if new_module_type != Alphabet.SENSOR: + try: + self.quantity_modules += 1 + module.id = str(self.quantity_modules) + self.stack.current.module.children[slot] = module + # RevolveBot.ItersectionCollisionException can be thrown at this line + self.check_intersection(self.stack.current.module, slot, module) + self.morph_mounting_container = None + self.stack.save_history() + self.stack.current.module = module + if new_module_type == Alphabet.JOINT_HORIZONTAL \ + or new_module_type == Alphabet.JOINT_VERTICAL: + self.decode_brain_node(word, module.id) + except RevolveBot.ItersectionCollisionException: + self.stack.current.module.children[slot] = None + self.quantity_modules -= 1 + else: + self.stack.current.module.children[slot] = module + self.morph_mounting_container = None + module.id = self.stack.current.module.id + 's' + str(slot) + self.decode_brain_node(word, module.id) + + def check_intersection(self, parent, slot, module): + """ + Update coordinates of module, raises exception if there is two blocks have the same coordinates. + :raises: RevolveBot.ItersectionCollisionException if there was a collision in the robot tree + """ + dic = {Orientation.FORWARD.value: 0, + Orientation.LEFT.value: 1, + Orientation.BACK.value: 2, + Orientation.RIGHT.value: 3} + + inverse_dic = {0: Orientation.FORWARD.value, + 1: Orientation.LEFT.value, + 2: Orientation.BACK.value, + 3: Orientation.RIGHT.value} + + # TODO remove orientation, should be useless now + direction = dic[parent.info['orientation'].value] + dic[slot] + direction = direction % len(dic) + new_direction = Orientation(inverse_dic[direction]) + module.info['orientation'] = new_direction + + # Generate coordinate for block, could throw exception + self.body.update_substrate(raise_for_intersections=True) + + def decode_brain_node(self, symbol, part_id): + from pyrevolve.genotype.plasticoding.plasticoding import NodeExtended + + self.quantity_nodes += 1 + node = NodeExtended() + node.id = f'node{self.quantity_nodes}' + node.weight = float(symbol[INDEX_PARAMS][0]) + node.part_id = part_id + + if symbol[INDEX_SYMBOL] == Alphabet.SENSOR: + + node.layer = 'input' + node.type = 'Input' + + # stacks sensor if there are no oscillators yet + if len(self.outputs_stack) == 0: + self.inputs_stack.append(node) + else: + # if it is the first senor ever + if len(self.inputs_stack) > 0: + self.inputs_stack = [node] + else: + self.inputs_stack.append(node) + + # connects sensor to all oscillators in the stack + for output_node in range(0, len(self.outputs_stack)): + self.outputs_stack[output_node].input_nodes.append(node) + node.output_nodes.append(self.outputs_stack[output_node]) + + connection = Connection() + connection.src = node.id + connection.dst = self.outputs_stack[output_node].id + + if output_node == len(self.outputs_stack) - 1: + connection.weight = node.weight + else: + connection.weight = float(self.outputs_stack[output_node].weight) + self.edges[connection.src, connection.dst] = connection + self.brain.connections.append(connection) + self.outputs_stack = [self.outputs_stack[-1]] + + if symbol[INDEX_SYMBOL] == Alphabet.JOINT_VERTICAL \ + or symbol[INDEX_SYMBOL] == Alphabet.JOINT_HORIZONTAL: + + node.layer = 'output' + node.type = 'Oscillator' + + params = Params() + params.period = float(symbol[INDEX_PARAMS][1]) + params.phase_offset = float(symbol[INDEX_PARAMS][2]) + params.amplitude = float(symbol[INDEX_PARAMS][3]) + node.params = params + self.brain.params[node.id] = params + + # stacks oscillator if there are no sensors yet + if len(self.inputs_stack) == 0: + self.outputs_stack.append(node) + else: + # if it is the first oscillator ever + if len(self.outputs_stack) > 0: + self.outputs_stack = [node] + else: + self.outputs_stack.append(node) + + # connects oscillator to all sensors in the stack + for input_node in range(0, len(self.inputs_stack)): + self.inputs_stack[input_node].output_nodes.append(node) + node.input_nodes.append(self.inputs_stack[input_node]) + + connection = Connection() + connection.src = self.inputs_stack[input_node].id + connection.dst = node.id + if input_node == len(self.inputs_stack) - 1: + connection.weight = node.weight + else: + connection.weight = float(self.inputs_stack[input_node].weight) + self.edges[connection.src, connection.dst] = connection + self.brain.connections.append(connection) + self.inputs_stack = [self.inputs_stack[-1]] + + self.brain.nodes[node.id] = node + + def add_imu_nodes(self): + for p in range(1, 7): + _id = 'node-core' + str(p) + node = Node() + node.layer = 'input' + node.type = 'Input' + node.part_id = self.body.id + node.id = _id + self.brain.nodes[_id] = node + + def decode_brain_moving(self, word): + symbol = word[INDEX_SYMBOL] + + # if there is at least both one sensor and one oscillator + if len(self.outputs_stack) > 0 and len(self.inputs_stack) > 0: + + intermediate = int(float(word[INDEX_PARAMS][0])) + sibling = int(float(word[INDEX_PARAMS][1])) + + if symbol == Alphabet.MOVE_REF_S: + + if len(self.inputs_stack[-1].output_nodes) < intermediate: + intermediate = len(self.inputs_stack[-1].output_nodes) - 1 + else: + intermediate = intermediate - 1 + + if len(self.inputs_stack[-1].output_nodes[intermediate].input_nodes) < sibling: + sibling = len(self.inputs_stack[-1].output_nodes[intermediate].input_nodes) - 1 + else: + sibling = sibling - 1 + + self.inputs_stack[-1] = self.inputs_stack[-1].output_nodes[intermediate].input_nodes[sibling] + + if symbol == Alphabet.MOVE_REF_O: + + if len(self.outputs_stack[-1].input_nodes) < intermediate: + intermediate = len(self.outputs_stack[-1].input_nodes) - 1 + else: + intermediate = intermediate - 1 + + if len(self.outputs_stack[-1].input_nodes[intermediate].output_nodes) < sibling: + sibling = len(self.outputs_stack[-1].input_nodes[intermediate].output_nodes) - 1 + else: + sibling = sibling - 1 + + self.outputs_stack[-1] = self.outputs_stack[-1].input_nodes[intermediate].output_nodes[sibling] + + def decode_brain_changing(self, word): + symbol = word[INDEX_SYMBOL] + conf = self._conf + + # if there is at least both one oscillator + if len(self.outputs_stack) > 0: + + if symbol == Alphabet.MUTATE_PER: + self.outputs_stack[-1].params.period += float(word[INDEX_PARAMS][0]) + if self.outputs_stack[-1].params.period > conf.oscillator_param_max: + self.outputs_stack[-1].params.period = conf.oscillator_param_max + if self.outputs_stack[-1].params.period < conf.oscillator_param_min: + self.outputs_stack[-1].params.period = conf.oscillator_param_min + + if symbol == Alphabet.MUTATE_AMP: + self.outputs_stack[-1].params.amplitude += float(word[INDEX_PARAMS][0]) + if self.outputs_stack[-1].params.amplitude > conf.oscillator_param_max: + self.outputs_stack[-1].params.amplitude = conf.oscillator_param_max + if self.outputs_stack[-1].params.amplitude < conf.oscillator_param_min: + self.outputs_stack[-1].params.amplitude = conf.oscillator_param_min + + if symbol == Alphabet.MUTATE_OFF: + self.outputs_stack[-1].params.phase_offset += float(word[INDEX_PARAMS][0]) + if self.outputs_stack[-1].params.phase_offset > conf.oscillator_param_max: + self.outputs_stack[-1].params.phase_offset = conf.oscillator_param_max + if self.outputs_stack[-1].params.phase_offset < conf.oscillator_param_min: + self.outputs_stack[-1].params.phase_offset = conf.oscillator_param_min + + if symbol == Alphabet.MUTATE_EDGE: + if len(self.edges) > 0: + if (self.inputs_stack[-1].id, self.outputs_stack[-1].id) in self.edges.keys(): + self.edges[self.inputs_stack[-1].id, self.outputs_stack[-1].id].weight \ + += float(word[INDEX_PARAMS][0]) + if self.edges[self.inputs_stack[-1].id, self.outputs_stack[-1].id].weight \ + > conf.weight_param_max: + self.edges[self.inputs_stack[-1].id, self.outputs_stack[-1].id].weight \ + = conf.weight_param_max + if self.edges[self.inputs_stack[-1].id, self.outputs_stack[-1].id].weight \ + < conf.weight_param_min: + self.edges[self.inputs_stack[-1].id, self.outputs_stack[-1].id].weight \ + = conf.weight_param_min + + # if there is at least both one sensor and one oscillator + if len(self.outputs_stack) > 0 and len(self.inputs_stack) > 0: + if symbol == Alphabet.LOOP: + + if (self.outputs_stack[-1].id, self.outputs_stack[-1].id) not in self.edges.keys(): + connection = Connection() + connection.src = self.outputs_stack[-1].id + connection.dst = connection.src + connection.weight = float(word[INDEX_PARAMS][0]) + self.edges[connection.src, connection.src] = connection + self.brain.connections.append(connection) + + if symbol == Alphabet.ADD_EDGE: + if (self.inputs_stack[-1].id, self.outputs_stack[-1].id) not in self.edges.keys(): + connection = Connection() + connection.src = self.inputs_stack[-1].id + connection.dst = self.outputs_stack[-1].id + connection.weight = float(word[INDEX_PARAMS][0]) + self.edges[connection.src, connection.dst] = connection + self.brain.connections.append(connection) + self.inputs_stack[-1].output_nodes.append(self.outputs_stack[-1]) + self.outputs_stack[-1].input_nodes.append(self.inputs_stack[-1]) diff --git a/pyrevolve/genotype/plasticoding/initialization.py b/pyrevolve/genotype/plasticoding/initialization.py index 7f784313f0..df6990d785 100644 --- a/pyrevolve/genotype/plasticoding/initialization.py +++ b/pyrevolve/genotype/plasticoding/initialization.py @@ -1,19 +1,24 @@ -from pyrevolve.genotype.plasticoding.plasticoding import Plasticoding -from pyrevolve.genotype.plasticoding.plasticoding import Alphabet +from __future__ import annotations import random +from pyrevolve.genotype.plasticoding.plasticoding import Alphabet +from pyrevolve.genotype.plasticoding.plasticoding import Plasticoding + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Dict, List + from pyrevolve.genotype.plasticoding.plasticoding import PlasticodingConfig -def _generate_random_grammar(conf): +def _generate_random_grammar(conf: PlasticodingConfig) -> Dict[Alphabet, List]: """ Initializing a new genotype, :param conf: e_max_groups, maximum number of groups of symbols :return: a random new Genome - :rtype: dictionary """ s_segments = random.randint(1, conf.e_max_groups) grammar = {} - for symbol in Alphabet.modules(): + for symbol in Alphabet.wordify(Alphabet.modules(conf.allow_vertical_brick)): if symbol[0] == conf.axiom_w: grammar[symbol[0]] = [[conf.axiom_w, []]] @@ -22,11 +27,13 @@ def _generate_random_grammar(conf): for s in range(0, s_segments): symbol_module = random.randint( - 1, len(Alphabet.modules()) - 1) + 1, len(Alphabet.modules(conf.allow_vertical_brick)) - 1) symbol_mounting = random.randint( 0, len(Alphabet.morphology_mounting_commands()) - 1) symbol_morph_moving = random.randint( - 0, len(Alphabet.morphology_moving_commands()) - 1) + 0, len(Alphabet.morphology_moving_commands(conf.use_movement_commands, + conf.use_rotation_commands, + conf.use_movement_stack)) - 1) symbol_contr_moving = random.randint( 0, len(Alphabet.controller_moving_commands()) - 1) symbol_changing = random.randint( @@ -34,27 +41,28 @@ def _generate_random_grammar(conf): grammar[symbol[0]].extend([ Plasticoding.build_symbol( - Alphabet.controller_moving_commands()[symbol_contr_moving], conf), + Alphabet.wordify(Alphabet.controller_moving_commands())[symbol_contr_moving], conf), Plasticoding.build_symbol( - Alphabet.controller_changing_commands()[symbol_changing], conf), + Alphabet.wordify(Alphabet.controller_changing_commands())[symbol_changing], conf), Plasticoding.build_symbol( - Alphabet.morphology_mounting_commands()[symbol_mounting], conf), + Alphabet.wordify(Alphabet.morphology_mounting_commands())[symbol_mounting], conf), Plasticoding.build_symbol( - Alphabet.modules()[symbol_module], conf), + Alphabet.wordify(Alphabet.modules(conf.allow_vertical_brick))[symbol_module], conf), Plasticoding.build_symbol( - Alphabet.morphology_moving_commands()[symbol_morph_moving], conf), + Alphabet.wordify(Alphabet.morphology_moving_commands( + conf.use_movement_commands, conf.use_rotation_commands, conf.use_movement_stack) + )[symbol_morph_moving], conf), ]) return grammar -def random_initialization(conf, next_robot_id): +def random_initialization(conf: PlasticodingConfig, _id: int) -> Plasticoding: """ Initializing a random genotype. - :type conf: PlasticodingConfig - :return: a Genome - :rtype: Plasticoding + :param conf: Plasticoding genotype configuration + :param _id: id of the newly created genotype + :return: a Plasticoding Genome """ - genotype = Plasticoding(conf, next_robot_id) + genotype = Plasticoding(conf, _id) genotype.grammar = _generate_random_grammar(conf) - return genotype diff --git a/pyrevolve/genotype/plasticoding/mutation/standard_mutation.py b/pyrevolve/genotype/plasticoding/mutation/standard_mutation.py index e877a947a1..ae4bba9063 100644 --- a/pyrevolve/genotype/plasticoding/mutation/standard_mutation.py +++ b/pyrevolve/genotype/plasticoding/mutation/standard_mutation.py @@ -1,8 +1,6 @@ import random from pyrevolve.genotype.plasticoding.plasticoding import Alphabet, Plasticoding -from ....custom_logging.logger import genotype_logger - - +from pyrevolve.custom_logging.logger import genotype_logger def handle_deletion(genotype): @@ -57,28 +55,28 @@ def generate_symbol(genotype_conf): symbol_category = random.randint(1, 5) # Modules if symbol_category == 1: - alphabet = random.randint(1, len(Alphabet.modules()) - 1) - symbol = Plasticoding.build_symbol(Alphabet.modules()[alphabet], genotype_conf) + # do not use the first symbol, the core + symbols = Alphabet.modules(genotype_conf.allow_vertical_brick)[1:] # Morphology mounting commands elif symbol_category == 2: - alphabet = random.randint(0, len(Alphabet.morphology_mounting_commands()) - 1) - symbol = Plasticoding.build_symbol(Alphabet.morphology_mounting_commands()[alphabet], genotype_conf) + symbols = Alphabet.morphology_mounting_commands() # Morphology moving commands elif symbol_category == 3: - alphabet = random.randint(0, len(Alphabet.morphology_moving_commands()) - 1) - symbol = Plasticoding.build_symbol(Alphabet.morphology_moving_commands()[alphabet], genotype_conf) + symbols = Alphabet.morphology_moving_commands(genotype_conf.use_movement_commands, + genotype_conf.use_rotation_commands, + genotype_conf.use_movement_stack) # Controller moving commands elif symbol_category == 4: - alphabet = random.randint(0, len(Alphabet.controller_moving_commands()) - 1) - symbol = Plasticoding.build_symbol(Alphabet.controller_moving_commands()[alphabet], genotype_conf) + symbols = Alphabet.controller_moving_commands() # Controller changing commands elif symbol_category == 5: - alphabet = random.randint(0, len(Alphabet.controller_changing_commands()) - 1) - symbol = Plasticoding.build_symbol(Alphabet.controller_changing_commands()[alphabet], genotype_conf) + symbols = Alphabet.controller_changing_commands() else: raise Exception( 'random number did not generate a number between 1 and 5. The value was: {}'.format(symbol_category)) + alphabet = random.randint(0, len(symbols) - 1) + symbol = Plasticoding.build_symbol(symbols[alphabet], genotype_conf) return symbol diff --git a/pyrevolve/genotype/plasticoding/plasticoding.py b/pyrevolve/genotype/plasticoding/plasticoding.py index 5cd314997d..61fe801dca 100644 --- a/pyrevolve/genotype/plasticoding/plasticoding.py +++ b/pyrevolve/genotype/plasticoding/plasticoding.py @@ -1,100 +1,19 @@ -# no mother classes have been defined yet! not sure how to separate the the filed in folders... - -from enum import Enum -from pyrevolve.genotype import Genotype -from pyrevolve.revolve_bot import RevolveBot -from pyrevolve.revolve_bot.revolve_module import Orientation -from pyrevolve.revolve_bot.revolve_module import CoreModule -from pyrevolve.revolve_bot.revolve_module import ActiveHingeModule -from pyrevolve.revolve_bot.revolve_module import BrickModule -from pyrevolve.revolve_bot.revolve_module import TouchSensorModule -from pyrevolve.revolve_bot.brain.brain_nn import BrainNN -from pyrevolve.revolve_bot.brain.brain_nn import Node -from pyrevolve.revolve_bot.brain.brain_nn import Connection -from pyrevolve.revolve_bot.brain.brain_nn import Params -from ...custom_logging.logger import logger +from __future__ import annotations import random import math import copy -import itertools - - -class Alphabet(Enum): - - # Modules - CORE_COMPONENT = 'C' - JOINT_HORIZONTAL = 'AJ1' - JOINT_VERTICAL = 'AJ2' - BLOCK = 'B' - SENSOR = 'ST' - - # MorphologyMountingCommands - ADD_RIGHT = 'addr' - ADD_FRONT = 'addf' - ADD_LEFT = 'addl' - - # MorphologyMovingCommands - MOVE_RIGHT = 'mover' - MOVE_FRONT = 'movef' - MOVE_LEFT = 'movel' - MOVE_BACK = 'moveb' - - # ControllerChangingCommands - ADD_EDGE = 'brainedge' - MUTATE_EDGE = 'brainperturb' - LOOP = 'brainloop' - MUTATE_AMP = 'brainampperturb' - MUTATE_PER = 'brainperperturb' - MUTATE_OFF = 'brainoffperturb' - - # ControllerMovingCommands - MOVE_REF_S = 'brainmoveFTS' - MOVE_REF_O = 'brainmoveTTS' - - @staticmethod - def modules(): - return [ - [Alphabet.CORE_COMPONENT, []], - [Alphabet.JOINT_HORIZONTAL, []], - [Alphabet.JOINT_VERTICAL, []], - [Alphabet.BLOCK, []], - [Alphabet.SENSOR, []], - ] - - @staticmethod - def morphology_mounting_commands(): - return [ - [Alphabet.ADD_RIGHT, []], - [Alphabet.ADD_FRONT, []], - [Alphabet.ADD_LEFT, []] - ] - - @staticmethod - def morphology_moving_commands(): - return [ - [Alphabet.MOVE_RIGHT, []], - [Alphabet.MOVE_FRONT, []], - [Alphabet.MOVE_LEFT, []], - [Alphabet.MOVE_BACK, []] - ] - @staticmethod - def controller_changing_commands(): - return [ - [Alphabet.ADD_EDGE, []], - [Alphabet.MUTATE_EDGE, []], - [Alphabet.LOOP, []], - [Alphabet.MUTATE_AMP, []], - [Alphabet.MUTATE_PER, []], - [Alphabet.MUTATE_OFF, []] - ] +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype import Genotype +from pyrevolve.genotype.plasticoding.alphabet import Alphabet, INDEX_SYMBOL, INDEX_PARAMS +from pyrevolve.genotype.plasticoding.decoder import GrammarExpander +from pyrevolve.revolve_bot.brain.brain_nn import Node - @staticmethod - def controller_moving_commands(): - return [ - [Alphabet.MOVE_REF_S, []], - [Alphabet.MOVE_REF_O, []] - ] +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Optional, Dict, List, Union + from pyrevolve.genotype.plasticoding import PlasticodingConfig + from pyrevolve.revolve_bot import RevolveBot class Plasticoding(Genotype): @@ -102,44 +21,45 @@ class Plasticoding(Genotype): L-system genotypic representation, enhanced with epigenetic capabilities for phenotypic plasticity, through Genetic Programming. """ - def __init__(self, conf, robot_id): + def __init__(self, conf: PlasticodingConfig, robot_id: Optional[int]): """ :param conf: configurations for lsystem :param robot_id: unique id of the robot :type conf: PlasticodingConfig """ - self.conf = conf - self.id = str(robot_id) - self.grammar = {} + self.conf: PlasticodingConfig = conf + assert robot_id is None or str(robot_id).isdigit() + self.id: int = int(robot_id) if robot_id is not None else -1 + self.grammar: Dict[Alphabet, List] = {} # Auxiliary variables - self.substrate_coordinates_all = {(0, 0): '1'} - self.valid = False + self.valid: bool = False self.intermediate_phenotype = None - self.phenotype = None - self.morph_mounting_container = None - self.mounting_reference = None - self.mounting_reference_stack = [] - self.quantity_modules = 1 - self.quantity_nodes = 0 - self.index_symbol = 0 - self.index_params = 1 - self.inputs_stack = [] - self.outputs_stack = [] - self.edges = {} + self.phenotype: Optional[RevolveBot] = None def clone(self): - return copy.deepcopy(self) - - def load_genotype(self, genotype_file): - with open(genotype_file) as f: + # Cannot use deep clone for this genome, because it is bugged sometimes + _id = self.id if self.id >= 0 else None + other = Plasticoding(self.conf, _id) + other.grammar = {} + for key, value in self.grammar.items(): + other.grammar[key] = copy.deepcopy(value) + other.valid = self.valid + # other.intermediate_phenotype = self.intermediate_phenotype + # other.phenotype = self.phenotype + return other + + def load_genotype(self, genotype_filename: str) -> None: + with open(genotype_filename) as f: lines = f.readlines() + self._load_genotype_from(lines) + def _load_genotype_from(self, lines: List[str]) -> None: for line in lines: line_array = line.split(' ') - repleceable_symbol = Alphabet(line_array[0]) - self.grammar[repleceable_symbol] = [] - rule = line_array[1:len(line_array)-1] + replaceable_symbol = Alphabet(line_array[0]) + self.grammar[replaceable_symbol] = [] + rule = line_array[1:len(line_array) - 1] for symbol_array in rule: symbol_array = symbol_array.split('_') symbol = Alphabet(symbol_array[0]) @@ -147,516 +67,78 @@ def load_genotype(self, genotype_file): params = symbol_array[1].split('|') else: params = [] - self.grammar[repleceable_symbol].append([symbol, params]) + self.grammar[replaceable_symbol].append([symbol, params]) + + def export_genotype(self, filepath: str) -> None: + with open(filepath, 'w+') as file: + self._export_genotype_open_file(file) - def export_genotype(self, filepath): - file = open(filepath, 'w+') + def _export_genotype_open_file(self, file) -> None: for key, rule in self.grammar.items(): line = key.value + ' ' for item_rule in range(0, len(rule)): - symbol = rule[item_rule][self.index_symbol].value - if len(rule[item_rule][self.index_params]) > 0: + symbol = rule[item_rule][INDEX_SYMBOL].value + if len(rule[item_rule][INDEX_PARAMS]) > 0: params = '_' - for param in range(0, len(rule[item_rule][self.index_params])): - params += str(rule[item_rule][self.index_params][param]) - if param < len(rule[item_rule][self.index_params])-1: + for param in range(0, len(rule[item_rule][INDEX_PARAMS])): + params += str(rule[item_rule][INDEX_PARAMS][param]) + if param < len(rule[item_rule][INDEX_PARAMS]) - 1: params += '|' symbol += params line += symbol + ' ' - file.write(line+'\n') - file.close() - - def load_and_develop(self, load, genotype_path='', id_genotype=None): - - self.id = id_genotype - if not load: - self.grammar = self.conf.initialization_genome(self.conf).grammar - logger.info('Robot {} was initialized.'.format(self.id)) - else: - self.load_genotype('{}{}.txt'.format(genotype_path, self.id)) - logger.info('Robot {} was loaded.'.format(self.id)) + file.write(line + '\n') - self.phenotype = self.develop() - - def check_validity(self): + def check_validity(self) -> None: if self.phenotype._morphological_measurements.measurement_to_dict()['hinge_count'] > 0: self.valid = True - def develop(self): - self.early_development() - phenotype = self.late_development() - return phenotype - - def early_development(self): - - self.intermediate_phenotype = [[self.conf.axiom_w, []]] - - for i in range(0, self.conf.i_iterations): - - position = 0 - for aux_index in range(0, len(self.intermediate_phenotype)): - - symbol = self.intermediate_phenotype[position] - if [symbol[self.index_symbol], []] in Alphabet.modules(): - # removes symbol - self.intermediate_phenotype.pop(position) - # replaces by its production rule - for ii in range(0, len(self.grammar[symbol[self.index_symbol]])): - self.intermediate_phenotype.insert(position+ii, - self.grammar[symbol[self.index_symbol]][ii]) - position = position+ii+1 - else: - position = position + 1 - # logger.info('Robot ' + str(self.id) + ' was early-developed.') - - def late_development(self): - - self.phenotype = RevolveBot() - self.phenotype._id = self.id if type(self.id) == str and self.id.startswith("robot") else "robot_{}".format(self.id) - self.phenotype._brain = BrainNN() - - for symbol in self.intermediate_phenotype: - - if symbol[self.index_symbol] == Alphabet.CORE_COMPONENT: - module = CoreModule() - self.phenotype._body = module - module.id = str(self.quantity_modules) - module.info = {'orientation': Orientation.NORTH, - 'new_module_type': Alphabet.CORE_COMPONENT} - module.orientation = 0 - module.rgb = [1, 1, 0] - self.mounting_reference = module - - if [symbol[self.index_symbol], []] in Alphabet.morphology_mounting_commands(): - self.morph_mounting_container = symbol[self.index_symbol] - - if [symbol[self.index_symbol], []] in Alphabet.modules() \ - and symbol[self.index_symbol] is not Alphabet.CORE_COMPONENT \ - and self.morph_mounting_container is not None: - - if type(self.mounting_reference) == CoreModule \ - or type(self.mounting_reference) == BrickModule: - slot = self.get_slot(self.morph_mounting_container).value - if type(self.mounting_reference) == ActiveHingeModule: - slot = Orientation.NORTH.value - - if self.quantity_modules < self.conf.max_structural_modules: - self.new_module(slot, - symbol[self.index_symbol], - symbol) - - if [symbol[self.index_symbol], []] in Alphabet.morphology_moving_commands(): - self.move_in_body(symbol) - - if [symbol[self.index_symbol], []] in Alphabet.controller_changing_commands(): - self.decode_brain_changing(symbol) - - if [symbol[self.index_symbol], []] in Alphabet.controller_moving_commands(): - self.decode_brain_moving(symbol) - - self.add_imu_nodes() - logger.info('Robot ' + str(self.id) + ' was late-developed.') - + def develop(self) -> RevolveBot: + self.phenotype = GrammarExpander(self)\ + .expand_grammar(self.conf.i_iterations, self.conf.axiom_w)\ + .decode_sentence() return self.phenotype - def move_in_body(self, symbol): - - if symbol[self.index_symbol] == Alphabet.MOVE_BACK \ - and len(self.mounting_reference_stack) > 0: - self.mounting_reference = self.mounting_reference_stack[-1] - self.mounting_reference_stack.pop() - - elif symbol[self.index_symbol] == Alphabet.MOVE_FRONT \ - and self.mounting_reference.children[Orientation.NORTH.value] is not None: - if type(self.mounting_reference.children[Orientation.NORTH.value]) is not TouchSensorModule: - self.mounting_reference_stack.append(self.mounting_reference) - self.mounting_reference = \ - self.mounting_reference.children[Orientation.NORTH.value] - - elif symbol[self.index_symbol] == Alphabet.MOVE_LEFT \ - and type(self.mounting_reference) is not ActiveHingeModule: - if self.mounting_reference.children[Orientation.WEST.value] is not None: - if type(self.mounting_reference.children[Orientation.WEST.value]) is not TouchSensorModule: - self.mounting_reference_stack.append(self.mounting_reference) - self.mounting_reference = \ - self.mounting_reference.children[Orientation.WEST.value] - - elif symbol[self.index_symbol] == Alphabet.MOVE_RIGHT \ - and type(self.mounting_reference) is not ActiveHingeModule: - if self.mounting_reference.children[Orientation.EAST.value] is not None: - if type(self.mounting_reference.children[Orientation.EAST.value]) is not TouchSensorModule: - self.mounting_reference_stack.append(self.mounting_reference) - self.mounting_reference = \ - self.mounting_reference.children[Orientation.EAST.value] - - elif (symbol[self.index_symbol] == Alphabet.MOVE_RIGHT \ - or symbol[self.index_symbol] == Alphabet.MOVE_LEFT) \ - and type(self.mounting_reference) is ActiveHingeModule \ - and self.mounting_reference.children[Orientation.NORTH.value] is not None: - self.mounting_reference_stack.append(self.mounting_reference) - self.mounting_reference = \ - self.mounting_reference.children[Orientation.NORTH.value] - - def decode_brain_changing(self, symbol): - - # if there is at least both one oscillator - if len(self.outputs_stack) > 0: - - if symbol[self.index_symbol] == Alphabet.MUTATE_PER: - self.outputs_stack[-1].params.period += float(symbol[self.index_params][0]) - if self.outputs_stack[-1].params.period > self.conf.oscillator_param_max: - self.outputs_stack[-1].params.period = self.conf.oscillator_param_max - if self.outputs_stack[-1].params.period < self.conf.oscillator_param_min: - self.outputs_stack[-1].params.period = self.conf.oscillator_param_min - - if symbol[self.index_symbol] == Alphabet.MUTATE_AMP: - self.outputs_stack[-1].params.amplitude += float(symbol[self.index_params][0]) - if self.outputs_stack[-1].params.amplitude > self.conf.oscillator_param_max: - self.outputs_stack[-1].params.amplitude = self.conf.oscillator_param_max - if self.outputs_stack[-1].params.amplitude < self.conf.oscillator_param_min: - self.outputs_stack[-1].params.amplitude = self.conf.oscillator_param_min - - if symbol[self.index_symbol] == Alphabet.MUTATE_OFF: - self.outputs_stack[-1].params.phase_offset += float(symbol[self.index_params][0]) - if self.outputs_stack[-1].params.phase_offset > self.conf.oscillator_param_max: - self.outputs_stack[-1].params.phase_offset = self.conf.oscillator_param_max - if self.outputs_stack[-1].params.phase_offset < self.conf.oscillator_param_min: - self.outputs_stack[-1].params.phase_offset = self.conf.oscillator_param_min - - if symbol[self.index_symbol] == Alphabet.MUTATE_EDGE: - if len(self.edges) > 0: - if (self.inputs_stack[-1].id, self.outputs_stack[-1].id) in self.edges.keys(): - self.edges[self.inputs_stack[-1].id, self.outputs_stack[-1].id].weight \ - += float(symbol[self.index_params][0]) - if self.edges[self.inputs_stack[-1].id, self.outputs_stack[-1].id].weight \ - > self.conf.weight_param_max: - self.edges[self.inputs_stack[-1].id, self.outputs_stack[-1].id].weight \ - = self.conf.weight_param_max - if self.edges[self.inputs_stack[-1].id, self.outputs_stack[-1].id].weight \ - < self.conf.weight_param_min: - self.edges[self.inputs_stack[-1].id, self.outputs_stack[-1].id].weight \ - = self.conf.weight_param_min - - # if there is at least both one sensor and one oscillator - if len(self.outputs_stack) > 0 and len(self.inputs_stack) > 0: - if symbol[self.index_symbol] == Alphabet.LOOP: - - if (self.outputs_stack[-1].id, self.outputs_stack[-1].id) not in self.edges.keys(): - connection = Connection() - connection.src = self.outputs_stack[-1].id - connection.dst = connection.src - connection.weight = float(symbol[self.index_params][0]) - self.edges[connection.src, connection.src] = connection - self.phenotype._brain.connections.append(connection) - - if symbol[self.index_symbol] == Alphabet.ADD_EDGE: - if (self.inputs_stack[-1].id, self.outputs_stack[-1].id) not in self.edges.keys(): - connection = Connection() - connection.src = self.inputs_stack[-1].id - connection.dst = self.outputs_stack[-1].id - connection.weight = float(symbol[self.index_params][0]) - self.edges[connection.src, connection.dst] = connection - self.phenotype._brain.connections.append(connection) - self.inputs_stack[-1].output_nodes.append(self.outputs_stack[-1]) - self.outputs_stack[-1].input_nodes.append(self.inputs_stack[-1]) - - def decode_brain_moving(self, symbol): - - # if there is at least both one sensor and one oscillator - if len(self.outputs_stack) > 0 and len(self.inputs_stack) > 0: - - intermediate = int(float(symbol[self.index_params][0])) - sibling = int(float(symbol[self.index_params][1])) - - if symbol[self.index_symbol] == Alphabet.MOVE_REF_S: - - if len(self.inputs_stack[-1].output_nodes) < intermediate: - intermediate = len(self.inputs_stack[-1].output_nodes) - 1 - else: - intermediate = intermediate - 1 - - if len(self.inputs_stack[-1].output_nodes[intermediate].input_nodes) < sibling: - sibling = len(self.inputs_stack[-1].output_nodes[intermediate].input_nodes) - 1 - else: - sibling = sibling - 1 - - self.inputs_stack[-1] = self.inputs_stack[-1].output_nodes[intermediate].input_nodes[sibling] - - if symbol[self.index_symbol] == Alphabet.MOVE_REF_O: - - if len(self.outputs_stack[-1].input_nodes) < intermediate: - intermediate = len(self.outputs_stack[-1].input_nodes) - 1 - else: - intermediate = intermediate - 1 - - if len(self.outputs_stack[-1].input_nodes[intermediate].output_nodes) < sibling: - sibling = len(self.outputs_stack[-1].input_nodes[intermediate].output_nodes) - 1 - else: - sibling = sibling - 1 - - self.outputs_stack[-1] = self.outputs_stack[-1].input_nodes[intermediate].output_nodes[sibling] - - def get_color(self, new_module_type): - - rgb = [] - if new_module_type == Alphabet.BLOCK: - rgb = [0, 0, 1] - if new_module_type == Alphabet.JOINT_HORIZONTAL: - rgb = [1, 0.08, 0.58] - if new_module_type == Alphabet.JOINT_VERTICAL: - rgb = [0.7, 0, 0] - if new_module_type == Alphabet.SENSOR: - rgb = [0.7, 0.7, 0.7] - return rgb - - def get_slot(self, morph_mounting_container): - - slot = None - if morph_mounting_container == Alphabet.ADD_FRONT: - slot = Orientation.NORTH - if morph_mounting_container == Alphabet.ADD_LEFT: - slot = Orientation.WEST - if morph_mounting_container == Alphabet.ADD_RIGHT: - slot = Orientation.EAST - return slot - - def get_angle(self, new_module_type, parent): - angle = 0 - if new_module_type == Alphabet.JOINT_VERTICAL: - if parent.info['new_module_type'] is Alphabet.JOINT_VERTICAL: - angle = 0 - else: - angle = 90 - else: - if parent.info['new_module_type'] is Alphabet.JOINT_VERTICAL: - angle = 270 - return angle - - def check_intersection(self, parent, slot, module): - """ - Update coordinates of module - :return: - """ - dic = {Orientation.NORTH.value: 0, - Orientation.WEST.value: 1, - Orientation.SOUTH.value: 2, - Orientation.EAST.value: 3} - - inverse_dic = {0: Orientation.NORTH.value, - 1: Orientation.WEST.value, - 2: Orientation.SOUTH.value, - 3: Orientation.EAST.value} - - direction = dic[parent.info['orientation'].value] + dic[slot] - if direction >= len(dic): - direction = direction - len(dic) - - new_direction = Orientation(inverse_dic[direction]) - if new_direction == Orientation.WEST: - coordinates = [parent.substrate_coordinates[0], - parent.substrate_coordinates[1] - 1] - if new_direction == Orientation.EAST: - coordinates = [parent.substrate_coordinates[0], - parent.substrate_coordinates[1] + 1] - if new_direction == Orientation.NORTH: - coordinates = [parent.substrate_coordinates[0] + 1, - parent.substrate_coordinates[1]] - if new_direction == Orientation.SOUTH: - coordinates = [parent.substrate_coordinates[0] - 1, - parent.substrate_coordinates[1]] - - module.substrate_coordinates = coordinates - module.info['orientation'] = new_direction - if (coordinates[0], coordinates[1]) in self.substrate_coordinates_all: - return True - else: - self.substrate_coordinates_all[coordinates[0], - coordinates[1]] = module.id - return False - - def new_module(self, slot, new_module_type, symbol): - - mount = False - if self.mounting_reference.children[slot] is None \ - and not (new_module_type == Alphabet.SENSOR - and type(self.mounting_reference) is ActiveHingeModule): - mount = True - - if type(self.mounting_reference) is CoreModule \ - and self.mounting_reference.children[1] is not None \ - and self.mounting_reference.children[2] is not None \ - and self.mounting_reference.children[3] is not None \ - and self.mounting_reference.children[0] is None: - slot = 0 - mount = True - - if mount: - if new_module_type == Alphabet.BLOCK: - module = BrickModule() - if new_module_type == Alphabet.JOINT_VERTICAL \ - or new_module_type == Alphabet.JOINT_HORIZONTAL: - module = ActiveHingeModule() - if new_module_type == Alphabet.SENSOR: - module = TouchSensorModule() - - module.info = {} - module.info['new_module_type'] = new_module_type - module.orientation = self.get_angle(new_module_type, - self.mounting_reference) - module.rgb = self.get_color(new_module_type) - - if new_module_type != Alphabet.SENSOR: - self.quantity_modules += 1 - module.id = str(self.quantity_modules) - intersection = self.check_intersection(self.mounting_reference, slot, module) - - if not intersection: - self.mounting_reference.children[slot] = module - self.morph_mounting_container = None - self.mounting_reference_stack.append(self.mounting_reference) - self.mounting_reference = module - if new_module_type == Alphabet.JOINT_HORIZONTAL \ - or new_module_type == Alphabet.JOINT_VERTICAL: - self.decode_brain_node(symbol, module.id) - else: - self.quantity_modules -= 1 - else: - self.mounting_reference.children[slot] = module - self.morph_mounting_container = None - module.id = self.mounting_reference.id+'s'+str(slot) - self.decode_brain_node(symbol, module.id) - - def decode_brain_node(self, symbol, part_id): - - self.quantity_nodes += 1 - node = NodeExtended() - node.id = 'node'+str(self.quantity_nodes) - node.weight = float(symbol[self.index_params][0]) - node.part_id = part_id - - if symbol[self.index_symbol] == Alphabet.SENSOR: - - node.layer = 'input' - node.type = 'Input' - - # stacks sensor if there are no oscillators yet - if len(self.outputs_stack) == 0: - self.inputs_stack.append(node) - else: - # if it is the first senor ever - if len(self.inputs_stack) > 0: - self.inputs_stack = [node] - else: - self.inputs_stack.append(node) - - # connects sensor to all oscillators in the stack - for output_node in range(0, len(self.outputs_stack)): - self.outputs_stack[output_node].input_nodes.append(node) - node.output_nodes.append(self.outputs_stack[output_node]) - - connection = Connection() - connection.src = node.id - connection.dst = self.outputs_stack[output_node].id - - if output_node == len(self.outputs_stack)-1: - connection.weight = node.weight - else: - connection.weight = float(self.outputs_stack[output_node].weight) - self.edges[connection.src, connection.dst] = connection - self.phenotype._brain.connections.append(connection) - self.outputs_stack = [self.outputs_stack[-1]] - - if symbol[self.index_symbol] == Alphabet.JOINT_VERTICAL \ - or symbol[self.index_symbol] == Alphabet.JOINT_HORIZONTAL: - - node.layer = 'output' - node.type = 'Oscillator' - - params = Params() - params.period = float(symbol[self.index_params][1]) - params.phase_offset = float(symbol[self.index_params][2]) - params.amplitude = float(symbol[self.index_params][3]) - node.params = params - self.phenotype._brain.params[node.id] = params - - # stacks oscillator if there are no sensors yet - if len(self.inputs_stack) == 0: - self.outputs_stack.append(node) - else: - # if it is the first oscillator ever - if len(self.outputs_stack) > 0: - self.outputs_stack = [node] - else: - self.outputs_stack.append(node) - - # connects oscillator to all sensors in the stack - for input_node in range(0, len(self.inputs_stack)): - self.inputs_stack[input_node].output_nodes.append(node) - node.input_nodes.append(self.inputs_stack[input_node]) - - connection = Connection() - connection.src = self.inputs_stack[input_node].id - connection.dst = node.id - if input_node == len(self.inputs_stack)-1: - connection.weight = node.weight - else: - connection.weight = float(self.inputs_stack[input_node].weight) - self.edges[connection.src, connection.dst] = connection - self.phenotype._brain.connections.append(connection) - self.inputs_stack = [self.inputs_stack[-1]] - - self.phenotype._brain.nodes[node.id] = node - - def add_imu_nodes(self): - for p in range(1, 7): - id = 'node-core'+str(p) - node = Node() - node.layer = 'input' - node.type = 'Input' - node.part_id = self.phenotype._body.id - node.id = id - self.phenotype._brain.nodes[id] = node - @staticmethod - def build_symbol(symbol, conf): + def build_symbol(symbol: Union[Alphabet, (Alphabet, List)], + conf: PlasticodingConfig + ) -> (Alphabet, List): """ Adds params for alphabet symbols (when it applies). :return: """ - index_symbol = 0 - index_params = 1 - - if symbol[index_symbol] is Alphabet.JOINT_HORIZONTAL \ - or symbol[index_symbol] is Alphabet.JOINT_VERTICAL: - - symbol[index_params] = [random.uniform(conf.weight_min, conf.weight_max), - random.uniform(conf.oscillator_param_min, - conf.oscillator_param_max), - random.uniform(conf.oscillator_param_min, - conf.oscillator_param_max), - random.uniform(conf.oscillator_param_min, - conf.oscillator_param_max)] - - if symbol[index_symbol] is Alphabet.SENSOR \ - or symbol[index_symbol] is Alphabet.ADD_EDGE \ - or symbol[index_symbol] is Alphabet.LOOP: - - symbol[index_params] = [random.uniform(conf.weight_min, conf.weight_max)] - - if symbol[index_symbol] is Alphabet.MUTATE_EDGE \ - or symbol[index_symbol] is Alphabet.MUTATE_AMP \ - or symbol[index_symbol] is Alphabet.MUTATE_PER \ - or symbol[index_symbol] is Alphabet.MUTATE_OFF: - - symbol[index_params] = [random.normalvariate(0, 1)] - - if symbol[index_symbol] is Alphabet.MOVE_REF_S \ - or symbol[index_symbol] is Alphabet.MOVE_REF_O: - + if type(symbol) is Alphabet: + symbol = Alphabet.wordify(symbol) + + if symbol[INDEX_SYMBOL] is Alphabet.JOINT_HORIZONTAL \ + or symbol[INDEX_SYMBOL] is Alphabet.JOINT_VERTICAL: + symbol[INDEX_PARAMS].clear() + symbol[INDEX_PARAMS].extend( + [random.uniform(conf.weight_min, conf.weight_max), + random.uniform(conf.oscillator_param_min, conf.oscillator_param_max), + random.uniform(conf.oscillator_param_min, conf.oscillator_param_max), + random.uniform(conf.oscillator_param_min, conf.oscillator_param_max)] + ) + + if symbol[INDEX_SYMBOL] is Alphabet.SENSOR \ + or symbol[INDEX_SYMBOL] is Alphabet.ADD_EDGE \ + or symbol[INDEX_SYMBOL] is Alphabet.LOOP: + symbol[INDEX_PARAMS].clear() + symbol[INDEX_PARAMS].append(random.uniform(conf.weight_min, conf.weight_max)) + + if symbol[INDEX_SYMBOL] is Alphabet.MUTATE_EDGE \ + or symbol[INDEX_SYMBOL] is Alphabet.MUTATE_AMP \ + or symbol[INDEX_SYMBOL] is Alphabet.MUTATE_PER \ + or symbol[INDEX_SYMBOL] is Alphabet.MUTATE_OFF: + symbol[INDEX_PARAMS].clear() + symbol[INDEX_PARAMS].append(random.normalvariate(0, 1)) + + if symbol[INDEX_SYMBOL] is Alphabet.MOVE_REF_S \ + or symbol[INDEX_SYMBOL] is Alphabet.MOVE_REF_O: intermediate_temp = random.normalvariate(0, 1) final_temp = random.normalvariate(0, 1) - symbol[index_params] = [math.ceil(math.sqrt(math.pow(intermediate_temp, 2))), - math.ceil(math.sqrt(math.pow(final_temp, 2)))] + symbol[INDEX_PARAMS].clear() + symbol[INDEX_PARAMS].append(math.ceil(math.sqrt(math.pow(intermediate_temp, 2)))) + symbol[INDEX_PARAMS].append(math.ceil(math.sqrt(math.pow(final_temp, 2)))) return symbol @@ -668,35 +150,3 @@ def __init__(self): self.input_nodes = [] self.output_nodes = [] self.params = None - - -from pyrevolve.genotype.plasticoding import initialization - - -class PlasticodingConfig: - def __init__(self, - initialization_genome=initialization.random_initialization, - e_max_groups=3, - oscillator_param_min=1, - oscillator_param_max=10, - weight_param_min=-1, - weight_param_max=1, - weight_min=-1, - weight_max=1, - axiom_w=Alphabet.CORE_COMPONENT, - i_iterations=3, - max_structural_modules=100, - robot_id=0 - ): - self.initialization_genome = initialization_genome - self.e_max_groups = e_max_groups - self.oscillator_param_min = oscillator_param_min - self.oscillator_param_max = oscillator_param_max - self.weight_param_min = weight_param_min - self.weight_param_max = weight_param_max - self.weight_min = weight_min - self.weight_max = weight_max - self.axiom_w = axiom_w - self.i_iterations = i_iterations - self.max_structural_modules = max_structural_modules - self.robot_id = robot_id diff --git a/pyrevolve/revolve_bot/body.py b/pyrevolve/revolve_bot/body.py index 2580e17fa9..d34de3854f 100644 --- a/pyrevolve/revolve_bot/body.py +++ b/pyrevolve/revolve_bot/body.py @@ -102,11 +102,7 @@ def generate(self): max_parts = self.max_parts if self.fix_parts \ else self.choose_max_parts() - root_part_type = self.choose_part( - parts=self.root_parts, - parent_part=None, - root_part=None, - root=True) + root_part_type = self.choose_part(self.root_parts, None, None, True) root_part = root_specs[root_part_type] body.root.id = "bodygen-root" body.root.type = root_part_type diff --git a/pyrevolve/revolve_bot/brain/__init__.py b/pyrevolve/revolve_bot/brain/__init__.py index 900e38923c..fbf045d2fb 100644 --- a/pyrevolve/revolve_bot/brain/__init__.py +++ b/pyrevolve/revolve_bot/brain/__init__.py @@ -1,4 +1,7 @@ from .base import Brain +from .bo_cpg import BrainCPGBO from .brain_nn import BrainNN +from .cpg import BrainCPG +from .cpg_target import BrainCPGTarget +from .cppn_cpg import BrainCPPNCPG from .rlpower_splines import BrainRLPowerSplines -from .bo_cpg import BrainCPGBO diff --git a/pyrevolve/revolve_bot/brain/base.py b/pyrevolve/revolve_bot/brain/base.py index b16ea73b8b..8938b3522e 100644 --- a/pyrevolve/revolve_bot/brain/base.py +++ b/pyrevolve/revolve_bot/brain/base.py @@ -2,10 +2,9 @@ class Brain(object): - @staticmethod def from_yaml(yaml_brain): - brain_type = yaml_brain['type'] + brain_type = yaml_brain["type"] if brain_type == pyrevolve.revolve_bot.brain.BrainNN.TYPE: return pyrevolve.revolve_bot.brain.BrainNN.from_yaml(yaml_brain) @@ -13,7 +12,14 @@ def from_yaml(yaml_brain): return pyrevolve.revolve_bot.brain.BrainRLPowerSplines.from_yaml(yaml_brain) elif brain_type == pyrevolve.revolve_bot.brain.BrainCPGBO.TYPE: return pyrevolve.revolve_bot.brain.BrainCPGBO.from_yaml(yaml_brain) + elif brain_type == pyrevolve.revolve_bot.brain.BrainCPG.TYPE: + return pyrevolve.revolve_bot.brain.BrainCPG.from_yaml(yaml_brain) + elif brain_type == pyrevolve.revolve_bot.brain.BrainCPPNCPG.TYPE: + return pyrevolve.revolve_bot.brain.BrainCPPNCPG.from_yaml(yaml_brain) + elif brain_type == pyrevolve.revolve_bot.brain.BrainCPGTarget.TYPE: + return pyrevolve.revolve_bot.brain.BrainCPGTarget.from_yaml(yaml_brain) else: + print("No matching brain type defined in yaml file.") return Brain() def to_yaml(self): diff --git a/pyrevolve/revolve_bot/brain/brain_nn.py b/pyrevolve/revolve_bot/brain/brain_nn.py index f58c0643ec..02cc08e2f2 100644 --- a/pyrevolve/revolve_bot/brain/brain_nn.py +++ b/pyrevolve/revolve_bot/brain/brain_nn.py @@ -109,10 +109,10 @@ def controller_sdf(self): for name, node in self.nodes.items(): assert(name == node.id) neuron = xml.etree.ElementTree.SubElement(controller, 'rv:neuron', { - 'layer': node.layer, - 'type': node.type, - 'id': node.id, - 'part_id': node.part_id, + 'layer': str(node.layer), + 'type': str(node.type), + 'id': str(node.id), + 'part_id': str(node.part_id), }) node_map[node.id] = neuron diff --git a/pyrevolve/revolve_bot/brain/cpg.py b/pyrevolve/revolve_bot/brain/cpg.py new file mode 100644 index 0000000000..90dc5e50e1 --- /dev/null +++ b/pyrevolve/revolve_bot/brain/cpg.py @@ -0,0 +1,105 @@ +""" +Note: Parameters are not set in this file. They are imported from the robot yaml-file, containing the +physical properties of the robot, as well as the brain (learner and controller) and the corresponding +parameters. +""" + +import xml.etree.ElementTree + +from .base import Brain + + +class BrainCPG(Brain): + TYPE = "cpg" + + def __init__(self): + # CPG hyper-parameters + self.abs_output_bound = None + self.use_frame_of_reference = "false" + self.output_signal_factor = "" + self.range_lb = None + self.range_ub = None + self.init_neuron_state = None + # Various + self.robot_size = None + self.n_learning_iterations = None + self.n_cooldown_iterations = None + self.load_brain = None + self.output_directory = None + self.run_analytics = None + self.reset_robot_position = None + self.reset_neuron_state_bool = None + self.reset_neuron_random = None + self.verbose = None + self.startup_time = None + self.evaluation_rate = None + self.weights = [] + + @staticmethod + def from_yaml(yaml_object): + BCPG = BrainCPG() + for my_type in ["controller", "learner"]: # , "meta"]: + try: + my_object = yaml_object[my_type] + for key, value in my_object.items(): + try: + setattr(BCPG, key, value) + except: + print("Couldn't set {}, {}", format(key, value)) + except: + print("Didn't load {} parameters".format(my_type)) + + return BCPG + + def to_yaml(self): + return { + "type": self.TYPE, + "controller": { + "abs_output_bound": self.abs_output_bound, + "reset_robot_position": self.reset_robot_position, + "reset_neuron_state_bool": self.reset_neuron_state_bool, + "reset_neuron_random": self.reset_neuron_random, + "load_brain": self.load_brain, + "use_frame_of_reference": self.use_frame_of_reference, + "run_analytics": self.run_analytics, + "init_neuron_state": self.init_neuron_state, + "output_directory": self.output_directory, + "verbose": self.verbose, + "range_lb": self.range_lb, + "range_ub": self.range_ub, + "output_signal_factor": self.output_signal_factor, + "startup_time": self.startup_time, + "weights": self.weights, + }, + } + + def learner_sdf(self): + return xml.etree.ElementTree.Element( + "rv:learner", + { + "type": "offline", + }, + ) + + def controller_sdf(self): + return xml.etree.ElementTree.Element( + "rv:controller", + { + "type": "cpg", + "abs_output_bound": str(self.abs_output_bound), + "reset_robot_position": str(self.reset_robot_position), + "reset_neuron_state_bool": str(self.reset_neuron_state_bool), + "reset_neuron_random": str(self.reset_neuron_random), + "load_brain": str(self.load_brain), + "use_frame_of_reference": str(self.use_frame_of_reference), + "run_analytics": str(self.run_analytics), + "init_neuron_state": str(self.init_neuron_state), + "output_directory": str(self.output_directory), + "verbose": str(self.verbose), + "range_lb": str(self.range_lb), + "range_ub": str(self.range_ub), + "output_signal_factor": str(self.output_signal_factor), + "startup_time": str(self.startup_time), + "weights": ";".join(str(x) for x in self.weights), + }, + ) diff --git a/pyrevolve/revolve_bot/brain/cpg_target.py b/pyrevolve/revolve_bot/brain/cpg_target.py new file mode 100644 index 0000000000..9e1f76206f --- /dev/null +++ b/pyrevolve/revolve_bot/brain/cpg_target.py @@ -0,0 +1,33 @@ +import xml.etree.ElementTree +from typing import Tuple + +from .cpg import BrainCPG + + +# Extends BrainCPG by including a Genome +class BrainCPGTarget(BrainCPG): + TYPE = "cpg-target" + + # unit vector, target direction + target: Tuple[float, float, float] = (0, 0, 0) + + @staticmethod + def from_yaml(yaml_object): + brain = BrainCPGTarget() + for my_type in ["controller"]: + try: + my_object = yaml_object[my_type] + for key, value in my_object.items(): + try: + setattr(brain, key, value) + except: + print("Couldn't set {}, {}", format(key, value)) + except: + print("Didn't load {} parameters".format(my_type)) + return brain + + def controller_sdf(self): + sdf = super().controller_sdf() + sdf.set("target", ";".join(str(x) for x in self.target)) + sdf.set("type", "cpg-target") + return sdf diff --git a/pyrevolve/revolve_bot/brain/cppn_cpg.py b/pyrevolve/revolve_bot/brain/cppn_cpg.py new file mode 100644 index 0000000000..86c4fb07fe --- /dev/null +++ b/pyrevolve/revolve_bot/brain/cppn_cpg.py @@ -0,0 +1,129 @@ +import sys +import xml.etree.ElementTree +import multineat + +from .cpg import BrainCPG + + +# Extends BrainCPG by including a Genome +class BrainCPPNCPG(BrainCPG): + TYPE = 'cppn-cpg' + + def __init__(self, neat_genome): + super().__init__() + self.genome = neat_genome + self.weights = None + + def to_yaml(self): + obj = super().to_yaml() + obj['controller']['cppn'] = self.genome.Serialize() + return obj + + @staticmethod + def from_yaml(yaml_object): + cppn_genome = multineat.Genome() + cppn_genome.Deserialize(yaml_object['controller']['cppn'].replace('inf', str(sys.float_info.max))) + del yaml_object['controller']['cppn'] + + BCPG = BrainCPPNCPG(cppn_genome) + for my_type in ["controller", "learner"]: #, "meta"]: + try: + my_object = yaml_object[my_type] + for key, value in my_object.items(): + try: + setattr(BCPG, key, value) + except: + print("Couldn't set {}, {}", format(key, value)) + except: + print("Didn't load {} parameters".format(my_type)) + + return BCPG + + def controller_sdf(self): + controller = xml.etree.ElementTree.Element('rv:controller', { + 'type': 'cppn-cpg', + 'abs_output_bound': str(self.abs_output_bound), + 'reset_robot_position': str(self.reset_robot_position), + 'reset_neuron_state_bool': str(self.reset_neuron_state_bool), + 'reset_neuron_random': str(self.reset_neuron_random), + 'load_brain': str(self.load_brain), + 'use_frame_of_reference': str(self.use_frame_of_reference), + 'run_analytics': str(self.run_analytics), + 'init_neuron_state': str(self.init_neuron_state), + 'output_directory': str(self.output_directory), + 'verbose': str(self.verbose), + 'range_lb': str(self.range_lb), + 'range_ub': str(self.range_ub), + 'signal_factor_all': str(self.signal_factor_all), + 'signal_factor_mid': str(self.signal_factor_mid), + 'signal_factor_left_right': str(self.signal_factor_left_right), + 'startup_time': str(self.startup_time), + }) + controller.append(self.genome_sdf()) + return controller + + def genome_sdf(self): + import multineat + + params = multineat.Parameters() + params.PopulationSize = 100 + params.DynamicCompatibility = True + params.NormalizeGenomeSize = True + params.WeightDiffCoeff = 0.1 + params.CompatTreshold = 2.0 + params.YoungAgeTreshold = 15 + params.SpeciesMaxStagnation = 15 + params.OldAgeTreshold = 35 + params.MinSpecies = 2 + params.MaxSpecies = 10 + params.RouletteWheelSelection = False + params.RecurrentProb = 0.0 + params.OverallMutationRate = 1.0 + + params.ArchiveEnforcement = False + + params.MutateWeightsProb = 0.05 + + params.WeightMutationMaxPower = 0.5 + params.WeightReplacementMaxPower = 8.0 + params.MutateWeightsSevereProb = 0.0 + params.WeightMutationRate = 0.25 + params.WeightReplacementRate = 0.9 + + params.MaxWeight = 8 + + params.MutateAddNeuronProb = 0.001 + params.MutateAddLinkProb = 0.3 + params.MutateRemLinkProb = 0.0 + + params.MinActivationA = 4.9 + params.MaxActivationA = 4.9 + + params.ActivationFunction_SignedSigmoid_Prob = 0.0 + params.ActivationFunction_UnsignedSigmoid_Prob = 1.0 + params.ActivationFunction_Tanh_Prob = 0.0 + params.ActivationFunction_SignedStep_Prob = 0.0 + + params.CrossoverRate = 0.0 + params.MultipointCrossoverRate = 0.0 + params.SurvivalRate = 0.2 + + params.MutateNeuronTraitsProb = 0 + params.MutateLinkTraitsProb = 0 + + params.AllowLoops = True + params.AllowClones = True + + params.ClearNeuronTraitParameters() + params.ClearLinkTraitParameters() + params.ClearGenomeTraitParameters() + + assert(self.genome is not None) + serialized_genome = self.genome.Serialize() + + element = xml.etree.ElementTree.Element('rv:genome', { + 'type': 'CPPN' + }) + element.text = serialized_genome + + return element diff --git a/pyrevolve/revolve_bot/brain/fixed_angle.py b/pyrevolve/revolve_bot/brain/fixed_angle.py new file mode 100644 index 0000000000..0309103880 --- /dev/null +++ b/pyrevolve/revolve_bot/brain/fixed_angle.py @@ -0,0 +1,27 @@ +import xml.etree.ElementTree +from pyrevolve.revolve_bot.brain import Brain + + +class FixedAngleBrain(Brain): + TYPE = 'fixed-angle' + + def __init__(self, angle: float): + self._angle = angle + + @staticmethod + def from_yaml(yaml_object): + return FixedAngleBrain(float(yaml_object['angle'])) + + def to_yaml(self): + return { + 'type': self.TYPE + } + + def learner_sdf(self): + return xml.etree.ElementTree.Element('rv:learner', {'type': 'offline'}) + + def controller_sdf(self): + return xml.etree.ElementTree.Element('rv:controller', { + 'type': 'fixed-angle', + 'angle': str(self._angle), + }) diff --git a/pyrevolve/revolve_bot/measure/measure_body.py b/pyrevolve/revolve_bot/measure/measure_body_2d.py similarity index 97% rename from pyrevolve/revolve_bot/measure/measure_body.py rename to pyrevolve/revolve_bot/measure/measure_body_2d.py index 76f984e1fc..c0c7a15f6a 100644 --- a/pyrevolve/revolve_bot/measure/measure_body.py +++ b/pyrevolve/revolve_bot/measure/measure_body_2d.py @@ -1,11 +1,11 @@ import math -from ..render.render import Render -from ..render.grid import Grid -from ..revolve_module import ActiveHingeModule, BrickModule, TouchSensorModule, BrickSensorModule, CoreModule -from ...custom_logging.logger import logger +from pyrevolve.custom_logging.logger import logger +from pyrevolve.revolve_bot.render.render import Render +from pyrevolve.revolve_bot.render.grid import Grid +from pyrevolve.revolve_bot.revolve_module import ActiveHingeModule, BrickModule, TouchSensorModule, BrickSensorModule, CoreModule -class MeasureBody: +class MeasureBody2D: def __init__(self, body): self.body = body diff --git a/pyrevolve/revolve_bot/measure/measure_body_3d.py b/pyrevolve/revolve_bot/measure/measure_body_3d.py new file mode 100644 index 0000000000..02206db228 --- /dev/null +++ b/pyrevolve/revolve_bot/measure/measure_body_3d.py @@ -0,0 +1,544 @@ +import math +from pyrevolve.custom_logging.logger import logger +from pyrevolve.revolve_bot.revolve_module import ActiveHingeModule, BrickModule, TouchSensorModule, BrickSensorModule, \ + CoreModule + + +class MeasureBody3D: + def __init__(self, body): + self.body = body + + # Absolute branching + self.branching_modules_count = None + # Relative branching + self.branching = None + # Absolute number of limbs + self.extremities = None + # Relative number of limbs + self.limbs = None + # Absolute length of limbs + self.extensiveness = None + # Relative length of limbs + self.length_of_limbs = None + # Coverage + self.coverage = None + # Relative number of effective active joints + self.joints = None + # Absolute number of effective active joints + self.active_hinges_count = None + # Proportion + self.proportion = None + # Width + self.width = None + # Height + self.height = None + # Z depth + self.z_depth = None + # Absolute size + self.absolute_size = None + # Relative size in respect of the max body size `self.max_permitted_modules` + self.size = None + # Proportion of sensor vs empty slots + self.sensors = None + # Body symmetry in the xy plane + self.symmetry = None + # Number of active joints + self.hinge_count = None + # Number of bricks + self.brick_count = None + # Number of brick sensors + self.brick_sensor_count = None + # Number of touch sensors + self.touch_sensor_count = None + # Number of free slots + self.free_slots = None + # Ratio of the height over the root of the area of the base + self.height_base_ratio = None + # Maximum number of modules allowed (sensors excluded) + self.max_permitted_modules = None + # Vertical symmetry + self.symmetry_vertical = None + # Base model density + self.base_density = None + # Bottom layer of the robot + self.bottom_layer = None + + def count_branching_bricks(self, module=None, init=True): + """ + Count amount of fully branching modules in body + """ + try: + if init: + self.branching_modules_count = 0 + if module is None: + module = self.body + + if module.has_children(): + children_count = 0 + for _slot, child_module in module.iter_children(): + if child_module is None: + continue + if not isinstance(child_module, TouchSensorModule) \ + and not isinstance(child_module, BrickSensorModule): + children_count += 1 + self.count_branching_bricks(child_module, False) + if (isinstance(module, BrickModule) and children_count == 3) or \ + (isinstance(module, CoreModule) and children_count == 4): + self.branching_modules_count += 1 + except Exception as e: + logger.exception(f'Exception: {e}. \nFailed counting branching bricks') + + def measure_branching(self): + """ + Measure branching by dividing fully branching by possible branching modules + """ + if self.absolute_size is None: + self.measure_absolute_size() + if self.branching_modules_count is None: + self.count_branching_bricks() + if self.branching_modules_count == 0 or self.absolute_size < 5: + self.branching = 0 + return self.branching + practical_limit_branching_bricks = math.floor((self.absolute_size - 2) / 3) + self.branching = self.branching_modules_count / practical_limit_branching_bricks + return self.branching + + def calculate_extremities_extensiveness(self, module=None, extremities=False, extensiveness=False, init=True): + """ + Calculate extremities or extensiveness in body + @param extremities: calculate extremities in body if true + @param extensiveness: calculate extensiveness in body if true + """ + try: + if module is None: + module = self.body + if init and extremities: + self.extremities = 0 + if init and extensiveness: + self.extensiveness = 0 + + children_count = 0 + for _slot, child_module in module.iter_children(): + if child_module is None: + continue + if not isinstance(child_module, TouchSensorModule): + children_count += 1 + self.calculate_extremities_extensiveness(child_module, extremities, extensiveness, False) + if children_count == 0 \ + and not (isinstance(module, CoreModule) or isinstance(module, TouchSensorModule)) \ + and extremities: + self.extremities += 1 + if children_count == 1 \ + and not (isinstance(module, CoreModule) or isinstance(module, TouchSensorModule)) \ + and extensiveness: + self.extensiveness += 1 + except Exception as e: + logger.exception(f'Exception: {e}. \nFailed calculating extremities or extensiveness') + + def measure_limbs(self): + """ + Measure limbs + :return: + """ + if self.absolute_size is None: + self.measure_absolute_size() + practical_limit_limbs = None + if self.absolute_size < 6: + practical_limit_limbs = self.absolute_size - 1 + else: + practical_limit_limbs = 2 * math.floor((self.absolute_size - 6) / 3) + ((self.absolute_size - 6) % 3) + 4 + + if self.extremities is None: + self.calculate_extremities_extensiveness(None, True, False) + if self.extremities == 0: + self.limbs = 0 + return 0 + self.limbs = self.extremities / practical_limit_limbs + return self.limbs + + def measure_length_of_limbs(self): + """ + Measure length of limbs + :return: + """ + if self.absolute_size is None: + self.measure_absolute_size() + if self.extensiveness is None: + self.calculate_extremities_extensiveness(None, False, True) + if self.absolute_size < 3: + self.length_of_limbs = 0 + return self.length_of_limbs + practical_limit_extensiveness = self.absolute_size - 2 + self.length_of_limbs = self.extensiveness / practical_limit_extensiveness + return self.length_of_limbs + + def collect_all_coordinates(self, module=None): + module = module or self.body + coordinates = set() + coordinates.add(module.substrate_coordinates) + if module.has_children(): + for _slot, child_module in module.iter_children(): + if child_module is None: + continue + children_coordinates = self.collect_all_coordinates(child_module) + coordinates = coordinates.union(children_coordinates) + return coordinates + + def measure_symmetry(self): + """ + Measure symmetry in the xy plane of the robot. + """ + try: + coordinates = self.collect_all_coordinates() + + horizontal_mirrored = 0 + horizontal_total = 0 + vertical_mirrored = 0 + vertical_total = 0 + # Calculate xy symmetry in body + for position in coordinates: + if position[0] != 0: + horizontal_total += 1 + if (-position[0], position[1], position[2]) in coordinates: + horizontal_mirrored += 1 + if position[1] != 0: + vertical_total += 1 + if (position[0], -position[1], position[2]) in coordinates: + vertical_mirrored += 1 + + horizontal_symmetry = horizontal_mirrored / horizontal_total if horizontal_mirrored > 0 else 0 + vertical_symmetry = vertical_mirrored / vertical_total if vertical_mirrored > 0 else 0 + + self.symmetry = max(horizontal_symmetry, vertical_symmetry) + return self.symmetry + + except Exception as e: + logger.exception(f'Exception: {e}. \nFailed measuring symmetry') + + def measure_vertical_symmetry(self): + """ + Measure the vertical symmetry of the robot. + """ + try: + coordinates = self.collect_all_coordinates() + + vertical_mirrored = 0 + vertical_total = 0 + # Calculate vertical symmetry in body + for position in coordinates: + if position[2] != 0: + vertical_total += 1 + if (position[0], position[1], -position[2]) in coordinates: + vertical_mirrored += 1 + + vertical_symmetry = vertical_mirrored / vertical_total if vertical_mirrored > 0 else 0 + + self.symmetry_vertical = vertical_symmetry + return self.symmetry_vertical + + except Exception as e: + logger.exception(f'Exception: {e}. \nFailed measuring vertical symmetry') + + def measure_coverage(self): + """ + Measure the coverage of the robot, specified by the amount of modules + divided by the spanning surface of the robot (excluding sensors) + :return: + """ + if self.absolute_size is None: + self.measure_absolute_size() + if self.width is None or self.height is None or self.z_depth is None: + self.measure_width_height_zdepth() + if self.width * self.height * self.z_depth != 0: + self.coverage = self.absolute_size / (self.width * self.height * self.z_depth) + else: + self.coverage = 0 + return self.coverage + + def find_bottom_layer(self): + if self.bottom_layer is None: + self.bottom_layer = self._find_bottom_layer(self.body) + + return self.bottom_layer + + def _find_bottom_layer(self, module, _bottom_layer=0): + my_bottom_layer = module.substrate_coordinates[2] + if my_bottom_layer < _bottom_layer: + _bottom_layer = my_bottom_layer + if module.has_children(): + for _slot, child_module in module.iter_children(): + if child_module is None: + continue + child_bottom_layer = self._find_bottom_layer(child_module) + if child_bottom_layer < _bottom_layer: + _bottom_layer = child_bottom_layer + return _bottom_layer + + def measure_base_density(self): + """ + Measure the coverage of the robot, specified by the amount of modules + divided by the spanning surface of the robot (excluding sensors) + :return: + """ + bottom_layer = self.find_bottom_layer() + + hinges, bricks, _brick_sensors, _touch_sensors \ + = self.count_blocks(_filter=lambda module: module.substrate_coordinates[2] != bottom_layer) + + size_first_layer = hinges + bricks + + if self.width is None or self.height is None or self.z_depth is None: + self.measure_width_height_zdepth() + + self.base_density = size_first_layer / (self.width * self.height) + return self.base_density + + def count_active_hinges(self, module=None, init=True): + """ + Count amount of active hinges + """ + try: + if module is None: + module = self.body + if init: + self.active_hinges_count = 0 + if module.has_children(): + if isinstance(module, ActiveHingeModule): + self.active_hinges_count += 1 + for _slot, child_module in module.iter_children(): + if child_module is None: + continue + self.count_active_hinges(child_module, False) + except Exception as e: + logger.exception(f'Exception: {e}. \nFailed calculating count') + + def measure_joints(self): + """ + Measure joints, characterizing the possible amount of joints + :return: + """ + if self.absolute_size is None: + self.measure_absolute_size() + if self.active_hinges_count is None: + self.count_active_hinges() + if self.active_hinges_count == 0 or self.absolute_size < 3: + self.joints = 0 + return self.joints + practical_limit_active_hinges = self.absolute_size - 2 + self.joints = self.active_hinges_count / practical_limit_active_hinges + return self.joints + + def measure_proportion(self): + """ + Measure proportion, specified by the 2d ratio of the body + :return: + """ + if self.width is None or self.height is None: + self.measure_width_height_zdepth() + if self.width < self.height: + self.proportion = self.width / self.height + else: + if self.width != 0: + self.proportion = self.height / self.width + return self.proportion + + def measure_height_base_ratio(self): + """ + Provides a ratio of the height divided by the length of the side of a square with equivalent area to the base. + """ + + if self.width is None or self.height is None or self.z_depth is None: + self.measure_width_height_zdepth() + if self.width * self.height != 0: + self.height_base_ratio = self.z_depth / math.sqrt(self.width * self.height) + else: + self.height_base_ratio = 0 + return self.height_base_ratio + + def count_free_slots(self, module=None, init=True): + """ + Count amount of free slots in body + """ + if module is None: + module = self.body + if init: + self.free_slots = 0 + children_count = 0 + for _slot, child_module in module.iter_children(): + if child_module is None: + continue + if not isinstance(child_module, TouchSensorModule): + children_count += 1 + self.count_free_slots(child_module, False) + if isinstance(module, CoreModule): + self.free_slots += (4 - children_count) + if isinstance(module, BrickModule): + self.free_slots += (3 - children_count) + + def measure_sensors(self, module=None): + """ + Measurement describes the proportion of free slots that contain sensors + """ + if self.free_slots is None: + self.count_free_slots() + if self.free_slots == 0: + self.free_slots = 0.0001 + self.sensors = self.touch_sensor_count / self.free_slots + return self.sensors + + def measure_absolute_size(self, module=None): + """ + Count total amount of modules in body excluding sensors + :return: + """ + try: + if self.absolute_size is None: + self.calculate_count() + self.absolute_size = self.brick_count + self.hinge_count + 1 + return self.absolute_size + except Exception as e: + logger.exception(f'Exception: {e}. \nFailed measuring absolute size') + + def calculate_count(self): + """ + Count amount of modules for each distinct type + """ + try: + self.hinge_count = 0 + self.brick_count = 0 + self.brick_sensor_count = 0 + self.touch_sensor_count = 0 + + _hinge_count, _brick_count, _brick_sensor_count, _touch_sensor_count \ + = self.count_blocks() + + self.hinge_count = _hinge_count + self.brick_count = _brick_count + self.brick_sensor_count = _brick_sensor_count + self.touch_sensor_count = _touch_sensor_count + + except Exception as e: + logger.exception(f'Exception: {e}. \nFailed calculating count') + + def count_blocks(self, + module=None, + _filter=lambda module: False): + """ + Count amount of modules for each distinct type + """ + hinge_count = 0 + brick_count = 0 + brick_sensor_count = 0 + touch_sensor_count = 0 + + if module is None: + module = self.body + elif isinstance(module, ActiveHingeModule) and not _filter(module): + hinge_count += 1 + elif isinstance(module, BrickModule) and not _filter(module): + brick_count += 1 + elif isinstance(module, BrickSensorModule) and not _filter(module): + brick_sensor_count += 1 + elif isinstance(module, TouchSensorModule) and not _filter(module): + touch_sensor_count += 1 + elif isinstance(module, CoreModule): + raise RuntimeError("Core module should never be passed in") + + if module.has_children(): + for _slot, child_module in module.iter_children(): + if child_module is None: + continue + _hinge_count, _brick_count, _brick_sensor_count, _touch_sensor_count \ + = self.count_blocks(child_module, _filter) + hinge_count += _hinge_count + brick_count += _brick_count + brick_sensor_count += _brick_sensor_count + touch_sensor_count += _touch_sensor_count + + return hinge_count, brick_count, brick_sensor_count, touch_sensor_count + + def measure_width_height_zdepth(self): + """ + Measure width and height of body, excluding sensors + """ + try: + coordinates = self.collect_all_coordinates() + self.width = 0 + self.height = 0 + self.z_depth = 0 + min_x = 0 + max_x = 0 + min_y = 0 + max_y = 0 + min_z = 0 + max_z = 0 + + for coordinate in coordinates: + min_x = coordinate[0] if coordinate[0] < min_x else min_x + max_x = coordinate[0] if coordinate[0] > max_x else max_x + min_y = coordinate[1] if coordinate[1] < min_y else min_y + max_y = coordinate[1] if coordinate[1] > max_y else max_y + min_z = coordinate[2] if coordinate[2] < min_z else min_z + max_z = coordinate[2] if coordinate[2] > max_z else max_z + + self.width = abs(min_x - max_x) + 1 + self.height = abs(min_y - max_y) + 1 + self.z_depth = abs(min_z - max_z) + 1 + except Exception as e: + logger.exception(f'Exception: {e}. \nFailed measuring width, height and length') + + def measure_all(self): + """ + Perform all measurements + :return: + """ + self.measure_limbs() + self.measure_length_of_limbs() + self.measure_width_height_zdepth() + self.measure_absolute_size() + self.measure_proportion() + self.measure_joints() + self.measure_coverage() + self.measure_symmetry() + self.measure_vertical_symmetry() + self.measure_branching() + self.measure_sensors() + self.measure_height_base_ratio() + self.measure_base_density() + self.find_bottom_layer() + return self.measurements_to_dict() + + def measurements_to_dict(self): + """ + Return dict of all measurements + :return: + """ + return { + 'branching': self.branching, + 'branching_modules_count': self.branching_modules_count, + 'limbs': self.limbs, + 'extremities': self.extremities, + 'length_of_limbs': self.length_of_limbs, + 'extensiveness': self.extensiveness, + 'coverage': self.coverage, + 'joints': self.joints, + 'hinge_count': self.hinge_count, + 'active_hinges_count': self.active_hinges_count, + 'brick_count': self.brick_count, + 'touch_sensor_count': self.touch_sensor_count, + 'brick_sensor_count': self.brick_sensor_count, + 'proportion': self.proportion, + 'width': self.width, + 'height': self.height, + 'z_depth': self.z_depth, + 'absolute_size': self.absolute_size, + 'sensors': self.sensors, + 'symmetry': self.symmetry, + 'vertical_symmetry': self.symmetry_vertical, + 'height_base_ratio': self.height_base_ratio, + 'base_density': self.base_density, + 'bottom_layer': self.bottom_layer, + } + + def __repr__(self): + return self.measurements_to_dict().__repr__() diff --git a/pyrevolve/revolve_bot/neural_net.py b/pyrevolve/revolve_bot/neural_net.py index 6f4b86bf4e..db5f2c25f7 100644 --- a/pyrevolve/revolve_bot/neural_net.py +++ b/pyrevolve/revolve_bot/neural_net.py @@ -7,7 +7,7 @@ import random import itertools -from pyrevolve.spec import NeuralNetwork +from pyrevolve.angle.representation import Neuron, NeuralConnection, NeuralNetwork from pyrevolve.spec import NeuralNetImplementation from pyrevolve.spec import BodyImplementation @@ -90,7 +90,8 @@ def generate(self, inputs, outputs, part_ids=None, num_hidden=None,): # Initialize network interface, i.e. inputs and outputs for layer, ids in (("input", inputs), ("output", outputs)): for neuron_id in ids: - neuron = net.neuron.add() + net.neuron.append(Neuron()) + neuron = net.neuron[-1] neuron.id = neuron_id neuron.layer = layer @@ -104,7 +105,8 @@ def generate(self, inputs, outputs, part_ids=None, num_hidden=None,): num_hidden = self.choose_num_hidden() if num_hidden is None \ else num_hidden for i in range(num_hidden): - neuron = net.neuron.add() + net.neuron.append(Neuron()) + neuron = net.neuron[-1] neuron.id = 'brian-gen-hidden-{}'.format(len(hidden)) # Assign a part ID to each hidden neuron, provided we @@ -128,10 +130,7 @@ def generate(self, inputs, outputs, part_ids=None, num_hidden=None,): weight = self.choose_weight() - conn = net.connection.add() - conn.src = src - conn.dst = dst - conn.weight = weight + net.connection.append(NeuralConnection(src, dst, [], weight)) return net @@ -169,7 +168,7 @@ def initialize_neuron(spec, neuron): :return: """ # Initialize random parameters - spec.set_parameters(neuron.param, spec.get_random_parameters()) + neuron.param = spec.get_random_parameters() def choose_neuron_type(self, layer): """ diff --git a/pyrevolve/revolve_bot/render/canvas.py b/pyrevolve/revolve_bot/render/canvas.py index 4b01b73089..4364deb065 100644 --- a/pyrevolve/revolve_bot/render/canvas.py +++ b/pyrevolve/revolve_bot/render/canvas.py @@ -1,6 +1,13 @@ +from __future__ import annotations + import cairo import math +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Union, List, Any + + class Canvas: # Current position of last drawn element x_pos = 0 @@ -21,8 +28,7 @@ class Canvas: # Rotating orientation in regard to parent module rotating_orientation = 0 - - def __init__(self, width, height, scale): + def __init__(self, width: int, height: int, scale: int): """Instantiate context and surface""" self.surface = cairo.ImageSurface(cairo.FORMAT_ARGB32, width*scale, height*scale) context = cairo.Context(self.surface) @@ -32,24 +38,24 @@ def __init__(self, width, height, scale): self.height = height self.scale = scale - - def get_position(self): + def get_position(self) -> (int, int): """Return current position on x and y axis""" return [Canvas.x_pos, Canvas.y_pos] - def set_position(self, x, y): + def set_position(self, x: int, y: int): """Set position of x and y axis""" Canvas.x_pos = x Canvas.y_pos = y - def set_orientation(self, orientation): + def set_orientation(self, orientation: int) -> bool: """Set new orientation of robot""" if orientation in [0, 1, 2, 3]: Canvas.orientation = orientation + return True else: return False - def calculate_orientation(self): + def calculate_orientation(self) -> None: """Calculate new orientation based on current orientation and last movement direction""" if (Canvas.previous_move == -1 or (Canvas.previous_move == 1 and Canvas.orientation == 1) or @@ -73,7 +79,7 @@ def calculate_orientation(self): (Canvas.previous_move == 2 and Canvas.orientation == 0)): self.set_orientation(3) - def move_by_slot(self, slot): + def move_by_slot(self, slot: int) -> None: """Move in direction by slot id""" if slot == 0: self.move_down() @@ -83,8 +89,10 @@ def move_by_slot(self, slot): self.move_right() elif slot == 3: self.move_left() + else: + RuntimeError("Slot can only be 0,1,2 or 3") - def move_right(self): + def move_right(self) -> None: """Set position one to the right in correct orientation""" if Canvas.orientation == 1: Canvas.x_pos += 1 @@ -96,7 +104,7 @@ def move_right(self): Canvas.y_pos -= 1 Canvas.previous_move = 2 - def move_left(self): + def move_left(self) -> None: """Set position one to the left""" if Canvas.orientation == 1: Canvas.x_pos -= 1 @@ -108,7 +116,7 @@ def move_left(self): Canvas.y_pos += 1 Canvas.previous_move = 3 - def move_up(self): + def move_up(self) -> None: """Set position one upwards""" if Canvas.orientation == 1: Canvas.y_pos -= 1 @@ -120,7 +128,7 @@ def move_up(self): Canvas.x_pos -= 1 Canvas.previous_move = 1 - def move_down(self): + def move_down(self) -> None: """Set position one downwards""" if Canvas.orientation == 1: Canvas.y_pos += 1 @@ -132,7 +140,7 @@ def move_down(self): Canvas.x_pos += 1 Canvas.previous_move = 0 - def move_back(self): + def move_back(self) -> None: """Move back to previous state on canvas""" if len(Canvas.movement_stack) > 1: Canvas.movement_stack.pop() @@ -142,7 +150,7 @@ def move_back(self): Canvas.orientation = last_movement[2] Canvas.rotating_orientation = last_movement[3] - def sign_id(self, mod_id): + def sign_id(self, mod_id: Union[int, List[Any]]) -> None: """Sign module with the id on the upper left corner of block""" self.context.set_font_size(0.3) self.context.move_to(Canvas.x_pos, Canvas.y_pos + 0.4) @@ -154,7 +162,7 @@ def sign_id(self, mod_id): self.context.show_text(mod_id) self.context.stroke() - def draw_controller(self, mod_id): + def draw_controller(self, mod_id) -> None: """Draw a controller (yellow) in the middle of the canvas""" self.context.rectangle(Canvas.x_pos, Canvas.y_pos, 1, 1) self.context.set_source_rgb(255, 255, 0) @@ -165,14 +173,14 @@ def draw_controller(self, mod_id): self.sign_id(mod_id) Canvas.movement_stack.append([Canvas.x_pos, Canvas.y_pos, Canvas.orientation, Canvas.rotating_orientation]) - def draw_hinge(self, mod_id): - """Draw a hinge (blue) on the previous object""" + def draw_hinge(self, mod_id) -> None: + """Draw a hinge (green) on the previous object""" self.context.rectangle(Canvas.x_pos, Canvas.y_pos, 1, 1) if (Canvas.rotating_orientation % 180 == 0): - self.context.set_source_rgb(1.0, 0.4, 0.4) + self.context.set_source_rgb(0, 1.0, 0) else: - self.context.set_source_rgb(1, 0, 0) + self.context.set_source_rgb(0, 1.0, 0) self.context.fill_preserve() self.context.set_source_rgb(0, 0, 0) self.context.set_line_width(0.01) @@ -181,8 +189,8 @@ def draw_hinge(self, mod_id): self.sign_id(mod_id) Canvas.movement_stack.append([Canvas.x_pos, Canvas.y_pos, Canvas.orientation, Canvas.rotating_orientation]) - def draw_module(self, mod_id): - """Draw a module (red) on the previous object""" + def draw_module(self, mod_id) -> None: + """Draw a module (blue) on the previous object""" self.context.rectangle(Canvas.x_pos, Canvas.y_pos, 1, 1) self.context.set_source_rgb(0, 0, 1) self.context.fill_preserve() @@ -193,7 +201,7 @@ def draw_module(self, mod_id): self.sign_id(mod_id) Canvas.movement_stack.append([Canvas.x_pos, Canvas.y_pos, Canvas.orientation, Canvas.rotating_orientation]) - def calculate_sensor_rectangle_position(self): + def calculate_sensor_rectangle_position(self) -> (float, float, float, float): """Calculate squeezed sensor rectangle position based on current orientation and last movement direction""" if (Canvas.previous_move == -1 or (Canvas.previous_move == 1 and Canvas.orientation == 1) or @@ -217,14 +225,14 @@ def calculate_sensor_rectangle_position(self): (Canvas.previous_move == 2 and Canvas.orientation == 0)): return Canvas.x_pos + 0.9, Canvas.y_pos, 0.1, 1 - def save_sensor_position(self): + def save_sensor_position(self) -> None: """Save sensor position in list""" x, y, x_scale, y_scale = self.calculate_sensor_rectangle_position() Canvas.sensors.append([x, y, x_scale, y_scale]) self.calculate_orientation() Canvas.movement_stack.append([Canvas.x_pos, Canvas.y_pos, Canvas.orientation, Canvas.rotating_orientation]) - def draw_sensors(self): + def draw_sensors(self) -> None: """Draw all sensors""" for sensor in Canvas.sensors: self.context.rectangle(sensor[0], sensor[1], sensor[2], sensor[3]) @@ -234,7 +242,7 @@ def draw_sensors(self): self.context.set_line_width(0.01) self.context.stroke() - def calculate_connector_to_parent_position(self): + def calculate_connector_to_parent_position(self) -> (float, float): """Calculate position of connector node on canvas""" parent = Canvas.movement_stack[-2] parent_orientation = parent[2] @@ -264,7 +272,7 @@ def calculate_connector_to_parent_position(self): # Connector is on bottom of parent return parent[0] + 0.5, parent[1] + 1 - def draw_connector_to_parent(self): + def draw_connector_to_parent(self) -> None: """Draw a circle between child and parent""" x, y = self.calculate_connector_to_parent_position() self.context.arc(x, y, 0.1, 0, math.pi*2) @@ -274,11 +282,11 @@ def draw_connector_to_parent(self): self.context.set_line_width(0.01) self.context.stroke() - def save_png(self, file_name): + def save_png(self, file_name: str) -> None: """Store image representation of canvas""" - self.surface.write_to_png('%s' % file_name) + self.surface.write_to_png(file_name) - def reset_canvas(self): + def reset_canvas(self) -> None: """Reset canvas variables to default values""" Canvas.x_pos = 0 Canvas.y_pos = 0 diff --git a/pyrevolve/revolve_bot/render/render.py b/pyrevolve/revolve_bot/render/render.py index 1d943553e7..9781064cd0 100644 --- a/pyrevolve/revolve_bot/render/render.py +++ b/pyrevolve/revolve_bot/render/render.py @@ -95,5 +95,5 @@ def render_robot(self, body, image_path): cv.reset_canvas() self.grid.reset_grid() - except Exception as e: - logger.exception('Could not render robot and save image file') + except Exception: + logger.exception(f'Could not render robot and save image file {image_path}') diff --git a/pyrevolve/revolve_bot/revolve_bot.py b/pyrevolve/revolve_bot/revolve_bot.py index f9b59456dc..725db0130d 100644 --- a/pyrevolve/revolve_bot/revolve_bot.py +++ b/pyrevolve/revolve_bot/revolve_bot.py @@ -1,24 +1,32 @@ """ Revolve body generator based on RoboGen framework """ +from __future__ import annotations + +import os +import math + import yaml -import traceback from collections import OrderedDict from collections import deque +import numpy as np from pyrevolve import SDF - +from pyrevolve.custom_logging.logger import logger from .revolve_module import CoreModule, TouchSensorModule, Orientation -from .revolve_module import Orientation +from .revolve_module import Orientation, rotate_matrix_x_axis, rotate_matrix_z_axis from .brain import Brain, BrainNN - from .render.render import Render from .render.brain_graph import BrainGraph -from .measure.measure_body import MeasureBody +from .measure.measure_body_3d import MeasureBody3D from .measure.measure_brain import MeasureBrain -from ..custom_logging.logger import logger -import os +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Optional, AnyStr, Union, Dict, Iterable + from .revolve_module import RevolveModule + from pyrevolve.tol.manage.measures import BehaviouralMeasurements + class RevolveBot: """ @@ -27,34 +35,36 @@ class RevolveBot: a robot's sdf mode """ - def __init__(self, _id=None, self_collide=True): - self._id = _id - self._body = None - self._brain = None - self._morphological_measurements = None - self._brain_measurements = None - self._behavioural_measurements = None - self.self_collide = self_collide - self.battery_level = 0.0 + def __init__(self, _id: int = None, self_collide: bool = True): + self._id: int = _id + self._body: Optional[CoreModule] = None + self._brain: Optional[Brain] = None + self._morphological_measurements: Optional[MeasureBody3D] = None + self._brain_measurements: Optional[MeasureBrain] = None + self._behavioural_measurements: Optional[BehaviouralMeasurements] = None + self.self_collide: bool = self_collide + self.battery_level: float = 0.0 + self.simulation_boundaries = None + self.failed_eval_attempt_count: int = 0 @property - def id(self): + def id(self) -> int: return self._id @property - def body(self): + def body(self) -> CoreModule: return self._body @property - def brain(self): + def brain(self) -> Brain: return self._brain - def size(self): + def size(self) -> int: robot_size = 1 + self._recursive_size_measurement(self._body) return robot_size - def _recursive_size_measurement(self, module): - count = 0 + def _recursive_size_measurement(self, module) -> int: + count: int = 0 for _, child in module.iter_children(): if child is not None: count += 1 + self._recursive_size_measurement(child) @@ -66,41 +76,43 @@ def measure_behaviour(self): :return: """ - pass + raise NotImplementedError("Behaviour measurement is not implemented here") - def measure_phenotype(self): + def measure_phenotype(self) -> None: self._morphological_measurements = self.measure_body() self._brain_measurements = self.measure_brain() logger.info('Robot ' + str(self.id) + ' was measured.') - def measure_body(self): + def measure_body(self) -> MeasureBody3D: """ - :return: instance of MeasureBody after performing all measurements + :return: instance of MeasureBody3D after performing all measurements """ if self._body is None: raise RuntimeError('Body not initialized') try: - measure = MeasureBody(self._body) + measure = MeasureBody3D(self._body) measure.measure_all() return measure except Exception as e: logger.exception('Failed measuring body') - def export_phenotype_measurements(self, data_path): + def export_phenotype_measurements(self, data_path) -> None: filepath = os.path.join(data_path, 'descriptors', f'phenotype_desc_{self.id}.txt') with open(filepath, 'w+') as file: - for key, value in self._morphological_measurements.measurements_to_dict().items(): - file.write(f'{key} {value}\n') - for key, value in self._brain_measurements.measurements_to_dict().items(): - file.write(f'{key} {value}\n') - - def measure_brain(self): + if self._morphological_measurements is not None: + for key, value in self._morphological_measurements.measurements_to_dict().items(): + file.write(f'{key} {value}\n') + if self._brain_measurements is not None: + for key, value in self._brain_measurements.measurements_to_dict().items(): + file.write(f'{key} {value}\n') + + def measure_brain(self) -> MeasureBrain: """ :return: instance of MeasureBrain after performing all measurements """ try: measure = MeasureBrain(self._brain, 10) - measure_b = MeasureBody(self._body) + measure_b = MeasureBody3D(self._body) measure_b.count_active_hinges() if measure_b.active_hinges_count > 0: measure.measure_all() @@ -108,21 +120,20 @@ def measure_brain(self): measure.set_all_zero() return measure except Exception as e: - logger.exception('Failed measuring brain') + logger.error(f'Failed measuring brain: {e}') - def load(self, text, conf_type): + def load(self, text: AnyStr, conf_type: str) -> None: """ Load robot's description from a string and parse it to Python structure :param text: Robot's description string :param conf_type: Type of a robot's description format - :return: """ if 'yaml' == conf_type: self.load_yaml(text) elif 'sdf' == conf_type: raise NotImplementedError("Loading from SDF not yet implemented") - def load_yaml(self, text): + def load_yaml(self, text: AnyStr) -> None: """ Load robot's description from a yaml string :param text: Robot's yaml description @@ -144,7 +155,7 @@ def load_yaml(self, text): self._brain = Brain() logger.exception('Failed to load brain, setting to None') - def load_file(self, path, conf_type='yaml'): + def load_file(self, path: str, conf_type: str = 'yaml') -> None: """ Read robot's description from a file and parse it to Python structure :param path: Robot's description file path @@ -156,15 +167,16 @@ def load_file(self, path, conf_type='yaml'): self.load(robot, conf_type) - def to_sdf(self, pose=SDF.math.Vector3(0, 0, 0.25), nice_format=None): + def to_sdf(self, + pose=SDF.math.Vector3(0, 0, 0.25), + nice_format: Union[bool, str] = None) -> AnyStr: if type(nice_format) is bool: nice_format = '\t' if nice_format else None return SDF.revolve_bot_to_sdf(self, pose, nice_format, self_collide=self.self_collide) - def to_yaml(self): + def to_yaml(self) -> AnyStr: """ Converts robot data structure to yaml - :return: """ yaml_dict = OrderedDict() @@ -175,115 +187,108 @@ def to_yaml(self): return yaml.dump(yaml_dict) - def save_file(self, path, conf_type='yaml'): + def save_file(self, path: str, conf_type: str = 'yaml') -> None: """ Save robot's description on a given file path in a specified format :param path: :param conf_type: :return: """ - robot = '' + robot: str if 'yaml' == conf_type: robot = self.to_yaml() elif 'sdf' == conf_type: robot = self.to_sdf(nice_format=True) + else: + raise NotImplementedError(f'Config type {conf_type} not supported') with open(path, 'w') as robot_file: robot_file.write(robot) - def update_substrate(self, raise_for_intersections=False): + def update_substrate(self, raise_for_intersections: bool = False) -> None: """ Update all coordinates for body components :param raise_for_intersections: enable raising an exception if a collision of coordinates is detected :raises self.ItersectionCollisionException: If a collision of coordinates is detected (and check is enabled) """ - substrate_coordinates_map = {(0, 0): self._body.id} - self._body.substrate_coordinates = (0, 0) - self._update_substrate(raise_for_intersections, self._body, Orientation.NORTH, substrate_coordinates_map) + substrate_coordinates_map: Dict[(int, int, int), int] = {(0, 0, 0): self._body.id} + self._body.substrate_coordinates = (0, 0, 0) + self._update_substrate(raise_for_intersections, self._body, np.identity(3), substrate_coordinates_map) class ItersectionCollisionException(Exception): """ A collision has been detected when updating the robot coordinates. Check self.substrate_coordinates_map to know more. """ - def __init__(self, substrate_coordinates_map): + def __init__(self, substrate_coordinates_map: Dict[(int, int, int), int]): super().__init__(self) - self.substrate_coordinates_map = substrate_coordinates_map + self.substrate_coordinates_map: Dict[(int, int, int), int] = substrate_coordinates_map def _update_substrate(self, - raise_for_intersections, - parent, - parent_direction, - substrate_coordinates_map): - """ - Internal recursive function for self.update_substrate() - :param raise_for_intersections: same as in self.update_substrate - :param parent: updates the children of this parent - :param parent_direction: the "absolute" orientation of this parent - :param substrate_coordinates_map: map for all already explored coordinates(useful for coordinates conflict checks) - """ - dic = {Orientation.NORTH: 0, - Orientation.WEST: 1, - Orientation.SOUTH: 2, - Orientation.EAST: 3} - inverse_dic = {0: Orientation.NORTH, - 1: Orientation.WEST, - 2: Orientation.SOUTH, - 3: Orientation.EAST} - - movement_table = { - Orientation.NORTH: ( 1, 0), - Orientation.WEST: ( 0, -1), - Orientation.SOUTH: (-1, 0), - Orientation.EAST: ( 0, 1), - } + raise_for_intersections: bool, + parent: RevolveModule, + global_rotation_matrix: np.array, + substrate_coordinates_map: Dict[(int, int, int), int]): + + step = np.array([[1], + [0], + [0]]) + + # rotation of parent + # parent.orientation != of type Orientation but is an angle + # Orientation of coreBlock is null! + + if parent.orientation != None: + rot = round(parent.orientation) + else: + rot = 0 + vertical_rotation_matrix = rotate_matrix_x_axis(rot * math.pi / 180.0 ) + global_rotation_matrix = np.matmul(global_rotation_matrix, vertical_rotation_matrix) for slot, module in parent.iter_children(): if module is None: continue - + # rotation for slot slot = Orientation(slot) - # calculate new direction - direction = dic[parent_direction] + dic[slot] - if direction >= len(dic): - direction = direction - len(dic) - new_direction = Orientation(inverse_dic[direction]) + # Z-axis rotation + slot_rotation = np.matmul(global_rotation_matrix, slot.get_slot_rotation_matrix()) + + # Do one step in the calculated direction + movement = np.matmul(slot_rotation, step) # calculate new coordinate - movement = movement_table[new_direction] coordinates = ( - parent.substrate_coordinates[0] + movement[0], - parent.substrate_coordinates[1] + movement[1], + parent.substrate_coordinates[0] + movement[0][0], + parent.substrate_coordinates[1] + movement[1][0], + parent.substrate_coordinates[2] + movement[2][0] ) module.substrate_coordinates = coordinates # For Karine: If you need to validate old robots, remember to add this condition to this if: - # if raise_for_intersections and coordinates in substrate_coordinates_map and type(module) is not TouchSensorModule: + # if raise_for_intersections and coordinates in substrate_coordinates_map and type(module) + # is not TouchSensorModule: if raise_for_intersections: if coordinates in substrate_coordinates_map: raise self.ItersectionCollisionException(substrate_coordinates_map) substrate_coordinates_map[coordinates] = module.id - self._update_substrate(raise_for_intersections, - module, - new_direction, - substrate_coordinates_map) + self._update_substrate(raise_for_intersections, module, slot_rotation, substrate_coordinates_map) - def _iter_all_elements(self): + def iter_all_elements(self) -> Iterable[RevolveModule]: to_process = deque([self._body]) while len(to_process) > 0: - elem = to_process.popleft() + elem: RevolveModule = to_process.popleft() for _i, child in elem.iter_children(): if child is not None: to_process.append(child) yield elem - def render_brain(self, img_path): + def render_brain(self, img_path: str) -> None: """ Render image of brain - @param img_path: path to where to store image + :param img_path: path to where to store image """ if self._brain is None: raise RuntimeError('Brain not initialized') @@ -297,7 +302,7 @@ def render_brain(self, img_path): else: raise RuntimeError('Brain {} image rendering not supported'.format(type(self._brain))) - def render_body(self, img_path): + def render_body(self, img_path: str) -> None: """ Render 2d representation of robot and store as png :param img_path: path of storing png file @@ -311,5 +316,5 @@ def render_body(self, img_path): except Exception as e: logger.exception('Failed rendering 2d robot') - def __repr__(self): + def __repr__(self) -> str: return f'RevolveBot({self.id})' diff --git a/pyrevolve/revolve_bot/revolve_module.py b/pyrevolve/revolve_bot/revolve_module.py index 16367bc5c7..85d6e9a266 100644 --- a/pyrevolve/revolve_bot/revolve_module.py +++ b/pyrevolve/revolve_bot/revolve_module.py @@ -1,8 +1,10 @@ """ Class containing the body parts to compose a Robogen robot """ +import math from collections import OrderedDict from enum import Enum +import numpy as np from pyrevolve import SDF @@ -22,23 +24,53 @@ def grams(x): # Module Orientation class Orientation(Enum): - SOUTH = 0 - NORTH = 1 - EAST = 2 - WEST = 3 + BACK = 0 + FORWARD = 1 + RIGHT = 2 + LEFT = 3 def short_repr(self): - if self == self.SOUTH: - return 'S' - elif self == self.NORTH: - return 'N' - elif self == self.EAST: - return 'E' - elif self == self.WEST: - return 'W' + if self == self.BACK: + return 'B' + elif self == self.FORWARD: + return 'F' + elif self == self.RIGHT: + return 'R' + elif self == self.LEFT: + return 'L' else: assert False + def get_slot_rotation_matrix(self): + if self == self.BACK: + return rotate_matrix_z_axis(math.pi) # 180 + elif self == self.FORWARD: + return rotate_matrix_z_axis(0.0) + elif self == self.RIGHT: + return rotate_matrix_z_axis(math.pi / 2.0) # 90 + elif self == self.LEFT: + return rotate_matrix_z_axis(math.pi / -2.0) # -90 + + +def rotate_matrix_z_axis(angle): + z_rotation_matrix = np.array([ + [round(np.cos(angle)), -1*round(np.sin(angle)), 0], + [round(np.sin(angle)), round(np.cos(angle)), 0], + [0, 0, 1] + ]) + + return z_rotation_matrix + + +def rotate_matrix_x_axis(angle): + x_rotation_matrix = np.array([ + [1, 0, 0], + [0, round(np.cos(angle)), -1*round(np.sin(angle))], + [0, round(np.sin(angle)), round(np.cos(angle))] + ]) + + return x_rotation_matrix + class RevolveModule: """ @@ -168,7 +200,7 @@ def to_sdf(self, tree_depth='', parent_link=None, child_link=None): Sensor SDF element may be None. """ name = 'component_{}_{}__box'.format(tree_depth, self.TYPE) - visual = SDF.Visual(name, self.rgb) + visual = SDF.Visual(name, self.color()) geometry = SDF.MeshGeometry(self.VISUAL_MESH) visual.append(geometry) @@ -179,7 +211,7 @@ def to_sdf(self, tree_depth='', parent_link=None, child_link=None): return visual, collision, None def boxslot(self, orientation=None): - orientation = Orientation.SOUTH if orientation is None else orientation + orientation = Orientation.BACK if orientation is None else orientation return BoxSlot(self.possible_slots(), orientation) def possible_slots(self): @@ -218,7 +250,7 @@ class CoreModule(RevolveModule): def __init__(self): super().__init__() - self.substrate_coordinates = (0, 0) + self.substrate_coordinates = (0, 0, 0) def possible_slots(self): return ( @@ -254,6 +286,9 @@ class ActiveHingeModule(RevolveModule): def __init__(self): super().__init__() self.children = {1: None} + self.oscillator_phase = None + self.oscillator_period = None + self.oscillator_amplitude = None def iter_children(self): return self.children.items() @@ -273,7 +308,7 @@ def to_sdf(self, tree_depth='', parent_link=None, child_link=None): name_servo = 'component_{}_{}__servo'.format(tree_depth, self.TYPE) name_servo2 = 'component_{}_{}__servo2'.format(tree_depth, self.TYPE) - visual_frame = SDF.Visual(name_frame, self.rgb) + visual_frame = SDF.Visual(name_frame, self.color()) geometry = SDF.MeshGeometry(self.VISUAL_MESH_FRAME) visual_frame.append(geometry) @@ -281,7 +316,7 @@ def to_sdf(self, tree_depth='', parent_link=None, child_link=None): geometry = SDF.BoxGeometry(self.COLLISION_BOX_FRAME) collision_frame.append(geometry) - visual_servo = SDF.Visual(name_servo, self.rgb) + visual_servo = SDF.Visual(name_servo, self.color()) geometry = SDF.MeshGeometry(self.VISUAL_MESH_SERVO) visual_servo.append(geometry) @@ -328,7 +363,7 @@ def possible_slots_servo(self): ) def boxslot_frame(self, orientation=None): - orientation = Orientation.SOUTH if orientation is None else orientation + orientation = Orientation.BACK if orientation is None else orientation boundaries = self.possible_slots_frame() return BoxSlotJoints( boundaries, @@ -337,15 +372,15 @@ def boxslot_frame(self, orientation=None): ) def boxslot_servo(self, orientation=None): - orientation = Orientation.SOUTH if orientation is None else orientation + orientation = Orientation.BACK if orientation is None else orientation boundaries = self.possible_slots_servo() return BoxSlotJoints(boundaries, orientation) def boxslot(self, orientation=None): - orientation = Orientation.SOUTH if orientation is None else orientation - if orientation is Orientation.SOUTH: + orientation = Orientation.BACK if orientation is None else orientation + if orientation is Orientation.BACK: return self.boxslot_frame(orientation) - elif orientation is Orientation.NORTH: + elif orientation is Orientation.FORWARD: return self.boxslot_servo(orientation) else: raise RuntimeError("Invalid orientation") @@ -401,8 +436,8 @@ def __init__(self): self.children = {} def boxslot(self, orientation=None): - orientation = Orientation.SOUTH if orientation is None else orientation - assert (orientation is Orientation.SOUTH) + orientation = Orientation.BACK if orientation is None else orientation + assert (orientation is Orientation.BACK) return BoxSlotTouchSensor(self.possible_slots()) def possible_slots(self): @@ -444,13 +479,13 @@ def __init__(self, boundaries, orientation: Orientation): def _calculate_box_slot_pos(self, boundaries, slot: Orientation): # boundaries = collision_elem.boundaries - if slot == Orientation.SOUTH: + if slot == Orientation.BACK: return SDF.math.Vector3(0, boundaries[1][0], 0) - elif slot == Orientation.NORTH: + elif slot == Orientation.FORWARD: return SDF.math.Vector3(0, boundaries[1][1], 0) - elif slot == Orientation.EAST: + elif slot == Orientation.RIGHT: return SDF.math.Vector3(boundaries[0][1], 0, 0) - elif slot == Orientation.WEST: + elif slot == Orientation.LEFT: return SDF.math.Vector3(boundaries[0][0], 0, 0) else: raise RuntimeError('invalid module orientation: {}'.format(slot)) @@ -460,13 +495,13 @@ def _calculate_box_slot_tangent(slot: Orientation): """ Return slot tangent """ - if slot == Orientation.SOUTH: + if slot == Orientation.BACK: return SDF.math.Vector3(0, 0, 1) - elif slot == Orientation.NORTH: + elif slot == Orientation.FORWARD: return SDF.math.Vector3(0, 0, 1) - elif slot == Orientation.EAST: + elif slot == Orientation.RIGHT: return SDF.math.Vector3(0, 0, 1) - elif slot == Orientation.WEST: + elif slot == Orientation.LEFT: return SDF.math.Vector3(0, 0, 1) # elif slot == 4: # # Right face tangent: back face @@ -485,9 +520,9 @@ def __init__(self, boundaries, orientation: Orientation, offset=(SDF.math.Vector super().__init__(boundaries, orientation) def _calculate_box_slot_pos(self, boundaries, slot: Orientation): - if slot == Orientation.SOUTH: + if slot == Orientation.BACK: return SDF.math.Vector3(boundaries[0][0], 0, 0) + self.offset[0] - elif slot == Orientation.NORTH: + elif slot == Orientation.FORWARD: return SDF.math.Vector3(boundaries[0][1], 0, 0) + self.offset[1] else: raise RuntimeError('invalid module orientation: {}'.format(slot)) @@ -497,9 +532,9 @@ def _calculate_box_slot_tangent(slot: Orientation): """ Return slot tangent """ - if slot == Orientation.SOUTH: + if slot == Orientation.BACK: return SDF.math.Vector3(0, 0, 1) - elif slot == Orientation.NORTH: + elif slot == Orientation.FORWARD: return SDF.math.Vector3(0, 0, 1) else: raise RuntimeError("Invalid orientation") @@ -507,10 +542,10 @@ def _calculate_box_slot_tangent(slot: Orientation): class BoxSlotTouchSensor(BoxSlot): def __init__(self, boundaries): - super().__init__(boundaries, Orientation.SOUTH) + super().__init__(boundaries, Orientation.BACK) def _calculate_box_slot_pos(self, boundaries, slot: Orientation): - if slot == Orientation.SOUTH: + if slot == Orientation.BACK: return SDF.math.Vector3(boundaries[0][0], 0, 0) else: raise RuntimeError('invalid module orientation: {}'.format(slot)) @@ -520,7 +555,7 @@ def _calculate_box_slot_tangent(slot: Orientation): """ Return slot tangent """ - if slot == Orientation.SOUTH: + if slot == Orientation.BACK: return SDF.math.Vector3(0, 1, 0) else: raise RuntimeError("Invalid orientation") diff --git a/pyrevolve/spec/implementation.py b/pyrevolve/spec/implementation.py index 386cb7fe3b..039512aa4d 100644 --- a/pyrevolve/spec/implementation.py +++ b/pyrevolve/spec/implementation.py @@ -441,12 +441,16 @@ def get_epsilon_mutated_parameters(self, params, serialize=False): :return: Mutated parameters :rtype: dict|list """ + if not isinstance(params, dict): params = self.unserialize_params(params) nw_params = {} for name, (_, spec) in list(self.parameters.items()): + if name in ['red', 'green', 'blue']: + continue epsilon = spec.epsilon + nw_params[name] = \ (1.0 - epsilon) * \ params[name] + epsilon * \ @@ -460,13 +464,7 @@ def set_parameters(self, container, params): :param container: Protobuf parameter container :param params: Serialized or listed parameters """ - if isinstance(params, dict): - params = self.serialize_params(params) - - del container[:] - for param in params: - p = container.add() - p.value = param + container = params class PartSpec(Parameterizable): diff --git a/pyrevolve/spec/msgs/body_pb2.py b/pyrevolve/spec/msgs/body_pb2.py index b6aa3ef246..a8a637f9d2 100644 --- a/pyrevolve/spec/msgs/body_pb2.py +++ b/pyrevolve/spec/msgs/body_pb2.py @@ -1,8 +1,7 @@ +# -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: body.proto -import sys -_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) from google.protobuf import descriptor as _descriptor from google.protobuf import message as _message from google.protobuf import reflection as _reflection @@ -20,7 +19,8 @@ package='revolve.msgs', syntax='proto2', serialized_options=None, - serialized_pb=_b('\n\nbody.proto\x12\x0crevolve.msgs\x1a\x0fparameter.proto\"\xb3\x01\n\x08\x42odyPart\x12\n\n\x02id\x18\x01 \x02(\t\x12\x0c\n\x04type\x18\x02 \x02(\t\x12\t\n\x01x\x18\x03 \x02(\x05\x12\t\n\x01y\x18\x04 \x02(\x05\x12\x13\n\x0borientation\x18\x05 \x02(\x01\x12+\n\x05\x63hild\x18\x06 \x03(\x0b\x32\x1c.revolve.msgs.BodyConnection\x12&\n\x05param\x18\x07 \x03(\x0b\x32\x17.revolve.msgs.Parameter\x12\r\n\x05label\x18\x08 \x01(\t\"Z\n\x0e\x42odyConnection\x12\x10\n\x08src_slot\x18\x01 \x02(\x05\x12\x10\n\x08\x64st_slot\x18\x02 \x02(\x05\x12$\n\x04part\x18\x03 \x02(\x0b\x32\x16.revolve.msgs.BodyPart\",\n\x04\x42ody\x12$\n\x04root\x18\x01 \x02(\x0b\x32\x16.revolve.msgs.BodyPart') + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\nbody.proto\x12\x0crevolve.msgs\x1a\x0fparameter.proto\"\xb3\x01\n\x08\x42odyPart\x12\n\n\x02id\x18\x01 \x02(\t\x12\x0c\n\x04type\x18\x02 \x02(\t\x12\t\n\x01x\x18\x03 \x02(\x05\x12\t\n\x01y\x18\x04 \x02(\x05\x12\x13\n\x0borientation\x18\x05 \x02(\x01\x12+\n\x05\x63hild\x18\x06 \x03(\x0b\x32\x1c.revolve.msgs.BodyConnection\x12&\n\x05param\x18\x07 \x03(\x0b\x32\x17.revolve.msgs.Parameter\x12\r\n\x05label\x18\x08 \x01(\t\"Z\n\x0e\x42odyConnection\x12\x10\n\x08src_slot\x18\x01 \x02(\x05\x12\x10\n\x08\x64st_slot\x18\x02 \x02(\x05\x12$\n\x04part\x18\x03 \x02(\x0b\x32\x16.revolve.msgs.BodyPart\",\n\x04\x42ody\x12$\n\x04root\x18\x01 \x02(\x0b\x32\x16.revolve.msgs.BodyPart' , dependencies=[parameter__pb2.DESCRIPTOR,]) @@ -33,63 +33,64 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='id', full_name='revolve.msgs.BodyPart.id', index=0, number=1, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='type', full_name='revolve.msgs.BodyPart.type', index=1, number=2, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='x', full_name='revolve.msgs.BodyPart.x', index=2, number=3, type=5, cpp_type=1, label=2, has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='y', full_name='revolve.msgs.BodyPart.y', index=3, number=4, type=5, cpp_type=1, label=2, has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='orientation', full_name='revolve.msgs.BodyPart.orientation', index=4, number=5, type=1, cpp_type=5, label=2, has_default_value=False, default_value=float(0), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='child', full_name='revolve.msgs.BodyPart.child', index=5, number=6, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='param', full_name='revolve.msgs.BodyPart.param', index=6, number=7, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='label', full_name='revolve.msgs.BodyPart.label', index=7, number=8, type=9, cpp_type=9, label=1, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -113,6 +114,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='src_slot', full_name='revolve.msgs.BodyConnection.src_slot', index=0, @@ -120,21 +122,21 @@ has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='dst_slot', full_name='revolve.msgs.BodyConnection.dst_slot', index=1, number=2, type=5, cpp_type=1, label=2, has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='part', full_name='revolve.msgs.BodyConnection.part', index=2, number=3, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -158,6 +160,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='root', full_name='revolve.msgs.Body.root', index=0, @@ -165,7 +168,7 @@ has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -191,25 +194,25 @@ DESCRIPTOR.message_types_by_name['Body'] = _BODY _sym_db.RegisterFileDescriptor(DESCRIPTOR) -BodyPart = _reflection.GeneratedProtocolMessageType('BodyPart', (_message.Message,), dict( - DESCRIPTOR = _BODYPART, - __module__ = 'body_pb2' +BodyPart = _reflection.GeneratedProtocolMessageType('BodyPart', (_message.Message,), { + 'DESCRIPTOR' : _BODYPART, + '__module__' : 'body_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.BodyPart) - )) + }) _sym_db.RegisterMessage(BodyPart) -BodyConnection = _reflection.GeneratedProtocolMessageType('BodyConnection', (_message.Message,), dict( - DESCRIPTOR = _BODYCONNECTION, - __module__ = 'body_pb2' +BodyConnection = _reflection.GeneratedProtocolMessageType('BodyConnection', (_message.Message,), { + 'DESCRIPTOR' : _BODYCONNECTION, + '__module__' : 'body_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.BodyConnection) - )) + }) _sym_db.RegisterMessage(BodyConnection) -Body = _reflection.GeneratedProtocolMessageType('Body', (_message.Message,), dict( - DESCRIPTOR = _BODY, - __module__ = 'body_pb2' +Body = _reflection.GeneratedProtocolMessageType('Body', (_message.Message,), { + 'DESCRIPTOR' : _BODY, + '__module__' : 'body_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.Body) - )) + }) _sym_db.RegisterMessage(Body) diff --git a/pyrevolve/spec/msgs/model_inserted_pb2.py b/pyrevolve/spec/msgs/model_inserted_pb2.py index 3e1056bcd0..fc0238d554 100644 --- a/pyrevolve/spec/msgs/model_inserted_pb2.py +++ b/pyrevolve/spec/msgs/model_inserted_pb2.py @@ -1,8 +1,7 @@ +# -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: model_inserted.proto -import sys -_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) from google.protobuf import descriptor as _descriptor from google.protobuf import message as _message from google.protobuf import reflection as _reflection @@ -21,7 +20,8 @@ package='revolve.msgs', syntax='proto2', serialized_options=None, - serialized_pb=_b('\n\x14model_inserted.proto\x12\x0crevolve.msgs\x1a\x0bmodel.proto\x1a\ntime.proto\"S\n\rModelInserted\x12\x1f\n\x04time\x18\x01 \x02(\x0b\x32\x11.gazebo.msgs.Time\x12!\n\x05model\x18\x02 \x02(\x0b\x32\x12.gazebo.msgs.Model') + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x14model_inserted.proto\x12\x0crevolve.msgs\x1a\x0bmodel.proto\x1a\ntime.proto\"S\n\rModelInserted\x12\x1f\n\x04time\x18\x01 \x02(\x0b\x32\x11.gazebo.msgs.Time\x12!\n\x05model\x18\x02 \x02(\x0b\x32\x12.gazebo.msgs.Model' , dependencies=[model__pb2.DESCRIPTOR,time__pb2.DESCRIPTOR,]) @@ -34,6 +34,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='time', full_name='revolve.msgs.ModelInserted.time', index=0, @@ -41,14 +42,14 @@ has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='model', full_name='revolve.msgs.ModelInserted.model', index=1, number=2, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -70,11 +71,11 @@ DESCRIPTOR.message_types_by_name['ModelInserted'] = _MODELINSERTED _sym_db.RegisterFileDescriptor(DESCRIPTOR) -ModelInserted = _reflection.GeneratedProtocolMessageType('ModelInserted', (_message.Message,), dict( - DESCRIPTOR = _MODELINSERTED, - __module__ = 'model_inserted_pb2' +ModelInserted = _reflection.GeneratedProtocolMessageType('ModelInserted', (_message.Message,), { + 'DESCRIPTOR' : _MODELINSERTED, + '__module__' : 'model_inserted_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.ModelInserted) - )) + }) _sym_db.RegisterMessage(ModelInserted) diff --git a/pyrevolve/spec/msgs/neural_net_pb2.py b/pyrevolve/spec/msgs/neural_net_pb2.py index 7f9d29f97b..9f2ceb5516 100644 --- a/pyrevolve/spec/msgs/neural_net_pb2.py +++ b/pyrevolve/spec/msgs/neural_net_pb2.py @@ -1,8 +1,7 @@ +# -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: neural_net.proto -import sys -_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) from google.protobuf import descriptor as _descriptor from google.protobuf import message as _message from google.protobuf import reflection as _reflection @@ -20,7 +19,8 @@ package='revolve.msgs', syntax='proto2', serialized_options=None, - serialized_pb=_b('\n\x10neural_net.proto\x12\x0crevolve.msgs\x1a\x0fparameter.proto\"<\n\x10NeuralConnection\x12\x0b\n\x03src\x18\x01 \x02(\t\x12\x0b\n\x03\x64st\x18\x02 \x02(\t\x12\x0e\n\x06weight\x18\x03 \x02(\x01\"i\n\x06Neuron\x12\n\n\x02id\x18\x01 \x02(\t\x12\r\n\x05layer\x18\x02 \x02(\t\x12\x0c\n\x04type\x18\x03 \x02(\t\x12\x0e\n\x06partId\x18\x04 \x01(\t\x12&\n\x05param\x18\x05 \x03(\x0b\x32\x17.revolve.msgs.Parameter\"i\n\rNeuralNetwork\x12$\n\x06neuron\x18\x01 \x03(\x0b\x32\x14.revolve.msgs.Neuron\x12\x32\n\nconnection\x18\x02 \x03(\x0b\x32\x1e.revolve.msgs.NeuralConnection\"\xb9\x01\n\x13ModifyNeuralNetwork\x12\x15\n\rremove_hidden\x18\x01 \x03(\t\x12(\n\nadd_hidden\x18\x02 \x03(\x0b\x32\x14.revolve.msgs.Neuron\x12,\n\x0eset_parameters\x18\x04 \x03(\x0b\x32\x14.revolve.msgs.Neuron\x12\x33\n\x0bset_weights\x18\x03 \x03(\x0b\x32\x1e.revolve.msgs.NeuralConnection') + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x10neural_net.proto\x12\x0crevolve.msgs\x1a\x0fparameter.proto\"<\n\x10NeuralConnection\x12\x0b\n\x03src\x18\x01 \x02(\t\x12\x0b\n\x03\x64st\x18\x02 \x02(\t\x12\x0e\n\x06weight\x18\x03 \x02(\x01\"i\n\x06Neuron\x12\n\n\x02id\x18\x01 \x02(\t\x12\r\n\x05layer\x18\x02 \x02(\t\x12\x0c\n\x04type\x18\x03 \x02(\t\x12\x0e\n\x06partId\x18\x04 \x01(\t\x12&\n\x05param\x18\x05 \x03(\x0b\x32\x17.revolve.msgs.Parameter\"i\n\rNeuralNetwork\x12$\n\x06neuron\x18\x01 \x03(\x0b\x32\x14.revolve.msgs.Neuron\x12\x32\n\nconnection\x18\x02 \x03(\x0b\x32\x1e.revolve.msgs.NeuralConnection\"\xb9\x01\n\x13ModifyNeuralNetwork\x12\x15\n\rremove_hidden\x18\x01 \x03(\t\x12(\n\nadd_hidden\x18\x02 \x03(\x0b\x32\x14.revolve.msgs.Neuron\x12,\n\x0eset_parameters\x18\x04 \x03(\x0b\x32\x14.revolve.msgs.Neuron\x12\x33\n\x0bset_weights\x18\x03 \x03(\x0b\x32\x1e.revolve.msgs.NeuralConnection' , dependencies=[parameter__pb2.DESCRIPTOR,]) @@ -33,28 +33,29 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='src', full_name='revolve.msgs.NeuralConnection.src', index=0, number=1, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='dst', full_name='revolve.msgs.NeuralConnection.dst', index=1, number=2, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='weight', full_name='revolve.msgs.NeuralConnection.weight', index=2, number=3, type=1, cpp_type=5, label=2, has_default_value=False, default_value=float(0), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -78,42 +79,43 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='id', full_name='revolve.msgs.Neuron.id', index=0, number=1, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='layer', full_name='revolve.msgs.Neuron.layer', index=1, number=2, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='type', full_name='revolve.msgs.Neuron.type', index=2, number=3, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='partId', full_name='revolve.msgs.Neuron.partId', index=3, number=4, type=9, cpp_type=9, label=1, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='param', full_name='revolve.msgs.Neuron.param', index=4, number=5, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -137,6 +139,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='neuron', full_name='revolve.msgs.NeuralNetwork.neuron', index=0, @@ -144,14 +147,14 @@ has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='connection', full_name='revolve.msgs.NeuralNetwork.connection', index=1, number=2, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -175,6 +178,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='remove_hidden', full_name='revolve.msgs.ModifyNeuralNetwork.remove_hidden', index=0, @@ -182,28 +186,28 @@ has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='add_hidden', full_name='revolve.msgs.ModifyNeuralNetwork.add_hidden', index=1, number=2, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='set_parameters', full_name='revolve.msgs.ModifyNeuralNetwork.set_parameters', index=2, number=4, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='set_weights', full_name='revolve.msgs.ModifyNeuralNetwork.set_weights', index=3, number=3, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -232,32 +236,32 @@ DESCRIPTOR.message_types_by_name['ModifyNeuralNetwork'] = _MODIFYNEURALNETWORK _sym_db.RegisterFileDescriptor(DESCRIPTOR) -NeuralConnection = _reflection.GeneratedProtocolMessageType('NeuralConnection', (_message.Message,), dict( - DESCRIPTOR = _NEURALCONNECTION, - __module__ = 'neural_net_pb2' +NeuralConnection = _reflection.GeneratedProtocolMessageType('NeuralConnection', (_message.Message,), { + 'DESCRIPTOR' : _NEURALCONNECTION, + '__module__' : 'neural_net_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.NeuralConnection) - )) + }) _sym_db.RegisterMessage(NeuralConnection) -Neuron = _reflection.GeneratedProtocolMessageType('Neuron', (_message.Message,), dict( - DESCRIPTOR = _NEURON, - __module__ = 'neural_net_pb2' +Neuron = _reflection.GeneratedProtocolMessageType('Neuron', (_message.Message,), { + 'DESCRIPTOR' : _NEURON, + '__module__' : 'neural_net_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.Neuron) - )) + }) _sym_db.RegisterMessage(Neuron) -NeuralNetwork = _reflection.GeneratedProtocolMessageType('NeuralNetwork', (_message.Message,), dict( - DESCRIPTOR = _NEURALNETWORK, - __module__ = 'neural_net_pb2' +NeuralNetwork = _reflection.GeneratedProtocolMessageType('NeuralNetwork', (_message.Message,), { + 'DESCRIPTOR' : _NEURALNETWORK, + '__module__' : 'neural_net_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.NeuralNetwork) - )) + }) _sym_db.RegisterMessage(NeuralNetwork) -ModifyNeuralNetwork = _reflection.GeneratedProtocolMessageType('ModifyNeuralNetwork', (_message.Message,), dict( - DESCRIPTOR = _MODIFYNEURALNETWORK, - __module__ = 'neural_net_pb2' +ModifyNeuralNetwork = _reflection.GeneratedProtocolMessageType('ModifyNeuralNetwork', (_message.Message,), { + 'DESCRIPTOR' : _MODIFYNEURALNETWORK, + '__module__' : 'neural_net_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.ModifyNeuralNetwork) - )) + }) _sym_db.RegisterMessage(ModifyNeuralNetwork) diff --git a/pyrevolve/spec/msgs/parameter_pb2.py b/pyrevolve/spec/msgs/parameter_pb2.py index c666d55583..124acfa80a 100644 --- a/pyrevolve/spec/msgs/parameter_pb2.py +++ b/pyrevolve/spec/msgs/parameter_pb2.py @@ -1,8 +1,7 @@ +# -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: parameter.proto -import sys -_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) from google.protobuf import descriptor as _descriptor from google.protobuf import message as _message from google.protobuf import reflection as _reflection @@ -19,7 +18,8 @@ package='revolve.msgs', syntax='proto2', serialized_options=None, - serialized_pb=_b('\n\x0fparameter.proto\x12\x0crevolve.msgs\"\x1a\n\tParameter\x12\r\n\x05value\x18\x01 \x02(\x01') + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x0fparameter.proto\x12\x0crevolve.msgs\"\x1a\n\tParameter\x12\r\n\x05value\x18\x01 \x02(\x01' ) @@ -31,6 +31,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='value', full_name='revolve.msgs.Parameter.value', index=0, @@ -38,7 +39,7 @@ has_default_value=False, default_value=float(0), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -58,11 +59,11 @@ DESCRIPTOR.message_types_by_name['Parameter'] = _PARAMETER _sym_db.RegisterFileDescriptor(DESCRIPTOR) -Parameter = _reflection.GeneratedProtocolMessageType('Parameter', (_message.Message,), dict( - DESCRIPTOR = _PARAMETER, - __module__ = 'parameter_pb2' +Parameter = _reflection.GeneratedProtocolMessageType('Parameter', (_message.Message,), { + 'DESCRIPTOR' : _PARAMETER, + '__module__' : 'parameter_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.Parameter) - )) + }) _sym_db.RegisterMessage(Parameter) diff --git a/pyrevolve/spec/msgs/robot_pb2.py b/pyrevolve/spec/msgs/robot_pb2.py index 885f0a1630..e83353d3ae 100644 --- a/pyrevolve/spec/msgs/robot_pb2.py +++ b/pyrevolve/spec/msgs/robot_pb2.py @@ -1,8 +1,7 @@ +# -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: robot.proto -import sys -_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) from google.protobuf import descriptor as _descriptor from google.protobuf import message as _message from google.protobuf import reflection as _reflection @@ -21,7 +20,8 @@ package='revolve.msgs', syntax='proto2', serialized_options=None, - serialized_pb=_b('\n\x0brobot.proto\x12\x0crevolve.msgs\x1a\nbody.proto\x1a\x10neural_net.proto\"a\n\x05Robot\x12\n\n\x02id\x18\x01 \x02(\x05\x12 \n\x04\x62ody\x18\x02 \x02(\x0b\x32\x12.revolve.msgs.Body\x12*\n\x05\x62rain\x18\x03 \x02(\x0b\x32\x1b.revolve.msgs.NeuralNetwork') + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x0brobot.proto\x12\x0crevolve.msgs\x1a\nbody.proto\x1a\x10neural_net.proto\"a\n\x05Robot\x12\n\n\x02id\x18\x01 \x02(\x05\x12 \n\x04\x62ody\x18\x02 \x02(\x0b\x32\x12.revolve.msgs.Body\x12*\n\x05\x62rain\x18\x03 \x02(\x0b\x32\x1b.revolve.msgs.NeuralNetwork' , dependencies=[body__pb2.DESCRIPTOR,neural__net__pb2.DESCRIPTOR,]) @@ -34,6 +34,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='id', full_name='revolve.msgs.Robot.id', index=0, @@ -41,21 +42,21 @@ has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='body', full_name='revolve.msgs.Robot.body', index=1, number=2, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='brain', full_name='revolve.msgs.Robot.brain', index=2, number=3, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -77,11 +78,11 @@ DESCRIPTOR.message_types_by_name['Robot'] = _ROBOT _sym_db.RegisterFileDescriptor(DESCRIPTOR) -Robot = _reflection.GeneratedProtocolMessageType('Robot', (_message.Message,), dict( - DESCRIPTOR = _ROBOT, - __module__ = 'robot_pb2' +Robot = _reflection.GeneratedProtocolMessageType('Robot', (_message.Message,), { + 'DESCRIPTOR' : _ROBOT, + '__module__' : 'robot_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.Robot) - )) + }) _sym_db.RegisterMessage(Robot) diff --git a/pyrevolve/spec/msgs/robot_states_pb2.py b/pyrevolve/spec/msgs/robot_states_pb2.py index 9d09a2680d..757b00093d 100644 --- a/pyrevolve/spec/msgs/robot_states_pb2.py +++ b/pyrevolve/spec/msgs/robot_states_pb2.py @@ -1,8 +1,7 @@ +# -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: robot_states.proto -import sys -_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) from google.protobuf import descriptor as _descriptor from google.protobuf import message as _message from google.protobuf import reflection as _reflection @@ -14,6 +13,7 @@ from pygazebo.msg import time_pb2 as time__pb2 from pygazebo.msg import pose_pb2 as pose__pb2 +from pygazebo.msg import vector3d_pb2 as vector3d__pb2 DESCRIPTOR = _descriptor.FileDescriptor( @@ -21,19 +21,74 @@ package='revolve.msgs', syntax='proto2', serialized_options=None, - serialized_pb=_b('\n\x12robot_states.proto\x12\x0crevolve.msgs\x1a\ntime.proto\x1a\npose.proto\"U\n\nRobotState\x12\n\n\x02id\x18\x01 \x02(\r\x12\x0c\n\x04name\x18\x02 \x02(\t\x12\x1f\n\x04pose\x18\x03 \x02(\x0b\x32\x11.gazebo.msgs.Pose\x12\x0c\n\x04\x64\x65\x61\x64\x18\x04 \x01(\x08\"]\n\x0bRobotStates\x12\x1f\n\x04time\x18\x01 \x02(\x0b\x32\x11.gazebo.msgs.Time\x12-\n\x0brobot_state\x18\x02 \x03(\x0b\x32\x18.revolve.msgs.RobotState') + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x12robot_states.proto\x12\x0crevolve.msgs\x1a\ntime.proto\x1a\npose.proto\x1a\x0evector3d.proto\"\xb5\x01\n\x0bOrientation\x12*\n\x0bvec_forward\x18\x01 \x01(\x0b\x32\x15.gazebo.msgs.Vector3d\x12\'\n\x08vec_left\x18\x02 \x01(\x0b\x32\x15.gazebo.msgs.Vector3d\x12\'\n\x08vec_back\x18\x03 \x01(\x0b\x32\x15.gazebo.msgs.Vector3d\x12(\n\tvec_right\x18\x04 \x01(\x0b\x32\x15.gazebo.msgs.Vector3d\"\x8a\x01\n\nRobotState\x12\n\n\x02id\x18\x01 \x02(\r\x12\x0c\n\x04name\x18\x02 \x02(\t\x12\x1f\n\x04pose\x18\x03 \x02(\x0b\x32\x11.gazebo.msgs.Pose\x12\x0c\n\x04\x64\x65\x61\x64\x18\x04 \x01(\x08\x12\x33\n\x10orientation_vecs\x18\x05 \x01(\x0b\x32\x19.revolve.msgs.Orientation\"]\n\x0bRobotStates\x12\x1f\n\x04time\x18\x01 \x02(\x0b\x32\x11.gazebo.msgs.Time\x12-\n\x0brobot_state\x18\x02 \x03(\x0b\x32\x18.revolve.msgs.RobotState' , - dependencies=[time__pb2.DESCRIPTOR,pose__pb2.DESCRIPTOR,]) + dependencies=[time__pb2.DESCRIPTOR,pose__pb2.DESCRIPTOR,vector3d__pb2.DESCRIPTOR,]) +_ORIENTATION = _descriptor.Descriptor( + name='Orientation', + full_name='revolve.msgs.Orientation', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='vec_forward', full_name='revolve.msgs.Orientation.vec_forward', index=0, + number=1, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='vec_left', full_name='revolve.msgs.Orientation.vec_left', index=1, + number=2, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='vec_back', full_name='revolve.msgs.Orientation.vec_back', index=2, + number=3, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='vec_right', full_name='revolve.msgs.Orientation.vec_right', index=3, + number=4, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto2', + extension_ranges=[], + oneofs=[ + ], + serialized_start=77, + serialized_end=258, +) + + _ROBOTSTATE = _descriptor.Descriptor( name='RobotState', full_name='revolve.msgs.RobotState', filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='id', full_name='revolve.msgs.RobotState.id', index=0, @@ -41,28 +96,35 @@ has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='name', full_name='revolve.msgs.RobotState.name', index=1, number=2, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='pose', full_name='revolve.msgs.RobotState.pose', index=2, number=3, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='dead', full_name='revolve.msgs.RobotState.dead', index=3, number=4, type=8, cpp_type=7, label=1, has_default_value=False, default_value=False, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='orientation_vecs', full_name='revolve.msgs.RobotState.orientation_vecs', index=4, + number=5, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -75,8 +137,8 @@ extension_ranges=[], oneofs=[ ], - serialized_start=60, - serialized_end=145, + serialized_start=261, + serialized_end=399, ) @@ -86,6 +148,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='time', full_name='revolve.msgs.RobotStates.time', index=0, @@ -93,14 +156,14 @@ has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='robot_state', full_name='revolve.msgs.RobotStates.robot_state', index=1, number=2, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -113,29 +176,42 @@ extension_ranges=[], oneofs=[ ], - serialized_start=147, - serialized_end=240, + serialized_start=401, + serialized_end=494, ) +_ORIENTATION.fields_by_name['vec_forward'].message_type = vector3d__pb2._VECTOR3D +_ORIENTATION.fields_by_name['vec_left'].message_type = vector3d__pb2._VECTOR3D +_ORIENTATION.fields_by_name['vec_back'].message_type = vector3d__pb2._VECTOR3D +_ORIENTATION.fields_by_name['vec_right'].message_type = vector3d__pb2._VECTOR3D _ROBOTSTATE.fields_by_name['pose'].message_type = pose__pb2._POSE +_ROBOTSTATE.fields_by_name['orientation_vecs'].message_type = _ORIENTATION _ROBOTSTATES.fields_by_name['time'].message_type = time__pb2._TIME _ROBOTSTATES.fields_by_name['robot_state'].message_type = _ROBOTSTATE +DESCRIPTOR.message_types_by_name['Orientation'] = _ORIENTATION DESCRIPTOR.message_types_by_name['RobotState'] = _ROBOTSTATE DESCRIPTOR.message_types_by_name['RobotStates'] = _ROBOTSTATES _sym_db.RegisterFileDescriptor(DESCRIPTOR) -RobotState = _reflection.GeneratedProtocolMessageType('RobotState', (_message.Message,), dict( - DESCRIPTOR = _ROBOTSTATE, - __module__ = 'robot_states_pb2' +Orientation = _reflection.GeneratedProtocolMessageType('Orientation', (_message.Message,), { + 'DESCRIPTOR' : _ORIENTATION, + '__module__' : 'robot_states_pb2' + # @@protoc_insertion_point(class_scope:revolve.msgs.Orientation) + }) +_sym_db.RegisterMessage(Orientation) + +RobotState = _reflection.GeneratedProtocolMessageType('RobotState', (_message.Message,), { + 'DESCRIPTOR' : _ROBOTSTATE, + '__module__' : 'robot_states_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.RobotState) - )) + }) _sym_db.RegisterMessage(RobotState) -RobotStates = _reflection.GeneratedProtocolMessageType('RobotStates', (_message.Message,), dict( - DESCRIPTOR = _ROBOTSTATES, - __module__ = 'robot_states_pb2' +RobotStates = _reflection.GeneratedProtocolMessageType('RobotStates', (_message.Message,), { + 'DESCRIPTOR' : _ROBOTSTATES, + '__module__' : 'robot_states_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.RobotStates) - )) + }) _sym_db.RegisterMessage(RobotStates) diff --git a/pyrevolve/spec/msgs/sdf_body_analyze_pb2.py b/pyrevolve/spec/msgs/sdf_body_analyze_pb2.py index 154482fab6..03f395edc6 100644 --- a/pyrevolve/spec/msgs/sdf_body_analyze_pb2.py +++ b/pyrevolve/spec/msgs/sdf_body_analyze_pb2.py @@ -1,8 +1,7 @@ +# -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: sdf_body_analyze.proto -import sys -_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) from google.protobuf import descriptor as _descriptor from google.protobuf import message as _message from google.protobuf import reflection as _reflection @@ -20,7 +19,8 @@ package='revolve.msgs', syntax='proto2', serialized_options=None, - serialized_pb=_b('\n\x16sdf_body_analyze.proto\x12\x0crevolve.msgs\x1a\x0evector3d.proto\"1\n\x07\x43ontact\x12\x12\n\ncollision1\x18\x01 \x02(\t\x12\x12\n\ncollision2\x18\x02 \x02(\t\"U\n\x0b\x42oundingBox\x12\"\n\x03min\x18\x01 \x02(\x0b\x32\x15.gazebo.msgs.Vector3d\x12\"\n\x03max\x18\x02 \x02(\x0b\x32\x15.gazebo.msgs.Vector3d\"n\n\x14\x42odyAnalysisResponse\x12.\n\x0b\x62oundingBox\x18\x01 \x01(\x0b\x32\x19.revolve.msgs.BoundingBox\x12&\n\x07\x63ontact\x18\x02 \x03(\x0b\x32\x15.revolve.msgs.Contact') + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x16sdf_body_analyze.proto\x12\x0crevolve.msgs\x1a\x0evector3d.proto\"1\n\x07\x43ontact\x12\x12\n\ncollision1\x18\x01 \x02(\t\x12\x12\n\ncollision2\x18\x02 \x02(\t\"U\n\x0b\x42oundingBox\x12\"\n\x03min\x18\x01 \x02(\x0b\x32\x15.gazebo.msgs.Vector3d\x12\"\n\x03max\x18\x02 \x02(\x0b\x32\x15.gazebo.msgs.Vector3d\"n\n\x14\x42odyAnalysisResponse\x12.\n\x0b\x62oundingBox\x18\x01 \x01(\x0b\x32\x19.revolve.msgs.BoundingBox\x12&\n\x07\x63ontact\x18\x02 \x03(\x0b\x32\x15.revolve.msgs.Contact' , dependencies=[vector3d__pb2.DESCRIPTOR,]) @@ -33,21 +33,22 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='collision1', full_name='revolve.msgs.Contact.collision1', index=0, number=1, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='collision2', full_name='revolve.msgs.Contact.collision2', index=1, number=2, type=9, cpp_type=9, label=2, - has_default_value=False, default_value=_b("").decode('utf-8'), + has_default_value=False, default_value=b"".decode('utf-8'), message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -71,6 +72,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='min', full_name='revolve.msgs.BoundingBox.min', index=0, @@ -78,14 +80,14 @@ has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='max', full_name='revolve.msgs.BoundingBox.max', index=1, number=2, type=11, cpp_type=10, label=2, has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -109,6 +111,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='boundingBox', full_name='revolve.msgs.BodyAnalysisResponse.boundingBox', index=0, @@ -116,14 +119,14 @@ has_default_value=False, default_value=None, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='contact', full_name='revolve.msgs.BodyAnalysisResponse.contact', index=1, number=2, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -149,25 +152,25 @@ DESCRIPTOR.message_types_by_name['BodyAnalysisResponse'] = _BODYANALYSISRESPONSE _sym_db.RegisterFileDescriptor(DESCRIPTOR) -Contact = _reflection.GeneratedProtocolMessageType('Contact', (_message.Message,), dict( - DESCRIPTOR = _CONTACT, - __module__ = 'sdf_body_analyze_pb2' +Contact = _reflection.GeneratedProtocolMessageType('Contact', (_message.Message,), { + 'DESCRIPTOR' : _CONTACT, + '__module__' : 'sdf_body_analyze_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.Contact) - )) + }) _sym_db.RegisterMessage(Contact) -BoundingBox = _reflection.GeneratedProtocolMessageType('BoundingBox', (_message.Message,), dict( - DESCRIPTOR = _BOUNDINGBOX, - __module__ = 'sdf_body_analyze_pb2' +BoundingBox = _reflection.GeneratedProtocolMessageType('BoundingBox', (_message.Message,), { + 'DESCRIPTOR' : _BOUNDINGBOX, + '__module__' : 'sdf_body_analyze_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.BoundingBox) - )) + }) _sym_db.RegisterMessage(BoundingBox) -BodyAnalysisResponse = _reflection.GeneratedProtocolMessageType('BodyAnalysisResponse', (_message.Message,), dict( - DESCRIPTOR = _BODYANALYSISRESPONSE, - __module__ = 'sdf_body_analyze_pb2' +BodyAnalysisResponse = _reflection.GeneratedProtocolMessageType('BodyAnalysisResponse', (_message.Message,), { + 'DESCRIPTOR' : _BODYANALYSISRESPONSE, + '__module__' : 'sdf_body_analyze_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.BodyAnalysisResponse) - )) + }) _sym_db.RegisterMessage(BodyAnalysisResponse) diff --git a/pyrevolve/spec/msgs/spline_policy_pb2.py b/pyrevolve/spec/msgs/spline_policy_pb2.py index 5b367e44d1..991c63a42e 100644 --- a/pyrevolve/spec/msgs/spline_policy_pb2.py +++ b/pyrevolve/spec/msgs/spline_policy_pb2.py @@ -1,8 +1,7 @@ +# -*- coding: utf-8 -*- # Generated by the protocol buffer compiler. DO NOT EDIT! # source: spline_policy.proto -import sys -_b=sys.version_info[0]<3 and (lambda x:x) or (lambda x:x.encode('latin1')) from google.protobuf import descriptor as _descriptor from google.protobuf import message as _message from google.protobuf import reflection as _reflection @@ -20,7 +19,8 @@ package='revolve.msgs', syntax='proto2', serialized_options=None, - serialized_pb=_b('\n\x13spline_policy.proto\x12\x0crevolve.msgs\x1a\x0fparameter.proto\"M\n\x06Spline\x12\r\n\x05index\x18\x01 \x02(\x05\x12\x0c\n\x04size\x18\x02 \x02(\x05\x12&\n\x05param\x18\x05 \x03(\x0b\x32\x17.revolve.msgs.Parameter\"\x17\n\x06Policy\x12\r\n\x05index\x18\x01 \x03(\x05\"6\n\x0cModifyPolicy\x12\x11\n\tadd_point\x18\x01 \x03(\x01\x12\x13\n\x0binterpolate\x18\x02 \x03(\t') + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\x13spline_policy.proto\x12\x0crevolve.msgs\x1a\x0fparameter.proto\"M\n\x06Spline\x12\r\n\x05index\x18\x01 \x02(\x05\x12\x0c\n\x04size\x18\x02 \x02(\x05\x12&\n\x05param\x18\x05 \x03(\x0b\x32\x17.revolve.msgs.Parameter\"\x17\n\x06Policy\x12\r\n\x05index\x18\x01 \x03(\x05\"6\n\x0cModifyPolicy\x12\x11\n\tadd_point\x18\x01 \x03(\x01\x12\x13\n\x0binterpolate\x18\x02 \x03(\t' , dependencies=[parameter__pb2.DESCRIPTOR,]) @@ -33,6 +33,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='index', full_name='revolve.msgs.Spline.index', index=0, @@ -40,21 +41,21 @@ has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='size', full_name='revolve.msgs.Spline.size', index=1, number=2, type=5, cpp_type=1, label=2, has_default_value=False, default_value=0, message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='param', full_name='revolve.msgs.Spline.param', index=2, number=5, type=11, cpp_type=10, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -78,6 +79,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='index', full_name='revolve.msgs.Policy.index', index=0, @@ -85,7 +87,7 @@ has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -109,6 +111,7 @@ filename=None, file=DESCRIPTOR, containing_type=None, + create_key=_descriptor._internal_create_key, fields=[ _descriptor.FieldDescriptor( name='add_point', full_name='revolve.msgs.ModifyPolicy.add_point', index=0, @@ -116,14 +119,14 @@ has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), _descriptor.FieldDescriptor( name='interpolate', full_name='revolve.msgs.ModifyPolicy.interpolate', index=1, number=2, type=9, cpp_type=9, label=3, has_default_value=False, default_value=[], message_type=None, enum_type=None, containing_type=None, is_extension=False, extension_scope=None, - serialized_options=None, file=DESCRIPTOR), + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), ], extensions=[ ], @@ -146,25 +149,25 @@ DESCRIPTOR.message_types_by_name['ModifyPolicy'] = _MODIFYPOLICY _sym_db.RegisterFileDescriptor(DESCRIPTOR) -Spline = _reflection.GeneratedProtocolMessageType('Spline', (_message.Message,), dict( - DESCRIPTOR = _SPLINE, - __module__ = 'spline_policy_pb2' +Spline = _reflection.GeneratedProtocolMessageType('Spline', (_message.Message,), { + 'DESCRIPTOR' : _SPLINE, + '__module__' : 'spline_policy_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.Spline) - )) + }) _sym_db.RegisterMessage(Spline) -Policy = _reflection.GeneratedProtocolMessageType('Policy', (_message.Message,), dict( - DESCRIPTOR = _POLICY, - __module__ = 'spline_policy_pb2' +Policy = _reflection.GeneratedProtocolMessageType('Policy', (_message.Message,), { + 'DESCRIPTOR' : _POLICY, + '__module__' : 'spline_policy_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.Policy) - )) + }) _sym_db.RegisterMessage(Policy) -ModifyPolicy = _reflection.GeneratedProtocolMessageType('ModifyPolicy', (_message.Message,), dict( - DESCRIPTOR = _MODIFYPOLICY, - __module__ = 'spline_policy_pb2' +ModifyPolicy = _reflection.GeneratedProtocolMessageType('ModifyPolicy', (_message.Message,), { + 'DESCRIPTOR' : _MODIFYPOLICY, + '__module__' : 'spline_policy_pb2' # @@protoc_insertion_point(class_scope:revolve.msgs.ModifyPolicy) - )) + }) _sym_db.RegisterMessage(ModifyPolicy) diff --git a/pyrevolve/tol/manage/measures.py b/pyrevolve/tol/manage/measures.py index 05b32cd0cd..3f6927cd25 100644 --- a/pyrevolve/tol/manage/measures.py +++ b/pyrevolve/tol/manage/measures.py @@ -1,16 +1,17 @@ import numpy as np +import math from pyrevolve.SDF.math import Vector3 from pyrevolve.util import Time -import math -import sys +from pyrevolve.angle.manage.robotmanager import RobotManager as RvRobotManager +from pyrevolve.revolve_bot.revolve_bot import RevolveBot class BehaviouralMeasurements: """ Calculates all the measurements and saves them in one object """ - def __init__(self, robot_manager = None, robot = None): + def __init__(self, robot_manager: RvRobotManager = None, robot: RevolveBot = None): """ :param robot_manager: Revolve Manager that holds the life of the robot :param robot: Revolve Bot for measurements relative to the robot morphology and brain @@ -43,7 +44,7 @@ def items(self): -def velocity(robot_manager): +def velocity(robot_manager: RvRobotManager): """ Returns the velocity over the maintained window :return: @@ -51,7 +52,7 @@ def velocity(robot_manager): return robot_manager._dist / robot_manager._time if robot_manager._time > 0 else 0 -def displacement(robot_manager): +def displacement(robot_manager: RvRobotManager): """ Returns a tuple of the displacement in both time and space between the first and last registered element in the speed @@ -68,11 +69,11 @@ def displacement(robot_manager): ) -def path_length(robot_manager): +def path_length(robot_manager: RvRobotManager): return robot_manager._dist -def displacement_velocity(robot_manager): +def displacement_velocity(robot_manager: RvRobotManager): """ Returns the displacement velocity, i.e. the velocity between the first and last recorded position of the @@ -86,14 +87,14 @@ def displacement_velocity(robot_manager): return np.sqrt(dist.x ** 2 + dist.y ** 2) / float(time) -def displacement_velocity_hill(robot_manager): +def displacement_velocity_hill(robot_manager: RvRobotManager): dist, time = displacement(robot_manager) if time.is_zero(): return 0.0 return dist.y / float(time) -def head_balance(robot_manager): +def head_balance(robot_manager: RvRobotManager): """ Returns the average rotation of teh head in the roll and pitch dimensions. :return: @@ -115,7 +116,7 @@ def head_balance(robot_manager): return balance -def contacts(robot_manager, robot): +def contacts(robot_manager: RvRobotManager, robot: RevolveBot): """ Measures the average number of contacts with the floor relative to the body size @@ -129,11 +130,14 @@ def contacts(robot_manager, robot): avg_contacts = 0 for c in robot_manager._contacts: avg_contacts += c - avg_contacts = avg_contacts / robot.phenotype._morphological_measurements.measurements_to_dict()['absolute_size'] + #TODO remove this IF, it's ugly as hell + if robot._morphological_measurements is None: + robot._morphological_measurements = robot.measure_body() + avg_contacts = avg_contacts / robot._morphological_measurements.absolute_size return avg_contacts -def logs_position_orientation(robot_manager, o, evaluation_time, robotid, path): +def logs_position_orientation(robot_manager: RvRobotManager, o, evaluation_time, robotid, path): with open(path + '/data_fullevolution/descriptors/positions_' + robotid + '.txt', "a+") as f: if robot_manager.second <= evaluation_time: robot_manager.avg_roll += robot_manager._orientations[o][0] diff --git a/pyrevolve/tol/manage/robotmanager.py b/pyrevolve/tol/manage/robotmanager.py index 78200e0010..495be9bd8c 100644 --- a/pyrevolve/tol/manage/robotmanager.py +++ b/pyrevolve/tol/manage/robotmanager.py @@ -30,14 +30,13 @@ def __init__( :param robot: RevolveBot :param position: :type position: Vector3 - :param time: + :param time: time the robot was created :type time: Time :param battery_level: Battery charge for this robot :type battery_level: float :return: """ - time = conf.evaluation_time if time is None else time - speed_window = int(float(time) * conf.pose_update_frequency) + 1 if position_log_size is None \ + speed_window = int(float(conf.evaluation_time) * conf.pose_update_frequency) if position_log_size is None \ else position_log_size super(RobotManager, self).__init__( robot=robot, diff --git a/pyrevolve/tol/spec/__init__.py b/pyrevolve/tol/spec/__init__.py index 8d89ace838..8550c849fc 100644 --- a/pyrevolve/tol/spec/__init__.py +++ b/pyrevolve/tol/spec/__init__.py @@ -4,6 +4,5 @@ from .body import get_body_generator from .brain import get_brain_spec -from .brain import get_brain_generator from .robot import get_tree_generator diff --git a/pyrevolve/tol/spec/brain.py b/pyrevolve/tol/spec/brain.py index 0cb2f2543d..efea2cc1f1 100644 --- a/pyrevolve/tol/spec/brain.py +++ b/pyrevolve/tol/spec/brain.py @@ -1,6 +1,6 @@ from __future__ import absolute_import -from pyrevolve.generate import NeuralNetworkGenerator +#from pyrevolve.generate import NeuralNetworkGenerator from pyrevolve.spec import default_neural_net from .. import constants @@ -15,16 +15,3 @@ def get_brain_spec(conf): """ return default_neural_net(conf.brain_mutation_epsilon) - -def get_brain_generator(conf): - """ - Returns a brain generator. - - :param conf: - :return: - """ - return NeuralNetworkGenerator( - get_brain_spec(conf), - max_hidden=constants.MAX_HIDDEN_NEURONS, - conn_prob=conf.p_connect_neurons - ) diff --git a/pyrevolve/tol/spec/robot.py b/pyrevolve/tol/spec/robot.py index e14b35e55c..34af25c7b7 100644 --- a/pyrevolve/tol/spec/robot.py +++ b/pyrevolve/tol/spec/robot.py @@ -2,7 +2,8 @@ from pyrevolve.angle.robogen.spec import RobogenTreeGenerator from ..spec.body import get_body_generator -from ..spec.brain import get_brain_generator +from ...revolve_bot.neural_net import NeuralNetworkGenerator +from ...spec import NeuralNetImplementation, NeuronSpec def get_tree_generator(conf): @@ -12,5 +13,13 @@ def get_tree_generator(conf): :rtype: TreeGenerator """ body_gen = get_body_generator(conf) - brain_gen = get_brain_generator(conf) + brain_spec = NeuralNetImplementation( + { + "Simple": NeuronSpec(params=["bias"]), + "Oscillator": NeuronSpec( + params=["period", "phaseOffset", "amplitude"] + ) + } + ) + brain_gen = NeuralNetworkGenerator(brain_spec) return RobogenTreeGenerator(body_gen, brain_gen, conf) diff --git a/pyrevolve/util/__init__.py b/pyrevolve/util/__init__.py index 869f6fc9c4..6995eb4b1e 100644 --- a/pyrevolve/util/__init__.py +++ b/pyrevolve/util/__init__.py @@ -4,4 +4,3 @@ from .functions import * from .futures import * -from .supervisor import Supervisor diff --git a/pyrevolve/util/supervisor/__init__.py b/pyrevolve/util/supervisor/__init__.py index a8aaee355d..c3961685ab 100644 --- a/pyrevolve/util/supervisor/__init__.py +++ b/pyrevolve/util/supervisor/__init__.py @@ -1,3 +1 @@ from __future__ import absolute_import - -from .supervisor import Supervisor diff --git a/pyrevolve/util/supervisor/analyzer_queue.py b/pyrevolve/util/supervisor/analyzer_queue.py index 1dfd3869b4..cac455832b 100644 --- a/pyrevolve/util/supervisor/analyzer_queue.py +++ b/pyrevolve/util/supervisor/analyzer_queue.py @@ -1,9 +1,13 @@ import os +from typing import Callable +from pyrevolve.evolution.population.population_config import PopulationConfig from pyrevolve.custom_logging.logger import logger from pyrevolve.gazebo.analyze import BodyAnalyzer from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue from pyrevolve.util.supervisor.supervisor_collision import CollisionSimSupervisor +from pyrevolve.revolve_bot import RevolveBot +from pyrevolve.tol.manage.robotmanager import RobotManager class AnalyzerQueue(SimulatorQueue): @@ -11,6 +15,7 @@ class AnalyzerQueue(SimulatorQueue): def __init__(self, n_cores: int, settings, port_start=11345, simulator_cmd='gzserver'): super(AnalyzerQueue, self).__init__(n_cores, settings, port_start, simulator_cmd) + self._enable_play_pause = False def _simulator_supervisor(self, simulator_name_postfix): return CollisionSimSupervisor( @@ -25,11 +30,15 @@ def _simulator_supervisor(self, simulator_name_postfix): async def _connect_to_simulator(self, settings, address, port): return await BodyAnalyzer.create(address, port) - async def _evaluate_robot(self, simulator_connection, robot, conf): + async def _evaluate_robot(self, + simulator_connection, + robot: RevolveBot, + conf: PopulationConfig, + _fitness_fun: Callable[[RobotManager, RevolveBot], float]): if robot.failed_eval_attempt_count == 3: logger.info(f'Robot {robot.phenotype.id} analyze failed (reached max attempt of 3), fitness set to None.') analyze_result = None return analyze_result else: - analyze_result = await simulator_connection.analyze_robot(robot.phenotype) + analyze_result = await simulator_connection.analyze_robot(robot) return analyze_result diff --git a/pyrevolve/util/supervisor/simulator_queue.py b/pyrevolve/util/supervisor/simulator_queue.py index 8f69eb52a9..2cd33f428a 100644 --- a/pyrevolve/util/supervisor/simulator_queue.py +++ b/pyrevolve/util/supervisor/simulator_queue.py @@ -1,9 +1,13 @@ import asyncio import os import time +from typing import Tuple, Callable, Optional +from pyrevolve.angle.manage.robotmanager import RobotManager +from pyrevolve.revolve_bot import RevolveBot +from pyrevolve.evolution.individual import Individual from pyrevolve.custom_logging.logger import logger -from pyrevolve.evolution.population import PopulationConfig +from pyrevolve.evolution.population.population_config import PopulationConfig from pyrevolve.tol.manage import World from pyrevolve.util.supervisor.supervisor_multi import DynamicSimSupervisor from pyrevolve.SDF.math import Vector3 @@ -11,7 +15,7 @@ class SimulatorQueue: - EVALUATION_TIMEOUT = 120 # seconds + EVALUATION_TIMEOUT = 30 # seconds def __init__(self, n_cores: int, settings, port_start=11345, simulator_cmd=None): assert (n_cores > 0) @@ -24,6 +28,7 @@ def __init__(self, n_cores: int, settings, port_start=11345, simulator_cmd=None) self._robot_queue = asyncio.Queue() self._free_simulator = [True for _ in range(n_cores)] self._workers = [] + self._enable_play_pause = True def _simulator_supervisor(self, simulator_name_postfix): return DynamicSimSupervisor( @@ -71,14 +76,15 @@ async def start(self): await asyncio.sleep(1) - def test_robot(self, robot, conf: PopulationConfig): + def test_robot(self, individual: Individual, robot: RevolveBot, conf: PopulationConfig, fitness_fun): """ + :param individual: robot individual :param robot: robot phenotype :param conf: configuration of the experiment - :return: + :return: asyncio future that resolves when the robot is evaluated """ future = asyncio.Future() - self._robot_queue.put_nowait((robot, future, conf)) + self._robot_queue.put_nowait((individual, robot, future, conf, fitness_fun)) return future async def _restart_simulator(self, i): @@ -88,9 +94,10 @@ async def _restart_simulator(self, i): logger.error("Restarting simulator") logger.error("Restarting simulator... disconnecting") try: - await asyncio.wait_for(self._connections[i].disconnect(), 10) + # timeout is in seconds + await asyncio.wait_for(self._connections[i].disconnect(), timeout=10.0) except asyncio.TimeoutError: - pass + logger.error("Restarting simulator... disconnecting timeout") logger.error("Restarting simulator... restarting") await self._supervisors[i].relaunch(10, address=address, port=port) await asyncio.sleep(10) @@ -98,24 +105,21 @@ async def _restart_simulator(self, i): self._connections[i] = await self._connect_to_simulator(self._settings, address, port) logger.debug("Restarting simulator done... connection done") - async def _worker_evaluate_robot(self, connection, robot, future, conf): + async def _worker_evaluate_robot(self, connection, robot: RevolveBot, future, conf, fitness_fun): await asyncio.sleep(0.01) start = time.time() try: timeout = self.EVALUATION_TIMEOUT # seconds - result = await asyncio.wait_for(self._evaluate_robot(connection, robot, conf), timeout=timeout) + result = await asyncio.wait_for(self._evaluate_robot(connection, robot, conf, fitness_fun), timeout=timeout) except asyncio.TimeoutError: # WAITED TO MUCH, RESTART SIMULATOR elapsed = time.time()-start logger.error(f"Simulator restarted after {elapsed}") return False except Exception: - logger.exception(f"Exception running robot {robot.phenotype}") + logger.exception(f"Exception running robot {robot}") return False - elapsed = time.time()-start - logger.info(f"time taken to do a simulation {elapsed}") - robot.failed_eval_attempt_count = 0 future.set_result(result) return True @@ -123,55 +127,73 @@ async def _worker_evaluate_robot(self, connection, robot, future, conf): async def _simulator_queue_worker(self, i): try: self._free_simulator[i] = True + if self._enable_play_pause: + await self._connections[i].pause(True) + await self._connections[i].reset(rall=True, time_only=True, model_only=False) while True: logger.info(f"simulator {i} waiting for robot") - (robot, future, conf) = await self._robot_queue.get() + (individual, robot, future, conf, fitness_fun) = await self._robot_queue.get() self._free_simulator[i] = False - logger.info(f"Picking up robot {robot.phenotype.id} into simulator {i}") - success = await self._worker_evaluate_robot(self._connections[i], robot, future, conf) + logger.info(f"Picking up robot {robot.id} into simulator {i}") + success = await self._worker_evaluate_robot(self._connections[i], robot, future, conf, fitness_fun) if success: if robot.failed_eval_attempt_count == 3: logger.info("Robot failed to be evaluated 3 times. Saving robot to failed_eval file") conf.experiment_management.export_failed_eval_robot(robot) robot.failed_eval_attempt_count = 0 - logger.info(f"simulator {i} finished robot {robot.phenotype.id}") + logger.info(f"simulator {i} finished robot {robot.id}") else: # restart of the simulator happened robot.failed_eval_attempt_count += 1 - logger.info(f"Robot {robot.phenotype.id} current failed attempt: {robot.failed_eval_attempt_count}") - await self._robot_queue.put((robot, future, conf)) + logger.info(f"Robot {robot.id} current failed attempt: {robot.failed_eval_attempt_count}") + await self._robot_queue.put((individual, robot, future, conf, fitness_fun)) await self._restart_simulator(i) + if self._enable_play_pause: + await self._connections[i].pause(True) + await self._connections[i].reset(rall=True, time_only=True, model_only=False) self._robot_queue.task_done() self._free_simulator[i] = True except Exception: logger.exception(f"Exception occurred for Simulator worker {i}") - async def _evaluate_robot(self, simulator_connection, robot, conf): - if robot.failed_eval_attempt_count == 3: - logger.info(f'Robot {robot.phenotype.id} evaluation failed (reached max attempt of 3), fitness set to None.') - robot_fitness = None - return robot_fitness, None + async def _evaluate_robot(self, + simulator_connection, + robot: RevolveBot, + conf: PopulationConfig, + fitness_fun: Callable[[RobotManager, RevolveBot], float]) \ + -> Tuple[Optional[float], Optional[measures.BehaviouralMeasurements]]: + if robot.failed_eval_attempt_count >= 3: + logger.info(f'Robot {robot.id} evaluation failed (reached max attempt of 3), fitness set to None.') + robot_fitness_none = None + measurements_none = None + return robot_fitness_none, measurements_none else: - # Change this `max_age` from the command line parameters (--evalution-time) - max_age = conf.evaluation_time - robot_manager = await simulator_connection.insert_robot(robot.phenotype, Vector3(0, 0, self._settings.z_start), max_age) + # Change this `max_age` from the command line parameters (--evalution-time and --grace-time) + max_age = conf.evaluation_time + conf.grace_time + pose_z = self._settings.z_start + if robot.simulation_boundaries is not None: + pose_z -= robot.simulation_boundaries.min.z + robot_manager = await simulator_connection.insert_robot(robot, Vector3(0, 0, pose_z), max_age) + if self._enable_play_pause: + await simulator_connection.pause(False) start = time.time() # Start a run loop to do some stuff - while not robot_manager.dead: # robot_manager.age() < max_age: + while not robot_manager.dead: + # while not robot_manager.age() < max_age: await asyncio.sleep(1.0 / 2) # 5= state_update_frequency end = time.time() elapsed = end-start logger.info(f'Time taken: {elapsed}') - robot_fitness = conf.fitness_function(robot_manager, robot) + robot_fitness = fitness_fun(robot_manager, robot) simulator_connection.unregister_robot(robot_manager) # await simulator_connection.delete_all_robots() # await simulator_connection.delete_robot(robot_manager) - # await simulator_connection.pause(True) + if self._enable_play_pause: + await simulator_connection.pause(True) await simulator_connection.reset(rall=True, time_only=True, model_only=False) return robot_fitness, measures.BehaviouralMeasurements(robot_manager, robot) - async def _joint(self): + async def _join(self): await self._robot_queue.join() - diff --git a/pyrevolve/util/supervisor/supervisor.py b/pyrevolve/util/supervisor/supervisor.py deleted file mode 100644 index 0f43f369db..0000000000 --- a/pyrevolve/util/supervisor/supervisor.py +++ /dev/null @@ -1,379 +0,0 @@ -from __future__ import absolute_import -from __future__ import print_function - -import atexit -import subprocess -import os -import psutil -import sys -import time - -from datetime import datetime - -from ...custom_logging.logger import logger - -from .nbsr import NonBlockingStreamReader as NBSR -mswindows = (sys.platform == "win32") - - -def terminate_process(proc): - """ - Recursively kills a process and all of its children - :param proc: Result of `subprocess.Popen` - - Inspired by http://stackoverflow.com/a/25134985/358873 - - TODO Check if terminate fails and kill instead? - :return: - """ - process = psutil.Process(proc.pid) - for child in process.children(recursive=True): - child.terminate() - - process.terminate() - - -class Supervisor(object): - """ - Utility class that allows you to automatically restore a crashing - experiment and continue to run it from a snapshotted. It does so - by assuming a snapshot functionality similar to that in Revolve.Angle's - WorldManager. The supervisor launches subprocesses for (a) a world - and (b) your manager / experiment. It determines a fixed output directory - for this experiment run, which is provided to the manager with - the `restore_arg` argument. - - The experiment is considered finished if any of the processes exit with 0 - code. If any of processes exit with non zero code, the experiment dies. - """ - - def __init__(self, - manager_cmd, - world_file, - output_directory=None, - manager_args=None, - simulator_cmd="gzserver", - simulator_args=None, - restore_arg="--restore-directory", - snapshot_world_file="snapshot.world", - restore_directory=None, - plugins_dir_path=None, - models_dir_path=None - ): - """ - - :param manager_cmd: The command used to run your manager / experiment - :param world_file: Full path (or relative to cwd) to the Gazebo world - file to load. - :param output_directory: Full path (or relative to cwd) to the output - directory, which will be the parent of the - restore directory. - :param manager_args: Commands to pass to the manager - :param simulator_cmd: Command to runs the Simulator - :param simulator_args: Arguments to the Simulator, *excluding* the world file name - :param restore_arg: Argument used to pass the snapshot/restore - directory name to the manager. Note that the - output directory is not passed as part of this - name, just the relative path. - :param snapshot_world_file: - :param restore_directory: - :param plugins_dir_path: Full path (or relative to cwd) to the simulator - plugins directory (setting env variable - GAZEBO_PLUGIN_PATH). - :param models_dir_path: Full path (or relative to cwd) to the simulator - models directory (setting env variable - GAZEBO_MODEL_PATH). - """ - self.restore_directory = datetime.now().strftime('%Y%m%d%H%M%S') \ - if restore_directory is None else restore_directory - self.output_directory = 'output' \ - if output_directory is None else os.path.abspath(output_directory) - self.snapshot_directory = os.path.join( - self.output_directory, - self.restore_directory) - self.snapshot_world_file = snapshot_world_file - self.restore_arg = restore_arg - self.simulator_args = simulator_args if simulator_args is not None else ["-u"] - self.simulator_cmd = simulator_cmd \ - if isinstance(simulator_cmd, list) else [simulator_cmd] - self.manager_args = manager_args if manager_args is not None else [] - self.manager_args += [self.restore_arg, self.snapshot_directory] - - self.world_file = os.path.abspath(world_file) - self.manager_cmd = manager_cmd \ - if isinstance(manager_cmd, list) else [manager_cmd] - - self.streams = {} - self.procs = {} - - # Terminate all processes when the supervisor exits - atexit.register(self._terminate_all) - - # Set plugins dir path for Gazebo - if plugins_dir_path is not None: - plugins_dir_path = os.path.abspath(plugins_dir_path) - try: - new_env_var = "{curr_paths}:{new_path}".format( - curr_paths=os.environ["GAZEBO_PLUGIN_PATH"], - new_path=plugins_dir_path) - except KeyError: - new_env_var = plugins_dir_path - os.environ["GAZEBO_PLUGIN_PATH"] = new_env_var - - # Set models dir path for Gazebo - if models_dir_path is not None: - models_dir_path = os.path.abspath(models_dir_path) - try: - new_env_var = "{curr_paths}:{new_path}".format( - curr_paths=os.environ["GAZEBO_MODEL_PATH"], - new_path=models_dir_path) - except KeyError: - new_env_var = models_dir_path - os.environ['GAZEBO_MODEL_PATH'] = new_env_var - - logger.info("Created Supervisor with:" - "\n\t- manager command: {} {}" - "\n\t- simulator command: {} {}" - "\n\t- world file: {}" - "\n\t- simulator plugin dir: {}" - "\n\t- simulator models dir: {}" - .format(manager_cmd, - manager_args, - simulator_cmd, - simulator_args, - world_file, - plugins_dir_path, - models_dir_path) - ) - - def launch_simulator(self): - logger.info("\nNOTE: launching only a simulator, not a manager script!\n") - self._launch_simulator() - - # Wait for the end - while True: - for proc_name in self.procs: - self._pass_through_stdout() - ret = self.procs[proc_name].poll() - if ret is not None: - if ret == 0: - sys.stdout.write("Program {} exited normally\n" - .format(proc_name)) - else: - sys.stderr.write("Program {} exited with code {}\n" - .format(proc_name, ret)) - - return ret - - def launch(self): - """ - (Re)launches the experiment. - :return: - """ - # if not os.path.exists(self.output_directory): - # os.mkdir(self.output_directory) - # if not os.path.exists(self.snapshot_directory): - # os.mkdir(self.snapshot_directory) - - logger.info("Launching all processes...") - self._launch_simulator() - self._launch_manager() - - while True: - for proc_name in self.procs: - # Write out all received stdout - self._pass_through_stdout() - - ret = self.procs[proc_name].poll() - if ret is not None: - if ret == 0: - sys.stdout.write("Program '{}' exited normally\n" - .format(proc_name)) - else: - sys.stderr.write("Program '{}' exited with code {}\n" - .format(proc_name, ret)) - - return ret - - # We could do this a lot less often, but this way we get - # output once every second. - time.sleep(0.1) - - def _pass_through_stdout(self): - """ - Passes process piped standard out through to normal stdout - :return: - """ - for NBSRout, NBSRerr in list(self.streams.values()): - try: - for _ in range(1000): - out = NBSRout.readline(0.005) - err = NBSRerr.readline(0.005) - - if not out and not err: - break - - if out: - self.write_stdout(out) - - if err: - self.write_stderr(err) - except Exception as e: - logger.exception("Exception while handling file reading") - - @staticmethod - def write_stdout(data): - """ - Overridable method to write to stdout, useful if you - want to apply some kind of filter, or write to a file - instead. - :param self: - :param data: - :return: - """ - sys.stdout.write(data) - - @staticmethod - def write_stderr(data): - """ - Overridable method to write to stderr, useful if you - want to apply some kind of filter, or write to a file - instead. - :param data: - :return: - """ - sys.stderr.write(data) - - def _terminate_all(self): - """ - Terminates all running processes - :return: - """ - logger.info("Terminating processes...") - for proc in list(self.procs.values()): - if proc.poll() is None: - terminate_process(proc) - - # flush output of all processes - # TODO: fix this better - self._pass_through_stdout() - - self.procs = {} - - def _add_output_stream(self, name): - """ - Creates a non blocking stream reader for the process with - the given name, and adds it to the streams that are passed - through. - :param name: - :return: - """ - self.streams[name] = (NBSR(self.procs[name].stdout, name), - NBSR(self.procs[name].stderr, name)) - - def _launch_simulator(self, ready_str="World plugin loaded", output_tag="simulator"): - """ - Launches the simulator - :return: - """ - logger.info("Launching the simulator...") - gz_args = self.simulator_cmd + self.simulator_args - snapshot_world = os.path.join( - self.snapshot_directory, - self.snapshot_world_file) - world = snapshot_world \ - if os.path.exists(snapshot_world) else self.world_file - gz_args.append(world) - self.procs[output_tag] = self._launch_with_ready_str( - cmd=gz_args, - ready_str=ready_str, - output_tag=output_tag) - self._add_output_stream(output_tag) - - def _launch_manager(self): - """ - :return: - """ - logger.info("Launching experiment manager...") - os.environ['PYTHONUNBUFFERED'] = 'True' - args = self.manager_cmd + self.manager_args - args += [self.restore_arg, self.restore_directory] - process = subprocess.Popen( - args, - bufsize=1, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE) - - self.procs['manager'] = process - self._add_output_stream('manager') - - @staticmethod - def _launch_with_ready_str(cmd, ready_str, output_tag="simulator"): - """ - :param cmd: - :param ready_str: - :return: - """ - process = subprocess.Popen( - cmd, - bufsize=1, - stdout=subprocess.PIPE, - stderr=subprocess.PIPE) - - # make out and err non-blocking pipes - if not mswindows: - import fcntl - for pipe in [process.stdout, process.stderr]: - fd = pipe.fileno() - fl = fcntl.fcntl(fd, fcntl.F_GETFL) - fcntl.fcntl(fd, fcntl.F_SETFL, fl | os.O_NONBLOCK) - else: - # hint on how to fix it here: - # https://github.com/cs01/gdbgui/issues/18#issuecomment-284263708 - sys.stderr.write("Windows may not give the optimal experience\n") - - ready = False - while not ready: - exit_code = process.poll() - if exit_code is not None: - # flush out all stdout and stderr - out, err = process.communicate() - if out is not None: - sys.stdout.write("{}\n".format(out.decode('utf-8'))) - if err is not None: - sys.stderr.write("{}\n".format(err.decode('utf-8'))) - raise RuntimeError("Error launching {}, exit with code {}" - .format(cmd, exit_code)) - - try: - out = process.stdout.readline().decode('utf-8') - if len(out) > 0: - sys.stdout.write("[{}-launch] {}".format(output_tag, out)) - if ready_str in out: - ready = True - except IOError: - pass - - if not mswindows: - try: - err = process.stderr.readline().decode('utf-8') - if len(err) > 0: - sys.stderr.write("[{}-launch] {}".format(output_tag, err)) - except IOError: - pass - - time.sleep(0.1) - - # make out and err blocking pipes again - if not mswindows: - import fcntl - for pipe in [process.stdout, process.stderr]: - fd = pipe.fileno() - fl = fcntl.fcntl(fd, fcntl.F_GETFL) - fcntl.fcntl(fd, fcntl.F_SETFL, fl & (~ os.O_NONBLOCK)) - else: - # hint on how to fix it here: - # https://github.com/cs01/gdbgui/issues/18#issuecomment-284263708 - sys.stderr.write("Windows may not give the optimal experience\n") - - return process diff --git a/pyrevolve/util/supervisor/supervisor_multi.py b/pyrevolve/util/supervisor/supervisor_multi.py index 7bc1cb7b73..0f681e40b3 100644 --- a/pyrevolve/util/supervisor/supervisor_multi.py +++ b/pyrevolve/util/supervisor/supervisor_multi.py @@ -144,8 +144,8 @@ def __init__(self, self._logger.info("Created Supervisor with:" f"\n\t- simulator command: {simulator_cmd} {simulator_args}" f"\n\t- world file: {world_file}" - f"\n\t- GAZEBO_PLUGIN_PATH: {plugins_dir_path}" - f"\n\t- GAZEBO_MODEL_PATH: {models_dir_path}") + f"\n\t- GAZEBO_PLUGIN_PATH={plugins_dir_path}" + f"\n\t- GAZEBO_MODEL_PATH={models_dir_path}") async def launch_simulator(self, address='localhost', port=11345): """ @@ -245,20 +245,30 @@ async def _launch_simulator(self, ready_str="World plugin loaded", output_tag="s env[key] = value env['GAZEBO_MASTER_URI'] = f'http://{address}:{port}' + # Search for gazebo dynamic library lookup folder process = subprocess.run(['which', self.simulator_cmd[0]], stdout=subprocess.PIPE) process.check_returncode() gazebo_libraries_path = process.stdout.decode() gazebo_libraries_path = os.path.dirname(gazebo_libraries_path) for lib_f in ['lib', 'lib64']: _gazebo_libraries_path = os.path.join(gazebo_libraries_path, '..', lib_f) - if os.path.isfile(os.path.join(_gazebo_libraries_path, 'libgazebo_common.so')): + lib_postfix = 'dylib' if platform.system() == 'Darwin' else 'so' + if os.path.isfile(os.path.join(_gazebo_libraries_path, f'libgazebo_common.{lib_postfix}')): gazebo_libraries_path = _gazebo_libraries_path break + # Platform dependant environment setup if platform.system() == 'Darwin': env['DYLD_LIBRARY_PATH'] = gazebo_libraries_path else: # linux env['LD_LIBRARY_PATH'] = gazebo_libraries_path + # remove screen scaling variables, gazebo does not handle screen scaling really well. + if 'QT_AUTO_SCREEN_SCALE_FACTOR' in env: + del env['QT_AUTO_SCREEN_SCALE_FACTOR'] + if 'QT_SCREEN_SCALE_FACTORS' in env: + del env['QT_SCREEN_SCALE_FACTORS'] + # force set x11(xcb) platform, since gazebo on wayland is broken + env['QT_QPA_PLATFORM'] = 'xcb' self.procs[output_tag] = await self._launch_with_ready_str( cmd=gz_args, ready_str=ready_str, diff --git a/pyrevolve/util/time.py b/pyrevolve/util/time.py index 54389761dc..aee8039526 100644 --- a/pyrevolve/util/time.py +++ b/pyrevolve/util/time.py @@ -1,5 +1,4 @@ -from __future__ import absolute_import -from __future__ import division +from __future__ import absolute_import, division from math import ceil @@ -31,7 +30,7 @@ def set(self, sec=None, nsec=None, dbl=None, msg=None): """ if dbl is not None: self.sec = int(dbl) - self.nsec = int(round((dbl - self.sec) * 10e9)) + self.nsec = int(round((dbl - self.sec) * 1e9)) elif msg: self.sec = msg.sec self.nsec = msg.nsec @@ -50,13 +49,13 @@ def _correct(self): :return: """ if self.nsec < 0: - n = ceil(abs(self.nsec / float(10e9))) + n = ceil(abs(self.nsec / float(1e9))) self.sec -= n - self.nsec += n * 10e9 - elif self.nsec >= 10e9: - n = int(self.nsec / 10e9) + self.nsec += n * 1e9 + elif self.nsec >= 1e9: + n = int(self.nsec / 1e9) self.sec += n - self.nsec -= n * 10e9 + self.nsec -= n * 1e9 def is_zero(self): """ @@ -125,9 +124,7 @@ def __add__(self, other): :return: """ if isinstance(other, Time): - return self.__class__( - sec=self.sec + other.sec, - nsec=self.nsec + other.nsec) + return self.__class__(sec=self.sec + other.sec, nsec=self.nsec + other.nsec) # Otherwise assume a number return self.__class__(dbl=float(self) + other) @@ -141,13 +138,14 @@ def __sub__(self, other): :return: """ if isinstance(other, Time): - return self.__class__( - sec=self.sec - other.sec, - nsec=self.nsec - other.nsec) + return self.__class__(sec=self.sec - other.sec, nsec=self.nsec - other.nsec) # Assume a number return self.__class__(dbl=float(self) - other) + def __truediv__(self, other): + return self.__class__(dbl=float(self) / float(other)) + def __rsub__(self, other): """ :param other: @@ -195,7 +193,7 @@ def __float__(self): Float / double representation of this time :return: """ - return self.sec + self.nsec / 10.0e9 + return self.sec + self.nsec / 1.0e9 def __str__(self): return "{}".format(float(self)) diff --git a/requirements.txt b/requirements.txt index d5bb76df3c..1f5f01b9f4 100644 --- a/requirements.txt +++ b/requirements.txt @@ -3,8 +3,8 @@ asyncio==3.4.3 futures==3.0.2 numpy>=1.9.2 PyYAML>=3.11 -protobuf>=3.0.0 -psutil==5.6.6 +protobuf>=3.12.0 +psutil==3.4.2 matplotlib pycairo>=1.18.0 graphviz>=0.10.1 @@ -12,5 +12,8 @@ joblib pandas neat-python>=0.92 python-dateutil>=2.8.0 +typeguard>=2.12.1 +# Non PIP packages -e git+https://github.com/ci-group/pygazebo.git@master#egg=pygazebo +thirdparty/MultiNEAT diff --git a/revolve.py b/revolve.py index b54d23a334..f4ecf3171f 100755 --- a/revolve.py +++ b/revolve.py @@ -68,4 +68,3 @@ def handler(_loop, context): print("STARTING") main() print("FINISHED") - diff --git a/test_py/evolution/__init__.py b/test_py/evolution/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/test_py/evolution/speciation/__init__.py b/test_py/evolution/speciation/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/test_py/evolution/speciation/species.py b/test_py/evolution/speciation/species.py new file mode 100644 index 0000000000..40116dfc44 --- /dev/null +++ b/test_py/evolution/speciation/species.py @@ -0,0 +1,75 @@ +import os +import shutil +import unittest + +import yaml + +from pyrevolve.evolution.individual import Individual +from pyrevolve.evolution.speciation.species import Species +from pyrevolve.genotype import Genotype + + +class FakeGenome(Genotype): + def __init__(self, _id: int): + self.id = _id + + +class SpeciesTest(unittest.TestCase): + TEST_FOLDER = '/tmp/species_test' + + def _create_clean_test_folder(self): + if os.path.exists(self.TEST_FOLDER): + shutil.rmtree(self.TEST_FOLDER) + os.mkdir(self.TEST_FOLDER) + + def _generate_individuals(self): + return [Individual(FakeGenome(i)) for i in range(20)] + + def _generate_species(self, _id: int = 42, individuals=None): + if individuals is None: + individuals = self._generate_individuals() + return Species(individuals, _id) + + def test_instancing(self): + s = self._generate_species() + self.assertIsInstance(s, Species) + + def test_serialize(self): + s = self._generate_species(33) + self._create_clean_test_folder() + species_file = os.path.join(self.TEST_FOLDER, 'species_33.yaml') + s.serialize(species_file) + + self.assertTrue(os.path.isfile(species_file)) + with open(species_file) as file: + data = yaml.load(file, Loader=yaml.SafeLoader) + + self.assertDictEqual(data, { + 'id': 33, + 'age': {'evaluations': 0, 'generations': 0, 'no_improvements': 0}, + 'individuals_ids': [i for i in range(20)] + }) + + def test_deserialize(self): + individuals = self._generate_individuals() + s = self._generate_species(33, individuals) + self._create_clean_test_folder() + species_file = os.path.join(self.TEST_FOLDER, 'species_33.yaml') + s.serialize(species_file) + + loaded_individuals = { + individual.id: individual for individual in individuals + } + s1 = Species.Deserialize(species_file, loaded_individuals) + + self.assertEqual(s._id, s1._id) + self.assertEqual(s.age.evaluations(), s1.age.evaluations()) + self.assertEqual(s.age.generations(), s1.age.generations()) + self.assertEqual(s.age.no_improvements(), s1.age.no_improvements()) + for (individual1, fit1), (individual2, fit2) in zip(s._individuals, s1._individuals): + self.assertEqual(individual1, individual2) + self.assertEqual(fit1, fit2) + + # test also the equality operators + self.assertEqual(s.age, s1.age) + self.assertEqual(s, s1) diff --git a/test_py/multineat/__init__.py b/test_py/multineat/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/test_py/multineat/test_import.py b/test_py/multineat/test_import.py new file mode 100644 index 0000000000..01b8e89750 --- /dev/null +++ b/test_py/multineat/test_import.py @@ -0,0 +1,55 @@ +#!/usr/bin/env python3 + +import unittest + + +class ImportTest(unittest.TestCase): + def test_import(self): + import multineat + # enums + self.assertIsNotNone(multineat.NeuronType) + self.assertIsNotNone(multineat.ActivationFunction) + self.assertIsNotNone(multineat.SearchMode) + + # random generator + self.assertIsNotNone(multineat.RNG) + + # nn + self.assertIsNotNone(multineat.Connection) + self.assertIsNotNone(multineat.Neuron) + self.assertIsNotNone(multineat.NeuralNetwork) + self.assertIsNotNone(multineat.NeuralNetwork) + + # gene + self.assertIsNotNone(multineat.LinkGene) + self.assertIsNotNone(multineat.NeuronGene) + self.assertIsNotNone(multineat.Genome) + + self.assertIsNotNone(multineat.Species) + + self.assertIsNotNone(multineat.Substrate) + + self.assertIsNotNone(multineat.PhenotypeBehavior) + + self.assertIsNotNone(multineat.Population) + self.assertIsNotNone(multineat.Innovation) + self.assertIsNotNone(multineat.InnovationDatabase) + + self.assertIsNotNone(multineat.Parameters) + + self.assertIsNotNone(multineat.DoublesList) + self.assertIsNotNone(multineat.DoublesList2D) + self.assertIsNotNone(multineat.FloatsList) + self.assertIsNotNone(multineat.FloatsList2D) + self.assertIsNotNone(multineat.IntsList) + self.assertIsNotNone(multineat.IntsList2D) + self.assertIsNotNone(multineat.GenomeList) + self.assertIsNotNone(multineat.NeuronList) + self.assertIsNotNone(multineat.ConnectionList) + self.assertIsNotNone(multineat.NeuronGeneList) + self.assertIsNotNone(multineat.LinkGeneList) + self.assertIsNotNone(multineat.PhenotypeBehaviorList) + + +if __name__ == "__main__": + unittest.main() diff --git a/test_py/multineat/test_raw_genome.py b/test_py/multineat/test_raw_genome.py new file mode 100644 index 0000000000..43f9a6b9f7 --- /dev/null +++ b/test_py/multineat/test_raw_genome.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 + +import unittest +import numpy as np +import multineat as NEAT + + +class RawGenomeTest(unittest.TestCase): + + @staticmethod + def parameters(): + params = NEAT.Parameters() + params.PopulationSize = 100 + params.DynamicCompatibility = True + params.NormalizeGenomeSize = True + params.WeightDiffCoeff = 0.1 + params.CompatTreshold = 2.0 + params.YoungAgeTreshold = 15 + params.SpeciesMaxStagnation = 15 + params.OldAgeTreshold = 35 + params.MinSpecies = 2 + params.MaxSpecies = 10 + params.RouletteWheelSelection = False + params.RecurrentProb = 0.0 + params.OverallMutationRate = 1.0 + + params.ArchiveEnforcement = False + + params.MutateWeightsProb = 0.05 + + params.WeightMutationMaxPower = 0.5 + params.WeightReplacementMaxPower = 8.0 + params.MutateWeightsSevereProb = 0.0 + params.WeightMutationRate = 0.25 + params.WeightReplacementRate = 0.9 + + params.MaxWeight = 8 + + params.MutateAddNeuronProb = 0.001 + params.MutateAddLinkProb = 0.3 + params.MutateRemLinkProb = 0.0 + + params.MinActivationA = 4.9 + params.MaxActivationA = 4.9 + + params.ActivationFunction_SignedSigmoid_Prob = 0.0 + params.ActivationFunction_UnsignedSigmoid_Prob = 1.0 + params.ActivationFunction_Tanh_Prob = 0.0 + params.ActivationFunction_SignedStep_Prob = 0.0 + + params.CrossoverRate = 0.0 + params.MultipointCrossoverRate = 0.0 + params.SurvivalRate = 0.2 + + params.MutateNeuronTraitsProb = 0 + params.MutateLinkTraitsProb = 0 + + params.AllowLoops = True + params.AllowClones = True + + params.ClearNeuronTraitParameters() + params.ClearLinkTraitParameters() + params.ClearGenomeTraitParameters() + + return params + + def activate_network(self, genome, _input=None): + genome.BuildPhenotype(self._net) + _input = np.array([1, 2, 3], dtype=float) if _input is None else _input + self._net.Input(_input) + self._net.Activate() + output = self._net.Output() + return output[0] + + def setUp(self): + self._params = self.parameters() + self._net = NEAT.NeuralNetwork() + self._rng = NEAT.RNG() + self._rng.Seed(0) + self._innov_db = NEAT.InnovationDatabase() + + def test_genome(self): + a = NEAT.Genome(1, 3, 0, 1, False, NEAT.ActivationFunction.UNSIGNED_SIGMOID, + NEAT.ActivationFunction.UNSIGNED_SIGMOID, 0, self._params, 0) + # a.PrintAllTraits() + self.assertEqual(a.GetID(), 1) + self.assertIsNotNone(self.activate_network(a)) + + a.SetID(2) + self.assertEqual(a.GetID(), 2) + + def test_genome_mutate(self): + a = NEAT.Genome(1, 3, 0, 1, False, NEAT.ActivationFunction.UNSIGNED_SIGMOID, + NEAT.ActivationFunction.UNSIGNED_SIGMOID, 0, self._params, 0) + self._innov_db.Init_with_genome(a) + output_1 = self.activate_network(a) + for i in range(10): + a.Mutate(False, NEAT.SearchMode.COMPLEXIFYING, self._innov_db, self._params, self._rng) + output_2 = self.activate_network(a) + # a.PrintAllTraits() + self.assertEqual(a.GetID(), 1) + self.assertIsNotNone(output_1) + self.assertNotAlmostEqual(output_1, output_2) + + def test_genome_clone(self): + a = NEAT.Genome(1, 3, 0, 1, False, NEAT.ActivationFunction.UNSIGNED_SIGMOID, + NEAT.ActivationFunction.UNSIGNED_SIGMOID, 0, self._params, 0) + self._innov_db.Init_with_genome(a) + a.Mutate(False, NEAT.SearchMode.COMPLEXIFYING, self._innov_db, self._params, self._rng) + b = NEAT.Genome(a) + self.assertEqual(a.NumNeurons(), b.NumNeurons()) + self.assertEqual(a.NumLinks(), b.NumLinks()) + self.assertEqual(a.NumInputs(), b.NumInputs()) + self.assertEqual(a.NumOutputs(), b.NumOutputs()) + + self.assertAlmostEqual(self.activate_network(a), self.activate_network(b)) + + def test_genome_crossover(self): + a = NEAT.Genome(1, 3, 0, 1, False, NEAT.ActivationFunction.UNSIGNED_SIGMOID, + NEAT.ActivationFunction.UNSIGNED_SIGMOID, 0, self._params, 0) + self._innov_db.Init_with_genome(a) + a.Mutate(False, NEAT.SearchMode.COMPLEXIFYING, self._innov_db, self._params, self._rng) + b = NEAT.Genome(a) + b.SetID(2) + for i in range(10): + b.Mutate(False, NEAT.SearchMode.COMPLEXIFYING, self._innov_db, self._params, self._rng) + # b.PrintAllTraits() + + c = a.Mate(b, True, True, self._rng, self._params) + c.SetID(3) + self.assertEqual(c.GetID(), 3) + output_a = self.activate_network(a) + output_b = self.activate_network(b) + output_c = self.activate_network(c) + self.assertIsNotNone(output_c) + self.assertNotAlmostEqual(output_a, output_c) + self.assertNotAlmostEqual(output_b, output_c) + + for i in range(10): + c.Mutate(False, NEAT.SearchMode.COMPLEXIFYING, self._innov_db, self._params, self._rng) + output_c = self.activate_network(c) + self.assertIsNotNone(output_c) + self.assertNotAlmostEqual(output_a, output_c) + self.assertNotAlmostEqual(output_b, output_c) + # c.PrintAllTraits() + + +if __name__ == "__main__": + unittest.main() diff --git a/test_py/multineat/test_xor.py b/test_py/multineat/test_xor.py new file mode 100644 index 0000000000..5f0120f67a --- /dev/null +++ b/test_py/multineat/test_xor.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 + +import unittest +import time +import numpy as np +import pickle as pickle +import multineat as NEAT +from multineat import EvaluateGenomeList_Serial +from multineat import GetGenomeList, ZipFitness +from concurrent.futures import ProcessPoolExecutor, as_completed + + +class XorTest(unittest.TestCase): + def test_xor(self): + def evaluate(genome): + net = NEAT.NeuralNetwork() + genome.BuildPhenotype(net) + + error = 0 + + # do stuff and return the fitness + net.Flush() + net.Input(np.array([1., 0., 1.])) # can input numpy arrays, too + # for some reason only np.float64 is supported + for _ in range(2): + net.Activate() + o = net.Output() + error += abs(1 - o[0]) + + net.Flush() + net.Input([0, 1, 1]) + for _ in range(2): + net.Activate() + o = net.Output() + error += abs(1 - o[0]) + + net.Flush() + net.Input([1, 1, 1]) + for _ in range(2): + net.Activate() + o = net.Output() + error += abs(o[0]) + + net.Flush() + net.Input([0, 0, 1]) + for _ in range(2): + net.Activate() + o = net.Output() + error += abs(o[0]) + + return (4 - error) ** 2 + + params = NEAT.Parameters() + params.PopulationSize = 100 + params.DynamicCompatibility = True + params.NormalizeGenomeSize = True + params.WeightDiffCoeff = 0.1 + params.CompatTreshold = 2.0 + params.YoungAgeTreshold = 15 + params.SpeciesMaxStagnation = 15 + params.OldAgeTreshold = 35 + params.MinSpecies = 2 + params.MaxSpecies = 10 + params.RouletteWheelSelection = False + params.RecurrentProb = 0.0 + params.OverallMutationRate = 1.0 + + params.ArchiveEnforcement = False + + params.MutateWeightsProb = 0.05 + + params.WeightMutationMaxPower = 0.5 + params.WeightReplacementMaxPower = 8.0 + params.MutateWeightsSevereProb = 0.0 + params.WeightMutationRate = 0.25 + params.WeightReplacementRate = 0.9 + + params.MaxWeight = 8 + + params.MutateAddNeuronProb = 0.001 + params.MutateAddLinkProb = 0.3 + params.MutateRemLinkProb = 0.0 + + params.MinActivationA = 4.9 + params.MaxActivationA = 4.9 + + params.ActivationFunction_SignedSigmoid_Prob = 0.0 + params.ActivationFunction_UnsignedSigmoid_Prob = 1.0 + params.ActivationFunction_Tanh_Prob = 0.0 + params.ActivationFunction_SignedStep_Prob = 0.0 + + params.CrossoverRate = 0.0 + params.MultipointCrossoverRate = 0.0 + params.SurvivalRate = 0.2 + + params.MutateNeuronTraitsProb = 0 + params.MutateLinkTraitsProb = 0 + + params.AllowLoops = True + params.AllowClones = True + + def getbest(i, seed=0): + g = NEAT.Genome(0, 3, 0, 1, False, NEAT.ActivationFunction.UNSIGNED_SIGMOID, + NEAT.ActivationFunction.UNSIGNED_SIGMOID, 0, params, 0) + pop = NEAT.Population(g, params, True, 1.0, i) + pop.RNG.Seed(seed) + + generations = 0 + for generation in range(1000): + genome_list = NEAT.GetGenomeList(pop) + fitness_list = EvaluateGenomeList_Serial(genome_list, evaluate, display=False) + NEAT.ZipFitness(genome_list, fitness_list) + pop.Epoch() + generations = generation + best = max(fitness_list) + if best > 15.0: + break + + return generations + + gens = [] + for run in range(10): + gen = getbest(run, run) + gens += [gen] + # print('Run:', run, 'Generations to solve XOR:', gen) + avg_gens = sum(gens) / len(gens) + + # print('All:', gens) + # print('Average:', avg_gens) + self.assertLess(avg_gens, 50) + + +if __name__ == "__main__": + unittest.main() diff --git a/test_py/plasticonding/test_development.py b/test_py/plasticonding/test_development.py index 248e7c0d3c..d2e409a1f6 100644 --- a/test_py/plasticonding/test_development.py +++ b/test_py/plasticonding/test_development.py @@ -2,15 +2,20 @@ import os import pyrevolve.revolve_bot -import pyrevolve.genotype.plasticoding.plasticoding +import pyrevolve.genotype.plasticoding LOCAL_FOLDER = os.path.dirname(__file__) class TestPlastiCoding(unittest.TestCase): def setUp(self): - self.conf = pyrevolve.genotype.plasticoding.plasticoding.PlasticodingConfig() - self.genotype = pyrevolve.genotype.plasticoding.plasticoding.initialization.random_initialization(self.conf, 176) + self.conf = pyrevolve.genotype.plasticoding.PlasticodingConfig( + allow_vertical_brick=False, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=False, + ) + self.genotype = pyrevolve.genotype.plasticoding.initialization.random_initialization(self.conf, 176) def test_development(self): robot = self.genotype.develop() @@ -33,7 +38,7 @@ def test_read_write_file(self): self.genotype.export_genotype(file1) - genotype2 = pyrevolve.genotype.plasticoding.plasticoding.Plasticoding(self.conf, self.genotype.id) + genotype2 = pyrevolve.genotype.plasticoding.Plasticoding(self.conf, self.genotype.id) genotype2.id = self.genotype.id genotype2.load_genotype(file1) genotype2.export_genotype(file2) @@ -47,7 +52,7 @@ def test_read_write_file(self): file2_txt.close() def test_collision(self): - genotype_180 = pyrevolve.genotype.plasticoding.plasticoding.Plasticoding(self.conf, 180) + genotype_180 = pyrevolve.genotype.plasticoding.Plasticoding(self.conf, 180) genotype_180.load_genotype(os.path.join(LOCAL_FOLDER, 'genotype_180.txt')) robot = genotype_180.develop() robot.update_substrate(raise_for_intersections=True) @@ -55,10 +60,15 @@ def test_collision(self): class Test176(unittest.TestCase): def setUp(self): - self.conf = pyrevolve.genotype.plasticoding.plasticoding.PlasticodingConfig() + self.conf = pyrevolve.genotype.plasticoding.PlasticodingConfig( + allow_vertical_brick=False, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=False, + ) _id = 176 - self.genotype = pyrevolve.genotype.plasticoding.plasticoding.Plasticoding(self.conf, _id) + self.genotype = pyrevolve.genotype.plasticoding.Plasticoding(self.conf, _id) self.genotype.load_genotype(os.path.join(LOCAL_FOLDER, 'genotype_176.txt')) self.robot = self.genotype.develop() diff --git a/test_robots.py b/test_robots.py index bfb4e156d3..b5168b0276 100755 --- a/test_robots.py +++ b/test_robots.py @@ -7,39 +7,48 @@ async def run(): - settings = parser.parse_args() - yaml_file = 'experiments/'+ settings.experiment_name +'/data_fullevolution/phenotypes/'+'phenotype_'+settings.test_robot+'.yaml' - - r = RevolveBot(_id=settings.test_robot) - r.load_file(yaml_file, conf_type='yaml') - #r.save_file('experiments/'+ settings +'/data_fullevolution/phenotype_35.sdf.xml', conf_type='sdf') - #r.render_body('experiments/'+ settings +'/data_fullevolution/phenotypes/phenotype_35.body.png') - - connection = await World.create(settings, world_address=("127.0.0.1", settings.port_start)) - await connection.insert_robot(r) + settings = parser.parse_args() + yaml_file = ( + "experiments/" + + settings.experiment_name + + "/data_fullevolution/phenotypes/" + + "phenotype_" + + settings.test_robot + + ".yaml" + ) + + r = RevolveBot(_id=settings.test_robot) + r.load_file(yaml_file, conf_type="yaml") + # r.save_file('experiments/'+ settings +'/data_fullevolution/phenotype_35.sdf.xml', conf_type='sdf') + # r.render_body('experiments/'+ settings +'/data_fullevolution/phenotypes/phenotype_35.body.png') + + connection = await World.create( + settings, world_address=("127.0.0.1", settings.port_start) + ) + await connection.insert_robot(r) def main(): - import traceback - - def handler(_loop, context): - try: - exc = context['exception'] - except KeyError: - print(context['message']) - return - traceback.print_exc() - raise exc - - try: - loop = asyncio.get_event_loop() - loop.set_exception_handler(handler) - loop.run_until_complete(run()) - except KeyboardInterrupt: - print("Got CtrlC, shutting down.") - - -if __name__ == '__main__': - print("STARTING") - main() - print("FINISHED") + import traceback + + def handler(_loop, context): + try: + exc = context["exception"] + except KeyError: + print(context["message"]) + return + traceback.print_exc() + raise exc + + try: + loop = asyncio.get_event_loop() + loop.set_exception_handler(handler) + loop.run_until_complete(run()) + except KeyboardInterrupt: + print("Got CtrlC, shutting down.") + + +if __name__ == "__main__": + print("STARTING") + main() + print("FINISHED") diff --git a/thirdparty/MultiNEAT b/thirdparty/MultiNEAT new file mode 160000 index 0000000000..7f40de3631 --- /dev/null +++ b/thirdparty/MultiNEAT @@ -0,0 +1 @@ +Subproject commit 7f40de3631b37270c73f367eb41456d3f1b09be5 diff --git a/tools/proto2python.sh b/tools/proto2python.sh index 3f96cd6e35..933fabac1d 100755 --- a/tools/proto2python.sh +++ b/tools/proto2python.sh @@ -3,7 +3,7 @@ set -e PROTO_FOLDER='../cpprevolve/revolve/gazebo/msgs' -GAZEBO_PROTO_FOLDER='/home/matteo/Tools/gazebo/include/gazebo-10/gazebo/msgs/proto/' +GAZEBO_PROTO_FOLDER=:"${HOME}/installed/include/gazebo-10/gazebo/msgs/proto/" PY_PROTOBUF_FOLDER='../pyrevolve/spec/msgs/' # Generate Python protobuf files diff --git a/worlds/plane.realtime.world b/worlds/plane.realtime.world index 5864bebad4..7aea8eef44 100644 --- a/worlds/plane.realtime.world +++ b/worlds/plane.realtime.world @@ -7,10 +7,10 @@ - 0.001 + 0.005 - 800 + 200