From c74372983b2e7db7c7771f2ceaf0009d60f98fe0 Mon Sep 17 00:00:00 2001
From: yue qi <391311qy@gmail.com>
Date: Thu, 13 Aug 2020 01:13:36 -0700
Subject: [PATCH] 'env'

---
 Sampling_based_Planning/rrt_3D/BIT_star3D.py  |  10 +-
 Sampling_based_Planning/rrt_3D/FMT_star3D.py  |  10 +-
 .../rrt_3D/__pycache__/env3D.cpython-37.pyc   | Bin 5161 -> 5064 bytes
 .../__pycache__/plot_util3D.cpython-37.pyc    | Bin 6025 -> 5425 bytes
 .../rrt_3D/__pycache__/queue.cpython-37.pyc   | Bin 3032 -> 3036 bytes
 .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 12303 -> 12307 bytes
 .../rrt_3D/dynamic_rrt3D.py                   |  10 +-
 Sampling_based_Planning/rrt_3D/env3D.py       |  99 +++++++-----------
 .../rrt_3D/informed_rrt_star3D.py             |  10 +-
 Sampling_based_Planning/rrt_3D/plot_util3D.py |  82 +++++----------
 Sampling_based_Planning/rrt_3D/rrt3D.py       |   4 +-
 .../rrt_3D/rrt_connect3D.py                   |  10 +-
 12 files changed, 79 insertions(+), 156 deletions(-)

diff --git a/Sampling_based_Planning/rrt_3D/BIT_star3D.py b/Sampling_based_Planning/rrt_3D/BIT_star3D.py
index 9c8c022..17dec01 100644
--- a/Sampling_based_Planning/rrt_3D/BIT_star3D.py
+++ b/Sampling_based_Planning/rrt_3D/BIT_star3D.py
@@ -26,7 +26,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, isinside, isinbound
-from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
+from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 from rrt_3D.queue import MinheapPQ
 
 #---------methods to draw ellipse during sampling
@@ -313,7 +313,7 @@ class BIT_star:
             
             # ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi))
             # ax.view_init(elev=0., azim=90.)
-            ax.view_init(elev=8., azim=90.)
+            ax.view_init(elev=90., azim=0.)
             # ax.view_init(elev=-8., azim=180)
             ax.clear()
             # drawing objects
@@ -333,11 +333,7 @@ class BIT_star:
             ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
             ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
             # adjust the aspect ratio
-            xmin, xmax = self.env.boundary[0], self.env.boundary[3]
-            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
-            zmin, zmax = self.env.boundary[2], self.env.boundary[5]
-            dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
-            ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
+            set_axes_equal(ax)
             make_transparent(ax)
             #plt.xlabel('s')
             #plt.ylabel('y')
diff --git a/Sampling_based_Planning/rrt_3D/FMT_star3D.py b/Sampling_based_Planning/rrt_3D/FMT_star3D.py
index d0885f5..6bcb361 100644
--- a/Sampling_based_Planning/rrt_3D/FMT_star3D.py
+++ b/Sampling_based_Planning/rrt_3D/FMT_star3D.py
@@ -17,7 +17,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
-from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
+from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 from rrt_3D.queue import MinheapPQ
 
 class FMT_star:
@@ -160,7 +160,7 @@ class FMT_star:
             
             # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
             # ax.view_init(elev=0., azim=90.)
-            ax.view_init(elev=8., azim=90.)
+            ax.view_init(elev=90., azim=0.)
             # ax.view_init(elev=-8., azim=180)
             ax.clear()
             # drawing objects
@@ -176,11 +176,7 @@ class FMT_star:
             ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
             ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
             # adjust the aspect ratio
