def h_walldist(state, fline, walls):
"""
The first time this function is called, for each gridpoint that's not inside a wall
it will cache a rough estimate of the length of the shortest path to the finish line.
The computation is done by a breadth-first search going backwards from the finish
line, one gridpoint at a time.
On all subsequent calls, this function will retrieve the cached value and add an
estimate of how long it will take to stop.
"""
global g_fline, g_walls
if fline != g_fline or walls != g_walls or grid == []:
edist_grid(fline, walls)
((x,y),(u,v)) = state
hval = float(grid[x][y])
# add a small penalty to favor short stopping distances
au = abs(u); av = abs(v);
sdu = au*(au-1)/2.0
sdv = av*(av-1)/2.0
sd = max(sdu,sdv)
penalty = sd/10.0
# compute location after fastest stop, and add a penalty if it goes through a wall
if u < 0: sdu = -sdu
if v < 0: sdv = -sdv
sx = x + sdu
sy = y + sdv
if racetrack.crash([(x,y),(sx,sy)],walls):
penalty += math.sqrt(au**2 + av**2)
hval = max(hval+penalty,sd)
return hval
def edist_grid(fline, walls):
global grid, g_fline, g_walls, xmax, ymax
xmax = max([max(x, x1) for ((x, y), (x1, y1)) in walls])
ymax = max([max(y, y1) for ((x, y), (x1, y1)) in walls])
grid = [[edistw_to_finish((x, y), fline, walls) for y in range(ymax + 1)]
for x in range(xmax + 1)]
flag = True
print('computing edist grid', end=' ')
sys.stdout.flush()
while flag:
print('.', end='')
sys.stdout.flush()
flag = False
for x in range(xmax + 1):
for y in range(ymax + 1):
for y1 in range(max(0, y - 1), min(ymax + 1, y + 2)):
for x1 in range(max(0, x - 1), min(xmax + 1, x + 2)):
if grid[x1][y1] != infinity and not racetrack.crash(
((x, y), (x1, y1)), walls):
if x == x1 or y == y1:
d = grid[x1][y1] + 1
else:
# In principle, it seems like a taxicab metric should be just as
# good, but Euclidean seems to work a little better in my tests.
d = grid[x1][y1] + 1.4142135623730951
if d < grid[x][y]:
grid[x][y] = d
flag = True
print(' done')
g_fline = fline
g_walls = walls
return grid
def xymax_grid(fline, walls):
global grid, g_metric, g_fline, g_walls, xmax, ymax
xmax = max([max(x, x1) for ((x, y), (x1, y1)) in walls])
ymax = max([max(y, y1) for ((x, y), (x1, y1)) in walls])
grid = [[xymaxw_to_line((x, y), fline, walls) for y in range(ymax + 1)]
for x in range(xmax + 1)]
flag = True
print('computing xymax grid', end=' ')
sys.stdout.flush()
while flag:
print('.', end='')
sys.stdout.flush()
flag = False
for x in range(xmax + 1):
for y in range(ymax + 1):
for y1 in range(max(0, y - 1), min(ymax + 1, y + 2)):
for x1 in range(max(0, x - 1), min(xmax + 1, x + 2)):
if grid[x1][y1] != infinity and not racetrack.crash(
((x, y), (x1, y1)), walls):
d = grid[x1][y1] + max(abs(x - x1), abs(y - y1))
if d < grid[x][y]:
grid[x][y] = d
flag = True
print(' done')
g_metric = 'xymax'
g_fline = fline
g_walls = walls
return grid
def edist_grid(fline,walls):
global grid, g_fline, g_walls, xmax, ymax
xmax = max([max(x,x1) for ((x,y),(x1,y1)) in walls])
ymax = max([max(y,y1) for ((x,y),(x1,y1)) in walls])
grid = [[edistw_to_line((x,y), fline, walls) for y in range(ymax+1)] for x in range(xmax+1)]
flag = True
print('computing edist grid', end=' '); sys.stdout.flush()
while flag:
print('.', end=''); sys.stdout.flush()
flag = False
for x in range(xmax+1):
for y in range(ymax+1):
for y1 in range(max(0,y-1),min(ymax+1,y+2)):
for x1 in range(max(0,x-1),min(xmax+1,x+2)):
if grid[x1][y1] != infinity and not racetrack.crash(((x,y),(x1,y1)),walls):
if x == x1 or y == y1:
d = grid[x1][y1] + 1
else:
d = grid[x1][y1] + 1.4142135623730951
if d < grid[x][y]:
grid[x][y] = d
flag = True
print(' done')
g_fline = fline
g_walls = walls
return grid
def dist(pt1, pt2, walls):
if not racetrack.crash((pt1, pt2), walls):
(x1, y1) = pt1
(x2, y2) = pt2
return math.sqrt((x1 - x2)**2 + (y1 - y2)**2)
else:
return infinity
def cost_to_go(s, z, finish, walls):
""" calculate cost to go Q(s,z)
s:current state, z:velocity (u,v) """
(u, v) = z
# next correct state without error:
(x_correct, y_correct) = (s[0][0]+u, s[0][1]+v)
# probability mass function:
u_same = 0.6 if abs(u) > 1 else 1
u_diff = 0.2
v_same = 0.6 if abs(v) > 1 else 1
v_diff = 0.2
cost = 0
for ((x_next, y_next), z_next) in next_possible_states_with_z(s, z):
weight = (u_same * (x_next == x_correct) + \
u_diff * (x_next != x_correct)) * \
(v_same * (y_next == y_correct) + \
v_diff * (y_next != y_correct))
s_next = ((x_next,y_next),z_next)
# penalize not moving if not at the goal state
if not racetrack.goal_test(s_next,finish):
if z_next == (0,0):
V[s_next] = 100
if racetrack.crash((s[0],s_next[0]), walls):
V[s_next] = 100
cost += weight * V[s_next]
return (1 + cost)
def h_proj1(state, fline, walls):
'''
The first time this function is called, for points near all corners in the map it will
cacluate its distance to the finish line. It does this by seeing what the nearest corner point
that can reach the finish line with a euclidean distance without crashing and stores that distance
in a stack. It will then find the closest corner point that can reach the point at the top of the
stack and add it's euclidean distance to that point + the distance fromt that point to the finish line.
By then end most/all of points near corner will have an estimate of it's distance to the finish line.
At each state, the heurisitc will find the nearest corner point it can go to, and add it's
euclidean distance to it + the points distance to the finish line.
'''
global s_corners, xmax, ymax, stack
corner = None
if not corners:
print("filling in corners -------------------------------------------")
corn(walls, fline)
((x, y), (u, v)) = state
((x1, y1), (x2, y2)) = fline
'''
If the state has a straight line distance to to the finish line without any walls between,
use the euclidean heuristic provided
'''
if heuristics.edistw_to_finish((x, y), fline, walls) != infinity:
return heuristics.h_esdist(state, fline, walls)
else:
mininum = math.inf
for i, c in enumerate(stack):
distance_to_corner = heuristics.edistw_to_finish(
(x, y), [(c[0][0], c[0][1]), (c[0][0], c[0][1])], walls)
corner_to_fline = c[1]
d = distance_to_corner + corner_to_fline
if d <= mininum:
corner = c
mininum = d
index = i
# add a small penalty to favor short stopping distances
au = abs(u)
av = abs(v)
sdu = au * (au - 1) / 2.0
sdv = av * (av - 1) / 2.0
sd = max(sdu, sdv)
penalty = sd / 10.0
# compute location after fastest stop, and add a penalty if it goes through a wall
if u < 0: sdu = -sdu
if v < 0: sdv = -sdv
sx = x + sdu
sy = y + sdv
if racetrack.crash([(x, y), (sx, sy)], walls):
penalty += math.sqrt(au**2 + av**2)
hval = max(mininum + penalty, sd)
return hval
def edistw_to_line(point, edge, walls):
"""
straight-line distance from (x,y) to the line ((x1,y1),(x2,y2)).
Return infinity if there's no way to do it without intersecting a wall
"""
# if min(x1,x2) <= x <= max(x1,x2) and min(y1,y2) <= y <= max(y1,y2):
# return 0
(x,y) = point
((x1,y1),(x2,y2)) = edge
if x1 == x2:
ds = [math.sqrt((x1-x)**2 + (y3-y)**2) \
for y3 in range(min(y1,y2),max(y1,y2)+1) \
if not racetrack.crash(((x,y),(x1,y3)), walls)]
else:
ds = [math.sqrt((x3-x)**2 + (y1-y)**2) \
for x3 in range(min(x1,x2),max(x1,x2)+1) \
if not racetrack.crash(((x,y),(x3,y1)), walls)]
ds.append(infinity)
return min(ds)
def edistw_to_finish(point, fline, walls):
"""
straight-line distance from (x,y) to the finish line ((x1,y1),(x2,y2)).
Return infinity if there's no way to do it without intersecting a wall
"""
(x, y) = point
ds = infinity
((x1, y1), (x2, y2)) = fline
# make a list of distances to each reachable point in fline
if x1 == x2: # fline is vertical, so iterate over y
for y3 in range(min(y1, y2), max(y1, y2) + 1):
if not rt.crash(((x, y), (x1, y3)), walls):
ds = min(ds, math.sqrt((x1 - x)**2 + (y3 - y)**2))
else: # fline is horizontal, so iterate over x
for x3 in range(min(x1, x2), max(x1, x2) + 1):
if not rt.crash(((x, y), (x3, y1)), walls):
ds = min(ds, math.sqrt((x3 - x)**2 + (y1 - y)**2))
return ds
def edistw_to_finish(point, fline, walls):
"""
straight-line distance from (x,y) to the finish line ((x1,y1),(x2,y2)).
Return infinity if there's no way to do it without intersecting a wall
"""
(x, y) = point
((x1, y1), (x2, y2)) = fline
# make a list of distances to each reachable point in fline
if x1 == x2: # fline is vertical, so iterate over y
ds = [math.sqrt((x1 - x) ** 2 + (y3 - y) ** 2) \
for y3 in range(min(y1, y2), max(y1, y2) + 1) \
if not rt.crash(((x, y), (x1, y3)), walls)]
else: # fline is horizontal, so iterate over x
ds = [math.sqrt((x3 - x) ** 2 + (y1 - y) ** 2) \
for x3 in range(min(x1, x2), max(x1, x2) + 1) \
if not rt.crash(((x, y), (x3, y1)), walls)]
ds.append(infinity) # for the case where ds is empty
return min(ds)
def UCT(s, depth):
global fline, g_walls, Q, t, seen
if racetrack.goal_test(s, fline):
return -50
if depth == 0:
return heuristics.h_walldist(s, fline, g_walls)
# if there are no applicable velocities at this state then that means we are going to crash
if not applicable(s):
return 200
if not s in seen:
seen.append(s)
t[s] = 0
for z in applicable(s):
Q[(s, z)] = 0
t[(s, z)] = 0
untried = list(filter(lambda x: t[(s, x)] == 0, applicable(s)))
z_prime = None
if untried:
z_prime = random.choice(untried)
else:
#after experminentation, choosing 6 for the value of k seems to converge better
x = list(
map(
lambda x: Q[
(s, x)] - 6 * math.sqrt(math.log(t[s]) / t[(s, x)]),
applicable(s)))
min_index, min_val = 0, x[0]
for i, y in enumerate(x):
if y < min_val:
min_index, min_val = i, y
z_prime = applicable(s)[min_index]
(loc, (vx, vy)) = s
#(wx,wy) = (z_prime[0]+vx , z_prime[1]+vy)
error = supervisor.steering_error(z_prime[0], z_prime[1])
new_state = ((loc[0] + z_prime[0] + error[0],
loc[1] + z_prime[1] + error[1]), z_prime)
if racetrack.crash((loc, new_state[0]), g_walls):
cost = 50 #a penalty
else:
cost = 1 + UCT(new_state, depth - 1)
Q[(s, z_prime)] = (t[(s, z_prime)] * Q[(s, z_prime)] +
cost) / (1 + t[(s, z_prime)])
t[s] = t[s] + 1
t[(s, z_prime)] = t[(s, z_prime)] + 1
return cost
def edist_grid(fline, walls):
"""
This functions creates a grid to cache values in the graph. Walls and
unreachable nodes are stored as infinity. The function uses a BFS
to traverse the grid and find distance values to each node from the
finish line.
"""
global grid, g_fline, g_walls, xmax, ymax
xmax = max([max(x, x1) for ((x, y), (x1, y1)) in walls])
ymax = max([max(y, y1) for ((x, y), (x1, y1)) in walls])
visited = []
# initialize grid
grid = [[infinity for y in range(ymax + 1)] for x in range(xmax + 1)]
# get all reachable points from finish and mark as visited
for x in range(xmax + 1):
for y in range(ymax + 1):
grid[x][y] = edistw_to_finish((x, y), fline, walls)
if grid[x][y] != infinity:
visited.append((x, y))
queue = visited[:]
while queue:
(x, y) = queue.pop(0)
# for each neighbor of the first node in queue
for y1 in range(max(0, y - 1), min(ymax + 1, y + 2)):
for x1 in range(max(0, x - 1), min(xmax + 1, x + 2)):
# if a neighbor is not a wall and not visited
# add it to queue and mark as visited
# then update grid with new value for (x, y)
if not rt.crash(((x, y), (x1, y1)), walls):
if (x1, y1) not in visited:
queue.append((x1, y1))
visited.append((x1, y1))
if x == x1 or y == y1:
d = grid[x1][y1] + 1
else:
# In principle, it seems like a taxicab metric should be just as
# good, but Euclidean seems to work a little better in my tests.
d = grid[x1][y1] + 1.4142135623730951
if d < grid[x][y]:
grid[x][y] = d
flag = True
g_fline = fline
g_walls = walls
return grid
def h_h2(state,
fline,
walls,
metric='edist',
crash_aware=True,
fline_aware=True):
global g_metric, g_fline, g_walls
if fline != g_fline or walls != g_walls or metric != g_metric:
# if fline != g_fline: print('fline = ', fline, '; g_fline =', g_fline)
# if walls != g_walls: print('walls different')
# if metric != g_metric: print('metric = ', metric, '; g_metric =', g_metric)
if metric == 'edist':
edist_grid(fline, walls)
elif metric == 'xymax':
xymax_grid(fline, walls)
else:
raise RuntimeError("'" + metric + "' is not a known metric")
((x, y), (u, v)) = state
hval = float(grid[x][y])
if crash_aware or fline_aware:
penalty = 0
# compute stopping distance
au = abs(u)
av = abs(v)
sdu = au * (au - 1) / 2.0
sdv = av * (av - 1) / 2.0
sd = max(sdu, sdv)
# compute location after fastest stop
if u < 0: sdu = -sdu
if v < 0: sdv = -sdv
sx = x + sdu
sy = y + sdv
if crash_aware:
if racetrack.crash([(x, y), (sx, sy)], walls):
if metric == 'edist': penalty += math.sqrt(au**2 + av**2)
elif metric == 'xymax': penalty += max(au, av)
if fline_aware:
penalty += sd / 10.0
hval = max(hval + penalty, sd)
return hval
def edist_grid_1(fline, walls):
"""
This method stores the grid with straight line distance to finish line. This heurstic is used
when there exists a wall between start point to finish line.
:param fline:
:param walls:
:return: grid
"""
global grid, g_fline, g_walls, xmax, ymax
xmax = max([max(x, x1) for ((x, y), (x1, y1)) in walls])
ymax = max([max(y, y1) for ((x, y), (x1, y1)) in walls])
grid = [[edistw_to_finish((x, y), fline, walls) for y in range(ymax + 1)]
for x in range(xmax + 1)]
flag = True
print('computing edist grid_cross', end=' ')
sys.stdout.flush()
while flag:
print('.', end='')
sys.stdout.flush()
flag = False
for x in range(xmax + 1):
for y in range(ymax + 1):
for y1 in range(max(0, y - 1), min(ymax + 1, y + 2)):
for x1 in range(max(0, x - 1), min(xmax + 1, x + 2)):
if grid[x1][y1] != infinity and not rt.crash(
((x, y), (x1, y1)), walls):
if x == x1 or y == y1:
d = grid[x1][y1] + 1
else:
d = grid[x1][y1] + 1.4142135623730951
if d < grid[x][y]:
grid[x][y] = d
flag = True
print(' done')
g_fline = fline
g_walls = walls
return grid
def lao_star(s0, finish, walls):
""" LAO * algorithm """
Envelope.add(s0)
if not next_possible_states(s0, walls):
# starting at dead end
V[s0] = 100
print("What !!!! The starting point is a dead end!!!")
else:
V[s0] = heuristics.h_walldist(s0, finish, walls)
count = 0
init = False
while leaves_not_goal(s0, policy, finish):
old_policy = dict(policy)
s = leaves_not_goal(s0, policy, finish).pop()
if not next_possible_states(s, walls):
# dead end
V[s] = 100
for s1 in next_possible_states(s, walls) - Envelope:
Envelope.add(s1)
if racetrack.crash((s[0],s1[0]), walls):
V[s1] = 100
else:
V[s1] = heuristics.h_walldist(s1, finish, walls)
# lao star
lao_update(s0, s, finish, walls)
# ao_update(s, finish, walls)
if(init and old_policy[s0] == policy[s0]):
# the new policy doesn't change the action taken at state s0
count += 1
if count > 15:
print("Exit !!! ")
break
init = True
return policy
def proj1(state, fline, walls):
global g_fline, g_walls, best_pt, best_dis, ref_pts
((x, y), (u, v)) = state
#if we have a direct path to the finish, just return the distance, no need for extra computations
d = edistw_to_finish((x, y), fline, walls)
if d != infinity:
hval = float(d)
# there is an obstacle in between the state and finish line
else:
if fline != g_fline or walls != g_walls or ref_pts == []:
references(fline, walls)
hval = infinity
for (d1, x1, y1) in ref_pts:
hval = min(hval, d1 + dist((x1, y1), (x, y), walls))
ref_pts.append((hval, x, y))
# add a small penalty to favor short stopping distances
au = abs(u)
av = abs(v)
sdu = au * (au - 1) / 2.0
sdv = av * (av - 1) / 2.0
sd = max(sdu, sdv)
penalty = sd / 10.0
# compute location after fastest stop, and add a penalty if it goes through a wall
if u < 0: sdu = -sdu
if v < 0: sdv = -sdv
sx = x + sdu
sy = y + sdv
if racetrack.crash([(x, y), (sx, sy)], walls):
penalty += math.sqrt(au**2 + av**2)
hval = max(hval + penalty, sd)
return hval
def h_proj1(state, fline, walls):
"""
This first time this function called will test if there is a striaght line from start to
finish line. If straight line exists will use diagonal distance as heuristic. Otherwise will
use straight line distance as heuristic.
:param state:
:param fline:
:param walls:
:return: hval
"""
global g_fline, g_walls, first_time, start_x, start_y, reached_fline, straight
((x, y), (u, v)) = state
# find closest point from finish line to the current state
x_f, y_f = cloest_point((x, y), fline)
# calculate diagonal distance if no walls
if not rt.crash(((x, y), (x_f, y_f)), walls) and first_time:
edist_grid((x, y), fline, walls)
start_x = x
start_y = y
straight = True
first_time = False
# calculate straight line distance if wall exist
if fline != g_fline or walls != g_walls or grid == []:
edist_grid_1(fline, walls)
first_time = False
hval = float(grid[x][y])
# add a small penalty to favor short stopping distances
au = abs(u)
av = abs(v)
sdu = au * (au - 1) / 2.0
sdv = av * (av - 1) / 2.0
sd = max(sdu, sdv)
penalty = sd / 10.0
# compute location after fastest stop, and add a penalty if it goes through a wall
if u < 0: sdu = -sdu
if v < 0: sdv = -sdv
sx = x + sdu
sy = y + sdv
if rt.crash([(x, y), (sx, sy)], walls):
penalty += math.sqrt(au**2 + av**2)
hval = max(hval + penalty, sd)
if not straight:
# add reward to favor points that are closer to goal if not go through wall
sdist = edist_diagonal((x - u, y - v), fline)
tdist = edist_diagonal((x, y), fline)
reward = 0
if not rt.crash(((x, y), (x - u, y - v)), walls) and sdist > tdist:
reward = -math.sqrt(2)
hval += reward
if not reached_fline:
if rt.intersect(((x - u, y - v), (x, y)), fline):
reward = -math.sqrt(2)
hval += reward
elif straight: # use straight line if no walls in between
# tie breaking for multiple similar heuristics
cloest_x, cloest_y = cloest_point((x, y), fline)
dx1 = x - cloest_x
dy1 = y - cloest_y
dx2 = start_x - cloest_x
dy2 = start_y - cloest_y
cross = abs(dx1 * dy2 - dx2 * dy1)
hval += cross * 0.001
# if straight line reached goal expand other nodes will have penalty for early stopping
if not reached_fline:
if point_on_line((x - u, y - v), fline):
reached_fline = True
if reached_fline:
return hval + math.sqrt(2)
return hval
def main(state, f_line, walls):
"""
Uses a trained neural network to return the best velocity
value for the program to find a path to the finish line.
This function uses the A* algorithm to do this.
:param state: The current state
:param f_line: The finish line of the arena
:param walls: The walls in the arena
"""
# This is the placeholder variable for the feed-forward
inp = tf.placeholder(shape=[1, 16], dtype=tf.float32)
L = tf.Variable(tf.random_uniform([16, 4], 0, 0.01))
# Helps the neural network choose actions
out = tf.matmul(inp, L)
predict = tf.argmax(out, 1)
# Get loss
next = tf.placeholder(shape=[1, 4], dtype=tf.float32)
# Done by adding squares of next minus out
loss = tf.reduce_sum(tf.square(next - out))
neur = tf.train.GradientDescentOptimizer(learning_rate=0.2)
# Minimizes the loss of the new updated model
updateModel = neur.minimize(loss)
# Time to use tensorflow for training below (by session)
with tf.Session() as sess:
# init velocity
Z_dict = {}
# init global variables for tf sessions
sess.run(tf.global_variables_initializer())
# Iterate through state vals
i = 0
while i < len(racetrack.next_states(state, walls)):
elem = racetrack.next_states(state, walls)[i]
i += 1
(a, b) = elem
# Set that val to 0
Z_dict[b] = 0
# Iterate through state vals again but for finding paths
j = 0
while j < len(racetrack.next_states(state, walls)):
elem = racetrack.next_states(state, walls)[j]
j += 1
(a, b) = elem
# A* algorithm to find route
road = route(elem, f_line, walls)
numcrashes = 0
# To train network, punish crashes by decrementing pts
if road:
for point in road:
if (racetrack.crash(point, walls)):
numcrashes -= 1
# To train network, punish crashes and reward not crashes
if (not racetrack.crash(elem, walls)): Z_dict[b] += 2
else: Z_dict[b] -= 5
# Adjust score based on crashes
Z_dict[b] += numcrashes
# Use constant for finding the best value
const = -9999999
(u, v) = (0, 0)
# Get the best possible velocity by iterating through the elements in Z_dict
keys = Z_dict.keys()
for elem in keys:
# Get the respective velocity from the appropriate key
velocity = Z_dict[elem]
# If the curr const value is not the best
if const < velocity:
(u, v) = elem # this is the best value so far
const = velocity # higher const
# Return the best velocity value
return (u, v)
def corn(walls, fline):
'''
This function will cacluate points near all corners of the maps and estimate it's distance to
the finish line.
'''
global corners, s_corners, xmax, ymax, target_corner, stack
xmax = max([max(x, x1) for ((x, y), (x1, y1)) in walls])
ymax = max([max(y, y1) for ((x, y), (x1, y1)) in walls])
#adding all corners to a corners list
for wall in walls:
corners.add(wall[0])
corners.add(wall[1])
#add points to the left,right,up, and down of each corner point to s_corners list
for c in corners:
if not racetrack.crash(
[(c[0] + 1, c[1]), (c[0] + 1, c[1])], walls
) and c[0] + 1 > 0 and c[0] + 1 < xmax and c[1] > 0 and c[1] < ymax:
s_corners.append((c[0] + 1, c[1])) #right
if not racetrack.crash(
[(c[0] - 1, c[1]), (c[0] - 1, c[1])], walls
) and c[0] - 1 > 0 and c[0] - 1 < xmax and c[1] > 0 and c[1] < ymax:
s_corners.append((c[0] - 1, c[1])) #left
if not racetrack.crash(
[(c[0], c[1] + 1), (c[0], c[1] + 1)], walls
) and c[0] > 0 and c[0] < xmax and c[1] + 1 > 0 and c[1] + 1 < ymax:
s_corners.append((c[0], c[1] + 1)) #up
if not racetrack.crash(
[(c[0], c[1] - 1), (c[0], c[1] - 1)], walls
) and c[0] > 0 and c[0] < xmax and c[1] - 1 > 0 and c[1] - 1 < ymax:
s_corners.append((c[0], c[1] - 1)) #down
#Find closest point in s_corners that has straight line path to finish line
for corner in s_corners:
dist = heuristics.edistw_to_finish(corner, fline, walls)
if dist != infinity:
if not target_corner:
target_corner = (corner, dist)
elif dist < target_corner[1]:
target_corner = (corner, dist)
stack.append(target_corner)
if target_corner:
s_corners.remove(target_corner[0])
max_limit = len(s_corners) * len(s_corners)
i = 0
#Working backwards from closest point in s_corners, estimates all points distance to finish line
if s_corners:
while i < max_limit:
min_dist = infinity
min_corner = None
for corner in s_corners:
temp = None
dist = heuristics.edistw_to_finish(corner,
[stack[0][0], stack[0][0]],
walls)
if dist != infinity and dist != 0:
if dist < min_dist:
min_dist = dist
min_corner = corner
if min_corner:
stack.insert(0, (min_corner, min_dist + stack[0][1]))
s_corners.remove(min_corner)
i += 1
def bfs(fline, walls):
"""
Use a breath-first-search from the fline to compute costs for points on the grid
(combine edistw_to_finish and edist_grid into one time traversal of the nodes;
significantly reduce the runtime)
:param: fline, walls
:return: return the grid
"""
global grid, g_fline, g_walls, xmax, ymax
xmax = max([max(x, x0) for ((x, y), (x0, y0)) in walls])
ymax = max([max(y, y0) for ((x, y), (x0, y0)) in walls])
grid = [[infinity for i in range(ymax + 1)] for j in range(xmax + 1)]
((x1, y1), (x2, y2)) = fline
frontier = deque([])
visited = []
# set the points on fline as 0 in the grid
# and put all neighbors of the fline in the frontier
if x1 == x2:
for y3 in range(min(y1, y2), max(y1, y2) + 1):
grid[x1][y3] = 0
visited.append((x1, y3))
for y3 in range(min(y1, y2), max(y1, y2) + 1):
for n in range(max(0, y3 - 1), min(ymax + 1, y3 + 2)):
for m in range(max(0, x1 - 1), min(xmax + 1, x1 + 2)):
if frontier.count((m, n)) == 0 and grid[m][n] == infinity:
frontier.append((m, n))
else:
for x3 in range(min(x1, x2), max(x1, x2) + 1):
grid[x3][y1] = 0
visited.append((x3, y1))
for x3 in range(min(x1, x2), max(x1, x2) + 1):
for n in range(max(0, y1 - 1), min(ymax + 1, y1 + 2)):
for m in range(max(0, x3 - 1), min(xmax + 1, x3 + 2)):
if frontier.count((m, n)) == 0 and grid[m][n] == infinity:
frontier.append((m, n))
# use breath-first-search to compute the costs in the grid
while frontier:
(v1, v2) = frontier.popleft()
visited.append((v1, v2))
grid[v1][v2] = edistw_to_finish((v1, v2), fline, walls)
# check if edistw_to_finish is able to compute the cost
# if not, compute the cost using its non-infinity neighbors
if grid[v1][v2] == infinity:
for g in range(max(0, v1 - 1), min(xmax + 1, v1 + 2)):
for h in range(max(0, v2 - 1), min(ymax + 1, v2 + 2)):
if grid[g][h] != infinity and not racetrack.crash(
((v1, v2), (g, h)), walls):
if g == v1 or h == v2:
d = grid[g][h] + 1
else:
d = grid[g][h] + 1.4142135623730951
if d < grid[v1][v2]:
grid[v1][v2] = d
if grid[v1][v2] != infinity:
for i in range(max(0, v1 - 1), min(xmax + 1, v1 + 2)):
for j in range(max(0, v2 - 1), min(ymax + 1, v2 + 2)):
if frontier.count((i, j)) == 0 and visited.count(
(i, j)) == 0: ##and grid[i][j] == infinity:
frontier.append((i, j))
g_fline = fline
g_walls = walls
return grid
def h_proj1(state, fline, walls):
"""
The first time this function is called, it will use optimized breath first search to find the cost for all the
points on the grid.
On all subsequent calls, this function will retrieve the cached value and add an
estimate of how long it will take to stop.
:param a: state, fline, walls
:return: estimate cost of the step
"""
global g_fline, g_walls, find
# if it has already found a solution path, stop exploring other nodes
if find:
return infinity
if fline != g_fline or walls != g_walls or grid == []:
bfs(fline, walls)
((x, y), (u, v)) = state
((h1, w1), (h2, w2)) = fline
li = listfl(fline)
hval = float(grid[x][y])
au = abs(u)
av = abs(v)
sdu = au * (au - 1) / 2.0
sdv = av * (av - 1) / 2.0
sd = max(sdu, sdv)
if u < 0: sdu = -sdu
if v < 0: sdv = -sdv
sx = x + sdu
sy = y + sdv
# check if it has already found a solution path
# if yes, stop exploring other nodes
if li.count((x, y)) > 0 and u == 0 and v == 0:
find = True
return negainfinity
# add a small penalty to favor short stopping distances
penalty = sd / 10.0
# compute location after fastest stop, and add a penalty if it goes through a wall
if racetrack.crash([(x, y), (sx, sy)], walls):
penalty += math.sqrt(au**2 + av**2)
else:
penalty -= sd / 10.0
# compute the slowest stopping distance
ssu = (au - 2) * (au - 1) / 2.0
ssv = (av - 2) * (av - 1) / 2.0
if u < 0: ssu = -ssu
if v < 0: ssv = -ssv
ssx = x + ssu
ssy = y + ssv
if not racetrack.crash([(x, y), (ssx, ssy)], walls):
# check if the slowest stop point land on the fline
# if yes, significantly reduce the return cost
if li.count((ssx, ssy)) > 0:
return (-100) * (grid[x][y] + math.sqrt(ssu**2 + ssv**2))
# check if the slowest stopping x or y point is on the fline and reduce the return cost
elif li.count((h1, ssy)) > 0 or li.count((h2, ssy)) > 0 or li.count(
(ssx, w1)) > 0 or li.count((ssx, w2)) > 0:
penalty -= sd / 10.0
hval += penalty
return hval
