From dfd40f86cd86bf6c67e774693ea18ff6bee11961 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Thu, 9 Jul 2020 03:01:40 -0700 Subject: [PATCH] 'utils3D.py' --- Search-based Planning/Search_3D/Astar3D.py | 31 +++---- Search-based Planning/Search_3D/Dstar3D.py | 52 +----------- .../Search_3D/LRT_Astar3D.py | 14 +-- .../Search_3D/RTA_Astar3D.py | 4 +- .../__pycache__/Astar3D.cpython-37.pyc | Bin 3337 -> 3178 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 4261 -> 5537 bytes .../Search_3D/bidirectional_Astar3D.py | 16 +--- Search-based Planning/Search_3D/utils3D.py | 80 ++++++++++++++---- 8 files changed, 96 insertions(+), 101 deletions(-) diff --git a/Search-based Planning/Search_3D/Astar3D.py b/Search-based Planning/Search_3D/Astar3D.py index 12b5e2e..da4c5c6 100644 --- a/Search-based Planning/Search_3D/Astar3D.py +++ b/Search-based Planning/Search_3D/Astar3D.py @@ -13,7 +13,7 @@ import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/") from Search_3D.env3D import env from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ - cost + cost, children, StateSpace from Search_3D.plot_util3D import visualization import queue import time @@ -27,6 +27,7 @@ class Weighted_A_star(object): [1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 1]]) self.env = env(resolution=resolution) + self.X = StateSpace(self.env) self.g = g_Space(self) # key is the point, store g value self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal) # self.AABB = getAABB(self.env.blocks) @@ -44,13 +45,13 @@ class Weighted_A_star(object): self.OPEN.put(self.x0, self.g[self.x0] + self.h[self.x0]) # item, priority = g + h self.lastpoint = self.x0 - def children(self, x): - allchild = [] - for j in self.Alldirec: - collide, child = isCollide(self, x, j) - if not collide: - allchild.append(child) - return allchild + # def children(self, x): + # allchild = [] + # for j in self.Alldirec: + # collide, child = isCollide(self, x, j) + # if not collide: + # allchild.append(child) + # return allchild def run(self, N=None): xt = self.xt @@ -60,12 +61,12 @@ 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) - allchild = self.children(xi) + # visualization(self) + allchild = children(self,xi) for xj in allchild: if xj not in self.CLOSED: gi, gj = self.g[xi], self.g[xj] - a = gi + cost(xi, xj) + a = gi + cost(self, xi, xj) if a < gj: self.g[xj] = a self.Parent[xj] = xi @@ -87,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 @@ -118,6 +119,6 @@ class Weighted_A_star(object): if __name__ == '__main__': sta = time.time() - Astar = Weighted_A_star(1) + Astar = Weighted_A_star(0.5) Astar.run() print(time.time() - sta) \ No newline at end of file diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index 2991238..3534379 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -3,64 +3,14 @@ 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, getRay, isinbound, isinball +from Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, initcost import pyrr - -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], diff --git a/Search-based Planning/Search_3D/LRT_Astar3D.py b/Search-based Planning/Search_3D/LRT_Astar3D.py index 528bbbb..dac6c0e 100644 --- a/Search-based Planning/Search_3D/LRT_Astar3D.py +++ b/Search-based Planning/Search_3D/LRT_Astar3D.py @@ -14,7 +14,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-base from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ - cost, obstacleFree + cost, obstacleFree, children from Search_3D.plot_util3D import visualization import queue @@ -34,8 +34,8 @@ class LRT_A_star2: for xi in self.Astar.CLOSED: lasthvals.append(self.Astar.h[xi]) # update h values if they are smaller - Children = self.Astar.children(xi) - minfval = min([cost(xi, xj, settings=0) + self.Astar.h[xj] for xj in Children]) + Children = children(self.Astar,xi) + minfval = min([cost(self.Astar,xi, xj, settings=0) + self.Astar.h[xj] for xj in Children]) # h(s) = h(s') if h(s) > c(s,s') + h(s') if self.Astar.h[xi] >= minfval: self.Astar.h[xi] = minfval @@ -47,9 +47,13 @@ class LRT_A_star2: ind = 0 # find the lowest path down hill while st in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop - Children = [i for i in self.Astar.children(st)] + Children = children(self.Astar,st) minh, minchild = np.inf, None for child in Children: + # check collision here, not a supper efficient + collide, _ = isCollide(self.Astar,st, child) + if collide: + continue h = self.Astar.h[child] if h <= minh: minh, minchild = h, child @@ -76,5 +80,5 @@ class LRT_A_star2: if __name__ == '__main__': - T = LRT_A_star2(resolution=0.5, N=150) + T = LRT_A_star2(resolution=0.5, N=100) T.run() diff --git a/Search-based Planning/Search_3D/RTA_Astar3D.py b/Search-based Planning/Search_3D/RTA_Astar3D.py index 88c18e0..818dd24 100644 --- a/Search-based Planning/Search_3D/RTA_Astar3D.py +++ b/Search-based Planning/Search_3D/RTA_Astar3D.py @@ -14,7 +14,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-base from Search_3D.env3D import env from Search_3D import Astar3D from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isCollide, \ - cost, obstacleFree + cost, obstacleFree, children from Search_3D.plot_util3D import visualization import queue @@ -48,7 +48,7 @@ class RTA_A_star: while sthval < maxhval: parentsvals , parents = [] , [] # find the max child - for xi in self.Astar.children(st): + for xi in children(self.Astar,st): if xi in self.Astar.CLOSED: parents.append(xi) parentsvals.append(self.Astar.h[xi]) 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 1e8e448c74035f6d2d4992f4ab5777a5154cd60a..d3052cbbc000654f550a1d03d73b38b691999da7 100644 GIT binary patch delta 1340 zcmZWpOKV$46rP!@`;_E|>{w4bekF=r6Ven?r}ZX}(n5n1>P@*&uCFzNE%{2ijBcm} zGG54~bW>txQ)mftD2492?oa48yCI9NOKG-UbdhteO43k?`F!R(-<&y*k-w?@QOe)V z=RE=+_S_2oInS5r@8|WwUZ?06J0-tF38&nO%AJZ|No^KY{VH%Qs`<5#3Aed(Mz|vu zM~1&7E_3&Y$rqwFu3Zp+SuAmHMEw=9GG4tPl!M=YQJ{eZgaBYJm1JI%vZ#HHwJ zZ#M}Ou^Wd?p}wZ|TQ?vrL43-I&v=$IZk$oy;5lw`3#`oKa^K`G_s)oK@ysr17}FcWJ?Qp--9A!*yNjJ7*s|JivX#t3L#SRQAndhG?}Hc7Joo&+Um zaxGNnlc^E@ojyRE;U?g0Y8N2JrzDtzLlCm+Cnvy}EWEMcv&s8?_l`{+W&qUVM zcbV@C8?YgC8=B(4+G07X?wPmLL#uf0Cra=Fa)R~J6XOXz{%nXgJF=fpxd#?-a!-A0 zeNey#42=;x$sGSU%-knm{GhZ|w}Bg)BSTsCyYw%$W#6lBBn1os4W|o)vm*u}HXh+c zbKH2%JW|i>+w_@wVV9SlCLRumlB6=O;zQx)F5wnT)83KOYR-A*bOnl#%LrA3H2`fK zh(y-VvcfnPT`mjgn@fEm=Q4*|eBJUAf{RcF(5~n{?g$C5O|BzZLehvtS6)WTjAg5v zXrrGFD1FtiYy0e=1*@%T zDC?k@z_m1reB9kq)9xyrQ8(SE+v#1)sM$ooR2!Kzyu1mvrpIyt9dCz8efw(4`{iL; zW-75!w~TV3Wx2Bk^`bZA#G>Nk`?g*WD@9(w&B_?q{!@Hc)*V0W*>x zBD^`AT3DLQ17J3N2f(4;s<+`}Q&u}c&}NA_gaSrzB1pQN*2MP(*wb%eHh r`)$!o(iG%2@-W^|n|A=^q$2!(^S@%$^W1u26Mi)gb=XW3F#Wy)w8%0o delta 1511 zcmZ8h&2Jk;6rY*>^jE%{*iM?XA!*_QA*i&JDx^w_1P2He9Ew)Z)y6YQY;SBc-iidR zm4m~9TiUq+LUsiQgoOAT5Pw1sXwE%V72G-T-nfK@wcc;v*SvXO>!-!9%Y|EoLYBbu z=kIR#^MgW({_$Yt@DoZnupk!t7Y2k9adBu7^AG9!)KeeOTk1!;vUwfu6(pdX1dJCrm;^Q-F}Kjb;TfIC#qlFajy;0Zq28hYoFd=KA+UBnR%k;75Lk70~(9L_+*y3cN+ z!vxS|GSxpfKxrBb)BPDh(3_+tN}#W2QBxu_WWXrOc&ezLS(j!@nl;s zgIC}>@(%fIv1=vJuau@%&-#>g?XHvBoqWprEOiEylF##ZNy?zbbh~272{qFfnx|uc zBlOGST~XWJO9T(M!cG#&wSV2WPwUFS5zNHY^#di?K&T9mcCC{cdT=L2fi=7nD(?Yj z;afd%Za|y+xzrlau6^(*wV?~&sC9S6u~MhU)IIlg`b?d;w^vpZ92bLqqSJ)427>^r zckrG`Zk=kj)!*)0vwgD>FyemEjJ%Y>A~$?E@txZC=4e4RyqAv_AYVC;P(ru}psihz z$TEsJM>&QtZVVCU5zi$@|SW zRm)$bBkH05{ieCb6lV&-EGxE&EUyADD+p5vXLrF^BNbhb+3X~(T3uI;R~58LWy zEz>% 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 890cc05c4b865badfb95c71e04783463dfbcd2f0..698ea5248d79ebb85e8bcd59e481ac04a8e6aa21 100644 GIT binary patch literal 5537 zcmai2O>o=B72d@kLF&h{t=O{TB#7fArs61e)Vitb@x+eC%}f*3jofjDqYMjSArcY^ zQVS>{HQ^XjZxM!TS1Q0LGts=7L_R=$*t zQ|f}ch?ynzqPm3kw0cRsjP{IrMR{n?s#W!>x(v)^^_seZnHR!y>h*m?|3$r_-c(oB z)t1#bA1;Dg{8}c>V9pHBk4?}mw?zG|C*V$O)@44Y!d?Hcld3341A}I9nYc+1AK*@I z17uRk3nIhf3u1*v29E%FWbz1b1+Nl^Q^t7AMsQa3dbkz2`uPIEIk>_U!Kzn9!#_%6$>0CR*_0pS}_l2$QDShG@}M*kVQ zE0|!q@1T>46nbM#v%XivSn|vajgy%fKE{%0eyV3CmdUOgdJ)^^roWS*e+qrQM1V9r zLun0^e(c9YED3f>>{;|nVjT=f6in<*bd=mSM#jh-T+PIW_{*_p8~ z^defz5QKHG_()_Xh%}B~#oN5J9eOcLB}`JU-;3hZ>%~0T(vhkg(LW$@xCaZY?&S7X z-w(pv=%)>*8^tP08aCQK^w3X}+{DE5nA$AfK#V8EnJ`>g!_SqAQmxoqch|0Ltp55q%^ zZKHrVOvp2YVS(cfsb5Avpg2GmwWUh0VE_fP^^M^V=Zg{SY%I^idnRM67=sJDh$`Pm zZ-#6MzeqLoMs|IYFsJP&?e&dU@MvmcucH49J;WgE;37KY#|pH33A&CA1!-=8nu&Lo z%gD8ejV;^>Nu~w#&*n!G>_GUxG7hY*f)<2Zd$-f`Qwo4<*K--2An}{p?G7Xt^t$~! zuhDk2wdEb---*(1lR>f5<>}@^>Nn9Z?T}sw z(=TL@qB9#0EIitm+QS^I4Gz|iaS+BIVO++83C;%QpsNxGPO5Bg2>AN?5y3NWdIZo=^;Yc%KD_m~w^YpQr3tvrxp>hTc}rmiSu6x3HJBZ)2}tPxzc1TNn}Dk|3l~ z@F{60mWrFXOf^GL?J_n)rrKpL8S?nVo8vz!nN`?`L52Q3Xb?mdJHu}ZS!b0(GQ6H$ zfoGI`Wdd#XY#jcY#%UDWxrChV@SjGO>sA5m1gy zauZT^W-_A&=cYVbA(yd`4zT;oV6brzK4UmAN`uAqbPvR(0-rJ5Q8hJ=O{5oasK+>i zQ~R+T)p*WWsx`Venp7!%2R9v^ZGJ=Fq!5IyJbI&7-$GTFMrV zE7@XpDqGscIU~ivpC5^XKjHoZ5|;%it1_dkLQi!FqDT)ZuGHP!<#^}oVO~RG)9WCM z?ZrjX==j=?TVd|_NjmI@nzVqkGDHI7D^)+8n_>KU?(F(O+S8iyEB!q}u?9$U2>OHE z8ghTg{Uh!laeshBzF`d+9szQGP*?4Oo-UEnpa#*zsGa5!_LqftPWR? zuC04tkfnQ}AGE!7dgM>?%P96xP#}3CRx}k=g=Wks-mP1CEsCSG4=r_*T(%||*5@bX zR%ta^Xg|U7gz`mUS}Y{wlRv_!JWNRLbW(nRJNX>|#DhF=Y#^WKwm3|vO4&2_EP9e$ zLs+wRks%J==UE#<{LaWuoxRGjo0*yY*cdqo$i{wE-{gH9B-;q+nT7N5=5+PJ%|~Kz zlXs|MhgIxQqlA2H=&zYL^4C>Bi zxo6Y&cXo1{;#Y{t1Aw4;t{m{^xvlrwYU(Xf(sFGY!uiRKqreo|dX zLO$qqI*|$~rB5jP6~;NF!c+hdq*7oWJ+#ien3tLqTZTQ#2B>;tC4PcVs|b1=_E#T{eBp$x-%m%7t&yvaTw9Wh5j)C zGTH)gk?IymVcf@jLe(PG3M7<^EF%3WW=erjs#H4u1_pzdnLXs?pPj*X2Lm>R0#M^n z+!6;h*kSS9oIHn}^Yc}Hz6yII5ty6zAJ%OaLhkG!#okW}?l$oTHVEX`r7aXG=Lt6E zr&f7ihv5;H6|5ARoj^@C5D@uvk#0*?C?rt)V%IcL*V3Vd%njdk-pDL{IhA`LG_!G| z(8KIK%)VF5rWE1eTu4q)f`FN)r5u6{*;ApuA5xD>>ZzWtpr6aVhKo}m#aEQ1Q3(oT z)ei{w1dCSCOVTkOBvcLxxdbg)II~Gx)aq@_mLZyA53$Im`w==RQc$X-V3cxj3tA(c zU1y!a5D`brftQh+*`>rrNnnKhim76SNHoQ2aMZf*wHvb>Nh7u6R2{9ah+ad z45Ee|C>_SnH}>h)zXbB+gTOpYojwWRjN73t+-9@uM{%?Hgs5!c#$~3D{P%g5srJs`9z0FVLar3 zY`V;DNo4GI!)7y={~@D9iC@*NaBI7^d#1L8@j18VR%=!C HAdmk5m-a@cP?vLemBU{RnN{Hi4#`Ac{(BLP2dRNKuQW)>?Zu$;RuC&WuY_ zt>r@^HR6KkazGUl&7mMJ2nliGh`8`4>!TabufQ3~q7@xXEpv0B&)I zCxP2M#nZqOJj1iV9iHQP;7MNKW583q$j7&@l1iE%<`ew6ZCc6j1AG$x%JPH!5bzwI z;?D!m^CSEPJ`EZLZ;Zd#VB&KK;-#S29IuR12y2Cujy{19$#Qgv-lxfEmwuKvF>oY= z&QXz$%4{Qx2?++IWC%0*kxkDeaYYj>L!4OiRN0rxfe(p_go;A+7vt~Xhrs$9zy5MqQ=Sm%QY41%&3;CqYeO zzJ!_Jq)f$h6j~p=F?2&?Ln=|C!Um;LL-x8W*Uyz-L@Rp*I~-+fe;$)7(b!rI+O8_G zn9aQ!Q-5UppQ(iaqNulG^P?t4ET6%t>* zhoL;tMzCj3(9*Rf?Hqb`48ES3OwyNrtJdze_zb8>%$LxBnpB*6;9RwQuQ)uS&_g|$C(s?wbaVSorq+yAwB`0>Ydsl46DF8w{=qkV60BWn%`K*!9nwdEk)GAFRZ?$F z;fh@5G&EP)3b}h;ucpi7=4-u#%4}pGfqsZ!mUB5B|CbYKi02Pr~*C+P(&q`SRMef2F1CV>rq7@Ub@9TWw*_h!MF_( zcN7$3y78}Rzy=7!>xb~_DX_`CX-Y2c|v-1Rh6A}Pk^?i7*}8DBAE5IRBPxJi%B47ycEP$kYlG>ZMuG|S_NES z^1#s5KAVWGR=3&N9P}1z_>Yca2m