From 6f6fed887c143445ca036e3667933fdd93843771 Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Fri, 12 Jun 2020 04:03:10 -0700 Subject: [PATCH 01/13] Setup experimental setup for task morphology research. For more information see https://docs.google.com/document/d/1Ptq1I5R5IhucFpeSHI93i3IMczgNWdhrttzVj3Iitwo/edit# Still has a problem with being able to analyze the brain [2020-06-12 03:55:47,260 revolve] ERROR Brain not supported [2020-06-12 03:55:47,260 revolve] ERROR Failed measuring brain: Brain not supported [2020-06-12 03:55:47,261 revolve] INFO Robot 36 was measured. [2020-06-12 03:55:47,322 revolve] WARNING Error rendering phenotype images: Brain image rendering not supported Test for yourself by running in the root folder: ./experiments/task-morphology/test_experiment.sh --- .../check_running_experiments.sh | 3 + .../task-morphology/experimentation_script.sh | 13 ++ experiments/task-morphology/manager.py | 171 ++++++++++++++++++ .../task-morphology/test_experiment.sh | 5 + pyrevolve/config.py | 7 + pyrevolve/evolution/fitness.py | 26 ++- revolve.sh | 8 + 7 files changed, 232 insertions(+), 1 deletion(-) 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 100755 experiments/task-morphology/test_experiment.sh create mode 100755 revolve.sh 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..3fa5c5dcbe --- /dev/null +++ b/experiments/task-morphology/experimentation_script.sh @@ -0,0 +1,13 @@ +#!/bin/bash + +port=${1:-6000} + +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 diff --git a/experiments/task-morphology/manager.py b/experiments/task-morphology/manager.py new file mode 100644 index 0000000000..d2548d3ba5 --- /dev/null +++ b/experiments/task-morphology/manager.py @@ -0,0 +1,171 @@ +#!/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.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_with_duplicates(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 + 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.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_species(population.genus, gen_num) diff --git a/experiments/task-morphology/test_experiment.sh b/experiments/task-morphology/test_experiment.sh new file mode 100755 index 0000000000..f30c10a2e9 --- /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.py --port-start=$port --world worlds/plane.world ; done diff --git a/pyrevolve/config.py b/pyrevolve/config.py index 2f22e1bd56..eba79dde9b 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, diff --git a/pyrevolve/evolution/fitness.py b/pyrevolve/evolution/fitness.py index 95285597e8..59bc33bc8c 100644 --- a/pyrevolve/evolution/fitness.py +++ b/pyrevolve/evolution/fitness.py @@ -98,7 +98,7 @@ def floor_is_lava(robot_manager, robot): return fitness -def rotation(robot_manager: RobotManager, _robot: RevolveBot, factor_orien_ds: float = 3.0): +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 @@ -127,3 +127,27 @@ def rotation(robot_manager: RobotManager, _robot: RevolveBot, factor_orien_ds: f fitness_value: float = orientations - factor_orien_ds * robot_manager._dist return fitness_value + + +# 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/revolve.sh b/revolve.sh new file mode 100755 index 0000000000..034e93b445 --- /dev/null +++ b/revolve.sh @@ -0,0 +1,8 @@ +#!/bin/bash +REVOLVE_DIR=$(dirname "$0") + +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 $@ From 35af7bcaad440a492b9b2c551afba9c7a4468677 Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Fri, 12 Jun 2020 15:43:36 +0200 Subject: [PATCH 02/13] Removed duplicate selection procedure --- experiments/task-morphology/manager.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/experiments/task-morphology/manager.py b/experiments/task-morphology/manager.py index d2548d3ba5..49566f57ce 100644 --- a/experiments/task-morphology/manager.py +++ b/experiments/task-morphology/manager.py @@ -4,7 +4,7 @@ from pyrevolve import parser from pyrevolve.evolution import fitness -from pyrevolve.evolution.selection import multiple_selection_with_duplicates, tournament_selection +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 @@ -121,7 +121,7 @@ async def run(): 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), + 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, From 29550ff19c15be86e971ea66fa4a10ea42cde5ce Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Sun, 14 Jun 2020 11:52:53 +0200 Subject: [PATCH 03/13] Removed left-over speciation snapshot call --- experiments/task-morphology/manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/experiments/task-morphology/manager.py b/experiments/task-morphology/manager.py index 49566f57ce..fe4f32cdf2 100644 --- a/experiments/task-morphology/manager.py +++ b/experiments/task-morphology/manager.py @@ -168,4 +168,4 @@ async def run(): 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) + experiment_management.export_snapshots(population.individuals, gen_num) From b138799a1b3fcbeeb7ac48e694b7b584e5f9403b Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Sun, 14 Jun 2020 11:56:58 +0200 Subject: [PATCH 04/13] Fix recovery procedure by removing old await procedures --- experiments/task-morphology/manager.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/experiments/task-morphology/manager.py b/experiments/task-morphology/manager.py index fe4f32cdf2..0edf507100 100644 --- a/experiments/task-morphology/manager.py +++ b/experiments/task-morphology/manager.py @@ -145,11 +145,11 @@ async def run(): if do_recovery: # loading a previous state of the experiment - await population.load_snapshot(gen_num) + population.load_snapshot(gen_num) if gen_num >= 0: logger.info('Recovered snapshot '+str(gen_num)+', pop with ' + str(len(population.individuals))+' individuals') if has_offspring: - individuals = await population.load_offspring(gen_num, population_size, offspring_size, next_robot_id) + individuals = population.load_offspring(gen_num, population_size, offspring_size, next_robot_id) gen_num += 1 logger.info('Recovered unfinished offspring '+str(gen_num)) From d525053c8d4313e3e4e5c86e05aefb3a6e0a3f51 Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Sun, 14 Jun 2020 15:20:46 +0200 Subject: [PATCH 05/13] Default experimentation script --- .../task-morphology/experimentation_script.sh | 28 +++++++++++++++---- 1 file changed, 22 insertions(+), 6 deletions(-) diff --git a/experiments/task-morphology/experimentation_script.sh b/experiments/task-morphology/experimentation_script.sh index 3fa5c5dcbe..546af6daee 100755 --- a/experiments/task-morphology/experimentation_script.sh +++ b/experiments/task-morphology/experimentation_script.sh @@ -1,13 +1,29 @@ #!/bin/bash +set -e +set -x -port=${1:-6000} -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_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_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_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_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 +#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=15000 +exp_name=Experiment_A +fitness=displacement +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 --n-cores 4 --port-start $((${start_port} + ($run*10))) --run $run +done \ No newline at end of file From 282b1815420e21233ebfe6502234e0077665c4d3 Mon Sep 17 00:00:00 2001 From: Matteo De Carlo Date: Fri, 19 Jun 2020 18:54:52 +0200 Subject: [PATCH 06/13] Update Experimentation script involving the fitness function --- experiments/task-morphology/experimentation_script.sh | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/experiments/task-morphology/experimentation_script.sh b/experiments/task-morphology/experimentation_script.sh index 546af6daee..1ff2d85084 100755 --- a/experiments/task-morphology/experimentation_script.sh +++ b/experiments/task-morphology/experimentation_script.sh @@ -16,14 +16,14 @@ set -x runs=10 runs_start=0 -start_port=15000 -exp_name=Experiment_A -fitness=displacement +start_port=13000 +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 --n-cores 4 --port-start $((${start_port} + ($run*10))) --run $run -done \ No newline at end of file + 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 4 --port-start $((${start_port} + ($run*10))) --run $run +done From 19a77d86939db71be7202410401b0908e1aefa06 Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Mon, 22 Jun 2020 01:08:15 -0700 Subject: [PATCH 07/13] Selection assert debug statement --- pyrevolve/evolution/selection.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/pyrevolve/evolution/selection.py b/pyrevolve/evolution/selection.py index 6316fa1e37..ed264e6c83 100644 --- a/pyrevolve/evolution/selection.py +++ b/pyrevolve/evolution/selection.py @@ -42,7 +42,10 @@ def multiple_selection(population: List[Individual], :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 From 51c90f494736ec46b277ad12a1886f6366c0739c Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Thu, 2 Jul 2020 09:28:20 -0700 Subject: [PATCH 08/13] Fixing generational image generation --- .../task-morphology/experimentation_script.sh | 3 +- experiments/task-morphology/manager_test.py | 172 ++++++++++++++++++ .../task-morphology/test_experiment.sh | 2 +- pyrevolve/experiment_management.py | 2 +- 4 files changed, 176 insertions(+), 3 deletions(-) create mode 100644 experiments/task-morphology/manager_test.py diff --git a/experiments/task-morphology/experimentation_script.sh b/experiments/task-morphology/experimentation_script.sh index 1ff2d85084..963d5c6a0e 100755 --- a/experiments/task-morphology/experimentation_script.sh +++ b/experiments/task-morphology/experimentation_script.sh @@ -17,6 +17,7 @@ set -x runs=10 runs_start=0 start_port=13000 +cores=4 exp_name=Experiment_B fitness=rotation log_suffix='' @@ -25,5 +26,5 @@ 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 4 --port-start $((${start_port} + ($run*10))) --run $run + 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_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 index f30c10a2e9..9ac62cad08 100755 --- a/experiments/task-morphology/test_experiment.sh +++ b/experiments/task-morphology/test_experiment.sh @@ -2,4 +2,4 @@ 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.py --port-start=$port --world worlds/plane.world ; done +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/pyrevolve/experiment_management.py b/pyrevolve/experiment_management.py index 8f9f84ce41..0d5904be19 100644 --- a/pyrevolve/experiment_management.py +++ b/pyrevolve/experiment_management.py @@ -205,7 +205,7 @@ def export_snapshots(self, individuals: List[Individual], gen_num: int) -> None: shutil.rmtree(path) os.mkdir(path) for ind in individuals: - self.export_phenotype_images(ind, os.path.join(self.experiment_folder, f'selectedpop_{gen_num}')) + self.export_phenotype_images(ind, path) logger.info(f'Exported snapshot {gen_num} with {len(individuals)} individuals') def export_snapshots_species(self, genus: Genus, gen_num: int) -> None: From d18527eb40bd7b20e343590d59df63e56738c87d Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Thu, 2 Jul 2020 20:04:17 +0200 Subject: [PATCH 09/13] Gazebo 10 and experimentation script --- cpprevolve/revolve/gazebo/CMakeLists.txt | 2 +- experiments/task-morphology/experimentation_script.sh | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/cpprevolve/revolve/gazebo/CMakeLists.txt b/cpprevolve/revolve/gazebo/CMakeLists.txt index e01a3970fa..a1e3241cf2 100644 --- a/cpprevolve/revolve/gazebo/CMakeLists.txt +++ b/cpprevolve/revolve/gazebo/CMakeLists.txt @@ -78,7 +78,7 @@ if (LOCAL_GAZEBO_DIR) NO_DEFAULT_PATH) message(WARNING "Using local Gazebo @ ${gazebo_DIR}") else() - find_package(gazebo 9 REQUIRED) + find_package(gazebo 10 REQUIRED) endif() include_directories(${GAZEBO_INCLUDE_DIRS}) link_directories(${GAZEBO_LIBRARY_DIRS}) diff --git a/experiments/task-morphology/experimentation_script.sh b/experiments/task-morphology/experimentation_script.sh index 1ff2d85084..77c04e61ce 100755 --- a/experiments/task-morphology/experimentation_script.sh +++ b/experiments/task-morphology/experimentation_script.sh @@ -17,8 +17,8 @@ set -x runs=10 runs_start=0 start_port=13000 -exp_name=Experiment_B -fitness=rotation +exp_name=Experiment_E +fitness=rotation_with_gait log_suffix='' manager=experiments/task-morphology/manager.py From cc23e67539f0837eb886d74fa8248cba3a302257 Mon Sep 17 00:00:00 2001 From: Daan Zeeuwe Date: Thu, 2 Jul 2020 13:37:30 -0700 Subject: [PATCH 10/13] Export list of identifiers of phenotypes. --- pyrevolve/experiment_management.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/pyrevolve/experiment_management.py b/pyrevolve/experiment_management.py index 0d5904be19..054c0f0b63 100644 --- a/pyrevolve/experiment_management.py +++ b/pyrevolve/experiment_management.py @@ -156,6 +156,11 @@ def export_behavior_measures(self, _id: int, measures: BehaviouralMeasurements) for key, val in measures.items(): 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: dirpath = dirpath if dirpath is not None \ else self._phenotype_images_folder @@ -204,8 +209,12 @@ def export_snapshots(self, individuals: List[Individual], gen_num: int) -> None: if os.path.exists(path): shutil.rmtree(path) os.mkdir(path) + + ids = [] for ind in 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 export_snapshots_species(self, genus: Genus, gen_num: int) -> None: From 43046e5b523f14cb1d301afdfa412f2900ee2f32 Mon Sep 17 00:00:00 2001 From: Matteo De Carlo Date: Tue, 28 Jul 2020 16:29:07 +0200 Subject: [PATCH 11/13] Added orientation vector to the status message. Update to python-protobuf library necessary. --- .../revolve/gazebo/msgs/robot_states.proto | 9 ++ 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 ++++--- requirements.txt | 2 +- tools/proto2python.sh | 2 +- 11 files changed, 266 insertions(+), 165 deletions(-) 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/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/requirements.txt b/requirements.txt index 1658ab8268..93ea2e0111 100644 --- a/requirements.txt +++ b/requirements.txt @@ -3,7 +3,7 @@ asyncio==3.4.3 futures==3.0.2 numpy>=1.9.2 PyYAML>=3.11 -protobuf>=3.0.0 +protobuf>=3.12.0 psutil==3.4.2 matplotlib pycairo>=1.18.0 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 From 5ef7feb78f2dcfe8e69bce5820c4010f28f68700 Mon Sep 17 00:00:00 2001 From: Matteo De Carlo Date: Tue, 28 Jul 2020 16:41:49 +0200 Subject: [PATCH 12/13] Fixes Issue #103 Issue #103 : Simulation time not calculated correctly in python --- pyrevolve/util/time.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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)) From 3a27740e29ec303a789485d644df5058278d3be7 Mon Sep 17 00:00:00 2001 From: Matteo De Carlo Date: Wed, 29 Jul 2020 14:12:25 +0200 Subject: [PATCH 13/13] Implemented new panoramic rotation fitness function also added charts to visualize single robot tests --- .../revolve/gazebo/plugin/WorldController.cpp | 154 ++++++++++++++- .../revolve/gazebo/plugin/WorldController.h | 3 + experiments/task-morphology/manager.py | 1 + pyrevolve/angle/manage/robotmanager.py | 16 +- pyrevolve/config.py | 16 +- pyrevolve/data_analisys/visualize_robot.py | 187 +++++++++++++++++- pyrevolve/evolution/fitness.py | 72 ++++++- .../evolution/population/population_config.py | 7 +- .../speciation/population_speciated_config.py | 9 +- pyrevolve/util/supervisor/simulator_queue.py | 4 +- 10 files changed, 440 insertions(+), 29 deletions(-) diff --git a/cpprevolve/revolve/gazebo/plugin/WorldController.cpp b/cpprevolve/revolve/gazebo/plugin/WorldController.cpp index 7b55c142a0..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(); 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/experiments/task-morphology/manager.py b/experiments/task-morphology/manager.py index 0edf507100..c7620c4515 100644 --- a/experiments/task-morphology/manager.py +++ b/experiments/task-morphology/manager.py @@ -125,6 +125,7 @@ async def run(): 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, diff --git a/pyrevolve/angle/manage/robotmanager.py b/pyrevolve/angle/manage/robotmanager.py index 769c4fa311..034404d4d7 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 @@ -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/config.py b/pyrevolve/config.py index eba79dde9b..0d81e4bce9 100644 --- a/pyrevolve/config.py +++ b/pyrevolve/config.py @@ -98,6 +98,12 @@ def str_to_address(v): "Loads a yaml robot." ) +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, @@ -142,12 +148,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 09a0b7879d..93aec01c99 100644 --- a/pyrevolve/data_analisys/visualize_robot.py +++ b/pyrevolve/data_analisys/visualize_robot.py @@ -2,16 +2,114 @@ import logging import sys import os +import math +import numpy as np 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), @@ -47,16 +145,85 @@ async def test_robot_run(robot_file_path: str): robot.update_substrate() robot.save_file(f'{robot_file_path}.sdf', conf_type='sdf') - await connection.pause(False) + 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 + 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 59bc33bc8c..f3b481b72a 100644 --- a/pyrevolve/evolution/fitness.py +++ b/pyrevolve/evolution/fitness.py @@ -2,13 +2,15 @@ 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 - from pyrevolve.SDF.math import Vector3 def _distance_flat_plane(pos1: Vector3, pos2: Vector3): @@ -106,10 +108,6 @@ def rotation(robot_manager: RobotManager, _robot: RevolveBot, factor_orien_ds: f assert len(robot_manager._orientations) == len(robot_manager._positions) for i in range(1, len(robot_manager._orientations)): - # TODO why are we ignoring time here? is it a good thing? - - pos_i_1: Vector3 = robot_manager._positions[i - 1] - pos_i: Vector3 = robot_manager._positions[i] rot_i_1 = robot_manager._orientations[i - 1] rot_i = robot_manager._orientations[i] @@ -129,6 +127,70 @@ def rotation(robot_manager: RobotManager, _robot: RevolveBot, factor_orien_ds: f 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) diff --git a/pyrevolve/evolution/population/population_config.py b/pyrevolve/evolution/population/population_config.py index afe82694c1..f5714c4ef1 100644 --- a/pyrevolve/evolution/population/population_config.py +++ b/pyrevolve/evolution/population/population_config.py @@ -29,7 +29,8 @@ def __init__(self, evaluation_time: float, experiment_name: str, experiment_management, - offspring_size: Optional[int] = None): + offspring_size: Optional[int] = None, + grace_time: float = 0.0): """ Creates a PopulationConfig object that sets the particular configuration for the population @@ -48,10 +49,11 @@ def __init__(self, :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 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 """ self.population_size = population_size self.genotype_constructor = genotype_constructor @@ -66,6 +68,7 @@ def __init__(self, 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 diff --git a/pyrevolve/evolution/speciation/population_speciated_config.py b/pyrevolve/evolution/speciation/population_speciated_config.py index 6c47105a4b..34ccac032f 100644 --- a/pyrevolve/evolution/speciation/population_speciated_config.py +++ b/pyrevolve/evolution/speciation/population_speciated_config.py @@ -38,7 +38,8 @@ def __init__(self, old_age_threshold: int = 30, old_age_fitness_penalty: float = 0.5, species_max_stagnation: int = 50, - offspring_size: Optional[int] = None): + offspring_size: Optional[int] = None, + grace_time: float = 0.0): """ Creates a PopulationSpeciatedConfig object that sets the particular configuration for the population with species @@ -57,7 +58,7 @@ def __init__(self, :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 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 @@ -71,6 +72,7 @@ def __init__(self, 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, @@ -87,7 +89,8 @@ def __init__(self, evaluation_time, experiment_name, experiment_management, - offspring_size) + offspring_size, + grace_time) 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 diff --git a/pyrevolve/util/supervisor/simulator_queue.py b/pyrevolve/util/supervisor/simulator_queue.py index 6c89aeca48..be91147f45 100644 --- a/pyrevolve/util/supervisor/simulator_queue.py +++ b/pyrevolve/util/supervisor/simulator_queue.py @@ -159,8 +159,8 @@ async def _evaluate_robot(self, simulator_connection, robot, conf): robot_fitness = None return robot_fitness, None else: - # Change this `max_age` from the command line parameters (--evalution-time) - max_age = conf.evaluation_time + # 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.phenotype.simulation_boundaries is not None: pose_z -= robot.phenotype.simulation_boundaries.min.z