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