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 GetRomsCorrelations(object): ''' Class to compute ROMS auto-correlations. ''' def __init__(self,maxLagHrs): ''' __init__(maxLagHrs) Args: maxLagHrs (int): maximum number of lag-hours ''' self.maxLagHrs = maxLagHrs self.gm = GliderModel(conf.riskMapDir+'RiskMap.shelf',gpplib.gppConf.romsDataDir) self.numPts = len( self.gm.x_pts ) self.cumVec_u = np.zeros((self.numPts**2, 2* maxLagHrs-1)) self.cumVec_v = np.zeros((self.numPts**2, 2* maxLagHrs-1)) self.cumVec_s = np.zeros((self.numPts**2, 2* maxLagHrs-1)) self.tLags = np.zeros((self.numPts**2, 2* maxLagHrs-1)) self.totAcc = 0 def PlotCorrelationWithLocations(self,lags,cVecs,X,Y,lat,lon,lyMax,lxMax,figTitle,\ yLabel='Auto-Correlation\nCurrent m/s',saveFigName=None,Thresh=0.6,TimeScale=24): ''' ''' fig = plt.figure() ax1 = fig.add_subplot(211) ax2 = fig.add_subplot(212) ax2.axes.set_xlim(-1,lyMax+1 ) ax2.axes.set_ylim(-1,lxMax) ax1.axes.set_xlim((0,self.maxLagHrs)) ax1.axes.set_ylim((-0.5,1.1)) ax1.grid(True) ax1.yaxis.label.set_text(yLabel) #x_pts,y_pts = np.linspace(0.1,lxMax-0.1,self.numPts),np.linspace(0.1,lyMax-0.1,self.numPts) i=0 CorrMat = np.zeros((len(X),len(Y))) #np.zeros((len(x_pts),len(y_pts))) BinCorrMat = np.zeros((len(X),len(Y))) #np.zeros((len(x_pts),len(y_pts))) import pdb; pdb.set_trace() for j in range(0,len(Y)): for k in range(0,len(X)): x,y = X[k],Y[j] #plt.acorr(s_mean[:,x,y],usevlines=True,detrend=mlab.detrend_mean,normed=True,maxlags=None) #lags,cVec,linecols,lines = ax1.acorr(u_mean[:,y,x],usevlines=False,normed=True,maxlags=None,lw=2,color='#%02x%02x%02x'%((x*5+50)%256,(x*5)%256,(y*5)%256)) if self.gm.GetRisk(lat[y],lon[x])<1: ax1.plot(lags[i],cVecs[i],'.',color='#%02x%02x%02x'%((x*5+50)%256,(x*5)%256,(y*5)%256)) CorrMat[k,j] = cVecs[i,lags[0,-1]+TimeScale] #ax2.plot(x,y,'*',color='#%02x%02x%02x'%((x*5+50)%256,(x*5)%256,(y*5)%256),lw=5,ms=14) if cVecs[i,lags[0,-1]+TimeScale]>=Thresh: ax2.plot(x,y,'*',color='#0000ff',lw=5,ms=20) BinCorrMat[k,j] = 1.0 else: if cVecs[i,lags[0,-1]+TimeScale]<Thresh: ax2.plot(x,y,'x',color='#ff0000',lw=5,ms=20) BinCorrMat[k,j] = 0.0 else: ax2.plot(x,y,'.',color='#ffff00',lw=5,ms=10) CorrMat[k,j] = 0.0 BinCorrMat[k,j] = -1.0 i=i+1 if saveFigName != None: saveFigName='%s_Thresh_%.2f_TScale_%d.pdf'%(saveFigName,Thresh,TimeScale) plt.savefig(saveFigName) return CorrMat,BinCorrMat def ThresholdLocations(self, Thresh=0.5, TimeScale=48 ): ''' Apply the given threshold of amount of correlation required by the given time. Also stores a few plots of the results. Args: Thresh=0.5 (float) : Threshold on a scale between (-1 and +1) above which we deem something well-correlated TimeScale=48 (int) : Time in hours at which the thresholding operation is performed. Returns: CorrMatU,CorrMatV,CorrMatS : Correlation matrices for the easting, northing and magnitudes. BinCorrMatU,BinCorrMatV,BinCorrMatS : Correlation matrices after thresholding ''' CorrMatU,BinCorrMatU = self.PlotCorrelationWithLocations(self.tLags,self.cumVec_u/float(self.totAcc),self.X,self.Y,self.lat,self.lon,self.lyMax,self.lxMax, \ 'Plot of Auto-correlations in ocean current predictions for \n%d days from %04d-%02d-%02d' \ %(self.totAcc,self.yy,self.mm,self.dd),'Auto-Correlation\nCurrent u m/s','AutoCorrelate_U_mean',Thresh,TimeScale) CorrMatV,BinCorrMatV = self.PlotCorrelationWithLocations(self.tLags,self.cumVec_v/float(self.totAcc),self.X,self.Y,self.lat,self.lon,self.lyMax,self.lxMax, \ 'Plot of Auto-correlations in ocean current predictions for \n%d days from %04d-%02d-%02d' \ %(self.totAcc,self.yy,self.mm,self.dd),'Auto-Correlation\nCurrent v m/s','AutoCorrelate_V_mean',Thresh,TimeScale) CorrMatS,BinCorrMatS = PlotCorrelationWithLocations(self.tLags,self.cumVec_s/float(self.totAcc),self.X,self.Y,self.lat,self.lon,self.lyMax,self.lxMax, \ 'Plot of Auto-correlations in ocean current predictions for \n%d days from %04d-%02d-%02d' \ %(self.totAcc,self.yy,self.mm,self.dd),'Auto-Correlation\nCurrent s m/s','AutoCorrelate_S_mean',Thresh,TimeScale) CorrShelf = shelve.open('CorrModel_%.2f_%d.shelf'%(Thresh,TimeScale)) CorrShelf['CorrMatU'],CorrShelf['BinCorrMatU'] = CorrMatU,BinCorrMatU CorrShelf['CorrMatV'],CorrShelf['BinCorrMatV'] = CorrMatV,BinCorrMatV CorrShelf['CorrMatS'],CorrShelf['BinCorrMatS'] = CorrMatS,BinCorrMatS CorrShelf['tLags'] = tLags CorrShelf['cumVec_u'],CorrShelf['cumVec_v'],CorrShelf['cumVec_s'] = cumVec_u, cumVec_v, cumVec_s CorrShelf['X'],CorrShelf['Y'],CorrShelf['lat'],CorrShelf['lon'] = X,Y,lat,lon CorrShelf['lyMax'], CorrShelf['lxMax'] = lyMax,lxMax CorrShelf.close() self.SaveCorrelationShelfToMatlabFormat(Thresh,TimeScale) CorrMatU = np.where(np.isnan(CorrMatU),0,CorrMatU) CorrMatV = np.where(np.isnan(CorrMatV),0,CorrMatV) CorrMatS = np.where(np.isnan(CorrMatS),0,CorrMatS) # Save to Matlab if Geoff needs this. figU = plt.figure() plt.imshow(CorrMatU.transpose(),origin='Upper',cmap=plt.get_cmap(plt.cm.jet_r)) plt.title('Auto-correlation coefficient U (threshold %.2f @ %d hour lag)'%(Thresh,TimeScale)) plt.colorbar() plt.savefig('CorrMatU_Thresh%.2f_Lag%d.pdf'%(Thresh,TimeScale)) figV = plt.figure() plt.imshow(CorrMatV.transpose(),origin='Upper',cmap=plt.get_cmap(plt.cm.jet_r)) plt.title('Auto-correlation coefficient V (threshold %.2f @ %d hour lag)'%(Thresh,TimeScale)) plt.colorbar() plt.savefig('CorrMatV_Thresh%.2f_Lag%d.pdf'%(Thresh,TimeScale)) figS = plt.figure() plt.imshow(CorrMatS.transpose(),origin='Upper',cmap=plt.get_cmap(plt.cm.jet_r)) plt.title('Auto-correlation coefficient S (threshold %.2f @ %d hour lag)'%(Thresh,TimeScale)) plt.colorbar() plt.savefig('CorrMatS_Thresh%.2f_Lag%d.pdf'%(Thresh,TimeScale)) return CorrMatU,CorrMatV,CorrMatS,BinCorrMatU,BinCorrMatV,BinCorrMatS def SaveCorrelationShelfToMatlabFormat(self,Thresh,TimeScale): ''' Convert from Correlation Shelf format to Matlab. ''' CorrShelf = shelve.open('CorrModel_%.2f_%d.shelf'%(Thresh,TimeScale)) corrs = CorrShelf sio.savemat('Correlations_%.2f_%d.mat'%(Thresh,TimeScale),corrs) CorrShelf.close() def GetCorrelationPlot(self,lat,lon,qty,yy,mm,dd,numDays,numPts): ''' GetCorrelationPlot(lat,lon,qty,yy,mm,dd,numDays,numPts) Args: lat,lon (floats) : latitude and longitude arrays qty (np.array) : quantity whose auto-correlation we are computing yy,mm,dd, numDays (ints): year, month, day, number of days numPts : number of points we are finding the auto-correlations for. Returns: tLags (np.array) : vector with the time-lags (always the same) cumVec (np.array): correlation coefficients at different lags X, Y (np.arrays) : locations for which we have computed these lxMax, lyMax (ints): the width and height of the ROMS map ''' # Now let us look at the correlation in this variability over time. (tMax,lyMax,lxMax) = qty.shape self.tMax,self.lyMax,self.lxMax = qty.shape qty = np.where(np.isnan(qty),0,qty) #x_pts,y_pts = gm.x_pts,gm.y_pts lat_pts,lon_pts = self.gm.lat_pts,self.gm.lon_pts x_pts,y_pts=[],[] for i in range(0,len(lat_pts)): x,y = self.gm.LatLonToRomsXY(lat_pts[i],lon_pts[i],lat,lon) x_pts.append(x) y_pts.append(y) X,Y = np.array(np.floor(x_pts),int),np.array(np.floor(y_pts),int) self.X,self.Y = X,Y cumVec = np.zeros((numPts**2,self.maxLagHrs *2 -1)) tLags = np.zeros((numPts**2,self.maxLagHrs *2 -1)) i=0 fig = plt.figure() for y in Y: for x in X: lags,cVec,linecols,lines = plt.acorr(qty[:,y,x],usevlines=False,normed=True,maxlags=None,lw=2) tLags[i] = lags cumVec[i] += cVec i=i+1 plt.close() return tLags,cumVec,X,Y,lxMax,lyMax def ComputeCorrelations(self,yy_start,mm_start,dd_start,yy_end,mm_end,dd_end): ''' ComputeCorrelations(yy_start,mm_start,dd_start,yy_end,mm_end,dd_end) Args: yy_start,mm_start,dd_start (ints) : date correlation computations start from. yy_end,mm_end,dd_end (ints) : date correlation computations end at. Returns: CorrMatU, CorrMatV, CorrMatS ''' numPts = self.numPts numDays = int(self.maxLagHrs/24 + 0.5) self.dr = DateRange(yy_start,mm_start,dd_start,yy_end,mm_end,dd_end) for yy,mm,dd in self.dr.DateList: self.yy,self.mm,self.dd = yy,mm,dd u,v,time1,depth,lat,lon = self.gm.GetRomsData(yy,mm,dd,numDays) u_mean, v_mean = np.mean(u,axis=1), np.mean(v,axis=1) s_mean = np.sqrt(u_mean**2 + v_mean**2) lags_u,cVec_u,X,Y,lxMax,lyMax = self.GetCorrelationPlot(lat,lon,u_mean,yy,mm,dd,numDays,numPts) lags_v,cVec_v,X,Y,lxMax,lyMax = self.GetCorrelationPlot(lat,lon,v_mean,yy,mm,dd,numDays,numPts) lags_s,cVec_s,X,Y,lxMax,lyMax = self.GetCorrelationPlot(lat,lon,s_mean,yy,mm,dd,numDays,numPts) self.cumVec_u+=cVec_u self.cumVec_v+=cVec_v self.cumVec_s+=cVec_s self.tLags = lags_u self.totAcc += 1 self.lat,self.lon = lat,lon
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