class CheckCurrentControlModeState(EventState): ''' Implements a state where the robot checks its current control mode. -- target_mode enum The control mode to check for being the current one (e.g. 3 for stand, 6 for manipulate). The state's class variables can also be used (e.g. CheckCurrentControlModeState.STAND). -- wait bool Whether to check once and return (False), or wait for the control mode to change (True). #> control_mode enum The current control mode when the state returned. <= correct Indicates that the current control mode is the target/expected one. <= incorrect Indicates that the current control mode is not the target/expected one. ''' NONE = 0 FREEZE = 1 STAND_PREP = 2 STAND = 3 STAND_MANIPULATE = 3 WALK = 4 STEP = 5 MANIPULATE = 6 USER = 7 CALIBRATE = 8 SOFT_STOP = 9 def __init__(self, target_mode, wait = False): ''' Constructor ''' super(CheckCurrentControlModeState, self).__init__(outcomes=['correct', 'incorrect'], output_keys=['control_mode']) self._status_topic = '/flor/controller/mode' self._sub = ProxySubscriberCached({self._status_topic: VigirAtlasControlMode}) self._sub.make_persistant(self._status_topic) self._target_mode = target_mode self._wait = wait def execute(self, userdata): ''' Execute this state ''' if self._sub.has_msg(self._status_topic): msg = self._sub.get_last_msg(self._status_topic) userdata.control_mode = msg.bdi_current_behavior if msg.bdi_current_behavior == self._target_mode: return 'correct' elif not self._wait: return 'incorrect' def on_enter(self, userdata): if self._wait: Logger.loghint("Waiting for %s" % str(self._target_mode))
class CheckCurrentControlModeState(EventState): ''' Implements a state where the robot checks its current control mode. -- target_mode enum The control mode to check for being the current one (e.g. 3 for stand, 6 for manipulate). The state's class variables can also be used (e.g. CheckCurrentControlModeState.STAND). -- wait bool Whether to check once and return (False), or wait for the control mode to change (True). #> control_mode enum The current control mode when the state returned. <= correct Indicates that the current control mode is the target/expected one. <= incorrect Indicates that the current control mode is not the target/expected one. ''' NONE = 0 FREEZE = 1 STAND_PREP = 2 STAND = 3 STAND_MANIPULATE = 3 WALK = 4 STEP = 5 MANIPULATE = 6 USER = 7 CALIBRATE = 8 SOFT_STOP = 9 def __init__(self, target_mode, wait=False): ''' Constructor ''' super(CheckCurrentControlModeState, self).__init__(outcomes=['correct', 'incorrect'], output_keys=['control_mode']) self._status_topic = '/flor/controller/mode' self._sub = ProxySubscriberCached( {self._status_topic: VigirAtlasControlMode}) self._sub.make_persistant(self._status_topic) self._target_mode = target_mode self._wait = wait def execute(self, userdata): ''' Execute this state ''' if self._sub.has_msg(self._status_topic): msg = self._sub.get_last_msg(self._status_topic) userdata.control_mode = msg.bdi_current_behavior if msg.bdi_current_behavior == self._target_mode: return 'correct' elif not self._wait: return 'incorrect' def on_enter(self, userdata): if self._wait: Logger.loghint("Waiting for %s" % str(self._target_mode))
class GetWristPoseState(EventState): ''' Retrieves the current wrist pose from the corresponding ROS topic. ># hand_side string Wrist whose pose will be returned (left, right) #> wrist_pose PoseStamped The current pose of the left or right wrist <= done Wrist pose has been retrieved. <= failed Failed to retrieve wrist pose. ''' def __init__(self): '''Constructor''' super(GetWristPoseState, self).__init__(outcomes = ['done', 'failed'], input_keys = ['hand_side'], output_keys = ['wrist_pose']) # Set up subscribers for listening to the latest wrist poses self._wrist_pose_topics = dict() self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose' self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose' self._sub = ProxySubscriberCached({ self._wrist_pose_topics['left']: PoseStamped, self._wrist_pose_topics['right']: PoseStamped}) self._sub.make_persistant(self._wrist_pose_topics['left']) self._sub.make_persistant(self._wrist_pose_topics['right']) self._failed = False def execute(self, userdata): if self._failed: return 'failed' else: return 'done' def on_enter(self, userdata): self._failed = False # Read the current wrist pose and write to userdata try: wrist_pose_topic = self._wrist_pose_topics[userdata.hand_side] wrist_pose = self._sub.get_last_msg(wrist_pose_topic) userdata.wrist_pose = wrist_pose rospy.loginfo('Retrieved %s wrist pose: \n%s', userdata.hand_side, wrist_pose) except Exception as e: Logger.logwarn('Failed to get the latest wrist pose:\n%s' % str(e)) self._failed = True return
class AttachObjectState(EventState): ''' Requests a grasp for a template from the template server. ># template_id int ID of the template to attach. ># hand_side string Hand side to which the template should be attached {left, right} #> template_pose PoseStamped Pose of the attached template in the reference frame of the wrist (r/l_hand) <= done Template has been attached. <= failed Failed to attach template. ''' def __init__(self): '''Constructor''' super(AttachObjectState, self).__init__(outcomes = ['done', 'failed'], input_keys = ['template_id', 'hand_side'], output_keys = ['template_pose']) # Set up service for attaching object template self._service_topic = "/stitch_object_template" self._srv = ProxyServiceCaller({ self._service_topic: SetAttachedObjectTemplate}) # Set up subscribers for listening to the latest wrist poses self._wrist_pose_topics = dict() self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose' self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose' self._sub = ProxySubscriberCached({ self._wrist_pose_topics['left']: PoseStamped, self._wrist_pose_topics['right']: PoseStamped}) self._sub.make_persistant(self._wrist_pose_topics['left']) self._sub.make_persistant(self._wrist_pose_topics['right']) self._failed = False def execute(self, userdata): if self._failed: return 'failed' return 'done' def on_enter(self, userdata): self._failed = False # The frame ID to which the object template will be attached to if userdata.hand_side == 'left': frame_id = "l_hand" elif userdata.hand_side == 'right': frame_id = "r_hand" else: Logger.logwarn('Unexpected value of hand side: %s Expected {left, right}' % str(userdata.hand_side)) self._failed = True return # Get the current wrist pose, which is required by the stitching service try: wrist_pose_topic = self._wrist_pose_topics[userdata.hand_side] current_wrist_pose = self._sub.get_last_msg(wrist_pose_topic) # Set the PoseStamped message's frame ID to the one # that the object template should be attached to: current_wrist_pose.header.frame_id = frame_id except Exception as e: Logger.logwarn('Failed to get the latest hand pose:\n%s' % str(e)) self._failed = True return # Send the stiching request and write response to userdata try: request = SetAttachedObjectTemplateRequest(template_id = userdata.template_id, pose = current_wrist_pose) self._srv_result = self._srv.call(self._service_topic, request) # The result contains the template's pose, mass, and center of mass userdata.template_pose = self._srv_result.template_pose except Exception as e: Logger.logwarn('Failed to send service call:\n%s' % str(e)) self._failed = True return
class AttachObjectState(EventState): ''' Requests a grasp for a template from the template server. ># template_id int ID of the template to attach. ># hand_side string Hand side to which the template should be attached {left, right} #> template_pose PoseStamped Pose of the attached template in the reference frame of the wrist (r/l_hand) <= done Template has been attached. <= failed Failed to attach template. ''' def __init__(self): '''Constructor''' super(AttachObjectState, self).__init__(outcomes=['done', 'failed'], input_keys=['template_id', 'hand_side'], output_keys=['template_pose']) # Set up service for attaching object template self._service_topic = "/stitch_object_template" self._srv = ProxyServiceCaller( {self._service_topic: SetAttachedObjectTemplate}) # Set up subscribers for listening to the latest wrist poses self._wrist_pose_topics = dict() self._wrist_pose_topics['left'] = '/flor/l_arm_current_pose' self._wrist_pose_topics['right'] = '/flor/r_arm_current_pose' self._sub = ProxySubscriberCached({ self._wrist_pose_topics['left']: PoseStamped, self._wrist_pose_topics['right']: PoseStamped }) self._sub.make_persistant(self._wrist_pose_topics['left']) self._sub.make_persistant(self._wrist_pose_topics['right']) self._failed = False def execute(self, userdata): if self._failed: return 'failed' return 'done' def on_enter(self, userdata): self._failed = False # The frame ID to which the object template will be attached to if userdata.hand_side == 'left': frame_id = "l_hand" elif userdata.hand_side == 'right': frame_id = "r_hand" else: Logger.logwarn( 'Unexpected value of hand side: %s Expected {left, right}' % str(userdata.hand_side)) self._failed = True return # Get the current wrist pose, which is required by the stitching service try: wrist_pose_topic = self._wrist_pose_topics[userdata.hand_side] current_wrist_pose = self._sub.get_last_msg(wrist_pose_topic) # Set the PoseStamped message's frame ID to the one # that the object template should be attached to: current_wrist_pose.header.frame_id = frame_id except Exception as e: Logger.logwarn('Failed to get the latest hand pose:\n%s' % str(e)) self._failed = True return # Send the stiching request and write response to userdata try: request = SetAttachedObjectTemplateRequest( template_id=userdata.template_id, pose=current_wrist_pose) self._srv_result = self._srv.call(self._service_topic, request) # The result contains the template's pose, mass, and center of mass userdata.template_pose = self._srv_result.template_pose except Exception as e: Logger.logwarn('Failed to send service call:\n%s' % str(e)) self._failed = True return
class ATLAScheckoutSM(Behavior): ''' A behavior meant to be run before high voltage is provided to ATLAS. It guides the robot through BDI's calibration, initial control mode changes, Praying Mantis calibration, and visual calibration check. ''' def __init__(self): super(ATLAScheckoutSM, self).__init__() self.name = 'ATLAS checkout' # parameters of this behavior # references to used behaviors self.add_behavior(PrayingMantisCalibrationSM, 'Praying Mantis Calibration') # Additional initialization code can be added inside the following tags # [MANUAL_INIT] self._status_topic = '/flor/controller/robot_status' self._sub = ProxySubscriberCached({self._status_topic: FlorRobotStatus}) self._sub.make_persistant(self._status_topic) # [/MANUAL_INIT] # Behavior comments: def create(self): # x:1125 y:169, x:430 y:273 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.left = 'left' _state_machine.userdata.right = 'right' _state_machine.userdata.none = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:728 y:81, x:318 y:310 _sm_confirm_calibration_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['left', 'right']) with _sm_confirm_calibration_0: # x:68 y:71 OperatableStateMachine.add('Go_to_MANIPULATE', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Look_Slightly_Down', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Low}) # x:75 y:301 OperatableStateMachine.add('Check_Left_Arm', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.CALIBRATE_ARMS, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Bring_Left_Arm_Down', 'failed': 'failed'}, autonomy={'done': Autonomy.Full, 'failed': Autonomy.Low}, remapping={'side': 'left'}) # x:76 y:394 OperatableStateMachine.add('Bring_Left_Arm_Down', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.SINGLE_ARM_STAND, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Check_Right_Arm', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.Low}, remapping={'side': 'left'}) # x:426 y:393 OperatableStateMachine.add('Check_Right_Arm', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.CALIBRATE_ARMS, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Bring_Right_Arm_Down', 'failed': 'failed'}, autonomy={'done': Autonomy.Full, 'failed': Autonomy.Low}, remapping={'side': 'right'}) # x:426 y:301 OperatableStateMachine.add('Bring_Right_Arm_Down', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.SINGLE_ARM_STAND, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Look_Straight', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.Low}, remapping={'side': 'right'}) # x:415 y:75 OperatableStateMachine.add('Go_to_STAND', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND), transitions={'changed': 'finished', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.Low}) # x:81 y:185 OperatableStateMachine.add('Look_Slightly_Down', TiltHeadState(desired_tilt=TiltHeadState.DOWN_30), transitions={'done': 'Check_Left_Arm', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low}) # x:449 y:178 OperatableStateMachine.add('Look_Straight', TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT), transitions={'done': 'Go_to_STAND', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low}) # x:748 y:57, x:306 y:173 _sm_checks_and_bdi_calibration_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['none']) with _sm_checks_and_bdi_calibration_1: # x:54 y:28 OperatableStateMachine.add('Get_Air_Pressure', CalculationState(calculation=self.get_air_sump_pressure), transitions={'done': 'Check_Air_Pressure'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'none', 'output_value': 'air_pressure'}) # x:46 y:411 OperatableStateMachine.add('Check_Hands_Initialized', OperatorDecisionState(outcomes=['ready', 'again'], hint='Did both hands initialize?', suggestion='ready'), transitions={'ready': 'Turn_Pump_ON', 'again': 'Request_Robot_Power'}, autonomy={'ready': Autonomy.High, 'again': Autonomy.Full}) # x:267 y:29 OperatableStateMachine.add('Air_Pressure_Warning', LogState(text='Check the air pressure! Has to be 120 +/- 10 PSI.', severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Full}) # x:50 y:149 OperatableStateMachine.add('Check_Air_Pressure', DecisionState(outcomes=['ok', 'alert'], conditions=lambda p: 'ok' if p > 110.0 and p <= 130.0 else 'alert'), transitions={'ok': 'Request_Robot_Power', 'alert': 'Air_Pressure_Warning'}, autonomy={'ok': Autonomy.Full, 'alert': Autonomy.Low}, remapping={'input_value': 'air_pressure'}) # x:500 y:286 OperatableStateMachine.add('Calibrate_ARMS', RobotStateCommandState(command=RobotStateCommandState.CALIBRATE_ARMS), transitions={'done': 'FREEZE_in_Between', 'failed': 'failed'}, autonomy={'done': Autonomy.Full, 'failed': Autonomy.Low}) # x:499 y:52 OperatableStateMachine.add('Calibrate_BIASES', RobotStateCommandState(command=RobotStateCommandState.CALIBRATE), transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Full, 'failed': Autonomy.Low}) # x:488 y:167 OperatableStateMachine.add('FREEZE_in_Between', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.FREEZE), transitions={'changed': 'Calibrate_BIASES', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.High}) # x:500 y:412 OperatableStateMachine.add('Turn_Pump_ON', RobotStateCommandState(command=RobotStateCommandState.START_HYDRAULIC_PRESSURE_ON), transitions={'done': 'Calibrate_ARMS', 'failed': 'failed'}, autonomy={'done': Autonomy.Full, 'failed': Autonomy.High}) # x:47 y:276 OperatableStateMachine.add('Request_Robot_Power', LogState(text='Request robot power from field team.', severity=Logger.REPORT_HINT), transitions={'done': 'Check_Hands_Initialized'}, autonomy={'done': Autonomy.Full}) with _state_machine: # x:74 y:88 OperatableStateMachine.add('Checks_and_BDI_Calibration', _sm_checks_and_bdi_calibration_1, transitions={'finished': 'Go_to_STAND_PREP', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'none': 'none'}) # x:79 y:264 OperatableStateMachine.add('Go_to_STAND_PREP', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND_PREP), transitions={'changed': 'Go_to_STAND', 'failed': 'failed'}, autonomy={'changed': Autonomy.Full, 'failed': Autonomy.Low}) # x:78 y:393 OperatableStateMachine.add('Go_to_STAND', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND), transitions={'changed': 'Praying Mantis Calibration', 'failed': 'failed'}, autonomy={'changed': Autonomy.Full, 'failed': Autonomy.Low}) # x:611 y:388 OperatableStateMachine.add('Praying Mantis Calibration', self.use_behavior(PrayingMantisCalibrationSM, 'Praying Mantis Calibration'), transitions={'finished': 'Decide_Calibration_Check', 'failed': 'Decide_Retry'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:624 y:249 OperatableStateMachine.add('Decide_Calibration_Check', OperatorDecisionState(outcomes=['check', 'skip'], hint='Do you want to confirm the arm calibration visually?', suggestion='skip'), transitions={'check': 'Confirm_Calibration', 'skip': 'Logging_Reminder'}, autonomy={'check': Autonomy.Full, 'skip': Autonomy.High}) # x:627 y:93 OperatableStateMachine.add('Confirm_Calibration', _sm_confirm_calibration_0, transitions={'finished': 'Logging_Reminder', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'left': 'left', 'right': 'right'}) # x:389 y:340 OperatableStateMachine.add('Decide_Retry', OperatorDecisionState(outcomes=['retry', 'fail'], hint='Try running Praying Mantis Calibration again?', suggestion='retry'), transitions={'retry': 'Praying Mantis Calibration', 'fail': 'failed'}, autonomy={'retry': Autonomy.High, 'fail': Autonomy.Full}) # x:907 y:164 OperatableStateMachine.add('Logging_Reminder', LogState(text='Start video logging', severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Full}) return _state_machine # Private functions can be added inside the following tags # [MANUAL_FUNC] def get_air_sump_pressure(self, input): '''Returns the current air sump pressure.''' robot_status = self._sub.get_last_msg(self._status_topic) air_pressure = robot_status.air_sump_pressure Logger.loginfo('Air Sump Pressure: %.2f' % air_pressure) return air_pressure