From 864373372f84f75a7f31423e99f418a911e2ee09 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 15 Apr 2024 15:33:59 +0200 Subject: [PATCH 01/57] [knowledge] First draft of knowledge source --- src/pycram/datastructures/knowledge_source.py | 76 ++++++++++++++++++ src/pycram/knowledge/__init__.py | 0 .../knowledge/food_cutting_knowledge.py | 78 +++++++++++++++++++ src/pycram/knowledge/knowrob_knowledge.py | 35 +++++++++ src/pycram/plan_failures.py | 7 ++ 5 files changed, 196 insertions(+) create mode 100644 src/pycram/datastructures/knowledge_source.py create mode 100644 src/pycram/knowledge/__init__.py create mode 100644 src/pycram/knowledge/food_cutting_knowledge.py create mode 100644 src/pycram/knowledge/knowrob_knowledge.py diff --git a/src/pycram/datastructures/knowledge_source.py b/src/pycram/datastructures/knowledge_source.py new file mode 100644 index 000000000..374211c14 --- /dev/null +++ b/src/pycram/datastructures/knowledge_source.py @@ -0,0 +1,76 @@ +from abc import ABC, abstractmethod +from ..designator import DesignatorDescription +from ..plan_failures import KnowledgeNotAvailable + + +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): + """ + Connect to the knowledge source + """ + raise NotImplementedError + + def __str__(self): + return f"{self.name} - Priority:{self.priority}" + + @abstractmethod + def query(self, designator): + raise NotImplementedError + + 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 + """ + raise KnowledgeNotAvailable(f"Pose for object {designator} not available in {self.name}") + + 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}") + + + diff --git a/src/pycram/knowledge/__init__.py b/src/pycram/knowledge/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/knowledge/food_cutting_knowledge.py b/src/pycram/knowledge/food_cutting_knowledge.py new file mode 100644 index 000000000..205473d3e --- /dev/null +++ b/src/pycram/knowledge/food_cutting_knowledge.py @@ -0,0 +1,78 @@ +from ..datastructures.knowledge_source import KnowledgeSource + +from rdflib import Graph, Namespace +from rdflib.namespace import OWL, RDFS + +from urllib import request +from typing_extensions import Iterable + +class FoodCuttingKnowledge(KnowledgeSource): + + def __init__(self): + super().__init__("Food KG", 1) + self.knowledge_graph = Graph() + + # define prefixes to be used in the query + SOMA = Namespace("http://www.ease-crc.org/ont/SOMA.owl#") + CUT2 = Namespace("http://www.ease-crc.org/ont/situation_awareness#") + CUT = Namespace("http://www.ease-crc.org/ont/food_cutting#") + DUL = Namespace("http://www.ontologydesignpatterns.org/ont/dul/DUL.owl#") + OBO = Namespace("http://purl.obolibrary.org/obo/") + self.knowledge_graph.bind("owl", OWL) + self.knowledge_graph.bind("rdfs", RDFS) + self.knowledge_graph.bind("soma", SOMA) + self.knowledge_graph.bind("cut2", CUT2) + self.knowledge_graph.bind("cut", CUT) + self.knowledge_graph.bind("dul", DUL) + self.knowledge_graph.bind("obo", OBO) + + @property + def is_available(self) -> bool: + return request.urlopen("https://api.krr.triply.cc/datasets/mkumpel/FruitCuttingKG/services/FoodCuttingKG/sparql?query=SELECT%20?subject%20?predicate%20?objectWHERE%20{?subject%20?predicate%20?object%20.}").getcode() != 404 + + @property + def is_connected(self) -> bool: + return self.is_available + + def connect(self): + pass + + def query(self, designator): + pass + + def get_repetitions(self, task, task_object) -> Iterable[str]: + # task = "cut:Quartering" + # tobject = "obo:FOODON_03301710" + repetitionsquery = """ SELECT ?rep WHERE { + SERVICE { + { + OPTIONAL{ %s rdfs:subClassOf ?action} + ?action rdfs:subClassOf* ?rep_node. + ?rep_node owl:onProperty cut:repetitions. + FILTER EXISTS { + ?rep_node owl:hasValue ?val.} + BIND("0.05" AS ?rep)} + UNION + { + OPTIONAL{ %s rdfs:subClassOf ?action } + ?action rdfs:subClassOf* ?rep_node. + ?rep_node owl:onProperty cut:repetitions. + FILTER EXISTS { + ?rep_node owl:minQualifiedCardinality ?val.} + BIND("more than 1" AS ?rep)}} }""" % (task, task) + for row in self.knowledge_graph.query(repetitionsquery): + yield row.rep + + def get_technique_for_task(self, task, task_object) -> Iterable[str]: + # task = "cut:Quartering" + # tobject = "obo:FOODON_03301710" + positionquery = """ SELECT ?pos WHERE { + SERVICE { + OPTIONAL { %s rdfs:subClassOf ?sub.} + ?sub rdfs:subClassOf* ?node. + ?node owl:onProperty cut:affordsPosition. + ?node owl:someValuesFrom ?position. + BIND(IF(?position = cut:halving_position, "halving", "slicing") AS ?pos) + } }""" % task + for row in self.knowledge_graph.query(positionquery): + yield row.pos diff --git a/src/pycram/knowledge/knowrob_knowledge.py b/src/pycram/knowledge/knowrob_knowledge.py new file mode 100644 index 000000000..f918f5150 --- /dev/null +++ b/src/pycram/knowledge/knowrob_knowledge.py @@ -0,0 +1,35 @@ +import rospy + +from ..datastructures.knowledge_source import KnowledgeSource +import rosservice +from ..designator import DesignatorDescription +try: + from rosprolog_client import Prolog +except ModuleNotFoundError as e: + rospy.logwarn(f"Could not import Prolog client from package rosprolog_client") + + +class KnowrobKnowledge(KnowledgeSource): + + def __init__(self): + super().__init__("Knowrob", 0) + self.prolog_client = None + + @property + def is_available(self) -> bool: + return '/rosprolog/query' in rosservice.get_service_list() + + @property + def is_connected(self) -> bool: + return self.prolog_client is not None + + def connect(self): + if self.is_available: + self.prolog_client = Prolog() + self.prolog_client.once(f"tripledb_load('package://iai_apartment/owl/iai-apartment.owl').") + + def query(self, designator: DesignatorDescription) -> DesignatorDescription: + pass + + def query_pose_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: + result = self.prolog_client.once(f"") diff --git a/src/pycram/plan_failures.py b/src/pycram/plan_failures.py index dceb68013..5f581c324 100644 --- a/src/pycram/plan_failures.py +++ b/src/pycram/plan_failures.py @@ -6,6 +6,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) From 7d16285c7037e960492d7bb449fc601add99a13d Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 16 Apr 2024 10:37:53 +0200 Subject: [PATCH 02/57] [knowledge] Added Knowledge engine --- src/pycram/datastructures/knowledge_engine.py | 15 +++++++++++++++ src/pycram/knowledge/__init__.py | 16 ++++++++++++++++ 2 files changed, 31 insertions(+) create mode 100644 src/pycram/datastructures/knowledge_engine.py diff --git a/src/pycram/datastructures/knowledge_engine.py b/src/pycram/datastructures/knowledge_engine.py new file mode 100644 index 000000000..fac816b17 --- /dev/null +++ b/src/pycram/datastructures/knowledge_engine.py @@ -0,0 +1,15 @@ +from .knowledge_source import KnowledgeSource + + +class KnowledgeEngine: + def __init__(self): + self.knowledge_sources = [] + + def _init_sources(self): + sources = KnowledgeSource.__subclasses__() + for src in sources: + self.knowledge_sources.append(src()) + self.knowledge_sources.sort(key=lambda x: x.priority) + + def query(self): + ... diff --git a/src/pycram/knowledge/__init__.py b/src/pycram/knowledge/__init__.py index e69de29bb..2539ca29c 100644 --- a/src/pycram/knowledge/__init__.py +++ b/src/pycram/knowledge/__init__.py @@ -0,0 +1,16 @@ +from ..datastructures.knowledge_engine import KnowledgeEngine + +import os +import sys + +path = os.path.dirname(os.path.abspath(__file__)) + +for py in [f[:-3] for f in os.listdir(path) if f.endswith('.py') and f != '__init__.py']: + mod = __import__('.'.join([__name__, py]), fromlist=[py]) + classes = [getattr(mod, x) for x in dir(mod) if isinstance(getattr(mod, x), type)] + print(classes) + for cls in classes: + setattr(sys.modules[__name__], cls.__name__, cls) + +knowledge_engine = KnowledgeEngine() + From db8874e6ab5f3671149271913649d54aa6abe26e Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 28 May 2024 14:12:29 +0200 Subject: [PATCH 03/57] [knowledge] Update architecture for knowledge engine --- src/pycram/datastructures/knowledge_engine.py | 33 +++++++++++++++++-- src/pycram/datastructures/knowledge_source.py | 10 ++++-- src/pycram/knowledge/__init__.py | 28 ++++++++-------- src/pycram/knowledge/knowrob_knowledge.py | 6 ++-- 4 files changed, 54 insertions(+), 23 deletions(-) diff --git a/src/pycram/datastructures/knowledge_engine.py b/src/pycram/datastructures/knowledge_engine.py index fac816b17..1f65a39cc 100644 --- a/src/pycram/datastructures/knowledge_engine.py +++ b/src/pycram/datastructures/knowledge_engine.py @@ -1,15 +1,42 @@ from .knowledge_source import KnowledgeSource +from ..designator import DesignatorDescription +from typing_extensions import Type class KnowledgeEngine: + """ + The knowledge engine is responsible for managing the knowledge sources and querying them to fill parameters of + designators + """ def __init__(self): self.knowledge_sources = [] - - def _init_sources(self): + # Initialize all knowledge sources sources = KnowledgeSource.__subclasses__() for src in sources: self.knowledge_sources.append(src()) self.knowledge_sources.sort(key=lambda x: x.priority) - def query(self): + def query(self, designator: Type[DesignatorDescription]): + """ + Query to fill parameters of a designator from the knowledge sources + + :return: + """ + ... + + def update(self): + """ + Update the knowledge sources with new information contained in a designator + + :return: + """ ... + + 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 + """ + ... + diff --git a/src/pycram/datastructures/knowledge_source.py b/src/pycram/datastructures/knowledge_source.py index 374211c14..ac178608d 100644 --- a/src/pycram/datastructures/knowledge_source.py +++ b/src/pycram/datastructures/knowledge_source.py @@ -48,10 +48,12 @@ def connect(self): def __str__(self): return f"{self.name} - Priority:{self.priority}" - @abstractmethod - def query(self, designator): - raise NotImplementedError + # @abstractmethod + # def query(self, designator): + # raise NotImplementedError + +class QueryKnowledge: def query_pose_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: """ Query the pose for an object from the knowledge source @@ -73,4 +75,6 @@ def query_grasp_for_object(self, designator: DesignatorDescription) -> Designato raise KnowledgeNotAvailable(f"Grasp for object {designator} not available in {self.name}") +class UpdateKnowledge: + pass diff --git a/src/pycram/knowledge/__init__.py b/src/pycram/knowledge/__init__.py index 2539ca29c..56ce57d18 100644 --- a/src/pycram/knowledge/__init__.py +++ b/src/pycram/knowledge/__init__.py @@ -1,16 +1,16 @@ -from ..datastructures.knowledge_engine import KnowledgeEngine +# from ..datastructures.knowledge_engine import KnowledgeEngine +# +# import os +# import sys -import os -import sys - -path = os.path.dirname(os.path.abspath(__file__)) - -for py in [f[:-3] for f in os.listdir(path) if f.endswith('.py') and f != '__init__.py']: - mod = __import__('.'.join([__name__, py]), fromlist=[py]) - classes = [getattr(mod, x) for x in dir(mod) if isinstance(getattr(mod, x), type)] - print(classes) - for cls in classes: - setattr(sys.modules[__name__], cls.__name__, cls) - -knowledge_engine = KnowledgeEngine() +# path = os.path.dirname(os.path.abspath(__file__)) +# +# for py in [f[:-3] for f in os.listdir(path) if f.endswith('.py') and f != '__init__.py']: +# mod = __import__('.'.join([__name__, py]), fromlist=[py]) +# classes = [getattr(mod, x) for x in dir(mod) if isinstance(getattr(mod, x), type)] +# print(classes) +# for cls in classes: +# setattr(sys.modules[__name__], cls.__name__, cls) +# knowledge_engine = KnowledgeEngine() +# diff --git a/src/pycram/knowledge/knowrob_knowledge.py b/src/pycram/knowledge/knowrob_knowledge.py index f918f5150..a01f79bf0 100644 --- a/src/pycram/knowledge/knowrob_knowledge.py +++ b/src/pycram/knowledge/knowrob_knowledge.py @@ -1,15 +1,15 @@ import rospy -from ..datastructures.knowledge_source import KnowledgeSource +from ..datastructures.knowledge_source import KnowledgeSource, QueryKnowledge, UpdateKnowledge import rosservice from ..designator import DesignatorDescription try: from rosprolog_client import Prolog except ModuleNotFoundError as e: - rospy.logwarn(f"Could not import Prolog client from package rosprolog_client") + rospy.logwarn(f"Could not import Prolog client from package rosprolog_client, Knowrob related features are not available.") -class KnowrobKnowledge(KnowledgeSource): +class KnowrobKnowledge(KnowledgeSource, QueryKnowledge, UpdateKnowledge): def __init__(self): super().__init__("Knowrob", 0) From 628cf93ab348c1ac09b168a437491fb9eaa6a1b4 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 31 May 2024 15:24:43 +0200 Subject: [PATCH 04/57] [datastructures] Added decision tree implementation --- src/pycram/datastructures/decision_tree.py | 90 +++++++++++++++++++ .../knowledge/decision_trees/__init__.py | 0 .../knowledge/decision_trees/object_tree.py | 0 src/pycram/knowledge/facts_knowledge.py | 0 4 files changed, 90 insertions(+) create mode 100644 src/pycram/datastructures/decision_tree.py create mode 100644 src/pycram/knowledge/decision_trees/__init__.py create mode 100644 src/pycram/knowledge/decision_trees/object_tree.py create mode 100644 src/pycram/knowledge/facts_knowledge.py diff --git a/src/pycram/datastructures/decision_tree.py b/src/pycram/datastructures/decision_tree.py new file mode 100644 index 000000000..9fc4a69ce --- /dev/null +++ b/src/pycram/datastructures/decision_tree.py @@ -0,0 +1,90 @@ +# used for delayed evaluation of typing until python 3.11 becomes mainstream +from __future__ import annotations + +from anytree import NodeMixin +from typing_extensions import Iterable, Callable + +from ..designator import DesignatorDescription + + +class DecisionTreeNode(NodeMixin): + + def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None): + self.parent = parent + self.true_path = None + self.false_path = None + if children: + self.children = children + + def __add__(self, other: DecisionTreeNode) -> DecisionTreeNode: + if isinstance(self, Decision): + self._add_true(other) + return self + else: + raise TypeError("Only Decision nodes can have children") + + def __sub__(self, other: DecisionTreeNode) -> DecisionTreeNode: + if isinstance(self, Decision): + self._add_false(other) + return self + else: + raise TypeError("Only Decision nodes can have children") + + def perform(self, designator: DesignatorDescription): + """ + To be implemented by subclasses + """ + raise NotImplementedError + + +class Decision(DecisionTreeNode): + + def __init__(self, condition: Callable): + super().__init__(parent=None, children=None) + self.condition = condition + + def perform(self, designator: DesignatorDescription) -> DesignatorDescription: + if self.condition(): + if self.true_path: + return self.true_path.perform(designator) + else: + raise ValueError(f"No true path defined for decision node: {self}") + else: + if self.false_path: + return self.false_path.perform(designator) + else: + raise ValueError(f"No false path defined for decision node: {self}") + + def _add_true(self, other): + if not self.true_path: + self.true_path = other + other.parent = self + else: + self.true_path._add_true(other) + + def _add_false(self, other): + if not self.false_path: + self.false_path = other + other.parent = self + else: + self.false_path._add_false(other) + + def __repr__(self): + if self.parent: + return f"{'+' if self.parent.true_path is self else '-'}{self.__class__.__name__}({self.condition})" + return f"{self.__class__.__name__}({self.condition})" + + +class Query(DecisionTreeNode): + + def __init__(self, query: Callable): + super().__init__(parent=None, children=None) + self.query = query + + def perform(self, designator) -> DesignatorDescription: + return self.query(designator) + + def __repr__(self): + if self.parent: + return f"{'+' if self.parent.true_path is self else '-'}{self.__class__.__name__}({self.query})" + return f"{self.__class__.__name__}({self.query})" diff --git a/src/pycram/knowledge/decision_trees/__init__.py b/src/pycram/knowledge/decision_trees/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/knowledge/decision_trees/object_tree.py b/src/pycram/knowledge/decision_trees/object_tree.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/knowledge/facts_knowledge.py b/src/pycram/knowledge/facts_knowledge.py new file mode 100644 index 000000000..e69de29bb From ec1d66aa3a2f05cf2ba25c23fbc781dc2e16442f Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 31 May 2024 15:25:51 +0200 Subject: [PATCH 05/57] [knowledge] Added more knowledge sources --- .../knowledge/decision_trees/object_tree.py | 12 +++++++ src/pycram/knowledge/facts_knowledge.py | 34 +++++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/src/pycram/knowledge/decision_trees/object_tree.py b/src/pycram/knowledge/decision_trees/object_tree.py index e69de29bb..2273c247b 100644 --- a/src/pycram/knowledge/decision_trees/object_tree.py +++ b/src/pycram/knowledge/decision_trees/object_tree.py @@ -0,0 +1,12 @@ +from ...datastructures.decision_tree import Decision, Query +from anytree import RenderTree + +object_tree = None + +context = (Decision(lambda designator: designator.type == "context") + + Decision(lambda designator: designator.type == "object") + - Query(lambda designator: object_tree.query_grasp_for_object(designator))) + +print(RenderTree(context)) + + diff --git a/src/pycram/knowledge/facts_knowledge.py b/src/pycram/knowledge/facts_knowledge.py index e69de29bb..7e834e7de 100644 --- a/src/pycram/knowledge/facts_knowledge.py +++ b/src/pycram/knowledge/facts_knowledge.py @@ -0,0 +1,34 @@ +from ..datastructures.knowledge_source import KnowledgeSource, QueryKnowledge, UpdateKnowledge +from ..designator import DesignatorDescription + + +class FactsKnowledge(KnowledgeSource, QueryKnowledge, UpdateKnowledge): + """ + 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 query(self, designator: DesignatorDescription) -> DesignatorDescription: + """ + Query the knowledge source for the facts + """ + pass + + def query_pose_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: + pass + + def query_grasp_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: + pass From 827b93c6eacbb6556f7b787343314938ed73401b Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 31 May 2024 16:04:46 +0200 Subject: [PATCH 06/57] [DecisionTree] Documentation --- src/pycram/datastructures/decision_tree.py | 72 +++++++++++++++++++++- 1 file changed, 70 insertions(+), 2 deletions(-) diff --git a/src/pycram/datastructures/decision_tree.py b/src/pycram/datastructures/decision_tree.py index 9fc4a69ce..7b4a704ad 100644 --- a/src/pycram/datastructures/decision_tree.py +++ b/src/pycram/datastructures/decision_tree.py @@ -8,8 +8,18 @@ class DecisionTreeNode(NodeMixin): + """ + Base class for nodes in the Decision Tree + """ def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None): + """ + A node in the decision tree needs a parent and can have children. + Children can be added using the + and - operators. + + :param parent: An instance of type DecisionTreeNode which is the parent of this node + :param children: A list of DecisionTreeNode instances which are the children of this node + """ self.parent = parent self.true_path = None self.false_path = None @@ -17,6 +27,13 @@ def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = Non self.children = children def __add__(self, other: DecisionTreeNode) -> DecisionTreeNode: + """ + Overloads the "+" operator to add a child to the true path of a decision node. This child is executed if the + condition of the decision node is true. + + :param other: DecisionTreeNode instance which should be executed if the condition of the decision node is true + :return: This DecisionTreeNode instance + """ if isinstance(self, Decision): self._add_true(other) return self @@ -24,6 +41,13 @@ def __add__(self, other: DecisionTreeNode) -> DecisionTreeNode: raise TypeError("Only Decision nodes can have children") def __sub__(self, other: DecisionTreeNode) -> DecisionTreeNode: + """ + Overloads the "-" operator to add a child to the false path of a decision node. This child is executed if the + condition of the decision node is false. + + :param other: DecisionTreeNode instance which should be executed if the condition of the decision node is false + :return: This DecisionTreeNode instance + """ if isinstance(self, Decision): self._add_false(other) return self @@ -32,18 +56,28 @@ def __sub__(self, other: DecisionTreeNode) -> DecisionTreeNode: def perform(self, designator: DesignatorDescription): """ - To be implemented by subclasses + To be implemented by subclasses. Defines the behavior of the node when it is executed. """ raise NotImplementedError class Decision(DecisionTreeNode): + """ + A decision node in the decision tree. It has a condition which is evaluated to determine the path to take. + """ def __init__(self, condition: Callable): super().__init__(parent=None, children=None) self.condition = condition def perform(self, designator: DesignatorDescription) -> DesignatorDescription: + """ + Calls the condition function and evaluates the result to determine the path to take. If the condition is true + the true_path is executed, otherwise the false_path is executed. + + :param designator: Designator for which the condition should be evaluated + :return: The result of the path that was executed, which should be the result of a Query + """ if self.condition(): if self.true_path: return self.true_path.perform(designator) @@ -56,6 +90,12 @@ def perform(self, designator: DesignatorDescription) -> DesignatorDescription: raise ValueError(f"No false path defined for decision node: {self}") def _add_true(self, other): + """ + Adds a child to the true path of this decision node. If the true path is already defined, the child is added to + the last node along the true path in this tree. + + :param other: DecisionTreeNode instance which should be added as the child of this node. + """ if not self.true_path: self.true_path = other other.parent = self @@ -63,28 +103,56 @@ def _add_true(self, other): self.true_path._add_true(other) def _add_false(self, other): + """ + Adds a child to the false path of this decision node. If the false path is already defined, the child is added to + the last node along the false path in this tree. + + :param other: DecisionTreeNode instance which should be added as the child of this node. + """ if not self.false_path: self.false_path = other other.parent = self else: self.false_path._add_false(other) - def __repr__(self): + def __repr__(self) -> str: + """ + Returns a string representation of the decision node. If the node has a parent, the string representation includes + if this node the true or false child of the parent. + + :return: A string representation of the decision node + """ if self.parent: return f"{'+' if self.parent.true_path is self else '-'}{self.__class__.__name__}({self.condition})" return f"{self.__class__.__name__}({self.condition})" class Query(DecisionTreeNode): + """ + A query node in the decision tree. It has a query function which is executed when the node is evaluated. + This node should function as a leaf node in the decision tree. + """ def __init__(self, query: Callable): super().__init__(parent=None, children=None) self.query = query def perform(self, designator) -> DesignatorDescription: + """ + Calls the query function of this node and returns the result. + + :param designator: The designator for which the query is used + :return: A designator with the result of the query as a parameter + """ return self.query(designator) def __repr__(self): + """ + Returns a string representation of the query node. If the node has a parent, the string representation includes + if this node the true or false child of the parent. + + :return: A string representation of the query node + """ if self.parent: return f"{'+' if self.parent.true_path is self else '-'}{self.__class__.__name__}({self.query})" return f"{self.__class__.__name__}({self.query})" From 925c4797404df07e7f2fa6615a438a350a50d5e9 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 13 Jun 2024 12:24:46 +0200 Subject: [PATCH 07/57] [aspects] Added aspects and conditions --- src/pycram/datastructures/aspects.py | 46 +++++++++++++++++++++++++ src/pycram/datastructures/conditions.py | 5 +++ 2 files changed, 51 insertions(+) create mode 100644 src/pycram/datastructures/aspects.py create mode 100644 src/pycram/datastructures/conditions.py diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/aspects.py new file mode 100644 index 000000000..621235c21 --- /dev/null +++ b/src/pycram/datastructures/aspects.py @@ -0,0 +1,46 @@ +from .pose import Pose +from typing_extensions import List + +from ..designator import ObjectDesignatorDescription + + +class Aspect: + + def __and__(self, other): + return AndAspect([self, other]) + + def __or__(self, other): + return OrAspect([self, other]) + + def simplify(self): + pass + + +class AndAspect(Aspect): + + def __init__(self, aspects: List[Aspect]): + self.aspects = aspects + + +class OrAspect(Aspect): + + def __init__(self, aspects: List[Aspect]): + self.aspects = aspects + + +class ReachableAspect(Aspect): + + def __init__(self, object_designator: ObjectDesignatorDescription): + self.object_designator = object_designator + + def reachable(self, pose: Pose) -> bool: + raise NotImplementedError + + +class GraspableAspect(Aspect): + + def __init__(self, object_designator: ObjectDesignatorDescription): + self.object_designator = object_designator + + def graspable(self) -> bool: + raise NotImplementedError diff --git a/src/pycram/datastructures/conditions.py b/src/pycram/datastructures/conditions.py new file mode 100644 index 000000000..dc1541ffb --- /dev/null +++ b/src/pycram/datastructures/conditions.py @@ -0,0 +1,5 @@ +from .aspects import ReachableAspect + + +def is_reachable(entity: ReachableAspect) -> bool: + entity.pose = None From 791731a410fade1645397ed2db7c7100d8ae230a Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 13 Jun 2024 12:26:25 +0200 Subject: [PATCH 08/57] [designator] Added knowledge pre conditions to pick up --- src/pycram/designators/action_designator.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 2a2c24ba2..5ee2aa24d 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -16,6 +16,7 @@ from .motion_designator import MoveJointsMotion, MoveGripperMotion, MoveArmJointsMotion, MoveTCPMotion, MoveMotion, \ LookingMotion, DetectingMotion, OpeningMotion, ClosingMotion from .object_designator import ObjectDesignatorDescription, BelieveObject, ObjectPart +from ..datastructures.aspects import GraspableAspect, ReachableAspect from ..local_transformer import LocalTransformer from ..plan_failures import ObjectUnfetchable, ReachabilityFailure from ..robot_descriptions import robot_description @@ -218,6 +219,7 @@ def __init__(self, ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description self.arms: List[str] = arms self.grasps: List[str] = grasps + self.knowledge_conditions = [GraspableAspect(self.object_designator_description) & ReachableAspect(self.object_designator_description)] if self.soma: self.init_ontology_concepts({"picking_up": self.soma.PickingUp}) From 9d5bcd2f225b86612fbcd4a5bc199ff5869bd290 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 9 Jul 2024 16:24:08 +0200 Subject: [PATCH 09/57] [aspects] New Aspects structure --- src/pycram/datastructures/aspects.py | 184 ++++++++++++++++++++++++++- 1 file changed, 177 insertions(+), 7 deletions(-) diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/aspects.py index 621235c21..134cbac08 100644 --- a/src/pycram/datastructures/aspects.py +++ b/src/pycram/datastructures/aspects.py @@ -1,46 +1,216 @@ +# used for delayed evaluation of typing until python 3.11 becomes mainstream +from __future__ import annotations + from .pose import Pose -from typing_extensions import List +from typing_extensions import List, Iterable +from anytree import NodeMixin, PreOrderIter, Node from ..designator import ObjectDesignatorDescription -class Aspect: +class Aspect(NodeMixin): + """ + Parent class to represent a semantic aspect 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. + """ + + def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None): + super().__init__() + self.parent = parent + if children: + self.children = children def __and__(self, other): + """ + Overload the and operator to create an AndAspect as part of the tree structure. + + :param other: The other aspect to combine with + :return: An AndAspect containing both, this and the other, aspect + """ return AndAspect([self, other]) def __or__(self, other): + """ + Overload the or operator to create an OrAspect as part of the tree structure. + + :param other: The other aspect to combine with + :return: An OrAspect containing both, this and the other, aspect + """ return OrAspect([self, other]) - def simplify(self): - pass + def __neg__(self): + """ + Overload the not operator to create a NotAspect as part of the tree structure. + + :return: A NotAspect containing this aspect + """ + return NotAspect(self) + + def __invert__(self): + """ + Overload the invert operator to create a NotAspect as part of the tree structure. + :return: A NotAspect containing this aspect + """ + return NotAspect(self) -class AndAspect(Aspect): +class AspectOperator(Aspect): + """ + Parent class for logical operators to combine multiple aspects in a tree structure. This class adds methods to + use Aspects 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, aspects: List[Aspect]): + """ + Initialize the AspectOperator with a list of aspects. The parent of this node is None, therefore the node is + always the root of the tree. + + :param aspects: A list of aspects to which are the children of this node + """ + super().__init__(parent=None, children=aspects) self.aspects = aspects + def simplify(self): + """ + Simplifies the tree structure by merging nodes of the same type. This is done by iterating over the tree and + 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 AndAspect(AspectOperator): + """ + Class to represent a logical and operator in a tree structure. This class inherits from AspectOperator and adds a + method to evaluate the children as an and operator. + """ -class OrAspect(Aspect): + def __init__(self, aspects: List[Aspect]): + """ + Initialize the AndAspect with a list of aspects as the children of this node. This node will be the root of the + tree. + + :param aspects: A list of aspects which are the children of this node + """ + super().__init__(aspects) + self.simplify() + + def __call__(self, *args, **kwargs): + """ + 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 + """ + result = True + for child in self.children: + result = result and child(*args, **kwargs) + if not result: + return False + + +class OrAspect(AspectOperator): + """ + Class to represent a logical or operator in a tree structure. This class inherits from AspectOperator and adds a + method to evaluate the children as an or operator. + """ def __init__(self, aspects: List[Aspect]): - self.aspects = aspects + """ + Initialize the OrAspect with a list of aspects as the children of this node. This node will be the root of the + tree. + + :param aspects: A list of aspects which are the children of this node + """ + super().__init__(aspects) + self.simplify() + + def __call__(self, *args, **kwargs): + """ + 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 + """ + result = False + for child in self.children: + result = result or child(*args, **kwargs) + if result: + return True + + +class NotAspect(AspectOperator): + """ + Class to represent a logical not operator in a tree structure. This class inherits from AspectOperator and adds a + method to evaluate the children as a not operator. + """ + + def __init__(self, aspect: Aspect): + """ + Initialize the NotAspect with an aspect as the child of this node. This node will be the root of the tree. + + :param aspect: The aspect which is the child of this node + """ + super().__init__([aspect]) + + def __call__(self, *args, **kwargs): + """ + 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 + """ + return not self.children[0](*args, **kwargs) class ReachableAspect(Aspect): def __init__(self, object_designator: ObjectDesignatorDescription): + super().__init__(None, None) self.object_designator = object_designator def reachable(self, pose: Pose) -> bool: raise NotImplementedError + def __call__(self, *args, **kwargs): + return self.reachable(*args, **kwargs) + class GraspableAspect(Aspect): def __init__(self, object_designator: ObjectDesignatorDescription): + super().__init__(None, None) self.object_designator = object_designator def graspable(self) -> bool: raise NotImplementedError + + def __call__(self, *args, **kwargs): + return self.graspable(*args, **kwargs) From fbce04f69a4e8a1095a0cebde70b41156af51f7f Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 10 Jul 2024 10:52:07 +0200 Subject: [PATCH 10/57] [KnowledgeEngine] Method to call source with highest priority --- src/pycram/datastructures/aspects.py | 22 ++++++- src/pycram/datastructures/knowledge_engine.py | 62 +++++++++++++++++-- src/pycram/datastructures/knowledge_source.py | 10 ++- 3 files changed, 81 insertions(+), 13 deletions(-) diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/aspects.py index 134cbac08..e611f2220 100644 --- a/src/pycram/datastructures/aspects.py +++ b/src/pycram/datastructures/aspects.py @@ -1,6 +1,8 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations +from abc import abstractmethod + from .pose import Pose from typing_extensions import List, Iterable from anytree import NodeMixin, PreOrderIter, Node @@ -55,6 +57,10 @@ def __invert__(self): """ return NotAspect(self) + @abstractmethod + def __call__(self, *args, **kwargs): + raise NotImplementedError("The __call__ method must be implemented in the subclass") + class AspectOperator(Aspect): """ @@ -97,6 +103,16 @@ def merge_nodes(node1: Node, node2: Node) -> None: node2.parent = None node1.children = node2.children + node1.children + def __call__(self, *args, **kwargs): + """ + Implementation of the abstract method, since this class only acts as a parent class for logical operators there + is no implementation here. + + :param args: A list of arguments + :param kwargs: A dictionary of keyword arguments + """ + pass + class AndAspect(AspectOperator): """ @@ -114,7 +130,7 @@ def __init__(self, aspects: List[Aspect]): super().__init__(aspects) self.simplify() - def __call__(self, *args, **kwargs): + def __call__(self, *args, **kwargs) -> bool: """ 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 @@ -147,7 +163,7 @@ def __init__(self, aspects: List[Aspect]): super().__init__(aspects) self.simplify() - def __call__(self, *args, **kwargs): + def __call__(self, *args, **kwargs) -> bool: """ 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 @@ -178,7 +194,7 @@ def __init__(self, aspect: Aspect): """ super().__init__([aspect]) - def __call__(self, *args, **kwargs): + def __call__(self, *args, **kwargs) -> bool: """ 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. diff --git a/src/pycram/datastructures/knowledge_engine.py b/src/pycram/datastructures/knowledge_engine.py index 1f65a39cc..ae6a1ed82 100644 --- a/src/pycram/datastructures/knowledge_engine.py +++ b/src/pycram/datastructures/knowledge_engine.py @@ -1,6 +1,12 @@ +import inspect + +import rospy + from .knowledge_source import KnowledgeSource -from ..designator import DesignatorDescription -from typing_extensions import Type +from ..designator import DesignatorDescription, ActionDesignatorDescription +from typing_extensions import Type, Callable, List + +from ..plan_failures import KnowledgeNotAvailable class KnowledgeEngine: @@ -8,21 +14,51 @@ class KnowledgeEngine: The knowledge engine is responsible for managing the knowledge sources and querying them to fill parameters of designators """ + + _instance = None + + 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): + if self._initialized: 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 + """ + # Class reference to all knowledge sources sources = KnowledgeSource.__subclasses__() for src in sources: self.knowledge_sources.append(src()) self.knowledge_sources.sort(key=lambda x: x.priority) - def query(self, designator: Type[DesignatorDescription]): + def update_sources(self): + """ + Update all knowledge sources, this will check if the sources are still and if new sources have become available. + """ + 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]): """ Query to fill parameters of a designator from the knowledge sources :return: """ - ... + conditions = designator.knowledge_conditions + conditions(designator) def update(self): """ @@ -40,3 +76,21 @@ def ground_solution(self, designator: Type[DesignatorDescription]) -> bool: """ ... + def call_source(self, query_function: Callable, *args, **kwargs): + """ + Call the given query function on the knowledge source with the highest priority that is connected + + :param query_function: The query function of the knowledge source + :param args: The arguments to pass to the query function + :param kwargs: The keyword arguments to pass to the query function + :return: The result of the query function + """ + self.update_sources() + for source in self.knowledge_sources: + print(source) + if (query_function.__name__ in list(source.__class__.__dict__.keys()) + and source.is_connected): + source_query_function = getattr(source, query_function.__name__) + return source_query_function(*args, **kwargs) + raise KnowledgeNotAvailable(f"Query function {query_function.__name__} is not available in any connected knowledge source") + diff --git a/src/pycram/datastructures/knowledge_source.py b/src/pycram/datastructures/knowledge_source.py index ac178608d..d3861d031 100644 --- a/src/pycram/datastructures/knowledge_source.py +++ b/src/pycram/datastructures/knowledge_source.py @@ -1,4 +1,5 @@ from abc import ABC, abstractmethod + from ..designator import DesignatorDescription from ..plan_failures import KnowledgeNotAvailable @@ -39,7 +40,7 @@ def is_connected(self) -> bool: raise NotImplementedError @abstractmethod - def connect(self): + def connect(self) -> bool: """ Connect to the knowledge source """ @@ -48,10 +49,6 @@ def connect(self): def __str__(self): return f"{self.name} - Priority:{self.priority}" - # @abstractmethod - # def query(self, designator): - # raise NotImplementedError - class QueryKnowledge: def query_pose_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: @@ -62,7 +59,8 @@ def query_pose_for_object(self, designator: DesignatorDescription) -> Designator :return: Designator with the pose information :raises KnowledgeNotAvailable: If the pose for the object is not available in this knowledge source """ - raise KnowledgeNotAvailable(f"Pose for object {designator} not available in {self.name}") + pass + def query_grasp_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: """ From 025f46aabbdcc7f09acdce35152f0807035e54ab Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 10 Jul 2024 10:53:35 +0200 Subject: [PATCH 11/57] [Action desig] Fixed Typing --- src/pycram/designators/action_designator.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 231cc2aba..c6eb2a219 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -98,8 +98,8 @@ def __init__(self, grippers: List[Arms], motions: List[GripperState], resolver=N :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ super().__init__(resolver, ontology_concept_holders) - self.grippers: List[GripperState] = grippers - self.motions: List[Arms] = motions + self.grippers: List[Arms] = grippers + self.motions: List[GripperState] = motions if self.soma: self.init_ontology_concepts({"setting_gripper": self.soma.SettingGripper}) @@ -220,7 +220,7 @@ def __init__(self, ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description self.arms: List[Arms] = arms self.grasps: List[Grasp] = grasps - self.knowledge_conditions = [GraspableAspect(self.object_designator_description) & ReachableAspect(self.object_designator_description)] + self.knowledge_conditions = GraspableAspect(self.object_designator_description) & ReachableAspect(self.object_designator_description) if self.soma: self.init_ontology_concepts({"picking_up": self.soma.PickingUp}) From eb6749acadce7f0e34aab16445ed214bd1949f44 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 16 Jul 2024 12:45:44 +0200 Subject: [PATCH 12/57] [Knowledge engine] Aspects and resolution of Aspects --- src/pycram/datastructures/aspects.py | 79 +++++++++++++++++-- src/pycram/datastructures/knowledge_engine.py | 42 ++++++++-- src/pycram/datastructures/knowledge_source.py | 8 +- src/pycram/designator.py | 11 ++- 4 files changed, 126 insertions(+), 14 deletions(-) diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/aspects.py index e611f2220..28ca8d2a5 100644 --- a/src/pycram/datastructures/aspects.py +++ b/src/pycram/datastructures/aspects.py @@ -7,7 +7,7 @@ from typing_extensions import List, Iterable from anytree import NodeMixin, PreOrderIter, Node -from ..designator import ObjectDesignatorDescription +from ..designator import ObjectDesignatorDescription, ActionDesignatorDescription class Aspect(NodeMixin): @@ -16,6 +16,11 @@ class Aspect(NodeMixin): Aspects can be combined using logical operators to create complex pre-conditions, the resulting expression is a datastructure of a tree. """ + resolved_aspect: Aspect + """ + Reference to the actual implementation of the aspect function in the KnowledgeSource. This reference is used when + evaluating the tree structure of aspects. + """ def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None): super().__init__() @@ -58,7 +63,15 @@ def __invert__(self): return NotAspect(self) @abstractmethod - def __call__(self, *args, **kwargs): + def __call__(self, designator: ActionDesignatorDescription, *args, **kwargs) -> bool: + """ + Abstract method that is called when the aspect is evaluated. This method must be implemented in the subclass. + + :param designator: The designator for which this aspect is part of the knowledge pre-condition + :param args: A list of arguments + :param kwargs: A dictionary of keyword arguments + :return: True if the aspect is fulfilled, False otherwise + """ raise NotImplementedError("The __call__ method must be implemented in the subclass") @@ -68,6 +81,8 @@ class AspectOperator(Aspect): use Aspects as part of a tree structure. Furthermore, there is a method to simplify the tree structure by merging Nodes of the same type. """ + aspects: List[Aspect] + def __init__(self, aspects: List[Aspect]): """ Initialize the AspectOperator with a list of aspects. The parent of this node is None, therefore the node is @@ -77,6 +92,7 @@ def __init__(self, aspects: List[Aspect]): """ super().__init__(parent=None, children=aspects) self.aspects = aspects + # self.resolved_aspects = [] def simplify(self): """ @@ -142,7 +158,12 @@ def __call__(self, *args, **kwargs) -> bool: """ result = True for child in self.children: - result = result and child(*args, **kwargs) + # Child is an Aspect and the resolved function should be called + if child.is_leaf: + result = result and child(*args, **kwargs) + # Child is an AspectOperator + else: + child(*args, **kwargs) if not result: return False @@ -175,7 +196,12 @@ def __call__(self, *args, **kwargs) -> bool: """ result = False for child in self.children: - result = result or child(*args, **kwargs) + # Child is an Aspect and the resolved function should be called + if child.is_leaf: + result = result or child(*args, **kwargs) + # Child is an AspectOperator + else: + result = child(*args, **kwargs) if result: return True @@ -203,6 +229,8 @@ def __call__(self, *args, **kwargs) -> bool: :param kwargs: A dictionary of keyword arguments to pass to the child :return: The negation of the result of the child """ + if self.children[0].is_leaf: + return not self.children[0].resolved_aspect(*args, **kwargs) return not self.children[0](*args, **kwargs) @@ -215,7 +243,7 @@ def __init__(self, object_designator: ObjectDesignatorDescription): def reachable(self, pose: Pose) -> bool: raise NotImplementedError - def __call__(self, *args, **kwargs): + def __call__(self, designator, *args, **kwargs): return self.reachable(*args, **kwargs) @@ -228,5 +256,44 @@ def __init__(self, object_designator: ObjectDesignatorDescription): def graspable(self) -> bool: raise NotImplementedError - def __call__(self, *args, **kwargs): + def __call__(self, designator, *args, **kwargs): return self.graspable(*args, **kwargs) + + +class SpaceIsFreeAspect(Aspect): + + def __init__(self, object_designator: ObjectDesignatorDescription): + super().__init__(None, None) + self.object_designator = object_designator + + def is_occupied(self) -> bool: + raise NotImplementedError + + def __call__(self, designator, *args, **kwargs): + return self.is_occupied(*args, **kwargs) + + +class GripperIsFreeAspect(Aspect): + + def __init__(self, object_designator: ObjectDesignatorDescription): + super().__init__(None, None) + self.object_designator = object_designator + + def is_free(self) -> bool: + raise NotImplementedError + + def __call__(self, designator, *args, **kwargs): + return self.is_free(*args, **kwargs) + + +class VisibleAspect(Aspect): + + def __init__(self, object_designator: ObjectDesignatorDescription): + super().__init__(None, None) + self.object_designator = object_designator + + def is_visible(self) -> bool: + raise NotImplementedError + + def __call__(self, designator, *args, **kwargs): + return self.is_visible(*args, **kwargs) diff --git a/src/pycram/datastructures/knowledge_engine.py b/src/pycram/datastructures/knowledge_engine.py index ae6a1ed82..b750fddc5 100644 --- a/src/pycram/datastructures/knowledge_engine.py +++ b/src/pycram/datastructures/knowledge_engine.py @@ -1,7 +1,9 @@ import inspect import rospy +from anytree import PreOrderIter +from .aspects import Aspect from .knowledge_source import KnowledgeSource from ..designator import DesignatorDescription, ActionDesignatorDescription from typing_extensions import Type, Callable, List @@ -33,18 +35,21 @@ def __init__(self): def init_sources(self): """ - Initialize all knowledge sources + Initialize all knowledge sources from the available subclasses of KnowledgeSource """ # Class reference to all knowledge sources sources = KnowledgeSource.__subclasses__() for src in sources: - self.knowledge_sources.append(src()) + 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): """ - Update all knowledge sources, this will check if the sources are still and if new sources have become available. + 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") @@ -57,9 +62,25 @@ def query(self, designator: Type[ActionDesignatorDescription]): :return: """ + self.update_sources() + conditions = designator.knowledge_conditions conditions(designator) + def resolve_aspects(self, aspects: Aspect): + """ + Traverses the tree of aspects and resolves the aspect functions to the corresponding function in the knowledge + source. + + :param aspects: Root node of the tree of aspects + """ + for child in PreOrderIter(aspects): + if child.is_leaf: + source = self.find_source_for_aspect(child) + resolved_aspect_function = source.__getattribute__( + [fun for fun in child.__class__.__dict__.keys() if not fun.startswith("__")][0]) + child.resolved_aspect = resolved_aspect_function + def update(self): """ Update the knowledge sources with new information contained in a designator @@ -87,10 +108,21 @@ def call_source(self, query_function: Callable, *args, **kwargs): """ self.update_sources() for source in self.knowledge_sources: - print(source) if (query_function.__name__ in list(source.__class__.__dict__.keys()) and source.is_connected): source_query_function = getattr(source, query_function.__name__) return source_query_function(*args, **kwargs) - raise KnowledgeNotAvailable(f"Query function {query_function.__name__} is not available in any connected knowledge source") + raise KnowledgeNotAvailable( + f"Query function {query_function.__name__} is not available in any connected knowledge source") + + def find_source_for_aspect(self, aspect: Type[Aspect]): + """ + Find the source for the given aspect + :param aspect: The aspect for which to find the source. + :return: Source that can provide the aspect. + """ + for source in self.knowledge_sources: + if (aspect.__class__ in list(source.__class__.__bases__) + and source.is_connected): + return source diff --git a/src/pycram/datastructures/knowledge_source.py b/src/pycram/datastructures/knowledge_source.py index d3861d031..de426ba0f 100644 --- a/src/pycram/datastructures/knowledge_source.py +++ b/src/pycram/datastructures/knowledge_source.py @@ -46,6 +46,13 @@ def connect(self) -> bool: """ 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}" @@ -61,7 +68,6 @@ def query_pose_for_object(self, designator: DesignatorDescription) -> Designator """ pass - def query_grasp_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: """ Query the grasp for an object from the knowledge source diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 8a7759531..758b4d86c 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -25,7 +25,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 @@ -379,12 +379,18 @@ def get_default_ontology_concept(self) -> owlready2.Thing | None: """ return self.ontology_concept_holders[0].ontology_concept if self.ontology_concept_holders else None + class ActionDesignatorDescription(DesignatorDescription, Language): """ Abstract class for action designator descriptions. Descriptions hold possible parameter ranges for action designators. """ + knowledge_conditions = None + """ + Knowledge condition that have to be fulfilled before executing the action. + """ + @dataclass class Action: """ @@ -464,6 +470,7 @@ def __init__(self, resolver=None, ontology_concept_holders: Optional[List[Ontolo Language.__init__(self) from .ontology.ontology import OntologyManager self.soma = OntologyManager().soma + self.knowledge_conditions = None def ground(self) -> Action: """Fill all missing parameters and chose plan to execute. """ @@ -630,7 +637,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: """ Returns the adjusted target pose based on special knowledge for "grasp front". From 646d0d2f663c32d88c6312ef6c3cc8a5eac35d08 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 16 Jul 2024 15:53:32 +0200 Subject: [PATCH 13/57] [facts knowledge] Reference inmplementation of reasoning --- src/pycram/datastructures/aspects.py | 53 ++++++++++------------- src/pycram/knowledge/facts_knowledge.py | 52 ++++++++++++++++++---- src/pycram/knowledge/knowrob_knowledge.py | 7 ++- src/pycram/world_concepts/world_object.py | 4 -- 4 files changed, 68 insertions(+), 48 deletions(-) diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/aspects.py index 28ca8d2a5..523092e54 100644 --- a/src/pycram/datastructures/aspects.py +++ b/src/pycram/datastructures/aspects.py @@ -3,11 +3,13 @@ from abc import abstractmethod +from .enums import Arms from .pose import Pose from typing_extensions import List, Iterable from anytree import NodeMixin, PreOrderIter, Node from ..designator import ObjectDesignatorDescription, ActionDesignatorDescription +from ..world_concepts.world_object import Object class Aspect(NodeMixin): @@ -61,18 +63,18 @@ def __invert__(self): :return: A NotAspect containing this aspect """ return NotAspect(self) - - @abstractmethod - def __call__(self, designator: ActionDesignatorDescription, *args, **kwargs) -> bool: - """ - Abstract method that is called when the aspect is evaluated. This method must be implemented in the subclass. - - :param designator: The designator for which this aspect is part of the knowledge pre-condition - :param args: A list of arguments - :param kwargs: A dictionary of keyword arguments - :return: True if the aspect is fulfilled, False otherwise - """ - raise NotImplementedError("The __call__ method must be implemented in the subclass") + # + # @abstractmethod + # def __call__(self, designator: ActionDesignatorDescription, *args, **kwargs) -> bool: + # """ + # Abstract method that is called when the aspect is evaluated. This method must be implemented in the subclass. + # + # :param designator: The designator for which this aspect is part of the knowledge pre-condition + # :param args: A list of arguments + # :param kwargs: A dictionary of keyword arguments + # :return: True if the aspect is fulfilled, False otherwise + # """ + # raise NotImplementedError("The __call__ method must be implemented in the subclass") class AspectOperator(Aspect): @@ -243,9 +245,6 @@ def __init__(self, object_designator: ObjectDesignatorDescription): def reachable(self, pose: Pose) -> bool: raise NotImplementedError - def __call__(self, designator, *args, **kwargs): - return self.reachable(*args, **kwargs) - class GraspableAspect(Aspect): @@ -253,12 +252,10 @@ def __init__(self, object_designator: ObjectDesignatorDescription): super().__init__(None, None) self.object_designator = object_designator - def graspable(self) -> bool: + @abstractmethod + def graspable(self, obj: Object) -> bool: raise NotImplementedError - def __call__(self, designator, *args, **kwargs): - return self.graspable(*args, **kwargs) - class SpaceIsFreeAspect(Aspect): @@ -266,12 +263,10 @@ def __init__(self, object_designator: ObjectDesignatorDescription): super().__init__(None, None) self.object_designator = object_designator - def is_occupied(self) -> bool: + @abstractmethod + def space_is_free(self, pose: Pose) -> bool: raise NotImplementedError - def __call__(self, designator, *args, **kwargs): - return self.is_occupied(*args, **kwargs) - class GripperIsFreeAspect(Aspect): @@ -279,12 +274,10 @@ def __init__(self, object_designator: ObjectDesignatorDescription): super().__init__(None, None) self.object_designator = object_designator - def is_free(self) -> bool: + @abstractmethod + def gripper_is_free(self, gripper: Arms) -> bool: raise NotImplementedError - def __call__(self, designator, *args, **kwargs): - return self.is_free(*args, **kwargs) - class VisibleAspect(Aspect): @@ -292,8 +285,6 @@ def __init__(self, object_designator: ObjectDesignatorDescription): super().__init__(None, None) self.object_designator = object_designator - def is_visible(self) -> bool: + @abstractmethod + def is_visible(self, obj: Object) -> bool: raise NotImplementedError - - def __call__(self, designator, *args, **kwargs): - return self.is_visible(*args, **kwargs) diff --git a/src/pycram/knowledge/facts_knowledge.py b/src/pycram/knowledge/facts_knowledge.py index 7e834e7de..9681c8819 100644 --- a/src/pycram/knowledge/facts_knowledge.py +++ b/src/pycram/knowledge/facts_knowledge.py @@ -1,8 +1,17 @@ +import numpy as np + +from ..datastructures.enums import Arms from ..datastructures.knowledge_source import KnowledgeSource, QueryKnowledge, UpdateKnowledge -from ..designator import DesignatorDescription +from ..datastructures.aspects import ReachableAspect, GraspableAspect, GripperIsFreeAspect +from ..datastructures.pose import Pose +from ..datastructures.world import World, UseProspectionWorld +from ..robot_description import RobotDescription +from ..world_concepts.world_object import Object +from ..world_reasoning import visible +from ..costmaps import OccupancyCostmap -class FactsKnowledge(KnowledgeSource, QueryKnowledge, UpdateKnowledge): +class FactsKnowledge(KnowledgeSource, ReachableAspect, GraspableAspect, GripperIsFreeAspect): """ Knowledge source for hard coded facts, this knowledge source acts as a fallback if no other knowledge source is available. @@ -21,14 +30,39 @@ def is_connected(self) -> bool: def connect(self): pass - def query(self, designator: DesignatorDescription) -> DesignatorDescription: - """ - Query the knowledge source for the facts - """ + def clear_state(self) -> None: pass - def query_pose_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: + def reachable(self, pose: Pose) -> bool: pass - def query_grasp_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: - pass + def graspable(self, obj: Object) -> bool: + with UseProspectionWorld(): + pro_obj = World.current_world.get_prospection_object_for_object(obj) + 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.] + + return + + def space_is_free(self, pose: Pose) -> bool: + 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 np.sum(origin_map) > 400 * 0.9 + + def gripper_is_free(self, gripper: Arms) -> bool: + 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 True + return False + + def is_visible(self, obj: Object) -> bool: + cam_pose = World.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) + return visible(obj, cam_pose) + + diff --git a/src/pycram/knowledge/knowrob_knowledge.py b/src/pycram/knowledge/knowrob_knowledge.py index a01f79bf0..cf448a34a 100644 --- a/src/pycram/knowledge/knowrob_knowledge.py +++ b/src/pycram/knowledge/knowrob_knowledge.py @@ -23,13 +23,12 @@ def is_available(self) -> bool: def is_connected(self) -> bool: return self.prolog_client is not None - def connect(self): + def connect(self) -> bool: if self.is_available: self.prolog_client = Prolog() self.prolog_client.once(f"tripledb_load('package://iai_apartment/owl/iai-apartment.owl').") - - def query(self, designator: DesignatorDescription) -> DesignatorDescription: - pass + return True def query_pose_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: + print("test") result = self.prolog_client.once(f"") diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index ac1b77e41..d363d75a0 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -80,10 +80,6 @@ def __init__(self, name: str, obj_type: ObjectType, path: str, self.tf_frame = ((self.prospection_world_prefix if self.world.is_prospection_world else "") + f"{self.name}") - # if robot_description is not None: - # if self.description.name == robot_description.name: - # self.world.set_robot_if_not_set(self) - self._init_joint_name_and_id_map() self._init_link_name_and_id_map() From c207645f85218f40d901d85d73c0ad1bd2f6ff02 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 19 Jul 2024 10:31:39 +0200 Subject: [PATCH 14/57] [Knowledge engine] Variables in Aspects --- src/pycram/datastructures/aspects.py | 112 ++++++++++++------ src/pycram/datastructures/knowledge_engine.py | 2 +- src/pycram/knowledge/facts_knowledge.py | 23 ++-- 3 files changed, 89 insertions(+), 48 deletions(-) diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/aspects.py index 523092e54..8cc8587a3 100644 --- a/src/pycram/datastructures/aspects.py +++ b/src/pycram/datastructures/aspects.py @@ -5,28 +5,45 @@ from .enums import Arms from .pose import Pose -from typing_extensions import List, Iterable +from typing_extensions import List, Iterable, Dict, Any, Callable from anytree import NodeMixin, PreOrderIter, Node from ..designator import ObjectDesignatorDescription, ActionDesignatorDescription from ..world_concepts.world_object import Object +def managed_io(func: Callable) -> Callable: + def wrapper(*args, **kwargs): + print("test") + return func(*args, **kwargs) + + return wrapper + + class Aspect(NodeMixin): """ Parent class to represent a semantic aspect 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. """ - resolved_aspect: Aspect + resolved_aspect_function: Callable """ Reference to the actual implementation of the aspect function in the KnowledgeSource. This reference is used when evaluating the tree structure of aspects. """ + variables: Dict[str, Any] + """ + Dictionary of variables and their values which are used in the aspect tree. This dictionary is only to be used in + the root node. + """ - def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None): + def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None, + input: str = None, output: str = None): super().__init__() self.parent = parent + self.input = input + self.output = output + self.variables = {} if children: self.children = children @@ -63,18 +80,30 @@ def __invert__(self): :return: A NotAspect containing this aspect """ return NotAspect(self) - # - # @abstractmethod - # def __call__(self, designator: ActionDesignatorDescription, *args, **kwargs) -> bool: - # """ - # Abstract method that is called when the aspect is evaluated. This method must be implemented in the subclass. - # - # :param designator: The designator for which this aspect is part of the knowledge pre-condition - # :param args: A list of arguments - # :param kwargs: A dictionary of keyword arguments - # :return: True if the aspect is fulfilled, False otherwise - # """ - # raise NotImplementedError("The __call__ method must be implemented in the subclass") + + def manage_io(self, func: Callable, *args, **kwargs) -> bool: + """ + Manages the input and output variables across the whole aspect tree. If the aspect has an input variable, the + value of this variable will be taken from the variables dictionary of the root node. If an output is defined, + the result of the function will be stored in the variables dictionary of the root node. + + :param func: Aspect 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 + """ + if self.input: + if self.input not in self.root.variables.keys(): + raise AttributeError(f"Variable {self.input} not found in variables") + input_var = self.root.variables[self.input] + result = func(input_var) + if self.output: + self.variables[self.output] = result + elif self.output: + result = func(*args, **kwargs) + self.variables[self.output] = result + return result + return func(*args, **kwargs) class AspectOperator(Aspect): @@ -121,16 +150,6 @@ def merge_nodes(node1: Node, node2: Node) -> None: node2.parent = None node1.children = node2.children + node1.children - def __call__(self, *args, **kwargs): - """ - Implementation of the abstract method, since this class only acts as a parent class for logical operators there - is no implementation here. - - :param args: A list of arguments - :param kwargs: A dictionary of keyword arguments - """ - pass - class AndAspect(AspectOperator): """ @@ -168,6 +187,7 @@ def __call__(self, *args, **kwargs) -> bool: child(*args, **kwargs) if not result: return False + return result class OrAspect(AspectOperator): @@ -206,6 +226,7 @@ def __call__(self, *args, **kwargs) -> bool: result = child(*args, **kwargs) if result: return True + return result class NotAspect(AspectOperator): @@ -231,60 +252,73 @@ def __call__(self, *args, **kwargs) -> bool: :param kwargs: A dictionary of keyword arguments to pass to the child :return: The negation of the result of the child """ - if self.children[0].is_leaf: - return not self.children[0].resolved_aspect(*args, **kwargs) return not self.children[0](*args, **kwargs) class ReachableAspect(Aspect): - def __init__(self, object_designator: ObjectDesignatorDescription): - super().__init__(None, None) - self.object_designator = object_designator + def __init__(self, pose: Pose, input: str = None, output: str = None): + super().__init__(None, None, input, output) + self.target_pose = pose def reachable(self, pose: Pose) -> bool: raise NotImplementedError + def __call__(self, *args, **kwargs): + return self.manage_io(self.resolved_aspect_function, self.target_pose) + class GraspableAspect(Aspect): - def __init__(self, object_designator: ObjectDesignatorDescription): - super().__init__(None, None) + def __init__(self, object_designator: ObjectDesignatorDescription, input: str = None, output: str = None): + super().__init__(None, None, input, output) self.object_designator = object_designator @abstractmethod def graspable(self, obj: Object) -> bool: raise NotImplementedError + def __call__(self, *args, **kwargs): + return self.manage_io(self.resolved_aspect_function, self.object_designator) + class SpaceIsFreeAspect(Aspect): - def __init__(self, object_designator: ObjectDesignatorDescription): - super().__init__(None, None) + def __init__(self, object_designator: ObjectDesignatorDescription, input: str = None, output: str = None): + super().__init__(None, None, input, output) self.object_designator = object_designator @abstractmethod def space_is_free(self, pose: Pose) -> bool: raise NotImplementedError + def __call__(self, *args, **kwargs): + return self.manage_io(self.resolved_aspect_function, self.object_designator) + class GripperIsFreeAspect(Aspect): - def __init__(self, object_designator: ObjectDesignatorDescription): - super().__init__(None, None) - self.object_designator = object_designator + def __init__(self, input: str = None, output: str = None): + super().__init__(None, None, input, output) @abstractmethod def gripper_is_free(self, gripper: Arms) -> bool: raise NotImplementedError + def __call__(self, *args, **kwargs): + return self.manage_io(self.resolved_aspect_function) + class VisibleAspect(Aspect): - def __init__(self, object_designator: ObjectDesignatorDescription): - super().__init__(None, None) + def __init__(self, object_designator: ObjectDesignatorDescription, input: str = None, output: str = None): + super().__init__(None, None, input, output) self.object_designator = object_designator @abstractmethod def is_visible(self, obj: Object) -> bool: raise NotImplementedError + + def __call__(self, *args, **kwargs): + return self.manage_io(self.resolved_aspect_function, self.object_designator) + diff --git a/src/pycram/datastructures/knowledge_engine.py b/src/pycram/datastructures/knowledge_engine.py index b750fddc5..ec2862c11 100644 --- a/src/pycram/datastructures/knowledge_engine.py +++ b/src/pycram/datastructures/knowledge_engine.py @@ -79,7 +79,7 @@ def resolve_aspects(self, aspects: Aspect): source = self.find_source_for_aspect(child) resolved_aspect_function = source.__getattribute__( [fun for fun in child.__class__.__dict__.keys() if not fun.startswith("__")][0]) - child.resolved_aspect = resolved_aspect_function + child.resolved_aspect_function = resolved_aspect_function def update(self): """ diff --git a/src/pycram/knowledge/facts_knowledge.py b/src/pycram/knowledge/facts_knowledge.py index 9681c8819..a694e4cd0 100644 --- a/src/pycram/knowledge/facts_knowledge.py +++ b/src/pycram/knowledge/facts_knowledge.py @@ -1,10 +1,12 @@ import numpy as np -from ..datastructures.enums import Arms +from ..datastructures.enums import Arms, ObjectType from ..datastructures.knowledge_source import KnowledgeSource, QueryKnowledge, UpdateKnowledge from ..datastructures.aspects import ReachableAspect, GraspableAspect, GripperIsFreeAspect from ..datastructures.pose import Pose from ..datastructures.world import World, UseProspectionWorld +from ..designators.location_designator import CostmapLocation +from ..designators.object_designator import BelieveObject from ..robot_description import RobotDescription from ..world_concepts.world_object import Object from ..world_reasoning import visible @@ -16,6 +18,7 @@ class FactsKnowledge(KnowledgeSource, ReachableAspect, GraspableAspect, GripperI 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) @@ -34,7 +37,10 @@ def clear_state(self) -> None: pass def reachable(self, pose: Pose) -> bool: - pass + robot_desig = BelieveObject(types=[ObjectType.ROBOT]) + c = CostmapLocation(pose, reachable_for=robot_desig.resolve()).resolve() + if c.pose: + return True def graspable(self, obj: Object) -> bool: with UseProspectionWorld(): @@ -45,13 +51,16 @@ def graspable(self, obj: Object) -> bool: 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.] - - return + 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_x or dist > obj_y or dist > obj_z: + return True + return False def space_is_free(self, pose: Pose) -> bool: 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] + origin_map = om.map[200 // 2 - 10: 200 // 2 + 10, 200 // 2 - 10: 200 // 2 + 10] return np.sum(origin_map) > 400 * 0.9 def gripper_is_free(self, gripper: Arms) -> bool: @@ -64,5 +73,3 @@ def gripper_is_free(self, gripper: Arms) -> bool: def is_visible(self, obj: Object) -> bool: cam_pose = World.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) return visible(obj, cam_pose) - - From 08d47fb44267677fddad8b51b5c43fc511ee039e Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 19 Jul 2024 12:23:16 +0200 Subject: [PATCH 15/57] [knowledge] Refactor of knowledge related files --- src/pycram/datastructures/aspects.py | 15 +++++++------ src/pycram/datastructures/conditions.py | 5 ----- .../knowledge/decision_trees/__init__.py | 0 .../knowledge/decision_trees/object_tree.py | 12 ---------- .../knowledge_engine.py | 12 +++++----- .../knowledge_source.py | 0 .../knowledge/knowledge_sources/__init__.py | 7 ++++++ .../facts_knowledge.py | 22 +++++++++---------- .../food_cutting_knowledge.py | 2 +- .../knowrob_knowledge.py | 4 ++-- 10 files changed, 35 insertions(+), 44 deletions(-) delete mode 100644 src/pycram/datastructures/conditions.py delete mode 100644 src/pycram/knowledge/decision_trees/__init__.py delete mode 100644 src/pycram/knowledge/decision_trees/object_tree.py rename src/pycram/{datastructures => knowledge}/knowledge_engine.py (91%) rename src/pycram/{datastructures => knowledge}/knowledge_source.py (100%) create mode 100644 src/pycram/knowledge/knowledge_sources/__init__.py rename src/pycram/knowledge/{ => knowledge_sources}/facts_knowledge.py (79%) rename src/pycram/knowledge/{ => knowledge_sources}/food_cutting_knowledge.py (98%) rename src/pycram/knowledge/{ => knowledge_sources}/knowrob_knowledge.py (87%) diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/aspects.py index 8cc8587a3..6e0b4739d 100644 --- a/src/pycram/datastructures/aspects.py +++ b/src/pycram/datastructures/aspects.py @@ -5,7 +5,7 @@ from .enums import Arms from .pose import Pose -from typing_extensions import List, Iterable, Dict, Any, Callable +from typing_extensions import List, Iterable, Dict, Any, Callable, Type from anytree import NodeMixin, PreOrderIter, Node from ..designator import ObjectDesignatorDescription, ActionDesignatorDescription @@ -26,7 +26,7 @@ class Aspect(NodeMixin): Aspects can be combined using logical operators to create complex pre-conditions, the resulting expression is a datastructure of a tree. """ - resolved_aspect_function: Callable + resolved_aspect_instance: Type[Aspect] """ Reference to the actual implementation of the aspect function in the KnowledgeSource. This reference is used when evaluating the tree structure of aspects. @@ -261,11 +261,12 @@ def __init__(self, pose: Pose, input: str = None, output: str = None): super().__init__(None, None, input, output) self.target_pose = pose + @abstractmethod def reachable(self, pose: Pose) -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): - return self.manage_io(self.resolved_aspect_function, self.target_pose) + return self.manage_io(self.resolved_aspect_instance.reachable, self.target_pose) class GraspableAspect(Aspect): @@ -279,7 +280,7 @@ def graspable(self, obj: Object) -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): - return self.manage_io(self.resolved_aspect_function, self.object_designator) + return self.manage_io(self.resolved_aspect_instance.graspable, self.object_designator) class SpaceIsFreeAspect(Aspect): @@ -293,7 +294,7 @@ def space_is_free(self, pose: Pose) -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): - return self.manage_io(self.resolved_aspect_function, self.object_designator) + return self.manage_io(self.resolved_aspect_instance.space_is_free, self.object_designator) class GripperIsFreeAspect(Aspect): @@ -306,7 +307,7 @@ def gripper_is_free(self, gripper: Arms) -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): - return self.manage_io(self.resolved_aspect_function) + return self.manage_io(self.resolved_aspect_instance.gripper_is_free, *args, **kwargs) class VisibleAspect(Aspect): @@ -320,5 +321,5 @@ def is_visible(self, obj: Object) -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): - return self.manage_io(self.resolved_aspect_function, self.object_designator) + return self.manage_io(self.resolved_aspect_instance.is_visible, self.object_designator) diff --git a/src/pycram/datastructures/conditions.py b/src/pycram/datastructures/conditions.py deleted file mode 100644 index dc1541ffb..000000000 --- a/src/pycram/datastructures/conditions.py +++ /dev/null @@ -1,5 +0,0 @@ -from .aspects import ReachableAspect - - -def is_reachable(entity: ReachableAspect) -> bool: - entity.pose = None diff --git a/src/pycram/knowledge/decision_trees/__init__.py b/src/pycram/knowledge/decision_trees/__init__.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/src/pycram/knowledge/decision_trees/object_tree.py b/src/pycram/knowledge/decision_trees/object_tree.py deleted file mode 100644 index 2273c247b..000000000 --- a/src/pycram/knowledge/decision_trees/object_tree.py +++ /dev/null @@ -1,12 +0,0 @@ -from ...datastructures.decision_tree import Decision, Query -from anytree import RenderTree - -object_tree = None - -context = (Decision(lambda designator: designator.type == "context") - + Decision(lambda designator: designator.type == "object") - - Query(lambda designator: object_tree.query_grasp_for_object(designator))) - -print(RenderTree(context)) - - diff --git a/src/pycram/datastructures/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py similarity index 91% rename from src/pycram/datastructures/knowledge_engine.py rename to src/pycram/knowledge/knowledge_engine.py index ec2862c11..50bea03b1 100644 --- a/src/pycram/datastructures/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -1,14 +1,14 @@ -import inspect - import rospy from anytree import PreOrderIter -from .aspects import Aspect +from ..datastructures.aspects import Aspect from .knowledge_source import KnowledgeSource from ..designator import DesignatorDescription, ActionDesignatorDescription from typing_extensions import Type, Callable, List from ..plan_failures import KnowledgeNotAvailable +# This import is needed since the subclasses of KnowledgeSource need to be imported to be known at runtime +from .knowledge_sources import * class KnowledgeEngine: @@ -77,9 +77,9 @@ def resolve_aspects(self, aspects: Aspect): for child in PreOrderIter(aspects): if child.is_leaf: source = self.find_source_for_aspect(child) - resolved_aspect_function = source.__getattribute__( - [fun for fun in child.__class__.__dict__.keys() if not fun.startswith("__")][0]) - child.resolved_aspect_function = resolved_aspect_function + # resolved_aspect_function = source.__getattribute__( + # [fun for fun in child.__class__.__dict__.keys() if not fun.startswith("__")][0]) + child.resolved_aspect_instance = source def update(self): """ diff --git a/src/pycram/datastructures/knowledge_source.py b/src/pycram/knowledge/knowledge_source.py similarity index 100% rename from src/pycram/datastructures/knowledge_source.py rename to src/pycram/knowledge/knowledge_source.py 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/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py similarity index 79% rename from src/pycram/knowledge/facts_knowledge.py rename to src/pycram/knowledge/knowledge_sources/facts_knowledge.py index a694e4cd0..b6dae7cd6 100644 --- a/src/pycram/knowledge/facts_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -1,16 +1,16 @@ import numpy as np -from ..datastructures.enums import Arms, ObjectType -from ..datastructures.knowledge_source import KnowledgeSource, QueryKnowledge, UpdateKnowledge -from ..datastructures.aspects import ReachableAspect, GraspableAspect, GripperIsFreeAspect -from ..datastructures.pose import Pose -from ..datastructures.world import World, UseProspectionWorld -from ..designators.location_designator import CostmapLocation -from ..designators.object_designator import BelieveObject -from ..robot_description import RobotDescription -from ..world_concepts.world_object import Object -from ..world_reasoning import visible -from ..costmaps import OccupancyCostmap +from ...datastructures.enums import Arms, ObjectType +from ..knowledge_source import KnowledgeSource +from ...datastructures.aspects import ReachableAspect, GraspableAspect, GripperIsFreeAspect +from ...datastructures.pose import Pose +from ...datastructures.world import World, UseProspectionWorld +from ...designators.location_designator import CostmapLocation +from ...designators.object_designator import BelieveObject +from ...robot_description import RobotDescription +from ...world_concepts.world_object import Object +from ...world_reasoning import visible +from ...costmaps import OccupancyCostmap class FactsKnowledge(KnowledgeSource, ReachableAspect, GraspableAspect, GripperIsFreeAspect): diff --git a/src/pycram/knowledge/food_cutting_knowledge.py b/src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py similarity index 98% rename from src/pycram/knowledge/food_cutting_knowledge.py rename to src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py index 205473d3e..73726f116 100644 --- a/src/pycram/knowledge/food_cutting_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py @@ -1,4 +1,4 @@ -from ..datastructures.knowledge_source import KnowledgeSource +from ..knowledge_source import KnowledgeSource from rdflib import Graph, Namespace from rdflib.namespace import OWL, RDFS diff --git a/src/pycram/knowledge/knowrob_knowledge.py b/src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py similarity index 87% rename from src/pycram/knowledge/knowrob_knowledge.py rename to src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py index cf448a34a..853afed8e 100644 --- a/src/pycram/knowledge/knowrob_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py @@ -1,8 +1,8 @@ import rospy -from ..datastructures.knowledge_source import KnowledgeSource, QueryKnowledge, UpdateKnowledge +from ..knowledge_source import KnowledgeSource, QueryKnowledge, UpdateKnowledge import rosservice -from ..designator import DesignatorDescription +from ...designator import DesignatorDescription try: from rosprolog_client import Prolog except ModuleNotFoundError as e: From e20cc2734c4f8e9b1169279e369d34eb594191e1 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 5 Aug 2024 16:49:16 +0200 Subject: [PATCH 16/57] [Aspects] Small fixes and better exception return --- src/pycram/datastructures/aspects.py | 34 +++++++++++++++++------- src/pycram/knowledge/knowledge_engine.py | 4 +-- src/pycram/plan_failures.py | 14 ++++++++++ 3 files changed, 41 insertions(+), 11 deletions(-) diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/aspects.py index 6e0b4739d..55633569d 100644 --- a/src/pycram/datastructures/aspects.py +++ b/src/pycram/datastructures/aspects.py @@ -9,6 +9,7 @@ from anytree import NodeMixin, PreOrderIter, Node from ..designator import ObjectDesignatorDescription, ActionDesignatorDescription +from ..plan_failures import ObjectNotVisible, ManipulationPoseUnreachable, NavigationGoalInCollision, ObjectUnfetchable from ..world_concepts.world_object import Object @@ -266,7 +267,10 @@ def reachable(self, pose: Pose) -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): - return self.manage_io(self.resolved_aspect_instance.reachable, self.target_pose) + res = self.manage_io(self.resolved_aspect_instance.reachable, self.target_pose) + if not res: + raise ManipulationPoseUnreachable(f"Pose {self.target_pose} is not reachable") + return res class GraspableAspect(Aspect): @@ -280,34 +284,44 @@ def graspable(self, obj: Object) -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): - return self.manage_io(self.resolved_aspect_instance.graspable, self.object_designator) + res = self.manage_io(self.resolved_aspect_instance.graspable, self.object_designator) + if not res: + raise ObjectUnfetchable(f"Object {self.object_designator} is not graspable") + return res class SpaceIsFreeAspect(Aspect): - def __init__(self, object_designator: ObjectDesignatorDescription, input: str = None, output: str = None): + def __init__(self, pose: Pose, input: str = None, output: str = None): super().__init__(None, None, input, output) - self.object_designator = object_designator + self.pose = pose @abstractmethod def space_is_free(self, pose: Pose) -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): - return self.manage_io(self.resolved_aspect_instance.space_is_free, self.object_designator) + res = self.manage_io(self.resolved_aspect_instance.space_is_free, self.pose) + if not res: + raise NavigationGoalInCollision(f"Pose {self.pose} is not free") + return res class GripperIsFreeAspect(Aspect): - def __init__(self, input: str = None, output: str = None): + def __init__(self, gripper: Arms, input: str = None, output: str = None): super().__init__(None, None, input, output) + self.gripper = gripper @abstractmethod def gripper_is_free(self, gripper: Arms) -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): - return self.manage_io(self.resolved_aspect_instance.gripper_is_free, *args, **kwargs) + res = self.manage_io(self.resolved_aspect_instance.gripper_is_free, self.gripper) + if not res: + raise ObjectNotVisible(f"Gripper {self.gripper} is not free") + return res class VisibleAspect(Aspect): @@ -321,5 +335,7 @@ def is_visible(self, obj: Object) -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): - return self.manage_io(self.resolved_aspect_instance.is_visible, self.object_designator) - + res = self.manage_io(self.resolved_aspect_instance.is_visible, self.object_designator) + if not res: + raise ObjectNotVisible(f"Object {self.object_designator} is not visible") + return res diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index 50bea03b1..403c379cb 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -64,8 +64,8 @@ def query(self, designator: Type[ActionDesignatorDescription]): """ self.update_sources() - conditions = designator.knowledge_conditions - conditions(designator) + condition = designator.knowledge_condition + condition(designator) def resolve_aspects(self, aspects: Aspect): """ diff --git a/src/pycram/plan_failures.py b/src/pycram/plan_failures.py index 63e48f46a..0511c437b 100644 --- a/src/pycram/plan_failures.py +++ b/src/pycram/plan_failures.py @@ -110,6 +110,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.""" @@ -182,6 +189,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.""" From dba333cb68a2629717d79d9b056a026f6528e486 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 5 Aug 2024 17:12:52 +0200 Subject: [PATCH 17/57] [designator] Removed resolver parameter --- src/pycram/designator.py | 25 ++--- src/pycram/designators/action_designator.py | 105 ++++++++++-------- src/pycram/designators/location_designator.py | 28 ++--- src/pycram/designators/object_designator.py | 18 ++- 4 files changed, 89 insertions(+), 87 deletions(-) diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 758b4d86c..92256ae22 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -328,18 +328,19 @@ class DesignatorDescription(ABC): :ivar resolve: The specialized_designators function to use for this designator, defaults to self.ground """ - def __init__(self, resolver: Optional[Callable] = None, ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): + def __init__(self, ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): """ Create a Designator description. - :param resolver: The grounding method used for the description. The grounding method creates a location instance that matches the description. :param ontology_concept_holders: A list of holders of ontology concepts that the designator is categorized as or associated with """ - if resolver is None: - self.resolve = self.ground + # self.resolve = self.ground self.ontology_concept_holders = [] if ontology_concept_holders is None else ontology_concept_holders + def resolve(self): + return self.ground() + def make_dictionary(self, properties: List[str]): """ Creates a dictionary of this description with only the given properties @@ -386,7 +387,7 @@ class ActionDesignatorDescription(DesignatorDescription, Language): Descriptions hold possible parameter ranges for action designators. """ - knowledge_conditions = None + knowledge_condition = None """ Knowledge condition that have to be fulfilled before executing the action. """ @@ -459,14 +460,13 @@ def insert(self, session: Session, *args, **kwargs) -> ORMAction: return action - def __init__(self, resolver=None, ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): + def __init__(self, ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): """ Base of all action designator descriptions. - :param resolver: An alternative resolver that returns an action designator :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) Language.__init__(self) from .ontology.ontology import OntologyManager self.soma = OntologyManager().soma @@ -516,8 +516,8 @@ class Location: The resolved pose of the location designator. Pose is inherited by all location designator. """ - def __init__(self, resolver=None, ontology_concept_holders: Optional[List[owlready2.Thing]] = None): - super().__init__(resolver, ontology_concept_holders) + def __init__(self, ontology_concept_holders: Optional[List[owlready2.Thing]] = None): + super().__init__(ontology_concept_holders) def ground(self) -> Location: """ @@ -663,16 +663,15 @@ def special_knowledge_adjustment_pose(self, grasp: Grasp, pose: Pose) -> Pose: return pose def __init__(self, names: Optional[List[str]] = None, types: Optional[List[ObjectType]] = None, - resolver: Optional[Callable] = None, ontology_concept_holders: Optional[List[owlready2.Thing]] = None): + ontology_concept_holders: Optional[List[owlready2.Thing]] = None): """ Base of all object designator descriptions. Every object designator has the name and type of the object. :param names: A list of names that could describe the object :param types: A list of types that could represent the object - :param resolver: An alternative specialized_designators that returns an object designator for the list of names and types :param ontology_concept_holders: A list of ontology concepts that the object is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.types: Optional[List[ObjectType]] = types self.names: Optional[List[str]] = names diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index c6eb2a219..9a84dfcc2 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -16,7 +16,8 @@ from .motion_designator import MoveJointsMotion, MoveGripperMotion, MoveArmJointsMotion, MoveTCPMotion, MoveMotion, \ LookingMotion, DetectingMotion, OpeningMotion, ClosingMotion from .object_designator import ObjectDesignatorDescription, BelieveObject, ObjectPart -from ..datastructures.aspects import GraspableAspect, ReachableAspect +from ..datastructures.aspects import GraspableAspect, ReachableAspect, GripperIsFreeAspect, SpaceIsFreeAspect, \ + VisibleAspect from ..local_transformer import LocalTransformer from ..plan_failures import ObjectUnfetchable, ReachabilityFailure # from ..robot_descriptions import robot_description @@ -49,16 +50,15 @@ class MoveTorsoAction(ActionDesignatorDescription): Action Designator for Moving the torso of the robot up and down """ - def __init__(self, positions: List[float], resolver=None, + def __init__(self, positions: List[float], ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): """ Create a designator description to move the torso of the robot up and down. :param positions: List of possible positions of the robots torso, possible position is a float of height in metres - :param resolver: An optional specialized_designators that returns a performable designator for a designator description. :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.positions: List[float] = positions if self.soma: @@ -87,19 +87,25 @@ class SetGripperAction(ActionDesignatorDescription): Set the gripper state of the robot """ - def __init__(self, grippers: List[Arms], motions: List[GripperState], resolver=None, + def __init__(self, grippers: List[Arms], motions: List[GripperState], ontology_concept_holders: Optional[List[Thing]] = None): """ Sets the gripper state, the desired state is given with the motion. Motion can either be 'open' or 'close'. :param grippers: A list of possible grippers :param motions: A list of possible motions - :param resolver: An alternative specialized_designators that returns a performable designator for a designator description :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.grippers: List[Arms] = grippers self.motions: List[GripperState] = motions + if len(self.grippers) == 1: + self.knowledge_condition = GripperIsFreeAspect(self.grippers[0]) + else: + root = GripperIsFreeAspect(self.grippers[0]) + for gripper in grippers[1:]: + root |= GripperIsFreeAspect(gripper) + self.knowledge_condition = root if self.soma: self.init_ontology_concepts({"setting_gripper": self.soma.SettingGripper}) @@ -130,8 +136,8 @@ class ReleaseAction(ActionDesignatorDescription): """ def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, - resolver=None, ontology_concept_holders: Optional[List[Thing]] = None): - super().__init__(resolver, ontology_concept_holders) + ontology_concept_holders: Optional[List[Thing]] = None): + super().__init__(ontology_concept_holders) self.grippers: List[Arms] = grippers self.object_designator_description = object_designator_description @@ -154,8 +160,8 @@ class GripAction(ActionDesignatorDescription): """ def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, - efforts: List[float], resolver=None, ontology_concept_holders: Optional[List[Thing]] = None): - super().__init__(resolver, ontology_concept_holders) + efforts: List[float], ontology_concept_holders: Optional[List[Thing]] = None): + super().__init__( ontology_concept_holders) self.grippers: List[Arms] = grippers self.object_designator_description: ObjectDesignatorDescription = object_designator_description self.efforts: List[float] = efforts @@ -172,16 +178,15 @@ class ParkArmsAction(ActionDesignatorDescription): Park the arms of the robot. """ - def __init__(self, arms: List[Arms], resolver=None, + def __init__(self, arms: List[Arms], ontology_concept_holders: Optional[List[Thing]] = None): """ Moves the arms in the pre-defined parking position. Arms are taken from pycram.enum.Arms :param arms: A list of possible arms, that could be used - :param resolver: An optional specialized_designators that returns a performable designator from the designator description :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.arms: List[Arms] = arms if self.soma: @@ -203,7 +208,7 @@ class PickUpAction(ActionDesignatorDescription): def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], - arms: List[Arms], grasps: List[Grasp], resolver=None, + arms: List[Arms], grasps: List[Grasp], ontology_concept_holders: Optional[List[Thing]] = None): """ Lets the robot pick up an object. The description needs an object designator describing the object that should be @@ -212,15 +217,15 @@ def __init__(self, :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 :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.object_designator_description: Union[ ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description self.arms: List[Arms] = arms self.grasps: List[Grasp] = grasps - self.knowledge_conditions = GraspableAspect(self.object_designator_description) & ReachableAspect(self.object_designator_description) + self.knowledge_condition = GraspableAspect(self.object_designator_description) & ReachableAspect( + self.object_designator_description.resolve().pose) if self.soma: self.init_ontology_concepts({"picking_up": self.soma.PickingUp}) @@ -247,17 +252,16 @@ class PlaceAction(ActionDesignatorDescription): def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], target_locations: List[Pose], - arms: List[Arms], resolver=None, ontology_concept_holders: Optional[List[Thing]] = None): + arms: List[Arms], ontology_concept_holders: Optional[List[Thing]] = None): """ Create an Action Description to place an object :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 :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.object_designator_description: Union[ ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description self.target_locations: List[Pose] = target_locations @@ -283,17 +287,23 @@ class NavigateAction(ActionDesignatorDescription): Navigates the Robot to a position. """ - def __init__(self, target_locations: List[Pose], resolver=None, + def __init__(self, target_locations: List[Pose], ontology_concept_holders: Optional[List[Thing]] = None): """ Navigates the robot to a location. :param target_locations: A list of possible target locations for the navigation. - :param resolver: An alternative specialized_designators that creates a performable designator from the list of possible parameter :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.target_locations: List[Pose] = target_locations + if len(self.target_locations) == 1: + self.knowledge_condition = SpaceIsFreeAspect(self.target_locations[0]) + else: + root = SpaceIsFreeAspect(self.target_locations[0]) + for location in self.target_locations[1:]: + root |= SpaceIsFreeAspect(location) + self.knowledge_condition = root if self.soma: self.init_ontology_concepts({"navigating": self.soma.Navigating}) @@ -315,7 +325,7 @@ class TransportAction(ActionDesignatorDescription): def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], arms: List[Arms], - target_locations: List[Pose], resolver=None, + target_locations: List[Pose], ontology_concept_holders: Optional[List[Thing]] = None): """ Designator representing a pick and place plan. @@ -323,10 +333,9 @@ def __init__(self, :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 :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.object_designator_description: Union[ ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description self.arms: List[Arms] = arms @@ -353,16 +362,15 @@ class LookAtAction(ActionDesignatorDescription): Lets the robot look at a position. """ - def __init__(self, targets: List[Pose], resolver=None, + def __init__(self, targets: List[Pose], ontology_concept_holders: Optional[List[Thing]] = None): """ Moves the head of the robot such that it points towards the given target location. :param targets: A list of possible locations to look at - :param resolver: An alternative specialized_designators that returns a performable designator for a list of possible target locations :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.targets: List[Pose] = targets if self.soma: @@ -382,17 +390,17 @@ class DetectAction(ActionDesignatorDescription): Detects an object that fits the object description and returns an object designator describing the object. """ - def __init__(self, object_designator_description: ObjectDesignatorDescription, resolver=None, + 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. :param object_designator_description: Object designator describing the object - :param resolver: An alternative specialized_designators :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.object_designator_description: ObjectDesignatorDescription = object_designator_description + self.knowledge_condition = VisibleAspect(self.object_designator_description) if self.soma: self.init_ontology_concepts({"looking_for": self.soma.LookingFor, @@ -414,19 +422,20 @@ class OpenAction(ActionDesignatorDescription): Can currently not be used """ - def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], resolver=None, + def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], ontology_concept_holders: Optional[List[Thing]] = None): """ Moves the arm of the robot to open a container. :param object_designator_description: Object designator describing the handle that should be used to open :param arms: A list of possible arms that should be used - :param resolver: A alternative specialized_designators that returns a performable designator for the lists of possible parameter. :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.object_designator_description: ObjectPart = object_designator_description self.arms: List[Arms] = arms + self.knowledge_condition = ReachableAspect( + self.object_designator_description.resolve().pose) & GripperIsFreeAspect(self.arms[0]) if self.soma: self.init_ontology_concepts({"opening": self.soma.Opening}) @@ -449,18 +458,19 @@ class CloseAction(ActionDesignatorDescription): """ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], - resolver=None, ontology_concept_holders: Optional[List[Thing]] = None): + ontology_concept_holders: Optional[List[Thing]] = None): """ Attempts to close an open container :param object_designator_description: Object designator description of the handle that should be used :param arms: A list of possible arms to use - :param resolver: An alternative specialized_designators that returns a performable designator for the list of possible parameter :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.object_designator_description: ObjectPart = object_designator_description self.arms: List[Arms] = arms + self.knowledge_condition = ReachableAspect( + self.object_designator_description.resolve().pose) & GripperIsFreeAspect(self.arms[0]) if self.soma: self.init_ontology_concepts({"closing": self.soma.Closing}) @@ -481,17 +491,16 @@ class GraspingAction(ActionDesignatorDescription): """ def __init__(self, arms: List[Arms], object_description: Union[ObjectDesignatorDescription, ObjectPart], - resolver: Callable = None, ontology_concept_holders: Optional[List[Thing]] = None): + ontology_concept_holders: Optional[List[Thing]] = 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 - :param resolver: An alternative specialized_designators to get a specified designator from the designator description :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ - super().__init__(resolver, ontology_concept_holders) + super().__init__(ontology_concept_holders) self.arms: List[Arms] = arms self.object_description: ObjectDesignatorDescription = object_description @@ -680,12 +689,14 @@ def perform(self) -> None: # 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"]) + left_poses = RobotDescription.current_robot_description.get_arm_chain(Arms.LEFT).get_static_joint_states( + kwargs["left_arm_config"]) # 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"]) + right_poses = RobotDescription.current_robot_description.get_arm_chain(Arms.RIGHT).get_static_joint_states( + kwargs["right_arm_config"]) MoveArmJointsMotion(left_poses, right_poses).perform() @@ -741,7 +752,8 @@ def perform(self) -> None: # 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()) + 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 @@ -806,7 +818,8 @@ def perform(self) -> None: # 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())) + 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() diff --git a/src/pycram/designators/location_designator.py b/src/pycram/designators/location_designator.py index d59f60a30..4bb4fcc79 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 @@ -211,16 +207,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: Object designator for the robot which should open the drawer - :param resolver: An alternative specialized_designators to create the location + :param robot_desig: Object designator for the robot which should open the drawer """ - super().__init__(resolver) + super().__init__() self.handle: ObjectPart.Object = handle_desig self.robot: ObjectDesignatorDescription.Object = robot_desig.world_object @@ -296,7 +291,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 @@ -305,9 +300,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 85d090499..6b3be1c95 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -14,6 +14,7 @@ if TYPE_CHECKING: import owlready2 + class BelieveObject(ObjectDesignatorDescription): """ Description for Objects that are only believed in. @@ -63,18 +64,15 @@ def insert(self, session: sqlalchemy.orm.session.Session) -> ORMObjectPart: def __init__(self, names: List[str], part_of: ObjectDesignatorDescription.Object, - type: Optional[str] = None, - resolver: Optional[Callable] = None): + type: Optional[str] = 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 - :param ontology_concept_holders: A list of ontology concepts that the object part is categorized as or associated with """ - super().__init__(names, type, resolver) + super().__init__(names, type) if not part_of: raise AttributeError("part_of cannot be None.") @@ -121,7 +119,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], ontology_concept_holders: Optional[List[owlready2.Thing]] = None): """ Describing an object resolved through knowrob. @@ -130,10 +128,9 @@ 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. :param ontology_concept_holders: A list of ontology concepts that the object is categorized as """ - super(LocatedObject, self).__init__(names, types, resolver, ontology_concept_holders) + super(LocatedObject, self).__init__(names, types, ontology_concept_holders) self.reference_frames: List[str] = reference_frames self.timestamps: List[float] = timestamps @@ -153,15 +150,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 From 016474a3d7b5965e05df735c0b3bf6def39e0b2e Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 6 Aug 2024 14:53:22 +0200 Subject: [PATCH 18/57] [knowledge engine] Correction of facts knowledge --- src/pycram/datastructures/aspects.py | 15 ++++----- src/pycram/designator.py | 9 ++++++ src/pycram/knowledge/knowledge_engine.py | 7 +++-- src/pycram/knowledge/knowledge_source.py | 6 ++-- .../knowledge_sources/facts_knowledge.py | 31 ++++++++++--------- .../food_cutting_knowledge.py | 6 +++- .../knowledge_sources/knowrob_knowledge.py | 7 +++-- test/test_action_designator.py | 6 ++-- 8 files changed, 54 insertions(+), 33 deletions(-) diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/aspects.py index 55633569d..d8b128cd6 100644 --- a/src/pycram/datastructures/aspects.py +++ b/src/pycram/datastructures/aspects.py @@ -8,8 +8,9 @@ from typing_extensions import List, Iterable, Dict, Any, Callable, Type from anytree import NodeMixin, PreOrderIter, Node -from ..designator import ObjectDesignatorDescription, ActionDesignatorDescription -from ..plan_failures import ObjectNotVisible, ManipulationPoseUnreachable, NavigationGoalInCollision, ObjectUnfetchable +# from ..designator import ObjectDesignatorDescription +from ..plan_failures import ObjectNotVisible, ManipulationPoseUnreachable, NavigationGoalInCollision, ObjectUnfetchable, \ + GripperOccupied from ..world_concepts.world_object import Object @@ -275,12 +276,12 @@ def __call__(self, *args, **kwargs): class GraspableAspect(Aspect): - def __init__(self, object_designator: ObjectDesignatorDescription, input: str = None, output: str = None): + def __init__(self, object_designator: 'ObjectDesignatorDescription', input: str = None, output: str = None): super().__init__(None, None, input, output) self.object_designator = object_designator @abstractmethod - def graspable(self, obj: Object) -> bool: + def graspable(self, object_designator: 'ObjectDesignatorDescription') -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): @@ -320,18 +321,18 @@ def gripper_is_free(self, gripper: Arms) -> bool: def __call__(self, *args, **kwargs): res = self.manage_io(self.resolved_aspect_instance.gripper_is_free, self.gripper) if not res: - raise ObjectNotVisible(f"Gripper {self.gripper} is not free") + raise GripperOccupied(f"Gripper {self.gripper} is not free") return res class VisibleAspect(Aspect): - def __init__(self, object_designator: ObjectDesignatorDescription, input: str = None, output: str = None): + def __init__(self, object_designator: 'ObjectDesignatorDescription', input: str = None, output: str = None): super().__init__(None, None, input, output) self.object_designator = object_designator @abstractmethod - def is_visible(self, obj: Object) -> bool: + def is_visible(self, object_designator: 'ObjectDesignatorDescription') -> bool: raise NotImplementedError def __call__(self, *args, **kwargs): diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 92256ae22..76eddd4b5 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -6,6 +6,9 @@ from inspect import isgenerator, isgeneratorfunction import rospy + +from .knowledge.knowledge_engine import KnowledgeEngine + try: import owlready2 except ImportError: @@ -472,6 +475,12 @@ def __init__(self, ontology_concept_holders: Optional[List[OntologyConceptHolder self.soma = OntologyManager().soma self.knowledge_conditions = None + def resolve(self): + if self.knowledge_condition: + ke = KnowledgeEngine() + ke.query(self) + return self.ground() + def ground(self) -> Action: """Fill all missing parameters and chose plan to execute. """ raise NotImplementedError(f"{type(self)}.ground() is not implemented.") diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index 403c379cb..d197d1e61 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -3,7 +3,7 @@ from ..datastructures.aspects import Aspect from .knowledge_source import KnowledgeSource -from ..designator import DesignatorDescription, ActionDesignatorDescription +# from ..designator import DesignatorDescription, ActionDesignatorDescription from typing_extensions import Type, Callable, List from ..plan_failures import KnowledgeNotAvailable @@ -56,7 +56,7 @@ def update_sources(self): elif not source.is_connected and source.is_available: source.connect() - def query(self, designator: Type[ActionDesignatorDescription]): + def query(self, designator: Type['ActionDesignatorDescription']): """ Query to fill parameters of a designator from the knowledge sources @@ -65,6 +65,7 @@ def query(self, designator: Type[ActionDesignatorDescription]): self.update_sources() condition = designator.knowledge_condition + self.resolve_aspects(condition) condition(designator) def resolve_aspects(self, aspects: Aspect): @@ -89,7 +90,7 @@ def update(self): """ ... - def ground_solution(self, designator: Type[DesignatorDescription]) -> bool: + def ground_solution(self, designator: Type['DesignatorDescription']) -> bool: """ Try to ground a solution from the knowledge sources in the belief state diff --git a/src/pycram/knowledge/knowledge_source.py b/src/pycram/knowledge/knowledge_source.py index de426ba0f..420f00cce 100644 --- a/src/pycram/knowledge/knowledge_source.py +++ b/src/pycram/knowledge/knowledge_source.py @@ -1,6 +1,6 @@ from abc import ABC, abstractmethod -from ..designator import DesignatorDescription +# from ..designator import DesignatorDescription from ..plan_failures import KnowledgeNotAvailable @@ -58,7 +58,7 @@ def __str__(self): class QueryKnowledge: - def query_pose_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: + def query_pose_for_object(self, designator: 'DesignatorDescription') -> 'DesignatorDescription': """ Query the pose for an object from the knowledge source @@ -68,7 +68,7 @@ def query_pose_for_object(self, designator: DesignatorDescription) -> Designator """ pass - def query_grasp_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: + def query_grasp_for_object(self, designator: 'DesignatorDescription') -> 'DesignatorDescription': """ Query the grasp for an object from the knowledge source diff --git a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py index b6dae7cd6..0bb51bbd2 100644 --- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -2,18 +2,18 @@ from ...datastructures.enums import Arms, ObjectType from ..knowledge_source import KnowledgeSource -from ...datastructures.aspects import ReachableAspect, GraspableAspect, GripperIsFreeAspect +from ...datastructures.aspects import ReachableAspect, GraspableAspect, GripperIsFreeAspect, VisibleAspect, SpaceIsFreeAspect from ...datastructures.pose import Pose from ...datastructures.world import World, UseProspectionWorld -from ...designators.location_designator import CostmapLocation -from ...designators.object_designator import BelieveObject +# from ...designators.location_designator import CostmapLocation +# from ...designators.object_designator import BelieveObject from ...robot_description import RobotDescription from ...world_concepts.world_object import Object from ...world_reasoning import visible from ...costmaps import OccupancyCostmap -class FactsKnowledge(KnowledgeSource, ReachableAspect, GraspableAspect, GripperIsFreeAspect): +class FactsKnowledge(KnowledgeSource, ReachableAspect, GraspableAspect, GripperIsFreeAspect, VisibleAspect, SpaceIsFreeAspect): """ Knowledge source for hard coded facts, this knowledge source acts as a fallback if no other knowledge source is available. @@ -36,15 +36,18 @@ def connect(self): def clear_state(self) -> None: pass + # def reachable(self, pose: Pose) -> bool: + # robot_desig = BelieveObject(types=[ObjectType.ROBOT]) + # c = CostmapLocation(pose, reachable_for=robot_desig.resolve()).resolve() + # if c.pose: + # return True + def reachable(self, pose: Pose) -> bool: - robot_desig = BelieveObject(types=[ObjectType.ROBOT]) - c = CostmapLocation(pose, reachable_for=robot_desig.resolve()).resolve() - if c.pose: - return True + return True - def graspable(self, obj: Object) -> bool: + def graspable(self, object_designator: 'ObjectDesignatorDescription') -> bool: with UseProspectionWorld(): - pro_obj = World.current_world.get_prospection_object_for_object(obj) + 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() @@ -67,9 +70,9 @@ def gripper_is_free(self, gripper: Arms) -> bool: 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 True - return False + return False + return True - def is_visible(self, obj: Object) -> bool: + def is_visible(self, object_designator: 'ObjectDesignatorDescription') -> bool: cam_pose = World.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) - return visible(obj, cam_pose) + return visible(object_designator.resolve().world_object, cam_pose) diff --git a/src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py b/src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py index 73726f116..c8ff41f5b 100644 --- a/src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py @@ -28,7 +28,8 @@ def __init__(self): @property def is_available(self) -> bool: - return request.urlopen("https://api.krr.triply.cc/datasets/mkumpel/FruitCuttingKG/services/FoodCuttingKG/sparql?query=SELECT%20?subject%20?predicate%20?objectWHERE%20{?subject%20?predicate%20?object%20.}").getcode() != 404 + pass + # return request.urlopen("https://api.krr.triply.cc/datasets/mkumpel/FruitCuttingKG/services/FoodCuttingKG/sparql?query=SELECT%20?subject%20?predicate%20?objectWHERE%20{?subject%20?predicate%20?object%20.}").getcode() != 404 @property def is_connected(self) -> bool: @@ -40,6 +41,9 @@ def connect(self): def query(self, designator): pass + def clear_state(self): + pass + def get_repetitions(self, task, task_object) -> Iterable[str]: # task = "cut:Quartering" # tobject = "obo:FOODON_03301710" diff --git a/src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py b/src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py index 853afed8e..f01112c56 100644 --- a/src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py @@ -2,7 +2,7 @@ from ..knowledge_source import KnowledgeSource, QueryKnowledge, UpdateKnowledge import rosservice -from ...designator import DesignatorDescription +# from ...designator import DesignatorDescription try: from rosprolog_client import Prolog except ModuleNotFoundError as e: @@ -29,6 +29,9 @@ def connect(self) -> bool: self.prolog_client.once(f"tripledb_load('package://iai_apartment/owl/iai-apartment.owl').") return True - def query_pose_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: + def query_pose_for_object(self, designator: 'DesignatorDescription') -> 'DesignatorDescription': print("test") result = self.prolog_client.once(f"") + + def clear_state(self) -> None: + pass diff --git a/test/test_action_designator.py b/test/test_action_designator.py index 9b09feac8..9ac352828 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -62,12 +62,12 @@ 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(description.ground().target_location, Pose([0.3, 0, 0], [0, 0, 0, 1])) self.assertTrue(description.ontology_concept_holders) - self.assertEqual(self.robot.get_pose(), Pose([1, 0, 0])) + self.assertEqual(self.robot.get_pose(), Pose([0.3, 0, 0])) def test_pick_up(self): object_description = object_designator.ObjectDesignatorDescription(names=["milk"]) From 575d114b50e361d3d2aab68917f0ea6c00c8f375 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 7 Aug 2024 15:13:52 +0200 Subject: [PATCH 19/57] [aspects] New aspect structure --- src/pycram/datastructures/aspects.py | 58 +++++++++---------- src/pycram/knowledge/knowledge_engine.py | 20 +++++-- .../knowledge_sources/facts_knowledge.py | 3 +- 3 files changed, 41 insertions(+), 40 deletions(-) diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/aspects.py index d8b128cd6..4b5bda080 100644 --- a/src/pycram/datastructures/aspects.py +++ b/src/pycram/datastructures/aspects.py @@ -10,7 +10,7 @@ # from ..designator import ObjectDesignatorDescription from ..plan_failures import ObjectNotVisible, ManipulationPoseUnreachable, NavigationGoalInCollision, ObjectUnfetchable, \ - GripperOccupied + GripperOccupied, PlanFailure from ..world_concepts.world_object import Object @@ -257,24 +257,39 @@ def __call__(self, *args, **kwargs) -> bool: return not self.children[0](*args, **kwargs) +class ResolvedAspect(Aspect): + + resolved_aspect_function: Callable + + aspect_exception: Type[PlanFailure] + + def __init__(self, resolved_aspect_function: Callable, aspect_exception: Type[PlanFailure], parent: NodeMixin = None, + input: str = None, output: str = None): + super().__init__(parent, None, input, output) + self.resolved_aspect_function = resolved_aspect_function + self.aspect_exception = aspect_exception + self.parameter = {} + + def __call__(self, *args, **kwargs): + result = self.manage_io(self.resolved_aspect_function, **self.parameter) + if not result: + raise self.aspect_exception(f"Aspect function {self.resolved_aspect_function} returned False") + + class ReachableAspect(Aspect): + aspect_exception = ManipulationPoseUnreachable def __init__(self, pose: Pose, input: str = None, output: str = None): super().__init__(None, None, input, output) - self.target_pose = pose + self.pose = pose @abstractmethod def reachable(self, pose: Pose) -> bool: raise NotImplementedError - def __call__(self, *args, **kwargs): - res = self.manage_io(self.resolved_aspect_instance.reachable, self.target_pose) - if not res: - raise ManipulationPoseUnreachable(f"Pose {self.target_pose} is not reachable") - return res - class GraspableAspect(Aspect): + aspect_exception = ObjectUnfetchable def __init__(self, object_designator: 'ObjectDesignatorDescription', input: str = None, output: str = None): super().__init__(None, None, input, output) @@ -284,14 +299,9 @@ def __init__(self, object_designator: 'ObjectDesignatorDescription', input: str def graspable(self, object_designator: 'ObjectDesignatorDescription') -> bool: raise NotImplementedError - def __call__(self, *args, **kwargs): - res = self.manage_io(self.resolved_aspect_instance.graspable, self.object_designator) - if not res: - raise ObjectUnfetchable(f"Object {self.object_designator} is not graspable") - return res - class SpaceIsFreeAspect(Aspect): + aspect_exception = NavigationGoalInCollision def __init__(self, pose: Pose, input: str = None, output: str = None): super().__init__(None, None, input, output) @@ -301,14 +311,9 @@ def __init__(self, pose: Pose, input: str = None, output: str = None): def space_is_free(self, pose: Pose) -> bool: raise NotImplementedError - def __call__(self, *args, **kwargs): - res = self.manage_io(self.resolved_aspect_instance.space_is_free, self.pose) - if not res: - raise NavigationGoalInCollision(f"Pose {self.pose} is not free") - return res - class GripperIsFreeAspect(Aspect): + aspect_exception = GripperOccupied def __init__(self, gripper: Arms, input: str = None, output: str = None): super().__init__(None, None, input, output) @@ -318,14 +323,9 @@ def __init__(self, gripper: Arms, input: str = None, output: str = None): def gripper_is_free(self, gripper: Arms) -> bool: raise NotImplementedError - def __call__(self, *args, **kwargs): - res = self.manage_io(self.resolved_aspect_instance.gripper_is_free, self.gripper) - if not res: - raise GripperOccupied(f"Gripper {self.gripper} is not free") - return res - class VisibleAspect(Aspect): + aspect_exception = ObjectNotVisible def __init__(self, object_designator: 'ObjectDesignatorDescription', input: str = None, output: str = None): super().__init__(None, None, input, output) @@ -334,9 +334,3 @@ def __init__(self, object_designator: 'ObjectDesignatorDescription', input: str @abstractmethod def is_visible(self, object_designator: 'ObjectDesignatorDescription') -> bool: raise NotImplementedError - - def __call__(self, *args, **kwargs): - res = self.manage_io(self.resolved_aspect_instance.is_visible, self.object_designator) - if not res: - raise ObjectNotVisible(f"Object {self.object_designator} is not visible") - return res diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index d197d1e61..32e7f8769 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -1,7 +1,9 @@ +import inspect + import rospy from anytree import PreOrderIter -from ..datastructures.aspects import Aspect +from ..datastructures.aspects import Aspect, ResolvedAspect from .knowledge_source import KnowledgeSource # from ..designator import DesignatorDescription, ActionDesignatorDescription from typing_extensions import Type, Callable, List @@ -64,8 +66,7 @@ def query(self, designator: Type['ActionDesignatorDescription']): """ self.update_sources() - condition = designator.knowledge_condition - self.resolve_aspects(condition) + condition = self.resolve_aspects(designator.knowledge_condition) condition(designator) def resolve_aspects(self, aspects: Aspect): @@ -78,9 +79,16 @@ def resolve_aspects(self, aspects: Aspect): for child in PreOrderIter(aspects): if child.is_leaf: source = self.find_source_for_aspect(child) - # resolved_aspect_function = source.__getattribute__( - # [fun for fun in child.__class__.__dict__.keys() if not fun.startswith("__")][0]) - child.resolved_aspect_instance = source + resolved_aspect_function = source.__getattribute__( + [fun for fun in child.__class__.__dict__.keys() if + not fun.startswith("__") and not fun == "aspect_exception"][0]) + + # child.resolved_aspect_instance = source + node = ResolvedAspect(resolved_aspect_function, child.aspect_exception, child.parent, child.input, child.output) + for param in inspect.signature(resolved_aspect_function).parameters.keys(): + node.parameter[param] = child.__getattribute__(param) + child.parent = None + return node.root def update(self): """ diff --git a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py index 0bb51bbd2..12b4793eb 100644 --- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -12,8 +12,7 @@ from ...world_reasoning import visible from ...costmaps import OccupancyCostmap - -class FactsKnowledge(KnowledgeSource, ReachableAspect, GraspableAspect, GripperIsFreeAspect, VisibleAspect, SpaceIsFreeAspect): +class FactsKnowledge(KnowledgeSource, GripperIsFreeAspect, VisibleAspect, SpaceIsFreeAspect, GraspableAspect, ReachableAspect): """ Knowledge source for hard coded facts, this knowledge source acts as a fallback if no other knowledge source is available. From 916c0ed62de82e54a1c840755605210eea168a87 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 5 Sep 2024 10:55:36 +0200 Subject: [PATCH 20/57] [Aspects] Renamed aspect to property --- .../{aspects.py => property.py} | 183 +++++++++--------- 1 file changed, 95 insertions(+), 88 deletions(-) rename src/pycram/datastructures/{aspects.py => property.py} (59%) diff --git a/src/pycram/datastructures/aspects.py b/src/pycram/datastructures/property.py similarity index 59% rename from src/pycram/datastructures/aspects.py rename to src/pycram/datastructures/property.py index 4b5bda080..aac5630ab 100644 --- a/src/pycram/datastructures/aspects.py +++ b/src/pycram/datastructures/property.py @@ -7,35 +7,18 @@ from .pose import Pose from typing_extensions import List, Iterable, Dict, Any, Callable, Type from anytree import NodeMixin, PreOrderIter, Node - -# from ..designator import ObjectDesignatorDescription from ..plan_failures import ObjectNotVisible, ManipulationPoseUnreachable, NavigationGoalInCollision, ObjectUnfetchable, \ GripperOccupied, PlanFailure -from ..world_concepts.world_object import Object - - -def managed_io(func: Callable) -> Callable: - def wrapper(*args, **kwargs): - print("test") - return func(*args, **kwargs) - - return wrapper - -class Aspect(NodeMixin): +class Property(NodeMixin): """ - Parent class to represent a semantic aspect as part of a knowledge pre-condition of designators. + 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. """ - resolved_aspect_instance: Type[Aspect] - """ - Reference to the actual implementation of the aspect function in the KnowledgeSource. This reference is used when - evaluating the tree structure of aspects. - """ variables: Dict[str, Any] """ - Dictionary of variables and their values which are used in the aspect tree. This dictionary is only to be used in + Dictionary of variables and their values which are used in the property tree. This dictionary is only to be used in the root node. """ @@ -51,45 +34,45 @@ def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = Non def __and__(self, other): """ - Overload the and operator to create an AndAspect as part of the tree structure. + Overload the and operator to create an And as part of the tree structure. - :param other: The other aspect to combine with - :return: An AndAspect containing both, this and the other, aspect + :param other: The other property to combine with + :return: An And containing both, this and the other, property """ - return AndAspect([self, other]) + return And([self, other]) def __or__(self, other): """ - Overload the or operator to create an OrAspect as part of the tree structure. + Overload the or operator to create an Or as part of the tree structure. - :param other: The other aspect to combine with - :return: An OrAspect containing both, this and the other, aspect + :param other: The other property to combine with + :return: An Or containing both, this and the other, property """ - return OrAspect([self, other]) + return Or([self, other]) def __neg__(self): """ - Overload the not operator to create a NotAspect as part of the tree structure. + Overload the not operator to create a Not as part of the tree structure. - :return: A NotAspect containing this aspect + :return: A Not containing this property """ - return NotAspect(self) + return Not(self) def __invert__(self): """ - Overload the invert operator to create a NotAspect as part of the tree structure. + Overload the invert operator to create a Not as part of the tree structure. - :return: A NotAspect containing this aspect + :return: A Not containing this property """ - return NotAspect(self) + return Not(self) def manage_io(self, func: Callable, *args, **kwargs) -> bool: """ - Manages the input and output variables across the whole aspect tree. If the aspect has an input variable, the + Manages the input and output variables across the whole property tree. If the property has an input variable, the value of this variable will be taken from the variables dictionary of the root node. If an output is defined, the result of the function will be stored in the variables dictionary of the root node. - :param func: Aspect function to call + :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 @@ -108,24 +91,21 @@ def manage_io(self, func: Callable, *args, **kwargs) -> bool: return func(*args, **kwargs) -class AspectOperator(Aspect): +class PropertyOperator(Property): """ - Parent class for logical operators to combine multiple aspects in a tree structure. This class adds methods to - use Aspects as part of a tree structure. Furthermore, there is a method to simplify the tree structure by merging + 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. """ - aspects: List[Aspect] - def __init__(self, aspects: List[Aspect]): + def __init__(self, properties: List[Property]): """ - Initialize the AspectOperator with a list of aspects. The parent of this node is None, therefore the node is + 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 aspects: A list of aspects to which are the children of this node + :param properties: A list of properties to which are the children of this node """ - super().__init__(parent=None, children=aspects) - self.aspects = aspects - # self.resolved_aspects = [] + super().__init__(parent=None, children=properties) def simplify(self): """ @@ -153,20 +133,20 @@ def merge_nodes(node1: Node, node2: Node) -> None: node1.children = node2.children + node1.children -class AndAspect(AspectOperator): +class And(PropertyOperator): """ - Class to represent a logical and operator in a tree structure. This class inherits from AspectOperator and adds a + Class to represent a logical and operator in a tree structure. This class inherits from PropertyOperator and adds a method to evaluate the children as an and operator. """ - def __init__(self, aspects: List[Aspect]): + def __init__(self, properties: List[Property]): """ - Initialize the AndAspect with a list of aspects as the children of this node. This node will be the root of the + Initialize the And with a list of aspects as the children of this node. This node will be the root of the tree. - :param aspects: A list of aspects which are the children of this node + :param properties: A list of aspects which are the children of this node """ - super().__init__(aspects) + super().__init__(properties) self.simplify() def __call__(self, *args, **kwargs) -> bool: @@ -181,10 +161,10 @@ def __call__(self, *args, **kwargs) -> bool: """ result = True for child in self.children: - # Child is an Aspect and the resolved function should be called + # Child is a Property and the resolved function should be called if child.is_leaf: result = result and child(*args, **kwargs) - # Child is an AspectOperator + # Child is a PropertyOperator else: child(*args, **kwargs) if not result: @@ -192,20 +172,20 @@ def __call__(self, *args, **kwargs) -> bool: return result -class OrAspect(AspectOperator): +class Or(PropertyOperator): """ - Class to represent a logical or operator in a tree structure. This class inherits from AspectOperator and adds a + Class to represent a logical or operator in a tree structure. This class inherits from PropertyOperator and adds a method to evaluate the children as an or operator. """ - def __init__(self, aspects: List[Aspect]): + def __init__(self, properties: List[Property]): """ - Initialize the OrAspect with a list of aspects as the children of this node. This node will be the root of the + Initialize the Or with a list of aspects as the children of this node. This node will be the root of the tree. - :param aspects: A list of aspects which are the children of this node + :param properties: A list of aspects which are the children of this node """ - super().__init__(aspects) + super().__init__(properties) self.simplify() def __call__(self, *args, **kwargs) -> bool: @@ -220,10 +200,10 @@ def __call__(self, *args, **kwargs) -> bool: """ result = False for child in self.children: - # Child is an Aspect and the resolved function should be called + # Child is a Property and the resolved function should be called if child.is_leaf: result = result or child(*args, **kwargs) - # Child is an AspectOperator + # Child is a PropertyOperator else: result = child(*args, **kwargs) if result: @@ -231,19 +211,19 @@ def __call__(self, *args, **kwargs) -> bool: return result -class NotAspect(AspectOperator): +class Not(PropertyOperator): """ - Class to represent a logical not operator in a tree structure. This class inherits from AspectOperator and adds a + Class to represent a logical not operator in a tree structure. This class inherits from PropertyOperator and adds a method to evaluate the children as a not operator. """ - def __init__(self, aspect: Aspect): + def __init__(self, property: Property): """ - Initialize the NotAspect with an aspect as the child of this node. This node will be the root of the tree. + Initialize the Not with an aspect as the child of this node. This node will be the root of the tree. - :param aspect: The aspect which is the child of this node + :param property: The property which is the child of this node """ - super().__init__([aspect]) + super().__init__([property]) def __call__(self, *args, **kwargs) -> bool: """ @@ -257,27 +237,54 @@ def __call__(self, *args, **kwargs) -> bool: return not self.children[0](*args, **kwargs) -class ResolvedAspect(Aspect): - - resolved_aspect_function: Callable +class ResolvedProperty(Property): + """ + Class to represent 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. + """ - aspect_exception: Type[PlanFailure] + 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_aspect_function: Callable, aspect_exception: Type[PlanFailure], parent: NodeMixin = None, + def __init__(self, resolved_property_function: Callable, property_exception: Type[PlanFailure], parent: NodeMixin = None, input: str = None, output: str = None): + """ + Initialize the ResolvedProperty with the resolved property function, the exception that should be raised if the property + is not fulfilled, the parent node, and the input and output variables. + + :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 + :param input: Input variable name of this property + :param output: Output variable name of this property + """ super().__init__(parent, None, input, output) - self.resolved_aspect_function = resolved_aspect_function - self.aspect_exception = aspect_exception + self.resolved_property_function = resolved_property_function + self.property_exception = property_exception self.parameter = {} - def __call__(self, *args, **kwargs): - result = self.manage_io(self.resolved_aspect_function, **self.parameter) + def __call__(self) -> bool: + """ + Manages the io of the call to the property function and then calles the function. If the function returns False, + the exception defined in :attr:`property_exception` will be raised. + + :return: The result of the property function + """ + result = self.manage_io(self.resolved_property_function, **self.parameter) if not result: - raise self.aspect_exception(f"Aspect function {self.resolved_aspect_function} returned False") + raise self.property_exception(f"Property function {self.resolved_property_function} returned False") + return result -class ReachableAspect(Aspect): - aspect_exception = ManipulationPoseUnreachable +class ReachableProperty(Property): + property_exception = ManipulationPoseUnreachable def __init__(self, pose: Pose, input: str = None, output: str = None): super().__init__(None, None, input, output) @@ -288,8 +295,8 @@ def reachable(self, pose: Pose) -> bool: raise NotImplementedError -class GraspableAspect(Aspect): - aspect_exception = ObjectUnfetchable +class GraspableProperty(Property): + property_exception = ObjectUnfetchable def __init__(self, object_designator: 'ObjectDesignatorDescription', input: str = None, output: str = None): super().__init__(None, None, input, output) @@ -300,8 +307,8 @@ def graspable(self, object_designator: 'ObjectDesignatorDescription') -> bool: raise NotImplementedError -class SpaceIsFreeAspect(Aspect): - aspect_exception = NavigationGoalInCollision +class SpaceIsFreeProperty(Property): + property_exception = NavigationGoalInCollision def __init__(self, pose: Pose, input: str = None, output: str = None): super().__init__(None, None, input, output) @@ -312,8 +319,8 @@ def space_is_free(self, pose: Pose) -> bool: raise NotImplementedError -class GripperIsFreeAspect(Aspect): - aspect_exception = GripperOccupied +class GripperIsFreeProperty(Property): + property_exception = GripperOccupied def __init__(self, gripper: Arms, input: str = None, output: str = None): super().__init__(None, None, input, output) @@ -324,8 +331,8 @@ def gripper_is_free(self, gripper: Arms) -> bool: raise NotImplementedError -class VisibleAspect(Aspect): - aspect_exception = ObjectNotVisible +class VisibleProperty(Property): + property_exception = ObjectNotVisible def __init__(self, object_designator: 'ObjectDesignatorDescription', input: str = None, output: str = None): super().__init__(None, None, input, output) From 693f7282bb8a985310c6c6d4f18fc839409938ec Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 5 Sep 2024 11:04:11 +0200 Subject: [PATCH 21/57] [knowledge/designator] Rename of property usage --- src/pycram/designators/action_designator.py | 28 +++++++++---------- src/pycram/knowledge/knowledge_engine.py | 22 +++++++-------- .../knowledge_sources/facts_knowledge.py | 4 +-- src/pycram/orm/object_designator.py | 4 +-- 4 files changed, 29 insertions(+), 29 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 9a84dfcc2..a647cd539 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -16,8 +16,8 @@ from .motion_designator import MoveJointsMotion, MoveGripperMotion, MoveArmJointsMotion, MoveTCPMotion, MoveMotion, \ LookingMotion, DetectingMotion, OpeningMotion, ClosingMotion from .object_designator import ObjectDesignatorDescription, BelieveObject, ObjectPart -from ..datastructures.aspects import GraspableAspect, ReachableAspect, GripperIsFreeAspect, SpaceIsFreeAspect, \ - VisibleAspect +from ..datastructures.property import GraspableProperty, ReachableProperty, GripperIsFreeProperty, SpaceIsFreeProperty, \ + VisibleProperty from ..local_transformer import LocalTransformer from ..plan_failures import ObjectUnfetchable, ReachabilityFailure # from ..robot_descriptions import robot_description @@ -100,11 +100,11 @@ def __init__(self, grippers: List[Arms], motions: List[GripperState], self.grippers: List[Arms] = grippers self.motions: List[GripperState] = motions if len(self.grippers) == 1: - self.knowledge_condition = GripperIsFreeAspect(self.grippers[0]) + self.knowledge_condition = GripperIsFreeProperty(self.grippers[0]) else: - root = GripperIsFreeAspect(self.grippers[0]) + root = GripperIsFreeProperty(self.grippers[0]) for gripper in grippers[1:]: - root |= GripperIsFreeAspect(gripper) + root |= GripperIsFreeProperty(gripper) self.knowledge_condition = root if self.soma: @@ -224,7 +224,7 @@ def __init__(self, ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description self.arms: List[Arms] = arms self.grasps: List[Grasp] = grasps - self.knowledge_condition = GraspableAspect(self.object_designator_description) & ReachableAspect( + self.knowledge_condition = GraspableProperty(self.object_designator_description) & ReachableProperty( self.object_designator_description.resolve().pose) if self.soma: @@ -298,11 +298,11 @@ def __init__(self, target_locations: List[Pose], super().__init__(ontology_concept_holders) self.target_locations: List[Pose] = target_locations if len(self.target_locations) == 1: - self.knowledge_condition = SpaceIsFreeAspect(self.target_locations[0]) + self.knowledge_condition = SpaceIsFreeProperty(self.target_locations[0]) else: - root = SpaceIsFreeAspect(self.target_locations[0]) + root = SpaceIsFreeProperty(self.target_locations[0]) for location in self.target_locations[1:]: - root |= SpaceIsFreeAspect(location) + root |= SpaceIsFreeProperty(location) self.knowledge_condition = root if self.soma: @@ -400,7 +400,7 @@ def __init__(self, object_designator_description: ObjectDesignatorDescription, """ super().__init__(ontology_concept_holders) self.object_designator_description: ObjectDesignatorDescription = object_designator_description - self.knowledge_condition = VisibleAspect(self.object_designator_description) + self.knowledge_condition = VisibleProperty(self.object_designator_description) if self.soma: self.init_ontology_concepts({"looking_for": self.soma.LookingFor, @@ -434,8 +434,8 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], super().__init__(ontology_concept_holders) self.object_designator_description: ObjectPart = object_designator_description self.arms: List[Arms] = arms - self.knowledge_condition = ReachableAspect( - self.object_designator_description.resolve().pose) & GripperIsFreeAspect(self.arms[0]) + self.knowledge_condition = ReachableProperty( + self.object_designator_description.resolve().pose) & GripperIsFreeProperty(self.arms[0]) if self.soma: self.init_ontology_concepts({"opening": self.soma.Opening}) @@ -469,8 +469,8 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], super().__init__(ontology_concept_holders) self.object_designator_description: ObjectPart = object_designator_description self.arms: List[Arms] = arms - self.knowledge_condition = ReachableAspect( - self.object_designator_description.resolve().pose) & GripperIsFreeAspect(self.arms[0]) + self.knowledge_condition = ReachableProperty( + self.object_designator_description.resolve().pose) & GripperIsFreeProperty(self.arms[0]) if self.soma: self.init_ontology_concepts({"closing": self.soma.Closing}) diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index 32e7f8769..40030bca2 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -3,7 +3,7 @@ import rospy from anytree import PreOrderIter -from ..datastructures.aspects import Aspect, ResolvedAspect +from ..datastructures.property import Property, ResolvedProperty from .knowledge_source import KnowledgeSource # from ..designator import DesignatorDescription, ActionDesignatorDescription from typing_extensions import Type, Callable, List @@ -69,22 +69,22 @@ def query(self, designator: Type['ActionDesignatorDescription']): condition = self.resolve_aspects(designator.knowledge_condition) condition(designator) - def resolve_aspects(self, aspects: Aspect): + def resolve_aspects(self, aspects: Property): """ - Traverses the tree of aspects and resolves the aspect functions to the corresponding function in the knowledge + Traverses the tree of properties and resolves the property functions to the corresponding function in the knowledge source. - :param aspects: Root node of the tree of aspects + :param aspects: Root node of the tree of properties """ for child in PreOrderIter(aspects): if child.is_leaf: source = self.find_source_for_aspect(child) resolved_aspect_function = source.__getattribute__( [fun for fun in child.__class__.__dict__.keys() if - not fun.startswith("__") and not fun == "aspect_exception"][0]) + not fun.startswith("__") and not fun == "property_exception"][0]) - # child.resolved_aspect_instance = source - node = ResolvedAspect(resolved_aspect_function, child.aspect_exception, child.parent, child.input, child.output) + # child.resolved_property_instance = source + node = ResolvedProperty(resolved_aspect_function, child.property_exception, child.parent, child.input, child.output) for param in inspect.signature(resolved_aspect_function).parameters.keys(): node.parameter[param] = child.__getattribute__(param) child.parent = None @@ -124,12 +124,12 @@ def call_source(self, query_function: Callable, *args, **kwargs): raise KnowledgeNotAvailable( f"Query function {query_function.__name__} is not available in any connected knowledge source") - def find_source_for_aspect(self, aspect: Type[Aspect]): + def find_source_for_aspect(self, aspect: Type[Property]): """ - Find the source for the given aspect + Find the source for the given property - :param aspect: The aspect for which to find the source. - :return: Source that can provide the aspect. + :param aspect: The property for which to find the source. + :return: Source that can provide the property. """ for source in self.knowledge_sources: if (aspect.__class__ in list(source.__class__.__bases__) diff --git a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py index 12b4793eb..7f3abad8f 100644 --- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -2,7 +2,7 @@ from ...datastructures.enums import Arms, ObjectType from ..knowledge_source import KnowledgeSource -from ...datastructures.aspects import ReachableAspect, GraspableAspect, GripperIsFreeAspect, VisibleAspect, SpaceIsFreeAspect +from ...datastructures.property import ReachableProperty, GraspableProperty, GripperIsFreeProperty, VisibleProperty, SpaceIsFreeProperty from ...datastructures.pose import Pose from ...datastructures.world import World, UseProspectionWorld # from ...designators.location_designator import CostmapLocation @@ -12,7 +12,7 @@ from ...world_reasoning import visible from ...costmaps import OccupancyCostmap -class FactsKnowledge(KnowledgeSource, GripperIsFreeAspect, VisibleAspect, SpaceIsFreeAspect, GraspableAspect, ReachableAspect): +class FactsKnowledge(KnowledgeSource, GripperIsFreeProperty, VisibleProperty, SpaceIsFreeProperty, GraspableProperty, ReachableProperty): """ Knowledge source for hard coded facts, this knowledge source acts as a fallback if no other knowledge source is available. diff --git a/src/pycram/orm/object_designator.py b/src/pycram/orm/object_designator.py index 6f66a785b..0b68361f2 100644 --- a/src/pycram/orm/object_designator.py +++ b/src/pycram/orm/object_designator.py @@ -1,10 +1,10 @@ from dataclasses import field from typing import Optional -from pycram.orm.base import Base, MapperArgsMixin, PoseMixin, Pose +from .base import Base, MapperArgsMixin, PoseMixin, Pose from sqlalchemy.orm import Mapped, mapped_column, declared_attr, relationship, MappedAsDataclass from sqlalchemy import ForeignKey -from pycram.datastructures.enums import ObjectType +from ..datastructures.enums import ObjectType class ObjectMixin(MappedAsDataclass): From 6652053222c54483f4518565432a7e4b7ce58c21 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 5 Sep 2024 11:05:14 +0200 Subject: [PATCH 22/57] [test] Fixed Test for pre-condition --- test/test_language.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/test/test_language.py b/test/test_language.py index bb3fec509..c28615765 100644 --- a/test/test_language.py +++ b/test/test_language.py @@ -139,14 +139,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) @@ -157,14 +157,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): From 9f38f727741a25f355574e0c98c78ef13e8217f0 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 5 Sep 2024 12:36:48 +0200 Subject: [PATCH 23/57] [knowledge engine] Added flag to disable knowledge engine --- src/pycram/knowledge/knowledge_engine.py | 34 ++++++++++++++++++------ 1 file changed, 26 insertions(+), 8 deletions(-) diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index 40030bca2..5a2a4cc81 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -19,7 +19,15 @@ class KnowledgeEngine: 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: @@ -29,6 +37,9 @@ def __new__(cls): def __init__(self): 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] = [] @@ -58,16 +69,19 @@ def update_sources(self): elif not source.is_connected and source.is_available: source.connect() - def query(self, designator: Type['ActionDesignatorDescription']): + def query(self, designator: Type['ActionDesignatorDescription']) -> bool: """ Query to fill parameters of a designator from the knowledge sources :return: """ + if not self.enabled: + rospy.logwarn("Knowledge engine is disabled") + return True self.update_sources() condition = self.resolve_aspects(designator.knowledge_condition) - condition(designator) + return condition(designator) def resolve_aspects(self, aspects: Property): """ @@ -78,7 +92,7 @@ def resolve_aspects(self, aspects: Property): """ for child in PreOrderIter(aspects): if child.is_leaf: - source = self.find_source_for_aspect(child) + 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]) @@ -96,7 +110,9 @@ def update(self): :return: """ - ... + if not self.enabled: + rospy.logwarn("Knowledge engine is disabled") + return True def ground_solution(self, designator: Type['DesignatorDescription']) -> bool: """ @@ -104,7 +120,9 @@ def ground_solution(self, designator: Type['DesignatorDescription']) -> bool: :return: True if the solution achieves the desired goal, False otherwise """ - ... + if not self.enabled: + rospy.logwarn("Knowledge engine is disabled") + return True def call_source(self, query_function: Callable, *args, **kwargs): """ @@ -124,14 +142,14 @@ def call_source(self, query_function: Callable, *args, **kwargs): raise KnowledgeNotAvailable( f"Query function {query_function.__name__} is not available in any connected knowledge source") - def find_source_for_aspect(self, aspect: Type[Property]): + def find_source_for_property(self, property: Type[Property]): """ Find the source for the given property - :param aspect: The property for which to find the source. + :param property: The property for which to find the source. :return: Source that can provide the property. """ for source in self.knowledge_sources: - if (aspect.__class__ in list(source.__class__.__bases__) + if (property.__class__ in list(source.__class__.__bases__) and source.is_connected): return source From a4891ac7dcb485fd15c581be8bf5ae9d136ba0d8 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 24 Sep 2024 16:55:38 +0200 Subject: [PATCH 24/57] [doc] Added documentation for knowledge interface --- doc/source/knowledge.rst | 76 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 76 insertions(+) create mode 100644 doc/source/knowledge.rst diff --git a/doc/source/knowledge.rst b/doc/source/knowledge.rst new file mode 100644 index 000000000..68ae7256a --- /dev/null +++ b/doc/source/knowledge.rst @@ -0,0 +1,76 @@ +========= +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. + +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. + +## TODO Picture to showcase + + +----------------- +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. The methods to access the knowledge are defined to reason about the +a given action or object. The methods are: + + * reachable + * graspable + * space_is_free + * gripper_is_free + * is_visible + +## TODO link to example of knowledge source + +---------------- +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 From 84c33f5a3c3dc04600de1dd06c5cb9d679f3ca2b Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 25 Sep 2024 13:21:21 +0200 Subject: [PATCH 25/57] [knowledge] Removed input and output variables --- src/pycram/datastructures/property.py | 53 ++++++++---------------- src/pycram/knowledge/knowledge_engine.py | 2 +- 2 files changed, 19 insertions(+), 36 deletions(-) diff --git a/src/pycram/datastructures/property.py b/src/pycram/datastructures/property.py index aac5630ab..75832300f 100644 --- a/src/pycram/datastructures/property.py +++ b/src/pycram/datastructures/property.py @@ -4,6 +4,7 @@ from abc import abstractmethod from .enums import Arms +from .dataclasses import ReasoningResult from .pose import Pose from typing_extensions import List, Iterable, Dict, Any, Callable, Type from anytree import NodeMixin, PreOrderIter, Node @@ -22,12 +23,9 @@ class Property(NodeMixin): the root node. """ - def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None, - input: str = None, output: str = None): + def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None): super().__init__() self.parent = parent - self.input = input - self.output = output self.variables = {} if children: self.children = children @@ -68,26 +66,14 @@ def __invert__(self): def manage_io(self, func: Callable, *args, **kwargs) -> bool: """ - Manages the input and output variables across the whole property tree. If the property has an input variable, the - value of this variable will be taken from the variables dictionary of the root node. If an output is defined, - the result of the function will be stored in the variables dictionary of the root node. + Manages the ReasoningResult of a property function. 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 """ - if self.input: - if self.input not in self.root.variables.keys(): - raise AttributeError(f"Variable {self.input} not found in variables") - input_var = self.root.variables[self.input] - result = func(input_var) - if self.output: - self.variables[self.output] = result - elif self.output: - result = func(*args, **kwargs) - self.variables[self.output] = result - return result return func(*args, **kwargs) @@ -253,19 +239,16 @@ class ResolvedProperty(Property): 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, - input: str = None, output: str = None): + def __init__(self, resolved_property_function: Callable, property_exception: Type[PlanFailure], parent: NodeMixin = None): """ Initialize the ResolvedProperty with the resolved property function, the exception that should be raised if the property - is not fulfilled, the parent node, and the input and output variables. + 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 - :param input: Input variable name of this property - :param output: Output variable name of this property """ - super().__init__(parent, None, input, output) + super().__init__(parent, None) self.resolved_property_function = resolved_property_function self.property_exception = property_exception self.parameter = {} @@ -286,20 +269,20 @@ def __call__(self) -> bool: class ReachableProperty(Property): property_exception = ManipulationPoseUnreachable - def __init__(self, pose: Pose, input: str = None, output: str = None): - super().__init__(None, None, input, output) + def __init__(self, pose: Pose): + super().__init__(None, None) self.pose = pose @abstractmethod - def reachable(self, pose: Pose) -> bool: + def reachable(self, pose: Pose) -> ReasoningResult: raise NotImplementedError class GraspableProperty(Property): property_exception = ObjectUnfetchable - def __init__(self, object_designator: 'ObjectDesignatorDescription', input: str = None, output: str = None): - super().__init__(None, None, input, output) + def __init__(self, object_designator: 'ObjectDesignatorDescription'): + super().__init__(None, None) self.object_designator = object_designator @abstractmethod @@ -310,8 +293,8 @@ def graspable(self, object_designator: 'ObjectDesignatorDescription') -> bool: class SpaceIsFreeProperty(Property): property_exception = NavigationGoalInCollision - def __init__(self, pose: Pose, input: str = None, output: str = None): - super().__init__(None, None, input, output) + def __init__(self, pose: Pose): + super().__init__(None, None) self.pose = pose @abstractmethod @@ -322,8 +305,8 @@ def space_is_free(self, pose: Pose) -> bool: class GripperIsFreeProperty(Property): property_exception = GripperOccupied - def __init__(self, gripper: Arms, input: str = None, output: str = None): - super().__init__(None, None, input, output) + def __init__(self, gripper: Arms): + super().__init__(None, None) self.gripper = gripper @abstractmethod @@ -334,8 +317,8 @@ def gripper_is_free(self, gripper: Arms) -> bool: class VisibleProperty(Property): property_exception = ObjectNotVisible - def __init__(self, object_designator: 'ObjectDesignatorDescription', input: str = None, output: str = None): - super().__init__(None, None, input, output) + def __init__(self, object_designator: 'ObjectDesignatorDescription'): + super().__init__(None, None) self.object_designator = object_designator @abstractmethod diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index 5a2a4cc81..40785bb11 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -98,7 +98,7 @@ def resolve_aspects(self, aspects: Property): not fun.startswith("__") and not fun == "property_exception"][0]) # child.resolved_property_instance = source - node = ResolvedProperty(resolved_aspect_function, child.property_exception, child.parent, child.input, child.output) + 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 From f3db003c828ef8fb572668881967a04e7b6451f1 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 25 Sep 2024 14:04:06 +0200 Subject: [PATCH 26/57] [Designator] Removed deprecated Designator class --- src/pycram/designator.py | 260 +-------------------------------------- 1 file changed, 1 insertion(+), 259 deletions(-) diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 76eddd4b5..52fa4dffc 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -51,7 +51,7 @@ def __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,264 +68,6 @@ 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 From 57ca550dbf553d9e63836dcebf558748b71b3098 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 25 Sep 2024 14:25:37 +0200 Subject: [PATCH 27/57] [property] Added reasoning result --- src/pycram/datastructures/dataclasses.py | 5 +++++ src/pycram/datastructures/property.py | 23 ++++++++++++++--------- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index d83bcd00f..aae3b82d8 100644 --- a/src/pycram/datastructures/dataclasses.py +++ b/src/pycram/datastructures/dataclasses.py @@ -296,3 +296,8 @@ class ObjectState(State): class WorldState(State): simulator_state_id: int object_states: Dict[str, ObjectState] + +@dataclass +class ReasoningResult: + success: bool + reasoned_parameter: Dict[str, Any] \ No newline at end of file diff --git a/src/pycram/datastructures/property.py b/src/pycram/datastructures/property.py index 75832300f..c1605ccc0 100644 --- a/src/pycram/datastructures/property.py +++ b/src/pycram/datastructures/property.py @@ -6,11 +6,14 @@ from .enums import Arms from .dataclasses import ReasoningResult from .pose import Pose -from typing_extensions import List, Iterable, Dict, Any, Callable, Type +from typing_extensions import List, Iterable, Dict, Any, Callable, Type, TYPE_CHECKING from anytree import NodeMixin, PreOrderIter, Node from ..plan_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. @@ -74,7 +77,9 @@ def manage_io(self, func: Callable, *args, **kwargs) -> bool: :param kwargs: keyword args to pass to the function :return: result of the function """ - return func(*args, **kwargs) + reasoning_result = func(*args, **kwargs) + self.root.variables.update(reasoning_result.reasoned_parameter) + return reasoning_result.success class PropertyOperator(Property): @@ -255,7 +260,7 @@ def __init__(self, resolved_property_function: Callable, property_exception: Typ def __call__(self) -> bool: """ - Manages the io of the call to the property function and then calles the function. If the function returns False, + 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 @@ -281,12 +286,12 @@ def reachable(self, pose: Pose) -> ReasoningResult: class GraspableProperty(Property): property_exception = ObjectUnfetchable - def __init__(self, object_designator: 'ObjectDesignatorDescription'): + def __init__(self, object_designator: ObjectDesignatorDescription): super().__init__(None, None) self.object_designator = object_designator @abstractmethod - def graspable(self, object_designator: 'ObjectDesignatorDescription') -> bool: + def graspable(self, object_designator: ObjectDesignatorDescription) -> ReasoningResult: raise NotImplementedError @@ -298,7 +303,7 @@ def __init__(self, pose: Pose): self.pose = pose @abstractmethod - def space_is_free(self, pose: Pose) -> bool: + def space_is_free(self, pose: Pose) -> ReasoningResult: raise NotImplementedError @@ -310,17 +315,17 @@ def __init__(self, gripper: Arms): self.gripper = gripper @abstractmethod - def gripper_is_free(self, gripper: Arms) -> bool: + def gripper_is_free(self, gripper: Arms) -> ReasoningResult: raise NotImplementedError class VisibleProperty(Property): property_exception = ObjectNotVisible - def __init__(self, object_designator: 'ObjectDesignatorDescription'): + def __init__(self, object_designator: ObjectDesignatorDescription): super().__init__(None, None) self.object_designator = object_designator @abstractmethod - def is_visible(self, object_designator: 'ObjectDesignatorDescription') -> bool: + def is_visible(self, object_designator: ObjectDesignatorDescription) -> ReasoningResult: raise NotImplementedError From 8d00b0aa95e76ac8d727e66ddb5d8e60590313c7 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 25 Sep 2024 16:52:38 +0200 Subject: [PATCH 28/57] [Knowledge Engine] Added reasoned parameter matching --- requirements.txt | 13 ++++++++ src/pycram/datastructures/enums.py | 5 +++ src/pycram/designator.py | 34 ++++++++++++++++++--- src/pycram/knowledge/knowledge_engine.py | 39 +++++++++++++++++++++++- src/pycram/process_module.py | 3 +- 5 files changed, 87 insertions(+), 7 deletions(-) diff --git a/requirements.txt b/requirements.txt index 7841173bb..ad3b5fb7e 100644 --- a/requirements.txt +++ b/requirements.txt @@ -17,3 +17,16 @@ psutil==5.9.7 lxml==4.9.1 typing_extensions==4.9.0 owlready2>=0.45 + +Pygments~=2.14.0 +rospy~=1.15.14 +tf~=1.13.2 +rosgraph~=1.15.14 +rosservice~=1.15.14 +rdflib~=6.3.2 +typeguard~=4.3.0 +matplotlib~=3.7.3 +actionlib~=1.13.2 +rosnode~=1.15.14 +roslaunch~=1.15.14 +catkin-pkg~=0.5.2 \ No newline at end of file diff --git a/src/pycram/datastructures/enums.py b/src/pycram/datastructures/enums.py index 301263473..ce2ef5439 100644 --- a/src/pycram/datastructures/enums.py +++ b/src/pycram/datastructures/enums.py @@ -2,6 +2,11 @@ from enum import Enum, auto +class ExecutionType(Enum): + """Enum for Execution Process Module types.""" + REAL = auto() + SIMULATED = auto() + SEMI_REAL = auto() class Arms(Enum): """Enum for Arms.""" diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 52fa4dffc..3a35a720d 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -1,11 +1,13 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations +import typing_extensions from dataclasses import dataclass, field, fields from abc import ABC, abstractmethod from inspect import isgenerator, isgeneratorfunction import rospy +import inspect from .knowledge.knowledge_engine import KnowledgeEngine @@ -51,7 +53,8 @@ def __init__(self, *args, **kwargs): class ResolutionError(Exception): - def __init__(self, missing_properties: List[str], wrong_type: Dict, current_type: Any, designator: DesignatorDescription): + 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" @@ -125,6 +128,26 @@ def get_default_ontology_concept(self) -> owlready2.Thing | None: """ return self.ontology_concept_holders[0].ontology_concept if self.ontology_concept_holders else None + def get_optional_parameter(self) -> List[str]: + """ + Returns a list of optional parameter names of this designator 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. + """ + 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. + + :return: + """ + return typing_extensions.get_type_hints(self.__init__) class ActionDesignatorDescription(DesignatorDescription, Language): """ @@ -158,7 +181,8 @@ class Action: def __post_init__(self): self.robot_position = World.robot.get_pose() - self.robot_torso_height = World.robot.get_joint_position(RobotDescription.current_robot_description.torso_joint) + self.robot_torso_height = World.robot.get_joint_position( + RobotDescription.current_robot_description.torso_joint) self.robot_type = World.robot.obj_type @with_tree @@ -240,7 +264,7 @@ def init_ontology_concepts(self, ontology_concept_classes: Dict[str, Type[owlrea if concept_class: existing_holders = OntologyConceptHolderStore().get_ontology_concept_holders_by_class(concept_class) self.ontology_concept_holders.extend(existing_holders if existing_holders \ - else [OntologyConceptHolder(concept_class(concept_name))]) + else [OntologyConceptHolder(concept_class(concept_name))]) def __iter__(self): """ @@ -277,7 +301,7 @@ def ground(self) -> Location: raise NotImplementedError(f"{type(self)}.ground() is not implemented.") -#this knowledge should be somewhere else i guess +# this knowledge should be somewhere else i guess SPECIAL_KNOWLEDGE = { 'bigknife': [("top", [-0.08, 0, 0])], @@ -451,4 +475,4 @@ def __iter__(self) -> Iterable[Object]: if self.types and obj.obj_type not in self.types: continue - yield self.Object(obj.name, obj.obj_type, obj) \ No newline at end of file + yield self.Object(obj.name, obj.obj_type, obj) diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index 40785bb11..2d45df6e6 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -1,16 +1,21 @@ +from __future__ import annotations + import inspect import rospy from anytree import PreOrderIter +from typeguard import check_type, TypeCheckError from ..datastructures.property import Property, ResolvedProperty from .knowledge_source import KnowledgeSource # from ..designator import DesignatorDescription, ActionDesignatorDescription -from typing_extensions import Type, Callable, List +from typing_extensions import Type, Callable, List, TYPE_CHECKING, Dict from ..plan_failures import KnowledgeNotAvailable # This import is needed since the subclasses of KnowledgeSource need to be imported to be known at runtime from .knowledge_sources import * +if TYPE_CHECKING: + from ..designator import ActionDesignatorDescription class KnowledgeEngine: @@ -153,3 +158,35 @@ def find_source_for_property(self, property: Type[Property]): if (property.__class__ in list(source.__class__.__bases__) and source.is_connected): return source + + def match_reasoned_parameter(self, condition: Type[Property], designator: Type[ActionDesignatorDescription]): + """ + Match the reasoned parameters, in the root node of the property expression, to the corresponding parameter in + the designator + """ + parameter = condition.root.variables + self._match_by_name(parameter, designator) + self._match_by_type(parameter, designator) + + @staticmethod + def _match_by_name(parameter: Dict[str, any], designator: Type[ActionDesignatorDescription]): + """ + Match the reasoned parameters to the corresponding parameter in the designator by name + """ + for key, value in parameter.items(): + if key in designator.get_optional_parameter() and designator.__getattribute__(key) is None: + designator.__setattr__(key, value) + + @staticmethod + def _match_by_type(parameter: Dict[str, any], designator: Type[ActionDesignatorDescription]): + """ + Match the reasoned parameters to the corresponding parameter in the designator by type + """ + for param in designator.get_optional_parameter(): + if designator.__getattribute__(param) is None: + for key, value in parameter.items(): + try: + check_type(value, designator.get_type_hints()[param]) + designator.__setattr__(param, value) + except TypeCheckError as e: + continue diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index 50ce0ee71..cb689df1c 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -16,6 +16,7 @@ from .language import Language from .robot_description import RobotDescription from typing_extensions import TYPE_CHECKING +from .datastructures.enums import ExecutionType if TYPE_CHECKING: from .designators.motion_designator import BaseMotion @@ -184,7 +185,7 @@ def plan(): def wrapper(*args, **kwargs): pre = ProcessModuleManager.execution_type - ProcessModuleManager.execution_type = "simulated" + ProcessModuleManager.execution_type = ExecutionType.SIMULATED ret = func(*args, **kwargs) ProcessModuleManager.execution_type = pre return ret From 4df1c49d13217296a4fbca7a09414d8846e9d001 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 26 Sep 2024 13:49:42 +0200 Subject: [PATCH 29/57] [action desigs] Added pre and post perform and small refactor --- src/pycram/designator.py | 32 +- src/pycram/designators/action_designator.py | 1544 ++++++++++--------- 2 files changed, 819 insertions(+), 757 deletions(-) diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 3a35a720d..b5acc6270 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -160,6 +160,11 @@ class ActionDesignatorDescription(DesignatorDescription, Language): 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: """ @@ -185,13 +190,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. diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index a647cd539..13fba336c 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -45,1039 +45,1071 @@ 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) """ - 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], - ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): + @abc.abstractmethod + def plan(self) -> None: """ - Create a designator description to move the torso of the robot up and down. + plan of the action. - :param positions: List of possible positions of the robots torso, possible position is a float of height in metres - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + Will be overwritten by each action. """ - super().__init__(ontology_concept_holders) - self.positions: List[float] = positions - - if self.soma: - self.init_ontology_concepts({"move_torso": self.soma.MoveTorso}) + pass - def ground(self) -> MoveTorsoActionPerformable: + def to_sql(self) -> Action: """ - Creates a performable action designator with the first element from the list of possible torso heights. + Convert this action to its ORM equivalent. - :return: A performable action designator + Needs to be overwritten by an action if it didn't overwrite the orm_class attribute with its ORM equivalent. + + :return: An instance of the ORM equivalent of the action with the parameters set """ - return MoveTorsoActionPerformable(self.positions[0]) + # get all class parameters (ignore inherited ones) + class_variables = {key: value for key, value in vars(self).items() + if key in inspect.getfullargspec(self.__init__).args} - def __iter__(self): + # get all orm class parameters (ignore inherited ones) + 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) + + def insert(self, session: Session, **kwargs) -> Action: """ - Iterates over all possible values for this designator and returns a performable action designator with the value. + Insert this action into the database. - :return: A performable action designator + 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. + + :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 position in self.positions: - yield MoveTorsoActionPerformable(position) + action = super().insert(session) + + # get all class parameters (ignore inherited ones) + class_variables = {key: value for key, value in vars(self).items() + if key in inspect.getfullargspec(self.__init__).args} + + # get all orm class parameters (ignore inherited ones) + orm_class_variables = inspect.getfullargspec(self.orm_class.__init__).args -class SetGripperAction(ActionDesignatorDescription): + # 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): """ - Set the gripper state of the robot + Move the torso of the robot up and down. """ - def __init__(self, grippers: List[Arms], motions: List[GripperState], - ontology_concept_holders: Optional[List[Thing]] = None): - """ - Sets the gripper state, the desired state is given with the motion. Motion can either be 'open' or 'close'. + position: float + """ + Target position of the torso joint + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMMoveTorsoAction) - :param grippers: A list of possible grippers - :param motions: A list of possible motions - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - self.grippers: List[Arms] = grippers - self.motions: List[GripperState] = motions - if len(self.grippers) == 1: - self.knowledge_condition = GripperIsFreeProperty(self.grippers[0]) - else: - root = GripperIsFreeProperty(self.grippers[0]) - for gripper in grippers[1:]: - root |= GripperIsFreeProperty(gripper) - self.knowledge_condition = root + @with_tree + def plan(self) -> None: + MoveJointsMotion([RobotDescription.current_robot_description.torso_joint], [self.position]).perform() - if self.soma: - self.init_ontology_concepts({"setting_gripper": self.soma.SettingGripper}) - def ground(self) -> SetGripperActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first element in the grippers and motions list. - :return: A performable designator - """ - return SetGripperActionPerformable(self.grippers[0], self.motions[0]) +@dataclass +class SetGripperActionPerformable(ActionAbstract): + """ + Set the gripper state of the robot. + """ - def __iter__(self): - """ - Iterates over all possible combinations of grippers and motions + 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) - :return: A performable designator with a combination of gripper and motion - """ - for parameter_combination in itertools.product(self.grippers, self.motions): - yield SetGripperActionPerformable(*parameter_combination) + @with_tree + def plan(self) -> None: + MoveGripperMotion(gripper=self.gripper, motion=self.motion).perform() -class ReleaseAction(ActionDesignatorDescription): +@dataclass +class ReleaseActionPerformable(ActionAbstract): """ Releases an Object from the robot. - Note: This action can not be used yet. + Note: This action can not ve used yet. """ - def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, - ontology_concept_holders: Optional[List[Thing]] = None): - super().__init__(ontology_concept_holders) - self.grippers: List[Arms] = grippers - self.object_designator_description = object_designator_description + gripper: Arms - if self.soma: - self.init_ontology_concepts({"releasing": self.soma.Releasing}) + object_designator: ObjectDesignatorDescription.Object - def ground(self) -> ReleaseActionPerformable: - return ReleaseActionPerformable(self.grippers[0], self.object_designator_description.ground()) + def plan(self) -> None: + raise NotImplementedError -class GripAction(ActionDesignatorDescription): +@dataclass +class GripActionPerformable(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. """ - def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, - efforts: List[float], ontology_concept_holders: Optional[List[Thing]] = None): - super().__init__( ontology_concept_holders) - self.grippers: List[Arms] = grippers - self.object_designator_description: ObjectDesignatorDescription = object_designator_description - self.efforts: List[float] = efforts - - if self.soma: - self.init_ontology_concepts({"holding": self.soma.Holding}) + gripper: Arms + object_designator: ObjectDesignatorDescription.Object + effort: float - def ground(self) -> GripActionPerformable: - return GripActionPerformable(self.grippers[0], self.object_designator_description.ground(), self.efforts[0]) + @with_tree + def plan(self) -> None: + raise NotImplementedError() -class ParkArmsAction(ActionDesignatorDescription): +@dataclass +class ParkArmsActionPerformable(ActionAbstract): """ Park the arms of the robot. """ - def __init__(self, arms: List[Arms], - ontology_concept_holders: Optional[List[Thing]] = None): - """ - Moves the arms in the pre-defined parking position. Arms are taken from pycram.enum.Arms + arm: Arms + """ + Entry from the enum for which arm should be parked + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMParkArmsAction) - :param arms: A list of possible arms, that could be used - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - self.arms: List[Arms] = arms + @with_tree + def plan(self) -> None: + # create the keyword arguments + kwargs = dict() + left_poses = None + right_poses = None - if self.soma: - self.init_ontology_concepts({"parking_arms": self.soma.ParkingArms}) + # 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"]) - def ground(self) -> ParkArmsActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first element of the list of possible arms + # 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: A performable designator - """ - return ParkArmsActionPerformable(self.arms[0]) + MoveArmJointsMotion(left_poses, right_poses).perform() -class PickUpAction(ActionDesignatorDescription): +@dataclass +class PickUpActionPerformable(ActionAbstract): """ - Designator to let the robot pick up an object. + Let the robot pick up an object. """ - def __init__(self, - object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], - arms: List[Arms], grasps: List[Grasp], - ontology_concept_holders: Optional[List[Thing]] = 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. - - :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 ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - 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) + object_designator: ObjectDesignatorDescription.Object + """ + Object designator describing the object that should be picked up + """ - if self.soma: - self.init_ontology_concepts({"picking_up": self.soma.PickingUp}) + arm: Arms + """ + The arm that should be used for pick up + """ - def ground(self) -> PickUpActionPerformable: - """ - Default specialized_designators, returns a performable designator with the first entries from the lists of possible parameter. + grasp: Grasp + """ + The grasp that should be used. For example, 'left' or 'right' + """ - :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() + 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) - return PickUpActionPerformable(obj_desig, self.arms[0], self.grasps[0]) + @with_tree + def plan(self) -> None: + # Store the object's data copy at execution + self.object_at_execution = self.object_designator.frozen_copy() + 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 + adjusted_oTm.multiply_quaternions(grasp) -class PlaceAction(ActionDesignatorDescription): - """ - Places an Object at a position using an arm. - """ + # 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 __init__(self, - object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], - target_locations: List[Pose], - arms: List[Arms], ontology_concept_holders: Optional[List[Thing]] = None): - """ - Create an Action Description to place an object + # Perform Gripper Rotate + # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) + # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform() - :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 ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - self.object_designator_description: Union[ - ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description - self.target_locations: List[Pose] = target_locations - self.arms: List[Arms] = arms + 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") - if self.soma: - self.init_ontology_concepts({"placing": self.soma.Placing}) + # 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() - def ground(self) -> PlaceActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first entries from the list of possible entries. + # 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) - :return: A performable designator - """ - obj_desig = self.object_designator_description if isinstance(self.object_designator_description, - ObjectDesignatorDescription.Object) else self.object_designator_description.resolve() + # Lift object + World.current_world.add_vis_axis(adjusted_oTm) + MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True).perform() - return PlaceActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) + # Remove the vis axis from the world + World.current_world.remove_vis_axis() -class NavigateAction(ActionDesignatorDescription): +@dataclass +class PlaceActionPerformable(ActionAbstract): """ - Navigates the Robot to a position. + Places an Object at a position using an arm. """ - def __init__(self, target_locations: List[Pose], - ontology_concept_holders: Optional[List[Thing]] = None): - """ - Navigates the robot to a location. + 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) - :param target_locations: A list of possible target locations for the navigation. - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - 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 + @with_tree + def plan(self) -> None: + object_pose = self.object_designator.world_object.get_pose() + local_tf = LocalTransformer() - if self.soma: - self.init_ontology_concepts({"navigating": self.soma.Navigating}) + # 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() - def ground(self) -> NavigateActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first entry of possible target locations. + 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() - :return: A performable designator - """ - return NavigateActionPerformable(self.target_locations[0]) +@dataclass +class NavigateActionPerformable(ActionAbstract): + """ + Navigates the Robot to a position. + """ -class TransportAction(ActionDesignatorDescription): + target_location: Pose """ - Transports an object to a position using an arm + Location to which the robot should be navigated """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMNavigateAction) - def __init__(self, - object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], - arms: List[Arms], - target_locations: List[Pose], - ontology_concept_holders: Optional[List[Thing]] = None): - """ - Designator representing a pick and place plan. + @with_tree + def plan(self) -> None: + MoveMotion(self.target_location).perform() - :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 ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - self.object_designator_description: Union[ - ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description - self.arms: List[Arms] = arms - self.target_locations: List[Pose] = target_locations - if self.soma: - self.init_ontology_concepts({"transporting": self.soma.Transporting}) +@dataclass +class TransportActionPerformable(ActionAbstract): + """ + Transports an object to a position using an arm + """ - def ground(self) -> TransportActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first entries from the lists of possible parameter. + 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 - """ - obj_desig = self.object_designator_description \ - if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) \ - else self.object_designator_description.resolve() + @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 posotion 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}") - return TransportActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) + 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() -class LookAtAction(ActionDesignatorDescription): +@dataclass +class LookAtActionPerformable(ActionAbstract): """ Lets the robot look at a position. """ - def __init__(self, targets: List[Pose], - ontology_concept_holders: Optional[List[Thing]] = None): - """ - Moves the head of the robot such that it points towards the given target location. - - :param targets: A list of possible locations to look at - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - self.targets: List[Pose] = targets - - if self.soma: - self.init_ontology_concepts({"looking_at": self.soma.LookingAt}) - - def ground(self) -> LookAtActionPerformable: - """ - Default specialized_designators that returns a performable designator with the first entry in the list of possible targets + target: Pose + """ + Position at which the robot should look, given as 6D pose + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMLookAtAction) - :return: A performable designator - """ - return LookAtActionPerformable(self.targets[0]) + @with_tree + def plan(self) -> None: + LookingMotion(target=self.target).perform() -class DetectAction(ActionDesignatorDescription): +@dataclass +class DetectActionPerformable(ActionAbstract): """ Detects an object that fits the object description and returns an object designator describing the object. """ - 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. - - :param object_designator_description: Object designator describing the object - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - self.object_designator_description: ObjectDesignatorDescription = object_designator_description - self.knowledge_condition = VisibleProperty(self.object_designator_description) - - if self.soma: - self.init_ontology_concepts({"looking_for": self.soma.LookingFor, - "checking_object_presence": self.soma.CheckingObjectPresence}) - - def ground(self) -> DetectActionPerformable: - """ - Default specialized_designators that returns a performable designator with the resolved object description. + object_designator: ObjectDesignatorDescription.Object + """ + Object designator loosely describing the object, e.g. only type. + """ + orm_class: Type[ActionAbstract] = field(init=False, default=ORMDetectAction) - :return: A performable designator - """ - return DetectActionPerformable(self.object_designator_description.resolve()) + @with_tree + def plan(self) -> None: + return DetectingMotion(object_type=self.object_designator.obj_type).perform() -class OpenAction(ActionDesignatorDescription): +@dataclass +class OpenActionPerformable(ActionAbstract): """ Opens a container like object + """ - Can currently not be used + object_designator: ObjectPart.Object + """ + Object designator 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) - def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], - ontology_concept_holders: Optional[List[Thing]] = None): - """ - Moves the arm of the robot to open a container. + @with_tree + def plan(self) -> None: + GraspingActionPerformable(self.arm, self.object_designator).perform() + OpeningMotion(self.object_designator, self.arm).perform() - :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 ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - self.object_designator_description: ObjectPart = object_designator_description - self.arms: List[Arms] = arms - self.knowledge_condition = ReachableProperty( - self.object_designator_description.resolve().pose) & GripperIsFreeProperty(self.arms[0]) + MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform() - if self.soma: - self.init_ontology_concepts({"opening": self.soma.Opening}) - def ground(self) -> OpenActionPerformable: - """ - Default specialized_designators that returns a performable designator with the resolved object description and the first entries - from the lists of possible parameter. +@dataclass +class CloseActionPerformable(ActionAbstract): + """ + Closes a container like object. + """ - :return: A performable designator - """ - return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0]) + 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) + @with_tree + def plan(self) -> None: + GraspingActionPerformable(self.arm, self.object_designator).perform() + ClosingMotion(self.object_designator, self.arm).perform() -class CloseAction(ActionDesignatorDescription): - """ - Closes a container like object. + MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform() - Can currently not be used + +@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) - def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], - ontology_concept_holders: Optional[List[Thing]] = None): - """ - Attempts to close an open container + @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() - :param object_designator_description: Object designator description of the handle that should be used - :param arms: A list of possible arms to use - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - self.object_designator_description: ObjectPart = object_designator_description - self.arms: List[Arms] = arms - self.knowledge_condition = ReachableProperty( - self.object_designator_description.resolve().pose) & GripperIsFreeProperty(self.arms[0]) + object_pose_in_gripper = lt.transform_pose(object_pose, + World.robot.get_link_tf_frame(gripper_name)) - if self.soma: - self.init_ontology_concepts({"closing": self.soma.Closing}) + pre_grasp = object_pose_in_gripper.copy() + pre_grasp.pose.position.x -= 0.1 - 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. + MoveTCPMotion(pre_grasp, self.arm).perform() + MoveGripperMotion(GripperState.OPEN, self.arm).perform() - :return: A performable designator - """ - return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0]) + MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).perform() + MoveGripperMotion(GripperState.CLOSE, self.arm, allow_gripper_collision=True).perform() -class GraspingAction(ActionDesignatorDescription): +@dataclass +class FaceAtPerformable(ActionAbstract): """ - Grasps an object described by the given Object Designator description + Turn the robot chassis such that is faces the ``pose`` and after that perform a look at action. """ - def __init__(self, arms: List[Arms], object_description: Union[ObjectDesignatorDescription, ObjectPart], - ontology_concept_holders: Optional[List[Thing]] = 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. + pose: Pose + """ + The pose to face + """ - :param arms: List of Arms that should be used for grasping - :param object_description: Description of the object that should be grasped - :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with - """ - super().__init__(ontology_concept_holders) - self.arms: List[Arms] = arms - self.object_description: ObjectDesignatorDescription = object_description + orm_class = ORMFaceAtAction - if self.soma: - self.init_ontology_concepts({"grasping": self.soma.Grasping}) + @with_tree + def plan(self) -> None: + # get the robot position + robot_position = World.robot.pose - 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. + # 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")) - :return: A performable action designator that contains specific arguments - """ - return GraspingActionPerformable(self.arms[0], self.object_description.resolve()) + # create new robot pose + new_robot_pose = Pose(robot_position.position_as_list(), orientation) + # turn robot + NavigateActionPerformable(new_robot_pose).perform() -# ---------------------------------------------------------------------------- -# ---------------- Performables ---------------------------------------------- -# ---------------------------------------------------------------------------- + # look at target + LookAtActionPerformable(self.pose).perform() @dataclass -class ActionAbstract(ActionDesignatorDescription.Action, abc.ABC): - """Base class for performable performables.""" - orm_class: Type[ORMAction] = field(init=False, default=None) +class MoveAndPickUpPerformable(ActionAbstract): """ - 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. + Navigate to `standing_position`, then turn towards the object and pick it up. """ - @abc.abstractmethod - def perform(self) -> None: - """ - Perform the action. + standing_position: Pose + """ + The pose to stand before trying to pick up the object + """ - Will be overwritten by each action. - """ - pass + object_designator: ObjectDesignatorDescription.Object + """ + The object to pick up + """ - def to_sql(self) -> Action: - """ - Convert this action to its ORM equivalent. + arm: Arms + """ + The arm to use + """ - Needs to be overwritten by an action if it didn't overwrite the orm_class attribute with its ORM equivalent. + grasp: Grasp + """ + The grasp to use + """ - :return: An instance of the ORM equivalent of the action with the parameters set - """ - # get all class parameters (ignore inherited ones) - class_variables = {key: value for key, value in vars(self).items() - if key in inspect.getfullargspec(self.__init__).args} + def plan(self): + NavigateActionPerformable(self.standing_position).perform() + FaceAtPerformable(self.object_designator.pose).perform() + PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform() - # get all orm class parameters (ignore inherited ones) - 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) +class MoveTorsoAction(ActionDesignatorDescription): + """ + Action Designator for Moving the torso of the robot up and down + """ + PerformableClass = MoveTorsoActionPerformable - def insert(self, session: Session, **kwargs) -> Action: + def __init__(self, positions: List[float], + ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): """ - 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. + Create a designator description to move the torso of the robot up and down. - :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 + :param positions: List of possible positions of the robots torso, possible position is a float of height in metres + :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ + super().__init__(ontology_concept_holders) + self.positions: List[float] = positions - action = super().insert(session) + if self.soma: + self.init_ontology_concepts({"move_torso": self.soma.MoveTorso}) - # get all class parameters (ignore inherited ones) - class_variables = {key: value for key, value in vars(self).items() - if key in inspect.getfullargspec(self.__init__).args} + def ground(self) -> MoveTorsoActionPerformable: + """ + Creates a performable action designator with the first element from the list of possible torso heights. - # get all orm class parameters (ignore inherited ones) - orm_class_variables = inspect.getfullargspec(self.orm_class.__init__).args + :return: A performable action designator + """ + return MoveTorsoActionPerformable(self.positions[0]) - # 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 __iter__(self): + """ + Iterates over all possible values for this designator and returns a performable action designator with the value. - return action + :return: A performable action designator + """ + for position in self.positions: + yield MoveTorsoActionPerformable(position) -@dataclass -class MoveTorsoActionPerformable(ActionAbstract): +class SetGripperAction(ActionDesignatorDescription): """ - Move the torso of the robot up and down. + Set the gripper state of the robot """ - position: float - """ - Target position of the torso joint - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMMoveTorsoAction) + performable_class = SetGripperActionPerformable - @with_tree - def perform(self) -> None: - MoveJointsMotion([RobotDescription.current_robot_description.torso_joint], [self.position]).perform() + def __init__(self, grippers: List[Arms], motions: List[GripperState], + ontology_concept_holders: Optional[List[Thing]] = None): + """ + Sets the gripper state, the desired state is given with the motion. Motion can either be 'open' or 'close'. + :param grippers: A list of possible grippers + :param motions: A list of possible motions + :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + self.grippers: List[Arms] = grippers + self.motions: List[GripperState] = motions + if len(self.grippers) == 1: + self.knowledge_condition = GripperIsFreeProperty(self.grippers[0]) + else: + root = GripperIsFreeProperty(self.grippers[0]) + for gripper in grippers[1:]: + root |= GripperIsFreeProperty(gripper) + self.knowledge_condition = root -@dataclass -class SetGripperActionPerformable(ActionAbstract): - """ - Set the gripper state of the robot. - """ + if self.soma: + self.init_ontology_concepts({"setting_gripper": self.soma.SettingGripper}) - 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) + def ground(self) -> SetGripperActionPerformable: + """ + Default specialized_designators that returns a performable designator with the first element in the grippers and motions list. - @with_tree - def perform(self) -> None: - MoveGripperMotion(gripper=self.gripper, motion=self.motion).perform() + :return: A performable designator + """ + return SetGripperActionPerformable(self.grippers[0], self.motions[0]) + + def __iter__(self): + """ + Iterates over all possible combinations of grippers and motions + :return: A performable designator with a combination of gripper and motion + """ + for parameter_combination in itertools.product(self.grippers, self.motions): + yield SetGripperActionPerformable(*parameter_combination) -@dataclass -class ReleaseActionPerformable(ActionAbstract): + +class ReleaseAction(ActionDesignatorDescription): """ Releases an Object from the robot. - Note: This action can not ve used yet. + Note: This action can not be used yet. """ - gripper: Arms + performable_class = ReleaseActionPerformable - object_designator: ObjectDesignatorDescription.Object + def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, + ontology_concept_holders: Optional[List[Thing]] = None): + super().__init__(ontology_concept_holders) + self.grippers: List[Arms] = grippers + self.object_designator_description = object_designator_description - def perform(self) -> None: - raise NotImplementedError + if self.soma: + self.init_ontology_concepts({"releasing": self.soma.Releasing}) + + def ground(self) -> ReleaseActionPerformable: + return ReleaseActionPerformable(self.grippers[0], self.object_designator_description.ground()) -@dataclass -class GripActionPerformable(ActionAbstract): +class GripAction(ActionDesignatorDescription): """ 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. """ - gripper: Arms - object_designator: ObjectDesignatorDescription.Object - effort: float + performable_class = GripActionPerformable - @with_tree - def perform(self) -> None: - raise NotImplementedError() + def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, + efforts: List[float], ontology_concept_holders: Optional[List[Thing]] = None): + super().__init__( ontology_concept_holders) + self.grippers: List[Arms] = grippers + self.object_designator_description: ObjectDesignatorDescription = object_designator_description + self.efforts: List[float] = efforts + if self.soma: + self.init_ontology_concepts({"holding": self.soma.Holding}) -@dataclass -class ParkArmsActionPerformable(ActionAbstract): - """ - Park the arms of the robot. - """ + def ground(self) -> GripActionPerformable: + return GripActionPerformable(self.grippers[0], self.object_designator_description.ground(), self.efforts[0]) - arm: Arms + +class ParkArmsAction(ActionDesignatorDescription): """ - Entry from the enum for which arm should be parked + Park the arms of the robot. """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMParkArmsAction) - @with_tree - def perform(self) -> None: - # create the keyword arguments - kwargs = dict() - left_poses = None - right_poses = None + performable_class = ParkArmsActionPerformable - # 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"]) - - # 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"]) - - MoveArmJointsMotion(left_poses, right_poses).perform() + def __init__(self, arms: List[Arms], + ontology_concept_holders: Optional[List[Thing]] = None): + """ + Moves the arms in the pre-defined parking position. Arms are taken from pycram.enum.Arms + :param arms: A list of possible arms, that could be used + :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + self.arms: List[Arms] = arms -@dataclass -class PickUpActionPerformable(ActionAbstract): - """ - Let the robot pick up an object. - """ + if self.soma: + self.init_ontology_concepts({"parking_arms": self.soma.ParkingArms}) - object_designator: ObjectDesignatorDescription.Object - """ - Object designator describing the object that should be picked up - """ + def ground(self) -> ParkArmsActionPerformable: + """ + Default specialized_designators that returns a performable designator with the first element of the list of possible arms - arm: Arms - """ - The arm that should be used for pick up - """ + :return: A performable designator + """ + return ParkArmsActionPerformable(self.arms[0]) - grasp: Grasp - """ - The grasp that should be used. For example, 'left' or 'right' - """ - object_at_execution: Optional[ObjectDesignatorDescription.Object] = field(init=False) +class PickUpAction(ActionDesignatorDescription): """ - 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. + Designator to let the robot pick up an object. """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMPickUpAction) - - @with_tree - def perform(self) -> None: - # Store the object's data copy at execution - self.object_at_execution = self.object_designator.frozen_copy() - 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 - - 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") + performable_class = PickUpActionPerformable - # Perform Gripper Rotate - # BulletWorld.current_bullet_world.add_vis_axis(gripper_rotate_pose) - # MoveTCPMotion(gripper_rotate_pose, self.arm).resolve().perform() + def __init__(self, + object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], + arms: List[Arms] = None, grasps: List[Grasp] = None, + ontology_concept_holders: Optional[List[Thing]] = 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. - 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") + :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 ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + 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) - # 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() + if self.soma: + self.init_ontology_concepts({"picking_up": self.soma.PickingUp}) - # 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) + def ground(self) -> PickUpActionPerformable: + """ + Default specialized_designators, returns a performable designator with the first entries from the lists of possible parameter. - # Lift object - World.current_world.add_vis_axis(adjusted_oTm) - MoveTCPMotion(adjusted_oTm, self.arm, allow_gripper_collision=True).perform() + :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() - # Remove the vis axis from the world - World.current_world.remove_vis_axis() + return PickUpActionPerformable(obj_desig, self.arms[0], self.grasps[0]) -@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], ontology_concept_holders: Optional[List[Thing]] = 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 + :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + self.object_designator_description: Union[ + ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description + self.target_locations: List[Pose] = target_locations + self.arms: List[Arms] = arms - 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() + if self.soma: + self.init_ontology_concepts({"placing": self.soma.Placing}) + def ground(self) -> PlaceActionPerformable: + """ + Default specialized_designators that returns a performable designator with the first entries from the list of possible entries. -@dataclass -class NavigateActionPerformable(ActionAbstract): + :return: A performable designator + """ + 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]) + + +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], + ontology_concept_holders: Optional[List[Thing]] = None): + """ + Navigates the robot to a location. + :param target_locations: A list of possible target locations for the navigation. + :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + 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): + if self.soma: + self.init_ontology_concepts({"navigating": self.soma.Navigating}) + + 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]) + + +class TransportAction(ActionDesignatorDescription): """ Transports an object to a position using an arm """ - 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) + performable_class = TransportActionPerformable - @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 posotion 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 __init__(self, + object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], + arms: List[Arms], + target_locations: List[Pose], + ontology_concept_holders: Optional[List[Thing]] = None): + """ + Designator representing a pick and place plan. + + :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 ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + self.object_designator_description: Union[ + ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description + self.arms: List[Arms] = arms + self.target_locations: List[Pose] = target_locations + + if self.soma: + self.init_ontology_concepts({"transporting": self.soma.Transporting}) + + def ground(self) -> TransportActionPerformable: + """ + Default specialized_designators that returns a performable designator with the first entries from the lists of possible parameter. + + :return: A performable designator + """ + obj_desig = self.object_designator_description \ + if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) \ + else self.object_designator_description.resolve() - 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 TransportActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) -@dataclass -class LookAtActionPerformable(ActionAbstract): +class LookAtAction(ActionDesignatorDescription): """ Lets the robot look at a position. """ - target: Pose - """ - Position at which the robot should look, given as 6D pose - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMLookAtAction) - - @with_tree - def perform(self) -> None: - LookingMotion(target=self.target).perform() + performable_class = LookAtActionPerformable + def __init__(self, targets: List[Pose], + ontology_concept_holders: Optional[List[Thing]] = None): + """ + Moves the head of the robot such that it points towards the given target location. -@dataclass -class DetectActionPerformable(ActionAbstract): - """ - Detects an object that fits the object description and returns an object designator describing the object. - """ + :param targets: A list of possible locations to look at + :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + self.targets: List[Pose] = targets - object_designator: ObjectDesignatorDescription.Object - """ - Object designator loosely describing the object, e.g. only type. - """ - orm_class: Type[ActionAbstract] = field(init=False, default=ORMDetectAction) + if self.soma: + self.init_ontology_concepts({"looking_at": self.soma.LookingAt}) - @with_tree - def perform(self) -> None: - return DetectingMotion(object_type=self.object_designator.obj_type).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 OpenActionPerformable(ActionAbstract): - """ - Opens a container like object - """ - object_designator: ObjectPart.Object - """ - Object designator describing the object that should be opened - """ - arm: Arms +class DetectAction(ActionDesignatorDescription): """ - Arm that should be used for opening the container + Detects an object that fits the object description and returns an object designator describing the object. """ - 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() - MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform() + performable_class = DetectActionPerformable + 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. -@dataclass -class CloseActionPerformable(ActionAbstract): - """ - Closes a container like object. - """ + :param object_designator_description: Object designator describing the object + :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + self.object_designator_description: ObjectDesignatorDescription = object_designator_description + self.knowledge_condition = VisibleProperty(self.object_designator_description) - 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) + if self.soma: + self.init_ontology_concepts({"looking_for": self.soma.LookingFor, + "checking_object_presence": self.soma.CheckingObjectPresence}) - @with_tree - def perform(self) -> None: - GraspingActionPerformable(self.arm, self.object_designator).perform() - ClosingMotion(self.object_designator, self.arm).perform() + def ground(self) -> DetectActionPerformable: + """ + Default specialized_designators that returns a performable designator with the resolved object description. - MoveGripperMotion(GripperState.OPEN, self.arm, allow_gripper_collision=True).perform() + :return: A performable designator + """ + return DetectActionPerformable(self.object_designator_description.resolve()) -@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 OpenAction(ActionDesignatorDescription): """ - Object Designator for the object that should be grasped + Opens a container like object + + Can currently not be used """ - 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 = OpenActionPerformable - object_pose_in_gripper = lt.transform_pose(object_pose, - World.robot.get_link_tf_frame(gripper_name)) + def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], + ontology_concept_holders: Optional[List[Thing]] = None): + """ + Moves the arm of the robot to open a container. - pre_grasp = object_pose_in_gripper.copy() - pre_grasp.pose.position.x -= 0.1 + :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 ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + self.object_designator_description: ObjectPart = object_designator_description + self.arms: List[Arms] = arms + self.knowledge_condition = ReachableProperty( + self.object_designator_description.resolve().pose) & GripperIsFreeProperty(self.arms[0]) - MoveTCPMotion(pre_grasp, self.arm).perform() - MoveGripperMotion(GripperState.OPEN, self.arm).perform() + if self.soma: + self.init_ontology_concepts({"opening": self.soma.Opening}) - MoveTCPMotion(object_pose, self.arm, allow_gripper_collision=True).perform() - MoveGripperMotion(GripperState.CLOSE, self.arm, allow_gripper_collision=True).perform() + 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]) -@dataclass -class FaceAtPerformable(ActionAbstract): - """ - Turn the robot chassis such that is faces the ``pose`` and after that perform a look at action. +class CloseAction(ActionDesignatorDescription): """ + Closes a container like object. - pose: Pose - """ - The pose to face + Can currently not be used """ - orm_class = ORMFaceAtAction + performable_class = CloseActionPerformable - @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], + ontology_concept_holders: Optional[List[Thing]] = None): + """ + Attempts to close an open 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 of the handle that should be used + :param arms: A list of possible arms to use + :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + self.object_designator_description: ObjectPart = object_designator_description + self.arms: List[Arms] = arms + self.knowledge_condition = ReachableProperty( + self.object_designator_description.resolve().pose) & GripperIsFreeProperty(self.arms[0]) - # create new robot pose - new_robot_pose = Pose(robot_position.position_as_list(), orientation) + if self.soma: + self.init_ontology_concepts({"closing": self.soma.Closing}) - # turn robot - NavigateActionPerformable(new_robot_pose).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. - # look at target - LookAtActionPerformable(self.pose).perform() + :return: A performable designator + """ + return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0]) -@dataclass -class MoveAndPickUpPerformable(ActionAbstract): +class GraspingAction(ActionDesignatorDescription): """ - Navigate to `standing_position`, then turn towards the object and pick it up. + Grasps an object described by the given Object Designator description """ - standing_position: Pose - """ - The pose to stand before trying to pick up the object - """ + performable_class = GraspingActionPerformable - object_designator: ObjectDesignatorDescription.Object - """ - The object to pick up - """ + def __init__(self, arms: List[Arms], object_description: Union[ObjectDesignatorDescription, ObjectPart], + ontology_concept_holders: Optional[List[Thing]] = 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 ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with + """ + super().__init__(ontology_concept_holders) + self.arms: List[Arms] = arms + self.object_description: ObjectDesignatorDescription = object_description - grasp: Grasp - """ - The grasp to use - """ + if self.soma: + self.init_ontology_concepts({"grasping": self.soma.Grasping}) + + def ground(self) -> GraspingActionPerformable: + """ + Default specialized_designators that takes the first element from the list of arms and the first solution for the object + designator description ond returns it. + + :return: A performable action designator that contains specific arguments + """ + return GraspingActionPerformable(self.arms[0], self.object_description.resolve()) - def perform(self): - NavigateActionPerformable(self.standing_position).perform() - FaceAtPerformable(self.object_designator.pose).perform() - PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform() From 9a364e851bf398b7d94f0dec1af8e46d348de2b5 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 2 Oct 2024 15:28:54 +0200 Subject: [PATCH 30/57] [datastructures] Added partial designator class --- .../datastructures/partial_designator.py | 107 ++++++++++++++++++ 1 file changed, 107 insertions(+) create mode 100644 src/pycram/datastructures/partial_designator.py diff --git a/src/pycram/datastructures/partial_designator.py b/src/pycram/datastructures/partial_designator.py new file mode 100644 index 000000000..270b9ec15 --- /dev/null +++ b/src/pycram/datastructures/partial_designator.py @@ -0,0 +1,107 @@ +from collections import Iterable +from typing_extensions import Type, List, Tuple, Any, Dict +from itertools import product +from inspect import signature + +from pycram.designator import ActionDesignatorDescription + + +class PartialDesignator: + """ + A partial designator is somewhat between a DesignatorDescription and a specified designator. Basically it is a + partially initialized specified designator 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 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 + self.args = args + self.kwargs = kwargs + + def __call__(self, *fargs, **fkwargs): + """ + Creats a new PartialDesignator with the given arguments and keyword arguments added. + + :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 + """ + newkeywords = {**self.kwargs, **fkwargs} + return PartialDesignator(self.performable, *self.args, *fargs, **newkeywords) + + 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. + + :return: A new performable object for each permutation of arguments and keyword arguments + """ + for args_combination in PartialDesignator.generate_permutations(self.args): + for kwargs_combination in PartialDesignator.generate_permutations(self.kwargs.values()): + yield self.performable(*args_combination, **dict(zip(self.kwargs.keys(), kwargs_combination))) + + @staticmethod + def generate_permutations(args: Iterable) -> List: + """ + Generates all possible permutations of the given arguments. This uses itertools.product to generate the + permutations. + + :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 + """ + performable_params = signature(self.performable).parameters + # List of all parameter names that need to be filled for the performable + param_names = list(performable_params.keys()) + + # Remove parameter that are filled by args + missing_after_args = param_names[len(self.args):] + # Remove parameter that are filled by keyword arguments and return the remaining parameters + return list(set(missing_after_args) - set(self.kwargs.keys())) + + From 95fcacf70b9055aad7dbbde3eeff53ba13856148 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 9 Oct 2024 15:24:15 +0200 Subject: [PATCH 31/57] [partial desig] Fixed typing imports --- src/pycram/datastructures/partial_designator.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/pycram/datastructures/partial_designator.py b/src/pycram/datastructures/partial_designator.py index 270b9ec15..3089a2849 100644 --- a/src/pycram/datastructures/partial_designator.py +++ b/src/pycram/datastructures/partial_designator.py @@ -1,19 +1,24 @@ +# used for delayed evaluation of typing until python 3.11 becomes mainstream +from __future__ import annotations + from collections import Iterable -from typing_extensions import Type, List, Tuple, Any, Dict +from typing_extensions import Type, List, Tuple, Any, Dict, TYPE_CHECKING from itertools import product from inspect import signature -from pycram.designator import ActionDesignatorDescription + +if TYPE_CHECKING: + from ..designator import ActionDesignatorDescription class PartialDesignator: """ - A partial designator is somewhat between a DesignatorDescription and a specified designator. Basically it is a - partially initialized specified designator which can take a list of input arguments (like a DesignatorDescription) + 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 need + 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 @@ -35,7 +40,7 @@ class PartialDesignator: Keyword arguments that are passed to the performable """ - def __init__(self, performable = Type[ActionDesignatorDescription.Action], *args, **kwargs): + def __init__(self, performable: Type[ActionDesignatorDescription.Action], *args, **kwargs): self.performable = performable self.args = args self.kwargs = kwargs From 88f6bf43bf657c84ec551a3acf0046c5c2916e7b Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 10 Oct 2024 15:10:44 +0200 Subject: [PATCH 32/57] [plan failures] Fixed reasoning and collision errors --- src/pycram/plan_failures.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pycram/plan_failures.py b/src/pycram/plan_failures.py index 0511c437b..b50507afe 100644 --- a/src/pycram/plan_failures.py +++ b/src/pycram/plan_failures.py @@ -409,10 +409,10 @@ 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) From 1948feb1a5e34c25e9a998a051bdc2ca78e0650c Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 11 Oct 2024 10:12:44 +0200 Subject: [PATCH 33/57] [partial desig] Added filters for None arguments --- src/pycram/datastructures/partial_designator.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/pycram/datastructures/partial_designator.py b/src/pycram/datastructures/partial_designator.py index 3089a2849..40d27987b 100644 --- a/src/pycram/datastructures/partial_designator.py +++ b/src/pycram/datastructures/partial_designator.py @@ -42,8 +42,9 @@ class PartialDesignator: def __init__(self, performable: Type[ActionDesignatorDescription.Action], *args, **kwargs): self.performable = performable - self.args = args - self.kwargs = kwargs + # Remove None values fom the given arguments and keyword arguments + self.args = tuple(filter(None, args)) + self.kwargs = {k:v for k,v in kwargs.items() if v is not None} def __call__(self, *fargs, **fkwargs): """ From 41d98d05b113ad492d2428c9cf5dbd2a7a6b206f Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 11 Oct 2024 10:27:48 +0200 Subject: [PATCH 34/57] [property] Added some helpful properties and doc --- src/pycram/datastructures/property.py | 56 ++++++++++++++++++++------- 1 file changed, 43 insertions(+), 13 deletions(-) diff --git a/src/pycram/datastructures/property.py b/src/pycram/datastructures/property.py index c1605ccc0..b3364455a 100644 --- a/src/pycram/datastructures/property.py +++ b/src/pycram/datastructures/property.py @@ -28,8 +28,10 @@ class Property(NodeMixin): def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None): super().__init__() - self.parent = parent - self.variables = {} + self.parent: Property = parent + self.variables: Dict[str, Any] = {} + self.executed: bool = False + self.result: ReasoningResult = None if children: self.children = children @@ -69,8 +71,8 @@ def __invert__(self): def manage_io(self, func: Callable, *args, **kwargs) -> bool: """ - Manages the ReasoningResult of a property function. 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. + 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 @@ -79,8 +81,35 @@ def manage_io(self, func: Callable, *args, **kwargs) -> bool: """ reasoning_result = func(*args, **kwargs) self.root.variables.update(reasoning_result.reasoned_parameter) + self.result = reasoning_result return reasoning_result.success + @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 + 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. This is done by iterating over the tree and checking if all + nodes are either a ResolvedProperty or a 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): """ @@ -97,6 +126,7 @@ def __init__(self, properties: List[Property]): :param properties: A list of properties to which are the children of this node """ super().__init__(parent=None, children=properties) + self.result = True def simplify(self): """ @@ -132,10 +162,10 @@ class And(PropertyOperator): def __init__(self, properties: List[Property]): """ - Initialize the And with a list of aspects as the children of this node. This node will be the root of the + 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 aspects which are the children of this node + :param properties: A list of properties which are the children of this node """ super().__init__(properties) self.simplify() @@ -152,7 +182,7 @@ def __call__(self, *args, **kwargs) -> bool: """ result = True for child in self.children: - # Child is a Property and the resolved function should be called + # Child is a Property and the executed function should be called if child.is_leaf: result = result and child(*args, **kwargs) # Child is a PropertyOperator @@ -171,10 +201,10 @@ class Or(PropertyOperator): def __init__(self, properties: List[Property]): """ - Initialize the Or with a list of aspects as the children of this node. This node will be the root of the + 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 aspects which are the children of this node + :param properties: A list of properties which are the children of this node """ super().__init__(properties) self.simplify() @@ -191,7 +221,7 @@ def __call__(self, *args, **kwargs) -> bool: """ result = False for child in self.children: - # Child is a Property and the resolved function should be called + # Child is a Property and the executed function should be called if child.is_leaf: result = result or child(*args, **kwargs) # Child is a PropertyOperator @@ -230,7 +260,7 @@ def __call__(self, *args, **kwargs) -> bool: class ResolvedProperty(Property): """ - Class to represent a resolved property function. It holds the reference to the respective function in the knowledge + Class to represent a executed 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. """ @@ -246,7 +276,7 @@ class ResolvedProperty(Property): def __init__(self, resolved_property_function: Callable, property_exception: Type[PlanFailure], parent: NodeMixin = None): """ - Initialize the ResolvedProperty with the resolved property function, the exception that should be raised if the property + 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 @@ -258,7 +288,7 @@ def __init__(self, resolved_property_function: Callable, property_exception: Typ self.property_exception = property_exception self.parameter = {} - def __call__(self) -> bool: + def __call__(self, *args, **kwargs) -> bool: """ 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. From 847a75646e9d5d7c300abd12c4dbfc8db7e92698 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 11 Oct 2024 10:30:52 +0200 Subject: [PATCH 35/57] [knowledge engine] Added Reasoning instance and clean up --- src/pycram/knowledge/knowledge_engine.py | 139 ++++++++++++++--------- 1 file changed, 87 insertions(+), 52 deletions(-) diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index 2d45df6e6..0f2b18e08 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -6,14 +6,15 @@ from anytree import PreOrderIter from typeguard import check_type, TypeCheckError +from ..datastructures.partial_designator import PartialDesignator from ..datastructures.property import Property, ResolvedProperty from .knowledge_source import KnowledgeSource -# from ..designator import DesignatorDescription, ActionDesignatorDescription -from typing_extensions import Type, Callable, List, TYPE_CHECKING, Dict +from typing_extensions import Type, Callable, List, TYPE_CHECKING, Dict, Any -from ..plan_failures import KnowledgeNotAvailable +from ..plan_failures import KnowledgeNotAvailable, ReasoningError # This import is needed since the subclasses of KnowledgeSource need to be imported to be known at runtime from .knowledge_sources import * + if TYPE_CHECKING: from ..designator import ActionDesignatorDescription @@ -41,6 +42,10 @@ def __new__(cls): 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") @@ -76,7 +81,7 @@ def update_sources(self): def query(self, designator: Type['ActionDesignatorDescription']) -> bool: """ - Query to fill parameters of a designator from the knowledge sources + Query to fill parameters of a designator_description from the knowledge sources :return: """ @@ -85,17 +90,19 @@ def query(self, designator: Type['ActionDesignatorDescription']) -> bool: return True self.update_sources() - condition = self.resolve_aspects(designator.knowledge_condition) + condition = self.resolve_properties(designator.knowledge_condition) return condition(designator) - def resolve_aspects(self, aspects: Property): + def resolve_properties(self, properties: Property): """ Traverses the tree of properties and resolves the property functions to the corresponding function in the knowledge - source. + source. Properties are executed in-place. - :param aspects: Root node of the tree of properties + :param properties: Root node of the tree of properties """ - for child in PreOrderIter(aspects): + 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__( @@ -107,11 +114,11 @@ def resolve_aspects(self, aspects: Property): for param in inspect.signature(resolved_aspect_function).parameters.keys(): node.parameter[param] = child.__getattribute__(param) child.parent = None - return node.root + return properties.root def update(self): """ - Update the knowledge sources with new information contained in a designator + Update the knowledge sources with new information contained in a designator_description :return: """ @@ -125,29 +132,11 @@ def ground_solution(self, designator: Type['DesignatorDescription']) -> bool: :return: True if the solution achieves the desired goal, False otherwise """ - if not self.enabled: + if not self.enabled: rospy.logwarn("Knowledge engine is disabled") return True - def call_source(self, query_function: Callable, *args, **kwargs): - """ - Call the given query function on the knowledge source with the highest priority that is connected - - :param query_function: The query function of the knowledge source - :param args: The arguments to pass to the query function - :param kwargs: The keyword arguments to pass to the query function - :return: The result of the query function - """ - self.update_sources() - for source in self.knowledge_sources: - if (query_function.__name__ in list(source.__class__.__dict__.keys()) - and source.is_connected): - source_query_function = getattr(source, query_function.__name__) - return source_query_function(*args, **kwargs) - raise KnowledgeNotAvailable( - f"Query function {query_function.__name__} is not available in any connected knowledge source") - - def find_source_for_property(self, property: Type[Property]): + def find_source_for_property(self, property: Type[Property]) -> Type[KnowledgeSource]: """ Find the source for the given property @@ -159,34 +148,80 @@ def find_source_for_property(self, property: Type[Property]): and source.is_connected): return source - def match_reasoned_parameter(self, condition: Type[Property], designator: Type[ActionDesignatorDescription]): + def match_reasoned_parameter(self, reasoned_parameter: Dict[str, any], + designator: Type[ActionDesignatorDescription]) -> Dict[str, any]: """ Match the reasoned parameters, in the root node of the property expression, to the corresponding parameter in - the designator + the designator_description """ - parameter = condition.root.variables - self._match_by_name(parameter, designator) - self._match_by_type(parameter, designator) + 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: Type[ActionDesignatorDescription]): + def _match_by_name(parameter: Dict[str, any], designator: Type[ActionDesignatorDescription]) -> Dict[str, any]: """ - Match the reasoned parameters to the corresponding parameter in the designator by name + 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.get_optional_parameter() and designator.__getattribute__(key) is None: - designator.__setattr__(key, value) + # 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: Type[ActionDesignatorDescription]): - """ - Match the reasoned parameters to the corresponding parameter in the designator by type - """ - for param in designator.get_optional_parameter(): - if designator.__getattribute__(param) is None: - for key, value in parameter.items(): - try: - check_type(value, designator.get_type_hints()[param]) - designator.__setattr__(param, value) - except TypeCheckError as e: - continue + def _match_by_type(parameter: Dict[str, any], designator: Type[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: + 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: Type[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) -> Type[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 designator in self.partial_designator(**matched_parameter): + yield designator From 6448a1a382195ede8d436c16ee9a63328e96ed72 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 11 Oct 2024 10:44:15 +0200 Subject: [PATCH 36/57] [Designator] Some small methods and doc --- src/pycram/designator.py | 71 ++++++++++++++++++++++------------------ 1 file changed, 39 insertions(+), 32 deletions(-) diff --git a/src/pycram/designator.py b/src/pycram/designator.py index b5acc6270..e5db34489 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -45,10 +45,10 @@ 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) @@ -73,14 +73,14 @@ def __init__(self, missing_properties: List[str], wrong_type: Dict, current_type 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, ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): """ Create a Designator description. - :param ontology_concept_holders: A list of holders of ontology concepts that the designator is categorized as or associated with + :param ontology_concept_holders: A list of holders of ontology concepts that the designator_description is categorized as or associated with """ # self.resolve = self.ground @@ -130,20 +130,20 @@ def get_default_ontology_concept(self) -> owlready2.Thing | None: def get_optional_parameter(self) -> List[str]: """ - Returns a list of optional parameter names of this designator description. + 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. + 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. + Returns the type hints of the __init__ method of this designator_description description. :return: """ @@ -151,7 +151,7 @@ def get_type_hints(self) -> Dict[str, Any]: 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. """ @@ -168,7 +168,7 @@ class ActionDesignatorDescription(DesignatorDescription, Language): @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) """ @@ -259,9 +259,18 @@ def insert(self, session: Session, *args, **kwargs) -> ORMAction: return action + @classmethod + def get_type_hints(cls) -> Dict[str, Any]: + """ + Returns the type hints of the __init__ method of this designator_description description. + + :return: + """ + return typing_extensions.get_type_hints(cls) + def __init__(self, ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): """ - Base of all action designator descriptions. + Base of all action designator_description descriptions. :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ @@ -271,11 +280,16 @@ def __init__(self, ontology_concept_holders: Optional[List[OntologyConceptHolder self.soma = OntologyManager().soma self.knowledge_conditions = None - def resolve(self): - if self.knowledge_condition: - ke = KnowledgeEngine() - ke.query(self) - return self.ground() + 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. """ @@ -283,7 +297,7 @@ def ground(self) -> Action: def init_ontology_concepts(self, ontology_concept_classes: Dict[str, Type[owlready2.Thing]]): """ - Initialize the ontology concept holders for this action designator + Initialize the ontology concept holders for this action designator_description :param ontology_concept_classes: The ontology concept classes that the action is categorized as or associated with :param ontology_concept_name: The name of the ontology concept instance to be created @@ -296,29 +310,22 @@ def init_ontology_concepts(self, ontology_concept_classes: Dict[str, Type[owlrea self.ontology_concept_holders.extend(existing_holders if existing_holders \ else [OntologyConceptHolder(concept_class(concept_name))]) - def __iter__(self): - """ - Iterate through all possible performables fitting this description - - :yield: A resolved action designator - """ - yield self.ground() 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, ontology_concept_holders: Optional[List[owlready2.Thing]] = None): @@ -345,7 +352,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. """ @@ -399,7 +406,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 @@ -408,7 +415,7 @@ def insert(self, session: Session) -> ORMObjectDesignator: def frozen_copy(self) -> 'ObjectDesignatorDescription.Object': """ - Returns a copy of this designator containing only the fields. + Returns a copy of this designator_description containing only the fields. :return: A copy containing only the fields of this class. The WorldObject attached to this pycram object is not copied. The _pose gets set to a method that statically returns the pose of the object when this method was called. """ @@ -470,7 +477,7 @@ def special_knowledge_adjustment_pose(self, grasp: Grasp, pose: Pose) -> Pose: def __init__(self, names: Optional[List[str]] = None, types: Optional[List[ObjectType]] = None, ontology_concept_holders: Optional[List[owlready2.Thing]] = 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 @@ -484,7 +491,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)) @@ -492,7 +499,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: From 652aa38ae997f9532517bf42ace51ed7613cfe35 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 11 Oct 2024 10:46:25 +0200 Subject: [PATCH 37/57] [general] renaming --- src/pycram/__init__.py | 2 +- src/pycram/language.py | 10 +++++----- src/pycram/process_module.py | 4 ++-- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/pycram/__init__.py b/src/pycram/__init__.py index d1d5875f4..8e87af9b3 100644 --- a/src/pycram/__init__.py +++ b/src/pycram/__init__.py @@ -11,7 +11,7 @@ Now launch run.py to start your program. Modules: -designator -- implementation of designators. +designator_description -- implementation of designators. fluent -- implementation of fluents and the whenever macro. helper -- implementation of helper classes and functions for internal usage only. language -- implementation of the CRAM language. diff --git a/src/pycram/language.py b/src/pycram/language.py index 81612a78b..3b6520f0b 100644 --- a/src/pycram/language.py +++ b/src/pycram/language.py @@ -42,7 +42,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 """ @@ -59,7 +59,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): @@ -71,7 +71,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): @@ -83,7 +83,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): @@ -99,7 +99,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 cb689df1c..f6d984396 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -50,11 +50,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: From 7c05875cdafdf97bab2dc5166d8a27c7e1477ec4 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 11 Oct 2024 10:47:23 +0200 Subject: [PATCH 38/57] [decision tree] Removed file --- src/pycram/datastructures/decision_tree.py | 158 --------------------- 1 file changed, 158 deletions(-) delete mode 100644 src/pycram/datastructures/decision_tree.py diff --git a/src/pycram/datastructures/decision_tree.py b/src/pycram/datastructures/decision_tree.py deleted file mode 100644 index 7b4a704ad..000000000 --- a/src/pycram/datastructures/decision_tree.py +++ /dev/null @@ -1,158 +0,0 @@ -# used for delayed evaluation of typing until python 3.11 becomes mainstream -from __future__ import annotations - -from anytree import NodeMixin -from typing_extensions import Iterable, Callable - -from ..designator import DesignatorDescription - - -class DecisionTreeNode(NodeMixin): - """ - Base class for nodes in the Decision Tree - """ - - def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = None): - """ - A node in the decision tree needs a parent and can have children. - Children can be added using the + and - operators. - - :param parent: An instance of type DecisionTreeNode which is the parent of this node - :param children: A list of DecisionTreeNode instances which are the children of this node - """ - self.parent = parent - self.true_path = None - self.false_path = None - if children: - self.children = children - - def __add__(self, other: DecisionTreeNode) -> DecisionTreeNode: - """ - Overloads the "+" operator to add a child to the true path of a decision node. This child is executed if the - condition of the decision node is true. - - :param other: DecisionTreeNode instance which should be executed if the condition of the decision node is true - :return: This DecisionTreeNode instance - """ - if isinstance(self, Decision): - self._add_true(other) - return self - else: - raise TypeError("Only Decision nodes can have children") - - def __sub__(self, other: DecisionTreeNode) -> DecisionTreeNode: - """ - Overloads the "-" operator to add a child to the false path of a decision node. This child is executed if the - condition of the decision node is false. - - :param other: DecisionTreeNode instance which should be executed if the condition of the decision node is false - :return: This DecisionTreeNode instance - """ - if isinstance(self, Decision): - self._add_false(other) - return self - else: - raise TypeError("Only Decision nodes can have children") - - def perform(self, designator: DesignatorDescription): - """ - To be implemented by subclasses. Defines the behavior of the node when it is executed. - """ - raise NotImplementedError - - -class Decision(DecisionTreeNode): - """ - A decision node in the decision tree. It has a condition which is evaluated to determine the path to take. - """ - - def __init__(self, condition: Callable): - super().__init__(parent=None, children=None) - self.condition = condition - - def perform(self, designator: DesignatorDescription) -> DesignatorDescription: - """ - Calls the condition function and evaluates the result to determine the path to take. If the condition is true - the true_path is executed, otherwise the false_path is executed. - - :param designator: Designator for which the condition should be evaluated - :return: The result of the path that was executed, which should be the result of a Query - """ - if self.condition(): - if self.true_path: - return self.true_path.perform(designator) - else: - raise ValueError(f"No true path defined for decision node: {self}") - else: - if self.false_path: - return self.false_path.perform(designator) - else: - raise ValueError(f"No false path defined for decision node: {self}") - - def _add_true(self, other): - """ - Adds a child to the true path of this decision node. If the true path is already defined, the child is added to - the last node along the true path in this tree. - - :param other: DecisionTreeNode instance which should be added as the child of this node. - """ - if not self.true_path: - self.true_path = other - other.parent = self - else: - self.true_path._add_true(other) - - def _add_false(self, other): - """ - Adds a child to the false path of this decision node. If the false path is already defined, the child is added to - the last node along the false path in this tree. - - :param other: DecisionTreeNode instance which should be added as the child of this node. - """ - if not self.false_path: - self.false_path = other - other.parent = self - else: - self.false_path._add_false(other) - - def __repr__(self) -> str: - """ - Returns a string representation of the decision node. If the node has a parent, the string representation includes - if this node the true or false child of the parent. - - :return: A string representation of the decision node - """ - if self.parent: - return f"{'+' if self.parent.true_path is self else '-'}{self.__class__.__name__}({self.condition})" - return f"{self.__class__.__name__}({self.condition})" - - -class Query(DecisionTreeNode): - """ - A query node in the decision tree. It has a query function which is executed when the node is evaluated. - This node should function as a leaf node in the decision tree. - """ - - def __init__(self, query: Callable): - super().__init__(parent=None, children=None) - self.query = query - - def perform(self, designator) -> DesignatorDescription: - """ - Calls the query function of this node and returns the result. - - :param designator: The designator for which the query is used - :return: A designator with the result of the query as a parameter - """ - return self.query(designator) - - def __repr__(self): - """ - Returns a string representation of the query node. If the node has a parent, the string representation includes - if this node the true or false child of the parent. - - :return: A string representation of the query node - """ - if self.parent: - return f"{'+' if self.parent.true_path is self else '-'}{self.__class__.__name__}({self.query})" - return f"{self.__class__.__name__}({self.query})" From da5d3dbac8f1ce0f02669176e69d40e613d7cb7e Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 11 Oct 2024 10:59:05 +0200 Subject: [PATCH 39/57] [action desig] Rename and first test of new desig resolution --- src/pycram/designators/action_designator.py | 124 +++++++++++--------- 1 file changed, 67 insertions(+), 57 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 13fba336c..aa908925d 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -16,8 +16,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 ..plan_failures import ObjectUnfetchable, ReachabilityFailure # from ..robot_descriptions import robot_description @@ -55,7 +57,7 @@ @dataclass class ActionAbstract(ActionDesignatorDescription.Action, abc.ABC): """Base class for performable performables.""" - orm_class: Type[ORMAction] = field(init=False, default=None) + orm_class: Type[ORMAction] = field(init=False, default=None, repr=False) """ 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. @@ -240,7 +242,7 @@ class PickUpActionPerformable(ActionAbstract): object_designator: ObjectDesignatorDescription.Object """ - Object designator describing the object that should be picked up + Object designator_description describing the object that should be picked up """ arm: Arms @@ -253,7 +255,7 @@ class PickUpActionPerformable(ActionAbstract): The grasp that should be used. For example, 'left' or 'right' """ - object_at_execution: Optional[ObjectDesignatorDescription.Object] = field(init=False) + 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. @@ -273,7 +275,7 @@ def plan(self) -> None: 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 + # 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") @@ -329,7 +331,7 @@ class PlaceActionPerformable(ActionAbstract): object_designator: ObjectDesignatorDescription.Object """ - Object designator describing the object that should be place + Object designator_description describing the object that should be place """ arm: Arms """ @@ -388,7 +390,7 @@ class TransportActionPerformable(ActionAbstract): object_designator: ObjectDesignatorDescription.Object """ - Object designator describing the object that should be transported. + Object designator_description describing the object that should be transported. """ arm: Arms """ @@ -450,12 +452,12 @@ def plan(self) -> None: @dataclass class DetectActionPerformable(ActionAbstract): """ - Detects an object that fits the object description and returns an object designator describing the object. + Detects an object that fits the object description and returns an object designator_description describing the object. """ object_designator: ObjectDesignatorDescription.Object """ - Object designator loosely describing the object, e.g. only type. + Object designator_description loosely describing the object, e.g. only type. """ orm_class: Type[ActionAbstract] = field(init=False, default=ORMDetectAction) @@ -472,7 +474,7 @@ class OpenActionPerformable(ActionAbstract): object_designator: ObjectPart.Object """ - Object designator describing the object that should be opened + Object designator_description describing the object that should be opened """ arm: Arms """ @@ -496,7 +498,7 @@ class CloseActionPerformable(ActionAbstract): object_designator: ObjectPart.Object """ - Object designator describing the object that should be closed + Object designator_description describing the object that should be closed """ arm: Arms """ @@ -613,18 +615,21 @@ def plan(self): FaceAtPerformable(self.object_designator.pose).perform() PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform() +# ---------------------------------------------------------------------------- +# Action Designators Description +# ---------------------------------------------------------------------------- class MoveTorsoAction(ActionDesignatorDescription): """ Action Designator for Moving the torso of the robot up and down """ - PerformableClass = MoveTorsoActionPerformable + performable_class = MoveTorsoActionPerformable def __init__(self, positions: List[float], ontology_concept_holders: Optional[List[OntologyConceptHolder]] = None): """ - Create a designator description to move the torso of the robot up and down. + Create a designator_description 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 ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with @@ -637,17 +642,17 @@ def __init__(self, positions: List[float], def ground(self) -> MoveTorsoActionPerformable: """ - Creates a performable action designator with the first element from the list of possible torso heights. + Creates a performable action designator_description with the first element from the list of possible torso heights. - :return: A performable action designator + :return: A performable action designator_description """ return MoveTorsoActionPerformable(self.positions[0]) def __iter__(self): """ - Iterates over all possible values for this designator and returns a performable action designator with the value. + Iterates over all possible values for this designator_description and returns a performable action designator_description with the value. - :return: A performable action designator + :return: A performable action designator_description """ for position in self.positions: yield MoveTorsoActionPerformable(position) @@ -685,9 +690,9 @@ def __init__(self, grippers: List[Arms], motions: List[GripperState], def ground(self) -> SetGripperActionPerformable: """ - Default specialized_designators that returns a performable designator with the first element in the grippers and motions list. + Default specialized_designators that returns a performable designator_description with the first element in the grippers and motions list. - :return: A performable designator + :return: A performable designator_description """ return SetGripperActionPerformable(self.grippers[0], self.motions[0]) @@ -695,7 +700,7 @@ def __iter__(self): """ Iterates over all possible combinations of grippers and motions - :return: A performable designator with a combination of gripper and motion + :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) @@ -773,9 +778,9 @@ def __init__(self, arms: List[Arms], def ground(self) -> ParkArmsActionPerformable: """ - Default specialized_designators that returns a performable designator with the first element of the list of possible arms + Default specialized_designators that returns a performable designator_description with the first element of the list of possible arms - :return: A performable designator + :return: A performable designator_description """ return ParkArmsActionPerformable(self.arms[0]) @@ -792,10 +797,10 @@ def __init__(self, arms: List[Arms] = None, grasps: List[Grasp] = None, ontology_concept_holders: Optional[List[Thing]] = None): """ - Lets the robot pick up an object. The description needs an object designator describing the object that should be + 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. - :param object_designator_description: List of possible object designator + :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 :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with @@ -811,19 +816,24 @@ def __init__(self, if self.soma: self.init_ontology_concepts({"picking_up": self.soma.PickingUp}) - def ground(self) -> PickUpActionPerformable: - """ - Default specialized_designators, returns a performable designator with the first entries from the lists of possible parameter. - - :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() - - return PickUpActionPerformable(obj_desig, self.arms[0], self.grasps[0]) - + # def ground(self) -> PickUpActionPerformable: + # """ + # Default specialized_designators, returns a performable designator_description with the first entries from the lists of possible parameter. + # + # :return: A performable designator_description + # """ + # if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object): + # obj_desig = self.object_designator_description + # else: + # obj_desig = self.object_designator_description.resolve() + # + # return PickUpActionPerformable(obj_desig, self.arms[0], self.grasps[0]) + + 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 class PlaceAction(ActionDesignatorDescription): """ @@ -855,9 +865,9 @@ def __init__(self, def ground(self) -> PlaceActionPerformable: """ - Default specialized_designators that returns a performable designator with the first entries from the list of possible entries. + Default specialized_designators that returns a performable designator_description with the first entries from the list of possible entries. - :return: A performable designator + :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() @@ -895,9 +905,9 @@ def __init__(self, target_locations: List[Pose], def ground(self) -> NavigateActionPerformable: """ - Default specialized_designators that returns a performable designator with the first entry of possible target locations. + Default specialized_designators that returns a performable designator_description with the first entry of possible target locations. - :return: A performable designator + :return: A performable designator_description """ return NavigateActionPerformable(self.target_locations[0]) @@ -917,7 +927,7 @@ def __init__(self, """ Designator representing a pick and place plan. - :param object_designator_description: Object designator description or a specified Object designator that should be transported + :param object_designator_description: Object designator_description description or a specified Object designator_description 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 ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with @@ -933,9 +943,9 @@ def __init__(self, def ground(self) -> TransportActionPerformable: """ - Default specialized_designators that returns a performable designator with the first entries from the lists of possible parameter. + Default specialized_designators that returns a performable designator_description with the first entries from the lists of possible parameter. - :return: A performable designator + :return: A performable designator_description """ obj_desig = self.object_designator_description \ if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) \ @@ -967,16 +977,16 @@ def __init__(self, targets: List[Pose], def ground(self) -> LookAtActionPerformable: """ - Default specialized_designators that returns a performable designator with the first entry in the list of possible targets + Default specialized_designators that returns a performable designator_description with the first entry in the list of possible targets - :return: A performable designator + :return: A performable designator_description """ return LookAtActionPerformable(self.targets[0]) class DetectAction(ActionDesignatorDescription): """ - Detects an object that fits the object description and returns an object designator describing the object. + Detects an object that fits the object description and returns an object designator_description describing the object. """ performable_class = DetectActionPerformable @@ -986,7 +996,7 @@ def __init__(self, object_designator_description: ObjectDesignatorDescription, """ Tries to detect an object in the field of view (FOV) of the robot. - :param object_designator_description: Object designator describing the object + :param object_designator_description: Object designator_description describing the object :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ super().__init__(ontology_concept_holders) @@ -999,9 +1009,9 @@ def __init__(self, object_designator_description: ObjectDesignatorDescription, def ground(self) -> DetectActionPerformable: """ - Default specialized_designators that returns a performable designator with the resolved object description. + Default specialized_designators that returns a performable designator_description with the executed object description. - :return: A performable designator + :return: A performable designator_description """ return DetectActionPerformable(self.object_designator_description.resolve()) @@ -1020,7 +1030,7 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], """ 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 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 :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ @@ -1035,10 +1045,10 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], def ground(self) -> OpenActionPerformable: """ - Default specialized_designators that returns a performable designator with the resolved object description and the first entries + Default specialized_designators that returns a performable designator_description with the executed object description and the first entries from the lists of possible parameter. - :return: A performable designator + :return: A performable designator_description """ return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0]) @@ -1057,7 +1067,7 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], """ Attempts to close an open container - :param object_designator_description: Object designator description of the handle that should be used + :param object_designator_description: Object designator_description description of the handle that should be used :param arms: A list of possible arms to use :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ @@ -1072,10 +1082,10 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], def ground(self) -> CloseActionPerformable: """ - Default specialized_designators that returns a performable designator with the resolved object designator and the first entry from + 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. - :return: A performable designator + :return: A performable designator_description """ return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0]) @@ -1107,9 +1117,9 @@ def __init__(self, arms: List[Arms], object_description: Union[ObjectDesignatorD 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. + designator_description description ond returns it. - :return: A performable action designator that contains specific arguments + :return: A performable action designator_description that contains specific arguments """ return GraspingActionPerformable(self.arms[0], self.object_description.resolve()) From c314a2b41c98cf873a28c7663da94cfd0e3d4466 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 11 Oct 2024 11:02:08 +0200 Subject: [PATCH 40/57] [knowledge source] Fixed typing --- src/pycram/knowledge/knowledge_source.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/pycram/knowledge/knowledge_source.py b/src/pycram/knowledge/knowledge_source.py index 420f00cce..0a5d422af 100644 --- a/src/pycram/knowledge/knowledge_source.py +++ b/src/pycram/knowledge/knowledge_source.py @@ -1,8 +1,12 @@ +from __future__ import annotations + from abc import ABC, abstractmethod -# from ..designator import DesignatorDescription +from typing_extensions import TYPE_CHECKING from ..plan_failures import KnowledgeNotAvailable +if TYPE_CHECKING: + from ..designator import DesignatorDescription class KnowledgeSource(ABC): """ @@ -58,7 +62,7 @@ def __str__(self): class QueryKnowledge: - def query_pose_for_object(self, designator: 'DesignatorDescription') -> 'DesignatorDescription': + def query_pose_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: """ Query the pose for an object from the knowledge source @@ -68,7 +72,7 @@ def query_pose_for_object(self, designator: 'DesignatorDescription') -> 'Designa """ pass - def query_grasp_for_object(self, designator: 'DesignatorDescription') -> 'DesignatorDescription': + def query_grasp_for_object(self, designator: DesignatorDescription) -> DesignatorDescription: """ Query the grasp for an object from the knowledge source From 12248b2ff2b7326035a09ed5c7149bcfdc148f84 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 11 Oct 2024 15:28:42 +0200 Subject: [PATCH 41/57] [doc] Added images and started doc about parameter reasoning --- doc/images/knowledge/knowledge_arch.png | Bin 0 -> 69399 bytes doc/images/knowledge/property_evaluation.png | Bin 0 -> 50338 bytes doc/images/knowledge/property_resolution.png | Bin 0 -> 85388 bytes doc/source/knowledge.rst | 31 +++++++++++++------ doc/source/knowledge_and_reasoning.rst | 24 ++++++++++++++ 5 files changed, 45 insertions(+), 10 deletions(-) create mode 100644 doc/images/knowledge/knowledge_arch.png create mode 100644 doc/images/knowledge/property_evaluation.png create mode 100644 doc/images/knowledge/property_resolution.png create mode 100644 doc/source/knowledge_and_reasoning.rst diff --git a/doc/images/knowledge/knowledge_arch.png b/doc/images/knowledge/knowledge_arch.png new file mode 100644 index 0000000000000000000000000000000000000000..48062ddbc0219fa267a8793725b67a6fd6beb692 GIT binary patch literal 69399 zcmZ_0bx@V<_dQHXcPb$u-AH#!cS$3SbV-*Y4bmXpDGf@42qN9x-6_)XUPpYs??1oK z%o%4qqvyV2$J%SJeTOP5N}(Y?LxzHaLX(k}P=$hm+k%3EZbf_ozL}mgqJx4WgOZUD zRd+YoPltEMmB9%@N&2xsj>%E4>)@8Oe0azl%pcs;)FkJCc3A#>;RlJIMsQCX5|T}9 z!puU&;fw`$kG#^w9Q(kZZ&c*Il~|~7B2fS1)4mEPCX!aeT-b3NAo z{s~z)vO#Sm0cI2!GN}Le2Xo5-!ygLq|NZe#Km;qmcGtuIpDQ36Tp@*m#{3^21}uLb z3##LK#{cmEDDe9~1pmj+m7pd1HfmmRhL*8?#9d4j$E*9k(weUX4#O-pEBtNvfx zCxdcCzxcoH`%fTw-(JQnw)&JB94)rwVmucb-Wtl>87ov0s$el}y}Z`GX=?WJ@EG}K zzck*|-0Jsmc+^B}ZHe{bRUYe`4&|cZY`Tc%%bI2}B5!meF2)`Pu|P!1?i(NO+jQ^5 zDMP$U&CyG>4OFV(8pK3VHNTkpZ~JYI$t^g55Ijr`Ue4n-rD=ACE)YeM1P zWB!_}Y2`{#3=V5r>;ZSY-ySRR?iY7AZEtZZguJSf zX)>?CUlxfxu9}jjxHH?rX^%JhEU55*$#ue-FzMAh;8Mxr)@dh2Hd4>`c%5yJB={%s zI2wl$u#i&8#7abBJc0SDxD3wuEs6Wtc8sUr$trw{@7=7FKRfQ4sRHZd`c?FnkeGzW zg>Mf#bw|qie()PMCJy1FY@f*|_9F{hN6n<(ch`~kc;pXTH7rNF7TO5ov+dUY*Qtm; zOIHnBoc@_US2-e+P})CWOsY(}(H+pDEtVY6{F4a?t9kyKw@oHgC+}RQc*K9^#HhG@ ze|O_@ai~w#+*6?I)k8f$Hl}84aF-IIb{b}LNjh`0Rixr~pWzpEJ=P~-+c>c~H>a!6 zJaT49F>3Wb{bF-r?#Iyr|2d3$hW}B^(o&n+(JCITSm4FdBHa;L8?^y95;kR4AQDcA z+W}ST1umVcavG20SqO}e6Dy3{;rxj<0td7r&YG$1@sLDB6a@um+x_Z|K(*&)Zb==v zY<%X??YCbsFK!U+Q=kX}T2v(i$5O2_TC34R+zZPP>^H&@|IB*{AK7~-xIc`szxF_S zFh|2Ao&9Zp$!smMJ6?>(LipD&{qKMqSqM9a#{4rCe;K6P%aaeJLf+C-<@&jMQx!L! zS7;cKU8LkE(>DyvUVgtAd~e};QRIcX!=GmwcZErkiv_Y;J?!UK`90hvWmE;nxIY#0 zSd~$_(uW(Q$xzzfQS&_+T76?`N#T$u|5?>`bR*vfe~6e6F*(-%3oJ#g)t-NI-`?gx zYOBO$G`V=}yx(ba>6VazHx<2C1#~m@@eoDx4__*>#iqAt%UgFnLl%aM6MVczAQDIAm?LXT0k{Tvd#T6o1ji9TLw`#N0b@3sAz8kdz0_(sn^^b7Tl z;9i`waTf?^_p-~(UUZ>aqrgqK&&zaO!n?3M z2v`IMI#a1_XKQ7@#9c%@Wmld2q8)$(7ePU0khV3LuC~{nz@QaRcH~A9g2i6od9GGX z7@xLe#)P@TB!uF~`Wq{{?lSvS-B*I!etw;x0`{Bz2&X{>nnEisR<>fsgUimSN`d_6 zaZf1NzoB)Q8^rnRjOS}|RkmNAw# zplA+thPckdIDFC*?SupqE?-ViKRiTXade?2`8##Gm{Fzb`a6&*@gK9oPTf8Z0S^ zi**s!{c02cLJIv)?9eYmv($^=z~a6dmOcM32&dW|6jU7gA9-b`P51~l1QYsudbkim z4G=>8-Q|00{O8c=a)=pr~(E>rNX8j&q0`6GU_7)nza zdZ26l#BA6op<49H>Gd<@^HnnfA#ZmmxWBf^3JYx0+&_%^KPS_JqYNGnfJLy}n=GpZ zk=FC>x~?%^T^bh)-8ik`7r$Z?#jZ4AT8Jx+67dDrUZBWoXK1m_OtpG}{8K#i;53cN zCKgJ=^+YD!%Q_9bCzv>FR197MoBav8WeC_gVZ7Cg#521IT}_9m!NQriLvjYN<<-*+ zhEF~s;RKy-4JmE(CgiH-%hH_?Jo$T4M$f?Y%KyMB`!Bf7AbQ{5em*~#E96>jayz0% z(ncZTBDQa>QER;|k5A&3>xrdepZhaXX@Y}Ub=bSn=d!&l%ICdJrf;eu&9&>e+Nna3 z-b`tjfScj}Imth;FpRa$If*bnlUlzaEdh(XVLofjOkwLkGCLkl~RZ z!8^;)ZU(UEzmvo&#(!tfD3MZ)hZ;S9@d`C~?LG7dfI|G~kVMkhsT6AA|GQ%fvNx~h zzS`zS5^>Wj@m}%zzK2uKG7>%4bI2!da4LR}&Wc&d5#W}VdH5^lhf5&$9>V;>4pVTu zEd0>uzKmM77(BgtQFMNj>C_aCsNGnBLapzer|11`LoaMHzr8^nIxekxbdFRot??AM zKRSZj&Z;EUWcX{E%q~i$bb$vv2F*a_!XLciNdQuuDFHBbzlA9Hc<~NoFir0ca_aUb zM-udjpnmSvP4&?&4R!(PE4zA+5*5)OaRiKE_;N&gNrc`SZiNJTjqxB7 ze0LabUWDZoPHRh&3U~}sfn^xxo?@fy4%xJ!j+IVU%wd!O?lc|I&TH741??|ON~=5i z(MMw?8l|c`qxp2kogtn(^w-dkV``*^a#Wle`!E>r7spNWLnC1byxUJc4U4JNC{FoI zWz9ve{}mOF#o&DoMd7EGC75@dLPOo3th&!_YZHz(h^8t^#V8XPG;(Bb#WuQ^l>Wrg zC|MrN*2zXU^}p#j_0<0*0a&PIt) zZ1n~wGkU%H%3f`R$EbY_0TlC3|BHk0Fck(ZIaG3q!yO+{cGys!?Tj`k(+w_Y#0sKO zCJ~^**`&9p&wbO*o0Fg86}Tby@@uHCS6=%WnV=jT_Gz((;p+DMa6#_(=0NV<&4rpe zItH^r4rTanGdkx)M%qoIJ&XX@3&Yd#gAL+h-Pc8N$d#%B9c79nZ>O`Msg`lkSRbR? z5(pzJQUBp}6l|WJRNR>mIn@iWg(@;F%}^(qSjs9R76&@aN=E1&IV!P*!2|wn2Cu2{ zB7+alxNJmvc=g7x6a-9@nDt9KuPlZ#RI)oHi{u-siGOB%5(`E@2$5zr=chhi?LwIy zeAu+z5TgbtB0onmoIbl1ir{6y<3+jS0PM~W{V${S%P#|zP472;d-Z$gXC#q&GB>rI z>n?>`{O5*Tk2|br*26nJ5BHrMNtf$0I_9_0nUak`^k)NDB`pS~F;0VNyd}eO{jaNv zzUq>csFppG48auji~C3#;Qtg+y8fmE{in8pgHmh~& zLcZ>hq|Eb|2kuCxQKJ5J*yjujat!i{U>!X%Iiim@B}GFvfftPWR4kP@u$$LRc7sIi ztG5R-0drbz?)OX4Wda!crRHo62CY&0#k{_t7EoD?XNx0HP0v)z2=QWVN<|UNfTAT2 zJ6~2M_oo7_M;Pu`0md^DIC8ID&IvzVJoW~2{eU|>*dK0%+t z=JQPE(z-B;Bth9C9FLUhn<=QHc6t*S)XT---_EsuSnz<;xv={_aS-Dl*sMg%9R)z9 z2wJ^^1b|Jp26^AQJvx%XQS?Ku#G^h(YW2NPbeXXZeGo<$gOy3T)pK5>tk1KD0iX>UKu^)$grhjZ#g@ ziS1{cR$4r(?7i(55JV0a3f0*_ z_U4DoW9*1h6cIcA>3A85_{8IMBeyLWHGiAUn1>HP;yJLH<;j86@j?p>&)dtYQ`xxQ zD9>uIi$fILj$G3~n=3|9NwBk?JMYK01^ zuVmt=>$=|<%Y*%)%Y&PSRT2t9|NCG}cNqS6`^p`9iC?s>j)&h!pOU%<^1#Bqmlp{< zA=PghrWus`_P(^KKrx-~mo5C3L{B&so*B38ti*us*Bt7B6cvzlKa5ch!b4+T!UhEE zA+^{a8MyrZiI&1`Pr6}J;rh4cnMMJq<@{wFA;Mz?G=s!w*sAw&g2Qe$e%5u{@_cUv zvYa;TXgzg(zIRzQ7K25-zdTOYt7r>=PZi0=l9F0j(tG_j>y4kundT;-?5d!LE#?K**gLF*T-ryw1%RtlpOn3DKVIS>H>r834R%_k=^` z-j2Pf^1eF7-{}XDqGw1mC*8nT5&xE=ukG&JV1f)SsVbNctyc~Bx10e^4* zds|$P#5|xx{^XHJF;axlxts;~$#Oq16xjVlT9Wu2cys_6tHmT^%I%QEv-S)QP|sj= zehv^;M^;rY)rV&Lw)D?^~sQ5UGHm zyk!~)?S%J_Wmx8hyFNcCb()h;;X~^nKE%ZwCJ!i%qaI;IXGv#$1EW!4u)ru7uRIA7 zz$k$0Rs+{)Mg2DTy_UFeFiJ)}tCAe%>(b$fX8}hf;IY8r*B~e;125_y&R~VPIX}p4 zblp?!n=C7qES*H>@A7%`&m5P_ZYR=goizcKQpWr`$?V_* z#16YDtH+HcAkfIR;XE(46`QVRmYO}-^OZ7%w$WDPA30@ez=KcSJnjPKR@Vp;ueB-#~8ufgdf^2SB^xyNb0wrnhA=(YF5y-&%OX6j;_ zS7Aqz^JXtzQItWmLja%i_gFv^MtGhc#ue z?UCaIoIfJt<;3R8skK4FDvaa}d?TN5nkv&5Z*n`#0oigioPZ@{+KbJ+KRKRPPLp2k zE2Bn{dMH2yWqVamCXyZ7^sz1f%FzSmd`6?rD~s+X9`JQMb9He(Je1vljHj|4Fga(RwNm~8YMj>~Rd zx=g1o=V{Fr+^ltnCK6$obGF#WQ%+PY1}Ycj8i$n*6*a}yrBWqBvPq}N|pVUA}_I}|Vz%f=o zf+B0Z9(=rIeu zt}?YP)|eZZA`HW26v+^{_ix1|9Jm_#8S$*RxlF4}U#c7)c{?$n#nooxbc4ft8`H_n zjmAP<_3O8tE_adkL$KkR8@iuRk3|-YT$W~;&e{5onofhct&vjQ%o?X7^XE7m5@FZb zDV{vz1Lt#Do_G6%1DX86oz4o3_O0H%HN($xy1G9-*G}`2E>_L|%DXvMsF)9ZcRgCf zWyjqh@6^w&dG{<~2 zawR`;qj_k&(d&;2r>oYK`3MM18sD49ODzB18d7g|AdmI?fq+43lcPC;3h~C*K_D&l zIsDXk3^q1_{_Z9lruLsiiKEoBjj4!sT#uWs_#FrB_na+;3WQobW8KF!%2g#d%-%m$ z%F<05tc@1mARBx-#K;QZ5Ld)Io26HP(Jch=^U0>VVK=h{3-L+X+|QL%$F(jLC;oGX zRS>?5WPSw6Cif)@@(-l9`LA|;IyxHetZ|);c&eUfi6z7pY#02%@5r2`7`~m%mCxR> z&wB)4;jXGZEYmVKiLX559J@EtncR3qL5mmZp89k}dyqcd>;?t%^-Pwk?>)#zz za>b^_s-lu5hj(O4V+72jD6NGLpLqh#YeZ<_p+Na6lm9y8U^17cD-5r1$(KaPC8R(p zU93k9h5-e-w`4%i(;MV%90H$^Ih*0{fRi z7-9zYkG_xf!0SN&+ad3l=jc20b*Yl&g-PLXnrU&q;aOB2iEj7#M6JzM)DOQg(4GoN zfBeo{C@<6VdFQvq5GdWl=|?ixiOR-(Rp_w&ylWrjSY z)w9EHaYx}4#&dDFz(1H&A2eINHN`%dUNjTC!5|GsBUMR_Mx-o+&*)h#@=P5E%cB1h zNw(9`v=k(K_*D}6+KXGNBMWj`{CTfN+naL^uT6Ph<~7xGV&3f^Hy19K$8YL{D3xN~ zzdM}y9WJ$WS!H4g7t9;p!=Bl}3*WN!+is~*5|j)%MBL@2ZFe$hak7&OjqkFLnuDb4 zbl7^DwleQ-?wa^s-TQ9c!x{!lGdJ5N;t94+cUA2B#3>Lq{=qF$YU zcRwnIyew_dh9O`+1q1JKfTqy>iG)UJsQazL>o%(z9e$Dpo$6no6WRoJ=e||s=C)sg zG-MM3A~R76qYK_&;L&~y=X6|9@h+Wjzj&hkmSAiI4}>6F)1gRn;jR+2)Njg%0v_Wb z65elnHKZTHa6`?Vx^%@wx3B+Lk7wEG*YRfbFFv2Jo>bpL-Cz1tpzvAjbB(FWN1mKl zktMz?6q8|QeZQ!DJPi|^?{KzAg?%MvJWS|2rdQ4SiY=>*g#tG`H)NAkji;JFq7YBqK^-r0OeHTC9q5_7R*sLX`M$#Ase#o^pgq2I-H zU)j5XXD)l23iMj7Fas;MPC<#EyD3t4{GA2`PtNi>^ddEzBV?24nQK2XFxwnt(CDy3Fz>_%+k6bevznYY6#O3Xw21D|>I&QF zSLI4cNSJ$YSSU@&7kCP3x4n@Mjqi?D#z?SmC12xOFIDAUr>dvmH}wxWqA)+B*o&FY z?Bp#|IVm|9{(4~lLuaarz)=qW;4aKLHn+yOp<4Eq;Els(8n4h$`K>Kmg`D8boLaMM zcQq~JVn>SAYT7Mh+j9ZaqEiWcoS5oYbtNv><8=3`ciQu}w>vMGmR+&` zQmkLaK>bnHk9+->O(8SFg2ATeao=X8H_U$j7#t+i;!4UAdXv<|qmRo@@#sq^usobL zl}R0ZxszvKcm2+NHBmyGkr4ClXs*JS=|I}c4}(M&6m-`w@E z3C$qOlunx+iT!;lE%d?9^rluZrw7WxKjtx_o$a`TgS8LK75EQU&DhPeH1 z;43gs;UNHR%h@&Gb}7@_7s~wnM<XE{FY3XY24e_f!AV0$6KBzoI4|0l25|py?gI z3mam?6n0m*+|mru7wzBp9Hy7YWG5V5f(I836MPg2pYma<-6Iax^|Dj&HcDDnujp5{ zo$s)$DAnuK9qpl)uvh0d6^Jxc(6F9j5DVor-`%EoW3hTo%GVX7ZPrQS;BY!Bwr*o$ zyl(qya`SG(iUD4^e4+7x!CP1w$9#<@@;Q6ZgCMW`&4t&)4a{FAQWtc&2l363d>#v` zO2Pof{RXN!yVBnDu1k(T7A`EG7vr7NCni5lM%Hk zEc0dJf&}OAubLb=ZMuaWLDxZ|HVmKXnMWG`(b`HuI*kECcNA%UXdI0i(;#pDXX-&J z#kAL<&i9|Gi_kWV$rVyLAJ)pqR|>CwuWNRDINWppxOMQu-g=oQSK6U+gGMNg-Mf{^ zq+gxPcd|se6mRpBQ&a_K>lP*Q#-#9I+rK=W&oQd?s5ZH4I%}Hgyp~Zp%fFGJD|Jjo zp|c^7c#&yQ?>p9(yra^Mk6j(FcK@nNQWOa--f{c*<>@j^*ZMvu7Aygg2ZgWc}$s__fjzuVZP( zViW{0g{zXP)b)b?-zia~OE!TPv_Vc0)*zB(sv$gy z*5LM%lE|!wmxjm$>pbt5#OG?WVq!1WFsW2S6xuDlyGB>b?yt7=G#X9kB(%kGI4$em z?Fn#^0q&!Q{v*-lQRv)QxC#ZniBG}eD&4)L!Z!-AbuN! z`V>1aJESmq9XbB^`zi6@&aXrv+h>CWSMnY|ueFw!m2CQblqw7wVrOgL?l45CpVowC zQN6o6C&^^>_|dAPlh&8SPq45)pT<enyY8XNy;8Dvn+etoI7P4kGDB@tb=hXd>!$}6v)bRjT0 zy`o99!kJCpS5$$BIlKmHO#J%R5DEpZ$;N1s$) zNDTviOk)I=4GG$Ljo9^WCY&)gfWSaxH7#wbQ4(dg+9R&QS}uwn35n>tBnc`*QLiX4 zB#8+A<<`uKZ=V%2c(R?0dVbCh!@L;m+0^GcG1ldkpK9xkJmse2ixG6DcAej?+xzv$ zVgx3{d2!v@$<;vS$x{69F!%||2UG3ArPS5M(-WR_|88P*HEmOH`r8B4+wO-!I8)yG=m7OM3H0NtkvgcW zX;p5i7mgmz8+pv>{AvvNVs9X%k`V)hV(OL8&U~bp@X1-D;*OM(Sp(i3xoNG$L>U7t zpSSu}@mxN-BoH6OqjJtB0;ZcJ(k5UL!3I&Bd8H#BnnsxKRt4fKCi> zE_rZB*uz^anZ>_{(ARAqd ziVKglU&F||1++I(;UoeacAF79<41+klz4IZPEJPs^B1*#zZFYcZ0R`RZHW3I?2qik^Tf~xnr!EzI1XO&AKo_9lxz;D+CGV6L!rRd=pV7{yIrLG#@NmDGfBo#9{Y;Bq2mg(^aZtl2 zc`So`4Wan2UDh;q&F;H=(4o#<$OJhkTSH?r;RG7))ol3cj*3m1ktubSlr&`AjFfK( z&G88x6$7P_UU;9}^4<;pc!lyDe@Lu{w@||_kVh)b@5_rVd;u}gL;15}-0!n|k^Gt6 z0zz4VHtD)y@~4K*%vz0KA_0EAF~v|>ExR~)2M zyC2n%3wKuiu0yZgr$c95;79$w&3!PmY?*}y8IL(fNIZ>wQ}MS@f!h$Ni$98}yTkWS z(!vbZM1EY4G}|6TsU>(`|MJoC6p_X^>=(?|zM6F_COq_GF|w|vQ5qdB=GVw#9~n|f zDDW_JlLt6&4q@O6VLeHUqf)cgtM``tI2j60IcRlNwaaNKQZ`qc9~g>#r7nP4^gah&&@3nF!=YAMUn1QLLGR1xpijh-k#|T<`y*uxfV0s0xJ1hDs`K&u znp1%=zPCzMRFhqbV9TJ@TNI%Cf_Hk6-w?D~(4u7AohEH&ipaKvU22KreH)mJSvt$p z`HKxL22vt7ne{NUE}617#GhakR2a4mOQXL?;v7$d7}ESyg(01OiTc^fj~l}hc59Ql zMl*KK5<|l~OPe}PCcVkZ%RocTSpsCNhwJdedB5e~76YkD?$ULW@$kSn2d9#jqrYRKUUi zt$JBbNfnpXY=o!tAq$gX8X~Fn)RzLsT&XxjOH~bZd8dicZ`w`hBpQc&13y2Jh`$Iz zQ=34@N6t0}mb&&cq9Eo+@PioW2P+dVK8w@S8&zZC<}T684;x|ww_}Qv{FC?Nv$dcB zU0Syc0ilBy&p%&8ar>poFVA?YDZ!^9`1(1JftufpP7~z*}16FonE6{M+1pC zr7-sR#%oz5h21L=d19^?#wjS`sZaUA_Ue3ID2`Q6&PU?6-qgyA`p|+pCetsRUTR$| zG`MAoe{8tv&Q%)9-~a&U4-Hnv?s%D0mmF?kXJ}l}lAwkC&{}uY$a@dxgiB&NRnysK zAO3uWR87IT%|?j*=LzfhQLTF~{8VUde$H(_D@`){Rc$prxoUNOQP8XCUF0sk-UAQH zmdsT+>{H##V=V8EO<3$L(=yt=H;JTr{av7>Na3_#&UbjHIYn?Jb`o^((_^%`x!JO_ zXoQ}_kUhj1S}?oSPuOb{6`#A%`Fq;fJwEo!ay;U{)Z-*M09JhSj{mw;={S~4t%B)B zF?Lra6RjP1u8Rj`Zc1R4&{)-|3S0@*9^>*7*-s*$)kyV<_&M|TIEE82t1rG({qqt? zu*GwC1hgaY z+Bg^;0+FAFl^Zmblzf+EiD(G2ysE2lQ9=N2<F z#>EY(~S>Rt7!tCRBhG)S0j6icf^g9o5bR{StC{T8szX>1t*ur zi7zrqEc&W2hQ0B{v~O#R<};g0D9V#@$zuNW43iDsT@|04o#nkvpjXx1r1_X`^fzN# zLo()UM0VSM8Ivsalp<|;iOvH#r~wjQjOlu^9B@1}SkxP!kbpw7fo+)OTv7L^<3?DR1HMKdAxRIn}A zBMLzgP~eQWSg@3+lqS-pe(m`xai;U#z@W+PpkR;DTZXm@@fQV$#R>&meuGh9VC_v3 zc02Kf3%G&ygw`k$L0Jv|L<=T9f{!uCBdggA)*?N|k~@rE+RqpUWl~oKry|9@yZDvm zPiI2iD&0H6?iADF+T?zfeQ_n4EK{>ipKq!%?lM1`n{P(vFnc3>!EyoL*{~!`ubNe) zSeAkt+V*Rv)++8tUT$R%JY4z{XiUUtz&^|qKi(Y~CR9MUo_ZFlJ%_I2;Y%rc^f#s4 z64V3)<%7F2g@YMu%1xd3=;D3s3G7(_=~>WO(olJK{bi>0bnvMU7I6_j4EEb%0W7UASwIevn(v4GOdm*jV#S`9 zPL&n@dg6gK;3Ucc)Cxwp0k*o|lYl6HGnSCAi9K?PQ!X@P$CZxx8pJ#~{w;)#^% zicX3f8{U?zKG?^{JY`N%G6u9gYDv7#=I3ui^G^pCn!J=YYEek|YLS)Vm9>)(a9;Oo zh|^R`&E?6&6&Xt|U0uiv@-&toUhb9BQ*=1!IPBz(_qW95q>L+@oR@M>A3MH$kkUfv!?y5a8ty{;qEi>f z_ikTWqeLTD=8x0bwTb>w*8c5EFshDCS{HWfgI61y#{_JN*qdz7f5VF%Nc#N>ga?mJ#c8~Nvi)Kp zUt*9=WhQ|%2V;48XC^LL2{DJ+1Jr1gRNEF?)H#;`hoClFYgIO?UtB*|pg=TRZH6*Y zW|%o!Yh6K~2+?)-OD#XsLO2|&2`-6haaa-S4d%ev{1BN4PM8rQAL)u&d;jPaKu`_o z|9`dBZS=Ze)^f5H|3k#}o*=zUiT=968s@pMZysCjtI=yf4TAOvFeEk{E4 zThQdU^_1ThfIR3@3Tl=35aqFrfrx zk-(F+ZBWphCK7>;qFpA_Nlq#d2B4Af#e@FzPtrQuV!e;Zm{{-KP?l{FsE=>2oKs%z zJfE$70}JF>?Ye^jNARd5aKs-${E_->6@l}}e%iq0Ijx7`F<9mtg0C|Q(2i?I+iXO)5GD*hW|^M;K*^N0xud+h+W6Yv=DJo~#0 zC^#5OGMU{jFZ0v&9?CZrf1YuyYr%#QvQZR)>$wf^ckh)?efgQI+)ybLn<@vWoK*4w zXCV=S$<=)bL{}U()2y=%&NTTZ6`6zjO{!n_d2Q^7w^#l(#h^KhDjeUjDv7fMxdzecfY;sxsJQ|VAV)wP2JR&|#SZmxVx>J6ZqXrR z4|Ek0xEFxF3@8l6)Pr$;S`0yBJd+!QEzcK*S!(ku6l`)EQS}uX%;cA{8qLctP)Jn; zIz?K5%C7euSiM~Kr`dokl16{N-m%#HU{*a}$2AO_N|sgk=MI@sD%@bYpqyM1V}W4P zJ35tC-@EJa$~WR+PuUA>(Ggo9Q8>T|8{Ct;KM%>jd%Bkp2@GM<{W|I8{d~H+$Jv4;C4hhkRh0w z$YQ7%TUu??aH^gOADY(_%iDR$Cf!Xk)4D+7s`AYbJ}%-2 zpc80c0dd*+QWpve5~V@lkXa`7LQ=>S`q0%lfJ-h0N3WR!mr(IKrrNaUZCpbgV-NaN zwE~R6QtJpr*V|VR2~ot>AysIU;&a+B>TM)COT-Oda@KEYz76 zk0Ne4cM^3Bc6)=4bQbEy34lg=t#P}wXfaj&M$1j_i=FjWSUw|HwID8QAwN3A5cA92!KlUH0 zE3SfCB@4gaZywBjqkFcs63dwdgK};)mS1z_1q)m02V>ago4?TXZj}1|V!!ibt>jon z6hD=r+RfdnHA$sm{{9F$;!)2+HVB_iGWf{pLP5vjOgFlg_A1e&x>Pk^oyJ)Qz?39Q zc%PSl2=HG-l&+h25sV&p15_vVA8>i22kJwdqGII+$DN(PECkhPneMkJ0YSkYIu9p$ zy*$JUczB4aw4jxE7B+wk$Nl1*iG|zrXrrOc`5(x7br5)&CI;S9(N=_AM6pBD%I_I`zh}vH7i4(H^p3d(UKdq8wL~ej^ zS~@%hW?yux-0EvzVAuN|JR`vu9lV1g;5}7Fr1aA}S?wxz+UUgv>T0RZ_iT`$BvJ|H zK~V68(op{@&7fd5X~;Z^6*Gm(0vo{~)G$cC8t%%6%^t;2UZd+4Lye%Vkka;|{{0W^ z`o5fK_n*Il=06-sHqg|V^}UjuPL+#oYO1g2VI-16zXgLr5UqP1Ix+vdi2Iu(VGa2l zs9^LkE&WqmI>s3iy*^POL)`@`{CGmQV}E{`!EsRO7g-q6GZQTPeZZ{VCF6z-0{s&s zE(mZ4!bf`yqyGLLh|zsz95Sd*TikL@Z9`faW_ z>L59)POs3}|IBqp=Qs(AL-Qav-vW;seKvzhxSXw)y`{N5l;IX$wpS2Mu;Tlk%zMS4%9=3*r_+dH_;`!1E zT~Pi-u?TW9q5>e&83i2)Abi1wxmfvu02%5s8|*bsw;9*|@`HMTFeS<(!u`8`tBXvi zqlJLLHe=hO6KQd>UOzcTu+rA1R=HjSEf}St-OgImU4It*4DdwiW$>rW#_5PfpI=&J z+sxDeNePFQdaN%YA|ZBkv*T)dS*W8{g`SEZ{-@U-_rKSBat*LmA2B#4D+XBQ=1Q36 zf02Y9jsacHhhaV`9skvK8cl-$!l2k03(Gv66{bAkkmU} zz3h3VCt!w6Enmd7nnIWaLmtcJ<2>K!+9y>MSGNoX=x~L^;GAT(axxyy_OB>ea8s&w ztk7MyT3UV7im^J04m&>ncKN(yrhrJo$6?l=JYso83ObdXOlJM>SOZPQ+S?QY4|Zzfh!E$L*TAD^Dy713d(3}+jJp5a4)R!n);!f> zV7Mg%(J9EX3>eAspI;WUa(b9qRFu5lr$eYQgrOJ&BUNAVy|2$?sE#gt?_0VWQ5lD` zeg*o>BMAH6X7PWsFWtHa8trCuJ+hNZV8Q&LW0R~#4!Wg&n&;JttP{s8KjymI*^vLH zcTr%MHV&i1X8++VW@xM(0UmFaK)#uxz90p_-ImAQx`H^I3rIhe*gP3J$5={SZoBzq z1MIETnD!3{7dzkY5N;clK9LF?FGkl8(ip@US0g3CpjSf^93`t4tA4KHww%>O{3YlF z2Aj^a8neV!S!!tVUsK@QsTRn~C@S_$@I`Zc{sJqrVq3Zv+T!|~TIM^ZHtgY2Q>jp2 zv+qh;9uP(T!CEn8!emo)lKwXcc?J;^`)s@WVKc{<}~y_I~qJKpuTJ zRfkC=Iq{i^sU4}1dsbM@ue1(F3z|&37D%5_u_yvH zx^t$&(WBMY|nX&O?H z{xr=`e$_k#X8IKO-bG~kd0d#DC?Ma3fXSR!J}NoQX?{Wvvlcx>p{|qTZo;#Fi`71e zu;l4rJ@Vfy3xzmM1wE71w|O#>E7j{IzKG^G$rb<79jPoGQgP+EGl)Qm$mU zY|N$oX^qWtmzq+u3x&kGR$$0(jX_+T9QwB*V!qA0>&`c(hSE+REnXLidB~7KpT`MI zQ1_@$D7QSEE8czi9GKGcQ#UXYv@0%~C8EQJ-Um1T5D~G5vvZQp7q|f9zh~MgwSPnw zCFj|jyK|3qFe435`=vH5gk;r^IKc#F(c+Qt6ms1Y!d8k`c_3Ns*j~!=j7>rzy_gVc z^SOB?xOe1N4h2s2mL%H3P^gdZ-+*7GFz~K@dqp;ifA>3# z28~rrc_!OzO(~(d*o7j0a|>M6^XfEzcAChV;UA>fn=3)w zwhs5Unj{&*d*U$R@NStRiqTSK7W0EFK;{zpt7i zHu{nt?3*Ry*?;C`uJY2doUnp{#)94js-si>D@J4DlH6zFINj3}0YFwyH=>x~dV(6~ zDq?VY+b4I!*!|RG@jXfSxoq==Z>S}WwLh55Me4JLew@tJ#zDOw-1|ONv9xXIdtV0` z*&M1yFRBZef~sS7FJAHpKgvv)riDpEwE3BQHsF9ZoZKkPoK*G z=k*z6c=g{*6ql3;S7XZSLXX@~vVs;708SU7e&)iziqFS7mlXZOT{k%brmCXq4=$&F&wn$#+2sAXwrmT$QWCZ&qn5MA2R z?r^>#Qc&_^c-U~c;9AhpY>H7mK4Cd@k*_s}IWd4aK(a0emh?kW@X4cOo$M3I>VH}Q zD3~ajEFkw?UtJY@Ar<8!4@Jm{$iou=#(tE+_)okR%cDbsX(|!$Bc$fgK7S(;#}RW_5d3&!CWXu2mkAI@LCU05Cse|JnVdKke9@iZ2g-y0 zf1-bw2znvRSyk2t47t>LUD#;Ic=&JX-vuFrEu!`FLGGbjHB6#njt;edx8T z^Yo)gK7b@z$qaTlx$&v=gMS@_cREL1zJ5F~U!?6&mK9Z4#vW7|)}WhVyj;hfRm zgTxNVt7Rj&a)TBzgoOpG@68_Z0v=AgBe@88<}svy#VrMpR}iREj-Vq>Q-cS5<+b1D zgv6?8cIeV@XP`hYz-Q7K2AeNZi7QZV19cxJ`Flb3?rCuR5oE`S;xk}|QYM^$DepZz z$|$IZ)h{8Ee<-;3z>4%7kXF4hD{UdU)$4xP>#rF|!D!-bL1t#b&|iT>D9+e45)R$F z{ps^xgeG90h))6_^BvvqYb$3{Trnk zRMbdt+tbej7SsIffTp|F^UvGfIGRs)XqYb`SA__%^zRYj{{D6lOFtOTf(#vzEmy)Z zG7svvdTW1-TX%EKXIrD;VRT0&2J-!5xV#SQ@B#%O7~87_x)-3LmsuwI4w6zOT%?Rf@1_OmInW3P4&^3USU-h@l4fPshu7l zXPK31ssRmod6$3KRT%-s2ocAA5<^*_9Aq8;O9LW454>O@x~7I5NSUbXlfGY(Ei1z@QVfC#ywKI3 z;YxlOM4QSEY_Gu*e&bk9G(YW%!1`c5m}Ye!zJd%{fGAjizWj?P&@iH4JdF9*wnI)7 zV#xM8P(c6T^Sf{M6!g2lO(Ojnh8KRZdO}Ou23QMem0Zc+8(4+SYxmdT6qVEG1Z`3g zv=t$g8BP@Ip-Jh@LO$2hhC+Tb6J76;uy@al}hmE2cq#iRLu*xsJO05h>eWYnqj zFHp>oP<5fUnXY`t&Ws8NzW0XM%qJ$DC(!?=3*(?X2J!+v-&>w^K`+fv2Z~=U-dCer zm?y9068Jdt|ky!YZg}V)eS%JJu;2r*M zyis?DK41+aZLzxp{Pi(en|Qu~7_~c`4NJo7sM_%JZ|MLQcevOfp1=@a2qvMG?{BXZ zW;kXc0Za|z#xBNejF_ykAalMDU11H!jy_g)b90;hW}k_@UOPNIUOc9}czZ=_-+CJ_ zB>n{Xxlps@joD#%=xevTSx&0SbOFaS0maM$NCn5zp2j#CYLVU?ORezsusq8L6CnH= zAm~6f-miCbxtw+^MaKmMJZ7a5%yV9XAl#41e6I8wUtW(0A%v=c;GG-~mJzoqVM1o) z3^}{`_!-;aE+WOw`u~ruw+xH24ZB7Kk(4e4B&Az&=#)6Q|t zq#Ff>mXwA81nCaHYo2$1@7_Q5pXYdvL!JA+uRPCnuC-3!L@{1z-kgG!PG&gpS%f!m zdsp=mkIb#{#;I%l%9G-crf;n0sxgYotePbOnJUo=bHWW5q?=oN1 zWnzsw0<8$pePEk87OlQI$t)T}V-j=V^r;p=4TzH*KMfJ~Q!rt{7!lnXn#%xF{^l|J z%%s^4zt#ItZX=d-M07gj0o#kBp@DWg8MB`6sPg`1Jhj8fU-OgrHB2zW%wpqYE`_Mdexn5u--*mK5>YiF=z`0=MNJ_8&^4x7; zAH0F6dl_7~$8g~H5tWVnn?ibx{!01z-bx%pgqw5IKT%N79{{}Ku6#o=d*wR+}6SeTE zg$pq8sd7FkjDB5J=LF!ez2ZfqAh@&~z+(jPmA;{)C!koh`9p$zIOuo(Nu|37(9G|lQKN#W z2o^l^2WSGQa1gVEV6k95ITHe7;Wnxck_x=bbN?aqW-bhX>buLY$_4y+d3om$itLo! zpR`0OEsGbLU@w55MCt3XykFZymAL57Q9pz6lm@tA^e(=Cko`48NwlBg)Vcvi*|6~h64DYN(0D^H2s zjL>zb_5fl+ARd4USb|XUJ&%o_|(=>go(l*FV|M)V7Se%haXPgD7TgT@7YU1ai* z)-rIF5p?}aGuP(H*_-er6J%>zFD+)B4#nG`QI$&Sd#Bx3Za?TJM; zZS_9AdH%sLjYB^MOi*5$OD-Kf_%v|w$N&?I+CTg@ZbhsIx7S`7(t%R(PsBf>pvgBs z6|oj}CJTXK)u%g~*JE555+I5N-#bQInUyK&_ZEo!AAw@mrwtR7L=yxDW>&9PQ1_xa z>J0`RWQ)P4$2~o`yYXt)ZpB>l#LQvqtTP1e8j zi=mMG&fFa2nEt+FR>1!C#V7*@tGVZy@9uO1Z*Nqv+bdGW=P&;6)h-IQ!X4NO6HD+M zC9t#mzz(pN;G_Wv={@w1EN<6l&Qw6IwD-F?t;TLpv(>mqrL$?!s$OduDBQk5Z+)-q zyS%TiPB}eyXOcj<7*k7mKHDUd2CWB@2^aPN<+6_d7zBji=ju@kWbP5#E0k8Z3Ij|` zPM0AlGpdnb{w{SCL8T|H1k<_wrYm ze##q_&j=K06oNE&4CFIxAT^=KK(}K8zvcAWIe%q}(@;0lfSVn)0oD-awO+o6UE|4S zB|=rWDF0_MKg8~I_XCLS-+;xoNdxDfnP{$<iX%b8Ts(QBuD0*FiiKUAa!S<2R1U193gg{ldF*n63@IX$j0Z;jzGZ&60o!?(-~ z#6j7k%*nU#XDa)vf%gfG7F%zJKpjY2XbZ3if{4K&Jx0Tc1o>YOFqT+Yt?p&f$QbR8 zBrLVxm(t7CWfm|4N)Iu#ORw*N*^A!-m!#e-H1ePBFVR7;o!{wt&ICrm0*R1q)p8d3 zZ^*?0b|3Y|QfAlh&d7q5G`^yTS)oRR^oRLfpK&)$l>^tLtcOG;Q63<3SufMT3B`P3 zpAHA3mCU3V3Kjqbif$^jJ|-BzIgHt$b32*Kr0$CB9Z485MLd%^lWaGw_c@v|^M zF4+qr<9h`I2mCoTw9@|EMbH&NgA3gBP+FOz=I&^DrwF zzpo$=>$#=lz}Kpip#(tf;))=$30HA8~$9pXiZNb0}1SDvQVQjh|~a zeqwol*<1PWI~KCkS^9!?`}B(;hrjTP%^|(odTRakumVQ1Xq71E=V=}%N4}k@p*%erIG!_>frD{YnJh zii~z&-)!K1AO2yyVFC)fxj-4D{jq%3pC_45M{WvhIAmPuAImFN-pbR3mfs(=;Ac6Y z#{J+;w0G~Z2<*yZuIZ)tZ(H7$e;2gMDQx`Y>;@?&-`;58kX|8lSTFZv{d|rW*;=0oKlf-=&@N&r>@!3L=ryH>;Kogv0iI8_#B@~c$IHyJlMvj zG?W2h93xnHADqkts3TjJ+!+OWb^OEGovVul*HF=!F)6E-tdPM14GWXo-(BN3^JRr| z4LsJ|^Z@W-aiqqFP=G}n(9`t0#Z9EdG_t^^7~Am{ivaf^VHD#7sO$RRi2*e_FsdSB zb)ot0y%IvQ-Sa$wRvy2pEH*dVd%EL)4nmu+(6d=UNo&Z+F6A!QA zK78u;>$XwUytIINo6yqImJ}s&)LcGWGf^hw$OfErq~0azu4`Pt&EeghZf1$;$1f;0 z3+DEKl^bN`jg6S+8GOWtu{w3*#)YIA#hPZu49BNV;IU|t@!DTF6#7;9+fTcJR&sJb zWW3+9Qz|Sny6(RPM1`RW<&y#t0JZ#_1XE7AhgT-wE4#k;q&chw(lxEWiqW#8=qXPnZE=7BCmxB)zqpDZUc8a z#z)}4(?QaG_+Qv&k6x3w)oHmEYcyWtNuBWOP?~CIgnI>>=t3YEoQOJK?1pE@Hl)kH?F27)KvcXgxe$*7z)u zj)}je^+dGAK%xENV}bF%@2=l{zXP$UYUX(Qx_K89bGZ(f(@OdIdJgqzZ5qLK`rNOx z&+PN6cFEr+$s6V?C05GO*V+x`^f51~3T^cMsM1ImjK(CSIfxZC{2(tqk_C|m%8p|9 z4bBU(Jz%V~JN8lbFxAX0<3V9!pTw;i1#ez%C>AHL=&-^hM=!(YJuweGC;SSLfBC$O zPQa~ZF7RNKH@EHCKOm-LEd*jp$b#Vz*(&_oM7a+h)ieJKUU|+>(sbsn?%`!qCAndn zOX)WwYFThdO4=$2Hc@T??KFSe0_mnoOit7M1|Jn|N@1?Tn{e_`4vjEWwygR6H8qG9 ziE)(IRhjO_FR_(KfBa*Oou+w+F{B~$S3Fuz9J>1`9vD`YRlmf4h!F=ku?f9RQ8dC- zLaKU*-ZRy3`j`h#c^-#M^+4J!tRnhf*%7O^q&%EYUT~BA?pZiML%>1nX5AHbY1^hS zN~|S{1Kpcxiv8)x%92|RxL$XPZtO2=!blnr=HJz)IP?oZNltc>ZBUodjm~B2<+Kqqo{&l)DpC|M6gI0G)A@$HLaZ6K%4r{;ay2Xk1y6qL-_Le%daATkXWEcD`WhFi`S*7%C6BJ& z*+&6EoZ{h;%Ll#nHCX1t1hRLZe@Mj0aIDRbG;Oahp8T%>l<~tAd49aDW5OB$4MREp zgL+V=(;pe*|A37mijD?-RWjU^>I&`Y2c9*eUY-U@kA(Y5NYQHysXDDEKPa{0De-vz z-Su<*lI)U4EryC%>5=d#=J=iIHHC}F6lMNcMZtCpvk2JhIMFmtKjDpdL90=#H^Yms zZGM0EC<@c6U>k$kmK%B0 zy%vryl@jxUL;n>SIM0>XiF>`)0xQ;FH7QCtB#~a=?R%rdfETV8}5znVwdF7 zb6{Pz9P()shdmD-1+xL61x~e97XU&^)T2pu8bAA;mA(2_1dt+<2;h;U#+@moKi%wk zo>kQnbjMG`t8%Cy_zPj)qHws{FLLHqONRwT>_$ORH5F2l_j$7B&2LW~bR^ca-2aYy z!rCC=7iB&%(c0@DGLo-qSe)H=D-jGxZifqm9!y4Nc5X~Ln2YZlbHxLq1cilpH6)?(+Hp_?Nnst;e;x3xmLnCeA>N5 z0gxW_)2*!;STK%l!;tUHQRfA&P62Y^txZ60%hK!K5;8K86!hg*fp#QSzTr>KmXwp3HL~)qf}_u?H-{h|TmUBO{ybSY_Tb(~oSG)#0~M*2(StH1;r58n*(+tUZDW&n3mpMoH~ z44P~nTwSb$;u4cHsLPoPT_?*h_FtyKG~IhP)_V$oubH6Tf_K?@EytIYy<8l(Bk}gt z7=Nae1bNd!OGyY^zlWN;K;LogrU|CkqHLX7^|v=RZM_pRBJBM8IRxetY4wKUy{Jfa zpmauG>MQ2r^y}R2pZPmlEQ@{{s(yY657wd6X|e1>8IZNK5F$KaUi#O(S^MS!$D1pk zoi+04c`U_V1Ff0t_S$|Q{i2=U`L01oLEut-bdY|zoAuSfN^9BGQ+?fD!9>|Zb&0-S z;Tz5aopLvZ?{0T3RX4%zm3pJ81$7aZCl9Y@(vV&hk=)nAYv;v&TEbrL`zi@7JG)7u zK0Du}HRw{=OcZ_}f3Vjn+noC#3$HDlm9qpWwIKSBSpl;!<0I?6jke0r&MW za?vAOhcd+X)0*pjlKZ|{-}>r5S_Sm1HaU~JP}ogk>+!wByq&HCV(lI}A|i5a;=<`s z$Dpu9KfG4(pM4GAd+fRLTdn7d#MtIC+B#^|Y>LDWR-~80jYZUiXPxSYhO&+&e9&jp z#n0Q$|E9QV{`edl(0omeEWOVBaP$@%T8IDnD=XsmXO`7~DOMt%_lCxuf5y+m`=JN- zdDtX^?|6;dnDM7b09k@sC%vtl^8^H92eYnvT(C76_r$vNS3ndlA6pJoW9YM1G2_2PFQFvw%mhL%iz_w3iy$5yB%*qa+KBj-dR zld2L^EdohBZ&e4L_C9KP$$`kd1vLttW?^289*cKZ+N@_ClVS2t2cHb3Lam8b`2r&e zXbZ%I97|}FxkF5KrSBwG2FofQ_0I`?abUzNCQ7dOGWEP~Vj_?fikRrx@RpOoZVZtD zUYJ0R&0WU6cNGVds$@vxRr(wJH)c(h8vHpnsB6gZ{QZ7Ia)pk>jqEqWOyD3P1wNvD z@yAUg)=H_-W^Cw=hU(Zxq(o32Dkwqqvd&bcCnxiVoGbRSoGu$CglzfFOxu%+wR&qh z{{ytbRl3HP-ST*wBP!>VZptW&Dof)!YLW6{;7|6YeV(%;2PbHz$S2l{O7bk6&(8U* z2jUbe;Sevnj=4%kOEq3RB6GMDSJm=$S&G1MrjbAXiFD_OiHBK+LPpwQUXN*caLG6d z&hVw*nK&<1zQbZxpNoMJZCup`c`SDXaCF%$)9sQ+Nq7*26Mc-8>Y)F)C+;!Bcm7X5=|-Kc_mRh;Znh)z+7VEODD`xU_4SF}nX&w`ao!{Io zSau*u44e9ADRZ2bUlA`;Z4-sOM`S;7>NR5Hy*}K+jtx%q*)0i)l?Xrpl}W-Wm2!q) z>34_Vw7xBEjw=;-{&@EVl{HB1$V_c^VD_h7b z>n9dB3r3ub)l=kWQ{dtn9uc7n_!#k{w%hrU*ioWL;8WZr!?s!78LD45sk1BlU?guQ z3O0B1cNv!_wwq!s{lM*ee-}ZSqut1glBbbMBwxNy!E9xuwDE!LltGkXNePu+dnczF zrge&~TB;r+<tyfxwA~V9;)ceEmCTV}d-uxD3touk~#rcbZ9KaM}w?+u7Q(WhS zY=0Wd-x^ryR(%z^WfZI>@kHE9Cy&s5yR|#ZA7~q2{Fa4AcC7YJ8YRtZ9go)j6?p)K0Xk)8zIAw4!bBSm3+oS|MXPGz@e12qhzbb`G<|Dglf*ABp?1pYJYJ0r6 zxD|`QgkXDMZbEpm!HcjxH@y%8pa^uj&nVCUDf`PNVbie_eyW?!3Ht5jWpOqeEuK3{ zUt?otT@R_c9@y-cM{+ht5#{?S3%|~V=CfW)C5E7Zl@{Uj~@@*F22UT_$G>! zDj#=O$L=uw0*VHDydWhT32^Z;Bomd%4b^v?uF)nVN|qm}XJMrWzRRpe5v&;M>Q;R1 z7n$AL+-|iAbnCoRWsMo$GtUGW=rO{AUSPOo?GEu)Ps4oa6awkTmDRBy#oxo&wD{dT zCd)&BD(#>Qz{eu!Z$0m$5hEAw>1V_}M)S@PdRSK6g3)!cp2F(dhK=7LPLyhuR7J2- z;5QS>WzpizFCOLO{030A(|vty-Q+qZG2XK<3KYwYmem8x?@TezomV^jgUetTh|m!v zpPt#`#;>py0nOrz-#PU5nMq)Prg7-=fSClH(zkg~chj3=Z7?$*ic|-NA%>+)+HMX{wDCxBhE+JBR@*~!>`y?S*DlTQ zkLF>!jtuxru%YNN7)eACm${D6xom(eD`lvedgGww6U%7sV|xA>&2(*3$rN_hZSVC@ z12xz-@$>K0mxw3guYsSX$KTz@SC`0a{YnYF^x==()cPwV+jV~?>&SRU&5?!j7R=yH z4G#x9!-I&Tn1g-HF6@^(f_HoyLaV}U{ySV5h?4GE%uph4`hP6FOZ#|m+(qr$L#ZIw zL587CU#%Ixqu1)|nZair!ru2Z)GYzL00Nra3dU=c7|er*;Nx*}B(_@;-CS$tRu9hV zhcSf)S5hds<7wzfkGjS(B$3DSXNq7tp8V8u!-UsABu)N9NTiG!B!=e(QmQ@D4om?g;$+ z(NXQnA+bncAIS!+`PgydsQBT+?zzc(T&O-DohdI^27?6M&k5lE){qmP4|y`m!s zSu^-Ux3U90enGYqzH?E*U2#6!Od$993>GVondJZb3JI!4 z#E(#}zKk=uY%2T#Mn69=$Ff6y<}9cOacaSy5$8eIqEcUq(5Yvj4i7R63%~dh%_n(D zOwkwNNuP)gn9&=x@UXFS7JQDzC$}fwIcz;MpksE+oc3>>t3z=}1q1K==?a&dyD6Xu zFQmnD9t?H)4jm{W2~EE7dNb2C#VT?MNEoW4yxL(>Y>y=N+-gC04jkrR3?2sR*a|8^ z)5NK~S{@rQzlXEd2bBU1E1qi|Z5AS3+cic+)vss$_Hm5B)=^icN4U0+IRgDE{F;yr zK`5<#Gn~nOR}_C@q6fiB<7O%|90?PAr7X9skA6VQF)}?Y{m);4^H+Ip!=kSm9}ZeJ zz1#YM_0DKLK8lVL5*qr>X6V@fC58e#ITdv_t>853!eep!oHfVWBVnOHT$V_198Q{B z16}JY1h^zyE)O4{w!SG?uf*6i63Xx&83TQsr1KJftdQHOwkNmx0D*rw1|k$}v?b;P zs#xMf5vLps0sGNENQ5FmUSMDzF_S;#hqwiMZ2FM}a6`QYCKo9YFA}0~&@37Ge8v@U zO&hf(Vf4cZpInI!jG=$U&ulJ&m-MdGYRs36wx6k{5jDEqNdO{7+>OV_e|;}j;X*y2 zd_4}9g~PwQ+Z`le(CoYR|XkEch8gI{rW|eBzVazYSzI zyZxCHN;N&#>M9Kiyu8o*J&=8ifaKc`OG~~1HcN)BD`Ep#y4b-F@_}kt7sSjzv=9P+ z&Csu${KoPqyWsJXcD&ys8IrdlhDb1HVB0Dv(sy<}NGUOGFcc}(i+(-95)AGurDe;2 z^l-Odf&bn;S2L_En11i2qmF$+A3SIopmO@>*NR^6T1i{fq|{e*e_gnEH2_shjbGr% zDz%HM06o(8jNP}+CKDO@U)apAPCV8xaGLQ@BC*K^FJ2Q8isy&kOP$E;=U;?)zzcbI!wRIeIx-(o6qfK^ue?u7oy_}0rFv3TfNil2VhK-BWK+U4DDz5 zZKhQi1CAZ|13E5hEd#FvYOtJ>*WRz-pfM~<1bbWh%c|^P>A=%`w79uXw+{0<;yw}a z5eI`)FkyjKmUFti@Z0uy`s-KE$QX!V==skd7C)J;e|x1>vKldCs8)0OWErH0-!Doyuw!k^6&7VwY`=ui)cR;AWv^B|?Ij(IHwU zX3&9!D}s!4H&;W^q5zwi6#Y4ic7u!g)m5hJ>+YiAlPhZ1M?P%hUJT36(8ax+iWhg; zTVt4RK(CD5nzw}vx5Nh!s0%nh1(~0QnbbL`bt@&r@iC1>!~RTHPMLXPxDfk z;c@F2F_dRDjm&o8i(a`5pt|IubecY{D2q+6SD0vaoi{!wQFNot9)Bv^U2g_yd!NHI z$;FZ}YwWFz5m+^^`B|H%gC7HYq3>kM&{F47WN($xS2=xu?jMT{fOntP;vr_F_Vq7% zC7yr$*LrEc(;{i0@%Nkn}pGrvBIL$LIE zNCWj&X}T`ozb9T*JErIq^xZXjQHev0WYqkl{+5nb;vl?ueRPGX1b<2JQzuA8Z--p} z-qzR_UeFYI-S&BXgvT$g+WF5>j$?eQjY=9+~0q-y)NpqbEHhkiH) zaGB4M2*EuXDH-Ye1bhFCgMkPS-TSW0q?EDl^;ig?!_mcld}Sy}dcY1Pm?9M@R8w=k z-48qwip8y8)H>^FQZwtLwTwWe9f|>?z*jz?AnQfOvQD}8Jz%MA5Dr0Pk*gMqCh_u! zl}g_kg?HBf8SQi_90Z#G2^)l#=gy?c)#u|E41wvE2)Ld|iIjeY0k>Rlo)yBh&S4D` zw`?g$u1NEp?dM!+@}D1dR$!3{wE5?Yf&WsSIbaX3DQRp(0D|`Br-(0DPeh~?(2eR- zi8jaG+xsImlcmyjyk+iOW_1l=K6A9FJWdd}T8nue6&#U;Tm<%gr?6MUawWPM zl1c>nxs%43LNHAHby5eqGtobDtysWAEp2lsOK}P6Ql=$+n?i7O^Sa)7kOEKxAmNeO z29ESNtpB)l@}8dVPDD!G9CTxwkIwB4I(NTt?Y=q%WnBNRg_kbR?0yz(*3s71SH897 zL`bfmLOk8EpR5X9gT9Ebr{5%nJlF#6A>Uvl7Ijjs&d65CG?zcd&cJWK@MSWr2lLCq0hJt3af_H-4Nf?C9K9jO?LE@SZ3W1%G*cx8Cw zYjPE~98P{UQ(DF#zbpWOO>8>PZ|^--k?_!i#1ZjP1S{_9i-CXxct%m$uG&lR9}}b2 zMi*tv5-tUY0=2NKkF78hDe}E^Iq`$TRIt5hmS+$*4Nolkxx8gfi|09%a3k_?-z zCvcH)rfgZLBiuy(a4^&(?63% zU*K_~)?ErIsjbB*HG<_)H!@LrvB+!5FVO&MTQ~%~GDQ&uIBVOrI{A}J)s`j{$_yTi z|17Ug9&n0eb=s z+pkZSf)i-up3u;6{r$sZ!s_CT*~BtY6u&HV-oMOn5;gM2=D&>3kTj~B_k8h3X5_{5 zyY`}{(Z@d^{Zl1{k|Lyy2x5)iOWWIJ>uyjVrgD+W5ah*(Q@iIY``bil>X_lk$W+|o zk(b^$%n@CR8R5`WL$51aUL#}*CCHN1pc2xfm3u5;R}c?hmR2Eig_S=bjAELA^yFY0 zf~Er&E(dQzWz|DU3T8`Ix*gmHo?g+#nrGJ{6)$ngxh)$3UtP!T*zSbWZ*sCkKh`b` zq1&7r^7>=k;}oS?;6^>0S*{+&urZjL{I>>4Ue9S#E?Qy7KQc!@Akz^#J=@1^I;8~8 zELNJL2WS{C%Yj8re5UBw>QpFcFED#>xS8QETfgjNQsTC77WdlCd46h1HoP@ihfB+2R*sNN8qIs*O>$HG_JYXbczdbJg%!Y__lG%UO^XvJL)Szz^v zXPP+P;$yXbBBzH*Ypkdk?@T!UY(FC^LiMxs*U|Y#hZn%Arv6A!eaPmo_i1h)Q9RBiP42-uSCG1EQnER>CY3nGgtWVuGxB{v06ncaw13eBWCuY*EM1m*7LIrkC0L0 zzsux~>HVjweb*z;MH}N<66m)`5dDz^Z_=zBaxeYqLX!hIOmGPf{VF~J0{o^R`pA%Q zn@z^$7ps7Z`7$L4WplRE%Memu6b-8nj2x|hcg~}l{!Blm)|K1aX{j#LeFMPYXN65a zfB6uMISsC8$wwS^(@?h@9o4+Yta|K6ZbiI^Ba2&a{}1iwN6F?Zk)G~7(*|8qirc<~ zY%2&Y?xPfL{SR-TLIe3Ux-r^Kv4)k;v6H`86xJMVQX?#!D@+=6uIEUR%VX2g*Ejr; z`EvfTT*QSGjIoSpY!vr6A8VW8_+n#aQK~F5JMIfQLS2B}ir{B0F@RQ&vt@~xa(1}J zy82&p8^hj$DQ59py?_eZoxDEf@5&){kIJaE+GIHh`qL^!%? z3g4l!?Q6LXPYfdS2sS~{w^8TtKaD%N%-JMJOt-eGSu+O`eG}qMdq?2Po%qGP_4=>h zNk1LoR9B>asY$9Fa;VO!a2gB3=n3I`l;y%jwCft$(wm z-j$to-|_jansw*+a`cepe*O*aYL7k5%xC%=I)6IyhGhtlA^nDT61*6y>&x;yQl`!R zm(Lztv>(uavx|7YjP~x@J)lX(cbi)}<)71yg@`d3%G`I`G9_($b{)fJ@>5?`LK8V|V$Pqi|l{ zFWfZj*uf3Yh#oMJ8mzR=#7(11NKm>>>9}SZ_d?uyz35WSl>lvUl`FuoOz+41z{bTd z3{fp6&NbpsHwfBxt)mT6&Hs+v1`_W`;1jCIPc>`@^-vuA=ptDp$OA&R92_NQ{}FPw zUPpw!Lg5oj`W&AT6pt8~dadHgpQZC(6^Yt!u-i#a)LU0z?}uVha=h)zJMwndJJ|Zw zL<+pxjly#n$;cx{Rl>BN8+y=39k=fNu2u4Zbk5^o=HfC^xBM-a+`-)#DehwB{n}u5 zu|1(zYchp>&0zWHC}IQgIkFzAt1`SB4fBCr8M`kjp6>+(MbbF`%ukfOwz7P8>|79% zA_*l}wPrv29{MQy=ACC~$kr1?)7wzV=%TQ&5*r*Mb>hC#bb)tqZ@8>?x2N+>4eNf4 zky@6#|2?n^@?hPS-o(by@H(N_Gw)fBGG#t7F`B$`mj5K%yi}7$-$G>Fua%Xj)fg3) zzvr^}Ey?6ym~giluxdKiqC_cYO{T7*^2jx)Me^LqIYzsxd&jtO8UkJHpqioQ0+cM< zKe{exrLa=gX+l9e$NY09_M_FeGwwl#I9%qclR3c~`y-7}gHHm+x~c@iKmHdIRbY@D zO1?t>;z8NNi`n$B(KL}g9uLisXtZUMB~VI(Jc<-x0v%BFgDk(HksQz;i*z5-|8+Y+ z4Q!V_cV>n4F}cOstX9AFXOOe9I!1p&YkxjJ3$KeIrAJW(6Tl6kpjd>vNRGP9 z?Wcy9iem&(fxjqT0I$`3IthVfX0^=|Fyy%#HS(v{VK#pA{<83iN3L_XOQ&^eh3#ctz|{X zE`+3wBKGF>jsC!aT~$eT1%wmfkOhNO1jh&cUoODS&FksKcW(_pvUC~y{kC3Xj_od? zMi!2@ENed|C)XX*B6AJuy#FxE3VXO>fB_2F-7grLE#0!7ttU+kWr-q36qzc`uXo7# zdCRIg0rh@L<^EZBqO|c*f$=^mh%))>8?*=Yk1pK@8l9PSl9iQtKTmi~;C0B&G zT#1$hmEZhgkJg9POucqJwoLcSMX?(04B%vxQ?HZcdEUcZUkzr-$6r?U2bMx2diCI@ zO`so`(~@SZeiKlpmKjCXI_lrQ=Q`ebA4^*ep)a~>%WM(3+~~k$NrDU_qOBc}ZV2g~ zizVlLK4C;3zQwW0jD8a&t9GaM^UiI^=qfh$H^2!&BDkPIPoSjo6iHc4vF0cNCFbR4 z077hgQAO~yniN(*hn$hKS%@$MeQYEshD@JwmVzIeBJPuK8$hQ z_4=-PzXX<6IqAAAGuYn_Rs6M^EW@CMv^wJCXWHdXd>&C@lv zw@t@eu4h-|{~?5ppYJWvwWZyBj*47S#^SlZeC5h7|nY~D6Kt$QOnpRmh{ z;h%mluO&m!N#^*{hdf-rCr+!QxCf%AsOZhAz_%__Tj4?i)>P?p+HV}>KPP}M7CQtf zJ>P}XK0)ulTnORxmht{El4_oPkcl`XW!1?~(>Jb2OY^*|2duXbAd4n8u;k_`^am!^ z`fN6WtZ`L82_tGR&~yaI0tP;^=aU@J^W~Co)GI=99Ki;1$W1z+SpY~WgNKlq{EYd> zhmZBF@#hbWoh>V>@7=kYFUWER-Q%swWqPi?@l&7evU#6_xGM{+?XA9hE}o84dQ)!n z83BDhZA7}%sSqYar#P=y0wuZkh~?FXMXe;pE{1aHM0L2l;z}(I!l}7vAC`9|cX>1p z;Fwa=#H`88SSQr@n-rtRTup57wigV~*8__EWq$Y8*~mM6`287F@Bp$+C8b2#!e14p zFD$dx24a3Tz9aF!yq7LOYTjhSaBzKvfAaJLBY#_*avu*ySkN_gOXk@N-}rqU?mpft zw(D3wT(H(f&@-kN9`ab|R%=_gdTL!UVLVj4S6X)GdlL25edRg-+J7*|L^xkBu%#St z3yow``&CYC$0r8Xx_nF7z=Ox{nO1ZY`c=x$3L??f|+MX*9trD zSNN>4m@TjL8^+MTo_|elIY#OI**NmsoB7v^n)?UydQn1=iBF18xU!5h!4Asu1APfz zFp?kr&>-x6Q1{-}(7Qt;OE5_@TcY$+&NDXk<2kuco8RLz8TpiQLl95II1UNyxDhMf zb&yhi3?7?Te8-b)Y5m)K@(ebB2(JVL1`o%l?_LUf5cGOj#X znVZuz%Pp?vE#ShHAPnV^j`HRI0f~J1N;U1HcG#=#O>W{D8>#s^7sYD}-%SM;M6TsP zbR^NlKW6okt7xe@+pm}pJO3-Zzm_9ClbQP`3K`K^wQ zib^#b+86*@s_NfG3mUBKEN0P7(9tpU-n5SHikxY``SMi zHeM(ReGQ2qisBCj-6qHPxmaSS&Em#>5Xdc)qi39(85uoFT~1N9>0?=jt(r;mVDNh( zoOTxKs882!V81q_L?icr$BP{3%cTbyJ)&fj0X4!{&j;$>!wkGL-_ywJ>4N~RgUnh+ z=iZ|(C?%H(;QKKL*iOkt1sl@$JPDI$l=vmgJI~W*?D$AeYtQk?>g(U1z!c5r@Z$fO zdI1@;+FJA5Z`EpOa=YI`V%>DSzM)#9mih=IbW0B$*Bj2*uC&-DDvO$OhO>j~eetdi zf=Iia-?foPTeTFZrb+6gkZ-V^bs~hKPD*%#eHr841glmPU3*#0A3ki)Wy_#&G5hPS z0mDbf0>Q1Da`p)CLJr4Q*J-Pnt4aCNwcZ9|x)u7QS}Q`+aj+3)wKIawP`BY7eTge8k)E9a4wf4t~@Z)sdC&4l5hw9MI7h5ztjfrEg2saq1`_dnSl%Gi&R?zy)}M}}e{ zZfzn^GX^&Z7C7yv`(lIr%Q`3_2KEWMp5>?~t&HO_uogk2tEa;K_#lt0v2OHLtK4_b zzda9VD3V(joW&crdRD6yJy;~*f1&e4<~7KJHHlZArI^S#d81OvF94@}BK=lV++w0FZDub{L;BnF#Lp?M>M=E=Ghbnp%p@PV^>uwdI}8aDHs&iOlr+3yR!fcf zmVLkyrajZ5$QMs5{;MWl;GM^b=|f)OC|f{m(zE`2AafJh?X_e#(xD~V^O(`PLd=t0 zplhLgh!{%XgAX)?>W-jsfa&h<=%$g+tb{@?b8$bb8;T4>dn^!5r@K&*1j%B+JIMcq zXr{rW8EC2oy{2+;-x!%X^)ylGe;EaKjSkQJe1GAMD-~)JBkwZJ%kzWb$?mf?wt63@ zHI$X3a5pGnOeh8fwqXVZK()m{JzP;qS!xwf5m>Wx z>s-Sj4Nx|Pks3J|fb3KW#qhyqp6#2*APWER8ODGKrTSREH}BUh?(WY3nkKtBv0Hv8 zL%yz3M~R$=8~vg5QtXHd%Z3CKtOo6$M{V+v`6thhdlgsq_`9izQ%zRTD4%(9OjS^l zStk&EZ?-*V*_q6}PcmG36Z|V2((*e{i*{)O;PH9Hw32QIL_4FU$*hr78(%?rjfT+u zWuB3OsNZT=CILg0jSD@fkW=TVCWFIDR23BPl~jJbU3^+?J4o?ns*K$E_t#|IBGrxI z%7L%pLEG5sYxZ^umYqqAW#}uTq#d~%$xC_xBWDXTzv%mQ3})H7gF@2Udfiex^Q_>kGB*W(pOtd zVm-YIC)(I@rt6G|*?-^Vhk#e3paxRgt3@YsD0gB&i)~X5Om!1cksvchn2b6O$c|pv z)U=9V3%V?PqP;o98qN@yrjAT|i3{xI8hwtj5XTl-nDsnM{6Xf_#=FL!{@ zYo*KQ_xjMYV@B9prvV3%z(q5$PM?#2Wg4eISQ2*FR^i&4l!4huGbfW%9s5MST25lO$^BN>_M_>Y|Af;qVwYSOF<1PORXf zrbev*%#!f}H5__-SON_M)%*daOL5NIkUp(6b@w%bUoal`@(_x@o{QNWD zb^|z%sz`{Sh_CvGLP(g?+&A_W79U#V0$0aWQLmW{m@i716c&(kxP3%XQa&W#N+>SF zco>Bp#-ECH^e!%X%eS0M_L?WGnPU8^w9o)sJO?z=h4vVeEK#+nJm*}6tFY)sFo1v! z{QHCvDGsg%hlz+Yz=67t`7j4WBI0eePLCmAr@gWT9GzuFF!(;iz_pbD{StHv@4CC2 z3|yv6)-N+I%4ZmD_45+zyUd?)RWzzZazUe9M;`e42e<;^xtu@$GjCLM9AM-1cz1cc7pYoW;v0#k)GI+Nuz?w=T0$(6BXww<;K+yZZ zio>F%@hHy^FE82X1ohagBqs*2Z0AwSR`}bv8yC*W{!%tLu+^yRo#JiPYMQ}sB#;0d zoPi}E&Ex2(R0PbUQr z2?=Q#djKg)M$H8Yd>2*24nV!*T+;=L{PhTklhabM%z>+zncuzII+j)VtesvTDgXu> zMP4Iq#N!N%=QNW%w(*ZG}Ckg54mGR?IVm+__5KtQ9HHzh5b%Km3i|= zl<_}S0!?90G-;-9`7=>eWNwW|Io<-P1zk$jeE(#eu;QY-P$Cc9ytWRkfsC(C9Tk7b zb#P<+Ou^P8SAvqVZJMC7k&M;aYyJE`WW8lnm0j2V4T#b$AR(!Ah;(;IBi)K3otrLE z8Wd0(HXz;Ijf5bvX^?I<4bshjZSUuP#xvgc+cn08`;2w2wdR`hcN{Z;lNy1Ig618@ z-bmcl;MvGNX#Ao<#RgMKuv0a)KzO0VkYC9CP6UY9`^Hyee_|ku64rN&a3Z)9K9k>?BTSrVE!DwKwe8_)hK3? zd?sH=&h-x85m+C+|4q2v6X&HGoX+)#U@HgwLNJ}HA^s7F|ZMS+~v#db9FBNydHYM{4Jh48F7ro=^FC3QF+2|^(%{4(0 z7ds{1qtvuZM5rLNQvSR0OM}_E>x*Rj-pU*p^a*L`8pVrWpQBAM0aAFeky2LhVWO}a zY;(YcEN`UIA^&cLeGZsFTLA;J$4{OpfVZWgZS1=MR*cK%G)RH|{D{v$<;r=t7rk9D z3*i@Z62ZCom45~68JKI$5^pXmb9myio`3+I#WSm=eKAS8(HM$_hnEMO_s5DGj+spF zZeS-GX9OtVA^tos|0>(y=j$~FDq=wHt|is?02SdB-Mo2sdu|#R!qHZiJ`1AK_QDux zFg$Of!1S%;JCXYV=DN%ABztnxn~a4S*O+H#XD2}_5tmHk@7w290%<7 zbjp-PTmv8e85|%|?!pxCz@qWu@uxfA>tWKKs4+0HwrZwKxV09)&P@Y16@>FY`oaiP z7vMYa!a?WPZy{?wi?UEiTIwz{n9Rk9t=1-R)IS0P=+r}5Dj{-3e0Iijl{{dJlTb!S zy&%m1eg*~JZd|uSV93!>f%|;6%~a_~U(L>mnv53{n67WpMLymcJr^#~$a(~(^xTVE zK27-qU^0)`ZGdyu7{Wm(D@yF;*dZlF1lW=lkIU&k|KB))s+x3O8R8UR-?jaZxZ|(gYJrO{ET7=RZzAUMU2N`*Cw^l0f(`Bs6M~#ATvt^D*u$dlJgbuchqX#0n@h!yo^m|f90u5@9<1?+=CW*M*dI^Dk7 zOceG8Sg!Dv;w<9Fr4f^h&WjFs_n@juw0+YlGcC)re`OlzuSO{Fn>fa>#!@u|_h}|5 z`j0b`J+T_5B7%d}MGhGmIacVjhsOyxqH}$3#YCnB{`Tk=Du){>)h70LF8sm>C(9wR z>o1^NgEZ((k%P~%dI#Ksc1qJZcA`Te9Ma4W?g%T6S^qN$hY@hak2|urSlb*SJWLb_Nwb};{R*f>tFFxvt%~na>(9d7 zSl&MXuaffl3!4q>WG*{$yZOero0}WTd6nAZ7oR{Jqyg6QEl{}0kqmgG@t}0ZP7w$) zV#inl2vOu{!2+l8p^yJcAK;iAs2Z_CimnQaiH>AB`xqCbY!V>1lLz9Xoz}9}1<9yz zi2ql#8U#wP>v_yuqVl*AB%iFl+r2Fo20pQZk>d%yUK5 z!su6AtN87LZQyz*V0qAtAYmVGBYLj*9*Qh8p-BKA;EH;QN&7|}3;~(Tit-=3XQ`ZE zfn{IwX&1b4PIe4@`Hc3B2+Hl%Nv?O>tsGD!^O?YXi+fB6;0UuTu#9PBsKyA~8-Z5L zzSvFJx}Z*)NGJf9z|*bi7_i36*Z|;^UvLqcSbXI&vA2gwPKl!x;e`4VxE8 zwQNB&!$jXNkZd zikjNFwX(}e~p0I~4w?r6G2CS}i_CYI&Ty2wy8k;FT*sLf49J_WUyGg3u z2yr`8KIEP}9${XmMP-%?9kOy%!6$(A4{vvXI+VWuYt~1Lsi9zY_525x6RwB)2;Q=_ zrU2SJ@YZDZ4aU~0a5gX0D|iWTY@;^R91AcF>I2)Oc4cdvIv@R0mfX*~#|k)^ZT}YB zpcw~oRnPODXt@)WCGelspi>d(Wfai|><8p{l8+f}R`2k2{?|Ux3dgU$!dO!z(1?9=f zxNZrDzY65hUj6N%wt67`gIdTXG_q<6n}mq4tqnjzv7ru`VCu0klH8UuZYAM6CbmO3 z&<~fEA9!6X(awK#OuySW@ahP}Kr~O%D%Rt2{i_hqmejfTI~aT(aM)9A0XlHB!M{r8 zcQAQAs#C1@ZNXy|=N4!D&5BVIUx~KalFQcE7=SW%k9_WK1W)(oBjOK5y53c|%m;ss z1&zvX|GH<&gS1ItZ6dthI{}9K?F$7z$Nb&8Zb+@`x?GQ5Y>`gGcWc6_;Qp!7rboQ+wB){M_=&VRT?%3AvyT(BN9NtpNR{s1Ai z*Iom0P@R}!(q}}GyRqs1kl?m$muJ0jr00h-ytfpya-z)Cl=|s+6B$Jb_Z6re=klw+ zdv%MWU@hGWq4uK*b??@cBNskz@@bouy03;QRN8 zShJK-4Y$y8S!z6>6J}UzyY;)Xufj}0;(WyjyN5mn9A0U_V`5Hdvprl$52xRbK840aBrNLO3f%A%hx4R|g`R1G0_GtG7H#2|(w=Er|lo zpf8+&=q#JzD}^QqA&rKUd6qfA2GS5wT+Ire2ZZ_@j0j!k*sre?+V(<^qy1^WWCNQ; z5M}540m{)gfttVXkqX1VjMq#xHbY>|8O4ieA@{v){WL0pWN>VK3>PWq!S+N4i;bXN z;{nfW3o3@@-E~}q*h$A0`ft4QOGz7|suaK7Q zdeyuRmn4OJE?&&Hc@hHKR~^xi5|y5u;y(5l3F!!qHsBVNn9z z12jlv)s58)Nq^^~iLh1HmY`#YxBYG>ZXbgnqu%_&lM;zm8hrkN&}n}Z=pW>sycIxi zOF~erao;(e|GfL{zoPXYlrE zeG3crRv5vs&NK_kGQu*ROk@UOS&Bv-ki(lk^?cYKr-pn!a30S!_VzlTcc(WNlGl5d zCg@9^D&+Yc|8o)pVCr&#sg5SIpG3g4j?L|HlsMBD{bGZL&#lXj`BMT>8o>1z*g&$x z_Oqyspi%p^dj`DJkKD1Prr)V|g*7Aw*1(bl10Ee2UbNnf0hk^5@-ed;Rt9o>yv*st zg9<8Wo`iJRD<@NT>!`yBI#k0poBy8jDQCcyAAx~{dKxmeh?x61r53S zCk(DNruIeIq^(BtY4=u|6@gEH#Zc_72pYHL7+{fa&ligz5|SJ|<3;Y&faCJcJ`P5M zCsFpb*z~BvoNfY=AC#BsH<9q#k7}g{Sr*HI-9`zvHX4fa324MO6HNUytmv0s8AFHO zfz39z(r68-`-+Zxjev^Bg-N@}q%&3oNRg2{C~+P37F|5{U;mc{$ctJ*K4`dMi`Glw zhNlBV`{CjwZswa4E7uawsFwb45>{2anYt+T6UqLk5kopvy3IA+Co@E|Hupo~w_O>n zRaR4_sVBGYi^sex%;p2JuZ&~9$!l>xF21roVfqZGHF-Q^(11y7`S#BW`p3xMq7U81 zOO0o0-rvnNSf{l%IZ3a#i}jupey9RU;*mU`17VPjtil=8S_L{xtuf|+eK^*Lr_Vj(ZkKB*GzR4>*uM>xsq5rsmS>a{A2^g9M2KY_D&_cpH^b!#<+ zOCZryfuR}(B!4Abk zaXT)gmyKIYu253-u~aUdi^8>JF7)g6trcan)Q8^;T4;D5^|XnKFO!>O3S@f6^t-xT9G-YGHwWi``>7(rj54$ra&ruUC< zsm$K44qRGnoNV!UEJZ-enNFg&1MtRAkA>jz(%DF;?MBcD(hq7-$i@s&ip@R9-xYxN z;oCXPR~H0r{#Wa8(+i_p^;06w<41T;bMJhUO=s8*%A+CY#(6O}qj*y{z@y7B(fqt$ zlHIUI=iYrI4dvQu#mLp9vp`%`I)+iDx;mXz*M0`Zi9h%uRk%7&#)p$T;+Q&o3n3xB zN~HjC*S|%y9(c8qJdXDdd0OaCZ^iBBxN2*Mzjt^hP9XlJGfU|j=M%%|Jp?zY>qh-( zqaQ7?2^{>}bf{E0C_gJDTTkWkE)Tj0ojnuj=y+(qx$GIZSUwcLUXY2d!0BSU80luesCr7lll@9FS0-qg$>Y++f`4Jo+(N{aq$%z%0RYD+i zCZiJBop`N1f6%(|0Jd+d$N@f3B#keu<||ZP>D>doyUSRRjhkZ%)18~j4;obFwGWw> zw3{6kO}57yBD#0x0Twk-!^k(~r)AuYrMXsa_j9TfRMd-XQ1{W~+OVtUK5jY&CYa2) zG8aN2usKct76usD=)?V1625;;;mb*`v1Tp1=xWYg^n^bhD_I2XYe(O;>Kb>PlY>JL z$S?T8<-GnWv49qd2I=6*9FL9JQ+t}f5-6NVf!r%-{2lnRG|85DjJ0j<<@vE+MbEx~ zvRG!&KcX1M70aTyJ;}d0>%Ldp3}l=IE@2WvLmAr*0wEblh z=>y6Bq}`&iy$!|3TPB+blx?e6Wz@rUjfZ0r_o{OpFond@q3*226_H5dP|#t#Y`zSx z(lsnfc-03Oq{GdfF5kPvP7T^tS(3(_pk5Ax=D<#){-Rcz0a7D;T#=}5K11Qov=sbX zfQ?gLMRE-om-~ap?SyaZ0P&{5f!i7Q=6ny2pFJJ%=`c(fKqIW1%5(bju|uk@az&Sx zBsa#VAtX*3*OZGT*y(xKBkwa{*p0o?Vlz@$Of%b1#bLG1HLb>?v`ofNr6S zl|iLXH|yTsn@>1tUWaMTSEu}n??YTaIN$`X7)1P>6wZ3-0!rm4{8*i}J1l*iVIkt< zpuQ}E$B3=>ZKSh)%@LPy7_~m1Z>lul&!aTA!`JC;Ol+e&p94ZBPCo$28ECVi%}n#s z=?NUUwN}9UPLjeB3tn!sxDXm2C+Y-CvWcpi@^c|P*97GNjZA3G+uO&h)ek&8Cz>2g zwk*X8ve{_T266LVw-r-%SA=T=Zx^T8(jQ7voq+~66!iZWbbcbUr!JT4Mfx_ z({2#mqvW z=x4xf^Q-4mp57wzgM6hadjdD^SJ&ng83*R(ET}8wF)%Ftf|bPt_w;p%H_kL2i3q5n~q6 z7Do+F|K#~4!E@3OVgRs=?@IHYT!7Qr$;m4?$kKx?iG~FQUDto}5i)LD4K=$K4@V38Fbjc)X;I!eMKy>y4cI;l48$2Aso-rPK++F?Xh0DB$1p z+cB;o!@$3BZv|(?#{qaBX;2v))_#_h_|*HTOxPx>QR?!zAUalQmp!EG&mmL!f*R4X zGvsZMU6Y%Bq?H=9zu)Aw!G6=`1~tk;;44~Ak*hydcxMy*i*jBu4r)+o`_yft3*^Zh zL;mOZ-BI@psq2T?PHZ1;PsXCk)+GN!>(7*OsI=%)Uu@YL(f*#3EEgv;vK)miUkSQ(plXkIf7vJ&u)wfkH;A1q zUc@gtj@tN()q3bsHURGnH&P)jpul&wMOfNcl#0SkuJ=kF7!7F*WSRm?ZMRcvj&{wI z+#&25TdGt6(iCq@c@wrIyIZ#v9-Oet_53yH*!e)XK;FUSw*`=v28VVDZ1g3&6VSC0 z(d4H(;Mjlu1F4^gvd;jq4Da7TgFtDj2ECM@-GX*EOylJ)zaua$JExCxwNq-pWjdWb zYEkKbL#+DgGhV+=X=Q}5_hC7VAml4JT1w~l=4$IoyG|2!N9bd!z1fF2a-@Z@t@(PM zvc_fMO#J4v|059)jIDjy#e?94k(wcJ+#37Y_?rll2xqJJ>miyRD^_oFJ(#YxVDk#+ zztk3HfJV9G<;cc+&onnI;QaBF+SUvO+ zF4>WyUIlA+14#GgJF+|xfiHM6HW(`(;<9wjTFWYCu5N7&fl3vR)1m-Xht|i9Ox|P~ zw6Yl?s1%}y2aCAX`(Vfl&{Aa^6cIzO&ktBRjYE?SmIMAkKRT8e5|ZQ=)>sDxI|{5x zJ?-XnEpAE4Dx4%A`?t8_JTwWXTio-5MYvz~_iwMq)BtiAF_SpMA?)>s8eS&!M1l#W ziGH9s1v(deOZ(4w_&-36CfY{|L6VA|?OxD8@B9Qe1SDKO5BvuU3}f+c`vD(!m_P^x zut%UMK)t}H-xx?QbIzYmuHxUEG2Ey0^8*%pVohTiVUCj~?teUfue{ivY~Z#sam@b` zj#;qD7|!2b{cH+-Fo+SEWsUazdBoTdrJt~~(5dwJ$NiTH8k2VI2x*ERU zNIiU8WImmu&zflFkd41LQyIc0^?1+{<^dZ_;SRYv+s|?QVa5~arG5q&WPi})oCI)wBby^drw|m%0fBsn-M^G7E zmRT-hxEVxkIby{K;dFss*0L;u_y3D(+|X$TocO&pB%?tXkgmuF4d{GM`)gUUBkCbd zRMM+1!9$du87O>rjntG$5`u%J=6SRc;XgV};j)MPkXAk6PZbUxL2d9MGa#$einiVM zN8_$=C@rT^wYMv)5W=ZeBXolCXfi+eca}~} zN5bSESW}O5g^E0JIE-+Hod(3iKFfCcgY_O-LML_SPRs)NBv}6jC$;?6(TN&{w$BVlBFGrob}c~5u0uB z$DwqsGFhBIniDRuWCeS;k{*D3?HNO)_zS7CMbN&H;$;+#kcl8yzVH2kY4hi}KEUDu zp<`!p1SQ-1vNi;_-YoeJh*WNN8$yP;W}77^eH{_|#Lux6X#6pM^TtyFpTIkK_>1$+H%-fs|Ck@SgiDN=b=NbgI&Ir(F8wO_`GJ+l^%rFZ7&4Q+7ppTmQFFMM`2zkXXh5B-CkzI_SHlD4 zGZ{-((2>uhcqXePagQIo4s$Y|Ph@nkktnPuriV#AOc8W7&hiH;{?q!5Hqc1;shAsd zw94b3DVxXsM4`!7Txy|gKv=`+kRDQHZ^MPpTr$A z0JKIB-|@bD4l@d=!Y*1XPX`R3(hIzft*$$htUFJnH2Pwhnf+Ol-S%fDoY^d{pG=T; z1Dx=edJ55RY0Da3kuIsn!RpiD)X(sL6(<#S3$96EGHt85U%06WHm}F!$zD-`Nb;t&wf3o4OY*IxrQ z!N>4+e2k(Zo#K4wPqf(P6Em)g*sf=fp4xTTl+x>B8`gdJn#_Ryrr-iCL)>HY)AA#~ zpP90;Od2b?xrBHOOjk%hNivGjqiP1#Y9oT?f+bJKTa#XzXFiaRY#oh62v0IO!Qoi2 zBcNKW^?c#lB_*gjz2PMr_VTN@&3v?UKM#>~{vp|+XcZ`Io6kLkF%Wjw_H!5Ua#3VT zE#7D9RKAG(Q#Q@+Q-N~Rps2Tl?`@e9spFerTIRK8AGCR|xz)jAVV+DlSicMV4)r)lO6ytB6 zl5<(RD7*7^oq-$d@|yJ~fVP#eO9y}9aXNb6mmvNzdo^O~(qbZBQNZS)EE|n=I-Y)6vnf zvrj2;-m=0ZH`zMQsqhJ%jAc3Ap3L5xcN-$Po~3nsvq_*O$qq>E64j(?qvSE~dt7+= z`q0HucY2@`lmm`mI0{1llOU2&SrUfv^MSq@20!^T&^3#HI-~$`zY3XcbGl3CNcIZl z1>HGihY0QP&lDWm?ELQrFPFM0ylpamdx=$3zC3fT+gr*c`d=?UJI0m1!fD8b#?!Ol z;-7-6nMdltjv}+r(IC%W^h_@CuJ5IPZwzA|^=(JkK-2lIY>VDaWlcVCV2t34XV>qT zFrUIFv|r7jBlBKaU)v6v5Gcdzu(FeWK|8GSgg?I4`KA3c4Yj@nUk~&xU@A_C(I3z9 z3V4lFya*V6()*U-$qsNlV5c&i^bJaoz-Kg*pRG0d3_BTFaA#luGu`((fiyFN9#5cl z1-fzR4ic;e&9;JZY}(s#_CFy+Cb9SmDRt<%Bky_&?dE7<8O9!ZFi={5I$Kwf=oOIm z_rcjU^K12=pMzpwSftRu;)katJSA{ab>LoUy0!W>k#qm{n=fGm(g&bC=BWHjTSm@e z2VXZTSr-tMX*eRzJgxt_bqNQCusjx%Y(mZ#(T$T`;BbNEn#2^^vbD```()YU8`v>T zyrIPKYVWYWK2+lUaF^`Q5+n47lG+#I`uUwT%)Hc=du#fYcSz6ik?&;3`|TgQ5sYyH(C9>%f0P>$*88gw2y|rxq8rO* zpm{v|c}m591u|+A{AdATkgMP=;TFSzg-`QMZW&&Zr-INUNYev6HXSXDK%mN4H&R6o zMhlEC0|zW-F2a>kQHtR@&cQU|!lQ>PZCbNzPAC}CrDGQI=i^I$c+}4B_IomgmVR&dFWi1>OKS}Nc?)ut_Vq1`ybRL=P3R! z7m{M>=YsBj2}DRX^zb zg8hc1;V(X&jUh23x=?BYaGnW8T29}zRW6t;6T7`xOvdFlD__-uu~0fZ0LbS^z(maiz%w{%=7!*|`k^%hL`$Ow8(C22UFxk5ceV@( z`nAi};{`Unz`Y;GB;L$8sMzbwe2A5=R+h%nMip-T9tI6M-f9_)`+QI&;>(1I5;dJf zxj==lM^_udeR~JERKIW>?shQL8bFX^x7gD2!uPBu^6IFjXKKTM;$Vb6q(GEZPssO* zCub=Z2f;J2I@tr4JRZPIT_sCXMHl6GNFXsPoaz1sC4NR9nyk-i5__j zny@|JvrymmvWK7h@j$!A)vnQz(M#ensq!^DLpN^sN8AnL*Yv4|02N4 zWGIfMmYsC%--t|`Ul;dqi$-S&C?W zyp6(WW@jI88s3|6|55Ct$%065IuHuD9z8Ra$<1He@;qA00KAox^L%qFM(6u1IoS#T zkP(K9jl(g+dfPS<*$__gb z#S)AWJSRMg4kL?cM|9T`XLL2K`tX&|pvQTF1A4CV7JLR~i)qVgk<=Duvp1{CBk2dbn0459%6{vKxw`pltP%|n}iRXic zCoIjc{w4U{s35)SyguKPZgSt(f`7qPAmsSKgaR7MF={zVYosGW5%3w|Ox#-vv;GJF z1J{_72RXv?in#c9sy`II(LL^6fTT9Me7%HKhJd|^gHio+Q7iqNQCfykaN*PUKDqz*&u=NVU7-Xh{Ya=(5ZnSl`;N>tK$uCrWrvb^ zBh*hiT>$^Mm8{moy9Sz*lxyrovZ_ERwK%3|2Md2U$;(*bJ z>WgPDI$lsb2kh1IPcMKCfld4|CL67G{d+P%0WLlHR8yo&bi{xST5lNu=;4oQWt=X$ zQL~3eOHPyP4$0~EM1%p@Nr7?YUq#b7KcM%Si`2tKEWf@0Blx{NOoS`~x0|p1I_WS| z{J$t7fTqxGWFzmZDQ8%69Z;x9;ghAtN>7E|vZ-#$Cx2+X^Bwz_o%tb*(kodP&FTxf z0VSf;XI3S)qW>6ucJzR*?4fhtAXISdhW`N_Y|Dl~(IL=33UGI@9tI`x6Swrgl}JgV@z_3{%;fT3aZMh5}n{R_2luqvgn92zd= zuv%U=qZ@{Y^1n9J#7ye`hu*7`b|+UmW&PFvGRjc8L(r_OP%A^kTp>LT*b#1OQB=m* zU{J1Ni93e@BqL#?%7Wf`;PMNoq;PApKOjc&La^Ob7TxaGC-C%-uYm-@a;_^(tI&mP z@gXfeDT)vcQb=$e5o4|(WMlIO|7g%AI=BJD2Irik%qPNcQQ|o0f1_umJh?RbXOqGP zY+QatQmLS)GZ|FpFC7sgd%UFk;(BxOcl#lBxLETsOxR@mYXUCNq9nBUzXkbEB*fsT(kX(d78?hlX>D1%%+RelQ>E){CD&O`SQ99U9L)hYCI$?!1MXgJc zhW@KI@EgFFb&apcq~RqnEN@y7h39x)f_l96NC=qh;Jv15MUst?_=bj8ghEUVJ~<;i zG=bA4?+ z_a!pN5Y#JBgUh}^`qb%TzxAh-9np(;6cSpL2uJ@xZUfv(H$i@|a5F&7n=@Ph-Vnqn z4`d)f_PHX+7$N0=3i^r41Moi67Y%P@_SN^z|HAAYBplh%VMLqT6GbN^``XZ!FJtKUm{ z7t5@9#m7ki_wl|nKapzhM+CYE-Hj=&@VNd0PHzmCpoVdJO~jD`8h*p~IN2m{ z-5L!7=8Kul8{j*Va%lb=Tuuly1c)T}|J4wr(W8+DGO74pcH-6yah9{t_KDb^23jMTA2>oX%m~2=>~toJ4Q* zxmI7p9LDy8=xBUkBJ$lE_kEWRpmJ7Tc_^-rxN?1}fBf#_G)O)P z%Z3*U7?0O!&KN`t+0T{Y)f-*5j6UMi*MSt#Hw(2Z4peM4u$p$NWiX>5Y5V|DOtlSY zgOw)Iz}&+GU}LZ6_<-kPU*qBF)EPF~d839~b zsNy9X?HgYrCwL+c=p(S8wjhkJ< zYHb&3mBG6Jvbe*sJdAFd4M@%;0I2IPEk~P+5TY3C%+WM3ov9Ee=f}N(EH>5F5N2aj z4tfZ1w9d<4KCWuu0 zk}hxV#X!G%k#2zaewF{8I}h$%&|N)_4=k{UOki_>c7E-^Lw_d<*}z8F1c4A_$4I)h zH38Uyx1zkik!mjjmpNwf6ZGIgRll7d8j1>kh&sHK(HWQkZq%O6pQWTb;<3j086ZVn z#x%0^-4Smp)g{Lc+=GY|LZtZa4q&E;1s?U2JKb?0oLf=-53jVBF0o)X4 zM}oAl43f4XDZ7EpH6C>a;3b8tqimmnn$Q3`A_w&d!UY3!jpW-4kI)?Yi0w&EYYListEJj$)ATjhEDA)T^~>=FBawP8L`@cWUR{)pD{K=zz- z7_i!#x%e7rR>^*oAXc#d>L#0_CBUk&g>#miBI?kJWf+$W1{~=VwZmdArg{IcMY^xt zS8`rLrbmbmT#`|>ksUR#{UdnC5q`WhaIC@#ps)f>#C8>YuTH}J7^ge9z!M1mdR2>1 zI+q_8r`K zFqChr8VuJY;yPZUH$dW7_<9CdyjlX-57D7UU!oKdsjylp8X{h1G~#8VGNgD1wS%V6 z%z%3z7pZ?Kh$^i3}W@0R|n%OrUUgcgOQpR)}jAG3xaYMp1CeSb7e=t6k2Z z+1-uE4m>~-=OET`7*x=FzCIcJaw!AL9$2{WA0WmCDc*Kl8)a{%CMyeoK#`)}2qo3A zwdvZDR683gA~v%#sL2LuM9q;**}<1=>69U%5HZ2MmzK0|5NxchSUN&O!)G@$e0|WS z2K<|IwF+J**{qztBnSD2mqb9azJ!oD9{;xDU;BYaDIjsWGnMy0?gPP;0WJnWpCmyI zd+vKTr!V!BpX(h$DDmf*u?0MeKjM>dB5QB)1u+(n=h6E@^^DI~sWg%(`mFnO(QITR z2W^H1_j~PYWVHM$J0e!lql4n{}NydG_8f4KB> zyjY*RbEs?|Et;8VWTGhEe!;LN{Ek`U_4#}}F1R-6WqOc%nP}pDaoF|~Dyp;ww}|}t zn@L&z4LBSq$|MSU^8w{Vughu-G6u35;pS8x|7}}~@TaN@!;8|<1V>|X1*RXW76%VY#8W=|G)%KUkIhz2dfh7G z^5?*q`hH))bG@7NIb>s?60|65Ta2Oz3_)6+YddasKQOUcXl^)O)FEaUFRDX1#`|Hb z{|$gO`={5T-;P4kj^(A10G!KlIA!{MKhi~aVWWpyc8L4%x}*Uafsf69Ml4~q!0i7P{#HW@j z^qf6Kl!JN;IaknmEjCCgK@rTDfRAUcsDTh&wN=xrfc*h7Zh&hFR;qR47P%xD)Rhpk z80lG0l+yFsOne87Q&87UNFNP4Tn3?u0X^rUV%hXx%SBN=eQBEYd4+f8$EVQNk!33j zuhV$QkQl{d0OX<5;Lo9iwchHCk}&(M)9~C3kDUd0zc~@heyAr(Iy%Q2XTDL>zMbe&is*7A@P@!B(q6dElvvPb!o=x0*h!!6_N$x-Y{$|oRr+uIFV z#c7aZo)Lahta58ucMRmIF+H?NPQYSOz+eYY1T{! zC8)U%q$@CM!M;4kVH@WanUS#2`b%-I;ZqD@2LCu#3?AA!l23;O3eyz`~#3Qnsjt25fYYbLQWh0&|f2?OkK@a>YF+(~1d;~b$@tgP( zX%wLZ4PKC09%7bzad^NZ(-@19iZ%!*7t`Gtp%JuDikFr@nn&8&szhy1>fKWzzoaS< zH&qnjY7<%#TP-Mla!c?ko=u-g)l(so8QRg1&WjHEb*E~6Jx#JmbE>VlJNS5y_1hDt zP;yKEW2LK&Ou~?xuLjKeF9kUYI?WR2CkpXvWqa1VhgNiT)PDHJGY4gc&c>)dEbZHZ z>CVm-F9$6dG5@h`XIIZ7r0fG#67p z6>^=VLm4ywRc&?D4Nva}**bPmqK?Zu6^ha>Az6*EI4#tEsXfFw+v=MF9d{SAyWVXH z!h97V_ccduFqyzYe!5BN;qiK$smt{Y0)fz2WzaQ-WKy9!6x{+2n?WhQtbY6WOzAct zXFmprkotZ_iG7C(Lb?s`R~YAEf?neamyX?vd2|Cf2I#cFTlaOIiLupWb5K>`3+t-Z z-KfE=fpSvY z<9^eIMl*NdNWB|#wAR@BcjDhZ?(JQlCaC^_(|(2H@7L^%9~ZUHj|00dt37@vpWbT+ zoHU=Ac&!s}&8>S{1i_nDe}$LaRmEvh)cQPjDj7Q4{l;}=Q1d`~k2hMx)45`Y*K%B8 zd$NcVZ1Alst!Ew-JsT<%sXd1J`*gAqipM1Et|~js3fh%Y328M#- z;>Wh*=#s|o>vw2WE{Zb;6YkvX*;Qv7{+~uOwToCe-Ypwjh|2g(4v@hX^TxiPyVXvI z5sCK$;mg6zyfiUOZxa!N&~j)^>5%)kS40c~vo`RS`=8qLg<8wB(=9d$$LAE)m9}e0 z{dX{_b`D^xo&gPlOqSL-+pk`ojuy&75c!Ms$dfIgk83`Hr;~PfP+T|BHJaPNnmxZu z4e1_LW07)fZdYv_W!P_pi;#cn~ z?8^8TJhIk$lg2KO`vcHYxwqNyDMa1oh?nCzI~ojXY?R`g_T%45-xcv0)jJaCmu8gJ zaFG|E?JgLDBllWi%eZ7REhSKOm zh)!;A#)c{ZklWz#H?!Jy-eAGQ#-INgvafP}2!tfxup$KSbSEa2>wLSY%z8t5lW3S! z-Z)s#7OpW+`D z`CFXMr942}9?FTjI5jb%{Pc#@qi~Q&^;fy?^;zf_nm;`<_xD6J&?ZWl{!+Q$ypYv= zWe`-@TSqr|B8DJK&a`SfgAvLNTY0Y7t00;;pyQ=q<(M_A#QFQey+<&kH_i7NO7|db z(o|O;n?k7TvT6CMb399%Dx!X>ha4(8QkWA=CoTw2o4&M|BN%-&3LovH7B;jP|Pnc->@0{_1mS!Y8L{QL7RIkn2 z&)9Gy2R+6%Of7S8+;;Xj;AqdncMS8tg`MQ@4+s=$2a|Fbll&F-S89~!$9MeNeq`R? zRa@PfD=1aOg!R#A?3IwSF$fvSU(48$;15E%8R@=+zxqS8riX7<)GqSrJN$2L<3KI@ z4F6NZ)b~ZP#nYIT;4nC?m|e%)Ydw6pSQ7g!MH{=OFJn@t%#iaE>#?KGF>!CqEXJo_ zmL7zcl+E9+l)MkpM6a>d;nviL)^%sf{qu$mFP5ZI9MgXBYf7H?`8;-$yM-ZTrgb{oQw5L-n|=z- zkKzoEI|YmI=cMaiaZDC^yYglqyS zygsrq2}Hcu_Z zOMB^cj`MTIxtW#&NP%Hh@E`t$5@GSUrv1i#&jn1CnUpQJ`KE&gd9mqIIE}IWND!oU zp3D8r7B)GNL7p4OuFEl_>!*a{xSKX+F&K|Y%qnzSd8Z$3zItTa^Y!LERz;>T1XgO! zx??hF_a-&d_CZxik*m#ABi(R>19i(SqitEhM3LC|#Ct6B8vExwST?oE``rnqZW~== zm}`Udwd{4?fCu{$Dc-nIQNewM>vQ{+i06t9S~ay_I&`MUV$4q3Fzq&jcH$#xqvZO~ zcto#IZbrmF!g|#(&th!ahTFQyV>y#(Nk7_x)@uC4jG^I25sugn!C4(qgP|LWTnipW881-vcu)I1PnHzsS=;M%?ObbDccz*^-18zPzZ;9rlrh)Q zK7^zzKPQN~*mZO}h6J5K#Vclal>6k-rib6n-$ck{QmW?%_Ez5$H|1~C7OcpmHVWsz zc5YKYV@-;%(z3@^Il^Yv3dYRX63gH9xD~uE_55Su%Q!isa;r=K6SpU}0NfNGoh8r`P=FM#wtW!r1qH>%HcF?z*4nzMntfeUJC) z2S<+j&i9(@I*KE0izq&5(A3*4R`?>3N=-O@ae&{FFWJTtWX;nQFPO9N ze&ILI?_`R`S~=cOA?$2j@i~~QShTam)YhA0$FLMG8g~jGtZ|znd34WOT+uu zI|nf3dXyHjFy_7#yKH;S{W&cE)X~0@XiRk9d+!)T`C5833*mCl9-MC*5$qBh5E}&x$YO!x}7he3ePAUG|)yWjU`mCI(_=| z9EFF+c=BDRLEVQ8e4tpIn=Kzxn>P!X32QzXbVJ}c`FD1cX1(652P1>!7KHnFJ|4Xp zDnQwJl3<&CS^IQ}oh67$BD{*NEx*x^A#iA?{PF7TwqTpSrky$K+~C9fzVw_V)|U0U zOuWPtK;Y<7Xdiy!F0z}~;uT#jKbx-uGb{>3y4U6*iXyR^8&#Dm74kj+%&$GnUxC0; zJ(%oAFXqb9tmkj4ea6fUy)~r1*}+pm|4_sVuW`ZVHL5y!@pWrYCUzG|NNg*Ny=mpR zuxMD^0_4AH)UQp#XmR>n=1Thr6h_Fn7t^(D4TI_lQV1aBh6H^5q{UeKA*)6=CX9t zZE9|QZ9J7{i-)M{Z?*Gou^-ePEevX@z+#3WG5=4FNl=GD+{fa`QZ-YxM={Mb_K!7}Zo zq+aI-$)Xltr9D#pe&%4Pnzx4b4RVg@pP{lzvMcUqimx7Bh8eYcJm(UB*-?KlnHGbv z57N5lt6h6~K`j~Y>0Qm;u7I$?-Q9x=ELz$_sHo~>Oye3mU|cB$W{Ce^D%%1O3bHH;y(QBcG;2I&g!S?08b)5CAKsVY7C!&9smlB@5#A#;saO3krm*sCy?KtqY5e9cpA#XG(V6F1T42ph1emI7grv~+-gD2pF<^}r z%+ZFUCV`vn+bhFcm#tG%1@Z<%Sz*?i>Ukbor-s_TgqIA+Ia5e-zvbt0HS+HWPShqm z(YoCac-AD<_}5+;_wJUsseDNK!GO2vyd&eaILPR+=`8zw<#noAykr)LO9_Gw1RlNK zVH4}m5Rd@%(t0!>qD?(@dcNvhH}V9qJJC*<`q%Uy$awXh&v+mo;K%k#dZ}a+99G9% z48g9MP>w8FjN+dCe^%l%O+RgN+& zf{hJQDGlqOJk7G#G>D4>b7=>X>n;Zv`6HH?CK5W9_FQaANMvNV6ml|hF6Al&T49ZC zNnc1v0ljim16qf1becIua5+C+{-5`Qo-;&2X&?9ImOmtaU*Rjmv_0Cp%p@^b5$(Nl zw6ALV9KZk!Kc)8~-+z~-!WPsXWD~v_Mts{7!GC9RsWelW81Af~YsclGp{5vahk=zd z0J2R_=%zn-7Ya=(JW`8?W3Q!{b|jfwjY9bh?mn8s@KF!s#*RTGigDcC1=`Y0NZXt%D0CSn^j5h%{Y6_V(3#> zxw>vBEb$I&(w54;s{nvawt~4aXOld&D3B37zpwySL%U=Sw3oi0k9RX@fL9)g`;=B6 zKKyz7xuLPAqUa2jiW=fEg`lUkrGX9om%#(O;lkU%@m@@L@^$Zp&{~MEWaCtJo>18C z`gEHKjViB7m_eS9Xs7hg#Q28t%kp6@UVAR)754p@-bg^x;;7XM4WHn_O5+`CNhozT zwQ{~Q-P@NBXBPo|um=w+!wwQ}n3IS$@PZ4SMN6yLn@bPL69BZpp?0jCHB%C0tlmf1VM`=+(1o^(P4~{cH}>8;-y^lCCV8Zj)U|%V zQx=k?D-D;WzCDRMT#ofl+InrTR;*NuIX9MVGmVRqr{hxM*)J(;f*anj#6!or)>i0O z*O6ah&5xX90Jt2`!^2$Ob(=J+~>e&X_*Yn!GnvBt{s z3aoD%x?wqecU5O}e;iTn7_xp!qV-Otf2}BLh2bzPSGWJVTyjGQc2L)MrMB~=Fe2E$ zx_q}Vsoy*JX!w>((JqCl#*F*7+GSn=j^`ImR_ks^-wM$BP ziT0}R&r@;h>pDwJBoH_;cS<=`dMyqfN(fKBIWD%P?2QW8R`NOI#04;Rh7^C~SH!bh zY3lP<6@_HKViE?^J8ke+VcNOlfd;P3BgZN#VAhpdB39ug(H1{8Y}26F7`XoGQJ2l2 zaO?gOY>FKDptN-enHw-0U$7n15ibO1iw_FZWcE_&G@qXglyrS-#AMi73MxgJVp_xR zy$1zwI*YbaomAB=J|(?@uQm*i!9JLPw)@lN-+P+h9lgL)Rp{;=tLx?rokALt?&O4x z1qf1SWs7TR#?8|Uc3KyG>@SS=-iq6~?^lMoj?GglI?s z`4DJ`OH5=a(d9INd72)=kA8$Zr5<;3o9~Mi&@;tE*DRSc$=~rEbe0jZ`bjT5YvY?| zf4hj5+6X=C5uJDIuEWP1hszEx)D&P#iT-4ICAwhVhN0Z7CKpcvI$|jq3)89gq@%q< zLfTI~oI+7RH{bvC?w&+n&LaoPxlT{Ij&0k5q|T=hN_7;ddx|;O4|R?oIt!l!9xX#5 z#RV6ezHg0Pr7Zvx1rHbf#wFgi11e$E=yKOZgk6bN3@$OvG_|S1$JPDiVKLbWtr?Rr ztt+sHm;{?r44$$q(3k*|E>$RH(vp=Rb-G0YnqKgy;5KJx_9Surz34YLMRJ zu4aB0u~KL_Houv zS8&HZBO#-_OR2MZL&99U!sYDYm1C#KOnFV3sRm>9@h&G?G z6v&VFsL3uEsOmq+B-{?M^Lv#cg4-MpAh7%UD^qN|Qk1x_&0(ebnvSzLQb(~XXlSLj zKWHCDo^_Sb&%(M{f&RXcO37%zCMU-BTr;)i9SZ}&B(|CM zlISEkg1*K>lII=z125*z&ETZ%7

%$&UE`-NK)$nNXo8Kr2ayPKdCL2!<%^RMQTF z1fe&7idDHaqvz{fT^~QY{O8iosTR7>#W|WeR~m0sF~c9_K175)lfFvn4F=?Nr>I^c z-+0GO%S4Wa!*-3M-D;6_(LTLqjE$$gWu3*7(!|NNZ< zD-PfaSF`)0qvPmL0WKKu>OX$a8JU21b*&9n(9)wJ2Qi- zG7xX#j|ge9PV4KXCZEX|#f~JbpqFS+CiFAiS*f|b$XrlXg}G{+=B=Uta^rQxC_x4s zwK2Oqwj1upQNKPS@I_q^HA4cKfh##Ku=OFw1t@QW#k7pWJ9D{LZ@?7fHf8>54tmiZ z2y4O*LGaGo{9uh64_ionf5igwFlGsELF;c3>lK`cwc;m{*B(A9NNRoxL4?-jGTxkg zb##~(mL#~ng?Fe-*Wb8|0_&l3Co7X6qco@mmG{jHP89#zEQmC#4tWijB+Zqg{O0Ow zl{le9GsDjUjvEi@`SmzPj6HO-wF~%&lv+j&x5!oe5n=tLWR&F9BohW-)!2^x-t_Mr zD;>gPB2a^e=}MeY8LCklrK3!lgmn{gGF}q`b`Xxl2zr6pIU0{>Hpt5kd;^hu$*iT$ z%YfPQ%{HMkMs#y7NTtGW^}Fn@H0_(b{uGq{yIFa0ue;Ea(X>`4A_8Ic@r>tHYVWnx=CzH7cImc*MFOD3pnSFIf{aJmOI~J>(OQ)T;ab&A z+jkp_J^Fhai!eKU3GK;%m`c~^?WS6o`RFBnP|~M*aU*CRl=JIdrWu9*Jo3QGIso8H zKvz0Vw$C0IBm=fQ1efUn-`N4QS84H+5hW@HZVsY{&Y>7tmK*HcOaoqL|BfPNdjjwz zt5+e|;9gs~w!>?$EjnLc9%3%gt=41eEg*Q?Pei7x3#h|qvL0rLD7&W$?N7(5$9w=n zB+s0E|27yJ%2Nt_jxP;L^e}EvcnU$^N7M6K)TMmx_a1GcxsYa`7Z*8|Uul8m(w_pH zTgC~?4QGehq(ul}9w`ie%B#Qh9m<7)Pcn1;1^hu&3~$}!!pt<)@2HS%ztkEZM;+__ zzM2uqrN$g9;f3K)etYT~`3EQvyr45Vb^gL*Z%_`^*IYI)j6Q#mYVKWCIx);!I?OIS zjChK?gy-`*I@o<~qjz1{dO_!!_7NLl`G65OxM`!6!_8th$o|yD(j+P)yxIEmvEgfA zRXC6L+P_zoLQWj{PIE!Xd31&4ma|TZJ=O>E{J}VYN2*?rfj_(wE9{Vzrtq4Km)s1b zy?!)0-b48v?aGHhE3|!gCQj(qdW7a9us$Ob!&b?DY_m&cm%oeF9_{7i8hkR?tKNz` za6Twox33?fxiFQKt2@;`+p|`>#byorIettv*?N|4OZdd8s=0NH#z4Fqr#E$-q&So=A;3-e%xth>KT!zxZpAaZdI~Mx5db? zPVvQrU=cUmMa4uAbO4jwpau-u!L+#QWuwYRCAYq@LqLrpqIbT}c)ffyKOvL}=fStv z8)K{#;!Bl#GVV=nInO5CoaxTEH~(R@D;fbADD+Jf zA1g%KXiaW7s@;o|A2rKdOOaJwXcx^(pbnP}Mtbf$H95=e*VbiND~-HJ zB<6F5GAM#s43<$^#V@jUxf9UyZay`kD%~M){_3?5=cP68t7=v)VXqK;7MX8RY=3A; z@WYXQ2cYZIO;hF8N^8zYnQfd0p_W%d!RND(=izl}Czo4+sZ9rXIRTY$)$a!DJ6E3( zkl^MaN2{4#;3JZIL2s`bS2(4gs*%=>0eBbnH+I6=UiMY#nv32_vlXc+e-&pRpOBYTeyV!VAh&-BiB;z)E6yxv*pohGjq>0C1~s|OY1M`i;i0E>=^0m?p6XDUaBGt zd&qXmGkyz8cJ}k8#u~?gIUDMdlOb09}_Oncfl%%MKTxv@DnL}-* zdtQ?O(gPB~%e1HmHv$8Ju~VkpWGyF5--}NJU2y7vw#dV;>${HErDOz7t9TI81L6p5 zUFqnX9=*ig(+(wVSx>stUTBMGqxCd*A7kk~@{GY0jBCp^yY$y|>avb1KmgGSGCjHG zQ>06DtO>||@2?kr8NE6?G7yNr@!i}14C>2Btu9X0gaJFlG^yKH5IV=oT|LDkOBgFC zIjnFAw}r?m=C6Pw7w&k5-7B(frF+t~u$qHGHGRA1&`=Ikd?-;J#!BIm8N7tCJqv|> zt7oq$B~^J8WI5*9<=G`a=3VC3VJ$}z-kyxgROquEG>hgnp1!^7JtnD(_I-*j3Zpjw z+UB#M@do`bZKFVm0^Hw4UsW9MlA3eW5U!(Gy=k7QSn(Yeb zxtW8XosvKAbdNa7=k{0j9m|K}2G>vftiP(-oW4-xylVF8sY#wL<9yS-c%|^9YK|cK zkmti8qx$!O&SJ+)j&SN0S(NA3tWMT}L?*>(f*AWR4fUBDVdS>FG1DcJp_a@?tDVnN zy;pSwx5Ugu)(%l6c{T$8+Vb!qvb)ikYIbmu+;Hk z?&X9`tjFSY6`T2u_A+sgwY%b6UqK+VTUCyLn(gMyI+#fn@R%!x zHAOAvS5CIa_BgG?%bcO$FuQb}7V!$GCYy|au-~fb_T@r@&g!MKpc(rv;U(jKyL^h} zYT?Iws`AIUOq#D(i@D~$yd(iUkZbIz8wd3uAx54Lj4cl_ZZV7dA>0CTcZy33gTOW6 zSlcWegTp%Wdn>9pBCEvgbCG3pF-%S)3GA`~!N}M9l_p2t9&>9bFgnnO&n-=D*?UN< zeXtFlGv_%z@30|K<=5GeN&&Q|!Ad(+LiU%tJ2?2KNW8y33JPvjd+}kBZBX>aI#A)vpu!STb0YMN8D?DLqvm=15nVmv05tAgY03z86>JYNVE$1)s{)g-C9Am&7N% zPSVRAasb%lr>Q?S1g-*aGr(zxtbf*O(=cri{YYu7-#A7{?Whv47GU!96IpSXfTf>i zwJLEdpq(YVWETBMIVc2LFk`{mP=4}9Gqx*Lss(*XOc4-6T;97u%<#)#Gvr4y@G`aI zVT^)Gq7wbGMoWFXh_c~UD{$o{Qz!5IQk&8PR0DHxec5dQGgQF4E^buKHJgt}oWtm_ ziLaPf#I1Go?_>8|=F{{_tW_U-0k=1JGpX3#t;U#y~kExp?!0?&0&|KWtC$Tf$ldHzu?SERD(treyx~ zM_$$Dw`)(d7t zqHEihCkY3k3_=xmShjA~#TBPGPkkt5Y#ksv#tS3!x_I^d~+gYPl}$p*tmE zi_TLPAf}XYtvc221I}$J{%rr|$4dLn;Vg|g89QldHDz^*bU@yX%vI=@1?Jo8Be ze_0|wh3$y}v4&@D!-ME4dtRo;gf?ic_r`1f<%SZ-iWbNp=x1H`Ge|a)ung3?UUZWo zY4$4{$1S6owFH$Ukq4s}uiw`zwuaJ~Q}y45;DYAcZ`%JdwX6tY<8BNX>VZAM;>}8o zL6WNXTSm5r&z7A=WX{tUQgAJICNGmC0WW2Y^K7@Q-t2M!5#6|C19+M90G3u)m!Reau zLXl!_Np)_4VxGJvIV>@JMm%RXx25oHdbPejv!m6O!>)KcBjcwh*vHj?wEN9D*^xsi zp96uFepi0-m*w-Xvn16=m4InIj5o{z7Y5PtAvP#+vD8Y9+3xe@3UwhAxL83zf*;Gx zrFO(EXw&!Rbj4X7(AUp5$7y(5IuH@2{a?fA0IfRiikh3=WwyXNI!)mgFUGDT(%WpK zRq7Z^Snaz5UkCt(zg^G_kq)3=suX?16tYP{nIPE>pix)nq)AXI46F&%d)F*TeqB@P zuhibFjSOqRSH_x+NIqdBMv>`{7Kb81B%8vbUwCS~<@K`VL75|tzVF;MbrC{qNgwo; zR?E|8{_#L14vULz&;Z;Q@S)a2UYz*t;8T)Ig9k6;T>C*X3`>j)X_y2f0hgz869ASdD9iqsQXl|& z(qrMsj03P{^_5Yt^ok^pt(Aso4$Eu}`+57Xn}nW3u|%j-j-!O&e3K8Oc&f{YqK6tsDz-&AgHqQpR4z8sC1LS3L z$Vv>b+qD+ue80uRpPvE@%7B3caiqsO0V}VM5y2bfx!=_^T-5xCFf|_Kw#au`n0cU^ z9qkyyZ^&-!vl*SH>=u07NigC5l9Ayn1sT?JK(pU5rDr@wX37VgrfGhXXW_qcNaXM9 zK+t$~T`(QbLw9H-LTH~UJ_u18h7)A1u+x-8q@g(djy!N3@%CZF$t zj>hwIA~wXpNCV8g*JabL@%B)x-!f*wBH_YBfAWdY3^f_$7h?X`$X5L1FI6R77ewYN zHzGun#gmH;2V`DtG{VEIx@gNN@7*;t^7B_Zrm(PmV*W$-g6 z*RX^VeTQnz5Gd>l80X_ANd@KRAOa;PPIYmD?Bw`1VhOY%MQP?}V*%5(_n24ybFi-; zr9mj3KJrja5v+ancVY`9$kK=dr+6z}7bC$e8!?uxG5O7lLN*js=OTtZ)%6bdZ)3bhS10Q4;MQfHhjcnXTTw>1FM_+wVzuQwU$3!;44c38E zk`HGRRMILKgXA~81gVCDs|esEUQ;jh2d>;*kh58C7IS>^*C~(>vmw?d6}i&T;Sye3 zB7R33J!)qXu@P)f(`wgMGu;CyT;1mUtXkgProNx{2E-;}`W8`R2?js+r$q`>(a{cs z&SW>qqaB%_;%-J^Ed8vi4W$jir-|rmg6+L(#6&<3OU}nQ39r+KEg39>i9eIIk2T&O z$(Rq*rmUtEBCQQZqOCgLg18WilSqauI})@0K-g6e4$$HU_dQ~T?Bd?Qv~e*p(|W)3 z57=AsSN@3)x4}7y!Fq_v8@TJjJ9PaLQG58Y^wdga_L`;ITvJa&)E8MW4ePqcEM!>V z%H6_09QAxTb>vOGojPL6)3VE z@dQVGZ$aYw8SsATi{z&l#)va}qXGB-oYRrFqx%)w->xe-+XHgj-Vp$unan-zvXzwFmsW(ll9sM72g8>PA9ciW!uoS9>4=HoVG_Z73Cj@1Y^Kn0Ooo9X8_M^+4h#^+dvsDXh|un1JprNC8O1oV3JrQ zk8V-m&SOUA-#Y9MT_b&!iku^1AMhKbnKg!dn9{RsT7YZV(KKq)ncrcpBg0td>qX0R zYU2_F%$r`4j3wg#zJ&1$#3l6g0@5_%$ykg>u&KZQ-cU0uK}5wBLf^s7E2`4ic-)hMj0tzR(m<>J{tiV}4x3@HYGOg@vN#JVdA-Is=vav{ zOkR2e?6d;t0BcZi%@iadIqj=>_qK)x%JlNDfbm}v9T*@j-2gv^FLO-2Wjk=6h+(w; zmJOwP_L7v8ob2j#&q)PFDf?U=qbeP$b5|jCVo2D(UEn{G_}-u-i@-g*4rWRcAzw&Zl6%!s4yYRd!t zy>CLIxfREb3Y3AP>vQ8uKB!l)aUiDK8E3o8qZS4*_dMsb;nal3|8}kaIG(@|>40a* zXJuxMR8~+!fKX3!iRVi9?1*#v@k;nv2>%b@lekmq$oA>pv}#E^d3JKS_9zncdntqQ zlixiAlZdsszwAF)mKX({c*4!M4z-6`!2P|9jo~MHg^E#+I?_BhnVevn=SOz4K_+?U z!FXn}ZzL@}PefJ}S1XtR!v|z68VuM{22I3=?%I5>{`S$*^^|4o0JZJ|c4ZB;LbL(A zU_vc781jTus%VczxA0zbc&%~mp<H(8I;62+sZpDxfg%iCE~B+ zcYqVcph{&3O3!~^6XbtS{@=VqwP9-@C=dXuh1d)P$R8d6k}N|1zN6zm4pN#`gXm*g zUO9`nctOZE0t`D8llZ7#nh%VH7^zo7-mkdzFBw3MgDfL;Qn{0z8sOSu0}bSES~!=* zAWa_V>H^WQwL|2+u=?SDH2MC$)Z zNqEJ&hy_R?OVHWx1oB+OX zo*?6taEj`$EBNbgDo0(=X#^W~I4)c?bJ{qd_KNM?jqGIsu(Quxat)o}scA!$0v{@tulxCLGFwLt+$sJD_x~>BN1*t*9?aeU ryLj>Y;E0;X&AsqtJEva-jNK1Dk-Q7rccO%^;(v3(8NK1FeU0Z$Q{LlH_ z&-cUqrXu@!)>?DTIp!E+?jRW{VI%|`1SlvdBvBCoIVdPtHYh0QY`90@U)a6x$e^GI zp+p7VC^%~Fq&`egn7r=MYLv%;hDXc6joXuwm5HGjeemf*rQjuTaBxiEix@jL@jyZ( zZ#WSLhhyITG0uB^mD+@vRL*0L5!>{c)Q0nvbJyYTO}1_4VX$lx7&x$mP|$FEP%x;T zP=Ei?iqZ=<5SLbiW6=MffBfs`?dW{-O9FrW?O(sxJ_(D;rrzTE9Ob`0&=YF;(Z+v& z6ckJuxo7>4t+mgN|MwNZ=cBT*KmK1Y#{%7jq)bQ9&Ht}w|M{b5eFw*XFUQlf7@p9g z-uaRq|38-bdktMk!~f%bVbvv}tUpf<1RVZ*WB#m~(83D&|7;oLCrc(Ufw*OwJfQ*q zW2x3Yw*URUP|(U4eDhYPN7`I}Z~mWWfX@%aO?mJ?o?f2>wd`LUkFrAa-%ABYgZIw| z{Qb-S|Iz$^?`S+x;MMD`-WUuf5VhUkHQ{kN%Ak`=yi}<&h#k)r7r(taCDUlMOB=ht zSV6g5jg}@=snk0fVp|k2nGAoyxq3RKRd*fwoZ+opt{CQzy8z@DQ4gOy>%BVNUfUeY z&N|BxjTW-m7^Ye4kBv5&EE0_(6(#0UI-k^4F4v^FKHraLG8`7^kD(+Jk9#Foq(E6H zpC6thnS7R~o9=Pv#Nm2j6(gWlV?tJ{R(H1Ju+ohLkAxKK_XMkd?|T#YOzJ2GT_H-j zoWQl5Gk;44ZQR^9J0KkYS8U!Sw)aI-tfe|Ee9H2#eM%|Ud%8Wf2G*FisO5gC->~0$ zy|cgYluhB)b8hEqiDXvEGL5G7j+mFSS;_7rF_h7E`wJQf&t5rV^y?4A_0=u9QyY$? zwd{VP>g2lG%$94iHy_~%ug}a1Lm;X%J>8nD%JO5a*{>SmsXCsDW4G23P2*}X+<}Eh zpSH$fHQW42n`d@=$<@IM$G0>L62m+Pzw^KE44*G7*hNEb@}u=3>JhHv(4On}`wK0M z*2@ChLt||o9(`%tP23A=%l-(sRL@G@ed;{JMMu|Zd3CfgV%fqxoX$&^%xWG*DWBI{ zYd)7F6ozwjG9m4m(jAy#B62VJ5S*(0+ zhbdnxJJY^_>gm}8$#gEa75~a~?b7ggyzcYP0!G)%A#`Sef){z(m|H_Xm@R{F!{24# zqnPmLip3J=}H~pS`=*{9{{#m003BpzIjZP+iq*kzBei&V9}b{I?qc@*K25fLSqAOWcVc}g7jGOS4BHBRXqquC_EP#L)d7nh-5eF%|sSPHJH1?)S1l( zolh7#*H&jmQeW2~67XKmF(>qCiR1n4`Px#4k6e}jywh5&@=i?kE7SYCTQb=!fh;28$QqEJ=P+JpaZpE* zOPV-L1(Osjl}1(S4~UJVaqIeu@yprGD-_E0#cR1^8*;E&80H}GfrT`rMmSp)NcNLg z+H=-2z;q0QJ>Q1xxkVrS^MCf75ETyOkheUyC-TYc@|H_AyYmh5&gXmP9^Z~Wp*iue z+bZuz(6wH4E}TT*ag6(Pi6Rs!%2lh1>A_2UwpEBgIlURp(|^AYUh;W%xa4qH@1wEt zT4~hEhH2SyJjG6@^Y3z%hd1-<}V|33hSry6{TW{c`0#OH(@Hmeoov&_wJkF6kA z^Yw=<(hu9;T6RwqP*qx88zg0}#>MHJ&L-%Qhng}qQ9i*-%32EwK0}fmv#C(+XC)t+ zTkZx$!b~K%Hj_U=PIp?z9${|1-s4PUF^y{p+MlZ{*BeYYjH4{mYSZMoIp80QDrj*&jRKWqv3jIH zHe2s#wa#eIIi<%^^SLU*+0OP{d;y_4|86=vo~Io_>8GAU=8r5WyJ}O`&SO)Qi4&x^z;hly}u9 zTi;66%x9E`jx%BU+r1v_-hVI;XV2nYblGniFHvE_&B|K#IV=4^t*R&}2!C5PTCT<0 z7fT(_&6>CDZ}XhZBIOo26JBN@mfCQ4^s>+}IG9@%Ggr4KT&`S`n^VYa=E|h>Cl(B9 zpa)1_hi#AEK|z!hLPEsV^P`n z)VC@U*NFf$oT9AzDa)y^YWyvXC-)Wtk_1Ywr<2N+j)%+1Jm2mQuXyK;uRK&fzr(Zc zbkcg5021C@0HM?qj}io0P=@LiO5TlCMUYq&Y={E%WNF%lfVqX8xMk2e(!QD30|^V22#KBeIE54PnFaICRpZ3Ia3Bwb?#A{?M9} z!zAsfAR|K;Jh-l?!OmpJT7KV>pZ;B+$2M)y(G=$D>BjUYU7N4dPOH7zRf)vHd)@nd-Dyu6I8cvb9uM(c()B1!VXWrsc%01ooSY6V778I7m!q! z75xz02s7Aj1%sbPc8B54-MiG8Qc)T82F3|Y-d{VHESzAylxbhs2|y8S(BS2?-}xvQ z@=S#e{q)h;{mkqL_xZeT3+dP56?;q(yZg)I4Yhq_xA$3T=~PCW_r^n~vc*a?*DllR zfw-~^W|Kv+r$%G;w^tv=4lpF6iY|GWg9;Exz;D&9X-)Ezxi(<{;L;5NF4vIEXgO5D z4hOdx9}n(jF94al@kBHp_D8FtoVK@&QOA2tg6Wm3gT|WstmO_HQB6ZGGq!qc3(xv) zl0F90KSsg-=;m^sXkLEEIzw`OzFWr&a)z>03cL316dm(bZlUYtQEg1Bd*N8a#Q$k5 zeJLQVBQ!KGVTSZu<~#)^m+c8`N%TA#t=1d4{`!<>)SBuDN$lTK$djFCjr8EoeJH=o z!nzSx_i&|NZKS`b>^^vBnD*3srg`QpUuBM3q2PVGLgCUIBk>XjTf%>;926`}Fx0XT z9}Pvc^%LkJ`AnVeW~bwlrk`iKb7YD|3Ubx&7$~*T{W0le$6Ng!j_n3+n$7dcx##M& zThLyAOGMl=)o(%L`Wh`LzF}7EF5*1V=(WYAK47(T{> zrNP)qYwG;c!B1(Zh+?V5b3gVAjlR^z2pP)~UZRtC-mB9W*Dus=_~v_HR zLnpw%KL5>sw5=U)psY#Qza^@4KY*5m$y>mtG8jzdoN_kQYu=`Xhlh{s?;aq`vB4C} zb2ips&YVWT7_R!N=c83?7ewQb{CSnMP?zg9^H?Ie*u>5W^Dm%ig#ulAk~_)7JeEt| zVXs-EDm+bdW4Iudu#2zr4gvJ$g!KT_!k%9-xe%a^x?;D5;*eobNK~-Qurz}wkr*fx zn%M~5s4b?gtHC7cyCoiOzH8L`B-vfmh09_>io$zSff;Y~EB!9r!vaW74lr{>Wad`Q z`R)0lu&NiS(2Gg{!65C6KWME@;}H076Wc*Uq&Hu0eKbJV)@u-bQz`SKbG*dqiLsto zv$3&~>J8fHK{`j3L0?V~hF)wTS2o%CY^}MeA2LmdxX-Wt|DG&kO{&h~J5Hh3F#m(o zueh)6US1a6IBj zt5v)wl>T00J5gCB4^Ua30jJq~>S<6$Fdn4Qp&j|Xg$HynK95o2+;6Y?YD}jr4qdIzdf>}%eMsZ$I*9(#lAo^5&?{NwI<(H@wZ1KNBy7EpY&*a z&=5|x%1ZaRk}Z4wqsZ<0Y`nqNuwj7IBllO>@y%nfyPTCsq`p?{4u4^aVOwmNO8(P6 z3E{<+rZdLb<%Be^sI0tG=%e2Z&M)?Z?!$>TT}doh^-CieN?xhjtw(CStGuoXDsjY-F<+j7hC(-2fM*@bCMFUf6(Js$#3Cy9%-67gOVOj3 zjifD(EQCK-D)lv_ZxC&Dz4ZPFD-UTxmbhW&>MY~o$u}Z-xTb4NT zf%7I2t`54QJOhLY8Shmf%l?y^h>1b+c7lfHe;?IXbFkpDAB2FX(RBXt**bhw_sKXR zYFxYb+Dl<+HJe`oHl*6SJ4-Nx6x7A>dTZV7Tr_m_ z&U+OpoL@=mfC}`s<;yutb!hL}+*_K@W=$)dj-NSI-|8N9$xF}CX)(jihqr^wqQA2_ z#^}0L(%X@-JcZRTUN@kCx90LeLxl(A(_%s9+Od})YM=)WI> z4IY!8nXBtTo*^CmReoj8vMp990((0%Gw2PI!QiILgXhq{qQg54MCfQbOvmH* zAcu^%xHvS}tOcL!o@d6qQlz>dq}MG+n>H@mI&nY6U5h& zn?wrxn6t4ND@RbZT`dAmk}!$G-h79rC&ybzt?(p3|{JSbyI357_KXGh71xC0D6bwG}VC6+)VJ!FvtF)6FrKY-KWl>&=sR(d}eRn9*#S-gssU+UamDUi3N6C^i0l^n`CViE!x0 z@>V=r8rABL;HHTIn7*<|{|dq=q4L8}E0t8>@Au{fZ{Y-XJN5sgB@M*`JQ(ce$-QUm48N(mOv3uGrz2sxzibA0@MMcOJAsUFjQ z%D0+II-PAX274l3G@Zw7^{|J)VG6e584N0%2+YPvdfg?H!sxqdqe!F~Gc&R5;&>qu zkw8+31bV`ap`2g$htLXw0`_Jftqt;A?=bybY-_V~UWWg9IDZ^P_{KY9wLRxHqP_l; z?&|0B#X2@*NJ2OI(7I0Rl;EHk`hV)Xo@bvrRVk5~V{16_Lsk}z? z105~sIL7WgzN+H{pb*4?mh4@oB8HC%rSczP1$6h|tvjh0AIoa%F6W3cx^^kcl{i`l z^0iyS?=03yzy4rE_ay`~K9U5D{l-3%9ubC?q5|+H*qrtyr{klfnc(d{d4rhgP35c` zFbuf-kNvJsgX;9(j;G%wKe&exX290&?*_~a^X7AUZBWOqVWrZzC;@#`=lvh|s8BAq znU|3J*KLJ@y$J-Mhk)i$URP*)7%tmtZoFnx5Jp&!zx82{E{Va-G^aZ}C?wkQK}#ST z2`&XLuD!Q|tB>+6Tmn5=0odmM=t=vipN%UI&xXrBl@+tHpF;lV(|1>T{kH`wH2GUrJo66Z^&$5AO3 zy+aA}N5YjO3$=y)owuSu-jbnhYN80M__25T`6?OS5jMQjZ*9)9wZq2;1N^Ug8t9=^ z2Imu_uCQn(XkWNR9zBkqY-t?i(pN-3aV3V%{`@?-6Ah9u8E%9Mrt-%$_e9Gda z@R~%0<6lbmuor2nHP3CeCL`c-q&I_vpCg>nhiFl9?OKrcrYqjqto9PosMS_y2@zp@ z6Z#E_085|+$r$iu#1)Q*I>x3JFzNf3S$3OW`xI++U%?WSRIuAca?O<{pZuHxH1Xy;mH{P&sBmxTmNL|F>)jJY@;J|P? z?8QLHj2W9X{iTicrfv&FKR!EnM0{qE);kkl(I`$?(41m?|2&NXE*KQdRJm3>AoQq> z2I((z$~0R{FuYCh!$1KS2TAt9`#6c;m4R@H6D*)rSA0a1)apm)Lu#{x9>t>-y6lUn z4Z&KwHC|Ze^iXF@vpd`9!8C3)P}A3WyR||0y@UY+Fl@}>O3xe@4vu$LXP&cBwb9ty z@qFq1XojDE7)I1Upy|O_D?YH2+RAIQn_9b`d+xTy>k-whzA%jVLF8Q?hV86xo62AO{U<0!d2z%@1F)P z64`|(z3wlt;>m{x%1WT_5+arx%uo_bwETL|RDxDdGBG85mItBBTS8B`<{KU9nR;!X zR~n59QDoxJ_?UF5P_1~OOD!0U$4R$yKXu%suG6O}B|WVULtAALefbB$LV4AbfP3mt zf=&4dw)&MOfat5j7*TRLB4f|-`pQt@Ucm^5<3$>T;fS%x6W0Pk$cu%;-~lui`e?Aw zM4n`ocQ<$(DQKx#=LH7pl*-hx85aCZgZqAMI_h&sBzw!QZR)6 z|IUh3STr@mO_G=nGDzw{PDPg6irR36c2pm%Dr2I=7%br| z>c2kQ6{4sp>-}U?f?e>qcf-~f`#F@xrUmG~=fynia4=qCd;^{9%#+VsZ;l4|Qi+ef z`BKp89S^exli5a`|MLt z0&%@+(#q1p;=VPNTWLET=EiGqql5-{(!qyfvxxa4-@OGa`|5!9CN4}FDI}34GU)mb zWq;D^Ll;eEEoC;J{pxHSL#-@@PAan0@1)KAJCvbdWFfaa3C*Po8d`Eksp9>cORILM zLXCV2ywF}$R4WRvQjc3Qb5&l#%*;$KH~S5Aotk)fqgHZBxFUR&wXCNUSr3+HhD6%sd*U~_rP#878*q^G|sI(96RMe(>@5t6pY5P{8fUQwdSiE0Ll z9pn-T?Gv7P^Fg7%t1<{X9Zg_1jsnuAI+aq1(Zetxv>tkpLQN%8{^<*|AT0tW5l;jL z%!AM#|J$h7=FTahJ%p%MOkQOpTr?_@H8CANkDHuurjfm#V?gc&gb;By-kGf#aHcEF zI0S0!YzofbA?6(q?%F|Fi`LN1&;0Z_-yrk>PPgl;SdC9C>!66s-Q0CTczf)-NfBf1 zsOih17d&<%!HJ*j5MPMTbtB>C*oSSgAtiD-+4Nbo-YAzUNjBPV(>fim9mk5EZ0o|1 z_@hJ%K6&;I%M}L}CiSl?r8N4k#w51XA}i<(M34t;EYitOCf>jHFG~~v)})N-p*q-; z?&fA|O829}d^b)Rywkgh2wstqjkIXrydpu7wf>{+E>Rv_4o2(j_Lg%o(_;boxgZP* zz1zkQM{||>QH+XI*xdFhG37EHhW1|+dydzKV})nANUxD!#B~K>^h{P9z8)?~WLk@k zxC&`pn;-=BoEc>T!e0ufuTPA&?E<)_N*87#lrd>a{Z}4`hVvx@sO54A3l?-QR48?( z6evl}xL%`MQ-w5hcMIenw!TFMVUYKZueF6BAhjAr2H}3#o{T2&!2U2%!RvnC(tOc* zOzySqR(Vmk=&CK3a>lV)H3DG{U}Nh)-PGxie{tm8{oWM6g<|bo%aA|^4_G_?I9d%8 zZst76WO6K8^^c&cslCZgYlzF0NF0yEPmiBqQjxYSI?CfDP}o{n zZoQDLQ<8Mb`JO%Atp7xQCacD7Z&vZD`8rKH?-`Jb!U{Aw1ZvG@h}%lvQC><6HYU$K z8R4Pky+6^F3BvVAVyxJmt&Kz?zj+BDg@q=MS%7EDv%(C94!918_u_Y2U>mlSLjC&~ z#lbN)=1h+m!j`Ew5TDQ6Mj&GIs8jhVoXxOXPkwS1EO6+G`N@=H)?g!7rXV_ohiA%a z_H@Vvt@hM)PesMZhT_$odBcgzy<4K$a(@H?uR#mf!v1;Y4>X&JJD0Pa$n!-PSxA)^OH2;(O821Uuw9Q~ z)AG=$wVWew(emJLwBNVrN#}5DL?mGS>RVG1Z#q@h-;!sx|2d*MIy{1R&l+X1%_BWO zoy%!2Qu>iAX|Zy79BFrIv%8P~(KpV05rFV5l1BVbw};#xNE8gp4TxOt&gX7R;&ZJ? z9JOya+r^LO4(RQr%rLHobb#(%*6I4Y!&;TsgSFltpR=T0Q{!g3Rz}n+WcqRT09O;6 zNNz^vihHL*=P!3l^WMEYUmc>J488skohK=25gIMDxk>H( zps6bm|0=N-G}+UnnZiTcx-VY8)7Bl?<38G*A~DDA3PcYUBDE+$mHDKuGIKe?TiG9o z36DWuktMJ~E~&pT&mmW2XWXYP5FVb2U&q10bAKZL6G)>r9+AY|7`eG0kkUC_!e6P@ z^Qk5!M{1L_R-SPzcek8Bi|G;0*7)Nb9%6pQY+HO)tIbWSU!b-7LO z@uPXUPX^*?_5EN{bs?(M7E7k~D=9TR9I8@Dk&uO@?XXtXQ)Zb6pEo`Mz5M2L&Sx4m zTPVhQ%VXKp7Oq!)Jx5-JV%eHx69&Uaf#|b8lowq|B$HXrPi{xTU!conHUpoM9Jy5L z)*+Xym%zgSt)B2$%g}hS>~2Y}+X4!&;OB*M`DZ5+#T!qKJ_8!O*RqWTMv4ir(3RS9 zvv5y{Gs!!QlbDSyXB|o!jFqLaK8QMWg`EhEj=Fyi7^^n>rVy9NetQ5fcxqgM0CkGP z{E@)@YG&g8#8^I8{`%}_tS4RMI{d}Twn9U;b-7j>t-g5NM_?Ct3(OMM!fw{|u&$y@ zApd>)EW*uP-58P9RAVw(=`EzO!jh{)`cIa2^Z{jc{e z$u>o;<#^jW`a?^MI+~NzZ0=?wT`t=MliI)FkR^$zKU@~4!?g@kw&dWpps?5sHzzPN z?^b;|;I8$9((T$4E$ogt4ZnB@Cs!o0Gd4dtSUfibt7_T5@o<80$r83YD#F=FB8j=r zIZlyzvO;0hNbfFBu^{IUz5v+E62hyo%3g&qK$l*~bU3oOnpzmJeC;e9I?O20%+5o+ zJsL>P0|;Bw?UGDwtF|0o-g!Jj~$~Jci{K zC!;mVPI%HjiE`PzT)w8p&u=R#eJLDOx9p6vSuRtwS(;C6*Xce2Mt`2UY+bA`m85Cj zuq(+gBwk>41bBi<&Vz$7sZ`nVX>pd(TV1zLx7k*AW|)qCX1rl74_Ij!^JwoG&y>%9 z9)z12@~q#7+^+k_-ISJEP7<1R8lWti&)V)AP8Fh~DWt_DNnYLF9FNo{!6L!j@$t2M z?M|R)1jJOE(@<)rcehU--Upy&-*gC={Q=8-gq{$h?qIo5--IK=u;P{PPwt-|_ zf?G(cRCquy`A1rf*D6ic(fK*rc3;8{mieX`bmVEsb`C-92M(^~w|dJ$nVo+6Z#I`j zz90<+WoptSO|6(>q#Z#JjWmYM@%o%wj`8t5ijVcqIM?%}Isv~3PGEQ9ec>=Qtl@wC z$f-`6;BqZLeKmr`l+3i&tc-h#Su5DNWROu*JEW0II?NOc`ZEpmX@3HjQza%dq-9YF zRZeimFt(8ymi$A;g9R-^I4AXSgOWM)UehRpR)ZqDYNMI{*cj#xUD0R}W{t*^qst{a z;*`&uve`RfjVqBEoPx^pqx~;0!zcpC*HTy<79?-4<>sGX973sm(eJJJapdWAg;UGH zLBkA~;PF5SQf$lli6*Yw7_Rf1CwZ&;xRqNGvEv2qZccgPSWX))t&wGysleall!gD& z+1>aJg@%O_n@_p_Lc2$@JN=TxbrbVvQ3QAy{<%Kyx#n?jyz!%5!33 zu9j(GF2dfgtkAaIOCeJF(>Fp|&_G-Z^%|(^2H=Jv7hV5cTU~&gEU#K^SRTKM8GXT& zRr_IEORh$aj8da9bdX;0jSjKJW~=Iz?OHGMzDV!4Wq2i z+@gp06HQ9?{bc&BB+lsx7SQh! z1)~v^3Ke9LUvQB=k(~4eOrX=%R{V=xa@dWg*loG*G(l`K6WR$hFu6LL7-oNmc<(z@%Dh zYqXH_=@r|Wa4PQ3rn7}nK$34&>0#Qa6_R9E;ojntLCF>b{G|CR<0=CP`((cphRZr# z`3Wu7J1nR^^ZNAyE|nOw<(QmYj)b;&M^VP;pl^&T$C0U;)Fc`kbBw@4WJx{eyBiMK z@~|56hWCziNRo`Rd8ZGQ=z*aNpbCm>NtLA_jP8NR&jB>zl3dSY>&G@q+4KWNOJ?R3 zkovTiG-q0OEluU8&-@C7jxEyb%&}**D5PxGIG%mmJY4CXIqLMlJq&sTt%W|VgHCFS z9|j;sulBKn%AopV(DzCfWB=Bwe#tm(K*>9ZkA<`0Yy>hgg@jR$p${?g&jUj)DYB1& zMZUguY6Bm6jH7gFU3M7Mf|<#e{m)ozuk3!FcU-SOa!;QBpiFLTLFLhWazs5o8pwZ+ z)v!ph>>ti)zV#HF$F9JPxq+!HaXfrW@zAQt{-`Tj+KG!94_LAMm-Fm)l*tRX0HGns zACAvAT+)aY#q=`81Vd}2IjOd?k!f!yk5^b&KDhEQDqa{&Vfef&-{H;q3~ubr=R^YGt+#a1bH)jmkYEfY{m3N|i=3F_aLcJA-RG-~<+n3sTV0(j z5#JE}#RBrR=L62jH8_4aTYO3S7`Dygdn4oe)S-=KhxZuwAl;d`PA7fYb4&jUnoox~ z^#(d$EkUC*agDZ%JLUo(7T`m^)?{)d?Q1S7>Fzf`9^lIxUSoPrOR2B^?FGs+kHmO# z9tDOBt^0_a2@Giw0mR#7f#5u}q|Z5W4V8?*?rm&llQ>F?6vhVtPt^!MA!UVB7<%)w+D&k>`XG8~GUAV23{m*$OS3W|QtAwYWScWl$OIw*2( zr6&45Ehde_qXPlgkoQ`FHaVCmQG$37Lx@n+A}xdba+{8W!RVu&(3pV9WOp1kUE60c>>9}S`e!!}_mXcuc0>)67D*VuhVcvKi0Z*6;d)rccC9~R z=HFkiISU|I^+tPX@wJqKP18+~(e1!%o_+zV=jtpLcE6PZULtGksnE3O?I^#^&2HVA zs*gW$H_7T1=MIEi-D6Ma2RY_*wHDVH9SiJ)5W{9NFaN_Zz{H3mVD#rTsGQ8=prbDrT0DK6 zM-TfKw}2Wb4=Sv6aB>GRNrn>}Fw2u+Qv3Bb<=F(|CX>Uo;L`>btdGK_zhlliFG)b_ zXhl0Myk`7Xs;=9A(B0ufUPe|!Y9*THMHfcMWYYzw(&Ef=$vW@obr3n^yo-p0RO8)^ zD3AXR!%wMP^ZWXk(R;3b(9!N?NB#t=>K`z6fdc#l_bN|bF~IV$T)wnRCTL9PvQI$+ zG8doa`|WQ_T;N}DyYL@qb~#ToxMsDX4a-(T~R zU`aOopp2&IiH8rxm4+Qo^@2&gd z7}0nUkG$TKRTxhQIem?By9I1y4zNgd1QTU!w1BaZ zx-2jLa014s9SC{Ar(l80YF0t)?~hklpa!NT+7n2t{=)cBFxDb~uR_*lQW%6YGUor# zXQ7?;{>=F@HsQtK0z`7m_P7+hYFShUhu(u^301NIOrPd=h?vaJuz#=u-X@F4aSLLx zAqEfx(Nl%-;a@REh{_9Y`f>ngwor6C$XIK;b&GM-01cV#8DTN>ri!>|u>(3E9ctK@EOIom3&6obK)GPRyJ4m%v?rK=aKCOf4fTOi| ztyy)PEkB?oap2mIgCefhANvnTAY9S_pS-XpYRN-bVK9VaJf0JD0{kvq-!gqrgfhh8 zF0j15WQMHHSbWzzSn3$h5`1#ch-{iAxCG|4Sm$#RMFUHmAWF=O(v@lWU-x1;u6?1! zWeq&I)@O^QsZ@0Ng_$`};J_3Os%SiID&mXR%>6$(uZ0`3XMO;)+1bt+Xw}p@c#HDx zdG{N)X|Wk}1QePUP8!Hp-B-fcbnFm0AA^7hYgZldOdZ0~awQ^gzsQ7OP~gilYafmd zdh;I4G=M_>>T9{yNHFf-`PWr=DS*JeLfdEdWL>k(mK;DUhX+^MBcCwYn8%zTOlay@ z4}phkNPT7s5V@S@R=_Qy0DBOb?tb+%&JE(iT@(lP7O1+bc_ZN1G-aXvo;8D`_IB9S zU^@omHxhSX)D4?cn|KXPqt5v`-U}EAcPl(7e+Oj8Svpm)@{nQe9f(B_0#Ny0!GTUe zun}9j4~Pt6-%3>0Ml<=xs|=+narIEfb*L>0+B1j}#>~SvgdVR1X|%cDZsOZdxTn7U z5Cvr5hik9DAiVqd!nYnx@5$c!&ooX4Dd629p_Eja-uJk=d{*;q$o8{6+)FR4bdTZn z=>*H7?A5_UjF*31+KZ3vKvpZs7Xo~34j8dx_00nBnMRLEfV``$L1J++Y4d*Y1pv|Y z4;=Au&LPfIgV+TFa+D6!dl&9Kr1t`F7lG{&iyjo|VDh3Z;i&xTVaQ+c1T@Qvd|#lg+Wu$QPFz>2oP49phlr^m8A0qyfpNts9!{GqAM{$V+pRjkH|=zri%+ z(t3}tcZ8>sAZR79hBzckj1Ee9nSPBHdDT;7D3&tA3LXm;oC^W(*eWo=#skN}tna95 zFt@8M7dO7gjdh_yVU$s8IPkND_{W}YO^S=fywtvr-`)p?nW#i2!+x%nFehNGg3Q7} z1To-DBu`?~0kUW%n6;1=kpLrWIR-;X)+n8!f%Rx~)7W1hgR^#piludv=40Y0p1m{K zzZXpoAUM1A426Ow0Amr^Q%^<)9;}OhZI|Xb9D8Z^)mHHt z$p*QWxP38uW~VTO(@Q$A0b?EZ6K)WJ9ZZ=;y6iRd1miwN#{2lz51|iavs}Qq+km>Q z0`$AxEifq&51ezezDfH1F`t%&fcI9t$zC!mIYbATFe|}XzQj^6mq8;h^8Y0uIXnSL zw65f-{leRJH1-9@ZF6HxuSR4>xC>6N+`N-nV4D(^2NvoOxK@G7rrYV?P9jt*5@<~H zMu&Y`pzl_e6A=;F<0KVSg3z-$Toz1r;6Jt$Ahj^+v@Bge9Ap}Khel%d0aJKhh7S3( z+b+tdIG_g#!@a0&5%AX#Z>EZ@K=S(B6|l0Ir(fN&YIpiZ>ZHyj2 zEq_AVgKQ}f9t?F>=)4`z5#`PUKH7O_6{9atk<@}j=10%3&UUGtj@MiCgTc5ZKF&D+ zVv^|&fTE|>YJCOq;DMd{ev;_lSWqwy<|L`zyYxd!>(LU!Af`5EhDGa6)_gFx)B|P+ zY~_G=c5d%&O-wx((>&X=W&i7Mh#{ZRHi07g;f3o|mnf_8EFur;feVc3;6(DUG`Df_ z{bu}2U!*oIMnqqT%`0!#$v!x06lW?*#aNh}1|>O=%? zi7f=;?XC6#>d{iR@jq7KvzjHi9&|h&|9r!5vY4sMHykP2klfzfV7ul4RA!W9(t4_1 zzp$|n=YxL@i~^_3N_SW^xH7^6(*R9Ouck}s$oANhw0=Rdu~4rh;>UR{;KTP0g4R%= z1^EWdjq!8^qp=$;+fc|LMmiQ=@)fCHhNBS)_XGlu3r(J@O%mg|+EPvm@5w)bhJ_(h z9*WQHJWWNu^(hjF?Q_FIeu&@iYo;`heMK)q>VNE#wcCE46ZlEj3NiLzIdz|6djSYO zlic$(uj?ByE|LNJ2M=yYI1wtQ7gN6UKuF%#Z7sFh-YG4Q7o|~VvZsAus^qsDqbtSJ z-)58l(P;`8vaWy(IoGCcx;M7@f8e^_sj}zo3iGh$(1FvYWZ-?w(OCycQv>k}D?#l* z9TlizzCRw}1=J~BPIV!Pln@emt66R=zx-YXm5c|#l!z^Sxa*0dLF$<)Gg2Mijyz!#O3nT4iE4bPi#N2eSS z27K3|U`QYbXkyU&4Vevs#K#7*7F%e$juxADGnM)wcBKk3HaPHtE$*rl+U$^1M~9Yv z4?L(x{*=YB9JU6)3(!-i_XXJzN`hYMm!r`+IVUrj)glH<)3JjGo7mbYg%;Z97iG>z z3R^9w4D6pJ=R(tY`rN1fnrff|mGow+1)-CbT>P-_&;_jmqru?wYi^n1Sb=;H{NklBS^_faLTN&Pr`3DU6Qh>-8oc1Dh_#>*) z$T+Y%;_t_O;g7}wZXs@`EYaB_rBa-^GIr!YsYe&aH9RmAMrb;2Tp@BG znXMd?27LE`b#(m?M{A?!x$8@tlg06;dm<_9)}gyUpdhb$2;4d(t-VYZ&FG`~*kHo6 z74n4xd##-DT!!co-wM^(G|hFa9MGmKfFu4~ieSd1RRI|GN4uA|u-^WW?Antd{(2*V zUfdfay=RouC(faX*geS zS(A(|sRgt?G^_ZpnGVpfh%N!oV4ZISI}GTqA6qgCV&n*uYOXR6)s11!0356hzTp7$ z)S!~)@CP8X!q?74`**J;F+2nQMh}n_ZY(7$UIL_AR z9lj^$?8nno)o>;WZXt!#J|i>6KXfxBTNw^Ja?y9X=omGORY(q) z7kOrS3%)}M1=DGw!A~i(aH#r6SlGUTL`DPbK6NNgcCUWzWgXC(tgaDs+NR))M44&M zPY)^5dqG#V7HJx!2o?gij9#5(|72-%@GN8)4YkQv&a3oV?qO&u;`^tO;Qts}GQ zhx-90JKD&61u&e_*sVKlQxH~w%-`Ji;4>P7!l+2xt3+waa+Rv?aBDCbeQk&L8e+;v zjR$+M9L1{J;O)tyg3WG~;g*#tEsSq6r-biT(vl;dzJPhAo8tgi(9UA9^Zc?8PE!>r z7fcTyfl2il#`52-1e_+!mOff5Oc2If8!*?0@?5gJu#Z!*1dc!`YhON!$8m(n0QllB zAcI(z_2Z^csH3DBp94_hK^xK6m8}_X(EXw6^Gd22jGXEcI=l69z3ct1!%;CBE3JAx zA+Y4eY^1piH$MEG8VT9q@Lt>X%6}kj{+M~mkdn86LZ#pR;rjEj-*VfP*4$K==bvn{Lc3#VWFfgj)^>}Sb=Vh z3+-s>Y?nFfOJC5Xpr`IXRdB!#Va$Dgi2?#*5%xy!R}{N zPc4{8ov(!v1?|y_IWi*RFHAiKY%geTN;~k-;(Qnp;Hw_W866MaCo{hSg5!9O3X}0d z)X1A*Sl=kx?(<3 zESZQ~ovL{fD#j)@wqHxN(oJwak#8qIpwnt4eX>4Om(^`K5I2in20^(2_|CnpCd+KU z^UDPRbg4ovjF12k>6TX}%8QgdoevkQsbCmY=#=SRP3_gMu^356XPzUf^+ueJ+O(iO z^KAeEeqR(td=iVPN@liBuGpDS(lcIIK%|k$Q%J`_#`?g4<$_r!rBTNvug7rQLFr`L z3cv0TstihqMw9(_1wXx>@E6}m`1!?|y8tw=1E^2f7ymicds4Q0kKa`VH3b~Ax9#`D z8Web=*jU+Yp*}FjYc5f~ZUI-QE?A^gPEJG=OJ?3^$AgzzHtA!~fR^wUizWtW_(_aq z0i+;3ju$CXJAvsFGLO3(KFDIAyc`KLIA~L%3d5~_tagmvr*5GYN>Ya!%lB~oAXXUf z4gW*L{6fA+;6sMvray6e`;cP@@*nsJ^T6VD#6oY0P{m=uR#m+XumIv@EywF|?DBAQ zGBL5j@!TB8z9?2bV&ieWPk2P^$P?3BG|6lOjWwqFLrIgB3(fc(HUdD+BgUjs_~?AP zCF&M^ERcTno8|-a4UliE*m6Qs>ayZuIC0)#MnW4qDnxU7Z)60YRdqh)j}IC5fn@@e z$9qk;b$m-$=2}O>YfnEep)b&jFlMz48>67 zn?LgaT@S%?>oW$QqXv3=uCKqV>Yi`EuQQ*n=$LQUsucdN--R@U5eXzt3=>(Iqr~(l z2_HlF2CmAEziW$aTU=jT&eWPG&)mhmSU&~)Q=Cutdo0#Y>7V05Ck)FGtMUY+h1#cE z&71oC+oqx=q&5n*$+Z1Km&uib+D5`Kup#xj4Q-Sjjl$T_Q92Y-<_3pSIF8N53yK^b z{i%OT`yeQ8-25s~VD5hzd)+#Lc^m(_XaWC9aFQ~0!&5E-xpXw^l55BG^({y0C5Mk* z@4^25&mnnsOax|WKmSf6ow_d3LDQ(5C;^YVeeV36G-t)lysQCC3_07iEzAbzwIUE2*^C#+uT70R8UlQw-X#pBQskFE1`iC#R>b>wR|6sbqQ5dY1wcjpwn% z4}o8Uk56D!wO0>*e8z|Vf2g|3uqv~zEgg!|DBT@`G)Q+yBi-Fy(jwiZlG5GX4bt7+ zC4w|aeEZD2&dfI#*UYbhbM~{Jz1O;Hku_5V?)2Fu?KF(t;jDw*os>)}imdxZJpJ$9 z!jBUoQa8PS|ei{1Usl+kEKs93U!G%OXDkH=mr$|RO8xLCBaNtWf0Wj2$$3^=bx0zaonh;*95G^eSdgn>Q&yudETKGP z$9s{&E91f7(!A99o@D&$BI(aIf+d5zOj^!6&-{Q*HYiBd5+WlmS;>^U2aYjHh`nK3 znJpxR9fbyBT3z-AXT0QBmYn-$V0)~ULekCU%K7X3kQET!EQb!<3|SArFI+2@eM_&L zkC@)9)WXD?B8Nf$r?)|^{|dt6OkrXD!+rU%Qb16~GOKz4GM2_W6*v@z0$dd=?2Fqm zLxK!pjN$zXhzkLflVnb?^Ecg2Bv1U3k@Bfh0ujY9g)+I~09j`8V$9N7?Uzu?>lE3p zYXjWp-?K$g(~iiFELGV;sOLt(8f?>PN0EPna4^Z#KXiaIHcBTf5II$>_q3~ zSDCN2|0{`~K(Bypv;H>m92TmL+FO=-WJ|gzIbb*C~TwZ}1ehfmODzSq7 z$3Ix2z!K;ng7HSep`p?-!C`w~-G;!Fu0sIS8b+|J6{o;*Skeb6a^N)c%rm!6KnXWC#!x8F zr~3C|f-|WeAW&!=DX-w5`M`G^0vI8LUCT* z>Jb2I!nXuz0Hm10MF5m%!J!lw1S%UCohC(`9v05wyMFD>H98k<4I~T(may8cXoX@k z9F&DKDlhbJESjE4>QrxjQH}*|FDD>;L|Q12 znYjnTe~5w6$g_Yhrm%X;`9f=M6+@}w|^o>1-Kdo zBy7z+&)cIL5L(tOidfv$%9)fKN>P!Z%aQbhhP#`x7y|V{v#dauhz*WV<~qf~`9j`? zD6k{hpfjJpz0f*ysWh@3jrk`i;sgcU2&;BE?-9F01&k;(YpN~4RckO^mi&BzmMJM% z`OFEQ(@p?HubE-Q0EmBr$sxEtgrZiceH9U+v7W*h{?(3n9FpN4P+jl^PvJTkc!8JV z{w&uM*iO)~tflx0@S=~!mH;PgGz6yx+9c7U@V(EjR6c=}OV4KVqCGIG0aBbC@Vw-D z1Cc;uArbrrP}jAT7QA+ELPck&D3tBmx7qcJ0g-c$$uA+!r@xNhwOPnr;^F?95furd zDgv7C103)WI^Tba1coWFrJS{y)GM>WQk^0T1!=0H^T2#+2Q{P-h;vIrjV(`miK~$t z5?N#ZYHu+?Ugh}jej9SaqW;61fkFq;H`Xc_QNm42Q5|?_gJV>)v__}JBS5oz?Z

|gU*%X|!Rar0Ib^xwWt zu^^{4BL`7`E*~8+|0M@W!5?Jm%%(#$8wrPqJ@AfBC^V;q$p9iLaEj&!O1kgai`}-MC*z<*SjSUrArBU+Z*3sx=ZBdW|QXXBrd(1L%5)yz1@*t8u^v3?G03pu_ zFpNM(bcNN)*G5%ty~vJ7m?0@xlU=YPGFzfK3#@#fFqSGey^+|;`hokNS15?`wQ|`j z6wPsYVv*c>T@0~sALxT&CvZ_BI)buJvYe7B@-d+U`md)1j_qa;NYaIexlgu-Sgct$ z5(*4WkVrkvq;kQJ@Ty~oAd&>QfZ+%*d{WDi36ge2gx`bYAj9P!MJh!mU8*O!zh+gs zK*H0Xa|^h`!@)%`4N94i57(-@n ztfh4_>wNFr4;t>=zO0XSq&Q5Bxqn1@km{8HT)5vf9Vqc%L6y{I$eokX&MyGfvnI1I zwi~R|@-&?(sQ4kVRRH4(8BWg3paWYx7guDBf$Pm*4Fk9&z{dhU%3wUG+HW#cI}kA1 zqUYK2d9NxSg{I*MvwWnT)HSz|IBh&@_J3=07Y2HApAv+bZZs$c zpFL&H8<0O}C5quCRJKm(0VB;8!Ks1BPb3B+P#B7m6E9`sC?npAPh|Gtz-t%_U{JtxhM;IGKt-T;bU3lUe*f== z^1FuEkaV|R&04}jFbYTu-i z_^jps-FCPUgmZ9zpPm0(%wR_FrT1bD+T5tVxj&zJ(?v61=#BykITKJ^O&7SoFhe~N zhX!{Vgb`RQqyuqK0(&QYqMelge!)MHa&WD`ZqGG~lJ{x>`wc1i@{BA2*9t);{a*OJ zg2(iOu#868v`!#8Q5@(tz_A4kl%j|#0wt_*z%Kje2?tAq&0#N@T#+d%5XWKxw12D+ zg4uQ1`SZn){EG;1;we?KX<*Lxqd;1A>oZ5Qv`7ea0wGCAClVPn2LQvg8Azi?_RsGG zYk21m)=!sS*bdJg5=fQIi^4I^;MqR|K8cdXn|iqz-}IJ52G!B*av^bG`79pGNO+IR z4gG+r=l_RE0B?dT&^>HEhjMWH0b!qel-7;@Q%Et)E_@S&8OP(nh4qi|MJ50T=s6+* zq7?WyB`A|4?YWRP<#xDoD~XqC$E(|R%T1%!bDd7~IOY>nYimfg4s zOr<*n7)ieA$o_NDP*S~1i3U7?Muu=vsnKBKFvXh=ng2Wy#Op)|es%*db9`WKe{rA! z@vw&PWOG1lFJa|IMLT0_JM?0l^IXA5fcYHu;4$9|MGnw--T=On>fRUw03OD3tH3Km zqmUT@Y#YQ2edYguF+o)x$X^zjJ_kXX+z$%!w!WICRbUZhT4+>G_ox2?Q2`JnA_utg z*h$QQQ-fw67fXOI2*mR!TWOW+`CaUdm6w8aAxK!C)il;}CT*A&$PVj|AqxV!24(R7 z!r1=zD}ktk4G?uOR)+ye9RPK;C~LMzPY8(Or0s<2`fN3gN69c<*tZvP$E+##7aU6oRZe&su$g3*y{bNE$%>Vj09;lILW^W z1qJ#+Rx<8jeI1b?X_gXPq^I+y>HR=KCeo#KZp&)F(GAHFDbTF5!Pgj&|IfxkJpu0` z%JJ#5;FW5JevoX$0KB(2B}?CiLxz1oLY-(4$50BF5~#eXAdwI&uNr~&H7T|P${zl8 zhyd}o6;+^us1*u&Xt9_qe~m>A$_>lM+>2w)9%-yMz_Mh{acT$KdGH3|pG8IW1bffl zc9<;n7XZQ_pm#0p$CZ-+>)U|$>99b1@g=q-g#*Cebmv;u(VuzTePD&&43u&|+fcd%OTVMZaHC?gjBQojiQzlt#8HF)qF zZPaqq7V=B?fX=lxgZ%d3?#Eqe8SaNiAov7oh7VQ_g0i@U$3?q=HfS{fUm66gF`9G{ zC!lhJUqI^@WrI|bF=Tu+Yg#)q14U~y0z=zvBucVy}wr%McsYd;_`0+1cA ziaGBX)GxTd&BC4~F=(=nUUW2w?O9q>5&aK{`TGJ`AqS19Y?98Nw5`xom;u-`bymQj z*k&Gp&*dlu3MGm-o3AYM$-y#TtLCnheeDK#4=?~Zk1jnyzSCdyVAYM_HR?W5C}-}h zBo{$T<~p!cV7kOB9Ia!2C=*7 zZeCuf_{-(Ytn6-|dqxp|D1=H1|zU!^`}XzDDB@3Dw3JYwdx zs3vk{4NKvA$;iGoFn*csK1j{T(SQ6fZ7B*=i*LR5>)%HZJM?RjonKUXxP3hu@R^M$ z+GzwYp5%uJPVjfyWUq^9Kio!PU_s-G{3mHDwwsm)*ZM2_3-;MFxkw^%)3=7D_;V*wWDTq=7-$ zD(aX#dJ7*UjdjL$j=0J7s8N!_E%e;>4pzByOOIt&xYf++5){zUfcqK*5)|vWV=M+$nb2gWZ3?Nxc<{R1q#^`H*)7!qQgC!3`Lna- z?8^$n2rE{(yy^S5a%ms3RZEf-0gwyHD+GDu-8JTbzE8&(JM`Bf1E1m3O7|`s;nSYS zCdi7}wtDi_b)_SW-3j_Uwy4aMXEPzlzc+O8DVc|y%R_uY0II7~^L=WS=8X-}Xi>Ys-@7C&n znJF+kr8e+cskOa(SgP*#sNTB}{1gdF2zBPK|Mug=sJ!4H`l(mKX1@xatud$`FHJ-@ zdoSQ~8h>%-Yu2S1QMvCBjNLy9vx)h1+__hNf30J_bisBUOy;x~r|IP{{n57#eGU}e zZG>7Tn#K-4%-lq4aC;Zqmx4dDuVW#mxwblhb2WDLGt=i!VrtLXpsPFrxcu`&3Bbc@ zI}_adU(gI>k=n}uoYHOQK2;?E7XhJ0iq)&EXnLXtRI&qaptW1XP$ZKXwV!=3y841T z#*!$F@^~ZmW9+^2W*La8=RF@2Ry|o0W2R)CpM4Ljymtpwk{}+KIa4L_CzE2g7MaET zVRsAM(_0I*TQ=D0?3%#3;dE2j@lQ{;h&C1SR&@Rx+^XRjf@_ZpJWn}`3K>If;mAb< zp`db_@Sr63fByB4O(6KsyBtwLMQ&KwXJfWyA6;8HHpzkS-GIR7%rK?lc**hMs+H%s zxaw+wTmbriKWpBw)Hblck}_EjK@V*N5>#@#^=p63=Qzz*#sIOpu88H*{OJQiUm^R4`Nx(6+fg$-p2^|!V)(Fjlc+EdcGa>$FhO0cwi*#$o1e_9bf2VDZ!)RTYPJ)E1QCJl&CFAiUT}qkhZFz{ zC4vN*QjmD?#vpFRN53mDK}a-IVxPsBLbx}a#9r$1CK49a*;TD28oVS$=0Z*6{vHnT z)4N;&&sBBY#q&L1Aw@s=-w!7mR4AUe*0X?kxm51yfCTYiRKZY8IumRfm4NJwyXIBtLlq*!gb+jvH$-fJ_@bF>Kh z)O(@MIp@VhhDw_wTr#aJOtv&MPz%IEL?iP-qrYPQ{C)62t_6KJ0#PtuKFWT1-SMNH zQX*QwV!D8i;=Id-Gmx=qD;a^Oh6i*$4UQ(s7RV=C?a-7>xtJ9(?M=sbLeXgWUnV#8&oqV_QDc7-ir!h1`r^;ktCjyTGNGu>q;R0~tpw-Sx_`?44qZRa| z2c<=odYKyl%3$6?xzR|h%E43-UBlUyb86A|7VZy@wdD-?%m zM0-J?|JM^#UmEH(w3A7Jf*A`o3yGcq6><S5K2L)G! z^i$jCKPMV*KKb*wm``ySmv}C}f9B$W64@%&X;9o5 zO{SBZ^2JQ}g8B;!MCzFmarcS;yBP^(+sS$&E*i@TSk0n&Jc`GLxb%ele$=! zTwP0~yj56OK)c-JY)Br2undzz`)9BDTB4m$+iy1CGCCbBsgD%ZiBT+<*e(<|couF8 z&+~q62LS~#Bfom z&gZeUg~eowy216TwAyA_1J^vBD67Ry_n++@?dynkf@r^4>P>tEa?6PgvIH`OCR&BX zZ%&KKQVyl;STm%aDDYzRMkF{}&PagR&1~H^@8VMQ!uQ=3RN_esR#^4GdO-#$ldElJ zb(E%&xm-|rla(+8$orC}i;M=kLp0EK$@b@~^5VylxrjVZ{FI`SKaxSA6aVi=j%00S zqt~|W-sE)K6mS{Mto2V1Y3eSTM$ZmL`3JwFb#3P-4bI3!vyM;RcHMK}MrsP8Hal5wK%2fxc-T8Mk;Q0&Yt@qJwG*9yaxPbqK2jP!9Sb3-% zz@|H_wL=xme)IEYV@;5slOU#sCFFG@j-z@P4Gsa2Rgq&1B=eX;AE6`r{#|1ML^L}d z`%M6;LqaUo>ugf1Z5C(f)#fDDw2_K6DmsJlK^S2I(6)U=rn4%Qdy<9=Gp)=C1XL4s zk55AS4-al}*y2s>Zt~KzqjK|?T0V$_o`D1su#yvVQvJ`{LcTT+yCk&)iC3()nG`b+ ziDK*$i;RUu!tl79-u`hU^du}XZMo61C*UbW_FAA(Q>cAwOchD>SAHjF!wN?u3zQz|QbK3}xpPV@Y@!3o zEOy`ZB058k3aB}cQAzE#PmT9ia;j+F@VWd@>rdzRK}-!pRJV)h^bow<9YC-O-6$qFJwaf3niPNL~K zLAJxR#pV8&mS|rhSkxY7$6MC>B*yK*&m1N-Kp8GX{&MCPF z*`v|pTxJfKgrZNx{DI&g?hCL&gem{f>sQ-tyaO;fbB?FwOevW)YvUp4{q0Jj99V7S z*2>x*SYmX7s!5|hfBw8RlDc@bJB0M_DhimP*>Tx#0tQME6fZZwkK2qx1MgXE`KoOQ z(r{oQIlksaG%v1>SL48_9toiJLZpmG0(FEKWZZ+xAX?flS;WH!Xz%^;UgvFM$4;FmLR7b)no-I0WfEOAK$wh}8K z@KuT&Pep)>o5jVQ#$wmv{K{+H=$WxEWJSIv18b!;52E{+#%rv&@cw&Zcp=sY)H5r1 zv58qAjkLnROZ-BoIC|rFDFjU-u$C`|Q)9U(R;XNR^~`o2UwExDaSa5?RU3VemkfM{ z$B0HI<}?)Ij*X5;36))0W3`|R`gd9oIg$VpaN(;E2;cpC>yWSEK;_|Z$fLl8btA>) zNq|bs9ZRx(A4!YTM+bMNOx&+a@I5jb9xA(oXPgrJGeVh7c?A1i^Oe*mwE3*I6KaI~ z9^?S5jsp;W%Lh}>|99?@tW9n7YN4DR0|{j~0l%Qbc7K<&7&N2s+R*;wM7BOmfp$|Z zi0A7w?me=A`Bbg|IHdk~t}sZ#4`hBJ^RkOYzM@;*8MUoLsv<+T!}J4(`alNWxRcXuX>+x*(z~qbHUOJ4fV;&UF+FDa-)-gl8{JOI-VkuwpvA^} z#q0D1lSXyOYv0iXZ2r9%vWY_XH{dJ|V>LvQXwnAZ`nJ+ESnPl^q&1|Ji3T^tLW!-5 z2y`U5FET$0h;>3-?-~S+549a9I^U@1pJ%bA3Y+q736dkMT&AOai-K?DVmvR=K8=Jw zS0}enYeNAMpaTTI;nqpW>yC*vybUZl`bE{K(LwlE^mj#B3}p4T56?+pF(QV2!+ zP6p$>eov=dU(T$Oyv~-)^Pa?QeBOBvb#JaUHAN+B99;dUT(CA4&bLAlf6bgaLFm3uPq&Klm>lSq*^xC{QU%ObUujfQEYtf84{H1gjO$9&WB= z+7vYR`RROQt#X$vm}EZHm*+nc;iN@2_G2;`eZ~OQdAHADC-yvsfEJVc>51 zpGz7QDWDO==x<4a6bYcFnpdKIUx3@KfA$*PP8+(34LZXsd4|QX_Zh_8bNH^?k8HGT z`x}`^I5mhkv0GDjr$t93fCjR{I15^TN-)zz9y>MS&r1QyX<|F*3|bq=Aqv1i@^nvJ zQ7`spXd;FvVki!hG4Y;e0JHV43kQ*BGA_*DQN7j{K1bWG^jP)?!Qfz2i18Iiz1Epz zRS*P^_N!tz#rWHeB*)l)uKr_jV&xu)4_y~Ps8JGBiw!Zr;TX@BE?4Rg zVl!&+?s{*^SR)cM6d5#Bo=$((9bjRw9@vS4I37TDC<}`Ps1kI2rxo0M+$jCr9e2KG zBW)Ukhkz;g_6&QiSn^LF<1az1+#LZDTk*(xGRVZF&`NyU#h}xq+6Ri#*{ze}>D4Ec zn87dyRByntP^F89<$Cj0^?!opO?)CMAJuZ6!ue;r3%o`O2(4ETRZ>WMyi8tv&nJ=e z+OPd$#e&G|JjU}^4-RDd3#fr&jFg!C#=t(aNzj-sl1%r$&v742d`|Q(@A(lNPaMK? zqL}RdSgyOf5xY*p<;H=VD%0)gq_D~sigJr-TJzb`)WzSwWYW1sJ?7m?A?6r#E(ar! zN%$301*%?uv-cDvQwQBzE7S{k-+++6t%Hc<=R|`REjUT+7Svthk+Iwt2Y>x`A!%4` zP;wfLq|Cnx>}LLV$NNsA+11zE0|{aO*Ee}XAl{P2XlTz#;0G6RFdhVBt3I9^clw=- z%ngc`tRG9Q+7A!EZ6R5>%4G_ZX?f3FbTK*nBZhu5)jRG+Vlrt-0AT|y$Y`(p@K`#h z4@@28I(M)Wc|Jbx@x7z_tU~Z^o?=FqRT*Ram{@IcmAf=+SOaOi5~1-l#+y3~=r?h4 z{!6A3_!9PHdx(#QD*>B#|M%q%71st3z9WEdzb_gy>ImnN8EMd&+IpA z7*#!wT?qMIWClLdSWEe)wOkA5H?Aoy*+njvtH<@CUrfpw0|sn_`I|g`57>O!5;3He zZZ{5qaWbZkB;S&=4#8_B%hPG|Wlue;mtX(uV46-6Fv?;qPUy;#_E$UU^?vT}>EFM*JYEE} z({=_F-OnYdo$TW1^VGt$q}fE*wUkc(}07^>5)HyHRBB@>N4< zv@`3$^(M2!p!Twmp4ujW*4xnmQ3vj9JE1qJFyIRD|J5mS(9f|rOh(ff0M8h&{rfUG zAI{YlDg=+MNG0KDv9^C4gNj11GXTElO}bEO82;LuzFS{nF<*O6y~!TAEWjn zgimiQ?X`KzrqGe+7#CP1)G-4*!lAS~PuMpSO&Q-^&H<8+}eOiZ7_+-6X z9Yiw^MhtTX61K;?#4Fv#g`zH(p5SHEV2z~f78X51^4XW~OV*F?fH;U0$n;`u-hPk* zkkWFRu)qeqCw3?%Ju%jW7mN9TvMn~=Oj(?{QZ6E?*4lUsg|zB^CeQ8H3*90?iL`I# zwh-VqIcWu*Ozc^Kd_kpq;vCDOc1xRt55Al zZ*Bx$w$9PQMAmy-^P@h}Cb4N+2a&f_c?1vN-{MUk;-i)ZVD zx+Cv%qAWePPV^z2q-P$f?p(S4wx*Yy(Ljpm?fJp}cp4sdcF1PmAa_eHs-SgQ)MA?g zr{KS5Eh02jP$(Xt8lr!!dKzMHq>bHSaM-Lqh@Mg-1Yi@ry}1-V0CZqZK;TB3+b^M{ z8Z&*^U{K=@fAi6f5@lTDJSFJG_D{`sz<*u*Ef)+X+? ze&=_u;dv&V?D9)@aS)qZFwSDWJyVj*(l;oLGyeEJbh?Nli;;NTGYpUa!A;WP(3$pq zhQCZIUss>8Kj*k$*3;bKl$@ud4uXVlxL>_>2tZ~LCI`Tfq5_UO7)Tex#vLh)@Zx1U zlpH%|4e{s~b`@*}HNDP}it@8=b{!mB(2cP|Q$KTCZh<4WIiKm_7xAk&sCd!=Wjz(V z-gr$!3x|x&x-rUsyDH?REA&jQ)uP5|_KHzsaA(+PIuxsyoD<;G4)+Vr>Wyv{PJ2u9 z&939UmLbc6Ft^E9Mz8MG!5ePWS9?_HI%-4nzO@+yMo!|jmDKxPS?-+gU$~PyD2k#j zin0d`+E#io)Ev&R8Nf!Clqz@xD|U@SYHNT`5yzc#C+|ij!hl%k#vAXpTsqYfWd*+J z^I{Ja+`?TcUas76y1^P57Fcx1w`7DBIqbY?!%}`v3wVsCwzr)8|9Ps|P(dMhyUsXe zAP1$=VOT>lp3}Y@e!mbwbE7xpxRpccTE#V|0*}XYPQIb2vkPB4-ZQz)_lDr~u!5}7 zeo)nR9g1JK#vzB*a_M_89^6x`ShTLdeh@ZkH0t*;R&f~q-l8Ol$)RGa7&cxykXq43J?GW7@Y&Mi)n=bN|=yCt0*8)@3!vmnM_rOsfgzE&?&m& z8C@E!>qAI0(N`(DDvb(_-V;`2TpQL;f7DgxalT%VR#t5}IJ0H>Of`jHx+o zVn5i-XO=!t0~GN+8e3BV{A80YzvGJk>P{RHlXk2Z!VAexk&xynQP=^H0YYtaC!KA5 zsCpWrI_B5j@n3BE#0UC01_uC2NgU5VULIWF@dmaXE0%xZ6oI1WUZ}M_KC1ZzS{KPg z>Wn~L(?n-H4lpJ4laxQFt|UM#joZHovN-lS?dxL;T}+#m0~c6-9$)0+VOg7uu7o+4 z)A^i_+H9GUIJ=D+(+6iLAcR2X??I84fA!j}oYiJx>fByOcM3#<9@byNZR#= z%O619-wp&TWDWl;u|5OCXV=eBg{>B;8sjXFem4Fd{uqil&)S06+mto~AR1b5C7%XM zNzK$;Xd}IIlMm&6F`w*EA*PU)~#uBzfdEl*7$8UzTUU7&rdrk^Y@MUYwM~kQXfZ;iiQxK(;NuyW0 zn=MrkzC2ns#C{rlmtU;E`kFvwVZeFV{E^#{l18gKKWbUKh~E;;NYwcntHF=ibV^jp zX?SwA2#wTCo5wY_p>a$QSC*GS{ACJro%QsNgm2+64hWBj)kW6YxB{G(D z8W7k`iD!R*KMbR^3^WGxVgm_SEi$q&)7P& z>s%sO6~~=7wrExY#|xCmKleouzDh?~McD)+NOC2u)pKKLOEg&DA4l8t+AY-HU~`}r zoT|vA=Qg2!T+`h`IszsgMJdrNpbW$HOZtCLhyf}n7!Q%_T_GX5wlhbg)7MJEu9do7 zco&|hdy$9W_RI+j)n3`>nl05yu(Fs6!HjK%xx@C!^m=Gu-mO~5_=qrUU;xQ|mLC=m z!)9W1+!^NBfWJMJTWYsqaIr3lc6=<%A)bIn!%3bxXEh#Bj^%Tvty&X$$*p16Yn9Wl z7sM{RP|a7`1|Ei3q4;y6Q+t_aw_g#qzEss+LC>Y0YOm=|oOlNNrA>L{hwK5xH*11H zX`jp@(Hy0qq#9Ln{j6$*sN(sR z8&Fw>Un8Mmj+;j$v!5|@tZu+Rr0Kn@J=%;SlYW4016=UD1HWhu#m^{eFR<`-d4euz*?r%0fi zr*TPj!RL_-Wa{60XkZyv^*f5!z+~rihZ1N;;`9GXFU8&SM|uh~Sk=XE--RxzCxHw? zioK)L^4uRpSxI3TI1LGl{fEKi8X5CZp@SxaMNk@ad~M<(414Y6hUQjqL&s~uTadv5 z&r1nuESV=SrI*@2JZmjg9%nZfDyAVDxvw<>ikP=_P?rM2H2inRSrIMcQm9ioc%Yy&h}g ze;M#|zD1K!`({3S@Ot+0#|niC>sxsEe6QP!EBi0H<}`nO#>E;X@t4_Oc47ZpF1Kq! zKS$%BwB+tlX5)YQvvYZxDb{CN@%Cb$A+Cm0BtoOc=zPxJB^)uI*&F4zhbikYfZ?fl zRXz2CqGZB9+Lhj@7<-CYvw$*x!dHq;wM(c~rXSrS@(n9yO~ZVVN+~%=J%|MZni_94 ziN9~w-LX?z^iQU6yULkAs7T*z;$KcdYs@l zSyBFRe;uwQyGybB5g|+p)3o*RU9GuPbY3->1~2_)`GWsjnBk%Y_fNh@U#Sk8jffDUWe@w>vD~8JP9m109gS_YCPUw6cg@_KX>%>^z!mVZb<-FJ=e)x zmi+yQR)g$U^Tog`?^F#oStIpU+cQg~Md+QkQTg#GNs6-?D51Zs1ceLC;=@NWjDgs= z#59dZnpGQa*Y`bIPsC%KsLEGbEV}R5ExG1iIkyeJKLnorhmCuL94prvMf~w&i2f!F z8g5ew;IBUkNanDWdWFTAc6VFEs8tKb@z4zU`C~tY zK=nN%;ufhj3mpa8w=KF7RC#6LDgDkX;1H?6#fIIpT%2LB{XG$FM{gl-fKfoF`V>5z zcnOo`d9RbT))d9bwl5V1_q(@n#zSssE<3;T^E!8A@gn+ZUBY>s4!{J&JKEP$+4aAC zs$YBHvzUEr=Nwa70O3@|9i5*{(2gY;i0xtU*J9r2C^Z5Mk9L!F;VFtx#};UgiW#pT zXRfFRax;xA4Y7DjrlN*%=b3f9c)aV~Iz)_#QA)Lgp2t$z@)5b|^Kyeh99((R(ei=V z3OLis;WlICF;T%!Kws`O1E6yCtjs{`?4Tn_V$Y$m?=2Lx<=+O7!s8SuYhytrPqJ5xnV$bWD7V`&)Y^OmAsIT{1uII$ner205SC20l5r4s)lNEW)RtlHps4PhkUCw0eJ}f?n!S~ zgToGFq8^<0YXE}&(eLor~S82X!0bWkHyzkTACEA^#s)m zXmA18>C-l%wKWZf6(-dG+LSKi^U^lz#-~IIVDvMIz>{9$)P=a1gw^K<{@Uuwf+s}_@J!H1(d|A+Ml#7_lyYhG#?|C`RMkMR zEIGbGA=C5X@AdpfezF<#!Wd+Q`4L&|lIghs;jeuNXfqSZB26A3QPy5}ZAYUgUTO?b zL;q<*NF<57dm;cQA_+k%r4!4dw+J_gP-Ahzy_)#_bZdA|>KYs&78P@Su-;GGF8I7)Z9dOM2rSHrLrK3i$`CWE&Rl6xv;DAV? z>-p=G=jFU9UjXXe61wDxElAbCe@l@T_0MiXL4@q47kq$hi7S{im8iw%T6*#-PXL-k zdTn94NU_w3W*%7CvH>7wB)3D}0`MHo&QC9xgac7rW6|tilCLen9!wRJe0^ga3xck= zw*?evjsOw(fK1dX8UyC5-F^`V87^LkgsSnuC8!4X&kXuOyxU$pJ|{I~!`NX%&+!8F zTvW>252|3UG#P3knMkl6tm(_SUFEm~KxFU?+NHYSQzwoCeup@s49s7OrB$JIq^E^h zY4cg_I|S2HsZbw*laPAV^DF{L)Mfdu*I;~p_pZ(WK>jHU8ANn)y%_RcLaPhRu0kfw zfN2Z_IPzymcnffG=BQN|_>E)$^|-&IK^K-akb|2zl5v)RkPO8NgPyhKt(Hg>us=X3 zNg{AV_3LR@W1>KJUnQaF%0Sx)tbMpBD0ttL=x*nAv;EoTbyjg@EE;gMhvuyzUGiG@porkY$GgmfNn(;IoE2O_6gF#Y zsH|f;&2IXn#3hIRzP`X4j41MA^$2+RSnM}M8k`RkugZvjY~n7|$~6A+vwR6qs>gFE zdl+aO9fWm?^1|@ep1;hK6^4DD=V5KD0E>Ua-_;vnYY%T;Od%DI$iD09B^Vrr_fidn z*yTu8?I_lGT(}ZY>54-W3hUJ+`dY8nMnJUwqe00${(E40;*aGzIcz4)pOKTBDH82J zVNds-%YlsqAvIi%7qPelew~7$!qBo29YJEA_aJ(1WwH}NK{E=qEmP3S=Q1Ssu^b{- z@TEidwPV)rgoU^>U-5N6{W5Ln%5TseY@#3Da|#FrneNw_Twub%2DAD2v4gC2PXv&T zNdg1m$EgSk8;~=oi)^7HQY1JB5m_0Ho$=hH_t)tA?GIKqDiH%W9F0Mdt8wOqUYfkWwj>s5xMR zex`oz=1chG36biX9BIot_7kWe20Q5M_Mbc~-TZB+o_00MNWZ}xBbm@}TU*peMPmqO zf9>lA3pq$RecKzI51S+M@bZ_VSR5xF*_Dv=z}2aDp?D8V zq50RS+QN2$&xqEiIEu*N5$s_KHNgOh;eDMJPh*GNTB{Xu#KZlvn@P|-Id;nn-Qo8j zW5x1XP%g@@vRVmJy|(DKo6kc22~-&$A4qZd(HlJ7?1;EsmbQF69ICn_8Vi32yZhYV zMl3bDE7jV}TRoH~P^(LsjHDf56T-mUUL+WLq=VLKK90$~eD`_Q&w=ehBtAqE@ucBa z*p*o(*wNQ*0jP8u-L(VHT3?iT3tK_hI6YfvRA*qCuy#2m2>?^ir&sb$zCQ>7iWdwA zd-n2m`)g55go(g0`f->7Au_S>lQ+xVmqNf5+Am`L%^8Tv+;laD(K#wjM}HCw!zeu- zJQ%%~Upm>vKAtLwgUnm4{kgOqV+*|~DZJ!XLfSix$Kh4Z>GLxso?dloON)TVS*iuQ z)xli#+U^UrjuzPzATCyNid9OIy2Ecif$3ySZcI+=S!mBe-GMS_nVfCT4~4u^+)r-u(Pa9YVvCS=xPe6_trzTeg@5{=Qv z9Qe+XCXfbU2exPajxSXL+%W}#a~8HaqpAj6IAhF)KdXRSZ->Mw0LLEWt{{Gn;xX!N z$HvQ!K&F6I;LL?YhRGzsB{{Ev=+u{4ve%#VCSRbrO4JCny)Jkbff*c)&f$H%jv$!C zu1ICSyr%7Uf$7PkhmUMen?IfeYz+y(GA9A}Ac&~z5sd@2=i}5nkaVXWx|;fMl8iM27gAeKNT%KG_0X5ESL%Y)Jy3wj|x}Kk^wT9VVA~ zr*g$1yED^(#SJ_z)xrDhb2t9QzoLQA@9Njn4l8~1-Sj60bg^oynoT~X;-O@K2y-KX z29a*;I=HPk^&R_@=&z7*7_CT$e|-|=z+MY}-hO5VJTGEXzI(o;=n{v&6&}R|uR#<* zWqi$ixL>*!uoVv<6Nd|wi_*e%d$_&VQ;0N4#p!F8gC>QapeHT_=>-eQ1RV{i`>2>5 zI0k;Uo3d@30s@FKu*fv_hQPg6DLEO-yvwNF_+`CA1aln6EO_Dipy->mx9w`Hwuxmt zQ@7)_o*HGk>K1%=geR`{ixmmc&J+X`TqDvzYrGy5pB09Bs5b}Cw?IH%PJLjjn*(S8 zzGQAY-(LQtSe}Z{v2MDIWso*ep{$Y=$9l`1f8erl&D@_#6*Z(=JxAPZ_s|Sx#VI~F zmn5v8>Qzmsjt@+&u~{7Hv(fp@USd4@bGhS<5lhP+&(`~`-^i| zQ0cN3u*hGG*?tO5$WGmHQ(v#Dvwb9ebwTn*%(3%C?6AbJHIoo+`NL)GM}*76S*Wuo zmDUhOm8+^{E4NkQ1tAwLvsa5BP*=8u=$L1ViM9N0Maes@cF`gT#^cCm0(144U)al8 zxcRn@eK5HlQcq(&dK_W{UCD7wC)}y@is$M;Wdfb?r(B9^^J&FPFiwJ});6vmgKEFD zR~QT&$@=xnudiM0-mXezbyJu4iExrDE|-Y*6}GcxzqUE3*@%jE@yO{jkw;7 zR$64LNRheW=1ZD{sg~S%KA!^Ev&+#E36)$r^X6H%YNmvK2ZF38s=lKSGtmY7n3i@9Ulq4Ml4njJ8zHdQiEZc>pnbUup#C)tSg-~8TR;ngOm0ObRHb5!YTd| zaQ4s};sVA7*1IOj8_MXrLtw8JW8QdHLtWp9|#N}^bZgrf7}vr#Z!mdeY9;)X?? zLKD&`q9Y*1Qxx)fafbWjn<13b!;-spuuQxyYu$(S*qogR;}ATzrIO4!xe`HYw5M!m80{Ri~hYR3y^Yp)mMVfb!nb zyT*Ffrpip{?D$opE?iO(ziz^*v6M72TNN%4adGi^gzr)MQ}xZNN0i^*&pWPcPn`Tt zckbK6i?9+hTKqn4xU@a^l2~Rd{wTi~Yt7P@*4;^r;&8zK#K`C-K$Cd}R8{^#b@`FeoNpuC@F}KO{!4zYm&m zj{~U^*h{U8g3EKM*~#Q;(VF9gf`oG)2+inhV5g?5%_lvT=_(3y^xhhYN)T_qBt>EW zfNJb#X>u4-AxDP2*p z%S)eQ@lZUdu|V1YtzzRMRfO%N`XlCt5Ij`6LgnIbCZTBC*RHEy39#v(m1?(GU}M7s zJ^Q8eV`FD9?c%7EK9d>5Pi_udwjyHzm@G~#BKgbgH}ws8Y+*G_b(8`!UDE+G*zgvF z=Sf6lTV9jBD3yReJpS~;yH(0HB$>^|s5`>n0@DHje*{A)(g}>4n@J=NFi&w-&1t?5 z4racnpDlZ2;VDaWC(jgR$&t$=m6rUVJDmIG;(XS~rrqSGOMXe)S>FTel7M$Hc~2sP6u2@B3#7>2p9Zhc8S_9fJohZ!G#w zYdq}L#EKjWmbnLd>a4yhI4u51B7Pg}K{eK_jDcBi?K3_5sNiSuZ$v}Gj=}9Y&5N3| z-Xyper1nJ6B~o?qsg#A3d@n(~AxOf_p1F0k5hhz);ryd39<;}adl_G8f^pHaThjk)RrlF#$Sz#vI5#zcg`Z0jzE zXt<2te$%&7E{XnY)Fz$9OHLO(d-@gmesl`Vfbg=#>qo_SH>l1rjP4t$!D;qHsKO>rX z&M9I;@GPFUME@Ze-)yBaXKKuhT+SNwJXfy8Y84R*vk`~0GWR#1aTTV4TKrW$Ntjt8 z^>~Jo>HeMVJ4)}!*|nDN_)1~%sCxtoMV`OdP&O2}^&C}x34)?5=sauTgGKqbtIx2O zq(0MFD)x<-wI06sTz0n`Ww(#sBRe%a^7ex)&coZT;)i<-4xLs{bKVcA&>yzCB{6nx zg@kqAWXqDNPm5TMPZ+0peE9yl1v_{`n2!Mui8HDF_w6S=G`(a(j4v^TSW-uO?x&vQ zYtKOx>np=1D$hq+X=v{P^SMJS%679=ZryW3ndK7F>SW><^EN5BGAx%w1QW z$MHK*-2$SiTX4&{)ol;jE1hY>F!~Jw;Wh`TvnyaQaP=XNOG)YuzWc~I#dY0GQdj1P z%cO>hSR@cmB%aJHKZ2hW4t_1%%6=;1+P8Og-q(fS4ZGwL^9RW!iIJ%s>F$z^tV<^> zhIXa0i-mJrO`QiO{v`Z1rLbP^XSDyW`7n#{oT1!UHpmf=VFf-uq{j=J(U|=0`Z# z!TNY~WVkvSB<~TB$ZEsuK`0v%;c8?5(w0Y3MVu;(*$wV}2dwq`)zH2wW@R#&pj*k$ z%|#lNRl`sr(KvmtlP#W%z1cz=nYqO*;MpVg7~)JFdLu%zauajH(1tG7ku~1T7VfeD z$LT^nksWLn1I7k7H%inIbm3}H2?S+Lp#L}NDZw@!3`JxW$Um`O}%3) zm9Q&KC*!($+663}{k(jIhPQ%Q65V;GHI|M^Uv4)fg~*5p=nZs60P)44NuASLEiAMQ zPsQhU>$k?6B%s_lH7)aIdvT9q5Z@># zU%nd@b=7&S^E49oZboE!I*-W**4j#0%!IVGva`1+IF&EO(yEoLw(LIM0OGh3H9P2l zcaJ#1l?rxo<4XHp9?TXStFd{}14!S|;uo`OJ5y=ja(`^Gpil5 zkENGZBljr?JUg_CW>YMlIHIUfsQ6&8xgnR+Co0Rb`209(XH)YF`XnU}3DZL*;q1)K zk9j_{0H@sm*ufai#sg7?#P|fHVQCa1w>pV0f`O_mC}~jCytYV%KfsW|f)OVQ;3K}$ zgiCc%l1pC>G=A@O!1qKmpo|_ZE9mass$UDR5h+QZD9-3Swl2tslz06Db!qrFeW%jj}G2X#dX(n0&ZY!*+65iNEx zBntmc08d_0MLJjZjuwGly)d!9KfNc~r3Kp%n#cq?B+h+_L9b1I2ju8r6SD}gxq#-e z{ZI3_FgKl@{L1tr?@CM*BgzA19F;Iw9rd8LJq~67EE|mk-(BCzaey723{aBH{#K7R zJ$7w38QzNvrIPmfGBxP&EJ353`FVREZai&?OG?mnR5U{3mb_YmX4Di&%GZG>PA^GC zF1gLFc|vM~iwRrGo07vx4QvmvwHM)@Y7fI`#wA)3kb@qD*E2aB`mPgRU%SRASm^t^ zP_(3`dr9uf2HX`zfL$-7qMZ7k$Fs*G$9yrxJ8ALUu1{+q=k#!i4oJCqroIL~Cs_3< zt4cMujJsc1a*~Bu1ZU$jczLOMd*!IFPT7T8mCa z_Qh=r6h2)KBr?z7Hkr$5rP&PjxPgW43piXe$Dv!4>ysRx=oCSJ?|J8~kV#I<{^I&3 zt16vK$8E_3L@bv!d#60DlX-R9C+^HNjPMeGLdq5}LY4-W34OubBqzcCVc6RoHEAqV zUw(!g7`Xkp(GBYPO0P-xQ3?ox-Njuf{fdRq*?BQoEmJH)V_DVJ>Zu-G5GI8Dcya$T zCfR6op%N)c<;pQ_^JkeEadkmOkw{iI_)DzSi2E3XM+r^WR7k)g?YT(eJw`@18$Pg| zEmq<$XjWV?r&`2dh!NjKOz4cHNdcf}Hixdt8-*!2m0hE%uhP9I>!?h(0^F4_ZY-h< zowkyxSeq8X2;sCX&Xa7;Rc>d`jWxn==Drm5kEXV1(bKA!nffp)uvlsGvZi;2_QNCx zMw>?gi*XABd4H}$XSt}}?wIObbvhX&ZV^r`AEugYqd1{*BW~?&V#WLMiL|H9 zAIgo`OR`L@x@h!c7#n`>#^45A+Zb}4ntt3dOe33u^%GWNhMxhzvgc)KE1W!gtZs`Y z%KT|h+Fx$d3b5;>b2a-}k3HLGtvwFA{)St~F{n+wr586Kn!~!FFres3_re{=A!Z$F z8rmx)j;p;G5{y2qajV&lIZ5;h zcMuRq4`y!PJmyMoUOd8^WREwJZ9d-nq{HB`_p$M<5uwK?mSug&*In`_5!iA*r@B#H z-Mur$q*qD&8PgB9){kLNyTTto%Sd|bs4H@hYB{%O=|OW;t*J)jpmaEd4WFHrTVALUldLrfyd{dnwT~`IfCurYg%mKK@Q7R<*|H5kHet13Ylg zA`D3Oxm{;!OG2~f)_>0KO9*(P1@g|4Tm#cA1<|K`g=a1=c8taR?)rK=Oy``bx|pJQ z8|&SNp61DTKFN{x;nR2Mf(T9wKLc*}WAF@8ZG;~(L zn)&6!gT2K*wRIM)#jWX=wYSF~Z$|HQ)0<^wt(rr#TX5;0I5=&?SmVgwnTVz;sbCHM zFs(2&8PYNPq04_wdr_iVS|yt7{N~3Ggc=jNK90+$cNV0YTwQy~v(oHPmt*$Fi=qsR z6;LTb0EV%V!E;cCgujN)_3)#|k=)a~X{TnRTG5hsm=lMowpNACKm=SCV<%#avNWz( zBKICBMJvRrB?e!+X6_9I-6<0VVZ;{sF4kgZGzSOpu$%^`rQM$@OYvzYi6#b4XaPc| z6InU=<;C?Th0yBR@O?jSCq|HmV+xJuW~5Rg36;O4z$wP)hYNPA;aT`wszQbVbylhs zc!f^)sDtoUpic;V8B8!p*AaUPR5*qU27d25$&7)(CHULtn3$gm^OmvHp^FCBL|aj8 zRa}oYzV~h=BnR}IPWve2QYX^I?Vs9k?TEUT(#ZO0u7ZM6wla3Jx4hX3mKAjw=lxv= z8W%LLRN^&!tipg~D-*Lr$$6y=1@Nbg{u?3wue5J)SNOx+gA3l%WEPFd)&={vMDy-CU@0A!K;$N2>r)n?b;!@5Scm>vnq7C{wNaNSuf^ zn5r9tz3pmR`B!nY4k>-eD2JOu^++mXJIPd28l_bu+KaF@x{w<~j3&+oc-(bFmR|

Tbl`&X)+0wU97$#7+6q=pg5>9@`OK=IMgp(18whMTerw7 zZ@s?X(J>wDF}TBIh`t&lEJmScT5fnl_Qk678bX3C7^=UxTzyc3uB5XF4{57Vq`9s- zy8{ug%WnTFrPz^(o|6*N?4(h(QQJ6Ol``TE8F&}|dMbLnNObU}fFN)!5oQ(P{Q#_T zq0{C$jsMgr{fQOuk=@ARF?kt{95q{fCxqZ((Hm14#K5JoB>J++=cSn>}l3(htAm}jtl2cF;c z^&`DB1F9Lrdmtk5E4vnbdH$oV+e1Ru zDEG$FD6T_|^IjLG!@kWKpDUf=PB=2W&wt=dTs;$&MtT;9*kDEd!AP*Pim5+SVWQPB zi8y2(G7~tO3H-u4+KUeX@aQxyiwQ2(_^pP=$1bLF&{fNZJ8$ufd}vEFhs>%ddd&`i z+f4@o?EX*v08PTn53rCQj%avHAxm2bJoLH()D&ipAV!J)&PbT+B2~JV@^F`5pe$`gM=Nxdq2L)550=Y z4_)fIF8Zpp!;aQ4UjcOuqZ?>Bmb1}wR2Na?jCDibjoExG;5B^{?n<;?rUA!?sNvIR zIOtGm)mpV{C$cZJ5r!1`1O5%G>E4`Bk88`ja?pr;7!bf^R9@2-Ngqt3A?CI?09EyY zmF1uTok|%Ig|I_>{G>=HaIH_qqm>q((4|mqH%Wb7 zZI`Jk&ifyleez;F+30lgl^4Xkz5%y#0~xm|Pm)8|JLKI6k%fYCuJVRc7o=&fl`~a7 zIOr*6p0J}XaWzs_53n=9U8~@04s1Q2`xBn8maN?M5O!cjHDg79KPxYaLqJ<^)jixb zjiC@Mtkmr*@5dFM0n9{Aaa{50+Yue;x(_`ivTTUAmR==Q-E7tb<#k4&3Bv!rdkXnS zOcZ)Wly&+wl8CF61uz!f35gI;h%UQZh?LTpZSkYcQA->eI#24>O%`5TwLyh{9L|h;pNJ@|z01l5g z3lG;Tm7ayL-U~O-i#BOx*0N>3%Lm0sOkNqZqE$xe?zc!#>JHSl?GYRXUhrR%rys1# zO{*Hg zz1i{eSPpnuw*m7*e;G>qQ^pkm1^-XSPp9)svZ(-3WL!R4#bg5v-)rSntlw%bE*j&< zQ+1y;-o+hU^u3NPkp>4C?Ba)U8$>s0VJWfQ(YR5oQ!)hG6iHDZLK zXc9cyUDWvgWu6)6RPXev5@a0^{Jwr7C>Lrbfw;gkQ1Q_K*hhOnRdeuZaXH>me8HRq z4B_l`-f!_5o!%u277b4~+yUMq>;vy_=vS;L2IZiVo62a!Mk(l(7BILAU+C3o+T^ZRc2BJT5=RHDwSE=J3~mRTrY zIy>4Qzw5`{)u}KBGC)gTF&e2q?Lk5wHIr04o4*wT-om+v^Dmr(&3 z!*tzqbM5Rzu##~q^ExCy!J^5@_L1H8=_-{^w_m%1x_w*Vc}vs%B}Fc?YAz(3#`St` z(F00Iu(N1dxe?nCfbFmXG^UV=tTmXi?h37KD*Uz-tHaZ=VQ*lzU7a9BO~iFZ8)=?; zxHJ8=UhJ0QIpDEN@)w^wE>r?Mx88zu$u|&m#Ee5i_7-5duUk z+M7CPU$-|H8`+KmLinsECa!fUzDo7a-RM8wozT!xCUi*b(*$~{b>>(HzQB5bv9b?; ze~wRAMy{q+ZqkNFUceW4L)ixr{PQ$Qn?hWd%!QQ1S6;{DfTtka^8fUB5*Z=4VyMYl zfxhWkQz-CADr%;2*>}I+j$59i9-1EezPD{e3^v@i4P!{TGv@cl(m0=1>cy>HWx|0l ziVq|P5b^W#HxlfRx4i-osq_GKh5_{D&PE_1Ho`eeG2K+mw$LISqNpak8!nUl=OOQD z!g>2};|R!#kvVp=wM**NQq3ibMg2TIj3X`_v!}rGTn|B$=iFLy8vgoiBPf z(8*OIqvHTCa=kDk{*uj6cYE%29E*0^Ku@{HSt#L&Zc(u6b!skhQCy*ypTyCo%fxOr z+zoAy?_~d@NF{uG1>aIqmpkrRa`uAG)FJU+8Zh*GcV8lQU$r6&6Q z1J$?0OMv(fX}2eC?%Mc-UWW!=5En|2=(lLp8Q!oys5K;3ee8Ge+FviH@tRoZLgxij zk?QqKOKG2M7mc!RqdJ=|n;$*ozXuv{Fqapy5Jyc0)Z8^1z~FBWTHkf$S66|bCuF;! z9%ZE*Inw^7pw4AS_8O359&Jl{BD&Z^jMI@Edlk{dh+w_T$OMtq%0yr&k~?{EKQDZ3A%;c!QLI)q2hJ@blfj`pa}|U(jD#76U0ac5vccw`_+|{^we9MO8F?8{M z!Q48~{@71O;v$XEx7!|@bW9j_;flIo6{>E5>4ZeBekaOk;aobJ0OFrcYHQ{vymC~grmy5MVoB+x$7_!7>T)co{Oa$ zS}B&qdOUt5R3KnOy;_3S0KID}<;UH5p0{!6M9ut?b(6N&AfMO}H(>#`hN2`StY&)417hcK*x;DP3C$p@s>& zGc_suAVv^OB#)%Xl1p{6vlbAoN|D+P$GdZ}oJz)=2d2Va-sqEHrAPThVvp!)$isGnG)CItbH2BZ*9p)&cVWQv5 zg04}fs3nxh17OZ3wTPO^uU7}WKm;rhz)T3iCN)K}5&))0*Z^gF;$^0x{b1G$3kuwZ z2s+eatb3CfZ0HGK6H^oPV#R~b4s~zuqoEG`p#{>(zY))8{Cyxa@O*uugnsxR(Yo=}3r`+X^AWgi z-F(7FsKWId5u`HAcr+3=n)*HY6yWoufpVAEVCqfkh#Wf4xc5?SyVo9v%9#AHFwCaw z37ck7$k`Wt^9FoZ0C@V{_D9vhMg(mQj<))b9lRAV*r0Rr4D@X>ZCt{c9$X;0!rU=% zEh&DUeU5n-r53JgWl6DN(V@>Fy zxwtcCz>PsUZ|l9bI5|ZGedntRrd7I~qtsW|Pxe5k-gE1{I>3y(i~nhi45;`l3ZBg2 z{8C=+DPc*qMi#k8@ug-Ax2b|?=gM_nNG0IVIgdO6#Ot=U<2$xKYiAEFi{Fq6@#RRn zPiR*WctI7SM+ZP)*{L`XrZWaa%*g_W2WoL`00@2DI7KNy8;gyyup{Grv^TdgkN8e` z@LiHn(60AAZ5`SuUM*2^;ftuOf-#(nxP~^T=;69t{AbgL90@{QhiPEr@E0IP9o`7; zs0ZR$Z4l?tecmp#D~nv(m7b_fSZkKr{KU!55-M}I?^$4BZ;VcM9CtH-$7U0?f@xLy zE>*jiB?j(cZIG>CROGi-0)aY(E^;o45L9YxU#U>gt(^I)EmPc)d4;h9x$&a~(#-K9vH64L+w>v#8=_;HBs0xK9D(ddUP@a%mN&h1({yhLK zKLMMBf{0622?$;lcMT)TsH>8O{NO{f|})Ti5nZUEkGaHa;t#@?(5~3IS^32=!jtyNB zy}Z~Rg%0U>Q{wG8+q0Ojz$fAK6PYuNpW@j7xKC;jUp|JB{g-(`LSO6iOk{dzfv&s# z*{BqVgyPj6%zxKYAj32(KU$jXJDf}fv$}Fm>FZfab{kL9Znb?!aM=7t{tA`_B;3% zae4(tA?r(}57E}R5DX@Py63S3kN3Ek)6rKk!hzYvE}Tg=Hyd3hEQHlpfu&CvGKRS; z8R{5L%Des?lc^h_9i3m!H`53i!+_GCNCuG!8uq>#LzQ8+x;W4AJ^nr)vKeu_7gD!k z)^o^KDo2k1my9}wv5lc%BxGE@=#^CZyN#+5gW=n0RfhggAN;#B zP7bj|_BPzQ0KxaWGyHR~>;%R%8l9ZYzfDoUb~lLXxWQFD>U#$|&%avRuVWe=(4`i| z90vUPxZf|j0^|{^f@e>S#Qa}8-oH+&2-#poJpb5{etjtNo8UEIdwq_SUHZSCl!pj7 zsYJHX!@nn(zdw2|9kj5v)w_=W^`sKnz)6kw`pNx$`+gs{5#_^d&2skQ|I5Xy!^go% zNn}fm{Y9Dm^Bp#3;6CofK7H|DPf8*ioRk{z=IMXEk9(TnK3aM<2>(8jzYqR>xPRX2 ze;@9jH}3yi%U$@~D2alC&ZwcHX#7w6`L`AS?nfC=|82$pw&H)sUq~VwLSDO-R`7XQ SRVE7fr*T79rAEo-$^QUKM|K4O literal 0 HcmV?d00001 diff --git a/doc/images/knowledge/property_resolution.png b/doc/images/knowledge/property_resolution.png new file mode 100644 index 0000000000000000000000000000000000000000..8a16277a5341b3c84365b7cb4d87371a768b60ff GIT binary patch literal 85388 zcmdqJ^;gwh*FFlHO-XmRloBE>-2&1mB_SxGlpx(8oq~W8o0jfwkdp3B>F&<6_Vc{= zecp4v=Px*8>>n6=tj}I^&CiT$UUM#jRh8wi9+5vnKtRBfe<`hwfB-#3KtP;FLjm6; z&@=NPAkZSnOG{`v8|)?{J8R031gQ8TV_|nB6JIQI_%G`g)DSy4v2@H+h1A#Ae@d$n zf@X`qythZ;dtmzdK4#at$o)k42Fh2?ZMN`pxA7v8d!xqLo{kU~0r7u6w)j4ddo#mj zA9|CQt?P%d|??c)Z;g&k+k|>>z*T$pt&kDMeK_sK;fU3Rp{*_*S*>5Y~|z^ zKa(B>(C9)vZ}#|Ss%=tKE^tpJKQRq)qIo0y=OeC)CO(Amhx13PF=Tm+nIX8RyHNc8|2Y!^ngD!l_Oh(m|Hy*?7L=e=NP`2#nIcvF zyKYJWd{daIiu2EbS~%e1Av2Jd5&)?jA;G5qN|gj(tMNI5``2)@)4-f=)zqa&6r z#Q(0-LkHT6CEWJ;uOT3yP5Fb}>y4xt9Ko3em1&1D|B=dD1FW@wV*Lf>pEDsy;CvES z1iG@Y;4{MbcU>V|dkT-v{#C}VBoI&kSvsB&IFlK3lsU>jQnloQwSq+aFaAA~cWNMS zDkIQU1rJ3D^1tiefNv}r{ayc6h9eyikArkLC>j3#310Fb{UgscOXu& z!QbEh8l1h&4AMbMC$PjsijEMX(B-a3m80d3jGE6Y>b_8P$pMHUvF!;vGr@Vs%GHhmhKNE3w_y{U_aB~n3!0epRadn z7CJp-`{Tb6YNw62)pTEh=J>8H;Pr3)AJ5+DY~Dl)i@A(+snA?8hkDZ^l;N_FIMq(| z_P1Op>`s-Acg*RoD*kvz$EcL>J^Hb{&JA=?I~c(|@_P zY;}(@`pZWnbm?HwFmXPX)y*^4I$+*0Ch?3}>= zco4XCr!Ah^zM!9)&Tnu(@?9uxVU|p}B^*BB5~Il3@GWNchy6K0e0Z2HH(UJjl-W%^ zOIgWcG&AixtHzUWoV*W)mNFm~+hPIsBD+{-DmWMZZo~`)40UNpZiCg7GEJ#(34? zq!TNJ&dij$)^1H*(r|ySw)B(PYKM2JPm2?9jkb~h6_TzW8{r*rysb%vp%1`B@=@rS zZC1%>KUG=YzKZhaKPJd8;g#jIBi~vNFNV;BdeEOHi#ZLLO)Xb44??DYgrd3Apa1<+ zG^~Z(xq$|vAEgA56*;WM4;9D!WWB$_X;6tm1+FL#+ytUq!CTLV%okIwV9 z@{$kU#R7a?Ke@BF`nETln5MCM*T#7;zIng?V{H8u*x1qO>oVA zYlQKPl6DXYJfM;7micG+?_05lR~;e5uLqIs-jV<>qO}F0L(IOC)27a?e5h99m^+RR z{q}uxI70*0aK+-)3i-}y=xMj{kO5T|kN%t_=n-%D-Pp(=;NCG`$jj5hbE6N ztA}(%VOqvmJu(d+$Md$c-)~C-mwJW*?7Z`bTt*ng7p7*4zk+~xgYW!sFS0jXE*I=9 zA{qTGok$Qm2HMh65_Vl_3O-9!I^%TdU`a9iIkV|LPSqUT zWwAg^n z@1W)W?n#7{gy^wOsWD?u3@6QU9l*@r`gHQOiZahWP39%51uJUD`ZsQm=QBvXVHv=v zL-C0M7?K3v^|GK54qf=wDo`QU473#2^vmozWnZmrpYyvA|1nIZjWrw25pVU~eqc#c z6kpyed|R5tSD=>-LvOb)wwx|gn2V8iW1%&Y1sduT*dZ;ar{$PO#imq+IaBakXF44$ z3`dIHOA|eLo&MbYa}?nY%M2auiNg=4S6aoo@@|fM>cKDV*86fJRzohR*A<2Akrl;h zpTP~7;p?JM3;C#`Vm()riVXP$2Rl3(;Xb-(?$@WmaUA*z z1kB2rdezpsD@o1YS*2q0HM7PnMi9PZqcymmh47WiWd*s#a~Nm}eJK-~Ec>9S5Jl^Q zEsH=P(J}|#>`%A*N-Qm+!;Af>N2Ck6FM}R zMX2?BgFLs{tL%RFV+Bfq5JFZ-uiGQkweA?XMI^s7THyWwpHl}g)8-DE^(V-TW~*d_ z0Q6Mr8YP@utrVXzDs4i%-0SAtDnZaA;dm~loxDtlIUUh)1CytlG~^wK)7*WUOyS>ND%~U8mcrP1FoUh>phT+MFFQ4c($vgtMM9cAO`pDGR1dXVe)o^iy z(Mv~jJ`=tupN z@4oMTtUdl`}X%%eM( zPCXq&is_8hNON5!ROyPKVpY$4>3D#F7kA%V#AO^Ox zZeynn_8vj_PGU=k*L|88Do(n+*_k1oDN#<9MF?=6bO$ z^E&@crOJvqGBgQZSH_5EP}~p*IPQ{Gnh#6v6^!NR#H_4Vrsk++%rrApn7@R~R#~QT z?Upiselt^Lm2UuWJnyPD7YptXIWQFvW9(;z2P*XmK?m#RiS(g5{=m&aj`~8RW2IUor|x@*xv3YzM*tU>cNq9wo)QU7F(gd$j&l z&Uqe-kr6`%Sz2X-!_NuVF*Ks?sYfPVIb-Kycc(ES_0jm<8m}6eo;h!TY_UN(u-U-} z-i_tTS6$jvBFMx2O|ilkrmjzJr`t-53b7+079~<$(DyN3&r;^5tDSJpjBP|dQ%WEr z76QnVeMaW%(S(+?W)j^mXj1sLg@WIzr`Yr`?>9L*m(OUnvRXAqa`(!W{q|gKxvoi3 zQ4yUr2^L@%E>%F${U+f5F1jy85raMW-u;H+4>tUk2~}4iKA96-#eRe~71MIG`f7FOs)fv!y319?xH%gE7-Bx!W~$F>U1 zn^5RKWD3a>=xUJF-b|&u@M&jQiuiPy84i)qP;MbsZ zVDFn!X+=6_1PRVn%wY(cfN#srWbyc~ruM#_P6PEUPu9KvhzWhUbhOgdqjxaU5eiQ? zPgGM8f_%J9;T3G3W(#gJbfQQuD=X2*355YU1^~HvbHHjcm2Kn$O`PMq$b)&|hWQo2R)NIRJ8u zWy%xu&tf3;O8~zUT-VXcAe_ARK|&p=uo#7d86L}37CU?U(Q#6vg3oA{Z<}G)=UYl3 ze55w(%=zUveqJYLKo^*MSQ%9?dwEXYM%dHm%_Su~1K*)geoVZ^40e3!Vmlh*-Q6eF#EsJSk^VmV|Z z{w5CQ-Yxzo8@c^7?DFmFwe}n8*iEg{0KlS&0#j`@3sjj&f$N=HCX^QUEkq+*ISV|l zAw_(uTlJA@&h$_a*Ot@=06f)RQ>Cpy8qZXJ3=+FnU8LAM7+8LwS_h^ki`MK!j);SU zlc%2bWWLEmaj%nzP4kb1M+O5fT9|kH;AV?l3#Z|Iyy+lJNhXB0Ue~Da&<*#J8c3i` z@h=(Ir?9lr6*-nBg*sF7Il~p3CG3W+oC8@u?B?oZW9-)l4A+6dVvO~{Y#OgyH4?|R zmn(2g-yk@?7G%)qDns=2O{Uw${&2<5^3CioTVDjRNx@r$N16%uBTQ$HC5UxSj8zf!bD{1Eze`Gy8ioQ%}S6X;N0bz56y(Baw=`lvKm zyL~y31*J$9zqEJYrt)PP{hGQ-HYKlbpEnz#dt2@~r|piYbY;k5TcX^m$I6tyE2 zjnA;5Ut0hoe7%bqYn9Tmv$Vv=x@6Ubod~dVZl+_dabd)#9B_ZW0=_Nbj zCpDG1UHi2j@!!m)c(ep0hH%00NFZ^mPR~WRZC& zUyGB-#&X~m8TX__6;npMeOq~NcDT0O%wl68nNY~lQYswBZy0X#Gl>LkdKET{qHD>M z1u}{s7s|5!c7-C6r?iAqOzMo~E6aouX8~wM;nhM)hxRoJ0osTQY46&DbJ_Zz)46cb zqFSDaMoiT?d}3Vx(33<~A>LvO74^85J`?yhcto>-|r`ehPK_o zg2)ry0@5tS6xgpFT;%Ct5|6SX3{@S{OQpBoo2ILoh^w zGtPP`xQ{f`9*+7r zY(~$k+%By1`b(kr2O0cNm-zQZWJP-2A=tF1^Rr$XTK_YXKKsy#E;v_&$Z$HWVx@rnq%byL1uC3 z`Of2b3$<@+*;JA+;y0`OxLAc%s9ox7w=rN9!etUgi%SUidPxiiDtLvKfu7;PO(5WP z-|(n*G{eaz?@eRCPc@@mCdR|<@uzv}X%He-^&g#KWG3p5NUJj77EP#Au_I*!G9J&z zF&5qW&kP-r4@iH!fi`$-M!v#CJkr%C0?`RN2~9d_C0hQW`Ul{J3fcGJKppg3^*SDp z^V?S##GJCvh08f0VTpXl-)3R+^~1o#SQO*fXdw_sIAv$eG92XvecYz9(BP75-FQ7# z_WG$f1aTC%#^Z+XpfffbCgP?J>RcsmYfM6@CA_XCR>)al11C(9A;0?(-eOFA^ulrX zrJ`0rCa9=?SY48GqFvGtEnU=|F5X;hZx#Zgant+Xw$rf)gk85Hl|e3YWCn|Z)&24i z0o3dC&m2!#bgSN@Q@!{SWZV(*A+OY=gccW`BeZG$O;BNyOKZLHA2piIkCw;kZNxG# zF;)=0%0v&%H#T_#xmqwt~^9f4Sv77OMQns z^u*_)0Yj_lmBka?3YPHoW;kEx!wIbM9pk?}G_FU8f@!p+UngxABmvvX-Ba=m8y5}F z=bhKRJjFzDMq40GOntpu(9G{ZiT}3rK%$06XfsZmgaMsGRq>VrDBtITFzWfb5w&wo zr614IbhwP>XqQ4k!e>MKw_znqYkRaT^JD<#(s+HE^<+Gc?A=RJB+yxEfivoph6-wA z5$uA-?P&EC!LogDo2uPde3#Mea>4}E5otP@sG?J5>Yy!VP6|C0Pkq&m-=kNlRczq; zjD*kEyAAFv0>)ne2@I?f0pqnq3kn2_f-e))_M@GJvwz0N#{*PSAEEsSY@u{?B;z;b zp%S!%Cf=?8bPNn9^o6wzCDgzBT)tRpf@)SZ|K1vxR0QoAVy*39vPh^hOjsU|UK)V_ z_n8QISHBD)2@p_EX^bM8d{JHk;QMAeqE?LekzRc z|LS!P3;FzQ3mk2ik1?NI@E{bld*6tIdhX{fg@(Di80lDc(o1&!9vrwp>RLQw9quHQ zC%a>61Bv{R*QZ-jLRXU1j0%L==b)D=o)4}2vMa8VBARu!Ghs0 z)H#pG!u-!rGXe@!eCeqVLm-aY36qjJNKfDStaNQb{DCvZ+gp=!OLUtS)(S>Z%IF1# z%`iX|$UdDc$XaDp-~2-Oi~^b@m1@Bt8oj@#l&@9ns2sT25e0-Jv(uHbQJr+IPBQ|~ z*W{eC2gV?`N#+Za5I2D~T>0L8%2jt?t$O6JGofTM(MpMH%K_fmz*_hM1oVG!ol8(!?zZZwY$mHN2)6UqWC;!7=vy8TfS+*jp-WEMFKlGZxdQ2=8``IMNx0t}w>P z-uQoJ0n*YtLOu;~|Ob3#Few-+h!lK~k z<3h%yg5EOI)=$b{5U0Yi=l$)K4?6UfB5*^CY=ao2z|`z+3m>%?3~&JQ!ebq?KS4;v zEUxV>E}97BWW6u#vx%%px!CD1@e2_EvF->znjtvt&!x@P+B0~<9DhOM%%jAofs>zY zj{`17nY;p<$;aCso(HhaPK_zR@vA;dvg4u!iGwJ_%4zea%3_i_6*P-2AKPv%%jD@a zlC$gAeFrKqTgg_%g5Lk9jP{#HW7De&1ED!*c|#@$x7B(toeuXIz7L2-(Wo?vhNZ^- z-vazgy~U%zRv7pknvU@Ig^d!nj-}FqFJ{wsiLRMr`>rjL1PKbt=ELJ-^=v0{-jA8E z3^j3Dc>fbQmUP=IzGbP^&*GrTgU9PHy}m{?=R}JA`|R0jYqe|+{apLWB5bO`3f7oh zr$HFchil9{&DyfV%OX^k`? z$urg2PBx1}KqcWXq{DiBz=MLgMbim!s1?-*i=#_of!nT|v(;9BkQt}d($Wf{;HOSr za;61LjKkCq2>xl_~phN_G{3B4&7cm5x-k_4Pu19ACfBe;^C9s!)ri#V{)mWeLRE78sW*=*MSn%B4c5wW^MiVL-fQ#uItP#) zs>B7;AZXekqz9gX()Jkn)Xfh?GvAS_zH)0fVD&c($xB7y`X{kZMi3(~Y95B(Da5|0 zh7Ny4w8Wj#}41a`WdzgVtcFUv_;BUYz>s zT(X{jDN<=XUy3sX+^yMQiZoNZ8hdAFGc$(JsC10gyPgn>tUxigMlyf-P_{h-oCYAS zoB$;yJYJxm7!XdHYq?NqkJ@FG_$pf&mI}ze$Y>@dS)!*}##(o89v9ujjWjBf=$5*Z zC5w7Vbhzl%y+@y4$w*sv(P@^DTAQ)aZ;#XA{xxdsNYw0jmZuO9&2Q+r-N}UrIty|s zdH%<1S?^iZhl0Lk5OtL|4Teh8Cti_t@0^$c7%5QsaZTalLM9il)q*6t z>4Xflv6Ic{*Q;0sDgvva61-Zu$Ra2g-u}J$e8TnV&cVqy))*$W#f3&^RBq(EH~GY% zXASMf9g;6f978B->Xgn;n;zrNFquFcs!ZN|EscqSX>+0j6dioapj7@(>zLQ9`Y#re`_tEt zLAc_vzvg*icUhuW|K*yX$y&(Tm;Gf%^5^EX^1C(F4Z|YV%a6B5^RsjLE(_!L+#)1W;#CZl zmd#}FhCy;;frnOq;%(3PmfmlVszYV?r_U+~ zyKSGQC$jr40Nv$y&ev6>G@{g>Y`kY4TTWx%yW@3-j@wKPWn1A*9Iy7JHOy66&MK>= z%bs1$|M}ZyJ~foKD?jC5_hskTLN%JJ#|=(uT%qpO7*VN4!x~J~Lt*2-HfQF_t<`qB*q!61@s z#O?APt4xFN&e+ui!G%+Vkj{d_(Bg07gVU|i<}HoR4|n{uD^B&y!t!5 z+nUH{)nt*+Ufc>nestNu35$ICrr_|k3IvmLefRf77O3dAZMy0RP1NcDjU#F!&$KM| zaI4`m+N0Q@v&Pu)0jW*>eG&=B+r#m=u;S7bR0_e))#_Ey-#RXHwqkuKF#?6vy3IE4 zY9)2@WLVZ8Qk&0A);sh&@?X&vzwvT`hMLXH6N3hU4?W+zH#~GGZZ3s7Y_^8d{EDmB zOKJc-v^PA+l8*^^U#Qc=a9(u>sJ>>4d{ESW_vPni>nQf7r~K-rC`L!Yq@48fSj18v z4ALn3KO#DI*&z?4(=Ipd=lAF#uZ+;Bh4HIAu8sP@v6g*nFp=OO=zO@uZ`HjRbhfwy zqEoC{)uORT{Bgbg#z3wT<_o3T=8Els*2_B2$Q1{r@wIDFd) zEh*>da{IQ$ z5!?lH-q(ruHs0(;=5#qG+(y~npBJido8-U_xcOm1M4Z`7!u|$Ff>=UDc)_t;+g!i1 zoaDz_%0O%iWvol*5{GYr9<_&E4}Vc#*xi1;j$MJ?C|2 z+JhC!KHMDRPM3NJ>He|aLdB-}%hqy@LH(5e6E(K&uGY((K7+t+9rw!bRPN>W2_mG0 zq0SoDL?WG$G!Okq?%1N9R_v~S{7z>o7fn+L@Hmu{1Vr_Np3&nRO~`n1o)v^vwwSpX zUdMQ{e*oMLPfUk(vkX)okBL2t0;9+C=6j$!-Zh)dS5mx#TjFM4Sde`FZF>VI2}jb4 zWlQ}8KCA0+f!Dr~LP0}jtn{0p^>vUadUFz>mVAjromZbzaW_j8R>Mu*<3d;E+>YK+E^?!2nOevvF1Wqzd=MP$9ko;LWL@(FACbB~h*$2Z99t+X$IVT_~Yb0P)OT#xg>yK_J%-%)&d!j3z zeSzXfYzJCC9TGAP!=|lDLLD>D4vnkiMSeBAz*d#_S&7jh6 zng}R(*S)=2{U@nCjBIpcZ)8`{<9t`gaC}u@_V((;V zPzrqUJ^Le0P^y8M@`ru_8O&GFkAnwqjxTJzi`ge+nzRaJ^nztR-szj<3Wc=^jIG-e z#L+K?OS!D{*x+%!`6{pY?S`Z_vXoFKa&4-2UKh98!KzpGYtSN4-nH> z4s2etamG6{#I1&W#08=5G?t4^^BaZyyAqtRJI~Mplbvu(y~(QfS3>sdVa4mKIX@`L z$_cNUq7Jagn%~y5B{IwlT3dTYytZV;AmPZ2b*KMbcX`pcwDxf| zjxxM=?7-03(i_iiKvXWD`4h8q_7Yo1qrf@X#&TqqLU(5A0hY=s+d~C8H zLHvj)Hgw9!iU&0*b${!M11&bM>E0SzUws2cFCiq3J!YZt8fXX^c?3YZIUBM<@t~M1=%oXTWy<&t7<$3 z%I)LInYv5xmv3UF)iQfsC_m0!oR~mBQDqC~?_}pyU6P~UM8)ii-;|J>sn?E${#vmo zg|_T!J&x-aZ%!1@`1R^a7D{AeG96Y*c=TSs^TNl7zm zPq>d?@_${^`C>64>7`@M_-2+;6opiKy07a^xo5v9 zA)!RE$f7Fyts7$De91S@b9><@IXc#NGo(?elW~##90nE7R3#4f+vFbOPy&PUvcqiU z%8(r)B|iT+k*5R(K-9X@9e1a9II9llV9pQn=)tMXDaC-5pc1kB8e#ip@wGd_(^92X zw4%AC%|dO+Vh^6`PDZ*b1?n#H{~iro`cv2#oGv0lfO{a1_lq6oOgt#8C@g}Pyk;Z2 zjG!m2?xG@Dv-w$L(Y2_e3a*&~D?+Z<&<6G9@4wdBPIdVRPfJ>9hU{gHd||_Umv^3W zf9%JNJSUDhGs9ucDn|7ya2i_JWb{-uPm>jlc@j8OzF^7@Y*nNWeAOVey}d2TeMuU- z$bj-ZUUv=!frA_~WXiP0c>7KKi*!V%kDFk-GxBendEFns zx}L51kWU>q$rDbuv*&AlJ`6A$;hkoh1^1ew z2dietViA=X;wO4A$Zk3VU9joSF5$4T22}0=d%yvzH{l%tU}dh+IW@GsXx-N{zNJW&rXd$W0rR%Fx zMT;h-_!PzO?p!N=MtV*~6Sj%XLh8I1S51ndHR)wSzE}9Kl*-?dE8^J*9o5B{j6@qr zg{(;mIds0tGk36*g^UwFeH(;-f|n;H7nw`ZQ2FVN20_G`YPii|r@iBoPjs3Q9j?d2 zBLeQLatcPi22ze(izS{^$;+1-V3gz*bOpIt< zbzE*oH{=>VJ>^Hzlc3E61(W}GT2UJnK&tG@6Ig8}d!xDHGs!4h2i4F@!jf2`+?o$! zqZ@Kl)B=Hbtqd=tG&cg4*77h=bBG(Rvzhv~G{|aBb$jRcn(zNll^wmNJ{<06y?9f%_BBi!Y1QDQwV)*h;hQEr`JE<{7@T$ilp*6eR zM6CD0Wbifi*ihpfzos(Zh4dS{J;CNMjE%9Zsh8VW$q%$3GUR!F#F5{=P=e+WhheqK zqL#zqwwh%!*zrlLczE|Gx8^J1Zr#MiCmH4^OyRGE>`p7c{4{(m1ep6ZlhUWT2wiJG zPk0aAjC>d16u9j`i*EXR`Pkv}XQ=Wi^-DH}61{7iC6sh=CMbcCR5*v7>b*NwqXAfB z8G2fb%02zhLHFewVAZ$>>KwZ;B~HCgTDp2OnfFlDwDnsSY0?$Nw(V7 zieed$Xoc7?X3FbNV&VhZT*yW?Qh&;3w7n^YVsfED$i%i-QZ^RgO9aHu_g*f)w>8ML@ErHNNjDv+9=XSRuIrnB^%46F85c2izIeVs|icFX&5PC8%liE2HQEy}2s9$(CRM=)tq4kr~Reon5} z08+u;z$n%e?d3>BU?la9wZ>vX2{HfW9^+=WKqZJ@|7eqWP;5`PT3z1$*7SaSuo{64 ziyhvh3T;5Vi4C(KpF(;SP4rkHY08^{=ROKjKNP^^ljjb-6n1+^7Istqpf&o*Li@l; zp@S@q@rDY#TlmGg@_`1vE-VR^#1lyVsZ=VMUaE-TFF32nwNcW_2k(>c^nF8GV=K*+ zeZWt^N=&9W2S!kdgzPUg?%1$7-MJIAj0}P16(>&)MTN@IE&f5l!re9N0jCIIcl-6X z$Oc`KU_5#Q3;e}_x~Yt!r`0{>&H33*&QAOJNzbVT zUBC7metxUiAoj2>iL=u`IvQiQ@kiOKG(N38>Hk}AY}zH>b&h)qJN!0jU(uVx{R>jQ zwW!nK{x~E~_#Rf)YSUk$pEz-Pc<(V&Yj0+rF0=Ro+|^UbCdhtW-g~0fEgc-A+Ux`R zd+Cw-@9y1HN(C(1^oUOvnjTadIVBDH%+=OtPM60&UH8c;WJ!nN`lQQ7NFQidxS#BM zq&hHoTts8P|mEv*w*w3|X`nf^4n^rGfdpN3+LR460xj=~~qwCO0O0z1S`_WBV{LZ7m2WR(U z!K%~`9LDa2dcQ1nUbtR9+Y@pQ!e>@ERnqV0tSUx<0ctDKVlunY-CcmuZ7a>&TL)w| z?w!6*_H*D8o*-w5f*2IgA4T|PnB>RIce+JMo|q3GbLSouA$-jp%@_Tulf|joMA%)C zG$)m<4~?}$vyi^Tr`a=`G&z17W{su1evM$n{#mawfkD_^ zoZ6&I9Wz4Zg7qD}YghN^Q=dO*9YJcD`iw zd=*`UhBw66wq#i)$?BXcG-_C<&yCxP+{5!S%DBX8#QM_q9V$$&NYI+m7IBSYX3jn=ZMB&% zdB^%3=Im2N;*`1rfq`YYh-#7;o9pjb>K7}hu5(rF9lI0Z!h#G3Li=)^ z>l*RpIH4`u)%#{eH`n<_xq)YVV7%-)k--19O~+47yrmE1qn|G4mHdwX8TQf66odS{ zGAO3)O4y&TPALk*pSjRDFUU?&jC7L#Jn52!m~Er zB|@VZP(?N}5>v`d8RG>$|M8C$YXAEJmUMlK-b@qVFVO5DQf|r+LIB8_?6;GzwB{#B9$%62iXi1t{+W$J+z`%H*#`?+&XEYa;B=LQB=tOw=?Dd#Zi-X{& z;>9`xwx~`P9bfv@0VPg>&pll{7QxZBVKT(@E6=s?7*as9zDVkUo>O@aFPY$L&00T& z^L#Ktck+&_+2G$5#GJFUi3WLs5Hf}r{Xe**c7FX9#kFOD_cS{~JN*EzGr;vpdx3T- zmCA0@2d2|Q-W2F$2#GErU2D8;y71^={w$qC;Iq%2^6Sv`Jd9p5H(go;?_Rpktu=9N zzd`UJLFMRjY;8BwpuysyvCgKUAvPy_X1+N^RLoV{y5Xn@S3QmFpTx!B;6)O$&vxSd3#H8u1<6 zcw?FRFTSXXAYB&Tqf2acOt9OT*l_PSTIKaF=c0Y#M8!-sPkO(` zE*9!x5x-)3f*v2KRjmGa0GdBjnYS|#fo%iIK}af$c*)Llfq(i^B(cuh}yp6OgpLRKoi)=z?t zcf!_@ZT-3^aqGcBKaP`uQ?zO4VsJ5ec~lv-Q2U zUG3+eg_3+@HuRS1JIU<@OPjh6mrbxZ1noRa<7LDWgo7K-`iI2-nFYAK%nNey+;O#E z*D=N$sw?83BmzIg5$gJvae$_4kb!tdv({cN*kfT=4cy$y3d-5&G4NZ+>YY+`qW51h zse){%I8AKo;?}dwxs1iFTB_EqX3AM-&N3!<5K1 zd>Ivk^jkOgI>S!`4qEk%JDB)IW$+6cYwQME!gys4sh0Qb1y?{p(Y$42-UE#mTp@|gCgg2Q;<>(KGl zr9EN66E&qDFYv%^sEK6Ji#@5MU$5xmS=2Vg26C>iPc4p>zyF{=M$a^FnBFzi*CIw* zJeZOw9!UBdewn#?zROz6|LHu-I_B5vZ~DI*t$r*p0g;;e(Hs5xO(G1N_H45`m@w1D zdcWV>i_a&-tNnvf23}ZMHM^&{Os20~2Y+wpO9ci^(b4|Cb0bU!jomyBj1YsiKs8~{ z2)C8>Uy5qQ%!5}{#1i1{Zb5xB8EF9)0815II00qps5D*QQ(>-xj!zF8bujI|d6oRg zGB6B%+BJF@{e!%L;63YN{?2bm>;(?GWgf+M$6!taJFe<(l&aUY1G7d47nZoK!;B6e z4L?0+IX{jMY37TX+dUjM-8pWos~Yf6s8=KjHZBh|-knOg7fR?ba48oQ{R<}ZSs{xE z>E7Ny;hFhl_mj;okcXeSdoSnwdfOC@sWpWeDp7r4fsVOtrmQ zT~Hh1U3wS01}o31&mRuBLdccE`^$ASLIxm9tG1LFo+8)_pW~m*r&+CAn5}-Qc0kny1OJ#JoaIdc=Xuq& zU;1ROl#RA(CJI*bRIkIlK~w65d)T;*{)KMkW`FZTbZ8bwq@i@Y?j$#i}<$vTl56`Fx$=H($-Lb3b!Iw~O+wLt(h*pCL!$`Ba048a6L4vCuwVApNo z^B~x~?o8qkk}F9yaH}=pg$dJInX`)z8QPB@-a!Rfir-VgY=xTG03;j&F$DwFgS$_~ z^;W8fwIA-18ntefY~tJWQ;(0JX0##hd>hSl=T+}+ z{3o`9%LbdI-OTbqOt`95$iUre8}_#j*?m&PF-+>Boo-oRM)bVNS)#~HIff*V&FM43%jDW2NQ)wE4ky!^lQIoRwMEWKx*-^Dh@KHdF>hnC4|!!46p0D$blc7epc##TW1~6ztTqJWn7W*dGCxA{`-aE^?*p2Vwgg z`cr_TPNYKZ#DPF1a6jIW=J1MgZ~jW+UGA zPZ9p7yp8Fciy5seHj2y)*Q-(|r0U?d#i#yaf)Ba7IsFBuxrV{*23wO)(Slvikl=&* zwz4^Y;Y0LEUF0HUDJ_j|7n^1gY4cu7v`@6se2-AW@O;H_(Sw*j9sTyt!DeRt_C6Yr z2l()6O%f*sE6V%r(24!hc;5IY)r}&32@L2fxur$L_|58%ujSkK;@JP@eyl_GM)ZmY z6&f4%{~E~z1UFKh#|AdLZe1T*TPvw}OA!=)55U9iVtI~Hj#a`J>S#p~{u6#_=A^$? zLr&Ok^(8sCDQ2N=H7r{DgRNa29{6t}lELEhy1ZNCz3~-J2L_Ko+mS`b$67+%4ZJoB z8|6MrJfN0A(awD|2)JjNt)8VY95kGft9^tYu?eyA_2xi#zrFN%63eNAj6q22qAZK( z69u1OQTM%c_l7=>gE?vKP>J)deZFsh=f~OCwfCQWv1Y9|p1kk-fm@*N(Ew`M zqXIs9sflN7T3;71@Db?FYhVO>T>d3B+1-QzjBWnT51+B9Tb4+cAA6B?VifX|*` z3I1AzSDEfH?mPJ{$7SJ1TyU90y%d9?wC&upJ-NlArY%mOL!e4c@d*=lMVcuPM5B`@ z7)eAYj-H}2V?izca~3K7J&Os-_m~4%00KUpg5OfN;!Cl>#Mo^LBtkIs6P@MZ)n$P9 z#-c*RGM=DJ;3Bm4wr=8)od7*sp`OZY++oY&^5a9h(H~_F6p{vHZ|_wbf~yp6S=Q(4 zwqZpBfu8+a(9tLVe|~Nez-IxyUN&xhSC`-gugL%XInvPKSE2@Id^yel&R9i>EX$F;vk^ zlN7mS^I`TkPQIEF1SQ@0^=*F9qQOPwm4tXr^E}Je>(g~y3hX>|Qoj8J zdO!k3evw+-==WY`;!qtxH5p!Pq=$59d|;S@=*btyK6N37QFZ_a{Ol9QAdk9< zxBcUN^o)n-4}ia&h#_H794Ga5yAQT9LHa!n!;eO{ALYT9f4(+53;ij2@{rIH|BF28 z9_$7GwN~Qhx1zj|t1MPsR&WL77a_kff)L4ucJ#=sh{KD_tigp`<2>gNA+6`LHlp>Y z&~e(AojFoX6{;EB>4Fb5Ejof_-p$bXH!Asf09*E=qaq-0_`O%VxcsB{N%R%gmpG*k zpgJd>#ZHJW2UZKe(jNt~S)pdrVha zEZ?O#vPC5micUkc1)L?hd@onvxAq-NqL)IN{?M2mo+13FT&4O+#_#Yxjj$8)$<}z9 zORW?rQ*K0d?pQVf%!*|CZCgc)7Ed;nBsz2cQUJ$AJOMwZ7=(ZCKb;TMqBM+D4bl$q zUK=2PDeWftVEFm4UrrGR(&IK}EG56j?U8KR#O4_y2Y|o_^~&fB<(oi%Bq_9BPXJhlKk18z z?!uB8WD2?I?6(0DT^Q8gWBd!G=r?2{04eA~J*3`A_j3|M3AklSLj+8EfwND+16&;= zUlOs80nYkMDIdN>I-#hzo43{%=f|WG@Er8#qo2l2cXY_7!0l_e2@o#%VqtoKq=^KF z7WS{3a$rUkTKk=T4LCjKQkD_YY3usLZD;*n^!2aBP^`_bSyo1XgkTO9Ng`hO&)Zn^ zw$lwp*=$MgP34BouMNM1fH=$u%428KL|10v*>Z2JQpokCnDkLLCVdbU#D?tAea(l9 zqoDWV3r-QCqI}Zaz<6&N@8)o}RC4g2UmW2HH+pVkVaE75WIk|9CJD=J{P{V35?C;U zl6rotVgw(X3UWTHX zpxj`!e)Fk5^4MGI$?aKr;7zRol6C_`wqHx{N>wh@riQ9U7f>a34%3R2KR9wYKJF)b z22WN;+tyQ0j{1VQ%dtk=@fQR$1)6cJ2O<#17D(KP(qr8aKSs3q9Yp~)fjWmCT^U3} zt}rZ79HlzG@n5Z+%A+Mfm;DXz4(vffq-2O@43&C5YCSJz3Lz@-5U;S${4Fvdf4VoP zbyxBUtC*OWMeRMz2mQ}Jaw_$Rdqb@%B938g_QA+@fO>+Aw97=b1Yqz-bVm_BWQS8d zt%Gi^wi7aWkph6H^8AjcpR_wy}#)Ex=RMeWisy?SGjD1d^YRc`<069mbZp(xZZ}S?y9d0 zf|7B=azFeeo}^9dY<;q2T%P7MUO3*Yk+$)fF_pI`Q|NWFfyQC{hxULkrHQoV;C~9c z%?QU)2^7jEL}x@nx>aLe9#(hFfwBPGpt#R2U4zS)=Pq6eGJuV3Pe@vXCj!ye`JX&m z$OX>#b+0|%NCI%^i&Ws?(e;qO#sL9b`{LuCGWpJN_(-1Io8`^+_1m1UAMfos%{pC6 z{iB#cKhsdM^}VKoWeIqaP9e@a@&8htEfHn`TbJp4^n`SW99B7Qn9-68Y>U5sX?!Wu zEy(&(Rz)x7yJq_qjs8yq^!T0>`0~a&MIEGvC}gg<^xE&fe`+x4I_T zeEUI&wY9ad`-m&R|H1&`{EL%rI>#tJC+c6Vw~r36CijI>pe!;5_O;yTMU8QVCiyG5 z_&C%(CX|$$OzRVqSktIZ9%2G4c(y7yD_``BSZG0VSBFVjiI(RiGka@?4KRE05Hb61 z>yxrWa-OJnDjV~C@ZE*?YImi43XcK>wRLwc`W4^B_=BGof$W|rOD%^w++p}-Ns=17 zR<70E**b?oisI3tcM^DQ`}>frz{CLgV1;*<;PLmdf|v})krK4Qzp4a(K#IdafrHox z?*te&jT*+kr#nReT4}U^iIs9&iP5Nxhvx;qrDl=1fK~*8B;C|nW271Em<7~Gs1m|@ z@5;V^{L6?&)RhYG$YSDZX}y6%e*>8aj)03gfzl2+$NRx$Hwi!#5hwR{#zCwPYk78P0ueO(@xM8be=spA3+@0Dx7)qq&!}v$NWoSUotH zZoui5CHkSsZNRDM(Zs;KBn9w8LPBc62+(%Y0=$8u$(hVo4)g953%{g&sL5ba`YeV|~nq z*Brgf6n%oPq3z~(wKY)^A6K@sx#^;dqH6e$a*rLV5eb++g@n8pAo~hsja6+qT&qx6 z`P3KpkU*MnlBD1%cG>g1QR~dTh4!L9CtF4UKgi_|LiBnHcAy_=1|1}ef0IoZ?3Bd= zAO)U`b^sDa95oNcH?T;5yS`~?eHJ%g295QNv953^k|fuAf9JJDOxf$S-{D!IAa|h@ zXZd=YGgKzvv^@So;VNwOm^Gxnvhb78Oa zP|oR3B8o6H6ihz(*HU7`xAh>|J>kd8${-mc2W+U&O3&`L*1(0BG|5xP*O3%#(;szD zN7Bt^KoVXANH1R)vpEgyTv!YWDI5@@Q;&@mQ|ygfFyi=^2zE!Q2v2ZM$^pL94ip0O4<-t=#z#ngy-7rbNfQp|@4 zq9(s{7}kV@pg^qvi1@|N{ZUZ*pI2Zr#_M}*bueSu=t}K2-)3II%YXFHyt4DXMDtkm z$k(6IVl1T1A^1b#!G= z@nd#1Jee{Nj;RCcGJpyw@Y(=*tSxmyC5|{prmQr_Q}W+=^&4{4%?HZ|8aXxALm-XPB9Uth_bwbiK)IN_+#03%l{@Ef^H(*IjC_Hi^M%n7N)5$kBdV32UrVKWK zW>p*(NpK(&atEQ7q_F2+#lSG?y!S|=rB01aFmQCq7b2hl&A@yh$!9Q>DUjnt*b2kk z0cX3cT_RLPl6fDL2G3&xIHm@*AHe|ly#)SYh%QEiz~(r_&bH6me zneKsTxl;5n4+u>e6seZ9(Y?}%T<*6)&laiq)EKaD>;MaAqb!}|#%J!rR-ghzL%x=V z06ner{C`S1kZ^A`99c^i3N~jVOUTiz-tJ&?YxF%Gv2Jf0;M(r*T61gWus0lTXk~DV zu&HN`>B>ATqCRMQ@A+Hs^V{Y^<@XQiMiqlKYnM0;3dKnKHhq3Gj(l@xOyy(svgH3Nca4@N?cE|!QqxGSIfHy#_CjIm?Bi2Kljx#0s zcZ4=}&jA^0^*i2)5vgVKW9go-A!EU>aVfZZ(!BJ@eMmB<`UcVih1r72u^Sa8Ip<~0 z+uqfPy(uAB;_#TCEk2+^dBeVo-(>Ua%Z_^*7jX~2*MyMom^Isv>xPX3z_Na#mh{d< znVw$OZyYtav|qmgc5%GVqN36lRK5_nW8R{+IAWAxxm_GoJy@8y$P}s{z=4(mT3b0w zD=C?zWWd&gzRc8#gP_`rp>!@G>GO-)7yBw@YhQ^vSffs-g~~LfnM!&SX-)kW`?Y$d zv*#aocB`aCK{1PmQ5>do4mu_f@L&!2@mhJT0}sC(g1a_A7<%6)BZ3wv<6gw+j-Qvc z{bZQSQRy(tj^%+x63`~4c}EoOFLfK4ox*R7@(=21%##3BkVqxITtHo5)f?5jI3io{ z-tUm}3l7>F)bncxnyuDtfn9Po6Y-P`1MhdSfAGu$Dyoc;l!Lo<$DPj)KKAHZLzY%K zmb4=8FyyxwDA<5~0zzI4lhOZ^q8ekwBq{C<*kcWNK`$4a&3a=L)c?N82cIwDYK|`F zA_A!e@8=&=3v%xX6;b--rv9Xrczv|SR_0qX0qP}vgM(Ub$7LF6s%#x;ra-baHD_`o z9=76z0QQHdD^67bbq=?#Ek6Zn9zfLg?V*3&#qrMks=-2L?C)hH?!NBL#(U?_ReYpD9S*Fb=v3P-zsjjweiIcME%EGsLc$)2_0 zSWnz%)p%yOT&%(AJaJ!^cI6$@nn*zW_s#EHs9F^iC4Ycx@}%ddubP?!NeJ2NG4g=vzZ94RrCxSJX~ zRGQVqEglqnR++)S*CJpFj8kHNd>~o7NpDZ?BN?+vxsOHY)R4P1p6l3OS}4|Mu7OncBRUx0S?W2H%VG zR85@2)$tX6x#^`vl8&?V>s$PO>$P-Q8KHebDlSA!tn)t4^A6o5y2AIRnC>c^OCtiI z4CD?rVj~?5Po3)sk%I~rANP^puZ-!^-T><7@z$FT?xx@^=J{Yq^+)94q{pZ7BglDh zTQ>5Vl z(Y+D$CT7tos8FqY*Q{3xZ!jE6T>sk*36+n*>(ib75cD@ahQqvZl{qif-ZLhKgP-Ks zl?$|Di@#Nw@;ugiM7P>)^&!xYZC68);2+gz4_5H9NyYJv;s1e{0M%!-5T3gB!QT5L)80 z?T%n#=8y{0dcjxH0JKYc>3}X*Bn(ykUo3!rw>4-=x=|pM?qkT_fT8Y50<)wow)T?Y zHe5zWk2nEHJJ*InEupUt9n$=ClHTfg-lPs{x#XJ9c{RqTr1pSMuLtm{H7e9+M>c6a zpevkigrU1Ng+Xm1FCPv(p|gkKJyJ*kRP%1iT7%;#Lk1x?h#5A+ECEP)jc+doD3(&M zH8cU8oQ5h60e*&a}68;=;+8p?5cVXkWGPuZ>@SK5IfY0pf4l`_U%~A zQMHjOEH~AW(jNNKl)oMarD*{k8etsf57O9mHtUx1WCEVPe~_GU9`N30@6K|rIJNo| zV|=qIAdp2YkDnQao!~$vt9-I}Tjv+(=Ffv8FWZ1hdfCLdmMejTE);o^$K0t>;X4MT za+BrUSz~pNyAt@k%ei&!h=u{gQ;?s)ft-@{VuF5;AGAElBK|OtM`fc~TT@b`n3sW1 z*2zgvwXZ#_MxFg2CH*|E;+hf={P3~fAM!UcBxj(hw(dkW2Hlr`#Xtb|nYAX8?NcPc zJ7ek1uB>(QhM55tx#CRZ7dd`Pf4mPo%k$thO|s=Am|aQEK2SFG{E8}60+wPJik4cq z1R4~;xt6)4l14a%q3NE?H9UNwN7{C_w_pTxvA))Yv33(UleVSYFx>skCXg2sz>Tj{GbUP|^BA~d=mO=n$m?VXY63nnLqB^oMgIB3V78r;gtIU#N8urS@*4@et@LWh1|G`ucKN==K1|303 zp+Fe?f(<3W$C?_k)_nnzA@Yaa@9G?$Cm6iz;iU8<0ZPh}#mlfbu|lg=0t#NL)qCw` zk3i7$35pPWy()i_fV$+-cGpwn7$xTPDy4$9=miLuYXrwFmK_J#n}yOABh#~8HwL|u zXU_%?tnh|e9yi{?6RGD^kMbS@cK$y)Ac*a8a-dcdtM<|=eQlXnfKSk2Xk#Uo1E?#< zZ%fx$HtEI&JUi~0o4dOs8Q09AmYBF&;DHlk|W^TO5AQBU;vL7?X-|9li_90y4GHD`G&8>?xAAe(pzmZeV4 zsNTr~w(P>koZH*`<3rGU&AnWpBg3}buR`#c^}+^Y`E9f3n&y*&7nxyTFn|H2FUi>| z;vGoyx=e|paDfp7A`HY275+CwVj~Y7F)#6WIbyUT2hunGiKTVndSLg(9(o^DIy&A0 z-|A%2K^b5nfAtfLPYM(@_Rab`K2 zBj&Z|QC#{}Udz}oe-12(l0C}5O>d?0RiFk7PTAxByf zHl{+S2Le&x*vusKBs_9V=wx5>Cmb^mg>DahKCd7Q={RtLxPlv>_%C)d?9P_tH2j?>0?3| zF{$LPXzwPx{7+SXJ7ftDSNqA|ngSi~5(Q3n1Om3MB#)3eN~}l?V2#L4Rnwc{L!1+1 z-roTny1OdNT?f~L{2fT)x`yD!YI~(5!qYorKYDNtrPW2E9-;bVwqNIS{PUO^CR9Z3 zL*M>!b~Oy=0!RV;SA8QtO!Owt74c-jSb_(HhKNj{R8CuGwx(RJ&!+5@;bvm zwK6*jpOx}EWNp7Z{So&Ss|84`3m9$SOGZb~u_2nH!LsZwaV&^p!ql&OwWw2l0sh#u zl?XIrASh2&jPwD;@8FaInXmyH-|_H!LFfBSpwXa`8sIyw6<4?SS-FN2;Y*6ibSxcP znmu?I9x>)csqp&)>fR%`ysXnQ-TuY-= z!K`GZl*m*5fS_tdiNvgVRa;0>@L5=T~^J8WxP` zj(HxS$Q@P)^cCo?PzUmYv8S)LG9`})7i;=uHe6Z8V42~wJZ81|74fm+;mLF?h$$AZ zU9)7*6)?tqBuR<+36%YoutSLz*OuJK2wdSPanL4xq_V5vaSn!hnA(@IoE5v=xtE+* zuZtW5Czs<)5d}8f8kHDTuY%lXgvAi`{2^6l$i4;|_=c%=Q#@$xXX*kANrc{wOg-U! zGARKu5z08^eKOPUe?R42bqS3J+Y>4!Iw2@VB7dK9LbSn$aA_7aQ%n>ad<7u(`@HW8 zq1YQ{n=?unkkyEu00LaN3U&!%hQ$iIvGWBTswy=+2ytkaN0wOCe&c|R3JkBaQb8hw z;k8et1J7$`c+h=4)|fQDk`FlmmwS1iI7I=grOD+Rx5|D&j5S!Xa`L1)TILfTX1D~aIobvqSL+OvXq-NeD(+M zTo!=}`jo2-@TUeUTG~h;I))2g1?I?G%%-fk(SH|(FvG>%CO_StE_(O0_9+0jyq#gZ zmxsa3532B!m_7M2pyKE1LDCMS&x}C6lW~K%PsZsHL5t&9XoiCt0z8Zv{wk0w1u0)V z3CiBa4=mCX)J`fuml<1anTcC(ND^`H;;lMX3+|4L@o+bc5WGtQ+ud%-v#?`f4{)vc zJwAVmyG5or8dt;@)JO{RxF}@4H5om{b8$Gx&uye8b@AZMz1jDe&otHhRY*W|xf_W( zyZy;73_T59%54_ciL0fLbT@AnTLX!tTOtvbN~V}g-wm<96=Ut$H9`6dU>t|R>U=7T z8mDRk755y?sx=S}WKHZ=kzVsq=rjT+<|h~HnshFh1SO-F&-oLq5&>p4CP`>?JM8#{ z&4<7W@9*`&joaj~hi$WN!+iv zu|X2DGoJdTv#$WlYtmLq)Pu#l;SMj2r854q^MZ~}#~bTI)EN+LqpofbeGv#k3sz~H zM~n)c(a`%S)vg&XF@rPQjO@n@Lo*@KIJxsYbmAW9Le<1d?uoEo9_zMd?n;zHFuXpg z^;XLZX+O`5XO;~vCV$RXl81@Ij>iNcBc4?t>Y%33z2RbnOUz*W>lx5sFkxlT{H$eS zWS>;kVVQ`%0PGEUOLV0_6&jyq##$q>nF~)BKfVt*t2ta`Vr$aO;AUlLlc&AqoSX`sxxOOD4~+x` zjvoXv7_ta3kpg3CJ+hctQXW?vpmbqI%n!s|nRD!Fn9ls&^>?Tu4dr(g9+LdoE>9uw zBz_n&Zg%_!*@rD10jvW9%ushl5{8KX&UA=6zWvyxz{2-C;1aI>(O9DmL*pXRwYz{Y z_RszxCnGcYtoMOFTH1>?+}qEM%xJ+;tCqP6oeTg+&P};kCJbX7hyfP}*O6#6fC_&G zcNV)2&Dyq_FavW{FC0@3g$^K8p-|XevJL>LFn1%ulbCVb`3yebN#qW>$flHxqYqK& z8rfo&gT=c_PIY|L$+1U?V)MvVcqm#CV}>@Gh#u@~ZGLY%z9(@;-6|WSeh*us00O4C zL47};cS4)#m{)LYVi>K4!Ic6M6Ma?&#B}I*#e?!vd&dT3y$KiOXT6$nMOtv* z5r0cn*oJa={3@#8h~e+xi-yEXF~6}y^c7`dgDaiihs{l}^byLLxtJLAi`WwJh%skJ z5u<9W42H77*Oxvg&v09!Dcf&4;}I+TevjJ62hUj5MX&;&NKibU(r88)3c7;Wdt^W@ zr>qr>v$$HUrhlAmv6F>c4MaWIPF3OvuQvWRA$qP+QOn zisrxyD~jpuVU39?1(K&YYReiG9{ia6WhY#)=FtOB@auo+?%RNARo}EefB_kmnemw@ z6ha(wrY%A+Jjfn63m4zIBadM&Y4H@7l%rg^ieM2Wp~f+!JhO0|Ef(QZWH!vx{cLxx z^2F?{ry%*Jhxr+;tGv!vE2d*@;moUTd}*pNx-q817%N2!=ED8_cn*{B3kgY2H+L5F zU&b@pKADa7w%_Zoj6s;8vYWwapvWy05P-3I5d}q1J%m{mw7e0y715@WlqdvPO!LPlOKMkcnF4 z0jqjMKEd8b#9jyTvkE-IRLydV2s;XdLewX8MBgiqu@bE%X7o?Rv;as<464Mwkm{51 z%{#aT7w{W&e>Q#O@158n+B<28of9RH=%=Tk-V*l3*?qoEr+z5=g-=v zl!~L;MbxS`S66@CX}}-pWQ!Z9Of9LEgmo(7QgAsm?^*>P>rf`?us(MF69 zgSTHVeQjbp!>8coq?YomYmNl=@lls!0)LOuB$)7!@twFwwGssEZ$z$^KgP#(_WbOQ zB+$enmZU_aMvp}Lk|qeuPiWdR?a%ovV3(~~zkixY%As4`=F{NvjXB`WQy^6I)eZy4 zFc@^5HIr4fk+*3etL4@^G-nMKS?0rp)F=wWP?54zi3y``_a?WodaAEF&&_H2(M4WI z0Wp}zCYbVI@%rZ|sB+}e!iVnd5F65sW`J9P4<6JK_8c==Cg_i_i0f1egKVHWvMe&_ zcwEhUe^KG&nnO1mQ6nB%F75kOesL)cjEKlGz9GIoaesvwP5J(G5y-^90v*JX!c#~a z4VrKVH1j7QV=jpZqyM!FG=dFY3WFPz3b`=#q6b%vAjrNFX!IverJzk6cr``q>Aebs{bM2HWv^Qfd>=KeNN4N~hrK+g_>FN+J46=XqXtzW1di(qBcmK)q(UJ`mVE3R?@G6Gd-|BDjRMP|4naz#E(SGFrF>$m%# zIDvYjuG{&NRYWgtjP(7z-Z*kCl|?2};u#y}_x9hUH2&=9z*FAVfmql?AB$QFpb<~(@L{m0-T9Xk!m6&70t!0KV$(#Io$_QNh+X?;0zY7%fRm?2F6=l7zY1OfL^T7 zQ0#Tl@g-A#p&f}~XbA~$R(@;`{4Ln*F1blDDu(~yc*;rdPjxMcK}ET`HC z8J47%_0!aFfGSl5cCKV0_-`UWR8_xwx=olW10`$JQs=X6entOBDH0N@dQ29dwz z(sL#k3>5(i3JUC7?cQq0+Z-?;)5TMw^+Gq*FR5~AIVHDWre`p{PHz0go)vB-jE|U=l5Rm6a;Z8qA3@Eg^J?Az5`hjs&BKQtyl&fq5IH|;K zNv7BP+~B?a$gLN0FThlnfg>D#x;D2Y(JQA^_CFrd6XzhhP(am^$g5Z>Z$zG1DVY+F zH^q$T@zh|Iu7)$vXBBZlf30RU)@px&F@tXT7!^J;x7a$(SwCEf`+t z%USdSQ>7RPVr#MulgQ!LQ0&mc^}i5?L;)k|4~R#rh=&Z6mv?!wV@48nzo1{)Ib!AD>KXT`a<4b`v?(PV6(!3W$^iqc z^?ayjqyHBi(~Ef@h{B2B-h0#;h$dE-yy)FaVRv^2wSjd%v>f9nlcTO^?ROv7uahao z7*qnz=;KJUigpU^!2a9mcFv+(m@Vp^Hq#{sC`YgJLO#5t^{(e{JB#`AQ2^9K@pFl9 zb4UkvdSQ){WwPx11bpeu-O0|sPCSjngV{nc_AFy4w56fKG!qo2B=Qb=VD}M*k_EpU zSF+D(Pssr+mRWocx=)=`2++Mw1rSlvYzy}VTFbjK%Dbn#bC&qCQ~`bTjn)VmF&y$4 zGK7zc>|cO%{-*ag(cN`^%5LQqNXBhd&C(Kwa|1 zZeU0>UbN-KQ9$dUa!JvKLd^O)klcf$?|?NZ00XGrhn5GLAaCQs02zWfm-e>Wy#rTw za57B16uKi~IhQ)TunO%398rRNDce8uLP|d;GgP!J{RPkj2~47RsV2Wi%>h3}95LcN zJMCizd?h?r^}Ql8QTL$Iil-c$6xh|*48j=hS$FyhBe|KL)5=2IsIb&Y1+@S3rV5DHR;Jz!H@AjH+y>c@0`ctzU zsDjyMBW=TnB?c53rG0PrvdaMA85q_isyRYlc+5_JE&3{S@qNJ0nwhT)ZNcHXd6F8S zIFG-}&g@;A`km4E3pp8lC<&u4aKkKY>N6dtyTlCx&#d#cEGB!^BoCcJ^&q70W1Uj- z@Qp=K-NhLte8FdB?1iN+=QjQB0Rj8D{1wX|)1g+#I?uf&?#(4Apo}T9s}Z#|dM)y9 ziSEk;&3V(+2DoGk)OBoou3|e$A0AqA@`L7^aiKCE`DOAV?ADvmJdYu@xSRc1gOEQ# zEn>lMGdlV4$*3)IybIJ{RDHNvgpnjlU-9tT=vt<}J1eO*{6ta^v2ytNX-(~=*g8o{ zgxwYJz3sO>K4U>Vr(?G#%gLSLF^NuniEZkZy%2D;7IzCo^9bH^_ zdxLn;Eq{ufRoJ>seyQz+ro~S==S0(vo`roW@p%TBK(@MY;>ll5RV?AfMBepZiZzOY zOBXS)17WuUI-Sm<>P?Xd0?Z+jOtIsiGYt=2=FuaKEImh5@jnxY&0persSTM4NF43C zJzt8wctbZDB4^Wo?D4oQgsn|}=DJJzGr{xuF4-&WQF?yg-1D@b@h_y^H}W#&a;1a2 zb)#{4zn`cN)1oNpsn1E^hxbO%sv)49{jDuk@`($zFF#b%r0l?&}G;c7pxYhfh(1k;E7j;&f`XWvWQ^I?K z6Bp6B71<{>L~wpDG|Bt7x0MJt*(jcQX6Ilwn-JNQ0CvZ1Wap=q!k1!LBxY9E=3o0CdfLC=Z2kEa$D{$RJIn%^(z$X>-lF#M?T^nB0x1uOA zV4dOVXqIJEOE{r;;WFwqrjC_s@8zxb@RQbwiSYmP)UUE zci3fE92xH~U8ag}U53bM5+sCUs7nsfQ~J3lv|#nptA|)>4Yi)&`R>Z!Azn{e^b3!b zQXc#rkaMx}rHpBv(CLUJocIW5{pLnQ^ae4mw38=4Ij+>bd32BbkPyNwcdQ48>{p#6 zK5@0U*%n>4s71ZmWA1^~G~eRcj_G`{=78_IGyNnwG5&!gGpO=R^Li%(@`CV6%R5P) z-i)>q;bkQ9E(uB9KiUJD4)3!p*L$QE8W)=x47}oJ4EVfoc>Lg&&xeQ7KOZ?h^Hv)Z zCI3a>h-ZB)=}%0H^Ef|quMx!Ff%Yz|40r{OtK&yHKbWdQb;RH40|DW>UQgf zq*xu8^UzjQr_A}4yvh-87SC|E*vP3u)hhgQI{G<6KRE(sQH%NY)CkBwh0XuS61V5# zq06SjH~{ISZ<&JU9feMLME?F9AWmmkQTAPvWxow`GI#n?GMb|BYrCo?JjCt_Bi50^ zoIxBvov4HPcmD8o`^1J^kGRu~+ooWtc=d2TVR=<#ZXwK`kG=0SV1|fBU??6J({$5$ z*T4(@4(9z7nm@8?2{C@Sn7l+p3{#`D=ct*FE!X=F-Ad+-uI81NO?=5pveTUbxc(9k z#S!Smg>;=~ul|b#X!8@gW!+|0GNBj%jE_DtP4xU_<#?j_Sqhq@&vb0yt3v5kE^V2vde*vNM_>> zQ^w7Q7=14Hm-Av!seKn?kFkHno}KW_&8E5ZT&2VZ-_yz9m&4V$;CYRymIM4M4%}mu z#h((+uudcntF?}NM7K`l=t74g=!)zGxO znDO=#IIqG6H0g!nKjU$8S&U!xsTSrt^k- z}Ky9n+w~9T~f} zKEHkO)1L6jW(^|~47DL&gfxf63x} z^4Yx}jWz%pKDkmeh%UD3gFVD>?X7z-%125ROch+c$CO0uN&M;++Ms7^Gspz>OfduZLVY(T-5EAz1QaF=PD zLNWm4|0XH~y?`u$M4LJSbnO6ba{Z2=_PMb8xS>fYnUE|j#lWm*ru=AqqyUhqEAh?2 z7)%5Hr{mKEvj^n=YjFh@B?Vy$KB=Xa`=s(bf8|c8m0xB5I31T!iJ(n1;d~KLrq%(| zcO7?TpB!hN+<2R;bWAwEh8& zBgr|KB0yOT$4o|{5p7_ATqSQuw3RQD`-cnT1`84HZWU^k(Qy{!-wW2oNC0ows=8Bc z%ykL!Ra%}SV6tVk|KHPx9s@xudhvQoULQPp8!#uiTpP>oR`DK z*>)mMW2gou7cno&y=KpAscX zlPnV37x*S`zmf=&%#rbWDSLHp-!Kx*!l)twvRG3R422uG81*6w7$)~|F<{}#@d;^F z`Z&i25IN)b(YojUsAu#rfmS={k-a39l%_5?!74zrhui12K3o z`Nh*6OJSFDkC_>sG~@m_LB4)_weOC=r?qkm2ZdyfOZUS;SndV5e+{6Ngftgba@E zd_Mfjc{_7l)BcV3FTvw)rk4^fEUD-fHK>una_;%-I~0?ciG2hnG5zR~B$@P7rak{y zy&nNZCBGf@FsprH=1+?NFd#u?n+e7^5WXjVx>R{QYj?%lzq1Q(UI= z!iRSMH(KK`r5kypw0=7?BAegeFxoC4)4uR+)fFoa&*a6>C6B6gcTg&?Et^>F1kPtq zd*>CM={HbKOR4_EgMo-Wo|g7$gk{g)I~}%4S=E<+d8w8+n{7lro)pHeeB#!DM|@XQ z)2g(lu{-Z}Th+^Um@85vHtFKSF0)2ih-pn$F9qunA>qMxqqDtuWap6|to+V6_B)1` z3xp;%fFCIy7Ywlk3FU==tj8jHVB+`g`@Qn}zb3%!yock%L>IP4{sem-5$mpeQIMQ_ zNx@v19;3;cp*9i&7Bd75G8+bT+2zOj$hZ|y^wwGRw*4K)n z(+tiiJ7oehMPgQMFh{`|w@if6Wyee8 z6pEuODXl?@ZAWTAg+SQT{;{wK=vQ)@PwX5G;0+~W-1qqtGX3Jwe6s>TEZ;E@i}?iA z*CBw}c$<3NYpVwCC+BS}mwVyF)mZ4`uMgn)U9!&Iz80;%0$ckW#rz!?&p-iV<h4mG`vcaxVPU58mr@gx@u53wpIq%r z|JLPwOa=qHjT9^0-k;K!Gd#43i7GW$`*C*JI>2Qm^ZrQ*=#S_FjB+_B78y#64)!eS zC=!#rNZCTO4**A?w1X#I2TIsQ)O1~h?%TRGg)x0ZLYYYan#4lzpQW{(B1#aF+jvEM zOK#(L`uL1X=cNc`3%z^zqjyLAmaP%&3G`ZE@PodA*w_@AkQhuXvv+6iebd11U?&l~ z@=>Wq+T?7`^Lf9Un8gc3G~(za1_?#bdMgPXQUN0|J{5i6|I`nQ8dKtwnb<(xxl;N5 zD9!)`8*QN&Z9a2=1x@g=Ci@+zY-#x)4U4ru=8`CTg^&)p%I0|%tOEQ-N&T;h@suT& zjiTh@jmTHE1K;UU==6kNMrGxjeh`a;%cxYnHHmUBT^@D(;e44OBPrZz=cKyDh1K2F zSBWLU+7}Ud@&2DreO&}5>b_P#EduK-P@_225I}5l1K8LY1Z-?bCGxKb7>@h7x+!6^ z^jQ=w2`koLDUXpZ0kQjiNPmDOsZc5#F)pxEd3=F;T8r(y71rohl-d(L-oqGtTiGFR z0Z&FcVvJAmm!EM3jj%Uo(D_m{rcB?2_zKJZ_OuX9*V#;E22VMlL2X|3Xvy<>Ae#fu z_-xa_Xuex6bEifwv-K6oH=#n(rEdLfHA!30NMS{>~V~s(>viM0^tc z$yuQ%m*(NrvCvNOuygX3Ww~SWZhFE=FooAV0CFANiju$g^bE*f8H17MDvLVVf*BJY zarOT+@%KshKD~VP{(SChqnl;PM`o`1T$P2qh1)aq5f8M^o zXsIo{<^Ztr0F(UP*EEs(g^=qYJ!{1gHUx5hYsLILcsvZ7JCbPJKji@uq_$nUXs2!X z;#c*@Fe)Ejtd_OD?X7;IP*5iRNB9A(EGFf>WiiD)SyL9M+Hh9|0^3x=+rj%0!$;m` zo|V0}Qqh-AV;cEMgT+)H@@ytqb#TQmP)7={#OcPUO zO7b+q+`v0nk+#bDv8q^MA~##OAyXdTB@!X`a;@6$;shEsDHqi=bWDC=RpW`2$YvVU zS~I1SiSV65IqWC5;XKo6+er@1hxG6+TTN*hHeUw3FHGe#w{w+d6 zxE@gu2+GkAAvJl(zBH`vVSA&>%67M@Dj_RkW3bJo9v^<*6((^y$UMa7NC4Alr!7*D z#G0?;(rC|adG%Kk4wh(9z;Yl1gl9Zpj1!a0WqRzw7y(%ai9ed4($r4iv?Tca&efB6 zSJI4zeS<>4Ng1|=#o>4W@-ld>M#wQSF&P}hCG_YhP*{FB-WXf3od&M{%gFs_(`EHt zq+I+8#8bWzS_G~u-nhI2m@f1+Aai{J^m)^N`WP>A)G)vJVsNQ)88u7Xe7qyXnQ{Ov)*4?=OO+)sipizuBg@ zg((n;f5J!gsb4+Z@aEhgeIxSKOQfH9W|k|%8$|a0bmMhYo+wjL5CUnc@w{OZ=+7I> zdXP*<-a|ZouNccJ@HQ;5KBI24uuyol8uC;6-Cr<0jx%3B54(la?fr z@&5~6p2T9l9$;KTa{qAmyL5xI^Ph@uvE8TcRt$T$`wlI183e$a+NW@?1X(07h4Z=? z-o=*j7AdD`CVc(p)eAM%3#GXYWR{H&u`vagb7p*sswMb$2rqI%T0#e`G2G7CUyz!x z@DA0Dcm|r#TgZmFMCeO*En#B*H0Ej%kGEpF`f(@r4hA5yLK|WS6Ml)ELEbC08~YUKV7kB@{ak zc0&I~9jA?E6?40P8RTWP6mTj&M3HF<3KZ$dcrCL}Kgj{2 zdRP^m`oeLvJ3suv?`Cj2Ca`Z>6U_hUyt!}Rh=xwA@JgoXXrWI2o`qNHH+ZKf0>GLS zt^IT{H1`6ey_Iw#BN6V!F>9mMo&xH7&xE_p8`(;V;$8a-m1`^+ItaJSZI1(nn9Wq~ zc%*}DmxiPPW$te3IiLGM|2-80%q@_aQ(WDa#TGP<`$lC;W+;=?ipIwwlq~4xickc{jma%BosY znRd)t+LnEA;4c3BFd?VHYD6E|0*Q;q?avQ#@Su@0<2L^7A6XG{ZWK{nY04iFIp_G$ zka!AJ3O;QPVwLQMtWR35kbiFj;41OvTq7y`NOGjEzh_u{_P-*tUF%7PAoQ0E;f$Tz zSW@Gip5$DX7<9`#>~+sBTtqc%4>^E5lGUHMj!47uy0ZQNtDiIw= zX-bKCp(xGiy*3-X+=I6Yl4zU?I&#yQpd`kXp&v}vN1~Eqd3DGje$lwqBZG=I0#c{-(w*a zJ;a*~afGig99x&AIi6jDo_(Rnm(1OIypqxP^qA`B7lj4)_6KpVDtXz%Gw={Jx8&B|^4}D9-xZ{H& zv15jWGHeR=8{?)DV?gasj%ZWNXA287X}{4@5EH1rM1@8)piRn4_A~GihYtzEzlSyG zgNuB4x#p1~SxxUYp1g+iCln$v50#Dx#^CfzjsK4269nD~Wj2zjUntjB8M7!^Y8K;v zaDytUpaz}el}tqtGYSj3-|a<7lv^+pcW2R5k2E;{^&=onU^uM4+UZXnOQ)HBvT^`~ z7Xa_4T`;&W%c|)LO|f={$<4B~Hm1_cV&*@)IKwwKv-eIWtoe({VCm3>jD#=q7H#&8 zoLw!W^iu|gM+pWb`}ZmkvG)Yn_jhzvu6T5{EwAgWrXS8@SK3lD_3>Q*t-xM)>WyhY z39k|ux=C`I_q%L5oawwL=>D}5x#|gfll9VHE|S^l{Wu5EU5keOCn#|=Y{+k8*GC#m z%H#V|R7tYv)2<4)eaVGWq_k!hdi)7Y%860Jv2P7@>U#=h6vW13DfqwQi_ms!kdm-= zMj?Tb!RNs4spo3{m}(bHSicqWP5R|V$U5L(dy*Mpq_P){6cGdkot_Q-XhgQ*s1)%c<%5&lngGr@$w}^tLB*6bDl( zFcDb@DMUISk!a+qU)Lto1n4y|e1pfx6}mu=Q8FYcgWoGbCuIm*kzxcto7_7W75rp6 zh6EGrvfRzws6Uhp zi6}-QHxHJeV{`#ijQwtc0T{!;9d-IQ^GgBx3hsq#BME(BdD1`FC-rKYu|_=>*&bqt zS^@I0L>jsr8p#Wv2LOT$^(36br{EFM`Vx1kUh@4?#$lOtlON9^n;yphrw7y8&KWF&M z`)BXg_-0iAoSo=mgR28P>e0jPJjy#N>B1^|*3m$)Cb5Pof{`xZv;6^IbNrgIaJaqw z&)Jj>ZgKpG_-b5`(S#>TwfO#W9lGNVJH(R02EFRQ+U=gwgz-ZCH7=H*c3l>?Uu@|L zP@a+#Ad2RQ30~S9G#T0e$u_i5<4KM4gALE*K(+qS(4rO&rNF5?(N=nJpgewa-wXw% zOFj+hJ)OB!S^9oLblTz52f7Q1$p1umP4wHOGO2k(2jKEG68I{d;;7I)LD2;^*kCMk zI8?Tu7Q#vG7!g%l%pUEP8;DvCibPgB+&QTw05GbU`~;eOV^K$NP%?7{70db|y(nM8 znsAS-4W{>hX*XS5MNLBrjS%nt6oT}G`NL@_qmg7kee2l7B0hz#f+1qhr;E3ypeN0i z)lcwgNF5Pjw9K|nNUHmTiKwzauycJ!@oD;;K!^yxElJCU z)ijTPwNM8`wG=@bUD+b^2xtdcN3r@DGePx0s?+MN&@|Ie<^fl9Uu2lnP9Xee zEKZt?`V27yPtVli$HtQ0^%!zZe?B=nWNMNUNDNH+oBBHGjANBi3*kz^^eWUitaDuq zi;9XMQZ;KRECf7Dh~+N0{<1qWngeD}Uyi#s|491~C-I_WF!n33sw>=9q(mTC(q!uX zIU|c766LbsD7vOWqPxr)z)(>&mEHFVgGrOH6 z!UXPR;fUt&R_K=l&_C&TY=c}sDQ%{I7s|az?VCv!SqW+MO=q=$c5ztjGT2|vNa5+Y zIpSDasH?|EjYvPOQ*D|V>~0B_qNTf+@Li`8B-Jr>;dUr) zA~@`sP+DJd*L_?yk^K%=%(L46R&=+aCWYsZxE=-(`YW^lTtSQ@;20NLgux^DK_US9 z&EOH;z^!67WP~ks#uIz~_cY(-Xwo14SC*(XW=TVBJXkH!F$l z4dKpvKAegW@fxyd4*|Lz%=l9^>R0J-j=_Xh|m4EVY6*f z5R?1(i;w=|mux)5k_NH_)?zhISJ=<2xR@ZRh=}pN73l(68hyDXyMG8sUXGuq{&;d9 z7syc}0C>nr!R((Kk)u{*2w43^ zVKvH+#9B?1Mn$xs%PAFdL*eq6%DH<+P;3@w*W>Z*|Fi%ZT^M5ZxCtI~bvb*<0B{qG zFW`Z{On>2GqAe>wfst<UCqM76MQ+I}VQep4)!YE`cX$}WzKWdXB)&XMy z^Dme{{uAjgJQT`Eg^TfI7^p0wK43q2DN}%-XIi{;DGB{+e~+QMyu+$Tctm2#OS5MMyZcrn=<5WD2euW@KiF1)-Ia>cSP;0GoP0 z120GGl+l7aSWL1KsvdL)%}X1wYAQ&xr3Tpp5qo4_+O;}pV06Ot{gCO=G*1AFwR9eZ zVd0R2T?%h-gNp8_dNGottOp6hgG6jlYLD-jI0tF|${SPvf=qolQ7(jldJaI6W!X2{ zfR6^}yRtm;@nS+EP4a3i-fg1o~O}XBWv)k)^fProq#BF zCL=T(V~tyuOdW+0u(uTP@cM0R(Eeb`@JXTL`vyL|01n4kVa#s2nMT`l(&<>OIxxNG zVXRKRl)Ae-%G)Ui4RS_HnaL4lZaJHKkmwgQ9 z8~l8fF_Z@7YeG~)0-sKJEcxE>)o=?iR>{7_l^0N^dZoIDG5me`^aYfGF*oU7;G~e_ zMFNtSdVyJRU7+|#<5vcs@Qsd3D+J5r#mN^E_m#JHXNPy8pb7+{R+CfOM5&mY{DateV8fRNST z`15e*#^OXAZpm&;hy-%6@3tcAEkZ94bP&K~KBm|2*-X#ZAjQ$WeS#9}x@ZQ%+6wj^ z$r-`t=A(%Kv(i#+1N>#!-p^F-yvY^m0yFsU9ZBN#E6mD!8o+{u@FhbiBhN{_;)atUSVJih3mn z>sKVV8aL&eunmBdi|}iwqJyMj1pNz57vj`K$Wl>@n9V?Z4kRltqQ))}6W)JqP9^q0 zgqB1}J)!%ko<>FyT4C69)yAU-5!?Mfc~xB1^^k7mR@mDM;M=HV?4ZLa4@HQ&ovzf# z!aSiNkHzBN#h1|LilR+o)e67$=j(3a8rBF7{De==V@EuiB^DhV96T*NMD*{&d$hW< zpFO|T%O=MaiXb3>52bU@RT)2nd{hE>8OGvI4~6C2^Q9_=jJRd>pohXjtf>3+;~Q|@ zcpVs7{jCLFd@7yItJmPNe#cFUNw8ZADiZ=r_@M@_pA*4>ukq&xWaGr0AJbS1^VvKy z^6gCya8BOQ+pPO zaK7?`+Mu6bzKOaT>F7oiK4S&D+Q2h1W(AzIe&wIO^j0q}IE+gZwG+v#l#;6;O-dv! z`2FkB5%94P+Wh*PE*t2g>Gs*S4PgFjgCf_3yS^8ZnYcFW@*Kd7Wq{F$&m)B=Uqu>k zfvAiFKt2`Q1$gWOKJ;!HL(QLcU60S~&M%-{!xxjRMl&Td8XcfOeh~hr0W35$RVsD= z_!NyzhYTDIL|Q3lDDo7kJ#S~rlZ|GIpQi)+wqLmask19%z}PAYPEbYrrc$3RiN!cX zHJ1Mg5_q@f`DIpwLo8kQBgC4EgJY%08Jk(XP6?l>cs*BmkAk>MAc3ZKFZ>NAQ;BT~ zI0n02?0eeIrPW%&c3S zQyLm$<5lwrR^V#K;Sy3n?!PN9BpV702?jU3*k$XmEIP@=P>EhM?R0Qjr z61ynLKVZsP;_+$Y0=_O?`ld_#622S(DAz=}&enQZRu@AF+U=GY;)Ox$cSo!Q;9hLf zcik?(*UqyeF#s6U?U5&5Prt4^r@IKhi01Rmsi^b6rfGU!EYYn<j(sayIR~c1%Lh~(Z&c!Os+X3LU*JGnUxVmJ zr$UM%8jTPYqQk6}4kR!#U_a`0utmUhLFaXifmN|}oU*?}@kqf=5Y4mL{CJD;=d&3dJ4rXq=+7ogHUIvCYx{YNc^_Q|Vn z7OLFOTE6*aVNJ;z3UFOv0l&Z@qpc{;YDUJc2fHUq-@*pIB1<}zNgVo%OiBg8127Mk zw%1o>1aP1+|JSjED9W)2kxnvu+rwFgBnX69pAax3!E#6*wfDo-pIvZB(?R}5cL)2p z+p-ts0DDr#m#41@&5Rgi;P-%S!HQX;P_r@J;CBr%6^-wV{9)fx`%VWdk@30eq{#2~ zO^E8YzZK+-Q&GbB5rjQk^Q*2*FG@Otq5;JnH;!E&V>=q-;}nc6j>yGo{i}i_F*qw$qUE-ay!Lif12fX z4J?f(kMQq9!y)`~u$kA{3}GxUAki@C(#x9~3){14TWydNHnR#KoRj5Grfi2d2118a zv;{{~x&nQ79Lt`V(HM`7pY)=h>+nsJaha&&X1_V5DpX0~MDaS=`22+L3%}zpg=_t~ z(|dC6f7_?&u&KbWXCKV(sILFi(&9Y$(#eNW=X|{B(kyq6a$j`n%%XX)eM8Hy!{teoczqJ$vi?Kcg=j!@Q&8?Sk+qB($+q`h%>pr}H z`<6m)CeaBD01XMs&FuRJJ|GkS;*Iz2XR=BKJ*A6GP#Vse2DYD+c!VM7U8^Zl>g}mp_84%CvX~${`&2@V~m*U3%cwq8P%so7cj71qt zL<-j#yF9%aVxKn6Yh8a&G4a0Ehcsot0f)Y%h^>7j?{RrM^m$o-@v3 z$h}w9OGG@qBoEZeV7d7d5H`A6vDXnUKWIb#I;#VUqlr@2B;b zc^*q!l=L%2%RyCWrN!FWWxpmbtgrucbFVDGRFfulq|~?wx8r_x$*AF7jBiiede=;e z;rKJr7~ksIpHezX4!RXKqcM=qoiSw5Dv0%TJ?WK=gV z|B!uZH}eo0Marexb$`n?x@Jv`awo_1omW3%*XKBMLA%k&=f{tXr=sWXLTBqus{wh) zf6L7`5s>M1v!!dHtDl6JtjCN{hLPj%TsJ-_uz9cNeA@)&&lK%PQEPcA9K{D$OyM6% z{krZ!-y9a)VOC7b4MKj7V!QcD-%L0ZV6$Ma9hU4?zrc}g@nZGuN|2BNRZgmckkcL5 z=NG>#n`NJhPjE8of>PlqC7aluj-7)9Zbl-9d7_>PU z`u&ru^F}I%0Oc>B2m7w~iN{W1K37YVk`#HL*2$dCbV2x3A%{P+8W>~~<}S!==f|vu)U^{FFwt?#9j(d!ykrLeYt4HAf2ch{`pBr zR}^yx`xBmGmA^k`?5BUfD$$otaC_rj7i~@ztL$9ab@)iq;5f&5gOC6EkNd7>hL!iw zN;Hk}{Si=t?YB1WmKj0Y84fC!TPp&pJ|W=0KM)G*O4g)UMxigM#onwSu(zT!wa^rwsiZ0pD6}VM!98U58!$-OP(8l zYAklSq=-j*-k)s`S44%e8?-7L@AQD&DD+otwA?|Jj^mZa+Ia>#u_gzSS)K6QfaOor zI9Lno2pzeFR2bzr8eZL0(r(ryG>cn>8DmGG7n59%9hsV<4N`hLW3sZ2v04t zsTMHY$8EFSC9c3T^P1BqR@QiDO2HG>6BRpPQWX@9{r>wfn1kc zhP&RwLJToKYx3I;#{=EoTYjsNRFC~%K3!ikfLm>BIkFt=T`T8trvvPo=F)X85yS2MTToyh-eKp+tat@sCq_qs-CC?m98_%WQehWB1gnA zpK-Duap2>oO%`dPY$rQbL0Ip?kRj#kho92<-dAEiYvhx;X>eRt^kF!2yL~aq7qAsq z;opsw`_K3_CSet6A)!szam@bpm9A2o?88UntIi`a@1cUkO9@&%75G9UQO3q!s!!3y zJWKUwc1dg{DVHruDLNkI6+x+n-xC}KI=hJW(GbQJC}P3vhjY@KAS&yn&r9D|w|5?& zwOSYf=mfvAI@?0@xA*aL<#c5}`#D?^r#t)2?&_UPuGGx*@$viM0i|MK$fzRl9sl;K zX!a3wjRLH%!ST5hXuhN~w)$enHssriWmn_-wZ}+NoLXP?)Hw3=_81$?8U7EsU|SaCqxzNK@hx;+ z&fdckr*tWnLgcuChn@koAdFim#!QS^rfY%7H?@3GM4_SI%5R{ypgsa4?zdp>(eAeZ z&8=BKWi$o`Jg={=_xZXiNxJ9^#oBu>wWAw`;ycSY%10Yi{iubk*k_ziN^d!Ht>cl} zFPAtxf*7Oldm`*NuNBfHh}HCD5N>ZZf^SeJk9hs)&91P}@xADdSNynIC-3X{;3xNE zdm&&Hqvab!_aK3=Xz-qsHPD8sj+|dL(kPhODGZaIk8pHy5O&L*DyF(5_)?=NGNYg? z9X6HVIVHGlvm1MaQXzZ?uVru7@BH|fEj`ZO?tWv`>dTSZ57y~~<2XNszFfvDn#r;r zsr~(&%QoVf|cUcRgM69XG$&rsCx1~#5l#%%VcyT8E0r4fvm z`%F=Bk$6akP6wA*l>J0FU}lG?8Ld_HT3V#JnvmV8Je9r`op9?I4{y|s(sln_k)p?a z?O8#>t?AP`TVc4l{AU`dHHws4N1c!Xg{87^^1g&{gih|ZaXWR=6&vr{1}KF^$j3R& z%Iu$?2j2kU&B}(&Nlp2=11eS=jP0jt`Q~8kB+HmYP$Bz>eutid1~&D}RIi#@ii--3 zQ=$PRFlDG%NqHf7o}(ZA771?WZd2hK4oz|Vomyv{!j>hS+$ezt6CP`=5l~p?H7nwI z>2FXcihT&+7QE?sKd#R1V4^lvq2lKrXeEqwHetdXL1`N=EbtEOr z?BT$WXe5Ys0AFp3{`z{=PgAa8O{BkHZj8;t{@> zZ&S01X#j-@i+%+;6}DFLH6|=iuD+Uo#MolcB?0yO1I9U?s~~cf_wbhs&@U%~Y3thN z;oPCW-YzAf36{`Fx5iubJ;H&ce%L1->k#U&F?iT!FkMObDa|Pao#_64d^KvTLZdDj z)2g;LJevmJg|{Gzq@bq!r%BcUSF;@_k?%ile`h9BL&`7*kE3F>>sZ1wo&hueygZ|M zM3W;}c|ew$HfK9eD2>#)#tX)=2-j&WLN5KkZ9-irHK%i-G?ifyB)O%OvsJq(n*xeu zm>X%uIT%>>?qaE?boirl>7MCyA4UWejG0*If5f3b84#+La0!QnD16+wIZZ5FN!Wa@ zO?imXftz{TW_ijWyD?8X4x3+phlWGwFvexvdShp1ZUGW@3KE%!;v9QdQk0+YOaR5b zkv|QNzbIsc*LF(djVc-6*9Cj{{n^iP?WjJuq#lP+t@fUY-On_>=*b0)#fBw%6b+x6 zO0q|~CV6bi!rV7dA9TkK0mhEk@BR#wVf@3YmHCvsh4w14aq+q*!Q6Ki$Hs&DYRvcq z>+5i8Q};qAQ~OBPkHCKjsw3Cb0}3_MIh$P~&~Fm7xou;Tm<

Qf)gPSn@BDw@t28 zL&OC8=%DdHb-Oq^Q1o_ef51G;wA7os)o(Xe(fEORSl(lAZ8VCJbI@~p}@Q6 z3;)u8B%o5-wU#WhCyu|Wvty~n6jd_#ihA0;U{+n6b5C9kmDh0n(#YalalutsrxQVY zf_f=lQt=Ygz)j}K>;Y&6bDvBVg4{elpOMG1`Nir0G1*_xn0G%@iWHpXG`ChtzCPE! z3x)adF|>k1_f(1d)c&3QP(R65=}3MV^@?}6EWE*fNsgp1L)@P9VCjuUk(+J=jXpP) zphTfi(f*EkTE$(aY(yZ1XtV(<_PCRy>OoBZfwkZw@OR0jb^(lw^zY6)&Whz~Ztro& zE-QHn4RLX2jFk|&^Y62`hpTEjthzbldYTYTp-Old?K^YI1*HU^$Eidu#U2tP!H?NH z?xu9sOra|u72aE7{^zzUz=O_easfG1i+qo>(zPm{f@M z57zJv_Tn|+NXB@4V>>2yZQ@PQq8}-2#)`6S4$Cz>&4TgSlnxzNvDMwz8uf*&@?y$q z;*2-=GP?yE#D*$x`VAtMChnCuNsyU?JM(D+6S{lYFi~8+L**b?-E+flBl2$S%Ql}B z-Z`zN)B01%s4jAi1f)MDVqMjS?oY#KfVQJ-6)|Y$BmIRGEuPKo;e5+iEH=jm<^BYw zixa0QeRSZJ%x5&J#*A(L7Vz+TZzh+V3LK;JKxlok2=yrTT^1zsuyz$E?nxK)QGBtZ zhc|7p`{X4G{%QWO&Yh=FeHmLK$GHJ9K5tcxS(Raxc+3vcb3s#+*j)Mgzm*+#(`^Qn z3_-VGMz2|3_wHw<2eRZ}=nK-PULF$YV?nvbLk5IWzq_o9eZGDtj+u*4N^7qvrH^8# z6)8urdEj9fE;mnyNh@+>PXO6vNZpB3DT80{Hde3?}uK#qg2FoA$PiaFx zrGE-?OeF+ah7^j``UiVIrU`qdul4cLM<5^#z3rhY_%*vRR4oSFixXdq5}(F4&KlLb zbD)z@340ms1+?Xo#nOd+RlfaL=b5p{W+PqyA_{^Dy6^2WhV?5Cts9%Rs`}h zHc*Rkc9|1j(TZ6R$`JX&GHTVr_GY)SNZdY#F^c5Yg(t5Vsbz8V$h_mgcV@o-tRU== zd78`7ry&iDQbp0L#yFjv?E!RDC+3aMFDBhf>G*eqohb(yvPi$;krMOS&1mk{m_`cmR*9KU%+vZJUQ_nUsr8$IJ%dd+guR&t63~Y z#gC&6S*$(x?B{EU@^)CczH_dgyQEY)(TU_Tl?#Q2bR=j=`877P(H1sVQhr)X+^Xv= zgEYn}@b&A(ydo{b%bHZ4wQW2#tSRyB+5dWm*^I~o#*HFDk!K)%DL~kO%H#K9*uRj< zMoom)d%Z*GWJQuDg2oHfA1LSZJhq-d=k=5ZQ`=kIN@F-Y+_YS_$~)+v;g$Hb39)|n(u&=rTSm6-st}E5#1T8h zs61a;=LPwfQa+|vdlj`F^+dQ6K9Lmf^B-eMGvo31e%!WYwRsevUG7gdQ5nao2)&GP zz{zlP7<0rBtFzw-zoueecW8uW7KAi>(TTQ@@P$mG-ld}8u^n)Tvd3J#2c>rtr6k@^ zD)Pr%4A(ava4^OmuN51&Wj(ki< zk|la*R{W8a(evuz{U=!453XD(st24|A?r$`F_S*^!JrZQK4sosU5yrQ(;y@&|MODJ z?U~<|#U~!^?WZqSx@~LTj(Tcf%TxFc2|XVFc+cy+E~SHbf#O$#hC`LZd%+7uKf|67 zo>+;M!=Vt+4DN%N(T4r-&?!TWXg?U`Q}~W1I=^?b-uWYR8Ke{?@{{p))(_%pxJPD7 zzZ;24VlDY_S5xj2h7RYnH=QT)1w6SO*kk_weA7yRQo-Nj#TIGX_Z;su)zrjeNpal; zJ;*8F=TkwS`n2C*%u~7=Cj@cL`@M(5xw}$!ueMbn3{41ccn5x#zf>#Ntqv+LK#S}3 z?Q|Zo0va0l^Il-eFv_Xl%h5o$U(OXeD5x-3GjQ4-l}iN^qayu750auW(p({<8qz(X zmQ;vZPnd0Waol&3+9ub?%YKZ_y@|dE?MTy17%arh%s3sx_d^lxE7JwHt*u~TwP{bu z{G;_mT0xA-0>$B0vH_dnGZOmn?rfO*_M-2Yr0nm>cRw@@vfbD2`!bPFt!0>r*^Ts! z$0s%r2qLzHDAq-?Mao=w%`1&s_O?yAY+n&|8Fnrx-~Z{9G$SY@j{@;Nyc!-C-rW_7 z@R%n&;lgg~93yFBdf5m`)*7hzsBHL0NJX}QK?6yDcNIRc!kTT!q?SFB&O1266B*|7 z+Js&DOhRNUXWTGwj`KxiT^yAYjpx+=X#s@3MAa#G^Lw@0O3xVD96%XFN>gGtN|%2M z`J+EY;5XGfxct5Pt#;mf+Honw69$rw$#A+ zXSjeiZlzY0N%dcuF$evmy5repzbcYmsWr*(9=0T+vL7Vj~GZ-(s_RyVe^&ozu)Y- z7&^7My?oc{oz-%y=Q=gT#&1pnU)Y1>>*b22_v#L$`n+Z9a^Z3`G9w9hF*q+w+VPV| zesHbY@s(-;*H`fCN>CHFBlw;4d8W$#hJH(8Q={QTn$XW8L)uHJGJf+y>Z`@jTnN1T z!QU<+Hh$KQ%4xM#WoZpUx@42KP4xv2(a-_zei}{P`c9AB2DxOq$Qo~P#Ds^v9y%V@ z?uQSFlnQMOchvao^!HxSRa`q1DR&=VNGs-&2d3M8*bt_xg0;08>bfa(_|R-1D6!Q< z{$|N>lw~oflw)i3uE}zOfz=!*gN03xQMKy>Zja^cJaPQ4UjgW#OnwHdaGPTQaV<*$*~PRs|q0D{N9M_Va8 z@8D@lVVf6@?_x&3rMCB3J&6v9rQ#+?2`e#v-7@eevx9#p^BL^dR=s&W$wU!;{b;h7 zi08qIbOMkimZgRbpl4#h&<1`5KUj}_(MfomVW9hKw{?d#L2~5N&u>MXgA zO~TMOH>M09XhkqU7gUO}qgLCG7AZzfCDo`y$Xyl4zBHtBCeci?rA?#YYGlEo7Fo4K zG1l=_>!}&22<=;JNcoKoh3sfUQ6niG4gob(DYElp1T{{vNp{w^_l6;I+WM-L!s82w zk>bOE>VE5!{tod(ten7z(4KM-X;wLKr~94 z^M*y$e-WC5O?64HD=*n~Rl;G&(kRG|Z*$Sz1oYxUkh^(|E97F59G?zu5hrISy;@|P!=?_Rpt@I&%;O{oGeebrYHn0{!5{znIXA$VxqR;4 zn<^?MkT-WQQ?o}?wXUtz&w};MOPkuz%m0=(q79ej;U#_U;voik>99}Wh;G>KT~kU* z{%v;wIg|fuD4T|z;pifNaTMAvW$yMG{xm5KG-4A#!z95l9M_9K@K;Fcog`;KxH&|m znWBha!M6Jo29>CVwc&Kp?aKZtOR$D=MMHZc%=t_(ex&D;r?&c#rL?$+%Ee#1iZw@5Wkqf;a-|BATLO=%YaK-iw=Y)(Y<8g zEzw<1I`KZmtbV-Hk<~FA^^p6Pgt-Uj27d&-L4N5NUx8#T=5g`%7!KR?;q06E^NH}e zCPk{jx4M_VUnRV@b!N6hi`kM1$};Zb>9{dLPk{|6ThdF%T} z8^2#=glI>Gl(aFJUW^UTS(Q4>#}N7m^zhwJP}CtMnt$9BnGK6%gwMrzXe^kdxXMq1 zh!H2v8uk(mWjZA=ktets!+AZm1JGKm?~L=;W8XfB^=dDsdHgN6a^+WS9<4&0`^p^z z&ipMpfpoZ)qlXv2^6P6LAb_;QJkQ*yYS2iKnoZTZ`SM(?5+Wi`k=oou;1dav$m|0~ z0r2kn(Wn0mwfRTutk%n3J>T3IGo6|AKcfO~TT*1i*KKE>v|hTGocvlIqLQP;=Y zJ1Q56^{jUdLndo921$lPWQiR5Qp;G8>9ULEY{C~;=V+BW@zG~8e$Uexj<{dtz;Rxh zc^$408I6(-O3GoMqtx2XIxm(x#erqiUmV#N{AUz7RBvGS;cSL^to*aj7BgqC&=bspb>vjmC0;!(F-Q-R;R?&4}gaahQ0Edk- z^|NSy#BAncoi2GUSNU6*zZB-P$yOk_lI-S2Qh3EVf|`IoW2tneN5I8H;Hw9>%hJCC z*)|*SNUtd{yO)^(JSf5{P*n?oD%%3Or?b)X*GF9xdbdC65KpsAeNI1* zw#44_tpal!T?zvAD?jO0bdQf_3I#KJbbQ$^y+h2V6eyG50*P=p^YJM zV^o@UpJuODH*9hZ)I zNTA*avnuJ_juaiEga~wOU=QYVXKb#F?})*{?g!xO%f*Pkxd5**VH?XE+}Jk54ul4R zaAIu*6GuxX=Od}pTzkf zp|U}D1wS5Bd2-}m9!QCR*HVO_FzuLix&4|R`A>c1tDVm#f}K3!&)%cCH&S3iauLQw zes}h<^igW}zrg$q+S^;A$z)IT)pyohRK;WIN}(nKv%w-`^ov(XikTg$RL>!Ls6kb? z{;%hR40<)TLY{C3(&&w^_-lK<`FBP`s1fsD=A0S&S9)2!;~|2g<8W9vm}k*nc+@Y0 z@ZbBY4J$f7H+!L7`g*m%tQpHQO1p3uO zW#K_64!!4Ld8}V=pT6Q7f;Ox~+29oiR18P4s) z6XGq!hH=>*K-suIK*9}Z2Y*7DeULa^f`g?SI2!roEWJL=rc%AIiBBrTnVAFi+ush5 ztEU{Qrt|cdjn{Nf<^**0`}s&ScV5z%lO?C_sp}8z>GUjQg_;IFI4ovIN>5>YZrq)F zcJQd5;OlKx>}DUBA-pF(kV!I-PpUY54{A+U_ELKb@wUO245vg-*M!Pr-LT(^8FiY= z>a@!rzCw}1rIqv3?;>}v`1k9c11lBZ-mmxlELvG&UH@(;VbE!2dZ(C8h7w+$Mv#;? zq`kH82350G+cwWjdXWQigAxRwbp$H~m7$Y&`DWssk@&t)dAJb6#S78MMYiur{v zOSAEvbD3DjbT7xmQD>)s-|vi07C*^%BDorS;H85AzIf+=QEMi+gMDE`48&Fo!?()9 z+l$ZCx8F*3Z$OC6S<3wXlu(l^gtDl@#H0_A%2JNLSKT%HP z2d`Np@PDRX{&<3{k|w^qaQ@RZIPe((`71B>qqkCcGf1OO#krsNhk>?gcYnMp)a3si zQgjyvO-?G`YE8Z*ff?8wh8VHFR65(mC-oa7TfsZQx^Qj$iB zahUzz$-*db5r`!YCU2@_4KN_tRLQ63oT z@lmK45eVG!$#a?|sa1ulYk}jI0*Y?TrrjKdM#}4&$)`r-t3VRL@8CoFbER8Y(2AS> z!i07>fy(pM#~!3-&7A+Z>XnSJ&<1?1nm$yW{t9!w-R_C$Z`A5ZtP#rS4VY+?YLea5 zD0ClTVDjSuu)TmMnXOgi_6Zh(+ua)$t=H^Xru94}1O>5-A<*qogF&9bySd_RajD)X zPb!0k8eM}RlJB~=a3Pd{zo+u72GxP-WDZ!*ypJ)z3w;IVZ35rOC}|I>{>mECGL1Ym z`g=Y?`@PV2*-RG_8#TXhDvc$B>r(bGjTY2~K6;P^o0OAyycYgx(YavBm^{1}i^erIzCk~<3Q=M*H zd17Qe<_!2gX4)`GoTdtu$X3?R-h(r43EXMm6$`n_G$kuD|-%j#Oo!FANu4W z3?u*hsSzC(f7WDEiT=`kk&oATe%$u|9~zf0W&I(Ct9>(rpyY9~O&tNGXMuQ_gruyX zo=n*B=4BF|GuB%<^D__^M_YBlx8@C`2@#Wws|%9({i6r+EmB!aCcz$T>;b~Ob{ zu05{YqbGBw<|)k_oPK9|=(t|7=(*L_u9QNxTd6G&lqx@Y5jrtQO=Uc@caDpK*Emxq zXYdIoZEy&zcr#f91X61914rh@Br?8P*QNCXf)H#V z4>N9c3F7B&c}Ef=ijvz*#;LEkm#IuvkIDb<*Y^a4MzmS&onqpNNhpnI6~OQ2e9|gZ z@BCZU-lfZ<0)M6uA4tLPRNne8K??@Ncg&F;8`bN1a#~;(p^P{RR!5Sy;;NFHOse@{ zzLt@zwsPFB_Uc|n7IW))elg?WFHE$aRRFM>Y+8D+2W`W>E5_(BXm|w$o}MyDI)L*s z|2{lgYGUyZ$FBEX_MO5PLKn)aR2;zcIG%h5_>0N!2A7j!$J4n(cEBPAz`NbI?F&U` z;i1`F=%}(U0Qy@NQ058_G8?JWJ>MS90u5%#kc)Odi8~E|8JP?!Vy+8)d>l#$4rD}W z^*WaMSi@ue@b^zLB7|TQpy3;M-@HxhNsjeJ4T?TQDb^}0yym@t+2v~_vb@O_--a`? z0TEHHEAfgIDO)|CgNwa(MXpR)9MvmHSG}q4a5Ocg^}l~?(4w0`=k?3R32#v@OMehf z__aL}RvWwrU&dK=fC8q30rY3pr3UaOA(?#c__`<&)#R}9xzh~K(LUB2{;Hdb(lK63 ztktvNJf#ZwGPkH7^_TcBXo?EQGliyxol~!YyC9Qx2@v#)%OTVxbHy5rI*C%0ndpES zll%S9Qihk7VMpD^2)Mt%Nk(1vChUaNjlTWOnB9MUDozloN{0GNuYS6hORR-uzXHU( zBY&s@fjJ7|9KN9U+gA ztm~Az6sltEPZh--nDUx8}3hiSh9JK?TxIk!GTMUg*aOZ|cimv0LW?ZGQEr8K^?DWb~@VX#y zY!C3)Tj5d(l`^EcbkZG&P z&8zCy>3xdS@pFQPgjFYZ#s5z80{%OKJhp;LQt;iRjIhtfQ)eWT(!{Q-Z}0QL2sc#F zxol_D7KmkM{(!ENG7#p_Ns?`_Hd*`+Q)eAi1s82`Qc^ku zq)WO(=>`EQ>27K1?(UNA?iQpwC8fJdx;x(Sd-G=AjQ-EP_ryMHuf2W)l`CQS)%63* z(N9hG3T~_4%eC72&40nqm_BEZq_F+@-aq*{=#ds%_2w8153&;zghuz(0$N-mO-fNvR&blJ@DY3V* zd^5UG?AK3N%#uPdVxP9*CP_(1;&MrHe2PDiQm)WwPgambB1}e-WFX#oclhtUoVKmb zw`rQ*dhEI0ED-FCq}-1zEU>M)LPMUj8ULnNE>n$HsaTQ(jEL^a@Q)^ATy@5`asp}N zkL4+BJNeelMici13R_fe9*6K-z6}usY#Ju}S`Hws6H}C!6sx&3jfkkb9BO!ZB(+f= z;8ZfEtzhh@F{1@?Lgo15ftDI0FqX+NE4VQn#pgb1Dqx>TU+dJ<53eH1ZN0L{p{UCB z>62L+kHf)H@gs~rp7~t4J_aoB<56AIf&15tg6zE)28iJnke{BGPYVW|I4*_3;u_t< zk)lPb!?rzATc#EiI_((2p;sdb$r})LIa!nA^@kB$-%+F-ZH*XxN1Xs>OFu8z?G%ji zoN|{e9nm4U1a?l-#2e=aeey5LD(kZn8viW8XZss#NH;sXYz`r1%K-B#wU}-st}lGr z&o=#QrQ!^{+g5*6EPlN?0#t6tnSp{-ELq@{YK`gm$%}BNA8p*S6Mzf80G48QuKB`Th^pBXY_^4&tk2iMrlg23^ z0#jyMdvWZvjhYFE!DLJ*X3EiG?(^{OXukU8uly>L6(rp^p7()>UY@do)3qp}q^8%z zCkaeAH1yw0uqNmR8zUOu?Mtt^d8I_QW6lTG;e3yNX^I9;1+YU$L^KN8X z1X}7xX_J!T*~lu@@_fH%_D6y4QViuu_Q9Z$90t|rZ<_%)ALKiqs$9bpO->Fj6>Ju2JX>VadBx(WS~^nBa`}7P zig3QgvwLlGuRICqb|-1TFTVDu_@3?kY;>O=xmVG&M?|h!||dq z{cd?5C#yE&|}xbT|B9uWyZNy)K>gq4JbYFyTL-eL`1a z4A-FfSVFJWC=UuBN-&4RVNf^q*}-oWj4I{FQVlKShv(fY=lfq*80Wx=XL<1p;&Yf` z1R-_fi|VU2G0e@2Nlily(0<>0k%7I6%4m^Hs?;9Ms`yxFdK$fL^)^%wyLZX2ei;eYf-fko1oos?k?=`3D2JS^WT ztP|P4lOWF)h=Mdk4-Y^FDsTa0iyXnr}$ zHeVB+8YLb4h+eoNS$ePo<)9X5nVa{jN=`42}Ij zCf&D0mgQ>?-ZxDeo1Bqn0*!|~%C{_UV>%okQ7#26qSQ-OoI%oY=ywli>|=3nK1GxL zy>^F`q|u%o=QJ*fl$3^^mSdyY2t2#(UMWMNH$pZYK`?@iKE=y-xtLcIK85({-sic2 z_%faa%}J}?F~{>S+9`#JD{0DSqX*SK&uFT{Kj=v#9WjqQBl!k}g`#hL+Ti6U!rgd5 zEowsFgkIVA7yO9nUT}?buQn#5ll5xe5*j<>(-Ta1NOjm>_3HM!k|jx>2R`f{FE%OZ%w(6LVFX1#Ey6pYoq*As(plGA?e7(K z5@JCFw1^Q|>{7I?IxPKewqIoYh?XA^&C0pqu>`(N7{OS3$ZA{4rSp<5aadaiMX$wpW7*y^^LhV?j<-YSp=d+j^)}U->5bO2P>A>NK2wr^lFQu&)ea7Nk$Q+4CLQ|+) zfq14WbZnMe?h5*30Xer4=?BF6ZzFl6NNOMU&xxvWArajaqT%SRyzZ}6Q`j9-+uqPa zRD?2Yqr`8*>d9Mr4BgJqzLw&9j40}lo56NH)!?07C)ZpoUyk!sJhI%(nO(xet?@xM z3al0@tQ@;6*$TrRfbg&TP$}wrmo=N>1L>{f0#Y@)k*2dYi8V$1O<7;5Q!!+jON(mZIiD_gE;`Y34^ME`Gh0!s zT=y_GH7>puEx^jy_{F2KtV-oLn)m%$<7zc1P?9uP5&oS{Ob36m3r1h=aw~ zF{G`&o)TbzD`*ON{kh?XXUPwCMH|?V%`f!h-{6bPIm|yx%a-?G+`3XUY^S;u5`QwP z?myL~2NfX~X>{gPjHcC^8MRoZJL6HetyE>G{71#_YC)xfP zb%I;jsPW8^IFWYthSi01LL;z3)J zY&oP#Lh3q|!_ercBpXg2O2w_oZYPm`q4qbcY;%p$l4Tzvax>&jb{EjmW3Qt5b_Z9u zkqB6eg^z;6Q4E?tmbgey5U_rWLGK_Nn1z;B>)UmsZQ)I2%&=4{Q~Qw&1IlacMu#DT zx907_hK*P_!=={I{S!X}TK4+~m4r~wufOy4UTZSjWHu4in(&)NGIidIO^&nBnaX#n>d?^n-qBUO zmtvZg0FtkUkgqR^$8xwgCAIbviHzpnM1zN{M#h6ghe?v3hYNJ$w(uoiABh{J z3P)0Q^1oy=VKGvquo@@sl&MH5S@30d9`Lm{|;3Kl!ZZeJ2VY?|dsmFLi{d(2$3rs+r2o($qY zM^j}JEliHi5X8%oC)LnMy559w40P6aJ#6-`#j4*LuaJ)2ZO5BfcUmtBk2aUoeB_8Vf%O&5{>PI%y+ahGiEu9$Ju{^mJM23BqwP)W|Mnhf z6G{AL;7vw=%jWZghvg=kq1ARTtL>)f-_VI z0$T=*FhTj_OVNo=v>8Tpo&*X~yV1)mDlOCjwM;UzN|NS>t9j$!z??o^siX= z^>F#WWK#<2L3S7~Ig!(Ts<@gRdb`zRwdA?cI|SPzq6Xpc_o2P%HeIoma^E+}*cd74 zKDAjtj@K1&1#gM$a=PFnh~%XziKM(z*S$eIIXp~H*EIY|$opMh?1qJkil}xLXYMXj zrOO)cpev}$0$Ydc`sr><$1yyEj*{JWAYdqdG^Yl^P30m(mAwg-b#e1WA~*l6hE4wy z3a-HjLNY=6%|i92Ie2nR_GTD^dmih`g=;<(0=8{mVoN0QwMh~4J%d^;dpA`~M*ldm zl`R*IjzefcGS)b89`?M&&uLiK3nM9G5W4DXB?(dGb#tU1zjCYONbFIdbq~?dtCQs9 zOccu4L?!yGXod}C{H(?MiH2VM*UXX}N8)4B0iy-pc4NHOl*Xe(0`uk7UnMFcXIze( zzw=GfYOzHcO}igPO8wgN3qb>`j}VJ#?z!5`L>1qpNBl-gWfbLqhA<=AUJA z$Ol`kH2aXf?Vbl&FMYWE0?%<8gZW=F#D5p7%?aUp>g)iyVc~Dwj!Fv<-A?9)q7XPq{a%;)F0+TF>=1y?<;!xAIOUQQZPr zkC0h`n6*eY!@voVSRV4>Z&4QKtUU-qMk?ds6goYap`)FxPq@ZsV+GSh=3NR4A&nbL z4YoaRwyY7$RtiT`c{VshCI+e`kdvw;B4R+~ztrMSGs3yo?nkv zjN;lO<%b_}b?D8D^e-(mmu16nSDZh@Z{9o3KY7(`;SZG8x~a=543|uzs1r!QHG!ly zJDrL?@o4+XYP^)lmP<*8OfTx`B^1)CFl;79d)y~3em2gN8sD`qk)HhWd0EM`kFoI< zU;acsN1ZmX1qp=A5H{(ptUXoWOiGCL%saqPO*nWW9+aPRR%x|UT(Zl=w)ByMJzS<( z5Vl^ZQQS3_hwtD_(7TmKg|-Ao6|?-ti$xF(+2!xWjv^R`6Xc{MnX{fNtbZy#anNkA z{4>)1db;{Prhk4r7mnTf9g$J$-+;Bxutqp*=pi&gnWvQp8lxN>P6Kl0&v5bH$y+=n zg8NlkIerUQ|Ae;q<`YF-n*FALP`LYldm&j!b*|R}!2yLhKdtu*!-@621ob`(jn20< zAxWG@uVegsp$e<+U=D<19f_5e>%%!|?d$2T-IsX$mq1FBq^06J(c1{rjmGa?P&(*W zWYlv%cZguhNiULbNxnucz=NwqxA&_&vC; z58iA=Z_Lw$@0*>lujqf}o}Ir4yS^v8t=MWg{S8t*K3qw#?PFb~Uzt^VgHPMXD-QCo zk~s1C^+TyR?GA36e!=j8`svjJZ4QTQq@NCd?kU50l_d+e2sgmMO{b>?i%CJT`Gw$i z;jhechD5G=fAH|Jl6svaP>~{st+r!06GMVr7HpnQy{#$dmMe59cZS<7H!n!}b!Gan zJ^c{a4Lc6e)4nGLDJ<2Q%B358aiibQ359q2s6NW3Um)=-cOH&`QKIp}H&nVUGZQ+g zGur(&PD-h0XxMnzO+#BgBN~Q0gy}BXqu7*?SPQom!%?u9n67F+C)G z{B%Zu``LfKamIYw!O{-d8F%xKI{3vi!BGTRx%fHASIiXzs6#K~yK8wRBVy}%-4~Sq zfr-g~k)MQ&kjO zdQ$Uk`cmuT0FfQOr@{M`|LshC&(o;#zwalnjmW-b!QZhABKHB}a@V)+ zXq^TXCNrh@s2tdVFYkf^{4)7YR2w$=L9te|h5%2<4Bm3;1$ajp$@P;ffi5z(-%_xc zZXOxlMHed+)NcRjH6oXYLhj#v%pXNds>VZ$e=L+^w4D09k|6=4nV5jHYt`XlTUmLH zM{mdcY`gw7kTOCGgl-(Qy_tz#w~at9)p!fCN}}%F<;T6u>9Vy>ih+QF6;|2#Cev|i z^>*`msmZ<4>;ah6KWj0=9nUJDxnZ0yxMShd5Bpd9Rq*l}_G727aSF@u=Akr|QM8MA!Ln1f+|$ z-IhPAG+*Sh@d)3kiOiO3^ufD=NzRZ<>QM_nA`PFwM^cXQbQX&S3h7MVvB=F(3O#42 zkTmD3K?PX`SK1n2vF-S<#U^o`pQ#&SF016b-mwt-RCKqit1Jyru*t_FVMBP`H+(k(!Rh{XwX=}LN zjS<@kVnlX!l5`UjX^%K;{=~UJ36e})!taF@!+G9}q)b+g57dgnF8IMB_1}-3m1e`0 z71ZKOFgz;B4V_gr1(bg2{nOID`NeHPBCuLp!Mj%r+PCFi^_9>|oyym}Y-Nb%4 z{Ubr_NI_=A7bK=~hcUBu=50f4F-pGTel@{ejLz`aVZu`0n`Oh+XT zw>R}F-$i*sLrWnY+WZmL`02J3-*B#lQBCL@NiaLilllh_PfI_rXY&)J(Gy7a zOOeU=C=Dpi2M?jVaWH;9f|@I?FSe&dm`ay514KxEIB*T2SXWTPS*p?C@X0)M?)@$s zbTdSqHF(r?NkO*-*Mhd% zq{-(F8`Cz{P`qFM7O~!@EnEQhxYQ@v>c3Lw&Dp{I9?#cv;%7bytKnkdZj=0((ohYg z-vQpn&{yf8K#|pH@smy7z+x?C0=PZJ(t@wwkt4V;ux1Tsr-n5qm`+lq>R&vo7 zdMAwPtA8;Zi=2b~U4O1voM;6;lXe@`Is1!C7)9ug93N+W|F?F#Q=R!5=S2F%mLx{0 z?uTNoLPN~(r>l6aEH1gYo1^>Jfzs@|B5TQ_haAVd5=vNcPAcB3kKa#+>pUa*C=rfrO@q6)tf(TCq@i@J$!9;Eo zz0vvy*Nt!C{LFh8UO!^5=qq{w11 zIc+x9Q}30*o4a5dU*>ZYZ5@$bqN%zWiO=A?YDFDgioGO4qx5@bD1N6j|QYVW?H=67%ch08cHg zt=_yR5wNfy(?J4z`5R(^l*mWG9%$Bu90$iAwfw2~>(r?y3&AseB|2bgN1xCBeGuX* zsYrv0qC>l$Zrtd9czZEXtf0TrvGk%XmEVu@%Yo4|yZi9=wNgJ5W3}0sevt^Djq_;U zJ>%17vQcg?I(^~Tu!NIW&#gDEqOiFHhqHZdM*^u6EV0dkX>J3Nudi-vhh(Ab?#Hs& z$wbA9*Dq@wOZ2G3ydStY_>HCq_{#@Wn!Jy0%>#=W_vbkJJz8URhXu^lLoUYQPNn@! zGh;PT`{LnBRk^&~^$t}+Z!hI8Lc(I^td||8t*SrQ2aFLe4ukQRoq{Bti_dGGRQoxO z%+w%vzxEA+9E~CMmu=j3)u4!kyZBD+mHYka_O>z8G^4^3HIjGi;d?O5p^kqi`;1x* z%4Nr0Yu~(Lc8+O0H#<_~ov@Fk@u*@@tNen-NkWjHu*btjy@Cb{`$UA_`=xbAFB)if z)4u}<@K4yb@w%k5PX~yHfa@+Yc|i2tN#s@$J`ZvYw|+qqowN1r7nGa4nTfw$Yu-uj zW>aCsVwuV>3cnVDU+zRsNE!cj2q)jKUD)jSs_KTXr#WY0#axXRZdtVzd< zQz(~)T$h%euF#`LN7k+mCsD2vn~v6CwCnuDbu3lZQ**zb55AgrkBv%5pxgGf+aYh@ zN_E=_CKjDo`+oiA@vMLLr6(A5D6JV?UWY-MPc{JkMS~yv>O06YN0Gu-ww@}X*?q_H zR|bryTYB!iN_FDEoRoxKJ_3b7yG{d8L&!iN|1Yr?;R193Qb+SI)N zLZGpJserTNjLFwnI%yCNG)94?pL5oP{sg`A0g>_X@#Z)ludXBZD+-3gGWm{A9!F*K zNeEAN9vPSQa?B!!g(PBReUm&FG?D8zde!yP9;jy?T4-C)t}{n|NVTiPEA$gzF5cCU z5u*#H2crty3ASrDe5ERNGy^1MPX|gjoXI? zJzXRR|La$0HUC->NON}BHVEH6<2(u=3!iZHtH~lx1F#leiHh5}1&%Zh2D~v;M;Z2X z+G`oZTaIOM8(FAM3@#`@Xk^K0QY;f&xRZ*X`EcCsPKAUJK{@pxHX-8g{imF2X2#~B1U$+YY}!gbyJ5l~l0l-~6|2grXQ%j_Wrsmj(zlZ{gNX*b zV!TZLSs6)eI^~;{Ks7c57s>T>lP5o8qS|M#lZe-vV!GYg=jXmx-?bZT? zo*~p0&<%4$-VAf4zP*EuOxQo(`cPv;j1b0of=`{p4qIhb4Fd!6T3Hs6}w z3{W>W)@`M%0+YZvTcKotYP*MPQ(2|Ml*w2wb(AT$Yr|WFPFXCx<7lFf2K1JbBr15} z12DTK=b{9%5zKn)B(6=C>d}XWuoQY+P&^GbT=oNS2luxP{ZY4K=VjT!D4mK`I&E$j z-773P64Dc;1ee%daSuu$gZR zePtY>ixs4YiF(gZZ}aOLDbOoo!T&#_vq;X!uzXB{M%X&!;P1BoA@OzJ<$6YyQnhlK zRTO%43Uk1|u)K>1hGf@qP9}qn1F~OB)=#-up46S4UR2``C?*V>U<|YN*xl*C>Ol0> zRlapAGsC1&$PI{4pySiU?m%+ehhI)Sut3hOOC5#H)rorl(PKtnRvL;Cy_0XDQr6;L zsjfjbi(lJr7f`}oc?Vly03C&r;VU&rz{kRc}N0p^%Tn} zu=RKN{!(@9qksyCGN!zr4s26b*u10VCMEN^N(R@9U5;@3Ft^o>-x^R7ofwxY9*-3f z1YBQ!Plk;g`r85kv+vT2o9Ak7@t$LAIkO-PJqwd=y9cY|!PhJ=;y0ob&~t#+g@>t0 zzh3jbD8gi}Yw>;B%lhnSzVr$$M0=>>pdAHjm8e{fw1}%O}3dwaB zDklht@F6$zKKG}27V-lC66!=5H6VB|yk1zC=q*=q(ctmg<^g_H#S%rz;HzctmHg~I zf!S8Yqkjk6*kA_vhl3`O@PWlxh+f`+jjm6=iLNhrq+xC^8zW_Ri9Q{DyqfTC-(Xl_ z>!%vko9y;t+wnyX5ljXzjx+ifcK|hIQWcYhUb9*BA7{|@wCz^1&jd!VZ%L!VMYU3m zbad5CT9<($(*)O;6jB%CP9dJFtM4W#%#AsgNv%4YiD_@X8R+Lu`&J9Kh;FOIIMUO@ zX0S7FVQ@?7YR$o)MKyNl8ZSuF%ew^rEOY+hx&09QPwa;aaMB$zY5rjM{02fIIFmMce2f z+W3t2nWYjw)zcN7zr%ok2Wd&wF;|jjx%x9S(G?OOR(cvNos6r8eZvy@8Ur7V?%_C} zLl_YBmu)XWO_Qx-+#Z#J+oTNX-1K%WyTd`~+BS=y^$YG9eArrxoNhmx{WJe&Yz#wfgC})S%cQB+|SGH#83`!ghPS zWJ-)FCtP`sb>+d8-|>8tHeIGF2MV?_QuZLJF|8&K zXO>wRC?Yg&_uYt}v&oBl>}FfQ*0}9zqIK<4Xj%jqI;w8BCk*q`HWS4)KP}cW$DcDv zcNX|5m`$cX;N8riVdk0+#*li+Wt-9ojc>L}Ig0`NJGu9>qvk3ADd~7t_@M^-DD(J2 zEbpUX9!~a!PbwMik`A*mZ^5 zs#}FblABoBC-gCA6natAgGV{EilQ`QZ8BNmliPiFmHmg$Us; zpmU6nCm*;4C|2^Ke}ja}olxq`vmr;d5_cHD`!(w(oxr~cOiG(?0(+4Zv=kqe=@ z+ikA5w`Vs?HUz`GIIrknef_^pIYquM{@+`T!y=JgHT+Sf+4i*)8r4OEVVoK=8et@z zUz`8+XGiPRr1V9-NrPN6bIyQcEz~LcJRsV$f141*p!`BFA`$U}I?H3yJ)w>YBZS;n zU^?zsMx_KgqctF;Dz>_ubI)$yn>Aoxm4n)!PqVo!fry?4?cJ+veYxA+h6L~FM+MWZ^M3mTwswp!O2vIBxKqu8F`4XVq#==y%&gDY^AK zr%6vJ5a7&+q?Ku5=}WS+_Ex?B}uI-d%$d`Wso-oGlHx(tKhnCU{6 zSFtqG4;uub)nF=q8Z*{r0!`b|Ly1Zm*RrIt>lkNspb8_#*# zDq``Q`};PHEwvAfn{M>`oJ{Yng5+Iy!D1pF%g?ZZ*{1qH8P6K6B+@#$D1F?1TxpiJ z4r90Npc6TgfvF=ryEm=<3{rfEuIJZtqQxqk~coT{D3hDhsfqA8$see4QGk8uiaD__c4B-A>O91z z`<=`r2sE-H%C@q2Uy+r zNnBMpY~Z}QX%s;t+JV;ou?8-}Rr7_<2V9olqJwI%g{1X-spKBb2lbi?ho;fO;L={{ zbHpHOZfZ!Y0o2)5DBifWD6t**bx|t5)N*-I>UFz#-jtM2$`DEXt#*u z`5y(!!N2GCm`|4IserSd=lI%d2FiV~ll!UX3=3W;eFE2h4JZEo*Q{r3s2&XDuoiRd z%BG9-9_Y>}Q<$(1#)-b>03WZN>d5W6dba0sg(y-rK-A@!eT=C4usX(v2d3fq%>U|i zPPN(zWWNP?yxi|K*vH!!3Gi|{L3uu0vEWI7kTePoi#Gi@6FrfvKo~7bpEKDBd@9*@ zOha*$@s-*wsi)*ENipbG^b?Xo{LUv@bw0tc;*615v!a1~Y9|YSL?=(ULI@Xs89s92 zRY4wcS%@ozFps8jaZyzPcx;Wx<$MqcsShD9{0ks}qK{x!naT7Oj;_UltBqCCtSfuO!2=pmbR3FIG=$-c}D_X8SzCknn)_H9*osH@~-s?|E_SrDOG|M)A>}#RVLo#t9*DQ?M$gXq|ICli&hIW3zJ5pVB)v;Ki6gs-k4N|_`}_5 zeEBWJ?89kIrga&+;gGK3Kx+0`f7C1DT{3h^ME!rtK(v1R=0g@=W=bZkvMq~B;GZ(tf zBj`2&`9HFP_GL{UhOta}mYHKW`Ek_JFkwbaoB7Z@8M{}-5Wy2ljo2Ch;HhL&jWUt+ z|2_gy9-eixP&(<$qFIDv?_8d(-2NKU$<6o3W^?kteKm#>vQV;?Q1c9b#VkD$)rFWm z2F`t$?RRC=tMz)H-$D)IUY=N9@y@41^VgiN82)A$N1^2 zOB@0fSy2(wARSmAT?{*SW@#k@xPVm$b6`J+O31xVhtE(y`izc#jOsg3>r*(5Ya^70 z>D3lG!{O3A0EE?!^BvqZkv+75O|D*4XY(K0f7N!))k?c(%l2TbNOMVnP5aY5p=hvn zWK<`A8`p!!w5HeOw#)r>rl4>iur_gi@ck$}Q78T;p5N)A+GnlNHc!1bVImlgemxwU zN#D!#T)-DxxbM(yX2P(c)~-Eh8x3-RGTG1+|^GJ4cA)?wf)#J*P5FFFL;!Z zQ#d<6#N+kUxbWy8!d@A&2_U(uysr_}BO$SF}GtZR}vfp7@Rl7OSboD-XDYssj zgW(Ofamm-0XXq(~V4VcNOcA)%!j-~o+&kZ{bRV@}A>mSxk#b<_lqI}4N!+| zVA4__JkLy?AMmA9Qhq}V_`O&`o&+v5XQpvFhCR8yw0NyrNz4JZiAysSH3Wom#0DHljB7GRI`R1A4L6Hwt0Q$2W(xt47 zgt)u;L5S%|P#+F0+_@Fah7?3J?)q!D8qv_`v^M7J<-%sI%mF+F6e=Fgx&DI)gOMH~ zG$g994hTeQ^*X+MFpd`EF(@Wg<3~ae6@#dJ05%DhT!GrHO`XwPl?=+yKc<^<@_F-k zxUgP4olnzM^c{h)`iaX@9^EEQ53>-Uoy2>4dOkr-yR~#)Hv>r1#gv?Y6dcdW@_tcQ6vdo)iO{g}uJ9eMNN&WQmMv9?FN_OK zVSwg|@oM};e?uKXOnu3r^NIfA%+lwsX+-4|?vAqsMzpooco;EmO`ja%4S{SLX95wQ zo9Ke#dw)~+-nkw&-J7EYj(8_>Or^AbB2EG=CN4O`==@YnN4EW6y51ayYk=Urb&QE< zc&Y}zJ5N8<>(8>%$k#;YH)kY}_RE{iAxsLT`P(CZJ$;9UZ%dBLcw9FF$~9YN>8G7f z<{7_d4aQN1x8@FgK6BI2e(y=R2c>RKG9lP&OzJxi-${vk&dtnJu2KKHcgx3pJUq3% zH30E7ncy2X8bM$3YqYE0+*n4f)JHI=Ar!SQf(J8&>u`&82}OV5kwHadi=}Ul3S6*y z`sb+NM*62cG>U^t%Oc&wb{cfONyV<+vByn6pk1H6Z})n3tJ@DNA&Ed)J_bW%M_wG#FB=R?J^L;P zOD@(=C*K_>+=+gj6eJ8piSaqXBVl=nSJ-E=oHku&5oD({(vh0D_)x4IqxOF|A*N zvpjj#L`9)FTf>Y0a->dCzTy@*9e2|p7_Z%bY@KNHT-OixR#fk*8BzD(((l4~4~Pl3 z6838JL9za5Uq60|&CfR7`(vxK^kE;?WaNAW5veTWp&Z_VZlohI8}3%q@}yBwuf%_J z5jtSbY(Z|eJckf=z6M`XF3>};wh%Lj-z5$e6< zK4h%4AL|95SIcVtmWw?^o9~2lo4 zzR>!N%HN}XT};*Y-RP)vw6ZsRQ7*BT+t)XTH{C(O>oK0J`W=Y8d~@oqmxDQ}30Nr~ zdYFD!WRZ@MZdLHW?vbf*(%V!^L64&b3>RxuG}o*e4k9~$d$X(d4%TvJf*S37wnE!b z*rpRA8%*^o=7~qfe2+##UYKY)0EM}X3q5>$qi(t1xHouC^spMWOd0l0_NTJ}7YaE9 zX@ka2=}dlDzl!ugW5h`|@_bG8q|Q&hcB$Op@_p392PDRNP=CkDq_D!4Yrixy!}4)` zApYbq$V;*wxFV|dx6>FIwncp53W?hNrikKkN>2KwD09*UMP1}~94gi5ozMHbXXlUK z*PZXRal7Cd-Gt-e)ll{BCIdS|T^wCVi88CS%~swClm8~$fJ8!Y35Ou!+$bgXvgXcV z-2H&|iatfH*>H66(`4>z6ifexP1MD%e>|YbtvN19@Cv} zinDy$?rKq2L^9%De(VQU{`>q2;4nZ-|KvJJKc=q$@Pl3-9nO4g4Xa^f(*32TG_tqa z>PLn$~Jo3Uq~4{ zUNeBVn#0Of^7m!BPS0vB2HFbC8)gWXzXeGV%5PaG$Xob;>`(2J>^ca5Qoh;2O1^ zB(SbF-KfRviL_+(>w|`1n2aU@2jN(dQ5jWy4I*TkU4mPzFSduS_kQ3T>AqH;?%L4A zNp}Z?Mo0$XZhqcanek)#e8j%om{WDg6+v*6K$o<0Q7~-x_kyy?Wg}N(LMu+1mY<~h ztRVd1vLM`qdWsh9n6)erT}Ju5dtB1QHw!PRcP4h-mVyL|OWT4mgqKS#(bC1G&r~3A z4DS1yY^l1oQtJKs%TtW-UNG=l1dk%jw#LZs!8#LpkTM$ZHxST7U)t=# zc@DJtIBB=pDKl!_O?l`C;Zm}o>hV|a!aYSX{V8(11YINpu-KV6g`YYEK!f&@+A8*R zIxZIoR{H1P$Xk}Xl}nwD7cYRz)vaE?n(9}%O@sA{InHY!B6cEf7 zcX{_97>zo#h^HD_(%;+%PKY_RP0}35Ih^(#;*t8E2gH+jY31G;PN*|3x*Mv8psar) zu`s`DUFSEr?p4q%gh{(O``MMQP%dgK+kM%pd1!CbFihj9pE=*W<8Byu3*57IPj@VA z{fXF2aTcEz!jk+lvjhH2vm^D#pf4g#n{`FY@H_&d z(OThl@RZZ)EBYyq_G>yTLCimkWoSjD-gWz!$*DT3OT!Ah;D|pDJ%39`3f`*T8>^L{ zX7Mci=VC$^L@w*)I&F(uug=$sLY7}B2Iw?elT!rjid7LoL;t3X`SIBq?UgHyBJCZh z@%2_akrw{c(DdXAH+Y%7h@jn&K_Gn$ajy>XwSdraKktsTY}+z@LOo5^(kqNg!*5BQ zMe5M>?%@fbD_)r0T-Is3v9XA4ttYan7ZdOW;; zOMsqS#cb1g@h%PHtbG`OmA2>R|G4ZZ<9a*u9<}ySCLcg@jo_M`%-1coiokU>!!C?f zG(qc+sJ%%0_z|5V4q#MoGLz(?_wwOjkU(NUK*CH0?nkf8{lLff?R)&EVrJ1;MDT!* zW^0NYW=h5zM2XgkG5FgkOQuIpaDDi-sxwJKTdeJP5m$y+U+ruv`WAP?{cGn^i*t0) zWs_dBPh%6f#5utl-vTNlN?~P91t+ynJ{Plz&_&-FEKbf{>l-pWC{Y@<_nNcri96Z# zA7gaI_$`qIx9uNxA$I24a?D8j&eGzfU2FmC6cnBx08u+0+p=i_5)V=^B>kIpx@4&kxzsd3fX28aE^Zidh zp}~<5k=RpVXl(?d;U)TG3M&}>LE}i7=|f$x-#7?f1Ck21M@D2}-}gxIqZV>})c3RD z!KiDm_xV9+e%FME1{UN9Dqh&U=TRd!Kf94qG&`LLeb@byM}htIr}k3$8=;B1-M6h< z#(lZ{rqGwY&-cQ6k>1{3!Xk?SVFWbgdRa zI1rZi1-B`GO4LCuzep-xT=9_3YEgnxc9ESb(aIpwnNT3JzspNa_kjqNrS=;8g)p?L z-0$I4yxc0X#1rnk0bO^g^>FRwLG$(Hmq~pG&E6~a5oq$5dWnym2#mexVSjt*(~>3r7{mHydjeN7pLik7uW|<5z;V&#T+9H&o1KlN99E^)_p(t8d@x9g-Z0 zai|8o5*YZ1bEt~GX#P)oUmgy1`~5%0s2Pee_I)sxO183ZV<%}*L?p>lk)>o`W@ODa zwo;bR^3+4t#8b$=glI!04I*V zI_EBl)W;H+DE7C)?a6Zs!@lAapRWl5D$3N^p^x2q$XXF+-@D#!pC9dT9;^=FT&wXi zgSA8(SPda`yy_^$-fM_=trfJyY5ifp=b&cqNB{~)+xj6d!sO#>@ZT zdovU62`i!=LBIm@+ewsxKm@k3$^r1q?gGG5^cYY@$BH910n_>>SP$^S;gT{7#D2`i zkO8DSL;8H|$J#=7&~cu_lE5s7uRd;keSJOyWl#fOZUz^oY8J(D!E!~gQXyE=4ru!> zXr#P1yL6VJ-rzQM;gSLY*5H`{Cbk8?hfQJO(&&rPXDp>I@G3MhirQI7)6N6IoPP!j zhKu?DrZ$qT`yV-{pMXtTyOzll7YTmp=K-;{x7Q+9fLNIjX%6oea#<8P6L<>@axF7b z*s(a#cbH$<9j>)M>uOT>5wUxj(i1E;=$MCokoeejB$wQ_lq@99K=8U<2YW^W+aUQw zIPSlq$TOE~eZHlxGdMe54)z9SO-FI&*$6g*AQnq-&<(Y_9ID1 zjKLG3PF&0{lykx@PJKKP#U>()u<&56k?uP06<|e|u@1|;rOLp$I#2js(sT%*#bmsH z#Bbze%ew9PhnIUSRqF8dwVw+?%jm)cj0y2BuFg^i(ubm-jC~U^)rPP;l_q7v#(47e zdq5FuLn!UOiPnf9(Sf@#KNvLA{`mZxyU;E3b!|wHUM#X?gJMX4u6y;40)#CdLjDM0 z^{l3s8zGy#`2*}TVNVQL+PWBa@wNlNL{c|ky5?omcd5>$Ha2cpVh^w(_oh`D>aij@EzBhlopoB~7hWs2+XY>4xLu%?k zG^@~EXnFHG-;iaKHb+JyIrVOx%qBG)hvfdDctw}a@tDx^R1*Cnq8ZH7eyCkX`TZrN zu{f!UYcg?wjN$x?J;gK&RrqIrjm?rtD3aBq5jk<%Zy;Qq7-XoK3hSVNp)I0s7DEA$ z3@|2%LBFPI1ZIQMbcMslt?ew%WQ>9p3t>g`?2s>`_W94DU ze2K%IyeXpkQ6^9;7Gr`e<3$H47K<5^=C7vD?trPc68ftH z$~pH~b(XHXw*Jq$cxy6fzHNB-rCaBB8c2#6+yW-l9f5*PGe9olP32kz&GowYw9Z4Q zy&EY7?_q7P!yXheSYo+r_bu`g@zBj7CE@&C1We!))Pt-9Z44hx!ObyH;{3?a|jpGF^^Ty#_3H_{wutO=`Z4?40t zPTq#=kD{+Kqr**9k{kC^nVGbt5}vHD9?+E=Q@Ew#nN$6HW7VR}$&^p&LfZQFw7n#F zb7^YW#!ut1m)UNb>B7OYx_rv+8NkXQoLw)8qa{$@_ko=|xUOx{DGtE-F0w^4eOKT-Cuw{gsn+5 z`p(W&LlYU!{Z3@6`OVr_;nvr=&FSys55GYEBn84IOz$$vO5qEr5|o4ea1|??ySV=q zf)@(30V}VQ?<;K#oL)&J@{+0I$sd;m7Y6tLqa`F}(mGGCq1&q6prsYaP0 z6-o@4oR(>B^kFIUTW@7a8=|V3{GX9@?+NvovQ3po&+%z%Pz+ zj7iIyiA_Y?&L~NEZ@1QRRxp$Wr|dc=S=m450Ega{APtHHs^*(XFYCJz;6R0<4mtUO z087FO8-o*7kA5CQYd#e4ptWasF9;5uADCW0fjUbHU0ZSSz(%@PA%$Tqvs(unDzxpduXhZ%fHP#Kgry%Mh5ec=-gz?N6(N2xme}bZ3Q0MkB0;!)e%^NeyLpKAZv3Yi3aErcPMx z4nfr;w41;ov67sSh}0UCCehu|F_B838TizF9;^Y^O7i@+0msR#M=9YsEGU48P(rMt!g?E2TuxTyQK9!Vf!Gc_Lsr+ zN7PjTAH&8wSuzwNV9MvYG6B-zP0;uyxM4*G?9aaUd!TQoVB1)e|3&7&Cqn$j$}%w6 zo)bkTaACHfTF&HtSTHw?tX&lmYMm4^!@*eGwSDB)@6Ua02MF*;iODFF_!*Dfl(ZgL)o3WXg z1ah*NAx0n!`}qy1Dy#sdG`r(BV5R;zm{(B3c3XY4Fnishk6&+x$%24Yg6GiM)UPiO z+-o@R9OV1zn6}TAp10|`!m(G-H>WnY%NVu`dhLL&=8wCDSO!?-uKYD1JX8o+J&6X< zzmMrHKJA&qM36Y(^oG042Ih{a+^@TL2^*tz_3A!U7|ZOvn=PUfTAjA%gsjQQZkH4k zZY2d3Q3D964JfEi5Isy$aKG6z|pD($Ygt2L}w}m={R1(OJ zU1?4SBm8T45|k>?1@=A3z-Wq5RnE6>2})>#uG%cNcQ1zS{ORPEWU+=eUil9j+f5F1fP5pFq0(DP4FgpXmv} zE!gf^%K}VIFRKo^z)6_j+JB~I0P4dum-^49z-agxwP(iQGlIDKi_j>HIC4KYXSd>o zwLUbxw|(lf>#UpVthU1qQ{rwkwo2#1Tj`%e(IOa@tB`2)7vf~nDi(K+6vmgN=02a(fiH|8Qo(__8KDaQ=js&Acy>$q;%+41SU*^1hDaU-LA!dtvd-;a1!ZrW(~VBc|G7^uxNI3u9f1ZoM+~ zE+3#_FTCw;$(NSEh7EqJVPysyYB%QB@{FgZJ6?*dc(&+C-fQEft8j9(U{wc3n(vs_ zv190$qW3TFu`0ycR{H^L1KaW~W&^wiGWsZoYS2<)skC0n#+$zmXIck&t%9#ZwQU0) zcVyi5)eMRblNKpUH@E<>9-rHZr|*9Gd)a8=FuSM_fMc<9D{c4Sis*!Hn1-~am@Xu~ zfbfWGb6=>N5)H5pbFAeXNIg8nGCtWqj>c~Dg?A$|qwyQhSAk>L`zZ5cFbtItFSSCW z6k}=-G9*HCe7pwm-l*p+;vP}kFhu&$O@T}o(1ePy-q7Yt?+Cm*3BMPHs#XID8<0d* z4!}9%^&5b^$6dL@2mQ|zN*Li7vbG^00z#*ke$G2{#FayK=ry<(*seRd zuFW*=ji#4EG zlXxTwV~5}hiG!K|4&Z3$tj%66)uY(5`*K2^wJg9IDH%{&(HvDl%SGO;krO?@!iP%o zMjn8=&vo5^fjo3Z{XB!X%fIIrjp(v9()wY)WSiBD**( zD2*mLK=Q;EuzQ#z(;0=+`3AV@{qx$0y%G0X|8-^8iJ~lUw@8zoAf-0|1Sb=4m2D$ zT)Tx+Z!T5z+3(Aff>I{F7|DfOu#vh34$x1-Wkf(!ntrX@b%@BXsho~+hY&|j*Y=n$ zajJtP2}DR*#9*EvT(ckiE$2MnyVz%Yf6cZMyak}{3E@Y)x~e(aBix= zq8ceWzu-hW^)sDjrw~C=SRjtn&m7$8zfTl<6zV<#D|q5g9r$2X*m%BuEu_Z!wPf&E zcb%@Sethng9z@i*?1av^A)%@3VB~2+Cw=2Fz+_T%7(T=N9>bm?sBs4|7ul?3QY5-O z+M9hbUoRr^-P0=*7@R+GT-5hy0>4sxGZzakOEbWNc8Xx71T9OH^;0d$WbEAu(0P`>;xSb;qRK{HR2tql8h~xcEhj9i^h7C-%54hpy@E@5==kj-R zE^112FPIWlado4@-2VPgBGP3CSvaNfS=na_5KwChV~R&-XG>D)k|jW&f;lE~%G#+G zii4R~Wch{QP`Fbi{Ml7^EnI{ugD3QO$>UdWi~vVuz*}GrAJNVb z&hX6&q4sz*{d1`j0zNl*lceCAy`Y=r`M#oTq?wfu#_H2bsQ*HjiUg3L>p!VI$pKS% z6k=^^7pmUd5U{7v)+KDb@?=m$JR=7zAxMJgt?IcJfxM6Bm*O$uX^6qIUVaH@wdv4R zrW1gZ@4@kfSG_^J*XnYW3_)vWh`w(#Lb_0AC9PO_{p0&TZzGB_`-Fs_%!LYw2lWB6 zb$yoilYeM98!u&xM17M1Ef2DT0unVuv0W_bC-)9K|Gi-g&K@Bz4D`ug&?qJ9Kw$W6R=ASt*NGUR-3UUMB^eD5Nepd-Ze=UQw-Y(wyj7_KSRA_!D6F7TB?lV0(aiQci#ZDpta zKE5jem?z^o0=bzU5c!wnm;zlr3Z*tGkH$YetK9F_+bOjdD#SPn3)3)YJ%|31Gf3oa z-;KruZaJ5GJKK)T=EIVNBIy=3S^y4SB?wP>hGbddE4DXndRV~VjBjRDbSi^WzYG&4=Km41RJZSfumn{)V$HbkJH z*FL9a5}kyuCypB`3CFa@no}T+gFyE#(OMs-ofSc7)3}nA+A9;+k#KHPMFUH$49GZ7 z$us~Y-az{d%*K{cT9q0503z8AxkO>#PGAv@g4-_XegFWdNfKhm1d+O%5l;p*Ag-81 z6!%mK(FTevy;sW|-G}_G)9WIa|M{Ae3b97K7#K7GOt@ta%V)!?kc;ojnn`Y0k=y5? zF3)JPae%^87?ORQ*lSbd1=NOIhr&nUzeC_CN13f)c&KCgx^g|@brTxdC<1rA2HJZc zJ1Y~@5Nk;+I_})2kNRJk7;hq~APa60xw&sBJeuC&^&EP%9T2=QvWrBQLFd7;s5k;z zSBmR_>rHOy6Yc7EKZ4R_QLQ$}XmBqgc4+@PoT$I5mp-_)G_*`dg8DR$nPQPZ*Dk72DdiT)ZXGZB@ua5db69ej#SGWMlj&G*UjX*wG#jn9LFx*YVrETjb#d-vD7+%(aZfkP!n4#<2^164&G~a-`?iY}TE_kn}utal^3c-}#q2FhAvu~mS8n^7@Z#9+$Ii3#diEm5%J;@1A#a`Xo5wK2+oCM;a(NEiX|IKj z9S{)gT<8KZ9tAeT*lXCtREQJ&D(9;PJBY%mf{oT{W!2I+1cwca_OdZcA;sdB+|&IA z1B^jb&lWD>?Q|e^<02(sGhl53sT43q7mtIj@{qAD49~EDkT~bsCjn;&<0%HOc$M=$ zhl1gUgkB|Z_kE5lvOzUYo^qH6(CozC8k8r?Hufpiwq6 zZ6a>=vB*KoCwVQ5nWrl8c)K_}T7bFpyJbKQB3g*;#C6C|^Ql}1*C&O9!yuJI(>q1D z-;~hx=_??~cDIz=xUCq*5&-h?`T-PQ$lS@`&Yw$66p@+s(tc@(|8?VZsM_QJ^aSic z(APhzY~g1hl=;cc4l8J`D~H*Zd}?+#6)cezZG2ldsf1*NAVBy2lKO(aGtP)wBBTH0 zjiCnwJcNptc#jy>=fBxq@Y&%lnSj zs}N%waA(ck3=KfQ_59lh$$-3Cf_T`HZ%L0wwvo#OWH~7PNeKq-oRgoQpT_sG&V@WG z^PfYADRHH>jYeWkp{Jj&Jsa>6-pnzR@KnUs;*poblQ6UN1Ooy8)~l++3w!OV7gVse-t#8?&=OvJ` zNaDeEe+Mj#nF4BxcPkuqj5FYZAYtsQaF?I+qh)S=vinr5J$y>~^~G2V5P)b^Y!4v? zCxmyb*7EBHeATjeQi%>jCm-k#`P-g*-m{)em9XLcg00|-vmStEHHNUj$0h!H;sV!F`jq>E8|;G9Dqqb_P%d1#+|lr*<*$2Kra zhFVy!yh<6bM<8JD+Y- zJKCe~$XxYYx1>>{&;^>Lv7edzQ0eI@Gc$YhYP#TnBm15=pP6)L_p*pQUWSw_o7rRP zljDz0TFNIy@USv-?RJt04TrQNOUqG~E@k~fW99d4LyDlBoHsmh>n2(6pa;i8@Sx6x zI~2p=g`fY=X{TVajB^1fgxB?#441xuua^T?GXcfO5+*Zwo`GH+L@Lr^HPmGj<8AVe z`2ws39;}@epH|@G#qaMr)|lh?%&;Ihr|sCcEoxT%0HWomrnw0E_JTY}VqhKpPckVy z#iLOTBxOb!x4cy z3MpPbdkke&JH$fx8Ns7*;?YUxmlXy`81t79I{+eawx2AH+T?))9a_D=Sf@ZZBI17_ zEvJ!s;e#tLZz&b9PwloU53sTw zPyiK6}{ibcs) z&!R=6@C;$tK_trf@vynxAdb<-B-4tAB2MIq2N1fg4TGP0$RBb3Y{5E8Ds?vqs zk*h)s`v zwRBVd)g#e7W%r^gs}T+WfSfTM9$Uy;!l+V{Gi{waPE^f~;8^&qyGvBBqV!PY$G~9> zr@J~aT?B_L;^pttL4Jjo-_UpNJBLE;_-JaNdz{eB5WIlOA^RAlpbMiT+*VcW0hfaB5uW(+!;*OeFf+!D%~{ zqZRbz=PGebbNbudkBvOS*Y4Y&C#&L43utJbl|B1)8MIPm#jps=a7czi->alJ6ayNA z_2I#fU%4$hB2K2DsHX`rtr8=R?*omEFovZCu6B^*wuSuMto}M&>NbfvF!y>Ms`uS} zQ7t3K=EQw3PemVm^%bJ1zTcbMBjz5A-M-X(r*Ll7ZYC|R817knW^hEYNHtx-*QmKW z;R(bZ2ePg`6&X(;OAOxT9x@e__6D1O@zILc2Kc7gr*7)3T94iyo>@*7$49TlXP;!J zKVr;X`3+su09m0=jWdXI9k?+K+|QmK5gZi&}pdu zu`XVlD0DyUq}Tq!UeSSA4k=$@jRchJ#!;66p5eZx+pDv}eZPc!Cis#*rGc^i6gmMe za0j@)l}{R1pg>2B#gGe@22bp)d-R1>IKQqommwdRK!qq8qT zROBO$dHC(SALswRwCdc|4534uDWGi^T7zxeg8Ttg_?_%R;~~Jb0+`dGEErFd_oYHm z;PS^~)k%AOaYgz?RWnT*C~>@CA0#7gU^$kH*Hgnsauf&GRX^mw@m=_O!H)e%(Tv8( zY9-t?rdA>y-m>m@v!%xKcc?dlV^}LoHAwo?_+RJ{g?VtHx41htE zF$qCUfU$3^tQQyiOP!j$8yBV-Yka5G!!#~qQ`+z=w&Yw_2|N*zHUp}^fqTuKy}l{U z9=E-lJARKey}Xh+H{>7k0{V3yR!Q1X?AKuEz^nVY%7wA!hwSw$b-v(zWRvCes`~lL z2QT_SDYC?%l2XwM`8UL!vxO9nF1QNouciBqOB_nJ!1!W9rFuH<#`ocx#78EA^w$F7 zP@W-L;A8xRiiwxYjywbEO}_!(kI6@`yXo}Aoh$~P3~o?d)%Llq>;;1=ZqYBl)S;>o zBH=fuDuZ84 zeks(5oV&d59fw_1?}{q@ z^X2968#cS9KrWHh<7A-PGv5(2g%3$4&VN3%fDd<7zTA3Zpynv`n1TeKSB{U|TK)_! z{CL-a^TwO6g}sZ#E?V$%nG*2h61uzfw?0f`#x5TC=ZewR6LztXdI*L%RDMPVIT2(^ zT_J_+X>~wj^MH_vhUDBE4G&CGC}cUD1>e?(xyQ-5Ctf_-xAg?ev(pELxN>XfS<$Tp zael9 z66{+FG5ybE55Q#a4XFS56-;0+Sqg^u>%Bbp*7`VDk#kk{+!NZG7flSNaOOA(_U#7d z;f}2jkL)J)>>2Il+j;`g*!HhzWdAD~h*bV78vkkyJn>(x@vlMozZ;Yik#Tm>9#cBf oQ5aeSYV2(PV<8#PI@mQPW%cBv3U-6HQ1G9r;UR+}J%{lB2S9kzO#lD@ literal 0 HcmV?d00001 diff --git a/doc/source/knowledge.rst b/doc/source/knowledge.rst index 68ae7256a..c5935d7f8 100644 --- a/doc/source/knowledge.rst +++ b/doc/source/knowledge.rst @@ -12,7 +12,12 @@ 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. +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 @@ -35,7 +40,11 @@ is to take a combined property and resolve the individual properties to the avai the methods needed to answer the question. The resolved properties are then combined in the same way as the input property and evaluated. -## TODO Picture to showcase +This image shows the process of resolving the properties through the knowledge engine: +.. image:: ../images/knowledge/property_resolution.png + :align: center + :alt: Property Resolution + ----------------- @@ -48,14 +57,16 @@ PyCRAM does not care how the knowledge is accesses or where it is from as as lon 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. The methods to access the knowledge are defined to reason about the -a given action or object. The methods are: - - * reachable - * graspable - * space_is_free - * gripper_is_free - * is_visible +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 + ## TODO link to example of knowledge source diff --git a/doc/source/knowledge_and_reasoning.rst b/doc/source/knowledge_and_reasoning.rst new file mode 100644 index 000000000..720462231 --- /dev/null +++ b/doc/source/knowledge_and_reasoning.rst @@ -0,0 +1,24 @@ +================================= +Knowledge and Reasoning in PyCRAM +================================= + +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. + +.. image:: ../images/knowledge/property_resolve.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 \ No newline at end of file From 61ec3fe171b45824b6b3b6fc10dfff3d95bcb3f1 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 4 Nov 2024 13:26:34 +0100 Subject: [PATCH 42/57] [examples] added knowledge source example --- doc/source/_toc.yml | 3 + doc/source/knowledge_examples.rst | 3 + examples/knowledge_source.md | 162 +++++++++++++++++++++++ examples/robot_description.md | 4 +- src/pycram/datastructures/dataclasses.py | 5 +- 5 files changed, 173 insertions(+), 4 deletions(-) create mode 100644 doc/source/knowledge_examples.rst create mode 100644 examples/knowledge_source.md diff --git a/doc/source/_toc.yml b/doc/source/_toc.yml index 9f02b49a9..664faed37 100644 --- a/doc/source/_toc.yml +++ b/doc/source/_toc.yml @@ -48,6 +48,9 @@ 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 - caption: API chapters: 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/knowledge_source.md b/examples/knowledge_source.md new file mode 100644 index 000000000..51b0600b9 --- /dev/null +++ b/examples/knowledge_source.md @@ -0,0 +1,162 @@ +--- +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 +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 + +world = BulletWorld(WorldMode.GUI) +pr2 = Object("pr2", ObjectType.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/robot_description.md b/examples/robot_description.md index 9782b2cf7..8b69cfa46 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/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index cb8b1b14c..2223e3906 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 @@ -698,10 +698,11 @@ class MultiverseContactPoint: 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] \ No newline at end of file + reasoned_parameter: Optional[Dict[str, Any]] = field(default_factory=dict) From 516afdba66464e8a72641c74af7e2ad1f9a284f3 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 4 Nov 2024 18:57:55 +0100 Subject: [PATCH 43/57] [example] added property example --- examples/properties.md | 112 +++++++++++++++++++++++ requirements.txt | 2 +- src/pycram/knowledge/knowledge_engine.py | 8 +- 3 files changed, 119 insertions(+), 3 deletions(-) create mode 100644 examples/properties.md diff --git a/examples/properties.md b/examples/properties.md new file mode 100644 index 000000000..91bb8c38d --- /dev/null +++ b/examples/properties.md @@ -0,0 +1,112 @@ +--- +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 + +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 + +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/requirements.txt b/requirements.txt index 8814d84c2..5a72c1076 100644 --- a/requirements.txt +++ b/requirements.txt @@ -10,7 +10,7 @@ SQLAlchemy>=2.0.9 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 diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index a8aa95011..15b2702ea 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -7,7 +7,7 @@ from typeguard import check_type, TypeCheckError from ..datastructures.partial_designator import PartialDesignator -from ..datastructures.property import Property, ResolvedProperty +from ..datastructures.property import Property, ResolvedProperty, PropertyOperator from .knowledge_source import KnowledgeSource from typing_extensions import Type, Callable, List, TYPE_CHECKING, Dict, Any @@ -114,7 +114,11 @@ def resolve_properties(self, properties: Property): for param in inspect.signature(resolved_aspect_function).parameters.keys(): node.parameter[param] = child.__getattribute__(param) child.parent = None - return properties.root + + if isinstance(properties.root, PropertyOperator): + return properties.root + else: + return node def update(self): """ From 9c04dd814bdf61081c605da79164ecd7d1e6d63b Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 15 Nov 2024 09:13:56 +0100 Subject: [PATCH 44/57] [tests] Started tests for knowledge --- src/pycram/datastructures/dataclasses.py | 4 +- src/pycram/datastructures/property.py | 105 +++++++++++------ src/pycram/designators/action_designator.py | 2 +- test/knowledge_testcase.py | 62 ++++++++++ test/test_knowledge.py | 82 +++++++++++++ test/test_property.py | 123 ++++++++++++++++++++ 6 files changed, 341 insertions(+), 37 deletions(-) create mode 100644 test/knowledge_testcase.py create mode 100644 test/test_knowledge.py create mode 100644 test/test_property.py diff --git a/src/pycram/datastructures/dataclasses.py b/src/pycram/datastructures/dataclasses.py index cb8b1b14c..1efe9cb92 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 @@ -704,4 +704,4 @@ class ReasoningResult: Result of a reasoning result of knowledge source """ success: bool - reasoned_parameter: Dict[str, Any] \ No newline at end of file + reasoned_parameter: Dict[str, Any] = field(default_factory=dict) \ No newline at end of file diff --git a/src/pycram/datastructures/property.py b/src/pycram/datastructures/property.py index faa0ab68e..19a9a093c 100644 --- a/src/pycram/datastructures/property.py +++ b/src/pycram/datastructures/property.py @@ -3,6 +3,8 @@ from abc import abstractmethod +from past.builtins import reduce + from .enums import Arms from .dataclasses import ReasoningResult from .pose import Pose @@ -14,6 +16,7 @@ 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. @@ -31,7 +34,7 @@ def __init__(self, parent: NodeMixin = None, children: Iterable[NodeMixin] = Non self.parent: Property = parent self.variables: Dict[str, Any] = {} self.executed: bool = False - self.result: ReasoningResult = None + self._result: ReasoningResult = ReasoningResult(False) if children: self.children = children @@ -69,7 +72,15 @@ def __invert__(self): """ return Not(self) - def manage_io(self, func: Callable, *args, **kwargs) -> bool: + @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. @@ -82,7 +93,7 @@ def manage_io(self, func: Callable, *args, **kwargs) -> bool: reasoning_result = func(*args, **kwargs) self.root.variables.update(reasoning_result.reasoned_parameter) self.result = reasoning_result - return reasoning_result.success + return reasoning_result @property def successful(self) -> bool: @@ -92,7 +103,7 @@ def successful(self) -> bool: :return: True if all nodes in the tree are True, False otherwise """ - success = self.result + success = self.result.success for node in PreOrderIter(self.root): success &= node.result.success return success @@ -126,7 +137,6 @@ def __init__(self, properties: List[Property]): :param properties: A list of properties to which are the children of this node """ super().__init__(parent=None, children=properties) - self.result = True def simplify(self): """ @@ -170,7 +180,18 @@ def __init__(self, properties: List[Property]): super().__init__(properties) self.simplify() - def __call__(self, *args, **kwargs) -> bool: + @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 @@ -180,17 +201,14 @@ def __call__(self, *args, **kwargs) -> bool: :param kwargs: A dictionary of keyword arguments to pass to the children :return: True if all children return True, False otherwise """ - result = True + self.executed = True + for child in self.children: - # Child is a Property and the executed function should be called - if child.is_leaf: - result = result and child(*args, **kwargs) - # Child is a PropertyOperator - else: - child(*args, **kwargs) - if not result: - return False - return result + res = child(*args, **kwargs) + if not res.success: + break + + return self.result class Or(PropertyOperator): @@ -209,7 +227,18 @@ def __init__(self, properties: List[Property]): super().__init__(properties) self.simplify() - def __call__(self, *args, **kwargs) -> bool: + @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 @@ -219,17 +248,11 @@ def __call__(self, *args, **kwargs) -> bool: :param kwargs: A dictionary of keyword arguments to pass to the children :return: True if one child returns True, False otherwise """ - result = False + self.executed = True + for child in self.children: - # Child is a Property and the executed function should be called - if child.is_leaf: - result = result or child(*args, **kwargs) - # Child is a PropertyOperator - else: - result = child(*args, **kwargs) - if result: - return True - return result + child(*args, **kwargs) + return self.result class Not(PropertyOperator): @@ -246,7 +269,17 @@ def __init__(self, property: Property): """ super().__init__([property]) - def __call__(self, *args, **kwargs) -> bool: + @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. @@ -255,7 +288,9 @@ def __call__(self, *args, **kwargs) -> bool: :param kwargs: A dictionary of keyword arguments to pass to the child :return: The negation of the result of the child """ - return not self.children[0](*args, **kwargs) + self.executed = True + self.children[0](*args, **kwargs) + return self.result class ResolvedProperty(Property): @@ -274,7 +309,8 @@ class ResolvedProperty(Property): 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): + 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. @@ -288,17 +324,18 @@ def __init__(self, resolved_property_function: Callable, property_exception: Typ self.property_exception = property_exception self.parameter = {} - def __call__(self, *args, **kwargs) -> bool: + 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 """ - result = self.manage_io(self.resolved_property_function, **self.parameter) - if not result: + 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 result + return self.result class ReachableProperty(Property): diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 3e85fd808..df0afa0b0 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -1036,7 +1036,7 @@ class OpenAction(ActionDesignatorDescription): performable_class = OpenActionPerformable - def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], + def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None, ontology_concept_holders: Optional[List[Thing]] = None): """ Moves the arm of the robot to open a container. diff --git a/test/knowledge_testcase.py b/test/knowledge_testcase.py new file mode 100644 index 000000000..9e9fa2944 --- /dev/null +++ b/test/knowledge_testcase.py @@ -0,0 +1,62 @@ +import unittest +from abc import abstractmethod + +from bullet_world_testcase 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): + 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): + 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_knowledge.py b/test/test_knowledge.py new file mode 100644 index 000000000..90ba24f68 --- /dev/null +++ b/test/test_knowledge.py @@ -0,0 +1,82 @@ +from bullet_world_testcase import BulletWorldTestCase +from knowledge_testcase import KnowledgeSourceTestCase, TestProperty, KnowledgeBulletTestCase +from pycram.datastructures.enums import Arms, Grasp +from pycram.datastructures.pose import Pose +from pycram.designators.action_designator import PickUpAction +from pycram.designators.object_designator import BelieveObject +from pycram.knowledge.knowledge_engine import KnowledgeEngine + +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": 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 TestReasoningInstance(KnowledgeSourceTestCase): + pass 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 From b17a28cb2029fb7e71b7a956a7c4dc3c2b10b081 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 20 Nov 2024 13:51:57 +0100 Subject: [PATCH 45/57] [tests] Added more tests for knowledge --- .../datastructures/partial_designator.py | 1 + src/pycram/knowledge/__init__.py | 17 +----- src/pycram/knowledge/knowledge_engine.py | 14 +++-- src/pycram/world_concepts/world_object.py | 4 +- test/test_knowledge.py | 61 ++++++++++++++++++- 5 files changed, 73 insertions(+), 24 deletions(-) diff --git a/src/pycram/datastructures/partial_designator.py b/src/pycram/datastructures/partial_designator.py index 40d27987b..b1fa5a63e 100644 --- a/src/pycram/datastructures/partial_designator.py +++ b/src/pycram/datastructures/partial_designator.py @@ -22,6 +22,7 @@ class PartialDesignator: 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): diff --git a/src/pycram/knowledge/__init__.py b/src/pycram/knowledge/__init__.py index 56ce57d18..247581dee 100644 --- a/src/pycram/knowledge/__init__.py +++ b/src/pycram/knowledge/__init__.py @@ -1,16 +1 @@ -# from ..datastructures.knowledge_engine import KnowledgeEngine -# -# import os -# import sys - -# path = os.path.dirname(os.path.abspath(__file__)) -# -# for py in [f[:-3] for f in os.listdir(path) if f.endswith('.py') and f != '__init__.py']: -# mod = __import__('.'.join([__name__, py]), fromlist=[py]) -# classes = [getattr(mod, x) for x in dir(mod) if isinstance(getattr(mod, x), type)] -# print(classes) -# for cls in classes: -# setattr(sys.modules[__name__], cls.__name__, cls) - -# knowledge_engine = KnowledgeEngine() -# +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 index 15b2702ea..6f861cee5 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -1,6 +1,8 @@ from __future__ import annotations import inspect +from enum import Enum +from typing import _GenericAlias import rospy from anytree import PreOrderIter @@ -12,8 +14,6 @@ from typing_extensions import Type, Callable, List, TYPE_CHECKING, Dict, Any from ..failures import KnowledgeNotAvailable, ReasoningError -# This import is needed since the subclasses of KnowledgeSource need to be imported to be known at runtime -from .knowledge_sources import * if TYPE_CHECKING: from ..designator import ActionDesignatorDescription @@ -184,13 +184,19 @@ def _match_by_type(parameter: Dict[str, any], designator: Type[ActionDesignatorD for key, value in parameter.items(): for parameter_name, type_hint in designator.performable_class.get_type_hints().items(): try: - check_type(value, type_hint) + # 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 diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 3c93d4272..318fe8df7 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -563,8 +563,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/test_knowledge.py b/test/test_knowledge.py index 90ba24f68..437699d9c 100644 --- a/test/test_knowledge.py +++ b/test/test_knowledge.py @@ -1,10 +1,13 @@ from bullet_world_testcase import BulletWorldTestCase from knowledge_testcase import KnowledgeSourceTestCase, TestProperty, KnowledgeBulletTestCase from pycram.datastructures.enums import Arms, Grasp +from pycram.datastructures.partial_designator import PartialDesignator from pycram.datastructures.pose import Pose -from pycram.designators.action_designator import PickUpAction +from pycram.designators.action_designator import PickUpAction, PickUpActionPerformable from pycram.designators.object_designator import BelieveObject 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): @@ -61,7 +64,7 @@ def test_match_by_type(self): self.assertEqual({"arm": Arms.RIGHT}, matched) def test_match_by_type_no_match(self): - params = {"test": 1, "leg": "left"} + params = {"test": {"test": 1}, "leg": "left"} desig = PickUpAction(BelieveObject(names=["milk"])) matched = self.knowledge_engine._match_by_type(params, desig) self.assertEqual({}, matched) @@ -78,5 +81,59 @@ def test_match_reasoned_parameter_full(self): 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.args, (test_object,)) + self.assertEqual(partial_desig.kwargs, {"arm": Arms.RIGHT}) + + 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.args, ()) + self.assertEqual(partial_desig.kwargs, {"arm": Arms.RIGHT}) + + 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(new_partial_desig.args, ()) + self.assertEqual(new_partial_desig.kwargs, {"arm": Arms.RIGHT, "grasp": Grasp.FRONT}) + + 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 TestReasoningInstance(KnowledgeSourceTestCase): pass From 42e3972366c6c90ea20ec2ce4cf6435df9dfedbd Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 22 Nov 2024 10:19:26 +0100 Subject: [PATCH 46/57] [doc] Added more doc for knowledge interface --- doc/source/_toc.yml | 4 +++ doc/source/knowledge.rst | 6 ++++- doc/source/knowledge_and_reasoning.rst | 36 +++++++++++++++++++++----- examples/knowledge_source.md | 1 + 4 files changed, 40 insertions(+), 7 deletions(-) diff --git a/doc/source/_toc.yml b/doc/source/_toc.yml index 664faed37..c1a1f3601 100644 --- a/doc/source/_toc.yml +++ b/doc/source/_toc.yml @@ -15,6 +15,7 @@ parts: - file: notebooks.rst - file: designators.rst - file: knowledge.rst + - file: knowledge_and_reasoning.rst - caption: Trouble Shooting chapters: @@ -28,6 +29,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 @@ -51,6 +54,7 @@ parts: - 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 index c5935d7f8..cb974554b 100644 --- a/doc/source/knowledge.rst +++ b/doc/source/knowledge.rst @@ -24,6 +24,7 @@ of entities in the world. Properties can be combined to create more complex expr 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(....)) @@ -41,6 +42,7 @@ the methods needed to answer the question. The resolved properties are then comb 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 @@ -68,7 +70,9 @@ available and which a knowledge source can implement are: - `VisibleProperty`: Checks if an object is visible -## TODO link to example of knowledge source +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 diff --git a/doc/source/knowledge_and_reasoning.rst b/doc/source/knowledge_and_reasoning.rst index 720462231..a86f46d60 100644 --- a/doc/source/knowledge_and_reasoning.rst +++ b/doc/source/knowledge_and_reasoning.rst @@ -1,6 +1,6 @@ -================================= -Knowledge and Reasoning in PyCRAM -================================= +======================= +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 @@ -11,9 +11,10 @@ Inference is done very similar to the normal reasoning process where the propert 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. +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_resolve.png +.. image:: ../images/knowledge/property_resolution.png :alt: Source Resolve :align: center @@ -21,4 +22,27 @@ We then evaluate the properties and generate the possible parameter solutions. .. image:: ../images/knowledge/property_evaluation.png :alt: Source Evaluate - :align: center \ No newline at end of file + :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/examples/knowledge_source.md b/examples/knowledge_source.md index 51b0600b9..676339e10 100644 --- a/examples/knowledge_source.md +++ b/examples/knowledge_source.md @@ -13,6 +13,7 @@ jupyter: --- # 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 From a05377a0e7dfac0e81eefec3d48237ea9f182c58 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 22 Nov 2024 10:20:38 +0100 Subject: [PATCH 47/57] [knowledge sources] Cleanup if test sources --- .../food_cutting_knowledge.py | 82 ------------------- .../knowledge_sources/knowrob_knowledge.py | 37 --------- 2 files changed, 119 deletions(-) delete mode 100644 src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py delete mode 100644 src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py diff --git a/src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py b/src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py deleted file mode 100644 index c8ff41f5b..000000000 --- a/src/pycram/knowledge/knowledge_sources/food_cutting_knowledge.py +++ /dev/null @@ -1,82 +0,0 @@ -from ..knowledge_source import KnowledgeSource - -from rdflib import Graph, Namespace -from rdflib.namespace import OWL, RDFS - -from urllib import request -from typing_extensions import Iterable - -class FoodCuttingKnowledge(KnowledgeSource): - - def __init__(self): - super().__init__("Food KG", 1) - self.knowledge_graph = Graph() - - # define prefixes to be used in the query - SOMA = Namespace("http://www.ease-crc.org/ont/SOMA.owl#") - CUT2 = Namespace("http://www.ease-crc.org/ont/situation_awareness#") - CUT = Namespace("http://www.ease-crc.org/ont/food_cutting#") - DUL = Namespace("http://www.ontologydesignpatterns.org/ont/dul/DUL.owl#") - OBO = Namespace("http://purl.obolibrary.org/obo/") - self.knowledge_graph.bind("owl", OWL) - self.knowledge_graph.bind("rdfs", RDFS) - self.knowledge_graph.bind("soma", SOMA) - self.knowledge_graph.bind("cut2", CUT2) - self.knowledge_graph.bind("cut", CUT) - self.knowledge_graph.bind("dul", DUL) - self.knowledge_graph.bind("obo", OBO) - - @property - def is_available(self) -> bool: - pass - # return request.urlopen("https://api.krr.triply.cc/datasets/mkumpel/FruitCuttingKG/services/FoodCuttingKG/sparql?query=SELECT%20?subject%20?predicate%20?objectWHERE%20{?subject%20?predicate%20?object%20.}").getcode() != 404 - - @property - def is_connected(self) -> bool: - return self.is_available - - def connect(self): - pass - - def query(self, designator): - pass - - def clear_state(self): - pass - - def get_repetitions(self, task, task_object) -> Iterable[str]: - # task = "cut:Quartering" - # tobject = "obo:FOODON_03301710" - repetitionsquery = """ SELECT ?rep WHERE { - SERVICE { - { - OPTIONAL{ %s rdfs:subClassOf ?action} - ?action rdfs:subClassOf* ?rep_node. - ?rep_node owl:onProperty cut:repetitions. - FILTER EXISTS { - ?rep_node owl:hasValue ?val.} - BIND("0.05" AS ?rep)} - UNION - { - OPTIONAL{ %s rdfs:subClassOf ?action } - ?action rdfs:subClassOf* ?rep_node. - ?rep_node owl:onProperty cut:repetitions. - FILTER EXISTS { - ?rep_node owl:minQualifiedCardinality ?val.} - BIND("more than 1" AS ?rep)}} }""" % (task, task) - for row in self.knowledge_graph.query(repetitionsquery): - yield row.rep - - def get_technique_for_task(self, task, task_object) -> Iterable[str]: - # task = "cut:Quartering" - # tobject = "obo:FOODON_03301710" - positionquery = """ SELECT ?pos WHERE { - SERVICE { - OPTIONAL { %s rdfs:subClassOf ?sub.} - ?sub rdfs:subClassOf* ?node. - ?node owl:onProperty cut:affordsPosition. - ?node owl:someValuesFrom ?position. - BIND(IF(?position = cut:halving_position, "halving", "slicing") AS ?pos) - } }""" % task - for row in self.knowledge_graph.query(positionquery): - yield row.pos diff --git a/src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py b/src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py deleted file mode 100644 index f01112c56..000000000 --- a/src/pycram/knowledge/knowledge_sources/knowrob_knowledge.py +++ /dev/null @@ -1,37 +0,0 @@ -import rospy - -from ..knowledge_source import KnowledgeSource, QueryKnowledge, UpdateKnowledge -import rosservice -# from ...designator import DesignatorDescription -try: - from rosprolog_client import Prolog -except ModuleNotFoundError as e: - rospy.logwarn(f"Could not import Prolog client from package rosprolog_client, Knowrob related features are not available.") - - -class KnowrobKnowledge(KnowledgeSource, QueryKnowledge, UpdateKnowledge): - - def __init__(self): - super().__init__("Knowrob", 0) - self.prolog_client = None - - @property - def is_available(self) -> bool: - return '/rosprolog/query' in rosservice.get_service_list() - - @property - def is_connected(self) -> bool: - return self.prolog_client is not None - - def connect(self) -> bool: - if self.is_available: - self.prolog_client = Prolog() - self.prolog_client.once(f"tripledb_load('package://iai_apartment/owl/iai-apartment.owl').") - return True - - def query_pose_for_object(self, designator: 'DesignatorDescription') -> 'DesignatorDescription': - print("test") - result = self.prolog_client.once(f"") - - def clear_state(self) -> None: - pass From a0609939624025a53da91f173a52de8406616896 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 22 Nov 2024 16:55:29 +0100 Subject: [PATCH 48/57] [action desigs] added __iter__ methods --- src/pycram/designator.py | 3 +- src/pycram/designators/action_designator.py | 108 +++++++++++++++--- src/pycram/knowledge/knowledge_engine.py | 12 +- .../knowledge_sources/facts_knowledge.py | 17 ++- 4 files changed, 113 insertions(+), 27 deletions(-) diff --git a/src/pycram/designator.py b/src/pycram/designator.py index 30490e28c..d0a24609e 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -6,6 +6,7 @@ from abc import ABC, abstractmethod from inspect import isgenerator, isgeneratorfunction +from .datastructures.property import Property from .ros.logging import logwarn, loginfo import inspect @@ -279,7 +280,7 @@ def __init__(self, ontology_concept_holders: Optional[List[OntologyConceptHolder Language.__init__(self) from .ontology.ontology import OntologyManager self.soma = OntologyManager().soma - self.knowledge_conditions = None + self.knowledge_condition = Property() def resolve(self) -> Type[ActionDesignatorDescription.Action]: """ diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index df0afa0b0..14cef316d 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -44,8 +44,6 @@ from dataclasses import dataclass, field - - # ---------------------------------------------------------------------------- # ---------------- Performables ---------------------------------------------- # ---------------------------------------------------------------------------- @@ -142,7 +140,6 @@ def plan(self) -> None: MoveJointsMotion([RobotDescription.current_robot_description.torso_joint], [self.position]).perform() - @dataclass class SetGripperActionPerformable(ActionAbstract): """ @@ -322,7 +319,7 @@ def plan(self) -> None: # 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 + # 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) @@ -403,14 +400,14 @@ class TransportActionPerformable(ActionAbstract): """ Object designator_description 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 """ + arm: Arms + """ + Arm that should be used + """ orm_class: Type[ActionAbstract] = field(init=False, default=ORMTransportAction) @with_tree @@ -626,6 +623,7 @@ def plan(self): FaceAtPerformable(self.object_designator.pose).perform() PickUpActionPerformable(self.object_designator, self.arm, self.grasp).perform() + # ---------------------------------------------------------------------------- # Action Designators Description # ---------------------------------------------------------------------------- @@ -754,7 +752,7 @@ class GripAction(ActionDesignatorDescription): def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, efforts: List[float], ontology_concept_holders: Optional[List[Thing]] = None): - super().__init__( ontology_concept_holders) + super().__init__(ontology_concept_holders) self.grippers: List[Arms] = grippers self.object_designator_description: ObjectDesignatorDescription = object_designator_description self.efforts: List[float] = efforts @@ -841,11 +839,14 @@ def __init__(self, # return PickUpActionPerformable(obj_desig, self.arms[0], self.grasps[0]) def __iter__(self) -> PickUpActionPerformable: - ri = ReasoningInstance(self, PartialDesignator(PickUpActionPerformable, self.object_designator_description, self.arms, self.grasps)) + 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 + class PlaceAction(ActionDesignatorDescription): """ Places an Object at a position using an arm. @@ -885,6 +886,13 @@ def ground(self) -> PlaceActionPerformable: 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): """ @@ -922,6 +930,15 @@ def ground(self) -> NavigateActionPerformable: """ return NavigateActionPerformable(self.target_locations[0]) + def __iter__(self) -> NavigateActionPerformable: + """ + Iterates over all possible target locations + + :return: A performable designator_description + """ + for location in self.target_locations: + yield NavigateActionPerformable(location) + class TransportAction(ActionDesignatorDescription): """ @@ -932,15 +949,15 @@ class TransportAction(ActionDesignatorDescription): def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], - arms: List[Arms], target_locations: List[Pose], + arms: List[Arms] = None, ontology_concept_holders: Optional[List[Thing]] = 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 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 arms: A List of possible arms that could be used for transporting :param ontology_concept_holders: A list of ontology concepts that the action is categorized as or associated with """ super().__init__(ontology_concept_holders) @@ -962,7 +979,17 @@ def ground(self) -> TransportActionPerformable: if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) \ else self.object_designator_description.resolve() - return TransportActionPerformable(obj_desig, self.arms[0], self.target_locations[0]) + return TransportActionPerformable(obj_desig, self.target_locations[0], self.arms[0]) + + 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 class LookAtAction(ActionDesignatorDescription): @@ -994,6 +1021,15 @@ def ground(self) -> LookAtActionPerformable: """ return LookAtActionPerformable(self.targets[0]) + def __iter__(self) -> LookAtActionPerformable: + """ + Iterates over all possible target locations + + :return: A performable designator_description + """ + for target in self.targets: + yield LookAtActionPerformable(target) + class DetectAction(ActionDesignatorDescription): """ @@ -1026,6 +1062,15 @@ def ground(self) -> DetectActionPerformable: """ return DetectActionPerformable(self.object_designator_description.resolve()) + def __iter__(self) -> DetectActionPerformable: + """ + 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 + """ + for desig in self.object_designator_description: + yield DetectActionPerformable(desig) + class OpenAction(ActionDesignatorDescription): """ @@ -1063,6 +1108,17 @@ def ground(self) -> OpenActionPerformable: """ return OpenActionPerformable(self.object_designator_description.resolve(), self.arms[0]) + 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 + class CloseAction(ActionDesignatorDescription): """ @@ -1073,7 +1129,7 @@ class CloseAction(ActionDesignatorDescription): performable_class = CloseActionPerformable - def __init__(self, object_designator_description: ObjectPart, arms: List[Arms], + def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = None, ontology_concept_holders: Optional[List[Thing]] = None): """ Attempts to close an open container @@ -1100,6 +1156,17 @@ def ground(self) -> CloseActionPerformable: """ return CloseActionPerformable(self.object_designator_description.resolve(), self.arms[0]) + def __iter__(self) -> CloseActionPerformable: + """ + Iterates over all possible solutions for this designator_description and returns a performable action designator. + + :yield: A performable fully parametrized Action designator + """ + ri = ReasoningInstance(self, + PartialDesignator(CloseActionPerformable, self.object_designator_description, self.arms)) + for desig in ri: + yield desig + class GraspingAction(ActionDesignatorDescription): """ @@ -1108,7 +1175,7 @@ class GraspingAction(ActionDesignatorDescription): performable_class = GraspingActionPerformable - def __init__(self, arms: List[Arms], object_description: Union[ObjectDesignatorDescription, ObjectPart], + def __init__(self, object_description: Union[ObjectDesignatorDescription, ObjectPart], arms: List[Arms] = None, ontology_concept_holders: Optional[List[Thing]] = None): """ Will try to grasp the object described by the given description. Grasping is done by moving into a pre grasp @@ -1134,3 +1201,14 @@ def ground(self) -> GraspingActionPerformable: """ 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/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index 6f861cee5..d2c2787e8 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -140,7 +140,7 @@ def ground_solution(self, designator: Type['DesignatorDescription']) -> bool: rospy.logwarn("Knowledge engine is disabled") return True - def find_source_for_property(self, property: Type[Property]) -> Type[KnowledgeSource]: + def find_source_for_property(self, property: Type[Property]) -> KnowledgeSource: """ Find the source for the given property @@ -153,7 +153,7 @@ def find_source_for_property(self, property: Type[Property]) -> Type[KnowledgeSo return source def match_reasoned_parameter(self, reasoned_parameter: Dict[str, any], - designator: Type[ActionDesignatorDescription]) -> 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 @@ -164,7 +164,7 @@ def match_reasoned_parameter(self, reasoned_parameter: Dict[str, any], return matched_parameter @staticmethod - def _match_by_name(parameter: Dict[str, any], designator: Type[ActionDesignatorDescription]) -> Dict[str, any]: + 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 """ @@ -176,7 +176,7 @@ def _match_by_name(parameter: Dict[str, any], designator: Type[ActionDesignatorD return result_dict @staticmethod - def _match_by_type(parameter: Dict[str, any], designator: Type[ActionDesignatorDescription]) -> Dict[str, any]: + 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 """ @@ -205,7 +205,7 @@ class ReasoningInstance: full designator at a later time. """ - def __init__(self, designator_description: Type[ActionDesignatorDescription], partial_designator: PartialDesignator): + def __init__(self, designator_description: ActionDesignatorDescription, partial_designator: PartialDesignator): """ Initialize the reasoning instance with the designator_description and the partial designator @@ -219,7 +219,7 @@ def __init__(self, designator_description: Type[ActionDesignatorDescription], pa self.designator_description.__getattribute__(param_name) is None] self.partial_designator = partial_designator - def __iter__(self) -> Type[ActionDesignatorDescription.Action]: + def __iter__(self) -> ActionDesignatorDescription.Action: """ Executes property structure, matches the reasoned and missing parameter and generates a completes designator. diff --git a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py index 7f3abad8f..77ec77c57 100644 --- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -1,5 +1,10 @@ +from __future__ import annotations + +from typing import TYPE_CHECKING + import numpy as np +from ...datastructures.dataclasses import ReasoningResult from ...datastructures.enums import Arms, ObjectType from ..knowledge_source import KnowledgeSource from ...datastructures.property import ReachableProperty, GraspableProperty, GripperIsFreeProperty, VisibleProperty, SpaceIsFreeProperty @@ -8,10 +13,12 @@ # from ...designators.location_designator import CostmapLocation # from ...designators.object_designator import BelieveObject from ...robot_description import RobotDescription -from ...world_concepts.world_object import Object from ...world_reasoning import visible from ...costmaps import OccupancyCostmap +if TYPE_CHECKING: + from ...designators.object_designator import ObjectDesignatorDescription + class FactsKnowledge(KnowledgeSource, GripperIsFreeProperty, VisibleProperty, SpaceIsFreeProperty, GraspableProperty, ReachableProperty): """ Knowledge source for hard coded facts, this knowledge source acts as a fallback if no other knowledge source is @@ -44,7 +51,7 @@ def clear_state(self) -> None: def reachable(self, pose: Pose) -> bool: return True - def graspable(self, object_designator: 'ObjectDesignatorDescription') -> bool: + def graspable(self, object_designator: ObjectDesignatorDescription) -> ReasoningResult: 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])) @@ -60,10 +67,10 @@ def graspable(self, object_designator: 'ObjectDesignatorDescription') -> bool: return True return False - def space_is_free(self, pose: Pose) -> bool: + 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 np.sum(origin_map) > 400 * 0.9 + return ReasoningResult(np.sum(origin_map) > 400 * 0.9) def gripper_is_free(self, gripper: Arms) -> bool: tool_frame_link = RobotDescription.current_robot_description.get_arm_chain(gripper).get_tool_frame() @@ -72,6 +79,6 @@ def gripper_is_free(self, gripper: Arms) -> bool: return False return True - def is_visible(self, object_designator: 'ObjectDesignatorDescription') -> bool: + def is_visible(self, object_designator: ObjectDesignatorDescription) -> ReasoningResult: cam_pose = World.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) return visible(object_designator.resolve().world_object, cam_pose) From d4caf38ddab3e8b3e89309c3cc65efc9bfccc664 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 26 Nov 2024 13:13:09 +0100 Subject: [PATCH 49/57] [test] Tests running again --- src/pycram/datastructures/property.py | 10 +++++++ src/pycram/designator.py | 5 ++-- src/pycram/designators/action_designator.py | 28 +++++++++++++++++-- .../knowledge_sources/facts_knowledge.py | 24 +++++++++------- test/test_action_designator.py | 8 +++--- test/test_orm.py | 5 ++-- 6 files changed, 58 insertions(+), 22 deletions(-) diff --git a/src/pycram/datastructures/property.py b/src/pycram/datastructures/property.py index 19a9a093c..24d6800a1 100644 --- a/src/pycram/datastructures/property.py +++ b/src/pycram/datastructures/property.py @@ -338,6 +338,16 @@ def __call__(self, *args, **kwargs) -> ReasoningResult: 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 diff --git a/src/pycram/designator.py b/src/pycram/designator.py index d0a24609e..b8adde50f 100644 --- a/src/pycram/designator.py +++ b/src/pycram/designator.py @@ -6,7 +6,7 @@ from abc import ABC, abstractmethod from inspect import isgenerator, isgeneratorfunction -from .datastructures.property import Property +from .datastructures.property import Property, EmptyProperty from .ros.logging import logwarn, loginfo import inspect @@ -280,7 +280,8 @@ def __init__(self, ontology_concept_holders: Optional[List[OntologyConceptHolder Language.__init__(self) from .ontology.ontology import OntologyManager self.soma = OntologyManager().soma - self.knowledge_condition = Property() + self.knowledge_condition = EmptyProperty() + self.ground = self.resolve def resolve(self) -> Type[ActionDesignatorDescription.Action]: """ diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 14cef316d..8bd9ac99a 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -724,7 +724,7 @@ class ReleaseAction(ActionDesignatorDescription): performable_class = ReleaseActionPerformable - def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, + def __init__(self, object_designator_description: ObjectDesignatorDescription, grippers: List[Arms] = None, ontology_concept_holders: Optional[List[Thing]] = None): super().__init__(ontology_concept_holders) self.grippers: List[Arms] = grippers @@ -736,6 +736,12 @@ def __init__(self, grippers: List[Arms], object_designator_description: ObjectDe 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 + class GripAction(ActionDesignatorDescription): """ @@ -750,8 +756,8 @@ class GripAction(ActionDesignatorDescription): performable_class = GripActionPerformable - def __init__(self, grippers: List[Arms], object_designator_description: ObjectDesignatorDescription, - efforts: List[float], ontology_concept_holders: Optional[List[Thing]] = None): + def __init__(self, object_designator_description: ObjectDesignatorDescription, grippers: List[Arms] = None, + efforts: List[float] = None, ontology_concept_holders: Optional[List[Thing]] = None): super().__init__(ontology_concept_holders) self.grippers: List[Arms] = grippers self.object_designator_description: ObjectDesignatorDescription = object_designator_description @@ -763,6 +769,13 @@ def __init__(self, grippers: List[Arms], object_designator_description: ObjectDe def ground(self) -> GripActionPerformable: return GripActionPerformable(self.grippers[0], self.object_designator_description.ground(), self.efforts[0]) + def __iter__(self): + ri = ReasoningInstance(self, + PartialDesignator(GripActionPerformable, self.grippers, self.object_designator_description, + self.efforts)) + for desig in ri: + yield desig + class ParkArmsAction(ActionDesignatorDescription): """ @@ -793,6 +806,15 @@ def ground(self) -> ParkArmsActionPerformable: """ return ParkArmsActionPerformable(self.arms[0]) + def __iter__(self) -> ParkArmsActionPerformable: + """ + Iterates over all possible solutions and returns a performable designator with the arm. + + :return: A performable designator_description + """ + for arm in self.arms: + yield ParkArmsActionPerformable(arm) + class PickUpAction(ActionDesignatorDescription): """ diff --git a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py index 77ec77c57..2ef274bd3 100644 --- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -7,7 +7,8 @@ from ...datastructures.dataclasses import ReasoningResult from ...datastructures.enums import Arms, ObjectType from ..knowledge_source import KnowledgeSource -from ...datastructures.property import ReachableProperty, GraspableProperty, GripperIsFreeProperty, VisibleProperty, SpaceIsFreeProperty +from ...datastructures.property import ReachableProperty, GraspableProperty, GripperIsFreeProperty, VisibleProperty, \ + SpaceIsFreeProperty, EmptyProperty from ...datastructures.pose import Pose from ...datastructures.world import World, UseProspectionWorld # from ...designators.location_designator import CostmapLocation @@ -19,7 +20,7 @@ if TYPE_CHECKING: from ...designators.object_designator import ObjectDesignatorDescription -class FactsKnowledge(KnowledgeSource, GripperIsFreeProperty, VisibleProperty, SpaceIsFreeProperty, GraspableProperty, ReachableProperty): +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. @@ -48,8 +49,8 @@ def clear_state(self) -> None: # if c.pose: # return True - def reachable(self, pose: Pose) -> bool: - return True + def reachable(self, pose: Pose) -> ReasoningResult: + return ReasoningResult(True) def graspable(self, object_designator: ObjectDesignatorDescription) -> ReasoningResult: with UseProspectionWorld(): @@ -64,21 +65,24 @@ def graspable(self, object_designator: ObjectDesignatorDescription) -> Reasoning RobotDescription.current_robot_description.get_manipulator_chains()] for dist in gripper_opening_dists: if dist > obj_x or dist > obj_y or dist > obj_z: - return True - return False + return ReasoningResult(True) + 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, gripper: Arms) -> bool: + def gripper_is_free(self, gripper: Arms) -> ReasoningResult: 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 False - return True + return ReasoningResult(False) + return ReasoningResult(True) def is_visible(self, object_designator: ObjectDesignatorDescription) -> ReasoningResult: cam_pose = World.robot.get_link_pose(RobotDescription.current_robot_description.get_camera_frame()) - return visible(object_designator.resolve().world_object, cam_pose) + return ReasoningResult(visible(object_designator.resolve().world_object, cam_pose)) + + def empty(self) -> ReasoningResult: + return ReasoningResult(True) diff --git a/test/test_action_designator.py b/test/test_action_designator.py index df060aa22..5021e8b45 100644 --- a/test/test_action_designator.py +++ b/test/test_action_designator.py @@ -35,13 +35,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") @@ -119,9 +119,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_orm.py b/test/test_orm.py index 6609e3992..4071d4dfc 100644 --- a/test/test_orm.py +++ b/test/test_orm.py @@ -219,8 +219,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" @@ -341,7 +340,7 @@ def test_believe_object(self): LookAtAction(targets=[Pose([1, -1.78, 0.55])]).resolve().perform() object_desig = DetectAction(BelieveObject(types=[ObjectType.MILK])).resolve().perform() - 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" From c80b5ce556ce082e110d494dea5649ee2082748e Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 26 Nov 2024 13:56:18 +0100 Subject: [PATCH 50/57] [examples] Fixed examples --- examples/action_designator.md | 4 ++-- examples/orm_example.md | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/examples/action_designator.md b/examples/action_designator.md index e0a4babf9..a961e7e58 100644 --- a/examples/action_designator.md +++ b/examples/action_designator.md @@ -243,9 +243,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/orm_example.md b/examples/orm_example.md index 6231a04a5..559cfbc5a 100644 --- a/examples/orm_example.md +++ b/examples/orm_example.md @@ -170,7 +170,7 @@ class SayingActionPerformable(ActionAbstract): orm_class = ORMSaying @with_tree - def perform(self) -> None: + def plan(self) -> None: print(self.text) ``` From faf1c0e43415f18cfb0372d3554bb561afed6c5f Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 26 Nov 2024 15:31:32 +0100 Subject: [PATCH 51/57] [property] Fixed wrong import --- src/pycram/datastructures/property.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/datastructures/property.py b/src/pycram/datastructures/property.py index 24d6800a1..cb2985b00 100644 --- a/src/pycram/datastructures/property.py +++ b/src/pycram/datastructures/property.py @@ -3,7 +3,7 @@ from abc import abstractmethod -from past.builtins import reduce +from functools import reduce from .enums import Arms from .dataclasses import ReasoningResult From 2ea96413ab453b8cb27df736fbf2105309e70eb3 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 26 Nov 2024 15:48:30 +0100 Subject: [PATCH 52/57] [examples] Adressed problems in ci --- examples/knowledge_source.md | 2 ++ examples/properties.md | 3 +++ 2 files changed, 5 insertions(+) diff --git a/examples/knowledge_source.md b/examples/knowledge_source.md index 676339e10..b1cc09f1b 100644 --- a/examples/knowledge_source.md +++ b/examples/knowledge_source.md @@ -139,6 +139,8 @@ 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 world = BulletWorld(WorldMode.GUI) pr2 = Object("pr2", ObjectType.ROBOT, "pr2.urdf") diff --git a/examples/properties.md b/examples/properties.md index 91bb8c38d..47e44e127 100644 --- a/examples/properties.md +++ b/examples/properties.md @@ -32,6 +32,7 @@ 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): @@ -72,6 +73,8 @@ information how a Knowledge Source is created pleas refere to the respective exa ```python from pycram.knowledge.knowledge_source import KnowledgeSource +from pycram.datastructures.pose import Pose +from pycram.datastructures.dataclasses import ReasoningResult class ExampleKnowledge(KnowledgeSource, ExampleProperty): From 3716870578a56be1f4c1e832ed62cff9f30e934b Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 26 Nov 2024 16:16:34 +0100 Subject: [PATCH 53/57] [knowledge] Adjusted doc according to comments --- src/pycram/datastructures/partial_designator.py | 3 +-- src/pycram/datastructures/property.py | 17 ++++++----------- test/knowledge_testcase.py | 6 ++++++ 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/pycram/datastructures/partial_designator.py b/src/pycram/datastructures/partial_designator.py index b1fa5a63e..4018d788e 100644 --- a/src/pycram/datastructures/partial_designator.py +++ b/src/pycram/datastructures/partial_designator.py @@ -72,8 +72,7 @@ def __iter__(self) -> Type[ActionDesignatorDescription.Action]: @staticmethod def generate_permutations(args: Iterable) -> List: """ - Generates all possible permutations of the given arguments. This uses itertools.product to generate the - permutations. + 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 diff --git a/src/pycram/datastructures/property.py b/src/pycram/datastructures/property.py index cb2985b00..14a53f272 100644 --- a/src/pycram/datastructures/property.py +++ b/src/pycram/datastructures/property.py @@ -111,8 +111,7 @@ def successful(self) -> bool: @property def resolved(self) -> bool: """ - Returns True if all nodes in the tree are resolved. This is done by iterating over the tree and checking if all - nodes are either a ResolvedProperty or a PropertyOperator. + 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 """ @@ -140,8 +139,7 @@ def __init__(self, properties: List[Property]): def simplify(self): """ - Simplifies the tree structure by merging nodes of the same type. This is done by iterating over the tree and - merging nodes of the same type. + Simplifies the tree structure by merging nodes of the same type. :return: Returns the root node of the tree """ @@ -166,8 +164,7 @@ def merge_nodes(node1: Node, node2: Node) -> None: class And(PropertyOperator): """ - Class to represent a logical and operator in a tree structure. This class inherits from PropertyOperator and adds a - method to evaluate the children as an and operator. + Represents a logical AND between multiple properties. """ def __init__(self, properties: List[Property]): @@ -213,8 +210,7 @@ def __call__(self, *args, **kwargs) -> ReasoningResult: class Or(PropertyOperator): """ - Class to represent a logical or operator in a tree structure. This class inherits from PropertyOperator and adds a - method to evaluate the children as an or operator. + Represents a logical OR between multiple properties. """ def __init__(self, properties: List[Property]): @@ -257,8 +253,7 @@ def __call__(self, *args, **kwargs) -> ReasoningResult: class Not(PropertyOperator): """ - Class to represent a logical not operator in a tree structure. This class inherits from PropertyOperator and adds a - method to evaluate the children as a not operator. + Represents a logical NOT of a single property. """ def __init__(self, property: Property): @@ -295,7 +290,7 @@ def __call__(self, *args, **kwargs) -> ReasoningResult: class ResolvedProperty(Property): """ - Class to represent a executed property function. It holds the reference to the respective function in the knowledge + 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. """ diff --git a/test/knowledge_testcase.py b/test/knowledge_testcase.py index 9e9fa2944..dfa15de3d 100644 --- a/test/knowledge_testcase.py +++ b/test/knowledge_testcase.py @@ -11,6 +11,9 @@ class TestProperty(Property): + """ + A Mock Property for testing the property implementation + """ property_exception = ReasoningError def __init__(self, pose: Pose): @@ -23,6 +26,9 @@ def test(self, pose: Pose) -> ReasoningResult: class TestKnowledge(KnowledgeSource, TestProperty): + """ + A Mock KnowledgeSource for testing the knowledge_source implementation + """ def __init__(self): super().__init__("test_source", 1) self.parameter = {} From a7c621105b060fa34f9f134d03e5a3a3e7cc464e Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Tue, 26 Nov 2024 18:21:17 +0100 Subject: [PATCH 54/57] [partial desig] Internally only uses kwargs --- .../datastructures/partial_designator.py | 37 ++++---- src/pycram/datastructures/property.py | 6 +- src/pycram/designators/action_designator.py | 23 +---- src/pycram/helper.py | 2 +- src/pycram/knowledge/knowledge_engine.py | 4 + .../knowledge_sources/facts_knowledge.py | 84 ++++++++++++++----- test/test_knowledge.py | 9 +- 7 files changed, 105 insertions(+), 60 deletions(-) diff --git a/src/pycram/datastructures/partial_designator.py b/src/pycram/datastructures/partial_designator.py index 4018d788e..4d8c221c1 100644 --- a/src/pycram/datastructures/partial_designator.py +++ b/src/pycram/datastructures/partial_designator.py @@ -2,6 +2,7 @@ from __future__ import annotations from collections import Iterable + from typing_extensions import Type, List, Tuple, Any, Dict, TYPE_CHECKING from itertools import product from inspect import signature @@ -44,8 +45,9 @@ class PartialDesignator: def __init__(self, performable: Type[ActionDesignatorDescription.Action], *args, **kwargs): self.performable = performable # Remove None values fom the given arguments and keyword arguments - self.args = tuple(filter(None, args)) - self.kwargs = {k:v for k,v in kwargs.items() if v is not None} + self.kwargs = dict(signature(self.performable).bind(*args, **kwargs).arguments) + # self.args = tuple(filter(None, args)) + # self.kwargs = {k:v for k,v in kwargs.items() if v is not None} def __call__(self, *fargs, **fkwargs): """ @@ -56,18 +58,19 @@ def __call__(self, *fargs, **fkwargs): :return: A new PartialDesignator with the given arguments and keyword arguments added """ newkeywords = {**self.kwargs, **fkwargs} - return PartialDesignator(self.performable, *self.args, *fargs, **newkeywords) + return PartialDesignator(self.performable, *fargs, **newkeywords) 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. + 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 args_combination in PartialDesignator.generate_permutations(self.args): - for kwargs_combination in PartialDesignator.generate_permutations(self.kwargs.values()): - yield self.performable(*args_combination, **dict(zip(self.kwargs.keys(), kwargs_combination))) + # for args_combination in PartialDesignator.generate_permutations(self.args): + 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: @@ -101,13 +104,17 @@ def missing_parameter(self) -> List[str]: :return: A list of parameter names that are missing from the performable """ - performable_params = signature(self.performable).parameters - # List of all parameter names that need to be filled for the performable - param_names = list(performable_params.keys()) - - # Remove parameter that are filled by args - missing_after_args = param_names[len(self.args):] - # Remove parameter that are filled by keyword arguments and return the remaining parameters - return list(set(missing_after_args) - set(self.kwargs.keys())) + missing = {k: v for k, v in self.kwargs.items() if v is None} + return list(missing.keys()) + + # performable_params = signature(self.performable).parameters + # # List of all parameter names that need to be filled for the performable + # param_names = list(performable_params.keys()) + # + # # Remove parameter that are filled by args + # # missing_after_args = param_names[len(self.args):] + # + # # Remove parameter that are filled by keyword arguments and return the remaining parameters + # return list(set(missing_after_args) - set(self.kwargs.keys())) diff --git a/src/pycram/datastructures/property.py b/src/pycram/datastructures/property.py index 14a53f272..967b4c00a 100644 --- a/src/pycram/datastructures/property.py +++ b/src/pycram/datastructures/property.py @@ -382,12 +382,12 @@ def space_is_free(self, pose: Pose) -> ReasoningResult: class GripperIsFreeProperty(Property): property_exception = GripperOccupied - def __init__(self, gripper: Arms): + def __init__(self, grippers: List[Arms]): super().__init__(None, None) - self.gripper = gripper + self.grippers = grippers @abstractmethod - def gripper_is_free(self, gripper: Arms) -> ReasoningResult: + def gripper_is_free(self, grippers: List[Arms]) -> ReasoningResult: raise NotImplementedError diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index c5f8463e4..9b3030b50 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -722,13 +722,6 @@ def __init__(self, grippers: List[Arms], motions: List[GripperState], super().__init__(ontology_concept_holders) self.grippers: List[Arms] = grippers self.motions: List[GripperState] = motions - if len(self.grippers) == 1: - self.knowledge_condition = GripperIsFreeProperty(self.grippers[0]) - else: - root = GripperIsFreeProperty(self.grippers[0]) - for gripper in grippers[1:]: - root |= GripperIsFreeProperty(gripper) - self.knowledge_condition = root if self.soma: self.init_ontology_concepts({"setting_gripper": self.soma.SettingGripper}) @@ -883,19 +876,6 @@ def __init__(self, if self.soma: self.init_ontology_concepts({"picking_up": self.soma.PickingUp}) - # def ground(self) -> PickUpActionPerformable: - # """ - # Default specialized_designators, returns a performable designator_description with the first entries from the lists of possible parameter. - # - # :return: A performable designator_description - # """ - # if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object): - # obj_desig = self.object_designator_description - # else: - # obj_desig = self.object_designator_description.resolve() - # - # return PickUpActionPerformable(obj_desig, self.arms[0], self.grasps[0]) - def __iter__(self) -> PickUpActionPerformable: ri = ReasoningInstance(self, PartialDesignator(PickUpActionPerformable, self.object_designator_description, self.arms, @@ -915,7 +895,7 @@ class PlaceAction(ActionDesignatorDescription): def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], target_locations: List[Pose], - arms: List[Arms], ontology_concept_holders: Optional[List[Thing]] = None): + arms: List[Arms] = None, ontology_concept_holders: Optional[List[Thing]] = None): """ Create an Action Description to place an object @@ -929,6 +909,7 @@ def __init__(self, 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) if self.soma: self.init_ontology_concepts({"placing": self.soma.Placing}) 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/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index d2c2787e8..71532f8fb 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -233,5 +233,9 @@ def __iter__(self) -> ActionDesignatorDescription.Action: 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(): + 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_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py index 2ef274bd3..3220510a8 100644 --- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -3,19 +3,19 @@ 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 +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 ...designators.location_designator import CostmapLocation -# from ...designators.object_designator import BelieveObject +from ...pose_generator_and_validator import PoseGenerator, reachability_validator from ...robot_description import RobotDescription from ...world_reasoning import visible -from ...costmaps import OccupancyCostmap +from ...costmaps import OccupancyCostmap, GaussianCostmap if TYPE_CHECKING: from ...designators.object_designator import ObjectDesignatorDescription @@ -43,16 +43,40 @@ def connect(self): def clear_state(self) -> None: pass - # def reachable(self, pose: Pose) -> bool: - # robot_desig = BelieveObject(types=[ObjectType.ROBOT]) - # c = CostmapLocation(pose, reachable_for=robot_desig.resolve()).resolve() - # if c.pose: - # return True - def reachable(self, pose: Pose) -> ReasoningResult: - return ReasoningResult(True) + """ + 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.copy() + 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) 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])) @@ -63,9 +87,13 @@ def graspable(self, object_designator: ObjectDesignatorDescription) -> Reasoning 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_x or dist > obj_y or dist > obj_z: - return ReasoningResult(True) + 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: @@ -73,16 +101,34 @@ def space_is_free(self, pose: Pose) -> ReasoningResult: 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, gripper: Arms) -> ReasoningResult: - 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) + 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/test/test_knowledge.py b/test/test_knowledge.py index 437699d9c..10f628665 100644 --- a/test/test_knowledge.py +++ b/test/test_knowledge.py @@ -131,9 +131,16 @@ def test_partial_desig_iter(self): 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) + 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) + class TestReasoningInstance(KnowledgeSourceTestCase): pass From 39e64f7307aaf743ce54e0170fb182794d22187e Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 27 Nov 2024 11:39:53 +0100 Subject: [PATCH 55/57] [knowledge] some tests and small fixes --- src/pycram/designators/action_designator.py | 6 ++---- src/pycram/knowledge/knowledge_engine.py | 1 + test/test_knowledge.py | 21 ++++++++++++++++++--- 3 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 9b3030b50..d72c8f610 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -1132,8 +1132,7 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = super().__init__(ontology_concept_holders) self.object_designator_description: ObjectPart = object_designator_description self.arms: List[Arms] = arms - self.knowledge_condition = ReachableProperty( - self.object_designator_description.resolve().pose) & GripperIsFreeProperty(self.arms[0]) + self.knowledge_condition = GripperIsFreeProperty(self.arms) if self.soma: self.init_ontology_concepts({"opening": self.soma.Opening}) @@ -1180,8 +1179,7 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] = super().__init__(ontology_concept_holders) self.object_designator_description: ObjectPart = object_designator_description self.arms: List[Arms] = arms - self.knowledge_condition = ReachableProperty( - self.object_designator_description.resolve().pose) & GripperIsFreeProperty(self.arms[0]) + self.knowledge_condition = GripperIsFreeProperty(self.arms) if self.soma: self.init_ontology_concepts({"closing": self.soma.Closing}) diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index 71532f8fb..ecb7a0663 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -234,6 +234,7 @@ def __iter__(self) -> ActionDesignatorDescription.Action: 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] diff --git a/test/test_knowledge.py b/test/test_knowledge.py index 10f628665..dd6e4cd95 100644 --- a/test/test_knowledge.py +++ b/test/test_knowledge.py @@ -1,10 +1,10 @@ from bullet_world_testcase import BulletWorldTestCase from knowledge_testcase import KnowledgeSourceTestCase, TestProperty, KnowledgeBulletTestCase -from pycram.datastructures.enums import Arms, Grasp +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 -from pycram.designators.object_designator import BelieveObject +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 @@ -141,6 +141,21 @@ def test_pickup_arm(self): 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 From 848accf838a5566802046dda50449d1b39a37ef5 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Wed, 27 Nov 2024 15:12:38 +0100 Subject: [PATCH 56/57] [knowledge] Changes to make tests work again --- src/pycram/datastructures/partial_designator.py | 17 +++++++++++++---- src/pycram/knowledge/knowledge_engine.py | 1 + .../knowledge_sources/facts_knowledge.py | 2 +- test/test_knowledge.py | 9 +++------ 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/src/pycram/datastructures/partial_designator.py b/src/pycram/datastructures/partial_designator.py index 4d8c221c1..f2439fae4 100644 --- a/src/pycram/datastructures/partial_designator.py +++ b/src/pycram/datastructures/partial_designator.py @@ -45,20 +45,29 @@ class PartialDesignator: 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(*args, **kwargs).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 # self.args = tuple(filter(None, args)) # self.kwargs = {k:v for k,v in kwargs.items() if v is not None} def __call__(self, *fargs, **fkwargs): """ - Creats a new PartialDesignator with the given arguments and keyword arguments added. + 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 """ - newkeywords = {**self.kwargs, **fkwargs} - return PartialDesignator(self.performable, *fargs, **newkeywords) + 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]: """ diff --git a/src/pycram/knowledge/knowledge_engine.py b/src/pycram/knowledge/knowledge_engine.py index ecb7a0663..f0d4d6496 100644 --- a/src/pycram/knowledge/knowledge_engine.py +++ b/src/pycram/knowledge/knowledge_engine.py @@ -238,5 +238,6 @@ def __iter__(self) -> ActionDesignatorDescription.Action: 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_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py index 3220510a8..8b40f9e9e 100644 --- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -51,7 +51,7 @@ def reachable(self, pose: Pose) -> ReasoningResult: :param pose: Pose which should be checked :return: A ReasoningResult with the result of the check and possible arms """ - ground_pose = pose.copy() + 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) diff --git a/test/test_knowledge.py b/test/test_knowledge.py index dd6e4cd95..9c186273a 100644 --- a/test/test_knowledge.py +++ b/test/test_knowledge.py @@ -87,21 +87,18 @@ 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.args, (test_object,)) - self.assertEqual(partial_desig.kwargs, {"arm": Arms.RIGHT}) + 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.args, ()) - self.assertEqual(partial_desig.kwargs, {"arm": Arms.RIGHT}) + 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(new_partial_desig.args, ()) - self.assertEqual(new_partial_desig.kwargs, {"arm": Arms.RIGHT, "grasp": Grasp.FRONT}) + 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) From 2bc42b6e6863cfa7aead83f3044b700e6dd50114 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Thu, 28 Nov 2024 09:01:48 +0100 Subject: [PATCH 57/57] [knowledge source] Postponed problems --- .../knowledge_sources/facts_knowledge.py | 37 ++++++++++--------- 1 file changed, 19 insertions(+), 18 deletions(-) diff --git a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py index 8b40f9e9e..d6b0594da 100644 --- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py +++ b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py @@ -51,24 +51,25 @@ def reachable(self, pose: Pose) -> ReasoningResult: :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) + # 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: """