yq edited A*3D

Search-based Planning/Astar_3D/Astar3D.py

@@ -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()
-    

Search-based Planning/Astar_3D/utils3D.py

@@ -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)])

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)

Search-based Planning/Astar_3D/env3D.py → 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])
 

Search-based Planning/Astar_3D/plot_util3D.py → 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')

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