mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-30 09:00:47 +08:00
yq edited A*3D
This commit is contained in:
yue qi
@@ -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()
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -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)])
|
||||
@@ -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)
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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])
|
||||
|
||||
+3
-3
@@ -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')
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user