diff --git a/Search-based Planning/Astar_3D/Astar3D.py b/Search-based Planning/Astar_3D/Astar3D.py deleted file mode 100644 index ef72dd7..0000000 --- a/Search-based Planning/Astar_3D/Astar3D.py +++ /dev/null @@ -1,38 +0,0 @@ -# this is the three dimensional A* algo -# !/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: yue qi -""" -import numpy as np - -import os -import sys - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") -from Astar_3D.env3D import env -from Astar_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest -import queue - - -class Weighted_A_star(object): - def __init__(self): - self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\ - [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\ - [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() - self.Space = StateSpace(self.env.boundary) # key is the point, store g value - self.OPEN = queue.QueuePrior() # store [point,priority] - self.start = getNearest(self.Space,self.env.start) - self.goal = getNearest(self.Space,self.env.goal) - self.h = Heuristic(self.Space,self.goal) - self.Parent = {} - self.CLOSED = {} - - - def run(self): - pass -if __name__ == '__main__': - Astar = Weighted_A_star() - \ No newline at end of file diff --git a/Search-based Planning/Astar_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Astar_3D/__pycache__/env3D.cpython-37.pyc deleted file mode 100644 index a3ab9d0..0000000 Binary files a/Search-based Planning/Astar_3D/__pycache__/env3D.cpython-37.pyc and /dev/null differ diff --git a/Search-based Planning/Astar_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Astar_3D/__pycache__/utils3D.cpython-37.pyc deleted file mode 100644 index aae19a2..0000000 Binary files a/Search-based Planning/Astar_3D/__pycache__/utils3D.cpython-37.pyc and /dev/null differ diff --git a/Search-based Planning/Astar_3D/utils3D.py b/Search-based Planning/Astar_3D/utils3D.py deleted file mode 100644 index 5db2fd9..0000000 --- a/Search-based Planning/Astar_3D/utils3D.py +++ /dev/null @@ -1,66 +0,0 @@ -import numpy as np - -def getRay(x, y): - direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] - return np.array([x, direc]) - -def getAABB(blocks): - AABB = [] - for i in blocks: - AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger - return AABB - -def getDist(pos1, pos2): - return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) - -def getNearest(Space,pt): - '''get the nearest point on the grid''' - mindis,minpt = 1000,None - for strpts in Space.keys(): - pts = dehash(strpts) - dis = getDist(pts,pt) - if dis < mindis: - mindis,minpt = dis,pts - return minpt - -def Heuristic(Space,t): - '''Max norm distance''' - h = {} - for k in Space.keys(): - h[k] = max(abs(t-dehash(k))) - return h - -def hash3D(x): - return str(x[0])+' '+str(x[1])+' '+str(x[2]) - -def dehash(x): - return np.array([float(i) for i in x.split(' ')]) - -def isinbound(i, x): - if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: - return True - return False - -def StateSpace(boundary,factor=0): - '''This function is used to get nodes and discretize the space. - State space is by x*y*z,3 where each 3 is a point in 3D.''' - xmin,xmax = boundary[0]+factor,boundary[3]-factor - ymin,ymax = boundary[1]+factor,boundary[4]-factor - zmin,zmax = boundary[2]+factor,boundary[5]-factor - xarr = np.arange(xmin,xmax,1) - yarr = np.arange(ymin,ymax,1) - zarr = np.arange(zmin,zmax,1) - V = np.meshgrid(xarr,yarr,zarr) - VV = np.reshape(V,[3,len(xarr)*len(yarr)*len(zarr)]) # all points in 3D - Space = {} - for v in VV.T: - Space[hash3D(v)] = 0 # this hashmap initialize all g values at 0 - return Space - -if __name__ == "__main__": - from env3D import env - env = env(resolution=1) - Space = StateSpace(env.boundary,0) - t = np.array([3.0,4.0,5.0]) - h = Heuristic(Space,t) - print(h[hash3D(t)]) \ No newline at end of file diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py new file mode 100644 index 0000000..c83db3a --- /dev/null +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -0,0 +1,94 @@ +# this is the three dimensional A* algo +# !/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: yue qi +""" +import numpy as np + +import os +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 getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost +from Search_3D.plot_util3D import visualization +import queue + + +class Weighted_A_star(object): + def __init__(self,resolution=0.2): + self.Alldirec = np.array([[1 ,0,0],[0,1 ,0],[0,0, 1],[1 ,1 ,0],[1 ,0,1 ],[0, 1, 1],[ 1, 1, 1],\ + [-1,0,0],[0,-1,0],[0,0,-1],[-1,-1,0],[-1,0,-1],[0,-1,-1],[-1,-1,-1],\ + [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 = StateSpace(self) # key is the point, store g value + self.start = getNearest(self.Space,self.env.start) + self.goal = getNearest(self.Space,self.env.goal) + self.Space[hash3D(getNearest(self.Space,self.start))] = 0 # set g(x0) = 0 + self.OPEN = queue.QueuePrior() # store [point,priority] + self.h = Heuristic(self.Space,self.goal) + self.Parent = {} + self.CLOSED = {} + self.V = [] + self.done = False + self.Path = [] + + def children(self,x): + allchild = [] + for j in self.Alldirec: + collide,child = isCollide(self,x,j) + if not collide: + allchild.append(child) + return allchild + + def run(self): + x0 = hash3D(self.start) + xt = hash3D(self.goal) + self.OPEN.put(x0,self.Space[x0] + self.h[x0]) # item, priority = g + h + self.ind = 0 + 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) + self.V.append(xi) + visualization(self) + self.CLOSED[strxi] = [] # add the point in CLOSED set + 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] + a = gi + cost(xi,xj) + if a < gj: + self.Space[strxj] = a + self.Parent[strxj] = xi + if strxj in self.OPEN.enumerate(): + #TODO: update priority of xj + # self.OPEN.put(strxj, a+1*self.h[strxj]) + pass + else: + #TODO: add xj in to OPEN set + self.OPEN.put(strxj, a+1*self.h[strxj]) + if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) + self.ind += 1 + self.done = True + #self.Path = self.path() + #visualization(self) + + def path(self): + path = [self.goal] + strx = hash3D(self.goal) + strstart = hash3D(self.start) + while strx != strstart: + path.append(self.Parent[strx]) + strx = hash3D(self.Parent[strx]) + path = np.array(np.flip(path,axis=0)) + return path + + +if __name__ == '__main__': + Astar = Weighted_A_star(1) + Astar.run() + PATH = Astar.path() + print(PATH) \ No newline at end of file diff --git a/Search-based Planning/Astar_3D/__pycache__/Astar3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc similarity index 100% rename from Search-based Planning/Astar_3D/__pycache__/Astar3D.cpython-37.pyc rename to Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc new file mode 100644 index 0000000..73e363c Binary files /dev/null and b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc new file mode 100644 index 0000000..637b3cd Binary files /dev/null and b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Search-based Planning/Astar_3D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc similarity index 100% rename from Search-based Planning/Astar_3D/__pycache__/queue.cpython-37.pyc rename to Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc new file mode 100644 index 0000000..8a0c0df Binary files /dev/null and b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Search-based Planning/Astar_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py similarity index 81% rename from Search-based Planning/Astar_3D/env3D.py rename to Search-based Planning/Search_3D/env3D.py index 977aa8d..d71b524 100644 --- a/Search-based Planning/Astar_3D/env3D.py +++ b/Search-based Planning/Search_3D/env3D.py @@ -7,7 +7,7 @@ import numpy as np -def getblocks(resolution): +def getblocks(): # AABBs block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00], [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00], @@ -19,22 +19,22 @@ def getblocks(resolution): Obstacles = [] for i in block: i = np.array(i) - Obstacles.append([j/resolution for j in i]) + Obstacles.append([j for j in i]) return np.array(Obstacles) -def getballs(resolution): +def getballs(): spheres = [[16,2.5,3,2],[10,2.5,1,1]] Obstacles = [] for i in spheres: - Obstacles.append([j/resolution for j in i]) + Obstacles.append([j for j in i]) return np.array(Obstacles) class env(): def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): self.resolution = resolution - self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) / resolution - self.blocks = getblocks(resolution) - self.balls = getballs(resolution) + self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) + self.blocks = getblocks() + self.balls = getballs() self.start = np.array([0.5, 2.5, 5.5]) self.goal = np.array([19.0, 2.5, 5.5]) diff --git a/Search-based Planning/Astar_3D/plot_util3D.py b/Search-based Planning/Search_3D/plot_util3D.py similarity index 95% rename from Search-based Planning/Astar_3D/plot_util3D.py rename to Search-based Planning/Search_3D/plot_util3D.py index b164fd6..0da691c 100644 --- a/Search-based Planning/Astar_3D/plot_util3D.py +++ b/Search-based Planning/Search_3D/plot_util3D.py @@ -54,11 +54,11 @@ def draw_line(ax,SET,visibility=1,color=None): def visualization(initparams): if initparams.ind % 10 == 0 or initparams.done: V = np.array(initparams.V) - E = initparams.E + # E = initparams.E Path = np.array(initparams.Path) start = initparams.env.start goal = initparams.env.goal - edges = E.get_edge() + # edges = E.get_edge() # generate axis objects ax = plt.subplot(111, projection='3d') ax.view_init(elev=0., azim=90) @@ -67,7 +67,7 @@ def visualization(initparams): draw_Spheres(ax, initparams.env.balls) draw_block_list(ax, initparams.env.blocks) draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0) - draw_line(ax,edges,visibility=0.25) + # draw_line(ax,edges,visibility=0.25) draw_line(ax,Path,color='r') ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',) ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') diff --git a/Search-based Planning/Astar_3D/queue.py b/Search-based Planning/Search_3D/queue.py similarity index 100% rename from Search-based Planning/Astar_3D/queue.py rename to Search-based Planning/Search_3D/queue.py diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py new file mode 100644 index 0000000..d34aaa2 --- /dev/null +++ b/Search-based Planning/Search_3D/utils3D.py @@ -0,0 +1,90 @@ +import numpy as np +import pyrr + +def getRay(x, y): + direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] + return np.array([x, direc]) + +def getAABB(blocks): + AABB = [] + for i in blocks: + AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger + return AABB + +def getDist(pos1, pos2): + return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) + +def getNearest(Space,pt): + '''get the nearest point on the grid''' + mindis,minpt = 1000,None + for strpts in Space.keys(): + pts = dehash(strpts) + dis = getDist(pts,pt) + if dis < mindis: + mindis,minpt = dis,pts + return minpt + +def Heuristic(Space,t): + '''Max norm distance''' + h = {} + for k in Space.keys(): + h[k] = max(abs(t-dehash(k))) + return h + +def hash3D(x): + return str(x[0])+' '+str(x[1])+' '+str(x[2]) + +def dehash(x): + return np.array([float(i) for i in x.split(' ')]) + +def isinbound(i, x): + if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: + return True + return False + +def StateSpace(initparams,factor=0): + '''This function is used to get nodes and discretize the space. + State space is by x*y*z,3 where each 3 is a point in 3D.''' + boundary = initparams.env.boundary + resolution = initparams.env.resolution + xmin,xmax = boundary[0]+factor*resolution,boundary[3]-factor*resolution + ymin,ymax = boundary[1]+factor*resolution,boundary[4]-factor*resolution + zmin,zmax = boundary[2]+factor*resolution,boundary[5]-factor*resolution + xarr = np.arange(xmin,xmax,resolution).astype(float) + yarr = np.arange(ymin,ymax,resolution).astype(float) + zarr = np.arange(zmin,zmax,resolution).astype(float) + V = np.meshgrid(xarr,yarr,zarr) + VV = np.reshape(V,[3,len(xarr)*len(yarr)*len(zarr)]) # all points in 3D + Space = {} + for v in VV.T: + Space[hash3D(v)] = np.inf # this hashmap initialize all g values at inf + return Space + +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)))) + ray = getRay(x, direc) + dist = getDist(x, child) + if not isinbound(initparams.env.boundary,child): + return True, child + for i in getAABB(initparams.env.blocks): + shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) + if shot is not None: + dist_wall = getDist(x, shot) + if dist_wall <= dist: # collide + return True, child + for i in initparams.env.balls: + shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i) + if shot != []: + dists_ball = [getDist(x, j) for j in shot] + if all(dists_ball <= dist): # collide + return True, child + return False, child + +def cost(i,j): + return getDist(i,j) + + +if __name__ == "__main__": + from env3D import env \ No newline at end of file