From 4f814bcb6833ac563a44fa78afcd4ac1ff2fc343 Mon Sep 17 00:00:00 2001 From: zhm-real Date: Thu, 9 Jul 2020 16:05:54 -0700 Subject: [PATCH] update --- Search-based Planning/.idea/workspace.xml | 47 +++++----- Search-based Planning/Search_3D/Dstar3D.py | 81 +++++++++--------- .../__pycache__/Astar3D.cpython-37.pyc | Bin 3182 -> 3155 bytes .../__pycache__/env3D.cpython-37.pyc | Bin 3647 -> 3620 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 4797 -> 4770 bytes .../__pycache__/queue.cpython-37.pyc | Bin 2999 -> 3010 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 6184 -> 6157 bytes 7 files changed, 64 insertions(+), 64 deletions(-) diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 0aedfb1..32b8a5e 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -21,6 +21,7 @@ + diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index 7d08cbd..22ccc52 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -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() \ No newline at end of file + D.run() 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 a14034838a09e1b4bd06f6b9625683610ddea84d..58807f373000453986b5e5491e0202bdad2ba640 100644 GIT binary patch delta 85 zcmaDSaan@PiI4DskXVwTP>_?Dmj_Zc`7?{wW@lDyb^x@C9Tflo delta 82 zcmcaC@lJxviI?RvG#L 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 d503d2e3e0948119cfd26b9f532380d65ceb17d3..0c432ca41e957068fae3b70fd42a18483ccd90e9 100644 GIT binary patch delta 101 zcmdllvqXl=iI4DskXVwTP>_?Dmj_ZcS%*n$a}AR|7vrtTYj}<_+D&HU6QA74yA1%E CN+Kx$ delta 94 zcmZ1?vtNeGiI&Nwlp+0jB!aV&MwI>h)K^Z$tX>V uDM&2I&@ISG%*)HnOV^!j&ZNbzucrq>o1K{axEP}*PvkkuXgfKa_bdQPd>udl 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 d54e50d25b9dccff52231b86c1d90274cb54e3e9..6ae7677054c3468a6275554178421b501dd01f78 100644 GIT binary patch delta 96 zcmdn1x=5AFiI)jD9y~x%u82@%E&K`aY-%CF3B&5 wanCHtC{5Ch^6-r*NG!=vD9B07%LA#JY{{gxxs&M!AL}iqqKaFa+XT%S0lbnRmjD0& delta 93 zcmZ3ax>uFUiI&Nwlp+0jB!aV&MwI>h)K^Z$tX>V tDM&2I&@ISG%*)HnOV^$3!lcEnucrq>oBf%@`B|fwiYlTuXA7D$0stQM8?pcZ diff --git a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc index 6871e7f84465862e00e9ed3414b5452d84a6cae1..7c5dea494f4ae39cdd7feaff7ba4b43695b52162 100644 GIT binary patch delta 110 zcmdlken_0hiIej`1kX%+1V8SBT2UFO6|YEzT~< zFNkr^EXgQM(v9-)jVVYh$xtZBNzBUwsoK1Tk&T`4*5=I|rA*v%Kog6UK!oz-h0GF@ Hy}5G$z9Ayl delta 99 zcmX>kzFnNhiIMlxF5;=A|n{W#pH}xTF?mm*f}3 rxM!ARlqTs$dHBW@B$i|-6yzl4<$+X9KE|lES&Ye0lrd_uhxh{kHpL!l delta 88 zcmeA*SYg2B#LLUY00g&^Hgfed>b56a#e^2878S=BTN)Z0#<-*wXP4v`#H44IWRxbw o6eN~p=oaK8=H+GPrRz?<$f(7xucrq>o9{3Nh%(-qtSkNi0Kcys+5i9m