def create(self):
# x:983 y:56, x:356 y:305, x:268 y:204
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'unkown_id'], input_keys=['part_type', 'pose', 'joint_values'], output_keys=['part_type_left'])
_state_machine.userdata.part_type = ''
_state_machine.userdata.pose = []
_state_machine.userdata.move_group = 'Left_Arm'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.tool_link = 'left_ee_link'
_state_machine.userdata.part_offset = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.arm_id = 'Left_Arm'
_state_machine.userdata.part_type_left = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:70 y:44
OperatableStateMachine.add('DecideOffset',
DecideOffsetProduct(target_time=0.5),
transitions={'succes': 'ComputePick', 'unknown_id': 'unkown_id'},
autonomy={'succes': Autonomy.Off, 'unknown_id': Autonomy.Off},
remapping={'part_type': 'part_type', 'part_offset': 'part_offset'})
# x:427 y:38
OperatableStateMachine.add('MoveToPartBelt',
MoveitToJointsDynAriacState(),
transitions={'reached': 'EnableGripper', 'planning_failed': 'failed', 'control_failed': 'EnableGripper'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:629 y:37
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={'continue': 'ReplacePartName', 'failed': 'failed', 'invalid_id': 'unkown_id'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_id': Autonomy.Off},
remapping={'arm_id': 'arm_id'})
# x:237 y:40
OperatableStateMachine.add('ComputePick',
ComputeGraspAriacState(joint_names=['left_elbow_joint', 'left_shoulder_lift_joint', 'left_shoulder_pan_joint', 'left_wrist_1_joint', 'left_wrist_2_joint', 'left_wrist_2_joint']),
transitions={'continue': 'MoveToPartBelt', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'part_offset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:779 y:36
OperatableStateMachine.add('ReplacePartName',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'value': 'part_type', 'result': 'part_type_left'})
return _state_machine
def create(self):
# x: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:1129 y:453, x:560 y:460
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_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.04
_state_machine.userdata.rotation = 0
_state_machine.userdata.part = 'gear_part_red'
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.home = 'Home'
_state_machine.userdata.safe = 'Gantry_Bin'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:95
OperatableStateMachine.add('home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'FindParts',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'home',
'move_group': '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:242 y:32
OperatableStateMachine.add('Pos',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PartPose',
'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:672 y:30
OperatableStateMachine.add(
'Compute ',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'MoveToPart',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_R',
'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:436 y:43
OperatableStateMachine.add(
'PartPose',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Compute ',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part',
'pose': 'pose'
})
# x:874 y:46
OperatableStateMachine.add('MoveToPart',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_2',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_R',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:730 y:175
OperatableStateMachine.add('Gripper_2',
GripperControl(enable=True),
transitions={
'continue': 'finished',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:148 y:154
OperatableStateMachine.add('SafePos',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Pos',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'safe',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:30 y:219
OperatableStateMachine.add('FindParts',
FindPart(time_out=0.2),
transitions={
'found': 'SafePos',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'found': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'part_type': 'part',
'gantry_pos': 'config_name',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame'
})
return _state_machine
def create(self):
# x:1144 y:563, x:642 y:465
_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 = 'Right_Arm'
_state_machine.userdata.pose = []
_state_machine.userdata.offset = 0.020
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_belt'
_state_machine.userdata.camera_frame = 'logical_camera_belt_frame'
_state_machine.userdata.home = 'Gantry_Home'
_state_machine.userdata.part = 'gasket_part_blue'
_state_machine.userdata.belt = 'Gantry_Belt_Right'
_state_machine.userdata.full_home = 'Full_Home_Blue'
_state_machine.userdata.clearance = 0.05
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:29 y:50
OperatableStateMachine.add(
'Detect_Blue_Part',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Move_Belt',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part',
'pose': 'pose'
})
# x:240 y:46
OperatableStateMachine.add('Compute_Pick',
ComputeGraspAriacState(),
transitions={
'continue': 'Move_to_Pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:454 y:50
OperatableStateMachine.add('Move_to_Pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_On',
'planning_failed': 'failed',
'control_failed': 'Gripper_On'
},
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:671 y:56
OperatableStateMachine.add('Gripper_On',
GripperControl(enable=True),
transitions={
'continue': 'Move_Belt_2',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:838 y:52
OperatableStateMachine.add('Move_Belt_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Add_Clearance',
'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': '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:50 y:152
OperatableStateMachine.add('Move_Belt',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Offset',
'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': '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:1046 y:358
OperatableStateMachine.add('Left_Arm_Home',
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': '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:1041 y:144
OperatableStateMachine.add('Compute_Grasp',
ComputeGraspAriacState(),
transitions={
'continue': 'Move_Clearance',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1013 y:50
OperatableStateMachine.add('Add_Clearance',
AddNumericState(),
transitions={'done': 'Compute_Grasp'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'clearance',
'value_b': 'offset',
'result': 'offset'
})
# x:1039 y:226
OperatableStateMachine.add('Move_Clearance',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Left_Arm_Home',
'planning_failed': 'Left_Arm_Home',
'control_failed': 'Move_Clearance'
},
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:195 y:108
OperatableStateMachine.add('Offset',
part_offsetCalc(),
transitions={
'succes': 'Compute_Pick',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part',
'part_offset': 'offset'
})
return _state_machine
def create(self):
# x:811 y:574, x:283 y:395
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'move_group_prefix', 'part_type', 'bin', 'camera_topic',
'camera_frame', 'agv_id', 'ref_frame'
])
_state_machine.userdata.agv_id = []
_state_machine.userdata.part_type = []
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.bin = []
_state_machine.userdata.camera_topic = []
_state_machine.userdata.camera_frame = []
_state_machine.userdata.ref_frame = []
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.arm_id = ''
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.pose = []
_state_machine.userdata.part_offset = 0.08
_state_machine.userdata.rotation = 0
_state_machine.userdata.agv1 = 'agv1'
_state_machine.userdata.bin3 = 'transferBin3'
_state_machine.userdata.bin4 = 'transferBin4'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('MoveToHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ArmId',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:306 y:28
OperatableStateMachine.add('MoveToBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectPartPose',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'bin',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:453 y:26
OperatableStateMachine.add(
'DetectPartPose',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'ComputeGrasp',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:659 y:29
OperatableStateMachine.add(
'ComputeGrasp',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'MoveToPart',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:177 y:27
OperatableStateMachine.add('PartOffset',
part_offsetCalc(),
transitions={
'succes': 'MoveToBin',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'part_offset': 'part_offset'
})
# x:825 y:30
OperatableStateMachine.add('MoveToPart',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'EnableGripper',
'planning_failed': 'failed',
'control_failed': 'EnableGripper'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1033 y:60
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={
'continue': 'agv1?',
'failed': 'MoveToPart',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1012 y:345
OperatableStateMachine.add('MoveToTransferBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DisableGripper',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'bin',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1018 y:130
OperatableStateMachine.add('agv1?',
EqualState(),
transitions={
'true': 'bin4',
'false': 'bin3'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'agv1',
'value_b': 'agv_id'
})
# x:880 y:207
OperatableStateMachine.add('bin3',
ReplaceState(),
transitions={'done': 'MoveToHome_3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin3',
'result': 'bin'
})
# x:880 y:273
OperatableStateMachine.add('bin4',
ReplaceState(),
transitions={'done': 'MoveToHome_3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'bin4',
'result': 'bin'
})
# x:1034 y:438
OperatableStateMachine.add('DisableGripper',
GripperControl(enable=False),
transitions={
'continue': 'MoveToHome_2',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:942 y:514
OperatableStateMachine.add('MoveToHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1074 y:235
OperatableStateMachine.add('MoveToHome_3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveToTransferBin',
'planning_failed': 'failed',
'control_failed': 'MoveToHome_3',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:30 y:102
OperatableStateMachine.add('ArmId',
chooseArmID(),
transitions={
'continue': 'PartOffset',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'arm_id': 'arm_id'
})
return _state_machine
def create(self):
# x:27 y:469, x:793 y:325
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['pose_on_agv', 'part_type'],
output_keys=['pose_on_agv_l', 'pose_on_agv_r'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.part_type = ''
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link_r = 'right_ee_link'
_state_machine.userdata.pose = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_groupR = 'Right_Arm'
_state_machine.userdata.arm_id_r = 'Right_Arm'
_state_machine.userdata.Grasp5 = 'bingr5Grasp'
_state_machine.userdata.arm_id_l = 'Left_Arm'
_state_machine.userdata.move_groupL = 'Left_Arm'
_state_machine.userdata.tool_link_l = 'left_ee_link'
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.pose_on_agv_l = []
_state_machine.userdata.pose_on_agv_r = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:204 y:22
OperatableStateMachine.add('move_home_pick',
self.use_behavior(
move_home_pickSM, 'move_home_pick'),
transitions={
'finished': 'Find_product_location',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:660 y:108
OperatableStateMachine.add('move_gantry_bin_gr1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'move_gantry_bin_gr1_2',
'planning_failed': 'Waitretry',
'control_failed': 'Waitretry',
'param_error': 'Waitretry'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'bin',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:471 y:43
OperatableStateMachine.add('Find_product_location',
FindCorrectBin(time_out=0.5),
transitions={
'continue': 'move_gantry_bin_gr1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'bin': 'bin',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'ref_frame': 'ref_frame'
})
# x:1089 y:258
OperatableStateMachine.add(
'compute_pick _r',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_groupR',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_r',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1096 y:74
OperatableStateMachine.add(
'find part',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Offset bepalen',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:1300 y:88
OperatableStateMachine.add('Offset bepalen',
DecideOffsetProduct(target_time=0.5),
transitions={
'succes': 'gripper check',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'part_offset': 'part_offset'
})
# x:1024 y:463
OperatableStateMachine.add('Gripper_enable',
GripperControl(enable=True),
transitions={
'continue': 'pose safe r',
'failed': 'Move to pick',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_r'})
# x:648 y:701
OperatableStateMachine.add('move_home_robot_r',
self.use_behavior(
move_home_robot_rSM,
'move_home_robot_r'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:1064 y:360
OperatableStateMachine.add('Move to pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_enable',
'planning_failed': 'failed',
'control_failed': 'Gripper_enable'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_groupR',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:870 y:107
OperatableStateMachine.add('move_gantry_bin_gr1_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'find part',
'planning_failed': 'Waitretry_2',
'control_failed': 'Waitretry_2',
'param_error': 'Waitretry_2'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'Grasp5',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:882 y:10
OperatableStateMachine.add(
'Waitretry_2',
WaitState(wait_time=2),
transitions={'done': 'move_gantry_bin_gr1_2'},
autonomy={'done': Autonomy.Off})
# x:686 y:12
OperatableStateMachine.add(
'Waitretry',
WaitState(wait_time=2),
transitions={'done': 'move_gantry_bin_gr1'},
autonomy={'done': Autonomy.Off})
# x:1306 y:173
OperatableStateMachine.add('gripper check',
GripperActiveCheck(),
transitions={
'Left': 'compute_pick _l',
'Right': 'compute_pick _r',
'failed': 'failed',
'Full': 'move_home_pick'
},
autonomy={
'Left': Autonomy.Off,
'Right': Autonomy.Off,
'failed': Autonomy.Off,
'Full': Autonomy.Off
},
remapping={
'arm_id': 'arm_id',
'tool_link': 'tool_link',
'move_group': 'move_group'
})
# x:1527 y:231
OperatableStateMachine.add(
'compute_pick _l',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick_l',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_groupL',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_l',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1573 y:425
OperatableStateMachine.add('Gripper_enable_2',
GripperControl(enable=True),
transitions={
'continue': 'pose safe l',
'failed': 'Move to pick_l',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_l'})
# x:1530 y:319
OperatableStateMachine.add('Move to pick_l',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_enable_2',
'planning_failed': 'failed',
'control_failed': 'Gripper_enable_2'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_groupL',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:865 y:537
OperatableStateMachine.add(
'pose safe r',
ReplaceState(),
transitions={'done': 'move_home_robot_r'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv_r'
})
# x:1502 y:553
OperatableStateMachine.add(
'pose safe l',
ReplaceState(),
transitions={'done': 'move_home_robot_r'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv_l'
})
return _state_machine
def create(self):
# x:27 y:469, x:242 y:360
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group_g = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.part_type = 'gear_part_blue'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.arm_id = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:145 y:28
OperatableStateMachine.add('move_home_pick',
self.use_behavior(
move_home_pickSM, 'move_home_pick'),
transitions={
'finished': 'Find_product_location',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:701 y:151
OperatableStateMachine.add('move_gantry_bin_gr1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'find part',
'planning_failed': 'Waitretry',
'control_failed': 'Waitretry',
'param_error': 'Waitretry'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'bin',
'move_group': 'move_group_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:479 y:25
OperatableStateMachine.add('Find_product_location',
FindCorrectBin(time_out=0.5),
transitions={
'continue': 'partype mesage',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'bin': 'bin',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'ref_frame': 'ref_frame'
})
# x:694 y:497
OperatableStateMachine.add(
'compute_pick _r',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:713 y:240
OperatableStateMachine.add(
'find part',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Offset bepalen',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:760 y:314
OperatableStateMachine.add('Offset bepalen',
DecideOffsetProduct(target_time=0.5),
transitions={
'succes': 'check_free_gripper',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'part_offset': 'part_offset'
})
# x:433 y:563
OperatableStateMachine.add('Gripper_enable',
GripperControl(enable=True),
transitions={
'continue': 'move_home_robot_r',
'failed': 'Move to pick',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:234 y:591
OperatableStateMachine.add('move_home_robot_r',
self.use_behavior(
move_home_robot_rSM,
'move_home_robot_r'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:619 y:590
OperatableStateMachine.add('Move to pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_enable',
'planning_failed': 'failed',
'control_failed': 'Gripper_enable'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:812 y:393
OperatableStateMachine.add('check_free_gripper',
GripperActiveCheck(),
transitions={
'Left': 'compute_pick _l',
'Right': 'compute_pick _r',
'failed': 'failed',
'Full': 'move_home_pick'
},
autonomy={
'Left': Autonomy.Off,
'Right': Autonomy.Off,
'failed': Autonomy.Off,
'Full': Autonomy.Off
},
remapping={
'arm_id': 'arm_id',
'tool_link': 'tool_link',
'move_group': 'move_group'
})
# x:942 y:468
OperatableStateMachine.add(
'compute_pick _l',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:627 y:57
OperatableStateMachine.add(
'partype mesage',
MessageState(),
transitions={'continue': 'move_gantry_bin_gr1'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:810 y:40
OperatableStateMachine.add(
'Waitretry',
WaitState(wait_time=2),
transitions={'done': 'move_gantry_bin_gr1'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:894 y:460, x:898 y:340
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['bin_id', 'camera_topic', 'camera_frame', 'part_type', 'PreGraspGantry', 'offset'])
_state_machine.userdata.bin_id = ''
_state_machine.userdata.home_gantry_id = 'Gantry_Home'
_state_machine.userdata.home_rightarm_id = 'Right_PreGrasp'
_state_machine.userdata.home_leftarm_id = 'Left_PreGrasp'
_state_machine.userdata.move_group_right = 'Right_Arm'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.move_group_left = 'Left_Arm'
_state_machine.userdata.move_group_gantry = 'Gantry'
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.PreGraspGantry = ''
_state_machine.userdata.SafePosition = 'Gantry_SafeHall2'
_state_machine.userdata.tool_link = 'right_ee_link'
_state_machine.userdata.rotation = 0
_state_machine.userdata.offset = ''
_state_machine.userdata.arm_id = 'right_arm'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:171 y:112
OperatableStateMachine.add('HomepositieRightArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'HomepositieLeftArm', 'planning_failed': 'Retry_1', 'control_failed': 'Retry_1', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'home_rightarm_id', 'move_group': 'move_group_right', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:346 y:112
OperatableStateMachine.add('HomepositieLeftArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'DetectPart', 'planning_failed': 'Retry_2', 'control_failed': 'Retry_2', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'home_leftarm_id', 'move_group': 'move_group_left', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:192 y:14
OperatableStateMachine.add('Retry_1',
WaitState(wait_time=3),
transitions={'done': 'HomepositieRightArm'},
autonomy={'done': Autonomy.Off})
# x:369 y:20
OperatableStateMachine.add('Retry_2',
WaitState(wait_time=3),
transitions={'done': 'HomepositieLeftArm'},
autonomy={'done': Autonomy.Off})
# x:674 y:117
OperatableStateMachine.add('DetectPart',
DetectPartCameraAriacState(time_out=0.5),
transitions={'continue': 'SafePosition', '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': 'part_pose'})
# x:1040 y:119
OperatableStateMachine.add('PreGraspGantry',
SrdfStateToMoveitAriac(),
transitions={'reached': 'ComputePick', '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': 'PreGraspGantry', '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:1059 y:29
OperatableStateMachine.add('Retry_4',
WaitState(wait_time=3),
transitions={'done': 'PreGraspGantry'},
autonomy={'done': Autonomy.Off})
# x:865 y:116
OperatableStateMachine.add('SafePosition',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PreGraspGantry', 'planning_failed': 'Retry_4_2', 'control_failed': 'Retry_4_2', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'SafePosition', '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:888 y:29
OperatableStateMachine.add('Retry_4_2',
WaitState(wait_time=3),
transitions={'done': 'SafePosition'},
autonomy={'done': Autonomy.Off})
# x:1222 y:129
OperatableStateMachine.add('ComputePick',
ComputeGraspAriacState(joint_names=['right_shoulder_pan_joint', 'right_shoulder_lift_joint', 'right_elbow_joint', 'right_wrist_1_joint', 'right_wrist_2_joint', 'right_wrist_3_joint']),
transitions={'continue': 'MoveToPick', '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', 'pose': 'part_pose', 'offset': 'offset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1403 y:125
OperatableStateMachine.add('MoveToPick',
MoveitToJointsDynAriacState(),
transitions={'reached': 'GripperOn', 'planning_failed': 'Retry_5', 'control_failed': 'GripperOn'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group_right', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1438 y:29
OperatableStateMachine.add('Retry_5',
WaitState(wait_time=3),
transitions={'done': 'MoveToPick'},
autonomy={'done': Autonomy.Off})
# x:1407 y:217
OperatableStateMachine.add('GripperOn',
VacuumGripperControlState(enable=True),
transitions={'continue': 'PreGraspRightArm', 'failed': 'ComputePick', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_id'})
# x:1412 y:298
OperatableStateMachine.add('PreGraspRightArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'SafePosition_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': 'home_rightarm_id', 'move_group': 'move_group_right', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1619 y:321
OperatableStateMachine.add('Retry_6',
WaitState(wait_time=3),
transitions={'done': 'PreGraspRightArm'},
autonomy={'done': Autonomy.Off})
# x:1398 y:392
OperatableStateMachine.add('SafePosition_2',
SrdfStateToMoveitAriac(),
transitions={'reached': 'finished', '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': 'SafePosition', '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:1604 y:418
OperatableStateMachine.add('Retry_6_2',
WaitState(wait_time=3),
transitions={'done': 'SafePosition_2'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:448 y:497, x:101 y:396
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.arm_id = ''
_state_machine.userdata.power_on = 80
_state_machine.userdata.power_off = 0
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix1 = '/ariac/arm1'
_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.move_group_prefix2 = '/ariac/arm2'
_state_machine.userdata.ref_frame = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_belt1'
_state_machine.userdata.camera_frame = 'logical_camera_belt1_frame'
_state_machine.userdata.part_type = 'disk_part'
_state_machine.userdata.pose = []
_state_machine.userdata.config_name_belt = 'robotBelt'
_state_machine.userdata.sensor_topic = '/ariac/break_beam_1'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.part_offset = 0.04
_state_machine.userdata.part_rotation = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:32 y:30
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={'continue': 'TurnOnConveyor'},
autonomy={'continue': Autonomy.Off})
# x:522 y:27
OperatableStateMachine.add('MoveR2home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveR1Belt',
'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_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:176 y:29
OperatableStateMachine.add('TurnOnConveyor',
SetConveyorbeltPowerState(),
transitions={
'continue': 'MoveR1Home',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power_on'})
# x:360 y:28
OperatableStateMachine.add('MoveR1Home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveR2home',
'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_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:690 y:25
OperatableStateMachine.add('MoveR1Belt',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectPart',
'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_belt',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:863 y:25
OperatableStateMachine.add(
'DetectPart',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'TurnOffConveyor',
'failed': 'WaitRetry',
'not_found': 'WaitRetry'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:715 y:141
OperatableStateMachine.add('WaitRetry',
WaitState(wait_time=0.1),
transitions={'done': 'DetectPart'},
autonomy={'done': Autonomy.Off})
# x:1127 y:363
OperatableStateMachine.add('MoveToPart',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'EnableGripper',
'planning_failed': 'failed',
'control_failed': 'EnableGripper'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix1',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1136 y:455
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={
'continue': 'MoveHome',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1115 y:267
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_prefix1',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1081 y:544
OperatableStateMachine.add('MoveHome',
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_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1043 y:58
OperatableStateMachine.add('TurnOffConveyor',
SetConveyorbeltPowerState(),
transitions={
'continue': 'DetectPart_2',
'fail': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'fail': Autonomy.Off
},
remapping={'power': 'power_off'})
# x:1071 y:136
OperatableStateMachine.add(
'DetectPart_2',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'ComputeGrasp',
'failed': 'failed',
'not_found': 'WaitRetry'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
return _state_machine
def create(self):
# x:1227 y:622, x:267 y:397
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.move_group_g = '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.powerOff = 0
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_6'
_state_machine.userdata.camera_frame = 'logical_camera_6_frame'
_state_machine.userdata.pose = []
_state_machine.userdata.camera_topic_7 = '/ariac/logical_camera_7'
_state_machine.userdata.camera_frame_7 = 'logical_camera_7_frame'
_state_machine.userdata.part_offset = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link = 'right_ee_link'
_state_machine.userdata.config_name_pregrasp = 'beltPreGrasp'
_state_machine.userdata.part_type = ''
_state_machine.userdata.move_group = 'Right_Arm'
_state_machine.userdata.joint_names_r = ['right_shoulder_pan_joint', 'right_shoulder_lift_joint', 'right_elbow_joint', 'right_wrist_1_joint', 'right_wrist_2_joint', 'right_wrist_3_joint']
_state_machine.userdata.robot_name_g = ''
_state_machine.userdata.arm_id = 'Right_Arm'
_state_machine.userdata.config_name_r_home = 'Right_Home'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'TurnOnConveyor'},
autonomy={'continue': Autonomy.Off})
# x:172 y:41
OperatableStateMachine.add('TurnOnConveyor',
SetConveyorbeltPowerState(),
transitions={'continue': 'move_home_belt', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'powerOn'})
# x:361 y:39
OperatableStateMachine.add('move_home_belt',
self.use_behavior(move_home_beltSM, 'move_home_belt'),
transitions={'finished': 'DetectFirstPartBelt', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:769 y:38
OperatableStateMachine.add('TurnOffConveyor',
SetConveyorbeltPowerState(),
transitions={'continue': 'DetectFirstPartBelt_2', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'powerOff'})
# x:1156 y:204
OperatableStateMachine.add('ComputeGrasp',
ComputeGraspAriacState(joint_names=['right_shoulder_pan_joint', 'right_shoulder_lift_joint', 'right_elbow_joint', 'right_wrist_1_joint', 'right_wrist_2_joint', 'right_wrist_3_joint']),
transitions={'continue': 'PickProduct', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'part_offset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:470 y:134
OperatableStateMachine.add('WaitRetry',
WaitState(wait_time=0.5),
transitions={'done': 'DetectFirstPartBelt'},
autonomy={'done': Autonomy.Off})
# x:1143 y:288
OperatableStateMachine.add('PickProduct',
MoveitToJointsDynAriacState(),
transitions={'reached': 'EnableGripper', 'planning_failed': 'failed', 'control_failed': 'EnableGripper'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:561 y:38
OperatableStateMachine.add('DetectFirstPartBelt',
DetectFirstPartCameraAriacState(part_list=['piston_rod_part_red', 'gasket_part_blue_0', 'gasket_part_blue_1', 'gasket_part_blue_2'], time_out=0.5),
transitions={'continue': 'TurnOffConveyor', 'failed': 'WaitRetry', 'not_found': 'WaitRetry'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part_type', 'pose': 'pose'})
# x:958 y:36
OperatableStateMachine.add('DetectFirstPartBelt_2',
DetectFirstPartCameraAriacState(part_list=['piston_rod_part_red', 'gasket_part_blue_0', 'gasket_part_blue_1', 'gasket_part_blue_2'], time_out=0.5),
transitions={'continue': 'MovePreGraspBelt', '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:1164 y:36
OperatableStateMachine.add('MovePreGraspBelt',
SrdfStateToMoveitAriac(),
transitions={'reached': 'DecideOffset', '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_pregrasp', 'move_group': 'move_group_g', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name_g', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1167 y:449
OperatableStateMachine.add('MoveUp',
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_r_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:1186 y:368
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={'continue': 'MoveUp', 'failed': 'PickProduct', 'invalid_id': 'PickProduct'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_id': Autonomy.Off},
remapping={'arm_id': 'arm_id'})
# x:1177 y:119
OperatableStateMachine.add('DecideOffset',
DecideOffsetProduct(target_time=0.5),
transitions={'succes': 'ComputeGrasp', 'unknown_id': 'DetectFirstPartBelt_2'},
autonomy={'succes': Autonomy.Off, 'unknown_id': Autonomy.Off},
remapping={'part_type': 'part_type', 'part_offset': 'part_offset'})
return _state_machine
def create(self):
# x:813 y:383, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.config_name_left_home = 'Left_Home'
_state_machine.userdata.move_group_left = 'Left_Arm'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.joint_value = []
_state_machine.userdata.joint_names = ''
_state_machine.userdata.pose = []
_state_machine.userdata.part_offset = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link = 'left_ee_link'
_state_machine.userdata.part_type = ''
_state_machine.userdata.arm_id = 'Left_Arm'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('MovePreGraspBeltRight',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DecideOffset',
'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_home',
'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:207 y:40
OperatableStateMachine.add('DecideOffset',
DecideOffsetProduct(target_time=0.5),
transitions={
'succes': 'ComputeGraspRight',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'part_offset': 'part_offset'
})
# x:369 y:39
OperatableStateMachine.add(
'ComputeGraspRight',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'MoveToPickRight',
'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',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:548 y:40
OperatableStateMachine.add('MoveToPickRight',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'EnableRightGripper',
'planning_failed': 'failed',
'control_failed':
'EnableRightGripper'
},
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:750 y:38
OperatableStateMachine.add('EnableRightGripper',
GripperControl(enable=True),
transitions={
'continue': 'MoveRightUp',
'failed': 'MovePreGraspBeltRight',
'invalid_id':
'MovePreGraspBeltRight'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:741 y:201
OperatableStateMachine.add('MoveRightUp',
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_home',
'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'
})
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:412 y:439, x:647 y:299
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type', 'pose_on_agv'],
output_keys=['pose_on_agv_r', 'pose_on_agv_l'])
_state_machine.userdata.powerOn = 100
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group_g = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.tool_link = ''
_state_machine.userdata.pose = ''
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.arm_id = ''
_state_machine.userdata.config_name_r = 'Right_Home_B'
_state_machine.userdata.config_name_l = 'Left_Home_B'
_state_machine.userdata.part_type = ''
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.pose_on_agv_l = []
_state_machine.userdata.pose_on_agv_r = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:20 y:123
OperatableStateMachine.add(
'part in pick prg',
MessageState(),
transitions={'continue': 'Find_product_location'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:338 y:86
OperatableStateMachine.add('move_gantry_bin_gr1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'find part',
'planning_failed': 'Waitretry',
'control_failed': 'Waitretry',
'param_error': 'Waitretry'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'bin',
'move_group': 'move_group_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:945 y:160
OperatableStateMachine.add(
'compute_pick _r',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick_r',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:544 y:68
OperatableStateMachine.add(
'find part',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Offset bepalen',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:760 y:17
OperatableStateMachine.add('Offset bepalen',
DecideOffsetProduct(target_time=0.5),
transitions={
'succes': 'check_free_gripper',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'part_offset': 'part_offset'
})
# x:959 y:326
OperatableStateMachine.add('Gripper_enable_r',
GripperControl(enable=True),
transitions={
'continue': 'Move_pre_grasp',
'failed': 'Move to pick_r',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:949 y:244
OperatableStateMachine.add('Move to pick_r',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_enable_r',
'planning_failed': 'failed',
'control_failed': 'Gripper_enable_r'
},
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:921 y:17
OperatableStateMachine.add('check_free_gripper',
GripperActiveCheck(),
transitions={
'Left': 'compute_pick _l',
'Right': 'compute_pick _r',
'failed': 'failed',
'Full': 'finished'
},
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:1292 y:51
OperatableStateMachine.add(
'compute_pick _l',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move to pick_l',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:873 y:409
OperatableStateMachine.add('Move_pre_grasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Agv pose R',
'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_r',
'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:1273 y:273
OperatableStateMachine.add('Gripper_enable_l',
GripperControl(enable=True),
transitions={
'continue': 'Move_pre_grasp_2',
'failed': 'Move to pick_l',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1239 y:164
OperatableStateMachine.add('Move to pick_l',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_enable_l',
'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:1269 y:384
OperatableStateMachine.add('Move_pre_grasp_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Agv pose L',
'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_l',
'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:436 y:3
OperatableStateMachine.add(
'Waitretry',
WaitState(wait_time=2),
transitions={'done': 'move_gantry_bin_gr1'},
autonomy={'done': Autonomy.Off})
# x:152 y:81
OperatableStateMachine.add('Find_product_location',
FindCorrectBin(time_out=0.5),
transitions={
'continue': 'move_gantry_bin_gr1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'part_type': 'part_type',
'bin': 'bin',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'ref_frame': 'ref_frame'
})
# x:769 y:508
OperatableStateMachine.add('Agv pose R',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv_r'
})
# x:1257 y:505
OperatableStateMachine.add('Agv pose L',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'pose_on_agv',
'result': 'pose_on_agv_l'
})
return _state_machine
def create(self):
# x:385 y:685, x:966 y:441
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.config_name_gantry = 'Gantry_Home'
_state_machine.userdata.move_group_gantry = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.move_group_right_arm = 'Right_Arm'
_state_machine.userdata.config_name_left_arm = 'Left_Home'
_state_machine.userdata.config_name_right_arm = 'Right_Home'
_state_machine.userdata.move_group_left_arm = 'Left_Arm'
_state_machine.userdata.config_name_PreSide = 'Gantry_Pre_Side'
_state_machine.userdata.config_name_PreGrasp = 'Gantry_PreGrasp_Right_bins_1'
_state_machine.userdata.camera_ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_1'
_state_machine.userdata.camera_frame = 'logical_camera_1_frame'
_state_machine.userdata.part_type = 'pulley_part_red'
_state_machine.userdata.offset = 0.085
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link_right = 'right_ee_link'
_state_machine.userdata.arm_id = 'right'
_state_machine.userdata.config_name_PreDrop = 'Gantry_Predrop_AGV_1'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:164 y:146
OperatableStateMachine.add('Move_left_arm_home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move_right_arm_home',
'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_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:201 y:38
OperatableStateMachine.add(
'Retry',
WaitState(wait_time=0.3),
transitions={'done': 'Move_left_arm_home'},
autonomy={'done': Autonomy.Off})
# x:332 y:148
OperatableStateMachine.add(
'Move_right_arm_home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move_gantry_home',
'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': '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:362 y:37
OperatableStateMachine.add(
'Retry_2',
WaitState(wait_time=0.3),
transitions={'done': 'Move_right_arm_home'},
autonomy={'done': Autonomy.Off})
# x:525 y:149
OperatableStateMachine.add('Move_gantry_home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreSide',
'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_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:549 y:32
OperatableStateMachine.add(
'Retry_3',
WaitState(wait_time=0.3),
transitions={'done': 'Move_gantry_home'},
autonomy={'done': Autonomy.Off})
# x:745 y:152
OperatableStateMachine.add('PreSide',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectPart',
'planning_failed': 'Retry_4',
'control_failed': 'Retry_4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_PreSide',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:773 y:27
OperatableStateMachine.add('Retry_4',
WaitState(wait_time=0.3),
transitions={'done': 'PreSide'},
autonomy={'done': Autonomy.Off})
# x:1111 y:151
OperatableStateMachine.add('PreGrasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePick',
'planning_failed': 'Retry_5',
'control_failed': 'Retry_5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_PreGrasp',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1129 y:33
OperatableStateMachine.add('Retry_5',
WaitState(wait_time=0.3),
transitions={'done': 'PreGrasp'},
autonomy={'done': Autonomy.Off})
# x:1308 y:150
OperatableStateMachine.add(
'ComputePick',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Move_to_pick',
'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': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:918 y:149
OperatableStateMachine.add(
'DetectPart',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'PreGrasp',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:1552 y:44
OperatableStateMachine.add('Retry_6',
WaitState(wait_time=0.3),
transitions={'done': 'Move_to_pick'},
autonomy={'done': Autonomy.Off})
# x:1501 y:148
OperatableStateMachine.add('Move_to_pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperOn',
'planning_failed': 'Retry_6',
'control_failed': 'GripperOn'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group':
'move_group_right_arm',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1510 y:247
OperatableStateMachine.add('GripperOn',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'Move_right_arm_home_2',
'failed': 'ComputePick',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1517 y:381
OperatableStateMachine.add(
'Move_right_arm_home_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreSide_2',
'planning_failed': 'Retry_7',
'control_failed': 'Retry_7',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_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:1729 y:398
OperatableStateMachine.add(
'Retry_7',
WaitState(wait_time=0.3),
transitions={'done': 'Move_right_arm_home_2'},
autonomy={'done': Autonomy.Off})
# x:1525 y:496
OperatableStateMachine.add('PreSide_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move_gantry_home_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':
'config_name_PreSide',
'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:1523 y:593
OperatableStateMachine.add('Move_gantry_home_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreDrop_AGV1',
'planning_failed': 'Retry_9',
'control_failed': 'Retry_9',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_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:1730 y:498
OperatableStateMachine.add('Retry_8',
WaitState(wait_time=0.3),
transitions={'done': 'PreSide_2'},
autonomy={'done': Autonomy.Off})
# x:1551 y:733
OperatableStateMachine.add(
'Retry_9',
WaitState(wait_time=0.3),
transitions={'done': 'Move_gantry_home_2'},
autonomy={'done': Autonomy.Off})
# x:1334 y:727
OperatableStateMachine.add('Retry_10',
WaitState(wait_time=0.3),
transitions={'done': 'PreDrop_AGV1'},
autonomy={'done': Autonomy.Off})
# x:1305 y:592
OperatableStateMachine.add('PreDrop_AGV1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'Retry_10',
'control_failed': 'Retry_10',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_PreDrop',
'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'
})
return _state_machine
def create(self):
# x:692 y:77, x:764 y:355
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'part_type1', 'part_type2', 'offset1', 'offset2',
'camera_topic1', 'camera_topic2', 'camera_frame1',
'camera_frame2', 'config_name_Gantry_pick1',
'config_name_Gantry_pick2', 'config_name_PrePick1',
'config_name_PrePick2'
])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type1 = ''
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.offset1 = ''
_state_machine.userdata.move_group_G = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic1 = ''
_state_machine.userdata.camera_frame1 = ''
_state_machine.userdata.tool_link_R = 'right_ee_link'
_state_machine.userdata.arm_id1 = 'left'
_state_machine.userdata.config_name_Gantry_pick1 = ''
_state_machine.userdata.null = 0
_state_machine.userdata.config_name_Left_home = 'Left_Home'
_state_machine.userdata.config_name_Right_home = 'Right_Home'
_state_machine.userdata.move_group_L = 'Left_Arm'
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.config_name_L = 'Links_PreDrop_Stelling'
_state_machine.userdata.config_name_R = 'Rechts_PreDrop_Stelling'
_state_machine.userdata.tool_link_L = 'left_ee_link'
_state_machine.userdata.rotation = 0
_state_machine.userdata.camera_ref_frame = 'world'
_state_machine.userdata.config_name_PrePick1 = ''
_state_machine.userdata.config_name_PreDrop = 'Links_PreDrop_AGV'
_state_machine.userdata.pose = []
_state_machine.userdata.config_name_Gantry_pick2 = ''
_state_machine.userdata.part_type2 = ''
_state_machine.userdata.camera_frame2 = ''
_state_machine.userdata.camera_topic2 = ''
_state_machine.userdata.offset2 = ''
_state_machine.userdata.arm_id2 = 'right'
_state_machine.userdata.config_name_PrePick2 = ''
_state_machine.userdata.config_name_GantrySafe = 'Gantry_Transportband_Home'
_state_machine.userdata.AtShelve = 0
_state_machine.userdata.true = 1
_state_machine.userdata.false = 0
_state_machine.userdata.Lshelve = 'Links_PreDrop_Stelling'
_state_machine.userdata.Rshelve = 'Rechts_PreDrop_Stelling'
_state_machine.userdata.empty = 'empty'
_state_machine.userdata.config_name_Gantry_AtShelve = 'Gantry_Stelling'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:939 y:13
OperatableStateMachine.add('LeftHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'RightHome',
'planning_failed': 'failed',
'control_failed': 'RightHome',
'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_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:1150 y:13
OperatableStateMachine.add(
'RightHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GripperDisable_2',
'planning_failed': 'failed',
'control_failed': 'GripperDisable_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_home',
'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:1650 y:396
OperatableStateMachine.add('CheckPartPoseBin',
DetectPartCameraAriacState(time_out=2),
transitions={
'continue': 'ComputePick',
'failed': 'failed',
'not_found': 'Retry'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic1',
'camera_frame': 'camera_frame1',
'part': 'part_type1',
'pose': 'pose'
})
# x:1660 y:495
OperatableStateMachine.add(
'ComputePick',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Pick',
'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',
'pose': 'pose',
'offset': 'offset1',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1430 y:397
OperatableStateMachine.add('Retry',
WaitState(wait_time=0.2),
transitions={'done': 'PrePick'},
autonomy={'done': Autonomy.Off})
# x:1673 y:283
OperatableStateMachine.add(
'PrePick',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'CheckPartPoseBin',
'planning_failed': 'failed',
'control_failed': 'CheckPartPoseBin',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_PrePick1',
'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:1451 y:763
OperatableStateMachine.add('LeftHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'empty?',
'planning_failed': 'failed',
'control_failed': 'empty?',
'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_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:1348 y:541
OperatableStateMachine.add('Pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperEnable',
'planning_failed': 'failed',
'control_failed': 'GripperEnable'
},
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:1679 y:758
OperatableStateMachine.add('PrePick_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'LeftHome_2',
'planning_failed': 'failed',
'control_failed': 'LeftHome_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_PrePick1',
'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:1657 y:659
OperatableStateMachine.add('GripperEnable',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'PrePick_2',
'failed': 'ComputePick',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id1'})
# x:475 y:757
OperatableStateMachine.add('Pregrasp_G_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PrePick_3',
'planning_failed': 'failed',
'control_failed': 'PrePick_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_Gantry_pick2',
'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:15 y:701
OperatableStateMachine.add('CheckPartPoseBin_2',
DetectPartCameraAriacState(time_out=2),
transitions={
'continue': 'ComputePick_2',
'failed': 'Retry_2',
'not_found': 'Retry_2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic2',
'camera_frame': 'camera_frame2',
'part': 'part_type2',
'pose': 'pose2'
})
# x:13 y:536
OperatableStateMachine.add(
'ComputePick_2',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'Pick_2',
'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',
'pose': 'pose2',
'offset': 'offset2',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:271 y:602
OperatableStateMachine.add('Retry_2',
WaitState(wait_time=0.2),
transitions={'done': 'PrePick_3'},
autonomy={'done': Autonomy.Off})
# x:255 y:742
OperatableStateMachine.add(
'PrePick_3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'CheckPartPoseBin_2',
'planning_failed': 'failed',
'control_failed': 'CheckPartPoseBin_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_PrePick2',
'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:285 y:456
OperatableStateMachine.add('Pick_2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperEnable_2',
'planning_failed': 'failed',
'control_failed': 'GripperEnable_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:28 y:367
OperatableStateMachine.add('GripperEnable_2',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'PrePick_2_2',
'failed': 'ComputePick_2',
'invalid_arm_id': 'PrePick_2_2'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id2'})
# x:39 y:257
OperatableStateMachine.add('PrePick_2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'RightHome_2',
'planning_failed': 'failed',
'control_failed': 'RightHome_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_PrePick2',
'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:1568 y:9
OperatableStateMachine.add('GripperDisable',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'ShelveCheck1',
'failed': 'ShelveCheck1',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id1'})
# x:1359 y:10
OperatableStateMachine.add('GripperDisable_2',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'GripperDisable',
'failed': 'GripperDisable',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id2'})
# x:37 y:150
OperatableStateMachine.add(
'RightHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'SavePosShelve2_2',
'planning_failed': 'failed',
'control_failed': 'SavePosShelve2_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_home',
'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:1657 y:92
OperatableStateMachine.add('ShelveCheck1',
EqualState(),
transitions={
'true': 'SetShelve1',
'false': 'Pregrasp_G'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'config_name_PrePick1',
'value_b': 'Lshelve'
})
# x:1441 y:206
OperatableStateMachine.add('SavePosShelve1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Pregrasp_G',
'planning_failed': 'failed',
'control_failed': 'Pregrasp_G',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_GantrySafe',
'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:1433 y:102
OperatableStateMachine.add('SetShelve1',
ReplaceState(),
transitions={'done': 'SavePosShelve1'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'true',
'result': 'AtShelve'
})
# x:1044 y:755
OperatableStateMachine.add('AtShelve?',
EqualState(),
transitions={
'true': 'ShelveCheck3',
'false': 'ShelveCheck2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'AtShelve',
'value_b': 'true'
})
# x:770 y:765
OperatableStateMachine.add('ShelveCheck3',
EqualState(),
transitions={
'true': 'Pregrasp_G_2',
'false': 'SavePosShelve2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'config_name_PrePick2',
'value_b': 'Rshelve'
})
# x:692 y:623
OperatableStateMachine.add('SavePosShelve2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Pregrasp_G_2',
'planning_failed': 'failed',
'control_failed': 'Pregrasp_G_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_GantrySafe',
'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:354 y:107
OperatableStateMachine.add('SavePosShelve2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_GantrySafe',
'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:1237 y:753
OperatableStateMachine.add('empty?',
EqualState(),
transitions={
'true': 'SavePosShelve2_2',
'false': 'AtShelve?'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'part_type2',
'value_b': 'empty'
})
# x:1671 y:179
OperatableStateMachine.add('Pregrasp_G',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PrePick',
'planning_failed': 'failed',
'control_failed': 'PrePick',
'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_pick1',
'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:870 y:686
OperatableStateMachine.add('ShelveCheck2',
EqualState(),
transitions={
'true': 'SavePosShelve2',
'false': 'Pregrasp_G_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'config_name_PrePick2',
'value_b': 'Rshelve'
})
return _state_machine
def create(self):
# x:328 y:654, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['part_type_R'])
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.pose = []
_state_machine.userdata.offset = 0.03
_state_machine.userdata.rotation = 0
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.tool_link = 'right_ee_link'
_state_machine.userdata.move_group = ''
_state_machine.userdata.config_name = 'Gantry_Pulley_R'
_state_machine.userdata.part_type_R = 'pulley_part_red'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('start',
StartAssignment(),
transitions={'continue': 'Find'},
autonomy={'continue': Autonomy.Off})
# x:285 y:343
OperatableStateMachine.add(
'Compute grasp',
ComputeGraspAriacState(joint_names=[
'right_elbow_joint', 'right_shoulder_lift_joint',
'right_shoulder_pan_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'move',
'failed': 'Compute grasp'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_R',
'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:279 y:232
OperatableStateMachine.add(
'cam',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Compute grasp',
'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_R',
'pose': 'pose'
})
# x:277 y:429
OperatableStateMachine.add('move',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'grip',
'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:274 y:503
OperatableStateMachine.add('grip',
GripperControl(enable=True),
transitions={
'continue': 'MOve_2_2',
'failed': 'move',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'move_group_R'})
# x:293 y:68
OperatableStateMachine.add('MOve_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'cam',
'planning_failed': 'failed',
'control_failed': 'MOve_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',
'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:469 y:553
OperatableStateMachine.add('MOve_2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'MOve_2_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',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:30 y:102
OperatableStateMachine.add('Find',
FindPart(time_out=0.2),
transitions={
'found': 'MOve_2',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'found': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'part_type': 'part_type_R',
'gantry_pos': 'gantry_pos',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame'
})
return _state_machine
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:52 y:611, x:343 y:359
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.agv_id = 'agv1'
_state_machine.userdata.part_type = 'gear_part'
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.pose = []
_state_machine.userdata.part_pose = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.offset = 0.1
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.config_name_bin3PreGrasp = 'bin3PreGrasp'
_state_machine.userdata.config_name_tray1PreDrop = 'tray1PreDrop'
_state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_5'
_state_machine.userdata.camera_frame = 'logical_camera_5_frame'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.agv_pose = []
_state_machine.userdata.part_offset = 0.035
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.conveyor_belt_power = 100.0
_state_machine.userdata.config_name_bin1PreGrasp = 'bin1PreGrasp'
_state_machine.userdata.arm_id = "arm1"
_state_machine.userdata.part_drop_offset = 0.1
_state_machine.userdata.StartText = 'Opdracht gestart'
_state_machine.userdata.StopText = 'Opdracht gestopt'
_state_machine.userdata.Shipments = []
_state_machine.userdata.material_locations = []
_state_machine.userdata.NumberOfShipments = 0
_state_machine.userdata.OrderId = ''
_state_machine.userdata.Products = []
_state_machine.userdata.NumberOfProducts = 0
_state_machine.userdata.MaterialsLocationList = []
_state_machine.userdata.config_name_bin3PreDrop = 'bin3PreDrop'
_state_machine.userdata.part = 'gasket_part'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:54 y:102
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'ConveyorBelt'},
autonomy={'continue': Autonomy.Off})
# x:197 y:603
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:654 y:113
OperatableStateMachine.add('MoveR1Home',
SrdfStateToMoveitAriac(),
transitions={'reached': 'DetectCameraPart', 'planning_failed': 'WaitRetry1', 'control_failed': 'WaitRetry1', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_home', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:666 y:12
OperatableStateMachine.add('WaitRetry1',
WaitState(wait_time=5),
transitions={'done': 'MoveR1Home'},
autonomy={'done': Autonomy.Off})
# x:836 y:111
OperatableStateMachine.add('DetectCameraPart',
DetectPartCameraAriacState(time_out=5.0),
transitions={'continue': 'MoveR1PreGrasp1', 'failed': 'failed', 'not_found': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'camera_ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part', 'pose': 'pose'})
# x:1039 y:116
OperatableStateMachine.add('MoveR1PreGrasp1',
SrdfStateToMoveitAriac(),
transitions={'reached': 'ComputePick', 'planning_failed': 'WaitRetry2', 'control_failed': 'WaitRetry2', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_bin1PreGrasp', 'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1048 y:21
OperatableStateMachine.add('WaitRetry2',
WaitState(wait_time=5),
transitions={'done': 'MoveR1PreGrasp1'},
autonomy={'done': Autonomy.Off})
# x:1038 y:200
OperatableStateMachine.add('ComputePick',
ComputeGraspAriacState(joint_names=['linear_arm_actuator_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']),
transitions={'continue': 'MoveR1ToPick1', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'move_group': 'move_group', 'move_group_prefix': 'move_group_prefix', 'tool_link': 'tool_link', 'pose': 'pose', 'offset': 'part_offset', 'rotation': 'part_rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1069 y:379
OperatableStateMachine.add('Wait',
WaitState(wait_time=1),
transitions={'done': 'transport_part_from_bin_to_agv_1'},
autonomy={'done': Autonomy.Off})
# x:1303 y:280
OperatableStateMachine.add('WaitRetry3',
WaitState(wait_time=5),
transitions={'done': 'MoveR1ToPick1'},
autonomy={'done': Autonomy.Off})
# x:1035 y:284
OperatableStateMachine.add('MoveR1ToPick1',
MoveitToJointsDynAriacState(),
transitions={'reached': 'EnableGripper', 'planning_failed': 'WaitRetry3', 'control_failed': 'EnableGripper'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'move_group_prefix': 'move_group_prefix', 'move_group': 'move_group', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:818 y:282
OperatableStateMachine.add('EnableGripper',
VacuumGripperControlState(enable=True),
transitions={'continue': 'Wait', 'failed': 'failed', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_id'})
# x:294 y:484
OperatableStateMachine.add('DisableGripper',
VacuumGripperControlState(enable=False),
transitions={'continue': 'EndAssignment', 'failed': 'failed', 'invalid_arm_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_arm_id': Autonomy.Off},
remapping={'arm_id': 'arm_id'})
# x:975 y:541
OperatableStateMachine.add('transport_part_from_bin_to_agv_1',
self.use_behavior(transport_part_from_bin_to_agv_1SM, 'transport_part_from_bin_to_agv_1'),
transitions={'finished': 'DisableGripper', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:222 y:106
OperatableStateMachine.add('ConveyorBelt',
SetConveyorbeltPowerState(),
transitions={'continue': 'MoveR1Home', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'conveyor_belt_power'})
return _state_machine
def create(self):
# x:1181 y:653, 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'])
_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.pose_on_agv = []
# 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:1076 y:319
OperatableStateMachine.add('MoveToTray',
SrdfStateToMoveitAriac(),
transitions={'reached': 'GripperDisabled', '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': 'failed', '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': '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:1060 y:407
OperatableStateMachine.add('GripperDisabled',
GripperControl(enable=False),
transitions={'continue': 'transport_part_form_bin_to_agv_state', 'failed': 'failed', 'invalid_id': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'invalid_id': Autonomy.Off},
remapping={'arm_id': 'arm_id'})
# x:852 y:488
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': 'MoveToHome_3', '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:971 y:604
OperatableStateMachine.add('MoveToHome_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_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'})
return _state_machine
def create(self):
# x:944 y:732, x:764 y:355
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.part_type = 'gear_part_red'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.offset = 0.025
_state_machine.userdata.move_group_G = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.config_name_home = 'home'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_bins1'
_state_machine.userdata.camera_frame = 'logical_camera_bins1_frame'
_state_machine.userdata.tool_link_R = 'right_ee_link'
_state_machine.userdata.arm_id = 'Left_Arm'
_state_machine.userdata.config_name_Gantry_pick = 'Gantry_Bins_LinksBovenRechts'
_state_machine.userdata.null = 0
_state_machine.userdata.config_name_Gantry_Transportband = 'Gantry_Transportband'
_state_machine.userdata.config_name_Gantry_AGV = 'Gantry_AGV_Rechts_L'
_state_machine.userdata.config_name_Left_home = 'Left_Home'
_state_machine.userdata.config_name_Right_home = 'Right_Home'
_state_machine.userdata.move_group_L = 'Left_Arm'
_state_machine.userdata.move_group_R = 'Right_Arm'
_state_machine.userdata.config_name_L = 'Links_PreDrop_Stelling'
_state_machine.userdata.config_name_R = 'Rechts_PreDrop_Stelling'
_state_machine.userdata.tool_link_L = 'left_ee_link'
_state_machine.userdata.rotation = 0
_state_machine.userdata.camera_ref_frame = 'world'
_state_machine.userdata.config_name_PrePick = 'Links_PreGrasp_Bins'
_state_machine.userdata.config_name_PreDrop = 'Links_PreDrop_AGV'
_state_machine.userdata.pose = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:73 y:57
OperatableStateMachine.add('StartAssignment',
StartAssignment(),
transitions={'continue': 'stop belt'},
autonomy={'continue': Autonomy.Off})
# x:1574 y:26
OperatableStateMachine.add('Pregrasp_G',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PrePick', 'planning_failed': 'failed', 'control_failed': 'PrePick', '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_pick', '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:261 y:30
OperatableStateMachine.add('stop belt',
SetConveyorbeltPowerState(),
transitions={'continue': 'PreLeftHome', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'power': 'null'})
# x:988 y:18
OperatableStateMachine.add('LeftHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'RightHome', 'planning_failed': 'failed', 'control_failed': 'RightHome', '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_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:1259 y:20
OperatableStateMachine.add('RightHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'Pregrasp_G', 'planning_failed': 'failed', 'control_failed': 'Pregrasp_G', '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_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:522 y:12
OperatableStateMachine.add('PreLeftHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'PreRightHome', 'planning_failed': 'failed', 'control_failed': 'PreRightHome', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_L', '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:759 y:14
OperatableStateMachine.add('PreRightHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'LeftHome', 'planning_failed': 'failed', 'control_failed': 'LeftHome', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_R', '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:1566 y:263
OperatableStateMachine.add('CheckPartPoseBin',
DetectPartCameraAriacState(time_out=2),
transitions={'continue': 'ComputePick', 'failed': 'failed', 'not_found': 'Retry'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off, 'not_found': Autonomy.Off},
remapping={'ref_frame': 'camera_ref_frame', 'camera_topic': 'camera_topic', 'camera_frame': 'camera_frame', 'part': 'part_type', 'pose': 'pose'})
# x:1579 y:349
OperatableStateMachine.add('ComputePick',
ComputeGraspAriacState(joint_names=['left_shoulder_pan_joint', 'left_shoulder_lift_joint', 'left_elbow_joint', 'left_wrist_1_joint', 'left_wrist_2_joint', 'left_wrist_3_joint']),
transitions={'continue': 'Pick', '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', 'pose': 'pose', 'offset': 'offset', 'rotation': 'rotation', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:1326 y:337
OperatableStateMachine.add('Retry',
WaitState(wait_time=0.2),
transitions={'done': 'PrePick'},
autonomy={'done': Autonomy.Off})
# x:1579 y:157
OperatableStateMachine.add('PrePick',
SrdfStateToMoveitAriac(),
transitions={'reached': 'CheckPartPoseBin', 'planning_failed': 'CheckPartPoseBin', '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_PrePick', '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:1329 y:732
OperatableStateMachine.add('LeftHome_2',
SrdfStateToMoveitAriac(),
transitions={'reached': 'finished', 'planning_failed': 'failed', 'control_failed': 'finished', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name_Left_home', '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:1602 y:444
OperatableStateMachine.add('Pick',
MoveitToJointsDynAriacState(),
transitions={'reached': 'GripperEnable', 'planning_failed': 'failed', 'control_failed': 'GripperEnable'},
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:1597 y:703
OperatableStateMachine.add('PrePick_2',
SrdfStateToMoveitAriac(),
transitions={'reached': 'LeftHome_2', 'planning_failed': 'failed', 'control_failed': 'LeftHome_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_PrePick', '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:1582 y:575
OperatableStateMachine.add('GripperEnable',
VacuumGripperControlState(enable=True),
transitions={'continue': 'PrePick_2', 'failed': 'Retry', '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: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:1374 y:114, x:1202 y:412
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'ARMidMAIN', 'PartPose', 'PartOffset', 'exactparttype',
'PartTYPE'
],
output_keys=['RightArmItem', 'LeftArmItem'])
_state_machine.userdata.ARMidMAIN = ''
_state_machine.userdata.arm_idR = 'right_arm'
_state_machine.userdata.move_groupR = 'Right_Arm'
_state_machine.userdata.move_groupL = 'Left_Arm'
_state_machine.userdata.tool_linkR = 'right_ee_link'
_state_machine.userdata.tool_linkL = 'left_ee_link'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.PartOffset = 0
_state_machine.userdata.PartPose = []
_state_machine.userdata.rotation = 0
_state_machine.userdata.arm_idL = 'left_arm'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.exactparttype = ''
_state_machine.userdata.RightArmItem = ''
_state_machine.userdata.LeftArmItem = ''
_state_machine.userdata.PartTYPE = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:232 y:59
OperatableStateMachine.add('UseRightArm?',
EqualState(),
transitions={
'true':
'ComputeBeltPickForRightArm',
'false': 'ComputeBeltPickForLeftArm'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'ARMidMAIN',
'value_b': 'arm_idR'
})
# x:484 y:63
OperatableStateMachine.add(
'ComputeBeltPickForRightArm',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'GoToPickRight',
'failed': 'WaitFailed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_groupR',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_linkR',
'pose': 'PartPose',
'offset': 'PartOffset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:484 y:155
OperatableStateMachine.add(
'ComputeBeltPickForLeftArm',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'GoToPickLeft',
'failed': 'WaitFailed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_groupL',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_linkL',
'pose': 'PartPose',
'offset': 'PartOffset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:701 y:57
OperatableStateMachine.add('GoToPickRight',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'TurnGripperONRight',
'planning_failed':
'TurnGripperONRight',
'control_failed':
'TurnGripperONRight'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_groupR',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:701 y:164
OperatableStateMachine.add('GoToPickLeft',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'TurnGripperONLeft',
'planning_failed':
'TurnGripperONLeft',
'control_failed':
'TurnGripperONLeft'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_groupL',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:874 y:58
OperatableStateMachine.add('TurnGripperONRight',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'SetRightArmItem',
'failed':
'ComputeBeltPickForRightArm',
'invalid_arm_id': 'WaitFailed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idR'})
# x:874 y:165
OperatableStateMachine.add('TurnGripperONLeft',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'SetLeftArmItem',
'failed':
'ComputeBeltPickForLeftArm',
'invalid_arm_id': 'WaitFailed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_idL'})
# x:848 y:478
OperatableStateMachine.add('WaitFailed',
WaitState(wait_time=2),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:1045 y:58
OperatableStateMachine.add('SetRightArmItem',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PartTYPE',
'result': 'RightArmItem'
})
# x:1041 y:163
OperatableStateMachine.add('SetLeftArmItem',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'PartTYPE',
'result': 'LeftArmItem'
})
return _state_machine
def create(self):
# x:1393 y:668, x:704 y:381
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['part_type', 'agv_id', 'pose_on_agv'])
_state_machine.userdata.part_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.move_group_prefix = '/ariac/arm1'
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.config_name_R1Bin1Pre = 'R1Bin1Pre'
_state_machine.userdata.config_name_R1AGVPre = 'R1AGVPre'
_state_machine.userdata.config_name_R2Bin5Pre = 'R2Bin5Pre'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.arm2_gebruiken = '/ariac/arm2'
_state_machine.userdata.part_pose = []
_state_machine.userdata.part = 'gasket_part'
_state_machine.userdata.part_offset = 0.030
_state_machine.userdata.part_drop_offset = 0.1
_state_machine.userdata.conveyor_power = 100.0
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.camera_frame = 'logical_camera_3_frame'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_3'
_state_machine.userdata.camera_ref_frame = 'arm1_linear_arm_actuator'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.bin_location = []
_state_machine.userdata.material_locations = []
_state_machine.userdata.zero_value = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:12 y:198
OperatableStateMachine.add(
'get part location',
GetMaterialLocationsState(),
transitions={'continue': 'bin location'},
autonomy={'continue': Autonomy.Off},
remapping={
'part': 'part_type',
'material_locations': 'material_locations'
})
# x:305 y:37
OperatableStateMachine.add(
'MoseMessage',
MessageState(),
transitions={'continue': 'detect object'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'pose_on_agv'})
# x:171 y:37
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'MoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:681 y:38
OperatableStateMachine.add('pre grasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'compute grasp',
'planning_failed': 'failed',
'control_failed': 'compute grasp',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R1Bin1Pre',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1433 y:179
OperatableStateMachine.add('move_agv',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'gripper uit',
'planning_failed': 'failed',
'control_failed': 'gripper uit',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R1AGVPre',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1255 y:51
OperatableStateMachine.add('gripper aan',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'wait',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1405 y:434
OperatableStateMachine.add('robot_2_selecteren',
ReplaceState(),
transitions={'done': 'robot_2_bewegen'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'arm2_gebruiken',
'result': 'move_group_prefix'
})
# x:1424 y:526
OperatableStateMachine.add('robot_2_bewegen',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_R2Bin5Pre',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:465 y:35
OperatableStateMachine.add(
'detect object',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'pre grasp',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part',
'pose': 'pose'
})
# x:862 y:38
OperatableStateMachine.add(
'compute grasp',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'pick_positie',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1475 y:50
OperatableStateMachine.add('wait',
WaitState(wait_time=1),
transitions={'done': 'move_agv'},
autonomy={'done': Autonomy.Off})
# x:1414 y:297
OperatableStateMachine.add('gripper uit',
VacuumGripperControlState(enable=False),
transitions={
'continue': 'robot_2_selecteren',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1047 y:40
OperatableStateMachine.add('pick_positie',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'gripper aan',
'planning_failed': 'failed',
'control_failed': 'gripper aan'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:150 y:300
OperatableStateMachine.add('bin location',
GetItemFromListState(),
transitions={
'done': 'AgvIdMessage',
'invalid_index': 'failed'
},
autonomy={
'done': Autonomy.Off,
'invalid_index': Autonomy.Off
},
remapping={
'list': 'material_locations',
'index': 'zero_value',
'item': 'bin_location'
})
# x:176 y:126
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
return _state_machine
def create(self):
# x:1770 y:298, x:81 y:372
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'part_type', 'agv_id', 'pose_on_agv', 'camera_ref_frame',
'camera_frame', 'camera_topic'
],
output_keys=['part_type', 'camera_ref_frame', 'camera_frame'])
_state_machine.userdata.part_type = ''
_state_machine.userdata.agv_id = ''
_state_machine.userdata.move_group_prefix1 = '/ariac/arm1'
_state_machine.userdata.move_group_prefix2 = '/ariac/arm2'
_state_machine.userdata.move_group = 'manipulator'
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.part_pose = []
_state_machine.userdata.joint_values = []
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.pose_on_agv = []
_state_machine.userdata.config_name_bin1PreGrasp = 'bin1PreGrasp'
_state_machine.userdata.camera_ref_frame2 = 'arm2_linear_arm_actuator'
_state_machine.userdata.camera_ref_frame1 = 'arm1_linear_arm_actuator'
_state_machine.userdata.camera_frame2 = 'Camera_bin_2_frame'
_state_machine.userdata.camera_frame5 = 'Camera_bin_5_frame'
_state_machine.userdata.camera_frame6 = 'Camera_bin_6_frame'
_state_machine.userdata.config_name_bin2PreGrasp = 'bin2PreGrasp'
_state_machine.userdata.config_name_bin5PreGrasp = 'bin5PreGrasp'
_state_machine.userdata.camera_ref_frame = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.camera_frame1 = 'Camera_bin_1_frame'
_state_machine.userdata.config_name_bin6PreGrasp = 'bin6PreGrasp'
_state_machine.userdata.joint_names = ''
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.gripper1 = 'arm1'
_state_machine.userdata.gripper2 = 'arm2'
_state_machine.userdata.camera_topic = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:71 y:54
OperatableStateMachine.add('binSelectie',
self.use_behavior(
binSelectieSM, 'binSelectie'),
transitions={
'finished': 'arm1',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'part_type': 'part_type',
'camera_topic': 'camera_topic',
'camera_ref_frame':
'camera_ref_frame',
'camera_frame': 'camera_frame',
'part_offset': 'part_offset'
})
# x:1675 y:697
OperatableStateMachine.add('PartTypeMessage',
MessageState(),
transitions={'continue': 'MoseMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_type'})
# x:1675 y:758
OperatableStateMachine.add(
'AgvIdMessage',
MessageState(),
transitions={'continue': 'PartTypeMessage'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'agv_id'})
# x:262 y:58
OperatableStateMachine.add('arm1',
EqualState(),
transitions={
'true': 'bin5',
'false': 'arm2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_ref_frame',
'value_b': 'camera_ref_frame1'
})
# x:266 y:389
OperatableStateMachine.add('arm2',
EqualState(),
transitions={
'true': 'bin1',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_ref_frame',
'value_b': 'camera_ref_frame2'
})
# x:470 y:353
OperatableStateMachine.add('bin1',
EqualState(),
transitions={
'true': 'pregraspbin1',
'false': 'bin2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame1'
})
# x:475 y:527
OperatableStateMachine.add('bin2',
EqualState(),
transitions={
'true': 'pregraspbin2',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame2'
})
# x:467 y:59
OperatableStateMachine.add('bin5',
EqualState(),
transitions={
'true': 'pregraspbin5',
'false': 'bin6'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame5'
})
# x:470 y:277
OperatableStateMachine.add('bin6',
EqualState(),
transitions={
'true': 'pregraspbin6',
'false': 'failed'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'camera_frame',
'value_b': 'camera_frame6'
})
# x:663 y:58
OperatableStateMachine.add('pregraspbin5',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin5',
'planning_failed': 'failed',
'control_failed': 'detectpartbin5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin5PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:652 y:273
OperatableStateMachine.add('pregraspbin6',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin6',
'planning_failed': 'failed',
'control_failed': 'detectpartbin6',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin6PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:653 y:351
OperatableStateMachine.add('pregraspbin1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin1',
'planning_failed': 'failed',
'control_failed': 'detectpartbin1',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:657 y:531
OperatableStateMachine.add('pregraspbin2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'detectpartbin2',
'planning_failed': 'failed',
'control_failed': 'detectpartbin2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin2PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1164 y:44
OperatableStateMachine.add('pickbin5',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'gripperkeuze',
'planning_failed': 'failed',
'control_failed': 'Retry'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix1',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1173 y:269
OperatableStateMachine.add('pickbin6',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripperaan_2',
'planning_failed': 'failed',
'control_failed': 'Retry_2'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix1',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1171 y:353
OperatableStateMachine.add('pickbin1',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripperaan_4',
'planning_failed': 'failed',
'control_failed': 'Retry_3'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix2',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1185 y:524
OperatableStateMachine.add('pickbin2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripperaan_3',
'planning_failed': 'failed',
'control_failed': 'Retry_4'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix2',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1568 y:43
OperatableStateMachine.add('pregraspbin5_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin5PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1568 y:262
OperatableStateMachine.add('pregraspbin6_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin5PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix1',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1566 y:349
OperatableStateMachine.add('pregraspbin1_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin1PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1569 y:515
OperatableStateMachine.add('pregraspbin2_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'finished',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_bin2PreGrasp',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix2',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1674 y:635
OperatableStateMachine.add('MoseMessage',
MessageState(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_pose'})
# x:818 y:46
OperatableStateMachine.add(
'detectpartbin5',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:1193 y:136
OperatableStateMachine.add('Retry',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze'},
autonomy={'done': Autonomy.Off})
# x:820 y:265
OperatableStateMachine.add(
'detectpartbin6',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5_2',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:820 y:348
OperatableStateMachine.add(
'detectpartbin1',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5_3',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:833 y:530
OperatableStateMachine.add(
'detectpartbin2',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Computegraspbin5_4',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'part_pose'
})
# x:997 y:44
OperatableStateMachine.add(
'Computegraspbin5',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1382 y:37
OperatableStateMachine.add('Gripperaan',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin5_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1363 y:129
OperatableStateMachine.add('gripperkeuze',
ReplaceState(),
transitions={'done': 'Gripperaan'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper1',
'result': 'arm_id'
})
# x:1000 y:254
OperatableStateMachine.add(
'Computegraspbin5_2',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1003 y:346
OperatableStateMachine.add(
'Computegraspbin5_3',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5_3',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1023 y:524
OperatableStateMachine.add(
'Computegraspbin5_4',
ComputeGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'Computegraspbin5_4',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix1',
'tool_link': 'tool_link',
'pose': 'part_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1193 y:198
OperatableStateMachine.add('Retry_2',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze_2'},
autonomy={'done': Autonomy.Off})
# x:1206 y:434
OperatableStateMachine.add('Retry_3',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze_3'},
autonomy={'done': Autonomy.Off})
# x:1200 y:628
OperatableStateMachine.add('Retry_4',
WaitState(wait_time=1),
transitions={'done': 'gripperkeuze_4'},
autonomy={'done': Autonomy.Off})
# x:1362 y:199
OperatableStateMachine.add('gripperkeuze_2',
ReplaceState(),
transitions={'done': 'Gripperaan_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper1',
'result': 'arm_id'
})
# x:1359 y:426
OperatableStateMachine.add('gripperkeuze_3',
ReplaceState(),
transitions={'done': 'Gripperaan_4'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper2',
'result': 'arm_id'
})
# x:1357 y:642
OperatableStateMachine.add('gripperkeuze_4',
ReplaceState(),
transitions={'done': 'Gripperaan_3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'gripper2',
'result': 'arm_id'
})
# x:1359 y:261
OperatableStateMachine.add('Gripperaan_2',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin6_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1377 y:511
OperatableStateMachine.add('Gripperaan_3',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin2_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1362 y:345
OperatableStateMachine.add('Gripperaan_4',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'pregraspbin1_2',
'failed': 'failed',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
return _state_machine
def create(self):
# x:92 y:307, x:966 y:441
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'offset_part_left', 'PreGrasp', 'camera_topic', 'camera_frame',
'offset_part_left', 'part_type', 'arm_id_left', 'part_pose'
],
output_keys=['part_pose_left'])
_state_machine.userdata.move_group_gantry = 'Gantry'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.move_group_right_arm = 'Right_Arm'
_state_machine.userdata.move_group_left_arm = 'Left_Arm'
_state_machine.userdata.config_name_pre_shelves = 'Gantry_PreGrasp_Pre_shelves'
_state_machine.userdata.PreGrasp = ''
_state_machine.userdata.camera_ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.offset_part_left = 0
_state_machine.userdata.rotation = 0
_state_machine.userdata.tool_link_left = 'left_ee_link'
_state_machine.userdata.arm_id_left = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.part_pose_left = []
_state_machine.userdata.config_name_left_arm2 = 'Left_Home_shelves'
_state_machine.userdata.config_name_shelves = 'Gantry_PreGrasp_shelves'
_state_machine.userdata.config_name_right_arm2 = 'Right_Home_shelves'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:745 y:152
OperatableStateMachine.add('PreShelf',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectPart',
'planning_failed': 'Retry_4',
'control_failed': 'Retry_4',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_pre_shelves',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:773 y:27
OperatableStateMachine.add('Retry_4',
WaitState(wait_time=0.3),
transitions={'done': 'PreShelf'},
autonomy={'done': Autonomy.Off})
# x:1558 y:259
OperatableStateMachine.add('PreGrasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePick',
'planning_failed': 'Retry_5',
'control_failed': 'Retry_5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'PreGrasp',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1730 y:262
OperatableStateMachine.add('Retry_5',
WaitState(wait_time=0.3),
transitions={'done': 'PreGrasp'},
autonomy={'done': Autonomy.Off})
# x:1548 y:361
OperatableStateMachine.add(
'ComputePick',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'Move_to_pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group_left_arm',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link_left',
'pose': 'pose',
'offset': 'offset_part_left',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:918 y:149
OperatableStateMachine.add(
'DetectPart',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'Move_left_arm_home2',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'camera_ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part_type',
'pose': 'pose'
})
# x:1734 y:461
OperatableStateMachine.add('Retry_6',
WaitState(wait_time=0.3),
transitions={'done': 'Move_to_pick'},
autonomy={'done': Autonomy.Off})
# x:1533 y:463
OperatableStateMachine.add('Move_to_pick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperOn',
'planning_failed': 'Retry_6',
'control_failed': 'GripperOn'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_left_arm',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1535 y:573
OperatableStateMachine.add('GripperOn',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'Move_left_arm_home_2',
'failed': 'ComputePick',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_left'})
# x:1378 y:653
OperatableStateMachine.add('Move_left_arm_home_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Shelves_2',
'planning_failed': 'Retry_7',
'control_failed': 'Retry_7',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_left_arm2',
'move_group': 'move_group_left_arm',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1426 y:747
OperatableStateMachine.add(
'Retry_7',
WaitState(wait_time=0.3),
transitions={'done': 'Move_left_arm_home_2'},
autonomy={'done': Autonomy.Off})
# x:867 y:641
OperatableStateMachine.add('PreShelf_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Pose',
'planning_failed': 'Retry_8',
'control_failed': 'Retry_8',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_pre_shelves',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:889 y:738
OperatableStateMachine.add('Retry_8',
WaitState(wait_time=0.3),
transitions={'done': 'PreShelf_2'},
autonomy={'done': Autonomy.Off})
# x:127 y:421
OperatableStateMachine.add('Pose',
ReplaceState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'part_pose',
'result': 'part_pose_left'
})
# x:1135 y:148
OperatableStateMachine.add('Move_left_arm_home2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Move_right_arm_home2',
'planning_failed': 'Retry_9',
'control_failed': 'Retry_9',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_left_arm2',
'move_group': 'move_group_left_arm',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1159 y:31
OperatableStateMachine.add(
'Retry_9',
WaitState(wait_time=0.3),
transitions={'done': 'Move_left_arm_home2'},
autonomy={'done': Autonomy.Off})
# x:1346 y:147
OperatableStateMachine.add(
'Move_right_arm_home2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Shelves',
'planning_failed': 'Retry_10',
'control_failed': 'Retry_10',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_right_arm2',
'move_group': 'move_group_right_arm',
'move_group_prefix': 'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out': 'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out': 'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1381 y:35
OperatableStateMachine.add(
'Retry_10',
WaitState(wait_time=0.3),
transitions={'done': 'Move_right_arm_home2'},
autonomy={'done': Autonomy.Off})
# x:1561 y:148
OperatableStateMachine.add('Shelves',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreGrasp',
'planning_failed': 'Retry_11',
'control_failed': 'Retry_11',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_shelves',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1591 y:35
OperatableStateMachine.add('Retry_11',
WaitState(wait_time=0.3),
transitions={'done': 'Shelves'},
autonomy={'done': Autonomy.Off})
# x:1063 y:639
OperatableStateMachine.add('Shelves_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreShelf_2',
'planning_failed': 'Retry_12',
'control_failed': 'Retry_12',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'config_name_shelves',
'move_group': 'move_group_gantry',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1074 y:746
OperatableStateMachine.add('Retry_12',
WaitState(wait_time=0.3),
transitions={'done': 'Shelves_2'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:982 y:568, x:604 y:185
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['part', 'arm_id'])
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group = ''
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.arm_id = 'Right_Arm'
_state_machine.userdata.tool_link = ''
_state_machine.userdata.pose = []
_state_machine.userdata.offset = 0.15
_state_machine.userdata.rotation = 0
_state_machine.userdata.part = 'gear_part_red'
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.home = 'Home'
_state_machine.userdata.safe = 'Full_Bin'
_state_machine.userdata.Up = 0.2
_state_machine.userdata.preShelf = 'Gantry_PreShelf'
_state_machine.userdata.shelf = 'Full_Shelf'
_state_machine.userdata.one = 1
_state_machine.userdata.bin = 0
_state_machine.userdata.zero = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:118 y:24
OperatableStateMachine.add('FindPart',
FindPart(time_out=0.2),
transitions={
'bin': 'BinTo1',
'shelf': 'BinTo0',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'bin': Autonomy.Off,
'shelf': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'part_type': 'part',
'arm_id': 'arm_id',
'gantry_pos': 'config_name',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame'
})
# x:839 y:25
OperatableStateMachine.add('MoveToPart',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'EnableGripper',
'planning_failed': 'failed',
'control_failed': 'EnableGripper'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1048 y:25
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={
'continue': 'OffsetUp',
'failed': 'MoveToPart',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:167 y:236
OperatableStateMachine.add('SafePosBetweenBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetPartPose',
'planning_failed': 'failed',
'control_failed':
'SafePosBetweenBin',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'safe',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:107 y:400
OperatableStateMachine.add('PreGrasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePick_3',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:361 y:25
OperatableStateMachine.add('Offset',
part_offsetCalc(),
transitions={
'succes': 'ComputePick',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part',
'part_offset': 'offset'
})
# x:1044 y:400
OperatableStateMachine.add('ArmHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'SafePosBetweenBin_2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'home',
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1038 y:475
OperatableStateMachine.add('SafePosBetweenBin_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed':
'SafePosBetweenBin_2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'safe',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1025 y:89
OperatableStateMachine.add('OffsetUp',
ReplaceState(),
transitions={'done': 'ComputePick_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Up',
'result': 'offset'
})
# x:970 y:155
OperatableStateMachine.add('ComputePick_2',
ComputeGraspAriacState(),
transitions={
'continue': 'MoveToPart_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:938 y:244
OperatableStateMachine.add('MoveToPart_2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Bin1?',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:103 y:469
OperatableStateMachine.add('ComputePick_3',
ComputeGraspAriacState(),
transitions={
'continue': 'MoveToPart_3',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:95 y:540
OperatableStateMachine.add('MoveToPart_3',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Offset',
'planning_failed': 'MoveToPart_3',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:647 y:24
OperatableStateMachine.add('ComputePick',
ComputeGraspAriacState(),
transitions={
'continue': 'MoveToPart',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:0 y:171
OperatableStateMachine.add('ToShelf',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ToShelf1',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'preShelf',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:0 y:233
OperatableStateMachine.add('ToShelf1',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetPartPose',
'planning_failed': 'failed',
'control_failed': 'ToShelf1',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'shelf',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:936 y:329
OperatableStateMachine.add('Bin1?',
EqualState(),
transitions={
'true': 'ArmHome_2',
'false': 'ToShelf1_2'
},
autonomy={
'true': Autonomy.Off,
'false': Autonomy.Off
},
remapping={
'value_a': 'bin',
'value_b': 'one'
})
# x:160 y:170
OperatableStateMachine.add(
'BinTo1',
ReplaceState(),
transitions={'done': 'SafePosBetweenBin'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'one',
'result': 'bin'
})
# x:830 y:467
OperatableStateMachine.add('ToShelf_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'preShelf',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:826 y:396
OperatableStateMachine.add('ToShelf1_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ToShelf_2',
'planning_failed': 'failed',
'control_failed': 'ToShelf1_2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'shelf',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:96 y:330
OperatableStateMachine.add(
'GetPartPose',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'PreGrasp',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part',
'pose': 'pose'
})
# x:0 y:113
OperatableStateMachine.add('BinTo0',
ReplaceState(),
transitions={'done': 'ToShelf'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'zero',
'result': 'bin'
})
return _state_machine
def create(self):
# x: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: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:894 y:460, x:898 y:340
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'bin_id', 'camera_topic', 'camera_frame', 'part_type',
'PreGraspGantry', 'offset'
])
_state_machine.userdata.bin_id = ''
_state_machine.userdata.home_gantry_id = 'Gantry_Home'
_state_machine.userdata.home_rightarm_id = 'Right_PreGrasp'
_state_machine.userdata.home_leftarm_id = 'Left_PreGrasp'
_state_machine.userdata.move_group_right = 'Right_Arm'
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.move_group_left = 'Left_Arm'
_state_machine.userdata.move_group_gantry = 'Gantry'
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.part_type = ''
_state_machine.userdata.PreGraspGantry = ''
_state_machine.userdata.SafePosition = 'Gantry_ShelfSAFE'
_state_machine.userdata.tool_link = 'left_ee_link'
_state_machine.userdata.rotation = 0
_state_machine.userdata.offset = ''
_state_machine.userdata.arm_id = 'left_arm'
_state_machine.userdata.PreGrasp_RightArm = 'Right_Shelf'
_state_machine.userdata.PreGrasp_LeftArm = 'Left_Shelf'
_state_machine.userdata.SafePositionConveyor = 'Gantry_Shelf_Conveyor'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:29 y:124
OperatableStateMachine.add('HomepositieRightArm',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'HomepositieLeftArm',
'planning_failed': 'Retry_1',
'control_failed': 'Retry_1',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'home_rightarm_id',
'move_group': 'move_group_right',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:213 y:119
OperatableStateMachine.add('HomepositieLeftArm',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'DetectPart',
'planning_failed': 'Retry_2',
'control_failed': 'Retry_2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'home_leftarm_id',
'move_group': 'move_group_left',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:46 y:34
OperatableStateMachine.add(
'Retry_1',
WaitState(wait_time=3),
transitions={'done': 'HomepositieRightArm'},
autonomy={'done': Autonomy.Off})
# x:230 y:33
OperatableStateMachine.add(
'Retry_2',
WaitState(wait_time=3),
transitions={'done': 'HomepositieLeftArm'},
autonomy={'done': Autonomy.Off})
# x:392 y:114
OperatableStateMachine.add(
'DetectPart',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'SafePosition_Conveyor',
'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': 'part_pose'
})
# x:1279 y:116
OperatableStateMachine.add('PreGraspGantry',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePick',
'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': 'PreGraspGantry',
'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:1304 y:30
OperatableStateMachine.add('Retry_6',
WaitState(wait_time=3),
transitions={'done': 'PreGraspGantry'},
autonomy={'done': Autonomy.Off})
# x:736 y:114
OperatableStateMachine.add('SafePosition',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreGrasp_RightArm',
'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': 'SafePosition',
'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:763 y:21
OperatableStateMachine.add('Retry_3',
WaitState(wait_time=3),
transitions={'done': 'SafePosition'},
autonomy={'done': Autonomy.Off})
# x:1463 y:117
OperatableStateMachine.add(
'ComputePick',
ComputeGraspAriacState(joint_names=[
'left_shoulder_pan_joint', 'left_shoulder_lift_joint',
'left_elbow_joint', 'left_wrist_1_joint',
'left_wrist_2_joint', 'left_wrist_3_joint'
]),
transitions={
'continue': 'MoveToPick',
'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',
'pose': 'part_pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1645 y:117
OperatableStateMachine.add('MoveToPick',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperOn',
'planning_failed': 'Retry_7',
'control_failed': 'GripperOn'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'move_group_left',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1679 y:35
OperatableStateMachine.add('Retry_7',
WaitState(wait_time=3),
transitions={'done': 'MoveToPick'},
autonomy={'done': Autonomy.Off})
# x:1465 y:242
OperatableStateMachine.add('GripperOn',
VacuumGripperControlState(enable=True),
transitions={
'continue': 'PreGraspLeftArm',
'failed': 'ComputePick',
'invalid_arm_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_arm_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1480 y:333
OperatableStateMachine.add('PreGraspLeftArm',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'SafePosition_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:1697 y:328
OperatableStateMachine.add('Retry_8',
WaitState(wait_time=3),
transitions={'done': 'PreGraspLeftArm'},
autonomy={'done': Autonomy.Off})
# x:1479 y:426
OperatableStateMachine.add('SafePosition_2',
SrdfStateToMoveitAriac(),
transitions={
'reached':
'SafePosition_Conveyor_2',
'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': 'SafePosition',
'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:1700 y:427
OperatableStateMachine.add('Retry_9',
WaitState(wait_time=3),
transitions={'done': 'SafePosition_2'},
autonomy={'done': Autonomy.Off})
# x:924 y:115
OperatableStateMachine.add('PreGrasp_RightArm',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreGrasp_LeftArm',
'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:1103 y:113
OperatableStateMachine.add('PreGrasp_LeftArm',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'PreGraspGantry',
'planning_failed': 'Retry_5',
'control_failed': 'Retry_5',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'PreGrasp_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:948 y:23
OperatableStateMachine.add(
'Retry_4',
WaitState(wait_time=3),
transitions={'done': 'PreGrasp_RightArm'},
autonomy={'done': Autonomy.Off})
# x:1126 y:21
OperatableStateMachine.add(
'Retry_5',
WaitState(wait_time=3),
transitions={'done': 'PreGrasp_LeftArm'},
autonomy={'done': Autonomy.Off})
# x:1479 y:517
OperatableStateMachine.add('SafePosition_Conveyor_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'Retry_10',
'control_failed': 'Retry_10',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name':
'SafePositionConveyor',
'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:1700 y:541
OperatableStateMachine.add(
'Retry_10',
WaitState(wait_time=3),
transitions={'done': 'SafePosition_Conveyor_2'},
autonomy={'done': Autonomy.Off})
# x:576 y:110
OperatableStateMachine.add('SafePosition_Conveyor',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'SafePosition',
'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':
'SafePositionConveyor',
'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:588 y:23
OperatableStateMachine.add(
'Retry_3_2',
WaitState(wait_time=3),
transitions={'done': 'SafePosition_Conveyor'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:1184 y:332, x:604 y:185
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['part', 'arm_id'])
_state_machine.userdata.config_name = ''
_state_machine.userdata.move_group = ''
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.arm_id = 'Right_Arm'
_state_machine.userdata.tool_link = 'right_ee_link'
_state_machine.userdata.pose = []
_state_machine.userdata.offset = 0.15
_state_machine.userdata.rotation = 0
_state_machine.userdata.part = 'gear_part_red'
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = ''
_state_machine.userdata.camera_frame = ''
_state_machine.userdata.home = 'Home'
_state_machine.userdata.safe = 'Gantry_Bin'
_state_machine.userdata.Up = 0.2
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:33 y:31
OperatableStateMachine.add('FindParts',
FindPart(time_out=0.2),
transitions={
'found': 'ArmHome',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'found': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'part_type': 'part',
'gantry_pos': 'config_name',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame'
})
# x:647 y:24
OperatableStateMachine.add(
'ComputePick',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'MoveToPart',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:16 y:242
OperatableStateMachine.add(
'GetPartPose',
DetectPartCameraAriacState(time_out=0.5),
transitions={
'continue': 'PreGrasp',
'failed': 'failed',
'not_found': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'not_found': Autonomy.Off
},
remapping={
'ref_frame': 'ref_frame',
'camera_topic': 'camera_topic',
'camera_frame': 'camera_frame',
'part': 'part',
'pose': 'pose'
})
# x:839 y:25
OperatableStateMachine.add('MoveToPart',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'EnableGripper',
'planning_failed': 'failed',
'control_failed': 'EnableGripper'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1048 y:25
OperatableStateMachine.add('EnableGripper',
GripperControl(enable=True),
transitions={
'continue': 'OffsetUp',
'failed': 'MoveToPart',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:27 y:178
OperatableStateMachine.add('SafePosBetweenBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetPartPose',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'safe',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:23 y:320
OperatableStateMachine.add('PreGrasp',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputePick_3',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:25 y:100
OperatableStateMachine.add('ArmHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'SafePosBetweenBin',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'home',
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:361 y:25
OperatableStateMachine.add('Offset',
part_offsetCalc(),
transitions={
'succes': 'ComputePick',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part',
'part_offset': 'offset'
})
# x:1041 y:361
OperatableStateMachine.add('ArmHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'SafePosBetweenBin_2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'home',
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1041 y:442
OperatableStateMachine.add('SafePosBetweenBin_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'safe',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1025 y:89
OperatableStateMachine.add('OffsetUp',
ReplaceState(),
transitions={'done': 'ComputePick_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'Up',
'result': 'offset'
})
# x:1034 y:188
OperatableStateMachine.add(
'ComputePick_2',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'MoveToPart_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1029 y:261
OperatableStateMachine.add('MoveToPart_2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'ArmHome_2',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:22 y:413
OperatableStateMachine.add(
'ComputePick_3',
ComputeGraspAriacState(joint_names=[
'right_shoulder_pan_joint', 'right_shoulder_lift_joint',
'right_elbow_joint', 'right_wrist_1_joint',
'right_wrist_2_joint', 'right_wrist_3_joint'
]),
transitions={
'continue': 'MoveToPart_3',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'pose': 'pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:331 y:410
OperatableStateMachine.add('MoveToPart_3',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Offset',
'planning_failed': 'MoveToPart_3',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
return _state_machine