-            xmin, xmax = self.env.boundary[0], self.env.boundary[3]
-            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
-            zmin, zmax = self.env.boundary[2], self.env.boundary[5]
-            dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
-            ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
+            set_axes_equal(ax)
             make_transparent(ax)
             #plt.xlabel('x')
             #plt.ylabel('y')
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 c05c8824fe141e40647949389dd3b8309995ae37..980d40c4570a322e041d982e0b103dc0050cf1e7 100644
GIT binary patch
delta 2434
zcmZuy&2L*p5Z`_KzWiCoaZ)=?Ti4{nPSch&fqsg*rllW9X{1n9ScWIt>%7!;;;?=c
zQMni?k>CIVw1{(yxNt%okT_IG9FX`6K=Oqixa5M6IB;cV{hpJezSYjo%+Aj4&i-cJ
zxA|XZ<E2<EOyDd2v*i4H@QZlyH6G&O9a1)Vghyetc#OyS0iNI^Jn5Qil#lY%4lUc)
z2p{9)JA{unLffny2KgXQgPcbB`!6eWWxIa9a=X(Ioyy|5g~f%+l}6{D*WRkQcfIv@
zYn834w|-))Ro&dYyXl^&w%oRWiH%NWwYss@0-aN<b{gDS@l7i7qBdn$gz%ijD;2q}
zE!@VlzY4vx1B6qqamICSaNRXwM9Jiql1wGpLn#Dv2F#)*+*nyE$u;Ct?PyQkzN=j!
z+w?J|<efL}5RW{fU658uA-3$ohVUhdK9dg^LVQgs&@2VaRKUyy%zVJ)0w!IoQ}8Pr
zp#}Lf%h9a-lXWIixR(~EgO;Q^4MR&)F)Dwv=a|8nWT6YE{GG|3Ghd?zn!hjkn)%|c
zCf^F(p|kRbkURDQ`0pqrfLHVmzQ<%+e=<6UGq&0v^=GL#ByZTSr}}!eTB{Y9$iSSK
zK$4VIGj+&qc$Y7iN~af|U6`ceuv`d#7(0u44G5v)h<wFZG{htb@;&3n$T%<p?=MpF
zyg5TBz~dw>$`8z|bVg?F^az?>fUZ2)uoOHj$zxVKnFVdZ5;+(IUPnyHudS*4G_w1Y
zD29dvuPkPO+>n1+xx_4rc_aljp|->bT!lW^QWtabr*NiM15JaJDJNx(ClO1K6%6oY
zL)Pei78!s^Xf6X98FJAz1G5V3&B45Hp)A)JW*f{|d<2j-+GmZ;`zj{8hyrm8NtQs=
z>*`fN#1Su|GU17=MR?ER);8N_UX<H#6b1*tw1Clz0oeUnS9?sm#EE<Kkp{EST&6S!
zlm=3O$9zqKTVOfD2^=XNfPaYa$pBsf&aRWE0nkWs_ai(qzN(MCVXh>KyC?S4>A=g2
z0RyO5iRR}O!VaSo1+zoI^+Sj;XX||-#2CnBqgHLTI`B5%M6+~S{u0ffIgKlx^VBP#
z?;z5MW&+M_8ii)l9G#&_XfhY89Y2C=5v1ZclINU2EWt!U6Vvj$*ercp{vNwwzZ|IY
z@ne+BoAIe$#1qxcPOIwOZEx01n8(k8K76oGATnJ5%02CKDn3?4hBURVo*|FvLsQ96
zyfUN<<t;-FlCH^_l7K1+EMX3)l8k38d*(By)}E<r;#1>B7YU3EH*7-9+_!vByMMzy
z?Y@m&7Wi=VF40NT=xS$(Mr_9zY$A6OZ}tZFAWbtJ+#=x7Rlws7fLFJ{;|Dw*Uh#PJ
zHEoqBG}_Ce1mPT1jVy91O;ja0iXtWq)CG9Vt3)u=>{YBBidc{Yf(5LwTJ&kPPdj~j
zzZ|Qz+Vy*m*LF7A_ZxkEqs<$A`C0O450Tu#)x-$XgqHLxbDU1FaXJoIifdt-SM5O@
z68I!KaFn~cj?3UPpkwN)wEYSgxE;J?hPd>h?$NHsfd`6D0>82h;rgc-X0YjHh@*Mz
zkq#XLtd=#krn#Sw3>`MD1X0NjbqJg><&r6vnyU}Wz`%zy^w8DO7Q<XHmj4&sLWK#$
zvOpjUyhFhjFCia6GKB=<?I+JXa+v3;3>OR)<Z`54s|_P-c<T+XI(W!y_!sG^c-<<M
zwqf1i$HbyqPD{}eeGnZ66bzQ()v&hgBd1VQ1m7Ntr8?g%@_8hK+K${MB$ts~M{*O1
zsu;3hAIO{p@fLKozfHq&+q7ABQk;}Kqo?Mv><FwyLX{@07=5!{Yc}eh`k@K@ZwT}u
XX5}BF=SE_-g)Pkz;Y2uo$)^7SP(`o-

literal 5161
zcmb7I&2QYs6`vuwyIk(-(~2$GPV35P>vdzvwcW-D414XUNs$7w8wUumKtNDC6t(hx
z$U{nz1R6ca1$t<K_L!h3>|^e|^iL?z{sTM}C{RGRo^$H&y&<_P*-qP8&CHuOpYQuo
z52mI(fwJ+=E&uC_Lj02kMX3U4qsG5QqeMq2sjR;2Sjtw;bJ4L?MOD!|s-|4@m5+p~
zEAP2bURXV{J2i|Os)=!v$F7=E(^ywmGinySr{>hWI-?fUTk337-IVH_T6``$4eVP|
z=dthnk=1F2Q|iJKOaCTp9*NGhvL1`p#ltzT9qcAsBmMrRgWd4b)1G&XM~d~XO&tpL
zMA`?a@t@Ho;)O)DQt^^{oB9SwitJ)k%n}Zyg4oE8Y+h}4%9p_MYCyckD)$3Jr|}s)
zOFL)zR)tmOOrEv!`_VT9ox&#?td*iSx6TR>YOhClI+q;y_q~>rS;L*I5@;P9WOjEH
zXLj5hw(P8S=m*27A7-_K0@yde*Fx8E$~V7S-5L$T)u12ludaphlVr5B8ugN`-Hp|q
zAlbUM(+`Hj-Y~ir^rMl+#2{XM91M2)!1)_N94h}&zGPMF#J{<=y0Sul|7B4)+`GB9
zvU8BRpZJ3y(Y<|=P3#b5^*yN@=({waLZYMBvgq~zkSIv!NZ+C=w2ek?KB{A6DGwtD
za}6Ff%_wr7iAX$?`&U!3CO%(&W`Wg`Sp3XN<qON`V`{C5BZy3jryqXC0!uLh^Jmth
zzyFPj+a627U_?8*$z`UPS}1_UVrsdaZ@*k%t`wM7JNoKNl%*T(Xp_pnZnV1+lv1%!
zi3N{Q<5@J3NX6&wGnvXiSWlhDMwBeimX%p6*E6}zBhwILzwz$$dvVeo4R-D>(=l{r
zRUEz@g~>*L)O`}K+(8%g<GTQgl`UJ-I!%{n)nI2Q94cs4g?D82hZ}Jcbo*hP$=<2^
zvU+*&S-Nf>jj*QWv@<Q|r6WE37NtG|Y)K~FLSknre++;n)n-ZEX8DK&GD5XeG9c^M
z))F+~Wr2YX6qpDSW%+rZiIgPgtZU-tG2L-YZmi|#8FUl$*?}VBs)ikzbN~K>2b$h}
znzR~7qsi}ME_TrfiwL?c^%CGNA&NrWYoo>sXhPAEO2Ap->2P+ovgu)&69gL@U0O+n
z`U)yD;VS?sJjD{VAoiS4_633+##AOSRmRu_CW1HfPwZWMWp9Zo_ZkfEl`t5X5xkK_
z5chz{6X9e_kuR#PYog0=mL7gg=6lSHCK|v^`6!b=%j84or4xt!O}$7P$d<>tnYq5-
z8}<_4|1t0}Nk_;#_#I9WYgUTcbubG{hruB9{mk?I!AR})32yrS)7_wNX7uIq^pp4K
z^O$%Sm6hME@c920mpy{C*iGRi>T9Sht)Bx(CG#CI#g{2_DzRTUsFhS~TiaFBHekQo
zVpfovQY0Xah#VSWUzK@l8->+x<Iq-3e;1v;M9uf8A!cXQJu<&1S+hIp?+%8(8Ocwf
z+RL=))Sgq!ATGU+YBX3@`FGd^b(0L&@6bu4>G}uMypgv*#OzN|4R0My;}e!$Dm9(R
zmVKTrd%m#j)XIHsVY70*Wf>zS!NC`uLuVA|b!O@2W_hEHWfOKeFZGXrDFtm~P(Fra
zM%<@h5>Ys@R23lwvd9C)C4S_hd8A2eCc?uq!UJ4V9v<rJ$HqJ3Q0PWmBYzV50-9s;
z!7R&?CnQ38%Kzllb(8i_QFBU+)4=@#wQ%Sc$?pdJ{+s;yNvGFBP8RQMg*uG&9G(6q
z#`2LL(LF?ofbS-k2M@l2Q6BkLihu`9859k$T1-s@#gY@`FQ|Ti$_YS9&PA>>sV%9A
zr>Dry>TuRRd+K%CMa&dZuc9sGOenI=HfsC`P3eE<AmbX+N=g~(@Wc+1g$DYHu|>|~
zngla6?x!|uYFf=;XI0JOiSwC_G{(3;c7)%-E&45L$ZWeD8vsyGVq*gT1R$-EmoS=>
zTJRH2T1k<}u>ISKa|Hk3R}%6Tl_T;AwZtvqdgAeXgM%Tx6WWWHlJ=rcrbvTT?Z`f|
zBLqS0o~D(c)lzlJL4lcJj5mRqRk+QcW8c##(j2D{8+4f{GC9hZf4#>_Z3w?ccvO^s
z+{=Np#lD+uFv){<!9yjz(QqQ%x*Mb2p$hZ?CjrK`$xe-dQzXsm6lMLL1Fc!jkav(`
zvuok}FtZ;%c#u`#_jHmu(J1I=GHJQ&{W52N&>LpX0k?<T?hk@}!ccpNR^O!iP8rl?
z;66d+kS(k#l-!j~{9Jj}nV0j{VG}|xePpS{q=FO<Sje{kB;p8}CJR|?+o{amJH!i7
z+ALNhVguKh_3(KvtFhtK6`b=r*65#L0^V^MUB?}UU$7~hA|<P@hhG>YA)_cA{XB>{
zoFXE|^K3v^L_Jlef=t~6wBl`2-v%zfIFtLQZ}bMEy^sQtPI_3F+vVXjT{fP|)#xwx
zvfA#jw>i>-(o{$oWe#}DCR0)JiS>J_7dTT#5KWQxAYXTR45dcmL;OXevLqIeBBicE
z8kFMz<v2>2%sN0h4kx{`my#i~ZoIcv;_|)5ROu~uXf@1t38wm*w~XgrebadEmA6rn
zWlr@uaiH@l-A~<3eC>iKE_h<;KYj~O@`N$xh9sUaZ|E*gAbCU_7_Tf7L7pS3^OfI1
zZ?St!4N+R$H^xrfdy+bbxNW@$weyK;mM`pvzRUP*;Hr!Hi6v|}X7EOeRlHd$BRvBf
zsR5?fpU@WCS7?cW$LHZI67R>I%!BcHt~zmEE{8hgVC(BxUrVtzBE57|hc&@rJ#;8p
zSKjZ4HeTb`@fHuYQDC;~+s#Rs(?ednUCEO~`l2N_pC^g=N#!n31MR0b<qll0!I0QN
zv*R}Ynqw5_BowGRjYJ2-xF3*X&Rji87($*!8Mvm<sVQ=`rp8{|_##Rhjhp5alfCGB
zgd@u}2`W3T77eeWY1NHSmO&=*vACC2lC3ZaI#cYn{A7ewelP4akbD+{%t3Y^c5M95
z;q^|2xAt*({2?0QcyiWSlneRql6_HLlvgaT;#xE_FY)ckzw`E4c~&mVY2dH$?_Jx2
zM_iU4%LzwiatmbMl$lI4^ogQR#(6AyU6y>GR+1mpl%Guy!2yR8!t0p$B`UjSS9)`9
z)rB?zLmxVWpf~jW)=XCOeKqRhJEt<-9UvT%gEqWALlDQx>S$v->?Zt8V<IF6Q%cWF
zJfTzhot-M;1?4ULj&VaT%G^8Vqv|f{KujNNj&yCe>CU*dS#Q>JYi@1Pp8nL8{{c~E
B@Ztaf

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 093e322dd4f0d6eabdd95dea79c62fa5fb59e1a7..2ea05e73d29004bc1a6d998358cf77e090b45456 100644
GIT binary patch
delta 1310
zcmYjQJ#5=X6y}kVMM;qyw23S`26{yUSSm~@t{NltkQB93w1a~HX%~Twaw5+ZVgC8<
zWXl2t1#p+l+zWZ>)*&#^u|U@@-8B_mJ9p~RcW24TiSN6|?>)Wu?(yE6_S;XZUsWn4
z4c5uOpZA`wzpY~Z`5R}?{+?6D?amiFogMpt@?#Mvw&#n%ux}?s46Gy|QRGLSMFKBo
zU<kSWf`mx`T(3_!b$ibjE_OE7@0&JbqSraFuYT4m#i|JzrR;<7arS$;j!v)U<;SQY
z>S^HuAx*4b?`dJSajC(3d3MjOU4q5cUTDp=?6*qe?(46=^dN#-O}(3=0X^+z|5f%-
zH`}c~D0|la=0AV$ZaisjX0NNyfc>-jbhXN<=#dfSJ$f=ELAJfx_{^M_{KyvxVI<_t
zhKTG32|Wp~0Oon>dX%$|CGinJ_Uq~==s`ALwWcOA4HK~lSO)>fRR+m}s1yDHN~+DY
zOKpN+N3iR#>#!TbNRf-yH9&n`t7{i%k`uXEK1G*kk_W4<ton_0MOk&Q7Qkw_XbOKe
zZp@}K8(=QFhFD2+=}KyP#s#{gO-hiokml1;U6Z(`c@PH4(#*7=Sitw#Vp>!z;F5K7
z(EU953C?*)jwyydPC1T`aOVIgK`iiqoKmbBi@8Grig}F1fC$W!0cDh9k|dNcO!$)7
zE@4>CurH1LTG_@_IIR}O-wk|@;fW)PlbCbAuR;bx;l~jcF+L<yArl9Fe~YU~WYkh$
zm?zW`jV)EnRuhxRg=$-5<nwoA1Qnmf>=>>sK8hKfqNQRCgh=>ayUhi0j^k6xj)M5C
z<-}q8WJtNp&D%Sj-A4~Qk9Mc+kSt5ER5oh}cgYH9<iV(%!E`KWqDrzDfdJOnlBHPi
zKoJWE5v%OqM#)ep4MWL$nBaFAfbcoAr{%S_WxL{GTYxt{VsQvxK(suEG@93^U<J5%
zOkT;wnEdtRS+kTq{_y^EZje4_at7zQkdkO_gk;n$$;OQWKkT^I`uThMSn18Oy8c~r
z)Pr|l@UD4&qm7qZVbo(J@~CtReb)_Ww{X6s&U@j@1vg%D<E2|!eoSCpf5_RItY1N0
zY6E~DLHGuMR)o1jrhW&N&_Z$_=rvSBY;g6>-4%4t2d5bbzXhP_w-EgF+vql8e&*Nq
Zk(2#Wo0R3)s>r}TTstgS=B8OP(f_RXY%Tx*

delta 1899
zcmZ8i&2Jk;6rY)0uQ&d7vPqnzg%wn7ZPB=D5h@Y1g$8b}P%DaLA-QYs+V$72nVqDz
zHCuxMO3H_XWQhYO`-ZqdIdbC9SP4{d0&##tB{=flI#e_}e!uy>nYVA=%)WX4uJo{+
zc{80(5P0fmUatLi`n!zDwmvD}^ZHKN>N&&muH$!wHz>E-;@)1pJgu<8M)|hYAN1O8
zt5&ys$F9B8vs@R{^7H{aqn9}swadHZN8hsa(z!gi)<JH7`2SEsB<T>Ft`VDcnZE`a
z{gU!0kA6vBr4!J!&Z^Be&<few24N)7gQa6V`*dbKnn?u7<C)yknL;#^3es{(>IEW`
zG69&DsRB{ZTPDZ`IZ=>tJ1a8=0n7sC0MC&FRmN)@VpGNsC?zsJ+nN(fL~?e@-k8IP
zO>UFo=5#lGewCQlTd*fWKD=|i$U@cWIs4)Cqx239r$44IZt_i-<0XV5K<mPdQ-A#W
z%I3A=>FM9;n_%>I=B+}yZ*`qo%MrB!_d22eADe!iIsaTfOtjs$7+Bou`@Dvp?jqC?
z8q>!a<6@`|`)wD|8iwj9;-iR<BR)pXY?Kkmc={ysBE2##WG_rMDYjG4bVa9Jg42cy
zfR6@(TmkWS00fa#GN~LWfhtu=x5z^RJ%gS?uRzZZ$ss*the}hmsm(stgP4rLX({7x
z5+*#i9I?k;(=zuh-nPV~?YSoWcA9%`W9RMzNT{0dyk560wt;)%lD%yW9sjbu{RVcs
z+i*5`NOw(f&k~VKJUky%$E<rp)A9#SLzvtWZO?Q**t2@3@XQh)?(oqLAD3P-_k2iF
z(}Ey|&crvpdIw)9`A)A{+KxOnV6V}$4v1;H0^)xUX)7R6kCYCBRDs<eYsx}{q#^;#
z&eS=1p9L&XfX5=P1~K4T#I+zU;xir3QK}OelbVdrwLrI7u!IvwBuQ*#og9%Pdd%n%
zJ7x?l6N1c^Y=*6+%qir4(pEPhm{~&7IgNTUrv`yQlLnDX23aRT3apgKD)m3B$w{*`
zn8l9Gz{xW%GPtlz!6nKxabdVJM@kLekl6w(R?C1j*mSb99I`o_5RQtxBnJ>yhuX0%
z(pBN2--@=N&FLh_xr_b8!6CkuOhyqS!^q)K^T9Hl%d*VELb<sTtbn#6^KyC40s|Zw
zawWoq)aMMwRwK=jtH9Uf8YG{7^Y<U%3ob4yd3=q>LF4c_$?H4@SX3fq0b3^r(73qD
zF%LM_NPY+5J%H(-`7;xaSEOp(_ju2)32wQ5)8l<iKbK!Z%M?JU+HGI(G*ZPGIaPIk
z-)V#zluxVI3RM?g2^FjES6BAi&ixa9s>Z<K{@}!DsMww;PS&e2j#URbYuxU0AJ-HJ
zdkFgo_YsBwRV|9M5`r;ASdA~POC_YYLN*+PY&58{@qq8*3s`Ha+PKL*S5#x;y4C1b
zV;Ekgszm`+LROhv3~8mPhO7(ar3>YyI}TO6OB^o`FCg>*z5?;D01)~DOHpW0Z&V7x
zGBiOmN{*eSXQ2+|m<IY9GgyixXaQzZ)BqVj|4fzKey!&<te)S#iX|Z`oJATcQ0a;q
z{{oh&GCl+ufs8}O`%x58P(_u03I_Z$gwGMsei3R9*h3H>zZ%g}5Fj*lxx{;q{#w@P
rmJ7o_JvCVDgym?Ca(9|HZqVlRT?7A)z#;mA(VvY<{F45vp3v#PvP9Yr

diff --git a/Sampling_based_Planning/rrt_3D/__pycache__/queue.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/queue.cpython-37.pyc
index 5a37d2f4d622f5727e4f8e40d2767e6d605fd0f6..d7f5ebed9b59f4164962970a7603d50cad350be8 100644
GIT binary patch
delta 104
zcmca1en*_oiI<m)0SKxW8OC$)Y~<5qvS?1WiU}=FEh>&Nwlp+0jB!aV&MwI>h)K^Z
z$tX>VDM&2I&@ISG%*)HnOV>@zNzX3=O5_&DOrF3b&KNVfnMs8srl_bS-q<B(^Li#t
FP5_gNA>#l5

delta 100
zcmca3enXtkiI<m)0SJ;*OyW0jZ{*WtGO5?k$j?pHPs~ZJ(055K&MwI>&`-}S$tX?I
zFGwuO&@ISG%*)HnOV>@zNzX3=O5_&nPtIo&XVjnE&ZNSjUsO~QZ|tJKc{7tHCjb-0
BALRf5

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 70b69f36b319d67adcc7b9098bb0e76287ab6630..ec095f0b58e4ff96f005c033a7d38ab8645c065f 100644
GIT binary patch
delta 107
zcmeBAn4G}v#LLUY00gIJ8gJy@$7Io(Y!wq)oLW>IV{B<?Y#8H`TAW>yUl5a?S&~tj
z6jP8`lA&9WlbDy6nU}7cn3JAg1eC}vj+y+INt`{VsHh~~*kv*=lN<+>9kbby`J4^_
Dhz}&n

delta 97
zcmbQ7(4WBV#LLUY00c=YCL6i;F_|>#XXNLm>L=!;R_ME=7H5~_7wD&FmSmJB=@%rH
yWat*;B<AI1=B4W<=A`Er0VQ&a^(Qkji!<s^mSk4p&@U<~i8pr9-|WhKP6q(U!yelJ

diff --git a/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
index a8a09d7..d4b7f10 100644
--- a/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
+++ b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py
@@ -12,7 +12,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
-from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
+from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 
 
 class dynamic_rrt_3D:
@@ -211,7 +211,7 @@ class dynamic_rrt_3D:
             ax = plt.subplot(111, projection='3d')
             # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
             # ax.view_init(elev=0., azim=90.)
-            ax.view_init(elev=0., azim=90.)
+            ax.view_init(elev=90., azim=0.)
             ax.clear()
             # drawing objects
             draw_Spheres(ax, self.env.balls)
@@ -226,11 +226,7 @@ class dynamic_rrt_3D:
             ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
             ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
             # adjust the aspect ratio
-            xmin, xmax = self.env.boundary[0], self.env.boundary[3]
-            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
-            zmin, zmax = self.env.boundary[2], self.env.boundary[5]
-            dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
-            ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
+            set_axes_equal(ax)
             make_transparent(ax)
             # plt.xlabel('s')
             # plt.ylabel('y')
diff --git a/Sampling_based_Planning/rrt_3D/env3D.py b/Sampling_based_Planning/rrt_3D/env3D.py
index 65a6ac2..9fc81ea 100644
--- a/Sampling_based_Planning/rrt_3D/env3D.py
+++ b/Sampling_based_Planning/rrt_3D/env3D.py
@@ -21,19 +21,24 @@ def R_matrix(z_angle,y_angle,x_angle):
 
 def getblocks():
     # AABBs
-    block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00],
-             [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00],
-             #[1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00],
-             #[1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00],
-             #[6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00],
-             [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00],
-             [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]]
+    block = [[4.00e+00, 1.20e+01, 0.00e+00, 5.00e+00, 2.00e+01, 5.00e+00],
+             [5.00e+00, 1.20e+01, 0.00e+00, 1.00e+01, 1.30e+01, 5.00e+00],
+             [1.00e+01, 1.20e+01, 0.00e+00, 1.40e+01, 1.30e+01, 5.00e+00],
+             [1.00e+01, 9.00e+00, 0.00e+00, 2.00e+01, 1.00e+01, 5.00e+00],
+             [9.00e+00, 6.00e+00, 0.00e+00, 1.00e+01, 1.00e+01, 5.00e+00]]
     Obstacles = []
     for i in block:
         i = np.array(i)
         Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
+def getballs():
+    spheres = [[2.0,6.0,2.5,1.0],[14.0,14.0,2.5,2]]
+    Obstacles = []
+    for i in spheres:
+        Obstacles.append([j for j in i])
+    return np.array(Obstacles)
+
 def getAABB(blocks):
     # used for Pyrr package for detecting collision
     AABB = []
@@ -41,6 +46,16 @@ def getAABB(blocks):
         AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)]))  # make AABBs alittle bit of larger
     return AABB
 
+def getAABB2(blocks):
+    # used in lineAABB
+    AABB = []
+    for i in blocks:
+        AABB.append(aabb(i))
+    return AABB
+
+def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]):
+    return block
+
 class aabb(object):
     # make AABB out of blocks, 
     # P: center point
