From f0753b48360af379ee4a2e4841799d7519dfebe0 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Sat, 25 Jul 2020 23:58:53 -0700 Subject: [PATCH] 'rrt' --- .../rrt_3D/__pycache__/env3D.cpython-37.pyc | Bin 5164 -> 5165 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 5796 -> 5797 bytes .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 5382 -> 7020 bytes Sampling-based Planning/rrt_3D/rrt3D.py | 9 +- Sampling-based Planning/rrt_3D/utils3D.py | 136 ++++++++++++++---- .../Search_3D/Anytime_Dstar3D.py | 2 +- .../Search_3D/DstarLite3D.py | 1 + .../__pycache__/env3D.cpython-37.pyc | Bin 5166 -> 5175 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 5721 -> 5722 bytes .../__pycache__/queue.cpython-37.pyc | Bin 4836 -> 4837 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 10428 -> 10429 bytes 11 files changed, 112 insertions(+), 36 deletions(-) diff --git a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc index 66c29a4029290839af1314c3eaa8022039eed5c1..71f8a09483e3e311344431fd9b647862e24febb0 100644 GIT binary patch delta 108 zcmZ3Zu~viIiII<58NKo0*rc5S5W%8sm~$oL!P% z5aXU%l2MwZ8|C2}6OdSv5s;IZmj_f7Guf9(ZnGQH3Vz00o8Jp&GO~FC%`D=aypU6P Ia-r}Z06P66O#lD@ diff --git a/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc index 754c2e4d3344a5fe67cc65d1bbc8462f92c0fd37..c82ed3ea42894ad6e3d8c553d95253901678cfe1 100644 GIT binary patch delta 91 zcmZ3YyHuCUiIMlxF5;=A|n{W#pH}xTF?mm*f}3 rxM!ARlqTs$dHBWzB$i|Z 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 cadb6c787d9c7f8092d539bb3d723ba45caae8db..9d70ca8ab5f3d59a1e4b87f44e2c2620f074f1ac 100644 GIT binary patch literal 7020 zcmaJ`NpBp-74GV7dWMT=iWaHO8p{f;F)Uh(;mCv{%Q7v)K_Uy5ECmxfljc-&IK$ag z-Bd`T2^bhTm&AYqQ(XP*jUNJ@6A_F*LGgAtbb8s_$#1t15f-P6w;ESZB+%{cH8baRY#aUx9akk zSM~VqSN+Iu6}E%OLs{gq6pieN9U+}*Ye(=rw>{b!tB#@WN&m4`9hU_e;C)0EWeM*I zIU+~#enF1Oal9wx5jlbPQTc+L#Cu8}l~Z`XC|{Jv@IEFha$3$j7S(BaT%JJBj65k% z;eA}5mM`IbLY|RjyiYd3_p&WtmS;g-kgv#d=shLBA!qSE{edN4mFFK@@_h8tz^R^* z;;vPB?I|>1-L44jtEk@zlg%@4%+1mNyJ4r_Zgv|Nmclra}N??trT+k;fwzj){?S)E(8x=>3buBiuCz~p&|JqU`Ao=?9`uyT2airqKD{o)A za%piPidU0ff3eX_R@Rml`(d(jp`S@|A#68#3Im;Z@j1dSDwWi(Ec}0sWJHT=NwXba zS(xi@XulC9ABP*GU@3M{SYl47F}&*}4gE-0#4+pY+o&|GEwO$hwHB;TuWs3?*tQa} zZR2rLTS9Z6Lu*p(40~+p40}jlguREQ2hi~z&CxfW`Y0-{_QQTZ>PkgTMex^7C?&M$ zFSUF1)mXdlzI*eg7ERWpDTsYtfG9Nd0vd6E!V<11iK{}rhkY;Uuee34Ccm+Q=zE7&km#$=DUSTVzr?6)M_~`cc}NQmFaxP-V#hDb}XRQ17^j* zRyPwrwFclC_HtJ(b)RPyq&IEdwUPid7#qH&59tbz_dZP$mGK0MGz28yTew^sfpvhdvk{sw#C|f-l-RG%}ixNtp*|JYEN7 zM)L?X{$3hcxXaEU11Pz;c`S~ia&x(CbW-ju#YtFiM|*Tld_Sdvf%cTpcJEfjRwt=z z5(QxQGS#1}sM8pI3y<*-dt9^y6hytbf(U)2Bvw#@%3j~dL@A@ssH9IS>3(Ogw`4R6 z=rbxYH0RMisFD@*8I>3smsDcZVWO(R7>j%stQZG-1+{y(*!&Ql@xTJ_2Zb&YyclfP zF(D@Ju^_K2)H&4ZBqRN3R_<9pg!C{|7RQXCZEMTfa!qUizr#HOvKa|^k08ldKHej| z)Y-SAIHZZ3+F?EHQv1?PJ*MY!%0i3ixty}5sYdH_N$rzVCz$$F=JZ&HS~v#32~ zZ57gj6szKiP-o4od@j#IYdDu@p`Fc5UFjW#8$7YqB1MBIqC1bZ+*J>2eVNJ0`zQ2Z zso_v!uJy7vM=b@Rrv6}f%y6K^r99T>bW_oZzjBWX;(=uPjW@pTVHbr6A zY|uPT^IZx|47Wl8xDfR~VARBQAARgP13~6lb(NMbwmYK4Y8fT1Ybd$TPy%vrl8hGL z$Y|-ChL-zs+RH@C-^CjI)oh&mQ0KmU{jFfg1Ng%zpjDtZTA-PNMEG8?>Vk`(<|{+C z=dR3h6F1q)W-avc%6JdIvuEY6o>@8LZ=W0oS2k-gmqv?5!+UtP)eTx%dH}zlfj>`` zZr!6C`3~~DrC5vikvz--cS#?OWNr)r4}D*dj;-rPgRKMK*SFS_ml)S zF&I-qU^(+ZcOc%hXDwHpH;?_gc+GP6mFjKK=E$eKlOEmzS5Sfc&~%pO0=tyTVw1p| zYAtKR0>&v316rlsaIj@y3#hqBx~LB+u(X%S0>hTzmYq5S7uaUnlu|7mCU~=>^qcp- zim$Fxog7h*)_d)>PPayh!769R>LLvi^OeG`-qaPMc6y{W6~$Gz*OqsxPS}kPYOFvS z`{?G)&oCk8bEtjJp0a0cU#M@PFINefD_6PYQ@z2yLkR?BYT95Ars z^Af@>+jp(?w-QnVjrarPmPoVCZn;T;W~GklPZ6I7u9_dtc{u;TT0h5Q0qAEyUu1fB z;HXwk)dJNlQMPB*WTPCGy;7p` zUa#FoBK#Aq8q>Ed2k{x{S%9d03|Z{B80C`QieQU)WG&K=fFtn7TXc?*^L;O9Chk*KWAp zj4P!*CpB^3h({TudIyE}p=6w1wcADdugcsNsz#^T-K!w=J`LFG3ilWA_h&)cjY7aU z`6b9=I_z1{0%iCUVoE4Fl;+9YK5fil1RxVL!^0BScLeqm2S9;tQimg?3!S)h?5T*` zV2pfC-J;?H6gr5Ls2?{sBNkvEHY*rj!0?5{YmKxZuXJw2{Y;5O5NvOR>*y)56r@JOxIqe=yFMHj{#HuMwQw)w=xxi0;V5xfbP|5r1 zHj#Zq1;=;-hx!nO_RYztS|U93d+^<7pyJZdZ$TWNKw$wRKK{+%aTznCz|0ICdG28} zHv%06NIv!yo5l`^4>=Uasen?&iVqbLI`pI9Z4PI zUVT98)vt<4iMBJSs|;j%4k$JbFAlBq?I zWcOO;X3mRo@tQnO3udLi4i~qCs-sR%{-clHz!Q^IBEH(LVwWOKR2_-KB;Gg#O1hr* zvg>JI7J;miE<~~s#Zi*$hs;b1GN>u6Hld%ICCM=n)`hx^KH9h9sJ(o^X>?Gl(LGbG z_74zcR@5HUHB!RPA_Ii1!aI`0Br-$M>8Rg68QxB|%quB*CU|~4tSTxkK$Dh~Y zkrv%&aDrQA=J?%QZ@Sltl4&JZ#%Q1Wr&0T<#@=m$lbj>-0Cw<~z#uIsB!h-CW{b z7dOuvN@=?n15_I^2T2xO7x1?UVnr8l2BWx2TrW3?)9R%8)52W8-Aiipr-LgCilEPb zCg8hulPl8|EMFv=teI{SI6vlem{V5H)HwU$-Iuo{MUjyMgZ6t%EhOSB&SBE);F?k2 vb@KF>4nDZw7%Q6rR~#uh*{QI?kV+pXR4+(hW^WX-ErHp>5JOYG_DlX@r$3I9}Iw;@HW| zmb&d)IXL3yKt&jFf)IUpfb>VXp+5Z~LhX}i{Z^WMyxH}7wr zA5DBX96#UG6lM5+i}Wo0)w~@~@jteQzMkrEQd=z3;iR2T%&7u%*e%WJa=N#8ChYV$ zhp?JKOm^)YjDNi|@+3kc%_7OKYV{p)XUc z-d>!}%T>3!zIdtR7H=<)WM-}`&S%}?d^uaGKxk2&;~n2$&>nMMQLh@A1lzGZz6-VP z9;LoC?uSGecB`Jy6pyNG=rQk8SIyrK^kY$k2nN)6`0VCkkU1SdUj&2ph?hSCT=p^Z zw1!sKUH*hJT+L%E`iep8a*&*g+3^Bf)rI(o-Qte!7+aQSPO}EDTW-v= zNKu0eL56II1?GehT~n--HB6F~iudd}?QOYg3dFjebQe^Kgl`1ZZ zC7*Mbvf1TjKeU`Jmu05)#VxsB%!|CQ*JS zf^Sr+V$CYw`K-uSFc-73cyih|a#d7B zQ1?1K8*A;~JP+|Z7?RS@v;;B?|7i7v(2?&!=rSO1EexsawHdStJ>YaQO3s5e&?fW- z5?qI2JEj{(+tNLwpzWf0aWop2T87ckPO-Y?sb5-K&PVs2NH0|H`S2!DX9>;`BnW~W zQMcQ(f3FC9gIcB-R^4s==}m0PIDiE;8AlCJsfr8gM%(>lpz0G62}+cUbL!8wnkJ5@ zkx(i$h>ZuT(LTg4s&CrIn?8L`@UnW;Ue|qoK~*~X`RD4h4tsXLv@b%?Wgd6naRZG8 zltz>W@9MfoZIrK?VuEH5J3ELN#i0`O^a7_<^iOIuhN#cT_z{r4uf6Qml70P>8zvHr3v%i$D>n^i7dptA3Ctg{1s#fI5-tRR5%6 z{3~UqM++3vAhVS70;}`dX;{V3qetsn?HJKyH~3EY!*@7xjyoyy4Sow*= 1: + d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]] + if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True + elif 0 < t < 1: + x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]] + k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]] + if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True return False +def lineAABB(p0, p1, dist, aabb): + # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 + # aabb should have the attributes of P, E as center point and extents + mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point + I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction + hl = dist / 2 # radius + T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]] + # do any of the principal axis form a separting axis? + if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False + if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False + if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False + # I.cross(x axis) ? + r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1]) + if abs(T[1] * I[2] - T[2] * I[1]) > r: return False + # I.cross(y axis) ? + r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0]) + if abs(T[2] * I[0] - T[0] * I[2]) > r: return False + # I.cross(z axis) ? + r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0]) + if abs(T[0] * I[1] - T[1] * I[0]) > r: return False + + return True + +def lineOBB(p0, p1, dist, obb): + # transform points to obb frame + res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T + # record old position and set the position to origin + oldP, obb.P= obb.P, [0,0,0] + # calculate segment-AABB testing + ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb) + # reset the position + obb.P = oldP + return ans + +def isCollide(initparams, x, child, dist=None): + '''see if line intersects obstacle''' + '''specified for expansion in A* 3D lookup table''' + if dist==None: + dist = getDist(x, child) + # check in bound + if not isinbound(initparams.env.boundary, child): + return True, dist + # check collision in AABB + for i in range(len(initparams.env.AABB)): + if lineAABB(x, child, dist, initparams.env.AABB[i]): + return True, dist + # check collision in ball + for i in initparams.env.balls: + if lineSphere(x, child, i): + return True, dist + # check collision with obb + for i in initparams.env.OBB: + if lineOBB(x, child, dist, i): + return True, dist + return False, dist + def nearest(initparams, x): V = np.array(initparams.V) @@ -92,13 +165,13 @@ def nearest(initparams, x): return initparams.V[0] xr = repmat(x, len(V), 1) dists = np.linalg.norm(xr - V, axis=1) - return initparams.V[np.argmin(dists)] + return tuple(initparams.V[np.argmin(dists)]) def steer(initparams, x, y): direc = (y - x) / np.linalg.norm(y - x) xnew = x + initparams.stepsize * direc - return xnew + return tuple(xnew) def near(initparams, x): @@ -106,7 +179,8 @@ def near(initparams, x): eta = initparams.eta gamma = initparams.gamma r = min(gamma * (np.log(cardV) / cardV), eta) - if initparams.done: r = 1 + if initparams.done: + r = 1 V = np.array(initparams.V) if initparams.i == 0: return [initparams.V[0]] @@ -118,16 +192,16 @@ def near(initparams, x): def cost(initparams, x): '''here use the additive recursive cost function''' - if all(x == initparams.env.start): + if all(x == tuple(initparams.env.start)): return 0 - xparent = initparams.Parent[hash3D(x)] + xparent = initparams.Parent[x] return cost(initparams, xparent) + getDist(x, xparent) def path(initparams, Path=[], dist=0): x = initparams.env.goal while not all(x == initparams.env.start): - x2 = initparams.Parent[hash3D(x)] + x2 = initparams.Parent[x] Path.append(np.array([x, x2])) dist += getDist(x, x2) x = x2 @@ -147,20 +221,20 @@ class edgeset(object): self.E = {} def add_edge(self, edge): - x, y = hash3D(edge[0]), hash3D(edge[1]) + x, y = edge[0], edge[1] if x in self.E: - self.E[x].append(y) + self.E[x].add(y) else: - self.E[x] = [y] + self.E[x] = set(y) def remove_edge(self, edge): x, y = edge[0], edge[1] - self.E[hash3D(x)].remove(hash3D(y)) + self.E[x].remove(y) def get_edge(self): edges = [] for v in self.E: for n in self.E[v]: # if (n,v) not in edges: - edges.append((dehash(v), dehash(n))) + edges.append((v, n)) return edges diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search-based Planning/Search_3D/Anytime_Dstar3D.py index 026a1c0..3bc1ad1 100644 --- a/Search-based Planning/Search_3D/Anytime_Dstar3D.py +++ b/Search-based Planning/Search_3D/Anytime_Dstar3D.py @@ -197,7 +197,7 @@ class Anytime_Dstar(object): self.INCONS = set() self.CLOSED = set() self.ComputeorImprovePath() - # TODO publish current epsilon sub optimal solution + # publish current epsilon sub optimal solution self.Path = self.path() # if epsilon == 1: # wait for change to occur diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search-based Planning/Search_3D/DstarLite3D.py index 47bdd89..7982d44 100644 --- a/Search-based Planning/Search_3D/DstarLite3D.py +++ b/Search-based Planning/Search_3D/DstarLite3D.py @@ -61,6 +61,7 @@ class D_star_Lite(object): 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): 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 727f29d6fa49ac32aa91b285d6071a26274080f9..4b7036f4c843d9728a1524b65380143ad1af6421 100644 GIT binary patch delta 279 zcmZ3dv0a1LiIwKlJdBLalRvZjG1^ad;%H;^ntYPu z17q0aU7YERL6eoZrZ7fr-p&=w#27!BoA&}^-sC&HMT{kz{rQww7;jB35_04&2ifQa zvXOQ2J|QPo7DhcriOGz@5sW^Qql9II<3REtZ*nkl@pG_oFv~GcZWo>;%E}=Cgd76g P0^A%xAi=^H!odgtV`4`A delta 270 zcmdn4u}*{6iI`Jnnxq@$;Tsc>SdtNtlbDwWR1`DWk4b*AJCh|3$hcw_keQrioBNnz85xx( zzhTZ}6q_8)a)VKIvk7Y;BctKuHnt>2oy{-Vco-S2CVyl1V>FxW%F)K?F!>C}2S)G7 zdpXk?-6yMZO=0xgyo)QCi7{w0AMXXm#L4%0ix|^42k|MfFh)%-6>{Xx0@>#VvX5o* zK_MqbugM(35kM0Xg=GW-L2?{Gf`gHZpM#BqdGZ9|NutagJRJNS>;hZ@Tpa8i>=Mj; HK^%+#;f+OX 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 8478a158b418cd5d64a96e77e66fae7cc8fd27d9..af8baf399a72f2b2dfb3918584cefb970a4ad84c 100644 GIT binary patch delta 92 zcmcbqb4!QYiIIV{ByUl5a?S&~tj t6jP8`lA&9WlbDy6nU}7cn3JAg1eC}v-Wjd4jW&MwI> sh;h#>$tX?Ijq>n~2}mr-2*^py%L6Lf9LqFWi1ijzQN^vzUqwjd4jW&MwI> hh;h#>$tX?Ijq>n~2}mr-2*^py%L6Lfe1=g{5CHlE8xjBj 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 2ad09522f987f6c4da357493cf493e9d68224eb1..5f01f1377fa2398784bd42b6662ee5913c546361 100644 GIT binary patch delta 81 zcmdlJxHpj7iIjd4jW&MwI> hh;h#>$tX?Ijq>n~2}mr-2*^py%L6LfT*Kt31_1vW8xQ~h