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
Example #2
0
	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
Example #3
0
	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):
        cart_lift = 0.20  # meters
        arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM
        wrist_frame_id = self.hand_side[0] + '_hand'
        # x:220 y:518, x:463 y:233
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'],
            input_keys=['hand_side', 'template_id'])
        _state_machine.userdata.hand_side = self.hand_side
        _state_machine.userdata.template_id = 0

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        # x:71 y:304, x:597 y:174
        _sm_lift_object_0 = OperatableStateMachine(
            outcomes=['finished', 'failed'],
            input_keys=['hand_side', 'template_id'])

        with _sm_lift_object_0:
            # x:255 y:28
            OperatableStateMachine.add('Get_Wrist_Pose',
                                       GetWristPoseState(),
                                       transitions={
                                           'done': 'Set_Target_for_Lifting',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'done': Autonomy.Off,
                                           'failed': Autonomy.High
                                       },
                                       remapping={
                                           'hand_side': 'hand_side',
                                           'wrist_pose': 'wrist_pose'
                                       })

            # x:525 y:29
            OperatableStateMachine.add(
                'Set_Target_for_Lifting',
                CalculationState(
                    calculation=self.lift_template(cart_lift, False)),
                transitions={'done': 'Transform_to_Wrist_Frame'},
                autonomy={'done': Autonomy.Off},
                remapping={
                    'input_value': 'wrist_pose',
                    'output_value': 'lift_target'
                })

            # x:887 y:296
            OperatableStateMachine.add(
                'Prepare_Plan_Frame_Wrist',
                CalculationState(
                    calculation=lambda pose: pose.header.frame_id),
                transitions={'done': 'Plan_Cartesian_Lift'},
                autonomy={'done': Autonomy.Off},
                remapping={
                    'input_value': 'lift_target_hand',
                    'output_value': 'hand_frame_id'
                })

            # x:523 y:296
            OperatableStateMachine.add(
                'Plan_Cartesian_Lift',
                PlanEndeffectorCartesianWaypointsState(
                    ignore_collisions=False,
                    include_torso=False,
                    keep_endeffector_orientation=True,
                    allow_incomplete_plans=True,
                    vel_scaling=0.1,
                    planner_id="RRTConnectkConfigDefault"),
                transitions={
                    'planned': 'Perform_Lifting',
                    'incomplete': 'Perform_Lifting',
                    'failed': 'failed'
                },
                autonomy={
                    'planned': Autonomy.Low,
                    'incomplete': Autonomy.High,
                    'failed': Autonomy.High
                },
                remapping={
                    'waypoints': 'lift_waypoints',
                    'hand': 'hand_side',
                    'frame_id': 'hand_frame_id',
                    'joint_trajectory': 'joint_trajectory',
                    'plan_fraction': 'plan_fraction'
                })

            # x:886 y:29
            OperatableStateMachine.add(
                'Transform_to_Wrist_Frame',
                GetPoseInFrameState(target_frame=wrist_frame_id),
                transitions={
                    'done': 'Convert_to_List_of_Poses',
                    'failed': 'failed'
                },
                autonomy={
                    'done': Autonomy.Off,
                    'failed': Autonomy.High
                },
                remapping={
                    'pose_in': 'lift_target',
                    'pose_out': 'lift_target_hand'
                })

            # x:888 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': 'lift_target_hand',
                    'output_value': 'lift_waypoints'
                })

            # x:206 y:297
            OperatableStateMachine.add(
                'Perform_Lifting',
                ExecuteTrajectoryMsgState(controller=arm_controller),
                transitions={
                    'done': 'finished',
                    'failed': 'failed'
                },
                autonomy={
                    'done': Autonomy.Off,
                    'failed': Autonomy.High
                },
                remapping={'joint_trajectory': 'joint_trajectory'})

        with _state_machine:
            # x:187 y:25
            OperatableStateMachine.add('Grasp_Object',
                                       self.use_behavior(
                                           GraspObjectSM, 'Grasp_Object'),
                                       transitions={
                                           'finished':
                                           'Confirm-Successful_Grasp',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'finished': Autonomy.Inherit,
                                           'failed': Autonomy.Inherit
                                       },
                                       remapping={
                                           'hand_side': 'hand_side',
                                           'template_id': 'template_id'
                                       })

            # x:181 y:275
            OperatableStateMachine.add('Attach_Object_to_Hand',
                                       AttachObjectState(),
                                       transitions={
                                           'done': 'Lift_Object',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'done': Autonomy.Off,
                                           'failed': Autonomy.High
                                       },
                                       remapping={
                                           'template_id': 'template_id',
                                           'hand_side': 'hand_side',
                                           'template_pose': 'template_pose'
                                       })

            # x:190 y:383
            OperatableStateMachine.add('Lift_Object',
                                       _sm_lift_object_0,
                                       transitions={
                                           'finished': 'finished',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'finished': Autonomy.Inherit,
                                           'failed': Autonomy.Inherit
                                       },
                                       remapping={
                                           'hand_side': 'hand_side',
                                           'template_id': 'template_id'
                                       })

            # x:176 y:159
            OperatableStateMachine.add(
                'Confirm-Successful_Grasp',
                LogState(text='Confirm that the grasp was successful!',
                         severity=Logger.REPORT_HINT),
                transitions={'done': 'Attach_Object_to_Hand'},
                autonomy={'done': Autonomy.Full})

        return _state_machine
Example #5
0
	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):
		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):
        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
Example #8
0
    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
		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