From 30ec7c4e549b416f199a3672c6d158f8a0e40706 Mon Sep 17 00:00:00 2001 From: Celina SuturoLaptop Date: Thu, 21 Mar 2024 10:41:13 +0100 Subject: [PATCH] [PickUpAndPlace] changed Bowl to Metalbowl --- .../pick-up-real.py | 62 ++++++++++--------- src/pycram/designators/action_designator.py | 6 +- src/pycram/external_interfaces/giskard.py | 3 +- 3 files changed, 38 insertions(+), 33 deletions(-) diff --git a/demos/pycram_hsrb_real_test_demos/pick-up-real.py b/demos/pycram_hsrb_real_test_demos/pick-up-real.py index fdb642cfd..9c6a3a88e 100644 --- a/demos/pycram_hsrb_real_test_demos/pick-up-real.py +++ b/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]}") diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index d9dbe2252..44c4788d5 100644 --- a/src/pycram/designators/action_designator.py +++ b/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") diff --git a/src/pycram/external_interfaces/giskard.py b/src/pycram/external_interfaces/giskard.py index 241169955..19a9082d4 100644 --- a/src/pycram/external_interfaces/giskard.py +++ b/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()