diff --git a/.github/workflows/notebook-test-ci.yml b/.github/workflows/notebook-test-ci.yml index 853904a5c..6a2a42555 100644 --- a/.github/workflows/notebook-test-ci.yml +++ b/.github/workflows/notebook-test-ci.yml @@ -80,4 +80,4 @@ jobs: run: | source /opt/ros/overlay_ws/devel/setup.bash roscd pycram/examples/tmp - treon --thread 1 -v --exclude=migrate_neems.ipynb \ No newline at end of file + treon --thread 1 -v --exclude=migrate_neems.ipynb --exclude=improving_actions.ipynb \ No newline at end of file diff --git a/.gitignore b/.gitignore index c9694815d..72eae2dfb 100644 --- a/.gitignore +++ b/.gitignore @@ -71,3 +71,4 @@ resources/cached/ # Example checkpoints examples/.ipynb_checkpoints/ +*/tmp/* \ No newline at end of file diff --git a/examples/action_designator.md b/examples/action_designator.md index e0a4babf9..0496ce710 100644 --- a/examples/action_designator.md +++ b/examples/action_designator.md @@ -41,11 +41,12 @@ from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.datastructures.enums import ObjectType, WorldMode from pycram.datastructures.pose import Pose +import pycrap -world = BulletWorld(WorldMode.GUI) -pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf", pose=Pose([1, 2, 0])) -apartmet = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.3, 2, 1.1])) +world = BulletWorld(WorldMode.DIRECT) +pr2 = Object("pr2", pycrap.Robot, "pr2.urdf", pose=Pose([1, 2, 0])) +apartmet = Object("apartment", pycrap.Apartment, "apartment.urdf") +milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([2.3, 2, 1.1])) ``` To move the robot we need to create a description and resolve it to an actual Designator. The description of navigation diff --git a/examples/bullet_world.md b/examples/bullet_world.md index b8903f248..4b5876817 100644 --- a/examples/bullet_world.md +++ b/examples/bullet_world.md @@ -22,8 +22,9 @@ First we need to import and create a BulletWorld. from pycram.worlds.bullet_world import BulletWorld from pycram.datastructures.pose import Pose from pycram.datastructures.enums import ObjectType, WorldMode +import pycrap -world = BulletWorld(mode=WorldMode.GUI) +world = BulletWorld(mode=WorldMode.DIRECT) ``` This new window is the BulletWorld, PyCRAMs internal physics simulation. You can use the mouse to move the camera @@ -41,7 +42,7 @@ To spawn new things in the BulletWorld we need to import the Object class and cr ```python from pycram.world_concepts.world_object import Object -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([0, 0, 1])) +milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([0, 0, 1])) ``` @@ -91,7 +92,7 @@ parameter. Since attachments are bi-directional it doesn't matter on which Objec First we need another Object ```python -cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1, 0, 1])) +cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", pose=Pose([1, 0, 1])) ``` ```python @@ -119,7 +120,7 @@ which contain every link or joint as key and a unique id, used by PyBullet, as v We will see this at the example of the PR2: ```python -pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +pr2 = Object("pr2", pycrap.Robot, "pr2.urdf") print(pr2.links) ``` diff --git a/examples/cram_plan_tutorial.md b/examples/cram_plan_tutorial.md index 121866419..4a47dfb99 100644 --- a/examples/cram_plan_tutorial.md +++ b/examples/cram_plan_tutorial.md @@ -32,13 +32,14 @@ from pycram.world_concepts.world_object import Object import anytree import pycram.failures import numpy as np +import pycrap np.random.seed(4) world = BulletWorld() -robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +robot = Object("pr2", pycrap.Robot, "pr2.urdf") robot_desig = ObjectDesignatorDescription(names=['pr2']).resolve() -apartment = Object("apartment", "environment", "apartment.urdf", pose=Pose([-1.5, -2.5, 0])) +apartment = Object("apartment", pycrap.Apartment, "apartment.urdf", pose=Pose([-1.5, -2.5, 0])) apartment_desig = ObjectDesignatorDescription(names=['apartment']).resolve() table_top = apartment.get_link_position("cooktop") # milk = Object("milk", "milk", "milk.stl", position=[table_top[0]-0.15, table_top[1], table_top[2]]) @@ -98,6 +99,7 @@ def get_n_random_positions(pose_list, n=4, dist=0.5, random=True): ``` ```python +import pycrap from pycram.costmaps import SemanticCostmap from pycram.pose_generator_and_validator import PoseGenerator @@ -106,17 +108,15 @@ poses_list = list(PoseGenerator(scm, number_of_samples=-1)) poses_list.sort(reverse=True, key=lambda x: np.linalg.norm(x.position_as_list())) object_poses = get_n_random_positions(poses_list) object_names = ["bowl", "breakfast_cereal", "spoon"] +object_types = [pycrap.Bowl, pycrap.Cereal, pycrap.Spoon] objects = {} object_desig = {} -for obj_name, obj_pose in zip(object_names, object_poses): - print(obj_name) - print(obj_pose) - objects[obj_name] = Object(obj_name, obj_name, obj_name + ".stl", +for obj_name, obj_type, obj_pose in zip(object_names, object_types, object_poses): + objects[obj_name] = Object(obj_name, obj_type, obj_name + ".stl", pose=Pose([obj_pose.position.x, obj_pose.position.y, table_top.z])) objects[obj_name].move_base_to_origin_pose() objects[obj_name].original_pose = objects[obj_name].pose - object_desig[obj_name] = ObjectDesignatorDescription(names=[obj_name], types=[obj_name]).resolve() -print(object_poses) + object_desig[obj_name] = ObjectDesignatorDescription(names=[obj_name], types=[obj_type]).resolve() ``` If You want to visualize all apartment frames diff --git a/examples/intro.md b/examples/intro.md index 12480fdea..c2251194a 100644 --- a/examples/intro.md +++ b/examples/intro.md @@ -28,16 +28,17 @@ It is possible to spawn objects and robots into the BulletWorld, these objects c A BulletWorld can be created by simply creating an object of the BulletWorld class. ```python +import pycrap from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.datastructures.enums import ObjectType, WorldMode from pycram.datastructures.pose import Pose -world = BulletWorld(mode=WorldMode.GUI) +world = BulletWorld(mode=WorldMode.DIRECT) -milk = Object("Milk", ObjectType.MILK, "milk.stl") -pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") -cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95])) +milk = Object("milk", pycrap.Milk, "milk.stl") +pr2 = Object("pr2", pycrap.Robot, "pr2.urdf") +cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95])) ``` The BulletWorld allows to render images from arbitrary positions. In the following example we render images with the @@ -90,7 +91,7 @@ Since everything inside the BulletWorld is an Object, even a complex environment in the same way as the milk. ```python -kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") +kitchen = Object("kitchen", pycrap.Kitchen, "kitchen.urdf") ``` ## Costmaps @@ -270,7 +271,7 @@ Designators are used, for example, by the PickUpAction to know which object shou ```python from pycram.designators.object_designator import * -milk_desig = BelieveObject(names=["Milk"]) +milk_desig = BelieveObject(names=["milk"]) milk_desig.resolve() ``` @@ -281,7 +282,7 @@ Location Designator can create a position in cartisian space from a symbolic des ```python from pycram.designators.object_designator import * -milk_desig = BelieveObject(names=["Milk"]) +milk_desig = BelieveObject(names=["milk"]) milk_desig.resolve() ``` @@ -293,8 +294,8 @@ Location Designators can create a position in cartesian space from a symbolic de from pycram.designators.location_designator import * from pycram.designators.object_designator import * -robot_desig = BelieveObject(types=[ObjectType.ROBOT]).resolve() -milk_desig = BelieveObject(names=["Milk"]).resolve() +robot_desig = BelieveObject(types=[pycrap.Robot]).resolve() +milk_desig = BelieveObject(names=["milk"]).resolve() location_desig = CostmapLocation(target=milk_desig, visible_for=robot_desig) print(f"Resolved: {location_desig.resolve()}") diff --git a/examples/language.md b/examples/language.md index 0b0862617..11b9280ab 100644 --- a/examples/language.md +++ b/examples/language.md @@ -73,12 +73,13 @@ plan. If you are performing a plan with a simulated robot, you need a BulletWorld. ```python +import pycrap from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.datastructures.enums import ObjectType world = BulletWorld() -pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +pr2 = Object("pr2", pycrap.Robot, "pr2.urdf") ``` ```python diff --git a/examples/local_transformer.md b/examples/local_transformer.md index 268af2024..2b3965fba 100644 --- a/examples/local_transformer.md +++ b/examples/local_transformer.md @@ -28,6 +28,7 @@ from pycram.world_concepts.world_object import Object from pycram.datastructures.pose import Transform, Pose from pycram.local_transformer import LocalTransformer from pycram.datastructures.enums import WorldMode +import pycrap ``` ## Initializing the World @@ -41,7 +42,7 @@ world first. ```python # Create an instance of the BulletWorld -world = BulletWorld(WorldMode.GUI) +world = BulletWorld(WorldMode.DIRECT) ``` ## Adding Objects to the World @@ -55,9 +56,9 @@ These objects will be used in subsequent tasks, to provide the frames to which w from pycram.worlds.bullet_world import Object from pycram.datastructures.enums import ObjectType -kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([0.9, 1, 0.95])) -bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([1.6, 1, 0.90])) +kitchen = Object("kitchen", pycrap.Kitchen, "kitchen.urdf") +milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([0.9, 1, 0.95])) +bowl = Object("bowl", pycrap.Bowl, "bowl.stl", pose=Pose([1.6, 1, 0.90])) ``` ## Creating a Local Transfomer diff --git a/examples/location_designator.md b/examples/location_designator.md index 7d1fcd5c2..58a0dac45 100644 --- a/examples/location_designator.md +++ b/examples/location_designator.md @@ -42,10 +42,11 @@ from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.datastructures.enums import ObjectType, WorldMode from pycram.datastructures.pose import Pose +import pycrap -world = BulletWorld(WorldMode.GUI) -apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") -pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") +world = BulletWorld(WorldMode.DIRECT) +apartment = Object("apartment", pycrap.Apartment, "apartment.urdf") +pr2 = Object("pr2", pycrap.Robot, "pr2.urdf") ``` Next up we will create the location designator description, the {meth}`~pycram.designators.location_designator.CostmapLocation` that we will be using needs a @@ -78,7 +79,7 @@ PR2 will be set to 0.2 since otherwise the arms of the robot will be too low to ```python pr2.set_joint_position("torso_lift_joint", 0.2) -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) ``` @@ -181,7 +182,7 @@ from pycram.datastructures.enums import ObjectType apartment_desig = BelieveObject(names=["apartment"]) handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve()) -robot_desig = BelieveObject(types=[ObjectType.ROBOT]) +robot_desig = BelieveObject(types=[pycrap.Robot]) access_location = AccessingLocation(handle_desig.resolve(), robot_desig.resolve()).resolve() print(access_location.pose) diff --git a/examples/migrate_neems.md b/examples/migrate_neems.md index 9fc8e63c5..123b788e8 100644 --- a/examples/migrate_neems.md +++ b/examples/migrate_neems.md @@ -56,15 +56,16 @@ from pycram.tasktree import with_tree from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.designators.object_designator import * +import pycrap class ExamplePlans: def __init__(self): self.world = BulletWorld("DIRECT") - self.pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") - self.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") - self.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - self.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) + self.pr2 = Object("pr2", pycrap.Robot, "pr2.urdf") + self.kitchen = Object("kitchen", pycrap.Kitchen, "kitchen.urdf") + self.milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) + self.cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) self.milk_desig = ObjectDesignatorDescription(names=["milk"]) self.cereal_desig = ObjectDesignatorDescription(names=["cereal"]) self.robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() diff --git a/examples/minimal_task_tree.md b/examples/minimal_task_tree.md index 124746378..d4af30aaa 100644 --- a/examples/minimal_task_tree.md +++ b/examples/minimal_task_tree.md @@ -32,16 +32,17 @@ from pycram.datastructures.pose import Pose from pycram.datastructures.enums import ObjectType, WorldMode import anytree import pycram.failures +import pycrap ``` Next we will create a bullet world with a PR2 in a kitchen containing milk and cereal. ```python world = BulletWorld(WorldMode.DIRECT) -pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") -kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) -cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) +pr2 = Object("pr2", pycrap.Robot, "pr2.urdf") +kitchen = Object("kitchen", pycrap.Kitchen, "kitchen.urdf") +milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) +cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) milk_desig = ObjectDesignatorDescription(names=["milk"]) cereal_desig = ObjectDesignatorDescription(names=["cereal"]) robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() diff --git a/examples/motion_designator.md b/examples/motion_designator.md index ba5746dbd..78ac06954 100644 --- a/examples/motion_designator.md +++ b/examples/motion_designator.md @@ -27,10 +27,11 @@ from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.datastructures.enums import ObjectType, WorldMode from pycram.datastructures.pose import Pose +import pycrap -world = BulletWorld(WorldMode.GUI) -pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.5, 0, 1])) +world = BulletWorld(WorldMode.DIRECT) +pr2 = Object("pr2", pycrap.Robot, "pr2.urdf") +milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.5, 0, 1])) ``` ## Move @@ -115,7 +116,7 @@ from pycram.process_module import simulated_robot with simulated_robot: LookingMotion(target=Pose([1.5, 0, 1], [0, 0, 0, 1])).perform() - motion_description = DetectingMotion(object_type=ObjectType.MILK) + motion_description = DetectingMotion(object_type=pycrap.Milk) obj = motion_description.perform() @@ -150,7 +151,7 @@ from pycram.designators.motion_designator import WorldStateDetectingMotion from pycram.process_module import simulated_robot with simulated_robot: - motion_description = WorldStateDetectingMotion(object_type=ObjectType.MILK) + motion_description = WorldStateDetectingMotion(object_type=pycrap.Milk) obj = motion_description.perform() diff --git a/examples/object_designator.md b/examples/object_designator.md index dbc8568a8..752476a61 100644 --- a/examples/object_designator.md +++ b/examples/object_designator.md @@ -36,8 +36,8 @@ from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.datastructures.enums import ObjectType, WorldMode from pycram.datastructures.pose import Pose - -world = BulletWorld(WorldMode.GUI) +import pycrap +world = BulletWorld(WorldMode.DIRECT) ``` ## Believe Object @@ -49,10 +49,12 @@ description which will be used to describe objects in the real world. Since {meth}`~pycram.designators.object_designator.BelieveObject` describes Objects in the BulletWorld we create a few. ```python -kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) -cereal = Object("froot_loops", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.3, 0.9, 0.95])) -spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([1.3, 1.1, 0.87])) +import pycrap + +kitchen = Object("kitchen", pycrap.Kitchen, "kitchen.urdf") +milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) +cereal = Object("froot_loops", pycrap.Cereal, "breakfast_cereal.stl", pose=Pose([1.3, 0.9, 0.95])) +spoon = Object("spoon", pycrap.Spoon, "spoon.stl", pose=Pose([1.3, 1.1, 0.87])) ``` Now that we have objects we can create an object designator to describe them. For the start we want an object designator @@ -73,7 +75,7 @@ the world. ```python from pycram.designators.object_designator import BelieveObject -object_description = BelieveObject(types=[ObjectType.MILK, ObjectType.BREAKFAST_CEREAL]) +object_description = BelieveObject(types=[pycrap.Milk, pycrap.Cereal]) print(object_description.resolve()) ``` @@ -107,7 +109,7 @@ For this we need some objects, so if you didn't already spawn them you can use t ```python from pycram.designators.object_designator import BelieveObject -object_description = BelieveObject(types=[ObjectType.MILK, ObjectType.BREAKFAST_CEREAL]) +object_description = BelieveObject(types=[pycrap.Milk, pycrap.Cereal]) for obj in object_description: print(obj, "\n") diff --git a/examples/ontology.md b/examples/ontology.md index 1c93353ab..a4f0c502a 100644 --- a/examples/ontology.md +++ b/examples/ontology.md @@ -5,513 +5,87 @@ jupyter: extension: .md format_name: markdown format_version: '1.3' - jupytext_version: 1.16.2 + jupytext_version: 1.16.3 kernelspec: - display_name: Python 3 + display_name: Python 3 (ipykernel) language: python name: python3 --- - +# Ontologies in PyCRAM -# Ontology interface +Cognitive Architectures often include some logical theory to them that enables higher level reasoning, and so does +PyCRAM. The PyCRAM typology PyCRAP is an ontology that can be used in PyCRAM to describe the belief of the robot +using concepts. These individuals for the ontology are created on the fly, and PyCRAP can then be used to for instance +filter for certain objects. -This tutorial demonstrates basic usages of __owlready2__ API for ontology manipulation. Notably, new ontology concept -triple classes (subject, predicate, object) will be dynamically created, with optional existing ontology parent classes -that are loaded from an OWL ontology. Then through the interconnected relations specified in triples, designators and -their corresponding ontology concepts can be double-way queried for input information in certain tasks, eg. making a -robot motion plan. - +PyCRAP is defined in its package next to the PyCRAM package and hence allows users and developers to +add new classes, instances, and relationships on demand. +Furthermore, this architecture allows the users to directly see what's in the ontology and how it is structured without +switching to tools like [Protégé](https://protege.stanford.edu/). +The linter and AI assistants like Copilot can also deal with this notation and guide the users without annoyances. -```python jupyter={"outputs_hidden": false} -from pathlib import Path -from typing import Type -from pycram.designator import ObjectDesignatorDescription -``` - - - -# Owlready2 - -[Owlready2](https://owlready2.readthedocs.io/en/latest/intro.html) is a Python package providing a transparent access to -OWL ontologies. It supports various manipulation operations, including but not limited to loading, modification, saving -ontologies. Built-in supported reasoners include [HermiT](http://www.hermit-reasoner.com) -and [Pellet](https://github.com/stardog-union/pellet). - - -```python jupyter={"outputs_hidden": false} -import logging - -try: - from owlready2 import * -except ImportError: - owlready2 = None - logging.error("Could not import owlready2, Ontology Manager could not be initialized!") - -logging.getLogger().setLevel(logging.INFO) -``` - - +Note that this area is under construction and may frequently change. -# Ontology Manager +## Usage -{class}`~pycram.ontology.ontology.OntologyManager` is the singleton class acting as the main interface between PyCram with ontologies, whereby object -instances in the former could query relevant information based on the semantic connection with their corresponding -ontology concepts. - -Such connection, as represented by triples (subject-predicate-object), could be also created on the fly if not -pre-existing in the loaded ontology. - -Also new and updated concepts with their properties defined in runtime could be stored into -an [SQLite3 file database](https://owlready2.readthedocs.io/en/latest/world.html) for reuse. - -Here we will use [SOMA ontology](https://ease-crc.github.io/soma) as the baseline to utilize the generalized concepts -provided by it. - - -```python jupyter={"outputs_hidden": false} -from pycram.ontology.ontology import OntologyManager, SOMA_HOME_ONTOLOGY_IRI -from pycram.ontology.ontology_common import OntologyConceptHolderStore, OntologyConceptHolder - -ontology_manager = OntologyManager(SOMA_HOME_ONTOLOGY_IRI) -main_ontology = ontology_manager.main_ontology -soma = ontology_manager.soma -dul = ontology_manager.dul -``` - -[General class axioms](https://owlready2.readthedocs.io/en/latest/general_class_axioms.html) of the loaded ontologies -can be queried by +You can access the ontology by importing the PyCRAP package: ```python -print(f"{main_ontology.name}: ", ontology_manager.get_ontology_general_class_axioms(main_ontology)) -print(f"{soma.name}: ", ontology_manager.get_ontology_general_class_axioms(soma)) -print(f"{dul.name}: ", ontology_manager.get_ontology_general_class_axioms(dul)) -``` - - - -## Ontology Concept Holder - -__OntologyConceptHolder__ class, encapsulating an __owlready2.Thing__ instance, is used primarily as the binding -connection between the `owlready2.Thing` ontology concept to PyCram designators. We make it that way, instead of -creating a custom concept class that inherits from `owlready2.Thing` for the reasons below: - -- `owlready2` API does not have very robust support for client classes to inherit from theirs with added (non-semantic) - attributes, particularly in our case, where classes like {class}`~pycram.designator.DesignatorDescription` have their `metaclass` as `ABCMeta`, - while it is `EntityClass` that is the metaclass used for basically all concepts (classes, properties) in `owlready2`. - Since those two metaclasses just bear no relationship, for the inheritance to work, the only way is to create a child - metaclass with both of those as parents, however without full support by `owlready2`, plus the second reason below - will point out it's not worth the effort. - - -- Essentially, we will have new ontology concept classes created dynamically, if their types inherit - from `owlready2.Thing`, all custom non-semantic (of types known only by PyCram) attributes, which are defined by their - own in child classes, will apparently be not savable into the ontology by `owlready2` api. Then the next time the - ontology is loaded, those same dynamic classes will not be created anymore, thus without those attributes either, - causing running error. - -As such, in short, an ontology concept class, either newly created on the fly or loaded from ontologies, has to -be `owlready2.Thing` or its pure derived class (without non-semantic attributes), so to make itself reusable upon -reloading. - -Notable attributes: - -- `ontology_concept`: An ontology concept of `owlready2.Thing` type or its pure child class (without custom non-semantic - attributes), either dynamically created, or loaded from an ontology - -- `designators`: a list of `DesignatorDescription` instances associated with `ontology_concept` - -- `resolve`: a `Callable` typically returning a list of `DesignatorDescription` as specific designators, - like `designators` or its subset, inferred from the ontology concept. In fact, it can be resolved to anything else - relevant, up to the caller. - - - - - -## Query ontology classes and their properties - -Classes in the loaded ontology can be queried based on their exact names, or part of them, or by namespace. -Here, we can see essential info (ancestors, super/sub-classes, properties, direct instances, etc.) of the found ontology -class. - - -```python jupyter={"outputs_hidden": false} -ontology_designed_container_class = ontology_manager.get_ontology_class('DesignedContainer') -ontology_manager.print_ontology_class(ontology_designed_container_class) -classes = ontology_manager.get_ontology_classes_by_subname('PhysicalObject'); -print(classes[0]) -classes = ontology_manager.get_ontology_classes_by_namespace('SOMA'); -print(classes[:2]) -``` - - -__Descendants__ of an ontology class can be also queried by - - -```python jupyter={"outputs_hidden": false} -ontology_manager.get_ontology_descendant_classes(ontology_designed_container_class)[:5] -``` - - - -## Create a new ontology concept class and its individual - -A new ontology class can be created dynamically as inheriting from an existing class in the loaded ontology. -Here we create the class and its instance, also known as [__individual -__](https://owlready2.readthedocs.io/en/latest/class.html#creating-equivalent-classes) in ontology terms, which is then -wrapped inside an {class}`~pycram.ontology.ontology_common.OntologyConceptHolder`. - - -```python jupyter={"outputs_hidden": false} -ontology_custom_container_class = ontology_manager.create_ontology_concept_class('CustomContainerConcept', - ontology_designed_container_class) -custom_container_concept_holder = OntologyConceptHolder( - ontology_custom_container_class(name='ontology_custom_container_concept', - namespace=main_ontology)) -``` - - - -## Access ontology concept classes and individuals - -All ontology classes created on the fly purely inherit (without added non-semantic attributes) from `owlready2.Thing`, -and so share the same namespace with the loaded ontology instance, `main_ontology`. They can then be accessible through -that namespace by __main_ontology.__. -The same applies for individuals of those classes, accessible by __main_ontology.__ - - -```python jupyter={"outputs_hidden": false} -ontology_manager.print_ontology_class(main_ontology.CustomContainerConcept) -print( - f"custom_container_concept is {main_ontology.ontology_custom_container_concept}: {custom_container_concept_holder.ontology_concept is main_ontology.ontology_custom_container_concept}") -``` - - -For ones already existing in the ontology, they can only be accessed through their corresponding ontology, eg: `soma` as -follows - - -```python jupyter={"outputs_hidden": false} -ontology_manager.print_ontology_class(soma.Cup) +import pycram +import pycrap ``` - - -## Connect ontology class individuals with designators - -After creating `custom_container_concept_holder` (wrapping `custom_container_concept` as an `owlready2.Thing`), we -connect it to a designator (say `obj_designator`) by: - -- Appending to `obj_designator.ontology_concept_holders` with `custom_container_concept_holder` - -- Appending to `custom_container_concept_holder.designators` with `obj_designator` - - - -```python jupyter={"outputs_hidden": false} -custom_container_designator = ObjectDesignatorDescription(names=["obj"]) -custom_container_designator.ontology_concept_holders.append(custom_container_concept_holder) -custom_container_concept_holder.designators.append(custom_container_designator) -``` - - -We can also automatize all the above setup with a single function call - - -```python jupyter={"outputs_hidden": false} -another_custom_container_designator = ontology_manager.create_ontology_linked_designator( - object_name="another_custom_container", - designator_class=ObjectDesignatorDescription, - ontology_concept_name="AnotherCustomContainerConcept", - ontology_parent_class=ontology_designed_container_class) -another_custom_container_concept = another_custom_container_designator.ontology_concept_holders[0].ontology_concept -print(f"Ontology concept: {another_custom_container_concept.name} of class {type(another_custom_container_concept)}") -another_custom_container_designator = OntologyConceptHolderStore().get_ontology_concept_holder_by_name( - main_ontology.AnotherCustomContainerConcept.instances()[0].name).get_default_designator() -print(f"Designator: {another_custom_container_designator.names[0]} of type {type(another_custom_container_designator)}") -``` - - - -## Create new ontology triple classes - -Concept classes of a triple, aka [__subject, predicate, object__], can be created dynamically. Here we will make an -example creating ones for [__handheld objects__] and [__placeholder objects__], with a pair of predicate and inverse -predicate signifying their mutual relation. - - -```python jupyter={"outputs_hidden": false} -PLACEABLE_ON_PREDICATE_NAME = "placeable_on" -HOLD_OBJ_PREDICATE_NAME = "hold_obj" -ontology_manager.create_ontology_triple_classes(ontology_subject_parent_class=soma.DesignedContainer, - subject_class_name="OntologyPlaceHolderObject", - ontology_object_parent_class=soma.Shape, - object_class_name="OntologyHandheldObject", - predicate_class_name=PLACEABLE_ON_PREDICATE_NAME, - inverse_predicate_class_name=HOLD_OBJ_PREDICATE_NAME, - ontology_property_parent_class=soma.affordsBearer, - ontology_inverse_property_parent_class=soma.isBearerAffordedBy) -ontology_manager.print_ontology_property(main_ontology.placeable_on) -ontology_manager.print_ontology_property(main_ontology.hold_obj) -``` - - -There, we use `soma.DesignedContainer` & `soma.Shape`, existing concept in SOMA ontology, as the parent classes for the -subject & object concepts respectively. -There is also a note that those classes will have the same namespace with `main_ontology`, so later on to be accessible -through it. - -Then now we define some instances of the newly created triple classes, and link them to object designators, again -using `ontology_manager.create_ontology_linked_designator()` - - -```python jupyter={"outputs_hidden": false} -def create_ontology_handheld_object_designator(object_name: str, ontology_parent_class: Type[owlready2.Thing]): - return ontology_manager.create_ontology_linked_designator(object_name=object_name, - designator_class=ObjectDesignatorDescription, - ontology_concept_name=f"Onto{object_name}", - ontology_parent_class=ontology_parent_class) - - -# Holdable Objects -cookie_box = create_ontology_handheld_object_designator("cookie_box", main_ontology.OntologyHandheldObject) -egg = create_ontology_handheld_object_designator("egg", main_ontology.OntologyHandheldObject) - -# Placeholder objects -placeholders = [create_ontology_handheld_object_designator(object_name, main_ontology.OntologyPlaceHolderObject) - for object_name in ['table', 'stool', 'shelf']] - -egg_tray = create_ontology_handheld_object_designator("egg_tray", main_ontology.OntologyPlaceHolderObject) -``` - - - -### Create ontology relations - -Now we will create ontology relations or predicates between __placeholder objects__ and __handheld objects__ -with `ontology_manager.set_ontology_relation()` - - -```python jupyter={"outputs_hidden": false} -for place_holder in placeholders: - ontology_manager.set_ontology_relation(subject_designator=cookie_box, object_designator=place_holder, - predicate_name=PLACEABLE_ON_PREDICATE_NAME) - -ontology_manager.set_ontology_relation(subject_designator=egg_tray, object_designator=egg, - predicate_name=HOLD_OBJ_PREDICATE_NAME) -``` - - - -## Query designators based on their ontology-concept relations - -Now we can make queries for designators from designators, based on the relation among their corresponding ontology -concepts setup above - - -```python jupyter={"outputs_hidden": false} -print(f"{cookie_box.names}'s placeholder candidates:", - f"""{[placeholder.names for placeholder in - ontology_manager.get_designators_by_subject_predicate(subject=cookie_box, - predicate_name=PLACEABLE_ON_PREDICATE_NAME)]}""") - -print(f"{egg.names}'s placeholder candidates:", - f"""{[placeholder.names for placeholder in - ontology_manager.get_designators_by_subject_predicate(subject=egg, - predicate_name=PLACEABLE_ON_PREDICATE_NAME)]}""") - -for place_holder in placeholders: - print(f"{place_holder.names} can hold:", - f"""{[placeholder.names for placeholder in - ontology_manager.get_designators_by_subject_predicate(subject=place_holder, - predicate_name=HOLD_OBJ_PREDICATE_NAME)]}""") - -print(f"{egg_tray.names} can hold:", - f"""{[placeholder.names for placeholder in - ontology_manager.get_designators_by_subject_predicate(subject=egg_tray, - predicate_name=HOLD_OBJ_PREDICATE_NAME)]}""") -``` - - - -# Practical examples - -## Example 1 - -How about creating ontology concept classes encapsulating {class}`pycram.datastructures.enums.ObjectType`? We can do it by: - - -```python jupyter={"outputs_hidden": false} -from pycram.datastructures.enums import ObjectType - -# Create a generic ontology concept class for edible objects -generic_edible_class = ontology_manager.create_ontology_concept_class('GenericEdible') - -# Create a list of object designators sharing the same concept class as [generic_edible_class] -edible_obj_types = [ObjectType.MILK, ObjectType.BREAKFAST_CEREAL] -for object_type in ObjectType: - if object_type in edible_obj_types: - # Create a designator for the edible object - ontology_manager.create_ontology_object_designator_from_type(object_type, generic_edible_class) - -print(f'{generic_edible_class.name} object types:') -for edible_ontology_concept in generic_edible_class.direct_instances(): - print(edible_ontology_concept, - [des.types for des in - OntologyConceptHolderStore().get_ontology_concept_holder_by_name(edible_ontology_concept.name).designators]) - -``` - - - -## Example 2 - -We could also make use of relations between ontology concepts that designators are associated with, to enable more -abstract inputs in robot motion plans. - -In a similar style to the scenario of __placeholder objects__ and __handheld objects__ above, but with a bit difference, -we will ask the robot to query which content holders (eg. cup, pitcher, bowl) whereby a milk box could be pourable into. - -Basically, we will provide an ontology-based implementation for the query: - -`abstract_ontology_concept -> specific_objects_in_world?` - -To achieve it, we will create triple classes and configure a customized `resolve()` for the abstract concept, which -returns its associated specific designators. -These designators are then used to again resolve for the target objects of interest, which become the inputs to a robot -motion plan. - -### Setup simulated environment - - +The ontology is structured in classes and instances. The classes are the concepts that are used to describe the world. +The individuals in the ontology can be created when spawning objects in the world. +For example, when you spawn a milk or a cereal box. ```python -from pycram.worlds.bullet_world import BulletWorld, Object +from pycram.worlds.bullet_world import BulletWorld +from pycram.datastructures.enums import WorldMode +from pycram.world_concepts.world_object import Object from pycram.datastructures.pose import Pose -from pycram.process_module import simulated_robot -from pycram.designators.action_designator import * -from pycram.designators.location_designator import * +world = BulletWorld(mode=WorldMode.DIRECT) -world = BulletWorld() -kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") -pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") -kitchen_designator = ObjectDesignatorDescription(names=["kitchen"]) -robot_designator = ObjectDesignatorDescription(names=["pr2"]).resolve() +milk = Object("milk", pycrap.Milk, "milk.stl") +cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", pose=Pose([1.4, 1, 0.95])) ``` -### Create PourableObject-LiquidHolder triple ontology classes +You can query the ontology using [owlready2](https://owlready2.readthedocs.io/en/v0.41/index.html). +For example, we can see all food objects like this: ```python -POURABLE_INTO_PREDICATE_NAME = "pourable_into" -HOLD_LIQUID_PREDICATE_NAME = "hold_liquid" -ontology_manager.create_ontology_triple_classes(ontology_subject_parent_class=soma.DesignedContainer, - subject_class_name="OntologyLiquidHolderObject", - ontology_object_parent_class=soma.Shape, - object_class_name="OntologyPourableObject", - predicate_class_name=POURABLE_INTO_PREDICATE_NAME, - inverse_predicate_class_name=HOLD_LIQUID_PREDICATE_NAME, - ontology_property_parent_class=soma.affordsBearer, - ontology_inverse_property_parent_class=soma.isBearerAffordedBy) +print("All food instances", list(world.ontology.search(type=pycrap.Food))) ``` -### Spawn a pourable object & liquid holders into the world and Create their designators +You can also search for objects in the world using the ontology: ```python -# Holdable obj -milk_box = Object("milk_box", ObjectType.MILK, "milk.stl") -milk_box_designator = create_ontology_handheld_object_designator(milk_box.name, main_ontology.OntologyPourableObject) - -# Liquid-holders -cup = Object("cup", ObjectType.JEROEN_CUP, "jeroen_cup.stl", pose=Pose([1.4, 1, 0.9])) -bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([1.4, 0.5, 0.9])) -pitcher = Object("pitcher", ObjectType.GENERIC_OBJECT, "Static_MilkPitcher.stl", pose=Pose([1.4, 0, 0.9])) -milk_holders = [cup, bowl, pitcher] -milk_holder_designators = [ - create_ontology_handheld_object_designator(obj.name, main_ontology.OntologyLiquidHolderObject) - for obj in milk_holders] -``` - -### Create an ontology relation between the designators of the pourable object & its liquid holders - -```python -for milk_holder_desig in milk_holder_designators: - ontology_manager.set_ontology_relation(subject_designator=milk_box_designator, object_designator=milk_holder_desig, - predicate_name=POURABLE_INTO_PREDICATE_NAME) -``` - -### Set up `resolve` for the ontology concept of the pourable object - -```python -milk_box_concept_holder = milk_box_designator.ontology_concept_holders[0] - - -def milk_box_concept_resolve(): - object_designator = ontology_manager.get_designators_by_subject_predicate(subject=milk_box_designator, - predicate_name=POURABLE_INTO_PREDICATE_NAME)[ - 0] - return object_designator, object_designator.resolve() - - -milk_box_concept_holder.resolve = milk_box_concept_resolve -``` - -Here, for demonstration purpose only, we specify the resolving result by `milk_box_concept_holder` as `cup`, the -first-registered (default) pourable-into target milk holder, utilizing the ontology relation setup above. - -Now, we can query the milk box's target liquid holder by resolving `milk_box_concept_holder` - -```python -target_milk_holder_designator, target_milk_holder = milk_box_concept_holder.resolve() -print( - f"Pickup target object: {target_milk_holder.name}, a content holder for {milk_box_designator.names} as in relation `{POURABLE_INTO_PREDICATE_NAME}`") +from pycram.designators.object_designator import OntologyObjectDesignatorDescription +object_designator = OntologyObjectDesignatorDescription(world.ontology.search(type=pycrap.Food)) +result_in_world = list(object_designator.__iter__()) +print(result_in_world) ``` -### Robot picks up the target liquid holder - -```python -with simulated_robot: - ParkArmsAction([Arms.BOTH]).resolve().perform() - - MoveTorsoAction([0.3]).resolve().perform() - - pickup_pose = CostmapLocation(target=target_milk_holder, reachable_for=robot_designator).resolve() - pickup_arm = pickup_pose.reachable_arms[0] - - print(pickup_pose, pickup_arm) - - NavigateAction(target_locations=[pickup_pose.pose]).resolve().perform() - - PickUpAction(object_designator_description=target_milk_holder_designator, arms=[pickup_arm], - grasps=[Grasp.FRONT]).resolve().perform() - - ParkArmsAction([Arms.BOTH]).resolve().perform() +Architecturally speaking, the ontology is a part of the world and is accessible through the world object. +Objects created in a world will only appear in the ontology of that world. - place_island = SemanticCostmapLocation("kitchen_island_surface", kitchen_designator.resolve(), - target_milk_holder_designator.resolve()).resolve() +## Extending the Ontology - place_stand = CostmapLocation(place_island.pose, reachable_for=robot_designator, reachable_arm=pickup_arm).resolve() +Feel free to extend the ontology with new classes and relations as you go! +These will be then reviewed in the pull requests so don't be shy! - NavigateAction(target_locations=[place_stand.pose]).resolve().perform() - - PlaceAction(target_milk_holder_designator, target_locations=[place_island.pose], - arms=[pickup_arm]).resolve().perform() - - ParkArmsAction([Arms.BOTH]).resolve().perform() -world.exit() -``` - -# Save ontologies to an OWL file - -After all the above operations on our ontologies, we now can save them to an OWL file on disk - -```python -ontology_manager.save(f"{Path.home()}/ontologies/New{main_ontology.name}.owl") -``` +If you are looking to integrate PyCRAP with some other ontology like [Soma](https://ease-crc.github.io/soma/), you +can do so by denoting the class equivalences in the PyCRAP ontology. +Details on this are found in the [owlready2 properties documentation](https://owlready2.readthedocs.io/en/v0.41/properties.html#obtaining-indirect-relations-considering-subproperty-transitivity-etc). -# Optimize ontology loading with SQLite3 +Currently, only objects spawned during runtime are tracked in the A-Box of PyCRAP. -Upon the initial ontology loading from OWL, an SQLite3 file is automatically created, acting as the quadstore cache for -the loaded ontologies. This allows them to be __selectively__ reusable the next time being loaded. -More info can be referenced [here](https://owlready2.readthedocs.io/en/latest/world.html). +The roadmap for a full integration is as follows: + - Map the entire belief state in the A-Box, including + - Relevant links from the URDF + - Robot descriptions + - Find a way to get action descriptions from ontological statements + - Use the ontology to guide the robot in its decision making + diff --git a/examples/orm_example.md b/examples/orm_example.md index 6231a04a5..442bcd7c1 100644 --- a/examples/orm_example.md +++ b/examples/orm_example.md @@ -48,12 +48,13 @@ from pycram.designators.object_designator import * from pycram.datastructures.pose import Pose from pycram.orm.base import ProcessMetaData import anytree +import pycrap world = BulletWorld(WorldMode.DIRECT) -pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") -kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) -cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) +pr2 = Object("pr2", pycrap.Robot, "pr2.urdf") +kitchen = Object("kitchen", pycrap.Kitchen, "kitchen.urdf") +milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) +cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) milk_desig = ObjectDesignatorDescription(names=["milk"]) cereal_desig = ObjectDesignatorDescription(names=["cereal"]) robot_desig = ObjectDesignatorDescription(names=["pr2"]).resolve() diff --git a/examples/orm_querying_examples.md b/examples/orm_querying_examples.md index 975bc1efe..f6ded7f47 100644 --- a/examples/orm_querying_examples.md +++ b/examples/orm_querying_examples.md @@ -12,244 +12,8 @@ jupyter: name: python3 --- -# ORM querying examples +# ORM querying -In this tutorial, we will get to see more examples of ORM querying. - - -First, we will gather a lot of data. In order to achieve that we will write a randomized experiment for grasping a couple of objects. -In the experiment the robot will try to grasp a randomized object using random poses and torso heights. - -```python -from tf import transformations -import itertools -from typing import Optional, List, Tuple -import numpy as np -import sqlalchemy.orm -import tqdm -import pycram.orm.base -from pycram.worlds.bullet_world import BulletWorld -from pycram.world_concepts.world_object import Object as BulletWorldObject -from pycram.designators.action_designator import MoveTorsoAction, PickUpAction, NavigateAction, ParkArmsAction, ParkArmsActionPerformable, MoveTorsoActionPerformable -from pycram.designators.object_designator import ObjectDesignatorDescription -from pycram.failures import PlanFailure -from pycram.process_module import ProcessModule -from pycram.datastructures.enums import Arms, ObjectType, Grasp, WorldMode -from pycram.process_module import simulated_robot -from pycram.orm.action_designator import PickUpAction as ORMPickUpAction -from pycram.orm.base import RobotState, Position, ProcessMetaData, Pose as ORMPose -from pycram.orm.tasktree import TaskTreeNode -from pycram.orm.object_designator import Object -from pycram.tasktree import task_tree, TaskTree -import pycram.orm -import sqlalchemy.sql -import pandas as pd - -from pycram.datastructures.pose import Pose - -np.random.seed(420) - -ProcessModule.execution_delay = False -ProcessMetaData().description = "Tutorial for learning from experience in a Grasping action." - - -class GraspingExplorer: - """Class to try randomized grasping plans.""" - - world: Optional[BulletWorld] - - def __init__(self, robots: Optional[List[Tuple[str, str]]] = None, objects: Optional[List[Tuple[str, str]]] = None, - arms: Optional[List[Arms]] = None, grasps: Optional[List[Grasp]] = None, - samples_per_scenario: int = 1000): - """ - Create a GraspingExplorer. - :param robots: The robots to use - :param objects: The objects to try to grasp - :param arms: The arms of the robot to use - :param grasps: The grasp orientations to use - :param samples_per_scenario: The number of tries per scenario. - """ - # store exploration space - if not robots: - self.robots: List[Tuple[str, str]] = [("pr2", "pr2.urdf")] - - if not objects: - self.objects: List[Tuple[str, ObjectType, str]] = [ - ("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl"), - ("bowl", ObjectType.BOWL, "bowl.stl"), - ("milk", ObjectType.MILK, "milk.stl"), - ("spoon", ObjectType.SPOON, "spoon.stl")] - - if not arms: - self.arms: List[Arms] = [Arms.LEFT, Arms.RIGHT] - - if not grasps: - self.grasps: List[Grasp] = [Grasp.LEFT, Grasp.RIGHT, Grasp.FRONT, Grasp.TOP] - - # store trials per scenario - self.samples_per_scenario: int = samples_per_scenario - - # chain hyperparameters - self.hyper_parameters = [self.robots, self.objects, self.arms, self.grasps] - - self.total_tries = 0 - self.total_failures = 0 - - def perform(self, session: sqlalchemy.orm.Session): - """ - Perform all experiments. - :param session: The database-session to insert the samples in. - """ - - # create progress bar - progress_bar = tqdm.tqdm( - total=np.prod([len(p) for p in self.hyper_parameters]) * self.samples_per_scenario) - - self.world = BulletWorld(WorldMode.DIRECT) - - # for every robot - for robot, robot_urdf in self.robots: - - # spawn it - robot = BulletWorldObject(robot, ObjectType.ROBOT, robot_urdf) - - # for every obj - for obj, obj_type, obj_stl in self.objects: - - # spawn it - bw_object = BulletWorldObject(obj, obj_type, obj_stl, pose=Pose([0, 0, 0.75], [0, 0, 0, 1])) - - # create object designator - object_designator = ObjectDesignatorDescription(names=[obj]) - - # for every arm and grasp pose - for arm, grasp in itertools.product(self.arms, self.grasps): - # sample positions in 2D - positions = np.random.uniform([-2, -2], [2, 2], (self.samples_per_scenario, 2)) - - # for every position - for position in positions: - - # set z axis to 0 - position = [*position, 0] - - # calculate orientation for robot to face the object - angle = np.arctan2(position[1], position[0]) + np.pi - orientation = list(transformations.quaternion_from_euler(0, 0, angle, axes="sxyz")) - - # try to execute a grasping plan - with simulated_robot: - ParkArmsActionPerformable(Arms.BOTH).perform() - # navigate to sampled position - NavigateAction([Pose(position, orientation)]).resolve().perform() - - # move torso - height = np.random.uniform(0., 0.33, 1)[0] - MoveTorsoActionPerformable(height).perform() - - # try to pick it up - try: - PickUpAction(object_designator, [arm], [grasp]).resolve().perform() - - # if it fails - except PlanFailure: - - # update failure stats - self.total_failures += 1 - - # reset BulletWorld - self.world.reset_world() - - # update progress bar - self.total_tries += 1 - - # insert into database - task_tree.root.insert(session, use_progress_bar=False) - task_tree.reset_tree() - - progress_bar.update() - progress_bar.set_postfix(success_rate=(self.total_tries - self.total_failures) / - self.total_tries) - - bw_object.remove() - robot.remove() - -``` - -Next we have to establish a connection to a database and execute the experiment a couple of times. Note that the (few) number of samples we generate is only for demonstrations. -For robust and reliable machine learning millions of samples are required. - - -```python -engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:") -session = sqlalchemy.orm.Session(bind=engine) -pycram.orm.base.Base.metadata.create_all(bind=engine) -session.commit() - -explorer = GraspingExplorer(samples_per_scenario=10) -explorer.perform(session) -``` - -The success_rate of the output above indicates how many of our samples succeeded in trying to grasp a randomized object. - - -Now that we have data to query from and a running session, we can actually start creating queries. -Let's say we want to select positions of robots that were able to grasp a specific object (in this case a "milk" object): - -```python -from sqlalchemy import select -from pycram.datastructures.enums import ObjectType - -milk = BulletWorldObject("milk", ObjectType.MILK, "milk.stl") - -# query all relative robot positions in regard to an objects position -# make sure to order the joins() correctly -query = (select(ORMPickUpAction.arm, ORMPickUpAction.grasp, RobotState.torso_height, Position.x, Position.y) - .join(TaskTreeNode.action.of_type(ORMPickUpAction)) - .join(ORMPickUpAction.robot_state) - .join(RobotState.pose) - .join(ORMPose.position) - .join(ORMPickUpAction.object).where(Object.obj_type == milk.obj_type) - .where(TaskTreeNode.status == "SUCCEEDED")) -print(query) - -df = pd.read_sql_query(query, session.get_bind()) -print(df) -``` - -If you are not familiar with sqlalchemy querying you might wonder what the of_type() function does and why we needed it in this query: - -In order to understand the importance of the of_type() function in the joins above it is crucial to understand the inheritance structure in the ORM package. The action necessary for this query is the PickUpAction. It inherits the Action class/table (which holds all the actions). The Action class itself on the other hand inherits Designator (which holds all the actions, but also all the motions). -We started our joins by joining TaskTreeNode on its relationship to Code and Code on its relationship to Designator. Next table we need is the PickUpAction table, but there is no specified relationship between Designator and PickUpAction. But we do know that a PickUpAction is actually a Designator, meaning, it inherits from Designator. So we can just "tell" the join to join Code on every Designator, that is "of_type" PickUpAction (.join(Code.designator.of_type(ORMPickUpAction))). -The effect of this function can also be seen in the printed query of above's output. - - -Another interesting query: Let's say we want to select the torso height and positions of robots relative to the object they were trying to grasp: - -```python -robot_pose = sqlalchemy.orm.aliased(ORMPose) -object_pose = sqlalchemy.orm.aliased(ORMPose) -robot_position = sqlalchemy.orm.aliased(Position) -object_position = sqlalchemy.orm.aliased(Position) - -query = (select(TaskTreeNode.status, Object.obj_type, - sqlalchemy.label("relative torso height", object_position.z - RobotState.torso_height), - sqlalchemy.label("x", robot_position.x - object_position.x), - sqlalchemy.label("y", robot_position.y - object_position.y)) - .join(TaskTreeNode.action.of_type(ORMPickUpAction)) - .join(ORMPickUpAction.robot_state) - .join(robot_pose, RobotState.pose) - .join(robot_position, robot_pose.position) - .join(ORMPickUpAction.object) - .join(object_pose, Object.pose) - .join(object_position, object_pose.position)) -print(query) - -df = pd.read_sql(query, session.get_bind()) -df["status"] = df["status"].apply(lambda x: str(x)) -print(df) -``` - -Obviously the query returned every row of the database since we didn't apply any filters. - -Why is this query interesting? This query not only required more joins and the usage of the of_type() function, but we actually needed to access two of the tables twice with different purposes, namely the Pose and Position tables. We wanted to get the position of the robot relative to the object position, meaning we had to obtain all robot positions and all object positions. If we want to access the same table twice, we have to make sure to rename (one of) the occurrences in our query in order to provide proper sql syntax. This can be done by creating aliases using the sqlalchemy.orm.aliased() function. Sqlalchemy will automatically rename all the aliased tables for you during runtime. +Querying the ORM can be done by writing custom queries using sqlalchemy. +However, if you regulary want to access a chunk of data, it is recommended to write a view that does this for you. +You can see the details of writing a view in here :py:func:`pycram.orm.base.views.PickUpWithContextView`. \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index ff36b1900..4b14d5a35 100644 --- a/requirements.txt +++ b/requirements.txt @@ -6,7 +6,7 @@ numpy==1.24.3 pytransform3d~=1.9.1 graphviz anytree>=2.8.0 -SQLAlchemy>=2.0.9 +SQLAlchemy>=2.0.36 tqdm==4.66.3 psutil==5.9.7 lxml==4.9.1 diff --git a/scripts/generate_database_for_ci.py b/scripts/generate_database_for_ci.py new file mode 100644 index 000000000..55a6d30bd --- /dev/null +++ b/scripts/generate_database_for_ci.py @@ -0,0 +1,65 @@ +""" +This script generates the required database for the CI pipeline to execute notebooks. +This database is required for the execution of pycram/examples/improving_actions.ipynb. + +For this script to work, you have to set a local variable in your environment called PYCRORM_CI_URI. +This variable contains the URI to the database is used for the CI pipeline with the credentials for an account that +has writing permissions to the database. + +ONLY EXECUTE THIS IF YOU ARE SURE THAT YOU WANT TO DELETE THE DATABASE AND CREATE A NEW ONE. +""" + +import os +import random + +import numpy as np +import sqlalchemy.orm + +from pycrap import Robot, Milk + +import pycram.orm.base +from pycram.designators.object_designator import ObjectDesignatorDescription +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +from pycram.datastructures.enums import WorldMode +from pycram.datastructures.pose import Pose +from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher +from pycram.process_module import ProcessModule, simulated_robot +from pycram.designators.specialized_designators.probabilistic.probabilistic_action import MoveAndPickUp, Arms, Grasp +from pycram.tasktree import task_tree +import pycram.orm.base + + +def main(): + ProcessModule.execution_delay = False + np.random.seed(69) + random.seed(69) + + pycrorm_uri = os.environ['PYCRORM_URI'] # os.environ['PYCRORM_CI_URI'] + pycrorm_uri = "mysql+pymysql://" + pycrorm_uri + + engine = sqlalchemy.create_engine(pycrorm_uri) + session = sqlalchemy.orm.sessionmaker(bind=engine)() + pycram.orm.base.Base.metadata.create_all(engine) + + world = BulletWorld(WorldMode.DIRECT) + + robot = Object("pr2", Robot, "pr2.urdf") + milk = Object("milk", Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) + viz_marker_publisher = VizMarkerPublisher() + milk_description = ObjectDesignatorDescription(types=[Milk]).ground() + + fpa = MoveAndPickUp(milk_description, arms=[Arms.LEFT, Arms.RIGHT], + grasps=[Grasp.FRONT.value, Grasp.LEFT.value, Grasp.RIGHT.value, Grasp.TOP.value]) + + pycram.orm.base.ProcessMetaData().description = "Experimenting with Pick Up Actions" + fpa.sample_amount = 100 + with simulated_robot: + fpa.batch_rollout() + task_tree.root.insert(session) + session.commit() + task_tree.reset_tree() + world.exit() + +if __name__ == "__main__": + main() diff --git a/scripts/test_notebook_examples.sh b/scripts/test_notebook_examples.sh new file mode 100644 index 000000000..f381fa86a --- /dev/null +++ b/scripts/test_notebook_examples.sh @@ -0,0 +1,11 @@ +#!/bin/bash +source /opt/ros/noetic/setup.bash +source ~/catkin_ws/devel/setup.bash +roscd pycram/examples +rm -rf tmp +mkdir tmp +jupytext --to notebook *.md +mv *.ipynb tmp +cd tmp +roslaunch pycram ik_and_description.launch & +treon --thread 1 -v --exclude=migrate_neems.ipynb --exclude=improving_actions.ipynb \ No newline at end of file diff --git a/src/pycram/datastructures/mixins.py b/src/pycram/datastructures/mixins.py new file mode 100644 index 000000000..5ff4dc555 --- /dev/null +++ b/src/pycram/datastructures/mixins.py @@ -0,0 +1,22 @@ +from typing_extensions import Type, Optional + +from pycrap import Base + + +class HasConcept: + """ + A mixin class that adds an ontological concept and individual to a class that will be registered in PyCRAP. + """ + + ontology_concept: Type[Base] = Base + """ + The ontological concept that this class represents. + """ + + ontology_individual: Optional[Base] = None + """ + The individual in the ontology that is connected with this class. + """ + + def __init__(self): + self.ontology_individual = self.ontology_concept() \ No newline at end of file diff --git a/src/pycram/datastructures/world.py b/src/pycram/datastructures/world.py index cca0b1e45..c69c704bf 100644 --- a/src/pycram/datastructures/world.py +++ b/src/pycram/datastructures/world.py @@ -11,6 +11,8 @@ from geometry_msgs.msg import Point from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type +import pycrap +from pycrap import PhysicalObject from ..cache_manager import CacheManager from ..config.world_conf import WorldConfig from ..datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks, @@ -72,6 +74,11 @@ class World(StateEntity, ABC): Global reference for the cache manager, this is used to cache the description files of the robot and the objects. """ + ontology: Optional[pycrap.Ontology] = None + """ + The ontology of this world. + """ + def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_cache: bool = False): """ Create a new simulation, the mode decides if the simulation should be a rendered window or just run in the @@ -85,6 +92,9 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_ca """ StateEntity.__init__(self) + + self.ontology = pycrap.Ontology() + self.latest_state_id: Optional[int] = None if clear_cache or (self.conf.clear_cache_at_start and not self.cache_manager.cache_cleared): @@ -112,7 +122,6 @@ def __init__(self, mode: WorldMode, is_prospection_world: bool = False, clear_ca self._update_local_transformer_worlds() self.mode: WorldMode = mode - # The mode of the simulation, can be "GUI" or "DIRECT" self.coll_callbacks: Dict[Tuple[Object, Object], CollisionCallbacks] = {} @@ -284,7 +293,7 @@ def simulation_time_step(self): @abstractmethod def load_object_and_get_id(self, path: Optional[str] = None, pose: Optional[Pose] = None, - obj_type: Optional[ObjectType] = None) -> int: + obj_type: Optional[Type[PhysicalObject]] = None) -> int: """ Load a description file (e.g. URDF) at the given pose and returns the id of the loaded object. @@ -903,6 +912,7 @@ def exit(self, remove_saved_states: bool = True) -> None: self.disconnect_from_physics_server() self.reset_robot() self.join_threads() + self.ontology.destroy_individuals() if World.current_world == self: World.current_world = None diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 4790bac19..1a9ee3eb5 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -1,10 +1,11 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -from dataclasses import dataclass, field, fields from abc import ABC, abstractmethod +from dataclasses import dataclass, field, fields from inspect import isgenerator, isgeneratorfunction +from pycrap import PhysicalObject, Agent from .ros.logging import logwarn, loginfo try: @@ -20,7 +21,7 @@ from .utils import GeneratorList, bcolors from threading import Lock from time import time -from typing_extensions import Type, List, Dict, Any, Optional, Union, get_type_hints, Callable, Iterable, TYPE_CHECKING, get_args, get_origin +from typing_extensions import Type, List, Dict, Any, Optional, Union, Callable, Iterable, TYPE_CHECKING from .local_transformer import LocalTransformer from .language import Language @@ -77,7 +78,6 @@ class Designator(ABC): :ivar timestamp: The timestamp of creation of reference or None if still not referencing an object. """ - resolvers = {} """ List of all designator resolvers. Designator resolvers are functions which take a designator as @@ -330,17 +330,15 @@ class DesignatorDescription(ABC): :ivar resolve: The specialized_designators function to use for this designator, defaults to self.ground """ - def __init__(self, resolver: Optional[Callable] = None, ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): + def __init__(self, resolver: Optional[Callable] = None): """ Create a Designator description. :param resolver: The grounding method used for the description. The grounding method creates a location instance that matches the description. - :param ontology_concept_holders: A list of holders of ontology concepts that the designator is categorized as or associated with """ if resolver is None: self.resolve = self.ground - self.ontology_concept_holders = [] if ontology_concept_holders is None else ontology_concept_holders def make_dictionary(self, properties: List[str]): """ @@ -374,11 +372,7 @@ def get_slots(self) -> List[str]: def copy(self) -> DesignatorDescription: return self - def get_default_ontology_concept(self) -> owlready2.Thing | None: - """ - :return: The first element of ontology_concept_holders if there is, else None - """ - return self.ontology_concept_holders[0].ontology_concept if self.ontology_concept_holders else None + class ActionDesignatorDescription(DesignatorDescription, Language): """ @@ -400,14 +394,15 @@ class Action: The torso height of the robot at the start of the action. """ - robot_type: ObjectType = field(init=False) + robot_type: Type[Agent] = field(init=False) """ The type of the robot at the start of the action. """ def __post_init__(self): self.robot_position = World.robot.get_pose() - self.robot_torso_height = World.robot.get_joint_position(RobotDescription.current_robot_description.torso_joint) + self.robot_torso_height = World.robot.get_joint_position( + RobotDescription.current_robot_description.torso_joint) self.robot_type = World.robot.obj_type @with_tree @@ -442,7 +437,7 @@ def insert(self, session: Session, *args, **kwargs) -> ORMAction: metadata = ProcessMetaData().insert(session) # create robot-state object - robot_state = RobotState(self.robot_torso_height, self.robot_type) + robot_state = RobotState(self.robot_torso_height, str(self.robot_type)) robot_state.pose = pose robot_state.process_metadata = metadata session.add(robot_state) @@ -454,37 +449,19 @@ def insert(self, session: Session, *args, **kwargs) -> ORMAction: return action - def __init__(self, resolver=None, ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): + def __init__(self, resolver=None): """ Base of all action designator descriptions. :param resolver: An alternative resolver that returns an action designator - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) Language.__init__(self) - from .ontology.ontology import OntologyManager - self.soma = OntologyManager().soma def ground(self) -> Action: """Fill all missing parameters and chose plan to execute. """ raise NotImplementedError(f"{type(self)}.ground() is not implemented.") - def init_ontology_concepts(self, ontology_concept_classes: Dict[str, Type[owlready2.Thing]]): - """ - Initialize the ontology concept holders for this action designator - - :param ontology_concept_classes: The ontology concept classes that the action is categorized as or associated with - :param ontology_concept_name: The name of the ontology concept instance to be created - """ - from .ontology.ontology_common import OntologyConceptHolderStore, OntologyConceptHolder - if not self.ontology_concept_holders: - for concept_name, concept_class in ontology_concept_classes.items(): - if concept_class: - existing_holders = OntologyConceptHolderStore().get_ontology_concept_holders_by_class(concept_class) - self.ontology_concept_holders.extend(existing_holders if existing_holders \ - else [OntologyConceptHolder(concept_class(concept_name))]) - def __iter__(self): """ Iterate through all possible performables fitting this description @@ -510,8 +487,8 @@ class Location: The resolved pose of the location designator. Pose is inherited by all location designator. """ - def __init__(self, resolver=None, ontology_concept_holders: Optional[List[owlready2.Thing]] = None): - super().__init__(resolver, ontology_concept_holders) + def __init__(self, resolver=None): + super().__init__(resolver) def ground(self) -> Location: """ @@ -520,7 +497,7 @@ def ground(self) -> Location: raise NotImplementedError(f"{type(self)}.ground() is not implemented.") -#this knowledge should be somewhere else i guess +# this knowledge should be somewhere else i guess SPECIAL_KNOWLEDGE = { 'bigknife': [("top", [-0.08, 0, 0])], @@ -575,7 +552,8 @@ def to_sql(self) -> ORMObjectDesignator: :return: The created ORM object. """ - return ORMObjectDesignator(name=self.name, obj_type=self.obj_type) + print(str(self.obj_type)) + return ORMObjectDesignator(name=self.name, obj_type=str(self.obj_type)) def insert(self, session: Session) -> ORMObjectDesignator: """ @@ -597,7 +575,10 @@ def insert(self, session: Session) -> ORMObjectDesignator: def frozen_copy(self) -> 'ObjectDesignatorDescription.Object': """ - :return: A copy containing only the fields of this class. The WorldObject attached to this pycram object is not copied. The _pose gets set to a method that statically returns the pose of the object when this method was called. + :return: A copy containing only the fields of this class. + The WorldObject attached to this pycram object is not copied. + The _pose gets set to a method + that statically returns the pose of the object when this method was called. """ result = ObjectDesignatorDescription.Object(self.name, self.obj_type, None) # get current object pose and set resulting pose to always be that @@ -654,17 +635,16 @@ def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: return pose_in_object return pose - def __init__(self, names: Optional[List[str]] = None, types: Optional[List[ObjectType]] = None, - resolver: Optional[Callable] = None, ontology_concept_holders: Optional[List[owlready2.Thing]] = None): + def __init__(self, names: Optional[List[str]] = None, types: Optional[List[Type[PhysicalObject]]] = None, + resolver: Optional[Callable] = None): """ Base of all object designator descriptions. Every object designator has the name and type of the object. :param names: A list of names that could describe the object :param types: A list of types that could represent the object :param resolver: An alternative specialized_designators that returns an object designator for the list of names and types - :param ontology_concept_holders: A list of ontology concepts that the object is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.types: Optional[List[ObjectType]] = types self.names: Optional[List[str]] = names @@ -695,6 +675,7 @@ def __iter__(self) -> Iterable[Object]: yield self.Object(obj.name, obj.obj_type, obj) + @dataclass class BaseMotion(ABC): @@ -737,26 +718,29 @@ def __post_init__(self): """ Checks if types are missing or wrong """ - right_types = get_type_hints(self) - attributes = self.__dict__.copy() - - missing = [] - wrong_type = {} - current_type = {} - - for k in attributes.keys(): - attribute = attributes[k] - attribute_type = type(attributes[k]) - right_type = right_types[k] - types = get_args(right_type) - if attribute is None: - if not any([x is type(None) for x in get_args(right_type)]): - missing.append(k) - elif attribute_type is not right_type: - if attribute_type not in types: - if attribute_type not in [get_origin(x) for x in types if x is not type(None)]: - wrong_type[k] = right_types[k] - current_type[k] = attribute_type - if missing != [] or wrong_type != {}: - raise ResolutionError(missing, wrong_type, current_type, self.__class__) - + return + # TODO include type checks for this again (use type guard?) + # + # right_types = get_type_hints(self) + # attributes = self.__dict__.copy() + # + # missing = [] + # wrong_type = {} + # current_type = {} + # + # for k in attributes.keys(): + # attribute = attributes[k] + # attribute_type = type(attributes[k]) + # right_type = right_types[k] + # types = get_args(right_type) + # if attribute is None: + # if not any([x is type(None) for x in get_args(right_type)]): + # missing.append(k) + # elif not issubclass(attribute_type, right_type): # attribute_type is not right_type: + # if attribute_type not in types: + # if attribute_type not in [get_origin(x) for x in types if x is not type(None)]: + # wrong_type[k] = right_types[k] + # current_type[k] = attribute_type + # if missing != [] or wrong_type != {}: + # raise ResolutionError(missing, wrong_type, current_type, self.__class__) + # diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 23aad0f5d..bad8e3acf 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -25,7 +25,6 @@ from ..designator import ActionDesignatorDescription from ..datastructures.pose import Pose from ..datastructures.world import World -from ..ontology.ontology import OntologyConceptHolder from ..orm.action_designator import (ParkArmsAction as ORMParkArmsAction, NavigateAction as ORMNavigateAction, PickUpAction as ORMPickUpAction, PlaceAction as ORMPlaceAction, @@ -45,20 +44,16 @@ class MoveTorsoAction(ActionDesignatorDescription): Action Designator for Moving the torso of the robot up and down """ - def __init__(self, positions: List[float], resolver=None, - ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): + def __init__(self, positions: List[float], resolver=None): """ Create a designator description to move the torso of the robot up and down. :param positions: List of possible positions of the robots torso, possible position is a float of height in metres :param resolver: An optional specialized_designators that returns a performable designator for a designator description. - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.positions: List[float] = positions - if self.soma: - self.init_ontology_concepts({"move_torso": self.soma.MoveTorso}) def ground(self) -> MoveTorsoActionPerformable: """ @@ -83,23 +78,18 @@ class SetGripperAction(ActionDesignatorDescription): Set the gripper state of the robot """ - def __init__(self, grippers: List[Arms], motions: List[GripperState], resolver=None, - ontology_concept_holders: Optional[List[Thing]] = None): + def __init__(self, grippers: List[Arms], motions: List[GripperState], resolver=None,): """ Sets the gripper state, the desired state is given with the motion. Motion can either be 'open' or 'close'. :param grippers: A list of possible grippers :param motions: A list of possible motions :param resolver: An alternative specialized_designators that returns a performable designator for a designator description - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.grippers: List[GripperState] = grippers self.motions: List[Arms] = motions - if self.soma: - self.init_ontology_concepts({"setting_gripper": self.soma.SettingGripper}) - def ground(self) -> SetGripperActionPerformable: """ Default specialized_designators that returns a performable designator with the first element in the grippers and motions list. @@ -126,14 +116,11 @@ class ReleaseAction(ActionDesignatorDescription): """ def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, - resolver=None, ontology_concept_holders: Optional[List[Thing]] = None): - super().__init__(resolver, ontology_concept_holders) + resolver=None): + super().__init__(resolver) self.grippers: List[Arms] = grippers self.object_designator_description = object_designator_description - if self.soma: - self.init_ontology_concepts({"releasing": self.soma.Releasing}) - def ground(self) -> ReleaseActionPerformable: return ReleaseActionPerformable(self.grippers[0], self.object_designator_description.ground()) @@ -150,15 +137,12 @@ class GripAction(ActionDesignatorDescription): """ def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, - efforts: List[float], resolver=None, ontology_concept_holders: Optional[List[Thing]] = None): - super().__init__(resolver, ontology_concept_holders) + efforts: List[float], resolver=None): + super().__init__(resolver) self.grippers: List[Arms] = grippers self.object_designator_description: ObjectDesignatorDescription = object_designator_description self.efforts: List[float] = efforts - if self.soma: - self.init_ontology_concepts({"holding": self.soma.Holding}) - def ground(self) -> GripActionPerformable: return GripActionPerformable(self.grippers[0], self.object_designator_description.ground(), self.efforts[0]) @@ -168,20 +152,16 @@ class ParkArmsAction(ActionDesignatorDescription): Park the arms of the robot. """ - def __init__(self, arms: List[Arms], resolver=None, - ontology_concept_holders: Optional[List[Thing]] = None): + def __init__(self, arms: List[Arms], resolver=None): """ Moves the arms in the pre-defined parking position. Arms are taken from pycram.enum.Arms :param arms: A list of possible arms, that could be used :param resolver: An optional specialized_designators that returns a performable designator from the designator description - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.arms: List[Arms] = arms - if self.soma: - self.init_ontology_concepts({"parking_arms": self.soma.ParkingArms}) def ground(self) -> ParkArmsActionPerformable: """ @@ -199,8 +179,7 @@ class PickUpAction(ActionDesignatorDescription): def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], - arms: List[Arms], grasps: List[Grasp], resolver=None, - ontology_concept_holders: Optional[List[Thing]] = None): + arms: List[Arms], grasps: List[Grasp], resolver=None): """ Lets the robot pick up an object. The description needs an object designator describing the object that should be picked up, an arm that should be used as well as the grasp from which side the object should be picked up. @@ -209,16 +188,13 @@ def __init__(self, :param arms: List of possible arms that could be used :param grasps: List of possible grasps for the object :param resolver: An optional specialized_designators that returns a performable designator with elements from the lists of possible paramter - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.object_designator_description: Union[ ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description self.arms: List[Arms] = arms self.grasps: List[Grasp] = grasps - if self.soma: - self.init_ontology_concepts({"picking_up": self.soma.PickingUp}) def ground(self) -> PickUpActionPerformable: """ @@ -242,7 +218,7 @@ class PlaceAction(ActionDesignatorDescription): def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], target_locations: List[Pose], - arms: List[Arms], resolver=None, ontology_concept_holders: Optional[List[Thing]] = None): + arms: List[Arms], resolver=None): """ Create an Action Description to place an object @@ -250,17 +226,13 @@ def __init__(self, :param target_locations: List of possible positions/orientations to place the object :param arms: List of possible arms to use :param resolver: Grounding method to resolve this designator - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.object_designator_description: Union[ ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description self.target_locations: List[Pose] = target_locations self.arms: List[Arms] = arms - if self.soma: - self.init_ontology_concepts({"placing": self.soma.Placing}) - def ground(self) -> PlaceActionPerformable: """ Default specialized_designators that returns a performable designator with the first entries from the list of possible entries. @@ -278,20 +250,16 @@ class NavigateAction(ActionDesignatorDescription): Navigates the Robot to a position. """ - def __init__(self, target_locations: List[Pose], resolver=None, - ontology_concept_holders: Optional[List[Thing]] = None): + def __init__(self, target_locations: List[Pose], resolver=None): """ Navigates the robot to a location. :param target_locations: A list of possible target locations for the navigation. :param resolver: An alternative specialized_designators that creates a performable designator from the list of possible parameter - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.target_locations: List[Pose] = target_locations - if self.soma: - self.init_ontology_concepts({"navigating": self.soma.Navigating}) def ground(self) -> NavigateActionPerformable: """ @@ -310,8 +278,7 @@ class TransportAction(ActionDesignatorDescription): def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], arms: List[Arms], - target_locations: List[Pose], resolver=None, - ontology_concept_holders: Optional[List[Thing]] = None): + target_locations: List[Pose], resolver=None): """ Designator representing a pick and place plan. @@ -319,17 +286,13 @@ def __init__(self, :param arms: A List of possible arms that could be used for transporting :param target_locations: A list of possible target locations for the object to be placed :param resolver: An alternative specialized_designators that returns a performable designator for the list of possible parameter - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.object_designator_description: Union[ ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description self.arms: List[Arms] = arms self.target_locations: List[Pose] = target_locations - if self.soma: - self.init_ontology_concepts({"transporting": self.soma.Transporting}) - def ground(self) -> TransportActionPerformable: """ Default specialized_designators that returns a performable designator with the first entries from the lists of possible parameter. @@ -348,21 +311,16 @@ class LookAtAction(ActionDesignatorDescription): Lets the robot look at a position. """ - def __init__(self, targets: List[Pose], resolver=None, - ontology_concept_holders: Optional[List[Thing]] = None): + def __init__(self, targets: List[Pose], resolver=None): """ Moves the head of the robot such that it points towards the given target location. :param targets: A list of possible locations to look at :param resolver: An alternative specialized_designators that returns a performable designator for a list of possible target locations - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.targets: List[Pose] = targets - if self.soma: - self.init_ontology_concepts({"looking_at": self.soma.LookingAt}) - def ground(self) -> LookAtActionPerformable: """ Default specialized_designators that returns a performable designator with the first entry in the list of possible targets @@ -377,21 +335,16 @@ class DetectAction(ActionDesignatorDescription): Detects an object that fits the object description and returns an object designator describing the object. """ - def __init__(self, object_designator_description: ObjectDesignatorDescription, resolver=None, - ontology_concept_holders: Optional[List[Thing]] = None): + def __init__(self, object_designator_description: ObjectDesignatorDescription, resolver=None): """ Tries to detect an object in the field of view (FOV) of the robot. :param object_designator_description: Object designator describing the object :param resolver: An alternative specialized_designators - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.object_designator_description: ObjectDesignatorDescription = object_designator_description - if self.soma: - self.init_ontology_concepts({"looking_for": self.soma.LookingFor, - "checking_object_presence": self.soma.CheckingObjectPresence}) def ground(self) -> DetectActionPerformable: """ @@ -409,23 +362,18 @@ class OpenAction(ActionDesignatorDescription): Can currently not be used """ - def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], resolver=None, - ontology_concept_holders: Optional[List[Thing]] = None): + def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], resolver=None): """ Moves the arm of the robot to open a container. :param object_designator_description: Object designator describing the handle that should be used to open :param arms: A list of possible arms that should be used :param resolver: A alternative specialized_designators that returns a performable designator for the lists of possible parameter. - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.object_designator_description: ObjectPart = object_designator_description self.arms: List[Arms] = arms - if self.soma: - self.init_ontology_concepts({"opening": self.soma.Opening}) - def ground(self) -> OpenActionPerformable: """ Default specialized_designators that returns a performable designator with the resolved object description and the first entries @@ -443,22 +391,18 @@ class CloseAction(ActionDesignatorDescription): Can currently not be used """ - def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], - resolver=None, ontology_concept_holders: Optional[List[Thing]] = None): + def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], resolver=None): """ Attempts to close an open container :param object_designator_description: Object designator description of the handle that should be used :param arms: A list of possible arms to use :param resolver: An alternative specialized_designators that returns a performable designator for the list of possible parameter - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.object_designator_description: ObjectPart = object_designator_description self.arms: List[Arms] = arms - if self.soma: - self.init_ontology_concepts({"closing": self.soma.Closing}) def ground(self) -> CloseActionPerformable: """ @@ -476,7 +420,7 @@ class GraspingAction(ActionDesignatorDescription): """ def __init__(self, arms: List[Arms], object_description: Union[ObjectDesignatorDescription, ObjectPart], - resolver: Callable = None, ontology_concept_holders: Optional[List[Thing]] = None): + resolver: Callable = None): """ Will try to grasp the object described by the given description. Grasping is done by moving into a pre grasp position 10 cm before the object, opening the gripper, moving to the object and then closing the gripper. @@ -484,15 +428,11 @@ def __init__(self, arms: List[Arms], object_description: Union[ObjectDesignatorD :param arms: List of Arms that should be used for grasping :param object_description: Description of the object that should be grasped :param resolver: An alternative specialized_designators to get a specified designator from the designator description - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(resolver) self.arms: List[Arms] = arms self.object_description: ObjectDesignatorDescription = object_description - if self.soma: - self.init_ontology_concepts({"grasping": self.soma.Grasping}) - def ground(self) -> GraspingActionPerformable: """ Default specialized_designators that takes the first element from the list of arms and the first solution for the object @@ -1073,7 +1013,7 @@ class MoveAndPickUpPerformable(ActionAbstract): The grasp to use """ - @with_tree + # @with_tree def perform(self): NavigateActionPerformable(self.standing_position).perform() FaceAtPerformable(self.object_designator.pose).perform() diff --git a/src/pycram/designators/motion_designator.py b/src/pycram/designators/motion_designator.py index 589d5f9ad..cf52d6802 100644 --- a/src/pycram/designators/motion_designator.py +++ b/src/pycram/designators/motion_designator.py @@ -2,6 +2,8 @@ from dataclasses import dataclass from sqlalchemy.orm import Session + +from pycrap import PhysicalObject from .object_designator import ObjectDesignatorDescription, ObjectPart, RealObject from ..designator import ResolutionError from ..orm.base import ProcessMetaData @@ -14,7 +16,7 @@ Motion as ORMMotionDesignator) from ..datastructures.enums import ObjectType, Arms, GripperState, ExecutionType -from typing_extensions import Dict, Optional, get_type_hints +from typing_extensions import Dict, Optional, get_type_hints, Type, Any from ..datastructures.pose import Pose from ..tasktree import with_tree from ..designator import BaseMotion @@ -149,7 +151,7 @@ class DetectingMotion(BaseMotion): Tries to detect an object in the FOV of the robot """ - object_type: ObjectType + object_type: Type[PhysicalObject] """ Type of the object that should be detected """ @@ -169,7 +171,7 @@ def perform(self): world_object) def to_sql(self) -> ORMDetectingMotion: - return ORMDetectingMotion(self.object_type) + return ORMDetectingMotion(str(self.object_type)) def insert(self, session: Session, *args, **kwargs) -> ORMDetectingMotion: motion = super().insert(session) diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 303dc7939..c851d4bd8 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -1,19 +1,51 @@ from __future__ import annotations import dataclasses -from typing_extensions import List, Optional, Callable, TYPE_CHECKING + +import owlready2 import sqlalchemy.orm +from owlready2.triplelite import _SearchList +from typing_extensions import TYPE_CHECKING, Iterable + from ..datastructures.enums import ObjectType from ..datastructures.world import World -from ..world_concepts.world_object import Object as WorldObject -from ..designator import ObjectDesignatorDescription +from ..external_interfaces.robokudo import * from ..orm.base import ProcessMetaData from ..orm.object_designator import (BelieveObject as ORMBelieveObject, ObjectPart as ORMObjectPart) -from ..datastructures.pose import Pose -from ..external_interfaces.robokudo import * +from ..world_concepts.world_object import Object as WorldObject if TYPE_CHECKING: - import owlready2 + pass + + +class OntologyObjectDesignatorDescription: + """ + Description for Objects that can be found using ontological reasoning + """ + + search_result: List + """ + The result from the search in the ontology. + """ + + def __init__(self, search_result: _SearchList): + self.search_result = list(search_result) + + def __iter__(self) -> Iterable[ObjectDesignatorDescription.Object]: + """ + :return: The objects in the current world which match the search result in the 'is_a' relation. + """ + for obj in World.current_world.objects: + + # expand is_a of the object individual + is_a = obj.ontology_individual.is_a + [obj.ontology_individual] + + # get the matching concepts + intersection = ([x for x in is_a if x in self.search_result]) + + # if it matches + if len(intersection) > 0: + yield obj class BelieveObject(ObjectDesignatorDescription): @@ -28,7 +60,7 @@ class Object(ObjectDesignatorDescription.Object): """ def to_sql(self) -> ORMBelieveObject: - return ORMBelieveObject(name=self.name, obj_type=self.obj_type) + return ORMBelieveObject(name=self.name, obj_type=str(self.obj_type)) def insert(self, session: sqlalchemy.orm.session.Session) -> ORMBelieveObject: metadata = ProcessMetaData().insert(session) @@ -74,7 +106,6 @@ def __init__(self, names: List[str], :param part_of: Parent object of which the part should be described :param type: Type of the part :param resolver: An alternative specialized_designators to resolve the input parameter to an object designator - :param ontology_concept_holders: A list of ontology concepts that the object part is categorized as or associated with """ super().__init__(names, type, resolver) @@ -123,8 +154,7 @@ class Object(ObjectDesignatorDescription.Object): """ def __init__(self, names: List[str], types: List[str], - reference_frames: List[str], timestamps: List[float], resolver: Optional[Callable] = None, - ontology_concept_holders: Optional[List[owlready2.Thing]] = None): + reference_frames: List[str], timestamps: List[float], resolver: Optional[Callable] = None): """ Describing an object resolved through knowrob. @@ -133,9 +163,8 @@ def __init__(self, names: List[str], types: List[str], :param reference_frames: Frame of reference in which the object position should be :param timestamps: Timestamps for which positions should be returned :param resolver: An alternative specialized_designators that resolves the input parameter to an object designator. - :param ontology_concept_holders: A list of ontology concepts that the object is categorized as """ - super(LocatedObject, self).__init__(names, types, resolver, ontology_concept_holders) + super(LocatedObject, self).__init__(names, types, resolver) self.reference_frames: List[str] = reference_frames self.timestamps: List[float] = timestamps diff --git a/src/pycram/designators/specialized_designators/action/dual_arm_pickup_action.py b/src/pycram/designators/specialized_designators/action/dual_arm_pickup_action.py index 6347b0a72..372ccf0f5 100644 --- a/src/pycram/designators/specialized_designators/action/dual_arm_pickup_action.py +++ b/src/pycram/designators/specialized_designators/action/dual_arm_pickup_action.py @@ -22,8 +22,7 @@ class DualArmPickupAction(PickUpAction): def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], - grasps: List[Grasp], resolver=None, - ontology_concept_holders: Optional[List[Thing]] = None): + grasps: List[Grasp], resolver=None): """ Specialized version of the PickUpAction designator which uses heuristics to solve for a dual pickup problem. The designator will choose the arm which is closest to the object that is to be picked up. @@ -32,13 +31,11 @@ def __init__(self, :param grasps: List of possible grasps which should be used for the pickup :param resolver: Optional specialized_designators that returns a performable designator with elements from the lists of possible parameter - :param ontology_concept_holders: List of ontology concepts that the action is categorized as or associated with """ super().__init__(object_designator_description, arms=[Arms.LEFT, Arms.RIGHT], grasps=grasps, - resolver=resolver, - ontology_concept_holders=ontology_concept_holders) + resolver=resolver) self.object_designator_description: Union[ ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description diff --git a/src/pycram/ontology/ontology.py b/src/pycram/ontology/ontology.py deleted file mode 100644 index f768d9315..000000000 --- a/src/pycram/ontology/ontology.py +++ /dev/null @@ -1,847 +0,0 @@ -from __future__ import annotations - -import inspect -import itertools -import logging -import os.path -import sqlite3 - -from pathlib import Path -from typing import Callable, Dict, List, Optional, Type, Tuple, Union - -from owlready2 import (Namespace, Ontology, World as OntologyWorld, Thing, EntityClass, Imp, - Property, ObjectProperty, OwlReadyError, types, - onto_path, default_world, get_namespace, get_ontology, destroy_entity, - sync_reasoner_pellet, sync_reasoner_hermit, - OwlReadyOntologyParsingError) -from owlready2.class_construct import GeneralClassAxiom - -from ..datastructures.enums import ObjectType -from ..helper import Singleton -from ..designator import DesignatorDescription, ObjectDesignatorDescription - -from ..ontology.ontology_common import (OntologyConceptHolderStore, OntologyConceptHolder, - ONTOLOGY_SQL_BACKEND_FILE_EXTENSION, - ONTOLOGY_SQL_IN_MEMORY_BACKEND) -from ..ros.logging import loginfo, logerr, logwarn - -SOMA_HOME_ONTOLOGY_IRI = "http://www.ease-crc.org/ont/SOMA-HOME.owl" -SOMA_ONTOLOGY_IRI = "http://www.ease-crc.org/ont/SOMA.owl" -SOMA_ONTOLOGY_NAMESPACE = "SOMA" -DUL_ONTOLOGY_NAMESPACE = "DUL" - - -class OntologyManager(object, metaclass=Singleton): - """ - Singleton class as the adapter accessing data of an OWL ontology, largely based on owlready2. - """ - - def __init__(self, main_ontology_iri: Optional[str] = None, main_sql_backend_filename: Optional[str] = None, - ontology_search_path: Optional[str] = None, - use_global_default_world: bool = True): - """ - Create the singleton object of OntologyManager class - - :param main_ontology_iri: Ontology IRI (Internationalized Resource Identifier), either a URL to a remote OWL file or the full name path of a local one - :param main_sql_backend_filename: a full file path (no need to already exist) being used as SQL backend for the ontology world. If None, in-memory is used instead - :param ontology_search_path: directory path from which a possibly existing ontology is searched. This is appended to `owlready2.onto_path`, a global variable containing a list of directories for searching local copies of ontologies (similarly to python `sys.path` for modules/packages). If not specified, the path is "$HOME/ontologies" - :param use_global_default_world: whether or not using the owlready2-provided global default persistent world - """ - if not ontology_search_path: - ontology_search_path = f"{Path.home()}/ontologies" - Path(ontology_search_path).mkdir(parents=True, exist_ok=True) - onto_path.append(ontology_search_path) - - #: A dictionary of OWL ontologies, keyed by ontology name (same as its namespace name), eg. 'SOMA' - self.ontologies: Dict[str, Ontology] = {} - - #: The main ontology instance created by Ontology Manager at initialization as the result of loading from `main_ontology_iri` - self.main_ontology: Optional[Ontology] = None - - #: The SOMA ontology instance, referencing :attr:`main_ontology` in case of ontology loading from `SOMA.owl`. - # Ref: http://www.ease-crc.org/ont/SOMA.owl - self.soma: Optional[Ontology] = None - - #: The DUL ontology instance, referencing :attr:`main_ontology` in case of ontology loading from `DUL.owl`. - # Ref: http://www.ease-crc.org/ont/DUL.owl - self.dul: Optional[Ontology] = None - - #: The main ontology world, the placeholder of triples created in :attr:`main_ontology`. - # Ref: https://owlready2.readthedocs.io/en/latest/world.html - self.main_ontology_world: Optional[OntologyWorld] = None - - #: Ontology IRI (Internationalized Resource Identifier), either a URL to a remote OWL file or the full name path of a local one - # Ref: https://owlready2.readthedocs.io/en/latest/onto.html - self.main_ontology_iri: str = main_ontology_iri if main_ontology_iri else SOMA_HOME_ONTOLOGY_IRI - - #: Namespace of the main ontology - self.main_ontology_namespace: Optional[Namespace] = None - - #: SQL backend for :attr:`main_ontology_world`, being either "memory" or a full file path (no need to already exist) - self.main_ontology_sql_backend = main_sql_backend_filename if main_sql_backend_filename else ONTOLOGY_SQL_IN_MEMORY_BACKEND - - # Create the main ontology world holding triples - self.create_main_ontology_world(use_global_default_world=use_global_default_world) - - # Create the main ontology & its namespace, fetching :attr:`soma`, :attr:`dul` if loading from SOMA ontology - self.create_main_ontology() - - @staticmethod - def print_ontology_class(ontology_class: Type[Thing]): - """ - Print information (ancestors, super classes, subclasses, properties, etc.) of an ontology class - - :param ontology_class: An ontology class - """ - if ontology_class is None: - return - loginfo(f"{ontology_class} {type(ontology_class)}") - loginfo(f"Defined class: {ontology_class.get_defined_class()}") - loginfo(f"Super classes: {ontology_class.is_a}") - loginfo(f"Equivalent to: {EntityClass.get_equivalent_to(ontology_class)}") - loginfo(f"Indirectly equivalent to: {ontology_class.get_indirect_equivalent_to()}") - loginfo(f"Ancestors: {list(ontology_class.ancestors())}") - loginfo(f"Subclasses: {list(ontology_class.subclasses())}") - loginfo(f"Disjoint unions: {ontology_class.get_disjoint_unions()}") - loginfo(f"Properties: {list(ontology_class.get_class_properties())}") - loginfo(f"Indirect Properties: {list(ontology_class.INDIRECT_get_class_properties())}") - loginfo(f"Instances: {list(ontology_class.instances())}") - loginfo(f"Direct Instances: {list(ontology_class.direct_instances())}") - loginfo(f"Inverse Restrictions: {list(ontology_class.inverse_restrictions())}") - loginfo("-------------------") - - @staticmethod - def print_ontology_property(ontology_property: Property): - """ - Print information (subjects, objects, relations, etc.) of an ontology property - - :param ontology_property: An ontology property - """ - if ontology_property is None: - return - property_class = type(ontology_property) - loginfo(f"{ontology_property} {property_class}") - loginfo(f"Relations: {list(ontology_property.get_relations())}") - loginfo(f"Domain: {ontology_property.get_domain()}") - loginfo(f"Range: {ontology_property.get_range()}") - if hasattr(property_class, "_equivalent_to"): - loginfo(f"Equivalent classes: {EntityClass.get_equivalent_to(property_class)}") - if hasattr(property_class, "_indirect"): - loginfo(f"Indirectly equivalent classes: {EntityClass.get_indirect_equivalent_to(property_class)}") - loginfo(f"Property chain: {ontology_property.get_property_chain()}") - loginfo(f"Class property type: {ontology_property.get_class_property_type()}") - loginfo("-------------------") - - @staticmethod - def get_default_ontology_search_path() -> Optional[str]: - """ - Get the first ontology search path from owlready2.onto_path - - :return: the path to the ontology search path if existing, otherwise None - """ - if onto_path: - return onto_path[0] - else: - logerr("No ontology search path has been configured!") - return None - - def get_main_ontology_dir(self) -> Optional[str]: - """ - Get path to the directory of :attr:`main_ontology_iri` if it is a local absolute path, - otherwise path to the default ontology search directory - - :return: the path to the directory of the main ontology IRI - """ - return os.path.dirname(self.main_ontology_iri) if os.path.isabs( - self.main_ontology_iri) else self.get_default_ontology_search_path() - - def is_main_ontology_sql_backend_in_memory(self) -> bool: - """ - Whether the main ontology's SQL backend is in-memory - - :return: true if the main ontology's SQL backend is in-memory - """ - return self.main_ontology_sql_backend == ONTOLOGY_SQL_IN_MEMORY_BACKEND - - def create_main_ontology_world(self, use_global_default_world: bool = True) -> None: - """ - Create the main ontology world, either reusing the owlready2-provided global default ontology world or create a new one - A backend sqlite3 file of same name with `main_ontology` is also created at the same folder with :attr:`main_ontology_iri` - (if it is a local absolute path). The file is automatically registered as cache for the main ontology world. - - :param use_global_default_world: whether or not using the owlready2-provided global default persistent world - :param sql_backend_filename: a full file path (no need to already exist) being used as SQL backend for the ontology world. If None, memory is used instead - """ - self.main_ontology_world = self.create_ontology_world( - sql_backend_filename=self.main_ontology_sql_backend, - use_global_default_world=use_global_default_world) - - @staticmethod - def create_ontology_world(use_global_default_world: bool = False, - sql_backend_filename: Optional[str] = None) -> OntologyWorld: - """ - Either reuse the owlready2-provided global default ontology world or create a new one. - - :param use_global_default_world: whether or not using the owlready2-provided global default persistent world - :param sql_backend_filename: an absolute file path (no need to already exist) being used as SQL backend for the ontology world. If it is None or non-absolute path, in-memory is used instead - :return: owlready2-provided global default ontology world or a newly created ontology world - """ - world = default_world - sql_backend_path_absolute = (sql_backend_filename and os.path.isabs(sql_backend_filename)) - if sql_backend_filename and (sql_backend_filename != ONTOLOGY_SQL_IN_MEMORY_BACKEND): - if not sql_backend_path_absolute: - logerr(f"For ontology world accessing, either f{ONTOLOGY_SQL_IN_MEMORY_BACKEND}" - f"or an absolute path to its SQL file backend is expected: {sql_backend_filename}") - return default_world - elif not sql_backend_filename.endswith(ONTOLOGY_SQL_BACKEND_FILE_EXTENSION): - logerr( - f"Ontology world SQL backend file path, {sql_backend_filename}," - f"is expected to be of extension {ONTOLOGY_SQL_BACKEND_FILE_EXTENSION}!") - return default_world - - sql_backend_path_valid = sql_backend_path_absolute - sql_backend_name = sql_backend_filename if sql_backend_path_valid else ONTOLOGY_SQL_IN_MEMORY_BACKEND - try: - if use_global_default_world: - # Reuse default world - if sql_backend_path_valid: - world.set_backend(filename=sql_backend_filename, exclusive=False, enable_thread_parallelism=True) - else: - world.set_backend(exclusive=False, enable_thread_parallelism=True) - loginfo(f"Using global default ontology world with SQL backend: {sql_backend_name}") - else: - # Create a new world with parallelized file parsing enabled - if sql_backend_path_valid: - world = OntologyWorld(filename=sql_backend_filename, exclusive=False, enable_thread_parallelism=True) - else: - world = OntologyWorld(exclusive=False, enable_thread_parallelism=True) - loginfo(f"Created a new ontology world with SQL backend: {sql_backend_name}") - except sqlite3.Error as e: - logerr(f"Failed accessing the SQL backend of ontology world: {sql_backend_name}", - e.sqlite_errorcode, e.sqlite_errorname) - return world - - def create_main_ontology(self) -> bool: - """ - Load ontologies from :attr:`main_ontology_iri` to :attr:`main_ontology_world` - If `main_ontology_iri` is a remote URL, Owlready2 first searches for a local copy of the OWL file (from `onto_path`), - if not found, tries to download it from the Internet. - - :return: True if loading succeeds - """ - ontology_info = self.load_ontology(self.main_ontology_iri) - if ontology_info: - self.main_ontology, self.main_ontology_namespace = ontology_info - if self.main_ontology and self.main_ontology.loaded: - self.soma = self.ontologies.get(SOMA_ONTOLOGY_NAMESPACE) - self.dul = self.ontologies.get(DUL_ONTOLOGY_NAMESPACE) - return ontology_info is not None - - def load_ontology(self, ontology_iri: str) -> Optional[Tuple[Ontology, Namespace]]: - """ - Load an ontology from an IRI - - :param ontology_iri: An ontology IRI - :return: A tuple including an ontology instance & its namespace - """ - if not ontology_iri: - logerr("Ontology IRI is empty") - return None - - is_local_ontology_iri = not (ontology_iri.startswith("http:") or ontology_iri.startswith("https:")) - - # If `ontology_iri` is a local path - if is_local_ontology_iri and not Path(ontology_iri).exists(): - # -> Create an empty ontology file if not existing - ontology_path = ontology_iri if os.path.isabs(ontology_iri) else ( - os.path.join(self.get_main_ontology_dir(), ontology_iri)) - with open(ontology_path, 'w'): - pass - - # Load ontology from `ontology_iri` - ontology = None - try: - if self.main_ontology_world: - ontology = self.main_ontology_world.get_ontology(ontology_iri).load(reload_if_newer=True) - else: - ontology = get_ontology(ontology_iri).load(reload_if_newer=True) - except OwlReadyOntologyParsingError as error: - logwarn(error) - if is_local_ontology_iri: - logerr(f"Main ontology failed being loaded from {ontology_iri}") - else: - logwarn(f"Main ontology failed being downloaded from the remote {ontology_iri}") - return None - - # Browse loaded `ontology`, fetching sub-ontologies - ontology_namespace = get_namespace(ontology_iri) - if ontology and ontology.loaded: - loginfo( - f'Ontology [{ontology.base_iri}]\'s name: {ontology.name} has been loaded') - loginfo(f'- main namespace: {ontology_namespace.name}') - loginfo(f'- loaded ontologies:') - - def fetch_ontology(ontology__): - self.ontologies[ontology__.name] = ontology__ - loginfo(ontology__.base_iri) - - self.browse_ontologies(ontology, condition=None, func=lambda ontology__: fetch_ontology(ontology__)) - else: - logerr(f"Ontology [{ontology.base_iri}]\'s name: {ontology.name} failed being loaded") - return ontology, ontology_namespace - - def initialized(self) -> bool: - """ - Check if the main ontology has been loaded - - :return: True if loaded, otherwise False - """ - return hasattr(self, "main_ontology") and self.main_ontology and self.main_ontology.loaded - - @staticmethod - def browse_ontologies(ontology: Ontology, - condition: Optional[Callable] = None, func: Optional[Callable] = None, **kwargs) -> None: - """ - Browse the loaded ontologies (including the main and imported ones), doing operations based on a condition. - - :param ontology: An ontology instance as the result of ontology loading - :param condition: a Callable condition that if not None needs to be passed before doing operations, otherwise just always carry the operations - :param func: a Callable specifying the operations to perform on all the loaded ontologies if condition is None, otherwise only the first ontology which meets the condition - """ - if ontology is None: - logerr(f"Ontology {ontology=} is None!") - return - elif not ontology.loaded: - logerr(f"Ontology {ontology} was not loaded!") - return - - will_do_func = func is not None - # No condition: Do func for all ontologies - if condition is None: - if will_do_func: - func(ontology, **kwargs) - for sub_onto in ontology.get_imported_ontologies(): - func(sub_onto, **kwargs) - # Else: Only do func for the first ontology which meets the condition - elif condition(ontology, **kwargs): - if will_do_func: func(ontology, **kwargs) - else: - for sub_onto in ontology.get_imported_ontologies(): - if condition(sub_onto, **kwargs) and will_do_func: - func(sub_onto, **kwargs) - break - - def save(self, target_filename: Optional[str] = None, overwrite: bool = False) -> bool: - """ - Save :attr:`main_ontology` to a file on disk, also caching :attr:`main_ontology_world` to a sqlite3 file - - :param target_filename: full name path of a file which the ontologies are saved into. - :param overwrite: overwrite an existing file if it exists. If empty, they are saved to the same original OWL file from which the main ontology was loaded, or a file at the same folder with ontology search path specified at constructor if it was loaded from a remote IRI. - :return: True if the ontology was successfully saved, False otherwise - """ - - # Save ontologies to OWL - is_current_ontology_local = os.path.isfile(self.main_ontology_iri) - current_ontology_filename = self.main_ontology_iri if is_current_ontology_local \ - else f"{self.get_main_ontology_dir()}/{Path(self.main_ontology_iri).name}" - save_to_same_file = is_current_ontology_local and (target_filename == current_ontology_filename) - if save_to_same_file and not overwrite: - logerr( - f"Ontologies cannot be saved to the originally loaded [{target_filename}] if not by overwriting") - return False - else: - save_filename = target_filename if target_filename else current_ontology_filename - self.main_ontology.save(save_filename) - if save_to_same_file and overwrite: - logwarn(f"Main ontology {self.main_ontology.name} has been overwritten to {save_filename}") - else: - loginfo(f"Main ontology {self.main_ontology.name} has been saved to {save_filename}") - - # Commit the whole graph data of the current ontology world, saving it into SQLite3, to be reused the next time - # the ontologies are loaded - main_ontology_sql_filename = self.main_ontology_world.filename - self.main_ontology_world.save() - if os.path.isfile(main_ontology_sql_filename): - loginfo( - f"Main ontology world for {self.main_ontology.name} has been cached and saved to SQL: {main_ontology_sql_filename}") - #else: it could be using memory cache as SQL backend - return True - - def create_ontology_concept_class(self, class_name: str, - ontology_parent_concept_class: Optional[Thing] = None, - ontology: Optional[Ontology] = None) \ - -> Optional[Type[Thing]]: - """ - Create a new concept class in a given ontology - - :param class_name: A given name to the new class - :param ontology_parent_concept_class: An optional parent ontology class of the new class - :param ontology: an owlready2.Ontology in which the concept class is created - :return: The created ontology class - """ - ontology = ontology if ontology else self.main_ontology - ontology_concept_class = self.get_ontology_class_by_ontology(ontology, class_name) - if ontology_concept_class: - return ontology_concept_class - - if getattr(ontology, class_name, None): - logerr(f"Ontology concept class {ontology.name}.{class_name} already exists") - return None - - with ontology: - return types.new_class(class_name, (Thing, ontology_parent_concept_class,) - if inspect.isclass(ontology_parent_concept_class) else (Thing,)) - - def create_ontology_property_class(self, class_name: str, - ontology_parent_property_class: Optional[Type[Property]] = None, - ontology: Optional[Ontology] = None) \ - -> Optional[Type[Property]]: - """ - Create a new property class in a given ontology - - :param class_name: A given name to the new class - :param ontology_parent_property_class: An optional parent ontology property class of the new class - :param ontology: an owlready2.Ontology in which the concept class is created - :return: The created ontology class - """ - ontology = ontology if ontology else self.main_ontology - parent_class = ontology_parent_property_class if (ontology_parent_property_class and - issubclass(ontology_parent_property_class, - Property)) \ - else None - - if getattr(ontology, class_name, None): - logerr(f"Ontology property class {ontology.name}.{class_name} already exists") - return None - - with ontology: - return types.new_class(class_name, (parent_class,) if parent_class else (Property,)) - - def get_ontology_classes_by_condition(self, condition: Callable, first_match_only=False, **kwargs) \ - -> List[Type[Thing]]: - """ - Get an ontology class by a given condition - - :param condition: condition of searching - :param first_match_only: whether to only fetch the first class matching the given condition - :return: The ontology class satisfying the given condition if found else None - """ - out_classes = [] - for ontology_class in list(self.main_ontology.classes()): - if condition(ontology_class, **kwargs): - out_classes.append(ontology_class) - if first_match_only: - return out_classes - - for sub_onto in self.main_ontology.get_imported_ontologies(): - for sub_ontology_class in list(sub_onto.classes()): - if condition(sub_ontology_class, **kwargs): - out_classes.append(sub_ontology_class) - if first_match_only: - return out_classes - - if not out_classes: - loginfo(f"No class with {kwargs} is found in the ontology {self.main_ontology}") - return out_classes - - @staticmethod - def get_ontology_class_by_ontology(ontology: Ontology, class_name: str) -> Optional[Type[Thing]]: - """ - Get an ontology class if it exists in a given ontology - - :param ontology: an ontology instance - :param class_name: name of the searched-for ontology class - :return: The ontology class if it exists under the namespace of the given ontology, None otherwise - """ - return getattr(ontology, class_name, None) if ontology else None - - def get_ontology_class(self, class_name: str) -> Optional[Type[Thing]]: - """ - Get an ontology class by name - - :param class_name: name of the searched-for ontology class - :return: The ontology class of the given name if existing else None - """ - - def is_matching_class_name(ontology_class: Type[Thing], ontology_class_name: str): - return ontology_class.name == ontology_class_name - - found_classes = self.get_ontology_classes_by_condition(condition=is_matching_class_name, - ontology_class_name=class_name, - first_match_only=True) - return found_classes[0] if len(found_classes) > 0 else None - - def get_ontology_classes_by_namespace(self, ontology_namespace: str) -> List[Type[Thing]]: - """ - Get all ontologies classes by namespace - - :param ontology_namespace: namespace of the searched-for ontology classes - :return: A list of the ontology classes under the given namespace - """ - - def is_matching_ontology_namespace(ontology_class: Type[Thing], ontology_namespace_: str): - return ontology_class.namespace.name == ontology_namespace_ - - return self.get_ontology_classes_by_condition(condition=is_matching_ontology_namespace, - ontology_namespace_=ontology_namespace) - - def get_ontology_classes_by_subname(self, class_subname: str) -> List[Type[Thing]]: - """ - Get all ontologies classes by subname - - :param class_subname: a string as part of the full names of the searched-for ontology classes - :return: A list of the ontology classes of which the name contains the given subname - """ - - def is_matching_class_subname(ontology_class: Type[Thing], ontology_class_subname: str): - return ontology_class_subname.lower() in ontology_class.name.lower() - - return self.get_ontology_classes_by_condition(condition=is_matching_class_subname, - ontology_class_subname=class_subname) - - def get_ontology_descendant_classes(self, ancestor_class: Type[Thing], class_subname: str = "") \ - -> List[Type[Thing]]: - """ - Get ontology descendant classes of an ancestor class given descendant class subname - - :param class_subname: a string as part of the ancestor class full name - :return: A list of the ontology descendant classes - """ - return [ontology_class for ontology_class in self.main_ontology.classes() - if (class_subname.lower() in ontology_class.name.lower()) and - (ancestor_class in ontology_class.ancestors())] - - def get_ontology_general_class_axioms(self, ontology: Optional[Ontology] = None) -> List[GeneralClassAxiom]: - """ - Get general class axioms of an ontology - Ref: https://owlready2.readthedocs.io/en/latest/general_class_axioms.html - - :param ontology: an ontology instance - :return: A list of ontology axioms in the ontology - """ - ontology = ontology if ontology else self.main_ontology - return list(ontology.general_class_axioms()) - - def create_ontology_triple_classes(self, subject_class_name: str, object_class_name: str, - predicate_class_name: str, inverse_predicate_class_name: Optional[str] = None, - predicate_python_attribute_name: Optional[str] = None, - inverse_predicate_python_attribute_name: Optional[str] = None, - ontology_subject_parent_class: Optional[Type[Thing]] = None, - ontology_object_parent_class: Optional[Type[Union[Thing, object]]] = None, - ontology_property_parent_class: Type[Property] = ObjectProperty, - ontology_inverse_property_parent_class: Type[Property] = ObjectProperty, - ontology: Optional[Ontology] = None) -> bool: - """ - Dynamically create ontology triple classes under same namespace with the main ontology, - as known as {subject, predicate, object}, with the relations among them - - :param subject_class_name: name of the subject class - :param object_class_name: name of the object class - :param predicate_class_name: name of predicate class, also used as a Python attribute of the subject class to query object instances - :param predicate_python_attribute_name: python attribute name designated for the predicate instance - :param inverse_predicate_class_name: name of inverse predicate - :param inverse_predicate_python_attribute_name: python attribute name designated for the inverse predicate instance - :param ontology_subject_parent_class: a parent class of the subject class - :param ontology_object_parent_class: a parent class of the object class - :param ontology_property_parent_class: a parent ontology property class, default: owlready2.ObjectProperty - :param ontology_inverse_property_parent_class: a parent ontology inverse property class, default: owlready2.ObjectProperty - :param ontology: an owlready2.Ontology in which triples are created - :return: True if the ontology triple classes are created successfully - """ - - if not predicate_python_attribute_name: - predicate_python_attribute_name = predicate_class_name - if not inverse_predicate_python_attribute_name: - inverse_predicate_python_attribute_name = inverse_predicate_class_name - ontology = ontology if ontology else self.main_ontology - - # This context manager ensures all classes created here-in share the same namepsace with `self.main_ontology` - with ontology: - # Subject - ontology_subject_class = self.create_ontology_concept_class(subject_class_name, - ontology_subject_parent_class, - ontology=ontology) - if not ontology_subject_class: - logerr(f"{ontology.name}: Failed creating ontology subject class named {subject_class_name}") - return False - - # Object - if not ontology_object_parent_class or issubclass(ontology_object_parent_class, Thing): - ontology_object_class = self.create_ontology_concept_class(object_class_name, - ontology_object_parent_class, - ontology=ontology) \ - if (object_class_name != subject_class_name) else ontology_subject_class - else: - ontology_object_class = ontology_object_parent_class - - if not ontology_object_class: - logerr(f"{ontology.name}: Failed creating ontology object class named {object_class_name}") - return False - - # Predicate - ontology_predicate_class = self.create_ontology_property_class(predicate_class_name, - ontology_property_parent_class, - ontology=ontology) - if not ontology_predicate_class: - logerr(f"{ontology.name}: Failed creating ontology predicate class named {predicate_class_name}") - return False - ontology_predicate_class.domain = [ontology_subject_class] - ontology_predicate_class.range = [ontology_object_class] - ontology_predicate_class.python_name = predicate_python_attribute_name - - # Inverse Predicate - if inverse_predicate_class_name: - ontology_inverse_predicate_class = self.create_ontology_property_class(inverse_predicate_class_name, - ontology_inverse_property_parent_class, - ontology=ontology) - if not ontology_inverse_predicate_class: - logerr( - f"{ontology.name}: Failed creating ontology inverse-predicate class named {inverse_predicate_class_name}") - return False - ontology_inverse_predicate_class.inverse_property = ontology_predicate_class - ontology_inverse_predicate_class.domain = [ontology_object_class] - ontology_inverse_predicate_class.range = [ontology_subject_class] - ontology_inverse_predicate_class.python_name = inverse_predicate_python_attribute_name - return True - - def create_ontology_linked_designator(self, designator_class: Type[DesignatorDescription], - ontology_concept_name: str, - object_name: str, - ontology_parent_class: Optional[Type[Thing]] = None) \ - -> Optional[DesignatorDescription]: - """ - Create a designator linked to a given ontology concept - - :param designator_class: A given designator class - :param ontology_concept_name: Ontology concept name - :param object_name: Name of object in case of the designator to be created is an Object Designator - :param ontology_parent_class: Parent ontology class from which the class of designator inherits - :return: A designator associated with an ontology concept - """ - ontology_concept_class = self.create_ontology_concept_class(ontology_concept_name, ontology_parent_class) - return self.create_ontology_linked_designator_by_concept(designator_class=designator_class, - ontology_concept_class=ontology_concept_class, - object_name=object_name) - - def create_ontology_linked_designator_by_concept(self, designator_class: Type[DesignatorDescription], - ontology_concept_class: Type[Thing], - object_name: str) \ - -> Optional[DesignatorDescription]: - """ - Create a designator that belongs to a given ontology concept class - - :param designator_class: A given designator class - :param ontology_concept_class: An ontology concept class with which the output designator is associated - :param object_name: Name of object in case of the designator to be created is an Object Designator - :return: An object designator associated with the given ontology concept class if created successfully (not already exists), None otherwise - """ - ontology_concept_name = f'{object_name}_concept' - if len(OntologyConceptHolderStore().get_designators_of_ontology_concept(ontology_concept_name)) > 0: - logerr( - f"A designator named [{object_name}] is already created for ontology concept [{ontology_concept_name}]") - return None - - # Create a designator of `designator_class` - is_object_designator = issubclass(designator_class, ObjectDesignatorDescription) - if is_object_designator: - if not object_name: - logerr( - f"An empty object name was given as creating its Object designator for ontology concept class [{ontology_concept_class.name}]") - return None - designator = designator_class(names=[object_name]) - else: - designator = designator_class() - - # Link designator with an ontology concept of `ontology_concept_class` - ontology_concept_holder = OntologyConceptHolderStore().get_ontology_concept_holder_by_name( - ontology_concept_name) - if ontology_concept_holder is None: - ontology_concept_holder = OntologyConceptHolder(ontology_concept_class(name=ontology_concept_name, - namespace=self.main_ontology)) - self.set_ontology_concept_designator_connection(designator, ontology_concept_holder) - return designator - - @staticmethod - def set_ontology_concept_designator_connection(designator: DesignatorDescription, - ontology_concept_holder: OntologyConceptHolder) -> None: - """ - Set two-way connection between a designator and an ontology concept - - :param designator: Designator - :param ontology_concept_holder: Ontology concept holder - """ - if ontology_concept_holder not in designator.ontology_concept_holders: - designator.ontology_concept_holders.append(ontology_concept_holder) - - if not ontology_concept_holder.has_designator(designator): - ontology_concept_holder.designators.append(designator) - - @staticmethod - def set_ontology_relation(subject_designator: DesignatorDescription, - object_designator: DesignatorDescription, - predicate_name: str) -> bool: - """ - Set ontology relation between subject and object designators - - :param subject_designator: An object designator as the ontology subject - :param object_designator: An object designator as the ontology object - :param predicate_name: Name of the predicate - :return: True if the relation is set, False otherwise - """ - for subject_concept_holder in subject_designator.ontology_concept_holders: - subject_concept = subject_concept_holder.ontology_concept - if hasattr(subject_concept, predicate_name): - object_concepts_list = getattr(subject_concept, predicate_name) - object_concepts_names = [concept.name for concept in object_concepts_list] - for holder in object_designator.ontology_concept_holders: - if holder.ontology_concept.name not in object_concepts_names: - object_concepts_list.append(holder.ontology_concept) - return True - else: - logerr(f"Ontology concept [{subject_concept.name}] has no predicate named [{predicate_name}]") - return False - - @staticmethod - def get_designators_by_subject_predicate(subject: DesignatorDescription, - predicate_name: str) -> List[DesignatorDescription]: - """ - Get list of designators of an ontology-object concept given a subject designator and predicate - - :param subject: The ontology-subject designator - :param predicate_name: The ontology-predicate name of the relation - :return: List of object designators - """ - return list(itertools.chain( - *[OntologyConceptHolderStore().get_designators_of_ontology_concept(object_concept.name) - for subject_concept_holder in subject.ontology_concept_holders - for object_concept in getattr(subject_concept_holder.ontology_concept, predicate_name, [])])) - - def create_ontology_object_designator_from_type(self, object_type: ObjectType, - ontology_concept_class: Type[Thing]) \ - -> Optional[ObjectDesignatorDescription]: - """ - Create an object designator associated with an ontology concept class from a given object type - - :param object_type: An enumerated type of object - :param ontology_concept_class: An ontology concept class - :return: An object designator if created successfully (if not already existing), otherwise None - """ - object_type_name = object_type.name.lower() - object_designator = \ - self.create_ontology_linked_designator_by_concept(designator_class=ObjectDesignatorDescription, - ontology_concept_class=ontology_concept_class, - object_name=object_type_name) - object_designator.types = [object_type_name] - return object_designator - - @staticmethod - def destroy_ontology_class(ontology_class, destroy_instances: bool = True): - """ - Destroy all classes of an ontology - - :param ontology_class: The ontology class to be destroyed - :param destroy_instances: Whether to destroy instances of those ontology classes - """ - if destroy_instances: - for ontology_individual in ontology_class.instances(): - destroy_entity(ontology_individual) - OntologyConceptHolderStore().remove_ontology_concept(ontology_class.name) - destroy_entity(ontology_class) - - def create_rule_reflexivity(self, ontology_concept_class_name: str, - predicate_name: str, - ontology: Optional[Ontology] = None) -> Imp: - """ - Create the rule of reflexivity for a given ontology concept class. - Same effect is obtained by creating a dynamic ontology predicate class, subclassing owlready2.ReflexiveProperty. - Ref: https://en.wikipedia.org/wiki/Reflexive_relation - - :param ontology_concept_class_name: Name of the ontology concept class having the relation defined - :param predicate_name: Name of the ontology predicate signifying the reflexive relation - :param ontology: The ontology for which the rule is created - :return: Rule of transitivity - """ - ontology = ontology if ontology else self.main_ontology - with ontology: - rule = Imp() - rule.set_as_rule(f"""{ontology_concept_class_name}(?a) - -> {predicate_name}(?a, ?a)""") - return rule - - def create_rule_symmetry(self, ontology_concept_class_name: str, - predicate_name: str, - ontology: Optional[Ontology] = None) -> Imp: - """ - Create the rule of transitivity for a given ontology concept class. - Same effect is obtained by creating a dynamic ontology predicate class, subclassing owlready2.SymmetricProperty. - Ref: https://en.wikipedia.org/wiki/Symmetric_relation - - :param ontology_concept_class_name: Name of the ontology concept class having the relation defined - :param predicate_name: Name of the ontology predicate signifying the symmetric relation - :param ontology: The ontology for which the rule is created - :return: Rule of symmetry - """ - ontology = ontology if ontology else self.main_ontology - with ontology: - rule = Imp() - rule.set_as_rule(f"""{ontology_concept_class_name}(?a), {ontology_concept_class_name}(?b), - {predicate_name}(?a, ?b) - -> {predicate_name}(?b, ?a)""") - return rule - - def create_rule_transitivity(self, ontology_concept_class_name: str, - predicate_name: str, - ontology: Optional[Ontology] = None) -> Imp: - """ - Create the rule of transitivity for a given ontology concept class. - Same effect is obtained by creating a dynamic ontology predicate class, subclassing owlready2.TransitiveProperty. - Ref: - - https://en.wikipedia.org/wiki/Transitive_relation - - https://owlready2.readthedocs.io/en/latest/properties.html#obtaining-indirect-relations-considering-subproperty-transitivity-etc - - :param ontology_concept_class_name: Name of the ontology concept class having the relation defined - :param predicate_name: Name of the ontology predicate signifying the transitive relation - :param ontology: The ontology for which the rule is created - :return: Rule of transitivity - """ - ontology = ontology if ontology else self.main_ontology - with ontology: - rule = Imp() - rule.set_as_rule( - f"""{ontology_concept_class_name}(?a), {ontology_concept_class_name}(?b), {ontology_concept_class_name}(?c), - {predicate_name}(?a, ?b), - {predicate_name}(?b, ?c) - -> {predicate_name}(?a, ?c)""") - return rule - - def reason(self, world: OntologyWorld = None, use_pellet_reasoner: bool = True) -> bool: - """ - Run the reasoning on a given ontology world or :attr:`main_ontology_world` with Pellet or HermiT reasoner, - the two currently supported by owlready2 - - By default, the reasoning works on `owlready2.default_world` - - The reasoning also automatically save ontologies (to either in-memory cache or a temporary sqlite3 file) - Ref: - - https://owlready2.readthedocs.io/en/latest/reasoning.html - - https://owlready2.readthedocs.io/en/latest/rule.html - - https://www.researchgate.net/publication/200758993_Benchmarking_OWL_reasoners - - https://www.researchgate.net/publication/345959058_OWL2Bench_A_Benchmark_for_OWL_2_Reasoners - - :param world: An owlready2.World to reason about. If None, use :attr:`main_ontology_world` - :param use_pellet_reasoner: Use Pellet reasoner, otherwise HermiT - :return: True if the reasoning was successful, otherwise False - """ - reasoner_name = None - reasoning_world = world if world else self.main_ontology_world - try: - if use_pellet_reasoner: - reasoner_name = "Pellet" - sync_reasoner_pellet(x=reasoning_world, infer_property_values=True, - infer_data_property_values=True) - else: - reasoner_name = "HermiT" - sync_reasoner_hermit(x=reasoning_world, infer_property_values=True) - except OwlReadyError as error: - logerr(f"{reasoner_name} reasoning failed: {error}") - return False - loginfo(f"{reasoner_name} reasoning finishes!") - return True diff --git a/src/pycram/ontology/ontology_common.py b/src/pycram/ontology/ontology_common.py deleted file mode 100644 index 0df9ef570..000000000 --- a/src/pycram/ontology/ontology_common.py +++ /dev/null @@ -1,188 +0,0 @@ -from __future__ import annotations - -import itertools -from typing import Callable, Dict, List, Optional, Type, TYPE_CHECKING - -from ..helper import Singleton -from ..ros.logging import logerr - -if TYPE_CHECKING: - from ..designator import DesignatorDescription - -from owlready2 import issubclass, Thing - -ONTOLOGY_SQL_BACKEND_FILE_EXTENSION = ".sqlite3" -ONTOLOGY_SQL_IN_MEMORY_BACKEND = "memory" -ONTOLOGY_OWL_FILE_EXTENSION = ".owl" - - -class OntologyConceptHolderStore(object, metaclass=Singleton): - """ - Singleton class storing all instances of `OntologyConceptHolder` - """ - - def __init__(self): - """ - Initialize the OntologyConceptHolderStore - """ - # Dictionary of all ontology concept holders, keyed by concept names - self.__all_ontology_concept_holders: Dict[str, OntologyConceptHolder] = {} - - def add_ontology_concept_holder(self, ontology_concept_name: str, ontology_concept_holder: OntologyConceptHolder)\ - -> bool: - """ - Add an ontology concept to the store - - :param ontology_concept_name: Name of the ontology concept to be removed - :return: True if the ontology concept can be added into the concept store (if not already existing), otherwise False - """ - if ontology_concept_name in self.__all_ontology_concept_holders: - logerr(f"OntologyConceptHolder for `{ontology_concept_name}` was already created!") - return False - else: - self.__all_ontology_concept_holders.setdefault(ontology_concept_name, ontology_concept_holder) - return True - - def remove_ontology_concept(self, ontology_concept_name: str) -> bool: - """ - Remove an ontology concept from the store - - :param ontology_concept_name: Name of the ontology concept to be removed - :return: True if the ontology concept can be removed from the concept store (if existing), otherwise False - """ - if ontology_concept_name in self.__all_ontology_concept_holders: - del self.__all_ontology_concept_holders[ontology_concept_name] - return True - return False - - def get_ontology_concepts_by_class(self, ontology_concept_class: Type[Thing]) -> List[Thing]: - """ - Get a list of ontology concepts for a given class - - :param ontology_concept_class: An ontology concept class - :return: A list of ontology concepts of which the type is either the given class or its subclass - """ - return list(itertools.chain( - *[concept_holder.ontology_concept - for concept_holder in self.__all_ontology_concept_holders.values() - if issubclass(concept_holder.ontology_concept, ontology_concept_class)])) - - def get_ontology_concept_by_name(self, ontology_concept_name: str) -> Optional[Thing]: - """ - Get the ontology concept of a given name if exists, otherwise None - - :param ontology_concept_name: Name of an ontology concept - :return: The ontology concept of a given name if exists or None otherwise - """ - concept_holder = self.__all_ontology_concept_holders.get(ontology_concept_name) - return concept_holder.ontology_concept if concept_holder else None - - def get_ontology_concept_holders_by_class(self, ontology_concept_class: Type[Thing]) \ - -> List[OntologyConceptHolder]: - """ - Get a list of ontology concept holders for a given ontology concept class - - :param ontology_concept_class: An ontology concept class - :return: A list of ontology concept holders as instances of a given ontology concept class - """ - return [concept_holder for concept_holder in self.__all_ontology_concept_holders.values() - if isinstance(concept_holder.ontology_concept, ontology_concept_class)] - - def get_ontology_concept_holder_by_name(self, ontology_concept_name: str) -> Optional[OntologyConceptHolder]: - """ - Get the ontology concept holder for one of a given name if exists, otherwise None - - :param ontology_concept_name: Name of an ontology concept - :return: The ontology concept holder for one of a given name if exists, otherwise None - """ - return self.__all_ontology_concept_holders.get(ontology_concept_name) - - @staticmethod - def get_ontology_concepts_of_designator(designator: DesignatorDescription) -> List[Thing]: - """ - Get the corresponding ontology concepts for a given designator - - :param designator: A designator associated with an ontology concept - :return: A list of ontology concepts corresponding with a given designator - """ - return [concept_holder.ontology_concept for concept_holder in designator.ontology_concept_holders] - - def get_designators_of_ontology_concept(self, ontology_concept_name: str) -> List[DesignatorDescription]: - """ - Get the corresponding designators associated with a given ontology concept - - :param ontology_concept_name: An ontology concept name - :return: A list of designators corresponding to a given ontology concept - """ - return self.__all_ontology_concept_holders[ontology_concept_name].designators \ - if ontology_concept_name in self.__all_ontology_concept_holders else [] - - -class OntologyConceptHolder(object): - """ - Wrapper of an ontology concept that is either dynamically created or loaded from an ontology. - NOTE: Since an ontology concept class, after being saved into an ontology file, must be reusable in the next time - the ontology is loaded, there must be no other attributes of it that should be created aside from ones inherited from `owlready2.Thing`! - - :ivar ontology_concept: An ontology concept, either dynamically created, or loaded from an ontology - """ - - def __init__(self, ontology_concept: Thing): - """ - Initialize a holder of a given ontology concept instance - - :param ontology_concept: An ontology concept instance - """ - - #: An ontology concept, either dynamically created, or loaded from an ontology - self.ontology_concept: Thing = ontology_concept - #: List of designators associated with this ontology concept - self.designators: List[DesignatorDescription] = [] - # A callable used to resolve the designators to whatever of interest, like designators or their resolving results - self.resolve: Optional[Callable] = None - - #: The store for all OntologyConceptHolder instances - self.concept_holder_store: OntologyConceptHolderStore = OntologyConceptHolderStore() - self.concept_holder_store.add_ontology_concept_holder(ontology_concept.name, self) - - @property - def name(self) -> str: - """ - Get name of the ontology concept owned by this holder - - :return: Ontology concept name - """ - return self.ontology_concept.name if self.ontology_concept else "" - - def get_default_designator(self) -> Optional[DesignatorDescription]: - """ - Get the first element of designators if there is, else None - - :return: The first designator associated with the ontology concept held by this holder if exists or None - """ - return self.designators[0] if len(self.designators) > 0 else None - - def has_designator(self, designator) -> bool: - """ - Check whether this ontology concept holder has a given designator registered with its ontology concept - - :return: True if a given designator was registered by this ontology concept holder, either by itself or under another of the same name - """ - if designator in self.designators: - return True - if not hasattr(designator, "name"): - return False - for our_designator in self.designators: - if hasattr(our_designator, "name") and (getattr(our_designator, "name") == getattr(designator, "name")): - return True - return False - - def __eq__(self, other: OntologyConceptHolder) -> bool: - """ - Equality check based on name of the ontology concept - - :param other: Other ontology concept instance to check against - :return: True if the ontology concept of the other holder has the same name with the current one, otherwise False - """ - return ((self.ontology_concept == other.ontology_concept) or - (self.ontology_concept.name == other.ontology_concept.name)) diff --git a/src/pycram/orm/base.py b/src/pycram/orm/base.py index 06918ebe2..0c8a2142a 100644 --- a/src/pycram/orm/base.py +++ b/src/pycram/orm/base.py @@ -1,7 +1,6 @@ """Implementation of base classes for orm modelling.""" import datetime import getpass -import os from dataclasses import field from typing import Optional @@ -51,11 +50,13 @@ class Base(_Base): @declared_attr def process_metadata_id(self) -> Mapped[int]: return mapped_column(ForeignKey(f'{ProcessMetaData.__tablename__}.id'), default=None, init=False) + """Related MetaData Object to store information about the context of this experiment.""" @declared_attr def process_metadata(self): return relationship(ProcessMetaData.__tablename__) + """model relationship between foreign key in ProcessMetaData table and the ids of all inheriting tables""" @@ -189,25 +190,25 @@ def __mapper_args__(self): class Position(Base): """ORM Class for 3D positions.""" - x: Mapped[float] - y: Mapped[float] - z: Mapped[float] + x: Mapped[float] = mapped_column(init=True) + y: Mapped[float] = mapped_column(init=True) + z: Mapped[float] = mapped_column(init=True) class Quaternion(Base): """ORM Class for Quaternions.""" - x: Mapped[float] - y: Mapped[float] - z: Mapped[float] - w: Mapped[float] + x: Mapped[float] = mapped_column(init=True) + y: Mapped[float] = mapped_column(init=True) + z: Mapped[float] = mapped_column(init=True) + w: Mapped[float] = mapped_column(init=True) class Pose(PositionMixin, QuaternionMixin, Base): """ORM Class for Poses.""" - time: Mapped[datetime.datetime] - frame: Mapped[str] + time: Mapped[datetime.datetime] = mapped_column(init=True) + frame: Mapped[str] = mapped_column(init=True) class Color(Base): @@ -222,8 +223,8 @@ class Color(Base): class RobotState(PoseMixin, Base): """ORM Representation of a robots state.""" - torso_height: Mapped[float] + torso_height: Mapped[float] = mapped_column(init=True) """The torso height of the robot.""" - type: Mapped[ObjectType] + type: Mapped[str] = mapped_column(init=True) """The type of the robot.""" diff --git a/src/pycram/orm/motion_designator.py b/src/pycram/orm/motion_designator.py index 9b7ff062a..3589c2ece 100644 --- a/src/pycram/orm/motion_designator.py +++ b/src/pycram/orm/motion_designator.py @@ -92,7 +92,7 @@ class DetectingMotion(Motion): """ id: Mapped[int] = mapped_column(ForeignKey(f'{Motion.__tablename__}.id'), primary_key=True, init=False) - object_type: Mapped[ObjectType] + object_type: Mapped[str] class WorldStateDetectingMotion(Motion): diff --git a/src/pycram/orm/object_designator.py b/src/pycram/orm/object_designator.py index 6f66a785b..790672ab3 100644 --- a/src/pycram/orm/object_designator.py +++ b/src/pycram/orm/object_designator.py @@ -1,10 +1,10 @@ from dataclasses import field from typing import Optional -from pycram.orm.base import Base, MapperArgsMixin, PoseMixin, Pose -from sqlalchemy.orm import Mapped, mapped_column, declared_attr, relationship, MappedAsDataclass from sqlalchemy import ForeignKey -from pycram.datastructures.enums import ObjectType +from sqlalchemy.orm import Mapped, mapped_column, declared_attr, relationship, MappedAsDataclass + +from pycram.orm.base import Base, MapperArgsMixin, PoseMixin class ObjectMixin(MappedAsDataclass): @@ -29,8 +29,8 @@ class Object(PoseMixin, Base): """ORM class of pycram.designators.object_designator.ObjectDesignator""" dtype: Mapped[str] = mapped_column(init=False) - obj_type: Mapped[Optional[ObjectType]] - name: Mapped[str] + obj_type: Mapped[Optional[str]] = mapped_column(init=True) + name: Mapped[str] = mapped_column(init=True) __mapper_args__ = { "polymorphic_identity": "Object", @@ -51,5 +51,4 @@ class ObjectPart(Object): class BelieveObject(MapperArgsMixin, Object): - id: Mapped[int] = mapped_column(ForeignKey(f'{Object.__tablename__}.id'), primary_key=True, init=False) diff --git a/src/pycram/testing.py b/src/pycram/testing.py new file mode 100644 index 000000000..c5e66d609 --- /dev/null +++ b/src/pycram/testing.py @@ -0,0 +1,76 @@ +import time +import unittest + +from .tasktree import task_tree +from .datastructures.world import UseProspectionWorld +from .worlds.bullet_world import BulletWorld +from .world_concepts.world_object import Object +from .datastructures.pose import Pose +from .robot_description import RobotDescription, RobotDescriptionManager +from .process_module import ProcessModule +from .datastructures.enums import ObjectType, WorldMode +from .object_descriptors.urdf import ObjectDescription +from .ros_utils.viz_marker_publisher import VizMarkerPublisher +from pycrap import ontology, Milk, Robot, Kitchen, Cereal +import owlready2 + +class EmptyBulletWorldTestCase(unittest.TestCase): + """ + Base class for unit tests that require and ordinary setup and teardown of the empty bullet-world. + """ + + world: BulletWorld + viz_marker_publisher: VizMarkerPublisher + extension: str = ObjectDescription.get_file_extension() + render_mode = WorldMode.DIRECT + + @classmethod + def setUpClass(cls): + cls.world = BulletWorld(mode=cls.render_mode) + ProcessModule.execution_delay = False + cls.viz_marker_publisher = VizMarkerPublisher() + + def setUp(self): + self.world.reset_world(remove_saved_states=True) + with UseProspectionWorld(): + pass + + + def tearDown(self): + task_tree.reset_tree() + time.sleep(0.05) + self.world.reset_world(remove_saved_states=True) + with UseProspectionWorld(): + pass + + @classmethod + def tearDownClass(cls): + cls.viz_marker_publisher._stop_publishing() + cls.world.exit() + + +class BulletWorldTestCase(EmptyBulletWorldTestCase): + """ + Class for unit tests that require a bullet-world with a PR2, kitchen, milk and cereal. + """ + + world: BulletWorld + viz_marker_publisher: VizMarkerPublisher + extension: str = ObjectDescription.get_file_extension() + + @classmethod + def setUpClass(cls): + super().setUpClass() + rdm = RobotDescriptionManager() + rdm.load_description("pr2") + cls.milk = Object("milk", Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) + cls.robot = Object(RobotDescription.current_robot_description.name, Robot, + RobotDescription.current_robot_description.name + cls.extension) + cls.kitchen = Object("kitchen", Kitchen, "kitchen" + cls.extension) + cls.cereal = Object("cereal", Cereal, "breakfast_cereal.stl", + pose=Pose([1.3, 0.7, 0.95])) + + + +class BulletWorldGUITestCase(BulletWorldTestCase): + render_mode = WorldMode.GUI diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 3c93d4272..6be44c825 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -5,6 +5,7 @@ from pathlib import Path import numpy as np +import owlready2 from deprecated import deprecated from geometry_msgs.msg import Point, Quaternion from typing_extensions import Type, Optional, Dict, Tuple, List, Union @@ -30,11 +31,13 @@ MJCF = None from ..robot_description import RobotDescriptionManager, RobotDescription from ..world_concepts.constraints import Attachment +from ..datastructures.mixins import HasConcept +from pycrap import PhysicalObject, ontology, Base, Agent Link = ObjectDescription.Link -class Object(WorldEntity): +class Object(WorldEntity, HasConcept): """ Represents a spawned Object in the World. """ @@ -49,7 +52,9 @@ class Object(WorldEntity): A dictionary that maps the file extension to the corresponding ObjectDescription type. """ - def __init__(self, name: str, obj_type: ObjectType, path: Optional[str] = None, + ontology_concept: Type[PhysicalObject] = PhysicalObject + + def __init__(self, name: str, concept: Type[PhysicalObject], path: Optional[str] = None, description: Optional[ObjectDescription] = None, pose: Optional[Pose] = None, world: Optional[World] = None, @@ -63,7 +68,7 @@ def __init__(self, name: str, obj_type: ObjectType, path: Optional[str] = None, for URDFs :func:`~Object.set_color` can be used. :param name: The name of the object - :param obj_type: The type of the object as an ObjectType enum. + :param concept: The type of the object as ontological concept from PyCRAP :param path: The path to the source file, if only a filename is provided then the resources directories will be searched, it could be None in some cases when for example it is a generic object. :param description: The ObjectDescription of the object, this contains the joints and links of the object. @@ -79,9 +84,14 @@ def __init__(self, name: str, obj_type: ObjectType, path: Optional[str] = None, pose = Pose() if pose is None else pose + # set ontology related information + self.ontology_concept = concept + if not self.world.is_prospection_world: + self.ontology_individual = self.ontology_concept(namespace=self.world.ontology.ontology) + self.name: str = name self.path: Optional[str] = path - self.obj_type: ObjectType = obj_type + self.color: Color = color self._resolve_description(path, description) self.cache_manager = self.world.cache_manager @@ -97,7 +107,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: Optional[str] = None, self.description.update_description_from_file(self.path) - if self.obj_type == ObjectType.ROBOT and not self.world.is_prospection_world: + # if the object is an agent in the belief state + if Agent in self.ontology_concept.is_a and not self.world.is_prospection_world: self._update_world_robot_and_description() self.id = self._spawn_object_and_get_id() @@ -114,6 +125,8 @@ def __init__(self, name: str, obj_type: ObjectType, path: Optional[str] = None, self.world.add_object(self) + + def _resolve_description(self, path: Optional[str] = None, description: Optional[ObjectDescription] = None) -> None: """ Find the correct description type of the object and initialize it and set the description of this object to it. @@ -283,6 +296,10 @@ def transform(self): """ return self.get_pose().to_transform(self.tf_frame) + @property + def obj_type(self) -> Type[PhysicalObject]: + return self.ontology_concept + def _spawn_object_and_get_id(self) -> int: """ Loads an object to the given World with the given position and orientation. The rgba_color will only be @@ -573,8 +590,10 @@ def remove(self) -> None: is currently attached to. After this call world remove object to remove this Object from the simulation/world. """ + # owlready2.destroy_entity(self.ontology_individual) self.world.remove_object(self) + def reset(self, remove_saved_states=False) -> None: """ Reset the Object to the state it was first spawned in. @@ -1398,3 +1417,4 @@ def __eq__(self, other): def __hash__(self): return hash((self.id, self.name, self.world)) + diff --git a/src/pycram/worlds/bullet_world.py b/src/pycram/worlds/bullet_world.py index 4b1ddff97..c3a3b5f86 100755 --- a/src/pycram/worlds/bullet_world.py +++ b/src/pycram/worlds/bullet_world.py @@ -10,6 +10,7 @@ from geometry_msgs.msg import Point from typing_extensions import List, Optional, Dict, Any +from pycrap import Floor from ..datastructures.dataclasses import Color, AxisAlignedBoundingBox, MultiBody, VisualShape, BoxVisualShape, \ ClosestPoint, LateralFriction, ContactPoint, ContactPointsList, ClosestPointsList from ..datastructures.enums import ObjectType, WorldMode, JointType @@ -58,7 +59,7 @@ def __init__(self, mode: WorldMode = WorldMode.DIRECT, is_prospection_world: boo self.set_gravity([0, 0, -9.8]) if not is_prospection_world: - _ = Object("floor", ObjectType.ENVIRONMENT, "plane.urdf", + _ = Object("floor", Floor, "plane.urdf", world=self) def _init_world(self, mode: WorldMode): diff --git a/src/pycrap/__init__.py b/src/pycrap/__init__.py new file mode 100644 index 000000000..3ced88bbf --- /dev/null +++ b/src/pycrap/__init__.py @@ -0,0 +1,5 @@ +from .base import * +from .objects import * +from .agent import * +from .location import * +from .ontology import * \ No newline at end of file diff --git a/src/pycrap/agent.py b/src/pycrap/agent.py new file mode 100644 index 000000000..26b666689 --- /dev/null +++ b/src/pycrap/agent.py @@ -0,0 +1,14 @@ +from .base import PhysicalObject + +class Agent(PhysicalObject): + """ + An agent is an entity that acts. + """ + +class Robot(Agent): + """ + A robot is a machine that can carry out a complex series of actions automatically. + """ + +class Human(Agent): + ... \ No newline at end of file diff --git a/src/pycrap/base.py b/src/pycrap/base.py new file mode 100644 index 000000000..0c263f15d --- /dev/null +++ b/src/pycrap/base.py @@ -0,0 +1,21 @@ +import tempfile + +import owlready2 +from owlready2 import Thing + +default_pycrap_ontology_file = tempfile.NamedTemporaryFile() +default_pycrap_ontology = owlready2.get_ontology("file://" + default_pycrap_ontology_file.name).load() + +class Base(Thing): + comment = __doc__ + namespace = default_pycrap_ontology + + @classmethod + def set_comment_to_docstring(cls): + cls.comment = cls.__doc__ + + +class PhysicalObject(Base): + """ + Any Object that has a proper space region. The prototypical physical object has also an associated mass, but the nature of its mass can greatly vary based on the epistemological status of the object (scientifically measured, subjectively possible, imaginary). + """ \ No newline at end of file diff --git a/src/pycrap/location.py b/src/pycrap/location.py new file mode 100644 index 000000000..0810bd94d --- /dev/null +++ b/src/pycrap/location.py @@ -0,0 +1,36 @@ +from .base import Base + +class Location(Base): + """ + A physical region. + """ + +class Room(Location): + """ + A region in a building. + """ + +class Bedroom(Room): + """ + A room where people sleep in. + """ + +class Kitchen(Room): + """ + A room where food is prepared. + """ + +class LivingRoom(Room): + """ + A room where people relax in. + """ + +class Bathroom(Room): + """ + A room where people wash in. + """ + +class Apartment(Location): + """ + A building with multiple rooms. + """ \ No newline at end of file diff --git a/src/pycrap/objects.py b/src/pycrap/objects.py new file mode 100644 index 000000000..614e525f5 --- /dev/null +++ b/src/pycrap/objects.py @@ -0,0 +1,88 @@ +from .base import PhysicalObject + + +class Container(PhysicalObject): + """ + Any object that can contain other objects. + """ + + +class Cup(Container): + """ + A cup is a small open container used for drinking. + """ + + +class Mug(Container): + equivalent_to = [Cup] + + +class MetalMug(Mug): + """ + A mug made of metal. + """ + + +class Food(PhysicalObject): + """ + Any substance that can be consumed by living organisms. + """ + + +class Pringles(Food): + """ + A brand of potato snack chips. + """ + + +class Milk(Food): + """ + A white liquid produced by the mammary glands of mammals. + """ + + +class Cutlery(PhysicalObject): + """ + Any implement, tool, or container used for serving or eating food. + """ + + +class Fork(Cutlery): + """ + A fork is a tool consisting of a handle with several narrow tines on one end. + """ + + +class Spoon(Cutlery): + """ + A spoon is a utensil consisting of a small shallow bowl oval or round in shape, with a handle. + """ + + +class Knife(Cutlery): + """ + A knife is a tool with a cutting edge or blade attached to a handle. + """ + + +class Plate(PhysicalObject): + """ + A plate is a broad, concave, but mainly flat vessel on which food can be served. + """ + + +class Bowl(Container, Plate): + """ + A bowl is a round, open-top container used in many cultures to serve food. + """ + + +class Cereal(Food): + """ + A traditional breakfast dish made from processed cereal grains. + """ + +class Floor(PhysicalObject): + """ + The lower surface of a room. + """ \ No newline at end of file diff --git a/src/pycrap/ontology.py b/src/pycrap/ontology.py new file mode 100644 index 000000000..2f3fcc05a --- /dev/null +++ b/src/pycrap/ontology.py @@ -0,0 +1,68 @@ +import tempfile + +import owlready2 +from .base import default_pycrap_ontology + + +class Ontology: + """ + Wrapper class for user-friendly access of the owlready2 ontology class. + + This class spawns a temporary file that is used to store the ontology. + This has to be done whenever several PyCRAP ontologies are needed that store different individuals. + """ + + ontology: owlready2.Ontology + """ + The owlready2 ontology that is used for reasoning. + """ + + file: tempfile.NamedTemporaryFile + """ + The file that the ontology is stored in. + """ + + + def __init__(self): + self.file = tempfile.NamedTemporaryFile(delete=True) + self.ontology = owlready2.get_ontology("file://" + self.path).load() + self.ontology.name = "PyCRAP" + + @property + def path(self) -> str: + return self.file.name + + def individuals(self): + return self.ontology.individuals() + + def destroy_individuals(self): + """ + Destroys all individuals in the ontology. + """ + for individual in self.individuals(): + owlready2.destroy_entity(individual) + + @staticmethod + def classes(): + """ + :return: All classes of the PyCRAP ontology. + """ + return default_pycrap_ontology.classes() + + def search(self, *args, **kwargs): + """ + :return: The search results of the ontology. + """ + return self.ontology.search(*args, **kwargs) + + def __enter__(self): + return self.ontology.__enter__() + + def __exit__(self, exc_type, exc_val, exc_tb): + return self.ontology.__exit__(exc_type, exc_val, exc_tb) + + def reason(self): + """ + Reason over the ontology. This may take a long time. + """ + owlready2.sync_reasoner([self.ontology, default_pycrap_ontology]) \ No newline at end of file diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py deleted file mode 100644 index 4bb0a27b9..000000000 --- a/test/bullet_world_testcase.py +++ /dev/null @@ -1,93 +0,0 @@ -import time -import unittest - -import pycram.tasktree -from pycram.datastructures.world import UseProspectionWorld -from pycram.worlds.bullet_world import BulletWorld -from pycram.world_concepts.world_object import Object -from pycram.datastructures.pose import Pose -from pycram.robot_description import RobotDescription, RobotDescriptionManager -from pycram.process_module import ProcessModule -from pycram.datastructures.enums import ObjectType, WorldMode -from pycram.object_descriptors.urdf import ObjectDescription -from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher -from pycram.ontology.ontology import OntologyManager, SOMA_ONTOLOGY_IRI - - -class BulletWorldTestCase(unittest.TestCase): - - world: BulletWorld - viz_marker_publisher: VizMarkerPublisher - extension: str = ObjectDescription.get_file_extension() - - @classmethod - def setUpClass(cls): - rdm = RobotDescriptionManager() - rdm.load_description("pr2") - cls.world = BulletWorld(mode=WorldMode.DIRECT) - cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, - RobotDescription.current_robot_description.name + cls.extension) - cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) - cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - pose=Pose([1.3, 0.7, 0.95])) - ProcessModule.execution_delay = False - cls.viz_marker_publisher = VizMarkerPublisher() - OntologyManager(SOMA_ONTOLOGY_IRI) - - def setUp(self): - self.world.reset_world(remove_saved_states=True) - with UseProspectionWorld(): - pass - - # DO NOT WRITE TESTS HERE!!! - # Test related to the BulletWorld should be written in test_bullet_world.py - # Tests in here would not be properly executed in the CI - - def tearDown(self): - pycram.tasktree.task_tree.reset_tree() - time.sleep(0.05) - self.world.reset_world(remove_saved_states=True) - with UseProspectionWorld(): - pass - - @classmethod - def tearDownClass(cls): - cls.viz_marker_publisher._stop_publishing() - cls.world.exit() - - -class BulletWorldGUITestCase(unittest.TestCase): - - world: BulletWorld - extension: str = ObjectDescription.get_file_extension() - - @classmethod - def setUpClass(cls): - rdm = RobotDescriptionManager() - rdm.load_description("pr2") - cls.world = BulletWorld(mode=WorldMode.GUI) - cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, - RobotDescription.current_robot_description.name + cls.extension) - cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) - cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - pose=Pose([1.3, 0.7, 0.95])) - ProcessModule.execution_delay = False - cls.viz_marker_publisher = VizMarkerPublisher() - - def setUp(self): - self.world.reset_world() - - # DO NOT WRITE TESTS HERE!!! - # Test related to the BulletWorld should be written in test_bullet_world.py - # Tests in here would not be properly executed in the CI - - def tearDown(self): - pass - - @classmethod - def tearDownClass(cls): - cls.viz_marker_publisher._stop_publishing() - cls.world.exit() - diff --git a/test/test_attachment.py b/test/test_attachment.py index bf8487942..68458bd97 100644 --- a/test/test_attachment.py +++ b/test/test_attachment.py @@ -1,10 +1,11 @@ import time -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.datastructures.enums import ObjectType from pycram.datastructures.pose import Pose from pycram.datastructures.world import UseProspectionWorld from pycram.world_concepts.world_object import Object +from pycrap import Milk, Cereal class TestAttachment(BulletWorldTestCase): @@ -60,8 +61,8 @@ def test_detachment_behavior(self): self.assertEqual(new_milk_pos.x, milk_pos.x) def test_prospection_object_attachments_not_changed_with_real_object(self): - milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cereal_2 = Object("cereal_2", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", + milk_2 = Object("milk_2", Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) + cereal_2 = Object("cereal_2", Cereal, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) time.sleep(0.05) milk_2.attach(cereal_2) diff --git a/test/test_bullet_world.py b/test/test_bullet_world.py index 565e98342..e9774138c 100644 --- a/test/test_bullet_world.py +++ b/test/test_bullet_world.py @@ -4,14 +4,15 @@ import rospkg -from bullet_world_testcase import BulletWorldTestCase, BulletWorldGUITestCase -from pycram.datastructures.enums import ObjectType, WorldMode +from pycram.testing import BulletWorldTestCase, BulletWorldGUITestCase +from pycram.datastructures.dataclasses import Color +from pycram.datastructures.enums import WorldMode from pycram.datastructures.pose import Pose -from pycram.robot_description import RobotDescription +from pycram.datastructures.world import UseProspectionWorld from pycram.object_descriptors.urdf import ObjectDescription -from pycram.datastructures.dataclasses import Color +from pycram.robot_description import RobotDescription from pycram.world_concepts.world_object import Object -from pycram.datastructures.world import UseProspectionWorld, World +from pycrap import Milk, Robot fix_missing_inertial = ObjectDescription.fix_missing_inertial @@ -53,14 +54,14 @@ def test_remove_object(self): self.assertTrue(milk_id in [obj.id for obj in self.world.objects]) self.world.remove_object(self.milk) self.assertTrue(milk_id not in [obj.id for obj in self.world.objects]) - BulletWorldTest.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + BulletWorldTest.milk = Object("milk", Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) def test_remove_robot(self): robot_id = self.robot.id self.assertTrue(robot_id in [obj.id for obj in self.world.objects]) self.world.remove_object(self.robot) self.assertTrue(robot_id not in [obj.id for obj in self.world.objects]) - BulletWorldTest.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, + BulletWorldTest.robot = Object(RobotDescription.current_robot_description.name, Robot, RobotDescription.current_robot_description.name + self.extension) def test_get_joint_position(self): @@ -143,7 +144,7 @@ def test_add_resource_path(self): self.assertTrue("test" in self.world.get_data_directories()) def test_no_prospection_object_found_for_given_object(self): - milk_2 = Object("milk_2", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + milk_2 = Object("milk_2", Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) try: prospection_milk_2 = self.world.get_prospection_object_for_object(milk_2) self.world.remove_object(milk_2) @@ -154,7 +155,7 @@ def test_no_prospection_object_found_for_given_object(self): def test_real_object_position_does_not_change_with_prospection_object(self): milk_2_pos = [1.3, 1, 0.9] - milk_2 = Object("milk_3", ObjectType.MILK, "milk.stl", pose=Pose(milk_2_pos)) + milk_2 = Object("milk_3", Milk, "milk.stl", pose=Pose(milk_2_pos)) time.sleep(0.05) milk_2_pos = milk_2.get_position() @@ -170,7 +171,7 @@ def test_real_object_position_does_not_change_with_prospection_object(self): def test_prospection_object_position_does_not_change_with_real_object(self): milk_2_pos = [1.3, 1, 0.9] - milk_2 = Object("milk_4", ObjectType.MILK, "milk.stl", pose=Pose(milk_2_pos)) + milk_2 = Object("milk_4", Milk, "milk.stl", pose=Pose(milk_2_pos)) time.sleep(0.05) milk_2_pos = milk_2.get_position() diff --git a/test/test_bullet_world_reasoning.py b/test/test_bullet_world_reasoning.py index 8d8c1061b..2c66942b8 100644 --- a/test/test_bullet_world_reasoning.py +++ b/test/test_bullet_world_reasoning.py @@ -1,7 +1,7 @@ import time import pycram.world_reasoning as btr -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.datastructures.pose import Pose from pycram.robot_description import RobotDescription diff --git a/test/test_cache_manager.py b/test/test_cache_manager.py index 9f208d90f..9e5909c43 100644 --- a/test/test_cache_manager.py +++ b/test/test_cache_manager.py @@ -1,7 +1,7 @@ import os from pathlib import Path -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.object_descriptors.urdf import ObjectDescription as URDFObject from pycram.config import world_conf as conf diff --git a/test/test_costmaps.py b/test/test_costmaps.py index d589a9a90..32452844a 100644 --- a/test/test_costmaps.py +++ b/test/test_costmaps.py @@ -6,7 +6,7 @@ from random_events.product_algebra import Event, SimpleEvent from random_events.interval import * -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.costmaps import OccupancyCostmap, AlgebraicSemanticCostmap, VisibilityCostmap from pycram.probabilistic_costmap import ProbabilisticCostmap from pycram.units import meter, centimeter diff --git a/test/test_database_resolver.py b/test/test_database_resolver.py deleted file mode 100644 index 7392dbac2..000000000 --- a/test/test_database_resolver.py +++ /dev/null @@ -1,191 +0,0 @@ -import os -import unittest -import sqlalchemy -import sqlalchemy.orm -import pycram.failures -from pycram.world_concepts.world_object import Object -from pycram.datastructures.world import World -from pycram.designators import action_designator -from pycram.designators.action_designator import MoveTorsoActionPerformable, PickUpActionPerformable, \ - NavigateActionPerformable, PlaceActionPerformable -from pycram.orm.base import Base -from pycram.designators.object_designator import ObjectDesignatorDescription -from pycram.process_module import ProcessModule -from pycram.process_module import simulated_robot -from pycram.datastructures.pose import Pose -from pycram.robot_description import RobotDescription -from pycram.tasktree import with_tree -from pycram.datastructures.enums import ObjectType, WorldMode -from pycram.designators.specialized_designators.location.database_location import DatabaseCostmapLocation -from pycram.worlds.bullet_world import BulletWorld - -pycrorm_uri = os.getenv('PYCRORM_URI') -if pycrorm_uri: - pycrorm_uri = "mysql+pymysql://" + pycrorm_uri - - -@unittest.skip -class DatabaseResolverTestCase(unittest.TestCase,): - world: World - milk: Object - robot: Object - engine: sqlalchemy.engine.Engine - session: sqlalchemy.orm.Session - - @classmethod - def setUpClass(cls) -> None: - global pycrorm_uri - cls.world = BulletWorld(WorldMode.DIRECT) - cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cls.robot = Object(RobotDescription.current_robot_description.name, - ObjectType.ROBOT, RobotDescription.current_robot_description.name + ".urdf") - ProcessModule.execution_delay = False - cls.engine = sqlalchemy.create_engine(pycrorm_uri) - - def setUp(self) -> None: - self.world.reset_world() - pycram.orm.base.Base.metadata.create_all(self.engine) - self.session = sqlalchemy.orm.Session(bind=self.engine) - self.session.commit() - - def tearDown(self) -> None: - self.world.reset_world() - pycram.tasktree.reset_tree() - pycram.orm.base.ProcessMetaData.reset() - self.session.rollback() - pycram.orm.base.Base.metadata.drop_all(self.engine) - self.session.close() - - @classmethod - def tearDownClass(cls) -> None: - cls.world.exit() - - @with_tree - def plan(self): - object_description = ObjectDesignatorDescription(names=["milk"]) - description = action_designator.PlaceAction(object_description, [Pose([1.3, 1, 0.9], [0, 0, 0, 1])], ["left"]) - with simulated_robot: - NavigateActionPerformable(Pose([0.6, 0.4, 0], [0, 0, 0, 1])).perform() - MoveTorsoActionPerformable(0.3).perform() - PickUpActionPerformable(object_description.resolve(), "left", "front").perform() - description.resolve().perform() - - def test_costmap_no_obstacles(self): - """Check if grasping a milk in the air works.""" - self.plan() - pycram.orm.base.ProcessMetaData().description = "costmap_no_obstacles_test" - pycram.tasktree.task_tree.root.insert(self.session) - - cml = DatabaseCostmapLocation(self.milk, self.session, reachable_for=self.robot) - sample = next(iter(cml)) - - with simulated_robot: - MoveTorsoActionPerformable(sample.torso_height).perform() - PickUpActionPerformable(ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve(), arm=sample.reachable_arm, - grasp=sample.grasp).perform() - - def test_costmap_with_obstacles(self): - kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") - self.plan() - pycram.orm.base.ProcessMetaData().description = "costmap_with_obstacles_test" - pycram.tasktree.task_tree.root.insert(self.session) - self.world.reset_current_world() - - cml = DatabaseCostmapLocation(self.milk, self.session, reachable_for=self.robot) - sample = next(iter(cml)) - - with simulated_robot: - NavigateActionPerformable(sample.pose).perform() - MoveTorsoActionPerformable(sample.torso_height).perform() - try: - PickUpActionPerformable( - ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve(), - arm=sample.reachable_arm, grasp=sample.grasp).perform() - except pycram.plan_failures.PlanFailure as p: - kitchen.remove() - raise p - kitchen.remove() - - def test_object_at_different_location(self): - kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") - self.plan() - - pycram.orm.base.ProcessMetaData().description = "object_at_different_location_test" - pycram.tasktree.task_tree.root.insert(self.session) - self.world.reset_current_world() - - new_milk = Object("new_milk", ObjectType.MILK, "milk.stl", pose=Pose([-1.45, 2.5, 0.95])) - cml = DatabaseCostmapLocation(new_milk, self.session, reachable_for=self.robot) - - sample = next(iter(cml)) - with simulated_robot: - NavigateActionPerformable(sample.pose).perform() - MoveTorsoActionPerformable(sample.torso_height).perform() - try: - PickUpActionPerformable( - ObjectDesignatorDescription(names=["new_milk"], types=[ObjectType.MILK]).resolve(), - arm=sample.reachable_arm, grasp=sample.grasp).perform() - except pycram.plan_failures.PlanFailure as p: - new_milk.remove() - kitchen.remove() - raise p - PlaceActionPerformable(ObjectDesignatorDescription(names=["new_milk"], types=[ObjectType.MILK]).resolve(), - arm=sample.reachable_arm, target_location=Pose([-1.45, 2.5, 0.95])).perform() - new_milk.remove() - kitchen.remove() - - @unittest.skip - def test_multiple_objects(self): - kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen.urdf") - new_milk = Object("new_milk", ObjectType.MILK, "milk.stl", pose=Pose([-1.45, 2.5, 0.9])) - - object_description = ObjectDesignatorDescription(names=["milk"]) - object_description_new_milk = ObjectDesignatorDescription(names=["new_milk"]) - description = action_designator.PlaceAction(object_description_new_milk, [Pose([-1.45, 2.5, 0.9], [0, 0, 0, 1])], ["left"]) - with simulated_robot: - NavigateActionPerformable(Pose([1, 0.4, 0], [0, 0, 0, 1])).perform() - MoveTorsoActionPerformable(0.3).perform() - PickUpActionPerformable(object_description.resolve(), "left", "front").perform() - PlaceActionPerformable(object_description.resolve(), "left", Pose([1.3, 1, 0.9], [0, 0, 0, 1])).perform() - NavigateActionPerformable(Pose([-1.75, 1.9, 0], [0, 0, 0, 1])).perform() - PickUpActionPerformable(object_description_new_milk.resolve(), "left", "front").perform() - description.resolve().perform() - - pycram.orm.base.ProcessMetaData().description = "multiple_objects_test" - pycram.tasktree.task_tree.root.insert(self.session) - self.world.reset_current_world() - - cml = DatabaseCostmapLocation(self.milk, self.session, reachable_for=self.robot) - cml_new_milk = DatabaseCostmapLocation(new_milk, self.session, reachable_for=self.robot) - - dcls = [cml, cml_new_milk] - for dcl in dcls: - sample = next(iter(dcl)) - with (simulated_robot): - NavigateActionPerformable(sample.pose).perform() - MoveTorsoActionPerformable(sample.torso_height).perform() - try: - if dcl.target.name == "milk": - PickUpActionPerformable( - ObjectDesignatorDescription(names=["milk"], types=[ObjectType.MILK]).resolve(), - arm=sample.reachable_arm, grasp=sample.grasp).perform() - else: - PickUpActionPerformable( - ObjectDesignatorDescription(names=["new_milk"], types=[ObjectType.MILK]).resolve(), - arm=sample.reachable_arm, grasp=sample.grasp).perform() - except pycram.plan_failures.PlanFailure as p: - new_milk.remove() - kitchen.remove() - raise p - PlaceActionPerformable(ObjectDesignatorDescription(names=[dcl.target.name], - types=[ObjectType.MILK]).resolve(), - arm=sample.reachable_arm, target_location=Pose([dcl.target.pose.position.x, - dcl.target.pose.position.y, - dcl.target.pose.position.z]) - ).perform() - new_milk.remove() - kitchen.remove() - - -if __name__ == '__main__': - unittest.main() diff --git a/test/test_description.py b/test/test_description.py index e324384ac..e377867e4 100644 --- a/test/test_description.py +++ b/test/test_description.py @@ -1,7 +1,7 @@ import os.path import pathlib -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase class DescriptionTest(BulletWorldTestCase): diff --git a/src/pycram/ontology/__init__.py b/test/test_designator/__init__.py similarity index 100% rename from src/pycram/ontology/__init__.py rename to test/test_designator/__init__.py diff --git a/test/test_action_designator.py b/test/test_designator/test_action_designator.py similarity index 98% rename from test/test_action_designator.py rename to test/test_designator/test_action_designator.py index 1c7fb5a3d..6b2b8c556 100644 --- a/test/test_action_designator.py +++ b/test/test_designator/test_action_designator.py @@ -8,9 +8,11 @@ from pycram.process_module import simulated_robot from pycram.datastructures.pose import Pose from pycram.datastructures.enums import ObjectType, Arms, GripperState, Grasp -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase import numpy as np +from pycrap import Milk + class TestActionDesignatorGrounding(BulletWorldTestCase): """Testcase for the grounding methods of action designators.""" @@ -100,7 +102,7 @@ def test_detect(self): with simulated_robot: detected_object = description.resolve().perform() self.assertEqual(detected_object.name, "milk") - self.assertEqual(detected_object.obj_type, ObjectType.MILK) + self.assertEqual(detected_object.obj_type, Milk) self.assertEqual(detected_object.world_object, self.milk) # Skipped since open and close work only in the apartment at the moment diff --git a/test/test_location_designator.py b/test/test_designator/test_location_designator.py similarity index 98% rename from test/test_location_designator.py rename to test/test_designator/test_location_designator.py index 673277c22..9a30f3563 100644 --- a/test/test_location_designator.py +++ b/test/test_designator/test_location_designator.py @@ -1,7 +1,7 @@ from pycram.designators.location_designator import * from pycram.robot_description import RobotDescription from pycram.datastructures.pose import Pose -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase class TestActionDesignatorGrounding(BulletWorldTestCase): diff --git a/test/test_move_and_pick_up.py b/test/test_designator/test_move_and_pick_up.py similarity index 88% rename from test/test_move_and_pick_up.py rename to test/test_designator/test_move_and_pick_up.py index 1a2008278..b837a9993 100644 --- a/test/test_move_and_pick_up.py +++ b/test/test_designator/test_move_and_pick_up.py @@ -5,7 +5,7 @@ from random_events.variable import Continuous, Symbolic from sortedcontainers import SortedSet -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.datastructures.enums import ObjectType, Arms, Grasp from pycram.designator import ObjectDesignatorDescription from pycram.designators.action_designator import MoveTorsoActionPerformable @@ -14,6 +14,7 @@ Grasp as PMGrasp) from pycram.failures import PlanFailure from pycram.process_module import simulated_robot +from pycrap import Milk class MoveAndPickUpTestCase(BulletWorldTestCase): @@ -25,7 +26,7 @@ def setUpClass(cls): random.seed(69) def test_variables(self): - object_designator = ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve() + object_designator = ObjectDesignatorDescription(types=[Milk]).resolve() move_and_pick = MoveAndPickUp(object_designator, arms=[Arms.LEFT, Arms.RIGHT], grasps=[Grasp.FRONT, Grasp.LEFT, Grasp.RIGHT, Grasp.TOP]) result = SortedSet([Symbolic("arm", PMArms), Symbolic("grasp", PMGrasp), @@ -34,7 +35,7 @@ def test_variables(self): self.assertEqual(all_variables, result) def test_grounding(self): - object_designator = ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve() + object_designator = ObjectDesignatorDescription(types=[Milk]).resolve() move_and_pick = MoveAndPickUp(object_designator, arms=[Arms.LEFT, Arms.RIGHT], grasps=[Grasp.FRONT, Grasp.LEFT, Grasp.RIGHT, Grasp.TOP]) @@ -44,7 +45,7 @@ def test_grounding(self): self.assertIsNotNone(model) def test_move_and_pick_up_with_mode(self): - object_designator = ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve() + object_designator = ObjectDesignatorDescription(types=[Milk]).resolve() move_and_pick = MoveAndPickUp(object_designator, arms=[Arms.LEFT, Arms.RIGHT], grasps=[Grasp.FRONT, Grasp.LEFT, Grasp.RIGHT, Grasp.TOP]) with simulated_robot: diff --git a/test/test_move_and_place.py b/test/test_designator/test_move_and_place.py similarity index 90% rename from test/test_move_and_place.py rename to test/test_designator/test_move_and_place.py index 74cec82c4..2b4232096 100644 --- a/test/test_move_and_place.py +++ b/test/test_designator/test_move_and_place.py @@ -3,7 +3,7 @@ import numpy as np -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.datastructures.enums import ObjectType, Arms, Grasp from pycram.datastructures.pose import Pose from pycram.designator import ObjectDesignatorDescription @@ -12,6 +12,7 @@ from pycram.designators.specialized_designators.probabilistic.probabilistic_action import (MoveAndPlace) from pycram.failures import PlanFailure from pycram.process_module import simulated_robot +from pycrap import Milk class MoveAndPlaceTestCase(BulletWorldTestCase): @@ -23,7 +24,7 @@ def setUpClass(cls): random.seed(69) def test_with_mode(self): - object_designator = ObjectDesignatorDescription(types=[ObjectType.MILK]).resolve() + object_designator = ObjectDesignatorDescription(types=[Milk]).resolve() target_location = Pose([1.3, 1, 0.9], [0, 0, 0, 1]) designator = MoveAndPlace(object_designator, target_location) diff --git a/test/test_designator/test_object_designator.py b/test/test_designator/test_object_designator.py new file mode 100644 index 000000000..b7ae91a25 --- /dev/null +++ b/test/test_designator/test_object_designator.py @@ -0,0 +1,38 @@ +import unittest +from pycram.testing import BulletWorldTestCase, EmptyBulletWorldTestCase +from pycram.designators.object_designator import * +from pycram.datastructures.enums import ObjectType +from pycrap import Milk, Food, Cereal + + +class TestObjectDesignator(BulletWorldTestCase): + + def test_object_grounding(self): + description = ObjectDesignatorDescription(["milk"], [Milk]) + obj = description.ground() + + self.assertEqual(obj.name, "milk") + self.assertEqual(obj.obj_type, Milk) + + def test_frozen_copy(self): + description = ObjectDesignatorDescription(["milk"], [Milk]) + obj = description.ground() + + frozen_copy = obj.frozen_copy() + self.assertEqual(obj.pose, frozen_copy.pose) + + +class OntologyObjectDesignatorDescriptionTestCase(BulletWorldTestCase): + + def test_type_query_for_food(self): + self.world.ontology.reason() + odd = OntologyObjectDesignatorDescription(self.world.ontology.search(type=Food)) + self.assertEqual(len(odd.search_result), 2) + result_in_world = list(odd.__iter__()) + self.assertEqual(len(result_in_world), 2) + + + + +if __name__ == '__main__': + unittest.main() diff --git a/test/test_failure_handling.py b/test/test_failure_handling.py index 190a48922..b0112dc63 100644 --- a/test/test_failure_handling.py +++ b/test/test_failure_handling.py @@ -11,6 +11,7 @@ from pycram.process_module import ProcessModule, simulated_robot from pycram.robot_description import RobotDescription from pycram.object_descriptors.urdf import ObjectDescription +from pycrap import Robot extension = ObjectDescription.get_file_extension() @@ -33,7 +34,7 @@ class FailureHandlingTest(unittest.TestCase): @classmethod def setUpClass(cls): cls.world = BulletWorld(WorldMode.DIRECT) - cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, + cls.robot = Object(RobotDescription.current_robot_description.name, Robot, RobotDescription.current_robot_description.name + extension) ProcessModule.execution_delay = True diff --git a/test/test_goal_validator.py b/test/test_goal_validator.py index 9b79cc114..ad390137b 100644 --- a/test/test_goal_validator.py +++ b/test/test_goal_validator.py @@ -2,7 +2,7 @@ from tf.transformations import quaternion_from_euler from typing_extensions import Optional, List -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.datastructures.enums import JointType from pycram.datastructures.pose import Pose from pycram.robot_description import RobotDescription diff --git a/test/test_language.py b/test/test_language.py index 362db9c0e..7771637f4 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -10,7 +10,7 @@ from pycram.datastructures.pose import Pose from pycram.language import Sequential, Language, Parallel, TryAll, TryInOrder, Monitor, Code from pycram.process_module import simulated_robot -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.robot_description import RobotDescription diff --git a/test/test_links.py b/test/test_links.py index 695e670c1..703b4f48a 100644 --- a/test/test_links.py +++ b/test/test_links.py @@ -1,4 +1,4 @@ -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.datastructures.dataclasses import Color diff --git a/test/test_local_transformer.py b/test/test_local_transformer.py index 283c8f31c..8a9bb880c 100644 --- a/test/test_local_transformer.py +++ b/test/test_local_transformer.py @@ -3,7 +3,7 @@ from pycram.local_transformer import LocalTransformer from pycram.datastructures.pose import Pose, Transform -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase class TestLocalTransformer(BulletWorldTestCase): diff --git a/test/test_logging.py b/test/test_logging.py index e516b4017..c03459266 100644 --- a/test/test_logging.py +++ b/test/test_logging.py @@ -1,4 +1,4 @@ -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.ros.logging import set_logger_level, logwarn, logerr from pycram.datastructures.enums import LoggerLevel diff --git a/test/test_object.py b/test/test_object.py index bede0300b..39b4830fa 100644 --- a/test/test_object.py +++ b/test/test_object.py @@ -1,6 +1,6 @@ import numpy as np -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.datastructures.enums import JointType, ObjectType from pycram.datastructures.pose import Pose @@ -12,12 +12,14 @@ from geometry_msgs.msg import Point, Quaternion import pathlib +from pycrap import ontology, Milk, Food + class TestObject(BulletWorldTestCase): def test_wrong_object_description_path(self): with self.assertRaises(UnsupportedFileExtension): - milk = Object("milk_not_found", ObjectType.MILK, "wrong_path.sk") + milk = Object("milk_not_found", Milk, "wrong_path.sk") def test_malformed_object_description(self): file_path = pathlib.Path(__file__).parent.resolve() @@ -25,7 +27,7 @@ def test_malformed_object_description(self): with open(malformed_file, "w") as file: file.write("malformed") with self.assertRaises(Exception): - Object("milk2", ObjectType.MILK, malformed_file) + Object("milk2", Milk, malformed_file) def test_move_base_to_origin_pose(self): self.milk.set_position(Point(1, 2, 3), base=False) @@ -158,7 +160,7 @@ def test_set_color(self): self.assertEqual(color, Color(0, 1, 0, 1)) def test_object_equal(self): - milk2 = Object("milk2", ObjectType.MILK, "milk.stl") + milk2 = Object("milk2", Milk, "milk.stl") self.assertNotEqual(self.milk, milk2) self.assertEqual(self.milk, self.milk) self.assertNotEqual(self.milk, self.cereal) @@ -169,6 +171,20 @@ class GenericObjectTestCase(BulletWorldTestCase): def test_init_generic_object(self): gen_obj_desc = GenericObjectDescription("robokudo_object", [0,0,0], [0.1, 0.1, 0.1]) - obj = Object("robokudo_object", ObjectType.MILK, None, gen_obj_desc) + obj = Object("robokudo_object", Milk, None, gen_obj_desc) pose = obj.get_pose() self.assertTrue(isinstance(pose, Pose)) + + +class OntologyIntegrationTestCase(BulletWorldTestCase): + + def test_querying(self): + # get all milks from the ontology + r = list(filter(lambda x: x in Milk.instances(), self.world.ontology.individuals())) + self.assertEqual(len(r), 1) + + milk2 = Object("milk2", Milk, "milk.stl") + + r = list(filter(lambda x: x in Milk.instances(), self.world.ontology.individuals())) + + self.assertEqual(len(r), 2) diff --git a/test/test_object_designator.py b/test/test_object_designator.py deleted file mode 100644 index 51e8c856a..000000000 --- a/test/test_object_designator.py +++ /dev/null @@ -1,25 +0,0 @@ -import unittest -from bullet_world_testcase import BulletWorldTestCase -from pycram.designators.object_designator import * -from pycram.datastructures.enums import ObjectType - - -class TestObjectDesignator(BulletWorldTestCase): - - def test_object_grounding(self): - description = ObjectDesignatorDescription(["milk"], [ObjectType.MILK]) - obj = description.ground() - - self.assertEqual(obj.name, "milk") - self.assertEqual(obj.obj_type, ObjectType.MILK) - - def test_frozen_copy(self): - description = ObjectDesignatorDescription(["milk"], [ObjectType.MILK]) - obj = description.ground() - - frozen_copy = obj.frozen_copy() - self.assertEqual(obj.pose, frozen_copy.pose) - - -if __name__ == '__main__': - unittest.main() diff --git a/test/test_ontology.py b/test/test_ontology.py deleted file mode 100644 index f4754d4b2..000000000 --- a/test/test_ontology.py +++ /dev/null @@ -1,312 +0,0 @@ -from __future__ import annotations - -import os.path -import unittest -import logging -from pathlib import Path -from typing import Type, Optional - -from pycram.designator import ObjectDesignatorDescription - -import rospy - -# Owlready2 -try: - from owlready2 import * -except ImportError: - owlready2 = None - rospy.logwarn("Could not import owlready2, Ontology unit-tests could not run!") - -# Java runtime, required by Owlready2 reasoning -java_runtime_installed = owlready2 is not None -if owlready2: - try: - subprocess.run(["java", "--version"], check=True) - except (FileNotFoundError, subprocess.CalledProcessError): - java_runtime_installed = False - rospy.logwarn("Java runtime is not installed, Ontology reasoning unit-test could not run!") - -from pycram.ontology.ontology import OntologyManager, SOMA_HOME_ONTOLOGY_IRI, SOMA_ONTOLOGY_IRI -from pycram.ontology.ontology_common import (OntologyConceptHolderStore, OntologyConceptHolder, - ONTOLOGY_SQL_BACKEND_FILE_EXTENSION, ONTOLOGY_OWL_FILE_EXTENSION, - ONTOLOGY_SQL_IN_MEMORY_BACKEND) - -DEFAULT_LOCAL_ONTOLOGY_IRI = "default.owl" -class TestOntologyManager(unittest.TestCase): - ontology_manager: OntologyManager - main_ontology: Optional[owlready2.Ontology] - soma: Optional[owlready2.Ontology] - dul: Optional[owlready2.Ontology] - - @classmethod - def setUpClass(cls): - # Try loading from remote `SOMA_ONTOLOGY_IRI`, which will fail given no internet access - cls.ontology_manager = OntologyManager(main_ontology_iri=SOMA_ONTOLOGY_IRI, - main_sql_backend_filename=os.path.join(Path.home(), - f"{Path(SOMA_ONTOLOGY_IRI).stem}{ONTOLOGY_SQL_BACKEND_FILE_EXTENSION}")) - if cls.ontology_manager.initialized(): - cls.soma = cls.ontology_manager.soma - cls.dul = cls.ontology_manager.dul - else: - # Else, load from `DEFAULT_LOCAL_ONTOLOGY_IRI` - cls.soma = None - cls.dul = None - cls.ontology_manager.main_ontology_iri = DEFAULT_LOCAL_ONTOLOGY_IRI - cls.ontology_manager.main_ontology_sql_backend = ONTOLOGY_SQL_IN_MEMORY_BACKEND - cls.ontology_manager.create_main_ontology_world() - cls.ontology_manager.create_main_ontology() - cls.main_ontology = cls.ontology_manager.main_ontology - - @classmethod - def tearDownClass(cls): - save_dir = cls.ontology_manager.get_main_ontology_dir() - owl_filepath = f"{save_dir}/{Path(cls.ontology_manager.main_ontology_iri).stem}{ONTOLOGY_OWL_FILE_EXTENSION}" - os.remove(owl_filepath) - cls.remove_sql_file(cls.ontology_manager.main_ontology_sql_backend) - - @classmethod - def remove_sql_file(cls, sql_filepath: str): - if os.path.exists(sql_filepath): - os.remove(sql_filepath) - sql_journal_filepath = f"{sql_filepath}-journal" - if os.path.exists(sql_journal_filepath): - os.remove(sql_journal_filepath) - - def test_ontology_manager(self): - self.assertIs(self.ontology_manager, OntologyManager()) - if owlready2: - self.assertTrue(self.ontology_manager.initialized()) - - @unittest.skipUnless(owlready2, 'Owlready2 is required') - def test_ontology_world(self): - # Main ontology world as the global default world - main_world = self.ontology_manager.main_ontology_world - self.assertIsNotNone(main_world) - self.assertTrue(main_world is owlready2.default_world) - - # Extra world with memory backend - extra_memory_world = self.ontology_manager.create_ontology_world(use_global_default_world=False) - self.assertIsNotNone(extra_memory_world) - self.assertTrue(extra_memory_world != owlready2.default_world) - - # Extra world with SQL backend from a non-existing SQL file - extra_world_sql_filename = f"{self.ontology_manager.get_main_ontology_dir()}/extra_world{ONTOLOGY_SQL_BACKEND_FILE_EXTENSION}" - extra_sql_world = self.ontology_manager.create_ontology_world(use_global_default_world=False, - sql_backend_filename=extra_world_sql_filename) - self.assertIsNotNone(extra_sql_world) - # Save it at [extra_world_sql_filename] - extra_sql_world.save() - self.assertTrue(os.path.isfile(extra_world_sql_filename)) - - # Extra world with SQL backend from an existing SQL file - extra_sql_world_2 = self.ontology_manager.create_ontology_world(use_global_default_world=False, - sql_backend_filename=extra_world_sql_filename) - self.assertIsNotNone(extra_sql_world_2) - - # Remove SQL file finally - self.remove_sql_file(extra_world_sql_filename) - - @unittest.skipUnless(owlready2, 'Owlready2 is required') - def test_ontology_concept_holder(self): - dynamic_ontology_concept_class = self.ontology_manager.create_ontology_concept_class('DynamicOntologyConcept') - dynamic_ontology_concept_holder = OntologyConceptHolder( - dynamic_ontology_concept_class(name='dynamic_ontology_concept1', - namespace=self.main_ontology)) - self.assertTrue(owlready2.isinstance_python(dynamic_ontology_concept_holder.ontology_concept, owlready2.Thing)) - - @unittest.skipUnless(owlready2, 'Owlready2 is required') - def test_loaded_ontologies(self): - self.assertIsNotNone(self.main_ontology) - self.assertTrue(self.main_ontology.loaded) - if self.ontology_manager.main_ontology_iri is SOMA_ONTOLOGY_IRI or \ - self.ontology_manager.main_ontology_iri is SOMA_HOME_ONTOLOGY_IRI: - self.assertIsNotNone(self.soma) - self.assertTrue(self.soma.loaded) - self.assertIsNotNone(self.dul) - self.assertTrue(self.dul.loaded) - - @unittest.skipUnless(owlready2, 'Owlready2 is required') - def test_ontology_concept_class_dynamic_creation(self): - dynamic_ontology_concept_class = self.ontology_manager.create_ontology_concept_class('DynamicOntologyConcept') - self.assertIsNotNone(dynamic_ontology_concept_class) - self.assertEqual(dynamic_ontology_concept_class.namespace, self.main_ontology) - self.assertIs(dynamic_ontology_concept_class, self.main_ontology.DynamicOntologyConcept) - self.assertIs(issubclass(dynamic_ontology_concept_class, owlready2.Thing), True) - dynamic_ontology_concept = dynamic_ontology_concept_class(name='dynamic_ontology_concept2', - namespace=self.main_ontology) - self.assertTrue(owlready2.isinstance_python(dynamic_ontology_concept, owlready2.Thing)) - - @unittest.skipUnless(owlready2, 'Owlready2 is required') - def test_ontology_triple_classes_dynamic_creation(self): - # Test dynamic triple classes creation without inheritance from existing parent ontology classes - self.assertTrue(self.ontology_manager.create_ontology_triple_classes(subject_class_name="OntologySubject", - object_class_name="OntologyObject", - predicate_class_name="predicate", - inverse_predicate_class_name="inverse_predicate")) - - subject_class = self.main_ontology.OntologySubject - self.assertIsNotNone(subject_class) - subject_individual = subject_class("subject") - self.assertIsNotNone(subject_individual.predicate) - - object_class = self.main_ontology.OntologyObject - self.assertIsNotNone(object_class) - object_individual = object_class("object") - self.assertIsNotNone(object_individual.inverse_predicate) - - # Test dynamic triple classes creation as inheriting from existing parent ontology classes - PLACEABLE_ON_PREDICATE_NAME = "placeable_on" - HOLD_OBJ_PREDICATE_NAME = "hold_obj" - self.assertTrue( - self.ontology_manager.create_ontology_triple_classes( - ontology_subject_parent_class=self.soma.Container if self.soma else None, - subject_class_name="OntologyPlaceHolderObject", - ontology_object_parent_class=self.dul.PhysicalObject - if self.dul else None, - object_class_name="OntologyHandheldObject", - predicate_class_name=PLACEABLE_ON_PREDICATE_NAME, - inverse_predicate_class_name=HOLD_OBJ_PREDICATE_NAME, - ontology_property_parent_class=self.soma.affordsBearer - if self.soma else None, - ontology_inverse_property_parent_class=self.soma.isBearerAffordedBy - if self.soma else None)) - - def create_ontology_handheld_object_designator(object_name: str, ontology_parent_class: Type[owlready2.Thing]): - return self.ontology_manager.create_ontology_linked_designator(object_name=object_name, - designator_class=ObjectDesignatorDescription, - ontology_concept_name=f"Onto{object_name}", - ontology_parent_class=ontology_parent_class) - - # Holdable object - egg = create_ontology_handheld_object_designator("egg", self.main_ontology.OntologyHandheldObject) - # Placeholder object - egg_tray = create_ontology_handheld_object_designator("egg_tray", self.main_ontology.OntologyPlaceHolderObject) - - # Create ontology relation between [Place-holder] and [Holdable obj] - self.ontology_manager.set_ontology_relation(subject_designator=egg, object_designator=egg_tray, - predicate_name=PLACEABLE_ON_PREDICATE_NAME) - - self.ontology_manager.set_ontology_relation(subject_designator=egg_tray, object_designator=egg, - predicate_name=HOLD_OBJ_PREDICATE_NAME) - - # Query potential designator candidates based on above-set ontology relations among them - egg_placeholders = [placeholder.names for placeholder in \ - self.ontology_manager.get_designators_by_subject_predicate(subject=egg, - predicate_name=PLACEABLE_ON_PREDICATE_NAME)] - self.assertTrue(len(egg_placeholders) == 1) - self.assertEqual(egg_placeholders[0], ["egg_tray"]) - - egg_tray_holdables = [placeholder.names for placeholder in \ - self.ontology_manager.get_designators_by_subject_predicate(subject=egg_tray, - predicate_name=HOLD_OBJ_PREDICATE_NAME)] - self.assertTrue(len(egg_tray_holdables) == 1) - self.assertEqual(egg_tray_holdables[0], ["egg"]) - - @unittest.skipUnless(owlready2, 'Owlready2 is required') - def test_ontology_class_destruction(self): - concept_class_name = 'DynamicOntologyConcept' - dynamic_ontology_concept_class = self.ontology_manager.create_ontology_concept_class(concept_class_name) - OntologyConceptHolder(dynamic_ontology_concept_class(name='dynamic_ontology_concept3', - namespace=self.main_ontology)) - - self.ontology_manager.destroy_ontology_class(dynamic_ontology_concept_class) - self.assertIsNone(self.ontology_manager.get_ontology_class(concept_class_name)) - self.assertFalse(OntologyConceptHolderStore().get_ontology_concepts_by_class(dynamic_ontology_concept_class)) - - @unittest.skipUnless(owlready2, 'Owlready2 is required') - @unittest.skipUnless(java_runtime_installed, 'Java runtime is required') - def test_ontology_reasoning(self): - REASONING_TEST_ONTOLOGY_IRI = f"reasoning_test{ONTOLOGY_OWL_FILE_EXTENSION}" - ENTITY_CONCEPT_NAME = "Entity" - CAN_TRANSPORT_PREDICATE_NAME = "can_transport" - TRANSPORTABLE_BY_PREDICATE_NAME = "transportable_by" - CORESIDE_PREDICATE_NAME = "coreside" - - # Create a test world (with memory SQL backend) for reasoning - reasoning_world = self.ontology_manager.create_ontology_world() - reasoning_ontology = reasoning_world.get_ontology(REASONING_TEST_ONTOLOGY_IRI) - - # Create Entity class & types of relations among its instances - self.assertTrue(self.ontology_manager.create_ontology_triple_classes(subject_class_name=ENTITY_CONCEPT_NAME, - object_class_name=ENTITY_CONCEPT_NAME, - predicate_class_name=CAN_TRANSPORT_PREDICATE_NAME, - inverse_predicate_class_name=TRANSPORTABLE_BY_PREDICATE_NAME, - ontology_property_parent_class=owlready2.ObjectProperty, - ontology_inverse_property_parent_class=owlready2.ObjectProperty, - ontology=reasoning_ontology)) - - self.assertTrue(self.ontology_manager.create_ontology_triple_classes(subject_class_name=ENTITY_CONCEPT_NAME, - object_class_name=ENTITY_CONCEPT_NAME, - predicate_class_name=CORESIDE_PREDICATE_NAME, - ontology_property_parent_class=owlready2.ObjectProperty, - ontology=reasoning_ontology)) - - # Define rules for `transportability` & `co-residence` in [reasoning_ontology] - with reasoning_ontology: - def can_transport_itself(a: reasoning_ontology.Entity) -> bool: - return a in a.can_transport - - def can_transport_entity(a: reasoning_ontology.Entity, b: reasoning_ontology.Entity) -> bool: - return b in a.can_transport - - def can_be_transported_by(a: reasoning_ontology.Entity, b: reasoning_ontology.Entity) -> bool: - return b in a.transportable_by - - def coresidents(a: reasoning_ontology.Entity, b: reasoning_ontology.Entity) -> bool: - return b in a.coreside - - # Rule1: Transitivity of transportability - self.ontology_manager.create_rule_transitivity(ontology_concept_class_name=ENTITY_CONCEPT_NAME, - predicate_name=CAN_TRANSPORT_PREDICATE_NAME, - ontology=reasoning_ontology) - - # Rule2: reflexivity of transportability - self.ontology_manager.create_rule_reflexivity(ontology_concept_class_name=ENTITY_CONCEPT_NAME, - predicate_name=CAN_TRANSPORT_PREDICATE_NAME, - ontology=reasoning_ontology) - - # Rule3 & 4: Symmetry & Transitivity of co-residence - self.ontology_manager.create_rule_transitivity(ontology_concept_class_name=ENTITY_CONCEPT_NAME, - predicate_name=CORESIDE_PREDICATE_NAME, - ontology=reasoning_ontology) - self.ontology_manager.create_rule_symmetry(ontology_concept_class_name=ENTITY_CONCEPT_NAME, - predicate_name=CORESIDE_PREDICATE_NAME, - ontology=reasoning_ontology) - - # Create entities - entities = [reasoning_ontology.Entity(name=f"e{i}") for i in range(3)] - entities[2].can_transport.append(entities[1]) - entities[1].can_transport.append(entities[0]) - entities[0].coreside.append(entities[1]) - entities[0].coreside.append(entities[2]) - - # Reason on [reasoning_world] - self.ontology_manager.reason(world=reasoning_world) - - # Test reflexivity - for entity in entities: - self.assertTrue(can_transport_itself(entity)) - - # Test transitivity - self.assertTrue(can_transport_entity(entities[2], entities[0])) - self.assertTrue(can_be_transported_by(entities[0], entities[2])) - - # Test symmetry - entities_num = len(entities) - for i in range(entities_num): - for j in range(entities_num): - if i != j: - self.assertTrue(coresidents(entities[i], entities[j])) - - @unittest.skipUnless(owlready2, 'Owlready2 is required') - def test_ontology_save(self): - save_dir = self.ontology_manager.get_main_ontology_dir() - owl_filepath = f"{save_dir}/{Path(self.ontology_manager.main_ontology_iri).stem}{ONTOLOGY_OWL_FILE_EXTENSION}" - self.assertTrue(self.ontology_manager.save(owl_filepath)) - self.assertTrue(Path(owl_filepath).is_file()) - sql_backend = self.ontology_manager.main_ontology_sql_backend - if sql_backend != ONTOLOGY_SQL_IN_MEMORY_BACKEND: - self.assertTrue(Path(sql_backend).is_file()) - -if __name__ == '__main__': - unittest.main() diff --git a/test/test_orm/__init__.py b/test/test_orm/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/test/test_database_merger.py b/test/test_orm/test_database_merger.py similarity index 100% rename from test/test_database_merger.py rename to test/test_orm/test_database_merger.py diff --git a/test/test_orm.py b/test/test_orm/test_orm.py similarity index 97% rename from test/test_orm.py rename to test/test_orm/test_orm.py index 6609e3992..3c5ef6af5 100644 --- a/test/test_orm.py +++ b/test/test_orm/test_orm.py @@ -2,6 +2,8 @@ import time import unittest import time + +import owlready2 from sqlalchemy import select import sqlalchemy.orm import pycram.orm.action_designator @@ -10,9 +12,8 @@ import pycram.orm.object_designator import pycram.orm.tasktree import pycram.tasktree -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase from pycram.datastructures.dataclasses import Color -from pycram.ontology.ontology import OntologyManager, SOMA_ONTOLOGY_IRI from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher from pycram.world_concepts.world_object import Object from pycram.designators import action_designator, object_designator, motion_designator @@ -25,6 +26,7 @@ from pycram.orm.views import PickUpWithContextView from pycram.datastructures.enums import Arms, Grasp, GripperState, ObjectType from pycram.worlds.bullet_world import BulletWorld +from pycrap import ontology, Apartment, Robot, Milk class DatabaseTestCaseMixin(BulletWorldTestCase): @@ -271,7 +273,7 @@ def test_setGripperAction(self): self.assertEqual(result[0].motion, GripperState.OPEN) def test_open_and_closeAction(self): - apartment = Object("apartment", ObjectType.ENVIRONMENT, "apartment.urdf") + apartment = Object("apartment", Apartment, "apartment.urdf") apartment_desig = BelieveObject(names=["apartment"]).resolve() handle_desig = object_designator.ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig, type=ObjectType.ENVIRONMENT).resolve() @@ -304,12 +306,11 @@ def setUpClass(cls): cls.engine = sqlalchemy.create_engine("sqlite+pysqlite:///:memory:", echo=False) environment_path = "apartment.urdf" cls.world = BulletWorld(WorldMode.DIRECT) - cls.robot = Object("pr2", ObjectType.ROBOT, path="pr2.urdf", pose=Pose([1, 2, 0])) - cls.apartment = Object(environment_path[:environment_path.find(".")], ObjectType.ENVIRONMENT, environment_path) - cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1, -1.78, 0.55], [1, 0, 0, 0]), + cls.robot = Object("pr2", Robot, path="pr2.urdf", pose=Pose([1, 2, 0])) + cls.apartment = Object(environment_path[:environment_path.find(".")], Apartment, environment_path) + cls.milk = Object("milk", Milk, "milk.stl", pose=Pose([1, -1.78, 0.55], [1, 0, 0, 0]), color=Color(1, 0, 0, 1)) cls.viz_marker_publisher = VizMarkerPublisher() - OntologyManager(SOMA_ONTOLOGY_IRI) def setUp(self): self.world.reset_world() @@ -326,6 +327,7 @@ def tearDown(self): @classmethod def tearDownClass(cls): + cls.world.ontology.destroy_individuals() cls.viz_marker_publisher._stop_publishing() cls.world.exit() @@ -340,7 +342,7 @@ def test_believe_object(self): LookAtAction(targets=[Pose([1, -1.78, 0.55])]).resolve().perform() - object_desig = DetectAction(BelieveObject(types=[ObjectType.MILK])).resolve().perform() + object_desig = DetectAction(BelieveObject(types=[Milk])).resolve().perform() TransportAction(object_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() diff --git a/test/test_pycrap/__init__.py b/test/test_pycrap/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/test/test_pycrap/test_pycrap.py b/test/test_pycrap/test_pycrap.py new file mode 100644 index 000000000..8fc76a6af --- /dev/null +++ b/test/test_pycrap/test_pycrap.py @@ -0,0 +1,37 @@ +import unittest +import pycrap +import inspect + +from pycrap import Ontology + + +def recursive_subclasses(cls): + """ + :param cls: The class. + :return: A list of the classes subclasses. + """ + return cls.__subclasses__() + [g for s in cls.__subclasses__() for g in recursive_subclasses(s)] + + +class CrapTestCase(unittest.TestCase): + + def setUp(self): + self.ontology = Ontology() + + def test_creation(self): + for cls in recursive_subclasses(pycrap.Base): + cls: pycrap.Base + cls.set_comment_to_docstring() + self.assertTrue(len(pycrap.PhysicalObject.comment) > 0) + + def test_multiple_worlds(self): + second_ontology = Ontology() + cup1 = pycrap.Cup(namespace=self.ontology.ontology) + cup2 = pycrap.Cup(namespace=second_ontology.ontology) + self.assertEqual(len(list(self.ontology.individuals())), 1) + self.assertEqual(len(list(second_ontology.individuals())), 1) + self.assertNotEqual(cup1, cup2) + + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/test/test_task_tree.py b/test/test_task_tree.py index 01bda73c8..8c50a1b2a 100644 --- a/test/test_task_tree.py +++ b/test/test_task_tree.py @@ -7,7 +7,7 @@ from pycram.tasktree import with_tree import unittest import anytree -from bullet_world_testcase import BulletWorldTestCase +from pycram.testing import BulletWorldTestCase import pycram.failures from pycram.designators import object_designator, action_designator