diff --git a/.github/workflows/new-pycram-ci.yml b/.github/workflows/new-pycram-ci.yml index 789d297d9..018a3a992 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/test_demo.py diff --git a/demos/pycram_bullet_world_demo/demo.py b/demos/pycram_bullet_world_demo/demo.py index f77cd358f..c3bb46a97 100644 --- a/demos/pycram_bullet_world_demo/demo.py +++ b/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()) 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..9bd26c9cc --- /dev/null +++ b/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 \ No newline at end of file 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})