def create(self):
# x:1132 y:61, x:520 y:383
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.joint_names_test = ["m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"]
_state_machine.userdata.joint_values_test = [5.851354801687673, 4.864461337582612, 1.0885434600152142, 2.0525195574127895, 4.140473804709629, 1.3734302573574513]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:111 y:45
OperatableStateMachine.add('home',
FeedbackSrdfStateToMoveit(config_name="Home", move_group="arm", action_topic='/move_group', robot_name="m1n6s200", position_topic='/m1n6s200_driver/joint_states', delta=1E-7),
transitions={'reached': 'Testing ', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off})
# x:457 y:49
OperatableStateMachine.add('Testing ',
FeedbackJointStateToMoveit(move_group="arm", action_topic="/move_group", robot_name="m1n6s200", position_topic="/m1n6s200_driver/joint_states", delta=1E-7),
transitions={'reached': 'go home2', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'joint_values': 'joint_values_test', 'joint_names': 'joint_names_test'})
# x:803 y:55
OperatableStateMachine.add('go home2',
SrdfStateToMoveit(config_name="Home", move_group="arm", action_topic='/move_group', robot_name="m1n6s200"),
transitions={'reached': 'finished', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
return _state_machine
def create(self):
# x:832 y:94, x:692 y:405
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.joint_values_left = [4.824961332294557, 4.986228519472087, 2.016893253088309, 9.056585287421564, 1.7655888505129436, 2.5387597755555658]
_state_machine.userdata.joint_values_right = [4.761117371482087, 4.435244724700612, 1.6085220010066865, 3.2511244744831167, 1.706054283818386, 2.4325377312649925]
_state_machine.userdata.joint_names = ["m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:106 y:117
OperatableStateMachine.add('sub',
SubscriberState(topic='/robotender', blocking=True, clear=False),
transitions={'received': 'decide', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'message'})
# x:280 y:116
OperatableStateMachine.add('decide',
DecisionState(outcomes=['go_left','go_right'], conditions=lambda x: "go_right" if x.data=="R" else "go_left"),
transitions={'go_left': 'left', 'go_right': 'right'},
autonomy={'go_left': Autonomy.Off, 'go_right': Autonomy.Off},
remapping={'input_value': 'message'})
# x:460 y:22
OperatableStateMachine.add('left',
FeedbackJointStateToMoveit(move_group="arm", action_topic="/move_group", robot_name="m1n6s200", position_topic='/m1n6s200_driver/joint_states', delta=1E-4),
transitions={'reached': 'sub', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'joint_values': 'joint_values_left', 'joint_names': 'joint_names'})
# x:452 y:234
OperatableStateMachine.add('right',
FeedbackJointStateToMoveit(move_group="arm", action_topic="/move_group", robot_name="m1n6s200", position_topic='/m1n6s200_driver/joint_states', delta=1E-4),
transitions={'reached': 'sub', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'joint_values': 'joint_values_right', 'joint_names': 'joint_names'})
return _state_machine
def create(self):
# x:1250 y:404, x:625 y:387
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.center = [
4.824961332294557, 4.986228519472087, 2.016893253088309,
9.056585287421564, 1.7655888505129436, 2.5387597755555658
]
_state_machine.userdata.prep_pour = [
4.761117371482087, 4.435244724700612, 1.6085220010066865,
3.2511244744831167, 1.706054283818386, 2.4325377312649925
]
_state_machine.userdata.pour = [
4.761121632539831, 4.435244724700612, 1.6085220010066865,
3.2511244744831167, 1.706051886973405, 4.577594147074765
]
_state_machine.userdata.post_pour = [
4.761117371482087, 4.435244724700612, 1.6085220010066865,
3.2511244744831167, 1.706054283818386, 2.4325377312649925
]
_state_machine.userdata.joint_names = [
"m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3",
"m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"
]
_state_machine.userdata.OPEN = [0, 0]
_state_machine.userdata.CLOSE = [5000, 5000]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:67 y:41
OperatableStateMachine.add(
'center1',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'Grasp',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'joint_values': 'center',
'joint_names': 'joint_names'
})
# x:325 y:162
OperatableStateMachine.add(
'prep pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'pour',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'joint_values': 'prep_pour',
'joint_names': 'joint_names'
})
# x:619 y:171
OperatableStateMachine.add(
'pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'post pour',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'joint_values': 'pour',
'joint_names': 'joint_names'
})
# x:897 y:158
OperatableStateMachine.add(
'post pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'center2',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'joint_values': 'post_pour',
'joint_names': 'joint_names'
})
# x:1036 y:338
OperatableStateMachine.add(
'release',
FingerPositionState(
result_topic=
"/m1n6s200_driver/fingers_action/finger_positions/result",
action_topic=
"/m1n6s200_driver/fingers_action/finger_positions",
robot_name="m1n6s200"),
transitions={
'reached': 'finished',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'finger_values': 'OPEN'})
# x:1097 y:190
OperatableStateMachine.add(
'center2',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'release',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'joint_values': 'center',
'joint_names': 'joint_names'
})
# x:58 y:171
OperatableStateMachine.add(
'Grasp',
FingerPositionState(
result_topic=
"/m1n6s200_driver/fingers_action/finger_positions/result",
action_topic=
"/m1n6s200_driver/fingers_action/finger_positions",
robot_name="m1n6s200"),
transitions={
'reached': 'prep pour',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'finger_values': 'CLOSE'})
return _state_machine
def create(self):
# x:1228 y:87, x:486 y:352
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.joint_names = [
'm1n6s200_joint_1', 'm1n6s200_joint_2', 'm1n6s200_joint_3',
'm1n6s200_joint_4', 'm1n6s200_joint_5', 'm1n6s200_joint_6'
]
_state_machine.userdata.pre_pour = [
5.126004529284896, 4.489626207843922, 1.7224576250785655,
3.7321632727008596, 1.7839496151743317, 8.499195258647903
]
_state_machine.userdata.pour = [
4.960468827512186, 4.2515385411264015, 1.6518825246109206,
3.919642356591386, 1.4049057588069342, 10.712974546387171
]
_state_machine.userdata.mid_pour = [
5.013071052731214, 4.230059347986586, 1.6150925520481298,
3.96125265072722, 1.2587368307973903, 10.195214485162762
]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:50 y:29
OperatableStateMachine.add(
'pre pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'mid pour',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pre_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:686 y:58
OperatableStateMachine.add(
'pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'post pour',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:349 y:41
OperatableStateMachine.add(
'mid pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'pour',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'mid_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:944 y:64
OperatableStateMachine.add(
'post pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pre_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
return _state_machine
def create(self):
# x:1201 y:538, x:404 y:334
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.joint_names = [
'm1n6s200_joint_1', 'm1n6s200_joint_2', 'm1n6s200_joint_3',
'm1n6s200_joint_4', 'm1n6s200_joint_5', 'm1n6s200_joint_6'
]
_state_machine.userdata.joint_values = [
5.013071052731214, 4.230059347986586, 1.6150925520481298,
3.96125265072722, 1.2587368307973903, 10.195214485162762
]
_state_machine.userdata.left_lift = [
5.065078327762718, 4.076029034755237, 0.9997600608585886,
3.7334871300787382, 1.8222228353062417, 8.792189850191805
]
_state_machine.userdata.left_cup = [
4.789882174416034, 4.891706540798559, 1.8607510531125273,
3.4168710986141604, 1.8134044431467171, 8.788656900689706
]
_state_machine.userdata.CLOSE = [6400, 6400]
_state_machine.userdata.prepost_pour = [
4.87152191026463, 4.381388416924268, 1.4040867036136804,
3.1928864677652644, 1.7481033337438106, 8.741622280045599
]
_state_machine.userdata.mid_pour = [
4.773423306247126, 4.387238050258627, 1.375200194217918,
3.1306545183606196, 1.5138697937558532, 7.627375919980731
]
_state_machine.userdata.finish_pour = [
4.778883319114006, 4.319199078097064, 1.3984297499841885,
2.961141918132095, 1.5586557743335523, 6.81882529933057
]
_state_machine.userdata.OPEN = [0, 0]
_state_machine.userdata.right_lift = [
5.739989927016048, 4.351818274076682, 1.1057325669550215,
3.5978672494631065, 1.9470734251058108, 2.586587751888408
]
_state_machine.userdata.right_cup = [
5.487217590047831, 4.894399529292847, 1.814038009170047,
3.419528667065951, 1.9052846991769412, 2.5484829766954453
]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:1175 y:485, x:467 y:379
_sm_pour_to_center_right_0 = OperatableStateMachine(
outcomes=['reached', 'failed'],
input_keys=[
'prepost_pour', 'mid_pour', 'joint_names', 'finish_pour',
'left_lift'
])
with _sm_pour_to_center_right_0:
# x:121 y:147
OperatableStateMachine.add(
'begin pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'mid pour',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'prepost_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:385 y:146
OperatableStateMachine.add(
'mid pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'finish pour',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'mid_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:660 y:159
OperatableStateMachine.add(
'finish pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'upright',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'finish_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:935 y:163
OperatableStateMachine.add(
'upright',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'init state',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'prepost_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:976 y:318
OperatableStateMachine.add(
'init state',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'reached',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'left_lift',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:360 y:407, x:911 y:461
_sm_right_grab_1 = OperatableStateMachine(
outcomes=['failed', 'reached'],
input_keys=[
'joint_names', 'right_lift', 'right_cup', 'CLOSE', 'left_lift'
])
with _sm_right_grab_1:
# x:27 y:135
OperatableStateMachine.add(
'right lifted no cup',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'right cup',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'right_lift',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:243 y:132
OperatableStateMachine.add(
'right cup',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'CLOSE',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'right_cup',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:471 y:131
OperatableStateMachine.add(
'CLOSE',
FingerPositionState(
result_topic=
"/m1n6s200_driver/fingers_action/finger_positions/result",
action_topic=
"/m1n6s200_driver/fingers_action/finger_positions",
robot_name="m1n6s200"),
transitions={
'reached': 'right lifted w cup',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'finger_values': 'CLOSE'})
# x:706 y:124
OperatableStateMachine.add(
'right lifted w cup',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'init location',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'right_lift',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:783 y:233
OperatableStateMachine.add(
'init location',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'reached',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'left_lift',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:525 y:426, x:1076 y:348
_sm_release_2 = OperatableStateMachine(
outcomes=['failed', 'reached'],
input_keys=['left_cup', 'OPEN', 'joint_names', 'left_lift'])
with _sm_release_2:
# x:89 y:157
OperatableStateMachine.add(
'prep place',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'place',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'left_lift',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:307 y:168
OperatableStateMachine.add(
'place',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'open',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'left_cup',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:544 y:171
OperatableStateMachine.add(
'open',
FingerPositionState(
result_topic=
"/m1n6s200_driver/fingers_action/finger_positions/result",
action_topic=
"/m1n6s200_driver/fingers_action/finger_positions",
robot_name="m1n6s200"),
transitions={
'reached': 'clear cup',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'finger_values': 'OPEN'})
# x:818 y:175
OperatableStateMachine.add(
'clear cup',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'reached',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'left_lift',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:1252 y:251, x:609 y:359
_sm_pour_to_center_3 = OperatableStateMachine(
outcomes=['reached', 'failed'],
input_keys=[
'prepost_pour', 'mid_pour', 'joint_names', 'finish_pour'
])
with _sm_pour_to_center_3:
# x:121 y:147
OperatableStateMachine.add(
'begin pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'mid pour',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'prepost_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:385 y:146
OperatableStateMachine.add(
'mid pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'finish pour',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'mid_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:660 y:159
OperatableStateMachine.add(
'finish pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'upright',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'finish_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:977 y:157
OperatableStateMachine.add(
'upright',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'reached',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'prepost_pour',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:465 y:367, x:1128 y:309
_sm_left_grab_4 = OperatableStateMachine(
outcomes=['failed', 'reached'],
input_keys=[
'joint_values', 'joint_names', 'left_lift', 'CLOSE', 'left_cup'
])
with _sm_left_grab_4:
# x:72 y:150
OperatableStateMachine.add(
'left lift no cup',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'left cup',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'left_lift',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:315 y:153
OperatableStateMachine.add(
'left cup',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'CLOSE',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'left_cup',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:858 y:155
OperatableStateMachine.add(
'left lift',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'reached',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'left_lift',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:584 y:154
OperatableStateMachine.add(
'CLOSE',
FingerPositionState(
result_topic=
"/m1n6s200_driver/fingers_action/finger_positions/result",
action_topic=
"/m1n6s200_driver/fingers_action/finger_positions",
robot_name="m1n6s200"),
transitions={
'reached': 'left lift',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'finger_values': 'CLOSE'})
with _state_machine:
# x:262 y:63
OperatableStateMachine.add('left grab',
_sm_left_grab_4,
transitions={
'failed': 'failed',
'reached': 'pour to center'
},
autonomy={
'failed': Autonomy.Inherit,
'reached': Autonomy.Inherit
},
remapping={
'joint_values': 'joint_values',
'joint_names': 'joint_names',
'left_lift': 'left_lift',
'CLOSE': 'CLOSE',
'left_cup': 'left_cup'
})
# x:468 y:60
OperatableStateMachine.add('pour to center',
_sm_pour_to_center_3,
transitions={
'reached': 'release',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'prepost_pour': 'prepost_pour',
'mid_pour': 'mid_pour',
'joint_names': 'joint_names',
'finish_pour': 'finish_pour'
})
# x:727 y:48
OperatableStateMachine.add('release',
_sm_release_2,
transitions={
'failed': 'failed',
'reached': 'right grab'
},
autonomy={
'failed': Autonomy.Inherit,
'reached': Autonomy.Inherit
},
remapping={
'left_cup': 'left_cup',
'OPEN': 'OPEN',
'joint_names': 'joint_names',
'left_lift': 'left_lift'
})
# x:917 y:106
OperatableStateMachine.add('right grab',
_sm_right_grab_1,
transitions={
'failed': 'failed',
'reached': 'pour to center right'
},
autonomy={
'failed': Autonomy.Inherit,
'reached': Autonomy.Inherit
},
remapping={
'joint_names': 'joint_names',
'right_lift': 'right_lift',
'right_cup': 'right_cup',
'CLOSE': 'CLOSE',
'left_lift': 'left_lift'
})
# x:1038 y:230
OperatableStateMachine.add('pour to center right',
_sm_pour_to_center_right_0,
transitions={
'reached': 'finished',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'prepost_pour': 'prepost_pour',
'mid_pour': 'mid_pour',
'joint_names': 'joint_names',
'finish_pour': 'finish_pour',
'left_lift': 'left_lift'
})
return _state_machine
def create(self):
# x:958 y:78, x:24 y:694
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.joint_names = ["m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"]
_state_machine.userdata.center_values = [4.825370393837993, 4.804768712277358, 1.7884682005958692, 2.781744729201632, 1.7624776125694588, 2.5668808924540394]
_state_machine.userdata.prep_pour_to_left = [4.8484381625680415, 4.172889801498073, 1.372345285529353, 3.0126762157540004, 1.4690217615247554, 2.627620406383804]
_state_machine.userdata.pour_to_left = [4.610045297589599, 4.293199701639057, 1.419019181003809, 3.012844793851002, 1.4674078859041673, 4.845438377916176]
_state_machine.userdata.post_pour_to_left = [4.8484381625680415, 4.172889801498073, 1.372345285529353, 3.0126762157540004, 1.4690217615247554, 2.627620406383804]
_state_machine.userdata.left_values = [4.501794723496712, 4.784133474886988, 1.6909002314255626, 2.766800400744653, 1.8037183931040444, 2.543646143523643]
_state_machine.userdata.prep_pour_to_center = [4.4696588912549435, 4.2865780179046835, 1.371823705429861, 2.7555946178259263, 1.6906042210704002, 2.5960829864389763]
_state_machine.userdata.pour_to_center = [4.700331784865464, 4.265325726089742, 1.4461706409493849, 2.7535296027166787, 1.4171899888090882, 0.5029200288136196]
_state_machine.userdata.post_pour_to_center = [4.4696588912549435, 4.2865780179046835, 1.371823705429861, 2.7555946178259263, 1.6906042210704002, 2.5960829864389763]
_state_machine.userdata.OPEN = [0,0]
_state_machine.userdata.CLOSE = [5000,5000]
_state_machine.userdata.pre_grab_left = [4.616985495390345, 4.361768642857545, 0.8309522662125534, 2.772490244413607, 1.7511775537481435, 2.6507113446153356]
_state_machine.userdata.back_off_center = [4.8380550301100405, 4.49428940291265, 1.2147491327564424, 2.784340512316133, 1.7494544885228622, 2.530367888644617]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365
_sm_stuff_0 = OperatableStateMachine(outcomes=['failed', 'planning_failed', 'control_failed', 'reached'], input_keys=['OPEN', 'CLOSE', 'center_values', 'prep_pour_to_left', 'pour_to_left', 'post_pour_to_left', 'back_off_center', 'joint_names'], output_keys=['joint_names'])
with _sm_stuff_0:
# x:41 y:57
OperatableStateMachine.add('startopen',
FingerPositionState(result_topic="/m1n6s200_driver/fingers_action/finger_positions/result", action_topic="/m1n6s200_driver/fingers_action/finger_positions", robot_name="m1n6s200"),
transitions={'reached': 'precenter', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'finger_values': 'OPEN'})
# x:246 y:134
OperatableStateMachine.add('graspcenter',
FingerPositionState(result_topic="/m1n6s200_driver/fingers_action/finger_positions/result", action_topic="/m1n6s200_driver/fingers_action/finger_positions", robot_name="m1n6s200"),
transitions={'reached': 'preppourleft', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'finger_values': 'CLOSE'})
# x:244 y:63
OperatableStateMachine.add('startcenter',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'graspcenter', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'center_values', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:453 y:58
OperatableStateMachine.add('preppourleft',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'pourleft', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'prep_pour_to_left', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:664 y:52
OperatableStateMachine.add('pourleft',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'postpourleft', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'pour_to_left', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:860 y:41
OperatableStateMachine.add('postpourleft',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'endcenter', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'post_pour_to_left', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:1068 y:40
OperatableStateMachine.add('endcenter',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'releasecenter', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'center_values', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:30 y:213
OperatableStateMachine.add('backoffcenter',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'reached', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'back_off_center', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:34 y:133
OperatableStateMachine.add('precenter',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'startcenter', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'back_off_center', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:1068 y:120
OperatableStateMachine.add('releasecenter',
FingerPositionState(result_topic="/m1n6s200_driver/fingers_action/finger_positions/result", action_topic="/m1n6s200_driver/fingers_action/finger_positions", robot_name="m1n6s200"),
transitions={'reached': 'backoffcenter', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'finger_values': 'OPEN'})
# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365
_sm_stuff_with_left_cup_1 = OperatableStateMachine(outcomes=['planning_failed', 'control_failed', 'failed', 'reached'], input_keys=['left_values', 'post_pour_to_center', 'pre_grab_left', 'joint_names', 'CLOSE', 'prep_pour_to_center', 'pour_to_center', 'OPEN'])
with _sm_stuff_with_left_cup_1:
# x:1156 y:40
OperatableStateMachine.add('pregrableft',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'left', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'pre_grab_left', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:313 y:283
OperatableStateMachine.add('postppourcenter',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'left2', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'post_pour_to_center', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:549 y:280
OperatableStateMachine.add('left2',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'releaseleft', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'left_values', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:608 y:142
OperatableStateMachine.add('graspleft',
FingerPositionState(result_topic="/m1n6s200_driver/fingers_action/finger_positions/result", action_topic="/m1n6s200_driver/fingers_action/finger_positions", robot_name="m1n6s200"),
transitions={'reached': 'preppourcenter', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'finger_values': 'CLOSE'})
# x:298 y:199
OperatableStateMachine.add('preppourcenter',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'pourcenter', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'prep_pour_to_center', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:30 y:243
OperatableStateMachine.add('pourcenter',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'postppourcenter', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'pour_to_center', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:797 y:266
OperatableStateMachine.add('releaseleft',
FingerPositionState(result_topic="/m1n6s200_driver/fingers_action/finger_positions/result", action_topic="/m1n6s200_driver/fingers_action/finger_positions", robot_name="m1n6s200"),
transitions={'reached': 'backoffleft', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'finger_values': 'OPEN'})
# x:1137 y:263
OperatableStateMachine.add('backoffleft',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'reached', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'pre_grab_left', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
# x:876 y:95
OperatableStateMachine.add('left',
FeedbackJointStateToMoveit(position_topic="/m1n6s200_driver/joint_states", move_group="arm", action_topic="/move_group", robot_name="m1n6s200"),
transitions={'reached': 'graspleft', 'planning_failed': 'planning_failed', 'control_failed': 'control_failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_values': 'left_values', 'joint_names': 'joint_names', 'move_group': 'move_group', 'action_topic': 'action_topic'})
with _state_machine:
# x:192 y:60
OperatableStateMachine.add('stuff',
_sm_stuff_0,
transitions={'failed': 'failed', 'planning_failed': 'failed', 'control_failed': 'failed', 'reached': 'Stuff with left cup'},
autonomy={'failed': Autonomy.Inherit, 'planning_failed': Autonomy.Inherit, 'control_failed': Autonomy.Inherit, 'reached': Autonomy.Inherit},
remapping={'OPEN': 'OPEN', 'CLOSE': 'CLOSE', 'center_values': 'center_values', 'prep_pour_to_left': 'prep_pour_to_left', 'pour_to_left': 'pour_to_left', 'post_pour_to_left': 'post_pour_to_left', 'back_off_center': 'back_off_center', 'joint_names': 'joint_names'})
# x:590 y:46
OperatableStateMachine.add('Stuff with left cup',
_sm_stuff_with_left_cup_1,
transitions={'planning_failed': 'failed', 'control_failed': 'failed', 'failed': 'failed', 'reached': 'finished'},
autonomy={'planning_failed': Autonomy.Inherit, 'control_failed': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'reached': Autonomy.Inherit},
remapping={'left_values': 'left_values', 'post_pour_to_center': 'post_pour_to_center', 'pre_grab_left': 'pre_grab_left', 'joint_names': 'joint_names', 'CLOSE': 'CLOSE', 'prep_pour_to_center': 'prep_pour_to_center', 'pour_to_center': 'pour_to_center', 'OPEN': 'OPEN'})
return _state_machine
def create(self):
# x:958 y:78, x:440 y:324
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.joint_names = [
"m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3",
"m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"
]
_state_machine.userdata.center_values = [
4.825370393837993, 4.804768712277358, 1.7884682005958692,
2.781744729201632, 1.7624776125694588, 2.5668808924540394
]
_state_machine.userdata.prep_pour_to_left = [
4.8484381625680415, 4.322889801498073, 1.372345285529353,
3.0126762157540004, 1.4690217615247554, 2.627620406383804
]
_state_machine.userdata.pour_to_left = [
4.566518592733344, 4.3267512703163105, 1.393352300207898,
-3.4085460570465727, 7.739404454396004, 4.906765118866303
]
_state_machine.userdata.post_pour_to_left = [
4.8484381625680415, 4.322889801498073, 1.372345285529353,
3.0126762157540004, 1.4690217615247554, 2.627620406383804
]
_state_machine.userdata.left_values = [
4.501794723496712, 4.784133474886988, 1.6909002314255626,
2.766800400744653, 1.8037183931040444, 2.543646143523643
]
_state_machine.userdata.prep_pour_to_center = [
4.5690588912549435, 4.3365780179046835, 1.371823705429861,
2.7555946178259263, 1.6906042210704002, 2.5960829864389763
]
_state_machine.userdata.pour_to_center = [
4.704875670317358, 4.372941136262645, 1.5029825249035005,
-3.5267722999506783, 7.63555022663062, 0.3984061360462231
]
_state_machine.userdata.post_pour_to_center = [
4.5690588912549435, 4.3365780179046835, 1.371823705429861,
2.7555946178259263, 1.6906042210704002, 2.5960829864389763
]
_state_machine.userdata.OPEN = [0, 0]
_state_machine.userdata.CLOSE = [6400, 6400]
_state_machine.userdata.pre_grab_left = [
4.616985495390345, 4.361768642857545, 0.8309522662125534,
2.772490244413607, 1.7511775537481435, 2.6507113446153356
]
_state_machine.userdata.back_off_center = [
4.8380550301100405, 4.49428940291265, 1.2147491327564424,
2.784340512316133, 1.7494544885228622, 2.530367888644617
]
_state_machine.userdata.mid_pour_center = [
4.595038384847002, 4.374602948782854, 1.4727919986799805,
-3.5220619669306554, 7.626154061672603, 1.4440939079313413
]
_state_machine.userdata.mid_pour_left = [
4.639588276194066, 4.306920307575145, 1.3567719184228966,
-3.3707214464002866, 7.72652274420329, 4.057045223556825
]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:617 y:525, x:724 y:512, x:799 y:501, x:1346 y:582
_sm_stuff_with_left_cup_0 = OperatableStateMachine(
outcomes=[
'planning_failed', 'control_failed', 'failed', 'reached'
],
input_keys=[
'left_values', 'post_pour_to_center', 'pre_grab_left',
'joint_names', 'CLOSE', 'prep_pour_to_center',
'pour_to_center', 'OPEN', 'mid_pour_center'
])
with _sm_stuff_with_left_cup_0:
# x:57 y:91
OperatableStateMachine.add(
'pregrableft',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'left',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pre_grab_left',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:1058 y:196
OperatableStateMachine.add(
'postppourcenter',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'left2',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.High,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'post_pour_to_center',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:1338 y:85
OperatableStateMachine.add(
'left2',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'releaseleft',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.Low,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'left_values',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:77 y:478
OperatableStateMachine.add(
'graspleft',
FingerPositionState(
result_topic=
"/m1n6s200_driver/fingers_action/finger_positions/result",
action_topic=
"/m1n6s200_driver/fingers_action/finger_positions",
robot_name="m1n6s200"),
transitions={
'reached': 'preppourcenter',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Low,
'failed': Autonomy.Off
},
remapping={'finger_values': 'CLOSE'})
# x:377 y:194
OperatableStateMachine.add(
'preppourcenter',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'mid pour',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.High,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'prep_pour_to_center',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:832 y:195
OperatableStateMachine.add(
'pourcenter',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'postppourcenter',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.High,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pour_to_center',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:1356 y:253
OperatableStateMachine.add(
'releaseleft',
FingerPositionState(
result_topic=
"/m1n6s200_driver/fingers_action/finger_positions/result",
action_topic=
"/m1n6s200_driver/fingers_action/finger_positions",
robot_name="m1n6s200"),
transitions={
'reached': 'backoffleft',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'finger_values': 'OPEN'})
# x:1348 y:408
OperatableStateMachine.add(
'backoffleft',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'reached',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pre_grab_left',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:76 y:267
OperatableStateMachine.add(
'left',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'graspleft',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.Low,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'left_values',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:605 y:173
OperatableStateMachine.add(
'mid pour',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'pourcenter',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'mid_pour_center',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:941 y:608, x:928 y:554, x:986 y:505, x:1150 y:560
_sm_stuff_1 = OperatableStateMachine(
outcomes=[
'failed', 'planning_failed', 'control_failed', 'reached'
],
input_keys=[
'OPEN', 'CLOSE', 'center_values', 'prep_pour_to_left',
'pour_to_left', 'post_pour_to_left', 'back_off_center',
'joint_names', 'mid_pour_left'
],
output_keys=['joint_names'])
with _sm_stuff_1:
# x:31 y:164
OperatableStateMachine.add(
'startopen',
FingerPositionState(
result_topic=
"/m1n6s200_driver/fingers_action/finger_positions/result",
action_topic=
"/m1n6s200_driver/fingers_action/finger_positions",
robot_name="m1n6s200"),
transitions={
'reached': 'precenter',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Low,
'failed': Autonomy.Off
},
remapping={'finger_values': 'OPEN'})
# x:263 y:371
OperatableStateMachine.add(
'graspcenter',
FingerPositionState(
result_topic=
"/m1n6s200_driver/fingers_action/finger_positions/result",
action_topic=
"/m1n6s200_driver/fingers_action/finger_positions",
robot_name="m1n6s200"),
transitions={
'reached': 'preppourleft',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Low,
'failed': Autonomy.Off
},
remapping={'finger_values': 'CLOSE'})
# x:270 y:272
OperatableStateMachine.add(
'startcenter',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'graspcenter',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.Low,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'center_values',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:566 y:100
OperatableStateMachine.add(
'preppourleft',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'midpourleft',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.High,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'prep_pour_to_left',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:1060 y:95
OperatableStateMachine.add(
'pourleft',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'postpourleft',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.High,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pour_to_left',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:1315 y:96
OperatableStateMachine.add(
'postpourleft',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'endcenter',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.High,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'post_pour_to_left',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:1621 y:101
OperatableStateMachine.add(
'endcenter',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'releasecenter',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.Low,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'center_values',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:1613 y:419
OperatableStateMachine.add(
'backoffcenter',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'reached',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'back_off_center',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:270 y:149
OperatableStateMachine.add(
'precenter',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'startcenter',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'back_off_center',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
# x:1625 y:262
OperatableStateMachine.add(
'releasecenter',
FingerPositionState(
result_topic=
"/m1n6s200_driver/fingers_action/finger_positions/result",
action_topic=
"/m1n6s200_driver/fingers_action/finger_positions",
robot_name="m1n6s200"),
transitions={
'reached': 'backoffcenter',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'finger_values': 'OPEN'})
# x:813 y:81
OperatableStateMachine.add(
'midpourleft',
FeedbackJointStateToMoveit(
position_topic="/m1n6s200_driver/joint_states",
move_group="arm",
action_topic="/move_group",
robot_name="m1n6s200"),
transitions={
'reached': 'pourleft',
'planning_failed': 'planning_failed',
'control_failed': 'control_failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'mid_pour_left',
'joint_names': 'joint_names',
'move_group': 'move_group',
'action_topic': 'action_topic'
})
with _state_machine:
# x:247 y:69
OperatableStateMachine.add('stuff',
_sm_stuff_1,
transitions={
'failed': 'failed',
'planning_failed': 'failed',
'control_failed': 'failed',
'reached': 'Stuff with left cup'
},
autonomy={
'failed': Autonomy.Inherit,
'planning_failed': Autonomy.Inherit,
'control_failed': Autonomy.Inherit,
'reached': Autonomy.Inherit
},
remapping={
'OPEN': 'OPEN',
'CLOSE': 'CLOSE',
'center_values': 'center_values',
'prep_pour_to_left':
'prep_pour_to_left',
'pour_to_left': 'pour_to_left',
'post_pour_to_left':
'post_pour_to_left',
'back_off_center':
'back_off_center',
'joint_names': 'joint_names',
'mid_pour_left': 'mid_pour_left'
})
# x:550 y:74
OperatableStateMachine.add('Stuff with left cup',
_sm_stuff_with_left_cup_0,
transitions={
'planning_failed': 'failed',
'control_failed': 'failed',
'failed': 'failed',
'reached': 'stuff'
},
autonomy={
'planning_failed': Autonomy.Inherit,
'control_failed': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'reached': Autonomy.Inherit
},
remapping={
'left_values': 'left_values',
'post_pour_to_center':
'post_pour_to_center',
'pre_grab_left': 'pre_grab_left',
'joint_names': 'joint_names',
'CLOSE': 'CLOSE',
'prep_pour_to_center':
'prep_pour_to_center',
'pour_to_center': 'pour_to_center',
'OPEN': 'OPEN',
'mid_pour_center': 'mid_pour_center'
})
return _state_machine