diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index da4c5c6..0331cd2 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -61,7 +61,7 @@ class Weighted_A_star(object): if xi not in self.CLOSED: self.V.append(np.array(xi)) self.CLOSED.add(xi) # add the point in CLOSED set - # visualization(self) + visualization(self) allchild = children(self,xi) for xj in allchild: if xj not in self.CLOSED: @@ -88,9 +88,9 @@ class Weighted_A_star(object): if xt in self.CLOSED: self.done = True self.Path = self.path() - # if N is None: - # visualization(self) - # plt.show() + if N is None: + visualization(self) + plt.show() return True return False diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index 3534379..003d8ec 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -3,12 +3,13 @@ import matplotlib.pyplot as plt import os import sys +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 -import pyrr +from Search_3D.plot_util3D import visualization class D_star(object): @@ -21,16 +22,19 @@ class D_star(object): 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 = {} # back pointers every state has one except xt. + 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) + # self.c = initcost(self) + # for visualization + self.ind = 0 + self.Path = [] + self.done = False - # put G (ending state) into the OPEN list - self.OPEN[self.xt] = 0 + def initH(self): # h set, all initialzed h vals are 0 for all states. @@ -53,7 +57,7 @@ class D_star(object): # -1 if it does not exist if self.OPEN: minv = np.inf - for k,v in enumerate(self.OPEN): + for v,k in enumerate(self.OPEN): if v < minv: minv = v return minv return -1 @@ -64,7 +68,7 @@ class D_star(object): # it also removes this min value form the OPEN set. if self.OPEN: minv = np.inf - for k,v 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 @@ -84,15 +88,16 @@ class D_star(object): 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] + self.c[y][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] + self.c[x][y] + 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): @@ -100,7 +105,7 @@ class D_star(object): self.insert(y, bb) else: for y in children(self,x): - bb = self.h[x] + self.c[x][y] + 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 @@ -115,14 +120,51 @@ class D_star(object): return self.get_kmin() def modify_cost(self,x,y,cval): - self.c[x][y] = cval # set the new cost to the cval - if self.tag[x] == 'Closed': self.insert(x,self.h[x]) - return self.get_kmin() + # TODO: implement own function + # self.c[x][y] = cval + # if self.tag[x] == 'Closed': self.insert(x,self.h[x]) + # return self.get_kmin() + pass + + def path(self): + path = [] + x = self.x0 + start = self.xt + while x != start: + path.append([np.array(x), np.array(self.b[x])]) + x = self.b[x] + return path def run(self): - # TODO: implementation of changing obstable in process - pass + # put G (ending state) into the OPEN list + self.OPEN[self.xt] = 0 + # first run + while True: + #TODO: self.x0 = + self.process_state() + visualization(self) + if self.tag[self.x0] == "Closed": + break + self.ind += 1 + self.Path = self.path() + self.done = True + visualization(self) + # 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.process_state() + self.env.move_block(a=[0,0,0.1],s=0.5,mode='translation') + self.Path = self.path() + visualization(self) + self.ind += 1 + if __name__ == '__main__': - D = D_star(1) \ No newline at end of file + D = D_star(1) + D.run() \ No newline at end of file diff --git a/Search-based Planning/Search_3D/LP_Astar3D.py b/Search-based Planning/Search_3D/LP_Astar3D.py index c64e59c..b1d2be5 100644 --- a/Search-based Planning/Search_3D/LP_Astar3D.py +++ b/Search-based Planning/Search_3D/LP_Astar3D.py @@ -167,7 +167,7 @@ class Lifelong_Astar(object): plt.pause(2) def change_env(self): - self.env.change() + self.env.New_block() self.done = False self.Path = [] self.CLOSED = set() diff --git a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc index d3052cb..a140348 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index c6679f6..d503d2e 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc index 5f1b8c0..d54e50d 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc index 7f432df..26df758 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py index 12b18ab..7e76a61 100644 --- a/Search-based Planning/Search_3D/env3D.py +++ b/Search-based Planning/Search_3D/env3D.py @@ -22,12 +22,12 @@ def getblocks(): Obstacles.append([j for j in i]) return np.array(Obstacles) -def getAABB(blocks): - # used for Pyrr package for detecting collision - AABB = [] - for i in blocks: - AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger - return AABB +# def getAABB(blocks): +# # used for Pyrr package for detecting collision +# AABB = [] +# for i in blocks: +# AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger +# return AABB class aabb(object): def __init__(self,AABB): @@ -60,11 +60,56 @@ class env(): self.balls = getballs() self.start = np.array([0.5, 2.5, 5.5]) self.goal = np.array([19.0, 2.5, 5.5]) + self.t = 0 # time - def change(self): + def New_block(self): newblock = add_block() self.blocks = np.vstack([self.blocks,newblock]) - self.AABB = getAABB(self.blocks) + self.AABB = getAABB2(self.blocks) + + def move_start(self, x): + self.start = x + + def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], G = None, block_to_move = 0, mode = 'uniform'): + # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, + # G is an orthorgonal transform in R3*3, in the Galilean transformation + # (x',t') = (x + tv, t) is uniform transformation + if mode == 'uniform': + ori = self.blocks[block_to_move] + self.blocks[block_to_move] = \ + np.array([ori[0] + self.t * v[0],\ + ori[1] + self.t * v[1],\ + ori[2] + self.t * v[2],\ + ori[3] + self.t * v[0],\ + ori[4] + self.t * v[1],\ + ori[5] + self.t * v[2]]) + + self.AABB[block_to_move].P = \ + [self.AABB[block_to_move].P[0] + self.t * v[0], \ + self.AABB[block_to_move].P[1] + self.t * v[1], \ + self.AABB[block_to_move].P[2] + self.t * v[2]] + # (x',t') = (x + a, t + s) is a translation + if mode == 'translation': + ori = self.blocks[block_to_move] + self.blocks[block_to_move] = \ + np.array([ori[0] + a[0],\ + ori[1] + a[1],\ + ori[2] + a[2],\ + ori[3] + a[0],\ + ori[4] + a[1],\ + ori[5] + a[2]]) + + self.AABB[block_to_move].P = \ + [self.AABB[block_to_move].P[0] + a[0], \ + self.AABB[block_to_move].P[1] + a[1], \ + self.AABB[block_to_move].P[2] + a[2]] + self.t += s + # (x',t') = (Gx, t) + if mode == 'rotation': # this makes AABB become a OBB + #TODO: implement this with rotation matrix + pass + + if __name__ == '__main__': newenv = env() diff --git a/Search-based Planning/Search_3D/plot_util3D.py b/Search-based Planning/Search_3D/plot_util3D.py index 3bf0e8a..03d94cb 100644 --- a/Search-based Planning/Search_3D/plot_util3D.py +++ b/Search-based Planning/Search_3D/plot_util3D.py @@ -53,7 +53,7 @@ def draw_line(ax,SET,visibility=1,color=None): def visualization(initparams): if initparams.ind % 20 == 0 or initparams.done: - V = np.array(initparams.V) + V = np.array(list(initparams.V)) # E = initparams.E Path = np.array(initparams.Path) start = initparams.env.start