From 5ddea571a77c116cd29020b0220783cd685c7a9f Mon Sep 17 00:00:00 2001 From: Aart Stuurman Date: Mon, 9 Aug 2021 12:59:11 +0200 Subject: [PATCH] Squash of complete experiment/heritability branch into one superheavy black whole megacommit --- .circleci/config.yml | 6 +- .dockerignore | 13 + .gitignore | 10 +- .gitmodules | 3 + Dockerfile | 5 +- check_running_experiments.sh | 3 + cpprevolve/revolve/brains/CMakeLists.txt | 5 + .../revolve/brains/controller/Controller.h | 4 +- .../brains/controller/DifferentialCPG.cpp | 371 +++++---- .../brains/controller/DifferentialCPG.h | 61 +- .../brains/controller/FixedAngleController.h | 39 + .../brains/controller/actuators/Actuator.h | 4 +- .../brains/controller/sensors/Sensor.h | 5 + cpprevolve/revolve/gazebo/CMakeLists.txt | 22 +- cpprevolve/revolve/gazebo/Types.h | 7 +- cpprevolve/revolve/gazebo/brains/Brains.h | 2 + .../revolve/gazebo/brains/DifferentialCPG.cpp | 11 +- .../gazebo/brains/DifferentialCPGClean.cpp | 63 ++ .../gazebo/brains/DifferentialCPGClean.h | 55 ++ .../gazebo/brains/DifferentialCPPNCPG.cpp | 53 ++ .../gazebo/brains/DifferentialCPPNCPG.h | 36 + .../gazebo/brains/FixedAngleController.h | 41 + .../revolve/gazebo/brains/NeuralNetwork.cpp | 18 +- cpprevolve/revolve/gazebo/brains/RLPower.cpp | 4 +- .../revolve/gazebo/brains/ThymioBrain.cpp | 4 +- .../revolve/gazebo/motors/ActuatorWrapper.h | 40 + .../revolve/gazebo/motors/JointMotor.cpp | 5 +- cpprevolve/revolve/gazebo/motors/JointMotor.h | 6 +- cpprevolve/revolve/gazebo/motors/Motor.cpp | 24 +- cpprevolve/revolve/gazebo/motors/Motor.h | 13 +- .../revolve/gazebo/motors/MotorFactory.cpp | 12 +- .../revolve/gazebo/motors/MotorFactory.h | 5 +- .../revolve/gazebo/motors/PositionMotor.cpp | 7 +- .../revolve/gazebo/motors/PositionMotor.h | 5 +- .../revolve/gazebo/motors/VelocityMotor.cpp | 7 +- .../revolve/gazebo/motors/VelocityMotor.h | 7 +- .../revolve/gazebo/msgs/robot_states.proto | 9 + .../revolve/gazebo/plugin/BodyAnalyzer.cpp | 8 + .../revolve/gazebo/plugin/FrameBuffer.cpp | 32 + .../revolve/gazebo/plugin/FrameBuffer.h | 46 ++ .../revolve/gazebo/plugin/RecorderCamera.cpp | 194 +++++ .../revolve/gazebo/plugin/RecorderCamera.h | 67 ++ .../revolve/gazebo/plugin/RobotController.cpp | 139 ++-- .../revolve/gazebo/plugin/RobotController.h | 2 + .../revolve/gazebo/plugin/VideoFileStream.cpp | 42 + .../revolve/gazebo/plugin/VideoFileStream.h | 29 + .../revolve/gazebo/plugin/WorldController.cpp | 162 +++- .../revolve/gazebo/plugin/WorldController.h | 3 + .../register_recorder_camera_plugin.cpp | 23 + .../revolve/gazebo/sensors/BatterySensor.cpp | 63 -- .../revolve/gazebo/sensors/BatterySensor.h | 53 -- .../revolve/gazebo/sensors/ImuSensor.cpp | 3 +- cpprevolve/revolve/gazebo/sensors/ImuSensor.h | 3 +- .../revolve/gazebo/sensors/LightSensor.cpp | 6 +- .../revolve/gazebo/sensors/LightSensor.h | 6 +- .../gazebo/sensors/PointIntensitySensor.cpp | 84 -- .../gazebo/sensors/PointIntensitySensor.h | 75 -- cpprevolve/revolve/gazebo/sensors/Sensor.cpp | 16 +- cpprevolve/revolve/gazebo/sensors/Sensor.h | 28 +- .../revolve/gazebo/sensors/SensorFactory.cpp | 12 - .../revolve/gazebo/sensors/SensorFactory.h | 2 + cpprevolve/revolve/gazebo/sensors/Sensors.h | 2 - .../revolve/gazebo/sensors/TouchSensor.cpp | 3 +- .../revolve/gazebo/sensors/TouchSensor.h | 3 +- .../revolve/gazebo/sensors/VirtualSensor.cpp | 58 -- .../revolve/gazebo/sensors/VirtualSensor.h | 84 -- .../revolve/raspberry/RaspController.cpp | 33 + direct_tree_test.py | 46 ++ docker/build_install_multineat.sh | 12 + docker/build_revolve.sh | 1 + .../MorphologyCompatibility.py | 138 ++++ experiments/brain-speciation/manager.py | 204 +++++ experiments/examples/manager_pop.py | 10 +- experiments/examples/one_robot.py | 53 ++ experiments/examples/tutorial3.py | 1 - experiments/examples/yaml/spider.yaml | 15 +- experiments/examples/yaml/spider9.yaml | 13 +- experiments/hardware/generate_robot_config.py | 69 ++ .../hardware/visualize_robot_in_simulation.py | 61 ++ experiments/heritability/manager.py | 160 ++++ experiments/heritability/manager_direct.py | 143 ++++ experiments/heritability/manager_lsystem.py | 178 +++++ experiments/heritability/test_manager.py | 138 ++++ experiments/lsystem-cppn-cpg/manager.py | 130 ++++ experiments/lsystem-cppn-cpg/manager_move.py | 130 ++++ .../lsystem-cppn-cpg/manager_move_block.py | 130 ++++ .../lsystem-cppn-cpg/manager_move_stack.py | 130 ++++ .../manager_move_stack_block.py | 130 ++++ experiments/lsystem-cppn-cpg/manager_stack.py | 130 ++++ .../lsystem-cppn-cpg/manager_stack_block.py | 130 ++++ .../consolidate_experiments.py | 109 +++ experiments/lsystem-improvements/manager.py | 119 +++ .../lsystem-improvements/summary_measures.R | 436 +++++++++++ .../check_running_experiments.sh | 3 + .../experimentation_script.sh | 30 + experiments/nsga2-gait-rotation/manager.py | 182 +++++ experiments/nsga2-gait-rotation/nsga2.py | 134 ++++ .../nsga2-gait-rotation/recovery_test.py | 183 +++++ experiments/nsga2-gait-rotation/test_nsga2.py | 130 ++++ .../check_running_experiments.sh | 3 + .../task-morphology/experimentation_script.sh | 32 + experiments/task-morphology/manager.py | 172 +++++ experiments/task-morphology/manager_test.py | 172 +++++ .../task-morphology/test_experiment.sh | 5 + experiments/unmanaged/create_charts.py | 85 +- pyrevolve/SDF/revolve_bot_sdf_builder.py | 15 +- pyrevolve/angle/evolve.py | 4 +- pyrevolve/angle/manage/robotmanager.py | 18 +- pyrevolve/angle/manage/world.py | 38 +- pyrevolve/angle/representation.py | 118 ++- .../angle/robogen/body_parts/__init__.py | 26 +- pyrevolve/angle/robogen/config.py | 4 + pyrevolve/angle/robogen/spec/body.py | 111 +-- pyrevolve/angle/robogen/spec/robot.py | 4 +- pyrevolve/config.py | 29 +- pyrevolve/data_analisys/visualize_robot.py | 197 ++++- pyrevolve/evolution/fitness.py | 132 ++++ pyrevolve/evolution/individual.py | 72 +- .../evolution/pop_management/generational.py | 3 - pyrevolve/evolution/population.py | 269 ------- .../__init__.py | 0 pyrevolve/evolution/population/population.py | 330 ++++++++ .../evolution/population/population_config.py | 85 ++ .../population_management.py} | 8 + pyrevolve/evolution/selection.py | 47 +- pyrevolve/evolution/speciation/__init__.py | 5 + pyrevolve/evolution/speciation/age.py | 65 ++ pyrevolve/evolution/speciation/genus.py | 274 +++++++ .../speciation/population_speciated.py | 130 ++++ .../speciation/population_speciated_config.py | 105 +++ .../population_speciated_management.py | 17 + pyrevolve/evolution/speciation/species.py | 237 ++++++ .../speciation/species_collection.py | 214 ++++++ pyrevolve/experiment_management.py | 480 ++++++++++-- pyrevolve/gazebo/manage/world.py | 86 ++- pyrevolve/genotype/direct_tree/__init__.py | 0 .../genotype/direct_tree/compound_mutation.py | 33 + .../direct_tree/direct_tree_config.py | 85 ++ .../direct_tree/direct_tree_crossover.py | 85 ++ .../direct_tree/direct_tree_genotype.py | 135 ++++ .../direct_tree/direct_tree_mutation.py | 278 +++++++ .../direct_tree/direct_tree_neat_genotype.py | 121 +++ .../direct_tree_random_generator.py | 157 ++++ .../genotype/direct_tree/direct_tree_utils.py | 75 ++ pyrevolve/genotype/genotype.py | 11 +- pyrevolve/genotype/lsystem_neat/__init__.py | 1 + pyrevolve/genotype/lsystem_neat/crossover.py | 48 ++ .../lsystem_neat/lsystem_neat_genotype.py | 121 +++ pyrevolve/genotype/lsystem_neat/mutation.py | 40 + .../genotype/neat_brain_genome/__init__.py | 1 + .../genotype/neat_brain_genome/crossover.py | 39 + .../genotype/neat_brain_genome/mutation.py | 21 + .../neat_brain_genome/neat_brain_genome.py | 246 ++++++ pyrevolve/genotype/plasticoding/__init__.py | 43 +- pyrevolve/genotype/plasticoding/alphabet.py | 128 ++++ .../crossover/standard_crossover.py | 14 +- pyrevolve/genotype/plasticoding/decoder.py | 541 +++++++++++++ .../genotype/plasticoding/initialization.py | 44 +- .../mutation/standard_mutation.py | 24 +- .../genotype/plasticoding/plasticoding.py | 724 +++--------------- pyrevolve/revolve_bot/body.py | 6 +- pyrevolve/revolve_bot/brain/__init__.py | 2 + pyrevolve/revolve_bot/brain/base.py | 5 + pyrevolve/revolve_bot/brain/brain_nn.py | 8 +- pyrevolve/revolve_bot/brain/cpg.py | 104 +++ pyrevolve/revolve_bot/brain/cppn_cpg.py | 129 ++++ pyrevolve/revolve_bot/brain/fixed_angle.py | 27 + .../{measure_body.py => measure_body_2d.py} | 10 +- .../revolve_bot/measure/measure_body_3d.py | 544 +++++++++++++ pyrevolve/revolve_bot/neural_net.py | 15 +- pyrevolve/revolve_bot/render/canvas.py | 58 +- pyrevolve/revolve_bot/render/render.py | 4 +- pyrevolve/revolve_bot/revolve_bot.py | 199 ++--- pyrevolve/revolve_bot/revolve_module.py | 113 ++- pyrevolve/spec/implementation.py | 12 +- pyrevolve/spec/msgs/body_pb2.py | 63 +- pyrevolve/spec/msgs/model_inserted_pb2.py | 19 +- pyrevolve/spec/msgs/neural_net_pb2.py | 82 +- pyrevolve/spec/msgs/parameter_pb2.py | 17 +- pyrevolve/spec/msgs/robot_pb2.py | 21 +- pyrevolve/spec/msgs/robot_states_pb2.py | 122 ++- pyrevolve/spec/msgs/sdf_body_analyze_pb2.py | 49 +- pyrevolve/spec/msgs/spline_policy_pb2.py | 45 +- pyrevolve/tol/manage/measures.py | 28 +- pyrevolve/tol/manage/robotmanager.py | 5 +- pyrevolve/tol/spec/__init__.py | 1 - pyrevolve/tol/spec/brain.py | 15 +- pyrevolve/tol/spec/robot.py | 13 +- pyrevolve/util/__init__.py | 1 - pyrevolve/util/supervisor/__init__.py | 2 - pyrevolve/util/supervisor/analyzer_queue.py | 13 +- pyrevolve/util/supervisor/simulator_queue.py | 81 +- pyrevolve/util/supervisor/supervisor.py | 379 --------- pyrevolve/util/supervisor/supervisor_multi.py | 16 +- pyrevolve/util/time.py | 2 +- requirements.txt | 6 +- revolve.py | 1 - revolve.sh | 9 + start_experiment.sh | 16 + test_py/evolution/__init__.py | 0 test_py/evolution/speciation/__init__.py | 0 test_py/evolution/speciation/species.py | 75 ++ test_py/multineat/__init__.py | 0 test_py/multineat/test_import.py | 55 ++ test_py/multineat/test_raw_genome.py | 149 ++++ test_py/multineat/test_xor.py | 134 ++++ test_py/plasticonding/test_development.py | 24 +- test_robots.py | 2 +- thirdparty/MultiNEAT | 1 + tools/proto2python.sh | 2 +- worlds/plane.realtime.world | 4 +- worlds/plane.recording.world | 79 ++ 212 files changed, 12308 insertions(+), 2810 deletions(-) create mode 100644 .dockerignore create mode 100755 check_running_experiments.sh create mode 100644 cpprevolve/revolve/brains/controller/FixedAngleController.h create mode 100644 cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.cpp create mode 100644 cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.h create mode 100644 cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.cpp create mode 100644 cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.h create mode 100644 cpprevolve/revolve/gazebo/brains/FixedAngleController.h create mode 100644 cpprevolve/revolve/gazebo/motors/ActuatorWrapper.h create mode 100644 cpprevolve/revolve/gazebo/plugin/FrameBuffer.cpp create mode 100644 cpprevolve/revolve/gazebo/plugin/FrameBuffer.h create mode 100644 cpprevolve/revolve/gazebo/plugin/RecorderCamera.cpp create mode 100644 cpprevolve/revolve/gazebo/plugin/RecorderCamera.h create mode 100644 cpprevolve/revolve/gazebo/plugin/VideoFileStream.cpp create mode 100644 cpprevolve/revolve/gazebo/plugin/VideoFileStream.h create mode 100644 cpprevolve/revolve/gazebo/plugin/register_recorder_camera_plugin.cpp delete mode 100644 cpprevolve/revolve/gazebo/sensors/BatterySensor.cpp delete mode 100644 cpprevolve/revolve/gazebo/sensors/BatterySensor.h delete mode 100644 cpprevolve/revolve/gazebo/sensors/PointIntensitySensor.cpp delete mode 100644 cpprevolve/revolve/gazebo/sensors/PointIntensitySensor.h delete mode 100644 cpprevolve/revolve/gazebo/sensors/VirtualSensor.cpp delete mode 100644 cpprevolve/revolve/gazebo/sensors/VirtualSensor.h create mode 100644 direct_tree_test.py create mode 100755 docker/build_install_multineat.sh create mode 100644 experiments/brain-speciation/MorphologyCompatibility.py create mode 100644 experiments/brain-speciation/manager.py create mode 100644 experiments/examples/one_robot.py create mode 100755 experiments/hardware/generate_robot_config.py create mode 100644 experiments/hardware/visualize_robot_in_simulation.py create mode 100644 experiments/heritability/manager.py create mode 100644 experiments/heritability/manager_direct.py create mode 100644 experiments/heritability/manager_lsystem.py create mode 100644 experiments/heritability/test_manager.py create mode 100644 experiments/lsystem-cppn-cpg/manager.py create mode 100644 experiments/lsystem-cppn-cpg/manager_move.py create mode 100644 experiments/lsystem-cppn-cpg/manager_move_block.py create mode 100644 experiments/lsystem-cppn-cpg/manager_move_stack.py create mode 100644 experiments/lsystem-cppn-cpg/manager_move_stack_block.py create mode 100644 experiments/lsystem-cppn-cpg/manager_stack.py create mode 100644 experiments/lsystem-cppn-cpg/manager_stack_block.py create mode 100755 experiments/lsystem-improvements/consolidate_experiments.py create mode 100755 experiments/lsystem-improvements/manager.py create mode 100644 experiments/lsystem-improvements/summary_measures.R create mode 100755 experiments/nsga2-gait-rotation/check_running_experiments.sh create mode 100755 experiments/nsga2-gait-rotation/experimentation_script.sh create mode 100644 experiments/nsga2-gait-rotation/manager.py create mode 100644 experiments/nsga2-gait-rotation/nsga2.py create mode 100644 experiments/nsga2-gait-rotation/recovery_test.py create mode 100755 experiments/nsga2-gait-rotation/test_nsga2.py create mode 100755 experiments/task-morphology/check_running_experiments.sh create mode 100755 experiments/task-morphology/experimentation_script.sh create mode 100644 experiments/task-morphology/manager.py create mode 100644 experiments/task-morphology/manager_test.py create mode 100755 experiments/task-morphology/test_experiment.sh delete mode 100644 pyrevolve/evolution/pop_management/generational.py delete mode 100644 pyrevolve/evolution/population.py rename pyrevolve/evolution/{pop_management => population}/__init__.py (100%) create mode 100644 pyrevolve/evolution/population/population.py create mode 100644 pyrevolve/evolution/population/population_config.py rename pyrevolve/evolution/{pop_management/steady_state.py => population/population_management.py} (53%) create mode 100644 pyrevolve/evolution/speciation/__init__.py create mode 100644 pyrevolve/evolution/speciation/age.py create mode 100644 pyrevolve/evolution/speciation/genus.py create mode 100644 pyrevolve/evolution/speciation/population_speciated.py create mode 100644 pyrevolve/evolution/speciation/population_speciated_config.py create mode 100644 pyrevolve/evolution/speciation/population_speciated_management.py create mode 100644 pyrevolve/evolution/speciation/species.py create mode 100644 pyrevolve/evolution/speciation/species_collection.py create mode 100644 pyrevolve/genotype/direct_tree/__init__.py create mode 100644 pyrevolve/genotype/direct_tree/compound_mutation.py create mode 100644 pyrevolve/genotype/direct_tree/direct_tree_config.py create mode 100644 pyrevolve/genotype/direct_tree/direct_tree_crossover.py create mode 100644 pyrevolve/genotype/direct_tree/direct_tree_genotype.py create mode 100644 pyrevolve/genotype/direct_tree/direct_tree_mutation.py create mode 100644 pyrevolve/genotype/direct_tree/direct_tree_neat_genotype.py create mode 100644 pyrevolve/genotype/direct_tree/direct_tree_random_generator.py create mode 100644 pyrevolve/genotype/direct_tree/direct_tree_utils.py create mode 100644 pyrevolve/genotype/lsystem_neat/__init__.py create mode 100644 pyrevolve/genotype/lsystem_neat/crossover.py create mode 100644 pyrevolve/genotype/lsystem_neat/lsystem_neat_genotype.py create mode 100644 pyrevolve/genotype/lsystem_neat/mutation.py create mode 100644 pyrevolve/genotype/neat_brain_genome/__init__.py create mode 100644 pyrevolve/genotype/neat_brain_genome/crossover.py create mode 100644 pyrevolve/genotype/neat_brain_genome/mutation.py create mode 100644 pyrevolve/genotype/neat_brain_genome/neat_brain_genome.py create mode 100644 pyrevolve/genotype/plasticoding/alphabet.py create mode 100644 pyrevolve/genotype/plasticoding/decoder.py create mode 100644 pyrevolve/revolve_bot/brain/cpg.py create mode 100644 pyrevolve/revolve_bot/brain/cppn_cpg.py create mode 100644 pyrevolve/revolve_bot/brain/fixed_angle.py rename pyrevolve/revolve_bot/measure/{measure_body.py => measure_body_2d.py} (97%) create mode 100644 pyrevolve/revolve_bot/measure/measure_body_3d.py delete mode 100644 pyrevolve/util/supervisor/supervisor.py create mode 100755 revolve.sh create mode 100755 start_experiment.sh create mode 100644 test_py/evolution/__init__.py create mode 100644 test_py/evolution/speciation/__init__.py create mode 100644 test_py/evolution/speciation/species.py create mode 100644 test_py/multineat/__init__.py create mode 100644 test_py/multineat/test_import.py create mode 100644 test_py/multineat/test_raw_genome.py create mode 100644 test_py/multineat/test_xor.py create mode 160000 thirdparty/MultiNEAT create mode 100644 worlds/plane.recording.world diff --git a/.circleci/config.yml b/.circleci/config.yml index c12d6bd149..c9445c6541 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -9,7 +9,7 @@ jobs: - run: git submodule update --init - setup_remote_docker: - docker_layer_caching: true +# docker_layer_caching: true - restore_cache: keys: @@ -45,7 +45,7 @@ jobs: steps: - setup_remote_docker: - docker_layer_caching: true +# docker_layer_caching: true - restore_cache: keys: @@ -69,7 +69,7 @@ jobs: steps: - setup_remote_docker: - docker_layer_caching: true +# docker_layer_caching: true - restore_cache: keys: 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/check_running_experiments.sh b/check_running_experiments.sh new file mode 100755 index 0000000000..45e4070682 --- /dev/null +++ b/check_running_experiments.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +for i in exp*log; do echo -n "$i "; value=$(grep "Exported snapshot" $i|tail -n1|sed -E "s/\[(.*),.*Exported snapshot ([0-9]+).*/\2 -> \1/g"); echo $value; done diff --git a/cpprevolve/revolve/brains/CMakeLists.txt b/cpprevolve/revolve/brains/CMakeLists.txt index 8b7f614ff7..07ad58b8ed 100644 --- a/cpprevolve/revolve/brains/CMakeLists.txt +++ b/cpprevolve/revolve/brains/CMakeLists.txt @@ -18,6 +18,8 @@ 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) + # Find NLOpt - Non Linear Optimization pkg_check_modules(NLOpt REQUIRED nlopt>=2.4) @@ -40,6 +42,9 @@ target_include_directories(revolve-learners target_include_directories(revolve-learners PUBLIC ${NLOpt_LIBRARY_DIRS}) +target_link_libraries(revolve-controllers + MultiNEAT::MultiNEAT) + install(TARGETS revolve-controllers revolve-learners RUNTIME DESTINATION bin LIBRARY DESTINATION lib) \ No newline at end of file diff --git a/cpprevolve/revolve/brains/controller/Controller.h b/cpprevolve/revolve/brains/controller/Controller.h index 1512af0d3a..4be01b3020 100644 --- a/cpprevolve/revolve/brains/controller/Controller.h +++ b/cpprevolve/revolve/brains/controller/Controller.h @@ -23,8 +23,8 @@ class Controller 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; diff --git a/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp b/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp index bba9ce77dc..46a21cc525 100644 --- a/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp +++ b/cpprevolve/revolve/brains/controller/DifferentialCPG.cpp @@ -22,39 +22,131 @@ #include "DifferentialCPG.h" // STL macros +#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) + const DifferentialCPG::ControllerParams params, + const std::vector> &actuators) + : next_state(nullptr) + , n_motors(actuators.size()) + , output(new double[actuators.size()]) + , sample(actuators.size(), 0) +{ + this->init_params_and_connections(params, actuators); + // Save weights for brain + assert(params.weights.size() == n_weights); + sample.resize(n_weights, 0); + for(size_t j = 0; j < n_weights; j++) + { + sample.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( + DifferentialCPG::ControllerParams params, + const std::vector> &actuators, + const NEAT::Genome &gen) : next_state(nullptr) , n_motors(actuators.size()) , output(new double[actuators.size()]) + , sample(actuators.size(), 0) +{ + this->init_params_and_connections(params, actuators); + + // build the NN according to the genome + NEAT::NeuralNetwork net; + gen.BuildPhenotype(net); + + // get weights for each connection + // assuming that connections are distinct for each direction + sample.resize(n_weights, 0); + std::vector inputs(8); + + for(const std::pair< const std::tuple< int, int, int>, 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; + + net.Input(inputs); + 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 sample[" << k << "]\t-> " << weight << std::endl; +#endif + sample.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; + net.Input(inputs); + 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 sample[" << k << "]\t-> " << weight << std::endl; +#endif + sample.at(k) = weight; // order of weights corresponds to order of connections. + } + + // Set ODE matrix at initialization + set_ode_matrix(); + + 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; @@ -68,12 +160,13 @@ DifferentialCPG::DifferentialCPG( this->abs_output_bound = params.abs_output_bound; size_t j=0; - for (const std::unique_ptr &actuator: actuators) + 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; @@ -82,60 +175,72 @@ DifferentialCPG::DifferentialCPG( { frame_of_reference = -1; } - // We are a right neuron + // 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 + 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) { // 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); // TODO: Verify for non-spiders if (std::fabs(dist_x + dist_y - 2) < 0.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 sample[" << 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 +249,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; } /** @@ -180,8 +272,8 @@ DifferentialCPG::~DifferentialCPG() * @param _step */ void DifferentialCPG::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) { @@ -200,44 +292,47 @@ 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++) { // 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< double > 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 sample[" << k << "]\t-> " << this->sample.at(k) << std::endl; +#endif + auto weight = this->sample.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 +340,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; } @@ -271,6 +368,7 @@ void DifferentialCPG::set_ode_matrix(){ // Add connection to seen connections if(l1 > l2) { + // swap l1 and l2 int l1_old = l1; l1 = l2; l2 = l1_old; @@ -283,14 +381,25 @@ void DifferentialCPG::set_ode_matrix(){ { 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" << 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 sample[" << sample_index << "]\t-> " << this->sample.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->sample.at(sample_index) * + (this->range_ub - this->range_lb) + this->range_lb; // Set connection in weight matrix matrix[l1][l2] = w; @@ -298,58 +407,56 @@ 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++; } } @@ -364,12 +471,9 @@ void DifferentialCPG::step( 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; @@ -408,17 +512,16 @@ void DifferentialCPG::step( // 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) + 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,7 +529,7 @@ 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]; // Use frame of reference if(use_frame_of_reference) @@ -434,11 +537,11 @@ void DifferentialCPG::step( 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); + this->output[j] = this->signal_factor_left_right*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0 * x_input / 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); + this->output[j] = this->signal_factor_mid*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0 * x_input / this->abs_output_bound)) - 1); } else { @@ -449,7 +552,7 @@ void DifferentialCPG::step( // 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] = this->signal_factor_all_*this->abs_output_bound*((2.0)/(1.0 + std::pow(2.718, -2.0 * x_input / this->abs_output_bound)) - 1); } } i++; diff --git a/cpprevolve/revolve/brains/controller/DifferentialCPG.h b/cpprevolve/revolve/brains/controller/DifferentialCPG.h index d223f45d9a..f21c993b34 100644 --- a/cpprevolve/revolve/brains/controller/DifferentialCPG.h +++ b/cpprevolve/revolve/brains/controller/DifferentialCPG.h @@ -8,9 +8,11 @@ #include "Controller.h" #include "actuators/Actuator.h" #include "sensors/Sensor.h" + #include +#include #include -#include +#include typedef std::vector< double > state_type; @@ -30,28 +32,37 @@ class DifferentialCPG double signal_factor_left_right; 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( + DifferentialCPG::ControllerParams params, + const std::vector> &_actuators); + + /// \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); + DifferentialCPG::ControllerParams params, + const std::vector> &_actuators, + const NEAT::Genome &config_cppn_genome); /// \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 std::vector> &actuators, + const std::vector> &sensors, const double _time, const double _step) override; @@ -61,6 +72,8 @@ class DifferentialCPG const double time, const double step); + void init_params_and_connections(const ControllerParams ¶ms, const std::vector> &actuators); + void set_ode_matrix(); private: @@ -68,24 +81,28 @@ class DifferentialCPG void reset_neuron_state(); 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 @@ -108,7 +125,7 @@ class DifferentialCPG double range_ub; /// \brief Loaded sample - Eigen::VectorXd sample; + std::vector sample; /// \brief The number of weights to optimize size_t n_weights; @@ -135,7 +152,7 @@ class DifferentialCPG bool use_frame_of_reference; double abs_output_bound; -}; + }; } diff --git a/cpprevolve/revolve/brains/controller/FixedAngleController.h b/cpprevolve/revolve/brains/controller/FixedAngleController.h new file mode 100644 index 0000000000..d94549ae29 --- /dev/null +++ b/cpprevolve/revolve/brains/controller/FixedAngleController.h @@ -0,0 +1,39 @@ +// +// 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) + : 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..21510077cf 100644 --- a/cpprevolve/revolve/brains/controller/actuators/Actuator.h +++ b/cpprevolve/revolve/brains/controller/actuators/Actuator.h @@ -28,9 +28,9 @@ class Actuator 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/Sensor.h b/cpprevolve/revolve/brains/controller/sensors/Sensor.h index 496e54bf5f..3f293da27f 100644 --- a/cpprevolve/revolve/brains/controller/sensors/Sensor.h +++ b/cpprevolve/revolve/brains/controller/sensors/Sensor.h @@ -17,6 +17,11 @@ class Sensor /// \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..53fdf56a3e 100644 --- a/cpprevolve/revolve/gazebo/CMakeLists.txt +++ b/cpprevolve/revolve/gazebo/CMakeLists.txt @@ -45,7 +45,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 +112,9 @@ include_directories( ${GAZEBO_PROTO_INCLUDE} ) +# Find OpenCV +find_package( OpenCV REQUIRED ) + # Add Gazebo C++ flags (this includes c++11) list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}") @@ -200,6 +203,7 @@ add_library( target_link_libraries( revolve-gazebo + revolve-controllers ${GAZEBO_LIBRARIES} ${Boost_LIBRARIES} ${GSL_LIBRARIES} @@ -248,6 +252,22 @@ target_link_libraries(TorusWorldPlugin ${GAZEBO_LIBRARIES} ) +# Create RecorderCamera plugin +add_library(RecorderCameraPlugin SHARED + plugin/RecorderCamera.cpp + plugin/register_recorder_camera_plugin.cpp + plugin/FrameBuffer.cpp + plugin/VideoFileStream.cpp +) +target_link_libraries(RecorderCameraPlugin + revolve-gazebo + ${GAZEBO_LIBRARIES} + CameraPlugin #Gazebo-cmake being crap + ${OpenCV_LIBS} +) +target_include_directories(RecorderCameraPlugin + PRIVATE ${OpenCV_INCLUDE_DIRS}) + # 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..99b75e8233 100644 --- a/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPG.cpp @@ -525,7 +525,8 @@ void DifferentialCPG::save_fitness(){ * Struct that holds the parameters on which BO is called. This is required * by limbo. */ -struct DifferentialCPG::Params{ +struct DifferentialCPG::Params + { struct bayes_opt_boptimizer : public limbo::defaults::bayes_opt_boptimizer { }; @@ -710,8 +711,8 @@ void DifferentialCPG::Update( unsigned int p = 0; for (const auto &sensor : _sensors) { - sensor->Read(this->input + p); - p += sensor->Inputs(); + sensor->read(this->input + p); + p += sensor->n_inputs(); } this->evaluator->Update(this->robot->WorldPose(), _time, _step); @@ -827,8 +828,8 @@ void DifferentialCPG::Update( p = 0; for (const auto &motor: _motors) { - motor->Update(this->output + p, _step); - p += motor->Outputs(); + motor->write(this->output + p, _step); + p += motor->n_outputs(); } } diff --git a/cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.cpp new file mode 100644 index 0000000000..ebedfd3b6f --- /dev/null +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.cpp @@ -0,0 +1,63 @@ +// +// 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) + : Brain() + , revolve::DifferentialCPG(load_params_from_sdf(brain_sdf), _motors) +{} + +DifferentialCPGClean::DifferentialCPGClean(const sdf::ElementPtr brain_sdf, + const std::vector &_motors, + const NEAT::Genome &genome) + : Brain() + , revolve::DifferentialCPG(load_params_from_sdf(brain_sdf), _motors, genome) +{} + + +void DifferentialCPGClean::Update(const std::vector &_motors, + const std::vector &_sensors, + const double _time, + const double _step) +{ + this->::revolve::DifferentialCPG::update(_motors, _sensors, _time, _step); +} + +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"); + revolve::DifferentialCPG::ControllerParams params; + params.reset_neuron_random = (controller_sdf->GetAttribute("reset_neuron_random")->GetAsString() == "true"); + params.use_frame_of_reference = (controller_sdf->GetAttribute("use_frame_of_reference")->GetAsString() == "true"); + params.init_neuron_state = stod(controller_sdf->GetAttribute("init_neuron_state")->GetAsString()); + params.range_ub = stod(controller_sdf->GetAttribute("range_ub")->GetAsString()); + params.signal_factor_all = stod(controller_sdf->GetAttribute("signal_factor_all")->GetAsString()); + params.signal_factor_mid = stod(controller_sdf->GetAttribute("signal_factor_mid")->GetAsString()); + params.signal_factor_left_right = stod(controller_sdf->GetAttribute("signal_factor_left_right")->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) { + 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..46ea487a54 --- /dev/null +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPGClean.h @@ -0,0 +1,55 @@ +// +// 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 Brain, private 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); + + /// \brief updates the motor signals + /// \param[in] _motors vector list of motors + /// \param[in] _sensors vector list of sensors + /// \param[in] _time double + /// \param[in] _step double + void Update(const std::vector &_motors, + const std::vector &_sensors, + const double _time, + const double _step) override; + + protected: + explicit DifferentialCPGClean(const sdf::ElementPtr brain_sdf, + const std::vector &_motors, + const NEAT::Genome &genome); + + /// \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/DifferentialCPPNCPG.cpp b/cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.cpp new file mode 100644 index 0000000000..eabd2013e3 --- /dev/null +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.cpp @@ -0,0 +1,53 @@ +// +// 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) + : DifferentialCPGClean( + brain_sdf, + motors, + DifferentialCPPNCPG::load_cppn_genome_from_sdf(brain_sdf)) +{} + +/// \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..73b58f35e7 --- /dev/null +++ b/cpprevolve/revolve/gazebo/brains/DifferentialCPPNCPG.h @@ -0,0 +1,36 @@ +// +// 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); + + 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..930a882a79 --- /dev/null +++ b/cpprevolve/revolve/gazebo/brains/FixedAngleController.h @@ -0,0 +1,41 @@ +// +// 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 Brain, private revolve::FixedAngleController +{ +public: + explicit FixedAngleController(double angle) + : Brain() + , revolve::FixedAngleController(angle) + {} + + void Update(const std::vector &motors, + const std::vector &sensors, + const double time, + const double step) override + { + revolve::FixedAngleController::update(motors, sensors, time, step); + } +}; + +} +} + + +#endif //GAZEBO_REVOLVE_FIXEDANGLECONTROLLER_H diff --git a/cpprevolve/revolve/gazebo/brains/NeuralNetwork.cpp b/cpprevolve/revolve/gazebo/brains/NeuralNetwork.cpp index 94fd84f372..77eebbbba7 100644 --- a/cpprevolve/revolve/gazebo/brains/NeuralNetwork.cpp +++ b/cpprevolve/revolve/gazebo/brains/NeuralNetwork.cpp @@ -164,7 +164,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 +177,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 +205,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 +217,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")) { @@ -405,8 +407,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 +421,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/RLPower.cpp b/cpprevolve/revolve/gazebo/brains/RLPower.cpp index 527ced5c72..a2b8e7f1dc 100644 --- a/cpprevolve/revolve/gazebo/brains/RLPower.cpp +++ b/cpprevolve/revolve/gazebo/brains/RLPower.cpp @@ -114,8 +114,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/ThymioBrain.cpp b/cpprevolve/revolve/gazebo/brains/ThymioBrain.cpp index ff3c93b8ad..54265a8998 100644 --- a/cpprevolve/revolve/gazebo/brains/ThymioBrain.cpp +++ b/cpprevolve/revolve/gazebo/brains/ThymioBrain.cpp @@ -66,7 +66,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/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..59764f49da 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..e512612ae8 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,6 +60,7 @@ 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"); @@ -74,7 +77,8 @@ MotorPtr MotorFactory::Create(sdf::ElementPtr _motorSdf) 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..530354909f 100644 --- a/cpprevolve/revolve/gazebo/motors/MotorFactory.h +++ b/cpprevolve/revolve/gazebo/motors/MotorFactory.h @@ -47,11 +47,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..5ddbbb2899 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.cpp @@ -30,8 +30,9 @@ 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) + const sdf::ElementPtr _motor, + const std::string &_coordinates) + : JointMotor(std::move(_model), _partId, _motorId, _motor, 1, _coordinates) , positionTarget_(0) , noise_(0) { @@ -76,7 +77,7 @@ PositionMotor::~PositionMotor() = default; // } ///////////////////////////////////////////////// -void PositionMotor::Update( +void PositionMotor::write( const double *outputs, double /*step*/) { diff --git a/cpprevolve/revolve/gazebo/motors/PositionMotor.h b/cpprevolve/revolve/gazebo/motors/PositionMotor.h index 7f07c831a5..429b696404 100644 --- a/cpprevolve/revolve/gazebo/motors/PositionMotor.h +++ b/cpprevolve/revolve/gazebo/motors/PositionMotor.h @@ -44,13 +44,14 @@ 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; diff --git a/cpprevolve/revolve/gazebo/motors/VelocityMotor.cpp b/cpprevolve/revolve/gazebo/motors/VelocityMotor.cpp index 9a45596621..d8be304c46 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,7 @@ VelocityMotor::~VelocityMotor() { } -void VelocityMotor::Update( +void VelocityMotor::write( const double *outputs, double /*step*/) { diff --git a/cpprevolve/revolve/gazebo/motors/VelocityMotor.h b/cpprevolve/revolve/gazebo/motors/VelocityMotor.h index 3a978c4922..29868d5359 100644 --- a/cpprevolve/revolve/gazebo/motors/VelocityMotor.h +++ b/cpprevolve/revolve/gazebo/motors/VelocityMotor.h @@ -45,7 +45,8 @@ namespace revolve ::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(); @@ -55,7 +56,7 @@ namespace revolve /// maximum velocity set by the motor. /// \param[in,out] outputs /// \param[in] step - virtual void Update( + virtual void write( const double *outputs, double step); @@ -85,7 +86,7 @@ namespace revolve /// \brief PID for this velocity motor protected: ::gazebo::common::PID pid_; - }; +}; } // namespace gazebo } // namespace revolve 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/FrameBuffer.cpp b/cpprevolve/revolve/gazebo/plugin/FrameBuffer.cpp new file mode 100644 index 0000000000..521af72ed3 --- /dev/null +++ b/cpprevolve/revolve/gazebo/plugin/FrameBuffer.cpp @@ -0,0 +1,32 @@ +// +// Created by matteo on 12/10/20. +// + +#include "FrameBuffer.h" +#include + +using namespace revolve; + +FrameBuffer::FrameBuffer(const void *buffer, unsigned short width, unsigned short height, unsigned short depth) + : cv_frame(height, width, CV_8UC3) //CV_MAKETYPE(cv::DataType::type, depth)) +{ + std::memcpy(cv_frame.data, buffer, width*height*depth*sizeof(signed char)); +} + +const cv::Mat& FrameBuffer::to_opencv() const +{ + cv::cvtColor(cv_frame, cv_frame, cv::COLOR_BGR2RGB); + return cv_frame; +} + + +cv::Mat FrameBufferRef::to_opencv() const +{ + int type = CV_MAKETYPE(cv::DataType::type, depth); + // Original image should be R8G8B8 + cv::Mat cv_frame(height, width, type); +// cv::cvtColor(cv_frame, cv_frame, cv::COLOR_RGB2BGR); + std::memcpy(cv_frame.data, this->buffer, width*height*depth*sizeof(char)); + + return cv_frame; +} \ No newline at end of file diff --git a/cpprevolve/revolve/gazebo/plugin/FrameBuffer.h b/cpprevolve/revolve/gazebo/plugin/FrameBuffer.h new file mode 100644 index 0000000000..e4c2b19675 --- /dev/null +++ b/cpprevolve/revolve/gazebo/plugin/FrameBuffer.h @@ -0,0 +1,46 @@ +// +// Created by matteo on 12/10/20. +// + +#ifndef REVOLVE_FRAMEBUFFER_H +#define REVOLVE_FRAMEBUFFER_H + +#include +#include + +namespace revolve { + +class FrameBuffer { +public: + FrameBuffer(const void *buffer, unsigned short width, unsigned short height, unsigned short depth); + ~FrameBuffer() = default; + + const cv::Mat &to_opencv() const; + +private: + cv::Mat cv_frame; +}; + +class FrameBufferRef { +public: + FrameBufferRef(const void *buffer, unsigned short width, unsigned short height, unsigned short depth, Ogre::PixelFormat) + : buffer(buffer) + , width(width) + , height(height) + , depth(depth) + {} + ~FrameBufferRef() = default; + + cv::Mat to_opencv() const; + +private: + const void *buffer; + const unsigned short width; + const unsigned short height; + const unsigned short depth; +}; + +} + + +#endif //REVOLVE_FRAMEBUFFER_H diff --git a/cpprevolve/revolve/gazebo/plugin/RecorderCamera.cpp b/cpprevolve/revolve/gazebo/plugin/RecorderCamera.cpp new file mode 100644 index 0000000000..2bd1fcdcb4 --- /dev/null +++ b/cpprevolve/revolve/gazebo/plugin/RecorderCamera.cpp @@ -0,0 +1,194 @@ +// +// Created by matteo on 12/7/20. +// + +#include "RecorderCamera.h" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace rvgz = revolve::gazebo; + +rvgz::RecorderCamera::RecorderCamera() + : gz::CameraPlugin() + , counter(0) + , total_frames(0) + , video(nullptr) +{} + +void rvgz::RecorderCamera::Load(gz::sensors::SensorPtr parent, sdf::ElementPtr pluginSDF) +{ + CameraPlugin::Load(parent, pluginSDF); + + std::string world_name = parent->WorldName(); + this->world = gz::physics::get_world(world_name); + this->SDF = pluginSDF; + this->camera_model = world->ModelByName("Recording"); + assert(camera_model); + + sdf::ElementPtr MovieLength = pluginSDF->GetElement("movie_length"); + double movie_length_seconds = std::stod(MovieLength->GetAttribute("seconds")->GetAsString()); + this->total_frames = static_cast(movie_length_seconds * parentSensor->UpdateRate()); + std::cerr << "movie length seconds = " << movie_length_seconds << std::endl + << "update rate = " << parentSensor->UpdateRate() << std::endl + << "total frames = " << total_frames << std::endl; +} + +void rvgz::RecorderCamera::OnNewFrame(const unsigned char *image, + unsigned int width, + unsigned int height, + unsigned int depth, + const std::string &/*format*/) +{ + // Format is actually broken. Says RGB format, image is BGR. + + // Check if video is finished + if (counter > total_frames) { + //TODO remove print + std::cerr << "FINI: counter > total_frames: " << counter << " > " << total_frames << std::endl; + return; + } + + // Here we lock the physics so we are sure not to lose any frame for the video + gz::physics::PhysicsEnginePtr physicsEngine = world->Physics(); + boost::recursive_mutex::scoped_lock lock_physics(*physicsEngine->GetPhysicsUpdateMutex()); + + if (!robot) { + robot = SearchForRobotModel(); + if (!robot) { + // search again in the next frame, after the camera is moved on top of the robot + return; + } + std::cerr << "MODEL FOUND! " << robot->GetName() << std::endl; + // MODEL IS FINALLY FOUND + this->RobotModelFound(); + // skip the first frame + image = nullptr; + } + if (world->IsPaused()) { + // don't capture frames if it's paused + return; + } + + assert(video); + + // reposition camera on top of the robot. + // This code effectively makes the camera follow the robot + ignition::math::Pose3d robot_pose = robot->WorldPose(); + ignition::math::Pose3d camera_pose = camera_model->WorldPose(); + camera_pose.Pos()[0] = robot_pose.Pos()[0] - 1.8; + camera_pose.Pos()[1] = robot_pose.Pos()[1]; +// camera_pose.Pos()[2] = robot_pose.Pos()[2] + 2; + + camera_model->SetWorldPose(camera_pose); + + + if (image == nullptr) + { + std::string camera_name = this->parentSensor->Camera()->Name(); + ::gazebo::common::Console::err(__FILE__, __LINE__) << "Can't save an empty image for camera: " << camera_name << std::endl; + return; + } + + counter++; + + // format reported to be R8G8B8 while actually is B8G8R8 + ::revolve::FrameBuffer frame(image, width, height, depth); + + float fps = camera->AvgFPS(); + if (fps != last_fps) { + std::cout << "FPS " << fps << std::endl; + last_fps = fps; + } + + (*video) << frame; + + // Check if video is finished + if (counter > total_frames) { + // delete the video object closes the video stream + ::gazebo::common::Console::msg() << "Video finished, closing the plugin" << std::endl; + video.reset(nullptr); + this->camera->Fini(); + lock_physics.release(); + ::gazebo::common::Console::msg() << "EXIT" << std::endl; + std::exit(0); + //this->parentSensor->Fini(); + } +} + +void revolve::gazebo::RecorderCamera::Init() +{ + ::gazebo::common::Console::msg() << "rvgz::RecorderCamera::Init\n"; + SensorPlugin::Init(); + //this->parentSensor->ConnectUpdated() + + //InitRecorder(); recorder is lazy-inited when the robot is found in the environemnt + robot.reset(); + video.reset(); +} + +void revolve::gazebo::RecorderCamera::Reset() +{ + SensorPlugin::Reset(); + robot.reset(); + video.reset(); +} + +void revolve::gazebo::RecorderCamera::InitRecorder() +{ + std::string filename = this->SaveFileName(); + std::string filepath = this->SaveFilePath() + filename; + + // make sure path exists + boost::filesystem::path folder_path = filepath; + folder_path.remove_filename(); + boost::filesystem::create_directory(folder_path); + + ::gazebo::common::Console::msg() << "Saving video recording in \"" << filepath << "\"\n"; + + video = std::make_unique<::revolve::VideoFileStream>( + filepath.c_str(), + parentSensor->UpdateRate(), + cv::Size(parentSensor->ImageWidth(),parentSensor->ImageHeight()) + ); +} + +std::string revolve::gazebo::RecorderCamera::SaveFilePath() const +{ + return SDF->Get("save_file_path"); +} + +std::string revolve::gazebo::RecorderCamera::SaveFileName() const +{ + return this->robot->GetName() + ".mp4"; +} + + +::gazebo::physics::ModelPtr revolve::gazebo::RecorderCamera::SearchForRobotModel() const +{ + namespace gzp = ::gazebo::physics; + gzp::Model_V models = world->Models(); + ::gazebo::common::Console::msg() << "Searching for robot:" << std::endl; + + gzp::ModelPtr robot_model; + for (gzp::ModelPtr &model_ : models) { + std::string model_name = model_->GetName(); + //std::string::rfind("string", 0) is a way of testing "starts_with" + if (model_name.rfind("robot_", 0) == 0) { + ::gazebo::common::Console::msg() << "ROBOT " << model_name << " FOUND!" << std::endl; + robot_model = model_; + break; + } + } + return robot_model; +} + +void revolve::gazebo::RecorderCamera::RobotModelFound() +{ + this->InitRecorder(); +} diff --git a/cpprevolve/revolve/gazebo/plugin/RecorderCamera.h b/cpprevolve/revolve/gazebo/plugin/RecorderCamera.h new file mode 100644 index 0000000000..dea3acddbf --- /dev/null +++ b/cpprevolve/revolve/gazebo/plugin/RecorderCamera.h @@ -0,0 +1,67 @@ +// +// Created by matteo on 12/7/20. +// + +#ifndef REVOLVE_RECORDERCAMERA_H +#define REVOLVE_RECORDERCAMERA_H + +#include +#include +#include "VideoFileStream.h" +#include + +namespace gz = ::gazebo; + +namespace revolve { +namespace gazebo { + +class RecorderCamera : public gz::CameraPlugin { +public: + RecorderCamera(); + virtual ~RecorderCamera() = default; + + void Load(gz::sensors::SensorPtr parent, sdf::ElementPtr sdf) override; + + void Init() override; + + void Reset() override; + + void OnNewFrame(const unsigned char *image, + unsigned int width, + unsigned int height, + unsigned int depth, + const std::string &format) override; + + std::string SaveFilePath() const; + std::string SaveFileName() const; + +private: + void InitRecorder(); + ::gazebo::physics::ModelPtr SearchForRobotModel() const; + void RobotModelFound(); + +private: + unsigned int counter; + unsigned int total_frames; + + /// \brief Video Recorder object + std::unique_ptr<::revolve::VideoFileStream> video; + + /// \brief Pointer to the world + ::gazebo::physics::WorldPtr world; + /// \brief Pointer to the Recording Model + ::gazebo::physics::ModelPtr camera_model; + /// \brief Pointer to the robot + ::gazebo::physics::ModelPtr robot; + /// \brief Pointer to the plugin SDF structure + sdf::ElementPtr SDF; + + // To print how many FPS the video recorder is doing, + // without overwelming the program output + float last_fps = -1; +}; + +} +} + +#endif //REVOLVE_RECORDERCAMERA_H diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp index a02aac322c..9165a9fda6 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.cpp +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include "RobotController.h" @@ -53,56 +54,62 @@ 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")) - { - std::cerr - << "No `rv:robot_config` element found, controller not initialized." - << std::endl; - return; - } - - 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; - } - - // Load motors - this->motorFactory_ = this->MotorFactory(_parent); - this->LoadActuators(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); - - // Call the battery loader - this->LoadBattery(robotConfiguration); - - // Call startup function which decides on actuation - this->Startup(_parent, _sdf); + try { + // 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"); + + 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 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); + + // Call the battery loader + this->LoadBattery(robotConfiguration); + + // 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; + } } ///////////////////////////////////////////////// @@ -200,25 +207,39 @@ 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_)); + 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) { brain_.reset(new DifferentialCPG(this->model_, _sdf, motors_, sensors_)); } + 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) + { + 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."); diff --git a/cpprevolve/revolve/gazebo/plugin/RobotController.h b/cpprevolve/revolve/gazebo/plugin/RobotController.h index 6fe8e7bd69..c72e36b3d2 100644 --- a/cpprevolve/revolve/gazebo/plugin/RobotController.h +++ b/cpprevolve/revolve/gazebo/plugin/RobotController.h @@ -29,6 +29,8 @@ #include #include +#include "revolve/brains/controller/sensors/Sensor.h" +#include "revolve/brains/controller/actuators/Actuator.h" namespace revolve { diff --git a/cpprevolve/revolve/gazebo/plugin/VideoFileStream.cpp b/cpprevolve/revolve/gazebo/plugin/VideoFileStream.cpp new file mode 100644 index 0000000000..e4c42db5dc --- /dev/null +++ b/cpprevolve/revolve/gazebo/plugin/VideoFileStream.cpp @@ -0,0 +1,42 @@ +// +// Created by matteo on 12/10/20. +// + +#include "VideoFileStream.h" +#include + +using namespace revolve; + +VideoFileStream::VideoFileStream(const char *filename, double fps, cv::Size frame_size) +// : video(filename, cv::VideoWriter::fourcc('V','P','9','0'), fps, std::move(frame_size), true) + : video(filename, cv::VideoWriter::fourcc('h','2','6','4'), fps, std::move(frame_size), true) + , filename(filename) +{ + if (!video.isOpened()) { + std::ostringstream error_message; + error_message << "Could not open video file \"" << filename << "\" for writing"; + throw std::runtime_error(error_message.str()); + } +} + +VideoFileStream::~VideoFileStream() +{ + video.release(); +} + +VideoFileStream &VideoFileStream::operator<<(const cv::Mat &framebuffer) +{ + video << framebuffer; + return *this; +} + +VideoFileStream &VideoFileStream::operator<<(const FrameBuffer &frame) +{ + return (*this) << frame.to_opencv(); +} + +VideoFileStream &VideoFileStream::operator<<(const FrameBufferRef frame) +{ + cv::Mat framebuffer = frame.to_opencv(); + return (*this) << framebuffer; +} diff --git a/cpprevolve/revolve/gazebo/plugin/VideoFileStream.h b/cpprevolve/revolve/gazebo/plugin/VideoFileStream.h new file mode 100644 index 0000000000..8ebab9cb98 --- /dev/null +++ b/cpprevolve/revolve/gazebo/plugin/VideoFileStream.h @@ -0,0 +1,29 @@ +// +// Created by matteo on 12/10/20. +// + +#ifndef REVOLVE_VIDEOFILESTREAM_H +#define REVOLVE_VIDEOFILESTREAM_H + +#include "FrameBuffer.h" +#include + +namespace revolve { + +class VideoFileStream { +public: + VideoFileStream(const char *filename, double fps, cv::Size frameSize); + ~VideoFileStream(); + + VideoFileStream& operator<<(const cv::Mat &frame); + VideoFileStream& operator<<(const FrameBuffer &frame); + VideoFileStream& operator<<(const FrameBufferRef frame); + +private: + cv::VideoWriter video; + const std::string filename; +}; + +} + +#endif //REVOLVE_VIDEOFILESTREAM_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_recorder_camera_plugin.cpp b/cpprevolve/revolve/gazebo/plugin/register_recorder_camera_plugin.cpp new file mode 100644 index 0000000000..134208f8f9 --- /dev/null +++ b/cpprevolve/revolve/gazebo/plugin/register_recorder_camera_plugin.cpp @@ -0,0 +1,23 @@ +/* +* Copyright (C) 2018 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: Matteo De Carlo +* +*/ + +#include +#include + +using namespace gazebo; +GZ_REGISTER_SENSOR_PLUGIN(revolve::gazebo::RecorderCamera) 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/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/direct_tree_test.py b/direct_tree_test.py new file mode 100644 index 0000000000..65e3308232 --- /dev/null +++ b/direct_tree_test.py @@ -0,0 +1,46 @@ +from pyrevolve.genotype.direct_tree.direct_tree_config import DirectTreeGenotypeConfig +from pyrevolve.genotype.direct_tree.direct_tree_genotype import DirectTreeGenotype + +if __name__ == "__main__": + + conf = DirectTreeGenotypeConfig( + min_parts=1, + mutation_p_delete_subtree=1, + mutation_p_duplicate_subtree=1, + mutation_p_swap_subtree=1, + mutation_p_mutate_oscillators=1, + mutation_p_mutate_oscillator=1, + mutation_p_generate_subtree=1, + ) + + genome1 = DirectTreeGenotype(conf, 1) + genome1.random_initialization() + genome1.export_genotype("/tmp/test.yaml") + robot1 = genome1.develop() + + # TEST LOAD AND SAVE + genome_reload = DirectTreeGenotype(conf, 1) + genome_reload.load_genotype("/tmp/test.yaml") + genome_reload.export_genotype("/tmp/test_reload.yaml") + + g1_file = None + g2_file = None + with open('/tmp/test.yaml') as f: + g1_file = f.read() + with open('/tmp/test_reload.yaml') as f: + g2_file = f.read() + + assert g1_file == g2_file + + # TEST MUTATION + genome3 = genome1.clone() + genome3 = genome3.mutate() + genome3.export_genotype("/tmp/test3.yaml") + + # TEST CROSSOVER + genome2 = DirectTreeGenotype(conf, 2) + genome2.random_initialization() + genome2.export_genotype("/tmp/test2.yaml") + + genome4 = genome1.crossover(genome2, 4) + genome4.export_genotype("/tmp/test4.yaml") 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/brain-speciation/MorphologyCompatibility.py b/experiments/brain-speciation/MorphologyCompatibility.py new file mode 100644 index 0000000000..a2fff685ee --- /dev/null +++ b/experiments/brain-speciation/MorphologyCompatibility.py @@ -0,0 +1,138 @@ +from __future__ import annotations + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import Optional + from pyrevolve.evolution.individual import Individual + + +class MorphologyCompatibility: + def __init__(self, + total_threshold: float = 1.0, + branching_modules_count: float = 0.0, + branching: float = 0.0, + extremities: float = 0.0, + limbs: float = 0.0, + extensiveness: float = 0.0, + length_of_limbs: float = 0.0, + coverage: float = 0.0, + joints: float = 0.0, + active_hinges_count: float = 0.0, + proportion: float = 0.0, + width: float = 0.0, + height: float = 0.0, + z_depth: float = 0.0, + absolute_size: float = 0.0, + size: float = 0.0, + sensors: float = 0.0, + symmetry: float = 0.0, + hinge_count: float = 0.0, + brick_count: float = 0.0, + brick_sensor_count: float = 0.0, + touch_sensor_count: float = 0.0, + free_slots: float = 0.0, + height_base_ratio: float = 0.0, + max_permitted_modules: Optional[int] = None, + symmetry_vertical: float = 0.0, + base_density: float = 0.0, + bottom_layer: float = 0.0, + ): + # Total threshold + self.total_threshold: float = total_threshold + + # Absolute branching + self.branching_modules_count: float = branching_modules_count + # Relative branching + self.branching: float = branching + # Absolute number of limbs + self.extremities: float = extremities + # Relative number of limbs + self.limbs: float = limbs + # Absolute length of limbs + self.extensiveness: float = extensiveness + # Relative length of limbs + self.length_of_limbs: float = length_of_limbs + # Coverage + self.coverage: float = coverage + # Relative number of effective active joints + self.joints: float = joints + # Absolute number of effective active joints + self.active_hinges_count: float = active_hinges_count + # Proportion + self.proportion: float = proportion + # Width + self.width: float = width + # Height + self.height: float = height + # Z depth + self.z_depth: float = z_depth + # Absolute size + self.absolute_size: float = absolute_size + # Relative size in respect of the max body size `max_permitted_modules` + self.size: float = size + # Proportion of sensor vs empty slots + self.sensors: float = sensors + # Body symmetry in the xy plane + self.symmetry: float = symmetry + # Number of active joints + self.hinge_count: float = hinge_count + # Number of bricks + self.brick_count: float = brick_count + # Number of brick sensors + self.brick_sensor_count: float = brick_sensor_count + # Number of touch sensors + self.touch_sensor_count: float = touch_sensor_count + # Number of free slots + self.free_slots: float = free_slots + # Ratio of the height over the root of the area of the base + self.height_base_ratio: float = height_base_ratio + # Maximum number of modules allowed (sensors excluded) + self.max_permitted_modules: Optional[int] = max_permitted_modules + # Vertical symmetry + self.symmetry_vertical: float = symmetry_vertical + # Base model density + self.base_density: float = base_density + # Bottom layer of the robot + self.bottom_layer: float = bottom_layer + + def compatible_individuals(self, + individual1: Individual, + individual2: Individual) -> bool: + morph_measure_1 = individual1.phenotype.measure_body() + morph_measure_2 = individual2.phenotype.measure_body() + _1 = morph_measure_1 + _2 = morph_measure_2 + + # TODO consider normalization of some of these values, some are already normalized by definition + + total_distance: float = 0.0 + total_distance += self.branching_modules_count * abs(_2.branching_modules_count - _1.branching_modules_count) + total_distance += self.branching * abs(_2.branching - _1.branching) + total_distance += self.extremities * abs(_2.extremities - _1.extremities) + total_distance += self.limbs * abs(_2.limbs - _1.limbs) + total_distance += self.extensiveness * abs(_2.extensiveness - _1.extensiveness) + total_distance += self.length_of_limbs * abs(_2.length_of_limbs - _1.length_of_limbs) + total_distance += self.coverage * abs(_2.coverage - _1.coverage) + total_distance += self.joints * abs(_2.joints - _1.joints) + total_distance += self.active_hinges_count * abs(_2.active_hinges_count - _1.active_hinges_count) + total_distance += self.proportion * abs(_2.proportion - _1.proportion) + total_distance += self.width * abs(_2.width - _1.width) + total_distance += self.height * abs(_2.height - _1.height) + total_distance += self.z_depth * abs(_2.z_depth - _1.z_depth) + total_distance += self.absolute_size * abs(_2.absolute_size - _1.absolute_size) + if self.max_permitted_modules is not None: + total_distance += self.size * \ + abs(_2.absolute_size - _1.absolute_size) / self.max_permitted_modules + total_distance += self.sensors * abs(_2.sensors - _1.sensors) + total_distance += self.symmetry * abs(_2.symmetry - _1.symmetry) + total_distance += self.hinge_count * abs(_2.hinge_count - _1.hinge_count) + total_distance += self.brick_count * abs(_2.brick_count - _1.brick_count) + total_distance += self.brick_sensor_count * abs(_2.brick_sensor_count - _1.brick_sensor_count) + total_distance += self.touch_sensor_count * abs(_2.touch_sensor_count - _1.touch_sensor_count) + total_distance += self.free_slots * abs(_2.free_slots - _1.free_slots) + total_distance += self.height_base_ratio * abs(_2.height_base_ratio - _1.height_base_ratio) + total_distance += self.symmetry_vertical * abs(_2.symmetry_vertical - _1.symmetry_vertical) + total_distance += self.base_density * abs(_2.base_density - _1.base_density) + total_distance += self.bottom_layer * abs(_2.bottom_layer - _1.bottom_layer) + + return total_distance <= self.total_threshold diff --git a/experiments/brain-speciation/manager.py b/experiments/brain-speciation/manager.py new file mode 100644 index 0000000000..83aeeff1e5 --- /dev/null +++ b/experiments/brain-speciation/manager.py @@ -0,0 +1,204 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +from pyrevolve import parser +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection_with_duplicates, tournament_selection +from pyrevolve.evolution.speciation.population_speciated import PopulationSpeciated +from pyrevolve.evolution.speciation.population_speciated_config import PopulationSpeciatedConfig +from pyrevolve.evolution.speciation.population_speciated_management import steady_state_speciated_population_management +from pyrevolve.experiment_management import ExperimentManagement +from pyrevolve.genotype.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig +from .MorphologyCompatibility import MorphologyCompatibility + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from pyrevolve.evolution.individual import Individual + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + body_conf = PlasticodingConfig( + max_structural_modules=20, + allow_vertical_brick=False, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=False, + allow_joint_joint_attachment=False, + ) + brain_conf = NeatBrainGenomeConfig() + brain_conf.multineat_params.DisjointCoeff = 0.3 + brain_conf.multineat_params.ExcessCoeff = 0.3 + brain_conf.multineat_params.WeightDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationADiffCoeff = 0.3 + brain_conf.multineat_params.ActivationBDiffCoeff = 0.3 + brain_conf.multineat_params.TimeConstantDiffCoeff = 0.3 + brain_conf.multineat_params.BiasDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationFunctionDiffCoeff = 0.3 + brain_conf.multineat_params.CompatTreshold = 3.0 + brain_conf.multineat_params.MinCompatTreshold = 3.0 + brain_conf.multineat_params.CompatTresholdModifier = 0.1 + brain_conf.multineat_params.CompatTreshChangeInterval_Generations = 1 + brain_conf.multineat_params.CompatTreshChangeInterval_Evaluations = 1 + genotype_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + + compatibitity_tester = MorphologyCompatibility( + total_threshold=1.0, + size=1.0, + brick_count=1.0, + proportion=1.0, + coverage=1.0, + joints=1.5, + branching=1.0, + symmetry=0.0, + max_permitted_modules=body_conf.max_structural_modules, + ) + + # experiment params # + + # Parse command line / file input arguments + args = parser.parse_args() + experiment_management = ExperimentManagement(args) + has_offspring = False + do_recovery = args.recovery_enabled and not experiment_management.experiment_is_new() + + logger.info(f'Activated run {args.run} of experiment {args.experiment_name}') + + if do_recovery: + gen_num, has_offspring, next_robot_id, next_species_id = \ + experiment_management.read_recovery_state(population_size, offspring_size, species=True) + + if gen_num == num_generations-1: + logger.info('Experiment is already complete.') + return + else: + gen_num = 0 + next_robot_id = 1 + next_species_id = 1 + + if gen_num < 0: + logger.info('Experiment continuing from first generation') + gen_num = 0 + + if next_robot_id < 0: + next_robot_id = 1 + + if next_species_id < 0: + next_species_id = 1 + + def are_individuals_brains_compatible_fn(individual1: Individual, + individual2: Individual) -> bool: + assert isinstance(individual1.genotype, LSystemCPGHyperNEATGenotype) + assert isinstance(individual2.genotype, LSystemCPGHyperNEATGenotype) + return individual1.genotype.is_brain_compatible(individual2.genotype, genotype_conf) + + def are_individuals_morphologies_compatible_fn(individual1: Individual, + individual2: Individual) -> bool: + return compatibitity_tester.compatible_individuals(individual1, individual2) + + population_conf = PopulationSpeciatedConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=genotype_conf, + fitness_function=fitness.displacement_velocity, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + selection=lambda individuals: tournament_selection(individuals, 2), + parent_selection=lambda individuals: multiple_selection_with_duplicates(individuals, 2, tournament_selection), + population_management=steady_state_speciated_population_management, + population_management_selector=tournament_selection, + evaluation_time=args.evaluation_time, + offspring_size=offspring_size, + experiment_name=args.experiment_name, + experiment_management=experiment_management, + # species stuff + # are_individuals_compatible_fn=are_individuals_brains_compatible_fn, + are_individuals_compatible_fn=are_individuals_morphologies_compatible_fn, + young_age_threshold=5, + young_age_fitness_boost=2.0, + old_age_threshold=35, + old_age_fitness_penalty=0.5, + species_max_stagnation=30, + ) + + n_cores = args.n_cores + + simulator_queue = SimulatorQueue(n_cores, args, args.port_start) + await simulator_queue.start() + + analyzer_queue = AnalyzerQueue(1, args, args.port_start+n_cores) + await analyzer_queue.start() + + population = PopulationSpeciated(population_conf, + simulator_queue, + analyzer_queue, + next_robot_id, + next_species_id) + + if do_recovery: + # loading a previous state of the experiment + population.load_snapshot(gen_num) + if gen_num >= 0: + logger.info(f'Recovered snapshot {gen_num}, pop with {len(population.genus)} individuals') + + # TODO partial recovery is not implemented, this is a substitute + has_offspring = False + next_robot_id = 1 + population.config.population_size + gen_num * population.config.offspring_size + population.next_robot_id = next_robot_id + + if has_offspring: + raise NotImplementedError('partial recovery not implemented') + recovered_individuals = population.load_partially_completed_generation(gen_num, population_size, offspring_size, next_robot_id) + gen_num += 1 + logger.info(f'Recovered unfinished offspring for generation {gen_num}') + + if gen_num == 0: + await population.initialize(recovered_individuals) + else: + population = await population.next_generation(gen_num, recovered_individuals) + + experiment_management.export_snapshots_species(population.genus, gen_num) + else: + # starting a new experiment + experiment_management.create_exp_folders() + await population.initialize() + experiment_management.export_snapshots_species(population.genus, gen_num) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_generation(gen_num) + experiment_management.export_snapshots_species(population.genus, gen_num) diff --git a/experiments/examples/manager_pop.py b/experiments/examples/manager_pop.py index ed42adced8..f8c5e1197a 100755 --- a/experiments/examples/manager_pop.py +++ b/experiments/examples/manager_pop.py @@ -12,7 +12,7 @@ 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.plasticoding import PlasticodingConfig from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue from pyrevolve.custom_logging.logger import logger @@ -29,7 +29,11 @@ async def run(): offspring_size = 50 genotype_conf = PlasticodingConfig( - max_structural_modules=100, + max_structural_modules=20, + allow_vertical_brick=True, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=True ) mutation_conf = MutationConfig( @@ -78,7 +82,7 @@ async def run(): experiment_management=experiment_management, ) - n_cores = settings.n_cores + n_cores = settings.n_cores settings = parser.parse_args() simulator_queue = SimulatorQueue(n_cores, settings, settings.port_start) 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/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/heritability/manager.py b/experiments/heritability/manager.py new file mode 100644 index 0000000000..525f01360e --- /dev/null +++ b/experiments/heritability/manager.py @@ -0,0 +1,160 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import os + +from pyrevolve import parser +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +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.experiment_management import ExperimentManagement +from pyrevolve.genotype.direct_tree.direct_tree_crossover import DirectTreeCrossoverConfig, Crossover +from pyrevolve.genotype.direct_tree.direct_tree_genotype import DirectTreeGenomeConfig +from pyrevolve.genotype.direct_tree.direct_tree_neat_genotype import DirectTreeNEATGenotypeConfig, \ + DirectTreeNEATGenotype +from pyrevolve.genotype.direct_tree.direct_tree_mutation import DirectTreeNEATMutationConfig, DirectTreeMutationConfig, Mutator +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from pyrevolve.evolution.individual import Individual + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 50 + population_size = 100 + offspring_size = 50 + + body_conf = DirectTreeGenomeConfig( + + ) + + brain_conf = NeatBrainGenomeConfig() + brain_conf.multineat_params.DisjointCoeff = 0.3 + brain_conf.multineat_params.ExcessCoeff = 0.3 + brain_conf.multineat_params.WeightDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationADiffCoeff = 0.3 + brain_conf.multineat_params.ActivationBDiffCoeff = 0.3 + brain_conf.multineat_params.TimeConstantDiffCoeff = 0.3 + brain_conf.multineat_params.BiasDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationFunctionDiffCoeff = 0.3 + brain_conf.multineat_params.CompatTreshold = 3.0 + brain_conf.multineat_params.MinCompatTreshold = 3.0 + brain_conf.multineat_params.CompatTresholdModifier = 0.1 + brain_conf.multineat_params.CompatTreshChangeInterval_Generations = 1 + brain_conf.multineat_params.CompatTreshChangeInterval_Evaluations = 1 + genotype_conf = DirectTreeNEATGenotypeConfig(body_conf, brain_conf) + + tree_mutation_conf = DirectTreeMutationConfig( + + ) + + genotype_mutation_conf = DirectTreeNEATMutationConfig( + tree_mutation_conf=tree_mutation_conf, + neat_conf=brain_conf, + ) + + mutation = Mutator() # TODO + + crossover_conf = DirectTreeCrossoverConfig( + + ) + + crossover = Crossover() # TODO + + # experiment params # + + # Parse command line / file input arguments + args = parser.parse_args() + experiment_management = ExperimentManagement(args) + has_offspring = False + do_recovery = args.recovery_enabled and not experiment_management.experiment_is_new() + + logger.info(f'Activated run {args.run} of experiment {args.experiment_name}') + + if do_recovery: + gen_num, has_offspring, next_robot_id, next_species_id = \ + experiment_management.read_recovery_state(population_size, offspring_size, species=False, n_developments=2) + + if gen_num == num_generations-1: + logger.info('Experiment is already complete.') + return + else: + gen_num = 0 + next_robot_id = 1 + + if gen_num < 0: + logger.info('Experiment continuing from first generation') + gen_num = 0 + + if next_robot_id < 0: + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=DirectTreeNEATGenotype, + genotype_conf=genotype_conf, + fitness_function=fitness.displacement_velocity, + objective_functions=None, + mutation_operator=mutation.mutate, + mutation_conf=genotype_mutation_conf, + crossover_operator=crossover.crossover, + crossover_conf=crossover_conf, + 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=args.evaluation_time, + grace_time=args.grace_time, + offspring_size=offspring_size, + experiment_name=args.experiment_name, + experiment_management=experiment_management, + ) + + n_cores = args.n_cores + + simulator_queue = SimulatorQueue(n_cores, args, args.port_start) + await simulator_queue.start() + + analyzer_queue = AnalyzerQueue(1, args, args.port_start+n_cores) + await analyzer_queue.start() + + population = Population(population_conf, simulator_queue, analyzer_queue, next_robot_id) + + if do_recovery: + # loading a previous state of the experiment + population.load_snapshot(gen_num, multi_development=True) + if gen_num >= 0: + logger.info(f'Recovered snapshot {gen_num}, pop with {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(f'Recovered unfinished offspring {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) + + 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/heritability/manager_direct.py b/experiments/heritability/manager_direct.py new file mode 100644 index 0000000000..d9feb54f2b --- /dev/null +++ b/experiments/heritability/manager_direct.py @@ -0,0 +1,143 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +from pyrevolve.genotype.direct_tree.direct_tree_crossover import crossover_list +from pyrevolve.genotype.direct_tree.direct_tree_mutation import mutate +from pyrevolve import parser +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +from pyrevolve.evolution.population.population import Population +from pyrevolve.evolution.population.population_config import PopulationConfig +from pyrevolve.evolution.population.population_management import generational_population_management +from pyrevolve.experiment_management import ExperimentManagement +from pyrevolve.genotype.direct_tree.direct_tree_genotype import DirectTreeGenotype, DirectTreeGenotypeConfig +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger + + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from pyrevolve.evolution.individual import Individual + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 50 + population_size = 100 + offspring_size = 100 + + morph_single_mutation_prob = 0.2 + morph_no_single_mutation_prob = 1-morph_single_mutation_prob # 0.8 + morph_no_all_mutation_prob = morph_no_single_mutation_prob**4 # 0.4096 + morph_at_least_one_mutation_prob = 1 - morph_no_all_mutation_prob # 0.5904 + + brain_single_mutation_prob = 0.5 + + 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=morph_single_mutation_prob, + mutation_p_delete_subtree=morph_single_mutation_prob, + mutation_p_generate_subtree=morph_single_mutation_prob, + mutation_p_swap_subtree=morph_single_mutation_prob, + mutation_p_mutate_oscillators=brain_single_mutation_prob, + mutation_p_mutate_oscillator=0.5, + mutate_oscillator_amplitude_sigma=0.3, + mutate_oscillator_period_sigma=0.3, + mutate_oscillator_phase_sigma=0.3, + ) + + # Parse command line / file input arguments + args = parser.parse_args() + experiment_management = ExperimentManagement(args) + has_offspring = False + do_recovery = args.recovery_enabled and not experiment_management.experiment_is_new() + + logger.info(f'Activated run {args.run} of experiment {args.experiment_name}') + + if do_recovery: + gen_num, has_offspring, next_robot_id, next_species_id = \ + experiment_management.read_recovery_state(population_size, offspring_size, species=False) + + if gen_num == num_generations-1: + logger.info('Experiment is already complete.') + return + else: + gen_num = 0 + next_robot_id = 1 + + if gen_num < 0: + logger.info('Experiment continuing from first generation') + gen_num = 0 + + if next_robot_id < 0: + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=lambda conf, _id: DirectTreeGenotype(conf, _id, random_init=True), + genotype_conf=genotype_conf, + fitness_function=fitness.displacement_velocity, + objective_functions=None, + mutation_operator=lambda genotype, gen_conf: mutate(genotype, gen_conf, in_place=False), + mutation_conf=genotype_conf, + crossover_operator=lambda parents, gen_conf, _cross_conf: crossover_list([p.genotype for p in parents], gen_conf), + crossover_conf=genotype_conf, + selection=lambda individuals: tournament_selection(individuals, 2), + parent_selection=lambda individuals: multiple_selection(individuals, 2, tournament_selection), + population_management=generational_population_management, + population_management_selector=None, # must be none for generational management function + evaluation_time=args.evaluation_time, + grace_time=args.grace_time, + offspring_size=offspring_size, + experiment_name=args.experiment_name, + experiment_management=experiment_management, + ) + + n_cores = args.n_cores + + simulator_queue = SimulatorQueue(n_cores, args, args.port_start) + await simulator_queue.start() + + analyzer_queue = AnalyzerQueue(1, args, args.port_start+n_cores) + await analyzer_queue.start() + + population = Population(population_conf, simulator_queue, analyzer_queue, next_robot_id) + + if do_recovery: + # loading a previous state of the experiment + population.load_snapshot(gen_num, multi_development=True) + if gen_num >= 0: + logger.info(f'Recovered snapshot {gen_num}, pop with {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(f'Recovered unfinished offspring {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) + + 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/heritability/manager_lsystem.py b/experiments/heritability/manager_lsystem.py new file mode 100644 index 0000000000..622c045b9d --- /dev/null +++ b/experiments/heritability/manager_lsystem.py @@ -0,0 +1,178 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +from pyrevolve.genotype.lsystem_neat import LSystemCPGHyperNEATGenotypeConfig, LSystemCPGHyperNEATGenotype +from pyrevolve.genotype.neat_brain_genome import NeatBrainGenomeConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import BrainType +from pyrevolve.genotype.plasticoding.crossover.standard_crossover import standard_crossover +from pyrevolve.genotype.plasticoding.mutation.standard_mutation import standard_mutation +from pyrevolve.genotype.plasticoding import PlasticodingConfig, random_initialization +from pyrevolve.genotype.plasticoding.crossover.crossover import CrossoverConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig +from pyrevolve import parser +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +from pyrevolve.evolution.population.population import Population +from pyrevolve.evolution.population.population_config import PopulationConfig +from pyrevolve.evolution.population.population_management import generational_population_management +from pyrevolve.experiment_management import ExperimentManagement +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation + + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from pyrevolve.evolution.individual import Individual + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 50 + population_size = 100 + offspring_size = 100 + + morph_single_mutation_prob = 0.2 + morph_no_single_mutation_prob = 1-morph_single_mutation_prob # 0.8 + morph_no_all_mutation_prob = morph_no_single_mutation_prob**4 # 0.4096 + morph_at_least_one_mutation_prob = 1 - morph_no_all_mutation_prob # 0.5904 + + brain_single_mutation_prob = 0.5 + + body_conf: PlasticodingConfig = PlasticodingConfig( + allow_joint_joint_attachment=False, + e_max_groups=3, + oscillator_param_min=0, + oscillator_param_max=5, + weight_param_min=0, + weight_param_max=0, + weight_min=0, + weight_max=0, + max_structural_modules=50, + allow_vertical_brick=True, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=False, + ) + brain_conf = NeatBrainGenomeConfig(brain_type=BrainType.CPG) + brain_conf.multineat_params.DisjointCoeff = 0.3 + brain_conf.multineat_params.ExcessCoeff = 0.3 + brain_conf.multineat_params.WeightDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationADiffCoeff = 0.3 + brain_conf.multineat_params.ActivationBDiffCoeff = 0.3 + brain_conf.multineat_params.TimeConstantDiffCoeff = 0.3 + brain_conf.multineat_params.BiasDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationFunctionDiffCoeff = 0.3 + brain_conf.multineat_params.CompatTreshold = 3.0 + brain_conf.multineat_params.MinCompatTreshold = 3.0 + brain_conf.multineat_params.CompatTresholdModifier = 0.1 + brain_conf.multineat_params.CompatTreshChangeInterval_Generations = 1 + brain_conf.multineat_params.CompatTreshChangeInterval_Evaluations = 1 + genotype_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf: MutationConfig = plasticMutationConfig( + mutation_prob=morph_at_least_one_mutation_prob, + genotype_conf=body_conf, + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=1.0, + ) + + # Parse command line / file input arguments + args = parser.parse_args() + experiment_management = ExperimentManagement(args) + has_offspring = False + do_recovery = args.recovery_enabled and not experiment_management.experiment_is_new() + + logger.info(f'Activated run {args.run} of experiment {args.experiment_name}') + + if do_recovery: + gen_num, has_offspring, next_robot_id, next_species_id = \ + experiment_management.read_recovery_state(population_size, offspring_size, species=False) + + if gen_num == num_generations-1: + logger.info('Experiment is already complete.') + return + else: + gen_num = 0 + next_robot_id = 1 + + if gen_num < 0: + logger.info('Experiment continuing from first generation') + gen_num = 0 + + if next_robot_id < 0: + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=lambda conf, _id: LSystemCPGHyperNEATGenotype(conf, _id), + genotype_conf=genotype_conf, + fitness_function=fitness.displacement_velocity, + objective_functions=None, + mutation_operator=lambda genotype, gen_conf: lmutation(genotype, gen_conf), + mutation_conf=lmutation_conf, + crossover_operator=lambda parents, gen_conf, cross_conf: lcrossover(parents, gen_conf, cross_conf), + crossover_conf=crossover_conf, + selection=lambda individuals: tournament_selection(individuals, 2), + parent_selection=lambda individuals: multiple_selection(individuals, 2, tournament_selection), + population_management=generational_population_management, + population_management_selector=None, # must be none for generational management function + evaluation_time=args.evaluation_time, + grace_time=args.grace_time, + offspring_size=offspring_size, + experiment_name=args.experiment_name, + experiment_management=experiment_management, + ) + + n_cores = args.n_cores + + simulator_queue = SimulatorQueue(n_cores, args, args.port_start) + await simulator_queue.start() + + analyzer_queue = AnalyzerQueue(1, args, args.port_start+n_cores) + await analyzer_queue.start() + + population = Population(population_conf, simulator_queue, analyzer_queue, next_robot_id) + + if do_recovery: + # loading a previous state of the experiment + population.load_snapshot(gen_num, multi_development=True) + if gen_num >= 0: + logger.info(f'Recovered snapshot {gen_num}, pop with {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(f'Recovered unfinished offspring {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) + + 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/heritability/test_manager.py b/experiments/heritability/test_manager.py new file mode 100644 index 0000000000..8e7960fc60 --- /dev/null +++ b/experiments/heritability/test_manager.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import os +import asyncio + +from pyrevolve import parser +from pyrevolve.angle.robogen.spec import RobogenTreeGenerator +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +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.experiment_management import ExperimentManagement +from pyrevolve.genotype.direct_tree.direct_tree_crossover import DirectTreeCrossoverConfig, Crossover +from pyrevolve.genotype.direct_tree.direct_tree_genotype import DirectTreeGenomeConfig, DirectTreeGenotype +from pyrevolve.genotype.direct_tree.direct_tree_neat_genotype import DirectTreeNEATGenotypeConfig, \ + DirectTreeNEATGenotype +from pyrevolve.genotype.direct_tree.direct_tree_mutation import DirectTreeNEATMutationConfig, DirectTreeMutationConfig, Mutator +from pyrevolve.tol.spec import get_tree_generator +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig +from pyrevolve.genotype.direct_tree import DirectTreeConfig + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from pyrevolve.evolution.individual import Individual + + +def tree_random_initialization(conf: DirectTreeConfig, _id: int) -> DirectTreeGenotype: + genotype: DirectTreeGenotype = DirectTreeGenotype(conf, _id) + genotype.generator = get_tree_generator(conf) + genotype.representation = genotype.generator.generate_tree() + return genotype + + +async def run(): + # experiment params # + num_generations = 10 + population_size = 10 + offspring_size = 5 + + genotype_config = DirectTreeConfig(min_parts=5, max_parts=11, max_inputs=3, max_outputs=6, + initial_parts_mu=8, initial_parts_sigma=1.5, + disable_sensors=True, enable_touch_sensor=False) + + crossover_config = DirectTreeCrossoverConfig() + mutation_config = DirectTreeMutationConfig() + + robogen_tree_generator: RobogenTreeGenerator = get_tree_generator(genotype_config) + mutation: Mutator = Mutator(robogen_tree_generator) + crossover: Crossover = Crossover(robogen_tree_generator) + + # Parse command line / file input arguments + args = parser.parse_args() + experiment_management = ExperimentManagement(args) + has_offspring = False + do_recovery = args.recovery_enabled and not experiment_management.experiment_is_new() + + logger.info(f'Activated run {args.run} of experiment {args.experiment_name}') + + if do_recovery: + gen_num, has_offspring, next_robot_id, next_species_id = \ + experiment_management.read_recovery_state(population_size, offspring_size, species=False, n_developments=2) + + if gen_num == num_generations-1: + logger.info('Experiment is already complete.') + return + else: + gen_num = 0 + next_robot_id = 1 + + if gen_num < 0: + logger.info('Experiment continuing from first generation') + gen_num = 0 + + if next_robot_id < 0: + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=tree_random_initialization, + genotype_conf=genotype_config, + fitness_function=fitness.displacement_velocity, + objective_functions=None, + mutation_operator=mutation.mutate, + mutation_conf=mutation_config, + crossover_operator=crossover.crossover, + crossover_conf=crossover_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=args.evaluation_time, + grace_time=args.grace_time, + offspring_size=offspring_size, + experiment_name=args.experiment_name, + experiment_management=experiment_management, + ) + + population = Population(population_conf, None, None, next_robot_id) + """ + if do_recovery: + # loading a previous state of the experiment + print(experiment_management._generations_folder) + population.load_snapshot(gen_num, multi_development=True) + if gen_num >= 0: + logger.info(f'Recovered snapshot {gen_num}, pop with {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(f'Recovered unfinished offspring {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) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_generation(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) + + +if __name__ == "__main__": + loop = asyncio.get_event_loop() + loop.run_until_complete(run()) + loop.close() diff --git a/experiments/lsystem-cppn-cpg/manager.py b/experiments/lsystem-cppn-cpg/manager.py new file mode 100644 index 0000000000..65bfd4724c --- /dev/null +++ b/experiments/lsystem-cppn-cpg/manager.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +from pyrevolve import parser +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +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.experiment_management import ExperimentManagement +from pyrevolve.genotype.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + body_conf = PlasticodingConfig( + max_structural_modules=20, + allow_vertical_brick=False, + use_movement_commands=False, + use_rotation_commands=False, + use_movement_stack=True, + ) + brain_conf = NeatBrainGenomeConfig() + lsystem_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.0, + ) + # experiment params # + + # 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() + + 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.') + return + else: + gen_num = 0 + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=lsystem_conf, + fitness_function=fitness.displacement_velocity, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + 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, + ) + + n_cores = settings.n_cores + + settings = parser.parse_args() + 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() + + 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) + if gen_num >= 0: + 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) + gen_num += 1 + logger.info('Recovered unfinished offspring '+str(gen_num)) + + if gen_num == 0: + await population.init_pop(individuals) + else: + population = await population.next_gen(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() + experiment_management.export_snapshots(population.individuals, gen_num) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_gen(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) diff --git a/experiments/lsystem-cppn-cpg/manager_move.py b/experiments/lsystem-cppn-cpg/manager_move.py new file mode 100644 index 0000000000..142482810f --- /dev/null +++ b/experiments/lsystem-cppn-cpg/manager_move.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +from pyrevolve import parser +from pyrevolve.evolution import fitness +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.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + body_conf = PlasticodingConfig( + max_structural_modules=20, + allow_vertical_brick=False, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=False, + allow_joint_joint_attachment=True, + ) + brain_conf = NeatBrainGenomeConfig() + lsystem_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + # experiment params # + + # 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() + + 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.') + return + else: + gen_num = 0 + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=lsystem_conf, + fitness_function=fitness.displacement_velocity, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + 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, + ) + + n_cores = settings.n_cores + + settings = parser.parse_args() + 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() + + 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) + if gen_num >= 0: + 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) + gen_num += 1 + logger.info('Recovered unfinished offspring '+str(gen_num)) + + if gen_num == 0: + await population.init_pop(individuals) + else: + population = await population.next_gen(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() + experiment_management.export_snapshots(population.individuals, gen_num) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_gen(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) diff --git a/experiments/lsystem-cppn-cpg/manager_move_block.py b/experiments/lsystem-cppn-cpg/manager_move_block.py new file mode 100644 index 0000000000..b7b49284fa --- /dev/null +++ b/experiments/lsystem-cppn-cpg/manager_move_block.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +from pyrevolve import parser +from pyrevolve.evolution import fitness +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.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + body_conf = PlasticodingConfig( + max_structural_modules=20, + allow_vertical_brick=False, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=False, + allow_joint_joint_attachment=False, + ) + brain_conf = NeatBrainGenomeConfig() + lsystem_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + # experiment params # + + # 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() + + 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.') + return + else: + gen_num = 0 + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=lsystem_conf, + fitness_function=fitness.displacement_velocity, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + 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, + ) + + n_cores = settings.n_cores + + settings = parser.parse_args() + 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() + + 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) + if gen_num >= 0: + 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) + gen_num += 1 + logger.info('Recovered unfinished offspring '+str(gen_num)) + + if gen_num == 0: + await population.init_pop(individuals) + else: + population = await population.next_gen(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() + experiment_management.export_snapshots(population.individuals, gen_num) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_gen(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) diff --git a/experiments/lsystem-cppn-cpg/manager_move_stack.py b/experiments/lsystem-cppn-cpg/manager_move_stack.py new file mode 100644 index 0000000000..5427c02056 --- /dev/null +++ b/experiments/lsystem-cppn-cpg/manager_move_stack.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +from pyrevolve import parser +from pyrevolve.evolution import fitness +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.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + body_conf = PlasticodingConfig( + max_structural_modules=20, + allow_vertical_brick=False, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=True, + allow_joint_joint_attachment=True, + ) + brain_conf = NeatBrainGenomeConfig() + lsystem_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + # experiment params # + + # 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() + + 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.') + return + else: + gen_num = 0 + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=lsystem_conf, + fitness_function=fitness.displacement_velocity, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + 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, + ) + + n_cores = settings.n_cores + + settings = parser.parse_args() + 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() + + 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) + if gen_num >= 0: + 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) + gen_num += 1 + logger.info('Recovered unfinished offspring '+str(gen_num)) + + if gen_num == 0: + await population.init_pop(individuals) + else: + population = await population.next_gen(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() + experiment_management.export_snapshots(population.individuals, gen_num) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_gen(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) diff --git a/experiments/lsystem-cppn-cpg/manager_move_stack_block.py b/experiments/lsystem-cppn-cpg/manager_move_stack_block.py new file mode 100644 index 0000000000..65c611ed49 --- /dev/null +++ b/experiments/lsystem-cppn-cpg/manager_move_stack_block.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +from pyrevolve import parser +from pyrevolve.evolution import fitness +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.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + body_conf = PlasticodingConfig( + max_structural_modules=20, + allow_vertical_brick=False, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=True, + allow_joint_joint_attachment=False, + ) + brain_conf = NeatBrainGenomeConfig() + lsystem_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + # experiment params # + + # 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() + + 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.') + return + else: + gen_num = 0 + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=lsystem_conf, + fitness_function=fitness.displacement_velocity, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + 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, + ) + + n_cores = settings.n_cores + + settings = parser.parse_args() + 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() + + 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) + if gen_num >= 0: + 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) + gen_num += 1 + logger.info('Recovered unfinished offspring '+str(gen_num)) + + if gen_num == 0: + await population.init_pop(individuals) + else: + population = await population.next_gen(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() + experiment_management.export_snapshots(population.individuals, gen_num) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_gen(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) diff --git a/experiments/lsystem-cppn-cpg/manager_stack.py b/experiments/lsystem-cppn-cpg/manager_stack.py new file mode 100644 index 0000000000..b818ede172 --- /dev/null +++ b/experiments/lsystem-cppn-cpg/manager_stack.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +from pyrevolve import parser +from pyrevolve.evolution import fitness +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.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + body_conf = PlasticodingConfig( + max_structural_modules=20, + allow_vertical_brick=False, + use_movement_commands=False, + use_rotation_commands=False, + use_movement_stack=True, + allow_joint_joint_attachment=True, + ) + brain_conf = NeatBrainGenomeConfig() + lsystem_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + # experiment params # + + # 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() + + 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.') + return + else: + gen_num = 0 + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=lsystem_conf, + fitness_function=fitness.displacement_velocity, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + 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, + ) + + n_cores = settings.n_cores + + settings = parser.parse_args() + 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() + + 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) + if gen_num >= 0: + 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) + gen_num += 1 + logger.info('Recovered unfinished offspring '+str(gen_num)) + + if gen_num == 0: + await population.init_pop(individuals) + else: + population = await population.next_gen(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() + experiment_management.export_snapshots(population.individuals, gen_num) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_gen(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) diff --git a/experiments/lsystem-cppn-cpg/manager_stack_block.py b/experiments/lsystem-cppn-cpg/manager_stack_block.py new file mode 100644 index 0000000000..f629514c7d --- /dev/null +++ b/experiments/lsystem-cppn-cpg/manager_stack_block.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +from pyrevolve import parser +from pyrevolve.evolution import fitness +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.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + body_conf = PlasticodingConfig( + max_structural_modules=20, + allow_vertical_brick=False, + use_movement_commands=False, + use_rotation_commands=False, + use_movement_stack=True, + allow_joint_joint_attachment=False, + ) + brain_conf = NeatBrainGenomeConfig() + lsystem_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + # experiment params # + + # 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() + + 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.') + return + else: + gen_num = 0 + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=lsystem_conf, + fitness_function=fitness.displacement_velocity, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + 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, + ) + + n_cores = settings.n_cores + + settings = parser.parse_args() + 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() + + 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) + if gen_num >= 0: + 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) + gen_num += 1 + logger.info('Recovered unfinished offspring '+str(gen_num)) + + if gen_num == 0: + await population.init_pop(individuals) + else: + population = await population.next_gen(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() + experiment_management.export_snapshots(population.individuals, gen_num) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_gen(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) diff --git a/experiments/lsystem-improvements/consolidate_experiments.py b/experiments/lsystem-improvements/consolidate_experiments.py new file mode 100755 index 0000000000..5e56c10926 --- /dev/null +++ b/experiments/lsystem-improvements/consolidate_experiments.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python + +import os + + +# set these variables according to your experiments # +dirpath = 'data' +experiments_type = [ + 'fixed_ground_swimming', + 'rotation_command' +] +runs = [1, 2, 3, 4, 5, 6, 7, 8, 9, 10] +# set these variables according to your experiments # + + +def build_headers(path): + + print(path + "/all_measures.txt") + file_summary = open(path + "/all_measures.tsv", "w+") + file_summary.write('robot_id\t') + + behavior_headers = [] + behavior_headers.append('velocity') + file_summary.write(behavior_headers[-1]+'\t') + behavior_headers.append('displacement_velocity') + file_summary.write(behavior_headers[-1]+'\t') + behavior_headers.append('displacement_velocity_hill') + file_summary.write(behavior_headers[-1]+'\t') + behavior_headers.append('head_balance') + file_summary.write(behavior_headers[-1]+'\t') + behavior_headers.append('contacts') + file_summary.write(behavior_headers[-1]+'\t') + # use this instead? but what if the guy is none? + # with open(path + '/data_fullevolution/descriptors/behavior_desc_robot_1.txt') as file: + # for line in file: + # measure, value = line.strip().split(' ') + # behavior_headers.append(measure) + # file_summary.write(measure+'\t') + + phenotype_headers = [] + with open(path + '/data_fullevolution/descriptors/phenotype_desc_robot_1.txt') as file: + for line in file: + measure, value = line.strip().split(' ') + phenotype_headers.append(measure) + file_summary.write(measure+'\t') + file_summary.write('fitness\n') + file_summary.close() + + file_summary = open(path + "/snapshots_ids.tsv", "w+") + file_summary.write('generation\trobot_id\n') + file_summary.close() + + return behavior_headers, phenotype_headers + + +if __name__ == '__main__': + for exp in experiments_type: + for run in runs: + + print(exp, run) + path = os.path.join(dirpath, exp, str(run)) + behavior_headers, phenotype_headers = build_headers(path) + + file_summary = open(path + "/all_measures.tsv", "a") + for r, d, f in os.walk(path+'/data_fullevolution/fitness'): + for file in f: + + robot_id = file.split('.')[0].split('_')[-1] + file_summary.write(robot_id+'\t') + + bh_file = path+'/data_fullevolution/descriptors/behavior_desc_robot_'+robot_id+'.txt' + if os.path.isfile(bh_file): + with open(bh_file) as file: + for line in file: + if line != 'None': + measure, value = line.strip().split(' ') + file_summary.write(value+'\t') + else: + for h in behavior_headers: + file_summary.write('None'+'\t') + else: + for h in behavior_headers: + file_summary.write('None'+'\t') + + pt_file = path+'/data_fullevolution/descriptors/phenotype_desc_robot_'+robot_id+'.txt' + if os.path.isfile(pt_file): + with open(pt_file) as file: + for line in file: + measure, value = line.strip().split(' ') + file_summary.write(value+'\t') + else: + for h in phenotype_headers: + file_summary.write('None'+'\t') + + with open(path+'/data_fullevolution/fitness/fitness_robot_'+robot_id+'.txt', 'r') as file: + fitness = file.read() + file_summary.write(fitness + '\n') + file_summary.close() + + with open(path + "/snapshots_ids.tsv", "a") as file_summary: + for r, d, f in os.walk(path): + for dir in d: + if 'selectedpop' in dir: + gen = dir.split('_')[1] + for r2, d2, f2 in os.walk(path + '/selectedpop_' + str(gen)): + for file in f2: + if 'body' in file: + id = file.split('.')[0].split('_')[-1] + file_summary.write(gen+'\t'+id+'\n') diff --git a/experiments/lsystem-improvements/manager.py b/experiments/lsystem-improvements/manager.py new file mode 100755 index 0000000000..ed42adced8 --- /dev/null +++ b/experiments/lsystem-improvements/manager.py @@ -0,0 +1,119 @@ +#!/usr/bin/env python3 +import asyncio + +from pyrevolve import parser +from pyrevolve.evolution import fitness +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.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue +from pyrevolve.custom_logging.logger import logger + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 100 + 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, + ) + # experiment params # + + # 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() + + 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.') + return + else: + gen_num = 0 + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=random_initialization, + genotype_conf=genotype_conf, + fitness_function=fitness.displacement_velocity, + mutation_operator=standard_mutation, + mutation_conf=mutation_conf, + crossover_operator=standard_crossover, + crossover_conf=crossover_conf, + 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, + ) + + n_cores = settings.n_cores + + settings = parser.parse_args() + 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() + + 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) + if gen_num >= 0: + 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) + gen_num += 1 + logger.info('Recovered unfinished offspring '+str(gen_num)) + + if gen_num == 0: + await population.init_pop(individuals) + else: + population = await population.next_gen(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() + experiment_management.export_snapshots(population.individuals, gen_num) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_gen(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) + + # output result after completing all generations... diff --git a/experiments/lsystem-improvements/summary_measures.R b/experiments/lsystem-improvements/summary_measures.R new file mode 100644 index 0000000000..3a9f022dfc --- /dev/null +++ b/experiments/lsystem-improvements/summary_measures.R @@ -0,0 +1,436 @@ +library(ggplot2) +library(sqldf) +library(plyr) +library(dplyr) +library(trend) +library(purrr) + +base_directory <- paste('data', sep='') + +output_directory = paste(base_directory, 'charts', sep='/') + +#### CHANGE THE PARAMETERS HERE #### + + +experiments_type = c( + 'fixed_ground_swimming', + 'rotation_command' +) + +initials = c('c', 'r') + +experiments_labels = c('Cube', 'Rot') + +runs = c(1,2,3,4,5,6,7,8,9,10) +gens = 100 +pop = 100 + +#### CHANGE THE PARAMETERS HERE #### + +sig = 0.05 +line_size = 30 +show_markers = TRUE +show_legends = TRUE +experiments_type_colors = c( '#009900', '#FF8000', '#BA1616', '#000099') # DARK:green, orange, red, blue + +measures_names = c( +'displacement_velocity_hill', +'head_balance', +'contacts', +'displacement_velocity', +'branching', +'branching_modules_count', +'limbs', +'extremities', +'length_of_limbs', +'extensiveness', +'coverage', +'joints', +'hinge_count', +'active_hinges_count', +'brick_count', +'touch_sensor_count', +'brick_sensor_count', +'proportion', +'width', +'height', +'absolute_size', +'sensors', +'symmetry', +'avg_period', +'dev_period', +'avg_phase_offset', +'dev_phase_offset', +'avg_amplitude', +'dev_amplitude', +'avg_intra_dev_params', +'avg_inter_dev_params', +'sensors_reach', +'recurrence', +'synaptic_reception', +'fitness' +) + +# add proper labels soon... +measures_labels = c( +'Speed', +'Balance', +'Contacts', +'displacement_velocity', +'branching', +'branching_modules_count', +'limbs', +'extremities', +'length_of_limbs', +'extensiveness', +'coverage', +'joints', +'hinge_count', +'active_hinges_count', +'brick_count', +'touch_sensor_count', +'brick_sensor_count', +'proportion', +'width', +'height', +'absolute_size', +'sensors', +'symmetry', +'avg_period', +'dev_period', +'avg_phase_offset', +'dev_phase_offset', +'avg_amplitude', +'dev_amplitude', +'avg_intra_dev_params', +'avg_inter_dev_params', +'sensors_reach', +'recurrence', +'synaptic_reception', +'Fitness' +) + + +measures_snapshots_all = NULL + +for (exp in 1:length(experiments_type)) +{ + for(run in runs) + { + input_directory <- paste(base_directory, '/', experiments_type[exp], '/', run, sep='') + measures = read.table(paste(input_directory,"/all_measures.tsv", sep=''), header = TRUE) + for( m in 1:length(measures_names)) + { + measures[measures_names[m]] = as.numeric(as.character(measures[[measures_names[m]]])) + } + + snapshots = read.table(paste(input_directory,"/snapshots_ids.tsv", sep=''), header = TRUE) + measures_snapshots = sqldf('select * from snapshots inner join measures using(robot_id) order by generation') + + measures_snapshots$run = run + measures_snapshots$run = as.factor(measures_snapshots$run) + measures_snapshots$method = experiments_type[exp] + + if ( is.null(measures_snapshots_all)){ + measures_snapshots_all = measures_snapshots + }else{ + measures_snapshots_all = rbind(measures_snapshots_all, measures_snapshots) + } + } +} + + +fail_test = sqldf(paste("select method,run,generation,count(*) as c from measures_snapshots_all group by 1,2,3 having c<",gens," order by 4")) + + +measures_snapshots_all = sqldf("select * from measures_snapshots_all where fitness IS NOT NULL") + + + +measures_averages_gens_1 = list() +measures_averages_gens_2 = list() + +measures_ini = list() +measures_fin = list() + +for (exp in 1:length(experiments_type)) +{ + + measures_aux = c() + query ='select run, generation' + for (i in 1:length(measures_names)) + { + query = paste(query,', avg(',measures_names[i],') as ',experiments_type[exp],'_',measures_names[i],'_avg', sep='') + } + query = paste(query,' from measures_snapshots_all + where method="',experiments_type[exp],'" group by run, generation', sep='') + + + measures_averages_gens_1[[exp]] = sqldf(query) + + temp = measures_averages_gens_1[[exp]] + + temp$generation = as.numeric(temp$generation) + + measures_ini[[exp]] = sqldf(paste("select * from temp where generation=1")) + measures_fin[[exp]] = sqldf(paste("select * from temp where generation=",gens-1)) + query = 'select generation' + for (i in 1:length(measures_names)) + { + # later renames vars _avg_SUMMARY, just to make it in the same style as the quantile variables + query = paste(query,', avg(',experiments_type[exp],'_',measures_names[i],'_avg) as ',experiments_type[exp],'_',measures_names[i],'_avg', sep='') + query = paste(query,', max(',experiments_type[exp],'_',measures_names[i],'_avg) as ',experiments_type[exp],'_',measures_names[i],'_max', sep='') + query = paste(query,', stdev(',experiments_type[exp],'_',measures_names[i],'_avg) as ',experiments_type[exp],'_',measures_names[i],'_stdev', sep='') + query = paste(query,', median(',experiments_type[exp],'_',measures_names[i],'_avg) as ',experiments_type[exp],'_',measures_names[i],'_median', sep='') + + measures_aux = c(measures_aux, paste(experiments_type[exp],'_',measures_names[i],'_avg', sep='') ) + } + query = paste(query,' from temp group by generation', sep="") + + temp2 = sqldf(query) + + p <- c(0.25, 0.75) + p_names <- map_chr(p, ~paste0('Q',.x*100, sep="")) + p_funs <- map(p, ~partial(quantile, probs = .x, na.rm = TRUE)) %>% + set_names(nm = p_names) + + quantiles = data.frame(temp %>% + group_by(generation) %>% + summarize_at(vars(measures_aux), funs(!!!p_funs)) ) + + measures_averages_gens_2[[exp]] = sqldf('select * from temp2 inner join quantiles using (generation)') + +} + + +for (exp in 1:length(experiments_type)) +{ + if(exp==1){ + measures_averages_gens = measures_averages_gens_2[[1]] + }else{ + measures_averages_gens = merge(measures_averages_gens, measures_averages_gens_2[[exp]], by = "generation") + } +} + +file <-file(paste(output_directory,'/trends.txt',sep=''), open="w") + +#tests trends in curves and difference between ini and fin generations + + +# ini VS fin +array_wilcoxon = list() +array_wilcoxon2 = list() + +# curve +array_mann = list() + + +for (m in 1:length(experiments_type)) +{ + + array_wilcoxon[[m]] = list() + array_mann[[m]] = list() + + for (i in 1:length(measures_names)) + { + + writeLines(paste(experiments_type[m],measures_names[i],'ini avg ',as.character( + mean(c(array(measures_ini[[m]][paste(experiments_type[m],"_",measures_names[i],"_avg",sep='')]) )[[1]]) ) ,sep=" "), file ) + + + writeLines(paste(experiments_type[m],measures_names[i],'fin avg ',as.character( + mean(c(array(measures_fin[[m]][paste(experiments_type[m],"_",measures_names[i],"_avg",sep='')]) )[[1]]) ) ,sep=" "), file ) + + array_wilcoxon[[m]][[i]] = wilcox.test(c(array(measures_ini[[m]][paste(experiments_type[m],"_",measures_names[i],"_avg",sep='')]))[[1]] , + c(array(measures_fin[[m]][paste(experiments_type[m],"_",measures_names[i],"_avg",sep='')]))[[1]] + ) + + writeLines(c( + paste(experiments_type[m],'iniVSfin',measures_names[i],'wilcox p: ',as.character(round(array_wilcoxon[[m]][[i]]$p.value,4)), sep=' ') + ,paste(experiments_type[m],'iniVSfin',measures_names[i],'wilcox est: ',as.character(round(array_wilcoxon[[m]][[i]]$statistic,4)), sep=' ') + + ), file) + + + #tests trends + array_mann[[m]][[i]] = mk.test(c(array(measures_averages_gens_2[[m]][paste(experiments_type[m],"_",measures_names[i],'_median',sep='')]) )[[1]], + continuity = TRUE) + + + writeLines(c( + paste(experiments_type[m],measures_names[i], ' Mann-Kendall median p', as.character(round(array_mann[[m]][[i]]$p.value,4)),sep=' '), + paste(experiments_type[m],measures_names[i], ' Mann-Kendall median s', as.character(round(array_mann[[m]][[i]]$statistic,4)),sep=' ') + ), file) + + } + +} + + + +# tests final generation among experiments_type + +aux_m = length(experiments_type) + +if (aux_m>1) +{ + + # fins + array_wilcoxon2[[1]] = list() + array_wilcoxon2[[2]] = list() + + aux_m = aux_m -1 + count_pairs = 0 + for(m in 1:aux_m) + { + aux = m+1 + for(m2 in aux:length(experiments_type)) + { + + count_pairs = count_pairs+1 + array_wilcoxon2[[1]][[count_pairs]] = list() + + for (i in 1:length(measures_names)) + { + + writeLines(paste(experiments_type[m],measures_names[i],'fin avg ',as.character( + mean(c(array(measures_fin[[m]][paste(experiments_type[m],"_",measures_names[i],"_avg",sep='')]) )[[1]]) ) ,sep=" "), file ) + + writeLines(paste(experiments_type[m2],measures_names[i],'fin avg ',as.character( + mean(c(array(measures_fin[[m2]][paste(experiments_type[m2],"_",measures_names[i],"_avg",sep='')]) )[[1]]) ) ,sep=" "), file ) + + array_wilcoxon2[[1]][[count_pairs]][[i]] = wilcox.test(c(array(measures_fin[[m]][paste(experiments_type[m],"_",measures_names[i],"_avg",sep='')]))[[1]] , + c(array(measures_fin[[m2]][paste(experiments_type[m2],"_",measures_names[i],"_avg",sep='')]))[[1]] + ) + + writeLines(c( + paste(experiments_type[m],'VS',experiments_type[m2],measures_names[i],'fin avg wilcox p: ',as.character(round(array_wilcoxon2[[1]][[count_pairs]][[i]]$p.value,4)), sep=' ') + ,paste(experiments_type[m],'VS',experiments_type[m2],measures_names[i],'fin avg wilcox est: ',as.character(round(array_wilcoxon2[[1]][[count_pairs]][[i]]$statistic,4)), sep=' ') + + ), file) + + } + + + array_wilcoxon2[[2]][[count_pairs]] = paste(initials[m],initials[m2],sep='') + + } + } + +} + +close(file) + +# plots measures + +for (type_summary in c('means','quants')) +{ + + + for (i in 1:length(measures_names)) + { + tests1 = '' + tests2 = '' + tests3 = '' + break_aux = 0 + + graph <- ggplot(data=measures_averages_gens, aes(x=generation)) + + for(m in 1:length(experiments_type)) + { + if(type_summary == 'means') + { + graph = graph + geom_errorbar(aes_string(ymin=paste(experiments_type[m],'_',measures_names[i],'_avg','-',experiments_type[m],'_',measures_names[i],'_stdev',sep=''), + ymax=paste(experiments_type[m],'_',measures_names[i],'_avg','+',experiments_type[m],'_',measures_names[i],'_stdev',sep='') ), + color=experiments_type_colors[m], + alpha=0.35,size=0.5,width=0.001) + } else # type_summary 'quants' + { + graph = graph + geom_errorbar(aes_string(ymin=paste(experiments_type[m],'_',measures_names[i],'_avg_Q25',sep=''), + ymax=paste(experiments_type[m],'_',measures_names[i],'_avg_Q75',sep='') ), + color=experiments_type_colors[m], + alpha=0.35,size=0.5,width=0.001) + } + } + + for(m in 1:length(experiments_type)) + { + if(type_summary == 'means') + { + if(show_legends == TRUE){ + graph = graph + geom_line(aes_string(y=paste(experiments_type[m],'_',measures_names[i],'_avg',sep=''), colour=shQuote(experiments_labels[m]) ), size=2) + }else{ + graph = graph + geom_line(aes_string(y=paste(experiments_type[m],'_',measures_names[i],'_avg',sep='') ),size=2, color = experiments_type_colors[m]) + } + } else { # type_summary 'quants' + if(show_legends == TRUE){ + graph = graph + geom_line(aes_string(y=paste(experiments_type[m],'_',measures_names[i],'_median',sep='') , colour=shQuote(experiments_labels[m]) ),size=2 ) + }else{ + graph = graph + geom_line(aes_string(y=paste(experiments_type[m],'_',measures_names[i],'_median',sep='') ),size=2, color = experiments_type_colors[m] ) + } + } + + if (length(array_mann)>0) + { + if (length(array_mann[[m]])>0) + { + if(!is.na(array_mann[[m]][[i]]$p.value)) + { + if(array_mann[[m]][[i]]$p.value<=sig) + { + if(array_mann[[m]][[i]]$statistic>0){ direction = "/ "} else { direction = "\\ "} + tests1 = paste(tests1, initials[m],direction,sep="") + } + } + } + } + } + + if (length(array_wilcoxon[[m]])>0) + { + for(m in 1:length(experiments_type)) + { + if(!is.na(array_wilcoxon[[m]][[i]]$p.value)) + { + if(array_wilcoxon[[m]][[i]]$p.value<=sig) + { + tests2 = paste(tests2, initials[m],'C ', sep='') + } + } + } + } + + if (length(array_wilcoxon2)>0) + { + for(p in 1:length(array_wilcoxon2[[1]])) + { + if (length(array_wilcoxon2[[1]][[p]])>0) + { + if(!is.na(array_wilcoxon2[[1]][[p]][[i]]$p.value)) + { + if(array_wilcoxon2[[1]][[p]][[i]]$p.value<=sig) + { + if(nchar(tests3)>line_size && break_aux == 0){ + tests3 = paste(tests3, '\n') + break_aux = 1 + } + tests3 = paste(tests3, array_wilcoxon2[[2]][[p]],'D ',sep='') + } + } + } + } + } + + graph = graph + + #coord_cartesian(ylim = c(0, 1))+ + labs( y=measures_labels[i], x="Generation") + if(show_markers == TRUE){ + graph = graph + labs( y=measures_labels[i], x="Generation", subtitle = paste(tests1,'\n', tests2, '\n', tests3, sep='')) + } + graph = graph + theme(legend.position="bottom" , legend.text=element_text(size=20), axis.text=element_text(size=30),axis.title=element_text(size=39), + plot.subtitle=element_text(size=25 )) + + ggsave(paste( output_directory,'/',type_summary,'_' ,measures_names[i],'_generations.pdf', sep=''), graph , device='pdf', height = 8, width = 10) + } + +} diff --git a/experiments/nsga2-gait-rotation/check_running_experiments.sh b/experiments/nsga2-gait-rotation/check_running_experiments.sh new file mode 100755 index 0000000000..45e4070682 --- /dev/null +++ b/experiments/nsga2-gait-rotation/check_running_experiments.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +for i in exp*log; do echo -n "$i "; value=$(grep "Exported snapshot" $i|tail -n1|sed -E "s/\[(.*),.*Exported snapshot ([0-9]+).*/\2 -> \1/g"); echo $value; done diff --git a/experiments/nsga2-gait-rotation/experimentation_script.sh b/experiments/nsga2-gait-rotation/experimentation_script.sh new file mode 100755 index 0000000000..9552548c37 --- /dev/null +++ b/experiments/nsga2-gait-rotation/experimentation_script.sh @@ -0,0 +1,30 @@ +#!/bin/bash +set -e +set -x + + +#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_A --fitness displacement --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done + +#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_B --fitness rotation --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done + +#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_C --fitness gait_with_rotation --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done + +#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_D --fitness gait_and_rotation --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done + +#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_E --fitness rotation_with_gait --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done + + +runs=10 +runs_start=0 +start_port=13000 +exp_name=Experiment_NSGA2 +fitness=nsga2_gait_rotation +cores=4 +log_suffix='' +manager=experiments/nsga2-gait-rotation/manager.py + +for i in $(seq $runs) +do + run=$(($i+runs_start)) + screen -d -m -S "${exp_name}_${run}" -L -Logfile "${exp_name}${log_suffix}_${run}.log" nice -n19 ./revolve.sh --manager $manager --experiment-name $exp_name --fitness $fitness --n-cores $cores --port-start $((${start_port} + ($run*10))) --run $run --recovery-enabled True +done diff --git a/experiments/nsga2-gait-rotation/manager.py b/experiments/nsga2-gait-rotation/manager.py new file mode 100644 index 0000000000..0cef1cdd72 --- /dev/null +++ b/experiments/nsga2-gait-rotation/manager.py @@ -0,0 +1,182 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import os +import matplotlib +import matplotlib.pyplot as plt + +from pyrevolve import parser +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +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.experiment_management import ExperimentManagement + +from pyrevolve.genotype.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue + +from pyrevolve.custom_logging.logger import logger + +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + +from .nsga2 import NSGA2 + + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from pyrevolve.evolution.individual import Individual + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + objective_functions = [fitness.displacement_velocity, fitness.panoramic_rotation] + + body_conf = PlasticodingConfig( + max_structural_modules=50, + allow_vertical_brick=True, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=False, + allow_joint_joint_attachment=False, + ) + brain_conf = NeatBrainGenomeConfig() + brain_conf.multineat_params.DisjointCoeff = 0.3 + brain_conf.multineat_params.ExcessCoeff = 0.3 + brain_conf.multineat_params.WeightDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationADiffCoeff = 0.3 + brain_conf.multineat_params.ActivationBDiffCoeff = 0.3 + brain_conf.multineat_params.TimeConstantDiffCoeff = 0.3 + brain_conf.multineat_params.BiasDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationFunctionDiffCoeff = 0.3 + brain_conf.multineat_params.CompatTreshold = 3.0 + brain_conf.multineat_params.MinCompatTreshold = 3.0 + brain_conf.multineat_params.CompatTresholdModifier = 0.1 + brain_conf.multineat_params.CompatTreshChangeInterval_Generations = 1 + brain_conf.multineat_params.CompatTreshChangeInterval_Evaluations = 1 + genotype_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf, len(objective_functions)) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + + # experiment params # + + # Parse command line / file input arguments + args = parser.parse_args() + experiment_management = ExperimentManagement(args) + has_offspring = False + do_recovery = args.recovery_enabled and not experiment_management.experiment_is_new() + + logger.info(f'Activated run {args.run} of experiment {args.experiment_name}') + + if do_recovery: + gen_num, has_offspring, next_robot_id, next_species_id = \ + experiment_management.read_recovery_state(population_size, offspring_size, species=False, n_developments=2) + + if gen_num == num_generations-1: + logger.info('Experiment is already complete.') + return + else: + gen_num = 0 + next_robot_id = 1 + + if gen_num < 0: + logger.info('Experiment continuing from first generation') + gen_num = 0 + + if next_robot_id < 0: + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=genotype_conf, + fitness_function=None, + objective_functions=objective_functions, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + selection=lambda individuals: tournament_selection(individuals, 2), + parent_selection=lambda individuals: multiple_selection(individuals, 2, tournament_selection), + population_management=lambda pop, offsprings: NSGA2(pop, offsprings, debug=True), + population_management_selector=None, + evaluation_time=args.evaluation_time, + grace_time=args.grace_time, + offspring_size=offspring_size, + experiment_name=args.experiment_name, + experiment_management=experiment_management, + ) + + n_cores = args.n_cores + + simulator_queue = SimulatorQueue(n_cores, args, args.port_start) + await simulator_queue.start() + + analyzer_queue = AnalyzerQueue(1, args, args.port_start+n_cores) + await analyzer_queue.start() + + population = Population(population_conf, + simulator_queue, + analyzer_queue, + next_robot_id) + + if do_recovery: + # loading a previous state of the experiment + population.load_snapshot(gen_num, multi_development=True) + if gen_num >= 0: + logger.info(f'Recovered snapshot {gen_num}, pop with {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(f'Recovered unfinished offspring {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) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_generation(gen_num) + experiment_management.export_snapshots(population.individuals, gen_num) + + # save plot from NSGAII + generation_folder = experiment_management.generation_folder(gen_num) + plt.savefig(os.path.join(generation_folder, f'nsga2_front_{gen_num}.pdf')) + plt.clf() diff --git a/experiments/nsga2-gait-rotation/nsga2.py b/experiments/nsga2-gait-rotation/nsga2.py new file mode 100644 index 0000000000..d09279da74 --- /dev/null +++ b/experiments/nsga2-gait-rotation/nsga2.py @@ -0,0 +1,134 @@ +from math import inf +from typing import List +import numpy as np +import matplotlib.pyplot as plt +from matplotlib import cm +from mpl_toolkits.mplot3d import Axes3D + +from pyrevolve.evolution.individual import Individual + + +def NSGA2(population_individuals: List[Individual], offspring: List[Individual], debug: bool = False): + population_size = len(population_individuals) + offspring_size = len(offspring) + # Preparate the objectives as a matrix of individuals in the rows and fitnesses in the columns. + objectives = np.zeros( + (population_size + offspring_size, max(1, len(population_individuals[0].objectives)))) # TODO fitnesses is 0 + # Fill the objectives with all individual from the population and offspring combined. + all_individuals = [individual for individual in population_individuals] + all_individuals.extend(offspring) + for index, individual in enumerate(all_individuals): + # Negative fitness due to minimum search, TODO can be changed to be a default maximization NSGA. + objectives[index, :] = [inf if objective is None else -objective for objective in individual.objectives] + # Perform the NSGA Algorithm + front_no, max_front = nd_sort(objectives, np.inf) + crowd_dis = crowding_distance(objectives, front_no) + sorted_fronts = sort_fronts(objectives, front_no, crowd_dis) + # Select the new individuals based on the population size, since they are sorted we can index them directly. + new_individuals = [all_individuals[index] for index in sorted_fronts[:population_size]] + if debug: + discarded_individuals = [all_individuals[index] for index in sorted_fronts[population_size:]] + _visualize(objectives, sorted_fronts, new_individuals, discarded_individuals) + return new_individuals + + +def _visualize(objectives, sorted_fronts, new_individuals, discarded_population): + number_of_fronts = len(sorted_fronts) + colors = cm.rainbow(np.linspace(1, 0, number_of_fronts)) + ax = None + if objectives.shape[1] == 3: + fig = plt.figure() + ax = fig.add_subplot(111, projection='3d') + for index, front in enumerate(sorted_fronts): + if objectives.shape[1] == 3: + ax.scatter(objectives[front, 0], objectives[front, 1], objectives[front, 2], s=100, + color=colors[index]) + else: + plt.scatter(-objectives[front, 0], -objectives[front, 1], s=50, + color=colors[index]) + for individual in new_individuals: + plt.scatter(individual.objectives[0], individual.objectives[1], s=5, color='black') + for individual in discarded_population: + plt.scatter(individual.objectives[0], individual.objectives[1], s=5, color='white') + + +# adapted from https://www.programmersought.com/article/6084850621/ +def nd_sort(objectives: np.ndarray, max_range: float) -> (List[int], int): + """ + Non-dominated Sorting algorithm + :param objectives: objective matrix + :param max_range: + :return: (front numbers, biggest front number) + """ + number_of_individuals, number_of_objectives = objectives.shape[0], objectives.shape[1] + sorted_matrix = np.lexsort(objectives[:, ::-1].T) # loc1 is the position of the new matrix element in the old matrix, sorted from the first column in order + sorted_objectives = objectives[sorted_matrix] + inverse_sorted_indexes = sorted_matrix.argsort() # loc2 is the position of the old matrix element in the new matrix + front_no = np.ones(number_of_individuals) * np.inf # Initialize all levels to np.inf + max_front_no = 0 # 0 + while np.sum(front_no < np.inf) < min(max_range, number_of_individuals): # The number of individuals assigned to the rank does not exceed the number of individuals to be sorted + max_front_no = max_front_no + 1 + for i in range(number_of_individuals): + if front_no[i] == np.inf: + dominated = False + for j in range(i): + if front_no[j] == max_front_no: + m = 0 + flag = 0 + while m < number_of_objectives and sorted_objectives[i, m] >= sorted_objectives[j, m]: + if sorted_objectives[i, m] == sorted_objectives[j, m]: # does not constitute a dominant relationship + flag = flag + 1 + m = m + 1 + if m >= number_of_objectives and flag < number_of_objectives: + dominated = True + break + if not dominated: + front_no[i] = max_front_no + front_no = front_no[inverse_sorted_indexes] + return front_no, max_front_no + + +# adapted from https://github.com/ChengHust/NSGA-II/blob/master/crowding_distance.py +def crowding_distance(objectives, front_number): + """ + The crowding distance of each Pareto front + :param objectives: objective vectors + :param front_number: front numbers + :return: crowding distance + """ + number_of_individuals, number_of_objectives = np.shape(objectives) # todo x, y? + crowd_dis = np.zeros(number_of_individuals) + # Initialize fronts + front = np.unique(front_number) + fronts = front[front != np.inf] + for f in range(len(fronts)): + front = np.array([k for k in range(len(front_number)) if front_number[k] == fronts[f]]) + # Find bounds + Fmax = objectives[front, :].max(0) + Fmin = objectives[front, :].min(0) + # For each objective sort the front + for i in range(number_of_objectives): + rank = np.argsort(objectives[front, i]) + # Initialize Crowding distance + crowd_dis[front[rank[0]]] = np.inf + crowd_dis[front[rank[-1]]] = np.inf + for j in range(1, len(front) - 1): + crowd_dis[front[rank[j]]] += (objectives[(front[rank[j + 1]], i)] - + objectives[(front[rank[j - 1]], i)]) / (Fmax[i] - Fmin[i]) + return crowd_dis + + +def sort_fronts(objectives, front_no, crowd_dis): + front_dict = dict() # dictionary indexed by front number inserting objective with crowd distance as tuple + for objective_index in range(len(objectives)): + if front_no[objective_index] not in front_dict.keys(): + front_dict[front_no[objective_index]] = [(crowd_dis[objective_index], objective_index)] + else: + front_dict[front_no[objective_index]].append((crowd_dis[objective_index], objective_index)) + sorted_fronts = [] + sorted_keys = sorted(front_dict.keys()) + for key in sorted_keys: + front_dict[key].sort(key=lambda x: x[0], reverse=True) + for element in front_dict[key]: + sorted_fronts.append(element[1]) + return sorted_fronts diff --git a/experiments/nsga2-gait-rotation/recovery_test.py b/experiments/nsga2-gait-rotation/recovery_test.py new file mode 100644 index 0000000000..913e105105 --- /dev/null +++ b/experiments/nsga2-gait-rotation/recovery_test.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +import os +import matplotlib +import matplotlib.pyplot as plt + +from pyrevolve import parser +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +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.experiment_management import ExperimentManagement + +from pyrevolve.genotype.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue + +from pyrevolve.custom_logging.logger import logger + +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + +from .nsga2 import NSGA2 + + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from pyrevolve.evolution.individual import Individual + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + objective_functions = [fitness.displacement_velocity, fitness.panoramic_rotation] + + body_conf = PlasticodingConfig( + max_structural_modules=50, + allow_vertical_brick=True, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=False, + allow_joint_joint_attachment=False, + ) + brain_conf = NeatBrainGenomeConfig() + brain_conf.multineat_params.DisjointCoeff = 0.3 + brain_conf.multineat_params.ExcessCoeff = 0.3 + brain_conf.multineat_params.WeightDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationADiffCoeff = 0.3 + brain_conf.multineat_params.ActivationBDiffCoeff = 0.3 + brain_conf.multineat_params.TimeConstantDiffCoeff = 0.3 + brain_conf.multineat_params.BiasDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationFunctionDiffCoeff = 0.3 + brain_conf.multineat_params.CompatTreshold = 3.0 + brain_conf.multineat_params.MinCompatTreshold = 3.0 + brain_conf.multineat_params.CompatTresholdModifier = 0.1 + brain_conf.multineat_params.CompatTreshChangeInterval_Generations = 1 + brain_conf.multineat_params.CompatTreshChangeInterval_Evaluations = 1 + genotype_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf, len(objective_functions)) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + + # experiment params # + + # Parse command line / file input arguments + args = parser.parse_args() + experiment_management = ExperimentManagement(args) + has_offspring = False + do_recovery = args.recovery_enabled and not experiment_management.experiment_is_new() + + logger.info(f'Activated run {args.run} of experiment {args.experiment_name}') + + if do_recovery: + gen_num, has_offspring, next_robot_id, next_species_id = \ + experiment_management.read_recovery_state(population_size, offspring_size, species=False) + + if gen_num == num_generations-1: + logger.info('Experiment is already complete.') + return + else: + gen_num = 0 + next_robot_id = 1 + + if gen_num < 0: + logger.info('Experiment continuing from first generation') + gen_num = 0 + + if next_robot_id < 0: + next_robot_id = 1 + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=genotype_conf, + fitness_function=None, + objective_functions=objective_functions, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + selection=lambda individuals: tournament_selection(individuals, 2), + parent_selection=lambda individuals: multiple_selection(individuals, 2, tournament_selection), + population_management=lambda pop, offsprings: NSGA2(pop, offsprings, debug=True), + population_management_selector=None, + evaluation_time=args.evaluation_time, + grace_time=args.grace_time, + offspring_size=offspring_size, + experiment_name=args.experiment_name, + experiment_management=experiment_management, + ) + + n_cores = args.n_cores + + # simulator_queue = SimulatorQueue(n_cores, args, args.port_start) + # await simulator_queue.start() + + # analyzer_queue = AnalyzerQueue(1, args, args.port_start+n_cores) + # await analyzer_queue.start() + + population = Population(population_conf, + None, + None, + next_robot_id) + + if do_recovery: + # loading a previous state of the experiment + population.load_snapshot(gen_num) + 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: + print("await population.initialize(individuals)") + else: + print("population = await population.next_generation(gen_num, individuals)") + + print("experiment_management.export_snapshots(population.individuals, gen_num)") + else: + # starting a new experiment + raise RuntimeError("We just want to test the recovery") + # experiment_management.create_exp_folders() + # await population.initialize() + # experiment_management.export_snapshots(population.individuals, gen_num) + + # while gen_num < num_generations-1: + # gen_num += 1 + # population = await population.next_generation(gen_num) + # experiment_management.export_snapshots(population.individuals, gen_num) + # + # # save plot from NSGAII + # generation_folder = experiment_management.generation_folder(gen_num) + # plt.savefig(os.path.join(generation_folder, f'nsga2_front_{gen_num}.pdf')) + # plt.clf() diff --git a/experiments/nsga2-gait-rotation/test_nsga2.py b/experiments/nsga2-gait-rotation/test_nsga2.py new file mode 100755 index 0000000000..4f67ffeb47 --- /dev/null +++ b/experiments/nsga2-gait-rotation/test_nsga2.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 + +import math +from typing import Optional, List + +import numpy as np +import matplotlib +from pyrevolve.evolution.individual import Individual +from .nsga2 import NSGA2 + +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +from pyrevolve.evolution.population.population import Population +from pyrevolve.evolution.population.population_config import PopulationConfig +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 import PlasticodingConfig + +# setup matplotlib renderer correctly +gui_env = ['TKAgg', 'GTKAgg', 'Qt4Agg', 'WXAgg'] +gui_env = ['tk', 'gtk', 'gtk3', 'wx', 'qt4', 'qt5', 'qt', 'osx', 'nbagg', 'notebook', 'agg', 'svg', 'pdf', 'ps', + 'inline', 'ipympl', 'widget'] +for gui in gui_env: + try: + print(f"testing matplotlib backend {gui}") + matplotlib.use(gui, warn=False, force=True) + from matplotlib import pyplot as plt + + break + except: + continue +print(f"Using matplotlib backend: {matplotlib.get_backend()}") + + +class MockPopulation(Population): + def __init__(self, conf: PopulationConfig, simulator_queue, analyzer_queue=None, next_robot_id=1): + super().__init__(conf, simulator_queue, analyzer_queue, next_robot_id) + self.individuals = [] + + def init_pop(self, recovered_individuals=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)): + print("create individual") + individual = self._new_individual( + self.config.genotype_constructor(self.config.genotype_conf, self.next_robot_id)) + self.individuals.append(individual) + self.next_robot_id += 1 + self.individuals = recovered_individuals + self.individuals + + def _new_individual(self, genotype, + parents: Optional[List[Individual]] = None): + individual = Individual(genotype) + individual.develop() + return individual + + +def main(): + # experiment params # + num_generations = 100 + population_size = 100 + offspring_size = 200 + genotype_conf = PlasticodingConfig( + max_structural_modules=20, + # max_joints=10, + ) + mutation_conf = MutationConfig( + mutation_prob=0.8, + genotype_conf=genotype_conf, + ) + crossover_conf = CrossoverConfig( + crossover_prob=0.8, + ) + # experiment params # + # Parse command line / file input arguments + gen_num = 0 + next_robot_id = 1 + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=random_initialization, + genotype_conf=genotype_conf, + fitness_function=fitness.displacement_velocity, + mutation_operator=standard_mutation, + mutation_conf=mutation_conf, + crossover_operator=standard_crossover, + crossover_conf=crossover_conf, + selection=lambda individuals: tournament_selection(individuals, 4), + parent_selection=lambda individuals: multiple_selection(individuals, 4, tournament_selection), + population_management=NSGA2, + population_management_selector=None, + evaluation_time=0, + offspring_size=offspring_size, + experiment_name="test", + experiment_management=None, + ) + population = MockPopulation(population_conf, None, None, next_robot_id) + population.init_pop() + offspring = [] + for i in range(population_conf.offspring_size): + individual = Individual(population_conf.genotype_constructor(population_conf.genotype_conf, 0)) + individual.develop() + offspring.append(individual) + + def initialize_fitness(individuals): + d3_enabled: bool = False + for individual in individuals: + theta = np.random.uniform(0, math.pi / 2) + omega = np.random.uniform(0, math.pi / 2) + r = np.random.uniform(0.5, 1) + if d3_enabled: + individual.objectives = [r * np.cos(theta) * np.cos(omega), + r * np.sin(theta), + r * np.cos(theta) * np.sin(omega)] + else: + individual.objectives = [r * np.cos(theta), + r * np.sin(theta)] + + initialize_fitness(population.individuals) + initialize_fitness(offspring) + survived_individuals = NSGA2(population.individuals, offspring, debug=True) + plt.show() + + +if __name__ == "__main__": + main() diff --git a/experiments/task-morphology/check_running_experiments.sh b/experiments/task-morphology/check_running_experiments.sh new file mode 100755 index 0000000000..45e4070682 --- /dev/null +++ b/experiments/task-morphology/check_running_experiments.sh @@ -0,0 +1,3 @@ +#!/bin/bash + +for i in exp*log; do echo -n "$i "; value=$(grep "Exported snapshot" $i|tail -n1|sed -E "s/\[(.*),.*Exported snapshot ([0-9]+).*/\2 -> \1/g"); echo $value; done diff --git a/experiments/task-morphology/experimentation_script.sh b/experiments/task-morphology/experimentation_script.sh new file mode 100755 index 0000000000..f7cc92f385 --- /dev/null +++ b/experiments/task-morphology/experimentation_script.sh @@ -0,0 +1,32 @@ +#!/bin/bash +set -e +set -x + + +#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_A --fitness displacement --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done + +#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_B --fitness rotation --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done + +#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_C --fitness gait_with_rotation --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done + +#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_D --fitness gait_and_rotation --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done + +#for i in $(seq 1 10); do ./revolve.sh --experiment-name Experiment_E --fitness rotation_with_gait --simulator-cmd=gzserver --manager experiments/task-morphology/manager.py --port-start=$port --world worlds/plane.world ; done + + +runs=10 +runs_start=0 +start_port=13000 +exp_name=Experiment_E +fitness=rotation_with_gait +cores=4 +exp_name=Experiment_B +fitness=rotation +log_suffix='' +manager=experiments/task-morphology/manager.py + +for i in $(seq $runs) +do + run=$(($i+runs_start)) + screen -d -m -S "${exp_name}_${run}" -L -Logfile "${exp_name}${log_suffix}_${run}.log" nice -n19 ./revolve.sh --manager $manager --experiment-name $exp_name --fitness $fitness --n-cores $cores --port-start $((${start_port} + ($run*10))) --run $run --recovery-enabled True +done diff --git a/experiments/task-morphology/manager.py b/experiments/task-morphology/manager.py new file mode 100644 index 0000000000..c7620c4515 --- /dev/null +++ b/experiments/task-morphology/manager.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +from pyrevolve import parser + +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +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.experiment_management import ExperimentManagement + +from pyrevolve.genotype.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue + +from pyrevolve.custom_logging.logger import logger + +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from pyrevolve.evolution.individual import Individual + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 200 + population_size = 100 + offspring_size = 50 + + body_conf = PlasticodingConfig( + max_structural_modules=50, + allow_vertical_brick=True, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=False, + allow_joint_joint_attachment=False, + ) + brain_conf = NeatBrainGenomeConfig() + brain_conf.multineat_params.DisjointCoeff = 0.3 + brain_conf.multineat_params.ExcessCoeff = 0.3 + brain_conf.multineat_params.WeightDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationADiffCoeff = 0.3 + brain_conf.multineat_params.ActivationBDiffCoeff = 0.3 + brain_conf.multineat_params.TimeConstantDiffCoeff = 0.3 + brain_conf.multineat_params.BiasDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationFunctionDiffCoeff = 0.3 + brain_conf.multineat_params.CompatTreshold = 3.0 + brain_conf.multineat_params.MinCompatTreshold = 3.0 + brain_conf.multineat_params.CompatTresholdModifier = 0.1 + brain_conf.multineat_params.CompatTreshChangeInterval_Generations = 1 + brain_conf.multineat_params.CompatTreshChangeInterval_Evaluations = 1 + genotype_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + + # experiment params # + + # Parse command line / file input arguments + args = parser.parse_args() + experiment_management = ExperimentManagement(args) + has_offspring = False + do_recovery = args.recovery_enabled and not experiment_management.experiment_is_new() + + logger.info(f'Activated run {args.run} of experiment {args.experiment_name}') + + if do_recovery: + gen_num, has_offspring, next_robot_id, next_species_id = \ + experiment_management.read_recovery_state(population_size, offspring_size, species=False) + + if gen_num == num_generations-1: + logger.info('Experiment is already complete.') + return + else: + gen_num = 0 + next_robot_id = 1 + + if gen_num < 0: + logger.info('Experiment continuing from first generation') + gen_num = 0 + + if next_robot_id < 0: + next_robot_id = 1 + + # Experimental selection of fitness function from config fitness argument. + fitness_function = getattr(fitness, args.fitness) + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=genotype_conf, + fitness_function=fitness_function, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + 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=args.evaluation_time, + grace_time=args.grace_time, + offspring_size=offspring_size, + experiment_name=args.experiment_name, + experiment_management=experiment_management, + ) + + n_cores = args.n_cores + + simulator_queue = SimulatorQueue(n_cores, args, args.port_start) + await simulator_queue.start() + + analyzer_queue = AnalyzerQueue(1, args, args.port_start+n_cores) + await analyzer_queue.start() + + population = Population(population_conf, + simulator_queue, + analyzer_queue, + next_robot_id) + + if do_recovery: + # loading a previous state of the experiment + population.load_snapshot(gen_num) + 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) + + 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/task-morphology/manager_test.py b/experiments/task-morphology/manager_test.py new file mode 100644 index 0000000000..f6481efe2b --- /dev/null +++ b/experiments/task-morphology/manager_test.py @@ -0,0 +1,172 @@ +#!/usr/bin/env python3 +from __future__ import annotations + +from pyrevolve import parser + +from pyrevolve.evolution import fitness +from pyrevolve.evolution.selection import multiple_selection, tournament_selection +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.experiment_management import ExperimentManagement + +from pyrevolve.genotype.lsystem_neat.crossover import CrossoverConfig as lCrossoverConfig +from pyrevolve.genotype.lsystem_neat.crossover import standard_crossover as lcrossover +from pyrevolve.genotype.lsystem_neat.mutation import LSystemNeatMutationConf as lMutationConfig +from pyrevolve.genotype.lsystem_neat.mutation import standard_mutation as lmutation +from pyrevolve.genotype.lsystem_neat.lsystem_neat_genotype import LSystemCPGHyperNEATGenotype, LSystemCPGHyperNEATGenotypeConfig +from pyrevolve.genotype.plasticoding.mutation.mutation import MutationConfig as plasticMutationConfig + +from pyrevolve.util.supervisor.analyzer_queue import AnalyzerQueue +from pyrevolve.util.supervisor.simulator_queue import SimulatorQueue + +from pyrevolve.custom_logging.logger import logger + +from pyrevolve.genotype.plasticoding import PlasticodingConfig +from pyrevolve.genotype.neat_brain_genome.neat_brain_genome import NeatBrainGenomeConfig + + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from pyrevolve.evolution.individual import Individual + + +async def run(): + """ + The main coroutine, which is started below. + """ + + # experiment params # + num_generations = 3 + population_size = 3 + offspring_size = 2 + + body_conf = PlasticodingConfig( + max_structural_modules=50, + allow_vertical_brick=True, + use_movement_commands=True, + use_rotation_commands=False, + use_movement_stack=False, + allow_joint_joint_attachment=False, + ) + brain_conf = NeatBrainGenomeConfig() + brain_conf.multineat_params.DisjointCoeff = 0.3 + brain_conf.multineat_params.ExcessCoeff = 0.3 + brain_conf.multineat_params.WeightDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationADiffCoeff = 0.3 + brain_conf.multineat_params.ActivationBDiffCoeff = 0.3 + brain_conf.multineat_params.TimeConstantDiffCoeff = 0.3 + brain_conf.multineat_params.BiasDiffCoeff = 0.3 + brain_conf.multineat_params.ActivationFunctionDiffCoeff = 0.3 + brain_conf.multineat_params.CompatTreshold = 3.0 + brain_conf.multineat_params.MinCompatTreshold = 3.0 + brain_conf.multineat_params.CompatTresholdModifier = 0.1 + brain_conf.multineat_params.CompatTreshChangeInterval_Generations = 1 + brain_conf.multineat_params.CompatTreshChangeInterval_Evaluations = 1 + genotype_conf = LSystemCPGHyperNEATGenotypeConfig(body_conf, brain_conf) + + plasticMutation_conf = plasticMutationConfig( + mutation_prob=0.8, + genotype_conf=body_conf + ) + + lmutation_conf = lMutationConfig( + plasticoding_mutation_conf=plasticMutation_conf, + neat_conf=brain_conf, + ) + + crossover_conf = lCrossoverConfig( + crossover_prob=0.8, + ) + + # experiment params # + + # Parse command line / file input arguments + args = parser.parse_args() + experiment_management = ExperimentManagement(args) + has_offspring = False + do_recovery = args.recovery_enabled and not experiment_management.experiment_is_new() + + logger.info(f'Activated run {args.run} of experiment {args.experiment_name}') + + if do_recovery: + gen_num, has_offspring, next_robot_id, next_species_id = \ + experiment_management.read_recovery_state(population_size, offspring_size, species=False) + + if gen_num == num_generations-1: + logger.info('Experiment is already complete.') + return + else: + gen_num = 0 + next_robot_id = 1 + + if gen_num < 0: + logger.info('Experiment continuing from first generation') + gen_num = 0 + + if next_robot_id < 0: + next_robot_id = 1 + + # Experimental selection of fitness function from config fitness argument. + fitness_function = getattr(fitness, args.fitness) + + population_conf = PopulationConfig( + population_size=population_size, + genotype_constructor=LSystemCPGHyperNEATGenotype, + genotype_conf=genotype_conf, + fitness_function=fitness_function, + mutation_operator=lmutation, + mutation_conf=lmutation_conf, + crossover_operator=lcrossover, + crossover_conf=crossover_conf, + 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=args.evaluation_time, + offspring_size=offspring_size, + experiment_name=args.experiment_name, + experiment_management=experiment_management, + ) + + n_cores = args.n_cores + + simulator_queue = SimulatorQueue(n_cores, args, args.port_start) + await simulator_queue.start() + + analyzer_queue = AnalyzerQueue(1, args, args.port_start+n_cores) + await analyzer_queue.start() + + population = Population(population_conf, + simulator_queue, + analyzer_queue, + next_robot_id) + + if do_recovery: + # loading a previous state of the experiment + population.load_snapshot(gen_num) + 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) + + while gen_num < num_generations-1: + gen_num += 1 + population = await population.next_generation(gen_num) + logger.info("after next generation\n") + experiment_management.export_snapshots(population.individuals, gen_num) diff --git a/experiments/task-morphology/test_experiment.sh b/experiments/task-morphology/test_experiment.sh new file mode 100755 index 0000000000..9ac62cad08 --- /dev/null +++ b/experiments/task-morphology/test_experiment.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +port=${1:-6000} + +for i in $(seq 1 1); do ./revolve.sh --experiment-name Test_Experiment --fitness displacement --simulator-cmd=gzserver --manager experiments/task-morphology/manager_test.py --port-start=$port --world worlds/plane.world --recovery-enabled True; done 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..4e68894eb3 100644 --- a/pyrevolve/SDF/revolve_bot_sdf_builder.py +++ b/pyrevolve/SDF/revolve_bot_sdf_builder.py @@ -11,9 +11,12 @@ 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) + if str(robot_id).isdigit(): + 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 +153,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 +204,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..f3b481b72a 100644 --- a/pyrevolve/evolution/fitness.py +++ b/pyrevolve/evolution/fitness.py @@ -1,5 +1,22 @@ +from __future__ import annotations import random as py_random +import math + +from pyrevolve.custom_logging.logger import logger +from pyrevolve.revolve_bot.revolve_module import Orientation from pyrevolve.tol.manage import measures +from pyrevolve.SDF.math import Vector3 + +from typing import TYPE_CHECKING +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): @@ -81,3 +98,118 @@ 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 + + +# This will not be part of future code, solely for experimental practice +def gait_with_rotation(_robot_manager, robot): + gait_fitness = displacement(_robot_manager, robot) + rotation_fitness = rotation(_robot_manager, robot) + + return 0.75 * gait_fitness + 0.25 * rotation_fitness + + +# This will not be part of future code, solely for experimental practice +def gait_and_rotation(_robot_manager, robot): + gait_fitness = displacement(_robot_manager, robot) + rotation_fitness = rotation(_robot_manager, robot) + + return 0.5 * gait_fitness + 0.5 * rotation_fitness + + +# This will not be part of future code, solely for experimental practice +def rotation_with_gait(_robot_manager, robot): + gait_fitness = displacement(_robot_manager, robot) + rotation_fitness = rotation(_robot_manager, robot) + + return 0.75 * rotation_fitness + 0.25 * gait_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..c09243561b --- /dev/null +++ b/pyrevolve/evolution/population/population.py @@ -0,0 +1,330 @@ +from __future__ import annotations +import asyncio +import os +import math +import re + +from pyrevolve.evolution import fitness +from pyrevolve.evolution.individual import Individual +from pyrevolve.custom_logging.logger import logger +from pyrevolve.evolution.population.population_config import PopulationConfig +from pyrevolve.revolve_bot.revolve_bot import RevolveBot +from pyrevolve.tol.manage.measures import BehaviouralMeasurements + +from typing import TYPE_CHECKING +if TYPE_CHECKING: + from typing import List, Optional, Callable + 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=False) -> 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 = _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(str(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) + + 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..9b1d148ef1 100644 --- a/pyrevolve/evolution/pop_management/steady_state.py +++ b/pyrevolve/evolution/population/population_management.py @@ -2,7 +2,15 @@ def steady_state_population_management(old_individuals, new_individuals, selector): + # Stead 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..27c4837a68 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: AnyStr, + 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..c91a48e598 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,7 +96,7 @@ 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: @@ -107,21 +109,20 @@ async def pause(self, pause=True): 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() @@ -130,7 +131,7 @@ async def reset( 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 +145,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 +155,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 +165,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/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..aad5efdbfe 100644 --- a/pyrevolve/revolve_bot/brain/__init__.py +++ b/pyrevolve/revolve_bot/brain/__init__.py @@ -2,3 +2,5 @@ from .brain_nn import BrainNN from .rlpower_splines import BrainRLPowerSplines from .bo_cpg import BrainCPGBO +from .cpg import BrainCPG +from .cppn_cpg import BrainCPPNCPG diff --git a/pyrevolve/revolve_bot/brain/base.py b/pyrevolve/revolve_bot/brain/base.py index b16ea73b8b..e5e715c212 100644 --- a/pyrevolve/revolve_bot/brain/base.py +++ b/pyrevolve/revolve_bot/brain/base.py @@ -13,7 +13,12 @@ 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) 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..e74123b7f4 --- /dev/null +++ b/pyrevolve/revolve_bot/brain/cpg.py @@ -0,0 +1,104 @@ +""" +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.signal_factor_all = "" + self.signal_factor_mid = None + self.signal_factor_left_right = None + 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, + 'signal_factor_all': self.signal_factor_all, + 'signal_factor_mid': self.signal_factor_mid, + 'signal_factor_left_right': self.signal_factor_left_right, + '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), + '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), + 'weights': ';'.join(str(x) for x in self.weights) + }) 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..5981d81283 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,7 +173,7 @@ 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): + def draw_hinge(self, mod_id) -> None: """Draw a hinge (blue) on the previous object""" self.context.rectangle(Canvas.x_pos, Canvas.y_pos, 1, 1) @@ -181,7 +189,7 @@ 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): + def draw_module(self, mod_id) -> None: """Draw a module (red) on the previous object""" self.context.rectangle(Canvas.x_pos, Canvas.y_pos, 1, 1) self.context.set_source_rgb(0, 0, 1) @@ -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..88d18d7197 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 @@ -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,19 +105,19 @@ 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 @@ -123,55 +130,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..059c6bb357 100644 --- a/pyrevolve/util/time.py +++ b/pyrevolve/util/time.py @@ -195,7 +195,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..93ea2e0111 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 @@ -13,4 +13,6 @@ pandas neat-python>=0.92 python-dateutil>=2.8.0 +# 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/revolve.sh b/revolve.sh new file mode 100755 index 0000000000..832a2ac865 --- /dev/null +++ b/revolve.sh @@ -0,0 +1,9 @@ +#!/bin/bash +REVOLVE_DIR=$(dirname "$0") + +ulimit -n 65536 +source "$REVOLVE_DIR/.venv/bin/activate" +#export LD_LIBRARY_PATH="~/Tools/gazebo/lib64/:$LD_LIBRARY_PATH" +#export PATH="~/Tools/gazebo/bin/:$PATH" + +exec ./revolve.py $@ diff --git a/start_experiment.sh b/start_experiment.sh new file mode 100755 index 0000000000..835cade259 --- /dev/null +++ b/start_experiment.sh @@ -0,0 +1,16 @@ +#!/bin/bash +set -e +set -x + +runs=10 +runs_start=0 +start_port=15000 +exp_name=expHeritabilityDirect +log_suffix='_tenebra' +manager=experiments/heritability/manager_direct.py + +for i in $(seq $runs) +do + run=$(($i+runs_start)) + screen -d -m -S "${exp_name}_${run}" -L -Logfile "${exp_name}${log_suffix}_${run}.log" nice -n19 ./revolve.sh --manager $manager --experiment-name $exp_name --n-cores 4 --port-start $((${start_port} + ($run*10))) --run $run +done 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..c2a74e3262 100755 --- a/test_robots.py +++ b/test_robots.py @@ -8,7 +8,7 @@ async def run(): settings = parser.parse_args() - yaml_file = 'experiments/'+ settings.experiment_name +'/data_fullevolution/phenotypes/'+'phenotype_'+settings.test_robot+'.yaml' + 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') 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 diff --git a/worlds/plane.recording.world b/worlds/plane.recording.world new file mode 100644 index 0000000000..fa952265fa --- /dev/null +++ b/worlds/plane.recording.world @@ -0,0 +1,79 @@ + + + + + 0 + 0 + 0 + + + + 0.005 + + + 200 + + + + + 0.1 + 10e-6 + + + 100 + 1e-8 + + + world + + + + + + true + -1 0 2 0 1 0 + + + + + 0.1 0.1 0.1 + + + + + + + /tmp/camera_save_tutorial + + 1.047 + + 1920 + 1080 + + + 0.1 + 100 + + + 1 + 30 + + + /home/matteo/.gazebo/videos/ + + + + + + + + + model://sun + + + model://tol_ground + + + + +