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

'statespace'

Browse Source
This commit is contained in:
yue qi
2020-07-08 23:53:27 -07:00
parent 7d50f7aa05
commit 3cbfb1eebf
9 changed files with 29 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/Search_3D/Astar3D.py
+3 -3
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, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
cost
from Search_3D.plot_util3D import visualization
import queue
@@ -27,7 +27,7 @@ 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 = StateSpace(self) # key is the point, store g value
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.AABB = getAABB(self.env.blocks)
self.Space[hash3D(getNearest(self.Space, self.start))] = 0 # set g(x0) = 0
@@ -109,7 +109,7 @@ class Weighted_A_star(object):
# utility used in LRTA*
def reset(self, xj):
self.Space = StateSpace(self) # key is the point, store g value
self.Space = 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)
Search-based Planning/Search_3D/Dstar3D.py
+1 -16
View File
@@ -8,24 +8,9 @@ from collections import defaultdict
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, isinbound, isinball
from Search_3D.utils3D import StateSpace, getDist, getRay, isinbound, isinball
import pyrr
def StateSpace(env, factor = 0):
boundary = env.boundary
resolution = 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)
g = set()
for x in xarr:
for y in yarr:
for z in zarr:
g.add((x,y,z))
return g
def getNearest(Space,pt):
'''get the nearest point on the grid'''
Search-based Planning/Search_3D/LP_Astar3D.py
+3 -3
View File
@@ -7,7 +7,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, StateSpace, Heuristic, getNearest, isinbound, isinball, hash3D, dehash, \
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, hash3D, dehash, \
cost, obstacleFree
from Search_3D.plot_util3D import visualization
import queue
@@ -23,10 +23,10 @@ class Lifelong_Astar(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.g = StateSpace(self)
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.v = StateSpace(self) # rhs(.) = g(.) = inf
self.v = g_Space(self) # rhs(.) = g(.) = inf
self.v[hash3D(self.start)] = 0 # rhs(x0) = 0
self.h = Heuristic(self.g, self.goal)
Search-based Planning/Search_3D/LRT_Astar3D.py
+1 -1
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, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
cost, obstacleFree
from Search_3D.plot_util3D import visualization
import queue
Search-based Planning/Search_3D/RTA_Astar3D.py
+1 -1
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, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, \
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, \
cost, obstacleFree
from Search_3D.plot_util3D import visualization
import queue
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
+3 -4
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 getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost
from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, hash3D, dehash, cost
from Search_3D.plot_util3D import visualization
import queue
@@ -25,9 +25,8 @@ 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 = StateSpace(self) # key is the point, store g value
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.AABB = getAABB(self.env.blocks)
self.Space[hash3D(self.start)] = 0 # set g(x0) = 0
self.Space[hash3D(self.goal)] = 0 # set g(x0) = 0
self.OPEN1 = queue.QueuePrior() # store [point,priority]
@@ -60,7 +59,7 @@ class Weighted_A_star(object):
self.CLOSED2.add(strxi2)
self.V.append(xi1)
self.V.append(xi2)
# visualization(self)
visualization(self)
allchild1, allchild2 = self.children(xi1), self.children(xi2)
self.evaluation(allchild1,strxi1,xi1,conf=1)
self.evaluation(allchild2,strxi2,xi2,conf=2)
Search-based Planning/Search_3D/utils3D.py
+17 -10
View File
@@ -44,24 +44,31 @@ def isinball(i, x):
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
def StateSpace(env, factor = 0):
boundary = env.boundary
resolution = 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
Space = set()
for x in xarr:
for y in yarr:
for z in zarr:
Space.add((x,y,z))
return Space
def g_Space(initparams):
'''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.'''
g = {}
Space = StateSpace(initparams.env)
for v in Space:
g[hash3D(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