def PlotCoursesOnMap(self):
gm = GliderModel(conf.myDataDir + 'RiskMap.shelf',
conf.myDataDir + 'roms')
plt.figure()
gm.PlotNewRiskMapFig()
#gm.GetXYfromLatLon()
for vessel in self.vesselList:
msgList = self.msgIndxByVessel['%d' % (vessel)]
trackX, trackY = [], []
for msg in msgList:
if msg.has_key('y'):
lat, lon = msg['y'], msg['x']
x, y = gm.GetXYfromLatLon(lat, lon)
trackX.append(x)
trackY.append(y)
plt.plot(trackX, trackY, '.-')
class PseudoWayPtGenerator(object):
''' Pseudo waypoint generator class which allows us to select a waypoint,
and then it reverse calculates a location to aim for such that we get to the
desired location.
'''
def __init__(self,
riskMap='RiskMap.shelf',
romsDataDir='/roms/',
**kwargs):
''' Initialize the pseudo waypoint generator
Args:
riskMap (str): path to and name of shelf containing the risk map etc.
romsDataDir (str): directory containing the ROMS data used for simulations.
Keyword Args:
use_realtime_data (bool): Flag that indicates if we should be using real-time data for
simulations or if we will be passed either a datetime or
date and time. True if we will use real-time. False if not.
use_datetime (datetime): Datetime object that indicates the date and time of the simulation.
yy,mm,dd,hr,mi (int): Year, Month, Day, Hour and Minute to start simulation at if not via a datetime.
no_comms_timeout_secs (int): No. of seconds of no communications due to which the glider will
resurface (>300).
roms_yy, roms_mm, roms_dd (int): Roms data file if we want to explicitly specify one.
glider_vel (float) : Velocity of the glider to be used in simulations (0<glider_vel<2)
max_dive_depth (float) : Maximum depth the glider will be diving to.
max_climb_depth (float) : Maximum depth the glider will be climbing to.
'''
self.gm = GliderModel(riskMap, romsDataDir)
self.rtc = RomsTimeConversion()
self.ll_conv = LLConvert()
self.er = EarthRadius(33.55)
self.dc = DistCalculator(self.er.R)
# We have not yet loaded the ROMS data.
self.romsDataLoaded = False
# Don't perform a full-simulation by default
self.perform_full_simulation = False
# Don't treat going off the map as a collision
self.hold_vals_off_map = True
# Maximum dive depth
self.max_dive_depth = 80.
self.no_comms_timeout_secs = 12 * 3600
# Take care of figuring out the time.
self.use_realtime_data = True
self.TestKwArgs(**kwargs)
def TestKwArgs(self, **kwargs):
if kwargs.has_key('use_realtime_data'):
use_realtime_data = self.use_realtime_data
if use_real_time_data:
self.use_real_time_data = True
self.dt = datetime.datetime.utcnow()
else:
self.use_real_time_data = False
# Since we are not using real-time data you better pass
# that data to me.
if kwargs.has_key('use_datetime'
): # We have been passed a datetime object.
dt = kwargs['use_datetime']
self.dt = dt
elif kwargs.has_key('yy'):
yy, mm, dd = kwargs['yy'], kwargs['mm'], kwargs[
'dd'] # We have been passed the date
hr, mi = kwargs['hr'], kwargs['mi']
self.dt = datetime.datetime(
yy, mm, dd, hr,
mi) # we assume we were passed date and time in UTC
else:
self.dt = None
if kwargs.has_key('no_comms_timeout_secs'):
no_comms_timeout_secs = kwargs['no_comms_timeout_secs']
if (no_comms_timeout_secs <= 299):
self.no_comms_timeout_secs = no_comms_timeout_secs
else:
raise
if kwargs.has_key('roms_yy'):
roms_yy, roms_mm, roms_dd, roms_numDays = kwargs[
'roms_yy'], kwargs['roms_mm'], kwargs['roms_dd'], kwargs[
'roms_numDays']
roms_dt = datetime.datetime(roms_yy, roms_mm, roms_dd, 0,
0) # Auto-test ROMS date
self.u, self.v, self.time1, self.depth, self.lat, self.lon = self.gm.GetRomsData(
roms_yy, roms_mm, roms_dd, roms_numDays, True, True)
self.romsDataLoaded = True
if kwargs.has_key('glider_vel'):
glider_vel = kwargs['glider_vel']
if glider_vel <= 0 or glider_vel >= 2:
print 'Sorry, gliders are not that quick yet... Ignoring this entry!'
else:
self.gm.gVel = glider_vel
if kwargs.has_key('glider_vel_nom'):
glider_vel_nom = kwargs['glider_vel_nom']
if glider_vel_nom <= 0 or glider_vel_nom >= 2:
print 'Sorry, gliders are not that quick yet... Ignoring this entry!'
else:
self.gm.gVelNom = glider_vel_nom
if kwargs.has_key('max_dive_depth'):
max_dive_depth = kwargs['max_dive_depth']
if max_dive_depth > 95:
max_dive_depth = 95
if max_dive_depth < 5:
max_dive_depth = 5
self.max_dive_depth = max_dive_depth
if kwargs.has_key('max_climb_depth'):
max_climb_depth = kwargs['max_climb_depth']
if max_climb_depth > 0:
max_climb_depth = 0
if max_climb_depth < 30:
max_climb_depth = 30
self.max_climb_depth = max_climb_depth
self.gm.MinDepth = max_climb_depth
if kwargs.has_key('perform_full_simulation'):
if kwargs['perform_full_simulation']:
self.perform_full_simulation = True
else:
self.perform_full_simulation = False
if kwargs.has_key('hold_vals_off_map'):
if kwargs['hold_vals_off_map']:
self.hold_vals_off_map = True
else:
self.hold_vals_off_map = False
def SimulateDive(self, start, goal, startT, **kwargs):
''' Simulate a dive given a start in (lat,lon), goal in (lat,lon) and a starting time.
'''
self.TestKwArgs(
**kwargs
) # First, set any keyword args that may have been passed in.
plot_figure = False
line_color, line_width = 'r-', 2.5
if kwargs.has_key('line_color'):
line_color = kwargs['line_color']
if kwargs.has_key('line_width'):
line_width = kwargs['line_width']
if kwargs.has_key('plot_figure'):
if kwargs['plot_figure']:
plot_figure = True
plt.figure()
self.gm.PlotNewRiskMapFig()
#plt.figure();
#plt.imshow(self.gm.riskMapArray,origin='lower')
#goalx,goaly = self.gm.GetPxPyFromLatLon(goal[0],goal[1])
goalx, goaly = self.gm.GetXYfromLatLon(goal[0], goal[1])
if kwargs.has_key('goal_marker'):
plt.plot(goalx, goaly, kwargs['goal_marker'])
if kwargs.has_key('plot_currents'):
if kwargs['plot_currents']:
self.gm.PlotCurrentField(startT)
self.gm.InitSim(start[0],start[1],goal[0],goal[1],self.max_dive_depth,startT, \
self.perform_full_simulation,self.hold_vals_off_map)
xFin,yFin,latFin,lonFin,latArray,lonArray,depthArray,tArray,possibleCollision,CollisionReason,totalDist = \
self.gm.SimulateDive_R(self.no_comms_timeout_secs/3600.)
diveTime = self.gm.t_prime - startT
self.diveTime,self.latArray,self.lonArray,self.depthArray,self.tArray,self.possibleCollision,self.totalDist = \
diveTime, latArray, lonArray, depthArray, tArray, possibleCollision, totalDist
if plot_figure:
if possibleCollision == False:
tempX, tempY = self.gm.GetXYfromLatLon(np.array(latArray),
np.array(lonArray))
x_sims, y_sims = tempX[-1:], tempY[-1:]
plt.plot(tempX, tempY, line_color)
else:
tempX, tempY = self.gm.GetXYfromLatLon(np.array(latArray),
np.array(lonArray))
plt.plot(tempX, tempY, 'r.-')
x_sims, y_sims = 0, 0
if self.gm.doneSimulating:
print 'Surfaced due to waypoint at %.4f, %.4f' % (latFin, lonFin)
else:
print 'Surfaced due to no-comms in time %.2f hrs' % (diveTime)
return latFin, lonFin, self.gm.doneSimulating
def GetPseudoWayPt(self, start, goal, startT):
latFin, lonFin, doneSimulating = self.SimulateDive(start, goal, startT)
goalLat, goalLon = goal
pLat = 2 * goalLat - latFin
pLon = 2 * goalLon - lonFin
return pLat, pLon, doneSimulating
def IteratePseudoWptCalculation(self, start, goal, startT, numTimes=2):
pGoal = goal
goalLat, goalLon = goal
p1Lat, p1Lon = goal
closest = float('inf')
for i in range(0, numTimes):
print 'Iter %d/%d' % (i + 1, numTimes)
pLat, pLon, doneSimulating = self.GetPseudoWayPt(
start, pGoal, startT)
pLatFin, pLonFin, doneSimulating = self.SimulateDive(
start, (pLat, pLon),
startT,
plot_figure=True,
line_color='g',
goal_marker='b.')
deltaLat, deltaLon = goalLat - pLatFin, goalLon - pLonFin
dist2goal = self.dc.GetDistBetweenLocs(goalLat, goalLon, pLatFin,
pLonFin)
if dist2goal < closest and doneSimulating:
p1Lat, p1Lon = pLat, pLon
closest = dist2goal
pGoal = (goalLat + deltaLat, goalLon + deltaLon)
return p1Lat, p1Lon
def GetPseudoWptForStartTimeRange(self,
start,
goal,
startT1,
startT2,
numTimes=2):
if startT1 > startT2:
print 'startT1 (%.1f) should be less than startT2 (%.1f)' % (
startT1, startT2)
pLats, pLons, errs = [], [], []
for startT in range(startT1, startT2):
pLat, pLon = self.IteratePseudoWptCalculation(
start, goal, startT, numTimes)
pLatFin, pLonFin, doneSimulating = self.SimulateDive(
start, (pLat, pLon), startT)
dist2goal = self.dc.GetDistBetweenLocs(goalLat, goalLon, pLatFin,
pLonFin)
pLats.append(pLat)
pLons.append(pLon)
errs.append(dist2goal)
return pLats, pLons, errs
class SA_GP_MDP(object):
def __init__(self,
shelfName='RiskMap3.shelf',
sfcst_directory='./',
dMax=1.5):
''' Args:
* shelfName (str) : Name of shelf-file that contains obstacle and risk maps.
* sfcst_directory( str ): Directory in which ROMS small forecast files have been stored.
'''
self.gm = GliderModel(shelfName, sfcst_directory)
self.gs = GliderSimulator(shelfName, sfcst_directory)
self.gsR = GliderSimulator(shelfName, sfcst_directory)
self.Dmax = dMax
self.maxLengths = 50
self.acceptR = 0.6
self.locG = self.gm.lpGraph.copy()
for a in self.locG.nodes():
print a
np.set_printoptions(precision=2)
self.possibleCollision = False
self.Rwd = {}
self.mdp = {}
self.gamma = 1.0
self.maxDepth = 80.
self.w_r = -1.
self.w_g = 10.
self.theta = {}
self.theta['w_r'], self.theta['w_g'] = self.w_r, self.w_g
self.RewardsHaveChanged = False
self.tMax = 48
self.tExtra = 12
#self.ReInitializeMDP()
def GetTransitionModelFromShelf(self,
yy,
mm,
dd,
s_indx,
e_indx,
posNoise=None,
currentNoise=None,
shelfDirectory='.'):
""" Loads up Transition models from the shelf for a given number of days, starting from a particular day, and a given amount of noise in position and/or a given amount of noise in the current predictions. We assume these models have been created earlier using ProduceTransitionModels.
Args:
* yy (int): year
* mm (int): month
* dd (int): day
* s_indx (int): start index for the transition model
* e_indx (int): end index for the transition model
* posNoise (float): Amount of std-deviation of the random noise used in picking the start location
* currentNoise (float): Amount of prediction noise in the ocean model
* shelfDirectory (str): Directory in which the Transition models are located.
Updates:
* self.gm.FinalLocs: Stores the final locations
"""
print 'Loading Transition model from Shelf'
self.posNoise = posNoise
self.currentNoise = currentNoise
#import pdb; pdb.set_trace()
if posNoise == None and currentNoise == None:
gmShelf = shelve.open(
'%s/gliderModelGP2_%04d%02d%02d_%d_%d.shelf' %
(shelfDirectory, yy, mm, dd, s_indx, e_indx),
writeback=False)
if posNoise != None:
if currentNoise != None:
gmShelf = shelve.open(
'%s/gliderModelGP2_%04d%02d%02d_%d_%d_%.3f_RN_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, s_indx, e_indx, posNoise,
currentNoise),
writeback=False)
else:
gmShelf = shelve.open(
'%s/gliderModelGP2_%04d%02d%02d_%d_%d_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, s_indx, e_indx, posNoise),
writeback=False)
if posNoise == None and currentNoise != None:
gmShelf = shelve.open(
'%s/gliderModelGP2_%04d%02d%02d_%d_%d_RN_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, s_indx, e_indx, currentNoise),
writeback=False)
self.gm.TransModel = gmShelf['TransModel']
#if gmShelf.has_key('FinalLocs'):
self.gm.FinalLocs = gmShelf['FinalLocs']
if gmShelf.has_key('TracksInModel'):
self.gm.TracksInModel = gmShelf['TracksInModel']
gmShelf.close()
# Now that we have loaded the new transition model, we better update our graph.
self.ReInitializeMDP()
def ReInitializeMDP(self):
self.mdp['U'] = np.zeros((
self.gm.Height,
self.gm.Width,
))
self.mdp['Uprime'] = np.zeros((self.gm.Height, self.gm.Width))
self.locG = self.gm.lpGraph.copy()
self.BuildTransitionGraph()
def BuildTransitionGraph(self):
''' Compute the transition graph from all the available edges in the transition model '''
self.g = nx.DiGraph()
for a in self.locG.nodes():
for b in self.locG.nodes():
if a != b:
i, j = self.GetXYfromNodeStr(a)
x, y = self.GetXYfromNodeStr(b)
edge_str = '%d,%d,%d,%d' % (i, j, x, y)
if self.gm.TransModel.has_key(edge_str):
W_, H_, T_ = self.gm.TransModel[edge_str][2].shape
rwd = self.GetRewardForStateAction((i, j), edge_str)
self.g.add_edge((i, j), (x, y),
weight=rwd,
trapped=False)
# Store a copy of g, so that we can re-run the MDP when needed. This
# is in case we end up adding multiple goals etc.
self.g_copy = self.g.copy()
def GetXYfromNodeStr(self, nodeStr):
''' Convert from the name of the node string to the locations.
Args:
nodeStr (string): A string in the form of '(x,y)'.
Returns:
x,y if the string is in the form expected.
None,None if a mal-formed string.
'''
m = re.match('\(([0-9\.]+),([0-9\.]+)\)', nodeStr)
if m:
return int(m.group(1)), int(m.group(2))
else:
return None, None
def GetRewardForStateAction(self, state, action):
''' Get the reward for a state-action pair. Use a dictionary
to perform a lookup if we have already computed this earlier.
Args:
state (tuple) : node we are currently evaluating. (t1,x1,y1)
action (tuple): action (or edge) we are currently computing the reward for.
Updates:
self.Rwd[state-action-pair] with computed reward.
Returns:
self.Rwd[state-action-pair] if previously computed, else compute it and return result.
'''
OffMapNotObs = True # Treat locations off the map as high-risk locations
state_action_pair = (state, action)
if self.Rwd.has_key(state_action_pair) and not self.RewardsHaveChanged:
return self.Rwd[state_action_pair]
else:
t, x, y = state
X, Y, T = self.gm.FinalLocs[action]
tot_rwd = 0.
totNum = len(np.nonzero(self.gm.FinalLocs[action][0][0]))
for i in range(0, totNum):
lat, lon = self.gm.GetLatLonfromXY(x + X[i], y + Y[i])
riskVal = self.gm.GetRisk(lat, lon, OffMapNotObs) * self.w_r
tot_rwd += riskVal
if totNum > 0:
self.Rwd[state_action_pair] = tot_rwd / totNum
else:
self.Rwd[state_action_pair] = tot_rwd
print state, action, self.Rwd[state_action_pair]
self.RewardsHaveChanged = False
return self.Rwd[state_action_pair]
def SetGoalAndInitTerminalStates(self, goal, rewardVal=10.):
''' Set the goal location and initialize everything including terminal states.
Args:
goal(x,y) : tuple with (x,y) given in graph coordinates.
rewardVal (float) : Reward value for getting to the goal. Defaults to 10.
'''
self.ReInitializeMDP()
self.mdp['U'][goal[1], goal[0]] = rewardVal
self.mdp['Uprime'] = self.mdp['U'].copy()
self.goal = goal
self.goalReached = False
''' The goal is a trapping state, so we remove all the out-going edges from the goal, and
add a self-edge with weight zero. '''
for u, v, d in self.g.in_edges((goal[0], goal[1]), data=True):
d['weight'] = rewardVal
d['trapped'] = False
d['goal_edge'] = True
for u, v, d in self.g.out_edges((goal[0], goal[1]), data=True):
self.g.remove_edge(u, v)
self.g.add_edge((goal[0], goal[1]), (goal[0], goal[1]), weight=0.)
def SetGoalAndRewardsAndInitTerminalStates(self, goal, theta):
''' Set the goal location and initialize everything including terminal states.
Args:
goal(x,y) : tuple with (x,y) given in graph coordinates.
theta (dict) : theta['w_r'] - reward for normal states,
theta['w_g'] - reward for goal states
'''
print 'Using new theta:', theta
self.w_r = theta['w_r']
self.w_g = theta['w_g']
self.theta = theta
self.RewardsHaveChanged = True
self.ReInitializeMDP()
self.mdp['U'][(goal[1], goal[0])] = self.w_g
self.mdp['Uprime'] = self.mdp['U'].copy()
self.goal = goal
self.goalReached = False
''' The goal is a trapping state, so we remove all the out-going edges from the goal, and
add a self-edge with weight zero. '''
for u, v, d in self.g.in_edges((goal[0], goal[1]), data=True):
d['weight'] = self.w_g
d['goal_edge'] = True
for u, v, d in self.g.out_edges((goal[0], goal[1]), data=True):
self.g.remove_edge(u, v)
self.g.add_edge((goal[0], goal[1]), (goal[0], goal[1]), weight=0.)
def doValueIteration(self, eps=0.00001, MaxRuns=50):
''' Perform Value Iteration.
Args:
eps( float ): epsilon -> a small value which indicates the maximum change in utilities over
an iteration.
MaxRuns (int): maximum number of runs to do value iteration for. If negative, we will quit when the
epsilon criterion is reached.
Note: set the value of gamma between 0 and 1. Gamma is 1 by default.
'''
print 'Doing Value Iteration'
for i in range(0, MaxRuns):
print 'Iteration %d' % (i)
iterStartTime = time.time()
lastTime = time.time()
self.mdp['U'] = self.mdp['Uprime'].copy()
self.delta = 0.
for nodeNum, node in enumerate(self.g.nodes()):
x, y = node
Utils = self.CalculateTransUtilities((x, y))
ExpUtilVec = Utils.values()
if nodeNum % 10 == 0:
print '%d) %.3f' % (nodeNum,
time.time() - iterStartTime), (x, y)
#print t1,x,y,ExpUtilVec
#import pdb; pdb.set_trace()
if len(ExpUtilVec):
try:
maxExpectedUtility = np.max(ExpUtilVec)
#print x,y,maxExpectedUtility
self.mdp['Uprime'][y, x] = maxExpectedUtility
except ValueError:
import pdb
pdb.set_trace()
maxExpectedUtility = np.max(ExpUtilVec.all())
print maxExpectedUtility, ExpUtilVec
absDiff = abs(self.mdp['Uprime'][y, x] - self.mdp['U'][y, x])
if (absDiff > self.delta):
self.delta = absDiff
print 'delta=%f' % (self.delta)
print 'U', self.mdp['U']
if (self.delta <= eps * (1 - self.gamma) / self.gamma):
print 'delta is smaller than eps %f. Terminating.' % (
self.delta)
break
iterEndTime = time.time()
print 'Took %f seconds for iteration %d.' % (iterEndTime -
iterStartTime, i)
# Get Policy tree.
self.GetPolicyTreeForMDP()
def CalculateTransUtilities(self, state):
''' Get all the transition utilities based upon all the actions we can take from this state.
Args:
state( x, y ) : tuple containing time, x and y locations in graph co-ordinates.
'''
x, y = state
# Transition graph should have all the outgoing edges from this place via neighbors of this node.
# To see weights in graph we can do self.g[(i,j)][(k,l)]['weight']
UtilVec = {}
for u, v, d in self.g.out_edges((x, y), data=True):
sa_rwd = d['weight']
''' Goals are trapping terminal states '''
if d.has_key('goal_edge'):
if self.goalReached:
sa_rwd = 0.
else:
self.goalReached = True
# Now, set the weight of this edge to zero.
d['weight'] = 0.
d['trapped'] = True
x1, y1 = u
x2, y2 = v
UtilVec[(
u, v
)] = sa_rwd + self.gamma * self.GetUtilityForStateTransition(u, v)
return UtilVec
def GetUtilityForStateTransition(self, state, state_prime):
''' Compute the utility for performing the state transition of going from state to state_prime.
Args:
state( tuple of int, int, int ) : state t, x, y in planning graph coods.
state_prime (int, int, int) : state_prime t,x,y in planning graph coods.
Returns:
Utility (float) : utility for this state transition. '''
x, y = state
xp, yp = state_prime
width, height = self.gm.Width, self.gm.Height
Util = 0.
totalProb = 0
transProb = 0
if yp == self.goal[1] and xp == self.goal[0]:
return self.mdp['U'][y, x]
stateTrans = '%d,%d,%d,%d' % (x, y, xp, yp)
if self.gm.TransModel.has_key(stateTrans):
transProbs = self.gm.TransModel[stateTrans][2]
tPSize = self.gm.TransModel[stateTrans][1]
zeroLoc = self.gm.TransModel[stateTrans][0]
W_, H_, T_ = self.gm.TransModel[stateTrans][2].shape
# iterate over all possible actions
for j in range(0, int(tPSize)):
for i in range(0, int(tPSize)):
if transProbs[j][i] > 0.:
state_action = (x + i - zeroLoc, y + j - zeroLoc)
if i != j:
if self.isOnMap(state_prime) and self.isOnMap(
state_action):
if not self.gm.ObsDetLatLon(self.gm.lat_pts[yp],self.gm.lon_pts[xp], \
self.gm.lat_pts[y], self.gm.lon_pts[x]):
Util += transProbs[j][i] * self.mdp['U'][
y + j - zeroLoc, x + i - zeroLoc]
transProb += transProbs[j][i]
if totalProb != 1:
transProb = 1 - totalProb
Util += transProb * -1. #self.mdp['U'][ t1,y,x ] ## Rest is probably a crash!!!
return Util
def GetRomsData(self,
yy,
mm,
dd,
numDays,
UpdateSelf=True,
usePredictionData=False):
''' Loads up ROMs data from a particular day, for a certain number of days and supports self-update
Args:
yy (int): year
mm (int): month
dd (int): day
numDays (int): number of days the model is being built over
UpdateSelf (bool): Should the MDP update itself with the ROMS data being loaded?
'''
useNewFormat = True
u, v, time1, depth, lat, lon = self.gm.GetRomsData(
yy, mm, dd, numDays, useNewFormat, usePredictionData)
u, v, time1, depth, lat, lon = self.gs.gm.GetRomsData(
yy, mm, dd, numDays, useNewFormat, usePredictionData)
u, v, time1, depth, lat, lon = self.gsR.gm.GetRomsData(
yy, mm, dd, numDays, useNewFormat, usePredictionData)
if UpdateSelf:
self.u, self.v, self.time1, self.depth, self.lat, self.lon = u, v, time1, depth, lat, lon
self.yy, self.mm, self.dd = yy, mm, dd
self.numDays = numDays
self.gm.numDays = numDays
return u, v, time1, depth, lat, lon
def isOnMap(self, s):
''' Test whether the state being tested is within the map.
Args:
s (tuple(x,y) ) : tuple with 't','x' and 'y' values
'''
x, y = s
if x < 0 or x >= self.gm.Width:
return False
if y < 0 or y >= self.gm.Height:
return False
return True
''' -------------------------------------------------------------------------------------------
---------------------------------- Code to create the policy tree -----------------------------
--------------------------------------------------------------------------------------------'''
def DisplayPolicyAtTimeT(self, FigHandle=None):
''' Display MDP policy
Args:
FigHandle -> Pyplot.Figure handle if we want to draw the policy on the
previous figure. If this is not passed, a new figure will be created.
'''
width, height = self.gm.Width, self.gm.Height
Xpolicy = np.zeros((width, height))
Ypolicy = np.zeros((width, height))
Xloc, Yloc = np.zeros((width, height)), np.zeros((width, height))
Uprime = self.mdp['U']
if FigHandle == None:
plt.figure()
self.gm.PlotNewRiskMapFig()
for a in self.locG.nodes():
x, y = self.GetXYfromNodeStr(a)
Xloc[x][y], Yloc[x][y] = x, y
UtilVec = np.zeros(8 * 60)
maxUtil = -float('inf')
k, maxU, maxV = 0, 0, 0
a1 = (x, y)
for u, v, d in self.g.out_edges(a1, data=True):
#i,j = self.GetXYfromNodeStr(v)
i, j = v
UtilVec[k] = Uprime[t, j, i]
if maxUtil <= UtilVec[k]:
maxUtil = UtilVec[k]
maxU, maxV = i - x, j - y
k = k + 1
Xpolicy[x][y], Ypolicy[x][y] = 0.5 * maxU, 0.5 * maxV
plt.quiver(Xloc,
Yloc,
Xpolicy,
Ypolicy,
scale=10 * math.sqrt(maxU**2 + maxV**2))
plt.title('MDP Policy')
return Xpolicy, Ypolicy
def GetPolicyTreeForMDP(self):
''' Get the policy tree for the MDP. This is required in order for us to be able to
simulate.
'''
width, height = self.gm.Width, self.gm.Height
Uprime = self.mdp['Uprime']
self.gm2 = nx.DiGraph()
for a in self.g.nodes():
x, y = a
UtilVec = np.zeros(8 * 60)
maxUtil = -float('inf')
k, maxU, maxV = 0, 0, 0
#import pdb; pdb.set_trace()
for u, v, d in self.g.out_edges(a, data=True):
i, j = v
UtilVec[k] = Uprime[j, i]
if maxUtil <= UtilVec[k]:
maxUtil = UtilVec[k]
maxU, maxV = i - x, j - y
bestNode = v
k = k + 1
if not (maxU == 0 and maxV == 0):
self.gm2.add_edge(a, bestNode, weight=maxUtil)
'''
if self.UseNetworkX == False:
dot = write(self.gm2)
G = gv.AGraph(dot)
G.layout(prog = 'dot')
G.draw('SCB_MDP_MSG.png')
'''
return self.gm2
def GetPolicyAtCurrentNode(self, curNode, goal, forceGoal=False):
#self.gLat, self.gLon = self.gm.GetLatLonfromXY(self.b[0], self.b[1])
#if (self.gLat,self.gLon)!=self.lastGoal:
# self.lastGoal = (self.gLat,self.gLon)
# self.PolicyToGoal.append(lastGoal)
print 'Current Node: (%f,%f)' % (curNode[0], curNode[1])
print 'Goal Node: (%f,%f)' % (self.goal[0], self.goal[1])
if curNode == self.goal:
targetX, targetY = self.goal[0], self.goal[1]
a = (t, int(curNode[0] + 0.5), int(curNode[1] + 0.5))
i, j = None, None
for u, v, d in self.gm2.out_edges(a, data=True):
i, j = v
targetX, targetY = i, j
#if targetX==None and targetY==None:
# targetX,targetY = self.FindNearestNodeOnGraph(curNode,t)
targetLat, targetLon = self.gm.GetLatLonfromXY(targetX, targetY)
if (targetX, targetY) != self.lastGoal:
self.lastGoal = (targetX, targetY)
self.PolicyToGoal.append((targetLat, targetLon))
print 'Next Node: (%f,%f)' % (targetX, targetY)
return targetX, targetY
def FindNearestNodeOnGraph(self, curNode, t):
nearest_dist = float('inf')
best_utility = -float('inf')
nearest_node = (None, None)
for a in self.locG.nodes():
i, j = curNode
x, y = self.GetXYfromNodeStr(a)
#t1,x,y = a
dist = math.sqrt((i - x)**2 + (j - y)**2)
#if not self.gm.ObsDetLatLon(self.gm.lat_pts[j],self.gm.lon_pts[i], \
# self.gm.lat_pts[y],self.gm.lon_pts[x]):
lat1, lon1 = self.gm.GetLatLonfromXY(i, j)
lat2, lon2 = self.gm.GetLatLonfromXY(x, y)
if not self.gm.ObsDetLatLon(lat1, lon1, lat2, lon2):
if self.mdp['U'][
y, x] > best_utility and dist < self.Dmax and self.mdp[
'U'][y, x] != 0.:
best_utility = self.mdp['U'][y, x]
nearest_node = (x, y)
return nearest_node
''' -------------------------------------------------------------------------------------------
---------------------------------- Code for Performing Simulations ----------------------------
--------------------------------------------------------------------------------------------'''
def GetRiskFromXY(self, x, y):
''' Looks up the risk value for x,y from the risk-map
Args:
x,y (float,float) : x and y in graph coordinates.
'''
lat, lon = self.gm.GetLatLonfromXY(x, y)
return self.gm.GetRisk(lat, lon)
def SimulateAndPlotMDP_PolicyExecution(self,
start,
goal,
simStartTime,
newFig=True,
lineType='r-',
NoPlot='False'):
''' Simulate and Plot the MDP policy execution.
Args:
start (x,y) : tuple containing the start vertex on the graph
goal (x,y) : tuple containing the goal vertex on the graph
k (int) : time in hours for the simulation to start from
newFig (bool) : default=True, creates a new figure if set to true. If multiple simulations need overlaying, this is set to false.
lineType (string): matplotlib line type. defaults to 'r-'
NoPlot (bool) : default = False. Indicates that we do not want this plotted. (FIXME: Needs implementation).
Returns:
totalRisk (float): total risk associated with the path
collisionState (bool): True if collided with land. False if no collision detected.
'''
if newFig:
plt.figure()
plt.title('Plotting Min-Exp Risk Ensemble')
self.gm.PlotNewRiskMapFig()
self.totalRisk = 0
self.distFromGoal = float('inf')
self.totalPathLength = 0.
self.totalTime = 0
self.goal = goal
self.start = start
self.numHops = 0
self.isSuccess = False
self.PolicyToGoal = []
self.lastGoal = (None, None)
return self.gs.SimulateAndPlot_PolicyExecution_NS(
start, goal, simStartTime, self.maxDepth,
self.GetPolicyAtCurrentNode, lineType, newFig)
def InitMDP_Simulation(self,
start,
goal,
startT,
lineType='r',
newFig=False):
''' Initialize Simulation of the MDP policy execution.
Args:
start (x,y) : tuple containing the start vertex on the graph
goal (x,y) : tuple containing the goal vertex on the graph
startT (int) : time in hours for the simulation to start from
lineType (string): matplotlib line type. defaults to 'r-'
newFig (bool) : default=True, creates a new figure if set to true. If multiple simulations need overlaying, this is set to false.
'''
self.DoFullSim = False
self.HoldValsOffMap = True
if newFig:
plt.figure()
plt.title('Plotting Min-Exp Risk Ensemble')
self.gm.PlotNewRiskMapFig()
self.totalRisk = 0
self.distFromGoal = float('inf')
self.totalPathLength = 0.
self.totalTime = 0
self.goal = goal
self.start = start
self.numHops = 0
self.isSuccess = False
self.a = self.start
self.done = False
self.Indx = 0
self.lineType = lineType
self.possibleCollision = False
#x_sims,y_sims = np.zeros((24*gm.numDays,1)),np.zeros((24*gm.numDays,1))
#if startT>=(24*self.gm.numDays):
# startT = 24*self.gm.numDays-1
self.startT = startT
self.x_sims, self.y_sims = 0, 0
self.finX, self.finY = start[0], start[1]
self.sLat, self.sLon = self.gm.GetLatLonfromXY(self.start[0],
self.start[1])
self.gLat, self.gLon = self.gm.GetLatLonfromXY(self.goal[0],
self.goal[1])
self.latArray, self.lonArray = [], []
#self.gm.InitSim(self.sLat,self.sLon,self.gLat,self.gLon,self.gm.MaxDepth,startT,self.DoFullSim,self.HoldValsOffMap)
#import pdb; pdb.set_trace()
self.lastLegComplete = True
self.bx, self.by = None, None
self.lastTransition = None
self.PolicyToGoal = [
] # A new addition which uses the simulated policy to goal
# to have a policy look-ahead so that in case
# the glider does not communicate back, there is a
# fall-back plan for it toward the goal.
self.lastGoal = (None, None)
def SimulateAndPlotMDP_PolicyExecution_R(self,
simulTime=-1,
PostDeltaSimCallback=None,
PostSurfaceCallbackFn=None):
''' Simulate and plot the MDP policy execution in a re-entrant manner. This simulation is very useful
when we want to create a movie of how things are progressing. It can be called over and over again
after a single call to InitMDP_Simulation_
Args:
simulTime (float) : indicates the maximum amount of time we want to simulate for in hours. Defaults to -1, which means that it will only exit after completing the simulation.
PostDeltaSimCallback: default=None. This is a user-defined callback function which will be executed upon completion of the simulation.
PostSurfaceCallbackFn: default=None. This is a user-defined callback function which will be executed when the glider surfaces. This might happen
in between a surfacing.
Returns:
totalRisk (float): total risk associated with the path
collisionState (bool): True if collided with land. False if no collision detected.
'''
i = self.Indx
tStart = self.startT
#self.lastLegComplete = self.gm.doneSimulating;
if self.lastLegComplete == True:
self.numHops += 1
try:
self.lastTransition = [(int(self.a[0] + 0.5),
int(self.a[1] + 0.5)),
(self.bx, self.by)]
#self.bx, self.by = self.GetPolicyAtCurrentNode('(%d,%d)' % (int(self.a[0]), int(self.a[1])), '(%d,%d)' % (self.goal[0], self.goal[1]))
self.bx, self.by = self.GetPolicyAtCurrentNode(
(self.a[0], self.a[1]), (self.bx, self.by))
if PostSurfaceCallbackFn != None:
PostSurfaceCallbackFn(self.latArray, self.lonArray)
self.b = (self.bx, self.by)
self.sLat, self.sLon = self.gm.GetLatLonfromXY(
self.a[0], self.a[1])
self.gLat, self.gLon = self.gm.GetLatLonfromXY(
self.b[0], self.b[1])
if (self.gLat, self.gLon) != self.lastGoal:
self.lastGoal = (self.gLat, self.gLon)
self.PolicyToGoal.append(self.lastGoal)
self.gm.InitSim(self.sLat, self.sLon, self.gLat, self.gLon,
self.gm.MaxDepth, tStart, self.DoFullSim,
self.HoldValsOffMap)
except TypeError:
self.bx, self.by = None, None
if self.bx != None and self.by != None:
#self.b = (self.bx, self.by)
#self.sLat, self.sLon = self.gm.GetLatLonfromXY(self.a[0], self.a[1])
#self.gLat, self.gLon = self.gm.GetLatLonfromXY(self.b[0], self.b[1])
xFin, yFin, latFin, lonFin, latArray, lonArray, depthArray, tArray, possibleCollision, CollisionReason, totalDist = \
self.gm.SimulateDive_R(simulTime) # If this is <1 it will have the same behavior as before.
self.xFin, self.yFin, self.latFin, self.lonFin, self.latArray, self.lonArray, self.depthArray, self.tArray, self.possibleCollision = \
xFin, yFin, latFin, lonFin, latArray, lonArray, depthArray, tArray, possibleCollision
self.totalPathLength += totalDist
self.CollisionReason = CollisionReason
self.lastLegComplete = self.gm.doneSimulating
if PostDeltaSimCallback != None:
PostDeltaSimCallback(latArray, lonArray)
self.distFromGoal = math.sqrt((self.a[0] - self.goal[0])**2 +
(self.a[1] - self.goal[1])**2)
if self.distFromGoal <= self.acceptR:
self.isSuccess = True
self.done = True
if len(tArray > 0):
self.totalTime += tArray[-1]
self.thisSimulTime = tArray[-1]
tStart = self.startT + self.totalTime / 3600.
if tStart >= 24 * self.gm.numDays:
tStart = 24 * self.gm.numDays - 1
if possibleCollision == False:
tempX, tempY = self.gm.GetXYfromLatLon(np.array(latArray),
np.array(lonArray))
self.x_sims, self.y_sims = tempX[-1:], tempY[-1:]
plt.plot(tempX, tempY, self.lineType)
if self.x_sims != [] and self.y_sims != []:
if self.lastLegComplete:
tempRisk = self.GetRiskFromXY(self.x_sims, self.y_sims)
self.finX, self.finY = self.x_sims, self.y_sims
self.totalRisk += tempRisk
else:
if self.lastLegComplete:
self.totalRisk += self.gm.GetRisk(self.sLat, self.sLon)
else:
if self.CollisionReason == 'RomsNanAtStart':
self.totalRisk += self.gm.GetRisk(self.sLat, self.sLon)
tempX, tempY = self.gm.GetXYfromLatLon(np.array(latArray),
np.array(lonArray))
if len(tempX) > 0 and len(tempY) > 0:
if self.CollisionReason == 'Obstacle' or self.CollisionReason == 'RomsNanLater':
self.totalRisk += self.GetRiskFromXY(
tempX[-1:], tempY[-1:])
self.finX, self.finY = tempX[-1], tempY[-1]
plt.plot(tempX, tempY, self.lineType)
self.x_sims, self.y_sims = 0, 0
self.done = True
return self.totalRisk, True # Landed on beach!
try:
self.a = (self.x_sims[0], self.y_sims[0])
self.finX, self.finY = self.a
except IndexError:
self.done = True
return self.totalRisk, True # Landed on beach
#import pdb; pdb.set_trace()
if self.lastLegComplete == True: # Finished simulating a leg.
i = i + 1
#if i > self.maxLengths:
# self.CollisionReason = 'MaxHopsExceeded'
# self.done = True
else: # Since this is a re-entrant simulator... Get done here...
self.done = True
else: # We did not get a policy here.
self.CollisionReason = 'DidNotFindPolicy'
self.done = True
return self.totalRisk, False
class GliderSimulator_R(object):
''' Stand-alone generic re-entrant simulator for a glider in ROMS data.
Earlier, each of my planners had its own implementation of a simulator built into it.
This is highly repetitive and as someone wisely said
"Repetition" is defined as "a host site for a defect infection".
So henceforth, we are going to use this as the entry-point for simulation.
Users create a simulation instance where they define the type of simulation they need,
and then forward propagate this.
This class could have been written using Generators on the GliderSimulator, but I have
used this version mainly because I intend porting this over to C++ which does not
have generators.
'''
def __init__(self, riskMap, romsDataDir='./', acceptR=0.6, **kwargs):
self.gm = GliderModel(riskMap, romsDataDir)
self.Init_Simulation()
self.acceptR = acceptR
def Init_Simulation(self):
''' Initialize Simulation of the MDP policy execution.
Args:
start (x,y) : tuple containing the start vertex on the graph
goal (x,y) : tuple containing the goal vertex on the graph
startT (int) : time in hours for the simulation to start from
lineType (string): matplotlib line type. defaults to 'r-'
newFig (bool) : default=True, creates a new figure if set to true. If multiple simulations need overlaying, this is set to false.
'''
self.DoFullSim = False
self.HoldValsOffMap = True
self.totalRisk = 0
self.distFromGoal = float('inf')
self.totalPathLength = 0.
self.totalTime = 0
self.numHops = 0
self.isSuccess = False
self.done = False
self.Indx = 0
self.possibleCollision = False
#x_sims,y_sims = np.zeros((24*gm.numDays,1)),np.zeros((24*gm.numDays,1))
#if startT>=(24*self.gm.numDays):
# startT = 24*self.gm.numDays-1
self.x_sims, self.y_sims = 0, 0
self.latArray, self.lonArray = [], []
#self.gm.InitSim(self.sLat,self.sLon,self.gLat,self.gLon,self.gm.MaxDepth,startT,self.DoFullSim,self.HoldValsOffMap)
#import pdb; pdb.set_trace()
self.lastLegComplete = True
self.bx, self.by = None, None
self.lastTransition = None
def Start_Simulation(self,
start,
goal,
startT,
maxDepth=80.,
lineType='r-',
newFig=False):
self.goal = goal
self.start = start
self.a = self.start
self.startT = startT
self.lineType = lineType
self.finX, self.finY = start[0], start[1]
self.sLat, self.sLon = self.gm.GetLatLonfromXY(self.start[0],
self.start[1])
self.gLat, self.gLon = self.gm.GetLatLonfromXY(self.goal[0],
self.goal[1])
self.Init_Simulation()
if newFig:
plt.figure()
plt.title('Plotting Min-Exp Risk Ensemble')
self.gm.PlotNewRiskMapFig()
def SetupCallbacks(self,
GetPolicyAtNodeCallbackFn=None,
PostDeltaSimCallbackFn=None,
PostSurfaceCallbackFn=None):
self.GetPolicyAtNodeCallbackFn = GetPolicyAtNodeCallbackFn
self.PostSurfaceCallbackFn = PostSurfaceCallbackFn
self.PostDeltaSimCallbackFn = PostDeltaSimCallbackFn
def GetRiskFromXY(self, x, y):
''' Looks up the risk value for x,y from the risk-map
Args:
x,y (float,float) : x and y in graph coordinates.
'''
lat, lon = self.gm.GetLatLonfromXY(x, y)
return self.gm.GetRisk(lat, lon)
def SimulateAndPlot_PolicyExecution_R(self, simulTime=-1):
''' Simulate and plot the MDP policy execution in a re-entrant manner. This simulation is very useful
when we want to create a movie of how things are progressing. It can be called over and over again
after a single call to InitMDP_Simulation_
Args:
simulTime (float) : indicates the maximum amount of time we want to simulate for in hours. Defaults to -1, which means that it will only exit after completing the simulation.
PostDeltaSimCallback: default=None. This is a user-defined callback function which will be executed upon completion of the simulation.
PostSurfaceCallbackFn: default=None. This is a user-defined callback function which will be executed when the glider surfaces. This might happen
in between a surfacing.
Returns:
totalRisk (float): total risk associated with the path
collisionState (bool): True if collided with land. False if no collision detected.
'''
i = self.Indx
tStart = self.startT
#self.lastLegComplete = self.gm.doneSimulating;
if self.lastLegComplete == True:
self.numHops += 1
try:
#print 'Simulate.py before getpolicy:',self.bx, self.by
self.lastTransition = [(int(self.a[0] + 0.5),
int(self.a[1] + 0.5)),
(self.bx, self.by)]
# Used to be an integer
#self.bx, self.by = self.GetPolicyAtNodeCallbackFn((int(self.a[0]+0.5),int(self.a[1]+0.5)), self.goal)
#self.bx, self.by = self.GetPolicyAtNodeCallbackFn(self.a[0],self.a[1], self.goal) # Seems to be causing issues with debugSA_MDP.py
self.bx, self.by = self.GetPolicyAtNodeCallbackFn(
(self.a[0], self.a[1]), self.goal)
#self.bx, self.by = self.GetPolicyAtNodeCallbackFn('(%d,%d)' % (int(self.a[0]), int(self.a[1])), '(%d,%d)' % (self.goal[0], self.goal[1]))
#print 'Simulate.py after getpolicy:',self.bx, self.by
if self.PostSurfaceCallbackFn != None:
self.PostSurfaceCallbackFn(self.latArray, self.lonArray)
self.plot(a[0], a[1], 'k.')
self.b = (self.bx, self.by)
self.sLat, self.sLon = self.gm.GetLatLonfromXY(
self.a[0], self.a[1])
self.gLat, self.gLon = self.gm.GetLatLonfromXY(
self.b[0], self.b[1])
self.gm.InitSim(self.sLat, self.sLon, self.gLat, self.gLon,
self.gm.MaxDepth, tStart, self.DoFullSim,
self.HoldValsOffMap)
except TypeError:
self.bx, self.by = None, None
if self.bx != None and self.by != None:
#self.b = (self.bx, self.by)
#self.sLat, self.sLon = self.gm.GetLatLonfromXY(self.a[0], self.a[1])
#self.gLat, self.gLon = self.gm.GetLatLonfromXY(self.b[0], self.b[1])
xFin, yFin, latFin, lonFin, latArray, lonArray, depthArray, tArray, possibleCollision, CollisionReason, totalDist = \
self.gm.SimulateDive_R(simulTime) # If this is <1 it will have the same behavior as before.
self.xFin, self.yFin, self.latFin, self.lonFin, self.latArray, self.lonArray, self.depthArray, self.tArray, self.possibleCollision = \
xFin, yFin, latFin, lonFin, latArray, lonArray, depthArray, tArray, possibleCollision
self.totalPathLength += totalDist
self.CollisionReason = CollisionReason
self.lastLegComplete = self.gm.doneSimulating
if self.PostDeltaSimCallbackFn != None:
self.PostDeltaSimCallbackFn(latArray, lonArray)
self.distFromGoal = math.sqrt((self.a[0] - self.goal[0])**2 +
(self.a[1] - self.goal[1])**2)
if self.distFromGoal <= self.acceptR:
self.isSuccess = True
self.done = True
if len(tArray) > 0:
self.totalTime += tArray[-1]
self.thisSimulTime = tArray[-1]
tStart = self.startT + self.totalTime / 3600.
if tStart >= 24 * self.gm.numDays:
tStart = 24 * self.gm.numDays - 1
if possibleCollision == False:
tempX, tempY = self.gm.GetXYfromLatLon(np.array(latArray),
np.array(lonArray))
self.x_sims, self.y_sims = tempX[-1:], tempY[-1:]
plt.plot(tempX, tempY, self.lineType)
if self.x_sims != [] and self.y_sims != []:
if self.lastLegComplete:
tempRisk = self.GetRiskFromXY(self.x_sims, self.y_sims)
self.finX, self.finY = self.x_sims, self.y_sims
self.totalRisk += tempRisk
else:
if self.lastLegComplete:
self.totalRisk += self.gm.GetRisk(self.sLat, self.sLon)
else:
if self.CollisionReason == 'RomsNanAtStart':
self.totalRisk += self.gm.GetRisk(self.sLat, self.sLon)
tempX, tempY = self.gm.GetXYfromLatLon(np.array(latArray),
np.array(lonArray))
if len(tempX) > 0 and len(tempY) > 0:
if self.CollisionReason == 'Obstacle' or self.CollisionReason == 'RomsNanLater':
self.totalRisk += self.GetRiskFromXY(
tempX[-1:], tempY[-1:])
self.finX, self.finY = tempX[-1], tempY[-1]
plt.plot(tempX, tempY, self.lineType)
self.x_sims, self.y_sims = 0, 0
self.done = True
return self.totalRisk, True # Landed on beach!
try:
self.a = (self.x_sims[0], self.y_sims[0])
self.finX, self.finY = self.a
except IndexError:
self.done = True
return self.totalRisk, True # Landed on beach
import pdb
pdb.set_trace()
if self.lastLegComplete == True: # Finished simulating a leg.
i = i + 1
else: # Since this is a re-entrant simulator... Get done here...
self.done = True
else: # We did not get a policy here.
self.CollisionReason = 'DidNotFindPolicy'
self.done = True
return self.totalRisk, False
def SimulateAndPlot_PolicyExecution_NS_R(self, simulTime=-1):
''' Simulate and plot the MDP policy execution in a re-entrant manner. This simulation is very useful
when we want to create a movie of how things are progressing. It can be called over and over again
after a single call to InitMDP_Simulation_
Args:
simulTime (float) : indicates the maximum amount of time we want to simulate for in hours. Defaults to -1, which means that it will only exit after completing the simulation.
PostDeltaSimCallback: default=None. This is a user-defined callback function which will be executed upon completion of the simulation.
PostSurfaceCallbackFn: default=None. This is a user-defined callback function which will be executed when the glider surfaces. This might happen
in between a surfacing.
Returns:
totalRisk (float): total risk associated with the path
collisionState (bool): True if collided with land. False if no collision detected.
'''
i = self.Indx
tStart = self.startT
#self.lastLegComplete = self.gm.doneSimulating;
if self.lastLegComplete == True:
self.numHops += 1
try:
#import pdb;pdb.set_trace()
#print 'Simulate.py before getpolicy:',self.bx, self.by
self.lastTransition = [(int(self.a[0] + 0.5),
int(self.a[1] + 0.5)),
(self.bx, self.by)]
# Used to be an integer
#self.bx, self.by = self.GetPolicyAtNodeCallbackFn((int(self.a[0]+0.5),int(self.a[1]+0.5)), self.goal)
#self.bx, self.by = self.GetPolicyAtNodeCallbackFn(self.a[0],self.a[1], self.goal) # Seems to be causing issues with debugSA_MDP.py
self.bx, self.by = self.GetPolicyAtNodeCallbackFn(
(self.a[0], self.a[1]), self.goal, tStart)
#self.bx, self.by = self.GetPolicyAtNodeCallbackFn('(%d,%d)' % (int(self.a[0]), int(self.a[1])), '(%d,%d)' % (self.goal[0], self.goal[1]))
#print 'Simulate.py after getpolicy:',self.bx, self.by
if self.PostSurfaceCallbackFn != None:
self.PostSurfaceCallbackFn(self.latArray, self.lonArray)
self.plot(a[0], a[1], 'k.')
self.b = (self.bx, self.by)
self.sLat, self.sLon = self.gm.GetLatLonfromXY(
self.a[0], self.a[1])
self.gLat, self.gLon = self.gm.GetLatLonfromXY(
self.b[0], self.b[1])
self.gm.InitSim(self.sLat, self.sLon, self.gLat, self.gLon,
self.gm.MaxDepth, tStart, self.DoFullSim,
self.HoldValsOffMap)
except TypeError:
self.bx, self.by = None, None
if self.bx != None and self.by != None:
#self.b = (self.bx, self.by)
#self.sLat, self.sLon = self.gm.GetLatLonfromXY(self.a[0], self.a[1])
#self.gLat, self.gLon = self.gm.GetLatLonfromXY(self.b[0], self.b[1])
xFin, yFin, latFin, lonFin, latArray, lonArray, depthArray, tArray, possibleCollision, CollisionReason, totalDist = \
self.gm.SimulateDive_R(simulTime) # If this is <1 it will have the same behavior as before.
self.xFin, self.yFin, self.latFin, self.lonFin, self.latArray, self.lonArray, self.depthArray, self.tArray, self.possibleCollision = \
xFin, yFin, latFin, lonFin, latArray, lonArray, depthArray, tArray, possibleCollision
self.totalPathLength += totalDist
self.CollisionReason = CollisionReason
self.lastLegComplete = self.gm.doneSimulating
if self.PostDeltaSimCallbackFn != None:
self.PostDeltaSimCallbackFn(latArray, lonArray)
self.distFromGoal = math.sqrt((self.a[0] - self.goal[0])**2 +
(self.a[1] - self.goal[1])**2)
if self.distFromGoal <= self.acceptR:
self.isSuccess = True
self.done = True
if len(tArray) > 0:
self.totalTime += tArray[-1]
self.thisSimulTime = tArray[-1]
tStart = self.startT + self.totalTime / 3600.
if tStart >= 24 * self.gm.numDays:
tStart = 24 * self.gm.numDays - 1
if possibleCollision == False:
tempX, tempY = self.gm.GetXYfromLatLon(np.array(latArray),
np.array(lonArray))
self.x_sims, self.y_sims = tempX[-1:], tempY[-1:]
plt.plot(tempX, tempY, self.lineType)
if self.x_sims != [] and self.y_sims != []:
if self.lastLegComplete:
tempRisk = self.GetRiskFromXY(self.x_sims, self.y_sims)
self.finX, self.finY = self.x_sims, self.y_sims
self.totalRisk += tempRisk
else:
if self.lastLegComplete:
self.totalRisk += self.gm.GetRisk(self.sLat, self.sLon)
else:
if self.CollisionReason == 'RomsNanAtStart':
self.totalRisk += self.gm.GetRisk(self.sLat, self.sLon)
tempX, tempY = self.gm.GetXYfromLatLon(np.array(latArray),
np.array(lonArray))
if len(tempX) > 0 and len(tempY) > 0:
if self.CollisionReason == 'Obstacle' or self.CollisionReason == 'RomsNanLater':
self.totalRisk += self.GetRiskFromXY(
tempX[-1:], tempY[-1:])
self.finX, self.finY = tempX[-1], tempY[-1]
plt.plot(tempX, tempY, self.lineType)
self.x_sims, self.y_sims = 0, 0
self.done = True
return self.totalRisk, True # Landed on beach!
try:
self.a = (self.x_sims[0], self.y_sims[0])
self.finX, self.finY = self.a
except IndexError:
self.done = True
return self.totalRisk, True # Landed on beach
import pdb
pdb.set_trace()
if self.lastLegComplete == True: # Finished simulating a leg.
i = i + 1
else: # Since this is a re-entrant simulator... Get done here...
self.done = True
else: # We did not get a policy here.
self.CollisionReason = 'DidNotFindPolicy'
self.done = True
return self.totalRisk, False