From 3135ea06c049c0798634b7054d07beedfae9aef4 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 29 Nov 2024 13:12:23 +0100 Subject: [PATCH 1/4] [ci] Added demo to test ci and some quick fixes --- .github/workflows/new-pycram-ci.yml | 6 +++++ demos/pycram_bullet_world_demo/demo.py | 26 +++++++++---------- src/pycram/designators/action_designator.py | 8 ++++-- .../knowledge_sources/facts_knowledge.py | 5 ++-- 4 files changed, 28 insertions(+), 17 deletions(-) diff --git a/.github/workflows/new-pycram-ci.yml b/.github/workflows/new-pycram-ci.yml index 789d297d9..10a1a8c94 100644 --- a/.github/workflows/new-pycram-ci.yml +++ b/.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/demo.py diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index c307fe7ce..be65018a4 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -9,22 +9,22 @@ from pycram.world_concepts.world_object import Object from pycram.datastructures.dataclasses import Color from pycram.ros.viz_marker_publisher import VizMarkerPublisher -from pycrap import Robot +from pycrap import Robot, Apartment, Milk, Cereal, Spoon, Bowl extension = ObjectDescription.get_file_extension() world = BulletWorld(WorldMode.GUI) robot = Object("pr2", Robot, f"pr2{extension}", pose=Pose([1, 2, 0])) -apartment = Object("apartment", ObjectType.ENVIRONMENT, f"apartment{extension}") +apartment = Object("apartment", Apartment, f"apartment{extension}") -milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([2.5, 2, 1.02]), +milk = Object("milk", Milk, "milk.stl", pose=Pose([2.5, 2, 1.02]), color=Color(1, 0, 0, 1)) -cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", +cereal = Object("cereal", Cereal, "breakfast_cereal.stl", pose=Pose([2.5, 2.3, 1.05]), color=Color(0, 1, 0, 1)) -spoon = Object("spoon", ObjectType.SPOON, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), +spoon = Object("spoon", Spoon, "spoon.stl", pose=Pose([2.4, 2.2, 0.85]), color=Color(0, 0, 1, 1)) -bowl = Object("bowl", ObjectType.BOWL, "bowl.stl", pose=Pose([2.5, 2.2, 1.02]), +bowl = Object("bowl", Bowl, "bowl.stl", pose=Pose([2.5, 2.2, 1.02]), color=Color(1, 1, 0, 1)) apartment.attach(spoon, 'cabinet10_drawer_top') @@ -50,17 +50,17 @@ def move_and_detect(obj_type): MoveTorsoAction([0.25]).resolve().perform() - milk_desig = move_and_detect(ObjectType.MILK) + 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(ObjectType.BREAKFAST_CEREAL) + 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(ObjectType.BOWL) + 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()) @@ -75,7 +75,7 @@ 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=[ObjectType.SPOON])).resolve().perform() + spoon_desig = DetectAction(BelieveObject(types=[Spoon])).resolve().perform() pickup_arm = Arms.LEFT if drawer_open_loc.arms[0] == Arms.RIGHT else Arms.RIGHT PickUpAction(spoon_desig, [pickup_arm], [Grasp.TOP]).resolve().perform() diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 592448950..ae96e5a08 100644 --- a/src/pycram/designators/action_designator.py +++ b/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: diff --git a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py b/src/pycram/knowledge/knowledge_sources/facts_knowledge.py index d6b0594da..367acaf9a 100644 --- a/src/pycram/knowledge/knowledge_sources/facts_knowledge.py +++ b/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}) From bc76292461a848a9a361a518410860bfb53daae5 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 29 Nov 2024 13:39:24 +0100 Subject: [PATCH 2/4] [ci] added test environment --- .github/workflows/new-pycram-ci.yml | 2 +- demos/pycram_bullet_world_demo/test_demo.py | 8 ++++++++ 2 files changed, 9 insertions(+), 1 deletion(-) create mode 100644 demos/pycram_bullet_world_demo/test_demo.py diff --git a/.github/workflows/new-pycram-ci.yml b/.github/workflows/new-pycram-ci.yml index 10a1a8c94..018a3a992 100644 --- a/.github/workflows/new-pycram-ci.yml +++ b/.github/workflows/new-pycram-ci.yml @@ -77,4 +77,4 @@ jobs: run: | source /opt/ros/overlay_ws/devel/setup.bash roscd pycram - python3 demos/pycram_bullet_world_demo/demo.py + python3 demos/pycram_bullet_world_demo/test_demo.py diff --git a/demos/pycram_bullet_world_demo/test_demo.py b/demos/pycram_bullet_world_demo/test_demo.py new file mode 100644 index 000000000..7114e0960 --- /dev/null +++ b/demos/pycram_bullet_world_demo/test_demo.py @@ -0,0 +1,8 @@ +from pycram.datastructures.world import World + +try: + import demo +except Exception as e: + print(e) + World.current_world.exit() + exit(1) \ No newline at end of file From 416c2efe49798b89e7b0501b92fae8eb3f14cda3 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Fri, 29 Nov 2024 15:42:14 +0100 Subject: [PATCH 3/4] [demo] Added seed for the demo --- demos/pycram_bullet_world_demo/demo.py | 2 ++ demos/pycram_bullet_world_demo/test_demo.py | 3 +-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index be65018a4..16aba3828 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -10,7 +10,9 @@ 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) diff --git a/demos/pycram_bullet_world_demo/test_demo.py b/demos/pycram_bullet_world_demo/test_demo.py index 7114e0960..9bd26c9cc 100644 --- a/demos/pycram_bullet_world_demo/test_demo.py +++ b/demos/pycram_bullet_world_demo/test_demo.py @@ -3,6 +3,5 @@ try: import demo except Exception as e: - print(e) World.current_world.exit() - exit(1) \ No newline at end of file + raise e \ No newline at end of file From a5ef16585c86b2ea3da3fe57f84589fd4f42a334 Mon Sep 17 00:00:00 2001 From: Jonas Dech Date: Mon, 2 Dec 2024 14:22:31 +0100 Subject: [PATCH 4/4] [demos] Moved the spoon a little bit --- demos/pycram_bullet_world_demo/demo.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index 12679e57d..c3bb46a97 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/demos/pycram_bullet_world_demo/demo.py @@ -25,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))