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:738 y:371, x:736 y:220
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.bin1Waypoint = "trash bin 2"
_state_machine.userdata.bin2Waypoint = "trash bin"
_state_machine.userdata.bin1Height = "1"
_state_machine.userdata.bin2Height = "1"
_state_machine.userdata.dropzone1Waypoint = "drop_zone1"
_state_machine.userdata.dropzone2Waypoint = "drop_zone2"
_state_machine.userdata.DoorName = "door_1_entry"
_state_machine.userdata.exit_door = "door_2_exit"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:810 y:644, x:808 y:125
_sm_go_to_drop_the_bag_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['dropzoneWaypoint', 'exit_door'])
with _sm_go_to_drop_the_bag_0:
# x:84 y:36
OperatableStateMachine.add('say drop',
SaraSay(sentence="I will go and drop this bag in the drop zone.", input_keys=[], emotion=0, block=False),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:66 y:598
OperatableStateMachine.add('open the gripper',
SetGripperState(width=0.1, effort=1),
transitions={'object': 'go to pose transport', 'no_object': 'go to pose transport'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:341 y:469
OperatableStateMachine.add('say cant go to drop zone',
SaraSay(sentence="I am not able to go to the drop zone. I will put the bag here and go to the second bin.", input_keys=[], emotion=0, block=True),
transitions={'done': 'run depose sac'},
autonomy={'done': Autonomy.Off})
# x:74 y:490
OperatableStateMachine.add('run depose sac',
RunTrajectory(file="poubelle_depose", duration=10),
transitions={'done': 'open the gripper'},
autonomy={'done': Autonomy.Off})
# x:69 y:726
OperatableStateMachine.add('go to pose transport',
RunTrajectory(file="transport", duration=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:76 y:386
OperatableStateMachine.add('Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'First bin/go to drop the bag/Action_Move'),
transitions={'finished': 'run depose sac', 'failed': 'say cant go to drop zone'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'dropzoneWaypoint'})
# x:570 y:519, x:575 y:330
_sm_find_the_bin_1 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_find_the_bin_1:
# x:89 y:62
OperatableStateMachine.add('place arm',
RunTrajectory(file="poubelle_init", duration=0),
transitions={'done': 'leve la tete'},
autonomy={'done': Autonomy.Off})
# x:270 y:378
OperatableStateMachine.add('get waypoint',
Get_Reacheable_Waypoint(),
transitions={'done': 'move'},
autonomy={'done': Autonomy.Off},
remapping={'pose_in': 'detectedBin', 'distance': 'distanceToBin', 'pose_out': 'poseToBin'})
# x:296 y:284
OperatableStateMachine.add('set distance to bin',
SetKey(Value=0.5),
transitions={'done': 'get waypoint'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distanceToBin'})
# x:278 y:201
OperatableStateMachine.add('pose form lidar to map',
TF_transformation(in_ref="front_hokuyo_link", out_ref="map"),
transitions={'done': 'set distance to bin', 'fail': 'failed'},
autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'in_pos': 'detectedObstacle', 'out_pos': 'detectedBin'})
# x:80 y:514
OperatableStateMachine.add('redo ajustements de la position',
ForLoop(repeat=2),
transitions={'do': 'find closest obstacle point', 'end': 'finished'},
autonomy={'do': Autonomy.Off, 'end': Autonomy.Off},
remapping={'index': 'index'})
# x:87 y:129
OperatableStateMachine.add('find closest obstacle point',
GetClosestObstacle(topic="/scan", maximumDistance=2),
transitions={'done': 'pose form lidar to map'},
autonomy={'done': Autonomy.Off},
remapping={'Angle': 'Angle', 'distance': 'distance', 'position': 'detectedObstacle'})
# x:288 y:77
OperatableStateMachine.add('leve la tete',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'find closest obstacle point'},
autonomy={'done': Autonomy.Off})
# x:287 y:464
OperatableStateMachine.add('move',
SaraMoveBase(reference="map"),
transitions={'arrived': 'redo ajustements de la position', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'pose': 'poseToBin'})
# x:573 y:546, x:622 y:92
_sm_go_to_bin_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin1Waypoint'])
with _sm_go_to_bin_2:
# x:86 y:104
OperatableStateMachine.add('bras sur le cote',
RunTrajectory(file="poubelle_transport", duration=0),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:254 y:199
OperatableStateMachine.add('Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'First bin/go to bin/Action_Move'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'bin1Waypoint'})
# x:600 y:617, x:666 y:442
_sm_find_the_bin_3 = OperatableStateMachine(outcomes=['finished', 'failed'])
with _sm_find_the_bin_3:
# x:89 y:62
OperatableStateMachine.add('place arm',
RunTrajectory(file="poubelle_init", duration=0),
transitions={'done': 'leve la tete'},
autonomy={'done': Autonomy.Off})
# x:268 y:410
OperatableStateMachine.add('get waypoint',
Get_Reacheable_Waypoint(),
transitions={'done': 'move'},
autonomy={'done': Autonomy.Off},
remapping={'pose_in': 'detectedBin', 'distance': 'distanceToBin', 'pose_out': 'poseToBin'})
# x:293 y:326
OperatableStateMachine.add('set distance to bin',
SetKey(Value=0.5),
transitions={'done': 'get waypoint'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'distanceToBin'})
# x:278 y:238
OperatableStateMachine.add('pose form lidar to map',
TF_transformation(in_ref="front_hokuyo_link", out_ref="map"),
transitions={'done': 'set distance to bin', 'fail': 'failed'},
autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'in_pos': 'detectedObstacle', 'out_pos': 'detectedBin'})
# x:80 y:598
OperatableStateMachine.add('redo ajustements de la position',
ForLoop(repeat=2),
transitions={'do': 'find closest obstacle point', 'end': 'finished'},
autonomy={'do': Autonomy.Off, 'end': Autonomy.Off},
remapping={'index': 'index'})
# x:89 y:168
OperatableStateMachine.add('find closest obstacle point',
GetClosestObstacle(topic="/scan", maximumDistance=2),
transitions={'done': 'pose form lidar to map'},
autonomy={'done': Autonomy.Off},
remapping={'Angle': 'Angle', 'distance': 'distance', 'position': 'detectedObstacle'})
# x:288 y:104
OperatableStateMachine.add('leve la tete',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'find closest obstacle point'},
autonomy={'done': Autonomy.Off})
# x:285 y:504
OperatableStateMachine.add('move',
SaraMoveBase(reference="map"),
transitions={'arrived': 'redo ajustements de la position', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'pose': 'poseToBin'})
# x:30 y:365, x:130 y:365
_sm_go_to_bin_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin2Waypoint'])
with _sm_go_to_bin_4:
# x:59 y:100
OperatableStateMachine.add('bras sur le cote',
RunTrajectory(file="poubelle_transport", duration=0),
transitions={'done': 'Action_Move'},
autonomy={'done': Autonomy.Off})
# x:254 y:199
OperatableStateMachine.add('Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'second bin/go to bin/Action_Move'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'bin2Waypoint'})
# x:418 y:580, x:694 y:339
_sm_go_to_drop_the_bag_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['dropzoneWaypoint', 'exit_door'])
with _sm_go_to_drop_the_bag_5:
# x:65 y:215
OperatableStateMachine.add('Action_Move',
self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'second bin/go to drop the bag/Action_Move'),
transitions={'finished': 'run depose sac', 'failed': 'say cant go to drop zone'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'dropzoneWaypoint'})
# x:385 y:324
OperatableStateMachine.add('say cant go to drop zone',
SaraSay(sentence="I am not able to go to the drop zone. I will put the bag here.", input_keys=[], emotion=0, block=True),
transitions={'done': 'run depose sac'},
autonomy={'done': Autonomy.Off})
# x:76 y:340
OperatableStateMachine.add('run depose sac',
RunTrajectory(file="poubelle_depose", duration=10),
transitions={'done': 'open the gripper'},
autonomy={'done': Autonomy.Off})
# x:74 y:505
OperatableStateMachine.add('go to pose transport',
RunTrajectory(file="transport", duration=0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:74 y:426
OperatableStateMachine.add('open the gripper',
SetGripperState(width=0.1, effort=1),
transitions={'object': 'go to pose transport', 'no_object': 'go to pose transport'},
autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off},
remapping={'object_size': 'object_size'})
# x:704 y:599, x:717 y:53
_sm_first_bin_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin1Waypoint', 'bin1Height', 'dropzoneWaypoint', 'exit_door'])
with _sm_first_bin_6:
# x:57 y:26
OperatableStateMachine.add('Init_Sequence',
self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'First bin/Init_Sequence'),
transitions={'finished': 'go to bin', 'failed': 'go to bin'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:257 y:329
OperatableStateMachine.add('Action_TakeBag',
self.use_behavior(sara_flexbe_behaviors__Action_TakeBagSM, 'First bin/Action_TakeBag'),
transitions={'finished': 'go to drop the bag', 'failed': 'Missed It once'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:275 y:35
OperatableStateMachine.add('go to bin',
_sm_go_to_bin_2,
transitions={'finished': 'find the bin', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'bin1Waypoint': 'bin1Waypoint'})
# x:286 y:164
OperatableStateMachine.add('find the bin',
_sm_find_the_bin_1,
transitions={'finished': 'Try twice', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:292 y:234
OperatableStateMachine.add('Try twice',
ForLoop(repeat=1),
transitions={'do': 'Action_TakeBag', 'end': 'failed'},
autonomy={'do': Autonomy.Off, 'end': Autonomy.Off},
remapping={'index': 'index'})
# x:102 y:262
OperatableStateMachine.add('Missed It once',
SaraSay(sentence="I will try one more time.", input_keys=[], emotion=0, block=True),
transitions={'done': 'Try twice'},
autonomy={'done': Autonomy.Off})
# x:267 y:432
OperatableStateMachine.add('go to drop the bag',
_sm_go_to_drop_the_bag_0,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'dropzoneWaypoint': 'dropzoneWaypoint', 'exit_door': 'exit_door'})
# x:946 y:467, x:907 y:75
_sm_second_bin_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin2Waypoint', 'bin2Height', 'dropzoneWaypoint', 'exit_door'])
with _sm_second_bin_7:
# x:57 y:26
OperatableStateMachine.add('Init_Sequence',
self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'second bin/Init_Sequence'),
transitions={'finished': 'go to bin', 'failed': 'go to bin'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:267 y:432
OperatableStateMachine.add('go to drop the bag',
_sm_go_to_drop_the_bag_5,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'dropzoneWaypoint': 'dropzoneWaypoint', 'exit_door': 'exit_door'})
# x:257 y:346
OperatableStateMachine.add('Action_TakeBag',
self.use_behavior(sara_flexbe_behaviors__Action_TakeBagSM, 'second bin/Action_TakeBag'),
transitions={'finished': 'go to drop the bag', 'failed': 'Missed It once'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:275 y:35
OperatableStateMachine.add('go to bin',
_sm_go_to_bin_4,
transitions={'finished': 'find the bin', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'bin2Waypoint': 'bin2Waypoint'})
# x:286 y:164
OperatableStateMachine.add('find the bin',
_sm_find_the_bin_3,
transitions={'finished': 'Try twice', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:298 y:254
OperatableStateMachine.add('Try twice',
ForLoop(repeat=1),
transitions={'do': 'Action_TakeBag', 'end': 'failed'},
autonomy={'do': Autonomy.Off, 'end': Autonomy.Off},
remapping={'index': 'index'})
# x:102 y:262
OperatableStateMachine.add('Missed It once',
SaraSay(sentence="I will try one more time.", input_keys=[], emotion=0, block=True),
transitions={'done': 'Try twice'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:15 y:242
OperatableStateMachine.add('button',
ContinueButton(),
transitions={'true': 'Action_Pass_Door', 'false': 'Action_Pass_Door'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off})
# x:485 y:201
OperatableStateMachine.add('try second bin',
SaraSay(sentence="I failed to take out the garbage from the first bin but I will try the second bin.", input_keys=[], emotion=0, block=True),
transitions={'done': 'second bin'},
autonomy={'done': Autonomy.Off})
# x:267 y:199
OperatableStateMachine.add('say take second bag',
SaraSay(sentence="Good! I will now get rid of the second bag.", input_keys=[], emotion=0, block=True),
transitions={'done': 'second bin'},
autonomy={'done': Autonomy.Off})
# x:267 y:376
OperatableStateMachine.add('second bin',
_sm_second_bin_7,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'bin2Waypoint': 'bin2Waypoint', 'bin2Height': 'bin2Height', 'dropzoneWaypoint': 'dropzone2Waypoint', 'exit_door': 'exit_door'})
# x:94 y:153
OperatableStateMachine.add('IllManageSomehow',
SaraSay(sentence="Hmmm. This is a hard door to pass...", input_keys=[], emotion=0, block=True),
transitions={'done': 'First bin'},
autonomy={'done': Autonomy.Off})
# x:277 y:63
OperatableStateMachine.add('First bin',
_sm_first_bin_6,
transitions={'finished': 'say take second bag', 'failed': 'try second bin'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'bin1Waypoint': 'bin1Waypoint', 'bin1Height': 'bin1Height', 'dropzoneWaypoint': 'dropzone1Waypoint', 'exit_door': 'exit_door'})
# x:65 y:53
OperatableStateMachine.add('Action_Pass_Door',
self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'Action_Pass_Door'),
transitions={'Done': 'First bin', 'Fail': 'IllManageSomehow'},
autonomy={'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit},
remapping={'DoorName': 'DoorName'})
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:831 y:173, x:520 y:222, x:335 y:32, x:701 y:385
_state_machine = OperatableStateMachine(
outcomes=['success', 'unreachable', 'not found', 'dropped'],
input_keys=['objectID', 'Entity'])
_state_machine.userdata.objectID = 1585
_state_machine.userdata.PreGripPose = "PreGripPose"
_state_machine.userdata.PostGripPose = "PostGripPose"
_state_machine.userdata.Entity = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365
_sm_look_at_it_for_3s_0 = ConcurrencyContainer(
outcomes=['done'],
input_keys=['ID'],
conditions=[('done', [('look', 'failed')]),
('done', [('wait', 'done')])])
with _sm_look_at_it_for_3s_0:
# x:30 y:54
OperatableStateMachine.add('look',
KeepLookingAt(),
transitions={'failed': 'done'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'ID'})
# x:187 y:111
OperatableStateMachine.add('wait',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:30 y:324
_sm_getting_close_1 = OperatableStateMachine(outcomes=['done'],
input_keys=['pose_app'])
with _sm_getting_close_1:
# x:36 y:40
OperatableStateMachine.add(
'gripperopen',
SetGripperState(width=0.15, effort=1),
transitions={
'object': 'move_approach',
'no_object': 'move_approach'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:30 y:129
OperatableStateMachine.add('move_approach',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'almost have it',
'failed': 'almost have it'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'pose_app'})
# x:39 y:204
OperatableStateMachine.add('almost have it',
SaraSay(sentence="I am close",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:30 y:324, x:130 y:324
_sm_get_away_from_failure_2 = OperatableStateMachine(
outcomes=['done', 'failed'], input_keys=['PostGripPose'])
with _sm_get_away_from_failure_2:
# x:30 y:40
OperatableStateMachine.add(
'open 2',
SetGripperState(width=0.1, effort=1),
transitions={
'object': 'move back',
'no_object': 'move back'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:235 y:50
OperatableStateMachine.add('move back',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PostGripPose'})
# x:30 y:324, x:130 y:324
_sm_lift_object_3 = OperatableStateMachine(
outcomes=['done', 'failed'], input_keys=['pose_lift', 'pose_ret'])
with _sm_lift_object_3:
# x:30 y:176
OperatableStateMachine.add('move_lift_object',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'genpose',
'failed': 'genpose'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'pose_lift'})
# x:228 y:176
OperatableStateMachine.add('move_Return',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'pose_ret'})
# x:38 y:45
OperatableStateMachine.add('genpose',
GenPoseEuler(x=-0.2,
y=0,
z=0,
roll=0,
pitch=0,
yaw=0),
transitions={'done': 'move_back'},
autonomy={'done': Autonomy.Off},
remapping={'pose': 'backPose'})
# x:211 y:40
OperatableStateMachine.add('move_back',
SaraMoveBase(reference="base_link"),
transitions={
'arrived': 'move_Return',
'failed': 'move_Return'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'backPose'})
# x:499 y:227, x:263 y:214, x:271 y:492
_sm_pregrip_4 = OperatableStateMachine(
outcomes=['fail', 'failed', 'done'],
input_keys=['PreGripPose', 'posobjet'],
output_keys=['pose_app', 'grippose', 'pose_lift', 'pose_ret'])
with _sm_pregrip_4:
# x:70 y:40
OperatableStateMachine.add('gen_gripPose',
GenGripperPose(l=0.0, z=0, planar=True),
transitions={
'done': 'checkifposeaccess',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'grippose'
})
# x:56 y:256
OperatableStateMachine.add('move_PreGrip',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'gen_returnPose',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PreGripPose'})
# x:30 y:398
OperatableStateMachine.add('gen_approachPose',
GenGripperPose(l=0.15, z=0,
planar=True),
transitions={
'done': 'gen_liftPose',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'pose_app'
})
# x:40 y:469
OperatableStateMachine.add('gen_liftPose',
GenGripperPose(l=0, z=0.1, planar=True),
transitions={
'done': 'done',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'pose_lift'
})
# x:43 y:326
OperatableStateMachine.add('gen_returnPose',
GenGripperPose(l=0.2,
z=0.1,
planar=True),
transitions={
'done': 'gen_approachPose',
'fail': 'fail'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'pose_in': 'posobjet',
'pose_out': 'pose_ret'
})
# x:63 y:184
OperatableStateMachine.add('say can reach',
SaraSay(sentence="I will grab it",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'move_PreGrip'},
autonomy={'done': Autonomy.Off})
# x:58 y:114
OperatableStateMachine.add('checkifposeaccess',
MoveitMove(move=False,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'say can reach',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grippose'})
# x:315 y:40, x:130 y:465
_sm_get_object_5 = OperatableStateMachine(
outcomes=['not_found', 'finished'],
input_keys=['objectID'],
output_keys=['posobjet'])
with _sm_get_object_5:
# x:55 y:40
OperatableStateMachine.add('getobject',
GetEntityByID(),
transitions={
'found': 'Say_See_It',
'not_found': 'not_found'
},
autonomy={
'found': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ID': 'objectID',
'Entity': 'Entity'
})
# x:77 y:370
OperatableStateMachine.add(
'getpose',
CalculationState(calculation=lambda x: x.position),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Entity',
'output_value': 'posobjet'
})
# x:71 y:262
OperatableStateMachine.add('Look at it for 3s',
_sm_look_at_it_for_3s_0,
transitions={'done': 'getpose'},
autonomy={'done': Autonomy.Inherit},
remapping={'ID': 'objectID'})
# x:31 y:133
OperatableStateMachine.add(
'Say_See_It',
SaraSay(sentence=lambda x: "I see the " + x.name,
input_keys=["Entity"],
emotion=0,
block=True),
transitions={'done': 'Look at it for 3s'},
autonomy={'done': Autonomy.Off},
remapping={'Entity': 'Entity'})
with _state_machine:
# x:77 y:23
OperatableStateMachine.add('Get object',
_sm_get_object_5,
transitions={
'not_found': 'not found',
'finished': 'transform point'
},
autonomy={
'not_found': Autonomy.Inherit,
'finished': Autonomy.Inherit
},
remapping={
'objectID': 'objectID',
'posobjet': 'posobjet'
})
# x:258 y:385
OperatableStateMachine.add(
'gripclose',
SetGripperState(width=0, effort=250),
transitions={
'object': 'say picked',
'no_object': 'get away from failure'
},
autonomy={
'object': Autonomy.Off,
'no_object': Autonomy.Off
},
remapping={'object_size': 'object_size'})
# x:53 y:374
OperatableStateMachine.add('move_on_object',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'gripclose',
'failed': 'cant reach'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'grippose'})
# x:650 y:290
OperatableStateMachine.add('move_PostGrip',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm"),
transitions={
'done': 'welcome',
'failed': 'unreachable'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'PostGripPose'})
# x:277 y:200
OperatableStateMachine.add('cant reach',
SaraSay(
sentence="Hum. I can't reach it.",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'unreachable'},
autonomy={'done': Autonomy.Off})
# x:265 y:291
OperatableStateMachine.add('say picked',
SaraSay(sentence="I think I got it",
input_keys=[],
emotion=1,
block=False),
transitions={'done': 'Lift object'},
autonomy={'done': Autonomy.Off})
# x:837 y:291
OperatableStateMachine.add('welcome',
SaraSay(sentence="you are welcome",
input_keys=[],
emotion=1,
block=True),
transitions={'done': 'success'},
autonomy={'done': Autonomy.Off})
# x:62 y:190
OperatableStateMachine.add('PreGrip',
_sm_pregrip_4,
transitions={
'fail': 'cant reach',
'failed': 'cant reach',
'done': 'Getting close'
},
autonomy={
'fail': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'done': Autonomy.Inherit
},
remapping={
'PreGripPose': 'PreGripPose',
'posobjet': 'posobjet',
'pose_app': 'pose_app',
'grippose': 'grippose',
'pose_lift': 'pose_lift',
'pose_ret': 'pose_ret'
})
# x:67 y:114
OperatableStateMachine.add('transform point',
TF_transformation(in_ref="map",
out_ref="base_link"),
transitions={
'done': 'PreGrip',
'fail': 'Get object'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'in_pos': 'posobjet',
'out_pos': 'posobjet'
})
# x:439 y:287
OperatableStateMachine.add('Lift object',
_sm_lift_object_3,
transitions={
'done': 'move_PostGrip',
'failed': 'move_PostGrip'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'pose_lift': 'pose_lift',
'pose_ret': 'pose_ret'
})
# x:441 y:391
OperatableStateMachine.add(
'get away from failure',
_sm_get_away_from_failure_2,
transitions={
'done': 'dropped',
'failed': 'dropped'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'PostGripPose': 'PostGripPose'})
# x:47 y:279
OperatableStateMachine.add('Getting close',
_sm_getting_close_1,
transitions={'done': 'move_on_object'},
autonomy={'done': Autonomy.Inherit},
remapping={'pose_app': 'pose_app'})
return _state_machine