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, +)