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 c5e1b37bc..fdb642cfd 100644 --- a/demos/pycram_hsrb_real_test_demos/pick-up-real.py +++ b/demos/pycram_hsrb_real_test_demos/pick-up-real.py @@ -9,8 +9,8 @@ CUTLERY = ["Spoon", "Fork", "Knife", "Plasticknife"] # Wished objects for the Demo -#wished_sorted_obj_list = ["Metalplate", "Bowl", "Metalmug", "Fork", "Cerealbox"] -wished_sorted_obj_list = ["Milkpack"] +wished_sorted_obj_list = ["Metalplate", "Bowl", "Metalmug", "Fork", "Cerealbox"] + # length of wished list for failure handling LEN_WISHED_SORTED_OBJ_LIST = len(wished_sorted_obj_list) diff --git a/demos/pycram_hsrb_real_test_demos/utils/misc.py b/demos/pycram_hsrb_real_test_demos/utils/misc.py index fa422d340..535f4f610 100644 --- a/demos/pycram_hsrb_real_test_demos/utils/misc.py +++ b/demos/pycram_hsrb_real_test_demos/utils/misc.py @@ -78,27 +78,28 @@ def try_pick_up(robot: BulletWorld.robot, obj: ObjectDesignatorDescription, gras PickUpAction(obj, ["left"], [grasps]).resolve().perform() except (EnvironmentUnreachable, GripperClosedCompletely): print("try pick up again") - TalkingMotion("Try pick up again") + TalkingMotion("Try pick up again").resolve().perform() # after failed attempt to pick up the object, the robot moves 30cm back on x pose + # TODO: x-pose und orentation sollten allgemein sein NavigateAction( [Pose([robot.get_pose().position.x + 0.3, robot.get_pose().position.y, - robot.get_pose().position.z])]).resolve().perform() + robot.get_pose().position.z], [0, 0, 1, 0])]).resolve().perform() ParkArmsAction([Arms.LEFT]).resolve().perform() # try to detect the object again object_desig = DetectAction(technique='default').resolve().perform() - # TODO nur wenn key (name des vorherigen objektes) in object_desig enthalten ist - # TODO umschreiben, geht so nicht mehr, da das dict in einem tupel ist - new_object = object_desig[1][obj.type] - print(new_object) - # when the robot just grasped next to the object - # TODO wieso unterscheiden wir hier überhaupt, wenn er daneben gegriffen hat, hat er das objekt - # TODO wahrscheinlich verschoben und sollte auch nochmal perceiven + new_object = sort_objects(robot, object_desig, [obj.type])[0] + # second try to pick up the object try: + TalkingMotion("try again").resolve().perform() PickUpAction(new_object, ["left"], [grasps]).resolve().perform() # ask for human interaction if it fails a second time - except: - TalkingMotion(f"Can you pleas give me the {obj.type} on the table?") + except (EnvironmentUnreachable, GripperClosedCompletely): + NavigateAction( + [Pose([robot.get_pose().position.x + 0.3, robot.get_pose().position.y, + robot.get_pose().position.z], [0, 0, 1, 0])]).resolve().perform() + ParkArmsAction([Arms.LEFT]).resolve().perform() + TalkingMotion(f"Can you pleas give me the {obj.type} on the table?").resolve().perform() MoveGripperMotion("open", "left").resolve().perform() time.sleep(4) MoveGripperMotion("close", "left").resolve().perform() diff --git a/demos/pycram_receptionist_demo/M2_Demo.py b/demos/pycram_receptionist_demo/M2_Demo.py index 1e32224ab..63fd7dc17 100644 --- a/demos/pycram_receptionist_demo/M2_Demo.py +++ b/demos/pycram_receptionist_demo/M2_Demo.py @@ -31,7 +31,7 @@ RobotStateUpdater("/tf", "/giskard_joint_states") robot_orientation_couch = axis_angle_to_quaternion([0, 0, 1], 180) -pose_couch = Pose([3, 5, 0]) +pose_couch = Pose([3, 5, 0], [0, 0, 1, 0]) robot_orientation_from_couch = axis_angle_to_quaternion([0, 0, 1], -90) pose_from_couch = Pose([4.2, 3.8, 0], robot_orientation_from_couch) @@ -53,7 +53,6 @@ def demo_ms2(area): guest1 = HumanDescription("guest1") # look for human - # TODO: only continue when human stands for longer than 3s in front of robot DetectAction(technique='human', state='start').resolve().perform() rospy.loginfo("human detected") @@ -68,18 +67,17 @@ def demo_ms2(area): # wait for human to say something rospy.sleep(15) - # guest_data format is = ['person infos: "name', 'drink"'] + # guest_data format is = [ "name", "drink"] guest_data = get_guest_info("1.0") print(get_guest_info("1.0")) - while guest_data == ['person_infos: "No name saved under this ID!"']: + while guest_data == [' "No name saved under this ID!"']: talk_error("no name") rospy.sleep(13) guest_data = get_guest_info("1.0") # set heard name and drink of guest - guest1.set_name(guest_data[0][13:]) + guest1.set_name(guest_data[0]) guest1.set_drink(guest_data[1]) - rospy.loginfo("after while") talk_request(guest_data) rospy.sleep(1) @@ -96,7 +94,7 @@ def demo_ms2(area): if area == 'to_couch': # wait for human to step out of way - rospy.sleep(5) + rospy.sleep(3) NavigateAction([pose_kitchen_to_couch]).resolve().perform() NavigateAction([pose_couch]).resolve().perform() TalkingMotion("Welcome to the living room").resolve().perform() @@ -114,5 +112,4 @@ def demo_ms2(area): introduce(guest1.name, guest1.fav_drink, host.name, host.fav_drink) -# demo_test('from_couch') -demo_ms2('now') +demo_ms2('to_couch') diff --git a/demos/pycram_receptionist_demo/tests/fortoday.py b/demos/pycram_receptionist_demo/tests/fortoday.py index e69de29bb..f37491bd9 100644 --- a/demos/pycram_receptionist_demo/tests/fortoday.py +++ b/demos/pycram_receptionist_demo/tests/fortoday.py @@ -0,0 +1,22 @@ +import rospy + +from pycram.designators.action_designator import DetectAction, NavigateAction +from pycram.designators.motion_designator import TalkingMotion +from pycram.fluent import Fluent +from demos.pycram_receptionist_demo.utils.misc import * +from pycram.helper import axis_angle_to_quaternion +from pycram.process_module import real_robot +import pycram.external_interfaces.giskard as giskardpy +from pycram.external_interfaces.knowrob import get_guest_info +from pycram.ros.robot_state_updater import RobotStateUpdater +from pycram.ros.viz_marker_publisher import VizMarkerPublisher +from pycram.designators.location_designator import * +from pycram.designators.object_designator import * +from pycram.bullet_world import BulletWorld, Object +from std_msgs.msg import String, Bool +from demos.pycram_receptionist_demo.deprecated import talk_actions +import pycram.external_interfaces.navigate as moveBase + +x = get_guest_info("1.0") +print(x[0]) +print(x[1]) \ No newline at end of file diff --git a/demos/pycram_receptionist_demo/utils/misc.py b/demos/pycram_receptionist_demo/utils/misc.py index ae0a8d68b..6964f888c 100644 --- a/demos/pycram_receptionist_demo/utils/misc.py +++ b/demos/pycram_receptionist_demo/utils/misc.py @@ -17,7 +17,7 @@ def talk_request(data: list): """ rospy.loginfo("in callback success") - toyas_text = "Hey " + data[0][13:] + " your favorite drink is " + data[ + toyas_text = "Hey " + data[0] + " your favorite drink is " + data[ 1] TalkingMotion(toyas_text).resolve().perform() rospy.sleep(1) diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index c2adb3ba4..d9dbe2252 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -342,7 +342,6 @@ def perform(self) -> None: # Execute Bool, because sometimes u only want to visualize the poses to test things if execute: MoveTCPMotion(oTmG, self.arm).resolve().perform() - raise EnvironmentUnreachable # Calculate and apply any special knowledge offsets based on the robot and object type # Note: This currently includes robot-specific logic that should be generalized tool_frame = robot_description.get_tool_frame(self.arm)