def create(self): pull_affordance = "pull" affordance_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == "left" else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM pull_displacement = 0.3 # meters # x:383 y:840, x:483 y:490 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.none = None _state_machine.userdata.step_back_distance = 1.0 # meters _state_machine.userdata.grasp_preference = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._pull_displacement = pull_displacement # [/MANUAL_CREATE] # x:1033 y:40, x:333 y:90, x:1033 y:190 _sm_go_to_grasp_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'again'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference']) with _sm_go_to_grasp_0: # x:33 y:49 OperatableStateMachine.add('Get_Grasp_Info', GetTemplateGraspState(), transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'Inform_Grasp_Failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.Low}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'grasp': 'grasp_pose'}) # x:40 y:293 OperatableStateMachine.add('Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_To_Grasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints'}) # x:242 y:292 OperatableStateMachine.add('Plan_To_Grasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Grasp_Pose', 'incomplete': 'Move_To_Grasp_Pose', 'failed': 'Decide_Which_Grasp'}, autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High}, remapping={'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:494 y:175 OperatableStateMachine.add('Move_To_Grasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Optional_Template_Adjustment', 'failed': 'Decide_Which_Grasp'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:226 y:177 OperatableStateMachine.add('Inform_Grasp_Failed', LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:970 y:294 OperatableStateMachine.add('Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'again'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:41 y:178 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_frame_id'}) # x:712 y:78 OperatableStateMachine.add('Optional_Template_Adjustment', OperatorDecisionState(outcomes=["grasp", "pregrasp", "skip"], hint="Consider adjusting the template's pose", suggestion="skip"), transitions={'grasp': 'Get_Grasp_Info', 'pregrasp': 'again', 'skip': 'finished'}, autonomy={'grasp': Autonomy.Full, 'pregrasp': Autonomy.Full, 'skip': Autonomy.High}) # x:754 y:294 OperatableStateMachine.add('Decide_Which_Grasp', OperatorDecisionState(outcomes=["same", "next"], hint='Try the same grasp or the next one?', suggestion='same'), transitions={'same': 'Optional_Template_Adjustment', 'next': 'Increase_Preference_Index'}, autonomy={'same': Autonomy.High, 'next': Autonomy.High}) # x:133 y:390, x:433 y:190, x:983 y:140 _sm_perform_grasp_1 = OperatableStateMachine(outcomes=['finished', 'failed', 'next'], input_keys=['hand_side', 'grasp_preference', 'template_id', 'pregrasp_pose'], output_keys=['grasp_preference']) with _sm_perform_grasp_1: # x:68 y:76 OperatableStateMachine.add('Get_Finger_Configuration', GetTemplateFingerConfigState(), transitions={'done': 'Close_Fingers', 'failed': 'failed', 'not_available': 'Inform_Closing_Failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'finger_config': 'finger_config'}) # x:293 y:328 OperatableStateMachine.add('Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_Back_To_Pregrasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_waypoints'}) # x:496 y:328 OperatableStateMachine.add('Plan_Back_To_Pregrasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_Back_To_Pregrasp_Pose', 'incomplete': 'Move_Back_To_Pregrasp_Pose', 'failed': 'failed'}, autonomy={'planned': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Low}, remapping={'waypoints': 'pregrasp_waypoints', 'hand': 'hand_side', 'frame_id': 'pregrasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:662 y:228 OperatableStateMachine.add('Move_Back_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Increase_Preference_Index', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:296 y:228 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_frame_id'}) # x:673 y:128 OperatableStateMachine.add('Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'next'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:81 y:228 OperatableStateMachine.add('Close_Fingers', HandTrajectoryState(hand_type=self.hand_type), transitions={'done': 'finished', 'failed': 'Extract_Frame_Id'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'finger_trajectory': 'finger_config', 'hand_side': 'hand_side'}) # x:490 y:75 OperatableStateMachine.add('Inform_Closing_Failed', LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:733 y:190, x:383 y:40 _sm_go_to_pregrasp_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference', 'pregrasp_pose']) with _sm_go_to_pregrasp_2: # x:27 y:68 OperatableStateMachine.add('Get_Pregrasp_Info', GetTemplatePregraspState(), transitions={'done': 'Plan_To_Pregrasp_Pose', 'failed': 'failed', 'not_available': 'Inform_Pregrasp_Failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pregrasp_pose'}) # x:269 y:153 OperatableStateMachine.add('Inform_Pregrasp_Failed', LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Low}) # x:537 y:228 OperatableStateMachine.add('Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'Decide_Which_Pregrasp'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:25 y:328 OperatableStateMachine.add('Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Get_Pregrasp_Info'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:266 y:228 OperatableStateMachine.add('Plan_To_Pregrasp_Pose', PlanEndeffectorPoseState(ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Pregrasp_Pose', 'failed': 'Decide_Which_Pregrasp'}, autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High}, remapping={'target_pose': 'pregrasp_pose', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory'}) # x:266 y:327 OperatableStateMachine.add('Decide_Which_Pregrasp', OperatorDecisionState(outcomes=["same", "next"], hint='Try the same pregrasp or the next one?', suggestion='same'), transitions={'same': 'Get_Pregrasp_Info', 'next': 'Increase_Preference_Index'}, autonomy={'same': Autonomy.High, 'next': Autonomy.High}) # x:30 y:444, x:162 y:478, x:230 y:478 _sm_planning_pipeline_3 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['stand_pose'], output_keys=['plan_header']) with _sm_planning_pipeline_3: # x:34 y:57 OperatableStateMachine.add('Create_Step_Goal', CreateStepGoalState(pose_is_pelvis=True), transitions={'done': 'Plan_To_Waypoint', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'target_pose': 'stand_pose', 'step_goal': 'step_goal'}) # x:553 y:481 OperatableStateMachine.add('Modify_Plan', InputState(request=InputState.FOOTSTEP_PLAN_HEADER, message='Modify plan, VALIDATE, and confirm.'), transitions={'received': 'finished', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'plan_header'}) # x:34 y:484 OperatableStateMachine.add('Plan_To_Waypoint', PlanFootstepsState(mode=self.parameter_set), transitions={'planned': 'Modify_Plan', 'failed': 'Decide_Replan_without_Collision'}, autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:139 y:314 OperatableStateMachine.add('Decide_Replan_without_Collision', OperatorDecisionState(outcomes=['replan', 'fail'], hint='Try replanning without collision avoidance.', suggestion='replan'), transitions={'replan': 'Replan_without_Collision', 'fail': 'failed'}, autonomy={'replan': Autonomy.Low, 'fail': Autonomy.Full}) # x:319 y:406 OperatableStateMachine.add('Replan_without_Collision', PlanFootstepsState(mode='drc_step_no_collision'), transitions={'planned': 'Modify_Plan', 'failed': 'failed'}, autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:1103 y:424, x:130 y:478 _sm_grasp_trigger_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'grasp_preference']) with _sm_grasp_trigger_4: # x:86 y:72 OperatableStateMachine.add('Go_to_Pregrasp', _sm_go_to_pregrasp_2, transitions={'finished': 'Open_Fingers', 'failed': 'Grasp_Manually'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'pregrasp_pose': 'pregrasp_pose'}) # x:789 y:172 OperatableStateMachine.add('Perform_Grasp', _sm_perform_grasp_1, transitions={'finished': 'finished', 'failed': 'Grasp_Manually', 'next': 'Close_Fingers'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'next': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'pregrasp_pose': 'pregrasp_pose'}) # x:332 y:178 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=0.0), transitions={'done': 'Go_to_Grasp', 'failed': 'Grasp_Manually'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.High}, remapping={'hand_side': 'hand_side'}) # x:332 y:28 OperatableStateMachine.add('Close_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=1.0), transitions={'done': 'Go_to_Pregrasp', 'failed': 'Grasp_Manually'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.High}, remapping={'hand_side': 'hand_side'}) # x:324 y:428 OperatableStateMachine.add('Grasp_Manually', OperatorDecisionState(outcomes=["fingers_closed", "abort"], hint="Grasp the object manually, continue when fingers are closed.", suggestion=None), transitions={'fingers_closed': 'finished', 'abort': 'failed'}, autonomy={'fingers_closed': Autonomy.Full, 'abort': Autonomy.Full}) # x:543 y:172 OperatableStateMachine.add('Go_to_Grasp', _sm_go_to_grasp_0, transitions={'finished': 'Perform_Grasp', 'failed': 'Grasp_Manually', 'again': 'Close_Fingers'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'again': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id'}) # x:30 y:478, x:130 y:478, x:230 y:478 _sm_walk_to_template_5 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['template_id', 'grasp_preference', 'hand_side']) with _sm_walk_to_template_5: # x:265 y:28 OperatableStateMachine.add('Decide_Request_Template', DecisionState(outcomes=['request', 'continue'], conditions=lambda x: 'continue' if x is not None else 'request'), transitions={'request': 'Request_Template', 'continue': 'Get_Stand_Pose'}, autonomy={'request': Autonomy.Low, 'continue': Autonomy.Off}, remapping={'input_value': 'template_id'}) # x:1033 y:106 OperatableStateMachine.add('Increment_Stand_Pose', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Inform_About_Retry'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:1162 y:29 OperatableStateMachine.add('Inform_About_Retry', LogState(text="Stand pose choice failed. Trying again.", severity=Logger.REPORT_INFO), transitions={'done': 'Get_Stand_Pose'}, autonomy={'done': Autonomy.Off}) # x:567 y:118 OperatableStateMachine.add('Inform_About_Fail', LogState(text="Unable to find a suitable stand pose for the template.", severity=Logger.REPORT_WARN), transitions={'done': 'Decide_Repeat_Request'}, autonomy={'done': Autonomy.Off}) # x:554 y:274 OperatableStateMachine.add('Get_Goal_from_Operator', InputState(request=InputState.WAYPOINT_GOAL_POSE, message="Provide a waypoint in front of the template."), transitions={'received': 'Walk_To_Waypoint', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'plan_header'}) # x:279 y:110 OperatableStateMachine.add('Request_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Specify target template"), transitions={'received': 'Get_Stand_Pose', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Off, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:825 y:461 OperatableStateMachine.add('Wait_For_Stand', CheckCurrentControlModeState(target_mode=CheckCurrentControlModeState.STAND, wait=True), transitions={'correct': 'finished', 'incorrect': 'failed'}, autonomy={'correct': Autonomy.Low, 'incorrect': Autonomy.Full}, remapping={'control_mode': 'control_mode'}) # x:1143 y:277 OperatableStateMachine.add('Decide_Stand_Preference', OperatorDecisionState(outcomes=["same", "next", "abort"], hint="Same or next stand pose?", suggestion="next"), transitions={'same': 'Inform_About_Retry', 'next': 'Increment_Stand_Pose', 'abort': 'aborted'}, autonomy={'same': Autonomy.Full, 'next': Autonomy.Full, 'abort': Autonomy.Full}) # x:842 y:152 OperatableStateMachine.add('Planning_Pipeline', _sm_planning_pipeline_3, transitions={'finished': 'Walk_To_Waypoint', 'failed': 'Decide_Stand_Preference', 'aborted': 'Decide_Stand_Preference'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'stand_pose': 'stand_pose', 'plan_header': 'plan_header'}) # x:833 y:276 OperatableStateMachine.add('Walk_To_Waypoint', ExecuteStepPlanActionState(), transitions={'finished': 'Wait_For_Stand', 'failed': 'Decide_Stand_Preference'}, autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:554 y:195 OperatableStateMachine.add('Decide_Repeat_Request', OperatorDecisionState(outcomes=['repeat_id', 'request_goal'], hint=None, suggestion=None), transitions={'repeat_id': 'Request_Template', 'request_goal': 'Get_Goal_from_Operator'}, autonomy={'repeat_id': Autonomy.Low, 'request_goal': Autonomy.High}) # x:547 y:27 OperatableStateMachine.add('Get_Stand_Pose', GetTemplateStandPoseState(), transitions={'done': 'Planning_Pipeline', 'failed': 'Inform_About_Fail', 'not_available': 'Inform_About_Fail'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'stand_pose': 'stand_pose'}) # x:133 y:340, x:383 y:140 _sm_perform_step_back_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['step_back_distance']) with _sm_perform_step_back_6: # x:78 y:78 OperatableStateMachine.add('Plan_Steps_Back', FootstepPlanRelativeState(direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={'planned': 'Do_Steps_Back', 'failed': 'failed'}, autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full}, remapping={'distance': 'step_back_distance', 'plan_header': 'plan_header'}) # x:74 y:228 OperatableStateMachine.add('Do_Steps_Back', ExecuteStepPlanActionState(), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:133 y:340, x:333 y:90 _sm_release_trigger_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'none']) with _sm_release_trigger_7: # x:82 y:78 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=0), transitions={'done': 'Take_Hand_Back', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'hand_side': 'hand_side'}) # x:96 y:178 OperatableStateMachine.add('Take_Hand_Back', LogState(text="Take hand slightly back", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Full}) # x:733 y:240, x:33 y:289 _sm_pull_trigger_8 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_pull_trigger_8: # x:202 y:28 OperatableStateMachine.add('Ready_To_Pull', LogState(text="Ready to pull the trigger down", severity=Logger.REPORT_INFO), transitions={'done': 'Get_Pull_Affordance'}, autonomy={'done': Autonomy.High}) # x:192 y:328 OperatableStateMachine.add('Plan_Pull', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Pull', 'incomplete': 'Execute_Pull', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:176 y:428 OperatableStateMachine.add('Execute_Pull', ExecuteTrajectoryMsgState(controller=affordance_controller), transitions={'done': 'Decide_Repeat_Pull', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:183 y:228 OperatableStateMachine.add('Scale_Pull_Affordance', CalculationState(calculation=lambda x: x), transitions={'done': 'Plan_Pull'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'affordance', 'output_value': 'affordance'}) # x:173 y:128 OperatableStateMachine.add('Get_Pull_Affordance', GetTemplateAffordanceState(identifier=pull_affordance), transitions={'done': 'Scale_Pull_Affordance', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'affordance'}) # x:437 y:228 OperatableStateMachine.add('Decide_Repeat_Pull', OperatorDecisionState(outcomes=['done', 'repeat'], hint="Pull further?", suggestion='done'), transitions={'done': 'finished', 'repeat': 'Get_Pull_Affordance'}, autonomy={'done': Autonomy.High, 'repeat': Autonomy.Full}) with _state_machine: # x:73 y:78 OperatableStateMachine.add('Request_Trigger_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place trigger template"), transitions={'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:337 y:78 OperatableStateMachine.add('Decide_Walking', OperatorDecisionState(outcomes=["walk", "stand"], hint="Walk to template?", suggestion="walk"), transitions={'walk': 'Walk_To_Template', 'stand': 'Set_Manipulate'}, autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full}) # x:844 y:322 OperatableStateMachine.add('Pull_Trigger', _sm_pull_trigger_8, transitions={'finished': 'Release_Trigger', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none'}) # x:836 y:422 OperatableStateMachine.add('Release_Trigger', _sm_release_trigger_7, transitions={'finished': 'Warn_Stand', 'failed': 'Warn_Stand'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'none': 'none'}) # x:826 y:678 OperatableStateMachine.add('Go_To_Stand_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Set_Stand', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'side': 'none'}) # x:566 y:78 OperatableStateMachine.add('Set_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Set_Template_Frame', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:858 y:578 OperatableStateMachine.add('Warn_Stand', LogState(text="Will go to stand now", severity=Logger.REPORT_INFO), transitions={'done': 'Go_To_Stand_Pose'}, autonomy={'done': Autonomy.High}) # x:566 y:678 OperatableStateMachine.add('Set_Stand', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND), transitions={'changed': 'Decide_Step_Back', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:337 y:678 OperatableStateMachine.add('Decide_Step_Back', OperatorDecisionState(outcomes=["walk", "stand"], hint="Step back?", suggestion="walk"), transitions={'walk': 'Perform_Step_Back', 'stand': 'finished'}, autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full}) # x:77 y:672 OperatableStateMachine.add('Perform_Step_Back', _sm_perform_step_back_6, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'step_back_distance': 'step_back_distance'}) # x:330 y:172 OperatableStateMachine.add('Walk_To_Template', _sm_walk_to_template_5, transitions={'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side'}) # x:841 y:222 OperatableStateMachine.add('Grasp_Trigger', _sm_grasp_trigger_4, transitions={'finished': 'Pull_Trigger', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'grasp_preference': 'grasp_preference'}) # x:846 y:128 OperatableStateMachine.add('Look_At_Trigger', LookAtTargetState(), transitions={'done': 'Grasp_Trigger'}, autonomy={'done': Autonomy.Off}, remapping={'frame': 'template_frame'}) # x:837 y:28 OperatableStateMachine.add('Set_Template_Frame', CalculationState(calculation=lambda x: "template_tf_%d" % x), transitions={'done': 'Look_At_Trigger'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'template_id', 'output_value': 'template_frame'}) return _state_machine
def create(self): 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): plug_in_affordance = "plug_in" plug_out_affordance = "plug_out" arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM # x:202 y:568, x:374 y:401 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.none = None _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.grasp_preference = 0 _state_machine.userdata.step_back_distance = 1.0 # meters # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:588 y:126, x:324 y:44 _sm_back_to_pregrasp_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'grasp_preference']) with _sm_back_to_pregrasp_0: # x:30 y:40 OperatableStateMachine.add('Get_Pregrasp', GetTemplatePregraspState(), transitions={ 'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'grasp_pose' }) # x:242 y:292 OperatableStateMachine.add( 'Plan_To_Pregrasp', PlanEndeffectorCartesianWaypointsState( ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={ 'planned': 'Move_To_Pregrasp_Pose', 'incomplete': 'Move_To_Pregrasp_Pose', 'failed': 'Get_Pregrasp' }, autonomy={ 'planned': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.Full }, remapping={ 'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction' }) # x:568 y:295 OperatableStateMachine.add( 'Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'finished', 'failed': 'Get_Pregrasp' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) # x:41 y:178 OperatableStateMachine.add( 'Extract_Frame_Id', CalculationState( calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_pose', 'output_value': 'grasp_frame_id' }) # x:40 y:293 OperatableStateMachine.add( 'Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_To_Pregrasp'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints' }) # x:598 y:30, x:220 y:148, x:1035 y:174 _sm_go_to_grasp_1 = OperatableStateMachine( outcomes=['finished', 'failed', 'again'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference']) with _sm_go_to_grasp_1: # x:33 y:49 OperatableStateMachine.add('Get_Grasp_Info', GetTemplateGraspState(), transitions={ 'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'grasp': 'grasp_pose' }) # x:40 y:293 OperatableStateMachine.add( 'Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_To_Grasp'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints' }) # x:242 y:292 OperatableStateMachine.add( 'Plan_To_Grasp', PlanEndeffectorCartesianWaypointsState( ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={ 'planned': 'Move_To_Grasp_Pose', 'incomplete': 'Move_To_Grasp_Pose', 'failed': 'Decide_Which_Grasp' }, autonomy={ 'planned': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full }, remapping={ 'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction' }) # x:494 y:175 OperatableStateMachine.add( 'Move_To_Grasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'Optional_Template_Adjustment', 'failed': 'Decide_Which_Grasp' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) # x:970 y:294 OperatableStateMachine.add( 'Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'again'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_preference', 'output_value': 'grasp_preference' }) # x:41 y:178 OperatableStateMachine.add( 'Extract_Frame_Id', CalculationState( calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_pose', 'output_value': 'grasp_frame_id' }) # x:727 y:50 OperatableStateMachine.add( 'Optional_Template_Adjustment', OperatorDecisionState( outcomes=["grasp", "pregrasp", "skip"], hint="Consider adjusting the template's pose", suggestion="skip"), transitions={ 'grasp': 'Get_Grasp_Info', 'pregrasp': 'again', 'skip': 'finished' }, autonomy={ 'grasp': Autonomy.Full, 'pregrasp': Autonomy.Full, 'skip': Autonomy.High }) # x:754 y:294 OperatableStateMachine.add( 'Decide_Which_Grasp', OperatorDecisionState( outcomes=["same", "next"], hint='Try the same grasp or the next one?', suggestion='same'), transitions={ 'same': 'Optional_Template_Adjustment', 'next': 'Increase_Preference_Index' }, autonomy={ 'same': Autonomy.High, 'next': Autonomy.High }) # x:596 y:113, x:351 y:62 _sm_go_to_pregrasp_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference', 'pregrasp_pose']) with _sm_go_to_pregrasp_2: # x:27 y:68 OperatableStateMachine.add('Get_Pregrasp_Info', GetTemplatePregraspState(), transitions={ 'done': 'Plan_To_Pregrasp_Pose', 'failed': 'failed', 'not_available': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pregrasp_pose' }) # x:537 y:228 OperatableStateMachine.add( 'Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'finished', 'failed': 'Decide_Which_Pregrasp' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) # x:25 y:328 OperatableStateMachine.add( 'Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Get_Pregrasp_Info'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_preference', 'output_value': 'grasp_preference' }) # x:266 y:228 OperatableStateMachine.add( 'Plan_To_Pregrasp_Pose', PlanEndeffectorPoseState( ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"), transitions={ 'planned': 'Move_To_Pregrasp_Pose', 'failed': 'Decide_Which_Pregrasp' }, autonomy={ 'planned': Autonomy.Low, 'failed': Autonomy.Full }, remapping={ 'target_pose': 'pregrasp_pose', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory' }) # x:266 y:327 OperatableStateMachine.add( 'Decide_Which_Pregrasp', OperatorDecisionState( outcomes=["same", "next"], hint='Try the same pregrasp or the next one?', suggestion='same'), transitions={ 'same': 'Get_Pregrasp_Info', 'next': 'Increase_Preference_Index' }, autonomy={ 'same': Autonomy.High, 'next': Autonomy.Full }) # x:30 y:365, x:130 y:365 _sm_perform_step_back_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['step_back_distance']) with _sm_perform_step_back_3: # x:78 y:78 OperatableStateMachine.add( 'Plan_Steps_Back', FootstepPlanRelativeState( direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={ 'planned': 'Do_Steps_Back', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Full }, remapping={ 'distance': 'step_back_distance', 'plan_header': 'plan_header' }) # x:74 y:228 OperatableStateMachine.add( 'Do_Steps_Back', ExecuteStepPlanActionState(), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'plan_header': 'plan_header'}) # x:550 y:574, x:130 y:365 _sm_plug_in_cable_4 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'target_template_id', 'hand_side', 'grasp_preference', 'template_id', 'template_pose' ]) with _sm_plug_in_cable_4: # x:82 y:59 OperatableStateMachine.add('Go_to_Pregrasp', _sm_go_to_pregrasp_2, transitions={ 'finished': 'Go_to_Grasp', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'target_template_id', 'pregrasp_pose': 'pregrasp_pose' }) # x:322 y:171 OperatableStateMachine.add('Go_to_Grasp', _sm_go_to_grasp_1, transitions={ 'finished': 'Detach_Cable', 'failed': 'failed', 'again': 'Go_to_Pregrasp' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'again': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'target_template_id' }) # x:516 y:306 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState( hand_type=self.hand_type, configuration=0), transitions={ 'done': 'Back_To_Pregrasp', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'hand_side': 'hand_side'}) # x:472 y:406 OperatableStateMachine.add('Back_To_Pregrasp', _sm_back_to_pregrasp_0, transitions={ 'finished': 'Close_Fingers', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference' }) # x:314 y:524 OperatableStateMachine.add('Close_Fingers', FingerConfigurationState( hand_type=self.hand_type, configuration=1), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'hand_side': 'hand_side'}) # x:527 y:204 OperatableStateMachine.add('Detach_Cable', DetachObjectState(), transitions={ 'done': 'Open_Fingers', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'template_pose': 'template_pose' }) # x:30 y:365, x:115 y:197 _sm_take_cable_5 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_take_cable_5: # x:81 y:54 OperatableStateMachine.add( 'Get_Plug_Out_Affordance', GetTemplateAffordanceState(identifier=plug_out_affordance), transitions={ 'done': 'Plan_Plug_Out', 'failed': 'failed', 'not_available': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'affordance' }) # x:348 y:73 OperatableStateMachine.add( 'Plan_Plug_Out', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={ 'done': 'Execute_Plug_Out', 'incomplete': 'Execute_Plug_Out', 'failed': 'failed' }, autonomy={ 'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full }, remapping={ 'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction' }) # x:322 y:255 OperatableStateMachine.add( 'Execute_Plug_Out', ExecuteTrajectoryMsgState(controller=affordance_controller), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) with _state_machine: # x:73 y:78 OperatableStateMachine.add( 'Request_Cable_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place cable template"), transitions={ 'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed' }, autonomy={ 'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full }, remapping={'data': 'template_id'}) # x:330 y:172 OperatableStateMachine.add( 'Walk_To_Template', self.use_behavior(WalktoTemplateSM, 'Walk_To_Template'), transitions={ 'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit }, remapping={ 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:337 y:78 OperatableStateMachine.add('Decide_Walking', OperatorDecisionState( outcomes=["walk", "stand"], hint="Walk to template?", suggestion="walk"), transitions={ 'walk': 'Walk_To_Template', 'stand': 'Set_Manipulate' }, autonomy={ 'walk': Autonomy.High, 'stand': Autonomy.Full }) # x:839 y:72 OperatableStateMachine.add('Grasp Object', self.use_behavior( GraspObjectSM, 'Grasp Object'), transitions={ 'finished': 'Attach_Cable', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:843 y:302 OperatableStateMachine.add('Take_Cable', _sm_take_cable_5, transitions={ 'finished': 'Request_Target_Template', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none' }) # x:566 y:78 OperatableStateMachine.add( 'Set_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'Grasp Object', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Full }) # x:822 y:545 OperatableStateMachine.add('Plug_In_Cable', _sm_plug_in_cable_4, transitions={ 'finished': 'Warn_Stand', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'target_template_id': 'target_template_id', 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'template_pose': 'template_pose' }) # x:826 y:185 OperatableStateMachine.add('Attach_Cable', AttachObjectState(), transitions={ 'done': 'Take_Cable', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'template_pose': 'template_pose' }) # x:811 y:400 OperatableStateMachine.add( 'Request_Target_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place target template"), transitions={ 'received': 'Plug_In_Cable', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed' }, autonomy={ 'received': Autonomy.Low, 'aborted': Autonomy.High, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full }, remapping={'data': 'target_template_id'}) # x:753 y:646 OperatableStateMachine.add( 'Warn_Stand', LogState(text="Going to stand pose", severity=Logger.REPORT_INFO), transitions={'done': 'Go_To_Stand_Pose'}, autonomy={'done': Autonomy.High}) # x:581 y:573 OperatableStateMachine.add( 'Go_To_Stand_Pose', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Set_Stand', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'side': 'none'}) # x:446 y:653 OperatableStateMachine.add('Decide_Step_Back', OperatorDecisionState( outcomes=["walk", "stand"], hint="Step back?", suggestion="walk"), transitions={ 'walk': 'Perform_Step_Back', 'stand': 'finished' }, autonomy={ 'walk': Autonomy.High, 'stand': Autonomy.Full }) # x:100 y:661 OperatableStateMachine.add( 'Perform_Step_Back', _sm_perform_step_back_3, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'step_back_distance': 'step_back_distance'}) # x:383 y:527 OperatableStateMachine.add( 'Set_Stand', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'Decide_Step_Back', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Full }) return _state_machine
def create(self): pull_affordance = "pull" affordance_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == "left" else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM # x:383 y:840, x:483 y:490 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.none = None _state_machine.userdata.step_back_distance = 1.0 # meters _state_machine.userdata.grasp_preference = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:133 y:340, x:383 y:140 _sm_perform_step_back_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['step_back_distance']) with _sm_perform_step_back_0: # x:78 y:78 OperatableStateMachine.add('Plan_Steps_Back', FootstepPlanRelativeState(direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={'planned': 'Do_Steps_Back', 'failed': 'failed'}, autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full}, remapping={'distance': 'step_back_distance', 'plan_header': 'plan_header'}) # x:74 y:228 OperatableStateMachine.add('Do_Steps_Back', ExecuteStepPlanActionState(), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:133 y:340, x:333 y:90 _sm_release_trigger_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'none']) with _sm_release_trigger_1: # x:82 y:78 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=0), transitions={'done': 'Take_Hand_Back', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'hand_side': 'hand_side'}) # x:96 y:178 OperatableStateMachine.add('Take_Hand_Back', LogState(text="Take hand slightly back", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Full}) # x:133 y:540, x:433 y:240 _sm_pull_trigger_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_pull_trigger_2: # x:73 y:78 OperatableStateMachine.add('Get_Pull_Affordance', GetTemplateAffordanceState(identifier=pull_affordance), transitions={'done': 'Plan_Pull', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'affordance'}) # x:92 y:228 OperatableStateMachine.add('Plan_Pull', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Pull', 'incomplete': 'Execute_Pull', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:76 y:378 OperatableStateMachine.add('Execute_Pull', ExecuteTrajectoryMsgState(controller=affordance_controller), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) with _state_machine: # x:73 y:78 OperatableStateMachine.add('Request_Trigger_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place trigger template"), transitions={'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:330 y:172 OperatableStateMachine.add('Walk_To_Template', self.use_behavior(WalktoTemplateSM, 'Walk_To_Template'), transitions={'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': 'template_id'}) # x:337 y:78 OperatableStateMachine.add('Decide_Walking', OperatorDecisionState(outcomes=["walk", "stand"], hint="Walk to template?", suggestion="walk"), transitions={'walk': 'Walk_To_Template', 'stand': 'Set_Manipulate'}, autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full}) # x:839 y:72 OperatableStateMachine.add('Grasp Object', self.use_behavior(GraspObjectSM, 'Grasp Object'), transitions={'finished': 'Pull_Trigger', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id'}) # x:844 y:172 OperatableStateMachine.add('Pull_Trigger', _sm_pull_trigger_2, transitions={'finished': 'Release_Trigger', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none'}) # x:836 y:272 OperatableStateMachine.add('Release_Trigger', _sm_release_trigger_1, transitions={'finished': 'Warn_Stand', 'failed': 'Warn_Stand'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'none': 'none'}) # x:826 y:528 OperatableStateMachine.add('Go_To_Stand_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Set_Stand', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'side': 'none'}) # x:566 y:78 OperatableStateMachine.add('Set_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Grasp Object', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:858 y:428 OperatableStateMachine.add('Warn_Stand', LogState(text="Will go to stand now", severity=Logger.REPORT_INFO), transitions={'done': 'Go_To_Stand_Pose'}, autonomy={'done': Autonomy.High}) # x:816 y:628 OperatableStateMachine.add('Set_Stand', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND), transitions={'changed': 'Decide_Step_Back', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:587 y:678 OperatableStateMachine.add('Decide_Step_Back', OperatorDecisionState(outcomes=["walk", "stand"], hint="Step back?", suggestion="walk"), transitions={'walk': 'Perform_Step_Back', 'stand': 'finished'}, autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full}) # x:327 y:672 OperatableStateMachine.add('Perform_Step_Back', _sm_perform_step_back_0, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'step_back_distance': 'step_back_distance'}) return _state_machine
def create(self): 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
def create(self): arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM # x:1234 y:456, x:92 y:189 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.reference_point = None _state_machine.userdata.current_target_angle_deg = self.start_angle_deg; _state_machine.userdata.dummy = None _state_machine.userdata.save_joints = ['l_shoulder_pitch', 'l_shoulder_roll', 'l_shoulder_yaw', 'l_elbow', 'l_wrist_yaw1', 'l_wrist_roll', 'l_wrist_yaw2'] _state_machine.userdata.move_to_poses = self.move_to_poses _state_machine.userdata.hand_offset = [0, 0, -0.065] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self.total_displacement = 0; # [/MANUAL_CREATE] # x:1136 y:74, x:130 y:480 _sm_save_target_arm_pose_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['joint_trajectory', 'hand_side', 'move_to_poses']) with _sm_save_target_arm_pose_0: # x:146 y:184 OperatableStateMachine.add('Decide_Move_Arm', DecisionState(outcomes=['move_arm', 'do_not_move', 'abort'], conditions=self.decide_move_arm), transitions={'move_arm': 'Move_Arm_To_Pose', 'do_not_move': 'Save_Arm_Pose_From_Trajectory', 'abort': 'failed'}, autonomy={'move_arm': Autonomy.Full, 'do_not_move': Autonomy.Low, 'abort': Autonomy.Low}, remapping={'input_value': 'move_to_poses'}) # x:330 y:232 OperatableStateMachine.add('Move_Arm_To_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Get_Current_Joint_Values', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:875 y:223 OperatableStateMachine.add('Save_Arm_Pose_From_Joints', CalculationState(calculation=self.save_arm_pose_from_joints), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'joint_config', 'output_value': 'success'}) # x:330 y:45 OperatableStateMachine.add('Save_Arm_Pose_From_Trajectory', CalculationState(calculation=self.save_arm_pose_from_trajectory), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'joint_trajectory', 'output_value': 'success'}) # x:622 y:225 OperatableStateMachine.add('Get_Current_Joint_Values', QueryJointPositionsState(side=self.hand_side, controller='traj_controller'), transitions={'retrieved': 'Save_Arm_Pose_From_Joints', 'failed': 'failed'}, autonomy={'retrieved': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'joint_config': 'joint_config'}) # x:553 y:74, x:273 y:212 _sm_get_reference_point_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side'], output_keys=['reference_point']) with _sm_get_reference_point_1: # x:30 y:40 OperatableStateMachine.add('Get_Hand_Frames', CalculationState(calculation=self.get_hand_frames), transitions={'done': 'Get_Reference_Point'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'hand_side', 'output_value': 'frames'}) # x:225 y:40 OperatableStateMachine.add('Get_Reference_Point', GetTFTransformState(), transitions={'done': 'Add_Offset', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'frames': 'frames', 'transform': 'reference_point'}) # x:411 y:83 OperatableStateMachine.add('Add_Offset', CalculationState(calculation=self.add_offset), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'reference_point', 'output_value': 'reference_point'}) # x:1432 y:637, x:44 y:619 _sm_calculate_key_frames_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['wheel_affordance', 'hand_side', 'reference_point', 'move_to_poses']) with _sm_calculate_key_frames_2: # x:30 y:40 OperatableStateMachine.add('Ignore_Quasi_Static_Constraint', SetRosParamState(parameter='/drake_ignore_quasi_static_constraint', value=True), transitions={'done': 'Plan_Wheel_Rotation', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}) # x:998 y:85 OperatableStateMachine.add('Continue_Rotation', DecisionState(outcomes=['continue_rotation', 'finished'], conditions=self.check_continue_rotation), transitions={'continue_rotation': 'Plan_Wheel_Rotation', 'finished': 'Reset_Quasi_Static_Constraint'}, autonomy={'continue_rotation': Autonomy.Low, 'finished': Autonomy.Low}, remapping={'input_value': 'wheel_affordance'}) # x:921 y:470 OperatableStateMachine.add('Calculate_New_Target_Angle', CalculationState(calculation=self.calc_rotation_angle), transitions={'done': 'Continue_Rotation'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'wheel_affordance', 'output_value': 'wheel_affordance'}) # x:327 y:459 OperatableStateMachine.add('Decide_Continue', OperatorDecisionState(outcomes=['continue', 'retry', 'abort'], hint=None, suggestion=None), transitions={'continue': 'Calculate_New_Target_Angle', 'retry': 'Plan_Wheel_Rotation', 'abort': 'failed'}, autonomy={'continue': Autonomy.High, 'retry': Autonomy.High, 'abort': Autonomy.High}) # x:670 y:190 OperatableStateMachine.add('Save_Target_Arm_Pose', _sm_save_target_arm_pose_0, transitions={'finished': 'Calculate_New_Target_Angle', 'failed': 'Decide_Continue'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'joint_trajectory': 'joint_trajectory', 'hand_side': 'hand_side', 'move_to_poses': 'move_to_poses'}) # x:1172 y:609 OperatableStateMachine.add('Reset_Quasi_Static_Constraint', SetRosParamState(parameter='/drake_ignore_quasi_static_constraint', value=False), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}) # x:334 y:65 OperatableStateMachine.add('Plan_Wheel_Rotation', PlanAffordanceState(vel_scaling=0.1, planner_id="drake", drake_sample_rate=4.0, drake_orientation_type=1, drake_link_axis=[1,0,0]), transitions={'done': 'Save_Target_Arm_Pose', 'incomplete': 'Decide_Continue', 'failed': 'Decide_Continue'}, autonomy={'done': Autonomy.Low, 'incomplete': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'affordance': 'wheel_affordance', 'hand': 'hand_side', 'reference_point': 'reference_point', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:936 y:246, x:87 y:494 _sm_get_template_affordance_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side'], output_keys=['wheel_affordance']) with _sm_get_template_affordance_3: # x:71 y:85 OperatableStateMachine.add('Request_Template_ID', InputState(request=InputState.SELECTED_OBJECT_ID, message="Provide the ID of the STEERING_WHEEL template."), transitions={'received': 'Get_Template_Affordance', 'aborted': 'failed', 'no_connection': 'Log_No_Connection', 'data_error': 'Log_Data_Error'}, autonomy={'received': Autonomy.Off, 'aborted': Autonomy.High, 'no_connection': Autonomy.Low, 'data_error': Autonomy.Low}, remapping={'data': 'wheel_template_id'}) # x:167 y:234 OperatableStateMachine.add('Log_Data_Error', LogState(text="Received wrong data format!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Low}) # x:328 y:236 OperatableStateMachine.add('Log_No_Connection', LogState(text='No Connection', severity=Logger.REPORT_HINT), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Low}) # x:600 y:230 OperatableStateMachine.add('Get_Template_Affordance', GetTemplateAffordanceState(identifier='turn_right'), transitions={'done': 'finished', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low, 'not_available': Autonomy.Low}, remapping={'template_id': 'wheel_template_id', 'hand_side': 'hand_side', 'affordance': 'wheel_affordance'}) with _state_machine: # x:61 y:59 OperatableStateMachine.add('Notify_Execution_Started', LogState(text="Execution has started. Consider making modifications if this is a re-run.", severity=Logger.REPORT_HINT), transitions={'done': 'Get_Reference_Point'}, autonomy={'done': Autonomy.Off}) # x:249 y:210 OperatableStateMachine.add('Get_Template_Affordance', _sm_get_template_affordance_3, transitions={'finished': 'Init_Affordance', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'wheel_affordance': 'wheel_affordance'}) # x:1080 y:318 OperatableStateMachine.add('Notify_Behavior_Exit', LogState(text='The behavior will now exit. Last chance to intervene!', severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.High}) # x:316 y:315 OperatableStateMachine.add('Init_Affordance', CalculationState(calculation=self.init_affordance), transitions={'done': 'Calculate_Key_Frames'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'wheel_affordance', 'output_value': 'wheel_affordance'}) # x:732 y:320 OperatableStateMachine.add('Decide_Save', OperatorDecisionState(outcomes=['save', 'no_save'], hint=None, suggestion=None), transitions={'save': 'Save_Key_Positions', 'no_save': 'Notify_Behavior_Exit'}, autonomy={'save': Autonomy.Full, 'no_save': Autonomy.Full}) # x:925 y:424 OperatableStateMachine.add('Save_Key_Positions', CalculationState(calculation=self.save_key_positions_to_disc), transitions={'done': 'Notify_Behavior_Exit'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'dummy', 'output_value': 'output_value'}) # x:471 y:319 OperatableStateMachine.add('Calculate_Key_Frames', _sm_calculate_key_frames_2, transitions={'finished': 'Decide_Save', 'failed': 'Decide_Save'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'wheel_affordance': 'wheel_affordance', 'hand_side': 'hand_side', 'reference_point': 'reference_point', 'move_to_poses': 'move_to_poses'}) # x:267 y:58 OperatableStateMachine.add('Get_Reference_Point', _sm_get_reference_point_1, transitions={'finished': 'Get_Template_Affordance', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'reference_point': 'reference_point'}) return _state_machine
def create(self): 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