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 87796637c..3d8ff5822 100644 --- a/demos/pycram_hsrb_real_test_demos/pick-up-real.py +++ b/demos/pycram_hsrb_real_test_demos/pick-up-real.py @@ -11,7 +11,7 @@ from pycram.ros.viz_marker_publisher import VizMarkerPublisher import pycram.external_interfaces.giskard as giskardpy from demos.pycram_hsrb_real_test_demos.utils.misc import * -#from pycram.external_interfaces.knowrob import get_table_pose +from pycram.external_interfaces.knowrob import get_table_pose # Initialize the Bullet world for simulation world = BulletWorld() @@ -28,7 +28,7 @@ robot.set_color([0.5, 0.5, 0.9, 1]) # Create environmental objects -apartment = Object("kitchen", ObjectType.ENVIRONMENT, "couch-kitchen.urdf") +apartment = Object("kitchen", ObjectType.ENVIRONMENT, "couch-kitchenmarch1.urdf") # Define orientation for objects object_orientation = axis_angle_to_quaternion([0, 0, 1], 180) @@ -116,7 +116,7 @@ def pickup_and_place_objects(robot, sorted_obj): PlaceGivenObjAction([sorted_obj[value].type], ["left"], [Pose([4.86, y_pos, 0])], ["front"]).resolve().perform() else: - PlaceAction(sorted_obj[value], ["left"], [grasp], [Pose([4.85, y_pos, z])]).resolve().perform() + PlaceAction(sorted_obj[value], ["left"], [grasp], [Pose([4.87, y_pos, z])]).resolve().perform() ParkArmsAction([Arms.LEFT]).resolve().perform() TalkingMotion("Navigating").resolve().perform() @@ -145,10 +145,10 @@ def get_z(obj_type: str): return PlacingZPose[obj_type.upper()].value def navigate_to(x,y, table_name): - if table_name == "popcorn table": - NavigateAction(target_locations=[Pose([x, y, 0], [0, 0, 1, 0])]).resolve().perform() - elif table_name == "long table": - NavigateAction(target_locations=[Pose([x, y, 0], [0, 0, 0, 1])]).resolve().perform() + if table_name == "popcorn table": + NavigateAction(target_locations=[Pose([x, y, 0], [0, 0, 1, 0])]).resolve().perform() + elif table_name == "long table": + NavigateAction(target_locations=[Pose([x, y, 0], [0, 0, 0, 1])]).resolve().perform() # def navigate_to(turn_around, y, table_name): @@ -160,7 +160,8 @@ def navigate_to(x,y, table_name): # :param orientation: defines the orientation of the robot respectively the name of the table to move to # """ # table = get_table_pose(table_name) -# print(f"table_pose: {table}") +# +# print(f"table_pose: {table.posestamped.pose.position.x}") # if turn_around: # NavigateAction(target_locations=[Pose([2, y, table.pose.position.z], table.pose.orentation)]).resolve().perform() # else: @@ -175,12 +176,10 @@ def navigate_and_detect(): """ TalkingMotion("Navigating").resolve().perform() #navigate_to(False, 1.8, "popcorn table") - navigate_to(1.6, 1.8, "popcorn table") + navigate_to(1.7, 1.8, "popcorn table") # 1.6 # popcorntable - # todo gucken ob ein aufruf genügt - #LookAtAction(targets=[Pose([0.8, 1.8, 0.21], object_orientation)]).resolve().perform() - LookAtAction(targets=[Pose([0.8, 1.8, 0.21], object_orientation)]).resolve().perform() + LookAtAction(targets=[Pose([0.8, 1.8, 0.21], object_orientation)]).resolve().perform() # 0.18 vanessa TalkingMotion("Perceiving").resolve().perform() try: object_desig = DetectAction(technique='all').resolve().perform() diff --git a/demos/pycram_receptionist_demo/M2_Demo.py b/demos/pycram_receptionist_demo/M2_Demo.py index 313abd823..1e32224ab 100644 --- a/demos/pycram_receptionist_demo/M2_Demo.py +++ b/demos/pycram_receptionist_demo/M2_Demo.py @@ -70,6 +70,7 @@ def demo_ms2(area): # guest_data format is = ['person infos: "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!"']: talk_error("no name") rospy.sleep(13) @@ -78,6 +79,7 @@ def demo_ms2(area): # set heard name and drink of guest guest1.set_name(guest_data[0][13:]) guest1.set_drink(guest_data[1]) + rospy.loginfo("after while") talk_request(guest_data) rospy.sleep(1) @@ -113,4 +115,4 @@ def demo_ms2(area): # demo_test('from_couch') -demo_ms2('to_couch') +demo_ms2('now') diff --git a/resources/couch-kitchenmarch1.urdf b/resources/couch-kitchenmarch1.urdf new file mode 100644 index 000000000..e9221b5cb --- /dev/null +++ b/resources/couch-kitchenmarch1.urdf @@ -0,0 +1,519 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/pycram/designators/action_designator.py b/src/pycram/designators/action_designator.py index d8bd555e1..05660a9c8 100644 --- a/src/pycram/designators/action_designator.py +++ b/src/pycram/designators/action_designator.py @@ -12,9 +12,10 @@ from .location_designator import CostmapLocation from .motion_designator import * from .object_designator import ObjectDesignatorDescription, BelieveObject, ObjectPart +from .. import helper from ..bullet_world import BulletWorld from ..designator import ActionDesignatorDescription -from ..enums import Arms +from ..enums import Arms, ObjectType from ..helper import multiply_quaternions, axis_angle_to_quaternion from ..local_transformer import LocalTransformer from ..orm.action_designator import (ParkArmsAction as ORMParkArmsAction, NavigateAction as ORMNavigateAction, @@ -240,7 +241,7 @@ def perform(self) -> None: kwargs["left_arm_config"] = "park" MoveArmJointsMotion(**kwargs).resolve().perform() # MoveTorsoAction([0.005]).resolve().perform() - MoveTorsoAction([0.2]).resolve().perform() + MoveTorsoAction([0.30]).resolve().perform() # add park right arm if wanted if self.arm in [Arms.RIGHT, Arms.BOTH]: kwargs["right_arm_config"] = "park" @@ -304,6 +305,115 @@ class Action(ActionDesignatorDescription.Action): @with_tree def perform(self) -> None: + # self.object_designator.type = "Bowl" + # # Initialize the local transformer and robot reference + # rospy.loginfo("pickup") + # lt = LocalTransformer() + # robot = BulletWorld.robot + # # Retrieve object and robot from designators + # object = self.object_designator.bullet_world_object + # # Calculate the object's pose in the map frame + # oTm = object.get_pose().copy() + # execute = True + # + # # Adjust object pose for top-grasping, if applicable + # if self.grasp == "top": + # # Handle special cases for certain object types (e.g., Cutlery, Bowl) + # # 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 + # # is hardcoded + # oTm.pose.position.z = 0.71 + # oTm.pose.position.z += 0.025 + # + # # Determine the grasp orientation and transform the pose to the base link frame + # grasp_rotation = robot_description.grasps.get_orientation_for_grasp(self.grasp) + # oTb = lt.transform_pose(oTm, robot.get_link_tf_frame("base_link")) + # # otbtest = oTb.copy() + # testmulti = helper.multiply_quaternions( + # [oTb.orientation.x, oTb.orientation.y, oTb.orientation.z, oTb.orientation.w], grasp_rotation) + # # otbtest.orientation = testmulti + # # Set pose to the grasp rotation + # oTb.orientation = grasp_rotation + # # Transform the pose to the map frame + # oTmG = lt.transform_pose(oTb, "map") + # BulletWorld.current_bullet_world.add_vis_axis(oTmG) + # # BulletWorld.current_bullet_world.add_vis_axis(otbtest) + # # + # # + # # Open the gripper before picking up the object + # # rospy.logwarn("Opening Gripper") + # MoveGripperMotion(motion="open", gripper=self.arm).resolve().perform() + # + # # Move to the pre-grasp position and visualize the action + # # rospy.logwarn("Picking up now") + # # BulletWorld.current_bullet_world.add_vis_axis(oTmG) + # # Execute Bool, because sometimes u only want to visualize the poses to test things + # if execute: + # MoveTorsoAction([0.25]).resolve().perform() + # rospy.loginfo('execute') + # MoveTCPMotion(oTmG, self.arm).resolve().perform() + # + # # 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) + # special_knowledge_offset = lt.transform_pose(oTmG, robot.get_link_tf_frame(tool_frame)) + # + # # todo: this is for hsrb only at the moment we will need a function that returns us special knowledge + # # depending on robot + # if robot.name == "hsrb": + # if self.grasp == "top": + # if self.object_designator.type == "Bowl": + # special_knowledge_offset.pose.position.y += 0.06 + # special_knowledge_offset.pose.position.x -= 0.03 # 0.022 + # + # + # push_base = special_knowledge_offset + # # todo: this is for hsrb only at the moment we will need a function that returns us special knowledge + # # depending on robot if we dont generlize this we will have a big list in the end of all robots + # if robot.name == "hsrb": + # z = 0.025 + # if self.grasp == "top": + # z = 0.025 + # if self.object_designator.type == "Bowl": + # z = 0.04 # 0.05 + # push_base.pose.position.z += z + # elif robot.name == "pr2": + # x = 0.01 + # if self.grasp == "top": + # x = 0.01 + # if self.object_designator.type == ObjectType.BOWL: + # x = 0.01 # 0.05 + # push_base.pose.position.x += x + # push_baseTm = lt.transform_pose(push_base, "map") + # special_knowledge_offsetTm = lt.transform_pose(push_base, "map") + # + # # Grasping from the top inherently requires calculating an offset, whereas front grasping involves + # # slightly pushing the object forward. + # # if self.grasp == "top": + # # rospy.logwarn("Offset now") + # BulletWorld.current_bullet_world.add_vis_axis(special_knowledge_offsetTm) + # if execute: + # MoveTCPMotion(special_knowledge_offsetTm, self.arm).resolve().perform() + # + # # rospy.logwarn("Pushing now") + # BulletWorld.current_bullet_world.add_vis_axis(push_baseTm) + # if execute: + # MoveTCPMotion(push_baseTm, self.arm).resolve().perform() + # tool_frame = robot_description.get_tool_frame(self.arm) + # robot.attach(object=self.object_designator.bullet_world_object, link=tool_frame) + # # Finalize the pick-up by closing the gripper and lifting the object + # # rospy.logwarn("Close Gripper") + # MoveGripperMotion(motion="close", gripper=self.arm).resolve().perform() + # + # # rospy.logwarn("Lifting now") + # liftingTm = push_baseTm + # liftingTm.pose.position.z += 0.04 + # BulletWorld.current_bullet_world.add_vis_axis(liftingTm) + # if execute: + # MoveTCPMotion(liftingTm, self.arm).resolve().perform() + # BulletWorld.current_bullet_world.remove_vis_axis() + # Initialize the local transformer and robot reference lt = LocalTransformer() @@ -321,8 +431,8 @@ def perform(self) -> None: 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 # is hardcoded - oTm.pose.position.z = 0.718 - oTm.pose.position.z += 0.04 + oTm.pose.position.z = 0.71 + oTm.pose.position.z += 0.035 # 0.025 # Determine the grasp orientation and transform the pose to the base link frame grasp_rotation = robot_description.grasps.get_orientation_for_grasp(self.grasp) @@ -355,7 +465,7 @@ def perform(self) -> None: if self.object_designator.type == "Bowl": print(f"x_pose: {special_knowledge_offset.pose.position.x}") print(f"y_pose: {special_knowledge_offset.pose.position.y}") - special_knowledge_offset.pose.position.y += 0.06 + special_knowledge_offset.pose.position.y += 0.065 special_knowledge_offset.pose.position.x -= 0.03 # 0.022 print(f"x_pose_after: {special_knowledge_offset.pose.position.x}") print(f"y_pose_after: {special_knowledge_offset.pose.position.y}") @@ -373,9 +483,9 @@ def perform(self) -> None: if robot.name == "hsrb": z = 0.04 if self.grasp == "top": - z = 0.039 + z = 0.025 # 0.039 if self.object_designator.type == "Bowl": - z = 0.045 # 0.05 + z = 0.044 # 0.05 push_base.pose.position.z += z push_baseTm = lt.transform_pose(push_base, "map") special_knowledge_offsetTm = lt.transform_pose(push_base, "map") @@ -424,6 +534,26 @@ def insert(self, session: sqlalchemy.orm.session.Session, **kwargs): return action + + + + def to_sql(self) -> ORMPickUpAction: + return ORMPickUpAction(self.arm, self.grasp) + + def insert(self, session: sqlalchemy.orm.session.Session, **kwargs): + action = super().insert(session) + # try to create the object designator + if self.object_at_execution: + od = self.object_at_execution.insert(session, ) + action.object = od.id + else: + action.object = None + + session.add(action) + session.commit() + + return action + def __init__(self, object_designator_description: Union[ObjectDesignatorDescription, ObjectDesignatorDescription.Object], arms: List[str], grasps: List[str], resolver=None): diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index b25e39333..0e77408b1 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -187,7 +187,7 @@ class HumanDescription: """ Class that represents humans. this class does not spawn a human in a simulation. """ - def __init__(self, name: str, fav_drink: Optional = None): + def __init__(self, name: String, fav_drink: Optional = None): """ :param name: name of human :param fav_drink: favorite drink of human diff --git a/src/pycram/external_interfaces/knowrob.py b/src/pycram/external_interfaces/knowrob.py index cbb2f6a0b..1dca6b816 100644 --- a/src/pycram/external_interfaces/knowrob.py +++ b/src/pycram/external_interfaces/knowrob.py @@ -136,11 +136,11 @@ def get_guest_info(id): :return: ['person_infos: "name', 'drink"'] """ - rospy.wait_for_service('name_server') + rospy.wait_for_service('info_server') try: - info_service = rospy.ServiceProxy('name_server', IsKnown) - guest_data = info_service(id) #guest_data = "name, drink" - result = str(guest_data).split(',') #result = ["name", " drink"] + info_service = rospy.ServiceProxy('info_server', IsKnown) + guest_data = info_service(id) #guest_data = person_infos: "Angel,Milk" + result = str(guest_data).split(',') #result = ['person_infos: "Angel', 'Milk"'] return result except rospy.ServiceException as e: print("Service call failed") @@ -153,7 +153,7 @@ def get_table_pose(table_name): print("waiting for server") rospy.wait_for_service('pose_server') try: - service = rospy.ServiceProxy('pose_server', ObjectPose) + service = rospy.ServiceProxy('pose_server', String) table_pose = service(table_name)# print(f"table_pose_knowrob: {table_pose}") return table_pose diff --git a/src/pycram/robot_descriptions/hsrb_description.py b/src/pycram/robot_descriptions/hsrb_description.py index 04d30c5f5..90d459b0f 100644 --- a/src/pycram/robot_descriptions/hsrb_description.py +++ b/src/pycram/robot_descriptions/hsrb_description.py @@ -30,7 +30,7 @@ def __init__(self): # Arm arm_joints = ["arm_flex_joint", "arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] arm_links = ["arm_flex_link", "arm_roll_link", "wrist_flex_link", "wrist_roll_link"] - arm_carry = {"park": [0, 1.5, -1.85, 0]} + arm_carry = {"park": [0, 1.3, -1.85, 0]} arm_placing_given_obj = {"place_human_given_obj": [-1.8, 0, -0.5, -1.5]} gripper_links = ["hand_l_distal_link", "hand_l_spring_proximal_link", "hand_palm_link", "hand_r_distal_link", "hand_r_spring_proximal_link", "hand_gripper_tool_frame"]