Exemple #1
0
	def create(self):
		# x:37 y:662, x:1132 y:221
		_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.04
		_state_machine.userdata.part_rotation = 0
		_state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop'
		_state_machine.userdata.agv_pose = []
		_state_machine.userdata.part_offsetPre = 0.1
		_state_machine.userdata.part_offsetDown = 0.045
		_state_machine.userdata.arm = ''
		_state_machine.userdata.part_type = ''
		_state_machine.userdata.pose = ''
		_state_machine.userdata.arm1 = '/ariac/arm1'
		_state_machine.userdata.arm2 = '/ariac/arm2'
		_state_machine.userdata.bin4_pose = ''
		_state_machine.userdata.offset = 0
		_state_machine.userdata.rotation = 0
		_state_machine.userdata.config_name_bin4PreGrasp = 'bin4PreGrasp'
		_state_machine.userdata.config_name = home

		# Additional creation code can be added inside the following tags
		# [MANUAL_CREATE]
		
		# [/MANUAL_CREATE]


		with _state_machine:
			# x:298 y:114
			OperatableStateMachine.add('MoveR1Home',
										SrdfStateToMoveitAriac(),
										transitions={'reached': 'Cameras2', '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:303 y:23
			OperatableStateMachine.add('WaitRetry1',
										WaitState(wait_time=1),
										transitions={'done': 'MoveR1Home'},
										autonomy={'done': Autonomy.Off})

			# x:865 y:119
			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': 'EnableGripper', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'move_group': 'move_group', 'move_group_prefix': 'arm1', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'part_offsetPre', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:1283 y:121
			OperatableStateMachine.add('MoveR1ToPick1',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'ComputePick2', 'planning_failed': 'WaitRetry3', 'control_failed': 'WaitRetry3'},
										autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
										remapping={'move_group_prefix': 'arm1', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:1683 y:117
			OperatableStateMachine.add('WaitRetry3',
										WaitState(wait_time=1),
										transitions={'done': 'MoveR1ToPick1'},
										autonomy={'done': Autonomy.Off})

			# x:1518 y:291
			OperatableStateMachine.add('WachtEven',
										WaitState(wait_time=2),
										transitions={'done': 'MoveR1PreGrasp2'},
										autonomy={'done': Autonomy.Off})

			# x:1489 y:392
			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': '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:1665 y:391
			OperatableStateMachine.add('WaitRetry4',
										WaitState(wait_time=5),
										transitions={'done': 'MoveR1PreGrasp2'},
										autonomy={'done': Autonomy.Off})

			# x:1492 y:476
			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:1671 y:488
			OperatableStateMachine.add('WaitRetry5',
										WaitState(wait_time=5),
										transitions={'done': 'MoveR1PreDrop'},
										autonomy={'done': Autonomy.Off})

			# x:1507 y:559
			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:1299 y:561
			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': 'arm1', 'tool_link': 'tool_link', 'pose': 'agv_pose', 'offset': 'part_offsetDown', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:1282 y:484
			OperatableStateMachine.add('MoveR1ToDrop',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'DisableGripper', 'planning_failed': 'WaitRetry6', 'control_failed': 'WaitRetry6'},
										autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
										remapping={'move_group_prefix': 'arm1', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:1157 y:561
			OperatableStateMachine.add('WaitRetry6',
										WaitState(wait_time=5),
										transitions={'done': 'MoveR1ToDrop'},
										autonomy={'done': Autonomy.Off})

			# x:1071 y:119
			OperatableStateMachine.add('EnableGripper',
										VacuumGripperControlState(enable=True, service_name='/ariac/arm1/gripper/control'),
										transitions={'continue': 'MoveR1ToPick1', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})

			# x:1476 y:197
			OperatableStateMachine.add('ComputePick2',
										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': 'MoveR1ToPick2', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'move_group': 'move_group', 'move_group_prefix': 'arm1', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:1273 y:269
			OperatableStateMachine.add('MoveR1ToPick2',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'WachtEven', 'planning_failed': 'WaitRetry7', 'control_failed': 'WaitRetry7'},
										autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
										remapping={'move_group_prefix': 'arm1', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:945 y:562
			OperatableStateMachine.add('DisableGripper',
										VacuumGripperControlState(enable=False, service_name='/ariac/arm1/gripper/control'),
										transitions={'continue': 'RetrunHomeR1', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})

			# x:1676 y:239
			OperatableStateMachine.add('WaitRetry7',
										WaitState(wait_time=1),
										transitions={'done': 'MoveR1ToPick2'},
										autonomy={'done': Autonomy.Off})

			# x:944 y:634
			OperatableStateMachine.add('RetrunHomeR1',
										SrdfStateToMoveitAriac(),
										transitions={'reached': 'finished', 'planning_failed': 'Waitretry8', 'control_failed': 'Waitretry8', 'param_error': 'Waitretry8'},
										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': '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:1161 y:640
			OperatableStateMachine.add('Waitretry8',
										WaitState(wait_time=1),
										transitions={'done': 'RetrunHomeR1'},
										autonomy={'done': Autonomy.Off})

			# x:499 y:113
			OperatableStateMachine.add('Cameras2',
										self.use_behavior(Cameras2SM, 'Cameras2'),
										transitions={'finished': 'DetectArm', 'failed': 'failed'},
										autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
										remapping={'part_type': 'part_type', 'arm': 'arm', 'part_pose': 'part_pose'})

			# x:682 y:172
			OperatableStateMachine.add('DetectArm',
										EqualState(),
										transitions={'true': 'ComputePick', 'false': 'ComputePickR2_1'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'arm', 'value_b': 'arm1'})

			# x:688 y:376
			OperatableStateMachine.add('EnableGripper2',
										VacuumGripperControlState(enable=True, service_name='/ariac/arm2gripper/control'),
										transitions={'continue': 'MoveR2ToPick2', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})

			# x:687 y:267
			OperatableStateMachine.add('ComputePickR2_1',
										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': 'EnableGripper2', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'move_group': 'move_group', 'move_group_prefix': 'arm2', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'offset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:688 y:478
			OperatableStateMachine.add('MoveR2ToPick2',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'ComputePickR2_2', 'planning_failed': 'MoveR2ToPick2', 'control_failed': 'MoveR2ToPick2'},
										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:689 y:609
			OperatableStateMachine.add('ComputePickR2_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': 'WachtEven2', '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': 'offset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:530 y:611
			OperatableStateMachine.add('WachtEven2',
										WaitState(wait_time=2),
										transitions={'done': 'MoveR2PreGrasp'},
										autonomy={'done': Autonomy.Off})

			# x:486 y:460
			OperatableStateMachine.add('MoveR2PreGrasp',
										SrdfStateToMoveitAriac(),
										transitions={'reached': 'GetBin4Pose', 'planning_failed': 'MoveR2PreGrasp', 'control_failed': 'MoveR2PreGrasp', 'param_error': 'failed'},
										autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
										remapping={'config_name': 'config_name_bin4PreGrasp', '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:299 y:488
			OperatableStateMachine.add('GetBin4Pose',
										GetObjectPoseState(object_frame='kit_tray_1', ref_frame='arm2_linear_arm_actuator'),
										transitions={'continue': 'ComputeDropR2', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'pose': 'bin4_pose'})

			# x:116 y:487
			OperatableStateMachine.add('ComputeDropR2',
										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': 'MoveR2ToDrop', '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': 'offset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:115 y:364
			OperatableStateMachine.add('MoveR2ToDrop',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'DisableGripper2', 'planning_failed': 'MoveR2ToDrop', 'control_failed': 'MoveR2ToDrop'},
										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:116 y:256
			OperatableStateMachine.add('DisableGripper2',
										VacuumGripperControlState(enable=True, service_name='/ariac/arm2/gripper/control'),
										transitions={'continue': 'ReturnHomeR2', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})

			# x:302 y:256
			OperatableStateMachine.add('ReturnHomeR2',
										SrdfStateToMoveitAriac(),
										transitions={'reached': 'Cameras2', 'planning_failed': 'ReturnHomeR2', 'control_failed': 'ReturnHomeR2', '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'})


		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:761 y:731, x:442 y:741
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'],
            input_keys=['agv_id', 'pose_on_agv_l', 'pose_on_agv_r'])
        _state_machine.userdata.config_name_left = 'Left_Home_B'
        _state_machine.userdata.config_name_right = 'Right_Home_B'
        _state_machine.userdata.config_name_gantry = 'Gantry_Home'
        _state_machine.userdata.move_group_g = 'Gantry'
        _state_machine.userdata.move_group_prefix_g = '/ariac/gantry'
        _state_machine.userdata.action_topic = '/move_group'
        _state_machine.userdata.robot_name = 'gantry'
        _state_machine.userdata.joint_values = []
        _state_machine.userdata.joint_names = []
        _state_machine.userdata.move_group_l = 'Left_Arm'
        _state_machine.userdata.move_group_r = 'Right_Arm'
        _state_machine.userdata.agv_id = ''
        _state_machine.userdata.config_name_agv1_r = 'Gantry_AGV1_R'
        _state_machine.userdata.config_name_agv1_l = 'Gantry_AGV1_L'
        _state_machine.userdata.config_name_agv2_l = 'Gantry_AGV2_L'
        _state_machine.userdata.config_name_agv2_r = 'Gantry_AGV2_R'
        _state_machine.userdata.agv2 = 'agv2'
        _state_machine.userdata.arm_id_r = 'Right_Arm'
        _state_machine.userdata.arm_id_l = 'Left_Arm'
        _state_machine.userdata.agv_pose = []
        _state_machine.userdata.offset = 0.09
        _state_machine.userdata.tool_link_r = 'right_ee_link'
        _state_machine.userdata.tool_link_l = 'left_ee_link'
        _state_machine.userdata.rotation = 0
        _state_machine.userdata.pose_on_agv_r = []
        _state_machine.userdata.pose_on_agv_l = []

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:43 y:101
            OperatableStateMachine.add('MoveGantryagv1',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'Getet agv pose',
                                           '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_agv2_r',
                                           'move_group': 'move_group_g',
                                           'move_group_prefix':
                                           'move_group_prefix_g',
                                           '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:57 y:574
            OperatableStateMachine.add('gripper active check',
                                       GripperActiveCheck(),
                                       transitions={
                                           'Left': 'finished',
                                           'Right': 'MoveGantryagv1-2',
                                           'failed': 'failed',
                                           'Full': 'MoveGantryagv1-2'
                                       },
                                       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:52 y:649
            OperatableStateMachine.add('MoveGantryagv1-2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'Getet agv pose_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': 'config_name_agv2_l',
                                           'move_group': 'move_group_g',
                                           'move_group_prefix':
                                           'move_group_prefix_g',
                                           '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:63 y:418
            OperatableStateMachine.add('gripper disabble',
                                       GripperControl(enable=False),
                                       transitions={
                                           'continue': 'MoveRightArm_2',
                                           'failed': 'failed',
                                           'invalid_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id_r'})

            # x:37 y:177
            OperatableStateMachine.add('Getet agv pose',
                                       GetObjectPoseState(
                                           object_frame='kit_tray_2',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'test',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'agv_pose'})

            # x:32 y:325
            OperatableStateMachine.add('move to drop',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'gripper disabble',
                                           '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_g',
                                           'move_group': 'move_group_r',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:17 y:258
            OperatableStateMachine.add(
                'test',
                ComputeDropPartOffsetGraspAriacState(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_r',
                    'move_group_prefix': 'move_group_prefix_g',
                    'tool_link': 'tool_link_r',
                    'part_pose': 'pose_on_agv_r',
                    'pose': 'agv_pose',
                    'offset': 'offset',
                    'rotation': 'rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:479 y:321
            OperatableStateMachine.add('gripper disabble_2',
                                       GripperControl(enable=False),
                                       transitions={
                                           'continue': 'MoveLeftArm_2',
                                           'failed': 'failed',
                                           'invalid_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id_l'})

            # x:355 y:72
            OperatableStateMachine.add('Getet agv pose_2',
                                       GetObjectPoseState(
                                           object_frame='kit_tray_2',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'test_2',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'agv_pose'})

            # x:440 y:245
            OperatableStateMachine.add('move to drop_2',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'gripper disabble_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_g',
                                           'move_group': 'move_group_l',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:322 y:159
            OperatableStateMachine.add(
                'test_2',
                ComputeDropPartOffsetGraspAriacState(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_2',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group_l',
                    'move_group_prefix': 'move_group_prefix_g',
                    'tool_link': 'tool_link_l',
                    'part_pose': 'pose_on_agv_l',
                    'pose': 'agv_pose',
                    'offset': 'offset',
                    'rotation': 'rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:59 y:504
            OperatableStateMachine.add('MoveRightArm_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'gripper active check',
                                           '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_right',
                                           'move_group': 'move_group_r',
                                           'move_group_prefix':
                                           'move_group_prefix_g',
                                           '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:509 y:417
            OperatableStateMachine.add('MoveLeftArm_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_left',
                                           'move_group': 'move_group_l',
                                           'move_group_prefix':
                                           'move_group_prefix_g',
                                           '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
Exemple #4
0
	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
Exemple #5
0
    def create(self):
        # x:52 y:611, x:343 y:359
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'])
        _state_machine.userdata.agv_id = 'agv1'
        _state_machine.userdata.part_type = 'gear_part'
        _state_machine.userdata.pose_on_agv = []
        _state_machine.userdata.pose = []
        _state_machine.userdata.part_pose = []
        _state_machine.userdata.joint_values = []
        _state_machine.userdata.joint_names = []
        _state_machine.userdata.part = 'gasket_part'
        _state_machine.userdata.offset = 0.1
        _state_machine.userdata.move_group = 'manipulator'
        _state_machine.userdata.move_group_prefix = '/ariac/arm1'
        _state_machine.userdata.config_name_home = 'home'
        _state_machine.userdata.action_topic = '/move_group'
        _state_machine.userdata.robot_name = ''
        _state_machine.userdata.config_name_bin3PreGrasp = 'bin3PreGrasp'
        _state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop'
        _state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator'
        _state_machine.userdata.camera_topic = '/ariac/logical_camera_5'
        _state_machine.userdata.camera_frame = 'logical_camera_5_frame'
        _state_machine.userdata.tool_link = 'ee_link'
        _state_machine.userdata.agv_pose = []
        _state_machine.userdata.part_offset = 0.035
        _state_machine.userdata.part_rotation = 0
        _state_machine.userdata.conveyor_belt_power = 100.0
        _state_machine.userdata.config_name_bin1PreGrasp = 'bin1PreGrasp'

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:54 y:102
            OperatableStateMachine.add(
                'StartAssignment',
                StartAssignment(),
                transitions={
                    'continue': 'transport_part_form_bin_to_agv_state'
                },
                autonomy={'continue': Autonomy.Off})

            # x:197 y:603
            OperatableStateMachine.add('EndAssignment',
                                       EndAssignment(),
                                       transitions={'continue': 'finished'},
                                       autonomy={'continue': Autonomy.Off})

            # x:189 y:100
            OperatableStateMachine.add(
                'transport_part_form_bin_to_agv_state',
                self.use_behavior(transport_part_form_bin_to_agv_stateSM,
                                  'transport_part_form_bin_to_agv_state'),
                transitions={
                    'finished': 'SetConveyorbeltPower',
                    'failed': 'failed'
                },
                autonomy={
                    'finished': Autonomy.Inherit,
                    'failed': Autonomy.Inherit
                },
                remapping={
                    'part_type': 'part_type',
                    'agv_id': 'agv_id',
                    'pose_on_agv': 'pose_on_agv'
                })

            # x:654 y:113
            OperatableStateMachine.add('MoveR1Home',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'DetectCameraPart',
                                           'planning_failed': 'WaitRetry1',
                                           'control_failed': 'WaitRetry1',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name_home',
                                           'move_group': 'move_group',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'action_topic': 'action_topic',
                                           'robot_name': 'robot_name',
                                           'config_name_out':
                                           'config_name_out',
                                           'move_group_out': 'move_group_out',
                                           'robot_name_out': 'robot_name_out',
                                           'action_topic_out':
                                           'action_topic_out',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:666 y:12
            OperatableStateMachine.add('WaitRetry1',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1Home'},
                                       autonomy={'done': Autonomy.Off})

            # x:836 y:111
            OperatableStateMachine.add('DetectCameraPart',
                                       DetectPartCameraAriacState(time_out=5),
                                       transitions={
                                           'continue': 'MoveR1PreGrasp1',
                                           'failed': 'failed',
                                           'not_found': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'not_found': Autonomy.Off
                                       },
                                       remapping={
                                           'ref_frame': 'camera_ref_frame',
                                           'camera_topic': 'camera_topic',
                                           'camera_frame': 'camera_frame',
                                           'part': 'part',
                                           'pose': 'pose'
                                       })

            # x:1039 y:116
            OperatableStateMachine.add('MoveR1PreGrasp1',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'ComputePick',
                                           'planning_failed': 'WaitRetry2',
                                           'control_failed': 'WaitRetry2',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_bin1PreGrasp',
                                           'move_group': 'move_group',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'action_topic': 'action_topic',
                                           'robot_name': 'robot_name',
                                           'config_name_out':
                                           'config_name_out',
                                           'move_group_out': 'move_group_out',
                                           'robot_name_out': 'robot_name_out',
                                           'action_topic_out':
                                           'action_topic_out',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1048 y:21
            OperatableStateMachine.add('WaitRetry2',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1PreGrasp1'},
                                       autonomy={'done': Autonomy.Off})

            # x:456 y:106
            OperatableStateMachine.add(
                'SetConveyorbeltPower',
                SetConveyorbeltPowerState(),
                transitions={
                    'continue': 'MoveR1Home',
                    'fail': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'fail': Autonomy.Off
                },
                remapping={'power': 'conveyor_belt_power'})

            # x:1038 y:200
            OperatableStateMachine.add(
                'ComputePick',
                ComputeGraspAriacState(joint_names=[
                    'linear_arm_actuator_joint', 'shoulder_pan_joint',
                    'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
                    'wrist_2_joint', 'wrist_3_joint'
                ]),
                transitions={
                    'continue': 'MoveR1ToPick1',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link',
                    'pose': 'pose',
                    'offset': 'part_offset',
                    'rotation': 'part_rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1069 y:379
            OperatableStateMachine.add('Wait',
                                       WaitState(wait_time=1),
                                       transitions={'done': 'MoveR1PreGrasp2'},
                                       autonomy={'done': Autonomy.Off})

            # x:1041 y:504
            OperatableStateMachine.add('MoveR1PreGrasp2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'MoveR1PreDrop',
                                           'planning_failed': 'WaitRetry4',
                                           'control_failed': 'WaitRetry4',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_bin1PreGrasp',
                                           'move_group': 'move_group',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'action_topic': 'action_topic',
                                           'robot_name': 'robot_name',
                                           'config_name_out':
                                           'config_name_out',
                                           'move_group_out': 'move_group_out',
                                           'robot_name_out': 'robot_name_out',
                                           'action_topic_out':
                                           'action_topic_out',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1303 y:280
            OperatableStateMachine.add('WaitRetry3',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1ToPick1'},
                                       autonomy={'done': Autonomy.Off})

            # x:1226 y:501
            OperatableStateMachine.add('WaitRetry4',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1PreGrasp2'},
                                       autonomy={'done': Autonomy.Off})

            # x:876 y:608
            OperatableStateMachine.add(
                'GetAgvPose',
                GetObjectPoseState(object_frame='kit_tray_1',
                                   ref_frame='arm1_linear_arm_actuator'),
                transitions={
                    'continue': 'ComputeDrop',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'agv_pose'})

            # x:1220 y:618
            OperatableStateMachine.add('WaitRerty5',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1PreDrop'},
                                       autonomy={'done': Autonomy.Off})

            # x:688 y:604
            OperatableStateMachine.add(
                'ComputeDrop',
                ComputeGraspAriacState(joint_names=[
                    'linear_arm_actuator_joint', 'shoulder_pan_joint',
                    'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
                    'wrist_2_joint', 'wrist_3_joint'
                ]),
                transitions={
                    'continue': 'MoveR1ToDrop',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link',
                    'pose': 'agv_pose',
                    'offset': 'part_offset',
                    'rotation': 'part_rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:430 y:601
            OperatableStateMachine.add('MoveR1ToDrop',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'EndAssignment',
                                           'planning_failed': 'WaitRetry6',
                                           'control_failed': 'WaitRetry6'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group': 'move_group',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:451 y:726
            OperatableStateMachine.add('WaitRetry6',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1ToDrop'},
                                       autonomy={'done': Autonomy.Off})

            # x:1035 y:284
            OperatableStateMachine.add('MoveR1ToPick1',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'Wait',
                                           'planning_failed': 'WaitRetry3',
                                           'control_failed': 'WaitRetry3'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group': 'move_group',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1042 y:611
            OperatableStateMachine.add('MoveR1PreDrop',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'GetAgvPose',
                                           'planning_failed': 'WaitRerty5',
                                           'control_failed': 'WaitRerty5',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_tray1PreDrop',
                                           'move_group': 'move_group',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'action_topic': 'action_topic',
                                           'robot_name': 'robot_name',
                                           'config_name_out':
                                           'config_name_out',
                                           'move_group_out': 'move_group_out',
                                           'robot_name_out': 'robot_name_out',
                                           'action_topic_out':
                                           'action_topic_out',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

        return _state_machine
    def create(self):
        # x:1723 y:19, x:585 y:171
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'],
            input_keys=[
                'ref_frame', 'config_name', 'move_group_prefix', 'arm_id',
                'agv_id', 'part_type'
            ],
            output_keys=[
                'move_group_prefix', 'config_name', 'arm_id', 'ref_frame',
                'camera_topic', 'camera_frame', 'part_offset', 'overzet'
            ])
        _state_machine.userdata.ref_frame = ''
        _state_machine.userdata.bin3_pose = ''
        _state_machine.userdata.config_name = ''
        _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.tool_link = 'ee_link'
        _state_machine.userdata.arm_id = ''
        _state_machine.userdata.offset = 0.5
        _state_machine.userdata.rotation = 0
        _state_machine.userdata.agv_id = ''
        _state_machine.userdata.agv1_id = 'agv1'
        _state_machine.userdata.agv2_id = 'agv2'
        _state_machine.userdata.part_offset = 0
        _state_machine.userdata.partoffset1 = 0.037
        _state_machine.userdata.partoffset2 = 0.085
        _state_machine.userdata.partoffset3 = 0.02
        _state_machine.userdata.partoffset4 = 0.025
        _state_machine.userdata.part_type = ''
        _state_machine.userdata.part1 = 'gasket_part'
        _state_machine.userdata.part2 = 'pulley_part'
        _state_machine.userdata.part3 = 'piston_rod_part'
        _state_machine.userdata.part4 = 'gear_part'
        _state_machine.userdata.move_group_prefix1 = '/ariac/arm1'
        _state_machine.userdata.move_group_prefix2 = '/ariac/arm2'
        _state_machine.userdata.config_namer1 = 'R1PreBin4'
        _state_machine.userdata.config_namer2 = 'R2PreBin4'
        _state_machine.userdata.arm1_id = 'arm1'
        _state_machine.userdata.arm2_id = 'arm2'
        _state_machine.userdata.camera_topic = ''
        _state_machine.userdata.camera_frame = ''
        _state_machine.userdata.camera_topic4 = '/ariac/Camera_Bin_4'
        _state_machine.userdata.camera_frame4 = 'Camera_Bin_4_frame'
        _state_machine.userdata.overzet = ''
        _state_machine.userdata.overzetnee = 'nee'
        _state_machine.userdata.ref_frame1 = 'arm1_linear_arm_actuator'
        _state_machine.userdata.ref_frame2 = 'arm2_linear_arm_actuator'
        _state_machine.userdata.config_name1 = 'R1PreBin1'
        _state_machine.userdata.config_name4 = 'R2PreBin6'

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:91 y:23
            OperatableStateMachine.add('Arm?',
                                       EqualState(),
                                       transitions={
                                           'true': 'R1',
                                           'false': 'R2'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'arm_id',
                                           'value_b': 'arm1_id'
                                       })

            # x:630 y:12
            OperatableStateMachine.add('PreBin3',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'ComputeDrop3',
                                           '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:907 y:30
            OperatableStateMachine.add(
                'ComputeDrop3',
                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': 'DropBin3',
                    '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': 'bin3_pose',
                    'offset': 'offset',
                    'rotation': 'rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1122 y:79
            OperatableStateMachine.add('DropBin3',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'OpengripperBin3',
                                           'planning_failed': 'failed',
                                           'control_failed': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group': 'move_group',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1142 y:173
            OperatableStateMachine.add('OpengripperBin3',
                                       UseGripper(enable=False),
                                       transitions={
                                           'continue': 'WachtenGripper',
                                           'failed': 'failed',
                                           'invalid_arm': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id'})

            # x:984 y:236
            OperatableStateMachine.add('WachtenGripper',
                                       WaitState(wait_time=1),
                                       transitions={'done': 'PreBin3Back'},
                                       autonomy={'done': Autonomy.Off})

            # x:799 y:244
            OperatableStateMachine.add('PreBin3Back',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'AGV?',
                                           '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:16 y:99
            OperatableStateMachine.add('AGV?',
                                       EqualState(),
                                       transitions={
                                           'true': 'PreBin4',
                                           'false': 'PreBin1'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'agv_id',
                                           'value_b': 'agv1_id'
                                       })

            # x:186 y:106
            OperatableStateMachine.add('R1',
                                       ReplaceState(),
                                       transitions={'done': 'Getbin3Pose'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'config_namer1',
                                           'result': 'config_name'
                                       })

            # x:254 y:189
            OperatableStateMachine.add('R2',
                                       ReplaceState(),
                                       transitions={'done': 'Getbin3Pose_2'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'config_namer2',
                                           'result': 'config_name'
                                       })

            # x:331 y:21
            OperatableStateMachine.add(
                'Getbin3Pose',
                GetObjectPoseState(object_frame='bin3_frame',
                                   ref_frame='arm1_linear_arm_actuator'),
                transitions={
                    'continue': 'PreBin3',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'bin3_pose'})

            # x:1615 y:384
            OperatableStateMachine.add('CameraTopic',
                                       ReplaceState(),
                                       transitions={'done': 'CameraFrame'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'camera_topic4',
                                           'result': 'camera_topic'
                                       })

            # x:1644 y:217
            OperatableStateMachine.add('CameraFrame',
                                       ReplaceState(),
                                       transitions={'done': 'Overzetnee'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'camera_frame4',
                                           'result': 'camera_frame'
                                       })

            # x:1651 y:97
            OperatableStateMachine.add('Overzetnee',
                                       ReplaceState(),
                                       transitions={'done': 'finished'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'overzetnee',
                                           'result': 'overzet'
                                       })

            # x:170 y:444
            OperatableStateMachine.add('rod',
                                       EqualState(),
                                       transitions={
                                           'true': 'RepaceOffset_3',
                                           'false': 'RepaceOffset_4'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'part_type',
                                           'value_b': 'part3'
                                       })

            # x:73 y:289
            OperatableStateMachine.add('gasket',
                                       EqualState(),
                                       transitions={
                                           'true': 'RepaceOffset',
                                           'false': 'pulley'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'part_type',
                                           'value_b': 'part1'
                                       })

            # x:171 y:357
            OperatableStateMachine.add('pulley',
                                       EqualState(),
                                       transitions={
                                           'true': 'RepaceOffset_2',
                                           'false': 'rod'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'part_type',
                                           'value_b': 'part2'
                                       })

            # x:86 y:713
            OperatableStateMachine.add('rod_2',
                                       EqualState(),
                                       transitions={
                                           'true': 'RepaceOffset_7',
                                           'false': 'RepaceOffset_8'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'part_type',
                                           'value_b': 'part3'
                                       })

            # x:43 y:523
            OperatableStateMachine.add('gasket_2',
                                       EqualState(),
                                       transitions={
                                           'true': 'RepaceOffset_5',
                                           'false': 'pulley_2'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'part_type',
                                           'value_b': 'part1'
                                       })

            # x:110 y:608
            OperatableStateMachine.add('pulley_2',
                                       EqualState(),
                                       transitions={
                                           'true': 'RepaceOffset_6',
                                           'false': 'rod_2'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'part_type',
                                           'value_b': 'part2'
                                       })

            # x:451 y:287
            OperatableStateMachine.add('RepaceOffset',
                                       ReplaceState(),
                                       transitions={'done': 'R1PreBin3'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'partoffset1',
                                           'result': 'part_offset'
                                       })

            # x:451 y:344
            OperatableStateMachine.add('RepaceOffset_2',
                                       ReplaceState(),
                                       transitions={'done': 'R1PreBin3'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'partoffset2',
                                           'result': 'part_offset'
                                       })

            # x:450 y:402
            OperatableStateMachine.add('RepaceOffset_3',
                                       ReplaceState(),
                                       transitions={'done': 'R1PreBin3'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'partoffset3',
                                           'result': 'part_offset'
                                       })

            # x:450 y:463
            OperatableStateMachine.add('RepaceOffset_4',
                                       ReplaceState(),
                                       transitions={'done': 'R1PreBin3'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'partoffset4',
                                           'result': 'part_offset'
                                       })

            # x:452 y:557
            OperatableStateMachine.add('RepaceOffset_5',
                                       ReplaceState(),
                                       transitions={'done': 'R1PreBin3_2'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'partoffset1',
                                           'result': 'part_offset'
                                       })

            # x:453 y:614
            OperatableStateMachine.add('RepaceOffset_6',
                                       ReplaceState(),
                                       transitions={'done': 'R1PreBin3_2'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'partoffset2',
                                           'result': 'part_offset'
                                       })

            # x:453 y:674
            OperatableStateMachine.add('RepaceOffset_7',
                                       ReplaceState(),
                                       transitions={'done': 'R1PreBin3_2'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'partoffset3',
                                           'result': 'part_offset'
                                       })

            # x:454 y:733
            OperatableStateMachine.add('RepaceOffset_8',
                                       ReplaceState(),
                                       transitions={'done': 'R1PreBin3_2'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'partoffset4',
                                           'result': 'part_offset'
                                       })

            # x:802 y:342
            OperatableStateMachine.add('R1PreBin3',
                                       ReplaceState(),
                                       transitions={'done': 'RefFrame'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'config_namer1',
                                           'result': 'config_name'
                                       })

            # x:958 y:342
            OperatableStateMachine.add('RefFrame',
                                       ReplaceState(),
                                       transitions={'done': 'MoveGroupPrefix'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'ref_frame1',
                                           'result': 'ref_frame'
                                       })

            # x:1115 y:342
            OperatableStateMachine.add('MoveGroupPrefix',
                                       ReplaceState(),
                                       transitions={'done': 'ArmID'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'move_group_prefix1',
                                           'result': 'move_group_prefix'
                                       })

            # x:1273 y:342
            OperatableStateMachine.add('ArmID',
                                       ReplaceState(),
                                       transitions={'done': 'CameraTopic'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'arm1_id',
                                           'result': 'arm_id'
                                       })

            # x:802 y:419
            OperatableStateMachine.add('R1PreBin3_2',
                                       ReplaceState(),
                                       transitions={'done': 'RefFrame_2'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'config_namer2',
                                           'result': 'config_name'
                                       })

            # x:958 y:419
            OperatableStateMachine.add(
                'RefFrame_2',
                ReplaceState(),
                transitions={'done': 'MoveGroupPrefix_2'},
                autonomy={'done': Autonomy.Off},
                remapping={
                    'value': 'ref_frame2',
                    'result': 'ref_frame'
                })

            # x:1115 y:419
            OperatableStateMachine.add('MoveGroupPrefix_2',
                                       ReplaceState(),
                                       transitions={'done': 'ArmID_2'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'move_group_prefix2',
                                           'result': 'move_group_prefix'
                                       })

            # x:1273 y:419
            OperatableStateMachine.add('ArmID_2',
                                       ReplaceState(),
                                       transitions={'done': 'CameraTopic'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'arm2_id',
                                           'result': 'arm_id'
                                       })

            # x:397 y:93
            OperatableStateMachine.add(
                'Getbin3Pose_2',
                GetObjectPoseState(object_frame='bin3_frame',
                                   ref_frame='arm2_linear_arm_actuator'),
                transitions={
                    'continue': 'PreBin3',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'bin3_pose'})

            # x:73 y:182
            OperatableStateMachine.add('PreBin4',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'gasket',
                                           '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_name4',
                                           '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:17 y:343
            OperatableStateMachine.add('PreBin1',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'gasket_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': 'config_name1',
                                           '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
Exemple #7
0
    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'
        _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.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
        _state_machine.userdata.MaterialsLocationList = []
        _state_machine.userdata.config_name_bin3PreDrop = 'bin3PreDrop'
        _state_machine.userdata.poseBin3 = []
        _state_machine.userdata.move_group_prefix2 = '/ariac/arm2'

        # 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:305 y:37
            OperatableStateMachine.add('MoseMessage',
                                       MessageState(),
                                       transitions={'continue': 'Wait'},
                                       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:499 y:40
            OperatableStateMachine.add('Wait',
                                       WaitState(wait_time=1),
                                       transitions={'done': 'MoveR1PreGrasp2'},
                                       autonomy={'done': Autonomy.Off})

            # x:647 y:43
            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:663 y:121
            OperatableStateMachine.add('WaitRetry4',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1PreGrasp2'},
                                       autonomy={'done': Autonomy.Off})

            # x:861 y:116
            OperatableStateMachine.add('WaitRerty5',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1PreDrop'},
                                       autonomy={'done': Autonomy.Off})

            # x:1148 y:38
            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': 'poseBin3',
                    'offset': 'part_offset',
                    'rotation': 'part_rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1013 y:153
            OperatableStateMachine.add('WaitRetry6',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1ToDrop'},
                                       autonomy={'done': Autonomy.Off})

            # x:821 y:39
            OperatableStateMachine.add('MoveR1PreDrop',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'GetBin3Pose',
                                           '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_bin3PreDrop',
                                           '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:1140 y:139
            OperatableStateMachine.add('MoveR1ToDrop',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'DisableGripper',
                                           'planning_failed': 'WaitRetry6',
                                           'control_failed': 'DisableGripper'
                                       },
                                       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:993 y:40
            OperatableStateMachine.add(
                'GetBin3Pose',
                GetObjectPoseState(object_frame='link::base',
                                   ref_frame='arm1_linear_arm_actuator'),
                transitions={
                    'continue': 'ComputeDrop',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'poseBin3'})

            # x:1154 y:350
            OperatableStateMachine.add('MoveR1Home',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'MoveR1Home_2',
                                           '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:992 y:355
            OperatableStateMachine.add('WaitRetry1',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1Home'},
                                       autonomy={'done': Autonomy.Off})

            # x:1163 y:445
            OperatableStateMachine.add('MoveR1Home_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'MoveR1PreDrop_2',
                                           'planning_failed': 'WaitRetry1_2',
                                           'control_failed': 'WaitRetry1_2',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name_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:1002 y:448
            OperatableStateMachine.add('WaitRetry1_2',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1Home_2'},
                                       autonomy={'done': Autonomy.Off})

            # x:1013 y:521
            OperatableStateMachine.add('WaitRerty5_2',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'MoveR1PreDrop_2'},
                                       autonomy={'done': Autonomy.Off})

            # x:1163 y:531
            OperatableStateMachine.add('MoveR1PreDrop_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'finished',
                                           'planning_failed': 'WaitRerty5_2',
                                           'control_failed': 'WaitRerty5_2',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_bin3PreDrop',
                                           '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:1141 y:228
            OperatableStateMachine.add('DisableGripper',
                                       VacuumGripperControlState(enable=False),
                                       transitions={
                                           'continue': 'MoveR1Home',
                                           '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
Exemple #8
0
    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:80 y:338, x:691 y:334
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'],
            input_keys=['part_type_right', 'part_type_left'])
        _state_machine.userdata.config_name_right_bin1 = 'rightArmBin1'
        _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.joint_values = []
        _state_machine.userdata.joint_names = ''
        _state_machine.userdata.part_type_right = ''
        _state_machine.userdata.part_type_left = ''
        _state_machine.userdata.config_name_right_bin2 = 'rightArmBin2'
        _state_machine.userdata.move_group_right_arm = 'Right_Arm'
        _state_machine.userdata.move_group_left_arm = 'Left_Arm'
        _state_machine.userdata.tool_link_right = 'right_ee_link'
        _state_machine.userdata.bin1_offset = 0
        _state_machine.userdata.rotation = 0
        _state_machine.userdata.poseBin1 = []
        _state_machine.userdata.tool_link_left = 'left_ee_link'
        _state_machine.userdata.arm1 = 'Right_Arm'
        _state_machine.userdata.arm2 = 'Left_Arm'
        _state_machine.userdata.config_name_right_home = 'Right_Home'
        _state_machine.userdata.config_name_left_home = 'Left_Home'
        _state_machine.userdata.config_name_left_bin1 = 'leftArmBin1'
        _state_machine.userdata.config_name_left_bin2 = 'leftArmBin2'
        _state_machine.userdata.poseBin2 = []
        _state_machine.userdata.iterator = 0.17
        _state_machine.userdata.offset = 0
        _state_machine.userdata.bin2_offset = 0
        _state_machine.userdata.config_name_right_left = 'gantryPosPlaceRightLeft'
        _state_machine.userdata.config_name_left_right = 'gantryPosPlaceLeftRight'

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:86 y:86
            OperatableStateMachine.add(
                'inspect_arm_part_type_right',
                self.use_behavior(inspect_arm_part_type_rightSM,
                                  'inspect_arm_part_type_right'),
                transitions={
                    'gasket_part': 'MoveToGasketBin',
                    'no_part': 'inspect_arm_part_type_left',
                    'piston_rod_part': 'MoveToPistonBin'
                },
                autonomy={
                    'gasket_part': Autonomy.Inherit,
                    'no_part': Autonomy.Inherit,
                    'piston_rod_part': Autonomy.Inherit
                },
                remapping={
                    'part_type_right': 'part_type_right',
                    'part_type_left': 'part_type_left'
                })

            # x:347 y:38
            OperatableStateMachine.add('MoveToPistonBin',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'GetPoseBin1',
                                           'planning_failed': 'failed',
                                           'control_failed': 'GetPoseBin1',
                                           '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_left',
                                           '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:348 y:161
            OperatableStateMachine.add('MoveToGasketBin',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'GetPoseBin2',
                                           'planning_failed': 'failed',
                                           'control_failed': 'GetPoseBin2',
                                           '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_right',
                                           '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:675 y:36
            OperatableStateMachine.add(
                'ComputeDropPartPiston',
                ComputeDropAriacState(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': 'MoveToPlacePiston',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group_right_arm',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link_right',
                    'pose': 'poseBin1',
                    'offset': 'bin1_offset',
                    'rotation': 'rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:675 y:161
            OperatableStateMachine.add(
                'ComputeDropPartGasket',
                ComputeDropAriacState(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': 'MoveToPlaceGasket',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group_right_arm',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link_right',
                    'pose': 'poseBin2',
                    'offset': 'bin2_offset',
                    'rotation': 'rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:860 y:37
            OperatableStateMachine.add('MoveToPlacePiston',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'DisableGripperPiston',
                                           'planning_failed':
                                           'ComputeDropPartPiston',
                                           'control_failed':
                                           'DisableGripperPiston'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group':
                                           'move_group_right_arm',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:869 y:158
            OperatableStateMachine.add('MoveToPlaceGasket',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'DisableGripperGasket',
                                           'planning_failed':
                                           'ComputeDropPartGasket',
                                           'control_failed':
                                           'DisableGripperGasket'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group':
                                           'move_group_right_arm',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1080 y:38
            OperatableStateMachine.add('DisableGripperPiston',
                                       GripperControl(enable=False),
                                       transitions={
                                           'continue': 'MoveRightHome',
                                           'failed': 'failed',
                                           'invalid_id': 'MoveToPlacePiston'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm1'})

            # x:1086 y:154
            OperatableStateMachine.add('DisableGripperGasket',
                                       GripperControl(enable=False),
                                       transitions={
                                           'continue': 'MoveRightHome_2',
                                           'failed': 'failed',
                                           'invalid_id': 'MoveToPlaceGasket'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm1'})

            # x:1243 y:36
            OperatableStateMachine.add(
                'MoveRightHome',
                SrdfStateToMoveitAriac(),
                transitions={
                    'reached': 'AddOffsetBin1',
                    'planning_failed': 'failed',
                    'control_failed': 'AddOffsetBin1',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name_right_home',
                    'move_group': 'move_group_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:1251 y:150
            OperatableStateMachine.add(
                'MoveRightHome_2',
                SrdfStateToMoveitAriac(),
                transitions={
                    'reached': 'AddOffsetBin2',
                    'planning_failed': 'failed',
                    'control_failed': 'AddOffsetBin2',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name_right_home',
                    'move_group': 'move_group_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:1405 y:510
            OperatableStateMachine.add(
                'inspect_arm_part_type_left',
                self.use_behavior(inspect_arm_part_type_leftSM,
                                  'inspect_arm_part_type_left'),
                transitions={
                    'gasket_part': 'MoveToGasketBinLeft',
                    'no_part': 'finished',
                    'piston_rod_part': 'MoveToPistonBinLeft'
                },
                autonomy={
                    'gasket_part': Autonomy.Inherit,
                    'no_part': Autonomy.Inherit,
                    'piston_rod_part': Autonomy.Inherit
                },
                remapping={
                    'part_type_right': 'part_type_right',
                    'part_type_left': 'part_type_left'
                })

            # x:1188 y:444
            OperatableStateMachine.add('MoveToPistonBinLeft',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'GetPoseBin1Left',
                                           'planning_failed': 'failed',
                                           'control_failed': 'GetPoseBin1Left',
                                           '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_right',
                                           '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:1197 y:594
            OperatableStateMachine.add('MoveToGasketBinLeft',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'GetPoseBin2_2',
                                           'planning_failed': 'failed',
                                           'control_failed': 'GetPoseBin2_2',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_right_left',
                                           '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:792 y:449
            OperatableStateMachine.add(
                'ComputeDropPartPistonLeft',
                ComputeDropAriacState(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': 'MoveToPlacePistonLeft',
                    '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': 'poseBin1',
                    'offset': 'bin1_offset',
                    'rotation': 'rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:810 y:591
            OperatableStateMachine.add(
                'ComputeDropPartGasketLeft',
                ComputeDropAriacState(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': 'MoveToPlaceGasketLeft',
                    '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': 'poseBin2',
                    'offset': 'bin2_offset',
                    'rotation': 'rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:585 y:448
            OperatableStateMachine.add('MoveToPlacePistonLeft',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached':
                                           'DisableGripperPistonLeft',
                                           'planning_failed':
                                           'ComputeDropPartPistonLeft',
                                           'control_failed':
                                           'DisableGripperPistonLeft'
                                       },
                                       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:581 y:590
            OperatableStateMachine.add('MoveToPlaceGasketLeft',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached':
                                           'DisableGripperGasketLeft',
                                           'planning_failed':
                                           'ComputeDropPartGasketLeft',
                                           'control_failed':
                                           'DisableGripperGasketLeft'
                                       },
                                       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:393 y:446
            OperatableStateMachine.add('DisableGripperPistonLeft',
                                       GripperControl(enable=False),
                                       transitions={
                                           'continue': 'MoveLeftHome',
                                           'failed': 'failed',
                                           'invalid_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm2'})

            # x:390 y:589
            OperatableStateMachine.add('DisableGripperGasketLeft',
                                       GripperControl(enable=False),
                                       transitions={
                                           'continue': 'MoveLeftHome_2',
                                           'failed': 'failed',
                                           'invalid_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm2'})

            # x:225 y:590
            OperatableStateMachine.add('MoveLeftHome_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'AddOffsetBin2_2',
                                           'planning_failed': 'failed',
                                           'control_failed': 'AddOffsetBin2_2',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_left_home',
                                           '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:219 y:444
            OperatableStateMachine.add('MoveLeftHome',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'AddOffsetBin1_2',
                                           'planning_failed': 'failed',
                                           'control_failed': 'AddOffsetBin1_2',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_left_home',
                                           '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:512 y:36
            OperatableStateMachine.add('GetPoseBin1',
                                       GetObjectPoseState(
                                           object_frame='bin1_frame',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'ComputeDropPartPiston',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'poseBin1'})

            # x:1407 y:36
            OperatableStateMachine.add(
                'AddOffsetBin1',
                AddNumericState(),
                transitions={'done': 'inspect_arm_part_type_left'},
                autonomy={'done': Autonomy.Off},
                remapping={
                    'value_a': 'bin1_offset',
                    'value_b': 'iterator',
                    'result': 'bin1_offset'
                })

            # x:516 y:160
            OperatableStateMachine.add('GetPoseBin2',
                                       GetObjectPoseState(
                                           object_frame='bin2_frame',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'ComputeDropPartGasket',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'poseBin2'})

            # x:1412 y:148
            OperatableStateMachine.add(
                'AddOffsetBin2',
                AddNumericState(),
                transitions={'done': 'inspect_arm_part_type_left'},
                autonomy={'done': Autonomy.Off},
                remapping={
                    'value_a': 'bin2_offset',
                    'value_b': 'iterator',
                    'result': 'bin2_offset'
                })

            # x:1010 y:446
            OperatableStateMachine.add(
                'GetPoseBin1Left',
                GetObjectPoseState(object_frame='bin1_frame',
                                   ref_frame='world'),
                transitions={
                    'continue': 'ComputeDropPartPistonLeft',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'poseBin1'})

            # x:1011 y:591
            OperatableStateMachine.add(
                'GetPoseBin2_2',
                GetObjectPoseState(object_frame='bin2_frame',
                                   ref_frame='world'),
                transitions={
                    'continue': 'ComputeDropPartGasketLeft',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'poseBin2'})

            # x:34 y:439
            OperatableStateMachine.add('AddOffsetBin1_2',
                                       AddNumericState(),
                                       transitions={'done': 'finished'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value_a': 'bin1_offset',
                                           'value_b': 'iterator',
                                           'result': 'bin1_offset'
                                       })

            # x:39 y:583
            OperatableStateMachine.add('AddOffsetBin2_2',
                                       AddNumericState(),
                                       transitions={'done': 'finished'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value_a': 'bin2_offset',
                                           'value_b': 'iterator',
                                           'result': 'bin2_offset'
                                       })

        return _state_machine
Exemple #10
0
    def create(self):
        # x:386 y:51, x:238 y:167
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'], input_keys=['part', 'arm_id'])
        _state_machine.userdata.rotation = 0
        _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
        _state_machine.userdata.ref_frame = 'world'
        _state_machine.userdata.camera_topic = ''
        _state_machine.userdata.camera_frame = ''
        _state_machine.userdata.home = 'Home'
        _state_machine.userdata.part = ''
        _state_machine.userdata.belt = 'Gantry_PreShelf'
        _state_machine.userdata.agv_pose = ''
        _state_machine.userdata.part_pose = []

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:30 y:40
            OperatableStateMachine.add('Move_Belt',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'Bin_Pose',
                                           'planning_failed': 'Waitstate',
                                           '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': 'belt',
                                           '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:26 y:242
            OperatableStateMachine.add('Waitstate',
                                       self.use_behavior(
                                           WaitstateSM, 'Waitstate'),
                                       transitions={
                                           'finished': 'Move_Belt',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'finished': Autonomy.Inherit,
                                           'failed': Autonomy.Inherit
                                       })

            # x:400 y:146
            OperatableStateMachine.add('Bin_Pose',
                                       GetObjectPoseState(object_frame='bin5',
                                                          ref_frame='world'),
                                       transitions={
                                           'continue': 'Compute_drop',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'bin_pose'})

            # x:614 y:186
            OperatableStateMachine.add('Compute_drop',
                                       ComputeDropPart(),
                                       transitions={
                                           'continue': 'Detect_Part',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group': 'move_group',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'part_pose': 'part_pose',
                                           'agv_pose': 'bin_pose',
                                           'offset': 'offset',
                                           'rotation': 'rotation',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:776 y:184
            OperatableStateMachine.add(
                'Detect_Part',
                DetectPartCameraAriacState(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',
                    'pose': 'part_pose'
                })

        return _state_machine
Exemple #11
0
	def create(self):
		# x:620 y:659, x:597 y:393
		_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin', 'move_group_prefix', 'camera_topic', 'camera_frame', 'ref_frame', 'agv_id', 'part_type', 'part_pose'])
		_state_machine.userdata.config_name_home = 'home'
		_state_machine.userdata.move_group_prefix = []
		_state_machine.userdata.move_group = 'manipulator'
		_state_machine.userdata.action_topic = '/move_group'
		_state_machine.userdata.robot_name = ''
		_state_machine.userdata.agv_id = []
		_state_machine.userdata.joint_values = []
		_state_machine.userdata.joint_names = []
		_state_machine.userdata.bin = []
		_state_machine.userdata.part_type = ''
		_state_machine.userdata.config_name_tray = 'tray1PreDrop'
		_state_machine.userdata.pose = []
		_state_machine.userdata.part_offset = 0.08
		_state_machine.userdata.part_rotation = 0
		_state_machine.userdata.tool_link = 'ee_link'
		_state_machine.userdata.arm_id = 'arm2'
		_state_machine.userdata.camera_topic = []
		_state_machine.userdata.camera_frame = []
		_state_machine.userdata.ref_frame = []
		_state_machine.userdata.part_pose = []
		_state_machine.userdata.agv_pose = []

		# Additional creation code can be added inside the following tags
		# [MANUAL_CREATE]
		
		# [/MANUAL_CREATE]


		with _state_machine:
			# x:93 y:87
			OperatableStateMachine.add('OffsetCalc',
										part_offsetCalc(),
										transitions={'succes': 'MoveToHome', 'unknown_id': 'failed'},
										autonomy={'succes': Autonomy.Off, 'unknown_id': Autonomy.Off},
										remapping={'part_type': 'part_type', 'part_offset': 'part_offset'})

			# x:247 y:61
			OperatableStateMachine.add('MoveToHome',
										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': '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:648 y:61
			OperatableStateMachine.add('MoveBin',
										SrdfStateToMoveitAriac(),
										transitions={'reached': 'ComputeGrasp', '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:1123 y:281
			OperatableStateMachine.add('MoveToTray',
										SrdfStateToMoveitAriac(),
										transitions={'reached': 'GetAgvPose', '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_tray', '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:808 y:60
			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': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:1061 y:134
			OperatableStateMachine.add('GripperEnabled',
										GripperControl(enable=True),
										transitions={'continue': 'MoveToHome_2', 'failed': 'MoveToPart', 'invalid_id': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_id': Autonomy.Off},
										remapping={'arm_id': 'arm_id'})

			# x:423 y:55
			OperatableStateMachine.add('DetectPartPose',
										DetectPartCameraAriacState(time_out=.5),
										transitions={'continue': 'MoveBin', '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:982 y:61
			OperatableStateMachine.add('MoveToPart',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'GripperEnabled', 'planning_failed': 'failed', 'control_failed': 'GripperEnabled'},
										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:1126 y:214
			OperatableStateMachine.add('MoveToHome_2',
										SrdfStateToMoveitAriac(),
										transitions={'reached': 'MoveToTray', 'planning_failed': 'failed', 'control_failed': 'MoveToHome_2', 'param_error': 'failed'},
										autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
										remapping={'config_name': 'config_name_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:1023 y:566
			OperatableStateMachine.add('GripperDisabled',
										GripperControl(enable=False),
										transitions={'continue': 'MoveToHome_3', 'failed': 'failed', 'invalid_id': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_id': Autonomy.Off},
										remapping={'arm_id': 'arm_id'})

			# x:932 y:632
			OperatableStateMachine.add('MoveToHome_3',
										SrdfStateToMoveitAriac(),
										transitions={'reached': 'finished', '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:1112 y:356
			OperatableStateMachine.add('GetAgvPose',
										GetObjectPoseState(object_frame='kit_tray_2', ref_frame='arm2_linear_arm_actuator'),
										transitions={'continue': 'ComputeDrop', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'pose': 'agv_pose'})

			# x:1042 y:497
			OperatableStateMachine.add('MoveR1ToDrop',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'GripperDisabled', 'planning_failed': 'failed', 'control_failed': 'MoveR1ToDrop'},
										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:1007 y:426
			OperatableStateMachine.add('ComputeDrop',
										ComputeDropPartOffsetGraspAriacState(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', 'part_pose': 'part_pose', 'pose': 'agv_pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})


		return _state_machine
	def create(self):
		# x:1249 y:486, x:614 y:299, x:1274 y:293, x:1759 y:363
		_state_machine = OperatableStateMachine(outcomes=['Next arm', 'failed', 'BothArmsEmpty', 'BinVol'], input_keys=['USEarmID', 'GantryLocation', 'RightArmItem', 'LeftArmItem'], output_keys=['RightArmItem', 'LeftArmItem'])
		_state_machine.userdata.arm_idL = 'left_arm'
		_state_machine.userdata.arm_idR = 'right_arm'
		_state_machine.userdata.USEarmID = ''
		_state_machine.userdata.move_groupR = 'Right_Arm'
		_state_machine.userdata.move_groupL = 'Left_Arm'
		_state_machine.userdata.tool_linkL = 'left_ee_link'
		_state_machine.userdata.tool_linkR = 'right_ee_link'
		_state_machine.userdata.DropPose = []
		_state_machine.userdata.offsetXPiston = 0
		_state_machine.userdata.offsetPiston = 0
		_state_machine.userdata.offsetYPiston = 0
		_state_machine.userdata.rotation = 0
		_state_machine.userdata.move_group_prefix = '/ariac/gantry'
		_state_machine.userdata.PistonOffset = 0.22
		_state_machine.userdata.GasketOffset = 0.15
		_state_machine.userdata.offsetGasket = 0
		_state_machine.userdata.offsetXGasket = -0.1
		_state_machine.userdata.offsetYGasket = 0
		_state_machine.userdata.GasketLocation = 'GasketLoc'
		_state_machine.userdata.PistonLocation = 'PistonLoc'
		_state_machine.userdata.GantryLocation = ''
		_state_machine.userdata.action_topic = '/move_group'
		_state_machine.userdata.Niks = 'Niks'
		_state_machine.userdata.RightArmItem = ''
		_state_machine.userdata.LeftArmItem = ''
		_state_machine.userdata.SideXoffset = 0
		_state_machine.userdata.SideYoffset = 0
		_state_machine.userdata.SideZoffset = 0
		_state_machine.userdata.GasketRow = 0
		_state_machine.userdata.PistonRow = 0
		_state_machine.userdata.RowTarget = 4
		_state_machine.userdata.GasketRound = 0
		_state_machine.userdata.PistonRound = 0
		_state_machine.userdata.RoundTarget = 12
		_state_machine.userdata.Zero_Value = 0
		_state_machine.userdata.One_value = 1
		_state_machine.userdata.move_groupG = 'Gantry'
		_state_machine.userdata.config_nameSH3 = 'Gantry_SafeHall3'
		_state_machine.userdata.robot_name = ''

		# Additional creation code can be added inside the following tags
		# [MANUAL_CREATE]
		
		# [/MANUAL_CREATE]


		with _state_machine:
			# x:47 y:31
			OperatableStateMachine.add('GasketLocation',
										EqualState(),
										transitions={'true': 'GetGasketBinPosition', 'false': 'PistonLocation'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'GasketLocation', 'value_b': 'GantryLocation'})

			# x:216 y:33
			OperatableStateMachine.add('GetGasketBinPosition',
										GetObjectPoseState(object_frame='bin12_frame', ref_frame='world'),
										transitions={'continue': 'SetGasketBinXOffset', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'pose': 'DropPose'})

			# x:208 y:544
			OperatableStateMachine.add('GetPistonBinPosition',
										GetObjectPoseState(object_frame='bin1_frame', ref_frame='world'),
										transitions={'continue': 'SetPistonBinYOffset', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'pose': 'DropPose'})

			# x:1363 y:346
			OperatableStateMachine.add('checkLeftArmAttached',
										CheckGripperattached(),
										transitions={'True': 'Next arm', 'False': 'BothArmsEmpty', 'invalid_arm_id': 'failed'},
										autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
										remapping={'arm_id': 'arm_idL'})

			# x:1028 y:38
			OperatableStateMachine.add('GripperUITRechts',
										VacuumGripperControlState(enable=False),
										transitions={'continue': 'ResetRightArmItem', 'failed': 'GoToDropRight', 'invalid_arm_id': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
										remapping={'arm_id': 'arm_idR'})

			# x:848 y:38
			OperatableStateMachine.add('GoToDropRight',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'GripperUITRechts', 'planning_failed': 'GripperUITRechts', 'control_failed': 'GripperUITRechts'},
										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:1563 y:393
			OperatableStateMachine.add('StartUP',
										self.use_behavior(StartUPSM, 'StartUP'),
										transitions={'finished': 'checkRightArmAttached', 'failed': 'failed'},
										autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})

			# x:1203 y:37
			OperatableStateMachine.add('ResetRightArmItem',
										ReplaceState(),
										transitions={'done': 'AddX2offsetGasket'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'Niks', 'result': 'RightArmItem'})

			# x:1029 y:100
			OperatableStateMachine.add('GripperUITLinks',
										VacuumGripperControlState(enable=False),
										transitions={'continue': 'ResetLeftArmItem', 'failed': 'GoToDropLeft', 'invalid_arm_id': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
										remapping={'arm_id': 'arm_idL'})

			# x:44 y:542
			OperatableStateMachine.add('PistonLocation',
										EqualState(),
										transitions={'true': 'GetPistonBinPosition', 'false': 'failed'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'PistonLocation', 'value_b': 'GantryLocation'})

			# x:848 y:98
			OperatableStateMachine.add('GoToDropLeft',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'GripperUITLinks', 'planning_failed': 'GripperUITLinks', 'control_failed': 'GripperUITLinks'},
										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:1203 y:100
			OperatableStateMachine.add('ResetLeftArmItem',
										ReplaceState(),
										transitions={'done': 'AddX2offsetGasket'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'Niks', 'result': 'LeftArmItem'})

			# x:1359 y:423
			OperatableStateMachine.add('checkRightArmAttached',
										CheckGripperattached(),
										transitions={'True': 'Next arm', 'False': 'checkLeftArmAttached', 'invalid_arm_id': 'failed'},
										autonomy={'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
										remapping={'arm_id': 'arm_idR'})

			# x:708 y:35
			OperatableStateMachine.add('ComputeRightDrop',
										ComputeBeltDrop(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': 'GoToDropRight', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'move_group': 'move_groupR', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_linkR', 'pose': 'DropPose', 'offset': 'SideZoffset', 'SideYoffset': 'SideYoffset', 'SideXoffset': 'SideXoffset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:706 y:99
			OperatableStateMachine.add('ComputeLeftDrop',
										ComputeBeltDrop(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': 'GoToDropLeft', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'move_group': 'move_groupL', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_linkL', 'pose': 'DropPose', 'offset': 'SideZoffset', 'SideYoffset': 'SideYoffset', 'SideXoffset': 'SideXoffset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:365 y:34
			OperatableStateMachine.add('SetGasketBinXOffset',
										ReplaceState(),
										transitions={'done': 'SetGasketBinYOffset'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'offsetXGasket', 'result': 'SideXoffset'})

			# x:362 y:99
			OperatableStateMachine.add('SetGasketBinYOffset',
										ReplaceState(),
										transitions={'done': 'RechterARM?2'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'offsetYGasket', 'result': 'SideYoffset'})

			# x:350 y:606
			OperatableStateMachine.add('SetPistonBinXOffset',
										ReplaceState(),
										transitions={'done': 'RechterARM?'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'offsetXPiston', 'result': 'SideXoffset'})

			# x:352 y:544
			OperatableStateMachine.add('SetPistonBinYOffset',
										ReplaceState(),
										transitions={'done': 'SetPistonBinXOffset'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'offsetYPiston', 'result': 'SideYoffset'})

			# x:1378 y:35
			OperatableStateMachine.add('AddX2offsetGasket',
										AddNumericState(),
										transitions={'done': 'CheckGasketRows'},
										autonomy={'done': Autonomy.Off},
										remapping={'value_a': 'offsetXGasket', 'value_b': 'GasketOffset', 'result': 'offsetXGasket'})

			# x:1378 y:98
			OperatableStateMachine.add('CheckGasketRows',
										EqualState(),
										transitions={'true': 'ResetGasketRow', 'false': 'AddGasketRound'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'GasketRound', 'value_b': 'RowTarget'})

			# x:1376 y:158
			OperatableStateMachine.add('ResetGasketRow',
										ReplaceState(),
										transitions={'done': 'AddYoffsetGasket'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'Zero_Value', 'result': 'offsetXGasket'})

			# x:1373 y:220
			OperatableStateMachine.add('AddYoffsetGasket',
										AddNumericState(),
										transitions={'done': 'AddGasketRound'},
										autonomy={'done': Autonomy.Off},
										remapping={'value_a': 'GasketOffset', 'value_b': 'offsetYGasket', 'result': 'offsetYGasket'})

			# x:1551 y:37
			OperatableStateMachine.add('AddGasketRound',
										AddNumericState(),
										transitions={'done': 'CheckGasketRound'},
										autonomy={'done': Autonomy.Off},
										remapping={'value_a': 'One_value', 'value_b': 'GasketRound', 'result': 'GasketRound'})

			# x:1551 y:102
			OperatableStateMachine.add('CheckGasketRound',
										EqualState(),
										transitions={'true': 'ResetGasketRound', 'false': 'BackToSH3'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'GasketRound', 'value_b': 'RoundTarget'})

			# x:1717 y:103
			OperatableStateMachine.add('ResetGasketRound',
										ReplaceState(),
										transitions={'done': 'BinVol'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'Zero_Value', 'result': 'offsetYGasket'})

			# x:534 y:33
			OperatableStateMachine.add('RechterARM?2',
										EqualState(),
										transitions={'true': 'ComputeRightDrop', 'false': 'ComputeLeftDrop'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'USEarmID', 'value_b': 'arm_idR'})

			# x:518 y:544
			OperatableStateMachine.add('RechterARM?',
										EqualState(),
										transitions={'true': 'ComputeRightDrop_2', 'false': 'ComputeLeftDrop_2'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'USEarmID', 'value_b': 'arm_idR'})

			# x:689 y:544
			OperatableStateMachine.add('ComputeRightDrop_2',
										ComputeBeltDrop(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': 'GoToDropRight_2', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'move_group': 'move_groupR', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_linkR', 'pose': 'DropPose', 'offset': 'SideZoffset', 'SideYoffset': 'SideYoffset', 'SideXoffset': 'SideXoffset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:688 y:609
			OperatableStateMachine.add('ComputeLeftDrop_2',
										ComputeBeltDrop(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': 'GoToDropLeft_2', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'move_group': 'move_groupL', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_linkL', 'pose': 'DropPose', 'offset': 'SideZoffset', 'SideYoffset': 'SideYoffset', 'SideXoffset': 'SideXoffset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})

			# x:836 y:545
			OperatableStateMachine.add('GoToDropRight_2',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'GripperUITRechts_2', 'planning_failed': 'GripperUITRechts_2', 'control_failed': 'GripperUITRechts_2'},
										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:834 y:607
			OperatableStateMachine.add('GoToDropLeft_2',
										MoveitToJointsDynAriacState(),
										transitions={'reached': 'GripperUITLinks_2', 'planning_failed': 'GripperUITLinks_2', 'control_failed': 'GripperUITLinks_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:1008 y:547
			OperatableStateMachine.add('GripperUITRechts_2',
										VacuumGripperControlState(enable=False),
										transitions={'continue': 'ResetRightArmItem_2', 'failed': 'GoToDropRight_2', 'invalid_arm_id': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
										remapping={'arm_id': 'arm_idR'})

			# x:1008 y:606
			OperatableStateMachine.add('GripperUITLinks_2',
										VacuumGripperControlState(enable=False),
										transitions={'continue': 'ResetLeftArmItem_2', 'failed': 'GoToDropLeft_2', 'invalid_arm_id': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
										remapping={'arm_id': 'arm_idL'})

			# x:1173 y:549
			OperatableStateMachine.add('ResetRightArmItem_2',
										ReplaceState(),
										transitions={'done': 'AddX2offsetPiston'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'Niks', 'result': 'RightArmItem'})

			# x:1173 y:607
			OperatableStateMachine.add('ResetLeftArmItem_2',
										ReplaceState(),
										transitions={'done': 'AddX2offsetPiston'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'Niks', 'result': 'LeftArmItem'})

			# x:1343 y:550
			OperatableStateMachine.add('AddX2offsetPiston',
										AddNumericState(),
										transitions={'done': 'CheckPistonRows'},
										autonomy={'done': Autonomy.Off},
										remapping={'value_a': 'offsetXPiston', 'value_b': 'PistonOffset', 'result': 'offsetXPiston'})

			# x:1517 y:602
			OperatableStateMachine.add('CheckPistonRound',
										EqualState(),
										transitions={'true': 'ResetPistonRound', 'false': 'StartUP'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'PistonRound', 'value_b': 'RoundTarget'})

			# x:1343 y:615
			OperatableStateMachine.add('CheckPistonRows',
										EqualState(),
										transitions={'true': 'ResetPistonRow', 'false': 'AddPistonRound'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'PistonRound', 'value_b': 'RowTarget'})

			# x:1342 y:676
			OperatableStateMachine.add('ResetPistonRow',
										ReplaceState(),
										transitions={'done': 'AddYoffsetPiston'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'Zero_Value', 'result': 'offsetXPiston'})

			# x:1340 y:735
			OperatableStateMachine.add('AddYoffsetPiston',
										AddNumericState(),
										transitions={'done': 'AddPistonRound'},
										autonomy={'done': Autonomy.Off},
										remapping={'value_a': 'PistonOffset', 'value_b': 'offsetYPiston', 'result': 'offsetYPiston'})

			# x:1514 y:669
			OperatableStateMachine.add('AddPistonRound',
										AddNumericState(),
										transitions={'done': 'CheckPistonRound'},
										autonomy={'done': Autonomy.Off},
										remapping={'value_a': 'One_value', 'value_b': 'PistonRound', 'result': 'PistonRound'})

			# x:1688 y:602
			OperatableStateMachine.add('ResetPistonRound',
										ReplaceState(),
										transitions={'done': 'BinVol'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'Zero_Value', 'result': 'offsetYPiston'})

			# x:1559 y:228
			OperatableStateMachine.add('BackToSH3',
										SrdfStateToMoveitAriac(),
										transitions={'reached': 'StartUP', '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_nameSH3', 'move_group': 'move_groupG', '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
Exemple #13
0
	def create(self):
		# x:1424 y:263, x:645 y:297
		_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['arm', 'partarmlinks', 'partarmrechts'], output_keys=['arm', 'arm_id', 'move_group', 'config_nameA', 'binpose', 'config_name_bin'])
		_state_machine.userdata.binpose = []
		_state_machine.userdata.arm = ''
		_state_machine.userdata.armlinks = 'links'
		_state_machine.userdata.armklaar = 'klaar'
		_state_machine.userdata.move_group = ''
		_state_machine.userdata.move_groupL = 'Left_Arm'
		_state_machine.userdata.move_groupR = 'Right_Arm'
		_state_machine.userdata.arm_id = ''
		_state_machine.userdata.armidl = 'left'
		_state_machine.userdata.armidr = 'right'
		_state_machine.userdata.config_nameA = ''
		_state_machine.userdata.config_namePLA = 'Left_Pre_Band'
		_state_machine.userdata.config_namePRA = 'Right_Pre_Band'
		_state_machine.userdata.config_name_bin = ''
		_state_machine.userdata.config_nameRechtsGasket = 'Gantry_R-gasketB_L-pistonR'
		_state_machine.userdata.config_nameRechtsPiston = 'Gantry_R-pistonR_L-gasketB'
		_state_machine.userdata.partarmlinks = ''
		_state_machine.userdata.partarmrechts = ''
		_state_machine.userdata.pistonpart = 'piston_rod_part_red'
		_state_machine.userdata.gasketpart = 'gasket_part_blue'

		# Additional creation code can be added inside the following tags
		# [MANUAL_CREATE]
		
		# [/MANUAL_CREATE]


		with _state_machine:
			# x:40 y:334
			OperatableStateMachine.add('Arm?',
										EqualState(),
										transitions={'true': 'WelkePartLinks?', 'false': 'WelkePart?'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'arm', 'value_b': 'armlinks'})

			# x:708 y:459
			OperatableStateMachine.add('ArmIDLinks',
										ReplaceState(),
										transitions={'done': 'MoveGroupLinks'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'armidl', 'result': 'arm_id'})

			# x:681 y:52
			OperatableStateMachine.add('ArmIDRechts',
										ReplaceState(),
										transitions={'done': 'MoveGroupRechts'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'armidr', 'result': 'arm_id'})

			# x:882 y:50
			OperatableStateMachine.add('MoveGroupRechts',
										ReplaceState(),
										transitions={'done': 'ArmNaarLinks'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'move_groupR', 'result': 'move_group'})

			# x:1105 y:53
			OperatableStateMachine.add('ArmNaarLinks',
										ReplaceState(),
										transitions={'done': 'ConfigRechtsHome'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'armlinks', 'result': 'arm'})

			# x:1249 y:458
			OperatableStateMachine.add('ArmNaarKlaar',
										ReplaceState(),
										transitions={'done': 'ConfigLinksHome'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'armklaar', 'result': 'arm'})

			# x:1207 y:161
			OperatableStateMachine.add('ConfigRechtsHome',
										ReplaceState(),
										transitions={'done': 'finished'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'config_namePRA', 'result': 'config_nameA'})

			# x:1227 y:359
			OperatableStateMachine.add('ConfigLinksHome',
										ReplaceState(),
										transitions={'done': 'finished'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'config_namePLA', 'result': 'config_nameA'})

			# x:473 y:56
			OperatableStateMachine.add('GetPoseBin2',
										GetObjectPoseState(object_frame='bin2_frame', ref_frame='world'),
										transitions={'continue': 'ArmIDRechts', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'pose': 'binpose'})

			# x:488 y:550
			OperatableStateMachine.add('GetPoseBin7',
										GetObjectPoseState(object_frame='bin7_frame', ref_frame='world'),
										transitions={'continue': 'ArmIDLinks', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'pose': 'binpose'})

			# x:951 y:460
			OperatableStateMachine.add('MoveGroupLinks',
										ReplaceState(),
										transitions={'done': 'ArmNaarKlaar'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'move_groupL', 'result': 'move_group'})

			# x:69 y:138
			OperatableStateMachine.add('WelkePart?',
										EqualState(),
										transitions={'true': 'PistonRechts', 'false': 'GasketRechts'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'partarmrechts', 'value_b': 'pistonpart'})

			# x:289 y:66
			OperatableStateMachine.add('PistonRechts',
										ReplaceState(),
										transitions={'done': 'GetPoseBin2'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'config_nameRechtsPiston', 'result': 'config_name_bin'})

			# x:289 y:196
			OperatableStateMachine.add('GasketRechts',
										ReplaceState(),
										transitions={'done': 'GetPoseBin7_2'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'config_nameRechtsGasket', 'result': 'config_name_bin'})

			# x:52 y:516
			OperatableStateMachine.add('WelkePartLinks?',
										EqualState(),
										transitions={'true': 'PistonLinks', 'false': 'GasketLinks'},
										autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
										remapping={'value_a': 'partarmlinks', 'value_b': 'pistonpart'})

			# x:293 y:440
			OperatableStateMachine.add('PistonLinks',
										ReplaceState(),
										transitions={'done': 'GetPoseBin2_2'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'config_nameRechtsGasket', 'result': 'config_name_bin'})

			# x:293 y:544
			OperatableStateMachine.add('GasketLinks',
										ReplaceState(),
										transitions={'done': 'GetPoseBin7'},
										autonomy={'done': Autonomy.Off},
										remapping={'value': 'config_nameRechtsPiston', 'result': 'config_name_bin'})

			# x:472 y:195
			OperatableStateMachine.add('GetPoseBin7_2',
										GetObjectPoseState(object_frame='bin7_frame', ref_frame='world'),
										transitions={'continue': 'ArmIDRechts', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'pose': 'binpose'})

			# x:483 y:446
			OperatableStateMachine.add('GetPoseBin2_2',
										GetObjectPoseState(object_frame='bin2_frame', ref_frame='world'),
										transitions={'continue': 'ArmIDLinks', 'failed': 'failed'},
										autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
										remapping={'pose': 'binpose'})


		return _state_machine
Exemple #14
0
    def create(self):
        # x:24 y:502, x:938 y:468
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'],
            input_keys=['part_type', 'agv_id', 'part_pose'],
            output_keys=['pose'])
        _state_machine.userdata.arm_id = ''
        _state_machine.userdata.agv_id = ''
        _state_machine.userdata.joint_values = []
        _state_machine.userdata.joint_names = []
        _state_machine.userdata.offset = 0.2
        _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.srdf_param = 'ur10.srdf'
        _state_machine.userdata.ref_frame1 = 'arm1_linear_arm_actuator'
        _state_machine.userdata.camera_frame = ''
        _state_machine.userdata.camera_topic = ''
        _state_machine.userdata.pose = []
        _state_machine.userdata.part = ''
        _state_machine.userdata.tool_link = 'ee_link'
        _state_machine.userdata.part_offset = 0
        _state_machine.userdata.part_rotation = 0
        _state_machine.userdata.agv_pose = []
        _state_machine.userdata.material_locations = []
        _state_machine.userdata.bin_location = ''
        _state_machine.userdata.zero_value = ''
        _state_machine.userdata.part_pose = []
        _state_machine.userdata.part_type = ''
        _state_machine.userdata.ref_frame2 = 'arm2_linear_arm_actuator'
        _state_machine.userdata.agv2 = 'agv2'
        _state_machine.userdata.ref_frame = ''
        _state_machine.userdata.part1 = 'gasket_part'
        _state_machine.userdata.part2 = 'pulley_part'
        _state_machine.userdata.part3 = 'piston_rod_part'
        _state_machine.userdata.part4 = 'gear_part'
        _state_machine.userdata.rotation = 0
        _state_machine.userdata.overzet = ''
        _state_machine.userdata.overzetja = 'ja'
        _state_machine.userdata.overzetnee = 'nee'
        _state_machine.userdata.agv1_id = 'agv1'
        _state_machine.userdata.agv2_id = 'agv2'
        _state_machine.userdata.config_namer1b1 = 'R1PreBin1'
        _state_machine.userdata.config_namer2b4 = 'R2PreBin6'
        _state_machine.userdata.config_name_preagv = ''

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:468 y:36
            OperatableStateMachine.add(
                'AgvIdMessage',
                MessageState(),
                transitions={'continue': 'PartTypeMessage'},
                autonomy={'continue': Autonomy.Off},
                remapping={'message': 'agv_id'})

            # x:648 y:35
            OperatableStateMachine.add('PartTypeMessage',
                                       MessageState(),
                                       transitions={'continue': 'MoseMessage'},
                                       autonomy={'continue': Autonomy.Off},
                                       remapping={'message': 'part_type'})

            # x:460 y:161
            OperatableStateMachine.add('R1PreGrasp1',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'OpenGripper',
                                           '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:688 y:174
            OperatableStateMachine.add('OpenGripper',
                                       UseGripper(enable=False),
                                       transitions={
                                           'continue': 'ComputePick',
                                           'failed': 'failed',
                                           'invalid_arm': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id'})

            # x:235 y:96
            OperatableStateMachine.add('CheckPosePartsinBIn',
                                       DetectPartCameraAriacState(time_out=2),
                                       transitions={
                                           'continue': 'R1PreGrasp1',
                                           'failed': 'retry',
                                           'not_found': 'retry'
                                       },
                                       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:1325 y:349
            OperatableStateMachine.add('R1PreGrasp1Back',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'Overzetten?',
                                           '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:884 y:176
            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': 'R1ToPick',
                    '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:1106 y:178
            OperatableStateMachine.add('R1ToPick',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'CloseGripper',
                                           'planning_failed': 'failed',
                                           'control_failed': 'R1PreGrasp1'
                                       },
                                       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:1336 y:188
            OperatableStateMachine.add('CloseGripper',
                                       UseGripper(enable=True),
                                       transitions={
                                           'continue': 'Wacht1',
                                           'failed': 'failed',
                                           'invalid_arm': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id'})

            # x:1517 y:718
            OperatableStateMachine.add('R1PreAGV1',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'ComputePlacePose',
                                           '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_preagv',
                                           '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:1352 y:259
            OperatableStateMachine.add('Wacht1',
                                       WaitState(wait_time=1),
                                       transitions={'done': 'R1PreGrasp1Back'},
                                       autonomy={'done': Autonomy.Off})

            # x:986 y:718
            OperatableStateMachine.add('R1ToPlace',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'OpenGripper2',
                                           'planning_failed': 'Retry',
                                           'control_failed': 'Retry'
                                       },
                                       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:293 y:698
            OperatableStateMachine.add('R1PreAGV1Back',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'Robot?',
                                           '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_preagv',
                                           '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:679 y:693
            OperatableStateMachine.add('OpenGripper2',
                                       UseGripper(enable=False),
                                       transitions={
                                           'continue': 'Wacht2',
                                           'failed': 'failed',
                                           'invalid_arm': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id'})

            # x:488 y:694
            OperatableStateMachine.add('Wacht2',
                                       WaitState(wait_time=1),
                                       transitions={'done': 'R1PreAGV1Back'},
                                       autonomy={'done': Autonomy.Off})

            # x:951 y:616
            OperatableStateMachine.add('Retry',
                                       WaitState(wait_time=0.1),
                                       transitions={'done': 'R1ToPlace'},
                                       autonomy={'done': Autonomy.Off})

            # x:834 y:33
            OperatableStateMachine.add(
                'MoseMessage',
                MessageState(),
                transitions={'continue': 'CameraRobotSelector'},
                autonomy={'continue': Autonomy.Off},
                remapping={'message': 'part_pose'})

            # x:1233 y:728
            OperatableStateMachine.add(
                'ComputePlacePose',
                ComputeGraspPartOffsetAriacState(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': 'R1ToPlace',
                    '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': 'offset',
                    'rotation': 'rotation',
                    'part_pose': 'part_pose',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:266 y:9
            OperatableStateMachine.add(
                'retry',
                WaitState(wait_time=0.5),
                transitions={'done': 'CheckPosePartsinBIn'},
                autonomy={'done': Autonomy.Off})

            # x:42 y:416
            OperatableStateMachine.add('RefMSG',
                                       MessageState(),
                                       transitions={'continue': 'TopicMSG'},
                                       autonomy={'continue': Autonomy.Off},
                                       remapping={'message': 'ref_frame'})

            # x:190 y:412
            OperatableStateMachine.add('TopicMSG',
                                       MessageState(),
                                       transitions={'continue': 'FrameMSG'},
                                       autonomy={'continue': Autonomy.Off},
                                       remapping={'message': 'camera_topic'})

            # x:330 y:414
            OperatableStateMachine.add(
                'FrameMSG',
                MessageState(),
                transitions={'continue': 'CheckPosePartsinBIn'},
                autonomy={'continue': Autonomy.Off},
                remapping={'message': 'camera_frame'})

            # x:133 y:217
            OperatableStateMachine.add('CameraRobotSelector',
                                       self.use_behavior(
                                           CameraRobotSelectorSM,
                                           'CameraRobotSelector'),
                                       transitions={
                                           'finished': 'RefMSG',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'finished': Autonomy.Inherit,
                                           'failed': Autonomy.Inherit
                                       },
                                       remapping={
                                           'agv_id':
                                           'agv_id',
                                           'part_type':
                                           'part_type',
                                           'part_pose':
                                           'part_pose',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'config_name':
                                           'config_name',
                                           'arm_id':
                                           'arm_id',
                                           'ref_frame':
                                           'ref_frame',
                                           'camera_topic':
                                           'camera_topic',
                                           'camera_frame':
                                           'camera_frame',
                                           'part':
                                           'part',
                                           'part_offset':
                                           'part_offset',
                                           'overzet':
                                           'overzet',
                                           'config_name_preagv':
                                           'config_name_preagv'
                                       })

            # x:1551 y:346
            OperatableStateMachine.add(
                'Overzetten?',
                EqualState(),
                transitions={
                    'true': 'AGV?',
                    'false': 'op_overzetbin_plaatsen_en_frame_kiezen'
                },
                autonomy={
                    'true': Autonomy.Off,
                    'false': Autonomy.Off
                },
                remapping={
                    'value_a': 'overzet',
                    'value_b': 'overzetnee'
                })

            # x:1179 y:60
            OperatableStateMachine.add(
                'op_overzetbin_plaatsen_en_frame_kiezen',
                self.use_behavior(op_overzetbin_plaatsen_en_frame_kiezenSM,
                                  'op_overzetbin_plaatsen_en_frame_kiezen'),
                transitions={
                    'finished': 'CheckPosePartsinBIn',
                    'failed': 'failed'
                },
                autonomy={
                    'finished': Autonomy.Inherit,
                    'failed': Autonomy.Inherit
                },
                remapping={
                    'ref_frame': 'ref_frame',
                    'config_name': 'config_name',
                    'move_group_prefix': 'move_group_prefix',
                    'arm_id': 'arm_id',
                    'agv_id': 'agv_id',
                    'part_type': 'part_type',
                    'camera_topic': 'camera_topic',
                    'camera_frame': 'camera_frame',
                    'part_offset': 'part_offset',
                    'overzet': 'overzet'
                })

            # x:1324 y:446
            OperatableStateMachine.add(
                'GetAGV1Pose',
                GetObjectPoseState(object_frame='kit_tray_1',
                                   ref_frame='arm1_linear_arm_actuator'),
                transitions={
                    'continue': 'R1PreAGV1_2',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'agv_pose'})

            # x:1531 y:492
            OperatableStateMachine.add(
                'GetAGV1Pose_2',
                GetObjectPoseState(object_frame='kit_tray_2',
                                   ref_frame='arm2_linear_arm_actuator'),
                transitions={
                    'continue': 'R1PreAGV1_3',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'agv_id'})

            # x:1670 y:414
            OperatableStateMachine.add('AGV?',
                                       EqualState(),
                                       transitions={
                                           'true': 'GetAGV1Pose',
                                           'false': 'GetAGV1Pose_2'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'agv_id',
                                           'value_b': 'agv1_id'
                                       })

            # x:207 y:475
            OperatableStateMachine.add('R1PreAGV1Back_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_namer1b1',
                                           '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:105 y:672
            OperatableStateMachine.add('R1PreAGV1Back_3',
                                       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_namer2b4',
                                           '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:517
            OperatableStateMachine.add('Robot?',
                                       EqualState(),
                                       transitions={
                                           'true': 'R1PreAGV1Back_2',
                                           'false': 'R1PreAGV1Back_3'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'agv_id',
                                           'value_b': 'agv1_id'
                                       })

            # x:1419 y:573
            OperatableStateMachine.add('R1PreAGV1_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'R1PreAGV1',
                                           '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_namer1b1',
                                           '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:1569 y:574
            OperatableStateMachine.add('R1PreAGV1_3',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'R1PreAGV1',
                                           '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_namer2b4',
                                           'move_group': 'move_group',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'action_topic': 'action_topic',
                                           'robot_name': 'robot_name',
                                           'config_name_out':
                                           'config_name_out',
                                           'move_group_out': 'move_group_out',
                                           'robot_name_out': 'robot_name_out',
                                           'action_topic_out':
                                           'action_topic_out',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

        return _state_machine
    def create(self):
        # x:737 y:206, x:797 y:409
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'],
            input_keys=[
                'agv_id', 'offset2', 'offset1', 'pose_on_agv1', 'pose_on_agv2',
                'part_type2'
            ])
        _state_machine.userdata.joint_values = []
        _state_machine.userdata.joint_names = []
        _state_machine.userdata.action_topic = '/move_group'
        _state_machine.userdata.move_group_prefix = '/ariac/gantry'
        _state_machine.userdata.move_group_G = 'Gantry'
        _state_machine.userdata.move_group_L = 'Left_Arm'
        _state_machine.userdata.move_group_R = 'Right_Arm'
        _state_machine.userdata.ref_frame = 'world'
        _state_machine.userdata.robot_name = ''
        _state_machine.userdata.agv_id = ''
        _state_machine.userdata.offset1 = ''
        _state_machine.userdata.offset2 = ''
        _state_machine.userdata.config_name_GantryL_AGVL = 'Gantry_AGV_Links_L'
        _state_machine.userdata.config_name_GantryL_AGVR = 'Gantry_AGV_Links_R'
        _state_machine.userdata.config_name_GantryR_AGVL = 'Gantry_AGV_Rechts_L'
        _state_machine.userdata.config_name_GantryR_AGVR = 'Gantry_AGV_Rechts_R'
        _state_machine.userdata.config_name_GantryAGVL = ''
        _state_machine.userdata.config_name_GantryAGVR = ''
        _state_machine.userdata.agv1 = 'agv1'
        _state_machine.userdata.agv2 = 'agv2'
        _state_machine.userdata.config_name_PlaceAGVL = 'Links_PreDrop_AGV'
        _state_machine.userdata.tool_link_R = 'right_ee_link'
        _state_machine.userdata.tool_link_L = 'left_ee_link'
        _state_machine.userdata.agv_pose = []
        _state_machine.userdata.rotation = 0
        _state_machine.userdata.pose_on_agv1 = []
        _state_machine.userdata.arm_idL = 'left'
        _state_machine.userdata.config_name_PlaceAGVR = 'Rechts_PreDrop_AGV'
        _state_machine.userdata.arm_idR = 'right'
        _state_machine.userdata.pose_on_agv2 = []
        _state_machine.userdata.config_name_homeL = 'Left_Home'
        _state_machine.userdata.config_name_homeR = 'Right_Home'
        _state_machine.userdata.config_name_Gantry_home = 'Gantry_Transportband_Home'
        _state_machine.userdata.part_type2 = ''
        _state_machine.userdata.empty = 'empty'
        _state_machine.userdata.kit_tray = 'kit_tray_1'
        _state_machine.userdata.KT1 = 'kit_tray_1'
        _state_machine.userdata.KT2 = 'kit_tray_2'

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:98 y:59
            OperatableStateMachine.add('AGV choice',
                                       EqualState(),
                                       transitions={
                                           'true': 'SetPosL1',
                                           'false': 'SetPosL2'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'agv_id',
                                           'value_b': 'agv2'
                                       })

            # x:340 y:29
            OperatableStateMachine.add('SetPosL1',
                                       ReplaceState(),
                                       transitions={'done': 'SetPosR1'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'config_name_GantryR_AGVL',
                                           'result': 'config_name_GantryAGVL'
                                       })

            # x:534 y:29
            OperatableStateMachine.add('SetPosR1',
                                       ReplaceState(),
                                       transitions={'done': 'MoveL to AGV'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'config_name_GantryR_AGVR',
                                           'result': 'config_name_GantryAGVR'
                                       })

            # x:340 y:91
            OperatableStateMachine.add('SetPosL2',
                                       ReplaceState(),
                                       transitions={'done': 'SetPosR2'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'config_name_GantryL_AGVL',
                                           'result': 'config_name_GantryAGVL'
                                       })

            # x:535 y:91
            OperatableStateMachine.add('SetPosR2',
                                       ReplaceState(),
                                       transitions={'done': 'MoveL to AGV'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'config_name_GantryL_AGVR',
                                           'result': 'config_name_GantryAGVR'
                                       })

            # x:1187 y:33
            OperatableStateMachine.add('Move to PrePlace',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'AGV choice_3',
                                           'planning_failed': 'failed',
                                           'control_failed': 'AGV choice_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_PlaceAGVL',
                                           '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:833 y:56
            OperatableStateMachine.add(
                'MoveL to AGV',
                SrdfStateToMoveitAriac(),
                transitions={
                    'reached': 'Move to PrePlace',
                    'planning_failed': 'failed',
                    'control_failed': 'Move to PrePlace',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name_GantryAGVL',
                    'move_group': 'move_group_G',
                    'move_group_prefix': 'move_group_prefix',
                    'action_topic': 'action_topic',
                    'robot_name': 'robot_name',
                    'config_name_out': 'config_name_out',
                    'move_group_out': 'move_group_out',
                    'robot_name_out': 'robot_name_out',
                    'action_topic_out': 'action_topic_out',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1472 y:127
            OperatableStateMachine.add('GetAGV1pose',
                                       GetObjectPoseState(
                                           object_frame='kit_tray_1',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'pose_on_AGV',
                                           'failed': 'retry'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'agv_pose'})

            # x:1604 y:295
            OperatableStateMachine.add(
                'pose_on_AGV',
                ComputeDropPartOffsetGraspAriacState(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': 'MoveToDrop1',
                    'failed': 'retry'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group_L',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link_L',
                    'part_pose': 'pose_on_agv1',
                    'pose': 'agv_pose',
                    'offset': 'offset1',
                    'rotation': 'rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1219 y:277
            OperatableStateMachine.add(
                'retry',
                WaitState(wait_time=0.5),
                transitions={'done': 'Move to PrePlace'},
                autonomy={'done': Autonomy.Off})

            # x:1650 y:386
            OperatableStateMachine.add('MoveToDrop1',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'DisableGripper',
                                           'planning_failed': 'failed',
                                           'control_failed': 'DisableGripper'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group': 'move_group_L',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1658 y:491
            OperatableStateMachine.add('DisableGripper',
                                       VacuumGripperControlState(enable=False),
                                       transitions={
                                           'continue': 'Move to PrePlace_2',
                                           'failed': 'Move to PrePlace_2',
                                           'invalid_arm_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_idL'})

            # x:1678 y:599
            OperatableStateMachine.add('Move to PrePlace_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'Move L to home ',
                                           'planning_failed': 'failed',
                                           'control_failed': 'Move L to home ',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_PlaceAGVL',
                                           '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:1294 y:690
            OperatableStateMachine.add(
                'MoveR to AGV',
                SrdfStateToMoveitAriac(),
                transitions={
                    'reached': 'Move to PrePlace_3',
                    'planning_failed': 'failed',
                    'control_failed': 'Move to PrePlace_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_GantryAGVR',
                    'move_group': 'move_group_G',
                    'move_group_prefix': 'move_group_prefix',
                    'action_topic': 'action_topic',
                    'robot_name': 'robot_name',
                    'config_name_out': 'config_name_out',
                    'move_group_out': 'move_group_out',
                    'robot_name_out': 'robot_name_out',
                    'action_topic_out': 'action_topic_out',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1066 y:683
            OperatableStateMachine.add('Move to PrePlace_3',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'pose_on_AGV_2',
                                           'planning_failed': 'failed',
                                           'control_failed': 'pose_on_AGV_2',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_PlaceAGVR',
                                           '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:491 y:669
            OperatableStateMachine.add('GetAGV1pose_2',
                                       GetObjectPoseState(
                                           object_frame='kit_tray_1',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'pose_on_AGV_2',
                                           'failed': 'retry_2'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'pose2'})

            # x:14 y:717
            OperatableStateMachine.add(
                'pose_on_AGV_2',
                ComputeDropPartOffsetGraspAriacState(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': 'MoveToDrop2',
                    'failed': 'retry_2'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group_R',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link_R',
                    'part_pose': 'pose_on_agv2',
                    'pose': 'agv_pose',
                    'offset': 'offset2',
                    'rotation': 'rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:586 y:481
            OperatableStateMachine.add(
                'retry_2',
                WaitState(wait_time=0.5),
                transitions={'done': 'Move to PrePlace_3'},
                autonomy={'done': Autonomy.Off})

            # x:43 y:604
            OperatableStateMachine.add('MoveToDrop2',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'DisableGripper_2',
                                           'planning_failed': 'failed',
                                           'control_failed': 'DisableGripper_2'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group': 'move_group_R',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:48 y:494
            OperatableStateMachine.add('DisableGripper_2',
                                       VacuumGripperControlState(enable=False),
                                       transitions={
                                           'continue': 'Move to PrePlace_2_2',
                                           'failed': 'Move to PrePlace_2_2',
                                           'invalid_arm_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_idR'})

            # x:61 y:378
            OperatableStateMachine.add('Move to PrePlace_2_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'Move R to home ',
                                           'planning_failed': 'failed',
                                           'control_failed': 'Move R to home ',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_PlaceAGVR',
                                           '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:1684 y:711
            OperatableStateMachine.add('Move L to home ',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'CheckSecondPart',
                                           'planning_failed': 'failed',
                                           'control_failed': 'CheckSecondPart',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name_homeL',
                                           '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:65 y:279
            OperatableStateMachine.add('Move R to home ',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'GantryHome',
                                           'planning_failed': 'failed',
                                           'control_failed': 'GantryHome',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name_homeR',
                                           '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:335 y:205
            OperatableStateMachine.add('GantryHome',
                                       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_Gantry_home',
                                           'move_group': 'move_group_G',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'action_topic': 'action_topic',
                                           'robot_name': 'robot_name',
                                           'config_name_out':
                                           'config_name_out',
                                           'move_group_out': 'move_group_out',
                                           'robot_name_out': 'robot_name_out',
                                           'action_topic_out':
                                           'action_topic_out',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1479 y:750
            OperatableStateMachine.add('CheckSecondPart',
                                       EqualState(),
                                       transitions={
                                           'true': 'GantryHome',
                                           'false': 'MoveR to AGV'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'part_type2',
                                           'value_b': 'empty'
                                       })

            # x:668 y:546
            OperatableStateMachine.add('GetAGV2pose_2',
                                       GetObjectPoseState(
                                           object_frame='kit_tray_2',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'pose_on_AGV_2',
                                           'failed': 'retry_2'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'pose2'})

            # x:749 y:650
            OperatableStateMachine.add('AGV choice_2',
                                       EqualState(),
                                       transitions={
                                           'true': 'GetAGV2pose_2',
                                           'false': 'GetAGV1pose_2'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'agv_id',
                                           'value_b': 'agv2'
                                       })

            # x:1462 y:28
            OperatableStateMachine.add('AGV choice_3',
                                       EqualState(),
                                       transitions={
                                           'true': 'GetAGV2pose',
                                           'false': 'GetAGV1pose'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'agv_id',
                                           'value_b': 'agv2'
                                       })

            # x:1666 y:199
            OperatableStateMachine.add('GetAGV2pose',
                                       GetObjectPoseState(
                                           object_frame='kit_tray_2',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'pose_on_AGV',
                                           'failed': 'retry'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'agv_pose'})

        return _state_machine
Exemple #16
0
    def create(self):
        # x:884 y:573, x:130 y:365
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'])
        _state_machine.userdata.rotation = 0
        _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 = 'Left_Arm'
        _state_machine.userdata.offset = 0.021
        _state_machine.userdata.ref_frame = 'world'
        _state_machine.userdata.camera_topic = '/ariac/logical_camera_stock1'
        _state_machine.userdata.camera_frame = 'logical_camera_stock1_frame'
        _state_machine.userdata.home = 'Gantry_Home'
        _state_machine.userdata.part = 'piston_rod_part_red'
        _state_machine.userdata.full_home = 'Full_Home'
        _state_machine.userdata.clearance = 0.05
        _state_machine.userdata.bin = 'Gantry_Bin_Red'
        _state_machine.userdata.bin_pose = []
        _state_machine.userdata.binOffset = -1
        _state_machine.userdata.one = 1

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:102 y:30
            OperatableStateMachine.add('Move_Bin',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'Get_Object_Place',
                                           '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:312 y:27
            OperatableStateMachine.add('Compute_Place',
                                       ComputeDropPart(),
                                       transitions={
                                           'continue': 'Move_To_Place',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group': 'arm_id',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'binOffset': 'binOffset',
                                           'bin_pose': 'bin_pose',
                                           'offset': 'offset',
                                           'rotation': 'rotation',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:698 y:26
            OperatableStateMachine.add('Gripper_Off',
                                       GripperControl(enable=False),
                                       transitions={
                                           'continue': 'Add_Clearance',
                                           'failed': 'failed',
                                           'invalid_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id'})

            # x:842 y:232
            OperatableStateMachine.add('Move_Clear',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'Move_Home',
                                           'planning_failed': 'Move_Home',
                                           'control_failed': 'Move_Clear'
                                       },
                                       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:844 y:24
            OperatableStateMachine.add('Add_Clearance',
                                       AddNumericState(),
                                       transitions={'done': 'Compute_Place_2'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value_a': 'clearance',
                                           'value_b': 'offset',
                                           'result': 'offset'
                                       })

            # x:846 y:330
            OperatableStateMachine.add('Move_Home',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'AddPart',
                                           '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': 'full_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:482 y:20
            OperatableStateMachine.add('Move_To_Place',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'Gripper_Off',
                                           'planning_failed': 'failed',
                                           'control_failed': 'Gripper_Off'
                                       },
                                       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:68 y:266
            OperatableStateMachine.add('Get_Object_Place',
                                       GetObjectPoseState(
                                           object_frame='bin5_frame',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'Offset',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'bin_pose'})

            # x:854 y:117
            OperatableStateMachine.add('Compute_Place_2',
                                       ComputeDropPart(),
                                       transitions={
                                           'continue': 'Move_Clear',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group': 'arm_id',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'binOffset': 'binOffset',
                                           'bin_pose': 'bin_pose',
                                           'offset': 'offset',
                                           'rotation': 'rotation',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:220 y:154
            OperatableStateMachine.add('Offset',
                                       part_offsetCalc(),
                                       transitions={
                                           'succes': 'Compute_Place',
                                           'unknown_id': 'failed'
                                       },
                                       autonomy={
                                           'succes': Autonomy.Off,
                                           'unknown_id': Autonomy.Off
                                       },
                                       remapping={
                                           'part_type': 'part',
                                           'part_offset': 'offset'
                                       })

            # x:815 y:435
            OperatableStateMachine.add('AddPart',
                                       AddNumericState(),
                                       transitions={'done': 'finished'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value_a': 'binOffset',
                                           'value_b': 'one',
                                           'result': 'binOffset'
                                       })

        return _state_machine
    def create(self):
        # x:779 y:685, x:708 y:446
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'],
            input_keys=[
                'part_pose_right', 'part_pose_left', 'PreDropright',
                'PreDropleft', 'arm_id_right', 'arm_id_left'
            ])
        _state_machine.userdata.part_pose_right = []
        _state_machine.userdata.part_pose_left = []
        _state_machine.userdata.PreDropleft = ''
        _state_machine.userdata.PreDropright = ''
        _state_machine.userdata.value_b = 0
        _state_machine.userdata.value_a = 0
        _state_machine.userdata.arm_id_left = ''
        _state_machine.userdata.arm_id_right = ''
        _state_machine.userdata.robot_name = ''
        _state_machine.userdata.tool_link_right = 'right_ee_link'
        _state_machine.userdata.tool_link_left = 'left_ee_link'
        _state_machine.userdata.rotation = 0
        _state_machine.userdata.move_group_left_arm = 'Left_Arm'
        _state_machine.userdata.move_group_right_arm = 'Right_Arm'
        _state_machine.userdata.config_name_right_arm = 'Right_Home'
        _state_machine.userdata.config_name_left_arm = 'Left_Home'
        _state_machine.userdata.move_group_prefix = '/ariac/gantry'
        _state_machine.userdata.move_group_gantry = 'Gantry'
        _state_machine.userdata.action_topic = '/move_group'
        _state_machine.userdata.drop_offset = 0.145
        _state_machine.userdata.config_name_gantry = 'Gantry_Home'

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:41 y:519
            OperatableStateMachine.add('GantryHome',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'ArmHome_3',
                                           '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_gantry',
                                           '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:216 y:127
            OperatableStateMachine.add('PreDropTray',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'ComputeDrop',
                                           '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': 'PreDropleft',
                                           '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:237 y:8
            OperatableStateMachine.add('Retry',
                                       WaitState(wait_time=0.5),
                                       transitions={'done': 'PreDropTray'},
                                       autonomy={'done': Autonomy.Off})

            # x:603 y:127
            OperatableStateMachine.add('MoveToDrop',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'GripperOff',
                                           'planning_failed': 'Retry_2',
                                           'control_failed': 'Retry_2'
                                       },
                                       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:640 y:3
            OperatableStateMachine.add('Retry_2',
                                       WaitState(wait_time=0.5),
                                       transitions={'done': 'MoveToDrop'},
                                       autonomy={'done': Autonomy.Off})

            # x:814 y:126
            OperatableStateMachine.add('GripperOff',
                                       VacuumGripperControlState(enable=False),
                                       transitions={
                                           'continue': 'ArmHome',
                                           'failed': 'failed',
                                           'invalid_arm_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id_left'})

            # x:1028 y:124
            OperatableStateMachine.add('ArmHome',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'GripperControl',
                                           'planning_failed': 'Retry_3',
                                           'control_failed': 'Retry_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_left_arm',
                                           '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:1054 y:7
            OperatableStateMachine.add('Retry_3',
                                       WaitState(wait_time=0.5),
                                       transitions={'done': 'ArmHome'},
                                       autonomy={'done': Autonomy.Off})

            # x:1216 y:123
            OperatableStateMachine.add('GripperControl',
                                       GripperControle(),
                                       transitions={
                                           'Active': 'GetAGVPose_2',
                                           'Not_active': 'GantryHome_2',
                                           'invalid_arm_id': 'failed'
                                       },
                                       autonomy={
                                           'Active': Autonomy.Off,
                                           'Not_active': Autonomy.Off,
                                           'invalid_arm_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id_right'})

            # x:1436 y:124
            OperatableStateMachine.add('GetAGVPose_2',
                                       GetObjectPoseState(
                                           object_frame='kit_tray_2',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'PreDropTray_2',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'agv_pose_right'})

            # x:1428 y:232
            OperatableStateMachine.add('PreDropTray_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'ComputeDrop_2',
                                           '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': 'PreDropright',
                                           '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:1644 y:233
            OperatableStateMachine.add('Retry_4',
                                       WaitState(wait_time=0.5),
                                       transitions={'done': 'PreDropTray_2'},
                                       autonomy={'done': Autonomy.Off})

            # x:1398 y:447
            OperatableStateMachine.add('MoveToDrop_2',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'GripperOff_2',
                                           'planning_failed': 'Retry_5',
                                           'control_failed': 'Retry_5'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group':
                                           'move_group_right_arm',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1637 y:452
            OperatableStateMachine.add('Retry_5',
                                       WaitState(wait_time=0.5),
                                       transitions={'done': 'MoveToDrop_2'},
                                       autonomy={'done': Autonomy.Off})

            # x:1397 y:582
            OperatableStateMachine.add('GripperOff_2',
                                       VacuumGripperControlState(enable=False),
                                       transitions={
                                           'continue': 'ArmHome_2',
                                           'failed': 'failed',
                                           'invalid_arm_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id_right'})

            # x:1417 y:689
            OperatableStateMachine.add(
                'ArmHome_2',
                SrdfStateToMoveitAriac(),
                transitions={
                    'reached': 'GantryHome_2',
                    'planning_failed': 'Retry_6',
                    'control_failed': 'Retry_6',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name_right_arm',
                    '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:1634 y:689
            OperatableStateMachine.add('Retry_6',
                                       WaitState(wait_time=0.5),
                                       transitions={'done': 'ArmHome_2'},
                                       autonomy={'done': Autonomy.Off})

            # x:53 y:627
            OperatableStateMachine.add('Retry_7',
                                       WaitState(wait_time=0.5),
                                       transitions={'done': 'GantryHome'},
                                       autonomy={'done': Autonomy.Off})

            # x:51 y:128
            OperatableStateMachine.add('GetAGVPose',
                                       GetObjectPoseState(
                                           object_frame='kit_tray_2',
                                           ref_frame='world'),
                                       transitions={
                                           'continue': 'PreDropTray',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'agv_pose_left'})

            # x:381 y:129
            OperatableStateMachine.add(
                'ComputeDrop',
                ComputeGraspPartOffsetAriacState(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': 'MoveToDrop',
                    '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': 'agv_pose_left',
                    'offset': 'drop_offset',
                    'rotation': 'rotation',
                    'part_pose': 'part_pose_left',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1388 y:336
            OperatableStateMachine.add(
                'ComputeDrop_2',
                ComputeGraspPartOffsetAriacState(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': 'MoveToDrop_2',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group_right_arm',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link_right',
                    'pose': 'agv_pose_right',
                    'offset': 'drop_offset',
                    'rotation': 'rotation',
                    'part_pose': 'part_pose_right',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:42 y:393
            OperatableStateMachine.add('ArmHome_3',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'ArmHome_2_2',
                                           'planning_failed': 'Retry_3_2',
                                           'control_failed': 'Retry_3_2',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_left_arm',
                                           '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:229 y:401
            OperatableStateMachine.add('Retry_3_2',
                                       WaitState(wait_time=0.5),
                                       transitions={'done': 'ArmHome_3'},
                                       autonomy={'done': Autonomy.Off})

            # x:43 y:250
            OperatableStateMachine.add(
                'ArmHome_2_2',
                SrdfStateToMoveitAriac(),
                transitions={
                    'reached': 'GetAGVPose',
                    'planning_failed': 'Retry_6_2',
                    'control_failed': 'Retry_6_2',
                    'param_error': 'failed'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name_right_arm',
                    '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:229 y:269
            OperatableStateMachine.add('Retry_6_2',
                                       WaitState(wait_time=0.5),
                                       transitions={'done': 'ArmHome_2_2'},
                                       autonomy={'done': Autonomy.Off})

            # x:985 y:660
            OperatableStateMachine.add('GantryHome_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'finished',
                                           'planning_failed': 'Retry_7_2',
                                           'control_failed': 'Retry_7_2',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name_gantry',
                                           '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:1006 y:777
            OperatableStateMachine.add('Retry_7_2',
                                       WaitState(wait_time=0.5),
                                       transitions={'done': 'GantryHome_2'},
                                       autonomy={'done': Autonomy.Off})

        return _state_machine
    def create(self):
        # x:860 y:686, x:845 y:354
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'],
            input_keys=['agv_id', 'part_pose_right', 'part_pose_left'])
        _state_machine.userdata.agv_id = ''
        _state_machine.userdata.agv1 = 'agv1'
        _state_machine.userdata.agv2 = 'agv2'
        _state_machine.userdata.Gantry_AGV1_right = 'Gantry_AGV1Right'
        _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.move_group_gantry = 'Gantry'
        _state_machine.userdata.PreGrasp_rightArm = 'Right_AGV'
        _state_machine.userdata.PreGrasp_leftArm = 'Left_AGV'
        _state_machine.userdata.tool_link_right = 'right_ee_link'
        _state_machine.userdata.tool_link_left = 'left_ee_link'
        _state_machine.userdata.offset = 0.115
        _state_machine.userdata.part_pose_right = ''
        _state_machine.userdata.rotation = 0
        _state_machine.userdata.part_pose_left = ''
        _state_machine.userdata.arm_right = 'right_arm'
        _state_machine.userdata.arm_left = 'left_arm'
        _state_machine.userdata.Gantry_AGV1_left = 'Gantry_AGV1Left'
        _state_machine.userdata.move_group_left = 'Left_Arm'
        _state_machine.userdata.gantry_home = 'Gantry_Home'
        _state_machine.userdata.Home_right_arm = 'Right_PreGrasp'
        _state_machine.userdata.Home_left_arm = 'Left_PreGrasp'

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:31 y:18
            OperatableStateMachine.add('Gantry_Home',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'AGV1',
                                           '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': 'gantry_home',
                                           '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:24 y:608
            OperatableStateMachine.add('AGV2',
                                       EqualState(),
                                       transitions={
                                           'true': 'GetAgv2Pose',
                                           'false': 'failed'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'agv_id',
                                           'value_b': 'agv2'
                                       })

            # x:254 y:615
            OperatableStateMachine.add('GetAgv2Pose',
                                       GetObjectPoseState(
                                           object_frame='kit_tray_2',
                                           ref_frame='Gantry'),
                                       transitions={
                                           'continue': 'finished',
                                           'failed': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off
                                       },
                                       remapping={'pose': 'agv_pose'})

            # x:202 y:85
            OperatableStateMachine.add(
                'GetAgv1Pose',
                GetObjectPoseState(object_frame='kit_tray_1',
                                   ref_frame='world'),
                transitions={
                    'continue': 'PreDropTrayGantryRight',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'agv_pose'})

            # x:351 y:86
            OperatableStateMachine.add('PreDropTrayGantryRight',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'PreDropTrayRightArm',
                                           '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': 'Gantry_AGV1_right',
                                           '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:378 y:4
            OperatableStateMachine.add(
                'Retry_1',
                WaitState(wait_time=3),
                transitions={'done': 'PreDropTrayGantryRight'},
                autonomy={'done': Autonomy.Off})

            # x:531 y:89
            OperatableStateMachine.add('PreDropTrayRightArm',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'ComputeDropPartPose',
                                           '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': 'PreGrasp_rightArm',
                                           '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:553 y:6
            OperatableStateMachine.add(
                'Retry_2',
                WaitState(wait_time=3),
                transitions={'done': 'PreDropTrayRightArm'},
                autonomy={'done': Autonomy.Off})

            # x:690 y:92
            OperatableStateMachine.add(
                'ComputeDropPartPose',
                ComputeGraspPartOffsetAriacState(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': 'MoveToDrop',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group_right',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link_right',
                    'pose': 'agv_pose',
                    'offset': 'offset',
                    'rotation': 'rotation',
                    'part_pose': 'part_pose_right',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:902 y:89
            OperatableStateMachine.add('MoveToDrop',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'GripperOff',
                                           'planning_failed': 'Retry_3',
                                           'control_failed': 'Retry_3'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group': 'move_group_right',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:933 y:10
            OperatableStateMachine.add('Retry_3',
                                       WaitState(wait_time=3),
                                       transitions={'done': 'MoveToDrop'},
                                       autonomy={'done': Autonomy.Off})

            # x:1086 y:86
            OperatableStateMachine.add('GripperOff',
                                       VacuumGripperControlState(enable=False),
                                       transitions={
                                           'continue': 'PreDropTrayRightArm_2',
                                           'failed': 'MoveToDrop',
                                           'invalid_arm_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_right'})

            # x:1267 y:87
            OperatableStateMachine.add('PreDropTrayRightArm_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached':
                                           'PreDropTrayRightArm_2_2',
                                           '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': 'PreGrasp_rightArm',
                                           '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:1291 y:8
            OperatableStateMachine.add(
                'Retry_4',
                WaitState(wait_time=3),
                transitions={'done': 'PreDropTrayRightArm_2'},
                autonomy={'done': Autonomy.Off})

            # x:1551 y:189
            OperatableStateMachine.add('PreDropTrayGantryLeft',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'PreDropTrayLeftArm',
                                           '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': 'Gantry_AGV1_left',
                                           '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:1555 y:266
            OperatableStateMachine.add('PreDropTrayLeftArm',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'ComputeDropPartPose_2',
                                           'planning_failed': 'Retry_6',
                                           'control_failed': 'Retry_6',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'PreGrasp_leftArm',
                                           '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:1518 y:345
            OperatableStateMachine.add(
                'ComputeDropPartPose_2',
                ComputeGraspPartOffsetAriacState(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': 'MoveToDrop_2',
                    'failed': 'failed'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group_left',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link_left',
                    'pose': 'agv_pose',
                    'offset': 'offset',
                    'rotation': 'rotation',
                    'part_pose': 'part_pose_left',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:1519 y:419
            OperatableStateMachine.add('MoveToDrop_2',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'GripperOff_2',
                                           'planning_failed': 'Retry_7',
                                           'control_failed': 'Retry_7'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group': 'move_group_left',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1523 y:510
            OperatableStateMachine.add('GripperOff_2',
                                       VacuumGripperControlState(enable=False),
                                       transitions={
                                           'continue': 'PreDropTrayLeftArm_2',
                                           'failed': 'MoveToDrop_2',
                                           'invalid_arm_id': 'failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_left'})

            # x:1542 y:606
            OperatableStateMachine.add('PreDropTrayLeftArm_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached':
                                           'PreDropTrayLeftArm_2_2_2',
                                           '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': 'PreGrasp_leftArm',
                                           '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:1719 y:183
            OperatableStateMachine.add(
                'Retry_5',
                WaitState(wait_time=3),
                transitions={'done': 'PreDropTrayGantryLeft'},
                autonomy={'done': Autonomy.Off})

            # x:1720 y:264
            OperatableStateMachine.add(
                'Retry_6',
                WaitState(wait_time=3),
                transitions={'done': 'PreDropTrayLeftArm'},
                autonomy={'done': Autonomy.Off})

            # x:1715 y:416
            OperatableStateMachine.add('Retry_7',
                                       WaitState(wait_time=3),
                                       transitions={'done': 'MoveToDrop_2'},
                                       autonomy={'done': Autonomy.Off})

            # x:1717 y:597
            OperatableStateMachine.add(
                'Retry_8',
                WaitState(wait_time=3),
                transitions={'done': 'PreDropTrayLeftArm_2'},
                autonomy={'done': Autonomy.Off})

            # x:207 y:9
            OperatableStateMachine.add('Retry',
                                       WaitState(wait_time=3),
                                       transitions={'done': 'Gantry_Home'},
                                       autonomy={'done': Autonomy.Off})

            # x:25 y:157
            OperatableStateMachine.add('AGV1',
                                       EqualState(),
                                       transitions={
                                           'true': 'AttachedRight',
                                           'false': 'AGV2'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'agv_id',
                                           'value_b': 'agv1'
                                       })

            # x:1462 y:93
            OperatableStateMachine.add('PreDropTrayRightArm_2_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'Attached_Left',
                                           'planning_failed': 'Retry_9_2',
                                           'control_failed': 'Retry_9_2',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'Home_right_arm',
                                           '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:1524 y:697
            OperatableStateMachine.add('PreDropTrayLeftArm_2_2_2',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'Home_Gantry',
                                           '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': 'Home_left_arm',
                                           '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:1720 y:705
            OperatableStateMachine.add(
                'Retry_9',
                WaitState(wait_time=3),
                transitions={'done': 'PreDropTrayLeftArm_2_2_2'},
                autonomy={'done': Autonomy.Off})

            # x:1495 y:6
            OperatableStateMachine.add(
                'Retry_9_2',
                WaitState(wait_time=3),
                transitions={'done': 'PreDropTrayRightArm_2_2'},
                autonomy={'done': Autonomy.Off})

            # x:1124 y:654
            OperatableStateMachine.add('Home_Gantry',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'finished',
                                           'planning_failed': 'Retry_9_3',
                                           'control_failed': 'Retry_9_3',
                                           'param_error': 'failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'gantry_home',
                                           '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:1147 y:732
            OperatableStateMachine.add('Retry_9_3',
                                       WaitState(wait_time=3),
                                       transitions={'done': 'Home_Gantry'},
                                       autonomy={'done': Autonomy.Off})

            # x:201 y:157
            OperatableStateMachine.add('AttachedRight',
                                       CheckGripperattached(),
                                       transitions={
                                           'True': 'TestMessagePositie1',
                                           'False': 'Home_Gantry',
                                           'invalid_arm_id': 'failed'
                                       },
                                       autonomy={
                                           'True': Autonomy.Off,
                                           'False': Autonomy.Off,
                                           'invalid_arm_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_right'})

            # x:1671 y:94
            OperatableStateMachine.add('Attached_Left',
                                       CheckGripperattached(),
                                       transitions={
                                           'True': 'PreDropTrayGantryLeft',
                                           'False': 'Home_Gantry',
                                           'invalid_arm_id': 'failed'
                                       },
                                       autonomy={
                                           'True': Autonomy.Off,
                                           'False': Autonomy.Off,
                                           'invalid_arm_id': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_left'})

            # x:199 y:253
            OperatableStateMachine.add(
                'TestMessagePositie1',
                MessageState(),
                transitions={'continue': 'GetAgv1Pose'},
                autonomy={'continue': Autonomy.Off},
                remapping={'message': 'part_pose_right'})

        return _state_machine
Exemple #19
0
    def create(self):
        # x:1630 y:562, x:1479 y:95
        _state_machine = OperatableStateMachine(
            outcomes=['finished', 'failed'], input_keys=['arm'])
        _state_machine.userdata.arm_id = 'arm1'
        _state_machine.userdata.power = 100
        _state_machine.userdata.NoPower = 0
        _state_machine.userdata.part = ''
        _state_machine.userdata.ref_frame1 = 'arm1_linear_arm_actuator'
        _state_machine.userdata.Camera1_topic = '/ariac/Camera_Converyor_Links'
        _state_machine.userdata.camera_frame1 = 'Camera_Converyor_Links_frame'
        _state_machine.userdata.part_type = ''
        _state_machine.userdata.pose = []
        _state_machine.userdata.joint_values = []
        _state_machine.userdata.joint_names = []
        _state_machine.userdata.move_group = 'manipulator'
        _state_machine.userdata.move_group_prefix = '/ariac/arm1'
        _state_machine.userdata.action_topic = '/move_group'
        _state_machine.userdata.arm = ''
        _state_machine.userdata.tool_link = 'ee_link'
        _state_machine.userdata.Disk_rotation = 0
        _state_machine.userdata.Disk_offset = 0.035
        _state_machine.userdata.config_name_R1PreBin3 = 'R1PreBin3'
        _state_machine.userdata.robot_name = ''
        _state_machine.userdata.config_name_R1PreConveyor = 'R1PreConveyor'
        _state_machine.userdata.Round_Nr = 1
        _state_machine.userdata.Round_target = 6
        _state_machine.userdata.Zero = 0
        _state_machine.userdata.ONE = 1
        _state_machine.userdata.SideYoffset = 0
        _state_machine.userdata.SideXoffset = 0
        _state_machine.userdata.PlusOffset = 0.20
        _state_machine.userdata.config_name_R1HOME = 'R1Home'
        _state_machine.userdata.Row_count = 3
        _state_machine.userdata.offset = 0.040
        _state_machine.userdata.rotation = 0
        _state_machine.userdata.BIN3_pose = []

        # Additional creation code can be added inside the following tags
        # [MANUAL_CREATE]

        # [/MANUAL_CREATE]

        with _state_machine:
            # x:49 y:25
            OperatableStateMachine.add('MoveToHOME',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'GripperUIT_2',
                                           'planning_failed': 'RetryHOME',
                                           'control_failed': 'RetryHOME',
                                           'param_error': 'WAIT_failed'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name': 'config_name_R1HOME',
                                           'move_group': 'move_group',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'action_topic': 'action_topic',
                                           'robot_name': 'robot_name',
                                           'config_name_out':
                                           'config_name_out',
                                           'move_group_out': 'move_group_out',
                                           'robot_name_out': 'robot_name_out',
                                           'action_topic_out':
                                           'action_topic_out',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1262 y:559
            OperatableStateMachine.add(
                'WAIT_completed',
                WaitState(wait_time=2),
                transitions={'done': 'SluitConveyorAF_2'},
                autonomy={'done': Autonomy.Off})

            # x:202 y:91
            OperatableStateMachine.add('Stopconveyor_2',
                                       SetConveyorbeltPowerState(),
                                       transitions={
                                           'continue': 'MoveToPreConveyor',
                                           'fail': 'WAIT_failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'fail': Autonomy.Off
                                       },
                                       remapping={'power': 'NoPower'})

            # x:1106 y:35
            OperatableStateMachine.add('WAIT_failed',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'SluitConveyorAF'},
                                       autonomy={'done': Autonomy.Off})

            # x:563 y:30
            OperatableStateMachine.add('WAIT_check',
                                       WaitState(wait_time=1),
                                       transitions={'done': 'CheckforDisk'},
                                       autonomy={'done': Autonomy.Off})

            # x:372 y:91
            OperatableStateMachine.add(
                'CheckforDisk',
                DetectFirstPartCameraAriacState(part_list=['disk_part'],
                                                time_out=2),
                transitions={
                    'continue': 'Stopconveyor_2',
                    'failed': 'WAIT_failed',
                    'not_found': 'WAIT_check'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off,
                    'not_found': Autonomy.Off
                },
                remapping={
                    'ref_frame': 'ref_frame1',
                    'camera_topic': 'Camera1_topic',
                    'camera_frame': 'camera_frame1',
                    'part': 'part',
                    'pose': 'pose'
                })

            # x:200 y:162
            OperatableStateMachine.add(
                'Computebeltpick',
                ComputeGraspAriacState(joint_names=[
                    'linear_arm_actuator_joint', 'shoulder_pan_joint',
                    'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
                    'wrist_2_joint', 'wrist_3_joint'
                ]),
                transitions={
                    'continue': 'MoveToConveyorPart',
                    'failed': 'WAIT_failed_pick'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link',
                    'pose': 'pose',
                    'offset': 'Disk_offset',
                    'rotation': 'Disk_rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:371 y:166
            OperatableStateMachine.add('MoveToConveyorPart',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'WAIT_gripperoke',
                                           'planning_failed':
                                           'WAIT_failed_pick',
                                           'control_failed': 'WAIT_failed_drop'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group': 'move_group',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1106 y:97
            OperatableStateMachine.add('WAIT_failed_pick',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'SluitConveyorAF'},
                                       autonomy={'done': Autonomy.Off})

            # x:201 y:299
            OperatableStateMachine.add('MoveToBin3',
                                       SrdfStateToMoveitAriac(),
                                       transitions={
                                           'reached': 'GetBin3Pose',
                                           'planning_failed': 'RetryBin3',
                                           'control_failed': 'RetryBin3',
                                           'param_error': 'WAIT_failed_pick'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off,
                                           'param_error': Autonomy.Off
                                       },
                                       remapping={
                                           'config_name':
                                           'config_name_R1PreBin3',
                                           'move_group': 'move_group',
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'action_topic': 'action_topic',
                                           'robot_name': 'robot_name',
                                           'config_name_out':
                                           'config_name_out',
                                           'move_group_out': 'move_group_out',
                                           'robot_name_out': 'robot_name_out',
                                           'action_topic_out':
                                           'action_topic_out',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:51 y:163
            OperatableStateMachine.add('Gripperaan',
                                       UseGripper(enable=True),
                                       transitions={
                                           'continue': 'Computebeltpick',
                                           'failed': 'WAIT_failed_pick',
                                           'invalid_arm': 'WAIT_failed_pick'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id'})

            # x:370 y:243
            OperatableStateMachine.add(
                'WAIT_gripperoke',
                WaitState(wait_time=0.5),
                transitions={'done': 'MoveToPreConveyor_2'},
                autonomy={'done': Autonomy.Off})

            # x:51 y:96
            OperatableStateMachine.add(
                'MoveToPreConveyor',
                SrdfStateToMoveitAriac(),
                transitions={
                    'reached': 'Gripperaan',
                    'planning_failed': 'RetryPreConveyor',
                    'control_failed': 'RetryPreConveyor',
                    'param_error': 'WAIT_failed_pick'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name_R1PreConveyor',
                    'move_group': 'move_group',
                    'move_group_prefix': 'move_group_prefix',
                    'action_topic': 'action_topic',
                    'robot_name': 'robot_name',
                    'config_name_out': 'config_name_out',
                    'move_group_out': 'move_group_out',
                    'robot_name_out': 'robot_name_out',
                    'action_topic_out': 'action_topic_out',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:22 y:707
            OperatableStateMachine.add(
                'RetryPreConveyor',
                WaitState(wait_time=1),
                transitions={'done': 'MoveToPreConveyor'},
                autonomy={'done': Autonomy.Off})

            # x:261 y:768
            OperatableStateMachine.add('RetryBin3',
                                       WaitState(wait_time=1),
                                       transitions={'done': 'MoveToBin3'},
                                       autonomy={'done': Autonomy.Off})

            # x:200 y:231
            OperatableStateMachine.add(
                'MoveToPreConveyor_2',
                SrdfStateToMoveitAriac(),
                transitions={
                    'reached': 'MoveToBin3',
                    'planning_failed': 'RetryPreConveyor_2',
                    'control_failed': 'RetryPreConveyor_2',
                    'param_error': 'WAIT_failed_pick'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name_R1PreConveyor',
                    'move_group': 'move_group',
                    'move_group_prefix': 'move_group_prefix',
                    'action_topic': 'action_topic',
                    'robot_name': 'robot_name',
                    'config_name_out': 'config_name_out',
                    'move_group_out': 'move_group_out',
                    'robot_name_out': 'robot_name_out',
                    'action_topic_out': 'action_topic_out',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:20 y:769
            OperatableStateMachine.add(
                'RetryPreConveyor_2',
                WaitState(wait_time=1),
                transitions={'done': 'MoveToPreConveyor_2'},
                autonomy={'done': Autonomy.Off})

            # x:356 y:566
            OperatableStateMachine.add('GripperUIT',
                                       UseGripper(enable=False),
                                       transitions={
                                           'continue': 'MoveToBin3_2',
                                           'failed': 'WAIT_failed_pick',
                                           'invalid_arm': 'WAIT_failed_pick'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id'})

            # x:1033 y:518
            OperatableStateMachine.add('AddRound',
                                       AddNumericState(),
                                       transitions={'done': 'CheckRound'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value_a': 'Round_Nr',
                                           'value_b': 'ONE',
                                           'result': 'Round_Nr'
                                       })

            # x:1032 y:584
            OperatableStateMachine.add('CheckRound',
                                       EqualState(),
                                       transitions={
                                           'true': 'ResetRound',
                                           'false': 'StartConveryor'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'Round_Nr',
                                           'value_b': 'Round_target'
                                       })

            # x:1031 y:654
            OperatableStateMachine.add('ResetRound',
                                       ReplaceState(),
                                       transitions={'done': 'WAIT_completed'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'Round_Nr',
                                           'result': 'Zero'
                                       })

            # x:360 y:382
            OperatableStateMachine.add(
                'ComputeDrop',
                ComputeBeltDrop(joint_names=[
                    'linear_arm_actuator_joint', 'shoulder_pan_joint',
                    'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
                    'wrist_2_joint', 'wrist_3_joint'
                ]),
                transitions={
                    'continue': 'ShowDropPose',
                    'failed': 'WAIT_failed_drop'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={
                    'move_group': 'move_group',
                    'move_group_prefix': 'move_group_prefix',
                    'tool_link': 'tool_link',
                    'pose': 'BIN3_pose',
                    'offset': 'offset',
                    'SideYoffset': 'SideYoffset',
                    'SideXoffset': 'SideXoffset',
                    'rotation': 'rotation',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

            # x:356 y:507
            OperatableStateMachine.add('MoveToDROP',
                                       MoveitToJointsDynAriacState(),
                                       transitions={
                                           'reached': 'GripperUIT',
                                           'planning_failed': 'RetryDROP',
                                           'control_failed': 'RetryDROP'
                                       },
                                       autonomy={
                                           'reached': Autonomy.Off,
                                           'planning_failed': Autonomy.Off,
                                           'control_failed': Autonomy.Off
                                       },
                                       remapping={
                                           'move_group_prefix':
                                           'move_group_prefix',
                                           'move_group': 'move_group',
                                           'action_topic': 'action_topic',
                                           'joint_values': 'joint_values',
                                           'joint_names': 'joint_names'
                                       })

            # x:1106 y:159
            OperatableStateMachine.add('WAIT_failed_drop',
                                       WaitState(wait_time=5),
                                       transitions={'done': 'SluitConveyorAF'},
                                       autonomy={'done': Autonomy.Off})

            # x:360 y:768
            OperatableStateMachine.add('RetryDROP',
                                       WaitState(wait_time=1),
                                       transitions={'done': 'MoveToDROP'},
                                       autonomy={'done': Autonomy.Off})

            # x:743 y:522
            OperatableStateMachine.add('AddYOffset',
                                       AddNumericState(),
                                       transitions={'done': 'CheckRow'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value_a': 'SideYoffset',
                                           'value_b': 'PlusOffset',
                                           'result': 'SideYoffset'
                                       })

            # x:742 y:579
            OperatableStateMachine.add('CheckRow',
                                       EqualState(),
                                       transitions={
                                           'true': 'ResetRow',
                                           'false': 'AddRound'
                                       },
                                       autonomy={
                                           'true': Autonomy.Off,
                                           'false': Autonomy.Off
                                       },
                                       remapping={
                                           'value_a': 'Row_count',
                                           'value_b': 'Round_Nr'
                                       })

            # x:741 y:647
            OperatableStateMachine.add('ResetRow',
                                       ReplaceState(),
                                       transitions={'done': 'AddXOffset'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value': 'Zero',
                                           'result': 'SideYoffset'
                                       })

            # x:739 y:711
            OperatableStateMachine.add('AddXOffset',
                                       AddNumericState(),
                                       transitions={'done': 'AddRound'},
                                       autonomy={'done': Autonomy.Off},
                                       remapping={
                                           'value_a': 'SideXoffset',
                                           'value_b': 'PlusOffset',
                                           'result': 'SideXoffset'
                                       })

            # x:156 y:768
            OperatableStateMachine.add(
                'RetryDropBin',
                WaitState(wait_time=1),
                transitions={'done': 'MoveToConveyorPart'},
                autonomy={'done': Autonomy.Off})

            # x:22 y:645
            OperatableStateMachine.add('RetryHOME',
                                       WaitState(wait_time=1),
                                       transitions={'done': 'MoveToHOME'},
                                       autonomy={'done': Autonomy.Off})

            # x:371 y:28
            OperatableStateMachine.add('StartConveryor',
                                       SetConveyorbeltPowerState(),
                                       transitions={
                                           'continue': 'WAIT_check',
                                           'fail': 'WAIT_failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'fail': Autonomy.Off
                                       },
                                       remapping={'power': 'power'})

            # x:203 y:26
            OperatableStateMachine.add('GripperUIT_2',
                                       UseGripper(enable=False),
                                       transitions={
                                           'continue': 'StartConveryor',
                                           'failed': 'WAIT_failed',
                                           'invalid_arm': 'WAIT_failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'failed': Autonomy.Off,
                                           'invalid_arm': Autonomy.Off
                                       },
                                       remapping={'arm_id': 'arm_id'})

            # x:1300 y:89
            OperatableStateMachine.add('SluitConveyorAF',
                                       SetConveyorbeltPowerState(),
                                       transitions={
                                           'continue': 'failed',
                                           'fail': 'WAIT_failed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'fail': Autonomy.Off
                                       },
                                       remapping={'power': 'NoPower'})

            # x:1396 y:560
            OperatableStateMachine.add('SluitConveyorAF_2',
                                       SetConveyorbeltPowerState(),
                                       transitions={
                                           'continue': 'finished',
                                           'fail': 'WAIT_completed'
                                       },
                                       autonomy={
                                           'continue': Autonomy.Off,
                                           'fail': Autonomy.Off
                                       },
                                       remapping={'power': 'NoPower'})

            # x:368 y:311
            OperatableStateMachine.add(
                'GetBin3Pose',
                GetObjectPoseState(object_frame='kit_tray_2',
                                   ref_frame='arm1_linear_arm_actuator'),
                transitions={
                    'continue': 'ComputeDrop',
                    'failed': 'WAIT_failed_drop'
                },
                autonomy={
                    'continue': Autonomy.Off,
                    'failed': Autonomy.Off
                },
                remapping={'pose': 'BIN3_pose'})

            # x:361 y:443
            OperatableStateMachine.add('ShowDropPose',
                                       MessageState(),
                                       transitions={'continue': 'MoveToDROP'},
                                       autonomy={'continue': Autonomy.Off},
                                       remapping={'message': 'BIN3_pose'})

            # x:536 y:562
            OperatableStateMachine.add(
                'MoveToBin3_2',
                SrdfStateToMoveitAriac(),
                transitions={
                    'reached': 'AddYOffset',
                    'planning_failed': 'WAIT_failed_drop',
                    'control_failed': 'WAIT_failed_drop',
                    'param_error': 'WAIT_failed_drop'
                },
                autonomy={
                    'reached': Autonomy.Off,
                    'planning_failed': Autonomy.Off,
                    'control_failed': Autonomy.Off,
                    'param_error': Autonomy.Off
                },
                remapping={
                    'config_name': 'config_name_R1PreBin3',
                    'move_group': 'move_group',
                    'move_group_prefix': 'move_group_prefix',
                    'action_topic': 'action_topic',
                    'robot_name': 'robot_name',
                    'config_name_out': 'config_name_out',
                    'move_group_out': 'move_group_out',
                    'robot_name_out': 'robot_name_out',
                    'action_topic_out': 'action_topic_out',
                    'joint_values': 'joint_values',
                    'joint_names': 'joint_names'
                })

        return _state_machine