From 2eb931c689919c6a5ab8912435ce5e74f0b77fa3 Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Sun, 10 Mar 2024 18:27:42 -0400 Subject: [PATCH] dds lowlevel interface is added --- Go2Py/.idlpy_manifest | 19 + Go2Py/idl/libcycloneddsidlcxx.so.0.10.2 | Bin 118224 -> 0 bytes Go2Py/make_msgs.sh | 25 -- Go2Py/robot/interface/dds.py | 173 ++++++++ Go2Py/robot/interface/ros2.py | 2 +- Go2Py/unitree_go/.idlpy_manifest | 19 + Go2Py/unitree_go/__init__.py | 9 + Go2Py/unitree_go/msg/.idlpy_manifest | 19 + Go2Py/unitree_go/msg/__init__.py | 9 + Go2Py/unitree_go/msg/dds_/.idlpy_manifest | 19 + Go2Py/unitree_go/msg/dds_/_BmsState_.py | 35 ++ Go2Py/unitree_go/msg/dds_/_Go2pyLowCmd.py | 30 ++ Go2Py/unitree_go/msg/dds_/_IMUState_.py | 31 ++ Go2Py/unitree_go/msg/dds_/_LowState_.py | 48 +++ Go2Py/unitree_go/msg/dds_/_MotorState_.py | 37 ++ Go2Py/unitree_go/msg/dds_/__init__.py | 13 + dds_test.ipynb | 384 ++++++++++++++++++ {Go2Py => deploy/dds_bridge}/idl/Imu.idl | 0 {Go2Py => deploy/dds_bridge}/idl/LowCmd.idl | 0 {Go2Py => deploy/dds_bridge}/idl/LowState.idl | 0 draft.ipynb | 0 msg_sources/idls/BmsState_.idl | 51 +++ msg_sources/idls/Go2pyLowCmd.idl | 13 + msg_sources/idls/IMUState_.idl | 36 ++ msg_sources/idls/LowState_.idl | 74 ++++ msg_sources/idls/MotorState_.idl | 40 ++ msg_sources/make_msgs.sh | 29 ++ setup.py | 19 +- 28 files changed, 1106 insertions(+), 28 deletions(-) create mode 100644 Go2Py/.idlpy_manifest delete mode 100755 Go2Py/idl/libcycloneddsidlcxx.so.0.10.2 delete mode 100755 Go2Py/make_msgs.sh create mode 100644 Go2Py/robot/interface/dds.py create mode 100644 Go2Py/unitree_go/.idlpy_manifest create mode 100644 Go2Py/unitree_go/__init__.py create mode 100644 Go2Py/unitree_go/msg/.idlpy_manifest create mode 100644 Go2Py/unitree_go/msg/__init__.py create mode 100644 Go2Py/unitree_go/msg/dds_/.idlpy_manifest create mode 100644 Go2Py/unitree_go/msg/dds_/_BmsState_.py create mode 100644 Go2Py/unitree_go/msg/dds_/_Go2pyLowCmd.py create mode 100644 Go2Py/unitree_go/msg/dds_/_IMUState_.py create mode 100644 Go2Py/unitree_go/msg/dds_/_LowState_.py create mode 100644 Go2Py/unitree_go/msg/dds_/_MotorState_.py create mode 100644 Go2Py/unitree_go/msg/dds_/__init__.py create mode 100644 dds_test.ipynb rename {Go2Py => deploy/dds_bridge}/idl/Imu.idl (100%) rename {Go2Py => deploy/dds_bridge}/idl/LowCmd.idl (100%) rename {Go2Py => deploy/dds_bridge}/idl/LowState.idl (100%) delete mode 100644 draft.ipynb create mode 100644 msg_sources/idls/BmsState_.idl create mode 100644 msg_sources/idls/Go2pyLowCmd.idl create mode 100644 msg_sources/idls/IMUState_.idl create mode 100644 msg_sources/idls/LowState_.idl create mode 100644 msg_sources/idls/MotorState_.idl create mode 100755 msg_sources/make_msgs.sh diff --git a/Go2Py/.idlpy_manifest b/Go2Py/.idlpy_manifest new file mode 100644 index 0000000..d4fc879 --- /dev/null +++ b/Go2Py/.idlpy_manifest @@ -0,0 +1,19 @@ +Go2pyLowCmd +msgs + + +BmsState_ +unitree_go + + +IMUState_ +unitree_go + + +LowState_ +unitree_go + + +MotorState_ +unitree_go + diff --git a/Go2Py/idl/libcycloneddsidlcxx.so.0.10.2 b/Go2Py/idl/libcycloneddsidlcxx.so.0.10.2 deleted file mode 100755 index 267b6aae029aebe469ae925ef6ba0fab5fd105db..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 118224 zcmeFa3wWJV_CJ1lIU1!-81XIRa?sL_(xRbMh|AG~*2JaMwMpAlqPIxSA!Wj;B-5NZ z4q?<}Fe7GMVuleziBe8mnz#&M%4A$-f|_~bbcni4%%K0zT5Iq3?04s!4&Td<|MQ5a z-gkfAwfA0Y?X}ll`|`f$s?hWkhYlIy(|^N!NBbz%d$J(K>Osl+3{A0bif=n#v2Rb` z9!xu2(PXkth*B4a$Sl8~;B%i*T`L@BM zNp+HMQqp^kDZZN2Rp%RuJe03ko5M zrC!!0l|eQ$?bMk*kfn$h#U4N3S*7Xu3-|nc+5^XRJh%6C%f3Htdednw*St3pk9}kD zZ#w=_UeftE-_W7H$s>0^d3g5&$#K4Mr9&HT-1XMj4XGhROQ&43jA>_pj{Z)?zaQdX z2_^7HrgjG3`Fv;MIurk9;omt5)0cDA&3U+YZ{Pjr0SBg%_s#wH#qTt&DPGj`TxjZp?HAVt|Bx!W@21hi|J)h;@bGv0 zkDGc!&4qI+Z#ej|_bz$qfYJls>pJVmNA?(QZ4SN7&4FK+qx>I3&}{AXdJaBQIm%g(1Aie0 zepZh1pU;6;=D_#Nfj^l8FUq0MQ*zLUbF}Z_Iq(V4XV5ktq;JH(Ynyf_@=9LeaP5x|44Q-^=d`psMlhpyVpC_?)cpDhuybas@4Xh@uZ$_$P{fl7%}8 zA7|ks75{(vT==|7@!zQAz4J{0egZoBI~VnmT%qkmVY5dFezYoQ#J?o1SyuV&O5WRV5W*Xj-VRXo6&C$xs$KFee3QcSEc}Q=e%jPUBmuK0Z%${3YHhX^M;qxsjyooU^LBa&e>#1z!|h1P*E-6!Rl+4fwN=NOhGLkqkAF>dGq3 zOZnpHB7HxntfqXnTODK|%8W#FDkF=^Y8Du*U0Sv+-39-1mx-25{qq3UW zi_2jrQWVi$MwT*rUKv`Ty1c5oyar9cjW3*4mtPXC1UkER?!4+lD1S})!ufNTfT(h@ zDhA+!@}&#QYid|rNKn4GrgBc@!pg`}gMsR@N-b{9Vx_<8a^aPDcR{FbZWZM;Er&%z zLueh;fU)w$bH#+fL3FV;SmH5K9i1ETEsEAe@BqC7t+ZIFZyp4!oL>n;rAp_OUqou6 zTjYaW$eOVg=;PALB~eiYp3H?M&0SQL5eis2%(}Y9hr7k}h^TNWo3o@^8WP3ME1$ct z46UFVw5&!+4?!0#xwM?}EG(DaMogd*%}}I#QI%@dj5=I<7Jd!0tdx>qrqVyJ760NFI_mwqe3(fAfgjDVtD>`@iMdeDVkhFSk zMRi7jiV*4%8i}a+0TT28tP4mt1i&N&pc_&j zXQ2c=h2#Rz4FS5VC{@&y!y_8HE1OfZWMMQSO+&3c4{Vd&%tw!2?3+J#AwHPuk{WEfxNDg9sn%h2gNS!1%x8Jt7YXDEmG z{BHPK>>@Za{xGUVjso?S}i_Q`pXxm7{1C?Gu%b?pC=i@wf$P`qS&AI$mu) zMer$V9aQ5b3Qyf21Fyz)y34{hssXy&!aq`YkA?3@3r6_sweVtvI~HE0@O}#~K2XXT zu<)*lDq#4jm;ahcDsAC0h38v%ufj)Lc*((1PSC>J6h6+v2NXWZ!mAFEa;8{#`=Npt zTll2Of`=`o{c(;Z3D!j+SeWyq{y%t`gaL2;i72a>*pDKL7!beY+@_j4hs_nl+;dvI`ukfIS zubCnF$65Fb3ZG=*uPc0tg>O`Nv4!_5JZ#~gDZIqOhn*_?%(C!+!b>fDH-%SN_!xy( zS@=YS*IM`y3XfU%2?}qr@KY7uY~klCyv4#VR(Pw0M-|>?;W34`TX<689TwiA@RWt$ zt?(`je?;Nk7XGxtdo28q3h%Y>UWGdrzDeQz7XGoq2Q1w8LuJo5dF^>8h38rL?h4Ph z@ck4%+QKI*JZRy?3Lj_T(-l6+!p~Ot6bqlD@L~&JpzyGTU#9R93ty)2Sr#5wc&UZ2 zP3fvhXnqFSYQ46kcKBhbz3w!cS0mt%a8;JZ9k+D!j?UD;3^s;g>1A#lo*vc&mk9 zr|>ol|GC23E&OhUcUbsC3Qt-1lM3&$@GgaSTlfZr_gMH_3h%Y>_Z99~_`en2Z{c~T zi+vAR_$~_fS?kGT6mG9GFFRApvDc+nDBRv>X;rwrPmxmiIICV=zP(SO%b#M=cd2sh zb^Z+s59gpSv2aaquiL+=$|<$z(+aP!@J|(9W#PkSir#81e0zn*EPNM*H(B@(6y9v% z$0@wU!lx;`-NH{(c!!0bq3~`C*X8$GxGtyP!ZrPXh0j#w=l|U6Z*@7NEnM?6$-;Fx z#TKsVOD)`Yv(#5%;l&ECvhbL~Yb`vb@R)`BZjthvEWB9Z%@!V0c#DOn6y9p#zFVdI zHVZFSc)Nwi6y9OsDTSvj-1iG9zstgl72a*(F@^V9_$9NYoY6{8dVcgv)$TzHZ&&y@ z3-4C=EDOK#EGfU#!mm?!g@v~$yv4#_QFyC`_ba^3!bhJi_4Zi!W`*}!xaP;P@cday zKdPOzer75>XyN4wA7|k)h0n6^=M-LQ;hLWc3-49*Ef)U$bA-=U3qMrhZ5Cdl@E!~Q zRN=iAuK95+JpWv!A7!UnKW8XBXyN4wA7|k)h0n6^4uzLmxaOzA!n+lHi-nImPwB_P zg9>l6@UX&rEc^?F_gc8-$FcDI^Ob&7|I_+8S>ZtoKS$x?EWAqLvn+h2!b>guIfYkP zc#pzcEPRg(lzuE+%hhJ#lN9}=>m(l*!^X=;{9^UTo0obE=dl zY~Xb6RR49|oGHh|OAPveCdPG^f$K9L^f>R z;EOegYo5WsiPswRZbhcOjH5q)Gc#A=Q zpeDw()xgg&@Ge8oCf;VyFEr@$4E{~L-JqXi(1(rkO}xXPFEQww40;n!8T4lx^j(Jj zJ-9(nXG`^8*wCAacN_Foni$s}0}mN^uYu1saL2&)xompeWbkR?{RaIsqnrT)pKsu! zo26gSu^_pVhd~3+?DXOMxq;&dj{7&sz;P7D{hMOoYKxAa6&pDD6#W-AaCKCPAC(xm z_O0|_mVsj<(ETel@R2TxGwKGuoq<;w`1S@~Yv7{{JZ9iK7pe_*+-%_b3>@Jt z22N+}^@Bss-_ZQs1>oa!yeHVnkHSpaHJm0`e4SckL?_uCU1K-oY z#~Juu20qEag9bju!1p%rVgsjlG4x;9!1vK0t|bP(uYu1p@G%BnYT#oHyu!fuGw><{ z-`~J%4Sbw|#|(VDfj1fW0t0V0@E;g>i-8|t;H?H;X5eiGKEc4-4g4Sj?=bL*2A(qT zNe13!;1vemZQutRc#nY}V&J_7eyD*v27Zx&_Z#?R10OK(LId~Rkg@;63_Q=k4>$09 z1D|5xqYYf2-K57s12>PJj5F}5MmduV{3rvTV&MASBYIqH;7bhpuz?pDc!_}@ZQ!#E z{1^i-HSl5suQ2dq4ZO<0k2COE13%utV+MYLfj1fWi3Z+m;HMjSi-Cs?yw$)@GVnG7 zKiRM&(w{(Z= zpUPXuf^8{$8}Q~WW8c9gu$PbMIgZ=h_tuszTbemdq0{ERm)tZ>a5wj@anm%h-Q4$p zo2Civ=DyqAG)-hT_g(L%X#%^sZ<(8>iRv z-84-gA-|iZi6i89(==g({BD{iijdz;(?k&RyJ?yLZtg2{(=_pe{BD{ie30Kw(}WK4 zyJ?!pL4G$)6E(>1rfGr(`Q0>4%pm_4TK*uX-*VG?bNVGWO%pB1@1|*j1^L}HO{^fl zo2CgBbeWr`2@~XZ(=;K1{BD{iLXh80(?kdIyXgZty`!6^ z2@m9V(=^e6{GV(2X@Z0HchfY1LHoODnz%rIH%${3$nU0ULIU~SG)+Vxzni8B2;_Iu zH1UA^Zki?>Xn!|N6Aj4krfGr!`Q0>4EFiy|rU?b)chfX6K>NFCnh-#KH%$`($p3FG ze=(=ua?=#}Lw+|+0YBt-(-i1Kem6}4KIC`PAx>ZKrYX>e{BC+0r!R5S6xc(4H%$RO zbQXs#ZrU?n; zchfXcgZw%TZ#wg2@}|%O`OmMAUS!diTj>j|v|auT>v_mZ7g^~-D}A7q9&4rdu+jxq zdX$y+Tj?)w%+=J#Csz6&R{C8l{ic8O=nWTnfk^aWOWrj?#yr9)P_$VwMl=>x6wSS!7Ul`gQ-qpY;wN`Hal(w_FW z(*Lm1?^@|Mt@JBa`VUt6X)C?jO0Tri_gU#Xt@JOf^a?ATw9<`My3R^Rt@I)*U2df> zu+lTF^b9KA;ft4O*rTtd=i)mK-Tj_sT>36O4n^yW2oen39 zo;e+HZPLG%E=LSR3J~g^g|`AI)ZKPVZ(y$&yt8kex`O`aOka2dUB-VIj`xSxynjr1 z&A_nmkoDo0zKraQ0)C+i$lKC8pY2xXCx0F*nu)x==m9gs^+i9S7ZAencO%<}6Gf+j zEoWDZ1I|1Im+Sopf^NtK$m`Zm`TH)zL%NxXd}oB0ZE6C8;dosOSc%VU3dd)}rpFs= zLDUqwab7qvd@Kb-;dsb7I2BIt10=e`ouMugEu09&!tqXL4aT!@BGye1WS^k%pMM5C zz81-raB@`ok3QcM6o4a32W5921;3iM2r)rJDl$^ip^kLEesU-#6y_~#4%^V9Hx zR6Ys2>}_8^)$mSu{2#;d*TP3^i0lMS(|dB^`fJ7vP{^O|1Ld-}x8OcJenaNI@#RSV zSVrreyzr3TaD5(_!X*_5uW^Ql*Ypk#$A>r+udH$QO8Yxt0Y1`N2aqj&%P<{+Ryv-b zX%7h#X8xhN&QLoben@yXbbxI6Fy4JgP4Q5B-$Kl-C^J2;LO!T;vQGE@lhb2#x|AC$ z3VRj$ZcYap#z`4yni%a-!WaS#`_YY}Yq821ZI#oc%0Zh^9+cCZ$Jx&GO98xp2g3@^a#fIbSoJU#QLCy4-(M%uxKWwIFna8E7aW)!y$R8km1{wyyeaA=C zbo3ECt)-!8%=-QzMDQn8glnjzfsJ^7j5KgB%5V+!{!c{%j{pqE2WjA&GSn`xlg&_j zNUWs)H{H0zLn#OzBaz07dAL2F{0GeR2#~JOq!~-*ESCxixefH@;)*oZ8~AP(3hu&v zJ9(sS=q5b^6Hf`hqWs6fr{RFR(rz>Sp1T)ov&(fY z|M-|JtE!`q?$tagL7`0QLt+i2rc_RdO@`8{{S~=7<6o*Is6l%iMmzY z%@zUB{_%I)WV=AV5u%if5z>am>u zM6^Ub_VVYrUCHb6qu1mS^;oD$`DVJC1$B0(rvPUgx%vaQNKS7?`~=01LXjiHRJzjJ zA#t@zK;=+3m8oo{4rV_C?M6O&z%`fOqCIVMv7PhX6smnqBUfBc;3zNTTKb+6Q<1wH z{;aa}o3y1T{pxlAjDu5c*3Oqx=@kZ zlb-tr)w<64BiH_|=}8;GPPU%3Bc?jxM3O{>HbhL->?>Gv$--MFm1+hS<0XsruFQe_v#~1Sk9GlWeHs?Po~L#?g{3i%5wiKa0QM zD&qqWtMq9xmzkL2FVHZL3@z#Z)z>e+*;D*QRD9BZ5694mOI(xwx|-NEol6tLMFLZH zZkGY{IOk<35M5ri%b#fym-H{E@G|~C;U@83E^g?Pw~%rWH%o|{6Fl6|*A&6c&;G~U ze9@PS8;2&1N&n}axO_b;dB($PtCVx7%AGZ1<541^yD5j{*FLR7 zzqO!DtPrQDe7~A&trQ=yMk3VrSu46vAUheXx8!JX24W32rlJR0W}PfNnt4-kZ;O zqS%(E&_f6u5Y$re5NgHM(Q9x?{~D+V2;w1n(&^mrcXk~VUxPZ<%F075oh-bWDU~u3 zF!PMtQGp+VkysODK!nzJ1Lwq~0)-Z33hkE0jfb93gr1)o>WWg#Q-Y#-hCN-lF$0}M z-SY_0DAoaw(Sc))AZ;6ALbb>_D6l>?gl6tZ{|-_RiNzc&c$dY(5rVpIU!ZX=4<{K3 zS@C~;3W~oPteT2Pr}dd3RQ;wY&@d)2(Uct{Wu^ov5<2`hu99@l=@yi%rUXcbdU#5} zPp;*TOKmU%@h{{|(p;>t^C0QuTTqbO$%nEa>zt9S(r-;5^L?=6>Ex&Qx!EgfU29ArM4yI~<@vR(+8N@Cl#Ub3|bC0Q`(d7v}2QA(sqqP9;b zjfG~4o-vb*ze>%uE258hXk&fdMqhMS;52n~CcndN=)i0oU-S>@AsT4 z9PV#D=x_(nArdTYx~%`43?jDF9XE27;FTu1 zN@+6(ptN(y|7oR-WTVit@a=hJSuG-Y`1@))!gSWrN(wv@Yr*~ec<68Q>+Aj&yfklq zXXtOmzVsMM=1Vf@OBYZwPm=k*^fp9=6=zXfeK!* zn5?raFXE(^A}3m@xR%zn$W_c+qg=%sj}!GwC-BG(6l$Ficw}whkr{uJR=q*$KOOZK z#<>1EDD%ly5Mm`xMu{#cI@8n7;fm1Y$G&AYdCVhEcv9eznW&<0vMrM`<%p+DS<}lj z2dy$xWcAOsD-n0yjmlecHz%{ApYXTw+(;saxaOq z&Up=BmZ0Fte5ge2Lj3dp;qrDG?0EczvVmPGC=ls5fEMZCJqmXnFVcMo9k|}`JHj@qhha?6HiprBmMUXj*1~g`%~{!@C~wBsh>{VFI&fw}1bD8&UV~ z?5o53QQLZ)B3alh8lTuBb-*LF!y~mdg`OI0VTccS*l)*`EUFz0wZR&wr_nAPaO{u$ zg%nJ?a2Qobm80i)ci|4D29Qu8MtR-9HafC{hR#0Nq%ZJr=qcWlz+MyCTVH{+GHO+h zLPfb)5tE^B!v50O!y%#D*dv&ibxwdy`s+3J|KWyO5bStNSvB^dXlUxe=&91s$->p- zv$O{fvut(?*U_+dlpgj@Ad&yqTG-V$f?eA2T#o@|)v0JE7~jZ8=u$Kh96G0Rtz;B- zyWBWd+GQaq+;&;=2HD6u=Q3`eZ%w;2fgMk~C>PVnb_(fW-Z#Du){8Jr4buL(U@^vE zp}{Easu;~T<}suB;;e$Dg))3+Y3yxG>hpwJnMU^Rd^YLP)7a?ETOGeCeo3MG_M?Yw zkRCQdB7qrr(htL9CkjCyBcXVid{m737LTf#;KCS_A9$0R)@zpk(;-YR;gK>iTuF#a zPJQFPVo$q6bl0BtdQI$Uf3~YZ_Vi5+SyRA{$DUlLp7c+6lq`dK%uI;o@$F$lw=-Ex zuWqr^;_33Ik@Oxb_-}91rNBxIY!OSCvEY0HYnIsKfLt5LFaONOL5+hfkRZvY8m8?d zjdK-pxs7vmk2KCtxTrym^G$6@+yZtyjiWkya5x%98bqQc?a@*YvI7=4XKxYmX6V2b z@)v&+A@5|xd~1Y!80=&daw;#$ag)Z_xtT(?L&!Jx5+PTj@mwK0UKJrz+@jwaAzuPJ z*@Qfc#woE|YDssG>2?;Q(|P69(9fHC3qrL9X zd>`vcFCe|a(Mp}l(IhIyDS$*_5%rKvIpzk^#hxO5PDLdBp|4o~fu79g=d6D<8E^Lb z--8LmjT>RLtByadRO3`1_(GaGbcY#UweXTluf6~4Dfvk@IMT+b!@b;?j_-U= zo&@qRHzRl^mcA0Ix%t1v%Wx@IdE&$1yoizC1wCpp8Tliu#U9<@Z9RuBUfh6=;qkyY zg4IlpB)+-@$7{mzmCbxv(ZrWz3>P~3#+PIjE@renfa7oVoYBWo*Eqxlt3H*e>&E-Y zE{ZxILe$km)z+`3qJ?k^=4Gq?DTE7z@J=GUUkIsIA$*kx2ZZoAA*?6D9}6KdB!u@7 z;b%g)s}PnE;RQsPT+t6gZjnc?qE?On>7(#(F+++E4cD(7jy38wDu|wRHn!{A`qiWa zU!dWyY^9B?61~jAWPSCNCBeDF$&w-1b)@Q93EOgIR5XovHJp?AfNq)VK;sgu{`E~1 zew!8H$$#Y4x!q6)L#_yv>Kp1MUd_aThO|hg%lPzVs2pC&Im1)gW@-?HOInPF9wn5$2-6Z? z9!l(SjdIcLFNJDQi7Udh;(m&wGsLNH71pksgZYV-Y=RdZd?k8qab3yTNZf zM|%0)nddRAZ8W?Kf2#6RdinCq^J0DuBT>&Oz5Jrg^B_I1$mFN=@@bjpSS$%N?C4D| z-#?Siqvs!H_@ngl5$^K3arXf*a>R zBCj796wSR^5liHwtO+~!RFCw$mWY-ij=L29DUGh>=w=V2MN zHx1l~Wk2@YP5FH5-#Y~3eigDe4ek4p zn~5wzUq!X^0$tBh#p@XKHgZZDVBGVXK+a zmr6Awj2zT;bE__W0w4TNABRP}Ker>Mqu&L@B3|g%bSdim4Pv_Zvlu41BTsFlMmO6l zJ-iuPSv*+Mo~pUg_VP1ZwwTL3%Qm$@d5C5}cZ*aR?3#wKJAT@XO=MY@h&K~jgj$qj>cw^vwYBhg z(y$6qGx!x^0xOByoQJ>7deHOW;Z!o>Qs@fGquZ3eb?r@7#rQ#b&u~2x!)vJ{kDX5N zX*M~^h-jAcM|*I`h4P3&cX-`q7{!&Rqket3U!8iaU!4ageb^yEx7^ZKg;5)fbZw21 zzA@;8>l@pN2DIIZw(sX`0>WCNgE?1>CYqY$8PQ_sewyV?$hpfQ-QfhfWwXj4B$K@) z{Y=tMrA$w9&Co#;3d;;DZ}w9lN8P-BHFx$vV-SN3bVenT&bZ{MEL+V^frq&mc{`;s z1ldg_auDcA32Kfgw-(hfWOz5Gn+rEvFA((BHIZMePeY-VenGn)c@qiK`}KgkU5;bj$MTIBq7`q#MjLx(@fkowN|qarU8$-)coFlryEYL`AVf#{R|8BC!(MzW11 zS<#wBAa)b3Bu_lWi$bbD(r^31aDreDc2J69f6062jTOXj(qGL7G#aZ|U22fhhg>VD zF87dY6Qv;G(U8#7Ph2@8R9DhJ%H%sM!%6mn#)`*C56=^%oJO-8s|g2K4TH*A3nR^} z6K<5S-C^1ok2BzZaSC$aMHgQ%&{#s<$n}+);&VtCR_8CVIE3udh4hF#npLNJ%lq3( zZ`p6H(Obq-B^Z$p2KX%+k&gpA-rgb>w~8EH(tj33TzZUJk*)6x(s&6=tw*G4D#vT- zaXNH890fa{Kqr~TnMR}(v=!&$f2cJm23aZwSuCmb?{ZVHvFdhWFOI&Q-8^r5BA0o- zVm9{HZT#Gvh0v!xp{Zkk#XS3@+j!ke4%^%hR+SN-UtRF@%U9mch??}j*2}4{(J${> zolEdHNWn?}7q@?Ho~u(P&o@#zx)t31v?uD7UFYaYsAYtynXm4(@}wrU_Dx4gO{KCC zjk*;^)Q*<#J&eMB--#`8c*S|FOl>w!K*+(J8=6wtXbkVh1*)D zZJZ@JN6Hpv7qr0qCW@!v91E|vccCY(I)@!(;h39eLie#b%q8=hv{-` z8ug;80W4w#9>!TE1bCxInThJ-pm;6 zqaF6K9J?5l_kQFh`*;zIIKS@@`&h+x@~sI{pJN_tCqi#9?=Yf$+Sx1IL?%uzjH<{~ z$D*GzxgJy3^O!1%8VdR1p*xU?^>zjf19}@?Uw+4z^>ue_iA+`{-|Q_pG2;$5JiN_Q zc%@OeA5Y}ijVfl;dER17`NPDb9sxo9Js4j#Fr zQiJmsl#F5GQc=UG)zm|5BGJUnTwd*TP!b1V)h_D>DGR$*1$9z?>D1`qTs)U-E>%y^QK+%WV$ ztUIifU^|uwYe7gFy&W2Lt{`49tke@j-=1$q+z58IHmq#&aRWoVSSuNZ;ocu*@wC_0 zC-l6-%1Uk_Q~mUD7E)Q9*K}|)zz0+PYGG1Bzmmk9BnwM!F-+tD;YM}5k>o70g;gID zL!BH<(a)tLEV|<}Y7mZ_ z^Y;YDL?m?BjfwaH3})KzDZJht&+1px|M}fcy6MBa?&xP*m4Bt_c-ShF{s%sFWxGpm zGqP=kk0&3<#Rqo82lH{9*$%&kLNZg3t65|I*yk=EQMvs}d>lTAkGuq{Cr!aS>rk5| z%>c(?anJb(IvgDAc;n2yK$BUr$C=SmddC2P4*K`ZHrp7Jp?|_Yx)39DLmDiJ)pf|> zkk)%(6Z^7+g}dLzHgN_nPBDrHRl87KPE=!r>Qfp(l0^p-)ow(Uq)!?|;kEG1l(P^? zdTBq74}uiP8&%1_MD{afpN7aV<@T>Euy}Tj)p? za|)96&N`?`ajIR-9*Vns;cf`lIH8nyNj?ue81ZOQ;hqRTq}|il%Qg+3rD2bT=Tq?{ zEdpSVriw4gT3q1igMsm(SYZ5&=D_%wt%30uv_bqB^rdjsR^`VEU|3?mQi z<b4pE)WU8d+NEAprXMaImaRR6k{Qq!b zCDmYVK%I8KM?gDl$2(TgPj|u>4}G4P`8glkLku2&30u+kV!Df3WWc`XzOPaa+I9ft z;a_oHyi@LAl?CVho2&*?1(a>Ra(ISaY6-nJkfU8p8@$llWitnO!hjf$ z-UQ>%lB7U{OvLjSrnmXzURYNRy!d;JhI%+>5IO047H=vrUBECt9u%Q3TfC%TlnDmt z46>YK?*gkxJ21h1I%JHxdms|mZAMBlRO)dbXjXhtK>8_#dfQabYRWE(P2e~|>TzVllt5D$a&B8q+( zq;l719@6aIeK>_4-jjJ)Vm?FzbvoB%9`YBGbg>oma0wn}GLtg+A9Ti;U?igP#% z8qPRpgz6cg$bms9&N))uuP2iN+R5v8J~q=Vs&T80wUa)ny85+XbUhB_IcKA{AP8&Z zw%{rxkP)gpMRhP!MdwS!CI$4!9EwLDqG2eEmytU|_@oIdp_NnvTU{S|&ewOdi&3+*$vhmNi5ZTkjXJUHr# z=nvfm5KQ6`8u`+A{es9~m+Zi$jC|~lceQYRJ>el;nA}fUs|u|T^-#$@n34}vufy9D z&&f1yCvQ2eaPaQKKqDz%6C6OD44tbUFNOvhHj)UPA@ap&F@iC+z_iXp-vSx+q<)a$ z_%9wq8Pr9S+taKT-IAOHd&f`NO3~g_!jNv*iyEXmLmRMu}AEJPIdvMg{N*JfffuYBceUs}Uq3&qy=~A~7HKW9mcQLr}uh(1x0A z>g%5OMOFx3oz7`(@JX1eFeN|A-QdM&@Sb=SA0?!*aYjnNdaE^~nyZhFR-pV;rp3Cr z)v3jH`58a!*DV%kM8p9_IQTyC;N2UPKS|siE#J zD83i*F;y~rDCr+=c+FdWxZ)2O+#W~J;3$VYYbxxT0j;o&uEHEB%$XY6R5Oa+84ffo zgmuu)R(+_SY^$Gat7fD;qwbawco^Tt^XXa6?z3#uB#M&bt7$VPzH%e}kAa={)&BPe zcFY~}z#6NTKj|TNqk)aaZ6}AP#%LcTIo#2H9H){L8-$bc35M*EUs7li!oEyL+|i=_ z9*PM*hkW&`>Hj)}V+c{4+L^6iosW99M6N)rOn17AeF*ym4T*v6U6v+til*UMQ9cmk zshs-e6t~`luIdObnU`Wx@~NZrKAo%=ZuMP^du)5st_9)stLY9Ws;F~8qZG-L9_8S3 zQZ|6(ix3HEN3|mWCSLfLGAz3^>?H>S*G_TzEDYYv!1U*LT4BT_I0N)9wcILkTd$nsLF^Vz6m0nr zeKv|anl(_E)&ImO(x8suK4eLv2Y_^gOWLhT$?vqY1so{82-Fs#+}fSYdZn~W%z zzfjv5>P3m`{Kq418eSG{Vz3a|Py?NXslX))>?w96of49)hpV zVNP*UJatn1C4AP@DTF)dL%UJKk^Dr`i)`=!cnbsFQ=Op~rr$u-DE9~uu;Q-)cPGWy zn$;jb%FtDW_)nan7eA&_Q>WNrXmCg3$+(}cx0!_hP$2+|+Lo{l4v>@uwnW8-_0`fwv+ zHJ-!a`@Fy8cGy{`cW34U4fHd#8tVbgSh7{I*g!)aJxuz2*Wn_2q%-9wX+0k3e5S>&EKPd)Ud1yGsr#PC z9bDJ(cqItdgepVL)06%SuEDf8VAi#}C;v^HEAwr1-lf6-q<2}8u<9g_MA#EAocIB? zN2l{X+J_qJb`w^=LkN@I6G^N285$MQA!c1QucbtP?wq@njk z(&470Bb20p?2>*{V{Q=ac=jBWq+knTgzljM0 zeU=nX1G`^aqNBG&^E1aR>2*o@Saf8H%(dsh#Mga?>w!#g*t_2>m&PU+lA3K1u=SL?1_gyriAiw?pwR* zo2h|$l;VNrv7fVE5TeRJv;caNv57wV4Gm5$JekV+y$LAn^_E{ z{oGZ}y7HK(AaI?_!NT=!j^hi6F^A!C9LSGH~L=-Yg z@f5`7ndB1``PBgH>pu3;SXvMooN0ua>7O66^EwTr`!GLJxVP0_79x190zVTWwv~;^ z!9w1}wAyQRqAnO9bi%u-$d(zdZR6XM_LKNarAm+w-Y*@DfR`dPsg zZ2nn@9MozBo0l+=w_9ZDqe(?AqRlpp^{wIf%odFG^wDUH_cNO8$p!CecRDM?%C}8~ zn(Djq@PTPmia4Runcl+YFAv}^nlJ*eQNu6B&^#>|t}$d6)-b z3GiDqT<1a9Y<}@sKCBXIb{7Et%KTF}0#Nle=UD-<0^U^LI~3lDR=^KlPUe8GY3G5q zgYG&*55bsy>q8Hcb)gM9ok^_AKtq_vR$w{^rVk{b7Ld$XA?ikv$sa4%62}e9;pKPe zQim2bUCVroN6>cz&Ff|4A1+dKQE@w5d8NWsTK~VIej)ng74oJ z%ns>Gp`-+@+|ep+v;((13KX2n^@myqP1oM}2b=6cF6AVq&uZdHAD>4}rcHLUuvOF06I<5dPp9Ddn#a1$GJw z*G`N0+lQUb2@u(}(`nEL+363tX}>j*N-5aMW~ck`03v1E-L4y~S2v#&?Qj~*`J!2)XNk}dI>Xp(|mF|ZM_ z$DVIWQ@d!_HkBJP`V8ve9`wGjy%%!7(<_$ZHIuWEUBA>~olv>qr;xxCC+HI@9dMNG zaFm`MX}l=5KGf+;pNzUe(CN&Py8nyPu7_BC19z~FG@Z9&vhSlB^ri1{Ix~MlS+w@b zB%`^c=v_MPpb&J&n{>KIoB(ziI1gCsbT%>#e`qjrwQ>wqxPjp#gpqPfq-y?dsiv7O zG<3M88x0193+))xQ9m#=@&o+uvO`GY^#em9f%N|HJQb3sJ54`PQmzvdkhn-E_QHDj zY1d-wZS7E$nJjF)iY`TG;JI@T=|q3WRaF;lCJ_S-e-ReiGIKq+zQW61BSCQf4>{dV z`FmucPPvMT8+?%Go0{uA2X@R(DO)VkCEk~gX-Rj_pGCTtt~aFHaGgl^2EcETbpHT5 zrgS>A+8L6?>NTtete4a z1OA!7vbh*-rr;y8ZT+I&A$^4yoaOaiZ@fB7$E$XHs@;}8Sf2D3ldF*lx&)$~p%hD# zaXE;5JDs)HawF3oH_{>b;HO;dZeTKL3dVd^g=41TW(8OaM>44Zc*uqf-C z$pF7a{WPR_#7-==Z>#uINJMj|!u?+%6?HnZlOjngsGKvYCK`856^lm}*( zbig16f666A&-T(CI;h44LWh&+w5(wXvhN^4j$-o2M2W<3urEYf-uFkgmUkAqt**J8 z_!HQW$IVeJ6-FoSpjgW8#3~2~A{Qy)FlxEM9s8my087X2f|?LdaQCDIVHkGd-mjD0 zb~-;nDbzjtK;k5u#Hd#gLxY2aZ_23eVjgR;Q;NY1$G;$%*IMrHWg{GicEgZEBOWbR zTt{@CY0q+|(~UM$mG|Tzy4gaf8jTv4>g^dscesZx`xsV9I`uRz+O`yJJNVnZsE2$j zMW7G?JP>0|P_9Tae5zZ_8Pk6wMBI2f1Aetca z$;YR#@Plz!xdFczlZr!$6aFM@T9f%>kXtncxI#Bv@ch4(>(Z{fEJRAm`a{p5vQQ$%FC&?7sX?I_A2wI+M)tkzy)o) z&2TVZ!iW=vG3)UF+E)+h)+ z;~@8{wK0Cikqfg4KB0wt?t@eV9`M_`nBSqirq>7r@)8uG%^33&iOD# zD1NF1-w7qiD(v3YlkcN;d1BRZ8P`Go$0=R^zb<9_=tf*Y?mo7fDwo&q`)bwvDqn1< zHMW{^$-@c2_#KtqYq)?m^WhKm!%HP+du%o7*cWJ!-=<;7Bo0G-LfYVWqIj1DOpsGd z@s)KTA3@;l2ib>77VZJlA{+QI#CIo3@p=N+IoGpcd}~%Oeg$^2O(hDb3(bWS{_S2Oa;!Bbm!b<`q>HbYZg#^TnKTTV54wp4uaniUEin&A zEfEqdrs{3BHmcd{WyWxo+MVnw+%==mQ$xY@$iPs0!ONG~&iTF9n1cmkf$Fmsp{D8j z^A+f$vVI-IsTAsonJ!=Q9%r3|tVUI|n@v?U4XR4MbgP++u>a(<+c9snayGN1>86XH zr6SLuB0T}i1mS*~)ES0yby$Mqc*>O>^fI%F#SQ;YU(BxK?U+Q@h>1K}0(K4~y*`Ep zWJgUl-S?zCg0~R=DW~SU*sp+k?lSJ^>NB{JS_M<-&FM4GtqUbt^ZUhyr1GY1T z?o^3$rfY7_pzg-`^Ff`Stn$|l%D=Ox9?$PrU{rK)vdH}r)MLQ&ppkNtp6#KA9<6fc zMpA~1ls%-J4d}EK3$ZtlxgE~ms^7ZEOx!SP=!u(?E=7rs>l-nXn*yL8K#Wye3~R#h zB7@|ZtGMO1AqqZ(+E2B9c_nk9CT456Jivb!SpAsD&$)kPh7*kb#jZ?rev$_w9_;&3 zF|{`)`l$+xgC|CfhWPMBZZI`YpDN=tw)$WTpG!6rrFn-aIEJ(4ZTx6BdJW1fp}x_p zr1=E~IcQJuO^Z)wBEDVq=`d86EX-R(m!emf(0q;b%v!;4rML0)5JXW(s6Wg2or)0O z%IJ^q@O{%DRwwcAWFHZ7YA77YR@|j#aqqog+U?0Wg^6F;=9LC5`y{*%;smZvlaLu* zPUb??EaXm4AriNFS}2GXau9n{2sS`EMoExXx*E(LJ9e40tB-;7A<#RFj1F4Cwq)se z7#PX)GButS!Pow+gT3+3j%oV{hFRyN+6STa_hD=Y$ z!mmb5x$1|*L7Obx?_#+xt|YPhs_p@liCNowV%ng>v1Ay zSHX8bm>JsXS7+ipGp|u11Py&%n2v%7?r|-t%{{E1pGAO7hk)DEj9$kDU>Da@!*6Be z^h~s~Waqao3eMyr)Q+*In0f!Hm{O=~@YJm{^oBB6cecYnbp1LcZ9|K-E7vl`=eVw1yc*5=vaDmi5~gK9F$fyER0;G_O12Wnkz>w+y_tTD z-O)eC>R$IzKt?AvY39lE!B4R^84tZtU-t&SZIKAQfw$l?K9EY~F&1M%x+w5)^eN1} z+R%Tf**j2*`f*(+|3j-3M2)h@`3$0e9!hwT^Ksrw4KyB#cn+0i8cgpd4OaD_ITlZv z$x~~u*ehNUb9VNMm#yLM=G?ZFd$Js-`W3UW&iM_%Z;@Ag4D4j{iez0ey!IGbC{2Yo zsCYgbJyNGDB~52sVQl#QT}1ruoQ`Mwy{d&BrqnU!)na?l8TCp{b1FJx`eM9~B38}8 zv9^#|pHbUxsIS}Li_)S$@J{Eq|IUEplwqJu`oVFJVf-GItIT9x8!&052-1!P52fa@ zJykJ{%KVE*2>R^bU?UgS_!Rde%QIZhqkR;HUa(LTD=*obd)S`ofBk>{MJl34+cBgED@fQEiuq=3%6TAa2+eX*Lxmn_dk$`v@TpQLKAon zZvI;aO4okxfmI=cYrmI3En>fW!i0KYXWBN$!9!&NDq731jQ9F37%zqzkJbL_Q!`g~ zt6Lqht&SQWqg0lk+8peLTHw}*&sOccX$cPl;~Z*q3IXs0jh|P8N_gD{M@EwV!zdWR z0#x7Iq|T&&vPy#-h2?Rdq$S%#;g-jU66d+WrE5K}Rg3k!O{{-=mTW%)J09y%<1#+} zI0LfKx*eu-o|)!*&w3jO>VRADzI(7c>CWR8bOXT&WsYarZIkq$Gf%XPL4pTc_scmU zv=%A38G{9URpHC4M9I~t$yIXn5>av;RXFE!goWS%nps(L_!kEMt4r5_Uks z3MFAPEXS4br&S{1Eo@HT8Ygrw*vTg0ilLB@PqAPp5^pk5|Im>VZjnSk`730?L6EoT z+?9+(1r6kYp%FTDV6+|7g7wUX8-pJp~%@8lQvofj750O3ZH9m*c0^>yF?O93~468N^;SKAMcG)J`6Lx{>Mj8ANxo z&?!eoF*e%H2QeTxN6x#gMKf7zS_`P@JPr#3N(ly(9*$$c;S9XC6zXUSZ9st?7zp}t zP2oC#EB@&%0w+x)C4lA_D}&yeiV`hCA-pNz84_uY!6Ep9g=9E1cc7oOeG;~1f0C?) z3X9h@@va815y$!U75d^gLh`0igY8T4k#9UesnXA&0opxrPx{k44ZT!PXXsYCAeo|T zXQ-7fNVfA;zsDn9cLUyZB8ERlXe~{UKQ(kv8*vaOT9%HA$0dIRa z;oqCwEFHJO>Hyvl`EU+Ns21DDK2Bmrv09|b3pYPX8|rlCUCd3?friIdU*4^+YxPCOkio9$9ZJvdfS%EADLhZP zAE^Ep9F}e-4nEw%`#z+mQ)SQMW}%oX)FHD$e0ywt=oaL?gPJzIcLLvG%eJ_^*45Zq>%5 z|H_Z}K@1PR=;sF|WQ0loBbX#1a~p0CgUjbGtTk0t5l+e{^W86W=?_!iIiJLc?h#Jv zk502R(hhUc4w<30p5(&^SpT!44wz{uYAzaKYl`~vOoR9^Cf46Y+SxUWPUrDQ)%H7j zy5}3K`(9+ef{8J760HRw=I_O77L7Rr;MCz4Z&~3^S3+e>w0_m-@I^-Aj!a6v`tvs&Y*}>(Jx>-syMQi5LuhUMAearb z(Tq`J7#0o@e8?FF(J3EQn9%}v%X269(ANf(@I*0{rhCzkp>KTPVTim9sO0WjLnK+n zj39s+EI#)48u>(8J(ojc9k!^k`=gYn%1vX1Y;b0LO=d@}H~wZzD3->#%fW;Q6|rwT z7(Z$(z6yq6mrhxreehFAB8_Sb`N=X4pj2k{9fv^Ue&TGX-SxLD#*$*=p;k0aD-FMR zw59J}8q_;OO(-0Cp-o?EU)wm&*@|)eiF1m@{0HNpWjMn)enC028^<#^e6i{jdY&x2 zuaYiB8*y=V#95538Alt`%`zVVQRw>|6eyEe{IxV*6vu0f-8`KxXhn(Kxtw$7qFC4p z2(_W?mQ&>1hVnP9L~a;*odUJfr*^Jjl@2zWzK1wRmWIC~y0c|Debh!dj06iDBqKgg zM6ZSGT50t=3!27(sKGVD1hER{!TB3Pb_r_(4-aWVOr%B)s!|>Eki4!1uEG23%)nG` zkqGUFZN>?PRToIJzIqWi>zE2O>ydMamYPahk3Ua@1PA?u$Mf|g>lcM$WtNK$vWr$z zk$tFLX;!0l5JK0wiL;)r(FgkE8-tct1BAj#}{lHW)Z@- z%g^P9j261L%TJ)&A&k-wPs#0aS`8?YA6mN|>XqB&qY0&#_#;dB5?GEePoqLYii(EQ z<;1>^K%e3>tF@0Ro*b^2$L z?$YVyl73R9)0ZRjDxFw_#7dngMdAUSI0K1$b>c)M+H`_marw1Q&>rk| zqOTaYjibqZC#BQGJaVQbPPDipPcJ3}G6dCDYhwg#nQfUd{XY z%pt&8$ofQ#cm+pe&&Q@p;|YN3SJVG}2tskeCp5rcLLLlUbs5$+goh4sU`Z+jo+iu} zQDTC>^k{wxF^t0dirs?XoYhCqX3r+w>#qt?S?*YdpW;IIG&vV8keP=9QwkDb-g%Y) zwa5Ioc)isvN2!PkzYL&MpZ*bGF%sp__##V(-ZlwmX_G0LHX(PIXkz_m(;upp;cPq9 zo!+L|IUZLmW_v#I`9mnyYlTMSFBnv+%qf603czfpz)QyN3-3^<+_*Co@XfQUV*Uq8+xpw*vtTv+asqL{i z?l$~1dCLYC*cs3gKp6D9P3qaI)MXHX;_MO*P0 zO-TN0bvbKP>|mHEAk9C>r;Ggy`V6Ven(G{Xu4Na+p1F?N`9vYT^h*OZv|`y4J^(3M zH0;7CI1mn;NJA+8$?kry`V1k`q+gyOTm>7zhwL&YsCPP{4S9nTtCOv+anVvzWCX-j zUkDCXp#k^<@5#a&X}Xv!@fP>R;biYqcU@e4p`B{mtVw1J2mui2d>71QkyEbo@#;+0{w3%rFtgj2^T4Lb?=PfU&^YTf|}x%O#hCN*uI7>Qc(c z>k|h{#sYdSudhLG2KApOH?!UD-H}#qH2y%@*_Zm{EMiXy``Ei$lG2` zQ|ES?e5Hru4u+3CeibPqRsYni^DbETQDD!1%?!ui#biAQ1BNwXy*dte6W6|h*RL_x z9#wz}sHtnfW5yi6LdL(&k&H*$87J9YhUG>TyP+8oU|%F@>L2t^9csb%_+c)1JVs#k z!+G@LjE+n?otxnL`>?p{Q#3M`dTFq2wWUv;b!0nRcP5vMR}}dzoF22l(?3DW(76t# z94{PXrTiRw*feJl%86T{yz4Vij?Pxww&290;}WO}{RhTke%YA%1U#O5;&z{z+)-$( z>4EGdJX*5T`OR5cVKY*|;7kjqmrcnVN>OoZfv^0r|6ZD*<`J zyYYUmFU%*C^mRo!(9**c905vWdg6xC=tN zIKO>5TBI_Jk6V9I8c15yz?)=^RG4L28j^_jdUMbSo`#7v1utX^EONJxaNy zVdDId8m3_hq1;BxYY3G#8cQhOF28_KX``on`gZv=E@3#8a0}7${OPd1Nq{rcqDi{hx#_dy1gY7{MJfyFpB zTkz-eNjaH37g3(Kkeft)y^TyHQ^UM5{jVQ22D$FagX`CG3b(;2 z@a`N=|6)Abl0HXji|~` z2S*Ds)|?6v;;FGdESFFKNkb&;fKIQfNP@;Am}L4w)XbyP?#MCLSL^fTBHx7ACyjT0 zh5QS!0`HKu;FqR@((hbubDPiHlBB5pUi5wxK!XD{dm9Xe_xAYV{ZNTInZW5KLQE$K zIDH$Kr9y5aU1vFI?WyTF&Ybyz=W&o5HSeGc8;a+dswZ1je=O8))nioE*M60%J0-Ig zvdbgnvna_F@+VeJtDwKHEaVr=wyVx=s_95AVGIqTX1QO?g%ReHhf5Z2dj=cf%yVFb zi>NYb3UcQj^Cv5l0A5W)^#P(zXBWtz4IkNj>9Y{^4A314L!QhL=!2OVJ&)~l-av(( zoY={b#(nu@k{kDY?755g5VG*|AL@w_4=|C4T~AoobFsG`#W3eXA0T!}08VEF9*g4C zzJ`Y^{#cgXNwnmixmJ zw%p%gPuY2pnh|Y_B1tX?ppopMOG;M!L_yDJ+Y& zO*7yI=GXZ+@9e|-q@8<7*$l?*lJ3{amq-Rni z4dB>}3_^wblyXmD-kqnJN4?>krFqCgARGo!+D)b%f~0>s?{AZVP_V5c4%tAy1jdW; z4_!fh)`eA*neoj0PxVR1;&)L-_1sK;Mff+TP5ED=5S2SwIMo#U1y=P|#4e$Zk^3N; zYaq0m*6Dl@GF=!A{%<0@UiF|@@AWg-diOgM*89mcwqB@>k8h|>LJ3;z=EE6iWjd4= zIgpx-&(WzTG@i@Bge6#)hL(A9wLV6(=^@;l9#bf$mSYjP-Efw!t0}Y*g?6dV-A$~2 zd?I&NDr#VTjJ82&`Ckf$Vxi=%>RFbyA`?Jo z>A6?wc?8%+4<5VtRC{2caS1I9mPYElygmNkFMo!Gql%GLX)UE1bE9H@k89Cqcu$go2sdHY1hdk+J z69;en{J(4Bm#_ppsLV(FziKbci{aA$brNg&9E>f_g=AmuZ#@+gRZyrl2-THDHHE0S zpJe4{*(Uy>@A&FkS~iTYtiol`$kNyi6w=~ALt~G=#YQz{AvX}0gJ9Wy>Tp}CzQb2M z>IAUn1LY(*Te#!JpHp=>jFR-vmD@JlZvHo+vYC_gZnUzSA zp2F_QEKy#kv)!>6C>xvctbR59-xt{~!O6ahhoj1b=qXV=8bY?Az@@XXff6hR z7mCRmSvxv%Fu~9080rclqAkmUJ7{o=-}rPd0m24ykY`7ZF?No`3gPqgd}JP!i#*U) za|N^IA}6-hT$8fpB44%DT*cXPk-OV!u99rI$n_!Dv|QS_p$P<;j5Zmmc^ZvvgA_)7 zdaLY|W=hR84vpkn%~h4j)ybzvk-^8ufI1tmQPz{ai}&lv=?KIvEj}Vc}<7=+Zp-Ajh=k zdgciXHi2*|+iu{RBgq41=`OFGjPA1hXznhaH>K^6`@y0tA-6=xv`d%Zr44c)Ka-=X zYF?|}pHUx^16^|)OF>!kFsM*YmP_L3@gKuYgg&Ovfd?9QkiY{ZO_@ymbwkMrU&r~| zsYKLVWMSc(RgC#*-bS{_k(ZZfIL7bZ@4fRMl2dEM-yH zrb9^+LW?rEW%2Xtekfi4u-EsHff+tP9f4WP=Z%;VaB%$*Q~;GK2$y;PK9P#WdNON(&K%$$z0MJ1nypqug_;%JgR-Qeqf|u`q#{~#iK~i^ zm5Rvq^m0X?(KIRHzdlA6^d0%N-Q?wYn0QCx0FwnX=n`M=EO<%ymydz@cd@gKMwUw? z%egAc6qTh~Y8hr{8HX&3CCfCG#jmm)C|PQ+vKX84DCG9h&NwsmYPT@8iT%3&Gn+h& z8jg}0#;O{At+L3LlK(I}%PemV5k)>3Cy3sebkiw5W{xWCR#H6yi` zo9aO79yishZAJN38EjrU9ODste^^*MnfC0-=(&8Nrj3#%ZnA}v$GXWTN*>Ev`}tLb0fjB#fz6j$|99Z76&V9g18UXM60TnR7c9^1(%jbf|ZMd5#&1l zfCGXTl`k%@E{iOwp1{z&^7&=ag^?i2J1|q;obrmYODmUD2QRB!xG*@UJQ%GhpQpK+ zSg~YL`NTy_=9X1WylBZG6D#K}oH%#s+=WXPm(QD5bKu7TH3!Z0 z0h(P|Gka0_qB-T&<8T@8yCPWU3ywW_Oigg?A!BN0k3Dou&DhCfYNn0^S~#ZWD0+1C zpl7~dQP4MGq;SN!gTWxPAEewzQCUat7o^gB`Y~Z%L}dV;g>?2f$}k>PxFw@B;Rob+ zHg3GC@`_+}c_dm*Itx5LNlg{7VR3DQtU=Xly|&#p&1 z@zTS2h{%+LTyJK3NCsco2`Vn6>&@k|UM?>U|D1Cg)d;YhsHL6^&K<0LBb9r$63=Fo zx;W%fvAiOMo4!O}vRGOgGHFNS9BKqm@toQ$nx{eRN<$vWhpG`R&p5J{s?r&fXJF*n zBGJ_E6#JO;PxZFeg_!{!z6EX|l=Qbaq|`vnNnb1ymPZT7Ohee8G++oT?ibs%9^d zvaJ!yNwuit!9*hEjK&@cUQUym?RYc&UgH@QZToVAdDK;JHl0XiihYI5U}lKoeKpL-%7p?_dKFpE;2NS@r(8)7a%nA+(ej5wESCzV;VT>`ytBr$NqQR; zf~2-JQyXF*E;n(2_9%DiHpF=4)?5Z3~O%^74qfcwhZ}FBSI2dpNC64N83X}TT`Vh*Fq#z zNenz}g9zf7muhYb7c12gG%TCxINejt9QG)S%0eNv^=&fUxNuI9oi4I@WA(z!rU<1x zdYo-TSbs8GOq1%UHmw4oqZTN?NIf|A{}<7m z>Rn@h?mxoI7KiNA*++>3NA_s>ud>sLpM@y&|#F-)nCJCy0 zswrKA`=?ByDfReXd>HK^-ZG4FKv-|-BWYZ#y=C7(vM|Rxl58%K zHq;puif5e77GZa-AwQYT=C+aRCelNFxfF_DM3}3RlVj|%e6SE%M>9lGT4r8?c0_eR zSEwFB<>9`xiug=%Lps~PA~#e@X0YHtmTyEWWO?$+5`h(;99ksco008%qM!Yx!y$aYIH$vmJbcew5suk z3(gGn=c-C>9Va<6lto3V|5E?C7=BgNhbvSjec@EvDrM+sQ&W*HzXH*k$4O{G&iRN! z)fp<;1oL+#J8A{0<$osn1N4FO=hv>)dUHY7sx=#Wx-LlcoZr1>1=o7xEXpoQZdacG>-wk+dZI|C;=M~2=; zrfZlaG!H2%1^2okCNWgXB@A6W5jw|%SIF~DXI^y}+YL)={NWkvp(NuKewL90W{_MW#L{#DcBRCbTXfxl+p zh59Ci4Kz`;G~-IUSCL!0aH3c*V(k{1EqdM-j9@awQY@KDC2)_CS}LD|7a-PR&`PAG z)J3+oBuX7SP~!4VYJ(!i`U9_!H=+omv5P88Dp?8*{Q;TQ;cfqv}Y^Exo;$`{f_TGBj+#f4F11?$h%_+>C>L?KkY zT=H0156A`vq@@_J2?I?{0tc`Rj;h^+E4Aq1)|oXEYcwq5khLHhE@oGEfP$0?y(Uzn zhN_g73b|U{O5xGP&3KcC|2eiFE^1Q4mgrK*MbMDF#NhLm$ayF>ctGpp3&((78+`eK;v3HF0n`tN_WsA8+X!ZB+G8aI``AVEk-* zzm!L9z*@}QxfM|bVMS6ME*?pLOoQumU%8aonwH_O^f`ys(%Q+6W1XrrI22tiir>n_ zikYjaOGR+fOUike^i)+G%ZQWWs1?l{2g;R0TNfl~=2k|Hr3)FnQn%stD)v=2_fV?=l=M@#48aV0I`AbYI2At9uSNvNIE4G4$QfRnC{k6 zXnlq{Gp`3zNg8qDi53hq7}qUj1V&B0^dcnJzZCtD_6bd1Pe*$@59KlPz>K;BHYPXr zI_i0@Pp!J=EN9Cm5AI4$<}9pjp#fkLgNfGosi4Qha_og5=a3y0mY;(g<0;@^`v+b@ zVZ|bm#ipvP=#GhJde|E*4U=iBana~uYr2p73GVBwG1XR!2InZ}P&u2;mkPBqVXI+f ze-L{;*cO;~lsH~uWp4+M)_8d-JleO@0Fp`m7)lb$=8`Fy4WM!5u!k*INTmxHdS{cv z>0m2@48TeSD9GdJ7`9ZTWGah|TZBgs7amtZ3TsLVRSdq^Y~-{u zKgsoNNo*=73n_6Hu7M5@b+V-q^}z1=|4DhoXnomqvOrdokm=e<2j2FJ@0=Bx2hF09 zEf(GQ(lL^2&hRhH$)yq6LA6*t+Edu{;c>JR)1N66OQJ12?wlQ&o0OZq-`*ucMIMeLGEH%f=b9uoE0 zG-3&7pa$T4bhTc_JrWTKkF1>uhG=CeWC51p7U*0(xp=l=g7Iv_k>lA0j%QUv8MkvR zZoq<46Vf$v;ZTaG%ODW)A_}#lLJWiL#9%I!ZVcipzpldAoAErfYVuS(SzK+WL#L)t zOywN4>0{1kU)Z4k)ZqdJFdI^1v@^kmoXGkm0!z5*YcLQmrFzmn_9*(pXm5PjE>SaJ z!|?ic&(T@(tHtD~OAz33M4y3wt5vOl^f76S#WS7G^e zR*Duzi}XH8;-DMfS+j@Ul~a{X{QZSW%oXtdqLkZG#Eir1&n7ovkKwFYSC?lZqNcE| zfp#cnF=$C(Uvz>iI9Uum8td;-5 z8N#+trqU3Zf}DNgt?2e4o5t%LVbcPqa@Z?taoaJ{(+#M?mBwBry@PA%o$icvPuLP~ z?`dsX?l~UlEe0scfr*9U3K@(qx0}kvAO6eIdOGi?V4I zP)MTy=>pa_G2F(+UdwY;IfLn`eWIQ2a$g*9=9G86ipIRg`m&5k70*N61iT3^ODa%7 zW@wTa;wzE2oXNe79r}bV#`Z24H#7(TJVrjtJuC)C>);cZMbaZ zBG!t8ur*UCmGRPuYnFDV**om%zyMXAw$X+N+md*QBluzKT)1(y2I~in5D!5c#KXci zD~6&Z-cgGndI%EP02?bxZs zjT516>`vc*9e?$A`TnW+n~A?>{QVHPU*fM89d%EEle$>@OA!hXN&dA;;}8ZFv*q=! zY^0{fWcn~6UV0AREA<=vQGkt`_I8sn*cWM}UB^v47VoEjWW)Kal%Y4vS%55HY$4VR z@&DR>^X^%F?>5*Ta*3HSt2o0v_<-;K2!C(>xbL^(ZzKM0$KP_`iukKV$3_fnw8yS* zo>F2IS+K=nx6+d6Z($I)2b0AuiqyC9Fk(E`9FMhx_Gm#P^k0~a$iCrX9#cP9ixB93 z#x3i{AU~WsHb*yfg^>`6-C;}nFt|mx{PURF^mMwu(utm|68Va@sLiIr&`!UyY6i^7bHpdxd5dQpqE zwbb4zVJmh%ClO>h3k*b&tzbG#XxBQu^rZKW73C2W7srs!#;IhArfim z$L<58A6a>{o@Heed31|~0eW#7vU<#$A$FhPl`zV{hVe88JlIUuM>{9CV>N-E$wg?X z)}z?w(Nm^Blg979c$=q(aTIaHMd{oqtwXFHuC z$Jp}2yzzKE5*W`J&UUjLUV{E9)IK=ncspsrn71O*sTj!$ zT{imRDmhYLUadZ8bdPZX2Ul)B)DtGL@tMZ~HS?LEeJbq`Gz%#|Z5hIj#JoJ=V^;P? zAuWiZ=WlLKu!Z^UM3^O@b3Kfshk5Bf;P1!c{Psi{qf0@G@kGe{5k93dQq>mzgi$cugkwn~16F5{Ih`B{Ot zLSFmRR%36?%C$6R_)oTI_b?Xqybb&d8hr=}X!o{)UjIj` zI%m#&8vOQSY=Yl}VyUzKOx)3aVSeBRCAXKB6oR$vq#7Il(4s@nG-yEWIeHBPyY(sc zVq4)BPZcuhw7Rp0@jXO(19mz-uzV-(v%fmze+BS9z$XCjdhU?_JmAz95BUvnl*<6) zfZzJBLw*ln>ZL<|3Gg#7AM$qtuKDdD{}I5s{vrPeO3oWH>Lvw)}LqkOjlz7F^> zU>81$_c-9afX@RC;iH@`|k%l;R@Ib@ZYb5y#ROP`-R^Gd~3h&KMVK_;FRNWFdIG%*a-NR z49X2y-U7V=zW{hE;JdQW8}JIi#{nM%d>(M>5cEF5m_Na15aWQm0lNY3D!|@=3rny! z;4{OpH{eytsnH{hwa z`To;@*8?6i)tD!5hu(lE?SkHb?*$wHEPvSdZvwpJBlt}!;Nacx6Tmm`M!5mM0667D zlMJ>bdI z0C7#a`T{d)xM9+1CmcJ4KJPyhxYO}B^3)-}8~$&mHk>12e%q80^PYFK&uN`G z16WSK5^(R&(H;q*7s0Xm+^cR3la z0sSoKtHLt6AY(}&!-Fl3|IgYoIzhh~^k1N@x;#jae>~Rdkpj=E-yQOkfR3l`l1aqi z0d)iLH~s#Qe@mE7dhBEtu7|y#{pPhp{te)FGA8eEc^m+J!|R9qJr(r39sOC*e+c^R z;XLYYbUI9d4VEKT_C{^dBew7$w+Xb7NxuJ!unf}k3DJ{yy1~@ozUqw7}`P7Zo zTf4sox;fSNe}{PE=u~(Ae!SC#j>q_?8NUC%ur770MG++P@JHuV)Xnt$Yr{Nbm#cy@ z4}j;&S%_KScX>^|-Q{x=vu&&H zW~oG=s7=2Fp1+$HsiV55Sy`gfKDRgR)K5PI`VTAU-*xmmLB9oqgIsf2qy)*H_TH9K8qh84G>?>lJnIC9VUq zUkUVYEcX4|D)PU9^Y?(e6ZF^;-=~8zo&Cw5HV6Ln2zai?7$9FEW0Nbxlc4_q^siRX zzu@SvfS$wH;Tz#R>K@??JfxY9u}cHS6l1WJ(}Dc)U&SA(981CT6vi31SCsQcSI+l? zejbf8!0Tj879BmHJ^=c?pwEUbj!xw~P1-Q=>;cc7HNKw?>p?sh2YP%HJU?IO`*eP= zlT-J0SC*$ir{gsrh2J|m$@p@ev-uRn?;9?_HzvX|i0Au(&6~h;PLJ<@F3eN+DVNV$ z&>z3h_jiQLLpIwS#H2iU&b%CBEU4minf#cmmtCO$81#=<5I{0s0?(w4zW<)EjIFE!x8IY|2YaAz4$~(mosMmwZ$3== z9?)+FeN{O9HG%vR==(^1MH!RMj(39oyQJ@*6fHwtP=-gq^Gu)bZx81&InSARz�i zXA19b!g-J#e#6-q;&}}`Z^tX)nJk7S`s1MY z7k&Tqu>aM)m7S01&x78J@$$_T=|^0>o`8PxG0^ESS0}&jF*XLJr*D*9do}14IzHyw z`&!UjcVfO!K|j&av!Ktt*7vtmrWWcT~^|j(!mIHJH=XN68D~<(~(A1?HCh74$0{eHZ9AgT6D| z2I@+#4ebN{-!KQQRX+zozvvF%|L=+z@VtuwW1!E)9Q9Kb{(Zmm?*`0aKaV+U0)0df z11>t26eRn^K_AJ{XV@-@MOo!y^6oBagUyn=^NpIKZ_-*oh znm%Lcgs*ofTOBhY2(|9!~M(eoJ1XK<++W>ghZHOy+7PvgS$oUhZAfaxk<(-qPZ z9$)m53YUsCqcmT^Mem*H(noWuJeuxixv_Zf6nVCK#e0RS2&_@m%=>?_bJ@3@Sws`g<}fM zO3h!Pr!cOtQ(?EljSBM$M-=W-xJThWh5HpAR5+?|Orh!0{1ti%;|eDO0fkdm2!4e^%fCaPcPZSjaQaH& zZ|D+uK;h!m@;sn$uR^m%@Ou@GDm3Q_zFXlwg)b|dzE&F$W^~{i>xA!n3V*8b zX@z47UsYJgFC~pRQQlEIs@GgZPQ@B^*7ZrX};Xf$+ zslulfjwyUy;ql#~_gfXdOW|CF?F!FT*rRZ>!eNCsD7;lNOv@E(N^Dg1)MuPZ#D@Ck)KSNIQMg~B zxlic|M-=W?Xzo|K!V!fw|Em&0x6e@+rSf|bFnvtzvrpl{8uqG{{+f$~a=XIa3XeAb zA1VKs!s!=Ba}r5WtkwTNmEY#ya7mQ&@aZp9{)bj*h~3ngW8C^dCH_rrouLx{W@G2;mH1Daztt(XGv5L_d#>jo0Io^0x@h6ykWL`lj>adY{PgVR>Gx}bU8UDA< zoM`O2O(nj;7;n7))tQsbF&nGlru(mn2se{VppPxZxr`rgpFx#l*Ym8L?aV*k&a`i|n*Mu$Cq3^`J$X+xa1Y~el4&qc z&J|7UI^UzpKaDp?U@r%r>G~f2NY0gt=Y7?{{Y>%S-GCchyq6mAzwt->13iSr{7u{I zambGFpX1G+0sm~pue(_IdA~Gn7bt#yLhwmZ*Uu?$KN-P43_Rugy_CrBRXcoH z@$pT9{{^1u`iA1C@dg}YUQql$0#bd&n*@Ot%h#`!UpDj@bBE%6=AUjxBl^%jZqnz& zTZECyN!OW*e-k+#uIt|?K*N% z`A2pNKkv21?KR3jUHS3T2fpqGzN(+AoX7rD_^#D*eUar%H?KtW{F?H=_g3Mj_Cwcq zl;3^t_j_`k!@eO8Io7l(t;a>w^tzN#a@>}l``PQFl zuPyoWbZW@BbUEMbg?{4A$xd8JoDrb^b)M~}QsQ5LC*RW!yLJs-uXhi>0RsLTn zKkqSzta*wr@Wwps#aH=fvmCTL)ziK&=~4cJ%KxD9rxaiMs35i}{wl>kr}%@4-v&I{ z;cXP?aKSzJx`y%7%|_LKMDf?FoWHwQ5HCtHbEo1TzF+VZ2kH8_;(w=snfIIn|GyRg zHxDqIxlMri9`M}WBX;|-@}K*0VH{TZuPXmognu&P2i4=(J|X;hl|K)7lK)DCf1&cn z9u&r}D*t7Qe@^ifAL*h!@+AM^PsuayL&xn6D&O2GjJEyWru_GPM)-M8I&SY*{wWdu zhZNrs!GBfp(<69#pGM{KB6xbQM)<}E{+Ei6NANEwesKivwYB6!+2N%D6^@L83A>bFGx69UW* z#XqO`kE@;^VmS?FRM(^J_-&8!-*!L*+@NykcL8LFlfEMe-v5u=$5qal%CYZff5rIe zW_Lu-|4{s%2>w;Y?~UM(K?9`n?u+230#EvEe@yh@-x}cdOyz$h!r!F)k0}3UNoLk4 z|NaR72IZgec%}S|@*jxs7nDD#{Hs*{pDF*r2>;#6zgPMBw+cw}ImJ(-0W&WCjRN4; z6n~}$ej3Nq^<$O)WJLa`;zuL+U#pz^RL<+x|CRsQ2>)cfuqOMDMey{SU$Xx_KM*}0 zP&t31_;z(b{#^rD+EmUf5jp25|8q}@936g{b&4JITpV##ZJo5GTj7L0I zeV)~h`3K5BqWo6>e+HiNz5L%K{S#`>7ZgALS;ecI$@Nq(7)Plb-9BJW1D@p6JtzG9 zTM698m49D^|16Drr@bKjTLhRjD(6PU^KT|V+pKa%Rn8g8|3Sr1xw5ieZc{ny$AnR* zi{>H4U-etT>oCY1P&uB;vGRYe{ExgM{F^o3R}??vRYB}f{E5el9qw1W<(~sQ*=@DT zM{>SaD1ZKU^1Ms&YZY&PFNix8f05$vRQ$IUUr_lQRepowZ&Cg$UKa%az5uto72kA7 z@H#y>{9sr*}%|1XuFf2RQ2{mMV8{L_^GON>WCI6qIRa#)%uq! zn17mi{3?;J>RG9Mp8u(GhPc`)#&p)e)#%l1tS_A);lc`kG&0=j|Hoi?k zAex5C->!l4muha0@nH?+zNH5KJvH#FYv3;cp7e=F^trMIf4&C(gEjDX*TDa64g9xi z;C~9dH{J~9w>9_?;;Yv0G~lVcmqf~Yn&O8e_%m6~RP)K7N#^`}RNO99{M*I^zeIIg zr}%$)O%T^A{tCwHLkMiHfiKm-?}XEm{!=1)o}zx$8NrX#kaK$t{3jSc)%@UBqU6o0 z>tEI2|5gqB4{G3lQ3F4W4w3vnI$k)=<_;^B!k@TaREjzr|o zs=?pF_^IZBJETJH(0Re~8vGa4zz?XL=s0#e@KoN$h(0&gkaGv{p87@@JWzxGvnt<< z$p2Ce{zq%zpJe=0^Zx&mT={p?xP3+C4@Bfo!UPbcaaThP{LC8oIOFj?h+atH;@`Uh zE~~+RUJd-^HSn8j;D>AAZ>WL4qXzzyHSm911Ah_XA;pvEJnJ98Pkyp7V&@;R9L#4m zZhw%igl{U;kpG&>nI4hzCeBwAhrl$A&)pIJi_m|OJ`EB4yK2a3s)1ip1HY;U{(>6# z-WvEKse=Pc%*Y95>>`XHS0wWx-ib8FyD zU!hbimHYc~ssui;te@&nlyGbYzPL^wiA^|BJef_T@O8vuB3T~BC(?091I|uJ z#TLgGHdkk&^NuqK`5YzAp~2Vj`wR4W$y9l85MRnqgq{*~d{YH0f5s3xr3XQfGt!Wx z`+}~Xo=X$wUbucm55DuANW|Kjo0|YidLC%z_#DjAnmnv59 zQt9yV&mvAZbEhD%BKS@&ogBdTbhZc%bI#+_s{FN0`rP>nUs}RI`EkafX-@2-PA&_^=DTrSmpeGM&7U$R>N!I4Pi-ky;il z4lxGBqeIXkd+`#XrZUC8LS_)hRHLF{^^j#a2aCbSKVjZjTWj+oe5%{tFQ$4ZQVo*@ zsw>jkvPiO$&sJ7dwyl*uqMyWZDz^Sh_|R(~OXUxgiq`S?62r%_C+NI;)4RQtPH=JJ z%?l(C5u)5JOAMXwg%2hdg{!p%x@Y@g&r%ZK+Ds|7+0da*I2ul$<8)v*eK)r^gKzF{ zS8S_%AU`wIpG(mB0nM?tcxzh(*+R$#5o9YN7m9*Gb8`h$Z5PC=+b_OgF@4Y49b_-Z zkybThb9^CxC4EJ@hCbR~~X&J=cY8aNTrr zD0CWXf0hn27qtyP&;d5EsToY>jhwuPeQTL)g3QYOL2u1a;w$EsBx zzT2DVFAw1x^f}$`psd`i!YBWUy4A!lY_Wbvt&Z}jnt@Xml~pOm znV(ry>(+P+zF%C(WwWk&Odr=rYkUzONs;bDK6E;oV4D|Hap8EaEpam_xRy49vwt>~ z1`>S(>Ao#QYbGvuVrxr_DJWN~m8U0t3c7g-&g`ILQ#25xVi0`sZT})Z?3+jQrdZJm z=fGJne0HK7E@owgbwfxKx8qYPN@>y(0jvT?I<>Vbj-wqR;5~f)D|a678p#y#(0-WK z`Yat(1zj`TMOEn+S{E${(x5WwT=`HMIz3#E-ekp12q9K+#nj%1HEC;E;P4a@;j1`N zktM|1+S(SvqlbgXHh#ppK15pp-eNAhHLdwNIfysa8+#jyU~R1@&EXk`Xvl|_!`@0= zN_8O%;;mA1ZA)5B9)*D2+m=8uZc#05OWKTty0#??Rl1xD-J2Vx-fu`!FMt!)$eZhq z<;B_-G`EZsB6K$2f@Yfs_jCz9xINl_7rg9aM7Yu4l3o(gomWp z_}q9^=@hImCb3`k4+{j^p5PKj50-IkBpnih<0CP!3zMqnfR4J@ryt?Qjrt`Ggu-Ks zBg)0Vp6X z2|f*exG@;~%8gE}{cw(|Gg?5Us-kH^l^EQHe!xkwqu&~wOHju{IMN8CIQL2N>Z}Wv zP^UqogCgI^A$YwQhNEK@w0#JXN}7U6co0(692US86Ns^cc{(p9p+_y@Xs0Sjn){67 zY^jE9t%jsfFfRFmYRIZ8Evm-3xEgXvHDq)3B2_ID0(%uzo2!(;CQcGda|6J4t{!RnP40M)Z@g-yC@?fJ`> ztxcSN?ztPfdJ;X$mapxCK$b%$ZE^p`Hno z?8R|*;{_sMpuMIy;h16$vvhD*GMmk9?Gm}j#3N)aJ}`nL z(Tq2GsDmCqCUJ%yAR&|I%(o>ALl_lnDY(!ip`Ql}mE{O}7CsvbGBC!iWYLVc-_?6N z$c7&wl;hClzJV=?{$wT_q{PTa>oLID8qO4xS(@O(HhO*q@-6fY6vne+sQ|TNOM;`d zP&S;?r)LmG@(kp=ZplPC9n+7=PO(Zp6w;FzoNq#1VK9Uyg`p|tWHni#avoe%kz_=! zRW(o0jk4n497>g?YEWe9bkR&I((o`Ka0i`UBk4HT(@cVs^>}F@+#|YX5>g88jzyb9 zV$&cNUU*Cm9#o|oQ@Tvt_XfU@m63Mt1rIs!>PH)ZR%0;Od zbdVuCql#(Si`bG9aU}8~dys51PNg>?SlW`=(UpA;iXK!AM<5PURfAN(IBrPkv<&L7 zXy;Y7ir`6o!%6CO;LcY^%5It1a1u<@g#yAK6|$5o=P|jYm7j0~#PCOkLN@r?V>njU@C|xA6Qh&)OpK2LWg8~~lZxn!U1?=dax)bvsP6>aGE7-Sop-u`aWLi?SksQqmD zuhaC)RiOQDl!c>Ixwt4jcHg-DdN%znz>!_CZBwq!N=W_{zN{V=cO!1X>Ag;&SjcCw zSY|+M7vRgn592AE-u|xC!npEVVAEMhKVJ!_x4%ENu#*fQx@E|Hf^!9hE7XHX? z#8eqJofY}%NP7EwRtvF>tm2B=?{k{o*1!ARt4CTvw@wD`AJ5&%&6fd))26q-kG1dv z)Hp&{H2+6I3#YfgqqVSG+mjU$ZNJ|Kfmm!d_V>2&ouYq~-#%OTQ>4W!R+rxXE_d-! zrhf)$$#-r3_V>G;ntn{waNlvW`PzEC3u%ePrnkTIjbAGX4}uoHY1{qXnPBY`;sOwa ziw7vav+3tX@HTxIMvBNOMs5BxL;(Gc|rm1v(8la{vGU diff --git a/Go2Py/make_msgs.sh b/Go2Py/make_msgs.sh deleted file mode 100755 index 72d7554..0000000 --- a/Go2Py/make_msgs.sh +++ /dev/null @@ -1,25 +0,0 @@ -#!/bin/bash - -GREEN='\033[0;32m' -NC='\033[0m' # No Color - -echo -e "${GREEN} Starting DDS type generation...${NC}" -# Clean -rm -r msgs -# Make -for file in idl/*.idl -do - echo "Processing $file file..." - idlc -l py $file -done -mkdir msgs/cpp -cd msgs/cpp -for file in ../../idl/*.idl -do - echo "Processing $file file..." - idlc -l ../../idl/libcycloneddsidlcxx.so.0.10.2 $file -done -cd ../.. -# rm -r ../cpp_bridge/include/go2py -# mv msgs/cpp ../cpp_bridge/include/go2py -echo -e "${GREEN} Done with DDS type generation${NC}" \ No newline at end of file diff --git a/Go2Py/robot/interface/dds.py b/Go2Py/robot/interface/dds.py new file mode 100644 index 0000000..8b8843f --- /dev/null +++ b/Go2Py/robot/interface/dds.py @@ -0,0 +1,173 @@ +import struct +import threading +import time +import numpy as np +import numpy.linalg as LA +from scipy.spatial.transform import Rotation as R + +from cyclonedds.domain import DomainParticipant +from Go2Py.unitree_go.msg.dds_ import Go2pyLowCmd_ +from cyclonedds.topic import Topic +from cyclonedds.pub import DataWriter + +from cyclonedds.domain import DomainParticipant +from cyclonedds.topic import Topic +from cyclonedds.sub import DataReader +from cyclonedds.util import duration +from Go2Py.unitree_go.msg.dds_ import LowState_ +from threading import Thread + +class GO2Real(): + def __init__( + self, + mode = 'lowlevel', # 'highlevel' or 'lowlevel' + vx_max=0.5, + vy_max=0.4, + ωz_max=0.5, + ): + assert mode in ['highlevel', 'lowlevel'], "mode should be either 'highlevel' or 'lowlevel'" + self.mode = mode + if self.mode == 'highlevel': + raise NotImplementedError('DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.') + + self.highcmd_topic_name = "rt/go2/twist_cmd" + self.lowcmd_topic_name = "rt/go2/lowcmd" + self.lowstate_topic_name = "rt/lowstate" + + self.participant = DomainParticipant() + self.lowstate_topic = Topic(self.participant, self.lowstate_topic_name, LowState_) + self.lowstate_reader = DataReader(self.participant, self.lowstate_topic) + + self.lowcmd_topic = Topic(self.participant, self.lowcmd_topic_name, Go2pyLowCmd_) + self.lowcmd_writer = DataWriter(self.participant, self.lowcmd_topic) + + self.state = None + self.setCommands = {'lowlevel':self.setCommandsLow}[self.mode] + self.lowstate_thread = Thread(target = self.lowstate_update) + self.running = True + self.lowstate_thread.start() + + def lowstate_update(self): + """ + Retrieve the state of the robot + """ + while self.running: + for msg in self.lowstate_reader.take_iter(timeout=duration(milliseconds=100.)): + self.state = msg + + def getIMU(self): + accel = self.state.imu_state.accelerometer + gyro = self.state.imu_state.gyroscope + quat = self.state.imu_state.quaternion + rpy = self.state.imu_state.rpy + temp = self.state.imu_state.temperature + return {'accel':accel, 'gyro':gyro, 'quat':quat, "rpy":rpy, 'temp':temp} + + def getFootContacts(self): + """Returns the raw foot contact forces""" + footContacts = self.state.foot_force + return np.array(footContacts) + + def getJointStates(self): + """Returns the joint angles (q) and velocities (dq) of the robot""" + motor_state = np.array([[self.state.motor_state[i].q, + robot.state.motor_state[i].dq, + robot.state.motor_state[i].ddq, + robot.state.motor_state[i].tau_est, + robot.state.motor_state[i].temperature] for i in range(12)]) + return {'q':motor_state[:,0], + 'dq':motor_state[:,1], + 'ddq':motor_state[:,2], + 'tau_est':motor_state[:,3], + 'temperature':motor_state[:,4]} + + def getRemoteState(self): + """A method to get the state of the wireless remote control. + Returns a xRockerBtn object: + - head: [head1, head2] + - keySwitch: xKeySwitch object + - lx: float + - rx: float + - ry: float + - L2: float + - ly: float + """ + wirelessRemote = self.state.wireless_remote[:24] + binary_data = bytes(wirelessRemote) + + format_str = "<2BH5f" + data = struct.unpack(format_str, binary_data) + + head = list(data[:2]) + lx = data[3] + rx = data[4] + ry = data[5] + L2 = data[6] + ly = data[7] + + _btn = bin(data[2])[2:].zfill(16) + btn = [int(char) for char in _btn] + btn.reverse() + + keySwitch = xKeySwitch(*btn) + rockerBtn = xRockerBtn(head, keySwitch, lx, rx, ry, L2, ly) + return rockerBtn + + def getCommandFromRemote(self): + """Do not use directly for control!!!""" + rockerBtn = self.getRemoteState() + + lx = rockerBtn.lx + ly = rockerBtn.ly + rx = rockerBtn.rx + + v_x = ly * self.vx_max + v_y = lx * self.vy_max + ω = rx * self.ωz_max + + return v_x, v_y, ω + + def getBatteryState(self): + """Returns the battery percentage of the robot""" + batteryState = self.state.bms_state + return batteryState.soc + + def setCommandsHigh(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2): + self.cmd_watchdog_timer = time.time() + _v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z) + self.highcmd.header.stamp = self.get_clock().now().to_msg() + self.highcmd.header.frame_id = "base_link" + self.highcmd.twist.linear.x = _v_x + self.highcmd.twist.linear.y = _v_y + self.highcmd.twist.angular.z = _ω_z + self.highcmd_publisher.publish(self.highcmd) + + def setCommandsLow(self, q, dq, kp, kd, tau_ff): + assert q.size == dq.size == kp.size == kd.size == tau_ff.size == 12, "q, dq, kp, kd, tau_ff should have size 12" + lowcmd = Go2pyLowCmd_( + q, + dq, + kp, + kd, + tau_ff + ) + self.lowcmd_writer.write(lowcmd) + + def close(self): + self.running = False + + def check_calf_collision(self, q): + self.pin_robot.update(q) + in_collision = self.pin_robot.check_calf_collision(q) + return in_collision + + def clip_velocity(self, v_x, v_y, ω_z): + _v = np.array([[v_x], [v_y]]) + _scale = np.sqrt(_v.T @ self.P_v_max @ _v)[0, 0] + + if _scale > 1.0: + scale = 1.0 / _scale + else: + scale = 1.0 + + return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max) \ No newline at end of file diff --git a/Go2Py/robot/interface/ros2.py b/Go2Py/robot/interface/ros2.py index 05ed19d..190b756 100644 --- a/Go2Py/robot/interface/ros2.py +++ b/Go2Py/robot/interface/ros2.py @@ -12,7 +12,7 @@ from rclpy.executors import MultiThreadedExecutor from geometry_msgs.msg import TransformStamped from Go2Py.joy import xKeySwitch, xRockerBtn from geometry_msgs.msg import TwistStamped -from unitree_go.msg import LowState, Go2pyLowCmd +from Go2Py.msgs.unitree_go.msg import LowState, Go2pyLowCmd from nav_msgs.msg import Odometry diff --git a/Go2Py/unitree_go/.idlpy_manifest b/Go2Py/unitree_go/.idlpy_manifest new file mode 100644 index 0000000..25ab5be --- /dev/null +++ b/Go2Py/unitree_go/.idlpy_manifest @@ -0,0 +1,19 @@ +BmsState_ +msg + + +Go2pyLowCmd +msg + + +IMUState_ +msg + + +LowState_ +msg + + +MotorState_ +msg + diff --git a/Go2Py/unitree_go/__init__.py b/Go2Py/unitree_go/__init__.py new file mode 100644 index 0000000..cbf3d21 --- /dev/null +++ b/Go2Py/unitree_go/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: Go2Py.unitree_go + +""" + +from . import msg +__all__ = ["msg", ] diff --git a/Go2Py/unitree_go/msg/.idlpy_manifest b/Go2Py/unitree_go/msg/.idlpy_manifest new file mode 100644 index 0000000..8491d25 --- /dev/null +++ b/Go2Py/unitree_go/msg/.idlpy_manifest @@ -0,0 +1,19 @@ +BmsState_ +dds_ + + +Go2pyLowCmd +dds_ + + +IMUState_ +dds_ + + +LowState_ +dds_ + + +MotorState_ +dds_ + diff --git a/Go2Py/unitree_go/msg/__init__.py b/Go2Py/unitree_go/msg/__init__.py new file mode 100644 index 0000000..8b01d8d --- /dev/null +++ b/Go2Py/unitree_go/msg/__init__.py @@ -0,0 +1,9 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: Go2Py.unitree_go.msg + +""" + +from . import dds_ +__all__ = ["dds_", ] diff --git a/Go2Py/unitree_go/msg/dds_/.idlpy_manifest b/Go2Py/unitree_go/msg/dds_/.idlpy_manifest new file mode 100644 index 0000000..2834913 --- /dev/null +++ b/Go2Py/unitree_go/msg/dds_/.idlpy_manifest @@ -0,0 +1,19 @@ +BmsState_ + +BmsState_ + +Go2pyLowCmd + +Go2pyLowCmd_ + +IMUState_ + +IMUState_ + +LowState_ + +LowState_ + +MotorState_ + +MotorState_ diff --git a/Go2Py/unitree_go/msg/dds_/_BmsState_.py b/Go2Py/unitree_go/msg/dds_/_BmsState_.py new file mode 100644 index 0000000..4f41d71 --- /dev/null +++ b/Go2Py/unitree_go/msg/dds_/_BmsState_.py @@ -0,0 +1,35 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: Go2Py.unitree_go.msg.dds_ + IDL file: BmsState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +import Go2Py.unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class BmsState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.BmsState_"): + version_high: types.uint8 + version_low: types.uint8 + status: types.uint8 + soc: types.uint8 + current: types.int32 + cycle: types.uint16 + bq_ntc: types.array[types.uint8, 2] + mcu_ntc: types.array[types.uint8, 2] + cell_vol: types.array[types.uint16, 15] + + diff --git a/Go2Py/unitree_go/msg/dds_/_Go2pyLowCmd.py b/Go2Py/unitree_go/msg/dds_/_Go2pyLowCmd.py new file mode 100644 index 0000000..cd97b13 --- /dev/null +++ b/Go2Py/unitree_go/msg/dds_/_Go2pyLowCmd.py @@ -0,0 +1,30 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: Go2Py.unitree_go.msg.dds_ + IDL file: Go2pyLowCmd.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +import Go2Py.unitree_go + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class Go2pyLowCmd_(idl.IdlStruct, typename="unitree_go.msg.dds_.Go2pyLowCmd_"): + q: types.array[types.float32, 12] + dq: types.array[types.float32, 12] + kp: types.array[types.float32, 12] + kd: types.array[types.float32, 12] + tau: types.array[types.float32, 12] + + diff --git a/Go2Py/unitree_go/msg/dds_/_IMUState_.py b/Go2Py/unitree_go/msg/dds_/_IMUState_.py new file mode 100644 index 0000000..1704375 --- /dev/null +++ b/Go2Py/unitree_go/msg/dds_/_IMUState_.py @@ -0,0 +1,31 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: Go2Py.unitree_go.msg.dds_ + IDL file: IMUState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +import Go2Py.unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class IMUState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.IMUState_"): + quaternion: types.array[types.float32, 4] + gyroscope: types.array[types.float32, 3] + accelerometer: types.array[types.float32, 3] + rpy: types.array[types.float32, 3] + temperature: types.uint8 + + diff --git a/Go2Py/unitree_go/msg/dds_/_LowState_.py b/Go2Py/unitree_go/msg/dds_/_LowState_.py new file mode 100644 index 0000000..3961e3e --- /dev/null +++ b/Go2Py/unitree_go/msg/dds_/_LowState_.py @@ -0,0 +1,48 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: Go2Py.unitree_go.msg.dds_ + IDL file: LowState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +import Go2Py.unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class LowState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.LowState_"): + head: types.array[types.uint8, 2] + level_flag: types.uint8 + frame_reserve: types.uint8 + sn: types.array[types.uint32, 2] + version: types.array[types.uint32, 2] + bandwidth: types.uint16 + imu_state: 'Go2Py.unitree_go.msg.dds_.IMUState_' + motor_state: types.array['Go2Py.unitree_go.msg.dds_.MotorState_', 20] + bms_state: 'Go2Py.unitree_go.msg.dds_.BmsState_' + foot_force: types.array[types.int16, 4] + foot_force_est: types.array[types.int16, 4] + tick: types.uint32 + wireless_remote: types.array[types.uint8, 40] + bit_flag: types.uint8 + adc_reel: types.float32 + temperature_ntc1: types.uint8 + temperature_ntc2: types.uint8 + power_v: types.float32 + power_a: types.float32 + fan_frequency: types.array[types.uint16, 4] + reserve: types.uint32 + crc: types.uint32 + + diff --git a/Go2Py/unitree_go/msg/dds_/_MotorState_.py b/Go2Py/unitree_go/msg/dds_/_MotorState_.py new file mode 100644 index 0000000..48a47f2 --- /dev/null +++ b/Go2Py/unitree_go/msg/dds_/_MotorState_.py @@ -0,0 +1,37 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: Go2Py.unitree_go.msg.dds_ + IDL file: MotorState_.idl + +""" + +from enum import auto +from typing import TYPE_CHECKING, Optional +from dataclasses import dataclass + +import cyclonedds.idl as idl +import cyclonedds.idl.annotations as annotate +import cyclonedds.idl.types as types + +# root module import for resolving types +import Go2Py.unitree_go + + +@dataclass +@annotate.final +@annotate.autoid("sequential") +class MotorState_(idl.IdlStruct, typename="Go2Py.unitree_go.msg.dds_.MotorState_"): + mode: types.uint8 + q: types.float32 + dq: types.float32 + ddq: types.float32 + tau_est: types.float32 + q_raw: types.float32 + dq_raw: types.float32 + ddq_raw: types.float32 + temperature: types.uint8 + lost: types.uint32 + reserve: types.array[types.uint32, 2] + + diff --git a/Go2Py/unitree_go/msg/dds_/__init__.py b/Go2Py/unitree_go/msg/dds_/__init__.py new file mode 100644 index 0000000..79464f9 --- /dev/null +++ b/Go2Py/unitree_go/msg/dds_/__init__.py @@ -0,0 +1,13 @@ +""" + Generated by Eclipse Cyclone DDS idlc Python Backend + Cyclone DDS IDL version: v0.11.0 + Module: Go2Py.unitree_go.msg.dds_ + +""" + +from ._BmsState_ import BmsState_ +from ._Go2pyLowCmd import Go2pyLowCmd_ +from ._IMUState_ import IMUState_ +from ._LowState_ import LowState_ +from ._MotorState_ import MotorState_ +__all__ = ["BmsState_", "Go2pyLowCmd_", "IMUState_", "LowState_", "MotorState_", ] diff --git a/dds_test.ipynb b/dds_test.ipynb new file mode 100644 index 0000000..09020b5 --- /dev/null +++ b/dds_test.ipynb @@ -0,0 +1,384 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from cyclonedds.domain import DomainParticipant\n", + "from Go2Py.unitree_go.msg.dds_ import Go2pyLowCmd_\n", + "from cyclonedds.topic import Topic\n", + "from cyclonedds.pub import DataWriter" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "participant = DomainParticipant()\n", + "topic = Topic(participant, \"rt/go2/lowcmd\", Go2pyLowCmd_)\n", + "writer = DataWriter(participant, topic)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import numpy as np\n", + "msg = Go2pyLowCmd_(np.zeros(12),\n", + "np.zeros(12),np.zeros(12),np.zeros(12),np.zeros(12))" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import time\n", + "while True:\n", + " writer.write(msg)\n", + " time.sleep(0.001)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from Go2Py.std_msgs.msg.dds_ import String_\n", + "participant = DomainParticipant()\n", + "topic = Topic(participant, \"rt/test\", String_)\n", + "writer = DataWriter(participant, topic)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "msg = String_(\"Hello\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import time\n", + "while True:\n", + " writer.write(msg)\n", + " time.sleep(0.001)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from cyclonedds.domain import DomainParticipant\n", + "from cyclonedds.topic import Topic\n", + "from cyclonedds.sub import DataReader\n", + "from cyclonedds.util import duration\n", + "from Go2Py.unitree_go.msg.dds_ import LowState_" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "participant = DomainParticipant()\n", + "topic = Topic(participant, 'rt/lowstate', LowState_)\n", + "reader = DataReader(participant, topic)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "for msg in reader.take_iter(timeout=duration(milliseconds=100.)):\n", + " print(msg)" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "reader.take(timeout=duration(milliseconds=100.))" + ] + }, + { + "cell_type": "code", + "execution_count": 1, + "metadata": {}, + "outputs": [], + "source": [ + "import struct\n", + "import threading\n", + "import time\n", + "import numpy as np\n", + "import numpy.linalg as LA\n", + "from scipy.spatial.transform import Rotation as R\n", + "\n", + "from cyclonedds.domain import DomainParticipant\n", + "from Go2Py.unitree_go.msg.dds_ import Go2pyLowCmd_\n", + "from cyclonedds.topic import Topic\n", + "from cyclonedds.pub import DataWriter\n", + "\n", + "from cyclonedds.domain import DomainParticipant\n", + "from cyclonedds.topic import Topic\n", + "from cyclonedds.sub import DataReader\n", + "from cyclonedds.util import duration\n", + "from Go2Py.unitree_go.msg.dds_ import LowState_\n", + "from threading import Thread" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [], + "source": [ + "class GO2Real():\n", + " def __init__(\n", + " self,\n", + " mode = 'lowlevel', # 'highlevel' or 'lowlevel'\n", + " vx_max=0.5,\n", + " vy_max=0.4,\n", + " ωz_max=0.5,\n", + " ):\n", + " assert mode in ['highlevel', 'lowlevel'], \"mode should be either 'highlevel' or 'lowlevel'\"\n", + " self.mode = mode\n", + " if self.mode == 'highlevel':\n", + " raise NotImplementedError('DDS interface for the highlevel commands is not implemented yet. Please use our ROS2 interface.')\n", + "\n", + " self.highcmd_topic_name = \"rt/go2/twist_cmd\"\n", + " self.lowcmd_topic_name = \"rt/go2/lowcmd\"\n", + " self.lowstate_topic_name = \"rt/lowstate\"\n", + "\n", + " self.participant = DomainParticipant()\n", + " self.lowstate_topic = Topic(self.participant, self.lowstate_topic_name, LowState_)\n", + " self.lowstate_reader = DataReader(self.participant, self.lowstate_topic)\n", + " \n", + " self.lowcmd_topic = Topic(self.participant, self.lowcmd_topic_name, Go2pyLowCmd_)\n", + " self.lowcmd_writer = DataWriter(self.participant, self.lowcmd_topic)\n", + "\n", + " self.state = None\n", + " self.setCommands = {'lowlevel':self.setCommandsLow}[self.mode]\n", + " self.lowstate_thread = Thread(target = self.lowstate_update)\n", + " self.running = True\n", + " self.lowstate_thread.start()\n", + "\n", + " def lowstate_update(self):\n", + " \"\"\"\n", + " Retrieve the state of the robot\n", + " \"\"\"\n", + " while self.running:\n", + " for msg in self.lowstate_reader.take_iter(timeout=duration(milliseconds=100.)):\n", + " self.state = msg\n", + "\n", + " def getIMU(self):\n", + " accel = self.state.imu_state.accelerometer\n", + " gyro = self.state.imu_state.gyroscope\n", + " quat = self.state.imu_state.quaternion\n", + " rpy = self.state.imu_state.rpy\n", + " temp = self.state.imu_state.temperature\n", + " return {'accel':accel, 'gyro':gyro, 'quat':quat, \"rpy\":rpy, 'temp':temp}\n", + "\n", + " def getFootContacts(self):\n", + " \"\"\"Returns the raw foot contact forces\"\"\"\n", + " footContacts = self.state.foot_force \n", + " return np.array(footContacts)\n", + "\n", + " def getJointStates(self):\n", + " \"\"\"Returns the joint angles (q) and velocities (dq) of the robot\"\"\"\n", + " motor_state = np.array([[self.state.motor_state[i].q,\n", + " robot.state.motor_state[i].dq,\n", + " robot.state.motor_state[i].ddq,\n", + " robot.state.motor_state[i].tau_est,\n", + " robot.state.motor_state[i].temperature] for i in range(12)])\n", + " return {'q':motor_state[:,0], \n", + " 'dq':motor_state[:,1],\n", + " 'ddq':motor_state[:,2],\n", + " 'tau_est':motor_state[:,3],\n", + " 'temperature':motor_state[:,4]}\n", + "\n", + " def getRemoteState(self):\n", + " \"\"\"A method to get the state of the wireless remote control. \n", + " Returns a xRockerBtn object: \n", + " - head: [head1, head2]\n", + " - keySwitch: xKeySwitch object\n", + " - lx: float\n", + " - rx: float\n", + " - ry: float\n", + " - L2: float\n", + " - ly: float\n", + " \"\"\"\n", + " wirelessRemote = self.state.wireless_remote[:24]\n", + " binary_data = bytes(wirelessRemote)\n", + "\n", + " format_str = \"<2BH5f\"\n", + " data = struct.unpack(format_str, binary_data)\n", + "\n", + " head = list(data[:2])\n", + " lx = data[3]\n", + " rx = data[4]\n", + " ry = data[5]\n", + " L2 = data[6]\n", + " ly = data[7]\n", + "\n", + " _btn = bin(data[2])[2:].zfill(16)\n", + " btn = [int(char) for char in _btn]\n", + " btn.reverse()\n", + "\n", + " keySwitch = xKeySwitch(*btn)\n", + " rockerBtn = xRockerBtn(head, keySwitch, lx, rx, ry, L2, ly)\n", + " return rockerBtn\n", + "\n", + " def getCommandFromRemote(self):\n", + " \"\"\"Do not use directly for control!!!\"\"\"\n", + " rockerBtn = self.getRemoteState()\n", + "\n", + " lx = rockerBtn.lx\n", + " ly = rockerBtn.ly\n", + " rx = rockerBtn.rx\n", + "\n", + " v_x = ly * self.vx_max\n", + " v_y = lx * self.vy_max\n", + " ω = rx * self.ωz_max\n", + " \n", + " return v_x, v_y, ω\n", + "\n", + " def getBatteryState(self):\n", + " \"\"\"Returns the battery percentage of the robot\"\"\"\n", + " batteryState = self.state.bms_state\n", + " return batteryState.soc\n", + "\n", + " def setCommandsHigh(self, v_x, v_y, ω_z, bodyHeight=0.0, footRaiseHeight=0.0, mode=2):\n", + " self.cmd_watchdog_timer = time.time()\n", + " _v_x, _v_y, _ω_z = self.clip_velocity(v_x, v_y, ω_z)\n", + " self.highcmd.header.stamp = self.get_clock().now().to_msg()\n", + " self.highcmd.header.frame_id = \"base_link\"\n", + " self.highcmd.twist.linear.x = _v_x\n", + " self.highcmd.twist.linear.y = _v_y\n", + " self.highcmd.twist.angular.z = _ω_z\n", + " self.highcmd_publisher.publish(self.highcmd)\n", + "\n", + " def setCommandsLow(self, q, dq, kp, kd, tau_ff):\n", + " assert q.size == dq.size == kp.size == kd.size == tau_ff.size == 12, \"q, dq, kp, kd, tau_ff should have size 12\"\n", + " lowcmd = Go2pyLowCmd_(\n", + " q,\n", + " dq, \n", + " kp,\n", + " kd,\n", + " tau_ff\n", + " )\n", + " self.lowcmd_writer.write(lowcmd)\n", + "\n", + " def close(self):\n", + " self.running = False\n", + "\n", + " def check_calf_collision(self, q):\n", + " self.pin_robot.update(q)\n", + " in_collision = self.pin_robot.check_calf_collision(q)\n", + " return in_collision\n", + "\n", + " def clip_velocity(self, v_x, v_y, ω_z):\n", + " _v = np.array([[v_x], [v_y]])\n", + " _scale = np.sqrt(_v.T @ self.P_v_max @ _v)[0, 0]\n", + "\n", + " if _scale > 1.0:\n", + " scale = 1.0 / _scale\n", + " else:\n", + " scale = 1.0\n", + "\n", + " return scale * v_x, scale * v_y, np.clip(ω_z, self.ωz_min, self.ωz_max)" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [], + "source": [ + "robot = GO2Real()" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "64" + ] + }, + "execution_count": 12, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "robot.getBatteryState()" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "BmsState_(version_high=1, version_low=9, status=8, soc=64, current=-1716, cycle=9, bq_ntc=b'\\x1e\\x1c', mcu_ntc=b' \\x1f', cell_vol=[3843, 3848, 3849, 3849, 3848, 3846, 3845, 3839, 0, 0, 0, 0, 0, 0, 0])" + ] + }, + "execution_count": 8, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "robot.state.bms_state" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/Go2Py/idl/Imu.idl b/deploy/dds_bridge/idl/Imu.idl similarity index 100% rename from Go2Py/idl/Imu.idl rename to deploy/dds_bridge/idl/Imu.idl diff --git a/Go2Py/idl/LowCmd.idl b/deploy/dds_bridge/idl/LowCmd.idl similarity index 100% rename from Go2Py/idl/LowCmd.idl rename to deploy/dds_bridge/idl/LowCmd.idl diff --git a/Go2Py/idl/LowState.idl b/deploy/dds_bridge/idl/LowState.idl similarity index 100% rename from Go2Py/idl/LowState.idl rename to deploy/dds_bridge/idl/LowState.idl diff --git a/draft.ipynb b/draft.ipynb deleted file mode 100644 index e69de29..0000000 diff --git a/msg_sources/idls/BmsState_.idl b/msg_sources/idls/BmsState_.idl new file mode 100644 index 0000000..8f2c7a0 --- /dev/null +++ b/msg_sources/idls/BmsState_.idl @@ -0,0 +1,51 @@ +// generated from rosidl_generator_dds_idl/resource/idl.idl.em +// with input from unitree_go:msg/BmsState.idl +// generated code does not contain a copyright notice + +#ifndef __unitree_go__msg__bms_state__idl__ +#define __unitree_go__msg__bms_state__idl__ +module Go2Py { +module unitree_go { + +module msg { + +module dds_ { + + +struct BmsState_ { +octet version_high; //电池版本 +octet version_low; //电池版本 + +// 0：SAFE,（未开启电池） +// 1：WAKE_UP,（唤醒事件） + +// 6：PRECHG, （电池预冲电中） +// 7：CHG, （电池正常充电中） +// 8：DCHG, （电池正常放电中） +// 9：SELF_DCHG, （电池自放电中） + +// 11：ALARM, （电池存在警告） +// 12：RESET_ALARM, （等待按键复位警告中） +// 13：AUTO_RECOVERY （复位中） +octet status; //电池状态信息。 + +octet soc; //电池电量信息：（类型：uint8_t）(范围1% - 100%) +long current; //充放电信息：（正：代表充电，负代表放电）可按照实际数值显示 +unsigned short cycle; //充电循环次数 +octet bq_ntc[2]; //电池内部两个NTC的温度（int8_t）（范围：-100 - 150）。 0- BAT1; 1- BAT2 + +octet mcu_ntc[2]; //电池NTC数组：0 - RES，1 - MOS （int8_t）（范围：-100 - 150）。 + +unsigned short cell_vol[15]; //电池内部15节电池的电压。 + + +}; + + +}; // module dds_ + +}; // module msg + +}; // module unitree_go +}; +#endif // __unitree_go__msg__bms_state__idl__ \ No newline at end of file diff --git a/msg_sources/idls/Go2pyLowCmd.idl b/msg_sources/idls/Go2pyLowCmd.idl new file mode 100644 index 0000000..0c1d386 --- /dev/null +++ b/msg_sources/idls/Go2pyLowCmd.idl @@ -0,0 +1,13 @@ +module unitree_go { + module msg { + module dds_{ + @final struct Go2pyLowCmd_ { + float q[12]; + float dq[12]; + float kp[12]; + float kd[12]; + float tau[12]; + }; + }; + }; +}; diff --git a/msg_sources/idls/IMUState_.idl b/msg_sources/idls/IMUState_.idl new file mode 100644 index 0000000..1e456f7 --- /dev/null +++ b/msg_sources/idls/IMUState_.idl @@ -0,0 +1,36 @@ +// generated from rosidl_generator_dds_idl/resource/idl.idl.em +// with input from unitree_go:msg/IMUState.idl +// generated code does not contain a copyright notice + +#ifndef __unitree_go__msg__imu_state__idl__ +#define __unitree_go__msg__imu_state__idl__ +module Go2Py { + +module unitree_go { + +module msg { + +module dds_ { + + +struct IMUState_ { +float quaternion[4]; //四元数数据 + +float gyroscope[3]; //角速度信息（0 -> x ,0 -> y ,0 -> z） + +float accelerometer[3]; //加速度信息（0 -> x ,0 -> y ,0 -> z） + +float rpy[3]; //欧拉角信息：默认为弧度值（可按照实际情况改为角度值），可按照实际数值显示（弧度值范围：-7 - +7，显示3位小数）。（数组：0-roll（翻滚角），1-pitch（俯仰角），2-yaw（偏航角））。 + +octet temperature; //IMU 温度信息（摄氏度）。 + +}; + + +}; // module dds_ + +}; // module msg + +}; // module unitree_go +}; +#endif // __unitree_go__msg__imu_state__idl__ \ No newline at end of file diff --git a/msg_sources/idls/LowState_.idl b/msg_sources/idls/LowState_.idl new file mode 100644 index 0000000..df04591 --- /dev/null +++ b/msg_sources/idls/LowState_.idl @@ -0,0 +1,74 @@ +// generated from rosidl_generator_dds_idl/resource/idl.idl.em +// with input from unitree_go:msg/LowState.idl +// generated code does not contain a copyright notice +#include "BmsState_.idl" +#include "IMUState_.idl" +#include "MotorState_.idl" + +#ifndef __unitree_go__msg__low_state__idl__ +#define __unitree_go__msg__low_state__idl__ + +module Go2Py { + +module unitree_go { + +module msg { + +module dds_ { + + +struct LowState_ { +octet head[2]; //帧头，数据校验用（0xFE,0xEF）。 + +octet level_flag; //沿用的，但是目前不用。 +octet frame_reserve; //沿用的，但是目前不用。 +unsigned long sn[2]; //已经改为文件存储形式，目前没用。 +unsigned long version[2]; //沿用的，但是目前不用。 +unsigned short bandwidth; //沿用的，但是目前不用。。 + +unitree_go::msg::dds_::IMUState_ imu_state; //IMU数据信息。 + +// FR_0 -> 0 , FR_1 -> 1 , FR_2 -> 2 电机顺序，目前只用12电机，后面保留。 +// FL_0 -> 3 , FL_1 -> 4 , FL_2 -> 5 +// RR_0 -> 6 , RR_1 -> 7 , RR_2 -> 8 +// RL_0 -> 9 , RL_1 -> 10 , RL_2 -> 11 +unitree_go::msg::dds_::MotorState_ motor_state[20]; //电机总数据。 +unitree_go::msg::dds_::BmsState_ bms_state; //电池总数据。 + +short foot_force[4]; //足端力（范围0-4095），可按照实际数值显示。（数组：0-FR，1-FL，2-RR, 3-RL） +short foot_force_est[4]; //沿用的，但是目前不用。 + +unsigned long tick; //1ms计时用，按照1ms递增。 +octet wireless_remote[40]; //遥控器原始数据。 + +//&0x80 - 电机 超时标志 1-超时 0-正常 +//&0x40 - 小Mcu 超时标志 1-超时 0-正常 +//&0x20 - 遥控器 超时标志 1-超时 0-正常 +//&0x10 - 电池 超时标志 1-超时 0-正常 + +//&0x04 - 自动充电 自动充电状态标志 1-不充电 0-充电 +//&0x02 - 板载电流错误标志 错误标志 1-板载电流异常 0-正常 +//&0x01 - 运控命令超时 超时标志 1-超时 0-正常 +octet bit_flag; //各个组件状态显示 + +float adc_reel; //卷线器电流（范围：0 - 3A）。 +octet temperature_ntc1; //主板中心温度值（范围：-20 - 100℃）。 +octet temperature_ntc2; //自动充电温度（范围：-20 - 100℃）。 +float power_v; //此电压值为主板电压 -> 电池电压 。 +float power_a; //此电流值为主板电流值 -> 电机电流。 + +unsigned short fan_frequency[4]; //风扇转速（目前可按照实际数值显示0-10000）。（0-左后转速 , 1-右后转速，2-前转速，单位转/分钟）（堵转检测：3-&0x01：左后堵转 , &0x02：右后堵转，&0x04：前堵转） + +unsigned long reserve; //保留位。 +unsigned long crc; //数据CRC校验用。 + +}; + + +}; // module dds_ + +}; // module msg + +}; // module unitree_go +}; +#endif // __unitree_go__msg__low_state__idl__ \ No newline at end of file diff --git a/msg_sources/idls/MotorState_.idl b/msg_sources/idls/MotorState_.idl new file mode 100644 index 0000000..478aa2b --- /dev/null +++ b/msg_sources/idls/MotorState_.idl @@ -0,0 +1,40 @@ +// generated from rosidl_generator_dds_idl/resource/idl.idl.em +// with input from unitree_go:msg/MotorState.idl +// generated code does not contain a copyright notice + +#ifndef __unitree_go__msg__motor_state__idl__ +#define __unitree_go__msg__motor_state__idl__ +module Go2Py { + +module unitree_go { + +module msg { + +module dds_ { + + +struct MotorState_ { +octet mode; //电机控制模式（Foc模式（工作模式）-> 0x01 ，stop模式（待机模式）-> 0x00。） +float q; //关机反馈位置信息：默认为弧度值（可按照实际情况改为角度值），可按照实际数值显示（弧度值范围：-7 - +7，显示3位小数）。 +float dq; //关节反馈速度 +float ddq; //关节反馈加速度 +float tau_est; //关节反馈力矩 + +float q_raw; //沿用的，但是目前不用。 +float dq_raw; //沿用的，但是目前不用。 +float ddq_raw; //沿用的，但是目前不用。 +octet temperature; //电机温度信息：类型：int8_t ，可按照实际数值显示（范围：-100 - 150）。 +unsigned long lost; //电机丢包信息：可按照实际数值显示（范围：0-9999999999）。 +unsigned long reserve[2]; //当前电机通信频率+电机错误标志位：（数组：0-电机错误标志位（范围：0-255，可按照实际数值显示），1-当前电机通信频率（范围：0-800，可按照实际数值显示）） + + +}; + + +}; // module dds_ + +}; // module msg + +}; // module unitree_go +}; +#endif // __unitree_go__msg__motor_state__idl__ \ No newline at end of file diff --git a/msg_sources/make_msgs.sh b/msg_sources/make_msgs.sh new file mode 100755 index 0000000..6a98a71 --- /dev/null +++ b/msg_sources/make_msgs.sh @@ -0,0 +1,29 @@ +#!/bin/bash + +GREEN='\033[0;32m' +NC='\033[0m' # No Color + +echo -e "${GREEN} Starting DDS type generation...${NC}" +# Clean +# rm -r Go2Py/go2py_msgs/dds_ +# rm -r ../Go2Py/unitree_go + +# Make +for file in idls/*.idl +do + echo "Processing $file file..." + idlc -l py $file +done +mv Go2Py/unitree_go ../Go2Py +# rm -r Go2Py +# mkdir msgs/cpp +# cd msgs/cpp +# for file in ../../idl/*.idl +# do +# echo "Processing $file file..." +# idlc -l ../../idl/libcycloneddsidlcxx.so.0.10.2 $file +# done +# cd ../.. +# # rm -r ../cpp_bridge/include/go2py +# # mv msgs/cpp ../cpp_bridge/include/go2py +# echo -e "${GREEN} Done with DDS type generation${NC}" \ No newline at end of file diff --git a/setup.py b/setup.py index fc1f76c..18de81e 100644 --- a/setup.py +++ b/setup.py @@ -1,3 +1,18 @@ -from setuptools import setup +# from setuptools import setup -setup() \ No newline at end of file +# setup() + +from setuptools import find_packages +from distutils.core import setup + +setup( + name='Go2Py', + version='1.0.0', + author='Gabriel Margolis', + license="BSD-3-Clause", + packages=find_packages(), + author_email='gmargo@mit.edu', + description='Toolkit for deployment of sim-to-real RL on the Unitree Go1.', + install_requires=[ + ] +) \ No newline at end of file