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