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 1/5] '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 From 89a04e5cbd502c95066cc1b2f02c54cbd462b8b6 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Sun, 26 Jul 2020 00:42:09 -0700 Subject: [PATCH 2/5] 'rrt' --- .../__pycache__/plot_util3D.cpython-37.pyc | Bin 5797 -> 5797 bytes .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 7020 -> 7138 bytes Sampling-based Planning/rrt_3D/rrt3D.py | 19 +++++++++++------- Sampling-based Planning/rrt_3D/utils3D.py | 16 +++++++++------ 4 files changed, 22 insertions(+), 13 deletions(-) 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 c82ed3ea42894ad6e3d8c553d95253901678cfe1..7cb6a21e0bd83dd91ad15b197fa593d0fee8c351 100644 GIT binary patch delta 19 ZcmZ3gyHuCUiI-N`tici``~zLg-%}A}vtRij+((`N_n@ z$pjgmFuss9}BJUu+aR-8?@5gM?%+_tzrgH{^!ZW{P z8FA1Wc)1VdK#4mB3=#cYp97*|?Pdewx^;6*CB{j8FM&z$0>azVDicSqXQg~Kr6%T6 zd)V;3)zn9No?QI?7#~o6$OT!8&h%(&v&n4wG&_9nM*6yL?V~U+iQddm#{nXn)y`$c z*_ik^BZ}nMvEVpqSO~JMR0&thQ87;3(iKP%e}rHR!AU{gD=}U{kVa);K^ox41zrig z*@EFl@wQ0UvU}M{aVz`pa-NKa3Dhn2ler0QwB$=8^dsmb5k-;af*Xc0946X0KRF2} zk#fw&jZ3q%9E6@qVMj&9B@`gZJ?Vxxg&FvP<@e^=^^GdRJUr@ zoaXQuV3<;)Yp~g<5YmnlzZwVEe5`4slUqnv6_%4+wggcL{IVY(4*FCl!7Z~d%#PAx9QebNoo2{cEl8wyBZH((#E z=$mZaSevRDTi?beyJEmB%DA|bJG_y-hy|_bi#isK%{VPk&kr5I9)gPm?+~;TP>A|) zs^sglA^EJEEZ3)kmI=1-(Wj0ec@2-Y1z**?HnR=ewmJCXY~Pr5nT*=ShkdVYv=Nyjpw3eN zjW9seFahmkwWQTfh6(zlZ3r_27YJxbph(~$NUO52;+GN?{#kiR`&7B&t%m*?T2!$( R(eJdj*d4Z=P21e&{{eyipc4Q9 delta 2119 zcmZ`)Piz!b7@s#YJ3F1S+y8Xi(o!iF2a`hDYH0%0mIBd~g3^K(!#drWZl|4{UB4Ny z;^H2Np1hdng`VJIVoXd3ClW8jlZl=@s816U4@Qg!@LOGyx)HBz3+YB z_r8Dc=ZW8+&(35rmV)2-f&TpMfjimnS+hvFK6r&s6lmsuvRf>}x%%Da8Y8|=LD z=-)NYD|Y%?%d+o8o;w&hrApw3MPPUovIH#%#m$@a-J1d(XY+Ow|5~4(7peG4CGc!b zSbpfo6$hMB1Y{$05;U3y1Gx4Kj;IwuQHSvs7Bf|o?ZN>Ad@=O}%kmy$&$E5F5A6uH z22TnyDRDU)ov2; z;2-?tJ*5ab%{6PRx&Fwsb&MUlTeIF%jbT!AfLocptu~SMY?GNOHp$=4@H`2&4LCv^ z27=gCC5Kf}Ms$DlLp52(7%lEHv<+v6ftSoH%} zny@3H{EwD#c7i|My7{sUHsu;jkt_of$O(!<56e)3LAm5WvE+yK!r>%ss2i|^RS?CE z*J*FtS9=Nf3mO7_6igZ?a6}^rRkg};O02B0TWUhzW_`+at;(v(npV|1m2Ty_9_v+| zvfXC$sGA%QrwC3X2s4VkO5|Vjq>T->H7UxN0#vU;-w3_8>U~X%DLm@Ir$|O5RAD)m zWpIWsw%^ap;@r+iSHMX=)N!!3?VtaY+t#+5mr?}>+zT*|2jMJ%OeFFTyoewSKa70W zTe3pWflApA8@)v=I&IxSD(Xd0u&@mL<#6bXf7Ov;3tV)3Xbj>W^zk=4duo&?_2spc z-oOI(!uYYf!mG%5A4k%B)K1xc;-P;4%f2e=^K>&SQ|$bS3#d@^GP44riig7nLzJ| z&>}Ckb?}JRJ-6s39TwBRp@Gr1>9}s5SboCU)&oS|Ygeze(lSaKK^FdNZgGkf2m`!Q z`Kq@i9#%=F?72^p-kdN?1;6lW&pQ*+NDB8O`%+SiU{?hTcX$FHM|t))KGHikwL-dl zf^~v2!FF6FMQS12o?yabl@^U0YMn$ 1: + while self.ind < self.maxiter and getDist(xnew, self.env.goal) > self.stepsize: xrand = sampleFree(self) xnearest = nearest(self, xrand) xnew = steer(self, xnearest, xrand) @@ -47,13 +48,17 @@ class rrtstar(): if not collide: self.V.append(xnew) # add point self.wireup(xnew, xnearest) + + if getDist(xnew, self.env.goal) <= self.stepsize: + goal = tuple(self.env.goal) + self.wireup(goal, xnew) + self.Path, D = path(self) + print('Total distance = ' + str(D)) # visualization(self) self.i += 1 self.ind += 1 - if getDist(xnew, self.env.goal) <= 1: - self.wireup(self.env.goal, xnew) - self.Path, D = path(self) - print('Total distance = ' + str(D)) + # if the goal is really reached + self.done = True visualization(self) plt.show() diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index 2c0d7d9..b256aa0 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -169,9 +169,12 @@ def nearest(initparams, x): def steer(initparams, x, y): - direc = (y - x) / np.linalg.norm(y - x) - xnew = x + initparams.stepsize * direc - return tuple(xnew) + dist, step = getDist(y, x), initparams.stepsize + increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step) + xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2]) + # direc = (y - x) / np.linalg.norm(y - x) + # xnew = x + initparams.stepsize * direc + return xnew def near(initparams, x): @@ -199,8 +202,8 @@ def cost(initparams, x): def path(initparams, Path=[], dist=0): - x = initparams.env.goal - while not all(x == initparams.env.start): + x = tuple(initparams.env.goal) + while x != tuple(initparams.env.start): x2 = initparams.Parent[x] Path.append(np.array([x, x2])) dist += getDist(x, x2) @@ -225,7 +228,8 @@ class edgeset(object): if x in self.E: self.E[x].add(y) else: - self.E[x] = set(y) + self.E[x] = set() + self.E[x].add(y) def remove_edge(self, edge): x, y = edge[0], edge[1] From 256812d165a6e36a00b1bfa80b32c3e7c6a9173c Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Sun, 26 Jul 2020 01:23:46 -0700 Subject: [PATCH 3/5] 'rrtstar' --- .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 7138 -> 6705 bytes Sampling-based Planning/rrt_3D/rrt3D.py | 3 +- Sampling-based Planning/rrt_3D/rrtstar3D.py | 32 +++++---- Sampling-based Planning/rrt_3D/utils3D.py | 68 +++++------------- 4 files changed, 38 insertions(+), 65 deletions(-) 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 7b6c76bb2a7fb8b4cd116e28dd2c3718198a5984..59a5593938e01c33629c936d945d8f1cf30d75bd 100644 GIT binary patch delta 805 zcmYLGOHUL*5bo-E^)BqNu#5YE6%$b01QCgkgW(`1ZlZ+X3pWHI3%x-WVNEkJ9D3qm ziC*M@9dE`!l%p|b;#H$JiSY+a;z7?|_25OTn;3gi-+Vp&Rn1q`-_N}{?T@EYNrLO^ z`O^Gy@3xB|<=xXvO zg&R>NH3F~_ZTJrX?ni$*`T(|6UuW?F)zxw7zc?5dG1i|W1Y`#!>_P~Jkp@Lh14|-n zL8#w$Xd^nI6S&erl_#xTkX8@L5^1++=;A38lN=07aJoVT*ZG_l$P&5@lA{u^Bo^e* zvO+H)jkZR+^d!v_4&2(J(p@1TlT@+-2@~4NlT7KPiw1?KLHiYB2(eWUYO%9aTV1Wi z*6Q+FY~5`x#WruQ)zw7iNZv?ioW+{p)8evDyP{!GZDvjlK1I6Ti$EX=?HDdTc?{d^ z)5sZ!spw2+3aWcr%#64TF2S%RZsKO~-zY8I*cfJ%qe!qvf^CBy9F8EXFADdT1nh|h zI7FNP4II^D+~6rDe_yEk5+&-Ss&oehpH0d^KS=1@>VVT?$kxIs3O1tBL)wA`G}cizjH z*`*o$Af=>$iUNy*|JTHVS^YQj|NQy$=KsIn zy!hyH<;vKY%W%{#&fNNZ_MlRSzYi9^0{`IQrFr$VdS;(BpHk1Nd7NifnOaci_nA81 zK9`B+tbz^ZKR2wy1qW7!*GtD7ZVX>_|K!jeULI=zd@$Uud;2=ge%2kx;GlZBWtOpsS9A};iM3Gvtl~}QT=p_6I z68+PGWNFDXcE+?90l2vP^<5yf}7ZYHmwN%1Mx!3V@{0vO)OKtU$bQf&7| zd*!5*ICZ8#iBEW9@32%RP9mpRIzk~$J~{LJ%g9R;!(6oEAh$a~zaQkH-@UD!!Ws@b zxupiT+q#6hnXk^(n|3SE>P@{)XJ02+Ah>GYslKrHEiR*J1O_fR_|;J?D5F+}3DhL$ zH*ohfO#&4QrAX5yT;Mw(27D;*i41ml2D(l~LW`9Evga`-C*c{7odi>s@fyPt@;eu{ z+uAI84X2cgAPmi>*MO$^)T`HSV%YCdPO8N{B;_%~G z(BED8nLuRP;}7N4sG+_WymU=bW6)ABSV0 zmAQZBOZe4noV^EI=C8Ah@W{;9GG_~Ybl2Rk`S6?hsrC_W26Z2PH+So=t#)X$M-US1 z643wm9~dRIjqnRf(FhWxkWyNO)rU2I&YYXNw?IE}(W08#rpUZjxXPWuyS;WRF5IE3UMp^$&F@ B@pJ$H diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py index 2edad1f..bc58a02 100644 --- a/Sampling-based Planning/rrt_3D/rrt3D.py +++ b/Sampling-based Planning/rrt_3D/rrt3D.py @@ -14,8 +14,7 @@ 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, \ - hash3D, dehash +from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset class rrtstar(): diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py index 318f38a..6e6eb43 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py @@ -13,7 +13,7 @@ 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, hash3D, dehash +from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset class rrtstar(): @@ -23,7 +23,7 @@ class rrtstar(): self.E = edgeset() self.V = [] self.i = 0 - self.maxiter = 10000 # at least 4000 in this env + self.maxiter = 2000 # at least 2000 in this env self.stepsize = 0.5 self.gamma = 500 self.eta = 2*self.stepsize @@ -32,48 +32,54 @@ class rrtstar(): def wireup(self,x,y): self.E.add_edge([x,y]) # add edge - self.Parent[hash3D(x)] = y + self.Parent[x] = y def removewire(self,xnear): - xparent = self.Parent[hash3D(xnear)] + xparent = self.Parent[xnear] a = [xnear,xparent] self.E.remove_edge(a) # remove and replace old the connection def reached(self): self.done = True + goal = tuple(self.env.goal) xn = near(self,self.env.goal) - c = [cost(self,x) for x in xn] + c = [cost(self,tuple(x)) for x in xn] xncmin = xn[np.argmin(c)] - self.wireup(self.env.goal,xncmin) - self.V.append(self.env.goal) + self.wireup(goal , tuple(xncmin)) + self.V.append(goal) self.Path,self.D = path(self) def run(self): - self.V.append(self.env.start) + self.V.append(tuple(self.env.start)) self.ind = 0 - xnew = self.env.start + xnew = tuple(self.env.start) print('start rrt*... ') self.fig = plt.figure(figsize = (10,8)) while self.ind < self.maxiter: xrand = sampleFree(self) xnearest = nearest(self,xrand) xnew = steer(self,xnearest,xrand) - if not isCollide(self,xnearest,xnew): + collide, _ = isCollide(self,xnearest,xnew) + if not collide: Xnear = near(self,xnew) self.V.append(xnew) # add point - visualization(self) + # visualization(self) # minimal path and minimal cost xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew) # connecting along minimal cost path for xnear in Xnear: + xnear = tuple(xnear) c1 = cost(self, xnear) + getDist(xnew, xnear) - if not isCollide(self, xnew, xnear) and c1 < cmin: + collide, _ = isCollide(self, xnew, xnear) + if not collide and c1 < cmin: xmin, cmin = xnear, c1 self.wireup(xnew, xmin) # rewire for xnear in Xnear: + xnear = tuple(xnear) c2 = cost(self, xnew) + getDist(xnew, xnear) - if not isCollide(self, xnew, xnear) and c2 < cost(self, xnear): + collide, _ = isCollide(self, xnew, xnear) + if not collide and c2 < cost(self, xnear): self.removewire(xnear) self.wireup(xnear, xnew) self.i += 1 diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index b256aa0..ddf2510 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -50,7 +50,7 @@ def sampleFree(initparams): return x return x - +# ---------------------- Collision checking algorithms def isinside(initparams, x): '''see if inside obstacle''' for i in initparams.env.blocks: @@ -58,32 +58,11 @@ def isinside(initparams, x): return True return False - def isinbound(i, x): if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: return True return False - -# def isCollide(initparams, x, y): -# '''see if line intersects obstacle''' -# ray = getRay(x, y) -# dist = getDist(x, y) -# if not isinbound(initparams.env.boundary, y): -# return True -# for i in getAABB(initparams.env.blocks): -# 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 -# for i in initparams.env.balls: -# 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 -# return False def lineSphere(p0, p1, ball): # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf c, r = ball[0:3], ball[-1] @@ -158,7 +137,7 @@ def isCollide(initparams, x, child, dist=None): return True, dist return False, dist - +# ---------------------- leaf node extending algorithms def nearest(initparams, x): V = np.array(initparams.V) if initparams.i == 0: @@ -167,6 +146,21 @@ def nearest(initparams, x): dists = np.linalg.norm(xr - V, axis=1) return tuple(initparams.V[np.argmin(dists)]) +def near(initparams, x): + x = np.array(x) + V = np.array(initparams.V) + cardV = len(initparams.V) + eta = initparams.eta + gamma = initparams.gamma + r = min(gamma * (np.log(cardV) / cardV), eta) + if initparams.done: + r = 1 + if initparams.i == 0: + return [initparams.V[0]] + xr = repmat(x, len(V), 1) + inside = np.linalg.norm(xr - V, axis=1) < r + nearpoints = V[inside] + return np.array(nearpoints) def steer(initparams, x, y): dist, step = getDist(y, x), initparams.stepsize @@ -176,26 +170,9 @@ def steer(initparams, x, y): # xnew = x + initparams.stepsize * direc return xnew - -def near(initparams, x): - cardV = len(initparams.V) - eta = initparams.eta - gamma = initparams.gamma - r = min(gamma * (np.log(cardV) / cardV), eta) - if initparams.done: - r = 1 - V = np.array(initparams.V) - if initparams.i == 0: - return [initparams.V[0]] - xr = repmat(x, len(V), 1) - inside = np.linalg.norm(xr - V, axis=1) < r - nearpoints = V[inside] - return np.array(nearpoints) - - def cost(initparams, x): '''here use the additive recursive cost function''' - if all(x == tuple(initparams.env.start)): + if x == tuple(initparams.env.start): return 0 xparent = initparams.Parent[x] return cost(initparams, xparent) + getDist(x, xparent) @@ -210,15 +187,6 @@ def path(initparams, Path=[], dist=0): x = x2 return Path, dist - -def hash3D(x): - return str(x[0]) + ' ' + str(x[1]) + ' ' + str(x[2]) - - -def dehash(x): - return np.array([float(i) for i in x.split(' ')]) - - class edgeset(object): def __init__(self): self.E = {} 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 4/5] '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) From 7afff6d1804ae2828b144004f8406df460524e42 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Sun, 26 Jul 2020 22:04:23 -0700 Subject: [PATCH 5/5] 'dynamicrrt' --- .../rrt_3D/__pycache__/rrt3D.cpython-37.pyc | Bin 0 -> 2351 bytes .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 6694 -> 8498 bytes .../rrt_3D/dynamic_rrt3D.py | 103 +++++++++++++----- Sampling-based Planning/rrt_3D/rrt3D.py | 46 +++++--- Sampling-based Planning/rrt_3D/rrtstar3D.py | 9 +- Sampling-based Planning/rrt_3D/utils3D.py | 80 ++++++++++++-- Search-based Planning/Search_3D/utils3D.py | 4 +- 7 files changed, 184 insertions(+), 58 deletions(-) create mode 100644 Sampling-based Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc diff --git a/Sampling-based Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/rrt3D.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..f8582b6e8aa8326b6dd9e323b1b3eea6ff6016d8 GIT binary patch literal 2351 zcmaJ?OK%)S5bmD$&OZEpv?Q?dJ}s! zd)i!)@)PVMe}kXVCnPSh#DOy>s%F;_pg`}=*HzWk-8J=^&+B!U!1&{btHB0Pzaeuz zY#=t^$$tl6#3zh0EvCK(vmWb-;TsfXMrti9_2i0wg_5rbb6DjSVU@AwU**@Bd#H&w{OXwa9=A{IQ{rFe-pP$qLJ5D5 zRr&i9DZjo-#&iW9xKn#dnzdoe-93nM5B@?(FPEY4IvL~MekQ!NO?M+4$b(Eg@{R}G zdmg!G;32Zzq{lMbJXQqf^%i3JQN&+W-d7wsuJa2WN{oZt_)mK^%qe-2;*oN$|y^f z(aAvB=!fz^*_?HG&SeKpJRdp`8}Q_}066g}gcrlAy&}HObY{S8unM!8^NRW=v%o2? z(!m!U3OjJ%jK_QdPc8u@8IytpBtoQJL`~|Y%wQBalWBk!(*f;r-C+jM73mhFM|#yU zYSpG(z2q7U&zULr&&s*pJOg8XLcIk>z$klLpdr!h2ZVHifbr{-t)_N{X?^AZfNSmj zsGCPa-dq$0NR+h$-b|&U&qM=x`lX_gvXk&AlHgqjoZLr!W$d5_4VI-`nQ{Q(zzBpw zD(&c=GP+q90|lPmio>qbqm(JVA4_Gyg#*Er4l~?u_IEiKd3){C z`)l{xn>;_1S-;(lTXa~1jji)79gI2-oY9|U23!RKqj*9KT4-Gj zYq@}U_q2kpL-&AK(e0)#@IvK;j0KpNVgXo?-b5)qQuMfV(gYx+5UZDw;DbKWIbKHS zEFHRN&tzeD`>j0{Sbb~HH;@(u6<-$`BZrRoP@;9TrXei*g1qfnCpMstZ+G+tC|s)?lGjvhVcw4E$dJ{76vkK^nX(3y3A8@aWxb^JvEJ53 z*2pfb*ED}HJ+V9wXvp@%iueVyz1Jx8b+GwlrqB!XRHH=B0!4b}7`-}oNa4=gROb+QAu7^Jt%^oK+v!E(ttQ_gw*H42W%fk`<*VmXh-DBoE0Y9rzU42VusBoJk;wKk zD9odjzx2%`5vI&{fGOLkZ+E8MlLm@z{yXJ#F-SjxWAje{2sNN48gy2JTA;30(jakZ z4KmRJbdlB;%W9bgzD70B9om?qa8gs--ELY~acP+FAW&`)BpDmTNY{ekIrQzxintBe zP2TzeQrIs323P`O1_1q-qt_hj%sWOY!}F6cN`qinlTpIG0e)W|d(E554g!{S;4(8E zBvAWW+E=xbY{8c*j`oBGXQ?Xsgm>`!kqazsW#B<-4wU+FhKkTRkW$uU7B^5!%W{as zaXyJp3q>}I1iq58!@a!x2!VC`(d3Q6Ezqx8@U>gpoV4S{R5XxLoP)3o`Ma_Ld%CD1 z)DWw4-@ijo#B6 KPp~NhPyYg_^*!wX literal 0 HcmV?d00001 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 083efd8d79dfd8a7619f91da826447caa0be9d3d..f8a989cb0a9d18db826ead722244c31b0ac28aa9 100644 GIT binary patch delta 3436 zcmbVPU2I!P6`q;<;~OV-9LGON+oVaFq;)pQrfHQeS}jep+e8sv7dBx7Uf8VdaZ=Zg z-OhD)z0r;QkgoP=5xbWM1X?MT5E74230@EgUJ!zZedP*x0`d2<;sGRh;X7m3`4PNu ztvPe%oHH}$cV_(E>pxhDu0$eXhVRGO(e0a;K8y~DG5+BdxAna2laa?fHz1?3|1rzO z<$@fLaZm>3RXHdVz!Gv%CS?j(Ql{k)q*8KNo8WqAeCp~W>OGjirJlQX5!LnC()8A$7i6~XfLC=_W+usSc2W>uduJi4@5(F!sCZRg=xQs;Vssj(0l{@)HA*m z()hqoGg@W_GVL|Y&Kg!{jch>+WVe_!p}e)9a!aML^3GVf=9Xn?tX^;%`C_%S85s3K zA;SZ6uP#fg8C#u)Vrl>&=qbD9TA}`^CIjQ{%1U4r^3|#vIGPO9U$SRf2}m*#0;u~-nqU~hW1bpp>?a>xO8=`t9>VM#vBdf{kbaA^mF^Ew!~;V!p>f!hkRUHian z*q-Sb$T{l+%CoitA!L;CM8tIR{J$HVSl>DnodDQbg%oEhLiQ`ldqsti;cAvTC*Yfk zPJ*DXSg$_Xt8G&;RsyqLE4hJDE!6^kTYFOVle~Q*ZnQvPJZQKm$7&su#Dm7!2+cqy zi{?gYe};o-kuUS8hyyBOb^}C#BK359rh!m7O6_^1Ee<1$s2l~Vg{uh(=e5t1u7Qkg zuEMS~=4Ol&dOimN^zz?NM3R?*e;Lgsn;9QPzNpJj-wgd#@HhSS@EpJS^qp`^ysEB( zo#|0a=+p}Ufho&wBQW#%LLo5r$}-^Vfw)&yb0qN@0;iKRaH{26>DH~4l?VRK$W^}P zKa70qnwF-$)jR<@rPVTlHGoX$ct5p5R9!o3(JRUKF>+}GCMHEfObeT<^{0=cDSksO zkYtL!6H0_y-7ON|ju+`zBWuY#J1=*(#wK|xjvet&#s@h+jCCxP(s zwu%H$&3PqXS6?KF*9mCHZTOY=EdRd$&G;05!~aSAw|vF__Ta+c&miHF&Wy+TDxWvh zTmJ6{f7mL3@=N0BAc`;rk!amqq6{aV&L!HI%Iv^seins?ORi$5SI`R&)q8-Nr^-LF zz%zGI`A;}UmZ3~BJ`^vo1L2tm#zSX|E(e?f8*x-m7O}dBg;kzF(SIp91*MD0@oS%e zfNaOdZSA*&zKYZiC`BSa`zYa;(cC5fN6E=_pLApo-UGG_%U-+^LNbK&Gvfa_d5vH7 zFQg`U;NMDpBTk8_VgP~RmKqsT-ShvHn&rRq6X`~hn1QWId-b>|8Y9Q|LFfB zJ>JT8UjV0E)P&qF8H#(Z;;7~34mk|>_%xh0MmV`j-|MC)jySo74oS`)PHwS>i7i&U z-dyzP^)Q1OlQaWw2;&e(;mKJ`-Dx>YTZKn)Op!AUU2pn`QD1;ENLbQ6XVvf&aQ)YY zrdxl(*rhCI2sfSdF)@VDCwPkQ$4YXy1aHjmmKxiHcg+-#r=~N3>IX4|auxC|_T}w5 z7kanAq}|SzF`gc`Yce|x);}=crPUbUA9%K9n+-0l=lDlMQ^_(URFy!DfFjWFKN(J+ zQ@g-_gQjcQ;f~=rjv0=qUH{|cuztamSPoIh2|D_a1_m8;88a}m*y6vNmBM;t);6vy zK+Fad`uZi;;^fLV^0|;(F5pDkRiA@+rfso$7x=Nk{?`)r5-I<0w9W}>Ba8PVwE6t@ zcDYt=Y;R|gS&wMq+XUVrP$ui*it^}RXxd45cs0o?egXMC(t;IsU~a0?|FB{V^Kj(j z0@gMVL_Q)|qD;7w9jk{ zYuiYru5Dcp(^W(jsuL&W-%Tecv<61!y3sc&B~-V}fcu}OhHvP|+q8>L6|o=DJLIL* zshLG?pY8 zE4ODrQomMWV3WWb1l}T000``Q;eM&u(5b22s`jDE^o{FaQk^*hYT&O@ jpW#?e$ccrcP9)rmpXJ1zw9^|-BZp&Xp=eKRJZk?3Eo_x- delta 1740 zcmZWpOKe+36rDTo*?w_t$Nx_fw{Dxbag-)$f>hE_syaVPLyD595fL=l9>=lc*v;6D zqRN&*iGU3f)f5&iP%A-KuptPE1xS206=KEi39(`WNZnN|Sirr{NsGdh-kG^~&V0_D zd*}1?gJ(Qxx7*3^^F4~M-srjGiSkG6PJgYH6fWW3gB73fh|WE>;ul`w!!{uNB7kjB z1VsqjkO+$iwqbEbL`4i|L`0Y9Mo(1qh&Z+}aa^3hwo4?$No>0dOt3XBPKndVcFZ!- zEBf}B=*#!)YAbO8i!9Z@KL;TVvMQ_3Ij%QYNh>+jSNa4P>ajjyH}yg_Q&!`)S#YU0 zZK0Nq3MTQdy@jyz22Nllm}0Nho4mmV#8?9eb*8ZmkVEL>F}A^^v!Vmra-ywS9K-|8 z0{ZkL9Gk^KJm732$4;E^;X=Z(h|4WAVW5Z(<+eu$CE$^yn}AOFJAt%Ok6>vg#br0S zdmZJ^F&@?-WwPGb( zy;Ttx(X81K7z)-w_U`}c{DVVUJ#?Ri;r+kdEq=v>=~RbIB1fJY6R$qUcOk%jKfTZ>yVuKA2P=1U`mK zY9e?Rrd2VRgm=|@!4OJ!CW)|O%Vngv zF6W5x5`h)E7u9c}L3m&J!hNuy&WC@2F%^uA1U^AV&B{#h!wd{*a#<}$zHGgM{?BO4 zxjl!Jvx9dfg6T;9F%E9k{GhEyl)RRJ|SR zg>TfSvAh0JoGZNurdG?>Q@VUzEp!dSx2n}u57D4$m-)@=cAkcan^Q)2s`U`NHF^LT zZhJeB7lk}dA8J!q$;{DfK_9W6#O(qa1=fU}5qjD1T}w9k4pw>zBN8Lq-_$T(7>0(n zhOxpot!J5SxhOR4uyBc|ax>Y9D;=Ck89mAVpU`e+G2WaDygUrIy@%&2 self.stepsize: + def run(self, Reversed = True, xrobot = None): + if Reversed: + if xrobot is None: + self.x0 = tuple(self.env.goal) + self.xt = tuple(self.env.start) + else: + self.x0 = tuple(self.env.goal) + self.xt = xrobot + xnew = self.env.goal + else: + xnew = self.env.start + self.V.append(self.x0) + while self.ind < self.maxiter: xrand = sampleFree(self) xnearest = nearest(self, xrand) xnew = steer(self, xnearest, xrand) collide, _ = isCollide(self, xnearest, xnew) if not collide: self.V.append(xnew) # add point + if self.Flag is not None: + self.Flag[xnew] = 'Valid' self.wireup(xnew, xnearest) - if getDist(xnew, self.env.goal) <= self.stepsize: - goal = tuple(self.env.goal) - self.wireup(goal, xnew) + if getDist(xnew, self.xt) <= self.stepsize: + self.wireup(self.xt, xnew) self.Path, D = path(self) print('Total distance = ' + str(D)) + if self.Flag is not None: + self.Flag[self.xt] = 'Valid' + break # visualization(self) self.i += 1 self.ind += 1 # if the goal is really reached - self.done = True - visualization(self) - plt.show() + # self.done = True + # visualization(self) + # plt.show() if __name__ == '__main__': - p = rrtstar() + p = rrt() starttime = time.time() p.run() print('time used = ' + str(time.time() - starttime)) diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py index 37013e9..e22e781 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py @@ -23,14 +23,17 @@ class rrtstar(): self.E = edgeset() self.V = [] self.i = 0 - self.maxiter = 12000 # at least 2000 in this env + self.maxiter = 5000 # 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) + self.xt = tuple(self.env.goal) + self.V.append(self.x0) + self.ind = 0 def wireup(self,x,y): self.E.add_edge([x,y]) # add edge self.Parent[x] = y @@ -42,7 +45,7 @@ class rrtstar(): def reached(self): self.done = True - goal = tuple(self.env.goal) + goal = self.xt xn = near(self,self.env.goal) c = [cost(self,tuple(x)) for x in xn] xncmin = xn[np.argmin(c)] @@ -51,8 +54,6 @@ class rrtstar(): self.Path,self.D = path(self) def run(self): - self.V.append(tuple(self.env.start)) - self.ind = 0 xnew = self.x0 print('start rrt*... ') self.fig = plt.figure(figsize = (10,8)) diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index 7df2910..107e16a 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -45,7 +45,7 @@ def sampleFree(initparams, bias = 0.1): return sampleFree(initparams) else: if i < bias: - return initparams.env.goal + 1 + return np.array(initparams.xt) + 1 else: return x return x @@ -54,12 +54,40 @@ def sampleFree(initparams, bias = 0.1): def isinside(initparams, x): '''see if inside obstacle''' for i in initparams.env.blocks: - if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: + if isinbound(i, x): + return True + for i in initparams.env.OBB: + if isinbound(i, x, mode = 'obb'): + return True + for i in initparams.env.balls: + if isinball(i, x): return True return False -def isinbound(i, x): - if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: +def isinbound(i, x, mode = False, factor = 0, isarray = False): + if mode == 'obb': + 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[:,1] < i[4] + factor) + compz = (i[2] - factor <= x[:,2]) & (x[:,2] < 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] + +def isinobb(i, x, isarray = False): + # transform the point from {W} to {body} + 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 isinball(i, x, factor = 0): + if getDist(i[0:3], x) <= i[3] + factor: return True return False @@ -178,8 +206,8 @@ def cost(initparams, x): def path(initparams, Path=[], dist=0): - x = tuple(initparams.env.goal) - while x != tuple(initparams.env.start): + x = initparams.xt + while x != initparams.x0: x2 = initparams.Parent[x] Path.append(np.array([x, x2])) dist += getDist(x, x2) @@ -202,10 +230,40 @@ class edgeset(object): x, y = edge[0], edge[1] self.E[x].remove(y) - def get_edge(self): + def get_edge(self, nodes = None): edges = [] - for v in self.E: - for n in self.E[v]: - # if (n,v) not in edges: - edges.append((v, n)) + if nodes is None: + for v in self.E: + for n in self.E[v]: + # if (n,v) not in edges: + edges.append((v, n)) + else: + for v in nodes: + for n in self.E[tuple(v)]: + edges.append((v, n)) return edges + + def isEndNode(self, node): + return node not in self.E + + +class Node: + def __init__(self, data): + self.data = data + self.sibling = None + self.child = None + +class Tree: + def __init__(self, start): + self.root = Node(start) + self.ind = 0 + self.index = {start:self.ind} + + def add_edge(self, edge): + # y exists in the tree while x does not + x, y = edge[0], edge[1] + + + + + diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index 5c51c89..b421f35 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -44,8 +44,8 @@ def isinbound(i, x, mode = False, factor = 0, isarray = 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) + compy = (i[1] - factor <= x[:,1]) & (x[:,1] < i[4] + factor) + compz = (i[2] - factor <= x[:,2]) & (x[:,2] < 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