|
|
@@ -8,32 +8,43 @@ 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
|
|
|
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.
|
|
|
@@ -46,7 +57,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
|
|
|
@@ -56,7 +67,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
|
|
|
@@ -67,11 +78,11 @@ 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
|
|
|
@@ -82,31 +93,31 @@ class D_star(object):
|
|
|
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
|
|
|
+ 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 \
|
|
|
- (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:
|
|
|
@@ -114,28 +125,28 @@ 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):
|
|
|
# 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
|
|
|
|
|
|
- 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 +158,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":
|
|
|
@@ -156,21 +167,32 @@ 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]
|
|
|
- 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.Path = self.path(s)
|
|
|
- visualization(self)
|
|
|
+ sparent = self.b[self.x0]
|
|
|
+ else:
|
|
|
+ sparent = self.b[s]
|
|
|
+
|
|
|
+ self.env.move_block(a=[0,0,-0.1],s=0.5,block_to_move=1,mode='translation')
|
|
|
+ # 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__':
|
|
|
D = D_star(1)
|
|
|
- D.run()
|
|
|
+ D.run()
|
zhm-real