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
def create(self): # x:37 y:662, x:476 y:317 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.conveyor_belt_power = 100.0 _state_machine.userdata.config_name_home = 'home' _state_machine.userdata.move_group = 'manipulator' _state_machine.userdata.move_group_prefix = '/ariac/arm1' _state_machine.userdata.action_topic = '/move_group' _state_machine.userdata.robot_name = '' _state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator' _state_machine.userdata.camera_topic = '/ariac/logical_camera_6' _state_machine.userdata.camera_frame = 'logical_camera_6_frame' _state_machine.userdata.part = 'gasket_part' _state_machine.userdata.config_name_bin6PreGrasp = 'bin6PreGrasp' _state_machine.userdata.tool_link = 'ee_link' _state_machine.userdata.part_offset = 0.1 _state_machine.userdata.part_rotation = 0 _state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop' _state_machine.userdata.agv_pose = [] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:71 y:109 OperatableStateMachine.add('StartAssignment', StartAssignment(), transitions={'continue': 'MoveR1Home'}, autonomy={'continue': Autonomy.Off}) # x:182 y:656 OperatableStateMachine.add('EndAssignment', EndAssignment(), transitions={'continue': 'finished'}, autonomy={'continue': Autonomy.Off}) # x:233 y:110 OperatableStateMachine.add('MoveR1Home', SrdfStateToMoveitAriac(), transitions={'reached': 'DetectCameraPart', 'planning_failed': 'WaitRetry1', 'control_failed': 'WaitRetry1', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_home', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:233 y:8 OperatableStateMachine.add('WaitRetry1', WaitState(wait_time=5), transitions={'done': 'MoveR1Home'}, autonomy={'done': Autonomy.Off}) # x:438 y:112 OperatableStateMachine.add('DetectCameraPart', DetectPartCameraAriacState(time_out=5), transitions={'continue': 'MoveR1PreGrasp1', 'failed': 'failed', 'not_found': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'ref_frame': 'camera_ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part', 'pose': 'pose'}) # x:641 y:111 OperatableStateMachine.add('MoveR1PreGrasp1', SrdfStateToMoveitAriac(), transitions={'reached': 'ComputePick', 'planning_failed': 'WaitRetry2', 'control_failed': 'WaitRetry2', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_bin6PreGrasp', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:640 y:6 OperatableStateMachine.add('WaitRetry2', WaitState(wait_time=5), transitions={'done': 'MoveR1PreGrasp1'}, autonomy={'done': Autonomy.Off}) # x:840 y:113 OperatableStateMachine.add('ComputePick', ComputeGraspAriacState(joint_names=['linear_arm_actuator_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']), transitions={'continue': 'MoveR1ToPick1', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1023 y:110 OperatableStateMachine.add('MoveR1ToPick1', MoveitToJointsDynAriacState(), transitions={'reached': 'WachtEven', 'planning_failed': 'WaitRetry3', 'control_failed': 'WaitRetry3'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1215 y:107 OperatableStateMachine.add('WaitRetry3', WaitState(wait_time=5), transitions={'done': 'MoveR1ToPick1'}, autonomy={'done': Autonomy.Off}) # x:1022 y:200 OperatableStateMachine.add('WachtEven', WaitState(wait_time=1), transitions={'done': 'MoveR1PreGrasp2'}, autonomy={'done': Autonomy.Off}) # x:1017 y:280 OperatableStateMachine.add('MoveR1PreGrasp2', SrdfStateToMoveitAriac(), transitions={'reached': 'MoveR1PreDrop', 'planning_failed': 'WaitRetry4', 'control_failed': 'WaitRetry4', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_bin6PreGrasp', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1215 y:278 OperatableStateMachine.add('WaitRetry4', WaitState(wait_time=5), transitions={'done': 'MoveR1PreGrasp2'}, autonomy={'done': Autonomy.Off}) # x:1005 y:369 OperatableStateMachine.add('MoveR1PreDrop', SrdfStateToMoveitAriac(), transitions={'reached': 'GetAgvPose', 'planning_failed': 'WaitRetry5', 'control_failed': 'WaitRetry5', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name_tray1PreDrop', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:1210 y:372 OperatableStateMachine.add('WaitRetry5', WaitState(wait_time=5), transitions={'done': 'MoveR1PreDrop'}, autonomy={'done': Autonomy.Off}) # x:1002 y:454 OperatableStateMachine.add('GetAgvPose', GetObjectPoseState(object_frame='kit_tray_1', ref_frame='arm1_linear_arm_actuator'), transitions={'continue': 'ComputeDrop', 'failed': 'ComputeDrop'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'agv_pose'}) # x:998 y:592 OperatableStateMachine.add('ComputeDrop', ComputeGraspAriacState(joint_names=['linear_arm_actuator_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']), transitions={'continue': 'MoveR1ToDrop', 'failed': 'MoveR1ToDrop'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'agv_pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:661 y:522 OperatableStateMachine.add('MoveR1ToDrop', MoveitToJointsDynAriacState(), transitions={'reached': 'notify_shipment_ready', 'planning_failed': 'WaitRetry6', 'control_failed': 'WaitRetry6'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off}, remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:661 y:640 OperatableStateMachine.add('WaitRetry6', WaitState(wait_time=5), transitions={'done': 'MoveR1ToDrop'}, autonomy={'done': Autonomy.Off}) # x:418 y:581 OperatableStateMachine.add('notify_shipment_ready', self.use_behavior(notify_shipment_readySM, 'notify_shipment_ready'), transitions={'finished': 'EndAssignment', 'failed': 'EndAssignment'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) return _state_machine
def create(self): # x: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
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
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
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
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
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
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
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
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