From 4530458e938042325f9bd303ef74aa964369dec5 Mon Sep 17 00:00:00 2001 From: Nathan DuPont Date: Thu, 4 Apr 2024 22:56:18 -0700 Subject: [PATCH] Moving robot code to new directory, adding linalg logic --- .github/workflows/ci.yaml | 16 ++ BUILD.bazel | 6 +- MODULE.bazel.lock | 4 +- requirements.in | 1 + requirements_lock.txt | 4 +- src/api/assets/BUILD.bazel | 1 + src/api/assets/bloop.mp3 | Bin 0 -> 8301 bytes src/api/assets/bong.mp3 | Bin 0 -> 31599 bytes src/api/v1/api.py | 4 +- src/api/v1/robot.py | 51 +++- src/api/v2/api.py | 63 ++++- src/robot/v1/BUILD.bazel | 18 ++ src/robot/v1/config.py | 24 ++ src/robot/v1/robot.py | 325 ++++++++++++++++++++++++++ src/robot/v1/structs.py | 30 +++ src/robot/v1/structs_test.py | 70 ++++++ src/robot/{v1 => v2}/README.md | 0 src/robot/{v1 => v2}/__init__.py | 0 src/robot/{v1 => v2}/publisher.py | 0 src/robot/{v1 => v2}/robot_control.py | 0 src/robot/{v1 => v2}/subscriber.py | 0 src/util/BUILD.bazel | 15 ++ src/util/linalg.py | 61 +++++ src/util/linalg_test.py | 229 ++++++++++++++++++ tools/lint/BUILD.bazel | 2 + 25 files changed, 896 insertions(+), 28 deletions(-) create mode 100644 src/api/assets/BUILD.bazel create mode 100644 src/api/assets/bloop.mp3 create mode 100644 src/api/assets/bong.mp3 create mode 100644 src/robot/v1/BUILD.bazel create mode 100644 src/robot/v1/config.py create mode 100644 src/robot/v1/robot.py create mode 100644 src/robot/v1/structs.py create mode 100644 src/robot/v1/structs_test.py rename src/robot/{v1 => v2}/README.md (100%) rename src/robot/{v1 => v2}/__init__.py (100%) rename src/robot/{v1 => v2}/publisher.py (100%) rename src/robot/{v1 => v2}/robot_control.py (100%) rename src/robot/{v1 => v2}/subscriber.py (100%) create mode 100644 src/util/BUILD.bazel create mode 100644 src/util/linalg.py create mode 100644 src/util/linalg_test.py diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index dd12acf..e2d4aa3 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -38,3 +38,19 @@ jobs: echo "$formatted_files" exit 1 fi + test: + runs-on: ubuntu-latest + steps: + - name: ๐Ÿš€ Checkout + uses: actions/checkout@v4 + - name: ๐ŸŒฟ Setup Bazel + uses: bazel-contrib/setup-bazel@0.8.0 + with: + bazelisk-cache: true + disk-cache: ${{ github.workflow }} + repository-cache: true + bazelrc: | + build --color=yes + build --show_timestamps + - name: ๐Ÿงช Run Tests + run: bazel test //src/... diff --git a/BUILD.bazel b/BUILD.bazel index a7c10f9..9a323e1 100644 --- a/BUILD.bazel +++ b/BUILD.bazel @@ -12,15 +12,15 @@ compile_pip_requirements( # Set up commands to build all images in the project command( - name = "api_v1_img", - command = "//src/api/v1:tarball", + name = "api_v2_img", + command = "//src/api/v2:tarball", visibility = ["//visibility:public"], ) multirun( name = "build_all_imgs", commands = [ - ":api_v1_img", + ":api_v2_img", ], jobs = 0, ) diff --git a/MODULE.bazel.lock b/MODULE.bazel.lock index 6c6c56b..c463acd 100644 --- a/MODULE.bazel.lock +++ b/MODULE.bazel.lock @@ -1,6 +1,6 @@ { "lockFileVersion": 6, - "moduleFileHash": "4fa47c1d36ebfbfef45d2813f94b4ba9f3e8ebfef3d74978a732860e3c6b9f37", + "moduleFileHash": "9116b2d8fb0ce39849813bdeb4be5ffcf1ad8eabf88914f56b198ba6b8ec515d", "flags": { "cmdRegistries": [ "https://bcr.bazel.build/" @@ -3973,7 +3973,7 @@ "os:linux,arch:amd64": { "bzlTransitiveDigest": "O6takntg2fy8btRIFrv9hsGqQu89+vUPw5N/OGj+EuI=", "recordedFileInputs": { - "@@//requirements_lock.txt": "76f9aa2120c2d28e6160be8d9a73b3a2510209fd5f4f20e659b0aca1fc0e7d4e" + "@@//requirements_lock.txt": "7f619226d8d5919afa9c944dbbecb48c08251f8ce7838b5c0d2a87db13e7d81f" }, "recordedDirentsInputs": {}, "envVariables": {}, diff --git a/requirements.in b/requirements.in index 061757c..7eec5ea 100644 --- a/requirements.in +++ b/requirements.in @@ -1,6 +1,7 @@ asyncio fastapi jinja2 +numpy opencv-python uvicorn[standard] websockets \ No newline at end of file diff --git a/requirements_lock.txt b/requirements_lock.txt index 221d807..2f1ffd1 100644 --- a/requirements_lock.txt +++ b/requirements_lock.txt @@ -177,7 +177,9 @@ numpy==1.26.4 \ --hash=sha256:edd8b5fe47dab091176d21bb6de568acdd906d1887a4584a15a9a96a1dca06ef \ --hash=sha256:f870204a840a60da0b12273ef34f7051e98c3b5961b61b0c2c1be6dfd64fbcd3 \ --hash=sha256:ffa75af20b44f8dba823498024771d5ac50620e6915abac414251bd971b4529f - # via opencv-python + # via + # -r requirements.in + # opencv-python opencv-python==4.9.0.80 \ --hash=sha256:1a9f0e6267de3a1a1db0c54213d022c7c8b5b9ca4b580e80bdc58516c922c9e1 \ --hash=sha256:3f16f08e02b2a2da44259c7cc712e779eff1dd8b55fdb0323e8cab09548086c0 \ diff --git a/src/api/assets/BUILD.bazel b/src/api/assets/BUILD.bazel new file mode 100644 index 0000000..45a3aaf --- /dev/null +++ b/src/api/assets/BUILD.bazel @@ -0,0 +1 @@ +# TODO implement to make assets publicly available diff --git a/src/api/assets/bloop.mp3 b/src/api/assets/bloop.mp3 new file mode 100644 index 0000000000000000000000000000000000000000..4e8f14a08077ce0f4384b3cd6b924a210f7d7264 GIT binary patch literal 8301 zcmeI1Ra8{ryZ`qv!!R@q-NO(Lh;-)wLw8Au(jh4!EkjCogGwVUpr{Da-3UmDiqZm7 zf*{O+-~Z-Z{MWfTx94-QW4&wd_3Y37u6IA{-5MAX2yg?3v5}F=^`0C6z}hyC9mOR0 zB}Mp!1O@*y`u{e#3*-NG_WwWB@No3J9=W~^Kmz~>G(dR`%MCm?h~1!gjm{0`H#pqz z=$gAF+YWH3zT)kK0RYki7MMT;MEMVq2^FI$lK+}_a7y(5>qn!7I(`g%?xc$j z+bOY<2;jSv%&a*_>Vw$cF-+nq1pu+oy&zK`9vHPHbrqdFK0X-xdtmzcV997sy!Xo2 zOSq@rr(k8C<$L#-65@%B-eHMJ6dyebW>;?B#YHd>`W(|)UMfbvz@hg4`r{N86-X4% zxKCu(6yl~~%tc<-y5yOXZ2C4&0@q4jw4F*o$qp`SPlnFZD5PGk>&q#F@}0Ezg5xMO zNES-~8$SRsnKXz)FVwOeJM^$1m9oYYOIfsU!zA7={9SHUly z2WBi!3lR~K!UFE37FHE-2;cbU1i{H{d(7Bb4z+&;paBkD*kuwi5cPJ5tkI^HBKhpl za%v#^^9P>vJW5=H#zO{yBVSx$q+TBSL7vA>R>Sli;f)@i1MuHGT5~?B3KjQ|ssfw~ zrK5z&*VUgJsC)`*WF*!gBKJ2DVDz49u%xQFyL2PVUgf-RJmlmrDMdCoRga#M;f3nR zlL4n<{P8{rq(X<)$VqS=GH#sz;Bv+_P0OBLGqzaWQm~$0{$)N252OXsd}yxeKmeVy zeXh2kQ<&eD5Vk)%XSh)+&i5o+JvPc#D2J~w4FL{Qho+BkejLdV=8e9p|J%FUpb`eboLq83|!BtZE6YEAt_G01HlMjw`WZU$d9V5*TAV ztzNrO4Kw245BwGOb$nttC)yo9mQz9vCPhHh@iz|{P5G?wsbJng=}e4Xf^eq%>POoH znYpDbxtaK*KQjKpLvu?Z5*z8PqwBrB@mCdk<&}DWmJSbZ{9_WP{p^%F*_pkywAIlN z$1{zCBp47ILWmG5#$4zBP~P?7x@G!yF{Gpx623Ye`_WEr&blEt;S)uy3XhyD4$mO} z-l_Gk;8U$^I}@&xxxq6DiHSRtC2-4ijD`ZfQj2%OW$ZX6rDct6`Ep3bcicI++Imom zNSsDk-y>1B7sqZ&`+#Q@jt1=`VS_esLRR>N04AjQnd6;Ae3Do`KK`~ZzJ2&8^<|&b zK29bFNvY}8Uc;~wS{An4F?y-sF9gf0(&?9v6xM=KzqTZ-d{esnBJI48GhAr;jQIqsGPtO-tA0o9NZp4pl9lNyG475-x1fVKU*cN><6LADu{EHIYck?JS zcOlP_#PjO+QyD%C4rV@#rcaFOWL)pmJ5;06mZ)gQ`H#v!cB5}j$aDx!QhMZ0^ipOzuJc5LX@d{{hA*dt62KI-YtH%P+Mkl+QgIN z<2U^6c@@eZ0}Y?;!IHxT(HM$U>EX2TP`dHNTRi?_=fi)`Rh>qQAj)#=k13eK}gME2wS?eIYxf3m>s+ z6-{;j01faAE#DI9b_`mwoVfAtix1bm$v?HXinf_61o=_JJPrm#JsRQ?{BBK=EOqqR z>gXqmq3&Gl&R0fp)swM>;S7eMPjW}8@edh_ZWjbRS0mi z57%SIw|u}a?uSqh6S-V^mVv{)5P5Nm!<{z;J$&%X{>b>wXyxIi<>AH?94&!PotTSu z%@dzWcsW1YzQpR@-G1J(+*tig{NhyEZWeAln+KJPkVl65)t5fYBf+d8;lPu*hDw{U z>4QKsp{RQRJ%qyM`a21Jjv{JZ^Le0a+11kZQO<T$+4)o&^(K5jWlJg>H4 zH})OLjpZXJ%yi^C&8>Y@F+`k_H7+!{%NX1|#H@cC&Zr{&Yu(&r;eb|DaLOKodQii& zwEn*LlUa4jw|R91;^v5#E%Ka9LYIEB*cl-SyV1!A<1Txifo_U!o7~@wPzYzV_H_mS z4wJ68XRIJJg6}mL-~{ymzWB|c0Gd*~LcM18nQR|=Um+^^7)1tyn{p|^JyILAq&7LA zMM8_EO#p7^g0N6p0IciyxB}cL{Z4wKZ{o?px}#IxrA-sBG#6iO3L)l6d-8a$d|~>- zI;Vk_mIQnqY`#M`=&%*QMat~Hed=mO<7c~A%Ve+DWZoepQ{Hv2d z)vQ(MLcakzh`$;eu>i%82)#hNPG{fK%GXdB@7|c<5eP_x{rZ1ywe+)T*H^FZltf9JPx&KC$qeVn(y>Mo@=?~99`hUNu2O?2=dhzQ+# zz+RNZZX3V|)4`dHd%0t_7J4GxO}^i<#Ct)#Ip|_~l_kybBRzrs7{CjC(6n%=VQE5)J}lvt_Di(=w?j((Tm=TP+%+eQN*a?^eae30@Lns_qq0JhEk(k9 z(JCadlf?cn6@Xw*%2FeGJ6FY;G74F>+#lf7f0KVA7&y;R*Qko>r$i5L9Wz)gStXFC z!C4L1gjI_^W`NqR%6k>HS{G5k01{IC!t4SGgS|{Ej7s&u^ej&RQ_LOSg}Wplj(?l=kpDLO_LTn7Kv&n3 z`qxDOc>mR$YHIymL~PWAi6WN*>g@#XiZLUR<5|$XHS?$WM~iGugFiG5RHjz!C!pj-m zzEa*uNmE%Nh7lx)9ZWye_Pg%OUHTDrn3;r8>bkIqr-CwX>)yHs5yn{d*PHh0GK(+2 z+MjhSH8p5CdUoNu$cVo^z3zo@yY|j$lM#5~ZA96v9m9?91J>8X`pp+Z&WtfdJ;Vp6 zuYapdFc;%qf7wpwJc9X`knhAJKgLNHL%QK>Zk`>0SLI(~f8SGr?<$XJ9G$EJH1>>W#r)f%d-i&rUPVl$|Qzgv1@-iUdRT&PZ5uBQAZ0hE=w@sTHx2^f!Q^L zK!%F?2$w4L3a8h|;m?#0ObjSZE(lY#`=!hdiK#>~e`zNgbCuXW8G}Tk1_A01yO&KZ z5PmC5p3EbBBW9@@eQej>@dGZ-ojsNm8*&!j0@mC4bR#j2j?jk|X(fS^J_SY5;41#% zDyD1-TfWgG2jQhEM#h=A>Bg7Y2tkl}hq7Z~+@P929GKCWvqGe{f z0=oN`Rfxt+0JIT{U%c?UqB43}vpl+e4Ev!YG_1UFCtzqF*XCdPQzE+gjxnL8O4RD{ z9s&S}G}6#OI~IYoMnPgAMG-c#RQ=+pB-<}KrZW5U>kZ&P`F}E7(>EK1LXn*lr>hi1R43r?pVFIb7b~b z>#ay{O#9}_#ez~5;9{2xE+^;IG`lb1YEf8}(URPsyUsTK8mzK4m6dUbZC=9ZT}KXg zTL1yvlK60U2;ftKE3)5y7cpH{V{#kk+0z~3Bw|Kb_X_Jxe+ z$@RN}Wi)jIAZilkK4(go@BlmdXf~50rg{TvVr z8eQ9n#wC5~A^ki*oU`ot_FYNY_u?_5%l+&|SVVK{z}k0nZ#RiCjt{Z;w`xRJ_UH;a zE;d#EcKqGHl3{x>b)_Ftr{K9^*wTq-Qvp-p+uTFnTeAF~xWPWd6?%4-F z&1(59zt2XeDW)AUYL{WsoI@w8w`~=0Cze*usUW3!h%*-D%*DyhN{HV}$CDqHjK$hl zb=ToC_2nnEL&@keIA)u!SkD!T000KKAX~a!{gPC#1V* zWL#we)*nTg$P&(cVK&Ds_>;!H(!n6#5#N=?xTFU&o9)X(X758CaYVVYCk9JUNMiAk zX|FAwVUFUNTbD9DPtI?rT>IJ(N@sNw6WkT`Z-BI8$s7)D4&SD^E9fnlOKnC^q=N`y z1YO}60E%&?POXO}bK3NFcM+5YmGQGlR_H!WVxPZ@bHd?FtA_y4#G4jzG!0X03#@xs zYmbiadu$}A`_6stnj{)-krF|Tr9rOcVpcM6Fuh+oxVb;$Ql}L!**Hmj@!abjQ-JU)}npw8l54LuA7h}gQAg!j~ zEUf%OlPO>6fQDvFU+VqaB-g{)m`)7YjejH-Pm@>A70Se^-9s4aq%2D4YkH^10R-YF zFZU%SLD~C`IYa;i?yZXp2v38#SUN04af^nf4OI7jd4o5N?O$S@*?dOiiu}Nna%o?2 zgl18fNOzY{$%;C{$~L0g2ZBysu&ZaRtbU{0BYxKLrU3?WTa;l^2Q4}|^D*E!B6`Ps zCX2rn=D^-1uSfHANy*$oj4B=P|6U#Ci#x!deCO$<;iV*Ac)E>`a=TuaI_tJ zvAch6fVKwlrE96gO&I3gp#vpEEukOO<;H*zq*&jfm2x9KLj=Q5d;Zc^%&lFdnzO-@ zh5h=veef@>?Yb)6_xm2f&eJkHY4@6E3$FW4dXNj(w0bI-p`6;*Mo~Xbm<*BGBDy+U z_j?k4M8-#J0dl>c89_6XylS>E(V1W~tUUfsR$Xx;;s;<>71OE8G*Wx<=55>7?33PV z5}@t;(#|=2e2^h<$X=IJk;SB~Zw1!0qv<`-WiW4hsrs?YlP|}2ZJ0Q^%Ucv0!4-j| zj5RtPQAx8V3+kluG(YSp`Feco_-)N*6dVp7YWu1I2S?>di0P3ltSL7*MmAVpFRgfo zp07lx_04T}hXp!|qKcgD`iX>e5-D;YA%+R72&0d(?w8{aA9T^kEa^6wUe!+mcvALB zMF5-kjemVCo+pjAD}zvxM#9=Wasj>n0olPx%&s7upcyIU1 zycqx6HFKFHb7BQZ!hL@B5s;DALM= zM1W`J8272bhhxF>w4NR#P2;g3Lgc?wvu8p9lzOZGaG}8QP?H;SeGGYKmu=x>``Yhh z?Q~WeKhOCQ?MH`|*9?B1_n9uv8@a+OQwJOJadF=0p3@4CEm^7aK2fcR@Njr(^M1mfFyx1H-4%M zy2}zfP#|j^pZjy(g_ySD6`q9Q*9S8&Ks>X$lMq>zk8*xUv{_&s?zCq}Y1sFk7J)|S zul79RSW4q^SJ8>x&SS`pfkna%`)>R@VDTK7b*}Sotfic%1BSr}b6wf7&Ndo%cz+a! zXT2k$)H1rgFYx*EMw9!lW|B11+q&Ta_sRZ~RG`^{;Bp?-7fwV~EMaQADmX5Rg=8m_6=rC~8P0Z!Mc z2gt7GrUBC4)Bqx&JDQa*J)aYHKvvZ<+Su2gWjnt2@4cQDIN>6E9ojf*l>UYQIr0L8C zTga@Jv9Fz8&sXI527F%-1V=1u003^Lc3kdG&qowPTAo3BRyJ& zfF~6H2Q48<96Q3^2E>(ax0ONaT!6zNS2TCIMC3Yi*;Gck69U}4p>tw8L*A_`H^ z288&xZ;WR8h<#cK`tUO!lLvQgOJyDA_wP8ZWEJ({X;Y;&WKJdQdr=pd2m>WnYF1^F z%oi}}#<@op^4=?Uz>4bw?SCq@bF^(pU9AR~QHc>VQhbAj!6!VunqX` z;H|UcYyb9Hfbf)^sYv#NOuUXV;AFj-` zzV0pvAL)3uEEZ2Q@Z5KgG3sr%a({2JAtx^{qaK)nVj7oZ`?GETE&cgpQ?%%;D2?E6 zh|_i^h!GAD-`O03B~K!G*XgKbB@s>5;4B+u!gP9ar_`ON@9NKO!??8E?)lqWjK;Tq z;Bv8azUMn#xaa>XO-!8EGYofij{9e3D{tfGq@*sm#;XGf8WE-D5IYnBKfwLX3d8N# zH&=(@Hj{^zixrvec<}L|LRpEmF7Z$8?sSxxE9~PMv&@DkPI)pTA|erM%8B%hop3@; zm>y9=9t|A}BqrLfZ1~j_B9%H;eF5>3*usKZl!o4Z=~;P&g{2kM)%8tnZ5`eH{evSD6H{}G ziz^>CH+MfB9v+`vUS8k){{4XX7s=n4t^dZ%`+wsF7*SIH&HammWlMPPf6xEl5pkbl z1E34UX_d=Y!K&;Kr!nNzqGJUE$s>afHIro@v2&gZ%vV zG{68_O~KAW(<@K=>KA{%K-}P`eXCzFkt@O7q;KUf33gUufP!bGLqp?5uNDENzQK`q zp?E{v*VV@{#va0F4`={D+g!BoX7kP5?~!T*K=X)pWXwoh#YvWem6rv5)0qs>nMb6v z#bEn~9Nj;K{VMLe_iO*M7E5&kg^yb|b$L&6AK9MSHhd<#?4W2lvT|3;to^|Jgho_HT9~Qhf=e;i#=AAFEmc)%dVM_i&+!xwEQkUIcW(_B^ z-mzv_ox7IZGNg8mG1vjX$OhAByUD|)K7SaEY&xl$gM!+3rn zMSB&6d{NiQH0QwY3 z?8wLm!SC5(_~46gf#{f3P)e4bEyzrWnc_%e`?*9d`_6Z)W)O$pEAD4)TS48L7SXgSYf1hH+n75~_i)YlusfGX- z*=x@|6|Gid)~b@HD-6AUp{x^;_8p0D((}0wT(LhYKjzUho)*7zpgL3E zK5Dax4Sp|9`qjqU_x}0vVjCZ)wAT;$wFCF^T>I?AyrqSB!48zKwCWY|_#Z_2m&d~F z-^4qA5TX582XM;{aU9?e$)+TjlfbbL?_NbYsN7{NcD)Ne52|^8=fJkT?mE-UKTgx| ztB*t;IVv5c*c%v|nUkpC%I9j9Juj$ojf{f;eVoIwIqg^tl5A{zo8OKc z{t1agA&kyLq2c^q7Hnv|Jfpr^skXRE6?4NrBnLGax!q^JcCgMD{@WY@g(gx`JgN=clYp-$m}4(-n7`W$@hnpx+frwo5xAES&jKInwU0_eFJz68FS2V8xYa zwxYr0WuFEnM;_ToE{-pCjde@bb|B7-;p^r%(2lzae)si8$7V}W8iO}*65xn5Utew( zvvndbm9Jors(zjfk@q=Q3W`P(3sxj@>;=<&!J#vslgBC}zAas` z6@MGb_-9Q?VVCm>HtCt~Wv;~Pvof<8>EUWhX4yE+%EWTJ^Od%%xQ5Itw@H$W^k*IE z-}2B!FHd5IA(#Mt!{HId7k(4bqa-+!~h0ESd9V#^X?%s0JaK3qM8*)o3mYSRkdD^xD(q9^!`WyzTK6 z7w;=60J0n!dmTD)_o&xu^fClAngn#eoyx3jZZ|2Uxa-{cf?5Aq0djZCifJN z{Q8#1-!iTQNs4wm#ijIGoh3#!G#+AP@Y^dc21)N@8lJn9LVRDo)DB7BT6tQ%IZU5K zy3mCaI|6v&0?94!^J;128D!Q*cAwP_506JWXj4SdDY)E|PpVjr^BNzl+HT$D35$CB zY!FtvL-akD{2SEsH*Z>Te*17tvK&iM z8Buy0;*j}HjwIo*L>fzmj@3PHbPLQfwV5})?fnA5U-cFs?Xi@`AE8I5H1QU7wllEo*48hVPVb0tIsiLorkX{KQukJmXjYI z%H5a#vPGSUx=fmfldhA32EZWFk;B1(2C{% z6T*Z;^el}KUN8(ny0efAC@2-Em*uv_hmFTMR}?VXl?{=41GJcMqQa;QylsbcG-@DC zei8Cat#TVWC6WgnIeJv95XA zivX97<~!Bfr-6(~pElO=S|jg5)ia-WyMqQlgVd)t&#XH*dfV4d&NOKBEtIWf6*Jo4 zz%c(tpbl-13hUN<1VOVF*W;|`LDF+GUcehp_%dzi^O(+il;XHiYVv3AzK_TQv;`Ju z@?;>S`DSchQpzEaOo>UIE+{>ewxyh?Ppu}MHMKnN>icE10ql>n&!$4`F`zM4A zg@~{(Q{?gM@_z0!x2Q5p^-Px4bZ8lxjblrUz);48skw5g9FWqzL|#-S7{Ll2E4@PY zV$zOCq;5x_NEt>N=tSjy&u-t9-#Q1CMq6W?Xzbis^mU#8-D7s&eL_uhECp7Xq$n7WN)>{&dMYkf#E??Z({;iI5c za6VcvlP@zka$@MmE5>J{5y;UmNW#J_7THQ8tcQiK_IR;i4?8ljiI0-pRdcNDri8UF zdtEZD(&8fGk%crF67>u@u%C_}WiZsM>4`EIP3W;wNl(4=eV!+}5{Dq>HxTyu_hOP{ zy91IR@yqhYsfA`%efD z3Q@7wPR|n`5h%;slsNE8m84U2ee0{dm2q|xT_83{x7Y(esPp=kjv*~nz#9L8KMQ`- z!bu52FP=ve2F%nm_})F&Ic#Y!xO1QlWa0cONOV*GbR$Ta*Yj$+$G6<{{JmKI68E<% z>LeYmd+&>*v&++}s=|C%l6K8f!+}?<-)egw@}c_xK$rj;J;1ec2AR_wst|X)-R*#~ z%*dh+8`Nww7tr2M#u2dDwEoy@M*JP8gw{~S8gQ8_%XfTL?^5L|V{(C@UY((`1u79^ zCL~bj`w?1THGQKd;_7@OKjOC_m!@gPiDzlwi-$a(d`5)xy~2*Cbbx^G z@fST;+l2V+KU%9YKrA5YHiS760p?&*utAwkV@k5GDfvWCmKEpz{=Ld#7tMn{P444k zcEP*zwfvt}cW0w9jCG)iP1hb`3_DB=hdM02x?b&+FE9^quf_M*HYlVRoIhLs3E?I{ zxcPJ`5~)%ItT81mFQ`&=Ok_1()P^SFoUo#6CtGj@zQ9W@O<&{?Mnbg*fK{1T0QUhR zPn>Gc2EwP|9(a`_kE!%__4{{?hRT9zb=>_KX=c5C(M{RDtuom;zOgV%McOa2HMW=Q zl9Hvmn*Q}Dn|1O8${7Ulxuos0{*^N*b9ElW5n(&6aAYqKK2w4B>obc>&kYG)!K~r< zpn8@?Y2BZpl)}0B_Gp(u_F5G_avwK8HdRDSw`240zWFFKrh$QyBryer63JCT(DSEa zTw3s8@5%HH>Iz&8*6orG_Q6yX(Oo&&OYgBrAv@VP|4HH3Xh#MD&oiEfK2eB|6<76?J|O{pfrMreyU-P-cpx;n@*bol zTS?YQASCp%M@Z$IC0gK*BX=ywXw_)fV;`pY9E|Gl+@l9wyLPkLqTJ<`4qo^5FJCvyhZ^L5^4gZQ_z8~ zs5CVklch)#90iwWrk8pUQAxA9|JqH$-P=m3#*ZRpO*sBN`_mT~+x(mOAQMSzrV%dyr4->9BW8)k=H$Wv%mNGvyZ?lUp%61w z?esp5Apv?d4S{E`QqZ!M-5eHx4>7f~KL?g&XDJz#3&!Bt1kW{pVBGNWkd#5S{Dj6m zK!-+2g~dO``4GQD`cWzCPDluI*D?+EjOu<~X`DY2VJl8GCCH_!taou_MLfP`n!bv! zi44tbRc8!KDq^%;C9K`BY8r5WVnRB2Iw?z`pYRaumMiW5jNz9Vrb!*&)}ut@+{S$DfBjxMfm(G-u6nr?~6* z{X1S{ZnmfkXo61YBRJ2C@2v(`f?RYr)pYl9yP9^&{#u-X7>3m`hlo5lzYWd4j&y`Y z?j0uCrYzK<#8s$J`;z!FT>g8Y-WWs2H79@L%-OX=m0#y!;4$JT!&`X>23)-wjKoZFrMutCPGc=gFItR0izb3GbLmBm0!B$+{3k>XMcRG2 zoS#laD_DV1Cv%FOG*_(dT4D%aPCKo6L+KP1TE2)2le;!iG(&fAHtD(Eek^b$3p zoDkLR3oCwWes?59#iCG0XB1+l9z1_}%5rh>L9+|7S0TMeO*eHn``F)aWMUHE*0t)?alZFKD^1Jo!%R%GB-U+$LTQztMC{-7RX{!yAe3W*yDpc`7)d#nRt;1H) zR~npM>51$%>It(tYnxJv*r=5#byq+wVYOI83C-W@_nwLu5YyOqVk~$6gxKNX!Au)^ zSd^L~Wp%EW7-3lcuk_T<-w$mjI-P{YfzQNu>v8F14~`VuaYUIf8(Ba1R~!+n1${vM z34;A?kac;W@aSD;WW>6&-*nTP-PPA4#8Me@_D$`+>;iqS8hs!$YducLVSUlriNu~* zoW~C)78U?%W7d`|>^lV+61~LVTMJ`FZHhh+U!b#8Ez}RF>RO)6BN07oKsbUc!WHrF zLh%K6xn6F!-QHn-O>ktSLp)~mCgB^ns$=t_d7JmtK{~fh*X*+8`&K)-l1w!PYgX5V zTp1o2(uLzt03*hsH(Np5237!|Fus`m=!@v0Axtt(viRg~$@?R2)s;dkoHsgLC!^>j z4M(d1(Kv_r1=2$^?qO8!t(6G zkFEj8era3Tzdy)pf{BEhcg3+hZWOq%4SV-M{=A$fqn;N`{& zCK0!2S)86VhYc1i+p&SulV=OsDBvP+^fpfeNGz*bHyZI4`bt~x8RnVYqPd37Bam@86C#Y+i( zRu#|nGOz-)tN=l9Ypq8_%5i(xJZ_;)afqh1s(1v$q6buyhu!4%`e!tzXZCqrKG$@c z3xW`pBy3samv1ia^STZ_f{4CDi|!uNSXI69fMln=b!U_RK#Nw0i3}t*ep%nYSHGCDV{bZ^JhOw3-qk3T3W(bpFB$$ zYuMp9jJxxxo__H9wJF%_oEDEl|0Rb#K66Bc6*jAWJq`ut+wc9@vK0jyahqR6of(gC z?9VTQ56?*m$-$V$^g%O#2Mqlu9GW|}yAUewkKc+Gn~T-Hnm^r}h`hxWy0Z48h82tX zb8stm@;uGM_uxYKO$6jqgpvK)DK0vr>ogw7vKrh2wV6ty);FRBgWtem`@^Y*(a&s= zyTtkByA!ZDgj}xeKg;OV1PajE_p0YfcA0(r#d_j~&E8-pdXu~6u%iA?C?EpKpGjX& zl`2IH2_sG#O9Y!TQ}g0&C-4=`Y5Rd^^XmAW4N4@Vet5>X-Sf`zB0iIu)x~rQ$1eSi zCGEM3QLOEu8>>D8MrFZ1j|#pIzSMe`pw#7?PpmW^$q~vcVbhv8iUln3?`+7a=@>R9 zqrxrmJeddme%c@q*O!Qy0UwM;LtQ#=0bLQ&6xNZVRgXG)09ir}PKIlNW)3k9-YLZo zxD>@iCARs9X?dTi{nH^Sc7xd=;`Rz2WjfcOxyle)FL4cL%4aBAa4Mt2yW^?OZ_Ol~ z%$C0uZ_pS&H}>dm?%Y|jW7N{TSLbG*`LngSfCcivblv31rGA0V=n<_wz;QY`Ju_QI zx1BnnDywS2=mVg1MFm-`?1C;E?)Pkbjpx#|(objS?3&rKG)VRvu)zK&l$3x}DzKF9 z$^{oG%d3{gFic$a({w#Pfq%hp@`)}?0RsTl`V)r`$!GFuX7jsS8_%W29wzJ&T>xFg z%?TRpm&&V5xXQ@&F7jUT#huyHpgjU%gy1kmy{&;mikv(~7#c^l(G46XS@E$2B_s|* zXMWPZ#C~1bhE7Uw=S5<58M21UxNfo4S)15$a0e%;s3wJPhF4mHPRTRa0M0G0b07z#wYbj8|O z^Ta5$-NdodKr>9=RXTbvIdqaGS?7hmnD&2N^qzB?c@Gin4MrdyUl_L7R_K<3E8fd9 zv8k3j$IcvrutPynao_WQlRrnZnHnl{(q>Q)cwa^M5{`V`+YBO=Em?Wh&rV5Mjj?6H z5BOKPC0=f7N)BcgGo$lo$z7Ee{~=2q$?loIP#$Dhhzc~t{V`dxn9+t6B`b&2rX%iA zS7^eSwDH=VbsFJH!2#M214@3nhTbts!_y2#q)%FQR=eUCxvAn4Q~wE-Ly?A_>7?hg zd+>*mpYmhqCND1hT@N`9ZQybG#3TmS&QmX8=?o4YBRY>QT}>%7Mu&ZcuN}Wg|2d_} zrdme(F!=Y&<3w;xU1qnQ!O45aju#oh zOMJ&5#NH91UM|>@Yd*UmLEHot-cIf$ z5``E2#(Sj&#)fNsOtBZAU@Xl&gL4b6TUxuLO&%Q0h?joR<(GG-drv0!_64WxPZtIH z7E64{eH|qrk%#9H)i$udq}9%NcICVyzPonniA|}HM5F>%;VFA1je%qkQRH%5SdRQh zFC9@HRsbxd(@#jrV_YySgJc;v|1+TkB7=c>UubD*;pL$;AgbBIdzqyGHIV1?eN%%$ z3VZO$$@M5VJR?i7?<%do4a;5%J&0m_m&U@|@qVoS&A6G8dxPU@kl>Xds|Yhm4L)it zA-89YP8*j6VZ|0-t5SHg!ZV%M@yv(BtW4z;o5sKzsQmXBhfH&gEIdn> znXAB#1HZZg-FAJuZ%H<)ceFLkaqyaJ0PofC5dWzp`Ea;(Y@@}$9C`w+t_7FH%|n+!nZQ_KfoOA{G|f_$0K$h7`VG zC}{9m4l#XShtc7d<=;GS7CmQGE~u%3+21ETA9u58Oy;`B|LR&;J$RVH<+++KfptP} zWwpzQ$*@WE=l*!n?^@DSalPO zIgF5ZE)uR9Gc*j+Tzddwl~zr%1ak3Nzkgz?Gagj~TiKer5=SQ^{xGKe*SJGGg?{3T zOCrF@&Va%&h4F?4OF%N=GIkdi6tv2?NIZD*iJ7RJZ8GHqNM7rnOVXupcuK#LHM!6= zfom8!FQUE?GJ4vji76hFlgxn^;3$6(cLuE7K}avspV5|rw6!tV>rB6+ZO-I}M#>~{ zJcH_Loa+_ZCgr>_muEh(G!d@qVB^>S_&|%R7diyAEk~TH?jcK85^PXH?nS9LR@Ui6 zgq8m!(Ud_FWE51)ooBx0# z2>rr31@0%s?J@_rcK8XTMj6{8@^&6;!Q|@8flMTZXE|xxW+ZJ=0O3lU?hfCqm%+9-PP!Shdt@rnhV-8*IFX0YA=|Dc5D&3c znpzmgm#`4-0UQq@c~`@i{Ps-f)L_w@_@7O>bi%6|tcJzzMc*#pO@oP=tjdRfLoAU1 z)`nR(J1B@GG(1{%B)^}SkqCl}^mIA1ON^ff{Ne+^pxF~5eAH2_=i|ylemf$l07yH( zIzs?ND3YzmRAU|FGA_(ZaLyxkA~M}L^#uNPUZKPGQqM2iCbBBzFr@wGWtJG?RpF~_ zghcP-^+Q6UyI{+@ONK~I%=0U#ZoC07tJnZLITW=Yw^NrjMVa@J|O@D^Y~H@lNtlx zuSj&vK?)iQd_g)N$9ogEb;hGJ);@G_QldME(DCtaXKfiypWhvm4*scYeCNOX`C@IY z``i6;-R-)R#D@qD*Tv>|eG3tq9 zk`wW$M%^bVTR1v+VqIg9vCn>O<`Z@F?``(eyQa?;=?tUUP6aR@S+V&r<+_uY&8t|O ztd8eYKhcS<^PPPj*8R3nOb|;WdmsL<;;98iqo6co+WVkN^Xwx!^&1 z;&!IGYmGO2Ie|-nWlXc>aBWis+25r`Jacc90FM0W!wap$dNE|G1!ub|8k8oiFNh+d zlQQ<)m7X}|x0+UUP0}B}ap!+^Qu>N(sqUo8zM;pxx#A`qxaM3vewW-tlT?U{@+x}E zOQ#XFpe@^$5XMPy+VcJ@lZn8VnLLu+U`?E_wn=&VX~6qgSWY=hRqcD*kee5Ui2J{5 zam1ex1Pp~am4j7@-nm-&R+-R}i(T6QnRBXJ7~GaQ#n?*o`qvK-81h3gVpAnIw;I~Z zjts&d4&V6RBCsw>H~mzIzQ*f~M}9}UjbL=`>__ikKMFbF@+aiO02QQsBK!2&G^?X8 zJdde}ulEmckP=GU&w&h{>2)!tehF4u+#)bM!t^xyYBC=)X`k8{LTHsYA~GpkPk2N;*>YCj`2Q1Hf*{@U=CQfO%PThtP5?gX? zcN93;?WV6wt~GbEuAaOW)3^~ESnUVjbWDQNZ8g}@(3o8^06Js{OmbBJmzkF`-kNjr zV{E_i!hx-j4{p!mR*@%{R1!h90uxc^pMv?omBB!)@&?GEW4FX`c<4h-E31&DPtJ7M zK8iI8hNK>y8tm@q#sXAjui6q8w?92pQ(KHb@<)H1_+yK~H+}u$&SZ^PATw)T>4o#g z$EH}*P#*fdEqJMXog<;YsTe9rAq&V)jy$aDAH&4zm)=owuo&!l@xMh zva742YmFgr2xU8o=op!oTkxt%*xNv%xpBX?wBJzGKoiCGXx#hUJ+2l^Aqu-=3%g_f zp_;8%)cLvQIb=+0(+i#8BQOL2V51c92~zkLZ6>;;+UiCmsVLhI?l&>nPA{q;e~2hE zRc-z4`S!9f;jdJI!)lqrx0EbIb*0%<_mygov@ope$~7L-@4FRkOBhJ%4(e)av?2sN zKSjWh2sp- z>C1WHr$--J+=|f@b%9`V{2`zTMlp)4helRiCzk`y3y<1Dz9BD1i9 zL%Rn3o|hmkDgVSs3|r&dx`|acz9W4dlx(ag(`EW@^q0W*aP7Z#2oX|2SG zD>5v@?cfgVVAL^%7NHzt_!y9`+7XFvHXK1=nr+-=UJmB8K7!U3qntq&%Lpg+_XS(}E>!vW|cJcB7M+Y6}$7Wcqjx-cyT*y%c z^^2W562Ajw4K^^W4ncnbiyy8SO58~$1=e4U%VI*vcw++lhp`* z(r$_IQ6OsyaRr+lsFh-#m3@bHE9^_M1*Pf{DQ8!Ly1AJ4ag^&b?bzvXOpR|n8sGf& zwH5k0(7G0KJ+T2W(7lT;RbX`5Vn3kNX6&zJQN3`Fhi%;muKX&NT`huJ)U*k`a^0A& z+oS9CpimwA#`$-d&mxf6F^gs|OpnTR#b;q_9DV7KHo5epg}*|5oYKNVPAVH0RMGiNttN?dYTkl4X?ZLL2Y*HCUFgjc0S!T&+W6^oW%Cp(zlJ!+{ z&u#HY_~PwEh1*|5IX`w7N!?YF+d90yFXK1-yQf~Pb|QLd>)mtoGN6|%%aQz`13w41 zmMINJ!h^(U(XJPUFT*8}j_^EWVnNSTy4E`UaJGNeNbUZM{M|(u>sU)EDf;i1cX`f; zr(QHPpdFmaU>lnbhFd6eTYcPav4Zo6VDNFiLL#ks5En#>tsqDOhci#*sq2J61#KGQ z+mRHMv@s%qq^*N&!uuTJqESTU=#mR=raU@XTPlHK}9cO#a?Lf1)f4x zEW>T9IdmB3as%(zCMZKrNQpY>s@jx$NmmTi{*S;C9%AGCU*RaW+T~kk|5T)i(mFwhky+G)gGyY9ZN!>q9smOX zOdbsEoPHjpjdaX{#R=W4QU`5p>na(2;5{rqj6qIUH60~0IjR!sy+XT;j91QAN)YDV zR}med!u56=J_8#%_%jXrVqPTui001w7F>LI?!E80d3*>U>helZ*5bj*~vQ2NqfT?RCKZi~0D zgSc1I*K03D$gW!;;V(-}I@sRpsUgrO3=kDLDS9o#96zEmFu(pLBc)x@4UkJ%MuCde zSp!?)MDdi`5RgpX_^K5|VmQs4SqYo>s-5X<%uH3Ii`LXDSit!WhNGnI^P{SWH--ZX z&mt9+;JXF#@S#OpSlx)sVYOFO+^*8NQ|DR&ZCc*DG41cEkwOzq!}ee_C-Pxd^17am z93uan>TN9FR>mnnUNXyF4$36IWD#Qe3k~eQTuPvd)Tk-1cE;dxy))=#=R^z8$sy}$ zGUo7jaArVluQwMl;3{(P*^{KnI8P(6|*o}M_|aa^OieHRcRt>o_|885JD#|E$dwV7U^&dGItGX9ElJq zO ze=z8Ktj5PjL-FYb4W}ss$;4ve1U?meUai4vRll{*jsBd6U$J|bK?gf*;&4#D0y_b{>=E<4&0%iqvR!z2i;KuDOLK>yGhwD{xRGdtiO_e0 z6ts!q?V4WM4RxY>IeaSpo$B|5`cSQ$&dkgUmoBc~0L79i9q*{DEwC%26>zrq?f4Hn zF0!bN#*l>)(mM_n)MG%L=^4FvuB(R^v&@_Qq-ZKHQuKhkCR7Z8p`HOL!u`=J%iih9 zii>MkOX;@hffa*@;O{4V3)igHVjTN95_)`*BCG#|&Y+N&q?(w8%V{#BPGZtbC zVQ#dBF<<`v%0)N$e)vtYGpGM)iCdQK^4qy5Y6-jAoz0+{$@!lqUGd_obvviKcOSO8 z`uk8>+QAY-FaVjk8>C4nfpk62k};B7Hp&ds;jLBC-*TV9`RHubs)4;n*QtBSZTJ)K zG|(AM=-Rylr|{kHZ}-&99k>W7`f~mw4Ce^K-OAxXN5M59|Q{{ zQ4ZXB;qcRv_V!h1jzGV>mn}(p2DWGfNmOcI1f86c&$*Hk_=pL;gJl7QPaSXJ(O)^F z{;g0Dr|Cy7HF>ojHuAyBj7zDTK9XOGo`LrCh%AEKjNw-ZD|(etdgj6GlOpy`on(eT z&nJH=PrMAtnI-h0%_D{43$4Rx(=>p%A+oCH2j{UdR(eVrOg6r5_z8>UF?Mx%yq;-C z%65adl%)*Il=zH6_0Um1*_+@&*ENh2{UDwVwC|<(@J(`1ehn)TN-6E)EKkC1gJp2& zZG4Z#$b8gJTHWVi#TWTnb_qjRv1K)F+(jX>S1-2y2^~WqNcKzO*2H|$R(WEM2fp~3 zg+kot?!YfJN1p;o+%D%Q)kcZS&!wpAI_FK27C1Yp>Spa zk%zS;?-xhin+40+)Z9#t)6!Qkywkj#l7?1u08o^7BPpz2gUp{Mi#$US>pUHTMQjd` znY$?A{UD-qH&tfQyA|th&*esIl&p{}QH3W(Q8*R3)PRZF8BCIu;ZuC{IJqmcmREQo z3Kj}1gbP{(1QA-(#`N4sGEs(1SU?FeAq0k$xJ1$v7#wW(lYw>aEGMkmY?bP`VYtYM zZuXGwLzjK$iI=N}If<`8FPUbA7~;O|x;}6zHUIY3>8SQp1P5E>{}&trjtA<4_bj zlvVr`Zcg zoJ(NBg_*~SC*V~A1K~n};qtr!q`vZDYuw?Ob7;Rep@m(*H2n~5%9Z@efq}$c11Ri+ zq6k>r@mniPy(-b?Ezg7Ep5IMSp26}IM#BKr{~jdSNg!AjSxEoe+7_Z$leD-LOyMdN z(-bfSCNUguDmuol89>6_A8D96W#I?&rIiJC}*;mnXAd5yU!E*;Ph- z_1I)z$F1h9vsQiY^bLgG8kU{iE|hikGUt)Jd)r=}wcND#XR!G(xzz=(b$%2C4>G} z3Q%MCH1ymjE76w-Bc{$9r+lY3L3Twn^|)ce)Yq)Obv;u|yt1ZY2it)0Ojo}uUDs^) z>F~v&!Tfc{#$oThP`PFwS2=U4|0EA=ea-R@FFOS$DlY+lj_)A{u7RAYajr5Z*150y z%~7htc6X30X%eCcWqGZx#AhM<@-YMrQyQ^>_@fxH#u|f77fgD-ZdU0E?XPv=ZAQs zM152Hj-BhjHB>0I9@mW}pwHzwg%D-HU(h#}`*t}p^Sol`#_hS7|ozHvT zOrpZ%x-S^uJ|Ikmfg)xI*H}Plr$T8=43mvQWl~EDOs~n7{<2lOhRirmah=WkElww{ z$WCzmJ1t#qTOKrIp4l-`8$Bp){G|P_gV00TPhk_NIAi(#Qk3`@Sfkf`>qTJv!pRX% z3bkSK+&HD|$(fG8(Fizk0D4&k6jx?5py)*AzqZ_20;J!5F1?@KL$b^?)?N#gVi_u- zN!bZ(VmOjDycOg4yf@L*V`{K>Hf%`a`i%8(j3ZzQC?z*8S2mdQnCw9wXx&eA)-AGn zUs&^b^jS;U$F3d@eO^3)!$~p#>pO@HuLWu6PyfqI)6^A347#HJ$$-~stsix)_-$HJ z3c0P1shO|ep-U3_DoStta;x3312d*6uC)ApocAj%@_5sFQX^;J7)>DY$1@~n_|kP& z%<6`P!;JF&S9J?J&%CtnU{cG?QdKdNpa`6xS2Z0kl74@VjMv=BM5oQ0ye7!a0lAijn)M73p0eacwQX~uyhgIYjIR>gUsFCU+46!y)-Kc*(?4rSokNX`AO{{f9eQ^ z(&y6x%wa+Kq_vhADBdz)(dt?`$6*e19b7Nvur5CL>BV-)Tu$+Xiz1(D74C0 zre3}GE5jEzfAHWZp;$L!Y4#B07!mewpvW^6UY&WsZyT zE3&TrUNIYSje-rV%qO`-24;o>@q13JzqQ{E5agW(Kh7BZwCgQCkNHro-aL4F8;`tP zuZe=A{(!5llTu>N-JiNDkIHA|Kecajf?Fpi+lY0@EsF3cK<{?ql3HyoM-zFT`E@@> zptp{Z9`z!2qs4L3S-@36sbPi6WG(B}yBD!l=4)}XNFT4{Ya5#>3}#+ywjN}v9IGtd zQ@Qbk+8SW(DXbF9phP%nsM)BnX%A1d+HDFFKY&~o3pl6!IZFmXqAo{N%rqWCm{&EK zs~9A^`Pc!)FbmqmohvvBA(>1|%UXYrCAr*6CZQ*Cf~j_ycfMX=Up{i6iK`xX_2a1&mcTMEv7yQoFhV6HD2!g z?QQ=Zk6+e!=)oycP;FuOu05e;$Y(`bqIn9gMsh+OiI(mo=zxg$wpFnq?px z;DL%gqN69@)dFv%#6L}=fMUF9ONh{p{j90))y*5_zz~4&2 zK?69(s29wx@x?sJl2CnJ$nE{~^)h6@`}qe+eKK6ZOV+Top@qc+UA1f*v!0Ci^`~Dm z34U@Py*4mv?~Hd}3(?ek*zL4`6i1vUgxp&dr_p)<3Tn?2Am@>{(!+kWX6;yZB)V!` zk{Ip3g~)#KsH+QI5nAS?bCh^1P7?JpkvFyA_YEWl`CgNY4gPn9_rnf&zN87$uX$5M zxam$;aJqf8*&VFggmF9pu8SrFXW0v%T)Pfrsup zUnN-t&}8)1OD<)e2IkKe3pKqpkyxY8?EIo7x@Ruh`7O5X(bxO@DS2`FtYDSr^Tvtu zGe5gWv)YW!*e!-7xJ7!zpazD{m3H6Ku6>0_>FW~+H_gg{i6V0N>*FLgow5$lOmM;2 zf`k@gC3AD{e^nayGF|}yhZ$p)4c~zihpN~|b?;oC>#$M_HR=U3b4r|*51l)se^+8Zur1yH%4ZwivsQIyqfnEBPQ2X-}T zXATB-I8NAg#s=vJAnEt#r)>hzUrJz?uUOA6&&bQdsJ;0aQyzW&UGl5CB6^biy)`$6 zupg}}`+|&&Q!sOAQ9%)0Q``phpZ^S*m1A&fX8iqAt+vHl#G*EUPk{zdhzVh*R>#15 zQ52UI{#JO3NlQe}5~TDqt%YEM^yr_k6ESRd!jU68o8SLwKn(?5Uh{~GQj;m2&mi|p zEilQxdhf@&S#NMZV7Q!Xx8b6)micXh>um^wSY^|T!Ah>WyEUmy#N(sMi!SoI1Aq{q zcO4=nF&^R66bX2Kxa(EP>z(tZO`Co9S^|4DYq7>0g`U6~2j9pa?oQ*4>Z)6}8Ru@;4a>*f8IR}(jBfcLbvWEX{hqH6zFBA5A6}qSFoX9n~&?Pd-0RGgQd~%?4jQ5;^*!c6D-@? zkjbZ+%<>gwi;{eWDFDbJ8}rbZ&^X)OMzu;UY5!4!wFdgbRL(R#!w;`|YCc8W8m-rv zhnQxHX7c3J?$aYhvZ0Kr#e^b(^nzIcAQks`_!C2SBxHh7jsX=%?Zu|h{{B#4i3s;O zc77(&iGK-W&K!?1h(~&5T^_gZ5*g9`y@_UBdhsL_iB(t`$eH;>F9b`-`Iw?cA^I(2 zERSg6wBS?q$AIcDFIhciIDSa`zovFE$T{G9YQ$uHYFqG2b@kOa)be34PvJX38%57L zOT zCY``Hk;p0FZ#hEl3i$V@rqJ#$JaM6+O!E(d_*bI)o6S+RxAA;d=;j9PQcAmUEiySiAH;?R{lbTTQ$6#@*eW;!bh5 z;7-xv1h*n>p%C0%iWGNuhvF^nPVqu;znmj$)<}TeJbseW-OjP{3ifTWq@)wO?2Z}iO5ub{XW2G6KPp)c?2d1r! zy}Ees+#IQW$M=@`Y+jo%>NhLJ+`BK{m`u?AhCt1)T{pKc?J zKRZM}=3ydV9Ppj3aQNn$;2g2UizAS2Ah7#)2w!Cp9cUk}^-rTg%`-wf5JGIQIzcDC z`%9~=Pfj>wiMBaX>iGuw-(omgK4xi*HPXf2ypds}6N&vm{)UgpVgLT%#*Jfc^TtQz zPwn~e+UMS~y0+H~_FZP!(rL|TRU%%Yufj1@DeceEHuBrFSgnQ?)&zp?l7Vh3bJQP; z=Bg(P8;d7GXPQ>u)a30-*;V!Zo@K*5?ETl>NE#6mD}F6XXD`erfFP7W8T{c#Fvb#=>GG$^7Q=!takKc{+HH;wiEMJeBa_0i7I7V8q2)`Rb6ue?sXe_%P{z@ zXDQ{HLH1p?onP7ALn(rXqW5c0Tvh|@NPCJd!T~uL{hzSj7n_Tlt zh2NQctRGT#4+w|~be&3GF|R35@6&l64|($h0Ls$+=H*iye`Vt85>L2zbr49F@PBK( zXp{*`rm0Sfd==V|^>ku=-}wvN*!%bl({p>Y)~l?fQwgb6072U6+Nw(%Q^SHL`SaI9 z%^3L5=jZz56Q=pf9bUqPS@>NBk#7g#y%o$doo?cZTm6cL+!-rJ$~$2r*EB+Z?nR)# z{AL=gTVon~t|Xz~^{2Hqxz+XRskEo91S)ED)xg?)+FIlC@U$6aGBz1akL$rt2@gjf zpJ_u_ddUKN2t4T#k3$IEFM}s2l7il^RBNO% zo$@MeIKlGI9@EgiBr{P<^D0R?Mh!yf6vN{qkj@=kg1%AwLWE3tpg7}7Luu4*4NZ%& zI>Y`DcW`H0X83ytj@HU|rT;}Zxf#bVSL&!_J5|K&#;0fhhlfHD)>+lZvt=#B;g##b zSpfFmoR{i{6+vX^PTn6gOAX5Y6s*gw%@Lm-f9cd_ck-L9YSgD;()ZJSua|prq4B3m zN@Kbr-7Gtw6hH?6VN!sJmxn~Ifm5Tr$jz>p4~tpzz)EF6a9{Q(!evn`GU`@3f5D_z z+VV3dAR3Vp+ycDb6@fok$wza%UO^?e_@YvGOgRC<&5dGP*9hG z1jf&su65381bmvX7}#g;#=LTjJ|?9N*f)w5eH3Ngydjq_tE=k;`)%k{9Rh`G@I7jh zRN`+@tEo2`uEg-I6*(Pn$vn_x&X$j8erFN0ve7f4h|H+a7Gs6|!mveNlW3S0%^Ec- zBka~lG7dG*{SP%y!))K>pMaU1NKZh3w(FrER@iC$hSFz~QL29T{LB z^35F(rDH(hSqjc`@$7Ume{lQo9^{wj^!uFb8KHfMTn|`-;yu2*>@Mr08;(5ghdd#e zr69u?4Mv+(a}RhM$Q?{6n~5~rN+Ml%*D~Wp?Ob@^ z(frNQpSqnRMkM;Us%k*d{RJ({7q9n`$3d5&sWgO}MHQpefvW3DBRWhrB>wmx_tp&L zB)$$=4=|8X-)MK3!w9=~_UvA)fkK;60YYmBMxu7oW4ED+msB)FvPeiAaGAu3sE$;O zl&E|~aU~;8=nP{S9FX2;80#T&-Oq|~v*Q|D!ue+=opZRgRtdeQkDPEsPvAw~? zR>Z)|Zs{^+OxdqzRzE!kTf18q@I@%QJvkC)q$TBiZ(Go|&FFD!mEw!$`VgmC7!|~k z#<5ext#OU6lUtBG?}S<(JQ#%_&XEV5g+5-vqTd745CGbBe8%#_g8p)pcJ!ClNa*|0 zX`sQ~0lL5le0H{UVLsL^-6%p`lbgDF`Nkh~8&4^7DbUWzy^oH={`BynQKH?mdbJAj z8Z{p-nuuI0I;)oyR(%>Oj!LuH$zyEww|Wkkq|Eux$IVU(&6~Ue^xEWMMTbnf+Db9i%XomKCQ0Yod{6o0=()gT zeMgH%d@`?e)JM3hlpNSHzV=#3WHI~sOu^;|f=CHgtqLKQ)eUugv?ywb4})0>-z#<) zamx2U7HkH0%<5+x$QghuX9*@1yhe_@e=j;IepY|Qb$otl_U$5NE>Dh~`}|PJr%_|0 zI(q7G%l!Lb2X<&&tBDP~2%7&}%)VF*?Oc+bGgsAxt3$uVH>WfhFVvkPIE|6fmWoiTi&pN_8H2$xK3V!%+> zn}-r9a%Uijo=GLt!aDZBBPpn|cF`mF1;!U~>7T;G?|nZ*4NeKCPgH=9|8K*+KwCk3k-w2SR zZHgmtvA+KxVZ7;%IzbWWLMz<*)safld!4$PugR}iQfN3R@`N}8OWMI68!IM8W|o)c zEyN{#dd#~HgiRj`eYk{2(4)->Cm{DPjarJ1i1aBy$nV!eD62qnFR0hiqUJ?x41vRu zx};;k*v7Y9>f*urL_OfrP;yOUM}DUHjL;^4kOQ94)g&y}q_3Q`Jduh&%M)<5SMJBh zII1WuBu+N4fxIqglR(6aFZTAg8ylVRPq!}cbCZ9=kQLdJ6@AxYyg}w=zLA!F;yn^u zsku5^8C7@hvH{KBW!DGb5;Im{6)c!yNAX{IF8ju8|NUuzdS4-B8F|@9EOCJ7DDQuR zvO8^@k$;1Gge;NZbo>upu)3 zI%VOE>afN-&=uu`*V3cI5!2f?Q?yDtn1Vhsm-BR%4Yd0kYM!lM=5Yn0(k zgo?^JTOSDpmrDJnGduKIE`F{DM)-ysIkqGl(HU9VP_2TfA#$Z*lM7SIt4b1kwv*;>sEKbFe zij!GJ{iQR7@zMj&caJA)QMGM{e^o-$oc}Zk%#9@9O)GRfrL`?SE$MBcEZ~6nFl5Ruf|^%S>pEKT$&L~-}wGK z^BX~mf{U+I2i@aS!_jPbetZsK--75PJY+aa(h5ucZYQZ{d`@A;P8KfWNCwc!z#1;f zHJ3#wkt{s%Lk7h8Rtq(1x5r9)CNv76(r6soDBZ}Akj$s?Vb~$$6c*&9s(b%r zqG=^j4rH9T<{0dEYy4A^WG@n-4Lmvjp3PnKJNbs~o-DGrTanI(@DpS(AYHY4v{o03j4`qA<&1WEf) z;aG`yUI>$yd*Uary4tU<$vdI(7FW!=doB%WQy6*QRs>KH1_gC#v{$*pL_w6d6pTO^ zeUK7BBLyQRJ{gN0DFj~eJ18h991b%az!Jd{(GoO-qeaOY%KHt^jFg3gG(3o#h2_u) z_fuc3!kO+fWwWt^uiZtrEMrFG{_3Z>H>z}_cKq$vNlrywHICIW_p?>!X>vImra#p2 zz8ft`nVzxKC_SBgZYe<0sLJh`D+}my>U5DI1#tN}69J|m9g9G9Y{N+~=lA5sPen*p zbv|Pc0_98@0fI*2eiD|kPm{t%@A1TK6V?h`$5*Z{vHzg}`UuNBnEBwHyb8NPz0P0U zjN(SUYMUWDR6*?N6xJ4fTe9TJmI7isrLm*ddCNieMZvl zQ>RvTM7f|IeU5z&m)j3T_zj*CvzD}%pdT}~J)ucd&f(B-J_>@5E_ZG ztde|M2x@NL_XeX+&j9Tzf1&W-GPeDD-jxS8E2!(G;*gF+;ANTi0& z?FpxV?Ta55we_V3ijjYH(ul0;{S15$%I|qujcu{oF7FGHn$sHD8dYL-&Q8O>cX2W^ z|M4|_s_KEBK8jxTWdx5Z8F9@4M0}!2eABihmEHuktWoBdvY{S*$kuYA`si_jMN*?} zk~TS9o=>i}JrN4S zl4Byof-f&XHXq+6-Y%(cY0VqHemL9~!STA@X@8swtRK${6u&7ig-S+Fq*Hmhs=XI; zlVBon;d}Wk!DAbi@CSNI zDjq-bp-ns<_$%oy)6Zt-yc0_K zJ3elF<4u!Gm?I)c@gJz5MT{=VGhh!aO+Y*KA(+n41~(mecC1Z5eTGg5+Rcb27?%Iq z#b*#QuFZ{0Vs~oNby`35$*3^t*i!APJJREuIZ!!wwf&Ud%89Ov>=m6NHQ@`*q9PfI@ojJ*s?@J=^E7Hu_&`$iQiFoUgFFw@kCEg?l93@O-n@iVe3aO$0n8P{Vt z78=zLCAm-qE#23Ij6dvpD*o=;eKx7AIp(yiH?x3kzY(7LyfBFfuf8Uq^Dr6=CH_zo zhwm*92Zg8p=rA3Y@jel`ypMID6_H)-{qr>Y@P0}(c@jLq%Pa^fP(mvuPb>1@fL_*6 zm=Ze2(ej|miZIxu1&bxHZD?EZ==oHeUHxhX;XCepBXa(HJf8^t%#&r;e`_6`8y3Ld zc5ke7Yw}MZ$q@u$5j;5#;_OsqRPJ=K4#y+POr~fpauOLCM{8s-28Qp zx=dI^_^fKO)xjspP$QG^rQ|)OmHGe7_f8)O`jW)bB@M@u8ipoQkFHRdk1n1K?=E`e z7@rpkKVA|UV4;}!fNnrTb{|4_m=vJ0(eZb%^RL;bDj83%%l#goCyFiJjlF0~w_w@d zloZcAv<4s~fagvSB*kUvqp55oz?dyL`qdnb@7SWt1*-+TnBt48#}s2;WW5@gOkmkk3J=lK|ftDQA5XB`KVr3B$5>50fK1-Fah3hR)ZBb-ycPzei zo|x*e7G4m7P|K{lohW_wCXiKeI{Ir&CYd*i5F4aiybF$fo-q~i6%Q)E;}R?_{<+Nn zg@_4VkCb4C6~@>Drk&7=P=C8VUR-qUDPJFLq(4#BP)1ugeAbwk_Aju=3!t7tA zOMKjo5Y1UT4Cluq)hd(HJqLiuJJovOO?3a$Ec;co4PPYB*y z0T3|U?I^MGWoG39b|cu{fDIl%TAD>7PG;xX_y{SBbFe!HDRakH^DJ`;TV>3|+vx#T zL85Ly6D!V5^tB6L3T#7x?`0QTpI3IU7NUHe7(q`UWilK`TAxC=x+TYEsN_?1A z-iIq=+TSmxL6`{>bH_8|uu@iJ{^uVLj!L|`7KwAca|A2-*;m~JzHY8v%wIuqY#0Gg zbohL>buP-Uhz?U35meP^iRg6ppx|x6@J~4*px1X+Z`&^Lma+b6asfzXFXBzbAuHU|3?ixQj>&VF_?bibJZJY&=J zyfJ0MT14)^2Blmh-kk&#$?p9ZN$1e-!Om`wNK5$||}4*pQx?f%!v9JYLQz0CGJ)dCz-g)GRV>UN*D zPXVh+hENH&FiGBG5p*VM*ksZYyE;{>tIp5HCPG0=-*|X@Rm2KauyHs~c-PjnFk-gQ zbyUT7_>q(RrgX#j_-v#1Q9=s3yAc70Mp_(~9E-~xx-<*-rxc$eUPeK?F?BJCR+wMS z{Nz&6gYQn1+g(uN-4%iEYehNipV?6iwD(|P`%4vHJ7#vhjup{}OmL>rUi0fpQB&z? zeIP$6*)rAUX?2O9W-)yflvmVp?PJP^S7(()U0DLo8 z*I|d)Sua$1!*O-FK$5Xm~iIvma)oBX(Z5TWv{mscD4 z)Odr{k*>D3c3*5Ip^zVRW}Xeb8|`Ak)A+J^8r{1Lz4;CeyzH4k|u zOVdv1YoU(C$j4PtPyDiIc=Ru4xWrJG#Jh8LeuF%UUVOGP9KSR{fUOp@M*sCIp$Ks8 zGt?C*u_5%ER#-EW=5AWzujd{rrs)8WZ4hiVq0rqvQhLU=lV)H{(H_O$vC4RT*;cE3 za-aBh&NslG)eTIL7aRML>MU7k#ZK%lo)I!zZaGIku&hzJBVt>tL25+HF*MjLDWMp<}mY0+6r~4KHErHyI<8`ALL^Y~B5+Ak%{8AXQ!Nu~0>>YSE0`o@;Yk zlYmomD*epG&C{cXziLyW{1b$Nn|j@I5CDg7(rOrLwxwH^0{E z9}kC=g+YaN<~2lTt;!@9r^XUR-L%{dG|o5!{vG zK-_zYgE+5>esX*qpBch*9sgo(^KK8`MFs+?9)cTpAP^iKt*X9mIX)wy+_&po`O zG5#5BgdQhnVC?%sM2^Zm~!tB_-DUN0B7AVu5s^XvLzA8 zFx@h|uF8K}?Xh>hRBPf|_IPlth4_>Cb^7kAJB_xmaN%Q1Cvt^j_s-Zy7tOCPAPW62 z8QH{n8u*e*@APPu%%35Ho@w>1Ey2vTlw?FjI;wet^mYnm+bGj=KbnhZ6gb3#!Li|S zwVjyyKI9X5Jd3WG6~pP-|`qbitxKfe$gwfm7g2bYtH7anXE7JBHX zxh%<2d2rr>r38gm(7-^c#)PA>{WFrx^((~sdrm1PCH5BvX}rS8LSXps3S%}N2@g=Y zrPH;QW8^F87{T3;z`5G+(Y+;?UytOa>DtTh7WZjl>sk{J>F-0e0}RU;Evu7$+X}vr z<(AaM*n(-evo-{2X&{=F!8Ly$4<1eLv)-{nMTrwX4=Qbc*;Oi|5%MxICK0kRs>L88 zS&J9>p=zvAx`%dWTZJoL@SsnDk9vi(vqv%~n-LlH$9I1}@XrNgCarndK$Tik-OOQ- zEYr%UIF;{8hj>A@-||MpYOxSe?*bZADQ1YGt^Vw#(j$$d;G!T{50U;G| z&!)z=z?8zySb1mo!SX`|Wtx z_NZY!MS3t227iWhN_>a5vVd9@Hjcy$Mr$lHCmt_mHJ5<{bNI&lhx}AY|GEH~piIRg z;)7^<{Y8|_HJy_Z6VDDP)Wud4M(9872-e>w!bJjDM))pKpu;^bWHSyPU=ykFXT|}F z8hnRZ8WdEdDq^AXkU7v;kwOv3n%VUsNhZ3m0}qLhFYBtu?$smp)h2C4A78qxuH)koirb?C{}04j1x)EYuEohcbMC(uH66V(A&gc?OehbhOtp6U;h z9eI%4AF@c7aIg5AXXW+rB;yy~w`q#I`~v!=nuVRzz)k4izfa9EahdCajdE*9HsgU6 z6wbY{xnEZHS<4}qwNx-}XUJ+e-D{RCnT{I11m5G`g90JQUPwYmDC9WN=2p6YD}O{G z4uuT0q3VUpTiPL<*$?whiFZQ~1BM&mTE1A0eNqv=dq(IKK=1^spKx%LTR0ynyD6w) z;%*4mh^xxDkTbKE#ceBNkUvJs*-t+jV;{jzx|!KIKCbiNG2yJC9i%ni3%A3x32-rY?vz0N3o2z7I>5Yuq(eW>k?@QhZMg1SL_ z_n>#+Uw`>ZK>)9n3xuM6cYIit+K%&GXBRCx78|TCfxCTO7r_7eC^aP17fy>jEaXd8 z7|jRP`5`MiR+HeTl1yh}1d8_=ScV@WLZS)Mco2h>sNkZX-6OEZ4gf$9CJ4cp2aj?| zruYLD9DbNl|GNPQ0+GDt%x35(YUul9>Pn`@byNq}KkeE&w(BPiBBL7;x;^QShLTtL zK{6yVJY+)HE53Yc-6gn}#0q_byD*@wVS;ZL8C8MbR#Rhn6I%fwv;a;jUR zkDA7n9L1IxMqt!vD32})y2vK7waV*aTiN(_3K(WCoC&f}_ej5F0Ksczpy5q&M#5Ea zv|d$DA&{wh__JY9=r)EvE}a^+9dL!ebYN-96i7mH;Y}HzjroE;5+&^Nm&^DeJL)N^ zP2Unu#BT0sF5x1STvFQ&a+*oDk*ceL@RiMk)*Q&% zX>)yjTfj?`FHOZNN9e|AC{5F?mV#+X^%OST;_FcnOb;Vgt{))AX>LFA^^Iy$|KM0D?-800T04=0!0O zVO29ah9vISd^l1sSUSV9T1M?(4pO?~N>RGqeRDAD<9p!XG1>h^ti9i33P}y3FpPq_ zqSGKfQe48~M30<{DZ#5i@P?SHAAUusMOo7zM|2@+K{Wo4IxY(!Ymn3kmmb+ilBGE7)O3mm*P0B?hpnW^i%!BSJP8QO19%zkFfXsHF9Sp(l|K&;l5#7~@{Jhr>$j zgvf4(&#zh3Un!R|A@~I*{c_VAR^QzG=5F#0_b23os~8>ofA=! zGQJVioa*T@9SKAvIwGmZ5cL4p$Ql*k3MLs>CAYNDwPDMVPzZ^gv3$X4@wLB{ znAtlWTaT`XJ@ZXuYxU}@!g^YPDV%mK>4z^NNGN#T6kVmZ=mgQW#Y~D_itJd zJO)C);*z2P0!FP#_(w?#D(S8aDgs=0QCv>%1E`00A3ufn&ZURQsE2 zEWpkkzUJD3*e4f!PK-B6XL-dI&+bT(kAZc;X}R!qBS8*q~TA$Wq)luk!H>WWTYz~s00cZmtJFo zr4tLrPUAPnqI`S{bT#R1ib}wuRIekq%mSaM{vZ9h41MrM-k+(g@`@{Cae^<|eM|q~ zIL?{T$|OdukUORjStue`6Y)K%K*BcZ*ee0n1eR>cU1k@_Yu>{P;W$FHflrYvp6 z%@&>FE!4JZg=KQm2@V1;WJJM8VvmdZfM?|H?($CQ785i7p2bIldUqxH@o$Xa4=H?v zEO~OLk4QNE1s4f;JteJYJU`|2OQd-zj-sh8>MMc^hY|a(&)~*Zw6qJt@Qp_#*gP>* z0%3_560%zEKvX_lb+8gPqJp0=$1&UWGBGdgITcBMwn@Lj;Bxx8d8F&|ep={v$BN4{ z?MuJh1BV4^q0PQB4M`x?VIs8;V(L0&3t z>b41h^hc2~)00)QFxA7q4kWU&6|vN8wWEO|WG&tWi0t0foWp%KHax{lf2{nu9C#x# zmd_J4hXzMRTCga>Mn8`NucM@QU12k9o=ryy+Z9!9;;!sF&}xmwRI%HHu!453zw3k& zmB>UHzNY;}Z*4)|i*99ON`esiWifEpS#9QaL2_n4;=#tM0grUx9{35_t0u=m4h()lwWY}GxWRSFkgSn zG`%(WaM!ebhttyK_Da$6;ieB+MPgg6F_pLAWA5tFo_b+zhS%|^A9Ez}!?@NYc}46rLGuD=1;C|>pniR2#7Yf=yU$kXO9I*S#($$OeJ#9U_ z(qeWz(g+cnbi|}A8D7*FfA$z4M~T4@Lg|*V)H0K{QVknHPX>e??1@>Mphv*A7Fy2x zSY-;t?e3UER4PEWk1P`Y^1XrupQrNA&*r2)m8nyYmhucl$Z);FlxZS-9Pk{%qd!^p zQMZ3rG>@UG(9vczC=V!4=q+|QBrA}tSfX$K+WfqC7v|+K^-9T}6lQ7V%cpyR8A17U z*asBo$Z$~#uD^7Lh)m=vY`)8 zC(IN9^+v_y<(sY#F~g@F!udrHm^}G=!Kmfj4tw<4wvVzpZAtuPee<$Z+LjlwQ=!KQ z^a|Sc!TmZr$q1!O(uP0bA{3KUNNx0!#qQ@n@aimcx6V9Za2-$!w5}RceOgxFZZwsK zhfCq$iHf}}1h(%@dE1$9x0SgfKJCsBF5wYlf`C_XbSp?bF^UQ}SPBflJPPR3Vg9kO znntFTfn5_BD!ozHWn70a2EG;Af_AX7LjH`4cb2dMxc7ASPU_kd9l?W2$?Z?W*b_(U z=NvVUJ@`avHd%w*lzfe%9TNG^R|wr`q~bwks}Mp;HsupC#OatNSD4^ zUvu>>=9{vAoXLa>0+4HBCEt0ZmUXFQIXHUdi((k|yRW>lrd|nuv08lbk8$J61U4V- zf}sBX?~Lthj4WczvH|NDA}!as(`4c8*J)xc6>aAj>$ Optional[int]: def __get_memory_property(self, property: str) -> int: self.__validate_command() - value = self.__read_bytes( - getattr(self.address_config, property).byte_offset, - getattr(self.address_config, property).byte_size, - ) + try: + config: MemorySegment = getattr(self.address_config, property) + except: + # Swallow the missing attribute error and throw a NotImplementedError for the motor instead + raise NotImplementedError( + f"FATAL | DXID: '{self.id}' | Motor does not support {property} attribute!" + ) + + value = self.__read_bytes(config.byte_offset, config.byte_size) if value is None: raise Exception(f"FATAL | DXID: '{self.id}' | Value could not be read!") @@ -195,9 +203,17 @@ def __get_memory_property(self, property: str) -> int: def __set_memory_property(self, property: str, value: int): self.__validate_command() + try: + config: MemorySegment = getattr(self.address_config, property) + except: + # Swallow the missing attribute error and throw a NotImplementedError for the motor instead + raise NotImplementedError( + f"FATAL | DXID: '{self.id}' | Motor does not support {property} attribute!" + ) + self.__write_bytes( - getattr(self.address_config, property).byte_offset, - getattr(self.address_config, property).byte_size, + config.byte_offset, + config.byte_size, value, ) @@ -375,12 +391,27 @@ def set_velocity(self, command: Position2D) -> bool: math.pi / 2 ) - for motor in self.motors: + # rear left + motor_0_speed = min(max(clamped_y - clamped_x - command.rotation.theta, -1), 1) + + # front left + motor_1_speed = min(max(clamped_y + clamped_x - command.rotation.theta, -1), 1) + + # front right + motor_2_speed = min(max(clamped_y - clamped_x - command.rotation.theta, -1), 1) + + # rear right + motor_3_speed = min(max(clamped_y + clamped_x - command.rotation.theta, -1), 1) + + speeds = [motor_0_speed, motor_1_speed, motor_2_speed, motor_3_speed] + + for i, motor in enumerate(self.motors): + speed = speeds[i] # TODO actual kinematics here - if clamped_y < 0: - motor.set_moving_speed((abs(clamped_y) * 1023) + 1023) + if speed < 0: + motor.set_moving_speed((abs(speed) * 1023) + 1023) else: - motor.set_moving_speed(clamped_y * 1023) + motor.set_moving_speed(speed * 1023) return True diff --git a/src/api/v2/api.py b/src/api/v2/api.py index 870ef9d..8c10512 100644 --- a/src/api/v2/api.py +++ b/src/api/v2/api.py @@ -1,27 +1,41 @@ -from typing import Union from fastapi import FastAPI, Request, WebSocket, WebSocketDisconnect from websockets.exceptions import ConnectionClosed from fastapi.templating import Jinja2Templates +from fastapi.middleware.cors import CORSMiddleware +from robot import ( + Position2D, + RobotControl, + DynamixelMotor, + DYNAMIXEL_MX_12_ADDR_CONFIG, +) +from pygame import mixer import asyncio import cv2 app = FastAPI() -camera = cv2.VideoCapture(0, cv2.CAP_DSHOW) +camera = cv2.VideoCapture(0) templates = Jinja2Templates(directory="templates") +mixer.init() +app.add_middleware( + CORSMiddleware, + allow_origins=["*"], + allow_credentials=True, + allow_methods=["*"], # Allow all HTTP methods + allow_headers=["*"], # Allow all headers +) -@app.get("/") -def read_root(): - return {"Hello": "World"} - - -@app.get("/items/{item_id}") -def read_item(item_id: int, q: Union[str, None] = None): - return {"item_id": item_id, "q": q} +motors = [ + DynamixelMotor(10, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)), + DynamixelMotor(11, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)), + DynamixelMotor(12, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True), + DynamixelMotor(13, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True), +] +ctrl = RobotControl("/dev/ttyUSB0", 1, motors) +ctrl.init(1000000) # https://stackoverflow.com/a/70626324 -# Not actually sure if this works with bazel @app.websocket("/ws") async def get_stream(websocket: WebSocket): await websocket.accept() @@ -36,3 +50,30 @@ async def get_stream(websocket: WebSocket): await asyncio.sleep(0.03) except (WebSocketDisconnect, ConnectionClosed): print("Client disconnected") + + +@app.post("/command") +async def command(request: Request): + data = await request.json() + + x_val = -data.get("left", 0) + data.get("right", 0) + y_val = -data.get("down", 0) + data.get("up", 0) + rl = data.get("rotateleft", 0) + rr = data.get("rotateright", 0) + + ctrl.set_velocity(Position2D(x_val, y_val, rr - rl)) + + return {"status": "success"} + + +@app.post("/sfx") +async def command(request: Request): + data = await request.json() + + if data.get("sfx") == "bloop": + mixer.music.load("./bloop.mp3") + elif data.get("sfx") == "bong": + mixer.music.load("./bong.mp3") + mixer.music.play() + + return {"status": "success"} diff --git a/src/robot/v1/BUILD.bazel b/src/robot/v1/BUILD.bazel new file mode 100644 index 0000000..c1429aa --- /dev/null +++ b/src/robot/v1/BUILD.bazel @@ -0,0 +1,18 @@ +load("@rules_python//python:defs.bzl", "py_library") + +# Put all robot files in this library +py_library( + name = "robot_src", + srcs = [ + "config.py", + "robot.py", + "structs.py", + ], + deps = [ + "@pypi//fastapi:pkg", + "@pypi//jinja2:pkg", + "@pypi//numpy:pkg", + "@pypi//opencv_python:pkg", + "@pypi//websockets:pkg", + ], +) diff --git a/src/robot/v1/config.py b/src/robot/v1/config.py new file mode 100644 index 0000000..d5ce2a0 --- /dev/null +++ b/src/robot/v1/config.py @@ -0,0 +1,24 @@ +from structs import DynamixelMotorAddressConfig, MemorySegment + +# Add all Dynamixel Motor Configs here +DYNAMIXEL_MX_12_ADDR_CONFIG = DynamixelMotorAddressConfig( + torque_enable=MemorySegment(24, 1), + led_enable=MemorySegment(25, 1), + d_gain=MemorySegment(26, 1), + i_gain=MemorySegment(27, 1), + p_gain=MemorySegment(28, 1), + goal_position=MemorySegment(30, 2), + moving_speed=MemorySegment(32, 2), + torque_limit=MemorySegment(34, 2), + present_position=MemorySegment(36, 2), + present_speed=MemorySegment(38, 2), + present_load=MemorySegment(40, 2), + present_input_voltage=MemorySegment(42, 1), + present_temperature=MemorySegment(43, 1), + registered=MemorySegment(44, 1), + moving=MemorySegment(46, 1), + lock=MemorySegment(47, 1), + punch=MemorySegment(48, 2), + realtime_tick=MemorySegment(50, 2), + goal_acceleration=MemorySegment(73, 1), +) diff --git a/src/robot/v1/robot.py b/src/robot/v1/robot.py new file mode 100644 index 0000000..81ef8c5 --- /dev/null +++ b/src/robot/v1/robot.py @@ -0,0 +1,325 @@ +# from dynamixel_sdk import * +import math +from typing import Optional +import dynamixel_sdk as dynamixel +import logging + +logger = logging.getLogger(__file__) + + +class DynamixelMotor: + def __init__( + self, + id: int, + address_config: MotorAddressConfig, + position: Position2D, + inverted: bool = False, + ): + self.id = id + self.address_config: MotorAddressConfig = address_config + self.position = position + self.inverted = inverted + self.port_handler = None + self.packet_handler = None + self.initialized = False + + def __validate_command(self): + """ + Run validation against the motor command + """ + if not self.initialized: + raise Exception("Motor needs to be initialized before commanding") + + def __write_bytes(self, offset: int, size: int, value: int): + if size == 1: + dxl_comm_result, dxl_error = self.packet_handler.write1ByteTxRx( + self.port_handler, self.id, offset, value + ) + elif size == 2: + dxl_comm_result, dxl_error = self.packet_handler.write2ByteTxRx( + self.port_handler, self.id, offset, value + ) + elif size == 4: + dxl_comm_result, dxl_error = self.packet_handler.write4ByteTxRx( + self.port_handler, self.id, offset, value + ) + else: + raise NotImplementedError + + if dxl_comm_result != dynamixel.COMM_SUCCESS: + logger.error( + f"FAILURE | DYNAMIXEL ID: '{self.id}' | WriteBytesDXLComm: '{self.packet_handler.getTxRxResult(dxl_comm_result)}'" + ) + return False + elif dxl_error != 0: + logger.error( + f"FAILURE | DYNAMIXEL ID: '{self.id}' | WriteBytesDXLError: '{self.packet_handler.getRxPacketError(dxl_error)}'" + ) + return False + return True + + def __read_bytes(self, offset: int, size: int) -> Optional[int]: + if size == 1: + dxl_value, dxl_comm_result, dxl_error = self.packet_handler.read1ByteTxRx( + self.port_handler, self.id, offset + ) + elif size == 2: + dxl_value, dxl_comm_result, dxl_error = self.packet_handler.read2ByteTxRx( + self.port_handler, self.id, offset + ) + elif size == 4: + dxl_value, dxl_comm_result, dxl_error = self.packet_handler.read4ByteTxRx( + self.port_handler, self.id, offset + ) + else: + raise NotImplementedError + + if dxl_comm_result != dynamixel.COMM_SUCCESS: + logger.error( + f"FAILURE | DXID: '{self.id}' | ReadBytesDXLComm: '{self.packet_handler.getTxRxResult(dxl_comm_result)}'" + ) + return False + elif dxl_error != 0: + logger.error( + f"FAILURE | DXID: '{self.id}' | ReadBytesDXLError: '{self.packet_handler.getRxPacketError(dxl_error)}'" + ) + return None + return dxl_value + + def __get_memory_property(self, property: str) -> int: + self.__validate_command() + + try: + config: MemorySegment = getattr(self.address_config, property) + except: + # Swallow the missing attribute error and throw a NotImplementedError for the motor instead + raise NotImplementedError( + f"FATAL | DXID: '{self.id}' | Motor does not support {property} attribute!" + ) + + value = self.__read_bytes(config.byte_offset, config.byte_size) + + if value is None: + raise Exception(f"FATAL | DXID: '{self.id}' | Value could not be read!") + + return value + + def __set_memory_property(self, property: str, value: int): + self.__validate_command() + + try: + config: MemorySegment = getattr(self.address_config, property) + except: + # Swallow the missing attribute error and throw a NotImplementedError for the motor instead + raise NotImplementedError( + f"FATAL | DXID: '{self.id}' | Motor does not support {property} attribute!" + ) + + self.__write_bytes( + config.byte_offset, + config.byte_size, + value, + ) + + def get_torque_enable(self) -> int: + return self.__get_memory_property("torque_enable") + + def set_torque_enable(self, is_enabled: int): + self.__set_memory_property("torque_enable", is_enabled) + + def get_led_enable(self) -> int: + return self.__get_memory_property("led_enable") + + def set_led_enable(self, is_enabled: int): + self.__set_memory_property("led_enable", is_enabled) + + def get_pid_gain(self) -> tuple[int, int, int]: + p = self.__get_memory_property("p_gain") + i = self.__get_memory_property("i_gain") + d = self.__get_memory_property("d_gain") + + return (p, i, d) + + def set_pid_gain(self, p: int, i: int, d: int): + self.__set_memory_property("p_gain", p) + self.__set_memory_property("i_gain", i) + self.__set_memory_property("d_gain", d) + + def get_goal_position(self) -> int: + return self.__get_memory_property("goal_position") + + def set_goal_position(self, position: int): + self.__set_memory_property("goal_position", position) + + def get_moving_speed(self) -> int: + return self.__get_memory_property("moving_speed") + + def set_moving_speed(self, speed: int): + if self.inverted: + if speed >= 1024: + speed -= 1024 + else: + speed += 1024 + + self.__set_memory_property("moving_speed", speed) + + def get_torque_limit(self) -> int: + return self.__get_memory_property("torque_limit") + + def set_torque_limit(self, limit: int): + self.__set_memory_property("torque_limit", limit) + + def get_present_position(self) -> int: + return self.__get_memory_property("present_position") + + def get_present_speed(self) -> int: + return self.__get_memory_property("present_speed") + + def get_present_load(self) -> int: + return self.__get_memory_property("present_load") + + def get_present_input_voltage(self) -> int: + return self.__get_memory_property("present_input_voltage") + + def get_present_temperature(self) -> int: + return self.__get_memory_property("present_temperature") + + def get_moving(self) -> int: + return self.__get_memory_property("moving") + + def get_realtime_tick(self) -> int: + return self.__get_memory_property("realtime_tick") + + def get_goal_acceleration(self) -> int: + return self.__get_memory_property("goal_acceleration") + + def set_goal_acceleration(self, acceleration: int): + self.__set_memory_property("goal_acceleration") + + def init(self, port_handler, packet_handler): + self.port_handler = port_handler + self.packet_handler = packet_handler + self.initialized = True + + def configure_default(self): + # Enable LED + self.set_led_enable(1) + + # Set default PID values intenral to the motor + self.set_pid_gain(8, 0, 8) + + # Enable the motor, and set to 100% max torque + self.set_torque_enable(1) + self.set_torque_limit(1023) + + # If we've made it this far without throwing an exception, + # we've configured our defaults successfully + logger.info( + f"SUCCESS | DYNAMIXEL ID: '{self.id}' | Default configuration applied!" + ) + + +class RobotControl: + def __init__( + self, + device_name: str, + protocol_version: int, + motors: list[DynamixelMotor], + ): + self.port_handler = dynamixel.PortHandler(device_name) + self.packet_handler = dynamixel.PacketHandler(protocol_version) + self.motors = motors + self.position: Vector2D = Vector2D(0, 0) + + self.__validate_motors() + + def __validate_motors(self): + # Confirm each motor has a unique ID + motor_ids = set([motor.id for motor in self.motors]) + + if len(motor_ids) != len(self.motors): + raise Exception("One or more motors have duplicate IDs defined!") + + def __open_port(self) -> bool: + try: + self.port_handler.openPort() + logger.info(f"SUCCESS | OpenPort") + except: + logger.error(f"FAILURE | OpenPort") + return False + return True + + def __set_baud(self, baud) -> bool: + """ + Set baud rate of the motor + + Args: + port_handler: Object responsible for handling port communications with the Dynamixel motors + + Returns: + bool: True if the command was sent successfully + """ + try: + self.port_handler.setBaudRate(baud) + logger.info(f"SUCCESS | SetBaud: '{baud}'") + except: + logger.error(f"FAILURE | SetBaud: '{baud}'") + return False + return True + + def init(self, baud): + if not self.__open_port(): + raise Exception("Unable to open port for Dynamixel!") + + if not self.__set_baud(baud): + raise Exception("Unable to set baud rate for Dynamixel!") + + for motor in self.motors: + motor.init(self.port_handler, self.packet_handler) + motor.configure_default() + + # TODO fix the verbiage here + def set_velocity(self, command: Position2D) -> bool: + """ + Translates a command of unit vectors and rotations into commands for + the motors on the robot. + + Args: + command: RobotCommand - Command of a unit vector and rotation with the + desired velocity of the robot. + """ + clamped_x = min(max(command.location.x, -1), 1) + clamped_y = min(max(command.location.y, -1), 1) + # TODO figure out how to get the smallest angle correctly + smallest_theta = ((command.rotation.theta + (math.pi / 2)) % math.pi) - ( + math.pi / 2 + ) + + for motor in self.motors: + # TODO actual kinematics here + if clamped_y < 0: + motor.set_moving_speed((abs(clamped_y) * 1023) + 1023) + else: + motor.set_moving_speed(clamped_y * 1023) + + return True + + +if __name__ == "__main__": + """ + NOTE: This isn't for actual execution. This entrypoint is only for + testing. + """ + # TODO set proper locations of the motors, once determined in CAD. + motors = [ + DynamixelMotor(10, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0)), + DynamixelMotor(11, DYNAMIXEL_MX_12_ADDR_CONFIG, Position2D(0, 0, 0), True), + ] + + # Set up the robot controller, with the correct baud rate for the MX12 motors + ctrl = RobotControl("/dev/ttyUSB0", 1, motors) + ctrl.init(1000000) + + from IPython import embed + + embed() diff --git a/src/robot/v1/structs.py b/src/robot/v1/structs.py new file mode 100644 index 0000000..056bd92 --- /dev/null +++ b/src/robot/v1/structs.py @@ -0,0 +1,30 @@ +from dataclasses import dataclass + + +@dataclass +class MemorySegment: + byte_offset: int + byte_size: int + + +@dataclass +class DynamixelMotorAddressConfig: + torque_enable: MemorySegment + led_enable: MemorySegment + d_gain: MemorySegment + i_gain: MemorySegment + p_gain: MemorySegment + goal_position: MemorySegment + moving_speed: MemorySegment + torque_limit: MemorySegment + present_position: MemorySegment + present_speed: MemorySegment + present_load: MemorySegment + present_input_voltage: MemorySegment + present_temperature: MemorySegment + registered: MemorySegment + moving: MemorySegment + lock: MemorySegment + punch: MemorySegment + realtime_tick: MemorySegment + goal_acceleration: MemorySegment diff --git a/src/robot/v1/structs_test.py b/src/robot/v1/structs_test.py new file mode 100644 index 0000000..b047b3b --- /dev/null +++ b/src/robot/v1/structs_test.py @@ -0,0 +1,70 @@ +# @dataclass +# class Vector2D: +# x: float +# y: float + +# def __add__(self, other): +# assert isinstance(other, Vector2D) + +# return Vector2D(self.x + other.x, self.y + other.y) + +# def __iadd__(self, other): +# assert isinstance(other, Vector2D) + +# self.x += other.x +# self.y += other.y + +# def __sub__(self, other): +# assert isinstance(other, Vector2D) + +# return Vector2D(self.x - other.x, self.y - other.y) + +# def __isub__(self, other): +# assert isinstance(other, Vector2D) + +# self.x -= other.x +# self.y -= other.y + + +# @dataclass +# class Rotation2D: +# theta: float + + +# @dataclass +# class Position2D: +# location: Vector2D +# rotation: Rotation2D + +# def __init__(self, x, y, theta): +# self.location = Vector2D(x, y) +# self.rotation = Rotation2D(theta) + + +# @dataclass +# class MemorySegment: +# byte_offset: int +# byte_size: int + + +# @dataclass +# class DynamixelMotorAddressConfig: +# torque_enable: MemorySegment +# led_enable: MemorySegment +# d_gain: MemorySegment +# i_gain: MemorySegment +# p_gain: MemorySegment +# goal_position: MemorySegment +# moving_speed: MemorySegment +# torque_limit: MemorySegment +# present_position: MemorySegment +# present_speed: MemorySegment +# present_load: MemorySegment +# present_input_voltage: MemorySegment +# present_temperature: MemorySegment +# registered: MemorySegment +# moving: MemorySegment +# lock: MemorySegment +# punch: MemorySegment +# realtime_tick: MemorySegment +# goal_acceleration: MemorySegment diff --git a/src/robot/v1/README.md b/src/robot/v2/README.md similarity index 100% rename from src/robot/v1/README.md rename to src/robot/v2/README.md diff --git a/src/robot/v1/__init__.py b/src/robot/v2/__init__.py similarity index 100% rename from src/robot/v1/__init__.py rename to src/robot/v2/__init__.py diff --git a/src/robot/v1/publisher.py b/src/robot/v2/publisher.py similarity index 100% rename from src/robot/v1/publisher.py rename to src/robot/v2/publisher.py diff --git a/src/robot/v1/robot_control.py b/src/robot/v2/robot_control.py similarity index 100% rename from src/robot/v1/robot_control.py rename to src/robot/v2/robot_control.py diff --git a/src/robot/v1/subscriber.py b/src/robot/v2/subscriber.py similarity index 100% rename from src/robot/v1/subscriber.py rename to src/robot/v2/subscriber.py diff --git a/src/util/BUILD.bazel b/src/util/BUILD.bazel new file mode 100644 index 0000000..af408fe --- /dev/null +++ b/src/util/BUILD.bazel @@ -0,0 +1,15 @@ +load("@rules_python//python:defs.bzl", "py_library", "py_test") + +py_library( + name = "linalg_src", + srcs = ["linalg.py"], + deps = [ + "@pypi//numpy:pkg", + ], +) + +py_test( + name = "linalg_test", + srcs = ["linalg_test.py"], + deps = [":linalg_src"], +) diff --git a/src/util/linalg.py b/src/util/linalg.py new file mode 100644 index 0000000..7082389 --- /dev/null +++ b/src/util/linalg.py @@ -0,0 +1,61 @@ +from typing import Union +import numpy as np +import math + +M_PI = math.pi +M_PI_2 = M_PI / 2 +M_PI_3 = M_PI / 3 +M_PI_4 = M_PI / 4 + + +def vector2d(x: Union[int, float], y: Union[int, float]): + """ + Returns a 2D vector as a numpy array. + + Params: + x [Union[int, float]]: X component of the vector + y [Union[int, float]]: Y component of the vector + """ + return np.array([x, y]) + + +def rotation2d(theta: Union[int, float]): + """ + Return a 2D rotation matrix as a numpy array, provided an angle in radians. + From https://scipython.com/book/chapter-6-numpy/examples/creating-a-rotation-matrix-in-numpy/ + + Positive rotations move vectors clockwise, whereas negative rotations move + vectors counterclockwise + + Params: + theta [Union[int, float]]: Angle in radians + """ + c, s = np.cos(theta), np.sin(theta) + return np.array(((c, -s), (s, c))) + + +def smallest_angle(theta: Union[int, float]): + """ + Given an angle in radians, returns the smallest rotation for the angle, + between -PI and PI + + Params: + theta [Union[int, float]]: Angle in radians + """ + # Reduce the angle down to 0-PI + reduced_theta = abs(theta) % (M_PI) + + # If the value is between pi and 2pi (modulus 2pi to not count multiple + # rotations), we need to use 360 minus the angle's value + opposite_hemisphere = (abs(theta) // M_PI) % 2 != 0 + + if opposite_hemisphere: + # If the shortest angle is in the opposite hemisphere, we reverse + # the angle and take the opposite of the angle between 0-180. + # Ex. For 210deg, the shortest angle is actually -150deg, whereas + # if directly flipped, it would be reported as -30deg + return math.copysign(M_PI - reduced_theta, -theta) + else: + # If we are in the same hemisphere, we can just remove any extra + # rotations and keep the same sign. + return math.copysign(reduced_theta, theta) diff --git a/src/util/linalg_test.py b/src/util/linalg_test.py new file mode 100644 index 0000000..5ce87ce --- /dev/null +++ b/src/util/linalg_test.py @@ -0,0 +1,229 @@ +import unittest +import math +from src.util.linalg import ( + vector2d, + rotation2d, + smallest_angle, + M_PI, + M_PI_2, + M_PI_3, + M_PI_4, +) + + +class TestVector2D(unittest.TestCase): + def test_add(self): + total = vector2d(1, 2) + vector2d(3, 4) + self.assertEqual(total[0], 4) + self.assertEqual(total[1], 6) + + def test_iadd(self): + vec_a = vector2d(1, 2) + vec_a += vector2d(5, 5) + self.assertEqual(vec_a[0], 6) + self.assertEqual(vec_a[1], 7) + + def test_sub(self): + total = vector2d(5, 5) - vector2d(3, 4) + self.assertEqual(total[0], 2) + self.assertEqual(total[1], 1) + + def test_isub(self): + vec_a = vector2d(5, 5) + vec_a -= vector2d(1, 2) + self.assertEqual(vec_a[0], 4) + self.assertEqual(vec_a[1], 3) + + def test_scalar_multiply(self): + total = vector2d(1, 2) * 3 + self.assertEqual(total[0], 3) + self.assertEqual(total[1], 6) + + def test_scalar_divide(self): + total = vector2d(8, 4) / 2 + self.assertEqual(total[0], 4) + self.assertEqual(total[1], 2) + + def test_45deg_rotation(self): + angle = M_PI_4 + starting_vec = vector2d(1, 2) + + # Figure out the expected angles programmatically + r = math.sqrt(5) + expected_angle_cw = math.atan2(starting_vec[1], starting_vec[0]) - M_PI_4 + expected_vec_cw = vector2d( + r * math.cos(expected_angle_cw), r * math.sin(expected_angle_cw) + ) + + expected_angle_ccw = math.atan2(starting_vec[1], starting_vec[0]) + M_PI_4 + expected_vec_ccw = vector2d( + r * math.cos(expected_angle_ccw), r * math.sin(expected_angle_ccw) + ) + + for theta, expected in [(angle, expected_vec_cw), (-angle, expected_vec_ccw)]: + rotation = rotation2d(theta) + + rotated = starting_vec.dot(rotation) + self.assertEqual(rotated.shape, (2,)) + self.assertAlmostEqual(rotated[0], expected[0]) + self.assertAlmostEqual(rotated[1], expected[1]) + + def test_90deg_rotation(self): + angle = M_PI_2 + starting_vec = vector2d(1, 2) + expected_vec = vector2d(2, -1) + + for theta, expected in [(angle, expected_vec), (-angle, -expected_vec)]: + rotation = rotation2d(theta) + + rotated = starting_vec.dot(rotation) + self.assertEqual(rotated.shape, (2,)) + self.assertAlmostEqual(rotated[0], expected[0]) + self.assertAlmostEqual(rotated[1], expected[1]) + + def test_180deg_rotation(self): + angle = M_PI + starting_vec = vector2d(1, 2) + expected_vec = vector2d(-1, -2) + + for theta, expected in [(angle, expected_vec), (-angle, expected_vec)]: + rotation = rotation2d(theta) + + rotated = starting_vec.dot(rotation) + self.assertEqual(rotated.shape, (2,)) + self.assertAlmostEqual(rotated[0], expected[0]) + self.assertAlmostEqual(rotated[1], expected[1]) + + def test_270deg_rotation(self): + angle = 3 * M_PI_2 + starting_vec = vector2d(1, 2) + expected_vec = vector2d(-2, 1) + + for theta, expected in [(angle, expected_vec), (-angle, -expected_vec)]: + rotation = rotation2d(theta) + + rotated = starting_vec.dot(rotation) + self.assertEqual(rotated.shape, (2,)) + self.assertAlmostEqual(rotated[0], expected[0]) + self.assertAlmostEqual(rotated[1], expected[1]) + + def test_360deg_rotation(self): + angle = 2 * M_PI + starting_vec = vector2d(1, 2) + expected_vec = vector2d(1, 2) + + for theta, expected in [(angle, expected_vec), (-angle, expected_vec)]: + rotation = rotation2d(theta) + + rotated = starting_vec.dot(rotation) + self.assertEqual(rotated.shape, (2,)) + self.assertAlmostEqual(rotated[0], expected[0]) + self.assertAlmostEqual(rotated[1], expected[1]) + + +class TestRotation2D(unittest.TestCase): + def test_add_90deg_90deg(self): + """ + Test that adding two 90 degree rotations adds to a + 180 degree rotation. + """ + rot_a = rotation2d(M_PI_2) + rot_b = rotation2d(M_PI_2) + expected_total = rotation2d(M_PI) + + total = rot_a.dot(rot_b) + self.assertTrue((total == expected_total).all()) + + def test_add_neg90deg_neg90deg(self): + """ + Test that adding two -90 degree rotations adds to a + -180 degree rotation. + """ + rot_a = rotation2d(-M_PI_2) + rot_b = rotation2d(-M_PI_2) + expected_total = rotation2d(-M_PI) + + total = rot_a.dot(rot_b) + self.assertTrue((total == expected_total).all()) + + def test_add_90deg_180deg(self): + """ + Test that adding a 90 and 180 degree rotation adds to + a 270 degree rotation + """ + rot_a = rotation2d(M_PI_2) + rot_b = rotation2d(M_PI) + expected_total = rotation2d(3 * M_PI_2) + + total = rot_a.dot(rot_b) + self.assertTrue((total == expected_total).all()) + + +class TestSmallestAngle(unittest.TestCase): + def test_small_positive_angle(self): + """ + Test a small, positive angle (between 0 and PI) to confirm the + same angle gets returned + """ + angle = smallest_angle(M_PI_4) + self.assertAlmostEqual(angle, M_PI_4) + + def test_small_negative_angle(self): + """ + Test a small, negative angle (between 0 and -PI) to confirm the + same angle gets returned + """ + angle = smallest_angle(-M_PI_4) + self.assertAlmostEqual(angle, -M_PI_4) + + def test_mid_positive_angle(self): + """ + Test a medium, positive angle (between PI and 2PI) to confirm that + the smaller negative angle is returned + """ + angle = smallest_angle(5 * M_PI_3) + self.assertAlmostEqual(angle, -M_PI_3) + + def test_mid_negative_angle(self): + """ + Test a medium, negative angle (between -PI and -2PI) to confirm that + the smaller positive angle is returned + """ + angle = smallest_angle(-5 * M_PI_3) + self.assertAlmostEqual(angle, M_PI_3) + + def test_large_positive_angle(self): + """ + Test a large, positive angle (between 2PI and 3PI) to confirm + that the same positive angle is returned + """ + angle = smallest_angle(5 * M_PI_2) + self.assertAlmostEqual(angle, M_PI_2) + + def test_large_negative_angle(self): + """ + Test a large, negative angle (between -2PI and -3PI) to confirm + that the same negative angle is returned + """ + angle = smallest_angle(-5 * M_PI_2) + self.assertAlmostEqual(angle, -M_PI_2) + + def test_multi_rollover_positive_angle(self): + """ + Test a large, positive angle (0 and PI, + n*2PI) to confirm + that the reduced positive angle is returned + """ + angle = smallest_angle(M_PI_3 + (14 * 2 * M_PI)) + self.assertAlmostEqual(angle, M_PI_3) + + def test_multi_rollover_negative_angle(self): + """ + Test a large, negative angle (0 and -PI, + n*-2PI) to confirm + that the reduced negative angle is returned + """ + angle = smallest_angle(-M_PI_3 + (14 * -2 * M_PI)) + self.assertAlmostEqual(angle, -M_PI_3) + + +if __name__ == "__main__": + unittest.main() diff --git a/tools/lint/BUILD.bazel b/tools/lint/BUILD.bazel index c35d65d..336d71e 100644 --- a/tools/lint/BUILD.bazel +++ b/tools/lint/BUILD.bazel @@ -18,6 +18,8 @@ alias( "@bazel_tools//src/conditions:linux_aarch64": "@ruff_aarch64-unknown-linux-gnu//:ruff", "@bazel_tools//src/conditions:darwin_arm64": "@ruff_aarch64-apple-darwin//:ruff", "@bazel_tools//src/conditions:darwin_x86_64": "@ruff_x86_64-apple-darwin//:ruff", + "@bazel_tools//src/conditions:windows_x64": "@ruff_x86_64-pc-windows-msvc//:ruff", + "@bazel_tools//src/conditions:windows_arm64": "@ruff_aarch64-pc-windows-msvc//:ruff", }), )