Merge pull request #235 from sunava/robokudo-real-robot

[detectaction] Robokudo real robot

demos/pycram_bullet_world_demo/demo.py

@@ -2,7 +2,7 @@
 from pycram.designators.action_designator import *
 from pycram.designators.location_designator import *
 from pycram.designators.object_designator import *
-from pycram.datastructures.enums import  WorldMode
+from pycram.datastructures.enums import WorldMode
 from pycram.datastructures.pose import Pose
 from pycram.process_module import simulated_robot, with_simulated_robot
 from pycram.object_descriptors.urdf import ObjectDescription
@@ -43,9 +43,8 @@ def move_and_detect(obj_type):
 
     LookAtAction(targets=[pick_pose]).resolve().perform()
 
-    object_desig = DetectAction(BelieveObject(types=[obj_type])).resolve().perform()
-
-    return object_desig
+    object_desig = DetectAction(technique=DetectionTechnique.TYPES, object_designator_description=BelieveObject(types=[obj_type])).resolve().perform()
+    return object_desig[0]
 
 
 with simulated_robot:
@@ -78,8 +77,9 @@ def move_and_detect(obj_type):
     # Detect and pickup the spoon
     LookAtAction([apartment.get_link_pose("handle_cab10_t")]).resolve().perform()
 
-    spoon_desig = DetectAction(BelieveObject(types=[Spoon])).resolve().perform()
-
+    spoon_desigs = DetectAction(technique=DetectionTechnique.TYPES,
+                               object_designator_description=BelieveObject(types=[Spoon])).resolve().perform()
+    spoon_desig = spoon_desigs[0]
     pickup_arm = Arms.LEFT if drawer_open_loc.arms[0] == Arms.RIGHT else Arms.RIGHT
     PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform()
 

examples/motion_designator.md

@@ -105,22 +105,30 @@ with simulated_robot:
 ## Detecting
 
 This is the motion designator implementation of detecting, if an object with the given object type is in the field of
-view (FOV) this motion designator will return an object designator describing the object.
+view (FOV) this motion designator will return a list of  object designators describing the objects. It is important to specify the 
+technique and state of the detection. You can also optional specify a region in which the object should be detected.
+
 
 Since we need an object that we can detect, we will spawn a milk for this.
 
 ```python
 from pycram.designators.motion_designator import DetectingMotion, LookingMotion
 from pycram.process_module import simulated_robot
+from pycram.datastructures.pose import Pose
+from pycram.datastructures.enums import DetectionTechnique, DetectionState
+from pycrap import  Milk
+from pycram.designators.object_designator import BelieveObject
+
 
 with simulated_robot:
     LookingMotion(target=Pose([1.5, 0, 1], [0, 0, 0, 1])).perform()
 
-    motion_description = DetectingMotion(object_type=pycrap.Milk)
+    motion_description = DetectingMotion(technique=DetectionTechnique.TYPES,state=DetectionState.START, object_designator_description=BelieveObject(types=[Milk]),
+                               region=None)
 
     obj = motion_description.perform()
 
-    print(obj)
+    print(obj[0])
 ```
 
 ## Move Arm Joints

src/pycram/datastructures/enums.py

@@ -129,15 +129,6 @@ class GripperType(Enum):
     CUSTOM = auto()
 
 
-class PerceptionTechniques(Enum):
-    """
-    Enum for techniques for perception tasks.
-    """
-    ALL = auto()
-    HUMAN = auto()
-    TYPES = auto()
-
-
 class ImageEnum(Enum):
     """
     Enum for image switch view on hsrb display.
@@ -164,6 +155,27 @@ class ImageEnum(Enum):
     CHAIR = 37
 
 
+class DetectionTechnique(int, Enum):
+    """
+    Enum for techniques for detection tasks.
+    """
+    ALL = 0
+    HUMAN = 1
+    TYPES = 2
+    REGION = 3
+    HUMAN_ATTRIBUTES = 4
+    HUMAN_WAVING = 5
+
+
+class DetectionState(int, Enum):
+    """
+    Enum for the state of the detection task.
+    """
+    START = 0
+    STOP = 1
+    PAUSE = 2
+
+
 class LoggerLevel(Enum):
     """
     Enum for the different logger levels.

src/pycram/datastructures/world.py

@@ -13,7 +13,7 @@
 from typing_extensions import List, Optional, Dict, Tuple, Callable, TYPE_CHECKING, Union, Type
 
 import pycrap
