From 5d587d0b601e41143cf07a03da8775380fcdb979 Mon Sep 17 00:00:00 2001 From: Peter Stahlecker Date: Mon, 30 Dec 2024 16:30:20 +0100 Subject: [PATCH] drone in tube, new --- examples-gallery/drone_in_tube_solution.npy | Bin 0 -> 17096 bytes examples-gallery/plot_drone_in_tube.py | 466 ++++++++++++++++++++ 2 files changed, 466 insertions(+) create mode 100644 examples-gallery/drone_in_tube_solution.npy create mode 100644 examples-gallery/plot_drone_in_tube.py diff --git a/examples-gallery/drone_in_tube_solution.npy b/examples-gallery/drone_in_tube_solution.npy new file mode 100644 index 0000000000000000000000000000000000000000..e1b1eab95847cf67fe00aa676b2ba58f653d6522 GIT binary patch literal 17096 zcmbTdXH*nH*EKrioO8}Wkf>y&3PFMhNLEl0K}0}Q1QatF6p4ah0z^Q`h=71lP0mWr zX~-EQXFi_$-1q&~x<7CI*{gfi%vAT(sdH-YUC~Ash8Hgr;Jk28#2juuuy+?z711GzCH;=Ey<>ln%PM$c)|BV0tZ==i| z-y_cWwGB=eoK-T6+XX+Scv5fQ+XoxE5*fc#4gs(At%pwYM}W0I#zCqA2Th`qpXvG# zz_x$d*`dmWFeByf9MKFRyf^%YeISeovZkyw|B@tzF=?*?6)|GSOWq!I_7Vv^vA7|$ zVNVKge<`T9^CN@pW^4H}CFJm6n7c=LgaWqJJQ{5OMF~qkqg-DVDi~y2H4^li3Z9f0 zyrP4sVE>b0rl&JhP;)3JH;|SZ8uNR9_3NX89aCnc?>(rXxYMWE><&ukd5^_WV21*B zcBMq}&6C4`9ocZgTr%kRY6=TAB!y>2ji(~&iQ%8GH$t8Y6G2GqQ$%D>0M!zIO=XcE zfry*E>%;VW;L*d39_@)uAm?LK_W1oO2#ZX6e7|A=44E|&8~IOw-iP-eEr0C=qGRz^ zUMm=Q6LdnKY4kUOhIDPnsT-x^XU3j<978@~5}i_`v#8~a_uN096%;l?ZW%?ij-)@0 zCwIGVp|iC<-rCK(=-ky@$8oAdG)6r>I2?<^K79D@x6Vq4aefR-mhdOS0$xAxvK=JG z-c!7vpXVXPUZ_uP;PlBb?=ej0ksUdvv50@#ewPBPT?@urzM#bHb*`K~5l@9}jclxD zBWjFv>$GnAA`SMb%}O+gjSl-M<=HvKM31$JO4q+wqQ!nh-NXl$(O^-GPh>-%Qe#Um=ks3e1e_nfUB388)FUo5v?fim?_oe)}g&jF~^Yea(*gc${+mB$ZE& zkV>4+Go#R5bQqPuZ+l?_*>M}-Z#-K@-X2!`9)HG>I%&In)U`io&@@bH$D$e-k0ffE za*u&q-OZc17ni}f<>i>43Y&m|<2k3F-#!o$?vXM%MF6wd#lI>p5y5C&mAt$kDQqSz z*XTPUhx4X0dHmO?V21SsQDGzv6qe@vLl{j5Z!v6ZxP7OG-)0A`eqU#RbvLTa%a<9T z;qtqWeU}*F=PsOKXR78TF(gE3OYWlOftfW`zd47yNr;kfU?<%gz0}#@c+H$ zq@2yZ1aOrc<)~o|M@p-*v;uRq3=McM3KMh7zIQ(9GB}pi3CS2v|8@WA@y0mlUG=mQQd;X1%-}(C?H-^=_L0$`t2zjV&}Ap zHq#9o0*kiMqpLnY^{MvI&2M3?RJRY%8}5vANqtAiEBSuqr&9zN@;AlBh7w{?szzB~ zH;FLA<>Y@q97(Xq(!7+=GE%Huq_e#D4;j|J&k%DEL5`Vu$(^L2q`*Sm6TMo_P+;%9 zZN5vZP++~=fJ%>o0`vJgZ&{j8jxA7a`J7fJ$8h~s9Sm>DumvIlrf1cpm`1wvDV;hJ zY}ZtUM)CtO_H=pe;Ris3(Z97>(*8++l^tXdNB%oP!9=;=UU2Lq17NvIKe~l}J$F*j z&|gDlK2Z_gDxj6c|~G2Omi+0?*dQ zvzOU6K-FbDnNr~%@Eo*l4C5kzi9gBfl${Bo)urImT3?7@xa7HW z{(_bqn!a5kJzq}-lQ@2M(>sts7v*a+b=9Ph)%ADDL_Z09sWuq@_7^d<|D~jG(~k)1 z5E2KSF(-r+iZNaL@&xd%=G%yFNgP~y&QDApcL+4)-#n%eIRNaO1b%Isdti}>?ewYR zbxo1jv$10+g4?GE!2v1@=1Tt$$UIvjk_+&ILj*XWPYR41> z))o>$4UJu|wQD4>^+uHd4?7tYrX%?@I!X?=&d*S2{iK9lw#kxYA=J?1Wh{JbLJO4{ z;_i;^(ZQZV9@orLdMG_qULtan0hWZN3h|RN!uVSk`IEmKuP5U#c{9&2K~=98MDG1e z@W%SNo(^wj_!MU~PS3>xR|rOWbjw(v)L`ik;_IyN#4|yaRYEox**8w-lfnjT9Xw{I zbl71suDM!oiXC2LsC|a_<$&z5Lf6>2IbnQOGCnMi6H-To`s-+ML1ONyM=#sBV4qFr z_gh!FVW;8NiNYCfNJjeerjiE_H0_)fDmdVQpAOR8XP)!Id+fI)J}vP=mOw=ooE9IH zvOs5(-TB~_V*i)Z5BOl;(a@V{4Sq<|xF+x}haXz%#@)NfCIFkO*EQ`P3&0K4P2z@r z0r=pu_oZ_xf{>o>=d<}3K^RxXY5HhI5Qg}e?>^NNg1+olyI!A!;ClsoFtj8D1!!rE z#npvjZvj(&ufH&K>;3onX}vJq3>`aNKq>-ndAnUX14Q6C&TP_&`yx=G?&{~7&myoO zN^9A#Rs_c94rYKk5oqBfccp?-6ngR$4@irNLa`y?$Pf)t*sHWePiZa+<36=;)ZP(= z1Fj3-y~0GHgRn*aj{;FhH-F+lbX*i}=BM!3vWUT@`|%`_`eIO$=|-OZ6ET?8dFO@D zH!-NG;39`>7K6_(k(Mq^iopk5JDQ?fVi5QLH1Yoru7FI7*^XSpWrWSwFIvzoqp{+j zjSD9hQK#0})LX83bZg_YscXnA3K$6(O1U_LI=*yhD@aVEZ?8_;di72svM-JOwcHa( zW~fX_I1!Kf+lGPe*8${Z`sCb0;=c$K>Nq<+9t8&kN2c+HRbXPLcJLs#1I_oRZah3W zh5n^H+sgbrg)~IM9^gs-ARXn6f2r@g0ng^;!&SN|pvXcVNVPu=vQw7tiz`lnoTE5` z?~zkz;HktaFMbl?&K3p7f0;o4&T8ia?s25>NW9q57LQJOO5jHphS8(nf`T*s{ivzR z-@hTT8wE4wEoz#!qi-m|WP7~{9jLQyn{rg45ZTAp_?{mqnCw>A;Bp+Wx640+$JYYa zzh>Ub&-;O%;qKb+CzF7#yHnufvw2XyBTen;zXUj}X-*BCS_YM~EWBO3OTfo>{uX=e zJh+kWt~1;*4fca0Uly>9gJ-l0YaV6;pubgGQcdeGz!t*#h>J79bfjlo&Myp+*LrYw zH+P_u;hLnk4TsT_%&_szl5tdFBtRGJJ&m?2@4QepnnU*{6)pp%1+;4{`z%ap5ecQK zyedDph@3K7xHU8v&?l9PmVtY-C?a0bHtq5h8qYZW%g7gxc;{j{8*shoEEiMh!H-5H zU0Pc%PtXOpTH4zA{n|jr4Xtc1i&pUaM9q=NtyU1@$snp&*$V2c1vi!ES^;@~Tq}P` zD@guBy7fu96{u6+pHABO1J;7s&z(Hm0&<>HB?>%g0=FYZ|5`UUfT7tyqtj;f;PcEv z++W^0AYOZ;PgS%QD1InXnwG5r?myq|D;?~LTS;QFe2;5=#*DN^=XcW@>UP+7rLT^VQ zSDqg5I&o$-FQgt!F>J66jTiqf;zl8^^Xtu0nh0uAyifrm3qM;JZtw@4V`BIF$Jvi) z1Xu=Nl#Qa#G2Z61^J7RtMZh>#Zvq)qDnu5WPa<=EF`#HZg&gAe@9N&1M%RpZD(^+i zp!9?^&u`y#{ZoGqf_9gY}`C>`IwX#t2YDiF@~w5PrHDh`+UIi zOgrlM7I!D;{4`<~HlEpBn?(3iH`k|Zx{$-_7edyWMDS!>Oohv}1F$dN$h49xKyP2| z1idD0LeBRU=2QsUPzc{&@FuOu@QE;-%8gUE$%hT(2G9yMA;K8?;F zN8N!-(Vk;?bTaek*EZPz+CQZ>`1x8pa%K6UACg>;_BgGaqo*p6aJEI#$uDI{rZlMx z7g>Tr8pZccwHBg?E3ei;e-64^+@)%l^%l|oQTU)xodkk|sMrY@i$VC$2YVcxH6TT0 zl5Hln1t=M>zLv+ggE{hK#d`N{pyX}V+Qrca1fMz1n3(m0i#oh#%nJK}O_itk41W)} zU|)N|rn?>Z#y)(zw9*Kkx+nSVpF-e+5C|vQ{sPv`(-@MTr=oewFKaU%7?L4hYkE`r z7L*EH@~EOM2G$8;95Nsxvk;0pbVPRJ+$FfzHV3 z8FsE|@R5Ba%k=W`eDDr_yHhj^I3@16>8s5H&zxRs?>`Fw7|$)_5HEq{$~P&!m5ZRx z<%!w&!#TiqhwS zkJe{GrKL<}Q4|a1oQI)|Z z;wix9Yfz#T@qZC_{Tsu%(!f%r?Jf}l>d_2c++fDoSCk<0QMZIA0}ym1bd*#Vf&Ttp zH}l7(p!vD)=hC`zu+m!daml?Ba9S_Z5{K3Sr@n|+bq#;Ob{eVDv%?M`Xf`z58VT2kit2-9FRah32xeSCP8`)i|nn{SrFCl=8$!J5hS>Iy0*DZqb^HUe}~vP z^!dC&cVy^1$}cyeN)4Y!Vl|vQk;nDKMr?M&$uQ12_$;amXvRQ41I&E>b)QENJ%&KgQnCdT6lO+ z)qZyfdAB=}T1ECFS#Cj&q=_C>HJg``@UjcJT^L7ll7N8Q{_R{!8r3 z&cpwUI9%j4F%iYmAJ{z=I+bV?-ZkK(fxQ6(DKw=&>%+jZ(q2gmQ2<(4;`@93eH!}o z!NYp?@lP}|Bvi=%y#xtl6B2c~l_8?<9!=`Szfr*JM2TF3V&vEUm^S`)0Yax*&wYPc zh@AW%oo|+m0x7l~>k_FApjC)&U9o=%jNTX^AaI!gZyu0p6^##psrqtvyk-y*c9C`1 zxHE*ry(#Ae9#5bYI-m2ev2oO;a-V{u6_1(=g;l7_#z5qw>A-W3$H91g4|1NF0F`-P zD`l}MaAJ;9;8WHNu;bNORB@jJ53^rdBrMK@`chjsA~}zNR5Yth%JH0 zmg~;WON(G;qfI)EW)TGH)xV)}od?Wawzq_)W`OYofBbXiNs#w9mN8v)1bkM<>874+ z2e~v0P2u+oK#$|m-``(L(3*78m@H{KI)Bo`N)8PoPLG)Ne4TMbrfwUN{CFC@7!~FJ zTs((X$N@)Y+XDLgxSd2DJdd{IzERp1&Y3G@z= zR5R2XMUPs`Ku+{22ka@p&~TKM#VAKY!Uzmvw+&g~ejFEaNEY4RE+)JB(z~1=Z!kc^ED*z^n%6WR_YMcz)WroI{%f zroXjrFNooUm9|#;3a*@x!Q}n#$D17RLOVft_iYaN+;=QezJwh!U9nMd;Fkto)7s8?!$gw&P=PE618|hG}-`cIn0Q9X*Z6&1JKuT(1u2z-=PCC$1 zlt@#-tO#CL`B)lwaqwK(S?goo@X{CTMgWfU z7#YS;qAVtwL55|^YZ}jnl3_f8|Gq`Zl3`cmPqesYl499{vb<-fNHD>|)RdeoVvGhz zE<`sIV#4Ex&YZM3topQ;LgkBX#FOTd?_ITk%FYJbo`r>|!(rbY1TO+B_Hw3Yv;=V9 zS+pkghzg3b;|bE^nc#)83B6(mcDN*Dl~{A13%-%ywJVL{hV1?do$LDCFmHhGyxjpO z6f}9VboUoKtQo#7o5sirA75CrRcdB}mF8a;*jyQ)(xtldWAZdGp=q#J*o++35lvov zdWRUgzZ(T{bCtcoWtZf8p=k?9 zAj=oHYmo(lBfjD;X#GI>Ce2D`3rkU7U^=(I<5R$+>iY4EP!AAWDfzC&y#js&M31#; zuK_K}Vx1omJAk?V6z;hNA>7IcOsG9L0>gyAz0%M)D2|r*q`CYP-I>U0bBz0eJZ6Q; ziLbY#9_g<+@~20TRCN)ZZ)_YIU$VO#)4B-W^E$T1FmHpWHFLI}_jkb~z3vw4>}>!H zuDjQ`ZUBMoTh*Rct3c9V!siFk3aEHHHCbV{0959zQv@hx00&)-a8n>2*avcV{}^Zm zMC$D4-`za}vUKv_T)C19sy0vObR6fUSES1|B5d1W@2N-lP|7NpQ4jH!zV{VePpM(M z;<}0m)5?{{6> z-~Oh8B|9>hg$6MUIH>OMR@?)2rR7yt|CT_lI^XX*M;(CR>&v`_$~LsdIaFe}KZ{W4 z596zO>*&fKbDgVC_mI6QD}L+?4&&CH>~uLG#3Jo8%8$B;F=S=%rt%6Y*5YeZTSq{S z(JcIW#CDq!Gl;t(_$-hHTfTF&&i0%Z>&W4=I>%0q*%#lmiId($%8cP(X*285t6yEW z)AI(=Z&kL9Q11hzcaw0%K#d$DGu$w1=p(`Kmp)6Z(k~+4jfN&8p*4_p{(2E(8xC5i z|63Ff-U5GWp8PV6Y6rCGhFLbt1ISW>lw`>Xj|#7o%+u%gptqLQ)ssUt2=_ARsq_74 zHC(PxBB%cC(w+1Dzp*+pSZ`ZcIpBW$-j~z-Q#S}<`o`uJdE{rF~Ec&s`nA6n4uF()}{Dj6pK{ylaK zQtO@7`_6&+*qu<-p5esII^|T5UtTtTZ+FS*JAC;%v!q-6r|JfKxmt~M$up#1e zcLq$9Ei5+Srop9gnt!u5=70`shvb0yKj4)6;+pmFHZVR%zC}n%1j8oV$$2bjVf12H zN{3M4isqAg>t;cb4pw}4rju}p6XiwTu(84B% z05NavLm)|u7&mVXfTmjWWbdz&D3JUckFC`X`lx&*O|Xd&+e^JU)Rakv6$MiWOp8!q zA_*NX@6~BAmVtx&%AB;=)x>bZtWg^5$96=$T0Av2xcNfwt|le+@R7)v!6YeWD>>BH z^@|V_d$oUK!e}4mU25~u=3GNfrV)RoNhi?qYNi4G?&OR)*Tf>1=VgP#i@Lugg#O~2yz~Yz-y`XA1-_SL+B#^!1>4? zG|;I$Cc+g9YV&)xs62mzERAqVL4xnV`r(S_L)vWgt*-6xzIrwKW3J{;eyIiZ^Ltvw zNwlFS)@fO7}Bdx6hRlv&_v9e^k#7-+P|2TczQF_K{(0=|(J&Z=Ws7E{6(g zJGAV!vk}1(F`jqR0!M)U=Ht3iyF)M#dbh)0fB;e_5>pz^k-$6WV-jm>>7aZT^S`J00VYYZz&`(bl$j~$(PP3#A8Pjh!t5Rt|iS`kv z;%n+~Jt1mh-$WTEnSP?Ce<2SO*)F>Ue;0)!)nWJQq*$TTV=ID&s{62=+dkxj&4q=b6zwM=ouKmrtwky@#)?>%r>}oQCPGrHGT)VCu!k4&>e+ zGpl%g01YQa3YR_}LGQNx`1I|L>i~V)K9;Bv6zptyw=RAF*~hA~OuKg=4JknrRqrfh zkw)nEq^%#U#>?xT^W6Y}ulY@!WyoQHy%5uGA}b8MeB%4BUIDn4a8~Btuo&FUbGI+q zmV`k(@kinm(r{Ko(9DTW1~%AKaW?bGLb}Pqo!~lINW~XoBq1vYaf1gyJx30PKN@+H z<}D8c=uJ4&M&#j$ZiR16z7`>T(yYr;9N!7RIX9%DcjbeXuAd{2F^4R}zw}3&-nswZHi9jiqm( zE9LBAMt>#H{!yc8%hZR8jufX_3fEA@JGT%^2LjCI)2q-FJUKR106=eCq>+c zW5moi?%eo!jsZKVc{jS0c^z@3Z#-^_Cx*cP$M*7TTDYgH_{_F=2MCm!CerP7q1Yg? zwjI$b)Wyz3D7aMvdgl7IpK45jt2>hvew*YF>+X=@#BoAa-LQAQiFB~_N!|0DwmtBp zVKwR%ZwpxY7?Qf>J&ImBWq&!DKY=)g*M|^gA8Pe-z{tn)f&6miif8pCFv06nNVjbQ z!5+bFK29Qd6tjIJ%z_3MG5hsjge)-T)A)T>Qf}z#u0sf=dEwoRrVDnm?C^5*g}Tg_ zRPeHE4mV%WB3R}wFa3CP9(BF{N!fJg09}h}h!Y$pzy$oxV2UdQ7(2CBoe2jHW6QL; zcg1lR5m;4H(iE*B``h((G?TNaf^)vCPpTg=Hl1;=l=_AecZK;D3XgR?Z}m4)H%GvR z@Gl2ZmT}OMa6LO!W)w`M485=|uL0FDK3A>}9oOkC!T*XDHqb-41%nn#VyxuR1#`V( zT8yKVWGPXd8EYim{Kj6yhSj$(m1{p`$L?SISP@6hf{}+#t}*jbV7c6!^iL(1P<61) zx_spfC`_&03RT_)O}wXGT&X|am+88(O8s2}gu^^nFTS4xG`4&!OI1U_fj#c9;eG?i zEqN)TM))0YpW?amv?T~gxjk@i{*@2JNjE-Ai~Rv-7S=1gYllH*_I-VW$ypGQ>#Hf? zxds#npK@>b0Au`W)J}{oV0d4(HIwJr419l-BlC#O{fQWFU z;nZ>cMa;>@MBuOvq)2#9Dn1@|(wmKhHM_-*^%gTp6&LQC_5#__=KMELp z)ZxR*>S9$gtobljF+Iak8$K+)XsmYLgb%wCMo@G?j}QCNbR;}-+)oJ~{ax6Y4`bka zP=4krAGX+1excBX4|{QI>Vx~k;hU<;RpyZvtNI zvtgwTXErzX(^7>i9CBjkT3ivsE-N+_8)XwWz=(ONy{=;TK!r6`T(ul7$6-DUXPG9< z50OI)=T?6C5sGJiNOd6tht>U}dNd_KfSD)?nHiW6U{5n>C5dbZFyo3jn;9zt%uv8u zSHheCqhj?+!yofyQ+k4BLC1JnO657%MFQ-Ij1Fz-5nyjjUhSx;6JV|v&k*rS5nvq7 zl#8w61emRxeVFF)=RchmWfvnS#QxZRdbB!8gw51@)pIv#7m~|4%ChCwm{f`wzO`t3}0!rQnc{*d*ggIe4M8p!GKt180-p zeo|Yh0Iyfelpfu!0xMGb|Nb)70KU4>;;qSAkl=UwvDrdBpz2#|sXEaFwsfAe6);e0V`$`gP@ZZ@Q9Z2}zqp%@auCIN>l&BM)vDS)GV zK1};|8o2bF7?l4o15mvidvoqA2yT+8{LwTA$m+TrT!-dC*`(oReuVrA+S3F51yaHZn63l&lwF1gN9YuSutNF(2KM+DBb|ty& zAD9|mvCR=(J=UT7PT5ASf==@ZIX!|kusLbaH2q)= zY`j>w4g5uA9*&*b0nl71WBKb2I9R|p?2zq(#s;x4JDXjQ9M{M_@NF0PJQJIY{ddfh zJ*hC8Jm$3u4imN}9`At;;@6p_Xb*famL#ww+Xq@wxx$yt_JQH%oBO9y_CY=K)nAH- z`@k(U)FR#X08~+|vNbjyfbL#;+H`mb47+KEwki(6(p&$WSfe9g^L1%XaPkP0SxygY zYvLfs5$@Y+1P+SJNVUfg<6tm8HN;Ge0De^VE^)Yh%#TnO9h#&NK;b9mIn+Z0uw^&C zu#kZeruh9{m^@1eNAn-gW5;~nGHF0k@r`~!>(=_^?b~kfxN2rj%b)|?bLxn;jBW!GmUJzm#vLGZXeW(c zq7PUlC|Wv;;KA1E`q_x(DKJ<^u<2cV%!hB>_dl7v3Wn(OmyS9&f!X6487t8}AX^?^ zUd(dpw?8TR4aCm`i~6CQ^=TO-G<*Wp#ZII|5P}MLBGZN1*%s z<#U%OjsTuDdz@td2++y=x4%0+%{Tq?A=r-g7aos10Kb})9xl)A106+$WX|vVK+#Y9 z{ReCxL<_yv)aO3{7z1h1kNgARQ_)y_#{LlSyUv*OFdc!0_^q_dX-9ya;qc4f!y{lS z_TTTn@Rrj-Ea?bnnia@<;f}z$1|fQz8;3wRSi5e0<^Z&&Zlx6@900ZJ@_m6p2Y@fE zhyuMi0In~(ALb_>fced%Jl~Q7U~01XU1R6~Ou5F{7Em986MOm*^XCtN+N}TU!PrA! zPkmC-WZ@84m%5z3r+owrYJ6`-#2!Cy+3!D9y+^=D`oH5j!~b7os#bR7X*dEqJEO(c zddKg_z5S!~%|jsmS*SS@cL2mZqo}-hcfl*KL8eBHZE!8;p)h~PIym8%r(!(10_M{{ zWa>xG0vhG6uqdW+fNBfLe?P;6P4Aq`=Th*%Fvq57RcRd5y|SoyurdimdKW~;{bqp% zhsDIl>_u=dRtf)|`yZfsRB`{=lQnP_`Kro}ZUC8rdow*s+o1H7m5;vLE|~uV&L+C< z18uP@0X% z#qaH#Ite13zkdky!UK;@gRCaO0kCJO)b^IY6Hx4xnVp<&0ta^TgL*X;;P$ztCYk)7 zfbHk$q6%CJFz?S8-P{WW9L{;=n%Q(`q#3;W{)2;Z{~0eaWPBZM5$1iaylPb?T63)q zJRES!yo;{~6s!iqFC`m+j*B?K!|EoGC#OpFd!+>|nA`(*Tw1}<-h9^^g?6xZ#VQ~M zbOJTYTUz#x-9W{_sY&)(FJS(U>I@p%D;OcE;MbYZ!3rg}6MuGz6Yk z^8WH;?*Ywgxi;8$QZfBMoWXww(Z5T_Ug_gbIw zdA5&$h?1ql>9G-@^D0+|iDwl2)Oq!8^!6y2RlLOQ-7yNzD*nj3WsV2uv!u6|XYfEZ zV9}L1d<=L@j}EqJkAr6YHv@Yl6F^YfWO%i40&G+EoIiLr2^=&3?lK+wLsQ37-b>z@ z0$-9JkM2I30``;G1##{vAVf{yCPz|^yzr8UK0M{#M&@;g6p!l zL*EbnCXYt#EA#^L0J2ZnCEehi7GH$#w=VFKJ49jp_`SZFy&uBv*9GdYvAL^W?*`Yj z1331u9x&N%M56q*4}=F6^OLp>08)o8UV-DhuNgC7yZL(*%v-m596FDIg!egC;-@Eo zRKv}&*6m5K;}oSg8!!zvT?y9c<7Pn4N8(y9nwW_k`^A=7PRUt0<3ZO%(K$iSQNR=w)8$h(4CIA}6-USiL9me6iv;dIFvK1z z5;A@4W4cxI&flUPtX3B2@eH+qM@QchwEoorrTGjQP+u(ZLREtF}z5182f(ic|KhhzeEd+@n9 zyv&nDfad5ScOlco;G2Z7|8L%)!!A7>4&ljSf+hT03PV#g{aO6!o!Qqwsj4{u+YvrHK>&bKH#N&ke$Z}m(zvy z8iy#M`_jR-^fyNM>ciS})^9@iP)@XX{>hyqNb~z`;XY2X%-60Qlp*Y#g?}-rh z-n^^i_6uGtoXxA#{53B;k#d&!@G3w+qK9k5))F$^w+|D zq7et3@2S!f(BmKtMd6Xh&LL3YsbYQ-MFih;C1joIrGsVtHjDfAEYMB<{-~uVJG{Y7 zX2W^xi`OhCER3h;f^jjS5xGyeV3C(6j!A=*7KaOW18P%uJ>3^*1lWLcb*N- z&_DMf!m+|1F1ZKy&a=SJF}kZMjx4Y}yjEWSSO@Htu=csK$p&p0)ab^3azJ7kaVHXc zF6d)>K<<{x4LfEx1H$QfAY;_K5050cq5by2@Yf&?sI8hXsb0YhW#16bnTt@tO1+w2 zmydPdYG=PIy3bO;`vPXK%rPmA} zm-;auQ7&Emmn~}-MG?n*Aqo43R;o2?t=|1Xt}FRRUB`TW3}G^k;Sw3t`9jh8rHmZv z%e>@mquBt$d?;Ggeht09$1L^lD-KJolsD9W)Qhqx7ZWWX9fIbNy{!NS0%-R<+1Wq8 z59P`@`mE^CV=k#yQv4m9*i#zktNU^oW=j`=2;TO-TsKc%=*zp3suNIwJ>% zy!_3JheY8gA1Wz6HbF?7{QZwY0uO9)c;FwK!2!qA4X)MNu)&Bec@@V>78v1{W-NDu z8D@RJ*Onij1EZYzCAXQx02^g4=H$@QLlNCiR^u8JP+q8kGcJ-5Uh7Xx*BEDorK-8U zG^@;Tq@mflQ*s^5=p^XfX4*j|9di$a6nD@@2=QWdHWju5YIzb}$*_hyh6Fc>31E8l z9(9>AC)|nPn~+m)1G~$Mc}f>?@SBtw>HG>El(V~A7sAN~*DMmhJUI3_bT$!3FYoX| zwf&5$H-UVxg^KdjLd!9KbxO+|mh-?mGAZ~|mIq?Dv-0Fgd10M4n}AaoKYWoRSgUm? z2q^Nu$z;qL1&E_N*-7(vIhUR}j1Mx>-jnw-;)fF3uSR}c3`P)5#&77jpr* z6OV0qv8G>j*5`?NutC<(-TUfXSoB>kw!)hn*fY{MgArfZurrL8bhVFIu+x)7zm$*T zLFD1DUz>j!G23~gGt7((*puJ&ovM_ym~iOeFZ)~M*a_Khss3~XSeUn_M@anAvA^5) zqJ(N6Q1Q1nWYygP7a6F^~fV&-m`kNcVZqe7(H4j78^sn_F88!y=BB= zeaV@ttp)siQ1hEM3kP4=i}XiLlfm9`=VhBh985~-efe4g1O0pImHuybc07I1ivZ+hcW9MB{ z|HVEbffjEP&xEnC!5}03{=`WhnE0FXyZSIcEFGO9^~MBX#t&<$6=qm?B{Pfq|| zE_+>ZO_3kosCvHX$HD`nFPybzk!Oc5#oZ%c+Auvc^ zcA4XIRMC3h##1&=Lg%*3Q)!NpFulGjao$Y~8npS;I~>nzVV%>V^~3`3ysNEBc@HlX zh{*^#CCUTcZ^bq!ta3qS-ZYwPf}Bt->s(&_dscY$l`##OK0U0sXssCcY!?va8`HQ% z|3l?B1x&>039#4;SDmOB=Yi#O*MivNIJ73--9O(W0F6toT%CL^2+7*Jm=0_C;rwl} z4_H4B%zrH0|815NcC85K^1HA@#g#$hfOD*H)y>N!FN+z{>5zrFw=+Q|I12_!nc(wS z2fn1M%y9bVm?3Wh3)EV>cumfL4VK;+Yrc!)fZtDu5#qf$puc&Wv_%^`OxpZzWqgJW z#(lZZc7Kc+8lAeu7re#*lb0&bCj%OI^=`y%{o`|qmL`dJ*}rcBw(raPXSXMi(S3)t zYi}8_-{SfyFXY59rgOhTK^6g)`I2Yl_hk<33#%BvE#km_Z63B_X>3^ew#exPSyoK- zWx)dDI~Ghe<%&dJFbfv%8~oM#CkrMOYE6< zS+KUtdmsAaSumYze37a4EZFcu|NeggiB;Ga@H)(Mr;YZyQfKZM^x=%=HP>o3l+$H@t-kX|E>K^*aY4=(-lmJur(jyJi+NsWDswp`8@*Z@||(WVx`)Ns~6eB{IkH@tl#`oakoX&9Kw z5OY^b9vo57ZRO1i zU)!@6KT6?)eBr+?anbO@IPaH~sbu^Rd>9;f`<@S8w;9P)ea8!(-JV{_kLHGVU)6@I z%5%Ww&o3%fdsv}!{}0a-qFnG23Ei%-pa2Y{DGy3r6M&CQo$A*z_+gmooSx}%z6gFG z+~Q!z1rI1CP|$mh5S@JTXXwZdr*D6~fX{u>!9 zY!o6f;QGS|!F6v~8Au1!?hKn71X4oWmC!vmCo&lH<@NswJpsc0ZSl|sKs1;wj+=J~ zKy|3!n9fcJKs#BLBfB=?Ki9FfYx{g1KpQuu{t*4;KPQ7W5Z#9fz_7$f$iizCz|Lu& zJ*0dYz#g|W=#a}Az{xhuHY1-Jz`tR}gsr+6z(TyU>a~R!!0qXt#Ii6Kz(Z|K%oolT zz-9kl27_S~z+!!NMKb9Uz(qZ24AW&1z=pM-az@Dyz-r98xpAxvz^AEC+rPgG!0-p) z_RG}@z<@V*$&YRfz{OL`ZH@;Fz!W%wp(-^7z*1j}Dz+B=zdpUA`!}8TzjQb4*X%X< zzaFgVgEvh0zp4Agl8hzYzY3r<(Nyj8KbEkVm#Jm{KiFm%tBD=$KYkt9L?GeZzudX0 wwFoiOzZ2XqRGiiHKhKaE3Gc`RK!)KcdoIrdKxY~&KW;nvKN*Z)vN<{MKf(xBR{#J2 literal 0 HcmV?d00001 diff --git a/examples-gallery/plot_drone_in_tube.py b/examples-gallery/plot_drone_in_tube.py new file mode 100644 index 00000000..19cf6a3b --- /dev/null +++ b/examples-gallery/plot_drone_in_tube.py @@ -0,0 +1,466 @@ +""" +Drone Flight in Tube +==================== + +Given a cuboid shaped drone of dimensions l x w x d with propellers at each +corner in a uniform gravitational field, find the propeller thrust trajectories +that will take it from a starting point to an ending point with minimum power. +The drone must not leave a tube defined by a curve (centerline) in space and a +radius. + +Interesting maybe this: +( In what follows all components are w.r.t. the inertial frame N.) +The curve is given as X(r) = (f(r, params), g(r, params), h(r, params)), +where r is the parameter of the curve. Let :math:`cut_{param}` be the parameter +of the curve where the distance of the drone from the curve is closest. Then +:math:`( \\dfrac{df}{dr}, \\dfrac{dg}{dr}, \\dfrac{dh}{dr} ) |_{r = cut_{param}}` +is the tangential vector on the curve at the point of closest distance from the +drone. + +So, I form the equation of the plane, which is perpendicular to the curve at the +point of closest distance, and contains the point of the drone. The intersection +of the curve and the plane gives the point of closest distance of the curve +from the drone. +This leads to a nonlinear equation for :math:`cut_{param}`, which I add to the +equations of motion by declaring a new state variable :math:`cut_{param}`. + +In addition, I introduce a new state variable :math:`dist`, which is the distance +from the drone from the curve. The reason I do this is so I can bound it to be +less than the radius of the tube. + +**Constants** + +- :math:`m` : drone mass, [kg] +- :math:`l` : length (along body x) [m] +- :math:`w` : width (along body y) [m] +- :math:`d` : depth (along body z) [m] +- :math:`c` : viscous friction coefficient of air [Nms] +- :math:`a1, a2, a3` : parameters of the curve (centerline) [m] +- :math:`radius` : radius of the tube [m] +- :math:`max_z` : maximum height of the drone [m] + + +**States** + +- :math:`x, y, z` : position of mass center [m] +- :math:`v_1, v_2, v_3` : body fixed speed of mass center [m] +- :math:`q_0, q_1, q_2, q_3` : quaternion measure numbers [rad] +- :math:`w_x, w_y, w_z` : body fixed angular rates [rad/s] +- :math:`dist` : distance of the drone from the curve [m] +- :math:`cut param` : parameter of the curve where the distance is closest. +- :math:`cutdt = \\dfrac{d}{dt} cut param` : I set :math:`cutdt \\geq 0`, so it never flies backwards. + + +**Specifieds** + +- :math:`F_1, F_2, F_3, F_4` : propeller propulsion forces [N] + +""" + +import sympy as sm +import sympy.physics.mechanics as me +import numpy as np +from opty import Problem, parse_free, create_objective_function +import matplotlib.pyplot as plt +import matplotlib.animation as animation + +# %% +# Equations of Motion +# ------------------- +m, l, w, d, g, c = sm.symbols('m, l, w, d, g, c', real=True) +x, y, z, vx, vy, vz = me.dynamicsymbols('x, y, z, v_x, v_y v_z', real=True) +q0, q1, q2, q3 = me.dynamicsymbols('q0, q1, q2, q3', real=True) +u0, wx, wy, wz = me.dynamicsymbols('u0, omega_x, omega_y, omega_z', real=True) +F1, F2, F3, F4 = me.dynamicsymbols('F1, F2, F3, F4', real=True) +t = me.dynamicsymbols._t + +O, Ao, P1, P2, P3, P4 = sm.symbols('O, A_o, P1, P2, P3, P4', cls=me.Point) +N, A = sm.symbols('N, A', cls=me.ReferenceFrame) + +A.orient_quaternion(N, (q0, q1, q2, q3)) + +Ao.set_pos(O, x*N.x + y*N.y + z*N.z) +P1.set_pos(Ao, l/2*A.x + w/2*A.y) +P2.set_pos(Ao, -l/2*A.x + w/2*A.y) +P3.set_pos(Ao, l/2*A.x - w/2*A.y) +P4.set_pos(Ao, -l/2*A.x - w/2*A.y) + +N_w_A = A.ang_vel_in(N) +N_v_P = Ao.pos_from(O).dt(N) + +kinematical = sm.Matrix([ + vx - N_v_P.dot(A.x), + vy - N_v_P.dot(A.y), + vz - N_v_P.dot(A.z), + u0 - q0.diff(t), + wx - N_w_A.dot(A.x), + wy - N_w_A.dot(A.y), + wz - N_w_A.dot(A.z), +]) + +A.set_ang_vel(N, wx*A.x + wy*A.y + wz*A.z) + +O.set_vel(N, 0) +Ao.set_vel(N, vx*A.x + vy*A.y + vz*A.z) +P1.v2pt_theory(Ao, N, A) +P2.v2pt_theory(Ao, N, A) +P3.v2pt_theory(Ao, N, A) +P4.v2pt_theory(Ao, N, A) + +# x: l, y: w, z: d +IA = me.inertia(A, m*(w**2 + d**2)/12, m*(l**2 + d**2)/12, m*(l**2 + w**2)/12) +drone = me.RigidBody('A', Ao, A, m, (IA, Ao)) + +prop1 = (P1, F1*A.z) +prop2 = (P2, F2*A.z) +prop3 = (P3, F3*A.z) +prop4 = (P4, F4*A.z) +# use a linear simplification of air drag for continuous derivatives +grav = (Ao, -m*g*N.z - c*Ao.vel(N)) + +# enforce the unit quaternion +holonomic = sm.Matrix([q0**2 + q1**2 + q2**2 + q3**2 - 1]) + +kane = me.KanesMethod( + N, + (x, y, z, q1, q2, q3), + (vx, vy, vz, wx, wy, wz), + kd_eqs=kinematical, + q_dependent=(q0,), + u_dependent=(u0,), + configuration_constraints=holonomic, + velocity_constraints=holonomic.diff(t), +) + +fr, frstar = kane.kanes_equations([drone], [prop1, prop2, prop3, prop4, grav]) + +eom = kinematical.col_join(fr + frstar).col_join(holonomic) + +# %% +# Define some functions for the distance of the drone to the center +# curve of the tube. +def plane(vector, point, x1, x2, x3): + """ + It returns the plane equations, whose normal vector is vector, and the + point is in the plane. + - point: given as tuple (p1, p2, p3) in the N frame + - vector: given as tuple (n1, n2, n3) in the N frame + - x1, x2, x3: symbols of the coordinates in the plane + The plane is returned in coordinate form: + n1*x1 + n2*x2 + n3*x3 - (n1*p1 + n2*p2 + n3*p3) = 0 + """ + p1, p2, p3 = point[0], point[1], point[2] + n1, n2, n3 = vector[0], vector[1], vector[2] + return n1*x1 + n2*x2 + n3*x3 - (n1*p1 + n2*p2 + n3*p3) + +# %% +def intersect(r1, curve, point, x1, x2, x3): + """ + returns a non-linear equation for r1, which, if inserted into curve, gives + the point of intersection. + curve: given as tuple(f(r, params), g(r, params), g(r, params)), + where r is the parameter of the curve, and params is the parameters + of the curve + point: given as tuple (p1, p2, p3), not on the curve, in N frame. + """ + f, g, h = curve[0], curve[1], curve[2] + fdr, gdr, hdr = f.diff(r), g.diff(r), h.diff(r) + + vector = (fdr.subs(r, r1), gdr.subs(r, r1), hdr.subs(r, r1)) + intersect_eqn = me.msubs(plane(vector, point, x1, x2, x3), + {x1: f.subs(r, r1), x2: g.subs(r, r1), x3: h.subs(r, r1)}) + return intersect_eqn + +# %% +def distance(N, r1, curve, point): + """ + returns the distance of the curve to the point + curve: given as tuple(f(r, params), g(r, params), g(r, params)), + where r is the parameter of the curve, and params is the parameters + of the curve, in the N frame. + point: given as tuple(p1, p2, p3), not on the curve. in the N frame. + """ + f, g, h = curve[0].subs(r, r1), curve[1].subs(r, r1), curve[2].subs(r, r1) + + P11 = f*N.x + g*N.y + h*N.z + P21 = point[0]*N.x + point[1]*N.y + point[2]*N.z + + dist = (P11 - P21).magnitude() + return dist + +# %% +# Define the curve and enlarge the equations of motion. +dist, cut_param, cutdt = me.dynamicsymbols('dist, cut_param, cutdt', real=True) +a1, a2, a3 = sm.symbols('a1, a2, a3', real=True) +x1, x2, x3 = sm.symbols('x1, x2, x3', real=True) + +r = sm.symbols('r', real=True) + +# %% +curve = (a1*sm.sin(np.pi*r), a2*sm.cos(np.pi*r), a3*r) + +h1 = intersect(cut_param, curve, (x, y, z), x1, x2, x3) +h2 = distance(N, cut_param, curve, (x, y, z)) +eom = eom.col_join(sm.Matrix([h1, dist-h2, -cutdt + cut_param.diff(t)])) + +print(f'Shape of eoms is {eom.shape}, they contain {sm.count_ops(eom)} operations') + +# %% +# Set up the Optimization and Solve It +# ------------------------------------ +t0, duration = 0, 5.0 +num_nodes = 101 +interval_value = duration/(num_nodes - 1) + +# %% +# Provide some values for the constants. +radius = 1.5 +max_z = 12.0 +start_param = 0.1 +par_map = { + c: 0.5*0.1*1.2, + d: 0.1, + g: 9.81, + l: 1.0, + m: 2.0, + w: 0.5, + a1: 5.0, + a2: 5.0, + a3: 5.0, +} + +state_symbols = (x, y, z, q0, q1, q2, q3, vx, vy, vz, u0, wx, wy, wz, + cut_param, dist, cutdt) +specified_symbols = (F1, F2, F3, F4) + +# %% +# Specify the objective function and form the gradient. +obj_func = sm.Integral(F1**2 + F2**2 + F3**2 + F4**2, t) + +sm.pprint(obj_func) +obj, obj_grad = create_objective_function( + obj_func, + state_symbols, + specified_symbols, + tuple(), + num_nodes, + interval_value, +) + + +# %% +# Specify the symbolic instance constraints. +eval_curve = sm.lambdify((r, a1, a2, a3), curve, cse=True) + +instance_constraints = ( + # start level + x.func(0.0) - eval_curve(start_param, par_map[a1], par_map[a2], + par_map[a3])[0], + y.func(0.0) - eval_curve(start_param, par_map[a1], par_map[a2], + par_map[a3])[1], + z.func(0.0) - eval_curve(start_param, par_map[a1], par_map[a2], + par_map[a3])[2] + 0.5, + cut_param.func(0.0) - start_param, + dist.func(0.0), + q0.func(0.0) - 1.0, + q1.func(0.0), + q2.func(0.0), + q3.func(0.0), + + # end level + x.func(duration) - eval_curve(max_z/par_map[a3], par_map[a1], par_map[a2], + par_map[a3])[0], + y.func(duration) - eval_curve(max_z/par_map[a3], par_map[a1], par_map[a2], + par_map[a3])[1], + z.func(duration) - max_z, + + # stationary at start and finish + vx.func(0.0), + vy.func(0.0), + vz.func(0.0), + u0.func(0.0), + wx.func(0.0), + wy.func(0.0), + wz.func(0.0), + vx.func(duration), + vy.func(duration), + vz.func(duration), + u0.func(duration), + wx.func(duration), + wy.func(duration), + wz.func(duration), +) +print('len(instance_constraints) =', len(instance_constraints)) +# %% +# Add some physical limits to the propeller thrust. +grenze = 100.0 +bounds = { + F1: (-grenze, grenze), + F2: (-grenze, grenze), + F3: (-grenze, grenze), + F4: (-grenze, grenze), + dist: (0.0, radius), + cut_param: (0.0, np.inf), + z: (0.0, max_z), + cutdt: (0.0, 5.0), +} + +# %% +# Create the optimization problem and set any options. +prob = Problem(obj, obj_grad, eom, state_symbols, + num_nodes, interval_value, + known_parameter_map=par_map, + instance_constraints=instance_constraints, + bounds=bounds) + +prob.add_option('nlp_scaling_method', 'gradient-based') +prob.add_option('max_iter', 1500) + +# %% +# Give a guess of a plausible route with constant thrust. Here I use the +# solution of a previous run as initial guess. + +initial_guess = np.zeros(prob.num_free) +x_guess, y_guess, z_guess = eval_curve(np.linspace(0.0, max_z/par_map[a3], + num=num_nodes), par_map[a1], par_map[a2], par_map[a3]) +initial_guess[0*num_nodes:1*num_nodes] = x_guess +initial_guess[1*num_nodes:2*num_nodes] = y_guess +initial_guess[2*num_nodes:3*num_nodes] = z_guess +initial_guess[-4*num_nodes:] = 10.0 # constant thrust + +initial_guess = np.load('drone_in_tube_solution.npy') +# %% +# Find an optimal solution. +for _ in range(1): + solution, info = prob.solve(initial_guess) + initial_guess = solution + print(info['status_msg']) + print(info['obj_val']) + +# %% +# Plot the optimal state and input trajectories. +prob.plot_trajectories(solution) + +# %% +# Plot the constraint violations. +prob.plot_constraint_violations(solution) + +# %% +# Plot the objective function as a function of optimizer iteration. +prob.plot_objective_value() + +# %% +# Animate the Motion of the Drone +# ------------------------------- +time = np.linspace(0.0, duration, num=num_nodes) +coordinates = Ao.pos_from(O).to_matrix(N) +for point in [P1, Ao, P2, Ao, P3, Ao, P4]: + coordinates = coordinates.row_join(point.pos_from(O).to_matrix(N)) +eval_point_coords = sm.lambdify((state_symbols, specified_symbols, + list(par_map.keys())), coordinates, cse=True) + +xs, us, ps = parse_free(solution, len(state_symbols), len(specified_symbols), + num_nodes) +coords = [] +for xi, ui in zip(xs.T, us.T): + coords.append(eval_point_coords(xi, ui, list(par_map.values()))) +coords = np.array(coords) # shape(n, 3, 8) + +# This function is to draw the 'tube', which the drome must not leave +def frenet_frame(f, g, h, r, num_points=100, tube_radius=bounds[dist][1]+0.25): + # Given to me by chatGPT + # Parameterize the curve + r_vals = r + f_vals = f(r_vals) + g_vals = g(r_vals) + h_vals = h(r_vals) + + # Compute derivatives for tangent vectors + f_prime = np.gradient(f_vals, r_vals) + g_prime = np.gradient(g_vals, r_vals) + h_prime = np.gradient(h_vals, r_vals) + tangent_vectors = np.stack([f_prime, g_prime, h_prime], axis=-1) + tangent_vectors /= np.linalg.norm(tangent_vectors, axis=1)[:, np.newaxis] + + # Approximate normal and binormal + normal_vectors = np.cross(tangent_vectors, np.roll(tangent_vectors, 1, + axis=0)) + normal_vectors /= np.linalg.norm(normal_vectors, axis=1)[:, np.newaxis] + binormal_vectors = np.cross(tangent_vectors, normal_vectors) + + # Create the circular cross-section + theta = np.linspace(0, 2 * np.pi, 30) + circle = np.stack([np.cos(theta), np.sin(theta)], axis=-1) + + # Generate tube surface + X, Y, Z = [], [], [] + for i in range(num_points): + frame = np.stack([normal_vectors[i], binormal_vectors[i]], axis=-1) + offset = circle @ frame.T * tube_radius + X.append(f_vals[i] + offset[:, 0]) + Y.append(g_vals[i] + offset[:, 1]) + Z.append(h_vals[i] + offset[:, 2]) + + return np.array(X), np.array(Y), np.array(Z) + + +def frame(i): + + fig = plt.figure() + ax = fig.add_subplot(projection='3d') + + x1, y1, z1 = eval_point_coords(xs[:, i], us[:, i], list(par_map.values())) + + drone_lines, = ax.plot(x1, y1, z1, + color='black', + marker='o', markerfacecolor='blue', markersize=4) + Ao_path, = ax.plot(coords[:i, 0, 0], coords[:i, 1, 0], coords[:i, 2, 0], + color='black', lw=0.75) + + title_template = 'Time = {:1.2f} s' + title_text = ax.set_title(title_template.format(time[i])) + + ax.set_xlim(-par_map[a1]-1, par_map[a1]+1) + ax.set_ylim(-par_map[a2]-1, par_map[a2]+1) + ax.set_zlim(0.0, bounds[z][1]+1) + + ax.set_xlabel(r'$x$ [m]') + ax.set_ylabel(r'$y$ [m]') + ax.set_zlabel(r'$z$ [m]') + + return fig, ax, title_text, drone_lines, Ao_path + + +fig, ax, title_text, drone_lines, Ao_path = frame(0) + +# Define the curve functions +f = lambda r: eval_curve(r, par_map[a1], par_map[a2], par_map[a3])[0] +g = lambda r: eval_curve(r, par_map[a1], par_map[a2], par_map[a3])[1] +h = lambda r: eval_curve(r, par_map[a1], par_map[a2], par_map[a3])[2] + +# Generate the tube +curve_param = np.linspace(0, max_z/par_map[a3], 100) +X, Y, Z = frenet_frame(f, g, h, r=curve_param) + +# Plot the tube +ax.plot_surface(X, Y, Z, rstride=1, cstride=1, color='grey', alpha=0.1, + edgecolor='red') + +def animate(i): + title_text.set_text('Time = {:1.2f} s'.format(time[i])) + drone_lines.set_data_3d(coords[i, 0, :], coords[i, 1, :], coords[i, 2, :]) + Ao_path.set_data_3d(coords[:i, 0, 0], coords[:i, 1, 0], coords[:i, 2, 0]) + +ani = animation.FuncAnimation(fig, animate, range(0, len(time), 2), + interval=int(interval_value*2000)) + + + +# %% +fig, ax, title_text, drone_lines, Ao_path = frame(50) +#frame(50) +ax.plot_surface(X, Y, Z, rstride=1, cstride=1, color='grey', alpha=0.1, + edgecolor='red') + +# sphinx_gallery_thumbnail_number = 5 +plt.show()