def create(self): concurrent_states = dict() concurrent_mapping = list() welcome_log = "Performing Tests..." # x:219 y:439, x:562 y:308 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.input_value = "bla" _state_machine.userdata.blubb = "lalala" _state_machine.userdata.embedded_input = 4 _state_machine.userdata.behavior_my_input = 8 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] concurrent_states['wait_short'] = WaitState(wait_time=2) concurrent_states['wait_long'] = WaitState(wait_time=4) #concurrent_states['calc_one'] = CalculationState(lambda x: "bla1") #concurrent_states['calc_two'] = FlexibleCalculationState(lambda x: "bla2", ["input1", "input2"]) concurrent_states['calc_print'] = CalculationState(self.print_input) #concurrent_states['behavior'] = FlexBEInnerTestBehaviorSM() #concurrent_states['behavior'].message = "Hello World!" concurrent_mapping.append({ 'outcome': 'done', 'condition': { 'wait_short': 'done', 'wait_long': 'done' } }) # [/MANUAL_CREATE] # x:88 y:457, x:340 y:145 _sm_embedded_statemachine_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['inner_value', 'embedded_input']) with _sm_embedded_statemachine_0: # x:30 y:40 OperatableStateMachine.add('Inner_Wait', WaitState(wait_time=self.wait_time), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.Low}) # x:37 y:165 OperatableStateMachine.add( 'Decision', OperatorDecisionState(outcomes=["finished", "failed"], hint="Choose one outcome.", suggestion="finished"), transitions={ 'finished': 'FlexBE Inner Test Behavior', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.High, 'failed': Autonomy.Low }) # x:41 y:308 OperatableStateMachine.add('Second_Inner_wait', WaitState(wait_time=self.wait_time), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Low}) # x:317 y:323 OperatableStateMachine.add( 'Inner_Calc', CalculationState(calculation=self.inner_calc), transitions={'done': 'Second_Inner_wait'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'inner_value', 'output_value': 'output_value' }) # x:258 y:229 OperatableStateMachine.add( 'FlexBE Inner Test Behavior', self.use_behavior( FlexBEInnerTestBehaviorSM, 'Embedded_Statemachine/FlexBE Inner Test Behavior'), transitions={ 'finished': 'FlexBE Inner Test Behavior 2', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'my_input': 'embedded_input'}) # x:502 y:217 OperatableStateMachine.add( 'FlexBE Inner Test Behavior 2', self.use_behavior( FlexBEInnerTestBehaviorSM, 'Embedded_Statemachine/FlexBE Inner Test Behavior 2'), transitions={ 'finished': 'Inner_Calc', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'my_input': 'embedded_input'}) with _state_machine: # x:30 y:40 OperatableStateMachine.add('Initial_Wait', WaitState(wait_time=self.wait_time), transitions={'done': 'TestInput'}, autonomy={'done': Autonomy.High}) # x:202 y:40 OperatableStateMachine.add('Welcome_Log', LogState(text=welcome_log, severity=0), transitions={'done': 'Second_Wait'}, autonomy={'done': Autonomy.Low}) # x:646 y:39 OperatableStateMachine.add('Second_Wait', WaitState(wait_time=self.wait_time), transitions={'done': 'Any_Calculation'}, autonomy={'done': Autonomy.Low}) # x:364 y:184 OperatableStateMachine.add('Embedded_Statemachine', _sm_embedded_statemachine_0, transitions={ 'finished': 'Final_Wait', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'inner_value': 'output_value', 'embedded_input': 'embedded_input' }) # x:201 y:188 OperatableStateMachine.add('Final_Wait', WaitState(wait_time=self.wait_time), transitions={'done': 'Decide_Back'}, autonomy={'done': Autonomy.High}) # x:183 y:316 OperatableStateMachine.add('Decide_Back', OperatorDecisionState( outcomes=["back", "done"], hint="Repeat inner state machine?", suggestion="back"), transitions={ 'back': 'Log_Go_Back', 'done': 'finished' }, autonomy={ 'back': Autonomy.High, 'done': Autonomy.Low }) # x:639 y:190 OperatableStateMachine.add( 'Any_Calculation', CalculationState(calculation=self.any_calculation), transitions={'done': 'Embedded_Statemachine'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'input_value', 'output_value': 'output_value' }) # x:33 y:147 OperatableStateMachine.add('TestInput', InputState(request=0, message="I need some data!"), transitions={ 'received': 'Welcome_Log', 'aborted': 'Test_Concurrency', 'no_connection': 'Final_Wait', 'data_error': 'Decide_Back' }, autonomy={ 'received': Autonomy.Full, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full }, remapping={'data': 'output_value'}) # x:288 y:100 {?input_keys = ['calc_print_input_value', 'behavior_my_input'],?output_keys = ['calc_print_output_value']} OperatableStateMachine.add('Test_Concurrency', ConcurrentState( states=concurrent_states, outcomes=['done'], outcome_mapping=concurrent_mapping), transitions={'done': 'Test_Output'}, autonomy={'done': Autonomy.Off}, remapping={ 'calc_print_input_value': 'input_value', 'behavior_my_input': 'behavior_my_input', 'calc_print_output_value': 'concurent_output' }) # x:491 y:99 OperatableStateMachine.add( 'Test_Output', CalculationState(calculation=self.print_input), transitions={'done': 'Second_Wait'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'concurent_output', 'output_value': 'output_value' }) # x:397 y:316 OperatableStateMachine.add( 'Log_Go_Back', LogState(text="Going back!", severity=Logger.REPORT_INFO), transitions={'done': 'Embedded_Statemachine'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:787 y:587, x:280 y:361 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.none = None _state_machine.userdata.grasp_preference = 0 _state_machine.userdata.step_back_distance = 0.5 # m # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:686 y:240, x:384 y:196 _sm_perform_walking_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['none', 'step_back_distance']) with _sm_perform_walking_0: # x:64 y:78 OperatableStateMachine.add( 'Plan_Realign_Feet', FootstepPlanRealignCenterState(), transitions={ 'planned': 'Execute_Realign_Feet', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Low }, remapping={'plan_header': 'plan_header'}) # x:624 y:378 OperatableStateMachine.add( 'Perform_Step_Back', ExecuteStepPlanActionState(), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'plan_header': 'plan_header'}) # x:328 y:378 OperatableStateMachine.add( 'Plan_Step_Back', FootstepPlanRelativeState( direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={ 'planned': 'Perform_Step_Back', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Low }, remapping={ 'distance': 'step_back_distance', 'plan_header': 'plan_header' }) # x:74 y:190 OperatableStateMachine.add( 'Execute_Realign_Feet', ExecuteStepPlanActionState(), transitions={ 'finished': 'Wait_For_Stand', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'plan_header': 'plan_header'}) # x:66 y:316 OperatableStateMachine.add( 'Wait_For_Stand', CheckCurrentControlModeState( target_mode=CheckCurrentControlModeState.STAND, wait=True), transitions={ 'correct': 'Plan_Step_Back', 'incorrect': 'failed' }, autonomy={ 'correct': Autonomy.Low, 'incorrect': Autonomy.Full }, remapping={'control_mode': 'control_mode'}) # x:733 y:290, x:133 y:290 _sm_back_to_stand_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['none']) with _sm_back_to_stand_1: # x:66 y:78 OperatableStateMachine.add( 'Set_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'Stand_Posture', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.High, 'failed': Autonomy.Low }) # x:376 y:78 OperatableStateMachine.add( 'Stand_Posture', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'Set_Stand', 'failed': 'Set_Stand' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'side': 'none'}) # x:666 y:78 OperatableStateMachine.add( 'Set_Stand', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'finished', 'failed': 'finished' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Low }) # x:120 y:404, x:298 y:222 _sm_step_back_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'hand_side', 'none', 'template_id', 'grasp_preference', 'step_back_distance' ]) with _sm_step_back_2: # x:76 y:28 OperatableStateMachine.add( 'Go_To_Stand_Posture', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'Set_To_Stand_Manipulate', 'failed': 'Set_To_Stand_Manipulate' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'side': 'none'}) # x:66 y:140 OperatableStateMachine.add( 'Set_To_Stand_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND_MANIPULATE), transitions={ 'changed': 'Perform_Walking', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.High, 'failed': Autonomy.Low }) # x:84 y:272 OperatableStateMachine.add('Perform_Walking', _sm_perform_walking_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'none': 'none', 'step_back_distance': 'step_back_distance' }) # x:755 y:48, x:401 y:428 _sm_preparation_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['none'], output_keys=['template_id']) with _sm_preparation_3: # x:66 y:164 OperatableStateMachine.add( 'Head_Look_Straight', TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT), transitions={ 'done': 'Place_Template', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }) # x:566 y:163 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.High, 'aborted': Autonomy.High, 'no_connection': Autonomy.Low, 'data_error': Autonomy.Low }, remapping={'data': 'template_id'}) # x:287 y:204 OperatableStateMachine.add('Log_No_Connection', LogState( text="Have no connection to OCS!", severity=Logger.REPORT_ERROR), transitions={'done': 'Decide_Input'}, autonomy={'done': Autonomy.Off}) # x:344 y:144 OperatableStateMachine.add('Log_Data_Error', LogState( text="Received wrong data format!", severity=Logger.REPORT_ERROR), transitions={'done': 'Decide_Input'}, autonomy={'done': Autonomy.Off}) # x:483 y:44 OperatableStateMachine.add( 'Fake_Input', CalculationState(calculation=lambda x: 0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'none', 'output_value': 'template_id' }) # x:236 y:40 OperatableStateMachine.add( 'Decide_Input', OperatorDecisionState( outcomes=['fake_id', 'ocs_request'], hint="How do you want to provide the template?", suggestion=None), transitions={ 'fake_id': 'Fake_Input', 'ocs_request': 'Request_Template_ID' }, autonomy={ 'fake_id': Autonomy.Full, 'ocs_request': Autonomy.Full }) # x:78 y:40 OperatableStateMachine.add( 'Place_Template', LogState(text="Please place the drill template.", severity=Logger.REPORT_HINT), transitions={'done': 'Decide_Input'}, autonomy={'done': Autonomy.Full}) with _state_machine: # x:44 y:72 OperatableStateMachine.add('Preparation', _sm_preparation_3, transitions={ 'finished': 'Ask_Perform_Walking', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'none': 'none', 'template_id': 'template_id' }) # x:547 y:571 OperatableStateMachine.add('Step_Back', _sm_step_back_2, transitions={ 'finished': 'finished', 'failed': 'Back_To_Stand' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'none': 'none', 'template_id': 'template_id', 'grasp_preference': 'grasp_preference', 'step_back_distance': 'step_back_distance' }) # x:284 y:78 OperatableStateMachine.add( 'Ask_Perform_Walking', OperatorDecisionState( outcomes=['walk', 'stand'], hint="Does the robot need to walk to the table?", suggestion='walk'), transitions={ 'walk': 'Walk to Template', 'stand': 'Manipulation Config' }, autonomy={ 'walk': Autonomy.High, 'stand': Autonomy.Full }) # x:531 y:22 OperatableStateMachine.add( 'Walk to Template', self.use_behavior(WalktoTemplateSM, 'Walk to Template'), transitions={ 'finished': 'Manipulation Config', 'failed': 'Walk_Manually', 'aborted': 'Walk_Manually' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit }, remapping={ 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:525 y:143 OperatableStateMachine.add( 'Manipulation Config', self.use_behavior(ManipulationConfigSM, 'Manipulation Config'), transitions={ 'finished': 'Head_Look_Down', 'failed': 'Manipulation_Config_Manually' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:264 y:228 OperatableStateMachine.add( 'Manipulation_Config_Manually', OperatorDecisionState( outcomes=["done", "abort"], hint="Make sure the robot is ready to grasp", suggestion=None), transitions={ 'done': 'Pickup Object', 'abort': 'failed' }, autonomy={ 'done': Autonomy.Full, 'abort': Autonomy.Full }) # x:769 y:78 OperatableStateMachine.add( 'Walk_Manually', LogState(text="Guide the robot to the template manually.", severity=Logger.REPORT_HINT), transitions={'done': 'Manipulation Config'}, autonomy={'done': Autonomy.Full}) # x:245 y:485 OperatableStateMachine.add('Back_To_Stand', _sm_back_to_stand_1, transitions={ 'finished': 'failed', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'none': 'none'}) # x:538 y:353 OperatableStateMachine.add('Pickup Object', self.use_behavior( PickupObjectSM, 'Pickup Object'), transitions={ 'finished': 'Head_Look_Straight', 'failed': 'Back_To_Stand' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:545 y:257 OperatableStateMachine.add( 'Head_Look_Down', TiltHeadState(desired_tilt=TiltHeadState.DOWN_45), transitions={ 'done': 'Pickup Object', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }) # x:540 y:473 OperatableStateMachine.add( 'Head_Look_Straight', TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT), transitions={ 'done': 'Step_Back', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }) 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): plug_in_affordance = "plug_in" plug_out_affordance = "plug_out" arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM # x:202 y:568, x:374 y:401 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.none = None _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.grasp_preference = 0 _state_machine.userdata.step_back_distance = 1.0 # meters # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:588 y:126, x:324 y:44 _sm_back_to_pregrasp_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'grasp_preference']) with _sm_back_to_pregrasp_0: # x:30 y:40 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': 'grasp_preference', 'pre_grasp': 'grasp_pose' }) # x:242 y:292 OperatableStateMachine.add( 'Plan_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': 'Get_Pregrasp' }, autonomy={ 'planned': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.Full }, remapping={ 'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction' }) # x:568 y:295 OperatableStateMachine.add( 'Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'finished', 'failed': 'Get_Pregrasp' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) # 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:40 y:293 OperatableStateMachine.add( 'Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_To_Pregrasp'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints' }) # x:598 y:30, x:220 y:148, x:1035 y:174 _sm_go_to_grasp_1 = OperatableStateMachine( outcomes=['finished', 'failed', 'again'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference']) with _sm_go_to_grasp_1: # x:33 y:49 OperatableStateMachine.add('Get_Grasp_Info', 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': '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.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full }, 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.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) # 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:727 y:50 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:596 y:113, x:351 y:62 _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': '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': 'pregrasp_pose' }) # x:537 y:228 OperatableStateMachine.add( 'Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'finished', 'failed': 'Decide_Which_Pregrasp' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, 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.Full }, 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.Full }) # x:30 y:365, x:130 y:365 _sm_perform_step_back_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['step_back_distance']) with _sm_perform_step_back_3: # 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:550 y:574, x:130 y:365 _sm_plug_in_cable_4 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'target_template_id', 'hand_side', 'grasp_preference', 'template_id', 'template_pose' ]) with _sm_plug_in_cable_4: # x:82 y:59 OperatableStateMachine.add('Go_to_Pregrasp', _sm_go_to_pregrasp_2, transitions={ 'finished': 'Go_to_Grasp', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'target_template_id', 'pregrasp_pose': 'pregrasp_pose' }) # x:322 y:171 OperatableStateMachine.add('Go_to_Grasp', _sm_go_to_grasp_1, transitions={ 'finished': 'Detach_Cable', 'failed': 'failed', 'again': 'Go_to_Pregrasp' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'again': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'target_template_id' }) # x:516 y:306 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState( hand_type=self.hand_type, configuration=0), transitions={ 'done': 'Back_To_Pregrasp', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'hand_side': 'hand_side'}) # x:472 y:406 OperatableStateMachine.add('Back_To_Pregrasp', _sm_back_to_pregrasp_0, transitions={ 'finished': 'Close_Fingers', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference' }) # x:314 y:524 OperatableStateMachine.add('Close_Fingers', FingerConfigurationState( hand_type=self.hand_type, configuration=1), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'hand_side': 'hand_side'}) # x:527 y:204 OperatableStateMachine.add('Detach_Cable', DetachObjectState(), transitions={ 'done': 'Open_Fingers', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'template_pose': 'template_pose' }) # x:30 y:365, x:115 y:197 _sm_take_cable_5 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_take_cable_5: # x:81 y:54 OperatableStateMachine.add( 'Get_Plug_Out_Affordance', GetTemplateAffordanceState(identifier=plug_out_affordance), transitions={ 'done': 'Plan_Plug_Out', '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:348 y:73 OperatableStateMachine.add( 'Plan_Plug_Out', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={ 'done': 'Execute_Plug_Out', 'incomplete': 'Execute_Plug_Out', '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:322 y:255 OperatableStateMachine.add( 'Execute_Plug_Out', ExecuteTrajectoryMsgState(controller=affordance_controller), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) with _state_machine: # x:73 y:78 OperatableStateMachine.add( 'Request_Cable_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place cable 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:330 y:172 OperatableStateMachine.add( 'Walk_To_Template', self.use_behavior(WalktoTemplateSM, 'Walk_To_Template'), transitions={ 'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit }, remapping={ 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': '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:839 y:72 OperatableStateMachine.add('Grasp Object', self.use_behavior( GraspObjectSM, 'Grasp Object'), transitions={ 'finished': 'Attach_Cable', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:843 y:302 OperatableStateMachine.add('Take_Cable', _sm_take_cable_5, transitions={ 'finished': 'Request_Target_Template', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none' }) # x:566 y:78 OperatableStateMachine.add( 'Set_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'Grasp Object', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Full }) # x:822 y:545 OperatableStateMachine.add('Plug_In_Cable', _sm_plug_in_cable_4, transitions={ 'finished': 'Warn_Stand', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'target_template_id': 'target_template_id', 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'template_pose': 'template_pose' }) # x:826 y:185 OperatableStateMachine.add('Attach_Cable', AttachObjectState(), transitions={ 'done': 'Take_Cable', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'template_pose': 'template_pose' }) # x:811 y:400 OperatableStateMachine.add( 'Request_Target_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place target template"), transitions={ 'received': 'Plug_In_Cable', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed' }, autonomy={ 'received': Autonomy.Low, 'aborted': Autonomy.High, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full }, remapping={'data': 'target_template_id'}) # x:753 y:646 OperatableStateMachine.add( 'Warn_Stand', LogState(text="Going to stand pose", severity=Logger.REPORT_INFO), transitions={'done': 'Go_To_Stand_Pose'}, autonomy={'done': Autonomy.High}) # x:581 y:573 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:446 y:653 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:100 y:661 OperatableStateMachine.add( 'Perform_Step_Back', _sm_perform_step_back_3, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'step_back_distance': 'step_back_distance'}) # x:383 y:527 OperatableStateMachine.add( 'Set_Stand', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'Decide_Step_Back', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Full }) return _state_machine
def create(self): # x:119 y:369, x:527 y:258 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['my_input']) _state_machine.userdata.my_input = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'Entry_Msg', LogState(text="Now at inner behavior...", severity=Logger.REPORT_INFO), transitions={'done': 'FlexBE Testprint Behavior'}, autonomy={'done': Autonomy.Off}) # x:34 y:240 OperatableStateMachine.add('Wait_A_Bit', WaitState(wait_time=2), transitions={'done': 'Print_State'}, autonomy={'done': Autonomy.High}) # x:279 y:327 OperatableStateMachine.add('Decide_Outcome', OperatorDecisionState( outcomes=['finished', 'failed'], hint="How should this end?", suggestion='finished'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Full, 'failed': Autonomy.Full }) # x:289 y:127 OperatableStateMachine.add( 'Print_State', CalculationState(calculation=self.print_number_and_inc), transitions={'done': 'Print_Param_Message'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'my_input', 'output_value': 'output_value' }) # x:273 y:212 OperatableStateMachine.add( 'Print_Param_Message', LogState(text="Wait, there is a message: " + self.message, severity=Logger.REPORT_HINT), transitions={'done': 'Decide_Outcome'}, autonomy={'done': Autonomy.Off}) # x:15 y:124 OperatableStateMachine.add('FlexBE Testprint Behavior', self.use_behavior( FlexBETestprintBehaviorSM, 'FlexBE Testprint Behavior'), transitions={'finished': 'Wait_A_Bit'}, autonomy={'finished': Autonomy.Inherit}) return _state_machine
def create(self): # x:1162 y:100, x:318 y:599 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:42 y:28 OperatableStateMachine.add('initialPub', KylePubState( cmd_topic='/makethisupcount', valueToPub=0), transitions={'done': 'GetVelocity'}, autonomy={'done': Autonomy.Off}) # x:721 y:137 OperatableStateMachine.add( 'move', KyleTwistState( cmd_topic='/turtlebot/stamped_cmd_vel_mux/input/navi'), transitions={ 'done': 'Should_Robot_Finish', 'getNewMove': 'GetVelocity' }, autonomy={ 'done': Autonomy.Off, 'getNewMove': Autonomy.Off }, remapping={ 'input_velocity': 'velocitymy', 'input_rotation_rate': 'angularmy' }) # x:277 y:51 OperatableStateMachine.add('getang', SubscriberState(topic="/makethisupang", blocking=True, clear=False), transitions={ 'received': 'Ball_NOT_in_image', 'unavailable': 'getCount' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'angularmy'}) # x:97 y:394 OperatableStateMachine.add( 'Rotate', TimedTwistState( target_time=.1, velocity=0, rotation_rate=.5, cmd_topic='/turtlebot/stamped_cmd_vel_mux/input/navi'), transitions={'done': 'GetVelocity'}, autonomy={'done': Autonomy.Off}) # x:496 y:22 OperatableStateMachine.add( 'Ball_NOT_in_image', KyleVerifyState(ValueToMeasureAgainst=7777.0), transitions={ 'verified': 'getCount', 'notVerified': 'move' }, autonomy={ 'verified': Autonomy.Off, 'notVerified': Autonomy.Off }, remapping={ 'inputValueVel': 'velocitymy', 'inputValueAng': 'angularmy' }) # x:983 y:151 OperatableStateMachine.add('Should_Robot_Finish', OperatorDecisionState( outcomes=["yes", "no"], hint="Should the Robot Stop?", suggestion=None), transitions={ 'yes': 'finished', 'no': 'failed' }, autonomy={ 'yes': Autonomy.Off, 'no': Autonomy.Off }) # x:501 y:211 OperatableStateMachine.add('getCount', SubscriberState( topic='/makethisupcount', blocking=True, clear=False), transitions={ 'received': 'verifyCount', 'unavailable': 'Should_Robot_Finish' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'countMessage'}) # x:450 y:321 OperatableStateMachine.add( 'verifyCount', KyleVerifyState(ValueToMeasureAgainst=45), transitions={ 'verified': 'failed', 'notVerified': 'increaseCount' }, autonomy={ 'verified': Autonomy.Off, 'notVerified': Autonomy.Off }, remapping={ 'inputValueVel': 'countMessage', 'inputValueAng': 'countMessage' }) # x:143 y:120 OperatableStateMachine.add('GetVelocity', SubscriberState(topic="/makethisupvel", blocking=True, clear=False), transitions={ 'received': 'getang', 'unavailable': 'getCount' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'velocitymy'}) # x:271 y:304 OperatableStateMachine.add( 'increaseCount', KylePubInputState(cmd_topic='/makethisupcount', increaseBy=1), transitions={'done': 'Rotate'}, autonomy={'done': Autonomy.Off}, remapping={'valueToIncrease': 'countMessage'}) return _state_machine
def create(self): # x:807 y:293, x:1163 y:18 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:35 y:227, x:161 y:228, x:363 y:243, x:536 y:22, x:472 y:241, x:547 y:181, x:544 y:99, x:674 y:184, x:263 y:228 _sm_container_0 = ConcurrencyContainer( outcomes=['finished', 'failed', 'danger', 'preempted'], input_keys=['plan'], conditions=[('failed', [('DWA', 'failed')]), ('finished', [('DWA', 'done')]), ('preempted', [('DWA', 'preempted')]), ('danger', [('Safety', 'cliff')]), ('danger', [('Safety', 'bumper')])]) with _sm_container_0: # x:101 y:78 OperatableStateMachine.add( 'DWA', FollowPathState(topic="low_level_planner"), transitions={ 'done': 'finished', 'failed': 'failed', 'preempted': 'preempted' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off, 'preempted': Autonomy.Off }, remapping={'plan': 'plan'}) # x:343 y:91 OperatableStateMachine.add( 'Safety', TurtlebotStatusState(bumper_topic='mobile_base/events/bumper', cliff_topic='mobile_base/events/cliff'), transitions={ 'bumper': 'danger', 'cliff': 'danger' }, autonomy={ 'bumper': Autonomy.Off, 'cliff': Autonomy.Off }) with _state_machine: # x:193 y:26 OperatableStateMachine.add('ClearCostmap', ClearCostmapsState(costmap_topics=[ 'high_level_planner/clear_costmap', 'low_level_planner/clear_costmap' ], timeout=5.0), transitions={ 'done': 'Receive Goal', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) # x:205 y:207 OperatableStateMachine.add( 'Receive Path', GetPathState(planner_topic="high_level_planner"), transitions={ 'planned': 'ExecutePlan', 'empty': 'Continue', 'failed': 'Continue' }, autonomy={ 'planned': Autonomy.Off, 'empty': Autonomy.Low, 'failed': Autonomy.Low }, remapping={ 'goal': 'goal', 'plan': 'plan' }) # x:194 y:301 OperatableStateMachine.add('ExecutePlan', OperatorDecisionState( outcomes=["yes", "no"], hint="Execute the current plan?", suggestion="yes"), transitions={ 'yes': 'Container', 'no': 'Continue' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full }) # x:446 y:275 OperatableStateMachine.add('Container', _sm_container_0, transitions={ 'finished': 'Log Success', 'failed': 'AutoReplan', 'danger': 'EStop', 'preempted': 'Continue' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'danger': Autonomy.Inherit, 'preempted': Autonomy.Inherit }, remapping={'plan': 'plan'}) # x:435 y:146 OperatableStateMachine.add( 'Continue', OperatorDecisionState( outcomes=["yes", "no", "recover", "clearcostmap"], hint="Continue planning to new goal?", suggestion="yes"), transitions={ 'yes': 'Receive Goal', 'no': 'finished', 'recover': 'LogRecovery', 'clearcostmap': 'ClearCostmap' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full, 'recover': Autonomy.Full, 'clearcostmap': Autonomy.Full }) # x:1052 y:227 OperatableStateMachine.add('Turtlebot Simple Recovery', self.use_behavior( TurtlebotSimpleRecoverySM, 'Turtlebot Simple Recovery'), transitions={ 'finished': 'AutoReplan', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:664 y:307 OperatableStateMachine.add('Log Success', LogState(text="Success!", severity=Logger.REPORT_HINT), transitions={'done': 'Continue'}, autonomy={'done': Autonomy.Off}) # x:664 y:374 OperatableStateMachine.add('Log Fail', LogState(text="Path execution failure", severity=Logger.REPORT_HINT), transitions={'done': 'Recover'}, autonomy={'done': Autonomy.Off}) # x:960 y:70 OperatableStateMachine.add('Log Recovered', LogState(text="Re-plan after recovery", severity=Logger.REPORT_HINT), transitions={'done': 'New Plan'}, autonomy={'done': Autonomy.Off}) # x:772 y:71 OperatableStateMachine.add( 'New Plan', GetPathState(planner_topic="high_level_planner"), transitions={ 'planned': 'Container', 'empty': 'Receive Goal', 'failed': 'Continue' }, autonomy={ 'planned': Autonomy.Off, 'empty': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'goal': 'goal', 'plan': 'plan' }) # x:203 y:113 OperatableStateMachine.add( 'Receive Goal', GetPoseState(topic='flex_nav_global/goal'), transitions={'done': 'Receive Path'}, autonomy={'done': Autonomy.Low}, remapping={'goal': 'goal'}) # x:866 y:374 OperatableStateMachine.add('Recover', OperatorDecisionState( outcomes=["yes", "no"], hint="Should we attempt recovery?", suggestion="yes"), transitions={ 'yes': 'LogRecovery', 'no': 'finished' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full }) # x:887 y:236 OperatableStateMachine.add( 'LogRecovery', LogState(text="Starting recovery behavior", severity=Logger.REPORT_HINT), transitions={'done': 'Turtlebot Simple Recovery'}, autonomy={'done': Autonomy.Off}) # x:875 y:162 OperatableStateMachine.add('AutoReplan', OperatorDecisionState( outcomes=["yes", "no"], hint="Re-plan to current goal?", suggestion="yes"), transitions={ 'yes': 'Log Recovered', 'no': 'Continue' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full }) # x:453 y:376 OperatableStateMachine.add( 'EStop', TimedStopState(timeout=0.25, cmd_topic='stamped_cmd_vel_mux/input/navi', odom_topic='mobile_base/odom'), transitions={ 'done': 'Log Fail', 'failed': 'Log Fail' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) return _state_machine
def create(self): # x:1423 y:790, x:784 y:398 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.action_topic = "/move_group" _state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory" _state_machine.userdata.move_group = "arm" _state_machine.userdata.config_vertical = "Vertical" _state_machine.userdata.config_home = "Home" _state_machine.userdata.config_retract = "Retract" _state_machine.userdata.components = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'MoveIt', SetupProxyMoveItClientState( robot_description="/robot_description", robot_description_semantic=None, move_group_capabilities="/move_group", action_type_and_topics=[[ "MoveGroupAction", ["/move_group"] ]], enter_wait_duration=0.0), transitions={ 'connected': 'Connected', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'connected': Autonomy.High, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'move_groups': 'move_groups' }) # x:146 y:145 OperatableStateMachine.add('Connected', LogState(text="Connected", severity=Logger.REPORT_HINT), transitions={'done': 'Vertical'}, autonomy={'done': Autonomy.Off}) # x:481 y:644 OperatableStateMachine.add('Vertical', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'GoalTolerances', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Low, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group', 'config_name': 'config_vertical', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:909 y:136 OperatableStateMachine.add( 'Plan', JointValuesToMoveItPlanState(timeout=5.0, enter_wait_duration=0.5, action_topic=None), transitions={ 'planned': 'ExecTraj', 'failed': 'failed', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Full, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'action_topic': 'action_topic', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values', 'joint_trajectory': 'joint_trajectory' }) # x:461 y:479 OperatableStateMachine.add('GoalTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.05], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'PathTolerances', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_goal_tolerances' }) # x:544 y:189 OperatableStateMachine.add('PathTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.08], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'DumpGoalTolerance', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_path_tolerances' }) # x:389 y:34 OperatableStateMachine.add( 'DumpGoalTolerance', LogKeyState(text="Goal tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'DumpPathTolerance'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_goal_tolerances'}) # x:653 y:50 OperatableStateMachine.add( 'DumpPathTolerance', LogKeyState(text="Path tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'Plan'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_path_tolerances'}) # x:760 y:584 OperatableStateMachine.add('Home', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'Plan', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group', 'config_name': 'config_home', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:551 y:799 OperatableStateMachine.add( 'Decision', OperatorDecisionState(outcomes=["home", "vertical", "stop"], hint=None, suggestion=None), transitions={ 'home': 'Home', 'vertical': 'Vertical', 'stop': 'finished' }, autonomy={ 'home': Autonomy.Off, 'vertical': Autonomy.Off, 'stop': Autonomy.Full }) # x:1085 y:601 OperatableStateMachine.add('ExecTraj', ExecuteKnownTrajectoryState( timeout=3.0, max_delay=5.0, wait_duration=0.25, action_topic="/execute_trajectory"), transitions={ 'done': 'Decision', 'failed': 'failed', 'param_error': 'ErrLog' }, autonomy={ 'done': Autonomy.High, 'failed': Autonomy.High, 'param_error': Autonomy.Off }, remapping={ 'action_topic': 'action_topic', 'trajectory': 'joint_trajectory', 'status_text': 'status_text', 'goal_names': 'goal_names', 'goal_values': 'goal_values' }) # x:999 y:715 OperatableStateMachine.add('ErrLog', LogState( text="Param error on exec traj", severity=Logger.REPORT_HINT), transitions={'done': 'StatusLog'}, autonomy={'done': Autonomy.Off}) # x:897 y:761 OperatableStateMachine.add('StatusLog', LogKeyState( text=" Status {}", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'status_text'}) 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): arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM turn_handle_affordance = 'Up' strafe_direction = FootstepPlanRelativeState.DIRECTION_RIGHT if self.hand_side == 'right' else FootstepPlanRelativeState.DIRECTION_LEFT # x:16 y:476, x:360 y:313 _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.zero = 0 _state_machine.userdata.both_arms = 'same' _state_machine.userdata.strafe_distance = 3.0 # meters _state_machine.userdata.stand_pose_preference = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:851 y:272, x:198 y:293 _sm_push_door_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['handle_template_id', 'hand_side', 'none']) with _sm_push_door_0: # x:156 y:74 OperatableStateMachine.add( 'Get_Push_Affordance', GetTemplateAffordanceState(identifier='push'), transitions={ 'done': 'Plan_Push_Door', 'failed': 'failed', 'not_available': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High }, remapping={ 'template_id': 'handle_template_id', 'hand_side': 'hand_side', 'affordance': 'door_affordance' }) # x:781 y:72 OperatableStateMachine.add( 'Push_Door', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'joint_trajectory': 'joint_trajectory'}) # x:465 y:73 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.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High }, remapping={ 'affordance': 'door_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction' }) # x:642 y:277, x:123 y:475 _sm_turn_handle_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['handle_template_id', 'hand_side', 'none']) with _sm_turn_handle_1: # x:73 y:78 OperatableStateMachine.add( 'Get_Handle_Affordance', GetTemplateAffordanceState(identifier=turn_handle_affordance), transitions={ 'done': 'Plan_Turn_Handle', 'failed': 'failed', 'not_available': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High }, remapping={ 'template_id': 'handle_template_id', 'hand_side': 'hand_side', 'affordance': 'handle_affordance' }) # x:342 y:78 OperatableStateMachine.add( 'Plan_Turn_Handle', PlanAffordanceState(vel_scaling=0.03, planner_id="RRTConnectkConfigDefault"), transitions={ 'done': 'Turn_Handle', 'incomplete': 'Turn_Handle', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High }, remapping={ 'affordance': 'handle_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction' }) # x:580 y:77 OperatableStateMachine.add( 'Turn_Handle', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'joint_trajectory': 'joint_trajectory'}) # x:1075 y:57, x:260 y:86, x:1048 y:173 _sm_go_to_grasp_2 = OperatableStateMachine( outcomes=['finished', 'failed', 'again'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference']) with _sm_go_to_grasp_2: # 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:727 y:50 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:30 y:365, x:130 y:365 _sm_go_to_pregrasp_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference', 'pregrasp_pose']) with _sm_go_to_pregrasp_3: # 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:372 y:343, x:567 y:184 _sm_unlock_door_4 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['handle_template_id', 'hand_side', 'none']) with _sm_unlock_door_4: # x:75 y:64 OperatableStateMachine.add('Turn_Handle', _sm_turn_handle_1, transitions={ 'finished': 'Decide_Push_Turn', 'failed': 'Unlock_Door_Manually' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'handle_template_id': 'handle_template_id', 'hand_side': 'hand_side', 'none': 'none' }) # x:328 y:178 OperatableStateMachine.add( 'Unlock_Door_Manually', OperatorDecisionState( outcomes=["ready", "abort"], hint='Let the operator open the door manually.', suggestion=None), transitions={ 'ready': 'finished', 'abort': 'failed' }, autonomy={ 'ready': Autonomy.Full, 'abort': Autonomy.Full }) # x:69 y:336 OperatableStateMachine.add( 'Decide_Push_Turn', OperatorDecisionState( outcomes=['push', 'turn', 'skip'], hint='Ask operator whether to push more or turn again.', suggestion='skip'), transitions={ 'push': 'Push_Door', 'turn': 'Turn_Handle', 'skip': 'finished' }, autonomy={ 'push': Autonomy.High, 'turn': Autonomy.High, 'skip': Autonomy.Full }) # x:72 y:485 OperatableStateMachine.add('Push_Door', _sm_push_door_0, transitions={ 'finished': 'Decide_Push_Turn', 'failed': 'Push_Door' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'handle_template_id': 'handle_template_id', 'hand_side': 'hand_side', 'none': 'none' }) # x:820 y:178 _sm_traverse_door_5 = OperatableStateMachine( outcomes=['finished'], input_keys=['strafe_distance']) with _sm_traverse_door_5: # x:59 y:67 OperatableStateMachine.add( 'Plan_Strafing', FootstepPlanRelativeState(direction=strafe_direction), transitions={ 'planned': 'Confirm_Plan', 'failed': 'Request_Adjustment' }, autonomy={ 'planned': Autonomy.Low, 'failed': Autonomy.High }, remapping={ 'distance': 'strafe_distance', 'plan_header': 'plan_header' }) # x:64 y:228 OperatableStateMachine.add( 'Request_Adjustment', LogState( text='Ask the operator to align the robot with the door.', severity=Logger.REPORT_HINT), transitions={'done': 'Plan_Strafing'}, autonomy={'done': Autonomy.Full}) # x:583 y:65 OperatableStateMachine.add( 'Strafe_through_Door', ExecuteStepPlanActionState(), transitions={ 'finished': 'Notify_Strafe_End', 'failed': 'Request_Adjustment' }, autonomy={ 'finished': Autonomy.Off, 'failed': Autonomy.High }, remapping={'plan_header': 'plan_header'}) # x:318 y:66 OperatableStateMachine.add( 'Confirm_Plan', OperatorDecisionState( outcomes=['ready', 'adjust'], hint='Check if the robot is perpendicular to the door.', suggestion=None), transitions={ 'ready': 'Strafe_through_Door', 'adjust': 'Request_Adjustment' }, autonomy={ 'ready': Autonomy.Full, 'adjust': Autonomy.Low }) # x:603 y:173 OperatableStateMachine.add( 'Notify_Strafe_End', LogState(text='End of strafing. Switching to MANIPULATE next!', severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.High}) # x:484 y:37, x:383 y:171 _sm_open_door_6 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'none', 'hand_side', 'grasp_preference', 'handle_template_id' ]) with _sm_open_door_6: # x:69 y:28 OperatableStateMachine.add( 'Look_Down', TiltHeadState(desired_tilt=TiltHeadState.DOWN_45), transitions={ 'done': 'Turn_Torso', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }) # x:643 y:451 OperatableStateMachine.add('Unlock_Door', _sm_unlock_door_4, transitions={ 'finished': 'Ask_for_Retry', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'handle_template_id': 'handle_template_id', 'hand_side': 'hand_side', 'none': 'none' }) # x:636 y:248 OperatableStateMachine.add( 'Ask_for_Retry', OperatorDecisionState(outcomes=["done", "retry"], hint="Now release handle?", suggestion="done"), transitions={ 'done': 'Decide_Push_or_Not', 'retry': 'Optional_Template_Adjustment' }, autonomy={ 'done': Autonomy.High, 'retry': Autonomy.Full }) # x:51 y:134 OperatableStateMachine.add( 'Turn_Torso', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.TURN_TORSO_FULL, vel_scaling=0.2, ignore_collisions=True, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Optional_Template_Adjustment', 'failed': 'Turn_Torso' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'hand_side'}) # x:625 y:34 OperatableStateMachine.add( 'Push_Door_with_Arm', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.DOOR_OPEN_POSE_SIDE, vel_scaling=0.3, ignore_collisions=True, link_paddings={}, is_cartesian=True), transitions={ 'done': 'finished', 'failed': 'Push_Door_with_Arm' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'hand_side'}) # x:44 y:246 OperatableStateMachine.add( 'Optional_Template_Adjustment', LogState( text= 'Ask the operator to adjust the template, if necessary.', severity=Logger.REPORT_HINT), transitions={'done': 'Confirm_Closed_Fingers'}, autonomy={'done': Autonomy.High}) # x:59 y:451 OperatableStateMachine.add('Go_to_Pregrasp', _sm_go_to_pregrasp_3, transitions={ 'finished': 'Go_to_Grasp', 'failed': 'Ask_for_Retry' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'handle_template_id', 'pregrasp_pose': 'pregrasp_pose' }) # x:348 y:451 OperatableStateMachine.add('Go_to_Grasp', _sm_go_to_grasp_2, transitions={ 'finished': 'Unlock_Door', 'failed': 'failed', 'again': 'Go_to_Pregrasp' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'again': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'handle_template_id' }) # x:54 y:352 OperatableStateMachine.add( 'Confirm_Closed_Fingers', LogState(text='Check that the fingers are closed!', severity=Logger.REPORT_HINT), transitions={'done': 'Go_to_Pregrasp'}, autonomy={'done': Autonomy.High}) # x:637 y:137 OperatableStateMachine.add( 'Decide_Push_or_Not', OperatorDecisionState( outcomes=['push', 'skip'], hint='Check whether the door is already open.', suggestion=None), transitions={ 'push': 'Push_Door_with_Arm', 'skip': 'finished' }, autonomy={ 'push': Autonomy.High, 'skip': Autonomy.High }) with _state_machine: # x:73 y:310 OperatableStateMachine.add( 'Get_Door_Template_ID', InputState(request=InputState.SELECTED_OBJECT_ID, message="Provide the ID of the DOOR."), transitions={ 'received': 'Start_in_STAND', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed' }, autonomy={ 'received': Autonomy.Off, 'aborted': Autonomy.Low, 'no_connection': Autonomy.Low, 'data_error': Autonomy.Low }, remapping={'data': 'template_id'}) # x:730 y:362 OperatableStateMachine.add( 'Go_To_Stand_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND_MANIPULATE), transitions={ 'changed': 'Tilt_Head_Straight', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:72 y:21 OperatableStateMachine.add( 'Walk_to_Template', self.use_behavior(WalktoTemplateSM, 'Walk_to_Template'), transitions={ 'finished': 'Go_to_MANIPULATE', 'failed': 'failed', 'aborted': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit }, remapping={ 'grasp_preference': 'stand_pose_preference', 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:303 y:25 OperatableStateMachine.add( 'Go_to_MANIPULATE', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'Open_or_Traverse', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:761 y:24 OperatableStateMachine.add('Open_Door', _sm_open_door_6, transitions={ 'finished': 'Move_Arms_Sides', 'failed': 'Open_Door_Manually' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'none': 'none', 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'handle_template_id': 'template_id' }) # x:758 y:591 OperatableStateMachine.add( 'Traverse_Door', _sm_traverse_door_5, transitions={'finished': 'Go_back_to_MANIPULATE'}, autonomy={'finished': Autonomy.Inherit}, remapping={'strafe_distance': 'strafe_distance'}) # x:996 y:78 OperatableStateMachine.add( 'Open_Door_Manually', LogState(text='Have the operator open the door manually!', severity=Logger.REPORT_HINT), transitions={'done': 'Move_Arms_Sides'}, autonomy={'done': Autonomy.Full}) # x:762 y:479 OperatableStateMachine.add( 'Tilt_Head_Straight', TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT), transitions={ 'done': 'Traverse_Door', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }) # x:739 y:156 OperatableStateMachine.add( 'Move_Arms_Sides', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.BOTH_ARMS_SIDES, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Center_Torso', 'failed': 'Move_Arms_Sides' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'both_arms'}) # x:432 y:600 OperatableStateMachine.add( 'Go_back_to_MANIPULATE', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'Move_to_Stand_Pose', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:167 y:601 OperatableStateMachine.add( 'Move_to_Stand_Pose', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={ 'done': 'End_in_STAND', 'failed': 'Move_to_Stand_Pose' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'both_arms'}) # x:160 y:468 OperatableStateMachine.add( 'End_in_STAND', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'finished', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:741 y:266 OperatableStateMachine.add( 'Center_Torso', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState. TURN_TORSO_CENTER_POSE, vel_scaling=0.1, ignore_collisions=True, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Go_To_Stand_Manipulate', 'failed': 'Center_Torso' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'both_arms'}) # x:63 y:179 OperatableStateMachine.add( 'Start_in_STAND', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'Walk_to_Template', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:555 y:27 OperatableStateMachine.add( 'Open_or_Traverse', OperatorDecisionState( outcomes=['open', 'traverse'], hint='If this is after a reset, the door will be open.', suggestion='open'), transitions={ 'open': 'Open_Door', 'traverse': 'Move_Arms_Sides' }, autonomy={ 'open': Autonomy.Low, 'traverse': Autonomy.High }) return _state_machine
def create(self): arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM # x:1234 y:456, x:92 y:189 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.reference_point = None _state_machine.userdata.current_target_angle_deg = self.start_angle_deg; _state_machine.userdata.dummy = None _state_machine.userdata.save_joints = ['l_shoulder_pitch', 'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow', 'l_wrist_yaw1', 'l_wrist_roll', 'l_wrist_yaw2'] _state_machine.userdata.move_to_poses = self.move_to_poses _state_machine.userdata.hand_offset = [0, 0, -0.065] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self.total_displacement = 0; # [/MANUAL_CREATE] # x:1136 y:74, x:130 y:480 _sm_save_target_arm_pose_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['joint_trajectory', 'hand_side', 'move_to_poses']) with _sm_save_target_arm_pose_0: # x:146 y:184 OperatableStateMachine.add('Decide_Move_Arm', DecisionState(outcomes=['move_arm', 'do_not_move', 'abort'], conditions=self.decide_move_arm), transitions={'move_arm': 'Move_Arm_To_Pose', 'do_not_move': 'Save_Arm_Pose_From_Trajectory', 'abort': 'failed'}, autonomy={'move_arm': Autonomy.Full, 'do_not_move': Autonomy.Low, 'abort': Autonomy.Low}, remapping={'input_value': 'move_to_poses'}) # x:330 y:232 OperatableStateMachine.add('Move_Arm_To_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Get_Current_Joint_Values', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:875 y:223 OperatableStateMachine.add('Save_Arm_Pose_From_Joints', CalculationState(calculation=self.save_arm_pose_from_joints), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'joint_config', 'output_value': 'success'}) # x:330 y:45 OperatableStateMachine.add('Save_Arm_Pose_From_Trajectory', CalculationState(calculation=self.save_arm_pose_from_trajectory), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'joint_trajectory', 'output_value': 'success'}) # x:622 y:225 OperatableStateMachine.add('Get_Current_Joint_Values', QueryJointPositionsState(side=self.hand_side, controller='traj_controller'), transitions={'retrieved': 'Save_Arm_Pose_From_Joints', 'failed': 'failed'}, autonomy={'retrieved': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'joint_config': 'joint_config'}) # x:553 y:74, x:273 y:212 _sm_get_reference_point_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side'], output_keys=['reference_point']) with _sm_get_reference_point_1: # x:30 y:40 OperatableStateMachine.add('Get_Hand_Frames', CalculationState(calculation=self.get_hand_frames), transitions={'done': 'Get_Reference_Point'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'hand_side', 'output_value': 'frames'}) # x:225 y:40 OperatableStateMachine.add('Get_Reference_Point', GetTFTransformState(), transitions={'done': 'Add_Offset', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'frames': 'frames', 'transform': 'reference_point'}) # x:411 y:83 OperatableStateMachine.add('Add_Offset', CalculationState(calculation=self.add_offset), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'reference_point', 'output_value': 'reference_point'}) # x:1432 y:637, x:44 y:619 _sm_calculate_key_frames_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['wheel_affordance', 'hand_side', 'reference_point', 'move_to_poses']) with _sm_calculate_key_frames_2: # x:30 y:40 OperatableStateMachine.add('Ignore_Quasi_Static_Constraint', SetRosParamState(parameter='/drake_ignore_quasi_static_constraint', value=True), transitions={'done': 'Plan_Wheel_Rotation', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}) # x:998 y:85 OperatableStateMachine.add('Continue_Rotation', DecisionState(outcomes=['continue_rotation', 'finished'], conditions=self.check_continue_rotation), transitions={'continue_rotation': 'Plan_Wheel_Rotation', 'finished': 'Reset_Quasi_Static_Constraint'}, autonomy={'continue_rotation': Autonomy.Low, 'finished': Autonomy.Low}, remapping={'input_value': 'wheel_affordance'}) # x:921 y:470 OperatableStateMachine.add('Calculate_New_Target_Angle', CalculationState(calculation=self.calc_rotation_angle), transitions={'done': 'Continue_Rotation'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'wheel_affordance', 'output_value': 'wheel_affordance'}) # x:327 y:459 OperatableStateMachine.add('Decide_Continue', OperatorDecisionState(outcomes=['continue', 'retry', 'abort'], hint=None, suggestion=None), transitions={'continue': 'Calculate_New_Target_Angle', 'retry': 'Plan_Wheel_Rotation', 'abort': 'failed'}, autonomy={'continue': Autonomy.High, 'retry': Autonomy.High, 'abort': Autonomy.High}) # x:670 y:190 OperatableStateMachine.add('Save_Target_Arm_Pose', _sm_save_target_arm_pose_0, transitions={'finished': 'Calculate_New_Target_Angle', 'failed': 'Decide_Continue'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'joint_trajectory': 'joint_trajectory', 'hand_side': 'hand_side', 'move_to_poses': 'move_to_poses'}) # x:1172 y:609 OperatableStateMachine.add('Reset_Quasi_Static_Constraint', SetRosParamState(parameter='/drake_ignore_quasi_static_constraint', value=False), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}) # x:334 y:65 OperatableStateMachine.add('Plan_Wheel_Rotation', PlanAffordanceState(vel_scaling=0.1, planner_id="drake", drake_sample_rate=4.0, drake_orientation_type=1, drake_link_axis=[1,0,0]), transitions={'done': 'Save_Target_Arm_Pose', 'incomplete': 'Decide_Continue', 'failed': 'Decide_Continue'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'affordance': 'wheel_affordance', 'hand': 'hand_side', 'reference_point': 'reference_point', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:936 y:246, x:87 y:494 _sm_get_template_affordance_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side'], output_keys=['wheel_affordance']) with _sm_get_template_affordance_3: # x:71 y:85 OperatableStateMachine.add('Request_Template_ID', InputState(request=InputState.SELECTED_OBJECT_ID, message="Provide the ID of the STEERING_WHEEL template."), transitions={'received': 'Get_Template_Affordance', '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': 'wheel_template_id'}) # x:167 y:234 OperatableStateMachine.add('Log_Data_Error', LogState(text="Received wrong data format!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Low}) # x:328 y:236 OperatableStateMachine.add('Log_No_Connection', LogState(text='No Connection', severity=Logger.REPORT_HINT), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Low}) # x:600 y:230 OperatableStateMachine.add('Get_Template_Affordance', GetTemplateAffordanceState(identifier='turn_right'), transitions={'done': 'finished', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low, 'not_available': Autonomy.Low}, remapping={'template_id': 'wheel_template_id', 'hand_side': 'hand_side', 'affordance': 'wheel_affordance'}) with _state_machine: # x:61 y:59 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': 'Get_Reference_Point'}, autonomy={'done': Autonomy.Off}) # x:249 y:210 OperatableStateMachine.add('Get_Template_Affordance', _sm_get_template_affordance_3, transitions={'finished': 'Init_Affordance', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'wheel_affordance': 'wheel_affordance'}) # x:1080 y:318 OperatableStateMachine.add('Notify_Behavior_Exit', LogState(text='The behavior will now exit. Last chance to intervene!', severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.High}) # x:316 y:315 OperatableStateMachine.add('Init_Affordance', CalculationState(calculation=self.init_affordance), transitions={'done': 'Calculate_Key_Frames'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'wheel_affordance', 'output_value': 'wheel_affordance'}) # x:732 y:320 OperatableStateMachine.add('Decide_Save', OperatorDecisionState(outcomes=['save', 'no_save'], hint=None, suggestion=None), transitions={'save': 'Save_Key_Positions', 'no_save': 'Notify_Behavior_Exit'}, autonomy={'save': Autonomy.Full, 'no_save': Autonomy.Full}) # x:925 y:424 OperatableStateMachine.add('Save_Key_Positions', CalculationState(calculation=self.save_key_positions_to_disc), transitions={'done': 'Notify_Behavior_Exit'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'dummy', 'output_value': 'output_value'}) # x:471 y:319 OperatableStateMachine.add('Calculate_Key_Frames', _sm_calculate_key_frames_2, transitions={'finished': 'Decide_Save', 'failed': 'Decide_Save'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'wheel_affordance': 'wheel_affordance', 'hand_side': 'hand_side', 'reference_point': 'reference_point', 'move_to_poses': 'move_to_poses'}) # x:267 y:58 OperatableStateMachine.add('Get_Reference_Point', _sm_get_reference_point_1, transitions={'finished': 'Get_Template_Affordance', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'reference_point': 'reference_point'}) return _state_machine
def create(self): # x:1500 y:655, x:784 y:398 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.action_topic = "/move_group" _state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory" _state_machine.userdata.move_group_arm = "arm" _state_machine.userdata.config_vertical = "Vertical" _state_machine.userdata.config_home = "Home" _state_machine.userdata.config_retract = "Retract" _state_machine.userdata.components = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'MoveIt', SetupProxyMoveItClientState( robot_description="/robot_description", robot_description_semantic=None, move_group_capabilities="/move_group", action_type_and_topics=None, enter_wait_duration=0.0), transitions={ 'connected': 'Connected', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'connected': Autonomy.High, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'move_groups': 'move_groups' }) # x:146 y:145 OperatableStateMachine.add('Connected', LogState(text="Connected", severity=Logger.REPORT_HINT), transitions={'done': 'RobotName'}, autonomy={'done': Autonomy.Off}) # x:645 y:678 OperatableStateMachine.add('Vertical', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'GoalTolerances', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Low, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_arm', 'config_name': 'config_vertical', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:823 y:29 OperatableStateMachine.add( 'Plan', JointValuesToMoveItPlanState(timeout=5.0, enter_wait_duration=0.5, action_topic=None), transitions={ 'planned': 'Execute', 'failed': 'failed', 'topics_unavailable': 'failed', 'param_error': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Full, 'topics_unavailable': Autonomy.Full, 'param_error': Autonomy.Full }, remapping={ 'action_topic': 'action_topic', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values', 'joint_trajectory': 'joint_trajectory' }) # x:1215 y:121 OperatableStateMachine.add( 'Execute', TrajectoryToFollowJointTrajectoryActionState( goal_time_tolerance=3.0, max_delay=-1.0, wait_duration=2.0, timeout=5.0, action_topic=None), transitions={ 'reached': 'Decision', 'goal_failed': 'GoalFailedLog', 'path_failed': 'PathTolLog', 'invalid_request': 'failed', 'param_error': 'failed', 'failed': 'failed' }, autonomy={ 'reached': Autonomy.Full, 'goal_failed': Autonomy.Off, 'path_failed': Autonomy.Off, 'invalid_request': Autonomy.Full, 'param_error': Autonomy.Full, 'failed': Autonomy.Full }, remapping={ 'trajectory_action_topic': 'trajectory_action_topic', 'joint_trajectory': 'joint_trajectory', 'joint_goal_tolerances': 'joint_goal_tolerances', 'joint_path_tolerances': 'joint_path_tolerances', 'status_text': 'status_text', 'goal_names': 'goal_names', 'goal_values': 'goal_values' }) # x:616 y:559 OperatableStateMachine.add('GoalTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.05], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'LogValues', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_goal_tolerances' }) # x:544 y:189 OperatableStateMachine.add('PathTolerances', SetJointTrajectoryTolerancesState( position_constraints=[0.08], velocity_constraints=[0.0], acceleration_constraints=[0.0]), transitions={ 'configured': 'DumpGoalTolerance', 'param_error': 'failed' }, autonomy={ 'configured': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'joint_names': 'joint_names', 'joint_tolerances': 'joint_path_tolerances' }) # x:389 y:34 OperatableStateMachine.add( 'DumpGoalTolerance', LogKeyState(text="Goal tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'DumpPathTolerance'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_goal_tolerances'}) # x:560 y:50 OperatableStateMachine.add( 'DumpPathTolerance', LogKeyState(text="Path tolerance {}", severity=Logger.REPORT_HINT), transitions={'done': 'Plan'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_path_tolerances'}) # x:889 y:579 OperatableStateMachine.add('Home', GetJointValuesFromSrdfConfigState(), transitions={ 'retrieved': 'Plan', 'param_error': 'failed' }, autonomy={ 'retrieved': Autonomy.Off, 'param_error': Autonomy.Full }, remapping={ 'robot_name': 'robot_name', 'selected_move_group': 'move_group_arm', 'config_name': 'config_home', 'move_group': 'move_group', 'joint_names': 'joint_names', 'joint_values': 'joint_values' }) # x:1265 y:670 OperatableStateMachine.add('Decision', OperatorDecisionState(outcomes=[ "home", "vertical", "PS", "stop" ], hint=None, suggestion=None), transitions={ 'home': 'Home', 'vertical': 'Vertical', 'PS': 'ApplyPS', 'stop': 'finished' }, autonomy={ 'home': Autonomy.Off, 'vertical': Autonomy.Off, 'PS': Autonomy.Low, 'stop': Autonomy.Full }) # x:48 y:261 OperatableStateMachine.add( 'ClearOM', ClearOctomapState(timeout=5.0, wait_duration=5, action_topic="/clear_octomap"), transitions={ 'done': 'QueryPlanners', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'action_topic': 'action_topic'}) # x:47 y:402 OperatableStateMachine.add( 'QueryPlanners', QueryPlannersState(timeout=5.0, wait_duration=0.001, action_topic="/query_planner_interface"), transitions={ 'done': 'LogPlanners', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'action_topic': 'action_topic', 'planner_interfaces': 'planner_interfaces' }) # x:108 y:527 OperatableStateMachine.add( 'LogPlanners', LogKeyState(text="Available Planners {}", severity=Logger.REPORT_HINT), transitions={'done': 'GetPlanningScene'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'planner_interfaces'}) # x:87 y:641 OperatableStateMachine.add('GetPlanningScene', GetPlanningSceneState( components=1023, timeout=5.0, wait_duration=5, action_topic="/get_planning_scene"), transitions={ 'done': 'LogPS', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={ 'action_topic': 'action_topic', 'scene': 'scene' }) # x:1411 y:534 OperatableStateMachine.add( 'ApplyPS', ApplyPlanningSceneState(timeout=5.0, wait_duration=5, action_topic="/apply_planning_scene"), transitions={ 'done': 'Decision', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'action_topic': 'action_topic'}) # x:363 y:684 OperatableStateMachine.add('LogPS', LogKeyState( text="Planning Scence {}", severity=Logger.REPORT_HINT), transitions={'done': 'Vertical'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'scene'}) # x:1157 y:336 OperatableStateMachine.add('PathTolLog', LogState(text="Path tolerance failure", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.High}) # x:1254 y:288 OperatableStateMachine.add( 'GoalFailedLog', LogState(text="Failed to reach goal tolerance", severity=Logger.REPORT_HINT), transitions={'done': 'Decision'}, autonomy={'done': Autonomy.High}) # x:279 y:190 OperatableStateMachine.add('RobotName', LogKeyState( text="Robot Name ({})", severity=Logger.REPORT_HINT), transitions={'done': 'ClearOM'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'robot_name'}) # x:632 y:279 OperatableStateMachine.add('LogValues', LogKeyState( text="Joint Values {}", severity=Logger.REPORT_HINT), transitions={'done': 'PathTolerances'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'joint_values'}) return _state_machine
def create(self): # x:921 y:57, x:456 y:275 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.null_path = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:433 y:240, x:533 y:40, x:733 y:40, x:433 y:190, x:433 y:290, x:333 y:40, x:157 y:40, x:333 y:390, x:133 y:390, x:686 y:592, x:533 y:390, x:737 y:611, x:752 y:572 _sm_navigate_0 = ConcurrencyContainer( outcomes=['finished', 'failed', 'preempt'], input_keys=['plan', 'null_path'], conditions=[('finished', [('Start DWA', 'done')]), ('finished', [('Execute Pure Pursuit', 'done')]), ('finished', [('Start SBPL', 'done')]), ('failed', [('Start DWA', 'failed')]), ('preempt', [('Start DWA', 'preempted')]), ('failed', [('Execute Pure Pursuit', 'failed')]), ('preempt', [('Execute Pure Pursuit', 'preempted')]), ('failed', [('Start SBPL', 'failed')]), ('preempt', [('Start SBPL', 'preempted')]), ('failed', [('Monitor Health', 'failed')])]) with _sm_navigate_0: # x:187 y:178 OperatableStateMachine.add( 'Execute Pure Pursuit', FollowTopicState( planner_topic='low_level_planner/DWAPlannerROS/local_plan', controller_topic='pure_pursuit'), transitions={ 'done': 'finished', 'failed': 'failed', 'preempted': 'preempt' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off, 'preempted': Autonomy.Off }) # x:600 y:178 OperatableStateMachine.add( 'Start DWA', FollowTopicState( planner_topic='mid_level_planner/SBPLLatticePlanner/plan', controller_topic='low_level_planner'), transitions={ 'done': 'finished', 'failed': 'failed', 'preempted': 'preempt' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off, 'preempted': Autonomy.Off }) # x:201 y:263 OperatableStateMachine.add( 'Start SBPL', FollowPathState(topic='mid_level_planner'), transitions={ 'done': 'finished', 'failed': 'failed', 'preempted': 'preempt' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off, 'preempted': Autonomy.Off }, remapping={'plan': 'plan'}) # x:597 y:278 OperatableStateMachine.add('Monitor Health', CreateStatusState(), transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Off}) with _state_machine: # x:191 y:50 OperatableStateMachine.add( 'Get Pose', GetPoseState(topic='/move_base_simple/goal'), transitions={'done': 'Get Path'}, autonomy={'done': Autonomy.Off}, remapping={'goal': 'goal'}) # x:191 y:177 OperatableStateMachine.add( 'Get Path', GetPathState(planner_topic='high_level_planner'), transitions={ 'planned': 'Execute', 'empty': 'Log Empty Path', 'failed': 'Log Fail' }, autonomy={ 'planned': Autonomy.Off, 'empty': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'goal': 'goal', 'plan': 'plan' }) # x:636 y:52 OperatableStateMachine.add('Continue', OperatorDecisionState( outcomes=['yes', 'no'], hint=None, suggestion=None), transitions={ 'yes': 'Get Pose', 'no': 'finished' }, autonomy={ 'yes': Autonomy.Off, 'no': Autonomy.Off }) # x:646 y:174 OperatableStateMachine.add('Navigate', _sm_navigate_0, transitions={ 'finished': 'Continue', 'failed': 'Log Rerouting', 'preempt': 'Get Pose' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'preempt': Autonomy.Inherit }, remapping={ 'plan': 'plan', 'null_path': 'null_path' }) # x:406 y:181 OperatableStateMachine.add('Execute', OperatorDecisionState( outcomes=['yes', 'no'], hint=None, suggestion=None), transitions={ 'yes': 'Navigate', 'no': 'Continue' }, autonomy={ 'yes': Autonomy.Off, 'no': Autonomy.Off }) # x:20 y:177 OperatableStateMachine.add('Log Empty Path', LogState(text='Empty path', severity=Logger.logerr), transitions={'done': 'Get Pose'}, autonomy={'done': Autonomy.Off}) # x:198 y:269 OperatableStateMachine.add('Log Fail', LogState( text='Failed to generate a path', severity=Logger.logerr), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:656 y:273 OperatableStateMachine.add( 'Try Again', GetPathState(planner_topic='high_level_planner'), transitions={ 'planned': 'Navigate', 'empty': 'Log Empty Path', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'empty': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'goal': 'goal', 'plan': 'plan' }) # x:841 y:178 OperatableStateMachine.add('Log Rerouting', LogState(text='Rerouting...', severity=Logger.logerr), transitions={'done': 'Try Again'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): warn_threshold = 0.1 # for joint offsets bag_folder_out = "~/ft_calib/ft_logs" initial_mode = "stand" motion_mode = "manipulate" transitiontime = 0.5 settlingtime = 0.5 txtfile_name_left_arm = "~/ft_calib/input/SI_E047_FT_Calib_Arms_l_arm.txt" txtfile_name_right_arm = "~/ft_calib/input/SI_E047_FT_Calib_Arms_r_arm.txt" calibration_chain = ["right_arm"] static_calibration_data = {} # x:783 y:13, x:649 y:73 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.parameter_keys_dict = None _state_machine.userdata.calibration_chain_user = calibration_chain _state_machine.userdata.txtfile_name_left_arm_user = txtfile_name_left_arm _state_machine.userdata.txtfile_name_right_arm_user = txtfile_name_right_arm # Additional creation code can be added inside the following tags # [MANUAL_CREATE] bag_folder_out = os.path.expanduser(bag_folder_out) if not os.path.exists(bag_folder_out): os.makedirs(bag_folder_out) # Create STAND posture trajectory _state_machine.userdata.stand_posture = AtlasFunctions.gen_stand_posture_trajectory( ) # [/MANUAL_CREATE] # x:861 y:31, x:1047 y:103 _sm_starting_point_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['experiment_name', 'trajectories']) with _sm_starting_point_0: # x:49 y:42 OperatableStateMachine.add( 'Gen_Starting_Name', CalculationState(calculation=lambda en: en + "_starting"), transitions={'done': 'Gen_Starting_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'experiment_name', 'output_value': 'starting_name' }) # x:42 y:231 OperatableStateMachine.add( 'Record_Starting_Point', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_for_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={ 'bagfile_name': 'output_bagfile_starting', 'rosbag_process': 'rosbag_process_starting' }) # x:38 y:330 OperatableStateMachine.add( 'Wait_for_Rosbag_Record', WaitState(wait_time=1.0), transitions={'done': 'Extract_Left_Arm_Part'}, autonomy={'done': Autonomy.Off}) # x:29 y:133 OperatableStateMachine.add( 'Gen_Starting_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join( bag_folder_out, en) + ".bag"), transitions={'done': 'Record_Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'starting_name', 'output_value': 'output_bagfile_starting' }) # x:536 y:47 OperatableStateMachine.add( 'Stop_Recording_Starting_Point', StopRecordLogsState(), transitions={'stopped': 'finished'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:228 y:757 OperatableStateMachine.add( 'Plan_to_Starting_Point_Left_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Right_Arm', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_left_arm', 'plan_to_goal': 'plan_to_goal_left_arm' }) # x:236 y:655 OperatableStateMachine.add( 'Plan_to_Starting_Point_Right_Arm', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Left_Leg', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_right_arm', 'plan_to_goal': 'plan_to_goal_right_arm' }) # x:272 y:47 OperatableStateMachine.add( 'Go_to_Starting_Point', ExecuteTrajectoryWholeBodyState(controllers=[]), transitions={ 'done': 'Stop_Recording_Starting_Point', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'trajectories': 'trajectories_all'}) # x:536 y:169 OperatableStateMachine.add( 'Report_Starting_Point_Failure', LogState(text="Failed to plan or go to starting point!", severity=Logger.REPORT_WARN), transitions={'done': 'Stop_Recording_When_Failed'}, autonomy={'done': Autonomy.Full}) # x:34 y:424 OperatableStateMachine.add( 'Extract_Left_Arm_Part', CalculationState( calculation=lambda t: {'left_arm': t['left_arm']} if 'left_arm' in t else None), transitions={'done': 'Extract_Right_Arm_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_left_arm' }) # x:21 y:507 OperatableStateMachine.add( 'Extract_Right_Arm_Part', CalculationState( calculation=lambda t: {'right_arm': t['right_arm']} if 'right_arm' in t else None), transitions={'done': 'Extract_Left_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_right_arm' }) # x:293 y:146 OperatableStateMachine.add( 'Combine_Plans', FlexibleCalculationState(calculation=self.combine_plans, input_keys=[ 'left_arm', 'right_arm', 'left_leg', 'right_leg', 'torso' ]), transitions={'done': 'Go_to_Starting_Point'}, autonomy={'done': Autonomy.Low}, remapping={ 'left_arm': 'plan_to_goal_left_arm', 'right_arm': 'plan_to_goal_right_arm', 'left_leg': 'plan_to_goal_left_leg', 'right_leg': 'plan_to_goal_right_leg', 'torso': 'plan_to_goal_torso', 'output_value': 'trajectories_all' }) # x:789 y:167 OperatableStateMachine.add( 'Stop_Recording_When_Failed', StopRecordLogsState(), transitions={'stopped': 'failed'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process_starting'}) # x:24 y:601 OperatableStateMachine.add( 'Extract_Left_Leg_Part', CalculationState( calculation=lambda t: {'left_leg': t['left_leg']} if 'left_leg' in t else None), transitions={'done': 'Extract_Right_Leg_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_left_leg' }) # x:22 y:665 OperatableStateMachine.add( 'Extract_Right_Leg_Part', CalculationState( calculation=lambda t: {'right_leg': t['right_leg']} if 'right_leg' in t else None), transitions={'done': 'Extract_Torso_Part'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_right_leg' }) # x:33 y:765 OperatableStateMachine.add( 'Extract_Torso_Part', CalculationState(calculation=lambda t: {'torso': t['torso']} if 'torso' in t else None), transitions={'done': 'Plan_to_Starting_Point_Left_Arm'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'trajectories', 'output_value': 'trajectories_torso' }) # x:227 y:410 OperatableStateMachine.add( 'Plan_to_Starting_Point_Right_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Torso', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_right_leg', 'plan_to_goal': 'plan_to_goal_right_leg' }) # x:237 y:531 OperatableStateMachine.add( 'Plan_to_Starting_Point_Left_Leg', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Plan_to_Starting_Point_Right_Leg', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_left_leg', 'plan_to_goal': 'plan_to_goal_left_leg' }) # x:241 y:296 OperatableStateMachine.add( 'Plan_to_Starting_Point_Torso', MoveItMoveGroupPlanState(vel_scaling=0.1), transitions={ 'done': 'Combine_Plans', 'failed': 'Report_Starting_Point_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'desired_goal': 'trajectories_torso', 'plan_to_goal': 'plan_to_goal_torso' }) # x:1090 y:55, x:340 y:59 _sm_execute_individual_trajectory_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['output_bagfile', 'trajectories', 'experiment_name']) with _sm_execute_individual_trajectory_1: # x:124 y:257 OperatableStateMachine.add( 'Start_Video_Logging', VideoLoggingState(command=VideoLoggingState.START, no_video=False, no_bags=True), transitions={'done': 'Starting_Point'}, autonomy={'done': Autonomy.Off}, remapping={ 'experiment_name': 'experiment_name', 'description': 'experiment_name' }) # x:484 y:106 OperatableStateMachine.add( 'Stop_Recording_After_Failure', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging_After_Failure'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:727 y:188 OperatableStateMachine.add('Wait_for_Settling', WaitState(wait_time=1.0), transitions={'done': 'Stop_Recording'}, autonomy={'done': Autonomy.Off}) # x:482 y:188 OperatableStateMachine.add( 'Execute_Trajs_from_Bagfile', ExecuteTrajectoryWholeBodyState(controllers=[]), transitions={ 'done': 'Wait_for_Settling', 'failed': 'Stop_Recording_After_Failure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Low }, remapping={'trajectories': 'trajectories'}) # x:903 y:97 OperatableStateMachine.add( 'Stop_Video_Logging', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'experiment_name': 'experiment_name', 'description': 'experiment_name' }) # x:472 y:26 OperatableStateMachine.add( 'Stop_Video_Logging_After_Failure', VideoLoggingState(command=VideoLoggingState.STOP, no_video=False, no_bags=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={ 'experiment_name': 'experiment_name', 'description': 'experiment_name' }) # x:285 y:378 OperatableStateMachine.add('Starting_Point', _sm_starting_point_0, transitions={ 'finished': 'Record_SysID_Test', 'failed': 'Stop_Video_Logging_After_Failure' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'experiment_name': 'experiment_name', 'trajectories': 'trajectories' }) # x:904 y:187 OperatableStateMachine.add( 'Stop_Recording', StopRecordLogsState(), transitions={'stopped': 'Stop_Video_Logging'}, autonomy={'stopped': Autonomy.Low}, remapping={'rosbag_process': 'rosbag_process'}) # x:494 y:292 OperatableStateMachine.add( 'Wait_For_Rosbag_Record', WaitState(wait_time=1.0), transitions={'done': 'Execute_Trajs_from_Bagfile'}, autonomy={'done': Autonomy.Low}) # x:507 y:384 OperatableStateMachine.add( 'Record_SysID_Test', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_For_Rosbag_Record'}, autonomy={'logging': Autonomy.Off}, remapping={ 'bagfile_name': 'output_bagfile', 'rosbag_process': 'rosbag_process' }) # x:1267 y:273, x:406 y:121 _sm_perform_calibration_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['output_bagfile', 'experiment_name', 'trajectories'], output_keys=['trajectories_command']) with _sm_perform_calibration_2: # x:136 y:171 OperatableStateMachine.add( 'Go_to_Intermediate_Mode', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Execute_Individual_Trajectory', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Low }) # x:514 y:314 OperatableStateMachine.add( 'Intermediate_Mode_before_exit', ChangeControlModeActionState(target_mode=motion_mode), transitions={ 'changed': 'Initial_Mode_before_exit', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:995 y:274 OperatableStateMachine.add( 'Initial_Mode_before_exit', ChangeControlModeActionState(target_mode=initial_mode), transitions={ 'changed': 'finished', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:222 y:296 OperatableStateMachine.add('Execute_Individual_Trajectory', _sm_execute_individual_trajectory_1, transitions={ 'finished': 'Intermediate_Mode_before_exit', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'output_bagfile': 'output_bagfile', 'trajectories': 'trajectories', 'experiment_name': 'experiment_name' }) # x:221 y:562, x:709 y:85 _sm_update_calibration_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'output_bagfile', 'trajectories', 'calibration_chain_key_local' ]) with _sm_update_calibration_3: # x:111 y:94 OperatableStateMachine.add( 'Calculate_Calibration', CalculateForceTorqueCalibration( calibration_chain=calibration_chain, settlingtime=settlingtime, static_calibration_data=static_calibration_data), transitions={ 'done': 'Ask_Perform_Update', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'bag_filename': 'output_bagfile', 'trajectories_command': 'trajectories', 'ft_calib_data': 'ft_calib_data' }) # x:420 y:515 OperatableStateMachine.add( 'Calibration_Successful', LogState(text="Successfully updated calibration offsets.", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:774 y:277 OperatableStateMachine.add( 'Calibration_Failed', LogState(text="Failed to apply calibration offsets!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:122 y:202 OperatableStateMachine.add( 'Ask_Perform_Update', OperatorDecisionState( outcomes=['update', 'no_update'], hint= "Do you want to apply the calculated offsets for calibration?", suggestion=None), transitions={ 'update': 'Generate_Keys_Dict', 'no_update': 'finished' }, autonomy={ 'update': Autonomy.Full, 'no_update': Autonomy.Full }) # x:207 y:308 OperatableStateMachine.add( 'Generate_Keys_Dict', FlexibleCalculationState( calculation=self.create_parameter_keys_dict, input_keys=["input"]), transitions={'done': 'Update_Dynamic_Reconfigure'}, autonomy={'done': Autonomy.Off}, remapping={ 'input': 'calibration_chain_key_local', 'output_value': 'parameter_keys_dict' }) # x:408 y:303 OperatableStateMachine.add( 'Update_Dynamic_Reconfigure', UpdateDynamicParameterImpedanceControllerState( controller_chain=calibration_chain), transitions={ 'updated': 'Calibration_Successful', 'failed': 'Calibration_Failed' }, autonomy={ 'updated': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'parameter_keys': 'parameter_keys_dict', 'parameter_values': 'ft_calib_data' }) with _state_machine: # x:160 y:20 OperatableStateMachine.add( 'Execution_Starting', LogState( text= "Execution has started. Please confirm transition to first state.", severity=Logger.REPORT_HINT), transitions={'done': 'Gen_Experiment_Name'}, autonomy={'done': Autonomy.Full}) # x:685 y:431 OperatableStateMachine.add('Update_Calibration', _sm_update_calibration_3, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'output_bagfile': 'output_bagfile', 'trajectories': 'trajectories_command', 'calibration_chain_key_local': 'calibration_chain_user' }) # x:99 y:533 OperatableStateMachine.add( 'Decision_Perform_Experiment', OperatorDecisionState( outcomes=["record_data", "use_existing"], hint= "Use existing measurement data bag file or record a new one?", suggestion=None), transitions={ 'record_data': 'Perform_Calibration', 'use_existing': 'Update_Calibration' }, autonomy={ 'record_data': Autonomy.Low, 'use_existing': Autonomy.Low }) # x:130 y:185 OperatableStateMachine.add( 'Gen_Experiment_Name', FlexibleCalculationState(calculation=lambda i: 'FTCalib', input_keys=[]), transitions={'done': 'Gen_Output_Bagfile_Name'}, autonomy={'done': Autonomy.Off}, remapping={'output_value': 'experiment_name'}) # x:134 y:267 OperatableStateMachine.add( 'Gen_Output_Bagfile_Name', CalculationState(calculation=lambda en: os.path.join( bag_folder_out, en) + ".bag"), transitions={'done': 'Generate_Textfiles_Dict'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'experiment_name', 'output_value': 'output_bagfile' }) # x:443 y:252 OperatableStateMachine.add('Perform_Calibration', _sm_perform_calibration_2, transitions={ 'finished': 'Update_Calibration', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'output_bagfile': 'output_bagfile', 'experiment_name': 'experiment_name', 'trajectories': 'trajectories_command', 'trajectories_command': 'trajectories_command' }) # x:70 y:421 OperatableStateMachine.add('Load_Traj_From_Txt', GenerateTrajectoryFromTxtfileState( chains=calibration_chain, transitiontime=transitiontime, settlingtime=settlingtime), transitions={ 'done': 'Decision_Perform_Experiment', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'txtfilepaths': 'txtfiles_dict', 'trajectories': 'trajectories_command' }) # x:92 y:350 OperatableStateMachine.add( 'Generate_Textfiles_Dict', FlexibleCalculationState(calculation=self.create_txtfiles_dict, input_keys=[ "calibration_chain", "txtfile_left_arm", "txtfile_right_arm" ]), transitions={'done': 'Load_Traj_From_Txt'}, autonomy={'done': Autonomy.Off}, remapping={ 'calibration_chain': 'calibration_chain_user', 'txtfile_left_arm': 'txtfile_name_left_arm_user', 'txtfile_right_arm': 'txtfile_name_right_arm_user', 'output_value': 'txtfiles_dict' }) return _state_machine
def create(self): arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM no_valve_collision = True turning_affordance = "open" turn_amount = 200 # degree turn_back_affordance = "close" turn_test_amount = 25 # degree # x:933 y:490, x:333 y:390 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.default_preference = 0 _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.step_back_distance = 1 # meters _state_machine.userdata.none = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._turn_amount = turn_amount self._turn_test_amount = turn_test_amount # [/MANUAL_CREATE] # x:30 y:478, x:130 y:478, x:230 y:478 _sm_planning_pipeline_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['stand_pose'], output_keys=['plan_header']) with _sm_planning_pipeline_0: # x:34 y:57 OperatableStateMachine.add('Create_Step_Goal', CreateStepGoalState(pose_is_pelvis=True), transitions={'done': 'Plan_To_Waypoint', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'target_pose': 'stand_pose', 'step_goal': 'step_goal'}) # x:553 y:481 OperatableStateMachine.add('Modify_Plan', InputState(request=InputState.FOOTSTEP_PLAN_HEADER, message='Modify plan, VALIDATE, and confirm.'), transitions={'received': 'finished', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'plan_header'}) # x:34 y:484 OperatableStateMachine.add('Plan_To_Waypoint', PlanFootstepsState(mode=self.parameter_set), transitions={'planned': 'Modify_Plan', 'failed': 'Decide_Replan_without_Collision'}, autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:139 y:314 OperatableStateMachine.add('Decide_Replan_without_Collision', OperatorDecisionState(outcomes=['replan', 'fail'], hint='Try replanning without collision avoidance.', suggestion='replan'), transitions={'replan': 'Replan_without_Collision', 'fail': 'failed'}, autonomy={'replan': Autonomy.Low, 'fail': Autonomy.Full}) # x:319 y:406 OperatableStateMachine.add('Replan_without_Collision', PlanFootstepsState(mode='drc_step_no_collision'), transitions={'planned': 'Modify_Plan', 'failed': 'failed'}, autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:483 y:540, x:183 y:290 _sm_perform_turn_back_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'back_rotation', 'none']) with _sm_perform_turn_back_1: # x:71 y:78 OperatableStateMachine.add('Get_Turn_Back_Affordance', GetTemplateAffordanceState(identifier=turn_back_affordance), transitions={'done': 'Set_Back_Rotation', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'turning_affordance'}) # x:419 y:228 OperatableStateMachine.add('Plan_Turn_Back_Affordance', PlanAffordanceState(vel_scaling=0.2, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Turn_Back_Affordance', 'incomplete': 'Execute_Turn_Back_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:411 y:378 OperatableStateMachine.add('Execute_Turn_Back_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'finished'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:434 y:78 OperatableStateMachine.add('Set_Back_Rotation', FlexibleCalculationState(calculation=self.set_back_rotation, input_keys=["turning_affordance", "back_rotation"]), transitions={'done': 'Plan_Turn_Back_Affordance'}, autonomy={'done': Autonomy.Off}, remapping={'turning_affordance': 'turning_affordance', 'back_rotation': 'back_rotation', 'output_value': 'turning_affordance'}) # x:383 y:40, x:433 y:490 _sm_perform_turning_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none'], output_keys=['back_rotation']) with _sm_perform_turning_2: # x:92 y:28 OperatableStateMachine.add('Adjust_Hand_Pose', LogState(text="Make sure hand is in valve", severity=Logger.REPORT_HINT), transitions={'done': 'Init_Back_Rotation'}, autonomy={'done': Autonomy.Full}) # x:926 y:228 OperatableStateMachine.add('Plan_Turning_Affordance', PlanAffordanceState(vel_scaling=0.3, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Turning_Affordance', 'incomplete': 'Execute_Turning_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:495 y:228 OperatableStateMachine.add('Set_Full_Rotation', CalculationState(calculation=self.set_full_rotation_angle), transitions={'done': 'Plan_Turning_Affordance'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'turning_affordance', 'output_value': 'turning_affordance'}) # x:818 y:78 OperatableStateMachine.add('Execute_Turning_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Accumulate_Rotation', 'failed': 'Accumulate_Rotation'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:534 y:78 OperatableStateMachine.add('Accumulate_Rotation', FlexibleCalculationState(calculation=lambda x: x[0] + self._turn_amount * x[1], input_keys=["back_rotation", "plan_fraction"]), transitions={'done': 'Decide_Turn_Again'}, autonomy={'done': Autonomy.Off}, remapping={'back_rotation': 'back_rotation', 'plan_fraction': 'plan_fraction', 'output_value': 'back_rotation'}) # x:287 y:128 OperatableStateMachine.add('Decide_Turn_Again', OperatorDecisionState(outcomes=["extract", "turn"], hint="Turn again or extract hand?", suggestion="turn"), transitions={'extract': 'finished', 'turn': 'Decide_If_Test'}, autonomy={'extract': Autonomy.High, 'turn': Autonomy.Full}) # x:73 y:228 OperatableStateMachine.add('Get_Turning_Affordance', GetTemplateAffordanceState(identifier=turning_affordance), transitions={'done': 'Decide_If_Test', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'turning_affordance'}) # x:302 y:228 OperatableStateMachine.add('Decide_If_Test', DecisionState(outcomes=["test", "full"], conditions=lambda x: "test" if x == 0 else "full"), transitions={'test': 'Set_Test_Rotation', 'full': 'Set_Full_Rotation'}, autonomy={'test': Autonomy.Low, 'full': Autonomy.Low}, remapping={'input_value': 'back_rotation'}) # x:494 y:178 OperatableStateMachine.add('Set_Test_Rotation', CalculationState(calculation=self.set_test_rotation_angle), transitions={'done': 'Plan_Test_Turning_Affordance'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'turning_affordance', 'output_value': 'turning_affordance'}) # x:92 y:128 OperatableStateMachine.add('Init_Back_Rotation', CalculationState(calculation=lambda x: 0), transitions={'done': 'Get_Turning_Affordance'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'none', 'output_value': 'back_rotation'}) # x:713 y:178 OperatableStateMachine.add('Plan_Test_Turning_Affordance', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Turning_Affordance', 'incomplete': 'Execute_Turning_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:733 y:190, x:433 y:140 _sm_extract_hand_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'preference', 'none']) with _sm_extract_hand_3: # x:77 y:55 OperatableStateMachine.add('Get_Pregrasp', GetTemplatePregraspState(), transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'preference', 'pre_grasp': 'pre_grasp'}) # x:296 y:328 OperatableStateMachine.add('Plan_To_Pregrasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=no_valve_collision, include_torso=False, keep_endeffector_orientation=True, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Pregrasp', 'incomplete': 'Move_To_Pregrasp', 'failed': 'failed'}, autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'waypoints': 'waypoints', 'hand': 'hand_side', 'frame_id': 'frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:96 y:328 OperatableStateMachine.add('Create_Pose_List', CalculationState(calculation=lambda x: [x.pose]), transitions={'done': 'Plan_To_Pregrasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pre_grasp', 'output_value': 'waypoints'}) # x:676 y:328 OperatableStateMachine.add('Move_To_Pregrasp', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:95 y:228 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda x: x.header.frame_id), transitions={'done': 'Create_Pose_List'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pre_grasp', 'output_value': 'frame_id'}) # x:733 y:190, x:433 y:190 _sm_insert_hand_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'preference', 'none']) with _sm_insert_hand_4: # x:83 y:78 OperatableStateMachine.add('Get_Grasp', GetTemplateGraspState(), transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'preference', 'grasp': 'grasp'}) # x:296 y:328 OperatableStateMachine.add('Plan_To_Grasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=no_valve_collision, include_torso=False, keep_endeffector_orientation=True, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Grasp', 'incomplete': 'Move_To_Grasp', 'failed': 'failed'}, autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'waypoints': 'waypoints', 'hand': 'hand_side', 'frame_id': 'frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:96 y:328 OperatableStateMachine.add('Create_Pose_List', CalculationState(calculation=lambda x: [x.pose]), transitions={'done': 'Plan_To_Grasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp', 'output_value': 'waypoints'}) # x:676 y:328 OperatableStateMachine.add('Move_To_Grasp', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:96 y:228 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda x: x.header.frame_id), transitions={'done': 'Create_Pose_List'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp', 'output_value': 'frame_id'}) # x:933 y:290, x:133 y:340 _sm_prepare_joint_limit_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_prepare_joint_limit_5: # x:71 y:78 OperatableStateMachine.add('Get_Turn_Back_Affordance', GetTemplateAffordanceState(identifier=turn_back_affordance), transitions={'done': 'Set_Back_Rotation', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'turning_affordance'}) # x:419 y:228 OperatableStateMachine.add('Plan_Turn_Back_Affordance', PlanAffordanceState(vel_scaling=0.2, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Turn_Back_Affordance', 'incomplete': 'Execute_Turn_Back_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'affordance': 'turning_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:411 y:378 OperatableStateMachine.add('Execute_Turn_Back_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Decide_Turn_Further', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:442 y:78 OperatableStateMachine.add('Set_Back_Rotation', CalculationState(calculation=self.set_full_rotation_angle), transitions={'done': 'Plan_Turn_Back_Affordance'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'turning_affordance', 'output_value': 'turning_affordance'}) # x:687 y:278 OperatableStateMachine.add('Decide_Turn_Further', DecisionState(outcomes=["turn_again", "done"], conditions=lambda x: "turn_again" if x > 0.95 else "done"), transitions={'turn_again': 'Plan_Turn_Back_Affordance', 'done': 'finished'}, autonomy={'turn_again': Autonomy.High, 'done': Autonomy.Low}, remapping={'input_value': 'plan_fraction'}) # x:30 y:478, x:130 y:478, x:230 y:478 _sm_walk_to_template_6 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['template_id', 'grasp_preference', 'hand_side']) with _sm_walk_to_template_6: # x:265 y:28 OperatableStateMachine.add('Decide_Request_Template', DecisionState(outcomes=['request', 'continue'], conditions=lambda x: 'continue' if x is not None else 'request'), transitions={'request': 'Request_Template', 'continue': 'Get_Stand_Pose'}, autonomy={'request': Autonomy.Low, 'continue': Autonomy.Off}, remapping={'input_value': 'template_id'}) # x:1033 y:106 OperatableStateMachine.add('Increment_Stand_Pose', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Inform_About_Retry'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:1162 y:29 OperatableStateMachine.add('Inform_About_Retry', LogState(text="Stand pose choice failed. Trying again.", severity=Logger.REPORT_INFO), transitions={'done': 'Get_Stand_Pose'}, autonomy={'done': Autonomy.Off}) # x:567 y:118 OperatableStateMachine.add('Inform_About_Fail', LogState(text="Unable to find a suitable stand pose for the template.", severity=Logger.REPORT_WARN), transitions={'done': 'Decide_Repeat_Request'}, autonomy={'done': Autonomy.Off}) # x:554 y:274 OperatableStateMachine.add('Get_Goal_from_Operator', InputState(request=InputState.WAYPOINT_GOAL_POSE, message="Provide a waypoint in front of the template."), transitions={'received': 'Walk_To_Waypoint', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'plan_header'}) # x:279 y:110 OperatableStateMachine.add('Request_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Specify target template"), transitions={'received': 'Get_Stand_Pose', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Off, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:825 y:461 OperatableStateMachine.add('Wait_For_Stand', CheckCurrentControlModeState(target_mode=CheckCurrentControlModeState.STAND, wait=True), transitions={'correct': 'finished', 'incorrect': 'failed'}, autonomy={'correct': Autonomy.Low, 'incorrect': Autonomy.Full}, remapping={'control_mode': 'control_mode'}) # x:1143 y:277 OperatableStateMachine.add('Decide_Stand_Preference', OperatorDecisionState(outcomes=["same", "next", "abort"], hint="Same or next stand pose?", suggestion="next"), transitions={'same': 'Inform_About_Retry', 'next': 'Increment_Stand_Pose', 'abort': 'aborted'}, autonomy={'same': Autonomy.Full, 'next': Autonomy.Full, 'abort': Autonomy.Full}) # x:842 y:152 OperatableStateMachine.add('Planning_Pipeline', _sm_planning_pipeline_0, transitions={'finished': 'Walk_To_Waypoint', 'failed': 'Decide_Stand_Preference', 'aborted': 'Decide_Stand_Preference'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'stand_pose': 'stand_pose', 'plan_header': 'plan_header'}) # x:833 y:276 OperatableStateMachine.add('Walk_To_Waypoint', ExecuteStepPlanActionState(), transitions={'finished': 'Wait_For_Stand', 'failed': 'Decide_Stand_Preference'}, autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:554 y:195 OperatableStateMachine.add('Decide_Repeat_Request', OperatorDecisionState(outcomes=['repeat_id', 'request_goal'], hint=None, suggestion=None), transitions={'repeat_id': 'Request_Template', 'request_goal': 'Get_Goal_from_Operator'}, autonomy={'repeat_id': Autonomy.Low, 'request_goal': Autonomy.High}) # x:547 y:27 OperatableStateMachine.add('Get_Stand_Pose', GetTemplateStandPoseState(), transitions={'done': 'Planning_Pipeline', 'failed': 'Inform_About_Fail', 'not_available': 'Inform_About_Fail'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'stand_pose': 'stand_pose'}) # x:183 y:590, x:383 y:290 _sm_manipulate_valve_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'preference', 'none']) with _sm_manipulate_valve_7: # x:144 y:72 OperatableStateMachine.add('Insert_Hand', _sm_insert_hand_4, transitions={'finished': 'Perform_Turning', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'preference': 'preference', 'none': 'none'}) # x:543 y:422 OperatableStateMachine.add('Extract_Hand', _sm_extract_hand_3, transitions={'finished': 'Decide_Repeat', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'preference': 'preference', 'none': 'none'}) # x:534 y:72 OperatableStateMachine.add('Perform_Turning', _sm_perform_turning_2, transitions={'finished': 'Extract_Hand', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none', 'back_rotation': 'back_rotation'}) # x:127 y:272 OperatableStateMachine.add('Perform_Turn_Back', _sm_perform_turn_back_1, transitions={'finished': 'Adjust_Hand_Rotation', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'back_rotation': 'back_rotation', 'none': 'none'}) # x:137 y:428 OperatableStateMachine.add('Decide_Repeat', OperatorDecisionState(outcomes=['insert_again', 'continue'], hint="Continue or adjust wry2 rotation and rotate again", suggestion='insert_again'), transitions={'insert_again': 'Perform_Turn_Back', 'continue': 'finished'}, autonomy={'insert_again': Autonomy.High, 'continue': Autonomy.Full}) # x:134 y:178 OperatableStateMachine.add('Adjust_Hand_Rotation', OperatorDecisionState(outcomes=['done'], hint="Adjust poking stick rotation", suggestion=None), transitions={'done': 'Insert_Hand'}, autonomy={'done': Autonomy.Full}) # x:83 y:340, x:333 y:140 _sm_step_back_8 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['distance']) with _sm_step_back_8: # x:28 y:63 OperatableStateMachine.add('Plan_Steps_Back', FootstepPlanRelativeState(direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={'planned': 'Execute_Steps_Back', 'failed': 'failed'}, autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full}, remapping={'distance': 'distance', 'plan_header': 'plan_header'}) # x:24 y:163 OperatableStateMachine.add('Execute_Steps_Back', ExecuteStepPlanActionState(), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:638 y:580, x:230 y:239 _sm_prepare_manipulation_9 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'grasp_preference', 'hand_side', 'none']) with _sm_prepare_manipulation_9: # x:65 y:36 OperatableStateMachine.add('Go_To_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Set_Template_Frame', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:586 y:463 OperatableStateMachine.add('Adjust_Hand_Rotation', OperatorDecisionState(outcomes=['done'], hint="Adjust poking stick rotation", suggestion=None), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Full}) # x:116 y:409 OperatableStateMachine.add('Get_Pregrasp', GetTemplatePregraspState(), transitions={'done': 'Plan_To_Pregrasp', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pre_grasp'}) # x:329 y:178 OperatableStateMachine.add('Plan_To_Pregrasp', PlanEndeffectorPoseState(ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Pregrasp', 'failed': 'failed'}, autonomy={'planned': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'target_pose': 'pre_grasp', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory'}) # x:576 y:178 OperatableStateMachine.add('Move_To_Pregrasp', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Prepare_Joint_Limit', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:577 y:322 OperatableStateMachine.add('Prepare_Joint_Limit', _sm_prepare_joint_limit_5, transitions={'finished': 'Adjust_Hand_Rotation', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none'}) # x:53 y:130 OperatableStateMachine.add('Set_Template_Frame', CalculationState(calculation=lambda x: 'template_tf_' + str(x)), transitions={'done': 'Look_At_Valve'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'template_id', 'output_value': 'template_frame'}) # x:51 y:213 OperatableStateMachine.add('Look_At_Valve', LookAtTargetState(), transitions={'done': 'Align_Valve_Log'}, autonomy={'done': Autonomy.Low}, remapping={'frame': 'template_frame'}) # x:55 y:301 OperatableStateMachine.add('Align_Valve_Log', LogState(text="Adjust pose of the valve template", severity=Logger.REPORT_HINT), transitions={'done': 'Get_Pregrasp'}, autonomy={'done': Autonomy.Full}) with _state_machine: # x:35 y:78 OperatableStateMachine.add('Request_Template_ID', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place the valve template"), transitions={'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.High, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:473 y:72 OperatableStateMachine.add('Prepare_Manipulation', _sm_prepare_manipulation_9, transitions={'finished': 'Manipulate_Valve', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'grasp_preference': 'default_preference', 'hand_side': 'hand_side', 'none': 'none'}) # x:237 y:28 OperatableStateMachine.add('Decide_Walking', OperatorDecisionState(outcomes=['walk', 'skip'], hint="Walk to the valve?", suggestion='skip'), transitions={'walk': 'Walk_To_Template', 'skip': 'Prepare_Manipulation'}, autonomy={'walk': Autonomy.Full, 'skip': Autonomy.Low}) # x:644 y:557 OperatableStateMachine.add('Step_Back', _sm_step_back_8, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'distance': 'step_back_distance'}) # x:637 y:428 OperatableStateMachine.add('Decide_Step_Back', OperatorDecisionState(outcomes=["stand", "step_back"], hint="Should the robot step back from the valve?", suggestion="step_back"), transitions={'stand': 'finished', 'step_back': 'Step_Back'}, autonomy={'stand': Autonomy.Full, 'step_back': Autonomy.High}) # x:738 y:228 OperatableStateMachine.add('Inform_About_Stand', LogState(text="Back to STAND", severity=Logger.REPORT_INFO), transitions={'done': 'Go_To_Stand'}, autonomy={'done': Autonomy.Low}) # x:733 y:72 OperatableStateMachine.add('Manipulate_Valve', _sm_manipulate_valve_7, transitions={'finished': 'Inform_About_Stand', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'default_preference', 'none': 'none'}) # x:616 y:328 OperatableStateMachine.add('Go_To_Stand', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND), transitions={'changed': 'Decide_Step_Back', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:230 y:122 OperatableStateMachine.add('Walk_To_Template', _sm_walk_to_template_6, transitions={'finished': 'Prepare_Manipulation', 'failed': 'failed', 'aborted': 'Prepare_Manipulation'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'grasp_preference': 'default_preference', 'hand_side': 'hand_side'}) return _state_machine
def create(self): # x:869 y:327, x:226 y:285 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:70 y:38 OperatableStateMachine.add( 'EStop', TimedStopState(timeout=0.25, cmd_topic='stamped_cmd_vel_mux/input/navi', odom_topic='mobile_base/odom'), transitions={ 'done': 'BackupQuery', 'failed': 'EStopFailure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) # x:810 y:144 OperatableStateMachine.add('Clear Costmap', ClearCostmapsState(costmap_topics=[ 'high_level_planner/clear_costmap', 'low_level_planner/clear_costmap' ], timeout=5.0), transitions={ 'done': 'Cleared', 'failed': 'CostmapFailure' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) # x:75 y:225 OperatableStateMachine.add('EStopFailure', LogState(text="Emergency stop failure", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:309 y:213 OperatableStateMachine.add('CostmapFailure', LogState( text="Failed to clear costmaps", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:617 y:40 OperatableStateMachine.add( 'ShortBackup', MoveDistanceState(target_time=2.0, distance=-0.175, cmd_topic='stamped_cmd_vel_mux/input/navi', odometry_topic='mobile_base/odom'), transitions={ 'done': 'ClearCostmapQuery', 'failed': 'ClearCostmapQuery' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Low }) # x:447 y:257 OperatableStateMachine.add( 'ClearCostmapQuery', OperatorDecisionState(outcomes=["yes", "no"], hint="Do you wish to clear costmaps?", suggestion="no"), transitions={ 'yes': 'Clear Costmap', 'no': 'SkipClearCostmap' }, autonomy={ 'yes': Autonomy.Full, 'no': Autonomy.High }) # x:647 y:319 OperatableStateMachine.add('SkipClearCostmap', LogState(text="Do not clear costmaps", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:842 y:237 OperatableStateMachine.add('Cleared', LogState(text="Cleared the costmaps", severity=Logger.REPORT_INFO), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:257 y:40 OperatableStateMachine.add('BackupQuery', OperatorDecisionState( outcomes=["long", "short", "no"], hint="Backup?", suggestion="short"), transitions={ 'no': 'ClearCostmapQuery', 'long': 'LongBackup', 'short': 'ShortBackup' }, autonomy={ 'no': Autonomy.Full, 'long': Autonomy.Full, 'short': Autonomy.High }) # x:459 y:81 OperatableStateMachine.add( 'LongBackup', MoveDistanceState(target_time=4.0, distance=-0.35, cmd_topic='stamped_cmd_vel_mux/input/navi', odometry_topic='mobile_base/odom'), transitions={ 'done': 'ClearCostmapQuery', 'failed': 'ClearCostmapQuery' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Low }) return _state_machine
def create(self): box_part_1_affordance = "open_part_1" box_part_2_affordance = "open_part_2" # x:211 y:659, x:307 y:517 _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.step_back_distance = 1.0 # meters _state_machine.userdata.hand_side_push = self.hand_side_push # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:758 y:166, x:513 y:45 _sm_back_to_pregrasp_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'grasp_preference']) with _sm_back_to_pregrasp_0: # x:33 y:49 OperatableStateMachine.add('Get_Pregrasp_Info', 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': 'grasp_preference', 'grasp': 'grasp_pose'}) # x:40 y:293 OperatableStateMachine.add('Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_To_Pregrasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints'}) # x:242 y:292 OperatableStateMachine.add('Plan_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': 'Get_Pregrasp_Info'}, autonomy={'planned': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:526 y:228 OperatableStateMachine.add('Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'Get_Pregrasp_Info'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # 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:634 y:104, x:343 y:102, x:1013 y:188 _sm_go_to_grasp_1 = OperatableStateMachine(outcomes=['finished', 'failed', 'again'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference']) with _sm_go_to_grasp_1: # x:33 y:49 OperatableStateMachine.add('Get_Grasp_Info', GetTemplateGraspState(), transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, 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.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, 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': 'finished', 'failed': 'Decide_Which_Grasp'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # 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:727 y:50 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.Full}) # x:772 y:165, x:514 y:83 _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:83 y:68 OperatableStateMachine.add('Get_Pregrasp_Info', GetTemplatePregraspState(), transitions={'done': 'Plan_To_Pregrasp_Pose', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pregrasp_pose'}) # x:531 y:294 OperatableStateMachine.add('Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'Decide_Which_Pregrasp'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, 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:314 y:219 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.High, 'failed': Autonomy.Full}, remapping={'target_pose': 'pregrasp_pose', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory'}) # x:309 y:395 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:365, x:130 y:365 _sm_perform_step_back_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['step_back_distance']) with _sm_perform_step_back_3: # 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:722 y:376, x:130 y:365 _sm_push_button_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'grasp_preference']) with _sm_push_button_4: # x:94 y:57 OperatableStateMachine.add('Go_to_Pregrasp', _sm_go_to_pregrasp_2, transitions={'finished': 'Go_to_Grasp', 'failed': 'failed'}, 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:362 y:206 OperatableStateMachine.add('Go_to_Grasp', _sm_go_to_grasp_1, transitions={'finished': 'Back_to_Pregrasp', 'failed': 'failed', 'again': 'Go_to_Pregrasp'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'again': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id'}) # x:619 y:282 OperatableStateMachine.add('Back_to_Pregrasp', _sm_back_to_pregrasp_0, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference'}) # x:1187 y:28, x:856 y:73 _sm_open_box_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'none']) with _sm_open_box_5: # x:93 y:98 OperatableStateMachine.add('Grasp_Box', self.use_behavior(GraspObjectSM, 'Open_Box/Grasp_Box'), transitions={'finished': 'Get_Open_Part_1_Affordance', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id'}) # x:275 y:250 OperatableStateMachine.add('Plan_Open_Part_1_Affordance', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Open_Part_1_Affordance', 'incomplete': 'Execute_Open_Part_1_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'affordance': 'box_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:280 y:331 OperatableStateMachine.add('Execute_Open_Part_1_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Get_Open_Part_2_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:274 y:178 OperatableStateMachine.add('Get_Open_Part_1_Affordance', GetTemplateAffordanceState(identifier=box_part_1_affordance), transitions={'done': 'Plan_Open_Part_1_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': 'box_affordance'}) # x:500 y:462 OperatableStateMachine.add('Plan_Open_Part_2_Affordance', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Open_Part_2_Affordance', 'incomplete': 'Execute_Open_Part_2_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'affordance': 'box_affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:728 y:461 OperatableStateMachine.add('Execute_Open_Part_2_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Open_Fingers', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:365 y:396 OperatableStateMachine.add('Get_Open_Part_2_Affordance', GetTemplateAffordanceState(identifier=box_part_2_affordance), transitions={'done': 'Plan_Open_Part_2_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': 'box_affordance'}) # x:964 y:433 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=0), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'hand_side': 'hand_side'}) with _state_machine: # x:73 y:78 OperatableStateMachine.add('Request_Box_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place Box 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:330 y:172 OperatableStateMachine.add('Walk_To_Template', self.use_behavior(WalktoTemplateSM, 'Walk_To_Template'), transitions={'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': '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:566 y:78 OperatableStateMachine.add('Set_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Open_Box', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:794 y:72 OperatableStateMachine.add('Open_Box', _sm_open_box_5, transitions={'finished': 'Warn_Stand_Pose_1', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'none': 'none'}) # x:646 y:365 OperatableStateMachine.add('Go_To_Stand_Pose_1', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Request_Button_Template', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'side': 'none'}) # x:642 y:449 OperatableStateMachine.add('Request_Button_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place Button template"), transitions={'received': 'Push_Button', '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:761 y:512 OperatableStateMachine.add('Push_Button', _sm_push_button_4, transitions={'finished': 'Warn_Stand_Pose_2', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side_push', 'template_id': 'template_id', 'grasp_preference': 'grasp_preference'}) # x:788 y:178 OperatableStateMachine.add('Warn_Stand_Pose_1', LogState(text="Going to STAND pose", severity=Logger.REPORT_INFO), transitions={'done': 'Close_Fingers'}, autonomy={'done': Autonomy.High}) # x:679 y:599 OperatableStateMachine.add('Warn_Stand_Pose_2', LogState(text="Going to STAND pose", severity=Logger.REPORT_INFO), transitions={'done': 'Go_To_Stand_Pose_2'}, autonomy={'done': Autonomy.High}) # x:493 y:598 OperatableStateMachine.add('Go_To_Stand_Pose_2', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Decide_Step_Back', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'side': 'none'}) # x:587 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:327 y:672 OperatableStateMachine.add('Perform_Step_Back', _sm_perform_step_back_3, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'step_back_distance': 'step_back_distance'}) # x:782 y:278 OperatableStateMachine.add('Close_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=1), transitions={'done': 'Go_To_Stand_Pose_1', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'hand_side': 'hand_side'}) 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): # 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): waypoint_distance = 1 # x:92 y:394, x:775 y:176 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.none = None _state_machine.userdata.waypoint = PoseStamped() # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:105 y:518, x:331 y:110, x:346 y:384 _sm_process_waypoint_0 = OperatableStateMachine( outcomes=['finished', 'waypoint_failure', 'data_failure'], input_keys=['waypoint']) with _sm_process_waypoint_0: # x:58 y:44 OperatableStateMachine.add( 'Plan_To_Waypoint', PlanFootstepsState(mode=PlanFootstepsState.MODE_STEP_2D, pose_is_pelvis=False), transitions={ 'planned': 'Perform_Walking', 'failed': 'waypoint_failure' }, autonomy={ 'planned': Autonomy.Low, 'failed': Autonomy.Low }, remapping={ 'target_pose': 'waypoint', 'footstep_plan': 'footstep_plan' }) # x:40 y:154 OperatableStateMachine.add( 'Perform_Walking', ExecuteStepPlanActionState(), transitions={ 'finished': 'Take_Camera_Image', 'failed': 'waypoint_failure' }, autonomy={ 'finished': Autonomy.High, 'failed': Autonomy.Low }, remapping={'footstep_plan': 'footstep_plan'}) # x:42 y:376 OperatableStateMachine.add('Send_Image_To_Operator', SendToOperatorState(), transitions={ 'done': 'finished', 'no_connection': 'data_failure' }, autonomy={ 'done': Autonomy.Low, 'no_connection': Autonomy.Low }, remapping={'data': 'camera_img'}) # x:53 y:266 OperatableStateMachine.add( 'Take_Camera_Image', GetCameraImageState(), transitions={'done': 'Send_Image_To_Operator'}, autonomy={'done': Autonomy.Off}, remapping={'camera_img': 'camera_img'}) # x:190 y:266 OperatableStateMachine.add( 'Take_Laser_Scan', GetLaserscanState(), transitions={'done': 'Send_Image_To_Operator'}, autonomy={'done': Autonomy.Off}, remapping={'laserscan': 'laserscan'}) # x:30 y:365 _sm_generate_waypoint_1 = OperatableStateMachine( outcomes=['finished'], input_keys=['waypoint', 'none'], output_keys=['waypoint_out']) with _sm_generate_waypoint_1: # x:32 y:50 OperatableStateMachine.add( 'Init_Radius', CalculationState(calculation=lambda x: 0.2), transitions={'done': 'Calculate_Next_Pose'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'none', 'output_value': 'sdv' }) # x:173 y:129 OperatableStateMachine.add( 'Calculate_Next_Pose', FlexibleCalculationState(calculation=self.calc_pose, input_keys=["pose", "radius"]), transitions={'done': 'Determine_Waypoint_Valid'}, autonomy={'done': Autonomy.Off}, remapping={ 'pose': 'waypoint', 'radius': 'sdv', 'output_value': 'waypoint_out' }) # x:162 y:260 OperatableStateMachine.add( 'Determine_Waypoint_Valid', PlanFootstepsState(mode=PlanFootstepsState.MODE_STEP_2D, pose_is_pelvis=False), transitions={ 'planned': 'finished', 'failed': 'Increase_Radius' }, autonomy={ 'planned': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'target_pose': 'waypoint_out', 'footstep_plan': 'footstep_plan' }) # x:299 y:193 OperatableStateMachine.add( 'Increase_Radius', CalculationState(calculation=lambda s: s + 0.1), transitions={'done': 'Calculate_Next_Pose'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'sdv', 'output_value': 'sdv' }) with _state_machine: # x:33 y:45 OperatableStateMachine.add( 'Generate_Waypoint', _sm_generate_waypoint_1, transitions={'finished': 'Process_Waypoint'}, autonomy={'finished': Autonomy.Inherit}, remapping={ 'waypoint': 'waypoint', 'none': 'none', 'waypoint_out': 'waypoint' }) # x:726 y:51 OperatableStateMachine.add( 'Abort_After_Failure', OperatorDecisionState( outcomes=["continue", "abort"], hint="Continue with next position regardless of failure?", suggestion="continue"), transitions={ 'continue': 'Generate_Waypoint', 'abort': 'failed' }, autonomy={ 'continue': Autonomy.Full, 'abort': Autonomy.Full }) # x:513 y:282 OperatableStateMachine.add( 'Log_Data_Failure', LogState(text="Failed to send captured data!", severity=Logger.REPORT_WARN), transitions={'done': 'Abort_After_Failure'}, autonomy={'done': Autonomy.Off}) # x:253 y:178 OperatableStateMachine.add('Process_Waypoint', _sm_process_waypoint_0, transitions={ 'finished': 'Check_Finished', 'waypoint_failure': 'Log_Waypoint_Failure', 'data_failure': 'Log_Data_Failure' }, autonomy={ 'finished': Autonomy.Inherit, 'waypoint_failure': Autonomy.Inherit, 'data_failure': Autonomy.Inherit }, remapping={'waypoint': 'waypoint'}) # x:42 y:284 OperatableStateMachine.add('Check_Finished', OperatorDecisionState( outcomes=["finished", "next"], hint="Going to next position?", suggestion="next"), transitions={ 'finished': 'finished', 'next': 'Generate_Waypoint' }, autonomy={ 'finished': Autonomy.Full, 'next': Autonomy.High }) # x:505 y:120 OperatableStateMachine.add( 'Log_Waypoint_Failure', LogState(text="Failed to walk to next waypoint!", severity=Logger.REPORT_WARN), transitions={'done': 'Abort_After_Failure'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:833 y:90, x:583 y:490 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:37 y:78 OperatableStateMachine.add( 'Ask_If_Wide_Stance', OperatorDecisionState(outcomes=['wide_stance', 'just_stand'], hint="Do you want to go to wide stance?", suggestion='wide_stance'), transitions={ 'wide_stance': 'Plan_To_Wide_Stance', 'just_stand': 'Set_Manipulate' }, autonomy={ 'wide_stance': Autonomy.Low, 'just_stand': Autonomy.Full }) # x:274 y:278 OperatableStateMachine.add( 'Go_To_Wide_Stance', ExecuteStepPlanActionState(), transitions={ 'finished': 'Wait_For_Stand', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'footstep_plan': 'plan_wide_stance'}) # x:19 y:278 OperatableStateMachine.add( 'Plan_To_Wide_Stance', FootstepPlanWideStanceState(), transitions={ 'planned': 'Go_To_Wide_Stance', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'footstep_plan': 'plan_wide_stance'}) # x:566 y:78 OperatableStateMachine.add( 'Set_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'finished', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Low }) # x:266 y:178 OperatableStateMachine.add( 'Wait_For_Stand', CheckCurrentControlModeState( target_mode=CheckCurrentControlModeState.STAND, wait=True), transitions={ 'correct': 'Set_Manipulate', 'incorrect': 'failed' }, autonomy={ 'correct': Autonomy.Low, 'incorrect': Autonomy.Full }, remapping={'control_mode': 'control_mode'}) return _state_machine
def create(self): # x:111 y:265, x:563 y:696 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.distance = 2 # m _state_machine.userdata.angle = 60 # deg # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:63 y:118 OperatableStateMachine.add('Where_Should_I_Go?', OperatorDecisionState(outcomes=[ 'walk', 'turn', 'special', 'done' ], hint=None, suggestion=None), transitions={ 'walk': 'Decide_Walking_Direction', 'turn': 'Decide_Turning_Direction', 'special': 'Decide_Special_Footstep_Plan', 'done': 'finished' }, autonomy={ 'walk': Autonomy.Off, 'turn': Autonomy.Off, 'special': Autonomy.Off, 'done': Autonomy.Off }) # x:280 y:112 OperatableStateMachine.add('Decide_Walking_Direction', OperatorDecisionState(outcomes=[ 'forward', 'backward', 'left', 'right' ], hint=None, suggestion=None), transitions={ 'forward': 'Walk_Forward', 'backward': 'Walk_Backward', 'left': 'Walk_Left', 'right': 'Walk_Right' }, autonomy={ 'forward': Autonomy.Off, 'backward': Autonomy.Off, 'left': Autonomy.Off, 'right': Autonomy.Off }) # x:288 y:321 OperatableStateMachine.add('Decide_Turning_Direction', OperatorDecisionState( outcomes=['left', 'right'], hint=None, suggestion=None), transitions={ 'left': 'Turn_Left', 'right': 'Turn_Right' }, autonomy={ 'left': Autonomy.Off, 'right': Autonomy.Off }) # x:271 y:503 OperatableStateMachine.add('Decide_Special_Footstep_Plan', OperatorDecisionState( outcomes=['widestance', 'realign'], hint=None, suggestion=None), transitions={ 'widestance': 'Wide_Stance', 'realign': 'Realign' }, autonomy={ 'widestance': Autonomy.Off, 'realign': Autonomy.Off }) # x:506 y:18 OperatableStateMachine.add( 'Walk_Forward', FootstepPlanRelativeState( direction=FootstepPlanRelativeState.DIRECTION_FORWARD), transitions={ 'planned': 'Execute_Plan', 'failed': 'Turn_Right' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Off }, remapping={ 'distance': 'distance', 'footstep_plan': 'footstep_plan' }) # x:506 y:85 OperatableStateMachine.add( 'Walk_Backward', FootstepPlanRelativeState( direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={ 'planned': 'Execute_Plan', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Off }, remapping={ 'distance': 'distance', 'footstep_plan': 'footstep_plan' }) # x:506 y:151 OperatableStateMachine.add( 'Walk_Left', FootstepPlanRelativeState( direction=FootstepPlanRelativeState.DIRECTION_LEFT), transitions={ 'planned': 'Execute_Plan', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Off }, remapping={ 'distance': 'distance', 'footstep_plan': 'footstep_plan' }) # x:507 y:215 OperatableStateMachine.add( 'Walk_Right', FootstepPlanRelativeState( direction=FootstepPlanRelativeState.DIRECTION_RIGHT), transitions={ 'planned': 'Execute_Plan', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Off }, remapping={ 'distance': 'distance', 'footstep_plan': 'footstep_plan' }) # x:511 y:297 OperatableStateMachine.add( 'Turn_Left', FootstepPlanTurnState( direction=FootstepPlanTurnState.TURN_LEFT), transitions={ 'planned': 'Execute_Plan', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Off }, remapping={ 'angle': 'angle', 'footstep_plan': 'footstep_plan' }) # x:511 y:365 OperatableStateMachine.add( 'Turn_Right', FootstepPlanTurnState( direction=FootstepPlanTurnState.TURN_RIGHT), transitions={ 'planned': 'Execute_Plan', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Off }, remapping={ 'angle': 'angle', 'footstep_plan': 'footstep_plan' }) # x:493 y:464 OperatableStateMachine.add( 'Wide_Stance', FootstepPlanWideStanceState(), transitions={ 'planned': 'Execute_Plan', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Off }, remapping={'footstep_plan': 'footstep_plan'}) # x:492 y:532 OperatableStateMachine.add( 'Realign', FootstepPlanRealignCenterState(), transitions={ 'planned': 'Execute_Plan', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Off }, remapping={'footstep_plan': 'footstep_plan'}) # x:856 y:257 OperatableStateMachine.add( 'Execute_Plan', ExecuteStepPlanActionState(), transitions={ 'finished': 'Where_Should_I_Go?', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'footstep_plan': 'footstep_plan'}) return _state_machine
def create(self): # x:533 y:590, x:583 y:140 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.none = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:66 y:78 OperatableStateMachine.add( 'Check_Manipulate', CheckCurrentControlModeState( target_mode=CheckCurrentControlModeState.MANIPULATE, wait=False), transitions={ 'correct': 'Set_Stand_Posture', 'incorrect': 'Set_Manipulate' }, autonomy={ 'correct': Autonomy.Low, 'incorrect': Autonomy.Low }, remapping={'control_mode': 'control_mode'}) # x:266 y:128 OperatableStateMachine.add( 'Set_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'Set_Stand_Posture', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Low }) # x:76 y:228 OperatableStateMachine.add( 'Set_Stand_Posture', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.2, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'Set_Stand', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'side': 'none'}) # x:333 y:378 OperatableStateMachine.add( 'Realign_Feet_Decision', OperatorDecisionState( outcomes=["realign", "just_stand"], hint="Need to realign the feet after wide stance?", suggestion="realign"), transitions={ 'realign': 'Plan_Realign', 'just_stand': 'finished' }, autonomy={ 'realign': Autonomy.Low, 'just_stand': Autonomy.Full }) # x:464 y:278 OperatableStateMachine.add( 'Plan_Realign', FootstepPlanRealignCenterState(), transitions={ 'planned': 'Execute_Realign', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'plan_header': 'plan_header'}) # x:524 y:378 OperatableStateMachine.add( 'Execute_Realign', ExecuteStepPlanActionState(), transitions={ 'finished': 'Wait_For_Stand', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'plan_header': 'plan_header'}) # x:66 y:328 OperatableStateMachine.add( 'Set_Stand', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'Realign_Feet_Decision', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Low }) # x:766 y:378 OperatableStateMachine.add( 'Wait_For_Stand', CheckCurrentControlModeState( target_mode=CheckCurrentControlModeState.STAND, wait=False), transitions={ 'correct': 'finished', 'incorrect': 'failed' }, autonomy={ 'correct': Autonomy.Low, 'incorrect': Autonomy.Full }, remapping={'control_mode': 'control_mode'}) return _state_machine
def create(self): joy_topic = '/hmi/joystick' # x:410 y:173, x:436 y:319 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.head_pose_joints = [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] _state_machine.userdata.rand_head_config = { 'min2356': [-0.5, 0.1, -1.0, -1.0], 'max2356': [0.5, 0.5, 1.0, 1.0] } # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:322 y:22 OperatableStateMachine.add( 'SelectPart', OperatorDecisionState(outcomes=[ 'history', 'construction', 'electronics', 'software', 'ending' ], hint='Select the part of presentation.', suggestion='history'), transitions={ 'history': 'WatchPresentaion', 'construction': 'WatchPresentaion_2', 'electronics': 'WatchPresentaion_3', 'software': 'WatchPresentaion_4', 'ending': 'ImHere' }, autonomy={ 'history': Autonomy.Full, 'construction': Autonomy.Full, 'electronics': Autonomy.Full, 'software': Autonomy.Full, 'ending': Autonomy.Full }) # x:580 y:313 OperatableStateMachine.add( 'NewBody', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1= 'thank_you_mutronics_then_renha_and_shiron_will_tell_about_my_future_body_proto3', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='look_on_printer_fast', t3=[0, 1.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WatchPresentaion_3', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:174 y:592 OperatableStateMachine.add( 'Continue', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1= 'but_we_have_to_continue_the_next_topic_is_my_electronic', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='head_node', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WatchPresentaion_4', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:44 y:382 OperatableStateMachine.add( 'Software', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1= 'thank_you_zuviel_i_really_need_the_new_and_more_powerful_on_board_computer', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='look_on_printer', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'finished', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:36 y:163 OperatableStateMachine.add('WaitKey13', WaitForMessageState( topic=joy_topic, condition=lambda x: x.buttons[12], buffered=False, clear=False), transitions={ 'received': 'TurnOff', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:31 y:243 OperatableStateMachine.add( 'ImHere', CompoundAction( t1=[0, 0.0], type1='motion/joint_trajectory', cmd1='look_on_hoof', t2=[0, 6.0], type2='voice/play_wav', cmd2='hello_im_here_again_but_youre_monster_anyway', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WaitKey13', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:37 y:83 OperatableStateMachine.add( 'TurnOff', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1= 'thank_you_for_your_questions_and_attention_our_presentation_terminates_here', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='bow_begin', t3=[0, 3.0], type3='motion/joint_trajectory', cmd3='bow_end', t4=[0, 4.5], type4='motion/joint_trajectory', cmd4='prance'), transitions={ 'success': 'finished', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:391 y:593 OperatableStateMachine.add( 'Interesting', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1= 'its_very_interesting_i_cant_wait_when_my_data_can_be_transferred_inside_my_new_body', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='head_lean_forward_begin', t3=[0, 1.0], type3='motion/joint_trajectory', cmd3='head_suprised', t4=[0, 2.5], type4='motion/joint_trajectory', cmd4='head_lean_forward_end'), transitions={ 'success': 'Continue', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:561 y:20 OperatableStateMachine.add( 'WatchPresentaion', self.use_behavior(WatchPresentaionSM, 'WatchPresentaion'), transitions={ 'finished': 'Chances', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'head_pose_joints': 'head_pose_joints', 'rand_head_config': 'rand_head_config' }) # x:557 y:211 OperatableStateMachine.add( 'WatchPresentaion_2', self.use_behavior(WatchPresentaionSM, 'WatchPresentaion_2'), transitions={ 'finished': 'NewBody', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'head_pose_joints': 'head_pose_joints', 'rand_head_config': 'rand_head_config' }) # x:549 y:436 OperatableStateMachine.add( 'WatchPresentaion_3', self.use_behavior(WatchPresentaionSM, 'WatchPresentaion_3'), transitions={ 'finished': 'Interesting', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'head_pose_joints': 'head_pose_joints', 'rand_head_config': 'rand_head_config' }) # x:49 y:468 OperatableStateMachine.add( 'WatchPresentaion_4', self.use_behavior(WatchPresentaionSM, 'WatchPresentaion_4'), transitions={ 'finished': 'Software', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'head_pose_joints': 'head_pose_joints', 'rand_head_config': 'rand_head_config' }) # x:581 y:118 OperatableStateMachine.add( 'Chances', CompoundAction(t1=[0, 0.0], type1='voice/play_wav', cmd1='the_chances_of_success_are_100_percent', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='little_shake_fast', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WatchPresentaion_2', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) return _state_machine
def create(self): l_random = [-2.47557862791, -1.21674644795, -0.761510070214, -0.219080422969, -2.68076430953, 0.189685935246, -3.8825693651] r_random = [1.10260663602, 2.67164332545, 3.1286914453, 1.52042152218, -1.94191336135, 0.805351035619, -3.64437671794] # x:33 y:340, x:435 y:356 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.joint_values = [0] * 7 _state_machine.userdata.l_random = l_random _state_machine.userdata.r_random = r_random _state_machine.userdata.random = l_random if self.planning_group == "l_arm_group" else r_random _state_machine.userdata.hand_side = 'left' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:151 y:30 OperatableStateMachine.add('Set_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Go_To_Stand', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.Off}) # x:65 y:513 OperatableStateMachine.add('Turn_Torso_Left_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.TURN_TORSO_LEFT_POSE, vel_scaling=0.3, ignore_collisions=True, link_paddings={}), transitions={'done': 'Go_To_Open_Door_Pose', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:312 y:565 OperatableStateMachine.add('Go_To_Open_Door_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.DOOR_OPEN_POSE_STRAIGHT, vel_scaling=0.1, ignore_collisions=False, link_paddings={}), transitions={'done': 'Back_To_Door_Ready_Pose', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:647 y:328 OperatableStateMachine.add('Go_To_Final_Stand', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={'done': 'Ask_If_Open', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:622 y:189 OperatableStateMachine.add('Ask_If_Open', OperatorDecisionState(outcomes=['open','push_again'], hint='Is the door open?', suggestion='open'), transitions={'open': 'finished', 'push_again': 'Log_Try_Push_Again'}, autonomy={'open': Autonomy.High, 'push_again': Autonomy.Full}) # x:355 y:189 OperatableStateMachine.add('Log_Try_Push_Again', LogState(text='Move robot closer to door', severity=Logger.REPORT_HINT), transitions={'done': 'Go_To_Door_Ready_Pose'}, autonomy={'done': Autonomy.Full}) # x:623 y:452 OperatableStateMachine.add('Turn_Torso_Center_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.TURN_TORSO_CENTER_POSE, vel_scaling=0.3, ignore_collisions=True, link_paddings={}), transitions={'done': 'Go_To_Final_Stand', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:126 y:316 OperatableStateMachine.add('Go_To_Door_Ready_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={'done': 'Go_To_Open_Door_Pose', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:100 y:203 OperatableStateMachine.add('Go_To_Stand', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.5, ignore_collisions=False, link_paddings={}), transitions={'done': 'Go_To_Door_Ready_Pose', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:551 y:580 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': 'Go_To_Final_Stand', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) return _state_machine
def create(self): # x:807 y:293, x:1163 y:18 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:35 y:227, x:161 y:228, x:363 y:243, x:536 y:22, x:472 y:241, x:547 y:181, x:544 y:99, x:674 y:184, x:263 y:228 _sm_container_0 = ConcurrencyContainer( outcomes=['finished', 'failed', 'danger', 'preempted'], input_keys=['plan'], conditions=[('failed', [('DWA', 'failed')]), ('finished', [('DWA', 'done')]), ('preempted', [('DWA', 'preempted')]), ('danger', [('Safety', 'cliff')]), ('danger', [('Safety', 'bumper')])]) with _sm_container_0: # x:101 y:78 OperatableStateMachine.add( 'DWA', FollowPathState(topic="low_level_planner"), transitions={ 'done': 'finished', 'failed': 'failed', 'preempted': 'preempted' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off, 'preempted': Autonomy.Off }, remapping={'plan': 'plan'}) # x:343 y:91 OperatableStateMachine.add( 'Safety', TurtlebotStatusState(bumper_topic='mobile_base/events/bumper', cliff_topic='mobile_base/events/cliff'), transitions={ 'bumper': 'danger', 'cliff': 'danger' }, autonomy={ 'bumper': Autonomy.Off, 'cliff': Autonomy.Off }) with _state_machine: # x:193 y:26 OperatableStateMachine.add('ClearCostmap', ClearCostmapsState(costmap_topics=[ 'high_level_planner/clear_costmap', 'low_level_planner/clear_costmap' ], timeout=5.0), transitions={ 'done': 'Receive Goal', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) # x:205 y:207 OperatableStateMachine.add( 'Receive Path', GetPathState(planner_topic="high_level_planner"), transitions={ 'planned': 'ExecutePlan', 'empty': 'Continue', 'failed': 'Continue' }, autonomy={ 'planned': Autonomy.Off, 'empty': Autonomy.Low, 'failed': Autonomy.Low }, remapping={ 'goal': 'goal', 'plan': 'plan' }) # x:194 y:301 OperatableStateMachine.add('ExecutePlan', OperatorDecisionState( outcomes=["yes", "no"], hint="Execute the current plan?", suggestion="yes"), transitions={ 'yes': 'Container', 'no': 'Continue' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full }) # x:446 y:275 OperatableStateMachine.add('Container', _sm_container_0, transitions={ 'finished': 'Log Success', 'failed': 'AutoReplan', 'danger': 'EStop', 'preempted': 'Continue' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'danger': Autonomy.Inherit, 'preempted': Autonomy.Inherit }, remapping={'plan': 'plan'}) # x:435 y:146 OperatableStateMachine.add( 'Continue', OperatorDecisionState( outcomes=["yes", "no", "recover", "clearcostmap"], hint="Continue planning to new goal?", suggestion="yes"), transitions={ 'yes': 'Receive Goal', 'no': 'finished', 'recover': 'LogRecovery', 'clearcostmap': 'ClearCostmap' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full, 'recover': Autonomy.Full, 'clearcostmap': Autonomy.Full }) # x:1052 y:227 OperatableStateMachine.add('Turtlebot Simple Recovery', self.use_behavior( TurtlebotSimpleRecoverySM, 'Turtlebot Simple Recovery'), transitions={ 'finished': 'AutoReplan', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:664 y:307 OperatableStateMachine.add('Log Success', LogState(text="Success!", severity=Logger.REPORT_HINT), transitions={'done': 'initialPub'}, autonomy={'done': Autonomy.Off}) # x:664 y:374 OperatableStateMachine.add('Log Fail', LogState(text="Path execution failure", severity=Logger.REPORT_HINT), transitions={'done': 'Recover'}, autonomy={'done': Autonomy.Off}) # x:960 y:70 OperatableStateMachine.add('Log Recovered', LogState(text="Re-plan after recovery", severity=Logger.REPORT_HINT), transitions={'done': 'New Plan'}, autonomy={'done': Autonomy.Off}) # x:772 y:71 OperatableStateMachine.add( 'New Plan', GetPathState(planner_topic="high_level_planner"), transitions={ 'planned': 'Container', 'empty': 'Receive Goal', 'failed': 'Continue' }, autonomy={ 'planned': Autonomy.Off, 'empty': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'goal': 'goal', 'plan': 'plan' }) # x:203 y:113 OperatableStateMachine.add( 'Receive Goal', GetPoseState(topic='flex_nav_global/goal'), transitions={'done': 'Receive Path'}, autonomy={'done': Autonomy.Low}, remapping={'goal': 'goal'}) # x:866 y:374 OperatableStateMachine.add('Recover', OperatorDecisionState( outcomes=["yes", "no"], hint="Should we attempt recovery?", suggestion="yes"), transitions={ 'yes': 'LogRecovery', 'no': 'finished' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full }) # x:887 y:236 OperatableStateMachine.add( 'LogRecovery', LogState(text="Starting recovery behavior", severity=Logger.REPORT_HINT), transitions={'done': 'Turtlebot Simple Recovery'}, autonomy={'done': Autonomy.Off}) # x:875 y:162 OperatableStateMachine.add('AutoReplan', OperatorDecisionState( outcomes=["yes", "no"], hint="Re-plan to current goal?", suggestion="yes"), transitions={ 'yes': 'Log Recovered', 'no': 'Continue' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full }) # x:453 y:376 OperatableStateMachine.add( 'EStop', TimedStopState(timeout=0.25, cmd_topic='stamped_cmd_vel_mux/input/navi', odom_topic='mobile_base/odom'), transitions={ 'done': 'Log Fail', 'failed': 'Log Fail' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) # x:75 y:417 OperatableStateMachine.add('initialPub', KylePubState( cmd_topic='/makethisupcount', valueToPub=0), transitions={'done': 'getVelocity'}, autonomy={'done': Autonomy.Off}) # x:215 y:471 OperatableStateMachine.add('getVelocity', SubscriberState(topic='/makethisupvel', blocking=True, clear=False), transitions={ 'received': 'getAng', 'unavailable': 'getCount' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'myvelocity'}) # x:402 y:461 OperatableStateMachine.add('getAng', SubscriberState(topic='/makethisupang', blocking=True, clear=False), transitions={ 'received': 'Ball_in_image', 'unavailable': 'getCount' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'angularmy'}) # x:559 y:450 OperatableStateMachine.add( 'Ball_in_image', KyleVerifyState(ValueToMeasureAgainst=7777.0), transitions={ 'verified': 'getCount', 'notVerified': 'move' }, autonomy={ 'verified': Autonomy.Off, 'notVerified': Autonomy.Off }, remapping={ 'inputValueVel': 'myvelocity', 'inputValueAng': 'angularmy' }) # x:706 y:496 OperatableStateMachine.add( 'move', KyleTwistState( cmd_topic='/turtlebot/stamped_cmd_vel_mux/input/navi'), transitions={ 'done': 'ShouldRobotStop', 'getNewMove': 'getVelocity' }, autonomy={ 'done': Autonomy.Off, 'getNewMove': Autonomy.Off }, remapping={ 'input_velocity': 'myvelocity', 'input_rotation_rate': 'angularmy' }) # x:960 y:523 OperatableStateMachine.add('ShouldRobotStop', OperatorDecisionState( outcomes=['yes', 'no'], hint="Should the Robot Stop?", suggestion=None), transitions={ 'yes': 'finished', 'no': 'Continue' }, autonomy={ 'yes': Autonomy.Off, 'no': Autonomy.Off }) # x:658 y:615 OperatableStateMachine.add('getCount', SubscriberState( topic='/makethisupcount', blocking=True, clear=False), transitions={ 'received': 'verifyCount', 'unavailable': 'ShouldRobotStop' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'mycount'}) # x:443 y:679 OperatableStateMachine.add( 'verifyCount', KyleVerifyState(ValueToMeasureAgainst=45), transitions={ 'verified': 'Continue', 'notVerified': 'increaseCount' }, autonomy={ 'verified': Autonomy.Off, 'notVerified': Autonomy.Off }, remapping={ 'inputValueVel': 'mycount', 'inputValueAng': 'mycount' }) # x:242 y:708 OperatableStateMachine.add( 'increaseCount', KylePubInputState(cmd_topic='/makethisupcount', increaseBy=1), transitions={'done': 'rotate'}, autonomy={'done': Autonomy.Off}, remapping={'valueToIncrease': 'mycount'}) # x:75 y:709 OperatableStateMachine.add( 'rotate', TimedTwistState( target_time=.1, velocity=0, rotation_rate=.5, cmd_topic='/turtlebot/stamped_cmd_vel_mux/input/navi'), transitions={'done': 'getVelocity'}, autonomy={'done': 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): pull_affordance = "pull" affordance_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == "left" else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM # 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] # [/MANUAL_CREATE] # x:133 y:340, x:383 y:140 _sm_perform_step_back_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['step_back_distance']) with _sm_perform_step_back_0: # 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_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'none']) with _sm_release_trigger_1: # 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:133 y:540, x:433 y:240 _sm_pull_trigger_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_pull_trigger_2: # x:73 y:78 OperatableStateMachine.add('Get_Pull_Affordance', GetTemplateAffordanceState(identifier=pull_affordance), transitions={'done': 'Plan_Pull', '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:92 y:228 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:76 y:378 OperatableStateMachine.add('Execute_Pull', ExecuteTrajectoryMsgState(controller=affordance_controller), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) 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:330 y:172 OperatableStateMachine.add('Walk_To_Template', self.use_behavior(WalktoTemplateSM, 'Walk_To_Template'), transitions={'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': '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:839 y:72 OperatableStateMachine.add('Grasp Object', self.use_behavior(GraspObjectSM, 'Grasp Object'), transitions={'finished': 'Pull_Trigger', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id'}) # x:844 y:172 OperatableStateMachine.add('Pull_Trigger', _sm_pull_trigger_2, 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:272 OperatableStateMachine.add('Release_Trigger', _sm_release_trigger_1, transitions={'finished': 'Warn_Stand', 'failed': 'Warn_Stand'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'none': 'none'}) # x:826 y:528 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': 'Grasp Object', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:858 y:428 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:816 y:628 OperatableStateMachine.add('Set_Stand', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND), transitions={'changed': 'Decide_Step_Back', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:587 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:327 y:672 OperatableStateMachine.add('Perform_Step_Back', _sm_perform_step_back_0, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'step_back_distance': 'step_back_distance'}) 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): arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM cut_angle = 380 # degrees insert_distance = 0.05 # meters (used for retract) push_distance = 0.08 # meters (for pushing cut piece) # x:950 y:178, x:402 y:195 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.grasp_preference = 0 _state_machine.userdata.tool_template_id = 0 _state_machine.userdata.none = None _state_machine.userdata.center = 'same' _state_machine.userdata.tool_side = 'left' _state_machine.userdata.wall_side = 'right' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._cut_angle = float(cut_angle) self._insert_distance = float(abs(insert_distance)) self._push_distance = float(abs(push_distance)) # [/MANUAL_CREATE] # x:603 y:35, x:95 y:265 _sm_preparation_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['none'], output_keys=['template_id']) with _sm_preparation_0: # x:327 y:29 OperatableStateMachine.add('Request_Template_ID', InputState(request=InputState.SELECTED_OBJECT_ID, message="Provide the ID of the CUTTING TOOL 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:329 y:189 OperatableStateMachine.add('Log_No_Connection', LogState(text="Have no connection to OCS!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:446 y:256 OperatableStateMachine.add('Log_Data_Error', LogState(text="Received wrong data format!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:730 y:464, x:504 y:59 _sm_push_inwards_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'none']) with _sm_push_inwards_1: # x:96 y:53 OperatableStateMachine.add('Get_Insert_Affordance', GetTemplateAffordanceState(identifier='insert'), transitions={'done': 'Set_Insert_Distance', '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': 'affordance'}) # x:101 y:293 OperatableStateMachine.add('Plan_Insert_Affordance', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Insert_Affordance', 'incomplete': 'Execute_Insert_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High}, remapping={'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:448 y:293 OperatableStateMachine.add('Execute_Insert_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Decide_Continue_Pushing', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:449 y:455 OperatableStateMachine.add('Decide_Continue_Pushing', OperatorDecisionState(outcomes=['push_more', 'done'], hint='The drill bit must penetrate the wall completely.', suggestion='done'), transitions={'push_more': 'Plan_Insert_Affordance', 'done': 'finished'}, autonomy={'push_more': Autonomy.Full, 'done': Autonomy.Full}) # x:107 y:167 OperatableStateMachine.add('Set_Insert_Distance', CalculationState(calculation=self.set_push_distance), transitions={'done': 'Plan_Insert_Affordance'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'affordance', 'output_value': 'affordance'}) # x:30 y:365, x:130 y:365 _sm_complete_cut_2 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_complete_cut_2: # x:101 y:144 OperatableStateMachine.add('Placeholder', LogState(text='Complete the cut by moving up or down', severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.High}) # x:917 y:149, x:462 y:284 _sm_push_cut_piece_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_push_cut_piece_3: # x:85 y:53 OperatableStateMachine.add('Get_Wall_Template_Pose', GetTemplatePoseState(), transitions={'done': 'Convert_to_List_of_Poses', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'template_id': 'template_id', 'template_pose': 'template_pose'}) # x:67 y:386 OperatableStateMachine.add('Plan_to_Wall_Center', PlanEndeffectorCartesianWaypointsState(ignore_collisions=False, include_torso=False, keep_endeffector_orientation=True, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Go_to_Wall_Center', 'incomplete': 'Go_to_Wall_Center', 'failed': 'failed'}, 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:84 y:160 OperatableStateMachine.add('Convert_to_List_of_Poses', CalculationState(calculation=lambda pose: [pose.pose]), transitions={'done': 'Prepare_Plan_Frame_Wrist'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'template_pose', 'output_value': 'waypoints'}) # x:86 y:523 OperatableStateMachine.add('Go_to_Wall_Center', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Push_Inwards', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:83 y:268 OperatableStateMachine.add('Prepare_Plan_Frame_Wrist', CalculationState(calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Plan_to_Wall_Center'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'template_pose', 'output_value': 'frame_id'}) # x:591 y:515 OperatableStateMachine.add('Push_Inwards', _sm_push_inwards_1, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'none': 'none'}) # x:709 y:397, x:504 y:96 _sm_insert_drill_bit_in_wall_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'none']) with _sm_insert_drill_bit_in_wall_4: # x:96 y:90 OperatableStateMachine.add('Get_Insert_Affordance', GetTemplateAffordanceState(identifier='insert'), transitions={'done': 'Plan_Insert_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': 'affordance'}) # x:102 y:248 OperatableStateMachine.add('Plan_Insert_Affordance', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Insert_Affordance', 'incomplete': 'Execute_Insert_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High}, remapping={'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:448 y:247 OperatableStateMachine.add('Execute_Insert_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Decide_Continue_Insertion', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:449 y:388 OperatableStateMachine.add('Decide_Continue_Insertion', OperatorDecisionState(outcomes=['insert_more', 'done'], hint='The drill bit must penetrate the wall completely.', suggestion='done'), transitions={'insert_more': 'Plan_Insert_Affordance', 'done': 'finished'}, autonomy={'insert_more': Autonomy.Full, 'done': Autonomy.Full}) # x:806 y:460, x:350 y:132 _sm_retract_drill_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'none']) with _sm_retract_drill_5: # x:33 y:61 OperatableStateMachine.add('Get_Insert_Affordance', GetTemplateAffordanceState(identifier='insert'), transitions={'done': 'Set_Retract_Distance', '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': 'affordance'}) # x:36 y:335 OperatableStateMachine.add('Plan_Retract_Affordance', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Retract_Affordance', 'incomplete': 'Execute_Retract_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High}, remapping={'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:532 y:336 OperatableStateMachine.add('Execute_Retract_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Decide_Continue_Extraction', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:41 y:199 OperatableStateMachine.add('Set_Retract_Distance', CalculationState(calculation=self.set_retract_distance), transitions={'done': 'Plan_Retract_Affordance'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'affordance', 'output_value': 'affordance'}) # x:533 y:450 OperatableStateMachine.add('Decide_Continue_Extraction', OperatorDecisionState(outcomes=['extract_more', 'done'], hint='The drill must exit completely.', suggestion='done'), transitions={'extract_more': 'Plan_Retract_Affordance', 'done': 'finished'}, autonomy={'extract_more': Autonomy.Full, 'done': Autonomy.Full}) # x:953 y:206, x:366 y:223 _sm_cut_circle_in_wall_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'drill_usability']) with _sm_cut_circle_in_wall_6: # x:59 y:56 OperatableStateMachine.add('Get_Cut_Circle_Affordance', GetTemplateAffordanceState(identifier='cut_circle'), transitions={'done': 'Set_Cut_Circle_Angle', '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': 'affordance'}) # x:540 y:57 OperatableStateMachine.add('Plan_Cut_Circle_Affordance', PlanAffordanceState(vel_scaling=0.03, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Cut_Circle_Affordance', 'incomplete': 'Execute_Cut_Circle_Affordance', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High}, remapping={'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'drill_usability', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:533 y:340 OperatableStateMachine.add('Execute_Cut_Circle_Affordance', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Decide_Done_Cutting', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:317 y:57 OperatableStateMachine.add('Set_Cut_Circle_Angle', CalculationState(calculation=self.set_cut_circle_angle), transitions={'done': 'Plan_Cut_Circle_Affordance'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'affordance', 'output_value': 'affordance'}) # x:698 y:199 OperatableStateMachine.add('Decide_Done_Cutting', OperatorDecisionState(outcomes=['cut_more', 'done'], hint='Keep going until a full circle has been cut.', suggestion='cut_more'), transitions={'cut_more': 'Plan_Cut_Circle_Affordance', 'done': 'finished'}, autonomy={'cut_more': Autonomy.High, 'done': Autonomy.High}) # x:860 y:82, x:360 y:283 _sm_hand_in_front_of_wall_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'preference']) with _sm_hand_in_front_of_wall_7: # x:55 y:77 OperatableStateMachine.add('Get_Wall_Pregrasp_Pose', GetTemplatePregraspState(), transitions={'done': 'Plan_to_Wall_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': 'preference', 'pre_grasp': 'pre_grasp'}) # x:579 y:77 OperatableStateMachine.add('Go_to_Wall_Pregrasp', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:316 y:79 OperatableStateMachine.add('Plan_to_Wall_Pregrasp', PlanEndeffectorPoseState(ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Go_to_Wall_Pregrasp', 'failed': 'failed'}, autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High}, remapping={'target_pose': 'pre_grasp', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory'}) # x:788 y:612, x:67 y:501 _sm_walk_to_and_pickup_tool_8 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'grasp_preference', 'template_id', 'none', 'tool_side'], output_keys=['drill_usability']) with _sm_walk_to_and_pickup_tool_8: # x:44 y:72 OperatableStateMachine.add('Preparation', _sm_preparation_0, transitions={'finished': 'Ask_Perform_Walking', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'none': 'none', 'template_id': 'template_id'}) # x:284 y:78 OperatableStateMachine.add('Ask_Perform_Walking', OperatorDecisionState(outcomes=['walk', 'stand'], hint="Does the robot need to walk to the table?", suggestion='walk'), transitions={'walk': 'Walk to Template', 'stand': 'Set_Manipulate'}, autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full}) # x:531 y:22 OperatableStateMachine.add('Walk to Template', self.use_behavior(WalktoTemplateSM, 'Walk_to_and_Pickup_Tool/Walk to Template'), transitions={'finished': 'Set_Manipulate', 'failed': 'Walk_Manually', 'aborted': 'Walk_Manually'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': 'template_id'}) # x:769 y:82 OperatableStateMachine.add('Walk_Manually', LogState(text="Guide the robot to the template manually.", severity=Logger.REPORT_HINT), transitions={'done': 'Set_Manipulate'}, autonomy={'done': Autonomy.Full}) # x:541 y:412 OperatableStateMachine.add('Pickup Object', self.use_behavior(PickupObjectSM, 'Walk_to_and_Pickup_Tool/Pickup Object'), transitions={'finished': 'Optional_Template_Alignment', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id'}) # x:546 y:323 OperatableStateMachine.add('Head_Look_Down', TiltHeadState(desired_tilt=TiltHeadState.DOWN_30), transitions={'done': 'Pickup Object', 'failed': 'Head_Look_Down'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}) # x:518 y:144 OperatableStateMachine.add('Set_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Turn_Torso', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.Full}) # x:528 y:235 OperatableStateMachine.add('Turn_Torso', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.TURN_TORSO_FULL, vel_scaling=0.2, ignore_collisions=True, link_paddings={}, is_cartesian=False), transitions={'done': 'Head_Look_Down', 'failed': 'Turn_Torso'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'side': 'tool_side'}) # x:532 y:603 OperatableStateMachine.add('Get_Drill_bit_Usability', GetTemplateUsabilityState(usability_name='bit', usability_id=100), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'template_id': 'template_id', 'usability_pose': 'drill_usability'}) # x:519 y:513 OperatableStateMachine.add('Optional_Template_Alignment', LogState(text='Request attached template alignment', severity=Logger.REPORT_HINT), transitions={'done': 'Get_Drill_bit_Usability'}, autonomy={'done': Autonomy.High}) # x:458 y:49, x:460 y:203 _sm_optionally_walk_to_wall_9 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['wall_template_id', 'grasp_preference', 'hand_side', 'center']) with _sm_optionally_walk_to_wall_9: # x:67 y:43 OperatableStateMachine.add('Decide_Walking', OperatorDecisionState(outcomes=['walk', 'stay'], hint='Decide whether to walk to the wall or just stay here.', suggestion='stay'), transitions={'walk': 'Retract_Arms', 'stay': 'finished'}, autonomy={'walk': Autonomy.Full, 'stay': Autonomy.High}) # x:642 y:338 OperatableStateMachine.add('Go_to_STAND_MANIPULATE', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND_MANIPULATE), transitions={'changed': 'Walk_to_the_Wall', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.Full}) # x:658 y:43 OperatableStateMachine.add('Walk_to_the_Wall', self.use_behavior(WalktoTemplateSM, 'Optionally_Walk_to_Wall/Walk_to_the_Wall'), transitions={'finished': 'finished', 'failed': 'failed', 'aborted': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': 'wall_template_id'}) # x:61 y:195 OperatableStateMachine.add('Retract_Arms', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.BOTH_ARMS_SIDES, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Center_Torso', 'failed': 'Retract_Arms'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'side': 'center'}) # x:62 y:333 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': 'Go_to_STAND_MANIPULATE', 'failed': 'Center_Torso'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'side': 'center'}) # x:176 y:449, x:303 y:229 _sm_cut_circle_in_wall_10 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'preference', 'none', 'drill_usability']) with _sm_cut_circle_in_wall_10: # x:90 y:54 OperatableStateMachine.add('Hand_in_front_of_Wall', _sm_hand_in_front_of_wall_7, transitions={'finished': 'Insert_Drill_bit_in_Wall', 'failed': 'Retry_Cutting'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'preference'}) # x:774 y:56 OperatableStateMachine.add('Cut_Circle_in_Wall', _sm_cut_circle_in_wall_6, transitions={'finished': 'Retract_Drill', 'failed': 'Retry_Cutting'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'drill_usability': 'drill_usability'}) # x:791 y:213 OperatableStateMachine.add('Retract_Drill', _sm_retract_drill_5, transitions={'finished': 'Complete_Cut', 'failed': 'Retry_Cutting'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'none': 'none'}) # x:458 y:224 OperatableStateMachine.add('Retry_Cutting', OperatorDecisionState(outcomes=['fail', 'pregrasp', 'insert', 'cut'], hint='Decide if and how to retry', suggestion=None), transitions={'fail': 'failed', 'pregrasp': 'Hand_in_front_of_Wall', 'insert': 'Insert_Drill_bit_in_Wall', 'cut': 'Cut_Circle_in_Wall'}, autonomy={'fail': Autonomy.Full, 'pregrasp': Autonomy.High, 'insert': Autonomy.High, 'cut': Autonomy.High}) # x:445 y:56 OperatableStateMachine.add('Insert_Drill_bit_in_Wall', _sm_insert_drill_bit_in_wall_4, transitions={'finished': 'Cut_Circle_in_Wall', 'failed': 'Retry_Cutting'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'none': 'none'}) # x:457 y:436 OperatableStateMachine.add('Push_Cut_Piece', _sm_push_cut_piece_3, transitions={'finished': 'finished', 'failed': 'Retry_Cutting'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none'}) # x:788 y:437 OperatableStateMachine.add('Complete_Cut', _sm_complete_cut_2, transitions={'finished': 'Push_Cut_Piece', 'failed': 'Retry_Cutting'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:651 y:83, x:392 y:405 _sm_request_wall_template_11 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['template_id']) with _sm_request_wall_template_11: # x:80 y:80 OperatableStateMachine.add('Head_Look_Straight', TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT), transitions={'done': 'Request_Template_ID', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}) # x:341 y:81 OperatableStateMachine.add('Request_Template_ID', InputState(request=InputState.SELECTED_OBJECT_ID, message="Provide the ID of the WALL 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:606 y:198 OperatableStateMachine.add('Log_No_Connection', LogState(text="Have no connection to OCS!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Low}) # x:445 y:192 OperatableStateMachine.add('Log_Data_Error', LogState(text="Received wrong data format!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Low}) with _state_machine: # x:72 y:28 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': 'Walk_to_and_Pickup_Tool'}, autonomy={'done': Autonomy.Full}) # x:890 y:59 OperatableStateMachine.add('Notify_Behavior_Exit', LogState(text='The behavior will now exit. Last chance to intervene!', severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.High}) # x:65 y:452 OperatableStateMachine.add('Request_Wall_Template', _sm_request_wall_template_11, transitions={'finished': 'Set_Manipulate', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'wall_template_id'}) # x:634 y:55 OperatableStateMachine.add('Cut_Circle_in_Wall', _sm_cut_circle_in_wall_10, transitions={'finished': 'Notify_Behavior_Exit', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'wall_template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'none': 'none', 'drill_usability': 'drill_usability'}) # x:621 y:322 OperatableStateMachine.add('Set_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Turn_Torso', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.Full}) # x:621 y:455 OperatableStateMachine.add('Optionally_Walk_to_Wall', _sm_optionally_walk_to_wall_9, transitions={'finished': 'Set_Manipulate', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'wall_template_id': 'wall_template_id', 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'center': 'center'}) # x:63 y:184 OperatableStateMachine.add('Walk_to_and_Pickup_Tool', _sm_walk_to_and_pickup_tool_8, transitions={'finished': 'Request_Wall_Template', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'tool_template_id', 'none': 'none', 'tool_side': 'tool_side', 'drill_usability': 'drill_usability'}) # x:630 y:197 OperatableStateMachine.add('Turn_Torso', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.TURN_TORSO_SLIGHTLY, vel_scaling=0.2, ignore_collisions=True, link_paddings={}, is_cartesian=False), transitions={'done': 'Cut_Circle_in_Wall', 'failed': 'Turn_Torso'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'side': 'wall_side'}) return _state_machine