From b926a802860e91c45b70e77df684dcca6e443df3 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Sat, 15 Jul 2017 13:01:45 -0700 Subject: [PATCH] first release lookuptable_generator --- .../lookuptable.png | Bin 0 -> 41678 bytes .../lookuptable_generator.py | 96 ++++++++++++++++++ .../model_predictive_trajectory_generator.py | 32 +++--- .../motion_model.py | 2 +- 4 files changed, 117 insertions(+), 13 deletions(-) create mode 100644 PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png create mode 100644 PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png new file mode 100644 index 0000000000000000000000000000000000000000..256cc8ad198a7b5d63fd93d773c0f38c9adc65da GIT binary patch literal 41678 zcmeGEXH=74^fiixE)YOOq=SfxfHaX_6%;|iLhm5Gh9aF%L_m-tA~h5XMIrRw5_&Hp z(n~_`5NZ+v;XM5QZ4wdR^@&h-%eN?V;b=1Y)qd{37|KT;K=- z-2`c*>{#7HFdYF{JeufG@-YE$D^ktuhi7kZui^+ z#Q@Kfvr(u3&nDjd3AzV7d(GxL=n3%j`&iO%mzVkc`u~0L|1UP|C&Kp|uUEaPOu*5- zw{PECR!(gO`U2;W%MNaDNtdwy^WzEaitgy=a+<&a9>U>-to*rMcw?26qA72bv zuRD!L^32T6n&#%_e$DC*Pb5V@5raXkt*xQgKtF%{xXQt?m3pB-m;8Tvjkc<6P3tUF zNQMLPU!7O2Fm@y3Ly7oBmH0|{&}GJ=*RVAhS5GCOcr_BKC1--8F&eyS&Q z?ann06Y6+^al8%hHr{{yc;#q&ipkN@5fMOh8K}WOACb45odq53&T*7BpJYtTT%4^b zFF`^G9OfbC-Y(Ns)FUG!tkQ0rG_>c;eMI{Y124>#L6Ench9&L>7Z&wa4Hf)uUfd;Y_IW#mh4C~x^-I|Z} zOO>&kMWu^=pk@EYHDXH;lDuowim>68J}&| zrwQC9e>lD zuU~b2Ol*}6MsoxN1s}hD{dx*pUtbTBK_!lE!1oabok300!LybHOPlg?HHe?qTO4vF zh^4*maSLpbh$IeVn3}Hzi)1nP``&ikpDL=ZtNZn3@LCMBP|!hLt9JL8?>iT$diS{W zJ5y-4MS|<2M~|R;kDHw_BxU}GtDsOg{626jsb)FJ$q`D>Q(e55k&zMigsrV@vSPNh zPJwpka&J;ULOS&eo4gPFr9P=q`CAHL7nUX}e@QF$U!`_dI23Ka3^ZxSHmr!oJ#+w~#)(1>G7hbe@jExPOR> zGN@B}`0$^-sJn%dkJzZkF&CE!YNY&_j>`bGKViLTcuS)BY4;ao6dG zpcc{KecD)7anjPJ1Dd-p(n1-Oa3G72bIEzXr8v2e;L9lV{tZN7DU!&}ryhTwFz*uC zyp|m(<~)A?Mfw8@JRXnRco`D2z3l?#kOBkORRtnqYM4{*-4&lk$#wsp&%9|8_Vn4% z6C;Jg0g1~c-4HST$gLRgZ;puxz}??eK)RR*<5$GMKmX3SN^zJ6?LTS4Eq+R#=T}s0 zZu3sMtJRjPk>q5}a+z%Gv6RXTm%#k--~ST<`h^O43y{xkc9nXQ+|Z-frP!A*Ut$9; zth?jK_Om}-Zi}KibWg<<2W0LmV`Jl!bKu9t>;_n0WIX)cmoroE#sAjYx*zfK#fzgU zrOWz<`xJU5bTC8m5JyZ1IpdqIg(NWYJq1!AIy%~6wys>%R}0wpjy-ZU&^rTD(^y{= zlqHG(W#ph;(E$Q_di3b^YsP@1^{};^5N_2d`r-rPlP6E^2?`1ll%%CuK$M{KPPq=*0d^~*Sr{u1lt zDJUrF&QE3%laiK3nD3{Nn}Rq2sBCL%!}Rj!{99jp8p|3sUTm_If7#W(W&94tP3=_BRy7};7l|GMr+6GW6;)^c}MFMb*@77;UVLLlJ{SJEh z6B84|e*L<4v^hpAWKh!4lgPK_nR=OXn@?KETgt7we^l(>ue2LHzyBFTAMBk{1Gkf*A!tYy7h*+W!F(&#c-E*p-cQPpJr|M)L9!+3cvbJ_b(e}2`uh%UUe|{_Y?>%f- zRi09oS*v&M9BmYaNZqA$7$JS89VsM0O&H34viX_!iiQTswQHd`zi%GaZXOtU&GX-`UccX;5R1CtB=t zNQw+3Fe__;^WAo20Vm{?46EZvW%Vh!>lE)3iJQO?(8cvrxXd|GAgY~(v`eqCP?-h@ zh}m4J4I>MioIIVf*q8sGE4C)`wfxO%sjyWE2=dImCVw<3770WjANJQ}YKe&(*y^G_Taf zN}EVQ>(kXj0O&;_iN~dv+v>RhCc_;(x13jSQ;#>S?d%lCXl0PcvgeIS<(dPKG20v{ z4UpI{fSiK46<9~}wN$spi<{?tm%8Fu6$7fey@7Aq0mku~rBU$Kt*{QUx{_VOp2QKC zg2UVmCbAcqS7w7G#Oz`p=iuUiLK99&*9L6TpL#EQ8#}wj2p01e+f=hi*O?mLyLa>3 z&J(!gBXe_aCv1KB^5ruO_Nmf-_|}65f4BDjPrMLRFiKr8YV>LNitYcks4V?R*ji{W zLtZFtd#$$ASgTKsJ$?5uwx~4H`R{F3>uyp;#$DvmTc7$?40ZyJ+%+((V2iZ+ht>X9 z1CW?1$5F}i}F&r-W-KhjwG16=uOfp%7z{V;cFtu$nw^M6rLUZ9cri=hJje`x41 zPcd0No3WOJWu{3Q+I}}r zIv)Coo!a)g1vUD?$WQhCkPB>xy9{UDPR$Ic z%rh0JMpKpcWT4AD06rXyA~>1L6`SRpe&Hhv3O0$Kn7#X%=*MbN`FGCQWyb`sM-VRsZX z^OunxAN5TVMvyWN9WDsL*7S9+q*}xSy%)|Q;gU0PDaC3ZuC|K%KPZFbctdO5fUpAj z&iD4^JOj~uysQszgtht092hZsA?^Dc1?x`Q9~hb1@6HqU){L8+7L!5$6_K#L783NJ z^i=KM?ER}!ApQnl^(FqC4@)T~Ag#4KkJK(Ucc%@vzchw?`9A&<@5xj6)I-xH>TPWdCla{n6ppS^`Fx11koB7=0l*_M;6I?G zvv3r^Y!tvxgv|#Mn?avHx4dprJ{S1&UG;qT=SQAPRU|QTYf@Yrbtxst1DV{L9zW*Pr{}o$aA=zsp z8%QJBp0qMkwz-0-%ppm-OQHu~6S;NcQkvO^$op1IOFQ5A4C~+e@w5K^OgjTvbHLcv zZuCR7OAHJ9mHXj&MwX(pYea-fk-o}k{&BPKnt?G*ZeICxZBV^8v^&1VI-&fxv`7i@ z$ntn+OYxprf-zV58)`{E#8KrUSB@0AfJ5 z^ueah?D70L%P}|d@+1S{Wnw3BpJK@$KMtyBWO41kfv%ni#m-w_0Rc;ErJ5Qr;UM`2 zkez{Lp!;@FPxvu9U0o(>F@!l=N&pMW%IdEd?Un~0z19~Z(|XOQ?9M5t>oeO|1HZqu z#l#Pfu4#22LDrnKaxU{#YXYa1i>NmR3Q|j|1u0Bw|6m4Ue@eI*npVNn!c7_%N z6x>fmU-rch4N@Jbx2D068y1(hNpVcj&@)hR(!25Uj!)228L&8hq@f9I4)l+u=Ux_< z^ZRppI$6G{oH9uH&FE;Ahq61Xlk-zfcTSi4)xwL^cNJpOSwKmR^m2SHfX0>tC56WACKejXmFxD zj`*oA3qc`?wiMu~@SlD>fnZjJG^-yEB3ldp#dCoJS5MBd@I}4O1<;qIu-w~q*O)x& z>R5|Uk5!-br>MNSrmE?3zqU@s{HeM@vodja+bFVWt}#q8(9h9rS13>St{{!WTt;c* z>Y!q6?(MCue64;Zm#;Eelb=%}_m{fcOKeZhz5t2&%xtnUDx5tJQN&iiUUmG`@M_XHB z=5O}m#hgOz>EgSJX*}}6h!@gLFBNET<7jbF=Kb`{c&^6WE)qupBu3)2=jL_HemC5` z*tqkm0}Y++di|!L<-dDFW^1L;L#Oun<`(4(>N5gL|Io+gw)2Kx} z#Mu`zu|DV7H0<7JcffqOif(4M3dw6_4WBVudVU8^T&FfOwyNl z!m@<(5K)&3Q7Q|lUi;XYA{@5AR3FFAj7t_s{iHQ3VOO^4sQ+`i+IpQsK1HwS65kqA z29sBwJgpE2`g5~Lph*jcz9I^gr3ni%RFHK{K_>A{nSe>OXWlcY)Ep z{6a*Ri4q_v$A79@N=2j^m=3JERvy$lCSg^;Wsv1NsqR~&QK`Rw(*@rA{7?RJ4>#|L z*y(L$`G$6W!Z-hhSfKM^tY>oGg-9sed8St4XfS&)Uo({NsgBMRC&EWqI%#gm+V%v> zDRp1wG>46nuvY)_u+qO0Yd8B(_ghGa5XQ`tT75d^--FAG_pwr9I$6LnhpXcM(gKbw zLuxdVCOg>f16cS{iG)b>MOO~PxA<^0pj07EMFT&#Bs@y ztFw^oKfgR>5Me*BK;lT)&@!v#=a0#%zzH*;>mWbrF@s3Ck!Ocw|IBC>{xtrBf((@= z&bj{gl|jcZk#_V~rLA$JFv!|a4Ayx*Q)^Z=s^Va(fI;=!$DCK7>xt-;9WxKh~r>Jj@ z57Gp+M<#163NMxdP0ei_E2WT0$c20XM7+Q)IvOC2S*@iO$kVy*1{>B)6Sor=fN2R) zW`4XPwK4C-P~b_u92^wcnmDi(Hsh_b!VP# z;gsUnKRvVrw(lA)o^zq_I;5$Uo&b8eS&O1qAj-(EQH8fXK3O5G^I4AE+A^%$nq>4l zbwtkxlKCnjngheeiWn;bB)dc`;$LA$=0Ztx$S-r8`b8v35K*=W$^*5bvwWsO!tGD) zhk109_j@q5Tx59}^gIQ_df&`xaAHm5ek_zL$M1bOGpUv~eE}SQq*J0>0pS6)(BHfk z%6Nkk7Km;0P99*=bqwS9gu-mHv9@e#F`pyC^bN1=hUwC~Ipqa?10wwJUVX!Ev|D$#IR} zK9a|s1)+;;Ly>0`GVY-hPo8=XA-cJjuDOP7ZyS~9KJXPW5R?rZZx~!CqimRTX-PI9 zWl$cFj3Ki%hLKUhTWXz{KWFXu2Nq~^wmfj-R0el+w5+^av@vG4;Qt?kioIs3yMAap zmTg>_&HGB+4F%@Ffb>X51uiO_+ciG{#_trrlG5QzME4j zlhv|028Pn!C<}oJC}scjewkf$dP>LE=7iFcmfQ|@&CBDgccnzoali}jwrH-RTfsrr z-JXhbD!<)gaG^i}dwAah+DU|7oQoEjfL@;HxaQ~p4(F4( zIJ1vmeQ4&*9FF?P7*@OazQ0p&Vz%CWASdDbZjuE5ZTG=B>+U-Ja@nls3EW_)@BEY) zzw^*A)i}JoFlcv6j4Vj|=ToD>hMk#-;ounZP@tD^9*4L7Oi+p;A*1}3n@c{da0mwa zuuoj_J~vMlXcPBl>zT^kAYTFP6ftkc_*RaD1n*tZ5moYf4%ckajdM!WFE-t zOs_T4!&MLBV~`Z6GmgDgJmBFrNc9Wb2UXRnCPELNY4Q(V3aeDw$%e6vY}@kyXF9vk z!6`B-V?9I|8CeK3B;=g7#%t7%RYSvF@${alG9jIrl5}CO7An6D6iPxAPg_l(FVNBJUcc<%s>wj7+V>a5?^08#7eXR9XDM2sQCDMxZxS6TCcg1sI+PUgkslFVGa1XyxQlS}B{y%s=Hj?c{GOmMSoy^SVC zw=*cm<~>h+JJ;Y1bh~DK;C_gtB4Y)?tL{d+=EW`ityu$ye zm?czl1K#OS1gFVNszn>|pubGmn0%(b_SQs8n7iWf^;qW;>R85RDtg0Y1OzGESb;zL z{=a4crmxUNi2K|(m7XKN&EHNW63U=BUKa|ACdK>UuH%Vz;PmbRN|TU&@d0YWRsV(% zfC+6iPRuU5Wcnc1V&h~zT&z$DC_}2afa^H-e@+bsC`BJWeE3EmN^)GjRo>0rzS#Mx z+-GN+JxUqGD&uaMtk?d3uf=B_a+{=n64Gxvx>n{>?0qgd0RpT}LYY;WbZ!`Uo>R7M zWqoysLdvzGgn@ctxubTLbOzm8CTtES^ox@14*oD}Bvr;!yZFvgm^>!k2E}?YKScV^ z-MHb|(si)H*;4LZw6^p5H6Ur)dL2y3`2x1d66l8VvUIm32)|W++zl;s1~Q1Qy=5su zIX(eUCIK`9)Vk(x-FQIjPt43T?GNO?1(Ip-8selxa>@IdIovC_@4qSV&7}5p4*4lL znPMPk_zxDpHh%0QzxJzf0B3QeInej|jT>))R~R-2NT!Hdm_vqDZ{J*5C51GRyvU)qvB5N? zvBq1TcZ!Cl2ygjbh;&6WnZ{Y~nux)-YES&%S{g@kvQ@yez)$rsl2b9d$Vtd@`i}b zx=DFk;UY<9!ETC4s76xV_H^JLiGlgXl-l$U3U}-<1p zj=-(=Ol~lps&`wfA<3GIziWWK9AoQ#0N`@MUcrm>_X0ZkmzvP~$r@L1@LsdM*1|}& z5awtU$Vuse{8kBjfGk_Bo74<=RV+Riv087$+kMj$vfOxsUQ?#JAzCwDNJmR{SKFo< zGLUu(8>su$BS(xR*`5VlilcYV=axEq^ZLn<$+F1ilrqtk6_PmiF|7H=f6r9NRTAZD z10>s9wn~d=NI{V#Gxj+Pe_1Tl)>j9|k5kRMh)@HIL4^CR=JT*j&TIu*n#lr}R?_+5 z{&&y4MMM*L-R2eOjXhu_pt(kgqd8C7KjnN>0f1d>!L(Z80-Cso*i%A}n}j(HJP&w8 zLu-^g(0_=?QtZ#iCMdmi&w3dByrAAOu?aYKf86{d_s?qNbjtQGZEl6T# za87gJ1a_VxC0R&zBamHbkK$@4gC7gj&K*0M{7gR=?lG1gv z)`q%0`L_Im1aBtt+O7jKzZo{WB1h=Ayi!u$3UsF6M$%6-%(v2bl zN6HHn>H_V3T)GLqa1Q?hW(Vcs9{TJiQaxU2kA?!?i50S+iJ$eWmluAze=y?w$nhP1 zm1EMN#3or5Gt%o2LxL{6 zZg=Q5oZXV3eAx;V&QDmFU_ZrEmLR!-=1PBPXrcp_)c(i8jc_8qoj$WM%I(WQ@`o5S z{PQ)Ro0XM9Shms*o28cPbQz6<)K)gfy$MTJN-{IMyM%N*J7X&x&SMzZ4nfZ6zPup0 zi7urnm+gtEt6x(N(m0jfxC*vi%-4c9b{Y=&kVRKXzqz{ZRO;_ws3}wu3t1_Ppg`Ae zNq>u7B9hOT%gQGfR^+(V7Lw&z*N$XTDz_-t|<2N@DihnciK zo{_Go`ub>hQao5ioUAO6)#u{$pTO|QCd)0k>C=`4M7@~>J6p8brQ~-f^=8MJu}k(XD#+2~hpzheF8W+VazvGdyNO=?LI1_z)*hF!bk zEBYMrt}(HFmValt{!rDkKcy))PB<_tSuSfbF(ndcdH8aFeLTc8Unr4nU{Qe~XE?tT zWafiBB4t*c1UNDwoQmyDvl$U*eas`u8^&`lLwc?J#WQqN)Ccn6QkdjYIp%K^9MHIY z9UY&sDRc8CYk`}f@|d&3%q2$jY;uAkr2+xIcj`?FZck6wuZ`|Q4D~gXsZ7+Bd&g!x zT)@V092)8!d%L()pDxvBYHw>kSB)x+cYQ>HnRaf;A=*IivCDjZ6gXLyq0IlAQQ6Pn z>b}ICJKzAox(EuVp$C=$PpgzGH+HE`e;b;C@W~L!|v<_&gnAf_!Qqy`YXX+)w%?T&7)749+AIDznlFytTub&YK zSN!p|^fDf$i0K`;N;(1d8FY41+cF%ikL4@*^eK#tN}(ug)dypFd@A%*y)u0o&I&W} ztT^^QT!nMu{|;q=gT2-sG)=o~(_;vzLx|sZaCou|_U=V{RR3vU3Pt1g_NH?FVAq84 z+aLCr;%z|L6e1>!=l!?^xD{a+*bAH~2{EAwcdL<~yq(HL`5Wlo;w}s#18M~;w?}#9 zmX;Rj9Jj749-bfRNbR`II%t`^F@JUiW)pnK5&3PiZ;--n0GJJ$+Wa(D$&bPTA43{) zG&Kcwr@bNeryd*bJoSY?&BV&5vG z?YV#Z=%9alo99{1)0bIq(a)b-%XsEgkXyRT1dh|Z2fF{;if8VF`rCkuU<$YkCatB< zSr8+rAg@jVE0OU8WzRpo?o{JNFi89fTPrW-Fs;f+x(n z(Gvv>NKwJJpXJ>{nFqtEic1-I>{v^+>nf*MKj^&aJN3VHC*s0-w^c+$L~K5UhDOF= zJ1-UfJIc7qQsR`10f)#CG8X{Fz)gEet`M+?1QUp@`~KR{fn>QCf04;2mdckogd-Yp zak%HoY6U6-;ATZERESw0lenkRk@oiOL>Q{f$w^6nZOG+I)A9IpR{s{piV{IV3s?aP zLNDhUO}>{Ji0=0P&D%ElnzB08v{mx0rJ>l{IU6_EZrAcRCD8I)%+Prp&IQm9ay7ifEED|;oUeZ4MXB4zcsd3XGYSG|njyOCS^ zojyAUP0i{IA{*W(_l2x1f?Bs+rSuhm(Y|@}q$J$#Tp9B0L08kc)Z+o7qhbJtiKrz# zrj3yRqZ{{F?O22lG9xL_x&6}fJsXB{zN@{banh4jRo3cbT1f%|+ryJc<=WtL)-tDX zE3sF_X8mJQ`lU6GGX(@==IDkflz^gYUE|Dx*(`D&$cWgr6LTGT&;KXqEO)WT8ZzLN zE&Hrfaa~XR3C)imGNhGW_QS>2>Qd6uYIywFw^|^Z_PgD4^Q;{7^*agBV`gc{Exsw2 zseC%=LtxIJ*xexKeCzCy{*CQYL`*sGb@kADrbZZ3WKn*wTdIe?9NKQ}7PBN^R7*4n zfZ-*VphR=Fz<}71UwPiy31$J1;;267-(yu-x*o}ivS{~K(h^fkeZLGTTOBN4LRorA z1328d8*xBW=FaIAtqwKw{qDqVDrrwU4kWs+*^h#=2rkCbR<#ef>IUH$kb- z-Ec~6R0~ELPF2}w9oak$KVheAAD^xs*AM>o%#f9;H7)ECx^LOAf}=+ZQsY!=NqT_E zf)SvwbNZropzcwlz2EAg@*&9jomnINawjPKQ((*1--uLUlks4k=F)mog>k}(KdLW6 zv6RQ-t#vb%!r|7350N?xaAy>>+wI|AbJXX>w;w=aKufukZa_d@4ih4;)0kO&ONJ6*VR?=Mb_h1fiQlZkG^ZNezalu zGp|cz{YHKU^p>c|Y7EtAL7?t}=~B0K`)6L(qodI>ix-FEzja(n&T-Z!I`8%vSQuOV zS@~a-Q9jDe^B}4~#_l&}@$ZiO#7mG-mc7@9X+D=__Zwu7Y#vrFoy_I9(M621ONR7( zjv2H7T#vn`_QI8uo$+G>i?}b{Jn8|DNV;C8wS=y79}JZG?i2>K&GtNV4%NgvnX7V4 zWv6xvX4i(+hJ=h+O_&+7nnF*}QSn?O8>d?fVr%dtDQKeZ(8}V)hQ-C^NbD}V%vB4E zU|A9W{U;x!I*yUyfHp}U_4f_>cw!+Gqq!g|VMrziMfb2vJHE z^x~<`_}b9|xY*muCo8F(_uYpT$JfW6#?`%;Qe`NIm}+q<_Vq1rKn%nA+S>7c$g?|| z@;)8CRaLY_@!Ru*P7~cc5-Rmrofn#KL*V1Zcm*j}(qQ6YpZp9zeDmrlcD#1!F&f}H< z7`KXh3&KQ&wsKgw1{r&HO|ZM*R=T{5HP7N}dA`+fhO18T+lRh*JUEai*w6!{s!4-#hcqRIEdkztB?h+y*2_0&EcMG@vG?=o64`nRUb0P4f=uB_5T7IL;evC= z>_9#9Ottms1>ERNScq(X`c{0t)t`HkuS1+t=Pmm2)heR8R+%*TR+)uXJGp>Yw!c8?}If- zcg1nnSt_Y<)6Q)91FiHUBbj@nC2}_=(V&PqIS&NEhNDq=f^E5BBjBbYef#k^#QhB>=}Mm9jg%q zY&04MbpFXCE|F(|ykb>Mn65}Qqnr&7l7%gI8N?3$T( zy#YQ&q?mObNQbvHG`Pk>V>#jbv4hi0cikJ)l38b&Zyd;ESV7t;_ZD(hsVE0|RqMRB z^!@XS%l;UX`OQd4IbHya7#2O(*FuF*WZcK(Os@j z{%Y9zR}4OQ#%)K|xcY zrh`sj+gnLqGAQ*hJ3BSfs+5U=Pxhe%*I);60&7%c#n{64C?_S8P#iOmM)#@c-5<|B z`GkXRjklmKhBjHzA%oZKhspw^LzGm%1g>xW2@BiHhfP9y?UtfXe390Np9n`H!p3E# zsAjC^X8h&@u>McGVY{Tr^^^|PtSA{P z3mq%P{pzp;GQ$lv`4pkgvFzR+Ny0FsFG}eoZni3VW_6Gm#?jtpXe8M_4H%_3b*qDavz^_#Raa^kDh7^3P8!C4=8{jD z>D0$-XIf5rp=O2h@;lUu8l>Od6N2xOg~!AgwhHkaoovfqRM>+qBI&7ee)f0bsU;+2 zsjMZZK)SNJz3|0|$OCM;)y&y+v!>3Qwm&Aiy5eUkDG$&qYX+tiD^2o_)~Kds8{&?f z_krnRv1$}8hldL*Ja;H(*v=fAxUB{Ae7`Thx9s4n_fzD>Ba$NG9-@Do>ag8Vjojqr zZezj)tT3PQI)#ekBslzhIsZlZ>c_@K8vXMj7R$}Q`%9b)r>KhuV)X{M!$(SI>CgNlY|s z^p_Cfkf8h?$8MxmW5?uX+geb8@Uu&D#cw$EdCAE1@GE7XGxG7_eT4)T#Ff2ccwTvW z;uKzHFV_GPMYr=%5n3y{H6T;qM zbFa%|DqM&^3c9Tdcl01lA^rBy6ei^%0H@=?K{aWpf@a?(h3Jw3yE0y2R@o(?_HN6L zZW)*dDLiV%-+k$q16`dOu#fj_UP;N2gm5YJpp-)p-kBgGB9RZDCO!KK4-0xqSs*WK zup(sA(tK^`;}w}NUo3_0S7oftsy;0>-|n|}+ut7x5y?Lb(qEQ>)BrbQG)aH**>#C% zeiS|#wGYgk-A52J2ImRb%3GXebiT?%XM7ueOd_a}ros00&CXb&NSZM5Eijesu?p1DgeHZGM}R(T7cK+*=Jw zyoR|B%3GMKq{0nC47tes9-ohuh8AQOqBLCT!k@be8+T}LtfYQvSiYwH#A7KA9sq)i zoGBohhxj*0anY=tbJ9Fl06ZqEv(I^=c#QY;g z*#BT?h_a&Msdn$E!hnItt=;_^k!{jIFdKq+6|;Gx>tHg_OcH*-J_My{2ZZPTO8lchR4+^bVy zZf|XoUiKF__leKfnfF617ak+{>#&Df+IH)W$NIs~{+81T(uDqX=TxLQBVyrSlD}dG zH+LH^r4LrMO;ru*B8{A&HfckwfR2mQiBKakK!0k$f2YQ6<wT{pEtwJI`@xt3CFigHEYtc7x38!I_$xh8iYSAGIE#plefLU31gS>KMR=5wcqu zuNrinV;=!DGn;?+D3!c0ANo7RYFs%~=X#UQ=FcBUBqjKs$?!Bb2+5@#9Kfj^o!h4> z!LZ%18)$J(nXACyd*sY`v8bEtXp_RQiZ2>e^ksFTK%4h^OP-$M)5q`EO#OCE`csG_ zzD15Y^!H<8-ZG2vh4ZA%T3jS1!f9BpV^llSCBULN(5@NSL#xVjbgRM%%c$$k&$jlJ zqNuirO!mzIwym0cEw*2f=Hxv(B4+w?z<1C+qNorpVBgD7B~OKH7Z!y&j>^3HQ3AZ^ zVrH>3C`pnZMqx^zS_U8pbD;()z&$iSAu@}XxI+QsMWdqQ@K~J?~f)tr#4v&%tyErcgFGVLEfpYGqTv!RA{BG10?TVbL1 zN;Q}$`5LpdV)1z9A`1@>mji$*i_1bY5}NJkK#25AUk};F^F>Nsza@7J-gmpl?l6pO zs=Gp`ASM|wHl+ANh_b2fGgZBnJl9*0z)S{w{$#s%4Z27!U70iUU3Gk4HZWzv-ge*5 z^y6JE_$zx#`NJ(K*H_mV!>Sz}S(Sqowu!+9O2L_>{$6FDYwkRb-K z)$7`wo$zePY!7m7bL!&$q5c$^1VrFs*%W-#NjCDHFmeK?G2VtMNN43Lf9)@UGP zYVJS$!2C8>8xWf=@Wep+)ilMK%}Pm!tH5l3kWT5Rm$(9LaTn;RK<$BEV~H7CT}gf9 zqlFIY?CoHp7jZU`FB=P)N#SpzW}1BGjkw#s?Gkyt67RgFKb-#{D!j-umZLj={G2cq zEAK6+s-dS<4`^1A5mA9s?F|MGc6{`yV?Gt5Oc12&BI1lCU2%y9uR@=G|6Z0O+rIV+ zvFR(m0La`r(P>F{V0txufpb8{bEB|F7fvyYe7(jb628f65zbm1}mygogOnAXag;fIG1G1OkS$F zp@oK5a2E*~E@aiyV;LL6JUd>zgxV~wt$XtH0XRdbD=irl*(W9=vpV{c?R324IOu$8 zCy2|!m#cQuhd1$C9+1?nVyxftU)E;$f+At|IV=D85#Zp62p1>g>KOi{QsfEPl#$WJ ziCuYxXI1lm%>rcemUviaZ{N5N;`Yy7)DTt33~PfQ0j&O*Xm5iduW`$&_Ef_&{uVYd z9{U2!VrzA+y-2|Z8LE9=Gi z$Z%L>eb6_f6(QtmsWjksNdXr&(WEJ*rCU)7x=@78A@Pv}$M{IICRSj!O^P{zy)!t% zz_hjSr?RIUzQF`JPXnD_rQIIu2WI)ACR`eRY*x(3PB&O6qbHE7D5p6&{W%ulw_`JK z`stn7)ES6ddU{_Epl$SQ|3UQY<5pV1Y+qEYW^LK)+|HF>+)6=tteIVBT@Ss$5= zq_?GLGjkLmD5oiUdL6dn-bTYdXfp-YQ3TAATjcNlD&dG12k=7ddGt zG?|{ds-MMVs6nm%g}4_oUgKSa&0F5~k-8)0*!IfY*h!K@#PC(~xz-Cf>+US_`VG%zP8;{iDP4CpnJ zWMFzJfR6KZYAIpDMYeKNMtV@qF+ffmdM19&XK&Tid-qEcUf}eg-^g{QcDS(5pO%G7 zuFO~>;9*GD%GnMXPDl~fpPXr$B2Xpa{G7+W zTD4Ci;b&1?A(g(Osqrw0ZtE2eK3c!E&}+S2d_Zz?>xb14ou@-1&!V2NVxNhd8qHsfQB7wP&=wB z8Up8#!o<}Ci8+kNj^2E?I}rLcvGs04v5AmAnd;*@;2vGC4tty1LK>?op{h|LP0EI@ zALCcAOyu4^N&fT^O#~eI!lmA%F^+`*USJR$8u;~OD@u3}_7ht-Cc-3cqd7Hxa^77Nj#O8UflZ8jfu0+cYTlh!`Atkfe?+V7W3g3xHT}{ax7zF!zc!Z5SOo7qf zHzJ*A@)!I2_v#{1np^fuzxSMPKN`M>VP2kfG9$b?U^_8|i* zS^OorMh#ohgwlkvJR$i5+?*#X=eqG`NMmIlGO%EYOrC+P^)zK5MwnR+Z`+;VryG(< zhD_8$qQcKl*riu{#e|9uh+cNDehnet53bJGnZn}2X$Jbfjv6(#(xa*8z}~y5s}lkO zXEWEC%W3T6uPS-RpkRlqT*^aY9+;b-6G`}_C*-2XzYcbxrgEvnO@aGScoYN5oa`x^ zDgU17%>egpE1mC!c;t;zcgyd&=hoxpV&m;bEu-kq0=H*-?GF0#&&#L$Q0C37xPaqD z{b0rUo3Bok0(uB%P$%0jI12_+<}5Igo#1lVjFf`a;{T^V{dmU2bbzQ{aG9 zbN2s(ueS_~GHkcM>6T6@0VzScQwb4imCga_mhSFS5$RL}rMo+&LvrZuA%>25FQ2{l zfA3H4SC2zx=Dx2y&$WJQ?2P>8R#X{TJ>DenGY7QxU%m5Fgtf_)uVhgCC+J|ap&M-QYojmm5MKPbT zP|wR@QlfvxCz?9d{3?zztF!a!uCl5`JvsHyx-Evp+0E~tUrzG$3MzK2gbXkC`|uAy zW$$gu*9|C1E~GqI|9$rhd!xnx!?&i7(wq^EEa|tSQ?FcnAPk=}AKm_XSHE!!gAfpG zFBf{bU>tuTe#jhp2sx6Gq0D;sPIRFD9V@=w-rVGG2XPu?>K8UoMX5ZusPH9#n0w?^ zxieEiUYcb3q@?5pB_g2K-i+A|D&)}6Kt?}mVdn9<)$dwQnA?x{vEBv>`gdC23&g2R^*si$mz=FJFQ!v;D|{$JKH1&`EavV};|Q z@N)U?sKNIdUmfeI=%iL`63T}GwPnaiS#3aD&EZ^E-%<T)g$Q`kvlOy)Azfj|NK9)6?0fO@89HQd?Wo!|fyysIShpM99d( zKwAvrI3AqKCGfSwZ&BJ1#ZsyG^5IgcZF?Lo1CmYiEX4ZP zl!Bn-;;r9-yE`iu!FK-b45(q~ySh#-3)^%P&~K4b%`A0aC1lWIDuu#0$_~-C$HEqgwiYbMOFq+ZW=sOU16oPF z^WUVJyd0tw{f;)YLeyOvR%~id+{MfIi(dEiOuyFdD&IQdkbYCnm7+4SYtH$yzJGN>|<1O>;yDGS)=0%0|CaF!}l=D@X<~g2YLAeM$k_$ zT9_&!43FrFjF!nA4v)O@oMmJxxyOIBxenw%=aAAWyYR(w9$S;727)7|LPM@2CATf{ zV(j#|wi&$8lM}Kw)z>BgO%}uT%q*vU!C>z+Y!;@rbzwASR9Cq%461xhZH!tDFY0$bOCqHcQ-R_rA`Rr`#o*o z7{vzl7cKY2lDegT#1c;Z+G4%7NlQPh=$KW5Hacn{hwe`uAw+v6B(=Ko{Ngk868%n2 zcOuI>`gImAVB8$Q%x`)l|D6^75by=x*zD8?SNpUs9gI4_1U$tPW?eAUVkM2fJV-+{Cl2Ys=X)_GN zg2cX(!kx7l-}u-MVZ6@z4Dx#t)8U<45UoR|s;DrPUi9_{TdKZTFf8#8>H`Q2F zd!>Huo=KY9gFN__8QC3L&3`Q$1YS1hh4kq~gwa&Xp=rPpMO|ti^sZfRE`J!q(wcp` z#5e&0WFC8rb?a?n_}>`x6SIcOiB=^p(qEX0y`7|sMj^4E%r9Z@8#Hs|Oc2ykTs zU{%mb8E<^!-=*$}d zusj)(KIET`UQw&23qATTl1MVQaeSd3O=Ez4iR72a$dmzXahocoNaXgyS7$XPKLwL< zi^s22Jd*XCY^I*sy;drk>f26EPEnbw{R7-0ZQ&iC2M#UmCW2m+1x!%_j z$phur?Z4r!v5jjup<*;zn*9e&J4+GH>#s^n?t3Vj)m~%Gw6jlBoBx&}jSJ>%bf1NgU5 zhKDxXSXJM*s3|ID$P^fgtA2Oc!Yj8+t+DX3<2>$XYR~l03wbfL{0Pm1{sj_qjR=8B ze?y7T_4-2SKXCd!NH$kxz;`-@EYpqB_q(4_e+JE%TU0Ebf(vS zI+j)TZ1|#|$RPsXxjnk!S*R4RHXifQyQUN-g|oB$fIyY&Uou0;u!Gg126x-Cuc62f1&x?`YZf=uvwxxy$sAg?!39$`lqPFWa(F$FT z&jwl^b38xyk5y7C%If`5JA1gKaIa-a>h#>>xa?anib%i}vi{bHeA}xy!b*>_8q=alSxU^HISm)f#PW}bd9vAAe|-U=gcH9 zyr<3_^1}`(Pe!&RNkXq~Yk9jJxisD+oVF>Gi51ouGZ>f3r0ge2V4_)<&7G6hSCh3B z#c6BQ{Ar+Zc*`$K+HuBkNFp%qv0$-c{^)bRWUnh8ppandZeeuYAw0fpb4g>Y%J289 zJ_$lCu1q^{FEv<~I&sOCs;R1Nh)h6JMrz*trtfrj0HaB^(N{xn@ zf45%u>is?>2rzj9>VYcipk|Ct77>!2@$Q|jsH?W#fbblwtuC#;&VJoDbn7esk)4q3 zv+z7djI1}E@wA0EA7J*C*2aZ@h+RQ{!r>@p)`x_C^p~_=&018y0oe~l65HswFT|ri zhfdRP&!-M}XxCizrdH4QTco#p}!z;|?K@8OHHe zx|X1r6rJZ9Hc*^hXVm~-vdt5qN0=r)^V?hWDCyh(8&$U7>K7D*D*C7TS=bsYz$T^K zL@ZUa5U`?(on?#G)RGMv9V6s>6=4K56R*ENB|W%{+%(@(iLt5H^?NVEck zUGk3oZXW`CwrS}!Q)^FrEGAyd;OsKM$)n$+Bg=oaK}Cm}%8ncA9u#w&>Xw(78V^2e z#A#I0&>;NEXxO7J7lQtS={dH_#HVV}W%|pkp{J<=UmzBIm$1eq+xgT#;-eWEF4^0D zIA99tsMfXDDZW6GROIEoyS1NJe^P6=yMvkW^?gN?GHO9#IpK2a&9f14Pr_Q=Dl4DF zrIw(g(zUR^M~m_1L&gD{l^@NktyXu??UrgCVOPWVfq8k1oO<$*-`meOCYwLh`7_MeHUV}L~u z!oi+Pu^B;6X{nu6&Vv(Q&f`x| z*|~M3E1BTHUHo#h!3E-bip|+}zvcOy_cMitS>M|y+PxKK=IFUpgZjD(sVgQ%DVZ+# zEtQtpDzMxlQ+Vr$V_WVp?@r{;hy)!o=OaAzYNMDhPNr(e$e&ht|y7F6|zJbE6!a|W|jBu4x+DX*>OzPe( zq&i32&b%%ET41h(lXP*&{>=SgrNjGdE1Iru#iaMr^+@?8DfugBvkU$ELtz{ywRoqM4O;Bt7)@xbd-r&MOB^z%J{4UKmqw zbh+H%dcB?y56uqvSxcjx{QooaJT=*SmiZ=a#b^Mgc{wdt<>gfuPM^>LB21 zKCN$>ZOhi3-*SqN6T~LK91%}29g9HEt;_Cuio}exABtu|E8?Glxe6*TfBH5ST)fUD zy5J^Ev(&g@g+KlS*XmcX70Ou11nz|h(y6AimXe{YxhlXjeeWgEyzVn=cUrPFC ztljs~i~3&$<>m81ti0uIZ(8hxa4ak}W{Y*LvB?D-RbhGJ-(DfEPyzF*1p19DKe^v4 z+irmppb&es4T5^y#lwxg@^9lY-NhxV(}hHvcb5ZTdZa8*odRIX*}&ZhBV)h5Zl^Fl z_N?-BPQ?|!4Wn4Q&5|5Htmq@uS>!7d7jcUgQdL^#wHQu|cFu%A@L!5}@}TE7-7?Rh z{+~f}9tb)9aS5O;^!_^{B6W7yKqHeJxzB6W7;A`&<2hzn6OGN8E|he>gpTH}=%BE@Nx8i__<9&JwV_%=h`+cRSH)rSGxj+1Z!eGHRe+&Q;!mJu7K~ z3)-1cF{t5qs}H=cY}`i8|CSs@q&G;@*x2pfEF8F=XP_O-6I0#Wi*mV$x!`5l((R&Y zJ+mB8KxxpL!5Wt-KG>S|sh#93Kl!xX=ky2Dnb-Ac^cvH%=%sFeqma<>_1z@l$HPf0 zYuf@`#AuXBi}gQ6hT>Ul__TSSB>bEA&_1`nz!5J>qAQp@zY=s%&LyE%;$HicXizEP z{ne$ATg}voM>337Ohhi%D?LT0rhi~)gRk<;DG6)5Sv2fTn?I>RZf<<=&WIof=)2#i zb+d(siFW5u@mtrqw23(d8kuPG{`m7JV=(8X#4iBlf{>Jn(PfBj5?0vGoHG7wevOTX zb^~Aacq|ZkIc)mpDCb9LR<-$HpnQx7>-IbS{b@Y@>&fht<5TJ8fzr})XoEnTw_t)G zhj!(Y8vey5mo_S?jN2;lLj&CA#aiM(;y^bUlrfdN6HKL0350|^QTx-+Q6xctX(Po{ zhGSzsv)I(Y3EWTw%_DS;fk;`^+)a3tyunl<&^qz%mEW4?`bNY@+;k5Hl z40Rj=1CJYQ6gilcuA@=$Qkq zZs5ANYxX1H)V&~YgX)|=&0KDDDBk!g>u6VWJhkLSsgnhho=lhLfMN|Y#qHPDJPuIy-V^;uYav<8>$RNV$FHwuewN*DG88h7!QVK1*5*rF{l)ox+-Y#ur{ z+R%8l^_=N)1i`yd@dQa9n#-b!m_?hgd|iH>j8F(wZpxQ6$+@AC6leRNPYkMl9`wko z6PnSgyqx__hA^I(q@uq&tXpUs^r=Qg*?CFJ7rK5>Wt&+znG;{-E`BHwAcg1~H2#t0 zSNr`*K$E*ecKrW&Sdi$LIU9!oQiW#2nbP(c?ui(6)CJR-1H0&QbX>O6jAT>*b5OMZ z%htOS8u88!7>k~qqvHMOn*87;RqmzZrigh-7zKwmLj z%~`@T#IpB%&P`uF*p`)iE`YlpyF=gn*V_MrzQakk!1l>uO?6D5mrw7QrofyCa-jxdru{`@Xtj~@)`S+ zM)ysa*P@$45rg)7^+2*J3y4#?5SEBjP9DWc-STf^)A*=gY&gTKs<2$IigLfE?=y8? z0kEat$+umdY!?o#siyf^2+R8|k@O__3P}JGd6IF7I2&1S^_kK%Xeng?j;0L(8?7A*YD1`@5oKD})X6Z<6*axo(CFw8r#@mrIpr6Zl)$rSY{|t&XKOz0Q6lMm z&RPHlLkyde6ULX-PYa6S;43zQe5Z9I~-xFp(0(- zXn6PpzlbZFg3aD;_UzdidkzqG>$J5ND@;9o8j8)|;`1)>0tUnSBy212aEg0~=ptxd zY=KHDjjyi4-wQb$91EmfW@d3zaJi>ro`vd%0%15a2L#Jugl=}Pkq4{T95MV&V(y0p1)@;3t8-BI=?xB1= zA|Od9Bw8e zjgPm12-_8Ob(3ZcX5#yjp!9}@YOd`5Heox;X-UGUqRe1-R_IxD z=i#(IJD*LJZ&iSF*oqG<=^O=qG>nR1 zTs~#<0Ut}Bocz?CkEXo8lw^p7UqiKk!pG12nd0JTtAWTv^(8~K;0Zvk7Cr8c^lRVW zOUlabKHvZQhgZzQg7!tzqWGmg|HW9W@Ip<@t%|)F0S8Is6dB*hK%VxUG$J=sMX!40 zAz;5mbGmKPx%Tl#7cWk4nB;?F3YcoRUHT5rfAOD3{GTZ$8s4!v+yvtq?yXsNCHaO& z7t1hXxP9-w*f3e4d~l#xrPrWr%SFPJKj+Q-6*;!G-`b3q%2 zb?kg9+-j}@xBQ6nGUbm>dGmg?^(iv8C#LZ#=>x5Fp`bb$HS*=R}b0SDhK}QoBcZ#etz0fZOuEer)aMt z@I?rzqBc50AmaO+{>#%PwuYBSv5D#WT7+WY==3|G$Zq1tM`v)^o`WGkWv&gO!fS=& zS(HTaDBzyM*6!Px-mZu~uKxp7iqNC06;uWDz!ejgFN-b&2>Q(ba{!=c5KmWXcWlL` z#K2)>g81CKp&11D-A7?~RM{rW;)&t1#)KLD8vm|q%flSD)PA}2-HAV$0q~PB-R`b% zF!yXkeN(c9?ZH8|6KqRkEthZ6bUAC5EvqVNmOAp42E8#+Q5z02GKTW+;I0nFR2mxa zPw~`!B>sN5-@77qsSKLF(aH%q6sI786_YT8!l3;NPtx;5DvYpAaoAXhIe8vF&e|>D z`UwZz*PJp=b##zsj?i_fX6|I3gqejejug9UuQ^>FRVuzI#lmtrlvGyzsgo+1&YY?| zUu%w;tM9;m&bcSz^LS%(ycRIyp0Xz*qw0jMo(_W0kbQd&WHw|S)~85*vDE^Ub&D7v zUBTCPH(w&M;TPd==xo!B#xslio655DJ^8YgB<;! zskqoHiiPp-j-%Ei{7K^;hsh6q%UqJuHyDyjM#mMYwdc6?u5%#OjEW+~DAI1?xOu`KoVjvWLf$0X}c5Ji^NzIbWS??^ur<(|R*t`@c^lA@9!j zp!weYhyN+II7es%>Zp{`%lFHStIFj8(vp5>TN;rf-08Kp+dHT>buOYFM+SYB`tM2A zLOm8bPC05I`&x8YmP2f0jIZ9{_*3zr(HeOo2dSj}J@f%#DLY>)-RnTwVWw)udbAFnY(xpjY85mGxuRou67a4a> z>nPF=%l@p1b}j0Y+JvHN!8!HQx9@3JS10mvcu(d5r0@6)*c z*9U-7SMqSJhBnRUbEVAiKVv50ZBwmH7yztH5zQ`?(XU5{xx)|8_B&U*hT=1%vzhrb z){9Kd*nuP2tpfqrxleI~}&=gQ}i@tFPwyoPq>`v!Hyr1NYq z($fF$;$sd>qqkIuIysP4C9jT!4eP26v2JFp_sm6J6~AY~GP^=5&m~~3{RNjwt#y}N zqM$7eT`l?Z6iG>kjQvX1ByjbJUJjry$nr#4jle%?097|FR@LHc{|3aZxK7JFgKMUw zWNt1VoL(^6RG2Y1H5b+_i@dvl-b2RDZjKHqGjc1FwKA>a=dwSy>9r~~GQ1E*W_!ck zJjQ?pt<6_Ym89-f;_fh=j;Yq88Wr{8OLn&nhNxd5Za7+sM$DF52bL{s-oiyci(jA^LOPlr zu@FHgQ=R&PjIv)Zm_SOWU~Ta{4DG+__;5*sc{hYVP7-(P%DQSM+vA(_vjw^R#5_h8 zE>45~ht^Ggy2_A1|52*GqV zA(|(nni8ti{nxIBeFb$w+m5)$@?{owoIOXD*}VL|i;cg1#?qd$iqC&_I~;&s9PVd* zc>7j{V09pg#yG&RLQeV3n_rZiVlau~Y)R_->C!N_Kg4}PK!Bc?Z#g=dJ@_Yafpq}4 z%O3h07_HHHNdRCq8YDi=PvyZ00fpZN5t68jN=SW!V{%b%@39h?;c zABE^9A=O6;fw3wh0pXLLhg~6gRYD~@MyCE@s=3RnIMa{HlRx1YVCjOIh|UOC>Cm_o zapDR-4IQlc$#XCrNAbw*S~V<;^RpRFXLU7>QDO6srvi=ex3fU@k|B*)I9WShmBp6jC| zNObbO8R?4wfS%}1aL(uTT6uAPpP*t_Tgg~q{cfIx_jBD=Wc)=5rAocXwEyjMTzERg zyb!hg#rK5i(ijcN|6N~f4!sX3%`|_iZLfFB^;Q|U#EsGh+IIg*jD2@|s$_BTaObiK zWnr$!n)kbGhz3k>Ep?G1ixnPm#?PYanq9xtIK_aGY0IX^+3vM{a!DhZw!GVp z%+luzCkjiBni?2dyg2kaU2e^E4ohOz?e12`azEH%+gYe!Qah?y^Z>1H?6ZL2vDtbe za?6*HisP#1{K3LkX3-A!oG%Kz)m32d=F{prenHrnPK>i_1ed5 zNw;Y1ccT*PSiDw~;kU;ryn-a9Jw3j#AXM|P+_QDr2m?zHw!qfUfRhPoZnq~VMkK9o zcPlO;=-Ga&Fi=wV=S?KCp7~@j{O^)u)&%;pArlvez7no%RSV5e+9hx_A z4N%l8gdy12@U0Y6r!I`XT4ZE+<^8T2sW!B(%6=KJKSa2tlUXGeh=eWUYwf+j6d2a~ zMk^VuQq~;o^3esNU-R1zVHKFyt*gA?gxXTJ8SR!$gclMezA3-D)DkN!;gn`vA1dZsy!gjGBzxxevsha2(Z9Q4PV3YGb=J=SX70B=wCw zg9iUD$tYrPBcP6?p`{K*XHUc?e?-}KtnYyB>e8bYo|AK1?DrE;$G%97@!1x{K1I@EvD{sGUnY`wgF?&GoQat zdB!TLXR6sHg|V?E`hg4#SCd|Hz}^SG0$SifNf*?m+82jrmPw~eQt5GX^)m*t+?HT1 z-Wy7#8QF9pCfOxxe(;|0OUA`*Kpf1j&NFExlDNyE#cq6{d4waQ9D!fn+vlRX6&aPw zqo}rhsnb+zYGCOr(Kgyc!B01l&TWDh!s`w)6KJWZ$mnOD>d%m^uDtxTo1qEyz?6ED)Z z*fiK12~42Sv=8Z8uddA@+uNhk*=y#2X@UOB54|hrc(^|&$C=r8Kw8`I@>3&^j}SMG zUzGGijya(8(m{B(v$?DnS*NF+RYs;q}a3Wk)qG-)N@oP5n3CQ&q^3xPcf=|E6X z!FxS}4V!38#~Nm=Um&3;vSUySni8z1^6MJ)in`nczpKRFfABR#sk;!WLtO zmA>WpR9TeYKQzfMq_|7?XWW`OI1E>+WrBhsqsNYV{MN8qK z71od;in4x-1lKq7&=pqX~F$xmOwKV;6Hm zYGPHDy9G5NUbFl(0myE$IIK8T0r-&Vt`uUOU>|G;;%V`D>%FShuXCQTgc$(oUAWVopC%bvR) zDTv<^g}bFYUnS7_eceJQGrZPbQha>U94a&&eRnL&;*}y`(>=ljL;)aGN%AAG94cWS zF6_>hy4m7%q&xg}lSqVKK-WxrANgN1_uZVEOGX9-y+AyWhIEA^FE-s;j2^rhJ8Ad( zPvI#j{4AR@^ccUhbLSk|nz>ZvGP3$*B;Bzx7VvM8sD-_~ztt4TVp_0*65?RF#2pWR zCO8Ll?1L&MH|V=pz8_MP=2iW;AmTj^yf3%U! z%1WS=GpK#IUEZA<9JS;F#MB{E-)-7nU*!_#T(;ninreXd#&tHxeLKXwM$HD+7UN+ubQH zh#NEqR49EQ!v0tTLRx%9`1ttV^c~Dqjkd>z9rJUN_kIE1kQf$Fijh?N$p+43Nvz`){gBFxY- zgFi+xsz-)kI}}^_BM?#>n$h{Epe&i@3d1*(b&XiW(z1rPvn`u{2xI9n$mL%(Bhi4b z^HVK5K36s(52VOZ@|vPOT-3F#f(Xu%iSK8l;CyDx(^n zBFS$+*_jd8vTe1|X4A`c0;T+}XfF=G>!k4n4R-b=Oi}{{Lb3sn$5yUo0|!?Q#);tAgA&*6bhJm;FS&vY-$m<{1`tCk6QPX6T7ub~f)u0}LpBlgoIL zW>W-~0=@1lkj?me(cye=Dl#EKAp)P4$N$cg=Ik4XBg12{;k1g3kygJ0lrNv!%_nYQ zmQ-KL$K=#-Eum9>rNGyu^0U+53)!L{ZS3|u4dsI&FU14rFpBb-ZfRSgCUxu~`| zv{DDVj_*IGQ1-fjk#Vq8;l>dYYV6Ifbp64ccjV+}IvYll+jkLz)@HjuGz_5%R!+L@ z;vPqGWkxN)z{$M=5V~cqO`etUIi*5@O?bR}a&8InMjeLo|A{>`yvouLv4$?QbeG}bUylL}66X((btadEyAYK!sSWsM}m!RBPm}4?@ zbCqzSUvhx{^SAZ(-nfyM&)~ExCbB^G{{GT*r~-CLko@;e>R$*USN#eK9x!)<^v^ci z^GkQwQXH6*CjSy}1*|>|+W!P7+CKV#=FT$^0|L_oB(`Z0rM6 z2q_R!@#MQ2{Xz5To4Y0T&?iW?8boVACi-XDH{{17YQmKbvO%Rcow+Wf@`_G8#KQJ6 zaD*H$(nepO+44%qtgscKYEQSP>0S*Bn>e*|x-gTJB(3xAuGNR>s5||5BqCiAZjM_#nA}zAD+=)T=!5hDUl+moeHN1-oc9A(Y^&cK|Ma6B&lM@qm^(#6 zLC(s0Vg8SqTpF%9ML;JJiObzg*y4TO36gfzl3{FlhP14Mlx3`(L}0H5`e# z!*uEiQ}{7;D$%}`=o8huaik&hjEKb??pz!MZ8pJ7Ql;squg_9T#Qh}z0Xmi)?ha_4 zSI}ijf~#TSc+w3$5Ug$ur(+`ZgD~2Exx-+^g20wq{;|Se#nVIq zzlG^+IV9`qu}tK^&R=X#nB+HIuN7pFbr{_VmTHYnu=xE+3YYN|IWCZ{fWOfq2TIU` zu81y&s*cl1wc_uf03VwUynImDYzJD}PZetl<7d^h#GMB_+gYd+lXHl3X#@nrSq5bv zmrJkvR4h}hVrk93u0k@K_%c+)-q#3;OTQKmtO?y9!|W5V9D%6kiPa(S#XjA)C58@L zbR|eXM1sWA9Ds2Dm!<4|z841KZs+qO)Ua031?=B^1|>zu#4ocPa9Dul)^P~Isk1EO zGo1Qsi?5tIfy|Qy-@)SydKpr`0FdCEPcoDHh{aUWC(?z?F!te@enJ3D+p3)-%1Z$VFEK+IjoyY z6cZxffhOAnSkX5|T`cKWDC$__PCGLMDZ&YgjCG5>Xd(3eNDeJYiwbn!k&6nCpFP*- zWBEAAVsh(VYtNtjCi74KiY@rJZ>0>LZ%VXqmK75NlQ?!yZr5;EvV3;#tD$p6*C%~* zJ`IW_v#;Xi(yz}TbJdE$>Q}VHfijWd3O3JtV00o#61SX*azH+PuT`=`q*v8@DX*qk zZQHjIneA_Sw_G(U_AN>!Iwn;`+ytqqr1`!U#vJ$ll@6uTALe$Bvg2TrWvSYdrme@{ z2L23(xt*B0rt+d9``z;S9|MBe$DJlv;1^=G091yBXtW%{yj5b7u7efBra(36&54}F zk(6@4Z^*vL^mrV<^D7pg)+(Y}fa|vPn#F7vCF4!U(QJoj1B(jVCqG{6AJ5VR&4R}w z=qu|^FPFb>TyK8{F-)=juUUw}Ns1(PL^Q%3B*^qqJIp_Q$@ZA$KR;6K1(MTdlELG< z6NQygv5WZr|Iq>jSOj;gNuT(?B~E-ca9(Wx=_xBJsNCDl19rP^Rf?!B`Cl%!oEJy4 zpP6-!8VXjQr$g<4TC{xGg+NF-D|T&2Il7s`_{R=E`s#KzKkD^Cl^5?SCsxe|PG1`A z7Yv~R&eOW>)zH?!+^Ng`{kr1G1hDac&gwRPo$CVONfu)k>-$tx zoWd>-*YNN-sxH&vM_PpxT%4o$3RJXjN`u3PIA`-dyD3_+j+|P$ez8LkCXh{q@t3U(Ba=-aHKR4 zt5aoJg{Cz5vwGC;k~f@-d(`F3okECu5L+dq|5Tt@d%PpRAwUEQ=bdYjO zVs`g(EV@Yc)tM%+?)twaDFs6aG`k5a3PMlaLbSc8-?o)c^GYPi+T8 z`~%(Urf^&FH#s(33{xD(c{i^_ED z#5iN~EZdRdms;3=Q$GegF5~*G$5JE1=2t+9td8Y4`$9iI-x4pRDJ;v?LcO=d$jD z+`i#-+QBxU=kK;##?KIWIl5YD?h6!brr_hfae(k3xgvMGzCxA<6%IdKP}Hc0pgK-Y zQW@8K@uJFXSungy>&|i7dB-ry~KO~)b*5^cTj{m%yhe`EJfGO8!Ov8S}8wMOam+J zyNjj5siJI$Da856{V))4^Nk@PLvpguzYm_ctP1>zlT8eIGsim0B4*KnaWKH1T zIFFn+t>76cvQ%@QeXZu!L z=Ku7Y=H`s$b=mr2l|ToN78Ix;p*gsU2dSCc z{)kKpijnmhW8LPPL2gmy&Q6`O3YYE5@48jtmK+)|fp(_PgfbZ~Imsgwwtf|)5T5*D z9g1A6A9p53^hxVH`>>;KK}$gM+EHCre_Ba(YvOq058R~XL9E>UIgcg2!sQXE_~g&= zkd)7>$AKlrYUuzjtht;f7(WW6)Z5=27O$ul8+mU<1pb;CPd%4YK&3G3hI~AIie6d8 z2G%f(-+;>(!N0mn76!S!P&T!RqC~)|n`i=w_RaN^^|`1I8?Rf- zeuUC-zx%f+$h#TQg>B5&m?KnU$Pex_(%y`lEhQv4imZtV{9R=iVCS+>Ugd7%QMyky z27TV|B|;7xPT+o@_1FEd-q(&xvCf{uc_(6LhFk{AgNT0gEgwJtlU#&9A`P~k<{=T_ zpIoIv^~{4!4LVoCmNK!)T2+A}pVI$Gbfx8z4de32e7J|g_`}*zrU!~_IM2l>TyN2v zW7*h18IqValF|Tojb~EOvDvI!w$io)zU0d2qVZ~WLTVq3M=Ka8C^V)XaI3yEd3k(@ zReXv5^}?@roW89fDSNbP&MKMwIpzMIukDiKPU?|=cn{C}&>h!5fBwAu@Zo7?RTYuO zsBXZ&&QG-!5m&mA&w#AsMY#AUZif`NfY%)R@ekWGMA zj1)vE)|S~$4R};pD!oZLzq#2r57TdQ=PcuQ=d3C#THn#|#vKN?OvlTEU#+cKQ#c@W zQZ(*yu5*)nISVr~Hnu@eDfN(({3%U_0!|JY&H+aa?kxMn2-yZO&CCJY3L(crr0Xh z$qKV2(9?JpJjF@$@gT+Qs~SpSwDn}5G-B?#@oKddi0YQh^8M0NyX+a#d0A2?f`Pwr zdffk6FhK(s8zSC@PixR-WPnE_8k~oUdAzuty`Hf(OymS-Q~LYC@?B)(V|jVsvZ5oF zRvKx`8a^*pQK}v2%(>9q9BEW)YIUO5^7YATSFFqMC?*nGmW1Mo&J1}pwC=XF4jOZ^ zDyfCV@K{8M8pNZ=l0Pq(!PmyvU?L|MhJT$ zGOQO;uo=Lqg7VLdG3M5&hpUmI;_80g;wx@TLo6(lxXxJ}8Yx$-Ugl(^nsYtqA!*3g zu{p912_uu}Gm@zhQnK8gexhP<$wF=NPy|Cb77@47!0;AiTb8dqCdzT4Y*xcJ zDazLG;&QBof*MpWvLS4bv!3B%!(G}#^)iVujIcBNIDdj|I1O1%T7}`N1C!JjfR=hJ zFGez@GTlx?32S8jZ4RLxB*dLR2&;rvRD{RRYMq&sdj1`wmWO9wGRVdKTeq=R#X-t@ zGeRmkG}|j&cWiF%Ko$gaIz3!-iumn)EwFqRs?CJV2DaGC$QV=>71Kve%u0bAWgjMn zgqK0KTl)`SD@c9$EVK{|XF?gQr$`FzmgjfxYz{FURp-L4N#r!X`{n3r0et z2d4MADM#bK%Z6Q;llXuB^j*w!2?(Q+JBg=#?E}}CRY>^x;_ku6ol@})N^dOkH*QM~ zK8D92f)IR-JCgKA5k41ke-7r_b7lXfle!+_kdlJq7OHHxIww$x5-Z4se1C}raxwHf zO)I`qjY{3xCBb;{Q2)g*f#CD8LJg*WY3t(d2O_fkV0B*A?Cz~hf9)*>a^%{L5jb+M z_}rZaNOM%ESDMde(?RBn+Nf5f*yIzmCrA<}-6Z|}{cN-zi*DLUP-);b{bwaTnbvoN z+~1sI;o^3cL%N*aOs#`~6i}f;4*b}V!%iz7_G>Rj-HiOL0BLk zLKOHfo7~jYbQk>RB3Jz`p5p#@6~T z7GV4Za<`#fz%YTj288XR=Wh?AzDoE>*}RVL{)87#ZB!@nG0Dm<_FoF{1UR#NPZb3~ z?zvfSY=__FVniU)j4=>gO8)c4f*^9G?3mbCxoj!FPv5?U@0T`>Y~JGhf6ZNaIF;?b zepzXeVNqtGr9_d)tYyknA5jq^V+kcgWS)~Db$(YKNd7fgCF`0$^ zdsq9MefHUVU)TBXd@g^uSl;1ze$Vrp?&rSu>^V2=_B!#w0}LVzB6H^zyW&nL=T#_o z4M8C#%wbyTs_E)EkWhD6r-=5GhCVvhZZ^2vkO2IcLOo4yL#e54t>B4)* z*kuDS^R<1emz>hNl54CvD*_r8fyU&qZ8OpC%M-eatrb(kQj1y%oa065P2Tix_ks8t z@O8LMw%`v53MME4^JG`0^-EM+&27B`RG(mF^XD!61kHP9Zf+B03r$#OHg#{x}?H3*-hbyjutkgTjRmNG*`H${pL zl(=$k0pD4%fVA}MsvjtQ2nnX4Lrn5v4GcP3dS2445lgR*3q;SmG3RDwm z85pv=npbSE8m;2{!25J6w!bj@@x$uUE80#tGI^5wB*P}9fi-1N|M!VTtDbMEg;<>^>xh%9HJ{=Dt1x$F(NOW?RjqbVcztvrB94;BsY)h5(^yx@~k@*EK8IM`G=%8S%7<}ncB?yWoS}^Ux7M3h&9|t8#@l?-#IUpAuj!`Bk-Wb7p=3w*x zfa2hlrbqJV0ebjqfs^3<9z%X5JKLL|9ExxD$y~LyO;m!S_|E;GKYf3K78*#a2|0sw zpH)MgAT1r8e4B+T*qSmkFJu2LdM$H4#o!-g{r!CMr(D(seN^m(om({^PjP*9eS9i8 za(6g%bBKyQs2E^}qmfcqEf{fx5E0I=5=L^L)8RvybLnul5>w|h13}(pIu8)<&9;s2 zfglz?=YV#vlSwznwZ0IL|9xY6Q$sUq(ln|SMy<&mF`z?o%h=kebx2Na-Q0_UTdknm`l zjTM{ZWH@Z@+GG_MvEYN|*QaPL4#_3C1Y>q1plu50KZ}PH%I@&UQftLLG&{jWJG^-7 zoxe(+xk@u}MtAQ4`C+F?4i3~YQCnH*uyUOUiJ_+t;`dJ%*3rgv=F%|nQp2GMaXLCG zsM=cRYvJz039sj(+@cgWqONvGn0B%)444%-)k*@CX-3}OW(@<8s?OUyvbW^g;NYPp zxv6ZnJwnhvBZKh6PrV$VOCijs)-de!8fHHxDsOjvmoH7@JyWqmv|iYpPqvFosP(M! z+e~sB)p3(5jQ$ie(HQjgsuz>rF0?4ZTxIdnB>6~x(J|*n>HFjuUJNLS=ctl_c?8oEv1E>805(S2_N*j9~0Or!I>8T-2W3c2q$gnChr@=kk_53HR2b1TM_d z2Sphszuc{6^YT*B+(EBy3 z>RZfwOs{TWGi|i@e{R$$))osoCZ4}IJL`T;goC{x7f?JheaWe%B!Dr&NYOT1zAj)m zdpbPJcGN?-_W~>zs3!cfsgMQ9UsI0%$v_#MQ?$xzzn<>@^&?OneqodsT5IhhFXPy6 zJhch(v!+&?Bh_J~_sQt8JLMUQ=;@{BE=_mgxVX4%h(2FqweqcN_dN1j>A6_pv9>S* z@{Qh8W5dIi#h=ep-Z^!_i;9z#HKeAd=1V=KRckYa&CXqB9`CJR8mtcv4-cQ1X9GJ9 zS-?Wk_o40kU>cOZ!tu@c!|TOnp!_&GHpars8yzY3mR1ksKOs~&I#7|Gj?mE1JdFX` zXXKn4I~!Yv-=c03$!Lf)J^zp09~$U@gYPsY-#+}3Gt|n{d+&0CVB%$J1X8>634M*A zqYC_dXoDH#JnuF<5IRU8?6KP%({r{5=LSk!|2WGdYEiNO&ThtAZ=ywTme4G7UYcz69tr|+ z+kA(f6AXYE4hjld-sstmn)!x?8Q@N_JsoI!9^RLkhiAHry5$C-Pjw%Kzr0d!_~tAC zqCXLUi9`WLN8Gw5++?R5L}d@k$go3mBHIAjjm4Jgu+lLwxb-p3HT{#1hS91vy*VB2 zs?g?{gG5C}s=*P>=wew04C?#O$_}`hD9^x~IL~xTKorukQI1X)b45sV7nRhdH@5^= zW_s@QIhHn#Pd|VDJW1Ax52R#|<8Xup1Rfy_ZQ|G7V*?H4L%l^eBdw}K(;Dn4ru4{# zI|}gyuh$By<0mA7njHFy7<+np5FlL%^0iH8_N9vzbC0IKKu9ZU_O^XMe=o(5e&de9qB#mQFaA=?>o2g>%ghm_t);d} zIBN+O4Ln)nw^E@uH39*rhMk=-thMiz7+iWuNy*1%#oOOl59=faCM9t~OZy7gD8ya2 z>WyG;gVlNzF2&ByKKkuNUftUFF|!Ku3m1a5kC(IFy?b{JTI1ihH|HVR=JEvVfAO<_ zbFLIe&mmR=4dN*28=b=~kJUJ(ug9OP2&P=lhy_A*gZi-;WhMj~v7w*30860_M5Z*K z5|3*EG1Xg-`TIg7-@Q1cwF79QIXyjn&wK;WdX>&sL|!VqHj)u3MvVZi7ca({Tti5C7Y>%_CEGLHeb3ye!* zeqz$nY>lb1@adF3vM9QfkDMtR#2E7?Cnwv~@CSaWhaVCugoK5kozk%a!mOwBRDr?4 z)U2$m@k&h%4O9p#8yj40^3{X|*95s^mArA1QaVDD20jRmM`nwbJH67*5CjmRvne6Z z^|K7$G{RbmO-b<%y=EXu^qNTiB4}@00V5C+8v1tJ3A&7*kNxY;*1H0&Z6Yza{(Cs` zNX|t7-L1!EAa>7fwG}*k*t$BD(S|5l=(TU(J`q1XD=WP2Bf0Z+^hpT`7pHs>$cFi< zwpNEMHsnActT9Ep8s>zBh2?zadS^V!=>n}ywNM6COH0dg{~~2*fP?wBs2Ai~CErsvMqsy6@XWGe>A2P!rc z`XH0aNCKj9i1=eF{5AzdMHys=d?ksVqY+5KuAby z`QA4c@EtI3kP_j*5aA3|3VC*sE@K;IQTfo=NP|e-==@wQ$pUv!zu5rfc2}=H$;ujG z5e-%TG^Ai%+=phMp!m+o9{JQMPG*#1BP-(Qzv1s6r%ljFK6^K>YV5{>@l<||8k;y>N^0W+zADt@>90a6T} z&Qq?2FW6v5)`X21YuhOf1p_QRNGm{hE(7MSyV!}8mDN7nU1*CtAt?zj85Ow+x;iy;GC&jxSiaxA%3%J+8_fN|$#S_?7na1BTEr+c*fe za%=)xDG#IB1V=tj@=XL8+m|-;}?kKqP&uGxdA``v}Kn-|w|g zS4-Vz?K%fdwq+darS%q+cnQz`4c=kCr7bZsWG~&&os;WBK z@qz>1^8Ci50mpg(ytDw1Cax)aBsMWIEbXKREQ^LVefx8-u6OK-h={OR839aLw#7U1 zO=m^d;YzCgC*5^6of*X$Fdl}f1ki%%8y`avL5hH%8k+gs_=(DsTf{0V~;IrIddoGZbE;rQ!J zoyza#=ZoW-n~G835Y$0`Pfm`oZ~No19!bfr{h2m_Q42w+rOv3U8n6K11tklOi`)Bi zjDJT3{Kq+C<8X*6c!czbH8=9gLjw_wGy~0x?yq|s05Y|ytPTvRP>@QMxmg9gy3Blh z_c;I#=AILZt}76$Pde!#24;#53aEQ@JIFx$2nw-w?-UvTEWBZEPFyi{s%x|VkRgy) zzLL^f=H+@&Soi}F85h5F98l2HYaqW_>AeyCzA(t{X++y@cXTvu-c;*~eUX0jW?X-s@1r;(~NS6Q~@c zprSZ{TKCc`KWZa)8cUQ0)s1Ji_;r>->GgNZj-sJfoVFL7DO)jZa|S1xx&K9S)4xFQ ayv>i%;P9&|$7Cb$bw)#1J@=Gl@ZSK3TdeT_ literal 0 HcmV?d00001 diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py new file mode 100644 index 00000000..957b73c6 --- /dev/null +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py @@ -0,0 +1,96 @@ +""" +Lookup Table generation for model predictive trajectory generator + +author: Atsushi Sakai +""" +from matplotlib import pyplot as plt +import numpy as np +import math +import model_predictive_trajectory_generator as planner +import motion_model + + +def calc_states_list(): + maxyaw = math.radians(-30.0) + + x = np.arange(1.0, 30.0, 5.0) + y = np.arange(0.0, 20.0, 2.0) + yaw = np.arange(-maxyaw, maxyaw, maxyaw) + + states = [] + for iyaw in yaw: + for iy in y: + for ix in x: + states.append([ix, iy, iyaw]) + # print(len(states)) + + return states + + +def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable): + + mind = float("inf") + minid = -1 + + for (i, table) in enumerate(lookuptable): + + dx = tx - table[0] + dy = ty - table[1] + dyaw = tyaw - table[2] + d = math.sqrt(dx**2 + dy**2 + dyaw**2) + if d <= mind: + minid = i + mind = d + + # print(minid) + + return lookuptable[minid] + + +def generate_lookup_table(): + + states = calc_states_list() + k0 = 0.0 + + # x, y, yaw, s, km, kf + lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]] + + for state in states: + bestp = search_nearest_one_from_lookuptable( + state[0], state[1], state[2], lookuptable) + # print(bestp) + + target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) + init_p = np.matrix( + [math.sqrt(state[0]**2 + state[1]**2), bestp[4], bestp[5]]).T + + x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p) + + if x is not None: + print("find good path") + lookuptable.append( + [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) + + print("finish lookuptable generation") + + for table in lookuptable: + xc, yc, yawc = motion_model.generate_trajectory( + table[3], table[4], table[5], k0) + plt.plot(xc, yc, "-r") + xc, yc, yawc = motion_model.generate_trajectory( + table[3], -table[4], -table[5], k0) + plt.plot(xc, yc, "-r") + + plt.grid(True) + plt.axis("equal") + plt.show() + + print("Done") + + +def main(): + generate_lookup_table() + + +if __name__ == '__main__': + main() diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py index cacc6240..f9fd749e 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py @@ -11,10 +11,11 @@ import motion_model from matplotrecorder import matplotrecorder # optimization parameter -maxiter = 1000 -h = np.matrix([0.1, 0.002, 0.002]).T # parameter sampling distanse +maxiter = 100 +h = np.matrix([0.1, 0.001, 0.001]).T # parameter sampling distanse matplotrecorder.donothing = True +show_graph = False def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): @@ -69,7 +70,7 @@ def selection_learning_param(dp, p, k0, target): mincost = float("inf") mina = 1.0 - maxa = 5.0 + maxa = 2.0 da = 0.5 for a in np.arange(mina, maxa, da): @@ -108,24 +109,29 @@ def optimize_trajectory(target, k0, p): # print(dc.T) cost = np.linalg.norm(dc) - print("cost is:" + str(cost)) if cost <= 0.05: - print("cost is:" + str(cost)) - print(p) + print("path is ok cost is:" + str(cost)) break J = calc_J(target, p, h, k0) - dp = - np.linalg.inv(J) * dc + try: + dp = - np.linalg.inv(J) * dc + except np.linalg.linalg.LinAlgError: + print("cannot calc path LinAlgError") + xc, yc, yawc, p = None, None, None, None + break alpha = selection_learning_param(dp, p, k0, target) p += alpha * np.array(dp) # print(p.T) - show_trajectory(target, xc, yc) + if show_graph: + show_trajectory(target, xc, yc) + else: + xc, yc, yawc, p = None, None, None, None + print("cannot calc path") - show_trajectory(target, xc, yc) - - print("done") + return xc, yc, yawc, p def test_optimize_trajectory(): @@ -136,7 +142,9 @@ def test_optimize_trajectory(): init_p = np.matrix([6.0, 0.0, 0.0]).T - optimize_trajectory(target, k0, init_p) + x, y, yaw, p = optimize_trajectory(target, k0, init_p) + + show_trajectory(target, x, y) matplotrecorder.save_movie("animation.gif", 0.1) # plt.plot(x, y, "-r") diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py index 021d633d..b31d7319 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py @@ -46,7 +46,7 @@ def generate_trajectory(s, km, kf, k0): kk = np.array([k0, km, kf]) t = np.arange(0.0, time, time / n) kp = scipy.interpolate.spline(tk, kk, t, order=2) - dt = time / n + dt = float(time / n) # plt.plot(t, kp) # plt.show()