diff --git a/doc/images/knowledge/knowledge_arch.png b/doc/images/knowledge/knowledge_arch.png new file mode 100644 index 000000000..48062ddbc Binary files /dev/null and b/doc/images/knowledge/knowledge_arch.png differ diff --git a/doc/images/knowledge/property_evaluation.png b/doc/images/knowledge/property_evaluation.png new file mode 100644 index 000000000..32306c850 Binary files /dev/null and b/doc/images/knowledge/property_evaluation.png differ diff --git a/doc/images/knowledge/property_resolution.png b/doc/images/knowledge/property_resolution.png new file mode 100644 index 000000000..8a16277a5 Binary files /dev/null and b/doc/images/knowledge/property_resolution.png differ diff --git a/doc/source/_toc.yml b/doc/source/_toc.yml index 3aebc861b..3395c904f 100644 --- a/doc/source/_toc.yml +++ b/doc/source/_toc.yml @@ -14,6 +14,8 @@ parts: - file: new_robot.rst - file: notebooks.rst - file: designators.rst + - file: knowledge.rst + - file: knowledge_and_reasoning.rst - file: costmap.rst - caption: Trouble Shooting @@ -28,6 +30,8 @@ parts: - file: notebooks/bullet_world - file: notebooks/language - file: notebooks/local_transformer + - file: notebooks/minimal_task_tree + - file: notebooks/improving_actions - file: designator_example.rst sections: - file: notebooks/action_designator @@ -48,6 +52,10 @@ parts: - file: notebooks/interface_examples/giskard.md - file: notebooks/interface_examples/robokudo.md - file: notebooks/ontology + - file: knowledge_examples.rst + sections: + - file: notebooks/knowledge_source.md + - file: notebooks/properties.md - caption: API chapters: diff --git a/doc/source/knowledge.rst b/doc/source/knowledge.rst new file mode 100644 index 000000000..cb974554b --- /dev/null +++ b/doc/source/knowledge.rst @@ -0,0 +1,91 @@ +========= +Knowledge +========= + +To be able to perform tasks in unknown environments a robot needs to have a way to reason and access +knowledge about the world. This knowledge can be represented in different ways and formats. In this +chapter we will discuss how PyCRAM access different kinds of knowledge and integrates them with +action designators. + +------- +Concept +------- +The concept of knowledge in PyCRAM is based on the idea that knowledge can be represented in different ways and provided +from different sources which can be specialized for different tasks. These different sources of knowledge are implemented +behind a common interface which provides a set of methods to query the knowledge. This architecture can be seen in the +following image: + +.. image:: ../images/knowledge/knowledge_arch.png + :align: center + :alt: Knowledge Architecture + +The methods provided by the knowledge sources, are called "properties" since they are used to reason about the properties +of entities in the world. Properties can be combined to create more complex expressions which describe conditions +that have to be true at the time an action designator is executed. Let's look at an example explaining this: + +.. code-block:: python + + GraspableProperty(ObjectDesignator(...)) + & ReachableProperty(Pose(....)) + +In this example, we have two properties, one that checks if an object is graspable and one that checks if a pose is reachable. +The `&` operator is used to combine the two properties into a single property that checks if both conditions are true at +the same time. This combined property stems from the PickUpAction where the object a robot wants to pick up has to be +reachable for the robot as well as being able to fit into the end effector of the robot. + +Since knowledge sources are usually specialized for a certain task, they do not need to be able to implement all methods +of the interface. This leads to a lot of knowledge sources which all implement a subset of the methods, therefore no +knowledge source can be used to answer all questions. To solve this problem, PyCRAM has a central interface for processing +the combined properties and querying the knowledge sources called the "KnowledgeEngine". The job of the KnowledgeEngine +is to take a combined property and resolve the individual properties to the available knowledge sources which implement +the methods needed to answer the question. The resolved properties are then combined in the same way as the input property +and evaluated. + +This image shows the process of resolving the properties through the knowledge engine: + +.. image:: ../images/knowledge/property_resolution.png + :align: center + :alt: Property Resolution + + + +----------------- +Knowledge Sources +----------------- +Knowledge does not have a unified form or representation, it can be available as an SQL database, a Knowledge Graph, +a simple JSON file, etc. To be able to handle a multitude of different representations of knowledge, PyCRAM uses the +concept of Knowledge Sources. A Knowledge Source is a class that implements a set of methods to access knowledge. Therefore, +PyCRAM does not care how the knowledge is accesses or where it is from as as long as the Knowledge Source implements the +abstract methods. + +The methods that a Knowledge Source must implement are some basic methods to manage connecting to the knowledge itself +and more importantly, methods to query the knowledge. Which methods are provided by each knowledge source decides each +knowledge source on its own by using the respective property as a mix-in of the knowledge source. The properties curren +available and which a knowledge source can implement are: + +- `GraspableProperty`: Checks if an object is graspable +- `ReachableProperty`: Checks if a pose is reachable +- `SpaceIsFreeProperty`: Checks if a space is free for the robot to move to +- `GripperIsFreeProperty`: Checks if the gripper is free to grasp an object +- `VisibleProperty`: Checks if an object is visible + + +If you want to learn more about the implementation of a Knowledge Source, you can look at the following example: + +:ref:`Knowledge Source example` + +---------------- +Knowledge Engine +---------------- +The Knowledge Engine is the central component in PyCRAM to reason about the world. It takes a combined property and +resolves the individual properties to the available knowledge sources which implement the methods needed to answer the +question. + +While the properties are resolved they also infer parameter which are needed to execute the action but may not be defined +in the action designator description. For example, the PickUpAction needs an arm to pick up an object with, however, the +arm does not need to be defined in the action designator and can be inferred from the properties and the state of the +world. + +After the properties are resolved, evaluated and the parameters are inferred, the Knowledge Engine grounds the action +in the belief state and tests if the found solution is valid and can achieve the goal. If the solution is valid, the +Knowledge Engine returns the solution and the action designator is performed. \ No newline at end of file diff --git a/doc/source/knowledge_and_reasoning.rst b/doc/source/knowledge_and_reasoning.rst new file mode 100644 index 000000000..a86f46d60 --- /dev/null +++ b/doc/source/knowledge_and_reasoning.rst @@ -0,0 +1,48 @@ +======================= +Knowledge and Reasoning +======================= + +The knowledge engine is able to infer parameters of a designator description from the context given by the properties +attached to its parameters. Since the properties are defined for the parameters of a designator description they add +semantic information to the designator description parameters. The knowledge engine is able to utilize this information +to infer the value of a parameter from the context. + +Inference is done very similar to the normal reasoning process where the property function of the designator description +is first resolved and then evaluated. The difference is that we now not only look at the result (if the properties are +satisfied or not) but also a the possible parameter solutions that are generated while reasoning. + +We start again by taking the properties of of the designator description and resolve them. This was already explained in +the :ref:`Knowledge ` documentation. + +.. image:: ../images/knowledge/property_resolution.png + :alt: Source Resolve + :align: center + +We then evaluate the properties and generate the possible parameter solutions. + +.. image:: ../images/knowledge/property_evaluation.png + :alt: Source Evaluate + :align: center + +As can be seen in the picture above the major part of inferring missing parameter is done by the Knowledge Source with +the semantic context provided by the properties. The magics consists now of matching the inferred parameter from the +Knowledge Sources with the parameter of the designator description. + +------------------------------- +Matching of inferred parameters +------------------------------- + +The parameter that are inferred by the Knowledge Source during reasoning need to be matched to the designator to be +usable in the execution of a designator. Matching of the inferred parameters is kinda ambiguous since the parameter are +provided by the Knowledge Source as a dictionary with a name and the value. Therefore the name given in the dictionary +might not match the designator. + +To solve the issue of aligning the inferred parameters from the Knowledge Source with the parameters of the designator +we employ two methods. The first is to match the names in the dictionary with the names of the parameters of the +designator. This is most reliable when the return from the Knowledge Source tries to adhere to the conventions of the +designator description. +The second method is to match the type of the inferred parameter with the type annotations in the designator. While this +seems like the more reliable method, it cloud happen that a designator has multiple parameters of the same type. In this +case the matching might not yield the correct result, since the first found parameter of the designator is matched with +the parameter of the Knowledge Source. + diff --git a/doc/source/knowledge_examples.rst b/doc/source/knowledge_examples.rst new file mode 100644 index 000000000..5090c017b --- /dev/null +++ b/doc/source/knowledge_examples.rst @@ -0,0 +1,3 @@ +======================= +Knowledge Examples +======================= \ No newline at end of file diff --git a/examples/action_designator.md b/examples/action_designator.md index 0496ce710..3acfd11a6 100644 --- a/examples/action_designator.md +++ b/examples/action_designator.md @@ -244,9 +244,9 @@ from pycram.datastructures.enums import Arms milk_desig = BelieveObject(names=["milk"]) description = TransportAction(milk_desig, - [Arms.LEFT], [Pose([2.4, 1.8, 1], - [0, 0, 0, 1])]) + [0, 0, 0, 1])], + [Arms.LEFT]) with simulated_robot: MoveTorsoAction([0.2]).resolve().perform() description.resolve().perform() diff --git a/examples/improving_actions.md b/examples/improving_actions.md index cdd5eabfc..17b5da281 100644 --- a/examples/improving_actions.md +++ b/examples/improving_actions.md @@ -75,12 +75,15 @@ session = sqlalchemy.orm.sessionmaker(bind=engine)() Now we construct an empty world with just a floating milk, where we can learn about PickUp actions. ```python +from pycrap import Robot, Milk + world = BulletWorld(WorldMode.DIRECT) print(world.prospection_world) -robot = Object("pr2", ObjectType.ROBOT, "pr2.urdf") -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +robot = Object("pr2", Robot, "pr2.urdf") +milk = Object("milk", Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) +viz_marker_publisher = VizMarkerPublisher() viz_marker_publisher = VizMarkerPublisher() -milk_description = ObjectDesignatorDescription(types=[ObjectType.MILK]).ground() +milk_description = ObjectDesignatorDescription(types=[Milk]).ground() ``` Next, we create a default, probabilistic model that describes how to pick up objects. We visualize the default policy. @@ -164,10 +167,11 @@ Next, we put the learned model to the test in a complex environment, where the m area. ```python -kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "apartment.urdf") +from pycrap import Apartment +kitchen = Object("apartment", Apartment, "apartment.urdf") milk.set_pose(Pose([0.5, 3.15, 1.04])) -milk_description = ObjectDesignatorDescription(types=[ObjectType.MILK]).ground() +milk_description = ObjectDesignatorDescription(types=[Milk]).ground() fpa = MoveAndPickUp(milk_description, arms=[Arms.LEFT, Arms.RIGHT], grasps=[Grasp.FRONT, Grasp.LEFT, Grasp.RIGHT, Grasp.TOP], policy=model) fpa.sample_amount = 200 diff --git a/examples/knowledge_source.md b/examples/knowledge_source.md new file mode 100644 index 000000000..855760bd5 --- /dev/null +++ b/examples/knowledge_source.md @@ -0,0 +1,166 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 (ipykernel) + language: python + name: python3 +--- + +# How to create Knowledge Source +(knowledge_source_header)= +This notebook will detail what a knowledge source does, how it works and how you can create your own. + +A knowledge source is part of the wider knowledge system in PyCRAM as explained [here](/knowledge). The purpose of a +knowledge source is to provide an interface to an external knowledge and reasoning system. + +A knowledge source essentially consists of two parts, management methods which take care of connecting to the knowledge +system as well as registering the knowledge source with the knowledge engine and the implementation of the respective +reasoning queries which this knowledge source is able to process. + +In this example we will walk through the process of creating a simple knowledge source and all steps involved in this process. + +## Creating the Knowledge Source structure + +We will start by creating the general structure of the Knowledge Source as well as the management methods. To do this +you have to create a new class which inherits from the ```KnowledgeSource``` class. + +```python +from pycram.knowledge.knowledge_source import KnowledgeSource + +class ExampleKnowledge(KnowledgeSource): + + def __init__(self): + super().__init__(name="example", priority=0) + self.parameter = {} + + def is_available(self) -> bool: + return True + + def is_connected(self) -> bool: + return True + + def connect(self): + pass + + def clear_state(self): + self.parameter = {} +``` + +What we did in the code above was creating a class which inherits from the ```KowledgeSource``` base class, in the +constructor of this new class we initialize the base class with the name of the new Knowledge Source as well as a +priority. The priority is used to determine the order of all Knowledge Sources, in case two Knowledge Sources provide +the same reasoning queries the one with the higher priority is used. + +Furthermore, we define a number of methods that manage the connection to the knowledge system namely the methods +```is_available```, ```is_connected``` and ```connect```. The first two methods just return a bool which states if the +knowledge system is available and connected to this Knowledge Source. The last method is used to create a connection +to the knowledge system. Since this is an example and we are not connecting to an external knowledge system the methods +are fairly trivial. + +The last method we defined is ```clear_state```, this is used to clear any state the knowledge source itself might hold +either of the knowledge system or of this Knowledge Source class itself. + + +# Managing the resolvable Properties +Properties serve two purposes in the management of knowledge in PyCRAM, the first is to define semantic properties of +parameter of action designator. The second is to specify which properties or knowledge queries a Knowledge Source can +answer. + +To define which properties a Knowledge Source can handle we simply use the respective properties as mix-in for the class +definition. With this let's take another look at our Knowledge Source with the handling of two properties. + +```python +from pycram.knowledge.knowledge_source import KnowledgeSource +from pycram.datastructures.property import ReachableProperty, SpaceIsFreeProperty +from pycram.datastructures.world import World +from pycram.datastructures.pose import Pose +from pycram.datastructures.dataclasses import ReasoningResult +from pycram.costmaps import OccupancyCostmap +from pycram.ros.logging import loginfo +import numpy as np + +class ExampleKnowledge(KnowledgeSource, ReachableProperty, SpaceIsFreeProperty): + + def __init__(self): + super().__init__(name="example", priority=0) + self.parameter = {} + + def is_available(self) -> bool: + return True + + def is_connected(self) -> bool: + return True + + def connect(self): + pass + + def clear_state(self): + self.parameter = {} + + def reachable(self, pose: Pose) -> ReasoningResult: + loginfo(f"Checking reachability for pose {pose}") + robot_pose = World.robot.pose + distance = pose.dist(robot_pose) + return ReasoningResult(distance < 0.6) + + def space_is_free(self, pose: Pose) -> ReasoningResult: + loginfo(f"Checking if the space is free around {pose}") + om = OccupancyCostmap(0.2, False, 100, 0.02, pose) + return ReasoningResult(np.sum(om.map) == 6561) +``` + +Now we extend our Knowledge Source with the capability to handle two properties, Reachable and SpaceIsFree. As you can +see all we needed to do for this is to use the respective properties as mix-ins besides the Knowledge Source as well as +implement the method for each property which does the actual reasoning. + +In this case the reasoning is kept fairly simple, since this is not the objective of this example. Reachable just +checks if a pose is within 60 centimeters of the robot while SpaceIsFree checks if a 2x2 meter square around the given +pose has no obstacles. + +The methods doing the reasoning have to return a ```ReasoningResult``` instance, which contains a bool stating if the +reasoning succeeded or failed as well as additional parameters which might be inferred during reasoning. The additional +parameters are stated as key-value pairs in a dictionary. + + +# Testing the Knowledge Source +Since we now have a Knowledge Source which also implements two properties we can check if the Knowledge Source is used +to resolve the correct properties. + +For this test we need a world as well as a robot. + +```python +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +from pycram.datastructures.enums import WorldMode, ObjectType +from pycram.knowledge.knowledge_engine import KnowledgeEngine +from pycram.datastructures.pose import Pose +from pycram.datastructures.property import ReachableProperty, SpaceIsFreeProperty +from pycrap import Robot + +world = BulletWorld(WorldMode.GUI) +pr2 = Object("pr2", Robot, "pr2.urdf") + +target_pose = Pose([0.3, 0, 0.2]) +property = ReachableProperty(target_pose) & SpaceIsFreeProperty(target_pose) + +ke = KnowledgeEngine() +resolved_property = ke.resolve_properties(property) + +print(f"Result of the property: {resolved_property()}") +``` + +As you can see we created a ```ReachableProperty``` as well as a ```SpaceIsFreeProperty``` and resolved them. For more +details on how properties and their resolution work please referee to the properties example. + +Afterwards, we execute the properties, here we can see the logging infos from our Knowledge Source as well as the +confirmation that the implementation for both properties worked correctly. + +```python +world.exit() +``` diff --git a/examples/orm_example.md b/examples/orm_example.md index 442bcd7c1..1aa4074ac 100644 --- a/examples/orm_example.md +++ b/examples/orm_example.md @@ -171,7 +171,7 @@ class SayingActionPerformable(ActionAbstract): orm_class = ORMSaying @with_tree - def perform(self) -> None: + def plan(self) -> None: print(self.text) ``` diff --git a/examples/properties.md b/examples/properties.md new file mode 100644 index 000000000..47e44e127 --- /dev/null +++ b/examples/properties.md @@ -0,0 +1,115 @@ +--- +jupyter: + jupytext: + text_representation: + extension: .md + format_name: markdown + format_version: '1.3' + jupytext_version: 1.16.3 + kernelspec: + display_name: Python 3 (ipykernel) + language: python + name: python3 +--- + +# Property +Properties are the part of knowledge which is most prominent to the user. In this example we will go over what properties +are, how they are defined and how they integrate into the bigger knowledge system. + +Properties are used in PyCRAM to annotate parameter of action designator with semantic properties. Properties are only +proxy objects which do not implement the actual functionality of the property, this is done in the knowledge Source +which can handle this property. For more information how Knowledge Sources work and how they interact with properties +please look at the knowledge_source example. + +In this example we will define an example property and use this as a running example to walk through all steps involved +in creating a new property. + +## Creating a new Property +To create a new property in PyCRAM we simply need to create a new class which inherits from the ```Property``` class. + +```python +from pycram.datastructures.property import Property +from pycram.datastructures.pose import Pose +from pycram.datastructures.dataclasses import ReasoningResult +from pycram.failures import ReasoningError +from abc import abstractmethod + +class ExampleProperty(Property): + + property_exception = ReasoningError + + def __init__(self, pose: Pose): + super().__init__() + self.pose = pose + + @abstractmethod + def example(self, pose: Pose) -> ReasoningResult: + raise NotImplementedError +``` + +And with that we created a new property. While the process of creating a new property is simple and straight forward +there are still a few things a user has to adhere to that should be kept in mind. + +* Only **one** method, besides the constructor, can defined otherwise the resolution will fail. +* A ```property_exception``` has to be defined, which is raised in case the reasoning about the property is not +* successful +* The reasoning method of the property can have an arbitraty amount of parameter, however, for each of these parameter +* an instance variable with the same name has to be present. + * This means for this example: when the ```example``` method is called later on, while evaluating all properties, + * it will be called with the value of ```self.pose``` as parameter. + +Now we can use this property to annotate a variable. + +```python +example_property = ExampleProperty(Pose([1, 1, 1])) +``` + +As already mentioned do properties not implement the actual reasoning functionality and are just proxy objects to +annotate parameter of action designator. The reasoning functionality is implemented in a Knowledge Source, so to test +our new property we need to create a Knowledge Source which can handle the ```ExampleProperty```. For detailed +information how a Knowledge Source is created pleas refere to the respective example. + +## A Knowledge Source for our Property + +```python +from pycram.knowledge.knowledge_source import KnowledgeSource +from pycram.datastructures.pose import Pose +from pycram.datastructures.dataclasses import ReasoningResult + +class ExampleKnowledge(KnowledgeSource, ExampleProperty): + + def __init__(self): + super().__init__(name="example", priority=0) + self.parameter = {} + + def is_available(self) -> bool: + return True + + def is_connected(self) -> bool: + return True + + def connect(self): + pass + + def clear_state(self): + self.parameter = {} + + def example(self, pose: Pose) -> ReasoningResult: + null_pose = Pose([1, 1, 1]) + distance = null_pose.dist(pose) + return ReasoningResult(distance == 0.0) + +``` + +Since we now have a Knowledge Source that can handle our ```ExampleProperty``` we can resolve and evaluate the property. +The reasoning implementation just checks if the distance between the given pose and a pose at [1, 1, 1] is zero, +meaning they point at the same point. + +```python +from pycram.knowledge.knowledge_engine import KnowledgeEngine + +ke = KnowledgeEngine() +resolved_property = ke.resolve_properties(example_property) + +print(f"Result of the property: {resolved_property()}") +``` diff --git a/examples/robot_description.md b/examples/robot_description.md index 0de1b831f..bf16a97de 100644 --- a/examples/robot_description.md +++ b/examples/robot_description.md @@ -16,9 +16,9 @@ jupyter: (robot_description_header)= The robot description contains semantic information about the robot which can not be extracted from the URDF in a -general way. This inludes kinematic chains, end-effectors, cameras and their parameter, etc. +general way. This includes kinematic chains, end-effectors, cameras and their parameter, etc. -In genral a Robot Description consists a number of different descriptions, these are: +In general a Robot Description consists a number of different descriptions, these are: * RobotDescription * KinematicChainDescription diff --git a/requirements.txt b/requirements.txt index 4b14d5a35..ff7fc5ea9 100644 --- a/requirements.txt +++ b/requirements.txt @@ -10,7 +10,7 @@ SQLAlchemy>=2.0.36 tqdm==4.66.3 psutil==5.9.7 lxml==4.9.1 -typing_extensions==4.9.0 +typing_extensions>=4.10.0 owlready2>=0.45 Pillow>=10.3.0 pynput~=1.7.7 @@ -23,4 +23,7 @@ deprecated probabilistic_model>=6.0.2 random_events>=3.1.2 sympy -pint>=0.21.1 \ No newline at end of file +pint>=0.21.1 + +Pygments~=2.14.0 +typeguard~=4.3.0 \ No newline at end of file diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index 395260ad2..f20fb4714 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -2,7 +2,7 @@ from abc import ABC, abstractmethod from copy import deepcopy, copy -from dataclasses import dataclass +from dataclasses import dataclass, field from typing_extensions import List, Optional, Tuple, Callable, Dict, Any, Union, TYPE_CHECKING @@ -710,3 +710,12 @@ class MultiverseContactPoint: body_name: str contact_force: List[float] contact_torque: List[float] + + +@dataclass +class ReasoningResult: + """ + Result of a reasoning result of knowledge source + """ + success: bool + reasoned_parameter: Dict[str, Any] = field(default_factory=dict) \ No newline at end of file diff --git a/src/pycram/datastructures/partial_designator.py b/src/pycram/datastructures/partial_designator.py new file mode 100644 index 000000000..dc82541f5 --- /dev/null +++ b/src/pycram/datastructures/partial_designator.py @@ -0,0 +1,118 @@ +# used for delayed evaluation of typing until python 3.11 becomes mainstream +from __future__ import annotations + +from collections.abc import Iterable + +from typing_extensions import Type, List, Tuple, Any, Dict, TYPE_CHECKING +from itertools import product +from inspect import signature + + +if TYPE_CHECKING: + from ..designator import ActionDesignatorDescription + + +class PartialDesignator: + """ + A partial designator_description is somewhat between a DesignatorDescription and a specified designator_description. Basically it is a + partially initialized specified designator_description which can take a list of input arguments (like a DesignatorDescription) + and generate a list of specified designators with all possible permutations of the input arguments. + + PartialDesignators are designed as generators, as such they need to be iterated over to yield the possible specified + designators. Please also keep in mind that at the time of iteration all parameter of the specified designator_description need + to be filled, otherwise a TypeError will be raised, see the example below for usage. + + .. code-block:: python + + # Example usage + partial_designator = PartialDesignator(PickUpActionPerformable, milk_object_designator, arm=[Arms.RIGHT, Arms.LEFT]) + for performable in partial_designator(Grasp.FRONT): + performable.perform() + """ + performable: Type[ActionDesignatorDescription.Action] + """ + Reference to the performable class that should be initialized + """ + args: Tuple[Any, ...] + """ + Arguments that are passed to the performable + """ + kwargs: Dict[str, Any] + """ + Keyword arguments that are passed to the performable + """ + + def __init__(self, performable: Type[ActionDesignatorDescription.Action], *args, **kwargs): + self.performable = performable + # Remove None values fom the given arguments and keyword arguments + self.kwargs = dict(signature(self.performable).bind_partial(*args, **kwargs).arguments) + for key in dict(signature(self.performable).parameters).keys(): + if key not in self.kwargs.keys(): + self.kwargs[key] = None + + + def __call__(self, *fargs, **fkwargs): + """ + Creates a new PartialDesignator with the given arguments and keyword arguments added. Existing arguments will + be prioritized over the new arguments. + + :param fargs: Additional arguments that should be added to the new PartialDesignator + :param fkwargs: Additional keyword arguments that should be added to the new PartialDesignator + :return: A new PartialDesignator with the given arguments and keyword arguments added + """ + newkeywors = {k: v for k, v in self.kwargs.items() if v is not None} + for key, value in fkwargs.items(): + if key in newkeywors.keys() and newkeywors[key] is None: + newkeywors[key] = value + elif key not in newkeywors.keys(): + newkeywors[key] = value + return PartialDesignator(self.performable, *fargs, **newkeywors) + + def __iter__(self) -> Type[ActionDesignatorDescription.Action]: + """ + Iterates over all possible permutations of the arguments and keyword arguments and creates a new performable + object for each permutation. In case there are conflicting parameters the args will be used over the keyword + arguments. + + :return: A new performable object for each permutation of arguments and keyword arguments + """ + for kwargs_combination in PartialDesignator.generate_permutations(self.kwargs.values()): + yield self.performable(**dict(zip(self.kwargs.keys(), kwargs_combination))) + + @staticmethod + def generate_permutations(args: Iterable) -> List: + """ + Generates the cartesian product of the given arguments. Arguments can also be a list of lists of arguments. + + :param args: An iterable with arguments + :yields: A list with a possible permutation of the given arguments + """ + iter_list = [x if PartialDesignator._is_iterable(x) else [x] for x in args] + for combination in product(*iter_list): + yield combination + + @staticmethod + def _is_iterable(obj: Any) -> bool: + """ + Checks if the given object is iterable. + + :param obj: The object that should be checked + :return: True if the object is iterable, False otherwise + """ + try: + iter(obj) + except TypeError: + return False + return True + + def missing_parameter(self) -> List[str]: + """ + Returns a list of all parameters that are missing for the performable to be initialized. + + :return: A list of parameter names that are missing from the performable + """ + missing = {k: v for k, v in self.kwargs.items() if v is None} + return list(missing.keys()) + + + diff --git a/src/pycram/datastructures/property.py b/src/pycram/datastructures/property.py new file mode 100644 index 000000000..967b4c00a --- /dev/null +++ b/src/pycram/datastructures/property.py @@ -0,0 +1,403 @@ +# used for delayed evaluation of typing until python 3.11 becomes mainstream +from __future__ import annotations + +from abc import abstractmethod + +from functools import reduce + +from .enums import Arms +from .dataclasses import ReasoningResult +from .pose import Pose +from typing_extensions import List, Iterable, Dict, Any, Callable, Type, TYPE_CHECKING +from anytree import NodeMixin, PreOrderIter, Node +from ..failures import ObjectNotVisible, ManipulationPoseUnreachable, NavigationGoalInCollision, ObjectUnfetchable, \ + GripperOccupied, PlanFailure + +if TYPE_CHECKING: + from ..designators.object_designator import ObjectDesignatorDescription + + +class Property(NodeMixin): + """ + Parent class to represent a semantic property as part of a knowledge pre-condition of designators. + Aspects can be combined using logical operators to create complex pre-conditions, the resulting expression is a + datastructure of a tree. + """ + variables: Dict[str, Any] + """ + Dictionary of variables and their values which are used in the property tree. This dictionary is only to be used in + the root node. + """ + + def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None): + super().__init__() + self.parent: Property = parent + self.variables: Dict[str, Any] = {} + self.executed: bool = False + self._result: ReasoningResult = ReasoningResult(False) + if children: + self.children = children + + def __and__(self, other): + """ + Overload the and operator to create an And as part of the tree structure. + + :param other: The other property to combine with + :return: An And containing both, this and the other, property + """ + return And([self, other]) + + def __or__(self, other): + """ + Overload the or operator to create an Or as part of the tree structure. + + :param other: The other property to combine with + :return: An Or containing both, this and the other, property + """ + return Or([self, other]) + + def __neg__(self): + """ + Overload the not operator to create a Not as part of the tree structure. + + :return: A Not containing this property + """ + return Not(self) + + def __invert__(self): + """ + Overload the invert operator to create a Not as part of the tree structure. + + :return: A Not containing this property + """ + return Not(self) + + @property + def result(self) -> ReasoningResult: + return self._result + + @result.setter + def result(self, value): + self._result = value + + def manage_io(self, func: Callable, *args, **kwargs) -> ReasoningResult: + """ + Manages the ReasoningResult of a property function and calls it. The success of the method will be passed to the + parent node while the reasoned parameters will be stored in the variables dictionary of the root node. + + :param func: Property function to call + :param args: args to pass to the function + :param kwargs: keyword args to pass to the function + :return: result of the function + """ + reasoning_result = func(*args, **kwargs) + self.root.variables.update(reasoning_result.reasoned_parameter) + self.result = reasoning_result + return reasoning_result + + @property + def successful(self) -> bool: + """ + Returns the result of the complete tree structure. This is done by iterating over the tree and checking if all + nodes return True. + + :return: True if all nodes in the tree are True, False otherwise + """ + success = self.result.success + for node in PreOrderIter(self.root): + success &= node.result.success + return success + + @property + def resolved(self) -> bool: + """ + Returns True if all nodes in the tree are resolved and are either of type ResolvedProperty or PropertyOperator. + + :return: True if all nodes in the tree are resolved, False otherwise + """ + res = True + for node in PreOrderIter(self.root): + res &= isinstance(node, (ResolvedProperty, PropertyOperator)) + return res + + +class PropertyOperator(Property): + """ + Parent class for logical operators to combine multiple properties in a tree structure. This class adds methods to + use Properties as part of a tree structure. Furthermore, there is a method to simplify the tree structure by merging + Nodes of the same type. + """ + + def __init__(self, properties: List[Property]): + """ + Initialize the PropertyOperator with a list of Properties. The parent of this node is None, therefore the node is + always the root of the tree. + + :param properties: A list of properties to which are the children of this node + """ + super().__init__(parent=None, children=properties) + + def simplify(self): + """ + Simplifies the tree structure by merging nodes of the same type. + + :return: Returns the root node of the tree + """ + for node in PreOrderIter(self.root): + for child in node.children: + if type(node) is type(child): + self.merge_nodes(node, child) + return self.root + + @staticmethod + def merge_nodes(node1: Node, node2: Node) -> None: + """ + Merges node1 with node2 in a tree. The children of node2 will be re-parented to node1 and node2 will be deleted + from the tree. + + :param node1: Node that should be left in the tree + :param node2: Node which children should be appended to node1 and then deleted + """ + node2.parent = None + node1.children = node2.children + node1.children + + +class And(PropertyOperator): + """ + Represents a logical AND between multiple properties. + """ + + def __init__(self, properties: List[Property]): + """ + Initialize the And with a list of properties as the children of this node. This node will be the root of the + tree. + + :param properties: A list of properties which are the children of this node + """ + super().__init__(properties) + self.simplify() + + @property + def result(self): + """ + Returns the result of this node. The result is the combination of results of all nodes below this one. + + :return: A ReasoningResult of the combination of all children + """ + suc = all([node.result.success for node in self.children]) + return ReasoningResult(suc, reduce(lambda a, b: {**a, **b}, + [node.result.reasoned_parameter for node in self.children])) + + def __call__(self, *args, **kwargs) -> ReasoningResult: + """ + Evaluate the children of this node as an and operator. This is done by iterating over the children and calling + them with the given arguments. If one child returns False, the evaluation will be stopped and False will be + returned. + + :param args: A list of arguments to pass to the children + :param kwargs: A dictionary of keyword arguments to pass to the children + :return: True if all children return True, False otherwise + """ + self.executed = True + + for child in self.children: + res = child(*args, **kwargs) + if not res.success: + break + + return self.result + + +class Or(PropertyOperator): + """ + Represents a logical OR between multiple properties. + """ + + def __init__(self, properties: List[Property]): + """ + Initialize the Or with a list of properties as the children of this node. This node will be the root of the + tree. + + :param properties: A list of properties which are the children of this node + """ + super().__init__(properties) + self.simplify() + + @property + def result(self): + """ + Returns the result of this node. The result is the combination of results of all nodes below this one. + + :return: A ReasoningResult of the combination of all children + """ + suc = any([node.result.success for node in self.children]) + return ReasoningResult(suc, reduce(lambda a, b: {**a, **b}, + [node.result.reasoned_parameter for node in self.children])) + + def __call__(self, *args, **kwargs) -> ReasoningResult: + """ + Evaluate the children of this node as an or operator. This is done by iterating over the children and calling + them with the given arguments. If one child returns True, the evaluation will be stopped and True will be + returned. + + :param args: A list of arguments to pass to the children + :param kwargs: A dictionary of keyword arguments to pass to the children + :return: True if one child returns True, False otherwise + """ + self.executed = True + + for child in self.children: + child(*args, **kwargs) + return self.result + + +class Not(PropertyOperator): + """ + Represents a logical NOT of a single property. + """ + + def __init__(self, property: Property): + """ + Initialize the Not with an aspect as the child of this node. This node will be the root of the tree. + + :param property: The property which is the child of this node + """ + super().__init__([property]) + + @property + def result(self): + """ + Returns the result of this node. The result is the result of the child since a Not Operator can only have one + child. + + :return: A ReasoningResult of the combination of all children + """ + return ReasoningResult(not self.children[0].result.success, self.children[0].result.reasoned_parameter) + + def __call__(self, *args, **kwargs) -> ReasoningResult: + """ + Evaluate the child of this node as a not operator. This is done by calling the child with the given arguments + and returning the negation of the result. + + :param args: A list of arguments to pass to the child + :param kwargs: A dictionary of keyword arguments to pass to the child + :return: The negation of the result of the child + """ + self.executed = True + self.children[0](*args, **kwargs) + return self.result + + +class ResolvedProperty(Property): + """ + Represents a resolved property function. It holds the reference to the respective function in the knowledge + source and the exception that should be raised if the property is not fulfilled. Its main purpose is to manage the + call to the property function as well as handle the input and output variables. + """ + + resolved_property_function: Callable + """ + Reference to the actual implementation of the property function in the KnowledgeSource. + """ + property_exception: Type[PlanFailure] + """ + Exception that should be raised if the property is not fulfilled. + """ + + def __init__(self, resolved_property_function: Callable, property_exception: Type[PlanFailure], + parent: NodeMixin = None): + """ + Initialize the ResolvedProperty with the executed property function, the exception that should be raised if the property + is not fulfilled, the parent node. + + :param resolved_property_function: Reference to the function in the knowledge source + :param property_exception: Exception that should be raised if the property is not fulfilled + :param parent: Parent node of this property + """ + super().__init__(parent, None) + self.resolved_property_function = resolved_property_function + self.property_exception = property_exception + self.parameter = {} + + def __call__(self, *args, **kwargs) -> ReasoningResult: + """ + Manages the io of the call to the property function and then calls the function. If the function returns False, + the exception defined in :attr:`property_exception` will be raised. + + :return: The result of the property function + """ + self.executed = True + self.result = self.manage_io(self.resolved_property_function, **self.parameter) + if not self.result.success: + raise self.property_exception(f"Property function {self.resolved_property_function} returned False") + return self.result + + + +class EmptyProperty(Property): + property_exception = PlanFailure + + def __init__(self): + super().__init__(None, None) + + def empty(self) -> ReasoningResult: + return ReasoningResult(True) + +class ReachableProperty(Property): + property_exception = ManipulationPoseUnreachable + + def __init__(self, pose: Pose): + super().__init__(None, None) + self.pose = pose + + @abstractmethod + def reachable(self, pose: Pose) -> ReasoningResult: + raise NotImplementedError + + +class GraspableProperty(Property): + property_exception = ObjectUnfetchable + + def __init__(self, object_designator: ObjectDesignatorDescription): + super().__init__(None, None) + self.object_designator = object_designator + + @abstractmethod + def graspable(self, object_designator: ObjectDesignatorDescription) -> ReasoningResult: + raise NotImplementedError + + +class SpaceIsFreeProperty(Property): + property_exception = NavigationGoalInCollision + + def __init__(self, pose: Pose): + super().__init__(None, None) + self.pose = pose + + @abstractmethod + def space_is_free(self, pose: Pose) -> ReasoningResult: + raise NotImplementedError + + +class GripperIsFreeProperty(Property): + property_exception = GripperOccupied + + def __init__(self, grippers: List[Arms]): + super().__init__(None, None) + self.grippers = grippers + + @abstractmethod + def gripper_is_free(self, grippers: List[Arms]) -> ReasoningResult: + raise NotImplementedError + + +class VisibleProperty(Property): + property_exception = ObjectNotVisible + + def __init__(self, object_designator: ObjectDesignatorDescription): + super().__init__(None, None) + self.object_designator = object_designator + + @abstractmethod + def is_visible(self, object_designator: ObjectDesignatorDescription) -> ReasoningResult: + raise NotImplementedError diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 1a9ee3eb5..6ccb0fead 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -5,9 +5,15 @@ from dataclasses import dataclass, field, fields from inspect import isgenerator, isgeneratorfunction +from typing_extensions import get_type_hints from pycrap import PhysicalObject, Agent +from .datastructures.property import Property, EmptyProperty from .ros.logging import logwarn, loginfo +import inspect + +from .knowledge.knowledge_engine import KnowledgeEngine + try: import owlready2 except ImportError: @@ -27,7 +33,7 @@ from .language import Language from .datastructures.pose import Pose from .robot_description import RobotDescription -from .datastructures.enums import ObjectType +from .datastructures.enums import ObjectType, Grasp import logging @@ -38,20 +44,18 @@ from .orm.base import RobotState, ProcessMetaData from .tasktree import with_tree -if TYPE_CHECKING: - from .ontology.ontology_common import OntologyConceptHolder - class DesignatorError(Exception): - """Implementation of designator errors.""" + """Implementation of designator_description errors.""" def __init__(self, *args, **kwargs): - """Create a new designator error.""" + """Create a new designator_description error.""" Exception.__init__(self, *args, **kwargs) class ResolutionError(Exception): - def __init__(self, missing_properties: List[str], wrong_type: Dict, current_type: Any, designator: Designator): + def __init__(self, missing_properties: List[str], wrong_type: Dict, current_type: Any, + designator: DesignatorDescription): self.error = f"\nSome requiered properties where missing or had the wrong type when grounding the Designator: {designator}.\n" self.missing = f"The missing properties where: {missing_properties}\n" self.wrong = f"The properties with the wrong type along with the currrent -and right type :\n" @@ -68,277 +72,19 @@ def __init__(self, missing_properties: List[str], wrong_type: Dict, current_type super(ResolutionError, self).__init__(self.message) -class Designator(ABC): - """ - Implementation of designators. DEPRECTAED SINCE DESIGNATOR DESCRIPTIONS ARE USED AS BASE CLASS - - Designators are objects containing sequences of key-value pairs. They can be resolved which means to generate real - parameters for executing performables from these pairs of key and value. - - :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 - argument and return a list of solutions. A solution can also be a generator. - """ - - def __init__(self, description: DesignatorDescription, parent: Optional[Designator] = None): - """Create a new desginator. - - Arguments: - :param description: A list of tuples (key-value pairs) describing this designator. - :param parent: The parent to equate with (default is None). - """ - self._mutex: Lock = Lock() - self._parent: Union[Designator, None] = None - self._successor: Union[Designator, None] = None - self._effective: bool = False - self._data: Any = None - self._solutions = None - self._index: int = 0 - self.timestamp = None - self._description: DesignatorDescription = description - - if parent is not None: - self.equate(parent) - - def equate(self, parent: Designator) -> None: - """Equate the designator with the given parent. - - Arguments: - parent -- the parent to equate with. - """ - if self.equal(parent): - return - - youngest_parent = parent.current() - first_parent = parent.first() - - if self._parent is not None: - first_parent._parent = self._parent - first_parent._parent._successor = first_parent - - self._parent = youngest_parent - youngest_parent._successor = self - - def equal(self, other: Designator) -> bool: - """Check if the designator describes the same entity as another designator, i.e. if they are equated. - - Arguments: - other -- the other designator. - """ - return other.first() is self.first() - - def first(self) -> Designator: - """Return the first ancestor in the chain of equated designators.""" - if self._parent is None: - return self - - return self._parent.first() - - def current(self) -> Designator: - """Return the newest designator, i.e. that one that has been equated last to the designator or one of its - equated designators.""" - if self._successor is None: - return self - - return self._successor.current() - - def _reference(self) -> Any: - """This is a helper method for internal usage only. - - This method is to be overwritten instead of the reference method. - """ - resolver = self.resolvers[self._description.resolver] - if self._solutions is None: - def generator(): - solution = resolver(self) - if isgeneratorfunction(solution): - solution = solution() - - if isgenerator(solution): - while True: - try: - yield next(solution) - except StopIteration: - break - else: - yield solution - - self._solutions = GeneratorList(generator) - - if self._data is not None: - return self._data - - try: - self._data = self._solutions.get(self._index) - return self._data - except StopIteration: - raise DesignatorError('There was no Solution for this Designator') - - def reference(self) -> Any: - """Try to dereference the designator and return its data object or raise DesignatorError if it is not an - effective designator. """ - with self._mutex: - ret = self._reference() - - self._effective = True - - if self.timestamp is None: - self.timestamp = time() - - return ret - - @abstractmethod - def next_solution(self): - """Return another solution for the effective designator or None if none exists. The next solution is a newly - constructed designator with identical properties that is equated to the designator since it describes the same - entity. """ - pass - - def solutions(self, from_root: Optional[Designator] = None): - """Return a generator for all solutions of the designator. - - Arguments: - from_root -- if not None, the generator for all solutions beginning from with the original designator is returned (default is None). - """ - if from_root is not None: - desig = self.first() - else: - desig = self - - def generator(desig): - while desig is not None: - try: - yield desig.reference() - except DesignatorError: - pass - - desig = desig.next_solution() - - return generator(desig) - - def copy(self, new_properties: Optional[List] = None) -> Designator: - """Construct a new designator with the same properties as this one. If new properties are specified, these will - be merged with the old ones while the new properties are dominant in this relation. - - Arguments: - new_properties -- a list of new properties to merge into the old ones (default is None). - """ - description = self._description.copy() - - if new_properties: - for key, value in new_properties: - description.__dict__[key] = value - - return self.__class__(description) - - def make_effective(self, properties: Optional[List] = None, - data: Optional[Any] = None, - timestamp: Optional[float] = None) -> Designator: - """Create a new effective designator of the same type as this one. If no properties are specified, this ones are used. - - Arguments: - new_properties -- a list of properties (default is None). - data -- the low-level data structure the new designator describes (default is None). - timestamp -- the timestamp of creation of reference (default is the current). - """ - if properties is None: - properties = self._description - - desig = self.__class__(properties) - desig._effective = True - desig._data = data - - if timestamp is None: - desig.timestamp = time() - else: - desig.timestamp = timestamp - - return desig - - def newest_effective(self) -> Designator: - """Return the newest effective designator.""" - - def find_effective(desig): - if desig is None or desig._effective: - return desig - - return find_effective(desig._parent) - - return find_effective(self.current()) - - def prop_value(self, key: str) -> Any: - """Return the first value matching the specified property key. - - Arguments: - key -- the key to return the value of. - """ - try: - return self._description.__dict__[key] - except KeyError: - logging.error(f"The given key '{key}' is not in this Designator") - return None - - def check_constraints(self, properties: List) -> bool: - """Return True if all the given properties match, False otherwise. - - Arguments: - properties -- the properties which have to match. A property can be a tuple in which case its first value is the - key of a property which must equal the second value. Otherwise it's simply the key of a property which must be - not None. - """ - for prop in properties: - if type(prop) == tuple: - key, value = prop - - if self.prop_value(key) != value: - return False - else: - if self.prop_value(prop) is None: - return False - - return True - - def make_dictionary(self, properties: List) -> Dict: - """ DEPRECATED, Moved to the description. Function only keept because of - backward compatability. - Return the given properties as dictionary. - - Arguments: - properties -- the properties to create a dictionary of. A property can be a tuple in which case its first value - is the dictionary key and the second value is the dictionary value. Otherwise it's simply the dictionary key - and the key of a property which is the dictionary value. - """ - - return self._description.make_dictionary(properties) - - def rename_prop(self, old: str, new: str) -> Designator: - old_value = self.prop_value(old) - if old_value is not None: - self._description.__dict__[new] = old_value - del self._description.__dict__[old] - else: - raise DesignatorError("Old property does not exists.") - return self.current() - - class DesignatorDescription(ABC): """ - :ivar resolve: The specialized_designators function to use for this designator, defaults to self.ground + :ivar resolve: The specialized_designators function to use for this designator_description, defaults to self.ground """ - def __init__(self, resolver: Optional[Callable] = None): + def __init__(self): """ Create a Designator description. - - :param resolver: The grounding method used for the description. The grounding method creates a location instance that matches the description. """ + pass - if resolver is None: - self.resolve = self.ground + def resolve(self): + return self.ground() def make_dictionary(self, properties: List[str]): """ @@ -374,16 +120,47 @@ def copy(self) -> DesignatorDescription: + def get_optional_parameter(self) -> List[str]: + """ + Returns a list of optional parameter names of this designator_description description. + """ + return [param_name for param_name, param in inspect.signature(self.__init__).parameters.items() if + param.default != param.empty] + + def get_all_parameter(self) -> List[str]: + """ + Returns a list of all parameter names of this designator_description description. + """ + return [param_name for param_name, param in inspect.signature(self.__init__).parameters.items()] + + def get_type_hints(self) -> Dict[str, Any]: + """ + Returns the type hints of the __init__ method of this designator_description description. + + :return: + """ + return get_type_hints(self.__init__) + class ActionDesignatorDescription(DesignatorDescription, Language): """ - Abstract class for action designator descriptions. + Abstract class for action designator_description descriptions. Descriptions hold possible parameter ranges for action designators. """ + knowledge_condition = None + """ + Knowledge condition that have to be fulfilled before executing the action. + """ + + performable_class: Type[ActionDesignatorDescription.Action] + """ + Reference to the performable class that is used to execute the action. + """ + @dataclass class Action: """ - The performable designator with a single element for each list of possible parameter. + The performable designator_description with a single element for each list of possible parameter. """ robot_position: Pose = field(init=False) """ @@ -405,13 +182,38 @@ def __post_init__(self): RobotDescription.current_robot_description.torso_joint) self.robot_type = World.robot.obj_type - @with_tree def perform(self) -> Any: """ Executes the action with the single parameters from the description. + + :return: The result of the action in the plan + """ + self.pre_perform() + result = self.plan() + self.post_perform() + return result + + @with_tree + def plan(self) -> Any: + """ + Plan of the action. To be overridden by subclasses. + + :return: The result of the action, if there is any """ raise NotImplementedError() + def pre_perform(self): + """ + This method is called before the perform method is executed. To be overridden by subclasses. + """ + pass + + def post_perform(self): + """ + This method is called after the perform method is executed. To be overridden by subclasses. + """ + pass + def to_sql(self) -> ORMAction: """ Create an ORM object that corresponds to this description. @@ -449,14 +251,34 @@ def insert(self, session: Session, *args, **kwargs) -> ORMAction: return action - def __init__(self, resolver=None): - """ - Base of all action designator descriptions. + @classmethod + def get_type_hints(cls) -> Dict[str, Any]: + """ + Returns the type hints of the __init__ method of this designator_description description. - :param resolver: An alternative resolver that returns an action designator + :return: + """ + return get_type_hints(cls) + + def __init__(self): """ - super().__init__(resolver) + Base of all action designator_description descriptions. + """ + super().__init__() Language.__init__(self) + self.knowledge_condition = EmptyProperty() + self.ground = self.resolve + + def resolve(self) -> Type[ActionDesignatorDescription.Action]: + """ + Resolves this designator_description to a performable designtor by using the reasoning of the knowledge engine. + This method will simply take the first result from iterating over the designator_description. + + :return: A fully specified Action Designator + """ + if getattr(self, "__iter__", None): + return next(iter(self)) + raise NotImplementedError(f"{type(self)} has no __iter__ method.") def ground(self) -> Action: """Fill all missing parameters and chose plan to execute. """ @@ -473,22 +295,22 @@ def __iter__(self): class LocationDesignatorDescription(DesignatorDescription): """ - Parent class of location designator descriptions. + Parent class of location designator_description descriptions. """ @dataclass class Location: """ Resolved location that represents a specific point in the world which satisfies the constraints of the location - designator description. + designator_description description. """ pose: Pose """ - The resolved pose of the location designator. Pose is inherited by all location designator. + The executed pose of the location designator_description. Pose is inherited by all location designator_description. """ - def __init__(self, resolver=None): - super().__init__(resolver) + def __init__(self): + super().__init__() def ground(self) -> Location: """ @@ -511,7 +333,7 @@ def ground(self) -> Location: class ObjectDesignatorDescription(DesignatorDescription): """ - Class for object designator descriptions. + Class for object designator_description descriptions. Descriptions hold possible parameter ranges for object designators. """ @@ -566,7 +388,7 @@ def insert(self, session: Session) -> ORMObjectDesignator: metadata = ProcessMetaData().insert(session) pose = self.pose.insert(session) - # create object orm designator + # create object orm designator_description obj = self.to_sql() obj.process_metadata = metadata obj.pose = pose @@ -610,7 +432,7 @@ def __repr__(self): [f"{f.name}={self.__getattribute__(f.name)}" for f in fields(self)] + [ f"pose={self.pose}"]) + ')' - def special_knowledge_adjustment_pose(self, grasp: str, pose: Pose) -> Pose: + def special_knowledge_adjustment_pose(self, grasp: Grasp, pose: Pose) -> Pose: """ Get the adjusted target pose based on special knowledge for "grasp front". @@ -635,16 +457,14 @@ 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[Type[PhysicalObject]]] = None, - resolver: Optional[Callable] = None): + def __init__(self, names: Optional[List[str]] = None, types: Optional[List[Type[PhysicalObject]]] = None): """ - Base of all object designator descriptions. Every object designator has the name and type of the object. + Base of all object designator_description descriptions. Every object designator_description 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 """ - super().__init__(resolver) + super().__init__() self.types: Optional[List[ObjectType]] = types self.names: Optional[List[str]] = names @@ -652,7 +472,7 @@ def ground(self) -> Union[Object, bool]: """ Return the first object from the world that fits the description. - :return: A resolved object designator + :return: A executed object designator_description """ return next(iter(self)) @@ -660,7 +480,7 @@ def __iter__(self) -> Iterable[Object]: """ Iterate through all possible objects fitting this description - :yield: A resolved object designator + :yield: A executed object designator_description """ # for every world object for obj in World.current_world.objects: diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index bad8e3acf..592448950 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -14,6 +14,10 @@ from .motion_designator import MoveJointsMotion, MoveGripperMotion, MoveArmJointsMotion, MoveTCPMotion, MoveMotion, \ LookingMotion, DetectingMotion, OpeningMotion, ClosingMotion from .object_designator import ObjectDesignatorDescription, BelieveObject, ObjectPart +from ..datastructures.partial_designator import PartialDesignator +from ..datastructures.property import GraspableProperty, ReachableProperty, GripperIsFreeProperty, SpaceIsFreeProperty, \ + VisibleProperty +from ..knowledge.knowledge_engine import ReasoningInstance from ..local_transformer import LocalTransformer from ..failures import ObjectUnfetchable, ReachabilityFailure from ..robot_description import RobotDescription @@ -39,1015 +43,1153 @@ from dataclasses import dataclass, field -class MoveTorsoAction(ActionDesignatorDescription): +# ---------------------------------------------------------------------------- +# ---------------- Performables ---------------------------------------------- +# ---------------------------------------------------------------------------- + + +@dataclass +class ActionAbstract(ActionDesignatorDescription.Action, abc.ABC): + """Base class for performable performables.""" + orm_class: Type[ORMAction] = field(init=False, default=None, repr=False) """ - Action Designator for Moving the torso of the robot up and down + The ORM class that is used to insert this action into the database. Must be overwritten by every action in order to + be able to insert the action into the database. """ - 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. + @abc.abstractmethod + def plan(self) -> None: """ - super().__init__(resolver) - self.positions: List[float] = positions - + plan of the action. - def ground(self) -> MoveTorsoActionPerformable: + Will be overwritten by each action. """ - Creates a performable action designator with the first element from the list of possible torso heights. + pass - :return: A performable action designator + def to_sql(self) -> Action: """ - return MoveTorsoActionPerformable(self.positions[0]) + Convert this action to its ORM equivalent. - def __iter__(self): - """ - Iterates over all possible values for this designator and returns a performable action designator with the value. + Needs to be overwritten by an action if it didn't overwrite the orm_class attribute with its ORM equivalent. - :return: A performable action designator + :return: An instance of the ORM equivalent of the action with the parameters set """ - for position in self.positions: - yield MoveTorsoActionPerformable(position) - - -class SetGripperAction(ActionDesignatorDescription): - """ - Set the gripper state of the robot - """ + # get all class parameters + class_variables = {key: value for key, value in vars(self).items() + if key in inspect.getfullargspec(self.__init__).args} - 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'. + # get all orm class parameters + orm_class_variables = inspect.getfullargspec(self.orm_class.__init__).args - :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 - """ - super().__init__(resolver) - self.grippers: List[GripperState] = grippers - self.motions: List[Arms] = motions + # list of parameters that will be passed to the ORM class. If the name does not match the orm_class equivalent + # or if it is a type that needs to be inserted into the session manually, it will not be added to the list + parameters = [value for key, value in class_variables.items() if key in orm_class_variables + and not isinstance(value, (ObjectDesignatorDescription.Object, Pose))] - def ground(self) -> SetGripperActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first element in the grippers and motions list. + return self.orm_class(*parameters) - :return: A performable designator + def insert(self, session: Session, **kwargs) -> Action: """ - return SetGripperActionPerformable(self.grippers[0], self.motions[0]) + Insert this action into the database. - def __iter__(self): - """ - Iterates over all possible combinations of grippers and motions + Needs to be overwritten by an action if the action has attributes that do not exist in the orm class + equivalent. In that case, the attributes need to be inserted into the session manually. - :return: A performable designator with a combination of gripper and motion + :param session: Session with a database that is used to add and commit the objects + :param kwargs: Possible extra keyword arguments + :return: The completely instanced ORM action that was inserted into the database """ - for parameter_combination in itertools.product(self.grippers, self.motions): - yield SetGripperActionPerformable(*parameter_combination) + action = super().insert(session) -class ReleaseAction(ActionDesignatorDescription): - """ - Releases an Object from the robot. + # get all class parameters + class_variables = {key: value for key, value in vars(self).items() + if key in inspect.getfullargspec(self.__init__).args} - Note: This action can not be used yet. - """ + # get all orm class parameters + orm_class_variables = inspect.getfullargspec(self.orm_class.__init__).args - def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, - resolver=None): - super().__init__(resolver) - self.grippers: List[Arms] = grippers - self.object_designator_description = object_designator_description + # loop through all class parameters and insert them into the session unless they are already added by the ORM + for key, value in class_variables.items(): + if key not in orm_class_variables: + variable = value.insert(session) + if isinstance(variable, ORMObject): + action.object = variable + elif isinstance(variable, ORMPose): + action.pose = variable + session.add(action) - def ground(self) -> ReleaseActionPerformable: - return ReleaseActionPerformable(self.grippers[0], self.object_designator_description.ground()) + return action -class GripAction(ActionDesignatorDescription): +@dataclass +class MoveTorsoActionPerformable(ActionAbstract): """ - Grip an object with the robot. - - :ivar grippers: The grippers to consider - :ivar object_designator_description: The description of objects to consider - :ivar efforts: The efforts to consider - - Note: This action can not be used yet. + Move the torso of the robot up and down. """ - def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, - 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 + position: float + """ + Target position of the torso joint + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMMoveTorsoAction) - def ground(self) -> GripActionPerformable: - return GripActionPerformable(self.grippers[0], self.object_designator_description.ground(), self.efforts[0]) + @with_tree + def plan(self) -> None: + MoveJointsMotion([RobotDescription.current_robot_description.torso_joint], [self.position]).perform() -class ParkArmsAction(ActionDesignatorDescription): +@dataclass +class SetGripperActionPerformable(ActionAbstract): """ - Park the arms of the robot. + Set the gripper state of the robot. """ - def __init__(self, arms: List[Arms], resolver=None): - """ - Moves the arms in the pre-defined parking position. Arms are taken from pycram.enum.Arms + gripper: Arms + """ + The gripper that should be set + """ + motion: GripperState + """ + The motion that should be set on the gripper + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMSetGripperAction) - :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 - """ - super().__init__(resolver) - self.arms: List[Arms] = arms + @with_tree + def plan(self) -> None: + MoveGripperMotion(gripper=self.gripper, motion=self.motion).perform() - def ground(self) -> ParkArmsActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first element of the list of possible arms +@dataclass +class ReleaseActionPerformable(ActionAbstract): + """ + Releases an Object from the robot. - :return: A performable designator - """ - return ParkArmsActionPerformable(self.arms[0]) + Note: This action can not ve used yet. + """ + gripper: Arms -class PickUpAction(ActionDesignatorDescription): - """ - Designator to let the robot pick up an object. - """ + object_designator: ObjectDesignatorDescription.Object - def __init__(self, - object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], - 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. + def plan(self) -> None: + raise NotImplementedError - :param object_designator_description: List of possible object designator - :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 - """ - super().__init__(resolver) - self.object_designator_description: Union[ - ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description - self.arms: List[Arms] = arms - self.grasps: List[Grasp] = grasps +@dataclass +class GripActionPerformable(ActionAbstract): + """ + Grip an object with the robot. - def ground(self) -> PickUpActionPerformable: - """ - Default specialized_designators, returns a performable designator with the first entries from the lists of possible parameter. + Note: This action can not be used yet. + """ - :return: A performable designator - """ - if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object): - obj_desig = self.object_designator_description - else: - obj_desig = self.object_designator_description.resolve() + gripper: Arms + object_designator: ObjectDesignatorDescription.Object + effort: float - return PickUpActionPerformable(obj_desig, self.arms[0], self.grasps[0]) + @with_tree + def plan(self) -> None: + raise NotImplementedError() -class PlaceAction(ActionDesignatorDescription): +@dataclass +class ParkArmsActionPerformable(ActionAbstract): """ - Places an Object at a position using an arm. + Park the arms of the robot. """ - def __init__(self, - object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], - target_locations: List[Pose], - arms: List[Arms], resolver=None): - """ - Create an Action Description to place an object + arm: Arms + """ + Entry from the enum for which arm should be parked + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMParkArmsAction) - :param object_designator_description: Description of object to place. - :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 - """ - 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 + @with_tree + def plan(self) -> None: + # create the keyword arguments + kwargs = dict() + left_poses = None + right_poses = None - def ground(self) -> PlaceActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first entries from the list of possible entries. + # add park left arm if wanted + if self.arm in [Arms.LEFT, Arms.BOTH]: + kwargs["left_arm_config"] = "park" + left_poses = RobotDescription.current_robot_description.get_arm_chain(Arms.LEFT).get_static_joint_states( + kwargs["left_arm_config"]) - :return: A performable designator - """ - obj_desig = self.object_designator_description if isinstance(self.object_designator_description, - ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() + # add park right arm if wanted + if self.arm in [Arms.RIGHT, Arms.BOTH]: + kwargs["right_arm_config"] = "park" + right_poses = RobotDescription.current_robot_description.get_arm_chain(Arms.RIGHT).get_static_joint_states( + kwargs["right_arm_config"]) - return PlaceActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) + MoveArmJointsMotion(left_poses, right_poses).perform() -class NavigateAction(ActionDesignatorDescription): +@dataclass +class PickUpActionPerformable(ActionAbstract): """ - Navigates the Robot to a position. + Let the robot pick up an object. """ - 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 - """ - super().__init__(resolver) - self.target_locations: List[Pose] = target_locations - - - def ground(self) -> NavigateActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first entry of possible target locations. - - :return: A performable designator - """ - return NavigateActionPerformable(self.target_locations[0]) + object_designator: ObjectDesignatorDescription.Object + """ + Object designator_description describing the object that should be picked up + """ + arm: Arms + """ + The arm that should be used for pick up + """ -class TransportAction(ActionDesignatorDescription): + grasp: Grasp """ - Transports an object to a position using an arm + The grasp that should be used. For example, 'left' or 'right' """ - def __init__(self, - object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], - arms: List[Arms], - target_locations: List[Pose], resolver=None): - """ - Designator representing a pick and place plan. + object_at_execution: Optional[ObjectDesignatorDescription.Object] = field(init=False, repr=False) + """ + The object at the time this Action got created. It is used to be a static, information holding entity. It is + not updated when the BulletWorld object is changed. + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMPickUpAction) - :param object_designator_description: Object designator description or a specified Object designator that should be transported - :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 - """ - 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 + def __post_init__(self): + super(ActionAbstract, self).__post_init__() + # Store the object's data copy at execution + self.object_at_execution = self.object_designator.frozen_copy() - def ground(self) -> TransportActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first entries from the lists of possible parameter. + @with_tree + def plan(self) -> None: + robot = World.robot + # Retrieve object and robot from designators + object = self.object_designator.world_object + # Get grasp orientation and target pose + grasp = RobotDescription.current_robot_description.grasps[self.grasp] + # oTm = Object Pose in Frame map + oTm = object.get_pose() + # Transform the object pose to the object frame, basically the origin of the object frame + mTo = object.local_transformer.transform_to_object_frame(oTm, object) + # Adjust the pose according to the special knowledge of the object designator_description + adjusted_pose = self.object_designator.special_knowledge_adjustment_pose(self.grasp, mTo) + # Transform the adjusted pose to the map frame + adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map") + # multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper - :return: A performable designator - """ - obj_desig = self.object_designator_description \ - if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) \ - else self.object_designator_description.resolve() + adjusted_oTm.multiply_quaternions(grasp) - return TransportActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) + # prepose depending on the gripper (its annoying we have to put pr2_1 here tbh + # gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame" + gripper_frame = robot.get_link_tf_frame( + RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame()) + # First rotate the gripper, so the further calculations makes sense + tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) + tmp_for_rotate_pose.pose.position.x = 0 + tmp_for_rotate_pose.pose.position.y = 0 + tmp_for_rotate_pose.pose.position.z = -0.1 + gripper_rotate_pose = object.local_transformer.transform_pose(tmp_for_rotate_pose, "map") + + # Perform Gripper Rotate + # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) + # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform() + oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) + oTg.pose.position.x -= 0.1 # in x since this is how the gripper is oriented + prepose = object.local_transformer.transform_pose(oTg, "map") -class LookAtAction(ActionDesignatorDescription): + # Perform the motion with the prepose and open gripper + World.current_world.add_vis_axis(prepose) + MoveTCPMotion(prepose, self.arm, allow_gripper_collision=True).perform() + MoveGripperMotion(motion=GripperState.OPEN, gripper=self.arm).perform() + + # Perform the motion with the adjusted pose -> actual grasp and close gripper + World.current_world.add_vis_axis(adjusted_oTm) + MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True).perform() + adjusted_oTm.pose.position.z += 0.03 + MoveGripperMotion(motion=GripperState.CLOSE, gripper=self.arm).perform() + tool_frame = RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame() + robot.attach(object, tool_frame) + + # Lift object + World.current_world.add_vis_axis(adjusted_oTm) + MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True).perform() + + # Remove the vis axis from the world + World.current_world.remove_vis_axis() + + # TODO find a way to use object_at_execution instead of object_designator in the automatic orm mapping in ActionAbstract + def to_sql(self) -> Action: + return ORMPickUpAction(arm=self.arm, grasp=self.grasp) + + def insert(self, session: Session, **kwargs) -> Action: + action = super(ActionAbstract, self).insert(session) + action.object = self.object_at_execution.insert(session) + + session.add(action) + return action + + +@dataclass +class PlaceActionPerformable(ActionAbstract): + """ + Places an Object at a position using an arm. + """ + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator_description describing the object that should be place + """ + arm: Arms + """ + Arm that is currently holding the object + """ + target_location: Pose + """ + Pose in the world at which the object should be placed + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMPlaceAction) + + @with_tree + def plan(self) -> None: + object_pose = self.object_designator.world_object.get_pose() + local_tf = LocalTransformer() + + # Transformations such that the target position is the position of the object and not the tcp + tcp_to_object = local_tf.transform_pose(object_pose, + World.robot.get_link_tf_frame( + RobotDescription.current_robot_description.get_arm_chain( + self.arm).get_tool_frame())) + target_diff = self.target_location.to_transform("target").inverse_times( + tcp_to_object.to_transform("object")).to_pose() + + MoveTCPMotion(target_diff, self.arm).perform() + MoveGripperMotion(GripperState.OPEN, self.arm).perform() + World.robot.detach(self.object_designator.world_object) + retract_pose = local_tf.transform_pose(target_diff, World.robot.get_link_tf_frame( + RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame())) + retract_pose.position.x -= 0.07 + MoveTCPMotion(retract_pose, self.arm).perform() + + +@dataclass +class NavigateActionPerformable(ActionAbstract): + """ + Navigates the Robot to a position. + """ + + target_location: Pose + """ + Location to which the robot should be navigated + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMNavigateAction) + + @with_tree + def plan(self) -> None: + MoveMotion(self.target_location).perform() + + +@dataclass +class TransportActionPerformable(ActionAbstract): + """ + Transports an object to a position using an arm + """ + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator_description describing the object that should be transported. + """ + target_location: Pose + """ + Target Location to which the object should be transported + """ + arm: Arms + """ + Arm that should be used + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMTransportAction) + + @with_tree + def plan(self) -> None: + robot_desig = BelieveObject(names=[RobotDescription.current_robot_description.name]) + ParkArmsActionPerformable(Arms.BOTH).perform() + pickup_loc = CostmapLocation(target=self.object_designator, reachable_for=robot_desig.resolve(), + reachable_arm=self.arm) + # Tries to find a pick-up position for the robot that uses the given arm + pickup_pose = None + for pose in pickup_loc: + if self.arm in pose.reachable_arms: + pickup_pose = pose + break + if not pickup_pose: + raise ObjectUnfetchable( + f"Found no pose for the robot to grasp the object: {self.object_designator} with arm: {self.arm}") + + NavigateActionPerformable(pickup_pose.pose).perform() + PickUpActionPerformable(self.object_designator, self.arm, Grasp.FRONT).perform() + ParkArmsActionPerformable(Arms.BOTH).perform() + try: + place_loc = CostmapLocation(target=self.target_location, reachable_for=robot_desig.resolve(), + reachable_arm=self.arm).resolve() + except StopIteration: + raise ReachabilityFailure( + f"No location found from where the robot can reach the target location: {self.target_location}") + NavigateActionPerformable(place_loc.pose).perform() + PlaceActionPerformable(self.object_designator, self.arm, self.target_location).perform() + ParkArmsActionPerformable(Arms.BOTH).perform() + + +@dataclass +class LookAtActionPerformable(ActionAbstract): """ Lets the robot look at a position. """ - def __init__(self, targets: List[Pose], resolver=None): - """ - Moves the head of the robot such that it points towards the given target location. + target: Pose + """ + Position at which the robot should look, given as 6D pose + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMLookAtAction) - :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 - """ - super().__init__(resolver) - self.targets: List[Pose] = targets + @with_tree + def plan(self) -> None: + LookingMotion(target=self.target).perform() - def ground(self) -> LookAtActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first entry in the list of possible targets - :return: A performable designator - """ - return LookAtActionPerformable(self.targets[0]) +@dataclass +class DetectActionPerformable(ActionAbstract): + """ + Detects an object that fits the object description and returns an object designator_description describing the object. + """ + + object_designator: ObjectDesignatorDescription.Object + """ + Object designator_description loosely describing the object, e.g. only type. + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMDetectAction) + + @with_tree + def plan(self) -> None: + return DetectingMotion(object_type=self.object_designator.obj_type).perform() + + +@dataclass +class OpenActionPerformable(ActionAbstract): + """ + Opens a container like object + """ + + object_designator: ObjectPart.Object + """ + Object designator_description describing the object that should be opened + """ + arm: Arms + """ + Arm that should be used for opening the container + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMOpenAction) + + @with_tree + def plan(self) -> None: + GraspingActionPerformable(self.arm, self.object_designator).perform() + OpeningMotion(self.object_designator, self.arm).perform() + + MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform() + + +@dataclass +class CloseActionPerformable(ActionAbstract): + """ + Closes a container like object. + """ + + object_designator: ObjectPart.Object + """ + Object designator_description describing the object that should be closed + """ + arm: Arms + """ + Arm that should be used for closing + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMCloseAction) + + @with_tree + def plan(self) -> None: + GraspingActionPerformable(self.arm, self.object_designator).perform() + ClosingMotion(self.object_designator, self.arm).perform() + + MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform() + + +@dataclass +class GraspingActionPerformable(ActionAbstract): + """ + Grasps an object described by the given Object Designator description + """ + arm: Arms + """ + The arm that should be used to grasp + """ + object_desig: Union[ObjectDesignatorDescription.Object, ObjectPart.Object] + """ + Object Designator for the object that should be grasped + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMGraspingAction) + + @with_tree + def plan(self) -> None: + if isinstance(self.object_desig, ObjectPart.Object): + object_pose = self.object_desig.part_pose + else: + object_pose = self.object_desig.world_object.get_pose() + lt = LocalTransformer() + gripper_name = RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame() + object_pose_in_gripper = lt.transform_pose(object_pose, + World.robot.get_link_tf_frame(gripper_name)) + + pre_grasp = object_pose_in_gripper.copy() + pre_grasp.pose.position.x -= 0.1 + + MoveTCPMotion(pre_grasp, self.arm).perform() + MoveGripperMotion(GripperState.OPEN, self.arm).perform() + + MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).perform() + MoveGripperMotion(GripperState.CLOSE, self.arm, allow_gripper_collision=True).perform() + + +@dataclass +class FaceAtPerformable(ActionAbstract): + """ + Turn the robot chassis such that is faces the ``pose`` and after that perform a look at action. + """ -class DetectAction(ActionDesignatorDescription): + pose: Pose """ - Detects an object that fits the object description and returns an object designator describing the object. + The pose to face """ - def __init__(self, object_designator_description: ObjectDesignatorDescription, resolver=None): - """ - Tries to detect an object in the field of view (FOV) of the robot. + orm_class = ORMFaceAtAction - :param object_designator_description: Object designator describing the object - :param resolver: An alternative specialized_designators - """ - super().__init__(resolver) - self.object_designator_description: ObjectDesignatorDescription = object_designator_description + @with_tree + def plan(self) -> None: + # get the robot position + robot_position = World.robot.pose + # calculate orientation for robot to face the object + angle = np.arctan2(robot_position.position.y - self.pose.position.y, + robot_position.position.x - self.pose.position.x) + np.pi + orientation = list(transformations.quaternion_from_euler(0, 0, angle, axes="sxyz")) - def ground(self) -> DetectActionPerformable: - """ - Default specialized_designators that returns a performable designator with the resolved object description. + # create new robot pose + new_robot_pose = Pose(robot_position.position_as_list(), orientation) - :return: A performable designator - """ - return DetectActionPerformable(self.object_designator_description.resolve()) + # turn robot + NavigateActionPerformable(new_robot_pose).perform() + # look at target + LookAtActionPerformable(self.pose).perform() -class OpenAction(ActionDesignatorDescription): - """ - Opens a container like object - Can currently not be used +@dataclass +class MoveAndPickUpPerformable(ActionAbstract): + """ + Navigate to `standing_position`, then turn towards the object and pick it up. """ - 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. - """ - super().__init__(resolver) - self.object_designator_description: ObjectPart = object_designator_description - self.arms: List[Arms] = arms - - def ground(self) -> OpenActionPerformable: - """ - Default specialized_designators that returns a performable designator with the resolved object description and the first entries - from the lists of possible parameter. - - :return: A performable designator - """ - return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0]) - + standing_position: Pose + """ + The pose to stand before trying to pick up the object + """ -class CloseAction(ActionDesignatorDescription): + object_designator: ObjectDesignatorDescription.Object + """ + The object to pick up """ - Closes a container like object. - Can currently not be used + arm: Arms + """ + The arm to use """ - def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], resolver=None): - """ - Attempts to close an open container + grasp: Grasp + """ + The grasp to use + """ - :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 - """ - super().__init__(resolver) - self.object_designator_description: ObjectPart = object_designator_description - self.arms: List[Arms] = arms + # @with_tree + def plan(self): + NavigateActionPerformable(self.standing_position).perform() + FaceAtPerformable(self.object_designator.pose).perform() + PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform() - def ground(self) -> CloseActionPerformable: - """ - Default specialized_designators that returns a performable designator with the resolved object designator and the first entry from - the list of possible arms. +@dataclass +class MoveAndPlacePerformable(ActionAbstract): + """ + Navigate to `standing_position`, then turn towards the object and pick it up. + """ - :return: A performable designator - """ - return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0]) + standing_position: Pose + """ + The pose to stand before trying to pick up the object + """ + object_designator: ObjectDesignatorDescription.Object + """ + The object to pick up + """ -class GraspingAction(ActionDesignatorDescription): + target_location: Pose """ - Grasps an object described by the given Object Designator description + The location to place the object. """ - def __init__(self, arms: List[Arms], object_description: Union[ObjectDesignatorDescription, ObjectPart], - 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. + arm: Arms + """ + The arm to use + """ - :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 - """ - super().__init__(resolver) - self.arms: List[Arms] = arms - self.object_description: ObjectDesignatorDescription = object_description + @with_tree + def plan(self): + NavigateActionPerformable(self.standing_position).perform() + FaceAtPerformable(self.target_location).perform() + PlaceActionPerformable(self.object_designator, self.arm, self.target_location).perform() - def ground(self) -> GraspingActionPerformable: - """ - Default specialized_designators that takes the first element from the list of arms and the first solution for the object - designator description ond returns it. - :return: A performable action designator that contains specific arguments - """ - return GraspingActionPerformable(self.arms[0], self.object_description.resolve()) # ---------------------------------------------------------------------------- -# ---------------- Performables ---------------------------------------------- +# Action Designators Description # ---------------------------------------------------------------------------- -@dataclass -class ActionAbstract(ActionDesignatorDescription.Action, abc.ABC): - """Base class for performable performables.""" - orm_class: Type[ORMAction] = field(init=False, default=None) +class MoveTorsoAction(ActionDesignatorDescription): """ - The ORM class that is used to insert this action into the database. Must be overwritten by every action in order to - be able to insert the action into the database. + Action Designator for Moving the torso of the robot up and down """ + performable_class = MoveTorsoActionPerformable - @abc.abstractmethod - def perform(self) -> None: + def __init__(self, positions: List[float]): """ - Perform the action. + Create a designator_description description to move the torso of the robot up and down. - Will be overwritten by each action. + :param positions: List of possible positions of the robots torso, possible position is a float of height in metres """ - pass + super().__init__() + self.positions: List[float] = positions - def to_sql(self) -> Action: + def ground(self) -> MoveTorsoActionPerformable: """ - Convert this action to its ORM equivalent. - - Needs to be overwritten by an action if it didn't overwrite the orm_class attribute with its ORM equivalent. + Creates a performable action designator_description with the first element from the list of possible torso heights. - :return: An instance of the ORM equivalent of the action with the parameters set + :return: A performable action designator_description """ - # get all class parameters - class_variables = {key: value for key, value in vars(self).items() - if key in inspect.getfullargspec(self.__init__).args} - - # get all orm class parameters - orm_class_variables = inspect.getfullargspec(self.orm_class.__init__).args - - # list of parameters that will be passed to the ORM class. If the name does not match the orm_class equivalent - # or if it is a type that needs to be inserted into the session manually, it will not be added to the list - parameters = [value for key, value in class_variables.items() if key in orm_class_variables - and not isinstance(value, (ObjectDesignatorDescription.Object, Pose))] - - return self.orm_class(*parameters) + return MoveTorsoActionPerformable(self.positions[0]) - def insert(self, session: Session, **kwargs) -> Action: + def __iter__(self): """ - Insert this action into the database. - - Needs to be overwritten by an action if the action has attributes that do not exist in the orm class - equivalent. In that case, the attributes need to be inserted into the session manually. + Iterates over all possible values for this designator_description and returns a performable action designator_description with the value. - :param session: Session with a database that is used to add and commit the objects - :param kwargs: Possible extra keyword arguments - :return: The completely instanced ORM action that was inserted into the database + :return: A performable action designator_description """ - - action = super().insert(session) - - # get all class parameters - class_variables = {key: value for key, value in vars(self).items() - if key in inspect.getfullargspec(self.__init__).args} - - # get all orm class parameters - orm_class_variables = inspect.getfullargspec(self.orm_class.__init__).args - - # loop through all class parameters and insert them into the session unless they are already added by the ORM - for key, value in class_variables.items(): - if key not in orm_class_variables: - variable = value.insert(session) - if isinstance(variable, ORMObject): - action.object = variable - elif isinstance(variable, ORMPose): - action.pose = variable - session.add(action) - - return action - - -@dataclass -class MoveTorsoActionPerformable(ActionAbstract): - """ - Move the torso of the robot up and down. - """ - - position: float - """ - Target position of the torso joint - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMMoveTorsoAction) - - @with_tree - def perform(self) -> None: - MoveJointsMotion([RobotDescription.current_robot_description.torso_joint], [self.position]).perform() + for position in self.positions: + yield MoveTorsoActionPerformable(position) -@dataclass -class SetGripperActionPerformable(ActionAbstract): +class SetGripperAction(ActionDesignatorDescription): """ - Set the gripper state of the robot. + Set the gripper state of the robot """ - gripper: Arms - """ - The gripper that should be set - """ - motion: GripperState - """ - The motion that should be set on the gripper - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMSetGripperAction) + performable_class = SetGripperActionPerformable - @with_tree - def perform(self) -> None: - MoveGripperMotion(gripper=self.gripper, motion=self.motion).perform() + def __init__(self, grippers: List[Arms], motions: List[GripperState]): + """ + 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 + """ + super().__init__() + self.grippers: List[Arms] = grippers + self.motions: List[GripperState] = motions -@dataclass -class ReleaseActionPerformable(ActionAbstract): - """ - Releases an Object from the robot. - Note: This action can not ve used yet. - """ + def ground(self) -> SetGripperActionPerformable: + """ + Default specialized_designators that returns a performable designator_description with the first element in the grippers and motions list. - gripper: Arms + :return: A performable designator_description + """ + return SetGripperActionPerformable(self.grippers[0], self.motions[0]) - object_designator: ObjectDesignatorDescription.Object + def __iter__(self): + """ + Iterates over all possible combinations of grippers and motions - def perform(self) -> None: - raise NotImplementedError + :return: A performable designator_description with a combination of gripper and motion + """ + for parameter_combination in itertools.product(self.grippers, self.motions): + yield SetGripperActionPerformable(*parameter_combination) -@dataclass -class GripActionPerformable(ActionAbstract): +class ReleaseAction(ActionDesignatorDescription): """ - Grip an object with the robot. + Releases an Object from the robot. Note: This action can not be used yet. """ - gripper: Arms - object_designator: ObjectDesignatorDescription.Object - effort: float + performable_class = ReleaseActionPerformable - @with_tree - def perform(self) -> None: - raise NotImplementedError() + def __init__(self, object_designator_description: ObjectDesignatorDescription, grippers: List[Arms] = None): + super().__init__() + self.grippers: List[Arms] = grippers + self.object_designator_description = object_designator_description + + + def ground(self) -> ReleaseActionPerformable: + return ReleaseActionPerformable(self.grippers[0], self.object_designator_description.ground()) + def __iter__(self): + ri = ReasoningInstance(self, + PartialDesignator(ReleaseActionPerformable, self.grippers, self.object_designator_description)) + for desig in ri: + yield desig -@dataclass -class ParkArmsActionPerformable(ActionAbstract): - """ - Park the arms of the robot. - """ - arm: Arms - """ - Entry from the enum for which arm should be parked +class GripAction(ActionDesignatorDescription): """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMParkArmsAction) + Grip an object with the robot. - @with_tree - def perform(self) -> None: - # create the keyword arguments - kwargs = dict() - left_poses = None - right_poses = None + :ivar grippers: The grippers to consider + :ivar object_designator_description: The description of objects to consider + :ivar efforts: The efforts to consider - # add park left arm if wanted - if self.arm in [Arms.LEFT, Arms.BOTH]: - kwargs["left_arm_config"] = "park" - left_poses = RobotDescription.current_robot_description.get_arm_chain(Arms.LEFT).get_static_joint_states(kwargs["left_arm_config"]) + Note: This action can not be used yet. + """ - # add park right arm if wanted - if self.arm in [Arms.RIGHT, Arms.BOTH]: - kwargs["right_arm_config"] = "park" - right_poses = RobotDescription.current_robot_description.get_arm_chain(Arms.RIGHT).get_static_joint_states(kwargs["right_arm_config"]) + performable_class = GripActionPerformable - MoveArmJointsMotion(left_poses, right_poses).perform() + def __init__(self, object_designator_description: ObjectDesignatorDescription, grippers: List[Arms] = None, + efforts: List[float] = None): + super().__init__() + self.grippers: List[Arms] = grippers + self.object_designator_description: ObjectDesignatorDescription = object_designator_description + self.efforts: List[float] = efforts -@dataclass -class PickUpActionPerformable(ActionAbstract): - """ - Let the robot pick up an object. - """ + def ground(self) -> GripActionPerformable: + return GripActionPerformable(self.grippers[0], self.object_designator_description.ground(), self.efforts[0]) - object_designator: ObjectDesignatorDescription.Object - """ - Object designator describing the object that should be picked up - """ + def __iter__(self): + ri = ReasoningInstance(self, + PartialDesignator(GripActionPerformable, self.grippers, self.object_designator_description, + self.efforts)) + for desig in ri: + yield desig - arm: Arms - """ - The arm that should be used for pick up - """ - grasp: Grasp +class ParkArmsAction(ActionDesignatorDescription): """ - The grasp that should be used. For example, 'left' or 'right' + Park the arms of the robot. """ - object_at_execution: Optional[ObjectDesignatorDescription.Object] = field(init=False) - """ - The object at the time this Action got created. It is used to be a static, information holding entity. It is - not updated when the BulletWorld object is changed. - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMPickUpAction) + performable_class = ParkArmsActionPerformable - def __post_init__(self): - super(ActionAbstract, self).__post_init__() - # Store the object's data copy at execution - self.object_at_execution = self.object_designator.frozen_copy() + def __init__(self, arms: List[Arms]): + """ + Moves the arms in the pre-defined parking position. Arms are taken from pycram.enum.Arms - @with_tree - def perform(self) -> None: - robot = World.robot - # Retrieve object and robot from designators - object = self.object_designator.world_object - # Get grasp orientation and target pose - grasp = RobotDescription.current_robot_description.grasps[self.grasp] - # oTm = Object Pose in Frame map - oTm = object.get_pose() - # Transform the object pose to the object frame, basically the origin of the object frame - mTo = object.local_transformer.transform_to_object_frame(oTm, object) - # Adjust the pose according to the special knowledge of the object designator - adjusted_pose = self.object_designator.special_knowledge_adjustment_pose(self.grasp, mTo) - # Transform the adjusted pose to the map frame - adjusted_oTm = object.local_transformer.transform_pose(adjusted_pose, "map") - # multiplying the orientation therefore "rotating" it, to get the correct orientation of the gripper + :param arms: A list of possible arms, that could be used + """ + super().__init__() + self.arms: List[Arms] = arms - adjusted_oTm.multiply_quaternions(grasp) - # prepose depending on the gripper (its annoying we have to put pr2_1 here tbh - # gripper_frame = "pr2_1/l_gripper_tool_frame" if self.arm == "left" else "pr2_1/r_gripper_tool_frame" - gripper_frame = robot.get_link_tf_frame(RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame()) - # First rotate the gripper, so the further calculations makes sense - tmp_for_rotate_pose = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) - tmp_for_rotate_pose.pose.position.x = 0 - tmp_for_rotate_pose.pose.position.y = 0 - tmp_for_rotate_pose.pose.position.z = -0.1 - gripper_rotate_pose = object.local_transformer.transform_pose(tmp_for_rotate_pose, "map") + def ground(self) -> ParkArmsActionPerformable: + """ + Default specialized_designators that returns a performable designator_description with the first element of the list of possible arms - # Perform Gripper Rotate - # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) - # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform() + :return: A performable designator_description + """ + return ParkArmsActionPerformable(self.arms[0]) - oTg = object.local_transformer.transform_pose(adjusted_oTm, gripper_frame) - oTg.pose.position.x -= 0.1 # in x since this is how the gripper is oriented - prepose = object.local_transformer.transform_pose(oTg, "map") + def __iter__(self) -> ParkArmsActionPerformable: + """ + Iterates over all possible solutions and returns a performable designator with the arm. - # Perform the motion with the prepose and open gripper - World.current_world.add_vis_axis(prepose) - MoveTCPMotion(prepose, self.arm, allow_gripper_collision=True).perform() - MoveGripperMotion(motion=GripperState.OPEN, gripper=self.arm).perform() + :return: A performable designator_description + """ + for arm in self.arms: + yield ParkArmsActionPerformable(arm) - # Perform the motion with the adjusted pose -> actual grasp and close gripper - World.current_world.add_vis_axis(adjusted_oTm) - MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True).perform() - adjusted_oTm.pose.position.z += 0.03 - MoveGripperMotion(motion=GripperState.CLOSE, gripper=self.arm).perform() - tool_frame = RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame() - robot.attach(object, tool_frame) - # Lift object - World.current_world.add_vis_axis(adjusted_oTm) - MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True).perform() +class PickUpAction(ActionDesignatorDescription): + """ + Designator to let the robot pick up an object. + """ - # Remove the vis axis from the world - World.current_world.remove_vis_axis() + performable_class = PickUpActionPerformable - #TODO find a way to use object_at_execution instead of object_designator in the automatic orm mapping in ActionAbstract - def to_sql(self) -> Action: - return ORMPickUpAction(arm=self.arm, grasp=self.grasp) + def __init__(self, + object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], + arms: List[Arms] = None, grasps: List[Grasp] = None): + """ + Lets the robot pick up an object. The description needs an object designator_description 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. - def insert(self, session: Session, **kwargs) -> Action: - action = super(ActionAbstract, self).insert(session) - action.object = self.object_at_execution.insert(session) + :param object_designator_description: List of possible object designator_description + :param arms: List of possible arms that could be used + :param grasps: List of possible grasps for the object + """ + super().__init__() + self.object_designator_description: Union[ + ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description + self.arms: List[Arms] = arms + self.grasps: List[Grasp] = grasps + self.knowledge_condition = GraspableProperty(self.object_designator_description) & ReachableProperty( + self.object_designator_description.resolve().pose) - session.add(action) - return action + def __iter__(self) -> PickUpActionPerformable: + ri = ReasoningInstance(self, + PartialDesignator(PickUpActionPerformable, self.object_designator_description, self.arms, + self.grasps)) + # Here is where the magic happens + for desig in ri: + yield desig -@dataclass -class PlaceActionPerformable(ActionAbstract): +class PlaceAction(ActionDesignatorDescription): """ Places an Object at a position using an arm. """ - object_designator: ObjectDesignatorDescription.Object - """ - Object designator describing the object that should be place - """ - arm: Arms - """ - Arm that is currently holding the object - """ - target_location: Pose - """ - Pose in the world at which the object should be placed - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMPlaceAction) + performable_class = PlaceActionPerformable - @with_tree - def perform(self) -> None: - object_pose = self.object_designator.world_object.get_pose() - local_tf = LocalTransformer() + def __init__(self, + object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], + target_locations: List[Pose], + arms: List[Arms] = None): + """ + Create an Action Description to place an object - # Transformations such that the target position is the position of the object and not the tcp - tcp_to_object = local_tf.transform_pose(object_pose, - World.robot.get_link_tf_frame( - RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame())) - target_diff = self.target_location.to_transform("target").inverse_times( - tcp_to_object.to_transform("object")).to_pose() + :param object_designator_description: Description of object to place. + :param target_locations: List of possible positions/orientations to place the object + :param arms: List of possible arms to use + """ + super().__init__() + self.object_designator_description: Union[ + ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description + self.target_locations: List[Pose] = target_locations + self.arms: List[Arms] = arms + self.knowledge_condition = ReachableProperty(self.object_designator_description.resolve().pose) - MoveTCPMotion(target_diff, self.arm).perform() - MoveGripperMotion(GripperState.OPEN, self.arm).perform() - World.robot.detach(self.object_designator.world_object) - retract_pose = local_tf.transform_pose(target_diff, World.robot.get_link_tf_frame( - RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame())) - retract_pose.position.x -= 0.07 - MoveTCPMotion(retract_pose, self.arm).perform() + def ground(self) -> PlaceActionPerformable: + """ + Default specialized_designators that returns a performable designator_description with the first entries from the list of possible entries. -@dataclass -class NavigateActionPerformable(ActionAbstract): + :return: A performable designator_description + """ + obj_desig = self.object_designator_description if isinstance(self.object_designator_description, + ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() + + return PlaceActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) + + def __iter__(self) -> PlaceActionPerformable: + ri = ReasoningInstance(self, + PartialDesignator(PlaceActionPerformable, self.object_designator_description, self.arms, + self.target_locations)) + for desig in ri: + yield desig + + +class NavigateAction(ActionDesignatorDescription): """ Navigates the Robot to a position. """ - target_location: Pose - """ - Location to which the robot should be navigated - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMNavigateAction) + performable_class = NavigateActionPerformable - @with_tree - def perform(self) -> None: - MoveMotion(self.target_location).perform() + def __init__(self, target_locations: List[Pose]): + """ + Navigates the robot to a location. + :param target_locations: A list of possible target locations for the navigation. + """ + super().__init__() + self.target_locations: List[Pose] = target_locations + if len(self.target_locations) == 1: + self.knowledge_condition = SpaceIsFreeProperty(self.target_locations[0]) + else: + root = SpaceIsFreeProperty(self.target_locations[0]) + for location in self.target_locations[1:]: + root |= SpaceIsFreeProperty(location) + self.knowledge_condition = root -@dataclass -class TransportActionPerformable(ActionAbstract): - """ - Transports an object to a position using an arm - """ + def ground(self) -> NavigateActionPerformable: + """ + Default specialized_designators that returns a performable designator_description with the first entry of possible target locations. - object_designator: ObjectDesignatorDescription.Object - """ - Object designator describing the object that should be transported. - """ - arm: Arms - """ - Arm that should be used - """ - target_location: Pose - """ - Target Location to which the object should be transported - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMTransportAction) + :return: A performable designator_description + """ + return NavigateActionPerformable(self.target_locations[0]) - @with_tree - def perform(self) -> None: - robot_desig = BelieveObject(names=[RobotDescription.current_robot_description.name]) - ParkArmsActionPerformable(Arms.BOTH).perform() - pickup_loc = CostmapLocation(target=self.object_designator, reachable_for=robot_desig.resolve(), - reachable_arm=self.arm) - # Tries to find a pick-up position for the robot that uses the given arm - pickup_pose = None - for pose in pickup_loc: - if self.arm in pose.reachable_arms: - pickup_pose = pose - break - if not pickup_pose: - raise ObjectUnfetchable( - f"Found no pose for the robot to grasp the object: {self.object_designator} with arm: {self.arm}") + def __iter__(self) -> NavigateActionPerformable: + """ + Iterates over all possible target locations - NavigateActionPerformable(pickup_pose.pose).perform() - PickUpActionPerformable(self.object_designator, self.arm, Grasp.FRONT).perform() - ParkArmsActionPerformable(Arms.BOTH).perform() - try: - place_loc = CostmapLocation(target=self.target_location, reachable_for=robot_desig.resolve(), - reachable_arm=self.arm).resolve() - except StopIteration: - raise ReachabilityFailure( - f"No location found from where the robot can reach the target location: {self.target_location}") - NavigateActionPerformable(place_loc.pose).perform() - PlaceActionPerformable(self.object_designator, self.arm, self.target_location).perform() - ParkArmsActionPerformable(Arms.BOTH).perform() + :return: A performable designator_description + """ + for location in self.target_locations: + yield NavigateActionPerformable(location) -@dataclass -class LookAtActionPerformable(ActionAbstract): +class TransportAction(ActionDesignatorDescription): """ - Lets the robot look at a position. + Transports an object to a position using an arm """ - target: Pose - """ - Position at which the robot should look, given as 6D pose - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMLookAtAction) + performable_class = TransportActionPerformable - @with_tree - def perform(self) -> None: - LookingMotion(target=self.target).perform() + def __init__(self, + object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], + target_locations: List[Pose], + arms: List[Arms] = None): + """ + Designator representing a pick and place plan. + :param object_designator_description: Object designator_description description or a specified Object designator_description that should be transported + :param target_locations: A list of possible target locations for the object to be placed + :param arms: A List of possible arms that could be used for transporting + """ + super().__init__() + self.object_designator_description: Union[ + ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description + self.arms: List[Arms] = arms + self.target_locations: List[Pose] = target_locations -@dataclass -class DetectActionPerformable(ActionAbstract): - """ - Detects an object that fits the object description and returns an object designator describing the object. - """ - object_designator: ObjectDesignatorDescription.Object - """ - Object designator loosely describing the object, e.g. only type. - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMDetectAction) + def ground(self) -> TransportActionPerformable: + """ + Default specialized_designators that returns a performable designator_description with the first entries from the lists of possible parameter. - @with_tree - def perform(self) -> None: - return DetectingMotion(object_type=self.object_designator.obj_type).perform() + :return: A performable designator_description + """ + obj_desig = self.object_designator_description \ + if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) \ + else self.object_designator_description.resolve() + return TransportActionPerformable(obj_desig, self.target_locations[0], self.arms[0]) -@dataclass -class OpenActionPerformable(ActionAbstract): - """ - Opens a container like object - """ + def __iter__(self) -> TransportActionPerformable: + obj_desig = self.object_designator_description \ + if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) \ + else self.object_designator_description.resolve() + ri = ReasoningInstance(self, + PartialDesignator(TransportActionPerformable, obj_desig, self.target_locations, + self.arms)) + for desig in ri: + yield desig - object_designator: ObjectPart.Object - """ - Object designator describing the object that should be opened - """ - arm: Arms + +class LookAtAction(ActionDesignatorDescription): """ - Arm that should be used for opening the container + Lets the robot look at a position. """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMOpenAction) - @with_tree - def perform(self) -> None: - GraspingActionPerformable(self.arm, self.object_designator).perform() - OpeningMotion(self.object_designator, self.arm).perform() + performable_class = LookAtActionPerformable - MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform() + def __init__(self, targets: List[Pose]): + """ + 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 + """ + super().__init__() + self.targets: List[Pose] = targets -@dataclass -class CloseActionPerformable(ActionAbstract): - """ - Closes a container like object. - """ + def ground(self) -> LookAtActionPerformable: + """ + Default specialized_designators that returns a performable designator_description with the first entry in the list of possible targets - object_designator: ObjectPart.Object - """ - Object designator describing the object that should be closed - """ - arm: Arms - """ - Arm that should be used for closing - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMCloseAction) + :return: A performable designator_description + """ + return LookAtActionPerformable(self.targets[0]) - @with_tree - def perform(self) -> None: - GraspingActionPerformable(self.arm, self.object_designator).perform() - ClosingMotion(self.object_designator, self.arm).perform() + def __iter__(self) -> LookAtActionPerformable: + """ + Iterates over all possible target locations - MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform() + :return: A performable designator_description + """ + for target in self.targets: + yield LookAtActionPerformable(target) -@dataclass -class GraspingActionPerformable(ActionAbstract): - """ - Grasps an object described by the given Object Designator description - """ - arm: Arms - """ - The arm that should be used to grasp - """ - object_desig: Union[ObjectDesignatorDescription.Object, ObjectPart.Object] +class DetectAction(ActionDesignatorDescription): """ - Object Designator for the object that should be grasped + Detects an object that fits the object description and returns an object designator_description describing the object. """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMGraspingAction) - @with_tree - def perform(self) -> None: - if isinstance(self.object_desig, ObjectPart.Object): - object_pose = self.object_desig.part_pose - else: - object_pose = self.object_desig.world_object.get_pose() - lt = LocalTransformer() - gripper_name = RobotDescription.current_robot_description.get_arm_chain(self.arm).get_tool_frame() + performable_class = DetectActionPerformable - object_pose_in_gripper = lt.transform_pose(object_pose, - World.robot.get_link_tf_frame(gripper_name)) + def __init__(self, object_designator_description: ObjectDesignatorDescription, + ontology_concept_holders: Optional[List[Thing]] = None): + """ + Tries to detect an object in the field of view (FOV) of the robot. - pre_grasp = object_pose_in_gripper.copy() - pre_grasp.pose.position.x -= 0.1 + :param object_designator_description: Object designator_description describing the object + """ + super().__init__() + self.object_designator_description: ObjectDesignatorDescription = object_designator_description + self.knowledge_condition = VisibleProperty(self.object_designator_description) - MoveTCPMotion(pre_grasp, self.arm).perform() - MoveGripperMotion(GripperState.OPEN, self.arm).perform() - MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).perform() - MoveGripperMotion(GripperState.CLOSE, self.arm, allow_gripper_collision=True).perform() + def ground(self) -> DetectActionPerformable: + """ + Default specialized_designators that returns a performable designator_description with the executed object description. + :return: A performable designator_description + """ + return DetectActionPerformable(self.object_designator_description.resolve()) -@dataclass -class FaceAtPerformable(ActionAbstract): - """ - Turn the robot chassis such that is faces the ``pose`` and after that perform a look at action. - """ + def __iter__(self) -> DetectActionPerformable: + """ + Iterates over all possible values for this designator_description and returns a performable action designator_description with the value. - pose: Pose + :return: A performable action designator_description + """ + for desig in self.object_designator_description: + yield DetectActionPerformable(desig) + + +class OpenAction(ActionDesignatorDescription): """ - The pose to face + Opens a container like object + + Can currently not be used """ - orm_class = ORMFaceAtAction + performable_class = OpenActionPerformable - @with_tree - def perform(self) -> None: - # get the robot position - robot_position = World.robot.pose + def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None): + """ + Moves the arm of the robot to open a container. - # calculate orientation for robot to face the object - angle = np.arctan2(robot_position.position.y - self.pose.position.y, - robot_position.position.x - self.pose.position.x) + np.pi - orientation = list(transformations.quaternion_from_euler(0, 0, angle, axes="sxyz")) + :param object_designator_description: Object designator_description describing the handle that should be used to open + :param arms: A list of possible arms that should be used + """ + super().__init__() + self.object_designator_description: ObjectPart = object_designator_description + self.arms: List[Arms] = arms + self.knowledge_condition = GripperIsFreeProperty(self.arms) - # create new robot pose - new_robot_pose = Pose(robot_position.position_as_list(), orientation) + def ground(self) -> OpenActionPerformable: + """ + Default specialized_designators that returns a performable designator_description with the executed object description and the first entries + from the lists of possible parameter. - # turn robot - NavigateActionPerformable(new_robot_pose).perform() + :return: A performable designator_description + """ + return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0]) - # look at target - LookAtActionPerformable(self.pose).perform() + def __iter__(self) -> OpenActionPerformable: + """ + Iterates over all possible values for this designator_description and returns a performable action designator_description with the value. + :return: A performable action designator_description + """ + ri = ReasoningInstance(self, + PartialDesignator(OpenActionPerformable, self.object_designator_description, self.arms)) + for desig in ri: + yield desig -@dataclass -class MoveAndPickUpPerformable(ActionAbstract): - """ - Navigate to `standing_position`, then turn towards the object and pick it up. - """ - standing_position: Pose - """ - The pose to stand before trying to pick up the object +class CloseAction(ActionDesignatorDescription): """ + Closes a container like object. - object_designator: ObjectDesignatorDescription.Object - """ - The object to pick up + Can currently not be used """ - arm: Arms - """ - The arm to use - """ + performable_class = CloseActionPerformable - grasp: Grasp - """ - The grasp to use - """ + def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None): + """ + Attempts to close an open container - # @with_tree - def perform(self): - NavigateActionPerformable(self.standing_position).perform() - FaceAtPerformable(self.object_designator.pose).perform() - PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform() + :param object_designator_description: Object designator_description description of the handle that should be used + :param arms: A list of possible arms to use + """ + super().__init__() + self.object_designator_description: ObjectPart = object_designator_description + self.arms: List[Arms] = arms + self.knowledge_condition = GripperIsFreeProperty(self.arms) -@dataclass -class MoveAndPlacePerformable(ActionAbstract): - """ - Navigate to `standing_position`, then turn towards the object and pick it up. - """ + def ground(self) -> CloseActionPerformable: + """ + Default specialized_designators that returns a performable designator_description with the executed object designator_description and the first entry from + the list of possible arms. - standing_position: Pose - """ - The pose to stand before trying to pick up the object - """ + :return: A performable designator_description + """ + return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0]) - object_designator: ObjectDesignatorDescription.Object - """ - The object to pick up - """ + def __iter__(self) -> CloseActionPerformable: + """ + Iterates over all possible solutions for this designator_description and returns a performable action designator. - target_location: Pose - """ - The location to place the object. - """ + :yield: A performable fully parametrized Action designator + """ + ri = ReasoningInstance(self, + PartialDesignator(CloseActionPerformable, self.object_designator_description, self.arms)) + for desig in ri: + yield desig - arm: Arms + +class GraspingAction(ActionDesignatorDescription): """ - The arm to use + Grasps an object described by the given Object Designator description """ - @with_tree - def perform(self): - NavigateActionPerformable(self.standing_position).perform() - FaceAtPerformable(self.target_location).perform() - PlaceActionPerformable(self.object_designator, self.arm, self.target_location).perform() + performable_class = GraspingActionPerformable + + def __init__(self, object_description: Union[ObjectDesignatorDescription, ObjectPart], arms: List[Arms] = 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. + + :param arms: List of Arms that should be used for grasping + :param object_description: Description of the object that should be grasped + """ + super().__init__() + self.arms: List[Arms] = arms + self.object_description: ObjectDesignatorDescription = object_description + + + def ground(self) -> GraspingActionPerformable: + """ + Default specialized_designators that takes the first element from the list of arms and the first solution for the object + designator_description description ond returns it. + + :return: A performable action designator_description that contains specific arguments + """ + return GraspingActionPerformable(self.arms[0], self.object_description.resolve()) + + def __iter__(self) -> CloseActionPerformable: + """ + Iterates over all possible solutions for this designator_description and returns a performable action + designator. + + :yield: A fully parametrized Action designator + """ + ri = ReasoningInstance(self, + PartialDesignator(GraspingActionPerformable, self.object_description, self.arms)) + for desig in ri: + yield desig + diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index 22b266123..395a46c2f 100644 --- a/src/pycram/designators/location_designator.py +++ b/src/pycram/designators/location_designator.py @@ -24,14 +24,13 @@ class Location(LocationDesignatorDescription): class Location(LocationDesignatorDescription.Location): pass - def __init__(self, pose: Pose, resolver=None): + def __init__(self, pose: Pose): """ Basic location designator that represents a single pose. :param pose: The pose that should be represented by this location designator - :param resolver: An alternative specialized_designators that returns a resolved location """ - super().__init__(resolver) + super().__init__() self.pose: Pose = pose def ground(self) -> Location: @@ -60,16 +59,14 @@ class Location(LocationDesignatorDescription.Location): Object to which the pose is relative """ - def __init__(self, relative_pose: Pose = None, reference_object: ObjectDesignatorDescription = None, - resolver=None): + def __init__(self, relative_pose: Pose = None, reference_object: ObjectDesignatorDescription = None): """ Location designator representing a location relative to a given object. :param relative_pose: Pose that should be relative, in world coordinate frame :param reference_object: Object to which the pose should be relative - :param resolver: An alternative specialized_designators that returns a resolved location for the input parameter """ - super().__init__(resolver) + super().__init__() self.relative_pose: Pose = relative_pose self.reference_object: ObjectDesignatorDescription = reference_object @@ -115,7 +112,7 @@ class Location(LocationDesignatorDescription.Location): def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], reachable_for: Optional[ObjectDesignatorDescription.Object] = None, visible_for: Optional[ObjectDesignatorDescription.Object] = None, - reachable_arm: Optional[Arms] = None, resolver: Optional[Callable] = None): + reachable_arm: Optional[Arms] = None): """ Location designator that uses costmaps as base to calculate locations for complex constrains like reachable or visible. In case of reachable the resolved location contains a list of arms with which the location is reachable. @@ -124,9 +121,8 @@ def __init__(self, target: Union[Pose, ObjectDesignatorDescription.Object], :param reachable_for: Object for which the reachability should be calculated, usually a robot :param visible_for: Object for which the visibility should be calculated, usually a robot :param reachable_arm: An optional arm with which the target should be reached - :param resolver: An alternative specialized_designators that returns a resolved location for the given input of this description """ - super().__init__(resolver) + super().__init__() self.target: Union[Pose, ObjectDesignatorDescription.Object] = target self.reachable_for: ObjectDesignatorDescription.Object = reachable_for self.visible_for: ObjectDesignatorDescription.Object = visible_for @@ -213,16 +209,15 @@ class Location(LocationDesignatorDescription.Location): List of arms that can be used to for accessing from this pose """ - def __init__(self, handle_desig: ObjectPart.Object, robot_desig: ObjectDesignatorDescription.Object, resolver=None): + def __init__(self, handle_desig: ObjectPart.Object, robot_desig: ObjectDesignatorDescription.Object): """ Describes a position from where a drawer can be opened. For now this position should be calculated before the drawer will be opened. Calculating the pose while the drawer is open could lead to problems. :param handle_desig: ObjectPart designator for handle of the drawer :param robot_desig: Object designator for the robot which should open the drawer - :param resolver: An alternative specialized_designators to create the location """ - super().__init__(resolver) + super().__init__() self.handle: ObjectPart.Object = handle_desig self.robot: ObjectDesignatorDescription.Object = robot_desig.world_object @@ -299,7 +294,7 @@ class SemanticCostmapLocation(LocationDesignatorDescription): class Location(LocationDesignatorDescription.Location): pass - def __init__(self, urdf_link_name, part_of, for_object=None, resolver=None): + def __init__(self, urdf_link_name, part_of, for_object=None): """ Creates a distribution over a urdf link to sample poses which are on this link. Can be used, for example, to find poses that are on a table. Optionally an object can be given for which poses should be calculated, in that case @@ -308,9 +303,8 @@ def __init__(self, urdf_link_name, part_of, for_object=None, resolver=None): :param urdf_link_name: Name of the urdf link for which a distribution should be calculated :param part_of: Object of which the urdf link is a part :param for_object: Optional object that should be placed at the found location - :param resolver: An alternative specialized_designators that creates a resolved location for the input parameter of this description """ - super().__init__(resolver) + super().__init__() self.urdf_link_name: str = urdf_link_name self.part_of: ObjectDesignatorDescription.Object = part_of self.for_object: Optional[ObjectDesignatorDescription.Object] = for_object diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index c851d4bd8..54cf71817 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -97,17 +97,15 @@ def insert(self, session: sqlalchemy.orm.session.Session) -> ORMObjectPart: def __init__(self, names: List[str], part_of: ObjectDesignatorDescription.Object, - type: Optional[ObjectType] = None, - resolver: Optional[Callable] = None): + type: Optional[ObjectType] = None): """ Describing the relationship between an object and a specific part of it. :param names: Possible names for the part :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 """ - super().__init__(names, type, resolver) + super().__init__(names, type) if not part_of: raise AttributeError("part_of cannot be None.") @@ -154,7 +152,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): + reference_frames: List[str], timestamps: List[float]): """ Describing an object resolved through knowrob. @@ -162,9 +160,8 @@ def __init__(self, names: List[str], types: List[str], :param types: List of possible types describing the object :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. """ - super(LocatedObject, self).__init__(names, types, resolver) + super(LocatedObject, self).__init__(names, types) self.reference_frames: List[str] = reference_frames self.timestamps: List[float] = timestamps @@ -186,15 +183,14 @@ class Object(ObjectDesignatorDescription.Object): """ def __init__(self, names: Optional[List[str]] = None, types: Optional[List[str]] = None, - world_object: WorldObject = None, resolver: Optional[Callable] = None): + world_object: WorldObject = None): """ :param names: :param types: :param world_object: - :param resolver: """ - super().__init__(resolver) + super().__init__() self.types: Optional[List[str]] = types self.names: Optional[List[str]] = names self.world_object: WorldObject = world_object diff --git a/src/pycram/failures.py b/src/pycram/failures.py index 736adf8ee..effe849d4 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -15,6 +15,13 @@ def __init__(self, *args, **kwargs): Exception.__init__(self, *args, **kwargs) +class KnowledgeNotAvailable(PlanFailure): + """Thrown when a knowledge source can not provide the information for a query.""" + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + class NotALanguageExpression(PlanFailure): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -112,6 +119,13 @@ def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) +class GripperOccupied(GripperLowLevelFailure): + """Thrown when the gripper is occupied by some object.""" + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + class LookingHighLevelFailure(HighLevelFailure): """High-level failure produced when looking for an object, i.e. it is not a hardware issue but one relating to the looking task, its parameters, and how they relate to the environment.""" @@ -186,6 +200,13 @@ def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) +class ObjectNotVisible(HighLevelFailure): + """Thrown when the robot cannot see an object of a given description in its surroundings.""" + + def __init__(self, *args, **kwargs): + super().__init__(*args, **kwargs) + + class ObjectNowhereToBeFound(HighLevelFailure): """Thrown when the robot cannot find an object of a given description in its surroundings.""" @@ -399,12 +420,12 @@ def __init__(self, *args, **kwargs): class ReasoningError(PlanFailure): - def __init__(*args, **kwargs): + def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) class CollisionError(PlanFailure): - def __init__(*args, **kwargs): + def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) diff --git a/src/pycram/helper.py b/src/pycram/helper.py index fb2f78927..08ae416f7 100644 --- a/src/pycram/helper.py +++ b/src/pycram/helper.py @@ -7,7 +7,7 @@ from typing_extensions import Dict, Optional import xml.etree.ElementTree as ET -from pycram.ros.logging import logwarn +from .ros.logging import logwarn class Singleton(type): diff --git a/src/pycram/knowledge/__init__.py b/src/pycram/knowledge/__init__.py new file mode 100644 index 000000000..247581dee --- /dev/null +++ b/src/pycram/knowledge/__init__.py @@ -0,0 +1 @@ +from .knowledge_sources import * \ No newline at end of file diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py new file mode 100644 index 000000000..f0d4d6496 --- /dev/null +++ b/src/pycram/knowledge/knowledge_engine.py @@ -0,0 +1,243 @@ +from __future__ import annotations + +import inspect +from enum import Enum +from typing import _GenericAlias + +import rospy +from anytree import PreOrderIter +from typeguard import check_type, TypeCheckError + +from ..datastructures.partial_designator import PartialDesignator +from ..datastructures.property import Property, ResolvedProperty, PropertyOperator +from .knowledge_source import KnowledgeSource +from typing_extensions import Type, Callable, List, TYPE_CHECKING, Dict, Any + +from ..failures import KnowledgeNotAvailable, ReasoningError + +if TYPE_CHECKING: + from ..designator import ActionDesignatorDescription + + +class KnowledgeEngine: + """ + The knowledge engine is responsible for managing the knowledge sources and querying them to fill parameters of + designators + """ + + enabled = True + """ + Flag to enable or disable the knowledge engine + """ + + _instance = None + """ + Singleton for the knowledge engine + """ + + def __new__(cls): + if cls._instance is None: + cls._instance = super(KnowledgeEngine, cls).__new__(cls) + cls._instance._initialized = False + return cls._instance + + def __init__(self): + """ + Initialize the knowledge engine, this includes the collection of all knowledge sources and the initialization of + each source. + """ + if self._initialized: return + if not self.enabled: + rospy.logwarn("Knowledge engine is disabled") + return + self.knowledge_sources = [] + # Initialize all knowledge sources + self.knowledge_sources: List[KnowledgeSource] = [] + self.init_sources() + self._initialized: bool = True + + def init_sources(self): + """ + Initialize all knowledge sources from the available subclasses of KnowledgeSource + """ + # Class reference to all knowledge sources + sources = KnowledgeSource.__subclasses__() + for src in sources: + if src not in [c.__class__ for c in self.knowledge_sources]: + self.knowledge_sources.append(src()) + self.knowledge_sources.sort(key=lambda x: x.priority) + + def update_sources(self): + """ + Updates all knowledge sources, this will check if the sources are still available and if new sources have + become available. + """ + self.init_sources() + for source in self.knowledge_sources: + if source.is_connected and not source.is_available: + rospy.logwarn(f"Knowledge source {source.name} is not available anymore") + elif not source.is_connected and source.is_available: + source.connect() + + def query(self, designator: Type['ActionDesignatorDescription']) -> bool: + """ + Query to fill parameters of a designator_description from the knowledge sources + + :return: + """ + if not self.enabled: + rospy.logwarn("Knowledge engine is disabled") + return True + self.update_sources() + + condition = self.resolve_properties(designator.knowledge_condition) + return condition(designator) + + def resolve_properties(self, properties: Property): + """ + Traverses the tree of properties and resolves the property functions to the corresponding function in the knowledge + source. Properties are executed in-place. + + :param properties: Root node of the tree of properties + """ + if properties.resolved: + return properties.root + for child in PreOrderIter(properties): + if child.is_leaf: + source = self.find_source_for_property(child) + resolved_aspect_function = source.__getattribute__( + [fun for fun in child.__class__.__dict__.keys() if + not fun.startswith("__") and not fun == "property_exception"][0]) + + # child.resolved_property_instance = source + node = ResolvedProperty(resolved_aspect_function, child.property_exception, child.parent) + for param in inspect.signature(resolved_aspect_function).parameters.keys(): + node.parameter[param] = child.__getattribute__(param) + child.parent = None + + if isinstance(properties.root, PropertyOperator): + return properties.root + else: + return node + + def update(self): + """ + Update the knowledge sources with new information contained in a designator_description + + :return: + """ + if not self.enabled: + rospy.logwarn("Knowledge engine is disabled") + return True + + def ground_solution(self, designator: Type['DesignatorDescription']) -> bool: + """ + Try to ground a solution from the knowledge sources in the belief state + + :return: True if the solution achieves the desired goal, False otherwise + """ + if not self.enabled: + rospy.logwarn("Knowledge engine is disabled") + return True + + def find_source_for_property(self, property: Type[Property]) -> KnowledgeSource: + """ + Find the source for the given property + + :param property: The property for which to find the source. + :return: Source that can provide the property. + """ + for source in self.knowledge_sources: + if (property.__class__ in list(source.__class__.__bases__) + and source.is_connected): + return source + + def match_reasoned_parameter(self, reasoned_parameter: Dict[str, any], + designator: ActionDesignatorDescription) -> Dict[str, any]: + """ + Match the reasoned parameters, in the root node of the property expression, to the corresponding parameter in + the designator_description + """ + matched_parameter = {} + matched_parameter.update(self._match_by_name(reasoned_parameter, designator)) + matched_parameter.update(self._match_by_type(reasoned_parameter, designator)) + return matched_parameter + + @staticmethod + def _match_by_name(parameter: Dict[str, any], designator: ActionDesignatorDescription) -> Dict[str, any]: + """ + Match the reasoned parameters to the corresponding parameter in the designator_description by name + """ + result_dict = {} + for key, value in parameter.items(): + # if key in designator_description.get_optional_parameter() and designator_description.__getattribute__(key) is None: + if key in designator.performable_class.get_type_hints().keys(): + result_dict[key] = value + return result_dict + + @staticmethod + def _match_by_type(parameter: Dict[str, any], designator: ActionDesignatorDescription) -> Dict[str, any]: + """ + Match the reasoned parameters to the corresponding parameter in the designator_description by type + """ + result_dict = {} + for key, value in parameter.items(): + for parameter_name, type_hint in designator.performable_class.get_type_hints().items(): + try: + # Distinction between Enum and other types, since check_type would check Enums and floats as an Enum + # is technically just a number. Also excludes type hints, since they do not work with issubclass + if issubclass(value.__class__, Enum) and not issubclass(type_hint.__class__, _GenericAlias): + if not issubclass(value.__class__, type_hint): + raise TypeCheckError(f"Expected type {type_hint} but got {value.__class__}") + else: + check_type(value, type_hint) + + result_dict[parameter_name] = value + except TypeCheckError as e: + continue + return result_dict + +class ReasoningInstance: + """ + A reasoning instance is a generator class that reasons about the missing parameter as well as the feasibility of + the designator_description given. + Since this is a generator it keeps its state while yielding full designator and can be used to generate the next + full designator at a later time. + """ + + def __init__(self, designator_description: ActionDesignatorDescription, partial_designator: PartialDesignator): + """ + Initialize the reasoning instance with the designator_description and the partial designator + + :param designator_description: The description from which the designator should be created + :param partial_designator: A partial initialized designator_description using the PartialDesignator class + """ + self.designator_description = designator_description + self.knowledge_engine = KnowledgeEngine() + self.resolved_property = self.knowledge_engine.resolve_properties(self.designator_description.knowledge_condition) + self.parameter_to_be_reasoned = [param_name for param_name in self.designator_description.get_optional_parameter() if + self.designator_description.__getattribute__(param_name) is None] + self.partial_designator = partial_designator + + def __iter__(self) -> ActionDesignatorDescription.Action: + """ + Executes property structure, matches the reasoned and missing parameter and generates a completes designator. + + :yield: A complete designator with all parameters filled + """ + self.resolved_property(self.designator_description) + + matched_parameter = self.knowledge_engine.match_reasoned_parameter(self.resolved_property.root.variables, self.designator_description) + # Check if the parameter that need to be filled are contained in the set of reasoned parameters + if not set(matched_parameter).issuperset(set(self.partial_designator.missing_parameter())): + not_reasoned = set(self.partial_designator.missing_parameter()).difference(set(matched_parameter)) + raise ReasoningError(f"The parameters {not_reasoned} could not be inferred from the knowledge sources. Therefore, a complete designator can not be generated. ") + + for key, value in self.partial_designator.kwargs.items(): + # This line means the parameter of the designator description will be preferred over the reasoned parameter + if value is None: + self.partial_designator.kwargs[key] = matched_parameter[key] + + + for designator in self.partial_designator(**matched_parameter): + yield designator diff --git a/src/pycram/knowledge/knowledge_source.py b/src/pycram/knowledge/knowledge_source.py new file mode 100644 index 000000000..f72adfe82 --- /dev/null +++ b/src/pycram/knowledge/knowledge_source.py @@ -0,0 +1,88 @@ +from __future__ import annotations + +from abc import ABC, abstractmethod + +from typing_extensions import TYPE_CHECKING +from ..failures import KnowledgeNotAvailable + +if TYPE_CHECKING: + from ..designator import DesignatorDescription + +class KnowledgeSource(ABC): + """ + Base class for all knowledge sources, knowledge sources provide information for specific use cases which the + robot might encounter during execution. + """ + + def __init__(self, name: str, priority: int): + """ + + :param name: Name of this knowledge source + :param priority: Priority by which this knowledge source is queried + """ + self.name = name + self.priority = priority + + @property + @abstractmethod + def is_available(self) -> bool: + """ + Check if the knowledge source is available + + :return: True if the knowledge source is available and can be queried + """ + raise NotImplementedError + + @property + @abstractmethod + def is_connected(self) -> bool: + """ + Check if the knowledge source is connected + + :return: True if the knowledge source is connected + """ + raise NotImplementedError + + @abstractmethod + def connect(self) -> bool: + """ + Connect to the knowledge source + """ + raise NotImplementedError + + @abstractmethod + def clear_state(self) -> None: + """ + Clear the state of the knowledge source + """ + raise NotImplementedError + + def __str__(self): + return f"{self.name} - Priority:{self.priority}" + + +class QueryKnowledge: + def query_pose_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: + """ + Query the pose for an object from the knowledge source + + :param designator: Designator for the object + :return: Designator with the pose information + :raises KnowledgeNotAvailable: If the pose for the object is not available in this knowledge source + """ + pass + + def query_grasp_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: + """ + Query the grasp for an object from the knowledge source + + :param designator: Designator for the object + :return: Designator with the grasp information + :raises KnowledgeNotAvailable: If the grasp for the object is not available in this knowledge source + """ + raise KnowledgeNotAvailable(f"Grasp for object {designator} not available in {self.name}") + + +class UpdateKnowledge: + pass + diff --git a/src/pycram/knowledge/knowledge_sources/__init__.py b/src/pycram/knowledge/knowledge_sources/__init__.py new file mode 100644 index 000000000..0a51bd684 --- /dev/null +++ b/src/pycram/knowledge/knowledge_sources/__init__.py @@ -0,0 +1,7 @@ +from os.path import dirname, basename, isfile, join +import glob + +modules = glob.glob(join(dirname(__file__), "*.py")) +__all__ = [basename(f)[:-3] for f in modules if isfile(f) and not f.endswith('__init__.py')] + +from . import * diff --git a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py new file mode 100644 index 000000000..d6b0594da --- /dev/null +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -0,0 +1,135 @@ +from __future__ import annotations + +from typing import TYPE_CHECKING + +import numpy as np +from typing_extensions import List + +from ...datastructures.dataclasses import ReasoningResult +from ...datastructures.enums import Arms, ObjectType, Grasp +from ..knowledge_source import KnowledgeSource +from ...datastructures.property import ReachableProperty, GraspableProperty, GripperIsFreeProperty, VisibleProperty, \ + SpaceIsFreeProperty, EmptyProperty +from ...datastructures.pose import Pose +from ...datastructures.world import World, UseProspectionWorld +from ...pose_generator_and_validator import PoseGenerator, reachability_validator +from ...robot_description import RobotDescription +from ...world_reasoning import visible +from ...costmaps import OccupancyCostmap, GaussianCostmap + +if TYPE_CHECKING: + from ...designators.object_designator import ObjectDesignatorDescription + +class FactsKnowledge(KnowledgeSource, GripperIsFreeProperty, VisibleProperty, SpaceIsFreeProperty, GraspableProperty, ReachableProperty, EmptyProperty): + """ + Knowledge source for hard coded facts, this knowledge source acts as a fallback if no other knowledge source is + available. + """ + + def __init__(self): + super().__init__(name="Facts", priority=99) + + @property + def is_available(self) -> bool: + return True + + @property + def is_connected(self) -> bool: + return True + + def connect(self): + pass + + def clear_state(self) -> None: + pass + + def reachable(self, pose: Pose) -> ReasoningResult: + """ + Check if a given pose is reachable by the robot. Simplified version of the CostmapLocation which can't be used + here due to cyclic imports. + + :param pose: Pose which should be checked + :return: A ReasoningResult with the result of the check and possible arms + """ + # ground_pose = Pose(pose.position_as_list()) + # ground_pose.position.z = 0 + # occupancy = OccupancyCostmap(0.32, False, 200, 0.02, ground_pose) + # gaussian = GaussianCostmap(200, 15, 0.02, ground_pose) + # final_map = occupancy + gaussian + # + # with UseProspectionWorld(): + # test_robot = World.current_world.get_prospection_object_for_object(World.robot) + # for maybe_pose in PoseGenerator(final_map, number_of_samples=200): + # hand_links = [] + # for description in RobotDescription.current_robot_description.get_manipulator_chains(): + # hand_links += description.end_effector.links + # valid, arms = reachability_validator(maybe_pose, test_robot, pose, + # allowed_collision={test_robot: hand_links}) + # if valid: + # return ReasoningResult(True, {"arm": arms}) + # + # return ReasoningResult(False) + return ReasoningResult(True) + + def graspable(self, object_designator: ObjectDesignatorDescription) -> ReasoningResult: + """ + Check if the object is graspable by the robot. + + :param object_designator: Designator of the object which should be grasped + :return: Reasoning Result with the result and a possible grasp + """ + with UseProspectionWorld(): + pro_obj = World.current_world.get_prospection_object_for_object(object_designator.resolve().world_object) + pro_obj.set_pose(Pose([0, 0, 0], [0, 0, 0, 1])) + bounding_box = pro_obj.get_axis_aligned_bounding_box() + + obj_x = bounding_box.max_x - bounding_box.min_x + obj_y = bounding_box.max_y - bounding_box.min_y + obj_z = bounding_box.max_z - bounding_box.min_z + gripper_opening_dists = [ee.end_effector.opening_distance for ee in + RobotDescription.current_robot_description.get_manipulator_chains()] + + for dist in gripper_opening_dists: + if dist > obj_y: + return ReasoningResult(True, {"grasp": Grasp.FRONT}) + elif dist > obj_x: + return ReasoningResult(True, {"grasp": Grasp.LEFT}) + + return ReasoningResult(False) + + def space_is_free(self, pose: Pose) -> ReasoningResult: + om = OccupancyCostmap(0.35, False, 200, 0.02, pose) + origin_map = om.map[200 // 2 - 10: 200 // 2 + 10, 200 // 2 - 10: 200 // 2 + 10] + return ReasoningResult(np.sum(origin_map) > 400 * 0.9) + + def gripper_is_free(self, grippers: List[Arms]) -> ReasoningResult: + """ + Checks for a list of grippers if they are holding something. + + :param grippers: A list of gripper that should be checked + :return: Result if a gripper is free and the resulting gripper + """ + for gripper in grippers: + tool_frame_link = RobotDescription.current_robot_description.get_arm_chain(gripper).get_tool_frame() + for att in World.robot.attachments.values(): + if att.parent_link == tool_frame_link or att.child_link == tool_frame_link: + return ReasoningResult(False) + return ReasoningResult(True, {"gripper": gripper}) + + def is_visible(self, object_designator: ObjectDesignatorDescription) -> ReasoningResult: + """ + Checks if an object is visible for the robot. + + :param object_designator: The object in question + :return: Reasoning result with the visibility of the object + """ + cam_pose = World.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) + return ReasoningResult(visible(object_designator.resolve().world_object, cam_pose)) + + def empty(self) -> ReasoningResult: + """ + Default property, which is always true. + + :return: Reasoning result with True + """ + return ReasoningResult(True) diff --git a/src/pycram/language.py b/src/pycram/language.py index 8ed8e17a2..983ff8e18 100644 --- a/src/pycram/language.py +++ b/src/pycram/language.py @@ -43,7 +43,7 @@ def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = Non def resolve(self) -> Language: """ - Dummy method for compatability to designator descriptions + Dummy method for compatability to designator_description descriptions :return: self reference """ @@ -60,7 +60,7 @@ def __add__(self, other: Language) -> Sequential: """ Language expression for sequential execution. - :param other: Another Language expression, either a designator or language expression + :param other: Another Language expression, either a designator_description or language expression :return: A :func:`~Sequential` object which is the new root node of the language tree """ if not issubclass(other.__class__, Language): @@ -72,7 +72,7 @@ def __sub__(self, other: Language) -> TryInOrder: """ Language expression for try in order. - :param other: Another Language expression, either a designator or language expression + :param other: Another Language expression, either a designator_description or language expression :return: A :func:`~TryInOrder` object which is the new root node of the language tree """ if not issubclass(other.__class__, Language): @@ -84,7 +84,7 @@ def __or__(self, other: Language) -> Parallel: """ Language expression for parallel execution. - :param other: Another Language expression, either a designator or language expression + :param other: Another Language expression, either a designator_description or language expression :return: A :func:`~Parallel` object which is the new root node of the language tree """ if not issubclass(other.__class__, Language): @@ -100,7 +100,7 @@ def __xor__(self, other: Language) -> TryAll: """ Language expression for try all execution. - :param other: Another Language expression, either a designator or language expression + :param other: Another Language expression, either a designator_description or language expression :return: A :func:`~TryAll` object which is the new root node of the language tree """ if not issubclass(other.__class__, Language): diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index dc3eabab7..957d0f5e9 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -49,11 +49,11 @@ def _execute(self, designator: BaseMotion) -> Any: def execute(self, designator: BaseMotion) -> Any: """ - Execute the given designator. If there is already another process module of the same kind the `self._lock` will + Execute the given designator_description. If there is already another process module of the same kind the `self._lock` will lock this thread until the execution of that process module is finished. This implicitly queues the execution of process modules. - :param designator: The designator to execute. + :param designator: The designator_description to execute. :return: Return of the Process Module if there is any """ if threading.get_ident() in Language.block_list: diff --git a/src/pycram/process_modules/pr2_process_modules.py b/src/pycram/process_modules/pr2_process_modules.py index 099662bf9..29898ba97 100644 --- a/src/pycram/process_modules/pr2_process_modules.py +++ b/src/pycram/process_modules/pr2_process_modules.py @@ -1,5 +1,5 @@ from threading import Lock -from typing_extensions import Any +from typing_extensions import Any, TYPE_CHECKING import actionlib @@ -11,7 +11,7 @@ from ..ros.logging import logdebug from ..utils import _apply_ik from ..local_transformer import LocalTransformer -from ..designators.object_designator import ObjectDesignatorDescription + from ..designators.motion_designator import MoveMotion, LookingMotion, \ DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ MoveGripperMotion, OpeningMotion, ClosingMotion @@ -23,6 +23,9 @@ from ..external_interfaces import giskard from ..external_interfaces.robokudo import * +if TYPE_CHECKING: + from ..designators.object_designator import ObjectDesignatorDescription + try: from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction, Pr2 except ImportError: diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 6be44c825..ec5741179 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -580,8 +580,8 @@ def base_origin_shift(self) -> np.ndarray: def __repr__(self): skip_attr = ["links", "joints", "description", "attachments"] - return self.__class__.__qualname__ + f"(" + ', \n'.join( - [f"{key}={value}" if key not in skip_attr else f"{key}: ..." for key, value in self.__dict__.items()]) + ")" + return self.__class__.__qualname__ + f"(name={self.name}, object_type={self.obj_type.name}, file_path={self.path}, pose={self.pose}, world={self.world})" + def remove(self) -> None: """ diff --git a/test/knowledge_testcase.py b/test/knowledge_testcase.py new file mode 100644 index 000000000..e6f191e16 --- /dev/null +++ b/test/knowledge_testcase.py @@ -0,0 +1,68 @@ +import unittest +from abc import abstractmethod + +from pycram.testing import BulletWorldTestCase +from pycram.datastructures.dataclasses import ReasoningResult +from pycram.datastructures.pose import Pose +from pycram.datastructures.property import Property +from pycram.failures import ReasoningError +from pycram.knowledge.knowledge_source import KnowledgeSource +from pycram.knowledge.knowledge_engine import KnowledgeEngine + + +class TestProperty(Property): + """ + A Mock Property for testing the property implementation + """ + property_exception = ReasoningError + + def __init__(self, pose: Pose): + super().__init__(None, None) + self.pose = pose + + @abstractmethod + def test(self, pose: Pose) -> ReasoningResult: + raise NotImplementedError + + +class TestKnowledge(KnowledgeSource, TestProperty): + """ + A Mock KnowledgeSource for testing the knowledge_source implementation + """ + def __init__(self): + super().__init__("test_source", 1) + self.parameter = {} + + def is_available(self): + return True + + def is_connected(self): + return True + + def connect(self): + pass + + def clear_state(self): + self.parameter = {} + + def test(self, pose: Pose) -> ReasoningResult: + return ReasoningResult(Pose([1, 2, 3]).dist(pose) < 0.1, {pose.frame: pose}) + + +class KnowledgeSourceTestCase(unittest.TestCase): + + @classmethod + def setUpClass(cls): + cls.knowledge_engine = KnowledgeEngine() + cls.knowledge_source = list(filter(lambda x: type(x) == TestKnowledge, cls.knowledge_engine.knowledge_sources))[ + 0] + + +class KnowledgeBulletTestCase(BulletWorldTestCase): + + @classmethod + def setUpClass(cls): + super().setUpClass() + cls.knowledge_engine = KnowledgeEngine() + cls.knowledge_source = list(filter(lambda x: type(x) == TestKnowledge, cls.knowledge_engine.knowledge_sources))[ + 0] diff --git a/test/test_designator/test_action_designator.py b/test/test_designator/test_action_designator.py index 6b2b8c556..764c82ba8 100644 --- a/test/test_designator/test_action_designator.py +++ b/test/test_designator/test_action_designator.py @@ -37,13 +37,13 @@ def test_set_gripper(self): def test_release(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) - description = action_designator.ReleaseAction([Arms.LEFT], object_description) + description = action_designator.ReleaseAction(object_description, [Arms.LEFT]) self.assertEqual(description.ground().gripper, Arms.LEFT) self.assertEqual(description.ground().object_designator.name, "milk") def test_grip(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) - description = action_designator.GripAction([Arms.LEFT], object_description, [0.5]) + description = action_designator.GripAction(object_description, [Arms.LEFT], [0.5]) self.assertEqual(description.ground().gripper, Arms.LEFT) self.assertEqual(description.ground().object_designator.name, "milk") @@ -59,11 +59,11 @@ def test_park_arms(self): self.assertEqual(self.world.robot.get_joint_position(joint), pose) def test_navigate(self): - description = action_designator.NavigateAction([Pose([1, 0, 0], [0, 0, 0, 1])]) + description = action_designator.NavigateAction([Pose([0.3, 0, 0], [0, 0, 0, 1])]) with simulated_robot: description.resolve().perform() - self.assertEqual(description.ground().target_location, Pose([1, 0, 0], [0, 0, 0, 1])) - self.assertEqual(self.robot.get_pose(), Pose([1, 0, 0])) + self.assertEqual(description.ground().target_location, Pose([0.3, 0, 0], [0, 0, 0, 1])) + self.assertEqual(self.robot.get_pose(), Pose([0.3, 0, 0])) def test_pick_up(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) @@ -121,9 +121,9 @@ def test_close(self): def test_transport(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) description = action_designator.TransportAction(object_description, - [Arms.LEFT], [Pose([-1.35, 0.78, 0.95], - [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])]) + [0.0, 0.0, 0.16439898301071468, 0.9863939245479175])], + [Arms.LEFT]) with simulated_robot: action_designator.MoveTorsoAction([0.2]).resolve().perform() description.resolve().perform() diff --git a/test/test_knowledge.py b/test/test_knowledge.py new file mode 100644 index 000000000..bba8849ff --- /dev/null +++ b/test/test_knowledge.py @@ -0,0 +1,158 @@ +from pycram.testing import BulletWorldTestCase +from knowledge_testcase import KnowledgeSourceTestCase, TestProperty, KnowledgeBulletTestCase +from pycram.datastructures.enums import Arms, Grasp, ObjectType +from pycram.datastructures.partial_designator import PartialDesignator +from pycram.datastructures.pose import Pose +from pycram.designators.action_designator import PickUpAction, PickUpActionPerformable, OpenAction +from pycram.designators.object_designator import BelieveObject, ObjectPart +from pycram.knowledge.knowledge_engine import KnowledgeEngine +from pycram.process_modules.pr2_process_modules import Pr2MoveArmJoints + + +class TestKnowledgeSource(KnowledgeSourceTestCase): + def test_knowledge_source_construct(self): + self.assertEqual(self.knowledge_source.name, "test_source") + self.assertEqual(self.knowledge_source.priority, 1) + + def test_available(self): + self.assertTrue(self.knowledge_source.is_available) + + def test_connected(self): + self.assertTrue(self.knowledge_source.is_connected) + + def test_init(self): + self.assertTrue(type(self.knowledge_source) in + [type(self.knowledge_source) for s in self.knowledge_engine.knowledge_sources]) + + +class TestKnowledgeEngine(KnowledgeSourceTestCase): + def test_singleton(self): + self.assertEqual(self.knowledge_engine, KnowledgeEngine()) + + def test_init(self): + self.assertTrue(self.knowledge_engine._initialized) + + def test_init_sources(self): + self.assertTrue(len(self.knowledge_engine.knowledge_sources) > 0) + + def test_update_sources(self): + self.knowledge_engine.update_sources() + self.assertTrue(self.knowledge_source.is_connected) + + def test_find_source(self): + prop = TestProperty(Pose([1, 2, 3])) + self.assertEqual(self.knowledge_engine.find_source_for_property(prop), self.knowledge_source) + + +class TestKnowledgeEngineBeliefState(KnowledgeBulletTestCase): + def test_match_by_name(self): + params = {"arm": 1, "leg": "left"} + desig = PickUpAction(BelieveObject(names=["milk"])) + matched = self.knowledge_engine._match_by_name(params, desig) + self.assertEqual({"arm": 1}, matched) + + def test_match_by_name_no_match(self): + params = {"test": 1, "leg": "left"} + desig = PickUpAction(BelieveObject(names=["milk"])) + matched = self.knowledge_engine._match_by_name(params, desig) + self.assertEqual({}, matched) + + def test_match_by_type(self): + params = {"test": Arms.RIGHT, "leg": "left"} + desig = PickUpAction(BelieveObject(names=["milk"])) + matched = self.knowledge_engine._match_by_type(params, desig) + self.assertEqual({"arm": Arms.RIGHT}, matched) + + def test_match_by_type_no_match(self): + params = {"test": {"test": 1}, "leg": "left"} + desig = PickUpAction(BelieveObject(names=["milk"])) + matched = self.knowledge_engine._match_by_type(params, desig) + self.assertEqual({}, matched) + + def test_match_reasoned_parameter(self): + params = {"arm": Arms.RIGHT, "leg": "left"} + desig = PickUpAction(BelieveObject(names=["milk"])) + matched = self.knowledge_engine.match_reasoned_parameter(params, desig) + self.assertEqual({"arm": Arms.RIGHT}, matched) + + def test_match_reasoned_parameter_full(self): + params = {"arm": Arms.RIGHT, "gasp": Grasp.FRONT} + desig = PickUpAction(BelieveObject(names=["milk"])) + matched = self.knowledge_engine.match_reasoned_parameter(params, desig) + self.assertEqual({"arm": Arms.RIGHT, "grasp": Grasp.FRONT}, matched) + + +class TestPartialDesignator(KnowledgeBulletTestCase): + def test_partial_desig_construction(self): + test_object = BelieveObject(names=["milk"]) + partial_desig = PartialDesignator(PickUpActionPerformable, test_object, arm=Arms.RIGHT) + self.assertEqual(partial_desig.performable, PickUpActionPerformable) + self.assertEqual(partial_desig.kwargs, {"arm": Arms.RIGHT, "object_designator": test_object, "grasp": None}) + + def test_partial_desig_construction_none(self): + partial_desig = PartialDesignator(PickUpActionPerformable, None, arm=Arms.RIGHT) + self.assertEqual(partial_desig.performable, PickUpActionPerformable) + self.assertEqual(partial_desig.kwargs, {"arm": Arms.RIGHT, "object_designator": None, "grasp": None}) + + def test_partial_desig_call(self): + partial_desig = PartialDesignator(PickUpActionPerformable, None, arm=Arms.RIGHT) + new_partial_desig = partial_desig(grasp=Grasp.FRONT) + self.assertEqual(new_partial_desig.performable, PickUpActionPerformable) + self.assertEqual({"arm": Arms.RIGHT, "grasp": Grasp.FRONT, "object_designator": None}, new_partial_desig.kwargs) + + def test_partial_desig_missing_params(self): + partial_desig = PartialDesignator(PickUpActionPerformable, None, arm=Arms.RIGHT) + missing_params = partial_desig.missing_parameter() + self.assertTrue("object_designator" in missing_params and "grasp" in missing_params) + + new_partial = partial_desig(grasp=Grasp.FRONT) + missing_params = new_partial.missing_parameter() + self.assertEqual(["object_designator"], missing_params) + + def test_is_iterable(self): + self.assertTrue(PartialDesignator._is_iterable([1, 2, 3])) + self.assertFalse(PartialDesignator._is_iterable(1)) + + def test_partial_desig_permutations(self): + l1 = [1, 2] + l2 = [Arms.RIGHT, Arms.LEFT] + permutations = PartialDesignator.generate_permutations([l1, l2]) + self.assertEqual([(1, Arms.RIGHT), (1, Arms.LEFT), (2, Arms.RIGHT), (2, Arms.LEFT)], list(permutations)) + + def test_partial_desig_iter(self): + test_object = BelieveObject(names=["milk"]) + test_object_resolved = test_object.resolve() + partial_desig = PartialDesignator(PickUpActionPerformable, test_object, arm=[Arms.RIGHT, Arms.LEFT]) + performables = list(partial_desig(grasp=[Grasp.FRONT, Grasp.TOP])) + self.assertEqual(4, len(performables)) + self.assertTrue(all([isinstance(p, PickUpActionPerformable) for p in performables])) + self.assertEqual([p.arm for p in performables], [Arms.RIGHT, Arms.RIGHT, Arms.LEFT, Arms.LEFT]) + self.assertEqual([p.grasp for p in performables], [Grasp.FRONT, Grasp.TOP, Grasp.FRONT, Grasp.TOP]) + self.assertEqual([p.object_designator for p in performables], [test_object_resolved] * 4) + + +class TestParameterInference(KnowledgeBulletTestCase): + def test_pickup_arm(self): + test_object = BelieveObject(names=["milk"]) + partial_desig = PickUpAction(test_object, [Arms.RIGHT]) + desig = partial_desig.resolve() + self.assertEqual(desig.grasp, Grasp.FRONT) + + def test_pickup_grasp(self): + test_object = BelieveObject(names=["milk"]) + partial_desig = PickUpAction(test_object, [Arms.RIGHT]) + desig = partial_desig.resolve() + self.assertEqual(desig.grasp, Grasp.FRONT) + + def test_open_gripper(self): + self.robot.set_pose(Pose([-0.192, 1.999, 0], [0, 0, 0.8999, -0.437])) + self.robot.set_joint_position("torso_lift_joint", 0.3) + env_object = BelieveObject(names=["kitchen"]).resolve() + handle_desig = ObjectPart(["kitchen_island_middle_upper_drawer_handle"], env_object) + partial_desig = OpenAction(handle_desig, [Arms.RIGHT]) + desig = partial_desig.resolve() + self.assertEqual(desig.arm, Arms.RIGHT) + + +class TestReasoningInstance(KnowledgeSourceTestCase): + pass diff --git a/test/test_language.py b/test/test_language.py index 7771637f4..4db6ffc57 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -214,14 +214,14 @@ def test_repeat_construction_error(self): self.assertRaises(AttributeError, lambda: (act + act2) * park) def test_perform_desig(self): - act = NavigateAction([Pose([1, 1, 0])]) + act = NavigateAction([Pose([0.3, 0.3, 0])]) act2 = MoveTorsoAction([0.3]) act3 = ParkArmsAction([Arms.BOTH]) plan = act + act2 + act3 with simulated_robot: plan.perform() - self.assertEqual(self.robot.get_pose(), Pose([1, 1, 0])) + self.assertEqual(self.robot.get_pose(), Pose([0.3, 0.3, 0])) self.assertEqual(self.robot.get_joint_position("torso_lift_joint"), 0.3) for joint, pose in RobotDescription.current_robot_description.get_static_joint_chain("right", "park").items(): self.assertEqual(self.world.robot.get_joint_position(joint), pose) @@ -232,14 +232,14 @@ def test_perform_code(self): def test_set(param): self.assertTrue(param) - act = NavigateAction([Pose([1, 1, 0])]) + act = NavigateAction([Pose([0.3, 0.3, 0])]) act2 = Code(test_set, {"param": True}) act3 = Code(test_set, {"param": True}) plan = act + act2 + act3 with simulated_robot: plan.perform() - self.assertEqual(self.robot.get_pose(), Pose([1, 1, 0])) + self.assertEqual(self.robot.get_pose(), Pose([0.3, 0.3, 0])) def test_perform_parallel(self): diff --git a/test/test_orm/test_orm.py b/test/test_orm/test_orm.py index 3c5ef6af5..bdda35afe 100644 --- a/test/test_orm/test_orm.py +++ b/test/test_orm/test_orm.py @@ -221,8 +221,7 @@ def test_parkArmsAction(self): def test_transportAction(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) - action = TransportActionPerformable(object_description.resolve(), Arms.LEFT, - Pose([1.3, 0.9, 0.9], [0, 0, 0, 1])) + action = TransportActionPerformable(object_description.resolve(), Pose([1.3, 0.9, 0.9], [0, 0, 0, 1]), Arms.LEFT) with simulated_robot: action.perform() pycram.orm.base.ProcessMetaData().description = "transportAction_test" @@ -343,7 +342,7 @@ def test_believe_object(self): LookAtAction(targets=[Pose([1, -1.78, 0.55])]).resolve().perform() object_desig = DetectAction(BelieveObject(types=[Milk])).resolve().perform() - TransportAction(object_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform() + TransportAction(object_desig, [Pose([4.8, 3.55, 0.8])], [Arms.LEFT]).resolve().perform() ParkArmsAction([Arms.BOTH]).resolve().perform() pycram.orm.base.ProcessMetaData().description = "BelieveObject_test" diff --git a/test/test_property.py b/test/test_property.py new file mode 100644 index 000000000..4ee54255f --- /dev/null +++ b/test/test_property.py @@ -0,0 +1,123 @@ +from pycram.datastructures.property import Property, And, Or, Not, ResolvedProperty +from pycram.datastructures.pose import Pose +from knowledge_testcase import TestProperty, KnowledgeSourceTestCase +from pycram.failures import ReasoningError + + +class TestCaseProperty(KnowledgeSourceTestCase): + def test_and_construct(self): + prop_1 = TestProperty(Pose([1, 2, 3])) + prop_2 = TestProperty(Pose([1, 2, 3])) + prop_3 = prop_1 & prop_2 + self.assertEqual(type(prop_3), And) + self.assertEqual(prop_3.children, (prop_1, prop_2)) + + def test_or_construct(self): + prop_1 = TestProperty(Pose([1, 2, 3])) + prop_2 = TestProperty(Pose([1, 2, 3])) + prop_3 = prop_1 | prop_2 + self.assertEqual(type(prop_3), Or) + self.assertEqual(prop_3.children, (prop_1, prop_2)) + + def test_not_construct(self): + prop_1 = TestProperty(Pose([1, 2, 3])) + prop_2 = ~prop_1 + self.assertEqual(type(prop_2), Not) + self.assertEqual(prop_2.children, (prop_1,)) + + def test_property_resolve(self): + prop_1 = TestProperty(Pose([1, 2, 3])) + prop_2 = TestProperty(Pose([1, 2, 3])) + prop_3 = prop_1 & prop_2 + resolved_property = self.knowledge_engine.resolve_properties(prop_3) + self.assertTrue(resolved_property.resolved) + self.assertEqual(type(resolved_property), And) + self.assertEqual(resolved_property, prop_3) + self.assertEqual(tuple((type(rp) for rp in resolved_property.children)), + (ResolvedProperty, ResolvedProperty)) + self.assertEqual(resolved_property.children[0].resolved_property_function, self.knowledge_source.test) + self.assertEqual(resolved_property.children[1].resolved_property_function, self.knowledge_source.test) + self.assertEqual(resolved_property.children[0].property_exception, ReasoningError) + self.assertEqual(len(resolved_property.children), 2) + self.assertTrue(resolved_property.children[0].is_leaf) + self.assertTrue(resolved_property.children[1].is_leaf) + + def test_property_execute_true(self): + prop_1 = TestProperty(Pose([1, 2, 3], frame="pose1")) + prop_2 = TestProperty(Pose([1, 2, 3], frame="pose2")) + prop_3 = prop_1 & prop_2 + resolved_property = self.knowledge_engine.resolve_properties(prop_3) + resolved_property() + self.assertTrue(resolved_property.resolved) + self.assertTrue(resolved_property.successful) + self.assertEqual(resolved_property.variables, {"pose1": Pose([1, 2, 3], frame="pose1"), + "pose2": Pose([1, 2, 3], frame="pose2")}) + self.assertTrue(resolved_property.executed) + + def test_property_execute_false(self): + prop_1 = TestProperty(Pose([1, 2, 3], frame="pose1")) + prop_2 = TestProperty(Pose([9, 9, 9], frame="pose2")) + prop_3 = prop_1 & prop_2 + resolved_property = self.knowledge_engine.resolve_properties(prop_3) + self.assertRaises(ReasoningError, resolved_property) + self.assertTrue(resolved_property.resolved) + self.assertFalse(resolved_property.successful) + self.assertTrue(resolved_property.executed) + self.assertEqual(resolved_property.variables, {"pose1": Pose([1, 2, 3], frame="pose1"), + "pose2": Pose([9, 9, 9], frame="pose2")}) + + def test_property_execute_and_sparse(self): + prop_1 = TestProperty(Pose([9, 9, 9], frame="pose1")) + prop_2 = TestProperty(Pose([1, 2, 3], frame="pose2")) + prop_3 = prop_1 & prop_2 + resolved_property = self.knowledge_engine.resolve_properties(prop_3) + self.assertRaises(ReasoningError, resolved_property) + self.assertTrue(resolved_property.resolved) + self.assertFalse(resolved_property.successful) + self.assertFalse(resolved_property.children[1].successful) + self.assertEqual(resolved_property.variables, {"pose1": Pose([9, 9, 9], frame="pose1")}) + self.assertTrue(resolved_property.executed) + + def test_property_execute_or_false(self): + prop_1 = TestProperty(Pose([1, 2, 3], frame="pose1")) + prop_2 = TestProperty(Pose([9, 9, 9], frame="pose2")) + prop_3 = prop_1 | prop_2 + resolved_property = self.knowledge_engine.resolve_properties(prop_3) + self.assertRaises(ReasoningError, resolved_property) + self.assertTrue(resolved_property.resolved) + self.assertFalse(resolved_property.successful) + self.assertTrue(resolved_property.executed) + self.assertEqual(resolved_property.variables, {"pose1": Pose([1, 2, 3], frame="pose1"), + "pose2": Pose([9, 9, 9], frame="pose2")}) + + def test_property_execute_or_true(self): + prop_1 = TestProperty(Pose([1, 2, 3], frame="pose1")) + prop_2 = TestProperty(Pose([1, 2, 3], frame="pose2")) + prop_3 = prop_1 | prop_2 + resolved_property = self.knowledge_engine.resolve_properties(prop_3) + resolved_property() + self.assertTrue(resolved_property.resolved) + self.assertTrue(resolved_property.successful) + self.assertTrue(resolved_property.executed) + self.assertEqual(resolved_property.variables, {"pose1": Pose([1, 2, 3], frame="pose1"), + "pose2": Pose([1, 2, 3], frame="pose2")}) + + def test_property_execute_not_true(self): + prop_1 = TestProperty(Pose([1, 2, 3], frame="pose1")) + prop_2 = ~prop_1 + resolved_property = self.knowledge_engine.resolve_properties(prop_2) + resolved_property() + self.assertTrue(resolved_property.resolved) + self.assertFalse(resolved_property.successful) + self.assertTrue(resolved_property.executed) + self.assertEqual(resolved_property.variables, {"pose1": Pose([1, 2, 3], frame="pose1")}) + + def test_property_execute_not_false(self): + prop_1 = TestProperty(Pose([9, 9, 9], frame="pose1")) + prop_2 = ~prop_1 + resolved_property = self.knowledge_engine.resolve_properties(prop_2) + self.assertRaises(ReasoningError, resolved_property) + self.assertTrue(resolved_property.resolved) + self.assertFalse(resolved_property.successful) + self.assertTrue(resolved_property.executed) + self.assertEqual(resolved_property.variables, {"pose1": Pose([9, 9, 9], frame="pose1")}) \ No newline at end of file