def create(self):
# x:1132 y:61, x:520 y:383
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.joint_names_test = ["m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"]
_state_machine.userdata.joint_values_test = [5.851354801687673, 4.864461337582612, 1.0885434600152142, 2.0525195574127895, 4.140473804709629, 1.3734302573574513]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:111 y:45
OperatableStateMachine.add('home',
FeedbackSrdfStateToMoveit(config_name="Home", move_group="arm", action_topic='/move_group', robot_name="m1n6s200", position_topic='/m1n6s200_driver/joint_states', delta=1E-7),
transitions={'reached': 'Testing ', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off})
# x:457 y:49
OperatableStateMachine.add('Testing ',
FeedbackJointStateToMoveit(move_group="arm", action_topic="/move_group", robot_name="m1n6s200", position_topic="/m1n6s200_driver/joint_states", delta=1E-7),
transitions={'reached': 'go home2', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'joint_values': 'joint_values_test', 'joint_names': 'joint_names_test'})
# x:803 y:55
OperatableStateMachine.add('go home2',
SrdfStateToMoveit(config_name="Home", move_group="arm", action_topic='/move_group', robot_name="m1n6s200"),
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'})
return _state_machine
def create(self):
# x:1108 y:307, x:1120 y:6
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:111 y:81
OperatableStateMachine.add('Go_Home_Position',
SrdfStateToMoveit(config_name="Home", move_group="arm", action_topic='/move_group', robot_name="m1n6s200"),
transitions={'reached': 'Get_Joint_Values', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Low, '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_values2', 'joint_names': 'joint_names'})
# x:561 y:79
OperatableStateMachine.add('Go_Retract_Position',
SrdfStateToMoveit(config_name="Retract", move_group="arm", action_topic='/move_group', robot_name="m1n6s200"),
transitions={'reached': 'Move_to_Retrieved_Joint_Values', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Low, '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_values2', 'joint_names': 'joint_names'})
# x:771 y:209
OperatableStateMachine.add('Move_to_Retrieved_Joint_Values',
MoveitToJointsState(move_group="arm", joint_names=["m1n6s200_joint_1","m1n6s200_joint_2","m1n6s200_joint_3","m1n6s200_joint_4","m1n6s200_joint_5","m1n6s200_joint_6","m1n6s200_joint_finger_1","m1n6s200_joint_finger_2"], 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_config': 'joint_values'})
# x:343 y:208
OperatableStateMachine.add('Get_Joint_Values',
GetJointValuesState(joints=["m1n6s200_joint_1","m1n6s200_joint_2","m1n6s200_joint_3","m1n6s200_joint_4","m1n6s200_joint_5","m1n6s200_joint_6","m1n6s200_joint_finger_1","m1n6s200_joint_finger_2"], topic_name="/m1n6s200_driver/joint_states"),
transitions={'retrieved': 'Go_Retract_Position'},
autonomy={'retrieved': Autonomy.Off},
remapping={'joint_values': 'joint_values'})
return _state_machine
def create(self):
joint_trajectory_action = 'motion/controller/joint_trajectory'
storage = 'joint_trajectory/'
voice_topic = 'voice/voice'
eye_cmd_topic = 'control'
# x:1034 y:292, x:622 y:639
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['be_evil', 'new_be_evil'],
output_keys=['be_evil'])
_state_machine.userdata.be_evil = self.be_evil
_state_machine.userdata.new_be_evil = self.new_be_evil
_state_machine.userdata.head_crooked_pose = [0, 0, 0, 0.4]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:96 y:163
OperatableStateMachine.add('CheckEvil',
DecisionState(
outcomes=['good', 'evil'],
conditions=lambda x: 'good'
if not x else 'evil'),
transitions={
'good': 'CheckChangeToEvil',
'evil': 'CheckChangeToGood'
},
autonomy={
'good': Autonomy.Off,
'evil': Autonomy.Off
},
remapping={'input_value': 'be_evil'})
# x:252 y:50
OperatableStateMachine.add(
'CheckChangeToEvil',
DecisionState(outcomes=['yes', 'no'],
conditions=lambda x: 'yes' if x else 'no'),
transitions={
'yes': 'RedEyes',
'no': 'NormalEyes'
},
autonomy={
'yes': Autonomy.Off,
'no': Autonomy.Off
},
remapping={'input_value': 'new_be_evil'})
# x:426 y:48
OperatableStateMachine.add('RedEyes',
TextCommandState(type='eyes/emotion',
command='red_eyes',
topic=eye_cmd_topic),
transitions={'done': 'Seizure'},
autonomy={'done': Autonomy.Off})
# x:737 y:30
OperatableStateMachine.add(
'Seizure',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='seizure_evil',
trajectory_ns=storage),
transitions={
'success': 'SetEvil',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:961 y:162
OperatableStateMachine.add(
'SetEvil',
CalculationState(calculation=lambda x: True),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'be_evil',
'output_value': 'be_evil'
})
# x:419 y:135
OperatableStateMachine.add('NormalEyes',
TextCommandState(type='eyes/emotion',
command='normal',
topic=eye_cmd_topic),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:195 y:281
OperatableStateMachine.add(
'CheckChangeToGood',
DecisionState(outcomes=['yes', 'no'],
conditions=lambda x: 'no' if x else 'yes'),
transitions={
'yes': 'NormalEyes2',
'no': 'RedEyes2'
},
autonomy={
'yes': Autonomy.Off,
'no': Autonomy.Off
},
remapping={'input_value': 'new_be_evil'})
# x:416 y:331
OperatableStateMachine.add(
'NormalEyes2',
TextCommandState(type='eyes/emotion',
command='normal',
topic=eye_cmd_topic),
transitions={'done': 'HeadAssumeBasicPose'},
autonomy={'done': Autonomy.Off})
# x:613 y:321
OperatableStateMachine.add(
'ShakeHead',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='head_shake',
trajectory_ns=storage),
transitions={
'success': 'LookAround',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:894 y:326
OperatableStateMachine.add(
'SetGood',
CalculationState(calculation=lambda x: False),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'be_evil',
'output_value': 'be_evil'
})
# x:671 y:437
OperatableStateMachine.add(
'LookAround',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='look_around',
trajectory_ns=storage),
transitions={
'success': 'SetGood',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:420 y:200
OperatableStateMachine.add('RedEyes2',
TextCommandState(type='eyes/emotion',
command='red_eyes',
topic=eye_cmd_topic),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:444 y:420
OperatableStateMachine.add('HeadAssumeBasicPose',
SrdfStateToMoveit(
config_name='head_basic',
move_group='head',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'ShakeHead',
'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'
})
return _state_machine
def create(self):
joy_topic = '/hmi/joystick'
joint_trajectory_action = '/motion/controller/joint_trajectory'
# x:30 y:365, x:1031 y:242
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.rand_moves_config = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:397 y:287, x:130 y:297, x:230 y:297, x:330 y:297, x:459 y:297, x:530 y:297
_sm_joystickmovements_0 = ConcurrencyContainer(
outcomes=['failed', 'button1234', 'timeout'],
output_keys=['joy_msg'],
conditions=[('timeout', [('JoystickControl', 'done')]),
('failed', [('WaitButton1234Pressed', 'unavailable')]),
('button1234', [('WaitButton1234Pressed', 'received')])
])
with _sm_joystickmovements_0:
# x:385 y:129
OperatableStateMachine.add(
'JoystickControl',
JoystickJointControl(
joints_topic='/joint_states',
goal_joints_topic=
'/motion/controller/joint_state/out_joints_src_reset',
joy_topic=joy_topic,
buttons=[(4, 0.6, 'ear_l_joint', -3.0, 1.0),
(6, -0.6, 'ear_l_joint', -3.0, 1.0),
(5, 0.6, 'ear_r_joint', -3.0, 1.0),
(7, -0.6, 'ear_r_joint', -3.0, 1.0)],
axes=[(0, 0, 'eyes_yaw', -1.5, 1.5),
(1, 0, 'eyes_pitch', -1.5, 1.5),
(4, -0.4, 'head_joint4', -1.5, 1.5),
(5, 0.4, 'head_joint2', -1.5, 1.5),
(3, 0.5, 'mouth_joint', -0.6, 0)],
timeout=5.0),
transitions={'done': 'timeout'},
autonomy={'done': Autonomy.Off})
# x:130 y:136
OperatableStateMachine.add(
'WaitButton1234Pressed',
WaitForMessageState(
topic=joy_topic,
condition=lambda msg: any(msg.buttons[0:4]),
buffered=False,
clear=True),
transitions={
'received': 'button1234',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'joy_msg'})
# x:625 y:339, x:47 y:263, x:323 y:268, x:703 y:252, x:630 y:296, x:443 y:267, x:227 y:263
_sm_randmovements_1 = ConcurrencyContainer(
outcomes=['failed', 'timeout', 'joy_msg'],
input_keys=['rand_moves_config'],
output_keys=['joy_msg'],
conditions=[('failed', [('WaitJoystick', 'unavailable')]),
('failed', [('RandHeadMoves', 'failed')]),
('joy_msg', [('WaitJoystick', 'received')]),
('timeout', [('RandHeadMoves', 'done')])])
with _sm_randmovements_1:
# x:88 y:74
OperatableStateMachine.add(
'WaitJoystick',
WaitForMessageState(
topic=joy_topic,
condition=lambda msg: any(msg.buttons) or any(msg.axes),
buffered=False,
clear=False),
transitions={
'received': 'joy_msg',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'joy_msg'})
# x:332 y:60
OperatableStateMachine.add(
'RandHeadMoves',
RandJointsMovements(controller='joint_state_head',
duration=2000,
interval=[2.0, 5.0],
joints=['head_joint2', 'head_joint4'],
minimal=[-0.2, -0.3],
maximal=[0.3, 0.3]),
transitions={
'done': 'timeout',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'rand_moves_config'})
with _state_machine:
# x:57 y:26
OperatableStateMachine.add('RandMovements',
_sm_randmovements_1,
transitions={
'failed': 'failed',
'timeout': 'RandMovements',
'joy_msg': 'CheckButtonPressed'
},
autonomy={
'failed': Autonomy.Inherit,
'timeout': Autonomy.Inherit,
'joy_msg': Autonomy.Inherit
},
remapping={
'rand_moves_config':
'rand_moves_config',
'joy_msg': 'joy_msg'
})
# x:413 y:17
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(
config_name='head_group_basic',
move_group='head_group',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'SelectMovement',
'planning_failed': 'Wait',
'control_failed': 'Wait',
'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:373 y:236
OperatableStateMachine.add(
'Greeting',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='greeting',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:85 y:283
OperatableStateMachine.add('JoystickMovements',
_sm_joystickmovements_0,
transitions={
'failed': 'failed',
'button1234': 'PlaceHead',
'timeout': 'RandMovements'
},
autonomy={
'failed': Autonomy.Inherit,
'button1234': Autonomy.Inherit,
'timeout': Autonomy.Inherit
},
remapping={'joy_msg': 'joy_msg'})
# x:612 y:186
OperatableStateMachine.add(
'SelectMovement',
DecisionState(outcomes=[
'none', 'button1', 'button2', 'button3', 'button4'
],
conditions=self.move_selector),
transitions={
'none': 'RandMovements',
'button1': 'Greeting',
'button2': 'HeadShake',
'button3': 'GrownUp',
'button4': 'Greeting'
},
autonomy={
'none': Autonomy.Off,
'button1': Autonomy.Off,
'button2': Autonomy.Off,
'button3': Autonomy.Off,
'button4': Autonomy.Off
},
remapping={'input_value': 'joy_msg'})
# x:412 y:317
OperatableStateMachine.add(
'HeadShake',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='head_shake',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:472 y:404
OperatableStateMachine.add(
'GrownUp',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='very_grown_up',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'JoystickMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:167 y:134
OperatableStateMachine.add(
'CheckButtonPressed',
DecisionState(outcomes=['button1234', 'other'],
conditions=lambda msg: 'button1234'
if any(msg.buttons[0:4]) else 'other'),
transitions={
'button1234': 'PlaceHead',
'other': 'JoystickMovements'
},
autonomy={
'button1234': Autonomy.Off,
'other': Autonomy.Off
},
remapping={'input_value': 'joy_msg'})
# x:686 y:104
OperatableStateMachine.add('PlaceHead_2',
SrdfStateToMoveit(
config_name='head_group_basic',
move_group='head_group',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'SelectMovement',
'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:703 y:18
OperatableStateMachine.add('Wait',
WaitState(wait_time=0.5),
transitions={'done': 'PlaceHead_2'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:1204 y:222, x:498 y:264
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.joint_values1 = [
3.6101066052, 4.75188046356, 1.07972986155, 1.21694317802,
2.24516011381, 1.83854561594
]
_state_machine.userdata.joint_values2 = [
4.874892939572453, 4.363024855943735, 1.1921655267125706,
2.7607065555383334, 1.8676212097766047, 8.829414450644498
]
_state_machine.userdata.joint_values3 = [
5.851354801687673, 4.864461337582612, 1.0885434600152142,
2.0525195574127895, 4.140473804709629, 1.3734302573574513,
0.16385561330951914, 0.16508762491930343
]
_state_machine.userdata.joint_values_preppourL = [
4.761117371482087, 4.435244724700612, 1.6085220010066865,
3.2511244744831167, 1.706054283818386, 2.4325377312649925
]
_state_machine.userdata.joint_values_pourL = [
4.761121632539831, 4.435244724700612, 1.6085220010066865,
3.2511244744831167, 1.706051886973405, 4.577594147074765
]
_state_machine.userdata.joint_values2_IN = [
4.857230855223047, 4.862187530643903, 1.8024411410452421,
2.7714042073210727, 1.7851791966496213, 8.812876220275106
]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:56 y:94
OperatableStateMachine.add('srdf home',
SrdfStateToMoveit(
config_name="Home",
move_group="arm",
action_topic='/move_group',
robot_name="m1n6s200"),
transitions={
'reached': 'staticCenter',
'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:631 y:100
OperatableStateMachine.add('staticLEFT',
JointStateToMoveit(),
transitions={
'reached': 'staticRight',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values1',
'joint_names': 'joint_names'
})
# x:53 y:281
OperatableStateMachine.add('staticCenter',
JointStateToMoveit(),
transitions={
'reached': 'staticCenterIN',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values2',
'joint_names': 'joint_names'
})
# x:1005 y:195
OperatableStateMachine.add('staticRight',
JointStateToMoveit(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values3',
'joint_names': 'joint_names'
})
# x:310 y:426
OperatableStateMachine.add('PrepPourLeftCup',
JointStateToMoveit(),
transitions={
'reached': 'PourLeftCup',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values':
'joint_values_preppourL',
'joint_names': 'joint_names'
})
# x:567 y:445
OperatableStateMachine.add('PourLeftCup',
JointStateToMoveit(),
transitions={
'reached': 'AfterPour',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values':
'joint_values_pourL',
'joint_names': 'joint_names'
})
# x:772 y:472
OperatableStateMachine.add('AfterPour',
JointStateToMoveit(),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values':
'joint_values_preppourL',
'joint_names': 'joint_names'
})
# x:55 y:491
OperatableStateMachine.add('staticCenterIN',
JointStateToMoveit(),
transitions={
'reached': 'PrepPourLeftCup',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values2_IN',
'joint_names': 'joint_names'
})
return _state_machine
def create(self):
control_topic = 'control'
joint_trajectory_topic = 'motion/controller/joint_trajectory'
joy_topic = '/hmi/joystick'
moveit_action = 'move_group'
# x:68 y:631, x:130 y:353
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.unused = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:414 y:360, x:130 y:353, x:506 y:357, x:233 y:351, x:720 y:363, x:715 y:411
_sm_randmovements_0 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['unused'],
conditions=[('failed', [('WaitKey', 'unavailable')]),
('finished', [('RandMovements', 'done')]),
('finished', [('WaitKey', 'received')]),
('failed', [('RandMovements', 'failed')])])
with _sm_randmovements_0:
# x:148 y:122
OperatableStateMachine.add('WaitKey',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'finished',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:366 y:118
OperatableStateMachine.add('RandMovements',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=120,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'unused'})
with _state_machine:
# x:108 y:53
OperatableStateMachine.add('RandMovements',
_sm_randmovements_0,
transitions={
'finished': 'HeadBasic',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'unused': 'unused'})
# x:351 y:150
OperatableStateMachine.add('HeadBasic',
SrdfStateToMoveit(
config_name='head_upright',
move_group='head',
action_topic=moveit_action,
robot_name=''),
transitions={
'reached': 'GoodBye',
'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:329 y:271
OperatableStateMachine.add(
'GoodBye',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='thank_you_for_attention_nopony_was_harmed',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='introduce_herself',
t3=[2, 2.0],
type3='voice/play_wav',
cmd3='am_i_really_a_good_actress',
t4=[2, 2.0],
type4='motion/joint_trajectory',
cmd4='head_suprised'),
transitions={
'success': 'Bow',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:288 y:547
OperatableStateMachine.add('RandMovements2',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=120,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'unused'})
# x:319 y:387
OperatableStateMachine.add('Bow',
CompoundAction(
t1=[0, 2.0],
type1='motion/joint_trajectory',
cmd1='bow_begin',
t2=[1, 2.0],
type2='motion/joint_trajectory',
cmd2='bow_end',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'RandMovements2',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
return _state_machine
def create(self):
# x:583 y:683, x:578 y:422
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.unused = None
_state_machine.userdata.rand_head_config = {'interval': [0.5, 1]}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:278 y:24
OperatableStateMachine.add('SesectBehavior',
OperatorDecisionState(
outcomes=['dance', 'rand_moves'],
hint='Select behavior',
suggestion='dance'),
transitions={
'dance': 'TestMovement',
'rand_moves': 'RandHead'
},
autonomy={
'dance': Autonomy.Full,
'rand_moves': Autonomy.Full
})
# x:114 y:352
OperatableStateMachine.add(
'TurnOffJointStateController',
SetBoolState(
service='motion/controller/joint_state/set_operational',
value=False),
transitions={
'true': 'SingASong',
'false': 'failed',
'failure': 'failed'
},
autonomy={
'true': Autonomy.High,
'false': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={
'success': 'success',
'message': 'message'
})
# x:136 y:518
OperatableStateMachine.add('SingASong',
TextCommandState(type='voice/play_wav',
command='mmm_song',
topic='control'),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Full})
# x:248 y:213
OperatableStateMachine.add(
'TestMovement',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='dance14',
trajectory_param='/saved_msgs/joint_trajectory'),
transitions={
'success': 'TurnOffJointStateController',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Low,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:939 y:351
OperatableStateMachine.add(
'RandSelector',
DecisionState(
outcomes=['first', 'second'],
conditions=(lambda x: 'first'
if random.uniform(0, 1) < 0.5 else 'second')),
transitions={
'first': 'PlaceHead',
'second': 'TestMovement'
},
autonomy={
'first': Autonomy.Full,
'second': Autonomy.Full
},
remapping={'input_value': 'unused'})
# x:704 y:603
OperatableStateMachine.add(
'HeadShake',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='head_shake',
trajectory_param='/saved_msgs/joint_trajectory'),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:958 y:510
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(
config_name='head_basic',
move_group='head',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'HeadShake',
'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:725 y:125
OperatableStateMachine.add(
'RandHead',
SweetieBotRandHeadMovements(controller='joint_state_head',
duration=120,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'RandSelector',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'rand_head_config'})
return _state_machine
def create(self):
# x:744 y:92, x:65 y:535
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:220 y:611, x:575 y:295
_sm_move_arm_up_and_down_0 = OperatableStateMachine(
outcomes=['finished', 'failed'])
with _sm_move_arm_up_and_down_0:
# x:387 y:20
OperatableStateMachine.add(
'say while moving the arm',
TTSBulgarian(
ttsbg_text=
'Сега ще движа ръката си нагоре и надолу, докато се движа в квадрат.'
),
transitions={
'failed': 'failed',
'done': 'move to transport pos'
},
autonomy={
'failed': Autonomy.Off,
'done': Autonomy.Off
})
# x:624 y:41
OperatableStateMachine.add('move to transport pos',
SrdfStateToMoveit(
config_name='transport_position',
move_group="robot",
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'ton reach up',
'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:748 y:213
OperatableStateMachine.add('ton reach up',
SrdfStateToMoveit(
config_name='reach_up',
move_group="robot",
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'back to trnsport pos',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:104 y:150
OperatableStateMachine.add('back to trnsport pos',
SrdfStateToMoveit(
config_name='transport_positiion',
move_group="robot",
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'to reach up again',
'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:112 y:287
OperatableStateMachine.add(
'to reach up again',
SrdfStateToMoveit(config_name='reach_up',
move_group="robot",
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'finally back to transport pos',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:139 y:423
OperatableStateMachine.add('finally back to transport pos',
SrdfStateToMoveit(
config_name='transport_position',
move_group="robot",
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:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365
_sm_move_base_and_arm_while_speaking_1 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[
('finished', [('Drive_In_Squre_Stop_If_Obstacle', 'finished'),
('move arm up and down', 'finished')]),
('failed', [('Drive_In_Squre_Stop_If_Obstacle', 'failed')]),
('failed', [('move arm up and down', 'failed')])
])
with _sm_move_base_and_arm_while_speaking_1:
# x:54 y:87
OperatableStateMachine.add(
'Drive_In_Squre_Stop_If_Obstacle',
self.use_behavior(
Drive_In_Squre_Stop_If_ObstacleSM,
'Move base and arm while speaking/Drive_In_Squre_Stop_If_Obstacle'
),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:390 y:87
OperatableStateMachine.add('move arm up and down',
_sm_move_arm_up_and_down_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:586 y:62, x:23 y:199, x:230 y:365, x:330 y:365
_sm_say_intro_2 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('finished', [('Say intro', 'done'),
('give it time tos peak', 'done')]),
('failed', [('Say intro', 'failed')])])
with _sm_say_intro_2:
# x:28 y:61
OperatableStateMachine.add(
'Say intro',
TTSBulgarian(
ttsbg_text=
'Нека Ви се представя, аз съм Телеуправляемия Сервизен Робот Робко 19. За мен е чест да участвам в ГОДИШНАТА МЕЖДУНАРОДНА НАУЧНА КОНФЕРЕНЦИЯ НА АВИАЦИОННИЯ ФАКУЛТЕТ. Създаден съм от групата по сервизна роботика по проект, Финансиран от Фонда Научни изследвания. Сега ще направя кратка демонстрация на част от моите възможности.'
),
transitions={
'failed': 'failed',
'done': 'finished'
},
autonomy={
'failed': Autonomy.Off,
'done': Autonomy.Off
})
# x:108 y:302
OperatableStateMachine.add('give it time tos peak',
WaitState(wait_time=15),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
with _state_machine:
# x:108 y:54
OperatableStateMachine.add('Say intro',
_sm_say_intro_2,
transitions={
'finished':
'Move base and arm while speaking',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:332 y:184
OperatableStateMachine.add('Move base and arm while speaking',
_sm_move_base_and_arm_while_speaking_1,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
# x:1127 y:126, x:210 y:362
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:293 y:96
OperatableStateMachine.add('MoveHome',
SrdfStateToMoveit(
config_name="HomePos",
move_group="manipulator",
action_topic='/move_group',
robot_name="ur5e"),
transitions={
'reached': 'MoveUp',
'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:570 y:89
OperatableStateMachine.add('MoveUp',
SrdfStateToMoveit(
config_name="GrabPhotoPos",
move_group="manipulator",
action_topic='/move_group',
robot_name="ur5e"),
transitions={
'reached': 'Move DropPos',
'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:826 y:163
OperatableStateMachine.add('Move DropPos',
SrdfStateToMoveit(
config_name="DropPhotoPos",
move_group="manipulator",
action_topic='/move_group',
robot_name="ur5e"),
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'
})
return _state_machine
def create(self):
control_topic = 'control'
voice_topic = 'control'
joint_trajectory_action = 'motion/controller/joint_trajectory'
joy_topic = '/hmi/joystick'
moveit_action = 'move_group'
storage = 'joint_trajectory'
# x:157 y:271, x:501 y:342
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.joint_head_turned_right = [0.0, 0.6, 0.0, 0.0]
_state_machine.userdata.head_forward_joints = [0.15, 0.0, -0.15, 0.0]
_state_machine.userdata.unused = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:803 y:85, x:480 y:341
_sm_turnright_0 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['joint_head_turned_right'])
with _sm_turnright_0:
# x:282 y:82
OperatableStateMachine.add(
'TurnHead',
MoveitToJointsState(
move_group='head',
joint_names=['joint51', 'joint52', 'joint53', 'joint54'],
action_topic=moveit_action),
transitions={
'reached': 'TurnBody',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={'joint_config': 'joint_head_turned_right'})
# x:466 y:69
OperatableStateMachine.add('TurnBody',
CompoundAction(
t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='turn_right',
t2=[1, 0.0],
type2='motion/joint_trajectory',
cmd2='turn_right',
t3=[2, 0.0],
type3='motion/joint_trajectory',
cmd3='turn_right',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'finished',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:30 y:353, x:148 y:355, x:529 y:348, x:443 y:347, x:840 y:329, x:752 y:347
_sm_waitbegining_1 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['unused'],
conditions=[('finished', [('WaitKey', 'received')]),
('failed', [('WaitKey', 'unavailable')]),
('failed', [('RandHeadMovemets', 'failed')]),
('finished', [('RandHeadMovemets', 'done')])])
with _sm_waitbegining_1:
# x:167 y:134
OperatableStateMachine.add('WaitKey',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'finished',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:442 y:126
OperatableStateMachine.add('RandHeadMovemets',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=600,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'unused'})
with _state_machine:
# x:25 y:176
OperatableStateMachine.add('WaitBegining',
_sm_waitbegining_1,
transitions={
'finished': 'HeadPose',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'unused': 'unused'})
# x:343 y:49
OperatableStateMachine.add('WaitKey',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'TurnRight',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:785 y:172
OperatableStateMachine.add('AskAboutProto1',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='what_proto1',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_suprised_slow',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey2',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:799 y:264
OperatableStateMachine.add('WaitKey2',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'AskAboutProto1Again',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:797 y:379
OperatableStateMachine.add('AskAboutProto1Again',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='tell_me_more_about_proto1',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_lean_forward_begin',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey4',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:801 y:518
OperatableStateMachine.add(
'WaitKey4',
WaitForMessageState(topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'AskWhatHappensToProto1',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:603 y:600
OperatableStateMachine.add('AskWhatHappensToProto1',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='what_happened_with_proto1',
t2=[0, 1.2],
type2='motion/joint_trajectory',
cmd2='hoof_wave',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey3',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:332 y:601
OperatableStateMachine.add('WaitKey3',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'SayItIsOk',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:554 y:51
OperatableStateMachine.add('TurnRight',
_sm_turnright_0,
transitions={
'finished': 'WatchPresentaion',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'joint_head_turned_right':
'joint_head_turned_right'
})
# x:81 y:597
OperatableStateMachine.add('SayItIsOk',
CompoundAction(
t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='head_lean_forward_end',
t2=[0, 0.0],
type2='voice/play_wav',
cmd2='yes_exactly_tell_me_more',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WatchPresentaion_2',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:137 y:50
OperatableStateMachine.add(
'Hello',
CompoundAction(t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='introduce_herself',
t2=[0, 0.0],
type2='voice/play_wav',
cmd2='hello_im_sweetie_bot_presentation',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:166 y:159
OperatableStateMachine.add('HeadPose',
SrdfStateToMoveit(
config_name='head_basic',
move_group='head',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'Hello',
'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:776 y:30
OperatableStateMachine.add(
'WatchPresentaion',
self.use_behavior(WatchPresentaionSM,
'WatchPresentaion',
default_keys=['rand_head_config']),
transitions={
'finished': 'AskAboutProto1',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'head_pose_joints': 'joint_head_turned_right',
'rand_head_config': 'rand_head_config'
})
# x:92 y:465
OperatableStateMachine.add(
'WatchPresentaion_2',
self.use_behavior(WatchPresentaionSM,
'WatchPresentaion_2',
default_keys=['rand_head_config']),
transitions={
'finished': 'Greeting',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'head_pose_joints': 'head_forward_joints',
'rand_head_config': 'rand_head_config'
})
# x:73 y:332
OperatableStateMachine.add(
'Greeting',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='greeting',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
return _state_machine
def create(self):
leap_topic = '/hmi/leap_motion/data'
eyes_cmd_topic = 'control'
voice_topic = 'control'
leap_pose_topic = '/hmi/leap_motion/pose'
# x:286 y:634, x:989 y:651
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.be_evil = False
_state_machine.userdata.pose = PoseStamped()
_state_machine.userdata.unused = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:353, x:343 y:356, x:209 y:349, x:843 y:453, x:473 y:355, x:638 y:361, x:907 y:552
_sm_randheadmovements_0 = ConcurrencyContainer(outcomes=['object_detected', 'failed'], input_keys=['unused'], conditions=[
('object_detected', [('WaitUntlObject', 'still_object')]),
('object_detected', [('WaitUntlObject', 'moving_object')]),
('failed', [('WaitUntlObject', 'no_object')]),
('object_detected', [('RandHeadMoveme', 'done')]),
('failed', [('RandHeadMoveme', 'failed')])
])
with _sm_randheadmovements_0:
# x:158 y:110
OperatableStateMachine.add('WaitUntlObject',
LeapMotionMonitor(leap_motion_topic=leap_topic, exit_states=['still_object', 'moving_object'], pose_topic=leap_pose_topic, parameters=[2.0,0.02,0.2]),
transitions={'no_object': 'failed', 'still_object': 'object_detected', 'moving_object': 'object_detected'},
autonomy={'no_object': Autonomy.Off, 'still_object': Autonomy.Off, 'moving_object': Autonomy.Off},
remapping={'pose': 'pose'})
# x:605 y:103
OperatableStateMachine.add('RandHeadMoveme',
SweetieBotRandHeadMovements(controller='joint_state_head', duration=120, interval=[1,4], max2356=[0.3,0.5,1,1], min2356=[-0.3,-0.1,-1,-1]),
transitions={'done': 'object_detected', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'config': 'unused'})
# x:30 y:353, x:357 y:355, x:671 y:353, x:140 y:353, x:237 y:356, x:530 y:353, x:742 y:441, x:730 y:353, x:452 y:353, x:732 y:406
_sm_waitnoobjecttimeout_1 = ConcurrencyContainer(outcomes=['no_object', 'timeout', 'failure', 'too_close'], conditions=[
('no_object', [('WaitNoObject', 'no_object')]),
('failure', [('WaitNoObject', 'moving_object')]),
('failure', [('WaitNoObject', 'still_object')]),
('failure', [('FollowObject', 'failed')]),
('timeout', [('Wait', 'done')]),
('too_close', [('FollowObject', 'too_close')])
])
with _sm_waitnoobjecttimeout_1:
# x:162 y:101
OperatableStateMachine.add('WaitNoObject',
LeapMotionMonitor(leap_motion_topic=leap_topic, exit_states=['no_object'], pose_topic=leap_pose_topic, parameters=[4.0,0.02,0.2]),
transitions={'no_object': 'no_object', 'still_object': 'failure', 'moving_object': 'failure'},
autonomy={'no_object': Autonomy.Off, 'still_object': Autonomy.Off, 'moving_object': Autonomy.Off},
remapping={'pose': 'pose'})
# x:463 y:95
OperatableStateMachine.add('FollowObject',
SweetieBotFollowHeadPoseSmart(pose_topic=leap_pose_topic, follow_joint_state_controller='joint_state_head', discomfort_time=4, neck_control_parameteres=[-0.13,0.3,0.20,0.2], deactivate=True, controlled_chains=['eyes','head']),
transitions={'failed': 'failure', 'too_close': 'too_close'},
autonomy={'failed': Autonomy.Off, 'too_close': Autonomy.Off})
# x:332 y:94
OperatableStateMachine.add('Wait',
WaitState(wait_time=self.play_timeout),
transitions={'done': 'timeout'},
autonomy={'done': Autonomy.Off})
# x:30 y:353, x:130 y:353, x:330 y:366, x:737 y:355, x:898 y:596, x:899 y:486, x:900 y:533, x:902 y:443, x:568 y:345
_sm_watchonleapmotionobject_2 = ConcurrencyContainer(outcomes=['no_object', 'still_object', 'failed', 'to_close'], output_keys=['pose'], conditions=[
('no_object', [('WaitTillObjectMOving', 'no_object')]),
('still_object', [('WaitTillObjectMOving', 'still_object')]),
('failed', [('WaitTillObjectMOving', 'moving_object')]),
('failed', [('HeadFollowsMovingObject', 'failed')]),
('to_close', [('HeadFollowsMovingObject', 'too_close')])
])
with _sm_watchonleapmotionobject_2:
# x:135 y:135
OperatableStateMachine.add('WaitTillObjectMOving',
LeapMotionMonitor(leap_motion_topic='/hmi/leap_motion/data', exit_states=['no_object', 'still_object'], pose_topic='/hmi/leap_motion/pose', parameters=[1.0,0.02,0.2]),
transitions={'no_object': 'no_object', 'still_object': 'still_object', 'moving_object': 'failed'},
autonomy={'no_object': Autonomy.High, 'still_object': Autonomy.High, 'moving_object': Autonomy.Off},
remapping={'pose': 'pose'})
# x:583 y:138
OperatableStateMachine.add('HeadFollowsMovingObject',
SweetieBotFollowHeadPoseSmart(pose_topic='/hmi/leap_motion/pose', follow_joint_state_controller='joint_state_head', discomfort_time=4.0, neck_control_parameteres=[-0.13,0.3,0.20,0.2], deactivate=True, controlled_chains=['head','eyes']),
transitions={'failed': 'failed', 'too_close': 'to_close'},
autonomy={'failed': Autonomy.Off, 'too_close': Autonomy.High})
# x:979 y:146, x:969 y:327
_sm_switchevil_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['be_evil'], output_keys=['be_evil'])
with _sm_switchevil_3:
# x:269 y:101
OperatableStateMachine.add('CheckState',
DecisionState(outcomes=[ 'evil', 'good' ], conditions=lambda x: 'evil' if x else 'good'),
transitions={'evil': 'NewState1', 'good': 'NewState2'},
autonomy={'evil': Autonomy.Off, 'good': Autonomy.Off},
remapping={'input_value': 'be_evil'})
# x:423 y:54
OperatableStateMachine.add('NewState1',
CalculationState(calculation=lambda x: not random.random() < self.evil_to_good_switch_probability),
transitions={'done': 'SwitchEvilMode'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'be_evil', 'output_value': 'new_be_evil'})
# x:425 y:159
OperatableStateMachine.add('NewState2',
CalculationState(calculation=lambda x: random.random() < self.good_to_evil_switch_probability),
transitions={'done': 'SwitchEvilMode'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'be_evil', 'output_value': 'new_be_evil'})
# x:678 y:91
OperatableStateMachine.add('SwitchEvilMode',
self.use_behavior(SwitchEvilModeSM, 'SwitchEvil/SwitchEvilMode'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil', 'new_be_evil': 'new_be_evil'})
with _state_machine:
# x:114 y:550
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(config_name='head_upright', move_group='head', action_topic='move_group', robot_name=''),
transitions={'reached': 'RandHeadMovements', '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:500 y:88
OperatableStateMachine.add('Brohoof',
self.use_behavior(BrohoofSM, 'Brohoof'),
transitions={'finished': 'WaitNoObjectTimeout', 'unreachable': 'Greeting', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'pose'})
# x:699 y:28
OperatableStateMachine.add('Greeting',
self.use_behavior(GreetingSM, 'Greeting'),
transitions={'finished': 'WaitNoObjectTimeout', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:455 y:230
OperatableStateMachine.add('Play',
self.use_behavior(PlaySM, 'Play'),
transitions={'finished': 'WaitNoObjectTimeout', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:78 y:247
OperatableStateMachine.add('SwitchEvil',
_sm_switchevil_3,
transitions={'finished': 'WatchOnLeapMotionObject', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:387 y:6
OperatableStateMachine.add('CheckEvil',
DecisionState(outcomes=['evil','good'], conditions=lambda x: 'evil' if x else 'good'),
transitions={'evil': 'Greeting', 'good': 'Brohoof'},
autonomy={'evil': Autonomy.Off, 'good': Autonomy.Off},
remapping={'input_value': 'be_evil'})
# x:144 y:77
OperatableStateMachine.add('WatchOnLeapMotionObject',
_sm_watchonleapmotionobject_2,
transitions={'no_object': 'Play', 'still_object': 'CheckEvil', 'failed': 'failed', 'to_close': 'Bad'},
autonomy={'no_object': Autonomy.Inherit, 'still_object': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'to_close': Autonomy.Inherit},
remapping={'pose': 'pose'})
# x:283 y:293
OperatableStateMachine.add('Bad',
self.use_behavior(BadSM, 'Bad'),
transitions={'finished': 'SwitchEvil', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:646 y:375
OperatableStateMachine.add('WaitNoObjectTimeout',
_sm_waitnoobjecttimeout_1,
transitions={'no_object': 'PlaceHead', 'timeout': 'Play', 'failure': 'failed', 'too_close': 'Bad'},
autonomy={'no_object': Autonomy.Inherit, 'timeout': Autonomy.Inherit, 'failure': Autonomy.Inherit, 'too_close': Autonomy.Inherit})
# x:106 y:411
OperatableStateMachine.add('RandHeadMovements',
_sm_randheadmovements_0,
transitions={'object_detected': 'SwitchEvil', 'failed': 'failed'},
autonomy={'object_detected': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'unused': 'unused'})
return _state_machine
def create(self):
# x:1190 y:670, x:11 y:681
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365
_sm_vstapitelno_slovo_0 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('finished', [('govori', 'done'),
('give time to speak', 'done')]),
('failed', [('govori', 'failed')])])
with _sm_vstapitelno_slovo_0:
# x:86 y:149
OperatableStateMachine.add(
'govori',
TTSBulgarian(
ttsbg_text=
'Нека Ви се представя, аз съм Телеуправляемия Сервизен Робот Робко 19. За мен е чест да участвам в ГОДИШНАТА МЕЖДУНАРОДНА НАУЧНА КОНФЕРЕНЦИЯ НА АВИАЦИОННИЯ ФАКУЛТЕТ. Създаден съм от групата по сервизна роботика по проект, Финансиран от Фонда Научни изследвания. Сега ще направя кратка демонстрация на част от моите възможности.'
),
transitions={
'failed': 'failed',
'done': 'finished'
},
autonomy={
'failed': Autonomy.Off,
'done': Autonomy.Off
})
# x:282 y:111
OperatableStateMachine.add('give time to speak',
WaitState(wait_time=20),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:844 y:69, x:586 y:560, x:242 y:373, x:16 y:367, x:156 y:366, x:578 y:490, x:857 y:563
_sm_move_to_pick_from_small_table_1 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('finished', [
('Move to pick_from_small_table', 'reached'),
('Timer give time for motion to execute', 'done'),
('say text', 'done')
]), ('failed', [('Move to pick_from_small_table', 'param_error')]),
('failed', [('Move to pick_from_small_table',
'planning_failed')]),
('failed', [('Move to pick_from_small_table',
'control_failed')]),
('failed', [('say text', 'failed')])])
with _sm_move_to_pick_from_small_table_1:
# x:71 y:40
OperatableStateMachine.add('Move to pick_from_small_table',
SrdfStateToMoveit(
config_name="pick_from_small_table",
move_group="robot",
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:312 y:116
OperatableStateMachine.add('Timer give time for motion to execute',
WaitState(wait_time=12),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:538 y:206
OperatableStateMachine.add(
'say text',
TTSBulgarian(ttsbg_text="Придвижвам ръката надолу и в дясно"),
transitions={
'failed': 'failed',
'done': 'finished'
},
autonomy={
'failed': Autonomy.Off,
'done': Autonomy.Off
})
# x:851 y:66, x:441 y:472, x:63 y:338, x:277 y:369, x:179 y:364, x:374 y:469, x:630 y:365
_sm_move_back_to_transport_position_2 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[
('finished', [('Timer give time for motion to execute',
'done'),
('move to transport_position', 'reached'),
('say text', 'done')]),
('failed', [('move to transport_position', 'planning_failed')
]),
('failed', [('move to transport_position', 'control_failed')]),
('failed', [('move to transport_position', 'param_error')]),
('failed', [('say text', 'failed')])
])
with _sm_move_back_to_transport_position_2:
# x:118 y:45
OperatableStateMachine.add('move to transport_position',
SrdfStateToMoveit(
config_name="transport_position",
move_group="robot",
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:310 y:128
OperatableStateMachine.add('Timer give time for motion to execute',
WaitState(wait_time=15),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:525 y:170
OperatableStateMachine.add(
'say text',
TTSBulgarian(ttsbg_text="Връщам ръката в изходн позиция."),
transitions={
'failed': 'failed',
'done': 'finished'
},
autonomy={
'failed': Autonomy.Off,
'done': Autonomy.Off
})
# x:614 y:41, x:129 y:345, x:179 y:346, x:347 y:338, x:430 y:365, x:530 y:365, x:630 y:365
_sm_move_to_place_on_tall_table_3 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('finished', [
('Move to place_on_tall_table', 'reached'),
('Timer give time for motion to execute', 'done'),
('say text', 'done')
]), ('failed', [
('Move to place_on_tall_table', 'planning_failed')
]), ('failed', [
('Move to place_on_tall_table', 'control_failed')
]), ('failed', [('Move to place_on_tall_table', 'param_error')]),
('failed', [('say text', 'failed')])])
with _sm_move_to_place_on_tall_table_3:
# x:30 y:40
OperatableStateMachine.add('Move to place_on_tall_table',
SrdfStateToMoveit(
config_name="place_on_tall_table",
move_group="robot",
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:277 y:66
OperatableStateMachine.add('Timer give time for motion to execute',
WaitState(wait_time=14),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:410 y:128
OperatableStateMachine.add(
'say text',
TTSBulgarian(ttsbg_text="Сега ще придвижа ръката наляво"),
transitions={
'failed': 'failed',
'done': 'finished'
},
autonomy={
'failed': Autonomy.Off,
'done': Autonomy.Off
})
# x:296 y:148, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365, x:530 y:365, x:630 y:365
_sm_move_to_reach_up_position_4 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[
('finished', [('Move to reach_up position', 'reached'),
('Timer give time for motion to execute',
'done'), ('say text', 'done')]),
('failed', [('Move to reach_up position', 'planning_failed')]),
('failed', [('Move to reach_up position', 'control_failed')]),
('failed', [('Move to reach_up position', 'param_error')]),
('failed', [('say text', 'failed')])
])
with _sm_move_to_reach_up_position_4:
# x:30 y:40
OperatableStateMachine.add('Move to reach_up position',
SrdfStateToMoveit(
config_name="reach_up",
move_group="robot",
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:246 y:38
OperatableStateMachine.add('Timer give time for motion to execute',
WaitState(wait_time=16.3),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:562 y:44
OperatableStateMachine.add(
'say text',
TTSBulgarian(
ttsbg_text="Сега ще придвижа ръката нагоре и напред"),
transitions={
'failed': 'failed',
'done': 'finished'
},
autonomy={
'failed': Autonomy.Off,
'done': Autonomy.Off
})
# x:825 y:119, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365, x:615 y:337, x:630 y:378
_sm_move_to_transport_position_5 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[
('finished', [('Timer give time for motion to execute',
'done'),
('Move to transport_position', 'reached'),
('Say text in Bulgarian', 'done')]),
('failed', [('Move to transport_position', 'param_error')]),
('failed', [('Move to transport_position', 'control_failed')]),
('failed', [('Move to transport_position', 'planning_failed')
]),
('failed', [('Say text in Bulgarian', 'failed')])
])
with _sm_move_to_transport_position_5:
# x:37 y:92
OperatableStateMachine.add('Move to transport_position',
SrdfStateToMoveit(
config_name="transport_position",
move_group="robot",
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:294 y:38
OperatableStateMachine.add('Timer give time for motion to execute',
WaitState(wait_time=10),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:387 y:156
OperatableStateMachine.add(
'Say text in Bulgarian',
TTSBulgarian(
ttsbg_text=
"Сега ще Ви демонстрирам някои движения на ръката си."),
transitions={
'failed': 'failed',
'done': 'finished'
},
autonomy={
'failed': Autonomy.Off,
'done': Autonomy.Off
})
with _state_machine:
# x:32 y:34
OperatableStateMachine.add('Vstapitelno slovo',
_sm_vstapitelno_slovo_0,
transitions={
'finished':
'Move to transport position',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:248 y:156
OperatableStateMachine.add('Move to reach_up position',
_sm_move_to_reach_up_position_4,
transitions={
'finished':
'Move to place_on_tall_table',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:263 y:253
OperatableStateMachine.add('Move to place_on_tall_table',
_sm_move_to_place_on_tall_table_3,
transitions={
'finished':
'Move to pick_from_small_table',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:651 y:612
OperatableStateMachine.add('Move back to transport_position',
_sm_move_back_to_transport_position_2,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:637 y:272
OperatableStateMachine.add('Move to pick_from_small_table',
_sm_move_to_pick_from_small_table_1,
transitions={
'finished':
'Move back to transport_position',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:252 y:63
OperatableStateMachine.add('Move to transport position',
_sm_move_to_transport_position_5,
transitions={
'finished':
'Move to reach_up position',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
return _state_machine
def create(self):
# x:942 y:70, x:835 y:318
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.JV = [
5.034934007384111, 4.749860722186541, 2.0863536874817012,
3.548160146665105, -4.830528404237258, -3.8607153900995290
]
_state_machine.userdata.JN = [
"m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3",
"m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"
]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:142 y:64
OperatableStateMachine.add(
'Get_Values',
GetJointValuesState(
joints=[
"m1n6s200_joint_1", "m1n6s200_joint_2",
"m1n6s200_joint_3", "m1n6s200_joint_4",
"m1n6s200_joint_5", "m1n6s200_joint_6"
],
topic_name="/m1n6s200_driver/joint_states"),
transitions={'retrieved': 'Log_Values'},
autonomy={'retrieved': Autonomy.Off},
remapping={'joint_values': 'joint_values'})
# x:394 y:102
OperatableStateMachine.add('Log_Values',
LogKeyState(
text='%d',
severity=Logger.REPORT_HINT),
transitions={'done': 'Go_Home'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_values'})
# x:677 y:64
OperatableStateMachine.add('Set_Specific_Vals',
MoveitToJointsDynState(
move_group="arm",
action_topic='/move_group'),
transitions={
'reached': 'finished',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Low,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'joint_values': 'JV',
'joint_names': 'JN'
})
# x:505 y:209
OperatableStateMachine.add('Go_Home',
SrdfStateToMoveit(
config_name="Home",
move_group="arm",
action_topic='/move_group',
robot_name="m1n6s200"),
transitions={
'reached': 'Set_Specific_Vals',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Low,
'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'
})
return _state_machine
def create(self):
joy_topic = '/hmi/joystick'
action_button = 12
# x:105 y:208, x:418 y:227
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.rand_head_config = {
'min2356': [-0.5, 0.1, -1.0, -1.0],
'max2356': [0.5, 0.5, 1.0, 1.0]
}
_state_machine.userdata.head_pose_joints = [0.0, 0.0, 0.0, 0.0, 0.0]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:284, x:130 y:284, x:230 y:284, x:330 y:284, x:430 y:293, x:530 y:293
_sm_waitstart_0 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['rand_head_config'],
conditions=[('finished', [('WaitKey1', 'received')]),
('failed', [('WaitKey1', 'unavailable')]),
('finished', [('RandMovements', 'done')]),
('failed', [('RandMovements', 'failed')])])
with _sm_waitstart_0:
# x:98 y:61
OperatableStateMachine.add(
'WaitKey1',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: x.buttons[action_button],
buffered=False,
clear=False),
transitions={
'received': 'finished',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:335 y:61
OperatableStateMachine.add(
'RandMovements',
SweetieBotRandHeadMovements(controller='joint_state_head',
duration=120,
interval=[1, 4],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'rand_head_config'})
with _state_machine:
# x:52 y:35
OperatableStateMachine.add('NormalEyes',
TextCommandState(type='eyes/emotion',
command='normal',
topic='control'),
transitions={'done': 'WaitStart'},
autonomy={'done': Autonomy.Off})
# x:642 y:281
OperatableStateMachine.add(
'HelloGoodThenEvil',
CompoundAction(t1=[0, 0.0],
type1='voice/play_wav',
cmd1='zdravstvuyte_dorogie_gosti',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='look_on_printer',
t3=[2, 0.0],
type3='motion/joint_trajectory',
cmd3='menace',
t4=[3, 4.0],
type4='motion/joint_trajectory',
cmd4='menace_canceled'),
transitions={
'success': 'MicrophoneTurnOffReaction',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:634 y:65
OperatableStateMachine.add('FakeApoplogy',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='a_tochno_sboi_pamayati',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_suprised',
t3=[0, 2.8],
type3='motion/joint_trajectory',
cmd3='head_node',
t4=[0, 6.5],
type4='motion/joint_trajectory',
cmd4='hoof_knock'),
transitions={
'success': 'WaitKey3',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:467 y:314
OperatableStateMachine.add(
'WaitKey4',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: x.buttons[action_button],
buffered=False,
clear=False),
transitions={
'received': 'HeadTurnRightEnd',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:253 y:412
OperatableStateMachine.add(
'FakeApologyAndMenace',
CompoundAction(t1=[0, 0.0],
type1='voice/play_wav',
cmd1='no_esli_snova_otkluchish_mikrofon',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='hoof_knock',
t3=[2, 0.0],
type3='motion/joint_trajectory',
cmd3='head_node',
t4=[0, 6.5],
type4='motion/joint_trajectory',
cmd4='begone'),
transitions={
'success': 'NormalLook',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:74 y:386
OperatableStateMachine.add(
'WaitKey5',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: x.buttons[action_button],
buffered=False,
clear=False),
transitions={
'received': 'LetsBegin',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:48 y:277
OperatableStateMachine.add('LetsBegin',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='nam_pora_nachinat',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_node',
t3=[1, 1.0],
type3='motion/joint_trajectory',
cmd3='point_right',
t4=[0, 7.0],
type4='motion/joint_trajectory',
cmd4='hoof_stamp'),
transitions={
'success': 'finished',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:439 y:35
OperatableStateMachine.add(
'WaitKey2',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: x.buttons[action_button],
buffered=False,
clear=False),
transitions={
'received': 'FakeApoplogy',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:236 y:35
OperatableStateMachine.add('HelloEvil',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='zdravstvuyte_kojannie_meshki',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='greeting',
t3=[0, 0.0],
type3='eyes/emotion',
cmd3='red_eyes',
t4=[0, 5.5],
type4='motion/joint_trajectory',
cmd4='applause'),
transitions={
'success': 'WaitKey2',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:243 y:129
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(
config_name='head_basic',
move_group='head',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'HelloEvil',
'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:73 y:124
OperatableStateMachine.add(
'WaitStart',
_sm_waitstart_0,
transitions={
'finished': 'PlaceHead',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'rand_head_config': 'rand_head_config'})
# x:642 y:365
OperatableStateMachine.add('MicrophoneTurnOffReaction',
CompoundAction(
t1=[0, 0.0],
type1='eyes/emotion',
cmd1='evil_look',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_turn_right_begin',
t3=[2, 0.0],
type3='motion/joint_trajectory',
cmd3='hoof_stamp',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey4',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:231 y:312
OperatableStateMachine.add('NormalLook',
CompoundAction(t1=[0, 0.0],
type1='eyes/emotion',
cmd1='normal',
t2=[1, 0.0],
type2='eyes/emotion',
cmd2='red_eyes',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey5',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:452 y:401
OperatableStateMachine.add('HeadTurnRightEnd',
CompoundAction(
t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='head_turn_right_end',
t2=[0, 0.0],
type2=None,
cmd2='',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'FakeApologyAndMenace',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:628 y:165
OperatableStateMachine.add(
'WaitKey3',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: x.buttons[action_button],
buffered=False,
clear=False),
transitions={
'received': 'HelloGoodThenEvil',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
return _state_machine
def create(self):
pick_group = 'manipulator'
names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'
]
gripper = 'epick_gripper_BasePickPointLink'
gripper_device = '/dev/ttyUSB0'
# x:48 y:593, x:317 y:372
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.captured_pointcloud = []
_state_machine.userdata.part_pose = []
_state_machine.userdata.pick_configuration = []
_state_machine.userdata.suction_cup_offset = 0.00
_state_machine.userdata.rotation = 0
_state_machine.userdata.move_group_prefix = ''
_state_machine.userdata.move_group = "manipulator"
_state_machine.userdata.ee_link = 'epick_gripper_BasePickPointLink'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:102
OperatableStateMachine.add('ActivateGripper',
EpickVacuumGripperControlState(
device=gripper_device,
gripper_command='a',
setteling_time=0),
transitions={
'continue': 'GoHomeStart',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:428 y:570
OperatableStateMachine.add('GoHomeEnd',
SrdfStateToMoveit(
config_name='HomePos',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'ResetGripper',
'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:217 y:38
OperatableStateMachine.add('GoPhotoPosition',
SrdfStateToMoveit(
config_name='PhotoPos',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'CapturePointcloud',
'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:743 y:39
OperatableStateMachine.add('GoPreGraspPosition',
SrdfStateToMoveit(
config_name='PreGraspPos',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'ComputePickPoint',
'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:911 y:297
OperatableStateMachine.add('GoObjectLiftPosition',
SrdfStateToMoveit(
config_name='PreGraspPos',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'GoHomeTransfer',
'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:909 y:393
OperatableStateMachine.add('GoHomeTransfer',
SrdfStateToMoveit(
config_name='HomePos',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'GoDropPosition',
'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:908 y:482
OperatableStateMachine.add('GoDropPosition',
SrdfStateToMoveit(
config_name='DropPos',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'DropObject',
'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:384 y:38
OperatableStateMachine.add(
'CapturePointcloud',
CapturePointcloudState(),
transitions={
'continue': 'CalculateObjectPose',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'pointcloud': 'captured_pointcloud'})
# x:569 y:37
OperatableStateMachine.add('CalculateObjectPose',
CalculateObjectPoseState(),
transitions={
'continue': 'PoseMessage',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'pointcloud': 'captured_pointcloud',
'object_pose': 'part_pose'
})
# x:671 y:137
OperatableStateMachine.add(
'PoseMessage',
MessageState(),
transitions={'continue': 'GoPreGraspPosition'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'part_pose'})
# x:920 y:41
OperatableStateMachine.add('ComputePickPoint',
ComputeGraspState(joint_names=names,
time_out=3.0),
transitions={
'continue': 'MoveToPick',
'failed': 'failed',
'time_out': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'time_out': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'move_group_prefix':
'move_group_prefix',
'tool_link': 'ee_link',
'pose': 'part_pose',
'offset': 'suction_cup_offset',
'rotation': 'rotation',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:914 y:125
OperatableStateMachine.add(
'MoveToPick',
binpicking_flexbe_states__MoveitToJointsDynState(),
transitions={
'reached': 'GrapsObject',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={
'move_group_prefix': 'move_group_prefix',
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:911 y:206
OperatableStateMachine.add('GrapsObject',
EpickVacuumGripperControlState(
device=gripper_device,
gripper_command='g',
setteling_time=3.0),
transitions={
'continue': 'GoObjectLiftPosition',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:906 y:569
OperatableStateMachine.add('DropObject',
EpickVacuumGripperControlState(
device=gripper_device,
gripper_command='c',
setteling_time=1.0),
transitions={
'continue': 'GoHomeEnd',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:170 y:574
OperatableStateMachine.add('ResetGripper',
EpickVacuumGripperControlState(
device=gripper_device,
gripper_command='r',
setteling_time=0),
transitions={
'continue': 'finished',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:30 y:40
OperatableStateMachine.add('GoHomeStart',
SrdfStateToMoveit(
config_name='HomePos',
move_group=pick_group,
action_topic='/move_group',
robot_name=""),
transitions={
'reached': 'GoPhotoPosition',
'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'
})
return _state_machine
def create(self):
moveit_action = 'move_group'
control_topic = 'control'
# x:156 y:182, x:526 y:284
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.head_normal_joints = [
0.15, 0.0, -0.15, 0.0, 0.0, 0.0
]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:89 y:62
OperatableStateMachine.add('HeadBasicPose',
SrdfStateToMoveit(
config_name='head_upright',
move_group='head',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'WakeUp',
'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:750 y:48
OperatableStateMachine.add('HeadBasicPose2',
SrdfStateToMoveit(
config_name='head_upright',
move_group='head',
action_topic=moveit_action,
robot_name=''),
transitions={
'reached': 'Menace',
'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:750 y:149
OperatableStateMachine.add('Menace',
CompoundAction(
t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='turn_right',
t2=[0, 0.0],
type2='voice/play_wav',
cmd2='you_can_not_imagine',
t3=[1, 0.0],
type3='motion/joint_trajectory',
cmd3='turn_right',
t4=[3, 0.0],
type4='motion/joint_trajectory',
cmd4='menace'),
transitions={
'success': 'EyesNormal',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Full,
'failure': Autonomy.Off
})
# x:523 y:43
OperatableStateMachine.add('TurnRight1',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='i_will_not_tolerate',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='turn_right',
t3=[2, 0.0],
type3='eyes/emotion',
cmd3='red_eyes',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'HeadBasicPose2',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:287 y:50
OperatableStateMachine.add('WakeUp',
CompoundAction(
t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='head_shake_emphasized',
t2=[1, 0.0],
type2='motion/joint_trajectory',
cmd2='head_turn_right',
t3=[2, 0.0],
type3='voice/play_wav',
cmd3='it_was_rude',
t4=[2, 1.6],
type4=None,
cmd4='i_will_not_tolearate'),
transitions={
'success': 'TurnRight1',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:752 y:298
OperatableStateMachine.add('EyesNormal',
TextCommandState(type='eyes/emotion',
command='normal',
topic=control_topic),
transitions={'done': 'MenaceCanceled'},
autonomy={'done': Autonomy.Full})
# x:714 y:453
OperatableStateMachine.add('MenaceCanceled',
CompoundAction(
t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='menace_canceled',
t2=[1, 0.0],
type2='motion/joint_trajectory',
cmd2='look_around',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'Wait',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:520 y:608
OperatableStateMachine.add(
'Hello',
CompoundAction(t1=[0, 0.0],
type1='voice/play_wav',
cmd1='hello_im_sweetie_bot_presentation',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='introduce_herself',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WatchPresentaion',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:768 y:606
OperatableStateMachine.add('Wait',
WaitState(wait_time=1),
transitions={'done': 'Hello'},
autonomy={'done': Autonomy.Off})
# x:297 y:596
OperatableStateMachine.add(
'WatchPresentaion',
self.use_behavior(WatchPresentaionSM,
'WatchPresentaion',
default_keys=['rand_head_config']),
transitions={
'finished': 'IsEnd',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'head_pose_joints': 'head_normal_joints',
'rand_head_config': 'rand_head_config'
})
# x:65 y:572
OperatableStateMachine.add(
'IsEnd',
CompoundAction(t1=[0, 0.0],
type1='voice/play_wav',
cmd1='is_it_over_you_promised_an_hour',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_suprised',
t3=[0, 6.0],
type3='voice/play_wav',
cmd3='i_do_not_understand_anything',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'HeadBasic3',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:84 y:425
OperatableStateMachine.add('HeadBasic3',
SrdfStateToMoveit(
config_name='head_upright',
move_group='head',
action_topic=moveit_action,
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'
})
return _state_machine
