def create(self): # x:1056 y:73, x:588 y:324 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.agv_id = 'agv1' _state_machine.userdata.part_type = 'gear_part' _state_machine.userdata.pose_on_agv = [] _state_machine.userdata.joint_values = [] _state_machine.userdata.joint_names = [] _state_machine.userdata.offset = 0.1 _state_machine.userdata.move_group = 'manipulator' _state_machine.userdata.move_group_prefix = '/ariac/arm1' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:52 y:43 OperatableStateMachine.add( 'StartAssignment', StartAssignment(), transitions={ 'continue': 'transport_part_form_bin_to_agv_state' }, autonomy={'continue': Autonomy.Off}) # x:822 y:60 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:510 y:50 OperatableStateMachine.add( 'transport_part_form_bin_to_agv_state', self.use_behavior(transport_part_form_bin_to_agv_stateSM, 'transport_part_form_bin_to_agv_state'), transitions={ 'finished': 'EndAssignment', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'part_type': 'part_type', 'agv_id': 'agv_id', 'pose_on_agv': 'pose_on_agv', 'pose': 'pose' }) return _state_machine
def create(self): part_list = ['gasket_part', 'piston_rod_part', 'gear_part', ' pulley_part', 'disk_part'] # x:842 y:52, x:158 y:179 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.camera_ref_frame = "arm1_linear_arm_actuator" _state_machine.userdata.camera_topic = "/ariac/logical_camera_1" _state_machine.userdata.camera_frame = "logical_camera_1_frame" _state_machine.userdata.pose = [] _state_machine.userdata.part = '' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:33 y:38 OperatableStateMachine.add('StaerAssignment', StartAssignment(), transitions={'continue': 'DetectFirstPartCamera'}, autonomy={'continue': Autonomy.Off}) # x:379 y:40 OperatableStateMachine.add('PartMessage', MessageState(), transitions={'continue': 'PoseMessage'}, autonomy={'continue': Autonomy.Off}, remapping={'message': 'part'}) # x:523 y:40 OperatableStateMachine.add('PoseMessage', MessageState(), transitions={'continue': 'EndAssignment'}, autonomy={'continue': Autonomy.Off}, remapping={'message': 'pose'}) # x:666 y:37 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:170 y:40 OperatableStateMachine.add('DetectFirstPartCamera', DetectFirstPartCameraAriacState(part_list=part_list, time_out=0.5), transitions={'continue': 'PartMessage', 'failed': 'failed', 'not_found': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'ref_frame': 'camera_ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part', 'pose': 'pose'}) return _state_machine
def create(self): # x:599 y:55, x:321 y:144 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.agv_id = 'agv1' _state_machine.userdata.shipment_type = 'order_0_shipment_0' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'StartAssignment', StartAssignment(), transitions={'continue': 'SubmitShipment'}, autonomy={'continue': Autonomy.Off}) # x:443 y:42 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:322 y:42 OperatableStateMachine.add('Wait', WaitState(wait_time=30), transitions={'done': 'EndAssignment'}, autonomy={'done': Autonomy.Off}) # x:167 y:42 OperatableStateMachine.add('SubmitShipment', SubmitShipmentState(), transitions={ 'continue': 'Wait', 'fail': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'agv_id': 'agv_id', 'shipment_type': 'shipment_type', 'inspection_result': 'inspection_result' }) return _state_machine
def create(self): # x:649 y:50, x:296 y:171 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.agv_id = 'agv1' _state_machine.userdata.part_type = 'gear_part' _state_machine.userdata.pose_on_agv = [] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:52 y:43 OperatableStateMachine.add( 'StartAssignment', StartAssignment(), transitions={ 'continue': 'transport_part_form_bin_to_agv_state' }, autonomy={'continue': Autonomy.Off}) # x:445 y:45 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:188 y:44 OperatableStateMachine.add( 'transport_part_form_bin_to_agv_state', self.use_behavior(transport_part_form_bin_to_agv_stateSM, 'transport_part_form_bin_to_agv_state'), transitions={ 'finished': 'EndAssignment', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'part_type': 'part_type', 'agv_id': 'agv_id', 'pose_on_agv': 'pose_on_agv' }) return _state_machine
def create(self): # x:705 y:41 _state_machine = OperatableStateMachine(outcomes=['finished']) _state_machine.userdata.MaterialLocations = [] _state_machine.userdata.Part = 'piston_rod_part' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:34 y:25 OperatableStateMachine.add( 'StartAssignment', StartAssignment(), transitions={'continue': 'GetMaterialsLocations'}, autonomy={'continue': Autonomy.Off}) # x:362 y:28 OperatableStateMachine.add( 'MaterialsLocationMessage', MessageState(), transitions={'continue': 'EndAssignment'}, autonomy={'continue': Autonomy.Off}, remapping={'message': 'MaterialLocations'}) # x:541 y:29 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:176 y:26 OperatableStateMachine.add( 'GetMaterialsLocations', GetMaterialLocationsState(), transitions={'continue': 'MaterialsLocationMessage'}, autonomy={'continue': Autonomy.Off}, remapping={ 'part': 'Part', 'material_locations': 'MaterialLocations' }) return _state_machine
def create(self): # x:636 y:50, x:263 y:129 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.arm = 'arm1' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:49 y:39 OperatableStateMachine.add( 'StartAssignment', StartAssignment(), transitions={ 'continue': 'transport_part_from_belt_to_bin_state' }, autonomy={'continue': Autonomy.Off}) # x:472 y:44 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:196 y:40 OperatableStateMachine.add( 'transport_part_from_belt_to_bin_state', self.use_behavior(transport_part_from_belt_to_bin_stateSM, 'transport_part_from_belt_to_bin_state'), transitions={ 'finished': 'EndAssignment', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'arm': 'arm'}) return _state_machine
def create(self): # x:821 y:56, x:1214 y:358 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.order_id = '' _state_machine.userdata.shipments = [] _state_machine.userdata.number_of_shipments = 0 _state_machine.userdata.old_order_id = '' _state_machine.userdata.shipment_index = 0 _state_machine.userdata.shipment_type = '' _state_machine.userdata.agv_id = '' _state_machine.userdata.products = [] _state_machine.userdata.number_of_products = 0 _state_machine.userdata.product_index = 0 _state_machine.userdata.part_type = '' _state_machine.userdata.material_locations = [] _state_machine.userdata.bin_index = 0 _state_machine.userdata.bin_id = '' _state_machine.userdata.one_value = 1 _state_machine.userdata.zero_value = 0 _state_machine.userdata.arm_right = 'right_arm' _state_machine.userdata.arm_left = 'left_arm' _state_machine.userdata.product_location1 = '' _state_machine.userdata.product_location2 = '' _state_machine.userdata.home_rightarm_id = 'Right_PreGrasp' _state_machine.userdata.move_group_right = 'Right_Arm' _state_machine.userdata.move_group_prefix = '/ariac/gantry' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' _state_machine.userdata.home_leftarm_id = 'Left_PreGrasp' _state_machine.userdata.move_group_left = 'Left_Arm' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:57 y:48 OperatableStateMachine.add('StartAssignment', StartAssignment(), transitions={'continue': 'GetOrder'}, autonomy={'continue': Autonomy.Off}) # x:218 y:48 OperatableStateMachine.add('GetOrder', GetOrderState(), transitions={'continue': 'Test_LastOrder'}, autonomy={'continue': Autonomy.Off}, remapping={'order_id': 'order_id', 'shipments': 'shipments', 'number_of_shipments': 'number_of_shipments'}) # x:422 y:47 OperatableStateMachine.add('Test_LastOrder', EqualState(), transitions={'true': 'EndAssignment', 'false': 'Remember_OldOrder'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'value_a': 'order_id', 'value_b': 'old_order_id'}) # x:614 y:124 OperatableStateMachine.add('Remember_OldOrder', ReplaceState(), transitions={'done': 'GetShipment'}, autonomy={'done': Autonomy.Off}, remapping={'value': 'order_id', 'result': 'old_order_id'}) # x:622 y:47 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:801 y:119 OperatableStateMachine.add('GetShipment', GetProductsFromShipmentState(), transitions={'continue': 'GetPart', 'invalid_index': 'failed'}, autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off}, remapping={'shipments': 'shipments', 'index': 'shipment_index', 'shipment_type': 'shipment_type', 'agv_id': 'agv_id', 'products': 'products', 'number_of_products': 'number_of_products'}) # x:1023 y:119 OperatableStateMachine.add('GetPart', GetPartFromProductsState(), transitions={'continue': 'MaterialLocation', 'invalid_index': 'failed'}, autonomy={'continue': Autonomy.Off, 'invalid_index': Autonomy.Off}, remapping={'products': 'products', 'index': 'product_index', 'type': 'part_type', 'pose': 'part_pose'}) # x:1217 y:116 OperatableStateMachine.add('MaterialLocation', GetMaterialLocationsState(), transitions={'continue': 'GetBin'}, autonomy={'continue': Autonomy.Off}, remapping={'part': 'part_type', 'material_locations': 'material_locations'}) # x:1403 y:114 OperatableStateMachine.add('GetBin', GetItemFromListState(), transitions={'done': 'CheckAttachedRight', 'invalid_index': 'failed'}, autonomy={'done': Autonomy.Off, 'invalid_index': Autonomy.Off}, remapping={'list': 'material_locations', 'index': 'bin_index', 'item': 'bin_id'}) # x:1361 y:507 OperatableStateMachine.add('IncrementProductIndex', AddNumericState(), transitions={'done': 'EndProduct'}, autonomy={'done': Autonomy.Off}, remapping={'value_a': 'product_index', 'value_b': 'one_value', 'result': 'product_index'}) # x:1360 y:585 OperatableStateMachine.add('EndProduct', EqualState(), transitions={'true': 'HomepositieRightArm', 'false': 'CheckAttachedRight2'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'value_a': 'product_index', 'value_b': 'number_of_products'}) # x:95 y:565 OperatableStateMachine.add('ResetProductIndex', ReplaceState(), transitions={'done': 'IncrementShipmentIndex'}, autonomy={'done': Autonomy.Off}, remapping={'value': 'zero_value', 'result': 'product_index'}) # x:98 y:469 OperatableStateMachine.add('IncrementShipmentIndex', AddNumericState(), transitions={'done': 'Notify_Shipment_Ready'}, autonomy={'done': Autonomy.Off}, remapping={'value_a': 'shipment_index', 'value_b': 'one_value', 'result': 'shipment_index'}) # x:89 y:277 OperatableStateMachine.add('EndShipment', EqualState(), transitions={'true': 'ResetShipmentIndex', 'false': 'GetShipment'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'value_a': 'shipment_index', 'value_b': 'number_of_shipments'}) # x:87 y:378 OperatableStateMachine.add('Notify_Shipment_Ready', self.use_behavior(Notify_Shipment_ReadySM, 'Notify_Shipment_Ready'), transitions={'finished': 'EndShipment', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'agv_id': 'agv_id', 'shipment_type': 'shipment_type'}) # x:1605 y:181 OperatableStateMachine.add('Transport_part_from_bin_to_agv', self.use_behavior(Transport_part_from_bin_to_agvSM, 'Transport_part_from_bin_to_agv'), transitions={'finished': 'SetVariablePart1', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'agv_id': 'agv_id', 'part_type': 'part_type', 'part_pose': 'part_pose', 'bin_id': 'bin_id'}) # x:95 y:183 OperatableStateMachine.add('ResetShipmentIndex', ReplaceState(), transitions={'done': 'GetOrder'}, autonomy={'done': Autonomy.Off}, remapping={'value': 'zero_value', 'result': 'shipment_index'}) # x:1354 y:248 OperatableStateMachine.add('CheckAttachedRight', CheckGripperattached(), transitions={'True': 'CheckAttachedLeft', 'False': 'Transport_part_from_bin_to_agv', 'invalid_arm_id': 'failed'}, autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off}, remapping={'arm_id': 'arm_right'}) # x:1356 y:335 OperatableStateMachine.add('CheckAttachedLeft', CheckGripperattached(), transitions={'True': 'IncrementProductIndex', 'False': 'Transport_part_from_bin_to_agv_LeftArm', 'invalid_arm_id': 'failed'}, autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off}, remapping={'arm_id': 'arm_left'}) # x:1572 y:418 OperatableStateMachine.add('Transport_part_from_bin_to_agv_LeftArm', self.use_behavior(Transport_part_from_bin_to_agv_LeftArmSM, 'Transport_part_from_bin_to_agv_LeftArm'), transitions={'finished': 'SetVariablePart2', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'agv_id': 'agv_id', 'part_type': 'part_type', 'part_pose': 'part_pose', 'bin_id': 'bin_id'}) # x:1654 y:275 OperatableStateMachine.add('SetVariablePart1', ReplaceState(), transitions={'done': 'IncrementProductIndex'}, autonomy={'done': Autonomy.Off}, remapping={'value': 'part_pose', 'result': 'product_location1'}) # x:1651 y:503 OperatableStateMachine.add('SetVariablePart2', ReplaceState(), transitions={'done': 'IncrementProductIndex'}, autonomy={'done': Autonomy.Off}, remapping={'value': 'part_pose', 'result': 'product_location2'}) # x:997 y:510 OperatableStateMachine.add('CheckAttachedRight2', CheckGripperattached(), transitions={'True': 'CheckAttachedLeft2', 'False': 'GetPart', 'invalid_arm_id': 'failed'}, autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off}, remapping={'arm_id': 'arm_right'}) # x:803 y:520 OperatableStateMachine.add('CheckAttachedLeft2', CheckGripperattached(), transitions={'True': 'PlaceOnAgv', 'False': 'GetPart', 'invalid_arm_id': 'failed'}, autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off}, remapping={'arm_id': 'arm_left'}) # x:533 y:450 OperatableStateMachine.add('PlaceOnAgv', self.use_behavior(PlaceOnAgvSM, 'PlaceOnAgv'), transitions={'finished': 'GetPart', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'agv_id': 'agv_id', 'part_pose_right': 'product_location1', 'part_pose_left': 'product_location2'}) # x:1371 y:670 OperatableStateMachine.add('HomepositieRightArm', SrdfStateToMoveitAriac(), transitions={'reached': 'HomepositieLeftArm', 'planning_failed': 'Retry_1', 'control_failed': 'Retry_1', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'home_rightarm_id', 'move_group': 'move_group_right', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1182 y:655 OperatableStateMachine.add('HomepositieLeftArm', SrdfStateToMoveitAriac(), transitions={'reached': 'PlaceOnAgv_2', 'planning_failed': 'Retry_2', 'control_failed': 'Retry_2', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'home_leftarm_id', 'move_group': 'move_group_left', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1558 y:671 OperatableStateMachine.add('Retry_1', WaitState(wait_time=3), transitions={'done': 'HomepositieRightArm'}, autonomy={'done': Autonomy.Off}) # x:1226 y:733 OperatableStateMachine.add('Retry_2', WaitState(wait_time=3), transitions={'done': 'HomepositieLeftArm'}, autonomy={'done': Autonomy.Off}) # x:832 y:626 OperatableStateMachine.add('PlaceOnAgv_2', self.use_behavior(PlaceOnAgvSM, 'PlaceOnAgv_2'), transitions={'finished': 'ResetProductIndex', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'agv_id': 'agv_id', 'part_pose_right': 'product_location1', 'part_pose_left': 'product_location2'}) return _state_machine
def create(self): # x:598 y:381, x:114 y:451 _state_machine = OperatableStateMachine(outcomes=['failed', 'finish']) _state_machine.userdata.powerOn = 100 _state_machine.userdata.powerOff = 0 _state_machine.userdata.part_type = '' _state_machine.userdata.pose = [] _state_machine.userdata.config_name_grasp_right = 'beltPreGrasp' _state_machine.userdata.move_group = 'Gantry' _state_machine.userdata.move_group_prefix = '/ariac/gantry' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' _state_machine.userdata.joint_names = '' _state_machine.userdata.joint_values = [] _state_machine.userdata.config_name_right = 'Right_Home' _state_machine.userdata.config_name_left = 'Left_Home' _state_machine.userdata.move_group_right = 'Right_Arm' _state_machine.userdata.move_group_left = 'Left_Arm' _state_machine.userdata.config_name_grasp_left = 'beltPreGrasp2' _state_machine.userdata.part_type_left = '' _state_machine.userdata.part_type_right = '' _state_machine.userdata.config_name_place = 'gantryPosPlace' _state_machine.userdata.config_name_right_left = 'gantryPosPlaceRightLeft' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:41 y:129 OperatableStateMachine.add('enable_grippers', self.use_behavior( enable_grippersSM, 'enable_grippers'), transitions={ 'finished': 'StartAssignment', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:195 y:41 OperatableStateMachine.add('ConveyorPowerOn', SetConveyorbeltPowerState(), transitions={ 'continue': 'move_home_belt', 'fail': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'power': 'powerOn'}) # x:384 y:41 OperatableStateMachine.add('move_home_belt', self.use_behavior( move_home_beltSM, 'move_home_belt'), transitions={ 'finished': 'detect_product_belt', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:576 y:42 OperatableStateMachine.add('detect_product_belt', self.use_behavior( detect_product_beltSM, 'detect_product_belt'), transitions={ 'finished': 'BeltPreGraspRight', 'failed': 'failed', 'no_products_belt': 'EndAssignment' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'no_products_belt': Autonomy.Inherit }, remapping={ 'part_type': 'part_type', 'pose': 'pose' }) # x:926 y:44 OperatableStateMachine.add('move_to_part_belt_right_arm', self.use_behavior( move_to_part_belt_right_armSM, 'move_to_part_belt_right_arm'), transitions={ 'finished': 'MoveRightSafety', 'failed': 'failed', 'unkown_id': 'BeltPreGraspRight' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'unkown_id': Autonomy.Inherit }, remapping={ 'part_type': 'part_type', 'pose': 'pose', 'joint_values': 'joint_values', 'part_type_right': 'part_type_right' }) # x:762 y:43 OperatableStateMachine.add( 'BeltPreGraspRight', SrdfStateToMoveitAriac(), transitions={ 'reached': 'move_to_part_belt_right_arm', 'planning_failed': 'detect_product_belt', 'control_failed': 'move_to_part_belt_right_arm', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_grasp_right', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:1156 y:45 OperatableStateMachine.add('MoveRightSafety', SrdfStateToMoveitAriac(), transitions={ 'reached': 'BeltPreGraspLeft', 'planning_failed': 'failed', 'control_failed': 'BeltPreGraspLeft', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_right', 'move_group': 'move_group_right', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:1158 y:124 OperatableStateMachine.add( 'BeltPreGraspLeft', SrdfStateToMoveitAriac(), transitions={ 'reached': 'ConveyorPowerOnRepeat', 'planning_failed': 'failed', 'control_failed': 'ConveyorPowerOnRepeat', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_grasp_left', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:1137 y:215 OperatableStateMachine.add('ConveyorPowerOnRepeat', SetConveyorbeltPowerState(), transitions={ 'continue': 'detect_product_belt_repeat', 'fail': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'power': 'powerOn'}) # x:1105 y:301 OperatableStateMachine.add('detect_product_belt_repeat', self.use_behavior( detect_product_beltSM, 'detect_product_belt_repeat'), transitions={ 'finished': 'move_to_part_belt_left_arm', 'failed': 'failed', 'no_products_belt': 'move_to_part_belt_left_arm' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'no_products_belt': Autonomy.Inherit }, remapping={ 'part_type': 'part_type', 'pose': 'pose' }) # x:39 y:40 OperatableStateMachine.add( 'StartAssignment', StartAssignment(), transitions={'continue': 'ConveyorPowerOn'}, autonomy={'continue': Autonomy.Off}) # x:1106 y:393 OperatableStateMachine.add('move_to_part_belt_left_arm', self.use_behavior( move_to_part_belt_left_armSM, 'move_to_part_belt_left_arm'), transitions={ 'finished': 'MoveLeftSafety', 'failed': 'failed', 'unkown_id': 'detect_product_belt_repeat' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'unkown_id': Autonomy.Inherit }, remapping={ 'part_type': 'part_type', 'pose': 'pose', 'joint_values': 'joint_values', 'part_type_left': 'part_type_left' }) # x:1159 y:478 OperatableStateMachine.add('MoveLeftSafety', SrdfStateToMoveitAriac(), transitions={ 'reached': 'MovePlacePos', 'planning_failed': 'failed', 'control_failed': 'MovePlacePos', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_left', 'move_group': 'move_group_left', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:1160 y:565 OperatableStateMachine.add('MovePlacePos', SrdfStateToMoveitAriac(), transitions={ 'reached': 'move_part_to_bin', 'planning_failed': 'failed', 'control_failed': 'move_part_to_bin', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_place', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:980 y:621 OperatableStateMachine.add( 'move_part_to_bin', self.use_behavior(move_part_to_binSM, 'move_part_to_bin'), transitions={ 'finished': 'move_home_belt_2', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'part_type_right': 'part_type_right', 'part_type_left': 'part_type_left' }) # x:721 y:648 OperatableStateMachine.add('move_home_belt_2', self.use_behavior( move_home_beltSM, 'move_home_belt_2'), transitions={ 'finished': 'ConveyorPowerOn', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:274 y:435 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finish'}, autonomy={'continue': Autonomy.Off}) return _state_machine
def create(self): # x:37 y:662, x:476 y:317 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.conveyor_belt_power = 100.0 _state_machine.userdata.config_name_home = 'home' _state_machine.userdata.move_group = 'manipulator' _state_machine.userdata.move_group_prefix = '/ariac/arm1' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' _state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator' _state_machine.userdata.camera_topic = '/ariac/logical_camera_6' _state_machine.userdata.camera_frame = 'logical_camera_6_frame' _state_machine.userdata.part = 'gasket_part' _state_machine.userdata.config_name_bin6PreGrasp = 'bin6PreGrasp' _state_machine.userdata.tool_link = 'ee_link' _state_machine.userdata.part_offset = 0.1 _state_machine.userdata.part_rotation = 0 _state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop' _state_machine.userdata.agv_pose = [] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:71 y:109 OperatableStateMachine.add('StartAssignment', StartAssignment(), transitions={'continue': 'MoveR1Home'}, autonomy={'continue': Autonomy.Off}) # x:182 y:656 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:233 y:110 OperatableStateMachine.add('MoveR1Home', SrdfStateToMoveitAriac(), transitions={'reached': 'DetectCameraPart', 'planning_failed': 'WaitRetry1', 'control_failed': 'WaitRetry1', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_home', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:233 y:8 OperatableStateMachine.add('WaitRetry1', WaitState(wait_time=5), transitions={'done': 'MoveR1Home'}, autonomy={'done': Autonomy.Off}) # x:438 y:112 OperatableStateMachine.add('DetectCameraPart', DetectPartCameraAriacState(time_out=5), transitions={'continue': 'MoveR1PreGrasp1', 'failed': 'failed', 'not_found': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'ref_frame': 'camera_ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part', 'pose': 'pose'}) # x:641 y:111 OperatableStateMachine.add('MoveR1PreGrasp1', SrdfStateToMoveitAriac(), transitions={'reached': 'ComputePick', 'planning_failed': 'WaitRetry2', 'control_failed': 'WaitRetry2', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_bin6PreGrasp', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:640 y:6 OperatableStateMachine.add('WaitRetry2', WaitState(wait_time=5), transitions={'done': 'MoveR1PreGrasp1'}, autonomy={'done': Autonomy.Off}) # x:840 y:113 OperatableStateMachine.add('ComputePick', ComputeGraspAriacState(joint_names=['linear_arm_actuator_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']), transitions={'continue': 'MoveR1ToPick1', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1023 y:110 OperatableStateMachine.add('MoveR1ToPick1', MoveitToJointsDynAriacState(), transitions={'reached': 'WachtEven', 'planning_failed': 'WaitRetry3', 'control_failed': 'WaitRetry3'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1215 y:107 OperatableStateMachine.add('WaitRetry3', WaitState(wait_time=5), transitions={'done': 'MoveR1ToPick1'}, autonomy={'done': Autonomy.Off}) # x:1022 y:200 OperatableStateMachine.add('WachtEven', WaitState(wait_time=1), transitions={'done': 'MoveR1PreGrasp2'}, autonomy={'done': Autonomy.Off}) # x:1017 y:280 OperatableStateMachine.add('MoveR1PreGrasp2', SrdfStateToMoveitAriac(), transitions={'reached': 'MoveR1PreDrop', 'planning_failed': 'WaitRetry4', 'control_failed': 'WaitRetry4', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_bin6PreGrasp', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1215 y:278 OperatableStateMachine.add('WaitRetry4', WaitState(wait_time=5), transitions={'done': 'MoveR1PreGrasp2'}, autonomy={'done': Autonomy.Off}) # x:1005 y:369 OperatableStateMachine.add('MoveR1PreDrop', SrdfStateToMoveitAriac(), transitions={'reached': 'GetAgvPose', 'planning_failed': 'WaitRetry5', 'control_failed': 'WaitRetry5', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_tray1PreDrop', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1210 y:372 OperatableStateMachine.add('WaitRetry5', WaitState(wait_time=5), transitions={'done': 'MoveR1PreDrop'}, autonomy={'done': Autonomy.Off}) # x:1002 y:454 OperatableStateMachine.add('GetAgvPose', GetObjectPoseState(object_frame='kit_tray_1', ref_frame='arm1_linear_arm_actuator'), transitions={'continue': 'ComputeDrop', 'failed': 'ComputeDrop'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'agv_pose'}) # x:998 y:592 OperatableStateMachine.add('ComputeDrop', ComputeGraspAriacState(joint_names=['linear_arm_actuator_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']), transitions={'continue': 'MoveR1ToDrop', 'failed': 'MoveR1ToDrop'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'agv_pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:661 y:522 OperatableStateMachine.add('MoveR1ToDrop', MoveitToJointsDynAriacState(), transitions={'reached': 'notify_shipment_ready', 'planning_failed': 'WaitRetry6', 'control_failed': 'WaitRetry6'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:661 y:640 OperatableStateMachine.add('WaitRetry6', WaitState(wait_time=5), transitions={'done': 'MoveR1ToDrop'}, autonomy={'done': Autonomy.Off}) # x:418 y:581 OperatableStateMachine.add('notify_shipment_ready', self.use_behavior(notify_shipment_readySM, 'notify_shipment_ready'), transitions={'finished': 'EndAssignment', 'failed': 'EndAssignment'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) return _state_machine
def create(self): # x:1274 y:459, x:654 y:517 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.move_groupL = 'Left_Arm' _state_machine.userdata.move_groupR = 'Right_Arm' _state_machine.userdata.armidl = 'left' _state_machine.userdata.armidr = 'right' _state_machine.userdata.robot_name = '' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.move_group_prefix = '/ariac/gantry' _state_machine.userdata.config_namePBL = 'Left_Pre_Band' _state_machine.userdata.config_namePBR = 'Right_Pre_Band' _state_machine.userdata.Power = 100 _state_machine.userdata.NoPower = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:51 y:37 OperatableStateMachine.add('StartAssignment', StartAssignment(), transitions={'continue': 'BeltOff'}, autonomy={'continue': Autonomy.Off}) # x:1464 y:55 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:1219 y:45 OperatableStateMachine.add('belt_to_bin', self.use_behavior(belt_to_binSM, 'belt_to_bin'), transitions={'finished': 'EndAssignment', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:616 y:39 OperatableStateMachine.add('RH', SrdfStateToMoveitAriac(), transitions={'reached': 'GripperUitR', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_namePBR', 'move_group': 'move_groupR', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:428 y:38 OperatableStateMachine.add('GripperUitL', VacuumGripperControlState(enable=False), transitions={'continue': 'RH', 'failed': 'failed', 'invalid_arm_id': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off}, remapping={'arm_id': 'armidl'}) # x:791 y:39 OperatableStateMachine.add('GripperUitR', VacuumGripperControlState(enable=False), transitions={'continue': 'BeltOn', 'failed': 'failed', 'invalid_arm_id': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off}, remapping={'arm_id': 'armidr'}) # x:225 y:36 OperatableStateMachine.add('LH', SrdfStateToMoveitAriac(), transitions={'reached': 'GripperUitL', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_namePBL', 'move_group': 'move_groupL', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:112 y:134 OperatableStateMachine.add('BeltOff', SetConveyorbeltPowerState(), transitions={'continue': 'LH', 'fail': 'failed'}, autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'power': 'NoPower'}) # x:1011 y:39 OperatableStateMachine.add('BeltOn', SetConveyorbeltPowerState(), transitions={'continue': 'belt_to_bin', 'fail': 'failed'}, autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'power': 'Power'}) return _state_machine
def create(self): # x:1132 y:54, x:687 y:211 _state_machine = OperatableStateMachine(outcomes=['finished', 'fail']) _state_machine.userdata.StartText = 'Opdracht gestart' _state_machine.userdata.StopText = 'Opdracht gestopt' _state_machine.userdata.Shipments = [] _state_machine.userdata.part = 'gear_part' _state_machine.userdata.material_locations = [] _state_machine.userdata.NumberOfShipments = 0 _state_machine.userdata.OrderId = '' _state_machine.userdata.Products = [] _state_machine.userdata.NumberOfProducts = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add('StartAssignment', StartAssignment(), transitions={'continue': 'GetOrder'}, autonomy={'continue': Autonomy.Off}) # x:838 y:39 OperatableStateMachine.add('EndAssigment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:286 y:41 OperatableStateMachine.add( 'GetOrder', GetOrderState(), transitions={'continue': 'get_shipments'}, autonomy={'continue': Autonomy.Off}, remapping={ 'order_id': 'OrderId', 'shipments': 'Shipments', 'number_of_shipments': 'NumberOfShipments' }) # x:614 y:40 OperatableStateMachine.add('get_shipments', self.use_behavior( get_shipmentsSM, 'get_shipments'), transitions={ 'finished': 'EndAssigment', 'fail': 'fail' }, autonomy={ 'finished': Autonomy.Inherit, 'fail': Autonomy.Inherit }, remapping={ 'Shipments': 'Shipments', 'NumberOfShipments': 'NumberOfShipments' }) return _state_machine
def create(self): # x:845 y:63, x:414 y:193 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.agv_id = 'agv1' _state_machine.userdata.part_type = 'gear_part' _state_machine.userdata.pose_on_agv = [] _state_machine.userdata.joint_values = [] _state_machine.userdata.joint_names = [] _state_machine.userdata.move_group = 'manipulator' _state_machine.userdata.config_name_home = 'home' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' _state_machine.userdata.tool_link = 'ee_link' _state_machine.userdata.config_name_R2Bin4Pre = 'R2Bin4Pre' _state_machine.userdata.arm_id = 'arm1' _state_machine.userdata.move_group_prefix = '/ariac/arm1' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:52 y:43 OperatableStateMachine.add( 'StartAssignment', StartAssignment(), transitions={'continue': 'arm_selector'}, autonomy={'continue': Autonomy.Off}) # x:625 y:49 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:326 y:46 OperatableStateMachine.add( 'transport_part_form_bin_to_agv_state', self.use_behavior(transport_part_form_bin_to_agv_stateSM, 'transport_part_form_bin_to_agv_state'), transitions={ 'finished': 'EndAssignment', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'part_type': 'part_type', 'agv_id': 'agv_id', 'pose_on_agv': 'pose_on_agv', 'move_group_prefix': 'move_group_prefix' }) # x:177 y:125 OperatableStateMachine.add( 'arm_selector', self.use_behavior(arm_selectorSM, 'arm_selector'), transitions={ 'finished': 'transport_part_form_bin_to_agv_state', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'arm_id': 'arm_id', 'move_group_prefix': 'move_group_prefix' }) return _state_machine
def create(self): # x:1253 y:57, x:671 y:211 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.shipments = [] _state_machine.userdata.order_id = '' _state_machine.userdata.number_of_shipments = 0 _state_machine.userdata.shipment_index = 1 _state_machine.userdata.shipment_type = '' _state_machine.userdata.agv_id = '' _state_machine.userdata.products = [] _state_machine.userdata.number_of_products = 0 _state_machine.userdata.product_index = 1 _state_machine.userdata.part_type = '' _state_machine.userdata.part_pose = [] _state_machine.userdata.move_group = 'manipulator' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add('StartAssignment', StartAssignment(), transitions={'continue': 'GetOrder'}, autonomy={'continue': Autonomy.Off}) # x:369 y:42 OperatableStateMachine.add('GetProductsFromShipment', GetProductsFromShipmentState(), transitions={ 'continue': 'GetPartFromProducts', 'invalid_index': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'invalid_index': Autonomy.Off }, remapping={ 'shipments': 'shipments', 'index': 'shipment_index', 'shipment_type': 'shipment_type', 'agv_id': 'agv_id', 'products': 'products', 'number_of_products': 'number_of_products' }) # x:574 y:43 OperatableStateMachine.add( 'GetPartFromProducts', GetPartFromProductsState(), transitions={ 'continue': 'transport_part_form_bin_to_agv_state', 'invalid_index': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'invalid_index': Autonomy.Off }, remapping={ 'products': 'products', 'index': 'product_index', 'type': 'part_type', 'pose': 'part_pose' }) # x:780 y:44 OperatableStateMachine.add( 'transport_part_form_bin_to_agv_state', self.use_behavior(transport_part_form_bin_to_agv_stateSM, 'transport_part_form_bin_to_agv_state'), transitions={ 'finished': 'EndAssignment', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'part_type': 'part_type', 'agv_id': 'agv_id', 'pose_on_agv': 'part_pose' }) # x:1040 y:45 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:181 y:44 OperatableStateMachine.add( 'GetOrder', GetOrderState(), transitions={'continue': 'GetProductsFromShipment'}, autonomy={'continue': Autonomy.Off}, remapping={ 'order_id': 'order_id', 'shipments': 'shipments', 'number_of_shipments': 'number_of_shipments' }) return _state_machine
def create(self): # x:1680 y:94, x:415 y:533 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.move_groupR = 'Right_Arm' _state_machine.userdata.move_group_prefix = '/ariac/gantry' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' _state_machine.userdata.ARM_idMAIN = '' _state_machine.userdata.move_groupL = 'Left_Arm' _state_machine.userdata.tool_link_right = 'right_ee_link' _state_machine.userdata.tool_link_left = 'left_ee_link' _state_machine.userdata.joint_names = '' _state_machine.userdata.joint_values = '' _state_machine.userdata.ConveyorPartoffset = 0 _state_machine.userdata.rotation = 0 _state_machine.userdata.arm_idR = 'right_arm' _state_machine.userdata.arm_idL = 'left_arm' _state_machine.userdata.Ja_value = 'Ja' _state_machine.userdata.Nee_value = 'Nee' _state_machine.userdata.BothArmsFull = 'Nee' _state_machine.userdata.config_nameLeftPregrasp = 'Left_PreGrasp' _state_machine.userdata.config_nameRightPregrasp = 'Right_PreGrasp' _state_machine.userdata.config_nameGantryROT = 'Gantry_SafeHall1' _state_machine.userdata.move_groupGantry = 'Gantry' _state_machine.userdata.Round_nrMAIN = 0 _state_machine.userdata.RoundTargetMAIN = 4 _state_machine.userdata.PartTYPE = '' _state_machine.userdata.RightArmItem = '' _state_machine.userdata.LeftArmItem = '' _state_machine.userdata.ONE = 1 _state_machine.userdata.USEarmID = '' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add('Startass', StartAssignment(), transitions={'continue': 'ADDround'}, autonomy={'continue': Autonomy.Off}) # x:562 y:111 OperatableStateMachine.add('Conveyor_State', self.use_behavior( Conveyor_StateSM, 'Conveyor_State'), transitions={ 'finished': 'BeltPickUP', 'failed': 'WaitFailed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'ARMidMAIN': 'ARMidMAIN', 'PartPose': 'PartPose', 'PartOffset': 'PartOffset', 'PartTYPE': 'PartTYPE', 'exactparttype': 'exactparttype' }) # x:1542 y:89 OperatableStateMachine.add('stopass', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:349 y:41 OperatableStateMachine.add('Armattachedchecker', self.use_behavior( ArmattachedcheckerSM, 'Armattachedchecker'), transitions={ 'finished': 'BothFULL?', 'failed': 'WaitFailed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'ARMidMAIN': 'ARMidMAIN', 'BothArmsFull': 'BothArmsFull', 'BothArmsEMPTY': 'BothArmsEMPTY' }) # x:561 y:45 OperatableStateMachine.add('BothFULL?', EqualState(), transitions={ 'true': 'ToRotationStation', 'false': 'Conveyor_State' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'value_a': 'BothArmsFull', 'value_b': 'Ja_value' }) # x:179 y:41 OperatableStateMachine.add('StartUP', self.use_behavior(StartUPSM, 'StartUP'), transitions={ 'finished': 'Armattachedchecker', 'failed': 'WaitFailed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:560 y:198 OperatableStateMachine.add('BeltPickUP', self.use_behavior( BeltPickUPSM, 'BeltPickUP'), transitions={ 'finished': 'WaitNewRound', 'failed': 'WaitFailed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'ARMidMAIN': 'ARMidMAIN', 'PartPose': 'PartPose', 'PartOffset': 'PartOffset', 'exactparttype': 'exactparttype', 'PartTYPE': 'PartTYPE', 'RightArmItem': 'RightArmItem', 'LeftArmItem': 'LeftArmItem' }) # x:757 y:46 OperatableStateMachine.add( 'ToRotationStation', SrdfStateToMoveitAriac(), transitions={ 'reached': 'GoToPreDrop', 'planning_failed': 'TransfertoDropFailed', 'control_failed': 'TransfertoDropFailed', 'param_error': 'TransfertoDropFailed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_nameGantryROT', 'move_group': 'move_groupGantry', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:203 y:204 OperatableStateMachine.add('WaitNewRound', WaitState(wait_time=0.25), transitions={'done': 'StartUP'}, autonomy={'done': Autonomy.Off}) # x:376 y:394 OperatableStateMachine.add('WaitFailed', WaitState(wait_time=2), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:749 y:354 OperatableStateMachine.add('TransfertoDropFailed', WaitState(wait_time=2), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:934 y:41 OperatableStateMachine.add('GoToPreDrop', self.use_behavior( GoToPreDropSM, 'GoToPreDrop'), transitions={ 'finished': 'DropItemsInBIN', 'failed': 'TransfertoDropFailed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'RightArmItem': 'RightArmItem', 'LeftArmItem': 'LeftArmItem', 'GantryLocation': 'GantryLocation', 'USEarmID': 'USEarmID' }) # x:1323 y:153 OperatableStateMachine.add('RoundCheck', EqualState(), transitions={ 'true': 'stopass', 'false': 'ADDround' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'value_a': 'Round_nrMAIN', 'value_b': 'RoundTargetMAIN' }) # x:21 y:348 OperatableStateMachine.add('ADDround', AddNumericState(), transitions={'done': 'StartUP'}, autonomy={'done': Autonomy.Off}, remapping={ 'value_a': 'Round_nrMAIN', 'value_b': 'ONE', 'result': 'Round_nrMAIN' }) # x:1116 y:42 OperatableStateMachine.add('DropItemsInBIN', self.use_behavior( DropItemsInBINSM, 'DropItemsInBIN'), transitions={ 'Next arm': 'GoToPreDrop', 'failed': 'TransfertoDropFailed', 'BothArmsEmpty': 'RoundCheck', 'BinVol': 'LogBinVol' }, autonomy={ 'Next arm': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'BothArmsEmpty': Autonomy.Inherit, 'BinVol': Autonomy.Inherit }, remapping={ 'USEarmID': 'USEarmID', 'GantryLocation': 'GantryLocation', 'RightArmItem': 'RightArmItem', 'LeftArmItem': 'LeftArmItem' }) # x:1323 y:46 OperatableStateMachine.add('LogBinVol', LogState(text="Een van de bins is vol", severity=Logger.REPORT_HINT), transitions={'done': 'stopass'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:52 y:611, x:343 y:359 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.agv_id = 'agv1' _state_machine.userdata.part_type = 'gear_part' _state_machine.userdata.pose_on_agv = [] _state_machine.userdata.pose = [] _state_machine.userdata.part_pose = [] _state_machine.userdata.joint_values = [] _state_machine.userdata.joint_names = [] _state_machine.userdata.offset = 0.1 _state_machine.userdata.move_group = 'manipulator' _state_machine.userdata.move_group_prefix = '/ariac/arm1' _state_machine.userdata.config_name_home = 'home' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' _state_machine.userdata.config_name_bin3PreGrasp = 'bin3PreGrasp' _state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop' _state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator' _state_machine.userdata.camera_topic = '/ariac/logical_camera_5' _state_machine.userdata.camera_frame = 'logical_camera_5_frame' _state_machine.userdata.tool_link = 'ee_link' _state_machine.userdata.agv_pose = [] _state_machine.userdata.part_offset = 0.035 _state_machine.userdata.part_rotation = 0 _state_machine.userdata.conveyor_belt_power = 100.0 _state_machine.userdata.config_name_bin1PreGrasp = 'bin1PreGrasp' _state_machine.userdata.arm_id = "arm1" _state_machine.userdata.part_drop_offset = 0.1 _state_machine.userdata.StartText = 'Opdracht gestart' _state_machine.userdata.StopText = 'Opdracht gestopt' _state_machine.userdata.Shipments = [] _state_machine.userdata.material_locations = [] _state_machine.userdata.NumberOfShipments = 0 _state_machine.userdata.OrderId = '' _state_machine.userdata.Products = [] _state_machine.userdata.NumberOfProducts = 0 _state_machine.userdata.MaterialsLocationList = [] _state_machine.userdata.config_name_bin3PreDrop = 'bin3PreDrop' _state_machine.userdata.part = 'gasket_part' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:54 y:102 OperatableStateMachine.add('StartAssignment', StartAssignment(), transitions={'continue': 'ConveyorBelt'}, autonomy={'continue': Autonomy.Off}) # x:197 y:603 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:654 y:113 OperatableStateMachine.add('MoveR1Home', SrdfStateToMoveitAriac(), transitions={'reached': 'DetectCameraPart', 'planning_failed': 'WaitRetry1', 'control_failed': 'WaitRetry1', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_home', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:666 y:12 OperatableStateMachine.add('WaitRetry1', WaitState(wait_time=5), transitions={'done': 'MoveR1Home'}, autonomy={'done': Autonomy.Off}) # x:836 y:111 OperatableStateMachine.add('DetectCameraPart', DetectPartCameraAriacState(time_out=5.0), transitions={'continue': 'MoveR1PreGrasp1', 'failed': 'failed', 'not_found': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'ref_frame': 'camera_ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part', 'pose': 'pose'}) # x:1039 y:116 OperatableStateMachine.add('MoveR1PreGrasp1', SrdfStateToMoveitAriac(), transitions={'reached': 'ComputePick', 'planning_failed': 'WaitRetry2', 'control_failed': 'WaitRetry2', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_bin1PreGrasp', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1048 y:21 OperatableStateMachine.add('WaitRetry2', WaitState(wait_time=5), transitions={'done': 'MoveR1PreGrasp1'}, autonomy={'done': Autonomy.Off}) # x:1038 y:200 OperatableStateMachine.add('ComputePick', ComputeGraspAriacState(joint_names=['linear_arm_actuator_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']), transitions={'continue': 'MoveR1ToPick1', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1069 y:379 OperatableStateMachine.add('Wait', WaitState(wait_time=1), transitions={'done': 'transport_part_from_bin_to_agv_1'}, autonomy={'done': Autonomy.Off}) # x:1303 y:280 OperatableStateMachine.add('WaitRetry3', WaitState(wait_time=5), transitions={'done': 'MoveR1ToPick1'}, autonomy={'done': Autonomy.Off}) # x:1035 y:284 OperatableStateMachine.add('MoveR1ToPick1', MoveitToJointsDynAriacState(), transitions={'reached': 'EnableGripper', 'planning_failed': 'WaitRetry3', 'control_failed': 'EnableGripper'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:818 y:282 OperatableStateMachine.add('EnableGripper', VacuumGripperControlState(enable=True), transitions={'continue': 'Wait', 'failed': 'failed', 'invalid_arm_id': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off}, remapping={'arm_id': 'arm_id'}) # x:294 y:484 OperatableStateMachine.add('DisableGripper', VacuumGripperControlState(enable=False), transitions={'continue': 'EndAssignment', 'failed': 'failed', 'invalid_arm_id': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off}, remapping={'arm_id': 'arm_id'}) # x:975 y:541 OperatableStateMachine.add('transport_part_from_bin_to_agv_1', self.use_behavior(transport_part_from_bin_to_agv_1SM, 'transport_part_from_bin_to_agv_1'), transitions={'finished': 'DisableGripper', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:222 y:106 OperatableStateMachine.add('ConveyorBelt', SetConveyorbeltPowerState(), transitions={'continue': 'MoveR1Home', 'fail': 'failed'}, autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'power': 'conveyor_belt_power'}) return _state_machine
def create(self): # x:70 y:501, x:515 y:301 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.part_pose = [] _state_machine.userdata.joint_values = [] _state_machine.userdata.joint_names = [] _state_machine.userdata.part = 'gasket_part' _state_machine.userdata.offset = 0.1 _state_machine.userdata.move_group = 'manipulator' _state_machine.userdata.move_group_prefix = '/ariac/arm1' _state_machine.userdata.config_name_home = 'home' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' _state_machine.userdata.config_name_bin3PreGrasp = 'bin3PreGrasp' _state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop' _state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator' _state_machine.userdata.camera_topic = '/ariac/logical_camera_1' _state_machine.userdata.camera_frame = 'logical_camera_1_frame' _state_machine.userdata.tool_link = 'ee_link' _state_machine.userdata.agv_pose = [] _state_machine.userdata.part_offset = 0.035 _state_machine.userdata.part_rotation = 0 _state_machine.userdata.conveyor_belt_power = 100.0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:20 y:140 OperatableStateMachine.add( 'StartAssignment', StartAssignment(), transitions={'continue': 'SetConveyorbeltPower'}, autonomy={'continue': Autonomy.Off}) # x:909 y:288 OperatableStateMachine.add('WachtEven', WaitState(wait_time=1), transitions={'done': 'MoveR1PreGrasp2'}, autonomy={'done': Autonomy.Off}) # x:746 y:427 OperatableStateMachine.add( 'GetAgvPose', GetObjectPoseState(object_frame='kit_tray_1', ref_frame='arm1_linear_arm_actuator'), transitions={ 'continue': 'ComuteDorp', 'failed': 'ComuteDorp' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'agv_pose'}) # x:373 y:13 OperatableStateMachine.add('WaitRetry1', WaitState(wait_time=5), transitions={'done': 'MoveR1Home'}, autonomy={'done': Autonomy.Off}) # x:755 y:14 OperatableStateMachine.add('WaitRetry2', WaitState(wait_time=5), transitions={'done': 'MoveR1PreGrasp1'}, autonomy={'done': Autonomy.Off}) # x:1107 y:222 OperatableStateMachine.add('WaitRetry3', WaitState(wait_time=5), transitions={'done': 'MoveR1ToPick1'}, autonomy={'done': Autonomy.Off}) # x:1104 y:355 OperatableStateMachine.add('WaitRetry4', WaitState(wait_time=5), transitions={'done': 'MoveR1PreGrasp2'}, autonomy={'done': Autonomy.Off}) # x:383 y:523 OperatableStateMachine.add('WaitRetry6', WaitState(wait_time=5), transitions={'done': 'MoveR1ToDrop'}, autonomy={'done': Autonomy.Off}) # x:1104 y:430 OperatableStateMachine.add('WaitRetry5', WaitState(wait_time=5), transitions={'done': 'MoveR1PreDrop'}, autonomy={'done': Autonomy.Off}) # x:158 y:420 OperatableStateMachine.add('DeliverShipment', self.use_behavior( notify_shipment_readySM, 'DeliverShipment'), transitions={ 'finished': 'EndAssignment', 'failed': 'ComuteDorp' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:733 y:148 OperatableStateMachine.add('MoveR1PreGrasp1', SrdfStateToMoveitAriac(), transitions={ 'reached': 'ComutePick', 'planning_failed': 'WaitRetry2', 'control_failed': 'WaitRetry2', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_bin3PreGrasp', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:910 y:355 OperatableStateMachine.add('MoveR1PreGrasp2', SrdfStateToMoveitAriac(), transitions={ 'reached': 'MoveR1PreDrop', 'planning_failed': 'WaitRetry4', 'control_failed': 'WaitRetry4', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_bin3PreGrasp', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:913 y:428 OperatableStateMachine.add('MoveR1PreDrop', SrdfStateToMoveitAriac(), transitions={ 'reached': 'GetAgvPose', 'planning_failed': 'WaitRetry5', 'control_failed': 'WaitRetry5', 'param_error': 'ComuteDorp' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_tray1PreDrop', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:908 y:220 OperatableStateMachine.add('MoveR1ToPick1', MoveitToJointsDynAriacState(), transitions={ 'reached': 'WachtEven', 'planning_failed': 'WaitRetry3', 'control_failed': 'WaitRetry3' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off }, remapping={ 'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:349 y:422 OperatableStateMachine.add('MoveR1ToDrop', MoveitToJointsDynAriacState(), transitions={ 'reached': 'DeliverShipment', 'planning_failed': 'WaitRetry6', 'control_failed': 'WaitRetry6' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off }, remapping={ 'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:526 y:146 OperatableStateMachine.add( 'DetectCameraPart', DetectPartCameraAriacState(time_out=5.0), transitions={ 'continue': 'MoveR1PreGrasp1', 'failed': 'failed', 'not_found': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ref_frame': 'camera_ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part', 'pose': 'pose' }) # x:908 y:148 OperatableStateMachine.add( 'ComutePick', ComputeGraspAriacState(joint_names=[ 'linear_arm_actuator_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ]), transitions={ 'continue': 'MoveR1ToPick1', 'failed': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:539 y:424 OperatableStateMachine.add( 'ComuteDorp', ComputeGraspAriacState(joint_names=[ 'linear_arm_actuator_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ]), transitions={ 'continue': 'MoveR1ToDrop', 'failed': 'MoveR1ToDrop' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'agv_pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:353 y:143 OperatableStateMachine.add('MoveR1Home', SrdfStateToMoveitAriac(), transitions={ 'reached': 'DetectCameraPart', 'planning_failed': 'WaitRetry1', 'control_failed': 'WaitRetry1', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_home', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:28 y:422 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:165 y:144 OperatableStateMachine.add( 'SetConveyorbeltPower', SetConveyorbeltPowerState(), transitions={ 'continue': 'MoveR1Home', 'fail': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'power': 'conveyor_belt_power'}) return _state_machine
def create(self): # x:1751 y:131, x:910 y:358 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.shipments = [] _state_machine.userdata.material_locations = [] _state_machine.userdata.NumberOfShipments = 0 _state_machine.userdata.OrderId = '' _state_machine.userdata.Products = [] _state_machine.userdata.NumberOfProducts = 0 _state_machine.userdata.power = 0 _state_machine.userdata.config_name_PreHomeR = 'Rechts_PreDrop_AGV' _state_machine.userdata.config_name_PreHomeL = 'Links_PreDrop_AGV' _state_machine.userdata.move_group_R = 'Right_Arm' _state_machine.userdata.move_group_L = 'Left_Arm' _state_machine.userdata.move_group_prefix = '/ariac/gantry' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' _state_machine.userdata.Old_Id = '' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:62 y:57 OperatableStateMachine.add('StartAssignment', StartAssignment(), transitions={'continue': 'TurnOffBelt'}, autonomy={'continue': Autonomy.Off}) # x:958 y:51 OperatableStateMachine.add('GetOrder', GetOrderState(), transitions={'continue': 'Id_Check'}, autonomy={'continue': Autonomy.Off}, remapping={'order_id': 'OrderId', 'shipments': 'Shipments', 'number_of_shipments': 'NumberOfShipments'}) # x:1552 y:126 OperatableStateMachine.add('EndAssigment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:1317 y:273 OperatableStateMachine.add('GetShipments', self.use_behavior(GetShipmentsSM, 'GetShipments'), transitions={'finished': 'GetOrder', 'failed': 'EndAssigment'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'Shipments': 'Shipments', 'NumberOfShipments': 'NumberOfShipments'}) # x:249 y:56 OperatableStateMachine.add('TurnOffBelt', SetConveyorbeltPowerState(), transitions={'continue': 'PreHomeL', 'fail': 'failed'}, autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'power': 'power'}) # x:514 y:36 OperatableStateMachine.add('PreHomeL', SrdfStateToMoveitAriac(), transitions={'reached': 'PreHomeR', 'planning_failed': 'failed', 'control_failed': 'PreHomeR', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_PreHomeL', 'move_group': 'move_group_L', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:739 y:53 OperatableStateMachine.add('PreHomeR', SrdfStateToMoveitAriac(), transitions={'reached': 'GetOrder', 'planning_failed': 'failed', 'control_failed': 'GetOrder', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_PreHomeR', 'move_group': 'move_group_R', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1318 y:161 OperatableStateMachine.add('Old_Id', ReplaceState(), transitions={'done': 'GetShipments'}, autonomy={'done': Autonomy.Off}, remapping={'value': 'OrderId', 'result': 'Old_Id'}) # x:1314 y:38 OperatableStateMachine.add('Id_Check', EqualState(), transitions={'true': 'EndAssigment', 'false': 'Old_Id'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'value_a': 'OrderId', 'value_b': 'Old_Id'}) return _state_machine
def create(self): # x:650 y:25, x:929 y:416 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.agv_id = '' _state_machine.userdata.part_type = '' _state_machine.userdata.products = [] _state_machine.userdata.product_index = 0 _state_machine.userdata.shipment_index = 0 _state_machine.userdata.material_locations = [] _state_machine.userdata.bin_index = 0 _state_machine.userdata.order_id = '' _state_machine.userdata.shipments = [] _state_machine.userdata.number_of_shipments = 0 _state_machine.userdata.shipment_type = '' _state_machine.userdata.number_of_products = 0 _state_machine.userdata.bin_id = '' _state_machine.userdata.old_order_id = '' _state_machine.userdata.one_value = 1 _state_machine.userdata.zero_value = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:45 y:112 OperatableStateMachine.add('StartAssignment', StartAssignment(), transitions={'continue': 'GetOrder'}, autonomy={'continue': Autonomy.Off}) # x:211 y:112 OperatableStateMachine.add('GetOrder', GetOrderState(), transitions={'continue': 'ConveyorOff'}, autonomy={'continue': Autonomy.Off}, remapping={ 'order_id': 'order_id', 'shipments': 'shipments', 'number_of_shipments': 'number_of_shipments' }) # x:424 y:113 OperatableStateMachine.add('TestLastOrder', EqualState(), transitions={ 'true': 'EndAssignment', 'false': 'RememberOldOrder' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'value_a': 'order_id', 'value_b': 'old_order_id' }) # x:650 y:114 OperatableStateMachine.add('RememberOldOrder', ReplaceState(), transitions={'done': 'GetShipment'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'order_id', 'result': 'old_order_id' }) # x:869 y:114 OperatableStateMachine.add('GetShipment', GetProductsFromShipmentState(), transitions={ 'continue': 'GetPart', 'invalid_index': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'invalid_index': Autonomy.Off }, remapping={ 'shipments': 'shipments', 'index': 'shipment_index', 'shipment_type': 'shipment_type', 'agv_id': 'agv_id', 'products': 'products', 'number_of_products': 'number_of_products' }) # x:1122 y:115 OperatableStateMachine.add('GetPart', GetPartFromProductsState(), transitions={ 'continue': 'MaterialLocation', 'invalid_index': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'invalid_index': Autonomy.Off }, remapping={ 'products': 'products', 'index': 'product_index', 'type': 'part_type', 'pose': 'part_pose' }) # x:1361 y:113 OperatableStateMachine.add('MaterialLocation', GetMaterialLocationsState(), transitions={'continue': 'GetBin'}, autonomy={'continue': Autonomy.Off}, remapping={ 'part': 'part_type', 'material_locations': 'material_locations' }) # x:1361 y:235 OperatableStateMachine.add('GetBin', GetItemFromListState(), transitions={ 'done': 'AGV_id', 'invalid_index': 'failed' }, autonomy={ 'done': Autonomy.Off, 'invalid_index': Autonomy.Off }, remapping={ 'list': 'material_locations', 'index': 'bin_index', 'item': 'bin_id' }) # x:759 y:683 OperatableStateMachine.add('IncrementShipmentIndex', AddNumericState(), transitions={'done': 'EndShipment'}, autonomy={'done': Autonomy.Off}, remapping={ 'value_a': 'shipment_index', 'value_b': 'one_value', 'result': 'shipment_index' }) # x:536 y:683 OperatableStateMachine.add('EndShipment', EqualState(), transitions={ 'true': 'Notify_shipment_ready', 'false': 'GetShipment' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'value_a': 'shipment_index', 'value_b': 'number_of_shipments' }) # x:446 y:6 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:206 y:680 OperatableStateMachine.add('Notify_shipment_ready', self.use_behavior( Notify_shipment_readySM, 'Notify_shipment_ready'), transitions={ 'finished': 'ResetShipmentIndex', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'shipment_type': 'shipment_type', 'agv_id': 'agv_id' }) # x:1357 y:675 OperatableStateMachine.add( 'GripperChoice', self.use_behavior(GripperChoiceSM, 'GripperChoice'), transitions={ 'finished': 'IncrementShipmentIndex', 'failed': 'failed', 'false': 'GetPart' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'false': Autonomy.Inherit }, remapping={ 'number_of_products': 'number_of_products', 'product_index': 'product_index', 'bin_id': 'bin_id', 'part_type': 'part_type', 'part_pose': 'part_pose', 'agv_id': 'agv_id' }) # x:194 y:377 OperatableStateMachine.add('ResetShipmentIndex', ReplaceState(), transitions={'done': 'GetOrder'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'zero_value', 'result': 'shipment_index' }) # x:216 y:19 OperatableStateMachine.add('ConveyorOff', SetConveyorbeltPowerState(), transitions={ 'continue': 'TestLastOrder', 'fail': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'power': 'zero_value'}) # x:1384 y:446 OperatableStateMachine.add( 'AGV_id', MessageState(), transitions={'continue': 'GripperChoice'}, autonomy={'continue': Autonomy.Off}, remapping={'message': 'agv_id'}) return _state_machine
def create(self): # x:52 y:611, x:343 y:359 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.agv_id = 'agv1' _state_machine.userdata.part_type = 'gear_part' _state_machine.userdata.pose_on_agv = [] _state_machine.userdata.pose = [] _state_machine.userdata.part_pose = [] _state_machine.userdata.joint_values = [] _state_machine.userdata.joint_names = [] _state_machine.userdata.part = 'gasket_part' _state_machine.userdata.offset = 0.1 _state_machine.userdata.move_group = 'manipulator' _state_machine.userdata.move_group_prefix = '/ariac/arm1' _state_machine.userdata.config_name_home = 'home' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' _state_machine.userdata.config_name_bin3PreGrasp = 'bin3PreGrasp' _state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop' _state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator' _state_machine.userdata.camera_topic = '/ariac/logical_camera_5' _state_machine.userdata.camera_frame = 'logical_camera_5_frame' _state_machine.userdata.tool_link = 'ee_link' _state_machine.userdata.agv_pose = [] _state_machine.userdata.part_offset = 0.035 _state_machine.userdata.part_rotation = 0 _state_machine.userdata.conveyor_belt_power = 100.0 _state_machine.userdata.config_name_bin1PreGrasp = 'bin1PreGrasp' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:54 y:102 OperatableStateMachine.add( 'StartAssignment', StartAssignment(), transitions={ 'continue': 'transport_part_form_bin_to_agv_state' }, autonomy={'continue': Autonomy.Off}) # x:197 y:603 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:189 y:100 OperatableStateMachine.add( 'transport_part_form_bin_to_agv_state', self.use_behavior(transport_part_form_bin_to_agv_stateSM, 'transport_part_form_bin_to_agv_state'), transitions={ 'finished': 'SetConveyorbeltPower', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'part_type': 'part_type', 'agv_id': 'agv_id', 'pose_on_agv': 'pose_on_agv' }) # x:654 y:113 OperatableStateMachine.add('MoveR1Home', SrdfStateToMoveitAriac(), transitions={ 'reached': 'DetectCameraPart', 'planning_failed': 'WaitRetry1', 'control_failed': 'WaitRetry1', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_home', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:666 y:12 OperatableStateMachine.add('WaitRetry1', WaitState(wait_time=5), transitions={'done': 'MoveR1Home'}, autonomy={'done': Autonomy.Off}) # x:836 y:111 OperatableStateMachine.add('DetectCameraPart', DetectPartCameraAriacState(time_out=5), transitions={ 'continue': 'MoveR1PreGrasp1', 'failed': 'failed', 'not_found': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ref_frame': 'camera_ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part', 'pose': 'pose' }) # x:1039 y:116 OperatableStateMachine.add('MoveR1PreGrasp1', SrdfStateToMoveitAriac(), transitions={ 'reached': 'ComputePick', 'planning_failed': 'WaitRetry2', 'control_failed': 'WaitRetry2', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_bin1PreGrasp', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:1048 y:21 OperatableStateMachine.add('WaitRetry2', WaitState(wait_time=5), transitions={'done': 'MoveR1PreGrasp1'}, autonomy={'done': Autonomy.Off}) # x:456 y:106 OperatableStateMachine.add( 'SetConveyorbeltPower', SetConveyorbeltPowerState(), transitions={ 'continue': 'MoveR1Home', 'fail': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'power': 'conveyor_belt_power'}) # x:1038 y:200 OperatableStateMachine.add( 'ComputePick', ComputeGraspAriacState(joint_names=[ 'linear_arm_actuator_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ]), transitions={ 'continue': 'MoveR1ToPick1', 'failed': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:1069 y:379 OperatableStateMachine.add('Wait', WaitState(wait_time=1), transitions={'done': 'MoveR1PreGrasp2'}, autonomy={'done': Autonomy.Off}) # x:1041 y:504 OperatableStateMachine.add('MoveR1PreGrasp2', SrdfStateToMoveitAriac(), transitions={ 'reached': 'MoveR1PreDrop', 'planning_failed': 'WaitRetry4', 'control_failed': 'WaitRetry4', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_bin1PreGrasp', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:1303 y:280 OperatableStateMachine.add('WaitRetry3', WaitState(wait_time=5), transitions={'done': 'MoveR1ToPick1'}, autonomy={'done': Autonomy.Off}) # x:1226 y:501 OperatableStateMachine.add('WaitRetry4', WaitState(wait_time=5), transitions={'done': 'MoveR1PreGrasp2'}, autonomy={'done': Autonomy.Off}) # x:876 y:608 OperatableStateMachine.add( 'GetAgvPose', GetObjectPoseState(object_frame='kit_tray_1', ref_frame='arm1_linear_arm_actuator'), transitions={ 'continue': 'ComputeDrop', 'failed': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'agv_pose'}) # x:1220 y:618 OperatableStateMachine.add('WaitRerty5', WaitState(wait_time=5), transitions={'done': 'MoveR1PreDrop'}, autonomy={'done': Autonomy.Off}) # x:688 y:604 OperatableStateMachine.add( 'ComputeDrop', ComputeGraspAriacState(joint_names=[ 'linear_arm_actuator_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint' ]), transitions={ 'continue': 'MoveR1ToDrop', 'failed': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'agv_pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:430 y:601 OperatableStateMachine.add('MoveR1ToDrop', MoveitToJointsDynAriacState(), transitions={ 'reached': 'EndAssignment', 'planning_failed': 'WaitRetry6', 'control_failed': 'WaitRetry6' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off }, remapping={ 'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:451 y:726 OperatableStateMachine.add('WaitRetry6', WaitState(wait_time=5), transitions={'done': 'MoveR1ToDrop'}, autonomy={'done': Autonomy.Off}) # x:1035 y:284 OperatableStateMachine.add('MoveR1ToPick1', MoveitToJointsDynAriacState(), transitions={ 'reached': 'Wait', 'planning_failed': 'WaitRetry3', 'control_failed': 'WaitRetry3' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off }, remapping={ 'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:1042 y:611 OperatableStateMachine.add('MoveR1PreDrop', SrdfStateToMoveitAriac(), transitions={ 'reached': 'GetAgvPose', 'planning_failed': 'WaitRerty5', 'control_failed': 'WaitRerty5', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name_tray1PreDrop', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) return _state_machine