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

'D*_update'

Browse Source
This commit is contained in:
yue qi
2020-07-09 16:05:30 -07:00
parent 1524e5bad7
commit 3bea68cfd3
1 changed files with 31 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/Search_3D/Dstar3D.py
+31 -10
View File
@@ -33,8 +33,18 @@ class D_star(object):
self.ind = 0
self.Path = []
self.done = False
self.Obstaclemap = {}
def update_obs(self):
for xi in self.X:
print('xi')
self.Obstaclemap[xi] = False
for aabb in self.env.blocks:
self.Obstaclemap[xi] = isinbound(aabb, xi)
if self.Obstaclemap[xi] == False:
for ball in self.env.balls:
self.Obstaclemap[xi] = isinball(ball, xi)
def initH(self):
# h set, all initialzed h vals are 0 for all states.
@@ -79,7 +89,7 @@ class D_star(object):
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
@@ -95,7 +105,7 @@ class D_star(object):
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
if 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 \
@@ -119,14 +129,15 @@ class D_star(object):
self.insert(y, self.h[y])
return self.get_kmin()
def modify_cost(self,x,y,cval):
def modify_cost(self,x):
# TODO: implement own function
# self.c[x][y] = cval
# if self.tag[x] == 'Closed': self.insert(x,self.h[x])
xparent = self.b[x]
if self.tag[x] == 'Closed': self.insert(x , self.h[xparent] + cost(self,x,xparent))
# return self.get_kmin()
pass
def modify(self, x):
self.modify_cost(x)
while True:
kmin = self.process_state()
if kmin >= self.h[x]: break
@@ -156,20 +167,30 @@ class D_star(object):
self.Path = self.path()
self.done = True
visualization(self)
plt.pause(0.5)
# plt.show()
# when the environemnt changes over time
s = tuple(self.env.start)
while s != self.xt:
if s == tuple(self.env.start):
s = self.b[self.x0]
sparent = self.b[self.x0]
else:
s = self.b[s]
self.modify(s)
sparent = self.b[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.update_obs()
print('updated')
if cost(self,s, sparent) == np.inf:
self.modify(s)
print('modified')
self.ind += 1
self.Path = self.path()
visualization(self)
plt.show()
if __name__ == '__main__':