def create(self): # x:254 y:640, x:565 y:232, x:530 y:448 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Count", "bottle", "behavior/Count/CountedObject"] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:41 y:32 OperatableStateMachine.add('get name', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Say_Start'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'className'}) # x:22 y:213 OperatableStateMachine.add('Action_count', self.use_behavior(sara_flexbe_behaviors__Action_countSM, 'Action_count'), transitions={'done': 'get paramname', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'className', 'Count': 'Count'}) # x:14 y:462 OperatableStateMachine.add('concat', FlexibleCalculationState(calculation=lambda x: "I counted "+str(x[0])+" "+str(x[1])+".", input_keys=["Count", "className"]), transitions={'done': 'Say_Count'}, autonomy={'done': Autonomy.Off}, remapping={'Count': 'Count', 'className': 'className', 'output_value': 'sentence'}) # x:28 y:388 OperatableStateMachine.add('store param', SetRosParamKey(), transitions={'done': 'concat'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Count', 'ParamName': 'ParamName'}) # x:45 y:290 OperatableStateMachine.add('get paramname', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'store param'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'ParamName'}) # x:17 y:633 OperatableStateMachine.add('set head back', SaraSetHeadAngle(pitch=-0.3, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:36 y:120 OperatableStateMachine.add('Say_Start', SaraSay(sentence=lambda x: "I'm starting to count the "+str(x)+"s.", input_keys=[], emotion=0, block=True), transitions={'done': 'Action_count'}, autonomy={'done': Autonomy.Off}) # x:27 y:546 OperatableStateMachine.add('Say_Count', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'set head back'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:391 y:586, x:438 y:304 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.StartPosition = "Operator" _state_machine.userdata.PositionBras = "IdlePose" _state_machine.userdata.title = "GPSR" _state_machine.userdata.EntryName = "door1/enter" _state_machine.userdata.ExitName = "door2/exit" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:135 y:542, x:527 y:395 _sm_validate_0 = OperatableStateMachine(outcomes=['done', 'bad'], input_keys=['sentence']) with _sm_validate_0: # x:56 y:62 OperatableStateMachine.add( 'Say_Command', SaraSay( sentence=lambda x: "I heard. " + x + ". Is that correct?", input_keys=[], emotion=0, block=True), transitions={'done': 'get speech'}, autonomy={'done': Autonomy.Off}) # x:44 y:165 OperatableStateMachine.add('get speech', GetSpeech(watchdog=10), transitions={ 'done': 'is yes', 'nothing': 'say repeate', 'fail': 'say repeate' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:91 y:307 OperatableStateMachine.add( 'is yes', CheckConditionState(predicate=lambda x: "yes" in x or "right" in x or "sure" in x), transitions={ 'true': 'done', 'false': 'say repeate' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'words'}) # x:423 y:332 OperatableStateMachine.add( 'say repeate', SaraSay(sentence="Please, repeat your command.", input_keys=[], emotion=1, block=True), transitions={'done': 'bad'}, autonomy={'done': Autonomy.Off}) # x:30 y:324 _sm_look_at_op_1 = OperatableStateMachine(outcomes=['fail']) with _sm_look_at_op_1: # x:61 y:31 OperatableStateMachine.add('set name', SetKey(Value="person"), transitions={'done': 'list persons'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:44 y:110 OperatableStateMachine.add('list persons', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'get closest', 'none_found': 'list persons' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'name', 'entity_list': 'entity_list', 'number': 'number' }) # x:43 y:211 OperatableStateMachine.add( 'get closest', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'look at'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'ID' }) # x:210 y:198 OperatableStateMachine.add('look at', KeepLookingAt(), transitions={'failed': 'list persons'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:307 y:35, x:335 y:491 _sm_get_commands_2 = OperatableStateMachine( outcomes=['fail', 'understood'], output_keys=['ActionForms', 'sentence']) with _sm_get_commands_2: # x:50 y:48 OperatableStateMachine.add( 'say ready', SaraSay(sentence="I'm ready for your commands.", input_keys=[], emotion=1, block=False), transitions={'done': 'GetSpeech'}, autonomy={'done': Autonomy.Off}) # x:32 y:408 OperatableStateMachine.add('SaraNLUgpsr', SaraNLUgpsr(), transitions={ 'understood': 'say understood', 'not_understood': 'say sorry', 'fail': 'say sorry' }, autonomy={ 'understood': Autonomy.Off, 'not_understood': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'sentence': 'sentence', 'ActionForms': 'ActionForms' }) # x:597 y:223 OperatableStateMachine.add( 'say sorry', SaraSay( sentence="Sorry, I could not understand what you said.", input_keys=[], emotion=1, block=True), transitions={'done': 'GetSpeech'}, autonomy={'done': Autonomy.Off}) # x:30 y:491 OperatableStateMachine.add('say understood', SaraSay(sentence="Ok", input_keys=[], emotion=1, block=True), transitions={'done': 'understood'}, autonomy={'done': Autonomy.Off}) # x:44 y:131 OperatableStateMachine.add('GetSpeech', GetSpeech(watchdog=5), transitions={ 'done': 'validate', 'nothing': 'GetSpeech', 'fail': 'fail' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'sentence'}) # x:29 y:259 OperatableStateMachine.add('validate', _sm_validate_0, transitions={ 'done': 'SaraNLUgpsr', 'bad': 'GetSpeech' }, autonomy={ 'done': Autonomy.Inherit, 'bad': Autonomy.Inherit }, remapping={'sentence': 'sentence'}) # x:122 y:413 _sm_end_3 = OperatableStateMachine(outcomes=['done'], input_keys=['ExitName']) with _sm_end_3: # x:30 y:40 OperatableStateMachine.add( 'win', SaraSay(sentence="Thank you. I'm going now.", input_keys=[], emotion=1, block=True), transitions={'done': 'Action_Pass_Door'}, autonomy={'done': Autonomy.Off}) # x:33 y:163 OperatableStateMachine.add( 'Action_Pass_Door', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'End/Action_Pass_Door'), transitions={ 'Done': 'say yay', 'Fail': 'done' }, autonomy={ 'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit }, remapping={'DoorName': 'ExitName'}) # x:210 y:262 OperatableStateMachine.add( 'say yay', SaraSay(sentence="I did it. I'm the best robot.", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:320 y:82, x:322 y:143, x:265 y:407, x:306 y:225, x:430 y:324 _sm_interact_operator_4 = ConcurrencyContainer( outcomes=['fail', 'understood'], output_keys=['ActionForms', 'sentence'], conditions=[('understood', [('Get Commands', 'understood')]), ('fail', [('Get Commands', 'fail')]), ('fail', [('look at op', 'fail')])]) with _sm_interact_operator_4: # x:95 y:45 OperatableStateMachine.add('Get Commands', _sm_get_commands_2, transitions={ 'fail': 'fail', 'understood': 'understood' }, autonomy={ 'fail': Autonomy.Inherit, 'understood': Autonomy.Inherit }, remapping={ 'ActionForms': 'ActionForms', 'sentence': 'sentence' }) # x:99 y:198 OperatableStateMachine.add('look at op', _sm_look_at_op_1, transitions={'fail': 'fail'}, autonomy={'fail': Autonomy.Inherit}) # x:325 y:387, x:314 y:190 _sm_fail_state_5 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['StartPosition']) with _sm_fail_state_5: # x:36 y:29 OperatableStateMachine.add( 'say failed', SaraSay(sentence="I failed. I'm going back to tell my master.", input_keys=[], emotion=1, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:42 y:301 OperatableStateMachine.add( 'get error', GetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={ 'done': 'Say_Error', 'failed': 'finished' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'Error'}) # x:48 y:414 OperatableStateMachine.add( 'Say_Error', SaraSay(sentence=lambda x: "Sorry, I failed because " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:25 y:172 OperatableStateMachine.add( 'Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Fail state/Action_Move'), transitions={ 'finished': 'get error', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'StartPosition'}) # x:588 y:141, x:590 y:545, x:642 y:410 _sm_do_the_actions_6 = OperatableStateMachine( outcomes=['finished', 'failed', 'critical fail'], input_keys=['ActionForms', 'OriginalPose']) with _sm_do_the_actions_6: # x:85 y:33 OperatableStateMachine.add('set i', SetKey(Value=0), transitions={'done': 'is form?'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'index'}) # x:48 y:339 OperatableStateMachine.add( 'GetForm', FlexibleCalculationState(calculation=lambda x: x[0][x[1]], input_keys=["ActionForms", "index"]), transitions={'done': 'Action_Executor'}, autonomy={'done': Autonomy.Off}, remapping={ 'ActionForms': 'ActionForms', 'index': 'index', 'output_value': 'ActionForm' }) # x:57 y:131 OperatableStateMachine.add( 'is form?', FlexibleCheckConditionState( predicate=lambda x: x[1] < len(x[0]), input_keys=["ActionForms", "index"]), transitions={ 'true': 'set setp', 'false': 'finished' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'ActionForms': 'ActionForms', 'index': 'index' }) # x:204 y:237 OperatableStateMachine.add( '++i', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'is form?'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'index', 'output_value': 'index' }) # x:72 y:479 OperatableStateMachine.add( 'Action_Executor', self.use_behavior(sara_flexbe_behaviors__Action_ExecutorSM, 'Do the actions/Action_Executor'), transitions={ 'finished': '++i', 'failed': 'failed', 'critical_fail': 'critical fail' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit }, remapping={'Action': 'ActionForm'}) # x:45 y:241 OperatableStateMachine.add('set setp', StoryboardSetStepKey(), transitions={'done': 'GetForm'}, autonomy={'done': Autonomy.Off}, remapping={'step': 'index'}) # x:424 y:175 _sm_initialisation_7 = OperatableStateMachine( outcomes=['done'], input_keys=['PositionBras', 'EntryName']) with _sm_initialisation_7: # x:62 y:79 OperatableStateMachine.add( 'Action_Pass_Door', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'Initialisation/Action_Pass_Door'), transitions={ 'Done': 'set step', 'Fail': 'done' }, autonomy={ 'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit }, remapping={'DoorName': 'EntryName'}) # x:70 y:306 OperatableStateMachine.add('set story', Set_Story(titre="GPSR", storyline=[]), transitions={'done': 'bras en lair'}, autonomy={'done': Autonomy.Off}) # x:288 y:375 OperatableStateMachine.add( 'say start', SaraSay(sentence="I'm ready to start the GPSR scenario.", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:78 y:205 OperatableStateMachine.add('set step', Set_a_step(step=0), transitions={'done': 'set story'}, autonomy={'done': Autonomy.Off}) # x:101 y:395 OperatableStateMachine.add('bras en lair', MoveitMove(move=True, waitForExecution=False, group="RightArm"), transitions={ 'done': 'say start', 'failed': 'say start' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'PositionBras'}) with _state_machine: # x:33 y:103 OperatableStateMachine.add('Initialisation', _sm_initialisation_7, transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'PositionBras': 'PositionBras', 'EntryName': 'EntryName' }) # x:767 y:196 OperatableStateMachine.add('Do the actions', _sm_do_the_actions_6, transitions={ 'finished': 'say succseed', 'failed': 'Fail state', 'critical fail': 'critical' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical fail': Autonomy.Inherit }, remapping={ 'ActionForms': 'ActionForms', 'OriginalPose': 'OriginalPose' }) # x:250 y:126 OperatableStateMachine.add( 'Fail state', _sm_fail_state_5, transitions={ 'finished': 'lift head', 'failed': 'critical' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'StartPosition': 'StartPosition'}) # x:250 y:257 OperatableStateMachine.add( 'critical', SaraSay( sentence="Critical failure! I require medical assistance.", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:52 y:468 OperatableStateMachine.add('for 3', ForLoop(repeat=3), transitions={ 'do': 'GetOriginalPose', 'end': 'End' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:747 y:475 OperatableStateMachine.add('say succseed', SaraSay( sentence="I succeed my mission.", input_keys=[], emotion=1, block=True), transitions={'done': 'for 3'}, autonomy={'done': Autonomy.Off}) # x:204 y:368 OperatableStateMachine.add('Interact operator', _sm_interact_operator_4, transitions={ 'fail': 'critical', 'understood': 'set step' }, autonomy={ 'fail': Autonomy.Inherit, 'understood': Autonomy.Inherit }, remapping={ 'ActionForms': 'ActionForms', 'sentence': 'sentence' }) # x:553 y:316 OperatableStateMachine.add('set story', StoryboardSetStoryFromAction(), transitions={'done': 'Do the actions'}, autonomy={'done': Autonomy.Off}, remapping={ 'titre': 'title', 'actionList': 'ActionForms' }) # x:404 y:377 OperatableStateMachine.add('set step', Set_a_step(step=0), transitions={'done': 'set story'}, autonomy={'done': Autonomy.Off}) # x:41 y:375 OperatableStateMachine.add( 'GetOriginalPose', Get_Robot_Pose(), transitions={'done': 'Interact operator'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'OriginalPose'}) # x:37 y:564 OperatableStateMachine.add('End', _sm_end_3, transitions={'done': 'finished'}, autonomy={'done': Autonomy.Inherit}, remapping={'ExitName': 'ExitName'}) # x:30 y:288 OperatableStateMachine.add('lift head', SaraSetHeadAngle(pitch=-0.3, yaw=0), transitions={'done': 'GetOriginalPose'}, autonomy={'done': Autonomy.Off}) # x:48 y:23 OperatableStateMachine.add('ContinueButton', ContinueButton(), transitions={ 'true': 'Initialisation', 'false': 'Initialisation' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:21 y:185 OperatableStateMachine.add( 'Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Action_Move'), transitions={ 'finished': 'lift head', 'failed': 'critical' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'StartPosition'}) return _state_machine
def create(self): # x:1023 y:473, x:791 y:291, x:797 y:103 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Guide", 'table', 'kitchen'] _state_machine.userdata.relative = False # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:458 _sm_groupwait_0 = OperatableStateMachine(outcomes=['end']) with _sm_groupwait_0: # x:30 y:40 OperatableStateMachine.add('waitwait', WaitState(wait_time=20), transitions={'done': 'end'}, autonomy={'done': Autonomy.Off}) # x:534 y:319 _sm_move_head_and_base_at_the_end_1 = OperatableStateMachine( outcomes=['failed']) with _sm_move_head_and_base_at_the_end_1: # x:52 y:31 OperatableStateMachine.add('setkeyorientation', SetKey(Value=1.5), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:51 y:114 OperatableStateMachine.add( 'action_turn', self.use_behavior( action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn' ), transitions={ 'finished': 'turn right head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:48 y:272 OperatableStateMachine.add('wait while head turn', WaitState(wait_time=4), transitions={'done': 'action_turn_2'}, autonomy={'done': Autonomy.Off}) # x:47 y:199 OperatableStateMachine.add( 'turn right head', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'wait while head turn'}, autonomy={'done': Autonomy.Off}) # x:45 y:348 OperatableStateMachine.add( 'action_turn_2', self.use_behavior( action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn_2' ), transitions={ 'finished': 'left to rigth', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:43 y:592 OperatableStateMachine.add('left to rigth', SaraSetHeadAngle(pitch=0, yaw=-1.57), transitions={'done': 'waitwait1'}, autonomy={'done': Autonomy.Off}) # x:420 y:602 OperatableStateMachine.add('right to left', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'waitwait2'}, autonomy={'done': Autonomy.Off}) # x:266 y:541 OperatableStateMachine.add('waitwait1', WaitState(wait_time=8), transitions={'done': 'right to left'}, autonomy={'done': Autonomy.Off}) # x:236 y:674 OperatableStateMachine.add('waitwait2', WaitState(wait_time=8), transitions={'done': 'left to rigth'}, autonomy={'done': Autonomy.Off}) # x:231 y:538 _sm_find_a_human_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID']) with _sm_find_a_human_2: # x:78 y:275 OperatableStateMachine.add('find the human', GetEntityByID(), transitions={ 'found': 'finished', 'not_found': 'find the human' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:415 y:99, x:318 y:246, x:442 y:295 _sm_move_head_and_base_end__3 = ConcurrencyContainer( outcomes=['failed'], conditions=[('failed', [('move head and base at the end', 'failed') ]), ('failed', [('Groupwait', 'end')])]) with _sm_move_head_and_base_end__3: # x:132 y:57 OperatableStateMachine.add('move head and base at the end', _sm_move_head_and_base_at_the_end_1, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:121 y:218 OperatableStateMachine.add('Groupwait', _sm_groupwait_0, transitions={'end': 'failed'}, autonomy={'end': Autonomy.Inherit}) # x:67 y:227 _sm_export_no_waypoint_4 = OperatableStateMachine( outcomes=['done'], output_keys=['waipoint', 'area_name']) with _sm_export_no_waypoint_4: # x:30 y:40 OperatableStateMachine.add('noWaypoint', SetKey(Value=None), transitions={'done': 'NoName'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'waipoint'}) # x:43 y:129 OperatableStateMachine.add('NoName', SetKey(Value=None), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'area_name'}) # x:30 y:458 _sm_export_waypoint_5 = OperatableStateMachine( outcomes=['done'], input_keys=['entity'], output_keys=['waipoint', 'area_name']) with _sm_export_waypoint_5: # x:58 y:107 OperatableStateMachine.add( 'Extract Wayppoint', CalculationState(calculation=lambda x: x.waypoint), transitions={'done': 'Extract Name'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'waipoint' }) # x:67 y:257 OperatableStateMachine.add( 'Extract Name', CalculationState(calculation=lambda x: x.name), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'area_name' }) # x:379 y:221 _sm_wait_to_compte_6 = OperatableStateMachine(outcomes=['finished']) with _sm_wait_to_compte_6: # x:77 y:195 OperatableStateMachine.add('one more wait', WaitState(wait_time=60), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:535 y:314 _sm_turn_around_7 = OperatableStateMachine(outcomes=['failed']) with _sm_turn_around_7: # x:47 y:45 OperatableStateMachine.add('set orientation', SetKey(Value=1.57), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:42 y:213 OperatableStateMachine.add('move head', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'waitwait'}, autonomy={'done': Autonomy.Off}) # x:53 y:294 OperatableStateMachine.add('waitwait', WaitState(wait_time=10), transitions={'done': 'action_turn_2'}, autonomy={'done': Autonomy.Off}) # x:33 y:122 OperatableStateMachine.add( 'action_turn', self.use_behavior( action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn' ), transitions={ 'finished': 'move head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:31 y:434 OperatableStateMachine.add( 'action_turn_2', self.use_behavior( action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn_2' ), transitions={ 'finished': 'head left right', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:35 y:595 OperatableStateMachine.add('head left right', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'wait turn head'}, autonomy={'done': Autonomy.Off}) # x:308 y:537 OperatableStateMachine.add('wait turn head', WaitState(wait_time=10), transitions={'done': 'head right left'}, autonomy={'done': Autonomy.Off}) # x:495 y:604 OperatableStateMachine.add('head right left', SaraSetHeadAngle(pitch=0, yaw=-1.57), transitions={'done': 'wait wait wait'}, autonomy={'done': Autonomy.Off}) # x:271 y:711 OperatableStateMachine.add('wait wait wait', WaitState(wait_time=10), transitions={'done': 'head left right'}, autonomy={'done': Autonomy.Off}) # x:845 y:395 _sm_find_human_8 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID']) with _sm_find_human_8: # x:223 y:137 OperatableStateMachine.add('find person', GetEntityByID(), transitions={ 'found': 'finished', 'not_found': 'find person' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:549 y:135, x:555 y:269, x:230 y:458 _sm_move_head_and_base_9 = ConcurrencyContainer( outcomes=['failed'], conditions=[('failed', [('turn around', 'failed')]), ('failed', [('wait to compte', 'finished')])]) with _sm_move_head_and_base_9: # x:268 y:77 OperatableStateMachine.add('turn around', _sm_turn_around_7, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:263 y:246 OperatableStateMachine.add('wait to compte', _sm_wait_to_compte_6, transitions={'finished': 'failed'}, autonomy={'finished': Autonomy.Inherit}) # x:493 y:206 _sm_container_10 = OperatableStateMachine(outcomes=['check']) with _sm_container_10: # x:230 y:160 OperatableStateMachine.add('wait long', WaitState(wait_time=40), transitions={'done': 'check'}, autonomy={'done': Autonomy.Off}) # x:30 y:458, x:706 y:447 _sm_navigate_to_the_point_11 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['pose']) with _sm_navigate_to_the_point_11: # x:174 y:122 OperatableStateMachine.add('set relative', SetKey(Value=False), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'relative'}) # x:347 y:191 OperatableStateMachine.add( 'Action_Move', self.use_behavior( Action_MoveSM, 'Try to reach/Container/navigate to the point/Action_Move' ), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'pose'}) # x:728 y:335, x:792 y:103, x:738 y:249, x:724 y:448 _sm_check_person_behind_12 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[('failed', [('move head and base', 'failed')]), ('finished', [('find human', 'finished')])]) with _sm_check_person_behind_12: # x:250 y:72 OperatableStateMachine.add('move head and base', _sm_move_head_and_base_9, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:261 y:238 OperatableStateMachine.add('find human', _sm_find_human_8, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:635 y:61, x:634 y:159, x:597 y:300, x:330 y:458, x:430 y:458, x:530 y:458 _sm_container_13 = ConcurrencyContainer( outcomes=['finished', 'failed', 'check'], input_keys=['waypoint'], conditions=[('check', [('Container', 'check')]), ('finished', [('navigate to the point', 'finished')]), ('failed', [('navigate to the point', 'failed')])]) with _sm_container_13: # x:315 y:51 OperatableStateMachine.add('navigate to the point', _sm_navigate_to_the_point_11, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'waypoint'}) # x:322 y:257 OperatableStateMachine.add('Container', _sm_container_10, transitions={'check': 'check'}, autonomy={'check': Autonomy.Inherit}) # x:98 y:451 _sm_get_area_containers_14 = OperatableStateMachine( outcomes=['done'], input_keys=['command'], output_keys=['containers']) with _sm_get_area_containers_14: # x:46 y:31 OperatableStateMachine.add( 'Set Empty Array', SetKey(Value=[]), transitions={'done': 'Set initial index'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'containers'}) # x:50 y:110 OperatableStateMachine.add('Set initial index', SetKey(Value=2), transitions={'done': 'Set Loop Max'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'iLoop'}) # x:49 y:192 OperatableStateMachine.add( 'Set Loop Max', CalculationState(calculation=lambda x: len(x)), transitions={'done': 'Check Loop End'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'command', 'output_value': 'iLoopMax' }) # x:514 y:278 OperatableStateMachine.add( 'Increment', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Print container array'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'iLoop', 'output_value': 'iLoop' }) # x:271 y:154 OperatableStateMachine.add( 'Add Container to Array', FlexibleCalculationState( calculation=lambda x: x[2] + [x[0][x[1]]], input_keys=['command', 'iLoop', 'containers']), transitions={'done': 'Increment'}, autonomy={'done': Autonomy.Off}, remapping={ 'command': 'command', 'iLoop': 'iLoop', 'containers': 'containers', 'output_value': 'containers' }) # x:42 y:287 OperatableStateMachine.add('Check Loop End', FlexibleCheckConditionState( predicate=lambda x: x[0] < x[1], input_keys=['iLoop', 'iLoopMax']), transitions={ 'true': 'Add Container to Array', 'false': 'done' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'iLoop': 'iLoop', 'iLoopMax': 'iLoopMax' }) # x:279 y:401 OperatableStateMachine.add('Print container array', LogKeyState( text="Containers: \n {}", severity=Logger.REPORT_HINT), transitions={'done': 'Check Loop End'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'containers'}) # x:626 y:228, x:607 y:71, x:230 y:458, x:330 y:458 _sm_operator_is_still_there_15 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[('finished', [('find a human', 'finished')]), ('failed', [('Move head and base end ', 'failed')])]) with _sm_operator_is_still_there_15: # x:207 y:54 OperatableStateMachine.add('Move head and base end ', _sm_move_head_and_base_end__3, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:222 y:190 OperatableStateMachine.add('find a human', _sm_find_a_human_2, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:1620 y:109, x:1648 y:375 _sm_try_to_find_area_16 = OperatableStateMachine( outcomes=['found', 'not_found'], input_keys=['area_to_search', 'containers'], output_keys=['area_name', 'waypoint']) with _sm_try_to_find_area_16: # x:517 y:67 OperatableStateMachine.add( 'WonderlandUniqueEnity', self.use_behavior(WonderlandUniqueEnitySM, 'Try to find area/WonderlandUniqueEnity'), transitions={ 'found': 'Export Waypoint', 'not_found': 'Export No Waypoint' }, autonomy={ 'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit }, remapping={ 'name': 'area_to_search', 'containers': 'containers', 'entity': 'entity' }) # x:966 y:71 OperatableStateMachine.add('Export Waypoint', _sm_export_waypoint_5, transitions={'done': 'say_going'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'entity': 'entity', 'waipoint': 'waypoint', 'area_name': 'area_name' }) # x:1247 y:295 OperatableStateMachine.add('Export No Waypoint', _sm_export_no_waypoint_4, transitions={'done': 'not_found'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'waipoint': 'waypoint', 'area_name': 'area_name' }) # x:1273 y:106 OperatableStateMachine.add( 'say_going', SaraSay(sentence=lambda x: "I'm going to the " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}) # x:323 y:632, x:638 y:631 _sm_try_to_reach_17 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['waypoint', 'relative', 'areaName', 'ID']) with _sm_try_to_reach_17: # x:252 y:161 OperatableStateMachine.add('Container', _sm_container_13, transitions={ 'finished': 'say_reached', 'failed': 'Say_not_reached', 'check': 'say check' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'check': Autonomy.Inherit }, remapping={'waypoint': 'waypoint'}) # x:581 y:163 OperatableStateMachine.add('check person behind', _sm_check_person_behind_12, transitions={ 'finished': 'say found', 'failed': 'say lost' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'ID': 'ID'}) # x:706 y:484 OperatableStateMachine.add( 'say lost', SaraSay(sentence="Oh no! I lost my operator!", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:445 y:69 OperatableStateMachine.add( 'say check', SaraSay(sentence="I check if my operator is still there", input_keys=[], emotion=1, block=True), transitions={'done': 'check person behind'}, autonomy={'done': Autonomy.Off}) # x:443 y:187 OperatableStateMachine.add( 'say found', SaraSay(sentence="Great. You are still there.", input_keys=[], emotion=1, block=True), transitions={'done': 'Container'}, autonomy={'done': Autonomy.Off}) # x:463 y:398 OperatableStateMachine.add( 'Say_not_reached', SaraSay(sentence=lambda x: "I have not reach the " + x + "!", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:61 y:366 OperatableStateMachine.add( 'say_reached', SaraSay(sentence=lambda x: "I have reach the " + x + "!", input_keys=[], emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:871 y:59 _sm_decompose_command_18 = OperatableStateMachine( outcomes=['done'], input_keys=['command'], output_keys=['containers', 'area']) with _sm_decompose_command_18: # x:163 y:34 OperatableStateMachine.add('Set State Command', Set_a_step(step=0), transitions={'done': 'Get area name'}, autonomy={'done': Autonomy.Off}) # x:387 y:58 OperatableStateMachine.add( 'Get area name', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Get area containers'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'command', 'output_value': 'area' }) # x:597 y:94 OperatableStateMachine.add('Get area containers', _sm_get_area_containers_14, transitions={'done': 'done'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'command': 'command', 'containers': 'containers' }) with _state_machine: # x:54 y:24 OperatableStateMachine.add( 'Get Person Id', GetRosParam(ParamName="behavior/FoundPerson/Id"), transitions={ 'done': 'GetPerson', 'failed': 'Cant Find Person' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'ID'}) # x:19 y:152 OperatableStateMachine.add( 'Decompose Command', _sm_decompose_command_18, transitions={'done': 'Try to find area'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'command': 'Action', 'containers': 'containers', 'area': 'area' }) # x:43 y:358 OperatableStateMachine.add('Try to reach', _sm_try_to_reach_17, transitions={ 'finished': 'operator is still there', 'failed': 'cause2' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'waypoint': 'waypoint', 'relative': 'relative', 'areaName': 'area_name', 'ID': 'ID' }) # x:360 y:109 OperatableStateMachine.add('Cant Find Person', SaraSay( sentence="I can't find a person.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:52 y:82 OperatableStateMachine.add('GetPerson', GetEntityByID(), transitions={ 'found': 'Decompose Command', 'not_found': 'Cant Find Person' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:36 y:219 OperatableStateMachine.add('Try to find area', _sm_try_to_find_area_16, transitions={ 'found': 'sayfollowme', 'not_found': 'Cant Find Person' }, autonomy={ 'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit }, remapping={ 'area_to_search': 'area', 'containers': 'containers', 'area_name': 'area_name', 'waypoint': 'waypoint' }) # x:27 y:438 OperatableStateMachine.add('operator is still there', _sm_operator_is_still_there_15, transitions={ 'finished': 'getentitybyID', 'failed': 'say lost operator' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'ID': 'ID'}) # x:347 y:370 OperatableStateMachine.add( 'say lost operator', SaraSay( sentence= "I have reach my goal but I lost the person I was guiding.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause3'}, autonomy={'done': Autonomy.Off}) # x:49 y:291 OperatableStateMachine.add('sayfollowme', SaraSay(sentence="Follow me please.", input_keys=[], emotion=1, block=True), transitions={'done': 'Try to reach'}, autonomy={'done': Autonomy.Off}) # x:808 y:451 OperatableStateMachine.add('head to middle', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:48 y:525 OperatableStateMachine.add('getentitybyID', GetEntityByID(), transitions={ 'found': 'get entity to point', 'not_found': 'say_reach_the_entity' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:249 y:562 OperatableStateMachine.add('get entity to point', WonderlandGetEntityVerbal(), transitions={ 'one': 'find the point', 'multiple': 'say_reach_the_entity', 'none': 'say_reach_the_entity', 'error': 'say_reach_the_entity' }, autonomy={ 'one': Autonomy.Off, 'multiple': Autonomy.Off, 'none': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'name': 'area_name', 'containers': 'containers', 'entities': 'entities', 'firstEntity': 'firstEntity' }) # x:708 y:529 OperatableStateMachine.add( 'Action_point_at', self.use_behavior(Action_point_atSM, 'Action_point_at'), transitions={ 'finished': 'say_reach_the_entity', 'failed': 'say_reach_the_entity' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'targetPoint': 'targetPoint'}) # x:539 y:558 OperatableStateMachine.add( 'find the point', CalculationState(calculation=lambda x: x.position), transitions={'done': 'Action_point_at'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entities', 'output_value': 'targetPoint' }) # x:547 y:121 OperatableStateMachine.add( 'cause1', SetKey(Value="I didn't find any persone"), transitions={'done': 'setrosparamfail'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:507 y:220 OperatableStateMachine.add( 'cause2', SetKey(Value="I did not reach the area"), transitions={'done': 'setrosparamfail'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:559 y:337 OperatableStateMachine.add( 'cause3', SetKey(Value="I lost the person I was guiding."), transitions={'done': 'setrosparamfail'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:656 y:250 OperatableStateMachine.add( 'setrosparamfail', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:523 y:462 OperatableStateMachine.add( 'say_reach_the_entity', SaraSay(sentence=lambda x: "Here is the " + x, input_keys=[], emotion=1, block=True), transitions={'done': 'head to middle'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:702 y:576, x:764 y:158, x:766 y:33 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Place", "table"] _state_machine.userdata.Empty = None _state_machine.userdata.IdlePos = "IdlePose" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:44 y:28 OperatableStateMachine.add( 'gripper contain', GetRosParam(ParamName="behavior/GripperContent"), transitions={ 'done': 'if contain something', 'failed': 'cause1' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'content'}) # x:222 y:497 OperatableStateMachine.add('Action_place', self.use_behavior( Action_placeSM, 'Action_place'), transitions={ 'finished': 'idlearm', 'failed': 'cause3' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pos': 'MapPosition'}) # x:39 y:367 OperatableStateMachine.add( 'genPoseArm', GenPoseEuler(x=0.75, y=-0.25, z=0.85, roll=0, pitch=0, yaw=0), transitions={'done': 'referential from robot to map'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'position'}) # x:8 y:433 OperatableStateMachine.add('referential from robot to map', TF_transformation(in_ref="base_link", out_ref="map"), transitions={ 'done': 'log pose', 'fail': 'log tf error' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'in_pos': 'position', 'out_pos': 'MapPosition' }) # x:25 y:98 OperatableStateMachine.add( 'if contain something', CheckConditionState(predicate=lambda x: x != ''), transitions={ 'true': 'cond', 'false': 'say nothing in gripper' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'content'}) # x:209 y:98 OperatableStateMachine.add( 'say nothing in gripper', SaraSay(sentence="It seems I have nothing in my gripper", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:28 y:236 OperatableStateMachine.add( 'construction phrase', FlexibleCalculationState( calculation=lambda x: "I will place this " + str(x[ 0]) + " on the " + str(x[1][1]), input_keys=["content", "Action"]), transitions={'done': 'Say_Place_object'}, autonomy={'done': Autonomy.Off}, remapping={ 'content': 'content', 'Action': 'Action', 'output_value': 'sentence' }) # x:33 y:167 OperatableStateMachine.add( 'cond', CheckConditionState(predicate=lambda x: x[1] != ''), transitions={ 'true': 'construction phrase', 'false': 'cause2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'Action'}) # x:257 y:413 OperatableStateMachine.add('log tf error', LogState(text="tf error", severity=Logger.REPORT_HINT), transitions={'done': 'cause3'}, autonomy={'done': Autonomy.Off}) # x:42 y:502 OperatableStateMachine.add( 'log pose', LogKeyState(text="the placement pose will be: {}", severity=Logger.REPORT_HINT), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'MapPosition'}) # x:493 y:535 OperatableStateMachine.add( 'empty hand', SetRosParam(ParamName="behavior/GripperContent"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Empty'}) # x:448 y:54 OperatableStateMachine.add( 'cause1', SetKey(Value="I didn't have any object in my gripper"), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:422 y:149 OperatableStateMachine.add( 'cause2', SetKey(Value="I didn't know where to place the object."), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:575 y:158 OperatableStateMachine.add( 'setrosparamcause', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:449 y:325 OperatableStateMachine.add( 'cause3', SetKey( Value="I was unable to calculate how to place the object." ), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:342 y:583 OperatableStateMachine.add('idlearm', MoveitMove(move=True, waitForExecution=False, group="RightArm", watchdog=15), transitions={ 'done': 'empty hand', 'failed': 'empty hand' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'IdlePos'}) # x:706 y:460 OperatableStateMachine.add( 'Say_Place_It_This_Place', SaraSay(sentence=lambda x: "I will place this " + x + " right there.", input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:35 y:301 OperatableStateMachine.add('Say_Place_object', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'genPoseArm'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:976 y:173, x:618 y:48 _state_machine = OperatableStateMachine(outcomes=['done', 'failed']) _state_machine.userdata.nameFilter = "" _state_machine.userdata.roomQuestion = "what room shall i clean, master ?" _state_machine.userdata.waypointGenerationDistance = 0.5 _state_machine.userdata.placeholder = "" _state_machine.userdata.doorName = "crowd" _state_machine.userdata.firstTimeInRoom = "yes" _state_machine.userdata.waypointToCheckDict = {"bedroom": ["bedroomWP1","bedroomWP2"]} _state_machine.userdata.placedObjects = 0 _state_machine.userdata.misplacedObject = "" _state_machine.userdata.skipDoorEntrance = True # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:153 y:437 _sm_deusexplacerecovery_0 = OperatableStateMachine(outcomes=['finished'], input_keys=['misplacedObject']) with _sm_deusexplacerecovery_0: # x:85 y:72 OperatableStateMachine.add('getContainerId', CalculationState(calculation=lambda x: x[1].containerId), transitions={'done': 'getContainerEntity'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'misplacedObject', 'output_value': 'containerId'}) # x:234 y:149 OperatableStateMachine.add('getContainerEntity', WonderlandGetEntityByID(), transitions={'found': 'ItGoesThere', 'not_found': 'finished', 'error': 'finished'}, autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off, 'error': Autonomy.Off}, remapping={'id': 'containerId', 'entity': 'containerEntity', 'depth_position': 'depth_position', 'depth_waypoint': 'depth_waypoint'}) # x:366 y:284 OperatableStateMachine.add('ItGoesThere', SaraSay(sentence=lambda x: "Looks like I dropped "+str(x[0][0].name)+". Can you put it in the "+ x[1].name+"?", input_keys=["misplacedObject","containerEntity"], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'misplacedObject': 'misplacedObject', 'containerEntity': 'containerEntity'}) # x:717 y:285 _sm_cantgotodestination_1 = OperatableStateMachine(outcomes=['finished'], input_keys=['misplacedObject']) with _sm_cantgotodestination_1: # x:48 y:60 OperatableStateMachine.add('getContainerId', CalculationState(calculation=lambda x: x[1].containerId), transitions={'done': 'getContainerEntity'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'misplacedObject', 'output_value': 'containerId'}) # x:463 y:245 OperatableStateMachine.add('Action_place', self.use_behavior(sara_flexbe_behaviors__Action_placeSM, 'PutObjectInDesiredContainer/CantGoToDestination/Action_place'), transitions={'finished': 'finished', 'failed': 'finished'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pos': 'droppingPose'}) # x:26 y:149 OperatableStateMachine.add('getContainerEntity', WonderlandGetEntityByID(), transitions={'found': 'ICantGetThere', 'not_found': 'ICantGetThereNCONT', 'error': 'ICantGetThereNCONT'}, autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off, 'error': Autonomy.Off}, remapping={'id': 'containerId', 'entity': 'containerEntity', 'depth_position': 'depth_position', 'depth_waypoint': 'depth_waypoint'}) # x:249 y:176 OperatableStateMachine.add('ICantGetThere', SaraSay(sentence=lambda x:"I can't find a way to "+str(x[0].name), input_keys=["containerEntity"], emotion=0, block=True), transitions={'done': 'GetDroppingPose'}, autonomy={'done': Autonomy.Off}, remapping={'containerEntity': 'containerEntity'}) # x:247 y:114 OperatableStateMachine.add('ICantGetThereNCONT', SaraSay(sentence="I can't find a way to the container", input_keys=[], emotion=0, block=True), transitions={'done': 'GetDroppingPose'}, autonomy={'done': Autonomy.Off}) # x:475 y:148 OperatableStateMachine.add('GetDroppingPose', GenPoseEuler(x=0.4, y=0, z=0.3, roll=0.0, pitch=0.0, yaw=0.0), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'droppingPose'}) # x:632 y:527 _sm_putdownobject_2 = OperatableStateMachine(outcomes=['done'], input_keys=['misplacedObject']) with _sm_putdownobject_2: # x:70 y:215 OperatableStateMachine.add('GetDroppingPose', GenPoseEuler(x=0.3, y=0, z=1.2, roll=0.0, pitch=0.0, yaw=0.0), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'droppingPose'}) # x:269 y:215 OperatableStateMachine.add('Action_place', self.use_behavior(sara_flexbe_behaviors__Action_placeSM, 'PutObjectInDesiredContainer/PutDownObject/Action_place'), transitions={'finished': 'done', 'failed': 'DeusExPlaceRecovery'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pos': 'droppingPose'}) # x:592 y:127 OperatableStateMachine.add('DeusExPlaceRecovery', _sm_deusexplacerecovery_0, transitions={'finished': 'done'}, autonomy={'finished': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:518 y:44, x:531 y:152 _sm_gotodesiredcontainer_3 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['misplacedObject']) with _sm_gotodesiredcontainer_3: # x:86 y:79 OperatableStateMachine.add('GetContainerWaypoint', CalculationState(calculation=lambda x:x[1].waypoint), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'misplacedObject', 'output_value': 'targetWaypoint'}) # x:275 y:79 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'PutObjectInDesiredContainer/GotoDesiredContainer/Action_Move'), transitions={'finished': 'done', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'targetWaypoint'}) # x:300 y:141, x:893 y:63 _sm_receiveitem_4 = OperatableStateMachine(outcomes=['failed', 'success']) with _sm_receiveitem_4: # x:69 y:47 OperatableStateMachine.add('opengripper', SetGripperState(width=0.25, effort=1), transitions={'object': 'setTarget1', 'no_object': 'setTarget1'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:280 y:263 OperatableStateMachine.add('Torque_Reader', ReadTorque(watchdog=10, Joint="right_elbow_pitch_joint", Threshold=1, min_time=1), transitions={'threshold': 'close_gripper', 'watchdog': 'Torque_Reader', 'fail': 'failed'}, autonomy={'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'torque': 'torque'}) # x:668 y:149 OperatableStateMachine.add('setTarget2', SetKey(Value="PostGripPose"), transitions={'done': 'Go_to_Pose'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:51 y:261 OperatableStateMachine.add('Go_to_receive_bag_pose', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={'done': 'Torque_Reader', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'target'}) # x:450 y:274 OperatableStateMachine.add('close_gripper', SetGripperState(width=0, effort=1), transitions={'object': 'thank you', 'no_object': 'failed'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:640 y:273 OperatableStateMachine.add('thank you', SaraSay(sentence="Thank you", input_keys=[], emotion=1, block=True), transitions={'done': 'setTarget2'}, autonomy={'done': Autonomy.Off}) # x:50 y:141 OperatableStateMachine.add('setTarget1', SetKey(Value="Help_me_carry"), transitions={'done': 'Go_to_receive_bag_pose'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:660 y:57 OperatableStateMachine.add('Go_to_Pose', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={'done': 'success', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'target'}) # x:169 y:514, x:508 y:495 _sm_deusexpickrecovery_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['misplacedObject']) with _sm_deusexpickrecovery_5: # x:81 y:50 OperatableStateMachine.add('GimmeTheObjict', SaraSay(sentence=lambda x: "Could you give me the "+x[0][0].name+", please ?", input_keys=["misplacedObject"], emotion=0, block=True), transitions={'done': 'ReceiveItem'}, autonomy={'done': Autonomy.Off}, remapping={'misplacedObject': 'misplacedObject'}) # x:213 y:130 OperatableStateMachine.add('ReceiveItem', _sm_receiveitem_4, transitions={'failed': 'Oh f**k', 'success': 'finished'}, autonomy={'failed': Autonomy.Inherit, 'success': Autonomy.Inherit}) # x:355 y:262 OperatableStateMachine.add('Oh f**k', SaraSay(sentence="Oh. Nevermind then.", input_keys=[], emotion=2, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:676 y:610, x:1158 y:270 _sm_grabmisplacedobject_6 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['misplacedObject']) with _sm_grabmisplacedobject_6: # x:72 y:35 OperatableStateMachine.add('GetObjectId', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'Action_pick'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'misplacedObject', 'output_value': 'misplacedObjectID'}) # x:307 y:89 OperatableStateMachine.add('oops', SaraSay(sentence="Oops. I dropped the object!", input_keys=[], emotion=5, block=True), transitions={'done': 'DeusExPickRecovery'}, autonomy={'done': Autonomy.Off}) # x:344 y:157 OperatableStateMachine.add('oops_2', SaraSay(sentence="Oops. Seems I can't reach the object!", input_keys=[], emotion=3, block=True), transitions={'done': 'DeusExPickRecovery'}, autonomy={'done': Autonomy.Off}) # x:362 y:239 OperatableStateMachine.add('oops_3', SaraSay(sentence="Oops. I can't see the object!", input_keys=[], emotion=3, block=True), transitions={'done': 'DeusExPickRecovery'}, autonomy={'done': Autonomy.Off}) # x:42 y:275 OperatableStateMachine.add('Action_pick', self.use_behavior(sara_flexbe_behaviors__Action_pickSM, 'PickMisplacedObject/GrabMisplacedObject/Action_pick', default_keys=['Entity']), transitions={'success': 'done', 'unreachable': 'oops_2', 'not found': 'oops_3', 'dropped': 'oops'}, autonomy={'success': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'not found': Autonomy.Inherit, 'dropped': Autonomy.Inherit}, remapping={'objectID': 'misplacedObjectID', 'Entity': 'Entity'}) # x:600 y:177 OperatableStateMachine.add('DeusExPickRecovery', _sm_deusexpickrecovery_5, transitions={'finished': 'done', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:30 y:426 _sm_scanaround_7 = OperatableStateMachine(outcomes=['done']) with _sm_scanaround_7: # x:172 y:46 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:304 y:122 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:165 y:187 OperatableStateMachine.add('center2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:195 y:276 OperatableStateMachine.add('w1', WaitState(wait_time=1), transitions={'done': 'center3'}, autonomy={'done': Autonomy.Off}) # x:309 y:49 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:308 y:187 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:56 y:191 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:40 y:270 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:317 y:277 OperatableStateMachine.add('center3', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:289 y:803, x:694 y:198, x:4 y:400 _sm_checkatwaypoints_8 = OperatableStateMachine(outcomes=['found', 'noneFound', 'failed'], input_keys=['waypointToCheckDict', 'cleaningRoom', 'nameFilter'], output_keys=['misplacedObject']) with _sm_checkatwaypoints_8: # x:60 y:44 OperatableStateMachine.add('GetNbOfWaypoints', FlexibleCalculationState(calculation=lambda x: len(x[1][x[0]]), input_keys=["cleaningRoom","waypointToCheckDict"]), transitions={'done': 'StartLoop'}, autonomy={'done': Autonomy.Off}, remapping={'cleaningRoom': 'cleaningRoom', 'waypointToCheckDict': 'waypointToCheckDict', 'output_value': 'lenWaypointDict'}) # x:137 y:471 OperatableStateMachine.add('ScanAround', _sm_scanaround_7, transitions={'done': 'StopCheckingForUnknownObj'}, autonomy={'done': Autonomy.Inherit}) # x:163 y:729 OperatableStateMachine.add('GetFirstElement', CalculationState(calculation=lambda x:x[0]), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity_list', 'output_value': 'misplacedObject'}) # x:354 y:538 OperatableStateMachine.add('ListEntities', list_entities_by_name(frontality_level=0.5, distance_max=10), transitions={'found': 'CheckMisplacedObjects', 'none_found': 'CheckAtEachLocation'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'nameFilter', 'entity_list': 'entity_list', 'number': 'number'}) # x:23 y:198 OperatableStateMachine.add('EverythingsFine', SaraSay(sentence="I'll check at another location.", input_keys=[], emotion=1, block=True), transitions={'done': 'GetLocation'}, autonomy={'done': Autonomy.Off}) # x:184 y:289 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'CheckForMisplacedObjects/CheckAtWaypoints/Action_Move'), transitions={'finished': 'CheckForUnknownObjs', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'waypointToGo'}) # x:511 y:338 OperatableStateMachine.add('CheckAtEachLocation', ForLoopWithInput(repeat=10), transitions={'do': 'CheckIfLastLocationOfList', 'end': 'noneFound'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index_in': 'index_in', 'index_out': 'locationToGet'}) # x:318 y:58 OperatableStateMachine.add('StartLoop', SetKey(Value=-1), transitions={'done': 'CheckAtEachLocation'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'index_in'}) # x:9 y:262 OperatableStateMachine.add('GetLocation', FlexibleCalculationState(calculation=lambda x: x[1][x[2]][x[0]], input_keys=["index_out","waypointToCheckDict","cleaningRoom"]), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'index_out': 'locationToGet', 'waypointToCheckDict': 'waypointToCheckDict', 'cleaningRoom': 'cleaningRoom', 'output_value': 'waypointToGo'}) # x:173 y:195 OperatableStateMachine.add('CheckIfLastLocationOfList', FlexibleCheckConditionState(predicate=lambda x: x[0]-1 >= x[1], input_keys=["lenWaypointDict","locationToGet"]), transitions={'true': 'noneFound', 'false': 'EverythingsFine'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'lenWaypointDict': 'lenWaypointDict', 'locationToGet': 'locationToGet'}) # x:418 y:665 OperatableStateMachine.add('CheckMisplacedObjects', CheckMisplacedObjects(position_tolerance=0.1, default_destination="bin"), transitions={'all_expected': 'CheckAtEachLocation', 'unexpected': 'GetFirstElement'}, autonomy={'all_expected': Autonomy.Off, 'unexpected': Autonomy.Off}, remapping={'entities': 'entity_list', 'expected_objects': 'expected_objects', 'unexpected_objects': 'unexpected_objects'}) # x:119 y:402 OperatableStateMachine.add('CheckForUnknownObjs', SetSegmentationRosParam(ValueTableSegmentation=True, ValueObjectSegmentation=True), transitions={'done': 'ScanAround'}, autonomy={'done': Autonomy.Off}) # x:114 y:561 OperatableStateMachine.add('StopCheckingForUnknownObj', SetSegmentationRosParam(ValueTableSegmentation=False, ValueObjectSegmentation=False), transitions={'done': 'ListEntities'}, autonomy={'done': Autonomy.Off}) # x:547 y:290 _sm_scanaround_9 = OperatableStateMachine(outcomes=['done'], input_keys=['input_value']) with _sm_scanaround_9: # x:172 y:46 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:304 y:122 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:165 y:187 OperatableStateMachine.add('center2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:195 y:276 OperatableStateMachine.add('w1', WaitState(wait_time=1), transitions={'done': 'center3'}, autonomy={'done': Autonomy.Off}) # x:367 y:47 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:308 y:187 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:56 y:191 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:40 y:270 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:317 y:277 OperatableStateMachine.add('center3', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:293 y:350, x:857 y:186 _sm_ask_10 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['roomQuestion'], output_keys=['roomAnswer']) with _sm_ask_10: # x:79 y:95 OperatableStateMachine.add('say question', SaraSay(sentence=lambda x: x[0], input_keys=["question"], emotion=0, block=True), transitions={'done': 'get answer'}, autonomy={'done': Autonomy.Off}, remapping={'question': 'roomQuestion'}) # x:92 y:304 OperatableStateMachine.add('get answer', GetSpeech(watchdog=10), transitions={'done': 'finished', 'nothing': 'retry ask', 'fail': 'retry ask'}, autonomy={'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'words': 'roomAnswer'}) # x:345 y:202 OperatableStateMachine.add('retry ask', ForLoop(repeat=2), transitions={'do': 'say not understand', 'end': 'say failed'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:232 y:90 OperatableStateMachine.add('say not understand', SaraSay(sentence="Sorry, I did not understand your answer.", input_keys=[], emotion=0, block=True), transitions={'done': 'say question'}, autonomy={'done': Autonomy.Off}) # x:604 y:151 OperatableStateMachine.add('say failed', SaraSay(sentence="Sorry, I can't understand your answer.", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:30 y:365, x:424 y:392 _sm_nlu_11 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['roomAnswer'], output_keys=['cleanupRoom']) with _sm_nlu_11: # x:212 y:89 OperatableStateMachine.add('NLU', SaraNLUgetRoom(), transitions={'understood': 'done', 'not_understood': 'failed', 'fail': 'failed'}, autonomy={'understood': Autonomy.Off, 'not_understood': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'sentence': 'roomAnswer', 'answer': 'cleanupRoom'}) # x:736 y:312 _sm_putobjectindesiredcontainer_12 = OperatableStateMachine(outcomes=['finished'], input_keys=['misplacedObject', 'placedObjects'], output_keys=['placedObjects']) with _sm_putobjectindesiredcontainer_12: # x:138 y:153 OperatableStateMachine.add('GotoDesiredContainer', _sm_gotodesiredcontainer_3, transitions={'done': 'PutDownObject', 'failed': 'CantGoToDestination'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:277 y:313 OperatableStateMachine.add('PutDownObject', _sm_putdownobject_2, transitions={'done': 'OneMoreObjectPlaced'}, autonomy={'done': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:339 y:80 OperatableStateMachine.add('CantGoToDestination', _sm_cantgotodestination_1, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:468 y:381 OperatableStateMachine.add('OneMoreObjectPlaced', CalculationState(calculation=lambda x: x+1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'placedObjects', 'output_value': 'placedObjects'}) # x:609 y:528, x:593 y:240 _sm_pickmisplacedobject_13 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['misplacedObject', 'waypointGenerationDistance']) with _sm_pickmisplacedobject_13: # x:82 y:71 OperatableStateMachine.add('GetObjectPosition', CalculationState(calculation=lambda x: x[0].position), transitions={'done': 'FindAWay'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'misplacedObject', 'output_value': 'misplacedObjectPosition'}) # x:67 y:304 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'PickMisplacedObject/Action_Move'), transitions={'finished': 'GrabMisplacedObject', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'misplacedObjectWaypoint'}) # x:82 y:202 OperatableStateMachine.add('FindAWay', Get_Reacheable_Waypoint(), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'misplacedObjectPosition', 'distance': 'waypointGenerationDistance', 'pose_out': 'misplacedObjectWaypoint'}) # x:58 y:514 OperatableStateMachine.add('GrabMisplacedObject', _sm_grabmisplacedobject_6, transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:957 y:154, x:535 y:369, x:800 y:304 _sm_checkformisplacedobjects_14 = OperatableStateMachine(outcomes=['noneLeft', 'found', 'failed'], input_keys=['nameFilter', 'waypointToCheckDict', 'cleaningRoom', 'placedObjects'], output_keys=['misplacedObject']) with _sm_checkformisplacedobjects_14: # x:241 y:59 OperatableStateMachine.add('CheckIf5Placed', CheckConditionState(predicate=lambda x: x >= 5), transitions={'true': 'OkItsClean', 'false': 'RetryOnce'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'placedObjects'}) # x:115 y:477 OperatableStateMachine.add('CheckMisplacedObjects', CheckMisplacedObjects(position_tolerance=0.1, default_destination="bin"), transitions={'all_expected': 'EverythingsFine', 'unexpected': 'GetFirstElement'}, autonomy={'all_expected': Autonomy.Off, 'unexpected': Autonomy.Off}, remapping={'entities': 'entity_list', 'expected_objects': 'expected_objects', 'unexpected_objects': 'unexpected_objects'}) # x:76 y:251 OperatableStateMachine.add('ScanAround', _sm_scanaround_9, transitions={'done': 'ListEntities'}, autonomy={'done': Autonomy.Inherit}, remapping={'input_value': 'nameFilter'}) # x:300 y:486 OperatableStateMachine.add('GetFirstElement', CalculationState(calculation=lambda x:x[0]), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity_list', 'output_value': 'misplacedObject'}) # x:77 y:360 OperatableStateMachine.add('ListEntities', list_entities_by_name(frontality_level=0.5, distance_max=10), transitions={'found': 'CheckMisplacedObjects', 'none_found': 'RetryOnce'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'nameFilter', 'entity_list': 'entity_list', 'number': 'number'}) # x:305 y:355 OperatableStateMachine.add('EverythingsFine', SaraSay(sentence="Everything seems in order. I'll check again.", input_keys=[], emotion=1, block=True), transitions={'done': 'RetryOnce'}, autonomy={'done': Autonomy.Off}) # x:738 y:175 OperatableStateMachine.add('OkItsClean', SaraSay(sentence="I cleaned the room!", input_keys=[], emotion=1, block=True), transitions={'done': 'noneLeft'}, autonomy={'done': Autonomy.Off}) # x:488 y:226 OperatableStateMachine.add('CheckAtWaypoints', _sm_checkatwaypoints_8, transitions={'found': 'found', 'noneFound': 'OkItsClean', 'failed': 'failed'}, autonomy={'found': Autonomy.Inherit, 'noneFound': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'waypointToCheckDict': 'waypointToCheckDict', 'cleaningRoom': 'cleaningRoom', 'nameFilter': 'nameFilter', 'misplacedObject': 'misplacedObject'}) # x:268 y:226 OperatableStateMachine.add('RetryOnce', ForLoop(repeat=1), transitions={'do': 'ScanAround', 'end': 'CheckAtWaypoints'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:644 y:459, x:651 y:336 _sm_gotoroom_15 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['cleanupRoom', 'firstTimeInRoom'], output_keys=['firstTimeInRoom']) with _sm_gotoroom_15: # x:141 y:121 OperatableStateMachine.add('IsItMyFirstTime', DecisionState(outcomes=["no", "yes"], conditions=lambda x: x), transitions={'no': 'GoingBackToTheRoom', 'yes': 'OkImGoingToTheRoom'}, autonomy={'no': Autonomy.Off, 'yes': Autonomy.Off}, remapping={'input_value': 'firstTimeInRoom'}) # x:130 y:220 OperatableStateMachine.add('GoingBackToTheRoom', SaraSay(sentence=lambda x: "I'm going back to the "+str(x[0])+".", input_keys=["cleanupRoom"], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'cleanupRoom': 'cleanupRoom'}) # x:342 y:171 OperatableStateMachine.add('OkImGoingToTheRoom', SaraSay(sentence=lambda x: "Okay, I'm going to the "+str(x[0])+".", input_keys=["cleanupRoom"], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'cleanupRoom': 'cleanupRoom'}) # x:330 y:448 OperatableStateMachine.add('IWentThere', SetKey(Value="no"), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'firstTimeInRoom'}) # x:320 y:326 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'GoToRoom/Action_Move'), transitions={'finished': 'IWentThere', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'cleanupRoom'}) # x:709 y:520, x:850 y:66 _sm_enterarena_16 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['roomQuestion', 'doorName', 'skipDoorEntrance'], output_keys=['cleanupRoom']) with _sm_enterarena_16: # x:77 y:28 OperatableStateMachine.add('Init_Sequence', self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'EnterArena/Init_Sequence'), transitions={'finished': 'SkipEntrance', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:278 y:473 OperatableStateMachine.add('NLU', _sm_nlu_11, transitions={'done': 'done', 'failed': 'Sorry'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'roomAnswer': 'roomAnswer', 'cleanupRoom': 'cleanupRoom'}) # x:467 y:87 OperatableStateMachine.add('Action_Pass_Door', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'EnterArena/Action_Pass_Door'), transitions={'Done': 'RetryOnce', 'Fail': 'failed'}, autonomy={'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit}, remapping={'DoorName': 'doorName'}) # x:234 y:259 OperatableStateMachine.add('RetryOnce', ForLoop(repeat=2), transitions={'do': 'Ask', 'end': 'failed'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:295 y:354 OperatableStateMachine.add('Sorry', SaraSay(sentence="Sorry, I misunderstood.", input_keys=[], emotion=0, block=True), transitions={'done': 'RetryOnce'}, autonomy={'done': Autonomy.Off}) # x:213 y:119 OperatableStateMachine.add('SkipEntrance', CheckConditionState(predicate=lambda x: x), transitions={'true': 'RetryOnce', 'false': 'Action_Pass_Door'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'skipDoorEntrance'}) # x:51 y:354 OperatableStateMachine.add('Ask', _sm_ask_10, transitions={'finished': 'NLU', 'failed': 'RetryOnce'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'roomQuestion': 'roomQuestion', 'roomAnswer': 'roomAnswer'}) with _state_machine: # x:131 y:130 OperatableStateMachine.add('EnterArena', _sm_enterarena_16, transitions={'done': 'GoToRoom', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'roomQuestion': 'roomQuestion', 'doorName': 'doorName', 'skipDoorEntrance': 'skipDoorEntrance', 'cleanupRoom': 'cleanupRoom'}) # x:286 y:217 OperatableStateMachine.add('GoToRoom', _sm_gotoroom_15, transitions={'done': 'CheckForMisplacedObjects', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'cleanupRoom': 'cleanupRoom', 'firstTimeInRoom': 'firstTimeInRoom'}) # x:652 y:203 OperatableStateMachine.add('CheckForMisplacedObjects', _sm_checkformisplacedobjects_14, transitions={'noneLeft': 'done', 'found': 'PickMisplacedObject', 'failed': 'failed'}, autonomy={'noneLeft': Autonomy.Inherit, 'found': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'nameFilter': 'nameFilter', 'waypointToCheckDict': 'waypointToCheckDict', 'cleaningRoom': 'cleanupRoom', 'placedObjects': 'placedObjects', 'misplacedObject': 'misplacedObject'}) # x:557 y:403 OperatableStateMachine.add('PickMisplacedObject', _sm_pickmisplacedobject_13, transitions={'finished': 'PutObjectInDesiredContainer', 'failed': 'GoToRoom'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject', 'waypointGenerationDistance': 'waypointGenerationDistance'}) # x:234 y:476 OperatableStateMachine.add('PutObjectInDesiredContainer', _sm_putobjectindesiredcontainer_12, transitions={'finished': 'GoToRoom'}, autonomy={'finished': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject', 'placedObjects': 'placedObjects'}) return _state_machine
def create(self): joint_names_left = [ "l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx" ] joint_names_right = [ "r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx" ] wait_time = 10.0 bagfolder = "" # calculated gains_list = { 'pid_gains': ['p', 'i', 'd'], 'bdi_gains': ['k_qd_p', 'ff_qd_d'], 'vigir_gains': ['ff_bang', 'ff_effort', 'ff_friction'] } amplitudes = [2, 5, 10] # set i to 1 frequencies = [1.26, 3.14, 6.28] _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.joints_left_up = [] # calculated _state_machine.userdata.joints_right_up = [] # calculated _state_machine.userdata.joint_index = 0 _state_machine.userdata.none = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # 'Basic' configuration for SIMULATION #_state_machine.userdata.joints_left_up = [0.00, 0.18, 1.57, 1.18, 0.00, 0.57] #_state_machine.userdata.joints_right_up = [0.00, -0.18, 1.57, -1.18, 0.00, -0.57] logs_folder = os.path.expanduser('~/joint_control_tests/') if not os.path.exists(logs_folder): os.makedirs(logs_folder) bagfolder = logs_folder + "run_" + time.strftime( "%Y-%m-%d-%H_%M") + "/" os.makedirs(bagfolder) self._joint_limits = self._joint_limits_rob if self.real_robot else self._joint_limits_sim # standard config joints_left_up = [0] * 6 for i in range(6): joint_range = self._joint_limits[i][1] - self._joint_limits[i][0] joints_left_up[5 - i] = self._joint_limits[i][0] + joint_range * 0.5 joints_right_up = [0] * 6 for i in range(6): joint_range = self._joint_limits[i + 6][1] - self._joint_limits[i + 6][0] joints_right_up[5 - i] = self._joint_limits[i + 6][0] + joint_range * 0.5 _state_machine.userdata.joints_left_up = joints_left_up _state_machine.userdata.joints_right_up = joints_right_up rospy.loginfo('Average left joint positions: ' + ' '.join(map(str, joints_left_up))) rospy.loginfo('Average right joint positions: ' + ' '.join(map(str, joints_right_up))) self._amplitudes = amplitudes self._frequencies = frequencies # [/MANUAL_CREATE] _sm_perform_sinusoide_motion = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'altered_gains', 'joint_index', 'traj_controller', 'nominal_gains' ]) with _sm_perform_sinusoide_motion: # x:64 y:39 OperatableStateMachine.add( 'Set_Logfile_Name', FlexibleCalculationState( calculation=lambda i: bagfolder + self._traj_controllers[i[ 1]][1] + "_amp_" + str(i[0][0]) + "_freq_" + str(i[0][ 2]) + ".bag", input_keys=["gain_percentage", "joint_index"]), transitions={'done': 'Start_Gain_Logs'}, autonomy={'done': Autonomy.Off}, remapping={ 'gain_percentage': 'altered_gains', 'joint_index': 'joint_index', 'output_value': 'bagfile_name' }) # x:332 y:61 OperatableStateMachine.add( 'Start_Gain_Logs', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_For_Logging'}, autonomy={'logging': Autonomy.Off}, remapping={ 'bagfile_name': 'bagfile_name', 'rosbag_process': 'rosbag_process' }) # x:154 y:397 OperatableStateMachine.add( 'Stop_Gain_Logs', StopRecordLogsState(), transitions={'stopped': 'finished'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:344 y:344 OperatableStateMachine.add( 'Reset_Joint_Gains', UpdateDynamicParameterState(param=gains_list), transitions={ 'updated': 'Stop_Gain_Logs', 'failed': 'failed' }, autonomy={ 'updated': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'traj_controller': 'traj_controller', 'parameter_value': 'nominal_gains' }) # x:564 y:89 OperatableStateMachine.add('Wait_For_Logging', WaitState(wait_time=2), transitions={'done': 'Set_Joint_Gain'}, autonomy={'done': Autonomy.Off}) # x:598 y:361 OperatableStateMachine.add( 'Wait_For_Motion', WaitState(wait_time=wait_time), transitions={'done': 'Reset_Joint_Gains'}, autonomy={'done': Autonomy.Off}) # x:562 y:234 OperatableStateMachine.add( 'Set_Joint_Gain', UpdateDynamicParameterState(param=gains_list), transitions={ 'updated': 'Wait_For_Motion', 'failed': 'failed' }, autonomy={ 'updated': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'traj_controller': 'traj_controller', 'parameter_value': 'altered_gains' }) _sm_move_joints_to_standard = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['joints_right_up', 'joints_left_up']) with _sm_move_joints_to_standard: # x:71 y:145 OperatableStateMachine.add( 'Move_Left_Arm_Back', MoveitMoveGroupState(planning_group="l_arm_group", joint_names=joint_names_left), transitions={ 'reached': 'Move_Right_Arm_Back', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Low, 'failed': Autonomy.High }, remapping={'target_joint_config': 'joints_left_up'}) # x:317 y:132 OperatableStateMachine.add( 'Move_Right_Arm_Back', MoveitMoveGroupState(planning_group="r_arm_group", joint_names=joint_names_right), transitions={ 'reached': 'finished', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Low, 'failed': Autonomy.High }, remapping={'target_joint_config': 'joints_right_up'}) _sm_test_individual_joint = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'joint_index', 'traj_controller', 'none', 'joints_right_up', 'joints_left_up' ]) with _sm_test_individual_joint: # x:45 y:60 OperatableStateMachine.add( 'Initialize_Iteration', CalculationState(calculation=lambda x: 0), transitions={'done': 'Move_Joints_To_Standard'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'none', 'output_value': 'iteration' }) # x:176 y:388 OperatableStateMachine.add( 'Decide_If_Tests_To_Go', DecisionState( outcomes=["done", "continue"], conditions=lambda it: "done" if it == (len(amplitudes) * len(frequencies)) else "continue"), transitions={ 'done': 'finished', 'continue': 'Calculate_Next_Gain_Value' }, autonomy={ 'done': Autonomy.Off, 'continue': Autonomy.Off }, remapping={'input_value': 'iteration'}) # x:144 y:298 OperatableStateMachine.add( 'Calculate_Next_Gain_Value', FlexibleCalculationState( calculation=self.calculate_gains, input_keys=["iteration", "nominal_gain"]), transitions={'done': 'Perform_Sinusoide_Motion'}, autonomy={'done': Autonomy.High}, remapping={ 'iteration': 'iteration', 'nominal_gain': 'nominal_gains', 'output_value': 'altered_gains' }) # x:190 y:193 OperatableStateMachine.add( 'Get_Joint_Gains', ReadDynamicParameterState(param=gains_list), transitions={ 'read': 'Calculate_Next_Gain_Value', 'failed': 'failed' }, autonomy={ 'read': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'traj_controller': 'traj_controller', 'parameter_value': 'nominal_gains' }) # x:158 y:505 OperatableStateMachine.add( 'Increment_Iteration_Counter', CalculationState(calculation=lambda it: it + 1), transitions={'done': 'Decide_If_Tests_To_Go'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'iteration', 'output_value': 'iteration' }) # x:210 y:53 OperatableStateMachine.add('Move_Joints_To_Standard', _sm_move_joints_to_standard, transitions={ 'finished': 'Get_Joint_Gains', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'joints_right_up': 'joints_right_up', 'joints_left_up': 'joints_left_up' }) # x:496 y:393 OperatableStateMachine.add('Perform_Sinusoide_Motion', _sm_perform_sinusoide_motion, transitions={ 'finished': 'Increment_Iteration_Counter', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'altered_gains': 'altered_gains', 'joint_index': 'joint_index', 'traj_controller': 'traj_controller', 'nominal_gains': 'nominal_gains' }) _sm_test_joint_controls = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'joint_index', 'none', 'joints_right_up', 'joints_left_up' ]) with _sm_test_joint_controls: # x:47 y:121 OperatableStateMachine.add( 'Decide_Joints_To_Go', DecisionState(outcomes=["done", "continue"], conditions=lambda idx: "done" if idx == 12 else "continue"), transitions={ 'done': 'finished', 'continue': 'Set_Trajectory_Controller' }, autonomy={ 'done': Autonomy.High, 'continue': Autonomy.Low }, remapping={'input_value': 'joint_index'}) # x:571 y:68 OperatableStateMachine.add('Test_Individual_Joint', _sm_test_individual_joint, transitions={ 'finished': 'Increment_Joint_Index', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'joint_index': 'joint_index', 'traj_controller': 'traj_controller', 'none': 'none', 'joints_right_up': 'joints_right_up', 'joints_left_up': 'joints_left_up' }) # x:279 y:41 OperatableStateMachine.add( 'Increment_Joint_Index', CalculationState(calculation=lambda it: it + 1), transitions={'done': 'Decide_Joints_To_Go'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_index', 'output_value': 'joint_index' }) # x:447 y:224 OperatableStateMachine.add( 'Set_Trajectory_Controller', CalculationState( calculation=lambda idx: self._traj_controllers[idx]), transitions={'done': 'Test_Individual_Joint'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_index', 'output_value': 'traj_controller' }) with _state_machine: # x:112 y:38 OperatableStateMachine.add( 'Check_Initial_Stand', CheckCurrentControlModeState( target_mode=CheckCurrentControlModeState.STAND, wait=False), transitions={ 'correct': 'Switch_To_Manipulate', 'incorrect': 'Set_Initial_Stand' }, autonomy={ 'correct': Autonomy.Low, 'incorrect': Autonomy.Low }, remapping={'control_mode': 'control_mode'}) # x:336 y:123 OperatableStateMachine.add( 'Set_Initial_Stand', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'Switch_To_Manipulate', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:60 y:235 OperatableStateMachine.add( 'Switch_To_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'Bring_Left_Arm_Up', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:105 y:428 OperatableStateMachine.add( 'Bring_Left_Arm_Up', MoveitMoveGroupState(planning_group="l_arm_group", joint_names=joint_names_left), transitions={ 'reached': 'Bring_Right_Arm_Up', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Low, 'failed': Autonomy.High }, remapping={'target_joint_config': 'joints_left_up'}) # x:323 y:482 OperatableStateMachine.add( 'Bring_Right_Arm_Up', MoveitMoveGroupState(planning_group="r_arm_group", joint_names=joint_names_right), transitions={ 'reached': 'Test_Joint_Controls', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.High, 'failed': Autonomy.High }, remapping={'target_joint_config': 'joints_right_up'}) # x:620 y:465 OperatableStateMachine.add('Test_Joint_Controls', _sm_test_joint_controls, transitions={ 'finished': 'Change_Back_To_Stand', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'joint_index': 'joint_index', 'none': 'none', 'joints_right_up': 'joints_right_up', 'joints_left_up': 'joints_left_up' }) # x:887 y:469 OperatableStateMachine.add( 'Change_Back_To_Stand', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'Log_Finished', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.Off }) # x:897 y:316 OperatableStateMachine.add( 'Log_Finished', LogState(text="Behavior finished successfully!", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): bag_folder_in = "~/sysid_tests" bag_folder_out = "" # optional (if not provided, an /out folder will be created in the bag_folder_in directory prefix = "" # optional (applies to bag and video files) input_bagfiles = [] # calculated (leave empty) output_bagfiles = [] # calculated (leave empty) desired_num_tests = 1 # num of tests per trajectory wait_time = 3.0 # before execution (for rosbag record) settling_time = 2.0 # after execution mode_for_tests = "dance" # "manipulate", "system_id", etc. desired_controllers = ["left_arm_traj_controller","right_arm_traj_controller", "left_leg_traj_controller","right_leg_traj_controller","torso_traj_controller"] # x:880 y:104, x:660 y:14 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.traj_index = 0 _state_machine.userdata.tests_counter = 0 _state_machine.userdata.none = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # Get folder containing input bagfiles and create folder for output bag_folder_in = os.path.expanduser(bag_folder_in) if not os.path.exists(bag_folder_in): Logger.logwarn('Path to input bag folder does not exist (%s)' % bag_folder_in) if not bag_folder_out: bag_folder_out = os.path.join(bag_folder_in, 'out') if not os.path.exists(bag_folder_out): os.makedirs(bag_folder_out) else: # The user has provided a directory for output bagfiles bag_folder_out = os.path.expanduser(bag_folder_out) if not os.path.exists(bag_folder_out): os.makedirs(bag_folder_out) # Initialize lists again, in case the user did alter them input_bagfiles = [] output_bagfiles = [] # Get all input bagfile names from bag folder and also name the corresponding output bagfiles os.chdir(bag_folder_in) for bagfile in sorted(glob.glob("*.bag")): input_bagfiles.append(os.path.join(bag_folder_in, bagfile)) bare_name = bagfile.split(".")[0] output_name = bare_name + "_" + time.strftime("%Y-%m-%d-%H_%M") + "_" # file name will be completed by flexible calculation state output_bagfiles.append(output_name) Logger.loginfo('Found %d input bag files in %s' % (len(input_bagfiles), bag_folder_in)) # Create STAND posture trajectories _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory() # [/MANUAL_CREATE] # x:836 y:51, x:858 y:296 _sm_starting_point_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['experiment_name', 'trajectories']) with _sm_starting_point_0: # x:49 y:42 OperatableStateMachine.add('Gen_Starting_Name', CalculationState(calculation=lambda en: en + "_starting"), transitions={'done': 'Gen_Starting_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'experiment_name', 'output_value': 'starting_name'}) # x:42 y:231 OperatableStateMachine.add('Record_Starting_Point', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_for_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={'bagfile_name': 'output_bagfile_starting', 'rosbag_process': 'rosbag_process_starting'}) # x:38 y:330 OperatableStateMachine.add('Wait_for_Rosbag_Record', WaitState(wait_time=wait_time), transitions={'done': 'Extract_Left_Arm_Part'}, autonomy={'done': Autonomy.Off}) # x:29 y:133 OperatableStateMachine.add('Gen_Starting_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join(bag_folder_out, en) + ".bag"), transitions={'done': 'Record_Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'starting_name', 'output_value': 'output_bagfile_starting'}) # x:536 y:47 OperatableStateMachine.add('Stop_Recording_Starting_Point', StopRecordLogsState(), transitions={'stopped': 'finished'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:228 y:757 OperatableStateMachine.add('Plan_to_Starting_Point_Left_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Right_Arm', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_left_arm', 'plan_to_goal': 'plan_to_goal_left_arm'}) # x:236 y:655 OperatableStateMachine.add('Plan_to_Starting_Point_Right_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Left_Leg', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_right_arm', 'plan_to_goal': 'plan_to_goal_right_arm'}) # x:272 y:47 OperatableStateMachine.add('Go_to_Starting_Point', ExecuteTrajectoryWholeBodyState(controllers=desired_controllers), transitions={'done': 'Stop_Recording_Starting_Point', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'trajectories': 'trajectories_all'}) # x:536 y:169 OperatableStateMachine.add('Report_Starting_Point_Failure', LogState(text="Failed to plan or go to starting point!", severity=Logger.REPORT_WARN), transitions={'done': 'Stop_Recording_When_Failed'}, autonomy={'done': Autonomy.Full}) # x:34 y:424 OperatableStateMachine.add('Extract_Left_Arm_Part', CalculationState(calculation=lambda t: {'left_arm': t['left_arm']} if 'left_arm' in t else None), transitions={'done': 'Extract_Right_Arm_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_left_arm'}) # x:21 y:507 OperatableStateMachine.add('Extract_Right_Arm_Part', CalculationState(calculation=lambda t: {'right_arm': t['right_arm']} if 'right_arm' in t else None), transitions={'done': 'Extract_Left_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_right_arm'}) # x:293 y:146 OperatableStateMachine.add('Combine_Plans', FlexibleCalculationState(calculation=self.combine_plans, input_keys=['left_arm', 'right_arm', 'left_leg', 'right_leg', 'torso']), transitions={'done': 'Go_to_Starting_Point'}, autonomy={'done': Autonomy.Low}, remapping={'left_arm': 'plan_to_goal_left_arm', 'right_arm': 'plan_to_goal_right_arm', 'left_leg': 'plan_to_goal_left_leg', 'right_leg': 'plan_to_goal_right_leg', 'torso': 'plan_to_goal_torso', 'output_value': 'trajectories_all'}) # x:789 y:167 OperatableStateMachine.add('Stop_Recording_When_Failed', StopRecordLogsState(), transitions={'stopped': 'failed'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:24 y:601 OperatableStateMachine.add('Extract_Left_Leg_Part', CalculationState(calculation=lambda t: {'left_leg': t['left_leg']} if 'left_leg' in t else None), transitions={'done': 'Extract_Right_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_left_leg'}) # x:22 y:665 OperatableStateMachine.add('Extract_Right_Leg_Part', CalculationState(calculation=lambda t: {'right_leg': t['right_leg']} if 'right_leg' in t else None), transitions={'done': 'Extract_Torso_Part'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_right_leg'}) # x:33 y:765 OperatableStateMachine.add('Extract_Torso_Part', CalculationState(calculation=lambda t: {'torso': t['torso']} if 'torso' in t else None), transitions={'done': 'Plan_to_Starting_Point_Left_Arm'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'trajectories', 'output_value': 'trajectories_torso'}) # x:227 y:410 OperatableStateMachine.add('Plan_to_Starting_Point_Right_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Torso', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_right_leg', 'plan_to_goal': 'plan_to_goal_right_leg'}) # x:237 y:531 OperatableStateMachine.add('Plan_to_Starting_Point_Left_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Plan_to_Starting_Point_Right_Leg', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_left_leg', 'plan_to_goal': 'plan_to_goal_left_leg'}) # x:241 y:296 OperatableStateMachine.add('Plan_to_Starting_Point_Torso', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={'done': 'Combine_Plans', 'failed': 'Report_Starting_Point_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'desired_goal': 'trajectories_torso', 'plan_to_goal': 'plan_to_goal_torso'}) # x:1118 y:103, x:348 y:32 _sm_execute_individual_trajectory_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['traj_index', 'tests_counter']) with _sm_execute_individual_trajectory_1: # x:79 y:28 OperatableStateMachine.add('Get_Input_Bagfile', CalculationState(calculation=lambda idx: input_bagfiles[idx]), transitions={'done': 'Gen_Experiment_Name'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'traj_index', 'output_value': 'input_bagfile'}) # x:904 y:187 OperatableStateMachine.add('Stop_Recording', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging'}, autonomy={'stopped': Autonomy.Low}, remapping={'rosbag_process': 'rosbag_process'}) # x:494 y:292 OperatableStateMachine.add('Wait_For_Rosbag_Record', WaitState(wait_time=wait_time), transitions={'done': 'Execute_Trajs_from_Bagfile'}, autonomy={'done': Autonomy.Low}) # x:507 y:384 OperatableStateMachine.add('Record_SysID_Test', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_For_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={'bagfile_name': 'output_bagfile', 'rosbag_process': 'rosbag_process'}) # x:484 y:106 OperatableStateMachine.add('Stop_Recording_After_Failure', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging_After_Failure'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:727 y:188 OperatableStateMachine.add('Wait_for_Settling', WaitState(wait_time=settling_time), transitions={'done': 'Stop_Recording'}, autonomy={'done': Autonomy.Off}) # x:482 y:188 OperatableStateMachine.add('Execute_Trajs_from_Bagfile', ExecuteTrajectoryWholeBodyState(controllers=desired_controllers), transitions={'done': 'Wait_for_Settling', 'failed': 'Stop_Recording_After_Failure'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'trajectories': 'trajectories'}) # x:48 y:283 OperatableStateMachine.add('Load_Trajs_from_Bagfile', LoadTrajectoryFromBagfileState(), transitions={'done': 'Start_Video_Logging', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'bagfile_name': 'input_bagfile', 'trajectories': 'trajectories'}) # x:65 y:113 OperatableStateMachine.add('Gen_Experiment_Name', FlexibleCalculationState(calculation=lambda i: prefix + output_bagfiles[i[0]] + str(i[1] + 1), input_keys=["idx", "counter"]), transitions={'done': 'Gen_Output_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'idx': 'traj_index', 'counter': 'tests_counter', 'output_value': 'experiment_name'}) # x:56 y:192 OperatableStateMachine.add('Gen_Output_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join(bag_folder_out, en) + ".bag"), transitions={'done': 'Load_Trajs_from_Bagfile'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'experiment_name', 'output_value': 'output_bagfile'}) # x:903 y:97 OperatableStateMachine.add('Stop_Video_Logging', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:472 y:26 OperatableStateMachine.add('Stop_Video_Logging_After_Failure', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:285 y:378 OperatableStateMachine.add('Starting_Point', _sm_starting_point_0, transitions={'finished': 'Record_SysID_Test', 'failed': 'Stop_Video_Logging_After_Failure'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'experiment_name': 'experiment_name', 'trajectories': 'trajectories'}) # x:71 y:384 OperatableStateMachine.add('Start_Video_Logging', VideoLoggingState(command=VideoLoggingState.START, no_video=False, no_bags=True), transitions={'done': 'Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'}) # x:475 y:405 _sm_execute_sysid_trajectories_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['traj_index', 'tests_counter'], output_keys=['traj_index']) with _sm_execute_sysid_trajectories_2: # x:367 y:24 OperatableStateMachine.add('Execute_Individual_Trajectory', _sm_execute_individual_trajectory_1, transitions={'finished': 'Increment_Tests_per_Traj_counter', 'failed': 'Wait_before_Fail'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'traj_index': 'traj_index', 'tests_counter': 'tests_counter'}) # x:714 y:27 OperatableStateMachine.add('Increment_Tests_per_Traj_counter', CalculationState(calculation=lambda counter: counter + 1), transitions={'done': 'More_Tests_for_this_Traj?'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'tests_counter', 'output_value': 'tests_counter'}) # x:738 y:207 OperatableStateMachine.add('More_Tests_for_this_Traj?', DecisionState(outcomes=['yes', 'no'], conditions=lambda counter: 'no' if counter >= desired_num_tests else 'yes'), transitions={'yes': 'Execute_Individual_Trajectory', 'no': 'Reset_Tests_counter'}, autonomy={'yes': Autonomy.Low, 'no': Autonomy.Low}, remapping={'input_value': 'tests_counter'}) # x:752 y:298 OperatableStateMachine.add('Reset_Tests_counter', CalculationState(calculation=lambda counter: 0), transitions={'done': 'Increment_Traj_Index'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'tests_counter', 'output_value': 'tests_counter'}) # x:752 y:399 OperatableStateMachine.add('Increment_Traj_Index', CalculationState(calculation=lambda idx: idx + 1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'traj_index', 'output_value': 'traj_index'}) # x:405 y:141 OperatableStateMachine.add('Wait_before_Fail', WaitState(wait_time=2.0), transitions={'done': 'Increment_Tests_per_Traj_counter'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:122 y:26 OperatableStateMachine.add('Starting_Execution', LogState(text="Execution is starting. Confirm first transition!", severity=Logger.REPORT_HINT), transitions={'done': 'Go_to_Desired_Mode'}, autonomy={'done': Autonomy.High}) # x:328 y:298 OperatableStateMachine.add('Execute SysID Trajectories', _sm_execute_sysid_trajectories_2, transitions={'finished': 'More_Trajectories?'}, autonomy={'finished': Autonomy.Inherit}, remapping={'traj_index': 'traj_index', 'tests_counter': 'tests_counter'}) # x:129 y:215 OperatableStateMachine.add('More_Trajectories?', DecisionState(outcomes=['yes', 'no'], conditions=lambda idx: 'no' if idx >= len(input_bagfiles) else 'yes'), transitions={'yes': 'Notify_Next_Trajectory', 'no': 'Move_to_Stand_Posture'}, autonomy={'yes': Autonomy.Low, 'no': Autonomy.High}, remapping={'input_value': 'traj_index'}) # x:592 y:103 OperatableStateMachine.add('Go_to_FREEZE_before_Exit', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.FREEZE), transitions={'changed': 'finished', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Low}) # x:584 y:214 OperatableStateMachine.add('Failed_To_Go_To_Stand_Posture', LogState(text="Failed to go to stand posture", severity=Logger.REPORT_WARN), transitions={'done': 'Go_to_FREEZE_before_Exit'}, autonomy={'done': Autonomy.Off}) # x:121 y:393 OperatableStateMachine.add('Notify_Next_Trajectory', LogState(text="Continuing with next trajectory.", severity=Logger.REPORT_INFO), transitions={'done': 'Execute SysID Trajectories'}, autonomy={'done': Autonomy.Off}) # x:112 y:102 OperatableStateMachine.add('Go_to_Desired_Mode', ChangeControlModeActionState(target_mode=mode_for_tests), transitions={'changed': 'More_Trajectories?', 'failed': 'Go_to_FREEZE_before_Exit'}, autonomy={'changed': Autonomy.High, 'failed': Autonomy.High}) # x:329 y:158 OperatableStateMachine.add('Move_to_Stand_Posture', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={'done': 'Go_to_FREEZE_before_Exit', 'failed': 'Failed_To_Go_To_Stand_Posture'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.High}, remapping={'side': 'none'}) return _state_machine
def create(self): waypoint_distance = 1 # x:92 y:394, x:775 y:176 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.none = None _state_machine.userdata.waypoint = PoseStamped() # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:105 y:518, x:331 y:110, x:346 y:384 _sm_process_waypoint_0 = OperatableStateMachine( outcomes=['finished', 'waypoint_failure', 'data_failure'], input_keys=['waypoint']) with _sm_process_waypoint_0: # x:58 y:44 OperatableStateMachine.add( 'Plan_To_Waypoint', PlanFootstepsState(mode=PlanFootstepsState.MODE_STEP_2D, pose_is_pelvis=False), transitions={ 'planned': 'Perform_Walking', 'failed': 'waypoint_failure' }, autonomy={ 'planned': Autonomy.Low, 'failed': Autonomy.Low }, remapping={ 'target_pose': 'waypoint', 'footstep_plan': 'footstep_plan' }) # x:40 y:154 OperatableStateMachine.add( 'Perform_Walking', ExecuteStepPlanActionState(), transitions={ 'finished': 'Take_Camera_Image', 'failed': 'waypoint_failure' }, autonomy={ 'finished': Autonomy.High, 'failed': Autonomy.Low }, remapping={'footstep_plan': 'footstep_plan'}) # x:42 y:376 OperatableStateMachine.add('Send_Image_To_Operator', SendToOperatorState(), transitions={ 'done': 'finished', 'no_connection': 'data_failure' }, autonomy={ 'done': Autonomy.Low, 'no_connection': Autonomy.Low }, remapping={'data': 'camera_img'}) # x:53 y:266 OperatableStateMachine.add( 'Take_Camera_Image', GetCameraImageState(), transitions={'done': 'Send_Image_To_Operator'}, autonomy={'done': Autonomy.Off}, remapping={'camera_img': 'camera_img'}) # x:190 y:266 OperatableStateMachine.add( 'Take_Laser_Scan', GetLaserscanState(), transitions={'done': 'Send_Image_To_Operator'}, autonomy={'done': Autonomy.Off}, remapping={'laserscan': 'laserscan'}) # x:30 y:365 _sm_generate_waypoint_1 = OperatableStateMachine( outcomes=['finished'], input_keys=['waypoint', 'none'], output_keys=['waypoint_out']) with _sm_generate_waypoint_1: # x:32 y:50 OperatableStateMachine.add( 'Init_Radius', CalculationState(calculation=lambda x: 0.2), transitions={'done': 'Calculate_Next_Pose'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'none', 'output_value': 'sdv' }) # x:173 y:129 OperatableStateMachine.add( 'Calculate_Next_Pose', FlexibleCalculationState(calculation=self.calc_pose, input_keys=["pose", "radius"]), transitions={'done': 'Determine_Waypoint_Valid'}, autonomy={'done': Autonomy.Off}, remapping={ 'pose': 'waypoint', 'radius': 'sdv', 'output_value': 'waypoint_out' }) # x:162 y:260 OperatableStateMachine.add( 'Determine_Waypoint_Valid', PlanFootstepsState(mode=PlanFootstepsState.MODE_STEP_2D, pose_is_pelvis=False), transitions={ 'planned': 'finished', 'failed': 'Increase_Radius' }, autonomy={ 'planned': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'target_pose': 'waypoint_out', 'footstep_plan': 'footstep_plan' }) # x:299 y:193 OperatableStateMachine.add( 'Increase_Radius', CalculationState(calculation=lambda s: s + 0.1), transitions={'done': 'Calculate_Next_Pose'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'sdv', 'output_value': 'sdv' }) with _state_machine: # x:33 y:45 OperatableStateMachine.add( 'Generate_Waypoint', _sm_generate_waypoint_1, transitions={'finished': 'Process_Waypoint'}, autonomy={'finished': Autonomy.Inherit}, remapping={ 'waypoint': 'waypoint', 'none': 'none', 'waypoint_out': 'waypoint' }) # x:726 y:51 OperatableStateMachine.add( 'Abort_After_Failure', OperatorDecisionState( outcomes=["continue", "abort"], hint="Continue with next position regardless of failure?", suggestion="continue"), transitions={ 'continue': 'Generate_Waypoint', 'abort': 'failed' }, autonomy={ 'continue': Autonomy.Full, 'abort': Autonomy.Full }) # x:513 y:282 OperatableStateMachine.add( 'Log_Data_Failure', LogState(text="Failed to send captured data!", severity=Logger.REPORT_WARN), transitions={'done': 'Abort_After_Failure'}, autonomy={'done': Autonomy.Off}) # x:253 y:178 OperatableStateMachine.add('Process_Waypoint', _sm_process_waypoint_0, transitions={ 'finished': 'Check_Finished', 'waypoint_failure': 'Log_Waypoint_Failure', 'data_failure': 'Log_Data_Failure' }, autonomy={ 'finished': Autonomy.Inherit, 'waypoint_failure': Autonomy.Inherit, 'data_failure': Autonomy.Inherit }, remapping={'waypoint': 'waypoint'}) # x:42 y:284 OperatableStateMachine.add('Check_Finished', OperatorDecisionState( outcomes=["finished", "next"], hint="Going to next position?", suggestion="next"), transitions={ 'finished': 'finished', 'next': 'Generate_Waypoint' }, autonomy={ 'finished': Autonomy.Full, 'next': Autonomy.High }) # x:505 y:120 OperatableStateMachine.add( 'Log_Waypoint_Failure', LogState(text="Failed to walk to next waypoint!", severity=Logger.REPORT_WARN), transitions={'done': 'Abort_After_Failure'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM no_valve_collision = True turning_affordance = "open" turn_amount = 200 # degree turn_back_affordance = "close" turn_test_amount = 25 # degree # x:933 y:490, x:333 y:390 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.default_preference = 0 _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.step_back_distance = 1 # meters _state_machine.userdata.none = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._turn_amount = turn_amount self._turn_test_amount = turn_test_amount # [/MANUAL_CREATE] # x:30 y:478, x:130 y:478, x:230 y:478 _sm_planning_pipeline_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['stand_pose'], output_keys=['plan_header']) with _sm_planning_pipeline_0: # x:34 y:57 OperatableStateMachine.add('Create_Step_Goal', CreateStepGoalState(pose_is_pelvis=True), transitions={'done': 'Plan_To_Waypoint', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'target_pose': 'stand_pose', 'step_goal': 'step_goal'}) # x:553 y:481 OperatableStateMachine.add('Modify_Plan', InputState(request=InputState.FOOTSTEP_PLAN_HEADER, message='Modify plan, VALIDATE, and confirm.'), transitions={'received': 'finished', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'plan_header'}) # x:34 y:484 OperatableStateMachine.add('Plan_To_Waypoint', PlanFootstepsState(mode=self.parameter_set), transitions={'planned': 'Modify_Plan', 'failed': 'Decide_Replan_without_Collision'}, autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:139 y:314 OperatableStateMachine.add('Decide_Replan_without_Collision', OperatorDecisionState(outcomes=['replan', 'fail'], hint='Try replanning without collision avoidance.', suggestion='replan'), transitions={'replan': 'Replan_without_Collision', 'fail': 'failed'}, autonomy={'replan': Autonomy.Low, 'fail': Autonomy.Full}) # x:319 y:406 OperatableStateMachine.add('Replan_without_Collision', PlanFootstepsState(mode='drc_step_no_collision'), transitions={'planned': 'Modify_Plan', 'failed': 'failed'}, autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:483 y:540, x:183 y:290 _sm_perform_turn_back_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'back_rotation', 'none']) with _sm_perform_turn_back_1: # x:71 y:78 OperatableStateMachine.add('Get_Turn_Back_Affordance', GetTemplateAffordanceState(identifier=turn_back_affordance), transitions={'done': 'Set_Back_Rotation', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'turning_affordance'}) # x:419 y:228 OperatableStateMachine.add('Plan_Turn_Back_Affordance', PlanAffordanceState(vel_scaling=0.2, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Turn_Back_Affordance', 'incomplete': 'Execute_Turn_Back_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:411 y:378 OperatableStateMachine.add('Execute_Turn_Back_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'finished'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:434 y:78 OperatableStateMachine.add('Set_Back_Rotation', FlexibleCalculationState(calculation=self.set_back_rotation, input_keys=["turning_affordance", "back_rotation"]), transitions={'done': 'Plan_Turn_Back_Affordance'}, autonomy={'done': Autonomy.Off}, remapping={'turning_affordance': 'turning_affordance', 'back_rotation': 'back_rotation', 'output_value': 'turning_affordance'}) # x:383 y:40, x:433 y:490 _sm_perform_turning_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none'], output_keys=['back_rotation']) with _sm_perform_turning_2: # x:92 y:28 OperatableStateMachine.add('Adjust_Hand_Pose', LogState(text="Make sure hand is in valve", severity=Logger.REPORT_HINT), transitions={'done': 'Init_Back_Rotation'}, autonomy={'done': Autonomy.Full}) # x:926 y:228 OperatableStateMachine.add('Plan_Turning_Affordance', PlanAffordanceState(vel_scaling=0.3, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Turning_Affordance', 'incomplete': 'Execute_Turning_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:495 y:228 OperatableStateMachine.add('Set_Full_Rotation', CalculationState(calculation=self.set_full_rotation_angle), transitions={'done': 'Plan_Turning_Affordance'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'turning_affordance', 'output_value': 'turning_affordance'}) # x:818 y:78 OperatableStateMachine.add('Execute_Turning_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Accumulate_Rotation', 'failed': 'Accumulate_Rotation'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:534 y:78 OperatableStateMachine.add('Accumulate_Rotation', FlexibleCalculationState(calculation=lambda x: x[0] + self._turn_amount * x[1], input_keys=["back_rotation", "plan_fraction"]), transitions={'done': 'Decide_Turn_Again'}, autonomy={'done': Autonomy.Off}, remapping={'back_rotation': 'back_rotation', 'plan_fraction': 'plan_fraction', 'output_value': 'back_rotation'}) # x:287 y:128 OperatableStateMachine.add('Decide_Turn_Again', OperatorDecisionState(outcomes=["extract", "turn"], hint="Turn again or extract hand?", suggestion="turn"), transitions={'extract': 'finished', 'turn': 'Decide_If_Test'}, autonomy={'extract': Autonomy.High, 'turn': Autonomy.Full}) # x:73 y:228 OperatableStateMachine.add('Get_Turning_Affordance', GetTemplateAffordanceState(identifier=turning_affordance), transitions={'done': 'Decide_If_Test', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'turning_affordance'}) # x:302 y:228 OperatableStateMachine.add('Decide_If_Test', DecisionState(outcomes=["test", "full"], conditions=lambda x: "test" if x == 0 else "full"), transitions={'test': 'Set_Test_Rotation', 'full': 'Set_Full_Rotation'}, autonomy={'test': Autonomy.Low, 'full': Autonomy.Low}, remapping={'input_value': 'back_rotation'}) # x:494 y:178 OperatableStateMachine.add('Set_Test_Rotation', CalculationState(calculation=self.set_test_rotation_angle), transitions={'done': 'Plan_Test_Turning_Affordance'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'turning_affordance', 'output_value': 'turning_affordance'}) # x:92 y:128 OperatableStateMachine.add('Init_Back_Rotation', CalculationState(calculation=lambda x: 0), transitions={'done': 'Get_Turning_Affordance'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'none', 'output_value': 'back_rotation'}) # x:713 y:178 OperatableStateMachine.add('Plan_Test_Turning_Affordance', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Turning_Affordance', 'incomplete': 'Execute_Turning_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:733 y:190, x:433 y:140 _sm_extract_hand_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'preference', 'none']) with _sm_extract_hand_3: # x:77 y:55 OperatableStateMachine.add('Get_Pregrasp', GetTemplatePregraspState(), transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'preference', 'pre_grasp': 'pre_grasp'}) # x:296 y:328 OperatableStateMachine.add('Plan_To_Pregrasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=no_valve_collision, include_torso=False, keep_endeffector_orientation=True, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Pregrasp', 'incomplete': 'Move_To_Pregrasp', 'failed': 'failed'}, autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'waypoints': 'waypoints', 'hand': 'hand_side', 'frame_id': 'frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:96 y:328 OperatableStateMachine.add('Create_Pose_List', CalculationState(calculation=lambda x: [x.pose]), transitions={'done': 'Plan_To_Pregrasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pre_grasp', 'output_value': 'waypoints'}) # x:676 y:328 OperatableStateMachine.add('Move_To_Pregrasp', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:95 y:228 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda x: x.header.frame_id), transitions={'done': 'Create_Pose_List'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pre_grasp', 'output_value': 'frame_id'}) # x:733 y:190, x:433 y:190 _sm_insert_hand_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'preference', 'none']) with _sm_insert_hand_4: # x:83 y:78 OperatableStateMachine.add('Get_Grasp', GetTemplateGraspState(), transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'preference', 'grasp': 'grasp'}) # x:296 y:328 OperatableStateMachine.add('Plan_To_Grasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=no_valve_collision, include_torso=False, keep_endeffector_orientation=True, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Grasp', 'incomplete': 'Move_To_Grasp', 'failed': 'failed'}, autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'waypoints': 'waypoints', 'hand': 'hand_side', 'frame_id': 'frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:96 y:328 OperatableStateMachine.add('Create_Pose_List', CalculationState(calculation=lambda x: [x.pose]), transitions={'done': 'Plan_To_Grasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp', 'output_value': 'waypoints'}) # x:676 y:328 OperatableStateMachine.add('Move_To_Grasp', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:96 y:228 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda x: x.header.frame_id), transitions={'done': 'Create_Pose_List'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp', 'output_value': 'frame_id'}) # x:933 y:290, x:133 y:340 _sm_prepare_joint_limit_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_prepare_joint_limit_5: # x:71 y:78 OperatableStateMachine.add('Get_Turn_Back_Affordance', GetTemplateAffordanceState(identifier=turn_back_affordance), transitions={'done': 'Set_Back_Rotation', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'turning_affordance'}) # x:419 y:228 OperatableStateMachine.add('Plan_Turn_Back_Affordance', PlanAffordanceState(vel_scaling=0.2, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Turn_Back_Affordance', 'incomplete': 'Execute_Turn_Back_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:411 y:378 OperatableStateMachine.add('Execute_Turn_Back_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Decide_Turn_Further', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:442 y:78 OperatableStateMachine.add('Set_Back_Rotation', CalculationState(calculation=self.set_full_rotation_angle), transitions={'done': 'Plan_Turn_Back_Affordance'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'turning_affordance', 'output_value': 'turning_affordance'}) # x:687 y:278 OperatableStateMachine.add('Decide_Turn_Further', DecisionState(outcomes=["turn_again", "done"], conditions=lambda x: "turn_again" if x > 0.95 else "done"), transitions={'turn_again': 'Plan_Turn_Back_Affordance', 'done': 'finished'}, autonomy={'turn_again': Autonomy.High, 'done': Autonomy.Low}, remapping={'input_value': 'plan_fraction'}) # x:30 y:478, x:130 y:478, x:230 y:478 _sm_walk_to_template_6 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['template_id', 'grasp_preference', 'hand_side']) with _sm_walk_to_template_6: # x:265 y:28 OperatableStateMachine.add('Decide_Request_Template', DecisionState(outcomes=['request', 'continue'], conditions=lambda x: 'continue' if x is not None else 'request'), transitions={'request': 'Request_Template', 'continue': 'Get_Stand_Pose'}, autonomy={'request': Autonomy.Low, 'continue': Autonomy.Off}, remapping={'input_value': 'template_id'}) # x:1033 y:106 OperatableStateMachine.add('Increment_Stand_Pose', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Inform_About_Retry'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:1162 y:29 OperatableStateMachine.add('Inform_About_Retry', LogState(text="Stand pose choice failed. Trying again.", severity=Logger.REPORT_INFO), transitions={'done': 'Get_Stand_Pose'}, autonomy={'done': Autonomy.Off}) # x:567 y:118 OperatableStateMachine.add('Inform_About_Fail', LogState(text="Unable to find a suitable stand pose for the template.", severity=Logger.REPORT_WARN), transitions={'done': 'Decide_Repeat_Request'}, autonomy={'done': Autonomy.Off}) # x:554 y:274 OperatableStateMachine.add('Get_Goal_from_Operator', InputState(request=InputState.WAYPOINT_GOAL_POSE, message="Provide a waypoint in front of the template."), transitions={'received': 'Walk_To_Waypoint', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'plan_header'}) # x:279 y:110 OperatableStateMachine.add('Request_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Specify target template"), transitions={'received': 'Get_Stand_Pose', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Off, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:825 y:461 OperatableStateMachine.add('Wait_For_Stand', CheckCurrentControlModeState(target_mode=CheckCurrentControlModeState.STAND, wait=True), transitions={'correct': 'finished', 'incorrect': 'failed'}, autonomy={'correct': Autonomy.Low, 'incorrect': Autonomy.Full}, remapping={'control_mode': 'control_mode'}) # x:1143 y:277 OperatableStateMachine.add('Decide_Stand_Preference', OperatorDecisionState(outcomes=["same", "next", "abort"], hint="Same or next stand pose?", suggestion="next"), transitions={'same': 'Inform_About_Retry', 'next': 'Increment_Stand_Pose', 'abort': 'aborted'}, autonomy={'same': Autonomy.Full, 'next': Autonomy.Full, 'abort': Autonomy.Full}) # x:842 y:152 OperatableStateMachine.add('Planning_Pipeline', _sm_planning_pipeline_0, transitions={'finished': 'Walk_To_Waypoint', 'failed': 'Decide_Stand_Preference', 'aborted': 'Decide_Stand_Preference'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'stand_pose': 'stand_pose', 'plan_header': 'plan_header'}) # x:833 y:276 OperatableStateMachine.add('Walk_To_Waypoint', ExecuteStepPlanActionState(), transitions={'finished': 'Wait_For_Stand', 'failed': 'Decide_Stand_Preference'}, autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:554 y:195 OperatableStateMachine.add('Decide_Repeat_Request', OperatorDecisionState(outcomes=['repeat_id', 'request_goal'], hint=None, suggestion=None), transitions={'repeat_id': 'Request_Template', 'request_goal': 'Get_Goal_from_Operator'}, autonomy={'repeat_id': Autonomy.Low, 'request_goal': Autonomy.High}) # x:547 y:27 OperatableStateMachine.add('Get_Stand_Pose', GetTemplateStandPoseState(), transitions={'done': 'Planning_Pipeline', 'failed': 'Inform_About_Fail', 'not_available': 'Inform_About_Fail'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'stand_pose': 'stand_pose'}) # x:183 y:590, x:383 y:290 _sm_manipulate_valve_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'preference', 'none']) with _sm_manipulate_valve_7: # x:144 y:72 OperatableStateMachine.add('Insert_Hand', _sm_insert_hand_4, transitions={'finished': 'Perform_Turning', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'preference': 'preference', 'none': 'none'}) # x:543 y:422 OperatableStateMachine.add('Extract_Hand', _sm_extract_hand_3, transitions={'finished': 'Decide_Repeat', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'preference': 'preference', 'none': 'none'}) # x:534 y:72 OperatableStateMachine.add('Perform_Turning', _sm_perform_turning_2, transitions={'finished': 'Extract_Hand', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none', 'back_rotation': 'back_rotation'}) # x:127 y:272 OperatableStateMachine.add('Perform_Turn_Back', _sm_perform_turn_back_1, transitions={'finished': 'Adjust_Hand_Rotation', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'back_rotation': 'back_rotation', 'none': 'none'}) # x:137 y:428 OperatableStateMachine.add('Decide_Repeat', OperatorDecisionState(outcomes=['insert_again', 'continue'], hint="Continue or adjust wry2 rotation and rotate again", suggestion='insert_again'), transitions={'insert_again': 'Perform_Turn_Back', 'continue': 'finished'}, autonomy={'insert_again': Autonomy.High, 'continue': Autonomy.Full}) # x:134 y:178 OperatableStateMachine.add('Adjust_Hand_Rotation', OperatorDecisionState(outcomes=['done'], hint="Adjust poking stick rotation", suggestion=None), transitions={'done': 'Insert_Hand'}, autonomy={'done': Autonomy.Full}) # x:83 y:340, x:333 y:140 _sm_step_back_8 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['distance']) with _sm_step_back_8: # x:28 y:63 OperatableStateMachine.add('Plan_Steps_Back', FootstepPlanRelativeState(direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={'planned': 'Execute_Steps_Back', 'failed': 'failed'}, autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full}, remapping={'distance': 'distance', 'plan_header': 'plan_header'}) # x:24 y:163 OperatableStateMachine.add('Execute_Steps_Back', ExecuteStepPlanActionState(), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:638 y:580, x:230 y:239 _sm_prepare_manipulation_9 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'grasp_preference', 'hand_side', 'none']) with _sm_prepare_manipulation_9: # x:65 y:36 OperatableStateMachine.add('Go_To_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Set_Template_Frame', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:586 y:463 OperatableStateMachine.add('Adjust_Hand_Rotation', OperatorDecisionState(outcomes=['done'], hint="Adjust poking stick rotation", suggestion=None), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Full}) # x:116 y:409 OperatableStateMachine.add('Get_Pregrasp', GetTemplatePregraspState(), transitions={'done': 'Plan_To_Pregrasp', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pre_grasp'}) # x:329 y:178 OperatableStateMachine.add('Plan_To_Pregrasp', PlanEndeffectorPoseState(ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Pregrasp', 'failed': 'failed'}, autonomy={'planned': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'target_pose': 'pre_grasp', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory'}) # x:576 y:178 OperatableStateMachine.add('Move_To_Pregrasp', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Prepare_Joint_Limit', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:577 y:322 OperatableStateMachine.add('Prepare_Joint_Limit', _sm_prepare_joint_limit_5, transitions={'finished': 'Adjust_Hand_Rotation', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none'}) # x:53 y:130 OperatableStateMachine.add('Set_Template_Frame', CalculationState(calculation=lambda x: 'template_tf_' + str(x)), transitions={'done': 'Look_At_Valve'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'template_id', 'output_value': 'template_frame'}) # x:51 y:213 OperatableStateMachine.add('Look_At_Valve', LookAtTargetState(), transitions={'done': 'Align_Valve_Log'}, autonomy={'done': Autonomy.Low}, remapping={'frame': 'template_frame'}) # x:55 y:301 OperatableStateMachine.add('Align_Valve_Log', LogState(text="Adjust pose of the valve template", severity=Logger.REPORT_HINT), transitions={'done': 'Get_Pregrasp'}, autonomy={'done': Autonomy.Full}) with _state_machine: # x:35 y:78 OperatableStateMachine.add('Request_Template_ID', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place the valve template"), transitions={'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.High, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:473 y:72 OperatableStateMachine.add('Prepare_Manipulation', _sm_prepare_manipulation_9, transitions={'finished': 'Manipulate_Valve', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'grasp_preference': 'default_preference', 'hand_side': 'hand_side', 'none': 'none'}) # x:237 y:28 OperatableStateMachine.add('Decide_Walking', OperatorDecisionState(outcomes=['walk', 'skip'], hint="Walk to the valve?", suggestion='skip'), transitions={'walk': 'Walk_To_Template', 'skip': 'Prepare_Manipulation'}, autonomy={'walk': Autonomy.Full, 'skip': Autonomy.Low}) # x:644 y:557 OperatableStateMachine.add('Step_Back', _sm_step_back_8, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'distance': 'step_back_distance'}) # x:637 y:428 OperatableStateMachine.add('Decide_Step_Back', OperatorDecisionState(outcomes=["stand", "step_back"], hint="Should the robot step back from the valve?", suggestion="step_back"), transitions={'stand': 'finished', 'step_back': 'Step_Back'}, autonomy={'stand': Autonomy.Full, 'step_back': Autonomy.High}) # x:738 y:228 OperatableStateMachine.add('Inform_About_Stand', LogState(text="Back to STAND", severity=Logger.REPORT_INFO), transitions={'done': 'Go_To_Stand'}, autonomy={'done': Autonomy.Low}) # x:733 y:72 OperatableStateMachine.add('Manipulate_Valve', _sm_manipulate_valve_7, transitions={'finished': 'Inform_About_Stand', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'default_preference', 'none': 'none'}) # x:616 y:328 OperatableStateMachine.add('Go_To_Stand', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND), transitions={'changed': 'Decide_Step_Back', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:230 y:122 OperatableStateMachine.add('Walk_To_Template', _sm_walk_to_template_6, transitions={'finished': 'Prepare_Manipulation', 'failed': 'failed', 'aborted': 'Prepare_Manipulation'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'grasp_preference': 'default_preference', 'hand_side': 'hand_side'}) return _state_machine
def create(self): # x:902 y:757, x:882 y:161 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:1232 y:907, x:1267 y:613, x:1271 y:566 _sm_find_table_and_place_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'no_table']) with _sm_find_table_and_place_0: # x:877 y:673 OperatableStateMachine.add('run traj', RunTrajectory(file="receive_object", duration=0), transitions={'done': 'say release'}, autonomy={'done': Autonomy.Off}) # x:532 y:572 OperatableStateMachine.add('Action_find_2', self.use_behavior(Action_findSM, 'take objects and bring the order to customer/find table and place/Action_find_2'), transitions={'done': 'get table position', 'failed': 'say do not find table'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'tableKey', 'entity': 'tableEntity'}) # x:576 y:514 OperatableStateMachine.add('set tableKey', SetKey(Value="table"), transitions={'done': 'Action_find_2'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'tableKey'}) # x:538 y:638 OperatableStateMachine.add('get table position', CalculationState(calculation=lambda x: x.position), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'tableEntity', 'output_value': 'pose'}) # x:736 y:677 OperatableStateMachine.add('say do not find table', SaraSay(sentence="I can not find the table. Please be ready to grab the object.", input_keys=[], emotion=0, block=True), transitions={'done': 'run traj'}, autonomy={'done': Autonomy.Off}) # x:41 y:231 OperatableStateMachine.add('find a table and a free spot', GetPositionToPlaceOnTable(), transitions={'done': 'Action_place', 'not_found': 'look down_left'}, autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'}) # x:291 y:805 OperatableStateMachine.add('Action_place', self.use_behavior(Action_placeSM, 'take objects and bring the order to customer/find table and place/Action_place'), transitions={'finished': 'finished', 'failed': 'say do not find table'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pos': 'pose'}) # x:50 y:102 OperatableStateMachine.add('look down', SaraSetHeadAngle(pitch=0, yaw=0.7), transitions={'done': 'wait'}, autonomy={'done': Autonomy.Off}) # x:214 y:378 OperatableStateMachine.add('find a table and a free spot_2', GetPositionToPlaceOnTable(), transitions={'done': 'Action_place', 'not_found': 'look down_right'}, autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'}) # x:240 y:232 OperatableStateMachine.add('look down_left', SaraSetHeadAngle(pitch=0.7, yaw=0.6), transitions={'done': 'wait_2'}, autonomy={'done': Autonomy.Off}) # x:387 y:513 OperatableStateMachine.add('find a table and a free spot_2_2', GetPositionToPlaceOnTable(), transitions={'done': 'Action_place', 'not_found': 'set tableKey'}, autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'}) # x:402 y:378 OperatableStateMachine.add('look down_right', SaraSetHeadAngle(pitch=-0.7, yaw=0.6), transitions={'done': 'wait_3'}, autonomy={'done': Autonomy.Off}) # x:66 y:169 OperatableStateMachine.add('wait', WaitState(wait_time=3), transitions={'done': 'find a table and a free spot'}, autonomy={'done': Autonomy.Off}) # x:242 y:294 OperatableStateMachine.add('wait_2', WaitState(wait_time=3), transitions={'done': 'find a table and a free spot_2'}, autonomy={'done': Autonomy.Off}) # x:411 y:444 OperatableStateMachine.add('wait_3', WaitState(wait_time=3), transitions={'done': 'find a table and a free spot_2_2'}, autonomy={'done': Autonomy.Off}) # x:1000 y:670 OperatableStateMachine.add('say release', SaraSay(sentence="Here is your order. I will open my gripper in 3, 2, 1, now.", input_keys=[], emotion=0, block=True), transitions={'done': 'open gripper'}, autonomy={'done': Autonomy.Off}) # x:1122 y:666 OperatableStateMachine.add('open gripper', SetGripperState(width=0.1, effort=1), transitions={'object': 'say good day', 'no_object': 'say good day'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:1170 y:771 OperatableStateMachine.add('say good day', SaraSay(sentence="Thank you. Have a good day.", input_keys=[], emotion=6, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:62 y:29 OperatableStateMachine.add('set distance', SetKey(Value=0.3), transitions={'done': 'look down'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distanceFromEdge'}) # x:30 y:458, x:130 y:458 _sm_keep_looking_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['personID']) with _sm_keep_looking_1: # x:30 y:40 OperatableStateMachine.add('keep', KeepLookingAt(), transitions={'failed': 'keep'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'personID'}) # x:30 y:458, x:130 y:458 _sm_ask_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['question'], output_keys=['answer']) with _sm_ask_2: # x:30 y:40 OperatableStateMachine.add('Action_Ask', self.use_behavior(Action_AskSM, 'ask and save order/ask AND look person/ask/Action_Ask'), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'question': 'question', 'answer': 'answer'}) # x:30 y:458, x:130 y:458 _sm_confirm_order_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['orderList']) with _sm_confirm_order_3: # x:114 y:67 OperatableStateMachine.add('produce question', CalculationState(calculation=lambda x: "Did you order :" + str(x[0].data)), transitions={'done': 'Action_Ask'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'orderList', 'output_value': 'question'}) # x:423 y:171 OperatableStateMachine.add('Action_Ask', self.use_behavior(Action_AskSM, 'ask and save order/confirm order/Action_Ask'), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'question': 'question', 'answer': 'answer'}) # x:348 y:66, x:351 y:97, x:339 y:159, x:336 y:195, x:518 y:89, x:515 y:175 _sm_ask_and_look_person_4 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['personID', 'question'], output_keys=['answer'], conditions=[ ('finished', [('ask', 'finished')]), ('failed', [('ask', 'failed')]), ('finished', [('keep looking', 'finished')]), ('failed', [('keep looking', 'failed')]) ]) with _sm_ask_and_look_person_4: # x:30 y:40 OperatableStateMachine.add('ask', _sm_ask_2, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'question': 'question', 'answer': 'answer'}) # x:30 y:138 OperatableStateMachine.add('keep looking', _sm_keep_looking_1, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'personID': 'personID'}) # x:30 y:458 _sm_look_around_2_5 = OperatableStateMachine(outcomes=['finished']) with _sm_look_around_2_5: # x:215 y:156 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:394 y:262 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:206 y:372 OperatableStateMachine.add('center2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:38 y:596 OperatableStateMachine.add('w1', WaitState(wait_time=4), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:413 y:159 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:415 y:370 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:39 y:371 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:25 y:482 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:30 y:458 _sm_look_around_6 = OperatableStateMachine(outcomes=['finished']) with _sm_look_around_6: # x:215 y:156 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:394 y:262 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:206 y:372 OperatableStateMachine.add('center2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:38 y:596 OperatableStateMachine.add('w1', WaitState(wait_time=4), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:413 y:159 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:415 y:370 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:39 y:371 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:25 y:482 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:30 y:458, x:130 y:458 _sm_turn_around_7 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_turn_around_7: # x:80 y:26 OperatableStateMachine.add('look around', _sm_look_around_6, transitions={'finished': 'pose with 180 rotation'}, autonomy={'finished': Autonomy.Inherit}) # x:66 y:286 OperatableStateMachine.add('rotation', SaraMoveBase(reference="base_link"), transitions={'arrived': 'look around_2', 'failed': 'failed'}, autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'pose'}) # x:56 y:388 OperatableStateMachine.add('look around_2', _sm_look_around_2_5, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}) # x:63 y:133 OperatableStateMachine.add('pose with 180 rotation', GenPoseEuler(x=0, y=0, z=0, roll=0, pitch=0, yaw=3.14), transitions={'done': 'rotation'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'pose'}) # x:30 y:458 _sm_get_waving_people_8 = OperatableStateMachine(outcomes=['finished'], output_keys=['waving_person']) with _sm_get_waving_people_8: # x:47 y:45 OperatableStateMachine.add('set name', SetKey(Value="person"), transitions={'done': 'list people'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:40 y:236 OperatableStateMachine.add('filter waving', FilterKey(filter_function=lambda x: x[0].pose.right_arm_up or x[0].pose.left_arm_up, input_keys=["input_list"]), transitions={'not_empty': 'get first', 'empty': 'list people'}, autonomy={'not_empty': Autonomy.Off, 'empty': Autonomy.Off}, remapping={'input_list': 'entity_list', 'output_list': 'waving_peoples'}) # x:53 y:326 OperatableStateMachine.add('get first', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'waving_peoples', 'output_value': 'waving_person'}) # x:40 y:133 OperatableStateMachine.add('list people', list_entities_by_name(frontality_level=0.5, distance_max=5), transitions={'found': 'filter waving', 'none_found': 'list people'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'}) # x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458 _sm_look_for_waving_people_9 = ConcurrencyContainer(outcomes=['finished', 'failed'], output_keys=['waving_person'], conditions=[ ('failed', [('turn around', 'failed')]), ('failed', [('turn around', 'finished')]), ('finished', [('get waving people', 'finished')]) ]) with _sm_look_for_waving_people_9: # x:268 y:118 OperatableStateMachine.add('get waving people', _sm_get_waving_people_8, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'waving_person': 'waving_person'}) # x:60 y:126 OperatableStateMachine.add('turn around', _sm_turn_around_7, transitions={'finished': 'failed', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:441 y:583, x:93 y:568 _sm_repeate_if_first_commande_10 = OperatableStateMachine(outcomes=['finished', 'repeate'], input_keys=['commandNumber'], output_keys=['commandNumber']) with _sm_repeate_if_first_commande_10: # x:242 y:97 OperatableStateMachine.add('if first command', CheckConditionState(predicate=lambda x: x == 1), transitions={'true': 'set second command', 'false': 'finished'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'commandNumber'}) # x:73 y:329 OperatableStateMachine.add('set second command', SetKey(Value=2), transitions={'done': 'repeate'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'commandNumber'}) # x:1053 y:687, x:1068 y:149 _sm_take_objects_and_bring_the_order_to_customer_11 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['barPosition', 'orderList', 'robotPositionToCustomer']) with _sm_take_objects_and_bring_the_order_to_customer_11: # x:73 y:26 OperatableStateMachine.add('set indexkey', SetKey(Value=0), transitions={'done': 'one element by one element from the list'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'indexKey'}) # x:591 y:684 OperatableStateMachine.add('increment indexKey', CalculationState(calculation=lambda x: x+1), transitions={'done': 'check if end of the list'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'indexKey', 'output_value': 'indexKey'}) # x:796 y:679 OperatableStateMachine.add('check if end of the list', FlexibleCheckConditionState(predicate=lambda x: len(x[0]) <= x[1], input_keys=["orderList", "indexKey"]), transitions={'true': 'finished', 'false': 'Action_Move_2'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'orderList': 'orderList', 'indexKey': 'indexKey'}) # x:31 y:243 OperatableStateMachine.add('Action_find', self.use_behavior(Action_findSM, 'take objects and bring the order to customer/Action_find'), transitions={'done': 'get entity ID', 'failed': 'say cannot find'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'item', 'entity': 'entity'}) # x:76 y:183 OperatableStateMachine.add('say search and grip', SaraSay(sentence=lambda x: "I am now searching for the "+x[0]+".", input_keys=["item"], emotion=0, block=True), transitions={'done': 'Action_find'}, autonomy={'done': Autonomy.Off}, remapping={'item': 'item'}) # x:258 y:261 OperatableStateMachine.add('say cannot find', SaraSay(sentence="I cannot find the item.", input_keys=[], emotion=0, block=True), transitions={'done': 'place arm'}, autonomy={'done': Autonomy.Off}) # x:63 y:393 OperatableStateMachine.add('Action_pick', self.use_behavior(Action_pickSM, 'take objects and bring the order to customer/Action_pick'), transitions={'success': 'say go to the customer', 'unreachable': 'say cannot pick', 'not found': 'say cannot pick', 'dropped': 'say cannot pick'}, autonomy={'success': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'not found': Autonomy.Inherit, 'dropped': Autonomy.Inherit}, remapping={'objectID': 'entityID'}) # x:61 y:548 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'take objects and bring the order to customer/Action_Move'), transitions={'finished': 'find table and place', 'failed': 'say cant get back to customer'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'robotPositionToCustomer'}) # x:83 y:326 OperatableStateMachine.add('get entity ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'Action_pick'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity', 'output_value': 'entityID'}) # x:568 y:407 OperatableStateMachine.add('say put in the gripper', SaraSay(sentence=lambda x: "Please, put the "+x[0]+" in my gripper. I will close it in 3, 2, 1, now.", input_keys=["item"], emotion=0, block=True), transitions={'done': 'close gripper'}, autonomy={'done': Autonomy.Off}, remapping={'item': 'item'}) # x:243 y:387 OperatableStateMachine.add('say cannot pick', SaraSay(sentence="I can not pick the item.", input_keys=[], emotion=0, block=True), transitions={'done': 'place arm'}, autonomy={'done': Autonomy.Off}) # x:388 y:502 OperatableStateMachine.add('say thank you', SaraSay(sentence="Thank you.", input_keys=[], emotion=0, block=True), transitions={'done': 'place arm 2'}, autonomy={'done': Autonomy.Off}) # x:72 y:465 OperatableStateMachine.add('say go to the customer', SaraSay(sentence="I will serve it to the customer.", input_keys=[], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:522 y:505 OperatableStateMachine.add('close gripper', SetGripperState(width=0, effort=1), transitions={'object': 'say thank you', 'no_object': 'open gripper'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:357 y:569 OperatableStateMachine.add('say cant get back to customer', SaraSay(sentence="I am not able to go back to the customer.", input_keys=[], emotion=0, block=True), transitions={'done': 'increment indexKey'}, autonomy={'done': Autonomy.Off}) # x:541 y:317 OperatableStateMachine.add('open gripper', SetGripperState(width=0.10, effort=1), transitions={'object': 'say put in the gripper', 'no_object': 'say put in the gripper'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:63 y:110 OperatableStateMachine.add('one element by one element from the list', FlexibleCalculationState(calculation=lambda x: x[0][x[1]].data, input_keys=["orderList", "indexKey"]), transitions={'done': 'say search and grip'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList', 'indexKey': 'indexKey', 'output_value': 'item'}) # x:376 y:324 OperatableStateMachine.add('place arm', RunTrajectory(file="receive_object", duration=0), transitions={'done': 'open gripper'}, autonomy={'done': Autonomy.Off}) # x:638 y:184 OperatableStateMachine.add('Action_Move_2', self.use_behavior(Action_MoveSM, 'take objects and bring the order to customer/Action_Move_2'), transitions={'finished': 'increment indexKey', 'failed': 'say cannot go back to bar'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'barPosition'}) # x:817 y:178 OperatableStateMachine.add('say cannot go back to bar', SaraSay(sentence="I am not able to go back to the barman. I will try a new command.", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:85 y:740 OperatableStateMachine.add('find table and place', _sm_find_table_and_place_0, transitions={'finished': 'retour repos', 'failed': 'retour repos', 'no_table': 'retour repos'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'no_table': Autonomy.Inherit}) # x:248 y:505 OperatableStateMachine.add('place arm 2', RunTrajectory(file="transport", duration=0), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:362 y:679 OperatableStateMachine.add('retour repos', RunTrajectory(file="repos", duration=0), transitions={'done': 'increment indexKey'}, autonomy={'done': Autonomy.Off}) # x:242 y:352, x:1086 y:243 _sm_get_the_order_12 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['orderList']) with _sm_get_the_order_12: # x:68 y:39 OperatableStateMachine.add('length 1', CheckConditionState(predicate=lambda x: len(x) == 1), transitions={'true': 'say order', 'false': 'length 2'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'orderList'}) # x:236 y:41 OperatableStateMachine.add('length 2', CheckConditionState(predicate=lambda x: len(x) ==2), transitions={'true': 'say order_2', 'false': 'say order_3'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'orderList'}) # x:228 y:111 OperatableStateMachine.add('say order_2', SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+" and one "+x[0][1].data+", please.", input_keys=["orderList"], emotion=0, block=True), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:374 y:109 OperatableStateMachine.add('say order_3', SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+", one "+x[0][1].data+" and one "+x[0][2].data+", please.", input_keys=["orderList"], emotion=0, block=True), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:67 y:117 OperatableStateMachine.add('say order', SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+", please.", input_keys=["orderList"], emotion=0, block=True), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:224 y:256 OperatableStateMachine.add('wait 5', WaitState(wait_time=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:216 y:528, x:572 y:238 _sm_go_to_the_barman_13 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['barPosition', 'barmanID']) with _sm_go_to_the_barman_13: # x:30 y:102 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'go to the barman/Action_Move'), transitions={'finished': 'if barman id is 0', 'failed': 'say cannot'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'barPosition'}) # x:365 y:99 OperatableStateMachine.add('say cannot', SaraSay(sentence="I can not reach my destination. I will take another order. If you have one, please raise your hand.", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:30 y:320 OperatableStateMachine.add('Action_findPersonByID', self.use_behavior(Action_findPersonByIDSM, 'go to the barman/Action_findPersonByID', default_keys=['className']), transitions={'found': 'finished', 'not_found': 'finished'}, autonomy={'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit}, remapping={'personID': 'barmanID', 'personEntity': 'personEntity'}) # x:115 y:230 OperatableStateMachine.add('if barman id is 0', CheckConditionState(predicate=lambda x: x == 0), transitions={'true': 'finished', 'false': 'Action_findPersonByID'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'barmanID'}) # x:898 y:561, x:871 y:123 _sm_ask_and_save_order_14 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['customerID'], output_keys=['orderList']) with _sm_ask_and_save_order_14: # x:70 y:112 OperatableStateMachine.add('say ready', SaraSay(sentence="Hello.", input_keys=[], emotion=0, block=True), transitions={'done': 'set question'}, autonomy={'done': Autonomy.Off}) # x:69 y:187 OperatableStateMachine.add('set question', SetKey(Value="What do you want to order?"), transitions={'done': 'ask AND look person'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'question'}) # x:411 y:138 OperatableStateMachine.add('say cannot', SaraSay(sentence="I am not able to understand your order. I will take another order.", input_keys=[], emotion=0, block=True), transitions={'done': 'set question'}, autonomy={'done': Autonomy.Off}) # x:59 y:272 OperatableStateMachine.add('ask AND look person', _sm_ask_and_look_person_4, transitions={'finished': 'nlu restaurant', 'failed': 'say cannot'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'personID': 'customerID', 'question': 'question', 'answer': 'answer'}) # x:411 y:453 OperatableStateMachine.add('nlu restaurant', SaraNLUrestaurant(), transitions={'understood': 'confirm order', 'fail': 'say cannot'}, autonomy={'understood': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'sentence': 'answer', 'orderList': 'orderList'}) # x:657 y:417 OperatableStateMachine.add('confirm order', _sm_confirm_order_3, transitions={'finished': 'say bring', 'failed': 'set question'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'orderList': 'orderList'}) # x:852 y:402 OperatableStateMachine.add('say bring', SaraSay(sentence=lambda x: "understood. I will bring you the "+str(x[0][0].data), input_keys=["orderList"], emotion=0, block=False), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:677 y:318, x:735 y:160 _sm_move_to_table_and_save_position_15 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['customerPosition'], output_keys=['robotPositionToCustomer']) with _sm_move_to_table_and_save_position_15: # x:69 y:24 OperatableStateMachine.add('set distance to person', SetKey(Value=0.7), transitions={'done': 'compute robot pose to customer'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distanceToPerson'}) # x:344 y:115 OperatableStateMachine.add('say cannot', SaraSay(sentence="I can not reach my destination. I will take another order. If you have one, please raise your hand.", input_keys=[], emotion=0, block=True), transitions={'done': 'save pose to not get flexbe problems'}, autonomy={'done': Autonomy.Off}) # x:497 y:129 OperatableStateMachine.add('save pose to not get flexbe problems', Get_Robot_Pose(), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'pose'}) # x:47 y:98 OperatableStateMachine.add('compute robot pose to customer', Get_Reacheable_Waypoint(), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'customerPosition', 'distance': 'distanceToPerson', 'pose_out': 'robotPositionToCustomer'}) # x:78 y:199 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'move to table and save position/Action_Move'), transitions={'finished': 'finished', 'failed': 'say cannot'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'robotPositionToCustomer'}) # x:112 y:525, x:363 y:46 _sm_detect_people_waving_16 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['customerPosition', 'customerID']) with _sm_detect_people_waving_16: # x:84 y:25 OperatableStateMachine.add('say looking', SaraSay(sentence="I am now looking for customers. If you want to order something, please raise your hand.", input_keys=[], emotion=0, block=False), transitions={'done': 'Look for waving people'}, autonomy={'done': Autonomy.Off}) # x:73 y:375 OperatableStateMachine.add('reachable position_2', Get_Reacheable_Waypoint(), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'customerPosition', 'distance': 'distance', 'pose_out': 'customerPosition'}) # x:71 y:291 OperatableStateMachine.add('set distance to customer_2', SetKey(Value=0.5), transitions={'done': 'reachable position_2'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:94 y:216 OperatableStateMachine.add('get attributes', GetAttribute(attributes=["ID", "position"]), transitions={'done': 'set distance to customer_2'}, autonomy={'done': Autonomy.Off}, remapping={'object': 'waving_person', 'ID': 'customerID', 'position': 'customerPosition'}) # x:63 y:120 OperatableStateMachine.add('Look for waving people', _sm_look_for_waving_people_9, transitions={'finished': 'get attributes', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'waving_person': 'waving_person'}) # x:874 y:499, x:747 y:100 _sm_save_bar_position_and_initiation_17 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['barPosition', 'commandNumber', 'barmanID']) with _sm_save_bar_position_and_initiation_17: # x:66 y:96 OperatableStateMachine.add('set question barman', SetKey(Value="Are you the bartender?"), transitions={'done': 'Action_FindPersonByQuestion'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'questionBarman'}) # x:488 y:85 OperatableStateMachine.add('say failed to find the barman', SaraSay(sentence="I am not able to find the barman. I will get the order and come back here to get the items.", input_keys=[], emotion=0, block=True), transitions={'done': 'barman ID to 0'}, autonomy={'done': Autonomy.Off}) # x:83 y:406 OperatableStateMachine.add('save barman ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'get current pose'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'personFound', 'output_value': 'barmanID'}) # x:512 y:183 OperatableStateMachine.add('barman ID to 0', SetKey(Value=0), transitions={'done': 'get current pose'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'barmanID'}) # x:488 y:391 OperatableStateMachine.add('get current pose', Get_Robot_Pose(), transitions={'done': 'set first command'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'barPosition'}) # x:55 y:214 OperatableStateMachine.add('Action_FindPersonByQuestion', self.use_behavior(Action_FindPersonByQuestionSM, 'save bar position and initiation/Action_FindPersonByQuestion'), transitions={'found': 'save barman ID', 'failed': 'say failed to find the barman', 'not_found': 'say failed to find the barman'}, autonomy={'found': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'not_found': Autonomy.Inherit}, remapping={'question': 'questionBarman', 'entityFound': 'personFound'}) # x:489 y:478 OperatableStateMachine.add('set first command', SetKey(Value=1), transitions={'done': 'say instructions'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'commandNumber'}) # x:689 y:477 OperatableStateMachine.add('say instructions', SaraSay(sentence="Good! I wil now start to serve people.", input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:30 y:40 OperatableStateMachine.add('save bar position and initiation', _sm_save_bar_position_and_initiation_17, transitions={'finished': 'detect people waving', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'barPosition': 'barPosition', 'commandNumber': 'commandNumber', 'barmanID': 'barmanID'}) # x:131 y:141 OperatableStateMachine.add('detect people waving', _sm_detect_people_waving_16, transitions={'finished': 'move to table and save position', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'customerPosition': 'customerPosition', 'customerID': 'customerID'}) # x:233 y:250 OperatableStateMachine.add('move to table and save position', _sm_move_to_table_and_save_position_15, transitions={'finished': 'ask and save order', 'failed': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'customerPosition': 'customerPosition', 'robotPositionToCustomer': 'robotPositionToCustomer'}) # x:286 y:337 OperatableStateMachine.add('ask and save order', _sm_ask_and_save_order_14, transitions={'finished': 'go to the barman', 'failed': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'customerID': 'customerID', 'orderList': 'orderList'}) # x:264 y:434 OperatableStateMachine.add('go to the barman', _sm_go_to_the_barman_13, transitions={'finished': 'get the order', 'failed': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'barPosition': 'barPosition', 'barmanID': 'barmanID'}) # x:278 y:526 OperatableStateMachine.add('get the order', _sm_get_the_order_12, transitions={'finished': 'take objects and bring the order to customer', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'orderList': 'orderList'}) # x:260 y:626 OperatableStateMachine.add('take objects and bring the order to customer', _sm_take_objects_and_bring_the_order_to_customer_11, transitions={'finished': 'repeate if first commande', 'failed': 'repeate if first commande'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'barPosition': 'barPosition', 'orderList': 'orderList', 'robotPositionToCustomer': 'robotPositionToCustomer'}) # x:118 y:735 OperatableStateMachine.add('repeate if first commande', _sm_repeate_if_first_commande_10, transitions={'finished': 'say finish', 'repeate': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'repeate': Autonomy.Inherit}, remapping={'commandNumber': 'commandNumber'}) # x:431 y:743 OperatableStateMachine.add('say finish', SaraSay(sentence="I reach the end of my scenario. I will go back to the bar.", input_keys=[], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:616 y:735 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'Action_Move'), transitions={'finished': 'finished', 'failed': 'finished'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'barPosition'}) return _state_machine
def create(self): arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM handle_down_affordance = 'turnCW' if self.hand_side == 'left' else 'turnCCW' door_affordance = 'push' handle_up_affordance = 'turnCCW' if self.hand_side == 'left' else 'turnCW' turn_threshold = 0.6 # x:433 y:590, x:333 y:340 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.grasp_preference = 0 _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.none = None _state_machine.userdata.waypoint_distance = 1.5 # meters # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:733 y:390, x:333 y:40 _sm_hand_back_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'none']) with _sm_hand_back_0: # x:84 y:28 OperatableStateMachine.add('Init_Grasp_Preference', CalculationState(calculation=lambda x: 0), transitions={'done': 'Get_Grasp_Pose'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'none', 'output_value': 'grasp_preference'}) # x:380 y:128 OperatableStateMachine.add('Inform_Pregrasp_Failed', LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:476 y:378 OperatableStateMachine.add('Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'Increase_Preference_Index'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:73 y:428 OperatableStateMachine.add('Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Get_Grasp_Pose'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:191 y:328 OperatableStateMachine.add('Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_Back_To_Pregrasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_waypoints'}) # x:344 y:189 OperatableStateMachine.add('Plan_Back_To_Pregrasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Pregrasp_Pose', 'incomplete': 'Move_To_Pregrasp_Pose', 'failed': 'failed'}, autonomy={'planned': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'waypoints': 'pregrasp_waypoints', 'hand': 'hand_side', 'frame_id': 'pregrasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:196 y:228 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_frame_id'}) # x:50 y:124 OperatableStateMachine.add('Get_Grasp_Pose', GetTemplateGraspState(), transitions={'done': 'Extract_Frame_Id', 'failed': 'Inform_Pregrasp_Failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'grasp': 'pregrasp_pose'}) # x:1041 y:400, x:987 y:18 _sm_unlock_door_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['handle_template_id', 'hand_side', 'none', 'door_template_id'], output_keys=['turn_fraction']) with _sm_unlock_door_1: # x:64 y:28 OperatableStateMachine.add('Get_Handle_Affordance_Down', GetTemplateAffordanceState(identifier=handle_down_affordance), transitions={'done': 'Plan_Turn_Handle_Down', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'handle_template_id', 'hand_side': 'hand_side', 'affordance': 'handle_affordance'}) # x:992 y:178 OperatableStateMachine.add('Plan_Push_Door', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Push_Door', 'incomplete': 'Push_Door', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'affordance': 'door_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:973 y:78 OperatableStateMachine.add('Get_Push_Affordance', GetTemplateAffordanceState(identifier=door_affordance), transitions={'done': 'Plan_Push_Door', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'door_template_id', 'hand_side': 'hand_side', 'affordance': 'door_affordance'}) # x:277 y:78 OperatableStateMachine.add('Plan_Turn_Handle_Down', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Store_Turn_Down', 'incomplete': 'Decide_Execute_Incomplete', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'affordance': 'handle_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:676 y:78 OperatableStateMachine.add('Turn_Handle', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Get_Push_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:976 y:278 OperatableStateMachine.add('Push_Door', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:71 y:263 OperatableStateMachine.add('Get_Handle_Affordance_Up', GetTemplateAffordanceState(identifier=handle_up_affordance), transitions={'done': 'Plan_Turn_Handle_Up', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'handle_template_id', 'hand_side': 'hand_side', 'affordance': 'handle_affordance'}) # x:284 y:270 OperatableStateMachine.add('Plan_Turn_Handle_Up', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Store_Turn_Up', 'incomplete': 'Store_Turn_Up', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'affordance': 'handle_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:70 y:178 OperatableStateMachine.add('Decide_Execute_Incomplete', DecisionState(outcomes=["up", "down"], conditions=lambda x: "up" if x > turn_threshold else "down"), transitions={'up': 'Store_Turn_Down', 'down': 'Get_Handle_Affordance_Up'}, autonomy={'up': Autonomy.Low, 'down': Autonomy.Low}, remapping={'input_value': 'plan_fraction'}) # x:494 y:78 OperatableStateMachine.add('Store_Turn_Down', CalculationState(calculation=lambda x: x), transitions={'done': 'Turn_Handle'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'plan_fraction', 'output_value': 'turn_fraction'}) # x:501 y:178 OperatableStateMachine.add('Store_Turn_Up', CalculationState(calculation=lambda x: -x), transitions={'done': 'Turn_Handle'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'plan_fraction', 'output_value': 'turn_fraction'}) # x:783 y:490, x:133 y:390 _sm_release_handle_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none', 'turn_fraction']) with _sm_release_handle_2: # x:33 y:120 OperatableStateMachine.add('Decide_Turn_Direction', DecisionState(outcomes=["up", "down"], conditions=lambda x: "up" if x > 0 else "down"), transitions={'up': 'Get_Handle_Affordance_Up', 'down': 'Get_Handle_Affordance_Down'}, autonomy={'up': Autonomy.Low, 'down': Autonomy.Low}, remapping={'input_value': 'turn_fraction'}) # x:744 y:372 OperatableStateMachine.add('Hand_Back', _sm_hand_back_0, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'none': 'none'}) # x:742 y:128 OperatableStateMachine.add('Plan_Turn_Handle', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Turn_Handle', 'incomplete': 'Turn_Handle', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'affordance': 'handle_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:926 y:128 OperatableStateMachine.add('Turn_Handle', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Open_Fingers', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:932 y:228 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=0.0), transitions={'done': 'Decide_Retract_Hand', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'hand_side': 'hand_side'}) # x:455 y:128 OperatableStateMachine.add('Reduce_Affordance_Displacement', FlexibleCalculationState(calculation=self.scale_affordance, input_keys=["affordance", "fraction"]), transitions={'done': 'Plan_Turn_Handle'}, autonomy={'done': Autonomy.Off}, remapping={'affordance': 'handle_affordance', 'fraction': 'turn_fraction', 'output_value': 'scaled_affordance'}) # x:214 y:178 OperatableStateMachine.add('Get_Handle_Affordance_Down', GetTemplateAffordanceState(identifier=handle_down_affordance), transitions={'done': 'Reduce_Affordance_Displacement', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'handle_affordance'}) # x:221 y:78 OperatableStateMachine.add('Get_Handle_Affordance_Up', GetTemplateAffordanceState(identifier=handle_up_affordance), transitions={'done': 'Reduce_Affordance_Displacement', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'handle_affordance'}) # x:936 y:378 OperatableStateMachine.add('Decide_Retract_Hand', OperatorDecisionState(outcomes=['keep', 'back'], hint="Take hand back from handle?", suggestion='keep'), transitions={'keep': 'finished', 'back': 'Hand_Back'}, autonomy={'keep': Autonomy.High, 'back': Autonomy.Full}) # x:124 y:577, x:483 y:290 _sm_traverse_door_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['waypoint_distance', 'none']) with _sm_traverse_door_3: # x:62 y:78 OperatableStateMachine.add('Generate_Traversing_Waypoint', CalculationState(calculation=lambda d: PoseStamped(header=Header(frame_id="pelvis"), pose=Pose(position=Point(x=d), orientation=Quaternion(w=1)))), transitions={'done': 'Convert_Waypoint_Frame'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'waypoint_distance', 'output_value': 'waypoint_pelvis'}) # x:68 y:170 OperatableStateMachine.add('Convert_Waypoint_Frame', GetPoseInFrameState(target_frame='world'), transitions={'done': 'Create_Step_Goal', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'pose_in': 'waypoint_pelvis', 'pose_out': 'waypoint_world'}) # x:77 y:358 OperatableStateMachine.add('Plan_Through_Door', PlanFootstepsState(mode=PlanFootstepsState.MODE_STEP_NO_COLLISION), transitions={'planned': 'Go_Through_Door', 'failed': 'Take_Arms_Side'}, autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:70 y:450 OperatableStateMachine.add('Go_Through_Door', ExecuteStepPlanActionState(), transitions={'finished': 'finished', 'failed': 'Take_Arms_Side'}, autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:77 y:256 OperatableStateMachine.add('Create_Step_Goal', CreateStepGoalState(pose_is_pelvis=True), transitions={'done': 'Plan_Through_Door', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'target_pose': 'waypoint_world', 'step_goal': 'step_goal'}) # x:286 y:333 OperatableStateMachine.add('Take_Arms_Side', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.BOTH_ARMS_SIDES, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Plan_Through_Door', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'side': 'none'}) # x:348 y:609, x:119 y:410 _sm_open_door_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['none', 'handle_template_id', 'door_template_id', 'hand_side']) with _sm_open_door_4: # x:87 y:78 OperatableStateMachine.add('Set_Template_Frame', CalculationState(calculation=lambda x: 'template_tf_' + str(x)), transitions={'done': 'Look_At_Handle_Hand'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'door_template_id', 'output_value': 'template_frame'}) # x:364 y:95 OperatableStateMachine.add('Log_Try_To_Open', LogState(text='Will now try to open', severity=Logger.REPORT_INFO), transitions={'done': 'Unlock_Door'}, autonomy={'done': Autonomy.High}) # x:576 y:273 OperatableStateMachine.add('Release_Handle', _sm_release_handle_2, transitions={'finished': 'Ask_If_Push', 'failed': 'Log_Remove_Hand'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'handle_template_id', 'hand_side': 'hand_side', 'none': 'none', 'turn_fraction': 'turn_fraction'}) # x:777 y:277 OperatableStateMachine.add('Log_Remove_Hand', LogState(text='Remove hand from handle', severity=Logger.REPORT_HINT), transitions={'done': 'Ask_If_Push'}, autonomy={'done': Autonomy.Full}) # x:440 y:196 OperatableStateMachine.add('Unlock_Door', _sm_unlock_door_1, transitions={'finished': 'Ask_for_Retry', 'failed': 'Ask_for_Retry'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'handle_template_id': 'handle_template_id', 'hand_side': 'hand_side', 'none': 'none', 'door_template_id': 'door_template_id', 'turn_fraction': 'turn_fraction'}) # x:597 y:109 OperatableStateMachine.add('Ask_for_Retry', OperatorDecisionState(outcomes=["release_handle", "retry"], hint="Now release handle?", suggestion="release_handle"), transitions={'release_handle': 'Release_Handle', 'retry': 'Log_Try_To_Open'}, autonomy={'release_handle': Autonomy.High, 'retry': Autonomy.Full}) # x:587 y:367 OperatableStateMachine.add('Ask_If_Push', OperatorDecisionState(outcomes=['push', 'grasp_again'], hint='Is the door slightly open?', suggestion='push'), transitions={'push': 'Look_Straight', 'grasp_again': 'Grasp_Handle'}, autonomy={'push': Autonomy.High, 'grasp_again': Autonomy.Full}) # x:283 y:472 OperatableStateMachine.add('Push Door Open', self.use_behavior(PushDoorOpenSM, 'Open_Door/Push Door Open'), transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}) # x:296 y:370 OperatableStateMachine.add('Look_Straight', LookAtTargetState(), transitions={'done': 'Push Door Open'}, autonomy={'done': Autonomy.Off}, remapping={'frame': 'none'}) # x:89 y:272 OperatableStateMachine.add('Grasp_Handle', self.use_behavior(GraspObjectSM, 'Open_Door/Grasp_Handle'), transitions={'finished': 'Log_Try_To_Open', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'handle_template_id'}) # x:83 y:178 OperatableStateMachine.add('Look_At_Handle_Hand', LookAtTargetState(), transitions={'done': 'Grasp_Handle'}, autonomy={'done': Autonomy.Off}, remapping={'frame': 'template_frame'}) with _state_machine: # x:82 y:28 OperatableStateMachine.add('Get_Door_Template_ID', InputState(request=InputState.SELECTED_OBJECT_ID, message="Provide the ID of the DOOR template."), transitions={'received': 'Manipulate_On', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'door_template_id'}) # x:616 y:478 OperatableStateMachine.add('Go_To_Stand', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND), transitions={'changed': 'Traverse_Door', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Low}) # x:81 y:122 OperatableStateMachine.add('Walk_to_Template', self.use_behavior(WalktoTemplateSM, 'Walk_to_Template'), transitions={'finished': 'Manipulate_On', 'failed': 'failed', 'aborted': 'Manipulate_On'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': 'door_template_id'}) # x:316 y:128 OperatableStateMachine.add('Manipulate_On', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Align_Door_Log', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:644 y:122 OperatableStateMachine.add('Open_Door', _sm_open_door_4, transitions={'finished': 'Wait_For_Gather_Data', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'none': 'none', 'handle_template_id': 'door_template_id', 'door_template_id': 'door_template_id', 'hand_side': 'hand_side'}) # x:634 y:278 OperatableStateMachine.add('Wait_For_Gather_Data', LogState(text='Gather data from inside', severity=Logger.REPORT_HINT), transitions={'done': 'Go_To_Stand'}, autonomy={'done': Autonomy.Full}) # x:390 y:472 OperatableStateMachine.add('Traverse_Door', _sm_traverse_door_3, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'waypoint_distance': 'waypoint_distance', 'none': 'none'}) # x:525 y:34 OperatableStateMachine.add('Align_Door_Log', LogState(text="Adjust pose of the door template", severity=Logger.REPORT_HINT), transitions={'done': 'Open_Door'}, autonomy={'done': Autonomy.Full}) return _state_machine
def create(self): warn_threshold = 0.1 # for joint offsets bag_folder_out = "~/ft_calib/ft_logs" initial_mode = "stand" motion_mode = "manipulate" transitiontime = 0.5 settlingtime = 0.5 txtfile_name_left_arm = "~/ft_calib/input/SI_E047_FT_Calib_Arms_l_arm.txt" txtfile_name_right_arm = "~/ft_calib/input/SI_E047_FT_Calib_Arms_r_arm.txt" calibration_chain = ["right_arm"] static_calibration_data = {} # x:783 y:13, x:649 y:73 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.parameter_keys_dict = None _state_machine.userdata.calibration_chain_user = calibration_chain _state_machine.userdata.txtfile_name_left_arm_user = txtfile_name_left_arm _state_machine.userdata.txtfile_name_right_arm_user = txtfile_name_right_arm # Additional creation code can be added inside the following tags # [MANUAL_CREATE] bag_folder_out = os.path.expanduser(bag_folder_out) if not os.path.exists(bag_folder_out): os.makedirs(bag_folder_out) # Create STAND posture trajectory _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory( ) # [/MANUAL_CREATE] # x:861 y:31, x:1047 y:103 _sm_starting_point_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['experiment_name', 'trajectories']) with _sm_starting_point_0: # x:49 y:42 OperatableStateMachine.add( 'Gen_Starting_Name', CalculationState(calculation=lambda en: en + "_starting"), transitions={'done': 'Gen_Starting_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'experiment_name', 'output_value': 'starting_name' }) # x:42 y:231 OperatableStateMachine.add( 'Record_Starting_Point', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_for_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={ 'bagfile_name': 'output_bagfile_starting', 'rosbag_process': 'rosbag_process_starting' }) # x:38 y:330 OperatableStateMachine.add( 'Wait_for_Rosbag_Record', WaitState(wait_time=1.0), transitions={'done': 'Extract_Left_Arm_Part'}, autonomy={'done': Autonomy.Off}) # x:29 y:133 OperatableStateMachine.add( 'Gen_Starting_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join( bag_folder_out, en) + ".bag"), transitions={'done': 'Record_Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'starting_name', 'output_value': 'output_bagfile_starting' }) # x:536 y:47 OperatableStateMachine.add( 'Stop_Recording_Starting_Point', StopRecordLogsState(), transitions={'stopped': 'finished'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:228 y:757 OperatableStateMachine.add( 'Plan_to_Starting_Point_Left_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Right_Arm', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_left_arm', 'plan_to_goal': 'plan_to_goal_left_arm' }) # x:236 y:655 OperatableStateMachine.add( 'Plan_to_Starting_Point_Right_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Left_Leg', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_right_arm', 'plan_to_goal': 'plan_to_goal_right_arm' }) # x:272 y:47 OperatableStateMachine.add( 'Go_to_Starting_Point', ExecuteTrajectoryWholeBodyState(controllers=[]), transitions={ 'done': 'Stop_Recording_Starting_Point', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'trajectories': 'trajectories_all'}) # x:536 y:169 OperatableStateMachine.add( 'Report_Starting_Point_Failure', LogState(text="Failed to plan or go to starting point!", severity=Logger.REPORT_WARN), transitions={'done': 'Stop_Recording_When_Failed'}, autonomy={'done': Autonomy.Full}) # x:34 y:424 OperatableStateMachine.add( 'Extract_Left_Arm_Part', CalculationState( calculation=lambda t: {'left_arm': t['left_arm']} if 'left_arm' in t else None), transitions={'done': 'Extract_Right_Arm_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_left_arm' }) # x:21 y:507 OperatableStateMachine.add( 'Extract_Right_Arm_Part', CalculationState( calculation=lambda t: {'right_arm': t['right_arm']} if 'right_arm' in t else None), transitions={'done': 'Extract_Left_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_right_arm' }) # x:293 y:146 OperatableStateMachine.add( 'Combine_Plans', FlexibleCalculationState(calculation=self.combine_plans, input_keys=[ 'left_arm', 'right_arm', 'left_leg', 'right_leg', 'torso' ]), transitions={'done': 'Go_to_Starting_Point'}, autonomy={'done': Autonomy.Low}, remapping={ 'left_arm': 'plan_to_goal_left_arm', 'right_arm': 'plan_to_goal_right_arm', 'left_leg': 'plan_to_goal_left_leg', 'right_leg': 'plan_to_goal_right_leg', 'torso': 'plan_to_goal_torso', 'output_value': 'trajectories_all' }) # x:789 y:167 OperatableStateMachine.add( 'Stop_Recording_When_Failed', StopRecordLogsState(), transitions={'stopped': 'failed'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:24 y:601 OperatableStateMachine.add( 'Extract_Left_Leg_Part', CalculationState( calculation=lambda t: {'left_leg': t['left_leg']} if 'left_leg' in t else None), transitions={'done': 'Extract_Right_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_left_leg' }) # x:22 y:665 OperatableStateMachine.add( 'Extract_Right_Leg_Part', CalculationState( calculation=lambda t: {'right_leg': t['right_leg']} if 'right_leg' in t else None), transitions={'done': 'Extract_Torso_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_right_leg' }) # x:33 y:765 OperatableStateMachine.add( 'Extract_Torso_Part', CalculationState(calculation=lambda t: {'torso': t['torso']} if 'torso' in t else None), transitions={'done': 'Plan_to_Starting_Point_Left_Arm'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_torso' }) # x:227 y:410 OperatableStateMachine.add( 'Plan_to_Starting_Point_Right_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Torso', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_right_leg', 'plan_to_goal': 'plan_to_goal_right_leg' }) # x:237 y:531 OperatableStateMachine.add( 'Plan_to_Starting_Point_Left_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Right_Leg', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_left_leg', 'plan_to_goal': 'plan_to_goal_left_leg' }) # x:241 y:296 OperatableStateMachine.add( 'Plan_to_Starting_Point_Torso', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Combine_Plans', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_torso', 'plan_to_goal': 'plan_to_goal_torso' }) # x:1090 y:55, x:340 y:59 _sm_execute_individual_trajectory_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['output_bagfile', 'trajectories', 'experiment_name']) with _sm_execute_individual_trajectory_1: # x:124 y:257 OperatableStateMachine.add( 'Start_Video_Logging', VideoLoggingState(command=VideoLoggingState.START, no_video=False, no_bags=True), transitions={'done': 'Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={ 'experiment_name': 'experiment_name', 'description': 'experiment_name' }) # x:484 y:106 OperatableStateMachine.add( 'Stop_Recording_After_Failure', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging_After_Failure'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:727 y:188 OperatableStateMachine.add('Wait_for_Settling', WaitState(wait_time=1.0), transitions={'done': 'Stop_Recording'}, autonomy={'done': Autonomy.Off}) # x:482 y:188 OperatableStateMachine.add( 'Execute_Trajs_from_Bagfile', ExecuteTrajectoryWholeBodyState(controllers=[]), transitions={ 'done': 'Wait_for_Settling', 'failed': 'Stop_Recording_After_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Low }, remapping={'trajectories': 'trajectories'}) # x:903 y:97 OperatableStateMachine.add( 'Stop_Video_Logging', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'experiment_name': 'experiment_name', 'description': 'experiment_name' }) # x:472 y:26 OperatableStateMachine.add( 'Stop_Video_Logging_After_Failure', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={ 'experiment_name': 'experiment_name', 'description': 'experiment_name' }) # x:285 y:378 OperatableStateMachine.add('Starting_Point', _sm_starting_point_0, transitions={ 'finished': 'Record_SysID_Test', 'failed': 'Stop_Video_Logging_After_Failure' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'experiment_name': 'experiment_name', 'trajectories': 'trajectories' }) # x:904 y:187 OperatableStateMachine.add( 'Stop_Recording', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging'}, autonomy={'stopped': Autonomy.Low}, remapping={'rosbag_process': 'rosbag_process'}) # x:494 y:292 OperatableStateMachine.add( 'Wait_For_Rosbag_Record', WaitState(wait_time=1.0), transitions={'done': 'Execute_Trajs_from_Bagfile'}, autonomy={'done': Autonomy.Low}) # x:507 y:384 OperatableStateMachine.add( 'Record_SysID_Test', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_For_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={ 'bagfile_name': 'output_bagfile', 'rosbag_process': 'rosbag_process' }) # x:1267 y:273, x:406 y:121 _sm_perform_calibration_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['output_bagfile', 'experiment_name', 'trajectories'], output_keys=['trajectories_command']) with _sm_perform_calibration_2: # x:136 y:171 OperatableStateMachine.add( 'Go_to_Intermediate_Mode', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Execute_Individual_Trajectory', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Low }) # x:514 y:314 OperatableStateMachine.add( 'Intermediate_Mode_before_exit', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Initial_Mode_before_exit', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:995 y:274 OperatableStateMachine.add( 'Initial_Mode_before_exit', ChangeControlModeActionState(target_mode=initial_mode), transitions={ 'changed': 'finished', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:222 y:296 OperatableStateMachine.add('Execute_Individual_Trajectory', _sm_execute_individual_trajectory_1, transitions={ 'finished': 'Intermediate_Mode_before_exit', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'output_bagfile': 'output_bagfile', 'trajectories': 'trajectories', 'experiment_name': 'experiment_name' }) # x:221 y:562, x:709 y:85 _sm_update_calibration_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'output_bagfile', 'trajectories', 'calibration_chain_key_local' ]) with _sm_update_calibration_3: # x:111 y:94 OperatableStateMachine.add( 'Calculate_Calibration', CalculateForceTorqueCalibration( calibration_chain=calibration_chain, settlingtime=settlingtime, static_calibration_data=static_calibration_data), transitions={ 'done': 'Ask_Perform_Update', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'bag_filename': 'output_bagfile', 'trajectories_command': 'trajectories', 'ft_calib_data': 'ft_calib_data' }) # x:420 y:515 OperatableStateMachine.add( 'Calibration_Successful', LogState(text="Successfully updated calibration offsets.", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:774 y:277 OperatableStateMachine.add( 'Calibration_Failed', LogState(text="Failed to apply calibration offsets!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:122 y:202 OperatableStateMachine.add( 'Ask_Perform_Update', OperatorDecisionState( outcomes=['update', 'no_update'], hint= "Do you want to apply the calculated offsets for calibration?", suggestion=None), transitions={ 'update': 'Generate_Keys_Dict', 'no_update': 'finished' }, autonomy={ 'update': Autonomy.Full, 'no_update': Autonomy.Full }) # x:207 y:308 OperatableStateMachine.add( 'Generate_Keys_Dict', FlexibleCalculationState( calculation=self.create_parameter_keys_dict, input_keys=["input"]), transitions={'done': 'Update_Dynamic_Reconfigure'}, autonomy={'done': Autonomy.Off}, remapping={ 'input': 'calibration_chain_key_local', 'output_value': 'parameter_keys_dict' }) # x:408 y:303 OperatableStateMachine.add( 'Update_Dynamic_Reconfigure', UpdateDynamicParameterImpedanceControllerState( controller_chain=calibration_chain), transitions={ 'updated': 'Calibration_Successful', 'failed': 'Calibration_Failed' }, autonomy={ 'updated': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'parameter_keys': 'parameter_keys_dict', 'parameter_values': 'ft_calib_data' }) with _state_machine: # x:160 y:20 OperatableStateMachine.add( 'Execution_Starting', LogState( text= "Execution has started. Please confirm transition to first state.", severity=Logger.REPORT_HINT), transitions={'done': 'Gen_Experiment_Name'}, autonomy={'done': Autonomy.Full}) # x:685 y:431 OperatableStateMachine.add('Update_Calibration', _sm_update_calibration_3, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'output_bagfile': 'output_bagfile', 'trajectories': 'trajectories_command', 'calibration_chain_key_local': 'calibration_chain_user' }) # x:99 y:533 OperatableStateMachine.add( 'Decision_Perform_Experiment', OperatorDecisionState( outcomes=["record_data", "use_existing"], hint= "Use existing measurement data bag file or record a new one?", suggestion=None), transitions={ 'record_data': 'Perform_Calibration', 'use_existing': 'Update_Calibration' }, autonomy={ 'record_data': Autonomy.Low, 'use_existing': Autonomy.Low }) # x:130 y:185 OperatableStateMachine.add( 'Gen_Experiment_Name', FlexibleCalculationState(calculation=lambda i: 'FTCalib', input_keys=[]), transitions={'done': 'Gen_Output_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'output_value': 'experiment_name'}) # x:134 y:267 OperatableStateMachine.add( 'Gen_Output_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join( bag_folder_out, en) + ".bag"), transitions={'done': 'Generate_Textfiles_Dict'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'experiment_name', 'output_value': 'output_bagfile' }) # x:443 y:252 OperatableStateMachine.add('Perform_Calibration', _sm_perform_calibration_2, transitions={ 'finished': 'Update_Calibration', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'output_bagfile': 'output_bagfile', 'experiment_name': 'experiment_name', 'trajectories': 'trajectories_command', 'trajectories_command': 'trajectories_command' }) # x:70 y:421 OperatableStateMachine.add('Load_Traj_From_Txt', GenerateTrajectoryFromTxtfileState( chains=calibration_chain, transitiontime=transitiontime, settlingtime=settlingtime), transitions={ 'done': 'Decision_Perform_Experiment', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'txtfilepaths': 'txtfiles_dict', 'trajectories': 'trajectories_command' }) # x:92 y:350 OperatableStateMachine.add( 'Generate_Textfiles_Dict', FlexibleCalculationState(calculation=self.create_txtfiles_dict, input_keys=[ "calibration_chain", "txtfile_left_arm", "txtfile_right_arm" ]), transitions={'done': 'Load_Traj_From_Txt'}, autonomy={'done': Autonomy.Off}, remapping={ 'calibration_chain': 'calibration_chain_user', 'txtfile_left_arm': 'txtfile_name_left_arm_user', 'txtfile_right_arm': 'txtfile_name_right_arm_user', 'output_value': 'txtfiles_dict' }) return _state_machine
def create(self): joint_names_left = [ "l_arm_shz", "l_arm_shx", "l_arm_ely", "l_arm_elx", "l_arm_wry", "l_arm_wrx" ] joint_names_right = [ "r_arm_shz", "r_arm_shx", "r_arm_ely", "r_arm_elx", "r_arm_wry", "r_arm_wrx" ] wait_time = 3.0 bagfolder = "" # calculated gains_list = { 'pid_gains': ['p', 'i', 'd'], 'bdi_gains': ['k_qd_p', 'ff_qd_d'], 'vigir_gains': ['ff_bang', 'ff_effort', 'ff_friction'] } # x:30 y:365, x:130 y:365 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.joints_left_up = [] # calculated _state_machine.userdata.joints_right_up = [] # calculated _state_machine.userdata.joint_index = 0 _state_machine.userdata.zero_time = [0.02] _state_machine.userdata.joint_positions_up = [] # calculated _state_machine.userdata.joint_positions_down = [] # calculated _state_machine.userdata.joint_index = 0 _state_machine.userdata.none = None _state_machine.userdata.init_time = [3.0] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # 'Basic' configuration for SIMULATION #_state_machine.userdata.joints_left_up = [0.00, 0.18, 1.57, 1.18, 0.00, 0.57] #_state_machine.userdata.joints_right_up = [0.00, -0.18, 1.57, -1.18, 0.00, -0.57] logs_folder = os.path.expanduser('~/joint_control_tests/') if not os.path.exists(logs_folder): os.makedirs(logs_folder) bagfolder = os.path.join(logs_folder, "run_" + time.strftime("%Y-%m-%d-%H_%M")) os.makedirs(bagfolder) self._joint_limits = self._joint_limits_rob if self.real_robot else self._joint_limits_sim # standard config joints_left_up = [0] * 6 for i in range(6): joint_range = self._joint_limits[i][1] - self._joint_limits[i][0] joints_left_up[5 - i] = self._joint_limits[i][0] + joint_range * 0.5 joints_right_up = [0] * 6 for i in range(6): joint_range = self._joint_limits[i + 6][1] - self._joint_limits[i + 6][0] joints_right_up[5 - i] = self._joint_limits[i + 6][0] + joint_range * 0.5 _state_machine.userdata.joints_left_up = joints_left_up _state_machine.userdata.joints_right_up = joints_right_up rospy.loginfo('Average left joint positions: ' + ' '.join(map(str, joints_left_up))) rospy.loginfo('Average right joint positions: ' + ' '.join(map(str, joints_right_up))) # left for i in range(6): joint_config_up = list(_state_machine.userdata.joints_left_up) joint_config_down = list(_state_machine.userdata.joints_left_up) joint_range = self._joint_limits[i][1] - self._joint_limits[i][0] joint_config_up[5 - i] = self._joint_limits[i][ 0] + joint_range * self.joint_upper_bounds joint_config_down[5 - i] = self._joint_limits[i][ 0] + joint_range * self.joint_lower_bounds self._joint_configs_up.append([joint_config_up]) self._joint_configs_down.append([joint_config_down]) rospy.loginfo('Left Joint Config Up: ' + ' '.join(map(str, joint_config_up))) rospy.loginfo('Left Joint Config Dn: ' + ' '.join(map(str, joint_config_down))) # right for i in range(6): joint_config_up = list(_state_machine.userdata.joints_right_up) joint_config_down = list(_state_machine.userdata.joints_right_up) joint_range = self._joint_limits[i + 6][1] - self._joint_limits[i + 6][0] joint_config_up[5 - i] = self._joint_limits[ i + 6][0] + joint_range * self.joint_upper_bounds joint_config_down[5 - i] = self._joint_limits[ i + 6][0] + joint_range * self.joint_lower_bounds self._joint_configs_up.append([joint_config_up]) self._joint_configs_down.append([joint_config_down]) rospy.loginfo('Right Joint Config Up: ' + ' '.join(map(str, joint_config_up))) rospy.loginfo('Right Joint Config Dn: ' + ' '.join(map(str, joint_config_down))) # [/MANUAL_CREATE] # x:30 y:365, x:130 y:365 _sm_move_joint_down_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'joint_index', 'joint_positions_down', 'zero_time', 'joints_right_up', 'joints_left_up', 'init_time' ]) with _sm_move_joint_down_0: # x:71 y:145 OperatableStateMachine.add( 'Move_Left_Arm_Back', MoveitMoveGroupState(planning_group="l_arm_group", joint_names=joint_names_left), transitions={ 'reached': 'Move_Right_Arm_Back', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Low, 'failed': Autonomy.High }, remapping={'target_joint_config': 'joints_left_up'}) # x:639 y:69 OperatableStateMachine.add( 'Move_Left_Joint_Down', ExecuteTrajectoryState( controller=ExecuteTrajectoryState.CONTROLLER_LEFT_ARM, joint_names=joint_names_left), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Off }, remapping={ 'joint_positions': 'joint_positions_down', 'time': 'init_time' }) # x:631 y:200 OperatableStateMachine.add( 'Move_Right_Joint_Down', ExecuteTrajectoryState( controller=ExecuteTrajectoryState.CONTROLLER_RIGHT_ARM, joint_names=joint_names_right), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.High, 'failed': Autonomy.Off }, remapping={ 'joint_positions': 'joint_positions_down', 'time': 'init_time' }) # x:201 y:54 OperatableStateMachine.add( 'Move_Right_Arm_Back', MoveitMoveGroupState(planning_group="r_arm_group", joint_names=joint_names_right), transitions={ 'reached': 'Decide_Left_Or_Right', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Low, 'failed': Autonomy.High }, remapping={'target_joint_config': 'joints_right_up'}) # x:429 y:62 OperatableStateMachine.add( 'Decide_Left_Or_Right', DecisionState(outcomes=["left", "right"], conditions=lambda it: "left" if it < 6 else "right"), transitions={ 'left': 'Move_Left_Joint_Down', 'right': 'Move_Right_Joint_Down' }, autonomy={ 'left': Autonomy.High, 'right': Autonomy.High }, remapping={'input_value': 'joint_index'}) # x:30 y:365, x:130 y:365 _sm_perform_gain_test_right_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'joint_positions_up', 'joint_positions_down', 'zero_time' ]) with _sm_perform_gain_test_right_1: # x:84 y:39 OperatableStateMachine.add('Initial_Wait', WaitState(wait_time=wait_time), transitions={'done': 'Perform_Step_Up'}, autonomy={'done': Autonomy.Off}) # x:80 y:218 OperatableStateMachine.add( 'Wait_Up', WaitState(wait_time=wait_time), transitions={'done': 'Perform_Step_Down'}, autonomy={'done': Autonomy.Off}) # x:44 y:331 OperatableStateMachine.add( 'Perform_Step_Down', ExecuteTrajectoryState( controller=ExecuteTrajectoryState.CONTROLLER_RIGHT_ARM, joint_names=joint_names_right), transitions={ 'done': 'Wait_Down', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'joint_positions': 'joint_positions_down', 'time': 'zero_time' }) # x:73 y:440 OperatableStateMachine.add( 'Wait_Down', WaitState(wait_time=wait_time), transitions={'done': 'Perforn_Step_Up_2'}, autonomy={'done': Autonomy.Off}) # x:414 y:401 OperatableStateMachine.add( 'Perforn_Step_Up_2', ExecuteTrajectoryState( controller=ExecuteTrajectoryState.CONTROLLER_RIGHT_ARM, joint_names=joint_names_right), transitions={ 'done': 'Wait_Up_2', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'joint_positions': 'joint_positions_up', 'time': 'zero_time' }) # x:442 y:291 OperatableStateMachine.add( 'Wait_Up_2', WaitState(wait_time=wait_time), transitions={'done': 'Perform_Step_Down_2'}, autonomy={'done': Autonomy.Off}) # x:416 y:167 OperatableStateMachine.add( 'Perform_Step_Down_2', ExecuteTrajectoryState( controller=ExecuteTrajectoryState.CONTROLLER_RIGHT_ARM, joint_names=joint_names_right), transitions={ 'done': 'Wait_Down_2', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'joint_positions': 'joint_positions_down', 'time': 'zero_time' }) # x:449 y:62 OperatableStateMachine.add('Wait_Down_2', WaitState(wait_time=wait_time), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:48 y:113 OperatableStateMachine.add( 'Perform_Step_Up', ExecuteTrajectoryState( controller=ExecuteTrajectoryState.CONTROLLER_RIGHT_ARM, joint_names=joint_names_right), transitions={ 'done': 'Wait_Up', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'joint_positions': 'joint_positions_up', 'time': 'zero_time' }) # x:30 y:365, x:130 y:365 _sm_perform_gain_test_left_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'joint_positions_up', 'joint_positions_down', 'zero_time' ]) with _sm_perform_gain_test_left_2: # x:84 y:39 OperatableStateMachine.add( 'Initial_Wait', WaitState(wait_time=wait_time), transitions={'done': 'Perform_Step_Up_1'}, autonomy={'done': Autonomy.Off}) # x:87 y:232 OperatableStateMachine.add( 'Wait_Up_1', WaitState(wait_time=wait_time), transitions={'done': 'Perform_Step_Down_1'}, autonomy={'done': Autonomy.Off}) # x:50 y:321 OperatableStateMachine.add( 'Perform_Step_Down_1', ExecuteTrajectoryState( controller=ExecuteTrajectoryState.CONTROLLER_LEFT_ARM, joint_names=joint_names_left), transitions={ 'done': 'Wait_Down_1', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'joint_positions': 'joint_positions_down', 'time': 'zero_time' }) # x:77 y:415 OperatableStateMachine.add( 'Wait_Down_1', WaitState(wait_time=wait_time), transitions={'done': 'Perforn_Step_Up_2'}, autonomy={'done': Autonomy.Off}) # x:51 y:131 OperatableStateMachine.add( 'Perform_Step_Up_1', ExecuteTrajectoryState( controller=ExecuteTrajectoryState.CONTROLLER_LEFT_ARM, joint_names=joint_names_left), transitions={ 'done': 'Wait_Up_1', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'joint_positions': 'joint_positions_up', 'time': 'zero_time' }) # x:414 y:401 OperatableStateMachine.add( 'Perforn_Step_Up_2', ExecuteTrajectoryState( controller=ExecuteTrajectoryState.CONTROLLER_LEFT_ARM, joint_names=joint_names_left), transitions={ 'done': 'Wait_Up_2', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'joint_positions': 'joint_positions_up', 'time': 'zero_time' }) # x:442 y:291 OperatableStateMachine.add( 'Wait_Up_2', WaitState(wait_time=wait_time), transitions={'done': 'Perform_Step_Down_2'}, autonomy={'done': Autonomy.Off}) # x:416 y:167 OperatableStateMachine.add( 'Perform_Step_Down_2', ExecuteTrajectoryState( controller=ExecuteTrajectoryState.CONTROLLER_LEFT_ARM, joint_names=joint_names_left), transitions={ 'done': 'Wait_Down_2', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'joint_positions': 'joint_positions_down', 'time': 'zero_time' }) # x:449 y:62 OperatableStateMachine.add('Wait_Down_2', WaitState(wait_time=wait_time), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:30 y:365, x:130 y:365 _sm_test_individual_joint_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'joint_positions_up', 'joint_positions_down', 'joint_index', 'traj_controller', 'none', 'zero_time', 'joints_right_up', 'joints_left_up', 'init_time' ]) with _sm_test_individual_joint_3: # x:45 y:60 OperatableStateMachine.add( 'Initialize_Iteration', CalculationState(calculation=lambda x: 0), transitions={'done': 'Move_Joint_Down'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'none', 'output_value': 'iteration' }) # x:520 y:555 OperatableStateMachine.add('Perform_Gain_Test_Left', _sm_perform_gain_test_left_2, transitions={ 'finished': 'Stop_Gain_Logs', 'failed': 'Stop_Gain_Logs' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'joint_positions_up': 'joint_positions_up', 'joint_positions_down': 'joint_positions_down', 'zero_time': 'zero_time' }) # x:176 y:388 OperatableStateMachine.add( 'Decide_If_Tests_To_Go', DecisionState(outcomes=["done", "continue"], conditions=lambda it: "done" if it == 5 else "continue"), transitions={ 'done': 'Reset_Joint_Gains', 'continue': 'Calculate_Next_Gain_Value' }, autonomy={ 'done': Autonomy.Off, 'continue': Autonomy.Off }, remapping={'input_value': 'iteration'}) # x:144 y:298 OperatableStateMachine.add( 'Calculate_Next_Gain_Value', FlexibleCalculationState( calculation=self.calculate_gains, input_keys=["iteration", "nominal_gain"]), transitions={'done': 'Set_Joint_Gain'}, autonomy={'done': Autonomy.Off}, remapping={ 'iteration': 'iteration', 'nominal_gain': 'nominal_gains', 'output_value': 'altered_gains' }) # x:395 y:268 OperatableStateMachine.add( 'Set_Joint_Gain', UpdateDynamicParameterState(param=gains_list), transitions={ 'updated': 'Set_Logfile_Name', 'failed': 'failed' }, autonomy={ 'updated': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'traj_controller': 'traj_controller', 'parameter_value': 'altered_gains' }) # x:190 y:193 OperatableStateMachine.add( 'Get_Joint_Gains', ReadDynamicParameterState(param=gains_list), transitions={ 'read': 'Calculate_Next_Gain_Value', 'failed': 'failed' }, autonomy={ 'read': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'traj_controller': 'traj_controller', 'parameter_value': 'nominal_gains' }) # x:158 y:505 OperatableStateMachine.add( 'Increment_Iteration_Counter', CalculationState(calculation=lambda it: it + 1), transitions={'done': 'Decide_If_Tests_To_Go'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'iteration', 'output_value': 'iteration' }) # x:798 y:435 OperatableStateMachine.add( 'Decide_Left_Or_Right', DecisionState(outcomes=["left", "right"], conditions=lambda it: "left" if it < 6 else "right"), transitions={ 'left': 'Perform_Gain_Test_Left', 'right': 'Perform_Gain_Test_Right' }, autonomy={ 'left': Autonomy.High, 'right': Autonomy.High }, remapping={'input_value': 'joint_index'}) # x:811 y:624 OperatableStateMachine.add('Perform_Gain_Test_Right', _sm_perform_gain_test_right_1, transitions={ 'finished': 'Stop_Gain_Logs', 'failed': 'Stop_Gain_Logs' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'joint_positions_up': 'joint_positions_up', 'joint_positions_down': 'joint_positions_down', 'zero_time': 'zero_time' }) # x:545 y:458 OperatableStateMachine.add( 'Start_Gain_Logs', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Decide_Left_Or_Right'}, autonomy={'logging': Autonomy.Off}, remapping={ 'bagfile_name': 'bagfile_name', 'rosbag_process': 'rosbag_process' }) # x:184 y:616 OperatableStateMachine.add( 'Stop_Gain_Logs', StopRecordLogsState(), transitions={'stopped': 'Increment_Iteration_Counter'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:576 y:346 OperatableStateMachine.add( 'Set_Logfile_Name', FlexibleCalculationState( calculation=lambda i: bagfolder + self._traj_controllers[i[ 1]][1] + "_k_p_" + str(i[0][0]) + ".bag", input_keys=["gain_percentage", "joint_index"]), transitions={'done': 'Start_Gain_Logs'}, autonomy={'done': Autonomy.Off}, remapping={ 'gain_percentage': 'altered_gains', 'joint_index': 'joint_index', 'output_value': 'bagfile_name' }) # x:210 y:53 OperatableStateMachine.add('Move_Joint_Down', _sm_move_joint_down_0, transitions={ 'finished': 'Get_Joint_Gains', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'joint_index': 'joint_index', 'joint_positions_down': 'joint_positions_down', 'zero_time': 'zero_time', 'joints_right_up': 'joints_right_up', 'joints_left_up': 'joints_left_up', 'init_time': 'init_time' }) # x:365 y:430 OperatableStateMachine.add( 'Reset_Joint_Gains', UpdateDynamicParameterState(param=gains_list), transitions={ 'updated': 'finished', 'failed': 'failed' }, autonomy={ 'updated': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'traj_controller': 'traj_controller', 'parameter_value': 'nominal_gains' }) # x:30 y:365, x:130 y:365 _sm_test_joint_controls_4 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'joint_index', 'none', 'zero_time', 'joints_right_up', 'joints_left_up', 'init_time' ]) with _sm_test_joint_controls_4: # x:47 y:121 OperatableStateMachine.add( 'Decide_Joints_To_Go', DecisionState( outcomes=["done", "continue"], conditions=lambda idx: "done" if idx == len(self._joint_configs_down) else "continue"), transitions={ 'done': 'finished', 'continue': 'Select_Next_Joint_Up' }, autonomy={ 'done': Autonomy.High, 'continue': Autonomy.Low }, remapping={'input_value': 'joint_index'}) # x:257 y:290 OperatableStateMachine.add( 'Select_Next_Joint_Up', CalculationState( calculation=lambda idx: self._joint_configs_up[idx]), transitions={'done': 'Select_Next_Joint_Down'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_index', 'output_value': 'joint_positions_up' }) # x:571 y:68 OperatableStateMachine.add('Test_Individual_Joint', _sm_test_individual_joint_3, transitions={ 'finished': 'Increment_Joint_Index', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'joint_positions_up': 'joint_positions_up', 'joint_positions_down': 'joint_positions_down', 'joint_index': 'joint_index', 'traj_controller': 'traj_controller', 'none': 'none', 'zero_time': 'zero_time', 'joints_right_up': 'joints_right_up', 'joints_left_up': 'joints_left_up', 'init_time': 'init_time' }) # x:529 y:324 OperatableStateMachine.add( 'Select_Next_Joint_Down', CalculationState( calculation=lambda idx: self._joint_configs_down[idx]), transitions={'done': 'Set_Trajectory_Controller'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_index', 'output_value': 'joint_positions_down' }) # x:222 y:51 OperatableStateMachine.add( 'Increment_Joint_Index', CalculationState(calculation=lambda it: it + 1), transitions={'done': 'Decide_Joints_To_Go'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_index', 'output_value': 'joint_index' }) # x:559 y:189 OperatableStateMachine.add( 'Set_Trajectory_Controller', CalculationState( calculation=lambda idx: self._traj_controllers[idx]), transitions={'done': 'Test_Individual_Joint'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_index', 'output_value': 'traj_controller' }) with _state_machine: # x:112 y:38 OperatableStateMachine.add( 'Check_Initial_Stand', CheckCurrentControlModeState( target_mode=CheckCurrentControlModeState.STAND, wait=False), transitions={ 'correct': 'Switch_To_Manipulate', 'incorrect': 'Set_Initial_Stand' }, autonomy={ 'correct': Autonomy.Low, 'incorrect': Autonomy.Low }, remapping={'control_mode': 'control_mode'}) # x:336 y:123 OperatableStateMachine.add( 'Set_Initial_Stand', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'Switch_To_Manipulate', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:60 y:235 OperatableStateMachine.add( 'Switch_To_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'Bring_Left_Arm_Up', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:105 y:428 OperatableStateMachine.add( 'Bring_Left_Arm_Up', MoveitMoveGroupState(planning_group="l_arm_group", joint_names=joint_names_left), transitions={ 'reached': 'Bring_Right_Arm_Up', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Low, 'failed': Autonomy.High }, remapping={'target_joint_config': 'joints_left_up'}) # x:323 y:482 OperatableStateMachine.add( 'Bring_Right_Arm_Up', MoveitMoveGroupState(planning_group="r_arm_group", joint_names=joint_names_right), transitions={ 'reached': 'Test_Joint_Controls', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.High, 'failed': Autonomy.High }, remapping={'target_joint_config': 'joints_right_up'}) # x:620 y:465 OperatableStateMachine.add('Test_Joint_Controls', _sm_test_joint_controls_4, transitions={ 'finished': 'Change_Back_To_Stand', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'joint_index': 'joint_index', 'none': 'none', 'zero_time': 'zero_time', 'joints_right_up': 'joints_right_up', 'joints_left_up': 'joints_left_up', 'init_time': 'init_time' }) # x:831 y:349 OperatableStateMachine.add( 'Change_Back_To_Stand', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'finished', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.Off }) return _state_machine
def create(self): # x:102 y:572, x:889 y:107 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.Pose1 = "PostGripPose" _state_machine.userdata.Pose2 = "IdlePose" _state_machine.userdata.actionList = [["Find", "bottle"], ["move", "kitchen"]] _state_machine.userdata.titre = "test" _state_machine.userdata.relative = False _state_machine.userdata.pitch = -0.8 _state_machine.userdata.Action1 = ["move", "counter"] _state_machine.userdata.Action2 = ["move", "table"] _state_machine.userdata.pose = "Dining room" _state_machine.userdata.say1 = "say one" _state_machine.userdata.say2 = "say two" _state_machine.userdata.index = -1 _state_machine.userdata.name = "person" _state_machine.userdata.name2 = "apple" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:28 y:104 OperatableStateMachine.add('set_2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'list'}, autonomy={'done': Autonomy.Off}) # x:190 y:78 OperatableStateMachine.add('calc', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'gen'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity_list', 'output_value': 'entity'}) # x:351 y:116 OperatableStateMachine.add('gen', GenPointedPoints(step=0.3, qty=5), transitions={'done': 'loop', 'not_pointing': 'say 1', 'failed': 'say2'}, autonomy={'done': Autonomy.Off, 'not_pointing': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'entity': 'entity', 'positionList': 'positionList'}) # x:218 y:246 OperatableStateMachine.add('loop', ForLoop(repeat=5), transitions={'do': 'flex calc', 'end': 'say 5'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:493 y:35 OperatableStateMachine.add('say 1', SaraSay(sentence="Point at something", input_keys=[], emotion=0, block=True), transitions={'done': 'list'}, autonomy={'done': Autonomy.Off}) # x:533 y:125 OperatableStateMachine.add('say2', SaraSay(sentence="Failed to generate points", input_keys=[], emotion=0, block=True), transitions={'done': 'list'}, autonomy={'done': Autonomy.Off}) # x:377 y:255 OperatableStateMachine.add('flex calc', FlexibleCalculationState(calculation=lambda x: x[0][x[1]], input_keys=["positionList","index"]), transitions={'done': 'look'}, autonomy={'done': Autonomy.Off}, remapping={'positionList': 'positionList', 'index': 'index', 'output_value': 'point'}) # x:554 y:251 OperatableStateMachine.add('look', LookAtPos(), transitions={'failed': 'say 3', 'done': 'list entities'}, autonomy={'failed': Autonomy.Off, 'done': Autonomy.Off}, remapping={'pos': 'point'}) # x:403 y:354 OperatableStateMachine.add('say 3', SaraSay(sentence="I can not look at this point", input_keys=[], emotion=0, block=True), transitions={'done': 'loop'}, autonomy={'done': Autonomy.Off}) # x:116 y:405 OperatableStateMachine.add('say 5', SaraSay(sentence="I didn't see anything", input_keys=[], emotion=0, block=True), transitions={'done': 'set'}, autonomy={'done': Autonomy.Off}) # x:150 y:493 OperatableStateMachine.add('set', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:30 y:40 OperatableStateMachine.add('list', list_entities_by_name(frontality_level=0.5, distance_max=10), transitions={'found': 'calc', 'none_found': 'list'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'}) # x:698 y:310 OperatableStateMachine.add('list entities', list_entities_near_point(radius=2.0), transitions={'found': 'say 4', 'none_found': 'wait'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'name2', 'position': 'point', 'entity_list': 'found_entity_list', 'number': 'number'}) # x:714 y:407 OperatableStateMachine.add('say 4', SaraSay(sentence="I see something.", input_keys=[], emotion=0, block=True), transitions={'done': 'set'}, autonomy={'done': Autonomy.Off}) # x:569 y:346 OperatableStateMachine.add('wait', WaitState(wait_time=3), transitions={'done': 'loop'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:860 y:786, x:837 y:171, x:828 y:43 _state_machine = OperatableStateMachine( outcomes=['found', 'failed', 'not_found'], input_keys=['question'], output_keys=['entityFound']) _state_machine.userdata.question = "" _state_machine.userdata.entityFound = "" _state_machine.userdata.personKey = "person" _state_machine.userdata.index = -1 _state_machine.userdata.rotation180degres = -3.14 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:458, x:130 y:458 _sm_keep_looking_at_person_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['personID']) with _sm_keep_looking_at_person_0: # x:79 y:77 OperatableStateMachine.add('keep looking', KeepLookingAt(), transitions={'failed': 'keep looking'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'personID'}) # x:30 y:458, x:130 y:458 _sm_ask_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['question'], output_keys=['answer']) with _sm_ask_1: # x:57 y:66 OperatableStateMachine.add('wait', WaitState(wait_time=3), transitions={'done': 'Action_Ask'}, autonomy={'done': Autonomy.Off}) # x:46 y:155 OperatableStateMachine.add( 'Action_Ask', self.use_behavior( sara_flexbe_behaviors__Action_AskSM, 'ask while looking at person/ask/Action_Ask'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'question': 'question', 'answer': 'answer' }) # x:871 y:712, x:874 y:127 _sm_get_real_id_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['entity', 'personKey', 'FIFO'], output_keys=['personID']) with _sm_get_real_id_2: # x:47 y:32 OperatableStateMachine.add( 'get posittion face or entity', CalculationState(calculation=lambda x: x.face.boundingBox. Center if x.face.id != '' else x.position), transitions={'done': 'get direction to point'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'pointToLook' }) # x:27 y:351 OperatableStateMachine.add('look at point', SaraSetHeadAngleKey(), transitions={'done': 'wait 1'}, autonomy={'done': Autonomy.Off}, remapping={ 'yaw': 'yaw', 'pitch': 'pitch' }) # x:40 y:423 OperatableStateMachine.add( 'wait 1', WaitState(wait_time=1), transitions={'done': 'list entity in front'}, autonomy={'done': Autonomy.Off}) # x:24 y:491 OperatableStateMachine.add( 'list entity in front', list_entities_by_name(frontality_level=0.5, distance_max=3), transitions={ 'found': 'fitler the entity list to remove id already checked', 'none_found': 'set key' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'personKey', 'entity_list': 'entity_list', 'number': 'number' }) # x:42 y:632 OperatableStateMachine.add( 'calcul first entity ID', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'say real id'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'filteredEntityList', 'output_value': 'personID' }) # x:518 y:578 OperatableStateMachine.add( 'say real id', SaraSay(sentence=lambda x: "The real ID is " + str(x[0]) + ".", input_keys=["personID"], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'personID': 'personID'}) # x:539 y:164 OperatableStateMachine.add('set key', SetKey(Value=0), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'personID'}) # x:34 y:234 OperatableStateMachine.add('pitch to 0.1 if no face', FlexibleCalculationState( calculation=lambda x: x[0] if x[1].face.id != '' else 0.1, input_keys=["pitch", "entity"]), transitions={'done': 'look at point'}, autonomy={'done': Autonomy.Off}, remapping={ 'pitch': 'pitch', 'entity': 'entity', 'output_value': 'pitch' }) # x:37 y:149 OperatableStateMachine.add('get direction to point', Get_direction_to_point( frame_origin="base_link", frame_reference="head_link"), transitions={ 'done': 'pitch to 0.1 if no face', 'fail': 'set key' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'targetPoint': 'pointToLook', 'yaw': 'yaw', 'pitch': 'pitch' }) # x:29 y:560 OperatableStateMachine.add( 'fitler the entity list to remove id already checked', FilterKey(filter_function=lambda x: x[0].ID not in x[1], input_keys=["input_list", "FIFO"]), transitions={ 'not_empty': 'calcul first entity ID', 'empty': 'set key' }, autonomy={ 'not_empty': Autonomy.Off, 'empty': Autonomy.Off }, remapping={ 'input_list': 'entity_list', 'FIFO': 'FIFO', 'output_list': 'filteredEntityList' }) # x:30 y:458, x:710 y:24 _sm_tourne_tete_et_base_3 = OperatableStateMachine( outcomes=['done', 'failed'], input_keys=['index', 'rotation180degres']) with _sm_tourne_tete_et_base_3: # x:57 y:27 OperatableStateMachine.add( 'decide', DecisionState(outcomes=["_0", "_1", "_2", "_3", "_4"], conditions=lambda x: "_" + str(x)), transitions={ '_0': 'look center', '_1': 'look center_2', '_2': 'look center_3', '_3': 'action_turn', '_4': 'look center_5' }, autonomy={ '_0': Autonomy.Off, '_1': Autonomy.Off, '_2': Autonomy.Off, '_3': Autonomy.Off, '_4': Autonomy.Off }, remapping={'input_value': 'index'}) # x:186 y:236 OperatableStateMachine.add('look right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'wait right'}, autonomy={'done': Autonomy.Off}) # x:208 y:307 OperatableStateMachine.add('wait right', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:322 y:109 OperatableStateMachine.add('look center_3', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait center_3'}, autonomy={'done': Autonomy.Off}) # x:325 y:174 OperatableStateMachine.add('wait center_3', WaitState(wait_time=3), transitions={'done': 'look right_2'}, autonomy={'done': Autonomy.Off}) # x:318 y:240 OperatableStateMachine.add('look right_2', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'wait right_2'}, autonomy={'done': Autonomy.Off}) # x:345 y:310 OperatableStateMachine.add('wait right_2', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:489 y:178 OperatableStateMachine.add('look left_2_2', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'waitleft_2_2'}, autonomy={'done': Autonomy.Off}) # x:506 y:247 OperatableStateMachine.add('waitleft_2_2', WaitState(wait_time=3), transitions={'done': 'look center_4'}, autonomy={'done': Autonomy.Off}) # x:501 y:319 OperatableStateMachine.add('look center_4', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait center_4'}, autonomy={'done': Autonomy.Off}) # x:520 y:407 OperatableStateMachine.add('wait center_4', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:476 y:109 OperatableStateMachine.add( 'action_turn', self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'tourne tete et base/action_turn'), transitions={ 'finished': 'look left_2_2', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation180degres'}) # x:199 y:108 OperatableStateMachine.add('look center_2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait center_2'}, autonomy={'done': Autonomy.Off}) # x:51 y:113 OperatableStateMachine.add('look center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait center'}, autonomy={'done': Autonomy.Off}) # x:56 y:174 OperatableStateMachine.add('wait center', WaitState(wait_time=4), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:206 y:173 OperatableStateMachine.add('wait center_2', WaitState(wait_time=2), transitions={'done': 'look right'}, autonomy={'done': Autonomy.Off}) # x:730 y:110 OperatableStateMachine.add('look center_5', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait center_5'}, autonomy={'done': Autonomy.Off}) # x:741 y:193 OperatableStateMachine.add('wait center_5', WaitState(wait_time=3), transitions={'done': 'look right_5'}, autonomy={'done': Autonomy.Off}) # x:733 y:296 OperatableStateMachine.add('look right_5', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'wait right_5'}, autonomy={'done': Autonomy.Off}) # x:731 y:422 OperatableStateMachine.add('wait right_5', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:449 y:45, x:454 y:135, x:447 y:195, x:446 y:252, x:430 y:458, x:530 y:458 _sm_ask_while_looking_at_person_4 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['personID', 'question'], output_keys=['answer'], conditions=[('finished', [('ask', 'finished')]), ('failed', [('ask', 'failed')]), ('finished', [('keep looking at person', 'finished')]), ('failed', [('keep looking at person', 'failed')])]) with _sm_ask_while_looking_at_person_4: # x:97 y:55 OperatableStateMachine.add('ask', _sm_ask_1, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'question': 'question', 'answer': 'answer' }) # x:65 y:197 OperatableStateMachine.add('keep looking at person', _sm_keep_looking_at_person_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'personID': 'personID'}) with _state_machine: # x:30 y:115 OperatableStateMachine.add( 'Init_Sequence', self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'Init_Sequence'), transitions={ 'finished': 'create fifo', 'failed': 'create fifo' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:164 y:62 OperatableStateMachine.add('for loop', ForLoopWithInput(repeat=4), transitions={ 'do': 'tourne tete et base', 'end': 'not_found' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={ 'index_in': 'index', 'index_out': 'index' }) # x:260 y:557 OperatableStateMachine.add('ask while looking at person', _sm_ask_while_looking_at_person_4, transitions={ 'finished': 'answer contains yes', 'failed': 'add id' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'personID': 'personID', 'question': 'question', 'answer': 'answer' }) # x:289 y:480 OperatableStateMachine.add( 'get personID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'ask while looking at person'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'personEntity', 'output_value': 'personID' }) # x:270 y:643 OperatableStateMachine.add( 'answer contains yes', RegexTester( regex=".*((yes)|(Yes)|(yep)|(sure)|(of course)).*"), transitions={ 'true': 'say ty', 'false': 'add id' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'answer', 'result': 'result' }) # x:173 y:260 OperatableStateMachine.add('look center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'for loop'}, autonomy={'done': Autonomy.Off}) # x:372 y:89 OperatableStateMachine.add('tourne tete et base', _sm_tourne_tete_et_base_3, transitions={ 'done': 'get list of person', 'failed': 'not_found' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'index': 'index', 'rotation180degres': 'rotation180degres' }) # x:263 y:322 OperatableStateMachine.add( 'fitler the entity list to remove id already checked', FilterKey(filter_function=lambda x: x[0].ID not in x[1], input_keys=["input_list", "FIFO"]), transitions={ 'not_empty': 'get first entity', 'empty': 'look center' }, autonomy={ 'not_empty': Autonomy.Off, 'empty': Autonomy.Off }, remapping={ 'input_list': 'entityList', 'FIFO': 'FIFO', 'output_list': 'filteredEntityList' }) # x:268 y:388 OperatableStateMachine.add( 'get first entity', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'get real id'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'filteredEntityList', 'output_value': 'personEntity' }) # x:296 y:168 OperatableStateMachine.add( 'get list of person', list_entities_by_name(frontality_level=0.5, distance_max=3), transitions={ 'found': 'fitler the entity list to remove id already checked', 'none_found': 'for loop' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'personKey', 'entity_list': 'entityList', 'number': 'numberOfEntity' }) # x:111 y:533 OperatableStateMachine.add( 'add id', FIFO_Add(), transitions={'done': 'say keep looking'}, autonomy={'done': Autonomy.Off}, remapping={ 'Entry': 'personID', 'FIFO': 'FIFO' }) # x:609 y:651 OperatableStateMachine.add('say ty', SaraSay(sentence="Thank you.", input_keys=[], emotion=0, block=True), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}) # x:113 y:386 OperatableStateMachine.add('say keep looking', SaraSay(sentence="Ok, never mind.", input_keys=[], emotion=0, block=True), transitions={'done': 'for loop'}, autonomy={'done': Autonomy.Off}) # x:472 y:438 OperatableStateMachine.add('get real id', _sm_get_real_id_2, transitions={ 'finished': 'ask while looking at person', 'failed': 'get personID' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'entity': 'personEntity', 'personKey': 'personKey', 'FIFO': 'FIFO', 'personID': 'personID' }) # x:35 y:53 OperatableStateMachine.add('create fifo', FIFO_New(), transitions={'done': 'for loop'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'FIFO'}) return _state_machine
def create(self): # x:814 y:45, x:514 y:274 _state_machine = OperatableStateMachine(outcomes=['Found', 'NotFound'], output_keys=['Operator']) _state_machine.userdata.Operator = None _state_machine.userdata.Name = "person" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:506 y:393, x:515 y:462 _sm_move_to_person_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['Operator']) with _sm_move_to_person_0: # x:30 y:83 OperatableStateMachine.add( 'Getpos', CalculationState(calculation=lambda x: x.position), transitions={'done': 'setDistance'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Operator', 'output_value': 'pose_in' }) # x:35 y:450 OperatableStateMachine.add('Action_Move', self.use_behavior( Action_MoveSM, 'Move to person/Action_Move'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'Pose'}) # x:47 y:368 OperatableStateMachine.add('set not rel', SetKey(Value=False), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'relative'}) # x:41 y:179 OperatableStateMachine.add('setDistance', SetKey(Value=1.5), transitions={'done': 'Close position'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:27 y:280 OperatableStateMachine.add('Close position', Get_Reacheable_Waypoint(), transitions={'done': 'set not rel'}, autonomy={'done': Autonomy.Off}, remapping={ 'pose_in': 'pose_in', 'distance': 'distance', 'pose_out': 'Pose' }) with _state_machine: # x:64 y:35 OperatableStateMachine.add( 'Get previous ID', GetRosParam(ParamName="behavior/Operator/Id"), transitions={ 'done': 'Get Operator', 'failed': 'for 3' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'ID'}) # x:271 y:37 OperatableStateMachine.add('Get Operator', GetEntityByID(), transitions={ 'found': 'Found', 'not_found': 'Say lost operator' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Operator' }) # x:263 y:155 OperatableStateMachine.add('Say lost operator', SaraSay(sentence="I lost my operator", input_keys=[], emotion=1, block=True), transitions={'done': 'for 3'}, autonomy={'done': Autonomy.Off}) # x:780 y:517 OperatableStateMachine.add('ask if operator', SaraSay(sentence="Are you my operator?", input_keys=[], emotion=1, block=True), transitions={'done': 'get speech'}, autonomy={'done': Autonomy.Off}) # x:70 y:273 OperatableStateMachine.add('for 3', ForLoop(repeat=3), transitions={ 'do': 'for 3_2', 'end': 'set None' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:249 y:357 OperatableStateMachine.add('say where are you', SaraSay( sentence="Operator. Where are you?", input_keys=[], emotion=1, block=True), transitions={'done': 'for 3'}, autonomy={'done': Autonomy.Off}) # x:281 y:265 OperatableStateMachine.add('set None', SetKey(Value=None), transitions={'done': 'NotFound'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Operator'}) # x:49 y:511 OperatableStateMachine.add('Get persons', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'get next closest', 'none_found': 'say where are you' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'Name', 'entity_list': 'entity_list', 'number': 'number' }) # x:461 y:475 OperatableStateMachine.add('Move to person', _sm_move_to_person_0, transitions={ 'finished': 'ask if operator', 'failed': 'NotFound' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'Operator': 'Operator'}) # x:783 y:161 OperatableStateMachine.add( 'set new ID', SetRosParam(ParamName="behavior/Operator/Id"), transitions={'done': 'Found'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ID'}) # x:775 y:269 OperatableStateMachine.add( 'get ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'set new ID'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Operator', 'output_value': 'ID' }) # x:784 y:433 OperatableStateMachine.add('get speech', GetSpeech(watchdog=5), transitions={ 'done': 'Yes ?', 'nothing': 'for 3_2', 'fail': 'NotFound' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:69 y:402 OperatableStateMachine.add('for 3_2', ForLoop(repeat=3), transitions={ 'do': 'Get persons', 'end': 'set None' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index2'}) # x:744 y:332 OperatableStateMachine.add( 'Yes ?', CheckConditionState(predicate=lambda x: "yes" in x), transitions={ 'true': 'get ID', 'false': 'for 3_2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'words'}) # x:263 y:535 OperatableStateMachine.add( 'get next closest', FlexibleCalculationState(calculation=lambda x: x[0][x[1]], input_keys=["entity_list", "index"]), transitions={'done': 'ask if operator'}, autonomy={'done': Autonomy.Off}, remapping={ 'entity_list': 'entity_list', 'index': 'index', 'output_value': 'Operator' }) return _state_machine
def create(self): # x:1474 y:331, x:56 y:575 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.half_turn = 3.1416 _state_machine.userdata.person = "person" _state_machine.userdata.operator_param = "behavior/Operaror/Id" _state_machine.userdata.join = ["Move", "spr/initialpose"] _state_machine.userdata.leave = ["Move", "door2/exit"] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:1182 y:163 _sm_rotate_0 = OperatableStateMachine(outcomes=['finished']) with _sm_rotate_0: # x:103 y:61 OperatableStateMachine.add('Look Left', SaraSetHeadAngle(pitch=-0.35, yaw=0.5), transitions={'done': 'Rotate Left'}, autonomy={'done': Autonomy.Off}) # x:794 y:54 OperatableStateMachine.add('Look Right', SaraSetHeadAngle(pitch=-0.35, yaw=-0.5), transitions={'done': 'Rotate Right'}, autonomy={'done': Autonomy.Off}) # x:325 y:61 OperatableStateMachine.add('Rotate Left', WaitState(wait_time=8), transitions={'done': 'Look Center'}, autonomy={'done': Autonomy.Off}) # x:961 y:65 OperatableStateMachine.add('Rotate Right', WaitState(wait_time=4), transitions={'done': 'Look Center 2'}, autonomy={'done': Autonomy.Off}) # x:1115 y:62 OperatableStateMachine.add('Look Center 2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:484 y:54 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=-0.35, yaw=0), transitions={'done': 'Rotate Center'}, autonomy={'done': Autonomy.Off}) # x:657 y:49 OperatableStateMachine.add('Rotate Center', WaitState(wait_time=4), transitions={'done': 'Look Right'}, autonomy={'done': Autonomy.Off}) # x:30 y:458 _sm_follow_head_1 = OperatableStateMachine(outcomes=['end'], input_keys=['person']) with _sm_follow_head_1: # x:214 y:48 OperatableStateMachine.add('list all entities', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'Get Nearest', 'none_found': 'list all entities' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'person', 'entity_list': 'entity_list', 'number': 'number' }) # x:456 y:51 OperatableStateMachine.add( 'Get Nearest', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'look'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'ID' }) # x:291 y:177 OperatableStateMachine.add( 'look', KeepLookingAt(), transitions={'failed': 'list all entities'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:12 y:125, x:1130 y:515 _sm_nlu_2 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_nlu_2: # x:156 y:37 OperatableStateMachine.add( 'say ask', SaraSay(sentence="You can ask me your questions.", input_keys=[], emotion=1, block=True), transitions={'done': 'Loop Questions'}, autonomy={'done': Autonomy.Off}) # x:1091 y:84 OperatableStateMachine.add('Listen', GetSpeech(watchdog=10), transitions={ 'done': 'Engine', 'nothing': 'Listen', 'fail': 'Listen' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'sentence'}) # x:1324 y:110 OperatableStateMachine.add('Engine', SaraNLUspr(), transitions={ 'understood': 'Say_Answer', 'not_understood': 'Listen', 'fail': 'Listen' }, autonomy={ 'understood': Autonomy.Off, 'not_understood': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'sentence': 'sentence', 'answer': 'answer' }) # x:632 y:77 OperatableStateMachine.add( 'Select Story', CalculationState(calculation=lambda x: x + 6), transitions={'done': 'Set_a_step'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'index', 'output_value': 'story' }) # x:874 y:78 OperatableStateMachine.add('Set_a_step', Set_a_step(step=1), transitions={'done': 'Listen'}, autonomy={'done': Autonomy.Off}) # x:398 y:81 OperatableStateMachine.add('Loop Questions', ForLoop(repeat=5), transitions={ 'do': 'Select Story', 'end': 'loop step' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:439 y:252 OperatableStateMachine.add( 'Say Blind Game', SaraSay(sentence="Let's play the blind game !", input_keys=[], emotion=1, block=True), transitions={'done': 'Loop Questions'}, autonomy={'done': Autonomy.Off}) # x:189 y:193 OperatableStateMachine.add('loop step', ForLoop(repeat=1), transitions={ 'do': 'Say Blind Game', 'end': 'finished' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:721 y:205 OperatableStateMachine.add('Say_Answer', SaraSay(sentence=lambda x: str(x), input_keys=[], emotion=0, block=True), transitions={'done': 'Loop Questions'}, autonomy={'done': Autonomy.Off}) # x:817 y:123, x:130 y:458 _sm_analyse_crowd_3 = OperatableStateMachine( outcomes=['finished', 'error']) with _sm_analyse_crowd_3: # x:87 y:97 OperatableStateMachine.add('clear database', WonderlandClearPeoples(), transitions={ 'done': 'Rotate', 'error': 'error' }, autonomy={ 'done': Autonomy.Off, 'error': Autonomy.Off }) # x:531 y:112 OperatableStateMachine.add('Add Update Persons', WonderlandAddUpdatePeople(), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:311 y:97 OperatableStateMachine.add( 'Rotate', _sm_rotate_0, transitions={'finished': 'Add Update Persons'}, autonomy={'finished': Autonomy.Inherit}) # x:1203 y:11, x:1006 y:366 _sm_init_scenario_4 = OperatableStateMachine( outcomes=['done', 'error']) with _sm_init_scenario_4: # x:30 y:42 OperatableStateMachine.add( 'Generate Vizbox Story', Set_Story(titre="Speech and Person Recognition", storyline=[ "Waiting Begining", "Join Game Room", "Waiting Crowd Placement", "Analysing Crowd", "Begin Game", "Find Operator", "Question 1", "Question 2", "Question 3", "Question 4", "Question 5", "Leave Arena" ]), transitions={'done': 'Set Story Step'}, autonomy={'done': Autonomy.Off}) # x:559 y:44 OperatableStateMachine.add('WaitForBegining', ContinueButton(), transitions={ 'true': 'Reset Persons', 'false': 'Reset Persons' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:807 y:61 OperatableStateMachine.add('Reset Persons', WonderlandClearPeoples(), transitions={ 'done': 'done', 'error': 'error' }, autonomy={ 'done': Autonomy.Off, 'error': Autonomy.Off }) # x:247 y:49 OperatableStateMachine.add('Set Story Step', Set_a_step(step=0), transitions={'done': 'setIDLE'}, autonomy={'done': Autonomy.Off}) # x:388 y:208 OperatableStateMachine.add('Reset Arm', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'Reset Persons', 'failed': 'error' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'target'}) # x:427 y:40 OperatableStateMachine.add('setIDLE', SetKey(Value="IdlePose"), transitions={'done': 'Reset Arm'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:30 y:458, x:230 y:458 _sm_join_area_5 = OperatableStateMachine( outcomes=['failed', 'finished'], input_keys=['join']) with _sm_join_area_5: # x:95 y:40 OperatableStateMachine.add( 'Say Join Area', SaraSay(sentence="I will join the playing room !", input_keys=[], emotion=1, block=True), transitions={'done': 'Join Arena'}, autonomy={'done': Autonomy.Off}) # x:92 y:134 OperatableStateMachine.add( 'Join Arena', self.use_behavior(sara_flexbe_behaviors__ActionWrapper_MoveSM, 'Join Area/Join Arena'), transitions={ 'finished': 'finished', 'failed': 'failed', 'critical_fail': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit }, remapping={'Action': 'join'}) # x:489 y:56, x:604 y:278 _sm_waiting_and_turn_6 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['half_turn']) with _sm_waiting_and_turn_6: # x:50 y:51 OperatableStateMachine.add( 'Want Play', SaraSay(sentence="Hum, I want to play riddles !", input_keys=[], emotion=1, block=True), transitions={'done': 'Wait 10s'}, autonomy={'done': Autonomy.Off}) # x:272 y:121 OperatableStateMachine.add( 'action_turn', self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Waiting And Turn/action_turn'), transitions={ 'finished': 'finished', 'failed': 'Cant turn' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'half_turn'}) # x:437 y:240 OperatableStateMachine.add('Cant turn', SaraSay(sentence="I can't turn !", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:63 y:178 OperatableStateMachine.add( 'Wait 10s', WaitState(wait_time=10), transitions={'done': 'Look In Front Of'}, autonomy={'done': Autonomy.Off}) # x:61 y:260 OperatableStateMachine.add('Look In Front Of', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}) # x:472 y:69, x:476 y:113, x:470 y:196, x:330 y:458, x:430 y:458 _sm_questions_7 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['person'], conditions=[('finished', [('NLU', 'finished')]), ('failed', [('NLU', 'failed')]), ('finished', [('Follow Head', 'end')])]) with _sm_questions_7: # x:85 y:58 OperatableStateMachine.add('NLU', _sm_nlu_2, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:84 y:185 OperatableStateMachine.add('Follow Head', _sm_follow_head_1, transitions={'end': 'finished'}, autonomy={'end': Autonomy.Inherit}, remapping={'person': 'person'}) # x:283 y:294, x:60 y:571 _sm_find_operator_8 = OperatableStateMachine( outcomes=['not_found', 'done'], input_keys=['person', 'operator_param']) with _sm_find_operator_8: # x:51 y:40 OperatableStateMachine.add( 'Ask Player', SaraSay(sentence="Who wan't to play with me ?", input_keys=[], emotion=1, block=True), transitions={'done': 'Wait Operator'}, autonomy={'done': Autonomy.Off}) # x:39 y:297 OperatableStateMachine.add( 'Get operator id', CalculationState(calculation=lambda x: x.entities[0].face.id), transitions={'done': 'Set Operator Id'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'operator' }) # x:33 y:384 OperatableStateMachine.add('Set Operator Id', SetRosParamKey(), transitions={'done': 'Operator Id'}, autonomy={'done': Autonomy.Off}, remapping={ 'Value': 'operator_param', 'ParamName': 'operator' }) # x:46 y:117 OperatableStateMachine.add('Wait Operator', WaitState(wait_time=6), transitions={'done': 'Find Operator'}, autonomy={'done': Autonomy.Off}) # x:35 y:495 OperatableStateMachine.add('Operator Id', LogKeyState( text="Operator find. Id: {}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'operator'}) # x:30 y:205 OperatableStateMachine.add('Find Operator', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'Get operator id', 'none_found': 'Find Operator' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'person', 'entity_list': 'entity_list', 'number': 'number' }) # x:703 y:198, x:88 y:199 _sm_tell_basic_stats_9 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_tell_basic_stats_9: # x:50 y:40 OperatableStateMachine.add('wait', WaitState(wait_time=0), transitions={'done': 'GetPeopleStats'}, autonomy={'done': Autonomy.Off}) # x:218 y:43 OperatableStateMachine.add('GetPeopleStats', WonderlandGetPersonStat(), transitions={ 'done': 'GenerateSentence', 'none': 'Nobody', 'error': 'failed' }, autonomy={ 'done': Autonomy.Off, 'none': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'women': 'women', 'men': 'men', 'others': 'others' }) # x:466 y:46 OperatableStateMachine.add('Nobody', SaraSay( sentence="There is nobody here !", input_keys=[], emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:162 y:243 OperatableStateMachine.add( 'GenerateSentence', FlexibleCalculationState(calculation=lambda x: "There is " + str(x[0] + x[1] + x[2]) + " persons.", input_keys=['men', 'women', 'others']), transitions={'done': 'Tell_Stats'}, autonomy={'done': Autonomy.Off}, remapping={ 'men': 'men', 'women': 'women', 'others': 'others', 'output_value': 'sentence' }) # x:151 y:345 OperatableStateMachine.add( 'Generate Sentence 2', FlexibleCalculationState( calculation=lambda x: "I recognize " + str(x[ 1]) + " women and " + str(x[0]) + " men.", input_keys=['men', 'women', 'others']), transitions={'done': 'Tell_Stats 2'}, autonomy={'done': Autonomy.Off}, remapping={ 'men': 'men', 'women': 'women', 'others': 'others', 'output_value': 'sentence' }) # x:380 y:172 OperatableStateMachine.add( 'Tell_Stats', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'Generate Sentence 2'}, autonomy={'done': Autonomy.Off}) # x:409 y:276 OperatableStateMachine.add('Tell_Stats 2', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:30 y:40 OperatableStateMachine.add('continue', ContinueButton(), transitions={ 'true': 'Init Scenario', 'false': 'Init Scenario' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:715 y:127 OperatableStateMachine.add('Tell basic stats', _sm_tell_basic_stats_9, transitions={ 'finished': 'Set Find Operator', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:899 y:126 OperatableStateMachine.add('Find Operator', _sm_find_operator_8, transitions={ 'not_found': 'Find Operator', 'done': 'Questions' }, autonomy={ 'not_found': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'person': 'person', 'operator_param': 'operator_param' }) # x:1120 y:126 OperatableStateMachine.add('Questions', _sm_questions_7, transitions={ 'finished': 'set head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'person': 'person'}) # x:343 y:49 OperatableStateMachine.add( 'Set Story Waiting', Set_a_step(step=2), transitions={'done': 'Waiting And Turn'}, autonomy={'done': Autonomy.Off}) # x:532 y:48 OperatableStateMachine.add('Set Analyse', Set_a_step(step=3), transitions={'done': 'Analyse Crowd'}, autonomy={'done': Autonomy.Off}) # x:696 y:47 OperatableStateMachine.add( 'Set Begin Game', Set_a_step(step=4), transitions={'done': 'Tell basic stats'}, autonomy={'done': Autonomy.Off}) # x:899 y:49 OperatableStateMachine.add('Set Find Operator', Set_a_step(step=5), transitions={'done': 'Find Operator'}, autonomy={'done': Autonomy.Off}) # x:332 y:127 OperatableStateMachine.add('Waiting And Turn', _sm_waiting_and_turn_6, transitions={ 'finished': 'Set Analyse', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'half_turn': 'half_turn'}) # x:1333 y:232 OperatableStateMachine.add('Set Go Out', Set_a_step(step=11), transitions={'done': 'Say And Of Game'}, autonomy={'done': Autonomy.Off}) # x:1517 y:148 OperatableStateMachine.add( 'Leave Arena', self.use_behavior(sara_flexbe_behaviors__ActionWrapper_MoveSM, 'Leave Arena'), transitions={ 'finished': 'finished', 'failed': 'finished', 'critical_fail': 'finished' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit }, remapping={'Action': 'leave'}) # x:176 y:42 OperatableStateMachine.add( 'Set Join', Set_a_step(step=1), transitions={'done': 'Set Story Waiting'}, autonomy={'done': Autonomy.Off}) # x:177 y:120 OperatableStateMachine.add('Join Area', _sm_join_area_5, transitions={ 'failed': 'failed', 'finished': 'Set Story Waiting' }, autonomy={ 'failed': Autonomy.Inherit, 'finished': Autonomy.Inherit }, remapping={'join': 'join'}) # x:1302 y:140 OperatableStateMachine.add( 'Say And Of Game', SaraSay( sentence= "The game is finished. I will leave the arena. Thank you for playing with me.", input_keys=[], emotion=1, block=True), transitions={'done': 'Leave Arena'}, autonomy={'done': Autonomy.Off}) # x:1120 y:237 OperatableStateMachine.add('set head', SaraSetHeadAngle(pitch=-0.2, yaw=0), transitions={'done': 'Set Go Out'}, autonomy={'done': Autonomy.Off}) # x:34 y:130 OperatableStateMachine.add('Init Scenario', _sm_init_scenario_4, transitions={ 'done': 'Set Join', 'error': 'failed' }, autonomy={ 'done': Autonomy.Inherit, 'error': Autonomy.Inherit }) # x:517 y:124 OperatableStateMachine.add('Analyse Crowd', _sm_analyse_crowd_3, transitions={ 'finished': 'Set Begin Game', 'error': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'error': Autonomy.Inherit }) return _state_machine
def create(self): initial_mode = "stand" motion_mode = "manipulate" mantis_mode = "manipulate_limits" percent_past_limits = 0.10 # before: 0.075 # x:788 y:72, x:474 y:133 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.target_limits = 'upper' _state_machine.userdata.cycle_counter = 1 _state_machine.userdata.stand_posture = None # calculated _state_machine.userdata.offsets = { 'left_arm': dict(), 'right_arm': dict() } # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._percent_past_limits = percent_past_limits # Create STAND posture trajectory _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory( ) # [/MANUAL_CREATE] # x:222 y:281, x:349 y:167 _sm_determine_offsets_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets']) with _sm_determine_offsets_0: # x:61 y:53 OperatableStateMachine.add( 'Get_Left_Joint_Positions', CurrentJointPositionsState(planning_group="l_arm_group"), transitions={ 'retrieved': 'Determine_Closest_Limits_Left', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.Low }, remapping={'joint_positions': 'joint_positions'}) # x:319 y:54 OperatableStateMachine.add( 'Determine_Closest_Limits_Left', CalculationState(calculation=self.get_closest_limits_left), transitions={'done': 'Store_Offsets_Left'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_positions', 'output_value': 'joint_limits' }) # x:598 y:162 OperatableStateMachine.add( 'Get_Right_Joint_Positions', CurrentJointPositionsState(planning_group="r_arm_group"), transitions={ 'retrieved': 'Determine_Closest_Limits_Right', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.Low }, remapping={'joint_positions': 'joint_positions'}) # x:584 y:275 OperatableStateMachine.add( 'Determine_Closest_Limits_Right', CalculationState(calculation=self.get_closest_limits_right), transitions={'done': 'Store_Offsets_Right'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joint_positions', 'output_value': 'joint_limits' }) # x:608 y:54 OperatableStateMachine.add( 'Store_Offsets_Left', FlexibleCalculationState( calculation=self.store_offsets_left, input_keys=['limits', 'value', 'offsets', 'counter']), transitions={'done': 'Get_Right_Joint_Positions'}, autonomy={'done': Autonomy.Off}, remapping={ 'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets' }) # x:340 y:274 OperatableStateMachine.add( 'Store_Offsets_Right', FlexibleCalculationState( calculation=self.store_offsets_right, input_keys=['limits', 'value', 'offsets', 'counter']), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'limits': 'joint_limits', 'value': 'joint_positions', 'offsets': 'offsets', 'counter': 'cycle_counter', 'output_value': 'offsets' }) # x:528 y:401, x:707 y:282 _sm_manipulate_limits_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'offsets'], output_keys=['offsets', 'traj_past_limits']) with _sm_manipulate_limits_1: # x:100 y:156 OperatableStateMachine.add( 'Prevent_Runtime_Failure', CalculationState(calculation=lambda x: dict()), transitions={'done': 'Go_to_MANIPULATE_LIMITS'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'cycle_counter', 'output_value': 'traj_past_limits' }) # x:387 y:55 OperatableStateMachine.add( 'Wait_for_Control_Mode_change', WaitState(wait_time=1.0), transitions={'done': 'Get_Left_Joint_Positions'}, autonomy={'done': Autonomy.Low}) # x:895 y:279 OperatableStateMachine.add( 'Gen_Traj_from_90%_to_110%', CalculationState(calculation=self.gen_traj_past_limits), transitions={'done': 'Go_to_110%_Joint_Limits'}, autonomy={'done': Autonomy.Low}, remapping={ 'input_value': 'current_joint_values', 'output_value': 'traj_past_limits' }) # x:893 y:391 OperatableStateMachine.add( 'Go_to_110%_Joint_Limits', ExecuteTrajectoryBothArmsState(controllers=[ 'left_arm_traj_controller', 'right_arm_traj_controller' ]), transitions={ 'done': 'Determine_Offsets', 'failed': 'Determine_Offsets' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'trajectories': 'traj_past_limits'}) # x:651 y:385 OperatableStateMachine.add('Determine_Offsets', _sm_determine_offsets_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'cycle_counter': 'cycle_counter', 'offsets': 'offsets' }) # x:648 y:54 OperatableStateMachine.add( 'Get_Left_Joint_Positions', CurrentJointPositionsState(planning_group="l_arm_group"), transitions={ 'retrieved': 'Get_Right_Joint_Positions', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.High }, remapping={'joint_positions': 'joint_positions_left'}) # x:904 y:53 OperatableStateMachine.add( 'Get_Right_Joint_Positions', CurrentJointPositionsState(planning_group="r_arm_group"), transitions={ 'retrieved': 'Generate_Joint_Positions_Struct', 'failed': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'failed': Autonomy.High }, remapping={'joint_positions': 'joint_positions_right'}) # x:886 y:168 OperatableStateMachine.add( 'Generate_Joint_Positions_Struct', FlexibleCalculationState(calculation=lambda ik: { 'left_arm': ik[0], 'right_arm': ik[1] }, input_keys=['left', 'right']), transitions={'done': 'Gen_Traj_from_90%_to_110%'}, autonomy={'done': Autonomy.Off}, remapping={ 'left': 'joint_positions_left', 'right': 'joint_positions_right', 'output_value': 'current_joint_values' }) # x:92 y:55 OperatableStateMachine.add( 'Go_to_MANIPULATE_LIMITS', ChangeControlModeActionState(target_mode=mantis_mode), transitions={ 'changed': 'Wait_for_Control_Mode_change', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:574 y:247, x:276 y:549 _sm_update_calibration_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['offsets']) with _sm_update_calibration_2: # x:46 y:44 OperatableStateMachine.add( 'Process_Offsets', CalculationState(calculation=self.process_offsets), transitions={'done': 'Print_Offset_Info'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'offsets' }) # x:227 y:45 OperatableStateMachine.add( 'Print_Offset_Info', CalculationState(calculation=self.print_offset_info), transitions={'done': 'Publish_Offsets'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'none' }) # x:390 y:158 OperatableStateMachine.add( 'Ask_Perform_Update', OperatorDecisionState( outcomes=['update', 'no_update'], hint= "Do you want to apply the calculated offsets for calibration?", suggestion=None), transitions={ 'update': 'Convert_Offset_Data', 'no_update': 'finished' }, autonomy={ 'update': Autonomy.Full, 'no_update': Autonomy.Full }) # x:232 y:337 OperatableStateMachine.add( 'Update_Calibration', UpdateJointCalibrationState( joint_names=self._joint_names['left_arm'][0:4] + self._joint_names['right_arm'][0:4]), transitions={ 'updated': 'Calibration_Successful', 'failed': 'Calibration_Failed' }, autonomy={ 'updated': Autonomy.Low, 'failed': Autonomy.High }, remapping={'joint_offsets': 'offset_list'}) # x:241 y:242 OperatableStateMachine.add( 'Convert_Offset_Data', CalculationState(calculation=lambda o: o['left_arm']['avg'] + o['right_arm']['avg']), transitions={'done': 'Update_Calibration'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'offset_list' }) # x:522 y:337 OperatableStateMachine.add( 'Calibration_Successful', LogState(text="Successfully updated calibration offsets.", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:246 y:445 OperatableStateMachine.add( 'Calibration_Failed', LogState(text="Failed to apply calibration offsets!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:399 y:44 OperatableStateMachine.add( 'Publish_Offsets', CalculationState(calculation=self.publish_offsets), transitions={'done': 'Ask_Perform_Update'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'offsets', 'output_value': 'none' }) # x:978 y:197, x:394 y:80 _sm_perform_checks_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['cycle_counter', 'target_limits', 'offsets'], output_keys=['cycle_counter', 'offsets']) with _sm_perform_checks_3: # x:105 y:74 OperatableStateMachine.add( 'Go_to_Intermediate_Mode', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Gen_Traj_to_90%_Limits', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:653 y:274 OperatableStateMachine.add('Manipulate_Limits', _sm_manipulate_limits_1, transitions={ 'finished': 'Gen_Traj_back_to_90%_Limits', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'cycle_counter': 'cycle_counter', 'offsets': 'offsets', 'traj_past_limits': 'traj_past_limits' }) # x:903 y:78 OperatableStateMachine.add( 'Increment_Cycle_counter', CalculationState(calculation=lambda counter: counter + 1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'cycle_counter', 'output_value': 'cycle_counter' }) # x:344 y:277 OperatableStateMachine.add( 'Move_to_90%_Joint_Limits', MoveitStartingPointState(vel_scaling=0.3), transitions={ 'reached': 'Manipulate_Limits', 'failed': 'Move_to_90%_Joint_Limits' }, autonomy={ 'reached': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'trajectories': 'trajectories_90'}) # x:114 y:276 OperatableStateMachine.add( 'Gen_Traj_to_90%_Limits', CalculationState(calculation=self.gen_traj_pre_limits), transitions={'done': 'Move_to_90%_Joint_Limits'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'target_limits', 'output_value': 'trajectories_90' }) # x:636 y:78 OperatableStateMachine.add( 'Go_back_to_90%_Joint_Limits', ExecuteTrajectoryBothArmsState(controllers=[ 'left_arm_traj_controller', 'right_arm_traj_controller' ]), transitions={ 'done': 'Increment_Cycle_counter', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'trajectories': 'traj_back_to_90'}) # x:636 y:172 OperatableStateMachine.add( 'Gen_Traj_back_to_90%_Limits', FlexibleCalculationState( calculation=self.gen_traj_back_from_limits, input_keys=['trajectories_90', 'traj_past_limits']), transitions={'done': 'Go_back_to_90%_Joint_Limits'}, autonomy={'done': Autonomy.Off}, remapping={ 'trajectories_90': 'trajectories_90', 'traj_past_limits': 'traj_past_limits', 'output_value': 'traj_back_to_90' }) with _state_machine: # x:110 y:52 OperatableStateMachine.add( 'Initial_Control_Mode', ChangeControlModeActionState(target_mode=initial_mode), transitions={ 'changed': 'Perform_Checks', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.High, 'failed': Autonomy.High }) # x:712 y:317 OperatableStateMachine.add( 'Initial_Mode_before_exit', ChangeControlModeActionState(target_mode=initial_mode), transitions={ 'changed': 'Update_Calibration', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:122 y:302 OperatableStateMachine.add('Perform_Checks', _sm_perform_checks_3, transitions={ 'finished': 'Are_We_Done_Yet?', 'failed': 'Intermediate_Mode_before_exit' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'cycle_counter': 'cycle_counter', 'target_limits': 'target_limits', 'offsets': 'offsets' }) # x:126 y:505 OperatableStateMachine.add( 'Are_We_Done_Yet?', DecisionState(outcomes=["done", "more"], conditions=lambda counter: "done" if counter >= 2 else "more"), transitions={ 'done': 'Intermediate_Mode_before_exit', 'more': 'Setup_next_Cycle' }, autonomy={ 'done': Autonomy.Low, 'more': Autonomy.High }, remapping={'input_value': 'cycle_counter'}) # x:15 y:404 OperatableStateMachine.add( 'Setup_next_Cycle', CalculationState(calculation=lambda lim: 'lower' if lim == 'upper' else 'upper'), transitions={'done': 'Perform_Checks'}, autonomy={'done': Autonomy.Low}, remapping={ 'input_value': 'target_limits', 'output_value': 'target_limits' }) # x:725 y:186 OperatableStateMachine.add('Update_Calibration', _sm_update_calibration_2, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'offsets': 'offsets'}) # x:726 y:427 OperatableStateMachine.add( 'Move_to_Stand_Posture', MoveitStartingPointState(vel_scaling=0.3), transitions={ 'reached': 'Initial_Mode_before_exit', 'failed': 'Move_to_Stand_Posture' }, autonomy={ 'reached': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'trajectories': 'stand_posture'}) # x:412 y:427 OperatableStateMachine.add( 'Intermediate_Mode_before_exit', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Move_to_Stand_Posture', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) return _state_machine