-from pycrap import PhysicalObject
+from pycrap import PhysicalObject, Floor, Apartment, Robot
 from ..cache_manager import CacheManager
 from ..config.world_conf import WorldConfig
 from ..datastructures.dataclasses import (Color, AxisAlignedBoundingBox, CollisionCallbacks,
@@ -401,6 +401,12 @@ def get_object_by_id(self, obj_id: int) -> Object:
         """
         return list(filter(lambda obj: obj.id == obj_id, self.objects))[0]
 
+    def get_scene_objects(self) -> List[Object]:
+        """
+        :return: A list of all objects in the world except the robot, floor, and apartment.
+        """
+        return [obj for obj in self.objects if obj.obj_type not in {Robot, Floor, Apartment}]
+
     def remove_visual_object(self, obj_id: int) -> bool:
         """
         Remove the object with the given id from the world, and saves a new original state for the world.
@@ -1637,6 +1643,7 @@ class UseProspectionWorld:
         with UseProspectionWorld():
             NavigateAction.Action([[1, 0, 0], [0, 0, 0, 1]]).perform()
     """
+
     def __init__(self):
         self.prev_world: Optional[World] = None
         # The previous world is saved to restore it after the with block is exited.

src/pycram/designators/action_designator.py

@@ -10,6 +10,7 @@
 from tf import transformations
 from typing_extensions import List, Union, Callable, Optional, Type
 
+from pycrap import PhysicalObject, Location
 from .location_designator import CostmapLocation
 from .motion_designator import MoveJointsMotion, MoveGripperMotion, MoveArmJointsMotion, MoveTCPMotion, MoveMotion, \
     LookingMotion, DetectingMotion, OpeningMotion, ClosingMotion
@@ -25,7 +26,8 @@
 
 from owlready2 import Thing
 
-from ..datastructures.enums import Arms, Grasp, GripperState, MovementType
+from ..datastructures.enums import Arms, Grasp, GripperState, DetectionTechnique, DetectionState, MovementType
+
 from ..designator import ActionDesignatorDescription
 from ..datastructures.pose import Pose
 from ..datastructures.world import World
@@ -467,17 +469,32 @@ def plan(self) -> None:
 class DetectActionPerformable(ActionAbstract):
     """
     Detects an object that fits the object description and returns an object designator_description describing the object.
+
+    If no object is found, an PerceptionObjectNotFound error is raised.
     """
 
-    object_designator: ObjectDesignatorDescription.Object
+    technique: DetectionTechnique
+    """
+    The technique that should be used for detection
+    """
+    state: DetectionState
+    """
+    The state of the detection, e.g Start Stop for continues perception
+    """
+    object_designator_description: Optional[ObjectDesignatorDescription] = None
     """
-    Object designator_description loosely describing the object, e.g. only type. 
+    The type of the object that should be detected, only considered if technique is equal to Type
+    """
+    region: Optional[Location] = None
+    """
+    The region in which the object should be detected
     """
     orm_class: Type[ActionAbstract] = field(init=False, default=ORMDetectAction)
 
     @with_tree
     def plan(self) -> None:
-        return DetectingMotion(object_type=self.object_designator.obj_type).perform()
+        return DetectingMotion(technique=self.technique,state=self.state, object_designator_description=self.object_designator_description,
+                               region=self.region).perform()
 
 
 @dataclass
@@ -684,8 +701,6 @@ def plan(self):
         PlaceActionPerformable(self.object_designator, self.arm, self.target_location).perform()
 
 
-
-
 # ----------------------------------------------------------------------------
 #               Action Designators Description
 # ----------------------------------------------------------------------------
@@ -742,7 +757,6 @@ def __init__(self, grippers: List[Arms], motions: List[GripperState]):
         self.grippers: List[Arms] = grippers
         self.motions: List[GripperState] = motions
 
-
     def ground(self) -> SetGripperActionPerformable:
         """
         Default specialized_designators that returns a performable designator_description with the first element in the grippers and motions list.
@@ -775,13 +789,13 @@ def __init__(self, object_designator_description: ObjectDesignatorDescription, g
         self.grippers: List[Arms] = grippers
         self.object_designator_description = object_designator_description
 
-
     def ground(self) -> ReleaseActionPerformable:
         return ReleaseActionPerformable(self.grippers[0], self.object_designator_description.ground())
 
     def __iter__(self):
         ri = ReasoningInstance(self,
-                                 PartialDesignator(ReleaseActionPerformable, self.grippers, self.object_designator_description))
+                               PartialDesignator(ReleaseActionPerformable, self.grippers,
+                                                 self.object_designator_description))
         for desig in ri:
             yield desig
 
@@ -806,14 +820,14 @@ def __init__(self, object_designator_description: ObjectDesignatorDescription, g
         self.object_designator_description: ObjectDesignatorDescription = object_designator_description
         self.efforts: List[float] = efforts
 
-
     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))
