def create(self):
# x:1056 y:73, x:588 y:324
_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.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.offset = 0.1
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:52 y:43
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={
'continue': 'transport_part_form_bin_to_agv_state'
},
autonomy={'continue': Autonomy.Off})
# x:822 y:60
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:510 y:50
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': 'EndAssignment',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'agv_id': 'agv_id',
'pose_on_agv': 'pose_on_agv',
'pose': 'pose'
})
return _state_machine
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:599 y:55, x:321 y:144
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.agv_id = 'agv1'
_state_machine.userdata.shipment_type = 'order_0_shipment_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': 'SubmitShipment'},
autonomy={'continue': Autonomy.Off})
# x:443 y:42
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:322 y:42
OperatableStateMachine.add('Wait',
WaitState(wait_time=30),
transitions={'done': 'EndAssignment'},
autonomy={'done': Autonomy.Off})
# x:167 y:42
OperatableStateMachine.add('SubmitShipment',
SubmitShipmentState(),
transitions={
'continue': 'Wait',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'agv_id': 'agv_id',
'shipment_type': 'shipment_type',
'inspection_result':
'inspection_result'
})
return _state_machine
def create(self):
# x:649 y:50, x:296 y:171
_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 = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:52 y:43
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={
'continue': 'transport_part_form_bin_to_agv_state'
},
autonomy={'continue': Autonomy.Off})
# x:445 y:45
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:188 y:44
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': 'EndAssignment',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'agv_id': 'agv_id',
'pose_on_agv': 'pose_on_agv'
})
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:636 y:50, x:263 y:129
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.arm = 'arm1'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:49 y:39
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={
'continue': 'transport_part_from_belt_to_bin_state'
},
autonomy={'continue': Autonomy.Off})
# x:472 y:44
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:196 y:40
OperatableStateMachine.add(
'transport_part_from_belt_to_bin_state',
self.use_behavior(transport_part_from_belt_to_bin_stateSM,
'transport_part_from_belt_to_bin_state'),
transitions={
'finished': 'EndAssignment',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'arm': 'arm'})
return _state_machine
def create(self):
# x:667 y:540, x:729 y:205
_state_machine = OperatableStateMachine(outcomes=['finished', 'fail'])
_state_machine.userdata.shipments = []
_state_machine.userdata.part_type = ''
_state_machine.userdata.material_locations = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.order_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.product_index = 0
_state_machine.userdata.shipment_index = 1
_state_machine.userdata.add_one = 1
_state_machine.userdata.part_pose = []
_state_machine.userdata.zero = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Start',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:905 y:34
OperatableStateMachine.add('GetProductPose',
GetPartFromProductsState(),
transitions={
'continue': 'message_3',
'invalid_index': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'product_index',
'type': 'part_type',
'pose': 'part_pose'
})
# x:1091 y:123
OperatableStateMachine.add('TransportPartToAgv',
self.use_behavior(
TransportPartToAgvSM,
'TransportPartToAgv'),
transitions={
'finished': 'IncrementProductIndex',
'failed': 'fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'agv_id': 'agv_id',
'part_type': 'part_type',
'part_pose': 'part_pose'
})
# x:1095 y:229
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'ShipmentReady?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'add_one',
'result': 'product_index'
})
# x:1092 y:486
OperatableStateMachine.add(
'IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'notify_shipment_ready'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'add_one',
'result': 'shipment_index'
})
# x:1094 y:329
OperatableStateMachine.add('ShipmentReady?',
EqualState(),
transitions={
'true': 'ResetProductIndex',
'false': 'GetProductPose'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:474 y:461
OperatableStateMachine.add('OrderReady?',
EqualState(),
transitions={
'true': 'ResetShiptIndex',
'false': 'GetProductsShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:857 y:483
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(
notify_shipment_readySM,
'notify_shipment_ready'),
transitions={
'finished': 'OrderReady?',
'failed': 'fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:402 y:33
OperatableStateMachine.add(
'message',
MessageLogger(),
transitions={'continue': 'GetProductsShipment'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'order_id'})
# x:766 y:40
OperatableStateMachine.add(
'message_2',
MessageLogger(),
transitions={'continue': 'GetProductPose'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'shipment_type'})
# x:1090 y:36
OperatableStateMachine.add(
'message_3',
MessageLogger(),
transitions={'continue': 'TransportPartToAgv'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:560 y:34
OperatableStateMachine.add('GetProductsShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'message_2',
'invalid_index': 'fail'
},
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:203 y:32
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'message'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id':
'order_id',
'shipments':
'shipments',
'number_of_shipments':
'number_of_shipments'
})
# x:1090 y:407
OperatableStateMachine.add(
'ResetProductIndex',
ReplaceState(),
transitions={'done': 'IncrementShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'product_index'
})
# x:338 y:318
OperatableStateMachine.add('ResetShiptIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'shipment_index'
})
return _state_machine
def create(self):
# x:821 y:56, x:1214 y:358
_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.old_order_id = ''
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.material_locations = []
_state_machine.userdata.bin_index = 0
_state_machine.userdata.bin_id = ''
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.arm_right = 'right_arm'
_state_machine.userdata.arm_left = 'left_arm'
_state_machine.userdata.product_location1 = ''
_state_machine.userdata.product_location2 = ''
_state_machine.userdata.home_rightarm_id = 'Right_PreGrasp'
_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.home_leftarm_id = 'Left_PreGrasp'
_state_machine.userdata.move_group_left = 'Left_Arm'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:57 y:48
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:218 y:48
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'Test_LastOrder'},
autonomy={'continue': Autonomy.Off},
remapping={'order_id': 'order_id', 'shipments': 'shipments', 'number_of_shipments': 'number_of_shipments'})
# x:422 y:47
OperatableStateMachine.add('Test_LastOrder',
EqualState(),
transitions={'true': 'EndAssignment', 'false': 'Remember_OldOrder'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'order_id', 'value_b': 'old_order_id'})
# x:614 y:124
OperatableStateMachine.add('Remember_OldOrder',
ReplaceState(),
transitions={'done': 'GetShipment'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'order_id', 'result': 'old_order_id'})
# x:622 y:47
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:801 y:119
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:1023 y:119
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:1217 y:116
OperatableStateMachine.add('MaterialLocation',
GetMaterialLocationsState(),
transitions={'continue': 'GetBin'},
autonomy={'continue': Autonomy.Off},
remapping={'part': 'part_type', 'material_locations': 'material_locations'})
# x:1403 y:114
OperatableStateMachine.add('GetBin',
GetItemFromListState(),
transitions={'done': 'CheckAttachedRight', 'invalid_index': 'failed'},
autonomy={'done': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'list': 'material_locations', 'index': 'bin_index', 'item': 'bin_id'})
# x:1361 y:507
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'EndProduct'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'one_value', 'result': 'product_index'})
# x:1360 y:585
OperatableStateMachine.add('EndProduct',
EqualState(),
transitions={'true': 'HomepositieRightArm', 'false': 'CheckAttachedRight2'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'number_of_products'})
# x:95 y:565
OperatableStateMachine.add('ResetProductIndex',
ReplaceState(),
transitions={'done': 'IncrementShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'zero_value', 'result': 'product_index'})
# x:98 y:469
OperatableStateMachine.add('IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'Notify_Shipment_Ready'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'shipment_index', 'value_b': 'one_value', 'result': 'shipment_index'})
# x:89 y:277
OperatableStateMachine.add('EndShipment',
EqualState(),
transitions={'true': 'ResetShipmentIndex', 'false': 'GetShipment'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'shipment_index', 'value_b': 'number_of_shipments'})
# x:87 y:378
OperatableStateMachine.add('Notify_Shipment_Ready',
self.use_behavior(Notify_Shipment_ReadySM, 'Notify_Shipment_Ready'),
transitions={'finished': 'EndShipment', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'shipment_type': 'shipment_type'})
# x:1605 y:181
OperatableStateMachine.add('Transport_part_from_bin_to_agv',
self.use_behavior(Transport_part_from_bin_to_agvSM, 'Transport_part_from_bin_to_agv'),
transitions={'finished': 'SetVariablePart1', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_type': 'part_type', 'part_pose': 'part_pose', 'bin_id': 'bin_id'})
# x:95 y:183
OperatableStateMachine.add('ResetShipmentIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'zero_value', 'result': 'shipment_index'})
# x:1354 y:248
OperatableStateMachine.add('CheckAttachedRight',
CheckGripperattached(),
transitions={'True': 'CheckAttachedLeft', 'False': 'Transport_part_from_bin_to_agv', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_right'})
# x:1356 y:335
OperatableStateMachine.add('CheckAttachedLeft',
CheckGripperattached(),
transitions={'True': 'IncrementProductIndex', 'False': 'Transport_part_from_bin_to_agv_LeftArm', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_left'})
# x:1572 y:418
OperatableStateMachine.add('Transport_part_from_bin_to_agv_LeftArm',
self.use_behavior(Transport_part_from_bin_to_agv_LeftArmSM, 'Transport_part_from_bin_to_agv_LeftArm'),
transitions={'finished': 'SetVariablePart2', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_type': 'part_type', 'part_pose': 'part_pose', 'bin_id': 'bin_id'})
# x:1654 y:275
OperatableStateMachine.add('SetVariablePart1',
ReplaceState(),
transitions={'done': 'IncrementProductIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'part_pose', 'result': 'product_location1'})
# x:1651 y:503
OperatableStateMachine.add('SetVariablePart2',
ReplaceState(),
transitions={'done': 'IncrementProductIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'part_pose', 'result': 'product_location2'})
# x:997 y:510
OperatableStateMachine.add('CheckAttachedRight2',
CheckGripperattached(),
transitions={'True': 'CheckAttachedLeft2', 'False': 'GetPart', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_right'})
# x:803 y:520
OperatableStateMachine.add('CheckAttachedLeft2',
CheckGripperattached(),
transitions={'True': 'PlaceOnAgv', 'False': 'GetPart', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_left'})
# x:533 y:450
OperatableStateMachine.add('PlaceOnAgv',
self.use_behavior(PlaceOnAgvSM, 'PlaceOnAgv'),
transitions={'finished': 'GetPart', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_pose_right': 'product_location1', 'part_pose_left': 'product_location2'})
# x:1371 y:670
OperatableStateMachine.add('HomepositieRightArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'HomepositieLeftArm', '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': 'home_rightarm_id', '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:1182 y:655
OperatableStateMachine.add('HomepositieLeftArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PlaceOnAgv_2', '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': 'home_leftarm_id', '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:1558 y:671
OperatableStateMachine.add('Retry_1',
WaitState(wait_time=3),
transitions={'done': 'HomepositieRightArm'},
autonomy={'done': Autonomy.Off})
# x:1226 y:733
OperatableStateMachine.add('Retry_2',
WaitState(wait_time=3),
transitions={'done': 'HomepositieLeftArm'},
autonomy={'done': Autonomy.Off})
# x:832 y:626
OperatableStateMachine.add('PlaceOnAgv_2',
self.use_behavior(PlaceOnAgvSM, 'PlaceOnAgv_2'),
transitions={'finished': 'ResetProductIndex', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_pose_right': 'product_location1', 'part_pose_left': 'product_location2'})
return _state_machine
def create(self):
# x:644 y:287
_state_machine = OperatableStateMachine(outcomes=['failed'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.part_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.config_name_grasp_right = 'beltPreGrasp'
_state_machine.userdata.move_group = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.joint_names = ''
_state_machine.userdata.joint_values = []
_state_machine.userdata.config_name_right = 'Right_Home'
_state_machine.userdata.move_group_right = 'Right_Arm'
_state_machine.userdata.part_type_right = ''
_state_machine.userdata.part_type_left = ''
_state_machine.userdata.config_name_place = 'gantryPosPlace'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:41 y:129
OperatableStateMachine.add('enable_grippers',
self.use_behavior(enable_grippersSM, 'enable_grippers'),
transitions={'finished': 'ExecuteMainBeltBin', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:195 y:41
OperatableStateMachine.add('ConveyorPowerOn',
SetConveyorbeltPowerState(),
transitions={'continue': 'move_home_belt', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'powerOn'})
# x:384 y:41
OperatableStateMachine.add('move_home_belt',
self.use_behavior(move_home_beltSM, 'move_home_belt'),
transitions={'finished': 'detect_product_belt', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:576 y:42
OperatableStateMachine.add('detect_product_belt',
self.use_behavior(detect_product_beltSM, 'detect_product_belt'),
transitions={'finished': 'BeltPreGraspRight', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'part_type': 'part_type', 'pose': 'pose'})
# x:926 y:44
OperatableStateMachine.add('move_to_part_belt_right_arm',
self.use_behavior(move_to_part_belt_right_armSM, 'move_to_part_belt_right_arm'),
transitions={'finished': 'MoveRightSafety', 'failed': 'failed', 'unkown_id': 'BeltPreGraspRight'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'unkown_id': Autonomy.Inherit},
remapping={'part_type': 'part_type', 'pose': 'pose', 'joint_values': 'joint_values', 'part_type_right': 'part_type_right'})
# x:762 y:43
OperatableStateMachine.add('BeltPreGraspRight',
SrdfStateToMoveitAriac(),
transitions={'reached': 'move_to_part_belt_right_arm', 'planning_failed': 'detect_product_belt', 'control_failed': 'move_to_part_belt_right_arm', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_grasp_right', '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:1156 y:45
OperatableStateMachine.add('MoveRightSafety',
SrdfStateToMoveitAriac(),
transitions={'reached': 'moveplacepos', 'planning_failed': 'failed', 'control_failed': 'moveplacepos', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_right', '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:39 y:40
OperatableStateMachine.add('ExecuteMainBeltBin',
StartAssignment(),
transitions={'continue': 'ConveyorPowerOn'},
autonomy={'continue': Autonomy.Off})
# x:1156 y:193
OperatableStateMachine.add('moveplacepos',
SrdfStateToMoveitAriac(),
transitions={'reached': 'move_part_to_bin', 'planning_failed': 'failed', 'control_failed': 'move_part_to_bin', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_place', '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:1131 y:348
OperatableStateMachine.add('move_part_to_bin',
self.use_behavior(move_part_to_binSM, 'move_part_to_bin'),
transitions={'finished': 'move_home_belt_2', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'part_type_right': 'part_type_right', 'part_type_left': 'part_type_left'})
# x:528 y:489
OperatableStateMachine.add('move_home_belt_2',
self.use_behavior(move_home_beltSM, 'move_home_belt_2'),
transitions={'finished': 'ConveyorPowerOn', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
# x:448 y:497, x:101 y:396
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.arm_id = ''
_state_machine.userdata.power_on = 80
_state_machine.userdata.power_off = 0
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix1 = '/ariac/arm1'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.move_group_prefix2 = '/ariac/arm2'
_state_machine.userdata.ref_frame = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_belt1'
_state_machine.userdata.camera_frame = 'logical_camera_belt1_frame'
_state_machine.userdata.part_type = 'disk_part'
_state_machine.userdata.pose = []
_state_machine.userdata.config_name_belt = 'robotBelt'
_state_machine.userdata.sensor_topic = '/ariac/break_beam_1'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.part_offset = 0.04
_state_machine.userdata.part_rotation = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:32 y:30
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={'continue': 'TurnOnConveyor'},
autonomy={'continue': Autonomy.Off})
# x:522 y:27
OperatableStateMachine.add('MoveR2home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveR1Belt',
'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_home',
'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:176 y:29
OperatableStateMachine.add('TurnOnConveyor',
SetConveyorbeltPowerState(),
transitions={
'continue': 'MoveR1Home',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power_on'})
# x:360 y:28
OperatableStateMachine.add('MoveR1Home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveR2home',
'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_home',
'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:690 y:25
OperatableStateMachine.add('MoveR1Belt',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectPart',
'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_belt',
'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:863 y:25
OperatableStateMachine.add(
'DetectPart',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'TurnOffConveyor',
'failed': 'WaitRetry',
'not_found': 'WaitRetry'
},
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:715 y:141
OperatableStateMachine.add('WaitRetry',
WaitState(wait_time=0.1),
transitions={'done': 'DetectPart'},
autonomy={'done': Autonomy.Off})
# x:1127 y:363
OperatableStateMachine.add('MoveToPart',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'EnableGripper',
'planning_failed': 'failed',
'control_failed': 'EnableGripper'
},
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:1136 y:455
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={
'continue': 'MoveHome',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1115 y:267
OperatableStateMachine.add(
'ComputeGrasp',
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': 'MoveToPart',
'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': 'pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1081 y:544
OperatableStateMachine.add('MoveHome',
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_home',
'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:1043 y:58
OperatableStateMachine.add('TurnOffConveyor',
SetConveyorbeltPowerState(),
transitions={
'continue': 'DetectPart_2',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power_off'})
# x:1071 y:136
OperatableStateMachine.add(
'DetectPart_2',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'ComputeGrasp',
'failed': 'failed',
'not_found': 'WaitRetry'
},
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'
})
return _state_machine
def create(self):
# x:1648 y:487, x:394 y:242
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.config_name_gantry_Pre_Conv_LA = 'Gantry_PreGrasp_Conv_LA'
_state_machine.userdata.move_group_gantry = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.poweroff = 0
_state_machine.userdata.Partsaantal = 0
_state_machine.userdata.Partsvast = 3
_state_machine.userdata.Plus1 = 1
_state_machine.userdata.poweron = 100
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_4'
_state_machine.userdata.camera_frame = 'logical_camera_4_frame'
_state_machine.userdata.part = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.move_group_right_arm = 'Right_Arm'
_state_machine.userdata.move_group_left_arm = 'Left_Arm'
_state_machine.userdata.arm_idL = 'left'
_state_machine.userdata.arm_idR = 'right'
_state_machine.userdata.config_name_Left_Home = ''
_state_machine.userdata.config_name_Right_Home = ''
_state_machine.userdata.config_name_gantry_Pre_Conv_RA = 'Gantry_PreGrasp_Conv_RA'
_state_machine.userdata.config_name_RA_pre = 'config_name_RA_pre'
_state_machine.userdata.config_name_LA_pre = 'config_name_LA_pre'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'Start',
StartAssignment(),
transitions={'continue': 'Move_to_Home'},
autonomy={'continue': Autonomy.Off})
# x:213 y:41
OperatableStateMachine.add('Move_to_Home',
self.use_behavior(
Move_to_HomeSM, 'Move_to_Home'),
transitions={
'finished': 'Oppakken',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'arm_idL':
'arm_idL',
'arm_idR':
'arm_idR',
'config_name_PreSide':
'config_name_PreSide',
'config_name_Right_Home':
'config_name_Right_Home',
'config_name_Left_Home':
'config_name_Left_Home',
'config_name_gantry':
'config_name_gantry'
})
# x:977 y:473
OperatableStateMachine.add('3 parts?',
EqualState(),
transitions={
'true': 'finished',
'false': 'Start conveyer'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'Partsvast',
'value_b': 'Partsaantal'
})
# x:1233 y:370
OperatableStateMachine.add('increase',
AddNumericState(),
transitions={'done': '3 parts?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'Plus1',
'value_b': 'Partsaantal',
'result': 'Partsaantal'
})
# x:601 y:399
OperatableStateMachine.add('Start conveyer',
SetConveyorbeltPowerState(),
transitions={
'continue': 'Oppakken',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'poweron'})
# x:823 y:35
OperatableStateMachine.add(
'Oppakken',
self.use_behavior(OppakkenSM, 'Oppakken'),
transitions={
'finished': 'Droppen',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part': 'part',
'pose': 'pose',
'move_group_prefix': 'move_group_prefix',
'move_group_right_arm': 'move_group_right_arm',
'move_group_left_arm': 'move_group_left_arm',
'action_topic': 'action_topic',
'camera_frame': 'camera_frame',
'camera_topic': 'camera_topic',
'poweroff': 'poweroff',
'ref_frame': 'ref_frame',
'move_group_gantry': 'move_group_gantry',
'robot_name': 'robot_name',
'config_name_Left_Home': 'config_name_Left_Home',
'config_name_Right_Home': 'config_name_Right_Home',
'arm_idL': 'arm_idL',
'arm_idR': 'arm_idR',
'config_name_gantry_Pre_Conv_RA':
'config_name_gantry_Pre_Conv_RA',
'config_name_gantry_Pre_Conv_LA':
'config_name_gantry_Pre_Conv_LA',
'config_name_RA_pre': 'config_name_RA_pre',
'config_name_LA_pre': 'config_name_LA_pre',
'offset_var_left': 'offset_var_left',
'offset_var_right': 'offset_var_right',
'offset_gasket': 'offset_gasket',
'offset_piston': 'offset_piston'
})
# x:1314 y:156
OperatableStateMachine.add('Droppen',
self.use_behavior(DroppenSM, 'Droppen'),
transitions={
'finished': 'increase',
'failed': 'failed',
'end': 'finished'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'end': Autonomy.Inherit
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group_right_arm':
'move_group_right_arm',
'move_group_left_arm':
'move_group_left_arm',
'action_topic':
'action_topic',
'camera_frame':
'camera_frame',
'camera_topic':
'camera_topic',
'ref_frame':
'ref_frame',
'move_group_gantry':
'move_group_gantry',
'robot_name':
'robot_name',
'offset_var_left':
'offset_var_left',
'offset_var_right':
'offset_var_right',
'offset_gasket':
'offset_gasket',
'offset_piston':
'offset_piston',
'arm_idL':
'arm_idL',
'arm_idR':
'arm_idR',
'config_name_Right_Home':
'config_name_Right_Home',
'config_name_Left_Home':
'config_name_Left_Home',
'config_name_RA_pre':
'config_name_RA_pre',
'config_name_LA_pre':
'config_name_LA_pre'
})
return _state_machine
def create(self):
# x:509 y:35, x:204 y:373
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.order_id = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.old_order_id = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:65 y:95
OperatableStateMachine.add('start',
StartAssignment(),
transitions={'continue': 'getorder'},
autonomy={'continue': Autonomy.Off})
# x:243 y:92
OperatableStateMachine.add(
'getorder',
GetOrderState(),
transitions={'continue': 'compare_last_order'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:465 y:94
OperatableStateMachine.add('get_products_from_shipments',
GetProductsFromShipmentState(),
transitions={
'continue':
'get_part_from_products',
'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:689 y:90
OperatableStateMachine.add(
'get_part_from_products',
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:986 y:193
OperatableStateMachine.add('increment_product_index',
AddNumericState(),
transitions={'done': 'einde_products'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:978 y:283
OperatableStateMachine.add('einde_products',
EqualState(),
transitions={
'true': 'reset_product index',
'false': 'get_part_from_products'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:972 y:480
OperatableStateMachine.add('increment_shipment_index',
AddNumericState(),
transitions={'done': 'einde_shipments'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'one_value',
'result': 'shipment_index'
})
# x:971 y:579
OperatableStateMachine.add('einde_shipments',
EqualState(),
transitions={
'true': 'notify_shipment_ready',
'false':
'get_products_from_shipments'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:978 y:384
OperatableStateMachine.add(
'reset_product index',
ReplaceState(),
transitions={'done': 'increment_shipment_index'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'product_index'
})
# x:572 y:362
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(
notify_shipment_readySM,
'notify_shipment_ready'),
transitions={
'finished':
'get_products_from_shipments',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:959 y:86
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': 'increment_product_index',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'agv_id': 'agv_id',
'pose_on_agv': 'part_pose'
})
# x:266 y:2
OperatableStateMachine.add('compare_last_order',
EqualState(),
transitions={
'true': 'finished',
'false': 'rememberoldorder'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'order_id',
'value_b': 'old_order_id'
})
# x:584 y:4
OperatableStateMachine.add(
'rememberoldorder',
ReplaceState(),
transitions={'done': 'get_products_from_shipments'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'order_id',
'result': 'old_order_id'
})
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: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': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:838 y:39
OperatableStateMachine.add('EndAssigment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:286 y:41
OperatableStateMachine.add(
'GetOrder',
GetOrderState(),
transitions={'continue': 'get_shipments'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'OrderId',
'shipments': 'Shipments',
'number_of_shipments': 'NumberOfShipments'
})
# 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:642 y:425, x:506 y:337
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
output_keys=['part_type', 'agv_id', 'pose_on_agv'])
_state_machine.userdata.order_id = ' '
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.camera_ref_frame = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.camera_topic = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:59 y:52
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:225 y:52
OperatableStateMachine.add(
'GetOrder',
GetOrderState(),
transitions={'continue': 'GetProductsFromShipment'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:442 y:53
OperatableStateMachine.add('GetProductsFromShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'GetPartFromProduct',
'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:675 y:55
OperatableStateMachine.add(
'GetPartFromProduct',
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': 'pose_on_agv'
})
# x:958 y:176
OperatableStateMachine.add(
'IncrementProductIndex',
AddNumericState(),
transitions={'done': 'EindeProductenLijst'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:963 y:273
OperatableStateMachine.add('EindeProductenLijst',
EqualState(),
transitions={
'true': 'IncrementShipmentIndex',
'false': 'GetPartFromProduct'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:964 y:363
OperatableStateMachine.add('IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'EindeShipment'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'one_value',
'result': 'shipment_index'
})
# x:967 y:449
OperatableStateMachine.add('EindeShipment',
EqualState(),
transitions={
'true': 'GetOrder',
'false': 'GetProductsFromShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:917 y:45
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': 'IncrementProductIndex',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'agv_id': 'agv_id',
'pose_on_agv': 'pose_on_agv',
'camera_ref_frame': 'camera_ref_frame',
'camera_frame': 'camera_frame',
'camera_topic': 'camera_topic'
})
return _state_machine
def create(self):
# x:1751 y:131, x:910 y:358
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.shipments = []
_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.power = 0
_state_machine.userdata.config_name_PreHomeR = 'Rechts_PreDrop_AGV'
_state_machine.userdata.config_name_PreHomeL = 'Links_PreDrop_AGV'
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.move_group_L = 'Left_Arm'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.Old_Id = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:62 y:57
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'TurnOffBelt'},
autonomy={'continue': Autonomy.Off})
# x:958 y:51
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'Id_Check'},
autonomy={'continue': Autonomy.Off},
remapping={'order_id': 'OrderId', 'shipments': 'Shipments', 'number_of_shipments': 'NumberOfShipments'})
# x:1552 y:126
OperatableStateMachine.add('EndAssigment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:1317 y:273
OperatableStateMachine.add('GetShipments',
self.use_behavior(GetShipmentsSM, 'GetShipments'),
transitions={'finished': 'GetOrder', 'failed': 'EndAssigment'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'Shipments': 'Shipments', 'NumberOfShipments': 'NumberOfShipments'})
# x:249 y:56
OperatableStateMachine.add('TurnOffBelt',
SetConveyorbeltPowerState(),
transitions={'continue': 'PreHomeL', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'power'})
# x:514 y:36
OperatableStateMachine.add('PreHomeL',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PreHomeR', 'planning_failed': 'failed', 'control_failed': 'PreHomeR', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_PreHomeL', 'move_group': 'move_group_L', '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:739 y:53
OperatableStateMachine.add('PreHomeR',
SrdfStateToMoveitAriac(),
transitions={'reached': 'GetOrder', 'planning_failed': 'failed', 'control_failed': 'GetOrder', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_PreHomeR', 'move_group': 'move_group_R', '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:1318 y:161
OperatableStateMachine.add('Old_Id',
ReplaceState(),
transitions={'done': 'GetShipments'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'OrderId', 'result': 'Old_Id'})
# x:1314 y:38
OperatableStateMachine.add('Id_Check',
EqualState(),
transitions={'true': 'EndAssigment', 'false': 'Old_Id'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'OrderId', 'value_b': 'Old_Id'})
return _state_machine
def create(self):
# x:37 y:662, x:476 y:317
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.conveyor_belt_power = 100.0
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_6'
_state_machine.userdata.camera_frame = 'logical_camera_6_frame'
_state_machine.userdata.part = 'gasket_part'
_state_machine.userdata.config_name_bin6PreGrasp = 'bin6PreGrasp'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.part_offset = 0.1
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop'
_state_machine.userdata.agv_pose = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:71 y:109
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'MoveR1Home'},
autonomy={'continue': Autonomy.Off})
# x:182 y:656
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:233 y:110
OperatableStateMachine.add('MoveR1Home',
SrdfStateToMoveitAriac(),
transitions={'reached': 'DetectCameraPart', '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:233 y:8
OperatableStateMachine.add('WaitRetry1',
WaitState(wait_time=5),
transitions={'done': 'MoveR1Home'},
autonomy={'done': Autonomy.Off})
# x:438 y:112
OperatableStateMachine.add('DetectCameraPart',
DetectPartCameraAriacState(time_out=5),
transitions={'continue': 'MoveR1PreGrasp1', '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:641 y:111
OperatableStateMachine.add('MoveR1PreGrasp1',
SrdfStateToMoveitAriac(),
transitions={'reached': 'ComputePick', 'planning_failed': 'WaitRetry2', 'control_failed': 'WaitRetry2', '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_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:640 y:6
OperatableStateMachine.add('WaitRetry2',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreGrasp1'},
autonomy={'done': Autonomy.Off})
# x:840 y:113
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': 'MoveR1ToPick1', '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:1023 y:110
OperatableStateMachine.add('MoveR1ToPick1',
MoveitToJointsDynAriacState(),
transitions={'reached': 'WachtEven', 'planning_failed': 'WaitRetry3', 'control_failed': 'WaitRetry3'},
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:1215 y:107
OperatableStateMachine.add('WaitRetry3',
WaitState(wait_time=5),
transitions={'done': 'MoveR1ToPick1'},
autonomy={'done': Autonomy.Off})
# x:1022 y:200
OperatableStateMachine.add('WachtEven',
WaitState(wait_time=1),
transitions={'done': 'MoveR1PreGrasp2'},
autonomy={'done': Autonomy.Off})
# x:1017 y:280
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_bin6PreGrasp', '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:1215 y:278
OperatableStateMachine.add('WaitRetry4',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreGrasp2'},
autonomy={'done': Autonomy.Off})
# x:1005 y:369
OperatableStateMachine.add('MoveR1PreDrop',
SrdfStateToMoveitAriac(),
transitions={'reached': 'GetAgvPose', 'planning_failed': 'WaitRetry5', 'control_failed': 'WaitRetry5', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_tray1PreDrop', '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:1210 y:372
OperatableStateMachine.add('WaitRetry5',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreDrop'},
autonomy={'done': Autonomy.Off})
# x:1002 y:454
OperatableStateMachine.add('GetAgvPose',
GetObjectPoseState(object_frame='kit_tray_1', ref_frame='arm1_linear_arm_actuator'),
transitions={'continue': 'ComputeDrop', 'failed': 'ComputeDrop'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'pose': 'agv_pose'})
# x:998 y:592
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': 'MoveR1ToDrop'},
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': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:661 y:522
OperatableStateMachine.add('MoveR1ToDrop',
MoveitToJointsDynAriacState(),
transitions={'reached': 'notify_shipment_ready', 'planning_failed': 'WaitRetry6', 'control_failed': 'WaitRetry6'},
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:661 y:640
OperatableStateMachine.add('WaitRetry6',
WaitState(wait_time=5),
transitions={'done': 'MoveR1ToDrop'},
autonomy={'done': Autonomy.Off})
# x:418 y:581
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(notify_shipment_readySM, 'notify_shipment_ready'),
transitions={'finished': 'EndAssignment', 'failed': 'EndAssignment'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
# x:32 y:406, x:900 y:385
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.offset = ''
_state_machine.userdata.move_group = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.ref_frame = 'torso_base_main_joint'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_bins1'
_state_machine.userdata.camera_frame = 'logical_camera_bins1_frame'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.arm_id = 'links'
_state_machine.userdata.config_name_Gantry_Bins_RechtsBovenRechts = 'Gantry_Bins_RechtsBovenRechts'
_state_machine.userdata.config_name_Gantry_Bins_RechtsBovenLinks = 'Gantry_Bins_RechtsBovenLinks'
_state_machine.userdata.config_name_Gantry_Bins_LinksBovenRechts = 'Gantry_Bins_LinksBovenRechts'
_state_machine.userdata.config_name_Gantry_Bins_LinksBovenLinks = 'Gantry_Bins_LinksBovenLinks'
_state_machine.userdata.null = 0
_state_machine.userdata.config_name_Gantry_Transportband = 'Gantry_Transportband'
_state_machine.userdata.config_name_Gantry_AGV_Rechts_L = 'Gantry_AGV_Rechts_L'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:73 y:57
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'stop belt'},
autonomy={'continue': Autonomy.Off})
# x:582 y:39
OperatableStateMachine.add('Pregrasp_G',
SrdfStateToMoveitAriac(),
transitions={'reached': 'Pregrasp_G_2', 'planning_failed': 'failed', 'control_failed': 'Pregrasp_G_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_Gantry_Bins_RechtsBovenLinks', '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:261 y:30
OperatableStateMachine.add('stop belt',
SetConveyorbeltPowerState(),
transitions={'continue': 'Pregrasp_G', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'null'})
# x:819 y:34
OperatableStateMachine.add('Pregrasp_G_2',
SrdfStateToMoveitAriac(),
transitions={'reached': 'Pregrasp_G_3', 'planning_failed': 'failed', 'control_failed': 'Pregrasp_G_3', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Gantry_Bins_RechtsBovenRechts', '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:1030 y:31
OperatableStateMachine.add('Pregrasp_G_3',
SrdfStateToMoveitAriac(),
transitions={'reached': 'Pregrasp_G_4', 'planning_failed': 'failed', 'control_failed': 'Pregrasp_G_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_Gantry_Bins_LinksBovenRechts', '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:1253 y:26
OperatableStateMachine.add('Pregrasp_G_4',
SrdfStateToMoveitAriac(),
transitions={'reached': 'Pregrasp_G_5', 'planning_failed': 'failed', 'control_failed': 'Pregrasp_G_5', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Gantry_Bins_LinksBovenLinks', '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:1264 y:264
OperatableStateMachine.add('Pregrasp_G_5',
SrdfStateToMoveitAriac(),
transitions={'reached': 'Pregrasp_G_6', 'planning_failed': 'failed', 'control_failed': 'Pregrasp_G_6', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Gantry_Transportband', '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:1288 y:401
OperatableStateMachine.add('Pregrasp_G_6',
SrdfStateToMoveitAriac(),
transitions={'reached': 'finished', 'planning_failed': 'finished', '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_Gantry_AGV_Rechts_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'})
return _state_machine
def create(self):
# x:944 y:732, x:764 y:355
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type = 'gear_part_red'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.offset = 0.025
_state_machine.userdata.move_group_G = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_bins1'
_state_machine.userdata.camera_frame = 'logical_camera_bins1_frame'
_state_machine.userdata.tool_link_R = 'right_ee_link'
_state_machine.userdata.arm_id = 'Left_Arm'
_state_machine.userdata.config_name_Gantry_pick = 'Gantry_Bins_LinksBovenRechts'
_state_machine.userdata.null = 0
_state_machine.userdata.config_name_Gantry_Transportband = 'Gantry_Transportband'
_state_machine.userdata.config_name_Gantry_AGV = 'Gantry_AGV_Rechts_L'
_state_machine.userdata.config_name_Left_home = 'Left_Home'
_state_machine.userdata.config_name_Right_home = 'Right_Home'
_state_machine.userdata.move_group_L = 'Left_Arm'
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.config_name_L = 'Links_PreDrop_Stelling'
_state_machine.userdata.config_name_R = 'Rechts_PreDrop_Stelling'
_state_machine.userdata.tool_link_L = 'left_ee_link'
_state_machine.userdata.rotation = 0
_state_machine.userdata.camera_ref_frame = 'world'
_state_machine.userdata.config_name_PrePick = 'Links_PreGrasp_Bins'
_state_machine.userdata.config_name_PreDrop = 'Links_PreDrop_AGV'
_state_machine.userdata.pose = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:73 y:57
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'stop belt'},
autonomy={'continue': Autonomy.Off})
# x:1574 y:26
OperatableStateMachine.add('Pregrasp_G',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PrePick', 'planning_failed': 'failed', 'control_failed': 'PrePick', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Gantry_pick', '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:261 y:30
OperatableStateMachine.add('stop belt',
SetConveyorbeltPowerState(),
transitions={'continue': 'PreLeftHome', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'null'})
# x:988 y:18
OperatableStateMachine.add('LeftHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'RightHome', 'planning_failed': 'failed', 'control_failed': 'RightHome', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Left_home', 'move_group': 'move_group_L', '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:1259 y:20
OperatableStateMachine.add('RightHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'Pregrasp_G', 'planning_failed': 'failed', 'control_failed': 'Pregrasp_G', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Right_home', 'move_group': 'move_group_R', '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:522 y:12
OperatableStateMachine.add('PreLeftHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PreRightHome', 'planning_failed': 'failed', 'control_failed': 'PreRightHome', '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_L', '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:759 y:14
OperatableStateMachine.add('PreRightHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'LeftHome', 'planning_failed': 'failed', 'control_failed': 'LeftHome', '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_R', '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:1566 y:263
OperatableStateMachine.add('CheckPartPoseBin',
DetectPartCameraAriacState(time_out=2),
transitions={'continue': 'ComputePick', 'failed': 'failed', 'not_found': 'Retry'},
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': 'pose'})
# x:1579 y:349
OperatableStateMachine.add('ComputePick',
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': 'Pick', 'failed': 'Retry'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'move_group': 'move_group_L', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link_L', 'pose': 'pose', 'offset': 'offset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1326 y:337
OperatableStateMachine.add('Retry',
WaitState(wait_time=0.2),
transitions={'done': 'PrePick'},
autonomy={'done': Autonomy.Off})
# x:1579 y:157
OperatableStateMachine.add('PrePick',
SrdfStateToMoveitAriac(),
transitions={'reached': 'CheckPartPoseBin', 'planning_failed': 'CheckPartPoseBin', '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_PrePick', 'move_group': 'move_group_L', '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:1329 y:732
OperatableStateMachine.add('LeftHome_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_Left_home', 'move_group': 'move_group_L', '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:1602 y:444
OperatableStateMachine.add('Pick',
MoveitToJointsDynAriacState(),
transitions={'reached': 'GripperEnable', 'planning_failed': 'failed', 'control_failed': 'GripperEnable'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group_L', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1597 y:703
OperatableStateMachine.add('PrePick_2',
SrdfStateToMoveitAriac(),
transitions={'reached': 'LeftHome_2', 'planning_failed': 'failed', 'control_failed': 'LeftHome_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_PrePick', 'move_group': 'move_group_L', '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:1582 y:575
OperatableStateMachine.add('GripperEnable',
VacuumGripperControlState(enable=True),
transitions={'continue': 'PrePick_2', 'failed': 'Retry', '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: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.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.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.part = 'gasket_part'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:54 y:102
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'ConveyorBelt'},
autonomy={'continue': Autonomy.Off})
# x:197 y:603
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:654 y:113
OperatableStateMachine.add('MoveR1Home',
SrdfStateToMoveitAriac(),
transitions={'reached': 'DetectCameraPart', '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:666 y:12
OperatableStateMachine.add('WaitRetry1',
WaitState(wait_time=5),
transitions={'done': 'MoveR1Home'},
autonomy={'done': Autonomy.Off})
# x:836 y:111
OperatableStateMachine.add('DetectCameraPart',
DetectPartCameraAriacState(time_out=5.0),
transitions={'continue': 'MoveR1PreGrasp1', '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:1039 y:116
OperatableStateMachine.add('MoveR1PreGrasp1',
SrdfStateToMoveitAriac(),
transitions={'reached': 'ComputePick', 'planning_failed': 'WaitRetry2', 'control_failed': 'WaitRetry2', '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:1048 y:21
OperatableStateMachine.add('WaitRetry2',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreGrasp1'},
autonomy={'done': Autonomy.Off})
# x:1038 y:200
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': 'MoveR1ToPick1', '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:1069 y:379
OperatableStateMachine.add('Wait',
WaitState(wait_time=1),
transitions={'done': 'transport_part_from_bin_to_agv_1'},
autonomy={'done': Autonomy.Off})
# x:1303 y:280
OperatableStateMachine.add('WaitRetry3',
WaitState(wait_time=5),
transitions={'done': 'MoveR1ToPick1'},
autonomy={'done': Autonomy.Off})
# x:1035 y:284
OperatableStateMachine.add('MoveR1ToPick1',
MoveitToJointsDynAriacState(),
transitions={'reached': 'EnableGripper', 'planning_failed': 'WaitRetry3', 'control_failed': 'EnableGripper'},
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:818 y:282
OperatableStateMachine.add('EnableGripper',
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:294 y:484
OperatableStateMachine.add('DisableGripper',
VacuumGripperControlState(enable=False),
transitions={'continue': 'EndAssignment', 'failed': 'failed', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_id'})
# x:975 y:541
OperatableStateMachine.add('transport_part_from_bin_to_agv_1',
self.use_behavior(transport_part_from_bin_to_agv_1SM, 'transport_part_from_bin_to_agv_1'),
transitions={'finished': 'DisableGripper', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:222 y:106
OperatableStateMachine.add('ConveyorBelt',
SetConveyorbeltPowerState(),
transitions={'continue': 'MoveR1Home', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'conveyor_belt_power'})
return _state_machine
def create(self):
# x:845 y:63, x:414 y:193
_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.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.config_name_R2Bin4Pre = 'R2Bin4Pre'
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:52 y:43
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={'continue': 'arm_selector'},
autonomy={'continue': Autonomy.Off})
# x:625 y:49
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:326 y:46
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': 'EndAssignment',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'agv_id': 'agv_id',
'pose_on_agv': 'pose_on_agv',
'move_group_prefix': 'move_group_prefix'
})
# x:177 y:125
OperatableStateMachine.add(
'arm_selector',
self.use_behavior(arm_selectorSM, 'arm_selector'),
transitions={
'finished': 'transport_part_form_bin_to_agv_state',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'arm_id': 'arm_id',
'move_group_prefix': 'move_group_prefix'
})
return _state_machine
def create(self):
# x:1253 y:57, x:671 y:211
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.shipments = []
_state_machine.userdata.order_id = ''
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_index = 1
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 1
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.move_group = 'manipulator'
# 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': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:369 y:42
OperatableStateMachine.add('GetProductsFromShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'GetPartFromProducts',
'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:574 y:43
OperatableStateMachine.add(
'GetPartFromProducts',
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:780 y:44
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': 'EndAssignment',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'agv_id': 'agv_id',
'pose_on_agv': 'part_pose'
})
# x:1040 y:45
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:181 y:44
OperatableStateMachine.add(
'GetOrder',
GetOrderState(),
transitions={'continue': 'GetProductsFromShipment'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
return _state_machine
def create(self):
# x:70 y:501, x:515 y:301
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_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_1'
_state_machine.userdata.camera_frame = 'logical_camera_1_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
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:20 y:140
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={'continue': 'SetConveyorbeltPower'},
autonomy={'continue': Autonomy.Off})
# x:909 y:288
OperatableStateMachine.add('WachtEven',
WaitState(wait_time=1),
transitions={'done': 'MoveR1PreGrasp2'},
autonomy={'done': Autonomy.Off})
# x:746 y:427
OperatableStateMachine.add(
'GetAgvPose',
GetObjectPoseState(object_frame='kit_tray_1',
ref_frame='arm1_linear_arm_actuator'),
transitions={
'continue': 'ComuteDorp',
'failed': 'ComuteDorp'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_pose'})
# x:373 y:13
OperatableStateMachine.add('WaitRetry1',
WaitState(wait_time=5),
transitions={'done': 'MoveR1Home'},
autonomy={'done': Autonomy.Off})
# x:755 y:14
OperatableStateMachine.add('WaitRetry2',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreGrasp1'},
autonomy={'done': Autonomy.Off})
# x:1107 y:222
OperatableStateMachine.add('WaitRetry3',
WaitState(wait_time=5),
transitions={'done': 'MoveR1ToPick1'},
autonomy={'done': Autonomy.Off})
# x:1104 y:355
OperatableStateMachine.add('WaitRetry4',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreGrasp2'},
autonomy={'done': Autonomy.Off})
# x:383 y:523
OperatableStateMachine.add('WaitRetry6',
WaitState(wait_time=5),
transitions={'done': 'MoveR1ToDrop'},
autonomy={'done': Autonomy.Off})
# x:1104 y:430
OperatableStateMachine.add('WaitRetry5',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreDrop'},
autonomy={'done': Autonomy.Off})
# x:158 y:420
OperatableStateMachine.add('DeliverShipment',
self.use_behavior(
notify_shipment_readySM,
'DeliverShipment'),
transitions={
'finished': 'EndAssignment',
'failed': 'ComuteDorp'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:733 y:148
OperatableStateMachine.add('MoveR1PreGrasp1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComutePick',
'planning_failed': 'WaitRetry2',
'control_failed': 'WaitRetry2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin3PreGrasp',
'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:910 y:355
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_bin3PreGrasp',
'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:913 y:428
OperatableStateMachine.add('MoveR1PreDrop',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetAgvPose',
'planning_failed': 'WaitRetry5',
'control_failed': 'WaitRetry5',
'param_error': 'ComuteDorp'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_tray1PreDrop',
'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:908 y:220
OperatableStateMachine.add('MoveR1ToPick1',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'WachtEven',
'planning_failed': 'WaitRetry3',
'control_failed': 'WaitRetry3'
},
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:349 y:422
OperatableStateMachine.add('MoveR1ToDrop',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'DeliverShipment',
'planning_failed': 'WaitRetry6',
'control_failed': 'WaitRetry6'
},
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:526 y:146
OperatableStateMachine.add(
'DetectCameraPart',
DetectPartCameraAriacState(time_out=5.0),
transitions={
'continue': 'MoveR1PreGrasp1',
'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:908 y:148
OperatableStateMachine.add(
'ComutePick',
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': 'MoveR1ToPick1',
'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:539 y:424
OperatableStateMachine.add(
'ComuteDorp',
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': 'MoveR1ToDrop'
},
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': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:353 y:143
OperatableStateMachine.add('MoveR1Home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectCameraPart',
'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:28 y:422
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:165 y:144
OperatableStateMachine.add(
'SetConveyorbeltPower',
SetConveyorbeltPowerState(),
transitions={
'continue': 'MoveR1Home',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'conveyor_belt_power'})
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:1274 y:459, x:654 y:517
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.move_groupL = 'Left_Arm'
_state_machine.userdata.move_groupR = 'Right_Arm'
_state_machine.userdata.armidl = 'left'
_state_machine.userdata.armidr = 'right'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.config_namePBL = 'Left_Pre_Band'
_state_machine.userdata.config_namePBR = 'Right_Pre_Band'
_state_machine.userdata.Power = 100
_state_machine.userdata.NoPower = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:51 y:37
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'BeltOff'},
autonomy={'continue': Autonomy.Off})
# x:1464 y:55
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:1219 y:45
OperatableStateMachine.add('belt_to_bin',
self.use_behavior(belt_to_binSM, 'belt_to_bin'),
transitions={'finished': 'EndAssignment', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:616 y:39
OperatableStateMachine.add('RH',
SrdfStateToMoveitAriac(),
transitions={'reached': 'GripperUitR', '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_namePBR', 'move_group': 'move_groupR', '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:428 y:38
OperatableStateMachine.add('GripperUitL',
VacuumGripperControlState(enable=False),
transitions={'continue': 'RH', 'failed': 'failed', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'armidl'})
# x:791 y:39
OperatableStateMachine.add('GripperUitR',
VacuumGripperControlState(enable=False),
transitions={'continue': 'BeltOn', 'failed': 'failed', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'armidr'})
# x:225 y:36
OperatableStateMachine.add('LH',
SrdfStateToMoveitAriac(),
transitions={'reached': 'GripperUitL', '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_namePBL', 'move_group': 'move_groupL', '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:112 y:134
OperatableStateMachine.add('BeltOff',
SetConveyorbeltPowerState(),
transitions={'continue': 'LH', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'NoPower'})
# x:1011 y:39
OperatableStateMachine.add('BeltOn',
SetConveyorbeltPowerState(),
transitions={'continue': 'belt_to_bin', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'Power'})
return _state_machine
def create(self):
# x:328 y:654, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['part_type_R'])
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.pose = []
_state_machine.userdata.offset = 0.03
_state_machine.userdata.rotation = 0
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.tool_link = 'right_ee_link'
_state_machine.userdata.move_group = ''
_state_machine.userdata.config_name = 'Gantry_Pulley_R'
_state_machine.userdata.part_type_R = 'pulley_part_red'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('start',
StartAssignment(),
transitions={'continue': 'Find'},
autonomy={'continue': Autonomy.Off})
# x:285 y:343
OperatableStateMachine.add(
'Compute grasp',
ComputeGraspAriacState(joint_names=[
'right_elbow_joint', 'right_shoulder_lift_joint',
'right_shoulder_pan_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'move',
'failed': 'Compute grasp'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_R',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:279 y:232
OperatableStateMachine.add(
'cam',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Compute grasp',
'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_R',
'pose': 'pose'
})
# x:277 y:429
OperatableStateMachine.add('move',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'grip',
'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:274 y:503
OperatableStateMachine.add('grip',
GripperControl(enable=True),
transitions={
'continue': 'MOve_2_2',
'failed': 'move',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'move_group_R'})
# x:293 y:68
OperatableStateMachine.add('MOve_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'cam',
'planning_failed': 'failed',
'control_failed': 'MOve_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',
'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:469 y:553
OperatableStateMachine.add('MOve_2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'MOve_2_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',
'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:30 y:102
OperatableStateMachine.add('Find',
FindPart(time_out=0.2),
transitions={
'found': 'MOve_2',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'found': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'part_type': 'part_type_R',
'gantry_pos': 'gantry_pos',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame'
})
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:1227 y:622, x:267 y:397
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.powerOn = 100
_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 = ''
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.powerOff = 0
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_6'
_state_machine.userdata.camera_frame = 'logical_camera_6_frame'
_state_machine.userdata.pose = []
_state_machine.userdata.camera_topic_7 = '/ariac/logical_camera_7'
_state_machine.userdata.camera_frame_7 = 'logical_camera_7_frame'
_state_machine.userdata.part_offset = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link = 'right_ee_link'
_state_machine.userdata.config_name_pregrasp = 'beltPreGrasp'
_state_machine.userdata.part_type = ''
_state_machine.userdata.move_group = 'Right_Arm'
_state_machine.userdata.joint_names_r = ['right_shoulder_pan_joint', 'right_shoulder_lift_joint', 'right_elbow_joint', 'right_wrist_1_joint', 'right_wrist_2_joint', 'right_wrist_3_joint']
_state_machine.userdata.robot_name_g = ''
_state_machine.userdata.arm_id = 'Right_Arm'
_state_machine.userdata.config_name_r_home = 'Right_Home'
# 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': 'TurnOnConveyor'},
autonomy={'continue': Autonomy.Off})
# x:172 y:41
OperatableStateMachine.add('TurnOnConveyor',
SetConveyorbeltPowerState(),
transitions={'continue': 'move_home_belt', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'powerOn'})
# x:361 y:39
OperatableStateMachine.add('move_home_belt',
self.use_behavior(move_home_beltSM, 'move_home_belt'),
transitions={'finished': 'DetectFirstPartBelt', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:769 y:38
OperatableStateMachine.add('TurnOffConveyor',
SetConveyorbeltPowerState(),
transitions={'continue': 'DetectFirstPartBelt_2', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'powerOff'})
# x:1156 y:204
OperatableStateMachine.add('ComputeGrasp',
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': 'PickProduct', '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:470 y:134
OperatableStateMachine.add('WaitRetry',
WaitState(wait_time=0.5),
transitions={'done': 'DetectFirstPartBelt'},
autonomy={'done': Autonomy.Off})
# x:1143 y:288
OperatableStateMachine.add('PickProduct',
MoveitToJointsDynAriacState(),
transitions={'reached': 'EnableGripper', 'planning_failed': 'failed', 'control_failed': 'EnableGripper'},
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:561 y:38
OperatableStateMachine.add('DetectFirstPartBelt',
DetectFirstPartCameraAriacState(part_list=['piston_rod_part_red', 'gasket_part_blue_0', 'gasket_part_blue_1', 'gasket_part_blue_2'], time_out=0.5),
transitions={'continue': 'TurnOffConveyor', 'failed': 'WaitRetry', 'not_found': 'WaitRetry'},
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:958 y:36
OperatableStateMachine.add('DetectFirstPartBelt_2',
DetectFirstPartCameraAriacState(part_list=['piston_rod_part_red', 'gasket_part_blue_0', 'gasket_part_blue_1', 'gasket_part_blue_2'], time_out=0.5),
transitions={'continue': 'MovePreGraspBelt', '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:1164 y:36
OperatableStateMachine.add('MovePreGraspBelt',
SrdfStateToMoveitAriac(),
transitions={'reached': 'DecideOffset', '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_pregrasp', 'move_group': 'move_group_g', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name_g', '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:1167 y:449
OperatableStateMachine.add('MoveUp',
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_r_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:1186 y:368
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={'continue': 'MoveUp', 'failed': 'PickProduct', 'invalid_id': 'PickProduct'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_id': Autonomy.Off},
remapping={'arm_id': 'arm_id'})
# x:1177 y:119
OperatableStateMachine.add('DecideOffset',
DecideOffsetProduct(target_time=0.5),
transitions={'succes': 'ComputeGrasp', 'unknown_id': 'DetectFirstPartBelt_2'},
autonomy={'succes': Autonomy.Off, 'unknown_id': Autonomy.Off},
remapping={'part_type': 'part_type', 'part_offset': 'part_offset'})
return _state_machine
def create(self):
# x:370 y:633, x:752 y:439
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group = ''
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.pose = []
_state_machine.userdata.rotation = 0
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_belt'
_state_machine.userdata.camera_frame = 'logical_camera_belt_frame'
_state_machine.userdata.home = 'Home'
_state_machine.userdata.safe = 'Gantry_Bin'
_state_machine.userdata.Up = 0.2
_state_machine.userdata.power_off = 0
_state_machine.userdata.power_on = 100
_state_machine.userdata.move_group_gantry = 'Gantry'
_state_machine.userdata.move_group_left = 'Left_Arm'
_state_machine.userdata.move_group_right = 'Right_Arm'
_state_machine.userdata.voortest = 'voor test'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:204 y:28
OperatableStateMachine.add('Start_Assignment',
StartAssignment(),
transitions={'continue': 'Detect_Part'},
autonomy={'continue': Autonomy.Off})
# x:358 y:118
OperatableStateMachine.add('Stop_Belt',
SetConveyorbeltPowerState(),
transitions={
'continue': 'Move_R1_Home',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power_off'})
# x:597 y:131
OperatableStateMachine.add('Move_R1_Home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move_R2_Home',
'planning_failed': 'Move_R1_Home',
'control_failed': 'Move_R1_Home',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'home',
'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:797 y:132
OperatableStateMachine.add('Move_R2_Home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Pick_Red_Part',
'planning_failed': 'Move_R2_Home',
'control_failed': 'Move_R2_Home',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'home',
'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:358 y:34
OperatableStateMachine.add(
'Detect_Part',
DetectFirstPartCameraAriacState(
part_list=['gasket_part_blue', 'piston_rod_part_red'],
time_out=0.5),
transitions={
'continue': 'Stop_Belt',
'failed': 'Detect_Part',
'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',
'pose': 'pose'
})
# x:1000 y:122
OperatableStateMachine.add('Pick_Red_Part',
self.use_behavior(
Pick_Red_PartSM, 'Pick_Red_Part'),
transitions={
'finished': 'Place_Red_Part',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:1002 y:218
OperatableStateMachine.add('Place_Red_Part',
self.use_behavior(
Place_Red_PartSM, 'Place_Red_Part'),
transitions={
'finished': 'Start_Belt',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:352 y:212
OperatableStateMachine.add('Stop_Belt_2',
SetConveyorbeltPowerState(),
transitions={
'continue': 'Pick_Blue_Part_Def',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power_off'})
# x:568 y:222
OperatableStateMachine.add(
'Detect_Part_2',
DetectFirstPartCameraAriacState(
part_list=['gasket_part_blue', 'piston_rod_part_red'],
time_out=0.5),
transitions={
'continue': 'Stop_Belt_2',
'failed': 'Detect_Part_2',
'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',
'pose': 'pose'
})
# x:798 y:220
OperatableStateMachine.add('Start_Belt',
SetConveyorbeltPowerState(),
transitions={
'continue': 'Detect_Part_2',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power_on'})
# x:350 y:290
OperatableStateMachine.add('Start_Belt_2',
SetConveyorbeltPowerState(),
transitions={
'continue': 'Detect_Part',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power_on'})
# x:154 y:192
OperatableStateMachine.add('Pick_Blue_Part_Def',
self.use_behavior(
Pick_Blue_Part_DefSM,
'Pick_Blue_Part_Def'),
transitions={
'finished': 'Place_Blue_Part',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:150 y:290
OperatableStateMachine.add('Place_Blue_Part',
self.use_behavior(
Place_Blue_PartSM,
'Place_Blue_Part'),
transitions={
'finished': 'Start_Belt_2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
# x:144 y:698, x:147 y:461
_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_index = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.shipment_type = ''
# 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': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:190 y:41
OperatableStateMachine.add(
'GetOrder',
GetOrderState(),
transitions={'continue': 'GetProductsFromShipment'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:409 y:40
OperatableStateMachine.add('GetProductsFromShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'GetPartFromProduct',
'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:670 y:38
OperatableStateMachine.add(
'GetPartFromProduct',
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:1211 y:34
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'EndProducts'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:1213 y:164
OperatableStateMachine.add('EndProducts',
EqualState(),
transitions={
'true': 'ResetProductIndex',
'false': 'GetPartFromProduct'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:1214 y:282
OperatableStateMachine.add(
'ResetProductIndex',
ReplaceState(),
transitions={'done': 'IncrementShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'product_index'
})
# x:1214 y:413
OperatableStateMachine.add('IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'EindShipments'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'one_value',
'result': 'shipment_index'
})
# x:1215 y:541
OperatableStateMachine.add('EindShipments',
EqualState(),
transitions={
'true': 'ResetShipmentIndex',
'false': 'notify_shipment_ready'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:1000 y:541
OperatableStateMachine.add('ResetShipmentIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'shipment_index'
})
# x:1211 y:782
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:911 y:38
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': 'IncrementProductIndex',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine