From 9fd274019331a900d10ad644aa6a69b7c25bccf5 Mon Sep 17 00:00:00 2001 From: Celina SuturoLaptop Date: Thu, 29 Feb 2024 16:25:13 +0100 Subject: [PATCH] [PickUp] tried to fix some issues --- src/pycram/designators/action_designator.py | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index 4c583b825..d8bd555e1 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -362,7 +362,7 @@ def perform(self) -> None: if self.object_designator.type == "Cutlery": print(f"Cutlery erkannt, rechne -x") print(special_knowledge_offset.pose.position.x) - special_knowledge_offset.pose.position.x -= 0.115 # 0.11 before, fork needs more + # special_knowledge_offset.pose.position.x -= 0.11 # 0.11 before, fork needs more print(special_knowledge_offset.pose.position.x) # if self.object_designator.type == "Fork": # special_knowledge_offset.pose.position.x -= 0.02 @@ -396,7 +396,7 @@ def perform(self) -> None: # Finalize the pick-up by closing the gripper and lifting the object rospy.logwarn("Close Gripper") - MoveGripperMotion(motion="close", gripper=self.arm).resolve().perform() + MoveGripperMotion(motion="close", gripper=self.arm, allow_gripper_collision=True).resolve().perform() rospy.logwarn("Lifting now") liftingTm = push_baseTm @@ -642,19 +642,14 @@ def perform(self) -> None: MoveJointsMotion(["wrist_flex_joint"], [-0.8]).resolve().perform() # correct a possible sloped orientation - NavigateAction([Pose([robot.get_pose().pose.position.x, robot.get_pose().pose.position.y, 0], - [robot.get_pose().orientation.x, robot.get_pose().orientation.y, - robot.get_pose().orientation.z, - robot.get_pose().orientation.w])]).resolve().perform() + NavigateAction([Pose([robot.get_pose().pose.position.x, robot.get_pose().pose.position.y, 0])]).resolve().perform() MoveGripperMotion(motion="open", gripper="left").resolve().perform() # Move away from the table # todo if turned in an other direction hsr is not moving backwards but forward NavigateAction( - [Pose([robot.get_pose().pose.position.x - 0.1, robot.get_pose().pose.position.y, 0], - [robot.get_pose().orientation.x, robot.get_pose().orientation.y, - robot.get_pose().orientation.z, robot.get_pose().orientation.w])]).resolve().perform() + [Pose([robot.get_pose().pose.position.x - 0.1, robot.get_pose().pose.position.y, 0])]).resolve().perform() # placing everything else except the Metalplate else: