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: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:537 y:364, x:433 y:362, x:430 y:252
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'no_products_belt'],
output_keys=['part_type', 'pose'])
_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.powerOff = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.retries = 0
_state_machine.userdata.plus = 1
_state_machine.userdata.zero = 0
_state_machine.userdata.condition = 15
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:77 y:33
OperatableStateMachine.add(
'ZeroTics',
ReplaceState(),
transitions={'done': 'DetectFirstProduct'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'retries'
})
# x:200 y:131
OperatableStateMachine.add('WaitRetry',
WaitState(wait_time=0.5),
transitions={'done': 'CountCameraTics'},
autonomy={'done': Autonomy.Off})
# x:505 y:34
OperatableStateMachine.add('TurnConveyorOff',
SetConveyorbeltPowerState(),
transitions={
'continue':
'DetectFirstProductRetry',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'powerOff'})
# x:708 y:56
OperatableStateMachine.add(
'DetectFirstProductRetry',
DetectFirstPartCameraAriacState(
part_list=['piston_rod_part_red', 'gasket_part_blue'],
time_out=0.5),
transitions={
'continue': 'finished',
'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:242 y:249
OperatableStateMachine.add('CountCameraTics',
AddNumericState(),
transitions={'done': 'CheckCondition'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'retries',
'value_b': 'plus',
'result': 'retries'
})
# x:276 y:34
OperatableStateMachine.add(
'DetectFirstProduct',
DetectFirstPartCameraAriacState(
part_list=['piston_rod_part_red', 'gasket_part_blue'],
time_out=0.5),
transitions={
'continue': 'TurnConveyorOff',
'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:336 y:118
OperatableStateMachine.add('CheckCondition',
EqualState(),
transitions={
'true': 'no_products_belt',
'false': 'DetectFirstProduct'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'retries',
'value_b': 'condition'
})
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: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: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: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: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: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: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:1189 y:652, x:1235 y:42
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['arm'])
_state_machine.userdata.arm = ''
_state_machine.userdata.power = 100
_state_machine.userdata.NoPower = 0
_state_machine.userdata.part1 = 'pulley_part'
_state_machine.userdata.ref_frame1 = 'arm1_linear_arm_actuator'
_state_machine.userdata.Camera1_topic = '/ariac/Camera_Converyor_Links'
_state_machine.userdata.camera_frame1 = 'Camera_Converyor_Links_frame'
_state_machine.userdata.part3 = 'piston_rod_part'
_state_machine.userdata.Camera2_topic = '/ariac/Camera_Converyor_Rechts'
_state_machine.userdata.ref_frame2 = 'arm2_linear_arm_actuator'
_state_machine.userdata.camera_frame2 = 'Camera_Converyor_Rechts_frame'
_state_machine.userdata.part_type = ''
_state_machine.userdata.part2 = 'gasket_part'
_state_machine.userdata.part4 = 'disk_part'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:131 y:27
OperatableStateMachine.add('StartConveryor',
SetConveyorbeltPowerState(),
transitions={'continue': 'WAIT_check', 'fail': 'WAIT_failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'power'})
# x:343 y:118
OperatableStateMachine.add('Stopconveyor',
SetConveyorbeltPowerState(),
transitions={'continue': 'WAIT_completed', 'fail': 'WAIT_failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'NoPower'})
# x:1053 y:645
OperatableStateMachine.add('WAIT_completed',
WaitState(wait_time=5),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:128 y:104
OperatableStateMachine.add('CheckPart1',
DetectPartCameraAriacState(time_out=2),
transitions={'continue': 'Stopconveyor', 'failed': 'WAIT_failed', 'not_found': 'CheckPart2'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'ref_frame1', 'camera_topic': 'Camera1_topic', 'camera_frame': 'camera_frame1', 'part': 'part1', 'pose': 'pose'})
# x:339 y:267
OperatableStateMachine.add('StartConveryor_2',
SetConveyorbeltPowerState(),
transitions={'continue': 'WWait_check2', 'fail': 'WAIT_failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'power'})
# x:678 y:403
OperatableStateMachine.add('Stopconveyor_2',
SetConveyorbeltPowerState(),
transitions={'continue': 'WAIT_completed', 'fail': 'WAIT_failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'NoPower'})
# x:896 y:33
OperatableStateMachine.add('WAIT_failed',
WaitState(wait_time=5),
transitions={'done': 'Stopconveyor_3'},
autonomy={'done': Autonomy.Off})
# x:3 y:88
OperatableStateMachine.add('WAIT_check',
WaitState(wait_time=2),
transitions={'done': 'CheckPart1'},
autonomy={'done': Autonomy.Off})
# x:130 y:166
OperatableStateMachine.add('CheckPart2',
DetectPartCameraAriacState(time_out=2),
transitions={'continue': 'Stopconveyor', 'failed': 'WAIT_failed', 'not_found': 'CheckPart3'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'ref_frame1', 'camera_topic': 'Camera1_topic', 'camera_frame': 'camera_frame1', 'part': 'part2', 'pose': 'pose'})
# x:130 y:228
OperatableStateMachine.add('CheckPart3',
DetectPartCameraAriacState(time_out=2),
transitions={'continue': 'StartConveryor_2', 'failed': 'WAIT_failed', 'not_found': 'CheckPart4'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'ref_frame1', 'camera_topic': 'Camera1_topic', 'camera_frame': 'camera_frame1', 'part': 'part3', 'pose': 'pose'})
# x:130 y:305
OperatableStateMachine.add('CheckPart4',
DetectPartCameraAriacState(time_out=2),
transitions={'continue': 'StartConveryor_2', 'failed': 'WAIT_failed', 'not_found': 'WAIT_check'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'ref_frame1', 'camera_topic': 'Camera1_topic', 'camera_frame': 'camera_frame1', 'part': 'part2', 'pose': 'pose'})
# x:341 y:390
OperatableStateMachine.add('CheckPart3_2',
DetectPartCameraAriacState(time_out=2),
transitions={'continue': 'Stopconveyor_2', 'failed': 'WAIT_failed', 'not_found': 'CheckPart4_2'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'ref_frame1', 'camera_topic': 'Camera1_topic', 'camera_frame': 'camera_frame1', 'part': 'part3', 'pose': 'pose'})
# x:341 y:479
OperatableStateMachine.add('CheckPart4_2',
DetectPartCameraAriacState(time_out=2),
transitions={'continue': 'Stopconveyor_2', 'failed': 'WAIT_failed', 'not_found': 'WWait_check2'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'ref_frame1', 'camera_topic': 'Camera1_topic', 'camera_frame': 'camera_frame1', 'part': 'part4', 'pose': 'pose'})
# x:342 y:330
OperatableStateMachine.add('WWait_check2',
WaitState(wait_time=2),
transitions={'done': 'CheckPart3_2'},
autonomy={'done': Autonomy.Off})
# x:1021 y:34
OperatableStateMachine.add('Stopconveyor_3',
SetConveyorbeltPowerState(),
transitions={'continue': 'failed', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'NoPower'})
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: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: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: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
def create(self):
# x:1630 y:562, x:1479 y:95
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['arm'])
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.power = 100
_state_machine.userdata.NoPower = 0
_state_machine.userdata.part = ''
_state_machine.userdata.ref_frame1 = 'arm1_linear_arm_actuator'
_state_machine.userdata.Camera1_topic = '/ariac/Camera_Converyor_Links'
_state_machine.userdata.camera_frame1 = 'Camera_Converyor_Links_frame'
_state_machine.userdata.part_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.arm = ''
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.Disk_rotation = 0
_state_machine.userdata.Disk_offset = 0.035
_state_machine.userdata.config_name_R1PreBin3 = 'R1PreBin3'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.config_name_R1PreConveyor = 'R1PreConveyor'
_state_machine.userdata.Round_Nr = 1
_state_machine.userdata.Round_target = 6
_state_machine.userdata.Zero = 0
_state_machine.userdata.ONE = 1
_state_machine.userdata.SideYoffset = 0
_state_machine.userdata.SideXoffset = 0
_state_machine.userdata.PlusOffset = 0.20
_state_machine.userdata.config_name_R1HOME = 'R1Home'
_state_machine.userdata.Row_count = 3
_state_machine.userdata.offset = 0.040
_state_machine.userdata.rotation = 0
_state_machine.userdata.BIN3_pose = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:49 y:25
OperatableStateMachine.add('MoveToHOME',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GripperUIT_2',
'planning_failed': 'RetryHOME',
'control_failed': 'RetryHOME',
'param_error': 'WAIT_failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1HOME',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1262 y:559
OperatableStateMachine.add(
'WAIT_completed',
WaitState(wait_time=2),
transitions={'done': 'SluitConveyorAF_2'},
autonomy={'done': Autonomy.Off})
# x:202 y:91
OperatableStateMachine.add('Stopconveyor_2',
SetConveyorbeltPowerState(),
transitions={
'continue': 'MoveToPreConveyor',
'fail': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'NoPower'})
# x:1106 y:35
OperatableStateMachine.add('WAIT_failed',
WaitState(wait_time=5),
transitions={'done': 'SluitConveyorAF'},
autonomy={'done': Autonomy.Off})
# x:563 y:30
OperatableStateMachine.add('WAIT_check',
WaitState(wait_time=1),
transitions={'done': 'CheckforDisk'},
autonomy={'done': Autonomy.Off})
# x:372 y:91
OperatableStateMachine.add(
'CheckforDisk',
DetectFirstPartCameraAriacState(part_list=['disk_part'],
time_out=2),
transitions={
'continue': 'Stopconveyor_2',
'failed': 'WAIT_failed',
'not_found': 'WAIT_check'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame1',
'camera_topic': 'Camera1_topic',
'camera_frame': 'camera_frame1',
'part': 'part',
'pose': 'pose'
})
# x:200 y:162
OperatableStateMachine.add(
'Computebeltpick',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'MoveToConveyorPart',
'failed': 'WAIT_failed_pick'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'Disk_offset',
'rotation': 'Disk_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:371 y:166
OperatableStateMachine.add('MoveToConveyorPart',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'WAIT_gripperoke',
'planning_failed':
'WAIT_failed_pick',
'control_failed': 'WAIT_failed_drop'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1106 y:97
OperatableStateMachine.add('WAIT_failed_pick',
WaitState(wait_time=5),
transitions={'done': 'SluitConveyorAF'},
autonomy={'done': Autonomy.Off})
# x:201 y:299
OperatableStateMachine.add('MoveToBin3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetBin3Pose',
'planning_failed': 'RetryBin3',
'control_failed': 'RetryBin3',
'param_error': 'WAIT_failed_pick'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R1PreBin3',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:51 y:163
OperatableStateMachine.add('Gripperaan',
UseGripper(enable=True),
transitions={
'continue': 'Computebeltpick',
'failed': 'WAIT_failed_pick',
'invalid_arm': 'WAIT_failed_pick'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:370 y:243
OperatableStateMachine.add(
'WAIT_gripperoke',
WaitState(wait_time=0.5),
transitions={'done': 'MoveToPreConveyor_2'},
autonomy={'done': Autonomy.Off})
# x:51 y:96
OperatableStateMachine.add(
'MoveToPreConveyor',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Gripperaan',
'planning_failed': 'RetryPreConveyor',
'control_failed': 'RetryPreConveyor',
'param_error': 'WAIT_failed_pick'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1PreConveyor',
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out': 'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out': 'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:22 y:707
OperatableStateMachine.add(
'RetryPreConveyor',
WaitState(wait_time=1),
transitions={'done': 'MoveToPreConveyor'},
autonomy={'done': Autonomy.Off})
# x:261 y:768
OperatableStateMachine.add('RetryBin3',
WaitState(wait_time=1),
transitions={'done': 'MoveToBin3'},
autonomy={'done': Autonomy.Off})
# x:200 y:231
OperatableStateMachine.add(
'MoveToPreConveyor_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveToBin3',
'planning_failed': 'RetryPreConveyor_2',
'control_failed': 'RetryPreConveyor_2',
'param_error': 'WAIT_failed_pick'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1PreConveyor',
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out': 'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out': 'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:20 y:769
OperatableStateMachine.add(
'RetryPreConveyor_2',
WaitState(wait_time=1),
transitions={'done': 'MoveToPreConveyor_2'},
autonomy={'done': Autonomy.Off})
# x:356 y:566
OperatableStateMachine.add('GripperUIT',
UseGripper(enable=False),
transitions={
'continue': 'MoveToBin3_2',
'failed': 'WAIT_failed_pick',
'invalid_arm': 'WAIT_failed_pick'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1033 y:518
OperatableStateMachine.add('AddRound',
AddNumericState(),
transitions={'done': 'CheckRound'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'Round_Nr',
'value_b': 'ONE',
'result': 'Round_Nr'
})
# x:1032 y:584
OperatableStateMachine.add('CheckRound',
EqualState(),
transitions={
'true': 'ResetRound',
'false': 'StartConveryor'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'Round_Nr',
'value_b': 'Round_target'
})
# x:1031 y:654
OperatableStateMachine.add('ResetRound',
ReplaceState(),
transitions={'done': 'WAIT_completed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Round_Nr',
'result': 'Zero'
})
# x:360 y:382
OperatableStateMachine.add(
'ComputeDrop',
ComputeBeltDrop(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'ShowDropPose',
'failed': 'WAIT_failed_drop'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'BIN3_pose',
'offset': 'offset',
'SideYoffset': 'SideYoffset',
'SideXoffset': 'SideXoffset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:356 y:507
OperatableStateMachine.add('MoveToDROP',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperUIT',
'planning_failed': 'RetryDROP',
'control_failed': 'RetryDROP'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1106 y:159
OperatableStateMachine.add('WAIT_failed_drop',
WaitState(wait_time=5),
transitions={'done': 'SluitConveyorAF'},
autonomy={'done': Autonomy.Off})
# x:360 y:768
OperatableStateMachine.add('RetryDROP',
WaitState(wait_time=1),
transitions={'done': 'MoveToDROP'},
autonomy={'done': Autonomy.Off})
# x:743 y:522
OperatableStateMachine.add('AddYOffset',
AddNumericState(),
transitions={'done': 'CheckRow'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'SideYoffset',
'value_b': 'PlusOffset',
'result': 'SideYoffset'
})
# x:742 y:579
OperatableStateMachine.add('CheckRow',
EqualState(),
transitions={
'true': 'ResetRow',
'false': 'AddRound'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'Row_count',
'value_b': 'Round_Nr'
})
# x:741 y:647
OperatableStateMachine.add('ResetRow',
ReplaceState(),
transitions={'done': 'AddXOffset'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Zero',
'result': 'SideYoffset'
})
# x:739 y:711
OperatableStateMachine.add('AddXOffset',
AddNumericState(),
transitions={'done': 'AddRound'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'SideXoffset',
'value_b': 'PlusOffset',
'result': 'SideXoffset'
})
# x:156 y:768
OperatableStateMachine.add(
'RetryDropBin',
WaitState(wait_time=1),
transitions={'done': 'MoveToConveyorPart'},
autonomy={'done': Autonomy.Off})
# x:22 y:645
OperatableStateMachine.add('RetryHOME',
WaitState(wait_time=1),
transitions={'done': 'MoveToHOME'},
autonomy={'done': Autonomy.Off})
# x:371 y:28
OperatableStateMachine.add('StartConveryor',
SetConveyorbeltPowerState(),
transitions={
'continue': 'WAIT_check',
'fail': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power'})
# x:203 y:26
OperatableStateMachine.add('GripperUIT_2',
UseGripper(enable=False),
transitions={
'continue': 'StartConveryor',
'failed': 'WAIT_failed',
'invalid_arm': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1300 y:89
OperatableStateMachine.add('SluitConveyorAF',
SetConveyorbeltPowerState(),
transitions={
'continue': 'failed',
'fail': 'WAIT_failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'NoPower'})
# x:1396 y:560
OperatableStateMachine.add('SluitConveyorAF_2',
SetConveyorbeltPowerState(),
transitions={
'continue': 'finished',
'fail': 'WAIT_completed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'NoPower'})
# x:368 y:311
OperatableStateMachine.add(
'GetBin3Pose',
GetObjectPoseState(object_frame='kit_tray_2',
ref_frame='arm1_linear_arm_actuator'),
transitions={
'continue': 'ComputeDrop',
'failed': 'WAIT_failed_drop'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'BIN3_pose'})
# x:361 y:443
OperatableStateMachine.add('ShowDropPose',
MessageState(),
transitions={'continue': 'MoveToDROP'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'BIN3_pose'})
# x:536 y:562
OperatableStateMachine.add(
'MoveToBin3_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AddYOffset',
'planning_failed': 'WAIT_failed_drop',
'control_failed': 'WAIT_failed_drop',
'param_error': 'WAIT_failed_drop'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_R1PreBin3',
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out': 'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out': 'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
return _state_machine