@@ -61,25 +76,8 @@ class obb(object):
         self.O = O
         self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]])
 
-def getAABB2(blocks):
-    # used in lineAABB
-    AABB = []
-    for i in blocks:
-        AABB.append(aabb(i))
-    return AABB
-
-def getballs():
-    spheres = [[16,2.5,4,2],[10,2.5,1,1]]
-    Obstacles = []
-    for i in spheres:
-        Obstacles.append([j for j in i])
-    return np.array(Obstacles)
-
-def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]):
-    return block
-
 class env():
-    def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1):
+    def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=20, zmax=5, resolution=1):
     # def __init__(self, xmin=-5, ymin=0, zmin=-5, xmax=10, ymax=5, zmax=10, resolution=1):  
         self.resolution = resolution
         self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) 
@@ -87,13 +85,10 @@ class env():
         self.AABB = getAABB2(self.blocks)
         self.AABB_pyrr = getAABB(self.blocks)
         self.balls = getballs()
-        self.OBB = np.array([obb([2.6,2.5,1],[0.2,2,1],R_matrix(0,0,45))])
-        #self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])]))
-        #print(self.OBB)
-        # self.OBB = []
-        self.start = np.array([0.5, 2.5, 5.5])
-        self.goal = np.array([19.0, 2.5, 5.5])
-        # self.goal = np.array([5.0, 2.5, 5.5])
+        self.OBB = np.array([obb([5.0,7.0,2.5],[0.5,2.0,2.5],R_matrix(135,0,0)),
+                             obb([12.0,4.0,2.5],[0.5,2.0,2.5],R_matrix(45,0,0))])
+        self.start = np.array([2.0, 2.0, 2.0])
+        self.goal = np.array([6.0, 16.0, 0.0])
         self.t = 0 # time 
 
     def New_block(self):
