From a6901d330d52b48e448de9e8879a40314a5dff46 Mon Sep 17 00:00:00 2001 From: Aart Stuurman Date: Thu, 6 Jan 2022 13:23:39 +0100 Subject: [PATCH] 0.0.0 alpha1 (#43) * Initial commit with basic version of EA loop (#2) Contains the basic loop, but very rough. Everything needs cleanup and API improvements. Isaac Gym simulation environment. Modular robot framework that can be used to create robots for the simulation environment. An optimization framework for evolutionary processes. A database system to store results and provide recovery in case of failure. * Expose sim params in isaac gym local runner * For isaac runner, add a bunch of todos. Make initial pos and rot configurable. * Review apis (#10) * For modular robots: remove slot, rotation is now directly part of Module * Remove back from BRICK module, because that's where it's attached so it can't be filled * Add brain 'cpg_random', which randomly initialized cpg weights * Remove old file * Rework an error message in ea selection multiple_unique * Added TODOs to database that transactions still need to be implmeented * Remove some more old BACK from BRICK Co-authored-by: Aart Stuurman * Add some missing packages to setup.py * Correct values in to_urdf * Fix bug in ea were population and new individuals were swapped in a function call * Add headless for isaac runner * Store individuals seperate from generation (#19) * Added individual class to ea * Individuals stored seperately from generations. generations now use references * Individual members now individually serialized * Create Serializable class. Use it to serialize Genotype and Evaluation nicer * Add isaac as a prereq for isaac env * Individuals now also list parents Co-authored-by: Aart Stuurman * Improve cpg performance (#20) Co-authored-by: Aart Stuurman * In EA, rename evaluation to fitness. (#24) Co-authored-by: Aart Stuurman * 22 simulation history (#25) * Can set update frequency. * Implement sampling frequency * Add getting path from AnyView * Fixed bug in DictView where dict 'contains' was not properly programmed. * EA optimizer now makes space in the database were the evaluator can do its work. * Save individual rng objects for each generation * Make physics State serializable Co-authored-by: Aart Stuurman * Emergency fix: fix incorrect assertion in EA * 21 analysis - intermediate merge (#29) * Add analyzer to ea optimization * Fix bug in ea analyzer where Generations did not always create Generation correctly. * Improve serialization of cppnneat genotypes * Add plot script for ea fitness * Add modular robot rerunner. Update ea plotter with max min avg * add prepare_sim to isaac runner so gpu can be used Co-authored-by: Aart Stuurman * Fix py.typed. Fix many mypy errors - intermediate merge for #3 (#30) Co-authored-by: Aart Stuurman * 28 improve database performance (#32) * Completely reworked db interface and inner workings. Db implementations can now be significantly faster. * Removed all existing db implementations * Added SQLite implementation * Updated all existing code to use the new db interface Co-authored-by: Aart Stuurman * 15 isaac leak (#34) * Isaac now runs in a subprocess every batch Co-authored-by: Aart Stuurman * Removed asyncinit dependency (#35) Co-authored-by: Aart Stuurman * Add aabb calculation for actor. Can be used to know how far to put robots above ground. (#37) Co-authored-by: Aart Stuurman * 27 robot friction (#39) * Add static friction to robot rigid bodies * Set friction paremeters to be arbitrarily chosen values that look good * Remove some old imports Co-authored-by: Aart Stuurman * for cppnneat brain genotype set output func to signed sign (#41) Co-authored-by: Aart Stuurman * Add modular robot size measurement and number of bricks to modular robot (#42) Co-authored-by: Aart Stuurman * Add support for multiple runs to plt_ea_fitness script. Co-authored-by: Aart Stuurman --- .gitignore | 19 +- core/revolve2/core/__init__.py | 0 core/revolve2/core/analysis/__init__.py | 0 .../core/analysis/modular_robot_rerunner.py | 40 ++ .../revolve2/core/analysis/plot_ea_fitness.py | 95 ++++ core/revolve2/core/database/__init__.py | 8 + core/revolve2/core/database/database.py | 21 + core/revolve2/core/database/database_error.py | 2 + core/revolve2/core/database/list.py | 71 +++ core/revolve2/core/database/list_impl.py | 22 + core/revolve2/core/database/node.py | 50 ++ core/revolve2/core/database/node_impl.py | 16 + core/revolve2/core/database/object.py | 32 ++ .../core/database/serialize/__init__.py | 3 + .../core/database/serialize/serializable.py | 16 + .../core/database/serialize/serialize.py | 23 + .../database/serialize/serialize_error.py | 2 + .../revolve2/core/database/sqlite/__init__.py | 2 + .../revolve2/core/database/sqlite/database.py | 56 ++ .../core/database/sqlite/list_impl.py | 71 +++ .../core/database/sqlite/node_impl.py | 143 +++++ core/revolve2/core/database/sqlite/schema.py | 36 ++ .../core/database/sqlite/transaction.py | 11 + core/revolve2/core/database/static_data.py | 27 + core/revolve2/core/database/transaction.py | 2 + core/revolve2/core/database/uninitialized.py | 2 + core/revolve2/core/modular_robot/__init__.py | 10 + .../core/modular_robot/active_hinge.py | 34 ++ core/revolve2/core/modular_robot/analyzer.py | 159 ++++++ .../core/modular_robot/analyzer_module.py | 110 ++++ core/revolve2/core/modular_robot/body.py | 18 + core/revolve2/core/modular_robot/brain.py | 31 ++ .../core/modular_robot/brains/__init__.py | 2 + .../revolve2/core/modular_robot/brains/cpg.py | 89 ++++ .../core/modular_robot/brains/cpg_random.py | 22 + core/revolve2/core/modular_robot/brick.py | 56 ++ core/revolve2/core/modular_robot/core.py | 67 +++ .../core/modular_robot/modular_robot.py | 35 ++ core/revolve2/core/modular_robot/module.py | 44 ++ .../revolve2/core/modular_robot/serialized.py | 3 + core/revolve2/core/modular_robot/to_actor.py | 324 ++++++++++++ core/revolve2/core/optimization/__init__.py | 0 .../revolve2/core/optimization/ea/__init__.py | 3 + .../revolve2/core/optimization/ea/analyzer.py | 211 ++++++++ .../optimization/ea/evolutionary_optimizer.py | 497 ++++++++++++++++++ .../core/optimization/ea/individual.py | 76 +++ .../optimization/ea/modular_robot/__init__.py | 3 + .../ea/modular_robot/body_genotype.py | 9 + .../ea/modular_robot/bodybrain_genotype.py | 25 + .../ea/modular_robot/brain_genotype.py | 10 + .../ea/population_management/__init__.py | 2 + .../ea/population_management/generational.py | 27 + .../ea/population_management/steady_state.py | 25 + .../optimization/ea/selection/__init__.py | 2 + .../ea/selection/multiple_unique.py | 32 ++ .../optimization/ea/selection/tournament.py | 24 + core/revolve2/core/physics/actor/__init__.py | 5 + core/revolve2/core/physics/actor/actor.py | 144 +++++ .../core/physics/actor/bounding_box.py | 9 + core/revolve2/core/physics/actor/collision.py | 12 + core/revolve2/core/physics/actor/joint.py | 15 + .../revolve2/core/physics/actor/rigid_body.py | 57 ++ .../core/physics/actor/sdf/__init__.py | 1 + .../revolve2/core/physics/actor/sdf/to_sdf.py | 168 ++++++ .../core/physics/actor/urdf/__init__.py | 1 + .../core/physics/actor/urdf/to_urdf.py | 177 +++++++ core/revolve2/core/physics/actor/visual.py | 13 + .../revolve2/core/physics/control/__init__.py | 1 + .../core/physics/control/actor_controller.py | 12 + .../control/actor_controllers/__init__.py | 1 + .../physics/control/actor_controllers/cpg.py | 45 ++ core/revolve2/core/physics/env/__init__.py | 6 + .../core/physics/env/actor_control.py | 14 + core/revolve2/core/physics/env/batch.py | 20 + core/revolve2/core/physics/env/environment.py | 9 + core/revolve2/core/physics/env/posed_actor.py | 12 + core/revolve2/core/physics/env/runner.py | 11 + core/revolve2/core/physics/env/state.py | 57 ++ core/revolve2/py.typed | 0 core/setup.py | 21 + envs/isaacgym/py.typed | 0 .../revolve2/envs/isaacgym/__init__.py | 1 + .../revolve2/envs/isaacgym/local_runner.py | 302 +++++++++++ envs/isaacgym/setup.py | 13 + genotypes/cppnneat/py.typed | 0 .../revolve2/genotypes/cppnneat/__init__.py | 2 + .../genotypes/cppnneat/body_genotype_v1.py | 214 ++++++++ .../genotypes/cppnneat/bodybrain_base.py | 83 +++ .../genotypes/cppnneat/brain_cpg_v1.py | 68 +++ .../cppnneat/brain_genotype_cpg_v1.py | 51 ++ genotypes/cppnneat/setup.py | 13 + 91 files changed, 4270 insertions(+), 8 deletions(-) create mode 100644 core/revolve2/core/__init__.py create mode 100644 core/revolve2/core/analysis/__init__.py create mode 100644 core/revolve2/core/analysis/modular_robot_rerunner.py create mode 100644 core/revolve2/core/analysis/plot_ea_fitness.py create mode 100644 core/revolve2/core/database/__init__.py create mode 100644 core/revolve2/core/database/database.py create mode 100644 core/revolve2/core/database/database_error.py create mode 100644 core/revolve2/core/database/list.py create mode 100644 core/revolve2/core/database/list_impl.py create mode 100644 core/revolve2/core/database/node.py create mode 100644 core/revolve2/core/database/node_impl.py create mode 100644 core/revolve2/core/database/object.py create mode 100644 core/revolve2/core/database/serialize/__init__.py create mode 100644 core/revolve2/core/database/serialize/serializable.py create mode 100644 core/revolve2/core/database/serialize/serialize.py create mode 100644 core/revolve2/core/database/serialize/serialize_error.py create mode 100644 core/revolve2/core/database/sqlite/__init__.py create mode 100644 core/revolve2/core/database/sqlite/database.py create mode 100644 core/revolve2/core/database/sqlite/list_impl.py create mode 100644 core/revolve2/core/database/sqlite/node_impl.py create mode 100644 core/revolve2/core/database/sqlite/schema.py create mode 100644 core/revolve2/core/database/sqlite/transaction.py create mode 100644 core/revolve2/core/database/static_data.py create mode 100644 core/revolve2/core/database/transaction.py create mode 100644 core/revolve2/core/database/uninitialized.py create mode 100644 core/revolve2/core/modular_robot/__init__.py create mode 100644 core/revolve2/core/modular_robot/active_hinge.py create mode 100644 core/revolve2/core/modular_robot/analyzer.py create mode 100644 core/revolve2/core/modular_robot/analyzer_module.py create mode 100644 core/revolve2/core/modular_robot/body.py create mode 100644 core/revolve2/core/modular_robot/brain.py create mode 100644 core/revolve2/core/modular_robot/brains/__init__.py create mode 100644 core/revolve2/core/modular_robot/brains/cpg.py create mode 100644 core/revolve2/core/modular_robot/brains/cpg_random.py create mode 100644 core/revolve2/core/modular_robot/brick.py create mode 100644 core/revolve2/core/modular_robot/core.py create mode 100644 core/revolve2/core/modular_robot/modular_robot.py create mode 100644 core/revolve2/core/modular_robot/module.py create mode 100644 core/revolve2/core/modular_robot/serialized.py create mode 100644 core/revolve2/core/modular_robot/to_actor.py create mode 100644 core/revolve2/core/optimization/__init__.py create mode 100644 core/revolve2/core/optimization/ea/__init__.py create mode 100644 core/revolve2/core/optimization/ea/analyzer.py create mode 100644 core/revolve2/core/optimization/ea/evolutionary_optimizer.py create mode 100644 core/revolve2/core/optimization/ea/individual.py create mode 100644 core/revolve2/core/optimization/ea/modular_robot/__init__.py create mode 100644 core/revolve2/core/optimization/ea/modular_robot/body_genotype.py create mode 100644 core/revolve2/core/optimization/ea/modular_robot/bodybrain_genotype.py create mode 100644 core/revolve2/core/optimization/ea/modular_robot/brain_genotype.py create mode 100644 core/revolve2/core/optimization/ea/population_management/__init__.py create mode 100644 core/revolve2/core/optimization/ea/population_management/generational.py create mode 100644 core/revolve2/core/optimization/ea/population_management/steady_state.py create mode 100644 core/revolve2/core/optimization/ea/selection/__init__.py create mode 100644 core/revolve2/core/optimization/ea/selection/multiple_unique.py create mode 100644 core/revolve2/core/optimization/ea/selection/tournament.py create mode 100644 core/revolve2/core/physics/actor/__init__.py create mode 100644 core/revolve2/core/physics/actor/actor.py create mode 100644 core/revolve2/core/physics/actor/bounding_box.py create mode 100644 core/revolve2/core/physics/actor/collision.py create mode 100644 core/revolve2/core/physics/actor/joint.py create mode 100644 core/revolve2/core/physics/actor/rigid_body.py create mode 100644 core/revolve2/core/physics/actor/sdf/__init__.py create mode 100644 core/revolve2/core/physics/actor/sdf/to_sdf.py create mode 100644 core/revolve2/core/physics/actor/urdf/__init__.py create mode 100644 core/revolve2/core/physics/actor/urdf/to_urdf.py create mode 100644 core/revolve2/core/physics/actor/visual.py create mode 100644 core/revolve2/core/physics/control/__init__.py create mode 100644 core/revolve2/core/physics/control/actor_controller.py create mode 100644 core/revolve2/core/physics/control/actor_controllers/__init__.py create mode 100644 core/revolve2/core/physics/control/actor_controllers/cpg.py create mode 100644 core/revolve2/core/physics/env/__init__.py create mode 100644 core/revolve2/core/physics/env/actor_control.py create mode 100644 core/revolve2/core/physics/env/batch.py create mode 100644 core/revolve2/core/physics/env/environment.py create mode 100644 core/revolve2/core/physics/env/posed_actor.py create mode 100644 core/revolve2/core/physics/env/runner.py create mode 100644 core/revolve2/core/physics/env/state.py create mode 100644 core/revolve2/py.typed create mode 100644 core/setup.py create mode 100644 envs/isaacgym/py.typed create mode 100644 envs/isaacgym/revolve2/envs/isaacgym/__init__.py create mode 100644 envs/isaacgym/revolve2/envs/isaacgym/local_runner.py create mode 100644 envs/isaacgym/setup.py create mode 100644 genotypes/cppnneat/py.typed create mode 100644 genotypes/cppnneat/revolve2/genotypes/cppnneat/__init__.py create mode 100644 genotypes/cppnneat/revolve2/genotypes/cppnneat/body_genotype_v1.py create mode 100644 genotypes/cppnneat/revolve2/genotypes/cppnneat/bodybrain_base.py create mode 100644 genotypes/cppnneat/revolve2/genotypes/cppnneat/brain_cpg_v1.py create mode 100644 genotypes/cppnneat/revolve2/genotypes/cppnneat/brain_genotype_cpg_v1.py create mode 100644 genotypes/cppnneat/setup.py diff --git a/.gitignore b/.gitignore index b6e47617d..9db88b59f 100644 --- a/.gitignore +++ b/.gitignore @@ -20,7 +20,6 @@ parts/ sdist/ var/ wheels/ -pip-wheel-metadata/ share/python-wheels/ *.egg-info/ .installed.cfg @@ -50,6 +49,7 @@ coverage.xml *.py,cover .hypothesis/ .pytest_cache/ +cover/ # Translations *.mo @@ -72,6 +72,7 @@ instance/ docs/_build/ # PyBuilder +.pybuilder/ target/ # Jupyter Notebook @@ -82,7 +83,9 @@ profile_default/ ipython_config.py # pyenv -.python-version +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version # pipenv # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. @@ -103,12 +106,6 @@ celerybeat.pid # Environments .env -.venv -env/ -venv/ -ENV/ -env.bak/ -venv.bak/ # Spyder project settings .spyderproject @@ -127,3 +124,9 @@ dmypy.json # Pyre type checker .pyre/ + +# pytype static type analyzer +.pytype/ + +# Cython debug symbols +cython_debug/ diff --git a/core/revolve2/core/__init__.py b/core/revolve2/core/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/core/revolve2/core/analysis/__init__.py b/core/revolve2/core/analysis/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/core/revolve2/core/analysis/modular_robot_rerunner.py b/core/revolve2/core/analysis/modular_robot_rerunner.py new file mode 100644 index 000000000..d4c961995 --- /dev/null +++ b/core/revolve2/core/analysis/modular_robot_rerunner.py @@ -0,0 +1,40 @@ +""" +Rerun(watch) a modular robot in isaac gym. +""" + +from pyrr import Quaternion, Vector3 +from revolve2.core.modular_robot import ModularRobot +from revolve2.core.physics.control import ActorController +from revolve2.core.physics.env import ActorControl, Batch, Environment, PosedActor +from revolve2.envs.isaacgym import LocalRunner + + +class ModularRobotRerunner: + _controller: ActorController + + async def rerun(self, robot: ModularRobot, control_frequency: float) -> None: + batch = Batch( + simulation_time=1000000, + sampling_frequency=0.0001, + control_frequency=control_frequency, + control=self._control, + ) + + actor, self._controller = robot.make_actor_and_controller() + + env = Environment() + env.actors.append(PosedActor(actor, Vector3([0.0, 0.0, 0.1]), Quaternion())) + batch.environments.append(env) + + runner = LocalRunner(LocalRunner.SimParams()) + await runner.run_batch(batch) + + def _control(self, dt: float, control: ActorControl) -> None: + self._controller.step(dt) + control.set_dof_targets(0, 0, self._controller.get_dof_targets()) + + +if __name__ == "__main__": + print( + "This file cannot be ran as a script. Import it and use the contained classes instead." + ) diff --git a/core/revolve2/core/analysis/plot_ea_fitness.py b/core/revolve2/core/analysis/plot_ea_fitness.py new file mode 100644 index 000000000..d9a6a84d5 --- /dev/null +++ b/core/revolve2/core/analysis/plot_ea_fitness.py @@ -0,0 +1,95 @@ +""" +Plot average, min, and max fitness over generations, using the results of the evolutionary optimizer. +Assumes fitness is a float and database is files. +""" + +import argparse +from statistics import mean + +import matplotlib.pyplot as plt +from revolve2.core.database.sqlite import Database +from revolve2.core.optimization.ea import Analyzer as EaAnalyzer +from revolve2.core.optimization.ea.analyzer import Generation + + +async def main() -> None: + parser = argparse.ArgumentParser() + parser.add_argument( + "databases", + nargs="+", + help="The databases to make plots for. If more then one database(multiple runs of the same experiment) is provided, their respective plots are averaged into one single plot.", + ) + args = parser.parse_args() + + max_fitnesses = [] + min_fitnesses = [] + mean_fitnesses = [] + + for db_file in args.databases: + db = await Database.create(db_file) + with db.begin_transaction() as txn: + analyzer = EaAnalyzer(txn, db.root) + + max_fitness = [ + max( + [ + analyzer.individuals[individual].fitness + for individual in generation + ], + ) + for generation in analyzer.generations + ] + + min_fitness = [ + min( + [ + analyzer.individuals[individual].fitness + for individual in generation + ], + ) + for generation in analyzer.generations + ] + + mean_fitness = [ + mean( + [ + analyzer.individuals[individual].fitness + for individual in generation + ], + ) + for generation in analyzer.generations + ] + max_fitnesses.append(max_fitness) + min_fitnesses.append(min_fitness) + mean_fitnesses.append(mean_fitness) + + assert all( + len(max_fitnesses[0]) == len(x) for x in max_fitnesses + ), "Not all databases have an equal amount of generations." + + mean_max_fitness = [mean(x) for x in list(map(list, zip(*max_fitnesses)))] + mean_min_fitness = [mean(x) for x in list(map(list, zip(*min_fitnesses)))] + mean_avg_fitness = [mean(x) for x in list(map(list, zip(*mean_fitnesses)))] + + x = [i for i in range(len(mean_max_fitness))] + + fig, ax = plt.subplots() + ax.plot( + x, + mean_max_fitness, + ) + ax.plot( + x, + mean_min_fitness, + ) + ax.plot( + x, + mean_avg_fitness, + ) + plt.show() + + +if __name__ == "__main__": + import asyncio + + asyncio.run(main()) diff --git a/core/revolve2/core/database/__init__.py b/core/revolve2/core/database/__init__.py new file mode 100644 index 000000000..f9896f5dc --- /dev/null +++ b/core/revolve2/core/database/__init__.py @@ -0,0 +1,8 @@ +from .database import Database +from .database_error import DatabaseError +from .list import List +from .node import Node +from .object import Object +from .static_data import StaticData, is_static_data +from .transaction import Transaction +from .uninitialized import Uninitialized diff --git a/core/revolve2/core/database/database.py b/core/revolve2/core/database/database.py new file mode 100644 index 000000000..df31e572e --- /dev/null +++ b/core/revolve2/core/database/database.py @@ -0,0 +1,21 @@ +from abc import ABC, abstractmethod + +from .node import Node +from .transaction import Transaction + + +class Database(ABC): + @abstractmethod + def begin_transaction(self) -> Transaction: + """ + Begin a transaction context. + """ + pass + + @property + @abstractmethod + def root(self) -> Node: + """ + Get the root node of the database. + """ + pass diff --git a/core/revolve2/core/database/database_error.py b/core/revolve2/core/database/database_error.py new file mode 100644 index 000000000..d4a509f45 --- /dev/null +++ b/core/revolve2/core/database/database_error.py @@ -0,0 +1,2 @@ +class DatabaseError(Exception): + pass diff --git a/core/revolve2/core/database/list.py b/core/revolve2/core/database/list.py new file mode 100644 index 000000000..1f512c401 --- /dev/null +++ b/core/revolve2/core/database/list.py @@ -0,0 +1,71 @@ +from typing import Optional + +from .database_error import DatabaseError +from .list_impl import ListImpl +from .node import Node +from .transaction import Transaction + + +class List: + _impl: Optional[ListImpl] + + def __init__(self, impl: ListImpl = None): + self._impl = impl + + @property + def is_stub(self) -> bool: + """ + If list is not yet linked to the database but a stub created by the user. + """ + return self._impl is None + + def get_or_append(self, txn: Transaction, index: int) -> Node: + """ + Get the item at the given index, or if the list is one short, + append to the list and return the new node. + If the list is more than one too short or the index is not the last index in the list, + DatabaseError is thrown. + """ + if self.is_stub: + raise DatabaseError( + "List not usable yet. It is a stub created by the user that has not yet been linked with the database." + ) + + return self._impl.get_or_append(txn, index) + + def append(self, txn: Transaction) -> Node: + """ + Append a new node to the list and return it. + """ + if self.is_stub: + raise DatabaseError( + "List not usable yet. It is a stub created by the user that has not yet been linked with the database." + ) + + return self._impl.append(txn) + + def get(self, txn: Transaction, index: int) -> Node: + """ + Get the item at the given index. + :raises: DatabaseError if out of bounds + """ + if self.is_stub: + raise DatabaseError( + "List not usable yet. It is a stub created by the user that has not yet been linked with the database." + ) + + return self._impl.get(txn, index) + + def len(self, txn: Transaction) -> int: + """ + Get the length of the list. + """ + if self.is_stub: + raise DatabaseError( + "List not usable yet. It is a stub created by the user that has not yet been linked with the database." + ) + + return self._impl.len(txn) + + def _set_impl(self, impl: ListImpl) -> None: + self._impl = impl diff --git a/core/revolve2/core/database/list_impl.py b/core/revolve2/core/database/list_impl.py new file mode 100644 index 000000000..08f7b57ce --- /dev/null +++ b/core/revolve2/core/database/list_impl.py @@ -0,0 +1,22 @@ +from abc import ABC, abstractmethod + +from .node import Node +from .transaction import Transaction + + +class ListImpl(ABC): + @abstractmethod + def get_or_append(self, txn: Transaction, index: int) -> Node: + pass + + @abstractmethod + def append(self, txn: Transaction) -> Node: + pass + + @abstractmethod + def get(self, txn: Transaction, index: int) -> Node: + pass + + @abstractmethod + def len(self, txn: Transaction) -> int: + pass diff --git a/core/revolve2/core/database/node.py b/core/revolve2/core/database/node.py new file mode 100644 index 000000000..1a6f80dc6 --- /dev/null +++ b/core/revolve2/core/database/node.py @@ -0,0 +1,50 @@ +from typing import Optional, Union + +from .database_error import DatabaseError +from .node_impl import NodeImpl +from .object import Object +from .transaction import Transaction +from .uninitialized import Uninitialized + + +class Node: + """ + Represents a node in a database. + Can be either an object or unitialized. + """ + + _impl: Optional[NodeImpl] + + def __init__(self, impl: NodeImpl = None): + self._impl = impl + + @property + def is_stub(self) -> bool: + """ + If node is not yet linked to the database but a stub created by the user. + """ + return self._impl is None + + def get_object(self, txn: Transaction) -> Union[Object, Uninitialized]: + """ + Read the underlying object from the database. + """ + if self.is_stub: + raise DatabaseError() + + return self._impl.get_object(txn) + + def set_object(self, txn: Transaction, object: Object) -> None: + """ + Set the underlying object in the database. + If object is not uninitialized, raises DatabaseError. + """ + if self.is_stub: + raise DatabaseError( + "Node not usable yet. It is a stub created by the user that has not yet been linked with the database." + ) + + return self._impl.set_object(txn, object) + + def _set_impl(self, impl: NodeImpl) -> None: + self._impl = impl diff --git a/core/revolve2/core/database/node_impl.py b/core/revolve2/core/database/node_impl.py new file mode 100644 index 000000000..dd189b9d2 --- /dev/null +++ b/core/revolve2/core/database/node_impl.py @@ -0,0 +1,16 @@ +from abc import ABC, abstractmethod +from typing import Union + +from .object import Object +from .transaction import Transaction +from .uninitialized import Uninitialized + + +class NodeImpl(ABC): + @abstractmethod + def get_object(self, txn: Transaction) -> Union[Object, Uninitialized]: + pass + + @abstractmethod + def set_object(self, txn: Transaction, object: Object) -> None: + pass diff --git a/core/revolve2/core/database/object.py b/core/revolve2/core/database/object.py new file mode 100644 index 000000000..1dfc31f3f --- /dev/null +++ b/core/revolve2/core/database/object.py @@ -0,0 +1,32 @@ +from typing import TYPE_CHECKING, Any, Dict, List, Union + +from .static_data import StaticData, cast, is_static_data + +if TYPE_CHECKING: + from .list import List as DbList + from .node import Node + +Object = Union[List["Object"], Dict[str, "Object"], StaticData, "Node", "DbList"] + + +def is_object(to_check: Any) -> bool: + from .list import List as DbList + from .node import Node + + if isinstance(to_check, DbList): + return True + elif isinstance(to_check, Node): + return True + elif isinstance(to_check, list): + return all([is_object(child) for child in to_check]) + elif isinstance(to_check, dict): + return all( + [ + isinstance(key, str) and is_object(val) + for key, val in cast(Dict[Any, Any], to_check).items() + ] + ) + elif is_static_data(to_check): + return True + else: + return False diff --git a/core/revolve2/core/database/serialize/__init__.py b/core/revolve2/core/database/serialize/__init__.py new file mode 100644 index 000000000..ad64119bf --- /dev/null +++ b/core/revolve2/core/database/serialize/__init__.py @@ -0,0 +1,3 @@ +from .serializable import Serializable +from .serialize import deserialize, serialize +from .serialize_error import SerializeError diff --git a/core/revolve2/core/database/serialize/serializable.py b/core/revolve2/core/database/serialize/serializable.py new file mode 100644 index 000000000..51f555a97 --- /dev/null +++ b/core/revolve2/core/database/serialize/serializable.py @@ -0,0 +1,16 @@ +from __future__ import annotations + +from abc import ABC, abstractclassmethod, abstractmethod + +from ..static_data import StaticData + + +class Serializable(ABC): + @abstractmethod + def serialize(self) -> StaticData: + pass + + @abstractclassmethod + def deserialize(cls, data: StaticData) -> Serializable: + # This should return an instance of the class implementing this interface + pass diff --git a/core/revolve2/core/database/serialize/serialize.py b/core/revolve2/core/database/serialize/serialize.py new file mode 100644 index 000000000..a7e688803 --- /dev/null +++ b/core/revolve2/core/database/serialize/serialize.py @@ -0,0 +1,23 @@ +from typing import Type, Union + +from ..static_data import StaticData, is_static_data +from .serializable import Serializable +from .serialize_error import SerializeError + + +def serialize(to_serialize: Union[Serializable, StaticData]) -> StaticData: + if isinstance(to_serialize, Serializable): + return to_serialize.serialize() + elif is_static_data(to_serialize): + return to_serialize + else: + raise SerializeError() + + +def deserialize(data: StaticData, type: Type) -> Union[Serializable, StaticData]: + if issubclass(type, Serializable): + return type.deserialize(data) + elif is_static_data(data): + return data + else: + raise SerializeError() diff --git a/core/revolve2/core/database/serialize/serialize_error.py b/core/revolve2/core/database/serialize/serialize_error.py new file mode 100644 index 000000000..da8d96f2e --- /dev/null +++ b/core/revolve2/core/database/serialize/serialize_error.py @@ -0,0 +1,2 @@ +class SerializeError(Exception): + pass diff --git a/core/revolve2/core/database/sqlite/__init__.py b/core/revolve2/core/database/sqlite/__init__.py new file mode 100644 index 000000000..4ff824493 --- /dev/null +++ b/core/revolve2/core/database/sqlite/__init__.py @@ -0,0 +1,2 @@ +from .database import Database +from .transaction import Transaction diff --git a/core/revolve2/core/database/sqlite/database.py b/core/revolve2/core/database/sqlite/database.py new file mode 100644 index 000000000..13c1d64d8 --- /dev/null +++ b/core/revolve2/core/database/sqlite/database.py @@ -0,0 +1,56 @@ +from __future__ import annotations + +import contextlib +import os + +from sqlalchemy import create_engine +from sqlalchemy.orm import sessionmaker + +from ..database import Database as DatabaseBase +from ..node import Node +from .node_impl import NodeImpl +from .schema import Base, DbNode +from .transaction import Transaction + + +class Database(DatabaseBase): + __create_key = object() + + _db_sessionmaker: sessionmaker + + def __init__(self, create_key) -> None: + if create_key is not self.__create_key: + raise ValueError( + "Sqlite database can only be created through its factory function." + ) + + async def create(root_directory: str) -> Database: + self = Database(Database.__create_key) + + if not os.path.isdir(root_directory): + os.makedirs(root_directory, exist_ok=True) + + engine = create_engine(f"sqlite:///{root_directory}/db.sqlite") + Base.metadata.create_all(engine) + self._db_sessionmaker = sessionmaker(bind=engine) + + self._create_root_node() + + return self + + @contextlib.contextmanager + def begin_transaction(self) -> Transaction: + session = self._db_sessionmaker() + yield Transaction(session) + session.commit() + session.close() + + @property + def root(self) -> Node: + return Node(NodeImpl(0)) + + def _create_root_node(self) -> None: + + with self.begin_transaction() as ses: + if ses._session.query(DbNode).count() == 0: + ses._session.add(DbNode(0, None, 0)) diff --git a/core/revolve2/core/database/sqlite/list_impl.py b/core/revolve2/core/database/sqlite/list_impl.py new file mode 100644 index 000000000..86011d8da --- /dev/null +++ b/core/revolve2/core/database/sqlite/list_impl.py @@ -0,0 +1,71 @@ +from revolve2.core.database import DatabaseError + +from ..list_impl import ListImpl as ListImplBase +from ..node import Node +from ..transaction import Transaction as TransactionBase +from .schema import DbListItem, DbNode +from .transaction import Transaction + + +class List(ListImplBase): + _id: int + + def __init__(self, id: int) -> None: + self._id = id + + def get_or_append(self, txn: TransactionBase, index: int) -> Node: + from .node_impl import NodeImpl + + assert isinstance(txn, Transaction) + assert index >= 0 + + allitems = txn._session.query(DbListItem).filter( + DbListItem.list_node_id == self._id + ) + if index > allitems.count(): + raise DatabaseError( + "Requesting to get or append index in list that is more than one out of bounds." + ) + elif index == allitems.count(): + child = DbNode(0, None) + txn._session.add(child) + txn._session.flush() + txn._session.add(DbListItem(index, self._id, child.id)) + return Node(NodeImpl(child.id)) + else: + item = allitems.filter(DbListItem.index == index).first() + return Node(NodeImpl(item.child_node_id)) + + def append(self, txn: TransactionBase) -> Node: + from .node_impl import NodeImpl + + assert isinstance(txn, Transaction) + + index = ( + txn._session.query(DbListItem) + .filter(DbListItem.list_node_id == self._id) + .count() + ) + + child = DbNode(0, None) + txn._session.add(child) + txn._session.flush() + txn._session.add(DbListItem(index, self._id, child.id)) + return Node(NodeImpl(child.id)) + + def get(self, txn: Transaction, index: int) -> Node: + from .node_impl import NodeImpl + + item = txn._session.query(DbListItem).filter( + DbListItem.list_node_id == self._id, DbListItem.index == index + ) + if item.count() != 1: + raise DatabaseError("Index out of bounds or database corrupted.") + return Node(NodeImpl(item.first().child_node_id)) + + def len(self, txn: Transaction) -> int: + return ( + txn._session.query(DbListItem) + .filter(DbListItem.list_node_id == self._id) + .count() + ) diff --git a/core/revolve2/core/database/sqlite/node_impl.py b/core/revolve2/core/database/sqlite/node_impl.py new file mode 100644 index 000000000..5e41d35ea --- /dev/null +++ b/core/revolve2/core/database/sqlite/node_impl.py @@ -0,0 +1,143 @@ +import base64 +import json +import re +from json.decoder import JSONDecodeError +from typing import Any, Dict, List, Tuple, Union + +from ..database_error import DatabaseError +from ..list import List as ListIface +from ..node import Node as NodeIface +from ..node_impl import NodeImpl as NodeImplBase +from ..object import Object, is_object +from ..transaction import Transaction as TransactionBase +from ..uninitialized import Uninitialized +from .list_impl import List as ListImpl +from .schema import DbNode +from .transaction import Transaction + + +class NodeImpl(NodeImplBase): + _id: int + + def __init__(self, id: int) -> None: + self._id = id + + def get_object(self, txn: TransactionBase) -> Union[Object, Uninitialized]: + assert isinstance(txn, Transaction) + + row = self._get_row(txn) + + if row.type == 0: + return Uninitialized() + elif row.type == 1: + object = row.object + if not isinstance(object, str): + raise DatabaseError("Object of Node should be string, but it is not.") + + try: + return json.loads(object, cls=_JSONDecoder) + except JSONDecodeError as err: + raise DatabaseError("Object of Node is not valid JSON.") + elif row.type == 2: + return ListIface(ListImpl(row.id)) + else: + raise DatabaseError("Database corrupted. Unexpected type.") + + def set_object(self, txn: TransactionBase, object: Object) -> None: + assert isinstance(txn, Transaction) + + if not is_object(object): + raise DatabaseError("Not a valid object.") + + dbrow = self._get_row(txn) + if dbrow.object is not None or dbrow.type != 0: + raise DatabaseError("Node object already set. Cannot overwrite.") + + node_stubs, list_stubs = self._find_node_stubs(object) + + if not all([stub.is_stub for stub in node_stubs]): + raise DatabaseError( + "Cannot add a node to an object that is not a stub. You are reusing nodes." + ) + + dbnodes = [DbNode(0, None) for _ in node_stubs] + dblistnodes = [DbNode(2, None) for _ in list_stubs] + + for node in dbnodes: + txn._session.add(node) + for node in dblistnodes: + txn._session.add(node) + + txn._session.flush() + + for node_stub, dbnode in zip(node_stubs, dbnodes): + node_stub._set_impl(NodeImpl(dbnode.id)) + for list_stub, dblistnode in zip(list_stubs, dblistnodes): + list_stub._set_impl(ListImpl(dblistnode.id)) + + jsoned = json.dumps(object, cls=_JSONEncoder) + dbrow.object = jsoned + dbrow.type = 1 + + def _get_row(self, txn: Transaction) -> Any: # TODO sqlalchemy typing + query = txn._session.query(DbNode).filter(DbNode.id == self._id) + if query.count() != 1: + raise DatabaseError( + "Database out of sync with program. Node does not exist in database but it should." + ) + return query.first() + + @classmethod + def _find_node_stubs( + cls, object: Object + ) -> Tuple[List[NodeIface], List[ListIface]]: + if isinstance(object, NodeIface): + return [object], [] + elif isinstance(object, ListIface): + return [], [object] + elif isinstance(object, list): + nodes = [] + lists = [] + for child in object: + new_nodes, new_lists = cls._find_node_stubs(child) + nodes += new_nodes + lists += new_lists + return (nodes, lists) + elif isinstance(object, dict): + nodes = [] + lists = [] + for child in object.values(): + new_nodes, new_lists = cls._find_node_stubs(child) + nodes += new_nodes + lists += new_lists + return (nodes, lists) + else: + return ([], []) + + +class _JSONEncoder(json.JSONEncoder): + def default(self, obj): + if isinstance(obj, NodeIface): + assert isinstance(obj._impl, NodeImpl) + return {"__type__": "node", "id": obj._impl._id} + elif isinstance(obj, ListIface): + assert isinstance(obj._impl, ListImpl) + return {"__type__": "node", "id": obj._impl._id} + elif isinstance(obj, bytes): + return {"__type__": "bytes", "bytes": base64.b64encode(obj).decode("ascii")} + else: + return json.JSONEncoder.default(self, obj) + + +class _JSONDecoder(json.JSONDecoder): + def __init__(self, *args, **kwargs): + json.JSONDecoder.__init__(self, object_hook=self.object_hook, *args, **kwargs) + + def object_hook(self, dct: Dict): + type = dct.get("__type__") + if type is None: + return dct + elif type == "node": + return NodeIface(NodeImpl(dct["id"])) + elif type == "bytes": + return base64.b64decode(dct["bytes"]) diff --git a/core/revolve2/core/database/sqlite/schema.py b/core/revolve2/core/database/sqlite/schema.py new file mode 100644 index 000000000..d8ce2854a --- /dev/null +++ b/core/revolve2/core/database/sqlite/schema.py @@ -0,0 +1,36 @@ +from typing import Optional + +from sqlalchemy import Column, Integer, String +from sqlalchemy.ext.declarative import declarative_base +from sqlalchemy.sql.expression import null + +Base = declarative_base() + + +class DbNode(Base): + __tablename__ = "nodes" + id = Column( + Integer, nullable=False, unique=True, autoincrement=True, primary_key=True + ) + type = Column(Integer, nullable=False) # 0: uninitialized, 1: object, 2: list + object = Column(String, nullable=True) + + def __init__(self, type: int, object: Optional[str], id: int = None) -> None: + self.id = id + self.type = type + self.object = object + + +class DbListItem(Base): + __tablename__ = "list_items" + id = Column( + Integer, nullable=False, unique=True, autoincrement=True, primary_key=True + ) + index = Column(Integer, nullable=False) + list_node_id = Column(Integer, nullable=False) + child_node_id = Column(Integer, nullable=False) + + def __init__(self, index: int, list_node_id: int, child_node_id: int) -> None: + self.index = index + self.list_node_id = list_node_id + self.child_node_id = child_node_id diff --git a/core/revolve2/core/database/sqlite/transaction.py b/core/revolve2/core/database/sqlite/transaction.py new file mode 100644 index 000000000..c9675d7c5 --- /dev/null +++ b/core/revolve2/core/database/sqlite/transaction.py @@ -0,0 +1,11 @@ +import logging +from typing import Any + +from ..transaction import Transaction as TransactionBase + + +class Transaction(TransactionBase): + _session: Any # TODO sqlalchemy typing bad + + def __init__(self, session: Any) -> None: + self._session = session diff --git a/core/revolve2/core/database/static_data.py b/core/revolve2/core/database/static_data.py new file mode 100644 index 000000000..61e7c8e66 --- /dev/null +++ b/core/revolve2/core/database/static_data.py @@ -0,0 +1,27 @@ +from typing import Any, Dict, List, Union, cast + +StaticData = Union[ + List["StaticData"], Dict[str, "StaticData"], str, float, int, bytes, None +] + + +def is_static_data(to_check: Any) -> bool: + if ( + to_check is None + or type(to_check) == int + or type(to_check) == float + or type(to_check) == str + or type(to_check) == bytes + ): + return True + elif type(to_check) == list: + return all([is_static_data(child) for child in to_check]) + elif type(to_check) == dict: + return all( + [ + type(key) == str and is_static_data(val) + for key, val in cast(Dict[Any, Any], to_check).items() + ] + ) + else: + return False diff --git a/core/revolve2/core/database/transaction.py b/core/revolve2/core/database/transaction.py new file mode 100644 index 000000000..ef9dd417a --- /dev/null +++ b/core/revolve2/core/database/transaction.py @@ -0,0 +1,2 @@ +class Transaction: + pass diff --git a/core/revolve2/core/database/uninitialized.py b/core/revolve2/core/database/uninitialized.py new file mode 100644 index 000000000..fceb37406 --- /dev/null +++ b/core/revolve2/core/database/uninitialized.py @@ -0,0 +1,2 @@ +class Uninitialized: + pass diff --git a/core/revolve2/core/modular_robot/__init__.py b/core/revolve2/core/modular_robot/__init__.py new file mode 100644 index 000000000..c3ac64802 --- /dev/null +++ b/core/revolve2/core/modular_robot/__init__.py @@ -0,0 +1,10 @@ +from .active_hinge import ActiveHinge +from .analyzer import Analyzer +from .analyzer_module import AnalyzerModule +from .body import Body +from .brain import Brain +from .brick import Brick +from .core import Core +from .modular_robot import ModularRobot +from .module import Module +from .serialized import Serialized diff --git a/core/revolve2/core/modular_robot/active_hinge.py b/core/revolve2/core/modular_robot/active_hinge.py new file mode 100644 index 000000000..d6f07c1bf --- /dev/null +++ b/core/revolve2/core/modular_robot/active_hinge.py @@ -0,0 +1,34 @@ +from typing import Optional + +from .module import Module +from .serialized import Serialized + + +class ActiveHinge(Module): + ATTACHMENT_INDEX = 0 + + def __init__(self, rotation: float): + super().__init__(Module.Type.ACTIVE_HINGE, 1, rotation) + + def serialize(self) -> Serialized: + """ + Serialize to a dictionary containing only the data types + Dict, List, str, int, float bool, + which in turn will only contain these data types as well. + """ + + serialized = super().serialize() + assert type(serialized) == dict + + if self.attachment is not None: + serialized["attachment"] = self.attachment.serialize() + + return serialized + + @property + def attachment(self) -> Optional[Module]: + return self.children[self.ATTACHMENT_INDEX] + + @attachment.setter + def attachment(self, module: Module) -> None: + self.children[self.ATTACHMENT_INDEX] = module diff --git a/core/revolve2/core/modular_robot/analyzer.py b/core/revolve2/core/modular_robot/analyzer.py new file mode 100644 index 000000000..bfd5b638a --- /dev/null +++ b/core/revolve2/core/modular_robot/analyzer.py @@ -0,0 +1,159 @@ +import math +from typing import List, Optional, Tuple + +from pyrr import Quaternion, Vector3 + +from .active_hinge import ActiveHinge +from .analyzer_module import AnalyzerModule +from .body import Body +from .brick import Brick +from .core import Core +from .module import Module + + +class Analyzer: + _core: AnalyzerModule + _num_modules: int + _active_hinges: List[AnalyzerModule] + _bricks: List[AnalyzerModule] + + def __init__(self, body: Body): + self._active_hinges = [] + self._bricks = [] + (self._core, self._num_modules) = self._init_tree_node(body.core, 0, None, None) + + def _init_tree_node( + self, + module: Module, + next_id: int, + parent: Optional[AnalyzerModule], + parent_child_index: Optional[int], + ) -> Tuple[AnalyzerModule, int]: + analyzer_module = AnalyzerModule(module, next_id, parent, parent_child_index) + next_id += 1 + + if module.type == Module.Type.ACTIVE_HINGE: + self._active_hinges.append(analyzer_module) + elif module.type == Module.Type.BRICK: + self._bricks.append(analyzer_module) + + for child_index, child in enumerate(module.children): + if child is not None: + (analyzer_child, next_id) = self._init_tree_node( + child, next_id, analyzer_module, child_index + ) + analyzer_module.children[child_index] = analyzer_child + + return (analyzer_module, next_id) + + @property + def num_modules(self) -> int: + return self._num_modules + + @property + def active_hinges(self) -> List[AnalyzerModule]: + """ + Get the active hinges in this modular robot. + """ + return self._active_hinges + + @property + def bricks(self) -> List[AnalyzerModule]: + """ + Get the bricks in this modular robot. + """ + return self._bricks + + def neighbours( + self, module: AnalyzerModule, within_range: int + ) -> List[AnalyzerModule]: + out_neighbours: List[AnalyzerModule] = [] + + open_nodes: List[Tuple[AnalyzerModule, Optional[AnalyzerModule]]] = [ + (module, None) + ] # (module, came_from) + + for _ in range(within_range): + new_open_nodes: List[Tuple[AnalyzerModule, Optional[AnalyzerModule]]] = [] + for (open_node, came_from) in open_nodes: + neighbours = [ + mod + for mod in open_node.children + [open_node.parent] + if mod is not None + and (came_from is None or mod.id is not came_from.id) + ] + out_neighbours += neighbours + new_open_nodes += list(zip(neighbours, [open_node] * len(neighbours))) + open_nodes = new_open_nodes + + return out_neighbours + + @property + def core(self) -> AnalyzerModule: + return self._core + + def size(self) -> Tuple[float, float, float, float, float, float]: + """ + Get size of robot measured in number of modules. + -x, +x, -y, +y, -z, +z + with respect to the core. + All angles must be multiples of 90 degrees. + """ + size = self._recursive_size(self._core, Vector3([0, 0, 0]), Quaternion()) + return ( + round(size[0]), + round(size[1]), + round(size[2]), + round(size[3]), + round(size[4]), + round(size[5]), + ) + + def _recursive_size( + self, module: AnalyzerModule, position: Vector3, orientation: Quaternion + ) -> Tuple[int, int, int, int, int, int]: + if module.type == Module.Type.CORE: + # for (child_index, angle) in [ + children = [ + (Core.FRONT, 0.0), + (Core.BACK, math.pi), + (Core.LEFT, math.pi / 2.0), + (Core.RIGHT, math.pi / 2.0 * 3), + ] + elif module.type == Module.Type.BRICK: + children = [ + (Brick.FRONT, 0.0), + (Brick.LEFT, math.pi / 2.0), + (Brick.RIGHT, math.pi / 2.0 * 3), + ] + elif module.type == Module.Type.ACTIVE_HINGE: + children = [(ActiveHinge.ATTACHMENT_INDEX, 0.0)] + + sizes = [ + (position.x, position.x, position.y, position.y, position.z, position.z) + ] + [ + self._recursive_size( + child, position + quat * Vector3([1.0, 0.0, 0.0]), quat + ) + for child, quat in [ + ( + module.children[index], + orientation + * Quaternion.from_eulers([0.0, 0.0, angle]) + * Quaternion.from_eulers( + [module.children[index].module.rotation, 0.0, 0.0] + ), + ) + for (index, angle) in children + if module.children[index] is not None + ] + ] + + minx = min(sizes, key=lambda size: size[0])[0] + maxx = max(sizes, key=lambda size: size[1])[1] + miny = min(sizes, key=lambda size: size[2])[2] + maxy = max(sizes, key=lambda size: size[3])[3] + minz = min(sizes, key=lambda size: size[4])[4] + maxz = max(sizes, key=lambda size: size[5])[5] + + return (minx, maxx, miny, maxy, minz, maxz) diff --git a/core/revolve2/core/modular_robot/analyzer_module.py b/core/revolve2/core/modular_robot/analyzer_module.py new file mode 100644 index 000000000..35b7684b3 --- /dev/null +++ b/core/revolve2/core/modular_robot/analyzer_module.py @@ -0,0 +1,110 @@ +from __future__ import annotations + +import math +from typing import List, Optional + +import numpy as np +from pyrr import Quaternion, Vector3 + +from .active_hinge import ActiveHinge +from .brick import Brick +from .core import Core +from .module import Module + + +class AnalyzerModule: + _id: int + _module: Module + _children: List[Optional[AnalyzerModule]] + _parent: Optional[AnalyzerModule] + _parent_child_index: Optional[int] + + def __init__( + self, + module: Module, + id: int, + parent: Optional[AnalyzerModule], + parent_child_index: Optional[int], + ): + self._module = module + self._id = id + self._children = [None] * len(module.children) + + assert (parent is None and parent_child_index is None) or ( + parent is not None and parent_child_index is not None + ) + self._parent = parent + self._parent_child_index = parent_child_index + + @property + def children(self) -> List[Optional[AnalyzerModule]]: + return self._children + + def grid_position(self) -> Vector3: + """ + Calculate the position of this module in a 3d grid with the core as center. + The distance between all modules is assumed to be one grid cell. + All module angles must be multiples of 90 degrees. + """ + position = Vector3() + + parent = self._parent + child_index = self._parent_child_index + while parent is not None and child_index is not None: + child = parent.module.children[child_index] + assert child is not None + assert np.isclose(child.rotation % (math.pi / 2.0), 0.0) + + position = Quaternion.from_eulers((child.rotation, 0.0, 0.0)) * position + position += Vector3([1, 0, 0]) + rotation: Quaternion + if parent.type == Module.Type.CORE: + if child_index == Core.FRONT: + rotation = Quaternion.from_eulers((0.0, 0.0, 0.0)) + elif child_index == Core.LEFT: + rotation = Quaternion.from_eulers((0.0, 0.0, math.pi / 2.0 * 1)) + elif child_index == Core.BACK: + rotation = Quaternion.from_eulers((0.0, 0.0, math.pi / 2.0 * 2)) + elif child_index == Core.RIGHT: + rotation = Quaternion.from_eulers((0.0, 0.0, math.pi / 2.0 * 3)) + else: + raise NotImplementedError() + elif parent.type == Module.Type.BRICK: + if child_index == Brick.FRONT: + rotation = Quaternion.from_eulers((0.0, 0.0, 0.0)) + elif child_index == Brick.LEFT: + rotation = Quaternion.from_eulers((0.0, 0.0, math.pi / 2.0 * 1)) + elif child_index == Brick.RIGHT: + rotation = Quaternion.from_eulers((0.0, 0.0, math.pi / 2.0 * 3)) + else: + raise NotImplementedError() + elif parent.type == Module.Type.ACTIVE_HINGE: + if child_index == ActiveHinge.ATTACHMENT_INDEX: + rotation = Quaternion() + else: + raise NotImplementedError() + else: + raise NotImplementedError() + position = rotation * position + position = Vector3.round(position) + + child_index = parent._parent_child_index + parent = parent._parent + + return position + + @property + def parent(self) -> Optional[AnalyzerModule]: + return self._parent + + @property + def type(self) -> Module.Type: + return self.module.type + + @property + def id(self) -> int: + return self._id + + @property + def module(self) -> Module: + return self._module diff --git a/core/revolve2/core/modular_robot/body.py b/core/revolve2/core/modular_robot/body.py new file mode 100644 index 000000000..04c332a26 --- /dev/null +++ b/core/revolve2/core/modular_robot/body.py @@ -0,0 +1,18 @@ +from .core import Core +from .serialized import Serialized + + +class Body: + core: Core + + def __init__(self): + self.core = Core(0.0) + + def serialize(self) -> Serialized: + """ + Serialize to a dictionary containing only the data types + Dict, List, str, int, float bool, + which in turn will only contain these data types as well. + """ + + return {"core": self.core.serialize()} diff --git a/core/revolve2/core/modular_robot/brain.py b/core/revolve2/core/modular_robot/brain.py new file mode 100644 index 000000000..6d9ae24fd --- /dev/null +++ b/core/revolve2/core/modular_robot/brain.py @@ -0,0 +1,31 @@ +from abc import ABC, abstractmethod +from typing import List + +from revolve2.core.physics.actor import Actor +from revolve2.core.physics.control import ActorController + +from .analyzer import Analyzer, AnalyzerModule +from .serialized import Serialized + + +class Brain(ABC): + @abstractmethod + def make_controller( + self, analyzer: Analyzer, actor: Actor, dof_ids: List[AnalyzerModule] + ) -> ActorController: + """ + Create a controller for the provided body + + :param analyzer: Tool for analyzing the modular robot + :param actor: Actor created from the modular robot + :param dof_ids: Map from actor joint index to AnalyzerModule + """ + pass + + @abstractmethod + def serialize(self) -> Serialized: + """ + Serialize to a dictionary containing only the data types + Dict, List, str, int, float bool, + which in turn will only contain these data types as well. + """ diff --git a/core/revolve2/core/modular_robot/brains/__init__.py b/core/revolve2/core/modular_robot/brains/__init__.py new file mode 100644 index 000000000..e403b0464 --- /dev/null +++ b/core/revolve2/core/modular_robot/brains/__init__.py @@ -0,0 +1,2 @@ +from .cpg import Cpg +from .cpg_random import CpgRandom diff --git a/core/revolve2/core/modular_robot/brains/cpg.py b/core/revolve2/core/modular_robot/brains/cpg.py new file mode 100644 index 000000000..54362c393 --- /dev/null +++ b/core/revolve2/core/modular_robot/brains/cpg.py @@ -0,0 +1,89 @@ +import math +from abc import ABC, abstractmethod +from typing import Dict, List, Tuple + +import numpy as np +from revolve2.core.modular_robot import ( + Analyzer, + AnalyzerModule, + Brain, + Module, + Serialized, +) +from revolve2.core.physics.actor import Actor +from revolve2.core.physics.control import ActorController +from revolve2.core.physics.control.actor_controllers import Cpg as ControllerCpg + + +class Cpg(Brain, ABC): + def make_controller( + self, analyzer: Analyzer, actor: Actor, dof_ids: List[AnalyzerModule] + ) -> ActorController: + active_hinges = analyzer.active_hinges + connections = self._find_connections(analyzer) + (internal_weights, external_weights) = self._make_weights( + active_hinges, connections + ) + assert len(internal_weights) == len(active_hinges) + assert len(external_weights) == len(connections) + + id_to_index: Dict[int, int] = {} + for i, active_hinge in enumerate(active_hinges): + id_to_index[active_hinge.id] = i + + intial_state = np.array([0.5 * math.sqrt(2)] * (len(active_hinges) * 2)) + weight_matrix = np.zeros((len(active_hinges) * 2, len(active_hinges) * 2)) + + for i in range(len(active_hinges)): + weight_matrix[i][len(active_hinges) + i] = internal_weights[i] + weight_matrix[len(active_hinges) + i][i] = -internal_weights[i] + + for i, (mod_a, mod_b) in enumerate(connections): + index_a = id_to_index[mod_a.id] + index_b = id_to_index[mod_b.id] + weight_matrix[index_a][index_b] = external_weights[i] + weight_matrix[index_b][index_a] = -external_weights[i] + + return ControllerCpg(intial_state, len(active_hinges), weight_matrix) + + @abstractmethod + def _make_weights( + self, + active_hinges: List[AnalyzerModule], + connections: List[Tuple[AnalyzerModule, AnalyzerModule]], + ) -> Tuple[List[float], List[float]]: + """ + Override to the weights between neurons. + + :param active_hinges: The active hinges corresponding to each cpg. + :param connection: Pairs of active hinges corresponding to pairs of cpgs that are connected. + :return: Two lists. The first list contains the internal weights in cpgs, corresponding to `active_hinges` + The second list contains the weights between connected cpgs, corresponding to `connections` + The lists should match the order of the input parameters. + """ + + @abstractmethod + def serialize(self) -> Serialized: + pass + + @staticmethod + def _find_connections( + analyzer: Analyzer, + ) -> List[Tuple[AnalyzerModule, AnalyzerModule]]: + active_hinges = analyzer.active_hinges + # sort by id, will be used later when ignoring existing connections + active_hinges.sort(key=lambda mod: mod.id) + + connections: List[Tuple[AnalyzerModule, AnalyzerModule]] = [] + for active_hinge in analyzer.active_hinges: + neighbours = analyzer.neighbours(active_hinge, 2) + # ignore existing connections and neighbours that are not an active hinge + neighbours = [ + neighbour + for neighbour in neighbours + if neighbour.id > active_hinge.id + and neighbour.type == Module.Type.ACTIVE_HINGE + ] + connections += zip([active_hinge] * len(neighbours), neighbours) + + return connections diff --git a/core/revolve2/core/modular_robot/brains/cpg_random.py b/core/revolve2/core/modular_robot/brains/cpg_random.py new file mode 100644 index 000000000..5caad8f31 --- /dev/null +++ b/core/revolve2/core/modular_robot/brains/cpg_random.py @@ -0,0 +1,22 @@ +import random +from typing import List, Tuple + +from revolve2.core.modular_robot import AnalyzerModule, Serialized + +from .cpg import Cpg + + +class CpgRandom(Cpg): + def _make_weights( + self, + active_hinges: List[AnalyzerModule], + connections: List[Tuple[AnalyzerModule, AnalyzerModule]], + ) -> Tuple[List[float], List[float]]: + # TODO use provided rng object, instead of global + return ( + [random.random() * 2.0 - 1 for _ in range(len(active_hinges))], + [random.random() * 2.0 - 1 for _ in range(len(connections))], + ) + + def serialize(self) -> Serialized: + return [] # TODO diff --git a/core/revolve2/core/modular_robot/brick.py b/core/revolve2/core/modular_robot/brick.py new file mode 100644 index 000000000..b05beaa80 --- /dev/null +++ b/core/revolve2/core/modular_robot/brick.py @@ -0,0 +1,56 @@ +from typing import Optional + +from .module import Module +from .serialized import Serialized + + +class Brick(Module): + FRONT = 0 + RIGHT = 1 + LEFT = 2 + + def __init__(self, rotation: float): + super().__init__(Module.Type.BRICK, 4, rotation) + + def serialize(self) -> Serialized: + """ + Serialize to a dictionary containing only the data types + Dict, List, str, int, float bool, + which in turn will only contain these data types as well. + """ + + serialized = super().serialize() + assert type(serialized) == dict + + if self.front is not None: + serialized["front"] = self.front.serialize() + if self.left is not None: + serialized["left"] = self.left.serialize() + if self.right is not None: + serialized["right"] = self.right.serialize() + + return serialized + + @property + def front(self) -> Optional[Module]: + return self.children[self.FRONT] + + @front.setter + def front(self, module: Module) -> None: + self.children[self.FRONT] = module + + @property + def right(self) -> Optional[Module]: + return self.children[self.RIGHT] + + @right.setter + def right(self, module: Module) -> None: + self.children[self.RIGHT] = module + + @property + def left(self) -> Optional[Module]: + return self.children[self.LEFT] + + @left.setter + def left(self, module: Module) -> None: + self.children[self.LEFT] = module diff --git a/core/revolve2/core/modular_robot/core.py b/core/revolve2/core/modular_robot/core.py new file mode 100644 index 000000000..db1d34242 --- /dev/null +++ b/core/revolve2/core/modular_robot/core.py @@ -0,0 +1,67 @@ +from typing import Optional + +from .module import Module +from .serialized import Serialized + + +class Core(Module): + FRONT = 0 + RIGHT = 1 + BACK = 2 + LEFT = 3 + + def __init__(self, rotation: float): + super().__init__(Module.Type.CORE, 4, rotation) + + def serialize(self) -> Serialized: + """ + Serialize to a dictionary containing only the data types + Dict, List, str, int, float bool, + which in turn will only contain these data types as well. + """ + + serialized = super().serialize() + assert type(serialized) == dict + + if self.front is not None: + serialized["front"] = self.front.serialize() + if self.back is not None: + serialized["back"] = self.back.serialize() + if self.left is not None: + serialized["left"] = self.left.serialize() + if self.right is not None: + serialized["right"] = self.right.serialize() + + return serialized + + @property + def front(self) -> Optional[Module]: + return self.children[self.FRONT] + + @front.setter + def front(self, module: Module) -> None: + self.children[self.FRONT] = module + + @property + def right(self) -> Optional[Module]: + return self.children[self.RIGHT] + + @right.setter + def right(self, module: Module) -> None: + self.children[self.RIGHT] = module + + @property + def back(self) -> Optional[Module]: + return self.children[self.BACK] + + @back.setter + def back(self, module: Module) -> None: + self.children[self.BACK] = module + + @property + def left(self) -> Optional[Module]: + return self.children[self.LEFT] + + @left.setter + def left(self, module: Module) -> None: + self.children[self.LEFT] = module diff --git a/core/revolve2/core/modular_robot/modular_robot.py b/core/revolve2/core/modular_robot/modular_robot.py new file mode 100644 index 000000000..ac5a059f0 --- /dev/null +++ b/core/revolve2/core/modular_robot/modular_robot.py @@ -0,0 +1,35 @@ +from typing import Dict, Tuple + +from revolve2.core.physics.actor import Actor +from revolve2.core.physics.control import ActorController + +from .body import Body +from .brain import Brain +from .serialized import Serialized + + +class ModularRobot: + body: Body + brain: Brain + + def __init__(self, body: Body, brain: Brain): + self.body = body + self.brain = brain + + def serialize(self) -> Dict[str, Serialized]: + """ + Serialize to a dictionary containing only the data types + Dict, List, str, int, float bool, + which in turn will only contain these data types as well. + """ + + return {"body": self.body.serialize(), "brain": self.brain.serialize()} + + def make_actor_and_controller(self) -> Tuple[Actor, ActorController]: + from .analyzer import Analyzer + from .to_actor import to_actor + + analyzer = Analyzer(self.body) + actor, dof_ids = to_actor(analyzer) + controller = self.brain.make_controller(analyzer, actor, dof_ids) + return (actor, controller) diff --git a/core/revolve2/core/modular_robot/module.py b/core/revolve2/core/modular_robot/module.py new file mode 100644 index 000000000..65bb8c19c --- /dev/null +++ b/core/revolve2/core/modular_robot/module.py @@ -0,0 +1,44 @@ +from __future__ import annotations + +from enum import Enum +from typing import TYPE_CHECKING, List, Optional + +from .serialized import Serialized + + +class Module: + class Type(Enum): + CORE = "core" + BRICK = "brick" + ACTIVE_HINGE = "active_hinge" + + _type: Type + _children: List[Optional[Module]] + _rotation: float + + def __init__(self, type: Type, num_children: int, rotation: float): + self._type = type + self._id = None + self._children = [None] * num_children + self._rotation = rotation + + def serialize(self) -> Serialized: + """ + Serialize to a dictionary containing only the data types + Dict, List, str, int, float bool, + which in turn will only contain these data types as well. + """ + + return {"type": self.type.value} + + @property + def type(self) -> Type: + return self._type + + @property + def children(self) -> List[Optional[Module]]: + return self._children + + @property + def rotation(self) -> float: + return self._rotation diff --git a/core/revolve2/core/modular_robot/serialized.py b/core/revolve2/core/modular_robot/serialized.py new file mode 100644 index 000000000..b744a2ece --- /dev/null +++ b/core/revolve2/core/modular_robot/serialized.py @@ -0,0 +1,3 @@ +from typing import List, Mapping, Union + +Serialized = Union[List["Serialized"], Mapping[str, "Serialized"], str, float, int] diff --git a/core/revolve2/core/modular_robot/to_actor.py b/core/revolve2/core/modular_robot/to_actor.py new file mode 100644 index 000000000..3773dd5eb --- /dev/null +++ b/core/revolve2/core/modular_robot/to_actor.py @@ -0,0 +1,324 @@ +import math +from typing import List, Tuple, cast + +from pyrr import Quaternion, Vector3 +from revolve2.core.physics.actor import Actor, Collision, Joint, RigidBody, Visual + +from .active_hinge import ActiveHinge +from .analyzer import Analyzer +from .analyzer_module import AnalyzerModule +from .brick import Brick +from .core import Core +from .module import Module + + +def to_actor(analyzer: Analyzer) -> Tuple[Actor, List[AnalyzerModule]]: + """ + Create actor from analyzer + + :returns: (the actor, analyzer modules matching the joints in the actor) + """ + return _ActorBuilder().build(analyzer) + + +class _ActorBuilder: + _STATIC_FRICTION = 0.8 + _DYNAMIC_FRICTION = 0.5 + + robot: Actor + dof_ids: List[AnalyzerModule] + + def build(self, analyzer: Analyzer) -> Tuple[Actor, List[AnalyzerModule]]: + self.robot = Actor([], []) + self.dof_ids = [] + + origin_body = RigidBody( + "origin", + Vector3(), + Quaternion(), + self._STATIC_FRICTION, + self._DYNAMIC_FRICTION, + ) + self.robot.bodies.append(origin_body) + + self._make_module(analyzer.core, origin_body, "origin", Vector3(), Quaternion()) + + return (self.robot, self.dof_ids) + + def _make_module( + self, + module: AnalyzerModule, + body: RigidBody, + name_prefix: str, + attachment_offset: Vector3, + orientation: Quaternion, + ) -> None: + if module.type == Module.Type.CORE: + self._make_core( + module, + body, + name_prefix, + attachment_offset, + orientation, + ) + elif module.type == Module.Type.BRICK: + self._make_brick( + module, + body, + name_prefix, + attachment_offset, + orientation, + ) + elif module.type == Module.Type.ACTIVE_HINGE: + self._make_active_hinge( + module, + body, + name_prefix, + attachment_offset, + orientation, + ) + else: + raise NotImplementedError("Module type not implemented") + + def _make_core( + self, + analyzer_module: AnalyzerModule, + body: RigidBody, + name_prefix: str, + attachment_point: Vector3, + orientation: Quaternion, + ) -> None: + BOUNDING_BOX = Vector3([0.089, 0.089, 0.0603]) # meter + MASS = 0.250 # kg + CHILD_OFFSET = 0.089 / 2.0 # meter + + module = cast(Core, analyzer_module.module) + + # attachment position is always at center of core + position = attachment_point + + body.collisions.append( + Collision( + f"{name_prefix}_core_collision", + position, + orientation, + MASS, + BOUNDING_BOX, + ) + ) + body.visuals.append( + Visual( + f"{name_prefix}_core_visual", + position, + orientation, + "model://rg_robot/meshes/CoreComponent.dae", + (1.0, 1.0, 0.0), + ) + ) + + for (name_suffix, child_index, angle) in [ + ("front", Core.FRONT, 0.0), + ("back", Core.BACK, math.pi), + ("left", Core.LEFT, math.pi / 2.0), + ("right", Core.RIGHT, math.pi / 2.0 * 3), + ]: + child = module.children[child_index] + if child is not None: + rotation = ( + orientation + * Quaternion.from_eulers([0.0, 0.0, angle]) + * Quaternion.from_eulers([child.rotation, 0, 0]) + ) + + analyzer_child = analyzer_module.children[child_index] + assert analyzer_child is not None + + self._make_module( + analyzer_child, + body, + f"{name_prefix}_{name_suffix}", + position + rotation * Vector3([CHILD_OFFSET, 0.0, 0.0]), + rotation, + ) + + def _make_brick( + self, + analyzer_module: AnalyzerModule, + body: RigidBody, + name_prefix: str, + attachment_point: Vector3, + orientation: Quaternion, + ) -> None: + BOUNDING_BOX = Vector3([0.06288625, 0.06288625, 0.0603]) # meter + MASS = 0.030 # kg + CHILD_OFFSET = 0.06288625 / 2.0 # meter + + module = cast(Brick, analyzer_module.module) + + position = attachment_point + orientation * Vector3( + [BOUNDING_BOX[0] / 2.0, 0.0, 0.0] + ) + + body.collisions.append( + Collision( + f"{name_prefix}_brick_collision", + position, + orientation, + MASS, + BOUNDING_BOX, + ) + ) + body.visuals.append( + Visual( + f"{name_prefix}_brick_visual", + position, + orientation, + "model://rg_robot/meshes/FixedBrick.dae", + (1.0, 0.0, 0.0), + ) + ) + + for (name_suffix, child_index, angle) in [ + ("front", Brick.FRONT, 0.0), + ("left", Brick.LEFT, math.pi / 2.0), + ("right", Brick.RIGHT, math.pi / 2.0 * 3), + ]: + child = module.children[child_index] + if child is not None: + rotation = ( + orientation + * Quaternion.from_eulers([0.0, 0.0, angle]) + * Quaternion.from_eulers([child.rotation, 0, 0]) + ) + + analyzer_child = analyzer_module.children[child_index] + assert analyzer_child is not None + + self._make_module( + analyzer_child, + body, + f"{name_prefix}_{name_suffix}", + position + rotation * Vector3([CHILD_OFFSET, 0.0, 0.0]), + rotation, + ) + + def _make_active_hinge( + self, + analyzer_module: AnalyzerModule, + body: RigidBody, + name_prefix: str, + attachment_point: Vector3, + orientation: Quaternion, + ) -> None: + FRAME_BOUNDING_BOX = Vector3([0.04525, 0.053, 0.0165891]) # meter + SERVO1_BOUNDING_BOX = Vector3([0.0583, 0.0512, 0.020]) # meter + SERVO2_BOUNDING_BOX = Vector3([0.002, 0.053, 0.053]) # meter + + FRAME_MASS = 0.011 # kg + SERVO1_MASS = 0.058 # kg + SERVO2_MASS = 0.0 # kg. we simplify by only using the weight of the first box + + SERVO_OFFSET = 0.0299 # meter. distance from frame to servo + JOINT_OFFSET = 0.0119 # meter. distance from frame to joint + + SERVO_BBOX2_POSITION = Vector3( + [SERVO1_BOUNDING_BOX[0] / 2.0 + SERVO2_BOUNDING_BOX[0] / 2.0, 0.0, 0.0] + ) + + ATTACHMENT_OFFSET = SERVO1_BOUNDING_BOX[0] / 2.0 + SERVO2_BOUNDING_BOX[0] + + module = cast(ActiveHinge, analyzer_module.module) + + frame_position = attachment_point + orientation * Vector3( + [FRAME_BOUNDING_BOX[0] / 2.0, 0.0, 0.0] + ) + servo_body_position = body.position + body.orientation * ( + frame_position + orientation * Vector3([SERVO_OFFSET, 0.0, 0.0]) + ) + servo_body_orientation = body.orientation * orientation + joint_position = body.position + body.orientation * ( + frame_position + orientation * Vector3([JOINT_OFFSET, 0.0, 0.0]) + ) + joint_orientation = body.orientation * orientation + + body.collisions.append( + Collision( + f"{name_prefix}_activehingeframe_collision", + frame_position, + orientation, + FRAME_MASS, + FRAME_BOUNDING_BOX, + ) + ) + body.visuals.append( + Visual( + f"{name_prefix}_activehingeframe_visual", + frame_position, + orientation, + "model://rg_robot/meshes/ActiveHinge_Frame.dae", + (0.0, 1.0, 0.0), + ) + ) + + next_body = RigidBody( + f"{name_prefix}_activehinge", + servo_body_position, + servo_body_orientation, + self._STATIC_FRICTION, + self._DYNAMIC_FRICTION, + ) + self.robot.bodies.append(next_body) + self.robot.joints.append( + Joint( + f"{name_prefix}_activehinge", + body, + next_body, + joint_position, + joint_orientation, + Vector3([0.0, 1.0, 0.0]), + ) + ) + self.dof_ids.append(analyzer_module) + + next_body.collisions.append( + Collision( + f"{name_prefix}_activehingemotor_collision1", + Vector3(), + Quaternion(), + SERVO1_MASS, + SERVO1_BOUNDING_BOX, + ) + ) + next_body.collisions.append( + Collision( + f"{name_prefix}_activehingemotor_collision2", + SERVO_BBOX2_POSITION, + Quaternion(), + SERVO2_MASS, + SERVO2_BOUNDING_BOX, + ) + ) + next_body.visuals.append( + Visual( + f"{name_prefix}_activehingemotor_visual", + Vector3(), + Quaternion(), + "model://rg_robot/meshes/ActiveCardanHinge_Servo_Holder.dae", + (0.0, 1.0, 0.0), + ) + ) + + child = module.children[ActiveHinge.ATTACHMENT_INDEX] + if child is not None: + rotation = Quaternion.from_eulers([child.rotation, 0.0, 0.0]) + + analyzer_child = analyzer_module.children[ActiveHinge.ATTACHMENT_INDEX] + assert analyzer_child is not None + + self._make_module( + analyzer_child, + next_body, + f"{name_prefix}_attachment", + rotation * Vector3([ATTACHMENT_OFFSET, 0.0, 0.0]), + rotation, + ) diff --git a/core/revolve2/core/optimization/__init__.py b/core/revolve2/core/optimization/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/core/revolve2/core/optimization/ea/__init__.py b/core/revolve2/core/optimization/ea/__init__.py new file mode 100644 index 000000000..a47b4a362 --- /dev/null +++ b/core/revolve2/core/optimization/ea/__init__.py @@ -0,0 +1,3 @@ +from .analyzer import Analyzer +from .evolutionary_optimizer import EvolutionaryOptimizer +from .individual import Individual diff --git a/core/revolve2/core/optimization/ea/analyzer.py b/core/revolve2/core/optimization/ea/analyzer.py new file mode 100644 index 000000000..002c2174f --- /dev/null +++ b/core/revolve2/core/optimization/ea/analyzer.py @@ -0,0 +1,211 @@ +from typing import Any, Dict, Iterator, List, Optional + +from revolve2.core.database import List as DbList +from revolve2.core.database import Node, Object, StaticData, Transaction, is_static_data +from revolve2.core.database.database_error import DatabaseError +from revolve2.core.database.serialize import SerializeError + + +class Generation: + _txn: Transaction + _individual_ids: List[int] + _gen_num: int + + def __init__(self, txn: Transaction, individual_ids: List[int], gen_num: int): + self._txn = txn + self._individual_ids = individual_ids + self._gen_num = gen_num + + def __iter__(self) -> Iterator[int]: + for id in self._individual_ids: + yield id + + @property + def individual_ids(self) -> List[int]: + return self._individual_ids + + @property + def gen_num(self) -> int: + return self._gen_num + + +class Generations: + _txn: Transaction + _list: DbList + + def __init__(self, txn: Transaction, list: DbList): + self._txn = txn + self._list = list + + def __getitem__(self, index: int) -> Generation: + try: + individual_ids = self._list.get(self._txn, index).get_object(self._txn) + if not isinstance(individual_ids, list) or not all( + [isinstance(id, int) for id in individual_ids] + ): + raise SerializeError() + return Generation(self._txn, individual_ids, index) + except DatabaseError as err: + raise SerializeError() from err + + def __len__(self) -> int: + return self._list.len(self._txn) + + def __iter__(self) -> Iterator[Generation]: + for i in range(len(self)): + yield self[i] + + +class Individual: + _id: int + _parents: Optional[List[int]] + _genotype: StaticData + _fitness: StaticData + + def __init__(self, data: dict): + self._id = data.get("id") + if self._id is None or not isinstance(self._id, int): + raise SerializeError() + + if "parents" not in data: + raise SerializeError() + self._parents = data["parents"] + if self._parents is not None: + if not isinstance(self._parents, list) or not all( + [isinstance(parent, int) for parent in self._parents] + ): + raise SerializeError() + + if "genotype" not in data: + raise SerializeError() + self._genotype = data["genotype"] + if not is_static_data(self._genotype): + raise SerializeError() + + if "fitness" not in data: + raise SerializeError() + self._fitness = data["fitness"] + if not is_static_data(self._fitness): + raise SerializeError() + + @property + def id(self) -> int: + return self._id + + @property + def parents(self) -> Optional[List[int]]: + return self._parents + + @property + def genotype(self) -> StaticData: + return self._genotype + + @property + def fitness(self) -> StaticData: + return self._fitness + + +class Individuals: + _txn: Transaction + _individuals: DbList + + def __init__(self, txn: Transaction, individuals: DbList): + self._txn = txn + self._individuals = individuals + + def __getitem__(self, index: int) -> Individual: + try: + individual = self._individuals.get(self._txn, index).get_object(self._txn) + if not isinstance(individual, dict): + raise SerializeError() + return Individual(individual) + except DatabaseError as err: + raise SerializeError from err + + def __len__(self) -> int: + return self._individuals.len(self._txn) + + def __iter__(self) -> Iterator[Generation]: + asdas = range(len(self)) + for i in range(len(self)): + yield self[i] + + +class Evaluations: + _txn: Transaction + _evals: DbList + + def __init__(self, txn: Transaction, evals: DbList): + self._txn = txn + self._evals = evals + + def __getitem__(self, index: int) -> Node: + try: + return self._evals.get(self._txn, index) + except DatabaseError as err: + raise SerializeError from err + + def __len__(self) -> int: + return self._evals.len(self._txn) + + def __iter__(self) -> Iterator[Node]: + for i in range(len(self)): + yield self[i] + + +class Analyzer: + _txn: Transaction + _ea: Dict[str, Object] + _evaluations: DbList + + def __init__(self, txn: Transaction, node: Node): + self._txn = txn + root = node.get_object(txn) + if not isinstance(root, Dict) or "ea" not in root or "evaluations" not in root: + raise SerializeError() + self._ea = root["ea"].get_object(txn) + if not isinstance(self._ea, dict): + raise SerializeError() + self._evaluations = root["evaluations"].get_object(txn) + if not isinstance(self._evaluations, DbList): + raise SerializeError() + + @property + def generations(self) -> Generations: + generations_node = self._ea.get("generations") + if generations_node is None or not isinstance(generations_node, Node): + raise SerializeError() + generations = generations_node.get_object(self._txn) + if not isinstance(generations, DbList): + raise SerializeError() + + return Generations(self._txn, generations) + + @property + def individuals(self) -> Individuals: + individuals_node = self._ea.get("individuals") + if individuals_node is None or not isinstance(individuals_node, Node): + raise SerializeError() + individuals = individuals_node.get_object(self._txn) + if not isinstance(individuals, DbList): + raise SerializeError() + + return Individuals(self._txn, individuals) + + @property + def offspring_size(self) -> int: + offspring_size = self._ea.get("offspring_size") + if offspring_size is None or not isinstance(offspring_size, int): + raise SerializeError() + return offspring_size + + @property + def population_size(self) -> int: + population_size = self._ea.get("population_size") + if population_size is None or not isinstance(population_size, int): + raise SerializeError() + return population_size + + @property + def evaluations(self) -> Evaluations: + return Evaluations(self._txn, self._evaluations) diff --git a/core/revolve2/core/optimization/ea/evolutionary_optimizer.py b/core/revolve2/core/optimization/ea/evolutionary_optimizer.py new file mode 100644 index 000000000..e4fda785f --- /dev/null +++ b/core/revolve2/core/optimization/ea/evolutionary_optimizer.py @@ -0,0 +1,497 @@ +from __future__ import annotations + +import logging +import pickle +from abc import ABC, abstractmethod +from random import Random +from typing import Generic, List, Optional, Type, TypeVar, Union, cast + +from revolve2.core.database import Database +from revolve2.core.database import List as DbList +from revolve2.core.database import Node, StaticData, Transaction +from revolve2.core.database.serialize import Serializable +from revolve2.core.database.serialize.serialize_error import SerializeError +from revolve2.core.database.uninitialized import Uninitialized + +from .individual import Individual + +Genotype = TypeVar("Genotype", bound=Union[Serializable, StaticData]) +Fitness = TypeVar("Fitness", bound=Union[Serializable, StaticData]) + + +class EvolutionaryOptimizer(ABC, Generic[Genotype, Fitness]): + __database: Database + __db_node: Node + + _rng: Random + + # next id to give to a new individual + __next_id: int + + # Types of genotype and fitness are stored as soon as they are available. + # Used to type check the return values of user functions. + __genotype_type: Type + __fitness_type: Optional[Type] + + __population_size: int + __offspring_size: int + + __generation_index: Optional[int] + __last_generation: Optional[List[Individual[Genotype, Fitness]]] + __initial_population: Optional[List[Genotype]] + + __db_ea: Node + __db_evaluations: DbList + + __db_rng_after_generation: Optional[DbList] + __db_generations: Optional[DbList] + __db_individuals: Optional[DbList] + + def __init__(self) -> None: + raise ValueError( + "Do not call this function. Call asyncinit instead. Inherit this class and implement __init__ to avoid this error." + ) + + async def asyncinit( + self, + database: Database, + db_node: Node, + random: Random, + population_size: int, + offspring_size: int, + initial_population: List[Genotype], + initial_fitness: Optional[List[Fitness]], + ) -> None: + self.__database = database + self.__db_node = db_node + + self._rng = random + + self.__next_id = 0 + + logging.info("Attempting to load checkpoint..") + if await self._load_checkpoint_or_init(): + logging.info( + f"Checkpoint found. Last complete generation was {self.__generation_index}." + ) + else: + logging.info( + f"No checkpoint has been made yet. Starting with generation 0." + ) + assert type(population_size) == int + self.__population_size = population_size + + assert type(offspring_size) == int + self.__offspring_size = offspring_size + + assert type(initial_population) == list + assert len(initial_population) == self.__population_size + assert len(initial_population) >= 1 + + # save genotype type so we can type check things later + self.__genotype_type = type(initial_population[0]) + + if initial_fitness is not None: + assert type(initial_fitness) == list + assert len(initial_fitness) == self.__population_size + self.__last_generation = [ + Individual[Genotype, Fitness]( + self._get_next_id(), genotype, fitness, None + ) + for genotype, fitness in zip(initial_population, initial_fitness) + ] + self.__generation_index = 0 + self.__initial_population = None + + # save fitness type so we can type check things later + self.__fitness_type = type(initial_fitness[0]) + + await self._save_zeroth_generation(self.__last_generation) + else: + self.__initial_population = initial_population + self.__last_generation = None + self.__generation_index = None + + # set fitness type to None + # we will set it when evaluating the first generation later + self.__fitness_type = None + + # same with these database nodes + self.__db_rng_after_generation = None + self.__db_generations = None + self.__db_individuals = None + + @abstractmethod + async def _evaluate_generation( + self, genotypes: List[Genotype], database: Database, db_node: Node + ) -> List[Fitness]: + """ + Evaluate a genotype. + + :param genotypes: The genotypes to evaluate. Must not be altered. + :return: The fitness result. + """ + + @abstractmethod + def _select_parents( + self, generation: List[Individual[Genotype, Fitness]], num_parents: int + ) -> List[List[Individual[Genotype, Fitness]]]: + """ + Select groups of parents that will create offspring. + + :param population: The generation to select sets of parents from. Must not be altered. + :return: The selected sets of parents. + """ + + def _select_survivors( + self, + old_individuals: List[Individual[Genotype, Fitness]], + new_individuals: List[Individual[Genotype, Fitness]], + num_survivors: int, + ) -> List[Individual[Genotype, Fitness]]: + """ + Select survivors from a group of individuals. These will form the next generation. + + :param individuals: The individuals to choose from. + :param num_survivors: How many individuals should be selected. + :returns: The selected individuals. + """ + + @abstractmethod + def _crossover(self, parents: List[Genotype]) -> Genotype: + """ + Combine a set of genotypes into a new genotype. + + :param parents: The set of genotypes to combine. Must not be altered. + :return: The new genotype. + """ + + @abstractmethod + def _mutate(self, genotype: Genotype) -> Genotype: + """ + Apply mutation to an genotype to create a new genotype. + + :param genotype: The original genotype. Must not be altered. + :return: The new genotype. + """ + + @abstractmethod + def _must_do_next_gen(self) -> bool: + """ + Decide if the optimizer must do another generation. + :returns: True if it must. + """ + + async def run(self) -> None: + if self.__initial_population is not None: + # let user evaluate. use unsafe version because we don't know fitness type yet. + db_evaluation_branch = await self._prepare_db_evaluation() + fitness = await self._evaluate_generation( + self.__initial_population, self.__database, db_evaluation_branch + ) + + # assert user return value + assert type(fitness) == list + assert len(fitness) == len(self.__initial_population) + assert all(type(e) == type(fitness[0]) for e in fitness) + + # save fitness type so we can type check things later + self.__fitness_type = type(fitness[0]) + + # combine provided genotypes and new fitness to create the first generation + self.__last_generation = [ + Individual[Genotype, Fitness]( + self._get_next_id(), genotype, fitness, None + ) + for genotype, fitness in zip(self.__initial_population, fitness) + ] + self.__generation_index = 0 + self.__initial_population = None + + await self._save_zeroth_generation(self.__last_generation) + + logging.info("Finished generation 0.") + + while self._safe_must_do_next_gen(): + assert self.__generation_index is not None + assert self.__last_generation is not None + + # let user select parents + parent_selections = self._safe_select_parents( + self.__last_generation, self.__offspring_size + ) + + # let user create offspring + offspring = [ + self._safe_mutate(self._safe_crossover(genotype)) + for genotype in [[p.genotype for p in s] for s in parent_selections] + ] + + # let user evaluate offspring + db_evaluation_branch = await self._prepare_db_evaluation() + fitness = await self._safe_evaluate_generation( + offspring, self.__database, db_evaluation_branch + ) + + # combine to create list of individuals + new_individuals = [ + Individual[Genotype, Fitness]( + self._get_next_id(), + genotype, + fitness, + [parent.id for parent in parents], + ) + for parents, genotype, fitness in zip( + parent_selections, offspring, fitness + ) + ] + + # let user select survivors between old and new individuals + survivors = self._safe_select_survivors( + self.__last_generation, new_individuals, self.__population_size + ) + + # set survivors as the next generation + self.__last_generation = survivors + self.__generation_index += 1 + + await self._save_generation(new_individuals) + + logging.info(f"Finished generation {self.__generation_index}.") + + def _get_next_id(self) -> int: + next_id = self.__next_id + self.__next_id += 1 + return next_id + + async def _save_zeroth_generation( + self, new_individuals: List[Individual[Genotype, Fitness]] + ) -> None: + """ + Save parameters to the database and do `_save_generation` for the initial population. + """ + + with self.__database.begin_transaction() as txn: + self.__db_rng_after_generation = DbList() + self.__db_generations = DbList() + self.__db_individuals = DbList() + + self.__db_ea.set_object( + txn, + { + "rng_after_generation": self.__db_rng_after_generation, + "genotype_type": pickle.dumps(self.__genotype_type), + "fitness_type": pickle.dumps(self.__fitness_type), + "population_size": self.__population_size, + "offspring_size": self.__offspring_size, + "generations": self.__db_generations, + "individuals": self.__db_individuals, + }, + ) + + await self._save_generation_notransaction(txn, new_individuals) + + async def _save_generation( + self, new_individuals: List[Individual[Genotype, Fitness]] + ) -> None: + """ + Append the last generation and accompanying random object to the checkpoint database. + + :param new_individuals: Individuals borns during last generation + """ + + with self.__database.begin_transaction() as txn: + await self._save_generation_notransaction(txn, new_individuals) + + async def _save_generation_notransaction( + self, txn: Transaction, new_individuals: List[Individual[Genotype, Fitness]] + ): + assert self.__last_generation is not None + assert self.__db_rng_after_generation is not None + assert self.__db_generations is not None + assert self.__db_individuals is not None + + logging.debug("Beginning saving generation..") + + self.__db_rng_after_generation.append(txn).set_object( + txn, pickle.dumps(self._rng.getstate()) + ) + self.__db_generations.append(txn).set_object( + txn, [individual.id for individual in self.__last_generation] + ) + for individual in new_individuals: + self.__db_individuals.append(txn).set_object(txn, individual.serialize()) + + logging.debug("Finished saving generation.") + + async def _load_checkpoint_or_init(self) -> bool: + """ + Deserialize from the database. If no checkpoint was found, the basic database structure is created. + + :returns: True if checkpoint could be loaded and everything is initialized from the database. + False if there was no checkpoint. + :raises SerializeError: If the database is in an incompatible state and the optimizer cannot continue. + Can leave this class in a partially deserialized state. + """ + + with self.__database.begin_transaction() as txn: + root = self.__db_node.get_object(txn) + + try: + if isinstance(root, Uninitialized): + self.__db_ea = Node() + self.__db_evaluations = DbList() + self.__db_node.set_object( + txn, {"evaluations": self.__db_evaluations, "ea": self.__db_ea} + ) + else: + self.__db_ea = root["ea"] + if not isinstance(self.__db_ea, Node): + raise SerializeError() + self.__db_evaluations = root["evaluations"].get_object(txn) + if not isinstance(self.__db_evaluations, DbList): + raise SerializeError() + + ea = self.__db_ea.get_object(txn) + if isinstance(ea, Uninitialized): + return False + + self.__db_rng_after_generation = ea[ + "rng_after_generation" + ].get_object(txn) + if not isinstance(self.__db_rng_after_generation, DbList): + raise SerializeError() + self.__population_size = ea["population_size"] + if not isinstance(self.__population_size, int): + raise SerializeError() + self.__offspring_size = ea["offspring_size"] + if not isinstance(self.__offspring_size, int): + raise SerializeError() + self.__db_generations = ea["generations"].get_object(txn) + if not isinstance(self.__db_generations, DbList): + raise SerializeError() + self.__db_individuals = ea["individuals"].get_object(txn) + if not isinstance(self.__db_individuals, DbList): + raise SerializeError() + + self.__genotype_type = pickle.loads(ea["genotype_type"]) + self.__fitness_type = pickle.loads(ea["fitness_type"]) + + self.__generation_index = ( + self.__db_generations.len(txn) - 1 + ) # first generation is 0 + + individual_ids = self.__db_generations.get( + txn, self.__generation_index + ).get_object(txn) + if not isinstance(individual_ids, list): + raise SerializeError() + self.__last_generation = [ + Individual.deserialize( + self.__db_individuals.get(txn, id).get_object(txn) + ) + for id in individual_ids + ] + self.__next_id = self.__db_individuals.len(txn) + + x = self.__db_rng_after_generation.get( + txn, self.__generation_index + ).get_object(txn) + + self._rng.setstate( + pickle.loads( + self.__db_rng_after_generation.get( + txn, self.__generation_index + ).get_object(txn) + ) + ) + + self.__initial_population = None + + return True + + except (SerializeError, pickle.PickleError, KeyError, TypeError) as err: + raise SerializeError( + "Database in state incompatible with this code. Remove database before trying again." + ) from err + + async def _prepare_db_evaluation(self) -> Node: + if self.__generation_index is None: + generation = 0 + else: + generation = self.__generation_index + 1 + + with self.__database.begin_transaction() as txn: + return self.__db_evaluations.get_or_append(txn, generation) + + def _safe_select_parents( + self, generation: List[Individual[Genotype, Fitness]], num_parents: int + ) -> List[List[Individual[Genotype, Fitness]]]: + parent_selections = self._select_parents(generation, num_parents) + assert type(parent_selections) == list + assert all(type(s) == list for s in parent_selections) + assert all([all(type(p) == Individual for p in s) for s in parent_selections]) + return parent_selections + + def _safe_crossover(self, parents: List[Genotype]) -> Genotype: + genotype = self._crossover(parents) + assert type(genotype) == self.__genotype_type + return genotype + + def _safe_mutate(self, genotype: Genotype) -> Genotype: + genotype = self._mutate(genotype) + assert type(genotype) == self.__genotype_type + return genotype + + async def _safe_evaluate_generation( + self, genotypes: List[Genotype], database: Database, db_node: Node + ) -> List[Fitness]: + fitnesss = await self._evaluate_generation(genotypes, database, db_node) + assert type(fitnesss) == list + assert len(fitnesss) == len(genotypes) + assert all(type(e) == self.__fitness_type for e in fitnesss) + return fitnesss + + def _safe_select_survivors( + self, + old_individuals: List[Individual[Genotype, Fitness]], + new_individuals: List[Individual[Genotype, Fitness]], + num_survivors: int, + ) -> List[Individual[Genotype, Fitness]]: + survivors = self._select_survivors( + old_individuals, new_individuals, num_survivors + ) + assert type(survivors) == list + assert len(survivors) == self.__population_size + assert all(type(s) == Individual for s in survivors) + return survivors + + def _safe_must_do_next_gen(self) -> bool: + must_do = self._must_do_next_gen() + assert type(must_do) == bool + return must_do + + @property + def population_size(self) -> int: + return self.__population_size + + @property + def offspring_size(self) -> int: + return self.__offspring_size + + @property + def generation_index(self) -> Optional[int]: + """ + Get the current generation. + The initial generation is numbered 0. + """ + + return self.__generation_index + + @property + def last_generation(self) -> Optional[List[Individual[Genotype, Fitness]]]: + """ + Get the last generation. + """ + + return self.__last_generation diff --git a/core/revolve2/core/optimization/ea/individual.py b/core/revolve2/core/optimization/ea/individual.py new file mode 100644 index 000000000..ab2fd9c36 --- /dev/null +++ b/core/revolve2/core/optimization/ea/individual.py @@ -0,0 +1,76 @@ +from __future__ import annotations + +import pickle +from dataclasses import dataclass +from typing import Generic, List, Optional, TypeVar, Union, cast + +from revolve2.core.database import StaticData +from revolve2.core.database.serialize import ( + Serializable, + SerializeError, + deserialize, + serialize, +) + +Genotype = TypeVar("Genotype", bound=Union[Serializable, StaticData]) +Fitness = TypeVar("Fitness", bound=Union[Serializable, StaticData]) + + +@dataclass +class Individual(Generic[Genotype, Fitness], Serializable): + id: int + genotype: Genotype + fitness: Fitness + parent_ids: Optional[List[int]] # None means this is from the initial population + + def serialize(self) -> StaticData: + return { + "id": self.id, + ".genotype_type": pickle.dumps(type(self.genotype)), + "genotype": serialize(self.genotype), + ".fitness_type": pickle.dumps(type(self.fitness)), + "fitness": serialize(self.fitness), + "parents": self.parent_ids, + } + + @classmethod + def deserialize(cls, data: StaticData) -> Individual[Genotype, Fitness]: + if type(data) != dict: + raise SerializeError() + + id_data = data.get("id") + if id_data is None or type(id_data) != int: + raise SerializeError() + id = cast(int, id_data) + + genotype_type_data = data.get(".genotype_type") + if genotype_type_data is None or type(genotype_type_data) != bytes: + raise SerializeError() + genotype_type = pickle.loads(genotype_type_data) # TODO catch error + if "genotype" not in data: + raise SerializeError() + genotype_data = data.get("genotype") + genotype = deserialize(genotype_data, genotype_type) + + fitness_type_data = data.get(".fitness_type") + if fitness_type_data is None or type(fitness_type_data) != bytes: + raise SerializeError() + fitness_type = pickle.loads(fitness_type_data) # TODO catch error + if "fitness" not in data: + raise SerializeError() + fitness_data = data.get("fitness") + fitness = deserialize(fitness_data, fitness_type) + + parents_data = data.get("parents") + if parents_data is None: + parent_ids = None + else: + if type(parents_data) != list: + raise SerializeError() + parent_ids: List[int] = [] + for parent_data in parents_data: + if type(parent_data) != int: + raise SerializeError() + parent_ids.append(parent_data) + + return Individual(id, genotype, fitness, parent_ids) diff --git a/core/revolve2/core/optimization/ea/modular_robot/__init__.py b/core/revolve2/core/optimization/ea/modular_robot/__init__.py new file mode 100644 index 000000000..0a5590951 --- /dev/null +++ b/core/revolve2/core/optimization/ea/modular_robot/__init__.py @@ -0,0 +1,3 @@ +from .body_genotype import BodyGenotype +from .bodybrain_genotype import BodybrainGenotype +from .brain_genotype import BrainGenotype diff --git a/core/revolve2/core/optimization/ea/modular_robot/body_genotype.py b/core/revolve2/core/optimization/ea/modular_robot/body_genotype.py new file mode 100644 index 000000000..64eb00fae --- /dev/null +++ b/core/revolve2/core/optimization/ea/modular_robot/body_genotype.py @@ -0,0 +1,9 @@ +from abc import ABC, abstractmethod + +from revolve2.core.modular_robot import Body as ModularRobotBody + + +class BodyGenotype(ABC): + @abstractmethod + def develop(self) -> ModularRobotBody: + pass diff --git a/core/revolve2/core/optimization/ea/modular_robot/bodybrain_genotype.py b/core/revolve2/core/optimization/ea/modular_robot/bodybrain_genotype.py new file mode 100644 index 000000000..de2815df8 --- /dev/null +++ b/core/revolve2/core/optimization/ea/modular_robot/bodybrain_genotype.py @@ -0,0 +1,25 @@ +from typing import Generic, TypeVar + +from revolve2.core.modular_robot import ModularRobot + +from .body_genotype import BodyGenotype +from .brain_genotype import BrainGenotype + +BodyGenotypeImpl = TypeVar("BodyGenotypeImpl", bound=BodyGenotype) +BrainGenotypeImpl = TypeVar("BrainGenotypeImpl", bound=BrainGenotype) + + +class BodybrainGenotype(Generic[BodyGenotypeImpl, BrainGenotypeImpl]): + _body_genotype: BodyGenotypeImpl + _brain_genotype: BrainGenotypeImpl + + def __init__( + self, body_genotype: BodyGenotypeImpl, brain_genotype: BrainGenotypeImpl + ): + self._body_genotype = body_genotype + self._brain_genotype = brain_genotype + + def develop(self) -> ModularRobot: + body = self._body_genotype.develop() + brain = self._brain_genotype.develop(body) + return ModularRobot(body, brain) diff --git a/core/revolve2/core/optimization/ea/modular_robot/brain_genotype.py b/core/revolve2/core/optimization/ea/modular_robot/brain_genotype.py new file mode 100644 index 000000000..029927b07 --- /dev/null +++ b/core/revolve2/core/optimization/ea/modular_robot/brain_genotype.py @@ -0,0 +1,10 @@ +from abc import ABC, abstractmethod + +from revolve2.core.modular_robot import Body as ModularRobotBody +from revolve2.core.modular_robot import Brain as ModularRobotBrain + + +class BrainGenotype(ABC): + @abstractmethod + def develop(self, body: ModularRobotBody) -> ModularRobotBrain: + pass diff --git a/core/revolve2/core/optimization/ea/population_management/__init__.py b/core/revolve2/core/optimization/ea/population_management/__init__.py new file mode 100644 index 000000000..8e32ec5ef --- /dev/null +++ b/core/revolve2/core/optimization/ea/population_management/__init__.py @@ -0,0 +1,2 @@ +from .generational import generational +from .steady_state import steady_state diff --git a/core/revolve2/core/optimization/ea/population_management/generational.py b/core/revolve2/core/optimization/ea/population_management/generational.py new file mode 100644 index 000000000..9e21ea45a --- /dev/null +++ b/core/revolve2/core/optimization/ea/population_management/generational.py @@ -0,0 +1,27 @@ +from typing import Callable, List, TypeVar + +from .. import selection + +Individual = TypeVar("Individual") + + +def generational( + old_individuals: List[Individual], + new_individuals: List[Individual], + selection_function: Callable[[List[Individual]], Individual], +) -> List[Individual]: + """ + Select n unique individuals from only the new individuals + using the provided selection function, + where n is the number of old individuals. + Also known as mu,lambda. + """ + + population_size = len(old_individuals) + selection_pool = new_individuals + + assert len(selection_pool) >= population_size + + return selection.multiple_unique( + new_individuals, population_size, selection_function + ) diff --git a/core/revolve2/core/optimization/ea/population_management/steady_state.py b/core/revolve2/core/optimization/ea/population_management/steady_state.py new file mode 100644 index 000000000..459b1124b --- /dev/null +++ b/core/revolve2/core/optimization/ea/population_management/steady_state.py @@ -0,0 +1,25 @@ +from typing import Callable, List, TypeVar + +from .. import selection + +Individual = TypeVar("Individual") + + +def steady_state( + old_individuals: List[Individual], + new_individuals: List[Individual], + selection_function: Callable[[List[Individual]], Individual], +) -> List[Individual]: + """ + Select n unique individuals from the combined set of old and new individuals + using the provided selection function, + where n is the number of old individuals. + Also known as mu+lambda. + """ + + population_size = len(old_individuals) + selection_pool = old_individuals + new_individuals + + return selection.multiple_unique( + selection_pool, population_size, selection_function + ) diff --git a/core/revolve2/core/optimization/ea/selection/__init__.py b/core/revolve2/core/optimization/ea/selection/__init__.py new file mode 100644 index 000000000..aee7dafb2 --- /dev/null +++ b/core/revolve2/core/optimization/ea/selection/__init__.py @@ -0,0 +1,2 @@ +from .multiple_unique import multiple_unique +from .tournament import tournament diff --git a/core/revolve2/core/optimization/ea/selection/multiple_unique.py b/core/revolve2/core/optimization/ea/selection/multiple_unique.py new file mode 100644 index 000000000..e72d5b6d9 --- /dev/null +++ b/core/revolve2/core/optimization/ea/selection/multiple_unique.py @@ -0,0 +1,32 @@ +from typing import Callable, List, TypeVar + +Individual = TypeVar("Individual") + + +def multiple_unique( + population: List[Individual], + selection_size: int, + selection_function: Callable[[List[Individual]], Individual], +) -> List[Individual]: + """ + Perform selection on population of distinct group, it can be used in the + form parent selection or survival selection. + It never selects the same individual more than once + :param population: list of individuals where to select from + :param selection_size: amount of individuals to select + :param selection_function: + """ + if len(population) < selection_size: + assert ( + len(population) >= selection_size + ), f"Population size({len(population)}) cannot be smaller than selection size({selection_size})" + + selected_individuals = [] + for _ in range(selection_size): + new_individual = False + while new_individual is False: + selected_individual = selection_function(population) + if selected_individual not in selected_individuals: + selected_individuals.append(selected_individual) + new_individual = True + return selected_individuals diff --git a/core/revolve2/core/optimization/ea/selection/tournament.py b/core/revolve2/core/optimization/ea/selection/tournament.py new file mode 100644 index 000000000..e68ef7526 --- /dev/null +++ b/core/revolve2/core/optimization/ea/selection/tournament.py @@ -0,0 +1,24 @@ +from random import Random +from typing import TYPE_CHECKING, List, Tuple, TypeVar + +if TYPE_CHECKING: + from _typeshed import SupportsLessThan + +Individual = TypeVar("Individual") +Fitness = TypeVar("Fitness", bound="SupportsLessThan") + + +def tournament( + rng: Random, population: List[Tuple[Individual, Fitness]], k +) -> Tuple[Individual, Fitness]: + """ + Perform tournament selection and return best individual + + :param rng: random number generator + :param population: list of individuals where to select from + :param k: amount of individuals to participate in tournament + """ + assert len(population) >= k + + participants = rng.choices(population=population, k=k) + return max(participants, key=lambda p: p[1]) diff --git a/core/revolve2/core/physics/actor/__init__.py b/core/revolve2/core/physics/actor/__init__.py new file mode 100644 index 000000000..80fc5abfc --- /dev/null +++ b/core/revolve2/core/physics/actor/__init__.py @@ -0,0 +1,5 @@ +from .actor import Actor +from .collision import Collision +from .joint import Joint +from .rigid_body import RigidBody +from .visual import Visual diff --git a/core/revolve2/core/physics/actor/actor.py b/core/revolve2/core/physics/actor/actor.py new file mode 100644 index 000000000..38c69bc85 --- /dev/null +++ b/core/revolve2/core/physics/actor/actor.py @@ -0,0 +1,144 @@ +from dataclasses import dataclass +from typing import List, Tuple + +from pyrr import Quaternion, Vector3 + +from .bounding_box import BoundingBox +from .joint import Joint +from .rigid_body import RigidBody + + +@dataclass +class Actor: + bodies: List[RigidBody] + joints: List[Joint] + + def calc_aabb(self) -> BoundingBox: + """ + Calculate the axis aligned bounding box for this actor. + This not the exact bounding box for the actor, + but the smallest box the actor fits in that is aligned + with the global axes. + + Not very efficient but it works and is fast enough for our use case + """ + xmin = 0 + xmax = 0 + ymin = 0 + ymax = 0 + zmin = 0 + zmax = 0 + + for body in self.bodies: + for collision in body.collisions: + box = _Box( + ( + Vector3( + [ + -collision.bounding_box.x, + -collision.bounding_box.y, + -collision.bounding_box.z, + ] + ) + / 2.0, + Vector3( + [ + collision.bounding_box.x, + -collision.bounding_box.y, + -collision.bounding_box.z, + ] + ) + / 2.0, + Vector3( + [ + -collision.bounding_box.x, + collision.bounding_box.y, + -collision.bounding_box.z, + ] + ) + / 2.0, + Vector3( + [ + collision.bounding_box.x, + collision.bounding_box.y, + -collision.bounding_box.z, + ] + ) + / 2.0, + Vector3( + [ + -collision.bounding_box.x, + -collision.bounding_box.y, + collision.bounding_box.z, + ] + ) + / 2.0, + Vector3( + [ + collision.bounding_box.x, + -collision.bounding_box.y, + collision.bounding_box.z, + ] + ) + / 2.0, + Vector3( + [ + -collision.bounding_box.x, + collision.bounding_box.y, + collision.bounding_box.z, + ] + ) + / 2.0, + Vector3( + [ + collision.bounding_box.x, + collision.bounding_box.y, + collision.bounding_box.z, + ] + ) + / 2.0, + ), + ) + box.rotate(collision.orientation) + box.translate(collision.position) + box.rotate(body.orientation) + box.translate(body.position) + + aabb = box.aabb() + xmax = max(xmax, aabb.offset.x + aabb.size.x / 2.0) + ymax = max(ymax, aabb.offset.y + aabb.size.y / 2.0) + zmax = max(zmax, aabb.offset.z + aabb.size.z / 2.0) + xmin = min(xmin, aabb.offset.x - aabb.size.x / 2.0) + ymin = min(ymin, aabb.offset.y - aabb.size.y / 2.0) + zmin = min(zmin, aabb.offset.z - aabb.size.z / 2.0) + + return BoundingBox( + Vector3([xmax - xmin, ymax - ymin, zmax - zmin]), + Vector3([(xmax + xmin) / 2.0, (ymax + ymin) / 2.0, (zmax + zmin) / 2.0]), + ) + + +@dataclass +class _Box: + # The 8 coordinates of the box. Order is irrelevant. + coordinates: Tuple[ + Vector3, Vector3, Vector3, Vector3, Vector3, Vector3, Vector3, Vector3 + ] + + def rotate(self, rotation: Quaternion) -> None: + self.coordinates = [rotation * coord for coord in self.coordinates] + + def translate(self, offset: Vector3) -> None: + self.coordinates = [coord + offset for coord in self.coordinates] + + def aabb(self) -> BoundingBox: + xmax = max([coord.x for coord in self.coordinates]) + ymax = max([coord.y for coord in self.coordinates]) + zmax = max([coord.z for coord in self.coordinates]) + xmin = min([coord.x for coord in self.coordinates]) + ymin = min([coord.y for coord in self.coordinates]) + zmin = min([coord.z for coord in self.coordinates]) + return BoundingBox( + Vector3([xmax - xmin, ymax - ymin, zmax - zmin]), + Vector3([(xmax + xmin) / 2.0, (ymax + ymin) / 2.0, (zmax + zmin) / 2.0]), + ) diff --git a/core/revolve2/core/physics/actor/bounding_box.py b/core/revolve2/core/physics/actor/bounding_box.py new file mode 100644 index 000000000..99b393d69 --- /dev/null +++ b/core/revolve2/core/physics/actor/bounding_box.py @@ -0,0 +1,9 @@ +from dataclasses import dataclass + +from pyrr import Vector3 + + +@dataclass +class BoundingBox: + size: Vector3 # size of each side. not half. + offset: Vector3 # offset from origin diff --git a/core/revolve2/core/physics/actor/collision.py b/core/revolve2/core/physics/actor/collision.py new file mode 100644 index 000000000..68307eed7 --- /dev/null +++ b/core/revolve2/core/physics/actor/collision.py @@ -0,0 +1,12 @@ +from dataclasses import dataclass + +from pyrr import Quaternion, Vector3 + + +@dataclass +class Collision: + name: str + position: Vector3 + orientation: Quaternion + mass: float + bounding_box: Vector3 # sizes of the lengths of the bounding box. not half. diff --git a/core/revolve2/core/physics/actor/joint.py b/core/revolve2/core/physics/actor/joint.py new file mode 100644 index 000000000..97a0a5bd5 --- /dev/null +++ b/core/revolve2/core/physics/actor/joint.py @@ -0,0 +1,15 @@ +from dataclasses import dataclass + +from pyrr import Quaternion, Vector3 + +from .rigid_body import RigidBody + + +@dataclass +class Joint: + name: str + body1: RigidBody + body2: RigidBody + position: Vector3 + orientation: Quaternion + axis: Vector3 diff --git a/core/revolve2/core/physics/actor/rigid_body.py b/core/revolve2/core/physics/actor/rigid_body.py new file mode 100644 index 000000000..08db794c9 --- /dev/null +++ b/core/revolve2/core/physics/actor/rigid_body.py @@ -0,0 +1,57 @@ +from dataclasses import dataclass, field +from typing import List + +from pyrr import Matrix33, Vector3 +from pyrr.objects.quaternion import Quaternion + +from .collision import Collision +from .visual import Visual + + +@dataclass +class RigidBody: + name: str + position: Vector3 + orientation: Quaternion + collisions: List[Collision] = field(default_factory=list, init=False) + visuals: List[Visual] = field(default_factory=list, init=False) + static_friction: float + dynamic_friction: float + + def mass(self) -> float: + return sum(collision.mass for collision in self.collisions) + + def center_of_mass(self) -> Vector3: + return ( + sum(collision.mass * collision.position for collision in self.collisions) + / self.mass() + ) + + def inertia_tensor(self) -> Matrix33: + com = self.center_of_mass() + inertia = Matrix33() + + for collision in self.collisions: + inertia[0][0] += collision.mass * ( + 1.0 + / 12.0 + * (collision.bounding_box.y ** 2 + collision.bounding_box.z ** 2) + + (collision.position.y - com.y) ** 2 + + (collision.position.z - com.z) ** 2 + ) + inertia[1][1] += collision.mass * ( + 1.0 + / 12.0 + * (collision.bounding_box.x ** 2 + collision.bounding_box.z ** 2) + + (collision.position.x - com.x) ** 2 + + (collision.position.z - com.z) ** 2 + ) + inertia[2][2] += collision.mass * ( + 1.0 + / 12.0 + * (collision.bounding_box.x ** 2 + collision.bounding_box.y ** 2) + + (collision.position.x - com.x) ** 2 + + (collision.position.y - com.y) ** 2 + ) + + return inertia diff --git a/core/revolve2/core/physics/actor/sdf/__init__.py b/core/revolve2/core/physics/actor/sdf/__init__.py new file mode 100644 index 000000000..18005da0a --- /dev/null +++ b/core/revolve2/core/physics/actor/sdf/__init__.py @@ -0,0 +1 @@ +from .to_sdf import to_sdf as to_sdf diff --git a/core/revolve2/core/physics/actor/sdf/to_sdf.py b/core/revolve2/core/physics/actor/sdf/to_sdf.py new file mode 100644 index 000000000..87e5fe0e9 --- /dev/null +++ b/core/revolve2/core/physics/actor/sdf/to_sdf.py @@ -0,0 +1,168 @@ +import xml.dom.minidom as minidom +import xml.etree.ElementTree as xml +from typing import Tuple, cast + +from pyrr import Quaternion, Vector3 + +from ..actor import Actor + + +def to_sdf( + physics_robot: Actor, + model_name: str, + position: Vector3, + orientation: Quaternion, +) -> str: + sdf = xml.Element("sdf", {"version": "1.6"}) + model = xml.SubElement(sdf, "model", {"name": model_name}) + model.append(_make_pose(position, orientation)) + + for body in physics_robot.bodies: + link = xml.SubElement(model, "link", {"name": body.name}) + link.append(_make_pose(body.position, body.orientation)) + xml.SubElement(link, "self_collide").text = "True" + + for collision in body.collisions: + link.append( + _make_box_collision( + collision.name, + collision.position, + collision.orientation, + collision.bounding_box, + ) + ) + + for visual in body.visuals: + link.append( + _make_visual( + visual.name, + visual.position, + visual.orientation, + visual.color, + visual.model, + ) + ) + + mass = body.mass() + + inertia = body.inertia_tensor() + center_of_mass = body.center_of_mass() + + inertial = xml.SubElement(link, "inertial") + inertial.append(_make_pose(center_of_mass, Quaternion())) + xml.SubElement(inertial, "mass").text = "{:e}".format(mass) + inertia_el = xml.SubElement(inertial, "inertia") + xml.SubElement(inertia_el, "ixx").text = "{:e}".format(inertia[0][0]) + xml.SubElement(inertia_el, "ixy").text = "{:e}".format(inertia[0][1]) + xml.SubElement(inertia_el, "ixz").text = "{:e}".format(inertia[0][2]) + xml.SubElement(inertia_el, "iyy").text = "{:e}".format(inertia[1][1]) + xml.SubElement(inertia_el, "iyz").text = "{:e}".format(inertia[1][2]) + xml.SubElement(inertia_el, "izz").text = "{:e}".format(inertia[2][2]) + + for joint in physics_robot.joints: + el = xml.SubElement( + model, + "joint", + {"name": f"{joint.name}", "type": "revolute"}, + ) + el.append( + _make_pose( + joint.body2.orientation.inverse + * (joint.position - joint.body2.position), + joint.body2.orientation.inverse * joint.orientation, + ) # joint position & orientation are relative to child frame + ) + xml.SubElement(el, "parent").text = joint.body1.name + xml.SubElement(el, "child").text = joint.body2.name + axis = xml.SubElement(el, "axis") + xml.SubElement(axis, "xyz").text = "{:e} {:e} {:e}".format(0.0, 1.0, 0.0) + xml.SubElement(axis, "use_parent_model_frame").text = "0" + limit = xml.SubElement(axis, "limit") + xml.SubElement(limit, "lower").text = "-7.853982e-01" + xml.SubElement(limit, "upper").text = "7.853982e-01" + xml.SubElement(limit, "effort").text = "1.765800e-01" + xml.SubElement(limit, "velocity").text = "5.235988e+00" + + return minidom.parseString( + xml.tostring(sdf, encoding="unicode", method="xml") + ).toprettyxml(indent=" ") + + +def _quaternion_to_euler(quaternion: Quaternion) -> Tuple[float, float, float]: + import warnings + + from scipy.spatial.transform import Rotation + + with warnings.catch_warnings(): + warnings.simplefilter( + "ignore", UserWarning + ) # ignore gimbal lock warning. it is irrelevant for us. + euler = Rotation.from_quat( + [quaternion.x, quaternion.y, quaternion.z, quaternion.w] + ).as_euler("xyz") + + return (cast(float, euler[0]), cast(float, euler[1]), cast(float, euler[2])) + + +def _make_pose(position: Vector3, orientation: Quaternion) -> xml.Element: + pose = xml.Element("pose") + pose.text = "{:e} {:e} {:e} {:e} {:e} {:e}".format( + *position, + *_quaternion_to_euler(orientation), + ) + return pose + + +def _make_visual( + name: str, + position: Vector3, + orientation: Quaternion, + color: Tuple[float, float, float], + model: str, +): + visual = xml.Element("visual", {"name": name}) + visual.append(_make_pose(position, orientation)) + + material = xml.SubElement(visual, "material") + xml.SubElement(material, "ambient").text = f"{color[0]} {color[1]} {color[2]} {1}" + xml.SubElement(material, "diffuse").text = f"{color[0]} {color[1]} {color[2]} {1}" + xml.SubElement(material, "specular").text = "0.1 0.1 0.1 1.0" + + geometry = xml.SubElement(visual, "geometry") + mesh = xml.SubElement(geometry, "mesh") + xml.SubElement(mesh, "uri").text = model + + return visual + + +def _make_box_collision( + name: str, + position: Vector3, + orientation: Quaternion, + box_size: Vector3, +): + collision = xml.Element("collision", {"name": name}) + collision.append(_make_pose(position, orientation)) + + surface = xml.SubElement(collision, "surface") + + contact = xml.SubElement(surface, "contact") + ode = xml.SubElement(contact, "ode") + xml.SubElement(ode, "kd").text = "{:e}".format(10000000.0 / 3.0) + xml.SubElement(ode, "kp").text = "{:e}".format(90000) + + friction = xml.SubElement(surface, "friction") + ode = xml.SubElement(friction, "ode") + xml.SubElement(ode, "mu").text = "{:e}".format(1.0) + xml.SubElement(ode, "mu2").text = "{:e}".format(1.0) + xml.SubElement(ode, "slip1").text = "{:e}".format(0.01) + xml.SubElement(ode, "slip2").text = "{:e}".format(0.01) + bullet = xml.SubElement(friction, "bullet") + xml.SubElement(bullet, "friction").text = "{:e}".format(1.0) + xml.SubElement(bullet, "friction2").text = "{:e}".format(1.0) + + geometry = xml.SubElement(collision, "geometry") + box = xml.SubElement(geometry, "box") + xml.SubElement(box, "size").text = "{:e} {:e} {:e}".format(*box_size) + + return collision diff --git a/core/revolve2/core/physics/actor/urdf/__init__.py b/core/revolve2/core/physics/actor/urdf/__init__.py new file mode 100644 index 000000000..fa7f3ef9c --- /dev/null +++ b/core/revolve2/core/physics/actor/urdf/__init__.py @@ -0,0 +1 @@ +from .to_urdf import to_urdf as to_urdf diff --git a/core/revolve2/core/physics/actor/urdf/to_urdf.py b/core/revolve2/core/physics/actor/urdf/to_urdf.py new file mode 100644 index 000000000..16c8b2000 --- /dev/null +++ b/core/revolve2/core/physics/actor/urdf/to_urdf.py @@ -0,0 +1,177 @@ +import xml.dom.minidom as minidom +import xml.etree.ElementTree as xml +from typing import Dict, List, Optional, Tuple, cast + +from pyrr import Quaternion, Vector3 + +from ..actor import Actor +from ..joint import Joint +from ..rigid_body import RigidBody + + +def to_urdf( + physics_robot: Actor, + name: str, + position: Vector3, + orientation: Quaternion, +) -> str: + urdf = xml.Element("robot", {"name": name}) + + tree: Dict[str, List[Joint]] = {} # parent to children + seen_children = set() + for joint in physics_robot.joints: + if joint.body2.name in seen_children: + raise RuntimeError( + "Physics robot cannot be converted to urdf. Cannot be represented as tree structure. One or more joints has multiple parents." + ) + seen_children.add(joint.body2.name) + if joint.body1.name not in tree: + tree[joint.body1.name] = [] + tree[joint.body1.name].append(joint) + + root: Optional[RigidBody] = None + for body in physics_robot.bodies: + if body.name not in seen_children: + if root is not None: + raise RuntimeError( + "Physics robot cannot be converted to urdf. Cannot be represented as tree structure. Some parts of the robot are detached; no single root body." + ) + root = body + if root is None: + raise RuntimeError( + "Physics robot cannot be converted to urdf. Require at least one body." + ) + + for el in _make_node(root, tree, position, orientation): + urdf.append(el) + + return minidom.parseString( + xml.tostring(urdf, encoding="unicode", method="xml") + ).toprettyxml(indent=" ") + + +def _make_node( + node: RigidBody, + tree: Dict[str, List[Joint]], + joint_pos: Vector3, + joint_ori: Quaternion, +) -> List[xml.Element]: + elements = [] + + link = xml.Element("link", {"name": node.name}) + elements.append(link) + + com_xyz = joint_ori.inverse * (node.position + node.center_of_mass() - joint_pos) + com_rpy = _quaternion_to_euler(joint_ori.inverse * node.orientation) + inertia = node.inertia_tensor() + + inertial = xml.SubElement(link, "inertial") + xml.SubElement( + inertial, + "origin", + { + "rpy": f"{com_rpy[0]} {com_rpy[1]} {com_rpy[2]}", + "xyz": f"{com_xyz[0]} {com_xyz[1]} {com_xyz[2]}", + }, + ) + xml.SubElement(inertial, "mass", {"value": "{:e}".format(node.mass())}) + xml.SubElement( + inertial, + "inertia", + { + "ixx": "{:e}".format(inertia[0][0]), + "ixy": "{:e}".format(inertia[0][1]), + "ixz": "{:e}".format(inertia[0][2]), + "iyy": "{:e}".format(inertia[1][1]), + "iyz": "{:e}".format(inertia[1][2]), + "izz": "{:e}".format(inertia[2][2]), + }, + ) + + for collision in node.collisions: + el = xml.SubElement(link, "collision") + geometry = xml.SubElement(el, "geometry") + xml.SubElement( + geometry, + "box", + { + "size": f"{collision.bounding_box[0]} {collision.bounding_box[1]} {collision.bounding_box[2]}" + }, + ) + xyz = joint_ori.inverse * ( + node.position - joint_pos + node.orientation * collision.position + ) + rpy = _quaternion_to_euler( + joint_ori.inverse * node.orientation * collision.orientation + ) + xml.SubElement( + el, + "origin", + { + "rpy": f"{rpy[0]} {rpy[1]} {rpy[2]}", + "xyz": f"{xyz[0]} {xyz[1]} {xyz[2]}", + }, + ) + + if node.name in tree: + for joint in tree[node.name]: + el = xml.Element("joint", name=joint.name, type="revolute") + xml.SubElement(el, "parent", {"link": node.name}) + xml.SubElement(el, "child", {"link": joint.body2.name}) + xyz = joint_ori.inverse * (joint.position - joint_pos) + rpy = _quaternion_to_euler(joint_ori.inverse * joint.orientation) + xml.SubElement( + el, + "origin", + { + "rpy": f"{rpy[0]} {rpy[1]} {rpy[2]}", + "xyz": f"{xyz[0]} {xyz[1]} {xyz[2]}", + }, + ) + xml.SubElement(el, "axis", {"xyz": "0 1 0"}) + xml.SubElement( + el, + "limit", + { # TODO move this to Actor as parameters + # 60 degrees to each side + "lower": "-1.047197551", + "upper": "1.047197551", + # motor specs: 9.4 kgfcm at 4.8V or 11 kgfcm at 6.0V + # about 9.6667 kgfcm at 5.0V, our operating voltage + # 9.6667 * 9.807 / 100 + "effort": "0.948013269", + # motor specs: 0.17 s/60deg at 4.8V or 0.14 s/60deg at 6.0V + # about 0.1652 s/60deg at 5.0V, our operating voltage + # 1 / 0.1652 * 60 / 360 * 2pi + "velocity": "6.338968228", + }, + ) + elements.append(el) + elements += _make_node( + joint.body2, + tree, + joint.position, + joint.orientation, + ) + + # visual = xml.SubElement(link, "visual") + # geometry = xml.SubElement(visual, "geometry") + # xml.SubElement(geometry, "box", {"size": "1 1 1"}) + + return elements + + +def _quaternion_to_euler(quaternion: Quaternion) -> Tuple[float, float, float]: + import warnings + + from scipy.spatial.transform import Rotation + + with warnings.catch_warnings(): + warnings.simplefilter( + "ignore", UserWarning + ) # ignore gimbal lock warning. it is irrelevant for us. + euler = Rotation.from_quat( + [quaternion.x, quaternion.y, quaternion.z, quaternion.w] + ).as_euler("xyz") + + return (cast(float, euler[0]), cast(float, euler[1]), cast(float, euler[2])) diff --git a/core/revolve2/core/physics/actor/visual.py b/core/revolve2/core/physics/actor/visual.py new file mode 100644 index 000000000..7ca292add --- /dev/null +++ b/core/revolve2/core/physics/actor/visual.py @@ -0,0 +1,13 @@ +from dataclasses import dataclass +from typing import Tuple + +from pyrr import Quaternion, Vector3 + + +@dataclass +class Visual: + name: str + position: Vector3 + orientation: Quaternion + model: str + color: Tuple[float, float, float] diff --git a/core/revolve2/core/physics/control/__init__.py b/core/revolve2/core/physics/control/__init__.py new file mode 100644 index 000000000..5caabe98c --- /dev/null +++ b/core/revolve2/core/physics/control/__init__.py @@ -0,0 +1 @@ +from .actor_controller import ActorController diff --git a/core/revolve2/core/physics/control/actor_controller.py b/core/revolve2/core/physics/control/actor_controller.py new file mode 100644 index 000000000..bef21d42c --- /dev/null +++ b/core/revolve2/core/physics/control/actor_controller.py @@ -0,0 +1,12 @@ +from abc import ABC, abstractmethod +from typing import List + + +class ActorController(ABC): + @abstractmethod + def step(self, dt: float) -> None: + pass + + @abstractmethod + def get_dof_targets(self) -> List[float]: + pass diff --git a/core/revolve2/core/physics/control/actor_controllers/__init__.py b/core/revolve2/core/physics/control/actor_controllers/__init__.py new file mode 100644 index 000000000..ea62f6adc --- /dev/null +++ b/core/revolve2/core/physics/control/actor_controllers/__init__.py @@ -0,0 +1 @@ +from .cpg import Cpg diff --git a/core/revolve2/core/physics/control/actor_controllers/cpg.py b/core/revolve2/core/physics/control/actor_controllers/cpg.py new file mode 100644 index 000000000..b27ca146f --- /dev/null +++ b/core/revolve2/core/physics/control/actor_controllers/cpg.py @@ -0,0 +1,45 @@ +from typing import List + +import numpy as np + +from ..actor_controller import ActorController + + +class Cpg(ActorController): + _weight_matrix: np.ndarray # nxn matrix matching number of neurons + _num_output_neurons: int + _state: np.ndarray + + def __init__( + self, + initial_state: np.ndarray, + num_output_neurons: int, + weight_matrix: np.ndarray, + ): + """ + First num_output_neurons will be dof targets + """ + assert initial_state.ndim == 1 + assert weight_matrix.ndim == 2 + assert weight_matrix.shape[0] == weight_matrix.shape[1] + assert initial_state.shape[0] == weight_matrix.shape[0] + + self._weight_matrix = weight_matrix + self._num_output_neurons = num_output_neurons + self._state = initial_state + + def step(self, dt: float) -> None: + self._state = self._rk45(self._state, self._weight_matrix, dt) + + @staticmethod + def _rk45(state, A, dt): + # TODO The scipy implementation of this function is very slow for some reason. + # investigate the performance and accuracy differences + A1 = np.matmul(A, state) + A2 = np.matmul(A, (state + dt / 2 * A1)) + A3 = np.matmul(A, (state + dt / 2 * A2)) + A4 = np.matmul(A, (state + dt * A3)) + return state + dt / 6 * (A1 + 2 * (A2 + A3) + A4) + + def get_dof_targets(self) -> List[float]: + return list(self._state[0 : self._num_output_neurons]) diff --git a/core/revolve2/core/physics/env/__init__.py b/core/revolve2/core/physics/env/__init__.py new file mode 100644 index 000000000..29c88aac3 --- /dev/null +++ b/core/revolve2/core/physics/env/__init__.py @@ -0,0 +1,6 @@ +from .actor_control import ActorControl +from .batch import Batch +from .environment import Environment +from .posed_actor import PosedActor +from .runner import Runner +from .state import ActorState, EnvironmentState, State diff --git a/core/revolve2/core/physics/env/actor_control.py b/core/revolve2/core/physics/env/actor_control.py new file mode 100644 index 000000000..fdf943d90 --- /dev/null +++ b/core/revolve2/core/physics/env/actor_control.py @@ -0,0 +1,14 @@ +from typing import List, Tuple + + +# TODO extend functionality and add error checking +class ActorControl: + _dof_targets: List[Tuple[int, int, List[float]]] + + def __init__(self) -> None: + self._dof_targets = [] + + def set_dof_targets( + self, environment: int, actor: int, targets: List[float] + ) -> None: + self._dof_targets.append((environment, actor, targets)) diff --git a/core/revolve2/core/physics/env/batch.py b/core/revolve2/core/physics/env/batch.py new file mode 100644 index 000000000..3ecb5ea75 --- /dev/null +++ b/core/revolve2/core/physics/env/batch.py @@ -0,0 +1,20 @@ +from dataclasses import dataclass, field +from typing import Callable, List + +from .actor_control import ActorControl +from .environment import Environment + + +@dataclass +class Batch: + simulation_time: int # seconds + + """ + Hz. Frequency for state sampling during the simulation. + The simulator will attempt to follow this as closely as possible, + but is dependent on the actual step frequency of the simulator. + """ + sampling_frequency: float + control_frequency: float # Hz. See `sampling_frequency`, but for actor control. + control: Callable[[float, ActorControl], None] # (dt, control) -> None + environments: List[Environment] = field(default_factory=list, init=False) diff --git a/core/revolve2/core/physics/env/environment.py b/core/revolve2/core/physics/env/environment.py new file mode 100644 index 000000000..bcaaeb872 --- /dev/null +++ b/core/revolve2/core/physics/env/environment.py @@ -0,0 +1,9 @@ +from dataclasses import dataclass, field +from typing import List + +from .posed_actor import PosedActor + + +@dataclass +class Environment: + actors: List[PosedActor] = field(default_factory=list, init=False) diff --git a/core/revolve2/core/physics/env/posed_actor.py b/core/revolve2/core/physics/env/posed_actor.py new file mode 100644 index 000000000..94c3e4606 --- /dev/null +++ b/core/revolve2/core/physics/env/posed_actor.py @@ -0,0 +1,12 @@ +from dataclasses import dataclass + +from pyrr import Quaternion, Vector3 + +from ..actor import Actor + + +@dataclass +class PosedActor: + actor: Actor + position: Vector3 + orientation: Quaternion diff --git a/core/revolve2/core/physics/env/runner.py b/core/revolve2/core/physics/env/runner.py new file mode 100644 index 000000000..8d2c3947a --- /dev/null +++ b/core/revolve2/core/physics/env/runner.py @@ -0,0 +1,11 @@ +from abc import ABC, abstractmethod +from typing import List, Tuple + +from .batch import Batch +from .state import State + + +class Runner(ABC): + @abstractmethod + async def run_batch(self, batch: Batch) -> List[Tuple[float, State]]: + pass diff --git a/core/revolve2/core/physics/env/state.py b/core/revolve2/core/physics/env/state.py new file mode 100644 index 000000000..3d6babe34 --- /dev/null +++ b/core/revolve2/core/physics/env/state.py @@ -0,0 +1,57 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import List + +from pyrr import Quaternion, Vector3 +from revolve2.core.database import StaticData +from revolve2.core.database.serialize import Serializable + + +@dataclass +class ActorState(Serializable): + position: Vector3 + orientation: Quaternion + + def serialize(self) -> StaticData: + return { + "position": [ + float(self.position.x), + float(self.position.y), + float(self.position.z), + ], + "orientation": [ + float(self.orientation.x), + float(self.orientation.y), + float(self.orientation.z), + float(self.orientation.w), + ], + } + + @classmethod + def deserialize(cls, data: StaticData) -> None: + raise NotImplementedError() + + +@dataclass +class EnvironmentState(Serializable): + actor_states: List[ActorState] + + def serialize(self) -> StaticData: + return [state.serialize() for state in self.actor_states] + + @classmethod + def deserialize(cls, data: StaticData) -> None: + raise NotImplementedError() + + +@dataclass +class State(Serializable): + envs: List[EnvironmentState] + + def serialize(self) -> StaticData: + return [env.serialize() for env in self.envs] + + @classmethod + def deserialize(cls, data: StaticData) -> None: + raise NotImplementedError() diff --git a/core/revolve2/py.typed b/core/revolve2/py.typed new file mode 100644 index 000000000..e69de29bb diff --git a/core/setup.py b/core/setup.py new file mode 100644 index 000000000..235b0132a --- /dev/null +++ b/core/setup.py @@ -0,0 +1,21 @@ +from setuptools import find_namespace_packages, setup + +setup( + name="revolve2-core", + version="0.0.0", + description="Core library for revolve2", + author="Computational Intelligence Group Vrije Universiteit", + url="https://github.com/ci-group/revolve2", + packages=find_namespace_packages(), + package_data={"revolve2": ["py.typed"]}, + install_requires=[ + "networkx>=2.6.3", + "numpy>=1.21.2", + "rootpath>=0.1.1", + "matplotlib>=3.4.3", + "scipy>=1.7.1", + "pyrr>=0.10.3", + "sqlalchemy>=1.4.28", + ], + zip_safe=False, +) diff --git a/envs/isaacgym/py.typed b/envs/isaacgym/py.typed new file mode 100644 index 000000000..e69de29bb diff --git a/envs/isaacgym/revolve2/envs/isaacgym/__init__.py b/envs/isaacgym/revolve2/envs/isaacgym/__init__.py new file mode 100644 index 000000000..8f80b2933 --- /dev/null +++ b/envs/isaacgym/revolve2/envs/isaacgym/__init__.py @@ -0,0 +1 @@ +from .local_runner import LocalRunner diff --git a/envs/isaacgym/revolve2/envs/isaacgym/local_runner.py b/envs/isaacgym/revolve2/envs/isaacgym/local_runner.py new file mode 100644 index 000000000..37c509bdc --- /dev/null +++ b/envs/isaacgym/revolve2/envs/isaacgym/local_runner.py @@ -0,0 +1,302 @@ +import multiprocessing as mp +import os +import tempfile +from dataclasses import dataclass +from typing import List, Optional, Tuple + +from pyrr import Quaternion, Vector3 +from revolve2.core.physics.actor.urdf import to_urdf as physbot_to_urdf +from revolve2.core.physics.env import (ActorControl, ActorState, Batch, + EnvironmentState, Runner, State) + +from isaacgym import gymapi + + +class LocalRunner(Runner): + class Simulator: + @dataclass + class GymEnv: + env: gymapi.Env # environment handle + actors: List[ + int + ] # actor handles, in same order as provided by environment description + + _gym: gymapi.Gym + _batch: Batch + + _sim: gymapi.Sim + _viewer: Optional[gymapi.Viewer] + _simulation_time: int + _gymenvs: List[ + GymEnv + ] # environments, in same order as provided by batch description + + def __init__( + self, + batch: Batch, + sim_params: gymapi.SimParams, + headless: bool, + ): + self._gym = gymapi.acquire_gym() + self._batch = batch + + self._sim = self._create_sim(sim_params) + self._gymenvs = self._create_envs() + + if headless: + self._viewer = None + else: + self._viewer = self._create_viewer() + + self._gym.prepare_sim(self._sim) + + def _create_sim(self, sim_params: gymapi.SimParams) -> gymapi.Sim: + sim = self._gym.create_sim(type=gymapi.SIM_PHYSX, params=sim_params) + + if sim is None: + raise RuntimeError() + + return sim + + def _create_envs(self) -> List[GymEnv]: + gymenvs: List[self.GymEnv] = [] + + # TODO this is only temporary. When we switch to the new isaac sim it should be easily possible to + # let the user create static object, rendering the group plane redundant. + # But for now we keep it because it's easy for our first test release. + plane_params = gymapi.PlaneParams() + plane_params.normal = gymapi.Vec3(0, 0, 1) + plane_params.distance = 0 + plane_params.static_friction = 0.8 + plane_params.dynamic_friction = 0.5 + plane_params.restitution = 0 + self._gym.add_ground(self._sim, plane_params) + + for env_descr in self._batch.environments: + env = self._gym.create_env( + self._sim, + gymapi.Vec3(-25.0, -25.0, 0.0), # TODO make these configurable + gymapi.Vec3(25.0, 25.0, 25.0), + 1, + ) + + gymenv = self.GymEnv(env, []) + + for actor_index, posed_actor in enumerate(env_descr.actors): + # sadly isaac gym can only read robot descriptions from a file, + # so we create a temporary file. + botfile = tempfile.NamedTemporaryFile( + mode="r+", delete=False, suffix=".urdf" + ) + botfile.writelines( + physbot_to_urdf( + posed_actor.actor, + f"robot_{actor_index}", + Vector3(), + Quaternion(), + ) + ) + botfile.close() + asset_root = os.path.dirname(botfile.name) + urdf_file = os.path.basename(botfile.name) + actor_asset = self._gym.load_urdf(self._sim, asset_root, urdf_file) + os.remove(botfile.name) + + if actor_asset is None: + raise RuntimeError() + + pose = gymapi.Transform() + pose.p = gymapi.Vec3( + posed_actor.position.x, + posed_actor.position.y, + posed_actor.position.z, + ) + pose.r = gymapi.Quat( + posed_actor.orientation.x, + posed_actor.orientation.y, + posed_actor.orientation.z, + posed_actor.orientation.w, + ) + actor_handle = self._gym.create_actor( + env, actor_asset, pose, f"robot_{actor_index}", 0, 0 + ) + + # TODO make all this configurable. + props = self._gym.get_actor_dof_properties(env, actor_handle) + props["driveMode"].fill(gymapi.DOF_MODE_POS) + props["stiffness"].fill(1000.0) + props["damping"].fill(600.0) + self._gym.set_actor_dof_properties(env, actor_handle, props) + + all_rigid_props = self._gym.get_actor_rigid_shape_properties( + env, actor_handle + ) + + for body, rigid_props in zip( + posed_actor.actor.bodies, + all_rigid_props, + ): + rigid_props.friction = body.static_friction + rigid_props.rolling_friction = body.dynamic_friction + + self._gym.set_actor_rigid_shape_properties( + env, actor_handle, all_rigid_props + ) + + gymenv.actors.append(actor_handle) + + gymenvs.append(gymenv) + + return gymenvs + + def _create_viewer(self) -> gymapi.Viewer: + # TODO provide some sensible default and make configurable + viewer = self._gym.create_viewer(self._sim, gymapi.CameraProperties()) + if viewer is None: + raise RuntimeError() + cam_pos = gymapi.Vec3(-4.0, -1.0, 4.0) + cam_target = gymapi.Vec3(0.0, 0.0, 0.0) + self._gym.viewer_camera_look_at(viewer, None, cam_pos, cam_target) + + return viewer + + def run(self) -> List[Tuple[float, State]]: + states: List[Tuple[float, State]] = [] # (time, state) + + control_step = 1 / self._batch.control_frequency + sample_step = 1 / self._batch.sampling_frequency + + last_control_time = 0 + last_sample_time = 0 + + while ( + time := self._gym.get_sim_time(self._sim) + ) < self._batch.simulation_time: + # do control if it is time + if time >= last_control_time + control_step: + last_control_time = int(time / control_step) * control_step + control = ActorControl() + self._batch.control(0.2, control) + + for (env_index, actor_index, targets) in control._dof_targets: + env_handle = self._gymenvs[env_index].env + actor_handle = self._gymenvs[env_index].actors[actor_index] + + if len(targets) != len( + self._batch.environments[env_index] + .actors[actor_index] + .actor.joints + ): + raise RuntimeError("Need to set a target for every dof") + self._gym.set_actor_dof_position_targets( + env_handle, + actor_handle, + targets, + ) + + # sample state if it is time + if time >= last_sample_time + sample_step: + last_sample_time = int(time / sample_step) * sample_step + states.append((time, self._get_state())) + + # step simulation + self._gym.simulate(self._sim) + self._gym.fetch_results(self._sim, True) + self._gym.step_graphics(self._sim) + + if self._viewer is not None: + self._gym.draw_viewer(self._viewer, self._sim, False) + + # sample one final time + states.append((time, self._get_state())) + + return states + + def cleanup(self) -> None: + if self._viewer is not None: + self._gym.destroy_viewer(self._viewer) + self._gym.destroy_sim(self._sim) + + def _get_state(self) -> State: + state = State([]) + + for gymenv in self._gymenvs: + state.envs.append(EnvironmentState([])) + for actor_index, _ in enumerate(gymenv.actors): + pose = self._gym.get_actor_rigid_body_states( + gymenv.env, actor_index, gymapi.STATE_POS + )["pose"] + position = pose["p"][0] # [0] is center of root element + orientation = pose["r"][0] # -> [0] is rotation of root element + state.envs[-1].actor_states.append( + ActorState( + Vector3([position[0], position[1], position[2]]), + Quaternion( + [ + orientation[0], + orientation[1], + orientation[2], + orientation[3], + ] + ), + ) + ) + + return state + + _sim_params: gymapi.SimParams + _headless: bool + + def __init__(self, sim_params: gymapi.SimParams, headless: bool = False): + self._sim_params = sim_params + self._headless = headless + + @staticmethod + def SimParams() -> gymapi.SimParams: + sim_params = gymapi.SimParams() + sim_params.dt = 0.02 + sim_params.substeps = 2 + sim_params.up_axis = gymapi.UP_AXIS_Z + sim_params.gravity = gymapi.Vec3(0.0, 0.0, -9.81) + + sim_params.physx.solver_type = 1 + sim_params.physx.num_position_iterations = 4 + sim_params.physx.num_velocity_iterations = 1 + sim_params.physx.num_threads = 1 + sim_params.physx.use_gpu = False + + return sim_params + + async def run_batch(self, batch: Batch) -> List[Tuple[float, State]]: + # sadly we must run Isaac Gym in a subprocess, because it has some big memory leaks. + result_queue = mp.Queue() + process = mp.Process( + target=self._run_batch_impl, + args=(result_queue, batch, self._sim_params, self._headless), + ) + process.start() + states = [] + # states are sent state by state(every sample) + # because sending all at once is too big for the queue. + # should be good enough for now. + # if the program hangs here in the future, + # improve the way the results are passed back to the parent program. + while (state := result_queue.get()) is not None: + states.append(state) + process.join() + return states + + @classmethod + def _run_batch_impl( + cls, + result_queue: mp.Queue, + batch: Batch, + sim_params: gymapi.SimParams, + headless: bool, + ) -> int: + simulator = cls.Simulator(batch, sim_params, headless) + states = simulator.run() + simulator.cleanup() + for state in states: + result_queue.put(state) + result_queue.put(None) diff --git a/envs/isaacgym/setup.py b/envs/isaacgym/setup.py new file mode 100644 index 000000000..328d60d08 --- /dev/null +++ b/envs/isaacgym/setup.py @@ -0,0 +1,13 @@ +from setuptools import find_namespace_packages, setup + +setup( + name="revolve2-envs-isaacgym", + version="0.0.0", + description="Isaac Gym environments for Revolve2", + author="Computational Intelligence Group Vrije Universiteit", + url="https://github.com/ci-group/revolve2", + packages=find_namespace_packages(), + package_data={"revolve2.envs.isaacgym": ["py.typed"]}, + install_requires=["revolve2-core", "isaacgym==1.0rc2"], + zip_safe=False, +) diff --git a/genotypes/cppnneat/py.typed b/genotypes/cppnneat/py.typed new file mode 100644 index 000000000..e69de29bb diff --git a/genotypes/cppnneat/revolve2/genotypes/cppnneat/__init__.py b/genotypes/cppnneat/revolve2/genotypes/cppnneat/__init__.py new file mode 100644 index 000000000..c33ce8d9d --- /dev/null +++ b/genotypes/cppnneat/revolve2/genotypes/cppnneat/__init__.py @@ -0,0 +1,2 @@ +from .body_genotype_v1 import BodyGenotypeV1 +from .brain_genotype_cpg_v1 import BrainGenotypeCpgV1 diff --git a/genotypes/cppnneat/revolve2/genotypes/cppnneat/body_genotype_v1.py b/genotypes/cppnneat/revolve2/genotypes/cppnneat/body_genotype_v1.py new file mode 100644 index 000000000..a271d6ded --- /dev/null +++ b/genotypes/cppnneat/revolve2/genotypes/cppnneat/body_genotype_v1.py @@ -0,0 +1,214 @@ +import math +from dataclasses import dataclass +from queue import Queue +from typing import Any, List, Optional, Set, Tuple + +import multineat +from revolve2.core.database import StaticData +from revolve2.core.database.serialize import Serializable, SerializeError +from revolve2.core.modular_robot import ActiveHinge +from revolve2.core.modular_robot import Body +from revolve2.core.modular_robot import Body as ModularRobotBody +from revolve2.core.modular_robot import Brick, Core, Module +from revolve2.core.optimization.ea.modular_robot import BodyGenotype + +from .bodybrain_base import BodybrainBase + + +class BodyGenotypeV1(BodyGenotype, BodybrainBase["BodyGenotypeV1"], Serializable): + @classmethod + def random( + cls, + innov_db: multineat.InnovationDatabase, + rng: multineat.RNG, + multineat_params: multineat.Parameters, + output_activation_func: multineat.ActivationFunction, + num_initial_mutations: int, + ) -> BodybrainBase: + return super(BodyGenotypeV1, cls).random( + innov_db, + rng, + multineat_params, + output_activation_func, + 5, # bias(always 1), pos_x, pos_y, pos_z, chain_length + 5, # empty, brick, activehinge, rot0, rot90 + num_initial_mutations, + ) + + @dataclass + class _Module: + position: Tuple[int, int, int] + forward: Tuple[int, int, int] + up: Tuple[int, int, int] + chain_length: int + module_reference: Module + + def develop(self) -> ModularRobotBody: + max_parts = 10 + + body_net = multineat.NeuralNetwork() + self._genotype.BuildPhenotype(body_net) + + to_explore: Queue[self.Module] = Queue() + grid: Set[Tuple(int, int, int)] = set() + + body = Body() + + to_explore.put(self._Module((0, 0, 0), (0, -1, 0), (0, 0, 1), 0, body.core)) + grid.add((0, 0, 0)) + part_count = 1 + + while not to_explore.empty(): + module: self._Module = to_explore.get() + + children: List[Tuple[int, int]] = [] # child index, rotation + + if module.module_reference.type == Module.Type.CORE: + children.append((Core.FRONT, 0)) + children.append((Core.LEFT, 1)) + children.append((Core.BACK, 2)) + children.append((Core.RIGHT, 3)) + elif module.module_reference.type == Module.Type.BRICK: + children.append((Brick.FRONT, 0)) + children.append((Brick.LEFT, 1)) + children.append((Brick.RIGHT, 3)) + elif module.module_reference.type == Module.Type.ACTIVE_HINGE: + children.append((ActiveHinge.ATTACHMENT_INDEX, 0)) + else: # Should actually never arrive here but just checking module type to be sure + raise RuntimeError() + + for (index, rotation) in children: + if part_count < max_parts: + child = self._add_child(body_net, module, index, rotation, grid) + if child is not None: + to_explore.put(child) + part_count += 1 + + return body + + @staticmethod + def _evaluate_cppg( + body_net: multineat.NeuralNetwork, + position: Tuple[int, int, int], + chain_length: int, + ) -> Tuple[Any, int]: + """ + get module type, orientation + """ + body_net.Input( + [1.0, position[0], position[1], position[2], chain_length] + ) # 1.0 is the bias input + body_net.Activate() + outputs = body_net.Output() + + # get module type from output probabilities + type_probs = [outputs[0], outputs[1], outputs[2]] + types = [None, Brick, ActiveHinge] + module_type = types[type_probs.index(min(type_probs))] + + # get rotation from output probabilities + rotation_probs = [outputs[3], outputs[4]] + rotation = rotation_probs.index(min(rotation_probs)) + + return (module_type, rotation) + + @classmethod + def _add_child( + cls, + body_net: multineat.NeuralNetwork, + module: _Module, + child_index: int, + rotation: int, + grid: Set[Tuple[int, int, int]], + ) -> Optional[_Module]: + forward = cls._rotate(module.forward, module.up, rotation) + position = cls._add(module.position, forward) + chain_length = module.chain_length + 1 + + # if grid cell is occupied, don't make a child + # else, set cell as occupied + if position in grid: + return None + else: + grid.add(position) + + child_type, orientation = cls._evaluate_cppg(body_net, position, chain_length) + if child_type is None: + return None + up = cls._rotate(module.up, forward, orientation) + + child = child_type(orientation * (math.pi / 2.0)) + module.module_reference.children[child_index] = child + + return cls._Module( + position, + forward, + up, + chain_length, + child, + ) + + @classmethod + def _add( + cls, a: Tuple[int, int, int], b: Tuple[int, int, int] + ) -> Tuple[int, int, int]: + return tuple(map(sum, zip(a, b))) + + @classmethod + def _timesscalar(cls, a: Tuple[int, int, int], scalar: int) -> Tuple[int, int, int]: + return (a[0] * scalar, a[1] * scalar, a[2] * scalar) + + @classmethod + def _cross( + cls, a: Tuple[int, int, int], b: Tuple[int, int, int] + ) -> Tuple[int, int, int]: + return ( + a[1] * b[2] - a[2] * b[1], + a[2] * b[0] - a[0] * b[2], + a[0] * b[1] - a[1] * b[0], + ) + + @classmethod + def _dot(cls, a: Tuple[int, int, int], b: Tuple[int, int, int]) -> int: + return a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + + @classmethod + def _rotate( + cls, a: Tuple[int, int, int], b: Tuple[int, int, int], angle: int + ) -> Tuple[int, int, int]: + """ + rotates a around b. angle from [0,1,2,3]. 90 degrees each + """ + cosangle: int + sinangle: int + if angle == 0: + cosangle = 1 + sinangle = 0 + elif angle == 1: + cosangle = 0 + sinangle = 1 + elif angle == 2: + cosangle = -1 + sinangle = 0 + else: + cosangle = 0 + sinangle = -1 + + return cls._add( + cls._add( + cls._timesscalar(a, cosangle), + cls._timesscalar(cls._cross(b, a), sinangle), + ), + cls._timesscalar(b, cls._dot(b, a) * (1 - cosangle)), + ) + + def serialize(self) -> StaticData: + return self._genotype.Serialize() + + @classmethod + def deserialize(cls, data: StaticData) -> Serializable: + if type(data) != str: + raise SerializeError() + genotype = multineat.Genome() + genotype.Deserialize(data) + return cls(genotype) diff --git a/genotypes/cppnneat/revolve2/genotypes/cppnneat/bodybrain_base.py b/genotypes/cppnneat/revolve2/genotypes/cppnneat/bodybrain_base.py new file mode 100644 index 000000000..48e06160a --- /dev/null +++ b/genotypes/cppnneat/revolve2/genotypes/cppnneat/bodybrain_base.py @@ -0,0 +1,83 @@ +from __future__ import annotations + +from dataclasses import dataclass +from typing import Generic, TypeVar + +import multineat + +Child = TypeVar("Child") + + +@dataclass +class BodybrainBase(Generic[Child]): + _genotype: multineat.Genome + + @classmethod + def random( + cls, + innov_db: multineat.InnovationDatabase, + rng: multineat.RNG, + multineat_params: multineat.Parameters, + output_activation_func: multineat.ActivationFunction, + num_inputs: int, + num_outputs: int, + num_initial_mutations: int, + ) -> BodybrainBase: + genotype = multineat.Genome( + 0, # ID + num_inputs, + 0, # n_hidden + num_outputs, + False, # FS_NEAT + output_activation_func, # output activation type + multineat.ActivationFunction.TANH, # hidden activation type + 0, # seed_type + multineat_params, + 0, # number of hidden layers + ) + + for _ in range(num_initial_mutations): + genotype.Mutate( + False, + multineat.SearchMode.COMPLEXIFYING, + innov_db, + multineat_params, + rng, + ) + + return cls(genotype) + + def mutate( + self, + multineat_params: multineat.Parameters, + innov_db: multineat.InnovationDatabase, + rng: multineat.RNG, + ) -> Child: + new_genotype = multineat.Genome(self._genotype) + new_genotype.Mutate( + False, + multineat.SearchMode.COMPLEXIFYING, + innov_db, + multineat_params, + rng, + ) + return type(self)(new_genotype) + + @classmethod + def crossover( + cls, + parent1: BodybrainBase, + parent2: BodybrainBase, + multineat_params: multineat.Parameters, + rng: multineat.RNG, + mate_average: bool, + interspecies_crossover: bool, + ) -> Child: + new_genotype = parent1._genotype.Mate( + parent2._genotype, + mate_average, + interspecies_crossover, + rng, + multineat_params, + ) + return cls(new_genotype) diff --git a/genotypes/cppnneat/revolve2/genotypes/cppnneat/brain_cpg_v1.py b/genotypes/cppnneat/revolve2/genotypes/cppnneat/brain_cpg_v1.py new file mode 100644 index 000000000..b1db5207b --- /dev/null +++ b/genotypes/cppnneat/revolve2/genotypes/cppnneat/brain_cpg_v1.py @@ -0,0 +1,68 @@ +from typing import List, Tuple + +import multineat +from revolve2.core.modular_robot import AnalyzerModule, Serialized +from revolve2.core.modular_robot.brains import Cpg as ModularRobotBrainCpg + + +class BrainCpgV1(ModularRobotBrainCpg): + _genotype: multineat.Genome + + def __init__(self, genotype: multineat.Genome): + self._genotype = genotype + + def _make_weights( + self, + active_hinges: List[AnalyzerModule], + connections: List[Tuple[AnalyzerModule, AnalyzerModule]], + ) -> Tuple[List[float], List[float]]: + brain_net = multineat.NeuralNetwork() + self._genotype.BuildPhenotype(brain_net) + + internal_weights = [ + self._evaluate_network( + brain_net, + [ + 1.0, + float(pos.x), + float(pos.y), + float(pos.z), + float(pos.x), + float(pos.y), + float(pos.z), + ], + ) + for pos in [active_hinge.grid_position() for active_hinge in active_hinges] + ] + + external_weights = [ + self._evaluate_network( + brain_net, + [ + 1.0, + float(pos1.x), + float(pos1.y), + float(pos1.z), + float(pos2.x), + float(pos2.y), + float(pos2.z), + ], + ) + for (pos1, pos2) in [ + (active_hinge1.grid_position(), active_hinge2.grid_position()) + for (active_hinge1, active_hinge2) in connections + ] + ] + + return (internal_weights, external_weights) + + @staticmethod + def _evaluate_network( + network: multineat.NeuralNetwork, inputs: List[float] + ) -> float: + network.Input(inputs) + network.Activate() + return network.Output()[0] + + def serialize(self) -> Serialized: + return [] # TODO diff --git a/genotypes/cppnneat/revolve2/genotypes/cppnneat/brain_genotype_cpg_v1.py b/genotypes/cppnneat/revolve2/genotypes/cppnneat/brain_genotype_cpg_v1.py new file mode 100644 index 000000000..8c5f1dde7 --- /dev/null +++ b/genotypes/cppnneat/revolve2/genotypes/cppnneat/brain_genotype_cpg_v1.py @@ -0,0 +1,51 @@ +import multineat +from revolve2.core.database import StaticData +from revolve2.core.database.serialize import Serializable, SerializeError +from revolve2.core.modular_robot import Body as ModularRobotBody +from revolve2.core.modular_robot import Brain as ModularRobotBrain +from revolve2.core.optimization.ea.modular_robot import BrainGenotype + +from .bodybrain_base import BodybrainBase +from .brain_cpg_v1 import BrainCpgV1 + + +class BrainGenotypeCpgV1( + BrainGenotype, BodybrainBase["BrainGenotypeCpgV1"], Serializable +): + @classmethod + def random( + cls, + innov_db: multineat.InnovationDatabase, + rng: multineat.RNG, + multineat_params: multineat.Parameters, + output_activation_func: multineat.ActivationFunction, + num_initial_mutations: int, + ) -> BodybrainBase: + assert multineat_params.MutateOutputActivationFunction == False + # other activation functions could work too, but this has been tested. + # if you want another one, make sure it's output is between -1 and 1. + assert output_activation_func == multineat.ActivationFunction.SIGNED_SINE + + return super(BrainGenotypeCpgV1, cls).random( + innov_db, + rng, + multineat_params, + output_activation_func, + 7, # bias(always 1), x1, y1, z1, x2, y2, z2 + 1, # weight + num_initial_mutations, + ) + + def develop(self, body: ModularRobotBody) -> ModularRobotBrain: + return BrainCpgV1(self._genotype) + + def serialize(self) -> StaticData: + return self._genotype.Serialize() + + @classmethod + def deserialize(cls, data: StaticData) -> None: + genotype = multineat.Genome() + if type(data) != str: + raise SerializeError() + genotype.Deserialize(data) + return cls(genotype) diff --git a/genotypes/cppnneat/setup.py b/genotypes/cppnneat/setup.py new file mode 100644 index 000000000..b940a68e2 --- /dev/null +++ b/genotypes/cppnneat/setup.py @@ -0,0 +1,13 @@ +from setuptools import find_namespace_packages, setup + +setup( + name="revolve2-genotype-cppnneat", + version="0.0.0", + description="CPPN+NEAT genotype for modular robots from Revolve2", + author="Computational Intelligence Group Vrije Universiteit", + url="https://github.com/ci-group/revolve2", + packages=find_namespace_packages(), + package_data={"revolve2.genotypes.cppnneat": ["py.typed"]}, + install_requires=["revolve2-core", "multineat"], + zip_safe=False, +)