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