@@ -105,31 +100,10 @@ class env():
     def move_start(self, x):
         self.start = x
 
-    def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_to_move = 0, obb_to_move = 0, mode = 'uniform'):
+    def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], block_to_move = 0, mode = 'translation'):
         # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, 
         # R is an orthorgonal transform in R3*3, is the rotation matrix
         # (s',t') = (s + tv, t) is uniform transformation
-        if mode == 'uniform':
-            ori = np.array(self.blocks[block_to_move])
-            self.blocks[block_to_move] = \
-                np.array([ori[0] + self.t * v[0],\
-                    ori[1] + self.t * v[1],\
-                    ori[2] + self.t * v[2],\
-                    ori[3] + self.t * v[0],\
-                    ori[4] + self.t * v[1],\
-                    ori[5] + self.t * v[2]])
-
-            self.AABB[block_to_move].P = \
-            [self.AABB[block_to_move].P[0] + self.t * v[0], \
-            self.AABB[block_to_move].P[1] + self.t * v[1], \
-            self.AABB[block_to_move].P[2] + self.t * v[2]]
-            # return a range of block that the block might moved
-            a = self.blocks[block_to_move]
-            # return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \
-            #                 a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]). \
-                    # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \
-                    #         ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
-            return a,ori
         # (s',t') = (s + a, t + s) is a translation
         if mode == 'translation':
             ori = np.array(self.blocks[block_to_move])
