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: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:598 y:381, x:114 y:451
_state_machine = OperatableStateMachine(outcomes=['failed', 'finish'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.powerOff = 0
_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.config_name_left = 'Left_Home'
_state_machine.userdata.move_group_right = 'Right_Arm'
_state_machine.userdata.move_group_left = 'Left_Arm'
_state_machine.userdata.config_name_grasp_left = 'beltPreGrasp2'
_state_machine.userdata.part_type_left = ''
_state_machine.userdata.part_type_right = ''
_state_machine.userdata.config_name_place = 'gantryPosPlace'
_state_machine.userdata.config_name_right_left = 'gantryPosPlaceRightLeft'
# 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': 'StartAssignment',
'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',
'no_products_belt': 'EndAssignment'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'no_products_belt': 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': 'BeltPreGraspLeft',
'planning_failed': 'failed',
'control_failed':
'BeltPreGraspLeft',
'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:1158 y:124
OperatableStateMachine.add(
'BeltPreGraspLeft',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ConveyorPowerOnRepeat',
'planning_failed': 'failed',
'control_failed': 'ConveyorPowerOnRepeat',
'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_left',
'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:1137 y:215
OperatableStateMachine.add('ConveyorPowerOnRepeat',
SetConveyorbeltPowerState(),
transitions={
'continue':
'detect_product_belt_repeat',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'powerOn'})
# x:1105 y:301
OperatableStateMachine.add('detect_product_belt_repeat',
self.use_behavior(
detect_product_beltSM,
'detect_product_belt_repeat'),
transitions={
'finished':
'move_to_part_belt_left_arm',
'failed':
'failed',
'no_products_belt':
'move_to_part_belt_left_arm'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'no_products_belt': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'pose': 'pose'
})
# x:39 y:40
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={'continue': 'ConveyorPowerOn'},
autonomy={'continue': Autonomy.Off})
# x:1106 y:393
OperatableStateMachine.add('move_to_part_belt_left_arm',
self.use_behavior(
move_to_part_belt_left_armSM,
'move_to_part_belt_left_arm'),
transitions={
'finished':
'MoveLeftSafety',
'failed':
'failed',
'unkown_id':
'detect_product_belt_repeat'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'unkown_id': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'pose': 'pose',
'joint_values': 'joint_values',
'part_type_left': 'part_type_left'
})
# x:1159 y:478
OperatableStateMachine.add('MoveLeftSafety',
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_left',
'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:1160 y:565
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:980 y:621
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:721 y:648
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
})
# x:274 y:435
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finish'},
autonomy={'continue': Autonomy.Off})
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: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: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: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:1680 y:94, x:415 y:533
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.move_groupR = '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.ARM_idMAIN = ''
_state_machine.userdata.move_groupL = 'Left_Arm'
_state_machine.userdata.tool_link_right = 'right_ee_link'
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.joint_names = ''
_state_machine.userdata.joint_values = ''
_state_machine.userdata.ConveyorPartoffset = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.arm_idR = 'right_arm'
_state_machine.userdata.arm_idL = 'left_arm'
_state_machine.userdata.Ja_value = 'Ja'
_state_machine.userdata.Nee_value = 'Nee'
_state_machine.userdata.BothArmsFull = 'Nee'
_state_machine.userdata.config_nameLeftPregrasp = 'Left_PreGrasp'
_state_machine.userdata.config_nameRightPregrasp = 'Right_PreGrasp'
_state_machine.userdata.config_nameGantryROT = 'Gantry_SafeHall1'
_state_machine.userdata.move_groupGantry = 'Gantry'
_state_machine.userdata.Round_nrMAIN = 0
_state_machine.userdata.RoundTargetMAIN = 4
_state_machine.userdata.PartTYPE = ''
_state_machine.userdata.RightArmItem = ''
_state_machine.userdata.LeftArmItem = ''
_state_machine.userdata.ONE = 1
_state_machine.userdata.USEarmID = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Startass',
StartAssignment(),
transitions={'continue': 'ADDround'},
autonomy={'continue': Autonomy.Off})
# x:562 y:111
OperatableStateMachine.add('Conveyor_State',
self.use_behavior(
Conveyor_StateSM, 'Conveyor_State'),
transitions={
'finished': 'BeltPickUP',
'failed': 'WaitFailed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'ARMidMAIN': 'ARMidMAIN',
'PartPose': 'PartPose',
'PartOffset': 'PartOffset',
'PartTYPE': 'PartTYPE',
'exactparttype': 'exactparttype'
})
# x:1542 y:89
OperatableStateMachine.add('stopass',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:349 y:41
OperatableStateMachine.add('Armattachedchecker',
self.use_behavior(
ArmattachedcheckerSM,
'Armattachedchecker'),
transitions={
'finished': 'BothFULL?',
'failed': 'WaitFailed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'ARMidMAIN': 'ARMidMAIN',
'BothArmsFull': 'BothArmsFull',
'BothArmsEMPTY': 'BothArmsEMPTY'
})
# x:561 y:45
OperatableStateMachine.add('BothFULL?',
EqualState(),
transitions={
'true': 'ToRotationStation',
'false': 'Conveyor_State'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'BothArmsFull',
'value_b': 'Ja_value'
})
# x:179 y:41
OperatableStateMachine.add('StartUP',
self.use_behavior(StartUPSM, 'StartUP'),
transitions={
'finished': 'Armattachedchecker',
'failed': 'WaitFailed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:560 y:198
OperatableStateMachine.add('BeltPickUP',
self.use_behavior(
BeltPickUPSM, 'BeltPickUP'),
transitions={
'finished': 'WaitNewRound',
'failed': 'WaitFailed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'ARMidMAIN': 'ARMidMAIN',
'PartPose': 'PartPose',
'PartOffset': 'PartOffset',
'exactparttype': 'exactparttype',
'PartTYPE': 'PartTYPE',
'RightArmItem': 'RightArmItem',
'LeftArmItem': 'LeftArmItem'
})
# x:757 y:46
OperatableStateMachine.add(
'ToRotationStation',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GoToPreDrop',
'planning_failed': 'TransfertoDropFailed',
'control_failed': 'TransfertoDropFailed',
'param_error': 'TransfertoDropFailed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_nameGantryROT',
'move_group': 'move_groupGantry',
'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:203 y:204
OperatableStateMachine.add('WaitNewRound',
WaitState(wait_time=0.25),
transitions={'done': 'StartUP'},
autonomy={'done': Autonomy.Off})
# x:376 y:394
OperatableStateMachine.add('WaitFailed',
WaitState(wait_time=2),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:749 y:354
OperatableStateMachine.add('TransfertoDropFailed',
WaitState(wait_time=2),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:934 y:41
OperatableStateMachine.add('GoToPreDrop',
self.use_behavior(
GoToPreDropSM, 'GoToPreDrop'),
transitions={
'finished': 'DropItemsInBIN',
'failed': 'TransfertoDropFailed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'RightArmItem': 'RightArmItem',
'LeftArmItem': 'LeftArmItem',
'GantryLocation': 'GantryLocation',
'USEarmID': 'USEarmID'
})
# x:1323 y:153
OperatableStateMachine.add('RoundCheck',
EqualState(),
transitions={
'true': 'stopass',
'false': 'ADDround'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'Round_nrMAIN',
'value_b': 'RoundTargetMAIN'
})
# x:21 y:348
OperatableStateMachine.add('ADDround',
AddNumericState(),
transitions={'done': 'StartUP'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'Round_nrMAIN',
'value_b': 'ONE',
'result': 'Round_nrMAIN'
})
# x:1116 y:42
OperatableStateMachine.add('DropItemsInBIN',
self.use_behavior(
DropItemsInBINSM, 'DropItemsInBIN'),
transitions={
'Next arm': 'GoToPreDrop',
'failed': 'TransfertoDropFailed',
'BothArmsEmpty': 'RoundCheck',
'BinVol': 'LogBinVol'
},
autonomy={
'Next arm': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'BothArmsEmpty': Autonomy.Inherit,
'BinVol': Autonomy.Inherit
},
remapping={
'USEarmID': 'USEarmID',
'GantryLocation': 'GantryLocation',
'RightArmItem': 'RightArmItem',
'LeftArmItem': 'LeftArmItem'
})
# x:1323 y:46
OperatableStateMachine.add('LogBinVol',
LogState(text="Een van de bins is vol",
severity=Logger.REPORT_HINT),
transitions={'done': 'stopass'},
autonomy={'done': Autonomy.Off})
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: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: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: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:52 y:611, x:343 y:359
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.agv_id = 'agv1'
_state_machine.userdata.part_type = 'gear_part'
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.pose = []
_state_machine.userdata.part_pose = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.part = 'gasket_part'
_state_machine.userdata.offset = 0.1
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.config_name_bin3PreGrasp = 'bin3PreGrasp'
_state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop'
_state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_5'
_state_machine.userdata.camera_frame = 'logical_camera_5_frame'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.agv_pose = []
_state_machine.userdata.part_offset = 0.035
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.conveyor_belt_power = 100.0
_state_machine.userdata.config_name_bin1PreGrasp = 'bin1PreGrasp'
# 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': 'transport_part_form_bin_to_agv_state'
},
autonomy={'continue': Autonomy.Off})
# x:197 y:603
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:189 y:100
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': 'SetConveyorbeltPower',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'agv_id': 'agv_id',
'pose_on_agv': 'pose_on_agv'
})
# 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),
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:456 y:106
OperatableStateMachine.add(
'SetConveyorbeltPower',
SetConveyorbeltPowerState(),
transitions={
'continue': 'MoveR1Home',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'conveyor_belt_power'})
# 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': 'MoveR1PreGrasp2'},
autonomy={'done': Autonomy.Off})
# x:1041 y:504
OperatableStateMachine.add('MoveR1PreGrasp2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveR1PreDrop',
'planning_failed': 'WaitRetry4',
'control_failed': 'WaitRetry4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1303 y:280
OperatableStateMachine.add('WaitRetry3',
WaitState(wait_time=5),
transitions={'done': 'MoveR1ToPick1'},
autonomy={'done': Autonomy.Off})
# x:1226 y:501
OperatableStateMachine.add('WaitRetry4',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreGrasp2'},
autonomy={'done': Autonomy.Off})
# x:876 y:608
OperatableStateMachine.add(
'GetAgvPose',
GetObjectPoseState(object_frame='kit_tray_1',
ref_frame='arm1_linear_arm_actuator'),
transitions={
'continue': 'ComputeDrop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_pose'})
# x:1220 y:618
OperatableStateMachine.add('WaitRerty5',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreDrop'},
autonomy={'done': Autonomy.Off})
# x:688 y:604
OperatableStateMachine.add(
'ComputeDrop',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'MoveR1ToDrop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'agv_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:430 y:601
OperatableStateMachine.add('MoveR1ToDrop',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'EndAssignment',
'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:451 y:726
OperatableStateMachine.add('WaitRetry6',
WaitState(wait_time=5),
transitions={'done': 'MoveR1ToDrop'},
autonomy={'done': Autonomy.Off})
# x:1035 y:284
OperatableStateMachine.add('MoveR1ToPick1',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Wait',
'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:1042 y:611
OperatableStateMachine.add('MoveR1PreDrop',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetAgvPose',
'planning_failed': 'WaitRerty5',
'control_failed': 'WaitRerty5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_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'
})
return _state_machine
