From 7d50f7aa05400ed4dff3a2d63994df393fa9460e Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Wed, 8 Jul 2020 23:36:27 -0700 Subject: [PATCH] 'D*' --- Search-based Planning/Search_3D/Dstar3D.py | 174 ++++++++++++++++-- .../__pycache__/Astar3D.cpython-37.pyc | Bin 3456 -> 3460 bytes .../__pycache__/env3D.cpython-37.pyc | Bin 2357 -> 2361 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 4783 -> 4787 bytes .../__pycache__/queue.cpython-37.pyc | Bin 2995 -> 2999 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 4118 -> 4122 bytes 6 files changed, 158 insertions(+), 16 deletions(-) diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index a1a31e9..3edf606 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -3,11 +3,12 @@ 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 getDist, getRay +from Search_3D.utils3D import getDist, getRay, isinbound, isinball import pyrr def StateSpace(env, factor = 0): @@ -19,29 +20,70 @@ def StateSpace(env, factor = 0): xarr = np.arange(xmin,xmax,resolution).astype(float) yarr = np.arange(ymin,ymax,resolution).astype(float) zarr = np.arange(zmin,zmax,resolution).astype(float) - g = {} + g = set() for x in xarr: for y in yarr: for z in zarr: - g[(x,y,z)] = np.inf + g.add((x,y,z)) return g -def Heuristic(initparams,x): - h = {} - x = np.array(x) - for xi in initparams.g.keys(): - h[xi] = max(abs(x-np.array(xi))) - return h - def getNearest(Space,pt): '''get the nearest point on the grid''' mindis,minpt = 1000,None - for pts in Space.keys(): + for pts in Space: dis = getDist(pts,pt) if dis < mindis: mindis,minpt = dis,pts return minpt +def isCollide(initparams, x, child): + '''see if line intersects obstacle''' + ray , dist = getRay(x, child) , getDist(x, child) + if not isinbound(initparams.env.boundary,child): + return True, dist + for i in initparams.env.AABB: + shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) + if shot is not None: + dist_wall = getDist(x, shot) + if dist_wall <= dist: # collide + return True, dist + for i in initparams.env.balls: + if isinball(i, child): + return True, dist + shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i) + if shot != []: + dists_ball = [getDist(x, j) for j in shot] + if all(dists_ball <= dist): # collide + return True, dist + return False, dist + +def children(initparams, x): + # get the neighbor of a specific state + allchild = [] + resolution = initparams.env.resolution + for direc in initparams.Alldirec: + child = tuple(map(np.add,x,np.multiply(direc,resolution))) + if isinbound(initparams.env.boundary,child): + allchild.append(child) + return allchild + +def cost(initparams, x, y): + # get the cost between two points, + # do collision check here + collide, dist = isCollide(initparams,x,y) + if collide: return np.inf + else: return dist + +def initcost(initparams): + # initialize cost dictionary, could be modifed lateron + c = defaultdict(lambda: defaultdict(dict)) # two key dicionary + for xi in initparams.X: + cdren = children(initparams, xi) + for child in cdren: + c[xi][child] = cost(initparams, xi, child) + return c + + class D_star(object): 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], @@ -50,10 +92,110 @@ class D_star(object): [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.g = StateSpace(self.env) - self.x0, self.xt = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal) - self.h = Heuristic(self,self.x0) # getting heuristic for x0 + 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.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 + + # initialize cost set + self.c = initcost(self) + + # 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. + h = {} + for xi in self.X: + h[xi] = 0 + return h + + def initTag(self): + # tag , New point (never been in the OPEN list) + # Open point ( currently in OPEN ) + # Closed (currently in CLOSED) + t = {} + for xi in self.X: + t[xi] = 'New' + return t + + def get_kmin(self): + # get the minimum of the k val in OPEN + # -1 if it does not exist + if self.OPEN: + minv = np.inf + for k,v in enumerate(self.OPEN): + if v < minv: minv = v + return minv + return -1 + + def min_state(self): + # returns the state in OPEN with min k(.) + # if empty, returns None and -1 + # it also removes this min value form the OPEN set. + if self.OPEN: + minv = np.inf + for k,v 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) + 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' + + def process_state(self): + x, kold = self.min_state() + self.tag[x] = 'Closed' + 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] + 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] + 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 + self.insert(y, bb) + else: + for y in children(self,x): + bb = self.h[x] + self.c[x][y] + if self.tag[y] == 'New' or \ + (self.b[y] == x and self.h[y] != bb): + self.b[y] = x + self.insert(y, bb) + else: + if self.b[y] != x and self.h[y] > bb: + 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.insert(y, self.h[y]) + 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() + + def run(self): + # TODO: implementation of changing obstable in process + pass + if __name__ == '__main__': - D = D_star(1) - print(D.h[D.x0]) \ No newline at end of file + D = D_star(1) \ No newline at end of file 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 0ef65629ce7fc550d8bf0a63cf6c4a0df88a26bf..26a51e0038cde9c6c45ad83a9d3e6c1a7fcb36dc 100644 GIT binary patch delta 211 zcmZpWZjt77;^pOH0D?$+=J+2QdH1tev?p7|gche36~`D`8X6nMxTF?mm*f}3q-U07 zlqSU#B$j087UU%6n7%;=NADba*Jaov5GRrOcrBR<&FtXO)N^zh&OhLne4`T zm+{tS6}BcOMuW}EIp#4kT1zM8^T{PVR*X@T*YlK!JA!1Gi%SxtSRH|65k#vgP?z}RKwe`2>Wn|A 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 07487bbd67a3dacb5a853863ba1bfb3d5a3878c1..018f3217116e9323a5e725b0fd03c86872a81f07 100644 GIT binary patch delta 128 zcmdlgv{Q)JiIYjg_B+k&O`mxfv>B delta 105 zcmdlfv{i`LiIA;<$oq& zG9QzKxPEYIVo`F2Zc<`#YKlTYPGVkOW?s7fWLqXxZm3+mv5WrZT&Ck}tO6X2Y>WWy C{UA~R 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 382a86404ccc29130ab73849c2fbd9cecb6f9997..5f1b8c04fef6b2abb72c74802b2494e718627190 100644 GIT binary patch delta 104 zcmZ3lx>=RSiI3fiVe)F%&&bbB)lbYxtj$SM7A0rsCM6c9rYHnJ73fcnVp8RX%EcSI=x^>~ H(i8vysfi}r 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 e89e676406aa39441c6ecaec67aa782628aa1574..6871e7f84465862e00e9ed3414b5452d84a6cae1 100644 GIT binary patch delta 117 zcmdlizFnNpiIRTVk~4IZ5{pw)6at_MprY}{E-{-g IGd|!108Ah#3jhEB delta 94 zcmdlkzFC~liIbBR@A)KQSk@Lf<8|IJ+djKtDaRB%?G* rfATy=2}S+j)Wo9X4Be!};?xv{fSkm8j1M>gir^pA 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 91f0836fa251b7593f0392889801c312c2e7cb1c..95abe6ac304028b13352c3760ef3ba6e6458e8be 100644 GIT binary patch delta 620 zcmYL_&ubGw6vv%pcavKD}3*%(7CPVi}m|c~e zL@$CM9v*m*p1gPxlz@ML=U%)H9{fAJ=)Tk+bNDd4`M&Ra^L=Z*vy7%;=*QBtdm~HV ze>U3W+)MYG(}_J1JB_RLM%`(7@qQAGoB>aUk9!UTq67VxmoV+jT2HmN{I`9drxs&B|59H?K(0#NOzwwx{@H27wo z0FgUGR^fH-<|#WZ?rhma98WKHM*I`a!>?SK)F$QpM{*KXB7-=O$RaL)ELB5K#+zFx~oWz2+-@JjEH_T-2DjF2ikESAX%jEi+` z?MV5lcxh!7uh=P-&CO<0Dx-1QXb!w6@DjmYns{-7-!&>~4P{^(XRlwuE)9`KtmA{~ z4!Pf#S;59qNh3<8Ka@&5j1pPE0)5JSU&0GxmvrH~!EM|_YCS)4_hT{LWE<^pj-3_% zhb&K{d|s-OJ@`;!NBM3+$=sNGfEIa51I9y2({$Hh$F#b4&}RmLX^S~T1z{mDL@|$; e4#jz*jp!ow5Py9`7I>6O)eE0ATB-S=dFwY#R-+RD delta 578 zcmYL_&rcIU6vut-?zUYRij5EoibYFsfp)`>5F=jN6oVRqiJHJ1m@LbNUD>ucy9zxR zO-wvO%u6_U;%*2rHU0to1)TR_Jos0bm^iNqaSorn%zWSXJMVodos?LeG4q-_?8D!! zr*QF#Ul&kpen4C zq_+`FWs5M=y_r3OArihBdRM(B@Jgi^q{|U#XgP`R`sa*UDy#Ute|~D3N-|5dTCHABwEd3q%I|t=Be4uz z5qnWg6;Nh^7;? zgiyoI;zqu-u}X?Q0EvM-Tk?3CQ?LtGDU*@0u E1B5Y&ssI20