@@ -154,13 +128,16 @@ class env():
                             ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution])
             # return a,ori
         # (s',t') = (Rx, t)
-        if mode == 'rotation': # this makes an OBB rotate
-            ori = [self.OBB[obb_to_move]]
-            self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2])
-            self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,-self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[0,0,0,1]])
-            return self.OBB[obb_to_move], ori[0]
+    def move_OBB(self, obb_to_move = 0, theta=[0,0,0], translation=[0,0,0]):
+    # theta stands for rotational angles around three principle axis in world frame
+    # translation stands for translation in the world frame
+        ori = [self.OBB[obb_to_move]]
+        # Calculate orientation
+        self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2])
+        # generating transformation matrix
+        self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,\
+            -self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[translation[0],translation[1],translation[2],1]])
+        return self.OBB[obb_to_move], ori[0]
           
-
-
 if __name__ == '__main__':
     newenv = env()
diff --git a/Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py b/Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py
index b8a5824..aa2d63e 100644
--- a/Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py
+++ b/Sampling_based_Planning/rrt_3D/informed_rrt_star3D.py
@@ -18,7 +18,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, isinside, near, nearest, path
-from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
+from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 from rrt_3D.queue import MinheapPQ
 
 
@@ -203,7 +203,7 @@ class IRRT:
             
             # ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi))
             # ax.view_init(elev=0., azim=90.)
