def create(self):
# x:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.jn = [-0.034, -24.0]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:382 y:109
OperatableStateMachine.add('iiii',
MoveitToJointsState(
move_group="robot",
joint_names=["Joint1", "Joint2"],
action_topic='/move_group'),
transitions={
'reached': 'failed',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Low,
'control_failed': Autonomy.Low
},
remapping={'joint_config': 'jn'})
return _state_machine
def create(self):
hello = "baka!!"
# x:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.target_joint = [0, 0, 0]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Initial_Wait',
WaitState(wait_time=self.waiting_time),
transitions={'done': 'test_moveit'},
autonomy={'done': Autonomy.Off})
# x:27 y:171
OperatableStateMachine.add('Print_Greeting',
LogState(text=hello,
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.High})
# x:172 y:148
OperatableStateMachine.add(
'test_moveit',
MoveitToJointsState(move_group='Gantry',
joint_names=[
"small_long_joint", "torso_rail_joint",
"torso_base_main_joint"
],
action_topic='/ariac/gantry/move_group'),
transitions={
'reached': 'Print_Greeting',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={'joint_config': 'target_joint'})
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):
# x:26 y:401, x:564 y:234
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.pick_waypoint = Pose2D(-0.8, -3.65, -1.57)
_state_machine.userdata.place_waypoint = Pose2D(-8.0, 4.13, 3.14)
_state_machine.userdata.picking_state = [
0.385, 0.476, 1.266, -0.722, 1.954, -1.708, 2.0197, -1.415
]
_state_machine.userdata.prepare_state = [
0.337, 1.334, 1.328, -0.145, 1.811, 0.0, 1.639, 0.042
]
_state_machine.userdata.start_point = Point(0.48, 0.0, 0.5)
_state_machine.userdata.look_up_point = Point(2.0, 0.0, 1.1)
_state_machine.userdata.see_apriltag = Point(0.8, 0.3, 0.7)
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:46 y:23
OperatableStateMachine.add('look_down',
HeadActionState(),
transitions={
'head_arrived': 'nav_pick',
'command_error': 'failed'
},
autonomy={
'head_arrived': Autonomy.Off,
'command_error': Autonomy.Off
},
remapping={'point2see': 'start_point'})
# x:753 y:23
OperatableStateMachine.add('detect_obj',
DetectObjState(),
transitions={
'continue': 'pick_obj',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1056 y:539
OperatableStateMachine.add(
'nav_place',
MoveBaseState(),
transitions={
'arrived': 'find_apriltag',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'place_waypoint'})
# x:469 y:456
OperatableStateMachine.add('detect_plane',
DetectPlaneState(),
transitions={
'continue': 'place_obj',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1042 y:207
OperatableStateMachine.add(
'pick_moving_state',
MoveitToJointsState(
move_group='arm_with_torso',
joint_names=[
'torso_lift_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
],
action_topic='/move_group'),
transitions={
'reached': 'look_up',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={'joint_config': 'picking_state'})
# x:527 y:24
OperatableStateMachine.add(
'prepare_state',
MoveitToJointsState(
move_group='arm_with_torso',
joint_names=[
'torso_lift_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
],
action_topic='/move_group'),
transitions={
'reached': 'detect_obj',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={'joint_config': 'prepare_state'})
# x:305 y:24
OperatableStateMachine.add('nav_pick',
MoveBaseState(),
transitions={
'arrived': 'prepare_state',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'waypoint': 'pick_waypoint'})
# x:1050 y:399
OperatableStateMachine.add(
'look_up',
HeadActionState(),
transitions={
'head_arrived': 'nav_place',
'command_error': 'failed'
},
autonomy={
'head_arrived': Autonomy.Off,
'command_error': Autonomy.Off
},
remapping={'point2see': 'look_up_point'})
# x:739 y:458
OperatableStateMachine.add('find_apriltag',
HeadActionState(),
transitions={
'head_arrived': 'detect_plane',
'command_error': 'failed'
},
autonomy={
'head_arrived': Autonomy.Off,
'command_error': Autonomy.Off
},
remapping={'point2see': 'see_apriltag'})
# x:216 y:456
OperatableStateMachine.add('place_obj',
PlaceState(),
transitions={
'continue': 'finished_pose',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:1041 y:23
OperatableStateMachine.add('pick_obj',
PickState(),
transitions={
'continue': 'pick_moving_state',
'failed': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off
})
# x:211 y:318
OperatableStateMachine.add(
'finished_pose',
MoveitToJointsState(
move_group='arm_with_torso',
joint_names=[
'torso_lift_joint', 'shoulder_pan_joint',
'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
],
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': 'picking_state'})
return _state_machine
def create(self):
# x:26 y:401, x:564 y:234
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.pick_waypoint = Pose2D(-1.057, 5.051, 1.826)
_state_machine.userdata.place_waypoint = Pose2D(-10.65, -17.47, -1.468)
_state_machine.userdata.picking_state = [0.385, 0.476, 1.266, -0.722, 1.954, -1.708, 2.0197, -1.415]
_state_machine.userdata.prepare_state = [0.337, 1.334, 1.328, -0.145, 1.811, 0.0, 1.639, 0.042]
_state_machine.userdata.start_point = Point(0.8, 0.0, 0.6)
_state_machine.userdata.look_up_point = Point(2.0, 0.0, 1.1)
_state_machine.userdata.see_apriltag = Point(0.8, 0.2, 0.7)
_state_machine.userdata.init_joints = [0.3675, 1.5585, 1.421, -0.1457, 1.1609, -0.027, 1.966, -1.371]
_state_machine.userdata.prepare_state2 = [1.334, 1.328, -0.145, 1.811, 0.0, 1.639, 0.042]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:25 y:72
OperatableStateMachine.add('init_arm_pose',
MoveitToJointsState(move_group='arm_with_torso', joint_names=['torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'], action_topic='/move_group'),
transitions={'reached': 'look_down', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'prepare_state'})
# x:1102 y:44
OperatableStateMachine.add('detect_obj',
DetectObjState(),
transitions={'continue': 'pick_obj', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:820 y:46
OperatableStateMachine.add('prepare_state',
MoveitToJointsState(move_group='arm_with_torso', joint_names=['torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'], action_topic='/move_group'),
transitions={'reached': 'detect_obj', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'prepare_state'})
# x:587 y:53
OperatableStateMachine.add('nav_pick',
MoveBaseState(),
transitions={'arrived': 'prepare_state', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'pick_waypoint'})
# x:1103 y:510
OperatableStateMachine.add('look_up',
HeadActionState(),
transitions={'head_arrived': 'nav_place', 'command_error': 'failed'},
autonomy={'head_arrived': Autonomy.Off, 'command_error': Autonomy.Off},
remapping={'point2see': 'look_up_point'})
# x:1095 y:214
OperatableStateMachine.add('pick_obj',
PickState(),
transitions={'continue': 'pick_moving_state', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:292 y:61
OperatableStateMachine.add('look_down',
HeadActionState(),
transitions={'head_arrived': 'nav_pick', 'command_error': 'failed'},
autonomy={'head_arrived': Autonomy.Off, 'command_error': Autonomy.Off},
remapping={'point2see': 'start_point'})
# x:537 y:507
OperatableStateMachine.add('find_apriltag',
HeadActionState(),
transitions={'head_arrived': 'detect_plane', 'command_error': 'failed'},
autonomy={'head_arrived': Autonomy.Off, 'command_error': Autonomy.Off},
remapping={'point2see': 'see_apriltag'})
# x:271 y:511
OperatableStateMachine.add('detect_plane',
DetectPlaneState(),
transitions={'continue': 'place_obj', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:178 y:396
OperatableStateMachine.add('place_obj',
PlaceState(),
transitions={'continue': 'finished_state', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1125 y:372
OperatableStateMachine.add('pick_moving_state',
MoveitToJointsState(move_group='arm_with_torso', joint_names=['torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'], action_topic='/move_group'),
transitions={'reached': 'look_up', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'picking_state'})
# x:177 y:249
OperatableStateMachine.add('finished_state',
MoveitToJointsState(move_group='arm', joint_names=['shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint'], 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': 'prepare_state2'})
# x:833 y:506
OperatableStateMachine.add('nav_place',
MoveBaseState(),
transitions={'arrived': 'find_apriltag', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'place_waypoint'})
return _state_machine
def create(self):
# x:26 y:401, x:564 y:234
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.pick_waypoint = Pose2D(-0.3, -3.8, 0)
_state_machine.userdata.place_waypoint = Pose2D(-8.0, 4.13, 3.14)
_state_machine.userdata.picking_state = [0.385, 0.476, 1.266, -0.722, 1.954, -1.708, 2.0197, -1.415]
_state_machine.userdata.prepare_state = [3.141, 2.75, 1.0, -1.57, 0, 1.483]
_state_machine.userdata.start_point = Point(0.48, 0.0, 0.5)
_state_machine.userdata.look_up_point = Point(2.0, 0.0, 1.1)
_state_machine.userdata.see_apriltag = Point(0.8, 0.3, 0.7)
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:123 y:37
OperatableStateMachine.add('prepare_state',
MoveitToJointsState(move_group='arm', joint_names=['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'], action_topic='/move_group'),
transitions={'reached': 'nav_pick', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'prepare_state'})
# x:1137 y:421
OperatableStateMachine.add('nav_place',
MoveBaseState(),
transitions={'arrived': 'detect_plane', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'place_waypoint'})
# x:706 y:544
OperatableStateMachine.add('detect_plane',
DetectPlaneState(),
transitions={'continue': 'place_obj', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1096 y:252
OperatableStateMachine.add('pick_moving_state',
MoveitToJointsState(move_group='arm', joint_names=['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'], action_topic='/move_group'),
transitions={'reached': 'nav_place', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'prepare_state'})
# x:576 y:44
OperatableStateMachine.add('nav_pick',
MoveBaseState(),
transitions={'arrived': 'detect_obj', 'failed': 'failed'},
autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'waypoint': 'pick_waypoint'})
# x:372 y:536
OperatableStateMachine.add('place_obj',
PlaceState(),
transitions={'continue': 'finished_pose', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:1094 y:67
OperatableStateMachine.add('pick_obj',
PickState(),
transitions={'continue': 'pick_moving_state', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
# x:119 y:540
OperatableStateMachine.add('finished_pose',
MoveitToJointsState(move_group='arm', joint_names=['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6'], 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': 'prepare_state'})
# x:822 y:30
OperatableStateMachine.add('detect_obj',
DetectObjState(),
transitions={'continue': 'pick_obj', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
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):
# x:1070 y:60, x:1070 y:209
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.joint_config = [-1.0, 0.5]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:110 y:168
OperatableStateMachine.add(
'flexbe_WaitState',
WaitState(wait_time=1.0),
transitions={'done': 'flexbe_GetJointValuesState'},
autonomy={'done': Autonomy.Off})
# x:516 y:175
OperatableStateMachine.add(
'flexbe_LogKeyState',
LogKeyState(text='joint_values: {}',
severity=Logger.REPORT_HINT),
transitions={'done': 'hsr_MoveitToJointsState'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_values'})
# x:753 y:82
OperatableStateMachine.add(
'hsr_MoveitToJointsState',
MoveitToJointsState(
move_group='whole_body',
joint_names=['arm_flex_joint', 'arm_lift_joint'],
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_config'})
# x:291 y:55
OperatableStateMachine.add(
'flexbe_GetJointValuesState',
GetJointValuesState(joints=[
'arm_flex_joint', 'arm_lift_joint', 'arm_roll_joint',
'base_l_drive_wheel_joint', 'base_r_drive_wheel_joint',
'base_roll_joint', 'hand_l_spring_proximal_joint',
'hand_motor_joint', 'hand_r_spring_proximal_joint',
'head_pan_joint', 'head_tilt_joint', 'wrist_flex_joint',
'wrist_roll_joint'
]),
transitions={'retrieved': 'flexbe_LogKeyState'},
autonomy={'retrieved': Autonomy.Off},
remapping={'joint_values': 'joint_values'})
return _state_machine
def create(self):
joy_topic = 'hmi/joystick'
joint_trajectory_action = 'motion/controller/joint_trajectory'
# x:44 y:403, x:375 y:319
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.head_turned_right_joints = [0.15, 0.6, -0.15, 0.0, 0.0, 0.0]
_state_machine.userdata.head_forward_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:71 y:228
OperatableStateMachine.add('PlaceHead',
MoveitToJointsState(move_group='head', joint_names=['joint51', 'joint52', 'joint53', 'joint54'], action_topic='move_group'),
transitions={'reached': 'WatchPresentaion', 'planning_failed': 'PlaceHead', 'control_failed': 'finished'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'head_turned_right_joints'})
# x:433 y:67
OperatableStateMachine.add('SupprisedByProto3',
CompoundAction(t1=[0,0.0], type1='voice/play_wav', cmd1='do_i_really_be_same', t2=[0,0.0], type2='motion/joint_trajectory', cmd2='head_suprised', 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:678 y:447
OperatableStateMachine.add('DoNotUndestandAnything',
CompoundAction(t1=[0,0.5], type1='voice/play_wav', cmd1='i_dont_understand', t2=[0,0.0], type2='motion/joint_trajectory', cmd2='head_shake', t3=[0,0.0], type3=None, cmd3='', t4=[0,0.0], type4=None, cmd4=''),
transitions={'success': 'WatchPresentaion_4', 'failure': 'failed'},
autonomy={'success': Autonomy.Off, 'failure': Autonomy.Off})
# x:674 y:329
OperatableStateMachine.add('WatchPresentaion_3',
self.use_behavior(WatchPresentaionSM, 'WatchPresentaion_3', default_keys=['rand_head_config']),
transitions={'finished': 'DoNotUndestandAnything', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'head_pose_joints': 'head_forward_joints', 'rand_head_config': 'rand_head_config'})
# x:672 y:574
OperatableStateMachine.add('WatchPresentaion_4',
self.use_behavior(WatchPresentaionSM, 'WatchPresentaion_4', default_keys=['rand_head_config']),
transitions={'finished': 'AskToContinue ', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'head_pose_joints': 'head_turned_right_joints', 'rand_head_config': 'rand_head_config'})
# x:439 y:569
OperatableStateMachine.add('AskToContinue ',
CompoundAction(t1=[0,0.0], type1='voice/play_wav', cmd1='what_else_intresting_can_you_tell', t2=[0,0.0], type2='motion/joint_trajectory', cmd2='hoof_wave', 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:293 y:566
OperatableStateMachine.add('Wait',
WaitState(wait_time=4.0),
transitions={'done': 'What'},
autonomy={'done': Autonomy.Off})
# x:96 y:566
OperatableStateMachine.add('What',
CompoundAction(t1=[0,0.0], type1='voice/play_wav', cmd1='what_i_dont', t2=[0,0.0], type2='motion/joint_trajectory', cmd2='head_suprised_aborted', t3=[2,0.0], type3='eyes/emotion', cmd3='blank', t4=[0,0.0], type4=None, cmd4=''),
transitions={'success': 'finished', 'failure': 'failed'},
autonomy={'success': Autonomy.Off, 'failure': Autonomy.Off})
# x:667 y:202
OperatableStateMachine.add('ILikeMyMane',
CompoundAction(t1=[0,0.0], type1='voice/play_wav', cmd1='i_really_like_my_mane', t2=[0,0.5], type2='motion/joint_trajectory', cmd2='head_node', t3=[0,0.0], type3=None, cmd3='', t4=[0,0.0], type4=None, cmd4=''),
transitions={'success': 'WatchPresentaion_3', 'failure': 'failed'},
autonomy={'success': Autonomy.Off, 'failure': Autonomy.Off})
# x:211 y:42
OperatableStateMachine.add('WatchPresentaion',
self.use_behavior(WatchPresentaionSM, 'WatchPresentaion', default_keys=['rand_head_config']),
transitions={'finished': 'SupprisedByProto3', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'head_pose_joints': 'head_turned_right_joints', 'rand_head_config': 'rand_head_config'})
# x:667 y:64
OperatableStateMachine.add('WatchPresentaion_2',
self.use_behavior(WatchPresentaionSM, 'WatchPresentaion_2', default_keys=['rand_head_config']),
transitions={'finished': 'ILikeMyMane', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'head_pose_joints': 'head_turned_right_joints', 'rand_head_config': 'rand_head_config'})
return _state_machine
def create(self):
# x:228 y:396, x:225 y:240, x:1010 y:659
_state_machine = OperatableStateMachine(outcomes=['finished', 'unreachable', 'failed'], input_keys=['pose'])
_state_machine.userdata.pose = PoseStamped(Header(frame_id = 'base_link'), Pose(Point(0.4, 0.0, -0.05), Quaternion(0, 0, 0, 1)))
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:118 y:85
OperatableStateMachine.add('CheckHandPosition',
DecisionState(outcomes=['good','bad'], conditions=self.checkHandPosition),
transitions={'good': 'GetHeadPose', 'bad': 'unreachable'},
autonomy={'good': Autonomy.Off, 'bad': Autonomy.Off},
remapping={'input_value': 'pose'})
# x:890 y:253
OperatableStateMachine.add('SayHello',
TextCommandState(type='voice/play_wav', command='hello_im_sweetie_bot_friedship_programms', topic='control'),
transitions={'done': 'Wait'},
autonomy={'done': Autonomy.Off})
# x:663 y:69
OperatableStateMachine.add('RaiseHead',
MoveitToJointsState(move_group='head', joint_names=['joint51', 'joint52', 'joint53', 'joint54'], action_topic='move_group'),
transitions={'reached': 'RaiseLegProgram', 'planning_failed': 'failed', 'control_failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off},
remapping={'joint_config': 'head_pose'})
# x:309 y:66
OperatableStateMachine.add('GetHeadPose',
GetJointValuesState(joints=['joint51', 'joint52', 'joint53', 'joint54']),
transitions={'retrieved': 'CalcRaisedHeadPose'},
autonomy={'retrieved': Autonomy.Off},
remapping={'joint_values': 'head_pose'})
# x:485 y:73
OperatableStateMachine.add('CalcRaisedHeadPose',
CalculationState(calculation=self.calculateHeadPoseFunc),
transitions={'done': 'RaiseHead'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'head_pose', 'output_value': 'head_pose'})
# x:891 y:65
OperatableStateMachine.add('RaiseLegProgram',
ExecuteJointTrajectory(action_topic='motion/controller/joint_trajectory', trajectory_param='brohoof_begin', trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={'success': 'SayHello', '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:510 y:396
OperatableStateMachine.add('ReturnLegProgrammed',
ExecuteJointTrajectory(action_topic='motion/controller/joint_trajectory', trajectory_param='brohoof_end', 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'})
# x:851 y:399
OperatableStateMachine.add('Wait',
WaitState(wait_time=self.wait_time),
transitions={'done': 'ReturnLegProgrammed'},
autonomy={'done': Autonomy.Off})
return _state_machine