From b7ccbaa0a0e9110fad9749bb75f800582c9295a4 Mon Sep 17 00:00:00 2001 From: Shilong Date: Mon, 21 Mar 2022 19:03:04 +0800 Subject: [PATCH] add s-curve --- Control/Quadcopter/controller.py | 2 +- Planning/figure/s_curve.png | Bin 0 -> 37151 bytes Planning/s_curve_motion.py | 177 +++++++++++++++++++++++++++++++ README.md | 22 ++-- 4 files changed, 184 insertions(+), 17 deletions(-) create mode 100644 Planning/figure/s_curve.png create mode 100644 Planning/s_curve_motion.py diff --git a/Control/Quadcopter/controller.py b/Control/Quadcopter/controller.py index d0e8312..6e5895b 100644 --- a/Control/Quadcopter/controller.py +++ b/Control/Quadcopter/controller.py @@ -61,7 +61,7 @@ def se3_control(pos_t, vel_t, quat_t, omega_t, pos_ref, vel_ref, acc_ref, yaw_re R_des[:, 1] = Y_b_des R_des[:, 2] = Z_b_des - er = 1/2*vee_map(wRb.T@R_des - R_des.T@wRb) + er = 1/2*vee_map(wRb.T@R_des - R_des.T@wRb) # R-R.T -> log(R) # reference angular velocity in body frame omega_ref = np.array([0, 0, psi_T_dot]) ew = omega_ref - omega diff --git a/Planning/figure/s_curve.png b/Planning/figure/s_curve.png new file mode 100644 index 0000000000000000000000000000000000000000..63579c1e59c7c7e327031edc6caa77ebab222fd7 GIT binary patch literal 37151 zcmdqJjR`+_ z^m}X${^GF_SFw3v{?5i;$4U?NT*t=3#N5Wj@Xc*IJu7QNb2CmBP8N;_w+(D;EUbB1 zSxx`v3oPbV`mC(J*Kyz?S1hDdtx+g^9pn#MhETd83iZWbTI|V7hnUrIdxsbEqcvOR ze=5}zyow2*CazX`OAWsefBAA(oUj({{vA9*vJW#N7C6T@EpBr^(QE%I@K#inIZWZI z*LrNsL69hVDzgopfV8!HZdL@%Yz*HcSF3pau^?Fm!pk(C1R~heY`bb$w~>F*v_;T) zz+avOcoH{+|9yx>dnEz+#PehDC3)nNx>_l8#(%%2O@~H^d}6^Cg1U-)nVVD^<%N8i z!vFvE%Ou{5m2IAD4K+WhXbojZ?p?~JmM6m{f9$`xX^~T(|7)ZABJOx^Wp-xfLv^(f zN9HLb0m7doRc2&kyK@^$Jv zx>c*)1bAZu_f`gtZzPw>-VhG>YxC>PSqrsvE8GY-R@OT&k2NYP>N1s(+iB^M{mHa9 z_1DA0#qN0h?YY*~@$y0R-)f)k4Rf4v^YADnOI%y8Jka!OcHI4|@aY=4#M(&F(eX-N z6sLZmco1vbdd+)`G53qJzHHT-_V)H7qM}Q~KN;fPjtzeV1&4<>c0_R+4(3w2xw(~= zm%k`6do2+})%ioWIh(X{50;kLyX52V|6q5kKTA3K;*;Ci-rx=Di_Pi!XL0;dLqkIx z5=J~}Z`*faLqx>HE{S^M&G&tOK`bzssg&g~AFlkj@B2;b#jg2S`-vZax4hz~^misTSoA)F6!n*qUPUX_OTJC3$B$9s=>O>6J;|o2jb~|Gu#iih}@?@<( z4ViC`_-Q&?jQ->602~_otpJzPt!8+>W|s?iz6Vw=3Xj!`O%jaJKkNYUmq_gW79^7e!{IC9gY6`*MOKsqa}vNx}~-C zp0~ZOk@6UDr|uTI;5Qv9q~YR< z5OCgS)9<4A%JRDHJq8x`l`B75cm{G``)TGax?h}7=jP@voFpeF|6WPWYbBOO&j^;J zc6~TmU1B=gwA`PCGv@vi_K06VAcst?%*x)rePSQnsmR|TFI-nnshywcQ#q>`(CltL{9wq z5i2Wx@^6*g_}GymYpBNc~zx7H2vu9E&2Xnd12wzagCRfw3=CcQc{w^SSfqC{l-EU-$p}kZ|I<%eZ0M~ij4Yavhu3#TU4#Cn%sT< zO$-(dU!{HUz+YL@(F=aKWBp7t+v{wMJA##l=KXZN_lqJUrLpmG!+~rvcqUehuDS&j zglcu6VJ{|xpV46M>$lrhOb`G%N89ss^z>01qh;0%%|X=qEnjH7sum!$_%~}XUS__$ zA}1#|QexiyCsn31C{L?8?y>0z%81s;X)?{x%&QaanVj5WYJ}#U2M=($xVYdvo4u*s ze3BS1yWuBG2EE&wX?$olMhYL@dSuk#OT_f_bmI9h+Z9aVXU}j^I}4qn>AgR6>M`S7 z_fiGl`RnLN%Vq0wz^zAhot*(+z7R+R+zQ_~Ac3lrA{~mt#>MS8xX3NFjtQg^ zXgfQYluLaYZeeeqZo4vYdbAjy>K1gTI+Q`~PliG=dMmZCynK(TR<-Mi9TJ|9?l^Sr z32sr7+_p&>7aLp`%#msLTn?wbJH9>ggVUZ}SZIP83ADZt3EqTP`e%K5HqWVZ9H;6+an<(BfHE(`W&`e=y-q456a)f z#DvYpL@eZP6m>k8ITb$tCJObs+>V*eD?NYmh2=;Kxx2abLI?ZB#7LoGJp`!VS)93@ z^{DnYUDv}+olm%ABErHb`!P$bguBZx(q$jT^YQb?K;#Ha9(;gGy*^f!lAcatKw=(; zp%S(>TGE>?M}2yFy1BcXA``)4GEq_1x1lf=w~HDnZDKhlqLTp-hU z{n~NnGXsk4R=}9+!T2czmOlkg2eXG`n+W6Vmcc$UX%B^gY3^3maVDg6kJy>r77>OxB)MzYKa+T zy*GZkl%LRVVAS&~xBUWnvrD zeQ!~WnLgI;{O}_b`5Wfu=HDKhs;xTUQSyqV%f@0~yEb1ms9E3M?icTV9tE{^d(^zz64vtd zP{8-pXdt_FAV;0YVe@tXITy>;%JbI4i?hiKRo;iFg|1kcSFea^B?B8EiKeEsf7vY6 zL5VV{DrHkGG`KD+E4$EepO*F`%j;hj>tj4|PRsX<`Z8)^FG+>m_(@nav$|wRIzn%>u(Z^H>I+>KNo7c*X6TZwJKaLv z4Dl{?u>WRpzh3ka#)T9I;}Ac%{peA6dYsc$z$Xwn-EgMdw#zTSOHlb5kyANLq1~aS zbzF?K?;!auCwN2$^=lFG38^t}PWIQH#S6y5sy3Uc-BHV7X*|{o`MS+`RYwct34iT= zut5V%HJ_|1H0t-24!xT=xDv%@&ochYR<}3p`Q=Y5US5oKcl~cYdRlhmE*Zh{no#&? zIV6{%0diwfIozYy%P@2MGVkpEF2Ve_wVafcl*kRNFnGeK3jvEM;l}at*F3Jrg-2BofJ|^Um)(TC99Mrd6SA_hBKL$1 zQ2u4Vk;~|tM2d@<3Q5xzcJD=4WF+ZjdBMXejGYyD++@_zgu9VZrefABW_o^p4R%so zc!V~HHzHx!2D8M)T-BdxN0WXetglddP;HLl>@Us^n0RAlc`RqdswO{XWf@n&9*?G@ z+S23QoZ1geoP?kZDnO8=G&DCetqkTVs}&j~qT0->&(h#wI%4^h(ZcxFOY2WBo+=R1 z)6hJzw6#@o+FRChi-(&dbrYQt8h6f76|DB2ht<-bR2O!er9a~E51IFoW~TzS8xyY$ z|D;G)b*KL}cnLi(Lrg+qVq3s>MzzQ&d}eMg1<>I=N?sdzO~-j6z4^BAh3cKrvV#d{ zBH=QpJ)<9S?mHLuq0tw_&*HJPw6x$MXeO=oyW?_D2priSpUl@0O;s;3TX2_)%!nQ3T}C1m9laChE*GID zHujtk)|F86tqk!U_7|m=vvB=#JJ?V1(FyYYY}FS?oLp^qdn`Z}AVJT;p|~_&Uh?MQ zt;?f)#0758wz1@xM-u?(1@wrPq)wyoE;n@&C}D;tN;`8+OI#8kie7# zz?XEz|WX=uz0%*+AT$R86PYhJ^|U{cO~J5}=@z-qmV z`$x=cf5*!YOx=~a$T{9(LzFQH3dR)~13;(Nn%mqc&}%msEzWTq!l(hb8cZvBgPgn_ zP>zhtIiSGlxw!@?HSrtg5t>d{#1&MkTpX)ToOk~|fdQiP!fd4QUR709qh=15;U7A5 z42Dd)RqRjnhm68<%VgItqr z4u-GGti0fXIG_{mEbeX3x6|Fd`={LAJdj$btIT?_&4o&G@Tb09vDtXrxcy{jmFtNE zq!t_nS7Ksf<1HgOX=${Y=H}-6Tkx?fG`59uWLDtje{43UvdRr;pbiE%s1SJ7=f_B;@Y)w9$Hjb=qt*!H(PtkY zwn=IVijFTdqKDA3I;($%FdC8{!TffEfI$1a#BevU)U^Jl9RCw-?c0zi2&~@SwOQzn zms=Ug*%8=SN{?qDV5k}Tp}RAF@kfRgO(n&2sN6o!(Ma~qo1~))V`yvR<@WlsO#$Cn za&h5BO2ZKv1SD3}|t|3%@oa|GMYu+(}uD^P$HGbKA?&#weliONHpgwNso-vBuim0mPO%J8Px!L5**ucObs(bz~6i!`PGgu9@pwn7F0LU0HTyD<_!9CX&&UEx@ z+sVOUYH<;neiJlb5g&o(<>MSJ;O(F-Q89SBxVXn4Yu=eBlsUG`g6uq*o;lUhlS zGa2y(b_rl_bpe&19?k~glCmZAq#QtPc>DG(;o&H1Z)3>d;SCZJ$z+KDUsp3mM{-Ai zicc~9Mjtz+V02s6OjR6fypJYH{#6AT4}3qaumJh*w~9x)tc)A)ESjCY&R=Dsc)wHiExLVZ*4 z_;0cZH9ihb`@sebM3Ip7p8*HgzM+?R&gFrYuz3><_(azkVpMOSC=5k?_4i zMy9Im_D<*O$urgHB~Bi*vC=!v*5>CS*Y#$Xm!C#IHZ|x?y9Pjheyf>!5x|lT+z$8F zLq;vuUfm#prV7WM46SOxgs#R%dmQ7wvgu5_JB~YV9)>0fIwLy=55o=BG8G`)>FG{4 z0=(}lWk$y9KD>o+1$hD0S8ag1Ac$SS4|swsRu6yh95xCZWm|v>lHh)J7;M`uetbOq3`L>(%$5zg*(B9 zy_>9Zh5McryRX7>-i(!60*?43N#&SQGyBMR;O)iv8EHV1i*jjN;?lQA#>7rzfEW4s z`8$+7xqJfytMs0UlR*uH*1+sk8Rt#ql+m&XRsLs>^%#Y<^sPGhoo&EzU%q|&mS;b0 zSM9z1&X<;#mv@p)8HxfsJA32tOetlr}YCtiHbsdB=_QsYl?;zQH7j#;Wd?@A;G&o&1ws`prJ&4>e_ ze{dt+a0KR{yuxeq&lXT4G`;;(0m|+?JUlL2jU--zLI7~I?u7%ideAm7#O$B&l=^QZxcA1&+x(%OQm1+eNl z06Je6EdZ#<)Cpa^$>jo?g-V%~USeKZZ0_rF4oEw}lT}WjOkU0HKqG4gBrx>LHtVPg zse&C*q7Dw+kj9tT7=Ckol`QLLzlJGV#X*eX)bA2Y@BLk3KB-ti+rIKU84E}am+e-D zBf%FJCzBUava)IJ7w39{lJfEdNMOYYjK+A5{e{HU6ZDo}TqYi1dizaK=T@6Ze!hZ*K+pGApw6rvAMEKtrsQN;occ0Jru~sn_$i|0;-mG-md(#?1 zf0P!eQQ;5_5Z4wm+0pKHS z4#HMKv$+YAKZHUXaF0=tLd*=2axhiFK-mKm2*Mj!Cl6&P&>|&tW-?qePruIdN{Q=) zab9H;5I16wkdE-o-(P;e0kZ_d-MgP4xjJ3KHOuWxPtVSf0RVbREeu&m2jMMQ$}L|{ zf;j@=f*P8d{$#6iBJDMVUgjSxxJ<2f6&Lp{&^*X&C+K=?0W;=JnC(ckT%JJ6o2kcC zlnP#)TCbPR;(dE)SO;AW$kVBrMn9ABU*SLXy9>-G1^r^0gMpH>hO%w}^PFO}n=91f zp#nV|KVs%P{QMO?m-V<{DBbwmpG5-rXa;#GC~Xdoj+W3a5WWs}tfL4O?=W)(>M^a; z!FneE2x%#)Dd38dfK8>>q}YPSzx_8kXuFX_s|R=|bZY`FwLS`y0#uoDwjm9i2109&SvFJhpRE1_u)^uQZ&L8{rz0GGXSeZ0 z1rH#Q3P4eG3=F`WNR*h3Uu9-~@%3>5KOB^QGnKPHVC%?qsG^Yys~YqNz4}kro@Twu zA%vWxzjH?fiIxt%t7hpLikGVS+tf~cK zhC}&@GpnmHd3~*&sjP(J%=9~uoc95I`TF_!K}Y=VvRR9D)J7QzgqcLZeT&>^8OE@= zv$2ksm`yK_r{5tn6cQhejg8{p)rvB=flDA{#8Ujq!RDKE9ti8se3|v?xuvD00&wEt zXc(A`fDKoebx9h3x*S=}e`bi2UmncM1A>PMs@79r55pGYoMp=#w%+^*3JnfUMA98; zz8%e<{rz9YIj@(wr1}zyE4Up_ViE2v(4?>`T>nviUKX%xO%+98K52SC@aB#byS2)sI(0JMbLys_?13HA!g_I&gn9j!k zeS>Xnkd%Z(e}X^*Tft3HQ4t#l=Vw&q3qP3=JeoqiJX3FaX>=9Lv_x`96~hLvo-dWx zlAn&!c$(&y6xWAPSWl_{6T;q0|Nn$A4y>LwLU9^kxIfk=*fQ2hz;Hdiy1Lq9E!U7E zT~1?mckNUDLuphFBLjmsAcrpyAMmv%2PdZ+6cqk24i6+^lhdw!V)QK}uM43=E4-2X zTMD>bs07dqrk0oQE1RKTXH-wp5TU%=Z z$PyEON>5K8n8qfbnVx>{=ZBLn7k`(50Sgj2NA$*Hk&$Z(goa$6sjc*$;jK#F82w#X z8gg0|MF}MYN$Nu8dg=a0=9@P{EKc;p(*134#@I2tr~gu48lED42D62P_tba2orJ+hSZIBfpPL!m*~SLodoLT+QH z#{jpi2VdJ?Cm`^Mh#&!C`^|8Hp742M46tfDfbv6QV*F*(fe2>@wliT$z6&I;Qb^Iw z{r#rF!HC+ASk{21c4mRqP6KF*v|dkdZ$!@kCRYp?pFMY-q&^O-ZEgpaD0=7at85#qN$^(|JV%3CY^Lm)8Zmu2SKE&gkVOv8%5lA5ot0QH9a|z1k?BUidLdYK;A8$j|{FA38ghwst4~Gn8?hZs3 zkd`y^^Nldb0|m`u5eQg}!+78ULY~?b9%HEw?f6S1mt631&0OKcMWB&`bf`Bl@SIqq*(GWmp3#Us*MLlIQ9P&@g(x2 z^O$Gk92Bnq%+!8X;XSz=kZ#<1uI%mQ1=R2}&<3DG&cf{75yKOjk->=c*2m@(&j4AX z9l{g?Wl9&&C?ci+BK-s{JqRQm6v6fD3V(TkNC(0Y4u0IqLBI3cBcu%jr&m{3hdPAb zZ_t-<9k`&B=h2TnMy9%>x$vo}sZr+}7w19HUZq5HX~Y${Z3MmMu~_qGwD}oWQ%M|T z=~1EL$=S0YODk1K-7gYzmCpCO(nHN!^yE`Ft_*0-6isYJbe z$#Hi6?swluE#^fOj-}j|hdC49VoLZ0j^;vl*TS~_<`*cA_1EYo_X-_?d}mE=mvnR%-gikftgRlKpe z4Wd8PhO1nitK7{;ivwC(q;5Snk{d`bJ(fQZkW4gVc=PSKkopA zx`2@F^>_~I6)A2>++-i99MDyK{QNkzI;jO+B7tjmStK0!EY~%&W6PjY$P^gEzt{d# z_6wKj$bhzdyZos@YD8XP%J>(%`iXfm|I>n#2l2T)T}N{dgkz7lkANy!U^dQs>!E@CUq=wXrDbGvA^0B}58%qlb)!2c?En4!5t3xH zg*J!{3Igaq4xqaLVT_ZbUQ&F#AwtN^*<1t1aA;I7y(8i^@Cbu+JO}UfGUiu@t>Aof z2!-f1LDz6D66(3lqVKnrT*pbri}3IZ@#-sMB-6#*6fq?TjsEr|zJ2%ZPqC>w2#k$5 ztQFcU`M|nq0J+}ifrbGeWB4ZJn+$X8$otl6YXen@u%7_<<$>OFx!b69)za zD&Pu$Swvtl4!Ec_0S@HVy>krflTb!yJ8VRHRdmX2Xq)}y< zYW=uV>DSke=9loq`f)<$y3wFOKT|BQa@-p{8#|aM#?z9j9UPCkaz>s%J)e83x?p&vslIJ)WijPfI?);tpEoD7)jyx9S^0!z^T+J*#(mE0%*eH!Gt$I< zl3Uwy!W6i@mqv;*dbK}SkM#}GG^8o`a~)#JDa|Qd<-f}!*s^gEi;*CmLA^0P81{)K z&~g5OXA3J0y`R4OSbc7a#ANI@E7 zTi!`)7w3)D#ja-jq6AZBvaahQ8DCcgn%k>a8Je|Iph{F7SHJ4ZoJqA+Ng_q;LxkFF z_g(TizU4iRxpX^I^Y%|F6PCSGPXdo$owFn>a@(~}ZG{#D12VUMf9zaMdzbnJZe|(k zAHyG~zDaEym^rE=a8oJTvT}JkC8B^u1e@&^(cKGMpN--;%&AsBqb;Xu%)hAt`AmpI zsUtUSar3Yub~Qkzu2Z%9oEjPzs z7&PZpuynB~xEPsl(&qjjTBJ5PQMx=%p|4D7((<}3T2r^6lyx^C;1NYN&5<$TOgn!4 zsB{#Gf{qmYj#jo%E7wJ)dU>V2){A?a8t6I%4UVt4?jYCpWSGwu$qn9YWpa;SXB zSG8i3&*kNOkgLdCUr|wUC&&@hY#^^cfd?a;N%`GeOE8ck^Uhr*lWxe=ig+-%xg!Av zMpKttGN!*`wk=YH@x5os22nySXC*W=G<<@By6rpIb(?^!SX8`VxnB?(@yFqC#Z*J~ zMQAm3eAt{^SX?1FQExZjY=`an-CgqZZlM#x)Y`j(Fg*9fHr_s z&K+0fTn(K@Un2hP7Fv^$!q17K-rIFJtQO!G02-|mu&u+%S_zJTCb0EDhusBb24F7e zH`SZK9nKkNRG)6kAXN`~E|0|&%KiL^!FIXt-mU|m-70Dd#OCQbFPu|Q9mhb46%!NN z1noOIJ|a509Vk|V^X=<{ub;KP`Fmk!!NctRsW#+vm_KJE;6@?kAG4SI>T-9tkEyXx zr2R#YC&a15_2u$2P(oKFQFao4Qj#c zcon=6si6H=?2j1p?+@vs;O=Zd+acN?^n{mF4v2!x#Doavc&x0CK6MM8_?NH$@ZAEzxT_Okdtv0j~aqfy?{%?@=Jr^Gp(S14RN-C=00V94N%Fp{lBiOT}NR zvN5Var@yd0RKf?TB%O0#(}k-dht9oNHp>pfcic{@T;WH9hV0@JwOy$G%hD`W>{+M& zUS2w#%YQH!uR6>HOCGM3Oty4&VNL;)Fqh~}C6YJz6A%(&IH;(s>=XhmX$tlfq4Bu~ zfR!{D$c`$zj)l0KfHMV`$&DK~kh$#bm&pn%HIO3YyOed ze#u+k0f%jk-!%O`vz7*B>cbbi_*LYN-vQ!<0~0(nZdm711d`JO$J%{JWT<}!7IY; zUztA#2C`V?c6I_pKn67s7`e2R%qJ?yDmCSR&IG!S20So;fqQ_Y9;NE;SE9^)NQ!%z z${8qGM_@C)YgY2YM`_@fX>@HTnmAz!D%kE3*NEM7Ia4vdb?L;2GK7)EkPBtV)mAT= zm3(y2pvV6@_Hg0Hz@?)wejbv2(Hp`F89>{60-s{>w}*zxbys2M44(6CjRzXyu>;0u zlE1=wbLt7~d_3xo;;21XRXQDn4-D>tc|SL&rs`kx)y9;m)E=e}Hn zb-*&I!uO(Nzcu#f))m&nA%gf6Zzbn$~HTYbAYGhS-_X!dE0`1uE^cIJR0vX+zquLSG8l31k=B{|k;S}1R-6n@$uW7Mo_Hhk zS`+nxyG*&lmm)#xbk=mH0EQfSqpIT)4#UY!<>j zu0&rL-Gib4!xJdpN|`(?Nj9JGsk;EuAA;Oggg5`SZFa{JhMohL;;4pQisQ$*O^c#x zNrTGStz-Xl^G}Z{%EnvT9}+|U|2 z0~9Xpod5?%VxQ?F9)-UI28 zK;PQmqX9*dtQ_tuhA6n5M1K4##<~>6zWOxCn>NdRpF!m)0M-xCD39AIHyRq6n)ltO zwZSr6W2=j=Egy^!dCOwO^N)QXa5wB@My^;NJR0EH_dxSpPY4xcf0ZFq^Gnn9&)W<6 zSEhhGl~hq7g=c;=sXxN1i0y3q*M-BJMG(7Yh{|^9Wx8+EbtQhFww(1J-mf@qce~MR zouSp~t$flq$V-=q=1qxy4!aMQibsg;4I;hJ;P(fh@{hmfwRUygrKhK#3?eALL(dce zxKGqBE;x4&ovgpe;nAR2tJEkBILp;_hb>lbm~}Y`I+l77I@`(PMWf<)ZG90>EDTAn zay42xyZP(kF5O_F@N?-BMM!?Z^Wq-*~8E?Xv|WxusQFdK zbfwTQS5%WYi}60&PA4+t^Q=5}8#T0R(5UV0|9r4+yqu4Ol?nRL_ZJLF$;mt8_Nzne zU<>=MASM!~0Z<$uTu7X5f@#iE{+skW=h@Fk@N|N@=-zFvTiO1#cbPo)RNC(2zLWUK z`#B0sm*PNOH}P#$)I;}C$=5~$@0$+PIyLWw9-!X6tE;QuEd~VyfJfzr{f1`e8^+H; z7d%~6rF1Uiu~A3b)-iLw6U!-U`Op8@{oD96Qy~~~MkHU(1lw8txI@8>W&-*y*iW#) z*?SZCpLgK7MO<)DXvJ1~#CXUan~-8);lYkT+<^%#!E&}~#V!3s`)DSeT4h4x5E1;%pznx55-o#Q1}*761@;N;Y09uR{<@mVu&;3R=D0@ zvCm9MasB>CpnU}O zypaUUUe?j31H(NPmR#$#CPMozY4T3I)XqDum6O@>@~1a@`3cFQfv}+G=!+00MS6? z`~t?`^TSzu#I6o5F2uS|&<_2Ol-Gta-evm=;MoOewctCw;eq(*;<K@f+EV7$JAT$)oOAK1tBG zNg)P-rlJJj4bt!6Wr9WH40vau8%!~9ynIj$^oH_vpxCzFKk-}lt*qm^XhN~M%HKTY zml@QhS^fBG4O41VXT!7^CQ_)lST&;ukDD5$k6jvU@zk+;ZkQ=aX5>3PCo3Z>RPV_h zn;jE;%NNIzy0KsFa>M~(6`=v4{;)x~ywTMK;ZpJaq8)J8K0eqW$#|m&#R8KE{{f?n zs8q`yO*=4aVANNia9pH>JZ%O-4m`XZLIah~_8^g%yPe|;oz^hrRlG5?u;rx$DnsjJh)S4?$Zwl^f6ghSq&MWUx+Zb#Uhr9$q}QR;l(ZM?QRVy&WxzkWLBJ!) z0~`gMU{EYJpX`G7AUc)*!Vsqe@de!^sRxrn8~zEbQ&LYe9tF2Yt#*Rh_^+e&?yjyh zaNT)TA&v}iK}W$7MqT1-wUOcB-}?srtm$B%C2?3=jCV(7x^|%5TRJ+Ry?ethPVS#g z{)Hvm0Ql2ll>(oUl#I;m>hMn}h!`Ge&v&?TYin!i=;*@!YBT~x{*Zm}lN-ZC|K>g` zK@x@ZsAcScY4gQ-!1Pe#!~2e9ZJO0d@*$|Nw2(E$_`_}|3~JLA^p+qCDExf)yb1&` zFf4(g8qD&M;D+GmH*?Ka7l`}%^%F?=Al){#-n*Cx1viR#)jRNh0&vc#(dE)ab9Quu zR~KMhLxP%&^BuUbuv}bRz(2|WlMJUd$)rw$!;JN`3YbHgu)fv2u%s;#>aTy4! zH1)I0c{?)@(+s$TM*^b_hwUnUu41c}NsRPoHY9vad?gT(uf;+8a_%MpfiRdw;ZX6w z&7}=yKz~ZUfzw0e=z zfq{e*GHByc@`i!l5DOR+lCc(mhUzaH!^ADbcLgyY2)Z&dD1g7M4hFp_Hk~@=Ny)cw z=>Xupf#;y$v-9VgbO}WYm$NMROim6q7Xy(34lwc zVy##mG5sT!g<34?6zFq6S}c?&VIiszr~=90`vCi60q9uhh~}>cQW(5_@Da-P77)pt zE{B_=Zl6^Y->5y_$dNCxaQdPDI`jCe%S{IwKrex(g$8{75?+Qf_!yh=qqq997IcfO zubePsd9){_WO;b8-D_Q(o!8pmYG42P5PN?_+HTLmY5;+YSCIg-O=6ab)VM!E9+RxyV2Q9wRNS>Jqf(pI@mYQeGO1? zYijBtLYC^=rguC8+8&WcZ8kqaQ5oHo$ohtMOOnnrU+LoHfRH5QCqT`qE5lo&gdgE8 z!=VhvXx-0MkxxC+{ra|Uh!EbRdF01Bki`_Pwe$kto+N#=LIfLLOU##E4CRP&%n2Mk zth$E18N#axDzaemVR z%poW>1;|)bM?e|D-ShMFg>G-@j(MA^U&SHjGq100*@X(pNz#EN-4?LcB*!<)9sETC zWH30cg0ny`6s+cHdhc}#if8ij@`=vkSDWW%W)u?^I!Ku?F}LP2=&UBJ|dvGeou}n_IV#!}RghlUF z*xJ*Q?cW5|;f`z7`X%K*))|z48|zJRCUI1KMuWv~ix@n3#1v6-uE=HK{y6jTv{D7| zd)PWzNYy=l*~(5@ zmMly3YXIjDFNIDs!cB{McQJ69%iy9Q_f?85Ahrw-maA;JjpzBbo|V*9zkCYV0!9Xs5YEmM`>Fv@Jt{OFAqUkyp=t*i0-9awYcW0KqR89B zVWEcZ)HDLk!oU*Xno*|xyJ+$B5dDCpR$ZfXv#~3=0cBu@2kcf7g$cLaXNZ`D!P>(2 z&)+giVk^)Ai58nGi6R}LiBT&MD$PenYCu7@2FA^Z3>=|Pw%PPVr(OglyF0nuNvxUH zUhx~m-)*^*J|Joa(+MbFvJCI!`v(I^oS*8lcrvP~*--(K$Jlf!kvipn4H5S8C1?XW z;y)sKkKUXCKSa?(Ky7c>;YA>RVs&WO-#Hbq|&FX+9nGVXtSl@EGwWH~*t z$GwAxb>rfX%}GQEO^<3F>|(2Y%l+aaoCkY(DW-2D1@i;+42YFCZ1;CfH3D+?J{00f zW9!s|I$KeG%7~I1SB?HJhAF(@WL0|d%DW3rc9nnoi#3+pjw7<4KET zB95bZqeAYxImNt8{CZw5W6D5+x+d4tZg$0=+Xkeo?XJvO@UwrftLvb_(5pAy#f?fE zA0;JWIO>J;UK10!C1fJ>FJ>+N$PAMxRL!YB_2FpaB#i=ugu*Y(l#Ske`Y#!6H&}!| z)MoS~KOvx8dHEZ*;$JBGFPIB-2IJg2bS&Len><$YruEFXRF~CQh^M{6y<+RXF(?da z{}H0hhr^LOe-qomsb0>#y}Bikr6f3tsENa4(8TBy;|x%|;wLUtHehMbg|Us)5D`mOL_MqIfOx!Z=eqr zHihIE;erXjxJi;)bZp1;Zsemsq~YpC%Ywg7}YpAdP-2pPQKQ2mq_v2A1h4^>vaKsjEo z8GZ{(BlZAYHmSGOMJG`JDIM>_y=M@*3Nj*XqF3dX6-@*4J>R;*RBtrWA%4$yI_D#p zbPbO}P?|e{x{`XAz)rm(Mba~A^sJrjTmT620ar#(z1kaL*$5A4URp@Gs zPuF$NV`l@3V1hv&7xc)lUz=!y`0IeAeJ(8Qfw-=SDC65;MnRj(DJP#@zr%D$5ev7r za~rbz7TLb^a?5UZDPshVO-v9k9u)`$b{(sBii_aB!V)YK;A=pZbOm5yY398>mH8nPFf#!w0 zR64W1u9-dPPc0M&L+Os3hDJ+q!$D5Asz{StgUGZAU|6D*$q+-GI8;HZF9jJx>hF383JStR$UW(* zsi}#)X3xRUr}~=+ZP?CYT0t1HZ^ZL6VF1*Bu0tkx>1=R)TYHK+nuaqz*pa-lju3{NE<7&lE$3&R||pRd>XnbR`egj$)9@9AVNl zn9`@b-Rl1oimDhWsahqZTm1m_K_fI)UwbCxBMMSpc2-{KUrGH)mK7KrteYTy%^k+T z0bU9kUvVf8xluo-3Da}eMtO=Oo=p*pj7?1M+&isMXI>RZbQiVZC3{4$4~9b;56;hI zF2#T%%<&97^2+_1t^*2+O=7I5TxIh`$M}W`;MW77k%9@dA@6j?tD)!oQou+D7BK<4J=tVd?3m#;xh`vL8o{; zHDFj3dO3Lfq_@a7nFaWQO3ryN)wJ-@X55_1u@n=*cHZm_D$%M*-_8R%CA%9 zDwfM&TRLlH^VRnIYk=cv$0C{y4j}0z@&M8R_BC9-@F$COe^OeSNXSI6;A%}&1MY%Z z;N__Rw`&%5A$bZq@pGX}6n{3HyAX+wibeL>udGC*6|XipGX;j|vi;#HxT>&+4n@{S z5r6}COo~6nglbyugmaZMT|dU#y*PrIeY-|You&6Ue{DAsgW1#p{o57~Dg+_Q8vj`i}#{N4kfKnTIKA2Nc86%gDN|EZL z$8aUVf>g+aud~+r-r+Ak&MGYs+o8FCzBnVVFH3nQaysT%V2w0)o$E*(eEIUN(VMBC z=`iA?kD;geIrMRY{V7GG$6C2US8AACm!W9xiHJAehU;R3gHpuEj_P^V344m^wg0Xg zbwNS9rZ#Gcgi!tUx;JnNL=#_cMyM7X@bgkWBOfAVs0=eB5E;|7ak-C^$9yZgr<%se zkWcI~fQyVCt8M>hDpmv@7sgtlY5iwK*Pv5zO=u!$)RkO08F+nU2gLzl-7n1Jik<08 zwaIiS-&V|X`nR(;SCVfI0Yn1>hUSX87BsbdZfTUb7m+>vN4WtCtiH@MH!7BYXaUO+ zGug2up_$k7#m|BW$POJxaQR&|ZqxE^`47%vM0*<{*)0o{X7dj}!$ap^n;Ioj$g=Ju z%leZkx^-$zho)Z{Xox}H9v2HfA!#u0EiCJz{FkUc$EW*K`1rP}uAnGtryJ_u&b>3B zlqkze`TM!0>%qa!yLmE|inr9Jw!>q+{9(LgCp5)l_4yQB)ENVp=spvr5=7i+ZuIw4 z^Hxat6}~*PqSoeW+spD~n~ZJlJ^RM-*+MnZxas-p%_^bd*t-wU9+;x@I~GbPaoZPp zHdmbSJhk^HKVQ3v%R+^%Kny(G$CBcz#>56uHSfgF!&_HuIy&wTTK~e(LY1O<=ba6e zr{ULC{-nP`NN>flM0dbaMsitvMc$W?jG#5o{>)cXxdZLHwX^mHI`=VGie|n8zv%l! zGvT|NLVnZ}9J?$8BUitT;dfFgvfl7wq>)CoKk>20CUeB=!^j{li*)&hOW9E7_(A!I zNVBVz(`vy8lTp62o6?+%@4|=NYRc%rs@A3JuM>~z)g>2=j@{j!Lu@PhP+2G4{Z- z_ewVtG^sbSdwB2YFv#Bht{yo~?I<)m;@`{t?L(SrbW_q_@zpALl1uUm+JhI`^WgGWD)&Yt~9=_VW@%qyc|n9uPkzT5|5kG3|HMh(2$%v-1=ITa@W16y!Pkl zRF(Pd0`JBUA#sJs;+M=PR_^V~4~zTf@cT5oo?h=cHzX~h#OzRHy~?`!k>~Z)oLUYo zp`FMLVI3adq{n?t8a9Hp%dh7j9x(FIMwH||n(DUoqstD_FN-PB;4T-rAX{F0Jy};a zKGHfzIQ^Q{6TQ>HlehD88)sGQZpV$M=gmyd zG?Y%*y=2qexRgWfoqplfGw>xGR|xMDt-CR0R6Yijt#+PGoCY&=ef(XTPPDzYU;Hkm z_rJ9p^7P^Dz7bpWvEtD$%j{Qujr{@LJ?L`slNBG&0GYO@ci5S(=Z{kfztAB0!}=vI z+U@Lp1;wEwV~hp{OB~zV>oyL=yI(Kb+PXM5*RSr)Yynn%xo2B)$dlcb$ID-Zdusxt zNn_96-e!1Bi-F=Bv?dOY&^{hslzpJ+VkTZEDN%C2T;+wy4H#gq{a4d-6R20>UetEt zv#csPqkENJezBqDql_KLQant%zPKPs6m-AlTBV0Cx{O5a=l-cxw4igmV1XRs`cvi~ z4X;~MGV{o3HwtI{B=xyUH`Mn;6xeI>6i9l1M&n1Z(U4gv2#?wkG{!?yJMHtsJfE8pEu88I{EN)%9!ztDf<+Z}^4J150E z&CXqR-E__vt~FYeeb&Cv`^YHi)N13{;n&E~FDVA_>04*?Vr+9vr(@B~R}3gzA*a8R zz*xR>@jdG|>ewB-%j|ESYhkv)r3&p6L%bN7rsnae!4t0hL}&aZyEWoHK-kq*9X7 z9nv94NSAb%fP{22v>+%b3X;;@A>Ap`B@EpiQqod;M&I3c_tWmj{i46$Fn8{`=bm`Z z^ZY}-^4JoIHSLkMiD+L8e+U}^yx8`NOGfwI*gHo>)}X2HPoEnh9at5&0`Wd!AsNVsE8z^D{BN+ zr#0+DLmf+1n+9F_h{%kvzKC)cpdxunHRlW04|<&g3uKZ*+{$O8^HS7SYV6AXc)sB^ zy=I9J+F^>NtlVHY{G>f@>85p7%5$A+w*qO$Ctb}%G9XcLBd{6tPTq{{A+ ziftG6KJ-(u5#>%v+tD)# zA_gD8)oVm^+U2H%hzKAs^)jl-6v|wA{Ad z9aiqW#m=Q{i%{SwfLW1T#KO<(Z_Y0)C&-oC^vcXZ9UN2*a=`!|q`XI<`eJ6&z2WSMO_9=c0zbR=R@eo8y?Oq(%wElcOv}dGLXt*YU`8>ggxQ7N2r+^Rl0u zFyb+D6JU3e%~D5BH4fZb+AT6~9|sGoJ5>zcNt|kHi>85K#lE0H;v#GB^#gqC#Fsz$ zi-z3}F+#sTEI&dn$-?JHyh-72NoI>k;(0^zjW)OnuW_IO#|O+RblGYEP!jK>wN!LI zYjcJ-zLj5kW^ns;VicY1=g#I> zvTUGmHct7vMxBeotXIe&IOE17Q|qMo+#A_>lE2WNz6m2*2i!@cUzw4j6ZBKjq*6ol$hCM&g zwe`abH!B~VUKP!5J#sPa+SQk(wF?0(oV6N1jQGevE6Xjhf{{GkBCLU!R-7-fud_ig zgJr8H(Z?Sn4G$f;)jK`=tuww?8Pdsf3Yqamx)>(lT6?iM^R!E{Hl#9rnoPxonJ?+E zFBKdT9N@0g1SvRwn(~a88bB&)9))E400#r8wC&J zTq&M>0#0DZLgdErI_0 zadpcn>wct!G)D3Ly;N<-G7`>eD99LPhG;k4tKWW+)maUTKWRZl}w-nj>^&qoF1x1rr8Qg_=njLPrzx+T-wi=Qjs&@zLe%N}ZRsj}b~mrcr|y0j zVu?$gQ_{VyfHmImyr>>!Pjl$KclKP8#l(cHA<@l~_G+q<zt8hH{ z!}CP$6-0KY>-a#)y(?7}{rP;$+Pj?4Ktk!|!LG0#J|?#!s^?{)TK#TKm?i4$TfX-0 zXMK}!(vKa>Q`bnJ_e?;9Sk1mIIOALEttfc^98&t!EsElaj+5&IPpPLUz?5+a37M2p zH{7sR8xiBkKM;5JdCv$(!5h!abf`@Wuqu##taz4cwOjCfF8LIkv7%OS<_UE2h||^W zj<7e<9iGoZ&rPBnE9x|`pp#(Db>+u{A!pTSUb{r#b-(QxmE%-afu`nJZ1pmFZ` zS;)ImTRoC72bGRxaL92`Rx_Oztj@?ElbYAhCt*{&#~{Qr`PA*$uvQpgbJ$M5k%v~2 zowRD7xObD+m8;HrJ+D_M=&E@QDNVzfX2zSt`YM)iK>hjz;@`T*`|bTB1T@r*nC#lm zS~^eT++lKid4#G$wH%Spo14ja2I3Sa7mdvX!b-HE2cm&gCE*(``uzAo#vkg&YR-A8 znX4NmvLw<;mJhy}R$V)=b?_Sv)XCX8u&4+#&Kf|Iod`Sz!E}N(w$X5FAbANLbN5bL zrnP*Ii8ALPeO9Vt)HG+Y?sQ?KT1?jCBbMyahvTtRn;yL#PtX9& zooVKeCp9}IlC4DE6M9&I4SF3fM*6=4;|I*SdA1N0_rjP95}aWHk_O|V${JX-@VSDf z7rz1I&zinqi^BZ-%gaRacMb^>x)M*3}RLf|CF$4GMnbVUr$on)XAYSMgvG?TZ5I&EqV5 z5UJu!@;0H-_{3b`<**~Wg4OU`ha7bYWIr)}SC2~8oVE2n`V)}7uyBQ(yif@Cr5!tN zD1hY438L)Zj;zNTh{3@S0BG%sokK+4$@oC0b-de|q%?IW+%8a=N@9lIeieTm=&Y)Z zEKpH9h}6NP(mWO2-~Hr3N^H4CXV&xP#XMRcJ>ye!cW@3S-AiA8aeDUiTg9CfDgX&F zekqF$JgSVMUizh}dtL);>}QqUr*Kf8pcKzi1Jk_|f@Nb6EG$uBY4MHs5mx&koN#70 z?={6oCPuJwUvrnX?Y!0rR$FZs$kq&F9@KUM8M5D3sjgdYh}2nf%G#<8zG!ZsEWMl{ z)~pq{t~~nU#OtT)nDc~?#@M6`M=F0n@kDkpK?cs zN}|r=eEce(!tt;t*id2}L_9!Dg|f60Wb%w6df0~SQ%WV= zqjUVV_E;?ghYC1uBh#h5Wlt-t^-+Ru&Xy^hxgF7six4n*bDGMiP*Rm@-aY%UuN~I6 z;6NWSIr`R9UB{YL#=Wa1|Bzf5ItQ>_fP3%~fNhG6o4?M@83M5_X&9_wBEyE(0qs}4 zHh1Ks6`5uF3j`v{rCaqqVD@=ZX)+C=a>}*gR{yeY>-_;7HsU_%t75=|?4*;yAL` z6Z(iU{(OwpqQ40jcPYgj7yD{hTMg%U?5wIIQu8$jD@)iuv`NpULf@}NJ*R__1*lkk z0bJmNypE|$KEtQ!rQ^GLZYiZJEb@;Mm_dyTFgj-s73L0lkX6Xk8a5bpww+~g<$3Dk+n0ZO- ztU<5K2&H19M_QYr ze%aAsK7wqJ_X@O>}!~_E7zkky;kyFNx-+Khwe>B>uAOJ6d0& zv9&k%ef+8yb3Z-)W+%cS*zm7CtG+Yd+{fI`qm3^*^FpOR8PRwPO%|cM`Qls`#$Vv! z`{QqoIP+bxyia^z>syM*wVt5&RT~BC_pLnXw?58pa1q(dY=}0{l#7NSA)tRi+Uga6 zo@kaCy9KXrY=oATIkr$Wdb*S}pk?pRkUwd*6noIM4ZZfA#~3mmq?;N}*=$to4RsmY zGJdy(8EU6{R$+%^qR?e8(DuhZte}={j+5M~IaH(3(;rz!B}fO)4VIx9cIb4*Rff`H z#JTj97Q;>`@ozr&PtVGe2xkv1;t+`4rIOJoI}3T33WQMF>||>cs8diC8#TrOW}pp) z`ek2#XwK^Rti7zH6Hf%i9}i1Y6=MbSGns1VG&dcora5w$sQTs4?eV>vWW(h(Qo-MO z6Zq4F26h4i_r4bz50n_4T0<)c1* zTFf~g7ZBM)G>d(VXopCw~@L z4rMGK)r@|K2ook~T7piU(eqLvX_47b#12+gdvu!eCg?z3Ya?x>$(s%Y_ZYuuG)Tm? z;z0f4`#@R^(~!;|WZ!joTnY@}NoWQ-pKY;f3a^r!yuLM5Tx~7Vv+S_jB}tt_gp0Ej z;R**ZV)t%b)|0lx&_KuAife256Tm^#0L67x_xI%&-qINGy&V3na0zx42e#XV!4Cdp zMG@H=YBB*vfkK~nU$}i@*KHBQJz>Tzo5cE;)z`KeliTt`Dhy9p?-UR*M?xzoa)VN1 zU+VJ2hK`ZZDE1EKK@&OHoQY@ygd!P4u;WaMj>fl9^Iy)xYX#0AlmO}npz~0#-Bg)6 zA;q`eNh6y^PDgL*?8^@r!Ks1Xf|Q|TUFUw@?}h4i>yX&V4cuDNdlIMYF1nspp03-ihqq(_iyl;TLXBFa6cxqF}Vm&VnKjp&0yePQA`{F&Q|-skg@~ zz=`yPlyE`U7cc8?IYmgF)FEkEha$7oh@jYC!)TECSE$Wx8pD0@GtM{nui6H6*ts1f z-m;@i`>t@`k*nSBZe1t~k{)JsXdjd{n#SHyaI~TQdms4@{HZ_nu*L(lCEMmpPl5O# zy*i#qKWBMRQ#{@X4AmAXW6C)jmNfP2kqhB-v;h$W)D0mPx#bI&Jg}d8A^*D2_rNcN&_Ue@u80eY>I|P| zl$|R2Cur|IBBULQr=S<&i4SdW!P5sfZEFMMuZqL(nJRsfyB0NK_ZL&cs%|V`ZT?t4S$qA^ zL-B#Q>}PMSl4X>Ubwc;sv{*1|%K+?sHnkE&R(lISDqapxoF*4egnU@UZM@BM=Uw4n zQD5!h+t`>DD={oiK^%0Ov3W@?Vl_gp-e!TJIw-&1$tNPOG)zr15+OU=FP)tWQ_<4# zrMfXNf^z(d^Cc5`!>2)g^8jRE_he)HnU$s5WLLgGz4Dc@r}O72z0f4j<#+a~$Hr4B z%_@~TvvMn#*J*Ac*XO~o#&Z#EW?IyxjWro;s?(RJn|?bnAp(E++N3#XstGksjE=C& ze`7OKSaz>*7G#Rll6m+QE$ly&nqO-qPk=$jDv$Gjt@jmGs5RbL^I=KqOM3L3rkb05 z>GOlW%gi_V!(=;bqH9o;I>d`RvPG^6a4+CMX#qce|1z8uw^Yo@jFbFAZ`b-cGXQ@@ zJt`wSmk!Fsl`I)r_t+_VRKJ6r(y@C(+LKJ-lj->eN(B`bJiMPglkWaJ6MfwnY2$+B z-84j{bi_OwuDY2U0$Ez~k3vl3ny8u%5xo5-Z5g{kgPm|EGfz5baSv#^aYp^6Ua@3= zGgElS%H0t1(&q9&dfd98fV{4Z+@3C zpr~uWJTlNJNq*jJD+V%6Oc@@-`K)akLq?pO48OwZ0tvPB-w2=MjyIG06~@gL=uouu zEFTv@o@<{*7kT)=?ceC%JUbG!yVTsZ{;uruvHpARcck@kP$Z``6uXxiTcZDbVXd(w zPbhu~SG;DwX>hOo*P-ED**Vgk$U}8bTe43~Pvjx2g<6i19^5DbVH2t`vo_!rBwTM& z*YTr`%u9>C^yu%Tg0+?`EL=bhKf<|5oxkufTlMg4EHD3(DAy@>SB*ondxrR3c5iO(LsNv~ zi^$K>ZmdtAaL`$)^(v+ud#)O{b>Q%MLpnsHjEUF+l)|454i!3Y&^)X*5%F-1sZ;QyJu2R>!Xay zXjPQzp!18+f6)~u1t)iIY|3zHCgu>6Oe2eemmEDDU%NS@lz`Yn0^I1;?52|t>C>Wo z*SkJ>kJKO?|M^duZRtcAyf{Z_*=XgG=*4IhX&R240fa6;t3S#@*6Sc^X}zXBLGPH{ ziP2fPNeBVH5v47-q!WO0OiUF8&4OV{+B%_W`nZS`*+(V@@Uo(_dCHglPCd$Omel;; z#y>N1ee)0;8IC;G2cg)<_Na_3fg&dNTDj!Jkjb8n3zJF+okCKlal&pPZX>()MTCC* zdgV8~faHwDnaY2=pL#^d zX6T#mCU1U?VAgADmAop$q}%e{7&GrCZl|0FwOw0KaP>!_UUK9s$g z`!J2e;Nc7#Zon37?~U2KTaUs7FS)Z%G(;Ok=u!E#<*7%Q8s2#NoXXAi%tHhs1MCVK z`!*IWCZU0>`7H-;D$%|>sC#1If91jslev)9%^P~6r;}LE z<1r;;bL4jL*Olzn+f>73!+dXh`f@guZ_Xv566-noJ*cpI6GV4DlOMg^IZ5nAl44L2^R)azI#OD`Y?5c2C zbbe(984(A7@9@cG+A(A^FV#=YxD5o;vMNXRo$|nAa6+iv=MWS!Bnr|r9da1Cn^9Ow zS!}z!tNPdt)#HO2-r=le7dE2vJ+ix8T{JZ@d~jUz9qI6_`Qwojdy{oD5Y5M*uO?jG z8u!k27O9SuBS75C<4^!B>qGk^=Ql0}7vI3?&3`7NR6O}eoChE;F`srnMZ-%()}@wy zF0pzAUn%=fm5p4chB}F-KwBTA(VovA6OY?NPA9Iy0e|%HciGho0wB%6ltPOf;aj@x zm#QSjd>pNdUVbru!Xo2q=!bs7By7VHtL0cI?{@>m^AEH9i2#?FEksCDJ=#G-1ig88^}B%-?lK99==}sZd)Cr zIb6%;IVbr`%7-Kl#K)w6=fqC8NjS6u?>mmWFOKgU>g|)$12!`rpkgNQ z+*3 z%E-d?d5V6@A{-6Ys=<-hln~t&5pkBu#FcG?4W)vtDrNQ}AFTp~^zYKRw41+Zx@6B-u zv3X1!$lbmg4KS1+&QN>D2__il(UT6}4yVz^&x!H9msQ8w@6>gbOGGk^pS{2;|Dz0c zfk`laIfv|m@BCjl;$Fz5_*hzkB6O#!$jZp}(w8uUIgf?KkJ)G=P(CswK4EbO;bJck zCa`3a5eMp8pdNm$eav~^*5l2en|}W$8sBgGD6=X?2nwz$l|xXH>k$W?<65z2Zd{uF zzQP7{CL`6cfOX(gNub6qsOy5>_v2bCy#EZMkMp*?M*G?NnZ|pi%VRu%XD?Nqgh|lD znn4$c!$eQgpu&uTzlq5Wa=_qsX<;#E(Hkv+fxTi}-2O*7%J@!--l)%oXtq7|y1@0pIC zC}E@zz@523j{eGOe74^tbWX{BwZN238%ahDKr(jra&aZ(BDiI=>+bg^e$X#R%R&m) z4{yPHR9x{@*uUy17j)}ajOzv3YVYhFazt`d@a@3*w?0Oo{d6) z^HH8AviNkOp=WT~Q8qM?A|VO)q~2rn_kogJ%mS%mPtV|z;fzrF2hl)~ri$?J1Zk?i z3055v0Pv_^{`mSj0wgEgt*;a%Hu z*G_UFluFOt+yjuib4Cd;wO)>Tq?FFEkGl1HtA4J8$$)&{Zo+Fg#(d}Rz64`XOURWN0S0@ zZaabid)R(6h}eXtabfl|=~!ce<$cu|$JOv%)*n*Z;WbB7LJ@3`>BH0pM_2fdk!$?> z(jM~ws`>P)(*e}}0TtVOXDFKUaU;cXoKr7%b(_Ps+oWg3%=yFQ#UdL)1jO7R#_IvU zU-rWO4NhHriNL-#0DSk#7Qr820|tSqX~eAKu{JquOjc3mmfRrk3Em6ElmpsX@mP6q zH0MkUOj*na5je8n^yb?9mZS@V>cFf#EHVfOKi0eU!~LFc8iNikJh*|x7c>Z}w(@5i z8q&{TtTBZPpSBlRFv~^MZ@ylOWxF!5f zHNsCRq79Dy@Q3XQXi?1>9y<4B?g8B9Q!!6Q0Eq=@hm9gxpw?iQQbz2Gn;!G#TfjVH zP^HLshc{d6`m(D|U@zj6y$$*Dx~*61W~EBqc_Pgt`N{CuBcNcG)hMmM{uJPy@TR3w zj~7HlP@b+_`yRF`S4{2)NvTUqfVYWDxV>;Kf|>To1p6E5Gtta*dB?6S{Gb(>mR+ zKiVJ+1jp1Gi%y@Q2}qVg=`@*lzJkm%x)#y zlcqy8rIh9v!V2*;0ns{=WoIMEip+YE1HMqr%8J?4pSQ5a#jqvyKcv2I5s58h*!QL- zbGzY&riEeQUaB5)akY0IsuC|~dzz5$lt8rIsJ?^xc)H+lxccks6cZ%Q`^AT7+^$$p zZ*Cm;$s0p}Mu97pqtr21Km>WCparEV>ex#r6#f@qT0c_Rf;AKjHX@Rblya^42Izu-$M8yYDjB@`iY*~bb5f)@=0KXXuN0+@ zHW%l*Tx*EZ7Lyk4QY^{TpF~8aW(Nn$(hWf5Wm~3)B1zo4(+^UuyD6gLwzb$W#ttN4 z5Z2EH~$2KXI{Bb3MLJRrrE!8G<*PXslLyeO1gHg z=OeM85j3aCNc*==M}ZCOiFG+K$pjC$A3)w6a!M5BYCRk;xu9u~Y;LiPTm}3Xl4OUJ1W1C%HR%03X(N*&~O2}ji z-?dkfFCh6ZJnHr{v0pY+XIPFEy}X%@tcu9D`?k6m$%ce4{ksrJl%c40vIU!&m>0Wy zB1J(xtno-)f1uA-2e_rxfm0I9w_Xek>*{ZINJSdf^ zNeAQqX|gEDs6B`$&C?_A=qTh5iIalQ28f~6C(iuP0F0^poXSSi^QWh0fuF{O)GcG8 zzH}D>8MoNQvoC;)A{+cT?n6uPOiSoj@yBhEBtY5OL0awNY>_0?zcCo7_UTU?Zho>7QDf}M8}F_{X;yJuc&Hm1#F{|) zrM;1qE;X=vtST~B5}-?1Wst9LXwtK_LXTbnlu?Xc#Q9?&?RKVc#{X$5(5puX^)39l z;_{Yx+nDsW-1NCmp;x>gM+SXQoD*S;EM)HHJQG{n|&Q^3}q!}HnJ zrFJ4tEZ_$KSTRB%@g+yzL!e#vsdM&8g8?0^`BeMQLbwg5sU9j@IKdp&Or)kGCk2}K zG?*k=3NeuV)cYWF(abYmeZFM7HNU=~UurH#sp< z?8e;nHIXIuxAoBAIkOs>pdA$Ma6pot0iZ~d=C21_o9D)h^7&VE`3r^LH~YZh@9byi zQ!XFYe-brIn|Z=q#Tx8IaUWT*x1q&Lf0T3tq~(N>1D?_w$ms{s1?ubaK`S&KXVX`a zY8OfY4&_>xw2wDXltZux2px5RQVwdG*jz{9l@n2XOb~w8%6?uP6Ljh%L%-p90=*TZ zD?n{HC82O#vaxyLVNK&rlL>#|II5qZP)B36oN@LA!Cg zSG^bEZT1IN+u!j-W!ewx4qm%QS~?F$eeMI?mU50i457C3UCL*a@=V*a$Jxojd>W?fUgAMGSyhpRl1>-YOF)<}8OT;6TpNLlsfdTJ zvrl13n&gUeb;dsba*e70DPc{j(i+fRbKbn#1++E-1kV-y%yMDXURSW9S(zcgNs2=J z5i(I1zAiDS)qr9j%UbqpGSw$Bva`Ea7z8$i*`4c9NkBeE8)mHB8IeZdH8-P6 zi93GbsU=2v%8URu@~a}Bh}|C;tKpebb0{um!NRR5X#U!aevV#c14XMP1x+(J8ZW$ z!g&SZ3}&fXbIsS&A96S}vIae#2b8)~ zZSH+HD*C8>j6?F&WZnpsi-d59(b5HI@t~DfGXwf3J^e1sQ64aZD6NfB!e+-GH!0vB zWA23%pdC)Z%}HfxidFJ7I3FN)EDsYeoxHjz#;yt=o%sN{<0_%85&=?K8*6LjfZ&D@ zfoV}Kn*`K7TwqaUJL+n0ax1zQ$=;prx)LBZ{NT773JU6Lb8sa@UA_BrKt@Py5>LvG zW^TY39*nQpR-?K$eD?=s>j7?nqcH)E!q6-{No?8g(Cuhkm|(YP5^d)+Y%6ESciK$R z-a$gnyP9H@(ZiWuu_WNtg*W%Mj^}D@QvQ#0Pb>2+^xLDB`0tA^<%a}^)=%t)jG3oW zAt+uABh3<8FoD7!Degr}8+HNq3T9vL@ps8@KDSd~b3GewpYJu={ajoKlC2FP?xVt3 z_Ixe4Z<0C}&Bx!GCq2_vl+$wTT(|wGYoV0wFK*P}>94UXXP*ie`n#r_fQ3Vs{v;F~lVcCi{8bb(an>OP@S8O?ZFcsWw|3Q}Tw zZ-i@#*oyJWK?8FYpx={h?Wa7<$kJ}l3nrFhBHkhs{mgd(tjH1V^%<@cRlyob3ifyN zC{~_Mg3ixOwC-oBtoX}ABHGO!w*5>H^AkoGm`+C+KAoaCV*5DEL6^baNi znsOnU7aq}){3+xUrhx3wSl1VfJ;w5@6^9b3Z?D5RsA#})pZ$xjM2h*@E#KoEEkL=( zHv;K38ig?rlzTD{-22FgpX%X{#|s|hCZ9SWsW_aB?K*q>1QOUETJu<;sii1h^@tn1 zYeiGx7n7ZeHg-7XUZlEh>6k>~twt?+sUweszSAS#+_EzaA9H#|#;AIWQxI0LM_B0Y z=7D$y;M}wXtlfY+2T*DS`u7Oao~_qlC?kZJ2Jckl<*C_z@gL*33I7rOL&IdAVAvkr zH0zzdkNot-dC=@LHG=JCiUa(l2fJPdJMN_Bu2QWG>u?V@d$GXM!5s3Ty>aaP{&5wJ zYMjoUxK#bV*Gb0aSE$ouwSj?O@m_p+Sa1bLDPJ(sm$e{fSx69nQs2<J5D2NJHu3Wm1+jH6 z;*c)QtwL`#y0ATgyAl8LYK?5E{$9-bdRj>4w*-@+QBLJtf34T}* z6tVCYk&Vks$!kz`FOjNXmg(v#Skn-HayCd57|a-mD|Yc1cptq4o~pp5aSWJIY8UIO znYz^C0Yy7XaWY2xFeb+#m{rG3hmilNAmbMm1thO8$2Ob21O4Bn4mc!*-|4Bz@_ULh=KHd{&z?8eZ=+x?ze2Oz)t7Qkc z!I)yO_ZB`ncj6oFG zxIffzdH98#*1kjew|x@RUp}W@0tmAu6GARSmJ2=U`#*QpkGtxJne%)Ik4ji05r;t( z0mlaXs13Sq>DgO66&R!KB^=3uiChca&;Uh*tGr!80oscVpsOsd z&`uyj6c`f;NU2j?W8~t~m@4Sd$uG&EDOfVWKSrnSF+(+k|K13=M&PZ;WGrOV3Drm0 z_iWhB=#_MiCbwI2l|kYt+oBQle-WTqEQ$U?2TArq8p4f>w*2x2o zMI)3PL>?n-OBe+9F_NtY#so?26QfP_AnGoETbhjq6sjx#k3)#N;MZQBjb6$&OOQdt z;~y|Y*WUq+C>`BB*$@q)@Fo%D;EOag=b#*-1g#VLvIOvPJbon@XyMx@^rDD5 z8>uT_qzoC*s^v$t5ba-IB7#9dpU+kUv>aVW+DMzEuJ4uKqk_U`)xZ1uK!7zB^>=rG zpr7%NY^0SlR3P{M0fb>i(a$geLAVj2am_yfQeeX1RszmzZy#?^h+o_GV=k#jFMj<` z6QrLwV#NRQ!boR>FpK?^8RxRPZNvrY}E% z#HIWj6nN#uNPu=+{8fjq$*>6&vYi>$KY;Z*$xw6yTN>Uk-<#=m9E(5^Gu>1FBYTtYTsyFXw5^SNulDO#u zca;cfm4Ac4StN1>nW^vJJgdk0fapDf_86?LoF7rY=OHqdU`e``$a6VIt zD#3#`D>J}yYw zAN_zU?q4T`2EN7X(wc7OzIC`|Vj{GqcY0;au=#Tc3Uci7|JDV>(5YkDNo~uER?SFX zI@sW34P*!C{r`+Lg-n{bH}%f9X~mG5fA4dnWXS?29WRrF<>lYEjPq~)NI&LAVGCi3 z44*>wV%a8m3|c)%{jhCnN*8GIssJtN!^6XLaBfig%KIK@-IYi^q@i+sMDhr!1_Ox6 z8dxbr59OrL(h5keTC+*PASIoEGhF3#vZeMxh8Fmz+wI|-A|&45Vr$cZU^P~S=nIh1#7aLu-Q3AdO`9>=5K^?tDmLX-pT`; zKXv*`CyZGdOH;cv@a2i|vm(L!5=xz|Kd-F3ai)>IDuDpzArSFh0OHtxV@$i_RuE$r zQ+#!D0$n`|gfVLJY1mPo#0MUMV#ZGvP2445lo6_u*SJ0mwB(PQH-HTqP?rlsqx1>{ z&p7w{0%&A(4-XGtKO`c0HCRg<5);JC_|xNEcs5P!n<&)B6ImK8BU3HLbe> z6H_O5chn^y4GM(lM}eydLcP^EVAr>)s1p5^k-U#Li=wlnAN5wVf&kIN$h?TWANaAx2PQtQVmD7+eTH+~t0#y= zN^@p#e6%w{1Cf!DF@RmtgeyWy6PU8u0Czc{9^7n?jI#vbyg=qouah8iYQWNzWyt-j za&(Q9Cf)7+PMpnqz4yrqV$!QDAFFW+G{L(PO!KD%Q|ER53#qm)cVJKs1nTnDvhD$! zQECwC&;a=B#f8`3kpt!o7&+Jo8Dcg34d~a5mEuSPC;sg@V3|g2!1QJc zUpcZ(9*YVXty(K|$29ffmAeDhPY4BgHtOf{7xkC;TN$NxwcW;TRmr?7WAH95Fh(f; z?FNXMRrRa>@ky@*--h@n@$0#3-pJgS2>mCrS6`8t5KbJ?6`Sf@6_D?L{<{qQtk|R! zXNH>l{rwdX|2*zQd=c!G&Ipr}*~Cw_W}Bqx<50VSg94L}aEAru#nRNFYG^{coSuM8mT{*F{%z$wlF! z<_Q9s-j7+#l)!q5>R+cI{zfELDyDf_j5#ICY3ky^9nZ}PeK8Bx#n*qkh5^wp6%~1J zMsn$@5xcTcGkCg}h_L8iv;_X<9hgg}6&g_TZ!!JbvRKhG$i&eXFq?pI9v(lN32r)Q z<2&{8JW6p8I{E+Y;PoWrcYnW~%$(Se?47vw%i?2JJs$`H|NVVl5QQy5JOnRid3{B? zT!+hawzExMPoC}-JralmC=tK$V_p!84Sd54x3jJLOC&Ph3gH{Xyc*T?MLglEQfC|R z6bNBr_t8an`L*)|vU6`7F8HAPZ(ueA2G-x-w8j2wp81P=bElZed9z zq7}VzG}p9MI8x}1o7*e$6nDa4DQvI9UPb?n^;0<8*9A++V#9K!&@(605y)IPqyN48 z_596y)lXJNbHd=z=y@)^Ad7DJ_<(czTl&dcErR3A1#}L*MRuT zUzsh$mj!{~;akL)$q~A4|Ia^@T#eI~${da)uAmSObP|^nlmdKmTcv%d?Zfb9Dkgyu zACOQD24epZV06VB`5e7F?(5g^w6wJU)9x=%(B+ST4A3hE?xesqDHymK|MmuJFi62Oqc})1;D~2eR6jA1Cfo0CgIJEL|4G4vp=_gH7vng zK?XhvKx;GN#uu0fH2y*UM)04@lg{lw=^Xs@SkoC@q|&@TxBhf6ObK-DT?VRj{&v#; z?ik31=HTD}_Gk>CfdpQKPUJ-k#gS<6YK!CRqOI1+@Y~z;mDX&#ECKGO25iMj%{151ULaJhOFEf6!H1aXc2ngN;Lch= zj|<54E?QHB&bwxNbmGsT%i4gST5JKHHH7PHYu5o+XTv|`2xQGJiaC(q(4f#0anqo` znUY3o|HzGGRv8oo)hf~g%CM+P_ZA!GOX&(;FrldY=J|8ea&~q$zI&=! zpefni%L}6o_`QHOcW3v9LRdVtxv43C@t{bT&0mC*C;~X1qsL&je#46BN>z9;O@+!k^tG!fctdd;|CwFc6lZ1tYMnN`aOHLx&rts6JPa{vbKQ8o$ z6(O!A@`s+-`S}^iM_ZejNvyl*B_$>8oWk_=^e`v}lWJ=PM8(AXn*(4l^~QPaj2AD+ zyP;n$HHzTl*1T>`PU%rm_@d9ADMri4%JT8@5(uVHk&%(<8yjnrcfslXgW7u z$J=0: + return r.real + +class MotionProfile2: + """Trapezoid trajectory, limit acceleration + """ + def __init__(self, x_peak: np.ndarray): + self.order = 2 + + assert len(x_peak) == 3, f"{x_peak} is not valid [position, velocity, acceleration]" + T = np.zeros(self.order+1) + T[2] = pos_real_root([x_peak[2], T[1]*x_peak[2], -x_peak[0]]) + x_max = T[2]*x_peak[2] + if T[2]*x_peak[2] > x_peak[1]: + T[2] = pos_real_root([x_peak[2], -x_peak[1]]) + else: + x_peak[1] = x_max + T[1] = pos_real_root([T[2]*x_peak[2], T[2]**2*x_peak[2]-x_peak[0]]) + self.T = T + self.x_peak = x_peak + + def get_T(self): return self.T[1] + 2*self.T[2] + def get_pva(self, t: float) -> np.ndarray: + """Given + + Args: + t (float): _description_ + + Returns: + np.ndarray: [p,v,a] + """ + p = 0 + v = 0 + a = 0 + if self.T[0] <= t <= self.T[2]: + p = (t-(self.T[0]))**2*self.x_peak[2]/2 + v = (t-(self.T[0]))*self.x_peak[2] + a = self.x_peak[2] + elif self.T[2] <= t <= self.T[1] + self.T[2]: + p = (t-(self.T[2]))*self.x_peak[1] + self.T[2]**2*self.x_peak[2]/2 + v = self.x_peak[1] + a = 0 + elif self.T[1] + self.T[2] <= t <= self.T[1] + 2*self.T[2]: + p = -(t-(self.T[1]+self.T[2]))**2*self.x_peak[2]/2 + (t-(self.T[1]+self.T[2]))*self.T[2]*self.x_peak[2]+self.T[1]*self.x_peak[1]+self.T[2]**2*self.x_peak[2]/2 + v = -(t-(self.T[1]+self.T[2]))*self.x_peak[2] + self.T[2]*self.x_peak[2] + a = -self.x_peak[2] + else: + p = self.x_peak[0] + v = 0 + a = 0 + return np.array([p,v,a]) + + +class MotionProfile3: + """S-curve trajectory, limit jerk + """ + def __init__(self, x_peak) -> None: + self.order = 3 + + assert len(x_peak) == 4, f"{x_peak} is not valid [position, velocity, acceleration, jerk]" + T = np.zeros(self.order+1) + T[3] = pos_real_root([2*x_peak[3], T[1]*x_peak[3]+3*T[2]*x_peak[3], T[1]*T[2]*x_peak[3] + T[2]**2*x_peak[3], -x_peak[0]]) + + x_max = (2*T[2] + 2*T[3])*T[3]*x_peak[3]/2 + if x_max > x_peak[1]: + T[3] = pos_real_root([x_peak[3], T[2]*x_peak[3], -x_peak[1]]) + else: + x_peak[1] = x_max + + x_max = T[3]*x_peak[3] + if x_max > x_peak[2]: + T[3] = pos_real_root([x_peak[3], -x_peak[2]]) + else: + x_peak[2] = x_max + + T[2] = pos_real_root([T[3]*x_peak[3], T[1]*T[3]*x_peak[3] + 3*T[3]**2*x_peak[3], T[1]*T[3]**2*x_peak[3] + 2*T[3]**3*x_peak[3] - x_peak[0]]) + x_max = (2*T[2] + 2*T[3])*T[3]*x_peak[3]/2 + if (x_max > x_peak[1]): + T[2] = pos_real_root([T[3]*x_peak[3], T[3]**2*x_peak[3] - x_peak[1]]) + else: + x_peak[1] = x_max + + T[1] = pos_real_root([T[2]*T[3]*x_peak[3] + T[3]**2*x_peak[3], T[2]**2*T[3]*x_peak[3] + 3*T[2]*T[3]**2*x_peak[3] + 2*T[3]**3*x_peak[3] - x_peak[0]]) + self.T = T + self.x_peak = x_peak + + def get_T(self): return self.T[1] + 2*self.T[2] + 4*self.T[3] + + def get_pva(self, t: float) -> np.ndarray: + p = 0 + v = 0 + a = 0 + if self.T[0] <= t <= self.T[3]: + p = (t-self.T[0])**3*self.x_peak[3]/6 + v = (t-self.T[0])**2*self.x_peak[3]/2 + a = (t-self.T[0])*self.x_peak[3] + elif self.T[3] <= t <= self.T[2] + self.T[3]: + p = (t-self.T[3])**2*self.x_peak[2]/2 + (t-self.T[3])*self.T[3]**2*self.x_peak[3]/2 + self.T[3]**3*self.x_peak[3]/6 + v = (t-self.T[3])*self.x_peak[2] + self.T[3]**2*self.x_peak[3]/2 + a = self.x_peak[2] + elif self.T[2] + self.T[3] <= t <= self.T[2] + 2*self.T[3]: + p = -(t-(self.T[2] + self.T[3]))**3*self.x_peak[3]/6 + (t-(self.T[2] + self.T[3]))**2*self.T[3]*self.x_peak[3]/2 + (t-(self.T[2] + self.T[3]))*(self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]/2) + self.T[2]**2*self.x_peak[2]/2 + self.T[2]*self.T[3]**2*self.x_peak[3]/2 + self.T[3]**3*self.x_peak[3]/6 + v = -(t-(self.T[2] + self.T[3]))**2*self.x_peak[3]/2 + (t-(self.T[2] + self.T[3]))*self.T[3]*self.x_peak[3] + (self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]/2) + a = -(t-(self.T[2] + self.T[3]))*self.x_peak[3] + self.T[3]*self.x_peak[3] + elif self.T[2] + 2*self.T[3] <= t <= self.T[1] + self.T[2] + 2*self.T[3]: + p = (t-(self.T[2] + 2*self.T[3]))*self.x_peak[1] + (self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]/2)*self.T[3] + self.T[2]**2*self.x_peak[2]/2 + self.T[2]*self.T[3]**2*self.x_peak[3]/2 + self.T[3]**3*self.x_peak[3]/2 + v = self.x_peak[1] + a = 0 + elif self.T[1] + self.T[2] + 2*self.T[3] <= t <= self.T[1] + self.T[2] + 3*self.T[3]: + p = -(t-(self.T[1] + self.T[2] + 2*self.T[3]))**3*self.x_peak[3]/6 + (t-(self.T[1] + self.T[2] + 2*self.T[3]))*(self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]) + (self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]/2)*self.T[3] + self.T[1]*self.x_peak[1] + self.T[2]**2*self.x_peak[2]/2 + self.T[2]*self.T[3]**2*self.x_peak[3]/2 + self.T[3]**3*self.x_peak[3]/2 + v = -(t-(self.T[1] + self.T[2] + 2*self.T[3]))**2*self.x_peak[3]/2 + (self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]) + a = -(t-(self.T[1] + self.T[2] + 2*self.T[3]))*self.x_peak[3] + elif self.T[1] + self.T[2] + 3*self.T[3] <= t <= self.T[1] + 2*self.T[2] + 3*self.T[3]: + p = -(t-(self.T[1] + self.T[2] + 3*self.T[3]))**2*self.x_peak[2]/2 + (t-(self.T[1] + self.T[2] + 3*self.T[3]))*(self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]/2) + (self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]/2)*self.T[3] + (self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3])*self.T[3] + self.T[1]*self.x_peak[1] + self.T[2]**2*self.x_peak[2]/2 + self.T[2]*self.T[3]**2*self.x_peak[3]/2 + self.T[3]**3*self.x_peak[3]/3 + v = -(t-(self.T[1] + self.T[2] + 3*self.T[3]))*self.x_peak[2] + (self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]/2) + a = -self.x_peak[2] + elif self.T[1] + 2*self.T[2] + 3*self.T[3] <= t <= self.T[1] + 2*self.T[2] + 4*self.T[3]: + p = (t-(self.T[1] + 2*self.T[2] + 3*self.T[3]))**3*self.x_peak[3]/6 - (t-(self.T[1] + 2*self.T[2] + 3*self.T[3]))**2*self.T[3]*self.x_peak[3]/2 + (t-(self.T[1] + 2*self.T[2] + 3*self.T[3]))*self.T[3]**2*self.x_peak[3]/2 + (self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]/2)*self.T[2] + (self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3]/2)*self.T[3] + (self.T[2]*self.x_peak[2] + self.T[3]**2*self.x_peak[3])*self.T[3] + self.T[1]*self.x_peak[1] + self.T[2]*self.T[3]**2*self.x_peak[3]/2 + self.T[3]**3*self.x_peak[3]/3 + v = (t-(self.T[1] + 2*self.T[2] + 3*self.T[3]))**2*self.x_peak[3]/2 - (t-(self.T[1] + 2*self.T[2] + 3*self.T[3]))*self.T[3]*self.x_peak[3] + self.T[3]**2*self.x_peak[3]/2 + a = (t-(self.T[1] + 2*self.T[2] + 3*self.T[3]))*self.x_peak[3] - self.T[3]*self.x_peak[3] + else: + p = self.x_peak[0] + v = 0 + a = 0 + return np.array([p,v,a]) + + +def main(): + plt.subplot(1,2,1) + traj2 = MotionProfile2([7.0, 3.0, 3.0]) + pos = [] + vel = [] + acc = [] + ts = np.linspace(0, traj2.get_T(), int(traj2.get_T()//0.01)) + for t in ts: + pva = traj2.get_pva(t) + pos.append(pva[0]) + vel.append(pva[1]) + acc.append(pva[2]) + plt.plot(ts, pos, label="position") + plt.plot(ts, vel, label="velocity") + plt.plot(ts, acc, label="acceleration") + plt.legend() + plt.title("Trapezoid Motion Profile") + + + plt.subplot(1,2,2) + traj3 = MotionProfile3([7.0, 3.0, 3.0, 10.0]) + pos = [] + vel = [] + acc = [] + ts = np.linspace(0, traj3.get_T(), int(traj3.get_T()//0.01)) + for t in ts: + pva = traj3.get_pva(t) + pos.append(pva[0]) + vel.append(pva[1]) + acc.append(pva[2]) + plt.plot(ts, pos, label="position") + plt.plot(ts, vel, label="velocity") + plt.plot(ts, acc, label="acceleration") + plt.legend() + plt.title("S-Curve Motion Profile") + plt.show() + + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/README.md b/README.md index aad9266..b593685 100644 --- a/README.md +++ b/README.md @@ -81,12 +81,9 @@ Potential Field ![potential_field](Planning/figure/potential_field.gif) -RL RRT ------- - -BSpline Curve +Spline Curve ---------- @@ -98,7 +95,7 @@ BSpline Curve
-Spline Curve +BSpline Curve ---------- @@ -119,17 +116,7 @@ Spline Curve
- - + Dubins/Reeds Shepp Curve ---------- @@ -143,6 +130,9 @@ Dubins/Reeds Shepp Curve +S-Curve Motion Profile +---------------------- +![s_curve](Planning/figure/s_curve.png) Dynamic Window Approach -----------------------