def grab(p, robot): entities = parse_object(p, robot) if not entities: say("No such entities!") return # Only filter to entities that do not have a shape but do have a convex hull entities = [ e for e in entities if not e.has_shape and len(e.convex_hull) > 0 ] if not entities: say("I am sorry, but I cannot grab that object") return arm = robot.leftArm machine = Grab(robot, arm=ds.UnoccupiedArmDesignator(robot.arms, arm), item=ds.EntityByIdDesignator(robot, id=entities[0].id)) stop() global ACTION ACTION = SmachAction(machine) ACTION.start()
def __init__(self, robot, grab_designator=None): """ Constructor :param robot: robot object :param grab_designator: EdEntityDesignator designating the item to grab. If not provided, a default one is constructed (grabs the closest object in the volume of the surface) """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) # Create designators self.empty_arm_designator = ds.UnoccupiedArmDesignator( robot, { "required_trajectories": ["prepare_grasp"], "required_goals": ["carrying_pose"], "required_gripper_types": [arms.GripperTypes.GRASPING] }, name="empty_arm_designator") self.grab_designator = ds.LockToId(robot=robot, to_be_locked=grab_designator) with self: @smach.cb_interface(outcomes=["locked"]) def lock(userdata=None): """ 'Locks' a locking designator """ # This determines that self.current_item cannot not resolve to a new value until it is unlocked again. self.grab_designator.lock() if self.grab_designator.resolve(): rospy.loginfo("Current_item is now locked to {0}".format( self.grab_designator.resolve().id)) return "locked" smach.StateMachine.add("LOCK_ITEM", smach.CBState(lock), transitions={'locked': 'GRAB_ITEM'}) smach.StateMachine.add("GRAB_ITEM", states.Grab(robot, self.grab_designator, self.empty_arm_designator), transitions={ 'done': 'UNLOCK_ITEM_SUCCEED', 'failed': 'UNLOCK_ITEM_FAIL' }) @smach.cb_interface(outcomes=["unlocked"]) def unlock(userdata=None): """ 'Unlocks' a locking designator """ self.grab_designator.unlock() return "unlocked" smach.StateMachine.add("UNLOCK_ITEM_SUCCEED", smach.CBState(unlock), transitions={'unlocked': 'succeeded'}) smach.StateMachine.add("UNLOCK_ITEM_FAIL", smach.CBState(unlock), transitions={'unlocked': 'failed'})
def setUp(self): entity = Entity("123", "dummy", "/map", None, None, {}, None, 0) self.robot.arms["leftArm"].occupied_by = entity self.arm_ds = ds.UnoccupiedArmDesignator( self.robot, { "required_goals": ['handover_to_human'], "required_gripper_types": [arms.GripperTypes.GRASPING] }) self.entity = Entity("456", "dummy", "/map", None, None, {}, None, 0)
def __init__(self, robot, grab_designator=None): """ Constructor :param robot: robot object :param grab_designator: EdEntityDesignator designating the item to grab. If not provided, a default one is constructed (grabs the closest object in the volume of the surface) """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) # Create designators self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms, robot.leftArm, name="empty_arm_designator") self.grab_designator = ds.LockToId(robot=robot, to_be_locked=grab_designator) with self: @smach.cb_interface(outcomes=["locked"]) def lock(userdata=None): """ 'Locks' a locking designator """ # This determines that self.current_item cannot not resolve to a new value until it is unlocked again. self.grab_designator.lock() if self.grab_designator.resolve(): rospy.loginfo("Current_item is now locked to {0}".format(self.grab_designator.resolve().id)) return "locked" smach.StateMachine.add("LOCK_ITEM", smach.CBState(lock), transitions={'locked': 'ANNOUNCE_ITEM'}) smach.StateMachine.add("ANNOUNCE_ITEM", states.Say(robot, EntityDescriptionDesignator(self.grab_designator, name="current_item_desc"), block=False), transitions={'spoken': 'GRAB_ITEM'}) smach.StateMachine.add("GRAB_ITEM", states.Grab(robot, self.grab_designator, self.empty_arm_designator), transitions={'done': 'UNLOCK_ITEM_SUCCEED', 'failed': 'UNLOCK_ITEM_FAIL'}) @smach.cb_interface(outcomes=["unlocked"]) def lock(userdata=None): """ 'Locks' a locking designator """ # This determines that self.current_item cannot not resolve to a new value until it is unlocked again. self.grab_designator.unlock() return "unlocked" smach.StateMachine.add("UNLOCK_ITEM_SUCCEED", smach.CBState(lock), transitions={'unlocked': 'succeeded'}) smach.StateMachine.add("UNLOCK_ITEM_FAIL", smach.CBState(lock), transitions={'unlocked': 'failed'})
def setup_statemachine(robot): state_machine = StateMachine(outcomes=['done']) furniture_designator = ds.VariableDesignator(resolve_type=Entity) entity_designator = ds.VariableDesignator(resolve_type=Entity) arm_designator = ds.UnoccupiedArmDesignator(robot, {}) with state_machine: # Intro StateMachine.add('START_CHALLENGE_ROBUST', StartChallengeRobust(robot, STARTING_POINT), transitions={'Done': 'SAY_START', 'Aborted': 'done', 'Failed': 'SAY_START'}) # Say we're gonna start StateMachine.add('SAY_START', Say(robot, "Hand me that it is!", block=False), transitions={'spoken': 'NAVIGATE_TO_START'}) # Drive to the start location StateMachine.add('NAVIGATE_TO_START', NavigateToWaypoint(robot, ds.EdEntityDesignator(robot, id=HOME_LOCATION)), transitions={'arrived': 'GET_FURNITURE_FROM_OPERATOR_POSE', 'unreachable': 'NAVIGATE_TO_START', # ToDo: other fallback 'goal_not_defined': 'done'}) # I'm not even going to fill this in # The pre-work StateMachine.add('GET_FURNITURE_FROM_OPERATOR_POSE', GetFurnitureFromOperatorPose(robot, furniture_designator.writeable), transitions={'done': 'INSPECT_FURNITURE'}) # Go to the furniture object that was pointing to see what's there StateMachine.add('INSPECT_FURNITURE', InspectFurniture(robot, furniture_designator, entity_designator.writeable), transitions={"succeeded": "IDENTIFY_OBJECT", "failed": "NAVIGATE_TO_START"}) # If no entities, try again # Point at the object StateMachine.add('IDENTIFY_OBJECT', IdentifyObject(robot, entity_designator, arm_designator), transitions={'done': 'NAVIGATE_TO_START', # Just keep on going 'failed': 'NAVIGATE_TO_START'}) # Just keep on going return state_machine
def __init__(self, dummy_robot): smach.StateMachine.__init__(self, outcomes=[ "succeeded", "failed", ]) # Create designators dummy_trashbin_designator = ds.EdEntityDesignator( dummy_robot, id=CHALLENGE_KNOWLEDGE.trashbin_id2, name='trashbin_designator') dummy_arm_designator_un = ds.UnoccupiedArmDesignator(dummy_robot, {}) dummy_arm_designator_oc = ds.OccupiedArmDesignator(dummy_robot, {}) with self: smach.StateMachine.add("PICK_UP_TRASH", PickUpTrash(dummy_robot, dummy_trashbin_designator, dummy_arm_designator_un), transitions={ "succeeded": "TURN_BASE", "failed": "TURN_BASE", "aborted": "failed" }) smach.StateMachine.add("TURN_BASE", states.ForceDrive(dummy_robot, 0, 0, 0.5, 3), transitions={"done": "DROP_TRASH"}) smach.StateMachine.add("DROP_TRASH", DropTrash(dummy_robot, dummy_arm_designator_oc), transitions={ "succeeded": "succeeded", "failed": "failed" })
def __init__(self, robot, selected_entity_designator, room_des): smach.StateMachine.__init__(self, outcomes=['done', 'failed']) store_entity_id_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id") store_entity_des = ds.EdEntityDesignator( robot, id_designator=store_entity_id_des) selected_entity_type_des = ds.AttrDesignator( selected_entity_designator, "type", resolve_type=str) store_area_name_des = ds.VariableDesignator(resolve_type=str, name="store_entity_id") trash_place_pose = DropPoseDesignator(robot, store_entity_des, 0.6, "drop_pose") category_des = ds.VariableDesignator(resolve_type=str, name="category_des") with self: smach.StateMachine.add( "SPEAK", Say(robot, ["I will pick-up the {object}", "Let's move the {object}"], object=selected_entity_type_des, block=True), transitions={"spoken": "GRAB"}) smach.StateMachine.add("GRAB", Grab( robot, selected_entity_designator, ds.UnoccupiedArmDesignator( robot, arm_properties={ "required_trajectories": ["prepare_grasp"], "required_goals": ["carrying_pose"], "required_gripper_types": [arms.GripperTypes.GRASPING] }, name="empty_arm_designator")), transitions={ "done": "SAY_GRAB_SUCCESS", "failed": "ARM_RESET" }) smach.StateMachine.add( "ARM_RESET", ArmToJointConfig( robot, ds.UnoccupiedArmDesignator( robot, arm_properties={"required_goals": ["reset"]}, name="empty_arm_designator"), "reset"), transitions={ "succeeded": "SAY_GRAB_FAILED", "failed": "SAY_GRAB_FAILED" }) smach.StateMachine.add( 'SAY_GRAB_SUCCESS', Say(robot, [ "Now I am going to move this item", "Let's clean up this object", "Away with this one", "Everything will be cleaned" ], block=False), transitions={"spoken": "GET_CATEGORY"}) smach.StateMachine.add( 'SAY_GRAB_FAILED', Say(robot, [ "I could not grab the item.", "I failed to grasp the item", "I cannot reach the item", "Item grab failed" ], block=False), transitions={"spoken": "failed"}) smach.StateMachine.add('CHECK_ARM_FREE', ArmFree(robot), transitions={ "yes": "done", "no": "CHECK_ARM_OCCUPIED" }) smach.StateMachine.add('CHECK_ARM_OCCUPIED', ArmOccupied(robot), transitions={ "yes": "GET_CATEGORY", "no": "done" }) # # ROBOT # smach.StateMachine.add('GET_CATEGORY', # EntityToCategory(robot, selected_entity_designator, category_des.writeable), # transitions={"done": "DETERMINE_PLACE_LOCATION", # "failed": "NAVIGATE_TO_TRASH"}) # OPERATOR smach.StateMachine.add('GET_CATEGORY', OperatorToCategory(robot, category_des.writeable, room_des), transitions={ "done": "DETERMINE_PLACE_LOCATION", "failed": "NAVIGATE_TO_TRASH" }) smach.StateMachine.add( 'DETERMINE_PLACE_LOCATION', CategoryToLocation(category_des, store_entity_id_des.writeable, store_area_name_des.writeable), transitions={ "trashbin": "INSPECT_TRASH", "other": "PLACE_TO_STORE", "failed": "NAVIGATE_TO_TRASH" }) smach.StateMachine.add('NAVIGATE_TO_TRASH', NavigateToPlace( robot, trash_place_pose, ds.OccupiedArmDesignator( robot, {}, name="occupied_arm_designator")), transitions={ "arrived": "PLACE_IN_TRASH", "unreachable": "SAY_PLACE_FAILED", "goal_not_defined": "SAY_PLACE_FAILED" }) smach.StateMachine.add('INSPECT_TRASH', Inspect(robot, store_entity_des), transitions={ "done": "PLACE_IN_TRASH", "failed": "SAY_PLACE_FAILED" }) arm_properties_place = { "required_trajectories": ["prepare_place"], "required_gripper_types": [arms.GripperTypes.GRASPING] } arm_designator_place = ds.OccupiedArmDesignator( robot, arm_properties_place, name="occupied_arm_designator") smach.StateMachine.add('PLACE_IN_TRASH', Place(robot, selected_entity_designator, trash_place_pose, arm_designator_place), transitions={ "done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED" }) arm_designator_place_store = ds.OccupiedArmDesignator( robot, arm_properties_place, name="occupied_arm_designator") smach.StateMachine.add('PLACE_TO_STORE', Place(robot, selected_entity_designator, store_entity_des, arm_designator_place_store, "on_top_of"), transitions={ "done": "SAY_PLACE_SUCCESS", "failed": "SAY_PLACE_FAILED" }) smach.StateMachine.add( 'SAY_PLACE_SUCCESS', Say(robot, [ "Bye bye!", "Yeah!", "Successfully disposed the item", "Another score for {}".format(robot.robot_name) ], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"}) smach.StateMachine.add( 'SAY_PLACE_FAILED', Say(robot, [ "I could not cleanup the item.", "I cannot put the item in the trashbin", "Item cleanup failed" ], block=False), transitions={"spoken": "CHECK_ARM_OCCUPIED"})
def execute(self, userdata=None): d = ds.UnoccupiedArmDesignator(self._robot, {}, name="empty_arm_designator2") if d.resolve(): return "yes" return "no"
def __init__(self, robot, manipulated_items): """@param manipulated_items is VariableDesignator that will be a list of items manipulated by the robot.""" self.manipulated_items = manipulated_items smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) self.cabinet = ds.EntityByIdDesignator(robot, id=CABINET, name="pick_shelf") # self.place_shelf = ds.EntityByIdDesignator(robot, id=PLACE_SHELF, name="place_shelf") not_manipulated = lambda entity: not entity in self.manipulated_items.resolve( ) def entity_z_pos(entity): """ Checks if the entity is between the minimum and maximum grasp height :param entity: :return: """ if not entity._pose: return False return MIN_GRASP_HEIGHT < entity._pose.p.z() < MAX_GRASP_HEIGHT # select the entity closest in x direction to the robot in base_link frame def weight_function(entity): # TODO: return x coordinate of entity.center_point in base_link frame epose = entity.pose.projectToFrame( robot.robot_name + "/base_link", robot.tf_listener) # Get position in base_link p = epose.frame.p return p.x()**2 self.current_item = ds.LockingDesignator(ds.EdEntityDesignator( robot, criteriafuncs=[ not_ignored, size, not_manipulated, min_entity_height, entity_z_pos, max_width ], weight_function=weight_function, debug=False, name="item"), name="current_item") #This makes that the empty spot is resolved only once, even when the robot moves. This is important because the sort is based on distance between robot and constraint-area # self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot, self.cabinet, name="placement", area=PLACE_SHELF), name="place_position") self.place_position = ds.LockingDesignator(EmptyShelfDesignator( robot, self.cabinet, name="placement", area=PLACE_SHELF), name="place_position") self.empty_arm_designator = ds.UnoccupiedArmDesignator( robot, { 'required_trajectories': ['prepare_grasp'], 'required_goals': ['carrying_pose'], 'required_gripper_types': [arms.GripperTypes.GRASPING], 'required_arm_name': PREFERRED_ARM }, name="empty_arm_designator") self.arm_with_item_designator = ds.ArmHoldingEntityDesignator( robot, {'required_objects': [self.current_item]}, { "required_trajectories": ["prepare_place"], "required_goals": ["reset", "handover_to_human"], 'required_gripper_types': [arms.GripperTypes.GRASPING] }, name="arm_with_item_designator") # print "{0} = pick_shelf".format(self.pick_shelf) # print "{0} = current_item".format(self.current_item) # print "{0} = place_position".format(self.place_position) # print "{0} = empty_arm_designator".format(self.empty_arm_designator) # print "{0} = arm_with_item_designator".format(self.arm_with_item_designator) with self: # smach.StateMachine.add( "NAV_TO_OBSERVE_PICK_SHELF", # #states.NavigateToObserve(robot, self.pick_shelf), # states.NavigateToSymbolic(robot, {self.pick_shelf:"in_front_of", EntityByIdDesignator(robot, id=ROOM):"in"}, self.pick_shelf), # transitions={ 'arrived' :'LOOKAT_PICK_SHELF', # 'unreachable' :'LOOKAT_PICK_SHELF', # 'goal_not_defined' :'LOOKAT_PICK_SHELF'}) smach.StateMachine.add("REMOVE_ENTITIES", RemoveSegmentedEntities(robot=robot), transitions={'done': 'LOOKAT_PICK_SHELF'}) smach.StateMachine.add("LOOKAT_PICK_SHELF", states.LookAtArea(robot, self.cabinet, area=PICK_SHELF), transitions={'succeeded': 'SEGMENT_SHELF'}) smach.StateMachine.add("SEGMENT_SHELF", SegmentShelf(robot, entity_id=CABINET, area_id=PICK_SHELF), transitions={'done': 'LOCK_ITEM'}) @smach.cb_interface(outcomes=['locked']) def lock(userdata=None): self.current_item.lock( ) #This determines that self.current_item cannot not resolve to a new value until it is unlocked again. if self.current_item.resolve(): rospy.loginfo("Current_item is now locked to {0}".format( self.current_item.resolve().id)) self.place_position.lock( ) #This determines that self.place_position will lock/cache its result after its resolved the first time. return 'locked' smach.StateMachine.add('LOCK_ITEM', smach.CBState(lock), transitions={'locked': 'ANNOUNCE_ITEM'}) smach.StateMachine.add("ANNOUNCE_ITEM", states.Say(robot, EntityDescriptionDesignator( self.current_item, name="current_item_desc"), block=False), transitions={'spoken': 'GRAB_ITEM'}) smach.StateMachine.add("GRAB_ITEM", Grab(robot, self.current_item, self.empty_arm_designator), transitions={ 'done': 'STORE_ITEM', 'failed': 'SAY_GRAB_FAILED' }) smach.StateMachine.add( "SAY_GRAB_FAILED", states.Say(robot, ["I couldn't grab this thing"], mood="sad"), transitions={'spoken': 'UNLOCK_ITEM_AFTER_FAILED_GRAB'} ) # Not sure whether to fail or keep looping with NAV_TO_OBSERVE_PICK_SHELF @smach.cb_interface(outcomes=['unlocked']) def unlock_and_ignore(userdata=None): global ignore_ids # import ipdb; ipdb.set_trace() if self.current_item.resolve(): ignore_ids += [self.current_item.resolve().id] rospy.loginfo("Current_item WAS now locked to {0}".format( self.current_item.resolve().id)) self.current_item.unlock( ) #This determines that self.current_item can now resolve to a new value on the next call self.place_position.unlock( ) #This determines that self.place_position can now resolve to a new position on the next call return 'unlocked' smach.StateMachine.add('UNLOCK_ITEM_AFTER_FAILED_GRAB', smach.CBState(unlock_and_ignore), transitions={'unlocked': 'failed'}) @smach.cb_interface(outcomes=['stored']) def store_as_manipulated(userdata=None): # manipulated_items.current += [self.current_item.current] item_list = manipulated_items.resolve() item_list += [self.current_item.resolve()] w = ds.VariableWriter(manipulated_items) w.write(item_list) return 'stored' smach.StateMachine.add( 'STORE_ITEM', smach.CBState(store_as_manipulated), transitions={'stored': 'LOOKAT_PLACE_SHELF'}) smach.StateMachine.add("LOOKAT_PLACE_SHELF", states.LookAtArea(robot, self.cabinet, area=PLACE_SHELF), transitions={'succeeded': 'PLACE_ITEM'}) smach.StateMachine.add("PLACE_ITEM", Place(robot, self.current_item, self.place_position, self.arm_with_item_designator), transitions={ 'done': 'RESET_HEAD_PLACE', 'failed': 'RESET_HEAD_HUMAN' }) smach.StateMachine.add( "RESET_HEAD_PLACE", states.CancelHead(robot), transitions={'done': 'UNLOCK_ITEM_AFTER_SUCCESSFUL_PLACE'}) smach.StateMachine.add( "RESET_HEAD_HUMAN", states.CancelHead(robot), transitions={'done': 'SAY_HANDOVER_TO_HUMAN'}) smach.StateMachine.add('UNLOCK_ITEM_AFTER_SUCCESSFUL_PLACE', smach.CBState(unlock_and_ignore), transitions={'unlocked': 'succeeded'}) smach.StateMachine.add( "SAY_HANDOVER_TO_HUMAN", states.Say(robot, [ "I'm can't get rid of this item myself, can somebody help me maybe?" ]), transitions={'spoken': 'HANDOVER_TO_HUMAN'}) smach.StateMachine.add('HANDOVER_TO_HUMAN', states.HandoverToHuman( robot, self.arm_with_item_designator), transitions={ 'succeeded': 'UNLOCK_AFTER_HANDOVER', 'failed': 'UNLOCK_AFTER_HANDOVER' }) smach.StateMachine.add('UNLOCK_AFTER_HANDOVER', smach.CBState(unlock_and_ignore), transitions={'unlocked': 'failed'})
return 'succeeded' if __name__ == "__main__": from robot_skills import get_robot if len(sys.argv) > 1: robot_name = sys.argv[1] point_at = sys.argv[2] look_at = sys.argv[3] rospy.init_node('test_follow_operator') robot = get_robot(robot_name) sm = PointAt( robot, arm_designator=ds.UnoccupiedArmDesignator( robot, {'required_goals': ['point_at']}), point_at_designator=ds.EdEntityDesignator(robot, id=point_at, name='point_at_des'), look_at_designator=ds.EdEntityDesignator(robot, id=look_at, name='look_at_des')) sm.execute() else: print( "Please provide robot name, point_at ID and look_at ID as argument." ) exit(1)
smach.StateMachine.add("LOWER_ARM", states.ArmToJointConfig(robot=robot, arm_designator=arm_occupied_designator, configuration="reset"), transitions={"succeeded": "RECEIVED_TRASH_BAG", "failed": "RECEIVED_TRASH_BAG"}) smach.StateMachine.add("RECEIVED_TRASH_BAG", states.Say(robot, "I received the thrash bag. I will throw" " it away, please move away.", block=True), transitions={'spoken': 'succeeded'}) smach.StateMachine.add("TIMEOUT", states.Say(robot, "I have not received anything, so I will just continue", block=False), transitions={'spoken': "failed"}) if __name__ == '__main__': import os import robot_smach_states.util.designators as ds from robot_skills import Hero rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0]) hero = Hero() hero.reset() arm = ds.UnoccupiedArmDesignator(hero, {}) # sm = HandoverFromHumanFigure(hero, arm, grabbed_entity_label='trash') # sm.execute() GrabTrash(hero, arm, 100, 2).execute()
import robot_smach_states.util.designators as ds from robot_smach_states import Grab if __name__ == "__main__": parser = argparse.ArgumentParser(description="Put an imaginary object in the world model and grasp it using the " "'Grab' smach state") parser.add_argument("x", type=float, help="x-coordinate (in map) of the imaginary object") parser.add_argument("y", type=float, help="y-coordinate (in map) of the imaginary object") parser.add_argument("z", type=float, help="z-coordinate (in map) of the imaginary object") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() rospy.init_node("test_grasping") robot = get_robot(args.robot) entity_id = "test_item" pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)), frame_id="/map") robot.ed.update_entity(id=entity_id, frame_stamped=pose) item = ds.EdEntityDesignator(robot, id=entity_id) arm = ds.UnoccupiedArmDesignator(robot, {}) grab_state = Grab(robot, item, arm) grab_state.execute()
arm_designator=arm_occupied_designator, configuration="reset"), transitions={"succeeded": "RECEIVED_TRASH_BAG", "failed": "RECEIVED_TRASH_BAG"}) smach.StateMachine.add("RECEIVED_TRASH_BAG", states.Say(robot, "I received the thrash bag. I will throw" " it away, please move away.", block=True), transitions={'spoken': 'succeeded'}) smach.StateMachine.add("TIMEOUT", states.Say(robot, "I have not received anything, so I will just continue", block=False), transitions={'spoken': "failed"}) if __name__ == '__main__': import os import robot_smach_states.util.designators as ds from robot_skills import Hero, arms rospy.init_node(os.path.splitext("test_" + os.path.basename(__file__))[0]) hero = Hero() hero.reset() arm = ds.UnoccupiedArmDesignator(hero, {"required_goals": ["reset", "handover"], "force_sensor_required": True, "required_gripper_types": [arms.GripperTypes.GRASPING]}) # sm = HandoverFromHumanFigure(hero, arm, grabbed_entity_label='trash') # sm.execute() GrabTrash(hero, arm, 100, 2).execute()
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) msg = "\n".join([ "==============================================", "== CHALLENGE HELP ME CARRY ==", "==============================================" ]) rospy.loginfo("\n" + msg) self.target_destination = ds.EntityByIdDesignator( robot, id=challenge_knowledge.default_place) self.car_waypoint = ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_car['id']) self.empty_arm_designator = ds.UnoccupiedArmDesignator( robot, arm_properties={ "required_goals": [ "handover_to_human", "reset", challenge_knowledge.driving_bag_pose, challenge_knowledge.drop_bag_pose ], "required_gripper_types": [GripperTypes.GRASPING] }, name="empty_arm_designator") # With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked. # For this challenge, unlocking is not needed. self.bag_arm_designator = self.empty_arm_designator.lockable() self.bag_arm_designator.lock() self.place_position = ds.LockingDesignator(EmptySpotDesignator( robot, self.target_destination, arm_designator=self.bag_arm_designator, name="placement", area=challenge_knowledge.default_area), name="place_position") # We don't actually grab something, so there is no need for an actual thing to grab self.current_item = ds.VariableDesignator(Entity( "dummy", "dummy", "/{}/base_link".format(robot.robot_name), kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [], datetime.datetime.now()), name="current_item") with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SET_INITIAL_POSE', 'abort': 'Aborted' }) smach.StateMachine.add('SET_INITIAL_POSE', states.SetInitialPose( robot, challenge_knowledge.starting_point), transitions={ 'done': 'FOLLOW_OPERATOR', "preempted": 'Aborted', 'error': 'FOLLOW_OPERATOR' }) # Follow the operator until (s)he states that you have arrived at the "car". smach.StateMachine.add('FOLLOW_OPERATOR', states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=True, replan=True), transitions={ 'stopped': 'ASK_FOR_TASK', 'lost_operator': 'ASK_FOR_TASK', 'no_operator': 'ASK_FOR_TASK' }) smach.StateMachine.add('ASK_FOR_TASK', states.Say(robot, ["Are we at the car already?"], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_TASK'}) smach.StateMachine.add( 'WAIT_FOR_TASK', hmc_states.WaitForOperatorCommand( robot, possible_commands=challenge_knowledge.commands.keys(), commands_as_outcomes=True), transitions={ 'no': 'FOLLOW_OPERATOR', 'yes': 'REMEMBER_CAR_LOCATION', 'abort': 'Aborted' }) smach.StateMachine.add('REMEMBER_CAR_LOCATION', hmc_states.StoreCarWaypoint(robot), transitions={ 'success': 'ASK_FOR_DESTINATION', 'abort': 'Aborted' }) smach.StateMachine.add( 'ASK_FOR_DESTINATION', states.Say(robot, ["Where should I bring the groceries?"], block=True, look_at_standing_person=True), transitions={'spoken': 'RECEIVE_DESTINATION'}) smach.StateMachine.add( 'RECEIVE_DESTINATION', hmc_states.WaitForOperatorCommand( robot, possible_commands=challenge_knowledge.destinations, commands_as_userdata=True, target=self.target_destination), transitions={ 'success': 'GRAB_ITEM', 'abort': 'Aborted' }) # Grab the item (bag) the operator hands to the robot, when they are at the "car". smach.StateMachine.add( 'GRAB_ITEM', states.HandoverFromHuman( robot, self.bag_arm_designator, "current_item", self.current_item, arm_configuration=challenge_knowledge.carrying_bag_pose), transitions={ 'succeeded': 'ARM_DRIVING_POSE', 'timeout': 'BACKUP_CLOSE_GRIPPER', # For now in simulation timeout is considered a success. 'failed': 'BACKUP_CLOSE_GRIPPER' }) smach.StateMachine.add('BACKUP_CLOSE_GRIPPER', states.SetGripper( robot, self.bag_arm_designator, gripperstate=GripperState.CLOSE), transitions={ 'succeeded': 'ARM_DRIVING_POSE', 'failed': 'ARM_DRIVING_POSE' }) smach.StateMachine.add('ARM_DRIVING_POSE', states.ArmToJointConfig( robot, self.bag_arm_designator, challenge_knowledge.driving_bag_pose), transitions={ 'succeeded': 'SAY_GOING_TO_ROOM', 'failed': 'SAY_GOING_TO_ROOM' }) smach.StateMachine.add( 'SAY_GOING_TO_ROOM', states.Say(robot, [ "Let me bring in your groceries", "Helping you carry stuff", "I'm going back inside" ], block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_DESTINATION'}) smach.StateMachine.add( 'GOTO_DESTINATION', states.NavigateToWaypoint( robot, self.target_destination, challenge_knowledge.default_target_radius), transitions={ 'arrived': 'PUTDOWN_ITEM', 'unreachable': 'GOTO_DESTINATION_BACKUP', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted' }) smach.StateMachine.add( 'GOTO_DESTINATION_BACKUP', states.NavigateToWaypoint( robot, self.target_destination, challenge_knowledge.backup_target_radius), transitions={ 'arrived': 'PUTDOWN_ITEM', 'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted' }) # Put the item (bag) down when the robot has arrived at the "drop-off" location (house). smach.StateMachine.add('PUTDOWN_ITEM', hmc_states.DropBagOnGround( robot, self.bag_arm_designator, challenge_knowledge.drop_bag_pose), transitions={'done': 'ASKING_FOR_HELP'}) smach.StateMachine.add( 'ASKING_FOR_HELP', # TODO: look and then face new operator states.Say( robot, "Please follow me and help me carry groceries into the house", block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_CAR'}) smach.StateMachine.add( 'GOTO_CAR', states.NavigateToWaypoint( robot, self.car_waypoint, challenge_knowledge.waypoint_car['radius']), # TODO: detect closed door transitions={ 'unreachable': 'OPEN_DOOR', 'arrived': 'AT_END', 'goal_not_defined': 'Aborted' }) smach.StateMachine.add( 'OPEN_DOOR', # TODO: implement functionality states.Say(robot, "Please open the door for me"), transitions={'spoken': 'GOTO_CAR'}) smach.StateMachine.add( 'AT_END', states.Say(robot, [ "We arrived at the car, goodbye", "You have reached your destination, goodbye", "The car is right here, see you later!" ], block=True, look_at_standing_person=True), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "help_me_carry")
def __init__(self, robot, grasp_furniture_id, place_furniture_id1, place_furniture_id2): """ Constructor :param robot: robot object :param grasp_furniture_id: string identifying the furniture object where to grasp the objects :param place_furniture_id1: string identifying the furniture object where to place objects 1 and 2 :param place_furniture_id2: string identifying the furniture object where to place object 3 """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"]) # Create designators grasp_furniture_designator = ds.EntityByIdDesignator( robot, id=grasp_furniture_id) grasp_designator1 = DefaultGrabDesignator( robot=robot, surface_designator=grasp_furniture_designator, area_description="on_top_of") grasp_designator2 = DefaultGrabDesignator( robot=robot, surface_designator=grasp_furniture_designator, area_description="on_top_of") grasp_designator3 = DefaultGrabDesignator( robot=robot, surface_designator=grasp_furniture_designator, area_description="on_top_of") # TODO use the same designator that is used for setting the table arm_designator = ds.UnoccupiedArmDesignator(robot, {}) place_furniture_designator1 = ds.EntityByIdDesignator( robot, id=place_furniture_id1) place_designator1 = EmptySpotDesignator( robot=robot, place_location_designator=place_furniture_designator1, arm_designator=arm_designator, area="on_top_of") place_furniture_designator3 = ds.EntityByIdDesignator( robot, id=place_furniture_id2) place_designator3 = EmptySpotDesignator( robot=robot, place_location_designator=place_furniture_designator3, arm_designator=arm_designator, area="on_top_of") with self: # Move to the inspect location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE1", states.NavigateToSymbolic( robot, {grasp_furniture_designator: "in_front_of"}, grasp_furniture_designator), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE', 'unreachable': 'MOVE_TO_GRASP_SURFACE2', 'goal_not_defined': 'INSPECT_GRASP_SURFACE' }) # Backup for moving to inspect location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE2", states.NavigateToSymbolic( robot, {grasp_furniture_designator: "large_in_front_of"}, grasp_furniture_designator), transitions={ 'arrived': 'INSPECT_GRASP_SURFACE', 'unreachable': 'INSPECT_GRASP_SURFACE', 'goal_not_defined': 'INSPECT_GRASP_SURFACE' }) # Inspect grasp furniture smach.StateMachine.add("INSPECT_GRASP_SURFACE", states.Inspect( robot=robot, entityDes=grasp_furniture_designator, objectIDsDes=None, searchArea="on_top_of", navigation_area="in_front_of"), transitions={ "done": "GRAB_ITEM_1", "failed": "failed" }) # Grasp the first item smach.StateMachine.add("GRAB_ITEM_1", GrabSingleItem( robot=robot, grab_designator=grasp_designator1), transitions={ "succeeded": "GRAB_ITEM_2", "failed": "GRAB_ITEM_2" }) # Grasp the second item smach.StateMachine.add("GRAB_ITEM_2", GrabSingleItem( robot=robot, grab_designator=grasp_designator2), transitions={ "succeeded": "MOVE_TO_PLACE", "failed": "MOVE_TO_PLACE" }) # Move to the place location smach.StateMachine.add( "MOVE_TO_PLACE", states.NavigateToSymbolic( robot, {place_furniture_designator1: "in_front_of"}, place_furniture_designator1), transitions={ 'arrived': 'PLACE_ITEM_1', 'unreachable': 'PLACE_ITEM_1', 'goal_not_defined': 'PLACE_ITEM_1' }) # Place the first item smach.StateMachine.add("PLACE_ITEM_1", PlaceSingleItem( robot=robot, place_designator=place_designator1), transitions={ "succeeded": "PLACE_ITEM_2", "failed": "PLACE_ITEM_2" }) # Place the second item smach.StateMachine.add("PLACE_ITEM_2", PlaceSingleItem( robot=robot, place_designator=place_designator1), transitions={ "succeeded": "MOVE_TO_GRASP_SURFACE3", "failed": "MOVE_TO_GRASP_SURFACE3" }) # Move back to the grasp surface to grasp the third item smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE3", states.NavigateToSymbolic( robot, {grasp_furniture_designator: "in_front_of"}, grasp_furniture_designator), transitions={ 'arrived': 'GRAB_ITEM_3', 'unreachable': 'MOVE_TO_GRASP_SURFACE4', 'goal_not_defined': 'GRAB_ITEM_3' }) # Backup for moving back to the grasp location smach.StateMachine.add( "MOVE_TO_GRASP_SURFACE4", states.NavigateToSymbolic( robot, {grasp_furniture_designator: "large_in_front_of"}, grasp_furniture_designator), transitions={ 'arrived': 'GRAB_ITEM_3', 'unreachable': 'GRAB_ITEM_3', 'goal_not_defined': 'GRAB_ITEM_3' }) # Grasp the third item smach.StateMachine.add("GRAB_ITEM_3", GrabSingleItem( robot=robot, grab_designator=grasp_designator3), transitions={ "succeeded": "MOVE_TO_PLACE_3", "failed": "MOVE_TO_PLACE_3" }) # Move to the place location smach.StateMachine.add( "MOVE_TO_PLACE_3", states.NavigateToSymbolic( robot, {place_furniture_designator3: "in_front_of"}, place_furniture_designator3), transitions={ 'arrived': 'PLACE_ITEM_3', 'unreachable': 'PLACE_ITEM_3', 'goal_not_defined': 'PLACE_ITEM_3' }) # Place the first item smach.StateMachine.add("PLACE_ITEM_3", PlaceSingleItem( robot=robot, place_designator=place_designator3), transitions={ "succeeded": "succeeded", "failed": "succeeded" })
def __init__(self, robot, seats_to_inspect, room, seat_is_for=None): smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed']) seats = SeatsInRoomDesignator(robot, seats_to_inspect, room, "seats_in_room") seat_ent_des = ds.VariableDesignator(resolve_type=Entity) if seat_is_for: ds.check_type(seat_is_for, str) else: seat_is_for = ds.Designator(' ') with self: smach.StateMachine.add( 'SAY_LETS_FIND_SEAT', states.SayFormatted(robot, [ "Let me find a place for {name} to sit. Please be patient while I check out where there's place to sit" ], name=seat_is_for, block=False), transitions={'spoken': 'ITERATE_NEXT_SEAT'}) smach.StateMachine.add('ITERATE_NEXT_SEAT', states.IterateDesignator( seats, seat_ent_des.writeable), transitions={ 'next': 'CHECK_SEAT_EMPTY', 'stop_iteration': 'SAY_NO_EMPTY_SEATS' }) smach.StateMachine.add('CHECK_SEAT_EMPTY', states.CheckVolumeEmpty( robot, seat_ent_des, 'on_top_of', 0.2), transitions={ 'occupied': 'ITERATE_NEXT_SEAT', 'empty': 'POINT_AT_EMPTY_SEAT', 'partially_occupied': 'POINT_AT_PARTIALLY_OCCUPIED_SEAT', 'failed': 'ITERATE_NEXT_SEAT' }) smach.StateMachine.add( 'POINT_AT_EMPTY_SEAT', states.PointAt(robot=robot, arm_designator=ds.UnoccupiedArmDesignator( robot, {'required_goals': ['point_at']}), point_at_designator=seat_ent_des, look_at_designator=seat_ent_des), transitions={ "succeeded": "SAY_SEAT_EMPTY", "failed": "SAY_SEAT_EMPTY" }) smach.StateMachine.add( 'SAY_SEAT_EMPTY', states.SayFormatted(robot, ["Please sit on the {seat}, {name}"], name=seat_is_for, seat=ds.AttrDesignator(seat_ent_des, 'id', resolve_type=str), block=True), transitions={'spoken': 'RESET_SUCCESS'}) smach.StateMachine.add( 'POINT_AT_PARTIALLY_OCCUPIED_SEAT', states.PointAt(robot=robot, arm_designator=ds.UnoccupiedArmDesignator( robot, {'required_goals': ['point_at']}), point_at_designator=seat_ent_des, look_at_designator=seat_ent_des), transitions={ "succeeded": "SAY_SEAT_PARTIALLY_OCCUPIED", "failed": "SAY_SEAT_PARTIALLY_OCCUPIED" }) smach.StateMachine.add( 'SAY_SEAT_PARTIALLY_OCCUPIED', states.SayFormatted(robot, [ "I think there's some space left here where you can sit {name}" ], name=seat_is_for, block=True), transitions={'spoken': 'RESET_SUCCESS'}) smach.StateMachine.add( 'SAY_NO_EMPTY_SEATS', states.SayFormatted(robot, [ "Sorry, there are no empty seats. I guess you just have to stand {name}" ], name=seat_is_for, block=True), transitions={'spoken': 'RESET_FAIL'}) smach.StateMachine.add('RESET_FAIL', states.ResetArms(robot), transitions={'done': 'failed'}) smach.StateMachine.add('RESET_SUCCESS', states.ResetArms(robot), transitions={'done': 'succeeded'})
import rospy import argparse # TU/e Robotics from robot_skills.get_robot import get_robot from robot_skills.arms import GripperState, GripperTypes # Robot Smach States from robot_smach_states.manipulation import SetGripper import robot_smach_states.util.designators as ds if __name__ == "__main__": parser = argparse.ArgumentParser( description="Test the gripper state whether it can open and close") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() rospy.init_node("Set_gripper_state") robot = get_robot(args.robot) arm = ds.UnoccupiedArmDesignator( robot, {"required_gripper_types": [GripperTypes.GRASPING]}) for gripper_state in [GripperState.CLOSE, GripperState.OPEN]: set_gripper_state = SetGripper(robot, arm, gripperstate=gripper_state) outcome = set_gripper_state.execute() rospy.loginfo("Set gripper state to {}, outcome: {}".format( gripper_state, outcome)) rospy.sleep(2.5)
robot = get_robot(args.robot) entity_id = "test_item" pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)), frame_id="/map") robot.ed.update_entity(id=entity_id, frame_stamped=pose) shape = RightPrism([ kdl.Vector(0, 0, 0), kdl.Vector(0, 0.05, 0), kdl.Vector(0.05, 0.05, 0), kdl.Vector(0.05, 0, 0) ], -0.1, 0.1) item = Entity(entity_id, "test_type", pose.frame_id, pose.frame, shape, None, None, None) item = ds.Designator(item) arm = ds.UnoccupiedArmDesignator(robot, arm_properties={ "required_trajectories": ["prepare_grasp"], "required_goals": ["carrying_pose"], "required_gripper_types": [arms.GripperTypes.GRASPING] }) grab_state = Grab(robot, item, arm) grab_state.execute()
def single_item(robot, results_writer, cls, support, waypoint, inspect_from_area=None, non_strict_class=False, search_area='on_top_of'): """ Benchmark grasping for a single item. Outputs a record dictionary :param robot: an instance of Robot :param results_writer: a csv.DictWriter to which the output record is written :param cls: class/type of item to grab :param support: ID of the entity supporting the item-to-grab :param waypoint: From where should the robot start the grasp :param inspect_from_area: Which area of the support-entity should the robot be in to start the inspection :param non_strict_class: If set to True, the robot is not strict about the type of item it grabs, eg. it continues grasping with another type of object :param search_area: which area of the support-entity to search/inspect for an item of the given class :return: a dict with the benchmark result """ grasp_cls = ds.Designator(cls, name='grasp_cls') support_entity = ds.EdEntityDesignator(robot, id=support, name='support_entity') entity_ids = ds.VariableDesignator([], resolve_type=[ClassificationResult], name='entity_ids') waypoint_des = ds.EdEntityDesignator(robot, id=waypoint, name='waypoint') arm = ds.LockingDesignator( ds.UnoccupiedArmDesignator(robot, name='unoccupied-arm', arm_properties={ "required_trajectories": ["prepare_grasp"], "required_goals": ["carrying_pose"], "required_gripper_types": [arms.GripperTypes.GRASPING] })) arm.lock() record = { 'robot': robot.robot_name, 'start_waypoint': waypoint, 'expected_class': cls, 'id': None, 'timestamp': datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S') } try: say_announce = Say( robot, sentence="Please put a {cls} {search_area} the {support}".format( cls=cls, support=support, search_area=search_area)) say_announce.execute() nav_to_start = NavigateToWaypoint(robot, waypoint_designator=waypoint_des, look_at_designator=support_entity) assert nav_to_start.execute( ) == 'arrived', "I did not arrive at the start" inspect = Inspect(robot=robot, entityDes=support_entity, objectIDsDes=entity_ids, navigation_area=inspect_from_area, searchArea=search_area) record['inspect_start'] = time.time() assert inspect.execute( ) == 'done', "I could not inspect the support entity" record['inspect_end'] = time.time() record['inspect_duration'] = '{:3.3f}'.format(record['inspect_end'] - record['inspect_start']) inspection_result = entity_ids.resolve( ) # type: List[ClassificationResult] if inspection_result: if grasp_cls.resolve() in ANY_OPTIONS or non_strict_class: rospy.loginfo("Any item will match") matching_results = inspection_result else: matching_results = [ result for result in inspection_result if result.type == grasp_cls.resolve() ] rospy.loginfo("Found {} items of class {}".format( len(matching_results), grasp_cls.resolve())) if matching_results: if len(matching_results) > 1: rospy.logwarn( "There are multiple items OK to grab, will select the last one" ) record['observed_class'] = ' '.join( [result.type for result in matching_results]) else: record['observed_class'] = matching_results[-1].type selected_entity_id = matching_results[-1].id rospy.loginfo("Selected entity {} for grasping".format( selected_entity_id)) grasp_entity = ds.EdEntityDesignator(robot, id=selected_entity_id) record['id'] = selected_entity_id[:6] entity = grasp_entity.resolve() # type: Entity if entity: vector_stamped = entity.pose.extractVectorStamped( ) # type: VectorStamped record['x'] = '{:.3f}'.format(vector_stamped.vector.x()) record['y'] = '{:.3f}'.format(vector_stamped.vector.y()) record['z'] = '{:.3f}'.format(vector_stamped.vector.z()) grab_state = Grab(robot, grasp_entity, arm) record['grab_start'] = time.time() assert grab_state.execute() == 'done', "I couldn't grasp" record['grab_end'] = time.time() record['grab_duration'] = '{:3.3f}'.format( record['grab_end'] - record['grab_start']) assert nav_to_start.execute( ) == 'arrived', "I could not navigate back to the start" rospy.logwarn("Robot will turn around to drop the {}".format( grasp_cls.resolve())) force_drive = ForceDrive(robot, 0, 0, 1, 3.14) # rotate 180 degs in pi seconds force_drive.execute() say_drop = Say( robot, sentence="I'm going to drop the item, please hold it!") say_drop.execute() drop_it = SetGripper(robot, arm_designator=arm, grab_entity_designator=grasp_entity) drop_it.execute() force_drive_back = ForceDrive( robot, 0, 0, -1, 3.14) # rotate -180 degs in pi seconds force_drive_back.execute() nav_to_start.execute() else: raise AssertionError("No {} found".format(grasp_cls.resolve())) else: rospy.logerr("No entities found at all :-(") except AssertionError as assertion_err: say_fail = Say(robot, sentence=assertion_err.message + ", sorry") say_fail.execute() finally: results_writer.writerow(record) return record
def setup_statemachine(robot): place_name = ds.EntityByIdDesignator(robot, id=challenge_knowledge.default_place, name="place_name") place_position = ds.LockingDesignator(ds.EmptySpotDesignator(robot, place_name, name="placement", area=challenge_knowledge.default_area), name="place_position") empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms, robot.rightArm, name="empty_arm_designator") # With the empty_arm_designator locked, it will ALWAYS resolve to the same arm (first resolve is cached), unless it is unlocked # For this challenge, unlocking is not needed bag_arm_designator = empty_arm_designator.lockable() bag_arm_designator.lock() # We don't actually grab something, so there is no need for an actual thing to grab current_item = ds.VariableDesignator(Entity("dummy", "dummy", "/{}/base_link".format(robot.robot_name), kdl_conversions.kdlFrameFromXYZRPY(0.6, 0, 0.5), None, {}, [], datetime.datetime.now()), name="current_item") sm = smach.StateMachine(outcomes=['Done','Aborted']) with sm: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={'initialized': 'SET_INITIAL_POSE', 'abort': 'Aborted'}) smach.StateMachine.add('SET_INITIAL_POSE', states.SetInitialPose(robot, challenge_knowledge.starting_point), transitions={'done': 'FOLLOW_OPERATOR', "preempted": 'Aborted', 'error': 'FOLLOW_OPERATOR'}) # TODO: learn operator state needs to be added before follow # smach.StateMachine.add('WAIT_TO_FOLLOW', # WaitForOperatorCommand(robot, possible_commands=['follow', 'follow me']), # transitions={'success': 'FOLLOW_OPERATOR', # 'abort': 'Aborted'}) smach.StateMachine.add('ASK_FOLLOW_OR_REMEMBER', states.Say(robot, ["Are we at the car or should I follow you?"], block=True), transitions={'spoken': 'WAIT_TO_FOLLOW_OR_REMEMBER'}) smach.StateMachine.add('WAIT_TO_FOLLOW_OR_REMEMBER', WaitForOperatorCommand(robot, possible_commands=[ "follow", 'follow me', "here is the car", "stop following", "stop following me", ], commands_as_outcomes=True), transitions={'follow': 'FOLLOW_OPERATOR', 'follow me': 'FOLLOW_OPERATOR', 'here is the car': 'REMEMBER_CAR_LOCATION', 'stop following': 'REMEMBER_CAR_LOCATION', 'stop following me': 'REMEMBER_CAR_LOCATION', 'abort': 'Aborted'}) # Follow the operator until (s)he states that you have arrived at the "car". smach.StateMachine.add('FOLLOW_OPERATOR', states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=True, replan=True), transitions={'stopped': 'ASK_FOLLOW_OR_REMEMBER', 'lost_operator': 'ASK_FOLLOW_OR_REMEMBER', 'no_operator': 'ASK_FOLLOW_OR_REMEMBER'}) smach.StateMachine.add('REMEMBER_CAR_LOCATION', StoreCarWaypoint(robot), transitions={'success': 'ASK_DESTINATION', 'abort': 'Aborted'}) smach.StateMachine.add('ASK_DESTINATION', states.Say(robot, ["Where should I bring the groceries?"], block=True), transitions={'spoken': 'WAIT_FOR_DESTINATION'}) smach.StateMachine.add('WAIT_FOR_DESTINATION', WaitForOperatorCommand(robot, possible_commands=challenge_knowledge.waypoints.keys(), commands_as_userdata=True), transitions={'success': 'GRAB_ITEM', 'abort': 'Aborted'}) # Grab the item (bag) the operator hands to the robot, when they are at the "car". smach.StateMachine.add('GRAB_ITEM', GrabItem(robot, bag_arm_designator, current_item), transitions={'succeeded': 'ARM_DRIVING_POSE', 'timeout': 'BACKUP_CLOSE_GRIPPER', # For now in simulation timeout is considered a succes. 'failed': 'BACKUP_CLOSE_GRIPPER'}, remapping={'target_room_in': 'command_recognized', 'target_room_out': 'target_room'}) smach.StateMachine.add('BACKUP_CLOSE_GRIPPER', states.SetGripper(robot, bag_arm_designator, gripperstate=GripperState.CLOSE), transitions={'succeeded': 'ARM_DRIVING_POSE', 'failed': 'ARM_DRIVING_POSE'}) smach.StateMachine.add('ARM_DRIVING_POSE', states.ArmToJointConfig(robot, bag_arm_designator, 'driving_bag_pose'), transitions={'succeeded': 'SAY_GOING_TO_ROOM', 'failed': 'SAY_GOING_TO_ROOM'}) smach.StateMachine.add('SAY_GOING_TO_ROOM', states.Say(robot, ["Let me bring in your groceries", "Helping you carry stuff", "I'm going back inside"], block=True), transitions={'spoken': 'GOTO_DESTINATION'}) smach.StateMachine.add('GOTO_DESTINATION', NavigateToRoom(robot), transitions={'arrived': 'PUTDOWN_ITEM', 'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted'}) # Put the item (bag) down when the robot has arrived at the "drop-off" location (house). smach.StateMachine.add('PUTDOWN_ITEM', DropBagOnGround(robot, bag_arm_designator), transitions={'succeeded': 'ASKING_FOR_HELP', 'failed': 'ASKING_FOR_HELP'}) smach.StateMachine.add('ASKING_FOR_HELP', #TODO: look and then face new operator states.Say(robot, "Please follow me and help me carry groceries into the house"), transitions={'spoken': 'GOTO_CAR'}) #transitions={'success': 'GOTO_CAR', # 'abort': 'Aborted'}) smach.StateMachine.add('GOTO_CAR', states.NavigateToWaypoint(robot, ds.EntityByIdDesignator(robot, id=challenge_knowledge.waypoint_car['id']), challenge_knowledge.waypoint_car['radius']), # TODO: detect closed door transitions={'unreachable': 'OPEN_DOOR', 'arrived': 'AT_END', 'goal_not_defined': 'Aborted'}) smach.StateMachine.add('OPEN_DOOR', #TODO: implement functionality states.Say(robot, "Please open the door for me"), transitions={'spoken': 'GOTO_CAR'}) #transitions={'success': 'GOTO_CAR', # 'abort': 'Aborted'}) smach.StateMachine.add('AT_END', states.Say(robot, ["We arrived at the car, goodbye", "You have reached your destination, goodbye", "The car is right here, see you later!"], block=True), transitions={'spoken': 'Done'}) ds.analyse_designators(sm, "help_me_carry") return sm
import robot_smach_states.util.designators as ds from robot_smach_states.manipulation import HandoverFromHuman, HandoverToHuman from robot_smach_states import LockDesignator if __name__ == "__main__": parser = argparse.ArgumentParser(description="Test handover to human state") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() rospy.init_node("test_handover_stuff") robot = get_robot(args.robot) rospy.loginfo("Creating arm designator") arm_designator = ds.UnoccupiedArmDesignator(robot=robot, arm_properties={"required_goals": ["handover_to_human", "reset"], "required_gripper_types": [arms.GripperTypes.GRASPING]}, name='arm_des').lockable() rospy.loginfo("Creating lock designator state") s0 = LockDesignator(arm_designator) rospy.loginfo("Creating handover from human state") s1 = HandoverFromHuman(robot, arm_designator, grabbed_entity_label="foo") rospy.loginfo("Creating handover to human state") s2 = HandoverToHuman(robot, arm_designator) rospy.loginfo("Executing lock designator state") s0.execute() rospy.loginfo("Executing handover from human state")
def __init__(self, robot): """ Initialization method :param robot: robot api object """ smach.StateMachine.__init__( self, outcomes=["succeeded", "failed", "aborted"]) # Create designators trashbin_designator = ds.EdEntityDesignator( robot=robot, id=CHALLENGE_KNOWLEDGE.trashbin_id, name='trashbin_designator') # Look if there is a second trash bin present # trashbin_designator2 = None if hasattr(CHALLENGE_KNOWLEDGE, "trashbin_id2"): trashbin_designator2 = ds.EdEntityDesignator( robot=robot, id=CHALLENGE_KNOWLEDGE.trashbin_id2, name='trashbin_designator2') next_state = "HELPER_WAYPOINT" rospy.loginfo("There is a second trash bin") else: rospy.loginfo("There is no second trash bin") next_state = "ANNOUNCE_END" # drop_zone_designator = ds.EdEntityDesignator(robot=robot, id=CHALLENGE_KNOWLEDGE.drop_zone_id) helper_waypoint_designator = ds.EdEntityDesignator( robot=robot, id=CHALLENGE_KNOWLEDGE.helper_waypoint) end_waypoint_designator = ds.EdEntityDesignator( robot=robot, id=CHALLENGE_KNOWLEDGE.end_waypoint) arm_designator = self.empty_arm_designator = ds.UnoccupiedArmDesignator( robot, {}, name="empty_arm_designator") with self: smach.StateMachine.add("START_CHALLENGE_ROBUST", states.StartChallengeRobust( robot, CHALLENGE_KNOWLEDGE.starting_point), transitions={ "Done": "SAY_START_CHALLENGE", "Aborted": "SAY_START_CHALLENGE", "Failed": "SAY_START_CHALLENGE" }) smach.StateMachine.add("SAY_START_CHALLENGE", states.Say( robot, "I will start cleaning up the trash", block=True), transitions={'spoken': "PICK_UP_TRASH"}) smach.StateMachine.add("PICK_UP_TRASH", PickUpTrash( robot=robot, trashbin_designator=trashbin_designator, arm_designator=arm_designator), transitions={ "succeeded": "DROP_DOWN_TRASH", "failed": "HELPER_WAYPOINT", "aborted": "ANNOUNCE_END" }) smach.StateMachine.add( "DROP_DOWN_TRASH", DropDownTrash(robot=robot, drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id), transitions={ "succeeded": "ANNOUNCE_TASK", "failed": "failed", "aborted": "aborted" }) smach.StateMachine.add( "ANNOUNCE_TASK", states.Say(robot, "First bag has been dropped at the collection zone", block=False), transitions={'spoken': next_state}) if next_state == "HELPER_WAYPOINT": smach.StateMachine.add( "HELPER_WAYPOINT", states.NavigateToWaypoint( robot=robot, waypoint_designator=helper_waypoint_designator), transitions={ "arrived": "PICK_UP_TRASH2", "goal_not_defined": "PICK_UP_TRASH2", "unreachable": "PICK_UP_TRASH2" }) smach.StateMachine.add( "PICK_UP_TRASH2", PickUpTrash(robot=robot, trashbin_designator=trashbin_designator2, arm_designator=arm_designator), transitions={ "succeeded": "DROP_DOWN_TRASH2", "failed": "ANNOUNCE_END", "aborted": "ANNOUNCE_END" }) smach.StateMachine.add( "DROP_DOWN_TRASH2", DropDownTrash( robot=robot, drop_zone_id=CHALLENGE_KNOWLEDGE.drop_zone_id), transitions={ "succeeded": "ANNOUNCE_TASK2", "failed": "failed", "aborted": "aborted" }) smach.StateMachine.add( "ANNOUNCE_TASK2", states.Say( robot, "Second bag has been dropped at the collection zone." "All the thrash has been taken care of", block=False), transitions={'spoken': 'ANNOUNCE_END'}) smach.StateMachine.add("ANNOUNCE_END", states.Say( robot, "I have finished taking out the trash.", block=False), transitions={'spoken': 'NAVIGATE_OUT'}) smach.StateMachine.add( "NAVIGATE_OUT", states.NavigateToWaypoint( robot=robot, waypoint_designator=end_waypoint_designator), transitions={ "arrived": "succeeded", "goal_not_defined": "succeeded", "unreachable": "succeeded" })
from robot_smach_states import LockDesignator if __name__ == "__main__": parser = argparse.ArgumentParser( description="Test handover to human state") parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)") args = parser.parse_args() rospy.init_node("test_handover_stuff") robot = get_robot(args.robot) rospy.loginfo("Creating arm designator") arm_designator = ds.UnoccupiedArmDesignator(robot=robot, arm_properties={}, name='arm_des').lockable() rospy.loginfo("Creating lock designator state") s0 = LockDesignator(arm_designator) rospy.loginfo("Creating handover from human state") s1 = HandoverFromHuman(robot, arm_designator, grabbed_entity_label="foo") rospy.loginfo("Creating handover to human state") s2 = HandoverToHuman(robot, arm_designator) rospy.loginfo("Executing lock designator state") s0.execute() rospy.loginfo("Executing handover from human state")
def __init__(self, robot): smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted']) self.target_destination = ds.EntityByIdDesignator( robot, id=challenge_knowledge.default_place) self.car_waypoint = ds.EntityByIdDesignator( robot, id=challenge_knowledge.waypoint_car['id']) self.place_position = ds.LockingDesignator(ds.EmptySpotDesignator( robot, self.target_destination, name="placement", area=challenge_knowledge.default_area), name="place_position") self.empty_arm_designator = ds.UnoccupiedArmDesignator( robot, {}, name="empty_arm_designator") # With the empty_arm_designator locked, it will ALWAYS resolve to the same arm, unless it is unlocked. # For this challenge, unlocking is not needed. self.bag_arm_designator = self.empty_arm_designator.lockable() self.bag_arm_designator.lock() # We don't actually grab something, so there is no need for an actual thing to grab self.current_item = ds.VariableDesignator(Entity( "dummy", "dummy", "/{}/base_link".format(robot.robot_name), kdl_conversions.kdl_frame_from_XYZRPY(0.6, 0, 0.5), None, {}, [], datetime.datetime.now()), name="current_item") with self: smach.StateMachine.add('INITIALIZE', states.Initialize(robot), transitions={ 'initialized': 'SET_INITIAL_POSE', 'abort': 'Aborted' }) smach.StateMachine.add('SET_INITIAL_POSE', states.SetInitialPose( robot, challenge_knowledge.starting_point), transitions={ 'done': 'FOLLOW_OPERATOR', 'preempted': 'Aborted', 'error': 'FOLLOW_OPERATOR' }) # Follow the operator until (s)he states that you have arrived at the "car". # smach.StateMachine.add('FOLLOW_OPERATOR', # states.FollowOperator(robot, operator_timeout=30, ask_follow=True, learn_face=True, replan=True), # transitions={'stopped': 'ASK_FOR_TASK', # 'lost_operator': 'ASK_FOR_TASK', # 'no_operator': 'FOLLOW_OPERATOR'}) # Use NEW: smach.StateMachine.add('FOLLOW_OPERATOR', states.FollowOperator2(robot), transitions={ 'Done': 'ASK_FOR_TASK', 'Failed': 'ASK_FOR_TASK', 'Aborted': 'FOLLOW_OPERATOR' }) smach.StateMachine.add('ASK_FOR_TASK', states.Say(robot, ["Are we at the car already?"], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_TASK'}) smach.StateMachine.add('WAIT_FOR_TASK', states.HearOptions(robot, ['yes', 'no']), transitions={ 'no': 'FOLLOW_OPERATOR', 'yes': 'CONFIRM_CAR_LOCATION', 'no_result': 'ASK_FOR_TASK' }) smach.StateMachine.add( 'CONFIRM_CAR_LOCATION', states.Say( robot, ["OK, I will remember this location as the car location."], block=True, look_at_standing_person=True), transitions={'spoken': 'REMEMBER_CAR_LOCATION'}) smach.StateMachine.add('REMEMBER_CAR_LOCATION', hmc_states.StoreCarWaypoint(robot), transitions={ 'success': 'ASK_FOR_DESTINATION', 'abort': 'Aborted' }) smach.StateMachine.add( 'ASK_FOR_DESTINATION', states.Say(robot, ["Where should I bring the groceries?"], block=True, look_at_standing_person=True), transitions={'spoken': 'RECEIVE_DESTINATION'}) smach.StateMachine.add( 'RECEIVE_DESTINATION', hmc_states.WaitForOperatorCommand( robot, possible_commands=challenge_knowledge.destinations, commands_as_userdata=True, target=self.target_destination), transitions={ 'success': 'GRAB_ITEM', 'abort': 'Aborted' }) # # smach.StateMachine.add('CONFIRM_DESTINATION', # states.Say(robot, [ # "I will deliver the groceries to the %s" % ds.EntityByIdDesignator(self.target_destination)], # block=True, # look_at_standing_person=True), # transitions={'spoken': 'GRAB_ITEM'}) # Grab the item (bag) the operator hands to the robot, when they are at the "car". smach.StateMachine.add( 'GRAB_ITEM', # states.HandoverFromHuman(robot, self.bag_arm_designator, "current_item", # self.current_item, # arm_configuration=challenge_knowledge.carrying_bag_pose), # transitions={'succeeded': 'ARM_DRIVING_POSE', # 'timeout': 'BACKUP_CLOSE_GRIPPER', # # For now in simulation timeout is considered a success. # 'failed': 'BACKUP_CLOSE_GRIPPER'}) states.Say(robot, [ "I can't pick up the groceries since I don't have arms. Please place them in my basket." ], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_GRAB_ITEM'}) smach.StateMachine.add('WAIT_FOR_GRAB_ITEM', states.WaitTime(robot), transitions={ 'waited': 'SAY_GOING_TO_ROOM', 'preempted': 'Aborted' }) # smach.StateMachine.add('BACKUP_CLOSE_GRIPPER', # states.SetGripper(robot, self.bag_arm_designator, gripperstate=GripperState.CLOSE), # transitions={'succeeded': 'ARM_DRIVING_POSE', # 'failed': 'ARM_DRIVING_POSE'}) # # smach.StateMachine.add('ARM_DRIVING_POSE', # states.ArmToJointConfig(robot, self.bag_arm_designator, # challenge_knowledge.driving_bag_pose), # transitions={'succeeded': 'SAY_GOING_TO_ROOM', # 'failed': 'SAY_GOING_TO_ROOM'}) smach.StateMachine.add( 'SAY_GOING_TO_ROOM', states.Say(robot, [ "Let me bring in your groceries", "Helping you carry stuff", "I'm going back inside" ], block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_DESTINATION'}) smach.StateMachine.add( 'GOTO_DESTINATION', states.NavigateToSymbolic( robot, {self.target_destination: "in_front_of"}, self.target_destination), transitions={ 'arrived': 'PUTDOWN_ITEM', 'unreachable': 'TURN_180_TO_REPLAN', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted' }) smach.StateMachine.add( 'TURN_180_TO_REPLAN', hmc_states.TurnToReplan(robot), transitions={ 'success': 'GOTO_DESTINATION_BACKUP', 'abort': 'GOTO_DESTINATION_BACKUP', # implement avoid obstacle behaviour later #'goal_not_defined': 'Aborted'}) }) smach.StateMachine.add( 'GOTO_DESTINATION_BACKUP', states.NavigateToSymbolic( robot, {self.target_destination: "in_front_of"}, self.target_destination), transitions={ 'arrived': 'PUTDOWN_ITEM', 'unreachable': 'PUTDOWN_ITEM', # implement avoid obstacle behaviour later 'goal_not_defined': 'Aborted' }) # Put the item (bag) down when the robot has arrived at the "drop-off" location (house). smach.StateMachine.add( 'PUTDOWN_ITEM', # hmc_states.DropBagOnGround(robot, self.bag_arm_designator, # challenge_knowledge.drop_bag_pose), states.Say(robot, [ "I can't put the groceries down since I have no arms. Please take them from my basket and put it down." ], block=True, look_at_standing_person=True), transitions={'spoken': 'WAIT_FOR_PUTDOWN_ITEM'}) smach.StateMachine.add('WAIT_FOR_PUTDOWN_ITEM', states.WaitTime(robot), transitions={ 'waited': 'ASKING_FOR_HELP', 'preempted': 'Aborted' }) smach.StateMachine.add( 'ASKING_FOR_HELP', # TODO: look and then face new operator states.Say( robot, "Please follow me and help me carry groceries into the house", block=True, look_at_standing_person=True), transitions={'spoken': 'GOTO_CAR'}) #'LEARN_OPERATOR'}) # smach.StateMachine.add('LEARN_OPERATOR', # hmc_states.LearnOperator(robot), # transitions={'learned': 'GOTO_CAR', # 'failed': 'GOTO_CAR'}) smach.StateMachine.add( 'GOTO_CAR', states.NavigateToWaypoint( robot, self.car_waypoint, challenge_knowledge.waypoint_car['radius']), # TODO: detect closed door transitions={ 'unreachable': 'OPEN_DOOR', 'arrived': 'AT_END', 'goal_not_defined': 'Aborted' }) smach.StateMachine.add( 'OPEN_DOOR', # TODO: implement functionality states.Say(robot, "Please open the door for me"), transitions={'spoken': 'GOTO_CAR'}) smach.StateMachine.add( 'AT_END', states.Say(robot, [ "We arrived at the car, goodbye", "You have reached your destination, goodbye", "The car is right here, see you later!" ], block=True, look_at_standing_person=True), transitions={'spoken': 'Done'}) ds.analyse_designators(self, "help_me_carry")
def __init__(self, robot, bar_designator, room_id, room_designator, objects_list_des, unav_drink_des, name_options, objects): """ Initialization method :param robot: robot api object :param bar_designator: (EntityDesignator) in which the bar location is stored :param room_id: room ID from challenge knowledge :param room_designator: (EntityDesignator) in which the room location is stored :param objects_list_des: (VariableDesignator) in which the available drinks are stored :param unav_drink_des: (VariableDesignator) in which the unavailable drink is stored :param name_options: Names from common knowledge :param objects: Objects from common knowledge """ smach.StateMachine.__init__(self, outcomes=["succeeded", "failed", "aborted"]) # Designators arm_designator = ds.UnoccupiedArmDesignator(robot=robot, arm_properties={}, name='arm_des').lockable() drink_str_designator = ds.VariableDesignator(resolve_type=str, name='drink_str_des') drink_designator = ds.EdEntityDesignator(robot=robot, type_designator=drink_str_designator, name='drink_des') operator_name = ds.VariableDesignator(resolve_type=str, name='name_des') operator_designator = ds.VariableDesignator(resolve_type=Entity, name='operator_des') learn_check_designator = ds.VariableDesignator(initial_value=True, resolve_type=bool, name='learn_check_des') hacky_arm_des = ds.VariableDesignator(initial_value=robot.get_arm(), name='hacky_arm_2') with self: # Lock the arm_designator smach.StateMachine.add("LOCK_ARM", states.LockDesignator(arm_designator), transitions={'locked': "GET_ORDER"}) # Get order smach.StateMachine.add("GET_ORDER", GetOrder(robot=robot, operator_name=operator_name, drink_designator=drink_str_designator, available_drinks_designator=objects_list_des, unavailable_drink_designator=unav_drink_des, name_options=name_options, objects=objects, learn_check_designator=learn_check_designator.writeable, target_room_designator=room_designator), transitions={"succeeded": "INSPECT_BAR", "failed": "failed", "aborted": "aborted"}) # Inspect bar smach.StateMachine.add("INSPECT_BAR", states.Inspect(robot=robot, entityDes=bar_designator, navigation_area="in_front_of"), transitions={"done": "GRASP_DRINK", "failed": "FALLBACK_BAR"}) # Grasp drink smach.StateMachine.add("GRASP_DRINK", states.Grab(robot=robot, item=drink_designator, arm=arm_designator), transitions={"done": "FIND_OPERATOR", "failed": "FALLBACK_BAR"}) # Inspect or grasp fallback - ask for assistance smach.StateMachine.add("FALLBACK_BAR", states.Say(robot=robot, sentence=DescriptionStrDesignator("fallback_bar", drink_str_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "HANDOVER_FROM_HUMAN"}) # Handover from human fallback smach.StateMachine.add("HANDOVER_FROM_HUMAN", states.HandoverFromHuman(robot=robot, arm_designator=arm_designator, grabbed_entity_designator=drink_designator), transitions={"succeeded": "RESET_ROBOT_2", "failed": "RESET_ROBOT_2", "timeout": "RESET_ROBOT_2"}) smach.StateMachine.add("RESET_ROBOT_2", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "CHECK_LEARN_OPERATOR", 'failed': "CHECK_LEARN_OPERATOR"}) smach.StateMachine.add("CHECK_LEARN_OPERATOR", states.CheckBool(learn_check_designator), transitions={"true": "FIND_OPERATOR", "false": "GO_TO_ROOM"}) smach.StateMachine.add("GO_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "SAY_NOT_FOUND", "unreachable": "failed", "goal_not_defined": "aborted"}) # Find operator smach.StateMachine.add("FIND_OPERATOR", states.FindPersonInRoom(robot=robot, area=room_id, name=operator_name, discard_other_labels=True, found_entity_designator=operator_designator.writeable), transitions={"found": "GOTO_OPERATOR", "not_found": "SAY_NOT_FOUND"}) # Move to this person smach.StateMachine.add("GOTO_OPERATOR", states.NavigateToObserve(robot=robot, entity_designator=operator_designator), transitions={"arrived": "SAY_THE_NAME", "unreachable": "SAY_NOT_FOUND", "goal_not_defined": "SAY_NOT_FOUND"}) # Say not found smach.StateMachine.add("SAY_NOT_FOUND", states.Say(robot=robot, sentence=DescriptionStrDesignator("not_found_operator", drink_str_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "RISE_FOR_HMI_2"}) # Say the name smach.StateMachine.add("SAY_THE_NAME", states.Say(robot=robot, sentence=DescriptionStrDesignator("found_operator", drink_str_designator, operator_name), look_at_standing_person=True), transitions={"spoken": "RISE_FOR_HMI_2"}) smach.StateMachine.add("RISE_FOR_HMI_2", states.RiseForHMI(robot=robot), transitions={"succeeded": "HAND_OVER", "failed": "HAND_OVER"}) # Hand over the drink to the operator smach.StateMachine.add("HAND_OVER", states.HandoverToHuman(robot=robot, arm_designator=arm_designator), transitions={"succeeded": "UNLOCK_ARM", "failed": "UNLOCK_ARM"}) smach.StateMachine.add("UNLOCK_ARM", states.UnlockDesignator(arm_designator), transitions={'unlocked': "RESET_ROBOT_3"}) smach.StateMachine.add("RESET_ROBOT_3", states.ArmToJointConfig(robot=robot, arm_designator=hacky_arm_des, configuration="reset"), transitions={'succeeded': "RETURN_TO_ROOM", 'failed': "RETURN_TO_ROOM"}) smach.StateMachine.add("RETURN_TO_ROOM", states.NavigateToRoom(robot=robot, entity_designator_room=room_designator), transitions={"arrived": "succeeded", "unreachable": "failed", "goal_not_defined": "aborted"})