-            ax.view_init(elev=8., azim=90.)
+            ax.view_init(elev=90., azim=0.)
             # ax.view_init(elev=-8., azim=180)
             ax.clear()
             # drawing objects
@@ -221,11 +221,7 @@ class IRRT:
             ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
             ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
             # adjust the aspect ratio
-            xmin, xmax = self.env.boundary[0], self.env.boundary[3]
-            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
-            zmin, zmax = self.env.boundary[2], self.env.boundary[5]
-            dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
-            ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
+            set_axes_equal(ax)
             make_transparent(ax)
             #plt.xlabel('s')
             #plt.ylabel('y')
diff --git a/Sampling_based_Planning/rrt_3D/plot_util3D.py b/Sampling_based_Planning/rrt_3D/plot_util3D.py
index d4488da..dc431fc 100644
--- a/Sampling_based_Planning/rrt_3D/plot_util3D.py
+++ b/Sampling_based_Planning/rrt_3D/plot_util3D.py
@@ -106,7 +106,7 @@ def visualization(initparams):
         
         # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
         # ax.view_init(elev=0., azim=90.)
-        ax.view_init(elev=8., azim=90.)
+        ax.view_init(elev=90., azim=0.)
         # ax.view_init(elev=-8., azim=180)
         ax.clear()
         # drawing objects
