From ebc170e316d0351723afd46d4c45f97a51ecee5a Mon Sep 17 00:00:00 2001 From: Trey Smith Date: Wed, 17 Jan 2024 14:45:07 -0500 Subject: [PATCH] Add custom planner (#122) --- .../data/jem_survey_dynamic.yaml | 6 +- .../data/sample_output_plan.png | Bin 0 -> 31611 bytes .../data/sample_output_plan.txt | 183 +-- .../data/sample_output_plan.yaml | 159 +-- .../survey_planner/pddl/domain_survey.pddl | 8 +- .../pddl/jem_survey_template.pddl | 6 - .../pddl/problem_jem_survey.pddl | 119 -- .../survey_planner/tools/mypy.ini | 16 + .../survey_planner/tools/problem_generator.py | 8 - .../survey_planner/tools/survey_planner.py | 1020 +++++++++++++++++ 10 files changed, 1126 insertions(+), 399 deletions(-) create mode 100644 astrobee/survey_manager/survey_planner/data/sample_output_plan.png create mode 100644 astrobee/survey_manager/survey_planner/tools/mypy.ini create mode 100755 astrobee/survey_manager/survey_planner/tools/survey_planner.py diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml index 316a3101..f7d8f9ae 100644 --- a/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml +++ b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml @@ -36,8 +36,10 @@ goals: # This let_other_robot_reach goal is effectively a very specific kind of between-robot ordering # constraint. It tells honey to let bumble get to bay 5 before taking its first panorama. Without # this constraint, POPF produces a very inefficient plan where bumble never leaves the dock until -# after honey finishes all its tasks and returns to dock. -- {type: let_other_robot_reach, robot: honey, order: 0, location: jem_bay5} +# after honey finishes all its tasks and returns to dock. (It's safe to comment this out if the +# planner doesn't need the hint.) +# - {type: let_other_robot_reach, robot: honey, order: 0, location: jem_bay5} + - {type: panorama, robot: honey, order: 1, location: jem_bay7} - {type: panorama, robot: honey, order: 2, location: jem_bay6} - {type: panorama, robot: honey, order: 3, location: jem_bay5} diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.png b/astrobee/survey_manager/survey_planner/data/sample_output_plan.png new file mode 100644 index 0000000000000000000000000000000000000000..61ea2afb11c543f7c26b30c240fc10442c0599b1 GIT binary patch literal 31611 zcmeEuWmr_}`|bi1Py}?VsHAQ|I;0yE5r;-P2L)_AWM-Pj9+Cb8@z{x8viw%f-ua)568YL6n>OzkbeT?_|!+6qF(lK{p}9 zgL@j!;}^&LBF0>On2+WXlA0M~&M{tY=6=a|F`+zu!a9mOKH~ zzpv^^S?dY^eR2KPS)G5shi<1sWdFV}es#r|;NKU)?+7{m{T_AcG3xBUFL>^s|M>6s z(8EXn{~{;G_NHzK68o@DhNq&+Wv+eAn#16s0uUwxDmsuj%m}!@mmd<90 z+`Vw&!nFRxo)%a2VhO*H&^0;|9NObdQ>XaLDRcVIzj>l3cVpxn4%%*CrMD6T=xW4f|IkSyUWo^p2@WyHW6we5ZW@e5XOIVZk}Cg4z_F_$_hOcy)0dz zjyfBR`TY6ws6zue{?|3^higdhyQSux9V-VNE7K+n`#RLr@gX6VD@Dq780LALrM#2T zvTn3X1vA#qV||v%gwITS04IL9DKWpYGUSz?ks%R8WtiB}ahLiEeC2GiLI%R#FrNj* z{m_sta777KJQR%S^q%FzZ1w9qw{ljRZsGdyt=NOsgFfV;rd=I=HZ@I2cmkPlFqE*L zbcH!`<#@59Wg@Jiu&__J9M^HkmB@(j^Hb3(sNyp;G~r=^ucLa+K(uXWh6D(8bmzLW5w2=wR%gbezl^~piAG*~OHjdJ{kG|*j{z--b*C@3a zDmhp*mumGp2s^$DBDk4Ea`lD(QQSfiR&v15eQbQi%hQt=#Ib?f&@*M>hZf6KkC#Q( z%Y$o{emSoXpDm(`yjxv8@NA~#Hl^q1v)8KqL#p_Bc-~!Q*D}Sg&sf^oafwp&GHsB8 zNIR3(U~m%4YjGAE-?wBMTOPzI@KE`B&|fX?;P289BP7^D5K!^V9&Pv?b>hRN@ZmKp zJ}V07c%SVd-AX${yPCB|87kxQii*tg@-5U9?>=?I1kWb-|&VRNSWp< zMPSaD6Ipd`S_U0P6tw!V%qfIHg49u44ovm_(!y{Bk& ztKDqt_WFuui;nk-HkRG7)xRw+cf>_ACOx;Gp?IY&>R~mr8y6jX^dTWZp~Pk0tEao< zIGLQhUAJnpc7Bo8|K7=7SMk^m$NSUU+uFJxUQBMR@j$G5tq89^H{TyDfJ5vg8aCR6 z=B5~qpG$70P|nOmBRk?noY$+d`_=d!sly%+#gFWb&^4Bh>wZZ$+K|4_G`t-vn9T#13SA%zS2?Q;(9+Rl3&_)HLV;E|xjh1nYIb zL@%E@tSDx-J6(gi`mRDofMSxbo~WpDdl2^#KM|2wsp|q?%-yNrVw%&$W!XoVuUp#D zvN@l1m4+DAz!n|8&t^Y@FW(5SOfa;eN9my&o z3L)e`om=Kq2nc7~Daf^<6Qc(=rlinXc7c2g*jHeR?=+vOS5;{-4Y@bmfyx&mmq@tR zD##rmfX^d;VSZ<*F(+ zc6?vLIzhJKjFZzuV!)kfD=AV3RdC_&G#!d}1em$)6cPu8B~7%3FKTsk=ebv(AWP=D z@o}Xg%tl6pJb8?WI#|5k&TeB1b$9yg^cTLj%0#Mhz-!(i+V}Td{VLxHzEUx@urNL# zhA}>LzSM93bMx6Og2_j9`#LiVG}pBIBOqZIsG=7~c(>~uZDfQ&kSNCTOEfeP4VBE~ z{4p0xZ{4>4o(@58)la8&i6s((P#Cu&&}39h;ri%kFSOg3#E5JN$Tks7l4{wr%*-UF7i5$~ovHr}F!fDD_i#Ljr*=Y`;HCZZzC_*cPs0^dU17ak_A9RRp6)k2N|f^JN3n%LBH0(G^h+cD5y|Wa^CfJ6}Hwvp^$*Jg98hqd+g94 zHkO)9+4&$-MamYt;=b(73j9hrr}x7RKB{Y%&!f^R9Os&cYSJQD3z|L1g24WaETYcV z8*wT{a+-!uSBHg6eCn$n6SVhb^Y{0e65@f^%VU?jL}K11d|OGW;N+$gM+JotPQ}~) zkQ|^Js6&G)^SPCk0_A`JV0Bh?qsZoP^K=9H7I4eK|K4(!@5SW03*^iywsGO66T_FF zz)bj~HAkDK!|A52q0`qoLps5YTCT`O7Zz&%@k|~&x5ejotKIFDWMtIM+JsAg3q!P@ zxf9tl+#McgDzVMEW?f4lPTmGvXDrno5?NpxzGf>UY^{|6S+_=I@dBNh(jEfao(MR*a6OY(1^V#lQ^l?L$cl=Nh*p`QviY?? zZg<^z#>r!)W2cYiE(jEvhUUXCZTWNKHKy@YZy{Vf@LP)2DsASa@NnDO=Fa}miBH29 z;T%=}#(aM}WT@6lpPD+>dA-^E3LOdR_Aa*b&RDg4Oz&W{vkE(?@X1^NdQU=1AZ< z^QDpHDf6~7Z3$HvutBaeAQ(yd2H(|qbdp<64F!BGEd{|8bK$~f=tMnyhWc!;XZxQ6 z0y0nbp-&(M>C^5&EkE-VgwQ4PDu}i_qZM5wfgU}6?ldMAKHFH#P+DO7FjHBANzAS0 z1AwV9d~;iv<0G0TU*qd3p`o9}cPDdGO?d7ZdarHC%7IIuWOB)UZTDZ zZn0{8ms6odw@CL~y&6E}^|umyten<+Vdr}<&@5{n+W9FVp>_L;aY0+WNKxkx)#Kd@ z7jCPveLI$eQwT+B^*&~E$6m$6#9CR><4KnzB>YmYpHXZdeM3F`*~V340$NbD353%d z1Ex6@*usvnD@?~*%hP^`!t)HYBaV6Q4YniOHlRX;Eg3_ui!;%F?`=o=GY#muM9)JI ztr}n#;I-L8zj=vCQ&5M=&of$Fl;%yL*GlcicET;BNL3g1NZ1#o{Pz9WCXffQE!3AF zxy~Uj9xi3ErLuhe%4O6P!`_d;bs@hEp|gy?EVdSNy?vZaW>nAzW0mMUF8M-3NkJ3k zMDNFUltfuUSheSBx`(#kgqTr%c~~@~rKM$Os%&&tsWam98wi@51Z$m}&sqUwpmW@$ zz1&?>dHg=bI79^WQoJiFEulq_N=1Rfj^{lFjGGrmOYI(M=nLRHhApKdp+EdCAjmC zfc0`$<5>t5>%^b5A!O(M-Gd_@0_?_Sboa-Tq$y{1=_TP<*W@HOPTDQko2bi?RIlnk zEv&3`UaMPuZQwrU${Hz8;-id2BKP+8wtXjh)G;znr%YxFz41W%P=p>ghwlrd<~6Vr zU$nju9bP0U95M09c#|vE#l1*uGE@f&1s_)dS^5qg-B!<+nMc?#)a`%yF@<1Q=&bWm zy|jIz_xduwKzXR8WH!l+6YsN@(0s#vCkn^;GBa|DZMu&eecX9Xodvzjb8Zf%az-R< z8S#&g@IOBrxW32m)7O~a^I?x{omqdMM9ky4T(i+(;Iq}|v5cAHHSdV`mz)crvHQ*} zx%P%Abi#Lk$Ynj&BNy7q>maW;j){%0g#uhQy?{L4 zyOrepk?!McXHt?r8 zGcDYq!wUq8{H;;7*urH-$?lcgli&X+5m^8Bw6w!`DU-(TvI@6g*9X}i4Gj$*voRqe zhnpgn78Wd`9d`lc?3iuO3*D{p+#i+qak3+kb1PerW~eEX(l*pEAK~rKNlZ)xSwo$G zs+ScYcgJ$Ov8qw8m0Em@cAeAU+zIK~s<)ksi;MHop?(&)7gM+YWqJi`Xvz1Sg*;<= z?3!REX8t?#Z#nD6;GUlD?n3fg^`vDL73ScjI$Pg>?B3Uv=hw;q2p<#db6s8D9rkv1 zt)rNtqSg(a>$d*>G9CwOb!{IUAI3ghiye8+5*^snQ@Z`pmUIE{%)Zr(p`#ssKk%Z3 zH6tD=hBc)Rxh!R#ZQtSW!qIiaw(?njzAntVbe}8#4Ivs9t!IZv(yJa+1P-GJ}I$k}m{Nxwc%YCH;W3GBg6?E(W==w6#REUiz+ zDulzOW-SdMuIYwjHvHB`liMh`T8xOZV^wuhYg*V*STWs&$h4YPO`6O9A8k{b6_S$EeI{3HwJ?Ug6va zH$9$C?FQ?!37GC}_JF#}D@pW)^a2INa9`Mc4d1LNKJn3{vFggq0$a1L_aJd6x6C+s zdM2u%(%rHvFqiYypl#-b-R0ObQeRrfgZP9x4&%Q`0eVcCUE&jr>~nnFph#srzb2w_vQm4 z04}5;BjcQ@?4t>O#4h?51bOA_A@O>KM1;nsi#zARxkZcCxA5(eV;>+ro+=uOQBvduqN8@XI_Poq3$&IAqox7dfd`d%jD!T zKpas0=dDeRqF7_Z@zad^RONshMgioK zS?R8h_TelefU?Q^;+D!8&ItR$KC{@#nmg%E}_cRq4< z*8dE86xr0Yx%pd+&ALFs5w<(|SXPH(rn0;nC8Me-@gl}I6J+nnWIJqi1>P_p2n zt&}jfWSgjdO$~sYyLsD$eu@NC(myj5=M_y%6kzJ-Y_1K^3uIj69^vjRqDX=9_LuWt^Y z>iKINbtf-PBw_G**z~1L&g>&yb-f8T!$vODkf>DDKZ4C#5~7tl`B+8&bYCf3*wV|hsx|OFTb&psO2hu*58sPWaz*~IGsE0`k^G&? zD{*HhAEW>Zaj7fdVzQgYwKBJ*Vf4{a0$?Z*>Mf3(kdAI|7~YB|z+U>AiGr!Y6@tI( zdKD%Vb}yYzc2H6Z@9yu9c>w$avXVhgYy-T(iD2!?oVO-|R26cq#aes+^RaFTcUf`# z{cb588!Bjf>177_E#Py>o+kAXtfk$(y?OwNL%Dk-;mkr>^z8M7*I~|n{k;e?6SES6 zIDk!72fC6s+%F5aSHQ=N^lH@rMjc-L+Gy(Vsicw5%d^W8Ql^EqXRCqXa_bF$hJ%~d z1BFWV_V%J;V$L9E&87x%wWK4$--gP4md|KOxU?gcnD6O}RJael?HV`oxIJFyV-gS; z*!|;&j>F$T7bw9|==9M5VH|kuw!e9MQT`UhBMKayr-lTmT7h;p6P6D)d++8W%Ou9XcW_%L}#CGmY` zX7df&(MUQmBSo+M(bU@n5YIhwN=mc+ZFyuIh=4yX^<%*IqOptRJ<`QF*CH>&qrIYI zQS9LeHlf^lVD{TJWSffbdibc{@u65+tjCpv@M!UEvP)9YX!RG>U)PGLynDRQaqs9oU8ls;8fES|3bQ2vl%=OMG1u zc+mDnvx+aTwvorot>e1#a)D?ACj=K}8hwCII`&8e@}#4pT!DzQTX*j}o1ynLpEzTd zV%elqhDYYi0g2qM@B$w7_vWU}qsJmroG0GFXEYdPGp4IMqAQ;Dn;&AP5;G>)7cp4t z2W9mC>79TQePED{ax(gPEGmZiTa1X;B;_rLs%%)`!sR zI6+%w^kcxoO1#|N;TZhzuBC{jzI%WeWUTyNVDmN*#T2W*)56TgYdmzzEYvzAc1H7& zSjo8rccVhgY^?XHKSLXuNnw4pf$|z%ICZg2XL~+y+_wXRgXkP}6~te*qy!Ml0~r}V zVq&*U4XZ(Ce~_)qQ-|B9IpAOy1&siLqu*giETF65Nbj@RW#BoV;n5^E{+>x8%5 zV5rK~y0B)w#d|Z2fm8I3K&l?9d^W>W+YhK}gO#l)!B-G;C8fAn2*K-$TjUoMycSOk zp>homoB6zAM>zmbOPmXoI(QPrp^K)muen<}>ZD*-y;Q+3q|Ft3*qn5L z7+;U7yb_Pr>l#XIvr;iyM$k{Zi{HnkUM5hqSz6jCb)0ILpVv*=U%?jt7M9CMGvT3> z{FM9qy6Qk%nu+t;w}ESP1e34&ddv9U0BHj%H`@v@{S;r5dpky?0N!MJ_+~+2jg4iq zr^FrzAgF0*_{7Bx9tm3Yl;E3N+1YJL0SUa4mzL(VKMcgYbc4dMiGhsAy!@s#vcmMU zmw@Gpi}g^?^Z|X)D{<-!z`J1yuhZyVBqVu$xPVN8n>IrqpUA*cc@2=5J2)#xOLkW5 zLB}TEwO_YsGnPis=DYpzwmpD_Hvr;p6`6|}ox8+#L~kV$O0#(WoM&O+m?b2n8qL^~+0G?=34##M zYG(mk#YZP;)gi^R2E_q>1D;LtX|0jQ89x&}S21GO+7`pNF}C6>G+^MYWY(FO0NNln zV^snGoDYQUZB^Kf#R}EVD`9}YZnWF`@|Bvkx5Nrq^ZM*<-aRNX7MCbaS>M7aMM@#o z-6!M=t5`5@9*!93PO4HIJ>cBPuDg)cFs=`YKm*Kg-j&)4Bf!ZA(3Wj`>{VUt(F?|z z2>D18N(mQ*+4gD{vTId9q*9I0*JgB=P_>lUZ86bz)4>v4w>|$6MHKIMgok?%iO+w% z;V`NZR?kvy_t_bX=Js6+8Y;KehH1mMplb|U3qG@aTY0oG|IPv=H6P$NoB=Fel5hj9 zt&n?8ZygriwsdG?Z9c^Vtk`10yar9BopRCA-`646G%S`mPirhDLt!G1l+5@Q+c#|~ z(eD!xJM`MG&O$s#c810F3A2$c{q|7fD?kbFDJ6&s6!{%4@VVl88Jj_3Utc*s#CEP6 zAFVK`uW6#am#mSka0<*TtYU+$h?z@_J3g8?-q-nR7QaQQwHz-TUrqlb^rEqN#3@02 z8%m%W-Jg$<6T6SPlwC$Ae(?Im)&j%#)sJLE5H!`iM_#{MN(6c|iGbi7j7=OG7J~c= zhi1DBr9=d!-)@7Yt!+E@u=032`8czvh}*|!f0!9`Y8es4#KgQjJUm<&6_>uGskH-f z?4GJjKx;^#PltmD9_KHE4nM17{F-c$}|Fzyy+<~y#?3;eu| zTA}Uk{YeQ)_M(UXbnZvSbWRs+P;_TqM5*J7! z-27hQC*z}k8c}+J)ElG^72J2K@(pr84i4&h{{DVIJn0>3Fqk34IEz?f(tI_y;?dDaq^%WYMoL7@ z{+CAnBzU=?Trj>A6D}OS1n0=v&w8{oL$TQ#PLyAr~<(s(&{c>^H?SabW>)&O%s0jM^r4@ zbiR8hGefr82$j~=%P*o3T_*%uRBuBGRV%5u3=%QU+`=Fe@Y;{llscrqp(vYOIB~$c zAOxAnD6u=}Aa}o9TYn$s&B$loF}ku=>^Ubk5#wIBkzn%fo!%dg+UduPnR=0)a|=Jr zz#OjsR+*S*&BcwYxh*EB2MGV5_WA)?Ir&b4OZOL$kM95}^9qt6_$6qh-=+~1k7#T} z3}KwDj5cDODJ;ZDq59qZ$q}@lC(AsI;<%?)iK*Rs;l?7c7*$Ow+3`s9dr`sdsGxmOnfxHP*6~0<}}M)1_iFGSY#%`8T9#L z!AMC8Aeqds5)*DxxkL_TOO~rwI<9IPyaysr>Xfu0Z&>Kq^4J&x4ZvsH z4^_XhEBS=Ixy#F22()F-ER#p^AgBt9TRIuak_xk|b%fWg$pN&U{90n;JG0QT^=%eb zh-3J3ZWk0RAzJ3=i^-_!*MMMB>a)*I1~bd6>^oPC6J`JxYJ-~#8k}R!e<3AuNI>E4 z9XG-_CucP!5J62Qa~*}X)#ob8+c`wR}=F&xz*OXp|t&fD7QHWM56PkD{WR<$#nj^(iwIC2Hzc6Byfo1&7AU zo(qkF!BuY+4CFxjgqXzr*+5}CXnbYs_h}XOebEBEr=i7a|DNkQG=#Xo32vfc7acJY~ew(K+EH z*RO%3(hw`<=SxjZy)9{HW5XeQX)D>2_jotJ)Hr((OrSZi^ttJtu=j_ldiBGAfMwqC zu>~vnRSF|XX$=)XZ7(;q z-~a3pDL@0#0chMbel3v@!b*ak6ojJg>*i$&zmpL0l}iU|vU>iUWu#JjZ&-s&5586Q z9q+G(Fh?GR_g8TcM)aPlW91oDL1d5|tkwfe2>OG~G2$K%VOTzxW-wkUSgMg_fpSa( z5I^1CZ+|54pjzJ!^NcoJHH@kgK`q~$xzyY`CJS>{e{1_9gOKIr3L>id;%uryxMhn# z(g4AE%}M2%Z6-*-W_Exr-k|_^^w#05{gD~0ssj}kjL6`B%Sq`;|BY*&jxz0HjckG& zP^x*kwxt#J$RsC^Pm}!6R@!it=KejvJ|B-A;(~{o(`SKXqR0rgPG%2Glf}qVXYI!+y@h1Koe3@juuDpPT)>)jqL8QhT$fj z3ED!pV&76gy@h@|D@TTc_T%xbT-6*q6MlZnpQb`jGP)2J-(Is8V$UH;?`k}BFr7!q zoaf?903|;JX*@>I)~LeP>yCs3jE`7lkE-jrhdt7Xrt{~gl>)No_ z?R}S%M7PHY%>kjh^~Vp@x~&5HnJ;AA9HLaNPlpo2_p^)?-6#ifRN%%z1asDVN;=QR zYrD{*Vzq%{0xkmN3|7$khcv7A!n2IB#f^YB7#o0K+J?FEGRg!@Z7J*P>yIDI3e9~c zVQBNGl>ESmUk#KJA79JnE5bc8Q&bXMd6YQ#p~mfA7u-ORrlX6@>mD2PG$*1Z2mM~i zs^>CI_)jNZXFcA$S`g-!gFqWXD(P_1oYJ$VxcIKr!B5JK28JUZ#ACL|E5?owG9J%R z1cBKwEr}x7H-|MiZw}UaS?}$;XJ$N;2VEK~eOHqFZMP9NSnIIb|NLx&-I^Z?W&qCN z3v=7l+YT>tsEj2l>V7kV@cUQx0G;s7P4T1{KBT$}BPtdI_d7Zp$C0)ZLv~chJ+z1VcS(p!o+a zs&O%gfxHB3CqRa)zg0UYvC@n@_=co)I4C;86`k~a-u>h0(zuw8xjVnCBxu756flU^ z!1Y?Rl|Cm#u?!|B6i&7{cuOChXc#E~1h6X!#KymXqD9-iQ@2xV;4u>k2CWX(TQ`7E zfki#&O#^ain@D66d%-gT5ck6)>}PdGD(%}=S5<)oXQl0$Rgdd<0ZS6VNZ*{ddENoawZuB@V>LhvSAyLDDm`2gH+%~bhGOcE(2ii7rdIA|!d!yU+^ z;{%4E&<6yfqQo1M{&@iB3V@WHRa`96T=zv2)|1L1>ljfF)P0zO&*y_F7kF?B%w zt?a^lp2M)JELo#B?b$}S_{WuL2JjiB91VG|{TiF0bq%210xsm&^`1pK=gRR&!=($9 zl<68)ZDp3FL7{|DJ-bHrI}UYbN>x(W!afI644w8zv-UvB5%5^hpNM6|Z&zXyxP9go zFbjp%=auCoa>i}f+FF~B_qR;%nS)Ai^MVYPAE=CNhc_Q|sR7P4-OQY!VZ{PBW)MKg zs-}OG=!025jKNJ4od2oL>+opBbD(-%<#QYd{`j_0KgvtU7CXWYx)_i(`P zFdGTCg&yT-46k&uX2tUcH0KDoA3mR$^21xM3mr4#wUdy#5i?u)oiT|WXAz!FdmRq(LkOr$pb&H{&i z1qZnB%J>Qm4Ncg-Kn`VP;iKi9EZ7c!oJF>e50<=z={M`o%dB|2fi^yo6I zkl@#&vGE-p9cjJ=S&5!(@bIm*wKW&$jGm;%`K%=0?{hm_TNy<~_A5IQ5)ugXix)4z zjBW6N)Z5xzsixus58thnfD6%-*pZ?C@UMU-4wB%=wGVO*si#wNO-27(BIeMG4TlNW zZR*~PQ^Mt}@%ESVU(8i&I(~P<8k3R)dMh7e0LY*t%vQo~>0=`Wtox_IT;FjFlLDK5 zbCF&-8^dlrNr^>Qs`tDCQZ#n}KP!Zd0poV@fLeOYXGE{h`5n!HhE!9j(@aa-hr3hB z$Fu)y#}My#WeO*@(Am)`L|65r5T|V>8D813|4o>Y5JC|N2-&wsur9zfveIqY;N6QC zK;Z)_;}ab}k-J;l+q1a5B4u}X_u-L|pim;(+i)rnbznm48}vT zakoW=9J4jvF!AC&<3QaQg-&nQ|7>2Z!h@$kw7~V3JX@KlChG4xP=?tUZO=WQ8~+0v zdta2-R}~tn9u!)CX6C<)jRZNztRo(9JX{3O1#STam0|N$E(Rmg&~TeODu(x&e23)z zqWD&?dZ4#5a1%X!eM-@a@R}}xD5)NIcmXZWqEO#9}>`#X^ z`NAIB{qH)80*exE=@W?mo`?xcWmrfuJEl^1Ze0WO*}Eb#KGwqP>Zmuk0(&3pqe;r6 zn>0dpqh2-*0p$1wK+kHHZ0%{*7Phyxo`I-|k?`8K_?jM2{@*tbVQJ=M!qU(Pj=wU8 zpEIC454LbS9}D%;Ck4NdshWw>FY(#mjt4@E?k9&QRGfMd+St9mp*kNgcvd;hHzCY> z`Zj)ir!Lw&$w!bso&QFD8i@Qy16(S;hX+Jq`XIXm#A;8zgB#09N1Iu2OPSv9 zU=0k%gG9{_Bet-4*Zv}8lHZ2?k&>Y8a0H0nnxEwM-(iNzq4Uu=7_Dv%2%>?r<$Ads z`gobdX&6bKJhm~|My^W*Ek(~jX)Mv)XM}c z4?Zg}eBQR1sTGZ3W?m8P?9hf}SNq9myVhq~OTF>mTc<(w;0}9Ajnf3=EKvwFkmQ%~ zy6d}SK)8jnVeSn&e4HU6oU(E$kiWk_Xt-i7P?-1UgVF(dU-axrMc%c)+omZCSzs}J z?(JV6lK{MNmOC7U1t60TuP*oLZh-2U*MBVm5Dtn{8b&x}1BMjbCw$xsKmB@pI^quy z&Ochqy4Jda*g;wdlseyLbE*BI!@3n5P98`laMEKOs<71u)BL>F{Tyd?g1>irL>7hF zKOzo9XEr|F0BSNgdgDH)wCE{-QO#s3{ws7)HfJaB_;IPEz1}RR4v{%HDZ{;q3Y#Hx zU!L|!8v$nLRCJM|<;i?8-LZ@G*=4uEV!<&vnD4;~NG)iuDJSRx;s6Lm;O1JGpSaM3 zw=)&DQO(ZauWvycL!G{Y4Euvk=M^7HShFluUzUM!EjUmnmFswT`jyR^8@i$Zh|weN zn$`P2wV@D}WZhX6vp#{SjE%SL1k$hJk4qlo zJI$9zF__7hWMG0Y9SAbB0L->64wY_D9Bun%O-Q8;!0CR~iA6d?!m+m8$Y& zv*rLz!sgM`9_St8Nkd87+}s@Q-@++Gx7O1YXxOld4#w+Ce_Qloz${>%Ig_+!}>?LeGA`BjH}`W`Ue zH){d6c({*#ad(f5cxiO{tbQih2(kAz3BK%@s&~>{ROBZBkOjovtF8ev|T!ie3H*ZcjD?-nhLHcDx&g?{KdR2eaQb$A>4F1RWI>r5Gp3 zSx`_QvNP(uf$#K_#BPXT!=)pIe$^@1Kfh{Af+4ARr*sZb4GPfd7*ICh(QqZ~s;~;!<@y^O1x9hq*P=)}K z?+9j)e4p<*E{K7RQ2%Di+&V~^rb-MVLzGl6lEdCWVM!T2_(7&^M@KmJo~GzuD9G5i9IL303c zqqM=3V$x6a;=;jGDD>JZgQw@3ls%&vZG-@5|f4I1F9Ei zIRAe4ic|I0IR!Lg_TcFHUm;Jv3pTq)4Q#A^H!^Y+dA=}ZWk(u_s%9>nP_f`4CjE_+X~_D~W^JERhjp^zJM09O%QdRmVc zA``)Pry~(j&jcw|-MJ#8Rk}-PjUK1W7x?^eIW;V389!etdRR{_O+*EqVXEUiQZGc^~K@b-xO zyPltH$H>QyZ&s=!#uJo)wqZa@t^*wasXePPlrC>~_c@)ggEUNe`w9)k3XE#?NGR!h zSNx8>dM;hMW1yUayk>b#{URX>N#llC(pMm=Mkoi)^Zz+}tc)&Z^Vh=(yjy`!+7klk zB_(nUT3rCxuP^upUzz)gQ8_@TR`{=PE|&v|mw3g!D^<3HLjgR*M)pAQ)e9xB^vSx~bCl(N-|ME3cqWeHQ!&&hsG34r0^L?iVaT&Dz;p1W* zIIWQfcJ?1_qmjz^I~))x7r!yVYMyKm6&o8{Zt+2Br=)sQDBVIWwkJzv0XzxS=iDig z1>h_3-+$jXc&)8hGVXF7B2`l8-%pe~nQVukSJ5R-^W~IWb?w#+yA?#x8ItIE;0ps= z1IQvddXEQ1b$hvt%_ST&v$Hr*pTX$a*h;^bALuD{3k(>f6`M5Sy5tyYe^+E=WUz5^ z7E~Dwk8^BKv`S$E{eKlgR|(Y^LJg)H>rVpCB15;DTt!M%Dr#9xPpHVmaI zM@R7D9w$MF_JAlGcacS?u=Jzc{4j@{@JEt`o()q;Jf-hE*)^-yC{C-c)YlKg=nHPi1pLm)O01gPzH#G*-bdqm zAv5e+`KpSFivBB?U$$m3S6dx^PRTTZq`$}hI5aWr-hMZK{d8|2!rw~=FNQr*wF^eb zajUNz8JDaA9#~MAcP93gS@vk?=wyHTB;4Z&^!Sj~Pl_R!{G6Vim{S5q0JIrJ&=ffzLnl%O@9G!$c zYy&d|KAua&$xAv}%;}fgyW`(2$MTDdtP*_nV!X^|eunU3tVKPcYTjLwMhn2#$m@>! z3-mQrRg-~omv5yjEBhM2-gMB`&n_#|P&VJ9yqubrc9o9K&;)5$8F$Ttz-;|)ZCP~h z-43A;Dk#}kv=I@#?bJNalfcL$+(;DiA0N@Ddo81W?nkcym$^=_JR^fTUCMFiH7Z(V zA#xoHC#5wF_bF1#A>s*s|07|J`Nm7in7G~NpS{v&8>TM2s^Gy_5lgFsF1deU?Y7?5 z@|Zs8EO(U;80A%t=gPDm4tVd6O!OBTR+pJHh2~;YWCC$;6ot}>S@9!pISJEsFP_U( zhomiHCGI`x@jk^4&?LJWm!5NNztjPQ)T7PJmg@$u2=sIH>&~|mBmGsa>@!xOGP7b8X29-Tud)l5TQEfFe<~~sX zgxLFJ*SPX4Xf1-(Si7$tW*A~XqKW~v;q$zhe+Jg)?+mvLJW#XY=!Gw%?sB`-AGc}IH5Dt-cBfZ>zG%B8uDHlo6`do4F4h2VnH-pI z4lnoIvKj;9JAF~dGu(b%fLEKdT>UaOOrpdL;X0;R2dZ2bV4Gpa?0Ot8&a%}!CaZMG z)DV4cfE#Ld`TNhgm1bIdx_^e(B^_nD2qX}L!941Zkj6$EU#CWb3Ifsf%= zLdG)+14d@BMhPFN%#!_sO@B*-nKK080zgIXTb#g7xPZzrDW_Mm!Y|edu*~V9l;?CN zzoqjGC^GF&L{C0lg^)P5uB`Dxyf$wKTI$L1gJ1k=cUGYHoMk;B>PI~(D3T?%`bF5I zs&S7wx~o^Nd|%ko(#Kl$vB@-pjaGEz%&??FG z%K2amUJC{TfJI=o%W7|Pei-YAeQIKIm4N}dLPrB?z^W+m0?e-J4p%<*+qY%FgxL?UN=3My z&ABez{8sT!Uc9@`B=Zk;gGkd~{yqPoc=}_yLKTdRve`3lo|znoU{&8-Z{^MdQysXj zw7^k2_wp^UvwRU3Ak_DnkLhKMmK$@AmFs$?y8h#4FCL5RX-;rGCfoT|c@{cD37l+K zz@+hl?O2s&<(ONXsQWTUYl)&9fJE!$5_NcIOZP;VDTf}*Q#(Ot&co=+;sjj*klzQm zoRA?9qcee^fGR>LY$s^>UWdupE3}}k=Z8?}3};x=ZOh7EQ`LPR-UCD5DB1AIB0{&% z0@@qa-~vIP*+oQj9UUD(QG!tRkLGc4Z&!6vTv7+&p4Z9oLiN{sNhzFI(nEkgUR{Bg zyah<47Nz(qih@?y7=C(WH}-=?jydy$$HUKm(A<{>^T3SrMZGEIy>IUR1>H>NIeQDj zf{`h~1PL##TCW`~P!&UN6#)@l0d^~Cy^^w(4@CqEIF0|@Ex*=G7-tWfsr6*Tq~Hx( z0OggJ4(!zK-J8053r)bj^NfEMN_cDkz})9gWQ3hyRmUc}%-i6{Qr&2utYh)NBO?{m zYD)h6^@00Qw%QCJTrcYF#)5Q)@6Wri1Wa@>qq zl9bxhJbr)#c&rOKyMHHIgjUMKKpJG_~~%HPYx+58W5EHSPxx9VC`PV-{9_UlDt@cMrfX*b>&Olb;2m$mv8i-n@CUw^ig<5NB7_EDahlsyXWo9~Q-%D}2(P z{#iDu#L%2vy}SZ%91-+`hoXl7dX?yVxMvBl3%v0c*Y;fB8&J%H9N4A)AHTQwr-!Ng zh!AWloEN^rc>zX=Za`9@6LER~AP#P%vgmZVW%{@g*Aj^l5iD|yLiC#TP z(uWbjUAu`c#2$HqewHB^Fe2!@07)|rIX`mJK@SK573}3eTAp_n-BkL}<#aVxvyBr= ztlDI54wLjr4a9A|Rj#q}te7nhgcXA4#}#LmV(_DL$y z_M3h;;AZ4PQ^Z*?Zjcfe;6?L3-%Pr^qW(!?2(EjJ{=6uQj^mP ztJ+mo5ARj4)K!1prx$a}_uN@7fHxj9xkna)ij0rXzs#(>`tr84IDHr|AD@8T=qKvq z3IH5LUm}ud%Uq6=K;eX_aRZgSgjBgx<#a!u_g(^DYP*Ax)}r@JH?*F+Sg2%HWd?^g zAUIPj$rrV-OWHouKT7Qlz0YtUVnzB7NOfq#V_}#l>hc?q_lv2!jUb(Z)S_-v}hwfvkNd? zl_uf&ZX+%cy3FvK*Vl$AEW~l7;EQEb#A>BLn$JOajNdq=xo>BFukliM33;6*@u8zm zcgXE{r|LGT7zafwN%0P83lM-PEDx zZi^p)z+8A+E|f)5(#W5fynAnJ0oYf#rwf8eEz=&SpH&+m?2!CyW( zrMg+@MhI#P#BQ`02XdoNvF<(*>YWV0hj(_c$NN~gVqL6wnDIz^GumxNey7G16q++x z>28mYqPr>Uls2xyr@1NY?S(2LU~gen+7rsL7PDMeeOO0`QvKE=1b%w)S3HH~BTCjg zEiUMQkD5kCT+yve&AfEPq-wotwuQgBYu8_tF(YGzzy7K7>5JkM^BEF8X#zyheHswr zpd^3L*-xH7vo*ICVLevm8q05q9<{5S2kbgE)M{s$9>L(f^-~aD3zYc`WYeA#SaBu* zh86IE%GAI=**+qicX8zIaJg1gfitZQ0NyjiaQ2GjGYvk3@j!py45Wj=MN)Z7{% zxJ&@U!QCfl3K}nU2!VpuNl3w9m%{6BcXa(P2cQVX5{)h)rnSWtU5!g6DdVS2)&Ywo(D2XQqxoy zz^vuoc&BG(W>(YR-@}QvK07p3?5yPj! zcrzkB@z_CZ1^M`=Zt1D8{JyCoV=R(fry1s0gh4Rq)6~%T4Hg#?K*><}>eU(Wm`C9- z7;u0w@u#TAl@xUwe_X@DPEHvC9+TnQ00}@!y_Le}%7Oqai_LHejQMB7@6|kc?_Fg+S;K6B7ytTfniYJWF>zox)iIjk|-=M=$`?8WBN6wms;Po};)VerpR zIIRcv85>~g0sDxb?KZmz_>aqP&}*{@BuMl4kW(&=^`5t{uZW&GP}MPfzJp zQm3Q#`>eJkE)hfvr6z4MKgjPEB2`1VTI88sERu8_fH)qsF)b=PdncR}XOLOlM%7Rv z?CgbhtX$Y4IPiXMJg4tcrPtSkhgkzqlA4_!cxGIl%7LZe`QYmo!kNQqc*~{tTLkv{ zR+_TN2jEPC1B8vU4`Poi%gF5$PSQ_#Wv{|I55LJoXy~@<)QxEn?ex+%T)QtTYLbN( zpHAXOS3bvli~9WYg@uKs-#*-P=t!%yDveUaZeLyyAkcS@cTB-ora!nfn7nDp;a@XW zByKuezz`WLDCv zZ5d4Wb72D+79ZAv+Eu#iJ{)h5ej>g|G|$jicKq@1KqvECB2EtMb7;T9j(NdTw0&-w z&nfVErEyDA&f;J96r_ZdTJtIx*El`-c3b?XUTL7cWwT^T6@mg`f4ETZ!BaA3ZfVmCxQ!$iu#Zq7&gB4)l*YbtB5mTeTw0>4*%vnE9vPJFQ3MJxh=)hwJe_@G{;>( zzBO8}F8Rr4YmYP*eJ_fA)Z#QiOtRxyw9INNj}IXG+JSrfS1OTp!M8}q86d=(Igg@I zM(YEK7XtebrT%DPq1!qWqqjI)=0wnvml=JO1>2mJW|VxNHGB4Y#g6KM%XlSa zHy^ z=Hj#ovZ-0_o-v1_nvWI_kkzEmc1P8@4H)Isn46hdJiRboA}IOb!#!eRVyY__;u@(d zMtaTkl4K>z?dMQ9^%rM)Bq~?){LPK&#AlHcCabYSy_eyTwVOM)Bsj{b*pD;f3v_Jy zHXAywb?Nfu$ED|ax`0ukrl3$L2;6M~Y+c~|6x}njN;nS|6ck9G`Z7&alKn;F&y8ER zYBj#{4yfta#yx{m@xR7k%;PASD~mW2rpwQT4D zg_2UY%NNl<=AWz(O&)#|ah#bs$a#<^e&$4%ujfm#__IrwE{&JFG7mR)%{BGaY~m>C zYyO5xododT_8+j}Ui0zMwYRtTZkpu1UzyGl=W&=e^;diQ{|iJfXZn~fBB=h`new`G z=a1FfWm@L%2^eWE>+@b0WxmR_Y|e1wgQ!*Vl1+}&CjzFwo?^QV>Nayw8>`Zj2r{Z* z=(vn@WK^UaS1$vVgzaKutQ=kDDN7Ja5s3TPEQsZ%u)K*D6J_EWx~Vm{gFNKvKYlVZ`RuzDm%L+PxIe%aiSYd41 zmqQw(1V!SXd_JPM-Q>ri`4mpk-U@b$)5^C*Zt{Ec$YfT-nsN($3sx5M62wP+vFhep zqYoz4Hw9~ZXxK;=+&;NtLsHe+^%=2*tPanmRS&6YWh#v=j=tX!RUo$#k3L(f-bta~ zwlOiIS>rt@TVAyKUGDWe8mS_%+wMQ$^DLZ%b$+#z%I2R6!k=RnBcOTt5bXRlqSR{* zK_PS=7JA0)i)5*r(>Q!%4RO(BD6;zMSZ2<24oX`RtUvfBo@JOPXlI>xbWU9^=}B&ofxML{Z?{`YY@E z)WyO4mKgqfR)Z4tqvcXkLxioScwH#`kUY$p>qlFFYV1uCHk(1J2e6Y3D+Q|pQ~PaA)}X*x~z z2`nQvriZq6;Ocq9uB^A>bm%x5En*Y3E;dSizM|Z@$#K68uD3)JE#%Jz!ZGBllAwW`aXk!_qjk9Suh z(#W_&87D{2vEFsTmr`1EhNEIuk1Ek^IE|~$=9ZQ7|C79*!uJTf7vEQaTx|)HG+Sn! z{cXR7J{KKb-56Nc#0#{~bMJ60Z+L!% zSGRflaee&|9_M;C2bcqZW=gLvT$hRr*;8gx72+F=4&$9}J(;^`R|SYx(K1v0C+XQO z=Mg@R6r@L=F;ANa6Y+N+Xnr}+m$lukro!B#WhADpDf*T%t*EKAxj3{B(wN21FE0u6 z)t=@H6Ki|K6`oR91-_nsVvn}3-_FnADM8&_60&Q(k^YV{!h3c|qcAcZ?z*iupC0TM z%^Q8Bn{1qfjB7B*dq-!s7GSGvDyW%`&rh_#1QPiZR*Je|CNh5<3uv3=?ccOn-}fMwHE-tQT*I#qFv>kGBa5DVw6Wt`OM#gP8wY%GD+{*!k1=PB+;Xbf!Cn`*Ib%XjlYV=xevp$|L zl_h9(jSbo`#aJu%ZBMo4yP>}l*+}BF@H!F!ujpkbnHJh|7Zhb?=VPKax*AeiYIkT9j>U8)n-RiokVSEf#2Vb8O16`XN)(khf%m z0s?55(!O%z9eu-(YsORQm)_Uh_7~7BC@RRs5F~r^Xaa~_A%uH+y|21k(i*; zuym*A12cVx&{=jCw(+L7?X%xZY~yZpt&s|~c$-g%_*YH2ZX<=Q_6$ht(x<;FRAfR% zY6F&Idr}KZbT80^mZ70xMaQdGuQqRrZytPn%h)9zl7%*G1f-0aF9Xsejy+li-53Jb z))Q5gm8|5W=gdBe1C^VN8dtDO_Mw83W|`Y9un+F5vG0i!l+*B~=Ix!5RMFs`?rwV+ zFWTUgDMNyg$Q=9OM7$RKmT)Z?JTm(SD1ADLf=_4gE*DG@%BR3O4wD9B9IBz24s(T=jE zp#A19+0EuZx{cIYt&q;-3n2)LUq?f#Rxn3eaH>p(6%~2L-<$UtY^m5@T2bk6qeqzc zW@&1M2M}EQ=O<^lcDX)Ay+$+vm$KuHviyp9V;#1HFGlWQ3EnxgGk0Xk;>C;oYPh!^ zyFqB>5BKD)kziz^rZ~{wzrksQ5KPEBD~Sx=*U2;q5K#Y!$Z+%aTQ72^0EjeE^$|7o zJ7R=G%MT2j5iLJ1G?te)^iH|7+NLx{=GYlF%G)Ee_E3tyKsn&r>HGkVzSP_XvX~Ow zv6p|F4)U%=vr;;`d)z1QRoivLOi&-;-A8mUB1GC$#cRbf-VJ>FAeU~VcSsh=I#jWSjUTyDL$AqM~`|lq`jBkfe?JDz_yMfJPSQZj59xtk5@D#|5ol?kK!E+t{)~ zemV14feX`jJakxo9Q?-~WPst?1_q4M(o#mzzJ2>>{E%jSUu)SM!q1v?oFo2(N*v#q zuzGz$_ruxZ$(4e5cuuwQ4hn@UjeJJ{6c2l8j$9AE723Oa-FnW>q}Cpgr#9m6VGU(! z$1W+0R_BGhn>eUOLPcLwtO8C*X`goSGFT#M_K(NxY41Wbzk3T(Kr^V`cr1 z{(Tv<9e;bDzT3Xjnod!D`$&kG;kJLayU4Tw7>=i;8ZaYV0mv`mf#re3mn_R`%n1oB1I==NFvm|E`a@s(R_t zCD8C&xo1#iz`BK3RYbG3?^$;kf?zu=a}vgOfzWlID==mMWq*I8pkzSy()4bnp1{ve;j7igg%?N~C#QCJYCC@@_! z0+JJ*eal^)Z=Ww&wn2Sla;||{|7x$0B0zWu%3+`9Yih+uNqh=_V3`Z;gj;=#LVpQ} znP&QW9_q{FXsI3(?&KFp+?N9;adr582n@4hhW9(-J+JP)n5pdK6ZFNDA=ts=5nS)!b+nT}+|6c`NxtQUKoiakhY! zG&FCI%Myn`A4#>UQL3VRkLAuk`*0z56+9|+XOm|Y4V7(Egi)i(J_M?M5ZsS8P zH*YT05x#!y8YJ4q0t0#&KD^Ab zsEp1&mp)K@aj>Z>DJW>{-@m^i^Ydpz!ey$BZ-y1cd$l}Yj4-8AWr_vsxj$K6mY z1)GhaO}-V!;3T-PT-B1Vkw-%r%SJDS;%UV|NQebFO(wEeLHM^({3gi?ZU zjD-7|06Xn&9)FDLReqJ-oP0uOb9izF>LNYACnP2w1SD*9sO~QmX24NpxKCww=k>wvGTcDJL^lQwej^@{Md zR^k;X1=W$Yqi65)ObtqXOW7;}iW!|T@T4>3W+_w_y5@t^22lk8!X zJo3j-Q~5z3QRb`?x6nu{GRm|+uzUA;?D6-$e*MarR7iBtE-4jOPR3t}Jm5VfOx(tX zX@T&7(P)dXqusBz!63DkDvKsnkJGL1vm9Z_;o7~o*J&s!{u84TQp!KD{NIuCv?JTe zgRgdv9hz>=|Cm`DuQem>+`u)1j8$6>)2mZkjbm$)L^HC69T1`_+i%*`JQ7NH)z38i z(K$?Gs8xdG0<$}uQ_DMAKZCx@EfdDHM6#bZhuRW=nbSF$@l1ZAKe3q|DR`79tvdbF zeIwg_`(DN;Ccf$1_5t5PQF%W@sA%Dvt7#B|bRda$cXu=PKLy-5aO6mYrlw|Fw(qxZ z7R3CEa=H8I@7ntqac`XR1XSr!x=99>a9g;b&ph!+=hv;&zrk;ImX%U4#qh~b$*!`9R?KQ8i_I){j!`c?ZAL= za)R_yWF20qYh(zS%i|lEafs`a>gg;(b5Q&}H*vD@JW#^d1)BfzMy!WD|01;Prw2AQ_S0PFP9RzTb$BX3 z2KMx8am-q;i1C!TJ9*J_{q{zjJ^V{Tb@L-5L{pGcPf@g~ofGp})Q&IS!c)2{HkR1H29 zd09Lf@2i$RXtt9qPdc=n-tfl@j(C&x^7N6ALu={2Flh(c>qaqd0eA3dByi@vIe@8zSL^JXNC8ZV(*;8s!&Be5H?J>%HjlyV%!!{IlEkP zq+JIeU`YzY+W30rY+zp{H8ovh<2$f3tNaf>_e6~@Aw)@#FXJl=3h%g0_(cQ2FEVS__5lkN>Bzru3f=P`HR8;?Ki^ z?_XY&^?3MstP4-hUM9jvTUAP|L8?UrT=6Q%(C)V?P)l&Se&+fWsbGF^13ybrT0V~ zO11DO?gv=*i5)hx;qt29s^fQcYA%~DvODDvzs^UI+CfkOYE(9Xtb*SZ@wO8AuzuQ| z#)TSL)ooYt!vjE{X0A`)Zfjiew~r!afg~m|)eHVsJ#pK*oh(BH$PEXy?Y8{#;-A9e zVhQfKoLq3svFHFM5i38+{Cu(4rMo~N&8%P%K>%2`T-#6HMzWER9CwC{xjA&boAD%K`c$P2^~h|6ulR|D&Qw_s~1BnG|-GJP|&0Tk_N%z>hZYE@@3+JDfg(Np40)>Vz4_U7p9YbE6^=Bd}ML9hzkJmBTi>tG0N%rCQk*69Z89vcOd@F;yz$&p(W)WrD zGHf{u&I=V0LvROIk*9*hsV^@WJ)p4BKqQkeMxfsmA#4PeKj4adx1g2%!1m?UcdGY9 zJsxTfO?Wqd#TF{in$EzF+BC-wgIe?ta!;1$UZaT;ltTp16X%U7_iO&}lcN<2$qxwe z$xt`G!I?7***p;vK1n`w+A+VoNWGPL<2s$$Jw7xVjS+N4g}v>a3AYjF zYySFY1yxYGKk7yEk4)G@qmPgze{7al_ZQFu5N-6BE&)9?qz$zE{mkh65g zWLtu#QO?8JHSTfFY1yKRiC3paw-$S>7Fd;Hp(!{!)>~IB+9D#n_*$k#1gGFV{y8@H zsE<@PUd!CLQ84lP$-<3$uf{3Jm933SV6)z{Ml9u0{tNmt@1G-igA*RlZ4Ja#!h!~7 z=Q=l^%Wb*cGH2(`jlM0E!Rua^E^XShNfDdu?jHOH^GoS!B)hu}x(2$84nCYL=SU6? z4h~;!WodZ;MQ40haP4L(RTn;HyAEm2=iSBszDutyF&oXO9?|LaUR-!S9e&J&`1pT5 zKQoDsh}aD^jo3u_yE*;F2bdMOogi#AfkClUFZOE%c=o5=l4tHwWp}?JwjZBdzOG(i zGysptu;*?=Q9pz)to3R8Ve8b?(gF(vEEBItDfuT6WXF?;dF+%Dx3%lmr5%6IL;O^z z#FlpM3Wx2+{>Nu2AdySbJNdpke*d&4t5(GH1|uPXq`PfxR5``+A@VP2OQsB3+BErBSm~&# z`2Ylty5~XO#0u>j>N$>N$6r2w0y%mcijs(7$dirZYtIn=n57w*r&O6eJQ$})-BDe% z?(Tbej2jhi^6c3&vl|~^DF(T5DoobHSl(y#hmr`nc=`PK7J|Mgr6a#RsObQF_b9>A z@T=+g2yJSRdnQFXl#Md#pmt}c<>fgmkK&!)LX}a$-HwxEcV7ube+x9JSI1I9*f{1Y@O5lGt$%P@Lz=hZJWa7i)~XCOR7X_m;0y%rnUg$Pdzat zpA6S!DEhS~pZ41MjQQh`6o0|(9Zn(-F4L!_Cgtpm+`uM#3N4!rmAX6!d_8?a#Y``g>h1ci61ZO9mn%#&QyTtEk|>n%CD|OmC3zMslm2hu_U-=y zZWsSMp_xaBfa9$f&pRi~XohQw|9U!5r?m&kf9k%7-{mR1R`e z%-6Tg%S>;~bbx_~9Mp5hIW9{-<=VH>`hV0WFc^&d+nC=Vb>tS(`QvkQjZbDAl8}%9 zekqyHeWtHX2mYP7`}fs!6Vk3vb<3dY)z6$tTn2?-ym&EfKVEMB@L(=|qBT77g&)WLhD>F|uUf%D`75(Oo0@;>$ce!#J7Ol94t>#UKBgilkzriot zymA`qHpkkWCSd}-jD%iISPqvgTo{2W({I@D&yOL%Sy@aq0gaPi!V2upZTP03-OlZz)3`4~H)=W||*Au1s_6tV@cFTpPB_&EWZ! z`FaQ}U^mvo`3VuiwTBDaKXRUV^rE+KwUw2xBgWx0)*v`w45#;BL8QewJN@|l0Xj(5 zkEuCAz4iN!d0_U27Yd(WOfzu1g+dx_w* z7)S&Wa6W=ayLi-}|H|`)y~A>o!#Zm0;sxIZ>j+P%I+32A{_^O|;nDYo*47F9)7_1; z994OI1-5{>0hN$<&Kp8mJlfM)h{(toVMZC`xTN8dr>YyK5r2TUp-82A?Ucs9=gO~Z zs5gOsKsc-RerBdL1YRMF7cXDB4LnB7J)2XP+}!x(3#6 zlQ|(u|C4{^Z{x_ztxy|Z4eA;_kR)uX#dF+7bb(&90Gk8xeUZhD7~jALFcB{X3Ouo(ZOfnF)Q*sJh-Lp z6>zachDN{zS=du+x=iRGUuba*;PIyQ>-W*V4xpoONIZHR@7urM3T@VkEe`bG7=P90eYkH!Qk;?Ky`+ T&$Z&@8H)SV_TJxf_`?4Ig(Bk+ literal 0 HcmV?d00001 diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt b/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt index b25de7b0..8672ee07 100644 --- a/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt +++ b/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt @@ -1,154 +1,31 @@ -$ (ulimit -t 10 && time ./optic_planner pddl/domain_survey.pddl pddl/problem_jem_survey.pddl) -Number of literals: 215 -Constructing lookup tables: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] -Post filtering unreachable actions: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] -(robot-order bumble) has finite bounds: [-1.000,4.000] -(robot-order honey) has finite bounds: [-1.000,4.000] -Have identified that smaller values of (robot-order bumble) are preferable -Have identified that smaller values of (robot-order honey) are preferable -Seeing if metric is defined in terms of task vars or a minimal value of makespan -- Yes it is -Recognised a monotonic-change-induced limit on -1.000* -- Must be >= the metric -Looking for achievers for goal index 0, fact (completed-panorama bumble o0 jem_bay4) with fID 83 - (panorama bumble o0 jem_bay4) -For limits: literal goal index 0, fact (completed-panorama bumble o0 jem_bay4), could be achieved by operator (panorama bumble o0 jem_bay4), which has other interesting effects (including one on (robot-available bumble) ) -Looking for achievers for goal index 1, fact (completed-panorama bumble o1 jem_bay3) with fID 75 - (panorama bumble o1 jem_bay3) -For limits: literal goal index 1, fact (completed-panorama bumble o1 jem_bay3), could be achieved by operator (panorama bumble o1 jem_bay3), which has other interesting effects (including one on (robot-available bumble) ) -Looking for achievers for goal index 2, fact (completed-panorama bumble o2 jem_bay2) with fID 67 - (panorama bumble o2 jem_bay2) -For limits: literal goal index 2, fact (completed-panorama bumble o2 jem_bay2), could be achieved by operator (panorama bumble o2 jem_bay2), which has other interesting effects (including one on (robot-available bumble) ) -Looking for achievers for goal index 3, fact (completed-panorama bumble o3 jem_bay1) with fID 59 - (panorama bumble o3 jem_bay1) -For limits: literal goal index 3, fact (completed-panorama bumble o3 jem_bay1), could be achieved by operator (panorama bumble o3 jem_bay1), which has other interesting effects (including one on (robot-available bumble) ) -Looking for achievers for goal index 4, fact (completed-stereo bumble o4 jem_bay1 jem_bay4) with fID 123 - (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) (stereo bumble o4 jem_bay1 jem_bay4 jem_bay3 jem_bay5) -For limits: literal goal index 4, fact (completed-stereo bumble o4 jem_bay1 jem_bay4), could be achieved by operator (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3), which has other interesting effects (including one on (location-available jem_bay4) ) -For limits: literal goal index 5, fact (robot-at bumble berth1), is static or a precondition -Looking for achievers for goal index 6, fact (completed-let-other-robot-reach honey o0 jem_bay5) with fID 155 - (let-other-robot-reach honey o0 jem_bay5 bumble) - Looking at numeric effects of (let-other-robot-reach honey o0 jem_bay5 bumble): 1 and 0 -For limits: literal goal index 6, fact (completed-let-other-robot-reach honey o0 jem_bay5), could be achieved by operator (let-other-robot-reach honey o0 jem_bay5 bumble), which has a non-trivial numeric effect ((robot-order honey) = 0.000) -Looking for achievers for goal index 7, fact (completed-panorama honey o1 jem_bay7) with fID 116 - (panorama honey o1 jem_bay7) -For limits: literal goal index 7, fact (completed-panorama honey o1 jem_bay7), could be achieved by operator (panorama honey o1 jem_bay7), which has other interesting effects (including one on (robot-available honey) ) -Looking for achievers for goal index 8, fact (completed-panorama honey o2 jem_bay6) with fID 108 - (panorama honey o2 jem_bay6) -For limits: literal goal index 8, fact (completed-panorama honey o2 jem_bay6), could be achieved by operator (panorama honey o2 jem_bay6), which has other interesting effects (including one on (robot-available honey) ) -Looking for achievers for goal index 9, fact (completed-panorama honey o3 jem_bay5) with fID 100 - (panorama honey o3 jem_bay5) -For limits: literal goal index 9, fact (completed-panorama honey o3 jem_bay5), could be achieved by operator (panorama honey o3 jem_bay5), which has other interesting effects (including one on (robot-available honey) ) -Looking for achievers for goal index 10, fact (completed-stereo honey o4 jem_bay7 jem_bay4) with fID 124 - (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3) (stereo honey o4 jem_bay7 jem_bay4 jem_bay3 jem_bay5) -For limits: literal goal index 10, fact (completed-stereo honey o4 jem_bay7 jem_bay4), could be achieved by operator (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3), which has other interesting effects (including one on (location-available jem_bay4) ) -For limits: literal goal index 11, fact (robot-at honey berth2), is static or a precondition -Assignment numeric effect ((robot-order bumble) = 0.000) makes effects on 0 be order-dependent -Assignment numeric effect ((robot-order honey) = 0.000) makes effects on 1 be order-dependent -Assignment numeric effect ((robot-order bumble) = 1.000) makes effects on 0 be order-dependent -Assignment numeric effect ((robot-order honey) = 1.000) makes effects on 1 be order-dependent -Assignment numeric effect ((robot-order bumble) = 2.000) makes effects on 0 be order-dependent -Assignment numeric effect ((robot-order honey) = 2.000) makes effects on 1 be order-dependent -Assignment numeric effect ((robot-order bumble) = 3.000) makes effects on 0 be order-dependent -Assignment numeric effect ((robot-order honey) = 3.000) makes effects on 1 be order-dependent -Assignment numeric effect ((robot-order bumble) = 4.000) makes effects on 0 be order-dependent -Assignment numeric effect ((robot-order honey) = 4.000) makes effects on 1 be order-dependent -27% of the ground temporal actions in this problem are compression-safe -Initial heuristic = 29.000, admissible cost estimate 930.008 -b (28.000 | 630.001) -Resorting to best-first search -Running WA* with W = 5.000, not restarting with goal states -b (28.000 | 630.001)b (28.000 | 70.003)b (27.000 | 870.005)b (26.000 | 870.005)b (25.000 | 880.005)b (24.000 | 880.005)b (23.000 | 1670.007)b (22.000 | 1670.007)b (21.000 | 1680.007)b (20.000 | 1680.007)b (19.000 | 2470.009)b (18.000 | 2470.009)b (17.000 | 2470.009)b (16.000 | 3270.011)b (15.000 | 3270.011)b (14.000 | 3870.012)b (13.000 | 3890.013)b (12.000 | 4470.013)b (12.000 | 3930.015)b (11.000 | 3950.016)b (10.000 | 3970.017)b (9.000 | 3990.018)b (7.000 | 4020.019)b (6.000 | 4650.021)b (4.000 | 4890.024)b (3.000 | 4910.025)b (2.000 | 5510.026)b (1.000 | 5510.026)(G) -; LP calculated the cost +$ ./tools/survey_planner.py +0.000: (undock bumble berth1 jem_bay7 jem_bay6 jem_bay8) [30.000] +30.001: (move bumble jem_bay7 jem_bay6 jem_bay5) [20.000] +50.002: (move bumble jem_bay6 jem_bay5 jem_bay4) [20.000] +70.003: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000] +70.004: (undock honey berth2 jem_bay7 jem_bay6 jem_bay8) [30.000] +90.004: (panorama bumble o0 jem_bay4) [780.000] +100.005: (panorama honey o1 jem_bay7) [780.000] +870.005: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] +880.006: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] +890.006: (panorama bumble o1 jem_bay3) [780.000] +900.007: (panorama honey o2 jem_bay6) [780.000] +1670.007: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] +1680.008: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] +1690.008: (panorama bumble o2 jem_bay2) [780.000] +1700.009: (panorama honey o3 jem_bay5) [780.000] +2470.009: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] +2480.010: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] +2490.010: (panorama bumble o3 jem_bay1) [780.000] +2500.011: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] +2520.012: (stereo honey o4 jem_bay7 jem_bay4 jem_bay3 jem_bay5) [600.000] +3120.013: (dock honey jem_bay7 berth2) [30.000] +3270.011: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay3 jem_bay5) [600.000] +3870.012: (move bumble jem_bay1 jem_bay2 jem_bay3) [20.000] +3890.013: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000] +3910.014: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000] +3930.015: (move bumble jem_bay4 jem_bay5 jem_bay6) [20.000] +3950.016: (move bumble jem_bay5 jem_bay6 jem_bay7) [20.000] +3970.017: (move bumble jem_bay6 jem_bay7 jem_bay8) [20.000] +3990.018: (dock bumble jem_bay7 berth1) [30.000] -; Plan found with metric 5540.027 -; Theoretical reachable cost 5540.028 -; States evaluated so far: 504 -; States pruned based on pre-heuristic cost lower bound: 0 -; Time 1.64 -0.000: (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) [30.000] -30.001: (move bumble jem_bay7 jem_bay6 jem_bay5) [20.000] -50.002: (move bumble jem_bay6 jem_bay5 jem_bay4) [20.000] -70.003: (let-other-robot-reach honey o0 jem_bay5 bumble) [0.000] -70.004: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000] -70.004: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] -90.005: (panorama bumble o0 jem_bay4) [780.000] -100.005: (panorama honey o1 jem_bay7) [780.000] -870.006: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] -880.006: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] -890.007: (panorama bumble o1 jem_bay3) [780.000] -900.007: (panorama honey o2 jem_bay6) [780.000] -1670.008: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] -1680.008: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] -1690.009: (panorama bumble o2 jem_bay2) [780.000] -1700.009: (dock honey jem_bay7 berth2) [30.000] -2470.010: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] -2490.011: (panorama bumble o3 jem_bay1) [780.000] -3270.012: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) [600.000] -3870.013: (move bumble jem_bay1 jem_bay2 jem_bay3) [20.000] -3890.014: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000] -3910.015: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000] -3930.016: (move bumble jem_bay4 jem_bay5 jem_bay6) [20.000] -3950.017: (move bumble jem_bay5 jem_bay6 jem_bay7) [20.000] -3970.018: (move bumble jem_bay6 jem_bay7 jem_bay8) [20.000] -3990.019: (dock bumble jem_bay7 berth1) [30.000] -4020.020: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] -4050.021: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] -4070.022: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] -4090.023: (panorama honey o3 jem_bay5) [780.000] -4870.024: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] -4890.025: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] -4910.026: (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3) [600.000] -5510.027: (dock honey jem_bay7 berth2) [30.000] - - * All goal deadlines now no later than 5540.027 -b (1.000 | 5470.024)(G) -; LP calculated the cost - -; Plan found with metric 5500.025 -; Theoretical reachable cost 5500.026 -; States evaluated so far: 866 -; States pruned based on pre-heuristic cost lower bound: 1 -; Time 2.85 -0.000: (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) [30.000] -30.001: (move bumble jem_bay7 jem_bay6 jem_bay5) [20.000] -50.002: (move bumble jem_bay6 jem_bay5 jem_bay4) [20.000] -70.003: (let-other-robot-reach honey o0 jem_bay5 bumble) [0.000] -70.004: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000] -70.004: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] -90.005: (panorama bumble o0 jem_bay4) [780.000] -100.005: (panorama honey o1 jem_bay7) [780.000] -870.006: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] -880.006: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] -890.007: (panorama bumble o1 jem_bay3) [780.000] -900.007: (panorama honey o2 jem_bay6) [780.000] -1670.008: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] -1690.009: (panorama bumble o2 jem_bay2) [780.000] -2470.010: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] -2490.011: (panorama bumble o3 jem_bay1) [780.000] -3270.012: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) [600.000] -3870.013: (move bumble jem_bay1 jem_bay2 jem_bay3) [20.000] -3870.013: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] -3890.014: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000] -3890.014: (panorama honey o3 jem_bay5) [780.000] -4670.015: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] -4690.016: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000] -4690.016: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] -4710.017: (dock honey jem_bay7 berth2) [30.000] -4710.017: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] -4730.018: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] -4740.018: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] -4770.019: (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3) [600.000] -5370.020: (dock honey jem_bay7 berth2) [30.000] -5370.020: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000] -5390.021: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000] -5410.022: (move bumble jem_bay4 jem_bay5 jem_bay6) [20.000] -5430.023: (move bumble jem_bay5 jem_bay6 jem_bay7) [20.000] -5450.024: (move bumble jem_bay6 jem_bay7 jem_bay8) [20.000] -5470.025: (dock bumble jem_bay7 berth1) [30.000] - - * All goal deadlines now no later than 5500.025 - -real 0m10.009s -user 0m9.767s -sys 0m0.240s diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml b/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml index 7c624cb1..45b0da90 100644 --- a/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml +++ b/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml @@ -25,7 +25,7 @@ - -8.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '70.004' +- start_time_seconds: '70.003' action: type: move robot: bumble @@ -41,7 +41,7 @@ type: undock robot: honey duration_seconds: '30.000' -- start_time_seconds: '90.005' +- start_time_seconds: '90.004' action: type: panorama robot: bumble @@ -61,7 +61,7 @@ - -9.7 - 4.8 duration_seconds: '780.000' -- start_time_seconds: '870.006' +- start_time_seconds: '870.005' action: type: move robot: bumble @@ -83,7 +83,7 @@ - -9.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '890.007' +- start_time_seconds: '890.006' action: type: panorama robot: bumble @@ -103,7 +103,7 @@ - -9.0 - 4.8 duration_seconds: '780.000' -- start_time_seconds: '1670.008' +- start_time_seconds: '1670.007' action: type: move robot: bumble @@ -114,89 +114,49 @@ - -5.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '1690.009' - action: - type: panorama - robot: bumble - location_name: jem_bay2 - location_pos: - - 11.0 - - -5.0 - - 4.8 - duration_seconds: '780.000' -- start_time_seconds: '2470.010' +- start_time_seconds: '1680.008' action: type: move - robot: bumble - from_name: jem_bay2 - to_name: jem_bay1 + robot: honey + from_name: jem_bay6 + to_name: jem_bay5 to_pos: - 11.0 - - -4.0 + - -8.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '2490.011' +- start_time_seconds: '1690.008' action: type: panorama robot: bumble - location_name: jem_bay1 + location_name: jem_bay2 location_pos: - 11.0 - - -4.0 - - 4.8 - duration_seconds: '780.000' -- start_time_seconds: '3270.012' - action: - type: stereo - robot: bumble - fplan: jem_stereo_mapping_bay1_to_bay3.fplan - base_name: jem_bay1 - bound_name: jem_bay4 - duration_seconds: '600.000' -- start_time_seconds: '3870.013' - action: - type: move - robot: bumble - from_name: jem_bay1 - to_name: jem_bay2 - to_pos: - - 11.0 - -5.0 - 4.8 - duration_seconds: '20.000' -- start_time_seconds: '3870.013' + duration_seconds: '780.000' +- start_time_seconds: '1700.009' action: - type: move + type: panorama robot: honey - from_name: jem_bay6 - to_name: jem_bay5 - to_pos: + location_name: jem_bay5 + location_pos: - 11.0 - -8.0 - 4.8 - duration_seconds: '20.000' -- start_time_seconds: '3890.014' + duration_seconds: '780.000' +- start_time_seconds: '2470.009' action: type: move robot: bumble from_name: jem_bay2 - to_name: jem_bay3 + to_name: jem_bay1 to_pos: - 11.0 - - -6.0 + - -4.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '3890.014' - action: - type: panorama - robot: honey - location_name: jem_bay5 - location_pos: - - 11.0 - - -8.0 - - 4.8 - duration_seconds: '780.000' -- start_time_seconds: '4670.015' +- start_time_seconds: '2480.010' action: type: move robot: honey @@ -207,18 +167,17 @@ - -9.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '4690.016' +- start_time_seconds: '2490.010' action: - type: move + type: panorama robot: bumble - from_name: jem_bay3 - to_name: jem_bay4 - to_pos: + location_name: jem_bay1 + location_pos: - 11.0 - - -7.0 + - -4.0 - 4.8 - duration_seconds: '20.000' -- start_time_seconds: '4690.016' + duration_seconds: '780.000' +- start_time_seconds: '2500.011' action: type: move robot: honey @@ -229,54 +188,40 @@ - -9.7 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '4710.017' +- start_time_seconds: '2520.012' + action: + type: stereo + robot: honey + fplan: jem_stereo_mapping_bay4_to_bay7.fplan + base_name: jem_bay7 + bound_name: jem_bay4 + duration_seconds: '600.000' +- start_time_seconds: '3120.013' action: type: dock robot: honey berth: berth2 duration_seconds: '30.000' -- start_time_seconds: '4710.017' +- start_time_seconds: '3270.011' action: - type: move + type: stereo robot: bumble - from_name: jem_bay4 - to_name: jem_bay3 - to_pos: - - 11.0 - - -6.0 - - 4.8 - duration_seconds: '20.000' -- start_time_seconds: '4730.018' + fplan: jem_stereo_mapping_bay1_to_bay3.fplan + base_name: jem_bay1 + bound_name: jem_bay4 + duration_seconds: '600.000' +- start_time_seconds: '3870.012' action: type: move robot: bumble - from_name: jem_bay3 + from_name: jem_bay1 to_name: jem_bay2 to_pos: - 11.0 - -5.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '4740.018' - action: - type: undock - robot: honey - duration_seconds: '30.000' -- start_time_seconds: '4770.019' - action: - type: stereo - robot: honey - fplan: jem_stereo_mapping_bay4_to_bay7.fplan - base_name: jem_bay7 - bound_name: jem_bay4 - duration_seconds: '600.000' -- start_time_seconds: '5370.020' - action: - type: dock - robot: honey - berth: berth2 - duration_seconds: '30.000' -- start_time_seconds: '5370.020' +- start_time_seconds: '3890.013' action: type: move robot: bumble @@ -287,7 +232,7 @@ - -6.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '5390.021' +- start_time_seconds: '3910.014' action: type: move robot: bumble @@ -298,7 +243,7 @@ - -7.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '5410.022' +- start_time_seconds: '3930.015' action: type: move robot: bumble @@ -309,7 +254,7 @@ - -8.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '5430.023' +- start_time_seconds: '3950.016' action: type: move robot: bumble @@ -320,7 +265,7 @@ - -9.0 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '5450.024' +- start_time_seconds: '3970.017' action: type: move robot: bumble @@ -331,7 +276,7 @@ - -9.7 - 4.8 duration_seconds: '20.000' -- start_time_seconds: '5470.025' +- start_time_seconds: '3990.018' action: type: dock robot: bumble diff --git a/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl index 5e197671..00f50c7d 100644 --- a/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl @@ -72,7 +72,7 @@ ;; completed-panorama: The goal to add if you want the plan to include collecting a ;; panorama. For now, goals specify ?robot and ?order parameters that constrain ;; multi-robot task allocation and task ordering. - (completed-panorama ?robot - robot ?order - order ?location - location ) + (completed-panorama ?robot - robot ?order - order ?location - location) ;; completed-stereo: The goal to add if you want the plan to include collecting a stereo ;; survey. For now, goals specify ?robot and ?order parameters that constrain multi-robot @@ -83,13 +83,13 @@ ;; used for collision checking. It's assumed that ?base and ?bound are not adjacent ;; locations. If future stereo surveys violate these assumptions the model will need to be ;; revisited. - (completed-stereo ?robot - robot ?order - order ?base ?bound - location ) + (completed-stereo ?robot - robot ?order - order ?base ?bound - location) ;; completed-let-other-robot-reach: The goal to add if you want one robot to wait for the ;; other to reach a certain location before pursuing its remaining goals (ones with larger ;; ?order values). This basically enables a user to provide a specific kind of ;; between-robots ordering hint to the planner. - (completed-let-other-robot-reach ?robot - robot ?order - order ?loc - location ) + (completed-let-other-robot-reach ?robot - robot ?order - order ?loc - location) ) (:functions @@ -326,7 +326,7 @@ ?other-loc - location ;; location other robot needs to reach ?other-robot - robot ) - :duration (= ?duration 0) + :duration (= ?duration 0.001) ;; VAL won't accept 0-duration actions :condition (and ;; Check robot mutex diff --git a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl index 8cc991f1..4e44cdfb 100644 --- a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl @@ -26,9 +26,3 @@ {{ dynamic_fluents }} ) ;; end :init ) ;; end problem - -;; Include raw high-level config in problem in case an (ISAAC-custom) planner prefers to use it. - -;; BEGIN CONFIG -{{ config }} -;; END CONFIG diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl index d099ca79..04aca907 100644 --- a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl @@ -22,7 +22,6 @@ (completed-panorama bumble o3 jem_bay1) (completed-stereo bumble o4 jem_bay1 jem_bay4) (robot-at bumble berth1) - (completed-let-other-robot-reach honey o0 jem_bay5) (completed-panorama honey o1 jem_bay7) (completed-panorama honey o2 jem_bay6) (completed-panorama honey o3 jem_bay5) @@ -162,121 +161,3 @@ (= (robot-order honey) -1) ) ;; end :init ) ;; end problem - -;; Include raw high-level config in problem in case an (ISAAC-custom) planner prefers to use it. - -;; BEGIN CONFIG -;; # Copyright (c) 2023, United States Government, as represented by the -;; # Administrator of the National Aeronautics and Space Administration. -;; # -;; # All rights reserved. -;; # -;; # The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking -;; # platform" software is licensed under the Apache License, Version 2.0 -;; # (the "License"); you may not use this file except in compliance with the -;; # License. You may obtain a copy of the License at -;; # -;; # http://www.apache.org/licenses/LICENSE-2.0 -;; # -;; # Unless required by applicable law or agreed to in writing, software -;; # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -;; # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -;; # License for the specific language governing permissions and limitations -;; # under the License. -;; -;; # Static configuration info used when generating a PDDL problem and also when executing actions in a -;; # PDDL plan. This info should be static in the sense that it nominally doesn't change during an ISS -;; # activity, so the survey manager doesn't have to modify it. However, an edge case is that an -;; # operator might want to manually edit something in here (like add a new symbolic location or nudge -;; # the position of a named bay away from an obstacle) and restart the survey manager. On the other -;; # hand, info that is *expected* to change as part of the survey manager conops belongs in -;; # jem_survey_dynamic.yaml. -;; -;; # Useful reference for positions and stereo survey trajectories: -;; # https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure -;; -;; bays: -;; # 3D coordinates for symbolic bays in ISS Analysis Coordinate System used by Astrobee -;; jem_bay1: [11.0, -4.0, 4.8] -;; jem_bay2: [11.0, -5.0, 4.8] -;; jem_bay3: [11.0, -6.0, 4.8] -;; jem_bay4: [11.0, -7.0, 4.8] -;; jem_bay5: [11.0, -8.0, 4.8] -;; jem_bay6: [11.0, -9.0, 4.8] -;; jem_bay7: [11.0, -9.7, 4.8] -;; -;; bogus_bays: [jem_bay0, jem_bay8] -;; berths: [berth1, berth2] -;; robots: [bumble, honey] -;; -;; stereo: -;; # Meta-data about stereo survey options -;; jem_bay1_to_bay3: -;; # fplan: Name of external fplan specification of trajectory in astrobee_ops/gds/plans/ISAAC/ -;; fplan: "jem_stereo_mapping_bay1_to_bay3.fplan" -;; # base_location: Where trajectory starts and ends for planning purposes (rough location, not exact) -;; base_location: jem_bay1 -;; # bound_location: The other end of the interval covered by the trajectory, for planner collision -;; # check purposes. (Note a trajectory may fly a bit into a bay that it doesn't claim to cover. -;; # The two surveys that cover the module purposefully overlap.) -;; bound_location: jem_bay4 -;; jem_bay4_to_bay7: -;; fplan: "jem_stereo_mapping_bay4_to_bay7.fplan" -;; base_location: jem_bay7 -;; bound_location: jem_bay4 -;; # Copyright (c) 2023, United States Government, as represented by the -;; # Administrator of the National Aeronautics and Space Administration. -;; # -;; # All rights reserved. -;; # -;; # The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking -;; # platform" software is licensed under the Apache License, Version 2.0 -;; # (the "License"); you may not use this file except in compliance with the -;; # License. You may obtain a copy of the License at -;; # -;; # http://www.apache.org/licenses/LICENSE-2.0 -;; # -;; # Unless required by applicable law or agreed to in writing, software -;; # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT -;; # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the -;; # License for the specific language governing permissions and limitations -;; # under the License. -;; -;; # Example dynamic configuration info used when generating a PDDL problem. For now, this is goal -;; # conditions and initial state. A likely conops is that the initial version of this file for a -;; # specific activity would be hand-generated, but it might later be automatically regenerated by the -;; # survey manager when a replan is needed (remove completed/failed goals, add retry goals, update -;; # initial state to match actual current state, etc.) See also jem_survey_static.yaml. -;; -;; goals: -;; -;; - {type: panorama, robot: bumble, order: 0, location: jem_bay4} -;; - {type: panorama, robot: bumble, order: 1, location: jem_bay3} -;; - {type: panorama, robot: bumble, order: 2, location: jem_bay2} -;; - {type: panorama, robot: bumble, order: 3, location: jem_bay1} -;; - {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3} -;; -;; # We want Bumble to return to its berth at the end of the run, but adding this goal causes POPF to -;; # get confused and greatly increase the total run time. For some reason, it doesn't notice it can -;; # use the same plan as without this goal and then add some motion actions at the end to achieve this -;; # goal. Instead, it falls back to only undocking one robot at a time, which slows things down by -;; # about 2x. -;; - {type: robot_at, robot: bumble, location: berth1} -;; -;; - {type: let_other_robot_reach, robot: honey, order: 0, location: jem_bay5} -;; - {type: panorama, robot: honey, order: 1, location: jem_bay7} -;; - {type: panorama, robot: honey, order: 2, location: jem_bay6} -;; - {type: panorama, robot: honey, order: 3, location: jem_bay5} -;; -;; # This is another objective we want to include that for some reason causes POPF to fail to generate -;; # a plan (hang indefinitely). No obvious reason why it should cause a problem. -;; - {type: stereo, robot: honey, order: 4, trajectory: jem_bay4_to_bay7} -;; -;; - {type: robot_at, robot: honey, location: berth2} -;; -;; init: -;; bumble: -;; location: berth1 -;; honey: -;; location: berth2 -;; END CONFIG diff --git a/astrobee/survey_manager/survey_planner/tools/mypy.ini b/astrobee/survey_manager/survey_planner/tools/mypy.ini new file mode 100644 index 00000000..c967aff3 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/tools/mypy.ini @@ -0,0 +1,16 @@ +# Config for mypy Python type checker - https://mypy.readthedocs.io/ + +[mypy] +# (suppress warning about this section being missing) + +[mypy-pyparsing] +# suppress error "No library stub for module 'pyparsing'" +ignore_missing_imports = True + +[mypy-numpy] +# suppress error "No library stub for module 'numpy'" +ignore_missing_imports = True + +[mypy-matplotlib] +# suppress error "No library stub for module 'matplotlib'" +ignore_missing_imports = True diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/problem_generator.py index 72c77327..cf6cce06 100755 --- a/astrobee/survey_manager/survey_planner/tools/problem_generator.py +++ b/astrobee/survey_manager/survey_planner/tools/problem_generator.py @@ -171,13 +171,6 @@ def __call__(self, match: re.Match) -> str: return self.params[param] -def comment_for_pddl(text: str) -> str: - """ - Return the result of commenting `text` using PDDL (Lisp-like) comment syntax. - """ - return "\n".join([f";; {line}".strip() for line in text.splitlines()]) - - class ProblemWriter(ABC): "Abstract class for writing a problem intance." @@ -345,7 +338,6 @@ def problem_generator( header_lines += f";; Config {i + 1}: {config_path}\n" full_config += config_path.read_text() writer.set_param("header", header_lines) - writer.set_param("config", comment_for_pddl(full_config)) bays = list(config["bays"].keys()) bogus_bays = config["bogus_bays"] diff --git a/astrobee/survey_manager/survey_planner/tools/survey_planner.py b/astrobee/survey_manager/survey_planner/tools/survey_planner.py new file mode 100755 index 00000000..745459c7 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/tools/survey_planner.py @@ -0,0 +1,1020 @@ +#!/usr/bin/env python3 + +""" +Custom planner for JEM survey domain. +""" + +import argparse +import heapq +import io +import itertools +import pathlib +import sys +from abc import ABC +from dataclasses import dataclass, field +from typing import ( + Any, + Callable, + Dict, + Generic, + Iterable, + Iterator, + List, + NamedTuple, + Optional, + Tuple, + Type, + TypeVar, +) + +import pyparsing as pp +import yaml + +from problem_generator import PDDL_DIR + +LocationName = str # Names PDDL object of type location +LocationIndex = int # Index of location in CONFIG.locations +RobotName = str # Names PDDL object of type robot +Duration = float # Time duration in seconds +Cost = int # Count of moves +CostMap = List[Cost] # Interpreted as mapping of LocationIndex -> Cost +RobotStatus = str # Options: ACTIVE, BLOCKED, DONE +Timestamp = float # Elapsed time since start of sim in seconds +EventCallback = Callable[["SimState"], None] +PendingEvent = Tuple[Timestamp, EventCallback] +ExecutionTrace = List["TraceEvent"] +OrderName = str # Names PDDL object of type order +PddlExpression = Any # Actually a pp.ParseResults. But 'Any' satisfies mypy. +PddlActionName = str # Names PDDL action +T = TypeVar("T") # So we can assign parametric types below +PddlTypeName = str # Names PDDL object type +PddlObjectName = str # Names PDDL object + +UNREACHABLE_COST = 999 + + +@dataclass +class Config: + "Container for custom planner config." + + robots: List[RobotName] = field(default_factory=list) + "PDDL objects of type robot" + + locations: List[LocationName] = field(default_factory=list) + "PDDL objects of type location" + + neighbors: List[List[LocationIndex]] = field(default_factory=list) + "Maps each LocationIndex to a list of neighbor indices." + + action_durations: Dict[PddlActionName, Duration] = field(default_factory=dict) + "Maps each PDDL action to its duration in seconds." + + def __post_init__(self) -> None: + "Set `location_lookup` to match `locations`." + + self.location_lookup = {name: ind for ind, name in enumerate(self.locations)} + # pylint: disable-next=pointless-string-statement # doc string + "Maps each PDDL LocationName to its LocationIndex." + + def update_location_lookup(self) -> None: + "Update `location_lookup` to match `locations`." + self.__post_init__() + + +# Will override fields of this empty placeholder during PDDL parsing phase +CONFIG = Config() + + +def get_cost_to_goal(goal_region: Iterable[LocationIndex]) -> CostMap: + """ + Return a cost array that maps each location index to its cost to the nearest goal location in + `goal_region` (measured in number of moves). + """ + result = [UNREACHABLE_COST] * len(CONFIG.locations) + opens = [(g, 0) for g in goal_region] + while opens: + curr, curr_cost = opens.pop(0) + if curr_cost < result[curr]: + result[curr] = curr_cost + next_cost = curr_cost + 1 + for n in CONFIG.neighbors[curr]: + opens.append((n, next_cost)) + return result + + +def get_best_neighbor(cost_map: CostMap, current_pos: LocationIndex) -> LocationIndex: + """ + Return the neighbor of `current_pos` that has lowest cost in `cost_map` (the logical target + location of the next move). Raise RuntimeError if the goal is unreachable from `current_pos`. + """ + if cost_map[current_pos] == UNREACHABLE_COST: + raise RuntimeError( + f"Can't reach goal region from {CONFIG.locations[current_pos]}" + ) + return min(CONFIG.neighbors[current_pos], key=cost_map.__getitem__) + + +def get_move_location( + goal_region: Iterable[LocationIndex], current_pos: LocationIndex +) -> LocationIndex: + """ + Return the neighbor of `current_pos` that is closest to `goal_region` (the logical target + location of the next move). Raise RuntimeError if `goal_region` is unreachable from + `current_pos`. + """ + return get_best_neighbor(get_cost_to_goal(goal_region), current_pos) + + +def is_location_berth(location: LocationName) -> bool: + "Return True if the location is a berth." + return "berth" in location + + +def is_location_flying(location: LocationName) -> bool: + "Return True if the location is a flying location." + return "bay" in location + + +class TraceEvent(NamedTuple): + "Represents an event in an execution trace." + timestamp: Timestamp + action: "Action" + duration: Duration + + def get_dump_dict(self) -> Dict[str, Any]: + "Return a dict to dump for debugging." + return { + "timestamp": self.timestamp, + "action": self.action, + "duration": self.duration, + } + + +@dataclass +class Action(ABC): + "Represents an action that can be invoked by the planner." + + robot: RobotName + "The robot that is executing the action." + + def get_duration(self) -> Duration: + "Return the estimated duration of the action in seconds." + return CONFIG.action_durations[self.get_pddl_name()] + + def invalid_reason( # pylint: disable=unused-argument + self, sim_state: "SimState" + ) -> str: + """ + If action is invalid in `sim_state`, return the reason why. If valid, return an empty + string. The default implementation always returns an empty string. Derived classes can + override as needed. + """ + return "" + + def is_valid(self, sim_state: "SimState") -> bool: + """ + Return True if the action can be executed in `sim_state`. + """ + return self.invalid_reason(sim_state) == "" + + def get_pddl_name(self) -> str: + "Return the name of this action type in the PDDL model." + return self.__class__.__name__.replace("Action", "").lower() + + def get_pddl_args(self) -> List[Any]: + "Return the arguments of this action type in the PDDL model." + raise NotImplementedError() # Note: Can't mark as @abstractmethod due to mypy limitation + + def __repr__(self): + args_str = " ".join((str(arg) for arg in self.get_pddl_args())) + return f"({self.get_pddl_name()} {args_str})" + + def start(self, sim_state: "SimState") -> None: + """ + Modify `sim_state` as needed at the beginning of action execution. Derived classes that + need to extend start() should typically invoke the parent method. + """ + sim_state.trace.append( + TraceEvent(sim_state.elapsed_time, self, self.get_duration()) + ) + robot_state = sim_state.robot_states[self.robot] + robot_state.action = self + # It seems to be a PDDL convention to separate events by an epsilon time difference + # to prevent ambiguity about ordering. + sim_state.elapsed_time += 0.001 + + def end(self, sim_state: "SimState") -> None: + """ + Modify `sim_state` as needed at the end of action execution. Derived classes that need to + extend end() should typically invoke the parent method. + """ + robot_state = sim_state.robot_states[self.robot] + robot_state.action = None + # It seems to be a PDDL convention to separate events by an epsilon time difference + # to prevent ambiguity about ordering. + sim_state.elapsed_time += 0.001 + + def apply(self, sim_state: "SimState") -> None: + """ + Apply the action, modifying `sim_state`. + """ + end_time = sim_state.elapsed_time + self.get_duration() + self.start(sim_state) + sim_state.events.push((end_time, self.end)) + + +@dataclass +class RobotState: + "Represents the state of a robot." + + pos: LocationName + "The current location of the robot." + + action: Optional[Action] + "The action the robot is currently executing (or None if it is idle)." + + reserved: List[LocationName] + """ + A list of locations the robot currently has reserved. (Places the robot is expected to move + through given the action it is currently executing.) + """ + + def get_dump_dict(self) -> Dict[str, Any]: + "Return a dict to dump for debugging." + return {"pos": self.pos, "action": repr(self.action), "reserved": self.reserved} + + +class PriorityQueue(Generic[T]): + "Priority queue implemented as a list that maintains the heap invariant." + + def __init__(self, seq: Optional[Iterable[T]] = None): + ":param seq: Initial contents of the queue (need not be ordered)." + if seq is None: + self._q: List[T] = [] + else: + self._q = list(seq) + heapq.heapify(self._q) + + def __iter__(self) -> Iterator[T]: + "Iterate through the queue in order non-destructively. (Not optimized, for debugging.)" + return iter(sorted(self._q)) + + def __bool__(self) -> bool: + "Return True if the queue is non-empty." + return bool(self._q) + + def push(self, item: T) -> None: + "Push a new item onto the queue." + heapq.heappush(self._q, item) + + def pop(self) -> T: + "Pop and return the head of the queue in priority order." + return heapq.heappop(self._q) + + +@dataclass +class SimState: + "Represents the overall state of the multi-robot sim." + + robot_states: Dict[RobotName, RobotState] + "Initial robot states." + + elapsed_time: Timestamp = 0.0 + "Elapsed time since start of simulation (seconds)." + + trace: ExecutionTrace = field(default_factory=list) + "Trace of timestamped actions executed so far in the sim." + + completed: Dict[Any, bool] = field(default_factory=dict) + "Tracks completion status for actions that subclass MarkCompleteAction." + + events: PriorityQueue[PendingEvent] = field(default_factory=PriorityQueue) + "Pending events queued by earlier actions." + + def get_dump_dict(self) -> Dict[str, Any]: + "Return a dict to dump for debugging." + return { + "elapsed_time": self.elapsed_time, + "robot_states": { + robot: state.get_dump_dict() + for robot, state in self.robot_states.items() + }, + "events": list(self.events), + "trace": [e.get_dump_dict() for e in self.trace], + "completed": self.completed, + } + + def warp(self) -> None: + """ + Warp the simulation forward in time to the next queued event and apply its event callback. + AKA "wait for something to happen". + """ + if not self.events: + return + event_time, event_func = self.events.pop() + self.elapsed_time = event_time + event_func(self) + + +@dataclass(repr=False) +class MarkCompleteAction(Action): + """ + Represents an action that explicitly marks itself complete when it finishes executing. + """ + + order: OrderName + "Propagates goal ordering constraint from problem instance. Required by PDDL model." + + def end(self, sim_state: SimState) -> None: + super().end(sim_state) + sim_state.completed[repr(self)] = True + + def is_complete(self, sim_state: SimState) -> bool: + """ + Return True if this action has been completed in `sim_state`. + """ + return sim_state.completed.get(repr(self), False) + + +def get_collision_check_locations( + from_pos: LocationName, to_pos: LocationName +) -> List[str]: + """ + Return neighbors of `to_pos` that are distinct from `from_pos` and are flying locations. A move + is invalid if one of these collision check locations is reserved by another robot. The PDDL + models of some actions also require collision check locations to be specified as arguments. + """ + to_ind = CONFIG.location_lookup[to_pos] + to_neighbors_ind = CONFIG.neighbors[to_ind] + to_neighbors = [CONFIG.locations[n] for n in to_neighbors_ind] + return [n for n in to_neighbors if is_location_flying(n) and n != from_pos] + + +def get_collision_reason( + from_pos: LocationName, to_pos: LocationName, robot: RobotName, sim_state: SimState +) -> str: + """ + Return the reason why a move from `from_pos` to `to_pos` with `robot` in `sim_state` fails + collision checking. Return an empty string if the move passes the collision check. + """ + check_locs = get_collision_check_locations(from_pos, to_pos) + other_robots = [r for r in CONFIG.robots if r != robot] + others_reserved = set( + itertools.chain( + *(sim_state.robot_states[robot].reserved for robot in other_robots) + ) + ) + reserved_check_locs = others_reserved.intersection(check_locs) + if reserved_check_locs: + return ( + f"Expected no collision check locations adjacent to `to_pos` {to_pos} to be reserved by" + f" other robots, but these are: {reserved_check_locs}" + ) + return "" # ok + + +@dataclass(repr=False) +class AbstractMoveAction(Action): + "Represents a single-step move action." + + from_pos: LocationName + "The robot's starting location." + + to: LocationName + "The location the robot should move to." + + def get_collision_check_locations(self) -> List[str]: + """ + Return collision check locations for this move action. + """ + return get_collision_check_locations(self.from_pos, self.to) + + def get_pddl_args(self) -> List[Any]: + return [self.robot, self.from_pos, self.to] + + def invalid_reason(self, sim_state: SimState) -> str: + robot_state = sim_state.robot_states[self.robot] + from_ind = CONFIG.location_lookup[robot_state.pos] + to_ind = CONFIG.location_lookup[self.to] + + # The robot can only move to an immediate neighbor of its current location + if to_ind not in CONFIG.neighbors[from_ind]: + return f"Expected `to` {self.to} to be a neighbor of `from_pos` {self.from_pos}" + + # The robot can't move to a location reserved by another robot. + for robot, robot_state in sim_state.robot_states.items(): + if robot != self.robot: + if self.to in robot_state.reserved: + return f"Found `to` {self.to} is reserved by other robot {robot}" + + # Collision check locations must not be reserved. + collision_reason = get_collision_reason( + self.from_pos, self.to, self.robot, sim_state + ) + if collision_reason: + return collision_reason + + return "" # valid + + def start(self, sim_state: SimState) -> None: + super().start(sim_state) + robot_state = sim_state.robot_states[self.robot] + robot_state.reserved = [robot_state.pos, self.to] + + def end(self, sim_state: SimState) -> None: + super().end(sim_state) + robot_state = sim_state.robot_states[self.robot] + robot_state.reserved = [self.to] + robot_state.pos = self.to + + +class MoveAction(AbstractMoveAction): + "Represents a move action from one flying location to another." + + def get_pddl_args(self) -> List[Any]: + # One check location argument is required for move + return super().get_pddl_args() + self.get_collision_check_locations()[:1] + + def invalid_reason(self, sim_state: SimState) -> str: + super_reason = super().invalid_reason(sim_state) + if super_reason: + return super_reason + if not is_location_flying(self.from_pos): + return f"Expected `from_pos` {self.from_pos} to be a flying location" + if not is_location_flying(self.to): + return f"Expected `to` {self.to} to be a flying location" + return "" # ok + + +class DockAction(AbstractMoveAction): + "Represents a dock action (from a flying location to a dock berth)." + + def invalid_reason(self, sim_state: SimState) -> str: + super_reason = super().invalid_reason(sim_state) + if super_reason: + return super_reason + if not is_location_flying(self.from_pos): + return f"Expected `from_pos` {self.from_pos} to be a flying location" + if not is_location_berth(self.to): + return f"Expected `to` {self.to} to be a berth" + return "" # ok + + +class UndockAction(AbstractMoveAction): + "Represents an undock action (from a dock berth to a flying location)." + + def get_pddl_args(self) -> List[Any]: + # Two check location arguments are required for undock + return super().get_pddl_args() + self.get_collision_check_locations()[:2] + + def invalid_reason(self, sim_state: SimState) -> str: + super_reason = super().invalid_reason(sim_state) + if super_reason: + return super_reason + if not is_location_berth(self.from_pos): + return f"Expected `from_pos` {self.from_pos} to be a berth" + if not is_location_flying(self.to): + return f"Expected `to` {self.to} to be a flying location" + return "" # ok + + +@dataclass(repr=False) +class PanoramaAction(MarkCompleteAction): + "Represents a panorama action." + + location: LocationName + "Where to acquire the panorama." + + def get_pddl_args(self) -> List[Any]: + return [self.robot, self.order, self.location] + + def invalid_reason(self, sim_state: SimState) -> str: + robot_state = sim_state.robot_states[self.robot] + if robot_state.pos != self.location: + return f"Expected robot pos {robot_state.pos} to match panorama pos {self.location}" + return "" # ok + + +@dataclass(repr=False) +class StereoAction(MarkCompleteAction): + "Represents a stereo survey action." + + base: LocationName + "The location where the stereo survey starts and ends." + + bound: LocationName + "The other end of the interval of locations that the robot visits during the survey." + + def get_pddl_args(self) -> List[Any]: + # Two collision check arguments are required for stereo action + check_locs = get_collision_check_locations(self.base, self.bound)[:2] + return [self.robot, self.order, self.base, self.bound] + check_locs + + def invalid_reason(self, sim_state: SimState) -> str: + robot_state = sim_state.robot_states[self.robot] + if robot_state.pos != self.base: + return f"Expected robot pos {robot_state.pos} to match stereo survey base {self.base}" + + # Collision check locations must not be reserved + collision_reason = get_collision_reason( + self.base, self.bound, self.robot, sim_state + ) + if collision_reason: + return collision_reason + + return "" # ok + + def start(self, sim_state: SimState) -> None: + super().start(sim_state) + robot_state = sim_state.robot_states[self.robot] + robot_state.reserved = [self.base, self.bound] + + def end(self, sim_state: SimState) -> None: + super().end(sim_state) + robot_state = sim_state.robot_states[self.robot] + robot_state.reserved = [self.base] + + +@dataclass +class Goal(ABC): + "Represents an abstract goal." + + robot: RobotName + "The robot primarily responsible for achieving the goal." + + def is_complete(self, exec_state: "ExecState") -> bool: + "Return True if the goal has been completed in `exec_state`." + raise NotImplementedError() # Note: Can't mark as @abstractmethod due to mypy limitation + + def get_next_action(self, exec_state: "ExecState") -> Action: + "Return the next action to apply to achieve the goal given `exec_state`." + raise NotImplementedError() # Note: Can't mark as @abstractmethod due to mypy limitation + + +@dataclass +class RobotExecState: + "Represents the execution state of a robot." + + goals: List[Goal] + "Goals of the robot." + + goal_index: int = 0 + """ + Number of goals that the robot has completed. If less than `len(goals)`, this can be + interpreted as the index of the current goal. (Or if all goals have been completed, there is no + current goal.) + """ + + goal_start_time: Timestamp = 0.0 + """ + When the most recently completed goal was completed. (Can be interpreted as when the current + goal became active.) Set to 0.0 if no goal has been completed yet. + """ + + status: RobotStatus = "INIT" + """ + Execution states of the robot. DONE = completed all goals, BLOCKED = the next action to perform + is not (yet) valid, ACTIVE = robot is actively working on a goal. + """ + + blocked_action: Optional[Action] = None + """ + If status is "BLOCKED", this is the next action that is currently invalid. Otherwise, None. + """ + + blocked_reason: str = "" + """ + If status is "BLOCKED", this is the reason why `blocked_action` is invalid. Otherwise, "". + """ + + def get_goal(self) -> Optional[Goal]: + """ + Return the currently active goal, or None if all goals have been achieved. + """ + if self.goal_index >= len(self.goals): + return None + return self.goals[self.goal_index] + + def is_active(self) -> bool: + "Return True if the robot is actively working on a goal." + return self.status == "ACTIVE" + + def is_done(self) -> bool: + "Return True if the robot has achieved all of its goals." + return self.status == "DONE" + + def get_dump_dict(self) -> Dict[str, Any]: + "Return a dict to dump for debugging." + result = { + "goals": [repr(g) for g in self.goals], + "goal_index": self.goal_index, + "goal_start_time": self.goal_start_time, + "status": self.status, + } + if self.status == "BLOCKED": + result["blocked_action"] = repr(self.blocked_action) + result["blocked_reason"] = repr(self.blocked_reason) + return result + + +@dataclass +class ExecState: + "Represents the execution state of the multi-robot system." + + sim_state: SimState + "The initial simulation state." + + robot_exec_states: Dict[RobotName, RobotExecState] + "The initial execution states of the robots in the multi-robot system." + + def is_any_robot_active(self) -> bool: + "Return True if any robot is actively working on a goal." + return any((rstate.is_active() for rstate in self.robot_exec_states.values())) + + def are_all_robots_done(self) -> bool: + "Return True if all robots have achieved all of their goals." + return all((rstate.is_done() for rstate in self.robot_exec_states.values())) + + def call_next_action_internal(self, robot: RobotName) -> RobotStatus: + "Helper for call_next_action() that returns the updated robot status." + robot_exec_state = self.robot_exec_states[robot] + while True: + robot_goal = robot_exec_state.get_goal() + if robot_goal is None: + return "DONE" + if robot_goal.is_complete(self): + robot_exec_state.goal_index += 1 + robot_exec_state.goal_start_time = self.sim_state.elapsed_time + continue + break + action = robot_goal.get_next_action(self) + + invalid_reason = action.invalid_reason(self.sim_state) + if invalid_reason: + robot_exec_state.blocked_action = action + robot_exec_state.blocked_reason = invalid_reason + return "BLOCKED" + robot_exec_state.blocked_action = None + robot_exec_state.blocked_reason = "" + + action.apply(self.sim_state) + return "ACTIVE" + + def call_next_action(self, robot: RobotName) -> None: + """ + Iterate through goals, starting with the current goal, until reaching a goal that is not + completed yet, and apply that goal's next action if it is valid in the current state. + Update the robot execution status. + """ + robot_exec_state = self.robot_exec_states[robot] + robot_exec_state.status = self.call_next_action_internal(robot) + + def run_step(self) -> None: + """ + Try to apply each robot's next action if it is idle. + """ + for robot in CONFIG.robots: + robot_state = self.sim_state.robot_states[robot] + if robot_state.action is None: + self.call_next_action(robot) + + def get_dump_dict(self) -> Dict[str, Any]: + "Return a dict to dump for debugging." + return { + "sim_state": self.sim_state.get_dump_dict(), + "robot_exec_states": { + robot: state.get_dump_dict() + for robot, state in self.robot_exec_states.items() + }, + } + + def __repr__(self) -> str: + return yaml.safe_dump(self.get_dump_dict(), sort_keys=False) + + def run(self) -> None: + """ + Achieve all goals in the multi-robot system. + """ + while True: + self.run_step() + self.sim_state.warp() + if self.are_all_robots_done(): + break + if not self.is_any_robot_active(): + print(self) + raise RuntimeError( + "Can't achieve all goals. Not done but no active robots!" + ) + + +@dataclass +class MoveGoal(Goal): + "Represents a move goal (may require multiple single-step move actions)." + + to: LocationName + "The location the robot should move to." + + def __repr__(self) -> str: + return f"(robot-at {self.robot} {self.to})" + + def is_complete(self, exec_state: ExecState) -> bool: + "Return True if the robot is already at the desired location in `exec_state`." + robot_state = exec_state.sim_state.robot_states[self.robot] + return robot_state.pos == self.to + + def get_next_action(self, exec_state: ExecState) -> Action: + "Return a single-step move action toward the desired location from `exec_state`." + robot_state = exec_state.sim_state.robot_states[self.robot] + to_ind = CONFIG.location_lookup[self.to] + pos_ind = CONFIG.location_lookup[robot_state.pos] + next_ind = get_move_location(goal_region=[to_ind], current_pos=pos_ind) + next_loc = CONFIG.locations[next_ind] + if is_location_berth(next_loc): + action_type: Type[AbstractMoveAction] = DockAction + elif is_location_berth(robot_state.pos): + action_type = UndockAction + else: + action_type = MoveAction + return action_type(robot=self.robot, from_pos=robot_state.pos, to=next_loc) + + +@dataclass +class MarkCompleteGoal(Goal): + "Represents a goal that is complete when a corresponding MarkCompleteAction finishes." + + order: OrderName + "Propagates goal ordering constraint from problem instance. Required by PDDL model." + + def get_completing_action(self) -> MarkCompleteAction: + "Return the MarkCompleteAction whose completion satisfies this MarkCompleteGoal." + raise NotImplementedError() # Note: Can't mark as @abstractmethod due to mypy limitation + + def __repr__(self) -> str: + return repr(self.get_completing_action()).replace("(", "(completed-", 1) + + def is_complete(self, exec_state: ExecState) -> bool: + return self.get_completing_action().is_complete(exec_state.sim_state) + + +@dataclass +class PanoramaGoal(MarkCompleteGoal): + "Represents a panorama goal." + + location: LocationName + "The location where the panorama should be acquired." + + def get_completing_action(self) -> MarkCompleteAction: + return PanoramaAction( + robot=self.robot, order=self.order, location=self.location + ) + + def get_next_action(self, exec_state: ExecState) -> Action: + """ + Return the next action needed to complete the panorama. The result will be either a move + toward the desired location, or the corresponding PanoramaAction if already there. + """ + move_goal = MoveGoal(self.robot, to=self.location) + if not move_goal.is_complete(exec_state): + return move_goal.get_next_action(exec_state) + return self.get_completing_action() + + +@dataclass +class StereoGoal(MarkCompleteGoal): + "Represents a stereo survey goal." + + base: LocationName + "The location where the stereo survey starts and ends." + + bound: LocationName + "The other end of the interval of locations that the robot visits during the survey." + + def get_completing_action(self) -> MarkCompleteAction: + return StereoAction( + robot=self.robot, order=self.order, base=self.base, bound=self.bound + ) + + def get_next_action(self, exec_state: ExecState) -> Action: + """ + Return the next action needed to complete the survey. The result will be either a move + toward the desired location, or the corresponding StereoAction if already there. + """ + move_goal = MoveGoal(self.robot, to=self.base) + if not move_goal.is_complete(exec_state): + return move_goal.get_next_action(exec_state) + return self.get_completing_action() + + +def format_trace(trace: ExecutionTrace) -> str: + "Return `trace` formatted in the standard PDDL plan output format used by POPF." + out = io.StringIO() + for event in trace: + print(f"{event.timestamp:.3f}: {event.action} [{event.duration:.3f}]", file=out) + return out.getvalue() + + +def parse_pddl(input_path: pathlib.Path) -> PddlExpression: + """ + Return the result of parsing the file at `input_path` as a LISP S-expression (encompasses + PDDL domains and problem instances). + """ + comment = pp.Regex(r";.*").setName("LISP style comment") + parser = pp.nestedExpr().ignore(comment) + return parser.parseString(input_path.read_text())[0] + + +def get_action_durations(domain: PddlExpression) -> Dict[PddlActionName, Duration]: + """ + Return action durations parsed from `domain`. + """ + actions = [e for e in domain[2:] if e[0] == ":durative-action"] + action_durations = {} + for action in actions: + action_name = action[1] + duration_index = action.asList().index(":duration") + duration_arg = action[duration_index + 1] + op, variable, duration_str = duration_arg + assert ( + op == "=" and variable == "?duration" + ), f"Expected :duration arg to have the form (= ?duration ...), got {duration_arg}" + try: + duration = float(duration_str) + except ValueError as exc: + raise RuntimeError( + f"Expected duration value to be a float, got {repr(duration_str)}" + ) from exc + action_durations[action_name] = duration + return action_durations + + +def get_location_config(problem: PddlExpression) -> Dict[str, Any]: + """ + Return location configuration (available locations and neighbor lists) parsed from `problem`. + """ + (init_predicates,) = [e for e in problem[2:] if e[0] == ":init"] + move_connected_predicates = [p for p in init_predicates if p[0] == "move-connected"] + dock_connected_predicates = [p for p in init_predicates if p[0] == "dock-connected"] + neighbor_edges = [(loc1, loc2) for pred, loc1, loc2 in move_connected_predicates] + + # add dock-connected edges in both directions + neighbor_edges += [(loc1, loc2) for pred, loc1, loc2 in dock_connected_predicates] + neighbor_edges += [(loc2, loc1) for pred, loc1, loc2 in dock_connected_predicates] + + locations = sorted(set(itertools.chain(*neighbor_edges))) + location_lookup = {name: ind for ind, name in enumerate(locations)} + neighbor_edge_ind = [ + (location_lookup[loc1], location_lookup[loc2]) for loc1, loc2 in neighbor_edges + ] + neighbors = [ + [loc2 for loc1, loc2 in neighbor_edge_ind if loc1 == loc_ind] + for loc_ind in range(len(locations)) + ] + return { + "locations": locations, + "location_lookup": location_lookup, + "neighbors": neighbors, + } + + +def get_goal_from_pddl(goal_expr: PddlExpression) -> Optional[Goal]: + """ + Return the custom planner goal type corresponding to PDDL `goal_expr`. + """ + goal_type = goal_expr[0] + if goal_type == "completed-panorama": + return PanoramaGoal( + robot=goal_expr[1], order=goal_expr[2], location=goal_expr[3] + ) + if goal_type == "completed-stereo": + return StereoGoal( + robot=goal_expr[1], + order=goal_expr[2], + base=goal_expr[3], + bound=goal_expr[4], + ) + if goal_type == "robot-at": + return MoveGoal(robot=goal_expr[1], to=goal_expr[2]) + + print( + f"WARNING: Can't map PDDL goal_type {goal_type} to custom planner goal type, ignoring", + file=sys.stderr, + ) + return None + + +def filter_none(seq: Iterable[Optional[T]]) -> List[T]: + "Return `seq` with None elements filtered out." + return [elt for elt in seq if elt is not None] + + +def get_objects_by_type( + problem: PddlExpression, +) -> Dict[PddlTypeName, List[PddlObjectName]]: + "Return robot names parsed from PDDL problem instance." + (objects_clause,) = [e for e in problem[2:] if e[0] == ":objects"] + # object_decls example: ["foo", "bar", "-", "int", "baz", "-", "float"] + object_decls = list(objects_clause[1:]) + + # desired objects_of_type in example: {"int": ["foo", "bar"], "float": ["baz"]} + objects_of_type = {} + while object_decls: + type_marker_ind = object_decls.index("-") + type_arg = object_decls[type_marker_ind + 1] + objects_of_type[type_arg] = object_decls[:type_marker_ind] + object_decls = object_decls[type_marker_ind + 2 :] + + return objects_of_type + + +def get_robot_goals(problem: PddlExpression) -> Dict[RobotName, List[Goal]]: + """ + Return a mapping of robot name to robot goals parsed from `problem`. + """ + (goal_clause,) = [e for e in problem[2:] if e[0] == ":goal"] + compound_expr = goal_clause[1] + if compound_expr[0] == "and": + goal_exprs = compound_expr[1:] + else: + goal_exprs = [compound_expr] + goals = filter_none([get_goal_from_pddl(goal_expr) for goal_expr in goal_exprs]) + robot_goals = { + robot: [g for g in goals if g.robot == robot] for robot in CONFIG.robots + } + return robot_goals + + +def get_robot_states(problem: PddlExpression) -> Dict[RobotName, RobotState]: + """ + Return a mapping of robot name to robot state parsed from `problem`. + """ + robot_states = { + robot: RobotState(pos="", action=None, reserved=[]) for robot in CONFIG.robots + } + (init_clause,) = [e for e in problem[2:] if e[0] == ":init"] + init_predicates = init_clause[1:] + robot_at_predicates = [p for p in init_predicates if p[0] == "robot-at"] + for _, robot, pos in robot_at_predicates: + robot_states[robot].pos = pos + robot_states[robot].reserved = [pos] + return robot_states + + +def survey_planner(domain_path: pathlib.Path, problem_path: pathlib.Path): + "Primary driver function for custom planning." + + domain_expr = parse_pddl(domain_path) + CONFIG.action_durations = get_action_durations(domain_expr) + + problem_expr = parse_pddl(problem_path) + CONFIG.robots = get_objects_by_type(problem_expr)["robot"] + + location_config = get_location_config(problem_expr) + CONFIG.locations = location_config["locations"] + CONFIG.location_lookup = location_config["location_lookup"] + CONFIG.neighbors = location_config["neighbors"] + + robot_goals = get_robot_goals(problem_expr) + robot_states = get_robot_states(problem_expr) + + sim_state = SimState(robot_states=robot_states) + robot_exec_states = { + robot: RobotExecState(robot_goals[robot]) for robot in CONFIG.robots + } + exec_state = ExecState(sim_state=sim_state, robot_exec_states=robot_exec_states) + + exec_state.run() + print(format_trace(exec_state.sim_state.trace)) + + +class CustomFormatter( + argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter +): + "Custom formatter for argparse that combines mixins." + + +def main(): + "Parse command-line arguments and invoke survey_planner()." + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + parser.add_argument( + "domain", + help="Path for input PDDL domain", + type=pathlib.Path, + default=PDDL_DIR / "domain_survey.pddl", + nargs="?", + ) + parser.add_argument( + "problem", + help="Path for input PDDL problem", + type=pathlib.Path, + default=PDDL_DIR / "problem_jem_survey.pddl", + nargs="?", + ) + args = parser.parse_args() + + survey_planner(domain_path=args.domain, problem_path=args.problem) + + +if __name__ == "__main__": + main()