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:884 y:573, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group = ''
_state_machine.userdata.move_group_prefix = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = ''
_state_machine.userdata.arm_id = 'Left_Arm'
_state_machine.userdata.offset = 0.021
_state_machine.userdata.ref_frame = 'world'
_state_machine.userdata.camera_topic = '/ariac/logical_camera_stock1'
_state_machine.userdata.camera_frame = 'logical_camera_stock1_frame'
_state_machine.userdata.home = 'Gantry_Home'
_state_machine.userdata.part = 'piston_rod_part_red'
_state_machine.userdata.full_home = 'Full_Home'
_state_machine.userdata.clearance = 0.05
_state_machine.userdata.bin = 'Gantry_Bin_Red'
_state_machine.userdata.bin_pose = []
_state_machine.userdata.binOffset = -1
_state_machine.userdata.one = 1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:102 y:30
OperatableStateMachine.add('Move_Bin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'Get_Object_Place',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'bin',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:312 y:27
OperatableStateMachine.add('Compute_Place',
ComputeDropPart(),
transitions={
'continue': 'Move_To_Place',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'binOffset': 'binOffset',
'bin_pose': 'bin_pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:698 y:26
OperatableStateMachine.add('Gripper_Off',
GripperControl(enable=False),
transitions={
'continue': 'Add_Clearance',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:842 y:232
OperatableStateMachine.add('Move_Clear',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Move_Home',
'planning_failed': 'Move_Home',
'control_failed': 'Move_Clear'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:844 y:24
OperatableStateMachine.add('Add_Clearance',
AddNumericState(),
transitions={'done': 'Compute_Place_2'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'clearance',
'value_b': 'offset',
'result': 'offset'
})
# x:846 y:330
OperatableStateMachine.add('Move_Home',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'AddPart',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'full_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:482 y:20
OperatableStateMachine.add('Move_To_Place',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Gripper_Off',
'planning_failed': 'failed',
'control_failed': 'Gripper_Off'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix':
'move_group_prefix',
'move_group': 'arm_id',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:68 y:266
OperatableStateMachine.add('Get_Object_Place',
GetObjectPoseState(
object_frame='bin5_frame',
ref_frame='world'),
transitions={
'continue': 'Offset',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'bin_pose'})
# x:854 y:117
OperatableStateMachine.add('Compute_Place_2',
ComputeDropPart(),
transitions={
'continue': 'Move_Clear',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'binOffset': 'binOffset',
'bin_pose': 'bin_pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:220 y:154
OperatableStateMachine.add('Offset',
part_offsetCalc(),
transitions={
'succes': 'Compute_Place',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part',
'part_offset': 'offset'
})
# x:815 y:435
OperatableStateMachine.add('AddPart',
AddNumericState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'value_a': 'binOffset',
'value_b': 'one',
'result': 'binOffset'
})
return _state_machine
def create(self):
# x: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: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
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:181 y:520, x:145 y:331
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['agv_id', 'part_pose', 'arm_id', 'part'])
_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.agv_id = ''
_state_machine.userdata.part_pose = []
_state_machine.userdata.arm_id = ''
_state_machine.userdata.pre_agv = ''
_state_machine.userdata.home = 'Home'
_state_machine.userdata.rotation = 0
_state_machine.userdata.offset = 0.2
_state_machine.userdata.agv_pose = []
_state_machine.userdata.part = ''
_state_machine.userdata.offset_up = 0.1
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:101 y:25
OperatableStateMachine.add('PartInGripper?',
GripperControl(enable=True),
transitions={
'continue': 'agv_pose',
'failed': 'finished',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:326 y:19
OperatableStateMachine.add('agv_pose',
GetAGVPose(ref_frame='world'),
transitions={
'continue': 'AgvPos',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'agv_id': 'agv_id',
'arm_id': 'arm_id',
'agv_pose': 'agv_pose',
'move_pos': 'pre_agv'
})
# x:1056 y:175
OperatableStateMachine.add('computeDrop',
ComputeDropPartOffsetGraspAriacState(),
transitions={
'continue': 'Move',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'part_pose': 'part_pose',
'agv_pose': 'agv_pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1108 y:243
OperatableStateMachine.add('Move',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Drop',
'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:1144 y:309
OperatableStateMachine.add('Drop',
GripperControl(enable=False),
transitions={
'continue': 'UpOffset',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:1129 y:99
OperatableStateMachine.add('Offset',
part_offsetCalc(),
transitions={
'succes': 'computeDrop',
'unknown_id': 'failed'
},
autonomy={
'succes': Autonomy.Off,
'unknown_id': Autonomy.Off
},
remapping={
'part_type': 'part',
'part_offset': 'offset'
})
# x:524 y:20
OperatableStateMachine.add('AgvPos',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'computeDrop_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': 'pre_agv',
'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:773 y:596
OperatableStateMachine.add('ArmHome',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'ArmHome',
'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:684 y:19
OperatableStateMachine.add('computeDrop_2',
ComputeDropPartOffsetGraspAriacState(),
transitions={
'continue': 'Move_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'part_pose': 'part_pose',
'agv_pose': 'agv_pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:915 y:19
OperatableStateMachine.add('Move_2',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'Offset',
'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:741 y:446
OperatableStateMachine.add('computeDrop_3',
ComputeDropPartOffsetGraspAriacState(),
transitions={
'continue': 'Move_3',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'arm_id',
'move_group_prefix':
'move_group_prefix',
'part_pose': 'part_pose',
'agv_pose': 'agv_pose',
'offset': 'offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:760 y:528
OperatableStateMachine.add('Move_3',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'ArmHome',
'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:765 y:369
OperatableStateMachine.add('UpOffset',
ReplaceState(),
transitions={'done': 'computeDrop_3'},
autonomy={'done': Autonomy.Off},
remapping={
'value': 'offset_up',
'result': 'offset'
})
return _state_machine
def create(self):
# x:1241 y:669, x:521 y:384
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=[
'bin', 'move_group_prefix', 'camera_topic', 'camera_frame',
'ref_frame', 'agv_id', 'part_type', 'part_pose'
])
_state_machine.userdata.agv_id = []
_state_machine.userdata.part_type = []
_state_machine.userdata.config_name_home = 'home'
_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.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.config_name_bin = bin
_state_machine.userdata.config_name_tray = 'tray2PreDrop'
_state_machine.userdata.pose = []
_state_machine.userdata.part_offset = 0.080
_state_machine.userdata.part_rotation = 0
_state_machine.userdata.tool_link = 'ee_link'
_state_machine.userdata.arm_id = 'arm1'
_state_machine.userdata.camera_topic = []
_state_machine.userdata.camera_frame = []
_state_machine.userdata.ref_frame = []
_state_machine.userdata.bin = []
_state_machine.userdata.part_pose = []
_state_machine.userdata.agv_pose = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:111 y:130
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:205 y:45
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:643 y:32
OperatableStateMachine.add('MoveBin',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'ComputeGrasp',
'planning_failed': 'failed',
'control_failed': 'ComputeGrasp',
'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:1079 y:316
OperatableStateMachine.add('MoveToTray',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'GetAgvPose',
'planning_failed': 'failed',
'control_failed': 'MoveToTray',
'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:809 y:42
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:1036 y:144
OperatableStateMachine.add('GripperEnabled',
GripperControl(enable=True),
transitions={
'continue': 'MoveToHome_2',
'failed': 'MoveToPart',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:425 y:30
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:990 y:59
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:1064 y:223
OperatableStateMachine.add('MoveToHome_2',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'MoveToTray',
'planning_failed': 'failed',
'control_failed': 'MoveToHome_2',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:913 y:572
OperatableStateMachine.add('GripperDisabled',
GripperControl(enable=False),
transitions={
'continue': 'MoveToHome_3',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id'})
# x:911 y:646
OperatableStateMachine.add('MoveToHome_3',
SrdfStateToMoveitAriac(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'MoveToHome_3',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name_home',
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'action_topic': 'action_topic',
'robot_name': 'robot_name',
'config_name_out':
'config_name_out',
'move_group_out': 'move_group_out',
'robot_name_out': 'robot_name_out',
'action_topic_out':
'action_topic_out',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1080 y:390
OperatableStateMachine.add(
'GetAgvPose',
GetObjectPoseState(object_frame='kit_tray_1',
ref_frame='arm1_linear_arm_actuator'),
transitions={
'continue': 'ComputeDrop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'agv_pose'})
# x:878 y:511
OperatableStateMachine.add('MoveR1ToDrop',
MoveitToJointsDynAriacState(),
transitions={
'reached': 'GripperDisabled',
'planning_failed': 'failed',
'control_failed': 'GripperDisabled'
},
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:912 y:452
OperatableStateMachine.add(
'ComputeDrop',
ComputeDropPartOffsetGraspAriacState(joint_names=[
'linear_arm_actuator_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint',
'wrist_2_joint', 'wrist_3_joint'
]),
transitions={
'continue': 'MoveR1ToDrop',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix': 'move_group_prefix',
'tool_link': 'tool_link',
'part_pose': 'part_pose',
'pose': 'agv_pose',
'offset': 'part_offset',
'rotation': 'part_rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
return _state_machine
