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
