<s> import os <EOL> import vtkAll as vtk <EOL> import math <EOL> import numpy as np <EOL> from collections import deque <EOL> from director import transformUtils <EOL> from director import lcmUtils <EOL> from director . timercallback import TimerCallback <EOL> from director import objectmodel as om <EOL> from director import visualization as vis <EOL> from director import applogic as app <EOL> from director . debugVis import DebugData <EOL> from director import ioUtils <EOL> from director . simpletimer import SimpleTimer <EOL> from director . utime import getUtime <EOL> import time <EOL> import drc as lcmdrc <EOL> import bot_core <EOL> import atlas <EOL> from pronto . indexed_measurement_t import indexed_measurement_t <EOL> class SystemStatusListener ( object ) : <EOL> def __init__ ( self , outputConsole ) : <EOL> self . outputConsole = outputConsole <EOL> lcmUtils . addSubscriber ( '<STR_LIT>' , bot_core . system_status_t , self . onSystemStatus ) <EOL> def onSystemStatus ( self , message ) : <EOL> message = '<STR_LIT>' + message . value <EOL> if self . outputConsole is not None : <EOL> self . outputConsole . append ( message ) <EOL> else : <EOL> print message <EOL> class AtlasDriver ( object ) : <EOL> def __init__ ( self ) : <EOL> self . lastAtlasStatusMessage = None <EOL> self . lastControllerStatusMessage = None <EOL> self . lastAtlasBatteryDataMessage = None <EOL> self . lastAtlasElectricArmStatusMessage = None <EOL> self . lastControllerRateMessage = None <EOL> self . maxPressureHistory = deque ( [ <NUM_LIT:0.0> ] , <NUM_LIT:10> ) <EOL> self . averageRecentMaxPressure = <NUM_LIT:0.0> <EOL> self . _setupSubscriptions ( ) <EOL> self . timer = SimpleTimer ( ) <EOL> self . sentStandUtime = None <EOL> self . startupStage = <NUM_LIT:0> <EOL> self . _behaviorMap = None <EOL> self . _controllerStatusMap = None <EOL> def _setupSubscriptions ( self ) : <EOL> lcmUtils . addSubscriber ( '<STR_LIT>' , lcmdrc . plan_status_t , self . onControllerStatus ) <EOL> lcmUtils . addSubscriber ( '<STR_LIT>' , atlas . battery_data_t , self . onAtlasBatteryData ) <EOL> lcmUtils . addSubscriber ( '<STR_LIT>' , atlas . electric_arm_status_t , self . onAtlasElectricArmStatus ) <EOL> lcmUtils . addSubscriber ( '<STR_LIT>' , lcmdrc . message_rate_t , self . onControllerRate ) <EOL> sub = lcmUtils . addSubscriber ( '<STR_LIT>' , atlas . status_t , self . onAtlasStatus ) <EOL> sub . setSpeedLimit ( <NUM_LIT> ) <EOL> sub = lcmUtils . addSubscriber ( '<STR_LIT>' , atlas . state_extra_t , self . onAtlasStateExtra ) <EOL> sub . setSpeedLimit ( <NUM_LIT:5> ) <EOL> def onAtlasStatus ( self , message ) : <EOL> self . lastAtlasStatusMessage = message <EOL> def onControllerStatus ( self , message ) : <EOL> self . lastControllerStatusMessage = message <EOL> def onControllerRate ( self , message ) : <EOL> self . lastControllerRateMessage = message <EOL> def onAtlasBatteryData ( self , message ) : <EOL> self . lastAtlasBatteryDataMessage = message <EOL> def onAtlasElectricArmStatus ( self , message ) : <EOL> self . lastAtlasElectricArmStatusMessage = message <EOL> def onAtlasStateExtra ( self , message ) : <EOL> self . maxPressureHistory . append ( max ( np . max ( message . psi_pos ) , np . max ( message . psi_neg ) ) ) <EOL> self . averageRecentMaxPressure = np . mean ( self . maxPressureHistory ) <EOL> def getBehaviorMap ( self ) : <EOL> '''<STR_LIT>''' <EOL> if not self . _behaviorMap : <EOL> msg = atlas . status_t <EOL> self . _behaviorMap = { <EOL> msg . BEHAVIOR_NONE : '<STR_LIT:none>' , <EOL> msg . BEHAVIOR_FREEZE : '<STR_LIT>' , <EOL> msg . BEHAVIOR_STAND_PREP : '<STR_LIT>' , <EOL> msg . BEHAVIOR_STAND : '<STR_LIT>' , <EOL> msg . BEHAVIOR_WALK : '<STR_LIT>' , <EOL> msg . BEHAVIOR_STEP : '<STR_LIT>' , <EOL> msg . BEHAVIOR_MANIPULATE : '<STR_LIT>' , <EOL> msg . BEHAVIOR_USER : '<STR_LIT:user>' , <EOL> msg . BEHAVIOR_CALIBRATE : '<STR_LIT>' , <EOL> msg . BEHAVIOR_SOFT_STOP : '<STR_LIT>' , <EOL> } <EOL> return self . _behaviorMap <EOL> def getControllerStatusMap ( self ) : <EOL> '''<STR_LIT>''' <EOL> if not self . _controllerStatusMap : <EOL> msg = lcmdrc . plan_status_t <EOL> self . _controllerStatusMap = { <EOL> msg . UNKNOWN : '<STR_LIT>' , <EOL> msg . STANDING : '<STR_LIT>' , <EOL> msg . WALKING : '<STR_LIT>' , <EOL> msg . HARNESSED : '<STR_LIT>' , <EOL> msg . QUASISTATIC : '<STR_LIT>' , <EOL> msg . BRACING : '<STR_LIT>' , <EOL> msg . CRAWLING : '<STR_LIT>' , <EOL> msg . DUMMY : '<STR_LIT>' , <EOL> msg . MANIPULATING : '<STR_LIT>' , <EOL> msg . RECOVERING : '<STR_LIT>' , <EOL> } <EOL> return self . _controllerStatusMap <EOL> def getCurrentBehaviorName ( self ) : <EOL> '''<STR_LIT>''' <EOL> if not self . lastAtlasStatusMessage : <EOL> return None <EOL> behaviors = self . getBehaviorMap ( ) <EOL> behaviorId = self . lastAtlasStatusMessage . behavior <EOL> assert behaviorId in behaviors <EOL> return behaviors [ behaviorId ] <EOL> def getControllerRate ( self ) : <EOL> '''<STR_LIT>''' <EOL> if self . lastControllerRateMessage : <EOL> return self . lastControllerRateMessage . rate_deci_hz / <NUM_LIT> <EOL> def getControllerStatus ( self ) : <EOL> '''<STR_LIT>''' <EOL> if not self . lastControllerStatusMessage : <EOL> return None <EOL> statusMap = self . getControllerStatusMap ( ) <EOL> state = self . lastControllerStatusMessage . plan_type <EOL> assert state in statusMap <EOL> return statusMap [ state ] <EOL> def getRecoveryEnabledStatus ( self ) : <EOL> if not self . lastControllerStatusMessage : <EOL> return None <EOL> if self . lastControllerStatusMessage . recovery_enabled : <EOL> return "<STR_LIT>" <EOL> else : <EOL> return "<STR_LIT>" <EOL> def getBracingEnabledStatus ( self ) : <EOL> if not self . lastControllerStatusMessage : <EOL> return None <EOL> if self . lastControllerStatusMessage . bracing_enabled : <EOL> return "<STR_LIT>" <EOL> else : <EOL> return "<STR_LIT>" <EOL> def getElectricArmEnabledStatus ( self , i ) : <EOL> assert <NUM_LIT:0> <= i <= <NUM_LIT:5> <EOL> if self . lastAtlasElectricArmStatusMessage : <EOL> return self . lastAtlasElectricArmStatusMessage . enabled [ i ] <EOL> return False <EOL> def getElectricArmTemperature ( self , i ) : <EOL> assert <NUM_LIT:0> <= i <= <NUM_LIT:5> <EOL> if self . lastAtlasElectricArmStatusMessage : <EOL> return self . lastAtlasElectricArmStatusMessage . temperature [ i ] <EOL> return <NUM_LIT:0.0> <EOL> def getElectricArmDriveCurrent ( self , i ) : <EOL> assert <NUM_LIT:0> <= i <= <NUM_LIT:5> <EOL> if self . lastAtlasElectricArmStatusMessage : <EOL> return self . lastAtlasElectricArmStatusMessage . drive_current [ i ] <EOL> return <NUM_LIT:0.0> <EOL> def getBatteryChargeRemaining ( self ) : <EOL> if self . lastAtlasBatteryDataMessage : <EOL> return self . lastAtlasBatteryDataMessage . remaining_charge_percentage <EOL> def getBatteryVoltage ( self ) : <EOL> if self . lastAtlasBatteryDataMessage : <EOL> return self . lastAtlasBatteryDataMessage . voltage <EOL> def getBatteryTemperature ( self ) : <EOL> if self . lastAtlasBatteryDataMessage : <EOL> return self . lastAtlasBatteryDataMessage . temperature <EOL> def getCurrentInletPressure ( self ) : <EOL> if self . lastAtlasStatusMessage : <EOL> return self . lastAtlasStatusMessage . pump_inlet_pressure <EOL> return <NUM_LIT:0.0> <EOL> def getCurrentSupplyPressure ( self ) : <EOL> if self . lastAtlasStatusMessage : <EOL> return self . lastAtlasStatusMessage . pump_supply_pressure <EOL> return <NUM_LIT:0.0> <EOL> def getCurrentReturnPressure ( self ) : <EOL> if self . lastAtlasStatusMessage : <EOL> return self . lastAtlasStatusMessage . pump_return_pressure <EOL> return <NUM_LIT:0.0> <EOL> def getCurrentAirSumpPressure ( self ) : <EOL> if self . lastAtlasStatusMessage : <EOL> return self . lastAtlasStatusMessage . air_sump_pressure <EOL> return <NUM_LIT:0.0> <EOL> def getCurrentPumpRpm ( self ) : <EOL> if self . lastAtlasStatusMessage : <EOL> return self . lastAtlasStatusMessage . current_pump_rpm <EOL> return <NUM_LIT:0.0> <EOL> def getMaxActuatorPressure ( self ) : <EOL> return self . averageRecentMaxPressure <EOL> def sendDesiredPumpPsi ( self , desiredPsi ) : <EOL> msg = atlas . pump_command_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . k_psi_p = <NUM_LIT:0.0> <EOL> msg . k_psi_i = <NUM_LIT:0.0> <EOL> msg . k_psi_d = <NUM_LIT:0.0> <EOL> msg . k_rpm_p = <NUM_LIT:0.0> <EOL> msg . k_rpm_i = <NUM_LIT:0.0> <EOL> msg . k_rpm_d = <NUM_LIT:0.0> <EOL> msg . ff_rpm_d = <NUM_LIT:0.0> <EOL> msg . ff_psi_d = <NUM_LIT:0.0> <EOL> msg . ff_const = <NUM_LIT:0.0> <EOL> msg . psi_i_max = <NUM_LIT:0.0> <EOL> msg . rpm_i_max = <NUM_LIT:0.0> <EOL> msg . cmd_max = <NUM_LIT> <EOL> msg . desired_psi = desiredPsi <EOL> msg . desired_rpm = <NUM_LIT> <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendBehaviorCommand ( self , behaviorName ) : <EOL> msg = lcmdrc . behavior_command_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . command = behaviorName <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendStopCommand ( self ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT>' ) <EOL> def sendFreezeCommand ( self ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT>' ) <EOL> def sendPrepCommand ( self ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT>' ) <EOL> def sendStandCommand ( self ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT>' ) <EOL> def sendCombinedStandCommand ( self ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT>' ) <EOL> self . startupStage = <NUM_LIT:1> <EOL> self . sentStandUtime = getUtime ( ) <EOL> def sendMITStandCommand ( self ) : <EOL> msg = lcmdrc . utime_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendRecoveryEnable ( self ) : <EOL> msg = lcmdrc . boolean_t ( ) <EOL> msg . data = True <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendBracingEnable ( self ) : <EOL> msg = lcmdrc . boolean_t ( ) <EOL> msg . data = True <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendBracingDisable ( self ) : <EOL> msg = lcmdrc . boolean_t ( ) <EOL> msg . data = False <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendRecoveryDisable ( self ) : <EOL> msg = lcmdrc . boolean_t ( ) <EOL> msg . data = False <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendRecoveryTriggerOn ( self ) : <EOL> msg = lcmdrc . recovery_trigger_t ( ) <EOL> msg . activate = True <EOL> msg . override = True <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendRecoveryTriggerOff ( self ) : <EOL> msg = lcmdrc . recovery_trigger_t ( ) <EOL> msg . activate = False <EOL> msg . override = True <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendManipCommand ( self ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT>' ) <EOL> def sendUserCommand ( self ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT:user>' ) <EOL> def sendCalibrateNullBiasCommand ( self ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT>' ) <EOL> def sendCalibrateElectricArmsCommand ( self ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT>' ) <EOL> def sendCalibrateElectricArmsFreezeCommand ( self ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT>' ) <EOL> def sendElectricArmEnabledState ( self , enabledState ) : <EOL> msg = atlas . electric_arm_enable_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . num_electric_arm_joints = <NUM_LIT:6> <EOL> assert len ( enabledState ) == msg . num_electric_arm_joints <EOL> msg . enable = enabledState <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendCalibrateEncodersCommand ( self ) : <EOL> msg = lcmdrc . utime_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendPlanUsingBdiHeight ( self , enabled ) : <EOL> msg = lcmdrc . plan_adjust_mode_t ( ) <EOL> msg . utime = getUtime ( ) <EOL> msg . mode = <NUM_LIT:1> if enabled else <NUM_LIT:0> <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def sendInitAtZero ( self , randomize = False ) : <EOL> self . sendReadyMessage ( ) <EOL> if randomize : <EOL> bounds = np . array ( [ <NUM_LIT> , <NUM_LIT> , <NUM_LIT> ] ) <EOL> pos = - bounds + np . random . rand ( <NUM_LIT:3> ) * <NUM_LIT:2> * bounds <EOL> yaw = np . random . rand ( ) * np . pi * <NUM_LIT:2> <EOL> else : <EOL> pos = [ <NUM_LIT:0> , <NUM_LIT:0> , <NUM_LIT> ] <EOL> yaw = <NUM_LIT:0.0> <EOL> self . sendInitMessage ( pos , yaw ) <EOL> def sendReadyMessage ( self ) : <EOL> ready_init = lcmdrc . utime_t ( ) <EOL> ready_init . utime = getUtime ( ) <EOL> lcmUtils . publish ( '<STR_LIT>' , ready_init ) <EOL> time . sleep ( <NUM_LIT:1> ) <EOL> def sendInitMessage ( self , pos , yaw ) : <EOL> init = indexed_measurement_t ( ) <EOL> init . utime = getUtime ( ) <EOL> init . state_utime = init . utime <EOL> init . measured_dim = <NUM_LIT:4> <EOL> init . z_effective = [ pos [ <NUM_LIT:0> ] , pos [ <NUM_LIT:1> ] , pos [ <NUM_LIT:2> ] , yaw ] <EOL> init . z_indices = [ <NUM_LIT:9> , <NUM_LIT:10> , <NUM_LIT:11> , <NUM_LIT:8> ] <EOL> init . measured_cov_dim = init . measured_dim * init . measured_dim <EOL> init . R_effective = [ <NUM_LIT:0> ] * init . measured_cov_dim <EOL> init . R_effective [ <NUM_LIT:0> ] = <NUM_LIT> <EOL> init . R_effective [ <NUM_LIT:5> ] = <NUM_LIT> <EOL> init . R_effective [ <NUM_LIT:10> ] = <NUM_LIT> <EOL> init . R_effective [ <NUM_LIT:15> ] = math . pow ( <NUM_LIT:50> * math . pi / <NUM_LIT> , <NUM_LIT:2> ) <EOL> lcmUtils . publish ( '<STR_LIT>' , init ) <EOL> def updateCombinedStandLogic ( self ) : <EOL> if ( self . sentStandUtime is not None ) : <EOL> if ( self . startupStage == <NUM_LIT:1> ) : <EOL> if ( getUtime ( ) > self . sentStandUtime + <NUM_LIT> ) : <EOL> self . sendInitAtZero ( ) <EOL> self . startupStage = <NUM_LIT:2> <EOL> elif ( self . startupStage == <NUM_LIT:2> ) : <EOL> if ( getUtime ( ) > self . sentStandUtime + <NUM_LIT> ) : <EOL> self . sendBehaviorCommand ( '<STR_LIT:user>' ) <EOL> self . sendMITStandCommand ( ) <EOL> self . startupStage = <NUM_LIT:0> <EOL> def getPelvisHeightLimits ( self ) : <EOL> '''<STR_LIT>''' <EOL> return ( <NUM_LIT> , <NUM_LIT> ) <EOL> def sendPelvisHeightCommand ( self , height ) : <EOL> heightLimit = self . getPelvisHeightLimits ( ) <EOL> assert heightLimit [ <NUM_LIT:0> ] <= height <= heightLimit [ <NUM_LIT:1> ] <EOL> pelvisParams = atlas . behavior_pelvis_servo_params_t ( ) <EOL> pelvisParams . com_v0 = <NUM_LIT:0.0> <EOL> pelvisParams . com_v1 = <NUM_LIT:0.0> <EOL> pelvisParams . pelvis_height = height <EOL> pelvisParams . pelvis_yaw = <NUM_LIT:0.0> <EOL> pelvisParams . pelvis_pitch = <NUM_LIT:0.0> <EOL> pelvisParams . pelvis_roll = <NUM_LIT:0.0> <EOL> msg = atlas . behavior_manipulate_params_t ( ) <EOL> msg . use_demo_mode = <NUM_LIT:0> <EOL> msg . use_desired = <NUM_LIT:1> <EOL> msg . desired = pelvisParams <EOL> lcmUtils . publish ( '<STR_LIT>' , msg ) <EOL> def init ( outputConsole = None ) : <EOL> global driver <EOL> driver = AtlasDriver ( ) <EOL> global systemStatus <EOL> systemStatus = SystemStatusListener ( outputConsole ) <EOL> return driver </s>
