From 0b146c7df557fc6b0848379031ca65c6d2359856 Mon Sep 17 00:00:00 2001
From: Philippe Pastor <philippe.pastor@isae.fr>
Date: Tue, 1 Oct 2024 15:42:54 +0200
Subject: [PATCH] version 1.0
---
.../2017_Ribeiro_Lustosa_Leandro_D.pdf | Bin
drone/__pycache__/drone_env.cpython-311.pyc | Bin 0 -> 4741 bytes
drone/__pycache__/drone_model.cpython-311.pyc | Bin 0 -> 5833 bytes
drone/__pycache__/model_uav.cpython-311.pyc | Bin 0 -> 5831 bytes
drone/__pycache__/rotation.cpython-311.pyc | Bin 0 -> 8679 bytes
drone/drone.yaml | 9 +
drone/drone_env.py | 72 ++++
drone/drone_model.py | 84 ++++
drone/main.py | 83 ++++
drone/rotation.py | 97 +++++
mavion/__pycache__/mavion.cpython-311.pyc | Bin 0 -> 11514 bytes
mavion/__pycache__/model.cpython-311.pyc | Bin 0 -> 13223 bytes
mavion/__pycache__/rotation.cpython-311.pyc | Bin 0 -> 8598 bytes
mavion/ctlfun.py | 14 -
mavion/main.py | 82 ++++
mavion/mavion.py | 386 ++++--------------
mavion/mavion_env.py | 70 ++++
mavion/quaternion.py | 86 ++++
mavion/rotation.py | 93 +++++
19 files changed, 746 insertions(+), 330 deletions(-)
rename {mavion => biblio}/2017_Ribeiro_Lustosa_Leandro_D.pdf (100%)
create mode 100644 drone/__pycache__/drone_env.cpython-311.pyc
create mode 100644 drone/__pycache__/drone_model.cpython-311.pyc
create mode 100644 drone/__pycache__/model_uav.cpython-311.pyc
create mode 100644 drone/__pycache__/rotation.cpython-311.pyc
create mode 100644 drone/drone.yaml
create mode 100644 drone/drone_env.py
create mode 100644 drone/drone_model.py
create mode 100644 drone/main.py
create mode 100644 drone/rotation.py
create mode 100644 mavion/__pycache__/mavion.cpython-311.pyc
create mode 100644 mavion/__pycache__/model.cpython-311.pyc
create mode 100644 mavion/__pycache__/rotation.cpython-311.pyc
delete mode 100644 mavion/ctlfun.py
create mode 100644 mavion/main.py
create mode 100644 mavion/mavion_env.py
create mode 100644 mavion/quaternion.py
create mode 100644 mavion/rotation.py
diff --git a/mavion/2017_Ribeiro_Lustosa_Leandro_D.pdf b/biblio/2017_Ribeiro_Lustosa_Leandro_D.pdf
similarity index 100%
rename from mavion/2017_Ribeiro_Lustosa_Leandro_D.pdf
rename to biblio/2017_Ribeiro_Lustosa_Leandro_D.pdf
diff --git a/drone/__pycache__/drone_env.cpython-311.pyc b/drone/__pycache__/drone_env.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..f8a960b9f5f4a46e6d430e0a89bdd889752e2fb5
GIT binary patch
literal 4741
zcmcf_TWk~A_0G#4+p!bJaex$ugvSQMmM)>NEn**Jf$h@D^s#7>6=^idOdNatNbgL5
zc%5xNsA?)p8>&=zS5&oDG|LKFrBeH`5>@$N|Lv8lS|dfGYOBhBK`c_Wm8zaIlNmoq
zs_sv(XU^Pn&V8JF?s+92`Tbr3E%>*8&CEdWuZY-9*z7!c7dkhILKG%V;*9xa;~ae1
zG@lXT0)sp*?aH|0ZnMv)#f&HJVTj74%=K|4MacUIiTf0n>N9&iEP0(M?rTI5RW9Y<
z+>NYqgxrPKYVm-={D4F~O+;f+#t`Q9WLm?{8Jf$g&=G$)FYDv#e7gBN7}I0`Oyg*~
z*=s%d7EpcyCsEB)#95Z8+$zxS+9TnFGpxd1W8%ERT_bTpaTUo7qwv=}ahD<h#a$#<
znYaiNxed`2;B5ACY}(!e7jnUi_>(-qo20-Lh=g>rDTtwjX47pI1D0~uNPuELWL9C+
zU1ucB;x{LeZD(=avvf!Kgydl_SOujsTgNJHhwAJC5{WjoQ<jv+rgyC2?4|IX<2QGP
z%ePv`tpCT(JzMVV7+is*oKu65ntDn(dU7a^{4m~GF~RH3+;)xKbFH#)fACsuk<)kE
zeHhnk_bYvNul-x#e5Al_yE0fx%R%5ygZA7nvxnkVM8&hs9^O?r)m`Ugut&eMN4w32
zY^vhxymOc_VDmceyEUWdnHk|PJ!8<Gp`dN=Xkp->{#fFjf#1CTm;A~rQSKkd%*MS+
z@fuAK<(ha66z|dmP4vJfUKvIgMPHaxDPk0DG|Dtl9Z}X0v{@NAIJb#5)Wij%J7EUK
zBZJfBd4tW)8G=lyykPj}a+;*iQdOJHr4_@isEKhZD~4arCfex2Ofox@Gu$)joU9)k
zH@J*^)!<=4&G27V(`_UDG^e+rzUf?cTGrJpyatz8$QUkdPM(H&+(hoG5zI|#D!nZ0
z$y`=48Agvh-G&KymF6@!m2NHo!wr`%(}b!UJ2YLL)1)bNA(vGoU8R{M?xI9_gV)sb
z48<!pd{oUUDwQ%hMK$=@WMWoBO^rbN;?V5L$?wu!O4YT=GvB`ud*k$E@~av7@}#LN
z3rcGC@{zd(LzJXsHmOTeK4=HlBQ^>3tbGTf;SzZi9xC!BZzHq=KH^gs86LTHsglEA
zEpnnBIZ@>6;?R0z4^Y$it3{60BgcN9s6}3Cv3@mmJ5`Gut>bh25A1#S1LhO&eQzx?
z@js9GdvCDx?xVhedf(p4_iBC5HM+YNVZ$T%15bNMD7<oLqgU+nJ|+M@M^NOS_LIE_
zZV!Dl{NeE0z?!h8e>(o~WM#NE8mo`SP?F*G-Ft4G|7Glh*qx(yWQ$*p)pp0~yG{PC
z_3)0BOXaB>lPi;`mUY<X8|8@`r&dm(de#SbuFRI{jr2+yP+w#0FfO{kYCcausbtzo
zF@QL!YYnK7x=0F#m`!06_Q&il8zqa7$lAD^!gp}l4lb_<9bB%1%PL$4m)D(C2q~*8
znJfyJP*YA|?UpMKX=u?^5IV-NimQDG9HTn{9>TE`=<;wr#k~y=0$(0xZNtTV-32^%
zOE#AxcJI3=7Vz|~F%X2G8ROBNv<T8U0k_~PxC>&zljzoox8Q|<=Z#t6$_z@lKoevl
z6eD*NPfG*X!{kjd3P)Y^Aog*2dI$kVQ-hsL(oyI{ee`+6If!2d2O-GdAn+JG7Fz}f
z!3zSOggp3cVmJse7)GNoRl`t=VPLb-ui<*Wwt$o$a6G+@ic)ER0<c6Hf&Nme94sF#
zhf293XQSigfqG!r#{0_?<#0W)2k_odDPJBfpDyn!{iw*V2L_k*>)BEkIDMgIW##S4
zXytTeUw!a7@YkZy=nt3qTRtGkmDlP!57zn*{(9ox*;?@EnhY^P_-&%r_wuLx55u*-
zb1-_#B^=^JWq*BetkySH6xIWMrEFQPT)cC3?RYiv&064OJ#Z4m>o0ogQ8-yg!H5@t
z7XO3=ulqT3ljv<exeAQC@LH|X#<6MOBh0<7a*^4lNGw2gt8kqa_9WQiBDcuvPFdG(
zb5M-(-DM@DTE~0L#OqjVfmEELu_YfNou`2L&R*aPPCrRzSg>OO%X~;Ms*?ENeQY29
z(eAtw^;<TM{?kg%-Lw{Wi(b?V$!;DpxFJANR&|QHH+E*^g(+2<lc}akCS97!&1V(O
z^a_flh2<`Xv3D8)+Bi57dI^0EfZ@)~VPXY$N$0N6BS`W{(sWwZG)dA>Wu_-AnT|lu
z0-g^z4q)zi9BA4*0RFi|)<yqycgg*p|EK=)*Ngs|I8ql!s^SQwyzo$cV7$>2TE195
z^O$49kWK(x_Y}F}n@>doLH|;9_^kER!sqJYbC?_K=ntv$&!K&vh4$4#2kM~%m?zM2
zu9o|Yg{t@!%Ro_acEjs64fNm8=}ZgT7-U<3D=;0w%n7whm&t-5IDxLS`dnl&tV)hV
zU}TQ@*&^ci86#^m%hZ<rJV>LkZ<~uOas`fpdt?~}oB^b1&EN}=-oQ8o(>N){SF<KG
z&F)-OwCW%pt0l`uFp$zQ1X$9xS&LxBRF_qO8J?_qMY0MnZ>zjATvWXxQ$<64m=<Fh
z%(nr}`yGS9?IwVv%>#f?Ik0o({LKqLyHGiE`}~><;+Oa!_=)en@4=f7FVzNO^?_K?
z(+Cfnmav5d3=To1px?S$8yvec`q|*&qPH<PQVotR2bY5tx#+S?WUw-H=i=H#wf96-
zJYiZ&l#Rur9<1K7az>RT!z*DaIG@ITk0e39NLw6=cTZnHVEPX_0L4QY{!K5LR?}%o
ziZb*KplTTFUbT*nA%Y_CAp^5CIEejw6xmz8`2JhB-dfwgL3RnxcY_l?A+$jpFys?<
zBf2#hatSYPkpBkEQ=7s>dkrBwpP5^*GN%WM@2spP=QDIKNN5Z@_z@^^wIgOANKQ*o
zu<JCJw&IRY!M_^J;yG<9$Pgian&rBcnO}sZDArwu8~$t<hd&<-4*ut03h*(pgu%r6
zg?P$R`^$6^P*k4w3jiA&!!QlfU$vhG*-^Eh26>^{`8*<#YUk7FM1lLr+fzEQ%w3Nz
hxf@<zF>(Fyl6!-Hi2=nu3yNCl%44K|z6H}1|34)`+0Xz0
literal 0
HcmV?d00001
diff --git a/drone/__pycache__/drone_model.cpython-311.pyc b/drone/__pycache__/drone_model.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..e0b739228ad83fa0adf9aeadf660a82692989f3f
GIT binary patch
literal 5833
zcma)AYi!%r73NzH%a-L=(#B{Lw@ITWb>ckQwN2ByP3NR>ocPfuRlTZ=Dcef)uu0m9
z6)H1^4514FRSI|Eh5}QEG;<br^N$U1hYaw5{qTVP5CQ%W2pH%v41@o9$O;tQkL?^%
zFUoPTE9vOmdk+uKz4tp0^4InCUIN<1KmKR*1aN=HPN^|P=HUfkZW96_kTD`e!ao(G
z;&g~6ag2^JaW=$SJQ?Fc9E>qBSKJ+P$2}nrNl-)&A+R?HfxAl)#C`bLE#wu*K_ci{
z$8KMcG`UeJ8JmvWBTbLRgrn0FLCSPZPVusslx-$H6&ooku>MNw9=-(K+eE}NFhmJn
zfx1D4Xo0>#gcyMV%Ayfq9tYGVxPZC^H&Bn@0W<1M_Jo*BM85M1_}4kZ$73TU<#;FD
z4H|wAuakHq8EB~~q(C~?mZoHsEw)Q#Jqjh4d4jl9mRY1~uBR0+{jrh)LLtJX(;#4m
zam=tKF_>W;GhEdS=a_MoX5>JL1A9Cr9yGZs@^@ja_si=ATP)0=tmEpss;k}KbG5HZ
z&Qp;Cz4cWyUdK#BX+|LMu=-3=M8xzac_AX2OkekzH%)rr<XMyKF_|;ngM+5)RNtEe
zL#Mh;?p$~v{6>%IIyZ3kT=?|45_2Z#H<{!_Bw;dm*d`MLWg?m&;@w0)l13!BMJAP)
zFc~4rkBLp#WqDELXT)ZlZ$Z)uB*>VI6p4+R%xE+gF*!*VBYfOsEl5dtf;*vow}1Sl
z;d5g0Qbd-9PYj&xJJmfLeJ;*V4_k6J4abv0B-S}GW4goPXd)_y!>I-fp`E2Zo|dEm
znI+Z&tyw{P<JOb&PpaHnW6Qip+w(=s;*NadQN8h~(Rfs43oVand*;umOvY1a-mbOI
z9|P812()FT`Nx6fssz@yJvM(uo65awJb6IhcF@>%5CnW{ZQJL^Vf4bMKi1n07;Oh&
zu&%JJU3=Hq_6!PC^QuqvJp|hbhZ7;-Ehm})<~G3k5{g(YO(_&QQp!ybDS&jEhS@Uc
z(u`b&d9~vxWDPhOyw;(IRcM7Br2sZm-#U6<kg1%Qh$w?Yi;g9NoY;;XGGS7uCrok(
z{^BOtC!rVNx$WF3?nD7j9v7!1IpwpD!|vhnORoW$CBCi?WQJ9$;A_YRvfZ~@=1!?(
zacs@kkm<>G&%Kp-OKX}ttMR$O&jsDrp^^oEV|JG+t94=rC~<5WY>2&SufcX~+B70p
z-XxGKx7v*vhLxx3Ew_;t$SQxM1*+0Zflg;MDvne|A!~%HiA;_NmV%gJ9SurteXo)f
z<A{C`z-#Pbs&$}lg>l#nBV&vc8fjKx0R(KHh#*+UT@#;14gm?6uv~c`j^e$<9XM)3
zLfnG`*8E=No&{o(Q=5RnPJyS&sX7}MR_6&2mF@!pSOi+<cc@&!-~7?F53gxmbJsK1
z=NOe#yATy8v%E&m3tGQ6`RUND%en3xf9HhWws)axp?~oR_}fz3a@TVIgCi@uR=WT4
z^p`K|T|KL9`N5(5+wT~I=kuZW^20weLhtK?@9U?+dH!O4L@@YBemt7LblDh>>3l4o
zoXAg#MpDvwDL;KBe|5%~PU(Cq-+E2=DTYt6P^h|X1TtWBsbOTd+|~<QSA#&t;ifF~
zaC@zm=FkxZ>Zes`O1of;5@^Sm8^$23ImSG4IY)uRm#q;XjVsI;?L1q7Q`i((DWj#u
zrntUG167?VYqdFZF`^a`miEf!d<qto`COHqrF#P&Ij4mRC2)ePdW8<_(8sN&coa_Y
zq+q=@R)7UKnyypa3XT4j_6cqoL0N1y>jinc3G5iDc*ZEFJvG`f;MO}VDqpYolzPRh
z)YbYE_>!kj?1Y=F;|{Fj1?yJ&L;#4-VHb~W41UnQel=KENvBp4tvH&9yJ>Q<Xo8Q8
znRGNUZL%X`Qj$beZPtw>6C=DFNx(1IPz=*o#RE7;Ua8S)GFUL0^mHU<(pWb#!nA-?
zwCIFICk0u;EN_MNGO^=3EzBfR9@~k5-3?+A1>pi**w>i(A$m!3W=3TSO>Oi0Ro4bb
zGzK8jcV(M1(=giL&yv}WERz{iY3xwhLs>R6uF~ItU{mI*7SOu2NzJ4CI}CqEu0JP#
z_Rbe$OTwRCe{gbzUk$8wukv4=SQ*xj4H(A;^v*$}b5P$ssQU+1##S-&QToHQc3$`I
zHvGF60vI9|4llO-q2u>^mk+PFR_Q<2>4$oaL%n*iSNB88M-!W~{c~5qJb%+C=e42F
z58XMw)V*|gnO=HUf9e(EsaN#Y<3{Uo-G5wV{^1W~u4{5`XyM3GOa6)Hb^i;7|AoB!
z1v@feBq%jJU@I~hz}zHma?Z-QlE9%thApgAksor^6?7$LjKR~zuWDrHI6FEVgL^0>
z^cahyfMbzU36&cK|GME$(_3nF)oO)YSmn7;Brr~&DCP@ypLhiB0SN*TZRL(Cg76m)
z0a?dtY@|9BqjGf5hYx^!Lft0j=p1mHn?g9ZCQX85wWjhVbBsbhx(=${-n(!p_sa)`
zwN-U`gkJT-1y!A1VXo>SVw3?l=WI6@-vmR%!$8(C%&ub{58HAON8dn#dDCQ55pdif
zYbQ<1MJeH`<+I{TARs}gLARTowY`lQol1<PwijzDF&ghY7E6xsG3j_G&b*G7^a1&H
zmMAo~-1dI#)w*ug&)2Kng+S}=?H_N~c4_^eQ+M18fzRsh)ail!Mqt1CdZD>B%g-Ox
zI<$dL_vAWq19$c;bSw-k?#VYF%=->nNDeaV0%#U{9R!wS6#%Dj6srIc_c7?Q!wxk)
zZc@`J@idH?)XYJXoUxaM$Nhm7NnR^1Ooqjzyp@F^R3#P2x3CsF=MT*FW_nd}Ezmr_
zUE7ZzJ@AYXcm}G1U23=b)Etxx0qo3y;})3>hW2!A5L=LMyhKA|=0d*hS^KNk?=$N6
zsnnXUDSJd~(tYiQuicU8@jOZm7lEsNI0^*ZnYu~eCR7R>j2?xu4IGTEb})q%D1{lt
zx*gK@B6}Bf-7o8d^v_iSK*hy{sz*^km#fcF?P4ad+y>t|d;+)M(rvbOlbT#N$2i?#
zkhn+o-J@*KB7B3aNy;WE*_dsI5%FapMX1geq57TsSd5Qf6!_z*cKcK%IWkd!YiSnY
zB<%tM$E=<B<;h>1%=IrYzw`dayVP~BezAUOQg46JXn%2*uuy146O$P@edOprueJQP
z=WnSSuim3fE*_D`lY;4v^H;-oqxJtn3(_L)uXqj#7NbR6S{Ss=N6Sr0kZp+4@G3>J
zvH1bQvxrSpSi&aq2xkeM_*(>L!T)03`Bj;<hHV<7P3AhZvE1ZR+sf1Vh9144$7tw*
zH0N*1Ol7WQ#mu$5`*Ax<lwkl$%dkRk23XNI8Gse7GAi+CSSch1EP-BRHbcs);T@2{
zqQipB&w#NY!{mV5+#q={kB+PqF-IwoT;Ly`JK^C8YM9EaDkprL(fa?nVvrDrz#8#w
zAP^bfLH;}tlN}wA#Tfiui6#Uqz|>$)L@!9~D8j8eIF``X4+2sTkj-d2x^#3ou-v^o
zwLG=bze;|2?ZGwu@M+`lX?<_6vA1_MvKq+`y`4XQ;jfp!x~#W{jP}rM5tHwNLNSEo
zJtUasOA(Nh!_ou~56M>#idOiHpacR)`~XN1Y&#KcHrQwh;$X3!0v4ewJrAS^wNT#g
z_?DnHn!C6eW~$dh7rZVMvEB@N@%2c8k4M5`(;LQ@CQ~uwec>=vfiZhVL}wKDA*m!Q
z+&7U&G7jWkaosy{08_a|5Vz7#fXtRZ8;qCTyFoa~b{E^SL6k`o%LX=xGHGaKf3QK6
zNoPIImdO(|y91(miFj$Y8N0Q_4i#t_zN$=2#V2O$eA^(6L?>oClM`|@9);YC;oJ1v
zgYYyO855yswu8ntoN_}+%*)Ya!m7Eg`r2M_3})@S>;w*ioxPdI#YyOr@RL3UvO$w1
vSs<G7rEh`Qm3Mv%#O}QFTOjIYt<UBuTXKWxB}rKFqv^k_@qceIYuWrij#0h_
literal 0
HcmV?d00001
diff --git a/drone/__pycache__/model_uav.cpython-311.pyc b/drone/__pycache__/model_uav.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..700269b85a0ebe6d39cf5eb5124e923d5d678ba5
GIT binary patch
literal 5831
zcma)AYitzP6`uD#y!Lwif&?ouP9Q80gLx!PLI_Ss4Z#@vNFW<Go58cT7xrP#j1699
zsZ~)ij#kPNIb~W!>ZT&p6sPr%R&pvTQK>&%DL;0k`LP<wQmU$|{LdAQ6s14fb7uB6
zYlwPx_w2d%o;h>wz2AAv{JOs0OF+Bu$N!8S2k!6KDK)0ZJUkD~EkYm!GDd_*_@`o2
zoDR_>j?pnD&W2cvCu3ZQgE1!Nin~MZxF_Tx35w_?1ok>1aCazzxCcMGg}eeeL<C*y
z*zFIJCO0M}V>6Mvr0KDkaCBxeNSUswX<inSvdzS&W1~d{)?Z28!<V6Zi-=eThA6=+
zP}j*2EzsAA5F;=^Su_I7<AAyZ7f`q02I>(!U`CzE9v72|$ah`=|GH-Rcx<$!9Pfm?
zLBsFiH4<+m11&X$6iDaV(v*y{#dfi*N1^00PY@T&GD}p=^|S(}KT%RZC`7n)8U)NR
zjv2Nj1~aT<hO3(495b%cj2tL&V2`K7gC<u+{tm45UU|J>i-j4KbzEInb+!9@uJ%>Q
zc`9<Cx4vq|>zHXM%?Jb@R=-J#h?u@4FGNI>>F+uHrb!Q;IAfB%CUd%HXvlP(?0<7`
z_+*dCoed9$-{>`6X9v%m4WBw&VonGBCX<|uBuoYm+hk&(OhgkzyqoAp(uf4N$fOdJ
zCL=`oaj^-zEH8@utk{h6El65{1R0Z&BC#=(8H>guCMU^agpZr71t|$numjq6yC+^A
zIV&bFMr3K^_~4oTlRYER=i~g$h$S}?PYRJ(c$%N-nw&M=;czq&mBZmwgN4toQXkJs
z(tyknYk}6RpuKVP>4m3NZmqFp!K3Z`qGf4YzVWc$c-Uw>tg?lcC$yalr&T87DKu}@
zS{IH0>n;S^veLqnz;aarYg?XJxU5a*-Zh@yr*GMBY}pS2zO}Zk3llJU{?i}pZTpP2
zeK1&8*wU`OYixNA1*&<~r}`d(ZG^*x5bl->O#*WZKz$KKtd^z}3f(B>CWsUOI!(iD
z8FFbxE`z+<Z4|Nwnhaj+(8DUU!j4e@8mfOCeJ{vV&Pzm$!I4GB6G2XF#}1h=sZ*0C
zIShYslkAt!hw#)oI>ZhX;N%H$T9Q*f`#9_#9=>!8$Q<!?eIPTUQUzZ_HjwSP*)o4p
zC5vNgzJ^S1wrBpW%v)O1{27hU1%58*zD|`a_#3m^RavbQJ3)zK(_lmFO*;nLv1!wY
zTzQi~uH0%jW&~EArXRbFv_MvQ8!b?kUJ7(NV^MLWDh632R83rR#IF=Y4C`o6>hbp~
zNimMt2jRQM9;R9c>Q)$s%`h^~IH8ed6&66i_KOIDb=)=aS>zCqfC<Zy_uwerOWcN|
zHYCKIIAG21LT)z@lbqfJ40akkRZi8}xUf1;fv9v32*4uHy0A^<3jXGgu6}q`>z==s
zxi-(Jq}q+BIFaQwazW4rw5d;rZ(hpv<oMgi^|oD$-HQWDhrr*K+g7?)2JRnvu>C>L
zU!MK)6}`K6wJkq1oPYZrW9VEy^j?1CM@Hy<edvAtWH`@X$d3vJAIVQd^A|4}6EU5S
z<&%^7DbYwuIxppCF6Xbz8Z#-KPvu*$>ORHrDHaM<w~assj4m~d>z3PkVe4uT$T-}T
zg&uCN)zTa~qCow$3QcJjj8OvZ7<0oI<TS^aM=ob5aQLz{0wi&T8K<3RD{u;%0xM;-
zwAd8a_h_K1^JJ|yXC_9}BEr&MxtvYGqB5VWva@t=z$53hP@x1)a8<9+VIBIo)fA7y
zDV`Lpx5f&v07uhxid&)4-_kz8Eh8w4t!BL-Z#RJ*Cl${)<+P_pI|kf(hehS<6`xYC
zc$K<Ze*$0f^ow0^lXcvIb-ZBRN}dP+@p<gxv5mtI+SjiI>nh3AN}&};6LB|9E*4Gj
zv2l}*CT2`_R7^^eh^o!H(PUzjmm>-I1sjTC`ii&@=g7-7T1^JaMU$S1#7r8iMn;$s
zkct+awCI!|OPJ%WuwEv1e5ZxkM9O13F|a#8OrjuMfD8K?Ge1NxY0k{5Orfc5VXx}i
z;E2WmMEdS*b7lrc8~j-^+nHrD<0_3EDtjQyW+qhn8xU;DT+sqrk2a-wbbqJe@5~M4
z<j>yuVtiTn)9d$7Jm6OYt39jySH~ZW=tl;PBZGR^kkK`y?-<hkLn>pdnEfdIVOl$<
z`*#@r9g6`B5sL?x+Wye_`&}ysAGlWOKiBC8`iuj8dazITL&`@Jo3jJ+m%%)L(<kS&
z;m;4;KDykqd~k(c-mO3Ls`1RLdh1c6^{DPYsxtrZ2Qt?*IXAp`Xt^c-)C;=*MZ^DM
z-u<E-888x*8Xm9}84O@<5H~nyU0g}vP#?n<R%*x(x$6445;Ml(+2U6<vh$oB9ge^~
z6cT!j#Zkbq$f<<Nje&n%cc<ycYIW6Wg<M$Wxlkl9PM;{|3wOVG2<`z10ugQHjw*ui
z7Y_hg$7*b}Iu)aGbkB$PfqX*UBIfBFaGRS#IJYKEf@HO(@+I?(LO!|<s@>i@a47f6
z2Zgm&b$Wze^`iw<onB$C>Jeg$0XOGtHy7UoL&SqX)-lYkV-*kEvL8p^K!SPGWK$7v
z+#qWwP0K|o;fm$6;>#c)L8w8uo13$}jTxIxjHb30Ybh}r?>Z7oj`A_-Xcx}Bj+gWU
z`F4&dG`8IGe(cq{Z`Lo=t38E4>#eOHZ`HPI1D{j3-HU<G>TcKRfxSjxuljnSxi!l#
z9M(Fu!B2PQI&*`!cP@4=4leD?H}B8;_FG5}GV20p7W*6omShzGCvg<301@{w=(58O
zH8WvSGb!;DjG5Hzev_QFmxaguffY%P6&EJMVp879!Vs#G3glZ@i=Fca=le2!D!CSD
zUf8Pb#g86%&ImjQRl#<(M}1}<%7p-S=D~4`%mzbyx;Kc&kZ-(1Lu2NAzHPVt)$8{d
z^?Oum&DWGYq&4ZjcEi{1Nc4CfrG|^Z)jk{p0`5%Rpl=Z>1rA1!LD>cl##TF+!U~ka
zjA7jl>3fO21G?^&^+EdQDgmJ4;zHGfD4@&L@2GY$6IgD8?;JjX+dtNA9`7bKxo)0u
zy2B80m+Zew*`P)E23eDoO;WNk+YTe*D?o}+ohw51JNJ<oAHN{*M^o+gsY-HWvI5uA
zEW%0J4g`)_JO0ZPzc`T_SY&?Z{f&3I`)>VG{qmIF{*uxD(i~x-(26D|GkEII;eTFj
z`EBptQrBO*OPO3eB2Od*(;erpgz-k}|AiK$MciNUED|h6i@3BfXq%6go0K5i5T)To
ziezK+1B7Q0o2amaP2>^I5<2m>2+o54rM&a2GHVT6G)9}sb!y|espYl@&*mF?^@d)f
zp%>DezbP}FxttX<SM%;C?JQA-0VplQ3cVR%Mc-fmR<z2f#G_%QkQlH8dWqQ#DXWHe
zKn9Br3o<_g#)1r!18#GJ<iR{TvQor6r9g6le|YYMhbO3EDle*>@Nq`#|L2M!LL3Hb
z#J7P!WPAttb3jaXY*ZFw@OL?y5Uc=GgE<ksAhoRsx9Z?nLfZfcNWDNdqwVnW;g!Hj
z&&u@5^n-y_^2@9Duj&U+83#}4yZVe>eXEhxNPhV3{JHagz4X;3y**^Khvtfyd>0go
zAtdi1!8BirfSeqbCV6;BzOrAm!e;~}5J2JwK#E}NK(yIlqa}!g#d-=@gs$`gkRsGV
zdB5XZg4$T_!e*GMUJG6DvQWf&Gw8+FBMCkp35QK@I1DdNrenza!eOWaWA==Q&M59d
zQb|_0Zz7Lm0?6Isx;t<HQ@KSDx6)65%#}YIjF;WDK{&})7u&Kylt~lI1~!N?X=r7C
zutAhbS3S;_$x}4D4WfC8cxkp7yS2m)6=)g0s7y@9Cui+^+aQfbCuh5olX5g3h1`qb
z+w|Lm@H8437oljjgT@w|azjbX%h6=Qs=2ND+Fo!BX6?Il00+U&-pr%o6m&`WNgo5*
yph=P}5KZ~gw?J&qJHG{DN8b4@5Os6bXY-UTxxw_2BrN&S^k3HazaKMe+5A7)o4q&y
literal 0
HcmV?d00001
diff --git a/drone/__pycache__/rotation.cpython-311.pyc b/drone/__pycache__/rotation.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..0242eab78cf10c159fbcfa00024125c5ba7a0a73
GIT binary patch
literal 8679
zcmcf`TWk|ocE+CgHE}{hc(l-fArD+=i$e&HmQbe@3ZwytG=&h_PBX@Y4GA7QK#*zk
zvAaqtS8Jr8I@(k+Rp=&aDj6x2vJxwW6_rwnm1ZO(p^+*f?HBu%75mkdcK4h+<MCWS
z0{z)Z=J>wvd7pFd{K@ThG4SlT|L^lZLh0{V$vzgn@GJ?1uNaXL*#V}94YC)rJ&b6%
z&NEDpRkQ}Jq{NBbZ<(Glu?(P1v;nk>c7P7i0njNr0lGvNKwjhlmW$;8-J%;{g;)Ww
zQmh2HN?Zl7N~{7{Emi|uEv^PwBh~<1Bd!6sR%A~x-nCB=&5oz@0H3k|`VeFvI~1(&
zK1%@jiiw)~E*fQ#jR7SuW#=$F19fYz9<tK?WIUVe=NU4>G=r>Q8X>YFYsmU7?D(Cr
z>nUcI>yVI@PZ3BQfLV)b8N95P36dm?cv+Rb7QykGfAL<}-+NLTx)_W`dJlD;=s4={
z?SJi(aHUt2h6aPZ(oj^0_74rdetAT-UJ#;^8)hNxk#+#1%+I_#zBeHxJxTvmU2;u|
zO?jp{c}@KcJL8$-W`wjS?VqnpuaO(~Ecg}z_nH^#7poS14{8>}^1h>T%`t^Pp5c$n
z-0^3y9?fDKG!PbdL7^0jfq^D=(Co}-c0QZDT!+^pAvHRW2-8Rdv?JTJ1_zXoXO}dd
zN##&i?frvSK1PmILk)F3QV$@@kw79k@nKR(aZ{mGc&29hs$BDi!tc!RJ7sRCQ~@K4
zYty&T0~Wy+*C(1hnw;xp408kZ%_?%gGwB?)QqI2eG>={x*SZcg<?R3#dBYN8W0vz4
zu#d71jdg~OJw=Xrt&$UdRGT36^$kT-E)tfaUaQK6RaQ{h!3aup^XAPO6J6n9A=)Hd
z69z-l%g}+0iM$VBlzHrMe%Tg33cAHkh~p>XCltqqQ9rbfdB*%>TjRD{)f4NIRmngy
zJk^!*r-bQ4lV>M4DOKJ~l~-|iVJu%hCX6+Y9xvoK(5I=^6DX8cEt>utEXW0C>Q4-;
z`67}-o+A80iCCi90q}bfIYge6X`O5v!^A9kJ6b8}p+S%*W{t7nAwfbS@s8hu48AjD
zdxMK{F-wf}5J_<zA#L81gPK;O%6sjapvt0>&=(|Htg>CIZJ>Wp7znA{;E;4lwSyF+
z!eEoPOl3P&HY%ZSQdvn_55Ezl0YsS<rc>;@q|34ndRF6*PXK^TI9zc@qH3Iv^C&|8
z%MiMZsjW9dN=0p^qE>O#j{0$s^A8nYR3yTR<E4z_C9TPo@Fl`Y-_79^d-FGnV@t-d
zMHg+tGaek<J9?r}yl9TZb8G=8S6aNKJx9KyCUuT-%p7epP^0}2H5$z_H#>xLO?9Z5
zMi;DPGG+(}IF}x6T8ZFk8c(kk;zC@Wg0V3SbzsVd6%(U=irkZk-?OOfWvL3P5gd}0
zwH*!j5_rFD(6|wM{|G>r^b>c*SUBF5@F#?cLy6r<UotS&oUBiIQvT_>)Ec?G@v(UX
z-Cw-7)ZFaqT0hKPc9uC^|Fqhjwtq1IM*YjHm~!{%dzz9MHUh{Zi9|^npg=20EYI`B
z*y~`q5Lej_FH1;<_|OQ`q;&v_FlKl_+73N9B!ck)!$Nh!H@-i<KOv02C3EWvl@@l;
z@Yw+c8lPN7fj)<}_EU`XDog-`h~Kq}L(y4Q!;3_C&CxngUTD(N8!#9r7<jqkEeTH|
zF#cA&I9_Pwgip;-_zL!?e3FrMgunp4VQKZkI0EWr>F{#I@jU%6JdzLA(Euu#^f~}@
zU?r&b4GmtDnxN-3?A-tWA_8Cjr4+xGti(U1a$BZyo5F9Cxos=(@@<$&^D<wyQKU_O
z^7}CJ%<&h|A?zzanOFnl0}`@mC%h3fb`<*Rgn#_a_?us^PkAOcOl?s3I+?3m0biU<
z<C~K`<a!X_LwPZR@RyixRt0!qUY`&R4#JCR%dnZd&MU#bXcKCKv={c;g~*~aM`Ydc
z)}%kVE5#;(v0F3zR+-ycXg4S{q7T1^0*&iRyfkYK6-D!OCSSj4ZHN5BBC;Z5&Tupi
zp!q=Zfo4a<K*)mzv`}de0F}K$Cm%#=kl;#hLT?Ma5fmT#u*zHZBzw~};rjZ>ouf0t
z?e^*Rv`5+4oY~l{ICf_oyR`@u80#GCjPD+8Ux7(qz>+kRasoB8BzN>dpN)z~n-Ykg
zP6`dREj}Jiuehp^;Lj0nAtBYO74tM}q<;_;2vvaudCIA|NQS0q%|RG}42$4gU2DE>
zn>w8GrNXy&Prg6(e)8aq{|C#wEgfF)-5tK`o_8xd4#+RJD*V9=e^BNQ7V1B`E}~P_
z;Gus#yK<pbJ2%p-qu5NbZRL5OXKZ1>q*gP$AJu9^84!&rI2Xe$FVi<;_uQEUxLGJH
zo{yU;Q7Dev3fMhoZUJuAD9TuWVss786ttDoHSoZg`FhJVoyFu%I)I=R0UA7n7gSu;
z5|-e-4DUz;{Xs4_bp(e8H2pmU?Km;=R{)^DPCmZ=R?|d_euNEAHr#BTXuZ=k-7@2u
z@z2%Ggl8LWw@$aFeQ80i-m5tGWt{s)+e*w!`R;r=eSRh|6Q1izduF?D4^9uh*u0eR
zef8YhG@JI!bLqfc+idMzt$9DlADV7mO-iYzo1ag7uJDZ+zES2H3tf*9um1xCu5^~9
ztpraKl>X$j7+eo65p*upSVPzHBPcb>&~7m=hLQ6oU`vfg0g|U+O>mCc2QY)Zb9WzP
zC7=Z31jGevhj!q>6whnZ^aoW(EgKq+1|`+eF9rvr{m~Ikm+*1}LWbc4RVy56=%C@$
zTp_eeM`83~cq1PH$fhS(Aio=lkBlDxpZH!XFvCuEOm!%(SHJI^>rVS;&&-|qOZB}C
zi-AAa-m6t!JEFLb{B6s_2076AllP(bQTG|S=d9A*o9XUVj`b?8-qCj+^A)4V3O%MF
z<z?6$voi2j7IRdD(<*&>%*m*zJYfaXu>#JO5a;2nPcq-qx*?Vo;Xhy`;{AeYJ)$uf
zdGx20+$b-R7XmRT>T<Q|a;<^oOt%n3%(|OE$w)^Kpj1iTre^~xdsTW5+GxR&w6h82
ziZ@%wuzwf;xroSKIgqeVa!@wu8~-f+nZm!4s!I8$YasTmoAH6K*nNA`4{h_u7umZV
z^Bv0OHid6n3@lYG`5xBXKmF+BDY@$d<z#o}WViBex59Tn>iJae6P2D|rYESlf+fX`
zPEQMv080f(Ed>|pFpx82>MD{J8)bNgnSuySZl0bIvgYmcLbG_<#9s)v!VAtDbS7jd
zHbsSbZBr<vsaS!Al5_A<7C6m)Lt|m4zvSG!{ay&iBJ|S|em?zWxkQJipW>LAf|4^t
zONe$WFEncfxv#hr@Y(haTyfJ_-+=3{m<5mIaLEM_9KF2*j2nvjjV&@ZcclScq#@T&
za$vpH=z=T)a})_}Etg;zxkcwS)zUcv@AbUg!j8EGI*tHLJOOWHCx}E(EfV|iFB!hG
zWx8ReYQ{HLb9=}14m~}tnvdQspD&kd4=l1jwmmq!<XZ|nY`)+ApndVcqt32O=V|#&
zk9@W_bLO1V`BA3xBl+ORa`h*QQ^+`lY!1>kaag-Sx!FF^p7P}KkJ|&&1F3WJ>RstG
z`1fP=gSAWSlIJ0Jf8&FVOX1=hy#1UFXgboYyt?U!Pv_4s1Qx>gy6z6l52Vl0l*pZ@
z<mN89`(yn-hyG*Uov@7WNpzOD5J4fKc0Jn<A}Zzc@;|YF&oWsbh8<`rc{7(yJoIrU
zZwF^E41m;XS{nvKdQGa`6!i3VAc0Pw^86PnAbAKaChTDs(`tn{(pV5$05|-l?S;o{
zXv0guHMCJK!o9*BX^c#~Bs&7;g_y0tJs3~p{T%KAMS0vS<$u5@#EN_jEBsI)VV*Fn
zTl;1xldlzU04Amn29I|Js`c`Pezh!mAs7{)9O=)qdM#f7)0a-e=nvtIyrP-Ct33W{
zBA}fowCrGVyZ${aoUWRS&hpC3yOcG}X<>n#7Z?0@LyPR9=K;4U+_x(&N0vNG{)ct)
z$-qyIN=pFp1-bjfOxI~l47<)MEoUJs%wS^pv%4~W2Cl~aNnz>`oW29o&6CHqGfTQE
z?VGQe-L7oht*mQN+<P+aJ#zV;(RY9Tf8j<x3!X~Wr&h}~^$OpR;TvSGVTCmIAHcv;
zHcsmc)J%4oryF!kHY^L~n6&sstpjG6tO07~SOsgFKDqo_owVVXM-q=nDAK<|Vp&=R
z06GzOG9p_h4X3b^lRVh@0fKG>MEh{H+=15)0jUYKvj>{b!5a|(Kv2>&AoqDvaW!OI
z4LNnHnYgOBH)Y(L<nm31!W0?4f`q;6)Uv_hOP5DTJXI~1`&GLb3^hrDsM-NEK}E~T
z@ot=a_>=0ev<(3s=(THBa^Z@1a^x<LT+>N-{UjYifX6SAtCP4(tdI~p%Mdjq_ujua
z-nuj-4i5xdrB7fu`n||I0GF*S%RXkR<?R0xU`u_%I_@2{KX&nBq4Djb_GKH(wzJEO
z2||kx8+72Xva6OE1GviAnq|fS)lSyE%ot!T%dW?{I)J6}s$T5X0rb{mRR>V5!>SH!
z7Pe}cF@TF@-zIn((8jUbmKg)AvappnUI&0SlnmhF5EBEe;@C#4>Y&^x<=U`BnEXG%
CP!1sg
literal 0
HcmV?d00001
diff --git a/drone/drone.yaml b/drone/drone.yaml
new file mode 100644
index 0000000..72471e7
--- /dev/null
+++ b/drone/drone.yaml
@@ -0,0 +1,9 @@
+---
+NAME : "DRONE"
+G : 9.81
+RHO : 1.225
+MASS : 0.5
+P_R_CG : 0.1
+INERTIA : [0.0036, 0.0036, 0.0072]
+PROP_KP : 7.84e-06
+PROP_KM : 2.400e-7
diff --git a/drone/drone_env.py b/drone/drone_env.py
new file mode 100644
index 0000000..87d0fb9
--- /dev/null
+++ b/drone/drone_env.py
@@ -0,0 +1,72 @@
+import numpy as np
+import gymnasium as gym
+from gymnasium.envs.classic_control import utils
+
+from drone_model import Drone
+from rotation import quat2eul
+
+deg2rad = np.pi/180
+rad2deg = 1 / deg2rad
+
+class DroneEnv(gym.Env):
+
+ def __init__(self, drone=Drone(), render_mode=None):
+
+ self.drone = drone
+ self.tau = 0.02 # seconds between state updates
+
+ # Angle at which to fail the episode
+ self.pos_threshold = np.array([2.4, 2.4, 2.4])
+ self.ang_threshold = np.array([15, 15, 15]) * deg2rad
+ self.vel_threshold = np.finfo(np.float32).max * np.ones(3)
+ self.rot_threshold = np.finfo(np.float32).max * np.ones(3)
+
+ high = np.concatenate(
+ [
+ self.pos_threshold * 2,
+ self.ang_threshold * 2,
+ self.vel_threshold,
+ self.rot_threshold
+ ],
+ dtype=np.float32,
+ )
+ self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
+ self.action_space = gym.spaces.Box(0, 1, shape=(4,), dtype=np.float32)
+ self.state = np.zeros(13)
+ self.obs = np.zeros(12)
+ self.target = np.zeros(12)
+ self.steps_beyond_terminated = None
+
+ def observation(self):
+ pos = self.state[0:3]
+ vel = self.state[7:10]
+ rot = self.state[10:13]
+ quat = self.state[3:7]
+ ang = quat2eul(quat)
+ d_pos = pos - self.target[0:3]
+ d_ang = ((ang - self.target[3:6]) + np.pi) % (2*np.pi) - np.pi
+ d_vel = vel - self.target[6:9]
+ d_rot = rot - self.target[9:12]
+ obs = np.concatenate([d_pos, d_ang, d_vel, d_rot])
+ return obs
+
+ def reset(self, seed=None, options=None):
+ super().reset(seed=seed)
+ # Note that if you use custom reset bounds, it may lead to out-of-bound
+ # state/observations.
+ low, high = utils.maybe_parse_reset_bounds(
+ options, -0.05, 0.05 # default low
+ ) # default high
+ # self.state = self.np_random.uniform(low=low, high=high, size=(13,))
+ self.state = np.array([0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0])
+ self.steps_beyond_terminated = None
+ self.obs = self.observation()
+ return self.obs, {}
+
+ def step(self, action):
+ new_state = self.drone.step(self.state, action, np.zeros(3), self.tau)
+ self.state = new_state
+ self.obs = self.observation()
+ terminated = (self.obs@self.obs < 1)
+ reward = 1 if terminated else 0 # Binary sparse rewards
+ return self.obs, reward, terminated, False, {}
diff --git a/drone/drone_model.py b/drone/drone_model.py
new file mode 100644
index 0000000..2fd00d6
--- /dev/null
+++ b/drone/drone_model.py
@@ -0,0 +1,84 @@
+import numpy as np
+from scipy.optimize import fsolve
+from scipy.integrate import solve_ivp
+import yaml
+from rotation import quatrot, quatmul
+
+class Drone:
+
+ def __init__(self, file="drone.yaml"):
+ stream = open(file, "r")
+ drone = yaml.load(stream, Loader=yaml.Loader)
+ self.NAME = drone['NAME']
+ self.RHO = drone['RHO']
+ self.G = drone['G']
+ self.MASS = drone['MASS']
+ self.INERTIA = np.diag(drone["INERTIA"])
+ self.P_R_CG = np.array(drone["P_R_CG"])
+ self.PROP_KP = drone['PROP_KP']
+ self.PROP_KM = drone['PROP_KM']
+
+ def thrust(self, n):
+ # prop forces computation / notice that negative thrust is not implemented
+ Kp = self.PROP_KP
+ T = Kp * n**2
+ # prop moments computation
+ Km = self.PROP_KM
+ N = np.sign(n) * Km * n**2
+ return T, N
+
+ def u_int(self, n):
+ # position of rotor wrt center of gravity
+ P_R_CG = self.P_R_CG
+ T, N = self.thrust(n)
+ u = np.array([T[0] + T[1] + T[2] + T[3], P_R_CG * (T[3] - T[2]), P_R_CG * (T[0] - T[1]), N[2] + N[3] - N[0] - N[1]])
+ return u
+
+ def dyn(self, x, u, w):
+
+ G = self.G
+ MASS = self.MASS
+ INERTIA = self.INERTIA
+
+ # state demultiplexing
+ quat = x[3:7]
+ vel = x[7:10]
+ rot = x[10:13]
+
+ # linear velocity derivative
+ dvdt = np.array([0, 0, G]) - 1/MASS * quatrot(quat, np.array([0, 0, u[0]]))
+
+ # angular velocity derivative
+ drdt = np.linalg.inv(INERTIA) @ (u[1:4] - np.cross(rot, INERTIA@rot))
+
+ # kinematic equations
+ dpdt = vel + w
+ dqdt = 1/2 * quatmul(quat, np.concatenate([[0], rot]))
+
+ return np.concatenate([dpdt, dqdt, dvdt, drdt])
+
+ def trim(self, vh, vz):
+ x = np.zeros(13)
+ x[3:7] = np.array([1, 0, 0, 0])
+ x[7] = vh
+ x[9] = vz
+
+ def func(n):
+ u = self.u_int(n)
+ dx = self.dyn(x, u, np.zeros(3))
+ return dx[9], dx[10], dx[11], dx[12]
+
+ y0 = np.array([100, 100, 100, 100])
+ y = fsolve(func, y0)
+ return y
+
+ def step(self, x, n, w, dt):
+ func = lambda t, s: self.dyn(s, self.u_int(n), w)
+ sol = solve_ivp(func, (0, dt), x, method='RK45', max_step=0.01)
+ return sol.y.T[-1]
+
+ def sim(self, t_span, x0, fctrl, fwind):
+ func = lambda t, s: self.dyn(s, self.u_int(fctrl(t, s, fwind(t, s))), fwind(t, s))
+ sol = solve_ivp(func, t_span, x0, method='RK45', max_step=0.01)
+ return sol
+
diff --git a/drone/main.py b/drone/main.py
new file mode 100644
index 0000000..d3e397a
--- /dev/null
+++ b/drone/main.py
@@ -0,0 +1,83 @@
+import numpy as np
+import matplotlib.pyplot as plt
+from drone_model import Drone
+from drone_env import DroneEnv
+
+deg2rad = np.pi/180
+rad2deg = 1 / deg2rad
+
+def upulse(t, t0, tf):
+ u = 0 if (t<t0 or t>=tf) else 1
+ return u
+
+def ustep(t, t0) :
+ u = 0 if t<t0 else 1
+ return u
+
+def uramp(t, t0) :
+ u = 0 if t<t0 else (t - t0)
+ return u
+
+
+if __name__ == "__main__":
+ drone = Drone()
+
+ vh = 0
+ vz = 0
+ # cap = 0
+ # omega = 0
+
+ n0 = drone.trim(vh, vz)
+ print(n0)
+
+ drone_env = DroneEnv(drone)
+ drone_env.target = np.array([0, 0, -20, 0, 0, 0, 0, 0, 0, 0, 0, 0])
+ obs0 = drone_env.reset()[0]
+ x0 = drone_env.state
+ print(drone_env.state)
+ obs1 = drone_env.step(n0)[0]
+ print(drone_env.state)
+
+ def ctl(t, s, w):
+ # d = (upulse(t, 5, 6))*dx*0.1
+ d = (upulse(t, 1, 1.1)-upulse(t, 1.1, 1.2))*np.linalg.norm(n0)*np.array([0.01, -0.01, 0.0, 0.0])
+ n = n0 + d*0.0
+ return n
+
+ def pid(t, s, w):
+ n = (10 * s[2])*np.array([1, 1, 1, 1])
+ return n
+
+ def wnd(t, s):
+ w = np.zeros(3)
+ return w
+
+ sim = drone.sim((0, 20), x0, pid, wnd)
+
+ fig, axs = plt.subplots(2, 2)
+
+ ax = axs[0][0]
+ ax.plot(sim.t, sim.y[0:3].T, label=('x','y','z'))
+ ax.legend()
+ ax.grid()
+ ax.set_title(label='positions')
+
+ ax = axs[1][0]
+ quat = sim.y[3:7].T
+ ax.plot(sim.t, quat, label=('w', 'x', 'y', 'z'))
+ ax.legend()
+ ax.grid()
+ ax.set_title(label='quaternion')
+
+ ax = axs[0][1]
+ ax.plot(sim.t, sim.y[7:10].T, label=('u','v','w'))
+ ax.legend()
+ ax.grid()
+ ax.set_title(label='vitesses')
+
+ ax = axs[1][1]
+ ax.plot(sim.t, sim.y[10:13].T, label=('p','q','r'))
+ ax.legend()
+ ax.grid()
+ ax.set_title(label='rotations')
+ plt.show()
diff --git a/drone/rotation.py b/drone/rotation.py
new file mode 100644
index 0000000..563c8f4
--- /dev/null
+++ b/drone/rotation.py
@@ -0,0 +1,97 @@
+import numpy as np
+
+deg2rad = np.pi/180
+rad2deg = 1 / deg2rad
+
+
+def hat(x):
+ return np.array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
+
+def inv_hat(m):
+ return np.array([m[2, 1], m[0, 2], m[1, 0]])
+
+# (a, n): a = angle, n = vecteur unitaire (axe de rotation)
+def quat2axang(q):
+ a = 2 * np.arccos(q[0])
+ n = q[1:4] / np.sqrt(1 - q[0] * q[0])
+ return a, n
+
+def dcm2axang(R):
+ t = np.trace(R)
+ n = 0.5 * inv_hat(R - R.T)
+ r = np.linalg.norm(n)
+ a = np.arctan2(r, (t - 1) / 2)
+ return a, n / r
+
+
+# Quaternions
+def quatmat(p):
+ m = np.array(
+ [
+ [p[0], -p[1], -p[2], -p[3]],
+ [p[1], p[0], -p[3], p[2]],
+ [p[2], p[3], p[0], -p[1]],
+ [p[3], -p[2], p[1], p[0]]
+ ]
+ )
+ return m
+
+def quatmul(p, q):
+ return quatmat(p)@q
+
+def quatnorm(q):
+ return np.dot(q, q)
+
+def quatconj(q):
+ return q * np.array([1, -1, -1, -1])
+
+def quatinv(q):
+ return quatconj(q) / quatnorm(q)
+
+def vect2quat(u):
+ return np.concatenate([[0], u])
+
+def quatrot(q, u):
+ v = quatmul(quatinv(q), quatmul(vect2quat(u), q))
+ return v[1:]
+
+
+def axang2quat(a, n):
+ return np.concatenate([[np.cos(a / 2)], n * np.sin(a / 2)])
+
+def eul2quat(a):
+ qr = np.array([np.cos(a[0]/2), np.sin(a[0]/2), 0, 0])
+ qp = np.array([np.cos(a[1]/2), 0, np.sin(a[1]/2), 0])
+ qy = np.array([np.cos(a[2]/2), 0, 0, np.sin(a[2]/2)])
+ return quatmul(qy, quatmul(qp, qr))
+
+
+# Matrices de Rotation (DCM)
+def axang2dcm(a, n):
+ dcm = (1 - np.cos(a)) * np.outer(n, n) + np.cos(a) * np.identity(3) - np.sin(a) * hat(n)
+ return dcm
+
+def quat2dcm(q):
+ w = q[1:4]
+ dcm = (2 * np.outer(w, w) + (q[0]**2 - w @ w) * np.identity(3) - 2 * q[0] * hat(w)) / quatnorm(q)
+ return dcm
+
+def eul2dcm(a):
+ Rx = np.array([[1, 0, 0], [0, np.cos(a[0]), np.sin(a[0])], [0, -np.sin(a[0]), np.cos(a[0])]])
+ Ry = np.array([[np.cos(a[1]), 0, -np.sin(a[1])], [0, 1, 0], [np.sin(a[1]), 0, np.cos(a[1])]])
+ Rz = np.array([[np.cos(a[2]), np.sin(a[2]), 0], [-np.sin(a[2]), np.cos(a[2]), 0], [0, 0, 1]])
+ return Rx @ Ry @ Rz
+
+
+# Angles d'Euler (phi, theta, psi) selon axes (x, y, z) : z en premier
+def quat2eul(q):
+ phi = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2)
+ theta = -np.arcsin(2 * (q[1] * q[3] - q[0] * q[2]))
+ psi = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2)
+ return np.array([phi, theta, psi])
+
+def dcm2eul(R):
+ phi = np.arctan2(R[1, 2], R[2, 2])
+ theta = -np.arcsin(R[0, 2])
+ psi = np.arctan2(R[0, 1], R[0, 0])
+ return np.array([phi, theta, psi])
diff --git a/mavion/__pycache__/mavion.cpython-311.pyc b/mavion/__pycache__/mavion.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..24a792234a4e25a5fbd16460d415c7793ccd4580
GIT binary patch
literal 11514
zcmcIqYit`=cAgg>dXN-JQ4dSj%a$$1wk#>OV=IXrS#Qght%sj+Lem<GqNA6Gv}`HE
zjj?bWGEfmZRca<sy7bmbXhdGnXbLDr+Njw4$O2n{1I!O$02eOWbm4yryjukM)1EU!
z4mp$*Cq?&?JaZrCo_p`P=broBJN(pS$|oRf`^Wzt{p}Wl_zz5^1Cxq8Jq3|F1WT~w
zIB|i5CpAtDW11ePCzuNiiE(CJH=)0vm*eEP;evr8x(QZyi(vJiPz3QA{FHFP$dW^Z
z)9?h7{Z3LMp97*F^-qn@c<z%@o*Z%cW~N6{HRMOS{?k_=d57@GJ-I-!1uS)oyg;+`
zE#d;hGC+0M>IFTk2WnsqK#i;sXdasfG@s3f)(a*53HOX|YVwDI3HjML>z)`NNjHKm
z8sSI7@9Dqku)zR{nM&YEmedX^NCl8L6|Sgqcq*XA3F3;%vqa_8r+FAcd0GI$6Rx!S
z2`FJSCD0!wK#5LMqR%SPYf21RB?e6ij#CaG&8R8K%PPs!l*mSdrBEkdQ&Nysr$AFu
zm@Wxaq+_5Fb2<)^3N!qlfVMwVH4fUaWbv6Yd{9!9RbtkZShGqjni5-9Ns*?+o-Wb!
zvN)X!y|iZd*Y(no=G(Nk?MG^tq-#Sf_6#44u{2$>sodfWe{Qd?s53kVmIKnto+-`o
zGo-1koA#|lWg(u~H*i8q*i>5S&yc2C8e0bAu1w38Q(M(kIZv$9QB}HB(~pd`5?54d
zP&*@QOF$g~Sa)?=ilX^UojEA*(z)7j)uj0~t*X}NCD@8zlWZkh32mx9EFjpQ(Q2wE
z8JmC=>!r04vqzNTSAD@8@UxX(dV}8ArhCtB)%0K08uY*F*Q9pTDVwZI*VWYAmd=G9
z)nvuhmac41=XxpNQS%|LO7v1*O4}ybLVda(OTcze-7k?G?n1|>+^mO_nEtjCof6qC
z(StoFCFVri(2%5S?>RZxAsNs1_ID4RYU`H@&UOyFhE5N5wY7Ij1s#LuGl^4!Cr`Ns
z+d6tr4@rjJ{?5VS-Zse~=k}dSM@~rQRFO+AIhT{}mu#KKJI|c#cb(|$>gsLp?d)$q
zujX}S<>^lK^tvWNuIp5@tG!!}w4@?!>6|vq@qYLLhA)n!BVRqi{mDsl_{brDP;~V8
z2o&Q?WQcQz#7s?lCM5>9O^F!?wQ=a+ITI3`a1K2-ht8QpKg^-a<<PrwXwDpZN{O1B
zmKfIO_HyX@IP`KHIwua7IEM>K(z!X#J<H)j=5WPvxYRhbeGV5FSAztnU839BCLps-
z62%Tm)U;31qgqC1l;=eHd0o<79-kU{Ln4PI8aSMc#P~hqqY^Xf8}~?he}MD2C!~Cq
zo0{~vSa-nf$E~;q!Ve8&trt&mQ&&6z|HY2Mll{GI7kzsGKwMNnMC#EvJu4YqF5jdt
z;Bw7b6rj<V&cK!Ae+$%olXzrxMA&Hi?dpZ<kp7XQeBpd_<^j9(M$*wLI9f$VYbgJ5
zaaD9iEZz}f=JQsq714HJW#;v(_N`HmXx|QT<7zgGqdH~~9SxA5zgkuoYZc2HA#Pf&
zsE<vE75hSrXe`@2@u;+X;d*o~?i8!{38nkR(*01m@NsEv%qf=c#`tP>ys9DTJS0}N
zqST62P3qga?e`kut;qw	e(t-Epz*IC51#+9ZE9tK)yt6J=W#Zp8BAhs2uwLg@jq
z^uTiQms^C=cI*On^3PDXrYDL^LB4U-ULrqiCDG!=ErM+uqzvDdlr4CpfyEo~;-#`g
z+dolX>K^v4Fkc&meLcylV?s%vSkecK1;{uS%frvA$s8dg!wY94hhpT%NAGps&5L&|
z^$0cl1pEGF=F5D+el%Iwj%>_=A-a9BK`5$=4c~iBC~8TxB?}L%>8P56-w{;lp+6!)
z@oPq+wDP0Ni{982euSz6V$}hm<bd3?-LY^m$}YYU-?#MQ(#y%Mhn73O?E9wp_Z7((
zPbVwS2==q0{VZ@AL-2VDc1UOtFXBo>s882`+ySI;1)fR>qIV_L6wOnJf#+ZsLH`}V
znyHY%xZv%njl6x3(eP(*ZqClY)AHW%e{V;pX-CE0Jj3g9cLdl2=zb2j3@7~r$C+`3
zPF<E`+KEL@AmB56-btsP+lL8?y&+L=Ov7)2Yrzb%-;Y=vokmR!cM!AmfidoyKQL!X
z`2!`1MSc#*O=8t#oxd2OQjf=l=7=paaN7|+_K|I|Eat?IP`+C%-z^mG4w2s$n&-PC
zN5g&debG9hurkWUnm@QE6mI{RyLTgzhaX{QtGKgOC~OUpk1W>t8&PYtEvgqR)uN?3
zX{_d2q1h4jU;qj1vTOhgFey0(V9*$V5mLBg$RU^qDOJ6}d`PL28Z3a6Y97HtNU4XE
zpb1iH`$03LR9F(UKuQH2!6HbhhmN2XQtCk?XbY%-79baHKyu)VG6ov72ULg#5ID!s
zg2j9>7_@#4T9|{L2OWGNZ$gL`ED2=LCUGTm2(j>H-jWM}f~6Yn%)!LLJ5Vm4&zFKx
zLrga8bR5&rmKLG}%UFspkoihvS|(G2Ol4^-y+OHAE-#f^Q(A&C0cW9{Z%6RPD>4|l
zA5a=B-%ygwr5R7=IHlulN<XxRSd_S-tgK!=Ln6IiBI;(N5j;Z@v$3S$7QUR{BF}Bb
zh8ixFM`n~YxVfGct~{CR$F-d;FQfCy4fT}8yXokxKTGrL`uPmGKTZEXy`P)S!dAYL
z-zv{Sm8M6TgLwsCwSkjn6esPKS2*$bD;bhH?OY>daC?z7A(26%3_vr8GlN1AduAa@
z*{AP;Jjr<Vnmf?Kj!bY3m}LA{xqze_;imk4+37p06?e}dpG41i#wD7Y3Mek0Vm%Uh
zebb&u%+PgD04U4Ng3Pfgj+LnEegLQ3)U<0*(hrY$0)RV}=cq&tO-p2l#LW06M<sGd
zB9BTmFi1?-WtT!d64iAX4kvOND5OtZ_9H5mcb(0NvL7*S4>vVeklKwf(FRrh|0N?+
z07JHtg^G}V)#}K3YzgTwvn)izL$ub0n5^eQs&p&tV@fGX5UmZeB>4&G&RjHqI&vmD
zaQnPqstQq%V{$}>!~OI9Are|HvPRmYwYR&&!4LyUlQpt0+!rE|%>sMgVAK(#A}3+1
z6AQP?*|52J0Z8w=?TgjNo8v?G_ut){Xif}0+%MD}5=ssU=2p?%8lnL_6%|L?!#AR2
zcrLmv{EHauhC5;b!LlnplGt|Nn{eOvEz|cWmPdugj+FtSv1`RIG#(Qy$G#a#4xIko
zx#XF1zk4<Ls#`eWPF@}nPK*eakr4CPVv9T<z8<+6o{ieW!RU-&ssH%$y|Fl#*l~Y$
zdAqRVh`8g3U^%jCb3{gN?}WiQ0ru~S?GS7YajRf^?m^4a!Nk$T%){Pgzp$%a+|@4F
z+P|a8q7L#g)M0NoL|bm}g6`UO$oo5$IPfrUxmnoxvbghQ!S?cZI;!ZftSz%G(h+_&
zM6DkyDOhTuMN31xSg`C#(24ekhGi1OY<p;0A%&*4l^sG;&o@?~>DV{V3r+okr9U|^
zoIHJA7&xE2a8Vezm~>qd1}-JtUSYtS9P<eSKEdK!Km4nLr6yJ)Sav?3mW&BoqU&M#
za);2+CN{JQmbP3S)B?heb%74z&4O+BgTbY9iOb6+4}B|kVNa*Hr&F+Xq9VF9iXd<A
zm6i9v257u4aps|Oc|dR;5uHZ_+Ywa8E2Ksn-^%#M79ETlK?|a#HfgL?@DJ=+YC!ZM
z;~yg+mK^-U0OrU5U>&3~_(u<^4E{l+q)H7UQp(^TL`oU_gGecZe-J5U@J}HEY?k^9
zDg!_OFF_7VYx2yH2N%SeJWBxGUMldW9Ap7lDyCwoG#5U3D*HNFTk9f>HfRmnexz>J
zh4Th=?SQg~n2Opo?V1ox@m6R*`)Zp01T^)Tsx7$(j$p|%q}V|Unb8@)k(AP48MJ1{
z{Rfa4(o%UZD%b2qDw1U3jRMc)qCHm6>tsAtl)1!~_vUgLS%Fl*8YO%gO4i8DpvXMl
zk%Lk;=LPJzo;O&*+jvX%tZC;^9$NvRKSZ`*g_?e3jSpmGYu>_U&1J#5i%OJ)5`&e1
zn{!*ws`+PbUBwWZQDwAi;H~R5wh6D1x3A|7R`GyBWXq`5oH0~nA5$?gOq?2cz1IHu
z>P=d;@Os{Y){@gU&KBebYj|_8mNf?IRnY`*(#TTRIji%Ma3aaM=**D|^A@bzxISOY
z*YR8U8ZV>mI~<Jj`k$b4dxD<o2|A7^XgSIO`6Z0iA=w7RS*%?Bp?;Jry$vqvGM9T?
z2l5?7;zA+=F+5z$fQsvan4}-~O}fXu678Fukqn5tCa#T31tU|FBkq7_5`NBY3MeCQ
zNaQuH539@fk3Y2&-B_asNiULNAQCm)EK&XN8*YIo{JJ210)AZ(KheUyicAbU!v-J;
zXe{8q)+|#j_HwT)#T*nf?6gd;vH?G;XmeDimMHLyW@jho@>A#x;@6?F|6TCHH;KoE
zMe|)Da@As=p9S;<Ky7!(06@uJijh?Mv67umm2ZWMgOGVO^;A%nHPRfh&(DAivjuLd
z>LSd%7hoMEs7OmhH$N7lHQ4O2z4XqD@4Xnk8nZ421p9WlfD9W$w$R|}j};tx?@)9&
z_I#X<4<@XM=Ff{0R|Wea(S9gX_ZcjYO_oSq_-up=UyL4=A!Ft~<7zZdu+)i`y4XN0
zaQ95yy)<(7VxsP@OR&IgCydivgmvHN;Lgwxw+W@Y;U-fs?S8;5-B>oP*o3_u;@%Fy
z)DfZ{Tda|xsCD5?)Ey%i+1szhez9y8c6JHH-Tzwm`-bH3>Ezi9$yYCmXJ1QR8cDJq
z@zQA0HJTjq3dhF8V`D<`m|z(LZT^4B0ptr<?xU8)gR$=Tpit2yRx}BwrhjPp{NQr;
z%AoLkkNA9#VCsRcD;f$>sF>MzgYN{RX90wYmioBu!R4i~gdZxjf6*s2_K1x=g0n}k
z^vc>PvSWX3@1Knhf828KaH1{o{4$+*PT2Ocxb0=ZaaeR57A$})5kUPS8i)<Y_a%yx
zaOG%uLA1P(G`^sqD6mU4fR|BF)PF(*P}F)HLj_c3xUo@DRV-VI$^koC4!+TPs32TM
zX1^eZ>qS)}NLO*0c4tJX8I+*)lR=$E0(fF2@U(n&nDg2WJfyy#y9_Mc2ofBG;u6t0
z!tJ5#);tiGn8_(_LNd6yk$`)$MLAL`H;9}MYoQmCsF^YN%}Ml#dwSg^BICJw&8HsP
zF!3qW@nh?7ftbCDAHiAa{JXEe^Ln&BM#gGm^deYFy=bb(8^)rDb^Z((3mn9*MD5Xm
z#gdpkHgK;*FgJ+ih7heRFM!K2d(;F+B+;@XY22YqwW?5P6IFxhr+*9NZQ^ac_J0zY
zsfGVbz@%i}=)a@q$*`WKP(?fiF&bkbBBURM$-HF@(wZqy4Fm3!b-8!SFyRKMoC-Ke
zxzow(lu2TNN1mxCc(XW?WhU~!J#aI9dJ1yirx5&KgpmcwAeGY>wbsTxgm?EO!b0tM
zSxUpL>Tome+-*2)P!<7dNh3+^4Nn%pQiE$uDz|Qs3F~935rY6VgbloQvEb5`1Dj*y
zpFod4Q+plM@w)6K2dV7;q3E*V<0uWfHz?OO%Gz*43(9)A1+gc1B!GK#d6nSi4~9SG
zmuSCll6w=Ax;eO-@H>qXIm;b}4DOhm8^G%l?ur~|ue#WPi~{6!<i}H#vYp<%nY!#4
zy*4>ASDwCl=X?{5FTqPd_qhLXBNqKV)bRfekndp`Rc;NR2pxqzu=A0lbm81b+~Q2k
z6K}ig{ot*|w-O|{-vi6_E94hDA0GMq$VywXs!MQmi;nJ4TgV+DLnGm?M@6;;W3(l@
z4Q}OIVqCKPIiYBmShOqDwpv_vr{}$%=m5MCYPok1q`r9XMWJ}FSiCpXxoR(i?dM?h
zbgX&tT<ltW@ZN0F-jpnCl2;zQnUD=wb}&esjH0zL#ARo~;Ve51iYDH|+?z=L3W!9_
zHgWG@3O$V<7tAGDMX#h36aby5js+9HK|a*)_n?zU@V22RG8jHSe;k2_HAIGL!?@8Y
zku^q_*Stn-LjGjN8=rHLH}E5v>O@l=_?|*rWM9+<Z!W~bN|ad?GAa&6H9}|``-x`v
zbQB0U80u~M4!Q{{M32Hk14p6p)nJGe&y3>BDcCcXbf18%&s2FqJ+IHWq|Es}+gxR1
zUH4W@_6+!9=Y42COvNC!K7dD~!;E&N3=#Lq{`;hC$xfX_1|-t2Ob=>SMyr18Uro);
zl&St;d}-W0ahY`=p4*x-SAW1WoiW#R@mEmIkFU&OdZHb_>iMuIHV`M_5~XsFSh?pW
zAuCf}aS}7ww{QO+-You2_rK5GI&z<q^b?-I*c2-nC)_t&DE`U+K!Df$c-mIX1V@`T
zB{Ug%smUFNw_O3nrfB%9Udog%p*5vU2?eUAgo%HK`u=tx-=i(9TJ|Kh-w^Z2TpDEn
zSJp+nv8#!amF-D$w_xrT&E0@UEw=e<^VcKX{F_PRmLIW^qtF7_2>mt#HbRG(5V5h1
zpqs>dlqF`peZc(d`Ze>9{z;zyX^6=4&%_|MKJX+3w!woJh9`|r?LMTnjGw|YL~wr%
z?Qjb~07$%#adc9WZgeETjl<)*Z<3YmDaVSqJHR$qnKqzocbB%GuYtpV2uQa5=;FG#
z8&G;@qI0=+dEjB+=Y2xMD`LYdEA=b&-;m!l|I4ngcL|mKVrBo$l!Zk>1or`w50RiX
zr|bq^dAj`5?n#Nd(Zu~77T!aG?uh#pkhB*<+fhshy#9oG=z;iek?R1El;QLz;Z8E^
zn>L)$*yZ&`6Uvuu1^!~n<$(xi0sbjt(mmmExukrTYhsGMHjeQ^mkZ97<4OsK8p%47
z;uggH5=jI}49TZJ?x#kN?{7KWZ)Ae*%8v@Useaa&UAlAR8nKSPP8xJIYlKQ{`*d|{
zgi6NAM%}?RLM4|-oo*+%*fcSlb=EaPB{j6JVvSHqKCQE3GM6Y$0sHIBlXR2UCZ=cQ
zJw!6Xdkl8~{tzpf{Ug5V*~Y2qfN#PFdoKgbl10h%O$Iz(4i4Rlqo{`>W$>S2tQ83=
z1bmpH-NRf0l890Ke+gubCP{LYuqD&qRiY%B{;m?WN$q!)s84FYt3<&~`Lo8nMUgPL
Pjp=uC{vS6fmHYX>Y@T>#
literal 0
HcmV?d00001
diff --git a/mavion/__pycache__/model.cpython-311.pyc b/mavion/__pycache__/model.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..7c5cbf6256b5b0c3ecaae610c25a66df56fb2735
GIT binary patch
literal 13223
zcmcIrYit|WmF94U6!jn}l6pVvk^B;)hb6_aW#wVZdRvw(Q?ea9PH37#QFQo_ha*{1
zhOK|34b>I4a#n%a!ro;HH!CB~LIF}hU35_u=nn_z0-W6eR+xnsUTlNH{jpHiEl~8&
z?m08!kV8p!vuLl(GxvG!x#ymH?>Fa;|K4ma&_THO-T#>=snhBH3lr%;ZAG4qLgbE)
z(J@3&cbSl%`pbHF>Vss6x=ayRMg|Qb<7J~9CxUsG^YprY9YejZV+^0@b-GXCr-aKU
zh8WX1jZZN-;v^*FL^Kkd@jW1<d^zF{%uIQ=D$9>_(`P3kc}M4yg#ZBx8U6djWs)J^
z*IlNttIGz)0My7Bf#xxJKuwGZXg-q<w16pqj*BGYuxBO^3IDM$BtKi`JfWaB-3j(+
zf*%RLXa8Zq4mkodmB14Wp&d_5&tcuIa9!oY>p3;9(_L5VJk;m3CwUk`WtxGG*SXW0
z*Fg!TDFJzu03`-Zi7~6hs42<ID#_E7;5g*~+)SF1{H&6EO^IwTSPE?lG$n;uZ3;Cd
zMd=cz2u5etq%3O6%<HbJ63xR%p{_Mu%7JyJ!*yFa1}#)>5YO<kaCEv3=-!@=D_k>e
z=Apn(eggCRsXE8dt0P@s(`!jO*RNx&KO&f7__Jwb1`R-3l^J_uzJVlI-%kQpJ8)5F
zV15U#*<+P^DfW{)jH5IyG2_sTMAaXRq~u40cKnKt+-2#unwI71T#!K362#S>{QB$^
z#8p0iy<e~G2{c`y@*|m2jm@YDP5-LoV0~q372P(zRcXGO*6IvG>+;=OZ?nSc7jmok
z2Ha|1$gT1laI1YGx2kWzZO;q2RqxKtz4wK!Yo61ZPPc~Hsm;^_D^b@zl<d)z)MvHX
zt0~!+RYGe@_NPl29UKRBBND;lDRd^{VSKDajdTt7N~CMxtVHxn)Nt3>m}KZ4IP2<>
zOcw`7`o~7QMx?@vz2ok&^RB+G?p~>|$8{-_7<HW;b-TKH2G5WAfA|9oKRuC-{QN1N
z2u_m4M-F-VzYw0{F7z8kz9%Q3BCd}Nkqn~)gYK}DH#pMk8XxSE@<!dGZSL-VInurr
z=}PBx$vO7kGrbqij<|<=`}zjE2YW}lFG=aVzO1}FS@NM#;DtjN8kQ_u-MHn9H*?Y>
z&I1w^nev4t3J*|;3W5SzJOo(W?^)cKS=^CX+z{DfB)EK8+>Ke>JXl<h64}kP0-0--
z^o&c=ho>Zp33&XHekvdtal8{VlEK5Wo;f8Mn%R2ZlnmE`5%0Sa3AI?9ViuP@i>sBz
zMaJUVV9}&mv`m&pg0{q>*|2CKEE<E8lBlRJI3ZCJfuK(^Mmg5!2}uTqjf8zsJd9}w
ze>^nVd1aK1T=#L&D?P5WBZFO60?hy+t|&0#N+`nkf-O^XlF983gae%0J#STzMoT&a
zH}&W(nEhM2$F|ZXCeeL+@ABTb@o{m<a(?3AXC)7-Q^l_c#jl9Pufz>cN-7fvmxtrj
zV*ZAsB2l{h8bnPSwz8$@aur048(9@fs}jeS-+|nMjf&dDb+N(;aq~tsovanB563Cd
zRIz*FiK9$rz4fq{B}(pA3HE)EnfFy$#j-!a-JM-2d04U9^-ubr89pCer+#S?jtr#s
zoEFN4#Ihl%Sojz!PHq&H#s}kr8)nNAvDBWR?iSoWksQB&QE)V^5<fZhMepbNYdt?3
z5L$Z#M{laAPpR0mJS-H~C427o3&o8q%)?2c_~;suD(c+KBk9+^){*6}{|*UuvuV<m
z*W9~y*Po2wpRnhsxaX))b`;CLa+EH2B$&JJt{i#zYO40w+Wwz4u5-V9FLit@RWmL)
z&Wn!oP|viH(biYxmCH91)5%NX-orw9t61I&6^edu)Kye1&n6AYfJnCr<!xel+iK4j
zr-kx2u=p`F`U@0(Rlom!!^$hEwqCKZPpIz~>-%xy_CDSvf1|u6aY-ycgd%C;d-kRF
z9})Mwf~~7JG^v!S8vZ>4L)2+7FCt6CsLwWm+(8@zPbI|gOjq$yjO6u*g69FNAy(k9
z<yOd`TtF83osdP0($vSrB?mqSh47?|L;nB$7&QH;=$of_LoN=1Q-JjRJBU(%>m0-Q
zfb0C&IL%G6(@~C|h_GH?l=g%fIur@{!dw)du+{@%zuJPE3`FUP>9CiBHw)Su2~ACN
z(1x4z(Niq!lEI*lP4l}pM~8f#aFmWr(9u9>D(F+$U|sAlG#&IrkxeKP4$je@si|PV
z$IyW=ePbGS*$5Y5>0p5Ku^w)k^|chJ3<gI=&ySCFs%aceNUjczTIf!inZ>N^08CvC
zZL@7oO`}Gd_1%~bu+S^+jdXO%=Z$Ll^34$Ftz#pbqY~elZ+4^M=QUELIeUJ5`<P%O
zmKEjo!I;MBh7l*-a4o`ss%8UGCyknNHN2cVD7lp~hn@5Yv<hjPWm>7d&-j9!^o(yZ
z;0^lV2?tJqn?v>^y*+YgbVKMc^!OHZM!S5T;M;0b(0Hb)C+vqgWi>2Rf$0X;!|cKZ
zCLGw{yu);d(Yibw7vQEDAN|%5wNJIvk+b8yZJl&D!UaG#+@y!g%)bYX^H3Cw&L5^3
zADrC`4g7Ax;3i>~72Cb(qdlyTW~QNaL}iOJ<&8{-L55}+7jO#uGnO5FRjr8qw|COM
zpl=3P%RPA`;Tc#pC^{^XsYo=y1!iDf1iYMgvUz$6M41RGy+>O#j9%`f{c~(2>Wx5U
z+BY!)wO}EFc=EErwWzELTxVX7yvWjvur9MGSl<M$z6kwR+uOLhV4<?mf`w_swI-`2
zYYn-2`+8k~lX`R35w<=98$u-Nq0h9qTAJt{--Kt5?t|Dc2s!EFI1hcgrFj@K>c>H(
z(aAtFbQEFd)M23_VL7uA59l03ng|B`lN{(a%(0Oms6tj5><pYI5NrljfyUGJ7p>Q@
zIZ01)+*Ie`!$C!;<{<1UusO8&1Ki~FwU$66O9l9b27nS@;4;Y5X~@DujaYOT2%sS_
z5T}tnf(bn{E9u{zf?r4?#wB7TimnD622HaaSi`97lk?^+w4)@kadZyIE!}1hp|jW*
zuf+8mW*a^>jXJY+$-Z>%cIm?Dd-l5(NhkgZl}%!0lTg$YC*S}!TNnG6PAv>A4khY^
zqM8JoZ2Q5qP_+NY?ETr*eEbs*b&7{Ng`&<l@s-uKIGeB~x)Mggx>vOBO_}zxozS~i
zb$>uYb?p2WcKe(|E>CmFnIMHIT@JB)NU29itN>E#!4)fnlzIflz`=1{b;ZQcrJ?p8
zvp`C9aKx;TQeX08#gI~6D=~B{Ad*&kw{z-iGB`6b4v*r@Asln?M8^FQE8$DvkT%W(
z$9a%6R>~LgW^{JM%DBwCP?oELxAGR=n(IP{m1}AvgqMRx9`%(xsk&Lx_3+7;YG2M3
znfZcm#G@SG0E}ZNP6!ilWZJS=1*7K+We8Cw(+Zj9$+Q%CG2{;ACb_&E%atBcgHW?b
z&UeVJ7=;Igb+9y6$ye^kORlHUU&di#;O&YeT34XV6UMS5zbx5az?IyU>$Y*cfD^Sd
zrV+AWvIWRkVFyN;Dw`Esr2YdV`>M&DI@)sFhfs=e!|Y~NVx+oXK~?>|lVf4Z!@
z%|Mk#j*MfglCRnU-cp`1!0+ne(belZOM84Q;H9u91Sjhb2v>oy^3g~)0NnA=H}S~d
zLJ4&lu=pJ0`+_cjtBQbB*&uT!;Jb;~ICFX;-qH%tLI*g=^#%b40j2;nc+(S|OhW{f
zn}TAtfVUP~{8>N>i^r-Teh{AgT=vvBOW77kv8_njkjSnH*&UK`ec)cIxI=L6viuYB
zDclTslAVjV6{r;ny8)!IfmyZ*3#jM~mXnO|{^1GwB`ORMMlyH-CPn4r+gYs~+!R7a
zi3H3lk-&#NiEZ_aPa<yaDxpM;-Slxl8FmhMPDWTp(%+1N#|1R(c1cD+H5~Xxl;?z`
zADfbh9*LR(Bqb4J5^+i*p@O9EyC&&}uSL-pBO`^KiGm*}w3GS5E%bm14>XJZx9lg`
zEYaD^maF5&4O?l>qbhE|%!)V(57AZ^r?Q^lA(2aK0dy#($U(F<$eiRSU<^y~;`yZu
ziF3Cv3FbWtF0qM4`{D#3lw#XbccSig|3WNIAz-mB9a$L4AQM-jG^t-Y3xGr{+An7V
z1o3}#Z8>l!{9!mbE|k3_mc6vnC6qO<8o+Ck|8vVfTGnqoa{OXi=ol6|h9BJ!I!1-o
zQK4*9u$&Vu=i(&zHZ1m~eG6|cc@{1w$c1+j6M}g^@G?7Bx>v~u{i|IM23NfghS&Op
zrc>)}LQ~KBn9wvJm<Jxc{D}LqBQ<{E%U4sE+`^DMb=4ybc?7d3u73hxYu}Rh_8}OK
z6MRn1$%BHu;WIn<bLv+weC}Ku7o4X==PAK{N-&?=u$OMDyJ8dUFMZbjuw(Vq>dfbZ
zYf+)GTWstW?A>3Jgr$dg0&SSv4T<*KjY*?mKPY1~{pwNh_OuCyUKbC&F4$lH+Mu_*
z0ZJ~)9VAG(7n+$5CON^}xZ+*i_rSmEc@S75AB5ICLd%JDqtMc~enV&(c;paT&Isl+
zkD{rui(k&B-n{(fd#ShI`SRN-w@*0hOHKHNvwp$s-)Ycw;L%F<X$QgyjXh#xk6`Z+
z%ssgyR~xLf3HGMXTo2z|y|z~Nd0^ckH1~?ly@I_Lhu^0eK5_eSu6oZ2&J&{ZgkV2`
zDmY2R^}jdiitPYc?Fm=HBv|W2YhB7zrvO#}7HUud>q`K0asVp@ATi^WF+eH<SdEa%
z09J&yD%Ti7+YDeuXqy472yHWf6`^eguoiJDfB_h)f>rbdG5YW5Gq^GbikYE?o*}bq
z02C$|xZBj^S$T67=mkKWRPPN{eL#fQXWx8k`?>-eh}mLrf3|Deth>=2+ByI_>yj$O
z*7R#eh|Jrd|Lj{-@)OYBr>X{J2})ySFW}+;E<{FS{7ziTV-?Vw1Hmm{D${Zq!dG(Y
zolo{WFmU<JQ@IeHG4ci(wiaL4+)MJ6X|Nfq;>-97<gDSVf_*+8AUx<bbA$h!x&?gs
z_PViZ-p*UIXH7eY^4JOiWaH}!Sf)k})y`maR*mMZOx9c$Zo9`tPBK8RftR7&-m_Z%
zg?q2%YxvsdXe^JnZP%Da1DTmH@s90vV|(~&eh>B#+smnUi(nZqdNDI<?VsPfORrYm
z$Xn4`a{9*Eg4`I*TVi#Li8t@OKWMnBI|yU&6Y#>Bb0e&kNS;BM#aR8$Joq}kp0DC*
zKc#I4{@nbLr?~w+#l7q)n$S}jf8hqHxIhrMDR8YM>yoMpb9cHw+J^=Efk;IeM_j-w
z<1Mxa3r`_=8;OkIPGW2TNgoi241{N-LU8hU!HXA$pL3rA(ui4!m}ZBuV&<;w#bU67
z*y1#jAtV=oNc!<MNk0O=@pgE^uM6VC@auy3a69`JR-%|0hJ&O>qBzfVn@q9T&t6fA
zStzEMDVg42xF`;NXY{7YRiGRRZxr(dTSyM#ccF3g9;p46?nzPcVqcutusRm!0I30}
zYl`Oqu5y%PWGnqd$xf##Yr!8Bw`^=Z0k9R<z)_j3OLiqa$?p3H1*=oE!cEtP#R~UL
z^-I*EAJ8Bq^h@nahQ&#MREXSUNcY51e&^K>UrpRd+U|0KW54LwzhH{n<F1Y86&(NY
zcw#*H@(Q`)TD7gVeNnP{LvS1y9mg|mUr+_nSN+1pC3fLT0`L{U%*U#;>P8}8u-1##
z`sBGJ_wj`l&qMFWS61skb_>=cGU~JxW805dxNgc@u?yu*VtJEbZu*RUIJ=g&ZWj*s
zh=&on0N%0Mmc|mc<qHW<lDNy<emnWCHH&blPblgC=lWkYq{h#uE?!Q(bw#}RcIv7(
z#rVXl6DjvZYRoU3o)k|{3MG?*brMwh=i&o^xo6?6d+m2SlKm?#p}JM9ZWYX}KWYD>
zW37MPCA>T!zKqCKfod>HMKN>Vk9{wexF}c;h}Huu_Rp?8oLr4UgYKUW0sa$P1_bAT
zU>%gTQ|v%_?MuwPD`M^8pXk4^tlQSf^|7^iq4l)bdRnL*5^IOxic+wiiBsq!{8oZX
zj;|b9ElI(pq4k((J(e;ZQ*ak<4(WjS2se;tzXk%hYa7OaufEcC;hDi<$!soa4LI*|
z(2jP;!aKF>FTLQFOhtCU9VL;2_S{6S84RJ_onr<K2TJ8Y$~Ry+-#o#NX&hm%LM0Yo
zy%YzryhWnF7`(*to`yRa+Ydy_^RQmd6K+@DHx)-QJAuWxB}w|3N%+l6q}MaG&09?2
zwUYX_hlyW7o9Hzlw{)6y{1`mIm%jhb_uff#Cka^d<XteA1ETo=`jLy5Y>OAbV&Dz#
zdcu)7cegC*NS?c2CRiFoOGBJg))?UHq$6P#tOrHw!IbHsGSjL)p-<F|Jkx&*kq>kq
z7`5N~Wo8z>`-3_8RGpXa8F^yC$mm~#cDx>9B*x;pxN!pJ@_kc`)Xact7jQFb$h{eb
z`BqUkx*qu)w@=K#8<aUhHOUk86n)2DRT@Yj2RvV&bwKV%dVE{B0waqNF+Iov1?i9(
z)6UkKElZ{@L%`QJL@vJ6>Lf<-)PhMt`>2hbIq^gxtSE3$>vK=;m|-C=soH8<UVJW?
zc!P!;u24BJxflY*{HZ#_F(VH%lUXH@%KoC#n2ky&NJYmuRjeDyRV257JS_wQHzHol
zknc!r6Ko{p_D#dRt{d+5g1)G-NM@miWS9phM%0-v5p(S8*yN0y3s*s$M>)Fvat3q5
z&2TahkXKI>=XC2-y>)BLIfD0D^OfnV6*drRc?~WhJ;CT3Em*VxO`^Af`~g-^P3^*P
z{1hC1y^kGxMaO}Ea~%5@$1!j|nAYpo&p+x~uN6AZiXCSK$EfHSjrVMnR>r&Ho+Tpg
zUFdsUY+p7d+7tWWrlmc}rYc{8k1@pJ#(398NyVLk4+j$GlEjbO?{}<pef;YESA~+p
zV#(onFMgV0gRd^mC)@76nVep6-JeT2T2n=>@>+9}PZ<y{>&l5z^z=UFq9$3KD`%df
zh4(OnM-m4j>E~M6?_dh|mM9wdRareJw{!zOv5>lzVB%k~-gO{<0ChYrvMdfPxfaeW
zo<ZDUixcs>1w7xB$R=gXZ`;)ELVjP2&Y?G#-o-z`TrZmIVYex=FC9tP;WkPvszIK`
zag(y8sOAWLqntRXXDBsnDf$n{JGe3E<KzS^A=nx;n=@|!Z#p%BA6391@zC%I@cLBc
zhX$Nkgqc_T?3W)f+q{u&UvtQNK|6@>5p=(xPeN>afIW;{pgsX{Gizo{_kb9AK(J`a
zPJ={n5)oCV2d!2Ht5KAXrf+Ro=^st61wEl_jOUH{+AT|sa=xjIrKXF&f@V=XQ($@$
zJ%2aw!-3?v6$0EVHO*p8^DUjMOxai@$~AQ4mEXKq@(=z0Isg8N2YSgE@^O<9Mlyvw
zvu<S1;<u%6r-`yCW^xiUwoFMrj#Poc@yRJRMZ!nsTc&gst!c}YkfCZynD{mDigp9}
z1KQGtwK=8z7pERu$`cg8$@+vpd1JL~eSgZ*FIf6TOFv*xt9@~L@#Ye{_+H9X^&A^H
z1wDX`kRMQBBV?S4>vpyg+%R!teMoJ$514<0cK&g@lIMR4BJ%uGNr-I^JfR2M;9)<8
zCy7rjj?h}hpHgQ`$Nn|+!+swKAcrN4<NhQWCcGRQga=$CFtR=6SP}anRGY6!8&LL6
zm$skZLXGHgAldd~STU@40G#)(_O8{fo%?*~iy@)mq}XtB{lNNxN5rGHUo`%*QK%Ua
zYev!*_BX(ST}JW|5<GIY>;`VL-O(veSklk7viGnMfq`r~><=K8-i6S16w@K$8y@wY
z2jWk$)=?l^hBK0~YW}4SXCirRyV1l8WLts%7r~<E%~^<F6T&5p&+V29+-~^p2u>V`
z7rEW=A{tamSky??8z?SZ_FG8)3ds+Vd<^8l*81we4BVw;f{`ey;g<ThNjVKiH(~m+
z$pxa(KyT_)a+x@8sNdA7WQJ%lbZqKWa+NR`4sGgGVzC-*n>v-$lZNU|ok|Kwg9DSf
zMA;EgMmSCl;px!SoN|D8AxpAEy@9E@mdF$r2nFEK#cfxzDwzTJIK&ShNcofv;vf_$
zgZ?qbI+36vVDD12`Ud*|l8C6H-vzQs5(KfKv!~MkHgt|u`rn4GE~Wjqp*xV${@c(M
Z-je?|DIZC|xOS#r%lW_CrBs&l{{Z9tS*id4
literal 0
HcmV?d00001
diff --git a/mavion/__pycache__/rotation.cpython-311.pyc b/mavion/__pycache__/rotation.cpython-311.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..749ea808773963bcc6e585f5880ffabfbb892714
GIT binary patch
literal 8598
zcmcf`TWk|o*4G|8wkJ+-2#*#TFnM80TO5*5XbE*np_DY>kfsf!k7*{Dupz-?2M984
zK6Y10<!X%-R7aairV8CeO(i3xQdVNMVMV1>qSB0HBxI>lrTt>RvSPI#{n*`e?u^HC
z{Rs4DCz<2>IQQOj&->o_lgs5a!Smx^|9$obl>UyD>|@pnkFP@E3zKLP$&l$Z5zW6f
zoi>XWfEJMh$ca{fR?!B~CYAv#6YT))q645qbOLmWJV0J72Usq;0J_8qfE8jTz)Eo)
zz;&V<pj)g0SS79pxLzcuOy258NJwWt(dfaSGI%ZUK8^$U!W1#|o!84EiJC<7Gk~E!
zyb0=-Ts>%E`zb!j_46i*VHiPYFkpxzXbD=rh84fjS3PB#=Q<^1-XjXIW8R!G4`0k!
z1xXUdyd*=egt2-1FT2hi=sPKmoF9mU`wn-V=sf1{8+_q{aH$X8!I9xUX(S><pzz|w
zv5e)M5RuAZ7;Ze=0btzp6Yq+>92XLvgny<ku^~y4o>@-bP(MfJJPX{Mkn*Jbi*>0D
za^t=w-%{XC^HTk?d)arddO0NTKPFedrSQko{BfB({ut&nF+8?G1Lbiy6iV?JIB4Jp
z%g=mn=kv+Sb$ZQ`6=s)kt$8!$8f}1fY*HIg*vPaCD$^t$>Y1{^;Y%MPN8C_D+;BaB
zEJp(I$ke+DA<4}IlcBll*~@bEOA5ay&F_)9J(3GBnXT&T^fmN=s*~ah#cq$?&Q;jQ
zHB>k-ir*QOj+rTIQh>$I#ny1Ht6+|KE5N?5nWH3XK5GWEuy(4f!#OFl9&u3eS|mIC
zWNd=e-#-%0aN&>?@mew@lp#Wf42Myo+qZ95ndl6S3Xvw^iZC3MQ18(hgx>}*ZhB~U
zeBK^A2D(M2#K{w}6N-JyxF1?4JQMzj+L-M|)zqehI}u2PX1bI9q%eDU`pon;#qCYI
zy^7rnxP1A9Fws1IypZ20bE;ZDL!q>4vHQP9KrT2^d!k^@=W!PL6yX<2#2m>Y!0&nF
z5Pgz{d9rOLQ`DTdqJ@zjY8ic^mMFMETv4=K1*v=uGWbT9?KLjSMa@y#LnXy^2Gwyt
zA5yg%7x$K_g0e;=p?`pCafWnfY(s;?!cZ{74Ub3{GG!peh%nsbwPr|HhD4;zFfv0V
zX%qa0aT*Y1R+tX4|AHpVTIi`nl#c*_P1v0=d)z(A$9NPW|9KEy#!T(?pi)tjuBcJ$
zHRFCna{Qs<vx;~~u|JcxKchA|<Gy$(;k!PXB-ekV*mtDuJ2cV8J(B|yFOQ!n6fdd~
z74Hu4a;3#v+H>T4YFg(Q&&;tV12xtUG1$;+@Zv!nYp6rbfL$<?!I&W;;8+&hFcYQI
zfX~hq<bqtDf{`djI51`1im6dQLheb_@0l~?qO=aGVMNKwx(p5X0(iehXhfw5{}F&D
z>7QK{6QNjl+#eUF4#!&(zC>WAIZ>bVB>l5>$qjOO<3l3`+h4r5)Y$CoYCnu!byywF
ze_F~MwttxbjQdyDnaW+`Z>mb7+XxVgTck?T00mY_qIsS#O0I(CLSQAGUP8AF@d1ZP
z8vzty%;=D`3wmm?w+aA;g{ru3@<8lBT$p@S<~9{7Ev%r@vl|L5J-L(ueGY5wr%aL;
z1^_|S?^?u>$UIT$q9I;&w04vinzZyHAS3BIU9MPb+!GH>z8Wh|7g{;xQ!^C4fb|)l
zglo_z2nO&KVbu%c2&k92)61z5;=k}ndjMYrs9@3y0E~f^lG;Bqd|vWF5AH3z1pq_@
zzWj43b|q1Xe@f-fbmdNk-zjrD*Wl%AFp}kEzHB47H|@#qL*JU?FOoyqS3okg2FM4b
zy?}z^3!|$*p|6koCtr!Z^5y2FXL`%b7KN{qxw<vzMc%OV=421a9>n)xUW}mprRJMe
z0UnswFGL20;l;FN)W}`OrGfrP6KaFB57v4KiADQGVqLMegg>!2NfN-=+B9D)bG3zb
zgK9?g;rCErX=Qksrwc}{VWTLkGf~3oH(P`J!z>cfWXy0>4%mPiNIuZ)s2J$>prI8T
z&`dMr5}SNbsX>A(9e~~@c*CgS=))>+lqJY@=aloyqqmOD2{$`tJ5nBHYjb*Qvtn;a
z+gsEK6qx9m=!&(BcdWsr&tOWHNjZTUS#n-?P3W^}PzTe>W%VF>HYwEAwm3eTUU5}H
zdj@+%F-k1}G8W9!EaAanP#}!!G{`ef%}FyfRcqP+12rd%jMcQ}%l4TgNnbK_vt|12
znYR;%=KSB87j3D~lJEBDZP%hp*?mxcwoTy=rTIfLcc@VR9WWQwsVZQ@uV+^-Fhf>^
z`4Gc8hRqDy)*c6XMhXKaGn?-Hm{y}G4Z*n>ZF!l#k-8_2ETGNIQ1N8i42eQ<+SZ`%
z31bUrvqVtF+7o4KaHe3boUMTe=3t=Ls_HDxBfW~?AObXaP-j%!j5#F1dlBBTFb>J(
zruKo+Ayt20hjujh@ZSJ{{yO;B<{M2@t=bVbG~IB$ZK~~7(`@UUXU@M+Hy4_3xY;(_
zmhz<px$0%bu|MtDKi*zqT+(;z<Jq%wfw|B^cgi#0b8~oh`02(ag>S1CHl|3*v&f|a
zw{7z^3pK{|Ab)7OaXBF+o34L4^{K))rujyhYb<m<dc6J*Y;di!Bx@yjnqc%Nr^VoU
zSc#x>VQ_U_%a5Q8mag4mUJRq>O`w*+Mgh{NU{0{`>;sfBwoYe)onV}RxM1m24?OjN
z@3pD=gQ}CUj*LbIq>Oz~92kxaM#fZK!pjW^8M+h9Sl~#*1`UVm3ZY$k6R_Li4SxV2
zo1R>P{BA5ZHhB<y;+x699GUK%=~SG%zU^A*N%`k{7kd9vb!W?R;LkO8YLpj_D$b)n
z?zrC|2fBXn-uFJ}>6K5PQF{8)J$=eseTuVh{Pl-?#rRu=9#fa{DlCp!8F(wRF)G4o
zl|4P?WK>k1FoWS(0q05x5OCI~nQv*`5UoY{4;0b+1;cz)V=&#*o>FpSyhNTKN6)Uw
z)uzdHJxph~g`i^A+{7E$bp!!QmF8_AS~U^4EFFb5R<JbfY(lvnhdvw_9tA)zBC=Nw
zH0;wHlq7tUpTs^<_~(-Dq;IwwV&A4YANY!vo7=u?Up&4{Zg(zrD%;x?zI{2c;$HFH
zufF@<gOjJ^?st@vJ?WD@${Rfj-}B(~$8x`@oE}J@9#EVECB+RYtp!LhqEZ1;O~FMr
z4CKt1xr=1Q#u%P%rl1lAH_wg;TJqL;s!=>^;!j0e;SJ|CHWShno7shNZbK+#cd^DE
zN{+!>S>QDH6-$M2|0T!ft@l(k7TG^5;V19knoD%p{WBah?x5rd(Hvym%2SP6L+)#?
z1bnuA1$W#m)z{$qD{96gIb3o91V>-i3C4{_0QDjR8JoM(fG*mQ>qqaIZPmN7NimyX
z=($DLm5jM-4Bo4GxkVY~7U(!m!mQ{#!h1j@T51vBkAI2KtsS!sbM86cLiNqvv%9tQ
z*u5CJUA|Z@*Bo3X-?!g8vf^6_+;6_yaj#?f;DfI2bk}=w?`io=U%K~wrR#%q*9Y>U
z59O+l6o-&@2-zH@ed>sMgL1uNsw3&i<sUbPW`~mR%j@^1dhzf3s(Tw($cpDacX#W(
ztt+A83%vG(1=xM0h`he(yN?&oE(MlCce-y6Ee@sLXLllZosyfo<(?0<{~Y-bc~{&#
zxi8*T;z9(&1!~vh10bSOJ}>_p3-~O9^<mh-(3sPIHq2!c4-L=Y?cfZC0g#zZZNp&5
z&dIbJf}Yk6TVT_tJpY9XNFTZv1NDWpLW_Y$mI}HTpbdXnd*SgKTK5ug4Xu}paL+)$
z#ub(#125@{K)Hrzpll890X|Fj6SxOE%Hv)s{{ucDTI6e3;fD&T9*CINj=<ZXOutsZ
z0a!xQ2ZN_sg5~15!HhL>ZXhCLEEmIrc~-CH3t;-vyD;b!yy53mvv-!qcEtngc|y$&
zrk~fohlNt^g~&XwJiAxf(3}#M$fCI9za3mA%bt7OvT(OdX+65)S@GYmlTQYIXjEDQ
zkT1wR@20!o!^E)rjM91rvcfbbhCjI~W4&-S?oS9ahvD=cm~EauuAW&^?v!t_dj5H3
zYm2g}RdMZ0yY|WD`^Mk+>HkF={VaGYQJ-8dSJx|iLz-`pxrQ~;*na>6OW8Q9FRTsZ
z=>{8<b<2V=1}%PC>wr-PYk-<DR>9ncPcFY!CvEuUk;Wq$inOngSeEd+33MXu6eC-v
z4PDsDN!zg#KZ{E5Am{;*r=DdHkeX0Cd!Y~CumAvplBxl@&y$L?A?<9)sZ;gTWyQ5E
z?b;@nZ_^c~2>Kcl_Eu-C!=o22j;RMOnt$V0Qu>Kcs>M<xf?WvkKAPV2=5Foig`9*J
zP15TK@SsJravEW&z0qyWo2Ww38}481uU;4tM~4R5q&`4L4;Ow3;HrfX^3YTzXa83@
zLORH*$pCGH_^?3(b_-dzYSMwzN~%{)I;e6G*Q!Ye8wuHrV>JL%<yF1ds{!b($EpUP
zT8C8)*v!PeYSMv|kk=@kI<#?Q=c-8u>&&DQ@ihQsLrDit4k^*WI*v4ARRiUEDc6Q2
H%FzD-1mgI0
literal 0
HcmV?d00001
diff --git a/mavion/ctlfun.py b/mavion/ctlfun.py
deleted file mode 100644
index 80f61a7..0000000
--- a/mavion/ctlfun.py
+++ /dev/null
@@ -1,14 +0,0 @@
-
-import numpy as np
-
-def upulse(t, t0, tf):
- u = 0 if (t<t0 or t>=tf) else 1
- return u
-
-def ustep(t, t0) :
- u = 0 if t<t0 else 1
- return u
-
-def uramp(t, t0) :
- u = 0 if t<t0 else (t - t0)
- return u
diff --git a/mavion/main.py b/mavion/main.py
new file mode 100644
index 0000000..5942126
--- /dev/null
+++ b/mavion/main.py
@@ -0,0 +1,82 @@
+import numpy as np
+from rotation import *
+import matplotlib.pyplot as plt
+from mavion import Mavion
+
+def upulse(t, t0, tf):
+ u = 0 if (t<t0 or t>=tf) else 1
+ return u
+
+def ustep(t, t0) :
+ u = 0 if t<t0 else 1
+ return u
+
+def uramp(t, t0) :
+ u = 0 if t<t0 else (t - t0)
+ return u
+
+if __name__ == "__main__":
+ mavion = Mavion()
+
+ vh = 0
+ vz = 0
+ cap = 0
+ omega = 0
+
+ dx, de, theta = mavion.trim(vh, vz)
+ theta = (theta + np.pi) % (2*np.pi) - np.pi
+ print(dx, de, theta*57.3)
+
+ pos = np.array([0, 0, -10])
+ ang = np.array([0, theta, cap])
+ vel = np.array([vh * np.cos(cap), vh * np.sin(cap), vz])
+ rot = np.array([0, 0, 0])
+ quat = eul2quat(ang)
+
+ x0 = np.concatenate([pos, quat, vel, rot])
+ print(mavion.equi(x0))
+
+ def ctl(t, s, w):
+ ddx = (upulse(t, 5, 6) - upulse(t, 6, 7))*dx*0.1
+ qt = np.array([1/np.sqrt(2), 0, 1/np.sqrt(2), 0])
+ q = s[3:7]
+ dde = 1.0 * (q - qt)[2]
+ u = np.array([-dx-ddx, dx+ddx, de+dde, de+dde])
+ return u
+
+ def wnd(t, s):
+ w = np.zeros(3)
+ return w
+
+ sim = mavion.sim((0, 60), x0, ctl, wnd)
+
+ fig, axs = plt.subplots(2, 2)
+
+ ax = axs[0][0]
+ ax.plot(sim.t, sim.y[0:3].T, label=('x','y','z'))
+ ax.legend()
+ ax.grid()
+ ax.set_title(label='positions')
+
+ ax = axs[1][0]
+ quat = sim.y[3:7].T
+ ax.plot(sim.t, quat, label=('q0', 'q1', 'q2', 'q3'))
+ ax.set_title(label='quaternion')
+ # eul = np.apply_along_axis(quat2eul, 1, sim.y[3:7].T)*57.3
+ # ax.plot(sim.t, eul, label=('phi', 'theta', 'psi'))
+ # ax.set_title(label='euler angles')
+ ax.legend()
+ ax.grid()
+
+ ax = axs[0][1]
+ ax.plot(sim.t, sim.y[7:10].T, label=('u','v','w'))
+ ax.legend()
+ ax.grid()
+ ax.set_title(label='vitesses')
+
+ ax = axs[1][1]
+ ax.plot(sim.t, sim.y[10:13].T, label=('p','q','r'))
+ ax.legend()
+ ax.grid()
+ ax.set_title(label='rotations')
+ plt.show()
diff --git a/mavion/mavion.py b/mavion/mavion.py
index 992b760..11d35c0 100644
--- a/mavion/mavion.py
+++ b/mavion/mavion.py
@@ -1,191 +1,74 @@
-import matplotlib.pyplot as plt
import numpy as np
-import gymnasium as gym
-from gymnasium.envs.classic_control import utils
-import yaml
+from rotation import *
from scipy.optimize import fsolve
from scipy.integrate import solve_ivp
-from ctlfun import *
-
-
-deg2rad = 2 * np.pi / 360
-
-def hat(x):
- mat = np.array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
- return mat
-
-def quat2dcm(q):
- dcm = np.array(
- [
- [
- q[0] ** 2 + q[1] ** 2 - q[2] ** 2 - q[3] ** 2,
- 2 * (q[1] * q[2] + q[0] * q[3]),
- 2 * (q[1] * q[3] - q[0] * q[2]),
- ],
- [
- 2 * (q[1] * q[2] - q[0] * q[3]),
- q[0] ** 2 - q[1] ** 2 + q[2] ** 2 - q[3] ** 2,
- 2 * (q[2] * q[3] - q[0] * q[1]),
- ],
- [
- 2 * (q[1] * q[3] + q[0] * q[2]),
- 2 * (q[2] * q[3] - q[0] * q[1]),
- q[0] ** 2 - q[1] ** 2 - q[2] ** 2 + q[3] ** 2,
- ],
- ]
- )
- return dcm
-
-def eul2quat(a):
- c = np.cos(a/2)
- s = np.sin(a/2)
- q = np.array(
- [
- c[0] * c[1] * c[2] + s[0] * s[1] * s[2],
- -c[0] * s[1] * s[2] + c[0] * c[1] * s[2],
- c[0] * s[1] * c[2] + s[0] * c[1] * s[2],
- c[0] * c[1] * s[2] - s[0] * s[1] * c[2],
- ]
- )
- return q
-
-def quat2eul(q):
- phi = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]))
- theta = np.arcsin(2 * (q[0] * q[2] - q[1] * q[3]))
- psi = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[3] * q[3] + q[2] * q[2]))
- return np.array([phi, theta, psi])
-
-def eul2dcm(a):
- Rx = np.array([[1, 0, 0],
- [0, np.cos(a[0]), np.sin(a[0])],
- [0, -np.sin(a[0]), np.cos(a[0])]])
-
- Ry = np.array([[np.cos(a[1]), 0, -np.sin(a[1])],
- [0, 1, 0],
- [np.sin(a[1]), 0, np.cos(a[1])]])
-
- Rz = np.array([[np.cos(a[2]), np.sin(a[2]), 0],
- [-np.sin(a[2]), np.cos(a[2]), 0],
- [0, 0, 1]])
-
- return Rx @ Ry @ Rz
-
-s2q = lambda x : np.concatenate([x[0:3], eul2quat(x[3:6]), x[6:12]])
-q2s = lambda x : np.concatenate([x[0:3], quat2eul(x[3:7]), x[7:13]])
-
+import yaml
class Mavion:
def __init__(self, file="mavion.yaml"):
stream = open(file, "r")
- drone = yaml.load(stream, Loader=yaml.Loader)
-
- self.NAME = drone['NAME']
- self.RHO = drone['RHO']
- self.G = drone['G']
- self.MASS = drone['MASS']
- self.CHORD = drone['CHORD']
- self.WINGSPAN = drone['WINGSPAN']
- self.WET_SURFACE = drone['WET_SURFACE']
- self.DRY_SURFACE = drone['DRY_SURFACE']
- self.PROP_RADIUS = drone['PROP_RADIUS']
-
- self.Cd0 = 0.1
- self.Cy0 = 0.1
- self.dR = -0.1 * self.CHORD
- self.PHI_fv = np.diag([self.Cd0, self.Cy0, (2 * np.pi + self.Cd0)])
- self.PHI_mv = np.array(
- [
- [0, 0, 0],
- [0, 0, -1 / self.CHORD * self.dR * (2 * np.pi + self.Cd0)],
- [0, 1 / self.WINGSPAN * self.dR * self.Cy0, 0],
- ]
- )
- self.PHI_mw = 1 / 2 * np.diag([0.5, 0.5, 0.5])
- self.PHI = np.block([[self.PHI_fv, self.PHI_mv], [self.PHI_mv, self.PHI_mw]])
- self.PHI_n = drone["PHI_n"]
-
- # acording to back-of-the-envelope computations
- self.INERTIA = np.diag(drone["INERTIA"])
+ drone_data = yaml.load(stream, Loader=yaml.Loader)
+
+ self.NAME = drone_data['NAME']
+ self.G = drone_data['G']
+ self.RHO = drone_data['RHO']
+ self.MASS = drone_data['MASS']
+ self.CHORD = drone_data['CHORD']
+ self.WINGSPAN = drone_data['WINGSPAN']
+ self.WET_SURFACE = drone_data['WET_SURFACE']
+ self.DRY_SURFACE = drone_data['DRY_SURFACE']
+ self.PROP_RADIUS = drone_data['PROP_RADIUS']
+ self.INERTIA = np.diag(drone_data["INERTIA"])
+
+ self.PROP_KP = drone_data['PROP_KP']
+ self.PROP_KM = drone_data['PROP_KM']
+ self.INERTIA_PROP_X = drone_data['INERTIA_PROP_X']
+ self.INERTIA_PROP_N = drone_data['INERTIA_PROP_N']
+
+ self.ELEVON_MEFFICIENCY = np.array(drone_data['ELEVON_MEFFICIENCY'])
+ self.ELEVON_FEFFICIENCY = np.array(drone_data['ELEVON_FEFFICIENCY'])
+ self.PHI_n = drone_data["PHI_n"]
# geometric parameters
- self.P_P1_CG = np.array(drone["P_P1_CG"]) * self.CHORD
- self.P_P2_CG = np.array(drone["P_P2_CG"]) * self.CHORD
- self.P_A1_CG = np.array(drone["P_A1_CG"]) * self.CHORD
- self.P_A2_CG = np.array(drone["P_A2_CG"]) * self.CHORD
-
- self.ELEVON_MEFFICIENCY = np.array(drone['ELEVON_MEFFICIENCY'])
- self.ELEVON_FEFFICIENCY = np.array(drone['ELEVON_FEFFICIENCY'])
- self.PROP_KP = drone['PROP_KP']
- self.PROP_KM = drone['PROP_KM']
- self.INERTIA_PROP_X = drone['INERTIA_PROP_X']
- self.INERTIA_PROP_N = drone['INERTIA_PROP_N']
-
- self.pos = np.zeros(3)
- self.vel = np.zeros(3)
- self.ang = np.zeros(3)
- self.rot = np.zeros(3)
- self.quat = eul2quat(self.ang)
-
- def thrust(self, dx):
- """
- THRUST thrust forces and moments modeling
- this function computes the propeller thrust by means of simple
- prop formulas commonly applied in quadrotor literature.
+ self.P_P1_CG = np.array(drone_data["P_P1_CG"]) * self.CHORD
+ self.P_P2_CG = np.array(drone_data["P_P2_CG"]) * self.CHORD
+ self.P_A1_CG = np.array(drone_data["P_A1_CG"]) * self.CHORD
+ self.P_A2_CG = np.array(drone_data["P_A2_CG"]) * self.CHORD
- INPUTS:
- motor rot. : dx in R(1x1)
- + required self specs:
- |---- PROP_KP in R(1x1)
- |---- PROP_KM in R(1x1)
- OUTPUTS:
- Prop force : T (N) (body axis) in R(3x1)
- Prop moment: N (Nm) (body axis) in R(3x1)
+ self.Cd0 = 0.1
+ self.Cy0 = 0.1
+ self.dR = -0.1 * self.CHORD
- vl: vehicle velocity in NED axis (m/s) [3x1 Real]
- wb: vehicle angular velocity in body axis (rad/s) [3x1 Real]
- q: quaternion attitude (according to MATLAB convention) [4x1 Real]
+ self.PHI_fv = np.diag([self.Cd0, self.Cy0, (2 * np.pi + self.Cd0)])
- NOTE1: notice that motor rotation's sign depend on which section we are due to
- counter-rotating propellers;
- NOTE2: elevon sign convention is positive pictch-up deflections.
- NOTE3: gyroscopic effects are implemented in the caller function!
+ self.PHI_mv = np.array([[0, 0, 0],
+ [0, 0, - self.dR * (2 * np.pi + self.Cd0) / self.CHORD],
+ [0, self.dR * self.Cy0 / self.WINGSPAN, 0]])
- refer to [1] for further information.
+ self.PHI_mw = 1 / 2 * np.diag([0.5, 0.5, 0.5])
- REFERENCES:
- [1] Lustosa L.R., Defay F., Moschetta J.-M., "The Phi-theory
- approach to flight control design of tail-sitter vehicles"
- @ http://lustosa-leandro.github.io
+ self.PHI = np.block([[self.PHI_fv, self.PHI_mv.T], [self.PHI_mv, self.PHI_mw]])
- """
+
+ def thrust(self, dx):
kp = self.PROP_KP
km = self.PROP_KM
-
# prop forces computation / notice that negative thrust is not implemented
T = kp * dx**2 * np.array([1, 0, 0])
-
# prop moments computation
N = np.sign(dx) * km * dx**2 * np.array([1, 0, 0])
-
- return [T, N]
+ return np.array([T, N])
def aero(self, quat, vel, rot, T, de, w):
- """
- AERO aerodynamic forces and moments computation (in a wing section!)
- this function computes the aerodynamic forces and moments in body axis
- in view of Phi-theory in a wing section. it includes propwash effects
- due to thrust.
- NOTE2: elevon sign convention is positive pictch-up deflections.
- """
# data extraction from self struct
PHI_fv = self.PHI_fv
PHI_mv = self.PHI_mv
PHI_mw = self.PHI_mw
- phi_n = self.PHI_n
+ PHI_n = self.PHI_n
RHO = self.RHO
Swet = self.WET_SURFACE
Sdry = self.DRY_SURFACE
@@ -211,25 +94,27 @@ class Mavion:
B = np.diag([ws, chord, ws])
# eta computation
- eta = np.sqrt(np.linalg.norm(vinf)**2 + phi_n * np.linalg.norm(B@rot)**2)
+ eta = np.sqrt(vinf@vinf + PHI_n * (B@rot)@(B@rot))
# Force computation
# airfoil contribution
- Fb = -1/2*RHO*S*eta * PHI_fv@vinf - 1/2*RHO*S*eta * PHI_mv@B@rot - 1/2*Swet/Sp * PHI_fv@T
+ Fb_a = -1/2*RHO*S*eta * PHI_fv@vinf - 1/2*RHO*S*eta * PHI_mv@B@rot - 1/2*Swet/Sp * PHI_fv@T
# elevon contribution
- Fb = Fb + 1/2*RHO*S*eta * PHI_fv@np.cross(de*Thetaf, vinf) \
+ Fb_e = 1/2*RHO*S*eta * PHI_fv@np.cross(de*Thetaf, vinf) \
+ 1/2*RHO*S*eta * PHI_mv@B@np.cross(de*Thetaf, rot) \
+ 1/2*Swet/Sp * PHI_fv@np.cross(de*Thetaf, T)
+ Fb = Fb_a + Fb_e
# Moment computation
# airfoil contribution
- Mb = -1/2*RHO*S*eta * B@PHI_mv@vinf - 1/2*RHO*S*eta * B@PHI_mw@B@rot - 1/2*Swet/Sp * B@PHI_mv@T
+ Mb_a = -1/2*RHO*S*eta * B@PHI_mv@vinf - 1/2*RHO*S*eta * B@PHI_mw@B@rot - 1/2*Swet/Sp * B@PHI_mv@T
# elevon contribution
- Mb = Mb + 1/2*RHO*S*eta * B@PHI_mv@np.cross(de*Thetam, vinf) \
+ Mb_e = 1/2*RHO*S*eta * B@PHI_mv@np.cross(de*Thetam, vinf) \
+ 1/2*RHO*S*eta * B@PHI_mw@B@np.cross(de*Thetam, rot) \
+ 1/2*Swet/Sp * B@PHI_mv@np.cross(de*Thetam, T)
+ Mb = Mb_a + Mb_e
- return [Fb, Mb]
+ return np.array([Fb, Mb])
def dyn(self, x, u, w):
@@ -252,33 +137,22 @@ class Mavion:
vel = x[7:10]
rot = x[10:13]
- # input demultiplexing
- dx1 = u[0]
- dx2 = u[1]
- de1 = u[2]
- de2 = u[3]
-
# linear velocity derivative
D = quat2dcm(quat)
- [T1, N1] = self.thrust(dx1)
- [T2, N2] = self.thrust(dx2)
- [A1, M1] = self.aero(quat, vel, rot, T1, de1, w)
- [A2, M2] = self.aero(quat, vel, rot, T2, de2, w)
+ [T1, N1] = self.thrust(u[0])
+ [T2, N2] = self.thrust(u[1])
+ [F1, M1] = self.aero(quat, vel, rot, T1, u[2], w)
+ [F2, M2] = self.aero(quat, vel, rot, T2, u[3], w)
- Fb = T1 + T2 + A1 + A2
+ Fb = T1 + T2 + F1 + F2
dvdt = np.array([0, 0, G]) + 1/MASS * D.T@Fb
# angular velocity derivative
- Ma = M1 + M2 + np.cross(P_A1_CG, A1) + np.cross(P_A2_CG, A2)
-
- P = rot[0]
- Q = rot[1]
- R = rot[2]
- Jpx = INERTIA_PROP_X
- Jpn = INERTIA_PROP_N
- tau1 = N1 - (P + dx1) * (Jpx - Jpn) * np.array([0, R, -Q])
- tau2 = N2 - (P + dx2) * (Jpx - Jpn) * np.array([0, R, -Q])
+ Ma = M1 + M2 + np.cross(P_A1_CG, F1) + np.cross(P_A2_CG, F2)
+
+ tau1 = N1 - (rot[0] + u[0]) * (INERTIA_PROP_X - INERTIA_PROP_N) * np.array([0, rot[2], -rot[1]])
+ tau2 = N2 - (rot[0] + u[1]) * (INERTIA_PROP_X - INERTIA_PROP_N) * np.array([0, rot[2], -rot[1]])
Mg = tau1 + tau2 + np.cross(P_P1_CG, T1) + np.cross(P_P2_CG, T2)
Mb = Ma + Mg
@@ -286,13 +160,17 @@ class Mavion:
# kinematic equations
dpdt = vel
+ dqdt = 1/2 * quatmul(quat, np.concatenate([[0], rot]))
- dqdt = np.zeros(4)
- dqdt[0] = -1/2 * np.dot(rot, quat[1:4])
- dqdt[1:4] = 1/2 * (quat[0]*rot - np.cross(rot, quat[1:4]))
-
return np.concatenate([dpdt, dqdt, dvdt, drdt])
+ def equi(self, x):
+ vel = x[7:10]
+ vh = np.linalg.norm(vel[0:2])
+ vz = vel[2]
+ cap = np.arctan2(vel[1], vel[0])
+ return np.array([vh, vz, cap])
+
def trim(self, vh, vz):
x = np.zeros(13)
x[7] = vh
@@ -300,145 +178,21 @@ class Mavion:
def func(y):
dx, de, theta = y
- eul = np.array([0, theta, 0])
- quat = eul2quat(eul)
- x[3:7] = quat
+ x[3:7] = np.array([np.cos(theta/2), 0, np.sin(theta/2), 0])
u = np.array([-dx, dx, de, de])
- dx_dt = self.dyn(x, u, np.zeros(3))
- return dx_dt[7], dx_dt[9], dx_dt[11]
+ dq_dt = self.dyn(x, u, np.zeros(3))
+ return dq_dt[7], dq_dt[9], dq_dt[11]
y0 = np.array([100, 0, 0])
y = fsolve(func, y0)
- return y
+ return y # vh, vz, tangage
def step(self, x, u, w, dt):
func = lambda t, s: self.dyn(s, u, w)
- sol = solve_ivp(func, (0, dt), x, method='RK45')
+ sol = solve_ivp(func, (0, dt), x, method='RK45', max_step=0.01)
return sol.y.T[-1]
def sim(self, t_span, x0, fctrl, fwind):
func = lambda t, s: self.dyn(s, fctrl(t, s, fwind(t, s)), fwind(t, s))
sol = solve_ivp(func, t_span, x0, method='RK45', max_step=0.01)
return sol
-
-
-class MavionEnv(gym.Env):
-
- def __init__(self, mavion=Mavion(), render_mode=None):
-
- self.tau = 0.02 # seconds between state updates
-
- # Angle at which to fail the episode
- self.pos_threshold = np.array([2.4, 2.4, 2.4])
- self.ang_threshold = np.array([15, 15, 15]) * deg2rad
- self.vel_threshold = np.finfo(np.float32).max * np.ones(3)
- self.rot_threshold = np.finfo(np.float32).max * np.ones(3)
-
- high = np.concatenate(
- [
- self.pos_threshold * 2,
- self.ang_threshold * 2,
- self.vel_threshold,
- self.rot_threshold
- ],
- dtype=np.float32,
- )
- self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
-
- self.action_space = gym.spaces.Box(0, 1, shape=(4,), dtype=np.float32)
-
- self.target = None
- self.obs = None
- self.steps_beyond_terminated = None
-
- self.mavion = mavion
-
- def reset(self, seed=None, options=None):
- super().reset(seed=seed)
-
- # Note that if you use custom reset bounds, it may lead to out-of-bound
- # state/observations.
- low, high = utils.maybe_parse_reset_bounds(
- options, -0.05, 0.05 # default low
- ) # default high
- self.target = np.zeros(12)
- self.obs = self.np_random.uniform(low=low, high=high, size=(12,))
- self.state = self.obs + self.target
- self.steps_beyond_terminated = None
-
- return self.obs, {}
-
- def step(self, action):
- x = s2q(self.state)
- u = action
- w = np.zeros(3)
- new_state = self.mavion.step(x, u, w, self.tau)
- self.state = q2s(new_state)
- terminated = np.array_equal(self.state, self.target)
- reward = 1 if terminated else 0 # Binary sparse rewards
- self.obs = self.state - self.target
- return self.obs, reward, terminated, False, {}
-
-
-if __name__ == "__main__":
- mavion = Mavion()
-
- vh = 0
- vz = 0
- dx, de, theta = mavion.trim(vh, vz)
- theta = (theta + np.pi) % (2*np.pi) - np.pi
- print(dx, de, theta*57.3)
-
- pos = np.array([0, 0, -10])
- eul = np.array([0, theta, 0])
- vel = np.array([vh, 0, vz])
- rot = np.array([0, 0, 0])
- st = np.concatenate([pos, eul, vel, rot])
- x0 = s2q(st)
-
- def ctl(t, s, w):
- d = upulse(t, 5, 6)*dx*0.0
- u = np.array([-dx-d, dx+d, de, de])
- return u
-
- def wnd(t, s):
- w = np.zeros(3)
- return w
-
- sim = mavion.sim((0, 60), x0, ctl, wnd)
-
- plt.style.use('_mpl-gallery')
- fig, axs = plt.subplots(2, 2)
-
- ax = axs[0][0]
- ax.plot(sim.t, sim.y[0:3].T, label=('x','y','z'))
- ax.legend()
- ax.set_title(label='positions')
-
- ax = axs[1][0]
- quat = sim.y[3:7].T
- eul = np.apply_along_axis(quat2eul, 1, quat)
- ax.plot(sim.t, eul*57.3, label=('phi','theta','psi'))
- ax.legend()
- ax.set_title(label='euler angles')
-
- ax = axs[0][1]
- ax.plot(sim.t, sim.y[7:10].T, label=('u','v','w'))
- ax.legend()
- ax.set_title(label='vitesses')
-
- ax = axs[1][1]
- ax.plot(sim.t, sim.y[10:13].T, label=('p','q','r'))
- ax.legend()
- ax.set_title(label='rotations')
- plt.show()
-
-
- # mavion_env = MavionEnv(mavion)
-
- # st1 = mavion_env.reset()[0]
- # print("reset :", st1)
-
- # mavion_env.state = st
- # st2 = mavion_env.step(u)[0]
- # print("new state :", st2)
diff --git a/mavion/mavion_env.py b/mavion/mavion_env.py
new file mode 100644
index 0000000..10cc9d1
--- /dev/null
+++ b/mavion/mavion_env.py
@@ -0,0 +1,70 @@
+import numpy as np
+import gymnasium as gym
+from gymnasium.envs.classic_control import utils
+from mavion import Mavion
+
+class MavionEnv(gym.Env):
+
+ def __init__(self, mavion=Mavion(), render_mode=None):
+
+ self.tau = 0.02 # seconds between state updates
+
+ # Angle at which to fail the episode
+ self.pos_threshold = np.array([2.4, 2.4, 2.4])
+ self.ang_threshold = np.array([15, 15, 15]) * deg2rad
+ self.vel_threshold = np.finfo(np.float32).max * np.ones(3)
+ self.rot_threshold = np.finfo(np.float32).max * np.ones(3)
+
+ high = np.concatenate(
+ [
+ self.pos_threshold * 2,
+ self.ang_threshold * 2,
+ self.vel_threshold,
+ self.rot_threshold
+ ],
+ dtype=np.float32,
+ )
+ self.observation_space = gym.spaces.Box(-high, high, dtype=np.float32)
+
+ self.action_space = gym.spaces.Box(0, 1, shape=(4,), dtype=np.float32)
+
+ self.target = None
+ self.obs = None
+ self.steps_beyond_terminated = None
+
+ self.mavion = mavion
+
+ def obs(self):
+ Vh = np.linalg.norm(self.mavion.vel[0:2])
+ Vv = self.mavion.vel[2]
+ Cap = np.arctan2(self.mavion.vel[1], self.mavion.vel[0])
+ phi = self.mavion.ang[0]
+ Turn_rate = self.mavion.G / Vh * np.tan(phi)
+ return np.array([Vh, Vv, Cap, Turn_rate])
+
+ def reset(self, seed=None, options=None):
+ super().reset(seed=seed)
+
+ # Note that if you use custom reset bounds, it may lead to out-of-bound
+ # state/observations.
+ low, high = utils.maybe_parse_reset_bounds(
+ options, -0.05, 0.05 # default low
+ ) # default high
+ self.target = np.zeros(12)
+ self.obs = self.np_random.uniform(low=low, high=high, size=(12,))
+ self.state = self.obs + self.target
+ self.steps_beyond_terminated = None
+
+ return self.obs, {}
+
+ def step(self, action):
+ x = s2q(self.state)
+ u = action
+ w = np.zeros(3)
+ new_state = self.mavion.step(x, u, w, self.tau)
+ self.state = q2s(new_state)
+ terminated = np.array_equal(self.state, self.target)
+ reward = 1 if terminated else 0 # Binary sparse rewards
+ self.obs = self.state - self.target
+ return self.obs, reward, terminated, False, {}
+
diff --git a/mavion/quaternion.py b/mavion/quaternion.py
new file mode 100644
index 0000000..92121f6
--- /dev/null
+++ b/mavion/quaternion.py
@@ -0,0 +1,86 @@
+
+import numpy as np
+
+def quatmul(p, q):
+ m = np.array(
+ [
+ [p[0], -p[1], -p[2], -p[3]],
+ [p[1], p[0], -p[3], p[2]],
+ [p[2], p[3], p[0], -p[1]],
+ [p[3], -p[2], p[1], p[0]]
+ ]
+ )
+ return m@q
+
+def quatmul(p, q):
+ r = np.zeros(4)
+ r[0] = p[0]*q[0] - np.dot(p[1:4],q[1:4])
+ r[1:4] = p[0]*q[1:4] + q[0]*p[1:4] + np.cross(p[1:4],q[1:4])
+ return r
+
+def quatnorm(q):
+ return np.dot(q, q)
+
+def quatinv(q):
+ r = np.zeros(4)
+ r[0] = q[0]
+ r[1:4] = -q[1:4]
+ r = r / quatnorm(q)
+ return r
+
+def quat2dcm(q):
+ dcm = np.array(
+ [
+ [q[0] ** 2 + q[1] ** 2 - q[2] ** 2 - q[3] ** 2, 2 * (q[1] * q[2] + q[0] * q[3]), 2 * (q[1] * q[3] - q[0] * q[2])],
+ [2 * (q[1] * q[2] - q[0] * q[3]), q[0] ** 2 - q[1] ** 2 + q[2] ** 2 - q[3] ** 2, 2 * (q[2] * q[3] - q[0] * q[1])],
+ [2 * (q[1] * q[3] + q[0] * q[2]), 2 * (q[2] * q[3] - q[0] * q[1]), q[0] ** 2 - q[1] ** 2 - q[2] ** 2 + q[3] ** 2]
+ ]
+ )
+ return dcm
+
+def dcm2quat(d):
+ q = np.zeros(4)
+ q[0] = np.sqrt()
+
+def eul2quat(a):
+ phi, tet, psi = a
+ q_phi = np.array([np.cos(phi/2), np.sin(phi/2), 0, 0])
+ q_tet = np.array([np.cos(tet/2), 0, np.sin(tet/2), 0])
+ q_psi = np.array([np.cos(psi/2), 0, 0, np.sin(psi/2)])
+ q = quatprod(q_psi, quatprod(q_tet, q_phi))
+ return q
+
+def eul2quat2(a):
+ c = np.cos(a/2)
+ s = np.sin(a/2)
+ q = np.array(
+ [
+ c[0] * c[1] * c[2] + s[0] * s[1] * s[2],
+ s[0] * c[1] * c[2] - c[0] * s[1] * s[2],
+ c[0] * s[1] * c[2] + s[0] * c[1] * s[2],
+ c[0] * c[1] * s[2] - s[0] * s[1] * c[2],
+ ]
+ )
+ return q
+
+def quat2eul(q):
+ phi = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]))
+ theta = np.arcsin(2 * (q[0] * q[2] - q[1] * q[3]))
+ psi = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[3] * q[3] + q[2] * q[2]))
+ return np.array([phi, theta, psi])
+
+def eul2dcm(a):
+ Rx = np.array([[1, 0, 0],
+ [0, np.cos(a[0]), np.sin(a[0])],
+ [0, -np.sin(a[0]), np.cos(a[0])]])
+
+ Ry = np.array([[np.cos(a[1]), 0, -np.sin(a[1])],
+ [0, 1, 0],
+ [np.sin(a[1]), 0, np.cos(a[1])]])
+
+ Rz = np.array([[np.cos(a[2]), np.sin(a[2]), 0],
+ [-np.sin(a[2]), np.cos(a[2]), 0],
+ [0, 0, 1]])
+
+ return Rx @ Ry @ Rz
+
diff --git a/mavion/rotation.py b/mavion/rotation.py
new file mode 100644
index 0000000..5e5ea07
--- /dev/null
+++ b/mavion/rotation.py
@@ -0,0 +1,93 @@
+import numpy as np
+
+def hat(x):
+ return np.array([[0, -x[2], x[1]], [x[2], 0, -x[0]], [-x[1], x[0], 0]])
+
+def inv_hat(m):
+ return np.array([m[2, 1], m[0, 2], m[1, 0]])
+
+# (a, n): a = angle, n = vecteur unitaire (axe de rotation)
+def quat2axang(q):
+ a = 2 * np.arccos(q[0])
+ n = q[1:4] / np.sqrt(1 - q[0] * q[0])
+ return a, n
+
+def dcm2axang(R):
+ t = np.trace(R)
+ n = 0.5 * inv_hat(R - R.T)
+ r = np.linalg.norm(n)
+ a = np.arctan2(r, (t - 1) / 2)
+ return a, n / r
+
+
+# Quaternions
+def quatmat(p):
+ m = np.array(
+ [
+ [p[0], -p[1], -p[2], -p[3]],
+ [p[1], p[0], -p[3], p[2]],
+ [p[2], p[3], p[0], -p[1]],
+ [p[3], -p[2], p[1], p[0]]
+ ]
+ )
+ return m
+
+def quatmul(p, q):
+ return quatmat(p)@q
+
+def quatnorm(q):
+ return np.dot(q, q)
+
+def quatconj(q):
+ return q * np.array([1, -1, -1, -1])
+
+def quatinv(q):
+ return quatconj(q) / quatnorm(q)
+
+def vect2quat(u):
+ return np.concatenate([[0], u])
+
+def quatrot(u, q):
+ v = quatmul(quatinv(q), quatmul(vect2quat(u), q))
+ return v[1:]
+
+
+def axang2quat(a, n):
+ return np.concatenate([[np.cos(a / 2)], n * np.sin(a / 2)])
+
+def eul2quat(a):
+ qr = np.array([np.cos(a[0]/2), np.sin(a[0]/2), 0, 0])
+ qp = np.array([np.cos(a[1]/2), 0, np.sin(a[1]/2), 0])
+ qy = np.array([np.cos(a[2]/2), 0, 0, np.sin(a[2]/2)])
+ return quatmul(qy, quatmul(qp, qr))
+
+
+# Matrices de Rotation (DCM)
+def axang2dcm(a, n):
+ dcm = (1 - np.cos(a)) * np.outer(n, n) + np.cos(a) * np.identity(3) - np.sin(a) * hat(n)
+ return dcm
+
+def quat2dcm(q):
+ w = q[1:4]
+ dcm = (2 * np.outer(w, w) + (q[0]**2 - w @ w) * np.identity(3) - 2 * q[0] * hat(w)) / quatnorm(q)
+ return dcm
+
+def eul2dcm(a):
+ Rx = np.array([[1, 0, 0], [0, np.cos(a[0]), np.sin(a[0])], [0, -np.sin(a[0]), np.cos(a[0])]])
+ Ry = np.array([[np.cos(a[1]), 0, -np.sin(a[1])], [0, 1, 0], [np.sin(a[1]), 0, np.cos(a[1])]])
+ Rz = np.array([[np.cos(a[2]), np.sin(a[2]), 0], [-np.sin(a[2]), np.cos(a[2]), 0], [0, 0, 1]])
+ return Rx @ Ry @ Rz
+
+
+# Angles d'Euler (phi, theta, psi) selon axes (x, y, z) : z en premier
+def quat2eul(q):
+ phi = np.arctan2(2 * (q[0] * q[1] + q[2] * q[3]), q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2)
+ theta = -np.arcsin(2 * (q[1] * q[3] - q[0] * q[2]))
+ psi = np.arctan2(2 * (q[0] * q[3] + q[1] * q[2]), q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2)
+ return np.array([phi, theta, psi])
+
+def dcm2eul(R):
+ phi = np.arctan2(R[1, 2], R[2, 2])
+ theta = -np.arcsin(R[0, 2])
+ psi = np.arctan2(R[0, 1], R[0, 0])
+ return np.array([phi, theta, psi])
--
GitLab