def create(self):
# x:1195 y:433, x:132 y:431, x:750 y:42, x:991 y:471
_state_machine = OperatableStateMachine(outcomes=['Given', 'Person_not_found', 'No_object_in_hand', 'fail'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:130 y:365
_sm_lookat_0 = OperatableStateMachine(outcomes=['failed'], input_keys=['ID'])
with _sm_lookat_0:
# x:114 y:127
OperatableStateMachine.add('look',
KeepLookingAt(),
transitions={'failed': 'look'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:299 y:300, x:263 y:535
_sm_give_1 = OperatableStateMachine(outcomes=['failed', 'given'], input_keys=['Object'])
with _sm_give_1:
# x:67 y:27
OperatableStateMachine.add('SetPose',
SetKey(Value="ShowGripper"),
transitions={'done': 'say_give'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'target'})
# x:53 y:413
OperatableStateMachine.add('read torque',
ReadTorque(watchdog=5, Joint="right_elbow_pitch_joint", Threshold=2, min_time=1),
transitions={'threshold': 'open gripper', 'watchdog': 'read torque', 'fail': 'failed'},
autonomy={'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'torque': 'torque'})
# x:52 y:500
OperatableStateMachine.add('open gripper',
SetGripperState(width=0.15, effort=1),
transitions={'object': 'given', 'no_object': 'given'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:64 y:248
OperatableStateMachine.add('say pull',
SaraSay(sentence="You can pull on it", input_keys=[], emotion=1, block=False),
transitions={'done': 'wait 1'},
autonomy={'done': Autonomy.Off})
# x:64 y:325
OperatableStateMachine.add('wait 1',
WaitState(wait_time=1),
transitions={'done': 'read torque'},
autonomy={'done': Autonomy.Off})
# x:57 y:175
OperatableStateMachine.add('moveArm',
MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15),
transitions={'done': 'say pull', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'target': 'target'})
# x:60 y:88
OperatableStateMachine.add('say_give',
SaraSay(sentence=lambda x: "Hi. I'm giving you this "+str(x), input_keys=[], emotion=0, block=True),
transitions={'done': 'moveArm'},
autonomy={'done': Autonomy.Off})
# x:596 y:480
_sm_follow_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID'])
with _sm_follow_2:
# x:180 y:123
OperatableStateMachine.add('follow',
SaraFollow(distance=1.5, ReplanPeriod=0.5),
transitions={'failed': 'follow'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:313 y:247, x:301 y:177, x:103 y:293, x:343 y:113, x:397 y:411, x:311 y:27, x:630 y:365
_sm_give_3 = ConcurrencyContainer(outcomes=['failed', 'given', 'continue'], input_keys=['ID', 'Object'], conditions=[
('failed', [('Give', 'failed')]),
('given', [('Give', 'given')]),
('given', [('Follow', 'finished')]),
('failed', [('LookAt', 'failed')])
])
with _sm_give_3:
# x:91 y:50
OperatableStateMachine.add('Follow',
_sm_follow_2,
transitions={'finished': 'given'},
autonomy={'finished': Autonomy.Inherit},
remapping={'ID': 'ID'})
# x:84 y:164
OperatableStateMachine.add('Give',
_sm_give_1,
transitions={'failed': 'failed', 'given': 'given'},
autonomy={'failed': Autonomy.Inherit, 'given': Autonomy.Inherit},
remapping={'Object': 'Object'})
# x:175 y:371
OperatableStateMachine.add('LookAt',
_sm_lookat_0,
transitions={'failed': 'failed'},
autonomy={'failed': Autonomy.Inherit},
remapping={'ID': 'ID'})
with _state_machine:
# x:77 y:29
OperatableStateMachine.add('Get hand content',
GetRosParam(ParamName="behavior/GripperContent"),
transitions={'done': 'is object in hand?', 'failed': 'fail'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'Value': 'Object'})
# x:58 y:108
OperatableStateMachine.add('is object in hand?',
CheckConditionState(predicate=lambda x: x),
transitions={'true': 'name', 'false': 'log empty hand'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'input_value': 'Object'})
# x:70 y:277
OperatableStateMachine.add('list persons',
list_entities_by_name(frontality_level=0.5, distance_max=10),
transitions={'found': 'get id', 'none_found': 'Person_not_found'},
autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off},
remapping={'name': 'name', 'entity_list': 'People_list', 'number': 'number'})
# x:414 y:37
OperatableStateMachine.add('log empty hand',
LogState(text="The hand is empty. Set the GripperContent rosParam", severity=Logger.REPORT_HINT),
transitions={'done': 'No_object_in_hand'},
autonomy={'done': Autonomy.Off})
# x:754 y:223
OperatableStateMachine.add('log moveitfail',
LogState(text="moveit failed", severity=Logger.REPORT_HINT),
transitions={'done': 'fail'},
autonomy={'done': Autonomy.Off})
# x:606 y:371
OperatableStateMachine.add('log movebase fail',
LogState(text="giving Failed", severity=Logger.REPORT_HINT),
transitions={'done': 'fail'},
autonomy={'done': Autonomy.Off})
# x:402 y:133
OperatableStateMachine.add('set idle pose',
SetKey(Value="IdlePose"),
transitions={'done': 'say_good'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'target'})
# x:751 y:108
OperatableStateMachine.add('moveArm2',
MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15),
transitions={'done': 'set none', 'failed': 'log moveitfail'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'target': 'target'})
# x:920 y:278
OperatableStateMachine.add('close gripper',
SetGripperState(width=0, effort=1),
transitions={'object': 'Given', 'no_object': 'Given'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:1048 y:236
OperatableStateMachine.add('remove gripper content',
SetRosParam(ParamName="GripperContent"),
transitions={'done': 'close gripper'},
autonomy={'done': Autonomy.Off},
remapping={'Value': 'none'})
# x:910 y:182
OperatableStateMachine.add('set none',
SetKey(Value=None),
transitions={'done': 'close gripper'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'none'})
# x:408 y:333
OperatableStateMachine.add('give',
_sm_give_3,
transitions={'failed': 'log movebase fail', 'given': 'set idle pose', 'continue': 'give'},
autonomy={'failed': Autonomy.Inherit, 'given': Autonomy.Inherit, 'continue': Autonomy.Inherit},
remapping={'ID': 'ID', 'Object': 'Object'})
# x:256 y:278
OperatableStateMachine.add('get id',
CalculationState(calculation=lambda x: x[0].ID),
transitions={'done': 'give'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'People_list', 'output_value': 'ID'})
# x:68 y:192
OperatableStateMachine.add('name',
SetKey(Value="person"),
transitions={'done': 'list persons'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'name'})
# x:591 y:138
OperatableStateMachine.add('say_good',
SaraSay(sentence=lambda x: "Good, enjoy your "+str(x), input_keys=[], emotion=0, block=True),
transitions={'done': 'moveArm2'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:682 y:306, x:452 y:252
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['pos'])
_state_machine.userdata.pos = {"x": 0.8, "y": -0.2, "z": 1}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458, x:530 y:458, x:630 y:458, x:59 y:533, x:830 y:458
_sm_group_0 = ConcurrencyContainer(
outcomes=['threshold', 'watchdog', 'fail'],
conditions=[('threshold', [('read', 'threshold')]),
('watchdog', [('read', 'watchdog')]),
('fail', [('read', 'fail')]),
('threshold', [('read yaw', 'threshold')]),
('fail', [('read yaw', 'fail')]),
('watchdog', [('read yaw', 'watchdog')])])
with _sm_group_0:
# x:86 y:125
OperatableStateMachine.add('read',
ReadTorque(
watchdog=1,
Joint="right_elbow_pitch_joint",
Threshold=0.7,
min_time=0.4),
transitions={
'threshold': 'threshold',
'watchdog': 'watchdog',
'fail': 'fail'
},
autonomy={
'threshold': Autonomy.Off,
'watchdog': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'torque': 'torque'})
# x:252 y:135
OperatableStateMachine.add('read yaw',
ReadTorque(
watchdog=1,
Joint="right_elbow_pitch_joint",
Threshold=0.5,
min_time=0.4),
transitions={
'threshold': 'threshold',
'watchdog': 'watchdog',
'fail': 'fail'
},
autonomy={
'threshold': Autonomy.Off,
'watchdog': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'torque': 'torque'})
# x:30 y:458
_sm_read_torque_1 = OperatableStateMachine(outcomes=['done'])
with _sm_read_torque_1:
# x:142 y:61
OperatableStateMachine.add('log',
LogState(text="going down",
severity=Logger.REPORT_HINT),
transitions={'done': 'Group'},
autonomy={'done': Autonomy.Off})
# x:131 y:164
OperatableStateMachine.add('Group',
_sm_group_0,
transitions={
'threshold': 'done',
'watchdog': 'log',
'fail': 'done'
},
autonomy={
'threshold': Autonomy.Inherit,
'watchdog': Autonomy.Inherit,
'fail': Autonomy.Inherit
})
# x:30 y:458
_sm_go_down_2 = OperatableStateMachine(outcomes=['done'],
input_keys=['GripPose'])
with _sm_go_down_2:
# x:126 y:194
OperatableStateMachine.add('movePlace',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'done'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'GripPose'})
# x:30 y:324, x:130 y:324
_sm_retreate_arm_3 = OperatableStateMachine(
outcomes=['failed', 'done'],
input_keys=['approach_pose', 'PreGripPose'])
with _sm_retreate_arm_3:
# x:30 y:40
OperatableStateMachine.add('ReturnApproachPose',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'ReturnPreGrip',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'approach_pose'})
# x:202 y:41
OperatableStateMachine.add('ReturnPreGrip',
MoveitMove(move=True,
waitForExecution=False,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PreGripPose'})
# x:30 y:324, x:130 y:324
_sm_releasing_4 = OperatableStateMachine(
outcomes=['object', 'no_object'])
with _sm_releasing_4:
# x:30 y:40
OperatableStateMachine.add('say touchdown',
SaraSay(sentence="Touchdown!",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'open gripper'},
autonomy={'done': Autonomy.Off})
# x:139 y:176
OperatableStateMachine.add(
'open gripper',
SetGripperState(width=0.14, effort=1),
transitions={
'object': 'object',
'no_object': 'no_object'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:30 y:324, x:130 y:324
_sm_moveback_5 = OperatableStateMachine(outcomes=['arrived', 'failed'])
with _sm_moveback_5:
# x:30 y:40
OperatableStateMachine.add('genpose',
GenPoseEuler(x=-0.3,
y=-0.3,
z=0,
roll=0,
pitch=0,
yaw=0),
transitions={'done': 'move back'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'backPose'})
# x:40 y:163
OperatableStateMachine.add('move back',
SaraMoveBase(reference="base_link"),
transitions={
'arrived': 'arrived',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'backPose'})
# x:536 y:72, x:231 y:292
_sm_prepare_grip_6 = OperatableStateMachine(
outcomes=['failed', 'done'],
input_keys=['pos'],
output_keys=['approach_pose', 'grip_pose'])
with _sm_prepare_grip_6:
# x:50 y:40
OperatableStateMachine.add('Gen place_pos',
GenGripperPose(l=0,
z=-0.05,
planar=True),
transitions={
'done': 'Gen approach_pos',
'fail': 'failed'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'pos',
'pose_out': 'grip_pose'
})
# x:30 y:176
OperatableStateMachine.add('MoveIt_isReachable',
MoveitMove(move=False,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'log app',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grip_pose'})
# x:37 y:108
OperatableStateMachine.add('Gen approach_pos',
GenGripperPose(l=0.0,
z=0.20,
planar=True),
transitions={
'done': 'log place pos',
'fail': 'failed'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'pos',
'pose_out': 'approach_pose'
})
# x:41 y:269
OperatableStateMachine.add('log app',
LogKeyState(
text="{}",
severity=Logger.REPORT_HINT),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'approach_pose'})
# x:360 y:180
OperatableStateMachine.add(
'log place pos',
LogKeyState(text="place pose is {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'MoveIt_isReachable'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'grip_pose'})
# x:30 y:324, x:130 y:324
_sm_pregrippose_7 = OperatableStateMachine(outcomes=['done', 'failed'],
output_keys=['PreGripPose'])
with _sm_pregrippose_7:
# x:30 y:40
OperatableStateMachine.add('setPreGripPose',
SetKey(Value="PrePlacePose"),
transitions={'done': 'gotoPreGrip'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'PreGripPose'})
# x:32 y:106
OperatableStateMachine.add('gotoPreGrip',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PreGripPose'})
# x:30 y:458, x:130 y:458, x:230 y:458
_sm_get_down_8 = ConcurrencyContainer(
outcomes=['done'],
input_keys=['GripPose'],
conditions=[('done', [('Go down', 'done')]),
('done', [('read torque', 'done')])])
with _sm_get_down_8:
# x:178 y:127
OperatableStateMachine.add('Go down',
_sm_go_down_2,
transitions={'done': 'done'},
autonomy={'done': Autonomy.Inherit},
remapping={'GripPose': 'GripPose'})
# x:405 y:150
OperatableStateMachine.add('read torque',
_sm_read_torque_1,
transitions={'done': 'done'},
autonomy={'done': Autonomy.Inherit})
# x:30 y:324, x:130 y:324
_sm_pretraitement_9 = OperatableStateMachine(outcomes=['fail', 'done'],
input_keys=['pos'],
output_keys=['pos'])
with _sm_pretraitement_9:
# x:30 y:40
OperatableStateMachine.add('TF_transformation',
TF_transformation(in_ref="map",
out_ref="base_link"),
transitions={
'done': 'LOG POSE',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'in_pos': 'pos',
'out_pos': 'pos'
})
# x:33 y:107
OperatableStateMachine.add('LOG POSE',
LogKeyState(
text="{}",
severity=Logger.REPORT_HINT),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'pos'})
with _state_machine:
# x:148 y:34
OperatableStateMachine.add('Pretraitement',
_sm_pretraitement_9,
transitions={
'fail': 'failed',
'done': 'PreGripPose'
},
autonomy={
'fail': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={'pos': 'pos'})
# x:634 y:410
OperatableStateMachine.add(
'close gripper',
SetGripperState(width=0, effort=1),
transitions={
'object': 'finished',
'no_object': 'finished'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:141 y:522
OperatableStateMachine.add('Get_down',
_sm_get_down_8,
transitions={'done': 'releasing'},
autonomy={'done': Autonomy.Inherit},
remapping={'GripPose': 'grip_pose'})
# x:159 y:352
OperatableStateMachine.add('look down',
SaraSetHeadAngle(pitch=0.6, yaw=-0.3),
transitions={'done': 'Move_approach'},
autonomy={'done': Autonomy.Off})
# x:160 y:127
OperatableStateMachine.add(
'PreGripPose',
_sm_pregrippose_7,
transitions={
'done': 'Prepare grip',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'PreGripPose': 'PreGripPose'})
# x:156 y:238
OperatableStateMachine.add('Prepare grip',
_sm_prepare_grip_6,
transitions={
'failed': 'failed',
'done': 'look down'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'pos': 'pos',
'approach_pose': 'approach_pose',
'grip_pose': 'grip_pose'
})
# x:139 y:444
OperatableStateMachine.add('Move_approach',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'Get_down',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'approach_pose'})
# x:623 y:525
OperatableStateMachine.add('Moveback',
_sm_moveback_5,
transitions={
'arrived': 'close gripper',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:298 y:520
OperatableStateMachine.add('releasing',
_sm_releasing_4,
transitions={
'object': 'retreate arm',
'no_object': 'retreate arm'
},
autonomy={
'object': Autonomy.Inherit,
'no_object': Autonomy.Inherit
})
# x:459 y:522
OperatableStateMachine.add('retreate arm',
_sm_retreate_arm_3,
transitions={
'failed': 'failed',
'done': 'Moveback'
},
autonomy={
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'approach_pose': 'approach_pose',
'PreGripPose': 'PreGripPose'
})
return _state_machine
def create(self):
# x:867 y:64, x:469 y:60
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'Closed_Gripper_Width', 'Open_Gripper_Width',
'Closed_Gripper_Width'
])
_state_machine.userdata.Closed_Gripper_Width = 1
_state_machine.userdata.Open_Gripper_Width = 255
_state_machine.userdata.effort = 50
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:101 y:292
OperatableStateMachine.add('place arm',
RunTrajectory(file="receive_bag",
duration=6),
transitions={'done': 'opengripper'},
autonomy={'done': Autonomy.Off})
# x:468 y:286
OperatableStateMachine.add(
'close_gripper',
SetGripperState(width=0, effort=1),
transitions={
'object': 'thank you',
'no_object': 'thank you'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:638 y:216
OperatableStateMachine.add('thank you',
SaraSay(sentence="Thank you",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'place back arm'},
autonomy={'done': Autonomy.Off})
# x:653 y:81
OperatableStateMachine.add('place back arm',
RunTrajectory(file="sac_transport",
duration=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:263 y:293
OperatableStateMachine.add('Torque_Reader',
ReadTorque(
watchdog=5,
Joint="right_elbow_pitch_joint",
Threshold=0.5,
min_time=1),
transitions={
'threshold': 'close_gripper',
'watchdog': 'close_gripper',
'fail': 'failed'
},
autonomy={
'threshold': Autonomy.Off,
'watchdog': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'torque': 'torque'})
# x:196 y:148
OperatableStateMachine.add(
'opengripper',
SetGripperState(width=0.25, effort=1),
transitions={
'object': 'Torque_Reader',
'no_object': 'Torque_Reader'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
return _state_machine
def create(self):
# x:463 y:361, x:402 y:174
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.effort = 0
_state_machine.userdata.Open_gripper = 255
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:53 y:30
OperatableStateMachine.add('place arm',
RunTrajectory(file="receive_bag",
duration=6),
transitions={'done': 'Open_gripper'},
autonomy={'done': Autonomy.Off})
# x:50 y:231
OperatableStateMachine.add('Say_to take_bag',
SaraSay(sentence="Here is your bag.",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'detectObject'},
autonomy={'done': Autonomy.Off})
# x:45 y:321
OperatableStateMachine.add('detectObject',
ReadTorque(
watchdog=10,
Joint="right_elbow_pitch_joint",
Threshold=0.5,
min_time=2),
transitions={
'threshold': 'Finished',
'watchdog': 'Finished',
'fail': 'Finished'
},
autonomy={
'threshold': Autonomy.Off,
'watchdog': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'torque': 'torque'})
# x:220 y:472
OperatableStateMachine.add(
'Close',
SetGripperState(width=0, effort=1),
transitions={
'object': 'place back bag',
'no_object': 'place back bag'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:47 y:466
OperatableStateMachine.add('Finished',
SaraSay(sentence="It was a pleasure",
input_keys=[],
emotion=0,
block=False),
transitions={'done': 'Close'},
autonomy={'done': Autonomy.Off})
# x:48 y:116
OperatableStateMachine.add(
'Open_gripper',
SetGripperState(width=0.1, effort=1),
transitions={
'object': 'Say_to take_bag',
'no_object': 'Say_to take_bag'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:420 y:470
OperatableStateMachine.add('place back bag',
RunTrajectory(file="repos", duration=0),
transitions={'done': 'finished'},
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):
# x:825 y:465, x:801 y:48
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:458, x:130 y:458
_sm_torque_control_0 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_torque_control_0:
# x:126 y:51
OperatableStateMachine.add('w2',
WaitState(wait_time=5),
transitions={'done': 'check torque'},
autonomy={'done': Autonomy.Off})
# x:120 y:195
OperatableStateMachine.add('check torque',
ReadTorque(
watchdog=20,
Joint="right_shoulder_pitch_joint",
Threshold=2.5,
min_time=0.5),
transitions={
'threshold': 'finished',
'watchdog': 'check torque',
'fail': 'check torque'
},
autonomy={
'threshold': Autonomy.Off,
'watchdog': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'torque': 'torque'})
# x:30 y:458, x:130 y:458
_sm_trajectory_down_1 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_trajectory_down_1:
# x:68 y:158
OperatableStateMachine.add('run down',
RunTrajectory(file="poubelle_app",
duration=16),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458, x:530 y:458
_sm_container_2 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('failed', [('test', 'done')]),
('finished', [('tor', 'threshold')]),
('failed', [('tor', 'watchdog')]),
('failed', [('tor', 'fail')])])
with _sm_container_2:
# x:67 y:108
OperatableStateMachine.add('test',
RunTrajectory(file="poubelle_valide",
duration=7),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:327 y:130
OperatableStateMachine.add('tor',
ReadTorque(
watchdog=7,
Joint="right_elbow_yaw_joint",
Threshold=1.6,
min_time=1),
transitions={
'threshold': 'finished',
'watchdog': 'failed',
'fail': 'failed'
},
autonomy={
'threshold': Autonomy.Off,
'watchdog': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'torque': 'torque'})
# x:30 y:365, x:130 y:365
_sm_trajectory_to_transport_pose_3 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_trajectory_to_transport_pose_3:
# x:30 y:40
OperatableStateMachine.add('trajectory to transport pose',
RunTrajectory(file="poubelle_transport",
duration=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:479 y:236, x:469 y:51, x:462 y:111, x:471 y:160, x:430 y:365, x:530 y:365
_sm_trajectory_down_with_torque_limit_4 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('finished', [('torque control', 'finished')]),
('finished', [('trajectory down', 'finished')]),
('failed', [('trajectory down', 'failed')]),
('failed', [('torque control', 'failed')])])
with _sm_trajectory_down_with_torque_limit_4:
# x:109 y:63
OperatableStateMachine.add('trajectory down',
_sm_trajectory_down_1,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:102 y:211
OperatableStateMachine.add('torque control',
_sm_torque_control_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:30 y:458, x:130 y:458
_sm_has_bag_in_gripper_5 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_has_bag_in_gripper_5:
# x:191 y:132
OperatableStateMachine.add('Container',
_sm_container_2,
transitions={
'finished': 'ok',
'failed': 'bad'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:18 y:266
OperatableStateMachine.add('ok',
SaraSay(sentence="Yay! I got it!",
input_keys=[],
emotion=6,
block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:238 y:279
OperatableStateMachine.add('bad',
SaraSay(sentence="Woops! I missed!",
input_keys=[],
emotion=3,
block=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:30 y:458, x:130 y:458
_sm_trajectory_up_6 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_trajectory_up_6:
# x:71 y:122
OperatableStateMachine.add('trajectory up',
RunTrajectory(file="poubelle_eloigne",
duration=10),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:30 y:458, x:130 y:458
_sm_close_gripper_7 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_close_gripper_7:
# x:79 y:177
OperatableStateMachine.add(
'close gripper',
SetGripperState(width=0, effort=1),
transitions={
'object': 'finished',
'no_object': 'retry close'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:272 y:237
OperatableStateMachine.add('retry close',
ForLoop(repeat=1),
transitions={
'do': 'close gripper',
'end': 'failed'
},
autonomy={
'do': Autonomy.Off,
'end': Autonomy.Off
},
remapping={'index': 'index'})
with _state_machine:
# x:89 y:35
OperatableStateMachine.add('head down',
SaraSetHeadAngle(pitch=0.8, yaw=0),
transitions={'done': 'open gripper'},
autonomy={'done': Autonomy.Off})
# x:484 y:164
OperatableStateMachine.add('close gripper',
_sm_close_gripper_7,
transitions={
'finished': 'trajectory up',
'failed': 'trajectory up'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:473 y:300
OperatableStateMachine.add('trajectory up',
_sm_trajectory_up_6,
transitions={
'finished': 'wait a little bit',
'failed': 'wait a little bit'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:232 y:434
OperatableStateMachine.add('has bag in gripper',
_sm_has_bag_in_gripper_5,
transitions={
'finished':
'trajectory to transport pose',
'failed': 'open gripper'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:467 y:33
OperatableStateMachine.add('trajectory down with torque limit',
_sm_trajectory_down_with_torque_limit_4,
transitions={
'finished': 'close gripper',
'failed': 'head down'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:283 y:83
OperatableStateMachine.add(
'open gripper',
SetGripperState(width=0.12, effort=0),
transitions={
'object': 'trajectory down with torque limit',
'no_object': 'trajectory down with torque limit'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:557 y:454
OperatableStateMachine.add('trajectory to transport pose',
_sm_trajectory_to_transport_pose_3,
transitions={
'finished': 'finished',
'failed': 'finished'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:356 y:357
OperatableStateMachine.add(
'wait a little bit',
WaitState(wait_time=1),
transitions={'done': 'has bag in gripper'},
autonomy={'done': Autonomy.Off})
return _state_machine