def __init__(self,
shelfName='RiskMap.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_R(shelfName, sfcst_directory)
self.Dmax = dMax
self.maxLengths = 50
self.acceptR = 0.6
#! Load the graph
self.g = self.gm.lpGraph.copy()
for a in self.g.nodes():
i, j = self.GetXYfromNodeStr(a)
np.set_printoptions(precision=2)
self.possibleCollision = False
self.Rwd = {}
self.mdp = {}
self.ReInitializeMDP()
self.gamma = 1.0
self.maxDepth = 80.
def __init__(self,
shelfName='RiskMap.shelf',
sfcst_directory='../../../matlab/',
dMax=1.5):
""" This function initializes the class.
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.Dmax = dMax
self.maxLengths = 50
self.numDays = 3
self.acceptR = 0.6
self.mdp = {}
States = {}
width, height = self.gm.Width, self.gm.Height
#for j in range(0, height):
# for i in range(0, width):
import networkx as nx
self.g = self.gm.lpGraph.copy()
for a in self.g.nodes():
i, j = self.GetXYfromNodeStr(a)
state_str = 'S_%d%d' % (i, j)
State = {}
State['x'] = i
State['y'] = j
States[state_str] = State
self.mdp['States'] = States
self.mdp['Obs'] = np.where(self.gm.riskMap >= 1., 1, 0)
self.mdp['Rwd'] = -self.gm.riskMap
np.set_printoptions(suppress=True)
np.set_printoptions(precision=2)
self.possibleCollision = False
def __init__(self,shelfName='RiskMap.shelf',sfcst_directory='./',dMax = 1.5):
self.gm = GliderModel(shelfName,sfcst_directory) # Initializes risk, obs maps.
self.gs = GliderSimulator(shelfName,sfcst_directory)
self.Dmax = dMax
self.maxLengths = 50
self.numDays = 3
self.acceptR = 0.6
self.maxDepth = 60.
self.UseNetworkX = None
self.SARisks={}
try:
if self.UseNetworkX == None:
import networkx as nx
self.UseNetworkX = True
except ImportError:
print 'Please install networkX'
pass
def __init__(self,
shelfName='RiskMap.shelf',
sfcst_directory='./',
dMax=1.5):
self.gm = GliderModel(shelfName,
sfcst_directory) # Initializes risk, obs maps.
self.gs = GliderSimulator(shelfName, sfcst_directory)
self.Dmax = dMax
self.maxLengths = 50
self.numDays = 3
self.acceptR = 0.6
self.UseNetworkX = None
self.maxDepth = 60.
'''
try:
from pygraph.classes.digraph import digraph
from pygraph.algorithms.searching import depth_first_search
from pygraph.algorithms.minmax import shortest_path_bellman_ford
from pygraph.algorithms.minmax import shortest_path
from pygraph.readwrite.dot import write
self.UseNetworkX = False
except ImportError:
print 'Please install or specify path to pygraph'
pass
'''
# Import graphviz
try:
import pygraphviz as gv
except ImportError:
print 'Please install or specify path to pygraphviz'
pass
try:
if self.UseNetworkX == None:
import networkx as nx
self.UseNetworkX = True
except ImportError:
print 'Please install networkX'
pass
class MDP(object):
''' Main State MDP class
'''
def __init__(self,
shelfName='RiskMap.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_R(shelfName, sfcst_directory)
self.Dmax = dMax
self.maxLengths = 50
self.acceptR = 0.6
#! Load the graph
self.g = self.gm.lpGraph.copy()
for a in self.g.nodes():
i, j = self.GetXYfromNodeStr(a)
np.set_printoptions(precision=2)
self.possibleCollision = False
self.Rwd = {}
self.mdp = {}
self.ReInitializeMDP()
self.gamma = 1.0
self.maxDepth = 80.
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 GetNodesFromEdgeStr(self, edgeStr):
''' Convert from a transition model edge string to the
respective edges on the graph
Args:
* edgeStr (str): string in the format '(x1,y1),(x2,y2)'
Returns:
* x1, y1, x2, y2 values above, or None, None, None, None if string did not match.
'''
m = re.match('([0-9]+),([0-9]+),([0-9]+),([0-9]+)', edgeStr)
x1, y1, x2, y2 = None, None, None, None
if m:
x1, y1, x2, y2 = int(m.group(1)), int(m.group(2)), int(
m.group(3)), int(m.group(4))
return x1, y1, x2, y2
def GetTransitionModelFromShelf(self,
yy,
mm,
dd,
numDays,
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
* numDays (int): number of days the model is being built over
* 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
"""
self.posNoise = posNoise
self.currentNoise = currentNoise
#import pdb; pdb.set_trace()
if posNoise == None and currentNoise == None:
gmShelf = shelve.open('%s/gliderModel_%04d%02d%02d_%d.shelf' %
(shelfDirectory, yy, mm, dd, numDays),
writeback=False)
if posNoise != None:
if currentNoise != None:
gmShelf = shelve.open(
'%s/gliderModel_%04d%02d%02d_%d_%.3f_RN_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, numDays, posNoise,
currentNoise),
writeback=False)
else:
gmShelf = shelve.open(
'%s/gliderModel_%04d%02d%02d_%d_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, numDays, posNoise),
writeback=False)
if posNoise == None and currentNoise != None:
gmShelf = shelve.open(
'%s/gliderModel_%04d%02d%02d_%d_RN_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, numDays, 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 BuildTransitionGraph(self):
''' Computes the transition graph from all the available edges in the transition model.
'''
for a in self.g.nodes():
for b in self.g.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):
rwd = self.GetRewardForState(i, j)
self.g.add_edge('(%d,%d)' % (i, j),
'(%d,%d)' % (x, y),
weight=rwd,
trapped=False)
# Store a copy of g, so that we can re-run the MDP when needed
# This is because, we will end up adding
self.g_copy = self.g.copy()
def GetRomsData(self, yy, mm, dd, numDays, UpdateSelf=True):
''' 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?
'''
u, v, time1, depth, lat, lon = self.gm.GetRomsData(yy, mm, dd, numDays)
u, v, time1, depth, lat, lon = self.gs.gm.GetRomsData(
yy, mm, dd, numDays)
u, v, time1, depth, lat, lon = self.gsR.gm.GetRomsData(
yy, mm, dd, numDays)
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 (dict {'x','y'}) : dictionary with '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
def GetRewardForState(self, x, y):
''' Get the reward for a state-action pair. We use a dictionary here
to perform a lookup if we have already computed this earlier.
Args:
state (node_str) : node we are currently evaluating
action (edge_str): action (or edge) we are currently computing the reward for.
Updates:
self.Rwd[state-action-pair-str] with computed reward.
Returns:
self.Rwd[state-action-pair-str] if previously computed, else computes it and returns that result.
'''
OffMapNotObs = False # Treat locations off the map as high-risk locations
state_str = '%d,%d' % (x, y)
if self.Rwd.has_key(state_str):
return self.Rwd[state_str]
else:
lat, lon = self.gm.GetLatLonfromXY(x, y)
riskVal = self.gm.GetRisk(lat, lon, OffMapNotObs)
self.Rwd[state_str] = -riskVal
return self.Rwd[state_str]
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)): state x,y in planning graph coods.
state_prime (tuple of (int,int)): state_prime x,y in P.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 state_prime == self.goal:
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]
# Iterate over all possible actions
for j in range(0, int(tPSize)):
for i in range(0, int(tPSize)):
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]):
transProb = transProbs[j][i]
totalProb += transProb
# This looks like a bug to me...
Util += transProb * self.mdp['U'][
y + j - zeroLoc][x + i - zeroLoc]
#Util += transProb * self.mdp['U'][y+j-zeroLoc][x+i-zeroLoc]
if totalProb != 1:
transProb = 1 - totalProb
Util += transProb * self.mdp['U'][y][x]
#print Util
return Util
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 x and y locations in graph-coordinates
'''
x, y = state
# Transition graph should have all the out-going 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('(%d,%d)' % (x, y), data=True):
#print u,v,d['weight']
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
x1, y1 = self.GetXYfromNodeStr(u)
x2, y2 = self.GetXYfromNodeStr(v)
stateTrans = '%d,%d,%d,%d' % (x1, y1, x2, y2)
UtilVec[
stateTrans] = sa_rwd + self.gamma * self.GetUtilityForStateTransition(
(x1, y1), (x2, y2))
return UtilVec
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.g = self.gm.lpGraph.copy()
self.BuildTransitionGraph()
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
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.out_edges('(%d,%d)' % (goal[0], goal[1]),
data=True):
self.g.remove_edge(u, v)
self.g.add_edge('(%d,%d)' % (goal[0], goal[1]),
'(%d,%d)' % (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 we are interested in.
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 if you want discounted rewards. By default gamma is 1.
'''
for i in range(0, MaxRuns):
self.mdp['U'] = self.mdp['Uprime'].copy()
self.delta = 0.
for nodeStr in self.g.nodes():
x, y = self.GetXYfromNodeStr(nodeStr)
Utils = self.CalculateTransUtilities((x, y))
ExpUtilVec = Utils.values()
maxExpectedUtility = max(ExpUtilVec)
#print x,y,maxExpectedUtility
self.mdp['Uprime'][y, x] = maxExpectedUtility
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
# Get Policy tree.
self.GetPolicyTreeForMDP()
''' -------------------------------------------------------------------------------------------
---------------------------------- Code to create the policy tree -----------------------------
--------------------------------------------------------------------------------------------'''
def DisplayPolicy(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.g.nodes():
x, y = self.GetXYfromNodeStr(a)
Xloc[x][y], Yloc[x][y] = x, y
UtilVec = np.zeros(10)
maxUtil = -float('inf')
k, maxU, maxV = 0, 0, 0
for u, v, d in self.g.out_edges(a, data=True):
i, j = self.GetXYfromNodeStr(v)
UtilVec[k] = Uprime[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)
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 = self.GetXYfromNodeStr(a)
UtilVec = np.zeros(10)
maxUtil = -float('inf')
k, maxU, maxV = 0, 0, 0
for u, v, d in self.g.out_edges(a, data=True):
i, j = self.GetXYfromNodeStr(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):
if curNode == goal:
return self.goal[0], self.goal[1]
try:
neighborNode = self.gm2.neighbors(
'(%d,%d)' % (int(curNode[0]), int(curNode[1])))
if len(neighborNode) > 0:
nextNode = neighborNode[0]
else:
nextNode = None
except nx.NetworkXError:
nextNode = None
if nextNode != None:
m = re.match('\(([0-9]+),([0-9]+)\)', nextNode) # Changed this!!!
#m2 =re.match('\(([0-9]+),([0-9]+)\)',goal)
if m:
return int(m.group(1)), int(m.group(2))
return self.FindNearestNodeOnGraph(curNode)
def FindNearestNodeOnGraph(self, curNode):
nearest_dist = float('inf')
best_utility = -float('inf')
nearest_node = (None, None)
for a in self.g.nodes():
i, j = curNode
x, y = self.GetXYfromNodeStr(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]):
if self.mdp['U'][y][x] > best_utility and dist < self.Dmax:
best_utility = self.mdp['U'][y][x]
nearest_node = (x, y)
return nearest_node
'''
def GetPolicyAtCurrentNode(self,curNode,goal,forceGoal=False):
if curNode == goal:
return self.goal[0],self.goal[1]
self.UseNetworkX = True
import networkx as nx
try:
if self.UseNetworkX:
neighborNode = self.gm2.neighbors(curNode)
if len(neighborNode)>0:
nextNode = neighborNode[0]
else:
nextNode = None
else:
nextNode = self.pol_tree.node_neighbors[curNode]
except KeyError:
nextNode=None
except nx.NetworkXError:
nextNode = None
if nextNode!=None:
m = re.match('\(([0-9]+),([0-9]+)\)', nextNode) # Changed this!!!
m2 =re.match('\(([0-9]+),([0-9]+)\)',goal)
if m:
return int(m.group(1)),int(m.group(2))
if m2:
return int(m2.group(1)),int(m2.group(2))
if forceGoal==True:
return self.goal[0],self.goal[1]
else:
return self.FindNearestNodeOnGraph()
def FindNearestNodeOnGraph(self,curNode):
nearest_dist = float('inf')
best_utility = -float('inf')
nearest_node = (None,None)
for a in self.g.nodes():
i,j = self.GetXYfromNodeStr(curNode)
x,y = self.GetXYfromNodeStr(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]):
if self.mdp['U'][y][x] > best_utility and dist<self.Dmax:
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
return self.gs.SimulateAndPlot_PolicyExecution(
start, goal, simStartTime, self.maxDepth,
self.GetPolicyAtCurrentNode, lineType, newFig)
self.a = start
done = False
i = 0
#x_sims,y_sims = np.zeros((24*gm.numDays,1)),np.zeros((24*gm.numDays,1))
if simStartTime >= (24 * self.gm.numDays):
simStartTime = 24 * self.gm.numDays - 1
x_sims, y_sims = 0, 0
self.finX, self.finY = start[0], start[1]
#import pdb; pdb.set_trace()
while (not done):
self.numHops += 1
#print self.a[0],self.a[1]
try:
self.plot(a[0], a[1], 'k.')
bx, by = self.GetPolicyAtCurrentNode(
'(%d,%d)' % (int(self.a[0] + 0.5), int(self.a[1] + 0.5)),
'(%d,%d)' % (goal[0], goal[1]))
except TypeError:
bx, by = None, None
#if bx==None and by==None:
# import pdb; pdb.set_trace()
if bx != None and by != None:
b = (bx, by)
#tempRisk = self.GetRiskFromXY(bx,by)
#totalRisk+=tempRisk
self.distFromGoal = math.sqrt((self.a[0] - goal[0])**2 +
(self.a[1] - goal[1])**2)
if self.distFromGoal <= self.acceptR:
self.isSuccess = True
done = True
sLat, sLon = self.gm.GetLatLonfromXY(self.a[0], self.a[1])
gLat, gLon = self.gm.GetLatLonfromXY(b[0], b[1])
tStart = simStartTime + self.totalTime / 3600.
if tStart >= 24 * self.gm.numDays:
tStart = 24 * self.gm.numDays - 1
xFin,yFin,latFin,lonFin,latArray,lonArray,depthArray,tArray,possibleCollision,CollisionReason,totalDist = \
self.gm.SimulateDive(sLat,sLon,gLat,gLon, self.gm.maxDepth, self.u, self.v, self.lat, self.lon, self.depth, tStart, False )
#gm.SimulateDive(gm.lat_pts[a[1]], gm.lon_pts[a[0]], gm.lat_pts[b[1]], gm.lon_pts[b[0]], gm.maxDepth, u, v, lat, lon, depth, k)
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
if len(tArray > 0):
self.totalTime += tArray[-1]
if possibleCollision == False:
tempX, tempY = self.gm.GetXYfromLatLon(
np.array(latArray), np.array(lonArray))
x_sims, y_sims = tempX[-1:][0], tempY[-1:][
0] # TODO: This might be wrong!
plt.plot(tempX, tempY, lineType)
if x_sims != [] and y_sims != []:
tempRisk = self.GetRiskFromXY(x_sims, y_sims)
self.finX, self.finY = x_sims, y_sims
self.totalRisk += tempRisk
else:
self.totalRisk += self.gm.GetRisk(sLat, sLon)
else:
if self.CollisionReason == 'RomsNanAtStart':
self.totalRisk += self.gm.GetRisk(sLat, 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, lineType)
x_sims, y_sims = 0, 0
done = True
return self.totalRisk, True # Landed on beach!
#plt.plot([a[0],x_sims[0]],[a[1],y_sims[0]],lineType)
try:
self.a = (x_sims[0], y_sims[0])
self.finX, self.finY = self.a
except IndexError:
done = True
return self.totalRisk, True # Landed on beach
#import pdb; pdb.set_trace()
i = i + 1
if i > self.maxLengths:
done = True
else: # We did not get a policy here.
self.CollisionReason = 'DidNotFindPolicy'
done = True
return self.totalRisk, False
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
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
if newFig:
plt.figure()
plt.title('Plotting Min-Exp Risk Ensemble')
self.gm.PlotNewRiskMapFig()
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]))
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])
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:
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 Replanner(object):
''' Replanner - A class that executes one of two types of planners (Min-Risk or Min-Expected-Risk).
The replanning stems from the fact that when the robot surfaces at the waypoint it thinks
it hit, it replans and resumes execution (at-least during simulation of such a planner).
'''
def __init__(self,
shelfName='RiskMap.shelf',
sfcst_directory='./',
dMax=1.5):
self.gm = GliderModel(shelfName,
sfcst_directory) # Initializes risk, obs maps.
self.gs = GliderSimulator(shelfName, sfcst_directory)
self.Dmax = dMax
self.maxLengths = 50
self.numDays = 3
self.acceptR = 0.6
self.UseNetworkX = None
self.maxDepth = 60.
'''
try:
from pygraph.classes.digraph import digraph
from pygraph.algorithms.searching import depth_first_search
from pygraph.algorithms.minmax import shortest_path_bellman_ford
from pygraph.algorithms.minmax import shortest_path
from pygraph.readwrite.dot import write
self.UseNetworkX = False
except ImportError:
print 'Please install or specify path to pygraph'
pass
'''
# Import graphviz
try:
import pygraphviz as gv
except ImportError:
print 'Please install or specify path to pygraphviz'
pass
try:
if self.UseNetworkX == None:
import networkx as nx
self.UseNetworkX = True
except ImportError:
print 'Please install networkX'
pass
def GetTransitionModelFromShelf(self,
yy,
mm,
dd,
numDays,
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
* numDays (int): number of days the model is being built over
* 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.
"""
self.posNoise = posNoise
self.currentNoise = currentNoise
if posNoise == None and currentNoise == None:
gmShelf = shelve.open('%s/gliderModel_%04d%02d%02d_%d.shelf' %
(shelfDirectory, yy, mm, dd, numDays),
writeback=False)
if posNoise != None:
if currentNoise != None:
gmShelf = shelve.open(
'%s/gliderModel_%04d%02d%02d_%d_%.3f_RN_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, numDays, posNoise,
currentNoise),
writeback=False)
else:
gmShelf = shelve.open(
'%s/gliderModel_%04d%02d%02d_%d_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, numDays, posNoise),
writeback=False)
if posNoise == None and currentNoise != None:
gmShelf = shelve.open(
'%s/gliderModel_%04d%02d%02d_%d_RN_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, numDays, currentNoise),
writeback=False)
self.gm.TransModel = gmShelf['TransModel']
self.gModel = {}
self.gModel['TransModel'] = gmShelf['TransModel']
gmShelf.close()
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 GetRomsData(self,yy,mm,dd,numDays,UseNewFormat=True,usePredictionData=False):
UpdateSelf=True
u,v,time1,depth,lat,lon = self.gs.gm.GetRomsData(yy,mm,dd,numDays,UseNewFormat,usePredictionData)
u,v,time1,depth,lat,lon = self.gm.GetRomsData(yy,mm,dd,numDays,UseNewFormat,usePredictionData)
self.u,self.v,self.time1,self.depth,self.lat,self.lon = u,v,time1,depth,lat,lon
self.numDays = numDays
return u,v,time1,depth,lat,lon
'''
def isOnMapRP(self, s):
width, height = self.gm.Width, self.gm.Height
if s[0] < 0 or s[0] >= width:
return False
if s[1] < 0 or s[1] >= height:
return False
return True
def GetExpRiskOld(self, v1, v2):
x, y = v1[0], v1[1]
xp, yp = v2[0], v2[1]
expRisk = 0
stateTrans = '%d,%d,%d,%d' % (x, y, xp, yp)
if self.gModel['TransModel'].has_key(stateTrans):
transProbs, tPSize, zeroLoc = self.gModel['TransModel'][stateTrans][2], \
self.gModel['TransModel'][stateTrans][1], \
self.gModel['TransModel'][stateTrans][0]
totalProb = 0
for j in range(0, int(tPSize)):
for i in range(0, int(tPSize)):
xp2, yp2 = x + i - zeroLoc, y + j - zeroLoc
transProb = transProbs[i][j]
totalProb += transProb
if self.isOnMapRP((xp2, yp2)):
expRisk += transProb * self.gm.GetRisk(
self.gm.lat_pts[yp2], self.gm.lon_pts[xp2])
else:
expRisk += transProb * 1.0 # Off map locations get high-risk!
else:
expRisk = None
return expRisk
def GetExpRisk(self, v1, v2):
x, y = v1[0], v1[1]
xp, yp = v2[0], v2[1]
expRisk = 0
stateTrans = '%d,%d,%d,%d' % (x, y, xp, yp)
if self.gModel['TransModel'].has_key(stateTrans):
transProbs, tPSize, zeroLoc = self.gModel['TransModel'][stateTrans][2], \
self.gModel['TransModel'][stateTrans][1], \
self.gModel['TransModel'][stateTrans][0]
totalProb = 0
for j in range(0, int(tPSize)):
for i in range(0, int(tPSize)):
xp2, yp2 = x + i - zeroLoc, y + j - zeroLoc
transProb = transProbs[i][j]
totalProb += transProb
if self.isOnMapRP((xp2, yp2)):
expRisk += transProb * self.gm.GetRisk(
self.gm.lat_pts[yp2], self.gm.lon_pts[xp2])
else:
expRisk += transProb * 1.0 # Off map locations get high-risk!
else:
expRisk = None
return expRisk
def CreateExpRiskGraph(self):
# Short-circuit this!
return self.CreateGraphUsingProximityGraph('MinExpRisk')
# This is what we used to do earlier. Won't be running
# unless we actually want it to.
self.g = nx.DiGraph()
for j in range(0, self.gm.Height):
for i in range(0, self.gm.Width):
self.g.add_node('(%d,%d)' % (i, j))
for j in range(0, self.gm.Height):
for i in range(0, self.gm.Width):
for y in range(0, self.gm.Height):
for x in range(0, self.gm.Width):
if math.sqrt((i - x)**2 + (j - y)**2) <= self.Dmax:
if self.gm.ObsDetLatLon(
self.gm.lat_pts[j], self.gm.lon_pts[i],
self.gm.lat_pts[y],
self.gm.lon_pts[x]) == False:
expRisk = self.GetExpRisk((i, j), (x, y))
if expRisk != None:
self.g.add_edge('(%d,%d)' % (i, j),
'(%d,%d)' % (x, y),
weight=expRisk)
### If we want to write this graph out.
#dot = write(g)
#G = gv.AGraph(dot)
#G.layout(prog ='dot')
#G.draw('SCB.png')
return self.g
def GetXYfromNodeStr(self, nodeStr):
''' Convert from the name of the node string to the locations.
'''
m = re.match('\(([0-9\.]+),([0-9\.]+)\)', nodeStr)
if m:
return int(m.group(1)), int(m.group(2))
else:
return None, None
def GetNodesFromEdgeStr(self, edgeStr):
''' Convert from a transition model edge string to the
respective edges on the graph
'''
m = re.match('([0-9]+),([0-9]+),([0-9]+),([0-9]+)', edgeStr)
x1, y1, x2, y2 = None, None, None, None
if m:
x1, y1, x2, y2 = int(m.group(1)), int(m.group(2)), int(
m.group(3)), int(m.group(4))
return x1, y1, x2, y2
def CreateGraphUsingProximityGraph(self, graphType='MinExpRisk'):
import networkx as nx
self.g = nx.DiGraph()
self.g = self.gm.lpGraph.copy()
num_nodes_compared, num_nodes_added = 0, 0
for a in self.g.nodes():
for b in self.g.nodes():
if a != b:
num_nodes_compared += 1
i, j = self.GetXYfromNodeStr(a)
x, y = self.GetXYfromNodeStr(b)
if math.sqrt((i - x)**2 + (j - y)**2) <= self.Dmax:
print 'Considering: (%d,%d) to (%d,%d):' % (i, j, x, y)
if self.gm.ObsDetLatLon(self.gm.lat_pts[j],
self.gm.lon_pts[i],
self.gm.lat_pts[y],
self.gm.lon_pts[x]) == False:
if graphType == 'MinExpRisk':
expRisk = self.GetExpRisk((i, j), (x, y))
if expRisk != None:
self.g.add_edge('(%d,%d)' % (i, j),
'(%d,%d)' % (x, y),
weight=expRisk)
print 'Added edge (%d,%d) to (%d,%d) with weight=%.2f' % (
i, j, x, y, expRisk)
else:
print 'Rejected edge (%d,%d) to (%d,%d) with weight=None' % (
i, j, x, y)
elif graphType == 'MinRisk':
riskVal = self.gm.GetRisk(
self.gm.lat_pts[y], self.gm.lon_pts[x])
if riskVal != None:
self.g.add_edge('(%d,%d)' % (i, j),
'(%d,%d)' % (x, y),
weight=riskVal)
print 'Added edge (%d,%d) to (%d,%d) with weight=%.2f' % (
i, j, x, y, riskVal)
num_nodes_added += 1
else:
print 'Rejected edge (%d,%d) to (%d,%d) with weight=None' % (
i, j, x, y)
elif graphType == 'ShortestPath':
self.g.add_edge('(%d,%d)' % (i, j),
'(%d,%d)' % (x, y),
weight=math.sqrt((x - i)**2. +
(y - j)**2.))
else:
print 'Rejected due to collision!'
print 'Total # of edges compared=%d, # of edges added=%d' % (
num_nodes_compared, num_nodes_added)
### If we want to write this graph out.
#dot = write(g)
#G = gv.AGraph(dot)
#G.layout(prog ='dot')
#G.draw('SCB.png')
return self.g
def CreateMinRiskGraph(self):
'''
A ROMS unaware planner. This planner totally avoids locations
'''
# Short-circuit this!
return self.CreateGraphUsingProximityGraph('MinRisk')
# This is what we used to do earlier. Won't be running
# unless we actually want it to.
import networkx as nx
self.g = nx.DiGraph()
for j in range(0, self.gm.Height):
for i in range(0, self.gm.Width):
if self.UseNetworkX:
self.g.add_node('(%d,%d)' % (i, j))
else:
self.g.add_nodes(['(%d,%d)' % (i, j)])
self.g.add_node_attribute('(%d,%d)' % (i, j),
('position', (i, j)))
for j in range(0, self.gm.Height):
for i in range(0, self.gm.Width):
for y in range(0, self.gm.Height):
for x in range(0, self.gm.Width):
if math.sqrt((i - x)**2 + (j - y)**2) <= self.Dmax:
if self.gm.ObsDetLatLon(
self.gm.lat_pts[j], self.gm.lon_pts[i],
self.gm.lat_pts[y],
self.gm.lon_pts[x]) == False:
riskVal = self.gm.GetRisk(
self.gm.lat_pts[y], self.gm.lon_pts[x])
if riskVal != None:
if self.UseNetworkX == True:
self.g.add_edge('(%d,%d)' % (i, j),
'(%d,%d)' % (x, y),
weight=riskVal)
else:
self.g.add_edge(
('(%d,%d)' % (i, j), '(%d,%d)' %
(x, y)),
wt=riskVal)
return self.g
def GetShortestPathMST(self, goal):
''' Find the shortest path Minimum-Spanning Tree
'''
if self.g != None:
if self.UseNetworkX:
import networkx as nx
self.sp_mst = nx.all_pairs_dijkstra_path(self.g)
self.dist = nx.all_pairs_dijkstra_path_length(self.g)
else:
from pygraph.algorithms.minmax import shortest_path
self.sp_mst, self.dist = shortest_path(
self.g, '(%d,%d)' % (goal[0], goal[1]))
return self.sp_mst, self.dist
else:
print 'CreateExpRiskGraph first before calling GetShortestPathMST(goal)'
return None, None
def GetPathXYfromPathList(self, path2goal):
''' Find the path in two lists (pathX,pathY) from the string values of path-list
'''
pathX, pathY = [], []
for pathEl in path2goal:
m = re.match('\(([0-9]+),([0-9]+)\)', pathEl)
if (m):
pathX.append(int(m.group(1)))
pathY.append(int(m.group(2)))
return pathX, pathY
def BackTrackNxPath(self, source, goal):
source_str = '(%d,%d)' % (source[0], source[1])
goal_str = '(%d,%d)' % (goal[0], goal[1])
try:
path2goal = self.sp_mst[source_str][goal_str]
except KeyError:
return None, None, None
except:
return None, None, None
pathX, pathY = self.GetPathXYfromPathList(path2goal)
return path2goal, pathX, pathY
def BackTrackPath(self, mst, source):
# source and goal are specified as tuples
#import pdb; pdb.set_trace()
source_str = '(%d,%d)' % (source[0], source[1])
path2goal = []
pathX, pathY = [], []
path_str = source_str
path2goal.append(path_str)
try:
while (mst[path_str] != None):
path_str = mst[path_str]
path2goal.append(path_str)
#path2goal.reverse()
print path2goal
pathX, pathY = self.GetPathXYfromPathList(path2goal)
except KeyError:
return None, None, None
return path2goal, pathX, pathY
def PlotMRpaths(self, goal):
fig = plt.figure()
plt.title('Plot of Min-Exp Risk MST (Goal at:(%d,%d))' %
(goal[0], goal[1]))
#plt.imshow(rMap.transpose()+0.5,origin='upper')
#plt.imshow(1-GetRiskMap(),origin='upper')
self.gm.PlotNewRiskMapFig(False)
sp_mst, dist = self.GetShortestPathMST(goal)
for a in self.g.nodes():
i, j = self.GetXYfromNodeStr(a)
path2goal, pathX, pathY = self.BackTrackNxPath((i, j), goal)
plt.plot(pathX, pathY, 'k*-')
#for j in range(0,self.gm.Height):
# for i in range(0,self.gm.Width):
# if not self.gm.GetObs(self.gm.lat_pts[j],self.gm.lon_pts[i]):
# path2goal,pathX,pathY = self.BackTrackNxPath((i,j), goal)
# plt.plot(pathX,pathY,'k*-') # TODO: Check why this is so!!!
#plt.xlim((-0.5,self.gm.Width-0.5))
#plt.ylim((-0.5,self.gm.Height-0.5))
return fig
def GetPolicyAtCurrentNode(self, curNode, goal, forceGoal=False):
#print 'curNode',curNode
x, y = curNode
return self.GetPolicyNxMR((x, y))
def GetPolicyNxMR(self, curNode):
if curNode == self.goal:
return self.goal[0], self.goal[1]
path2goal, pathX, pathY = self.BackTrackNxPath(curNode, self.goal)
if pathX == None and pathY == None:
return self.FindNearestNodeOnGraph(curNode, self.goal)
if len(path2goal) > 1:
return pathX[1], pathY[1]
else:
return pathX[0], pathY[0]
def FindNearestNodeOnGraph(self, curNode, goal):
nearest_dist = float('inf')
nearest_node = (None, None)
#import pdb; pdb.set_trace()
for a in self.g.nodes():
i, j = curNode
x, y = self.GetXYfromNodeStr(a)
dist = math.sqrt((i - x)**2 + (j - y)**2)
if not self.gm.ObsDetLatLon(self.gm.lat_pts[int(j)],self.gm.lon_pts[int(i)], \
self.gm.lat_pts[int(y)],self.gm.lon_pts[int(x)]):
if (dist < self.Dmax):
if self.dist['(%d,%d)' %
(x, y)]['(%d,%d)' %
(goal[0], goal[1])] < nearest_dist:
nearest_dist = self.dist['(%d,%d)' %
(x, y)]['(%d,%d)' %
(goal[0], goal[1])]
nearest_node = (x, y)
return nearest_node
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 GetPolicyMR(self, mst, curNode):
m1 = re.match('\(([0-9]+),([0-9]+)\)', curNode)
if m1:
if mst.has_key(curNode):
nextNode = mst[curNode]
if nextNode != None:
m2 = re.match('\(([0-9]+),([0-9]+)\)', nextNode)
if m2:
return int(m2.group(1)), int(m2.group(2))
return int(m1.group(1)), int(m1.group(2))
return None, None
def GetRiskFromXY(self, x, y):
lat, lon = self.gm.GetLatLonfromXY(x, y)
return self.gm.GetRisk(lat, lon)
def SimulateAndPlotMR_PolicyExecution(self,
start,
goal,
mst,
simStartTime,
newFig=True,
lineType='y-'):
self.totalRisk = 0
self.totalPathLength = 0.
self.distFromGoal = float('inf')
self.totalTime = 0.
self.goal = goal
self.start = start
self.numHops = 0
self.isSuccess = False
if newFig:
plt.figure()
plt.title('Plotting Min-Exp Risk Ensemble')
self.gm.PlotNewRiskMapFig()
return self.gs.SimulateAndPlot_PolicyExecution(
start, goal, simStartTime, self.maxDepth,
self.GetPolicyAtCurrentNode, lineType, newFig)
a = start
done = False
i = 0
k = simStartTime
#x_sims,y_sims = np.zeros((24*gm.numDays,1)),np.zeros((24*gm.numDays,1))
if k >= (24 * self.gm.numDays):
k = 24 * self.gm.numDays - 1
x_sims, y_sims = 0, 0
self.finX, self.finY = start[0], start[1]
#import pdb; pdb.set_trace()
while (not done):
self.numHops += 1
try:
bx, by = self.GetPolicyNxMR((int(a[0] + 0.5), int(a[1] + 0.5)))
print 'At :(%.1f,%.1f). Going to: (%d,%d)' % (a[0], a[1], bx,
by)
except TypeError:
bx, by = None, None
if bx != None and by != None:
b = (bx, by)
self.distFromGoal = math.sqrt((a[0] - goal[0])**2 +
(a[1] - goal[1])**2)
if self.distFromGoal <= self.acceptR:
self.isSuccess = True
done = True
sLat, sLon = self.gm.GetLatLonfromXY(a[0], a[1])
gLat, gLon = self.gm.GetLatLonfromXY(b[0], b[1])
tStart = k + self.totalTime / 3600.
if tStart >= 24 * self.gm.numDays:
tStart = 24 * self.gm.numDays - 1
xFin,yFin,latFin,lonFin,latArray,lonArray,depthArray,tArray,possibleCollision,CollisionReason,totalDist = \
self.gm.SimulateDive(sLat,sLon,gLat,gLon, self.gm.maxDepth, self.u, self.v, self.lat, self.lon, self.depth, tStart, False )
self.totalPathLength += totalDist
self.CollisionReason = CollisionReason
#gm.SimulateDive(gm.lat_pts[a[1]], gm.lon_pts[a[0]], gm.lat_pts[b[1]], gm.lon_pts[b[0]], gm.maxDepth, u, v, lat, lon, depth, k)
# This will allow us to compute how far from the goal we are...
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
if len(tArray > 0):
self.totalTime += tArray[-1]
if possibleCollision == False:
tempX, tempY = self.gm.GetXYfromLatLon(
np.array(latArray), np.array(lonArray))
x_sims, y_sims = tempX[-1:][0], tempY[-1:][0]
plt.plot(tempX, tempY, lineType)
if x_sims != [] and y_sims != []:
tempRisk = self.GetRiskFromXY(x_sims, y_sims)
self.finX, self.finY = x_sims, y_sims
self.totalRisk += tempRisk
else:
self.totalRisk += self.gm.GetRisk(sLat, sLon)
else:
if self.CollisionReason == 'RomsNanAtStart':
self.totalRisk += self.gm.GetRisk(sLat, 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, lineType)
x_sims, y_sims = 0, 0
done = True
return self.totalRisk, True # landed = true
#plt.plot([a[0],x_sims[0]],[a[1],y_sims[0]],lineType)
try:
a = (x_sims[0], y_sims[0])
self.finX, self.finY = a
except IndexError:
done = True
return self.totalRisk, True
#import pdb; pdb.set_trace()
i = i + 1
if i > self.maxLengths:
done = True
else:
self.CollisionReason = 'DidNotFindPolicy'
done = True
return self.totalRisk, False
class MDP(object):
""" Main MDP Class. Implementation of an MDP class that does minimum-risk planning.
"""
def __init__(self,
shelfName='RiskMap.shelf',
sfcst_directory='../../../matlab/',
dMax=1.5):
""" This function initializes the class.
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.Dmax = dMax
self.maxLengths = 50
self.numDays = 3
self.acceptR = 0.6
self.mdp = {}
States = {}
width, height = self.gm.Width, self.gm.Height
#for j in range(0, height):
# for i in range(0, width):
import networkx as nx
self.g = self.gm.lpGraph.copy()
for a in self.g.nodes():
i, j = self.GetXYfromNodeStr(a)
state_str = 'S_%d%d' % (i, j)
State = {}
State['x'] = i
State['y'] = j
States[state_str] = State
self.mdp['States'] = States
self.mdp['Obs'] = np.where(self.gm.riskMap >= 1., 1, 0)
self.mdp['Rwd'] = -self.gm.riskMap
np.set_printoptions(suppress=True)
np.set_printoptions(precision=2)
self.possibleCollision = False
def GetXYfromNodeStr(self, nodeStr):
''' Convert from the name of the node string to the locations.
'''
m = re.match('\(([0-9\.]+),([0-9\.]+)\)', nodeStr)
if m:
return int(m.group(1)), int(m.group(2))
else:
return None, None
def GetNodesFromEdgeStr(self, edgeStr):
''' Convert from a transition model edge string to the
respective edges on the graph
Args:
* edgeStr (str): string in the format '(x1,y1),(x2,y2)'
Returns:
* x1, y1, x2, y2 values above, or None, None, None, None if string did not match.
'''
m = re.match('([0-9]+),([0-9]+),([0-9]+),([0-9]+)', edgeStr)
x1, y1, x2, y2 = None, None, None, None
if m:
x1, y1, x2, y2 = int(m.group(1)), int(m.group(2)), int(
m.group(3)), int(m.group(4))
return x1, y1, x2, y2
def GetTransitionModelFromShelf(self,
yy,
mm,
dd,
numDays,
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
* numDays (int): number of days the model is being built over
* 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.
"""
self.posNoise = posNoise
self.currentNoise = currentNoise
#import pdb; pdb.set_trace()
if posNoise == None and currentNoise == None:
gmShelf = shelve.open('%s/gliderModel_%04d%02d%02d_%d.shelf' %
(shelfDirectory, yy, mm, dd, numDays),
writeback=False)
if posNoise != None:
if currentNoise != None:
gmShelf = shelve.open(
'%s/gliderModel_%04d%02d%02d_%d_%.3f_RN_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, numDays, posNoise,
currentNoise),
writeback=False)
else:
gmShelf = shelve.open(
'%s/gliderModel_%04d%02d%02d_%d_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, numDays, posNoise),
writeback=False)
if posNoise == None and currentNoise != None:
gmShelf = shelve.open(
'%s/gliderModel_%04d%02d%02d_%d_RN_%.3f.shelf' %
(shelfDirectory, yy, mm, dd, numDays, 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']
self.gModel = {}
self.gModel['TransModel'] = gmShelf['TransModel']
gmShelf.close()
self.mdp['transProbs'] = self.gModel['TransModel']
self.mdp['width'], self.mdp['height'] = self.gm.Width, self.gm.Height
def GetRomsData(self, yy, mm, dd, numDays, UpdateSelf=True):
''' Loads up ROMs data from a particular day, for a certain number of days and supports self-update
:param self:
:param yy: year
:param mm: month
:param dd: day
:param numDays: number of days the model is being built over
:param UpdateSelf: Should the MDP update itself with the ROMS data being loaded?
:type UpdateSelf:
'''
u, v, time1, depth, lat, lon = self.gm.GetRomsData(yy, mm, dd, numDays)
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 (dict {'x','y'}) : dictionary with 'x' and 'y' values
'''
width, height = self.gm.Width, self.gm.Height
if s['x'] < 0 or s['x'] >= width:
return False
if s['y'] < 0 or s['y'] >= height:
return False
return True
def GetTrapStateForObs(self, x, y):
''' Get TrapStateForObstacles
'''
State = {}
State['x'], State['y'], State['trapped'] = x, y, False
return State
def SetGoalAndInitTerminalStates(self, goal, rewardVal=10):
''' Set the Goal location and initialize terminal states.
'''
TermStates = []
GState = {}
GState['x'], GState['y'], GState['trapped'] = goal[0], goal[1], False
TermStates.append(GState)
self.mdp['Rwd'] = -self.gm.riskMap
self.mdp['Rwd'][goal[0], goal[1]] = rewardVal
width, height = self.gm.Width, self.gm.Height
# Also add obstacles as terminal states.
for y in range(0, height):
for x in range(0, width):
if self.gm.GetObs(self.gm.lat_pts[x], self.gm.lon_pts[y]):
state = self.GetTrapStateForObs(x, y)
TermStates.append(state)
self.mdp['TermStates'] = TermStates
def identifyTerminalState(self, State):
TermStates = self.mdp['TermStates']
for i in range(0, len(TermStates)):
if TermStates[i]['x'] == State['x'] and TermStates[i][
'y'] == State['y']:
if not TermStates[i]['trapped']:
TermStates[i]['trapped'] = True
return True, False, TermStates
else:
return True, True, TermStates
return False, False, TermStates # Returns: isGoalState,isTrapped,TermStates
def GetUtilityForStateTransition(self, State, sPrime):
x, y = State['x'], State['y']
xp, yp = sPrime['x'], sPrime['y']
width, height = self.mdp['width'], self.mdp['height']
Util = 0.
totalProb = 0
transProb = 0
stateTrans = '%d,%d,%d,%d' % (x, y, xp, yp)
if self.mdp['transProbs'].has_key(stateTrans):
transProbs = self.mdp['transProbs'][stateTrans][2]
tPSize = self.mdp['transProbs'][stateTrans][1]
zeroLoc = self.mdp['transProbs'][stateTrans][0]
stateAction = {}
Obs = self.mdp['Obs']
# Iterate over all possible actions
for j in range(0, int(tPSize)):
for i in range(0, int(tPSize)):
stateAction['x'], stateAction[
'y'] = x + i - zeroLoc, y + j - zeroLoc
if i != j:
if self.isOnMap(sPrime) and self.isOnMap(stateAction):
if not self.gm.ObsDetLatLon(self.gm.lat_pts[sPrime['y']],self.gm.lon_pts[sPrime['x']], \
self.gm.lat_pts[stateAction['y']],self.gm.lon_pts[stateAction['x']]):
transProb = transProbs[j][i]
totalProb += transProb
Util += transProb * self.mdp['U'][
x + i - zeroLoc][y + j - zeroLoc]
if totalProb != 1:
transProb = 1 - totalProb
Util += transProb * self.mdp['U'][x][y]
#print Util
return Util
def CalculateTransUtilities(self, State):
x, y = State['x'], State['y']
width, height = self.mdp['width'], self.mdp['height']
# We have 4 possible actions we can try. Up, Down, Left and Right.
UtilVec = {}
sPrime = {}
for j in range(-1, 2):
for i in range(-1, 2):
if i != j:
sPrime['x'], sPrime['y'] = x + i, y + j
if not self.gm.GetObs(self.gm.lat_pts[y],
self.gm.lon_pts[x]):
stateTrans = '%d,%d,%d,%d' % (x, y, x + i, y + j)
UtilVec[
stateTrans] = self.GetUtilityForStateTransition(
State, sPrime)
return UtilVec
def CalculateTransUtilitiesOld(self, State):
x, y = State['x'], State['y']
width, height = self.mdp['width'], self.mdp['height']
# We have 4 possible actions we can try. Up, Down, Left and Right.
UtilVec = {}
sPrime = {}
for i in range(-1, 2):
for j in range(-1, 2):
if i != j:
sPrime['x'], sPrime['y'] = x + i, y + j
if not self.gm.GetObs(self.gm.lat_pts[y],
self.gm.lon_pts[x]):
stateTrans = '%d,%d,%d,%d' % (x, y, x + i, y + j)
UtilVec[
stateTrans] = self.GetUtilityForStateTransition(
State, sPrime)
return UtilVec
def doValueIteration(self, eps, MaxRuns=25):
delta = 0
gamma = 1
width, height = self.gm.Width, self.gm.Height
U = np.zeros((width, height))
Uprime = np.zeros((width, height))
R = self.mdp['Rwd']
print R
self.mdp['U'] = U
self.mdp['Uprime'] = Uprime
for i in range(0, MaxRuns):
print '------------------------------- Running Iteration: %d, Delta=%.3f.' % (
i, delta)
U = Uprime.copy()
self.mdp['U'] = U
self.mdp['Uprime'] = Uprime
delta = 0
for State in self.mdp['States'].values():
Rwd = R[State['x']][State['y']]
#print Rwd
if not self.gm.GetObs(self.gm.lat_pts[State['y']],
self.gm.lon_pts[State['x']]):
Utils = self.CalculateTransUtilities(State)
ExpUtilVec = Utils.values(
) #[item for sublist in Utils.values() for item in sublist]
maxExpectedUtility = max(ExpUtilVec)
isTermState, isTrapped, TermStateUpdate = self.identifyTerminalState(
State)
if isTermState: # Don't give it a reward the second time around
if isTrapped:
Rwd = 0
else:
Rwd = R[State['x']][State['y']]
self.mdp['TermStates'] = TermStateUpdate
Uprime[State['x']][State[
'y']] = Rwd + gamma * U[State['x']][State['y']]
else:
Uprime[State['x']][
State['y']] = Rwd + gamma * maxExpectedUtility
absDiff = abs(Uprime[State['x']][State['y']] -
U[State['x']][State['y']])
if (absDiff > delta):
delta = absDiff
print 'delta=%f' % (delta)
print 'U', U
print 'Uprime', Uprime
if (delta <= eps * (1 - gamma) / gamma):
print 'delta is smaller than eps %f. Terminating.' % (delta)
break
self.pol_tree = self.GetPolicyTreeForMDP()
'''
def DisplayPolicy(self,FigHandle = None): # TODO: Check this up again... Most of the indices are reversed!!!
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 self.mdp.has_key('TermStates'):
TermStates= self.mdp['TermStates'][0]
else:
TermStates={}; TermStates['x']='None'; TermStates['y']=None
if FigHandle==None:
plt.figure()
self.gm.PlotNewRiskMapFig()
#for y in range(0,height):
# for x in range(0,width):
for a in self.g.nodes():
x,y = self.GetXYfromNodeStr(a)
Xloc[x][y],Yloc[x][y] = x,y
UtilVec = np.zeros(10)
maxUtil = -float('inf')
k,maxU,maxV= 0,0,0
if not self.gm.GetObs(self.gm.lat_pts[y],self.gm.lon_pts[x]) and not(TermStates['x']==x and TermStates['y']==y):
for j in range(-1,2):
for i in range(-1,2):
if (not(i==0 and j==0) and (x+i)>=0 and (x+i)<width and (y+j)>=0 and (y+j)<height \
and not self.gm.ObsDetLatLon(self.gm.lat_pts[y],self.gm.lon_pts[x],self.gm.lat_pts[y+j],self.gm.lon_pts[x+i])):
UtilVec[k] = Uprime[x+i][y+j]
if maxUtil<=UtilVec[k]:
maxUtil = UtilVec[k]
maxU,maxV = i,j
k=k+1
Xpolicy[x][y],Ypolicy[x][y] = 0.5*maxU, 0.5*maxV
plt.quiver(Xloc,Yloc,Xpolicy,Ypolicy)
plt.title('MDP Policy')
return Xpolicy,Ypolicy
'''
def DisplayPolicy(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.g.nodes():
x, y = self.GetXYfromNodeStr(a)
Xloc[x][y], Yloc[x][y] = x, y
UtilVec = np.zeros(10)
maxUtil = -float('inf')
k, maxU, maxV = 0, 0, 0
for u, v, d in self.g.out_edges(a, data=True):
i, j = self.GetXYfromNodeStr(v)
UtilVec[k] = Uprime[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)
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 = self.GetXYfromNodeStr(a)
UtilVec = np.zeros(10)
maxUtil = -float('inf')
k, maxU, maxV = 0, 0, 0
for u, v, d in self.g.out_edges(a, data=True):
i, j = self.GetXYfromNodeStr(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):
if curNode == goal:
return self.goal[0], self.goal[1]
self.UseNetworkX = True
import networkx as nx
try:
if self.UseNetworkX:
neighborNode = self.gm2.neighbors(curNode)
if len(neighborNode) > 0:
nextNode = neighborNode[0]
else:
nextNode = None
else:
nextNode = self.pol_tree.node_neighbors[curNode]
except KeyError:
nextNode = None
except nx.NetworkXError:
nextNode = None
if nextNode != None:
m = re.match('\(([0-9]+),([0-9]+)\)', nextNode) # Changed this!!!
m2 = re.match('\(([0-9]+),([0-9]+)\)', goal)
if m:
return int(m.group(1)), int(m.group(2))
if m2:
return int(m2.group(1)), int(m2.group(2))
if forceGoal == True:
return self.goal[0], self.goal[1]
else:
return self.FindNearestNodeOnGraph()
'''
def GetPolicyTreeForMDP(self):
try:
import pygraphviz as gv
except ImportError:
print 'pygraphviz not installed'
try:
import networkx as nx
self.UseNetworkX = True
except ImportError:
print 'networkX not installed. Please install it!'
width, height = self.gm.Width, self.gm.Height
Uprime = self.mdp['Uprime']
TermStates= self.mdp['TermStates'][0]
if self.UseNetworkX == True:
self.gm2 = nx.DiGraph()
else:
self.gm2 = digraph()
for y in range(0,height):
for x in range(0,width):
#if mdp['Obs'][x][y]!=1:
if not self.gm.GetObs(self.gm.lat_pts[y],self.gm.lon_pts[x]): #mdp['Obs'][y][x]!=1:
if self.UseNetworkX == True:
self.gm2.add_node('(%d,%d)'%(x,y))
else:
self.gm2.add_nodes(['(%d,%d)'%(x,y)])
self.gm2.add_node_attribute('(%d,%d)'%(x,y),('position',(x,y)))
for y in range(0,height):
for x in range(0,width):
if not self.gm.GetObs(self.gm.lat_pts[y],self.gm.lon_pts[x]):
#if mdp['Obs'][x][y]!=1:
maxUtil = -float('inf')
UtilVec = np.zeros(20)
k,maxU,maxV = 0,0,0
if not (TermStates['x']==x and TermStates['y']==y):
for j in range(-1,2):
for i in range(-1,2):
#if (not(i==0 and j==0) and (x+i)>=0 and (x+i)<width and (y+j)>=0 and (y+j)<height and mdp['Obs'][x+i][y+j]!=1):
if (not(i==0 and j==0) and (x+i)>=0 and (x+i)<width and (y+j)>=0 and (y+j)<height \
and not self.gm.ObsDetLatLon(self.gm.lat_pts[y],self.gm.lon_pts[x],self.gm.lat_pts[y+j],self.gm.lon_pts[x+i])):
#UtilVec[k] = Uprime[x+i][y+j]
UtilVec[k] = Uprime[x+i][y+j]
if maxUtil<=UtilVec[k]:
maxUtil = UtilVec[k]
maxU,maxV = i,j
k=k+1
if not(maxU==0 and maxV==0):
if self.UseNetworkX == False:
self.gm2.add_edge(('(%d,%d)'%(x,y),'(%d,%d)'%(x+maxU,y+maxV)),wt = maxUtil)
else:
self.gm2.add_edge('(%d,%d)'%(x,y),'(%d,%d)'%(x+maxU,y+maxV),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):
if curNode == goal:
return self.goal[0],self.goal[1]
self.UseNetworkX = True
import networkx as nx
try:
if self.UseNetworkX:
neighborNode = self.gm2.neighbors(curNode)
if len(neighborNode)>0:
nextNode = neighborNode[0]
else:
nextNode = None
else:
nextNode = self.pol_tree.node_neighbors[curNode]
except KeyError:
nextNode=None
except nx.NetworkXError:
nextNode = None
if nextNode!=None:
m = re.match('\(([0-9]+),([0-9]+)\)', nextNode) # Changed this!!!
m2 =re.match('\(([0-9]+),([0-9]+)\)',goal)
if m:
return int(m.group(1)),int(m.group(2))
if m2:
return int(m2.group(1)),int(m2.group(2))
if forceGoal==True:
return self.goal[0],self.goal[1]
else:
return self.FindNearestNodeOnGraph(curNode)
'''
def FindNearestNodeOnGraph(self, curNode):
nearest_dist = float('inf')
nearest_node = (None, None)
for a in self.g.nodes():
i, j = self.GetXYfromNodeStr(curNode)
x, y = self.GetXYfromNodeStr(a)
dist = math.sqrt((i - x)**2 + (j - y)**2)
if (dist < nearest_dist):
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]):
nearest_dist = dist
nearest_node = (x, y)
return nearest_node
def GetRiskFromXY(self, x, y):
lat, lon = self.gm.GetLatLonfromXY(x, y)
return self.gm.GetRisk(lat, lon)
def SimulateAndPlotMDP_PolicyExecution(self,
start,
goal,
k,
newFig=True,
lineType='r-',
NoPlot='False'):
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
a = start
done = False
i = 0
#x_sims,y_sims = np.zeros((24*gm.numDays,1)),np.zeros((24*gm.numDays,1))
if k >= (24 * self.gm.numDays):
k = 24 * self.gm.numDays - 1
x_sims, y_sims = 0, 0
self.finX, self.finY = start[0], start[1]
#import pdb; pdb.set_trace()
while (not done):
self.numHops += 1
try:
bx, by = self.GetPolicyAtCurrentNode(
'(%d,%d)' % (int(a[0] + 0.5), int(a[1] + 0.5)),
'(%d,%d)' % (goal[0], goal[1]))
except TypeError:
bx, by = None, None
if bx != None and by != None:
b = (bx, by)
#tempRisk = self.GetRiskFromXY(bx,by)
#totalRisk+=tempRisk
self.distFromGoal = math.sqrt((a[0] - goal[0])**2 +
(a[1] - goal[1])**2)
if self.distFromGoal <= self.acceptR:
self.isSuccess = True
done = True
sLat, sLon = self.gm.GetLatLonfromXY(a[0], a[1])
gLat, gLon = self.gm.GetLatLonfromXY(b[0], b[1])
tStart = k + self.totalTime / 3600.
if tStart >= 24 * self.gm.numDays:
tStart = 24 * self.gm.numDays - 1
xFin,yFin,latFin,lonFin,latArray,lonArray,depthArray,tArray,possibleCollision,CollisionReason,totalDist = \
self.gm.SimulateDive(sLat,sLon,gLat,gLon, self.gm.maxDepth, self.u, self.v, self.lat, self.lon, self.depth, tStart, False )
#gm.SimulateDive(gm.lat_pts[a[1]], gm.lon_pts[a[0]], gm.lat_pts[b[1]], gm.lon_pts[b[0]], gm.maxDepth, u, v, lat, lon, depth, k)
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
if len(tArray > 0):
self.totalTime += tArray[-1]
if possibleCollision == False:
tempX, tempY = self.gm.GetXYfromLatLon(
np.array(latArray), np.array(lonArray))
x_sims, y_sims = tempX[-1:][0], tempY[-1:][
0] # TODO: This might be wrong!
plt.plot(tempX, tempY, lineType)
if x_sims != [] and y_sims != []:
tempRisk = self.GetRiskFromXY(x_sims, y_sims)
self.finX, self.finY = x_sims, y_sims
self.totalRisk += tempRisk
else:
self.totalRisk += self.gm.GetRisk(sLat, sLon)
else:
if self.CollisionReason == 'RomsNanAtStart':
self.totalRisk += self.gm.GetRisk(sLat, 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, lineType)
x_sims, y_sims = 0, 0
done = True
return self.totalRisk, True # Landed on beach!
#plt.plot([a[0],x_sims[0]],[a[1],y_sims[0]],lineType)
try:
a = (x_sims[0], y_sims[0])
self.finX, self.finY = a
except IndexError:
done = True
return self.totalRisk, True # Landed on beach
#import pdb; pdb.set_trace()
i = i + 1
if i > self.maxLengths:
done = True
else: # We did not get a policy here.
self.CollisionReason = 'DidNotFindPolicy'
done = True
return self.totalRisk, False
def InitMDP_Simulation(self,
start,
goal,
startT,
lineType='r',
newFig=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
'''
The Re-Entrant version of the MDP - Policy Execution.
'''
def SimulateAndPlotMDP_PolicyExecution_R(self,
simulTime=-1,
PostDeltaSimCallback=None,
PostSurfaceCallbackFn=None):
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]))
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])
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:][0], tempY[-1:][0]
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 SA_Replanner(object):
''' Replanner - A class that executes one of two types of planners (Min-Risk or Min-Expected-Risk).
The replanning stems from the fact that when the robot surfaces at the waypoint it thinks
it hit, it replans and resumes execution (at-least during simulation of such a planner).
'''
def __init__(self,shelfName='RiskMap.shelf',sfcst_directory='./',dMax = 1.5):
self.gm = GliderModel(shelfName,sfcst_directory) # Initializes risk, obs maps.
self.gs = GliderSimulator(shelfName,sfcst_directory)
self.Dmax = dMax
self.maxLengths = 50
self.numDays = 3
self.acceptR = 0.6
self.maxDepth = 60.
self.UseNetworkX = None
self.SARisks={}
try:
if self.UseNetworkX == None:
import networkx as nx
self.UseNetworkX = True
except ImportError:
print 'Please install networkX'
pass
def GetTransitionModelFromShelf(self,yy,mm,dd,numDays,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
* numDays (int): number of days the model is being built over
* 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.
"""
self.posNoise = posNoise; self.currentNoise = currentNoise
if posNoise==None and currentNoise==None:
gmShelf = shelve.open('%s/gliderModel_%04d%02d%02d_%d.shelf'%(shelfDirectory,yy,mm,dd,numDays), writeback=False )
if posNoise!=None:
if currentNoise!=None:
gmShelf = shelve.open('%s/gliderModel_%04d%02d%02d_%d_%.3f_RN_%.3f.shelf'%(shelfDirectory,yy,mm,dd,numDays,posNoise,currentNoise),writeback=False)
else:
gmShelf = shelve.open('%s/gliderModel_%04d%02d%02d_%d_%.3f.shelf'%(shelfDirectory,yy,mm,dd,numDays,posNoise), writeback=False)
if posNoise==None and currentNoise!=None:
gmShelf=shelve.open('%s/gliderModel_%04d%02d%02d_%d_RN_%.3f.shelf'%(shelfDirectory,yy,mm,dd,numDays,currentNoise), writeback=False)
self.gm.TransModel = gmShelf['TransModel']
self.gm.FinalLocs = gmShelf['FinalLocs']
self.gm.TracksInModel = gmShelf['TracksInModel']
self.gModel = {}
self.gModel['TransModel'] = gmShelf['TransModel']
gmShelf.close()
def GetRomsData(self,yy,mm,dd,numDays,UpdateSelf=True,usePredictionData=False):
useNewFormat = True
u,v,time1,depth,lat,lon = self.gs.gm.GetRomsData(yy,mm,dd,numDays,useNewFormat,usePredictionData)
u,v,time1,depth,lat,lon = self.gm.GetRomsData(yy,mm,dd,numDays,useNewFormat,usePredictionData)
self.u,self.v,self.time1,self.depth,self.lat,self.lon = u,v,time1,depth,lat,lon
self.numDays = numDays
return u,v,time1,depth,lat,lon
def isOnMapRP(self,s):
width, height = self.gm.Width, self.gm.Height
if s[0]<0 or s[0]>=width:
return False
if s[1]<0 or s[1]>=height:
return False
return True
def GetExpRiskForStateAction(self,state_str,action_str):
''' Get the expected risk for a state-action pair.We use a dictionary here to
perform a lookup if we have already computed this earlier.
Args:
state (node_str) :
'''
OffMapNotObs = False
state_action_str = '%s_%s'%(state_str,action_str)
if self.SARisks.has_key(state_action_str):
return self.SARisks[state_action_str]
else:
m=re.match('([0-9\.]+),([0-9\.]+)',state_str)
if m:
x,y = float(m.group(1)),float(m.group(2))
X = self.gm.FinalLocs[action_str][0]; Y = self.gm.FinalLocs[action_str][1]
tot_cost = 0
for i in range(0,len(X)):
lat,lon = self.gm.GetLatLonfromXY(x+X[i],y+Y[i])
#print X[i],Y[i],lat,lon
tot_cost += self.gm.GetRisk(lat,lon,OffMapNotObs)
self.SARisks[state_action_str] = tot_cost
return self.SARisks[state_action_str]
def CreateExpRiskGraph(self):
import networkx as nx
self.g = self.gm.lpGraph.copy()
OffMapNotObs = False
for a in self.g.nodes():
for b in self.g.nodes():
x,y = self.GetXYfromNodeStr(a)
i,j = self.GetXYfromNodeStr(b)
if a!=b:
if self.gm.FinalLocs.has_key('%d,%d,%d,%d'%(x,y,i,j)):
expRisk = self.GetExpRiskForStateAction('%d,%d'%(x,y),'%d,%d,%d,%d'%(x,y,i,j))
if expRisk !=None:
self.g.add_edge('(%d,%d)'%(x,y),'(%d,%d)'%(i,j),weight=expRisk)
print 'Added edge (%d,%d) to (%d,%d) with weight=%.2f'%(i,j,x,y,expRisk)
return self.g
def GetXYfromNodeStr(self,nodeStr):
''' Convert from the name of the node string to the locations.
'''
m = re.match('\(([0-9\.]+),([0-9\.]+)\)',nodeStr)
if m:
return int(m.group(1)),int(m.group(2))
else:
return None, None
def GetNodesFromEdgeStr(self,edgeStr):
''' Convert from a transition model edge string to the
respective edges on the graph
'''
m = re.match('([0-9]+),([0-9]+),([0-9]+),([0-9]+)',edgeStr)
x1,y1,x2,y2 = None,None,None,None
if m:
x1,y1,x2,y2 = int(m.group(1)),int(m.group(2)),int(m.group(3)),int(m.group(4))
return x1,y1,x2,y2
def CreateGraphUsingProximityGraph(self,graphType='MinExpRisk'):
import networkx as nx
self.g = nx.DiGraph()
self.g = self.gm.lpGraph.copy()
for a in self.g.nodes():
for b in self.g.nodes():
if a!=b:
i,j = self.GetXYfromNodeStr(a); x,y = self.GetXYfromNodeStr(b)
if math.sqrt((i-x)**2+(j-y)**2)<=self.Dmax:
#print 'Considering: (%d,%d) to (%d,%d):'%(i,j,x,y)
if self.gm.ObsDetLatLon(self.gm.lat_pts[j],self.gm.lon_pts[i],self.gm.lat_pts[y],self.gm.lon_pts[x])==False:
if graphType == 'MinExpRisk':
expRisk = self.GetExpRisk((i,j),(x,y))
if expRisk !=None:
self.g.add_edge('(%d,%d)'%(i,j),'(%d,%d)'%(x,y),weight=expRisk)
#print 'Added edge (%d,%d) to (%d,%d) with weight=%.2f'%(i,j,x,y,expRisk)
else:
print 'Rejected edge (%d,%d) to (%d,%d) with weight=None'%(i,j,x,y)
elif graphType == 'MinRisk':
riskVal = self.gm.GetRisk(self.gm.lat_pts[y],self.gm.lon_pts[x])
if riskVal!=None:
self.g.add_edge('(%d,%d)'%(i,j),'(%d,%d)'%(x,y),weight=riskVal)
### If we want to write this graph out.
#dot = write(g)
#G = gv.AGraph(dot)
#G.layout(prog ='dot')
#G.draw('SCB.png')
return self.g
def CreateMinRiskGraph(self):
'''
A ROMS unaware planner. This planner totally avoids locations
'''
# Short-circuit this!
return self.CreateGraphUsingProximityGraph('MinRisk')
# This is what we used to do earlier. Won't be running
# unless we actually want it to.
import networkx as nx
self.g = nx.DiGraph()
for j in range(0,self.gm.Height):
for i in range(0,self.gm.Width):
if self.UseNetworkX:
self.g.add_node('(%d,%d)'%(i,j))
else:
self.g.add_nodes(['(%d,%d)'%(i,j)])
self.g.add_node_attribute('(%d,%d)'%(i,j),('position',(i,j)))
for j in range(0,self.gm.Height):
for i in range(0,self.gm.Width):
for y in range(0,self.gm.Height):
for x in range(0,self.gm.Width):
if math.sqrt((i-x)**2+(j-y)**2)<=self.Dmax:
if self.gm.ObsDetLatLon(self.gm.lat_pts[j],self.gm.lon_pts[i],self.gm.lat_pts[y],self.gm.lon_pts[x])==False:
riskVal = self.gm.GetRisk(self.gm.lat_pts[y],self.gm.lon_pts[x])
if riskVal!=None:
if self.UseNetworkX==True:
self.g.add_edge('(%d,%d)'%(i,j),'(%d,%d)'%(x,y),weight=riskVal)
else:
self.g.add_edge(('(%d,%d)'%(i,j),'(%d,%d)'%(x,y)),wt=riskVal)
return self.g
def GetShortestPathMST(self,goal):
''' Find the shortest path Minimum-Spanning Tree
'''
if self.g!= None:
if self.UseNetworkX:
import networkx as nx
self.sp_mst = nx.all_pairs_dijkstra_path(self.g)
self.dist = nx.all_pairs_dijkstra_path_length(self.g)
else:
from pygraph.algorithms.minmax import shortest_path
self.sp_mst, self.dist = shortest_path(self.g,'(%d,%d)'%(goal[0],goal[1]))
return self.sp_mst, self.dist
else:
print 'CreateExpRiskGraph first before calling GetShortestPathMST(goal)'
return None, None
def GetPathXYfromPathList(self,path2goal):
''' Find the path in two lists (pathX,pathY) from the string values of path-list
'''
pathX,pathY = [],[]
for pathEl in path2goal:
m = re.match('\(([0-9]+),([0-9]+)\)', pathEl)
if(m):
pathX.append(int(m.group(1)))
pathY.append(int(m.group(2)))
return pathX,pathY
def BackTrackNxPath(self,source,goal):
source_str = '(%d,%d)'%(source[0],source[1])
goal_str = '(%d,%d)'%(goal[0],goal[1])
try:
path2goal = self.sp_mst[source_str][goal_str]
except KeyError:
return None, None, None
pathX,pathY = self.GetPathXYfromPathList(path2goal)
return path2goal, pathX, pathY
def BackTrackPath(self,mst,source):
# source and goal are specified as tuples
#import pdb; pdb.set_trace()
source_str = '(%d,%d)'%(source[0],source[1])
path2goal = []
pathX,pathY = [],[]
path_str = source_str
path2goal.append(path_str)
try:
while(mst[path_str]!=None):
path_str = mst[path_str]
path2goal.append(path_str)
#path2goal.reverse()
print path2goal
pathX,pathY = self.GetPathXYfromPathList(path2goal)
except KeyError:
return None, None, None
return path2goal, pathX, pathY
def PlotMRpaths(self,goal,figHandle=None):
if figHandle==None:
fig = plt.figure()
else:
fig = figHandle
plt.title('Plot of SA - Min-Exp Risk MST (Goal at:(%d,%d))'%(goal[0],goal[1]))
#plt.imshow(rMap.transpose()+0.5,origin='upper')
#plt.imshow(1-GetRiskMap(),origin='upper')
self.gm.PlotNewRiskMapFig(False)
sp_mst,dist = self.GetShortestPathMST(goal)
for j in range(0,self.gm.Height):
for i in range(0,self.gm.Width):
if not self.gm.GetObs(self.gm.lat_pts[j],self.gm.lon_pts[i]):
if self.UseNetworkX:
path2goal,pathX,pathY = self.BackTrackNxPath((i,j), goal)
else:
path2goal,pathX,pathY = self.BackTrackPath(sp_mst,(i,j))
plt.plot(pathX,pathY,'k*-') # TODO: Check why this is so!!!
#plt.xlim((-0.5,self.gm.Width-0.5))
#plt.ylim((-0.5,self.gm.Height-0.5))
return fig
def GetPolicyAtCurrentNode(self,curNode,goal,forceGoal=False):
#print 'curNode',curNode
x,y = curNode #self.GetXYfromNodeStr(curNode)
return self.GetPolicyNxMR((x,y))
def GetPolicyNxMR(self,curNode):
#newcurNode = self.FindNearestNodeOnGraph(curNode,self.goal)
if curNode == self.goal:
return self.goal[0],self.goal[1]
path2goal,pathX,pathY = self.BackTrackNxPath(curNode, self.goal)
if pathX==None and pathY==None:
#import pdb; pdb.set_trace()
newcurNode = self.FindNearestNodeOnGraph(curNode,self.goal)
return newcurNode
#path2goal, pathX, pathY = self.BackTrackNxPath(newcurNode, self.goal)
#print 'debug: ',curNode, newcurNode, pathX, pathY
if len(path2goal)>1:
return pathX[1],pathY[1]
else:
return pathX[0],pathY[0]
def FindNearestNodeOnGraph(self,curNode,goal):
nearest_dist = float('inf')
nearest_node = (None,None)
#import pdb; pdb.set_trace()
for a in self.g.nodes():
i,j = curNode
x,y = self.GetXYfromNodeStr(a)
dist = math.sqrt((i-x)**2+(j-y)**2)
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.dist['(%d,%d)'%(x,y)]['(%d,%d)'%(goal[0],goal[1])] < nearest_dist and dist<self.Dmax:
nearest_dist = self.dist['(%d,%d)'%(x,y)]['(%d,%d)'%(goal[0],goal[1])]
nearest_node = (x,y)
return nearest_node
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 GetPolicyMR(self,mst,curNode):
m1 = re.match('\(([0-9]+),([0-9]+)\)', curNode)
if m1:
if mst.has_key(curNode):
nextNode = mst[curNode]
if nextNode!=None:
m2 = re.match('\(([0-9]+),([0-9]+)\)', nextNode )
if m2:
return int(m2.group(1)),int(m2.group(2))
return int(m1.group(1)),int(m1.group(2))
return None,None
def GetRiskFromXY(self,x,y):
lat,lon = self.gm.GetLatLonfromXY(x,y)
return self.gm.GetRisk(lat,lon)
def SimulateAndPlotMR_PolicyExecution(self,start,goal,mst,simStartTime,newFig=True,lineType='y-'):
if newFig:
plt.figure()
plt.title('Plotting Min-Exp Risk Ensemble')
self.gm.PlotNewRiskMapFig()
self.totalRisk = 0
self.totalPathLength = 0.
self.distFromGoal = float('inf')
self.totalTime = 0.
self.goal = goal
self.start = start
self.numHops = 0
self.isSuccess = False
return self.gs.SimulateAndPlot_PolicyExecution(start,goal,simStartTime,self.maxDepth,self.GetPolicyAtCurrentNode)
#---------- Moving away from running our own version of the simulation -----
a= start
done = False
i=0
#x_sims,y_sims = np.zeros((24*gm.numDays,1)),np.zeros((24*gm.numDays,1))
if k>=(24*self.gm.numDays):
k = 24*self.gm.numDays-1
x_sims,y_sims = 0,0
self.finX,self.finY = start[0],start[1]
#import pdb; pdb.set_trace()
while (not done):
self.numHops += 1
try:
if self.UseNetworkX==True:
bx,by = self.GetPolicyNxMR((int(a[0]+0.5),int(a[1]+0.5)))
else:
bx,by = self.GetPolicyMR(mst,'(%d,%d)'%(int(a[0]+0.5),int(a[1]+0.5)))
except TypeError:
bx,by = None, None
if bx!=None and by!=None:
b = (bx,by)
self.distFromGoal = math.sqrt((a[0]-goal[0])**2+(a[1]-goal[1])**2)
if self.distFromGoal <=self.acceptR:
self.isSuccess = True
done = True
sLat,sLon = self.gm.GetLatLonfromXY(a[0],a[1])
gLat,gLon = self.gm.GetLatLonfromXY(b[0],b[1])
tStart = k + self.totalTime/3600.
if tStart>=24*self.gm.numDays:
tStart = 24*self.gm.numDays-1
xFin,yFin,latFin,lonFin,latArray,lonArray,depthArray,tArray,possibleCollision,CollisionReason,totalDist = \
self.gm.SimulateDive(sLat,sLon,gLat,gLon, self.gm.maxDepth, self.u, self.v, self.lat, self.lon, self.depth, tStart, False )
self.totalPathLength += totalDist
self.CollisionReason = CollisionReason
#gm.SimulateDive(gm.lat_pts[a[1]], gm.lon_pts[a[0]], gm.lat_pts[b[1]], gm.lon_pts[b[0]], gm.maxDepth, u, v, lat, lon, depth, k)
# This will allow us to compute how far from the goal we are...
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
if len(tArray>0):
self.totalTime+=tArray[-1]
if possibleCollision == False:
tempX,tempY = self.gm.GetXYfromLatLon(np.array(latArray),np.array(lonArray))
x_sims,y_sims = tempX[-1:][0],tempY[-1:][0]
plt.plot(tempX,tempY,lineType)
if x_sims!=[] and y_sims!=[]:
tempRisk = self.GetRiskFromXY(x_sims,y_sims)
self.finX, self.finY = x_sims, y_sims
self.totalRisk+=tempRisk
else:
self.totalRisk+= self.gm.GetRisk(sLat,sLon)
else:
if self.CollisionReason == 'RomsNanAtStart':
self.totalRisk+= self.gm.GetRisk(sLat, 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,lineType)
x_sims,y_sims = 0,0
done=True
return self.totalRisk,True # landed = true
#plt.plot([a[0],x_sims[0]],[a[1],y_sims[0]],lineType)
try:
a = (x_sims[0],y_sims[0])
except IndexError:
done = True
return self.totalRisk, True
#import pdb; pdb.set_trace()
i=i+1
if i>self.maxLengths:
done = True
else:
self.CollisionReason = 'DidNotFindPolicy'
done = True
return self.totalRisk,False
def InitSARP_Simulation(self,start,goal,startT,lineType='r',newFig = False):
''' Initialize Simulation of the SARP 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 SimulateAndPlotSARP_PolicyExecution_R(self,simulTime=-1,PostDeltaSimCallback=None,PostSurfaceCallbackFn=None):
''' Simulate and plot the SARP 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