[PickUpAndPlace] changed Bowl to Metalbowl

demos/pycram_hsrb_real_test_demos/pick-up-real.py

@@ -9,7 +9,7 @@
 CUTLERY = ["Spoon", "Fork", "Knife", "Plasticknife"]
 
 # Wished objects for the Demo
-wished_sorted_obj_list = ["Metalplate", "Bowl", "Metalmug", "Fork", "Cerealbox"]
+wished_sorted_obj_list = ["Spoon", "Metalbowl"]
 
 
 # length of wished list for failure handling
@@ -42,6 +42,8 @@
 # Define orientation for objects
 object_orientation = axis_angle_to_quaternion([0, 0, 1], 180)
 
+giskardpy.sync_worlds()
+
 
 class PlacingZPose(Enum):
     """
@@ -52,7 +54,7 @@ class PlacingZPose(Enum):
     FORK = 0.8
     PLASTICKNIFE = 0.8
     KNIFE = 0.8
-    BOWL = 0.84
+    Metalbowl = 0.84
     MILKPACK = 0.88
     METALMUG = 0.8
     CEREALBOX = 0.9
@@ -75,7 +77,7 @@ def pickup_and_place_objects(sorted_obj: list):
         if sorted_obj[value].type in CUTLERY:
             sorted_obj[value].type = "Cutlery"
 
-        if sorted_obj[value].type in ["Bowl", "Cutlery"]:
+        if sorted_obj[value].type in ["Metalbowl", "Cutlery"]:
             grasp = "top"
 
         if sorted_obj[value].type == "Metalplate":
@@ -94,32 +96,33 @@ def pickup_and_place_objects(sorted_obj: list):
         # placing the object
         ParkArmsAction([Arms.LEFT]).resolve().perform()
         TalkingMotion("Navigating").resolve().perform()
-        navigate_to(1.8, 1.8, "long table")
-        navigate_to(4.1, y_pos, "long table")
-        TalkingMotion("Placing").resolve().perform()
-
-        z = get_z(sorted_obj[value].type)
-        if sorted_obj[value].type == "Metalplate":
-            # with special defined placing movement for the plate
-            PlaceGivenObjAction([sorted_obj[value].type], ["left"],
-                                [Pose([4.86, y_pos, z])],["front"]).resolve().perform()
-        else:
-            PlaceAction(sorted_obj[value], ["left"], [grasp],
-                        [Pose([4.87, y_pos, z])]).resolve().perform()
-
-        ParkArmsAction([Arms.LEFT]).resolve().perform()
-        TalkingMotion("Navigating").resolve().perform()
-        navigate_to(3.9, 2, "popcorn table")
-
-        # adjust y_pos for the next placing round
-        if sorted_obj[value].type == "Metalplate":
-            y_pos += 0.3
-        else:
-            y_pos += 0.16
-
-        # navigates back if a next object exists
+        MoveGripperMotion("open","left").resolve().perform()
+        # navigate_to(1.8, 1.8, "long table")
+        # navigate_to(4.1, y_pos, "long table")
+        # TalkingMotion("Placing").resolve().perform()
+        #
+        # z = get_z(sorted_obj[value].type)
+        # if sorted_obj[value].type == "Metalplate":
+        #     # with special defined placing movement for the plate
+        #     PlaceGivenObjAction([sorted_obj[value].type], ["left"],
+        #                         [Pose([4.86, y_pos, z])],["front"]).resolve().perform()
+        # else:
+        #     PlaceAction(sorted_obj[value], ["left"], [grasp],
+        #                 [Pose([4.87, y_pos, z])]).resolve().perform()
+        #
+        # ParkArmsAction([Arms.LEFT]).resolve().perform()
+        # TalkingMotion("Navigating").resolve().perform()
+        # navigate_to(3.9, 2, "popcorn table")
+        #
+        # # adjust y_pos for the next placing round
+        # if sorted_obj[value].type == "Metalplate":
+        #     y_pos += 0.3
+        # else:
+        #     y_pos += 0.16
+        #
+        # # navigates back if a next object exists
         if value + 1 < len(sorted_obj):
-            navigate_to(1.6, sorted_obj[value + 1].pose.position.y, "popcorn table")
+             navigate_to(1.6, sorted_obj[value + 1].pose.position.y, "popcorn table")
 
 
 def get_z(obj_type: str):
@@ -161,6 +164,7 @@ def navigate_and_detect():
     TalkingMotion("Perceiving").resolve().perform()
     try:
         object_desig = DetectAction(technique='all').resolve().perform()
+        giskardpy.sync_worlds()
     except PerceptionObjectNotFound:
         object_desig = {}
     return object_desig
@@ -223,7 +227,7 @@ def navigate_and_detect():
 
             for val in range(len(wished_sorted_obj_list)):
                 grasp = "front"
-                if wished_sorted_obj_list[val] in (["Bowl"] + CUTLERY):
+                if wished_sorted_obj_list[val] in (["Metalbowl"] + CUTLERY):
                     grasp = "top"
 
                 print(f"next object is: {wished_sorted_obj_list[val]}")

src/pycram/designators/action_designator.py

@@ -316,7 +316,7 @@ def perform(self) -> None:
 
             # Adjust object pose for top-grasping, if applicable
             if self.grasp == "top":
-                # Handle special cases for certain object types (e.g., Cutlery, Bowl)
+                # Handle special cases for certain object types (e.g., Cutlery, Metalbowl)
                 # Note: This includes hardcoded adjustments and should ideally be generalized
                 if self.object_designator.type == "Cutlery":
                     # todo: this z is the popcorn-table height, we need to define location to get that z otherwise it
@@ -351,7 +351,7 @@ def perform(self) -> None:
             #  depending on robot
             if robot.name == "hsrb":
                 if self.grasp == "top":
-                    if self.object_designator.type == "Bowl":
+                    if self.object_designator.type == "Metalbowl":
                         special_knowledge_offset.pose.position.y += 0.085
                         special_knowledge_offset.pose.position.x -= 0.03
 
@@ -362,7 +362,7 @@ def perform(self) -> None:
                 z = 0.04
                 if self.grasp == "top":
                     z = 0.025
-                    if self.object_designator.type == "Bowl":
+                    if self.object_designator.type == "Metalbowl":
                         z = 0.044
                 push_base.pose.position.z += z
             push_baseTm = lt.transform_pose(push_base, "map")

src/pycram/external_interfaces/giskard.py

@@ -170,6 +170,7 @@ def achieve_joint_goal(goal_poses: Dict[str, float]) -> 'MoveResult':
     :return: MoveResult message for this goal
     """
     sync_worlds()
+    rospy.sleep(5)
     giskard_wrapper.set_joint_goal(goal_poses)
     return giskard_wrapper.plan_and_execute()
 
@@ -184,7 +185,7 @@ def achieve_cartesian_goal(goal_pose: Pose, tip_link: str, root_link: str) -> 'M
     :param root_link: The starting link of the chain which should be used to achieve this goal
     :return: MoveResult message for this goal
     """
-
+    sync_worlds()
     giskard_wrapper.avoid_all_collisions()
     giskard_wrapper.set_cart_goal(_pose_to_pose_stamped(goal_pose), tip_link, root_link)
     return giskard_wrapper.plan_and_execute()