def create(self):
# x:625 y:34, x:36 y:176, x:34 y:79, x:622 y:154
_state_machine = OperatableStateMachine(outcomes=['to_agv', 'failed', 'part_not_in_bin', 'transfer'], input_keys=['move_group_prefix', 'part_type', 'agv_id'], output_keys=['bin', 'move_group_prefix', 'camera_topic', 'camera_frame', 'ref_frame'])
_state_machine.userdata.part_type = []
_state_machine.userdata.agv_id = []
_state_machine.userdata.bin = ''
_state_machine.userdata.move_group_prefix = []
_state_machine.userdata.arm1 = '/ariac/arm1'
_state_machine.userdata.arm2 = '/ariac/arm2'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.ref_frame = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:157 y:25
OperatableStateMachine.add('FindPart',
FindPart(time_out=0.5),
transitions={'in_range': 'to_agv', 'out_of_range': 'Arm1Active?', 'failed': 'failed', 'not_found': 'part_not_in_bin'},
autonomy={'in_range': Autonomy.Off, 'out_of_range': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'part_type': 'part_type', 'agv_id': 'agv_id', 'bin': 'bin', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'ref_frame': 'ref_frame'})
# x:145 y:154
OperatableStateMachine.add('Arm1Active?',
EqualState(),
transitions={'true': 'SwitchArmTo2', 'false': 'SwitchArmTo1'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'arm1', 'value_b': 'move_group_prefix'})
# x:342 y:108
OperatableStateMachine.add('SwitchArmTo1',
ReplaceState(),
transitions={'done': 'transfer'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'arm1', 'result': 'move_group_prefix'})
# x:342 y:185
OperatableStateMachine.add('SwitchArmTo2',
ReplaceState(),
transitions={'done': 'transfer'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'arm2', 'result': 'move_group_prefix'})
return _state_machine
def create(self):
# x:983 y:56, x:356 y:305, x:268 y:204
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'unkown_id'], input_keys=['part_type', 'pose', 'joint_values'], output_keys=['part_type_left'])
_state_machine.userdata.part_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.move_group = 'Left_Arm'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.tool_link = 'left_ee_link'
_state_machine.userdata.part_offset = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.arm_id = 'Left_Arm'
_state_machine.userdata.part_type_left = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:70 y:44
OperatableStateMachine.add('DecideOffset',
DecideOffsetProduct(target_time=0.5),
transitions={'succes': 'ComputePick', 'unknown_id': 'unkown_id'},
autonomy={'succes': Autonomy.Off, 'unknown_id': Autonomy.Off},
remapping={'part_type': 'part_type', 'part_offset': 'part_offset'})
# x:427 y:38
OperatableStateMachine.add('MoveToPartBelt',
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:629 y:37
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={'continue': 'ReplacePartName', 'failed': 'failed', 'invalid_id': 'unkown_id'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_id': Autonomy.Off},
remapping={'arm_id': 'arm_id'})
# x:237 y:40
OperatableStateMachine.add('ComputePick',
ComputeGraspAriacState(joint_names=['left_elbow_joint', 'left_shoulder_lift_joint', 'left_shoulder_pan_joint', 'left_wrist_1_joint', 'left_wrist_2_joint', 'left_wrist_2_joint']),
transitions={'continue': 'MoveToPartBelt', '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:779 y:36
OperatableStateMachine.add('ReplacePartName',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'part_type', 'result': 'part_type_left'})
return _state_machine
def create(self):
# x:501 y:113, x:89 y:300
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['arm_id'], output_keys=['move_group_prefix'])
_state_machine.userdata.prefix_arm1 = 'ariac/arm1'
_state_machine.userdata.prefix_arm2 = 'ariac/arm2'
_state_machine.userdata.arm1 = 'arm1'
_state_machine.userdata.arm2 = 'arm2'
_state_machine.userdata.arm_id = ''
_state_machine.userdata.move_group_prefix = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:41 y:41
OperatableStateMachine.add('RobotEqualState',
EqualState(),
transitions={'true': 'RobotValueState', 'false': 'RobotEqualState_2'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'arm_id', 'value_b': 'arm1'})
# x:239 y:40
OperatableStateMachine.add('RobotValueState',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'prefix_arm1', 'result': 'move_group_prefix'})
# x:41 y:161
OperatableStateMachine.add('RobotEqualState_2',
EqualState(),
transitions={'true': 'RobotValueState_2', 'false': 'failed'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'arm_id', 'value_b': 'arm2'})
# x:242 y:160
OperatableStateMachine.add('RobotValueState_2',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'prefix_arm2', 'result': 'move_group_prefix'})
return _state_machine
def create(self):
# x:1250 y:199, x:781 y:695
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['bin_id', 'part_type', 'arm_id_left', 'part_pose'],
output_keys=['part_pose_left'])
_state_machine.userdata.bin_id = ''
_state_machine.userdata.bin14 = 'bin14'
_state_machine.userdata.bin3 = 'bin3'
_state_machine.userdata.shelf6 = 'shelf6'
_state_machine.userdata.shelf3 = 'shelf3'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_topic_bin14 = '/ariac/logical_camera_2'
_state_machine.userdata.camera_topic_bin3 = '/ariac/logical_camera_1'
_state_machine.userdata.camera_topic_shelf6 = '/ariac/logical_camera_6'
_state_machine.userdata.camera_topic_shelf3 = '/ariac/logical_camera_5'
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.camera_frame_bin14 = 'logical_camera_2_frame'
_state_machine.userdata.camera_frame_bin3 = 'logical_camera_1_frame'
_state_machine.userdata.camera_frame_shelf6 = 'logical_camera_6_frame'
_state_machine.userdata.camera_frame_shelf3 = 'logical_camera_5_frame'
_state_machine.userdata.offset_part_left = 0
_state_machine.userdata.offset_pulley_red = 0.081
_state_machine.userdata.offset_gear_red = 0.027
_state_machine.userdata.offset_gear_blue = 0.027
_state_machine.userdata.offset_gasket_red = 0.035
_state_machine.userdata.PreGrasp = ''
_state_machine.userdata.PreGrasp_bin14 = 'Gantry_PreGrasp_Left_bins_LA'
_state_machine.userdata.PreGrasp_bin3 = 'Gantry_PreGrasp_Right_bins_LA'
_state_machine.userdata.PreGrasp_shelf6 = 'Gantry_PreGrasp_shelves_RAR'
_state_machine.userdata.PreGrasp_shelf3 = 'Gantry_PreGrasp_shelves_LAR'
_state_machine.userdata.part_type = ''
_state_machine.userdata.arm_id_left = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.part_pose_left = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Binkeuze1',
EqualState(),
transitions={
'true': 'Camera_topic1',
'false': 'Binkeuze2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'bin14'
})
# x:30 y:132
OperatableStateMachine.add('Binkeuze2',
EqualState(),
transitions={
'true': 'Camera_topic2',
'false': 'Shelfkeuze1'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'bin3'
})
# x:30 y:224
OperatableStateMachine.add('Shelfkeuze1',
EqualState(),
transitions={
'true': 'Camera_topic3',
'false': 'Shelfkeuze2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'shelf6'
})
# x:30 y:316
OperatableStateMachine.add('Shelfkeuze2',
EqualState(),
transitions={
'true': 'Camera_topic4',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'shelf3'
})
# x:203 y:37
OperatableStateMachine.add('Camera_topic1',
ReplaceState(),
transitions={'done': 'Camera_frame1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_bin14',
'result': 'camera_topic'
})
# x:201 y:134
OperatableStateMachine.add('Camera_topic2',
ReplaceState(),
transitions={'done': 'Camera_frame2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_bin3',
'result': 'camera_topic'
})
# x:202 y:225
OperatableStateMachine.add('Camera_topic3',
ReplaceState(),
transitions={'done': 'Camera_frame3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_shelf6',
'result': 'camera_topic'
})
# x:202 y:316
OperatableStateMachine.add('Camera_topic4',
ReplaceState(),
transitions={'done': 'Camera_frame4'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_shelf3',
'result': 'camera_topic'
})
# x:369 y:39
OperatableStateMachine.add('Camera_frame1',
ReplaceState(),
transitions={'done': 'Offset1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_bin14',
'result': 'camera_frame'
})
# x:370 y:136
OperatableStateMachine.add('Camera_frame2',
ReplaceState(),
transitions={'done': 'Offset2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_bin3',
'result': 'camera_frame'
})
# x:370 y:226
OperatableStateMachine.add('Camera_frame3',
ReplaceState(),
transitions={'done': 'Offset3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_shelf6',
'result': 'camera_frame'
})
# x:371 y:317
OperatableStateMachine.add('Camera_frame4',
ReplaceState(),
transitions={'done': 'Offset4'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_shelf3',
'result': 'camera_frame'
})
# x:539 y:39
OperatableStateMachine.add('Offset1',
ReplaceState(),
transitions={'done': 'PreGrasp1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_gear_red',
'result': 'offset_part_left'
})
# x:539 y:135
OperatableStateMachine.add('Offset2',
ReplaceState(),
transitions={'done': 'PreGrasp2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_pulley_red',
'result': 'offset_part_left'
})
# x:539 y:226
OperatableStateMachine.add('Offset3',
ReplaceState(),
transitions={'done': 'PreGrasp3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_gasket_red',
'result': 'offset_part_left'
})
# x:539 y:318
OperatableStateMachine.add('Offset4',
ReplaceState(),
transitions={'done': 'PreGrasp4'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_gear_blue',
'result': 'offset_part_left'
})
# x:710 y:40
OperatableStateMachine.add('PreGrasp1',
ReplaceState(),
transitions={'done': 'ProgramBinsLeft'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGrasp_bin14',
'result': 'PreGrasp'
})
# x:709 y:135
OperatableStateMachine.add('PreGrasp2',
ReplaceState(),
transitions={'done': 'ProgramBinsLeft'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGrasp_bin3',
'result': 'PreGrasp'
})
# x:710 y:227
OperatableStateMachine.add(
'PreGrasp3',
ReplaceState(),
transitions={'done': 'ProgramShelvesLeft'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGrasp_shelf6',
'result': 'PreGrasp'
})
# x:709 y:317
OperatableStateMachine.add(
'PreGrasp4',
ReplaceState(),
transitions={'done': 'ProgramShelvesLeft'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGrasp_shelf3',
'result': 'PreGrasp'
})
# x:940 y:90
OperatableStateMachine.add(
'ProgramBinsLeft',
self.use_behavior(ProgramBinsLeftSM, 'ProgramBinsLeft'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'offset_part_left': 'offset_part_left',
'PreGrasp': 'PreGrasp',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'offset_part_left': 'offset_part_left',
'part_type': 'part_type',
'arm_id_left': 'arm_id_left',
'part_pose': 'part_pose',
'part_pose_left': 'part_pose_left'
})
# x:938 y:262
OperatableStateMachine.add(
'ProgramShelvesLeft',
self.use_behavior(ProgramShelvesLeftSM, 'ProgramShelvesLeft'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'offset_part_left': 'offset_part_left',
'PreGrasp': 'PreGrasp',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'offset_part_left': 'offset_part_left',
'part_type': 'part_type',
'arm_id_left': 'arm_id_left',
'part_pose': 'part_pose',
'part_pose_left': 'part_pose_left'
})
return _state_machine
def create(self):
# x:811 y:574, x:283 y:395
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'move_group_prefix', 'part_type', 'bin', 'camera_topic',
'camera_frame', 'agv_id', 'ref_frame'
])
_state_machine.userdata.agv_id = []
_state_machine.userdata.part_type = []
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.bin = []
_state_machine.userdata.camera_topic = []
_state_machine.userdata.camera_frame = []
_state_machine.userdata.ref_frame = []
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.arm_id = ''
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.pose = []
_state_machine.userdata.part_offset = 0.08
_state_machine.userdata.rotation = 0
_state_machine.userdata.agv1 = 'agv1'
_state_machine.userdata.bin3 = 'transferBin3'
_state_machine.userdata.bin4 = 'transferBin4'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('MoveToHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ArmId',
'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_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:306 y:28
OperatableStateMachine.add('MoveToBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectPartPose',
'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': 'bin',
'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:453 y:26
OperatableStateMachine.add(
'DetectPartPose',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'ComputeGrasp',
'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:659 y:29
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_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:177 y:27
OperatableStateMachine.add('PartOffset',
part_offsetCalc(),
transitions={
'succes': 'MoveToBin',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'part_offset': 'part_offset'
})
# x:825 y:30
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_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1033 y:60
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={
'continue': 'agv1?',
'failed': 'MoveToPart',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1012 y:345
OperatableStateMachine.add('MoveToTransferBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DisableGripper',
'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': 'bin',
'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:1018 y:130
OperatableStateMachine.add('agv1?',
EqualState(),
transitions={
'true': 'bin4',
'false': 'bin3'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv1',
'value_b': 'agv_id'
})
# x:880 y:207
OperatableStateMachine.add('bin3',
ReplaceState(),
transitions={'done': 'MoveToHome_3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin3',
'result': 'bin'
})
# x:880 y:273
OperatableStateMachine.add('bin4',
ReplaceState(),
transitions={'done': 'MoveToHome_3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin4',
'result': 'bin'
})
# x:1034 y:438
OperatableStateMachine.add('DisableGripper',
GripperControl(enable=False),
transitions={
'continue': 'MoveToHome_2',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:942 y:514
OperatableStateMachine.add('MoveToHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_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:1074 y:235
OperatableStateMachine.add('MoveToHome_3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveToTransferBin',
'planning_failed': 'failed',
'control_failed': 'MoveToHome_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_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:30 y:102
OperatableStateMachine.add('ArmId',
chooseArmID(),
transitions={
'continue': 'PartOffset',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'arm_id': 'arm_id'
})
return _state_machine
def create(self):
# x:1770 y:298, x:81 y:372
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'part_type', 'agv_id', 'pose_on_agv', 'camera_ref_frame',
'camera_frame', 'camera_topic'
],
output_keys=['part_type', 'camera_ref_frame', 'camera_frame'])
_state_machine.userdata.part_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.move_group_prefix1 = '/ariac/arm1'
_state_machine.userdata.move_group_prefix2 = '/ariac/arm2'
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.part_pose = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.config_name_bin1PreGrasp = 'bin1PreGrasp'
_state_machine.userdata.camera_ref_frame2 = 'arm2_linear_arm_actuator'
_state_machine.userdata.camera_ref_frame1 = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_frame2 = 'Camera_bin_2_frame'
_state_machine.userdata.camera_frame5 = 'Camera_bin_5_frame'
_state_machine.userdata.camera_frame6 = 'Camera_bin_6_frame'
_state_machine.userdata.config_name_bin2PreGrasp = 'bin2PreGrasp'
_state_machine.userdata.config_name_bin5PreGrasp = 'bin5PreGrasp'
_state_machine.userdata.camera_ref_frame = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.camera_frame1 = 'Camera_bin_1_frame'
_state_machine.userdata.config_name_bin6PreGrasp = 'bin6PreGrasp'
_state_machine.userdata.joint_names = ''
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.gripper1 = 'arm1'
_state_machine.userdata.gripper2 = 'arm2'
_state_machine.userdata.camera_topic = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:71 y:54
OperatableStateMachine.add('binSelectie',
self.use_behavior(
binSelectieSM, 'binSelectie'),
transitions={
'finished': 'arm1',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'camera_topic': 'camera_topic',
'camera_ref_frame':
'camera_ref_frame',
'camera_frame': 'camera_frame',
'part_offset': 'part_offset'
})
# x:1675 y:697
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'MoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:1675 y:758
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
# x:262 y:58
OperatableStateMachine.add('arm1',
EqualState(),
transitions={
'true': 'bin5',
'false': 'arm2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_ref_frame',
'value_b': 'camera_ref_frame1'
})
# x:266 y:389
OperatableStateMachine.add('arm2',
EqualState(),
transitions={
'true': 'bin1',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_ref_frame',
'value_b': 'camera_ref_frame2'
})
# x:470 y:353
OperatableStateMachine.add('bin1',
EqualState(),
transitions={
'true': 'pregraspbin1',
'false': 'bin2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame1'
})
# x:475 y:527
OperatableStateMachine.add('bin2',
EqualState(),
transitions={
'true': 'pregraspbin2',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame2'
})
# x:467 y:59
OperatableStateMachine.add('bin5',
EqualState(),
transitions={
'true': 'pregraspbin5',
'false': 'bin6'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame5'
})
# x:470 y:277
OperatableStateMachine.add('bin6',
EqualState(),
transitions={
'true': 'pregraspbin6',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame6'
})
# x:663 y:58
OperatableStateMachine.add('pregraspbin5',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin5',
'planning_failed': 'failed',
'control_failed': 'detectpartbin5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin5PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:652 y:273
OperatableStateMachine.add('pregraspbin6',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin6',
'planning_failed': 'failed',
'control_failed': 'detectpartbin6',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin6PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:653 y:351
OperatableStateMachine.add('pregraspbin1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin1',
'planning_failed': 'failed',
'control_failed': 'detectpartbin1',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:657 y:531
OperatableStateMachine.add('pregraspbin2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin2',
'planning_failed': 'failed',
'control_failed': 'detectpartbin2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin2PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1164 y:44
OperatableStateMachine.add('pickbin5',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'gripperkeuze',
'planning_failed': 'failed',
'control_failed': 'Retry'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix1',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1173 y:269
OperatableStateMachine.add('pickbin6',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripperaan_2',
'planning_failed': 'failed',
'control_failed': 'Retry_2'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix1',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1171 y:353
OperatableStateMachine.add('pickbin1',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripperaan_4',
'planning_failed': 'failed',
'control_failed': 'Retry_3'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix2',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1185 y:524
OperatableStateMachine.add('pickbin2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripperaan_3',
'planning_failed': 'failed',
'control_failed': 'Retry_4'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix2',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1568 y:43
OperatableStateMachine.add('pregraspbin5_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin5PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1568 y:262
OperatableStateMachine.add('pregraspbin6_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin5PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1566 y:349
OperatableStateMachine.add('pregraspbin1_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1569 y:515
OperatableStateMachine.add('pregraspbin2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin2PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1674 y:635
OperatableStateMachine.add('MoseMessage',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_pose'})
# x:818 y:46
OperatableStateMachine.add(
'detectpartbin5',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:1193 y:136
OperatableStateMachine.add('Retry',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze'},
autonomy={'done': Autonomy.Off})
# x:820 y:265
OperatableStateMachine.add(
'detectpartbin6',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5_2',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:820 y:348
OperatableStateMachine.add(
'detectpartbin1',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5_3',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:833 y:530
OperatableStateMachine.add(
'detectpartbin2',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5_4',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:997 y:44
OperatableStateMachine.add(
'Computegraspbin5',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1382 y:37
OperatableStateMachine.add('Gripperaan',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin5_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1363 y:129
OperatableStateMachine.add('gripperkeuze',
ReplaceState(),
transitions={'done': 'Gripperaan'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper1',
'result': 'arm_id'
})
# x:1000 y:254
OperatableStateMachine.add(
'Computegraspbin5_2',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1003 y:346
OperatableStateMachine.add(
'Computegraspbin5_3',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5_3',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1023 y:524
OperatableStateMachine.add(
'Computegraspbin5_4',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5_4',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1193 y:198
OperatableStateMachine.add('Retry_2',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze_2'},
autonomy={'done': Autonomy.Off})
# x:1206 y:434
OperatableStateMachine.add('Retry_3',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze_3'},
autonomy={'done': Autonomy.Off})
# x:1200 y:628
OperatableStateMachine.add('Retry_4',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze_4'},
autonomy={'done': Autonomy.Off})
# x:1362 y:199
OperatableStateMachine.add('gripperkeuze_2',
ReplaceState(),
transitions={'done': 'Gripperaan_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper1',
'result': 'arm_id'
})
# x:1359 y:426
OperatableStateMachine.add('gripperkeuze_3',
ReplaceState(),
transitions={'done': 'Gripperaan_4'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper2',
'result': 'arm_id'
})
# x:1357 y:642
OperatableStateMachine.add('gripperkeuze_4',
ReplaceState(),
transitions={'done': 'Gripperaan_3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper2',
'result': 'arm_id'
})
# x:1359 y:261
OperatableStateMachine.add('Gripperaan_2',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin6_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1377 y:511
OperatableStateMachine.add('Gripperaan_3',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin2_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1362 y:345
OperatableStateMachine.add('Gripperaan_4',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin1_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
return _state_machine
def create(self):
# x:1374 y:114, x:1202 y:412
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'ARMidMAIN', 'PartPose', 'PartOffset', 'exactparttype',
'PartTYPE'
],
output_keys=['RightArmItem', 'LeftArmItem'])
_state_machine.userdata.ARMidMAIN = ''
_state_machine.userdata.arm_idR = 'right_arm'
_state_machine.userdata.move_groupR = 'Right_Arm'
_state_machine.userdata.move_groupL = 'Left_Arm'
_state_machine.userdata.tool_linkR = 'right_ee_link'
_state_machine.userdata.tool_linkL = 'left_ee_link'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.PartOffset = 0
_state_machine.userdata.PartPose = []
_state_machine.userdata.rotation = 0
_state_machine.userdata.arm_idL = 'left_arm'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.exactparttype = ''
_state_machine.userdata.RightArmItem = ''
_state_machine.userdata.LeftArmItem = ''
_state_machine.userdata.PartTYPE = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:232 y:59
OperatableStateMachine.add('UseRightArm?',
EqualState(),
transitions={
'true':
'ComputeBeltPickForRightArm',
'false': 'ComputeBeltPickForLeftArm'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'ARMidMAIN',
'value_b': 'arm_idR'
})
# x:484 y:63
OperatableStateMachine.add(
'ComputeBeltPickForRightArm',
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': 'GoToPickRight',
'failed': 'WaitFailed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_groupR',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_linkR',
'pose': 'PartPose',
'offset': 'PartOffset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:484 y:155
OperatableStateMachine.add(
'ComputeBeltPickForLeftArm',
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': 'GoToPickLeft',
'failed': 'WaitFailed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_groupL',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_linkL',
'pose': 'PartPose',
'offset': 'PartOffset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:701 y:57
OperatableStateMachine.add('GoToPickRight',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'TurnGripperONRight',
'planning_failed':
'TurnGripperONRight',
'control_failed':
'TurnGripperONRight'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_groupR',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:701 y:164
OperatableStateMachine.add('GoToPickLeft',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'TurnGripperONLeft',
'planning_failed':
'TurnGripperONLeft',
'control_failed':
'TurnGripperONLeft'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_groupL',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:874 y:58
OperatableStateMachine.add('TurnGripperONRight',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'SetRightArmItem',
'failed':
'ComputeBeltPickForRightArm',
'invalid_arm_id': 'WaitFailed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idR'})
# x:874 y:165
OperatableStateMachine.add('TurnGripperONLeft',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'SetLeftArmItem',
'failed':
'ComputeBeltPickForLeftArm',
'invalid_arm_id': 'WaitFailed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idL'})
# x:848 y:478
OperatableStateMachine.add('WaitFailed',
WaitState(wait_time=2),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:1045 y:58
OperatableStateMachine.add('SetRightArmItem',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PartTYPE',
'result': 'RightArmItem'
})
# x:1041 y:163
OperatableStateMachine.add('SetLeftArmItem',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PartTYPE',
'result': 'LeftArmItem'
})
return _state_machine
def create(self):
# x:509 y:35, x:204 y:373
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.order_id = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.old_order_id = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:65 y:95
OperatableStateMachine.add('start',
StartAssignment(),
transitions={'continue': 'getorder'},
autonomy={'continue': Autonomy.Off})
# x:243 y:92
OperatableStateMachine.add(
'getorder',
GetOrderState(),
transitions={'continue': 'compare_last_order'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:465 y:94
OperatableStateMachine.add('get_products_from_shipments',
GetProductsFromShipmentState(),
transitions={
'continue':
'get_part_from_products',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipment_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:689 y:90
OperatableStateMachine.add(
'get_part_from_products',
GetPartFromProductsState(),
transitions={
'continue': 'transport_part_form_bin_to_agv_state',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'product_index',
'type': 'part_type',
'pose': 'part_pose'
})
# x:986 y:193
OperatableStateMachine.add('increment_product_index',
AddNumericState(),
transitions={'done': 'einde_products'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:978 y:283
OperatableStateMachine.add('einde_products',
EqualState(),
transitions={
'true': 'reset_product index',
'false': 'get_part_from_products'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:972 y:480
OperatableStateMachine.add('increment_shipment_index',
AddNumericState(),
transitions={'done': 'einde_shipments'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'one_value',
'result': 'shipment_index'
})
# x:971 y:579
OperatableStateMachine.add('einde_shipments',
EqualState(),
transitions={
'true': 'notify_shipment_ready',
'false':
'get_products_from_shipments'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:978 y:384
OperatableStateMachine.add(
'reset_product index',
ReplaceState(),
transitions={'done': 'increment_shipment_index'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'product_index'
})
# x:572 y:362
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(
notify_shipment_readySM,
'notify_shipment_ready'),
transitions={
'finished':
'get_products_from_shipments',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:959 y:86
OperatableStateMachine.add(
'transport_part_form_bin_to_agv_state',
self.use_behavior(transport_part_form_bin_to_agv_stateSM,
'transport_part_form_bin_to_agv_state'),
transitions={
'finished': 'increment_product_index',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'agv_id': 'agv_id',
'pose_on_agv': 'part_pose'
})
# x:266 y:2
OperatableStateMachine.add('compare_last_order',
EqualState(),
transitions={
'true': 'finished',
'false': 'rememberoldorder'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'order_id',
'value_b': 'old_order_id'
})
# x:584 y:4
OperatableStateMachine.add(
'rememberoldorder',
ReplaceState(),
transitions={'done': 'get_products_from_shipments'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'order_id',
'result': 'old_order_id'
})
return _state_machine
def create(self):
# x:1233 y:271, x:524 y:568
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['agv_id', 'part_type', 'part_pose', 'bin_id'])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = ''
_state_machine.userdata.bin_id = ''
_state_machine.userdata.bin_gear_red = 'bin14'
_state_machine.userdata.bin_pully_red = 'bin3'
_state_machine.userdata.shelf_gasket_red = 'shelf6'
_state_machine.userdata.shelf_gear_blue = 'shelf3'
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.camera_frame_gear_red = 'logical_camera_BinGroup1_frame'
_state_machine.userdata.camera_frame_pully_red = 'logical_camera_BinGroup3_frame'
_state_machine.userdata.camera_frame_gear_blue = 'logical_camera_Shelf2_frame'
_state_machine.userdata.camera_frame_gasket_red = 'logical_camera_Shelf1_frame'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_topic_gear_red = '/ariac/logical_camera_BinGroup1'
_state_machine.userdata.camera_topic_pully_red = '/ariac/logical_camera_BinGroup3'
_state_machine.userdata.camera_topic_gear_blue = '/ariac/logical_camera_Shelf2'
_state_machine.userdata.camera_topic_gasket_red = '/ariac/logical_camera_Shelf1'
_state_machine.userdata.PreGraspGantry = ''
_state_machine.userdata.PreGraspGantry_GearRed = 'Gantry_Bin3_4_inverse'
_state_machine.userdata.PreGraspGantry_PullyRed = 'Gantry_Bin11_12_inverse'
_state_machine.userdata.offset = 0
_state_machine.userdata.offsetPully = 0.085
_state_machine.userdata.offsetGear = 0.025
_state_machine.userdata.offsetGasket = 0.035
_state_machine.userdata.PreGraspGantry_GasketRed = 'Gantry_ShelfLeftRed'
_state_machine.userdata.PreGraspGantry_GearBlue = 'Gantry_ShelfLeftBlue'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
# x:188 y:36
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'PoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:346 y:36
OperatableStateMachine.add(
'PoseMessage',
MessageState(),
transitions={'continue': 'BinIdMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_pose'})
# x:492 y:38
OperatableStateMachine.add(
'BinIdMessage',
MessageState(),
transitions={'continue': 'Selector_BinGearRed'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'bin_id'})
# x:24 y:124
OperatableStateMachine.add('Selector_BinGearRed',
EqualState(),
transitions={
'true': 'CameraFrameGearRed',
'false': 'Selector_BinPullyRed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'bin_gear_red'
})
# x:24 y:239
OperatableStateMachine.add('Selector_BinPullyRed',
EqualState(),
transitions={
'true': 'CameraFramePullyRed',
'false': 'Selector_ShelfGasketRed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'bin_pully_red'
})
# x:24 y:345
OperatableStateMachine.add('Selector_ShelfGasketRed',
EqualState(),
transitions={
'true': 'CameraFrameGasketRed',
'false': 'Selector_ShelfGearBlue'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'shelf_gasket_red'
})
# x:23 y:448
OperatableStateMachine.add('Selector_ShelfGearBlue',
EqualState(),
transitions={
'true': 'CameraFrameGearBlue',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin_id',
'value_b': 'shelf_gear_blue'
})
# x:231 y:124
OperatableStateMachine.add(
'CameraFrameGearRed',
ReplaceState(),
transitions={'done': 'CameraTopicGearRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_gear_red',
'result': 'camera_frame'
})
# x:230 y:228
OperatableStateMachine.add(
'CameraFramePullyRed',
ReplaceState(),
transitions={'done': 'CameraTopicPullyRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_pully_red',
'result': 'camera_frame'
})
# x:230 y:331
OperatableStateMachine.add(
'CameraFrameGasketRed',
ReplaceState(),
transitions={'done': 'CameraTopicGasketRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_gasket_red',
'result': 'camera_frame'
})
# x:232 y:431
OperatableStateMachine.add(
'CameraFrameGearBlue',
ReplaceState(),
transitions={'done': 'CameraTopicGearBlue'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame_gear_blue',
'result': 'camera_frame'
})
# x:601 y:128
OperatableStateMachine.add('PreGraspLeftGantry',
ReplaceState(),
transitions={'done': 'OffsetGearRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGraspGantry_GearRed',
'result': 'PreGraspGantry'
})
# x:598 y:228
OperatableStateMachine.add('PreGraspLeftGantry_2',
ReplaceState(),
transitions={'done': 'OffsetPullyRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGraspGantry_PullyRed',
'result': 'PreGraspGantry'
})
# x:413 y:125
OperatableStateMachine.add(
'CameraTopicGearRed',
ReplaceState(),
transitions={'done': 'PreGraspLeftGantry'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_gear_red',
'result': 'camera_topic'
})
# x:413 y:226
OperatableStateMachine.add(
'CameraTopicPullyRed',
ReplaceState(),
transitions={'done': 'PreGraspLeftGantry_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_pully_red',
'result': 'camera_topic'
})
# x:420 y:336
OperatableStateMachine.add(
'CameraTopicGasketRed',
ReplaceState(),
transitions={'done': 'PreGraspLeftGantry_3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_gasket_red',
'result': 'camera_topic'
})
# x:422 y:434
OperatableStateMachine.add(
'CameraTopicGearBlue',
ReplaceState(),
transitions={'done': 'PreGraspLeftGantry_4'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic_gear_blue',
'result': 'camera_topic'
})
# x:795 y:128
OperatableStateMachine.add('OffsetGearRed',
ReplaceState(),
transitions={'done': 'Path_Left_Bins'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetGear',
'result': 'offset'
})
# x:788 y:228
OperatableStateMachine.add('OffsetPullyRed',
ReplaceState(),
transitions={'done': 'Path_Left_Bins'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetPully',
'result': 'offset'
})
# x:805 y:333
OperatableStateMachine.add(
'OffsetGasketRed',
ReplaceState(),
transitions={'done': 'Path_Shelves_Left'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetGasket',
'result': 'offset'
})
# x:808 y:436
OperatableStateMachine.add(
'OffsetGearBlue',
ReplaceState(),
transitions={'done': 'Path_Shelves_Left'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetGear',
'result': 'offset'
})
# x:1001 y:175
OperatableStateMachine.add('Path_Left_Bins',
self.use_behavior(
Path_Left_BinsSM, 'Path_Left_Bins'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part_type': 'part_type',
'PreGraspGantry': 'PreGraspGantry',
'offset': 'offset'
})
# x:1000 y:355
OperatableStateMachine.add('Path_Shelves_Left',
self.use_behavior(
Path_Shelves_LeftSM,
'Path_Shelves_Left'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part_type': 'part_type',
'PreGraspGantry': 'PreGraspGantry',
'offset': 'offset'
})
# x:606 y:329
OperatableStateMachine.add('PreGraspLeftGantry_3',
ReplaceState(),
transitions={'done': 'OffsetGasketRed'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGraspGantry_GasketRed',
'result': 'PreGraspGantry'
})
# x:604 y:424
OperatableStateMachine.add('PreGraspLeftGantry_4',
ReplaceState(),
transitions={'done': 'OffsetGearBlue'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PreGraspGantry_GearBlue',
'result': 'PreGraspGantry'
})
return _state_machine
def create(self):
# x: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:667 y:540, x:729 y:205
_state_machine = OperatableStateMachine(outcomes=['finished', 'fail'])
_state_machine.userdata.shipments = []
_state_machine.userdata.part_type = ''
_state_machine.userdata.material_locations = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.order_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.product_index = 0
_state_machine.userdata.shipment_index = 1
_state_machine.userdata.add_one = 1
_state_machine.userdata.part_pose = []
_state_machine.userdata.zero = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Start',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:905 y:34
OperatableStateMachine.add('GetProductPose',
GetPartFromProductsState(),
transitions={
'continue': 'message_3',
'invalid_index': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'product_index',
'type': 'part_type',
'pose': 'part_pose'
})
# x:1091 y:123
OperatableStateMachine.add('TransportPartToAgv',
self.use_behavior(
TransportPartToAgvSM,
'TransportPartToAgv'),
transitions={
'finished': 'IncrementProductIndex',
'failed': 'fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'agv_id': 'agv_id',
'part_type': 'part_type',
'part_pose': 'part_pose'
})
# x:1095 y:229
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'ShipmentReady?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'add_one',
'result': 'product_index'
})
# x:1092 y:486
OperatableStateMachine.add(
'IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'notify_shipment_ready'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'add_one',
'result': 'shipment_index'
})
# x:1094 y:329
OperatableStateMachine.add('ShipmentReady?',
EqualState(),
transitions={
'true': 'ResetProductIndex',
'false': 'GetProductPose'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:474 y:461
OperatableStateMachine.add('OrderReady?',
EqualState(),
transitions={
'true': 'ResetShiptIndex',
'false': 'GetProductsShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:857 y:483
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(
notify_shipment_readySM,
'notify_shipment_ready'),
transitions={
'finished': 'OrderReady?',
'failed': 'fail'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:402 y:33
OperatableStateMachine.add(
'message',
MessageLogger(),
transitions={'continue': 'GetProductsShipment'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'order_id'})
# x:766 y:40
OperatableStateMachine.add(
'message_2',
MessageLogger(),
transitions={'continue': 'GetProductPose'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'shipment_type'})
# x:1090 y:36
OperatableStateMachine.add(
'message_3',
MessageLogger(),
transitions={'continue': 'TransportPartToAgv'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:560 y:34
OperatableStateMachine.add('GetProductsShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'message_2',
'invalid_index': 'fail'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipment_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:203 y:32
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'message'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id':
'order_id',
'shipments':
'shipments',
'number_of_shipments':
'number_of_shipments'
})
# x:1090 y:407
OperatableStateMachine.add(
'ResetProductIndex',
ReplaceState(),
transitions={'done': 'IncrementShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'product_index'
})
# x:338 y:318
OperatableStateMachine.add('ResetShiptIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'shipment_index'
})
return _state_machine
def create(self):
# x:1534 y:100, x:62 y:356
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
output_keys=['ARMidMAIN', 'BothArmsFull', 'BothArmsEMPTY'])
_state_machine.userdata.arm_idR = 'right_arm'
_state_machine.userdata.arm_idL = 'left_arm'
_state_machine.userdata.ARMidMAIN = ''
_state_machine.userdata.BothArmsFull = ''
_state_machine.userdata.Ja_value = 'Ja'
_state_machine.userdata.Nee_value = 'Nee'
_state_machine.userdata.BothArmsEMPTY = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:70 y:32
OperatableStateMachine.add('CheckGripperRight',
CheckGripperattached(),
transitions={
'True': 'CheckGripperLeft',
'False': 'CheckGripperLeft_2',
'invalid_arm_id': 'failed'
},
autonomy={
'True': Autonomy.Off,
'False': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idR'})
# x:69 y:94
OperatableStateMachine.add('CheckGripperLeft',
CheckGripperattached(),
transitions={
'True': 'SetBothArmFull',
'False': 'ResetBothArmFull',
'invalid_arm_id': 'failed'
},
autonomy={
'True': Autonomy.Off,
'False': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idL'})
# x:702 y:22
OperatableStateMachine.add('SetArmRight',
ReplaceState(),
transitions={'done': 'LogRighterarm'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_idR',
'result': 'ARMidMAIN'
})
# x:702 y:85
OperatableStateMachine.add('SetArmLeft',
ReplaceState(),
transitions={'done': 'LogLeftArm'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_idL',
'result': 'ARMidMAIN'
})
# x:528 y:142
OperatableStateMachine.add('SetBothArmFull',
ReplaceState(),
transitions={'done': 'BothArmsFullLOG'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Ja_value',
'result': 'BothArmsFull'
})
# x:888 y:19
OperatableStateMachine.add(
'LogRighterarm',
LogState(text="The Right arm has nothing attached to it",
severity=Logger.REPORT_HINT),
transitions={'done': 'Arm_id is:'},
autonomy={'done': Autonomy.Off})
# x:887 y:77
OperatableStateMachine.add(
'LogLeftArm',
LogState(text="The Left arm has nothing attached to it",
severity=Logger.REPORT_HINT),
transitions={'done': 'Arm_id is:'},
autonomy={'done': Autonomy.Off})
# x:530 y:84
OperatableStateMachine.add('ResetBothArmFull',
ReplaceState(),
transitions={'done': 'SetArmLeft'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Nee_value',
'result': 'BothArmsFull'
})
# x:530 y:26
OperatableStateMachine.add('ResetBothArmFull_2',
ReplaceState(),
transitions={'done': 'SetArmRight'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Nee_value',
'result': 'BothArmsFull'
})
# x:1149 y:41
OperatableStateMachine.add('Arm_id is:',
LogKeyState(
text="The chosen arm_id is:{}",
severity=Logger.REPORT_HINT),
transitions={'done': 'BothArmsFullLOG'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'ARMidMAIN'})
# x:1365 y:90
OperatableStateMachine.add('BothArmsFullLOG',
LogKeyState(
text='Zijn beide armen vol: {}',
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'BothArmsFull'})
# x:300 y:28
OperatableStateMachine.add('CheckGripperLeft_2',
CheckGripperattached(),
transitions={
'True': 'ResetBothArmFull_2',
'False': 'SetBothArmEMPTY',
'invalid_arm_id': 'failed'
},
autonomy={
'True': Autonomy.Off,
'False': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idL'})
# x:319 y:412
OperatableStateMachine.add(
'SetBothArmEMPTY',
ReplaceState(),
transitions={'done': 'ResetBothArmFull_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Ja_value',
'result': 'BothArmsEMPTY'
})
return _state_machine
def create(self):
# x:1169 y:86, x:82 y:552
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type'],
output_keys=[
'camera_topic', 'camera_ref_frame', 'camera_frame',
'part_offset'
])
_state_machine.userdata.part_type = ''
_state_machine.userdata.part1 = 'gear_part'
_state_machine.userdata.part2 = 'piston_rod_part'
_state_machine.userdata.part3 = 'pully_part'
_state_machine.userdata.part4 = 'gasket_part'
_state_machine.userdata.move_group_prefix1 = 'ariac/arm1'
_state_machine.userdata.move_group_prefix2 = 'ariac/arm2'
_state_machine.userdata.camera_ref_frame = ''
_state_machine.userdata.camera_ref_frame1 = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame1 = 'Camera_bin_1_frame'
_state_machine.userdata.camera_ref_frame2 = 'arm2_linear_arm_actuator'
_state_machine.userdata.camera_topic2 = '/ariac/Camera_bin_2'
_state_machine.userdata.camera_topic5 = '/ariac/Camera_bin_5'
_state_machine.userdata.camera_topic6 = '/ariac/Camera_bin_6'
_state_machine.userdata.camera_topic1 = '/ariac/Camera_bin_1'
_state_machine.userdata.camera_frame2 = 'Camera_bin_2_frame'
_state_machine.userdata.camera_frame5 = 'Camera_bin_5_frame'
_state_machine.userdata.camera_frame6 = 'Camera_bin_6_frame'
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.part_offset = 0
_state_machine.userdata.offset1 = 0
_state_machine.userdata.offset2 = 0
_state_machine.userdata.offset3 = 0
_state_machine.userdata.offset4 = 0
_state_machine.userdata.tool_link = 'ee_link'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:131 y:69
OperatableStateMachine.add('partselectie1',
EqualState(),
transitions={
'true': 'camerarefframe1',
'false': 'partselectie2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type',
'value_b': 'part1'
})
# x:129 y:184
OperatableStateMachine.add('partselectie2',
EqualState(),
transitions={
'true': 'camerarefframe1_2',
'false': 'partselectie3'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type',
'value_b': 'part2'
})
# x:127 y:309
OperatableStateMachine.add('partselectie3',
EqualState(),
transitions={
'true': 'camerarefframe1_3',
'false': 'partselectie4'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type',
'value_b': 'part3'
})
# x:126 y:425
OperatableStateMachine.add('partselectie4',
EqualState(),
transitions={
'true': 'camerarefframe1_4',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type',
'value_b': 'part4'
})
# x:315 y:64
OperatableStateMachine.add(
'camerarefframe1',
ReplaceState(),
transitions={'done': 'camerarefframe1_5'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_ref_frame1',
'result': 'camera_ref_frame'
})
# x:306 y:177
OperatableStateMachine.add(
'camerarefframe1_2',
ReplaceState(),
transitions={'done': 'camerarefframe1_6'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_ref_frame1',
'result': 'camera_ref_frame'
})
# x:307 y:293
OperatableStateMachine.add(
'camerarefframe1_3',
ReplaceState(),
transitions={'done': 'camerarefframe1_7'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_ref_frame2',
'result': 'camera_ref_frame'
})
# x:306 y:408
OperatableStateMachine.add(
'camerarefframe1_4',
ReplaceState(),
transitions={'done': 'camerarefframe1_8'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_ref_frame2',
'result': 'camera_ref_frame'
})
# x:486 y:64
OperatableStateMachine.add(
'camerarefframe1_5',
ReplaceState(),
transitions={'done': 'camerarefframe1_9'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic1',
'result': 'camera_topic'
})
# x:477 y:177
OperatableStateMachine.add(
'camerarefframe1_6',
ReplaceState(),
transitions={'done': 'camerarefframe1_10'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic2',
'result': 'camera_topic'
})
# x:480 y:294
OperatableStateMachine.add(
'camerarefframe1_7',
ReplaceState(),
transitions={'done': 'camerarefframe1_11'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic5',
'result': 'camera_topic'
})
# x:480 y:406
OperatableStateMachine.add(
'camerarefframe1_8',
ReplaceState(),
transitions={'done': 'camerarefframe1_12'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_topic6',
'result': 'camera_topic'
})
# x:666 y:60
OperatableStateMachine.add('camerarefframe1_9',
ReplaceState(),
transitions={'done': 'offsetgearpart'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame1',
'result': 'camera_frame'
})
# x:660 y:179
OperatableStateMachine.add(
'camerarefframe1_10',
ReplaceState(),
transitions={'done': 'offsetPistonrodpart'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame2',
'result': 'camera_frame'
})
# x:666 y:289
OperatableStateMachine.add('camerarefframe1_11',
ReplaceState(),
transitions={'done': 'offsetpullypart'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame5',
'result': 'camera_frame'
})
# x:671 y:398
OperatableStateMachine.add(
'camerarefframe1_12',
ReplaceState(),
transitions={'done': 'offsetGasketpart'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'camera_frame6',
'result': 'camera_frame'
})
# x:852 y:58
OperatableStateMachine.add('offsetgearpart',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset1',
'result': 'part_offset'
})
# x:854 y:178
OperatableStateMachine.add('offsetPistonrodpart',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset2',
'result': 'part_offset'
})
# x:860 y:287
OperatableStateMachine.add('offsetpullypart',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset3',
'result': 'part_offset'
})
# x:864 y:395
OperatableStateMachine.add('offsetGasketpart',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset4',
'result': 'part_offset'
})
return _state_machine
def create(self):
# x:1067 y:518, x:672 y:189
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['shipment_index', 'part_index'],
output_keys=[
'part_type_L', 'part_type_R', 'agv_id', 'part_pose_L',
'part_pose_R', 'shipment_index', 'part_index'
])
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = ''
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.part_index = 0
_state_machine.userdata.part_type_R = ''
_state_machine.userdata.part_pose_L = []
_state_machine.userdata.part_type_L = []
_state_machine.userdata.part_pose_R = []
_state_machine.userdata.add_one = 1
_state_machine.userdata.reset = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.none = 'None'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:253 y:450
OperatableStateMachine.add('ShipmentReady?',
EqualState(),
transitions={
'true': 'ResetPartIndex',
'false': 'GetRPartPose'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
# x:442 y:38
OperatableStateMachine.add('GetShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'GetRPartPose',
'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:658 y:37
OperatableStateMachine.add('GetRPartPose',
GetPartFromProductsState(),
transitions={
'continue': 'IncrementPartIndex',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'part_index',
'type': 'part_type_R',
'pose': 'part_pose_R'
})
# x:853 y:181
OperatableStateMachine.add('GetLPartPose',
GetPartFromProductsState(),
transitions={
'continue': 'IncrementPartIndex_2',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'part_index',
'type': 'part_type_L',
'pose': 'part_pose_L'
})
# x:1046 y:185
OperatableStateMachine.add('IncrementShipIndex',
AddNumericState(),
transitions={'done': 'CheckAGV'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'add_one',
'result': 'shipment_index'
})
# x:262 y:171
OperatableStateMachine.add('OrderReady?',
EqualState(),
transitions={
'true': 'ResetShipmentIndex',
'false': 'GetShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_shipments',
'value_b': 'shipment_index'
})
# x:853 y:253
OperatableStateMachine.add(
'IncrementPartIndex_2',
AddNumericState(),
transitions={'done': 'ShipmentReady2?2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'part_index',
'value_b': 'add_one',
'result': 'part_index'
})
# x:853 y:111
OperatableStateMachine.add('ShipmentReady2?',
EqualState(),
transitions={
'true': 'None',
'false': 'GetLPartPose'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
# x:249 y:387
OperatableStateMachine.add('ResetPartIndex',
ReplaceState(),
transitions={'done': 'ShipIndex0'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'reset',
'result': 'part_index'
})
# x:260 y:107
OperatableStateMachine.add('ResetShipmentIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'reset',
'result': 'shipment_index'
})
# x:261 y:38
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'GetShipment'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id':
'order_id',
'shipments':
'shipments',
'number_of_shipments':
'number_of_shipments'
})
# x:1044 y:110
OperatableStateMachine.add(
'None',
ReplaceState(),
transitions={'done': 'IncrementShipIndex'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'none',
'result': 'part_type_L'
})
# x:850 y:36
OperatableStateMachine.add('IncrementPartIndex',
AddNumericState(),
transitions={'done': 'ShipmentReady2?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'part_index',
'value_b': 'add_one',
'result': 'part_index'
})
# x:1041 y:257
OperatableStateMachine.add('ShipmentReady2?2',
EqualState(),
transitions={
'true': 'IncrementShipIndex',
'false': 'CheckAGV'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'number_of_products',
'value_b': 'part_index'
})
# x:215 y:313
OperatableStateMachine.add('ShipIndex0',
EqualState(),
transitions={
'true': 'OrderReady?',
'false': 'SendShipment'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'reset'
})
# x:1159 y:330
OperatableStateMachine.add('CheckAGV',
self.use_behavior(
CheckAGVSM, 'CheckAGV'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'agv_id': 'agv_id'})
# x:325 y:244
OperatableStateMachine.add('SendShipment',
NotifyShipmentReadyState(),
transitions={
'continue': 'OrderReady?',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={
'agv_id': 'agv_id',
'shipment_type': 'shipment_type',
'success': 'success',
'message': 'message'
})
return _state_machine
def create(self):
# x:579 y:679, x:313 y:185, x:323 y:666
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'false'],
input_keys=[
'number_of_products', 'product_index', 'bin_id', 'part_type',
'part_pose', 'agv_id'
],
output_keys=['product_index'])
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.one_value = 1
_state_machine.userdata.bin_id = ''
_state_machine.userdata.zero_value = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.agv_id = ''
_state_machine.userdata.arm_id_right = 'right'
_state_machine.userdata.arm_id_left = 'left'
_state_machine.userdata.part_pose_right = []
_state_machine.userdata.part_pose_left = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:49 y:41
OperatableStateMachine.add('GripperControlLeft',
GripperControle(),
transitions={
'Active': 'GripperControlRight',
'Not_active': 'SelectorStateLeft',
'invalid_arm_id': 'failed'
},
autonomy={
'Active': Autonomy.Off,
'Not_active': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_left'})
# x:50 y:185
OperatableStateMachine.add('SelectorStateLeft',
self.use_behavior(
SelectorStateLeftSM,
'SelectorStateLeft'),
transitions={
'finished': 'IncrementProductIndex',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'part_type': 'part_type',
'arm_id_left': 'arm_id_left',
'part_pose': 'part_pose',
'part_pose_left': 'part_pose_left'
})
# x:516 y:37
OperatableStateMachine.add('GripperControlRight',
GripperControle(),
transitions={
'Active': 'AgvSelector',
'Not_active': 'SelectorStateRight',
'invalid_arm_id': 'failed'
},
autonomy={
'Active': Autonomy.Off,
'Not_active': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_right'})
# x:510 y:189
OperatableStateMachine.add(
'SelectorStateRight',
self.use_behavior(SelectorStateRightSM, 'SelectorStateRight'),
transitions={
'finished': 'IncrementProductIndex_2',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'part_type': 'part_type',
'arm_id_right': 'arm_id_right',
'part_pose': 'part_pose',
'part_pose_right': 'part_pose_right'
})
# x:49 y:302
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'EndProduct'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:49 y:398
OperatableStateMachine.add('EndProduct',
EqualState(),
transitions={
'true': 'AgvSelector',
'false': 'false'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:507 y:303
OperatableStateMachine.add('IncrementProductIndex_2',
AddNumericState(),
transitions={'done': 'EndProduct_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:508 y:396
OperatableStateMachine.add('EndProduct_2',
EqualState(),
transitions={
'true': 'AgvSelector',
'false': 'false'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:509 y:478
OperatableStateMachine.add('ResetProductIndex',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'product_index'
})
# x:271 y:473
OperatableStateMachine.add('EndProduct_3',
EqualState(),
transitions={
'true': 'ResetProductIndex',
'false': 'false'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:267 y:347
OperatableStateMachine.add('AgvSelector',
self.use_behavior(
AgvSelectorSM, 'AgvSelector'),
transitions={
'finished': 'EndProduct_3',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_pose_right':
'part_pose_right',
'part_pose_left': 'part_pose_left',
'agv_id': 'agv_id',
'arm_id_right': 'arm_id_right',
'arm_id_left': 'arm_id_left'
})
return _state_machine
def create(self):
# x:982 y:568, x:604 y:185
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['part', 'arm_id'])
_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.arm_id = 'Right_Arm'
_state_machine.userdata.tool_link = ''
_state_machine.userdata.pose = []
_state_machine.userdata.offset = 0.15
_state_machine.userdata.rotation = 0
_state_machine.userdata.part = 'gear_part_red'
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.home = 'Home'
_state_machine.userdata.safe = 'Full_Bin'
_state_machine.userdata.Up = 0.2
_state_machine.userdata.preShelf = 'Gantry_PreShelf'
_state_machine.userdata.shelf = 'Full_Shelf'
_state_machine.userdata.one = 1
_state_machine.userdata.bin = 0
_state_machine.userdata.zero = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:118 y:24
OperatableStateMachine.add('FindPart',
FindPart(time_out=0.2),
transitions={
'bin': 'BinTo1',
'shelf': 'BinTo0',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'bin': Autonomy.Off,
'shelf': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'part_type': 'part',
'arm_id': 'arm_id',
'gantry_pos': 'config_name',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame'
})
# x:839 y:25
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_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1048 y:25
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={
'continue': 'OffsetUp',
'failed': 'MoveToPart',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:167 y:236
OperatableStateMachine.add('SafePosBetweenBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetPartPose',
'planning_failed': 'failed',
'control_failed':
'SafePosBetweenBin',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'safe',
'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:107 y:400
OperatableStateMachine.add('PreGrasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePick_3',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:361 y:25
OperatableStateMachine.add('Offset',
part_offsetCalc(),
transitions={
'succes': 'ComputePick',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part',
'part_offset': 'offset'
})
# x:1044 y:400
OperatableStateMachine.add('ArmHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'SafePosBetweenBin_2',
'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': 'home',
'move_group': 'arm_id',
'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:1038 y:475
OperatableStateMachine.add('SafePosBetweenBin_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed':
'SafePosBetweenBin_2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'safe',
'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:1025 y:89
OperatableStateMachine.add('OffsetUp',
ReplaceState(),
transitions={'done': 'ComputePick_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Up',
'result': 'offset'
})
# x:970 y:155
OperatableStateMachine.add('ComputePick_2',
ComputeGraspAriacState(),
transitions={
'continue': 'MoveToPart_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:938 y:244
OperatableStateMachine.add('MoveToPart_2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Bin1?',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:103 y:469
OperatableStateMachine.add('ComputePick_3',
ComputeGraspAriacState(),
transitions={
'continue': 'MoveToPart_3',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:95 y:540
OperatableStateMachine.add('MoveToPart_3',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Offset',
'planning_failed': 'MoveToPart_3',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:647 y:24
OperatableStateMachine.add('ComputePick',
ComputeGraspAriacState(),
transitions={
'continue': 'MoveToPart',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:0 y:171
OperatableStateMachine.add('ToShelf',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ToShelf1',
'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': 'preShelf',
'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:0 y:233
OperatableStateMachine.add('ToShelf1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetPartPose',
'planning_failed': 'failed',
'control_failed': 'ToShelf1',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'shelf',
'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:936 y:329
OperatableStateMachine.add('Bin1?',
EqualState(),
transitions={
'true': 'ArmHome_2',
'false': 'ToShelf1_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin',
'value_b': 'one'
})
# x:160 y:170
OperatableStateMachine.add(
'BinTo1',
ReplaceState(),
transitions={'done': 'SafePosBetweenBin'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'one',
'result': 'bin'
})
# x:830 y:467
OperatableStateMachine.add('ToShelf_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'preShelf',
'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:826 y:396
OperatableStateMachine.add('ToShelf1_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ToShelf_2',
'planning_failed': 'failed',
'control_failed': 'ToShelf1_2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'shelf',
'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:96 y:330
OperatableStateMachine.add(
'GetPartPose',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'PreGrasp',
'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',
'pose': 'pose'
})
# x:0 y:113
OperatableStateMachine.add('BinTo0',
ReplaceState(),
transitions={'done': 'ToShelf'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'bin'
})
return _state_machine
def create(self):
# x:677 y:290, x:12 y:267
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type', 'agv_id'],
output_keys=['arm_id', 'move_group_prefix', 'config_name_bin'])
_state_machine.userdata.config_name_bin = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.arm_id = ''
_state_machine.userdata.move_group_prefix = ''
_state_machine.userdata.config_name_bin = ''
_state_machine.userdata.camera_topic1 = '/ariac/logical_camera_bin1'
_state_machine.userdata.camera_topic2 = '/ariac/logical_camera_bin2'
_state_machine.userdata.camera_topic3 = '/ariac/logical_camera_bin3'
_state_machine.userdata.camera_topic4 = '/ariac/logical_camera_bin4'
_state_machine.userdata.camera_topic5 = '/ariac/logical_camera_bin5'
_state_machine.userdata.camera_topic6 = '/ariac/logical_camera_bin6'
_state_machine.userdata.camera_frame = 'logical_camera_frame'
_state_machine.userdata.camera_ref_frame = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:101 y:40
OperatableStateMachine.add(
'DetectPartCamera',
DetectPartCameraAriacState(time_out=1.0),
transitions={
'continue': 'ReplaceTransferBin',
'failed': 'failed',
'not_found': 'DetectPartCamera_2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic1',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:322 y:41
OperatableStateMachine.add('ReplaceTransferBin',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_bin',
'result': 'bin1PreGrasp'
})
# x:101 y:117
OperatableStateMachine.add(
'DetectPartCamera_2',
DetectPartCameraAriacState(time_out=1.0),
transitions={
'continue': 'ReplaceTransferBin_2',
'failed': 'failed',
'not_found': 'DetectPartCamera_3'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic2',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:322 y:121
OperatableStateMachine.add('ReplaceTransferBin_2',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_bin',
'result': 'bin2PreGrasp'
})
# x:101 y:209
OperatableStateMachine.add(
'DetectPartCamera_3',
DetectPartCameraAriacState(time_out=1.0),
transitions={
'continue': 'ReplaceTransferBin_3',
'failed': 'failed',
'not_found': 'DetectPartCamera_4'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic3',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:322 y:207
OperatableStateMachine.add('ReplaceTransferBin_3',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_bin',
'result': 'bin3PreGrasp'
})
# x:101 y:301
OperatableStateMachine.add(
'DetectPartCamera_4',
DetectPartCameraAriacState(time_out=1.0),
transitions={
'continue': 'ReplaceTransferBin_4',
'failed': 'failed',
'not_found': 'DetectPartCamera_5'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic4',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:326 y:304
OperatableStateMachine.add('ReplaceTransferBin_4',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_bin',
'result': 'bin4PreGrasp'
})
# x:101 y:393
OperatableStateMachine.add(
'DetectPartCamera_5',
DetectPartCameraAriacState(time_out=1.0),
transitions={
'continue': 'ReplaceTransferBin_5',
'failed': 'failed',
'not_found': 'DetectPartCamera_6'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic5',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:328 y:392
OperatableStateMachine.add('ReplaceTransferBin_5',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_bin',
'result': 'bin5PreGrasp'
})
# x:101 y:485
OperatableStateMachine.add(
'DetectPartCamera_6',
DetectPartCameraAriacState(time_out=1.0),
transitions={
'continue': 'ReplaceTransferBin_6',
'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_topic6',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:327 y:488
OperatableStateMachine.add('ReplaceTransferBin_6',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_bin',
'result': 'bin6PreGrasp'
})
return _state_machine
def create(self):
# x:144 y:698, x:147 y:461
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.shipment_type = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:190 y:41
OperatableStateMachine.add(
'GetOrder',
GetOrderState(),
transitions={'continue': 'GetProductsFromShipment'},
autonomy={'continue': Autonomy.Off},
remapping={
'order_id': 'order_id',
'shipments': 'shipments',
'number_of_shipments': 'number_of_shipments'
})
# x:409 y:40
OperatableStateMachine.add('GetProductsFromShipment',
GetProductsFromShipmentState(),
transitions={
'continue': 'GetPartFromProduct',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'shipments':
'shipments',
'index':
'shipment_index',
'shipment_type':
'shipment_type',
'agv_id':
'agv_id',
'products':
'products',
'number_of_products':
'number_of_products'
})
# x:670 y:38
OperatableStateMachine.add(
'GetPartFromProduct',
GetPartFromProductsState(),
transitions={
'continue': 'transport_part_form_bin_to_agv_state',
'invalid_index': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'products': 'products',
'index': 'product_index',
'type': 'part_type',
'pose': 'part_pose'
})
# x:1211 y:34
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'EndProducts'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'product_index',
'value_b': 'one_value',
'result': 'product_index'
})
# x:1213 y:164
OperatableStateMachine.add('EndProducts',
EqualState(),
transitions={
'true': 'ResetProductIndex',
'false': 'GetPartFromProduct'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'product_index',
'value_b': 'number_of_products'
})
# x:1214 y:282
OperatableStateMachine.add(
'ResetProductIndex',
ReplaceState(),
transitions={'done': 'IncrementShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'product_index'
})
# x:1214 y:413
OperatableStateMachine.add('IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'EindShipments'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'shipment_index',
'value_b': 'one_value',
'result': 'shipment_index'
})
# x:1215 y:541
OperatableStateMachine.add('EindShipments',
EqualState(),
transitions={
'true': 'ResetShipmentIndex',
'false': 'notify_shipment_ready'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'shipment_index',
'value_b': 'number_of_shipments'
})
# x:1000 y:541
OperatableStateMachine.add('ResetShipmentIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero_value',
'result': 'shipment_index'
})
# x:1211 y:782
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(
notify_shipment_readySM,
'notify_shipment_ready'),
transitions={
'finished':
'GetProductsFromShipment',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:911 y:38
OperatableStateMachine.add(
'transport_part_form_bin_to_agv_state',
self.use_behavior(transport_part_form_bin_to_agv_stateSM,
'transport_part_form_bin_to_agv_state'),
transitions={
'finished': 'IncrementProductIndex',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
# x:821 y:56, x:1214 y:358
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.old_order_id = ''
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.products = []
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.material_locations = []
_state_machine.userdata.bin_index = 0
_state_machine.userdata.bin_id = ''
_state_machine.userdata.one_value = 1
_state_machine.userdata.zero_value = 0
_state_machine.userdata.arm_right = 'right_arm'
_state_machine.userdata.arm_left = 'left_arm'
_state_machine.userdata.product_location1 = ''
_state_machine.userdata.product_location2 = ''
_state_machine.userdata.home_rightarm_id = 'Right_PreGrasp'
_state_machine.userdata.move_group_right = 'Right_Arm'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.home_leftarm_id = 'Left_PreGrasp'
_state_machine.userdata.move_group_left = 'Left_Arm'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:57 y:48
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'GetOrder'},
autonomy={'continue': Autonomy.Off})
# x:218 y:48
OperatableStateMachine.add('GetOrder',
GetOrderState(),
transitions={'continue': 'Test_LastOrder'},
autonomy={'continue': Autonomy.Off},
remapping={'order_id': 'order_id', 'shipments': 'shipments', 'number_of_shipments': 'number_of_shipments'})
# x:422 y:47
OperatableStateMachine.add('Test_LastOrder',
EqualState(),
transitions={'true': 'EndAssignment', 'false': 'Remember_OldOrder'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'order_id', 'value_b': 'old_order_id'})
# x:614 y:124
OperatableStateMachine.add('Remember_OldOrder',
ReplaceState(),
transitions={'done': 'GetShipment'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'order_id', 'result': 'old_order_id'})
# x:622 y:47
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:801 y:119
OperatableStateMachine.add('GetShipment',
GetProductsFromShipmentState(),
transitions={'continue': 'GetPart', 'invalid_index': 'failed'},
autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'shipments': 'shipments', 'index': 'shipment_index', 'shipment_type': 'shipment_type', 'agv_id': 'agv_id', 'products': 'products', 'number_of_products': 'number_of_products'})
# x:1023 y:119
OperatableStateMachine.add('GetPart',
GetPartFromProductsState(),
transitions={'continue': 'MaterialLocation', 'invalid_index': 'failed'},
autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'products': 'products', 'index': 'product_index', 'type': 'part_type', 'pose': 'part_pose'})
# x:1217 y:116
OperatableStateMachine.add('MaterialLocation',
GetMaterialLocationsState(),
transitions={'continue': 'GetBin'},
autonomy={'continue': Autonomy.Off},
remapping={'part': 'part_type', 'material_locations': 'material_locations'})
# x:1403 y:114
OperatableStateMachine.add('GetBin',
GetItemFromListState(),
transitions={'done': 'CheckAttachedRight', 'invalid_index': 'failed'},
autonomy={'done': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'list': 'material_locations', 'index': 'bin_index', 'item': 'bin_id'})
# x:1361 y:507
OperatableStateMachine.add('IncrementProductIndex',
AddNumericState(),
transitions={'done': 'EndProduct'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'one_value', 'result': 'product_index'})
# x:1360 y:585
OperatableStateMachine.add('EndProduct',
EqualState(),
transitions={'true': 'HomepositieRightArm', 'false': 'CheckAttachedRight2'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'number_of_products'})
# x:95 y:565
OperatableStateMachine.add('ResetProductIndex',
ReplaceState(),
transitions={'done': 'IncrementShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'zero_value', 'result': 'product_index'})
# x:98 y:469
OperatableStateMachine.add('IncrementShipmentIndex',
AddNumericState(),
transitions={'done': 'Notify_Shipment_Ready'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'shipment_index', 'value_b': 'one_value', 'result': 'shipment_index'})
# x:89 y:277
OperatableStateMachine.add('EndShipment',
EqualState(),
transitions={'true': 'ResetShipmentIndex', 'false': 'GetShipment'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'shipment_index', 'value_b': 'number_of_shipments'})
# x:87 y:378
OperatableStateMachine.add('Notify_Shipment_Ready',
self.use_behavior(Notify_Shipment_ReadySM, 'Notify_Shipment_Ready'),
transitions={'finished': 'EndShipment', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'shipment_type': 'shipment_type'})
# x:1605 y:181
OperatableStateMachine.add('Transport_part_from_bin_to_agv',
self.use_behavior(Transport_part_from_bin_to_agvSM, 'Transport_part_from_bin_to_agv'),
transitions={'finished': 'SetVariablePart1', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_type': 'part_type', 'part_pose': 'part_pose', 'bin_id': 'bin_id'})
# x:95 y:183
OperatableStateMachine.add('ResetShipmentIndex',
ReplaceState(),
transitions={'done': 'GetOrder'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'zero_value', 'result': 'shipment_index'})
# x:1354 y:248
OperatableStateMachine.add('CheckAttachedRight',
CheckGripperattached(),
transitions={'True': 'CheckAttachedLeft', 'False': 'Transport_part_from_bin_to_agv', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_right'})
# x:1356 y:335
OperatableStateMachine.add('CheckAttachedLeft',
CheckGripperattached(),
transitions={'True': 'IncrementProductIndex', 'False': 'Transport_part_from_bin_to_agv_LeftArm', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_left'})
# x:1572 y:418
OperatableStateMachine.add('Transport_part_from_bin_to_agv_LeftArm',
self.use_behavior(Transport_part_from_bin_to_agv_LeftArmSM, 'Transport_part_from_bin_to_agv_LeftArm'),
transitions={'finished': 'SetVariablePart2', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_type': 'part_type', 'part_pose': 'part_pose', 'bin_id': 'bin_id'})
# x:1654 y:275
OperatableStateMachine.add('SetVariablePart1',
ReplaceState(),
transitions={'done': 'IncrementProductIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'part_pose', 'result': 'product_location1'})
# x:1651 y:503
OperatableStateMachine.add('SetVariablePart2',
ReplaceState(),
transitions={'done': 'IncrementProductIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'part_pose', 'result': 'product_location2'})
# x:997 y:510
OperatableStateMachine.add('CheckAttachedRight2',
CheckGripperattached(),
transitions={'True': 'CheckAttachedLeft2', 'False': 'GetPart', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_right'})
# x:803 y:520
OperatableStateMachine.add('CheckAttachedLeft2',
CheckGripperattached(),
transitions={'True': 'PlaceOnAgv', 'False': 'GetPart', 'invalid_arm_id': 'failed'},
autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_left'})
# x:533 y:450
OperatableStateMachine.add('PlaceOnAgv',
self.use_behavior(PlaceOnAgvSM, 'PlaceOnAgv'),
transitions={'finished': 'GetPart', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_pose_right': 'product_location1', 'part_pose_left': 'product_location2'})
# x:1371 y:670
OperatableStateMachine.add('HomepositieRightArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'HomepositieLeftArm', 'planning_failed': 'Retry_1', 'control_failed': 'Retry_1', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'home_rightarm_id', 'move_group': 'move_group_right', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1182 y:655
OperatableStateMachine.add('HomepositieLeftArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PlaceOnAgv_2', 'planning_failed': 'Retry_2', 'control_failed': 'Retry_2', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'home_leftarm_id', 'move_group': 'move_group_left', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1558 y:671
OperatableStateMachine.add('Retry_1',
WaitState(wait_time=3),
transitions={'done': 'HomepositieRightArm'},
autonomy={'done': Autonomy.Off})
# x:1226 y:733
OperatableStateMachine.add('Retry_2',
WaitState(wait_time=3),
transitions={'done': 'HomepositieLeftArm'},
autonomy={'done': Autonomy.Off})
# x:832 y:626
OperatableStateMachine.add('PlaceOnAgv_2',
self.use_behavior(PlaceOnAgvSM, 'PlaceOnAgv_2'),
transitions={'finished': 'ResetProductIndex', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'agv_id': 'agv_id', 'part_pose_right': 'product_location1', 'part_pose_left': 'product_location2'})
return _state_machine
def create(self):
# x:1184 y:332, x:604 y:185
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['part', 'arm_id'])
_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.arm_id = 'Right_Arm'
_state_machine.userdata.tool_link = 'right_ee_link'
_state_machine.userdata.pose = []
_state_machine.userdata.offset = 0.15
_state_machine.userdata.rotation = 0
_state_machine.userdata.part = 'gear_part_red'
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.home = 'Home'
_state_machine.userdata.safe = 'Gantry_Bin'
_state_machine.userdata.Up = 0.2
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:33 y:31
OperatableStateMachine.add('FindParts',
FindPart(time_out=0.2),
transitions={
'found': 'ArmHome',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'found': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'part_type': 'part',
'gantry_pos': 'config_name',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame'
})
# x:647 y:24
OperatableStateMachine.add(
'ComputePick',
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': 'MoveToPart',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:16 y:242
OperatableStateMachine.add(
'GetPartPose',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'PreGrasp',
'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',
'pose': 'pose'
})
# x:839 y:25
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_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1048 y:25
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={
'continue': 'OffsetUp',
'failed': 'MoveToPart',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:27 y:178
OperatableStateMachine.add('SafePosBetweenBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetPartPose',
'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': 'safe',
'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:23 y:320
OperatableStateMachine.add('PreGrasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePick_3',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:25 y:100
OperatableStateMachine.add('ArmHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'SafePosBetweenBin',
'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': 'home',
'move_group': 'arm_id',
'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:361 y:25
OperatableStateMachine.add('Offset',
part_offsetCalc(),
transitions={
'succes': 'ComputePick',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part',
'part_offset': 'offset'
})
# x:1041 y:361
OperatableStateMachine.add('ArmHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'SafePosBetweenBin_2',
'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': 'home',
'move_group': 'arm_id',
'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:1041 y:442
OperatableStateMachine.add('SafePosBetweenBin_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'safe',
'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:1025 y:89
OperatableStateMachine.add('OffsetUp',
ReplaceState(),
transitions={'done': 'ComputePick_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Up',
'result': 'offset'
})
# x:1034 y:188
OperatableStateMachine.add(
'ComputePick_2',
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': 'MoveToPart_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1029 y:261
OperatableStateMachine.add('MoveToPart_2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'ArmHome_2',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:22 y:413
OperatableStateMachine.add(
'ComputePick_3',
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': 'MoveToPart_3',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:331 y:410
OperatableStateMachine.add('MoveToPart_3',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Offset',
'planning_failed': 'MoveToPart_3',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
return _state_machine
def create(self):
# x:92 y:307, x:966 y:441
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'offset_part_left', 'PreGrasp', 'camera_topic', 'camera_frame',
'offset_part_left', 'part_type', 'arm_id_left', 'part_pose'
],
output_keys=['part_pose_left'])
_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.move_group_right_arm = 'Right_Arm'
_state_machine.userdata.move_group_left_arm = 'Left_Arm'
_state_machine.userdata.config_name_pre_shelves = 'Gantry_PreGrasp_Pre_shelves'
_state_machine.userdata.PreGrasp = ''
_state_machine.userdata.camera_ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.offset_part_left = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.arm_id_left = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.part_pose_left = []
_state_machine.userdata.config_name_left_arm2 = 'Left_Home_shelves'
_state_machine.userdata.config_name_shelves = 'Gantry_PreGrasp_shelves'
_state_machine.userdata.config_name_right_arm2 = 'Right_Home_shelves'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:745 y:152
OperatableStateMachine.add('PreShelf',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectPart',
'planning_failed': 'Retry_4',
'control_failed': 'Retry_4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_pre_shelves',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:773 y:27
OperatableStateMachine.add('Retry_4',
WaitState(wait_time=0.3),
transitions={'done': 'PreShelf'},
autonomy={'done': Autonomy.Off})
# x:1558 y:259
OperatableStateMachine.add('PreGrasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePick',
'planning_failed': 'Retry_5',
'control_failed': 'Retry_5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'PreGrasp',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1730 y:262
OperatableStateMachine.add('Retry_5',
WaitState(wait_time=0.3),
transitions={'done': 'PreGrasp'},
autonomy={'done': Autonomy.Off})
# x:1548 y:361
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': 'Move_to_pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_left_arm',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_left',
'pose': 'pose',
'offset': 'offset_part_left',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:918 y:149
OperatableStateMachine.add(
'DetectPart',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Move_left_arm_home2',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:1734 y:461
OperatableStateMachine.add('Retry_6',
WaitState(wait_time=0.3),
transitions={'done': 'Move_to_pick'},
autonomy={'done': Autonomy.Off})
# x:1533 y:463
OperatableStateMachine.add('Move_to_pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperOn',
'planning_failed': 'Retry_6',
'control_failed': 'GripperOn'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_left_arm',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1535 y:573
OperatableStateMachine.add('GripperOn',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'Move_left_arm_home_2',
'failed': 'ComputePick',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_left'})
# x:1378 y:653
OperatableStateMachine.add('Move_left_arm_home_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Shelves_2',
'planning_failed': 'Retry_7',
'control_failed': 'Retry_7',
'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_arm2',
'move_group': 'move_group_left_arm',
'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:1426 y:747
OperatableStateMachine.add(
'Retry_7',
WaitState(wait_time=0.3),
transitions={'done': 'Move_left_arm_home_2'},
autonomy={'done': Autonomy.Off})
# x:867 y:641
OperatableStateMachine.add('PreShelf_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Pose',
'planning_failed': 'Retry_8',
'control_failed': 'Retry_8',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_pre_shelves',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:889 y:738
OperatableStateMachine.add('Retry_8',
WaitState(wait_time=0.3),
transitions={'done': 'PreShelf_2'},
autonomy={'done': Autonomy.Off})
# x:127 y:421
OperatableStateMachine.add('Pose',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'part_pose',
'result': 'part_pose_left'
})
# x:1135 y:148
OperatableStateMachine.add('Move_left_arm_home2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move_right_arm_home2',
'planning_failed': 'Retry_9',
'control_failed': 'Retry_9',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_left_arm2',
'move_group': 'move_group_left_arm',
'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:1159 y:31
OperatableStateMachine.add(
'Retry_9',
WaitState(wait_time=0.3),
transitions={'done': 'Move_left_arm_home2'},
autonomy={'done': Autonomy.Off})
# x:1346 y:147
OperatableStateMachine.add(
'Move_right_arm_home2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Shelves',
'planning_failed': 'Retry_10',
'control_failed': 'Retry_10',
'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_arm2',
'move_group': 'move_group_right_arm',
'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:1381 y:35
OperatableStateMachine.add(
'Retry_10',
WaitState(wait_time=0.3),
transitions={'done': 'Move_right_arm_home2'},
autonomy={'done': Autonomy.Off})
# x:1561 y:148
OperatableStateMachine.add('Shelves',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreGrasp',
'planning_failed': 'Retry_11',
'control_failed': 'Retry_11',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_shelves',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1591 y:35
OperatableStateMachine.add('Retry_11',
WaitState(wait_time=0.3),
transitions={'done': 'Shelves'},
autonomy={'done': Autonomy.Off})
# x:1063 y:639
OperatableStateMachine.add('Shelves_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreShelf_2',
'planning_failed': 'Retry_12',
'control_failed': 'Retry_12',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_shelves',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1074 y:746
OperatableStateMachine.add('Retry_12',
WaitState(wait_time=0.3),
transitions={'done': 'Shelves_2'},
autonomy={'done': Autonomy.Off})
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:533 y:79, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['arm_id'],
output_keys=['move_group_prefix'])
_state_machine.userdata.prefix_arm1 = 'ariac/arm1'
_state_machine.userdata.prefix_arm2 = 'ariac/arm2'
_state_machine.userdata.arm1 = 'arm1'
_state_machine.userdata.arm2 = 'arm2'
_state_machine.userdata.arm_id = ''
_state_machine.userdata.move_group_prefix = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:51 y:45
OperatableStateMachine.add('vergelijk1',
EqualState(),
transitions={
'true': 'value1',
'false': 'vergelijk1_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'arm_id',
'value_b': 'arm1'
})
# x:50 y:168
OperatableStateMachine.add('vergelijk1_2',
EqualState(),
transitions={
'true': 'value1_2',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'arm_id',
'value_b': 'arm2'
})
# x:280 y:49
OperatableStateMachine.add('value1',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'prefix_arm1',
'result': 'move_group_prefix'
})
# x:279 y:169
OperatableStateMachine.add('value1_2',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'prefix_arm2',
'result': 'move_group_prefix'
})
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:1089 y:770, x:1143 y:423, x:1266 y:674
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed', 'end'],
input_keys=[
'move_group_prefix', 'move_group_right_arm',
'move_group_left_arm', 'action_topic', 'camera_frame',
'camera_topic', 'ref_frame', 'move_group_gantry', 'robot_name',
'offset_var_left', 'offset_var_right', 'offset_gasket',
'offset_piston', 'arm_idL', 'arm_idR',
'config_name_Right_Home', 'config_name_Left_Home',
'config_name_RA_pre', 'config_name_LA_pre'
])
_state_machine.userdata.move_group_prefix = ''
_state_machine.userdata.move_group_right_arm = ''
_state_machine.userdata.move_group_left_arm = ''
_state_machine.userdata.action_topic = ''
_state_machine.userdata.camera_frame = '/ariac/logical_camera_0'
_state_machine.userdata.camera_topic = 'logical_camera_0_frame'
_state_machine.userdata.ref_frame = ''
_state_machine.userdata.move_group_gantry = ''
_state_machine.userdata.robot_name = ''
_state_machine.userdata.offset_var_left = 0
_state_machine.userdata.offset_var_right = 0
_state_machine.userdata.offset_gasket = 0
_state_machine.userdata.offset_piston = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.tool_link_right = 'right_ee_link'
_state_machine.userdata.arm_idL = ''
_state_machine.userdata.arm_idR = ''
_state_machine.userdata.config_name_Right_Home = ''
_state_machine.userdata.config_name_Left_Home = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.bin_id = ''
_state_machine.userdata.bin1 = 'bin1'
_state_machine.userdata.bin2 = 'bin2'
_state_machine.userdata.offset_id = 0
_state_machine.userdata.arm_id = ''
_state_machine.userdata.config_name_R_bin1 = 'Gantry_PreGrasp_R_bin1'
_state_machine.userdata.config_name_L_bin1 = 'Gantry_PreGrasp_L_bin1'
_state_machine.userdata.config_name_bin1_var = ''
_state_machine.userdata.config_name_RA_pre = ''
_state_machine.userdata.config_name_LA_pre = ''
_state_machine.userdata.offsetx_bin1 = 0
_state_machine.userdata.offsety_bin1 = 0
_state_machine.userdata.offsetx_set = 0.33418
_state_machine.userdata.offsety_set = 0.1827866667
_state_machine.userdata.plus1 = 1
_state_machine.userdata.xproduct_bin1 = 0
_state_machine.userdata.yproduct_bin1 = 0
_state_machine.userdata.drie = 3
_state_machine.userdata.twee = 2
_state_machine.userdata.zero = 0
_state_machine.userdata.offsety_bin2 = 0
_state_machine.userdata.offsetx_bin2 = 0
_state_machine.userdata.yproduct_bin2 = 0
_state_machine.userdata.xproduct_bin2 = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:1623 y:384, x:886 y:382, x:1759 y:258
_sm_offset_verschuiven_0 = OperatableStateMachine(
outcomes=['finished', 'failed', 'end'],
input_keys=[
'bin_id', 'bin1', 'zero', 'twee', 'drie', 'bin2',
'offsetx_set', 'offsety_set', 'plus1', 'offsetx_bin1',
'offsety_bin2', 'offsetx_bin2', 'offsety_bin1',
'xproduct_bin1', 'yproduct_bin1', 'xproduct_bin2',
'yproduct_bin2'
],
output_keys=[
'offsety_bin2', 'offsetx_bin2', 'offsety_bin1', 'offsetx_bin1'
])
with _sm_offset_verschuiven_0:
# x:146 y:40
OperatableStateMachine.add('bin1 vergelijk',
EqualState(),
transitions={
'true': 'set offsetyyy',
'false': 'set offsetyy'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin1',
'value_b': 'bin_id'
})
# x:610 y:50
OperatableStateMachine.add(
'x producten tellen',
AddNumericState(),
transitions={'done': '3 products x bin?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'xproduct_bin1',
'value_b': 'plus1',
'result': 'xproduct_bin1'
})
# x:818 y:45
OperatableStateMachine.add('3 products x bin?',
EqualState(),
transitions={
'true': 'set offsety',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'xproduct_bin1',
'value_b': 'drie'
})
# x:1069 y:54
OperatableStateMachine.add(
'set offsety',
ReplaceState(),
transitions={'done': 'set offsety back to zero'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_set',
'result': 'offsety_bin1'
})
# x:1263 y:51
OperatableStateMachine.add('set offsety back to zero',
ReplaceState(),
transitions={'done': '2 products x?'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsety_bin1'
})
# x:1471 y:55
OperatableStateMachine.add('2 products x?',
EqualState(),
transitions={
'true': 'st offsetx back to zero',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'twee',
'value_b': 'xproduct_bin1'
})
# x:389 y:38
OperatableStateMachine.add(
'set offsetyyy',
AddNumericState(),
transitions={'done': 'x producten tellen'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'offsety_set',
'value_b': 'offsetx_bin1',
'result': 'offsetx_bin1'
})
# x:610 y:161
OperatableStateMachine.add(
'y producten tellen_2',
AddNumericState(),
transitions={'done': '3 products y bin2?'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'yproduct_bin2',
'value_b': 'plus1',
'result': 'yproduct_bin2'
})
# x:818 y:152
OperatableStateMachine.add('3 products y bin2?',
EqualState(),
transitions={
'true': 'set offsety_2',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'yproduct_bin2',
'value_b': 'drie'
})
# x:1066 y:165
OperatableStateMachine.add(
'set offsety_2',
ReplaceState(),
transitions={'done': 'set offsety back to zero_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_set',
'result': 'offsetx_bin2'
})
# x:1263 y:176
OperatableStateMachine.add(
'set offsety back to zero_2',
ReplaceState(),
transitions={'done': '2 products x bin2?'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsety_bin2'
})
# x:1472 y:149
OperatableStateMachine.add('2 products x bin2?',
EqualState(),
transitions={
'true':
'set offsetx back to zero bin2',
'false': 'finished'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'twee',
'value_b': 'xproduct_bin2'
})
# x:389 y:152
OperatableStateMachine.add(
'set offsetyy',
AddNumericState(),
transitions={'done': 'y producten tellen_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'offsety_set',
'value_b': 'offsety_bin2',
'result': 'offsety_bin2'
})
# x:1651 y:50
OperatableStateMachine.add('st offsetx back to zero',
ReplaceState(),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsetx_bin1'
})
# x:1653 y:146
OperatableStateMachine.add('set offsetx back to zero bin2',
ReplaceState(),
transitions={'done': 'end'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'offsetx_bin2'
})
# x:30 y:401, x:130 y:401
_sm_setten_voor_right_1 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'arm_idR', 'move_group_right_arm', 'config_name_Right_Home',
'tool_link_right', 'config_name_RA_pre', 'offset_var_right'
],
output_keys=[
'arm_id_var', 'move_group_var', 'config_name_var',
'tool_link_var', 'config_name_pre_var', 'offset_id'
])
with _sm_setten_voor_right_1:
# x:30 y:40
OperatableStateMachine.add('Set arm id',
ReplaceState(),
transitions={'done': 'Set config name'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_idR',
'result': 'arm_id_var'
})
# x:30 y:117
OperatableStateMachine.add('Set config name',
ReplaceState(),
transitions={'done': 'Set toollink'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_Right_Home',
'result': 'config_name_var'
})
# x:30 y:194
OperatableStateMachine.add('Set toollink',
ReplaceState(),
transitions={'done': 'Set move group'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'tool_link_right',
'result': 'tool_link_var'
})
# x:30 y:271
OperatableStateMachine.add(
'Set move group',
ReplaceState(),
transitions={'done': 'Set config name_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'move_group_right_arm',
'result': 'move_group_var'
})
# x:230 y:266
OperatableStateMachine.add(
'Set config name_2',
ReplaceState(),
transitions={'done': 'set config pre pick'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_var_right',
'result': 'offset_id'
})
# x:233 y:344
OperatableStateMachine.add('laat zien',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'move_group_var'})
# x:438 y:268
OperatableStateMachine.add('set config pre pick',
ReplaceState(),
transitions={'done': 'laat zien'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_RA_pre',
'result': 'config_name_pre_var'
})
# x:30 y:401
_sm_setten_voor_left_2 = OperatableStateMachine(
outcomes=['finished'],
input_keys=[
'arm_idL', 'move_group_left_arm', 'config_name_Left_Home',
'tool_link_left', 'config_name_LA_pre', 'offset_var_left'
],
output_keys=[
'arm_id_var', 'move_group_var', 'config_name_var',
'tool_link_var', 'config_name_pre_var', 'offset_id'
])
with _sm_setten_voor_left_2:
# x:30 y:40
OperatableStateMachine.add('Set arm id',
ReplaceState(),
transitions={'done': 'Set config name'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_idL',
'result': 'arm_id_var'
})
# x:30 y:117
OperatableStateMachine.add('Set config name',
ReplaceState(),
transitions={'done': 'Set toollink'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_Left_Home',
'result': 'config_name_var'
})
# x:30 y:194
OperatableStateMachine.add('Set toollink',
ReplaceState(),
transitions={'done': 'Set move group'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'tool_link_left',
'result': 'tool_link_var'
})
# x:30 y:271
OperatableStateMachine.add('Set move group',
ReplaceState(),
transitions={'done': 'Set offset'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'move_group_left_arm',
'result': 'move_group_var'
})
# x:397 y:386
OperatableStateMachine.add('laat zien',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'move_group_var'})
# x:383 y:274
OperatableStateMachine.add('set config name pre pick',
ReplaceState(),
transitions={'done': 'laat zien'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_LA_pre',
'result': 'config_name_pre_var'
})
# x:209 y:266
OperatableStateMachine.add(
'Set offset',
ReplaceState(),
transitions={'done': 'set config name pre pick'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_var_left',
'result': 'offset_id'
})
with _state_machine:
# x:97 y:17
OperatableStateMachine.add(
'Setten voor Left',
_sm_setten_voor_left_2,
transitions={'finished': 'Left product'},
autonomy={'finished': Autonomy.Inherit},
remapping={
'arm_idL': 'arm_idL',
'move_group_left_arm': 'move_group_left_arm',
'config_name_Left_Home': 'config_name_Left_Home',
'tool_link_left': 'tool_link_left',
'config_name_LA_pre': 'config_name_LA_pre',
'offset_var_left': 'offset_var_left',
'arm_id_var': 'arm_id_var',
'move_group_var': 'move_group_var',
'config_name_var': 'config_name_var',
'tool_link_var': 'tool_link_var',
'config_name_pre_var': 'config_name_pre_var',
'offset_id': 'offset_id'
})
# x:190 y:708
OperatableStateMachine.add(
'Setten voor Right',
_sm_setten_voor_right_1,
transitions={
'finished': 'Right product',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'arm_idR': 'arm_idR',
'move_group_right_arm': 'move_group_right_arm',
'config_name_Right_Home': 'config_name_Right_Home',
'tool_link_right': 'tool_link_right',
'config_name_RA_pre': 'config_name_RA_pre',
'offset_var_right': 'offset_var_right',
'arm_id_var': 'arm_id_var',
'move_group_var': 'move_group_var',
'config_name_var': 'config_name_var',
'tool_link_var': 'tool_link_var',
'config_name_pre_var': 'config_name_pre_var',
'offset_id': 'offset_id'
})
# x:77 y:172
OperatableStateMachine.add('Left product',
EqualState(),
transitions={
'true': 'bin1',
'false': 'bin2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'offset_gasket',
'value_b': 'offset_id'
})
# x:311 y:29
OperatableStateMachine.add('bin1',
ReplaceState(),
transitions={'done': 'Zie bin1_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin1',
'result': 'bin_id'
})
# x:300 y:252
OperatableStateMachine.add('bin2',
ReplaceState(),
transitions={'done': 'Zie bin1_2_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin2',
'result': 'bin_id'
})
# x:116 y:534
OperatableStateMachine.add('Right product',
EqualState(),
transitions={
'true': 'bin1_2',
'false': 'bin2_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'offset_gasket',
'value_b': 'offset_id'
})
# x:1673 y:531
OperatableStateMachine.add('turn off vacuum',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'Move to home',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_var'})
# x:299 y:610
OperatableStateMachine.add('bin1_2',
ReplaceState(),
transitions={'done': 'Zie bin1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin1',
'result': 'bin_id'
})
# x:294 y:377
OperatableStateMachine.add('bin2_2',
ReplaceState(),
transitions={'done': 'Zie bin2_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin2',
'result': 'bin_id'
})
# x:731 y:21
OperatableStateMachine.add('gantry bin pose',
ReplaceState(),
transitions={'done': 'Move to bin'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_L_bin1',
'result': 'config_name_bin1_var'
})
# x:722 y:342
OperatableStateMachine.add('gantry bin pose_2',
ReplaceState(),
transitions={'done': 'Move to bin'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'config_name_R_bin1',
'result': 'config_name_bin1_var'
})
# x:1691 y:675
OperatableStateMachine.add('Move to home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Offset verschuiven',
'planning_failed': 'retry_4',
'control_failed': 'retry_4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_var',
'move_group': 'move_group_var',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:987 y:701
OperatableStateMachine.add('Right robot end',
EqualState(),
transitions={
'true': 'finished',
'false': 'Setten voor Right'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'move_group_right_arm',
'value_b': 'move_group_var'
})
# x:954 y:146
OperatableStateMachine.add('retry',
WaitState(wait_time=0.1),
transitions={'done': 'Move to bin'},
autonomy={'done': Autonomy.Off})
# x:1578 y:610
OperatableStateMachine.add('retry_4',
WaitState(wait_time=0.1),
transitions={'done': 'Move to home'},
autonomy={'done': Autonomy.Off})
# x:956 y:21
OperatableStateMachine.add('Move to bin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Check for bin',
'planning_failed': 'retry',
'control_failed': 'retry',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1_var',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1691 y:248
OperatableStateMachine.add(
'Computebin right',
ComputeDropBinsState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move to drop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_var',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_var',
'pose': 'pose',
'offset': 'offset_id',
'rotation': 'rotation',
'offsety': 'offsety',
'offsetx': 'offsetx',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1550 y:336
OperatableStateMachine.add(
'Computebin left',
ComputeDropBinsState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move to drop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_var',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_var',
'pose': 'pose',
'offset': 'offset_id',
'rotation': 'rotation',
'offsety': 'offsety',
'offsetx': 'offsetx',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:516 y:599
OperatableStateMachine.add('Zie bin1',
GetObjectPoseState(
object_frame='bin1_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:529 y:34
OperatableStateMachine.add('Zie bin1_2',
GetObjectPoseState(
object_frame='bin1_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:534 y:239
OperatableStateMachine.add('Zie bin1_2_2',
GetObjectPoseState(
object_frame='bin2_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:523 y:373
OperatableStateMachine.add('Zie bin2_2',
GetObjectPoseState(
object_frame='bin2_frame',
ref_frame='world'),
transitions={
'continue': 'gantry bin pose',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose'})
# x:1668 y:30
OperatableStateMachine.add('Right robot end_2',
EqualState(),
transitions={
'true': 'Computebin right',
'false': 'Computebin left'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'move_group_right_arm',
'value_b': 'move_group_var'
})
# x:1657 y:420
OperatableStateMachine.add('Move to drop',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'turn off vacuum',
'planning_failed': 'failed',
'control_failed': 'turn off vacuum'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_var',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1428 y:672
OperatableStateMachine.add('Offset verschuiven',
_sm_offset_verschuiven_0,
transitions={
'finished': 'Right robot end',
'failed': 'failed',
'end': 'end'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit,
'end': Autonomy.Inherit
},
remapping={
'bin_id': 'bin_id',
'bin1': 'bin1',
'zero': 'zero',
'twee': 'twee',
'drie': 'drie',
'bin2': 'bin2',
'offsetx_set': 'offsetx_set',
'offsety_set': 'offsety_set',
'plus1': 'plus1',
'offsetx_bin1': 'offsetx_bin1',
'offsety_bin2': 'offsety_bin2',
'offsetx_bin2': 'offsetx_bin2',
'offsety_bin1': 'offsety_bin1',
'xproduct_bin1': 'xproduct_bin1',
'yproduct_bin1': 'yproduct_bin1',
'xproduct_bin2': 'xproduct_bin2',
'yproduct_bin2': 'yproduct_bin2'
})
# x:1156 y:3
OperatableStateMachine.add('Check for bin',
EqualState(),
transitions={
'true': 'set offset voor bin 1',
'false': 'set offset voor bin 2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin1',
'value_b': 'bin_id'
})
# x:1406 y:0
OperatableStateMachine.add(
'set offset voor bin 1',
ReplaceState(),
transitions={'done': 'set offset voor y bin 1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_bin1',
'result': 'offsetx'
})
# x:1441 y:64
OperatableStateMachine.add(
'set offset voor y bin 1',
ReplaceState(),
transitions={'done': 'Right robot end_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsety_bin1',
'result': 'offsety'
})
# x:1236 y:110
OperatableStateMachine.add(
'set offset voor bin 2',
ReplaceState(),
transitions={'done': 'set offset voor y bin 2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsetx_bin2',
'result': 'offsetx'
})
# x:1425 y:141
OperatableStateMachine.add(
'set offset voor y bin 2',
ReplaceState(),
transitions={'done': 'Right robot end_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offsety_bin2',
'result': 'offsety'
})
return _state_machine
def create(self):
# x:1166 y:283, x:970 y:277
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type'],
output_keys=['arm', 'part_pose'])
_state_machine.userdata.bin = 'part_type'
_state_machine.userdata.bin1 = 'gear_part'
_state_machine.userdata.bin2 = 'piston_rod'
_state_machine.userdata.bin3 = ''
_state_machine.userdata.bin4 = ''
_state_machine.userdata.bin5 = 'pulley_part'
_state_machine.userdata.bin6 = 'gastket_part'
_state_machine.userdata.arm = ''
_state_machine.userdata.arm1 = '/ariac/arm1'
_state_machine.userdata.arm2 = '/ariac/arm2'
_state_machine.userdata.ref_frame_bin2 = 'arm2_linear_arm_actuator'
_state_machine.userdata.ref_frame_bin3 = 'arm1_linear_arm_actuator'
_state_machine.userdata.ref_frame_bin4 = 'arm1_linear_arm_actuator'
_state_machine.userdata.ref_frame_bin5 = 'arm1_linear_arm_actuator'
_state_machine.userdata.ref_frame_bin6 = 'arm1_linear_arm_actuator'
_state_machine.userdata.ref_frame_bin1 = 'arm2_linear_arm_actuator'
_state_machine.userdata.camera_topic_bin1 = '/ariac/logical_camera_1'
_state_machine.userdata.camera_topic_bin2 = '/ariac/logical_camera_2'
_state_machine.userdata.camera_topic_bin3 = '/ariac/logical_camera_3'
_state_machine.userdata.camera_topic_bin4 = '/ariac/logical_camera_4'
_state_machine.userdata.camera_topic_bin5 = '/ariac/logical_camera_5'
_state_machine.userdata.camera_topic_bin6 = '/ariac/logical_camera_6'
_state_machine.userdata.camera_frame_bin1 = 'locical_camera_1_frame'
_state_machine.userdata.camera_frame_bin2 = 'locical_camera_2_frame'
_state_machine.userdata.camera_frame_bin3 = 'locical_camera_3_frame'
_state_machine.userdata.camera_frame_bin4 = 'locical_camera_4_frame'
_state_machine.userdata.camera_frame_bin5 = 'locical_camera_5_frame'
_state_machine.userdata.camera_frame_bin6 = 'locical_camera_6_frame'
_state_machine.userdata.part_pose = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.config_name_bin1 = 'bin1PreGrasp'
_state_machine.userdata.config_name_bin2 = 'bin2PreGrasp'
_state_machine.userdata.config_name_bin3 = 'bin3PreGrasp'
_state_machine.userdata.config_name_bin4 = 'bin4PreGrasp'
_state_machine.userdata.config_name_bin5 = 'bin5PreGrasp'
_state_machine.userdata.config_name_bin6 = 'bin6PreGrasp'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.move_group = 'manipulator'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:32 y:490
OperatableStateMachine.add('StateBin6',
EqualState(),
transitions={
'true': 'Bin6',
'false': 'StateBin5'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin',
'value_b': 'bin6'
})
# x:30 y:132
OperatableStateMachine.add('StateBin2',
EqualState(),
transitions={
'true': 'Bin2',
'false': 'StateBin1'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin',
'value_b': 'bin2'
})
# x:30 y:224
OperatableStateMachine.add('Statebin3',
EqualState(),
transitions={
'true': 'Bin3',
'false': 'StateBin2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin',
'value_b': 'bin3'
})
# x:30 y:316
OperatableStateMachine.add('StateBin4',
EqualState(),
transitions={
'true': 'Bin4',
'false': 'Statebin3'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin',
'value_b': 'bin4'
})
# x:30 y:408
OperatableStateMachine.add('StateBin5',
EqualState(),
transitions={
'true': 'Bin5',
'false': 'StateBin4'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin',
'value_b': 'bin5'
})
# x:193 y:66
OperatableStateMachine.add(
'Bin1',
ReplaceState(),
transitions={'done': 'DetectCameraPart1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm',
'result': 'arm2'
})
# x:192 y:209
OperatableStateMachine.add(
'Bin3',
ReplaceState(),
transitions={'done': 'DetectCameraPart3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm',
'result': 'arm1'
})
# x:192 y:286
OperatableStateMachine.add(
'Bin4',
ReplaceState(),
transitions={'done': 'DetectCameraPart4'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm',
'result': 'arm1'
})
# x:192 y:363
OperatableStateMachine.add(
'Bin5',
ReplaceState(),
transitions={'done': 'DetectCameraPart5'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm',
'result': 'arm1'
})
# x:192 y:440
OperatableStateMachine.add(
'Bin6',
ReplaceState(),
transitions={'done': 'DetectCameraPart6'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm',
'result': 'arm1'
})
# x:192 y:132
OperatableStateMachine.add(
'Bin2',
ReplaceState(),
transitions={'done': 'DetectCameraPart2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm',
'result': 'arm2'
})
# x:355 y:102
OperatableStateMachine.add(
'DetectCameraPart1',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'MoveR2PreGrasp_Bin1',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame_bin1',
'camera_topic': 'camera_topic_bin1',
'camera_frame': 'camera_frame_bin1',
'part': 'part_type',
'pose': 'part_pose'
})
# x:355 y:182
OperatableStateMachine.add(
'DetectCameraPart2',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'MoveR2PreGrasp_Bin2',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame_bin2',
'camera_topic': 'camera_topic_bin2',
'camera_frame': 'camera_frame_bin2',
'part': 'part_type',
'pose': 'part_pose'
})
# x:354 y:246
OperatableStateMachine.add(
'DetectCameraPart3',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'MoveR1PreGrasp_Bin3',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame_bin3',
'camera_topic': 'camera_topic_bin3',
'camera_frame': 'camera_frame_bin3',
'part': 'part_type',
'pose': 'part_pose'
})
# x:355 y:310
OperatableStateMachine.add(
'DetectCameraPart4',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'MoveR1PreGrasp_Bin4',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame_bin4',
'camera_topic': 'camera_topic_bin4',
'camera_frame': 'camera_frame_bin4',
'part': 'part_type',
'pose': 'part_pose'
})
# x:355 y:373
OperatableStateMachine.add(
'DetectCameraPart5',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'MoveR1PreGrasp_Bin5',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame_bin5',
'camera_topic': 'camera_topic_bin5',
'camera_frame': 'camera_frame_bin5',
'part': 'part_type',
'pose': 'part_pose'
})
# x:355 y:445
OperatableStateMachine.add(
'DetectCameraPart6',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'MoveR1PreGrasp_Bin6',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame_bin6',
'camera_topic': 'camera_topic_bin6',
'camera_frame': 'camera_frame_bin6',
'part': 'part_type',
'pose': 'part_pose'
})
# x:520 y:459
OperatableStateMachine.add('MoveR1PreGrasp_Bin6',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed':
'MoveR1PreGrasp_Bin6',
'control_failed':
'MoveR1PreGrasp_Bin6',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_bin6',
'move_group': 'move_group',
'move_group_prefix': 'arm1',
'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:525 y:127
OperatableStateMachine.add('MoveR2PreGrasp_Bin1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed':
'MoveR2PreGrasp_Bin1',
'control_failed':
'MoveR2PreGrasp_Bin1',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_bin1',
'move_group': 'move_group',
'move_group_prefix': 'arm2',
'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:525 y:196
OperatableStateMachine.add('MoveR2PreGrasp_Bin2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed':
'MoveR2PreGrasp_Bin2',
'control_failed':
'MoveR2PreGrasp_Bin2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_bin2',
'move_group': 'move_group',
'move_group_prefix': 'arm2',
'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:526 y:259
OperatableStateMachine.add('MoveR1PreGrasp_Bin3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed':
'MoveR1PreGrasp_Bin3',
'control_failed':
'MoveR1PreGrasp_Bin3',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_bin3',
'move_group': 'move_group',
'move_group_prefix': 'arm1',
'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:527 y:322
OperatableStateMachine.add('MoveR1PreGrasp_Bin4',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed':
'MoveR1PreGrasp_Bin4',
'control_failed':
'MoveR1PreGrasp_Bin4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_bin4',
'move_group': 'move_group',
'move_group_prefix': 'arm1',
'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:524 y:390
OperatableStateMachine.add('MoveR1PreGrasp_Bin5',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed':
'MoveR1PreGrasp_Bin5',
'control_failed':
'MoveR1PreGrasp_Bin5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_bin5',
'move_group': 'move_group',
'move_group_prefix': 'arm1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:30 y:40
OperatableStateMachine.add('StateBin1',
EqualState(),
transitions={
'true': 'Bin1',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin',
'value_b': 'bin1'
})
return _state_machine
def create(self):
# x:867 y:12, x:483 y:355
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.order_id = ''
_state_machine.userdata.shipments = []
_state_machine.userdata.number_of_shipments = 0
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.shipment_index = 0
_state_machine.userdata.agv_id = ''
_state_machine.userdata.number_of_products = 0
_state_machine.userdata.products = []
_state_machine.userdata.product_index = 0
_state_machine.userdata.part_type = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.increment = 1
_state_machine.userdata.Null = 0
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.old_order_id = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:49 y:117
OperatableStateMachine.add('startassignment',
StartAssignment(),
transitions={'continue': 'get_order'},
autonomy={'continue': Autonomy.Off})
# x:207 y:119
OperatableStateMachine.add('get_order',
GetOrderState(),
transitions={'continue': 'TestLastOrder'},
autonomy={'continue': Autonomy.Off},
remapping={'order_id': 'order_id', 'shipments': 'shipments', 'number_of_shipments': 'number_of_shipments'})
# x:410 y:121
OperatableStateMachine.add('getProductsfromshipment',
GetProductsFromShipmentState(),
transitions={'continue': 'ShipmentID', 'invalid_index': 'failed'},
autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'shipments': 'shipments', 'index': 'shipment_index', 'shipment_type': 'shipment_type', 'agv_id': 'agv_id', 'products': 'products', 'number_of_products': 'number_of_products'})
# x:848 y:122
OperatableStateMachine.add('transport_part_form_bin_to_agv_state',
self.use_behavior(transport_part_form_bin_to_agv_stateSM, 'transport_part_form_bin_to_agv_state'),
transitions={'finished': 'IncreaseProductIndex', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'part_type': 'part_type', 'agv_id': 'agv_id', 'part_pose': 'part_pose', 'pose': 'pose'})
# x:649 y:120
OperatableStateMachine.add('GetPartinfo',
GetPartFromProductsState(),
transitions={'continue': 'transport_part_form_bin_to_agv_state', 'invalid_index': 'failed'},
autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off},
remapping={'products': 'products', 'index': 'product_index', 'type': 'part_type', 'pose': 'part_pose'})
# x:928 y:241
OperatableStateMachine.add('IncreaseProductIndex',
AddNumericState(),
transitions={'done': 'CheckProduct_index'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'increment', 'result': 'product_index'})
# x:934 y:323
OperatableStateMachine.add('CheckProduct_index',
EqualState(),
transitions={'true': 'SETnull', 'false': 'GetPartinfo'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'product_index', 'value_b': 'number_of_products'})
# x:770 y:514
OperatableStateMachine.add('IncreaseShipmentIndex',
AddNumericState(),
transitions={'done': 'Checkshipmentindex'},
autonomy={'done': Autonomy.Off},
remapping={'value_a': 'shipment_index', 'value_b': 'increment', 'result': 'shipment_index'})
# x:396 y:499
OperatableStateMachine.add('Checkshipmentindex',
EqualState(),
transitions={'true': 'get_order', 'false': 'notify_shipment_ready'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'number_of_shipments', 'value_b': 'shipment_index'})
# x:990 y:530
OperatableStateMachine.add('SETnull',
ReplaceState(),
transitions={'done': 'IncreaseShipmentIndex'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'Null', 'result': 'product_index'})
# x:721 y:355
OperatableStateMachine.add('notify_shipment_ready',
self.use_behavior(notify_shipment_readySM, 'notify_shipment_ready'),
transitions={'finished': 'getProductsfromshipment', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:388 y:13
OperatableStateMachine.add('TestLastOrder',
EqualState(),
transitions={'true': 'finished', 'false': 'RememberOldOrder'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'order_id', 'value_b': 'old_order_id'})
# x:632 y:44
OperatableStateMachine.add('RememberOldOrder',
ReplaceState(),
transitions={'done': 'getProductsfromshipment'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'order_id', 'result': 'old_order_id'})
# x:40 y:265
OperatableStateMachine.add('ShipmentID',
MessageState(),
transitions={'continue': 'GetPartinfo'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'shipment_type'})
return _state_machine
def create(self):
# x:27 y:469, x:793 y:325
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['pose_on_agv', 'part_type'],
output_keys=['pose_on_agv_l', 'pose_on_agv_r'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.config_name = ''
_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 = 'gantry'
_state_machine.userdata.part_type = ''
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link_r = 'right_ee_link'
_state_machine.userdata.pose = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_groupR = 'Right_Arm'
_state_machine.userdata.arm_id_r = 'Right_Arm'
_state_machine.userdata.Grasp5 = 'bingr5Grasp'
_state_machine.userdata.arm_id_l = 'Left_Arm'
_state_machine.userdata.move_groupL = 'Left_Arm'
_state_machine.userdata.tool_link_l = 'left_ee_link'
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.pose_on_agv_l = []
_state_machine.userdata.pose_on_agv_r = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:204 y:22
OperatableStateMachine.add('move_home_pick',
self.use_behavior(
move_home_pickSM, 'move_home_pick'),
transitions={
'finished': 'Find_product_location',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:660 y:108
OperatableStateMachine.add('move_gantry_bin_gr1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'move_gantry_bin_gr1_2',
'planning_failed': 'Waitretry',
'control_failed': 'Waitretry',
'param_error': 'Waitretry'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'bin',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:471 y:43
OperatableStateMachine.add('Find_product_location',
FindCorrectBin(time_out=0.5),
transitions={
'continue': 'move_gantry_bin_gr1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'bin': 'bin',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'ref_frame': 'ref_frame'
})
# x:1089 y:258
OperatableStateMachine.add(
'compute_pick _r',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_groupR',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_r',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1096 y:74
OperatableStateMachine.add(
'find part',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Offset bepalen',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:1300 y:88
OperatableStateMachine.add('Offset bepalen',
DecideOffsetProduct(target_time=0.5),
transitions={
'succes': 'gripper check',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'part_offset': 'part_offset'
})
# x:1024 y:463
OperatableStateMachine.add('Gripper_enable',
GripperControl(enable=True),
transitions={
'continue': 'pose safe r',
'failed': 'Move to pick',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_r'})
# x:648 y:701
OperatableStateMachine.add('move_home_robot_r',
self.use_behavior(
move_home_robot_rSM,
'move_home_robot_r'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:1064 y:360
OperatableStateMachine.add('Move to pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_enable',
'planning_failed': 'failed',
'control_failed': 'Gripper_enable'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_groupR',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:870 y:107
OperatableStateMachine.add('move_gantry_bin_gr1_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'find part',
'planning_failed': 'Waitretry_2',
'control_failed': 'Waitretry_2',
'param_error': 'Waitretry_2'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'Grasp5',
'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:882 y:10
OperatableStateMachine.add(
'Waitretry_2',
WaitState(wait_time=2),
transitions={'done': 'move_gantry_bin_gr1_2'},
autonomy={'done': Autonomy.Off})
# x:686 y:12
OperatableStateMachine.add(
'Waitretry',
WaitState(wait_time=2),
transitions={'done': 'move_gantry_bin_gr1'},
autonomy={'done': Autonomy.Off})
# x:1306 y:173
OperatableStateMachine.add('gripper check',
GripperActiveCheck(),
transitions={
'Left': 'compute_pick _l',
'Right': 'compute_pick _r',
'failed': 'failed',
'Full': 'move_home_pick'
},
autonomy={
'Left': Autonomy.Off,
'Right': Autonomy.Off,
'failed': Autonomy.Off,
'Full': Autonomy.Off
},
remapping={
'arm_id': 'arm_id',
'tool_link': 'tool_link',
'move_group': 'move_group'
})
# x:1527 y:231
OperatableStateMachine.add(
'compute_pick _l',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick_l',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_groupL',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_l',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1573 y:425
OperatableStateMachine.add('Gripper_enable_2',
GripperControl(enable=True),
transitions={
'continue': 'pose safe l',
'failed': 'Move to pick_l',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_l'})
# x:1530 y:319
OperatableStateMachine.add('Move to pick_l',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_enable_2',
'planning_failed': 'failed',
'control_failed': 'Gripper_enable_2'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_groupL',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:865 y:537
OperatableStateMachine.add(
'pose safe r',
ReplaceState(),
transitions={'done': 'move_home_robot_r'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv_r'
})
# x:1502 y:553
OperatableStateMachine.add(
'pose safe l',
ReplaceState(),
transitions={'done': 'move_home_robot_r'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv_l'
})
return _state_machine
def create(self):
# x:615 y:109, x:113 y:271
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['arm_id'],
output_keys=['move_group_prefix'])
_state_machine.userdata.prefix_arm1 = '/ariac/arm1'
_state_machine.userdata.prefix_arm2 = '/ariac/arm2'
_state_machine.userdata.arm1 = 'arm1'
_state_machine.userdata.arm2 = 'arm2'
_state_machine.userdata.arm_id = ''
_state_machine.userdata.move_group_prefix = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:54 y:52
OperatableStateMachine.add('compare_1',
EqualState(),
transitions={
'true': 'Value_1',
'false': 'compare_1_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'arm_id',
'value_b': 'arm1'
})
# x:319 y:58
OperatableStateMachine.add('Value_1',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_id',
'result': 'prefix_arm1'
})
# x:53 y:150
OperatableStateMachine.add('compare_1_2',
EqualState(),
transitions={
'true': 'Value_1_2',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'arm_id',
'value_b': 'arm2'
})
# x:318 y:155
OperatableStateMachine.add('Value_1_2',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm_id',
'result': 'prefix_arm2'
})
return _state_machine
def create(self):
# x:1393 y:668, x:704 y:381
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type', 'agv_id', 'pose_on_agv'])
_state_machine.userdata.part_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.config_name_R1Bin1Pre = 'R1Bin1Pre'
_state_machine.userdata.config_name_R1AGVPre = 'R1AGVPre'
_state_machine.userdata.config_name_R2Bin5Pre = 'R2Bin5Pre'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.arm2_gebruiken = '/ariac/arm2'
_state_machine.userdata.part_pose = []
_state_machine.userdata.part = 'gasket_part'
_state_machine.userdata.part_offset = 0.030
_state_machine.userdata.part_drop_offset = 0.1
_state_machine.userdata.conveyor_power = 100.0
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.camera_frame = 'logical_camera_3_frame'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_3'
_state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.bin_location = []
_state_machine.userdata.material_locations = []
_state_machine.userdata.zero_value = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:12 y:198
OperatableStateMachine.add(
'get part location',
GetMaterialLocationsState(),
transitions={'continue': 'bin location'},
autonomy={'continue': Autonomy.Off},
remapping={
'part': 'part_type',
'material_locations': 'material_locations'
})
# x:305 y:37
OperatableStateMachine.add(
'MoseMessage',
MessageState(),
transitions={'continue': 'detect object'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'pose_on_agv'})
# x:171 y:37
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'MoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:681 y:38
OperatableStateMachine.add('pre grasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'compute grasp',
'planning_failed': 'failed',
'control_failed': 'compute grasp',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R1Bin1Pre',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1433 y:179
OperatableStateMachine.add('move_agv',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'gripper uit',
'planning_failed': 'failed',
'control_failed': 'gripper uit',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R1AGVPre',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1255 y:51
OperatableStateMachine.add('gripper aan',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'wait',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1405 y:434
OperatableStateMachine.add('robot_2_selecteren',
ReplaceState(),
transitions={'done': 'robot_2_bewegen'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm2_gebruiken',
'result': 'move_group_prefix'
})
# x:1424 y:526
OperatableStateMachine.add('robot_2_bewegen',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R2Bin5Pre',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:465 y:35
OperatableStateMachine.add(
'detect object',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'pre grasp',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part',
'pose': 'pose'
})
# x:862 y:38
OperatableStateMachine.add(
'compute grasp',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'pick_positie',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1475 y:50
OperatableStateMachine.add('wait',
WaitState(wait_time=1),
transitions={'done': 'move_agv'},
autonomy={'done': Autonomy.Off})
# x:1414 y:297
OperatableStateMachine.add('gripper uit',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'robot_2_selecteren',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1047 y:40
OperatableStateMachine.add('pick_positie',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'gripper aan',
'planning_failed': 'failed',
'control_failed': 'gripper aan'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:150 y:300
OperatableStateMachine.add('bin location',
GetItemFromListState(),
transitions={
'done': 'AgvIdMessage',
'invalid_index': 'failed'
},
autonomy={
'done': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'list': 'material_locations',
'index': 'zero_value',
'item': 'bin_location'
})
# x:176 y:126
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
return _state_machine