saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-29 08:34:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

'changed'

Browse Source
This commit is contained in:
yue qi
2020-07-09 00:46:50 -07:00
parent 3cbfb1eebf
commit e25dd1fc81
9 changed files with 151 additions and 176 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/Search_3D/Astar3D.py
+29 -32
View File
@@ -12,7 +12,7 @@ import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
from Search_3D.env3D import env
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
cost
from Search_3D.plot_util3D import visualization
import queue
@@ -27,21 +27,21 @@ class Weighted_A_star(object):
[1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 1]])
self.env = env(resolution=resolution)
self.Space = g_Space(self) # key is the point, store g value
self.start, self.goal = getNearest(self.Space, self.env.start), getNearest(self.Space, self.env.goal)
self.g = g_Space(self) # key is the point, store g value
self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
# self.AABB = getAABB(self.env.blocks)
self.Space[hash3D(getNearest(self.Space, self.start))] = 0 # set g(x0) = 0
self.g[getNearest(self.g, self.start)] = 0 # set g(x0) = 0
self.h = Heuristic(self.Space, self.goal)
self.h = Heuristic(self.g, self.goal)
self.Parent = {}
self.CLOSED = set()
self.V = []
self.done = False
self.Path = []
self.ind = 0
self.x0, self.xt = hash3D(self.start), hash3D(self.goal)
self.x0, self.xt = self.start, self.goal
self.OPEN = queue.QueuePrior() # store [point,priority]
self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0]) # item, priority = g + h
self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0]) # item, priority = g + h
self.lastpoint = self.x0
def children(self, x):
@@ -54,29 +54,27 @@ class Weighted_A_star(object):
def run(self, N=None):
xt = self.xt
strxi = self.x0
xi = self.x0
while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty
strxi = self.OPEN.get()
xi = dehash(strxi)
if strxi not in self.CLOSED:
self.V.append(xi)
self.CLOSED.add(strxi) # add the point in CLOSED set
xi = self.OPEN.get()
if xi not in self.CLOSED:
self.V.append(np.array(xi))
self.CLOSED.add(xi) # add the point in CLOSED set
visualization(self)
allchild = self.children(xi)
for xj in allchild:
strxj = hash3D(xj)
if strxj not in self.CLOSED:
gi, gj = self.Space[strxi], self.Space[strxj]
if xj not in self.CLOSED:
gi, gj = self.g[xi], self.g[xj]
a = gi + cost(xi, xj)
if a < gj:
self.Space[strxj] = a
self.Parent[strxj] = xi
if (a, strxj) in self.OPEN.enumerate():
self.g[xj] = a
self.Parent[xj] = xi
if (a, xj) in self.OPEN.enumerate():
# update priority of xj
self.OPEN.put(strxj, a + 1 * self.h[strxj])
self.OPEN.put(xj, a + 1 * self.h[xj])
else:
# add xj in to OPEN set
self.OPEN.put(strxj, a + 1 * self.h[strxj])
self.OPEN.put(xj, a + 1 * self.h[xj])
# For specified expanded nodes, used primarily in LRTA*
if N:
if len(self.CLOSED) % N == 0:
@@ -84,7 +82,7 @@ class Weighted_A_star(object):
if self.ind % 100 == 0: print('number node expanded = ' + str(len(self.V)))
self.ind += 1
self.lastpoint = strxi
self.lastpoint = xi
# if the path finding is finished
if xt in self.CLOSED:
self.done = True
@@ -98,22 +96,21 @@ class Weighted_A_star(object):
def path(self):
path = []
strx = self.lastpoint
# strstart = hash3D(getNearest(self.Space, self.env.start))
strstart = self.x0
while strx != strstart:
path.append([dehash(strx), self.Parent[strx]])
strx = hash3D(self.Parent[strx])
x = self.lastpoint
start = self.x0
while x != start:
path.append([x, self.Parent[x]])
x = self.Parent[x]
# path = np.flip(path, axis=0)
return path
# utility used in LRTA*
def reset(self, xj):
self.Space = g_Space(self) # key is the point, store g value
self.g = g_Space(self) # key is the point, store g value
self.start = xj
self.Space[hash3D(getNearest(self.Space, self.start))] = 0 # set g(x0) = 0
self.x0 = hash3D(xj)
self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0]) # item, priority = g + h
self.g[getNearest(self.g, self.start)] = 0 # set g(x0) = 0
self.x0 = xj
self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0]) # item, priority = g + h
self.CLOSED = set()
# self.h = h(self.Space, self.goal)
Search-based Planning/Search_3D/Dstar3D.py
-8
View File
@@ -12,14 +12,6 @@ from Search_3D.utils3D import StateSpace, getDist, getRay, isinbound, isinball
import pyrr
def getNearest(Space,pt):
'''get the nearest point on the grid'''
mindis,minpt = 1000,None
for pts in Space:
dis = getDist(pts,pt)
if dis < mindis:
mindis,minpt = dis,pts
return minpt
def isCollide(initparams, x, child):
'''see if line intersects obstacle'''
Search-based Planning/Search_3D/LP_Astar3D.py
+43 -48
View File
@@ -25,9 +25,9 @@ class Lifelong_Astar(object):
self.env = env(resolution=resolution)
self.g = g_Space(self)
self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
self.x0, self.xt = hash3D(self.start), hash3D(self.goal)
self.x0, self.xt = self.start, self.goal
self.v = g_Space(self) # rhs(.) = g(.) = inf
self.v[hash3D(self.start)] = 0 # rhs(x0) = 0
self.v[self.start] = 0 # rhs(x0) = 0
self.h = Heuristic(self.g, self.goal)
self.OPEN = queue.QueuePrior() # store [point,priority]
@@ -50,26 +50,25 @@ class Lifelong_Astar(object):
def costset(self):
NodeToChange = set()
for strxi in self.CHILDREN.keys():
children = self.CHILDREN[strxi]
xi = dehash(strxi)
for xi in self.CHILDREN.keys():
children = self.CHILDREN[xi]
toUpdate = [self.cost(xj,xi) for xj in children]
if strxi in self.COST:
if xi in self.COST:
# if the old cost not equal to new cost
diff = np.not_equal(self.COST[strxi],toUpdate)
diff = np.not_equal(self.COST[xi],toUpdate)
cd = np.array(children)[diff]
for i in cd:
NodeToChange.add(hash3D(i))
self.COST[strxi] = toUpdate
NodeToChange.add(tuple(i))
self.COST[xi] = toUpdate
else:
self.COST[strxi] = toUpdate
self.COST[xi] = toUpdate
return NodeToChange
def getCOSTset(self,strxi,xj):
ind, children = 0, self.CHILDREN[strxi]
def getCOSTset(self,xi,xj):
ind, children = 0, self.CHILDREN[xi]
for i in children:
if all(i == xj):
return self.COST[strxi][ind]
if i == xj:
return self.COST[xi][ind]
ind += 1
@@ -77,15 +76,14 @@ class Lifelong_Astar(object):
allchild = []
resolution = self.env.resolution
for direc in self.Alldirec:
child = np.array(list(map(np.add,x,np.multiply(direc,resolution))))
child = tuple(map(np.add,x,np.multiply(direc,resolution)))
if isinbound(self.env.boundary,child):
allchild.append(child)
return allchild
def getCHILDRENset(self):
for strxi in self.g.keys():
xi = dehash(strxi)
self.CHILDREN[strxi] = self.children(xi)
for xi in self.g.keys():
self.CHILDREN[xi] = self.children(xi)
def isCollide(self, x, child):
ray , dist = getRay(x, child) , getDist(x, child)
@@ -112,56 +110,54 @@ class Lifelong_Astar(object):
if collide: return np.inf
else: return dist
def key(self,strxi,epsilion = 1):
return [min(self.g[strxi],self.v[strxi]) + epsilion*self.h[strxi],min(self.g[strxi],self.v[strxi])]
def key(self,xi,epsilion = 1):
return [min(self.g[xi],self.v[xi]) + epsilion*self.h[xi],min(self.g[xi],self.v[xi])]
def path(self):
path = []
strx = self.xt
strstart = self.x0
x = self.xt
start = self.x0
ind = 0
while strx != strstart:
j = dehash(strx)
nei = self.CHILDREN[strx]
gset = [self.g[hash3D(xi)] for xi in nei]
while x != start:
j = x
nei = self.CHILDREN[x]
gset = [self.g[xi] for xi in nei]
# collision check and make g cost inf
for i in range(len(nei)):
if self.isCollide(nei[i],j)[0]:
gset[i] = np.inf
parent = nei[np.argmin(gset)]
path.append([dehash(strx), parent])
strx = hash3D(parent)
path.append([x, parent])
x = parent
if ind > 100:
break
ind += 1
return path
#------------------Lifelong Plannning A*
def UpdateMembership(self,strxi, xi, xparent=None):
if strxi != self.x0:
self.v[strxi] = min([self.g[hash3D(j)] + self.getCOSTset(strxi,j) for j in self.CHILDREN[strxi]])
self.OPEN.check_remove(strxi)
if self.g[strxi] != self.v[strxi]:
self.OPEN.put(strxi,self.key(strxi))
def UpdateMembership(self, xi, xparent=None):
if xi != self.x0:
self.v[xi] = min([self.g[j] + self.getCOSTset(xi,j) for j in self.CHILDREN[xi]])
self.OPEN.check_remove(xi)
if self.g[xi] != self.v[xi]:
self.OPEN.put(xi,self.key(xi))
def ComputePath(self):
print('computing path ...')
while self.key(self.xt) > self.OPEN.top_key() or self.v[self.xt] != self.g[self.xt]:
strxi = self.OPEN.get()
xi = dehash(strxi)
xi = self.OPEN.get()
# if g > rhs, overconsistent
if self.g[strxi] > self.v[strxi]:
self.g[strxi] = self.v[strxi]
if self.g[xi] > self.v[xi]:
self.g[xi] = self.v[xi]
# add xi to expanded node set
if strxi not in self.CLOSED:
if xi not in self.CLOSED:
self.V.append(xi)
self.CLOSED.add(strxi)
self.CLOSED.add(xi)
else: # underconsistent and consistent
self.g[strxi] = np.inf
self.UpdateMembership(strxi, xi)
for xj in self.CHILDREN[strxi]:
strxj = hash3D(xj)
self.UpdateMembership(strxj, xj)
self.g[xi] = np.inf
self.UpdateMembership(xi)
for xj in self.CHILDREN[xi]:
self.UpdateMembership(xj)
# visualization(self)
self.ind += 1
@@ -176,9 +172,8 @@ class Lifelong_Astar(object):
self.Path = []
self.CLOSED = set()
N = self.costset()
for strxi in N:
xi = dehash(strxi)
self.UpdateMembership(strxi,xi)
for xi in N:
self.UpdateMembership(xi)
if __name__ == '__main__':
sta = time.time()
Search-based Planning/Search_3D/LRT_Astar3D.py
+17 -20
View File
@@ -13,7 +13,7 @@ import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
from Search_3D.env3D import env
from Search_3D import Astar3D
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
cost, obstacleFree
from Search_3D.plot_util3D import visualization
import queue
@@ -26,40 +26,37 @@ class LRT_A_star2:
def updateHeuristic(self):
# Initialize hvalues at infinity
for strxi in self.Astar.CLOSED:
self.Astar.h[strxi] = np.inf
for xi in self.Astar.CLOSED:
self.Astar.h[xi] = np.inf
Diff = True
while Diff: # repeat DP until converge
hvals, lasthvals = [], []
for strxi in self.Astar.CLOSED:
xi = dehash(strxi)
lasthvals.append(self.Astar.h[strxi])
for xi in self.Astar.CLOSED:
lasthvals.append(self.Astar.h[xi])
# update h values if they are smaller
Children = self.Astar.children(xi)
minfval = min([cost(xi, xj, settings=0) + self.Astar.h[hash3D(xj)] for xj in Children])
minfval = min([cost(xi, xj, settings=0) + self.Astar.h[xj] for xj in Children])
# h(s) = h(s') if h(s) > c(s,s') + h(s')
if self.Astar.h[strxi] >= minfval:
self.Astar.h[strxi] = minfval
hvals.append(self.Astar.h[strxi])
if self.Astar.h[xi] >= minfval:
self.Astar.h[xi] = minfval
hvals.append(self.Astar.h[xi])
if lasthvals == hvals: Diff = False
def move(self):
strst = self.Astar.x0
st = self.Astar.start
st = self.Astar.x0
ind = 0
# find the lowest path down hill
while strst in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop
# strChildren = self.children(st)
strChildren = [hash3D(i) for i in self.Astar.children(st)]
while st in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop
Children = [i for i in self.Astar.children(st)]
minh, minchild = np.inf, None
for child in strChildren:
for child in Children:
h = self.Astar.h[child]
if h <= minh:
minh, minchild = h, dehash(child)
minh, minchild = h, child
self.path.append([st, minchild])
strst, st = hash3D(minchild), minchild
for (_, strp) in self.Astar.OPEN.enumerate():
if strp == strst:
st = minchild
for (_, p) in self.Astar.OPEN.enumerate():
if p == st:
break
ind += 1
if ind > 1000:
Search-based Planning/Search_3D/RTA_Astar3D.py
+18 -21
View File
@@ -13,7 +13,7 @@ import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
from Search_3D.env3D import env
from Search_3D import Astar3D
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \
cost, obstacleFree
from Search_3D.plot_util3D import visualization
import queue
@@ -23,43 +23,40 @@ class RTA_A_star:
self.N = N # node to expand
self.Astar = Astar3D.Weighted_A_star(resolution=resolution) # initialize A star
self.path = [] # empty path
self.strst = []
self.st = []
self.localhvals = []
def updateHeuristic(self):
# Initialize hvalues at infinity
self.localhvals = []
nodeset, vals = [], []
for (_,strxi) in self.Astar.OPEN.enumerate():
nodeset.append(strxi)
vals.append(self.Astar.Space[strxi] + self.Astar.h[strxi])
strj, fj = nodeset[np.argmin(vals)], min(vals)
self.strst = strj
for (_,xi) in self.Astar.OPEN.enumerate():
nodeset.append(xi)
vals.append(self.Astar.g[xi] + self.Astar.h[xi])
j, fj = nodeset[np.argmin(vals)], min(vals)
self.st = j
# single pass update of hvals
for strxi in self.Astar.CLOSED:
# xi = dehash(strxi)
self.Astar.h[strxi] = fj - self.Astar.Space[strxi]
self.localhvals.append(self.Astar.h[strxi])
for xi in self.Astar.CLOSED:
self.Astar.h[xi] = fj - self.Astar.g[xi]
self.localhvals.append(self.Astar.h[xi])
def move(self):
strst, localhvals = self.strst, self.localhvals
st, localhvals = self.st, self.localhvals
maxhval = max(localhvals)
st = dehash(strst)
sthval = self.Astar.h[strst]
sthval = self.Astar.h[st]
# find the lowest path up hill
while sthval < maxhval:
parentsvals , parents = [] , []
# find the max child
for xi in self.Astar.children(st):
strxi = hash3D(xi)
if strxi in self.Astar.CLOSED:
if xi in self.Astar.CLOSED:
parents.append(xi)
parentsvals.append(self.Astar.h[strxi])
parentsvals.append(self.Astar.h[xi])
lastst = st
st, strst = parents[np.argmax(parentsvals)], hash3D(st)
st = parents[np.argmax(parentsvals)]
self.path.append([st,lastst]) # add to path
sthval = self.Astar.h[strst]
self.Astar.reset(dehash(self.strst))
sthval = self.Astar.h[st]
self.Astar.reset(self.st)
def run(self):
while True:
@@ -74,5 +71,5 @@ class RTA_A_star:
if __name__ == '__main__':
T = RTA_A_star(resolution=0.5, N=100)
T = RTA_A_star(resolution=1, N=100)
T.run()
Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_3D/bidirectional_Astar3D.py
+40 -42
View File
@@ -13,7 +13,7 @@ import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
from Search_3D.env3D import env
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, cost
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, cost
from Search_3D.plot_util3D import visualization
import queue
@@ -25,14 +25,14 @@ class Weighted_A_star(object):
[1,-1,0],[-1,1,0],[1,0,-1],[-1,0, 1],[0,1, -1],[0, -1,1],\
[1,-1,-1],[-1,1,-1],[-1,-1,1],[1,1,-1],[1,-1,1],[-1,1,1]])
self.env = env(resolution = resolution)
self.Space = g_Space(self) # key is the point, store g value
self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
self.Space[hash3D(self.start)] = 0 # set g(x0) = 0
self.Space[hash3D(self.goal)] = 0 # set g(x0) = 0
self.g = g_Space(self) # key is the point, store g value
self.start, self.goal = getNearest(self.g,self.env.start), getNearest(self.g,self.env.goal)
self.g[self.start] = 0 # set g(x0) = 0
self.g[self.goal] = 0 # set g(x0) = 0
self.OPEN1 = queue.QueuePrior() # store [point,priority]
self.OPEN2 = queue.QueuePrior()
self.h1 = Heuristic(self.Space,self.goal) # tree NO.1
self.h2 = Heuristic(self.Space,self.start) # tree NO.2
self.h1 = Heuristic(self.g,self.goal) # tree NO.1
self.h2 = Heuristic(self.g,self.start) # tree NO.2
self.Parent1, self.Parent2 = {}, {}
self.CLOSED1, self.CLOSED2 = set(), set()
self.V = []
@@ -48,21 +48,20 @@ class Weighted_A_star(object):
return allchild
def run(self):
x0, xt = hash3D(self.start), hash3D(self.goal)
self.OPEN1.put(x0, self.Space[x0] + self.h1[x0]) # item, priority = g + h
self.OPEN2.put(xt, self.Space[xt] + self.h2[xt]) # item, priority = g + h
x0, xt = self.start, self.goal
self.OPEN1.put(x0, self.g[x0] + self.h1[x0]) # item, priority = g + h
self.OPEN2.put(xt, self.g[xt] + self.h2[xt]) # item, priority = g + h
self.ind = 0
while not self.CLOSED1.intersection(self.CLOSED2): # while xt not reached and open is not empty
strxi1, strxi2 = self.OPEN1.get(), self.OPEN2.get()
xi1, xi2 = dehash(strxi1), dehash(strxi2)
self.CLOSED1.add(strxi1) # add the point in CLOSED set
self.CLOSED2.add(strxi2)
xi1, xi2 = self.OPEN1.get(), self.OPEN2.get()
self.CLOSED1.add(xi1) # add the point in CLOSED set
self.CLOSED2.add(xi2)
self.V.append(xi1)
self.V.append(xi2)
visualization(self)
allchild1, allchild2 = self.children(xi1), self.children(xi2)
self.evaluation(allchild1,strxi1,xi1,conf=1)
self.evaluation(allchild2,strxi2,xi2,conf=2)
self.evaluation(allchild1,xi1,conf=1)
self.evaluation(allchild2,xi2,conf=2)
if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
self.ind += 1
self.common = self.CLOSED1.intersection(self.CLOSED2)
@@ -71,45 +70,44 @@ class Weighted_A_star(object):
visualization(self)
plt.show()
def evaluation(self, allchild, strxi, xi, conf):
def evaluation(self, allchild, xi, conf):
for xj in allchild:
strxj = hash3D(xj)
if conf == 1:
if strxj not in self.CLOSED1:
gi, gj = self.Space[strxi], self.Space[strxj]
if xj not in self.CLOSED1:
gi, gj = self.g[xi], self.g[xj]
a = gi + cost(xi,xj)
if a < gj:
self.Space[strxj] = a
self.Parent1[strxj] = xi
if (a, strxj) in self.OPEN1.enumerate():
self.OPEN1.put(strxj, a+1*self.h1[strxj])
self.g[xj] = a
self.Parent1[xj] = xi
if (a, xj) in self.OPEN1.enumerate():
self.OPEN1.put(xj, a+1*self.h1[xj])
else:
self.OPEN1.put(strxj, a+1*self.h1[strxj])
self.OPEN1.put(xj, a+1*self.h1[xj])
if conf == 2:
if strxj not in self.CLOSED2:
gi, gj = self.Space[strxi], self.Space[strxj]
if xj not in self.CLOSED2:
gi, gj = self.g[xi], self.g[xj]
a = gi + cost(xi,xj)
if a < gj:
self.Space[strxj] = a
self.Parent2[strxj] = xi
if (a, strxj) in self.OPEN2.enumerate():
self.OPEN2.put(strxj, a+1*self.h2[strxj])
self.g[xj] = a
self.Parent2[xj] = xi
if (a, xj) in self.OPEN2.enumerate():
self.OPEN2.put(xj, a+1*self.h2[xj])
else:
self.OPEN2.put(strxj, a+1*self.h2[strxj])
self.OPEN2.put(xj, a+1*self.h2[xj])
def path(self):
# TODO: fix path
path = []
strgoal = hash3D(self.goal)
strstart = hash3D(self.start)
strx = list(self.common)[0]
while strx != strstart:
path.append([dehash(strx),self.Parent1[strx]])
strx = hash3D(self.Parent1[strx])
strx = list(self.common)[0]
while strx != strgoal:
path.append([dehash(strx),self.Parent2[strx]])
strx = hash3D(self.Parent2[strx])
goal = self.goal
start = self.start
x = list(self.common)[0]
while x != start:
path.append([x,self.Parent1[x]])
x = self.Parent1[x]
x = list(self.common)[0]
while x != goal:
path.append([x,self.Parent2[x]])
x = self.Parent2[x]
path = np.flip(path,axis=0)
return path
Search-based Planning/Search_3D/utils3D.py
+4 -5
View File
@@ -14,8 +14,7 @@ def getManDist(pos1, pos2):
def getNearest(Space,pt):
'''get the nearest point on the grid'''
mindis,minpt = 1000,None
for strpts in Space.keys():
pts = dehash(strpts)
for pts in Space:
dis = getDist(pts,pt)
if dis < mindis:
mindis,minpt = dis,pts
@@ -25,7 +24,7 @@ def Heuristic(Space,t):
'''Max norm distance'''
h = {}
for k in Space.keys():
h[k] = max(abs(t-dehash(k)))
h[k] = max(abs(np.array([t[0]-k[0],t[1]-k[1],t[2]-k[2]])))
return h
def hash3D(x):
@@ -66,13 +65,13 @@ def g_Space(initparams):
g = {}
Space = StateSpace(initparams.env)
for v in Space:
g[hash3D(v)] = np.inf # this hashmap initialize all g values at inf
g[v] = np.inf # this hashmap initialize all g values at inf
return g
def isCollide(initparams, x, direc):
'''see if line intersects obstacle'''
resolution = initparams.env.resolution
child = np.array(list(map(np.add,x,np.multiply(direc,resolution))))
child = tuple(map(np.add,x,np.multiply(direc,resolution)))
ray , dist = getRay(x, child) , getDist(x, child)
if not isinbound(initparams.env.boundary,child):
return True, child