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
