From 6fc5615e1b2005a2f64fb06b285b4dab47bb81d6 Mon Sep 17 00:00:00 2001
From: Guang <4914320+gy2256@users.noreply.github.com>
Date: Fri, 18 Aug 2023 08:18:23 +0100
Subject: [PATCH] modified the code to display results

---
 .../__pycache__/__init__.cpython-310.pyc      | Bin 0 -> 278 bytes
 .../__pycache__/__init__.cpython-39.pyc       | Bin 0 -> 276 bytes
 .../__pycache__/maputils.cpython-310.pyc      | Bin 0 -> 3150 bytes
 PathPlanning/__pycache__/rrt.cpython-310.pyc  | Bin 0 -> 3698 bytes
 PathPlanning/__pycache__/rrt.cpython-39.pyc   | Bin 0 -> 3664 bytes
 .../__pycache__/rrt_star.cpython-310.pyc      | Bin 0 -> 3396 bytes
 .../__pycache__/rrtutils.cpython-310.pyc      | Bin 0 -> 2469 bytes
 .../__pycache__/rrtutils.cpython-39.pyc       | Bin 0 -> 2459 bytes
 .../__pycache__/sampleutils.cpython-310.pyc   | Bin 0 -> 4896 bytes
 .../__pycache__/__init__.cpython-310.pyc      | Bin 0 -> 250 bytes
 Quadrotor/__pycache__/params.cpython-310.pyc  | Bin 0 -> 794 bytes
 .../__pycache__/quadrotor.cpython-310.pyc     | Bin 0 -> 4085 bytes
 Quadrotor/__pycache__/quadsim.cpython-310.pyc | Bin 0 -> 2833 bytes
 .../__pycache__/quaternion.cpython-310.pyc    | Bin 0 -> 3275 bytes
 Quadrotor/__pycache__/utils.cpython-310.pyc   | Bin 0 -> 3041 bytes
 TrajGen/__pycache__/__init__.cpython-310.pyc  | Bin 0 -> 287 bytes
 TrajGen/__pycache__/trajGen.cpython-310.pyc   | Bin 0 -> 3822 bytes
 TrajGen/__pycache__/trajutils.cpython-310.pyc | Bin 0 -> 1738 bytes
 __pycache__/controller.cpython-310.pyc        | Bin 0 -> 2055 bytes
 runsim.py                                     |  51 +++++++++++-------
 test.py                                       |  13 +++++
 21 files changed, 44 insertions(+), 20 deletions(-)
 create mode 100644 PathPlanning/__pycache__/__init__.cpython-310.pyc
 create mode 100644 PathPlanning/__pycache__/__init__.cpython-39.pyc
 create mode 100644 PathPlanning/__pycache__/maputils.cpython-310.pyc
 create mode 100644 PathPlanning/__pycache__/rrt.cpython-310.pyc
 create mode 100644 PathPlanning/__pycache__/rrt.cpython-39.pyc
 create mode 100644 PathPlanning/__pycache__/rrt_star.cpython-310.pyc
 create mode 100644 PathPlanning/__pycache__/rrtutils.cpython-310.pyc
 create mode 100644 PathPlanning/__pycache__/rrtutils.cpython-39.pyc
 create mode 100644 PathPlanning/__pycache__/sampleutils.cpython-310.pyc
 create mode 100644 Quadrotor/__pycache__/__init__.cpython-310.pyc
 create mode 100644 Quadrotor/__pycache__/params.cpython-310.pyc
 create mode 100644 Quadrotor/__pycache__/quadrotor.cpython-310.pyc
 create mode 100644 Quadrotor/__pycache__/quadsim.cpython-310.pyc
 create mode 100644 Quadrotor/__pycache__/quaternion.cpython-310.pyc
 create mode 100644 Quadrotor/__pycache__/utils.cpython-310.pyc
 create mode 100644 TrajGen/__pycache__/__init__.cpython-310.pyc
 create mode 100644 TrajGen/__pycache__/trajGen.cpython-310.pyc
 create mode 100644 TrajGen/__pycache__/trajutils.cpython-310.pyc
 create mode 100644 __pycache__/controller.cpython-310.pyc
 create mode 100644 test.py

