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

update

Browse Source
This commit is contained in:
zhm-real
2020-07-09 16:05:54 -07:00
parent 1524e5bad7
commit 4f814bcb68
7 changed files with 64 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/.idea/workspace.xml
Generated
+24 -23
View File
@@ -21,6 +21,7 @@
<component name="ChangeListManager">
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_3D/Dstar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/Dstar3D.py" afterDir="false" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
@@ -94,6 +95,27 @@
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="Dstar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs>
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<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_3D/Dstar3D.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
<option name="MODULE_MODE" value="false" />
<option name="REDIRECT_INPUT" value="false" />
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="Field_D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
@@ -115,27 +137,6 @@
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="Potential_Field" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs>
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
<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/Potential_Field.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
<option name="MODULE_MODE" value="false" />
<option name="REDIRECT_INPUT" value="false" />
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="ReedsShepp" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
@@ -203,17 +204,17 @@
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.D_star" />
<item itemvalue="Python.Field_D_star" />
<item itemvalue="Python.Potential_Field" />
<item itemvalue="Python.ReedsShepp" />
<item itemvalue="Python.simulation" />
<item itemvalue="Python.Dstar3D" />
</list>
<recent_temporary>
<list>
<item itemvalue="Python.D_star" />
<item itemvalue="Python.Dstar3D" />
<item itemvalue="Python.simulation" />
<item itemvalue="Python.ReedsShepp" />
<item itemvalue="Python.Field_D_star" />
<item itemvalue="Python.Potential_Field" />
</list>
</recent_temporary>
</component>
Search-based Planning/Search_3D/Dstar3D.py
+40 -41
View File
@@ -8,25 +8,26 @@ 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 StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, initcost
from Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, \
initcost
from Search_3D.plot_util3D import visualization
class D_star(object):
def __init__(self,resolution = 1):
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.env = env(resolution=resolution)
self.X = StateSpace(self.env)
self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
self.b = defaultdict(lambda: defaultdict(dict))# back pointers every state has one except xt.
self.OPEN = {} # OPEN list, here use a hashmap implementation. hash is point, key is value
self.h = self.initH() # estimate from a point to the end point
self.tag = self.initTag() # set all states to new
self.V = set()# vertice in closed
self.b = defaultdict(lambda: defaultdict(dict)) # back pointers every state has one except xt.
self.OPEN = {} # OPEN list, here use a hashmap implementation. hash is point, key is value
self.h = self.initH() # estimate from a point to the end point
self.tag = self.initTag() # set all states to new
self.V = set() # vertice in closed
# initialize cost set
# self.c = initcost(self)
# for visualization
@@ -34,8 +35,6 @@ class D_star(object):
self.Path = []
self.done = False
def initH(self):
# h set, all initialzed h vals are 0 for all states.
h = {}
@@ -47,7 +46,7 @@ class D_star(object):
# tag , New point (never been in the OPEN list)
# Open point ( currently in OPEN )
# Closed (currently in CLOSED)
t = {}
t = {}
for xi in self.X:
t[xi] = 'New'
return t
@@ -57,7 +56,7 @@ class D_star(object):
# -1 if it does not exist
if self.OPEN:
minv = np.inf
for v,k in enumerate(self.OPEN):
for v, k in enumerate(self.OPEN):
if v < minv: minv = v
return minv
return -1
@@ -68,46 +67,46 @@ class D_star(object):
# it also removes this min value form the OPEN set.
if self.OPEN:
minv = np.inf
for v,k in enumerate(self.OPEN):
for v, k in enumerate(self.OPEN):
if v < minv: mink, minv = k, v
return mink, self.OPEN.pop(mink)
return None, -1
def insert(self, x, h_new):
# inserting a key and value into OPEN list (x, kx)
# depending on following situations
if self.tag[x] == 'New':
kx = h_new
if self.tag[x] == 'Open':
kx = min(self.OPEN[x],h_new)
kx = min(self.OPEN[x], h_new)
if self.tag[x] == 'Closed':
kx = min(self.h[x], h_new)
self.OPEN[x] = kx
self.h[x],self.tag[x] = h_new, 'Open'
self.h[x], self.tag[x] = h_new, 'Open'
def process_state(self):
x, kold = self.min_state()
self.tag[x] = 'Closed'
self.V.add(x)
if x == None: return -1
if kold < self.h[x]: # raised states
for y in children(self,x):
a = self.h[y] + cost(self,y,x)
if kold < self.h[x]: # raised states
for y in children(self, x):
a = self.h[y] + cost(self, y, x)
if self.h[y] <= kold and self.h[x] > a:
self.b[x], self.h[x] = y , a
elif kold == self.h[x]:# lower
for y in children(self,x):
bb = self.h[x] + cost(self,x,y)
self.b[x], self.h[x] = y, a
elif kold == self.h[x]: # lower
for y in children(self, x):
bb = self.h[x] + cost(self, x, y)
if self.tag[y] == 'New' or \
(self.b[y] == x and self.h[y] != bb) or \
(self.b[y] != x and self.h[y] > bb):
(self.b[y] == x and self.h[y] != bb) or \
(self.b[y] != x and self.h[y] > bb):
self.b[y] = x
self.insert(y, bb)
else:
for y in children(self,x):
bb = self.h[x] + cost(self,x,y)
else:
for y in children(self, x):
bb = self.h[x] + cost(self, x, y)
if self.tag[y] == 'New' or \
(self.b[y] == x and self.h[y] != bb):
(self.b[y] == x and self.h[y] != bb):
self.b[y] = x
self.insert(y, bb)
else:
@@ -115,11 +114,11 @@ class D_star(object):
self.insert(x, self.h[x])
else:
if self.b[y] != x and self.h[y] > bb and \
self.tag[y] == 'Closed' and self.h[y] == kold:
self.tag[y] == 'Closed' and self.h[y] == kold:
self.insert(y, self.h[y])
return self.get_kmin()
def modify_cost(self,x,y,cval):
def modify_cost(self, x, y, cval):
# TODO: implement own function
# self.c[x][y] = cval
# if self.tag[x] == 'Closed': self.insert(x,self.h[x])
@@ -131,11 +130,12 @@ class D_star(object):
kmin = self.process_state()
if kmin >= self.h[x]: break
def path(self, goal = None):
def path(self, goal=None):
path = []
if not goal:
x = self.x0
else: x = goal
else:
x = goal
start = self.xt
while x != start:
path.append([np.array(x), np.array(self.b[x])])
@@ -147,7 +147,7 @@ class D_star(object):
self.OPEN[self.xt] = 0
# first run
while True:
#TODO: self.x0 =
# TODO: self.x0 =
self.process_state()
visualization(self)
if self.tag[self.x0] == "Closed":
@@ -162,16 +162,15 @@ class D_star(object):
while s != self.xt:
if s == tuple(self.env.start):
s = self.b[self.x0]
else:
else:
s = self.b[s]
self.modify(s)
self.env.move_block(a=[0,0,-0.1],s=0.5,block_to_move=1,mode='translation')
# self.modify(s)
self.env.move_block(a=[0, 0, -0.1], s=0.5, block_to_move=1, mode='translation')
self.Path = self.path(s)
visualization(self)
self.ind += 1
if __name__ == '__main__':
D = D_star(1)
D.run()
D.run()
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.