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