From a5d8c190b37f6a306bf293094cc87497fe2f6a8e Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Thu, 9 Jul 2020 13:38:46 -0700 Subject: [PATCH] 'D*' --- Search-based Planning/Search_3D/Astar3D.py | 8 +- Search-based Planning/Search_3D/Dstar3D.py | 76 ++++++++++++++---- Search-based Planning/Search_3D/LP_Astar3D.py | 2 +- .../__pycache__/Astar3D.cpython-37.pyc | Bin 3178 -> 3182 bytes .../__pycache__/env3D.cpython-37.pyc | Bin 2917 -> 3647 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 4787 -> 4797 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 6184 -> 6184 bytes Search-based Planning/Search_3D/env3D.py | 61 ++++++++++++-- .../Search_3D/plot_util3D.py | 2 +- 9 files changed, 118 insertions(+), 31 deletions(-) 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 d3052cbbc000654f550a1d03d73b38b691999da7..a14034838a09e1b4bd06f6b9625683610ddea84d 100644 GIT binary patch delta 474 zcmaDQ@lJx*iIE z(^E@|WG8Q6S4|ZI337r6DUb|jYF=q>YEfcIYLO~PP!c4_oRgYYqyS>E78GUXmE2-3 zE-5Nf1If#QL|F~2esW(q=g4ugGy0exsq^1wXPy~nvdA8d>dPDZH1n>u zs03MoY-mx54i{9APw5I@1hgQ_a4a6)^f~#0QyDM^nCS74iina!>V`rkB0*GvzY5BR zz6fY7P964yVo@--^(@*1F;2;qdl}cRB0rXzyvn?{;`$O`9fI*5i)*N%V`bdY5{#)j zTpKd5UQgn+V>=C4g5YVBttpQy-t3v9t~&rz-Q-2KQ}zINAs9;g|8vIcerIb*n$wmn z*v*Be4wN~F^f%l`qq4ee+YiUx@UW HX1D(U$|qr- 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 c6679f6afab2c14438e66e36e045a554f207dd7b..d503d2e3e0948119cfd26b9f532380d65ceb17d3 100644 GIT binary patch delta 1719 zcmaJ>Pj4GV6rZ=VJ3C(6TgNeu9TF+DO&eEL4*bid2ofX$67q#pEuod;*|<&|uQY31 z6uB}eIdTB4H~<&;030}S;>@Kza6u!kJ>&}@(G%~D9h%poF$K!$L(5TIx#z6K_nGpPIA==4?ip`+%2yKQLgiF| ztFQ8^po%I~*Ho!14nZxc@;S65+AXUJ+Evb+RxS?IE60x3aqf(@@^K-~9~I94m~uX0 zjq2%A@XODSoa`U|*ty`@@9te;+}+*1z3iF$>75!zrk$(Aqu&t82F{c=CFn5w()+kX z(gru;pr3S(Q@xTI?+sYbLh-P)gj>3V7&fG?X8XCV?M+f#q|j!M(z=5)rGzjLPy-1CoaO9?-2I!B5KT!iVuW5h z$NK8I4F`SKg7rzR89$6{vUgEC6-dZi8BU7`b$hyfAzrxGulkjXcCIkDaeU zKb{`>9C~aA^#yJq^fn9R6O>z@;U}yMRYJK?C48ichW~@?(s~g#LAvy^kX{~4-VnxQ znjXY~j|gwd*}>w9IHJl~RDasK3Yz*Yym0%BbeTA+dVP(E?F#e`O@64`gS6is_L6~- zIvHB7A*Q8I)uX9&^c^Dh@m1IaYZadvnM2z^YG7nwzg4s^A{r(Um85Y`5*4?2qI)kd zXRG}YI!Rwg!dwY~Q-jLv+u&=k23t;m(lUgzZOzlSB zAVM(pO(K*8RH_rdO5{}{w%1Y!`b{G55LxsM%h~2}9XH;_nM&#k7;PXu$u)_MjG~iv zZxBU|FnbcL-Y_CLIEp(%+pnjMYXkMP?J#7a>)BF%8E$8r`CB#WiY`blt0hnR(y#IG Ip2U9gFV>Ypi~s-t delta 979 zcmZ8eOKVd>6rP#;ntPMnv`L$!HLZcP)hoq~paosDRS=|fBlx&b(j?~^`!LnHp_P(0 zLh4Qg8F1aYaH9*|y733NbD5>!qAS5);5oOnr~~IaXTEuy$M?1Tv*0ggG9JVAqwdu9 z-8X)zw~squAp;I#lZOHfFb`O0fCV;s^ELwq=^iMuQ)eSY^s^f(oU z%Roz^-9(G;Ahg&Xe?H%3x7f$(J6;5AxaDfMjaq926kg}tGN*m%!P%;MU|#Y;tVXh=(M5^ z9gg89rl(Ap+~adx<@KcTT_|7wHfAlA!xKGauJE$vele#;a9=@-=|9QUi2h(MWrz(7 zHH=Q>^r~IfRjbG=`nt7~FAzD3a7^l{B7*+HFX>;_dtTLV?N_}iq?%}F%fVHy_vAt!N&(m_(+Ja(vgrCNG~H zEeB50?q4v2_x@`+8;{ufBNOHZQ6pyRt14(bEz= zWs%Y0eV%dBs7$N{eyzyPNL!JU>4}UApn(oXS7b(NL@t_kMKdbcN!yb_yc zWd^bZqDlC-QSz4n0ip@?sA-{QW@Mcj<*)LTZ(>v2kT gMz|+Xe_SCPtLGr($Lb?k<_GFKXq(MnyFI@C17l`*xc~qF delta 554 zcmZvYJ5R$f5XWuD2}zoiG?ccXbA<|GfJ(6OS{M=HjnsDNAw&d43K@9VFm&$Z4KcLh z6QItl>@fDzaIOLqw(h69`=3vD{-}*={-HHX!W{TR zLP?qz=A`fRdJofSkcEZcC>z9z!wWj3z#!K*uxvYE`;>;dYU~h%yv0-1{H{g z6sK_p5zJ-YoC5c&R2>_aT^Mt?>7FMx{&up0PDa@vVAMosn*e*geNA_cVo 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