From c398cefee76e3c3e0334a1cef1ea1aae9ea4bfc1 Mon Sep 17 00:00:00 2001 From: 391311qy <391311qy@gmail.com> Date: Thu, 25 Jun 2020 18:18:33 -0700 Subject: [PATCH] updated plotting --- .../rrt_3D/__pycache__/env3D.cpython-37.pyc | Bin 1864 -> 1933 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 4688 -> 4829 bytes .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 5054 -> 5347 bytes Sampling-based Planning/rrt_3D/plot_util3D.py | 117 ++++++++---------- Sampling-based Planning/rrt_3D/rrt3D.py | 17 ++- Sampling-based Planning/rrt_3D/rrtstar3D.py | 12 +- Sampling-based Planning/rrt_3D/utils3D.py | 10 +- 7 files changed, 77 insertions(+), 79 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 0d92e14fd9c29e05393649982ac43aa08ca431bd..03312dddc8e16234be4ab1342b0a4aa710aacbdd 100644 GIT binary patch delta 86 zcmX@X*UQi4#LLUY00j2>8@aAA8qd(r$j?pHPs~ZJ(055K&MwI>(09))$tX?IFGwuO n&@ISG%*)HnOV>@zNzX3=O5_$#{>*5}s;8%~r?;7h$%YjGdixu1 delta 52 zcmeC>Kf%Z4#LLUY00d91Hga8K6b{zU$j?pHugb_T)ek7jPcKT$o&1YYYO*eq{N_R? G8CC#?LJyt* 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 1e666e09555b7fce66b225b1d8fb0d4f7a2c20af..b0f8a3b12d763a0397a45656fbba76787ac233e0 100644 GIT binary patch literal 4829 zcmai1&5t8T74NG4aJ&6EGoG*AO=1EBykuu0W)(%tDht_3A`r8zvOvH}aN4f6$K$r! zTir8jTkRa!M7tbDLIkd8=0+|EA^rpY0mQLRNF2}{IB-Bh5dwa%+B0lce6(G!-+T4C zzTfY?daYFQ3_Q^{{`!MI*9_y2B%D7cn6IM5DTpux3ymRbaz;E4&7sw_G;fCX&}llF zXVB-GuI4T9`DT8h_nHM^3+Dr)Srj?p;$0Fe!V`rLShFl@q9n@TDxxB)sH=)4v5fbU zVE2s1>KR60+-Wf7d~FiMJG4PWpuD3I-Jb$6I}?9@DEr+jWL^!8JFd!cvm&QYEI%36Ho zq%5^+MvB$jG}aABwwA1gKXoriXAC7QEaN% z>|;Z&qs>o#zP%p}gKa+yCfmC~JV>JP_M81=|FFG1_LKc<Xc&duGW#A{lz43k^ zgFHqYmq83xV^y|+-?QvGlbg_7Y`+7+MbX>;8kot=l?f2c*h+Di8WH=t_SWG#thKiL%>%7{-eA6`#b4Pf=#E-iwTEpbJ$L(tps(CLJ$B z6Vt2==CLbGz5wn|=lypqs=!l1DWK?S{}Sf~XPZtvemzsrw6ixj$XJBM8{DEkdvh&Q zJ>;G%Oko{oen4$;TAH4PH@sQc;Bq1le|Ev;QSS&39N9q+&Vjv4x}I~&u*eMl2A{xb zXQChqA98%vdB_rH;QpA&_fx_FH_dMvr;Hh+7ZWFwQd;}6s2p4KLRvU+2`djp zyl034j#zcp@alMI=ix&2WIw34!>BWe>(NL{J?W44wT(sxNjRtMZW#FqP|z01bR3-7 zP@d75keZ`oBQKGTtnZwWV<@{nN?yLM*q~v`XGr0}X@duSk?hCH3q&u_=QwV7GPC!k z^ry;>_x*98>{%cIHn#Lfy+B#it0ccr`8Y^gM}bUYRU);kiQM583@ z^of(rP!h)Jp~w|dv#w$z#f0KVVUu@5`4VaQxTDzq)2>p{uA z*&FbgFS2E}#>(s!_9AGN*P!#5By7-&c;Q2YFBj19LkKDB83R+kKS?x%gmWv!>E6`1 z4i2Z5q%OzF&h&JSGd(!yf$G{GP_xj3gC6Jl0KPzokCV!AN~~jh?&4gSsY@s1et7{s zi&&rss`nJ|LYt=7tYIm8940#g2qfv{d=oplv#VTMaoW{JUaxf1y!ZOIm3P#S`|W<% zPo~+DKTi#PmdNLbd>%wu^x4y^oY$*NHP>LXz+e}vNEp0<)skJkum;WVEl9Md%pEI0zg+Xg~s8UhQd$sM>@a(d^n#QelN1#~2ZgCfC_ zu;KW*G-7wNZ>VcNh;zpL7l;@RsHCaz{qMVii!{xLBS|R!nFx%Ql3MZT55*=D_ zm&D}-cUfE^PRGw@?wnD^nPoK*IK=@1OnH>LWrGCyOCZzI04aA+uh@4LdtF&~ku@nZ7##sG z6F8-^dXXP0w+BR|o|FlPRk`@EP3eKkAN7L=t^TN=AbSV{AT5P=`57V_puR$FyY1+3 zBz!s5ZU&_7_zBWFIJ%`zwC36yb=c5ES14 zL6XM^Q6D0$Lmt2`8$SZ}rW|i%<*A7`e`GvjkN6|A3v?Fz0~g?8BL0s^@XCRqySx5i}V-ESmIyK(mb3Ql0+nPrT z4BpYagZPxATqR40DMp<-b5|hGBK>0|n!;Q+9vhF@f@6>Qg5$9CkhSFjVmgNelq2&I zxcflsrP)o%OOYql42-OR94lYLO$fPAfl_i<75|4-30C&P3T=F|2Ks81VGWyu%1M>DQ>+MS5gsU$qkP%EYz!=frnmGAC=69c*=%yeQ*Q-5 zUgBunSka~VGFD|dt)QPO?v^W%R??+(8TZE;+Nq^0ntF)r9J*C4)zVe)Yv~%!oqOl( zH`u-fzWw1=7VIgQW)a`a5Mr7&`}QZ0bC1}HNq## z^xLso(Qx+Zc2o@+VmyATb!CbuX{;lU)HsiH5$I&1r#fwF0Idre1=XHW6HmlWGW4#fw+ ztpRXrFvHCmPWJDsn50d44<&vD#9+_!BEYI>y37J#SpY1LmCY)DfxQ5Ds&WVN8n5vp zM{b1LB5tZhHhb<<0KIh+iM=1juM$#d$VMzQfuIdX{tUe|xtyR(QDz8ALp=yRriLYd z0t5LGk)IMFdr6l@*#-FyN=#d3IDk|DgKI4}bWMZbkzarw=_8$(JB?+vgncIb3j1wb zwRIqIvbSu(MAe=^x;TTn9U(cYnwS1l)t1gdv&%qEBO}aj!#q1=_ULdpo@NJ?ZtPj4 x(?O1sM@g`*hx$b9Aav8SQ|`@SBo4#iRq8)p1JN5nn}LYo=3U!c^YZR<{{pTco(}*3 literal 4688 zcmb_g&2JmW72lct;BxsPN}^@Sb`r*IfzVB)pbCQ2a1$p{3JH;$sPn-1a;)sAI+ur0c@T6#*w`!34lHz28T%mfoWH4b>w z5ax-w08cv7XxW5Juci12w8yqGPuK}xScTqxMsF8-2W5%sbT6yu}Z*5*-@6VmD4k^Yx}ljSo10E=Ja_YB(~j*kH(oB4Mxd0=!O{|4>CKR zq@m2rZk%LhG8ko6JPMP{N(S?=UCNwpIHG!nze&c9GB(ZFEMxO84S5k`{`1FM`|&W` zn(xPxtvfRA%V4-Q4$}Qs#!)aD4MzP}f~X(MLApOow(bVQF=p?-(g~7KwC)s5wxmq= zws*G1QJn5g(m}MnvpJq+)punWq~YE1ekj8-CQDX84Ax`~wvONPyaBGkp<2h8+_Vmro9U!(g{obK>*oESKB!+6L)HW%i?TG+}sV9KO36c$<76Pp?LoqMoS zoM)*xPZPtP{u@t!TR*>hD`TBD&+H%??+2OPjiXq$xn7W8*V}n8kYNvp61JU;2h+^% z1W}Y^Y>@G3lJObdd7_UpC+nCZs|p!B*vmJbz@dzUc4aV+U19QD;Ldkjfk3a`?XPgl zutu^|{Q9P#-R31Y$k=;Pf-CCtl53gjA@^Nj3hN0!vv6o}B7F-bd`{TlO5}CI1y{zX zj_|-yZ|K1}Xz!A)@0_rOsTe#2R!lYwFRgse(aZgu_vjwZG!;?KHS5Mgdxofz{flxB zp0R+B9GJq@8mGtMKwXXF!`w>&vjI3!#H8g7+p=BToQGh z#gb?|u@+^taa9?6@RWDg5Eaee=U${8GD>#w{^O z;77Aox*xW}(a}K0qahr!)rsQnVWP*-o-PTmGP@VWK?*dpMLHXYr#6(Q7Kqdw*^s*ux+N>TZj6MZQ35aPsZQRT5Y)W21}- zpC3g&??rNxhT-FG#`d4}lO=sxy7yi*NK)EdLU?Cbx0uUragV*kR@fS^vp3kwW}P*7 z6FT`_wCEBf(zAy595DU@$OIfSzyX;Zl?Bj)v$QcA2ljyjQ>L;6Q*{r>1pp^7Npk@M zIPl0_MCl2FoN|sUCk8XpDhz{q)E+{c)(dHgB*l*#C&r?rN`2$dlz*L$R0#&%gF(X> zHw=2gm2|e0^2IhATE+`9t0e8j92~xa*^8_3a`$kCD?u7I} zwXWBiyC|9) z1~(S9MZKt@v(IZT=$fTPqmb$J^O`kXv%FYQbyZt8R6{L+uBhd8!=%wx7tO^5+{L=8 zsrpH7EgNb{H46TcT2?E>r|GISpwFf5@$=#$-6*&h#5!@s zz4GJ_(6z({=|(R27s(=ZV7qktG=};6d|UWtC!V&s?@=a{=S993h=WOz2AwGEldI5k z>VqKWSz#0&WmYgB48MXP4K#9@S};kD=yL@BZ-dcpOv4WYc^FDcFZ`vPKgSlI%Q$0w z49*hWTcee1Q}>sMG>K3=?EmY_zx{rkiV7%miEloYFw1 znbnVjC^N@V>X(v9hvKqdJ{p7%_6DOtim)Ap05giKvPFd8LB0l(xqYA)tuM3D8;4|L zJQU6cz{c$z-z7*Rob;`H)TA4v$d|TvGPe^?Mk0_i-y;Z|MuQY1#AO@>42d|4?gVjOnnz~47D3x0$Es*(4LW;*X*(Q5~Ut;vTg1+>- zf>EfI$EsZZ1mkuoywO9!jd%(=$#oDUdkhJI`4H(H(mHn8_yw>7-BR3)Y!yz(j9(+@ExRjM7rffM{_t^D@ew0w3eiyTj7128^!!)K-@V4fW3WIkv?<`7b3CRmikrHD{ZBCqpOS;F%Iw_TUY&>QK z#~$;7<7ibz6fGkr;@8J#5G6kXA1=^(D*w>&6q2TRR2edDq7e@ z3-mqjS);jBMO9U;M|0Ix{js@ML$s@}WBffx2X(A^iOLh~pL8kk8i^dG%l2jC&_evX zsn<*?>(a>6e2I9Pr=r)1b89S?G14*+-BU}5r7MtDfb7fo&|5{HRkfliV)O}zuBoL} z)dYV*U4V7Dw@-hIOVTM@fvSIeD~C`@(~y2qW|EW3&nhT$Yd3Suqi=3`gUI(le2>uM z3}%QwgBjv;n33cme)B;rBe9nvS4(=a9BRwf2r8>I+AfhA2tafOWIH+vyO}wRMP>$_ z#9!6O`Ssp@6Zu{;erC+f6mi;K^KD770)-A1cWHP61^GiF9}&4n>N`t*fN?1PC9i`R>?+qNW4g>@F0w_B)y)RK%B~VVaYCLZ^2aK0s)3BMiVx8$ zo4@cirN3WUQ{W($+QYWq%o$NDveaI{0$dp~%Tgc?&)K=2KvmaaqK4dR$be@9(E6u#&E&70}WbQp^5w1xgCg$L~vq%d)I{9oA0RUGo3D763(mlp8a^}v%&dFDLlUluk}$3U)xl_8-LI)^^4g<2i4!^E2|m@hY!y>#+w+P z4+w-y84(bpfu9yhc0;FS#G;VPWrIZdJY+rG1Kip(4xRC@J$oUW;U3`Do_Xl3#-37W z$Z#X@--iFu2}!tICK(KdE(VjK8*urQY{KMdayzSR239m6j#WLn;4PGAD>usS2t>~U z2wEb8WRFT&nbmJ0`{IrWw3=4cecDL|I#(Xo>Sv49LR}WN8KT@`VFYPUVzY;2T3y2CHrS=>5m?ui|T6hLncKuOYf?tSm!EphEpDr zVI2z}bST1|D(47U|f!Q@<&gG;#B@eS=7*Iz|*uFf#f_fU;yE})K$LSZyt%YwJS;C-r_~ufOY?7nu ziIM4M1BjP614ZNl1#aO2L)MW^Qg@p=NK+4*hK@QBxd*$$ver${&q=>-P3C02jCnri z%^w=C8*>%U4=Ld2FI}#yu6SGP7!EIEcpQb$Qg{GKl4EKjp6Q!`23Nv~s=jgt#OH#- z1$M}r#2RG{boC&9q?;z3Si=XSP{}Pdd}2DuV+_YtD$x$^>`(MxS%Jdihbs_p8n?Az zAoP>a%yE_Y!!y6@kXl?UrWW@YP(W*KNa!XtYDKSU$m(XYXR59vQk40BUc5XGGq*6j zu$fUP|K~!ke0U)j2^|w`bG0>Gyxv@LK6-xA5;xo3Jf}80x>T9VEqTYVSn&M0N@@9+ zJf%KwZY5IRHFqY~o}UpU&#L7_hWgdVi2?4wG4(JpMR!zha`#~l^hL0z!n+#{wZ^Ou z-$0EtQRNrKFN+o}Ijn|KuctXAd7dE!a18mDT1oZCKY`FFD7CAvQm@5cSkJ0oQN+|aPnYj}60E~Iws3@x$Ex+U|a$_IJ2$kyKms9ULaHM1>F>#DFVTi|CAChG(i z1yQAy5oQNkShlLIpr04fIx{S{!)@9i@CTPNi)wTYt$@V!&S`NyK8+BO5HLu88Hzj$ zt9<)<+Sz*_YcKqR0z2cB(st3!RNgJ*y}X|-f*uh4k*!1UAb_hKmeS>eF;v z=Q%8IV}wbu$=2AES*$;rKb<}@Ck?EAj1&DD8m>-m~8f|ZdGQB`8i*n nQ_I`?vfNT^_+@LbOx+aMYy-J89BHZPIPiB!o7Qep6anKnkcS1X-d@n%Ow9len2( zS}Uv@AqTZcMW`Kd00|UJNKnCzI|q&+aYR+bt>VgwKY;hfj$3hHSM%o0yqWj;d%I7) zpGVvasZ@gD^Vi9LuK$*P==Q*`4@Z7DSYU!Jagi0dZOGe~ndlXL+f4NNJvAer6)?-Z z{mO;kJKr5&F0c9Ho6F@7$KR3Vf?QZr?R@&DHU100yqYjC9f-IvOH5vkSkWljvn*sg zP6Tz-A!4YWs-But8J8U7!;rw&;SM;UMq+=% zfO^3i+?qgH;OG-BtfEZz;bPIC9em!+L7oI`3ojC*o6ZN3F&O+*fcr=tsgh=16@Ve}dxMi3xa1eV4e4{BYaY)-^h0(IMyo z7vyo|7CEi)v2mCE%H*5OY$g@ zP9o%~93^s$$QTmOmIK67^>;xw4ig^o*2;RYBK&+}-CvNm$}8((pngp5JCR3WkVe9| z1xXF4G-d-qGlhGtY!Fyl*|_>BHPA=l)^LPogrPpi8(F3XQoUfRQ>o*&4o1e*YHDE1 z2tv7QE|zhk+Ihw^XY+RD)`IlcL)nIs21yck63G&dCZN+`!V%EBEkF~PDL=YWw~?Vr z6|p`Qfgq+TVRcHpHUhyal?+Ryi>19Me$RX2$@?C6dhGdHNV4E1WDX}uppqj*UM507 zlhbO$%^|2BxTD#1+z51kViGgxY!^Zc(|JZ+PiNt@s-%x~iNrR`+oDY*adt!sYwxPZ z>Gz=WsN>K}RM%FnslFk@6zg;g-rZQCn>FZ0L^akb-9Gq+oW`>rm*;WP2>4Gfcg^L6 zVEH4I1{&nBne;`$5BzY91g%PeVi0vPjWVj&%j!L?&W8&6w-ZW_|tQ!NYBQH_i?pxe}nI6+$w@jB+3x*U0kVzWOvXR-&fU5xkB}fmNY$ny`o8zsV}Ej1nJV6u9ok7yz}C z6||;jqYg{R1Ka4EV;jji^blbIGuS1q*d%Gz0rq!#hacb~(7!5diX^1@5NxLRGFwn3?wF44v^z=1aXOv2V-e@Z F{{`C 1: + while self.ind < self.maxiter and getDist(xnew, self.env.goal) > 1: xrand = sampleFree(self) xnearest = nearest(self, xrand) xnew = steer(self, xnearest, xrand) @@ -44,12 +47,14 @@ class rrtstar(): self.wireup(xnew, xnearest) # visualization(self) self.i += 1 - ind += 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)) + self.done = True visualization(self) + plt.show() if __name__ == '__main__': diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py index b7b93b9..ef08f55 100644 --- a/Sampling-based Planning/rrt_3D/rrtstar3D.py +++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py @@ -6,6 +6,7 @@ import numpy as np from numpy.matlib import repmat from collections import defaultdict import time +import matplotlib.pyplot as plt import os import sys @@ -37,7 +38,6 @@ class rrtstar(): xparent = self.Parent[hash3D(xnear)] a = [xnear,xparent] self.E.remove_edge(a) # remove and replace old the connection - #self.Parent.pop(hash3D(xnear), None) def reached(self): self.done = True @@ -50,17 +50,18 @@ class rrtstar(): def run(self): self.V.append(self.env.start) - ind = 0 + self.ind = 0 xnew = self.env.start print('start rrt*... ') - while ind < self.maxiter: + 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): 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 @@ -82,12 +83,13 @@ class rrtstar(): self.removewire(xnear) self.wireup(xnear, xnew) self.i += 1 - ind += 1 + self.ind += 1 # max sample reached self.reached() print('time used = ' + str(time.time()-starttime)) print('Total distance = '+str(self.D)) visualization(self) + plt.show() if __name__ == '__main__': diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py index ca0b313..7a984c3 100644 --- a/Sampling-based Planning/rrt_3D/utils3D.py +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -53,11 +53,17 @@ 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: @@ -67,8 +73,8 @@ def isCollide(initparams, x, y): for i in initparams.env.balls: shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i) if shot != []: - dists_wall = [getDist(x, j) for j in shot] - if all(dists_wall <= dist): # collide + dists_ball = [getDist(x, j) for j in shot] + if all(dists_ball <= dist): # collide return True return False