def create(self):
part_list = ['gasket_part', 'piston_rod_part', 'gear_part', ' pulley_part', 'disk_part']
# x:842 y:52, x:158 y:179
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.camera_ref_frame = "arm1_linear_arm_actuator"
_state_machine.userdata.camera_topic = "/ariac/logical_camera_1"
_state_machine.userdata.camera_frame = "logical_camera_1_frame"
_state_machine.userdata.pose = []
_state_machine.userdata.part = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:33 y:38
OperatableStateMachine.add('StaerAssignment',
StartAssignment(),
transitions={'continue': 'DetectFirstPartCamera'},
autonomy={'continue': Autonomy.Off})
# x:379 y:40
OperatableStateMachine.add('PartMessage',
MessageState(),
transitions={'continue': 'PoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part'})
# x:523 y:40
OperatableStateMachine.add('PoseMessage',
MessageState(),
transitions={'continue': 'EndAssignment'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'pose'})
# x:666 y:37
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:170 y:40
OperatableStateMachine.add('DetectFirstPartCamera',
DetectFirstPartCameraAriacState(part_list=part_list, time_out=0.5),
transitions={'continue': 'PartMessage', 'failed': 'failed', 'not_found': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'camera_ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part', 'pose': 'pose'})
return _state_machine
def create(self):
# x:30 y:365
_state_machine = OperatableStateMachine(outcomes=['finished'])
_state_machine.userdata.GetalA = 10
_state_machine.userdata.GetalB = 5
_state_machine.userdata.GetalC = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Add',
AddNumericState(),
transitions={'done': 'Result'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'GetalA',
'value_b': 'GetalB',
'result': 'GetalC'
})
# x:211 y:41
OperatableStateMachine.add('Result',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'GetalC'})
return _state_machine
def create(self):
# x:502 y:44, x:223 y:140
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type', 'agv_id', 'pose_on_agv'])
_state_machine.userdata.part_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.pose_on_agv = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
# x:305 y:37
OperatableStateMachine.add('MoseMessage',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'pose_on_agv'})
# x:171 y:37
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'MoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
return _state_machine
def create(self):
# x:11 y:218
_state_machine = OperatableStateMachine(outcomes=['finished'])
_state_machine.userdata.number_of_interations = 5
_state_machine.userdata.iteration = 0
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:134 y:46
OperatableStateMachine.add('Add',
AddNumericState(),
transitions={'done': 'Compare'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'one_value',
'value_b': 'iteration',
'result': 'iteration'
})
# x:348 y:202
OperatableStateMachine.add('Message',
MessageState(),
transitions={'continue': 'Add'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'iteration'})
# x:345 y:46
OperatableStateMachine.add('Compare',
GreaterNumericState(),
transitions={
'true': 'finished',
'false': 'Message'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'iteration',
'value_b': 'number_of_interations'
})
return _state_machine
def create(self):
# x:705 y:41
_state_machine = OperatableStateMachine(outcomes=['finished'])
_state_machine.userdata.MaterialLocations = []
_state_machine.userdata.Part = 'piston_rod_part'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:34 y:25
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={'continue': 'GetMaterialsLocations'},
autonomy={'continue': Autonomy.Off})
# x:362 y:28
OperatableStateMachine.add(
'MaterialsLocationMessage',
MessageState(),
transitions={'continue': 'EndAssignment'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'MaterialLocations'})
# x:541 y:29
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:176 y:26
OperatableStateMachine.add(
'GetMaterialsLocations',
GetMaterialLocationsState(),
transitions={'continue': 'MaterialsLocationMessage'},
autonomy={'continue': Autonomy.Off},
remapping={
'part': 'Part',
'material_locations': 'MaterialLocations'
})
return _state_machine
def create(self):
# x:1393 y:668, x:704 y:381
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type', 'agv_id', 'pose_on_agv'])
_state_machine.userdata.part_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.config_name_R1Bin1Pre = 'R1Bin1Pre'
_state_machine.userdata.config_name_R1AGVPre = 'R1AGVPre'
_state_machine.userdata.config_name_R2Bin5Pre = 'R2Bin5Pre'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.arm2_gebruiken = '/ariac/arm2'
_state_machine.userdata.part_pose = []
_state_machine.userdata.part = 'gasket_part'
_state_machine.userdata.part_offset = 0.030
_state_machine.userdata.part_drop_offset = 0.1
_state_machine.userdata.conveyor_power = 100.0
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.camera_frame = 'logical_camera_3_frame'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_3'
_state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.bin_location = []
_state_machine.userdata.material_locations = []
_state_machine.userdata.zero_value = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:12 y:198
OperatableStateMachine.add(
'get part location',
GetMaterialLocationsState(),
transitions={'continue': 'bin location'},
autonomy={'continue': Autonomy.Off},
remapping={
'part': 'part_type',
'material_locations': 'material_locations'
})
# x:305 y:37
OperatableStateMachine.add(
'MoseMessage',
MessageState(),
transitions={'continue': 'detect object'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'pose_on_agv'})
# x:171 y:37
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'MoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:681 y:38
OperatableStateMachine.add('pre grasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'compute grasp',
'planning_failed': 'failed',
'control_failed': 'compute grasp',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R1Bin1Pre',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1433 y:179
OperatableStateMachine.add('move_agv',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'gripper uit',
'planning_failed': 'failed',
'control_failed': 'gripper uit',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R1AGVPre',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1255 y:51
OperatableStateMachine.add('gripper aan',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'wait',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1405 y:434
OperatableStateMachine.add('robot_2_selecteren',
ReplaceState(),
transitions={'done': 'robot_2_bewegen'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm2_gebruiken',
'result': 'move_group_prefix'
})
# x:1424 y:526
OperatableStateMachine.add('robot_2_bewegen',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R2Bin5Pre',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:465 y:35
OperatableStateMachine.add(
'detect object',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'pre grasp',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part',
'pose': 'pose'
})
# x:862 y:38
OperatableStateMachine.add(
'compute grasp',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'pick_positie',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1475 y:50
OperatableStateMachine.add('wait',
WaitState(wait_time=1),
transitions={'done': 'move_agv'},
autonomy={'done': Autonomy.Off})
# x:1414 y:297
OperatableStateMachine.add('gripper uit',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'robot_2_selecteren',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1047 y:40
OperatableStateMachine.add('pick_positie',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'gripper aan',
'planning_failed': 'failed',
'control_failed': 'gripper aan'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:150 y:300
OperatableStateMachine.add('bin location',
GetItemFromListState(),
transitions={
'done': 'AgvIdMessage',
'invalid_index': 'failed'
},
autonomy={
'done': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'list': 'material_locations',
'index': 'zero_value',
'item': 'bin_location'
})
# x:176 y:126
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
return _state_machine
def create(self):
# x:719 y:341, x:826 y:25
_state_machine = OperatableStateMachine(
outcomes=['finished', 'fail'],
input_keys=['Products', 'NumberOfProducts'])
_state_machine.userdata.ProductIterator = 0
_state_machine.userdata.OneValue = 1
_state_machine.userdata.ProductType = ''
_state_machine.userdata.ProductPose = []
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.MaterialsLocationList = []
_state_machine.userdata.MaterialLocation = ''
_state_machine.userdata.MaterailLocationIndex = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:356 y:121
OperatableStateMachine.add('GetProduct',
GetPartFromProductsState(),
transitions={
'continue': 'ProductTypeMessage',
'invalid_index': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'Products',
'index': 'ProductIterator',
'type': 'ProductType',
'pose': 'ProductPose'
})
# x:817 y:258
OperatableStateMachine.add(
'IncrementProductIterator',
AddNumericState(),
transitions={'done': 'CompareProductIterator'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'ProductIterator',
'value_b': 'OneValue',
'result': 'ProductIterator'
})
# x:625 y:256
OperatableStateMachine.add('CompareProductIterator',
EqualState(),
transitions={
'true': 'finished',
'false': 'GetProduct'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'ProductIterator',
'value_b': 'NumberOfProducts'
})
# x:569 y:121
OperatableStateMachine.add(
'ProductTypeMessage',
MessageState(),
transitions={'continue': 'ProductPoseMassage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ProductType'})
# x:728 y:120
OperatableStateMachine.add(
'ProductPoseMassage',
MessageState(),
transitions={'continue': 'GetMaterialsLocation'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ProductPose'})
# x:877 y:120
OperatableStateMachine.add(
'GetMaterialsLocation',
GetMaterialLocationsState(),
transitions={'continue': 'MaterialsLocationListMessage'},
autonomy={'continue': Autonomy.Off},
remapping={
'part': 'ProductType',
'material_locations': 'MaterialsLocationList'
})
# x:1046 y:119
OperatableStateMachine.add(
'MaterialsLocationListMessage',
MessageState(),
transitions={'continue': 'GerMaterailLocation'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'MaterialsLocationList'})
# x:1226 y:120
OperatableStateMachine.add('GerMaterailLocation',
GetItemFromListState(),
transitions={
'done': 'MaterailLocationMessage',
'invalid_index': 'fail'
},
autonomy={
'done': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'list': 'MaterialsLocationList',
'index': 'MaterailLocationIndex',
'item': 'MaterialLocation'
})
# x:1406 y:124
OperatableStateMachine.add(
'MaterailLocationMessage',
MessageState(),
transitions={'continue': 'IncrementProductIterator'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'MaterialLocation'})
return _state_machine
def create(self):
# x:1089 y:770, x:1143 y:423, x:1266 y:674
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'end'],
input_keys=[
'move_group_prefix', 'move_group_right_arm',
'move_group_left_arm', 'action_topic', 'camera_frame',
'camera_topic', 'ref_frame', 'move_group_gantry', 'robot_name',
'offset_var_left', 'offset_var_right', 'offset_gasket',
'offset_piston', 'arm_idL', 'arm_idR',
'config_name_Right_Home', 'config_name_Left_Home',
'config_name_RA_pre', 'config_name_LA_pre'
])
_state_machine.userdata.move_group_prefix = ''
_state_machine.userdata.move_group_right_arm = ''
_state_machine.userdata.move_group_left_arm = ''
_state_machine.userdata.action_topic = ''
_state_machine.userdata.camera_frame = '/ariac/logical_camera_0'
_state_machine.userdata.camera_topic = 'logical_camera_0_frame'
_state_machine.userdata.ref_frame = ''
_state_machine.userdata.move_group_gantry = ''
_state_machine.userdata.robot_name = ''
_state_machine.userdata.offset_var_left = 0
_state_machine.userdata.offset_var_right = 0
_state_machine.userdata.offset_gasket = 0
_state_machine.userdata.offset_piston = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.tool_link_right = 'right_ee_link'
_state_machine.userdata.arm_idL = ''
_state_machine.userdata.arm_idR = ''
_state_machine.userdata.config_name_Right_Home = ''
_state_machine.userdata.config_name_Left_Home = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.bin_id = ''
_state_machine.userdata.bin1 = 'bin1'
_state_machine.userdata.bin2 = 'bin2'
_state_machine.userdata.offset_id = 0
_state_machine.userdata.arm_id = ''
_state_machine.userdata.config_name_R_bin1 = 'Gantry_PreGrasp_R_bin1'
_state_machine.userdata.config_name_L_bin1 = 'Gantry_PreGrasp_L_bin1'
_state_machine.userdata.config_name_bin1_var = ''
_state_machine.userdata.config_name_RA_pre = ''
_state_machine.userdata.config_name_LA_pre = ''
_state_machine.userdata.offsetx_bin1 = 0
_state_machine.userdata.offsety_bin1 = 0
_state_machine.userdata.offsetx_set = 0.33418
_state_machine.userdata.offsety_set = 0.1827866667
_state_machine.userdata.plus1 = 1
_state_machine.userdata.xproduct_bin1 = 0
_state_machine.userdata.yproduct_bin1 = 0
_state_machine.userdata.drie = 3
_state_machine.userdata.twee = 2
_state_machine.userdata.zero = 0
_state_machine.userdata.offsety_bin2 = 0
_state_machine.userdata.offsetx_bin2 = 0
_state_machine.userdata.yproduct_bin2 = 0
_state_machine.userdata.xproduct_bin2 = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:1623 y:384, x:886 y:382, x:1759 y:258
_sm_offset_verschuiven_0 = OperatableStateMachine(
outcomes=['finished', 'failed', 'end'],
input_keys=[
'bin_id', 'bin1', 'zero', 'twee', 'drie', 'bin2',
'offsetx_set', 'offsety_set', 'plus1', 'offsetx_bin1',
'offsety_bin2', 'offsetx_bin2', 'offsety_bin1',
'xproduct_bin1', 'yproduct_bin1', 'xproduct_bin2',
'yproduct_bin2'
],
output_keys=[
'offsety_bin2', 'offsetx_bin2', 'offsety_bin1', 'offsetx_bin1'
])
with _sm_offset_verschuiven_0:
# x:146 y:40
OperatableStateMachine.add('bin1 vergelijk',
EqualState(),
transitions={
'true': 'set offsetyyy',
'false': 'set offsetyy'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin1',
'value_b': 'bin_id'
})
# x:610 y:50
OperatableStateMachine.add(
'x producten tellen',
AddNumericState(),
transitions={'done': '3 products x bin?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'xproduct_bin1',
'value_b': 'plus1',
'result': 'xproduct_bin1'
})
# x:818 y:45
OperatableStateMachine.add('3 products x bin?',
EqualState(),
transitions={
'true': 'set offsety',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'xproduct_bin1',
'value_b': 'drie'
})
# x:1069 y:54
OperatableStateMachine.add(
'set offsety',
ReplaceState(),
transitions={'done': 'set offsety back to zero'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_set',
'result': 'offsety_bin1'
})
# x:1263 y:51
OperatableStateMachine.add('set offsety back to zero',
ReplaceState(),
transitions={'done': '2 products x?'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsety_bin1'
})
# x:1471 y:55
OperatableStateMachine.add('2 products x?',
EqualState(),
transitions={
'true': 'st offsetx back to zero',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'twee',
'value_b': 'xproduct_bin1'
})
# x:389 y:38
OperatableStateMachine.add(
'set offsetyyy',
AddNumericState(),
transitions={'done': 'x producten tellen'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'offsety_set',
'value_b': 'offsetx_bin1',
'result': 'offsetx_bin1'
})
# x:610 y:161
OperatableStateMachine.add(
'y producten tellen_2',
AddNumericState(),
transitions={'done': '3 products y bin2?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'yproduct_bin2',
'value_b': 'plus1',
'result': 'yproduct_bin2'
})
# x:818 y:152
OperatableStateMachine.add('3 products y bin2?',
EqualState(),
transitions={
'true': 'set offsety_2',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'yproduct_bin2',
'value_b': 'drie'
})
# x:1066 y:165
OperatableStateMachine.add(
'set offsety_2',
ReplaceState(),
transitions={'done': 'set offsety back to zero_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_set',
'result': 'offsetx_bin2'
})
# x:1263 y:176
OperatableStateMachine.add(
'set offsety back to zero_2',
ReplaceState(),
transitions={'done': '2 products x bin2?'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsety_bin2'
})
# x:1472 y:149
OperatableStateMachine.add('2 products x bin2?',
EqualState(),
transitions={
'true':
'set offsetx back to zero bin2',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'twee',
'value_b': 'xproduct_bin2'
})
# x:389 y:152
OperatableStateMachine.add(
'set offsetyy',
AddNumericState(),
transitions={'done': 'y producten tellen_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'offsety_set',
'value_b': 'offsety_bin2',
'result': 'offsety_bin2'
})
# x:1651 y:50
OperatableStateMachine.add('st offsetx back to zero',
ReplaceState(),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsetx_bin1'
})
# x:1653 y:146
OperatableStateMachine.add('set offsetx back to zero bin2',
ReplaceState(),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsetx_bin2'
})
# x:30 y:401, x:130 y:401
_sm_setten_voor_right_1 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'arm_idR', 'move_group_right_arm', 'config_name_Right_Home',
'tool_link_right', 'config_name_RA_pre', 'offset_var_right'
],
output_keys=[
'arm_id_var', 'move_group_var', 'config_name_var',
'tool_link_var', 'config_name_pre_var', 'offset_id'
])
with _sm_setten_voor_right_1:
# x:30 y:40
OperatableStateMachine.add('Set arm id',
ReplaceState(),
transitions={'done': 'Set config name'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_idR',
'result': 'arm_id_var'
})
# x:30 y:117
OperatableStateMachine.add('Set config name',
ReplaceState(),
transitions={'done': 'Set toollink'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_Right_Home',
'result': 'config_name_var'
})
# x:30 y:194
OperatableStateMachine.add('Set toollink',
ReplaceState(),
transitions={'done': 'Set move group'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'tool_link_right',
'result': 'tool_link_var'
})
# x:30 y:271
OperatableStateMachine.add(
'Set move group',
ReplaceState(),
transitions={'done': 'Set config name_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'move_group_right_arm',
'result': 'move_group_var'
})
# x:230 y:266
OperatableStateMachine.add(
'Set config name_2',
ReplaceState(),
transitions={'done': 'set config pre pick'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_var_right',
'result': 'offset_id'
})
# x:233 y:344
OperatableStateMachine.add('laat zien',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'move_group_var'})
# x:438 y:268
OperatableStateMachine.add('set config pre pick',
ReplaceState(),
transitions={'done': 'laat zien'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_RA_pre',
'result': 'config_name_pre_var'
})
# x:30 y:401
_sm_setten_voor_left_2 = OperatableStateMachine(
outcomes=['finished'],
input_keys=[
'arm_idL', 'move_group_left_arm', 'config_name_Left_Home',
'tool_link_left', 'config_name_LA_pre', 'offset_var_left'
],
output_keys=[
'arm_id_var', 'move_group_var', 'config_name_var',
'tool_link_var', 'config_name_pre_var', 'offset_id'
])
with _sm_setten_voor_left_2:
# x:30 y:40
OperatableStateMachine.add('Set arm id',
ReplaceState(),
transitions={'done': 'Set config name'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_idL',
'result': 'arm_id_var'
})
# x:30 y:117
OperatableStateMachine.add('Set config name',
ReplaceState(),
transitions={'done': 'Set toollink'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_Left_Home',
'result': 'config_name_var'
})
# x:30 y:194
OperatableStateMachine.add('Set toollink',
ReplaceState(),
transitions={'done': 'Set move group'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'tool_link_left',
'result': 'tool_link_var'
})
# x:30 y:271
OperatableStateMachine.add('Set move group',
ReplaceState(),
transitions={'done': 'Set offset'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'move_group_left_arm',
'result': 'move_group_var'
})
# x:397 y:386
OperatableStateMachine.add('laat zien',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'move_group_var'})
# x:383 y:274
OperatableStateMachine.add('set config name pre pick',
ReplaceState(),
transitions={'done': 'laat zien'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_LA_pre',
'result': 'config_name_pre_var'
})
# x:209 y:266
OperatableStateMachine.add(
'Set offset',
ReplaceState(),
transitions={'done': 'set config name pre pick'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_var_left',
'result': 'offset_id'
})
with _state_machine:
# x:97 y:17
OperatableStateMachine.add(
'Setten voor Left',
_sm_setten_voor_left_2,
transitions={'finished': 'Left product'},
autonomy={'finished': Autonomy.Inherit},
remapping={
'arm_idL': 'arm_idL',
'move_group_left_arm': 'move_group_left_arm',
'config_name_Left_Home': 'config_name_Left_Home',
'tool_link_left': 'tool_link_left',
'config_name_LA_pre': 'config_name_LA_pre',
'offset_var_left': 'offset_var_left',
'arm_id_var': 'arm_id_var',
'move_group_var': 'move_group_var',
'config_name_var': 'config_name_var',
'tool_link_var': 'tool_link_var',
'config_name_pre_var': 'config_name_pre_var',
'offset_id': 'offset_id'
})
# x:190 y:708
OperatableStateMachine.add(
'Setten voor Right',
_sm_setten_voor_right_1,
transitions={
'finished': 'Right product',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'arm_idR': 'arm_idR',
'move_group_right_arm': 'move_group_right_arm',
'config_name_Right_Home': 'config_name_Right_Home',
'tool_link_right': 'tool_link_right',
'config_name_RA_pre': 'config_name_RA_pre',
'offset_var_right': 'offset_var_right',
'arm_id_var': 'arm_id_var',
'move_group_var': 'move_group_var',
'config_name_var': 'config_name_var',
'tool_link_var': 'tool_link_var',
'config_name_pre_var': 'config_name_pre_var',
'offset_id': 'offset_id'
})
# x:77 y:172
OperatableStateMachine.add('Left product',
EqualState(),
transitions={
'true': 'bin1',
'false': 'bin2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'offset_gasket',
'value_b': 'offset_id'
})
# x:311 y:29
OperatableStateMachine.add('bin1',
ReplaceState(),
transitions={'done': 'Zie bin1_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin1',
'result': 'bin_id'
})
# x:300 y:252
OperatableStateMachine.add('bin2',
ReplaceState(),
transitions={'done': 'Zie bin1_2_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin2',
'result': 'bin_id'
})
# x:116 y:534
OperatableStateMachine.add('Right product',
EqualState(),
transitions={
'true': 'bin1_2',
'false': 'bin2_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'offset_gasket',
'value_b': 'offset_id'
})
# x:1673 y:531
OperatableStateMachine.add('turn off vacuum',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'Move to home',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_var'})
# x:299 y:610
OperatableStateMachine.add('bin1_2',
ReplaceState(),
transitions={'done': 'Zie bin1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin1',
'result': 'bin_id'
})
# x:294 y:377
OperatableStateMachine.add('bin2_2',
ReplaceState(),
transitions={'done': 'Zie bin2_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin2',
'result': 'bin_id'
})
# x:731 y:21
OperatableStateMachine.add('gantry bin pose',
ReplaceState(),
transitions={'done': 'Move to bin'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_L_bin1',
'result': 'config_name_bin1_var'
})
# x:722 y:342
OperatableStateMachine.add('gantry bin pose_2',
ReplaceState(),
transitions={'done': 'Move to bin'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_R_bin1',
'result': 'config_name_bin1_var'
})
# x:1691 y:675
OperatableStateMachine.add('Move to home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Offset verschuiven',
'planning_failed': 'retry_4',
'control_failed': 'retry_4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_var',
'move_group': 'move_group_var',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:987 y:701
OperatableStateMachine.add('Right robot end',
EqualState(),
transitions={
'true': 'finished',
'false': 'Setten voor Right'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'move_group_right_arm',
'value_b': 'move_group_var'
})
# x:954 y:146
OperatableStateMachine.add('retry',
WaitState(wait_time=0.1),
transitions={'done': 'Move to bin'},
autonomy={'done': Autonomy.Off})
# x:1578 y:610
OperatableStateMachine.add('retry_4',
WaitState(wait_time=0.1),
transitions={'done': 'Move to home'},
autonomy={'done': Autonomy.Off})
# x:956 y:21
OperatableStateMachine.add('Move to bin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Check for bin',
'planning_failed': 'retry',
'control_failed': 'retry',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1_var',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1691 y:248
OperatableStateMachine.add(
'Computebin right',
ComputeDropBinsState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move to drop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_var',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_var',
'pose': 'pose',
'offset': 'offset_id',
'rotation': 'rotation',
'offsety': 'offsety',
'offsetx': 'offsetx',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1550 y:336
OperatableStateMachine.add(
'Computebin left',
ComputeDropBinsState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move to drop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_var',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_var',
'pose': 'pose',
'offset': 'offset_id',
'rotation': 'rotation',
'offsety': 'offsety',
'offsetx': 'offsetx',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:516 y:599
OperatableStateMachine.add('Zie bin1',
GetObjectPoseState(
object_frame='bin1_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:529 y:34
OperatableStateMachine.add('Zie bin1_2',
GetObjectPoseState(
object_frame='bin1_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:534 y:239
OperatableStateMachine.add('Zie bin1_2_2',
GetObjectPoseState(
object_frame='bin2_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:523 y:373
OperatableStateMachine.add('Zie bin2_2',
GetObjectPoseState(
object_frame='bin2_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:1668 y:30
OperatableStateMachine.add('Right robot end_2',
EqualState(),
transitions={
'true': 'Computebin right',
'false': 'Computebin left'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'move_group_right_arm',
'value_b': 'move_group_var'
})
# x:1657 y:420
OperatableStateMachine.add('Move to drop',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'turn off vacuum',
'planning_failed': 'failed',
'control_failed': 'turn off vacuum'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_var',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1428 y:672
OperatableStateMachine.add('Offset verschuiven',
_sm_offset_verschuiven_0,
transitions={
'finished': 'Right robot end',
'failed': 'failed',
'end': 'end'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'end': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'bin1': 'bin1',
'zero': 'zero',
'twee': 'twee',
'drie': 'drie',
'bin2': 'bin2',
'offsetx_set': 'offsetx_set',
'offsety_set': 'offsety_set',
'plus1': 'plus1',
'offsetx_bin1': 'offsetx_bin1',
'offsety_bin2': 'offsety_bin2',
'offsetx_bin2': 'offsetx_bin2',
'offsety_bin1': 'offsety_bin1',
'xproduct_bin1': 'xproduct_bin1',
'yproduct_bin1': 'yproduct_bin1',
'xproduct_bin2': 'xproduct_bin2',
'yproduct_bin2': 'yproduct_bin2'
})
# x:1156 y:3
OperatableStateMachine.add('Check for bin',
EqualState(),
transitions={
'true': 'set offset voor bin 1',
'false': 'set offset voor bin 2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin1',
'value_b': 'bin_id'
})
# x:1406 y:0
OperatableStateMachine.add(
'set offset voor bin 1',
ReplaceState(),
transitions={'done': 'set offset voor y bin 1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_bin1',
'result': 'offsetx'
})
# x:1441 y:64
OperatableStateMachine.add(
'set offset voor y bin 1',
ReplaceState(),
transitions={'done': 'Right robot end_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsety_bin1',
'result': 'offsety'
})
# x:1236 y:110
OperatableStateMachine.add(
'set offset voor bin 2',
ReplaceState(),
transitions={'done': 'set offset voor y bin 2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_bin2',
'result': 'offsetx'
})
# x:1425 y:141
OperatableStateMachine.add(
'set offset voor y bin 2',
ReplaceState(),
transitions={'done': 'Right robot end_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsety_bin2',
'result': 'offsety'
})
return _state_machine
def create(self):
# x:24 y:502, x:938 y:468
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type', 'agv_id', 'part_pose'],
output_keys=['pose'])
_state_machine.userdata.arm_id = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.offset = 0.2
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = ''
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.srdf_param = 'ur10.srdf'
_state_machine.userdata.ref_frame1 = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.pose = []
_state_machine.userdata.part = ''
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.part_offset = 0
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.agv_pose = []
_state_machine.userdata.material_locations = []
_state_machine.userdata.bin_location = ''
_state_machine.userdata.zero_value = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.part_type = ''
_state_machine.userdata.ref_frame2 = 'arm2_linear_arm_actuator'
_state_machine.userdata.agv2 = 'agv2'
_state_machine.userdata.ref_frame = ''
_state_machine.userdata.part1 = 'gasket_part'
_state_machine.userdata.part2 = 'pulley_part'
_state_machine.userdata.part3 = 'piston_rod_part'
_state_machine.userdata.part4 = 'gear_part'
_state_machine.userdata.rotation = 0
_state_machine.userdata.overzet = ''
_state_machine.userdata.overzetja = 'ja'
_state_machine.userdata.overzetnee = 'nee'
_state_machine.userdata.agv1_id = 'agv1'
_state_machine.userdata.agv2_id = 'agv2'
_state_machine.userdata.config_namer1b1 = 'R1PreBin1'
_state_machine.userdata.config_namer2b4 = 'R2PreBin6'
_state_machine.userdata.config_name_preagv = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:468 y:36
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
# x:648 y:35
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'MoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:460 y:161
OperatableStateMachine.add('R1PreGrasp1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'OpenGripper',
'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',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:688 y:174
OperatableStateMachine.add('OpenGripper',
UseGripper(enable=False),
transitions={
'continue': 'ComputePick',
'failed': 'failed',
'invalid_arm': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:235 y:96
OperatableStateMachine.add('CheckPosePartsinBIn',
DetectPartCameraAriacState(time_out=2),
transitions={
'continue': 'R1PreGrasp1',
'failed': 'retry',
'not_found': 'retry'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:1325 y:349
OperatableStateMachine.add('R1PreGrasp1Back',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Overzetten?',
'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',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:884 y:176
OperatableStateMachine.add(
'ComputePick',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'R1ToPick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1106 y:178
OperatableStateMachine.add('R1ToPick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'CloseGripper',
'planning_failed': 'failed',
'control_failed': 'R1PreGrasp1'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1336 y:188
OperatableStateMachine.add('CloseGripper',
UseGripper(enable=True),
transitions={
'continue': 'Wacht1',
'failed': 'failed',
'invalid_arm': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1517 y:718
OperatableStateMachine.add('R1PreAGV1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePlacePose',
'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_preagv',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1352 y:259
OperatableStateMachine.add('Wacht1',
WaitState(wait_time=1),
transitions={'done': 'R1PreGrasp1Back'},
autonomy={'done': Autonomy.Off})
# x:986 y:718
OperatableStateMachine.add('R1ToPlace',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'OpenGripper2',
'planning_failed': 'Retry',
'control_failed': 'Retry'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:293 y:698
OperatableStateMachine.add('R1PreAGV1Back',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Robot?',
'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_preagv',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:679 y:693
OperatableStateMachine.add('OpenGripper2',
UseGripper(enable=False),
transitions={
'continue': 'Wacht2',
'failed': 'failed',
'invalid_arm': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:488 y:694
OperatableStateMachine.add('Wacht2',
WaitState(wait_time=1),
transitions={'done': 'R1PreAGV1Back'},
autonomy={'done': Autonomy.Off})
# x:951 y:616
OperatableStateMachine.add('Retry',
WaitState(wait_time=0.1),
transitions={'done': 'R1ToPlace'},
autonomy={'done': Autonomy.Off})
# x:834 y:33
OperatableStateMachine.add(
'MoseMessage',
MessageState(),
transitions={'continue': 'CameraRobotSelector'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_pose'})
# x:1233 y:728
OperatableStateMachine.add(
'ComputePlacePose',
ComputeGraspPartOffsetAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'R1ToPlace',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'agv_pose',
'offset': 'offset',
'rotation': 'rotation',
'part_pose': 'part_pose',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:266 y:9
OperatableStateMachine.add(
'retry',
WaitState(wait_time=0.5),
transitions={'done': 'CheckPosePartsinBIn'},
autonomy={'done': Autonomy.Off})
# x:42 y:416
OperatableStateMachine.add('RefMSG',
MessageState(),
transitions={'continue': 'TopicMSG'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ref_frame'})
# x:190 y:412
OperatableStateMachine.add('TopicMSG',
MessageState(),
transitions={'continue': 'FrameMSG'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'camera_topic'})
# x:330 y:414
OperatableStateMachine.add(
'FrameMSG',
MessageState(),
transitions={'continue': 'CheckPosePartsinBIn'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'camera_frame'})
# x:133 y:217
OperatableStateMachine.add('CameraRobotSelector',
self.use_behavior(
CameraRobotSelectorSM,
'CameraRobotSelector'),
transitions={
'finished': 'RefMSG',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'agv_id':
'agv_id',
'part_type':
'part_type',
'part_pose':
'part_pose',
'move_group_prefix':
'move_group_prefix',
'config_name':
'config_name',
'arm_id':
'arm_id',
'ref_frame':
'ref_frame',
'camera_topic':
'camera_topic',
'camera_frame':
'camera_frame',
'part':
'part',
'part_offset':
'part_offset',
'overzet':
'overzet',
'config_name_preagv':
'config_name_preagv'
})
# x:1551 y:346
OperatableStateMachine.add(
'Overzetten?',
EqualState(),
transitions={
'true': 'AGV?',
'false': 'op_overzetbin_plaatsen_en_frame_kiezen'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'overzet',
'value_b': 'overzetnee'
})
# x:1179 y:60
OperatableStateMachine.add(
'op_overzetbin_plaatsen_en_frame_kiezen',
self.use_behavior(op_overzetbin_plaatsen_en_frame_kiezenSM,
'op_overzetbin_plaatsen_en_frame_kiezen'),
transitions={
'finished': 'CheckPosePartsinBIn',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'ref_frame': 'ref_frame',
'config_name': 'config_name',
'move_group_prefix': 'move_group_prefix',
'arm_id': 'arm_id',
'agv_id': 'agv_id',
'part_type': 'part_type',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part_offset': 'part_offset',
'overzet': 'overzet'
})
# x:1324 y:446
OperatableStateMachine.add(
'GetAGV1Pose',
GetObjectPoseState(object_frame='kit_tray_1',
ref_frame='arm1_linear_arm_actuator'),
transitions={
'continue': 'R1PreAGV1_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_pose'})
# x:1531 y:492
OperatableStateMachine.add(
'GetAGV1Pose_2',
GetObjectPoseState(object_frame='kit_tray_2',
ref_frame='arm2_linear_arm_actuator'),
transitions={
'continue': 'R1PreAGV1_3',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_id'})
# x:1670 y:414
OperatableStateMachine.add('AGV?',
EqualState(),
transitions={
'true': 'GetAGV1Pose',
'false': 'GetAGV1Pose_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_id',
'value_b': 'agv1_id'
})
# x:207 y:475
OperatableStateMachine.add('R1PreAGV1Back_2',
SrdfStateToMoveitAriac(),
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_namer1b1',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:105 y:672
OperatableStateMachine.add('R1PreAGV1Back_3',
SrdfStateToMoveitAriac(),
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_namer2b4',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:471 y:517
OperatableStateMachine.add('Robot?',
EqualState(),
transitions={
'true': 'R1PreAGV1Back_2',
'false': 'R1PreAGV1Back_3'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_id',
'value_b': 'agv1_id'
})
# x:1419 y:573
OperatableStateMachine.add('R1PreAGV1_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'R1PreAGV1',
'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_namer1b1',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1569 y:574
OperatableStateMachine.add('R1PreAGV1_3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'R1PreAGV1',
'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_namer2b4',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
return _state_machine
def create(self):
# x:27 y:469, x:242 y:360
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
output_keys=['part_type', 'pose_on_agv'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group_g = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link = ''
_state_machine.userdata.pose_on_agv = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.arm_id = ''
_state_machine.userdata.part_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.one = 1
_state_machine.userdata.product_itt = 0
_state_machine.userdata.shipments_index = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:64 y:31
OperatableStateMachine.add(
'Start_assignment',
StartAssignment(),
transitions={'continue': 'move_home_belt_nvd'},
autonomy={'continue': Autonomy.Off})
# x:431 y:28
OperatableStateMachine.add(
'get_orders',
GetOrderState(),
transitions={'continue': 'Get shipments'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:615 y:25
OperatableStateMachine.add('Get shipments',
GetProductsFromShipmentState(),
transitions={
'continue': 'get part',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipments_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:833 y:35
OperatableStateMachine.add('get part',
GetPartFromProductsState(),
transitions={
'continue': 'message part main prg',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'part_index',
'type': 'part_type',
'pose': 'pose_on_agv'
})
# x:1263 y:258
OperatableStateMachine.add('Order_picking',
self.use_behavior(
Order_pickingSM, 'Order_picking'),
transitions={
'finished': 'add_itterator order',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'pose_on_agv': 'pose_on_agv',
'pose_on_agv_r': 'pose_on_agv_r',
'pose_on_agv_l': 'pose_on_agv_l'
})
# x:1148 y:353
OperatableStateMachine.add(
'add_itterator order',
AddNumericState(),
transitions={'done': 'part index message'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'one',
'value_b': 'part_index',
'result': 'part_index'
})
# x:914 y:501
OperatableStateMachine.add('Check_product_amount',
EqualState(),
transitions={
'true': 'move_to_agv',
'false': 'gripper check'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
# x:1053 y:12
OperatableStateMachine.add(
'message part main prg',
MessageState(),
transitions={'continue': 'choice point'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:1154 y:437
OperatableStateMachine.add(
'part index message',
MessageState(),
transitions={'continue': 'Check_product_amount'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_index'})
# x:1277 y:24
OperatableStateMachine.add('choice point',
choiceoflocation(time_out=0.5),
transitions={
'bingr0': 'Order_picking',
'bingr1': 'Order_picking',
'bingr2': 'Order_picking',
'bingr3': 'Order_picking',
'bingr4':
'shelve pick_behavior_nvd',
'bingr5':
'shelve pick_behavior_nvd',
'failed': 'failed'
},
autonomy={
'bingr0': Autonomy.Off,
'bingr1': Autonomy.Off,
'bingr2': Autonomy.Off,
'bingr3': Autonomy.Off,
'bingr4': Autonomy.Off,
'bingr5': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'part_type': 'part_type'})
# x:999 y:213
OperatableStateMachine.add('shelve pick_behavior_nvd',
self.use_behavior(
shelvepick_behavior_nvdSM,
'shelve pick_behavior_nvd'),
transitions={
'finished': 'add_itterator order',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'pose_on_agv': 'pose_on_agv',
'part_type': 'part_type',
'pose_on_agv_l': 'pose_on_agv_l',
'pose_on_agv_r': 'pose_on_agv_r'
})
# x:235 y:9
OperatableStateMachine.add('move_home_belt_nvd',
self.use_behavior(
move_home_belt_nvdSM,
'move_home_belt_nvd'),
transitions={
'finished': 'get_orders',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:896 y:411
OperatableStateMachine.add('gripper check',
GripperActiveCheck(),
transitions={
'Left': 'Get shipments',
'Right': 'Get shipments',
'failed': 'failed',
'Full': 'move_to_agv'
},
autonomy={
'Left': Autonomy.Off,
'Right': Autonomy.Off,
'failed': Autonomy.Off,
'Full': Autonomy.Off
},
remapping={
'arm_id': 'arm_id',
'tool_link': 'tool_link',
'move_group': 'move_group'
})
# x:635 y:505
OperatableStateMachine.add('move_to_agv',
self.use_behavior(
move_to_agvSM, 'move_to_agv'),
transitions={
'finished':
'Check_product_amount_2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'agv_id': 'agv_id',
'pose_on_agv_l': 'pose_on_agv_l',
'pose_on_agv_r': 'pose_on_agv_r'
})
# x:438 y:593
OperatableStateMachine.add('Check_product_amount_2',
EqualState(),
transitions={
'true': 'finished',
'false': 'Get shipments'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
return _state_machine
def create(self):
# x:27 y:469, x:242 y:360
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group_g = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.part_type = 'gear_part_blue'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.arm_id = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:145 y:28
OperatableStateMachine.add('move_home_pick',
self.use_behavior(
move_home_pickSM, 'move_home_pick'),
transitions={
'finished': 'Find_product_location',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:701 y:151
OperatableStateMachine.add('move_gantry_bin_gr1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'find part',
'planning_failed': 'Waitretry',
'control_failed': 'Waitretry',
'param_error': 'Waitretry'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'bin',
'move_group': 'move_group_g',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:479 y:25
OperatableStateMachine.add('Find_product_location',
FindCorrectBin(time_out=0.5),
transitions={
'continue': 'partype mesage',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'bin': 'bin',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'ref_frame': 'ref_frame'
})
# x:694 y:497
OperatableStateMachine.add(
'compute_pick _r',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:713 y:240
OperatableStateMachine.add(
'find part',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Offset bepalen',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:760 y:314
OperatableStateMachine.add('Offset bepalen',
DecideOffsetProduct(target_time=0.5),
transitions={
'succes': 'check_free_gripper',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'part_offset': 'part_offset'
})
# x:433 y:563
OperatableStateMachine.add('Gripper_enable',
GripperControl(enable=True),
transitions={
'continue': 'move_home_robot_r',
'failed': 'Move to pick',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:234 y:591
OperatableStateMachine.add('move_home_robot_r',
self.use_behavior(
move_home_robot_rSM,
'move_home_robot_r'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:619 y:590
OperatableStateMachine.add('Move to pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_enable',
'planning_failed': 'failed',
'control_failed': 'Gripper_enable'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:812 y:393
OperatableStateMachine.add('check_free_gripper',
GripperActiveCheck(),
transitions={
'Left': 'compute_pick _l',
'Right': 'compute_pick _r',
'failed': 'failed',
'Full': 'move_home_pick'
},
autonomy={
'Left': Autonomy.Off,
'Right': Autonomy.Off,
'failed': Autonomy.Off,
'Full': Autonomy.Off
},
remapping={
'arm_id': 'arm_id',
'tool_link': 'tool_link',
'move_group': 'move_group'
})
# x:942 y:468
OperatableStateMachine.add(
'compute_pick _l',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:627 y:57
OperatableStateMachine.add(
'partype mesage',
MessageState(),
transitions={'continue': 'move_gantry_bin_gr1'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:810 y:40
OperatableStateMachine.add(
'Waitretry',
WaitState(wait_time=2),
transitions={'done': 'move_gantry_bin_gr1'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:867 y:12, x:483 y:355
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.products = []
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.increment = 1
_state_machine.userdata.Null = 0
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.old_order_id = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:49 y:117
OperatableStateMachine.add('startassignment',
StartAssignment(),
transitions={'continue': 'get_order'},
autonomy={'continue': Autonomy.Off})
# x:207 y:119
OperatableStateMachine.add('get_order',
GetOrderState(),
transitions={'continue': 'TestLastOrder'},
autonomy={'continue': Autonomy.Off},
remapping={'order_id': 'order_id', 'shipments': 'shipments', 'number_of_shipments': 'number_of_shipments'})
# x:410 y:121
OperatableStateMachine.add('getProductsfromshipment',
GetProductsFromShipmentState(),
transitions={'continue': 'ShipmentID', 'invalid_index': 'failed'},
autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'shipments': 'shipments', 'index': 'shipment_index', 'shipment_type': 'shipment_type', 'agv_id': 'agv_id', 'products': 'products', 'number_of_products': 'number_of_products'})
# x:848 y:122
OperatableStateMachine.add('transport_part_form_bin_to_agv_state',
self.use_behavior(transport_part_form_bin_to_agv_stateSM, 'transport_part_form_bin_to_agv_state'),
transitions={'finished': 'IncreaseProductIndex', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'part_type': 'part_type', 'agv_id': 'agv_id', 'part_pose': 'part_pose', 'pose': 'pose'})
# x:649 y:120
OperatableStateMachine.add('GetPartinfo',
GetPartFromProductsState(),
transitions={'continue': 'transport_part_form_bin_to_agv_state', 'invalid_index': 'failed'},
autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'products': 'products', 'index': 'product_index', 'type': 'part_type', 'pose': 'part_pose'})
# x:928 y:241
OperatableStateMachine.add('IncreaseProductIndex',
AddNumericState(),
transitions={'done': 'CheckProduct_index'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'increment', 'result': 'product_index'})
# x:934 y:323
OperatableStateMachine.add('CheckProduct_index',
EqualState(),
transitions={'true': 'SETnull', 'false': 'GetPartinfo'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'number_of_products'})
# x:770 y:514
OperatableStateMachine.add('IncreaseShipmentIndex',
AddNumericState(),
transitions={'done': 'Checkshipmentindex'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'shipment_index', 'value_b': 'increment', 'result': 'shipment_index'})
# x:396 y:499
OperatableStateMachine.add('Checkshipmentindex',
EqualState(),
transitions={'true': 'get_order', 'false': 'notify_shipment_ready'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'number_of_shipments', 'value_b': 'shipment_index'})
# x:990 y:530
OperatableStateMachine.add('SETnull',
ReplaceState(),
transitions={'done': 'IncreaseShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Null', 'result': 'product_index'})
# x:721 y:355
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(notify_shipment_readySM, 'notify_shipment_ready'),
transitions={'finished': 'getProductsfromshipment', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:388 y:13
OperatableStateMachine.add('TestLastOrder',
EqualState(),
transitions={'true': 'finished', 'false': 'RememberOldOrder'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'order_id', 'value_b': 'old_order_id'})
# x:632 y:44
OperatableStateMachine.add('RememberOldOrder',
ReplaceState(),
transitions={'done': 'getProductsfromshipment'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'order_id', 'result': 'old_order_id'})
# x:40 y:265
OperatableStateMachine.add('ShipmentID',
MessageState(),
transitions={'continue': 'GetPartinfo'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'shipment_type'})
return _state_machine
def create(self):
# x:1132 y:54, x:687 y:211
_state_machine = OperatableStateMachine(outcomes=['finished', 'fail'])
_state_machine.userdata.StartText = 'Opdracht gestart'
_state_machine.userdata.StopText = 'Opdracht gestopt'
_state_machine.userdata.Shipments = []
_state_machine.userdata.part = 'gear_part'
_state_machine.userdata.material_locations = []
_state_machine.userdata.NumberOfShipments = 0
_state_machine.userdata.OrderId = ''
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'StartMessage'},
autonomy={'continue': Autonomy.Off})
# x:803 y:41
OperatableStateMachine.add('EndAssigment',
EndAssignment(),
transitions={'continue': 'StopMessage'},
autonomy={'continue': Autonomy.Off})
# x:158 y:41
OperatableStateMachine.add('StartMessage',
MessageState(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'StartText'})
# x:963 y:44
OperatableStateMachine.add('StopMessage',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'StopText'})
# x:286 y:41
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'OrderIdMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'order_id': 'OrderId', 'shipments': 'Shipments', 'number_of_shipments': 'NumberOfShipments'})
# x:466 y:41
OperatableStateMachine.add('OrderIdMessage',
MessageState(),
transitions={'continue': 'get_shipments'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'OrderId'})
# x:614 y:40
OperatableStateMachine.add('get_shipments',
self.use_behavior(get_shipmentsSM, 'get_shipments'),
transitions={'finished': 'EndAssigment', 'fail': 'fail'},
autonomy={'finished': Autonomy.Inherit, 'fail': Autonomy.Inherit},
remapping={'Shipments': 'Shipments', 'NumberOfShipments': 'NumberOfShipments'})
return _state_machine
def create(self):
# x:346 y:592, x:899 y:56
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['offsetx', 'offsety'], output_keys=['offsetx', 'offsety'])
_state_machine.userdata.offsetx = 0
_state_machine.userdata.offsety = 0
_state_machine.userdata.opschuifx = 0.2
_state_machine.userdata.opschuify = 0.2
_state_machine.userdata.een = 1
_state_machine.userdata.nul = 0
_state_machine.userdata.tellerx = 0
_state_machine.userdata.tellery = 0
_state_machine.userdata.twee = 2
_state_machine.userdata.message = 'BinisVol!'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:121 y:40
OperatableStateMachine.add('CheckY',
EqualState(),
transitions={'true': 'ResetY', 'false': 'AddY'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'tellery', 'value_b': 'twee'})
# x:103 y:280
OperatableStateMachine.add('AddTellerY',
AddNumericState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'tellery', 'value_b': 'een', 'result': 'tellery'})
# x:339 y:34
OperatableStateMachine.add('ResetY',
ReplaceState(),
transitions={'done': 'CheckX'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'tellery', 'result': 'nul'})
# x:369 y:276
OperatableStateMachine.add('AddX',
AddNumericState(),
transitions={'done': 'AddTellerX'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'offsetx', 'value_b': 'opschuifx', 'result': 'offsetx'})
# x:383 y:409
OperatableStateMachine.add('AddTellerX',
AddNumericState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'tellerx', 'value_b': 'een', 'result': 'tellerx'})
# x:340 y:138
OperatableStateMachine.add('CheckX',
EqualState(),
transitions={'true': 'ResetX', 'false': 'AddX'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'tellerx', 'value_b': 'twee'})
# x:576 y:131
OperatableStateMachine.add('ResetX',
ReplaceState(),
transitions={'done': 'ResetX'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'tellerx', 'result': 'nul'})
# x:776 y:130
OperatableStateMachine.add('BinisVol',
MessageState(),
transitions={'continue': 'failed'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'message'})
# x:114 y:170
OperatableStateMachine.add('AddY',
AddNumericState(),
transitions={'done': 'AddTellerY'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'offsety', 'value_b': 'opschuify', 'result': 'offsety'})
return _state_machine
def create(self):
# x:1233 y:271, x:524 y:568
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['agv_id', 'part_type', 'part_pose', 'bin_id'])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = ''
_state_machine.userdata.bin_id = ''
_state_machine.userdata.bin_gear_red = 'bin14'
_state_machine.userdata.bin_pully_red = 'bin3'
_state_machine.userdata.shelf_gasket_red = 'shelf6'
_state_machine.userdata.shelf_gear_blue = 'shelf3'
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.camera_frame_gear_red = 'logical_camera_BinGroup1_frame'
_state_machine.userdata.camera_frame_pully_red = 'logical_camera_BinGroup3_frame'
_state_machine.userdata.camera_frame_gear_blue = 'logical_camera_Shelf2_frame'
_state_machine.userdata.camera_frame_gasket_red = 'logical_camera_Shelf1_frame'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_topic_gear_red = '/ariac/logical_camera_BinGroup1'
_state_machine.userdata.camera_topic_pully_red = '/ariac/logical_camera_BinGroup3'
_state_machine.userdata.camera_topic_gear_blue = '/ariac/logical_camera_Shelf2'
_state_machine.userdata.camera_topic_gasket_red = '/ariac/logical_camera_Shelf1'
_state_machine.userdata.PreGraspGantry = ''
_state_machine.userdata.PreGraspGantry_GearRed = 'Gantry_Bin3_4_inverse'
_state_machine.userdata.PreGraspGantry_PullyRed = 'Gantry_Bin11_12_inverse'
_state_machine.userdata.offset = 0
_state_machine.userdata.offsetPully = 0.085
_state_machine.userdata.offsetGear = 0.025
_state_machine.userdata.offsetGasket = 0.035
_state_machine.userdata.PreGraspGantry_GasketRed = 'Gantry_ShelfLeftRed'
_state_machine.userdata.PreGraspGantry_GearBlue = 'Gantry_ShelfLeftBlue'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
# x:188 y:36
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'PoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:346 y:36
OperatableStateMachine.add(
'PoseMessage',
MessageState(),
transitions={'continue': 'BinIdMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_pose'})
# x:492 y:38
OperatableStateMachine.add(
'BinIdMessage',
MessageState(),
transitions={'continue': 'Selector_BinGearRed'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'bin_id'})
# x:24 y:124
OperatableStateMachine.add('Selector_BinGearRed',
EqualState(),
transitions={
'true': 'CameraFrameGearRed',
'false': 'Selector_BinPullyRed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'bin_gear_red'
})
# x:24 y:239
OperatableStateMachine.add('Selector_BinPullyRed',
EqualState(),
transitions={
'true': 'CameraFramePullyRed',
'false': 'Selector_ShelfGasketRed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'bin_pully_red'
})
# x:24 y:345
OperatableStateMachine.add('Selector_ShelfGasketRed',
EqualState(),
transitions={
'true': 'CameraFrameGasketRed',
'false': 'Selector_ShelfGearBlue'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'shelf_gasket_red'
})
# x:23 y:448
OperatableStateMachine.add('Selector_ShelfGearBlue',
EqualState(),
transitions={
'true': 'CameraFrameGearBlue',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'shelf_gear_blue'
})
# x:231 y:124
OperatableStateMachine.add(
'CameraFrameGearRed',
ReplaceState(),
transitions={'done': 'CameraTopicGearRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_gear_red',
'result': 'camera_frame'
})
# x:230 y:228
OperatableStateMachine.add(
'CameraFramePullyRed',
ReplaceState(),
transitions={'done': 'CameraTopicPullyRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_pully_red',
'result': 'camera_frame'
})
# x:230 y:331
OperatableStateMachine.add(
'CameraFrameGasketRed',
ReplaceState(),
transitions={'done': 'CameraTopicGasketRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_gasket_red',
'result': 'camera_frame'
})
# x:232 y:431
OperatableStateMachine.add(
'CameraFrameGearBlue',
ReplaceState(),
transitions={'done': 'CameraTopicGearBlue'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_gear_blue',
'result': 'camera_frame'
})
# x:601 y:128
OperatableStateMachine.add('PreGraspLeftGantry',
ReplaceState(),
transitions={'done': 'OffsetGearRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGraspGantry_GearRed',
'result': 'PreGraspGantry'
})
# x:598 y:228
OperatableStateMachine.add('PreGraspLeftGantry_2',
ReplaceState(),
transitions={'done': 'OffsetPullyRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGraspGantry_PullyRed',
'result': 'PreGraspGantry'
})
# x:413 y:125
OperatableStateMachine.add(
'CameraTopicGearRed',
ReplaceState(),
transitions={'done': 'PreGraspLeftGantry'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_gear_red',
'result': 'camera_topic'
})
# x:413 y:226
OperatableStateMachine.add(
'CameraTopicPullyRed',
ReplaceState(),
transitions={'done': 'PreGraspLeftGantry_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_pully_red',
'result': 'camera_topic'
})
# x:420 y:336
OperatableStateMachine.add(
'CameraTopicGasketRed',
ReplaceState(),
transitions={'done': 'PreGraspLeftGantry_3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_gasket_red',
'result': 'camera_topic'
})
# x:422 y:434
OperatableStateMachine.add(
'CameraTopicGearBlue',
ReplaceState(),
transitions={'done': 'PreGraspLeftGantry_4'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_gear_blue',
'result': 'camera_topic'
})
# x:795 y:128
OperatableStateMachine.add('OffsetGearRed',
ReplaceState(),
transitions={'done': 'Path_Left_Bins'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetGear',
'result': 'offset'
})
# x:788 y:228
OperatableStateMachine.add('OffsetPullyRed',
ReplaceState(),
transitions={'done': 'Path_Left_Bins'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetPully',
'result': 'offset'
})
# x:805 y:333
OperatableStateMachine.add(
'OffsetGasketRed',
ReplaceState(),
transitions={'done': 'Path_Shelves_Left'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetGasket',
'result': 'offset'
})
# x:808 y:436
OperatableStateMachine.add(
'OffsetGearBlue',
ReplaceState(),
transitions={'done': 'Path_Shelves_Left'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetGear',
'result': 'offset'
})
# x:1001 y:175
OperatableStateMachine.add('Path_Left_Bins',
self.use_behavior(
Path_Left_BinsSM, 'Path_Left_Bins'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part_type': 'part_type',
'PreGraspGantry': 'PreGraspGantry',
'offset': 'offset'
})
# x:1000 y:355
OperatableStateMachine.add('Path_Shelves_Left',
self.use_behavior(
Path_Shelves_LeftSM,
'Path_Shelves_Left'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part_type': 'part_type',
'PreGraspGantry': 'PreGraspGantry',
'offset': 'offset'
})
# x:606 y:329
OperatableStateMachine.add('PreGraspLeftGantry_3',
ReplaceState(),
transitions={'done': 'OffsetGasketRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGraspGantry_GasketRed',
'result': 'PreGraspGantry'
})
# x:604 y:424
OperatableStateMachine.add('PreGraspLeftGantry_4',
ReplaceState(),
transitions={'done': 'OffsetGearBlue'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGraspGantry_GearBlue',
'result': 'PreGraspGantry'
})
return _state_machine
def create(self):
# x:412 y:439, x:647 y:299
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type', 'pose_on_agv'],
output_keys=['pose_on_agv_r', 'pose_on_agv_l'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group_g = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.arm_id = ''
_state_machine.userdata.config_name_r = 'Right_Home_B'
_state_machine.userdata.config_name_l = 'Left_Home_B'
_state_machine.userdata.part_type = ''
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.pose_on_agv_l = []
_state_machine.userdata.pose_on_agv_r = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:20 y:123
OperatableStateMachine.add(
'part in pick prg',
MessageState(),
transitions={'continue': 'Find_product_location'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:338 y:86
OperatableStateMachine.add('move_gantry_bin_gr1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'find part',
'planning_failed': 'Waitretry',
'control_failed': 'Waitretry',
'param_error': 'Waitretry'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'bin',
'move_group': 'move_group_g',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:945 y:160
OperatableStateMachine.add(
'compute_pick _r',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick_r',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:544 y:68
OperatableStateMachine.add(
'find part',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Offset bepalen',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:760 y:17
OperatableStateMachine.add('Offset bepalen',
DecideOffsetProduct(target_time=0.5),
transitions={
'succes': 'check_free_gripper',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'part_offset': 'part_offset'
})
# x:959 y:326
OperatableStateMachine.add('Gripper_enable_r',
GripperControl(enable=True),
transitions={
'continue': 'Move_pre_grasp',
'failed': 'Move to pick_r',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:949 y:244
OperatableStateMachine.add('Move to pick_r',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_enable_r',
'planning_failed': 'failed',
'control_failed': 'Gripper_enable_r'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:921 y:17
OperatableStateMachine.add('check_free_gripper',
GripperActiveCheck(),
transitions={
'Left': 'compute_pick _l',
'Right': 'compute_pick _r',
'failed': 'failed',
'Full': 'finished'
},
autonomy={
'Left': Autonomy.Off,
'Right': Autonomy.Off,
'failed': Autonomy.Off,
'Full': Autonomy.Off
},
remapping={
'arm_id': 'arm_id',
'tool_link': 'tool_link',
'move_group': 'move_group'
})
# x:1292 y:51
OperatableStateMachine.add(
'compute_pick _l',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick_l',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:873 y:409
OperatableStateMachine.add('Move_pre_grasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Agv pose R',
'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_r',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1273 y:273
OperatableStateMachine.add('Gripper_enable_l',
GripperControl(enable=True),
transitions={
'continue': 'Move_pre_grasp_2',
'failed': 'Move to pick_l',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1239 y:164
OperatableStateMachine.add('Move to pick_l',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_enable_l',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1269 y:384
OperatableStateMachine.add('Move_pre_grasp_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Agv pose L',
'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_l',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:436 y:3
OperatableStateMachine.add(
'Waitretry',
WaitState(wait_time=2),
transitions={'done': 'move_gantry_bin_gr1'},
autonomy={'done': Autonomy.Off})
# x:152 y:81
OperatableStateMachine.add('Find_product_location',
FindCorrectBin(time_out=0.5),
transitions={
'continue': 'move_gantry_bin_gr1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'bin': 'bin',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'ref_frame': 'ref_frame'
})
# x:769 y:508
OperatableStateMachine.add('Agv pose R',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv_r'
})
# x:1257 y:505
OperatableStateMachine.add('Agv pose L',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv_l'
})
return _state_machine
def create(self):
# x:52 y:611, x:343 y:359
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.agv_id = 'agv1'
_state_machine.userdata.part_type = 'gear_part'
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.pose = []
_state_machine.userdata.part_pose = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.part = 'gasket_part'
_state_machine.userdata.offset = 0.1
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.config_name_bin3PreGrasp = 'bin3PreGrasp'
_state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop'
_state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_5'
_state_machine.userdata.camera_frame = 'logical_camera_5_frame'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.agv_pose = []
_state_machine.userdata.part_offset = 0.035
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.conveyor_belt_power = 100.0
_state_machine.userdata.config_name_bin1PreGrasp = 'bin1PreGrasp'
_state_machine.userdata.arm_id = "arm1"
_state_machine.userdata.part_drop_offset = 0.1
_state_machine.userdata.StartText = 'Opdracht gestart'
_state_machine.userdata.StopText = 'Opdracht gestopt'
_state_machine.userdata.Shipments = []
_state_machine.userdata.part = 'gear_part'
_state_machine.userdata.material_locations = []
_state_machine.userdata.NumberOfShipments = 0
_state_machine.userdata.OrderId = ''
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.MaterialsLocationList = []
_state_machine.userdata.config_name_bin3PreDrop = 'bin3PreDrop'
_state_machine.userdata.poseBin3 = []
_state_machine.userdata.move_group_prefix2 = '/ariac/arm2'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
# x:305 y:37
OperatableStateMachine.add('MoseMessage',
MessageState(),
transitions={'continue': 'Wait'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'pose_on_agv'})
# x:171 y:37
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'MoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:499 y:40
OperatableStateMachine.add('Wait',
WaitState(wait_time=1),
transitions={'done': 'MoveR1PreGrasp2'},
autonomy={'done': Autonomy.Off})
# x:647 y:43
OperatableStateMachine.add('MoveR1PreGrasp2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveR1PreDrop',
'planning_failed': 'WaitRetry4',
'control_failed': 'WaitRetry4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:663 y:121
OperatableStateMachine.add('WaitRetry4',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreGrasp2'},
autonomy={'done': Autonomy.Off})
# x:861 y:116
OperatableStateMachine.add('WaitRerty5',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreDrop'},
autonomy={'done': Autonomy.Off})
# x:1148 y:38
OperatableStateMachine.add(
'ComputeDrop',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'MoveR1ToDrop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'poseBin3',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1013 y:153
OperatableStateMachine.add('WaitRetry6',
WaitState(wait_time=5),
transitions={'done': 'MoveR1ToDrop'},
autonomy={'done': Autonomy.Off})
# x:821 y:39
OperatableStateMachine.add('MoveR1PreDrop',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetBin3Pose',
'planning_failed': 'WaitRerty5',
'control_failed': 'WaitRerty5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin3PreDrop',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1140 y:139
OperatableStateMachine.add('MoveR1ToDrop',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'DisableGripper',
'planning_failed': 'WaitRetry6',
'control_failed': 'DisableGripper'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:993 y:40
OperatableStateMachine.add(
'GetBin3Pose',
GetObjectPoseState(object_frame='link::base',
ref_frame='arm1_linear_arm_actuator'),
transitions={
'continue': 'ComputeDrop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'poseBin3'})
# x:1154 y:350
OperatableStateMachine.add('MoveR1Home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveR1Home_2',
'planning_failed': 'WaitRetry1',
'control_failed': 'WaitRetry1',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:992 y:355
OperatableStateMachine.add('WaitRetry1',
WaitState(wait_time=5),
transitions={'done': 'MoveR1Home'},
autonomy={'done': Autonomy.Off})
# x:1163 y:445
OperatableStateMachine.add('MoveR1Home_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveR1PreDrop_2',
'planning_failed': 'WaitRetry1_2',
'control_failed': 'WaitRetry1_2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1002 y:448
OperatableStateMachine.add('WaitRetry1_2',
WaitState(wait_time=5),
transitions={'done': 'MoveR1Home_2'},
autonomy={'done': Autonomy.Off})
# x:1013 y:521
OperatableStateMachine.add('WaitRerty5_2',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreDrop_2'},
autonomy={'done': Autonomy.Off})
# x:1163 y:531
OperatableStateMachine.add('MoveR1PreDrop_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'WaitRerty5_2',
'control_failed': 'WaitRerty5_2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin3PreDrop',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1141 y:228
OperatableStateMachine.add('DisableGripper',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'MoveR1Home',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
return _state_machine
def create(self):
# x:291 y:423, x:814 y:225
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['Shipments', 'NumberOfShipments'])
_state_machine.userdata.Shipments = []
_state_machine.userdata.NumberOfShipments = 0
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.AgvId = ''
_state_machine.userdata.ShipmentIndex = 0
_state_machine.userdata.ShipmentType = ''
_state_machine.userdata.ShipmentIterator = 0
_state_machine.userdata.OneValue = 1
_state_machine.userdata.agv_id = ''
_state_machine.userdata.agv_ready_state = "ready_to_deliver"
_state_machine.userdata.agv_state = ''
_state_machine.userdata.Zero = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:55 y:39
OperatableStateMachine.add('GetProducts',
GetProductsFromShipmentState(),
transitions={
'continue': 'number of products',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments': 'Shipments',
'index': 'ShipmentIterator',
'shipment_type': 'ShipmentType',
'agv_id': 'agv_id',
'products': 'Products',
'number_of_products':
'NumberOfProducts'
})
# x:600 y:38
OperatableStateMachine.add('Get_Products',
self.use_behavior(
Get_ProductsSM, 'Get_Products'),
transitions={
'finished': 'NotifyShipmentReady',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'Products': 'Products',
'NumberOfProducts':
'NumberOfProducts',
'agv_id': 'agv_id'
})
# x:870 y:36
OperatableStateMachine.add('NotifyShipmentReady',
NotifyShipmentReadyState(),
transitions={
'continue': 'agv_busy',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'agv_id': 'agv_id',
'shipment_type': 'ShipmentType',
'success': 'success',
'message': 'message'
})
# x:1131 y:38
OperatableStateMachine.add('agv_busy',
WaitState(wait_time=8),
transitions={'done': 'Shipment nr.'},
autonomy={'done': Autonomy.Off})
# x:1145 y:384
OperatableStateMachine.add('ShipmentIterator',
AddNumericState(),
transitions={'done': 'Shipment nr._2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'ShipmentIterator',
'value_b': 'OneValue',
'result': 'ShipmentIterator'
})
# x:680 y:378
OperatableStateMachine.add('CompareIterator',
EqualState(),
transitions={
'true': 'ResetIterator',
'false': 'GetProducts'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'NumberOfShipments',
'value_b': 'ShipmentIterator'
})
# x:399 y:24
OperatableStateMachine.add(
'number of products',
MessageState(),
transitions={'continue': 'Get_Products'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'NumberOfProducts'})
# x:1171 y:283
OperatableStateMachine.add(
'Shipment nr.',
MessageState(),
transitions={'continue': 'ShipmentIterator'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ShipmentIterator'})
# x:915 y:383
OperatableStateMachine.add(
'Shipment nr._2',
MessageState(),
transitions={'continue': 'CompareIterator'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ShipmentIterator'})
# x:1346 y:166
OperatableStateMachine.add('AGVstatus',
EqualState(),
transitions={
'true': 'Shipment nr.',
'false': 'agv_busy'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_state',
'value_b': 'agv_ready_state'
})
# x:1378 y:49
OperatableStateMachine.add('getAGVstatus',
GetAgvStatusState(),
transitions={
'continue': 'AGVstatus',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'agv_id': 'agv_id',
'agv_state': 'agv_state'
})
# x:436 y:410
OperatableStateMachine.add('ResetIterator',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'ShipmentIterator'
})
return _state_machine
def create(self):
# x:1313 y:295, x:1054 y:659
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['agv_id', 'part_type', 'part_pose'],
output_keys=[
'move_group_prefix', 'config_name', 'arm_id', 'ref_frame',
'camera_topic', 'camera_frame', 'part', 'part_offset',
'overzet', 'config_name_preagv'
])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.move_group_prefix = ''
_state_machine.userdata.config_name = ''
_state_machine.userdata.arm_id = ''
_state_machine.userdata.ref_frame = ''
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.part = ''
_state_machine.userdata.part_offset = 0
_state_machine.userdata.move_group_prefix1 = '/ariac/arm1'
_state_machine.userdata.move_group_prefix2 = '/ariac/arm2'
_state_machine.userdata.config_name_r1b2 = 'R1PreBin2'
_state_machine.userdata.config_name_r1b3 = 'R1PreBin3'
_state_machine.userdata.config_name_r1b4 = 'R1PreBin4'
_state_machine.userdata.config_name_r2b3 = 'R2PreBin3'
_state_machine.userdata.config_name_r2b4 = 'R2PreBin4'
_state_machine.userdata.config_name_r2b6 = 'R2PreBin6'
_state_machine.userdata.config_name_r2b5 = 'R2PreBin5'
_state_machine.userdata.config_name_r1b1 = 'R1PreBin1'
_state_machine.userdata.arm1_id = 'arm1'
_state_machine.userdata.arm2_id = 'arm2'
_state_machine.userdata.ref_frame1 = 'arm1_linear_arm_actuator'
_state_machine.userdata.ref_frame2 = 'arm2_linear_arm_actuator'
_state_machine.userdata.camera_topic2 = '/ariac/Camera_Bin_2'
_state_machine.userdata.camera_topic3 = '/ariac/Camera_Bin_3'
_state_machine.userdata.camera_topic4 = '/ariac/Camera_Bin_4'
_state_machine.userdata.camera_topic5 = '/ariac/Camera_Bin_5'
_state_machine.userdata.camera_topic6 = '/ariac/Camera_Bin_6'
_state_machine.userdata.camera_topic1 = '/ariac/Camera_Bin_1'
_state_machine.userdata.camera_frame2 = 'Camera_Bin_2_frame'
_state_machine.userdata.camera_frame3 = 'Camera_Bin_3_frame'
_state_machine.userdata.camera_frame4 = 'Camera_Bin_4_frame'
_state_machine.userdata.camera_frame5 = 'Camera_Bin_5_frame'
_state_machine.userdata.camera_frame6 = 'Camera_Bin_6_frame'
_state_machine.userdata.camera_frame1 = 'Camera_Bin_1_frame'
_state_machine.userdata.part1 = 'gasket_part'
_state_machine.userdata.part2 = 'pulley_part'
_state_machine.userdata.part3 = 'piston_rod_part'
_state_machine.userdata.part4 = 'gear_part'
_state_machine.userdata.part_offset2 = 0.085
_state_machine.userdata.part_offset3 = 0.02
_state_machine.userdata.part_offset4 = 0.025
_state_machine.userdata.part_offset1 = 0.038
_state_machine.userdata.agv1_id = 'agv1'
_state_machine.userdata.agv2_id = 'agv2'
_state_machine.userdata.overzetja = 'ja'
_state_machine.userdata.overzetnee = 'nee'
_state_machine.userdata.overzet = ''
_state_machine.userdata.config_name_preagv = ''
_state_machine.userdata.config_name_agv1 = 'R1PreAGV1'
_state_machine.userdata.config_name_agv2 = 'R2PreAGV2'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:28 y:28
OperatableStateMachine.add('AGV1?',
EqualState(),
transitions={
'true': 'Agv1pregrasp',
'false': 'Agv2pregrasp'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_id',
'value_b': 'agv1_id'
})
# x:191 y:345
OperatableStateMachine.add('Rod?',
EqualState(),
transitions={
'true': 'Overzetten1',
'false': 'Overzetten1_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type',
'value_b': 'part3'
})
# x:235 y:234
OperatableStateMachine.add('Pulley?',
EqualState(),
transitions={
'true': 'Overzettennee2',
'false': 'Rod?'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type',
'value_b': 'part2'
})
# x:604 y:60
OperatableStateMachine.add('r1b1',
ReplaceState(),
transitions={'done': 'Offset1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_r1b1',
'result': 'config_name'
})
# x:606 y:141
OperatableStateMachine.add('r1b2',
ReplaceState(),
transitions={'done': 'Offset2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_r1b2',
'result': 'config_name'
})
# x:646 y:444
OperatableStateMachine.add('r2b5',
ReplaceState(),
transitions={'done': 'Offset3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_r2b5',
'result': 'config_name'
})
# x:632 y:536
OperatableStateMachine.add('r2b6',
ReplaceState(),
transitions={'done': 'Offset4'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_r2b6',
'result': 'config_name'
})
# x:796 y:50
OperatableStateMachine.add('Offset1',
ReplaceState(),
transitions={'done': 'CameraTopic1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'part_offset1',
'result': 'part_offset'
})
# x:797 y:149
OperatableStateMachine.add('Offset2',
ReplaceState(),
transitions={'done': 'CameraTopic2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'part_offset2',
'result': 'part_offset'
})
# x:838 y:432
OperatableStateMachine.add('Offset3',
ReplaceState(),
transitions={'done': 'CameraTopic5'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'part_offset3',
'result': 'part_offset'
})
# x:832 y:525
OperatableStateMachine.add('Offset4',
ReplaceState(),
transitions={'done': 'CameraTopic6'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'part_offset4',
'result': 'part_offset'
})
# x:158 y:506
OperatableStateMachine.add('Gasket2?',
EqualState(),
transitions={
'true': 'Overzetten1_3',
'false': 'Pulley2?'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type',
'value_b': 'part1'
})
# x:161 y:698
OperatableStateMachine.add('Rod2?',
EqualState(),
transitions={
'true': 'Overzettennee3',
'false': 'Overzettennee4'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type',
'value_b': 'part3'
})
# x:158 y:597
OperatableStateMachine.add('Pulley2?',
EqualState(),
transitions={
'true': 'Overzetten1_4',
'false': 'Rod2?'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type',
'value_b': 'part2'
})
# x:407 y:258
OperatableStateMachine.add('Overzetten1',
ReplaceState(),
transitions={'done': 'r2b5'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'overzetja',
'result': 'overzet'
})
# x:401 y:335
OperatableStateMachine.add('Overzetten1_2',
ReplaceState(),
transitions={'done': 'r2b6'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'overzetja',
'result': 'overzet'
})
# x:383 y:412
OperatableStateMachine.add('Overzetten1_3',
ReplaceState(),
transitions={'done': 'r1b1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'overzetja',
'result': 'overzet'
})
# x:374 y:496
OperatableStateMachine.add('Overzetten1_4',
ReplaceState(),
transitions={'done': 'r1b2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'overzetja',
'result': 'overzet'
})
# x:1046 y:61
OperatableStateMachine.add('CameraTopic1',
ReplaceState(),
transitions={'done': 'CameraFrame1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic1',
'result': 'camera_topic'
})
# x:1046 y:138
OperatableStateMachine.add('CameraTopic2',
ReplaceState(),
transitions={'done': 'CameraFrame_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic2',
'result': 'camera_topic'
})
# x:1032 y:429
OperatableStateMachine.add('CameraTopic5',
ReplaceState(),
transitions={'done': 'CameraFrame_5'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic5',
'result': 'camera_topic'
})
# x:1037 y:525
OperatableStateMachine.add('CameraTopic6',
ReplaceState(),
transitions={'done': 'CameraFrame_6'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic6',
'result': 'camera_topic'
})
# x:1229 y:66
OperatableStateMachine.add('CameraFrame1',
ReplaceState(),
transitions={'done': 'Refframe'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame1',
'result': 'camera_frame'
})
# x:1229 y:143
OperatableStateMachine.add('CameraFrame_2',
ReplaceState(),
transitions={'done': 'Refframe'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame2',
'result': 'camera_frame'
})
# x:1254 y:424
OperatableStateMachine.add('CameraFrame_5',
ReplaceState(),
transitions={'done': 'Refframe_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame5',
'result': 'camera_frame'
})
# x:1258 y:520
OperatableStateMachine.add('CameraFrame_6',
ReplaceState(),
transitions={'done': 'Refframe_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame6',
'result': 'camera_frame'
})
# x:1399 y:121
OperatableStateMachine.add(
'Refframe',
ReplaceState(),
transitions={'done': 'movegroupprefix1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'ref_frame1',
'result': 'ref_frame'
})
# x:1564 y:120
OperatableStateMachine.add('movegroupprefix1',
ReplaceState(),
transitions={'done': 'armid'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'move_group_prefix1',
'result': 'move_group_prefix'
})
# x:1667 y:219
OperatableStateMachine.add('armid',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm1_id',
'result': 'arm_id'
})
# x:1432 y:451
OperatableStateMachine.add(
'Refframe_2',
ReplaceState(),
transitions={'done': 'movegroupprefix1_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'ref_frame2',
'result': 'ref_frame'
})
# x:1599 y:452
OperatableStateMachine.add('movegroupprefix1_2',
ReplaceState(),
transitions={'done': 'armid_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'move_group_prefix2',
'result': 'move_group_prefix'
})
# x:1667 y:296
OperatableStateMachine.add('armid_2',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm2_id',
'result': 'arm_id'
})
# x:221 y:124
OperatableStateMachine.add('Gasket?',
EqualState(),
transitions={
'true': 'Overzettennee1',
'false': 'Pulley?'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type',
'value_b': 'part1'
})
# x:403 y:71
OperatableStateMachine.add('Overzettennee1',
ReplaceState(),
transitions={'done': 'oz'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'overzetnee',
'result': 'overzet'
})
# x:386 y:167
OperatableStateMachine.add('Overzettennee2',
ReplaceState(),
transitions={'done': 'r1b2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'overzetnee',
'result': 'overzet'
})
# x:409 y:588
OperatableStateMachine.add('Overzettennee3',
ReplaceState(),
transitions={'done': 'r2b5'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'overzetnee',
'result': 'overzet'
})
# x:460 y:705
OperatableStateMachine.add('Overzettennee4',
ReplaceState(),
transitions={'done': 'r2b6'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'overzetnee',
'result': 'overzet'
})
# x:465 y:0
OperatableStateMachine.add('oz',
MessageState(),
transitions={'continue': 'r1b1'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'overzet'})
# x:30 y:90
OperatableStateMachine.add('Agv1pregrasp',
ReplaceState(),
transitions={'done': 'Gasket?'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_agv1',
'result': 'config_name_preagv'
})
# x:5 y:360
OperatableStateMachine.add('Agv2pregrasp',
ReplaceState(),
transitions={'done': 'Gasket2?'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_agv2',
'result': 'config_name_preagv'
})
return _state_machine
def create(self):
# x:926 y:151, x:511 y:227
_state_machine = OperatableStateMachine(
outcomes=['finished', 'fail'],
input_keys=['Shipments', 'NumberOfShipments'])
_state_machine.userdata.Shipments = []
_state_machine.userdata.NumberOfShipments = 0
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.AgvID = ''
_state_machine.userdata.ShipmentIndex = 1
_state_machine.userdata.ShipmentType = ''
_state_machine.userdata.ShipmentIterator = 0
_state_machine.userdata.OneValue = 1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:43 y:24
OperatableStateMachine.add('GetProducts',
GetProductsFromShipmentState(),
transitions={
'continue': 'ShipmenTypeMessage',
'invalid_index': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments': 'Shipments',
'index': 'ShipmentIterator',
'shipment_type': 'ShipmentType',
'agv_id': 'AgvID',
'products': 'Products',
'number_of_products':
'NumberOfProducts'
})
# x:740 y:34
OperatableStateMachine.add(
'IncrementShipmentsIterator',
AddNumericState(),
transitions={'done': 'CompareShepmentsIterator'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'ShipmentIterator',
'value_b': 'OneValue',
'result': 'ShipmentIterator'
})
# x:740 y:120
OperatableStateMachine.add('CompareShepmentsIterator',
EqualState(),
transitions={
'true': 'finished',
'false': 'GetProducts'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'ShipmentIterator',
'value_b': 'NumberOfShipments'
})
# x:564 y:30
OperatableStateMachine.add(
'get_products',
self.use_behavior(get_productsSM, 'get_products'),
transitions={
'finished': 'IncrementShipmentsIterator',
'fail': 'fail'
},
autonomy={
'finished': Autonomy.Inherit,
'fail': Autonomy.Inherit
},
remapping={
'Products': 'Products',
'NumberOfProducts': 'NumberOfProducts'
})
# x:252 y:25
OperatableStateMachine.add(
'ShipmenTypeMessage',
MessageState(),
transitions={'continue': 'AgvIdMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ShipmentType'})
# x:416 y:28
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'get_products'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'AgvID'})
return _state_machine
def create(self):
# x:1770 y:298, x:81 y:372
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'part_type', 'agv_id', 'pose_on_agv', 'camera_ref_frame',
'camera_frame', 'camera_topic'
],
output_keys=['part_type', 'camera_ref_frame', 'camera_frame'])
_state_machine.userdata.part_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.move_group_prefix1 = '/ariac/arm1'
_state_machine.userdata.move_group_prefix2 = '/ariac/arm2'
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.part_pose = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.config_name_bin1PreGrasp = 'bin1PreGrasp'
_state_machine.userdata.camera_ref_frame2 = 'arm2_linear_arm_actuator'
_state_machine.userdata.camera_ref_frame1 = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_frame2 = 'Camera_bin_2_frame'
_state_machine.userdata.camera_frame5 = 'Camera_bin_5_frame'
_state_machine.userdata.camera_frame6 = 'Camera_bin_6_frame'
_state_machine.userdata.config_name_bin2PreGrasp = 'bin2PreGrasp'
_state_machine.userdata.config_name_bin5PreGrasp = 'bin5PreGrasp'
_state_machine.userdata.camera_ref_frame = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.camera_frame1 = 'Camera_bin_1_frame'
_state_machine.userdata.config_name_bin6PreGrasp = 'bin6PreGrasp'
_state_machine.userdata.joint_names = ''
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.gripper1 = 'arm1'
_state_machine.userdata.gripper2 = 'arm2'
_state_machine.userdata.camera_topic = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:71 y:54
OperatableStateMachine.add('binSelectie',
self.use_behavior(
binSelectieSM, 'binSelectie'),
transitions={
'finished': 'arm1',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'camera_topic': 'camera_topic',
'camera_ref_frame':
'camera_ref_frame',
'camera_frame': 'camera_frame',
'part_offset': 'part_offset'
})
# x:1675 y:697
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'MoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:1675 y:758
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
# x:262 y:58
OperatableStateMachine.add('arm1',
EqualState(),
transitions={
'true': 'bin5',
'false': 'arm2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_ref_frame',
'value_b': 'camera_ref_frame1'
})
# x:266 y:389
OperatableStateMachine.add('arm2',
EqualState(),
transitions={
'true': 'bin1',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_ref_frame',
'value_b': 'camera_ref_frame2'
})
# x:470 y:353
OperatableStateMachine.add('bin1',
EqualState(),
transitions={
'true': 'pregraspbin1',
'false': 'bin2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame1'
})
# x:475 y:527
OperatableStateMachine.add('bin2',
EqualState(),
transitions={
'true': 'pregraspbin2',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame2'
})
# x:467 y:59
OperatableStateMachine.add('bin5',
EqualState(),
transitions={
'true': 'pregraspbin5',
'false': 'bin6'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame5'
})
# x:470 y:277
OperatableStateMachine.add('bin6',
EqualState(),
transitions={
'true': 'pregraspbin6',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame6'
})
# x:663 y:58
OperatableStateMachine.add('pregraspbin5',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin5',
'planning_failed': 'failed',
'control_failed': 'detectpartbin5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin5PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:652 y:273
OperatableStateMachine.add('pregraspbin6',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin6',
'planning_failed': 'failed',
'control_failed': 'detectpartbin6',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin6PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:653 y:351
OperatableStateMachine.add('pregraspbin1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin1',
'planning_failed': 'failed',
'control_failed': 'detectpartbin1',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:657 y:531
OperatableStateMachine.add('pregraspbin2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin2',
'planning_failed': 'failed',
'control_failed': 'detectpartbin2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin2PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1164 y:44
OperatableStateMachine.add('pickbin5',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'gripperkeuze',
'planning_failed': 'failed',
'control_failed': 'Retry'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix1',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1173 y:269
OperatableStateMachine.add('pickbin6',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripperaan_2',
'planning_failed': 'failed',
'control_failed': 'Retry_2'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix1',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1171 y:353
OperatableStateMachine.add('pickbin1',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripperaan_4',
'planning_failed': 'failed',
'control_failed': 'Retry_3'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix2',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1185 y:524
OperatableStateMachine.add('pickbin2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripperaan_3',
'planning_failed': 'failed',
'control_failed': 'Retry_4'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix2',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1568 y:43
OperatableStateMachine.add('pregraspbin5_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin5PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1568 y:262
OperatableStateMachine.add('pregraspbin6_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin5PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1566 y:349
OperatableStateMachine.add('pregraspbin1_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1569 y:515
OperatableStateMachine.add('pregraspbin2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin2PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1674 y:635
OperatableStateMachine.add('MoseMessage',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_pose'})
# x:818 y:46
OperatableStateMachine.add(
'detectpartbin5',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:1193 y:136
OperatableStateMachine.add('Retry',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze'},
autonomy={'done': Autonomy.Off})
# x:820 y:265
OperatableStateMachine.add(
'detectpartbin6',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5_2',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:820 y:348
OperatableStateMachine.add(
'detectpartbin1',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5_3',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:833 y:530
OperatableStateMachine.add(
'detectpartbin2',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5_4',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:997 y:44
OperatableStateMachine.add(
'Computegraspbin5',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1382 y:37
OperatableStateMachine.add('Gripperaan',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin5_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1363 y:129
OperatableStateMachine.add('gripperkeuze',
ReplaceState(),
transitions={'done': 'Gripperaan'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper1',
'result': 'arm_id'
})
# x:1000 y:254
OperatableStateMachine.add(
'Computegraspbin5_2',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1003 y:346
OperatableStateMachine.add(
'Computegraspbin5_3',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5_3',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1023 y:524
OperatableStateMachine.add(
'Computegraspbin5_4',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5_4',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1193 y:198
OperatableStateMachine.add('Retry_2',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze_2'},
autonomy={'done': Autonomy.Off})
# x:1206 y:434
OperatableStateMachine.add('Retry_3',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze_3'},
autonomy={'done': Autonomy.Off})
# x:1200 y:628
OperatableStateMachine.add('Retry_4',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze_4'},
autonomy={'done': Autonomy.Off})
# x:1362 y:199
OperatableStateMachine.add('gripperkeuze_2',
ReplaceState(),
transitions={'done': 'Gripperaan_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper1',
'result': 'arm_id'
})
# x:1359 y:426
OperatableStateMachine.add('gripperkeuze_3',
ReplaceState(),
transitions={'done': 'Gripperaan_4'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper2',
'result': 'arm_id'
})
# x:1357 y:642
OperatableStateMachine.add('gripperkeuze_4',
ReplaceState(),
transitions={'done': 'Gripperaan_3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper2',
'result': 'arm_id'
})
# x:1359 y:261
OperatableStateMachine.add('Gripperaan_2',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin6_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1377 y:511
OperatableStateMachine.add('Gripperaan_3',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin2_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1362 y:345
OperatableStateMachine.add('Gripperaan_4',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin1_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
return _state_machine
def create(self):
# x:587 y:648, x:611 y:249
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['Products', 'NumberOfProducts', 'agv_id'])
_state_machine.userdata.ProductIterator = 0
_state_machine.userdata.OneValue = 1
_state_machine.userdata.ProductType = ''
_state_machine.userdata.ProductPose = []
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.MaterialsLocationList = []
_state_machine.userdata.MaterialLocation = ''
_state_machine.userdata.MaterialLocationIndex = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.part_type1 = 'empty'
_state_machine.userdata.part_type2 = 'empty'
_state_machine.userdata.empty = 'empty'
_state_machine.userdata.done = 'false'
_state_machine.userdata.true = 'true'
_state_machine.userdata.false = 'false'
_state_machine.userdata.pose_on_agv1 = []
_state_machine.userdata.pose_on_agv2 = []
_state_machine.userdata.Zero = 0
_state_machine.userdata.Minus = -1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:84 y:253
OperatableStateMachine.add('SetCountlimit',
AddNumericState(),
transitions={'done': 'GetProduct'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'NumberOfProducts',
'value_b': 'Minus',
'result': 'NumberOfProducts'
})
# x:892 y:34
OperatableStateMachine.add('GetMaterialLocation',
GetItemFromListState(),
transitions={
'done': 'product iterator_2',
'invalid_index': 'failed'
},
autonomy={
'done': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'list': 'MaterialsLocationList',
'index': 'MaterialLocationIndex',
'item': 'MaterialLocation'
})
# x:1161 y:108
OperatableStateMachine.add('Orderverwerking',
self.use_behavior(
OrderverwerkingSM,
'Orderverwerking'),
transitions={
'finished': 'ResetPart1',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type1': 'part_type1',
'part_type2': 'part_type2',
'pose_on_agv1': 'pose_on_agv1',
'agv_id': 'agv_id',
'pose_on_agv2': 'pose_on_agv2'
})
# x:399 y:673
OperatableStateMachine.add('ProductITerator',
AddNumericState(),
transitions={'done': 'GetProduct'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'ProductIterator',
'value_b': 'OneValue',
'result': 'ProductIterator'
})
# x:672 y:673
OperatableStateMachine.add('CompareIterator',
EqualState(),
transitions={
'true': 'check arm',
'false': 'ProductITerator'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'NumberOfProducts',
'value_b': 'ProductIterator'
})
# x:585 y:37
OperatableStateMachine.add(
'GetMaterialsLocation',
GetMaterialLocationsState(),
transitions={'continue': 'GetMaterialLocation'},
autonomy={'continue': Autonomy.Off},
remapping={
'part': 'ProductType',
'material_locations': 'MaterialsLocationList'
})
# x:1391 y:708
OperatableStateMachine.add('SetPart1',
ReplaceState(),
transitions={'done': 'CompareIterator'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'ProductType',
'result': 'part_type1'
})
# x:1327 y:31
OperatableStateMachine.add('set arm1',
EqualState(),
transitions={
'true': 'SetPose1',
'false': 'set arm2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type1',
'value_b': 'empty'
})
# x:671 y:461
OperatableStateMachine.add('SetPart2',
ReplaceState(),
transitions={'done': 'GetProduct'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'ProductType',
'result': 'part_type2'
})
# x:1289 y:268
OperatableStateMachine.add('set arm2',
EqualState(),
transitions={
'true': 'SetPose2',
'false': 'Orderverwerking'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type2',
'value_b': 'empty'
})
# x:1012 y:262
OperatableStateMachine.add('ResetPart1',
ReplaceState(),
transitions={'done': 'ResetPart2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'empty',
'result': 'part_type1'
})
# x:1010 y:360
OperatableStateMachine.add('ResetPart2',
ReplaceState(),
transitions={'done': 'ResetPose1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'empty',
'result': 'part_type2'
})
# x:715 y:573
OperatableStateMachine.add('check arm',
EqualState(),
transitions={
'true': 'ResetIterator',
'false': 'done'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type1',
'value_b': 'empty'
})
# x:509 y:361
OperatableStateMachine.add('done?',
EqualState(),
transitions={
'true': 'ResetIterator',
'false': 'ProductITerator'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'done',
'value_b': 'true'
})
# x:1155 y:596
OperatableStateMachine.add('done',
ReplaceState(),
transitions={'done': 'Orderverwerking'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'true',
'result': 'done'
})
# x:979 y:471
OperatableStateMachine.add('SetPose2',
ReplaceState(),
transitions={'done': 'SetPart2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv2'
})
# x:1454 y:327
OperatableStateMachine.add('SetPose1',
ReplaceState(),
transitions={'done': 'SetPart1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv1'
})
# x:748 y:373
OperatableStateMachine.add('ResetPose1',
ReplaceState(),
transitions={'done': 'ResetPose2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'pose_on_agv1'
})
# x:754 y:269
OperatableStateMachine.add('ResetPose2',
ReplaceState(),
transitions={'done': 'done?'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'pose_on_agv2'
})
# x:283 y:59
OperatableStateMachine.add(
'product iterator',
MessageState(),
transitions={'continue': 'GetMaterialsLocation'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ProductIterator'})
# x:1134 y:23
OperatableStateMachine.add(
'product iterator_2',
MessageState(),
transitions={'continue': 'set arm1'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'ProductIterator'})
# x:138 y:480
OperatableStateMachine.add('GetProduct',
GetPartFromProductsState(),
transitions={
'continue': 'product iterator',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'Products',
'index': 'ProductIterator',
'type': 'ProductType',
'pose': 'pose_on_agv'
})
# x:502 y:551
OperatableStateMachine.add('ResetIterator',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'ProductIterator'
})
return _state_machine
def create(self):
# x:650 y:25, x:929 y:416
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.products = []
_state_machine.userdata.product_index = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.material_locations = []
_state_machine.userdata.bin_index = 0
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.bin_id = ''
_state_machine.userdata.old_order_id = ''
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:45 y:112
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:211 y:112
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'ConveyorOff'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id':
'order_id',
'shipments':
'shipments',
'number_of_shipments':
'number_of_shipments'
})
# x:424 y:113
OperatableStateMachine.add('TestLastOrder',
EqualState(),
transitions={
'true': 'EndAssignment',
'false': 'RememberOldOrder'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'order_id',
'value_b': 'old_order_id'
})
# x:650 y:114
OperatableStateMachine.add('RememberOldOrder',
ReplaceState(),
transitions={'done': 'GetShipment'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'order_id',
'result': 'old_order_id'
})
# x:869 y:114
OperatableStateMachine.add('GetShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'GetPart',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipment_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:1122 y:115
OperatableStateMachine.add('GetPart',
GetPartFromProductsState(),
transitions={
'continue': 'MaterialLocation',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'product_index',
'type': 'part_type',
'pose': 'part_pose'
})
# x:1361 y:113
OperatableStateMachine.add('MaterialLocation',
GetMaterialLocationsState(),
transitions={'continue': 'GetBin'},
autonomy={'continue': Autonomy.Off},
remapping={
'part':
'part_type',
'material_locations':
'material_locations'
})
# x:1361 y:235
OperatableStateMachine.add('GetBin',
GetItemFromListState(),
transitions={
'done': 'AGV_id',
'invalid_index': 'failed'
},
autonomy={
'done': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'list': 'material_locations',
'index': 'bin_index',
'item': 'bin_id'
})
# x:759 y:683
OperatableStateMachine.add('IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'EndShipment'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'one_value',
'result': 'shipment_index'
})
# x:536 y:683
OperatableStateMachine.add('EndShipment',
EqualState(),
transitions={
'true': 'Notify_shipment_ready',
'false': 'GetShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:446 y:6
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:206 y:680
OperatableStateMachine.add('Notify_shipment_ready',
self.use_behavior(
Notify_shipment_readySM,
'Notify_shipment_ready'),
transitions={
'finished': 'ResetShipmentIndex',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'shipment_type': 'shipment_type',
'agv_id': 'agv_id'
})
# x:1357 y:675
OperatableStateMachine.add(
'GripperChoice',
self.use_behavior(GripperChoiceSM, 'GripperChoice'),
transitions={
'finished': 'IncrementShipmentIndex',
'failed': 'failed',
'false': 'GetPart'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'false': Autonomy.Inherit
},
remapping={
'number_of_products': 'number_of_products',
'product_index': 'product_index',
'bin_id': 'bin_id',
'part_type': 'part_type',
'part_pose': 'part_pose',
'agv_id': 'agv_id'
})
# x:194 y:377
OperatableStateMachine.add('ResetShipmentIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'shipment_index'
})
# x:216 y:19
OperatableStateMachine.add('ConveyorOff',
SetConveyorbeltPowerState(),
transitions={
'continue': 'TestLastOrder',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'zero_value'})
# x:1384 y:446
OperatableStateMachine.add(
'AGV_id',
MessageState(),
transitions={'continue': 'GripperChoice'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
return _state_machine
def create(self):
# x:27 y:469, x:242 y:360
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group_g = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.arm_id = ''
_state_machine.userdata.part_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.one = 1
_state_machine.userdata.product_itt = 0
_state_machine.userdata.shipments_index = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:64 y:31
OperatableStateMachine.add(
'Start_assignment',
StartAssignment(),
transitions={'continue': 'move_home_belt'},
autonomy={'continue': Autonomy.Off})
# x:431 y:28
OperatableStateMachine.add(
'get_orders',
GetOrderState(),
transitions={'continue': 'Get shipments'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:615 y:25
OperatableStateMachine.add('Get shipments',
GetProductsFromShipmentState(),
transitions={
'continue': 'get part',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipments_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:833 y:35
OperatableStateMachine.add('get part',
GetPartFromProductsState(),
transitions={
'continue': 'message part main prg',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'part_index',
'type': 'part_type',
'pose': 'pose'
})
# x:261 y:28
OperatableStateMachine.add('move_home_belt',
self.use_behavior(
move_home_beltSM, 'move_home_belt'),
transitions={
'finished': 'get_orders',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:1153 y:72
OperatableStateMachine.add('Order_picking',
self.use_behavior(
Order_pickingSM, 'Order_picking'),
transitions={
'finished': 'add_itterator order',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'part_type': 'part_type'})
# x:1038 y:147
OperatableStateMachine.add(
'add_itterator order',
AddNumericState(),
transitions={'done': 'part index message'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'one',
'value_b': 'part_index',
'result': 'part_index'
})
# x:1034 y:328
OperatableStateMachine.add('Check_product_amount',
EqualState(),
transitions={
'true': 'move_home_belt',
'false': 'Get shipments'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
# x:1053 y:12
OperatableStateMachine.add(
'message part main prg',
MessageState(),
transitions={'continue': 'Order_picking'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:1141 y:221
OperatableStateMachine.add(
'part index message',
MessageState(),
transitions={'continue': 'Check_product_amount'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_index'})
return _state_machine
def create(self):
# x:860 y:686, x:845 y:354
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['agv_id', 'part_pose_right', 'part_pose_left'])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.agv1 = 'agv1'
_state_machine.userdata.agv2 = 'agv2'
_state_machine.userdata.Gantry_AGV1_right = 'Gantry_AGV1Right'
_state_machine.userdata.move_group_right = 'Right_Arm'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.move_group_gantry = 'Gantry'
_state_machine.userdata.PreGrasp_rightArm = 'Right_AGV'
_state_machine.userdata.PreGrasp_leftArm = 'Left_AGV'
_state_machine.userdata.tool_link_right = 'right_ee_link'
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.offset = 0.115
_state_machine.userdata.part_pose_right = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.part_pose_left = ''
_state_machine.userdata.arm_right = 'right_arm'
_state_machine.userdata.arm_left = 'left_arm'
_state_machine.userdata.Gantry_AGV1_left = 'Gantry_AGV1Left'
_state_machine.userdata.move_group_left = 'Left_Arm'
_state_machine.userdata.gantry_home = 'Gantry_Home'
_state_machine.userdata.Home_right_arm = 'Right_PreGrasp'
_state_machine.userdata.Home_left_arm = 'Left_PreGrasp'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:31 y:18
OperatableStateMachine.add('Gantry_Home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AGV1',
'planning_failed': 'Retry',
'control_failed': 'Retry',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'gantry_home',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:24 y:608
OperatableStateMachine.add('AGV2',
EqualState(),
transitions={
'true': 'GetAgv2Pose',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_id',
'value_b': 'agv2'
})
# x:254 y:615
OperatableStateMachine.add('GetAgv2Pose',
GetObjectPoseState(
object_frame='kit_tray_2',
ref_frame='Gantry'),
transitions={
'continue': 'finished',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_pose'})
# x:202 y:85
OperatableStateMachine.add(
'GetAgv1Pose',
GetObjectPoseState(object_frame='kit_tray_1',
ref_frame='world'),
transitions={
'continue': 'PreDropTrayGantryRight',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_pose'})
# x:351 y:86
OperatableStateMachine.add('PreDropTrayGantryRight',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreDropTrayRightArm',
'planning_failed': 'Retry_1',
'control_failed': 'Retry_1',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'Gantry_AGV1_right',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:378 y:4
OperatableStateMachine.add(
'Retry_1',
WaitState(wait_time=3),
transitions={'done': 'PreDropTrayGantryRight'},
autonomy={'done': Autonomy.Off})
# x:531 y:89
OperatableStateMachine.add('PreDropTrayRightArm',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputeDropPartPose',
'planning_failed': 'Retry_2',
'control_failed': 'Retry_2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'PreGrasp_rightArm',
'move_group': 'move_group_right',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:553 y:6
OperatableStateMachine.add(
'Retry_2',
WaitState(wait_time=3),
transitions={'done': 'PreDropTrayRightArm'},
autonomy={'done': Autonomy.Off})
# x:690 y:92
OperatableStateMachine.add(
'ComputeDropPartPose',
ComputeGraspPartOffsetAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'MoveToDrop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_right',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_right',
'pose': 'agv_pose',
'offset': 'offset',
'rotation': 'rotation',
'part_pose': 'part_pose_right',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:902 y:89
OperatableStateMachine.add('MoveToDrop',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperOff',
'planning_failed': 'Retry_3',
'control_failed': 'Retry_3'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_right',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:933 y:10
OperatableStateMachine.add('Retry_3',
WaitState(wait_time=3),
transitions={'done': 'MoveToDrop'},
autonomy={'done': Autonomy.Off})
# x:1086 y:86
OperatableStateMachine.add('GripperOff',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'PreDropTrayRightArm_2',
'failed': 'MoveToDrop',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_right'})
# x:1267 y:87
OperatableStateMachine.add('PreDropTrayRightArm_2',
SrdfStateToMoveitAriac(),
transitions={
'reached':
'PreDropTrayRightArm_2_2',
'planning_failed': 'Retry_4',
'control_failed': 'Retry_4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'PreGrasp_rightArm',
'move_group': 'move_group_right',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1291 y:8
OperatableStateMachine.add(
'Retry_4',
WaitState(wait_time=3),
transitions={'done': 'PreDropTrayRightArm_2'},
autonomy={'done': Autonomy.Off})
# x:1551 y:189
OperatableStateMachine.add('PreDropTrayGantryLeft',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreDropTrayLeftArm',
'planning_failed': 'Retry_5',
'control_failed': 'Retry_5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'Gantry_AGV1_left',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1555 y:266
OperatableStateMachine.add('PreDropTrayLeftArm',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputeDropPartPose_2',
'planning_failed': 'Retry_6',
'control_failed': 'Retry_6',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'PreGrasp_leftArm',
'move_group': 'move_group_left',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1518 y:345
OperatableStateMachine.add(
'ComputeDropPartPose_2',
ComputeGraspPartOffsetAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'MoveToDrop_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_left',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_left',
'pose': 'agv_pose',
'offset': 'offset',
'rotation': 'rotation',
'part_pose': 'part_pose_left',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1519 y:419
OperatableStateMachine.add('MoveToDrop_2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperOff_2',
'planning_failed': 'Retry_7',
'control_failed': 'Retry_7'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_left',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1523 y:510
OperatableStateMachine.add('GripperOff_2',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'PreDropTrayLeftArm_2',
'failed': 'MoveToDrop_2',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_left'})
# x:1542 y:606
OperatableStateMachine.add('PreDropTrayLeftArm_2',
SrdfStateToMoveitAriac(),
transitions={
'reached':
'PreDropTrayLeftArm_2_2_2',
'planning_failed': 'Retry_8',
'control_failed': 'Retry_8',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'PreGrasp_leftArm',
'move_group': 'move_group_left',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1719 y:183
OperatableStateMachine.add(
'Retry_5',
WaitState(wait_time=3),
transitions={'done': 'PreDropTrayGantryLeft'},
autonomy={'done': Autonomy.Off})
# x:1720 y:264
OperatableStateMachine.add(
'Retry_6',
WaitState(wait_time=3),
transitions={'done': 'PreDropTrayLeftArm'},
autonomy={'done': Autonomy.Off})
# x:1715 y:416
OperatableStateMachine.add('Retry_7',
WaitState(wait_time=3),
transitions={'done': 'MoveToDrop_2'},
autonomy={'done': Autonomy.Off})
# x:1717 y:597
OperatableStateMachine.add(
'Retry_8',
WaitState(wait_time=3),
transitions={'done': 'PreDropTrayLeftArm_2'},
autonomy={'done': Autonomy.Off})
# x:207 y:9
OperatableStateMachine.add('Retry',
WaitState(wait_time=3),
transitions={'done': 'Gantry_Home'},
autonomy={'done': Autonomy.Off})
# x:25 y:157
OperatableStateMachine.add('AGV1',
EqualState(),
transitions={
'true': 'AttachedRight',
'false': 'AGV2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv_id',
'value_b': 'agv1'
})
# x:1462 y:93
OperatableStateMachine.add('PreDropTrayRightArm_2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Attached_Left',
'planning_failed': 'Retry_9_2',
'control_failed': 'Retry_9_2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'Home_right_arm',
'move_group': 'move_group_right',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1524 y:697
OperatableStateMachine.add('PreDropTrayLeftArm_2_2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Home_Gantry',
'planning_failed': 'Retry_9',
'control_failed': 'Retry_9',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'Home_left_arm',
'move_group': 'move_group_left',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1720 y:705
OperatableStateMachine.add(
'Retry_9',
WaitState(wait_time=3),
transitions={'done': 'PreDropTrayLeftArm_2_2_2'},
autonomy={'done': Autonomy.Off})
# x:1495 y:6
OperatableStateMachine.add(
'Retry_9_2',
WaitState(wait_time=3),
transitions={'done': 'PreDropTrayRightArm_2_2'},
autonomy={'done': Autonomy.Off})
# x:1124 y:654
OperatableStateMachine.add('Home_Gantry',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'Retry_9_3',
'control_failed': 'Retry_9_3',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'gantry_home',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1147 y:732
OperatableStateMachine.add('Retry_9_3',
WaitState(wait_time=3),
transitions={'done': 'Home_Gantry'},
autonomy={'done': Autonomy.Off})
# x:201 y:157
OperatableStateMachine.add('AttachedRight',
CheckGripperattached(),
transitions={
'True': 'TestMessagePositie1',
'False': 'Home_Gantry',
'invalid_arm_id': 'failed'
},
autonomy={
'True': Autonomy.Off,
'False': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_right'})
# x:1671 y:94
OperatableStateMachine.add('Attached_Left',
CheckGripperattached(),
transitions={
'True': 'PreDropTrayGantryLeft',
'False': 'Home_Gantry',
'invalid_arm_id': 'failed'
},
autonomy={
'True': Autonomy.Off,
'False': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_left'})
# x:199 y:253
OperatableStateMachine.add(
'TestMessagePositie1',
MessageState(),
transitions={'continue': 'GetAgv1Pose'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_pose_right'})
return _state_machine
def create(self):
# x:1630 y:562, x:1479 y:95
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['arm'])
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.power = 100
_state_machine.userdata.NoPower = 0
_state_machine.userdata.part = ''
_state_machine.userdata.ref_frame1 = 'arm1_linear_arm_actuator'
_state_machine.userdata.Camera1_topic = '/ariac/Camera_Converyor_Links'
_state_machine.userdata.camera_frame1 = 'Camera_Converyor_Links_frame'
_state_machine.userdata.part_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.arm = ''
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.Disk_rotation = 0
_state_machine.userdata.Disk_offset = 0.035
_state_machine.userdata.config_name_R1PreBin3 = 'R1PreBin3'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.config_name_R1PreConveyor = 'R1PreConveyor'
_state_machine.userdata.Round_Nr = 1
_state_machine.userdata.Round_target = 6
_state_machine.userdata.Zero = 0
_state_machine.userdata.ONE = 1
_state_machine.userdata.SideYoffset = 0
_state_machine.userdata.SideXoffset = 0
_state_machine.userdata.PlusOffset = 0.20
_state_machine.userdata.config_name_R1HOME = 'R1Home'
_state_machine.userdata.Row_count = 3
_state_machine.userdata.offset = 0.040
_state_machine.userdata.rotation = 0
_state_machine.userdata.BIN3_pose = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:49 y:25
OperatableStateMachine.add('MoveToHOME',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GripperUIT_2',
'planning_failed': 'RetryHOME',
'control_failed': 'RetryHOME',
'param_error': 'WAIT_failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1HOME',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1262 y:559
OperatableStateMachine.add(
'WAIT_completed',
WaitState(wait_time=2),
transitions={'done': 'SluitConveyorAF_2'},
autonomy={'done': Autonomy.Off})
# x:202 y:91
OperatableStateMachine.add('Stopconveyor_2',
SetConveyorbeltPowerState(),
transitions={
'continue': 'MoveToPreConveyor',
'fail': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'NoPower'})
# x:1106 y:35
OperatableStateMachine.add('WAIT_failed',
WaitState(wait_time=5),
transitions={'done': 'SluitConveyorAF'},
autonomy={'done': Autonomy.Off})
# x:563 y:30
OperatableStateMachine.add('WAIT_check',
WaitState(wait_time=1),
transitions={'done': 'CheckforDisk'},
autonomy={'done': Autonomy.Off})
# x:372 y:91
OperatableStateMachine.add(
'CheckforDisk',
DetectFirstPartCameraAriacState(part_list=['disk_part'],
time_out=2),
transitions={
'continue': 'Stopconveyor_2',
'failed': 'WAIT_failed',
'not_found': 'WAIT_check'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame1',
'camera_topic': 'Camera1_topic',
'camera_frame': 'camera_frame1',
'part': 'part',
'pose': 'pose'
})
# x:200 y:162
OperatableStateMachine.add(
'Computebeltpick',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'MoveToConveyorPart',
'failed': 'WAIT_failed_pick'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'Disk_offset',
'rotation': 'Disk_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:371 y:166
OperatableStateMachine.add('MoveToConveyorPart',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'WAIT_gripperoke',
'planning_failed':
'WAIT_failed_pick',
'control_failed': 'WAIT_failed_drop'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1106 y:97
OperatableStateMachine.add('WAIT_failed_pick',
WaitState(wait_time=5),
transitions={'done': 'SluitConveyorAF'},
autonomy={'done': Autonomy.Off})
# x:201 y:299
OperatableStateMachine.add('MoveToBin3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetBin3Pose',
'planning_failed': 'RetryBin3',
'control_failed': 'RetryBin3',
'param_error': 'WAIT_failed_pick'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R1PreBin3',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:51 y:163
OperatableStateMachine.add('Gripperaan',
UseGripper(enable=True),
transitions={
'continue': 'Computebeltpick',
'failed': 'WAIT_failed_pick',
'invalid_arm': 'WAIT_failed_pick'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:370 y:243
OperatableStateMachine.add(
'WAIT_gripperoke',
WaitState(wait_time=0.5),
transitions={'done': 'MoveToPreConveyor_2'},
autonomy={'done': Autonomy.Off})
# x:51 y:96
OperatableStateMachine.add(
'MoveToPreConveyor',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Gripperaan',
'planning_failed': 'RetryPreConveyor',
'control_failed': 'RetryPreConveyor',
'param_error': 'WAIT_failed_pick'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1PreConveyor',
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out': 'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out': 'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:22 y:707
OperatableStateMachine.add(
'RetryPreConveyor',
WaitState(wait_time=1),
transitions={'done': 'MoveToPreConveyor'},
autonomy={'done': Autonomy.Off})
# x:261 y:768
OperatableStateMachine.add('RetryBin3',
WaitState(wait_time=1),
transitions={'done': 'MoveToBin3'},
autonomy={'done': Autonomy.Off})
# x:200 y:231
OperatableStateMachine.add(
'MoveToPreConveyor_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveToBin3',
'planning_failed': 'RetryPreConveyor_2',
'control_failed': 'RetryPreConveyor_2',
'param_error': 'WAIT_failed_pick'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1PreConveyor',
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out': 'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out': 'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:20 y:769
OperatableStateMachine.add(
'RetryPreConveyor_2',
WaitState(wait_time=1),
transitions={'done': 'MoveToPreConveyor_2'},
autonomy={'done': Autonomy.Off})
# x:356 y:566
OperatableStateMachine.add('GripperUIT',
UseGripper(enable=False),
transitions={
'continue': 'MoveToBin3_2',
'failed': 'WAIT_failed_pick',
'invalid_arm': 'WAIT_failed_pick'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1033 y:518
OperatableStateMachine.add('AddRound',
AddNumericState(),
transitions={'done': 'CheckRound'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'Round_Nr',
'value_b': 'ONE',
'result': 'Round_Nr'
})
# x:1032 y:584
OperatableStateMachine.add('CheckRound',
EqualState(),
transitions={
'true': 'ResetRound',
'false': 'StartConveryor'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'Round_Nr',
'value_b': 'Round_target'
})
# x:1031 y:654
OperatableStateMachine.add('ResetRound',
ReplaceState(),
transitions={'done': 'WAIT_completed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Round_Nr',
'result': 'Zero'
})
# x:360 y:382
OperatableStateMachine.add(
'ComputeDrop',
ComputeBeltDrop(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'ShowDropPose',
'failed': 'WAIT_failed_drop'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'BIN3_pose',
'offset': 'offset',
'SideYoffset': 'SideYoffset',
'SideXoffset': 'SideXoffset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:356 y:507
OperatableStateMachine.add('MoveToDROP',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperUIT',
'planning_failed': 'RetryDROP',
'control_failed': 'RetryDROP'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1106 y:159
OperatableStateMachine.add('WAIT_failed_drop',
WaitState(wait_time=5),
transitions={'done': 'SluitConveyorAF'},
autonomy={'done': Autonomy.Off})
# x:360 y:768
OperatableStateMachine.add('RetryDROP',
WaitState(wait_time=1),
transitions={'done': 'MoveToDROP'},
autonomy={'done': Autonomy.Off})
# x:743 y:522
OperatableStateMachine.add('AddYOffset',
AddNumericState(),
transitions={'done': 'CheckRow'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'SideYoffset',
'value_b': 'PlusOffset',
'result': 'SideYoffset'
})
# x:742 y:579
OperatableStateMachine.add('CheckRow',
EqualState(),
transitions={
'true': 'ResetRow',
'false': 'AddRound'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'Row_count',
'value_b': 'Round_Nr'
})
# x:741 y:647
OperatableStateMachine.add('ResetRow',
ReplaceState(),
transitions={'done': 'AddXOffset'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'SideYoffset'
})
# x:739 y:711
OperatableStateMachine.add('AddXOffset',
AddNumericState(),
transitions={'done': 'AddRound'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'SideXoffset',
'value_b': 'PlusOffset',
'result': 'SideXoffset'
})
# x:156 y:768
OperatableStateMachine.add(
'RetryDropBin',
WaitState(wait_time=1),
transitions={'done': 'MoveToConveyorPart'},
autonomy={'done': Autonomy.Off})
# x:22 y:645
OperatableStateMachine.add('RetryHOME',
WaitState(wait_time=1),
transitions={'done': 'MoveToHOME'},
autonomy={'done': Autonomy.Off})
# x:371 y:28
OperatableStateMachine.add('StartConveryor',
SetConveyorbeltPowerState(),
transitions={
'continue': 'WAIT_check',
'fail': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power'})
# x:203 y:26
OperatableStateMachine.add('GripperUIT_2',
UseGripper(enable=False),
transitions={
'continue': 'StartConveryor',
'failed': 'WAIT_failed',
'invalid_arm': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1300 y:89
OperatableStateMachine.add('SluitConveyorAF',
SetConveyorbeltPowerState(),
transitions={
'continue': 'failed',
'fail': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'NoPower'})
# x:1396 y:560
OperatableStateMachine.add('SluitConveyorAF_2',
SetConveyorbeltPowerState(),
transitions={
'continue': 'finished',
'fail': 'WAIT_completed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'NoPower'})
# x:368 y:311
OperatableStateMachine.add(
'GetBin3Pose',
GetObjectPoseState(object_frame='kit_tray_2',
ref_frame='arm1_linear_arm_actuator'),
transitions={
'continue': 'ComputeDrop',
'failed': 'WAIT_failed_drop'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'BIN3_pose'})
# x:361 y:443
OperatableStateMachine.add('ShowDropPose',
MessageState(),
transitions={'continue': 'MoveToDROP'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'BIN3_pose'})
# x:536 y:562
OperatableStateMachine.add(
'MoveToBin3_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AddYOffset',
'planning_failed': 'WAIT_failed_drop',
'control_failed': 'WAIT_failed_drop',
'param_error': 'WAIT_failed_drop'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1PreBin3',
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out': 'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out': 'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
return _state_machine