def create(self):
pick_group = 'robot1'
names1 = [
'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
'robot1_elbow_joint', 'robot1_wrist_1_joint',
'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
]
gripper1 = "vacuum_gripper1_suction_cup"
# x:935 y:528, x:36 y:516
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.part_pose = []
_state_machine.userdata.pick_configuration = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:76 y:45
OperatableStateMachine.add('Move Robot 1 Home',
SrdfStateToMoveit(
config_name='R1Home',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'Feed part',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:584 y:175
OperatableStateMachine.add('Compute pick',
ComputeGraspState(group=pick_group,
offset=0.0,
joint_names=names1,
tool_link=gripper1),
transitions={
'continue': 'Activate gripper',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick_configuration',
'joint_names': 'joint_names'
})
# x:677 y:255
OperatableStateMachine.add('Activate gripper',
VacuumGripperControlState(
enable=True,
service_name='/gripper1/control'),
transitions={
'continue': 'Move to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:738 y:336
OperatableStateMachine.add(
'Move to pick',
MoveitToJointsDynState(move_group=pick_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Move Robot 1 Home_2',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick_configuration',
'joint_names': 'joint_names'
})
# x:825 y:418
OperatableStateMachine.add('Move Robot 1 Home_2',
SrdfStateToMoveit(
config_name='R1Home',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
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',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:269 y:57
OperatableStateMachine.add('Feed part',
ControlFeederState(activation=True),
transitions={
'succeeded': 'Locate part',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:412 y:112
OperatableStateMachine.add(
'Locate part',
DetectPartCameraState(ref_frame='robot1_base',
camera_topic='/hrwros/logical_camera_1',
camera_frame='logical_camera_1_frame'),
transitions={
'continue': 'Compute pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
return _state_machine
def create(self):
names1 = [
'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
'robot1_elbow_joint', 'robot1_wrist_1_joint',
'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
]
pick1_group = 'robot1'
robot1_loc = Pose2D(x=3.8, y=2.1, theta=-90.0)
gripper1 = "vacuum_gripper1_suction_cup"
pick2_group = 'robot2'
robot1_loc_o = Pose2D(x=-4.3, y=-0.9, theta=0.0)
names2 = [
'robot2_shoulder_pan_joint', 'robot2_shoulder_lift_joint',
'robot2_elbow_joint', 'robot2_wrist_1_joint',
'robot2_wrist_2_joint', 'robot2_wrist_3_joint'
]
gripper2 = "vacuum_gripper2_suction_cup"
# x:208 y:207, x:594 y:345
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.part_pose = []
_state_machine.userdata.robot1_loc = robot1_loc
_state_machine.userdata.pose_turtlebot = []
_state_machine.userdata.pick1_configuration = []
_state_machine.userdata.place1_configuration = []
_state_machine.userdata.speed = 100
_state_machine.userdata.robot1_loc_o = robot1_loc_o
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:32 y:56
OperatableStateMachine.add(
'Move R1 Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Start Conveyor',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1122 y:138
OperatableStateMachine.add('Compute pick',
ComputeGraspState(group=pick1_group,
offset=0.0,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Activate Gripper 1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick1_configuration',
'joint_names': 'joint_names'
})
# x:381 y:59
OperatableStateMachine.add('Start feeder',
ControlFeederState(activation=True),
transitions={
'succeeded': 'Wait for part',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:718 y:59
OperatableStateMachine.add('Stop feeder',
ControlFeederState(activation=False),
transitions={
'succeeded': 'Stop Conveyor',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1104 y:222
OperatableStateMachine.add('Activate Gripper 1',
VacuumGripperControlState(
enable=True,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:971 y:519
OperatableStateMachine.add('Compute place Turtlebot',
ComputeGraspState(group=pick1_group,
offset=0.6,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Move R1 to place',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'pose_turtlebot',
'joint_values':
'place1_configuration',
'joint_names': 'joint_names'
})
# x:1127 y:493
OperatableStateMachine.add(
'LocateTurtlebot',
LocateFactoryDeviceState(model_name='mobile_base',
output_frame_id='world'),
transitions={
'succeeded': 'Compute place Turtlebot',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose_turtlebot'})
# x:1125 y:370
OperatableStateMachine.add(
'Move R1 back Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Navigate To Robot1',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:5 y:296
OperatableStateMachine.add(
'Move R2 back to Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R2 To Bin',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1113 y:59
OperatableStateMachine.add(
'Detect Part Camera',
DetectPartCameraState(ref_frame='robot1_base',
camera_topic='/hrwros/logical_camera_1',
camera_frame='logical_camera_1_frame'),
transitions={
'continue': 'Compute pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:556 y:60
OperatableStateMachine.add('Wait for part',
SubscriberState(
topic='/break_beam_sensor_change',
blocking=True,
clear=True),
transitions={
'received': 'Stop feeder',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:1121 y:304
OperatableStateMachine.add(
'Move R1 to pick',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Move R1 back Home',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick1_configuration',
'joint_names': 'joint_names'
})
# x:827 y:519
OperatableStateMachine.add(
'Move R1 to place',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Deactivate Gripper',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'place1_configuration',
'joint_names': 'joint_names'
})
# x:215 y:107
OperatableStateMachine.add('Start Conveyor',
SetConveyorPowerState(stop=False),
transitions={
'succeeded': 'Start feeder',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'speed'})
# x:886 y:60
OperatableStateMachine.add('Stop Conveyor',
SetConveyorPowerState(stop=True),
transitions={
'succeeded': 'Detect Part Camera',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'speed'})
# x:1136 y:432
OperatableStateMachine.add('Navigate To Robot1',
hrwros_factory_states__MoveBaseState(),
transitions={
'arrived': 'LocateTurtlebot',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'robot1_loc'})
# x:661 y:517
OperatableStateMachine.add('Deactivate Gripper',
VacuumGripperControlState(
enable=False,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 Home_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:501 y:519
OperatableStateMachine.add(
'Move R1 Home_2',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Navigate To Robot1_2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:353 y:522
OperatableStateMachine.add('Navigate To Robot1_2',
hrwros_factory_states__MoveBaseState(),
transitions={
'arrived': 'Move R2 Home',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'robot1_loc_o'})
# x:25 y:530
OperatableStateMachine.add(
'Detect object near robot2',
DetectPartCameraState(ref_frame='robot2_base',
camera_topic='/hrwros/logical_camera_2',
camera_frame='logical_camera_2_frame'),
transitions={
'continue': 'Compute pick_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:42 y:470
OperatableStateMachine.add('Compute pick_2',
ComputeGraspState(group=pick2_group,
offset=0.0,
joint_names=names2,
tool_link=gripper2,
rotation=3.1415),
transitions={
'continue':
'Activate Gripper 2 robot2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick1_configuration',
'joint_names': 'joint_names'
})
# x:20 y:410
OperatableStateMachine.add('Activate Gripper 2 robot2',
VacuumGripperControlState(
enable=True,
service_name='/gripper2/control'),
transitions={
'continue': 'Move R1 to pick_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:211 y:525
OperatableStateMachine.add(
'Move R2 Home',
hrwros_factory_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'Detect object near robot2',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:21 y:353
OperatableStateMachine.add(
'Move R1 to pick_2',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick2_group,
offset=0.0,
tool_link=gripper2,
action_topic='/move_group'),
transitions={
'reached': 'Move R2 back to Home',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick1_configuration',
'joint_names': 'joint_names'
})
# x:10 y:220
OperatableStateMachine.add(
'Move R2 To Bin',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Place',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Deactivate R2 Gripper',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:9 y:133
OperatableStateMachine.add('Deactivate R2 Gripper',
VacuumGripperControlState(
enable=False,
service_name='/gripper2/control'),
transitions={
'continue': 'finished',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
return _state_machine
def create(self):
pick_group = 'robot1'
names1 = [
'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
'robot1_elbow_joint', 'robot1_wrist_1_joint',
'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
]
gripper1 = "vacuum_gripper1_suction_cup"
# x:1010 y:559, x:40 y:517
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.part_pose = []
_state_machine.userdata.place_configuration = []
_state_machine.userdata.conveyor_speed = 100
_state_machine.userdata.joint_names = []
_state_machine.userdata.pick_configuration = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:73 y:34
OperatableStateMachine.add(
'Move Robot 1 Home',
hrwros_factory_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'Stars Conveyor Belt',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:673 y:227
OperatableStateMachine.add('Compute pick',
ComputeGraspState(group=pick_group,
offset=0.0,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Activate gripper',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick_configuration',
'joint_names': 'joint_names'
})
# x:661 y:330
OperatableStateMachine.add('Activate gripper',
VacuumGripperControlState(
enable=True,
service_name='/gripper1/control'),
transitions={
'continue': 'Move to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:668 y:439
OperatableStateMachine.add(
'Move to pick',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Move Robot 1 Home_2',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick_configuration',
'joint_names': 'joint_names'
})
# x:668 y:554
OperatableStateMachine.add(
'Move Robot 1 Home_2',
hrwros_factory_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
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',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'place_configuration',
'joint_names': 'joint_names'
})
# x:507 y:36
OperatableStateMachine.add('Feed part',
ControlFeederState(activation=True),
transitions={
'succeeded': 'Locate part',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:701 y:31
OperatableStateMachine.add(
'Locate part',
DetectPartCameraState(ref_frame='robot1_base',
camera_topic='/hrwros/logical_camera_1',
camera_frame='logical_camera_1_frame'),
transitions={
'continue': 'Stops Conveyor Belt',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:272 y:31
OperatableStateMachine.add('Deactivate Gripper',
VacuumGripperControlState(
enable=False,
service_name='/gripper1/control'),
transitions={
'continue': 'Feed part',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:93 y:111
OperatableStateMachine.add('Stars Conveyor Belt',
SetConveyorPowerState(stop=False),
transitions={
'succeeded': 'Deactivate Gripper',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'conveyor_speed'})
# x:665 y:136
OperatableStateMachine.add('Stops Conveyor Belt',
SetConveyorPowerState(stop=True),
transitions={
'succeeded': 'Compute pick',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'conveyor_speed'})
return _state_machine
def create(self):
names1 = [
'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
'robot1_elbow_joint', 'robot1_wrist_1_joint',
'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
]
pick1_group = 'robot1'
pick2_group = 'robot2'
robot1_loc = Pose2D(x=-0.2, y=2.03, theta=0.0)
names2 = [
'robot2_shoulder_pan_joint', 'robot2_shoulder_lift_joint',
'robot2_elbow_joint', 'robot2_wrist_1_joint',
'robot2_wrist_2_joint', 'robot2_wrist_3_joint'
]
gripper1 = "vacuum_gripper1_suction_cup"
gripper2 = "vacuum_gripper2_suction_cup"
robot2_loc = Pose2D(x=-8.2, y=-1.4, theta=0.0)
# x:9 y:195, x:413 y:347
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.part_pose = []
_state_machine.userdata.speed = '100.0'
_state_machine.userdata.robot1_loc = robot1_loc
_state_machine.userdata.pick2_configuration = []
_state_machine.userdata.pose_turtlebot = []
_state_machine.userdata.robot2_loc = robot2_loc
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:87 y:217
OperatableStateMachine.add('Move R2 to Home',
SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R1 Home',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1025 y:148
OperatableStateMachine.add('Compute Pick for R1',
ComputeGraspState(group=pick1_group,
offset=0.0,
joint_names=names1,
tool_link=gripper1),
transitions={
'continue': 'Activate Gripper 1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick1_configuration',
'joint_names': 'joint_names'
})
# x:885 y:309
OperatableStateMachine.add(
'Move R1 to Pick',
MoveitToJointsDynState(move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Move R1 back Home',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick1_configuration',
'joint_names': 'joint_names'
})
# x:421 y:15
OperatableStateMachine.add('Start feeder',
ControlFeederState(activation=True),
transitions={
'succeeded': 'Wait for part',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:588 y:25
OperatableStateMachine.add('Stop feeder',
ControlFeederState(activation=False),
transitions={
'succeeded': 'Stop Conveyor',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:845 y:227
OperatableStateMachine.add('Activate Gripper 1',
VacuumGripperControlState(
enable=True,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 to Pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:874 y:578
OperatableStateMachine.add('Compute place Turtlebot',
ComputeGraspState(group=pick1_group,
offset=0.55,
joint_names=names1,
tool_link=gripper1),
transitions={
'continue': 'Move R1 to place',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'pose_turtlebot',
'joint_values':
'place1_configuration',
'joint_names': 'joint_names'
})
# x:890 y:658
OperatableStateMachine.add(
'Move R1 to place',
MoveitToJointsDynState(move_group=pick1_group,
offset=0.55,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'place1_configuration',
'joint_names': 'joint_names'
})
# x:882 y:494
OperatableStateMachine.add(
'LocateTurtlebot',
LocateFactoryDeviceState(model_name='mobile_base',
output_frame_id='world'),
transitions={
'succeeded': 'Compute place Turtlebot',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose_turtlebot'})
# x:915 y:398
OperatableStateMachine.add('Move R1 back Home',
SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R1 Home',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:56 y:468
OperatableStateMachine.add('Move R2 back Home',
SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
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',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1011 y:56
OperatableStateMachine.add(
'Locate Part Camera1',
DetectPartCameraState(ref_frame='robot1_base',
camera_topic='/hrwros/logical_camera_1',
camera_frame='logical_camera_1_frame'),
transitions={
'continue': 'Compute Pick for R1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:508 y:144
OperatableStateMachine.add('Wait for part',
SubscriberState(
topic='/break_beam_sensor_change',
blocking=True,
clear=False),
transitions={
'received': 'Stop feeder',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:58 y:55
OperatableStateMachine.add('Move R1 Home',
SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Start Conveyor',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:212 y:22
OperatableStateMachine.add('Start Conveyor',
SetConveyorPowerState(stop=False),
transitions={
'succeeded': 'Start feeder',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'speed'})
# x:817 y:25
OperatableStateMachine.add('Stop Conveyor',
SetConveyorPowerState(stop=True),
transitions={
'succeeded': 'Locate Part Camera1',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'speed'})
return _state_machine
def create(self):
names1 = [
'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
'robot1_elbow_joint', 'robot1_wrist_1_joint',
'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
]
pick1_group = 'robot1'
robot1_loc = Pose2D(x=3.8, y=2.2, theta=-90.0)
gripper1 = "vacuum_gripper1_suction_cup"
robot2_loc = Pose2D(x=-4.3, y=-0.9, theta=0.0)
pick2_group = 'robot2'
gripper2 = "vacuum_gripper2_suction_cup"
names2 = [
'robot2_shoulder_pan_joint', 'robot2_shoulder_lift_joint',
'robot2_elbow_joint', 'robot2_wrist_1_joint',
'robot2_wrist_2_joint', 'robot2_wrist_3_joint'
]
# x:31 y:297, x:742 y:423
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.part_pose = []
_state_machine.userdata.robot1_loc = robot1_loc
_state_machine.userdata.pose_turtlebot = []
_state_machine.userdata.pick1_configuration = []
_state_machine.userdata.place1_configuration = []
_state_machine.userdata.conveyor_speed = 100
_state_machine.userdata.robot2_loc = robot2_loc
_state_machine.userdata.pick2_configuration = []
_state_machine.userdata.place2_configuration = []
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:13 y:113
OperatableStateMachine.add(
'Move R1 Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Navigate to robot1',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1347 y:308
OperatableStateMachine.add('Compute pick',
ComputeGraspState(group=pick1_group,
offset=0.0,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Activate Gripper R1',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick1_configuration',
'joint_names': 'joint_names'
})
# x:1169 y:9
OperatableStateMachine.add('Start feeder',
ControlFeederState(activation=True),
transitions={
'succeeded': 'Wait for part',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1342 y:99
OperatableStateMachine.add('Stop feeder',
ControlFeederState(activation=False),
transitions={
'succeeded': 'stop conveyor',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1336 y:382
OperatableStateMachine.add('Activate Gripper R1',
VacuumGripperControlState(
enable=True,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:561 y:24
OperatableStateMachine.add('Compute place Turtlebot',
ComputeGraspState(group=pick1_group,
offset=0.8,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Move R1 to place',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'pose_turtlebot',
'joint_values':
'place1_configuration',
'joint_names': 'joint_names'
})
# x:379 y:32
OperatableStateMachine.add(
'LocateTurtlebot',
LocateFactoryDeviceState(model_name='mobile_base',
output_frame_id='world'),
transitions={
'succeeded': 'Compute place Turtlebot',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'pose_turtlebot'})
# x:1323 y:841
OperatableStateMachine.add(
'Move R1 back Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R2 Home',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1339 y:240
OperatableStateMachine.add(
'Detect Part Camera',
DetectPartCameraState(ref_frame='robot1_base',
camera_topic='/hrwros/logical_camera_1',
camera_frame='logical_camera_1_frame'),
transitions={
'continue': 'Compute pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:1369 y:26
OperatableStateMachine.add('Wait for part',
SubscriberState(
topic='/break_beam_sensor_change',
blocking=True,
clear=True),
transitions={
'received': 'Stop feeder',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:742 y:11
OperatableStateMachine.add(
'Move R1 to place',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'start conveyor',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'place1_configuration',
'joint_names': 'joint_names'
})
# x:948 y:8
OperatableStateMachine.add('start conveyor',
SetConveyorPowerState(stop=False),
transitions={
'succeeded': 'Start feeder',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'conveyor_speed'})
# x:1338 y:171
OperatableStateMachine.add('stop conveyor',
SetConveyorPowerState(stop=True),
transitions={
'succeeded': 'Detect Part Camera',
'failed': 'failed'
},
autonomy={
'succeeded': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'speed': 'conveyor_speed'})
# x:205 y:59
OperatableStateMachine.add('Navigate to robot1',
hrwros_factory_states__MoveBaseState(),
transitions={
'arrived': 'LocateTurtlebot',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'robot1_loc'})
# x:1313 y:762
OperatableStateMachine.add('Deactivate gripper 1',
VacuumGripperControlState(
enable=False,
service_name='/gripper1/control'),
transitions={
'continue': 'Move R1 back Home',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1351 y:448
OperatableStateMachine.add(
'Move R1 to pick',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Move R1 PreGrasp',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick1_configuration',
'joint_names': 'joint_names'
})
# x:1007 y:842
OperatableStateMachine.add('navigate to robot2',
hrwros_factory_states__MoveBaseState(),
transitions={
'arrived': 'Detect Part Camera_R2',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'robot2_loc'})
# x:839 y:839
OperatableStateMachine.add(
'Detect Part Camera_R2',
DetectPartCameraState(ref_frame='robot2_base',
camera_topic='/hrwros/logical_camera_2',
camera_frame='logical_camera_2_frame'),
transitions={
'continue': 'Compute pick_R2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:685 y:836
OperatableStateMachine.add('Compute pick_R2',
ComputeGraspState(group=pick2_group,
offset=0.0,
joint_names=names2,
tool_link=gripper2,
rotation=3.1415),
transitions={
'continue': 'Activate Gripper R2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick2_configuration',
'joint_names': 'joint_names'
})
# x:307 y:840
OperatableStateMachine.add(
'Move R2 to pick',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick2_group,
offset=0.0,
tool_link=gripper2,
action_topic='/move_group'),
transitions={
'reached': 'Move R2 Home_2',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick2_configuration',
'joint_names': 'joint_names'
})
# x:487 y:839
OperatableStateMachine.add('Activate Gripper R2',
VacuumGripperControlState(
enable=True,
service_name='/gripper2/control'),
transitions={
'continue': 'Move R2 to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1344 y:566
OperatableStateMachine.add(
'Move R1 Home_2',
hrwros_factory_states__SrdfStateToMoveit(
config_name='R1Home',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Compute place Turtlebot_2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1323 y:696
OperatableStateMachine.add(
'Move R1 to place_2',
hrwros_factory_states__MoveitToJointsDynState(
move_group=pick1_group,
offset=0.0,
tool_link=gripper1,
action_topic='/move_group'),
transitions={
'reached': 'Deactivate gripper 1',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'place1_configuration',
'joint_names': 'joint_names'
})
# x:1307 y:631
OperatableStateMachine.add('Compute place Turtlebot_2',
ComputeGraspState(group=pick1_group,
offset=0.6,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Move R1 to place_2',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'pose_turtlebot',
'joint_values':
'place1_configuration',
'joint_names': 'joint_names'
})
# x:1342 y:505
OperatableStateMachine.add(
'Move R1 PreGrasp',
hrwros_factory_states__SrdfStateToMoveit(
config_name='R1PreGrasp',
move_group=pick1_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R1 Home_2',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:95 y:817
OperatableStateMachine.add(
'Move R2 Home_2',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Move R2Place',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:30 y:721
OperatableStateMachine.add(
'Move R2Place',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Place',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'Deactivate Gripper R2',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:1150 y:847
OperatableStateMachine.add(
'Move R2 Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
transitions={
'reached': 'navigate to robot2',
'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',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:34 y:486
OperatableStateMachine.add(
'Move R2 back Home',
flexbe_manipulation_states__SrdfStateToMoveit(
config_name='R2Home',
move_group=pick2_group,
action_topic='/move_group',
robot_name=''),
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',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:22 y:588
OperatableStateMachine.add('Deactivate Gripper R2',
VacuumGripperControlState(
enable=False,
service_name='/gripper2/control'),
transitions={
'continue': 'Move R2 back Home',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
return _state_machine
def create(self):
pick_group = 'robot1'
home1 = [1.57, -1.57, 1.24, -1.57, -1.57, 0]
names1 = [
'robot1_shoulder_pan_joint', 'robot1_shoulder_lift_joint',
'robot1_elbow_joint', 'robot1_wrist_1_joint',
'robot1_wrist_2_joint', 'robot1_wrist_3_joint'
]
gripper1 = "vacuum_gripper1_suction_cup"
# x:1080 y:384, x:225 y:414
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.part_pose = []
_state_machine.userdata.pick_configuration = []
_state_machine.userdata.home1 = home1
_state_machine.userdata.conveyor_speed = 100
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:189 y:44
OperatableStateMachine.add(
'Detect Part',
DetectPartCameraState(ref_frame='robot1_base',
camera_topic='/hrwros/logical_camera_1',
camera_frame='logical_camera_1_frame'),
transitions={
'continue': 'ComputePoseConf:',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pose': 'part_pose'})
# x:623 y:170
OperatableStateMachine.add(
'Move Robot1 to pick',
flexbe_manipulation_states__MoveitToJointsDynState(
move_group=pick_group, action_topic='/move_group'),
transitions={
'reached': 'ActivateGripper',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'pick_configuration',
'joint_names': 'joint_names'
})
# x:659 y:263
OperatableStateMachine.add(
'ActivateGripper',
VacuumGripperControlState(enable=True,
service_name='/gripper1/control'),
transitions={
'continue': 'Move Robot to home configuration',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:628 y:362
OperatableStateMachine.add(
'Move Robot to home configuration',
flexbe_manipulation_states__MoveitToJointsDynState(
move_group=pick_group, action_topic='/move_group'),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'home1',
'joint_names': 'joint_names'
})
# x:598 y:77
OperatableStateMachine.add('ComputePoseConf:',
ComputeGraspState(group=pick_group,
offset=0.0,
joint_names=names1,
tool_link=gripper1,
rotation=3.1415),
transitions={
'continue': 'Move Robot1 to pick',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pose': 'part_pose',
'joint_values':
'pick_configuration',
'joint_names': 'joint_names'
})
return _state_machine