Merge pull request #232 from Tigul/demo-ci

[ci] Added demo to test ci

.github/workflows/new-pycram-ci.yml

@@ -72,3 +72,9 @@ jobs:
           source /opt/ros/overlay_ws/devel/setup.bash 
           roscd pycram
           pytest -v test
+
+      - name: Run Demos
+        run: | 
+          source /opt/ros/overlay_ws/devel/setup.bash 
+          roscd pycram
+          python3 demos/pycram_bullet_world_demo/test_demo.py

demos/pycram_bullet_world_demo/demo.py

@@ -10,7 +10,10 @@
 from pycram.datastructures.dataclasses import Color
 from pycram.ros.viz_marker_publisher import VizMarkerPublisher
 from pycrap import Robot, Apartment, Milk, Cereal, Spoon, Bowl
+import numpy as np
 
+
+np.random.seed(420)
 extension = ObjectDescription.get_file_extension()
 
 world = BulletWorld(WorldMode.GUI)
@@ -22,7 +25,7 @@
               color=Color(1, 0, 0, 1))
 cereal = Object("cereal", Cereal, "breakfast_cereal.stl",
                 pose=Pose([2.5, 2.3, 1.05]), color=Color(0, 1, 0, 1))
-spoon = Object("spoon", Spoon, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]),
+spoon = Object("spoon", Spoon, "spoon.stl", pose=Pose([2.4, 2.24, 0.85]),
                color=Color(0, 0, 1, 1))
 bowl = Object("bowl", Bowl, "bowl.stl", pose=Pose([2.5, 2.2, 1.02]),
               color=Color(1, 1, 0, 1))
@@ -52,15 +55,15 @@ def move_and_detect(obj_type):
 
     milk_desig = move_and_detect(Milk)
 
-    TransportAction(milk_desig, [Arms.LEFT], [Pose([4.8, 3.55, 0.8])]).resolve().perform()
+    TransportAction(milk_desig,  [Pose([4.8, 3.55, 0.8])], [Arms.LEFT]).resolve().perform()
 
     cereal_desig = move_and_detect(Cereal)
 
-    TransportAction(cereal_desig, [Arms.RIGHT], [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])]).resolve().perform()
+    TransportAction(cereal_desig,  [Pose([5.2, 3.4, 0.8], [0, 0, 1, 1])],[Arms.RIGHT]).resolve().perform()
 
     bowl_desig = move_and_detect(Bowl)
 
-    TransportAction(bowl_desig, [Arms.LEFT], [Pose([5, 3.3, 0.8], [0, 0, 1, 1])]).resolve().perform()
+    TransportAction(bowl_desig,  [Pose([5, 3.3, 0.8], [0, 0, 1, 1])], [Arms.LEFT]).resolve().perform()
 
     # Finding and navigating to the drawer holding the spoon
     handle_desig = ObjectPart(names=["handle_cab10_t"], part_of=apartment_desig.resolve())

demos/pycram_bullet_world_demo/test_demo.py

@@ -0,0 +1,7 @@
+from pycram.datastructures.world import World
+
+try:
+    import demo
+except Exception as e:
+    World.current_world.exit()
+    raise e

src/pycram/designators/action_designator.py

@@ -849,8 +849,10 @@ def __init__(self,
             ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description
         self.arms: List[Arms] = arms
         self.grasps: List[Grasp] = grasps
+        object_desig = self.object_designator_description if isinstance(self.object_designator_description,
+                                                                        ObjectDesignatorDescription.Object) else self.object_designator_description.resolve()
         self.knowledge_condition = GraspableProperty(self.object_designator_description) & ReachableProperty(
-            self.object_designator_description.resolve().pose)
+            object_desig.pose)
 
     def __iter__(self) -> PickUpActionPerformable:
         ri = ReasoningInstance(self,
@@ -882,9 +884,11 @@ def __init__(self,
         super().__init__()
         self.object_designator_description: Union[
             ObjectDesignatorDescription, ObjectDesignatorDescription.Object] = object_designator_description
+        object_desig = self.object_designator_description if isinstance(self.object_designator_description,
+                                                                       ObjectDesignatorDescription.Object) else self.object_designator_description.resolve()
         self.target_locations: List[Pose] = target_locations
         self.arms: List[Arms] = arms
-        self.knowledge_condition = ReachableProperty(self.object_designator_description.resolve().pose)
+        self.knowledge_condition = ReachableProperty(object_desig.pose)
 
 
     def ground(self) -> PlaceActionPerformable:

src/pycram/knowledge/knowledge_sources/facts_knowledge.py

@@ -79,7 +79,8 @@ def graspable(self, object_designator: ObjectDesignatorDescription) -> Reasoning
         :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)
+            object_desig = object_designator.resolve() if hasattr(object_designator, "resolve") else object_designator
+            pro_obj = World.current_world.get_prospection_object_for_object(object_desig.world_object)
             pro_obj.set_pose(Pose([0, 0, 0], [0, 0, 0, 1]))
             bounding_box = pro_obj.get_axis_aligned_bounding_box()
 
@@ -112,7 +113,7 @@ def gripper_is_free(self, grippers: List[Arms]) -> ReasoningResult:
         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:
+                if att.parent_link.name == tool_frame_link or att.child_link.name == tool_frame_link:
                     return ReasoningResult(False)
             return ReasoningResult(True, {"gripper": gripper})