@@ -122,74 +122,40 @@ def visualization(initparams):
         ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
         ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
         # adjust the aspect ratio
-        xmin, xmax = initparams.env.boundary[0], initparams.env.boundary[3]
-        ymin, ymax = initparams.env.boundary[1], initparams.env.boundary[4]
-        zmin, zmax = initparams.env.boundary[2], initparams.env.boundary[5]
-        dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin
-        ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
+        set_axes_equal(ax)
         make_transparent(ax)
         #plt.xlabel('s')
         #plt.ylabel('y')
         ax.set_axis_off()
         plt.pause(0.0001)
 
-
-def make_get_proj(self, rx, ry, rz):
-    '''
-    Return a variation on :func:`~mpl_toolkit.mplot2d.axes3d.Axes3D.getproj` that
-    makes the box aspect ratio equal to *rx:ry:rz*, using an axes object *self*.
+def set_axes_equal(ax):
+    '''Make axes of 3D plot have equal scale so that spheres appear as spheres,
+    cubes as cubes, etc..  This is one possible solution to Matplotlib's
+    ax.set_aspect('equal') and ax.axis('equal') not working for 3D.
+    https://stackoverflow.com/questions/13685386/matplotlib-equal-unit-length-with-equal-aspect-ratio-z-axis-is-not-equal-to
+    Input
+      ax: a matplotlib axis, e.g., as output from plt.gca().
     '''
 
-    rm = max(rx, ry, rz)
-    kx = rm / rx;
-    ky = rm / ry;
-    kz = rm / rz
+    x_limits = ax.get_xlim3d()
+    y_limits = ax.get_ylim3d()
+    z_limits = ax.get_zlim3d()
 
-    # Copied directly from mpl_toolkit/mplot3d/axes3d.py. New or modified lines are
-    # marked by ##
-    def get_proj():
-        relev, razim = np.pi * self.elev / 180, np.pi * self.azim / 180
+    x_range = abs(x_limits[1] - x_limits[0])
+    x_middle = np.mean(x_limits)
+    y_range = abs(y_limits[1] - y_limits[0])
+    y_middle = np.mean(y_limits)
+    z_range = abs(z_limits[1] - z_limits[0])
+    z_middle = np.mean(z_limits)
 
-        xmin, xmax = self.get_xlim3d()
-        ymin, ymax = self.get_ylim3d()
-        zmin, zmax = self.get_zlim3d()
+    # The plot bounding box is a sphere in the sense of the infinity
+    # norm, hence I call half the max range the plot radius.
+    plot_radius = 0.5*max([x_range, y_range, z_range])
 
-        # transform to uniform world coordinates 0-1.0,0-1.0,0-1.0
-        worldM = proj3d.world_transformation(xmin, xmax,
-                                             ymin, ymax,
-                                             zmin, zmax)
-        ratio = 0.5
-        # adjust the aspect ratio                          ##
-        aspectM = proj3d.world_transformation(-kx + 1, kx,  ##
-                                              -ky + 1, ky,  ##
-                                              -kz + 1, kz)  ##
-
-        # look into the middle of the new coordinates
-        R = np.array([0.5, 0.5, 0.5])
-
-        xp = R[0] + np.cos(razim) * np.cos(relev) * self.dist * ratio
-        yp = R[1] + np.sin(razim) * np.cos(relev) * self.dist * ratio
-        zp = R[2] + np.sin(relev) * self.dist * ratio
-        E = np.array((xp, yp, zp))
-
-        self.eye = E
-        self.vvec = R - E
-        self.vvec = self.vvec / np.linalg.norm(self.vvec)
-
-        if abs(relev) > np.pi / 2:
-            # upside down
-            V = np.array((0, 0, -1))
-        else:
-            V = np.array((0, 0, 1))
-        zfront, zback = -self.dist * ratio, self.dist * ratio
-
-        viewM = proj3d.view_transformation(E, R, V)
-        perspM = proj3d.persp_transformation(zfront, zback)
-        M0 = np.dot(viewM, np.dot(aspectM, worldM))  ##
-        M = np.dot(perspM, M0)
-        return M
-
-    return get_proj
+    ax.set_xlim3d([x_middle - plot_radius, x_middle + plot_radius])
+    ax.set_ylim3d([y_middle - plot_radius, y_middle + plot_radius])
+    ax.set_zlim3d([z_middle - plot_radius, z_middle + plot_radius])
 
 def make_transparent(ax):
     # make the panes transparent
diff --git a/Sampling_based_Planning/rrt_3D/rrt3D.py b/Sampling_based_Planning/rrt_3D/rrt3D.py
index f2ee776..096ea74 100644
--- a/Sampling_based_Planning/rrt_3D/rrt3D.py
+++ b/Sampling_based_Planning/rrt_3D/rrt3D.py
@@ -44,8 +44,8 @@ class rrt():
         while self.ind < self.maxiter:
             xrand = sampleFree(self)
             xnearest = nearest(self, xrand)
-            xnew = steer(self, xnearest, xrand)
-            collide, _ = isCollide(self, xnearest, xnew)
+            xnew, dist = steer(self, xnearest, xrand)
+            collide, _ = isCollide(self, xnearest, xnew, dist=dist)
             if not collide:
                 self.V.append(xnew)  # add point
                 self.wireup(xnew, xnearest)
diff --git a/Sampling_based_Planning/rrt_3D/rrt_connect3D.py b/Sampling_based_Planning/rrt_3D/rrt_connect3D.py
index 08d9be6..dad3176 100644
--- a/Sampling_based_Planning/rrt_3D/rrt_connect3D.py
+++ b/Sampling_based_Planning/rrt_3D/rrt_connect3D.py
@@ -16,7 +16,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_ba
 
 from rrt_3D.env3D import env
 from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset
-from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
+from rrt_3D.plot_util3D import set_axes_equal, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent
 
 
 class Tree():
@@ -161,7 +161,7 @@ class rrt_connect():
             for i in tree_b.Parent:
                 b_edges.append([i,tree_b.Parent[i]])
             ax = plt.subplot(111, projection='3d')
-            ax.view_init(elev=8., azim=90.)
+            ax.view_init(elev=90., azim=0.)
             ax.clear()
             draw_Spheres(ax, self.env.balls)
             draw_block_list(ax, self.env.blocks)
@@ -173,11 +173,7 @@ class rrt_connect():
             draw_line(ax, Path, color='r')
             ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')
             ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k')
-            xmin, xmax = self.env.boundary[0], self.env.boundary[3]
-            ymin, ymax = self.env.boundary[1], self.env.boundary[4]
-            zmin, zmax = self.env.boundary[2], self.env.boundary[5]
-            dx, dy, _ = xmax - xmin, ymax - ymin, zmax - zmin
-            ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy)
+            set_axes_equal(ax)
             make_transparent(ax)
             ax.set_axis_off()
             plt.pause(0.0001)