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

update 3D

Browse Source
This commit is contained in:
zhm-real
2020-06-29 12:49:24 -07:00
parent 39f5fa7ae7
commit 4da4112cde
9 changed files with 61 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/.idea/workspace.xml
Generated
+14 -13
View File
@@ -20,9 +20,10 @@
</component>
<component name="ChangeListManager">
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
<change afterPath="$PROJECT_DIR$/Search_2D/LPAstar.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_2D/astar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/astar.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_2D/LPAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LPAstar.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_3D/Astar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/Astar3D.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" afterDir="false" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
@@ -68,7 +69,7 @@
</list>
</option>
</component>
<component name="RunManager" selected="Python.astar">
<component name="RunManager" selected="Python.LRT_Astar3D">
<configuration name="ARAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
@@ -90,7 +91,7 @@
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="LRTA_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
<configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
@@ -98,11 +99,11 @@
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
<option name="IS_MODULE_SDK" value="true" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<option name="SCRIPT_NAME" value="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D\LRTAstar.py" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/Astar3D.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
@@ -111,7 +112,7 @@
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="LRTAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
<configuration name="LRT_Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
@@ -119,11 +120,11 @@
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
<option name="IS_MODULE_SDK" value="true" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LRTAstar.py" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
@@ -197,19 +198,19 @@
</configuration>
<list>
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.LRTA_star" />
<item itemvalue="Python.LRTAstar" />
<item itemvalue="Python.RTAAstar" />
<item itemvalue="Python.ARAstar" />
<item itemvalue="Python.astar" />
<item itemvalue="Python.LRT_Astar3D" />
<item itemvalue="Python.Astar3D" />
</list>
<recent_temporary>
<list>
<item itemvalue="Python.LRT_Astar3D" />
<item itemvalue="Python.Astar3D" />
<item itemvalue="Python.astar" />
<item itemvalue="Python.ARAstar" />
<item itemvalue="Python.RTAAstar" />
<item itemvalue="Python.LRTAstar" />
<item itemvalue="Python.LRTA_star" />
</list>
</recent_temporary>
</component>
Search-based Planning/Search_2D/LPAstar.py
+1
View File
@@ -15,3 +15,4 @@ from Search_2D import env
class LpaStar:
def __init__(self):
return
Search-based Planning/Search_3D/Astar3D.py
+35 -28
View File
@@ -12,46 +12,51 @@ 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.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.5):
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, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
def __init__(self, resolution=1):
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, self.goal = getNearest(self.Space, self.env.start), getNearest(self.Space, self.env.goal)
self.AABB = getAABB(self.env.blocks)
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.Space[hash3D(getNearest(self.Space, self.start))] = 0 # set g(x0) = 0
self.h = Heuristic(self.Space, 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.OPEN = queue.QueuePrior() # store [point,priority]
self.OPEN.put(self.x0, self.Space[self.x0] + self.h[self.x0]) # item, priority = g + h
def children(self,x):
def children(self, x):
allchild = []
for j in self.Alldirec:
collide,child = isCollide(self,x,j)
collide, child = isCollide(self, x, j)
if not collide:
allchild.append(child)
return allchild
def run(self, N=None):
x0, xt = hash3D(self.start), hash3D(self.goal)
self.OPEN.put(x0, self.Space[x0] + self.h[x0]) # item, priority = g + h
while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty
strxi = self.OPEN.get()
xt = self.xt
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.CLOSED.add(strxi) # add the point in CLOSED set
self.CLOSED.add(strxi) # add the point in CLOSED set
self.V.append(xi)
visualization(self)
allchild = self.children(xi)
@@ -59,22 +64,23 @@ class Weighted_A_star(object):
strxj = hash3D(xj)
if strxj not in self.CLOSED:
gi, gj = self.Space[strxi], self.Space[strxj]
a = gi + cost(xi,xj)
a = gi + cost(xi, xj)
if a < gj:
self.Space[strxj] = a
self.Parent[strxj] = xi
if (a, strxj) in self.OPEN.enumerate():
# update priority of xj
self.OPEN.put(strxj, a+1*self.h[strxj])
self.OPEN.put(strxj, a + 1 * self.h[strxj])
else:
# add xj in to OPEN set
self.OPEN.put(strxj, a+1*self.h[strxj])
self.OPEN.put(strxj, a + 1 * self.h[strxj])
# For specified expanded nodes, used primarily in LRTA*
if N is not None:
if len(self.V) % N == 0:
if N:
if len(self.CLOSED) % N == 0:
break
if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
if self.ind % 100 == 0: print('iteration number = ' + str(self.ind))
self.ind += 1
# if the path finding is finished
if xt in self.CLOSED:
self.done = True
@@ -87,11 +93,12 @@ class Weighted_A_star(object):
strx = hash3D(self.goal)
strstart = hash3D(self.start)
while strx != strstart:
path.append([dehash(strx),self.Parent[strx]])
path.append([dehash(strx), self.Parent[strx]])
strx = hash3D(self.Parent[strx])
path = np.flip(path,axis=0)
path = np.flip(path, axis=0)
return path
if __name__ == '__main__':
Astar = Weighted_A_star(1)
Astar.run()
Astar.run()
Search-based Planning/Search_3D/LRT_Astar3D.py
+11 -19
View File
@@ -12,8 +12,9 @@ import sys
sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
from Search_3D.env3D import env
from Search_3D.Astar3D import Weighted_A_star
from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost
from Search_3D import Astar3D
from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
cost
from Search_3D.plot_util3D import visualization
import queue
@@ -91,31 +92,22 @@ import queue
# return path
class LRT_A_star2():
def __init__(self,resolution=0.5, N=7):
def __init__(self, resolution=0.5, N=7):
self.lookahead = N
self.Astar = Weighted_A_star()
self.Astar.env.resolution = resolution
def expand(self):
self.Astar.run(self.lookahead)
self.Astar = Astar3D.Weighted_A_star()
while True:
self.Astar.run(self.lookahead)
def updateHeuristic(self):
for strxi in self.Astar.CLOSED:
self.Astar.h[strxi] = np.inf
xi = dehash(strxi)
self.Astar.h[strxi] = min([cost(xi,xj) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)])
self.Astar.h[strxi] = min([cost(xi, xj) + self.Astar.h[hash3D(xj)] for xj in self.Astar.children(xi)])
def move(self):
print(np.argmin([j[0] for j in self.Astar.OPEN.enumerate()]))
def run(self):
xt = hash3D(self.Astar.goal)
while xt not in self.Astar.CLOSED:
self.expand()
#self.updateHeuristic()
if __name__ == '__main__':
T = LRT_A_star2(resolution = 1, N = 2)
T.run()
T = LRT_A_star2(resolution=1, N=50)
Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_3D/__pycache__/queue.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.