+                               PartialDesignator(GripActionPerformable, self.grippers,
+                                                 self.object_designator_description,
+                                                 self.efforts))
         for desig in ri:
             yield desig
 
@@ -834,7 +848,6 @@ def __init__(self, arms: List[Arms]):
         super().__init__()
         self.arms: List[Arms] = arms
 
-
     def ground(self) -> ParkArmsActionPerformable:
         """
         Default specialized_designators that returns a performable designator_description with the first element of the list of possible arms
@@ -920,7 +933,6 @@ def __init__(self,
         self.arms: List[Arms] = arms
         self.knowledge_condition = ReachableProperty(object_desig.pose)
 
-
     def ground(self) -> PlaceActionPerformable:
         """
         Default specialized_designators that returns a performable designator_description with the first entries from the list of possible entries.
@@ -1011,6 +1023,7 @@ def __init__(self,
         self.target_locations: List[Pose] = target_locations
         self.pickup_prepose_distance: float = pickup_prepose_distance
 
+
     def ground(self) -> TransportActionPerformable:
         """
         Default specialized_designators that returns a performable designator_description with the first entries from the lists of possible parameter.
@@ -1024,6 +1037,7 @@ def ground(self) -> TransportActionPerformable:
         return TransportActionPerformable(obj_desig, self.target_locations[0],  self.arms[0],
                                           self.pickup_prepose_distance)
 
+
     def __iter__(self) -> TransportActionPerformable:
         obj_desig = self.object_designator_description \
             if isinstance(self.object_designator_description, ObjectDesignatorDescription.Object) \
@@ -1051,7 +1065,6 @@ def __init__(self, targets: List[Pose]):
         super().__init__()
         self.targets: List[Pose] = targets
 
-
     def ground(self) -> LookAtActionPerformable:
         """
         Default specialized_designators that returns a performable designator_description with the first entry in the list of possible targets
@@ -1077,34 +1090,35 @@ class DetectAction(ActionDesignatorDescription):
 
     performable_class = DetectActionPerformable
 
-    def __init__(self, object_designator_description: ObjectDesignatorDescription,
-                 ontology_concept_holders: Optional[List[Thing]] = None):
+    def __init__(self, technique: DetectionTechnique, state: Optional[DetectionState] = None,
+                 object_designator_description: Optional[ObjectDesignatorDescription] = None, region: Optional[Location] = None):
         """
         Tries to detect an object in the field of view (FOV) of the robot.
 
-        :param object_designator_description: Object designator_description describing the object
-        """
+      """
         super().__init__()
-        self.object_designator_description: ObjectDesignatorDescription = object_designator_description
-        self.knowledge_condition = VisibleProperty(self.object_designator_description)
-
+        self.technique: DetectionTechnique = technique
+        self.state: DetectionState = DetectionState.START if state is None else state
+        self.object_designator_description: Optional[ObjectDesignatorDescription] = object_designator_description
+        self.region: Optional[Location] = region
+        #TODO: Implement knowledge condition
+        # self.knowledge_condition = VisibleProperty(self.object_designator_description)
 
     def ground(self) -> DetectActionPerformable:
         """
         Default specialized_designators that returns a performable designator_description with the executed object description.
 
         :return: A performable designator_description
         """
-        return DetectActionPerformable(self.object_designator_description.resolve())
+        return DetectActionPerformable(self.technique, self.state, self.object_designator_description, self.region)
 
     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)
+        yield DetectActionPerformable(self.technique, self.state, self.object_designator_description, self.region)
 
 
 class OpenAction(ActionDesignatorDescription):
@@ -1170,7 +1184,6 @@ def __init__(self, object_designator_description: ObjectPart, arms: List[Arms] =
         self.arms: List[Arms] = arms
         self.knowledge_condition = GripperIsFreeProperty(self.arms)
 
-
     def ground(self) -> CloseActionPerformable:
         """
         Default specialized_designators that returns a performable designator_description with the executed object designator_description and the first entry from
@@ -1211,7 +1224,6 @@ def __init__(self, object_description: Union[ObjectDesignatorDescription, Object
         self.arms: List[Arms] = arms
         self.object_description: ObjectDesignatorDescription = object_description
 
-
     def ground(self) -> GraspingActionPerformable:
         """
         Default specialized_designators that takes the first element from the list of arms and the first solution for the object
@@ -1232,4 +1244,3 @@ def __iter__(self) -> CloseActionPerformable:
                                PartialDesignator(GraspingActionPerformable, self.object_description, self.arms))
         for desig in ri:
             yield desig
-