diff --git a/PathPlanning/__pycache__/__init__.cpython-310.pyc b/PathPlanning/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..644db30ac32e6500b41749627105ba7cf017bf96
GIT binary patch
literal 278
zcmYk0O-{ow5QXhHDHUoZjzJd17pM>yXjK{zQa08owx}ilL}O>sBXJ=X*|OpatQdz4
zPx5^4c}8Q$?e><D_HUp1K>RA<zbXn(bkaQ)%`|t+^PcnKD_42Zi^U69`=)Qm*Bxt{
zZ%*SapT+t7LbxGx8L-?OU_7>~Ss-RC+xs;+m8gqii5|u&TbHn=_`z^RSjx{)z9xf7
zdKe^{WB`YY8lpDRU;p?#fyS64?mFuy2ie9TPmnJs2O-$-E|s#uW~Cm+>%89ks3&J0
O%WqS8XTvu9j{gF&#6<xB

literal 0
HcmV?d00001

diff --git a/PathPlanning/__pycache__/__init__.cpython-39.pyc b/PathPlanning/__pycache__/__init__.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..db20dc4bd75e9e6b34d86793e97612b195b08a75
GIT binary patch
literal 276
zcmYk0O-{ow5QXhHDOJ_pgDk)os8BD^sx%;^Y^+giQA_@b#?GQg;zBI4WyKX(F%BD^
z<oVw7jK+rDZo^3Xw@-Z}ewFZF6@>$xbWcSy%^mZ+=e+pJRbKRB@xs-<=^OHO$J*wb
z(|F5gak;z@?g(85EH_6OPwi?Jh#AZFeoam#>Y`YphjGf*C9ElaFkBIq@^g}}$zYNm
z28kvaz~QQfsEzcuKR!>OG3JO5o%NH0Y-5mT$k(%j5NvptO4(quQjg<pUhjR>lQU1{
Lx2Zh0<y-y>7@kDo

literal 0
HcmV?d00001

diff --git a/PathPlanning/__pycache__/maputils.cpython-310.pyc b/PathPlanning/__pycache__/maputils.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..ab1c23941475abb3f1209504e4b0f722e8f97dcb
GIT binary patch
literal 3150
zcma);&u`>N6~|rW@{e{pnckU&odvXb`LWOk5~N)*XcQ*P&cKQT(Xas)TD!6wSEb{O
z+xC>*WYTh<GS@k9gwtd>?VkWwPMncA)Cp-1D0fbX0lu%C4w;Apwz~S&?|Scj-YZw@
z>yd+J`@_G;-whn+GpbyDTvXo0uepapIul1S$qP2)6V6C>3wI*$FA8twPkiRQ??_Mj
zPaWx>@<|{A89sF;p>+40@nF$LaXd@wc;4nk1G(C7+B7SaZ9LA~!#JyoB5(4ljI%?P
zJ<7{zgmLM-J*@QI_`Snarx<nrFfFA{(W7Q*UfhlIbhh93WqsGXQ|j2vs#$c7lRIN(
zyu6g^$u6Y92P$jRa$2a2`mR2g`aVFbawZHqbNPyNrFhCFt`fa=r)=U~=;q6TjG$v6
z*W}RZdQ)!58yG#1BWQigxIa$ozxA8^b?2OR$h<3ik}UAMk6-f+icWM+;&je)*0D~U
zanyM0cI=cvayu8LK#!An9rgLr*>gT+yW_yPGM|}ns@kO0N*OnoPmEir(u6h4mT1+O
z!OG^1@%O8FDVwopM5BrWv%cy|;DT0f;3($CYGPHuX;rC1w0`R7oA~?3?$*y5rJJo;
znr&fimFny;$ttO~W+(msPv)uARa@zA?d7w1k<t=d4{?@<MOv2dF6P#jhRyoKgh`T@
zd7C6RuzE8@;j;}EF&}Tl7DL!^b2TL6=w3Z!zDpBsqSC^6XU>wtOg!Pl>{ACObWYg=
z{FZL(90?ebOZORvkHq2|v~iw7@hdP+^Ixg*5>jiGjIH*>Ke>{;w})R7qnN^L9}hp`
zOGaCx#y?>nAZ!lYU-QS<NCy~XZ8vre-~6^=xpAxgBiqy$w=lv)+vEYjTi^Q<>J8}{
zJByeMu=4Kac2Q+%(cFWu8Vt6hT35BwZLZ*0O%qM{ux~ms1Ms0r3NJHI<$R_PP0Dyb
zqQDpnJ;G?~V8d#|i{{#~G!lgK8h#By5V}6Q3HS7FS)J-7x6tk{xutWCh-&8u);U80
zf#g01%V&&afp*F6I2^o7a*MkdrBcSFI=*zmIoW>~no-&^eqI8=R<EI=*KJE$*Q%6b
zu8E;ZxI+4xD#pdaU&e^WM=|0<sImyiylBtJud9bVMn1OD`Ev+BhdsAo-FD6hB^!}P
z;)?{90C*ljhq!f@Ugte<J{~Q7=;=QXz=KaboH5BBx%!((pwRO1ZJQj$;(G^3P;q_`
zw}&d$=}Ek=+G7O(kt1~*Bjd%;tj~~AHL=Z?EBzWxhWY6a$D!e6ZTuoH(_(5wS?L+_
zK-p>vCljK*snbmL_VY4ifOZ^MV;RqGWL)fGgalKDmBz0t%x?4>n10=egUf9Fb@Vob
z&xmh;iI9CBA<O?Q_+p)mdO7YTJO~X!#G3Xi5C{(h4__iOJNAfC3|AwV)9qy-!4_}c
zr;CB?RL2D<kD*a6`>4H0XuQu|Zdw$enn19}H(pU4D{Z_v7}3|23G=d_zJ>7(oh{<G
zFX&!%=@`u~;X&jPidKg|K<GH-9zy(#z3M;{AP#qP{oT$53~wz(hx_t%BzehScWyg?
zL`c`xg{}K7jX)ntv>fox>wMfgLi*3VWzaDI7Er)Ez9FMM8$!~>`XAc$KL-T;PMorj
z+-0~NETfJc1zp$;x(F)>+*v|~&jQTeq1kOH2d7wz<nh|#W?fVuttc+ztvM$TS-3QX
zinV}YkH*{po}$vmUsPHuJr>j7{)V34?@Zse&mUL9pLZ_0{<Jd|CeXNB;4l+py`aBC
z>)t`Jcr#1OxWp0cISwiv_bK=?Z8f{&f&LnX8ZXs|5gYV2WoZgpGeGzzxU#ZCvzE>G
zt6U~BZBsL(1(Rm355T_hY7M5xYd!WneFF56N>!gi-2Li_VTEDGlK^5;7YKx381_rU
zCfA_}XkEl0UE2+%>Jg7X{|N6JY{<kvBQHcee1VC74c)~W#lppmU8E?=E7q!GX6-tT
z=n5>;nMx89CCRLk^Md4Il02TLMc<=eB~6D2);7;(3SUOjW@i0WT8`+|Z&UF;6?9Ku
zDQ;1f*rv<f25jg=jE6XM5c*-X;o07oZaK<;l<+O$e}~ZF!w+~Ods$!sy9L-GTmdw`
zNu+1{eOq@WWlLPal25(=N1lo$0stfN_H+I#PaPS2LizRLoq8DUU9?grwlD32Igl^s
zU&t7ZY)mJ&VJgnpYorNIZtLDf9VfSSpXvwn@fz;-{8O4eCktASom&@@_i-gr*Z(Tu
zX+l=trD1{oKFRM<K^8Ua&@k|JV%Xxkqqfoi1N@q|P!LliFx9`}#ZU~9W5L)99}>Od
z#XCaNobkW}Ng}HZ=y<pumFhnPJY4zpNgqG2%+i)nEb@JPdXZ!!$hzMya^)}<B0|T6
W+dY%_NLTxXCY)gyV)G4mc;`Pm$>w_i

literal 0
HcmV?d00001

diff --git a/PathPlanning/__pycache__/rrt.cpython-310.pyc b/PathPlanning/__pycache__/rrt.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..f16e241cb535b67fa495ed3fbccd0061cc7d2501
GIT binary patch
literal 3698
zcmZu!TaOz_74E9Oxb5*clZiK(ToxJ@&>E2p5EjsGU;<gRTt`ci6_T~6)p7NB+SBfK
zs>@8qawMdLS0o-m`!MD$KLrV15vnI79#$jq3s%7LoztGHwymzN%jxQ>?|kR0<8pPi
z<>0#atG}y%TymU$QRD38qH!O0MTK&d$eq66ryRR|Pt<+>IFOET#*Kc69;w{C`Pg~S
zQJ(T&I?8_``z<9NIlbTrvpCyF0%cEF@znvf&t50r%h8+ixB9XvnzHFOJ#|C*DtIaS
zu4<_8rPKFPUo}<hrSn4c1GS>s=xeA|)xk4Vm(&`bO?6qV<JnSIR2R<`^_sehXItG=
z8|pe{TUFPvwp-SH`0(+^^<ujBmz95Ref+0C()DWVNMURco>?u`CGH2fs~@8<&Ro0@
zb0-$4Nv_GchqgNpP<rzQN`Ed!ffCmp)Eg$8JEI0$Hs>ySTFSv<<hEvvR;;&Os?_?G
zNwl%vuuO7$d7M0p(`P0vRGg>9(CpeZ8i=c8Jjv5oCnmKa8D%Ebcy~9Mq}Em0xbHtS
zI!&#|r8`b0z0i79n(t_$fnGsj-CraV-A1i{X_A=TSf^E)Pfb=9dX<biC|)`GH9S7~
z$>s-Ds;kX$GT4M&n&`o9Jiv-K#|QQEm(xV)(v<r4qij6Q6Pjo9ea!oQo)kq^3^%nl
zcP0lmjN`1xOdJ!c)f$SH-x2};+M+F5V%CO(?i`y92%zK3M}=<o#7C$ARp$_%7|DeY
zjsdbG?}d2R`SsRw^d9;P=ecB?U=EAt7(Z+Pf4Pv);jdA62ycv9b8pmI$m(ZoDdrx{
z1<Muk>8QP+8CLg%rg3@$*3`dzhi&2x>@O`q69K<IZF3mN+1s@HDl)riL>Naqx*SLB
z<H(c|IWyYd&GIx#ep6-&C`IKH3=i_O+U~9C%h;iH6Q$T~*4IffOxZE^(l9k(K%vTU
zTo82{1-z10hB%a_TCd~XesfUfc~-#}R!pp?vdXaY+3z-(;ElrA)=pM{R<y#NudmQY
zMb-~Bct=zB+smoz4~0d#&o!n-U0kJiya_f-wdw|nj_im~w((n+ZP9hRB7}G4Z#_8+
z$-iVgpr|^&sP5rr-*!<k&PW^*_AVmH-EbC=mRDlC=UG`yY>?BUht?}fJ=U+oz9(&y
z0AAu@%FkM*{v0g7hP$Hgb=`U~1k8Xm>G&d&oh;^uXu!P)dM1ub0X!0Wp1y^J$+;ZK
z4F`0hx;J-k5-CO3*?=^}lHP$VNCdl>O&^p+kq%77MBwES50dB@(kLsKEGpA5gWIBV
z2NrDEik8=Y1|}iw5ohCwRt_Lj>NqbJC-yy$H_bk0p!Dalp{1wwo2cs^6@(0NWM}7s
zXZ0<Nd;@nya?$o$aOhP8tS4qoawQY)T$~b=C-8j*4L|}=K9tV|;!zkmat|qP`~~2d
ziw&piTtnRGFTu1IFn4Y6q?{J2(qBfO4Qi@ekDi)TqvzI4_49I{)NeYOfc2zSXDAue
zQu#gfR<xeBydh>SUNdXxxkVEp=w<?b9hI7Z3tGGjZ?Ay^9t1_02Fd>;>i0*%X&WAX
z>k-cz)ys`2eBje6!=v6#RO+Z+R2}}4GAm3k<XvjwrM^uCD=@904p*YLZldT(-gVza
za9-G~rl=#TR;2N3G8Ca3ej19|7259wxj46R@>YPGH~s{bAvQ@KgRkTW9vY$ClZWoY
z(U+;k{ZdDB0WV^V_41^@>T*AU!y_hu<_bkW`8)SI$8pb{z4M?PPo^ei!m|RM(MVEE
zmPbYqF+v@+a8l3q(Wkfo8=YjQoQ78BxeXFf0r2oq$L#SFt&@Xe#&{%jq#G)|2w{Zj
zeu}#y_4H&QHbC<>olXDiK*44mg7Q=?&*>NOi)4dQq%46DJQNEDr_rq?VqY-sh!FD`
zQ-4QZcW@>lPZ&CL_s>A+58@#wR+;!c&Y-8h@*o`ktUJj|!x-%EmN+hkI+^Uw<n5si
zW~IhHj_AwVy#^l{`kQEQkk`Q@2Qgzr3SAn=0ZY70p6PGH(8^~A7jUpGrIY=bzw!Z0
zIPN=uqbqKZg#Wh&;?(Gzb`qUVfVhXoTq-ejbjx6CFR(2-9I<IJbPPyE2lL3o=3U-A
za!D0@Bx(D6h&E<7=sUc5K0~t|oR&$JCr|Qpd$!Jf1<*O^{6yUDdBeZ`m9Bqp4ePw_
zFI$=@zHA+B>F=V26T`;|S!(?qYWdcmXoRDF8?8DbzlE9=kC)WZ@6f<oR9q06CSyTs
z`4FQOeF}%4cvH?+i83dn+vkuX%FxY}F{mt&dc!brC|wyzCHD~Y#syL?QW#l4htTVy
z_jTBjBFQ2!pvnoh&LO8eoWwLpU>B0=oV?VDl5^NvAXQ<6L9Y{pYkF0Kv_OoJBacWC
zXI~s3i37$=!yQQ2V5%u~!0|IqA#Y_-^;WDq$&C$mvf)&xtkE|3!Lziw`>yp8JoSA{
zrAdo5$yBZK4Vn^357N`!a(~Iq8e#(5x|r@F*fl$OkptLN8z@5PU5nGl7Saxn>y%EO
zVJg3XFAz7G47x!z%0rQeZ}&Qsn-<A9jbq!2<8i5`$g*SGj^n4(B(F#GHG1a_6t)dL
zGTC69n%z>>M=Twbnq`~BSVvTxZusZaJS7g-C|}aV0isM5p$Hx6N<qKYf5MKuiv4(D
zJM_aaTnQVU23rGdznG3E2X%zwuYw^U^Xv(7Y^ou-<EQ=+rnEQlhaom)neSz$x`Re(
z?y5WFl)K6{mTd?pcj))<3erDq@NXcm_<NtVY#ZLIG5S8G=zQvgPFt)a(OZ}Q2ULz{
Af&c&j

literal 0
HcmV?d00001

diff --git a/PathPlanning/__pycache__/rrt.cpython-39.pyc b/PathPlanning/__pycache__/rrt.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..3bc107a782205246b29072694635ea5e4a376493
GIT binary patch
literal 3664
zcmZu!U2hyo6|JiNn09+?CyqC9k_AR#SuraT17RV`1`^1^0s?DEvO*dyv^rDmagTet
zC)G7L8I5^Kctzq7v=1KM@>5oVSA^;biHDUXe!&U}&b>W$VrzTU)%9_^y6W6>Pu)pZ
zR$_tc&aeNW|G6f_zp3&1<)Lv8cTI&BS{9-w`Kd-;&zDVKFbb8BV$|wI=uz4$mL7=@
zg!XmtLg?VR>cv_<6y5L$vxw~@iLxu5e5X(C*DoK9d$?<jBI9rMl)j<^9lnq~Pq%dR
zLiGGB&`UagA)d=#sF!sceJ#DBSMiMWnm&i;l0L83@r?Bay@BVlenVfxv#qb{O??@&
zt>{Zw)0@tF@ZiyB_#Cc{dyC4y?tb={KhyQ{?h!4eAMtooP+j4^kGuXU3M*#vxtxha
zqNcc}W<J{9EJW$gS}22=9EMt67O1yuG!w%XTQ1E!^u$_VF>2c|Hp|Z6u5{*t+NQ=j
ze^8}`J3mUFCfQS)l{zW1a$tAeIT}dnbTlrq#H2QJ5gFw+GkAA59cRwd`KT8>uqMl#
z&!sm?$KA;Jbyn;ca=%$d;k;j_W79^hw>D1gZep^!Dke6sO0z;nt0-Oy^9CND|7`1{
zIy3dwDD7{-E=x^+H|b-=Tcc;q^H-Bpo61(^`onxQDN>qe>qE@@VUd<)UJka5u{Xxg
zT$ChvncE~GRO@pnL=ekZhWu;Gww$)%pc}_#eFEtC3Q(b&J@E-DKvf*V6GJtZQdl56
z^q<T3#czIpfZoGkE)EpigfmziVEnKJ{MB3?z+c1Y5Z)NZGk+M*RlUiUa^}-quv{je
z4%>5@VP#Jm8mBj4P5p~^*d|$p{n{Kfk?`x2O{Tw^-lpBx9lM)#2;<I<sYV_4amQ92
za%N|LH!reI`nxLEK&ewb#&EyL>h111a~?Z%UaB>_%>^bc2N^rYtqn2@29&xQB_&a(
zRl+M-ZHYr!X3RR??Jf1IqR4Cb!pX7obzWO`KKtE;W4uvX7w_aHXhkdR2Ic~NROY?N
zfOj-?uf3SM`A}45`&?scG{r@F#~)*}RI4{p2(>C#RixVZt;@EzAva`%o@qqhCDT5|
z)bT}i5(D<^1}atz<sl*OA&$IFF^8bMl-ph3sdDVXf|fjRep#83`4a5A$}JJR3p8x?
zMQb!)hUGVK*Zgg-Sq#C__t1NM5yegx^CL9iT!cK+#-s2Z%01uQ!ot){4b`RqnW*m1
zysJb>xgj<o42hz5APNehE~k_GRas_zTQd!KdBlRWa|~!FFPSPjH!y?KI@Jy=*fQ=c
zuKgT5LeL}1Mjcu?fXt}lv{anfcYWS8dz^tXU%`eJel}l6-E^rSWQZX<J7@fA-o(ha
zaMvUj!f&e>L3$D4I$a`HGTlx^DM5Jx-`i*a5`gltI*^D)Y1Pm>q^JqzfM+H*#fG?q
zs4;KBv>P&WUHG_~l)5%wL!S#9qC20ShBTw=H9QT9YM-=kG9H8Tq*bpGGHitM`{=D{
zJwicTVqP;V=&3~$A?RiTzJp3bz&S16gSW4M13u(LmL%oDFg%d9MaKa9_T7g(W2aeY
zhr$NltaCh?jdUv0X_nK3{kY0Y+l_dCh8StCQ^CsnO{$SbH~v~et*dyyJr6;7W}D^;
z#)+ggDXLKCWUL}Ny+HdsBNC@JOkN6c^M-$m$^e@qPr*lO2+s^r?x{m>F3dG*albNm
zW)j}R7^@`rKe@(z1Pk}t5GpG*{nQ`4%N)C1Z~ET-YBZkMj7iR#^O`D>MzSC>c8Ck=
zXrz*QwvYaY3xK`Cg?J2gED9H<a6f?I!;Kjeq{gJrj#=W9w2@Zm>@0X4O!9Nw%}Jzu
z(0UVho3u(!R|&*d>Uc`Ah*2aIj2~qMgx#T>3!FZ;7FfmB8xmFlqZ#a|%K~Q*@`8bw
zd4B~yf07S4fy(6{appXUwGTQ4(~WUaSw>)gx5Du-FzI-As;&=QIIRqJaYSF<?zZ@d
zFyBUl!@3C<)lZl?q{x+p0I;O1^r`tS44r!V><kJn)+XIg_$wd5WWl6W0I{(E;g!KD
zr6dxafN&d)nbLA1%(}(4o?}bpTiC7~2n!<75j^y<ZI8E&Y*B}wDB3t5m#yg)`U-EG
zPs@A<=VF={>Ej~Xp00CW30zJ(KbP0L{^0L_qw7C+2Tca|>0Lv#UbK$xnp<e$bO>-3
zR>pjfT7e731_5Yj$xRG?2Q?=jEhuB&qk(s+I3p@cj0COaV~p1HDS>#4)pUi(ax%Jo
z3MV29-AopX$^xgKSf&gmCPSswo-%J+4@h}PR#e&HUJt#uVMpqt%C-$OQk(`eXc8wj
zoUXJ;Oy`Q~oTfA>kvNR!NJ<!C5bFfthFEn!D-m1d#v@X{>9s!6HDJsH+<|EICx(&)
z95tg1f>xDvciDO4!n$xLA52WfdhEg<KF#Wz?>j%m)7-;UhIH4EFg1$aqA8KyAQ9cI
z_7|*dASSS#hw09O-SCnZ*@w*{v4{|ybOs@91oA%Dq3j&2j6c7CFAy`C45mdj(nqA+
z+uc>lLd$fNC5elZWK`)1^6A92ljO-HEt(N?iQf4t3fG1X*}Ol>>~5u-<CG3X!$M8+
zYdTcC+VC%^`HDDPqs+(<2Z%B?9k-DO$I$P;@oIEY$=HwDQ4mGZa@2}iYzwS|axxk}
zYa$ze2`s@_<d2a{Qw>QRKh2LZp}UGd28pezVlTJ#4KylyQ{Nz$+|;hMXhSZ!!+d~O
gkmebStiP!FdtbC@7u{(P`T?cltPK&+Uzd^kKb-7fX8-^I

literal 0
HcmV?d00001

diff --git a/PathPlanning/__pycache__/rrt_star.cpython-310.pyc b/PathPlanning/__pycache__/rrt_star.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e62fdbf2187718b9de60ca2aa57a058d13dd8661
GIT binary patch
literal 3396
zcmZ`*TW=&s6|SnjOwYw*uh+?1Aw-Q52$QhuL?9HQh-?TKA%PX_K%@~-t20#|_iRu1
zY*o*C*BVO^UMar;o@O5RH}D7e1w7Of5)aY-1B8I^o$4O1$ATW!WvWhH&Ue0Z>Xchs
z9ft4yPyZ_ae2cNa)1T{~k3aWO%nwjWCV9?2p;qL4Du#kH=}ABLrvA|9>=#T1GJMKp
zsDevx7|PZ$8${1JmJYb(Z=u@$WpPsKsglR(be1dq9CI}3k5rbA=+E_!PN&bDpi*qe
zB^!!1Z;Q6q_T`-~*$H<iwuhc<$>=E?`m!xMXam`mJ+z_Rl6|x-xh;3lM)H>2McbCQ
z<sRCOd`Z5Hw)-XX*|3NE?%USU(I>}Ms=sn)vp4QHoxgqX)t~-I-?I;%{{?fSs1|TF
z_1h>Oin)iXVwY^m7knWW-ojr5i*V6ewKyBI1xrNbVVqwI^nJ;W*)x7P2y9^HGo@{m
zBw3MFNn*PvS&`;RQA%YrZT$j3Qv*1NtZ!6)g0;AEW&LrP=GOalI<vQ?>ElE_u2dnD
zTovQ$)Naqx>NL^Hl=-~M%EIoDcvA0`=(JKcqE%U?^fPYTNixb)W0K?<`{&`oZ;jIC
zV498&u!BnV=rkGOW(U(Jb^DunDs@?v`tETyo#!c?aPTos{5VgGA}huRT2~3(^WN-<
z-hw(ULW>yK4?^yL9f}=(zSF>eZ;ir;2(<n}bOMx*P|Q<Qk}ueVB~**0NBtG&tn$$d
zF8NQ{XYmE*mf?zB0By*$7GlLOAi0cS8L<)<B->tg7T%=0@F(4sFz;X%R->IB?qIez
z*)s1*aS;@dUG}k80E^N%KM^2}cm5;N(ZtoMiV5TRM3>XpnIx{tm{5+-PP1Ia>7z1}
zz#=X`$Mh(NVTtC4gKZ55WblO2z>Jkv%6h4kHqdD?R{9Q2ZjV(3GZnI&I#SqH0Xrxx
zfcX`u<4s}s_NdJB%#eNgOz+ada0aoWvYnbxbj3lacW8=d*3*Twy=JR=<ET()b<ZbZ
z{R%|u>KnAkpFv%^uh~Zxi9YZ1Nc8aA6&)eI=!=N=_<3(_OA;Rmh^{{(GwJQ@_y`@~
z$`%}!ft6~%Ss}bQi+MCo{iRR-RF5wM^w}g_kyTp{xPBLHv~1JZq_g0Y4(v!{-DU4;
zy<0(Jv<LZ}?&9ddmhF)8wV&Co(P>#4mDG^wZ$d)9hRTLoon=~CUuLFqxavKce4VN{
zsQNZleN;9~XEQk7z;ht$w`qdH!VMtZ#<a?iEjpYK?1?@6s6Owm@dFV?1ljr{7og(c
z`w2P=0jj@Bw4ONlCI~wdKxJM7Y#xZt9nk^c#D4&NXe0>`n0Vqp6q19);`|TCs){Mt
z+^cmb>Jp>#@(dh{L9}^Z5qF8B;5kt$&WfgIVi2<vzxdovPPEx)RBG%oD+}iUQv|sw
z{DLH|mK}nYn|NABMH14lLQ(x4RD+=Au1`nUR(%uWrfQ*0y6OY0`5}r!ISO`xZbyVX
z<l+neALlQvQQk282Hr%~E4=9j4)3-LBaE;d178G-hP*`A2Gb9Z9BE=An8Ox9qQt-2
zyW;Ew2xlff%ksPdm=H}J=GtL%I1o+&yFb(AEFB|CBi@=SsY=B4^pP?H|0c`mxxPuY
zcQE!d6hkpI@^;{|=l!+nHMKWfbVKLwVGVI>!X>|hP+AFzM+^>$V7kXHT|fo%$SJ*5
z9TE?Io)yyZ$Gu;q)+mQ##ZcpMHqELhE;MG3RN;Wp6z9h%_+Gvb9+2l*4^NjS=&dMb
z_*af?`$5vgs<q;z?l34E8@RryIQiYLP@Lk3P^9?oTJaZ-zhQ84wkv~E2!-QN1adF>
zO58#081VB0n1Bv_(~w1Lw}3?{_`C7=Olj3T3gj%Xq~q|r*suu=wtXdSTg`Si75F~H
zk5CLn&|UH3i6j~k6*la)adl#S9TQdz5#lz&t3Yhv4}{)#k--Jjg-GaO3&@&aanNx0
zyGLp?*9I2KpTzT-1oNCSbYxr-b5%`g^4i*3X`a_^NO!K4r7JYb4!gChI&k#=A}I;~
z4#o6P^+gA^>X4WI`#fG-?EiVeMxH?ArScK&B~jJn{k^zwFU&+tJpFp*Pl6_7hM+As
z--1*Nc>(f-NxRMxKx#LqZ!f%KumEd1%Pu5(E6VNoKHFn(2!vS2h`V)S8`qDtLkGk+
znXHk^n)Da0KF0ez*mA+nhM$-7LYn$bq41*LLuEaP52LJTX18f9Y!W}>5nd#h3s|38
z>&a}|c!UioW@{VF@eFA_pe=92#1Ap{8Hyox_wZhaczt$6?>~O{%Gdt2&`=+UjVW8x
zl;~>8bT#E|csh}-7U@(WTkRytw3PFl`n@E1I8XC>#^rkYE-j*m)I~E*;i!HR9aYHi
zn(kz`nyhoA@;phdU7J!3P3ecG2u9v%2!)6VPjLG6{}pwjz35i79krq`@^@NJ@^H{X
yileJxw^jwIZ{DQS{*;m%7hXK@%ZXn*(Lv@Cp6LBrwI5TEG6X`zdPs5h#D4*3J~w0l

literal 0
HcmV?d00001

diff --git a/PathPlanning/__pycache__/rrtutils.cpython-310.pyc b/PathPlanning/__pycache__/rrtutils.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..cad3ffc20cc2f62aa65c8ffda6fe4b0e25e5b2f7
GIT binary patch
literal 2469
zcmaJ?OK%)S5bo}I?CfUkI0-~CPYa2Nfg}qY$R$FNKtj2!f-OK=3AKjljy=xo%%-Oo
z;>}tDIamIJ%`Jb3Lti=N%#90t)w7%RBci3Qe$8~%S5;qiy|&h4X!m~jQ+*mS_7^Qq
zw*ZU#=nV<Q6ffDBk2t5fD8(qi7?j~S8bzFa!&IoE=S)RMViaQ)s}5EjtU5~Uurzsz
zTUg3nR8&eo9b(qq*1659-BRatcDe~H?xQ!iAT%3s#YRGLC7v_r4OEB``c#ZDQXQ3G
zj8zwUdM+H+O6PRLveBH-e1+b;2VvPUpYszwXHqBz8CbR-VvH2qVK4YFjh(0_F3OC_
z4jiAjc#;`iSr-)5BNyg%V^iV6MwgFVoY%Ec%?oDMarHmL!FP=|&0w761EU+AnS4*?
zFljJ8Xvc4-nKHGl&F!6HJS{U@)Yaf&X7?VJSydI)?!Xv3wME(7nH;!8%AzW)lrdRP
zqR(UA=d(4!d1rZ=bDS(2%?Y#zOi{J~xzCUI4l@WfgY7}7E0bXUf}1`DZ+t5*W9d3l
z;tMG+VevQJ)#J1NLZd&p;Nde0p^nY@CcN1y<X%@Qye`L!^Xe{1xvOnq^;k-Bqj>{j
zwy{w7>gWQ>5TALJ8<;45>~TW5IH9By!`9}Ho*mxWdUlwmfr}s2W}I1Xjv;X7DhaRC
z3y&U7H+o{Eyn@4qkZr6S-kuE84#lR*##&0(lX6_EX-V_GluxEv*`63O%v>XJox}|i
zD{Ze-8bU&`X((O^OL!6`K|k>OIkwKU0IU1xo}1_Sp~IFTM;zQ}IRZ9%j&xL4^}v-x
zT~d9FU9|=`t-EO38vQ?{`2f1+L>IP8biwRcEC>`R+7F31@VBAVOCuMkV(hvVGE|mD
zV_kv}O40!asi%hAx=04|F6O5^LjLzoBX>2Fp|Q_*<lnlt^7ScCF(R+{WVne5ar}fG
z!nipH%fWXUC>Sk6a6OuH75ol5q~Yw^1JJ9;V5Z)(d+20ks4|-wx)g<~H!<}@8(Zim
zjs2!e9=2Ntt_!5PYQW*q8h(=csa-CCbhUKR11fw#?Ew?8^Qhm)-aF`R3Iv4PGkt-3
z>B0-lqqOn3zrci;*H$-9b8#F%h6o$qkN)7>>_pIhh&{#cqDmliM3rE6B~xmPBGzo#
z+I!(81rxQ<P|b94Hks&3nJvF4DpWb^19^tpNu__0bjvw#F2MC4;@7r>i5vVXpLN@S
z;v}coEgRt>I^^ih=McXwP<a%BP|QK_!(bkK&3;S{QMvbndAP~GXPiA*R}@z<OSiS1
znyT4)e7iyw){Q-d4sqTse%6)W8m3{3s<Zg8tqz1bjoOM85+uhTJK)QVEL3CO#PNBM
z-N$lq#V2GtQCa+YT_k*+hyTtlw{Ty5b0&}kn7R)nJ$r2+SCAD4WMPB3IKo4?$pD`o
zt$>Ow!gq5C_cT}^$un3Dabv+!589BW9<U3Esy^6oh<_Bw4s!>>i9($N>wo+VXFj6W
zMW2JBYc0fu4HwEaC6ot9?-&7Gy$|H=*Vf`7E6cm2hS(UdlBO(&Y0qrZZ*P%!n*>!`
z*KaHy)qJe&UabtV%6v>>K*GbNq&fv>RfUOu6xfC?qAD!n9#4`aPU3!S=rm165fWeP
zwh%<sbUZmQgvVXM!!jvrTNXdz<))dTwDu+3|NA<XF1gpf9AA=ie9H8px6jwb)s6oE
DYAE>M

literal 0
HcmV?d00001

diff --git a/PathPlanning/__pycache__/rrtutils.cpython-39.pyc b/PathPlanning/__pycache__/rrtutils.cpython-39.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..026d994581bad4686489c5deab9790634f48133d
GIT binary patch
literal 2459
zcmaJ?OK;mo5Z+xrL@Bc4G;JEEFQY(%vQ6Uj(p-WfKnoPP2&s*>fPi4Za@RH;id2@X
zHdf@5bM1eyd&?i{VXr;qFZ9%JmQpOsK`HF)V?@q;GxN=6Yim7*=i#q^tNRgS|IlXn
z39xyD(U4F~@sf@BkaJp#QVavkK^cysVZ_;wOob|X#Z+`6hB0=r>R{KwuA{^*OOrSF
z3QM_*ic0Ah1FX6`I=5N1U+SE6%a6e35k_+dLbD-PY$z00;uVA4K!uoLPsNxc)lmuN
zSao5i=fXj)bWUGbJhUb>-(xhNLRfanXZ(!Mm=uaZ29_O$m?On@*=s&XV<)Pyi!x)f
zBge-s9%n{Z)&)hi=fb>hY${yX=yK1+d0iXTyk=${pZ<5S{d1#Dvpvf4ZKE5VnfyTJ
zaA|vV)XqOmGG%I8o4dQkXi{djsH^QKnLT(?W>r;G``gCYi7m?J-uTERQWjNVrHsjY
z5{&nG%%^LF^WH+6bJENoS`%mwn6hjE@{phKU1pGK2H%5HS0=&wH8*`s-uYHs#nyGC
z#6P6Gf=%Ot$*29fMX$IN;R^;~j?MTcqS+ec-c~BSEys&hb62F?*S4^FBqfD$VRB<`
z@~!d`&Jh3hI5)6R{L~|bd2vQ%CkCz8U%ouPv-R>gO#>J2)n=4g?~oyQ<~<T*bqPPh
zZuHnlc@5_aZx$hMHG{N6*{QOTmeTd49Mx)4(z-9@^GQ~=8bgkm>m)uP@ga$GYkox`
zH-v<;(@<Ixo<vEI1pYL~!G$Nl?h%IP<|;pQ*fQjVgBvYJz(&uJj_Rr&xRR(Vs*ky=
z))1t17wuT1|A#c6!`6)G!uE+SSe=SFfdWP6ArS}BHdK0P<N{TUT(?4n%CcyzOOQZG
zHozeD!cb7>#b7?hdMO@06cn->7(NsX)gGhfr|aI!cb7a>A4SEN!fh;w<7ey`&doSj
z4!+Ak!D$(S>(Pv>;7`yY4X4*1gI+}jH}#f1z#uO}mD$YDN71NyV^fc{v4w8Z*dMy&
zNqcnUx<IO{1{@x);U%eG*u@q|&$ljmOpOnyJz&c5Dr#y?q-=a+4ib+WOrPUkgzysc
zsAxRyZ?PcewcU-=TAT)uA=1VVqrdnLI}>ys;!N=e=n_aB5ha*j%aq!pj5S-f_EC64
z$wV(SpI~-zHXiFrna}*DsL<uCPviw^Czbv~(pOf%xd5O40<X3$O!WCpKJB&%g(j!m
zEgr%{WXLg^`w%TAk3kTI8OVJc%z_`-(~V>F?ZaReZn9q(XU}gb%Bq;AJK9c6)oeYx
zTcHQ*#x7w)miLR_b>)u+Y1m@w%>USS2Ev?1ZNCZ$kmJuB&}Bvzsxj|Fy$Z4WOwK>?
zHF@rPwYb3(KE2ukeXGBaLV`-er;skZRk(@+uCT#OoZt%FWPncBRY2_(iMzdkx(w9k
z@B&Ul(Vz3v12$jL0TrSPiKITw2!_872!XjvI#H+<p#H^gpnD4u<XmeZ&V9Jlrm2)X
zK#B?hJii$z((mlSK~|Oz$PDq2otH~l4$_{vNv~~^xJ80mhU+&L*J(b|_MldV_+(NN
z_eglS<n|Jr^X~f*6x5#^Izso21@9$EOg}@qG#N%Hc&*z85LJ`W_{b0%cMaFcxU6ki
k{DvEwR))&iw{HLLy9~PIVS8VEN1^eJu8&gbi}j8F0C~mrkN^Mx

literal 0
HcmV?d00001

diff --git a/PathPlanning/__pycache__/sampleutils.cpython-310.pyc b/PathPlanning/__pycache__/sampleutils.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..4e95318931cf2937863102f478c0036b5ca4d4a0
GIT binary patch
literal 4896
zcmdT|OLN=E5yk-E)3n63RhH~cb|;DRpktAKB~DyU%4_+tTjkYyB{{BQwu%K~KoT+t
za0j3)smPPP$35i_SV#Y%9Ojyn_m--ha!J0PAwh}O9&!v+)0`IuGu>Z*-J|5}*{X%-
z?(@Hk;yKIu2Q`jA4jT7R(j5q4wJpJf9kI5JUngP%r|mLJxT5gJY8OOZlttwYYZt{w
zqAJdyR(foSnmGH$5@%oA?J`<RVi~Pvv?{{hwi@RS|5fFD+Zzm{fRC~;O3{24huuUD
zOm`Uf_^UAMhcVw=7vUg?(=ds>i1!bMflP<}Kn7P$BZ#7Km?oiUVv#RmS|bo<ZOPH%
zTU$F<uJZd|>|Ehl!h1o+r5B52z~e*&ycg~TG4CZ_bd{%>Co_&g?vJDl;*1Z%I2?=y
z+)vW1iuJuw)=%Uee!uTYFYEJ-K4wb~Bf-E6qdPqG1~1H*Z-(ymXrSpQgX(+e)7oq>
zRg6NMrRP(7_PD4NP&QD~ClHyn%ifaC?IU|KZ#n39RxI?fF?!#+V|K(=tck5#g=q`3
z-WJF9*y-6v3>tS2KOGtUMh84Kiud@92OPZ_e-Ng=WccQLEo{^pC1uA$RWzznrRm$s
z9)`++Hk6Zwu_|~{dIt?xF{zwkKU8dAv4eM(tYe9Pf8BbP!dR_==eJ~#2A=f$9Un?>
z4Gzrj*CS8JBunJ_Hq;z>8I0U|3dKH+yf_93T4t{rBhBG~S~7+ORx;Z35<Zg>nwRVq
zwxs_gAZ@)T3`-I|9cK2Bj!P(7$KRkb*YOzI6wa#EwU3;MojJSi*d~mN;`h$PAx(Zv
zI)*h~+VV2Sm?({{@9eI9(b~4qE{W22kn_Gq*^8l0K!cN(375xpe-hT(40NjdUKHt9
zwt_A&9{a%^-Q@gXw;TE)Ksj55d}J^1p}o{#Hc6xau(BZKT?u?={eZ{o##!`?c>Tp}
zdCvP;HoVhn0hq%$7|EnLdWBbIlq5~xYjwjYNL$0vizxK9S<=>C5=1Wp*}QS>`t9aW
zbjcsc<A$SbwnxrmILkUbtI3n@6lmn!=7A$)Fn8zFtnk$+JF&!~vIHL{VVtF>6N1)U
zek_e*ZFot>e*YglI}Ka8-hP;>^2;C?;`8ZdqaexIWD!CYW7x)vl>Ov0<x(Ahb}&sj
zIE`W>d6A|Pq>cE9URs_bag&U;icSs1wdFc1vo!!x{uF&ap>X!lH>s@w$Wa+vyP(&L
z*2Kx!7%+QnPvFpFcLgJ>09M03{6VX4Q7JqGBZ9t8!sr+~o@N{$=m{iDuwg1>B2A^a
zg`_9KQHmK`rh*yUCujVqffvlUxZ&K2(~T227q_de2Wu75z6Y8o^)(8rXtqsUM4OWz
zqg!5rP=$Rm{Gw&fnjIVa$R;MHWaetM%4$sh9919X#6vc@hm!sVBCy&FVGl0ec6wG1
zlpt*3Ah5Xzbsz-P3ZewtR8;-pEci^ncKFZJtu>8h-h#dI*Y#T;xcn@d=@52@e>KcP
z;53hr@a8lSocNN@2$Mfb{1Nr;g&ufaf*<nvQ!B%!Ud%regD_3Q7l^?5F0l6Jpn@>X
zLSOSk3WD(Dez@06WUmFEZ(YB2?N;-~&6~G01CU$$<Y=>p>N&vMXpETw_}J<&;@A$D
z6w=AuvGbY<1~$Ez6%eZs?T%=iM(pXxFNNJh->!~tm{l4hnqf{^I26;o$1{GVeJl`>
z-r5ZBoFF6v;v~Y|NvFvZ2>8^8!?|nFg5Ey3b$$991#u6W_k5Y8DGxlqKc_ZYYQAn)
zdV}D^LI`?T2U2q@A9$Gz_YG?=yhb_8t3*ZuqsLcSa5viB2Vo}nxhec|pUc0IZ)#4Q
zn~jS61eVh-CO<_@IoPOja9&NC$_Wkv<%-bjDfR?h>83%{H8a7pK`JlP>kE8Y?sSk#
zWu49g45l@RTBXQ}>=IjJb#{31;esbMXY)@F<2lH#V5N+B6z~FjA`RN5;?xn8cip;$
zKyK|8R>81i0u9@26g9D!<LRA}(<`>4Ptb5>15HCRKSQlu+}GM#JWnyzSrp~SnqF$`
zlI^m?)ep{L=54-X`XG&>f+k{EveQx3PG^vaQAG7xr}OQ|i>4!`PDdnurz3w$n^`9@
z*Qc&g5R$)u_!CN6hA3MuTPiNqb+3<(6Avlu9!g3IJ5F($+$@k=bxv-gjNHB`Dp1%N
zwLGJtDaU!YXKuI1@v{iMNOMnRg4`}x|2$9Lk*@39Je=n1n&C7U3933B*BQ$+BS%7$
z_+TfK2<UnLr({WHm_3vY5Mgehq>mtulVxVVbwHT*nx!d^vI2Y65;oFM5a?SZ`XEDO
z3t16_>I&z3puSTQlry-S#+zyF2GicyC?E6bHT3J8M!7yNIBI2U%0oNJb7GoDL^K*j
zs%Dr%LtP?iBHvIh-Ifrf>9%Cto^2e1r$#2>mN2MOq8dEB#%lIC`*3XmObc#)Ts;)D
z^BWQQ8M1Fi$H(0U&Mw$_a(jg7^A;&dk0GicVz@eLI0DLBK^iqF=zCIu6uDdIP^_88
z5-7_#e3z5pX(43b?SrXj_rVuInwiU>Cfup`(VU3J{|BO;`A_Z@GU(w(l<!aHw1!5?
zhKk+vTiD<hlw<hp*HrsG2_iL39N;@+H6KUOyryh<y=}`-5)~{Oj<_a5(S|Eme!?!`
zT2-?TFDx3)@Y;#=_6*+30lkNY0WkD!k=+FA;r?VIx?oe=xEknyFu+{MTmTz(`o@_Q
zC#6XlW7EuFQo&ehTs&g>w@i25aYZ<Ht#NT&0oj*#%XMo~9aqT#4*!<x?6JPzXRneI
z6tc)L`u_|MX8&iH-&nSSY$W6K^hizN)cA*{Gx8dW*3^Ak#5UIwa3`{Qxq=@p5N+q?
zBe<(VGD4Ov$xCs|-V^dyx+a25UPY~O#zd&wsLMY<wC#s5=kx1|eW}<p#kLjuOS?1(
zg@}SL<sWI<Ja%a3{2ltnC@J+X6))H|yN<HPmMDZ>UbOUS^suQD>qbeoX#Hy>u9LVy
zVji^UOUJ%Gk6bq~@K+QAGeoX(samPkCG|r+=jlBn1M``xURM?1Xu|_@bEA8`){(yO
pn~+Yqz2+^w%xT|M<+~>0{+aBc&+l60N{yABYNfimw0!Q9{{njB-JSpd

literal 0
HcmV?d00001

diff --git a/Quadrotor/__pycache__/__init__.cpython-310.pyc b/Quadrotor/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..00c03ff7755ca92f1d2c4e785f220799e8a4279c
GIT binary patch
literal 250
zcmd1j<>g`k0-Nw_DRw~mF^Gc<n1CDyATE{x5-AKRj5!Rsj8Tk?AU0DDQ!aB9Gmy<3
z%%I8gk`bsxlkpa3U}<7XQGQ8&5s<?U;sj^r`f0L6aTdZPia^Fjv4i-<nYl&GK;BA*
zA{HP8CVqMAhZd(673=3FChHfa7N;f_C1=DZ=clCV=T<^!xE;Dc!%K4#OEUBG^x=Z~
j@$s2?nI-Y@dIgoYIBatBQ%ZAE?Lh7;23f+xAixL!U9LaU

literal 0
HcmV?d00001

diff --git a/Quadrotor/__pycache__/params.cpython-310.pyc b/Quadrotor/__pycache__/params.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..645e7b1677b8198d7009a5705be0323f46ab355f
GIT binary patch
literal 794
zcmZWmJ#5oJ6uxJloz!vuN{Xl$7??cJFd_)~VW@}%l}dz=jv;b;mrF`)hjS7U7AAHO
z6AUPnDGMWFKnzHQ9VC!WNDK_9Vq)b-a2J<S!8^Ur-+TAo_w45=n{|+~%9VGNnh1T;
z!9mNYpsqZ|fDlC#`iKBEh(S$aQcN(lh{a%Pb42ZJcmyQHU@JXcU@65Nnj#sRW=07y
zvxJCC7f6;mB*(HWcNCF4!=%70QruI>Yy(BCpgn^))|kYK=UG0366Ba;rM|c~X;JY*
zT0wWJ<92Y;rCFl!H>(xr6<J-&ikfp<DU`HA()a&#Zm5bS3N7U2cJ+SNxbyz;;H>ky
z#*f|pvi|Jh@jBmnKD>7XKdtcx_qEMxp6QwV$L#ByiyJ3vLM;$|esXoKwM2KSo;i>C
zofplw)9y-gtCEH)N^Zv2$FDBkI#uI(oVGut&ObIze<<!OSNZ7XnY%ALgQ|yjwYpG$
z@L%xoT<!qqBJZ%Y!cj1oNFy9b9Qbi8ffL6ULA%dFKI}@n%X+*!l$I}qKatRscE1<;
zeJ;&jIF?vJQ+4*jRo#DMRT|d<<sAt^!UYNEB{b$$4e4w|bkgn!KVTBh&5&5u@2TNi
zvwS&bA}$C1W?8V9`C_x%-i#<K2b1KvKJuxEhLJeY>II{|KkP+ec|Tbm_)0cjQD2g-
z1Q8weSyitht|&+&Y+%qQXSf&}F6cNtN5^u(g&f#FT`Oleio0Nd1NQeq$_3}wZ;DXI
A6aWAK

literal 0
HcmV?d00001

diff --git a/Quadrotor/__pycache__/quadrotor.cpython-310.pyc b/Quadrotor/__pycache__/quadrotor.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e838623b371becd49e3880c5101cfe1395ce6ad1
GIT binary patch
literal 4085
zcmaJ^&5ztj6?fTgx7+<OpGhW@*`*eO*oMx`hO9Oa0%QXUi6Enp1R<OR%hToQ=~=to
zQ+8*jTlQ&|6XJ%%wVTtzKf#{?@(GF4+&FMTis1Lk-IL4;;Z{ArSM^*~@73?UR~<K-
zj)vzCAN^Hqu4vjnNO<=#AlySsLJ*<3nqWc?nX9X}5t**ZsLu#1k>y(8O;HJ}k?q=w
zx58TFxK31e>rumPsJ?30j9PAsX@MndQ9IS$rJrcR5%teCQHQiF^k-VTG5M|IkJ5u!
z-r-MzG?4t^$RG5aAoBa+9e%JUZr}RoUXP^hZX7vbzZ(pa;10k4?8nZNFz}Or?*~E1
z_hk_9H0C38_yfVCSoHVDJU!?q9lke8`DZ~egox3-_;tYhgZ)@We%g-*+x+=~Klm*9
zz&V5Y+HRW_&cKfXk&cF8aK_L_T|XcB*xvvjw2i`k`s7#7<EL?2)IgrcG>A@OF#>gK
z=u1CJipsMTpG{VVal&6?pE6E)90fhUOKZLR=n(FqB}4+vWjF{NfNKa{7@updNlwSR
zA}mqGTe%hQYUxzh77fvaToWxgcB!agPXf!u(*Hed;O8RmhJKPb4#$(w?l`}}Z*tL(
z;98jHhuCQs-zmSrp@RDfzj=$F@SEHG#Yu;cJABgNCjuPW1nuY;4I@VwI>OixuMU^5
z)FQNydX5hQ8Sobum0$6G!jDFC->SoVvG|<(&<7qyp)bjOakrn2`3o6`p(o-L>-5v^
zLCKE&V`9oBACwD{UD$O82QNnnJU>c@z{0+aBi@Y%DNPhewaLeeFO;1f00{0#lmGWg
zdBkvDGEUMU;z}kEC-AErb+JERMqU^Udg(!j_b@3O-{C>p-BxRMDexAL4W@$jBedkt
zAgMNGg3VZ_d3vV5G;$+1^Ga%DMrLM}QzN%hGqX-vUd<4$SG7z#)P;UmgJjGlLrLab
zG9gvZH7iP5Xj)2IXjYYE&%dkP)zZq;!u-`4JG2RJgtgxp*qK$LZE~9v!oN@O7<VSG
zxSyu|bR+`ml5re%)Jb)4NEb12)`X>!VoZ`V&L|4bEFx7R<dictPy^?huVWGP{{?l|
z*c1D0y|4yw{V*x4C_okwyX{(`4~9i$637_Qr|>Pwt&%1!YW+bP^dvxE7?`T4DFwof
z+e(7eTMWmtZ5L(|g!_d7!(4k|Phk>f+=?=%Fknw%Ajk^y2pQl{n&epJ-#^>=MFJG;
zM1FTi21(#cfV7MCcA|0lJ`ctl&-xM2O?hbNymJSWm7%*m92d5SZS_;nyN0=w4UmT6
zum-bP>%#ASHd#HNVtesXmwdZ;$fx87g|T0Q!I`RL{~O>Z2hyf&1{CFbY8;wql}yjr
zKE_gGYV|Z>{E_v5Nb1Rrw0dYm*UXG*Ei-2<uVfV}7-}vnvyg?%38ErS`eYuLK{}Fy
zg!}x~$tMVfRQ69U<cBgn$oQBt!XnSSLZF7DXh40@O&)!lN4I$NF^^@x*B|hM3p12J
zT83e5v*Z=B&f(Q|<Yf%1Os#xgu;+4}dUWX(de<u|zLfr067F1QFBW5OA9);T0YutW
zxk<WKxwd?tl#Jttdv22s>^`&~kh0q%@t3g-g_4W<G4bV03Lz!=0N=}Rf_w`tX@Rs%
zhh4|P((#&Xa_Q}HzB9vx6Qy`D(bS33?ov8a<c;ziPT5qS(aAELhOsLt+Ro@Q&^9D}
zR=Q)=PdtEFdBiIZrmZVmm5pEC^4IX;ZM1}DxJ(;;2X{~B^IX^^9ROiZ?eb$V7k8<4
zj4DUSNp+ymm}#d#BkVxUqux^!`?q%Ap0Bb*lH+6@1jkJYLYX6JB}toXv(gAo9g2|4
zi&_tnt^)si5EF7o3l3LKe=}Ko>-2NIE?K&G-UCB^RL1-bre(%UgaP2Mzij4KUd`>i
zmOFVpZ{*Fql`rMX`AWV@={##>&8(Fz^(b7{VCotU2*&2?+4`ZGUlJ^>99q}3?9#NV
z(mxV<X7&sUx<h=k=p!vOdtF<MZD{LSepwj#6@gWRxu#`JqF1!6HGeOa?+q>A5S4PK
zY`LywD`=~At!HMqlM$D#fM3a0Az>W+s?xz|&!nACtJ&I&t!vqpYy)&{9joM5m3_<f
zUC)4Zo7i6snw6<Dqf%DSsHcIR%^92N8ya-3p}#3Ca9dd|+fv2L!JEoY9d9aFmsP>q
z61L*2C`nra1quCBthR&${_0$(uJ}@?q4-j#sdy@9TcQR2+FXB0@umK<;!FJ%@LK@f
z>Ta(*^nd<PA#W~e_hbY8bLsDYQ3zD!OH%4NBe0yQnxl#v1-4uwg1fh-FndOrJ)=q5
zEtP-dH)u#jWQVxxL{t(o>CPZOgj}$mYxW1P9~ISoRE#tj6qPQjTvD({@)8u<@0Iz9
zrDa$fxR4a=LBSpu?4)4hf=yiWgle5R1})49@%qs*+6mgY(2phs8y4)SU{ZbsQ=-g1
zJw}duy60B>ZZ`;B>!%*FTk6(`22#e-HIHbvin`Xi*6>I|lvl|Deoj=bqrUrS$wLsE
zz0X!zL$?^$H<|Sx)4a|uBMj+hzOfAbjboT^maEqhn6~<rV;8BZzZ<aU)%-%hR<Rm2
z6|i4`$qd0z!ORpHW;(BRQ-p6%RkzHb`UX(qt~IsL0Ags`LcgZXba1tj!~IJ;bc7)^
z%1~21BOn`8IWa$~D)l0G=hYI~tUhgkA}nY6w1vuQ^z<2Hl&|a4C8CGR8M}g&aj$9$
z4FIg{Dx}G6Y`O#(WzzPFf<{4L_*6lo#L(ACi-1&ABmV^ViU<s@4q#ls=;Q%te*ipu
zD(M#Cu9P<(RA)c*Ui;xFP`Gg$)W7eQ*IRjoX8i<YQT}w1%I{F;cR>pMaq2dPG9G&6
z9XoN?-<3T4*@%XL_!1NfYcv$V&mpEzmCt4zzjb|sZ2}FG<+n+=RNQSlbP=NeWu8|!
zo`*kpqmcN9M}Q3HBUKNVxvuBQuhI^wxT;DZZxE+2`G`1vktlRh9iRwJD2Q#%WOmJ7
zwi~u-R~sKn8inH@<5g4!qi8rTFAhsdw?7<jFD{OO@>fyEr6t6lDBODzNj2Ja%6Osw
yk1EcK$|&uJ$=kcBDqBSv^<^=mCI8n>3;S+q{SOEY$tsAcVUsF!)<GS8RsS!mP}ZUV

literal 0
HcmV?d00001

diff --git a/Quadrotor/__pycache__/quadsim.cpython-310.pyc b/Quadrotor/__pycache__/quadsim.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..ad423d47f242943bfc23438e9c84b3f7b2354815
GIT binary patch
literal 2833
zcmZuz-H+Qu5cjS><K(XQrG6t-2wGCnJD?9pP*AH@MFMdGl?o{;vfQjM=Q}&jt(|g-
zoTn>~_dg`ORpJlfl@}iN6$vT-LPb@6vp%QS16w;gJ3HQ;o!`u?v!x~9z;oxvKgAyn
z!}yB^rymo8yJ-1$Ai@wVF*;25JT*HOt7LZSaDy4C+wru7Cymta_~1=pCC#+eX))s~
zL)gN3ZV2ayca{WuWVGEmRyDTUOxYqj7)Eo3Ax4ewhoO*Jk;yJ8oPI0}?xN+}Adxnv
z!v*UZg8#~ByB$k!!#bvLh4<X(SfU|(^tM<NEwS{RbsVuSmc==6u2>PPnDewb8_K1P
zAI0f*+mvL>8#4a{!};$ZT@R#wh$kQ2N6U9X1S^feC{0i%c<Ej-z;SSn=FGw>UE+^O
z&n`KrQxfl%mN15fyk6osM{LiVSVg0>O8ba(8ih}XJ0{JhFyK3TyE2Y;p9>K9%}K+<
zB<i<YiWSQG0VBmlp_apboQ6f5^#d6ekuu|cq1L)tzmQoH#6<)tCUeygQ6A(4>G&tp
zNhFmy$Z|=JmmEYjF${L&9Po_Vp0e^N*^#R>y+Wi(WQ`QPuqfhUD5B2#8w++MA>Hqe
zI;*t>3(*Thp2vqTzutPBM>5|^!|s-h@+g$u-JqL^Xe%96{USha0j*)8?cZ7mw+;Yf
z9;cgwk@A8d?#D$C5LEdFh+{j<$GglN?yxo1Vk>;Sa)SJ3t==X4>&FH|O~Wj~l*SZo
zkImQwknHiPIWr~zYL7<NzAfKDZ%-WnOcHlyKH(E{V)YwZ?vZ@PD;XqwN^%!rW6ey4
zm91(e_fvyYAOY|fY`g7O9;TGlc&J7%Dk{J#w-XATh8*^T-RxN;Wea0vVyv9uKmgaa
zEy)FX9c5Qgv&V{ks~umF3-poosE7ua!R5;!Ex4=2F0wVg#x~fvRXdB6-f|QL$RZ!!
z#h}n`I${rupB!@4l+REp4s0CPoLVy+OkRRo+g7#=`@Kk>!$@8uLJ_B$KW8!#!Hx{m
zsLi$UiUnt_ZPlTaWZB?7e3v^Q%NpkKMs4kBrH5TIvwmo+h1pc<G`>KWu+W$yDK8lN
zl%9sh8VNqNN|Q7S!kIAsk9NL9$mw+D+jH^_@};4}agbz%)IjQdA)8gR!YH6QBwbP{
zdk_wh2}|^)RUMuqsjWPG7Aadh<%}!j71Bk^345QdFt5Vy)wo&v0rAi!W9o-aPNv*P
z%YOqY5pgI*R|{jxD6R`W<t5j#&ZefQ{KBTzg0pLG&*>TT3Qpxp*vAe$$WBR}|CORr
zqe@%CKgI=eO23I3PV&~VOY*5ZGX#A43%>+h0#AE(c^h$|i}m|3{Uy7FfRDtS4%Fs~
zb(J&Dq(B+7o+d+httX?XuUIBY9bWwi9aT=b51oeYkvb#!G0E8`ZUU|0R~mUdj`Y<0
zpVzNnw;S>U{rOJZ8_GyI_YR}{_5<bR$jiedPAhJ-;KsyR8epBfN!*WeW$O+*>D;_Y
zE1FpGEVS1#LAK;nBs~o*)iIx)$9oy+d7h0|YmgUF_7>E*;|)p;4fWq(2x@Yuz-`sz
zJLK3}0szlQaDP{lo{6B)se!L84F^Bs$nlMnl4d<IC-%gdprj#m?9!QW0BBClQm4tm
z4J3>;wHJHYw6jhoXX?@%g3Y7U6YeF{j1o{+R%#3Dk*U9;m1|9Nq3PBu>LBq>ZcD;k
zQoPSKp}e2lL;&V%B&Ru!2l5^S6>un-p;{#{Z;N0cvq2<_ILhTZX*l6v5cP%P{eiMW
zDZ>%MK_mDfraQg{HPA~~#CIA*D*-!HY^2y&xhEItS*&khi7(J{x=BCdS2-16lwjUs
zH`z5dUaxUJRWsUr`(ag08+7gUQ8I%-`9Y9oVwe!$3W9@Sm{c?JB9^X7EtMZ#IV8oH
zq|HwmMBygsR*>W>7fs)zp5bxMIP+h5Yo71hzS~;T8tr9e_lN0VB)@^KTE)KyMSK!J
z#f7Y6->Ld4d9I{`Bq*{h*^i5S6N9X{EjB~SB%$1sF}$q>@^f0U+0Bw9>gvCi>ZhcE
hb5*~L)t?Gk`hQ1I-kr)DUsB1?Mbg9dMt=dI{9m%qotFRr

literal 0
HcmV?d00001

diff --git a/Quadrotor/__pycache__/quaternion.cpython-310.pyc b/Quadrotor/__pycache__/quaternion.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..3c20e7040e017bf477a4ae5e649e4f6564821330
GIT binary patch
literal 3275
zcmai1&2Jk;6yKTMwH?QC^WnDXCrgkJ7R45{T&k9Wf<RPi1Qdi6Raz}O8^?*g>&&j5
z#@bx^aqK_9sX12sKOFXi#HkVo&OHFXH?wOy4j*gH%$s>{-n{P_M@vgp1K*t=ei1(d
z`i&;DpNYw*Xz^uq!Vs)yY_l&7!G-zE5av_9%>{dCG_BXb8(Tj0v)^R@BdiA4D-7DP
zZE6nNR$vQD*w2ioY`Y{%qWsL*E{lp-z*rGgv50X&)Wj0Ts;G-|7#GE|Six8mt6~k~
zl4wAJ^I7$qp_c?Q>_*}E>J*Nhok%)2KX+u5cnOfP&u2s=*h1SxiyP<?V~?dQfs`+J
zYPhB_z@2Y3S!RpmXb?25jE94)?8Tmx-ce@n^dc|G*yAS8tT^cH$TGORGUNh=HxJt1
z$AOI7ea~;pAPzj~@49{@f_DE%kC0SAv`BvNu-hN@l!)!gV*6mCRBLdQRb00lb`#gN
zNnDb>!NzM-Wm^T_C(9Qf2~<K;Itl=Z@sg$dgdKAQF+dCubWVB0z|6$VoXjlDsM#qj
zLwHcwn3qzk!%~yhQu~V0A&#ZgNJ~#CCiwU>XA|a5<j_x?sUFUbjQY+HYB;07hvLr2
z@j@~24<?;n;3T_2;&rqNzus$>C3#7b-kBM9!_4%f7zRI(3FgCow&+Kpj{peKnq{DS
zamGfOt@vaN6Mx$pg|eTq1LYgtQT%5$63HF8infB4xQ?!BR#~0bS%a<f3L9UXG4m|F
z&p=yz1XEL{CYz_m9s=kULjdW(V3H1)mtdh2B(4gm^S~<4me{y4bL52J-@|tqTj3Ul
zGkMDP_z8b#$aO3>O=Z1oQJvrha7Ir{!<96d9YJ+DFI33|sp}F4CDAe)ugplKv1i1g
zs8s5cX%P)<pVwdlC5S%ZDiHs0Rv5VMmEx@G{8@xOV`_?Wbr$&x@d{(VWGBW&owMWG
zRNC9sCRZv`4#{93(O)_%hjg%Z9Ze}*pHD8m@s!FW0(n%4@e=?N0;SS;&V{kVFTntN
zM#DHpF`=Xr3@cn<eVs-#AFX_}6}EH<Dgax~$4ZV}P}pidTg<Vwd|Xo4dOkasQ!UfH
zW1Ub+u57^w6rQSU^4AJ}8w-HRZ*oJ!q?CL|!J1yvNE-6JinO31(tNL`*L*a6&WDzY
zy^22P$!I;NQT+40iaw`N^h8)VaRI^5^WxZb5!))Et^u1_0~v|=&?>tw3ajf9ulNSK
zb-v0=jQ>Tg{-03k&3JVhL#VEb(Wx+`iZm&%PcSJmP_Q*Bq9}?~UQ;Icz8SCFH=Zy-
z#nz%Mr8r306_iwp5L`XBS~Z3B92jBKmH5I`<(T%EDT?wMuHO?XwN`40%DyG<q6Ayl
zjALL^zJr`^E{yMf8ze&+QWbSZH*_&|koVn@*9*eLiFOp~y)cq}uh$(1qB&EdwQtGZ
z%7a@O`{FxXZ+0X_AT!I421j{ihDS=eGE)Ys1e50gWhLBnRH1EEa+<cTGZvjE*NAjg
zQ9+navODjwsuvg4xCOG93fl#<#@5+o^8%}=-}wBDW2W?Hd_&jpCNa4WeUO93iSdXZ
z)78gba&-xsfM7YYC?E@-q8wFy1GY}hb>weqA|ne7vUUc;1hO}^8kErm1y)W`U<Xkk
z6LDQq)}P@&;iAsb6sOP85@^gkf09^9p!kzK^YZIc3n?|lY5rPvB_(!iiqq#nf{xiS
zRV}f{zt6;2@wjsOULw1X|0lkx&h*<&l0kgC-44RmVRyef2t?OwMY7YT89jvZH(l)F
zk>JJyJdnJeTbwt`>8n1nf7A^-jvw_0L;c!w;z1Ayr#R*C@URv7KXzMTuipy0yRA+%
zY7hE;+mlJx?*(!D>j#^+Zr%L&!<!$qXPK+Yl?VMF_jF-Xh02x`qB;kb2&z%{KDs>U
zRix{fr@LL!BXd6BR}sL4O0G8H9gxKix|&^ORd$_kunorl+%PZa;|9BoIV#k(sgYV|
z`D``obmfI!9~Wp=MG1?<ut#vsbr0|m$T6srBJy!7F5*nQrl`91gJc)=^deRy{R5D6
zGs?H9bEtclI;DP_AWF5Fj_!ZS(V1v5y&@`xh1}(MS5;~iGyg5s>WZpqW_CF25011g
aY2yqt#alhrDtEL7pHO{@=~Y-UR{7tBkGfs}

literal 0
HcmV?d00001

diff --git a/Quadrotor/__pycache__/utils.cpython-310.pyc b/Quadrotor/__pycache__/utils.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..fd805da3b6d965dcd0503f985c8bf112989b5914
GIT binary patch
literal 3041
zcmZuzOLH4V5Z>8cNo)C$5R4xr6hkV3h;pP5QbiF=o>VGDQBGnzq~Z#=)|!#Dv36IU
z9a)x2t_qkNMe!HxWBvd){sSitd*u{PoO1|#JzC4L$jbBRcF%P8H$B}GPfivse2b6%
z6hDqx)?Y-t{@EaIpe0Yx32V&~OxR7fW@B`kPUx<=VQwuK=GXFJVXY8)YaX+ND{?<s
zYhxlW3K)yR6Jr?1MNy1noDdUY662(p5+^WD346sVo$UQw^xL`-tE>FJ)Kc-gZ9l3P
zW#|XZtGuxxKKu02jXF`~Y8)1uK~+YHyvlE_d{w;Plzt+4P0A*(DJi**c^eEr5<HAW
zQ0sEt2$BW9(bjxZ$`(lccO%}GJcw$s3Vj{KQJJqc{Ae@zp!fprT1%x|>LfvwI@LHy
zU7w~oU;EMJ)J?XOt}>F!KAPvzZlfg}owoK_A9&4M;}%AHo_dF<x^uwbvAgu#enHD#
z5X#SOMRSEy7|uP+OLl5UE$CIs@0M7~R-Rkx1pe{zZslPjRZ<E4YDLLJ`l{Mkuf{@F
z!tP*v(DsFjb*wI~1Yx^re5?$am9`F=Nx9Wcooy+{v1LMraHihEaLhRuj2xPA?t4r$
zIRn@B*e=uLCfpmhwB2WW>~!YnA+=}k+D675k;%;uS3t(;v%cMTY7U^uFR22wC<;yh
zLzo-_Lzoz(z&CRPlB38(MK&r2UCB+^a_Y1iL7Kx+YajC@Ft`o6lMqV1<yf!A%l98o
zfl6qj33d*@B2)7qs<hMLM{}~nK=3yvbiQ!W-lHCFPg~mQ0}%A!8Om_yGAD=p!5Slb
zBsEJ?SPc*X1g)<-uW<%D8i|3<Lv2QiY{*s(YypM6jEq@{-HlbRNzs~Pc28TzQt}|<
z*PSd<S?^+z;{%)vbaS-7zyT-!lB>Ac#5~Z|24CP^zhmS|u9^TsYLYqvPElZ$95qAp
zS?cDfqcA>p9QPmNIAT2wzB!H!_e1uC<zK0H!8sO@bXXK3<~VF@KZwUcB+h0LIBV^)
zJ`?VytG+<QGLf5wdm=w=?b@Qyw=n`J;hC#BHV**$_GxR^1+Qp$<3nB!ya~gb9P;wf
zZ{UJPkNAg{XPDtupvOfgT3dovf@f@k6m*#X&J8UAJ!*lISx1%(_J?llK-STIL+dhL
zKH36zm|~#$_e?W*Ml&pUqvqjGneT+hs75!lhBN~u^sslR`AVh<9v{<Wd_6=PYevx{
zO+)S3ZshN%hFqpSuh~BG!vXTmzSDR5Za>%0*WCkz`EH?KK-QS@b4Z~k0+;WtuHJj_
z@a8JN9Y@<z>4f_nk!4aKN48hN4l+N=gwK%>QEnpSf3viv;?R_b8e%){1W}!L<VF%`
zxxj_=n+fj(x?v=$RCGEG8F8%^FPYU1ty@<sl}@KqmhEaYz=|ko`Bh0p3#nQ+l1g31
zq138dL2fn%C6%hNGJy@0&&j&g_{gqG5DB?cZs^eH(Eg@OU_F1dzPx@5ckq~7Mx>Ng
z6H`*?I;m6C5m`sW_s?~cQ8Vg~>sVpZtjVdv<a@|9`@D^1is}UJH<dbb$x#Y}dStG8
z$y4uuo!VL{%BZQm)j``q>!S5iwl#X?$8aVCPtRaMas}O-=dqK_W7GJ}FphLQZKxS=
zX-?D(J7?33=4Wh|sk2ykEI&}fAy-U(_y!Y1DV{ES_OG*4v7REuA=Vsvs{G<I<c57m
z<PR9L9^ske;<xAEA!DsrpIFKJ9q{S*7>#(P!cuQ60qQu_WIJX4OSaH+&zCRPP^}A*
z>@<T&rcN!?DJ!BBxp7NI1GEZ>zb*BSPIDcFkb=A`@T5ptZ4{bQMqsIRlM4T8d?#qi
zoA4tmQ>WMjJNrsq1m{?pq6#?ln<{mka-6e|TTAt|hWo=T%`z-zkn7xiTR8CEJlgus
zSIA2T_`MN{3;#nRztr=z@}HLDHk|FKkH{d3mcx$@!a+-@QP%$frhHe}Wda7xSl<HD
zExaZHs@?O>C*1rk88GPSQ@)hCz$qL>XKb-9Be~O3*S@BWsruW`W~|<Ou%p%1t7Ot*
zGp^#kUMmk}aTa;Mg~Jf)0#>Mx(WUOwIEe7nV>>D9rmUCd2YaM;tu?B)c(nZ;(n}8%
z7lAp!&akups?R}|PNaqPbrDzB*VA0o4qM&qp3vntg>X<3j~*=MF43FIygE#+GXZT*
p*X;S48K!0qUN4Jy+P0hW8o`~={dO&{aE5ud=XiO~^UhD?{sZA1e}Di0

literal 0
HcmV?d00001

diff --git a/TrajGen/__pycache__/__init__.cpython-310.pyc b/TrajGen/__pycache__/__init__.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..ce34b40d4f30774fdd536d420a96cb5571ce70ba
GIT binary patch
literal 287
zcmYk1!Ab)$5QdZ8Emm=94?cj`J+yg&B7z6)MJQE-PzYf&U?ka;NfzBl>I?O-S5Llz
zCnr?Uf%#{?VTPFrb$!9eRxd}hqxhNOzZC?x<nfxsFvA`5TymcC!WF)hrLSZ~Ty9xY
zeQ|1Q_*f^@Z}-pv>J+dk#`&Ga)`5E0!#>z9#c}ok&VKwA9-E0g`yIo)R-R|-lXVeG
zVys9G3@16y^1T+%5ikm`TY(TkWBaPwU_kic6gRy#80dt>*81M*WJ4#OPPYlAY-f{F
W%l$A;S3a2D!A*W3=1rIEivIw;UrfLN

literal 0
HcmV?d00001

diff --git a/TrajGen/__pycache__/trajGen.cpython-310.pyc b/TrajGen/__pycache__/trajGen.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..aeb5c41e3c7e5cead3ed1b99fbf09428858fc5ea
GIT binary patch
literal 3822
zcma)9OK%)m6~6b@vmbUmi4)s#7{Y+8=>WC_NFe0NBr{<Wq>P!2JwlWQwQBlS+tu!_
zZr`eQVpA<4If!8~|A2+P%!c2<3bCFAi@O6>Ud|RF!FTTMcDrK`v{mQ6U+4SI!~MCr
zD#LH{=YNs$HOBr<jmtj=jR*MhA45ncd6z{TrRcJrh#XFRPS@>uk%zh~y{^B*9y967
z;3bp6iHHKpw^<`R$9lHa;M(t|S=`-4E$k19w3i+y=Nx0`;P+ZI>+<j7{14EZn6o1x
zuS-X|FL~rhPx>!e<jO#XC_Pz`Rg}K0$vVnF&dGU{p}Zy+P*&s{xXTSaSE#u2ILQ(f
z7kzc^VefMZ(Q+@H?W0b|zmG4EAWB}cCMyLbFC9oBsmCdK$Nf}{+>uv$XnUm#=@-Fi
zSh9|PMsq49JFT8EsEAhAXiPe0q%Ik*<Zhi44vG|UE+86ny7D|e8uZhw$aT<*Uo;Ps
zuJ(50UN6>OU&%yiFW-v?iFRb#(@r<Zw8#e9J5E$T*J5W|(e1T!6hGI&UJ}bR+im#T
z&693h)#$W3i8&H)kXV3t#ndv2e>~avBu`Yn(TiIfD#?>rwf35=zDzcHN49)6jHT*B
zec#<qd&6#Ar2TAT$Le)stl#y)kq(>9G)s$SGr+$2EeN06zp5$cd5Cg>AK#h*Wqq=&
zMe3OR2p>YrV6uh^^a@<gIP@<@4x!TFg*XK!XMB+@G3wi9ulQEO)%6c?5#rl+77y&Z
zuVV}C<ilRWRZCRoJKAsclXg4TZmXXcMjKYF(0I6;6nKALETWYYMdtY;cdy)jGOb03
zPJV<(h>yzr*ATcZOiGI1h!tCRB%}H=8}Wj7M2Ru!egV#OoK^NaR=6E7tmA*qi1@!{
zZElDk%F6GBtbXCj+6o&9!#qLk>QrDT*m0BeR4vi>so@_uXn9xPvXuK5eu|U}D|F)*
zX&(9S|LnbAJbf6sX_g$!yn<dtnB2gZ({oDpIc~ho)G8WAR}HS-MEw<y+?`&UUDn@b
zH|~8V<NJ4LHEk2J<F{v#pSXqf&2GOHck}zG*hLMY-QsAFL{58{q2c$EVy`cCt<}$f
zRcwrfzi7-Eerxgcp?0&r>RG0f+M^KnF(gpmKvNk4_^n|N98iR*qKm1olei1fa4aue
znYP>JQT8$4q6x$ZV`Nmx$$;;G3~PK*+`2-D$)jfIOa@3=H+23JD&$Z=39-x;2}oY@
z6AlWqQ&7Q>98RWiPTkTy<0HQm(%E;^v(g7GoCQ|;3v3jWfrOK)0d|rEe;bq<g=H{l
zPfF8YG5b}{c#V}|$HOk(WPO{jFlt}S=OB__V$P^q_@_Zx9iLlWV7O1Mtd_O5u<neW
zr7nD0BQ!213Z5i+p2nHJW)0*5HFXd3wUcItR>=kz50JK!M%_SQ`Xg_c?Ps~>&oqCi
z`FljqLsUMFoR8ZBh`~Vfj|_4aCvs*MC&KkDd^vT&>4<q=6(pC%GG9k+5q-zEXQ_DE
zW6U5&&WyNkkoz8ZY;fZ+TryT*W}FaerC6jfQ?_A%Bloot*YuKEL+|7YAjg<lB&I!N
z#+ZfpJMDwx06x;8bEdtUtN=Ss_#^hqwGl#*bWb?Kk`D*z<35m*FM(sk4a^M?+WZwZ
zo+FmbjH~kwp3<%ouQWUVly<f&rFR0Zm9D*q^y!o(WCnCAYT474aNd8r0p$)S;{V;j
z?q}ZNOy6;EL@yoh1GdxI%g90|x+$IMdnIgQX6AUlz2n4gNRO~daU!6}Z(2>xteU)Z
zy!wB?^x!Mt;2A3kw~WhG#LB6uXeaJ=)jImlDZGp=N6phDvI<3h*Tib;<UAE;yNM14
z{q7Nxf<`d47R@uw`|A6&;0F+zr>|SZ2D*QUFQ-dTK)wy%wI=TIx^tJ`;|tCc{)DeN
z-{JGlBeIXL&DzInRy<=OWc*|%#zg*%3gu4}?^f8ptL7(la&{E$$i-3LzIVo@18^`8
z#$@`+aZJRx({ZhVpbp|2<01oaCp5q+#C#AHt`m;RwRISPKnre?MhHT*U=F6`n^pif
z8<5FKk7BTKx8bN#gajB&Cn&?Ra^l0i1zQHQ-G9FN>dOy5`{-R0LoJr{=yM7*jfxE?
z){#BmOH^W`3I#@sCZ*ImAQd7-i|UV(JXML@F5)6-RLAK^q28fcp2<nHGeBnNAQ6n*
zxYdH}Bx+yXq!~W;ko_z9${cMklf0SJwx3{TPEqcTSQhi*ZC;1}qrY3ub#c6U72AuI
zGsu$TyFm7jXq3EQrwCSOeAPNY<YI**dEpk`DLEZ+CM1mL1X(hA0u#PG)IvK3BoGHX
z74%d~%9y=Huu_=3nR+`lFy<XQM>^DAWIvi2ud#imwq|FHcc6?%;5Kdn9HqAkr(kXZ
zygQXLFs3+0bc$j6H)KS+zgqmm<G&sk@17HAh(i>l)VE1c9;+Rs<q$}449X7luYF|7
z$Pa-Svhdtu<L2oulJh|dW2P)qJ%Whpt)WuQabh0T2P!$dXdws9k$B-B8={6!IZX3V
z{e;$<h&Byt574)V&t%r~FyLig<#our;w|nYs8#v#&8xs-+pUIBgrXui8&yiwv$&Tu
zo4VR;_WE+zrFy;DJQ&7ZJECsWe3Lj4$8D}eE<}Am;sJ^2{ic^BM@C;x|6+vjGvQq5
zg_ZDT_-6fvvTja$*|0Y_vOW$0l^JbEUfxR4OULO7y<=FU-Q2ACPe<u+(_Z~PrKu(-
M3t1fs3Hf#LZ=EtY*8l(j

literal 0
HcmV?d00001

diff --git a/TrajGen/__pycache__/trajutils.cpython-310.pyc b/TrajGen/__pycache__/trajutils.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..bad974f2b0c3812e03ec6235174dadc3ec5d87e4
GIT binary patch
literal 1738
zcmb_c&2A$_5bo-pKReFPCMG{*B`mF2<O4AhaI*qpBeB{;q$NQ}Myu6mx*aAN&rG_<
zB#y=sl6SQ^z=1c|9Q_tN1&2N%aX@|r1X`-b3CVJ%M_pZA-P2w5)mNP@F4h^g?l-^6
zA05Vir^($WaPm2J(?cPdgoO0~N1pJ3=m}s-NFkjG?>TZsdeVOZJy+h7HCac^lM4!t
z{U+;!Y{*5l1a~Y;av3c(2?wmbI>YnXej98(4F^gV!#q*8v8PO|l{_fILS1a<nQ48m
zl2#Z+tz)H6TBGocy3C3hg%OdrH_bzZI@wH#q67&iT(1}GHN0Z4pwA_j;zdw!=}33N
z_t*rkU@S^;z;KVaWHdv~k<JlZvL>Ez#W8j!y_aH(jh#!tRag3#xU$Klb0yZ;*e#vX
z#k~UWV=s9ZHH<IVw<1iE2+)z6#!(>;*RY#S6i2LN&w??YdI=@oI1xYdQ+~jv3~}-E
zEE!3q+uZV<k1U)_xpgy*77K-@bN+nN`PwLLI)gCkXk}EWqwk(anN*#@Xg)q2hEium
zrawH02g4*R;w<eP>hSnWm39gohee#2oqS~L-6S?elnwITfczt8HyP!i@?xi(WKo!y
zT~u!N%_z8;OX3YSP1Ck#d74`{)H)nlSBL45vhIb_nbAJ3TQ96^ZBI9Fs%s<`Xu&O%
z9vJIInd<jV&&jhhQ-!H1)q0Txoj{SB6%+xq!13yT-G_Bp0SBtp8It;KUeMok%OmXO
z7Ze4eKA}v*YhFSrD9<GO9P{rK$cG8+v1d=lF79w7l*pSaG4^l;s+I28|5yKX9F%^^
zPh9<N8C;Kk8Eh~qO5Q{SCEsA<nxxxY-05vH+=sG>Y{Q#b>29&Iwnl8LZ8PK&RBZA$
z$SdL%p4CTIojyEmovxJ*a?9DTpw`P6%#8Acpo>JMdKGp34v8j-brjagbtc<&3x{)<
zMV1#9QVUNj>@5r|#1@XNK)~mC6T%hV)_<&wiNloOHt(Vc5bFjkVQ(PjP51)%5BzWb
z#&Pxg^cU@k$fcV`XAwgx^uI!dsu3}Tk|Cy5MWTh5h<%eGQi5s}Er{N=#X$?^pjvec
zcI;rKVa4_F6_V~xSS@Yu4jzBZFaGT_eGj!MSSLJ>&D5>1u|X22CJ!TJMU)vUOq^QR
z6k&8S_4IrAgLMuE;dw<(3+EO_7B2n^%F3fyM~QlV7LM{PP78A%e>6n1C0NC;fw<_8
zar%D)Q6gQp@!4tOqw^LJuJkG({5wGChv-3-j|vI>0ZrQ^c1Tn?zD@M<W0l0`w;?^i
zJ@b(Gbx2%)g0?oFQ;(_)bsF>*>2>R-!$Cfp<Gc`MNunZpZHy*5XnK{a0<Cyrc^+2~
iRfpbj=GLR2JILfPQM>f2m?abeYe1djS6^+c`~LvA)N=3u

literal 0
HcmV?d00001

diff --git a/__pycache__/controller.cpython-310.pyc b/__pycache__/controller.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e37e32bbb2912236ac248f4e233688b82d591009
GIT binary patch
literal 2055
zcmY*aOK;mo5Z(t#kuoLIvMkAVVDyPbEfV*EqCgSUMFJGap+V~;NWzQ3ii~VU6piKD
zfs5rS?X~w7J=neF(tCeJZ$0d(Ko5ogLQxd`hLW9xz<xV3yE~eh{bp6PT1|%U#)Ci7
z-y!`)%FCYv<#UYkXF$qQK4me-ET$rtvCN&=;bfOnH}_(%mYr!M_hUZ~;()V^pEk!V
zb${V!JYHI8Uaf@-tx;>Oh33~<JM$*XaRgk6SAd<kgZ0%kIO9n-ZKg}#v3M;F(-!7l
z%I~sa`y3InJ40b*c`&nbRFp$*y~9K$c?qk(`_-d+#crXkJkGMAJg3A};{ssVBH6zH
z7+jKr3mD43fBKQW=R1ndj_2BMJLeMGNB}OrO$g-q7Do9iXUy;ks|1i&QVYW-@&`6?
zD#tje9s6FS{IOVPm5UuNR^0)sJjfno4{}2{jMrt_H;pdqleeJHDqlB^4{K8|0mIW4
z>MW>ww5JM8VBCmRP17_k?b>8|>D68EykwTh8>A4w0q^*YaPb>$cxmA`T3uGPO}qBC
zYyurlmQN#to7iMkL>~2u5zn~p5T<f0&v>;0`>Kh|%9*G-rn5NHsn4v|Ygolv7b{)U
zoj7I{BbqpmCF-!2I(p4Gu&hPQtYLN{Hek<?S=F0IWR^|Th0P$Zs!vgv{t`2**xxr@
zpfpj+*IAd<@eIrWwd^6!^*Z9d=^@vFS*Kjbs2Of!1C`o%EWjoA&N*G@P%7LVtKs}o
z8pDL|&z<4Al_$wl>jBb2E24{)3Jl}-tUN4AV%pS-Cx;lvRwkp7<+<f!D`tn*O_WMb
ztTV~f6YEV6X32DHomruBMSM~efOSWzC`&6wd&;ZzBG1MN-0fmK7;aj()QQe)kY?rH
zTx=kkJ*cstE+i|z*?k&|_@)&ZLdlM8Fe>sq-9tdJ`vq;b{C@0@j+ClxB1_`td<#Vv
zduhEX7U})ii*MH6#Y3EhjLCa6H+c`|v%6cn6d8ite`<p|=7s5j<zGH$>U|{h&#mqI
zWv0sQJQ;1PtjrQM+TR-$X||o8%;&F<k`(nU)U~?@`O!4d2gPiARLrz0rqfJaKRiLb
zk7iwXD?bHz+~c8)L?}bvlUu?UTe2y7(uWkufe1O)uyRFqWhgw}<lcWxcLVF{LmVGb
z;V-^61iI0>hyR6uK4FyvaxjU5Ne*P{WdYgM(l|zR*(WTK8hi!2T<W_%(~g#lj`Gk^
z9`*8^nYy9Tzpyn@q5Vw;JJ_!SBZ=wI@FsxmwKf2gEB}>!i<QN`#i>5+3-r=yXnec?
zbvGi$7GlIZaT?-{aOjPwJD*M3u!m&V%chB#qj~q&9kEiiOvAJm9ic+hK(z>{6<+R*
zxuK(|WU5233INk?E4muBM$jYZ6ATE5-gBa_le9q)5xhq5I>8$RD}Z5Jy-D(004q|h
z-X`rGf~y2Wf_DLN<8c3=re~n{Go6sA-lNrP1lI|+35b7l_yCd(K%%|<;y6=xp_UX3
zh~$Jk<XfQ1Re6>7cpFrzsq!(FhkfhLj`G73Md4c~PxStaoNRE(Tt)w&@$#j9{YW27
z%lfUTTY8>(U7@;Gmzk5#vV2i7%Wqrv+YIl4`Uoy-!YT^orDW=~>Wuswd66DXv(M<R
Y$}0e$MMx*&ZPBiWC#hSS!k6LB|GY-T^Z)<=

literal 0
HcmV?d00001

diff --git a/runsim.py b/runsim.py
index a22fba9..4f163cd 100644
--- a/runsim.py
+++ b/runsim.py
@@ -1,4 +1,6 @@
 import numpy as np
+import matplotlib
+import platform
 import matplotlib.pyplot as plt
 import mpl_toolkits.mplot3d.axes3d as Axes3D
 
@@ -6,6 +8,7 @@
 from TrajGen import trajGenerator, Helix_waypoints, Circle_waypoints
 from Quadrotor import QuadSim
 import controller
+
 np.random.seed(8)
 
 # 3D boxes   lx, ly, lz, hx, hy, hz
@@ -17,42 +20,50 @@
              [70, 50, 0, 80, 80, 100]]
 
 # limits on map dimensions
-bounds = np.array([0,100])
+bounds = np.array([0, 100])
 # create map with obstacles
-mapobs = Map(obstacles, bounds, dim = 3)
+mapobs = Map(obstacles, bounds, dim=3)
 
-#plan a path from start to goal
-start = np.array([80,20,10])
-goal = np.array([30,80,80])
+# plan a path from start to goal
+start = np.array([80, 20, 10])
+goal = np.array([30, 80, 80])
 
-rrt = RRTStar(start = start, goal = goal,
-              Map = mapobs, max_iter = 500,
-              goal_sample_rate = 0.1)
+rrt = RRTStar(start=start, goal=goal,
+              Map=mapobs, max_iter=500,
+              goal_sample_rate=0.1)
 
 waypoints, min_cost = rrt.plan()
 
+# scale the waypoints to real dimensions
+#waypoints = 0.02 * waypoints
+
+# Generate trajectory through waypoints
+#traj = trajGenerator(waypoints, max_vel=10, gamma=1e6)
 
-#scale the waypoints to real dimensions
-waypoints = 0.02*waypoints
+# initialise simulation with given controller and trajectory
+#Tmax = traj.TS[-1]
+#des_state = traj.get_des_state
+#sim = QuadSim(controller, des_state, Tmax)
+waypoints = Helix_waypoints(5)
 
 #Generate trajectory through waypoints
-traj = trajGenerator(waypoints, max_vel = 10, gamma = 1e6)
+traj = trajGenerator(waypoints,max_vel = 10,gamma = 1e6)
 
 #initialise simulation with given controller and trajectory
 Tmax = traj.TS[-1]
 des_state = traj.get_des_state
 sim = QuadSim(controller,des_state,Tmax)
-
-#create a figure
+# create a figure
 fig = plt.figure()
-ax = Axes3D.Axes3D(fig)
-ax.set_xlim((0,2))
-ax.set_ylim((0,2))
-ax.set_zlim((0,2))
+ax=fig.add_subplot(111,projection='3d')
+#ax = Axes3D.Axes3D(fig)
+ax.set_xlim((0, 2))
+ax.set_ylim((0, 2))
+ax.set_zlim((0, 2))
 
-#plot the waypoints and obstacles
+# plot the waypoints and obstacles
 rrt.draw_path(ax, waypoints)
-mapobs.plotobs(ax, scale = 0.02)
+mapobs.plotobs(ax, scale=0.02)
 
-#run simulation
+# run simulation
 sim.run(ax)
diff --git a/test.py b/test.py
new file mode 100644
index 0000000..0813905
--- /dev/null
+++ b/test.py
@@ -0,0 +1,13 @@
+from mpl_toolkits.mplot3d import Axes3D
+import matplotlib.pyplot as plt
+import numpy as np
+import pandas as pd
+
+df_test=pd.DataFrame(data=np.random.normal(0,1,(20,3)),columns=['a','b','c'])
+fig=plt.figure()
+ax=fig.add_subplot(111,projection='3d')
+ax.scatter=(df_test['a'],df_test['b'],df_test['c'])
+ax.set_xlabel('a')
+ax.set_ylabel('b')
+ax.set_zlabel('c')
+plt.show()
\ No newline at end of file