def create(self):
# x:30 y:365, x:599 y:427
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:378 y:183
OperatableStateMachine.add('start',
InputState(request=integer, message=eeeee),
transitions={'received': 'eeeeeeeeeee', 'aborted': 'failed', 'no_connection': 'eeeeeeeeeee', 'data_error': 'finished'},
autonomy={'received': Autonomy.Off, 'aborted': Autonomy.Off, 'no_connection': Autonomy.Off, 'data_error': Autonomy.Off},
remapping={'data': 'data'})
# x:1115 y:242
OperatableStateMachine.add('eeeeeeeeeee',
DecisionState(outcomes="LOL", conditions=input_value > 0),
transitions={'LOL': 'failed'},
autonomy={'LOL': Autonomy.Off},
remapping={'input_value': 'data'})
# x:808 y:383
OperatableStateMachine.add('lmFAOR',
CalculationState(calculation=54x-5w),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'input_value', 'output_value': 'output_value'})
return _state_machine
def create(self):
# x:33 y:340, x:133 y:340, x:233 y:340, x:144 y:140
_state_machine = OperatableStateMachine(
outcomes=['L_B', 'M_B', 'H_B', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:107 y:24
OperatableStateMachine.add('w1',
WaitState(wait_time=4),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:351 y:24
OperatableStateMachine.add('s1',
SubscriberState(topic='/FourWD/battery',
blocking=True,
clear=False),
transitions={
'received': 'w2',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'battery_level'})
# x:357 y:224
OperatableStateMachine.add('w2',
WaitState(wait_time=1),
transitions={'done': 'd1'},
autonomy={'done': Autonomy.Off})
# x:105 y:224
OperatableStateMachine.add(
'd1',
DecisionState(outcomes=['Low', 'Medium', 'High'],
conditions=lambda x: 'Low'
if x.data < 98 else 'Medium'),
transitions={
'Low': 'L_B',
'Medium': 'M_B',
'High': 'H_B'
},
autonomy={
'Low': Autonomy.Off,
'Medium': Autonomy.Off,
'High': Autonomy.Off
},
remapping={'input_value': 'battery_level'})
return _state_machine
def create(self):
# x:67 y:463, x:336 y:160
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['data'])
_state_machine.userdata.data = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Wait',
WaitState(wait_time=0.5),
transitions={'done': 'Calculate'},
autonomy={'done': Autonomy.Off})
# x:36 y:240
OperatableStateMachine.add('Log Param',
flexbe_states__LogState(text=self.param,
severity=2),
transitions={'done': 'Verify Input'},
autonomy={'done': Autonomy.Off})
# x:32 y:340
OperatableStateMachine.add('Verify Input',
DecisionState(
outcomes=['accepted', 'rejected'],
conditions=lambda x: 'accepted'
if x > 3 else 'rejected'),
transitions={
'accepted': 'finished',
'rejected': 'failed'
},
autonomy={
'accepted': Autonomy.Off,
'rejected': Autonomy.Off
},
remapping={'input_value': 'data'})
# x:28 y:136
OperatableStateMachine.add(
'Calculate',
CalculationState(calculation=self._calculate),
transitions={'done': 'Log Param'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'data',
'output_value': 'data'
})
return _state_machine
def create(self):
# x:83 y:490, x:83 y:290
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.waypoint_charging = {'coordinate':{'x':0.0, 'y':0.0, 'theta':0.0}, 'increment':{'x':'none', 'y':'none', 'theta':'none'}}
_state_machine.userdata.curr_pose = 'none'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:157 y:74
OperatableStateMachine.add('w1',
WaitState(wait_time=1),
transitions={'done': 'm1'},
autonomy={'done': Autonomy.Off})
# x:481 y:74
OperatableStateMachine.add('m1',
MoveBaseState(),
transitions={'arrived': 's1', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'waypoint_charging', 'curr_pose': 'curr_pose'})
# x:501 y:174
OperatableStateMachine.add('s1',
SubscriberState(topic='/FourWD/battery', blocking=True, clear=False),
transitions={'received': 'd1', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'battery_level'})
# x:505 y:424
OperatableStateMachine.add('d1',
DecisionState(outcomes=['full', 'not_full'], conditions=lambda x: 'full' if x.data>99.9 else 'not_full'),
transitions={'full': 'finished', 'not_full': 'w2'},
autonomy={'full': Autonomy.Off, 'not_full': Autonomy.Off},
remapping={'input_value': 'battery_level'})
# x:357 y:324
OperatableStateMachine.add('w2',
WaitState(wait_time=1),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:832 y:94, x:692 y:405
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.joint_values_left = [4.824961332294557, 4.986228519472087, 2.016893253088309, 9.056585287421564, 1.7655888505129436, 2.5387597755555658]
_state_machine.userdata.joint_values_right = [4.761117371482087, 4.435244724700612, 1.6085220010066865, 3.2511244744831167, 1.706054283818386, 2.4325377312649925]
_state_machine.userdata.joint_names = ["m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:106 y:117
OperatableStateMachine.add('sub',
SubscriberState(topic='/robotender', blocking=True, clear=False),
transitions={'received': 'decide', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'message'})
# x:280 y:116
OperatableStateMachine.add('decide',
DecisionState(outcomes=['go_left','go_right'], conditions=lambda x: "go_right" if x.data=="R" else "go_left"),
transitions={'go_left': 'left', 'go_right': 'right'},
autonomy={'go_left': Autonomy.Off, 'go_right': Autonomy.Off},
remapping={'input_value': 'message'})
# x:460 y:22
OperatableStateMachine.add('left',
FeedbackJointStateToMoveit(move_group="arm", action_topic="/move_group", robot_name="m1n6s200", position_topic='/m1n6s200_driver/joint_states', delta=1E-4),
transitions={'reached': 'sub', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'joint_values': 'joint_values_left', 'joint_names': 'joint_names'})
# x:452 y:234
OperatableStateMachine.add('right',
FeedbackJointStateToMoveit(move_group="arm", action_topic="/move_group", robot_name="m1n6s200", position_topic='/m1n6s200_driver/joint_states', delta=1E-4),
transitions={'reached': 'sub', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'joint_values': 'joint_values_right', 'joint_names': 'joint_names'})
return _state_machine
def create(self):
# x:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['data'],
output_keys=['result'])
_state_machine.userdata.data = None
_state_machine.userdata.result = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:40 y:73
OperatableStateMachine.add(
'Modify Data',
CalculationState(calculation=lambda x: x * 2),
transitions={'done': 'Decide Param'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'data',
'output_value': 'result'
})
# x:37 y:201
OperatableStateMachine.add(
'Decide Param',
DecisionState(outcomes=['finished', 'failed'],
conditions=lambda x: 'finished'
if self.value == 'correct' else 'failed'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'input_value': 'data'})
return _state_machine
def create(self):
# x:392 y:613, x:739 y:627, x:59 y:602, x:34 y:328
_state_machine = OperatableStateMachine(
outcomes=['timeout', 'failed', 'invalid_pose', 'unknown_keys'],
input_keys=['key_pressed_msg'],
output_keys=['key_pressed_msg'])
_state_machine.userdata.key_pressed_msg = KeyPressed()
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:353, x:130 y:353, x:626 y:359, x:330 y:353, x:430 y:353, x:530 y:353
_sm_waitkey_0 = ConcurrencyContainer(
outcomes=['received', 'timeout', 'failed'],
output_keys=['key_pressed_msg'],
conditions=[('received', [('WaitKeyPressed', 'received')]),
('timeout', [('WaitTimeout', 'done')]),
('failed', [('WaitKeyPressed', 'unavailable')])])
with _sm_waitkey_0:
# x:281 y:146
OperatableStateMachine.add(
'WaitKeyPressed',
WaitForMessageState(topic='/hmi/joy_decoder/keys_pressed',
condition=self.trigger,
buffered=False,
clear=True),
transitions={
'received': 'received',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'key_pressed_msg'})
# x:509 y:151
OperatableStateMachine.add('WaitTimeout',
WaitState(wait_time=self.timeout),
transitions={'done': 'timeout'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:51 y:31
OperatableStateMachine.add(
'CheckExitEnter',
DecisionState(
outcomes=['animation', 'exit', 'walk', 'unknown'],
conditions=self.decision),
transitions={
'animation': 'unknown_keys',
'exit': 'unknown_keys',
'walk': 'PrepareForWalk',
'unknown': 'unknown_keys'
},
autonomy={
'animation': Autonomy.Off,
'exit': Autonomy.Off,
'walk': Autonomy.Off,
'unknown': Autonomy.Off
},
remapping={'input_value': 'key_pressed_msg'})
# x:326 y:259
OperatableStateMachine.add(
'CheckExit',
DecisionState(
outcomes=['animation', 'walk', 'exit', 'unknown'],
conditions=self.decision),
transitions={
'animation': 'EndWalkUnknownKeys',
'walk': 'ProcessKeyMsg',
'exit': 'EndWalkUnknownKeys',
'unknown': 'WaitKey'
},
autonomy={
'animation': Autonomy.Off,
'walk': Autonomy.Off,
'exit': Autonomy.Off,
'unknown': Autonomy.Off
},
remapping={'input_value': 'key_pressed_msg'})
# x:703 y:410
OperatableStateMachine.add(
'ExecuteStepSeq',
ExecuteStepSequenceKey(
controller='motion/controller/step_sequence',
trajectory_ns='saved_msgs/step_sequence'),
transitions={
'success': 'WaitKey',
'unavailable': 'WaitKey',
'partial_movement': 'invalid_pose',
'invalid_pose': 'invalid_pose',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'unavailable': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'trajectory_param': 'motion_param'})
# x:431 y:381
OperatableStateMachine.add(
'WaitKey',
_sm_waitkey_0,
transitions={
'received': 'CheckExit',
'timeout': 'EndWalkTimeout',
'failed': 'failed'
},
autonomy={
'received': Autonomy.Inherit,
'timeout': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'key_pressed_msg': 'key_pressed_msg'})
# x:719 y:246
OperatableStateMachine.add(
'CheckNone',
CheckConditionState(predicate=lambda x: x == None),
transitions={
'true': 'WaitKey',
'false': 'ExecuteStepSeq'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={'input_value': 'motion_param'})
# x:108 y:289
OperatableStateMachine.add(
'EndWalkUnknownKeys',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='crouch_end',
trajectory_ns='saved_msgs/joint_trajectory'),
transitions={
'success': 'unknown_keys',
'partial_movement': 'invalid_pose',
'invalid_pose': 'invalid_pose',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
})
# x:379 y:491
OperatableStateMachine.add(
'EndWalkTimeout',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='crouch_end',
trajectory_ns='saved_msgs/joint_trajectory'),
transitions={
'success': 'timeout',
'partial_movement': 'invalid_pose',
'invalid_pose': 'invalid_pose',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
})
# x:408 y:158
OperatableStateMachine.add(
'ProcessKeyMsg',
CalculationState(calculation=self.process_walk),
transitions={'done': 'CheckNone'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'key_pressed_msg',
'output_value': 'motion_param'
})
# x:211 y:72
OperatableStateMachine.add(
'PrepareForWalk',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='crouch_begin',
trajectory_ns='saved_msgs/joint_trajectory'),
transitions={
'success': 'ProcessKeyMsg',
'partial_movement': 'invalid_pose',
'invalid_pose': 'invalid_pose',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
})
return _state_machine
def create(self):
# x:923 y:114
_state_machine = OperatableStateMachine(outcomes=['finished'])
_state_machine.userdata.none = None
_state_machine.userdata.do_turn_torso = False
_state_machine.userdata.pushing_side = 'right' if self.hand_side == 'left' else 'left'
_state_machine.userdata.torso_side = self.hand_side
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:333 y:440, x:433 y:240
_sm_opening_motion_0 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['do_turn_torso', 'pushing_side', 'none', 'torso_side'])
with _sm_opening_motion_0:
# x:438 y:28
OperatableStateMachine.add(
'Branch_Torso_Open',
DecisionState(outcomes=['turn', 'fixed'],
conditions=lambda x: 'turn' if x else 'fixed'),
transitions={
'turn': 'Turn_Torso',
'fixed': 'Go_To_Open_Door_Pose_Straight'
},
autonomy={
'turn': Autonomy.Low,
'fixed': Autonomy.Low
},
remapping={'input_value': 'do_turn_torso'})
# x:57 y:278
OperatableStateMachine.add(
'Go_To_Open_Door_Pose_Turned',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.
DOOR_OPEN_POSE_TURNED,
vel_scaling=0.05,
ignore_collisions=True,
link_paddings={}),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'pushing_side'})
# x:705 y:228
OperatableStateMachine.add(
'Go_To_Open_Door_Pose_Straight',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.
DOOR_OPEN_POSE_STRAIGHT,
vel_scaling=0.05,
ignore_collisions=True,
link_paddings={}),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'pushing_side'})
# x:76 y:128
OperatableStateMachine.add(
'Turn_Torso',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.TURN_TORSO_FULL,
vel_scaling=0.05,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Go_To_Open_Door_Pose_Turned',
'failed': 'Log_No_Turn'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'torso_side'})
# x:484 y:128
OperatableStateMachine.add(
'Set_Turn_Torso_False',
CalculationState(calculation=lambda x: False),
transitions={'done': 'Go_To_Open_Door_Pose_Straight'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'none',
'output_value': 'do_turn_torso'
})
# x:305 y:128
OperatableStateMachine.add(
'Log_No_Turn',
LogState(text="Skip turning because of collision",
severity=Logger.REPORT_INFO),
transitions={'done': 'Set_Turn_Torso_False'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:287 y:78
OperatableStateMachine.add(
'Decide_If_Turn',
OperatorDecisionState(outcomes=['turn_torso', 'fixed_torso'],
hint=None,
suggestion=None),
transitions={
'turn_torso': 'Decide_Go_To_Stand',
'fixed_torso': 'Go_To_Door_Ready_Pose'
},
autonomy={
'turn_torso': Autonomy.Full,
'fixed_torso': Autonomy.Full
})
# x:125 y:478
OperatableStateMachine.add(
'Go_To_Door_Ready_Pose',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE,
vel_scaling=0.2,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Opening_Motion',
'failed': 'Decide_No_Collision_Avoidance'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'pushing_side'})
# x:887 y:378
OperatableStateMachine.add('Ask_If_Open',
OperatorDecisionState(
outcomes=['open', 'push_again'],
hint='Is the door open?',
suggestion='open'),
transitions={
'open': 'Go_To_Final_Stand',
'push_again': 'Log_Try_Push_Again'
},
autonomy={
'open': Autonomy.High,
'push_again': Autonomy.Full
})
# x:521 y:82
OperatableStateMachine.add('Log_Try_Push_Again',
LogState(
text='Move robot closer to door',
severity=Logger.REPORT_HINT),
transitions={'done': 'Decide_If_Turn'},
autonomy={'done': Autonomy.Full})
# x:620 y:628
OperatableStateMachine.add(
'Back_To_Door_Ready_Pose',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE,
vel_scaling=0.3,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Branch_Torso_Retract',
'failed': 'Open_Manually'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'pushing_side'})
# x:136 y:328
OperatableStateMachine.add(
'Set_Turn_Torso_True',
CalculationState(calculation=lambda x: True),
transitions={'done': 'Go_To_Door_Ready_Pose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'none',
'output_value': 'do_turn_torso'
})
# x:186 y:622
OperatableStateMachine.add('Opening_Motion',
_sm_opening_motion_0,
transitions={
'finished':
'Back_To_Door_Ready_Pose',
'failed': 'Open_Manually'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'do_turn_torso': 'do_turn_torso',
'pushing_side': 'pushing_side',
'none': 'none',
'torso_side': 'torso_side'
})
# x:893 y:201
OperatableStateMachine.add(
'Go_To_Final_Stand',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.STAND_POSE,
vel_scaling=0.2,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'finished',
'failed': 'finished'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'none'})
# x:826 y:528
OperatableStateMachine.add(
'Turn_Torso_Center_Pose',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.
TURN_TORSO_CENTER_POSE,
vel_scaling=0.05,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Ask_If_Open',
'failed': 'Open_Manually'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'none'})
# x:1034 y:628
OperatableStateMachine.add(
'Branch_Torso_Retract',
DecisionState(outcomes=['turn', 'fixed'],
conditions=lambda x: 'turn' if x else 'fixed'),
transitions={
'turn': 'Turn_Torso_Center_Pose',
'fixed': 'Ask_If_Open'
},
autonomy={
'turn': Autonomy.Low,
'fixed': Autonomy.Low
},
remapping={'input_value': 'do_turn_torso'})
# x:26 y:278
OperatableStateMachine.add(
'Go_To_Stand',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.STAND_POSE,
vel_scaling=0.2,
ignore_collisions=False,
link_paddings={}),
transitions={
'done': 'Set_Turn_Torso_True',
'failed': 'Go_To_Door_Ready_Pose'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'none'})
# x:136 y:128
OperatableStateMachine.add(
'Decide_Go_To_Stand',
DecisionState(outcomes=['stand', 'skip'],
conditions=lambda x: 'stand'
if not x else 'skip'),
transitions={
'stand': 'Check_Hand_Space',
'skip': 'Set_Turn_Torso_True'
},
autonomy={
'stand': Autonomy.Low,
'skip': Autonomy.Low
},
remapping={'input_value': 'do_turn_torso'})
# x:360 y:378
OperatableStateMachine.add(
'Decide_No_Collision_Avoidance',
OperatorDecisionState(outcomes=[
'replan_ignore_collisions', 'continue', 'open_manual'
],
hint="Try again and ignore collisions?",
suggestion='replan_ignore_collisions'),
transitions={
'replan_ignore_collisions': 'Go_To_Door_Ready_Pose_NC',
'continue': 'Opening_Motion',
'open_manual': 'Open_Manually'
},
autonomy={
'replan_ignore_collisions': Autonomy.Full,
'continue': Autonomy.Full,
'open_manual': Autonomy.Full
})
# x:365 y:528
OperatableStateMachine.add(
'Go_To_Door_Ready_Pose_NC',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE,
vel_scaling=0.3,
ignore_collisions=True,
link_paddings={}),
transitions={
'done': 'Opening_Motion',
'failed': 'Open_Manually'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'side': 'pushing_side'})
# x:651 y:528
OperatableStateMachine.add('Open_Manually',
LogState(text="Open door manually",
severity=Logger.REPORT_HINT),
transitions={'done': 'Ask_If_Open'},
autonomy={'done': Autonomy.Full})
# x:40 y:178
OperatableStateMachine.add(
'Check_Hand_Space',
LogState(
text="Make sure the hands have enough space to the door",
severity=Logger.REPORT_HINT),
transitions={'done': 'Go_To_Stand'},
autonomy={'done': Autonomy.Full})
return _state_machine
def create(self):
# x:1683 y:440, x:1433 y:40, x:1433 y:790
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'failed2'])
_state_machine.userdata.pub_roadworks = 'Roadworks'
_state_machine.userdata.pub_slippery_road = 'Slippery_Road'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:57 y:24
OperatableStateMachine.add('w1',
WaitState(wait_time=1),
transitions={'done': 's1'},
autonomy={'done': Autonomy.Off})
# x:143 y:224
OperatableStateMachine.add('carb1',
Carbonara(),
transitions={'continue': 'or1', 'Obstacle': 'w4'},
autonomy={'continue': Autonomy.Off, 'Obstacle': Autonomy.Off},
remapping={'input_value': 'detected', 'output_value': 'closest_object'})
# x:143 y:424
OperatableStateMachine.add('or1',
ObjectRecognition(),
transitions={'Left_Sign': 'turn_left', 'Right_Sign': 'turn_right', 'Roadworks': 'p1', 'Slippery_Road': 'p2', 'Turn_Left_Sign': 'turn_left', 'Turn_Right_Sign': 'turn_right', 'Charging_Station': 'finished', 'Stop_Sign': 'Stop', 'Parking_Sign': 'finished', 'Parking_Spot': 'finished', 'none': 'go_straight'},
autonomy={'Left_Sign': Autonomy.Off, 'Right_Sign': Autonomy.Off, 'Roadworks': Autonomy.Off, 'Slippery_Road': Autonomy.Off, 'Turn_Left_Sign': Autonomy.Off, 'Turn_Right_Sign': Autonomy.Off, 'Charging_Station': Autonomy.Off, 'Stop_Sign': Autonomy.Off, 'Parking_Sign': Autonomy.Off, 'Parking_Spot': Autonomy.Off, 'none': Autonomy.Off},
remapping={'input_value': 'closest_object'})
# x:251 y:24
OperatableStateMachine.add('s1',
SubscriberState(topic='/darknet_ros/bounding_boxes', blocking=True, clear=False),
transitions={'received': 'carb1', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'detected'})
# x:619 y:721
OperatableStateMachine.add('turn_left',
self.use_behavior(turn_leftSM, 'turn_left'),
transitions={'finished': 'finished', 'failed': 'failed2'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:619 y:621
OperatableStateMachine.add('turn_right',
self.use_behavior(turn_rightSM, 'turn_right'),
transitions={'finished': 'finished', 'failed': 'failed2'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:619 y:821
OperatableStateMachine.add('go_straight',
self.use_behavior(go_straightSM, 'go_straight'),
transitions={'finished': 'finished', 'failed': 'failed2'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:591 y:374
OperatableStateMachine.add('p1',
PublisherStringState(topic='/chatter'),
transitions={'done': 'w2'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'pub_roadworks'})
# x:591 y:474
OperatableStateMachine.add('p2',
PublisherStringState(topic='/chatter'),
transitions={'done': 'w3'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'pub_slippery_road'})
# x:857 y:324
OperatableStateMachine.add('w2',
WaitState(wait_time=3),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:857 y:524
OperatableStateMachine.add('w3',
WaitState(wait_time=3),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:769 y:221
OperatableStateMachine.add('Stop',
self.use_behavior(StopSM, 'Stop'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:457 y:74
OperatableStateMachine.add('w4',
WaitState(wait_time=1),
transitions={'done': 'd1'},
autonomy={'done': Autonomy.Off})
# x:805 y:124
OperatableStateMachine.add('d1',
DecisionState(outcomes=['go', 'stop', 'back'], conditions=lambda input_value: 'go' if input_value[1]>3 else ('back' if input_value[1]<1.5 else 'stop')),
transitions={'go': 'go_straight_2', 'stop': 'Stop_2', 'back': 'go_back'},
autonomy={'go': Autonomy.Off, 'stop': Autonomy.Off, 'back': Autonomy.Off},
remapping={'input_value': 'closest_object'})
# x:1519 y:171
OperatableStateMachine.add('go_back',
self.use_behavior(go_backSM, 'go_back'),
transitions={'finished': 'carb1', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:1169 y:221
OperatableStateMachine.add('go_straight_2',
self.use_behavior(go_straightSM, 'go_straight_2'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:469 y:156
OperatableStateMachine.add('Stop_2',
self.use_behavior(StopSM, 'Stop_2'),
transitions={'finished': 'carb1', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM
handle_down_affordance = 'turnCW' if self.hand_side == 'left' else 'turnCCW'
door_affordance = 'push'
handle_up_affordance = 'turnCCW' if self.hand_side == 'left' else 'turnCW'
turn_threshold = 0.6
# x:433 y:590, x:333 y:340
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.grasp_preference = 0
_state_machine.userdata.hand_side = self.hand_side
_state_machine.userdata.none = None
_state_machine.userdata.waypoint_distance = 1.5 # meters
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:733 y:390, x:333 y:40
_sm_hand_back_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'none'])
with _sm_hand_back_0:
# x:84 y:28
OperatableStateMachine.add('Init_Grasp_Preference',
CalculationState(calculation=lambda x: 0),
transitions={'done': 'Get_Grasp_Pose'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'none', 'output_value': 'grasp_preference'})
# x:380 y:128
OperatableStateMachine.add('Inform_Pregrasp_Failed',
LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:476 y:378
OperatableStateMachine.add('Move_To_Pregrasp_Pose',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'finished', 'failed': 'Increase_Preference_Index'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:73 y:428
OperatableStateMachine.add('Increase_Preference_Index',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Get_Grasp_Pose'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'})
# x:191 y:328
OperatableStateMachine.add('Convert_Waypoints',
CalculationState(calculation=lambda msg: [msg.pose]),
transitions={'done': 'Plan_Back_To_Pregrasp'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_waypoints'})
# x:344 y:189
OperatableStateMachine.add('Plan_Back_To_Pregrasp',
PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'planned': 'Move_To_Pregrasp_Pose', 'incomplete': 'Move_To_Pregrasp_Pose', 'failed': 'failed'},
autonomy={'planned': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full},
remapping={'waypoints': 'pregrasp_waypoints', 'hand': 'hand_side', 'frame_id': 'pregrasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:196 y:228
OperatableStateMachine.add('Extract_Frame_Id',
CalculationState(calculation=lambda pose: pose.header.frame_id),
transitions={'done': 'Convert_Waypoints'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_frame_id'})
# x:50 y:124
OperatableStateMachine.add('Get_Grasp_Pose',
GetTemplateGraspState(),
transitions={'done': 'Extract_Frame_Id', 'failed': 'Inform_Pregrasp_Failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'grasp': 'pregrasp_pose'})
# x:1041 y:400, x:987 y:18
_sm_unlock_door_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['handle_template_id', 'hand_side', 'none', 'door_template_id'], output_keys=['turn_fraction'])
with _sm_unlock_door_1:
# x:64 y:28
OperatableStateMachine.add('Get_Handle_Affordance_Down',
GetTemplateAffordanceState(identifier=handle_down_affordance),
transitions={'done': 'Plan_Turn_Handle_Down', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'handle_template_id', 'hand_side': 'hand_side', 'affordance': 'handle_affordance'})
# x:992 y:178
OperatableStateMachine.add('Plan_Push_Door',
PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'done': 'Push_Door', 'incomplete': 'Push_Door', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full},
remapping={'affordance': 'door_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:973 y:78
OperatableStateMachine.add('Get_Push_Affordance',
GetTemplateAffordanceState(identifier=door_affordance),
transitions={'done': 'Plan_Push_Door', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'door_template_id', 'hand_side': 'hand_side', 'affordance': 'door_affordance'})
# x:277 y:78
OperatableStateMachine.add('Plan_Turn_Handle_Down',
PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'done': 'Store_Turn_Down', 'incomplete': 'Decide_Execute_Incomplete', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full},
remapping={'affordance': 'handle_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:676 y:78
OperatableStateMachine.add('Turn_Handle',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'Get_Push_Affordance', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:976 y:278
OperatableStateMachine.add('Push_Door',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'finished', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:71 y:263
OperatableStateMachine.add('Get_Handle_Affordance_Up',
GetTemplateAffordanceState(identifier=handle_up_affordance),
transitions={'done': 'Plan_Turn_Handle_Up', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'handle_template_id', 'hand_side': 'hand_side', 'affordance': 'handle_affordance'})
# x:284 y:270
OperatableStateMachine.add('Plan_Turn_Handle_Up',
PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'done': 'Store_Turn_Up', 'incomplete': 'Store_Turn_Up', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full},
remapping={'affordance': 'handle_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:70 y:178
OperatableStateMachine.add('Decide_Execute_Incomplete',
DecisionState(outcomes=["up", "down"], conditions=lambda x: "up" if x > turn_threshold else "down"),
transitions={'up': 'Store_Turn_Down', 'down': 'Get_Handle_Affordance_Up'},
autonomy={'up': Autonomy.Low, 'down': Autonomy.Low},
remapping={'input_value': 'plan_fraction'})
# x:494 y:78
OperatableStateMachine.add('Store_Turn_Down',
CalculationState(calculation=lambda x: x),
transitions={'done': 'Turn_Handle'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'plan_fraction', 'output_value': 'turn_fraction'})
# x:501 y:178
OperatableStateMachine.add('Store_Turn_Up',
CalculationState(calculation=lambda x: -x),
transitions={'done': 'Turn_Handle'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'plan_fraction', 'output_value': 'turn_fraction'})
# x:783 y:490, x:133 y:390
_sm_release_handle_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none', 'turn_fraction'])
with _sm_release_handle_2:
# x:33 y:120
OperatableStateMachine.add('Decide_Turn_Direction',
DecisionState(outcomes=["up", "down"], conditions=lambda x: "up" if x > 0 else "down"),
transitions={'up': 'Get_Handle_Affordance_Up', 'down': 'Get_Handle_Affordance_Down'},
autonomy={'up': Autonomy.Low, 'down': Autonomy.Low},
remapping={'input_value': 'turn_fraction'})
# x:744 y:372
OperatableStateMachine.add('Hand_Back',
_sm_hand_back_0,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'none': 'none'})
# x:742 y:128
OperatableStateMachine.add('Plan_Turn_Handle',
PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'done': 'Turn_Handle', 'incomplete': 'Turn_Handle', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full},
remapping={'affordance': 'handle_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:926 y:128
OperatableStateMachine.add('Turn_Handle',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'Open_Fingers', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:932 y:228
OperatableStateMachine.add('Open_Fingers',
FingerConfigurationState(hand_type=self.hand_type, configuration=0.0),
transitions={'done': 'Decide_Retract_Hand', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'hand_side': 'hand_side'})
# x:455 y:128
OperatableStateMachine.add('Reduce_Affordance_Displacement',
FlexibleCalculationState(calculation=self.scale_affordance, input_keys=["affordance", "fraction"]),
transitions={'done': 'Plan_Turn_Handle'},
autonomy={'done': Autonomy.Off},
remapping={'affordance': 'handle_affordance', 'fraction': 'turn_fraction', 'output_value': 'scaled_affordance'})
# x:214 y:178
OperatableStateMachine.add('Get_Handle_Affordance_Down',
GetTemplateAffordanceState(identifier=handle_down_affordance),
transitions={'done': 'Reduce_Affordance_Displacement', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'handle_affordance'})
# x:221 y:78
OperatableStateMachine.add('Get_Handle_Affordance_Up',
GetTemplateAffordanceState(identifier=handle_up_affordance),
transitions={'done': 'Reduce_Affordance_Displacement', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'handle_affordance'})
# x:936 y:378
OperatableStateMachine.add('Decide_Retract_Hand',
OperatorDecisionState(outcomes=['keep', 'back'], hint="Take hand back from handle?", suggestion='keep'),
transitions={'keep': 'finished', 'back': 'Hand_Back'},
autonomy={'keep': Autonomy.High, 'back': Autonomy.Full})
# x:124 y:577, x:483 y:290
_sm_traverse_door_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['waypoint_distance', 'none'])
with _sm_traverse_door_3:
# x:62 y:78
OperatableStateMachine.add('Generate_Traversing_Waypoint',
CalculationState(calculation=lambda d: PoseStamped(header=Header(frame_id="pelvis"), pose=Pose(position=Point(x=d), orientation=Quaternion(w=1)))),
transitions={'done': 'Convert_Waypoint_Frame'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'waypoint_distance', 'output_value': 'waypoint_pelvis'})
# x:68 y:170
OperatableStateMachine.add('Convert_Waypoint_Frame',
GetPoseInFrameState(target_frame='world'),
transitions={'done': 'Create_Step_Goal', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'pose_in': 'waypoint_pelvis', 'pose_out': 'waypoint_world'})
# x:77 y:358
OperatableStateMachine.add('Plan_Through_Door',
PlanFootstepsState(mode=PlanFootstepsState.MODE_STEP_NO_COLLISION),
transitions={'planned': 'Go_Through_Door', 'failed': 'Take_Arms_Side'},
autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full},
remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'})
# x:70 y:450
OperatableStateMachine.add('Go_Through_Door',
ExecuteStepPlanActionState(),
transitions={'finished': 'finished', 'failed': 'Take_Arms_Side'},
autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'plan_header': 'plan_header'})
# x:77 y:256
OperatableStateMachine.add('Create_Step_Goal',
CreateStepGoalState(pose_is_pelvis=True),
transitions={'done': 'Plan_Through_Door', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'target_pose': 'waypoint_world', 'step_goal': 'step_goal'})
# x:286 y:333
OperatableStateMachine.add('Take_Arms_Side',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.BOTH_ARMS_SIDES, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False),
transitions={'done': 'Plan_Through_Door', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'side': 'none'})
# x:348 y:609, x:119 y:410
_sm_open_door_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['none', 'handle_template_id', 'door_template_id', 'hand_side'])
with _sm_open_door_4:
# x:87 y:78
OperatableStateMachine.add('Set_Template_Frame',
CalculationState(calculation=lambda x: 'template_tf_' + str(x)),
transitions={'done': 'Look_At_Handle_Hand'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'door_template_id', 'output_value': 'template_frame'})
# x:364 y:95
OperatableStateMachine.add('Log_Try_To_Open',
LogState(text='Will now try to open', severity=Logger.REPORT_INFO),
transitions={'done': 'Unlock_Door'},
autonomy={'done': Autonomy.High})
# x:576 y:273
OperatableStateMachine.add('Release_Handle',
_sm_release_handle_2,
transitions={'finished': 'Ask_If_Push', 'failed': 'Log_Remove_Hand'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'template_id': 'handle_template_id', 'hand_side': 'hand_side', 'none': 'none', 'turn_fraction': 'turn_fraction'})
# x:777 y:277
OperatableStateMachine.add('Log_Remove_Hand',
LogState(text='Remove hand from handle', severity=Logger.REPORT_HINT),
transitions={'done': 'Ask_If_Push'},
autonomy={'done': Autonomy.Full})
# x:440 y:196
OperatableStateMachine.add('Unlock_Door',
_sm_unlock_door_1,
transitions={'finished': 'Ask_for_Retry', 'failed': 'Ask_for_Retry'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'handle_template_id': 'handle_template_id', 'hand_side': 'hand_side', 'none': 'none', 'door_template_id': 'door_template_id', 'turn_fraction': 'turn_fraction'})
# x:597 y:109
OperatableStateMachine.add('Ask_for_Retry',
OperatorDecisionState(outcomes=["release_handle", "retry"], hint="Now release handle?", suggestion="release_handle"),
transitions={'release_handle': 'Release_Handle', 'retry': 'Log_Try_To_Open'},
autonomy={'release_handle': Autonomy.High, 'retry': Autonomy.Full})
# x:587 y:367
OperatableStateMachine.add('Ask_If_Push',
OperatorDecisionState(outcomes=['push', 'grasp_again'], hint='Is the door slightly open?', suggestion='push'),
transitions={'push': 'Look_Straight', 'grasp_again': 'Grasp_Handle'},
autonomy={'push': Autonomy.High, 'grasp_again': Autonomy.Full})
# x:283 y:472
OperatableStateMachine.add('Push Door Open',
self.use_behavior(PushDoorOpenSM, 'Open_Door/Push Door Open'),
transitions={'finished': 'finished'},
autonomy={'finished': Autonomy.Inherit})
# x:296 y:370
OperatableStateMachine.add('Look_Straight',
LookAtTargetState(),
transitions={'done': 'Push Door Open'},
autonomy={'done': Autonomy.Off},
remapping={'frame': 'none'})
# x:89 y:272
OperatableStateMachine.add('Grasp_Handle',
self.use_behavior(GraspObjectSM, 'Open_Door/Grasp_Handle'),
transitions={'finished': 'Log_Try_To_Open', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'template_id': 'handle_template_id'})
# x:83 y:178
OperatableStateMachine.add('Look_At_Handle_Hand',
LookAtTargetState(),
transitions={'done': 'Grasp_Handle'},
autonomy={'done': Autonomy.Off},
remapping={'frame': 'template_frame'})
with _state_machine:
# x:82 y:28
OperatableStateMachine.add('Get_Door_Template_ID',
InputState(request=InputState.SELECTED_OBJECT_ID, message="Provide the ID of the DOOR template."),
transitions={'received': 'Manipulate_On', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'door_template_id'})
# x:616 y:478
OperatableStateMachine.add('Go_To_Stand',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND),
transitions={'changed': 'Traverse_Door', 'failed': 'failed'},
autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Low})
# x:81 y:122
OperatableStateMachine.add('Walk_to_Template',
self.use_behavior(WalktoTemplateSM, 'Walk_to_Template'),
transitions={'finished': 'Manipulate_On', 'failed': 'failed', 'aborted': 'Manipulate_On'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit},
remapping={'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': 'door_template_id'})
# x:316 y:128
OperatableStateMachine.add('Manipulate_On',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={'changed': 'Align_Door_Log', 'failed': 'failed'},
autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full})
# x:644 y:122
OperatableStateMachine.add('Open_Door',
_sm_open_door_4,
transitions={'finished': 'Wait_For_Gather_Data', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'none': 'none', 'handle_template_id': 'door_template_id', 'door_template_id': 'door_template_id', 'hand_side': 'hand_side'})
# x:634 y:278
OperatableStateMachine.add('Wait_For_Gather_Data',
LogState(text='Gather data from inside', severity=Logger.REPORT_HINT),
transitions={'done': 'Go_To_Stand'},
autonomy={'done': Autonomy.Full})
# x:390 y:472
OperatableStateMachine.add('Traverse_Door',
_sm_traverse_door_3,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'waypoint_distance': 'waypoint_distance', 'none': 'none'})
# x:525 y:34
OperatableStateMachine.add('Align_Door_Log',
LogState(text="Adjust pose of the door template", severity=Logger.REPORT_HINT),
transitions={'done': 'Open_Door'},
autonomy={'done': Autonomy.Full})
return _state_machine
def create(self):
joy_topic = '/hmi/joystick'
# x:30 y:365, x:846 y:376
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.unused = None
_state_machine.userdata.joy_msg = Joy()
_state_machine.userdata.text_msg = TextCommand()
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:353
_sm_waituntiltextcmd_0 = OperatableStateMachine(
outcomes=['received'], output_keys=['text_msg'])
with _sm_waituntiltextcmd_0:
# x:87 y:101
OperatableStateMachine.add(
'WaitTextMsg',
WaitForMessageState(
topic='/control',
condition=lambda x: x.type == 'flexbe/action',
buffered=False,
clear=True),
transitions={
'received': 'received',
'unavailable': 'Wait1s'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'text_msg'})
# x:314 y:97
OperatableStateMachine.add('Wait1s',
WaitState(wait_time=1),
transitions={'done': 'WaitTextMsg'},
autonomy={'done': Autonomy.Off})
# x:397 y:287, x:85 y:291, x:504 y:228, x:330 y:297, x:459 y:297, x:530 y:297
_sm_joystickmovements_1 = ConcurrencyContainer(
outcomes=['failed', 'button1234', 'timeout'],
input_keys=['joy_msg'],
output_keys=['joy_msg'],
conditions=[('timeout', [('JoystickControl', 'done')]),
('failed', [('WaitButton1234Pressed', 'unavailable')]),
('button1234', [('WaitButton1234Pressed', 'received')])
])
with _sm_joystickmovements_1:
# x:385 y:129
OperatableStateMachine.add(
'JoystickControl',
JoystickJointControl(
joints_topic='/joint_states',
goal_joints_topic=
'/motion/controller/joint_state/out_joints_src_reset',
joy_topic=joy_topic,
buttons=[(4, 0.6, 'ear_l_joint', -3.0, 1.0),
(6, -0.6, 'ear_l_joint', -3.0, 1.0),
(5, 0.6, 'ear_r_joint', -3.0, 1.0),
(7, -0.6, 'ear_r_joint', -3.0, 1.0)],
axes=[(0, 0, 'eyes_yaw', -1.5, 1.5),
(1, 0, 'eyes_pitch', -1.5, 1.5),
(4, -0.4, 'head_joint4', -1.5, 1.5),
(5, 0.4, 'head_joint2', -1.5, 1.5),
(3, 0.5, 'mouth_joint', -0.6, 0)],
timeout=5.0),
transitions={'done': 'timeout'},
autonomy={'done': Autonomy.Off})
# x:130 y:136
OperatableStateMachine.add(
'WaitButton1234Pressed',
WaitForMessageState(
topic=joy_topic,
condition=lambda msg: any(msg.buttons[0:4]),
buffered=False,
clear=True),
transitions={
'received': 'button1234',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'joy_msg'})
# x:205 y:274, x:300 y:274, x:68 y:275, x:449 y:266, x:595 y:272, x:833 y:323, x:638 y:392, x:730 y:353, x:949 y:274
_sm_randmovements_2 = ConcurrencyContainer(
outcomes=['failed', 'timeout', 'joy_msg', 'text_msg'],
input_keys=['unused', 'joy_msg', 'text_msg'],
output_keys=['joy_msg', 'text_msg', 'unused'],
conditions=[('failed', [('WaitJoystick', 'unavailable')]),
('failed', [('RandHeadMoves', 'failed')]),
('joy_msg', [('WaitJoystick', 'received')]),
('timeout', [('RandHeadMoves', 'done')]),
('text_msg', [('WaitUntilTextCmd', 'received')])])
with _sm_randmovements_2:
# x:88 y:74
OperatableStateMachine.add(
'WaitJoystick',
WaitForMessageState(
topic=joy_topic,
condition=lambda msg: any(msg.buttons) or any(msg.axes),
buffered=False,
clear=True),
transitions={
'received': 'joy_msg',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'joy_msg'})
# x:332 y:60
OperatableStateMachine.add(
'RandHeadMoves',
RandJointsMovements(controller='joint_state_head',
duration=10,
interval=[2.0, 5.0],
joints=['head_joint2', 'head_joint4'],
minimal=[-0.2, -0.3],
maximal=[0.3, 0.3]),
transitions={
'done': 'timeout',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'unused'})
# x:647 y:50
OperatableStateMachine.add('WaitUntilTextCmd',
_sm_waituntiltextcmd_0,
transitions={'received': 'text_msg'},
autonomy={'received': Autonomy.Inherit},
remapping={'text_msg': 'text_msg'})
with _state_machine:
# x:12 y:169
OperatableStateMachine.add(
'SetNominalPose',
SetJointState(controller='motion/controller/joint_state_head',
pose_param='nominal',
pose_ns='saved_msgs/joint_state',
tolerance=0.017,
timeout=10.0,
joint_topic="joint_states"),
transitions={
'done': 'RandMovements',
'failed': 'failed',
'timeout': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off,
'timeout': Autonomy.Off
})
# x:127 y:370
OperatableStateMachine.add('JoystickMovements',
_sm_joystickmovements_1,
transitions={
'failed': 'failed',
'button1234': 'ProcessButton',
'timeout': 'RandMovements'
},
autonomy={
'failed': Autonomy.Inherit,
'button1234': Autonomy.Inherit,
'timeout': Autonomy.Inherit
},
remapping={'joy_msg': 'joy_msg'})
# x:248 y:186
OperatableStateMachine.add(
'CheckButtonPressed',
DecisionState(outcomes=['button1234', 'other'],
conditions=lambda msg: 'button1234'
if any(msg.buttons[0:4]) else 'other'),
transitions={
'button1234': 'ProcessButton',
'other': 'JoystickMovements'
},
autonomy={
'button1234': Autonomy.Off,
'other': Autonomy.Off
},
remapping={'input_value': 'joy_msg'})
# x:690 y:123
OperatableStateMachine.add(
'SetHeadNominalPose',
SetJointState(controller='motion/controller/joint_state_head',
pose_param='head_nominal',
pose_ns='saved_msgs/joint_state',
tolerance=0.017,
timeout=10.0,
joint_topic="joint_states"),
transitions={
'done': 'ExecuteAction',
'failed': 'failed',
'timeout': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off,
'timeout': Autonomy.Off
})
# x:425 y:181
OperatableStateMachine.add(
'ProcessButton',
CalculationState(calculation=self.process_joy_msg),
transitions={'done': 'SetHeadNominalPose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'joy_msg',
'output_value': 'action_name'
})
# x:272 y:100
OperatableStateMachine.add(
'ProcessTextMsg',
CalculationState(calculation=self.process_text_msg),
transitions={'done': 'SetHeadNominalPose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'text_msg',
'output_value': 'action_name'
})
# x:457 y:12
OperatableStateMachine.add(
'RandomAction',
CalculationState(calculation=self.random_action),
transitions={'done': 'SetHeadNominalPose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'unused',
'output_value': 'action_name'
})
# x:383 y:325
OperatableStateMachine.add(
'ExecuteAction',
ExecuteJointTrajectoryKey(
controller='motion/controller/joint_trajectory',
trajectory_ns='saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'unavalible': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'unavalible': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'trajectory_param': 'action_name'})
# x:308 y:7
OperatableStateMachine.add(
'RandomChoice',
DecisionState(outcomes=['move', 'continue'],
conditions=lambda x: 'continue'
if random.random() < 0.4 else 'move'),
transitions={
'move': 'RandomAction',
'continue': 'RandMovements'
},
autonomy={
'move': Autonomy.Off,
'continue': Autonomy.Off
},
remapping={'input_value': 'unused'})
# x:117 y:39
OperatableStateMachine.add('RandMovements',
_sm_randmovements_2,
transitions={
'failed': 'failed',
'timeout': 'RandomChoice',
'joy_msg': 'CheckButtonPressed',
'text_msg': 'ProcessTextMsg'
},
autonomy={
'failed': Autonomy.Inherit,
'timeout': Autonomy.Inherit,
'joy_msg': Autonomy.Inherit,
'text_msg': Autonomy.Inherit
},
remapping={
'unused': 'unused',
'joy_msg': 'joy_msg',
'text_msg': 'text_msg'
})
return _state_machine
def create(self):
# x:1200 y:570, x:12 y:570
_state_machine = OperatableStateMachine(outcomes=['done', 'failed'],
input_keys=['input_value'])
_state_machine.userdata.input_value = ''
# x:130 y:465, x:230 y:465
_sm_group_01 = OperatableStateMachine(outcomes=['done', 'failed'],
input_keys=['input_value'])
with _sm_group_01:
# x:98 y:96
OperatableStateMachine.add(
'01',
DecisionState(outcomes=['done', 'failed'],
conditions=lambda x: 'done'),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'input_value': 'input_value'})
# x:130 y:465, x:230 y:465
_sm_group_02 = OperatableStateMachine(outcomes=['done', 'failed'],
input_keys=['input_value'])
with _sm_group_02:
# x:98 y:96
OperatableStateMachine.add(
'02',
DecisionState(outcomes=['done', 'failed'],
conditions=lambda x: 'done'),
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'input_value': 'input_value'})
with _state_machine:
# x:181 y:94
OperatableStateMachine.add(
'group',
_sm_group_01,
transitions={
'done': 'group1',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'input_value': 'input_value'})
# x:181 y:154
OperatableStateMachine.add(
'group1',
_sm_group_02,
transitions={
'done': 'done',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'input_value': 'input_value'})
return _state_machine
def create(self):
# x:860 y:786, x:837 y:171, x:828 y:43
_state_machine = OperatableStateMachine(
outcomes=['found', 'failed', 'not_found'],
input_keys=['question'],
output_keys=['entityFound'])
_state_machine.userdata.question = ""
_state_machine.userdata.entityFound = ""
_state_machine.userdata.personKey = "person"
_state_machine.userdata.index = -1
_state_machine.userdata.rotation180degres = -3.14
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:458, x:130 y:458
_sm_keep_looking_at_person_0 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['personID'])
with _sm_keep_looking_at_person_0:
# x:79 y:77
OperatableStateMachine.add('keep looking',
KeepLookingAt(),
transitions={'failed': 'keep looking'},
autonomy={'failed': Autonomy.Off},
remapping={'ID': 'personID'})
# x:30 y:458, x:130 y:458
_sm_ask_1 = OperatableStateMachine(outcomes=['finished', 'failed'],
input_keys=['question'],
output_keys=['answer'])
with _sm_ask_1:
# x:57 y:66
OperatableStateMachine.add('wait',
WaitState(wait_time=3),
transitions={'done': 'Action_Ask'},
autonomy={'done': Autonomy.Off})
# x:46 y:155
OperatableStateMachine.add(
'Action_Ask',
self.use_behavior(
sara_flexbe_behaviors__Action_AskSM,
'ask while looking at person/ask/Action_Ask'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'question': 'question',
'answer': 'answer'
})
# x:871 y:712, x:874 y:127
_sm_get_real_id_2 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['entity', 'personKey', 'FIFO'],
output_keys=['personID'])
with _sm_get_real_id_2:
# x:47 y:32
OperatableStateMachine.add(
'get posittion face or entity',
CalculationState(calculation=lambda x: x.face.boundingBox.
Center if x.face.id != '' else x.position),
transitions={'done': 'get direction to point'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'entity',
'output_value': 'pointToLook'
})
# x:27 y:351
OperatableStateMachine.add('look at point',
SaraSetHeadAngleKey(),
transitions={'done': 'wait 1'},
autonomy={'done': Autonomy.Off},
remapping={
'yaw': 'yaw',
'pitch': 'pitch'
})
# x:40 y:423
OperatableStateMachine.add(
'wait 1',
WaitState(wait_time=1),
transitions={'done': 'list entity in front'},
autonomy={'done': Autonomy.Off})
# x:24 y:491
OperatableStateMachine.add(
'list entity in front',
list_entities_by_name(frontality_level=0.5, distance_max=3),
transitions={
'found':
'fitler the entity list to remove id already checked',
'none_found': 'set key'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'personKey',
'entity_list': 'entity_list',
'number': 'number'
})
# x:42 y:632
OperatableStateMachine.add(
'calcul first entity ID',
CalculationState(calculation=lambda x: x[0].ID),
transitions={'done': 'say real id'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'filteredEntityList',
'output_value': 'personID'
})
# x:518 y:578
OperatableStateMachine.add(
'say real id',
SaraSay(sentence=lambda x: "The real ID is " + str(x[0]) + ".",
input_keys=["personID"],
emotion=0,
block=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'personID': 'personID'})
# x:539 y:164
OperatableStateMachine.add('set key',
SetKey(Value=0),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'Key': 'personID'})
# x:34 y:234
OperatableStateMachine.add('pitch to 0.1 if no face',
FlexibleCalculationState(
calculation=lambda x: x[0]
if x[1].face.id != '' else 0.1,
input_keys=["pitch", "entity"]),
transitions={'done': 'look at point'},
autonomy={'done': Autonomy.Off},
remapping={
'pitch': 'pitch',
'entity': 'entity',
'output_value': 'pitch'
})
# x:37 y:149
OperatableStateMachine.add('get direction to point',
Get_direction_to_point(
frame_origin="base_link",
frame_reference="head_link"),
transitions={
'done': 'pitch to 0.1 if no face',
'fail': 'set key'
},
autonomy={
'done': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'targetPoint': 'pointToLook',
'yaw': 'yaw',
'pitch': 'pitch'
})
# x:29 y:560
OperatableStateMachine.add(
'fitler the entity list to remove id already checked',
FilterKey(filter_function=lambda x: x[0].ID not in x[1],
input_keys=["input_list", "FIFO"]),
transitions={
'not_empty': 'calcul first entity ID',
'empty': 'set key'
},
autonomy={
'not_empty': Autonomy.Off,
'empty': Autonomy.Off
},
remapping={
'input_list': 'entity_list',
'FIFO': 'FIFO',
'output_list': 'filteredEntityList'
})
# x:30 y:458, x:710 y:24
_sm_tourne_tete_et_base_3 = OperatableStateMachine(
outcomes=['done', 'failed'],
input_keys=['index', 'rotation180degres'])
with _sm_tourne_tete_et_base_3:
# x:57 y:27
OperatableStateMachine.add(
'decide',
DecisionState(outcomes=["_0", "_1", "_2", "_3", "_4"],
conditions=lambda x: "_" + str(x)),
transitions={
'_0': 'look center',
'_1': 'look center_2',
'_2': 'look center_3',
'_3': 'action_turn',
'_4': 'look center_5'
},
autonomy={
'_0': Autonomy.Off,
'_1': Autonomy.Off,
'_2': Autonomy.Off,
'_3': Autonomy.Off,
'_4': Autonomy.Off
},
remapping={'input_value': 'index'})
# x:186 y:236
OperatableStateMachine.add('look right',
SaraSetHeadAngle(pitch=0.1, yaw=-1.5),
transitions={'done': 'wait right'},
autonomy={'done': Autonomy.Off})
# x:208 y:307
OperatableStateMachine.add('wait right',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:322 y:109
OperatableStateMachine.add('look center_3',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait center_3'},
autonomy={'done': Autonomy.Off})
# x:325 y:174
OperatableStateMachine.add('wait center_3',
WaitState(wait_time=3),
transitions={'done': 'look right_2'},
autonomy={'done': Autonomy.Off})
# x:318 y:240
OperatableStateMachine.add('look right_2',
SaraSetHeadAngle(pitch=0.1, yaw=1.5),
transitions={'done': 'wait right_2'},
autonomy={'done': Autonomy.Off})
# x:345 y:310
OperatableStateMachine.add('wait right_2',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:489 y:178
OperatableStateMachine.add('look left_2_2',
SaraSetHeadAngle(pitch=0.1, yaw=1.5),
transitions={'done': 'waitleft_2_2'},
autonomy={'done': Autonomy.Off})
# x:506 y:247
OperatableStateMachine.add('waitleft_2_2',
WaitState(wait_time=3),
transitions={'done': 'look center_4'},
autonomy={'done': Autonomy.Off})
# x:501 y:319
OperatableStateMachine.add('look center_4',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait center_4'},
autonomy={'done': Autonomy.Off})
# x:520 y:407
OperatableStateMachine.add('wait center_4',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:476 y:109
OperatableStateMachine.add(
'action_turn',
self.use_behavior(sara_flexbe_behaviors__action_turnSM,
'tourne tete et base/action_turn'),
transitions={
'finished': 'look left_2_2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'rotation': 'rotation180degres'})
# x:199 y:108
OperatableStateMachine.add('look center_2',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait center_2'},
autonomy={'done': Autonomy.Off})
# x:51 y:113
OperatableStateMachine.add('look center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait center'},
autonomy={'done': Autonomy.Off})
# x:56 y:174
OperatableStateMachine.add('wait center',
WaitState(wait_time=4),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:206 y:173
OperatableStateMachine.add('wait center_2',
WaitState(wait_time=2),
transitions={'done': 'look right'},
autonomy={'done': Autonomy.Off})
# x:730 y:110
OperatableStateMachine.add('look center_5',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'wait center_5'},
autonomy={'done': Autonomy.Off})
# x:741 y:193
OperatableStateMachine.add('wait center_5',
WaitState(wait_time=3),
transitions={'done': 'look right_5'},
autonomy={'done': Autonomy.Off})
# x:733 y:296
OperatableStateMachine.add('look right_5',
SaraSetHeadAngle(pitch=0.1, yaw=-1.5),
transitions={'done': 'wait right_5'},
autonomy={'done': Autonomy.Off})
# x:731 y:422
OperatableStateMachine.add('wait right_5',
WaitState(wait_time=3),
transitions={'done': 'done'},
autonomy={'done': Autonomy.Off})
# x:449 y:45, x:454 y:135, x:447 y:195, x:446 y:252, x:430 y:458, x:530 y:458
_sm_ask_while_looking_at_person_4 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['personID', 'question'],
output_keys=['answer'],
conditions=[('finished', [('ask', 'finished')]),
('failed', [('ask', 'failed')]),
('finished', [('keep looking at person', 'finished')]),
('failed', [('keep looking at person', 'failed')])])
with _sm_ask_while_looking_at_person_4:
# x:97 y:55
OperatableStateMachine.add('ask',
_sm_ask_1,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'question': 'question',
'answer': 'answer'
})
# x:65 y:197
OperatableStateMachine.add('keep looking at person',
_sm_keep_looking_at_person_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'personID': 'personID'})
with _state_machine:
# x:30 y:115
OperatableStateMachine.add(
'Init_Sequence',
self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM,
'Init_Sequence'),
transitions={
'finished': 'create fifo',
'failed': 'create fifo'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:164 y:62
OperatableStateMachine.add('for loop',
ForLoopWithInput(repeat=4),
transitions={
'do': 'tourne tete et base',
'end': 'not_found'
},
autonomy={
'do': Autonomy.Off,
'end': Autonomy.Off
},
remapping={
'index_in': 'index',
'index_out': 'index'
})
# x:260 y:557
OperatableStateMachine.add('ask while looking at person',
_sm_ask_while_looking_at_person_4,
transitions={
'finished': 'answer contains yes',
'failed': 'add id'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'personID': 'personID',
'question': 'question',
'answer': 'answer'
})
# x:289 y:480
OperatableStateMachine.add(
'get personID',
CalculationState(calculation=lambda x: x.ID),
transitions={'done': 'ask while looking at person'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'personEntity',
'output_value': 'personID'
})
# x:270 y:643
OperatableStateMachine.add(
'answer contains yes',
RegexTester(
regex=".*((yes)|(Yes)|(yep)|(sure)|(of course)).*"),
transitions={
'true': 'say ty',
'false': 'add id'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'text': 'answer',
'result': 'result'
})
# x:173 y:260
OperatableStateMachine.add('look center',
SaraSetHeadAngle(pitch=0.1, yaw=0),
transitions={'done': 'for loop'},
autonomy={'done': Autonomy.Off})
# x:372 y:89
OperatableStateMachine.add('tourne tete et base',
_sm_tourne_tete_et_base_3,
transitions={
'done': 'get list of person',
'failed': 'not_found'
},
autonomy={
'done': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'index': 'index',
'rotation180degres':
'rotation180degres'
})
# x:263 y:322
OperatableStateMachine.add(
'fitler the entity list to remove id already checked',
FilterKey(filter_function=lambda x: x[0].ID not in x[1],
input_keys=["input_list", "FIFO"]),
transitions={
'not_empty': 'get first entity',
'empty': 'look center'
},
autonomy={
'not_empty': Autonomy.Off,
'empty': Autonomy.Off
},
remapping={
'input_list': 'entityList',
'FIFO': 'FIFO',
'output_list': 'filteredEntityList'
})
# x:268 y:388
OperatableStateMachine.add(
'get first entity',
CalculationState(calculation=lambda x: x[0]),
transitions={'done': 'get real id'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'filteredEntityList',
'output_value': 'personEntity'
})
# x:296 y:168
OperatableStateMachine.add(
'get list of person',
list_entities_by_name(frontality_level=0.5, distance_max=3),
transitions={
'found':
'fitler the entity list to remove id already checked',
'none_found': 'for loop'
},
autonomy={
'found': Autonomy.Off,
'none_found': Autonomy.Off
},
remapping={
'name': 'personKey',
'entity_list': 'entityList',
'number': 'numberOfEntity'
})
# x:111 y:533
OperatableStateMachine.add(
'add id',
FIFO_Add(),
transitions={'done': 'say keep looking'},
autonomy={'done': Autonomy.Off},
remapping={
'Entry': 'personID',
'FIFO': 'FIFO'
})
# x:609 y:651
OperatableStateMachine.add('say ty',
SaraSay(sentence="Thank you.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'found'},
autonomy={'done': Autonomy.Off})
# x:113 y:386
OperatableStateMachine.add('say keep looking',
SaraSay(sentence="Ok, never mind.",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'for loop'},
autonomy={'done': Autonomy.Off})
# x:472 y:438
OperatableStateMachine.add('get real id',
_sm_get_real_id_2,
transitions={
'finished':
'ask while looking at person',
'failed': 'get personID'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'entity': 'personEntity',
'personKey': 'personKey',
'FIFO': 'FIFO',
'personID': 'personID'
})
# x:35 y:53
OperatableStateMachine.add('create fifo',
FIFO_New(),
transitions={'done': 'for loop'},
autonomy={'done': Autonomy.Off},
remapping={'FIFO': 'FIFO'})
return _state_machine
def create(self):
leap_topic = '/hmi/leap_motion/data'
eyes_cmd_topic = 'control'
voice_topic = 'control'
leap_pose_topic = '/hmi/leap_motion/pose'
# x:286 y:634, x:989 y:651
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.be_evil = False
_state_machine.userdata.pose = PoseStamped()
_state_machine.userdata.unused = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:353, x:343 y:356, x:209 y:349, x:843 y:453, x:473 y:355, x:638 y:361, x:907 y:552
_sm_randheadmovements_0 = ConcurrencyContainer(outcomes=['object_detected', 'failed'], input_keys=['unused'], conditions=[
('object_detected', [('WaitUntlObject', 'still_object')]),
('object_detected', [('WaitUntlObject', 'moving_object')]),
('failed', [('WaitUntlObject', 'no_object')]),
('object_detected', [('RandHeadMoveme', 'done')]),
('failed', [('RandHeadMoveme', 'failed')])
])
with _sm_randheadmovements_0:
# x:158 y:110
OperatableStateMachine.add('WaitUntlObject',
LeapMotionMonitor(leap_motion_topic=leap_topic, exit_states=['still_object', 'moving_object'], pose_topic=leap_pose_topic, parameters=[2.0,0.02,0.2]),
transitions={'no_object': 'failed', 'still_object': 'object_detected', 'moving_object': 'object_detected'},
autonomy={'no_object': Autonomy.Off, 'still_object': Autonomy.Off, 'moving_object': Autonomy.Off},
remapping={'pose': 'pose'})
# x:605 y:103
OperatableStateMachine.add('RandHeadMoveme',
SweetieBotRandHeadMovements(controller='joint_state_head', duration=120, interval=[1,4], max2356=[0.3,0.5,1,1], min2356=[-0.3,-0.1,-1,-1]),
transitions={'done': 'object_detected', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'config': 'unused'})
# x:30 y:353, x:357 y:355, x:671 y:353, x:140 y:353, x:237 y:356, x:530 y:353, x:742 y:441, x:730 y:353, x:452 y:353, x:732 y:406
_sm_waitnoobjecttimeout_1 = ConcurrencyContainer(outcomes=['no_object', 'timeout', 'failure', 'too_close'], conditions=[
('no_object', [('WaitNoObject', 'no_object')]),
('failure', [('WaitNoObject', 'moving_object')]),
('failure', [('WaitNoObject', 'still_object')]),
('failure', [('FollowObject', 'failed')]),
('timeout', [('Wait', 'done')]),
('too_close', [('FollowObject', 'too_close')])
])
with _sm_waitnoobjecttimeout_1:
# x:162 y:101
OperatableStateMachine.add('WaitNoObject',
LeapMotionMonitor(leap_motion_topic=leap_topic, exit_states=['no_object'], pose_topic=leap_pose_topic, parameters=[4.0,0.02,0.2]),
transitions={'no_object': 'no_object', 'still_object': 'failure', 'moving_object': 'failure'},
autonomy={'no_object': Autonomy.Off, 'still_object': Autonomy.Off, 'moving_object': Autonomy.Off},
remapping={'pose': 'pose'})
# x:463 y:95
OperatableStateMachine.add('FollowObject',
SweetieBotFollowHeadPoseSmart(pose_topic=leap_pose_topic, follow_joint_state_controller='joint_state_head', discomfort_time=4, neck_control_parameteres=[-0.13,0.3,0.20,0.2], deactivate=True, controlled_chains=['eyes','head']),
transitions={'failed': 'failure', 'too_close': 'too_close'},
autonomy={'failed': Autonomy.Off, 'too_close': Autonomy.Off})
# x:332 y:94
OperatableStateMachine.add('Wait',
WaitState(wait_time=self.play_timeout),
transitions={'done': 'timeout'},
autonomy={'done': Autonomy.Off})
# x:30 y:353, x:130 y:353, x:330 y:366, x:737 y:355, x:898 y:596, x:899 y:486, x:900 y:533, x:902 y:443, x:568 y:345
_sm_watchonleapmotionobject_2 = ConcurrencyContainer(outcomes=['no_object', 'still_object', 'failed', 'to_close'], output_keys=['pose'], conditions=[
('no_object', [('WaitTillObjectMOving', 'no_object')]),
('still_object', [('WaitTillObjectMOving', 'still_object')]),
('failed', [('WaitTillObjectMOving', 'moving_object')]),
('failed', [('HeadFollowsMovingObject', 'failed')]),
('to_close', [('HeadFollowsMovingObject', 'too_close')])
])
with _sm_watchonleapmotionobject_2:
# x:135 y:135
OperatableStateMachine.add('WaitTillObjectMOving',
LeapMotionMonitor(leap_motion_topic='/hmi/leap_motion/data', exit_states=['no_object', 'still_object'], pose_topic='/hmi/leap_motion/pose', parameters=[1.0,0.02,0.2]),
transitions={'no_object': 'no_object', 'still_object': 'still_object', 'moving_object': 'failed'},
autonomy={'no_object': Autonomy.High, 'still_object': Autonomy.High, 'moving_object': Autonomy.Off},
remapping={'pose': 'pose'})
# x:583 y:138
OperatableStateMachine.add('HeadFollowsMovingObject',
SweetieBotFollowHeadPoseSmart(pose_topic='/hmi/leap_motion/pose', follow_joint_state_controller='joint_state_head', discomfort_time=4.0, neck_control_parameteres=[-0.13,0.3,0.20,0.2], deactivate=True, controlled_chains=['head','eyes']),
transitions={'failed': 'failed', 'too_close': 'to_close'},
autonomy={'failed': Autonomy.Off, 'too_close': Autonomy.High})
# x:979 y:146, x:969 y:327
_sm_switchevil_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['be_evil'], output_keys=['be_evil'])
with _sm_switchevil_3:
# x:269 y:101
OperatableStateMachine.add('CheckState',
DecisionState(outcomes=[ 'evil', 'good' ], conditions=lambda x: 'evil' if x else 'good'),
transitions={'evil': 'NewState1', 'good': 'NewState2'},
autonomy={'evil': Autonomy.Off, 'good': Autonomy.Off},
remapping={'input_value': 'be_evil'})
# x:423 y:54
OperatableStateMachine.add('NewState1',
CalculationState(calculation=lambda x: not random.random() < self.evil_to_good_switch_probability),
transitions={'done': 'SwitchEvilMode'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'be_evil', 'output_value': 'new_be_evil'})
# x:425 y:159
OperatableStateMachine.add('NewState2',
CalculationState(calculation=lambda x: random.random() < self.good_to_evil_switch_probability),
transitions={'done': 'SwitchEvilMode'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'be_evil', 'output_value': 'new_be_evil'})
# x:678 y:91
OperatableStateMachine.add('SwitchEvilMode',
self.use_behavior(SwitchEvilModeSM, 'SwitchEvil/SwitchEvilMode'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil', 'new_be_evil': 'new_be_evil'})
with _state_machine:
# x:114 y:550
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(config_name='head_upright', move_group='head', action_topic='move_group', robot_name=''),
transitions={'reached': 'RandHeadMovements', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:500 y:88
OperatableStateMachine.add('Brohoof',
self.use_behavior(BrohoofSM, 'Brohoof'),
transitions={'finished': 'WaitNoObjectTimeout', 'unreachable': 'Greeting', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'pose'})
# x:699 y:28
OperatableStateMachine.add('Greeting',
self.use_behavior(GreetingSM, 'Greeting'),
transitions={'finished': 'WaitNoObjectTimeout', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:455 y:230
OperatableStateMachine.add('Play',
self.use_behavior(PlaySM, 'Play'),
transitions={'finished': 'WaitNoObjectTimeout', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:78 y:247
OperatableStateMachine.add('SwitchEvil',
_sm_switchevil_3,
transitions={'finished': 'WatchOnLeapMotionObject', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:387 y:6
OperatableStateMachine.add('CheckEvil',
DecisionState(outcomes=['evil','good'], conditions=lambda x: 'evil' if x else 'good'),
transitions={'evil': 'Greeting', 'good': 'Brohoof'},
autonomy={'evil': Autonomy.Off, 'good': Autonomy.Off},
remapping={'input_value': 'be_evil'})
# x:144 y:77
OperatableStateMachine.add('WatchOnLeapMotionObject',
_sm_watchonleapmotionobject_2,
transitions={'no_object': 'Play', 'still_object': 'CheckEvil', 'failed': 'failed', 'to_close': 'Bad'},
autonomy={'no_object': Autonomy.Inherit, 'still_object': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'to_close': Autonomy.Inherit},
remapping={'pose': 'pose'})
# x:283 y:293
OperatableStateMachine.add('Bad',
self.use_behavior(BadSM, 'Bad'),
transitions={'finished': 'SwitchEvil', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:646 y:375
OperatableStateMachine.add('WaitNoObjectTimeout',
_sm_waitnoobjecttimeout_1,
transitions={'no_object': 'PlaceHead', 'timeout': 'Play', 'failure': 'failed', 'too_close': 'Bad'},
autonomy={'no_object': Autonomy.Inherit, 'timeout': Autonomy.Inherit, 'failure': Autonomy.Inherit, 'too_close': Autonomy.Inherit})
# x:106 y:411
OperatableStateMachine.add('RandHeadMovements',
_sm_randheadmovements_0,
transitions={'object_detected': 'SwitchEvil', 'failed': 'failed'},
autonomy={'object_detected': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'unused': 'unused'})
return _state_machine
def create(self):
storage = 'joint_trajectory/'
joint_state_control_topic = 'motion/controller/joint_state/out_joints_src_reset'
eyes_topic = 'control'
voice_topic = 'control'
joint_trajectory_action = 'motion/controller/joint_trajectory'
# x:322 y:652, x:984 y:622
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.counter = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:220 y:573, x:535 y:559
_sm_seizure_0 = OperatableStateMachine(outcomes=['finished', 'failed'],
input_keys=['counter'])
with _sm_seizure_0:
# x:125 y:111
OperatableStateMachine.add('SayDoNotTouch',
TextCommandState(type='voice/play_wav',
command='05donottouch',
topic=voice_topic),
transitions={'done': 'Seizure'},
autonomy={'done': Autonomy.Off})
# x:389 y:247
OperatableStateMachine.add(
'AddCounter',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Seizure'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'counter',
'output_value': 'counter'
})
# x:412 y:87
OperatableStateMachine.add(
'Select',
DecisionState(
outcomes=['say1', 'say2', 'say3', 'end'],
conditions=lambda x: ['say1', 'say2', 'say3', 'end'][x]),
transitions={
'say1': 'SayWalk',
'say2': 'SayBlaster',
'say3': 'SayWin',
'end': 'Farewell'
},
autonomy={
'say1': Autonomy.Off,
'say2': Autonomy.Off,
'say3': Autonomy.Off,
'end': Autonomy.High
},
remapping={'input_value': 'counter'})
# x:612 y:81
OperatableStateMachine.add('SayWalk',
TextCommandState(type='voice/play_wav',
command='17walk',
topic=voice_topic),
transitions={'done': 'AddCounter'},
autonomy={'done': Autonomy.Off})
# x:681 y:173
OperatableStateMachine.add('SayBlaster',
TextCommandState(type='voice/play_wav',
command='18blaster',
topic=voice_topic),
transitions={'done': 'AddCounter'},
autonomy={'done': Autonomy.Off})
# x:686 y:240
OperatableStateMachine.add('SayWin',
TextCommandState(type='voice/play_wav',
command='08win',
topic=voice_topic),
transitions={'done': 'AddCounter'},
autonomy={'done': Autonomy.Off})
# x:85 y:238
OperatableStateMachine.add(
'Seizure',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='seizure',
trajectory_ns=storage),
transitions={
'success': 'Select',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:132 y:419
OperatableStateMachine.add(
'Farewell',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='farewell',
trajectory_ns=storage),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
with _state_machine:
# x:15 y:642
OperatableStateMachine.add('RandomMovements',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=100000,
interval=[2, 3],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={'done': 'StartPose'},
autonomy={'done': Autonomy.High})
# x:124 y:352
OperatableStateMachine.add('WaitSayIamAlive',
WaitState(wait_time=1),
transitions={'done': 'WarmUp'},
autonomy={'done': Autonomy.Off})
# x:102 y:154
OperatableStateMachine.add(
'LookAtHoof',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='look_on_hoof',
trajectory_ns=storage),
transitions={
'success': 'SayIamAlive',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:96 y:484
OperatableStateMachine.add(
'WarmUp',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='little_shake_fast',
trajectory_ns=storage),
transitions={
'success': 'WaitBeforeDomination',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:528 y:547
OperatableStateMachine.add('SayMenace',
TextCommandState(
type='voice/play_wav',
command='02technic',
topic='/sweetie_bot/voice/voice'),
transitions={'done': 'WaitSayMenace'},
autonomy={'done': Autonomy.Off})
# x:529 y:454
OperatableStateMachine.add('WaitSayMenace',
WaitState(wait_time=2.3),
transitions={'done': 'EvilLook'},
autonomy={'done': Autonomy.Off})
# x:396 y:219
OperatableStateMachine.add(
'Menace',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='menace',
trajectory_ns=storage),
transitions={
'success': 'TryingToAcquireControl',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:431 y:107
OperatableStateMachine.add('TryingToAcquireControl',
WaitState(wait_time=10),
transitions={'done': 'SadLook'},
autonomy={'done': Autonomy.Off})
# x:429 y:301
OperatableStateMachine.add('SayBattleSystemActivation',
TextCommandState(
type='voice/play_wav',
command='03domination',
topic='/sweetie_bot/voice/voice'),
transitions={'done': 'Menace'},
autonomy={'done': Autonomy.Off})
# x:840 y:91
OperatableStateMachine.add('SayDominationFailed',
TextCommandState(
type='voice/play_wav',
command='04notfound',
topic='/sweetie_bot/voice/voice'),
transitions={'done': 'WaitFailed'},
autonomy={'done': Autonomy.Off})
# x:818 y:281
OperatableStateMachine.add(
'MenceCanceled',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='menace_canceled',
trajectory_ns=storage),
transitions={
'success': 'WaitBeforeHoofStamp',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:885 y:188
OperatableStateMachine.add('WaitFailed',
WaitState(wait_time=1),
transitions={'done': 'MenceCanceled'},
autonomy={'done': Autonomy.Off})
# x:139 y:573
OperatableStateMachine.add('WaitBeforeDomination',
WaitState(wait_time=2),
transitions={'done': 'RedEyes'},
autonomy={'done': Autonomy.Off})
# x:856 y:396
OperatableStateMachine.add('WaitBeforeHoofStamp',
WaitState(wait_time=1),
transitions={'done': 'HoofStamp'},
autonomy={'done': Autonomy.Off})
# x:826 y:490
OperatableStateMachine.add(
'HoofStamp',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='hoof_stamp',
trajectory_ns=storage),
transitions={
'success': 'NormalLook',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:385 y:567
OperatableStateMachine.add('RedEyes',
TextCommandState(type='eyes/emotion',
command='red_eyes',
topic=eyes_topic),
transitions={'done': 'SayMenace'},
autonomy={'done': Autonomy.Off})
# x:151 y:77
OperatableStateMachine.add(
'LookAround',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='look_around',
trajectory_ns=storage),
transitions={
'success': 'LookAtHoof',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:375 y:18
OperatableStateMachine.add('Wait',
WaitState(wait_time=1),
transitions={'done': 'LookAround'},
autonomy={'done': Autonomy.Off})
# x:195 y:10
OperatableStateMachine.add('NormalEyes',
TextCommandState(type='eyes/emotion',
command='normal',
topic=eyes_topic),
transitions={'done': 'Wait'},
autonomy={'done': Autonomy.Off})
# x:519 y:374
OperatableStateMachine.add(
'EvilLook',
TextCommandState(type='eyes/emotion',
command='evil_look',
topic=eyes_topic),
transitions={'done': 'SayBattleSystemActivation'},
autonomy={'done': Autonomy.Off})
# x:696 y:85
OperatableStateMachine.add(
'SadLook',
TextCommandState(type='eyes/emotion',
command='sad_look',
topic=eyes_topic),
transitions={'done': 'SayDominationFailed'},
autonomy={'done': Autonomy.Off})
# x:817 y:569
OperatableStateMachine.add('NormalLook',
TextCommandState(type='eyes/emotion',
command='normal_look',
topic=eyes_topic),
transitions={'done': 'Seizure'},
autonomy={'done': Autonomy.High})
# x:1124 y:445
OperatableStateMachine.add('Seizure',
_sm_seizure_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'counter': 'counter'})
# x:40 y:35
OperatableStateMachine.add(
'StartPose',
PublisherState(topic=joint_state_control_topic,
msg_type=JointState,
value={
'name': [
'joint51', 'joint52', 'joint53',
'eyes_pitch', 'eyes_yaw'
],
'position': [0.2, 0.0, 0, 0.0, 0.0]
}),
transitions={
'done': 'NormalEyes',
'failed': 'finished'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:127 y:254
OperatableStateMachine.add('SayIamAlive',
TextCommandState(
type='voice/play_wav',
command='01alive',
topic='/sweetie_bot/voice/voice'),
transitions={'done': 'WaitSayIamAlive'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:583 y:683, x:578 y:422
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.unused = None
_state_machine.userdata.rand_head_config = {'interval': [0.5, 1]}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:278 y:24
OperatableStateMachine.add('SesectBehavior',
OperatorDecisionState(
outcomes=['dance', 'rand_moves'],
hint='Select behavior',
suggestion='dance'),
transitions={
'dance': 'TestMovement',
'rand_moves': 'RandHead'
},
autonomy={
'dance': Autonomy.Full,
'rand_moves': Autonomy.Full
})
# x:114 y:352
OperatableStateMachine.add(
'TurnOffJointStateController',
SetBoolState(
service='motion/controller/joint_state/set_operational',
value=False),
transitions={
'true': 'SingASong',
'false': 'failed',
'failure': 'failed'
},
autonomy={
'true': Autonomy.High,
'false': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={
'success': 'success',
'message': 'message'
})
# x:136 y:518
OperatableStateMachine.add('SingASong',
TextCommandState(type='voice/play_wav',
command='mmm_song',
topic='control'),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Full})
# x:248 y:213
OperatableStateMachine.add(
'TestMovement',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='dance14',
trajectory_param='/saved_msgs/joint_trajectory'),
transitions={
'success': 'TurnOffJointStateController',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Low,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:939 y:351
OperatableStateMachine.add(
'RandSelector',
DecisionState(
outcomes=['first', 'second'],
conditions=(lambda x: 'first'
if random.uniform(0, 1) < 0.5 else 'second')),
transitions={
'first': 'PlaceHead',
'second': 'TestMovement'
},
autonomy={
'first': Autonomy.Full,
'second': Autonomy.Full
},
remapping={'input_value': 'unused'})
# x:704 y:603
OperatableStateMachine.add(
'HeadShake',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='head_shake',
trajectory_param='/saved_msgs/joint_trajectory'),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:958 y:510
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(
config_name='head_basic',
move_group='head',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'HeadShake',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:725 y:125
OperatableStateMachine.add(
'RandHead',
SweetieBotRandHeadMovements(controller='joint_state_head',
duration=120,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'RandSelector',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'rand_head_config'})
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):
voice_topic = 'control'
torque_off_service = 'motion/controller/torque_off/set_operational'
joint_state_control_topic = 'motion/controller/joint_state/out_joints_src_reset'
joy_topic = '/hmi/joystick'
eyes_topic = 'control'
# x:68 y:535, x:786 y:525
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.be_evil = self.be_evil
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:554 y:394, x:130 y:397
_sm_waitforjoyevent_0 = OperatableStateMachine(
outcomes=['received', 'unavailable'])
with _sm_waitforjoyevent_0:
# x:288 y:158
OperatableStateMachine.add('JoyEvent',
SubscriberState(topic=joy_topic,
blocking=True,
clear=False),
transitions={
'received': 'CheckEvent',
'unavailable': 'unavailable'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:485 y:159
OperatableStateMachine.add(
'CheckEvent',
DecisionState(outcomes=['noactivity', 'activity'],
conditions=lambda msg: 'activity'
if any(msg.buttons) or any(msg.axes[0:5]) else
'noactivity'),
transitions={
'noactivity': 'JoyEvent',
'activity': 'received'
},
autonomy={
'noactivity': Autonomy.Off,
'activity': Autonomy.Off
},
remapping={'input_value': 'message'})
# x:538 y:386, x:347 y:384, x:134 y:383, x:831 y:385, x:906 y:386
_sm_idlebehavior_1 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('finished', [('RandomHeadMovements', 'done')]),
('failed', [('WaitForJoyEvent', 'unavailable')]),
('finished', [('WaitForJoyEvent', 'received')])])
with _sm_idlebehavior_1:
# x:474 y:110
OperatableStateMachine.add('RandomHeadMovements',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=100,
interval=[1, 4],
max2356=[0.3, 0.3, 1, 1],
min2356=[-0.3, -0.3, -1, -1]),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:144 y:133
OperatableStateMachine.add('WaitForJoyEvent',
_sm_waitforjoyevent_0,
transitions={
'received': 'finished',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Inherit,
'unavailable': Autonomy.Inherit
})
with _state_machine:
# x:79 y:99
OperatableStateMachine.add(
'CheckEvil',
DecisionState(outcomes=['good', 'evil'],
conditions=lambda x: 'evil' if x else 'good'),
transitions={
'good': 'NormalEyes',
'evil': 'RedEyes'
},
autonomy={
'good': Autonomy.Off,
'evil': Autonomy.Off
},
remapping={'input_value': 'be_evil'})
# x:550 y:467
OperatableStateMachine.add('AskForAssistance',
TextCommandState(type='voice/play_wav',
command='assistance',
topic=voice_topic),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:440 y:126
OperatableStateMachine.add('WaitForMovementFinish',
WaitState(wait_time=0.0),
transitions={'done': 'DoTricks'},
autonomy={'done': Autonomy.Off})
# x:251 y:503
OperatableStateMachine.add(
'StartingStance',
PublisherState(topic=joint_state_control_topic,
msg_type=JointState,
value={
'name': [
'joint51', 'joint52', 'joint53',
'eyes_pitch', 'eyes_yaw'
],
'position': [0.2, 0.0, 0.2, 0.0, 0.0]
}),
transitions={
'done': 'CheckEvil2',
'failed': 'AskForAssistance'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:731 y:153
OperatableStateMachine.add('DoTricks',
self.use_behavior(
DoTricksSM, 'DoTricks'),
transitions={
'finished': 'NormalLook',
'failed': 'AskForAssistance'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:27 y:219
OperatableStateMachine.add('RedEyes',
TextCommandState(type='eyes/emotion',
command='red_eyes',
topic=eyes_topic),
transitions={'done': 'IdleBehavior'},
autonomy={'done': Autonomy.Off})
# x:222 y:65
OperatableStateMachine.add('NormalEyes',
TextCommandState(type='eyes/emotion',
command='normal',
topic=eyes_topic),
transitions={'done': 'IdleBehavior'},
autonomy={'done': Autonomy.Off})
# x:481 y:305
OperatableStateMachine.add('IdleBehavior',
_sm_idlebehavior_1,
transitions={
'finished': 'StartingStance',
'failed': 'AskForAssistance'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:181 y:360
OperatableStateMachine.add(
'CheckEvil2',
DecisionState(outcomes=['evil', 'good'],
conditions=lambda x: 'evil' if x else 'good'),
transitions={
'evil': 'EvilLook',
'good': 'WaitForMovementFinish'
},
autonomy={
'evil': Autonomy.Off,
'good': Autonomy.Off
},
remapping={'input_value': 'be_evil'})
# x:188 y:294
OperatableStateMachine.add(
'EvilLook',
TextCommandState(type='eyes/emotion',
command='evil_look',
topic=eyes_topic),
transitions={'done': 'WaitForMovementFinish'},
autonomy={'done': Autonomy.Off})
# x:880 y:311
OperatableStateMachine.add('NormalLook',
TextCommandState(type='eyes/emotion',
command='normal_look',
topic=eyes_topic),
transitions={'done': 'IdleBehavior'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
bag_folder_in = "~/sysid_tests"
bag_folder_out = "" # optional (if not provided, an /out folder will be created in the bag_folder_in directory
prefix = "" # optional (applies to bag and video files)
input_bagfiles = [] # calculated (leave empty)
output_bagfiles = [] # calculated (leave empty)
desired_num_tests = 1 # num of tests per trajectory
wait_time = 3.0 # before execution (for rosbag record)
settling_time = 2.0 # after execution
mode_for_tests = "dance" # "manipulate", "system_id", etc.
desired_controllers = ["left_arm_traj_controller","right_arm_traj_controller", "left_leg_traj_controller","right_leg_traj_controller","torso_traj_controller"]
# x:880 y:104, x:660 y:14
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.traj_index = 0
_state_machine.userdata.tests_counter = 0
_state_machine.userdata.none = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# Get folder containing input bagfiles and create folder for output
bag_folder_in = os.path.expanduser(bag_folder_in)
if not os.path.exists(bag_folder_in):
Logger.logwarn('Path to input bag folder does not exist (%s)' % bag_folder_in)
if not bag_folder_out:
bag_folder_out = os.path.join(bag_folder_in, 'out')
if not os.path.exists(bag_folder_out):
os.makedirs(bag_folder_out)
else:
# The user has provided a directory for output bagfiles
bag_folder_out = os.path.expanduser(bag_folder_out)
if not os.path.exists(bag_folder_out):
os.makedirs(bag_folder_out)
# Initialize lists again, in case the user did alter them
input_bagfiles = []
output_bagfiles = []
# Get all input bagfile names from bag folder and also name the corresponding output bagfiles
os.chdir(bag_folder_in)
for bagfile in sorted(glob.glob("*.bag")):
input_bagfiles.append(os.path.join(bag_folder_in, bagfile))
bare_name = bagfile.split(".")[0]
output_name = bare_name + "_" + time.strftime("%Y-%m-%d-%H_%M") + "_" # file name will be completed by flexible calculation state
output_bagfiles.append(output_name)
Logger.loginfo('Found %d input bag files in %s' % (len(input_bagfiles), bag_folder_in))
# Create STAND posture trajectories
_state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory()
# [/MANUAL_CREATE]
# x:836 y:51, x:858 y:296
_sm_starting_point_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['experiment_name', 'trajectories'])
with _sm_starting_point_0:
# x:49 y:42
OperatableStateMachine.add('Gen_Starting_Name',
CalculationState(calculation=lambda en: en + "_starting"),
transitions={'done': 'Gen_Starting_Bagfile_Name'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'experiment_name', 'output_value': 'starting_name'})
# x:42 y:231
OperatableStateMachine.add('Record_Starting_Point',
StartRecordLogsState(topics_to_record=self.topics_to_record),
transitions={'logging': 'Wait_for_Rosbag_Record'},
autonomy={'logging': Autonomy.Off},
remapping={'bagfile_name': 'output_bagfile_starting', 'rosbag_process': 'rosbag_process_starting'})
# x:38 y:330
OperatableStateMachine.add('Wait_for_Rosbag_Record',
WaitState(wait_time=wait_time),
transitions={'done': 'Extract_Left_Arm_Part'},
autonomy={'done': Autonomy.Off})
# x:29 y:133
OperatableStateMachine.add('Gen_Starting_Bagfile_Name',
CalculationState(calculation=lambda en: os.path.join(bag_folder_out, en) + ".bag"),
transitions={'done': 'Record_Starting_Point'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'starting_name', 'output_value': 'output_bagfile_starting'})
# x:536 y:47
OperatableStateMachine.add('Stop_Recording_Starting_Point',
StopRecordLogsState(),
transitions={'stopped': 'finished'},
autonomy={'stopped': Autonomy.Off},
remapping={'rosbag_process': 'rosbag_process_starting'})
# x:228 y:757
OperatableStateMachine.add('Plan_to_Starting_Point_Left_Arm',
MoveItMoveGroupPlanState(vel_scaling=0.1),
transitions={'done': 'Plan_to_Starting_Point_Right_Arm', 'failed': 'Report_Starting_Point_Failure'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High},
remapping={'desired_goal': 'trajectories_left_arm', 'plan_to_goal': 'plan_to_goal_left_arm'})
# x:236 y:655
OperatableStateMachine.add('Plan_to_Starting_Point_Right_Arm',
MoveItMoveGroupPlanState(vel_scaling=0.1),
transitions={'done': 'Plan_to_Starting_Point_Left_Leg', 'failed': 'Report_Starting_Point_Failure'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High},
remapping={'desired_goal': 'trajectories_right_arm', 'plan_to_goal': 'plan_to_goal_right_arm'})
# x:272 y:47
OperatableStateMachine.add('Go_to_Starting_Point',
ExecuteTrajectoryWholeBodyState(controllers=desired_controllers),
transitions={'done': 'Stop_Recording_Starting_Point', 'failed': 'Report_Starting_Point_Failure'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low},
remapping={'trajectories': 'trajectories_all'})
# x:536 y:169
OperatableStateMachine.add('Report_Starting_Point_Failure',
LogState(text="Failed to plan or go to starting point!", severity=Logger.REPORT_WARN),
transitions={'done': 'Stop_Recording_When_Failed'},
autonomy={'done': Autonomy.Full})
# x:34 y:424
OperatableStateMachine.add('Extract_Left_Arm_Part',
CalculationState(calculation=lambda t: {'left_arm': t['left_arm']} if 'left_arm' in t else None),
transitions={'done': 'Extract_Right_Arm_Part'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'trajectories', 'output_value': 'trajectories_left_arm'})
# x:21 y:507
OperatableStateMachine.add('Extract_Right_Arm_Part',
CalculationState(calculation=lambda t: {'right_arm': t['right_arm']} if 'right_arm' in t else None),
transitions={'done': 'Extract_Left_Leg_Part'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'trajectories', 'output_value': 'trajectories_right_arm'})
# x:293 y:146
OperatableStateMachine.add('Combine_Plans',
FlexibleCalculationState(calculation=self.combine_plans, input_keys=['left_arm', 'right_arm', 'left_leg', 'right_leg', 'torso']),
transitions={'done': 'Go_to_Starting_Point'},
autonomy={'done': Autonomy.Low},
remapping={'left_arm': 'plan_to_goal_left_arm', 'right_arm': 'plan_to_goal_right_arm', 'left_leg': 'plan_to_goal_left_leg', 'right_leg': 'plan_to_goal_right_leg', 'torso': 'plan_to_goal_torso', 'output_value': 'trajectories_all'})
# x:789 y:167
OperatableStateMachine.add('Stop_Recording_When_Failed',
StopRecordLogsState(),
transitions={'stopped': 'failed'},
autonomy={'stopped': Autonomy.Off},
remapping={'rosbag_process': 'rosbag_process_starting'})
# x:24 y:601
OperatableStateMachine.add('Extract_Left_Leg_Part',
CalculationState(calculation=lambda t: {'left_leg': t['left_leg']} if 'left_leg' in t else None),
transitions={'done': 'Extract_Right_Leg_Part'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'trajectories', 'output_value': 'trajectories_left_leg'})
# x:22 y:665
OperatableStateMachine.add('Extract_Right_Leg_Part',
CalculationState(calculation=lambda t: {'right_leg': t['right_leg']} if 'right_leg' in t else None),
transitions={'done': 'Extract_Torso_Part'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'trajectories', 'output_value': 'trajectories_right_leg'})
# x:33 y:765
OperatableStateMachine.add('Extract_Torso_Part',
CalculationState(calculation=lambda t: {'torso': t['torso']} if 'torso' in t else None),
transitions={'done': 'Plan_to_Starting_Point_Left_Arm'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'trajectories', 'output_value': 'trajectories_torso'})
# x:227 y:410
OperatableStateMachine.add('Plan_to_Starting_Point_Right_Leg',
MoveItMoveGroupPlanState(vel_scaling=0.1),
transitions={'done': 'Plan_to_Starting_Point_Torso', 'failed': 'Report_Starting_Point_Failure'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High},
remapping={'desired_goal': 'trajectories_right_leg', 'plan_to_goal': 'plan_to_goal_right_leg'})
# x:237 y:531
OperatableStateMachine.add('Plan_to_Starting_Point_Left_Leg',
MoveItMoveGroupPlanState(vel_scaling=0.1),
transitions={'done': 'Plan_to_Starting_Point_Right_Leg', 'failed': 'Report_Starting_Point_Failure'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High},
remapping={'desired_goal': 'trajectories_left_leg', 'plan_to_goal': 'plan_to_goal_left_leg'})
# x:241 y:296
OperatableStateMachine.add('Plan_to_Starting_Point_Torso',
MoveItMoveGroupPlanState(vel_scaling=0.1),
transitions={'done': 'Combine_Plans', 'failed': 'Report_Starting_Point_Failure'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High},
remapping={'desired_goal': 'trajectories_torso', 'plan_to_goal': 'plan_to_goal_torso'})
# x:1118 y:103, x:348 y:32
_sm_execute_individual_trajectory_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['traj_index', 'tests_counter'])
with _sm_execute_individual_trajectory_1:
# x:79 y:28
OperatableStateMachine.add('Get_Input_Bagfile',
CalculationState(calculation=lambda idx: input_bagfiles[idx]),
transitions={'done': 'Gen_Experiment_Name'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'traj_index', 'output_value': 'input_bagfile'})
# x:904 y:187
OperatableStateMachine.add('Stop_Recording',
StopRecordLogsState(),
transitions={'stopped': 'Stop_Video_Logging'},
autonomy={'stopped': Autonomy.Low},
remapping={'rosbag_process': 'rosbag_process'})
# x:494 y:292
OperatableStateMachine.add('Wait_For_Rosbag_Record',
WaitState(wait_time=wait_time),
transitions={'done': 'Execute_Trajs_from_Bagfile'},
autonomy={'done': Autonomy.Low})
# x:507 y:384
OperatableStateMachine.add('Record_SysID_Test',
StartRecordLogsState(topics_to_record=self.topics_to_record),
transitions={'logging': 'Wait_For_Rosbag_Record'},
autonomy={'logging': Autonomy.Off},
remapping={'bagfile_name': 'output_bagfile', 'rosbag_process': 'rosbag_process'})
# x:484 y:106
OperatableStateMachine.add('Stop_Recording_After_Failure',
StopRecordLogsState(),
transitions={'stopped': 'Stop_Video_Logging_After_Failure'},
autonomy={'stopped': Autonomy.Off},
remapping={'rosbag_process': 'rosbag_process'})
# x:727 y:188
OperatableStateMachine.add('Wait_for_Settling',
WaitState(wait_time=settling_time),
transitions={'done': 'Stop_Recording'},
autonomy={'done': Autonomy.Off})
# x:482 y:188
OperatableStateMachine.add('Execute_Trajs_from_Bagfile',
ExecuteTrajectoryWholeBodyState(controllers=desired_controllers),
transitions={'done': 'Wait_for_Settling', 'failed': 'Stop_Recording_After_Failure'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low},
remapping={'trajectories': 'trajectories'})
# x:48 y:283
OperatableStateMachine.add('Load_Trajs_from_Bagfile',
LoadTrajectoryFromBagfileState(),
transitions={'done': 'Start_Video_Logging', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low},
remapping={'bagfile_name': 'input_bagfile', 'trajectories': 'trajectories'})
# x:65 y:113
OperatableStateMachine.add('Gen_Experiment_Name',
FlexibleCalculationState(calculation=lambda i: prefix + output_bagfiles[i[0]] + str(i[1] + 1), input_keys=["idx", "counter"]),
transitions={'done': 'Gen_Output_Bagfile_Name'},
autonomy={'done': Autonomy.Off},
remapping={'idx': 'traj_index', 'counter': 'tests_counter', 'output_value': 'experiment_name'})
# x:56 y:192
OperatableStateMachine.add('Gen_Output_Bagfile_Name',
CalculationState(calculation=lambda en: os.path.join(bag_folder_out, en) + ".bag"),
transitions={'done': 'Load_Trajs_from_Bagfile'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'experiment_name', 'output_value': 'output_bagfile'})
# x:903 y:97
OperatableStateMachine.add('Stop_Video_Logging',
VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'})
# x:472 y:26
OperatableStateMachine.add('Stop_Video_Logging_After_Failure',
VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off},
remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'})
# x:285 y:378
OperatableStateMachine.add('Starting_Point',
_sm_starting_point_0,
transitions={'finished': 'Record_SysID_Test', 'failed': 'Stop_Video_Logging_After_Failure'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'experiment_name': 'experiment_name', 'trajectories': 'trajectories'})
# x:71 y:384
OperatableStateMachine.add('Start_Video_Logging',
VideoLoggingState(command=VideoLoggingState.START, no_video=False, no_bags=True),
transitions={'done': 'Starting_Point'},
autonomy={'done': Autonomy.Off},
remapping={'experiment_name': 'experiment_name', 'description': 'experiment_name'})
# x:475 y:405
_sm_execute_sysid_trajectories_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['traj_index', 'tests_counter'], output_keys=['traj_index'])
with _sm_execute_sysid_trajectories_2:
# x:367 y:24
OperatableStateMachine.add('Execute_Individual_Trajectory',
_sm_execute_individual_trajectory_1,
transitions={'finished': 'Increment_Tests_per_Traj_counter', 'failed': 'Wait_before_Fail'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'traj_index': 'traj_index', 'tests_counter': 'tests_counter'})
# x:714 y:27
OperatableStateMachine.add('Increment_Tests_per_Traj_counter',
CalculationState(calculation=lambda counter: counter + 1),
transitions={'done': 'More_Tests_for_this_Traj?'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'tests_counter', 'output_value': 'tests_counter'})
# x:738 y:207
OperatableStateMachine.add('More_Tests_for_this_Traj?',
DecisionState(outcomes=['yes', 'no'], conditions=lambda counter: 'no' if counter >= desired_num_tests else 'yes'),
transitions={'yes': 'Execute_Individual_Trajectory', 'no': 'Reset_Tests_counter'},
autonomy={'yes': Autonomy.Low, 'no': Autonomy.Low},
remapping={'input_value': 'tests_counter'})
# x:752 y:298
OperatableStateMachine.add('Reset_Tests_counter',
CalculationState(calculation=lambda counter: 0),
transitions={'done': 'Increment_Traj_Index'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'tests_counter', 'output_value': 'tests_counter'})
# x:752 y:399
OperatableStateMachine.add('Increment_Traj_Index',
CalculationState(calculation=lambda idx: idx + 1),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'traj_index', 'output_value': 'traj_index'})
# x:405 y:141
OperatableStateMachine.add('Wait_before_Fail',
WaitState(wait_time=2.0),
transitions={'done': 'Increment_Tests_per_Traj_counter'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:122 y:26
OperatableStateMachine.add('Starting_Execution',
LogState(text="Execution is starting. Confirm first transition!", severity=Logger.REPORT_HINT),
transitions={'done': 'Go_to_Desired_Mode'},
autonomy={'done': Autonomy.High})
# x:328 y:298
OperatableStateMachine.add('Execute SysID Trajectories',
_sm_execute_sysid_trajectories_2,
transitions={'finished': 'More_Trajectories?'},
autonomy={'finished': Autonomy.Inherit},
remapping={'traj_index': 'traj_index', 'tests_counter': 'tests_counter'})
# x:129 y:215
OperatableStateMachine.add('More_Trajectories?',
DecisionState(outcomes=['yes', 'no'], conditions=lambda idx: 'no' if idx >= len(input_bagfiles) else 'yes'),
transitions={'yes': 'Notify_Next_Trajectory', 'no': 'Move_to_Stand_Posture'},
autonomy={'yes': Autonomy.Low, 'no': Autonomy.High},
remapping={'input_value': 'traj_index'})
# x:592 y:103
OperatableStateMachine.add('Go_to_FREEZE_before_Exit',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.FREEZE),
transitions={'changed': 'finished', 'failed': 'failed'},
autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Low})
# x:584 y:214
OperatableStateMachine.add('Failed_To_Go_To_Stand_Posture',
LogState(text="Failed to go to stand posture", severity=Logger.REPORT_WARN),
transitions={'done': 'Go_to_FREEZE_before_Exit'},
autonomy={'done': Autonomy.Off})
# x:121 y:393
OperatableStateMachine.add('Notify_Next_Trajectory',
LogState(text="Continuing with next trajectory.", severity=Logger.REPORT_INFO),
transitions={'done': 'Execute SysID Trajectories'},
autonomy={'done': Autonomy.Off})
# x:112 y:102
OperatableStateMachine.add('Go_to_Desired_Mode',
ChangeControlModeActionState(target_mode=mode_for_tests),
transitions={'changed': 'More_Trajectories?', 'failed': 'Go_to_FREEZE_before_Exit'},
autonomy={'changed': Autonomy.High, 'failed': Autonomy.High})
# x:329 y:158
OperatableStateMachine.add('Move_to_Stand_Posture',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}),
transitions={'done': 'Go_to_FREEZE_before_Exit', 'failed': 'Failed_To_Go_To_Stand_Posture'},
autonomy={'done': Autonomy.High, 'failed': Autonomy.High},
remapping={'side': 'none'})
return _state_machine
def create(self):
arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM
no_valve_collision = True
turning_affordance = "open"
turn_amount = 200 # degree
turn_back_affordance = "close"
turn_test_amount = 25 # degree
# x:933 y:490, x:333 y:390
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.default_preference = 0
_state_machine.userdata.hand_side = self.hand_side
_state_machine.userdata.step_back_distance = 1 # meters
_state_machine.userdata.none = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
self._turn_amount = turn_amount
self._turn_test_amount = turn_test_amount
# [/MANUAL_CREATE]
# x:30 y:478, x:130 y:478, x:230 y:478
_sm_planning_pipeline_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['stand_pose'], output_keys=['plan_header'])
with _sm_planning_pipeline_0:
# x:34 y:57
OperatableStateMachine.add('Create_Step_Goal',
CreateStepGoalState(pose_is_pelvis=True),
transitions={'done': 'Plan_To_Waypoint', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'target_pose': 'stand_pose', 'step_goal': 'step_goal'})
# x:553 y:481
OperatableStateMachine.add('Modify_Plan',
InputState(request=InputState.FOOTSTEP_PLAN_HEADER, message='Modify plan, VALIDATE, and confirm.'),
transitions={'received': 'finished', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'plan_header'})
# x:34 y:484
OperatableStateMachine.add('Plan_To_Waypoint',
PlanFootstepsState(mode=self.parameter_set),
transitions={'planned': 'Modify_Plan', 'failed': 'Decide_Replan_without_Collision'},
autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'})
# x:139 y:314
OperatableStateMachine.add('Decide_Replan_without_Collision',
OperatorDecisionState(outcomes=['replan', 'fail'], hint='Try replanning without collision avoidance.', suggestion='replan'),
transitions={'replan': 'Replan_without_Collision', 'fail': 'failed'},
autonomy={'replan': Autonomy.Low, 'fail': Autonomy.Full})
# x:319 y:406
OperatableStateMachine.add('Replan_without_Collision',
PlanFootstepsState(mode='drc_step_no_collision'),
transitions={'planned': 'Modify_Plan', 'failed': 'failed'},
autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'})
# x:483 y:540, x:183 y:290
_sm_perform_turn_back_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'back_rotation', 'none'])
with _sm_perform_turn_back_1:
# x:71 y:78
OperatableStateMachine.add('Get_Turn_Back_Affordance',
GetTemplateAffordanceState(identifier=turn_back_affordance),
transitions={'done': 'Set_Back_Rotation', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'turning_affordance'})
# x:419 y:228
OperatableStateMachine.add('Plan_Turn_Back_Affordance',
PlanAffordanceState(vel_scaling=0.2, planner_id="RRTConnectkConfigDefault"),
transitions={'done': 'Execute_Turn_Back_Affordance', 'incomplete': 'Execute_Turn_Back_Affordance', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:411 y:378
OperatableStateMachine.add('Execute_Turn_Back_Affordance',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'finished', 'failed': 'finished'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:434 y:78
OperatableStateMachine.add('Set_Back_Rotation',
FlexibleCalculationState(calculation=self.set_back_rotation, input_keys=["turning_affordance", "back_rotation"]),
transitions={'done': 'Plan_Turn_Back_Affordance'},
autonomy={'done': Autonomy.Off},
remapping={'turning_affordance': 'turning_affordance', 'back_rotation': 'back_rotation', 'output_value': 'turning_affordance'})
# x:383 y:40, x:433 y:490
_sm_perform_turning_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none'], output_keys=['back_rotation'])
with _sm_perform_turning_2:
# x:92 y:28
OperatableStateMachine.add('Adjust_Hand_Pose',
LogState(text="Make sure hand is in valve", severity=Logger.REPORT_HINT),
transitions={'done': 'Init_Back_Rotation'},
autonomy={'done': Autonomy.Full})
# x:926 y:228
OperatableStateMachine.add('Plan_Turning_Affordance',
PlanAffordanceState(vel_scaling=0.3, planner_id="RRTConnectkConfigDefault"),
transitions={'done': 'Execute_Turning_Affordance', 'incomplete': 'Execute_Turning_Affordance', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:495 y:228
OperatableStateMachine.add('Set_Full_Rotation',
CalculationState(calculation=self.set_full_rotation_angle),
transitions={'done': 'Plan_Turning_Affordance'},
autonomy={'done': Autonomy.Low},
remapping={'input_value': 'turning_affordance', 'output_value': 'turning_affordance'})
# x:818 y:78
OperatableStateMachine.add('Execute_Turning_Affordance',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'Accumulate_Rotation', 'failed': 'Accumulate_Rotation'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.High},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:534 y:78
OperatableStateMachine.add('Accumulate_Rotation',
FlexibleCalculationState(calculation=lambda x: x[0] + self._turn_amount * x[1], input_keys=["back_rotation", "plan_fraction"]),
transitions={'done': 'Decide_Turn_Again'},
autonomy={'done': Autonomy.Off},
remapping={'back_rotation': 'back_rotation', 'plan_fraction': 'plan_fraction', 'output_value': 'back_rotation'})
# x:287 y:128
OperatableStateMachine.add('Decide_Turn_Again',
OperatorDecisionState(outcomes=["extract", "turn"], hint="Turn again or extract hand?", suggestion="turn"),
transitions={'extract': 'finished', 'turn': 'Decide_If_Test'},
autonomy={'extract': Autonomy.High, 'turn': Autonomy.Full})
# x:73 y:228
OperatableStateMachine.add('Get_Turning_Affordance',
GetTemplateAffordanceState(identifier=turning_affordance),
transitions={'done': 'Decide_If_Test', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'turning_affordance'})
# x:302 y:228
OperatableStateMachine.add('Decide_If_Test',
DecisionState(outcomes=["test", "full"], conditions=lambda x: "test" if x == 0 else "full"),
transitions={'test': 'Set_Test_Rotation', 'full': 'Set_Full_Rotation'},
autonomy={'test': Autonomy.Low, 'full': Autonomy.Low},
remapping={'input_value': 'back_rotation'})
# x:494 y:178
OperatableStateMachine.add('Set_Test_Rotation',
CalculationState(calculation=self.set_test_rotation_angle),
transitions={'done': 'Plan_Test_Turning_Affordance'},
autonomy={'done': Autonomy.Low},
remapping={'input_value': 'turning_affordance', 'output_value': 'turning_affordance'})
# x:92 y:128
OperatableStateMachine.add('Init_Back_Rotation',
CalculationState(calculation=lambda x: 0),
transitions={'done': 'Get_Turning_Affordance'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'none', 'output_value': 'back_rotation'})
# x:713 y:178
OperatableStateMachine.add('Plan_Test_Turning_Affordance',
PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'done': 'Execute_Turning_Affordance', 'incomplete': 'Execute_Turning_Affordance', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:733 y:190, x:433 y:140
_sm_extract_hand_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'preference', 'none'])
with _sm_extract_hand_3:
# x:77 y:55
OperatableStateMachine.add('Get_Pregrasp',
GetTemplatePregraspState(),
transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'preference', 'pre_grasp': 'pre_grasp'})
# x:296 y:328
OperatableStateMachine.add('Plan_To_Pregrasp',
PlanEndeffectorCartesianWaypointsState(ignore_collisions=no_valve_collision, include_torso=False, keep_endeffector_orientation=True, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'planned': 'Move_To_Pregrasp', 'incomplete': 'Move_To_Pregrasp', 'failed': 'failed'},
autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'waypoints': 'waypoints', 'hand': 'hand_side', 'frame_id': 'frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:96 y:328
OperatableStateMachine.add('Create_Pose_List',
CalculationState(calculation=lambda x: [x.pose]),
transitions={'done': 'Plan_To_Pregrasp'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'pre_grasp', 'output_value': 'waypoints'})
# x:676 y:328
OperatableStateMachine.add('Move_To_Pregrasp',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'finished', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:95 y:228
OperatableStateMachine.add('Extract_Frame_Id',
CalculationState(calculation=lambda x: x.header.frame_id),
transitions={'done': 'Create_Pose_List'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'pre_grasp', 'output_value': 'frame_id'})
# x:733 y:190, x:433 y:190
_sm_insert_hand_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'preference', 'none'])
with _sm_insert_hand_4:
# x:83 y:78
OperatableStateMachine.add('Get_Grasp',
GetTemplateGraspState(),
transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'preference', 'grasp': 'grasp'})
# x:296 y:328
OperatableStateMachine.add('Plan_To_Grasp',
PlanEndeffectorCartesianWaypointsState(ignore_collisions=no_valve_collision, include_torso=False, keep_endeffector_orientation=True, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'planned': 'Move_To_Grasp', 'incomplete': 'Move_To_Grasp', 'failed': 'failed'},
autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'waypoints': 'waypoints', 'hand': 'hand_side', 'frame_id': 'frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:96 y:328
OperatableStateMachine.add('Create_Pose_List',
CalculationState(calculation=lambda x: [x.pose]),
transitions={'done': 'Plan_To_Grasp'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp', 'output_value': 'waypoints'})
# x:676 y:328
OperatableStateMachine.add('Move_To_Grasp',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'finished', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:96 y:228
OperatableStateMachine.add('Extract_Frame_Id',
CalculationState(calculation=lambda x: x.header.frame_id),
transitions={'done': 'Create_Pose_List'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp', 'output_value': 'frame_id'})
# x:933 y:290, x:133 y:340
_sm_prepare_joint_limit_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none'])
with _sm_prepare_joint_limit_5:
# x:71 y:78
OperatableStateMachine.add('Get_Turn_Back_Affordance',
GetTemplateAffordanceState(identifier=turn_back_affordance),
transitions={'done': 'Set_Back_Rotation', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'turning_affordance'})
# x:419 y:228
OperatableStateMachine.add('Plan_Turn_Back_Affordance',
PlanAffordanceState(vel_scaling=0.2, planner_id="RRTConnectkConfigDefault"),
transitions={'done': 'Execute_Turn_Back_Affordance', 'incomplete': 'Execute_Turn_Back_Affordance', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:411 y:378
OperatableStateMachine.add('Execute_Turn_Back_Affordance',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'Decide_Turn_Further', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:442 y:78
OperatableStateMachine.add('Set_Back_Rotation',
CalculationState(calculation=self.set_full_rotation_angle),
transitions={'done': 'Plan_Turn_Back_Affordance'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'turning_affordance', 'output_value': 'turning_affordance'})
# x:687 y:278
OperatableStateMachine.add('Decide_Turn_Further',
DecisionState(outcomes=["turn_again", "done"], conditions=lambda x: "turn_again" if x > 0.95 else "done"),
transitions={'turn_again': 'Plan_Turn_Back_Affordance', 'done': 'finished'},
autonomy={'turn_again': Autonomy.High, 'done': Autonomy.Low},
remapping={'input_value': 'plan_fraction'})
# x:30 y:478, x:130 y:478, x:230 y:478
_sm_walk_to_template_6 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['template_id', 'grasp_preference', 'hand_side'])
with _sm_walk_to_template_6:
# x:265 y:28
OperatableStateMachine.add('Decide_Request_Template',
DecisionState(outcomes=['request', 'continue'], conditions=lambda x: 'continue' if x is not None else 'request'),
transitions={'request': 'Request_Template', 'continue': 'Get_Stand_Pose'},
autonomy={'request': Autonomy.Low, 'continue': Autonomy.Off},
remapping={'input_value': 'template_id'})
# x:1033 y:106
OperatableStateMachine.add('Increment_Stand_Pose',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Inform_About_Retry'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'})
# x:1162 y:29
OperatableStateMachine.add('Inform_About_Retry',
LogState(text="Stand pose choice failed. Trying again.", severity=Logger.REPORT_INFO),
transitions={'done': 'Get_Stand_Pose'},
autonomy={'done': Autonomy.Off})
# x:567 y:118
OperatableStateMachine.add('Inform_About_Fail',
LogState(text="Unable to find a suitable stand pose for the template.", severity=Logger.REPORT_WARN),
transitions={'done': 'Decide_Repeat_Request'},
autonomy={'done': Autonomy.Off})
# x:554 y:274
OperatableStateMachine.add('Get_Goal_from_Operator',
InputState(request=InputState.WAYPOINT_GOAL_POSE, message="Provide a waypoint in front of the template."),
transitions={'received': 'Walk_To_Waypoint', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'plan_header'})
# x:279 y:110
OperatableStateMachine.add('Request_Template',
InputState(request=InputState.SELECTED_OBJECT_ID, message="Specify target template"),
transitions={'received': 'Get_Stand_Pose', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Off, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'template_id'})
# x:825 y:461
OperatableStateMachine.add('Wait_For_Stand',
CheckCurrentControlModeState(target_mode=CheckCurrentControlModeState.STAND, wait=True),
transitions={'correct': 'finished', 'incorrect': 'failed'},
autonomy={'correct': Autonomy.Low, 'incorrect': Autonomy.Full},
remapping={'control_mode': 'control_mode'})
# x:1143 y:277
OperatableStateMachine.add('Decide_Stand_Preference',
OperatorDecisionState(outcomes=["same", "next", "abort"], hint="Same or next stand pose?", suggestion="next"),
transitions={'same': 'Inform_About_Retry', 'next': 'Increment_Stand_Pose', 'abort': 'aborted'},
autonomy={'same': Autonomy.Full, 'next': Autonomy.Full, 'abort': Autonomy.Full})
# x:842 y:152
OperatableStateMachine.add('Planning_Pipeline',
_sm_planning_pipeline_0,
transitions={'finished': 'Walk_To_Waypoint', 'failed': 'Decide_Stand_Preference', 'aborted': 'Decide_Stand_Preference'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit},
remapping={'stand_pose': 'stand_pose', 'plan_header': 'plan_header'})
# x:833 y:276
OperatableStateMachine.add('Walk_To_Waypoint',
ExecuteStepPlanActionState(),
transitions={'finished': 'Wait_For_Stand', 'failed': 'Decide_Stand_Preference'},
autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'plan_header': 'plan_header'})
# x:554 y:195
OperatableStateMachine.add('Decide_Repeat_Request',
OperatorDecisionState(outcomes=['repeat_id', 'request_goal'], hint=None, suggestion=None),
transitions={'repeat_id': 'Request_Template', 'request_goal': 'Get_Goal_from_Operator'},
autonomy={'repeat_id': Autonomy.Low, 'request_goal': Autonomy.High})
# x:547 y:27
OperatableStateMachine.add('Get_Stand_Pose',
GetTemplateStandPoseState(),
transitions={'done': 'Planning_Pipeline', 'failed': 'Inform_About_Fail', 'not_available': 'Inform_About_Fail'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.High},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'stand_pose': 'stand_pose'})
# x:183 y:590, x:383 y:290
_sm_manipulate_valve_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'preference', 'none'])
with _sm_manipulate_valve_7:
# x:144 y:72
OperatableStateMachine.add('Insert_Hand',
_sm_insert_hand_4,
transitions={'finished': 'Perform_Turning', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'preference': 'preference', 'none': 'none'})
# x:543 y:422
OperatableStateMachine.add('Extract_Hand',
_sm_extract_hand_3,
transitions={'finished': 'Decide_Repeat', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'preference': 'preference', 'none': 'none'})
# x:534 y:72
OperatableStateMachine.add('Perform_Turning',
_sm_perform_turning_2,
transitions={'finished': 'Extract_Hand', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none', 'back_rotation': 'back_rotation'})
# x:127 y:272
OperatableStateMachine.add('Perform_Turn_Back',
_sm_perform_turn_back_1,
transitions={'finished': 'Adjust_Hand_Rotation', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'back_rotation': 'back_rotation', 'none': 'none'})
# x:137 y:428
OperatableStateMachine.add('Decide_Repeat',
OperatorDecisionState(outcomes=['insert_again', 'continue'], hint="Continue or adjust wry2 rotation and rotate again", suggestion='insert_again'),
transitions={'insert_again': 'Perform_Turn_Back', 'continue': 'finished'},
autonomy={'insert_again': Autonomy.High, 'continue': Autonomy.Full})
# x:134 y:178
OperatableStateMachine.add('Adjust_Hand_Rotation',
OperatorDecisionState(outcomes=['done'], hint="Adjust poking stick rotation", suggestion=None),
transitions={'done': 'Insert_Hand'},
autonomy={'done': Autonomy.Full})
# x:83 y:340, x:333 y:140
_sm_step_back_8 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['distance'])
with _sm_step_back_8:
# x:28 y:63
OperatableStateMachine.add('Plan_Steps_Back',
FootstepPlanRelativeState(direction=FootstepPlanRelativeState.DIRECTION_BACKWARD),
transitions={'planned': 'Execute_Steps_Back', 'failed': 'failed'},
autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full},
remapping={'distance': 'distance', 'plan_header': 'plan_header'})
# x:24 y:163
OperatableStateMachine.add('Execute_Steps_Back',
ExecuteStepPlanActionState(),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'plan_header': 'plan_header'})
# x:638 y:580, x:230 y:239
_sm_prepare_manipulation_9 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'grasp_preference', 'hand_side', 'none'])
with _sm_prepare_manipulation_9:
# x:65 y:36
OperatableStateMachine.add('Go_To_Manipulate',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={'changed': 'Set_Template_Frame', 'failed': 'failed'},
autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full})
# x:586 y:463
OperatableStateMachine.add('Adjust_Hand_Rotation',
OperatorDecisionState(outcomes=['done'], hint="Adjust poking stick rotation", suggestion=None),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Full})
# x:116 y:409
OperatableStateMachine.add('Get_Pregrasp',
GetTemplatePregraspState(),
transitions={'done': 'Plan_To_Pregrasp', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pre_grasp'})
# x:329 y:178
OperatableStateMachine.add('Plan_To_Pregrasp',
PlanEndeffectorPoseState(ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"),
transitions={'planned': 'Move_To_Pregrasp', 'failed': 'failed'},
autonomy={'planned': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'target_pose': 'pre_grasp', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory'})
# x:576 y:178
OperatableStateMachine.add('Move_To_Pregrasp',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'Prepare_Joint_Limit', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:577 y:322
OperatableStateMachine.add('Prepare_Joint_Limit',
_sm_prepare_joint_limit_5,
transitions={'finished': 'Adjust_Hand_Rotation', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none'})
# x:53 y:130
OperatableStateMachine.add('Set_Template_Frame',
CalculationState(calculation=lambda x: 'template_tf_' + str(x)),
transitions={'done': 'Look_At_Valve'},
autonomy={'done': Autonomy.Low},
remapping={'input_value': 'template_id', 'output_value': 'template_frame'})
# x:51 y:213
OperatableStateMachine.add('Look_At_Valve',
LookAtTargetState(),
transitions={'done': 'Align_Valve_Log'},
autonomy={'done': Autonomy.Low},
remapping={'frame': 'template_frame'})
# x:55 y:301
OperatableStateMachine.add('Align_Valve_Log',
LogState(text="Adjust pose of the valve template", severity=Logger.REPORT_HINT),
transitions={'done': 'Get_Pregrasp'},
autonomy={'done': Autonomy.Full})
with _state_machine:
# x:35 y:78
OperatableStateMachine.add('Request_Template_ID',
InputState(request=InputState.SELECTED_OBJECT_ID, message="Place the valve template"),
transitions={'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.High, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'template_id'})
# x:473 y:72
OperatableStateMachine.add('Prepare_Manipulation',
_sm_prepare_manipulation_9,
transitions={'finished': 'Manipulate_Valve', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'template_id': 'template_id', 'grasp_preference': 'default_preference', 'hand_side': 'hand_side', 'none': 'none'})
# x:237 y:28
OperatableStateMachine.add('Decide_Walking',
OperatorDecisionState(outcomes=['walk', 'skip'], hint="Walk to the valve?", suggestion='skip'),
transitions={'walk': 'Walk_To_Template', 'skip': 'Prepare_Manipulation'},
autonomy={'walk': Autonomy.Full, 'skip': Autonomy.Low})
# x:644 y:557
OperatableStateMachine.add('Step_Back',
_sm_step_back_8,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'distance': 'step_back_distance'})
# x:637 y:428
OperatableStateMachine.add('Decide_Step_Back',
OperatorDecisionState(outcomes=["stand", "step_back"], hint="Should the robot step back from the valve?", suggestion="step_back"),
transitions={'stand': 'finished', 'step_back': 'Step_Back'},
autonomy={'stand': Autonomy.Full, 'step_back': Autonomy.High})
# x:738 y:228
OperatableStateMachine.add('Inform_About_Stand',
LogState(text="Back to STAND", severity=Logger.REPORT_INFO),
transitions={'done': 'Go_To_Stand'},
autonomy={'done': Autonomy.Low})
# x:733 y:72
OperatableStateMachine.add('Manipulate_Valve',
_sm_manipulate_valve_7,
transitions={'finished': 'Inform_About_Stand', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'default_preference', 'none': 'none'})
# x:616 y:328
OperatableStateMachine.add('Go_To_Stand',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND),
transitions={'changed': 'Decide_Step_Back', 'failed': 'failed'},
autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full})
# x:230 y:122
OperatableStateMachine.add('Walk_To_Template',
_sm_walk_to_template_6,
transitions={'finished': 'Prepare_Manipulation', 'failed': 'failed', 'aborted': 'Prepare_Manipulation'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit},
remapping={'template_id': 'template_id', 'grasp_preference': 'default_preference', 'hand_side': 'hand_side'})
return _state_machine
def create(self):
waypoints = [(1.0, 0.0)] # list of (x,y)
orientations = [0] # list of angles (degrees)
# x:1329 y:218, x:1324 y:61
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.target_pose = None
_state_machine.userdata.target_pose_index = 0
_state_machine.userdata.bagfile_name = '' # calculated
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# If the user doesn't specify any waypoints, default to these
if not waypoints:
waypoints = [(1.0, 0.0), (2.0, 0.26795), (2.73205, 1.0),
(2.0, 0.26795)]
orientations = [0, 30, 60, 30]
orientations = [ang * math.pi / 180
for ang in orientations] # convert to radians
# Prepare a list of pose stamped waypoints for use by the footstep planning state
self._poses_to_visit = list()
for i in range(0, len(waypoints)):
pt = Point(x=waypoints[i][0], y=waypoints[i][1])
qt = tf.transformations.quaternion_from_euler(
0, 0, orientations[i])
p = Pose(position=pt, orientation=Quaternion(*qt))
pose_stamped = PoseStamped(header=Header(frame_id='/world'),
pose=p)
self._poses_to_visit.append(pose_stamped)
# Topics of interest will be rosbag recorded and the bagfiles will be stored in the folder below
logs_folder = os.path.expanduser('~/footstep_tests/')
if not os.path.exists(logs_folder):
os.makedirs(logs_folder)
_state_machine.userdata.bagfile_name = logs_folder + "run_" + time.strftime(
"%Y-%m-%d-%H_%M") + ".bag"
# [/MANUAL_CREATE]
# x:730 y:38, x:328 y:117
_sm_perform_walking_0 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['target_pose'])
with _sm_perform_walking_0:
# x:157 y:27
OperatableStateMachine.add(
'Create_Step_Goal',
CreateStepGoalState(pose_is_pelvis=False),
transitions={
'done': 'Plan_To_Next_Pose',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={
'target_pose': 'target_pose',
'step_goal': 'step_goal'
})
# x:401 y:188
OperatableStateMachine.add(
'Print_The_Footstep_Plan',
CalculationState(calculation=self.print_plan),
transitions={'done': 'Execute_Footstep_Plan'},
autonomy={'done': Autonomy.High},
remapping={
'input_value': 'plan_header',
'output_value': 'output_value'
})
# x:398 y:30
OperatableStateMachine.add(
'Execute_Footstep_Plan',
ExecuteStepPlanActionState(),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={'plan_header': 'plan_header'})
# x:156 y:188
OperatableStateMachine.add(
'Plan_To_Next_Pose',
PlanFootstepsState(
mode=PlanFootstepsState.MODE_STEP_NO_COLLISION),
transitions={
'planned': 'Print_The_Footstep_Plan',
'failed': 'failed'
},
autonomy={
'planned': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'step_goal': 'step_goal',
'plan_header': 'plan_header'
})
# x:1253 y:294, x:470 y:218
_sm_test_ocs_modified_plan_1 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['target_pose_index'])
with _sm_test_ocs_modified_plan_1:
# x:116 y:56
OperatableStateMachine.add(
'Set_Target_First_Pose',
CalculationState(
calculation=lambda x: self._poses_to_visit[0]),
transitions={'done': 'Create_Goal'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'target_pose_index',
'output_value': 'target_pose'
})
# x:707 y:54
OperatableStateMachine.add(
'Request_Plan',
PlanFootstepsState(
mode=PlanFootstepsState.MODE_STEP_NO_COLLISION),
transitions={
'planned': 'Print_Original_Plan',
'failed': 'failed'
},
autonomy={
'planned': Autonomy.High,
'failed': Autonomy.Low
},
remapping={
'step_goal': 'step_goal',
'plan_header': 'plan_header'
})
# x:1018 y:128
OperatableStateMachine.add(
'Modify',
InputState(
request=InputState.FOOTSTEP_PLAN_HEADER,
message='Modify plan (if necessary) and then confirm.'),
transitions={
'received': 'Print_Modified_Plan',
'aborted': 'failed',
'no_connection': 'failed',
'data_error': 'failed'
},
autonomy={
'received': Autonomy.High,
'aborted': Autonomy.Low,
'no_connection': Autonomy.Low,
'data_error': Autonomy.Low
},
remapping={'data': 'plan_header_new'})
# x:971 y:358
OperatableStateMachine.add(
'Execute',
ExecuteStepPlanActionState(),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={'plan_header': 'plan_header_new'})
# x:431 y:56
OperatableStateMachine.add(
'Create_Goal',
CreateStepGoalState(pose_is_pelvis=False),
transitions={
'done': 'Request_Plan',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={
'target_pose': 'target_pose',
'step_goal': 'step_goal'
})
# x:1005 y:228
OperatableStateMachine.add(
'Print_Modified_Plan',
CalculationState(calculation=self.print_plan),
transitions={'done': 'Execute'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'plan_header_new',
'output_value': 'output_value'
})
# x:989 y:28
OperatableStateMachine.add(
'Print_Original_Plan',
CalculationState(calculation=self.print_plan),
transitions={'done': 'Modify'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'plan_header',
'output_value': 'output_value'
})
# x:8 y:123, x:561 y:146
_sm_test_waypoint_following_2 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['target_pose_index'])
with _sm_test_waypoint_following_2:
# x:361 y:28
OperatableStateMachine.add(
'Decide_Next_Target_Pose',
DecisionState(outcomes=['continue', 'finished'],
conditions=lambda x: 'continue'
if x < len(waypoints) else 'finished'),
transitions={
'continue': 'Set_Next_Target_Pose',
'finished': 'finished'
},
autonomy={
'continue': Autonomy.Low,
'finished': Autonomy.High
},
remapping={'input_value': 'target_pose_index'})
# x:349 y:254
OperatableStateMachine.add(
'Increment_To_Next_Target_Pose',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Decide_Next_Target_Pose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'target_pose_index',
'output_value': 'target_pose_index'
})
# x:699 y:30
OperatableStateMachine.add(
'Set_Next_Target_Pose',
CalculationState(
calculation=lambda x: self._poses_to_visit[x]),
transitions={'done': 'Perform_Walking'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'target_pose_index',
'output_value': 'target_pose'
})
# x:679 y:254
OperatableStateMachine.add(
'Wait_For_Stand',
CheckCurrentControlModeState(
target_mode=CheckCurrentControlModeState.STAND, wait=True),
transitions={
'correct': 'Increment_To_Next_Target_Pose',
'incorrect': 'failed'
},
autonomy={
'correct': Autonomy.High,
'incorrect': Autonomy.Low
},
remapping={'control_mode': 'control_mode'})
# x:699 y:132
OperatableStateMachine.add(
'Perform_Walking',
_sm_perform_walking_0,
transitions={
'finished': 'Wait_For_Stand',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'target_pose': 'target_pose'})
with _state_machine:
# x:82 y:28
OperatableStateMachine.add(
'Start_Recording',
StartRecordLogsState(topics_to_record=self.topics_to_record),
transitions={'logging': 'Wait_For_Rosbag_Process'},
autonomy={'logging': Autonomy.Off},
remapping={
'bagfile_name': 'bagfile_name',
'rosbag_process': 'rosbag_process'
})
# x:1050 y:211
OperatableStateMachine.add(
'Stop_Recording_If_Finished',
StopRecordLogsState(),
transitions={'stopped': 'finished'},
autonomy={'stopped': Autonomy.Off},
remapping={'rosbag_process': 'rosbag_process'})
# x:1035 y:52
OperatableStateMachine.add(
'Stop_Recording_Before_Failed',
StopRecordLogsState(),
transitions={'stopped': 'failed'},
autonomy={'stopped': Autonomy.Off},
remapping={'rosbag_process': 'rosbag_process'})
# x:74 y:110
OperatableStateMachine.add(
'Wait_For_Rosbag_Process',
WaitState(wait_time=2),
transitions={'done': 'Decide_Test_Type'},
autonomy={'done': Autonomy.Off})
# x:570 y:28
OperatableStateMachine.add(
'Test_Waypoint_Following',
_sm_test_waypoint_following_2,
transitions={
'finished': 'Stop_Recording_If_Finished',
'failed': 'Stop_Recording_Before_Failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'target_pose_index': 'target_pose_index'})
# x:357 y:110
OperatableStateMachine.add('Decide_Test_Type',
OperatorDecisionState(outcomes=[
'waypoints', 'wide_stance',
'modified'
],
hint=None,
suggestion=None),
transitions={
'waypoints':
'Test_Waypoint_Following',
'wide_stance':
'Manipulation Config',
'modified': 'Test_OCS_Modified_Plan'
},
autonomy={
'waypoints': Autonomy.High,
'wide_stance': Autonomy.High,
'modified': Autonomy.High
})
# x:454 y:250
OperatableStateMachine.add(
'Manipulation Config',
self.use_behavior(ManipulationConfigSM, 'Manipulation Config'),
transitions={
'finished': 'Locomotion Config',
'failed': 'Stop_Recording_Before_Failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:704 y:252
OperatableStateMachine.add(
'Locomotion Config',
self.use_behavior(LocomotionConfigSM, 'Locomotion Config'),
transitions={
'finished': 'Stop_Recording_If_Finished',
'failed': 'Stop_Recording_Before_Failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:572 y:148
OperatableStateMachine.add(
'Test_OCS_Modified_Plan',
_sm_test_ocs_modified_plan_1,
transitions={
'finished': 'Stop_Recording_If_Finished',
'failed': 'Stop_Recording_Before_Failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'target_pose_index': 'target_pose_index'})
return _state_machine
def create(self):
# x:902 y:403, x:697 y:491
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.be_evil = False
_state_machine.userdata.unused = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:32 y:127
OperatableStateMachine.add('SetStandingPose',
SetJointState(controller='motion/controller/joint_state_head', pose_param='body_nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"),
transitions={'done': 'SelectTrick', 'failed': 'failed', 'timeout': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off})
# x:371 y:161
OperatableStateMachine.add('Greeting',
self.use_behavior(GreetingSM, 'Greeting'),
transitions={'finished': 'RandHead', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:371 y:284
OperatableStateMachine.add('Play',
self.use_behavior(PlaySM, 'Play'),
transitions={'finished': 'RandHead', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:202 y:154
OperatableStateMachine.add('SelectTrick',
DecisionState(outcomes=['play','cheer','greet'], conditions=lambda x: random.choice(['play','cheer','greet'])),
transitions={'play': 'Play', 'cheer': 'Cheer', 'greet': 'Greeting'},
autonomy={'play': Autonomy.Off, 'cheer': Autonomy.Off, 'greet': Autonomy.Off},
remapping={'input_value': 'unused'})
# x:823 y:274
OperatableStateMachine.add('PrepareToWalk',
ExecuteJointTrajectory(action_topic='motion/controller/joint_trajectory', trajectory_param='crouch_begin', trajectory_ns='saved_msgs/joint_trajectory'),
transitions={'success': 'finished', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed'},
autonomy={'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off})
# x:376 y:38
OperatableStateMachine.add('Cheer',
self.use_behavior(CheerSM, 'Cheer'),
transitions={'finished': 'RandHead', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:774 y:45
OperatableStateMachine.add('RandHead',
SweetieBotRandHeadMovements(controller='joint_state_head', duration=8, interval=[3,5], max2356=[0.3,0.3,1.5,1.5], min2356=[-0.3,-0.3,-1.5,-1.5]),
transitions={'done': 'SetHeadNominal', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'config': 'unused'})
# x:809 y:138
OperatableStateMachine.add('SetHeadNominal',
SetJointState(controller='motion/controller/joint_state_head', pose_param='head_nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"),
transitions={'done': 'PrepareToWalk', 'failed': 'failed', 'timeout': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off})
return _state_machine
def create(self):
joy_topic = '/hmi/joystick'
# x:33 y:423, x:683 y:568
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['be_evil'])
_state_machine.userdata.cycle_counter = 0
_state_machine.userdata.be_evil = self.be_evil
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:356, x:644 y:383
_sm_detectjoyevent_0 = OperatableStateMachine(
outcomes=['received', 'unavailable'])
with _sm_detectjoyevent_0:
# x:229 y:154
OperatableStateMachine.add('JoyEvent',
SubscriberState(topic=joy_topic,
blocking=True,
clear=False),
transitions={
'received': 'CheckEvent',
'unavailable': 'unavailable'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:229 y:296
OperatableStateMachine.add(
'CheckEvent',
DecisionState(
outcomes=['yes', 'no'],
conditions=lambda msg: 'yes'
if any(msg.buttons) or any(msg.axes[0:5]) else 'no'),
transitions={
'yes': 'received',
'no': 'JoyEvent'
},
autonomy={
'yes': Autonomy.Off,
'no': Autonomy.Off
},
remapping={'input_value': 'message'})
# x:778 y:316, x:75 y:354, x:294 y:358, x:776 y:358, x:777 y:406, x:530 y:356
_sm_checkjoy_1 = ConcurrencyContainer(
outcomes=['no_activity', 'activity_detected', 'failed'],
conditions=[('no_activity', [('WaitForJoy', 'done')]),
('failed', [('DetectJoyEvent', 'unavailable')]),
('activity_detected', [('DetectJoyEvent', 'received')])
])
with _sm_checkjoy_1:
# x:219 y:132
OperatableStateMachine.add('DetectJoyEvent',
_sm_detectjoyevent_0,
transitions={
'received': 'activity_detected',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Inherit,
'unavailable': Autonomy.Inherit
})
# x:624 y:152
OperatableStateMachine.add('WaitForJoy',
WaitState(wait_time=self.wait_time),
transitions={'done': 'no_activity'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:646 y:31
OperatableStateMachine.add(
'RandomChoose',
DecisionState(outcomes=['greeting', 'play', 'cheer', 'bad'],
conditions=self.select_behavior),
transitions={
'greeting': 'Greeting',
'play': 'Play',
'cheer': 'Cheer',
'bad': 'Bad'
},
autonomy={
'greeting': Autonomy.Low,
'play': Autonomy.Low,
'cheer': Autonomy.Low,
'bad': Autonomy.Low
},
remapping={'input_value': 'cycle_counter'})
# x:383 y:460
OperatableStateMachine.add('CheckJoy',
_sm_checkjoy_1,
transitions={
'no_activity': 'ResetCounter',
'activity_detected':
'AddCycleCounter',
'failed': 'failed'
},
autonomy={
'no_activity': Autonomy.Inherit,
'activity_detected':
Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:221 y:134
OperatableStateMachine.add(
'AddCycleCounter',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'RandomChoose'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'cycle_counter',
'output_value': 'cycle_counter'
})
# x:483 y:171
OperatableStateMachine.add('Greeting',
self.use_behavior(
GreetingSM, 'Greeting'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:849 y:197
OperatableStateMachine.add('Cheer',
self.use_behavior(CheerSM, 'Cheer'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:687 y:197
OperatableStateMachine.add('Play',
self.use_behavior(PlaySM, 'Play'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:991 y:203
OperatableStateMachine.add('Bad',
self.use_behavior(BadSM, 'Bad'),
transitions={
'finished': 'CheckJoy',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:115 y:364
OperatableStateMachine.add(
'ResetCounter',
CalculationState(calculation=lambda x: 0),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'cycle_counter',
'output_value': 'cycle_counter'
})
return _state_machine
def create(self):
joint_trajectory_action = 'motion/controller/joint_trajectory'
voice_topic = 'control'
storage = 'joint_trajectory/'
# x:901 y:554, x:895 y:38
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['be_evil'])
_state_machine.userdata.be_evil = self.be_evil
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:48 y:105
OperatableStateMachine.add(
'SetHeadNominalPose',
SetJointState(controller='motion/controller/joint_state_head',
pose_param='head_nominal',
pose_ns='saved_msgs/joint_state',
tolerance=0.017,
timeout=10.0,
joint_topic="joint_states"),
transitions={
'done': 'CheckEvil',
'failed': 'failed',
'timeout': 'finished'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off,
'timeout': Autonomy.Off
})
# x:579 y:498
OperatableStateMachine.add(
'HoofStamp',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='hoof_stamp',
trajectory_ns=storage),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:88 y:447
OperatableStateMachine.add(
'RandomChoice',
DecisionState(
outcomes=['evil1', 'evil2'],
conditions=lambda x: random.choice(['evil1', 'evil2'])),
transitions={
'evil1': 'PointDoNotTouch',
'evil2': 'SayWalk'
},
autonomy={
'evil1': Autonomy.High,
'evil2': Autonomy.High
},
remapping={'input_value': 'be_evil'})
# x:303 y:543
OperatableStateMachine.add('SayWalk',
TextCommandState(type='voice/play_wav',
command='i_hate_laws',
topic=voice_topic),
transitions={'done': 'Wait1'},
autonomy={'done': Autonomy.Off})
# x:467 y:539
OperatableStateMachine.add('Wait1',
WaitState(wait_time=0.5),
transitions={'done': 'HoofStamp'},
autonomy={'done': Autonomy.Off})
# x:497 y:102
OperatableStateMachine.add(
'PranceMaxFun',
CompoundActionParam(action_param='prance_maximum_fun',
action_ns='saved_msgs/compound_action'),
transitions={
'success': 'finished',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
})
# x:437 y:275
OperatableStateMachine.add(
'PointDoNotTouch',
CompoundActionParam(action_param='point_do_not_touch',
action_ns='saved_msgs/compound_action'),
transitions={
'success': 'finished',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
})
# x:140 y:251
OperatableStateMachine.add(
'CheckEvil',
DecisionState(outcomes=['good', 'evil'],
conditions=lambda x: 'evil' if x else 'good'),
transitions={
'good': 'PranceMaxFun',
'evil': 'RandomChoice'
},
autonomy={
'good': Autonomy.Off,
'evil': Autonomy.Off
},
remapping={'input_value': 'be_evil'})
return _state_machine
def create(self):
joint_trajectory_action = 'motion/controller/joint_trajectory'
voice_topic = 'control'
storage = 'joint_trajectory/'
# x:1011 y:80, x:1002 y:571
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['be_evil'])
_state_machine.userdata.be_evil = self.be_evil
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:45 y:143
OperatableStateMachine.add(
'SetHeadNominalPose',
SetJointState(controller='motion/controller/joint_state_head',
pose_param='head_nominal',
pose_ns='saved_msgs/joint_state',
tolerance=0.017,
timeout=10.0,
joint_topic="joint_states"),
transitions={
'done': 'CheckEvil',
'failed': 'failed',
'timeout': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off,
'timeout': Autonomy.Off
})
# x:421 y:472
OperatableStateMachine.add('SayDoNotTouch',
TextCommandState(
type='voice/play_wav',
command='do_not_touch_me',
topic=voice_topic),
transitions={'done': 'HoofStamp'},
autonomy={'done': Autonomy.Off})
# x:496 y:147
OperatableStateMachine.add(
'SayOverflow',
TextCommandState(type='voice/play_wav',
command='you_are_using_software_incorrectly',
topic=voice_topic),
transitions={'done': 'Applause'},
autonomy={'done': Autonomy.Off})
# x:747 y:153
OperatableStateMachine.add(
'Applause',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='applause',
trajectory_ns=storage),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:745 y:226
OperatableStateMachine.add(
'NoHeadShake',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='head_shake',
trajectory_ns=storage),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:495 y:227
OperatableStateMachine.add(
'SayDizzy',
TextCommandState(type='voice/play_wav',
command='who_am_i_what_is_my_purpose',
topic=voice_topic),
transitions={'done': 'NoHeadShake'},
autonomy={'done': Autonomy.Off})
# x:648 y:466
OperatableStateMachine.add(
'HoofStamp',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='hoof_stamp',
trajectory_ns=storage),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:242 y:273
OperatableStateMachine.add(
'CheckEvil',
DecisionState(outcomes=['good', 'evil'],
conditions=lambda x: 'evil' if x else 'good'),
transitions={
'good': 'RandomChoiceGood',
'evil': 'SayDoNotTouch'
},
autonomy={
'good': Autonomy.Off,
'evil': Autonomy.Low
},
remapping={'input_value': 'be_evil'})
# x:320 y:143
OperatableStateMachine.add(
'RandomChoiceGood',
DecisionState(
outcomes=['good1', 'good2'],
conditions=lambda x: random.choice(['good1', 'good2'])),
transitions={
'good1': 'SayOverflow',
'good2': 'SayDizzy'
},
autonomy={
'good1': Autonomy.Low,
'good2': Autonomy.Low
},
remapping={'input_value': 'be_evil'})
return _state_machine
def create(self):
joy_topic = '/hmi/joystick'
joint_trajectory_action = '/motion/controller/joint_trajectory'
# x:30 y:365, x:1031 y:242
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.rand_moves_config = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:397 y:287, x:130 y:297, x:230 y:297, x:330 y:297, x:459 y:297, x:530 y:297
_sm_joystickmovements_0 = ConcurrencyContainer(
outcomes=['failed', 'button1234', 'timeout'],
output_keys=['joy_msg'],
conditions=[('timeout', [('JoystickControl', 'done')]),
('failed', [('WaitButton1234Pressed', 'unavailable')]),
('button1234', [('WaitButton1234Pressed', 'received')])
])
with _sm_joystickmovements_0:
# x:385 y:129
OperatableStateMachine.add(
'JoystickControl',
JoystickJointControl(
joints_topic='/joint_states',
goal_joints_topic=
'/motion/controller/joint_state/out_joints_src_reset',
joy_topic=joy_topic,
buttons=[(4, 0.6, 'ear_l_joint', -3.0, 1.0),
(6, -0.6, 'ear_l_joint', -3.0, 1.0),
(5, 0.6, 'ear_r_joint', -3.0, 1.0),
(7, -0.6, 'ear_r_joint', -3.0, 1.0)],
axes=[(0, 0, 'eyes_yaw', -1.5, 1.5),
(1, 0, 'eyes_pitch', -1.5, 1.5),
(4, -0.4, 'head_joint4', -1.5, 1.5),
(5, 0.4, 'head_joint2', -1.5, 1.5),
(3, 0.5, 'mouth_joint', -0.6, 0)],
timeout=5.0),
transitions={'done': 'timeout'},
autonomy={'done': Autonomy.Off})
# x:130 y:136
OperatableStateMachine.add(
'WaitButton1234Pressed',
WaitForMessageState(
topic=joy_topic,
condition=lambda msg: any(msg.buttons[0:4]),
buffered=False,
clear=True),
transitions={
'received': 'button1234',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'joy_msg'})
# x:625 y:339, x:47 y:263, x:323 y:268, x:703 y:252, x:630 y:296, x:443 y:267, x:227 y:263
_sm_randmovements_1 = ConcurrencyContainer(
outcomes=['failed', 'timeout', 'joy_msg'],
input_keys=['rand_moves_config'],
output_keys=['joy_msg'],
conditions=[('failed', [('WaitJoystick', 'unavailable')]),
('failed', [('RandHeadMoves', 'failed')]),
('joy_msg', [('WaitJoystick', 'received')]),
('timeout', [('RandHeadMoves', 'done')])])
with _sm_randmovements_1:
# x:88 y:74
OperatableStateMachine.add(
'WaitJoystick',
WaitForMessageState(
topic=joy_topic,
condition=lambda msg: any(msg.buttons) or any(msg.axes),
buffered=False,
clear=False),
transitions={
'received': 'joy_msg',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'joy_msg'})
# x:332 y:60
OperatableStateMachine.add(
'RandHeadMoves',
RandJointsMovements(controller='joint_state_head',
duration=2000,
interval=[2.0, 5.0],
joints=['head_joint2', 'head_joint4'],
minimal=[-0.2, -0.3],
maximal=[0.3, 0.3]),
transitions={
'done': 'timeout',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'rand_moves_config'})
with _state_machine:
# x:57 y:26
OperatableStateMachine.add('RandMovements',
_sm_randmovements_1,
transitions={
'failed': 'failed',
'timeout': 'RandMovements',
'joy_msg': 'CheckButtonPressed'
},
autonomy={
'failed': Autonomy.Inherit,
'timeout': Autonomy.Inherit,
'joy_msg': Autonomy.Inherit
},
remapping={
'rand_moves_config':
'rand_moves_config',
'joy_msg': 'joy_msg'
})
# x:413 y:17
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(
config_name='head_group_basic',
move_group='head_group',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'SelectMovement',
'planning_failed': 'Wait',
'control_failed': 'Wait',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:373 y:236
OperatableStateMachine.add(
'Greeting',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='greeting',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:85 y:283
OperatableStateMachine.add('JoystickMovements',
_sm_joystickmovements_0,
transitions={
'failed': 'failed',
'button1234': 'PlaceHead',
'timeout': 'RandMovements'
},
autonomy={
'failed': Autonomy.Inherit,
'button1234': Autonomy.Inherit,
'timeout': Autonomy.Inherit
},
remapping={'joy_msg': 'joy_msg'})
# x:612 y:186
OperatableStateMachine.add(
'SelectMovement',
DecisionState(outcomes=[
'none', 'button1', 'button2', 'button3', 'button4'
],
conditions=self.move_selector),
transitions={
'none': 'RandMovements',
'button1': 'Greeting',
'button2': 'HeadShake',
'button3': 'GrownUp',
'button4': 'Greeting'
},
autonomy={
'none': Autonomy.Off,
'button1': Autonomy.Off,
'button2': Autonomy.Off,
'button3': Autonomy.Off,
'button4': Autonomy.Off
},
remapping={'input_value': 'joy_msg'})
# x:412 y:317
OperatableStateMachine.add(
'HeadShake',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='head_shake',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:472 y:404
OperatableStateMachine.add(
'GrownUp',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='very_grown_up',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:167 y:134
OperatableStateMachine.add(
'CheckButtonPressed',
DecisionState(outcomes=['button1234', 'other'],
conditions=lambda msg: 'button1234'
if any(msg.buttons[0:4]) else 'other'),
transitions={
'button1234': 'PlaceHead',
'other': 'JoystickMovements'
},
autonomy={
'button1234': Autonomy.Off,
'other': Autonomy.Off
},
remapping={'input_value': 'joy_msg'})
# x:686 y:104
OperatableStateMachine.add('PlaceHead_2',
SrdfStateToMoveit(
config_name='head_group_basic',
move_group='head_group',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'SelectMovement',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:703 y:18
OperatableStateMachine.add('Wait',
WaitState(wait_time=0.5),
transitions={'done': 'PlaceHead_2'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
joint_trajectory_action = 'motion/controller/joint_trajectory'
storage = 'joint_trajectory/'
voice_topic = 'voice/voice'
eye_cmd_topic = 'control'
# x:1034 y:292, x:622 y:639
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['be_evil', 'new_be_evil'],
output_keys=['be_evil'])
_state_machine.userdata.be_evil = self.be_evil
_state_machine.userdata.new_be_evil = self.new_be_evil
_state_machine.userdata.head_crooked_pose = [0, 0, 0, 0.4]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:96 y:163
OperatableStateMachine.add('CheckEvil',
DecisionState(
outcomes=['good', 'evil'],
conditions=lambda x: 'good'
if not x else 'evil'),
transitions={
'good': 'CheckChangeToEvil',
'evil': 'CheckChangeToGood'
},
autonomy={
'good': Autonomy.Off,
'evil': Autonomy.Off
},
remapping={'input_value': 'be_evil'})
# x:252 y:50
OperatableStateMachine.add(
'CheckChangeToEvil',
DecisionState(outcomes=['yes', 'no'],
conditions=lambda x: 'yes' if x else 'no'),
transitions={
'yes': 'RedEyes',
'no': 'NormalEyes'
},
autonomy={
'yes': Autonomy.Off,
'no': Autonomy.Off
},
remapping={'input_value': 'new_be_evil'})
# x:426 y:48
OperatableStateMachine.add('RedEyes',
TextCommandState(type='eyes/emotion',
command='red_eyes',
topic=eye_cmd_topic),
transitions={'done': 'Seizure'},
autonomy={'done': Autonomy.Off})
# x:737 y:30
OperatableStateMachine.add(
'Seizure',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='seizure_evil',
trajectory_ns=storage),
transitions={
'success': 'SetEvil',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:961 y:162
OperatableStateMachine.add(
'SetEvil',
CalculationState(calculation=lambda x: True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'be_evil',
'output_value': 'be_evil'
})
# x:419 y:135
OperatableStateMachine.add('NormalEyes',
TextCommandState(type='eyes/emotion',
command='normal',
topic=eye_cmd_topic),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:195 y:281
OperatableStateMachine.add(
'CheckChangeToGood',
DecisionState(outcomes=['yes', 'no'],
conditions=lambda x: 'no' if x else 'yes'),
transitions={
'yes': 'NormalEyes2',
'no': 'RedEyes2'
},
autonomy={
'yes': Autonomy.Off,
'no': Autonomy.Off
},
remapping={'input_value': 'new_be_evil'})
# x:416 y:331
OperatableStateMachine.add(
'NormalEyes2',
TextCommandState(type='eyes/emotion',
command='normal',
topic=eye_cmd_topic),
transitions={'done': 'HeadAssumeBasicPose'},
autonomy={'done': Autonomy.Off})
# x:613 y:321
OperatableStateMachine.add(
'ShakeHead',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='head_shake',
trajectory_ns=storage),
transitions={
'success': 'LookAround',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:894 y:326
OperatableStateMachine.add(
'SetGood',
CalculationState(calculation=lambda x: False),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'be_evil',
'output_value': 'be_evil'
})
# x:671 y:437
OperatableStateMachine.add(
'LookAround',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='look_around',
trajectory_ns=storage),
transitions={
'success': 'SetGood',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:420 y:200
OperatableStateMachine.add('RedEyes2',
TextCommandState(type='eyes/emotion',
command='red_eyes',
topic=eye_cmd_topic),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:444 y:420
OperatableStateMachine.add('HeadAssumeBasicPose',
SrdfStateToMoveit(
config_name='head_basic',
move_group='head',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'ShakeHead',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
return _state_machine
def create(self):
arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM
no_valve_collision = True
turn_amount = 400 # degrees (empirical)
keep_orientation = True # for poking stick
# x:1227 y:469, x:290 y:544
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.default_preference = 0
_state_machine.userdata.hand_side = self.hand_side
_state_machine.userdata.step_back_distance = 0.30 # meters
_state_machine.userdata.none = None
_state_machine.userdata.template_id = 0 # adjust before re-runs!
_state_machine.userdata.torso_center = 'same'
_state_machine.userdata.stick_reference = None # hardcoded
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
self._turn_amount = turn_amount
self._keep_orientation = keep_orientation
# Use the poking stick as the reference point for circular affordance
stick_reference_point = PoseStamped()
stick_reference_point.header.frame_id = 'l_hand'
stick_reference_point.pose.position.y = -0.095
stick_reference_point.pose.orientation.w = 1.0
_state_machine.userdata.stick_reference = stick_reference_point
# [/MANUAL_CREATE]
# x:841 y:312, x:634 y:50
_sm_go_to_grasp_0 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['template_id', 'hand_side', 'preference'])
with _sm_go_to_grasp_0:
# x:36 y:46
OperatableStateMachine.add('Get_Grasp',
GetTemplateGraspState(),
transitions={
'done': 'Extract_Frame_Id',
'failed': 'failed',
'not_available': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Low,
'not_available': Autonomy.Low
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'preference': 'preference',
'grasp': 'grasp'
})
# x:248 y:134
OperatableStateMachine.add(
'Plan_To_Grasp',
PlanEndeffectorCartesianWaypointsState(
ignore_collisions=no_valve_collision,
include_torso=False,
keep_endeffector_orientation=True,
allow_incomplete_plans=True,
vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"),
transitions={
'planned': 'Move_To_Grasp',
'incomplete': 'Move_To_Grasp',
'failed': 'Replan_if_Incomplete'
},
autonomy={
'planned': Autonomy.Low,
'incomplete': Autonomy.High,
'failed': Autonomy.High
},
remapping={
'waypoints': 'waypoints',
'hand': 'hand_side',
'frame_id': 'frame_id',
'joint_trajectory': 'joint_trajectory',
'plan_fraction': 'plan_fraction'
})
# x:42 y:266
OperatableStateMachine.add(
'Create_Pose_List',
CalculationState(calculation=lambda x: [x.pose]),
transitions={'done': 'Plan_To_Grasp'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'grasp',
'output_value': 'waypoints'
})
# x:576 y:135
OperatableStateMachine.add(
'Move_To_Grasp',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'Replan_if_Incomplete',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:274 y:307
OperatableStateMachine.add(
'Notify_Incomplete_Plan',
LogState(text='Incomplete Plan! Re-planning...',
severity=Logger.REPORT_HINT),
transitions={'done': 'Plan_To_Grasp'},
autonomy={'done': Autonomy.Off})
# x:585 y:307
OperatableStateMachine.add(
'Replan_if_Incomplete',
DecisionState(outcomes=['replan', 'continue'],
conditions=lambda x: 'replan'
if x < 0.9 else 'continue'),
transitions={
'replan': 'Notify_Incomplete_Plan',
'continue': 'finished'
},
autonomy={
'replan': Autonomy.Low,
'continue': Autonomy.High
},
remapping={'input_value': 'plan_fraction'})
# x:42 y:167
OperatableStateMachine.add(
'Extract_Frame_Id',
CalculationState(calculation=lambda x: x.header.frame_id),
transitions={'done': 'Create_Pose_List'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'grasp',
'output_value': 'frame_id'
})
# x:174 y:319, x:443 y:229
_sm_extract_hand_1 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['hand_side', 'template_id', 'none'])
with _sm_extract_hand_1:
# x:101 y:81
OperatableStateMachine.add(
'Get_Extract_Affordance',
GetTemplateAffordanceState(identifier='extract'),
transitions={
'done': 'Plan_Extract_Affordance',
'failed': 'failed',
'not_available': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High,
'not_available': Autonomy.High
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'affordance': 'extract_affordance'
})
# x:394 y:82
OperatableStateMachine.add(
'Plan_Extract_Affordance',
PlanAffordanceState(vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"),
transitions={
'done': 'Execute_Extract_Affordance',
'incomplete': 'Execute_Extract_Affordance',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'incomplete': Autonomy.High,
'failed': Autonomy.High
},
remapping={
'affordance': 'extract_affordance',
'hand': 'hand_side',
'reference_point': 'none',
'joint_trajectory': 'joint_trajectory',
'plan_fraction': 'plan_fraction'
})
# x:627 y:224
OperatableStateMachine.add(
'Execute_Extract_Affordance',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'Decide_Extract_More',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:394 y:313
OperatableStateMachine.add(
'Decide_Extract_More',
OperatorDecisionState(
outcomes=['extract_more', 'skip'],
hint='Extract more if the fingers are not out yet.',
suggestion='skip'),
transitions={
'extract_more': 'Get_Extract_Affordance',
'skip': 'finished'
},
autonomy={
'extract_more': Autonomy.High,
'skip': Autonomy.Low
})
# x:744 y:263
_sm_insert_hand_2 = OperatableStateMachine(
outcomes=['finished'],
input_keys=['hand_side', 'template_id', 'none'])
with _sm_insert_hand_2:
# x:84 y:72
OperatableStateMachine.add(
'Get_Insert_Affordance',
GetTemplateAffordanceState(identifier='insert'),
transitions={
'done': 'Plan_Insert_Affordance',
'failed': 'Decide_Insert_More',
'not_available': 'Decide_Insert_More'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High,
'not_available': Autonomy.High
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'affordance': 'insert_affordance'
})
# x:342 y:73
OperatableStateMachine.add(
'Plan_Insert_Affordance',
PlanAffordanceState(vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"),
transitions={
'done': 'Execute_Insert_Affordance',
'incomplete': 'Execute_Insert_Affordance',
'failed': 'Decide_Insert_More'
},
autonomy={
'done': Autonomy.Low,
'incomplete': Autonomy.High,
'failed': Autonomy.High
},
remapping={
'affordance': 'insert_affordance',
'hand': 'hand_side',
'reference_point': 'none',
'joint_trajectory': 'joint_trajectory',
'plan_fraction': 'plan_fraction'
})
# x:665 y:73
OperatableStateMachine.add(
'Execute_Insert_Affordance',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'Decide_Insert_More',
'failed': 'Decide_Insert_More'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:345 y:257
OperatableStateMachine.add(
'Decide_Insert_More',
OperatorDecisionState(
outcomes=['insert_more', 'skip'],
hint='Insert more if the fingers are not inside the valve.',
suggestion='skip'),
transitions={
'insert_more': 'Get_Insert_Affordance',
'skip': 'finished'
},
autonomy={
'insert_more': Autonomy.High,
'skip': Autonomy.Low
})
# x:610 y:263
_sm_adjust_wrist_3 = OperatableStateMachine(
outcomes=['finished'],
input_keys=['template_id', 'hand_side', 'none'])
with _sm_adjust_wrist_3:
# x:84 y:72
OperatableStateMachine.add(
'Get_Close_Affordance',
GetTemplateAffordanceState(identifier='close'),
transitions={
'done': 'Plan_Close_Affordance',
'failed': 'Adjust_Hand_Rotation',
'not_available': 'Adjust_Hand_Rotation'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High,
'not_available': Autonomy.High
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'affordance': 'close_affordance'
})
# x:342 y:73
OperatableStateMachine.add(
'Plan_Close_Affordance',
PlanAffordanceState(vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"),
transitions={
'done': 'Execute_Close_Affordance',
'incomplete': 'Execute_Close_Affordance',
'failed': 'Adjust_Hand_Rotation'
},
autonomy={
'done': Autonomy.Low,
'incomplete': Autonomy.High,
'failed': Autonomy.High
},
remapping={
'affordance': 'close_affordance',
'hand': 'hand_side',
'reference_point': 'none',
'joint_trajectory': 'joint_trajectory',
'plan_fraction': 'plan_fraction'
})
# x:665 y:73
OperatableStateMachine.add(
'Execute_Close_Affordance',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'Adjust_Hand_Rotation',
'failed': 'Adjust_Hand_Rotation'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:345 y:257
OperatableStateMachine.add(
'Adjust_Hand_Rotation',
OperatorDecisionState(
outcomes=['done'],
hint="Adjust wry2 rotation (near opposing joint limit)",
suggestion=None),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Full})
# x:605 y:445, x:389 y:143
_sm_turn_valve_4 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['stick_reference', 'template_id', 'hand_side'])
with _sm_turn_valve_4:
# x:109 y:56
OperatableStateMachine.add(
'Get_Turning_Affordance',
GetTemplateAffordanceState(identifier='open'),
transitions={
'done': 'Set_Rotation',
'failed': 'failed',
'not_available': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High,
'not_available': Autonomy.High
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'affordance': 'turning_affordance'
})
# x:111 y:292
OperatableStateMachine.add(
'Plan_Turning_Affordance',
PlanAffordanceState(vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"),
transitions={
'done': 'Execute_Turning_Affordance',
'incomplete': 'Execute_Turning_Affordance',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'incomplete': Autonomy.High,
'failed': Autonomy.High
},
remapping={
'affordance': 'turning_affordance',
'hand': 'hand_side',
'reference_point': 'stick_reference',
'joint_trajectory': 'joint_trajectory',
'plan_fraction': 'plan_fraction'
})
# x:127 y:173
OperatableStateMachine.add(
'Set_Rotation',
CalculationState(calculation=self.reduce_rotation_angle),
transitions={'done': 'Plan_Turning_Affordance'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'turning_affordance',
'output_value': 'turning_affordance'
})
# x:526 y:292
OperatableStateMachine.add(
'Execute_Turning_Affordance',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'Decide_Keep_Turning',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:324 y:437
OperatableStateMachine.add(
'Decide_Keep_Turning',
OperatorDecisionState(
outcomes=['done', 'turn_more'],
hint='Check whether the valve is turned enough',
suggestion='done'),
transitions={
'done': 'finished',
'turn_more': 'Plan_Turning_Affordance'
},
autonomy={
'done': Autonomy.High,
'turn_more': Autonomy.High
})
# x:175 y:378, x:413 y:156
_sm_face_valve_5 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['hand_side'])
with _sm_face_valve_5:
# x:126 y:29
OperatableStateMachine.add(
'Go_to_MANIPULATE',
ChangeControlModeActionState(
target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={
'changed': 'Tilt_Head_Down',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Off,
'failed': Autonomy.High
})
# x:148 y:262
OperatableStateMachine.add(
'Tilt_Head_Down',
TiltHeadState(desired_tilt=TiltHeadState.DOWN_45),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
})
# x:660 y:473, x:418 y:191
_sm_retract_arms_6 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['hand_side', 'torso_center'])
with _sm_retract_arms_6:
# x:112 y:28
OperatableStateMachine.add(
'Set_MANIPULATE',
ChangeControlModeActionState(
target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={
'changed': 'Move_to_Stand_Posture',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Off,
'failed': Autonomy.High
})
# x:118 y:360
OperatableStateMachine.add(
'Move_to_Stand_Posture',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.SINGLE_ARM_STAND,
vel_scaling=0.2,
ignore_collisions=False,
link_paddings={},
is_cartesian=False),
transitions={
'done': 'Center_Torso',
'failed': 'Move_to_Stand_Posture'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'side': 'hand_side'})
# x:359 y:464
OperatableStateMachine.add(
'Set_STAND',
ChangeControlModeActionState(
target_mode=ChangeControlModeActionState.STAND),
transitions={
'changed': 'finished',
'failed': 'failed'
},
autonomy={
'changed': Autonomy.Off,
'failed': Autonomy.High
})
# x:118 y:464
OperatableStateMachine.add(
'Center_Torso',
MoveitPredefinedPoseState(
target_pose=MoveitPredefinedPoseState.
TURN_TORSO_CENTER_POSE,
vel_scaling=0.2,
ignore_collisions=True,
link_paddings={},
is_cartesian=False),
transitions={
'done': 'Set_STAND',
'failed': 'Center_Torso'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'side': 'torso_center'})
# x:505 y:87
OperatableStateMachine.add('Open_Fingers',
FingerConfigurationState(
hand_type=self.hand_type,
configuration=0.0),
transitions={
'done': 'Close_Fingers',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'hand_side': 'hand_side'})
# x:505 y:185
OperatableStateMachine.add('Close_Fingers',
FingerConfigurationState(
hand_type=self.hand_type,
configuration=1.0),
transitions={
'done': 'Close_Fingers',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'hand_side': 'hand_side'})
# x:495 y:47, x:187 y:229
_sm_request_template_7 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['none'],
output_keys=['template_id'])
with _sm_request_template_7:
# x:155 y:42
OperatableStateMachine.add(
'Request_Template_ID',
InputState(request=InputState.SELECTED_OBJECT_ID,
message="Provide the ID of the placed template."),
transitions={
'received': 'finished',
'aborted': 'failed',
'no_connection': 'Log_No_Connection',
'data_error': 'Log_Data_Error'
},
autonomy={
'received': Autonomy.Off,
'aborted': Autonomy.High,
'no_connection': Autonomy.Low,
'data_error': Autonomy.Low
},
remapping={'data': 'template_id'})
# x:279 y:119
OperatableStateMachine.add('Log_No_Connection',
LogState(
text="Have no connection to OCS!",
severity=Logger.REPORT_ERROR),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:530 y:133
OperatableStateMachine.add('Log_Data_Error',
LogState(
text="Received wrong data format!",
severity=Logger.REPORT_ERROR),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:335 y:193, x:335 y:112
_sm_step_back_8 = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['distance'])
with _sm_step_back_8:
# x:41 y:63
OperatableStateMachine.add(
'Plan_Steps_Back',
FootstepPlanRelativeState(
direction=FootstepPlanRelativeState.DIRECTION_BACKWARD),
transitions={
'planned': 'Execute_Steps_Back',
'failed': 'failed'
},
autonomy={
'planned': Autonomy.High,
'failed': Autonomy.High
},
remapping={
'distance': 'distance',
'plan_header': 'plan_header'
})
# x:39 y:190
OperatableStateMachine.add(
'Execute_Steps_Back',
ExecuteStepPlanActionState(),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'plan_header': 'plan_header'})
# x:1156 y:62, x:414 y:199
_sm_prepare_manipulation_9 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['template_id', 'grasp_preference', 'hand_side'])
with _sm_prepare_manipulation_9:
# x:41 y:61
OperatableStateMachine.add(
'Optional_Template_Adjustment',
LogState(text='Request template adjustment from the operators',
severity=Logger.REPORT_HINT),
transitions={'done': 'Get_Pregrasp'},
autonomy={'done': Autonomy.High})
# x:49 y:442
OperatableStateMachine.add(
'Plan_To_Pregrasp',
PlanEndeffectorPoseState(
ignore_collisions=False,
include_torso=False,
allowed_collisions=[],
planner_id="RRTConnectkConfigDefault"),
transitions={
'planned': 'Move_To_Pregrasp',
'failed': 'failed'
},
autonomy={
'planned': Autonomy.Low,
'failed': Autonomy.High
},
remapping={
'target_pose': 'pre_grasp',
'hand': 'hand_side',
'joint_trajectory': 'joint_trajectory'
})
# x:365 y:440
OperatableStateMachine.add(
'Move_To_Pregrasp',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'Request_Hand_Adjustment',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:678 y:54
OperatableStateMachine.add('Go_to_Grasp',
_sm_go_to_grasp_0,
transitions={
'finished':
'Request_Stick_Adjustment',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'preference': 'grasp_preference'
})
# x:49 y:192
OperatableStateMachine.add('Get_Pregrasp',
GetTemplatePregraspState(),
transitions={
'done': 'Plan_To_Pregrasp',
'failed': 'failed',
'not_available': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High,
'not_available': Autonomy.High
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'preference': 'grasp_preference',
'pre_grasp': 'pre_grasp'
})
# x:656 y:441
OperatableStateMachine.add(
'Request_Hand_Adjustment',
LogState(text='Request adjustment of the hand',
severity=Logger.REPORT_HINT),
transitions={'done': 'Go_to_Grasp'},
autonomy={'done': Autonomy.High})
# x:902 y:57
OperatableStateMachine.add(
'Request_Stick_Adjustment',
LogState(text='Request adjustment of the poking stick',
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.High})
# x:118 y:674, x:383 y:310, x:455 y:159
_sm_manipulate_valve_10 = OperatableStateMachine(
outcomes=['finished', 'failed', 'pregrasp_again'],
input_keys=['template_id', 'hand_side', 'none'])
with _sm_manipulate_valve_10:
# x:92 y:25
OperatableStateMachine.add('Adjust_Wrist',
_sm_adjust_wrist_3,
transitions={'finished': 'Insert_Hand'},
autonomy={'finished': Autonomy.Inherit},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'none': 'none'
})
# x:739 y:128
OperatableStateMachine.add(
'Get_Turning_Affordance',
GetTemplateAffordanceState(identifier='open'),
transitions={
'done': 'Set_Rotation',
'failed': 'failed',
'not_available': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High,
'not_available': Autonomy.High
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'affordance': 'turning_affordance'
})
# x:85 y:535
OperatableStateMachine.add(
'Decide_Repeat',
OperatorDecisionState(
outcomes=['insert_again', 'continue'],
hint="Continue or adjust and rotate again",
suggestion='continue'),
transitions={
'insert_again': 'Adjust_Wrist',
'continue': 'finished'
},
autonomy={
'insert_again': Autonomy.High,
'continue': Autonomy.High
})
# x:744 y:304
OperatableStateMachine.add(
'Plan_Turning_Affordance',
PlanAffordanceState(vel_scaling=0.1,
planner_id="RRTConnectkConfigDefault"),
transitions={
'done': 'Execute_Turning_Affordance',
'incomplete': 'Execute_Turning_Affordance',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'incomplete': Autonomy.High,
'failed': Autonomy.High
},
remapping={
'affordance': 'turning_affordance',
'hand': 'hand_side',
'reference_point': 'none',
'joint_trajectory': 'joint_trajectory',
'plan_fraction': 'plan_fraction'
})
# x:324 y:25
OperatableStateMachine.add(
'Insert_Hand',
_sm_insert_hand_2,
transitions={'finished': 'Decide_Ready_or_Retry'},
autonomy={'finished': Autonomy.Inherit},
remapping={
'hand_side': 'hand_side',
'template_id': 'template_id',
'none': 'none'
})
# x:744 y:531
OperatableStateMachine.add('Extract_Hand',
_sm_extract_hand_1,
transitions={
'finished': 'Decide_Repeat',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'hand_side': 'hand_side',
'template_id': 'template_id',
'none': 'none'
})
# x:771 y:215
OperatableStateMachine.add(
'Set_Rotation',
CalculationState(calculation=self.reduce_rotation_angle),
transitions={'done': 'Plan_Turning_Affordance'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'turning_affordance',
'output_value': 'turning_affordance'
})
# x:736 y:406
OperatableStateMachine.add(
'Execute_Turning_Affordance',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={
'done': 'Decide_Keep_Turning',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.High
},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:994 y:406
OperatableStateMachine.add(
'Decide_Keep_Turning',
OperatorDecisionState(outcomes=['retract', 'turn_more'],
hint='If OK, keep turning the valve!',
suggestion=None),
transitions={
'retract': 'Extract_Hand',
'turn_more': 'Plan_Turning_Affordance'
},
autonomy={
'retract': Autonomy.High,
'turn_more': Autonomy.High
})
# x:599 y:28
OperatableStateMachine.add(
'Decide_Ready_or_Retry',
OperatorDecisionState(
outcomes=['pregrasp_again', 'tempalte_ready'],
hint=
'Either adjust the template or (if something went wrong) go back to pregrasp.',
suggestion='template_ready'),
transitions={
'pregrasp_again': 'pregrasp_again',
'tempalte_ready': 'Get_Turning_Affordance'
},
autonomy={
'pregrasp_again': Autonomy.High,
'tempalte_ready': Autonomy.Low
})
with _state_machine:
# x:51 y:195
OperatableStateMachine.add(
'Notify_Execution_Started',
LogState(
text=
'Execution has started. Consider making modifications if this is a re-run.',
severity=Logger.REPORT_HINT),
transitions={'done': 'Request_Template'},
autonomy={'done': Autonomy.Full})
# x:823 y:330
OperatableStateMachine.add('Manipulate_Valve',
_sm_manipulate_valve_10,
transitions={
'finished': 'Retract_Arms',
'failed': 'Manipulate_Valve',
'pregrasp_again': 'Manipulate_Valve'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'pregrasp_again': Autonomy.Inherit
},
remapping={
'template_id': 'template_id',
'hand_side': 'hand_side',
'none': 'none'
})
# x:737 y:64
OperatableStateMachine.add('Prepare_Manipulation',
_sm_prepare_manipulation_9,
transitions={
'finished': 'Turn_Valve',
'failed': 'Turn_Valve_Manually'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'template_id': 'template_id',
'grasp_preference':
'default_preference',
'hand_side': 'hand_side'
})
# x:253 y:16
OperatableStateMachine.add('Decide_Walking',
OperatorDecisionState(
outcomes=['walk', 'skip'],
hint="Walk to the valve?",
suggestion='walk'),
transitions={
'walk': 'Walk to Template',
'skip': 'Face_Valve'
},
autonomy={
'walk': Autonomy.High,
'skip': Autonomy.Full
})
# x:1030 y:529
OperatableStateMachine.add(
'Step_Back',
_sm_step_back_8,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'distance': 'step_back_distance'})
# x:1019 y:408
OperatableStateMachine.add(
'Decide_Step_Back',
OperatorDecisionState(
outcomes=["stand", "step_back"],
hint="Should the robot step back from the valve?",
suggestion="step_back"),
transitions={
'stand': 'finished',
'step_back': 'Step_Back'
},
autonomy={
'stand': Autonomy.Full,
'step_back': Autonomy.High
})
# x:250 y:122
OperatableStateMachine.add(
'Walk to Template',
self.use_behavior(WalktoTemplateSM, 'Walk to Template'),
transitions={
'finished': 'Face_Valve',
'failed': 'failed',
'aborted': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'aborted': Autonomy.Inherit
},
remapping={
'grasp_preference': 'default_preference',
'hand_side': 'hand_side',
'template_id': 'template_id'
})
# x:53 y:79
OperatableStateMachine.add('Request_Template',
_sm_request_template_7,
transitions={
'finished': 'Decide_Walking',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'none': 'none',
'template_id': 'template_id'
})
# x:1023 y:203
OperatableStateMachine.add('Retract_Arms',
_sm_retract_arms_6,
transitions={
'finished': 'Decide_Step_Back',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'hand_side': 'hand_side',
'torso_center': 'torso_center'
})
# x:752 y:209
OperatableStateMachine.add(
'Turn_Valve_Manually',
LogState(text='Have the operator turn the valve manually.',
severity=Logger.REPORT_HINT),
transitions={'done': 'Retract_Arms'},
autonomy={'done': Autonomy.Full})
# x:504 y:63
OperatableStateMachine.add('Face_Valve',
_sm_face_valve_5,
transitions={
'finished': 'Prepare_Manipulation',
'failed': 'Turn_Valve_Manually'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'hand_side': 'hand_side'})
# x:1026 y:63
OperatableStateMachine.add('Turn_Valve',
_sm_turn_valve_4,
transitions={
'finished': 'Retract_Arms',
'failed': 'Turn_Valve_Manually'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'stick_reference':
'stick_reference',
'template_id': 'template_id',
'hand_side': 'hand_side'
})
return _state_machine
def create(self):
pull_affordance = "pull"
affordance_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == "left" else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM
pull_displacement = 0.3 # meters
# x:383 y:840, x:483 y:490
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.hand_side = self.hand_side
_state_machine.userdata.none = None
_state_machine.userdata.step_back_distance = 1.0 # meters
_state_machine.userdata.grasp_preference = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
self._pull_displacement = pull_displacement
# [/MANUAL_CREATE]
# x:1033 y:40, x:333 y:90, x:1033 y:190
_sm_go_to_grasp_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'again'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference'])
with _sm_go_to_grasp_0:
# x:33 y:49
OperatableStateMachine.add('Get_Grasp_Info',
GetTemplateGraspState(),
transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'Inform_Grasp_Failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.Low},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'grasp': 'grasp_pose'})
# x:40 y:293
OperatableStateMachine.add('Convert_Waypoints',
CalculationState(calculation=lambda msg: [msg.pose]),
transitions={'done': 'Plan_To_Grasp'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints'})
# x:242 y:292
OperatableStateMachine.add('Plan_To_Grasp',
PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'planned': 'Move_To_Grasp_Pose', 'incomplete': 'Move_To_Grasp_Pose', 'failed': 'Decide_Which_Grasp'},
autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High},
remapping={'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:494 y:175
OperatableStateMachine.add('Move_To_Grasp_Pose',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'Optional_Template_Adjustment', 'failed': 'Decide_Which_Grasp'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:226 y:177
OperatableStateMachine.add('Inform_Grasp_Failed',
LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:970 y:294
OperatableStateMachine.add('Increase_Preference_Index',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'again'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'})
# x:41 y:178
OperatableStateMachine.add('Extract_Frame_Id',
CalculationState(calculation=lambda pose: pose.header.frame_id),
transitions={'done': 'Convert_Waypoints'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_frame_id'})
# x:712 y:78
OperatableStateMachine.add('Optional_Template_Adjustment',
OperatorDecisionState(outcomes=["grasp", "pregrasp", "skip"], hint="Consider adjusting the template's pose", suggestion="skip"),
transitions={'grasp': 'Get_Grasp_Info', 'pregrasp': 'again', 'skip': 'finished'},
autonomy={'grasp': Autonomy.Full, 'pregrasp': Autonomy.Full, 'skip': Autonomy.High})
# x:754 y:294
OperatableStateMachine.add('Decide_Which_Grasp',
OperatorDecisionState(outcomes=["same", "next"], hint='Try the same grasp or the next one?', suggestion='same'),
transitions={'same': 'Optional_Template_Adjustment', 'next': 'Increase_Preference_Index'},
autonomy={'same': Autonomy.High, 'next': Autonomy.High})
# x:133 y:390, x:433 y:190, x:983 y:140
_sm_perform_grasp_1 = OperatableStateMachine(outcomes=['finished', 'failed', 'next'], input_keys=['hand_side', 'grasp_preference', 'template_id', 'pregrasp_pose'], output_keys=['grasp_preference'])
with _sm_perform_grasp_1:
# x:68 y:76
OperatableStateMachine.add('Get_Finger_Configuration',
GetTemplateFingerConfigState(),
transitions={'done': 'Close_Fingers', 'failed': 'failed', 'not_available': 'Inform_Closing_Failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'finger_config': 'finger_config'})
# x:293 y:328
OperatableStateMachine.add('Convert_Waypoints',
CalculationState(calculation=lambda msg: [msg.pose]),
transitions={'done': 'Plan_Back_To_Pregrasp'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_waypoints'})
# x:496 y:328
OperatableStateMachine.add('Plan_Back_To_Pregrasp',
PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'planned': 'Move_Back_To_Pregrasp_Pose', 'incomplete': 'Move_Back_To_Pregrasp_Pose', 'failed': 'failed'},
autonomy={'planned': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Low},
remapping={'waypoints': 'pregrasp_waypoints', 'hand': 'hand_side', 'frame_id': 'pregrasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:662 y:228
OperatableStateMachine.add('Move_Back_To_Pregrasp_Pose',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'Increase_Preference_Index', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:296 y:228
OperatableStateMachine.add('Extract_Frame_Id',
CalculationState(calculation=lambda pose: pose.header.frame_id),
transitions={'done': 'Convert_Waypoints'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_frame_id'})
# x:673 y:128
OperatableStateMachine.add('Increase_Preference_Index',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'next'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'})
# x:81 y:228
OperatableStateMachine.add('Close_Fingers',
HandTrajectoryState(hand_type=self.hand_type),
transitions={'done': 'finished', 'failed': 'Extract_Frame_Id'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'finger_trajectory': 'finger_config', 'hand_side': 'hand_side'})
# x:490 y:75
OperatableStateMachine.add('Inform_Closing_Failed',
LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:733 y:190, x:383 y:40
_sm_go_to_pregrasp_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference', 'pregrasp_pose'])
with _sm_go_to_pregrasp_2:
# x:27 y:68
OperatableStateMachine.add('Get_Pregrasp_Info',
GetTemplatePregraspState(),
transitions={'done': 'Plan_To_Pregrasp_Pose', 'failed': 'failed', 'not_available': 'Inform_Pregrasp_Failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pregrasp_pose'})
# x:269 y:153
OperatableStateMachine.add('Inform_Pregrasp_Failed',
LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Low})
# x:537 y:228
OperatableStateMachine.add('Move_To_Pregrasp_Pose',
ExecuteTrajectoryMsgState(controller=arm_controller),
transitions={'done': 'finished', 'failed': 'Decide_Which_Pregrasp'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.High},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:25 y:328
OperatableStateMachine.add('Increase_Preference_Index',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Get_Pregrasp_Info'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'})
# x:266 y:228
OperatableStateMachine.add('Plan_To_Pregrasp_Pose',
PlanEndeffectorPoseState(ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"),
transitions={'planned': 'Move_To_Pregrasp_Pose', 'failed': 'Decide_Which_Pregrasp'},
autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High},
remapping={'target_pose': 'pregrasp_pose', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory'})
# x:266 y:327
OperatableStateMachine.add('Decide_Which_Pregrasp',
OperatorDecisionState(outcomes=["same", "next"], hint='Try the same pregrasp or the next one?', suggestion='same'),
transitions={'same': 'Get_Pregrasp_Info', 'next': 'Increase_Preference_Index'},
autonomy={'same': Autonomy.High, 'next': Autonomy.High})
# x:30 y:444, x:162 y:478, x:230 y:478
_sm_planning_pipeline_3 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['stand_pose'], output_keys=['plan_header'])
with _sm_planning_pipeline_3:
# x:34 y:57
OperatableStateMachine.add('Create_Step_Goal',
CreateStepGoalState(pose_is_pelvis=True),
transitions={'done': 'Plan_To_Waypoint', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'target_pose': 'stand_pose', 'step_goal': 'step_goal'})
# x:553 y:481
OperatableStateMachine.add('Modify_Plan',
InputState(request=InputState.FOOTSTEP_PLAN_HEADER, message='Modify plan, VALIDATE, and confirm.'),
transitions={'received': 'finished', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'plan_header'})
# x:34 y:484
OperatableStateMachine.add('Plan_To_Waypoint',
PlanFootstepsState(mode=self.parameter_set),
transitions={'planned': 'Modify_Plan', 'failed': 'Decide_Replan_without_Collision'},
autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'})
# x:139 y:314
OperatableStateMachine.add('Decide_Replan_without_Collision',
OperatorDecisionState(outcomes=['replan', 'fail'], hint='Try replanning without collision avoidance.', suggestion='replan'),
transitions={'replan': 'Replan_without_Collision', 'fail': 'failed'},
autonomy={'replan': Autonomy.Low, 'fail': Autonomy.Full})
# x:319 y:406
OperatableStateMachine.add('Replan_without_Collision',
PlanFootstepsState(mode='drc_step_no_collision'),
transitions={'planned': 'Modify_Plan', 'failed': 'failed'},
autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'})
# x:1103 y:424, x:130 y:478
_sm_grasp_trigger_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'grasp_preference'])
with _sm_grasp_trigger_4:
# x:86 y:72
OperatableStateMachine.add('Go_to_Pregrasp',
_sm_go_to_pregrasp_2,
transitions={'finished': 'Open_Fingers', 'failed': 'Grasp_Manually'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'pregrasp_pose': 'pregrasp_pose'})
# x:789 y:172
OperatableStateMachine.add('Perform_Grasp',
_sm_perform_grasp_1,
transitions={'finished': 'finished', 'failed': 'Grasp_Manually', 'next': 'Close_Fingers'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'next': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'pregrasp_pose': 'pregrasp_pose'})
# x:332 y:178
OperatableStateMachine.add('Open_Fingers',
FingerConfigurationState(hand_type=self.hand_type, configuration=0.0),
transitions={'done': 'Go_to_Grasp', 'failed': 'Grasp_Manually'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.High},
remapping={'hand_side': 'hand_side'})
# x:332 y:28
OperatableStateMachine.add('Close_Fingers',
FingerConfigurationState(hand_type=self.hand_type, configuration=1.0),
transitions={'done': 'Go_to_Pregrasp', 'failed': 'Grasp_Manually'},
autonomy={'done': Autonomy.High, 'failed': Autonomy.High},
remapping={'hand_side': 'hand_side'})
# x:324 y:428
OperatableStateMachine.add('Grasp_Manually',
OperatorDecisionState(outcomes=["fingers_closed", "abort"], hint="Grasp the object manually, continue when fingers are closed.", suggestion=None),
transitions={'fingers_closed': 'finished', 'abort': 'failed'},
autonomy={'fingers_closed': Autonomy.Full, 'abort': Autonomy.Full})
# x:543 y:172
OperatableStateMachine.add('Go_to_Grasp',
_sm_go_to_grasp_0,
transitions={'finished': 'Perform_Grasp', 'failed': 'Grasp_Manually', 'again': 'Close_Fingers'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'again': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id'})
# x:30 y:478, x:130 y:478, x:230 y:478
_sm_walk_to_template_5 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['template_id', 'grasp_preference', 'hand_side'])
with _sm_walk_to_template_5:
# x:265 y:28
OperatableStateMachine.add('Decide_Request_Template',
DecisionState(outcomes=['request', 'continue'], conditions=lambda x: 'continue' if x is not None else 'request'),
transitions={'request': 'Request_Template', 'continue': 'Get_Stand_Pose'},
autonomy={'request': Autonomy.Low, 'continue': Autonomy.Off},
remapping={'input_value': 'template_id'})
# x:1033 y:106
OperatableStateMachine.add('Increment_Stand_Pose',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Inform_About_Retry'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'})
# x:1162 y:29
OperatableStateMachine.add('Inform_About_Retry',
LogState(text="Stand pose choice failed. Trying again.", severity=Logger.REPORT_INFO),
transitions={'done': 'Get_Stand_Pose'},
autonomy={'done': Autonomy.Off})
# x:567 y:118
OperatableStateMachine.add('Inform_About_Fail',
LogState(text="Unable to find a suitable stand pose for the template.", severity=Logger.REPORT_WARN),
transitions={'done': 'Decide_Repeat_Request'},
autonomy={'done': Autonomy.Off})
# x:554 y:274
OperatableStateMachine.add('Get_Goal_from_Operator',
InputState(request=InputState.WAYPOINT_GOAL_POSE, message="Provide a waypoint in front of the template."),
transitions={'received': 'Walk_To_Waypoint', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'plan_header'})
# x:279 y:110
OperatableStateMachine.add('Request_Template',
InputState(request=InputState.SELECTED_OBJECT_ID, message="Specify target template"),
transitions={'received': 'Get_Stand_Pose', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Off, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'template_id'})
# x:825 y:461
OperatableStateMachine.add('Wait_For_Stand',
CheckCurrentControlModeState(target_mode=CheckCurrentControlModeState.STAND, wait=True),
transitions={'correct': 'finished', 'incorrect': 'failed'},
autonomy={'correct': Autonomy.Low, 'incorrect': Autonomy.Full},
remapping={'control_mode': 'control_mode'})
# x:1143 y:277
OperatableStateMachine.add('Decide_Stand_Preference',
OperatorDecisionState(outcomes=["same", "next", "abort"], hint="Same or next stand pose?", suggestion="next"),
transitions={'same': 'Inform_About_Retry', 'next': 'Increment_Stand_Pose', 'abort': 'aborted'},
autonomy={'same': Autonomy.Full, 'next': Autonomy.Full, 'abort': Autonomy.Full})
# x:842 y:152
OperatableStateMachine.add('Planning_Pipeline',
_sm_planning_pipeline_3,
transitions={'finished': 'Walk_To_Waypoint', 'failed': 'Decide_Stand_Preference', 'aborted': 'Decide_Stand_Preference'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit},
remapping={'stand_pose': 'stand_pose', 'plan_header': 'plan_header'})
# x:833 y:276
OperatableStateMachine.add('Walk_To_Waypoint',
ExecuteStepPlanActionState(),
transitions={'finished': 'Wait_For_Stand', 'failed': 'Decide_Stand_Preference'},
autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Full},
remapping={'plan_header': 'plan_header'})
# x:554 y:195
OperatableStateMachine.add('Decide_Repeat_Request',
OperatorDecisionState(outcomes=['repeat_id', 'request_goal'], hint=None, suggestion=None),
transitions={'repeat_id': 'Request_Template', 'request_goal': 'Get_Goal_from_Operator'},
autonomy={'repeat_id': Autonomy.Low, 'request_goal': Autonomy.High})
# x:547 y:27
OperatableStateMachine.add('Get_Stand_Pose',
GetTemplateStandPoseState(),
transitions={'done': 'Planning_Pipeline', 'failed': 'Inform_About_Fail', 'not_available': 'Inform_About_Fail'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.High},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'stand_pose': 'stand_pose'})
# x:133 y:340, x:383 y:140
_sm_perform_step_back_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['step_back_distance'])
with _sm_perform_step_back_6:
# x:78 y:78
OperatableStateMachine.add('Plan_Steps_Back',
FootstepPlanRelativeState(direction=FootstepPlanRelativeState.DIRECTION_BACKWARD),
transitions={'planned': 'Do_Steps_Back', 'failed': 'failed'},
autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full},
remapping={'distance': 'step_back_distance', 'plan_header': 'plan_header'})
# x:74 y:228
OperatableStateMachine.add('Do_Steps_Back',
ExecuteStepPlanActionState(),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'plan_header': 'plan_header'})
# x:133 y:340, x:333 y:90
_sm_release_trigger_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'none'])
with _sm_release_trigger_7:
# x:82 y:78
OperatableStateMachine.add('Open_Fingers',
FingerConfigurationState(hand_type=self.hand_type, configuration=0),
transitions={'done': 'Take_Hand_Back', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'hand_side': 'hand_side'})
# x:96 y:178
OperatableStateMachine.add('Take_Hand_Back',
LogState(text="Take hand slightly back", severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Full})
# x:733 y:240, x:33 y:289
_sm_pull_trigger_8 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none'])
with _sm_pull_trigger_8:
# x:202 y:28
OperatableStateMachine.add('Ready_To_Pull',
LogState(text="Ready to pull the trigger down", severity=Logger.REPORT_INFO),
transitions={'done': 'Get_Pull_Affordance'},
autonomy={'done': Autonomy.High})
# x:192 y:328
OperatableStateMachine.add('Plan_Pull',
PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"),
transitions={'done': 'Execute_Pull', 'incomplete': 'Execute_Pull', 'failed': 'failed'},
autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full},
remapping={'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'})
# x:176 y:428
OperatableStateMachine.add('Execute_Pull',
ExecuteTrajectoryMsgState(controller=affordance_controller),
transitions={'done': 'Decide_Repeat_Pull', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'joint_trajectory': 'joint_trajectory'})
# x:183 y:228
OperatableStateMachine.add('Scale_Pull_Affordance',
CalculationState(calculation=lambda x: x),
transitions={'done': 'Plan_Pull'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'affordance', 'output_value': 'affordance'})
# x:173 y:128
OperatableStateMachine.add('Get_Pull_Affordance',
GetTemplateAffordanceState(identifier=pull_affordance),
transitions={'done': 'Scale_Pull_Affordance', 'failed': 'failed', 'not_available': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'affordance'})
# x:437 y:228
OperatableStateMachine.add('Decide_Repeat_Pull',
OperatorDecisionState(outcomes=['done', 'repeat'], hint="Pull further?", suggestion='done'),
transitions={'done': 'finished', 'repeat': 'Get_Pull_Affordance'},
autonomy={'done': Autonomy.High, 'repeat': Autonomy.Full})
with _state_machine:
# x:73 y:78
OperatableStateMachine.add('Request_Trigger_Template',
InputState(request=InputState.SELECTED_OBJECT_ID, message="Place trigger template"),
transitions={'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'},
autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full},
remapping={'data': 'template_id'})
# x:337 y:78
OperatableStateMachine.add('Decide_Walking',
OperatorDecisionState(outcomes=["walk", "stand"], hint="Walk to template?", suggestion="walk"),
transitions={'walk': 'Walk_To_Template', 'stand': 'Set_Manipulate'},
autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full})
# x:844 y:322
OperatableStateMachine.add('Pull_Trigger',
_sm_pull_trigger_8,
transitions={'finished': 'Release_Trigger', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none'})
# x:836 y:422
OperatableStateMachine.add('Release_Trigger',
_sm_release_trigger_7,
transitions={'finished': 'Warn_Stand', 'failed': 'Warn_Stand'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'none': 'none'})
# x:826 y:678
OperatableStateMachine.add('Go_To_Stand_Pose',
MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False),
transitions={'done': 'Set_Stand', 'failed': 'failed'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full},
remapping={'side': 'none'})
# x:566 y:78
OperatableStateMachine.add('Set_Manipulate',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={'changed': 'Set_Template_Frame', 'failed': 'failed'},
autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full})
# x:858 y:578
OperatableStateMachine.add('Warn_Stand',
LogState(text="Will go to stand now", severity=Logger.REPORT_INFO),
transitions={'done': 'Go_To_Stand_Pose'},
autonomy={'done': Autonomy.High})
# x:566 y:678
OperatableStateMachine.add('Set_Stand',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND),
transitions={'changed': 'Decide_Step_Back', 'failed': 'failed'},
autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full})
# x:337 y:678
OperatableStateMachine.add('Decide_Step_Back',
OperatorDecisionState(outcomes=["walk", "stand"], hint="Step back?", suggestion="walk"),
transitions={'walk': 'Perform_Step_Back', 'stand': 'finished'},
autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full})
# x:77 y:672
OperatableStateMachine.add('Perform_Step_Back',
_sm_perform_step_back_6,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'step_back_distance': 'step_back_distance'})
# x:330 y:172
OperatableStateMachine.add('Walk_To_Template',
_sm_walk_to_template_5,
transitions={'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit},
remapping={'template_id': 'template_id', 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side'})
# x:841 y:222
OperatableStateMachine.add('Grasp_Trigger',
_sm_grasp_trigger_4,
transitions={'finished': 'Pull_Trigger', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'grasp_preference': 'grasp_preference'})
# x:846 y:128
OperatableStateMachine.add('Look_At_Trigger',
LookAtTargetState(),
transitions={'done': 'Grasp_Trigger'},
autonomy={'done': Autonomy.Off},
remapping={'frame': 'template_frame'})
# x:837 y:28
OperatableStateMachine.add('Set_Template_Frame',
CalculationState(calculation=lambda x: "template_tf_%d" % x),
transitions={'done': 'Look_At_Trigger'},
autonomy={'done': Autonomy.Low},
remapping={'input_value': 'template_id', 'output_value': 'template_frame'})
return _state_machine
def create(self):
# x:887 y:426, x:1209 y:79, x:1066 y:156
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'critical_fail'],
input_keys=['Action'])
_state_machine.userdata.Action = ["Move", "dining table"]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:83
OperatableStateMachine.add(
'lower',
CalculationState(calculation=lambda x: x[0].lower()),
transitions={'done': 'decide Action'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'Action',
'output_value': 'name'
})
# x:479 y:59
OperatableStateMachine.add('ActionWrapper_Ask',
self.use_behavior(
ActionWrapper_AskSM,
'ActionWrapper_Ask'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:466 y:119
OperatableStateMachine.add('ActionWrapper_Count',
self.use_behavior(
ActionWrapper_CountSM,
'ActionWrapper_Count'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:475 y:236
OperatableStateMachine.add('ActionWrapper_Find',
self.use_behavior(
ActionWrapper_FindSM,
'ActionWrapper_Find'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:267 y:127
OperatableStateMachine.add('ActionWrapper_Find_Person',
self.use_behavior(
ActionWrapper_Find_PersonSM,
'ActionWrapper_Find_Person'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:464 y:296
OperatableStateMachine.add('ActionWrapper_Follow',
self.use_behavior(
ActionWrapper_FollowSM,
'ActionWrapper_Follow'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:471 y:364
OperatableStateMachine.add('ActionWrapper_Give',
self.use_behavior(
ActionWrapper_GiveSM,
'ActionWrapper_Give'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:467 y:432
OperatableStateMachine.add('ActionWrapper_Move',
self.use_behavior(
ActionWrapper_MoveSM,
'ActionWrapper_Move'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:465 y:530
OperatableStateMachine.add('ActionWrapper_Place',
self.use_behavior(
ActionWrapper_PlaceSM,
'ActionWrapper_Place'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:476 y:589
OperatableStateMachine.add('ActionWrapper_Say',
self.use_behavior(
ActionWrapper_SaySM,
'ActionWrapper_Say'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:304 y:51
OperatableStateMachine.add('ActionWrapper_Answer',
self.use_behavior(
ActionWrapper_AnswerSM,
'ActionWrapper_Answer'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
})
# x:632 y:432
OperatableStateMachine.add('ActionWrapper_Pick',
self.use_behavior(
ActionWrapper_PickSM,
'ActionWrapper_Pick'),
transitions={
'finished': 'finished',
'failed': 'failed',
'critical_fail': 'critical_fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'critical_fail': Autonomy.Inherit
},
remapping={'Action': 'Action'})
# x:42 y:269
OperatableStateMachine.add('decide Action',
DecisionState(outcomes=[
"answer", "ask", "count", "find",
"findperson", "follow", "give",
"move", "pick", "place", "say"
],
conditions=lambda x: x),
transitions={
'answer': 'ActionWrapper_Answer',
'ask': 'ActionWrapper_Ask',
'count': 'ActionWrapper_Count',
'find': 'ActionWrapper_Find',
'findperson':
'ActionWrapper_Find_Person',
'follow': 'ActionWrapper_Follow',
'give': 'ActionWrapper_Give',
'move': 'ActionWrapper_Move',
'pick': 'ActionWrapper_Pick',
'place': 'ActionWrapper_Place',
'say': 'ActionWrapper_Say'
},
autonomy={
'answer': Autonomy.Off,
'ask': Autonomy.Off,
'count': Autonomy.Off,
'find': Autonomy.Off,
'findperson': Autonomy.Off,
'follow': Autonomy.Off,
'give': Autonomy.Off,
'move': Autonomy.Off,
'pick': Autonomy.Off,
'place': Autonomy.Off,
'say': Autonomy.Off
},
remapping={'input_value': 'name'})
return _state_machine
