From d229bceaaf3df6ee7ec7365cda6a97d81b8a81d9 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Sun, 26 Jul 2020 14:44:35 -0700 Subject: [PATCH] 'update' --- .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 6705 -> 6694 bytes .../rrt_3D/dynamic_rrt3D.py | 70 ++++++++++++++++++ Sampling-based Planning/rrt_3D/rrt3D.py | 1 + Sampling-based Planning/rrt_3D/rrtstar3D.py | 5 +- Sampling-based Planning/rrt_3D/utils3D.py | 9 +-- .../Search_3D/Anytime_Dstar3D.py | 44 +++++------ .../Search_3D/DstarLite3D.py | 28 ++----- .../__pycache__/utils3D.cpython-37.pyc | Bin 10429 -> 10919 bytes Search-based Planning/Search_3D/utils3D.py | 37 ++++----- 9 files changed, 128 insertions(+), 66 deletions(-) create mode 100644 Sampling-based Planning/rrt_3D/dynamic_rrt3D.py diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc index 59a5593938e01c33629c936d945d8f1cf30d75bd..083efd8d79dfd8a7619f91da826447caa0be9d3d 100644 GIT binary patch delta 1183 zcmYjQO>7%g5Z>88Z~Yg0{j+uwCvNk{B4|q*R4Ryq+$M$okSdXa>O;b2pVQiLZ1i@E zkl4Z@;RKgzxX=?w6*p9&01`+=;)qZW9N7~R2RQYDgajvMHiGb^`DWfX^XARlnf+qn zvxQtMmrEyPm%^2ytu#$QirVlq#H4V-j4%6?e<}5qr z@bBy-xcJjZ37^fq2p6!MyT9-PyG(*O>ks>P_6A!)==r-c#r#=@3j}UT9_3)-9G=f# zg2lrq53oATOj6u9S+OC#!L3fL^*aM;vx`s1#PMl z3Y(WXAbos^A(mPzuao&-_jwBV zV_zf*P7_2xT~>Bgndh+yX-9iCg1WM=hBOEN?r^fa_^KYOZ=a2Ie;9**m<{+5rHHsjYKeMobpJaGy)o`s1J>OOk>AM;CjSE zxUg{8QynBkt4bhMNGw>O2q88_>IMk}>Utz3b_l_O1wVju3`(zbzjNoD?|ygYo^QT5 z_4O&Y=DKl>$FD2pYpt8E4}aa93qMHU@S(Onf-WXGW-x_mj-xn&8ID;T#Vp4h<}lAO zk0;Q>!Xa#X=wp#H1suZ?M<2^L&asFSILUGBx`x{8I!@sv&J*@|QUDZOnr}cSPPe*fCQ(HZ) zZM0)(c^c;=&l7D{jnOCOprxU@eYL2GemtdLRbZ^0F8cvqH$7I_wLnWq; zCDbCVCeOmk(P0uGF)ftAtB%n}sodkjj;wmn>2%e+tc?q`npzPJOHBw_UOuuKJFO0i zl3(`9aZFR|^mqCx*r555pS+rAxXKodn83O~l~S2Umd?uZ8HP&y z;UNRUw$%s}ZmDy^ctK!J;Cb53ybPOkFY|Ny;c-J!TBO^fHMqz<@E4Xi(c`vrQ=zT* zVOLjKv4}Le2fi4=WFzQ(F$DBgKnriu1Hgb zHMot~JL~|iEBO)rgM_RuF?_}I|Awc*ggB^6^kw!&?)VHfF4Qf7*QlB+4vTJNZU*5F ze@i1b<$og*Nd^suA;S_`NQ!eV)1BP=ut25!EPPLE`HwRT%u#8E$mll1im9&B@A*gJ zK4rZyC&kEC&Ao%Sno<&OKzFjEgSLl1nf zpsw)x9?!wIIp7$Mg{GDWT^-K(*-B##2m}XIG&S;dwfQkr;{A|7vI44vj6}9 diff --git a/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py b/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py new file mode 100644 index 0000000..7a8b934 --- /dev/null +++ b/Sampling-based Planning/rrt_3D/dynamic_rrt3D.py @@ -0,0 +1,70 @@ +""" +This is dynamic rrt code for 3D +@author: yue qi +""" +import numpy as np +from numpy.matlib import repmat +from collections import defaultdict +import time +import matplotlib.pyplot as plt + +import os +import sys + +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based Planning/") +from rrt_3D.env3D import env +from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset + +class dynamic_rrt_3D: + + def __init__(self): + self.env = env() + self.Parent = {} + self.E = edgeset() # edgeset + self.V = [] # nodeset + self.i = 0 + self.maxiter = 2000 # at least 2000 in this env + self.stepsize = 0.5 + self.gamma = 500 + self.eta = 2*self.stepsize + self.Path = [] + self.done = False + + def RegrowRRT(self): + self.TrimRRT() + self.GrowRRT() + + def TrimRRT(self): + S = set() + i = 1 + for qi in self.V: + qp = self.Parent(qi) + if qp.flag == 'Invalid': + qi.flag = 'Invalid' + if qi.flag != 'Invalid': + S.add(qi) + i += 1 + self.V, self.E = self.CreateTreeFromNodes(S) + + def InvalidateNodes(self, obstacle): + E = self.FindAffectedEdges(obstacle) + for e in E: + qe = self.ChildEndpointNode(e) + qe.flag = 'Invalid' + + + def GrowRRT(self): + # TODO + pass + + def CreateTreeFromNodes(self, S): + #TODO + pass + + def FindAffectedEdges(self, obstacle): + #TODO + pass + + def ChildEndpointNode(self): + #TODO + pass diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py index bc58a02..3edaa9c 100644 --- a/Sampling-based Planning/rrt_3D/rrt3D.py +++ b/Sampling-based Planning/rrt_3D/rrt3D.py @@ -29,6 +29,7 @@ class rrtstar(): self.stepsize = 0.5 self.Path = [] self.done = False + self.x0 = tuple(self.env.start) def wireup(self, x, y): self.E.add_edge([x, y]) # add edge diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py index 6e6eb43..37013e9 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py @@ -23,12 +23,13 @@ class rrtstar(): self.E = edgeset() self.V = [] self.i = 0 - self.maxiter = 2000 # at least 2000 in this env + self.maxiter = 12000 # at least 2000 in this env self.stepsize = 0.5 self.gamma = 500 self.eta = 2*self.stepsize self.Path = [] self.done = False + self.x0 = tuple(self.env.start) def wireup(self,x,y): self.E.add_edge([x,y]) # add edge @@ -52,7 +53,7 @@ class rrtstar(): def run(self): self.V.append(tuple(self.env.start)) self.ind = 0 - xnew = tuple(self.env.start) + xnew = self.x0 print('start rrt*... ') self.fig = plt.figure(figsize = (10,8)) while self.ind < self.maxiter: diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index ddf2510..7df2910 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -37,14 +37,14 @@ def getDist(pos1, pos2): ''' -def sampleFree(initparams): +def sampleFree(initparams, bias = 0.1): '''biased sampling''' x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6]) i = np.random.random() if isinside(initparams, x): return sampleFree(initparams) else: - if i < 0.1: + if i < bias: return initparams.env.goal + 1 else: return x @@ -172,10 +172,9 @@ def steer(initparams, x, y): def cost(initparams, x): '''here use the additive recursive cost function''' - if x == tuple(initparams.env.start): + if x == initparams.x0: return 0 - xparent = initparams.Parent[x] - return cost(initparams, xparent) + getDist(x, xparent) + return cost(initparams, initparams.Parent[x]) + getDist(x, initparams.Parent[x]) def path(initparams, Path=[], dist=0): diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search-based Planning/Search_3D/Anytime_Dstar3D.py index 3bc1ad1..43c0307 100644 --- a/Search-based Planning/Search_3D/Anytime_Dstar3D.py +++ b/Search-based Planning/Search_3D/Anytime_Dstar3D.py @@ -91,28 +91,30 @@ class Anytime_Dstar(object): # scan graph for changed cost, if cost is changed update it CHANGED = set() for xi in self.CLOSED: - if xi in self.CHILDREN: - oldchildren = self.CHILDREN[xi] # A - if isinbound(old, xi, mode) or isinbound(new, xi, mode): - newchildren = set(children(self, xi)) # B - removed = oldchildren.difference(newchildren) - intersection = oldchildren.intersection(newchildren) - added = newchildren.difference(oldchildren) - self.CHILDREN[xi] = newchildren - for xj in removed: - self.COST[xi][xj] = cost(self, xi, xj) - for xj in intersection.union(added): - self.COST[xi][xj] = cost(self, xi, xj) - CHANGED.add(xi) - else: - if isinbound(old, xi, mode) or isinbound(new, xi, mode): - CHANGED.add(xi) - children_added = set(children(self, xi)) - self.CHILDREN[xi] = children_added - for xj in children_added: - self.COST[xi][xj] = cost(self, xi, xj) + if isinbound(old, xi, mode) or isinbound(new, xi, mode): + newchildren = set(children(self, xi)) # B + self.CHILDREN[xi] = newchildren + for xj in newchildren: + self.COST[xi][xj] = cost(self, xi, xj) + CHANGED.add(xi) return CHANGED + # def updateGraphCost(self, range_changed=None, new=None, old=None, mode=False): + # # TODO scan graph for changed cost, if cost is changed update it + # # make the graph cost via vectorization + # CHANGED = set() + # Allnodes = np.array(list(self.CLOSED)) + # isChanged = isinbound(old, Allnodes, mode = mode, isarray = True) & \ + # isinbound(new, Allnodes, mode = mode, isarray = True) + # Changednodes = Allnodes[isChanged] + # for xi in Changednodes: + # xi = tuple(xi) + # CHANGED.add(xi) + # self.CHILDREN[xi] = set(children(self, xi)) + # for xj in self.CHILDREN: + # self.COST[xi][xj] = cost(self, xi, xj) + + # --------------main functions for Anytime D star def key(self, s, epsilon=1): @@ -228,5 +230,5 @@ class Anytime_Dstar(object): if __name__ == '__main__': - AD = Anytime_Dstar(resolution=0.5) + AD = Anytime_Dstar(resolution=1) AD.Main() diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search-based Planning/Search_3D/DstarLite3D.py index 7982d44..d328f64 100644 --- a/Search-based Planning/Search_3D/DstarLite3D.py +++ b/Search-based Planning/Search_3D/DstarLite3D.py @@ -50,30 +50,16 @@ class D_star_Lite(object): self.Path = [] self.done = False - def updatecost(self,range_changed=None, new=None, old=None, mode=False): + def updatecost(self, range_changed=None, new=None, old=None, mode=False): # scan graph for changed cost, if cost is changed update it CHANGED = set() for xi in self.CLOSED: - if xi in self.CHILDREN: - oldchildren = self.CHILDREN[xi]# A - if isinbound(old, xi, mode) or isinbound(new, xi, mode): - newchildren = set(children(self,xi))# B - removed = oldchildren.difference(newchildren) - intersection = oldchildren.intersection(newchildren) - added = newchildren.difference(oldchildren) - self.CHILDREN[xi] = newchildren - for xj in removed: - self.COST[xi][xj] = cost(self, xi, xj) - for xj in intersection.union(added): - self.COST[xi][xj] = cost(self, xi, xj) - CHANGED.add(xi) - else: - if isinbound(old, xi, mode) or isinbound(new, xi, mode): - CHANGED.add(xi) - children_added = set(children(self,xi)) - self.CHILDREN[xi] = children_added - for xj in children_added: - self.COST[xi][xj] = cost(self, xi, xj) + if isinbound(old, xi, mode) or isinbound(new, xi, mode): + newchildren = set(children(self, xi)) # B + self.CHILDREN[xi] = newchildren + for xj in newchildren: + self.COST[xi][xj] = cost(self, xi, xj) + CHANGED.add(xi) return CHANGED def getcost(self, xi, xj): 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 5f01f1377fa2398784bd42b6662ee5913c546361..1db5f8f5417fa1de2b6afbdcb015ce1ea35f7edf 100644 GIT binary patch delta 2494 zcmah~Nl#oy5bk>0JZ1(07%-T{EHexbjBN}~yafXW9AX(SEK(+x$T0nYfmuk;fWUqZ zlEI1+#fq(#BE=`8NJ%*4G>1r8{y^kw4tY{e@r4|+9dk<68^B=2^0d18>stC%S5?>i zc;l;y$Ut$ipW(0J@A|2}9>#v7&F+sReK@m^A4RT*FY#htvH{TmALOOH3{j8|@p66$ zQ86Fp6})l-9zE{3e3>`$CS+FdX5O;FqLn^fAZR z$I$WpPNTCLQ_w-E;no+Xn+;e@o=&Nc#@l6!(`7p-l zxG@Y7=D?I+`a=Df0$(DVNN1*}Ez^aB@L{xFEtTQ6Ys|#rxr`u%m=Lkms2q2 z7=jAj0?|$j=3!jWD{zDHOmZoep33H8@da1Qr0uM$ChfEfL!yHejM(C$o#vJyBDnRE z=ba&{NUBdKk7wcwqLO6PUJ4{Ys1?aMoY@j2Owr&J^g&7fLD2?4mn;voRKJdV7h*2- zN0p%!oU1r*i!OOJPzOVDKF|c`^PdF%P+&sdDd~j4{8q_|a)!)AIgu_TR*`sDj}spX;lmmSMuMN|LO~nCk5L#X~;C=f0T zxUeib<+CF_@R6*k{S?OYTeWw}y(*bf5}JnVX)=5^*5-|G5ERlu{^e5%E58mC#8fuGZf_Fd*ojn8R2ynH7VC z)oaMTg2>WEBQAxkONNsd7XH8w+$yxbUuR6f->2(?Ml@!X=6|Leby#Q4)2?roJ% zo8F7iTeK9BCN-)bEWm>%B7@vEIo3QdNWQ*Wx1BDlpsK+N6;8!%y;iv7r2M7%XxF>Q zam^I={KR5%)m3BZRk46PF;4ZNT{o(hg|fE9y8&`rd6O~RlXnkX6QC*M~kk;(sQv~ zE|$iJwM_P9L{db&f}A0A2nos?2~+f3m={B|S(fo|6ZFda;TCY@R`>@Pmou#=CRRx3 zAlXgdMY2*>64+6>u+#g3zn3)EOr#T-vuy4r`5i&ajjk0MNeE>k zjAWbM$~JZKZR)PuUfh}yja#Z{!&O*B+L2f=YN^9su7)wH_zh$|M+Js5Dkv^i#MRSF zsl`<<2vH4za0A$)7%Q6y7&p+v~McoWyBT>Nfs&wrP_7L1~({iAhRRnzSWFP*X^4&$ijvUWf5E z4JB175^+F6G|a84P|JlA2jnYUIdDUqSbHnO%H{ z47zW@23h za3J>A>b@{HA$zD2bfE%=$mfa|q4b=rp&J ziZJDT9JUb_WQ)VBzMb7dnAMf*f!k|7M_u);GODhj&I;N90Tk%lhTnxt!jH%el$MrE zo4YV2_EAO^c~a5pL=Or4|EcB5|3H=PpNqn#4om!=RI!GAs?CggDJK+g@No8C^d42!WG3xGJ&YR0|~hG|*t zXAjHQ;@S8!c<(~F$S#y{lm*qvg@OyqV!yl}KLU5;*YOYFVym?0Z3w(XPtVC8_g;ga z<#c}*%Cgb_RiI4tWqB&G2WI7?WVc*OXg$9n*IbYB#sPvS8IXTK+DX0w(`TW}({3rY2$c zB5BCKp#A?{AFE;MD~C{b7hB3={3LEMoa4-`kNg)u;bvj#iuoptm|$MHWC zn-#QpV?c5v3kT$TMrzG-I*6=yh{aJt;}ocGyfzUR33**<67oEHYrjdiWI)`(;A->p zcByX8&%3H))lJuL+RKM0#XBTOQFxKU_8AsboL<+D$51%pEaj+!T>p}=Z24!t?-Kss z-l&-;$w_k(gNA>#5zu^^UsLg?YeC(w=->K<(ObhAwL=+Ni MCzeaYGZSNf1Ay;^od5s; diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index 0cd9905..5c51c89 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -39,23 +39,32 @@ def heuristic_fun(initparams, k, t=None): t = initparams.goal return max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])]) -def isinbound(i, x, mode=False, factor = 0): +def isinbound(i, x, mode = False, factor = 0, isarray = False): if mode == 'obb': - return isinobb(i, x) - if i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] + factor: - return True - return False + return isinobb(i, x, isarray) + if isarray: + compx = (i[0] - factor <= x[:,0]) & (x[:,0] < i[3] + factor) + compy = (i[1] - factor <= x[:,1]) & (x[:,0] < i[4] + factor) + compz = (i[2] - factor <= x[:,2]) & (x[:,0] < i[5] + factor) + return compx & compy & compz + else: + return i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] + factor def isinball(i, x, factor = 0): if getDist(i[0:3], x) <= i[3] + factor: return True return False -def isinobb(i, x): +def isinobb(i, x, isarray = False): # transform the point from {W} to {body} - pt = i.T@np.append(x,1) - block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] - return isinbound(block, pt) + if isarray: + pts = (i.T@np.column_stack((x, np.ones(len(x)))).T).T[:,0:3] + block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] + return isinbound(block, pts, isarray = isarray) + else: + pt = i.T@np.append(x,1) + block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] + return isinbound(block, pt) def OBB2AABB(obb): # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 @@ -244,7 +253,6 @@ def StateSpace(env, factor=0): Space.add((x, y, z)) return Space - def g_Space(initparams): '''This function is used to get nodes and discretize the space. State space is by x*y*z,3 where each 3 is a point in 3D.''' @@ -254,7 +262,6 @@ def g_Space(initparams): g[v] = np.inf # this hashmap initialize all g values at inf return g - def isCollide(initparams, x, child, dist): '''see if line intersects obstacle''' '''specified for expansion in A* 3D lookup table''' @@ -277,7 +284,6 @@ def isCollide(initparams, x, child, dist): return True, dist return False, dist - def children(initparams, x, settings = 0): # get the neighbor of a specific state allchild = [] @@ -299,7 +305,6 @@ def children(initparams, x, settings = 0): if settings == 1: return allcost - def obstacleFree(initparams, x): for i in initparams.env.blocks: if isinbound(i, x): @@ -345,11 +350,9 @@ if __name__ == "__main__": obb1 = obb([2.6,2.5,1],[0.2,2,2],R_matrix(0,0,45)) # obb2 = obb([1,1,0],[1,1,1],[[1/np.sqrt(3)*1,1/np.sqrt(3)*1,1/np.sqrt(3)*1],[np.sqrt(3/2)*(-1/3),np.sqrt(3/2)*2/3,np.sqrt(3/2)*(-1/3)],[np.sqrt(1/8)*(-2),0,np.sqrt(1/8)*2]]) p0, p1 = [2.9,2.5,1],[1.9,2.5,1] - dist = getDist(p0,p1) + pts = np.array([[1,2,3],[4,5,6],[7,8,9],[2,2,2],[1,1,1],[3,3,3]]) start = time.time() - for i in range(3000*27): - lineAABB(p0,p1,dist,obb1) - #lineOBB(p0,p1,dist,obb1) + isinbound(obb1, pts, mode='obb', factor = 0, isarray = True) print(time.time() - start)