From 1ee59c5679834dac3bdcd42fa575612d74964402 Mon Sep 17 00:00:00 2001 From: m-guberina <gubi.guberina@gmail.com> Date: Thu, 12 Oct 2023 10:27:44 +0200 Subject: [PATCH] main.py in manipulator_viz does inverse kinematics and it works. calibration is off, but thats fixable. there is also rendering, but its way too slow for real-time running. it should be either a separate thread, updating slowly, or a implemented differently. adding some ros bindings and doing rviz is one option, but hopefully there is something simpler too --- .../__pycache__/make_run.cpython-310.pyc | Bin 0 -> 1487 bytes .../__pycache__/make_run.cpython-311.pyc | Bin 0 -> 3068 bytes manipulator_viz/arms/another_debug_dh | 9 + manipulator_viz/arms/j2n6s300_dh_params | 6 + manipulator_viz/arms/j2s7s300_dh_params | 7 + manipulator_viz/arms/kinova_jaco_params | 7 + manipulator_viz/arms/kuka_lbw_iiwa_dh_params | 7 + manipulator_viz/arms/lbr_iiwa_jos_jedan_dh | 7 + manipulator_viz/arms/robot_parameters2 | 9 + manipulator_viz/arms/testing_dh_parameters | 6 + .../arms/ur10e_dh_parameters_from_the_ur_site | 6 + manipulator_viz/arms/ur5e_dh | 6 + manipulator_viz/ellipse.py | 56 ++ manipulator_viz/embedding_in_tk_sgskip.py | 196 ++++++ manipulator_viz/main.py | 71 +++ manipulator_viz/make_run.py | 45 ++ .../manipulator_visual_motion_analyzer.py | 601 ++++++++++++++++++ .../robot_stuff/InverseKinematics.py | 204 ++++++ .../InverseKinematics.cpython-310.pyc | Bin 0 -> 4522 bytes .../InverseKinematics.cpython-311.pyc | Bin 0 -> 7195 bytes .../__pycache__/drawing.cpython-310.pyc | Bin 0 -> 1424 bytes .../__pycache__/drawing.cpython-311.pyc | Bin 0 -> 2272 bytes .../drawing_for_anim.cpython-310.pyc | Bin 0 -> 1220 bytes .../drawing_for_anim.cpython-311.pyc | Bin 0 -> 2032 bytes .../__pycache__/follow_curve.cpython-310.pyc | Bin 0 -> 2363 bytes .../__pycache__/follow_curve.cpython-311.pyc | Bin 0 -> 4189 bytes .../__pycache__/forw_kinm.cpython-310.pyc | Bin 0 -> 11436 bytes .../__pycache__/forw_kinm.cpython-311.pyc | Bin 0 -> 27512 bytes .../__pycache__/inv_kinm.cpython-310.pyc | Bin 0 -> 6565 bytes .../__pycache__/inv_kinm.cpython-311.pyc | Bin 0 -> 14346 bytes .../joint_as_hom_mat.cpython-310.pyc | Bin 0 -> 3429 bytes .../joint_as_hom_mat.cpython-311.pyc | Bin 0 -> 7553 bytes .../__pycache__/utils.cpython-310.pyc | Bin 0 -> 1462 bytes .../__pycache__/utils.cpython-311.pyc | Bin 0 -> 2594 bytes .../webots_api_helper_funs.cpython-310.pyc | Bin 0 -> 4103 bytes .../webots_api_helper_funs.cpython-311.pyc | Bin 0 -> 11565 bytes manipulator_viz/robot_stuff/drawing.py | 36 ++ .../robot_stuff/drawing_for_anim.py | 38 ++ manipulator_viz/robot_stuff/follow_curve.py | 102 +++ manipulator_viz/robot_stuff/forw_kinm.py | 540 ++++++++++++++++ manipulator_viz/robot_stuff/inv_kinm.py | 434 +++++++++++++ .../robot_stuff/joint_as_hom_mat.py | 146 +++++ manipulator_viz/robot_stuff/utils.py | 36 ++ .../robot_stuff/webots_api_helper_funs.py | 191 ++++++ manipulator_viz/run_classic_ik_algs.py | 57 ++ simple_raw_comm/.main.cpp.swp | Bin 0 -> 20480 bytes 46 files changed, 2823 insertions(+) create mode 100644 manipulator_viz/__pycache__/make_run.cpython-310.pyc create mode 100644 manipulator_viz/__pycache__/make_run.cpython-311.pyc create mode 100644 manipulator_viz/arms/another_debug_dh create mode 100644 manipulator_viz/arms/j2n6s300_dh_params create mode 100644 manipulator_viz/arms/j2s7s300_dh_params create mode 100644 manipulator_viz/arms/kinova_jaco_params create mode 100644 manipulator_viz/arms/kuka_lbw_iiwa_dh_params create mode 100644 manipulator_viz/arms/lbr_iiwa_jos_jedan_dh create mode 100644 manipulator_viz/arms/robot_parameters2 create mode 100644 manipulator_viz/arms/testing_dh_parameters create mode 100644 manipulator_viz/arms/ur10e_dh_parameters_from_the_ur_site create mode 100644 manipulator_viz/arms/ur5e_dh create mode 100644 manipulator_viz/ellipse.py create mode 100644 manipulator_viz/embedding_in_tk_sgskip.py create mode 100644 manipulator_viz/main.py create mode 100644 manipulator_viz/make_run.py create mode 100644 manipulator_viz/manipulator_visual_motion_analyzer.py create mode 100644 manipulator_viz/robot_stuff/InverseKinematics.py create mode 100644 manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/drawing.cpython-310.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/drawing.cpython-311.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-310.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-311.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/utils.cpython-310.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/utils.cpython-311.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc create mode 100644 manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc create mode 100644 manipulator_viz/robot_stuff/drawing.py create mode 100644 manipulator_viz/robot_stuff/drawing_for_anim.py create mode 100644 manipulator_viz/robot_stuff/follow_curve.py create mode 100644 manipulator_viz/robot_stuff/forw_kinm.py create mode 100644 manipulator_viz/robot_stuff/inv_kinm.py create mode 100644 manipulator_viz/robot_stuff/joint_as_hom_mat.py create mode 100644 manipulator_viz/robot_stuff/utils.py create mode 100644 manipulator_viz/robot_stuff/webots_api_helper_funs.py create mode 100644 manipulator_viz/run_classic_ik_algs.py create mode 100644 simple_raw_comm/.main.cpp.swp diff --git a/manipulator_viz/__pycache__/make_run.cpython-310.pyc b/manipulator_viz/__pycache__/make_run.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..666063b969d262a0388b6791d5e746142929737c GIT binary patch literal 1487 zcmd1j<>g{vU|@)xtCs4|!NBks#6iYP3=9ko3=9m#77PpwDGVu$ISjdsQH+crHd78$ z2!v*a(kx(_IfpfhErlh8wS^&yJ%u@#L6hwz$V^SfTOyu$WvNBQsot4+skw<InaRbj zd1ac6w-~kjl374%pqQV5fdS+&XOPu%7#SE!7#1+rFx4<-G1V|jGGsBQF)d`oEtbNF zkWXPuVN7F&sYa1uLa2htFJw$%p2JbgQo>Th)XeC@5ZhnNTEhUdw}vr`wVA1wt%h*{ zTMb(ZOAYfvW=4?P*i%?j*jnJ~vN&8Ani&@|*0R^I*RZ6pOET23+A!3x)v%;+K-n;# zaMrL1Fx0T5aH5HG!R*Z9%HkGasO2bOSin=lkj1-zuZCkG$ZXCU&Ki~!E{Hy`3_nOF zhPjrjmb;b*q`roGA!7|o3O7Ve4NnbA3J;XcUBkUVU?GDfLoF{@uOQq$A&@U67#1?t z^40Lwu%z%pb--LKRKt?O2N5f@sbL9b(B$`f$;`mO@RE&zfkBhw7E@vIEw;k=l>Cz7 zTf(`Cd6@<AsX00EshR1id1Z+?NP?LK@x^5+@kJn&ECunY#kZKsif{3zWEPhc$Cu>C zr{^c;+~S28k(rm0nw(mk4oM*&?2{QGEHDX5rXVK-gVKu-0|P@kLk&YLYb|39V-~|i zrb4D*hLsGOjJH@zGEz$tHJNTP=^5N&&C1WrD=Fe*U|;|fzryu1@^e%5)ANf9@>3Fv zz&s-aH>Wf&MZcgZu_CirzqBa6I5W2(CpA7fKd+=HKSv+z%F>*~lKi6hvdk*|+{EnE z_@dH0y@JYHTsAqG#U;u4xdnEzAQyt1E5uMG4N?<Snx|)zlb@WJQ*5V)P*(&B%p!J> z+qgjlFNoj(5u6}`2Sk8!h^Fu@ro4h%tW~K+`Ng-`it>}dfti<*pLUD2C^0WR^%e^_ z7`Y(s4K7J7xW$@SRFqhGiwztA#kV-J5|i_jG86M|v1cVF$Cngk-eL^7#Zp{YRC0?g zCo?ZGC;b+4N@~e1=HjxHTWpC%>A9JCw^;J>i*j!<7sRJ(^55cu218D2(Ji*j?D*8Y zvRmwV@tGy5Ma8$c!LEu2#bCuPmXySj#9NG+V87jB14l^lEk@s4+~5#|s$z`4#gUSk zm<~!Gx0s9aOKu4k=O*Ulq!yRJ6VfdnP(p#F6L1tl2v7pJ#SRTzP)HS<F)%QQFpDtq zF!C|-F!C^RFmf=m{ovsiV&r3#VFHOrFi9~AG4e19F$#hCQj9!|VvH<|MW&EMEer8O zaY<=fnjS_eRs@RCTf)d{Qi>AGGxO4Ki69GR=9R@~XXfSJ5=0g)Ey>I&zQvkXnp;p= rBm#<Akfjj6z*A=t*jF4jx%nxjIjMG_)L#tp8xI4>GaQT@OgwA=DtnVD literal 0 HcmV?d00001 diff --git a/manipulator_viz/__pycache__/make_run.cpython-311.pyc b/manipulator_viz/__pycache__/make_run.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b6e66b210515ea6eaaccbc1a3ece27817db58f85 GIT binary patch literal 3068 zcmZ3^%ge>Uz`zhWS1r|_gMr~Ohy%k+P{wBq1_p-d3@HpLj5!Rsj8Tk?AU0DDQwW4+ zhSDrxnmLCxiY<kug&~SPg*li(ll3LY7){1oBA$6=sYS)9-kEu+xrrs2$;GaDWtxn) z7`6P8SwL!_7~~*k1_p-D7OWs^OBkWz3=9leaCQw-4PzF}febawl5j;?AiZE-8q+dH z28PveJ+wC|g^^f0Qy5bi)0l~K9|@*15o<qjHlwOaVV=WL%TmG#4qPx%gBHRr3=<e* z57e?UG1M^Nik}+BELN~)1_p*@ObiUG;jviDR>Qb}4JL<7*RZ9q)G#k&W?)zik6lIv zl=x(asZU{T!D)9E2TYX<!!ia2hSe}G1FBDJ*=yKqSW?&!sj7z6hJk^hhOLGrg&ji_ zS4xD3R1I4d3j;$9O9}_kdbwau!xfHMa1~kHFts3CYB^BCg$E{A0%L&k5ig9hfDgt( zrfWD*!vmF9%UQ!&!;->@?iU<3@FSZHW24&;!_2@?%T>!=%Y)tZHQcCfs9{OrLbtz$ zr-mhk8$*=4hI@ek%wA*~H3cB_)$-!-i6F9W7#nB&2qCF}r}!!^2K2PXSHoAslEQ<q zADY&1r7DEEEGfL`x+XC89I9anX3*sGdkM-aFG2Z9lj9asVeu`t!uXW@lHyy!xruq1 z1@WmlIq|8P>8W{Ti8)AunFaC1WhwDRAeAfy@u|hPn97Q8@up-JmlVgB<j1GyC+6JZ zg&2{Umy(*CTAU8?=?D8{cv;K9zyPvd7*vkSfUA#mh8l)g&RWJA#w>_(1_p+SOg&t| z3@aHl8E>(cWTciPYBJqo(lfZlnw6iKS5gcr859(Lh3jYJ=cekX=NA{`rz94Ec}56s zPHA3>enC-UMP{*nX;FM}W^O@FYJ75jUP)1Yjy~9-r8$Wu`9<+%nN|9^iP@>~MWuOq z1(m<JY;rP-OOo?*3+$?-K@vfwd3rWE`N@en#ddlKNm&L4hGHoO28ITP3sPYaw8C_T z=^Sea`+`*1XON$YK*dWDDAyN(GFTBP<rjgnX%Q%g7J;%_5hy=u3g2SNE4am4m0FZv ze2c9pKM5Szc`5m6w^)l3^U_mqv4De_3*wpJlGK7*tcgWMiIun5AVG7BBP%gEKPfXY z?-qMjVsd;*QRXeikXtOpg+(Q|*m5%S5_8gTF{h-K++r>+OS#3CSd^ZdnRkmNFTW`F z7IQ&-swV#}E@&9#q!!&`%gl~X%`3abo)@24l3G-JiyQ2!cu)*i++s;dEJ?h@m<jgV zEjDme72jg?y~Pa<UZ^U@_*)z)nThG3^mB{3D8J;EU~z6@PEKlZ2|S_Q;sGTjSUM^O zg^&U&C=z90V7SE&jgBHq1_lODm=(J-fx`bQ8v_qtkHv)W8EJEJS9o8Mv$!H>dqKqR zBDeh&Zu`6Zf;|;8#22V7(cQp(MakxhlH&ylr;Gf~SNNUp%E-;}SdhM=a6!yP8KWyQ zMsR@*iVNZ|%9vh}F}=&n-xD#Rd;#NxoQu55S9q20a`S>MOPt_ykz4T!x8hwfi76Q~ z$}ftkUlCJB;d~I}<YD{3z{$hb;PO?BflqjXd1qWt+y@3>R?CkdqJ!~<p!5W(8E$iY zS7=|6G26kmhx?+S%N0SF4z?Q{eAhW7FL6lDP+Oq9p!y<*@f8l^3mnEz1%;<bKoZ#o zw=KR0w6B=>T`>v0AQg5|D(s3>*hRtcD}v!2Y&Rt2I#@bFdblPScJkhkQR-mni0I*) zP<W9;@_~fh9Nq=lS0s$CNLcSkyC~s;q>@Ld$9IPI6&|%KJbD|%w#e?NzGCQo#W47a zVdzDk&<;kB57Xx+&B>gfJ1cjE<3%~WD{^`l#q_U;>34A7P}b{U>4?9=Av+`SB8S3V zY1uiJ^KEC@t`NK^t$jsW`yz)#2V;lDgy5%|+H1r&NL<vkx}s^-!E%9D=?aJP0>_IS zYBw}>Q28<{3nG@sFN(jYYIa4{?23%}1rEu(;*wJ;r`JuYTadOScZ2s8HHRx|E(gkv zR9{pJx+oreMLf8J=Z3OAvRiIQDa_Gb!F@%_<cgF<2TMm-56^_aPX12*8{GU2-bLaJ z3=Enaw`3t{ytt$^Elm%jby@@}z-|d6t4S$JEYHkKza@e!n3-1=pPiYPdrJ^mw6r8M zr}!3YUTJPYWsw8}0|PiE7lEn}u=U_%{)@vVH$SB`C)KXVo`Hb@RC*RCFfcHDU}j`w tyuo010Tta~P`m&`HyD&Iz|ai_`3o@gfI;X241M5HWEA+ofJuN|007Nrs%HQI literal 0 HcmV?d00001 diff --git a/manipulator_viz/arms/another_debug_dh b/manipulator_viz/arms/another_debug_dh new file mode 100644 index 0000000..ab4e0ac --- /dev/null +++ b/manipulator_viz/arms/another_debug_dh @@ -0,0 +1,9 @@ +0.0;0.0;1.0;0.0 +0.0;0.0;0.5;0.0 +0.0;0.0;1.0;0.0 +0.0;0.0;0.2;0.0 +0.0;0.0;1.0;0.0 +0.0;0.0;0.2;0.0 +0.0;0.0;1.0;0.0 +0.0;0.0;1.0;0.0 +0.0;0.0;1.0;0.0 diff --git a/manipulator_viz/arms/j2n6s300_dh_params b/manipulator_viz/arms/j2n6s300_dh_params new file mode 100644 index 0000000..44eed8e --- /dev/null +++ b/manipulator_viz/arms/j2n6s300_dh_params @@ -0,0 +1,6 @@ +0.2766;0.0;0.0;1.5707966 +0.0;0.0;0.41;3.14159265 +-0.0098;0.0;0.0;1.57079632 +-0.25008;0.0;0.0;1.04719755 +-0.085563;0.0;0.0;1.04719755 +-0.202781;0.0;0.0;3.1415926 diff --git a/manipulator_viz/arms/j2s7s300_dh_params b/manipulator_viz/arms/j2s7s300_dh_params new file mode 100644 index 0000000..7d1b3fa --- /dev/null +++ b/manipulator_viz/arms/j2s7s300_dh_params @@ -0,0 +1,7 @@ +-0.2755;0.0;0.0;1.5708 +0.0;0.0;0.0;1.5708 +-0.41;0.0;0.0;1.5708 +-0.0098;0.0;0.0;1.5708 +-0.31110;0.0;0.0;1.5708 +0.0;0.0;0.0;1.5708 +-0.20380;0.0;0.0;3.14159 diff --git a/manipulator_viz/arms/kinova_jaco_params b/manipulator_viz/arms/kinova_jaco_params new file mode 100644 index 0000000..57b26c9 --- /dev/null +++ b/manipulator_viz/arms/kinova_jaco_params @@ -0,0 +1,7 @@ +-0.2755;;0;1.5707963 +0;;0;1.5707963 +-0.4100;;0;1.5707963 +-0.0098;;0;1.5707963 +-0.3111;;0;1.5707963 +0;;0;1.5707963 +-0.2638;;0;3.1415926 diff --git a/manipulator_viz/arms/kuka_lbw_iiwa_dh_params b/manipulator_viz/arms/kuka_lbw_iiwa_dh_params new file mode 100644 index 0000000..3411eae --- /dev/null +++ b/manipulator_viz/arms/kuka_lbw_iiwa_dh_params @@ -0,0 +1,7 @@ +0.36;0.0;0.0;-1.5708 +0.0;0.0;0.0;1.5708 +0.42;0.0;0.0;1.5708 +0.0;0.0;0.0;-1.5708 +0.40;0.0;0.0;-1.5708 +0.0;0.0;0.0;1.5708 +0.126;0.0;0.0;0.0 diff --git a/manipulator_viz/arms/lbr_iiwa_jos_jedan_dh b/manipulator_viz/arms/lbr_iiwa_jos_jedan_dh new file mode 100644 index 0000000..69b01f6 --- /dev/null +++ b/manipulator_viz/arms/lbr_iiwa_jos_jedan_dh @@ -0,0 +1,7 @@ +0.34;0.0;0.0;-1.5708 +0.0;0.0;0.0;1.5708 +0.40;0.0;0.0;1.5708 +0.0;0.0;0.0;-1.5708 +0.40;0.0;0.0;-1.5708 +0.0;0.0;0.0;1.5708 +0.125;0.0;0.0;0.0 diff --git a/manipulator_viz/arms/robot_parameters2 b/manipulator_viz/arms/robot_parameters2 new file mode 100644 index 0000000..6487a20 --- /dev/null +++ b/manipulator_viz/arms/robot_parameters2 @@ -0,0 +1,9 @@ +.20;.02;.37;.00 +.00;.55;.91;.17 +.05;.39;.76;.00 +-.10;.10;.48;-.17 +.05;.19;.58;.00 +-.10;.10;.50;-.17 +.05;.19;.60;.00 +-.10;.10;.52;-.17 +.05;.19;.61999;.00 diff --git a/manipulator_viz/arms/testing_dh_parameters b/manipulator_viz/arms/testing_dh_parameters new file mode 100644 index 0000000..8c60750 --- /dev/null +++ b/manipulator_viz/arms/testing_dh_parameters @@ -0,0 +1,6 @@ +0.1273;0.0;0.0;1.5708 +0.0;0.0;-0.612;0.0 +0.0;0.0;-0.5723;0.0 +0.1639;0.0;0.0;1.5708 +0.1157;0.0;0.0;-1.5708 +0.0922;0.0;0.0;0.0 diff --git a/manipulator_viz/arms/ur10e_dh_parameters_from_the_ur_site b/manipulator_viz/arms/ur10e_dh_parameters_from_the_ur_site new file mode 100644 index 0000000..991315d --- /dev/null +++ b/manipulator_viz/arms/ur10e_dh_parameters_from_the_ur_site @@ -0,0 +1,6 @@ +0.1807;0.0;0;1.5707963 +0.0;0.0;-0.6127;0.0 +0.0;0.0;-0.57155;0.0 +0.17415;0.0;0.0;1.5707963 +0.11985;0.0;0.0;-1.5707963 +0.11655;0.0;0.0;0.0 diff --git a/manipulator_viz/arms/ur5e_dh b/manipulator_viz/arms/ur5e_dh new file mode 100644 index 0000000..09af722 --- /dev/null +++ b/manipulator_viz/arms/ur5e_dh @@ -0,0 +1,6 @@ +0.163;0.0;0;1.5707963 +0.0;0.0;-0.4250;0.0 +0.0;0.0;-0.39225;0.0 +0.134;0.0;0.0;1.5707963 +0.100;0.0;0.0;-1.5707963 +0.100;0.0;0.0;0.0 diff --git a/manipulator_viz/ellipse.py b/manipulator_viz/ellipse.py new file mode 100644 index 0000000..9cacb93 --- /dev/null +++ b/manipulator_viz/ellipse.py @@ -0,0 +1,56 @@ +import numpy as np +from numpy import linalg +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.pyplot as plt +from matplotlib import cm +import matplotlib.colors as colors +fig = plt.figure(figsize=(8,8)) +ax = fig.add_subplot(111, projection='3d') # number of ellipsoids +ellipNumber = 1 +#set colour map so each ellipsoid as a unique colour +norm = colors.Normalize(vmin=0, vmax=ellipNumber) +cmap = cm.jet +m = cm.ScalarMappable(norm=norm, cmap=cmap) #compute each and plot each ellipsoid iteratively +indx = 10 +A = np.array([[np.random.random_sample(),0,0], + [0,np.random.random_sample(),0], + [0,0,np.random.random_sample()]]) +center = [indx*np.random.random_sample(),indx*np.random.random_sample(),indx*np.random.random_sample()] +# find the rotation matrix and radii of the axes +U, s, rotation = linalg.svd(A) +radii = 1.0/np.sqrt(s) * 0.3 #reduce radii by factor 0.3 +# calculate cartesian coordinates for the ellipsoid surface +u = np.linspace(0.0, 2.0 * np.pi, 60) +v = np.linspace(0.0, np.pi, 60) +x = radii[0] * np.outer(np.cos(u), np.sin(v)) +y = radii[1] * np.outer(np.sin(u), np.sin(v)) +z = radii[2] * np.outer(np.ones_like(u), np.cos(v)) +for i in range(len(x)): + for j in range(len(x)): + [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], rotation) + center +ellipse = ax.plot_surface(x, y, z, rstride=3, cstride=3, + color=m.to_rgba(indx), linewidth=0.1, + alpha=0.3, shade=True) + +# repeat jsut to udapte +indx = 10 +A = np.array([[np.random.random_sample(),0,0], + [0,np.random.random_sample(),0], + [0,0,np.random.random_sample()]]) +center = [indx*np.random.random_sample(),indx*np.random.random_sample(),indx*np.random.random_sample()] +# find the rotation matrix and radii of the axes +U, s, rotation = linalg.svd(A) +radii = 1.0/np.sqrt(s) * 0.3 #reduce radii by factor 0.3 +# calculate cartesian coordinates for the ellipsoid surface +u = np.linspace(0.0, 2.0 * np.pi, 60) +v = np.linspace(0.0, np.pi, 60) +x = radii[0] * np.outer(np.cos(u), np.sin(v)) +y = radii[1] * np.outer(np.sin(u), np.sin(v)) +z = radii[2] * np.outer(np.ones_like(u), np.cos(v)) +for i in range(len(x)): + for j in range(len(x)): + [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], rotation) + center + +ellipse.set_verts(x,y) + +plt.show() diff --git a/manipulator_viz/embedding_in_tk_sgskip.py b/manipulator_viz/embedding_in_tk_sgskip.py new file mode 100644 index 0000000..1c1abd9 --- /dev/null +++ b/manipulator_viz/embedding_in_tk_sgskip.py @@ -0,0 +1,196 @@ +#import tkinter +# override ugly tk widgets with system-based ttk ones +# change the import * thing if it's better for your ide +from tkinter import * +from tkinter.ttk import * + +from matplotlib.backends.backend_tkagg import ( + FigureCanvasTkAgg, NavigationToolbar2Tk) +import matplotlib.pyplot as plt +# Implement the default Matplotlib key bindings. +from matplotlib.backend_bases import key_press_handler +from matplotlib.figure import Figure + +from robot_stuff.InverseKinematics import InverseKinematicsEnv +from robot_stuff.drawing import * +from robot_stuff.inv_kinm import * + +import numpy as np + + +def policy(robot, desired_goal): +# del_thet = invKinmQPSingAvoidE_kI(robot, desired_goal) +# del_thet = invKinm_Jac_T(robot, desired_goal) +# del_thet = invKinm_PseudoInv(robot, desired_goal) +# del_thet = invKinm_dampedSquares(robot, desired_goal) + del_thet = invKinm_PseudoInv_half(robot, desired_goal) +# del_thet = invKinmQP(robot, desired_goal) +# del_thet = invKinmQPSingAvoidE_kI(robot, desired_goal) +# del_thet = invKinmQPSingAvoidE_kM(robot, desired_goal) +# del_thet = invKinmQPSingAvoidManipMax(robot, desired_goal) + + return del_thet + +# TODO move all tkinter stuff to one file, +# and all plot stuff to one file to avoid this schizo mess +root = Tk() +root.wm_title("Embedding in Tk") + +# mock data 'cos i was lazy with cleaning the rest +start = 0 +end = 10 +n_pts = 2000 +dt = (end - start) / n_pts +time = np.linspace(start,end,n_pts) + +angular_v_x = np.sin(time/2) +angular_v_y = np.cos(time/3) +angular_v_z = np.sin(time/5) +v_x = time / 2 +v_y = time **2 / (time + 2) +v_z = np.cos(time / 10) + +# integrate velocities to get positions +angular_p_x = np.cumsum(angular_v_x * dt) +angular_p_y = np.cumsum(angular_v_y * dt) +angular_p_z = np.cumsum(angular_v_z * dt) +p_x = np.cumsum(v_x * dt) +p_y = np.cumsum(v_y * dt) +p_z = np.cumsum(v_z * dt) + + +# when you plot something on the axis, it returns you the object of that plot +# you then update this later on +# this is much much faster than replotting everything every time (even without blitting, +# which can be implemented if necessary) + +# the functions to update these plot objects can be found on matplotlib pages by just googling + + + +# the whole figure +fig = Figure() + +# init robot stuff +ik_env = InverseKinematicsEnv() +# no way this flies, but we have to try +ik_env.ax = fig.add_subplot(121, projection='3d') +# these are for axes scaling which does not happen automatically +ik_env.ax.plot(np.array([0]), np.array([0]), np.array([1.5]), c='b') +ik_env.ax.plot(np.array([0]), np.array([0]), np.array([-1.5]), c='b') +# NOTE hope this applies just to this ax, otherwise change to ax.xlim or whatever +ik_env.ax.set_xlim([-1.5,1.5]) +ik_env.ax.set_ylim([-0.5,0.5]) +color_link = 'black' +ik_env.robot.initDrawing(ik_env.ax, color_link) + +# do the run + + +# TODO may use this as trajectory later? +#position_line, = ax.plot(p_x, p_y, p_z, color='blue') +## the quive sucks for this purpose as the tangets stick to the curve and you don't see anything +##mask = np.arange(n_pts) % 200 == 0 +##ax.quiver(p_x[mask], p_y[mask], p_z[mask], v_x[mask], v_y[mask], v_z[mask], color='orange') +## there's no colormap over a line, so just plot colored line segments (many lines) +#for i in range(n_pts - 1): +# ax.plot(p_x[i:i+2], p_y[i:i+2], p_z[i:i+2], color=plt.cm.plasma((i)/n_pts)) +#fig.colorbar(plt.cm.ScalarMappable(norm=None, cmap=plt.cm.plasma), orientation='horizontal') +# +## needs to be a line 'cos that's what we can update +#point_in_time_position_line, = ax.plot(np.zeros(1), np.zeros(1),np.zeros(1), +# marker='.', markersize=12, linestyle='None', color='red') + + + +# whatever side bullshit +ax = fig.add_subplot(322) +ax.set_xlabel('t/s') +ang_v_x_line, = ax.plot(time, angular_v_x) +point_in_time_angular_v_x_line, = ax.plot(np.zeros(1), np.zeros(1), + marker='.', markersize=12, linestyle='None', color='red') + +ax = fig.add_subplot(324) +ax.set_xlabel('t/s') +ang_v_z_line, = ax.plot(time, angular_v_y) +point_in_time_angular_v_y_line, = ax.plot(np.zeros(1), np.zeros(1), + marker='.', markersize=12, linestyle='None', color='red') + +ax = fig.add_subplot(326) +ax.set_xlabel('t/s') +ang_v_y_line, = ax.plot(time, angular_v_z) +point_in_time_angular_v_z_line, = ax.plot(np.zeros(1), np.zeros(1), + marker='.', markersize=12, linestyle='None', color='red') + + + + + +canvas = FigureCanvasTkAgg(fig, master=root) +canvas.draw() + +# pack_toolbar=False will make it easier to use a layout manager later on. +toolbar = NavigationToolbar2Tk(canvas, root, pack_toolbar=False) +toolbar.update() + +canvas.mpl_connect( + "key_press_event", lambda event: print(f"you pressed {event.key}")) +canvas.mpl_connect("key_press_event", key_press_handler) + +button_quit = Button(master=root, text="Quit", command=root.destroy) + + +# new_val is what the widget gives you +# you define what needs to happen to plots in Figure below, +# and call canvas.draw +def update_point(new_val): + new_val = int(np.floor(float(new_val))) + + # all these are in lists as that's what the line plot wants, + # but we use single points +# point_in_time_position_line.set_data_3d([p_x[new_val]], [p_y[new_val]], [p_z[new_val]]) + point_in_time_angular_v_x_line.set_data([time[new_val]], [angular_v_x[new_val]]) + point_in_time_angular_v_y_line.set_data([time[new_val]], [angular_v_y[new_val]]) + point_in_time_angular_v_z_line.set_data([time[new_val]], [angular_v_z[new_val]]) + + # required to update canvas and attached toolbar! + canvas.draw() + +slider_update = Scale(root, from_=0, to=n_pts - 1, length=500, orient=HORIZONTAL, + command=update_point)#, label="Frequency [Hz]") + +# TODO no way in hell this flies, but we have to try +# does not update main frame until it finishes +def updateIKAnim(): + # do the step + for i in range(50): + action = policy(ik_env.robot, ik_env.goal) + action_fix = list(action) + action_fix.append(1.0) + action_fix = np.array(action_fix) + obs, reward, done, info = ik_env.step(action_fix) + # animate + ik_env.robot.drawStateAnim() + ik_env.ax.set_title(str(ik_env.n_of_tries_for_point) + 'th iteration toward goal') + #ik_env.ax.set_title(str(np.random.random()) + 'th iteration toward goal') + drawPoint(ik_env.ax, ik_env.goal, 'red') + canvas.draw() + root.update() + +button_anim = Button(master=root, text="anim_step", command=updateIKAnim) + + + +# Packing order is important. Widgets are processed sequentially and if there +# is no space left, because the window is too small, they are not displayed. +# The canvas is rather flexible in its size, so we pack it last which makes +# sure the UI controls are displayed as long as possible. +# NOTE: imo it's fine to just pack now and work on the layout later once +# everything we want to plot is known +button_anim.pack(side=BOTTOM) +button_quit.pack(side=BOTTOM) +slider_update.pack(side=BOTTOM) +toolbar.pack(side=BOTTOM, fill=X) +canvas.get_tk_widget().pack(side=TOP, fill=BOTH, expand=True) + +mainloop() diff --git a/manipulator_viz/main.py b/manipulator_viz/main.py new file mode 100644 index 0000000..ff53d75 --- /dev/null +++ b/manipulator_viz/main.py @@ -0,0 +1,71 @@ +# idc if i need this +# Implement the default Matplotlib key bindings. +from robot_stuff.InverseKinematics import InverseKinematicsEnv +from robot_stuff.drawing import * +from robot_stuff.inv_kinm import * + +import numpy as np +import matplotlib.pyplot as plt + +from rtde_control import RTDEControlInterface as RTDEControl +from rtde_receive import RTDEReceiveInterface as RTDEReceive + +rtde_c = RTDEControl("192.168.1.102") + +rtde_r = RTDEReceive("192.168.1.102") +q = rtde_r.getActualQ() + +# Parameters +acceleration = 0.5 +dt = 1.0/500 # 2ms + +ik_env = InverseKinematicsEnv() +ik_env.damping = 25 +ik_env.goal[0] = 0.21 +ik_env.goal[1] = 0.38 +ik_env.goal[2] = 0.5 +#ik_env.render() + + +ik_env.robot.setJoints(q) +ik_env.robot.calcJacobian() +print(ik_env.robot.p_e) +#print(type(init_q)) +#exit() + +# putting it into this class so that python remembers it 'cos reasons, whatever +controller = invKinm_dampedSquares +#controller = invKinmSingAvoidanceWithQP_kI +#while True: +while True: + q = rtde_r.getActualQ() + ik_env.robot.setJoints(q) + ik_env.robot.calcJacobian() + q_dots = controller(ik_env.robot, ik_env.goal) + thetas = np.array([joint.theta for joint in ik_env.robot.joints]) +# ik_env.render() + #print(ik_env.robot.p_e.copy()) + #print(ik_env.goal) + distance = np.linalg.norm(ik_env.robot.p_e.copy() - ik_env.goal) + print(distance) + if distance < 0.01: + t_start = rtde_c.initPeriod() + t_start = rtde_c.initPeriod() + rtde_c.speedJ(np.zeros(6), acceleration, dt) + rtde_c.waitPeriod(t_start) + break + +# print(q_dots) +# print(thetas) + + t_start = rtde_c.initPeriod() + rtde_c.speedJ(q_dots, acceleration, dt) + rtde_c.waitPeriod(t_start) + +for i in range(20): + t_start = rtde_c.initPeriod() + rtde_c.speedJ(np.zeros(6), acceleration, dt) + rtde_c.waitPeriod(t_start) + + +print("done") diff --git a/manipulator_viz/make_run.py b/manipulator_viz/make_run.py new file mode 100644 index 0000000..240c7fd --- /dev/null +++ b/manipulator_viz/make_run.py @@ -0,0 +1,45 @@ +from robot_stuff.InverseKinematics import InverseKinematicsEnv +from robot_stuff.drawing import * +from robot_stuff.inv_kinm import * +from robot_stuff.utils import * + +import numpy as np + +# starting off with random goals, who cares rn +# policy is one of the inverse kinematics control algoritms +# TODO (optional): make it follow a predefined trajectory +# TODO (optional): vectorize it if it isn't fast enough (at least you avoid dynamic allocations) +def makeRun(controller, ik_env, n_iters, robot_index): + # we'll shove everything into lists, and have it ready made + data = { + "qs" : np.zeros((n_iters, ik_env.robots[robot_index].ndof)), + "q_dots" : np.zeros((n_iters, ik_env.robots[robot_index].ndof)), + "manip_ell_eigenvals" : np.zeros((n_iters, 3)) , + "manip_elip_svd_rots" : np.zeros((n_iters, 3, 3)), + "p_es" : np.zeros((n_iters, 3)), + "vs" : np.zeros((n_iters, 6)), # all links linear velocities # NOT USED ATM + "dists_to_goal" : np.zeros(n_iters), + "manip_indeces" : np.zeros(n_iters), + } + + for i in range(n_iters): + q_dots = controller(ik_env.robots[robot_index], ik_env.goal) + ik_env.simpleStep(q_dots, 1.0, robot_index) + + thetas = np.array([joint.theta for joint in ik_env.robots[robot_index].joints]) + data['qs'][i] = thetas + data['q_dots'][i] = q_dots + # NOTE: this is probably not correct, but who cares + data['vs'][i] = ik_env.robots[robot_index].jacobian @ q_dots + M = ik_env.robots[robot_index].jac_tri @ ik_env.robots[robot_index].jac_tri.T + manip_index = np.sqrt(np.linalg.det(M)) + data['manip_indeces'][i] = manip_index + _, diag_svd, rot = np.linalg.svd(M) + # TODO check this: idk if this or the square roots are eigenvalues + data["manip_ell_eigenvals"][i] = np.sqrt(diag_svd) + data["manip_elip_svd_rots"][i] = rot + smallest_eigenval = diag_svd[diag_svd.argmin()] + dist_to_goal = np.linalg.norm(ik_env.robots[robot_index].p_e - ik_env.goal) + data['dists_to_goal'][i] = dist_to_goal + data['p_es'][i] = ik_env.robots[robot_index].p_e + return data diff --git a/manipulator_viz/manipulator_visual_motion_analyzer.py b/manipulator_viz/manipulator_visual_motion_analyzer.py new file mode 100644 index 0000000..d15e61d --- /dev/null +++ b/manipulator_viz/manipulator_visual_motion_analyzer.py @@ -0,0 +1,601 @@ +# override ugly tk widgets with system-based ttk ones +# change the import * thing if it's better for your ide +from tkinter import * +from tkinter.ttk import * + +from matplotlib.backends.backend_tkagg import ( + FigureCanvasTkAgg, NavigationToolbar2Tk) +import matplotlib.pyplot as plt +# Implement the default Matplotlib key bindings. +from matplotlib.backend_bases import key_press_handler +from matplotlib.figure import Figure +import pyautogui +from matplotlib.gridspec import GridSpec + +from robot_stuff.InverseKinematics import InverseKinematicsEnv +from robot_stuff.drawing import * +from robot_stuff.inv_kinm import * +from make_run import makeRun + +import numpy as np +import sys + +sys.path.append('../geometry_matplotlib') +#from common import exit_on_x, make_rotation_matrix3D +#from plot3D import plot3D_camera, plot3D, set_equal_axis3D, camera_data_for_plot3D + +DPI = 80 +SLIDER_SIZE = 200 +#DPI = 200 + +# TODO: call update_point function with the current slider value after updating +# stuff like eliipse on/off so that you don't need to move the button to get it +# just call the slider string value to get the current slider value +# TODO: finish writing up reseting to the same starting position (joint values) +# TODO: readjust figure size so that it is not so javla small + + +####################################################################### +# CONTROL STUFF # +####################################################################### + + +def getController(controller_name): + + if controller_name == "invKinmQPSingAvoidE_kI": + return invKinmQPSingAvoidE_kI + if controller_name == "invKinm_Jac_T": + return invKinm_Jac_T + if controller_name == "invKinm_PseudoInv": + return invKinm_PseudoInv + if controller_name == "invKinm_dampedSquares": + return invKinm_dampedSquares + if controller_name == "invKinm_PseudoInv_half": + return invKinm_PseudoInv_half + if controller_name == "invKinmQP": + return invKinmQP + if controller_name == "invKinmQPSingAvoidE_kI": + return invKinmQPSingAvoidE_kI + if controller_name == "invKinmQPSingAvoidE_kM": + return invKinmQPSingAvoidE_kM + if controller_name == "invKinmQPSingAvoidManipMax": + return invKinmQPSingAvoidManipMax + + return invKinm_dampedSquares + + +root = Tk() +root.wm_title("Embedding in Tk") + +n_iters = 200 +time = np.arange(n_iters) + +# do the run +ik_env = InverseKinematicsEnv() +ik_env.robots.append(Robot_raw(robot_name="no_sim")) +ik_env.damping = 25 +# putting it into this class so that python remembers it 'cos reasons, whatever +controller1 = getController("") +controller2 = getController("invKinm_Jac_T") +ik_env.data.append(makeRun(controller1, ik_env, n_iters, 0)) +ik_env.data.append(makeRun(controller2, ik_env, n_iters, 1)) + +screensize = pyautogui.size() +SCREEN_WIDTH = screensize.width +SCREEN_HEIGHT = screensize.height +SCALING_FACTOR_HEIGHT = 0.8 +SCALING_FACTOR_WIDTH = 0.5 + +####################################################################### +# YARA'S PLOT # +####################################################################### + +fig_ee = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI) +#exit_on_x(fig_ee) +#gs = GridSpec(3, 3, figure=fig_manip_graphs) + +# Instantiate 3D reconstruction plot +#import scipy.io +#data = scipy.io.loadmat('../geometry_matplotlib/data/orebro_castle') +##viewR = make_rotation_matrix3D(0, 0, 0) +#recX, recP = data['U'], data['P'][0] +#scale = recX.std() / 10 +#cam_indices = np.arange(0,recP.shape[0],4) # np.random.permutation(recP.shape[0])[:200] +#rec3D_ax = fig.add_subplot(gs[:,0], projection='3d') +rec3D_ax = fig_ee.add_subplot(projection='3d') +rec3D_ax.set(xticklabels=[], yticklabels=[], zticklabels=[]) +#X_indices = np.arange(0,recX.shape[1],15) #np.random.permutation(recX.shape[1]) +#num_pts_subset = 25 +#for P_ in recP[cam_indices]: +# camera_center_ = plot3D_camera(P_, ax=rec3D_ax, scale=scale, only_center=True) +# camera_center_.set(markersize=1) +#camera_center, camera_coordinate_system = plot3D_camera(recP[0], ax=rec3D_ax, scale=scale) +#rec_points3D = plot3D(recX[:,X_indices[:num_pts_subset]], ax=rec3D_ax, title='')[0][0] +#set_equal_axis3D(rec3D_ax) +#rec3D_ax.view_init(elev=0, azim=0, roll=0) +#rec3D_ax.set_title('3D Reconstruction') +########################################################################### + +####################################################################### +# MANIPULATOR PLOT # +####################################################################### + +# robot plot +#ik_env.ax = fig.add_subplot(132, projection='3d') +#ik_env.ax = fig.add_subplot(gs[:,1], projection='3d') +fig_manipulator = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI) +ik_env.ax = fig_manipulator.add_subplot(projection='3d') +ik_env.ax.plot(np.array([0]), np.array([0]), np.array([1.5]), c='b') +ik_env.ax.plot(np.array([0]), np.array([0]), np.array([-1.5]), c='b') +ik_env.ax.set_xlim([-1.0,1.0]) +ik_env.ax.set_ylim([-1.0,1.0]) +link_colors = ['black', 'violet'] +trajectory_plots = [] +for robot_index, robot in enumerate(ik_env.robots): + robot.initDrawing(ik_env.ax, link_colors[robot_index]) + # ee point + ik_env.p_e_point_plots.append(drawPoint(ik_env.ax, ik_env.data[robot_index]['p_es'][0], 'red', 'o')) + # let's add the trajectory to this + trajectory_plot, = ik_env.ax.plot(ik_env.data[robot_index]['p_es'][:,0], ik_env.data[robot_index]['p_es'][:,1], ik_env.data[robot_index]['p_es'][:,2], color='blue') + trajectory_plots.append(trajectory_plot) +# goal point +ik_env.goal_point_plot = drawPoint(ik_env.ax, ik_env.goal, 'maroon', '*') + + +# let's add the manipulability ellipse +for robot_index, robot in enumerate(ik_env.robots): + radii = 1.0/ik_env.data[robot_index]["manip_ell_eigenvals"][0] * 0.1 + u = np.linspace(0.0, 2.0 * np.pi, 60) + v = np.linspace(0.0, np.pi, 60) + x = radii[0] * np.outer(np.cos(u), np.sin(v)) + y = radii[1] * np.outer(np.sin(u), np.sin(v)) + z = radii[2] * np.outer(np.ones_like(u), np.cos(v)) + for i in range(len(x)): + for j in range(len(x)): + [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], ik_env.data[robot_index]["manip_elip_svd_rots"][0]) + ik_env.data[robot_index]['p_es'][0] + + ik_env.ellipsoid_surf_plots.append(ik_env.ax.plot_surface(x, y, z, rstride=3, cstride=3, + color='pink', linewidth=0.1, + alpha=0.3, shade=True)) + +####################################################################### +# manipulability plots # +####################################################################### + +# manipulability ellipsoid eigenvalues +fig_manip_graphs = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI) +#ax = fig_manip_graphs.add_subplot(gs[0,-1]) +ax = fig_manip_graphs.add_subplot(311) +ax.set_title('Manipulability ellipsoid eigenvalues') +ax.grid() +ax.set_xticks([]) +eigen1_lines = [] +eigen2_lines = [] +eigen3_lines = [] +for robot_index, robot in enumerate(ik_env.robots): + eigen1_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,0], color=link_colors[robot_index]) + eigen1_lines.append(eigen1_line) + eigen2_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,1], color=link_colors[robot_index]) + eigen2_lines.append(eigen2_line) + eigen3_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,2], color=link_colors[robot_index]) + eigen3_lines.append(eigen3_line) +point_in_time_eigen1_line = ax.axvline(x=0, color='red') + +# manipulability index (volume of manipulability ellipsoid) +#ax = fig.add_subplot(gs[1,-1]) +ax = fig_manip_graphs.add_subplot(312) +ax.set_title('Manipulability index') +ax.grid() +ax.set_xticks([]) +manip_index_lines = [] +for robot_index, robot in enumerate(ik_env.robots): + manip_index_line, = ax.plot(time, ik_env.data[robot_index]['manip_indeces'], color=link_colors[robot_index]) + manip_index_lines.append(manip_index_line) +point_in_time_manip_index_line = ax.axvline(x=0, color='red') + +# dist to goal (this could be/should be elsewhere) +#ax = fig.add_subplot(gs[2,-1]) +ax = fig_manip_graphs.add_subplot(313) +ax.set_title('Distance to goal') +ax.grid() +ax.set_xlabel('iter') +dist_to_goal_lines = [] +for robot_index, robot in enumerate(ik_env.robots): + dist_to_goal_line, = ax.plot(time, ik_env.data[robot_index]['dists_to_goal'], color=link_colors[robot_index]) + dist_to_goal_lines.append(dist_to_goal_line) +point_in_time_dist_to_goal_line = ax.axvline(x=0, color='red') + + +####################################################################### +# IMU plots # +####################################################################### + +fig_imu = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI) +#ax = fig_manip_graphs.add_subplot(gs[0,-1]) +v_x_lines = [] +v_y_lines = [] +v_z_lines = [] +omega_x_lines = [] +omega_y_lines = [] +omega_z_lines = [] + +ax_v_x = fig_imu.add_subplot(321) +ax_v_x.grid() +ax_v_y = fig_imu.add_subplot(322) +ax_v_y.grid() +ax_v_z = fig_imu.add_subplot(323) +ax_v_z.grid() +ax_omega_x = fig_imu.add_subplot(324) +ax_omega_x.grid() +ax_omega_y = fig_imu.add_subplot(325) +ax_omega_y.grid() +ax_omega_z = fig_imu.add_subplot(326) +ax_omega_z.grid() +ax_v_x.set_title('Linear velocity x') +ax_v_y.set_title('Linear velocity y') +ax_v_z.set_title('Linear velocity z') +ax_omega_x.set_title('Angular velocity x') +ax_omega_y.set_title('Angular velocity y') +ax_omega_z.set_title('Angular velocity z') +ax_v_x.set_xticks([]) +ax_v_y.set_xticks([]) +ax_v_z.set_xticks([]) +ax_omega_x.set_xticks([]) +ax_omega_y.set_xticks([]) +ax_omega_z.set_xticks([]) +for robot_index, robot in enumerate(ik_env.robots): + v_x_line, = ax_v_x.plot(time, ik_env.data[robot_index]["vs"][:,0], color=link_colors[robot_index]) + v_y_line, = ax_v_y.plot(time, ik_env.data[robot_index]["vs"][:,1], color=link_colors[robot_index]) + v_z_line, = ax_v_z.plot(time, ik_env.data[robot_index]["vs"][:,2], color=link_colors[robot_index]) + omega_x_line, = ax_omega_x.plot(time, ik_env.data[robot_index]["vs"][:,3], color=link_colors[robot_index]) + omega_y_line, = ax_omega_y.plot(time, ik_env.data[robot_index]["vs"][:,4], color=link_colors[robot_index]) + omega_z_line, = ax_omega_z.plot(time, ik_env.data[robot_index]["vs"][:,5], color=link_colors[robot_index]) + v_x_lines.append(v_x_line) + v_y_lines.append(v_y_line) + v_z_lines.append(v_z_line) + omega_x_lines.append(omega_x_line) + omega_y_lines.append(omega_y_line) + omega_z_lines.append(omega_z_line) + +point_in_time_ax_v_x_line = ax_v_x.axvline(x=0, color='red') +point_in_time_ax_v_y_line = ax_v_y.axvline(x=0, color='red') +point_in_time_ax_v_z_line = ax_v_z.axvline(x=0, color='red') +point_in_time_ax_omega_x_line = ax_omega_x.axvline(x=0, color='red') +point_in_time_ax_omega_y_line = ax_omega_y.axvline(x=0, color='red') +point_in_time_ax_omega_z_line = ax_omega_z.axvline(x=0, color='red') + + +####################################################################### +# putting plots into: frames -> notebooks -> tabs # +####################################################################### +# adding whatever into this +notebook_left = Notebook(root, height=int(SCREEN_HEIGHT))#, width=700) +notebook_right = Notebook(root, height=int(SCREEN_HEIGHT))#, width=700) +frame_manipulator = Frame(notebook_left)#, width=400, height=400) +frame_manip_graphs = Frame(notebook_right)#, width=400, height=400) +frame_imu = Frame(notebook_right)#, width=400, height=400) +frame_ee = Frame(notebook_left)#, width=400, height=400) +tabs_left = notebook_left.add(frame_manipulator, text='manipulator') +tabs_left = notebook_left.add(frame_ee, text="end-effector") +tabs_right = notebook_right.add(frame_manip_graphs, text='manipulator-graphs') +tabs_right = notebook_right.add(frame_imu, text="ee-graphs") +#frame_manipulator.pack(fill='both', expand=True) +#frame_manip_graphs.pack(fill='both', expand=True) +notebook_left.grid(row=0, column=0, sticky='ew') +notebook_right.grid(row=0, column=1, sticky='ew') +#notebook_left.pack(fill='both', expand=True) +#canvas = FigureCanvasTkAgg(fig, master=root) + +# tkinterize these plots +canvas_manipulator = FigureCanvasTkAgg(fig_manipulator, master=frame_manipulator) +canvas_manipulator_widget = canvas_manipulator.get_tk_widget() +canvas_manipulator_widget.grid(row=0, column=0) +canvas_manipulator._tkcanvas.grid(row=1, column=0) +canvas_manipulator.draw() + +canvas_ee = FigureCanvasTkAgg(fig_ee, master=frame_ee) +canvas_ee_widget = canvas_ee.get_tk_widget() +canvas_ee_widget.grid(row=0, column=0) +canvas_ee._tkcanvas.grid(row=1, column=0) +canvas_ee.draw() + +canvas_manip_graphs = FigureCanvasTkAgg(fig_manip_graphs, master=frame_manip_graphs) +canvas_manip_graphs_widget = canvas_manip_graphs.get_tk_widget() +canvas_manip_graphs_widget.grid(row=0, column=0) +canvas_manip_graphs._tkcanvas.grid(row=1, column=0) +canvas_manip_graphs.draw() + +canvas_imu = FigureCanvasTkAgg(fig_imu, master=frame_imu) +canvas_imu_widget = canvas_manip_graphs.get_tk_widget() +canvas_imu_widget.grid(row=0, column=0) +canvas_imu._tkcanvas.grid(row=1, column=0) +canvas_imu.draw() + +# pack_toolbar=False will make it easier to use a layout manager later on. +toolbar_manipulator = NavigationToolbar2Tk(canvas_manipulator, frame_manipulator, pack_toolbar=False) +toolbar_manipulator.update() +toolbar_manipulator.grid(column=0, row=2) +toolbar_manip_graphs = NavigationToolbar2Tk(canvas_manip_graphs, frame_manip_graphs, pack_toolbar=False) +toolbar_manip_graphs.update() +toolbar_manip_graphs.grid(column=0, row=2) +toolbar_ee = NavigationToolbar2Tk(canvas_ee, frame_ee, pack_toolbar=False) +toolbar_ee.update() +toolbar_ee.grid(column=0, row=2) +toolbar_imu = NavigationToolbar2Tk(canvas_imu, frame_imu, pack_toolbar=False) +toolbar_imu.update() +toolbar_imu.grid(column=0, row=2) + +####################################################################### +# functions for widgets to control the plots # +####################################################################### + +# keyboard inputs +# whatever, doesn't hurt, could be used +#canvas.mpl_connect( +# "key_press_event", lambda event: print(f"you pressed {event.key}")) +#canvas.mpl_connect("key_press_event", key_press_handler) + +# new_val is what the widget gives you +# you define what needs to happen to plots in Figure below, +# and call canvas.draw +def update_points(new_val): +# ee plot +########################################################################### + index = int(np.floor(float(new_val))) + + # Update 3D reconstruction plot + # plot3D_camera(recP[index], scale=scale) +# C, Q = camera_data_for_plot3D(recP[cam_indices[index]]) +# camera_center.set_data_3d(*C) +# C = C.flatten() +# for ii in range(len(camera_coordinate_system)): +# camera_coordinate_system[ii].set_data([C[0], C[0] + scale * Q[0,ii]], +# [C[1], C[1] + scale * Q[1,ii]]) +# camera_coordinate_system[ii].set_3d_properties([C[2], C[2] + scale * Q[2,ii]]) +# recX_ = recX[:,X_indices[:num_pts_subset*(index+1)]] +# rec_points3D.set_data_3d(recX_[0,:], recX_[1,:], recX_[2,:]) +########################################################################### + + ik_env.ax.set_title(str(index) + 'th iteration toward goal') + # animate 3d manipulator + for robot_index, robot in enumerate(ik_env.robots): + ik_env.robots[robot_index].setJoints(ik_env.data[robot_index]["qs"][index]) + ik_env.robots[robot_index].drawStateAnim() + # all these are in lists as that's what the line plot wants, + # despite the fact that we have single points + point_in_time_eigen1_line.set_xdata([time[index]]) + point_in_time_manip_index_line.set_xdata([time[index]]) + point_in_time_dist_to_goal_line.set_xdata([time[index]]) + point_in_time_ax_v_x_line.set_xdata([time[index]]) + point_in_time_ax_v_y_line.set_xdata([time[index]]) + point_in_time_ax_v_z_line.set_xdata([time[index]]) + point_in_time_ax_omega_x_line.set_xdata([time[index]]) + point_in_time_ax_omega_y_line.set_xdata([time[index]]) + point_in_time_ax_omega_z_line.set_xdata([time[index]]) + + # ellipsoid update + radii = 1.0/ik_env.data[robot_index]["manip_ell_eigenvals"][index] * 0.1 + u = np.linspace(0.0, 2.0 * np.pi, 60) + v = np.linspace(0.0, np.pi, 60) + x = radii[0] * np.outer(np.cos(u), np.sin(v)) + y = radii[1] * np.outer(np.sin(u), np.sin(v)) + z = radii[2] * np.outer(np.ones_like(u), np.cos(v)) + for i in range(len(x)): + for j in range(len(x)): + [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], ik_env.data[robot_index]["manip_elip_svd_rots"][index]) + ik_env.data[robot_index]['p_es'][index] + + if ellipse_on_off_var.get(): + try: + ik_env.ellipsoid_surf_plots[robot_index].remove() + except ValueError: + pass + ik_env.ellipsoid_surf_plots[robot_index] = ik_env.ax.plot_surface(x, y, z, rstride=3, cstride=3, + color='pink', linewidth=0.1, + alpha=0.3, shade=True) + + ik_env.p_e_point_plots[robot_index].set_data([ik_env.data[robot_index]['p_es'][index][0]], [ik_env.data[robot_index]['p_es'][index][1]]) + ik_env.p_e_point_plots[robot_index].set_3d_properties([ik_env.data[robot_index]['p_es'][index][2]]) + canvas_ee.draw() + canvas_manipulator.draw() + canvas_manip_graphs.draw() + canvas_imu.draw() + # TODO update may not be needed as we're going by slider here + root.update() + + +def update_goal_x(new_val): + goal_x = float(new_val) + ik_env.goal[0] = goal_x + ik_env.goal_point_plot.set_data([ik_env.goal[0]], [ik_env.goal[1]]) + ik_env.goal_point_plot.set_3d_properties([ik_env.goal[2]]) + canvas_ee.draw() + canvas_manipulator.draw() + canvas_manip_graphs.draw() + # TODO update may not be needed as we're going by slider here + root.update() + +def update_goal_y(new_val): + goal_y = float(new_val) + ik_env.goal[1] = goal_y + ik_env.goal_point_plot.set_data([ik_env.goal[0]], [ik_env.goal[1]]) + ik_env.goal_point_plot.set_3d_properties([ik_env.goal[2]]) + canvas_ee.draw() + canvas_manipulator.draw() + canvas_manip_graphs.draw() + # TODO update may not be needed as we're going by slider here + root.update() + +def update_goal_z(new_val): + goal_z = float(new_val) + ik_env.goal[2] = goal_z + ik_env.goal_point_plot.set_data([ik_env.goal[0]], [ik_env.goal[1]]) + ik_env.goal_point_plot.set_3d_properties([ik_env.goal[2]]) + canvas_ee.draw() + canvas_manipulator.draw() + canvas_manip_graphs.draw() + # TODO update may not be needed as we're going by slider here + root.update() + + + +def reset(): + ik_env.reset() +# ik_env.goal_point_plot.remove() +# ik_env.goal_point_plot = drawPoint(ik_env.ax, ik_env.goal, 'red', 'o') +# print(controller_string1.get()) +# print(controller_string2.get()) + controller1 = getController(controller_string1.get()) + controller2 = getController(controller_string2.get()) + controllers = [controller1, controller2] + for robot_index, robot in enumerate(ik_env.robots): + ik_env.data.append(makeRun(controllers[robot_index], ik_env, n_iters, robot_index)) + trajectory_plots[robot_index].set_data(ik_env.data[robot_index]['p_es'][:,0], ik_env.data[robot_index]['p_es'][:,1]) + trajectory_plots[robot_index].set_3d_properties(ik_env.data[robot_index]['p_es'][:,2]) + eigen1_lines[robot_index].set_ydata(ik_env.data[robot_index]["manip_ell_eigenvals"][:,0]) + eigen2_lines[robot_index].set_ydata(ik_env.data[robot_index]["manip_ell_eigenvals"][:,1]) + eigen3_lines[robot_index].set_ydata(ik_env.data[robot_index]["manip_ell_eigenvals"][:,2]) + manip_index_lines[robot_index].set_ydata(ik_env.data[robot_index]['manip_indeces']) + dist_to_goal_lines[robot_index].set_ydata(ik_env.data[robot_index]['dists_to_goal']) + update_points(0) + canvas_ee.draw() + canvas_manipulator.draw() + canvas_manip_graphs.draw() + root.update() + +def play(): + pass + +# ellipse on/off +def add_remove_ellipse(): + try: + for robot_index, robot in enumerate(ik_env.robots): + ik_env.ellipsoid_surf_plots[robot_index].remove() + except ValueError: + pass + + +# NOTE: this thing is not used +same_starting_position_on_off_var = IntVar() +same_starting_position_on_off_var.set(0) +same_starting_position_checkbutton= Checkbutton(root, text = "same starting position on/off", + variable = same_starting_position_on_off_var, + onvalue = 1, + offvalue = 0, +# command=same_starting_position_cmd, + ) + + +####################################################################### +# PLACING ELEMENTS IN THE GUI # +####################################################################### + +# LEFT PANE BELOW MANIP/EE PLOTS +frame_below_manipulator_plot = Frame(frame_manipulator) +frame_below_manipulator_plot.grid(column=0, row=3) + +# set controller 1 +frame_controller_menu1 = Frame(frame_below_manipulator_plot) +controller_string1 = StringVar(frame_controller_menu1) +controller_string1.set("invKinm_dampedSquares") # default value +controller_menu1 = OptionMenu(frame_controller_menu1, controller_string1, + "invKinmQPSingAvoidE_kI", + "invKinm_Jac_T", + "invKinm_PseudoInv", + "invKinm_dampedSquares", + "invKinm_PseudoInv_half", + "invKinmQP", + "invKinmQPSingAvoidE_kI", + "invKinmQPSingAvoidE_kM", + "invKinmQPSingAvoidManipMax", + ) +controller_string1.set("invKinm_dampedSquares") +controller_menu1.grid(column=1, row=0) +Label(frame_controller_menu1, text="Robot 1 controller:", background='yellow').grid(row=0, column=0, pady=4, padx = 4) +frame_controller_menu1.grid(column=0, row=0) + +# set controller 2 +frame_controller_menu2 = Frame(frame_below_manipulator_plot) +controller_string2 = StringVar(frame_controller_menu2) +controller_menu2 = OptionMenu(frame_controller_menu2, controller_string2, + "invKinmQPSingAvoidE_kI", + "invKinm_Jac_T", + "invKinm_PseudoInv", + "invKinm_dampedSquares", + "invKinm_PseudoInv_half", + "invKinmQP", + "invKinmQPSingAvoidE_kI", + "invKinmQPSingAvoidE_kM", + "invKinmQPSingAvoidManipMax", + ) +controller_string2.set("invKinm_Jac_T") # default value +Label(frame_controller_menu1, text="Robot 1 controller:", background='black', foreground='white').grid(row=0, column=0, pady=4, padx = 4) +controller_menu2.grid(column=1, row=0) +Label(frame_controller_menu2, text="Robot 2 controller:", background='#EE82EE').grid(row=0, column=0, pady=4, padx = 4) +frame_controller_menu2.grid(column=0, row=1) + + +ellipse_on_off_var = IntVar() +ellipse_on_off_var.set(1) + +ellipse_checkbutton= Checkbutton(frame_below_manipulator_plot, text = "ellipse on/off", + variable = ellipse_on_off_var, + onvalue = 1, + offvalue = 0, + command=add_remove_ellipse, + ) +ellipse_checkbutton.grid(column=1, row=0) + + +frame_goal_x = Frame(frame_below_manipulator_plot) +Label(frame_goal_x, text="goal x").grid(row=0, column=0, pady=4, padx = 4) +slider_goal_x = Scale(frame_goal_x, from_=-1.0, to=1.0, length=SLIDER_SIZE, orient=HORIZONTAL, + command=update_goal_x) +slider_goal_x.grid(column=1, row=0) +frame_goal_x.grid(column=1, row=1) + +frame_goal_y = Frame(frame_below_manipulator_plot) +Label(frame_goal_y, text="goal y").grid(row=0, column=0, pady=4, padx = 4) +slider_goal_y = Scale(frame_goal_y, from_=-1.0, to=1.0, length=SLIDER_SIZE, orient=HORIZONTAL, + command=update_goal_y) +slider_goal_y.grid(column=1, row=0) +frame_goal_y.grid(column=1, row=2) + + +frame_goal_z = Frame(frame_below_manipulator_plot) +Label(frame_goal_z, text="goal z").grid(row=0, column=0, pady=4, padx = 4) +slider_goal_z = Scale(frame_goal_z, from_=-1.0, to=1.0, length=SLIDER_SIZE, orient=HORIZONTAL, + command=update_goal_z) +slider_goal_z.grid(column=1, row=0) +frame_goal_z.grid(column=1, row=3) + + +frame_update = Frame(frame_manip_graphs) +Label(frame_update, text="Time").grid(row=0, column=0, pady=4, padx = 4) +slider_update = Scale(frame_update, from_=0, to=n_iters - 1, length=SLIDER_SIZE, orient=HORIZONTAL, + command=update_points)#, label="Frequency [Hz]") +slider_update.grid(column=1, row=0) +frame_update.grid(column=0, row=3) + +frame_update2 = Frame(frame_imu) +Label(frame_update2, text="Time").grid(row=0, column=0, pady=4, padx = 4) +slider_update = Scale(frame_update2, from_=0, to=n_iters - 1, length=SLIDER_SIZE, orient=HORIZONTAL, + command=update_points)#, label="Frequency [Hz]") +slider_update.grid(column=1, row=0) +frame_update2.grid(column=0, row=3) + + +button_quit = Button(master=frame_manip_graphs, text="Quit", command=root.destroy) +button_reset = Button(master=frame_manip_graphs, text="New run", command=reset) +button_reset.grid(column=0, row=4) +button_quit.grid(column=0, row=5) + +button_quit2 = Button(master=frame_imu, text="Quit", command=root.destroy) +button_reset2 = Button(master=frame_imu, text="New run", command=reset) +button_reset2.grid(column=0, row=4) +button_quit2.grid(column=0, row=5) + +update_points(0) +slider_goal_x.set(ik_env.goal[0]) +slider_goal_y.set(ik_env.goal[1]) +slider_goal_z.set(ik_env.goal[2]) +mainloop() diff --git a/manipulator_viz/robot_stuff/InverseKinematics.py b/manipulator_viz/robot_stuff/InverseKinematics.py new file mode 100644 index 0000000..71c50f5 --- /dev/null +++ b/manipulator_viz/robot_stuff/InverseKinematics.py @@ -0,0 +1,204 @@ +# kinematics related imports +from robot_stuff.forw_kinm import * +from robot_stuff.inv_kinm import * +from robot_stuff.follow_curve import * +from robot_stuff.utils import * + +# general imports +import numpy as np + +# ######################################### NOTE ######################### +# i'm half-adding another robot for the purposes of the plot. +# this means that half of the code will end up being broken (as a lot of it is already anyway). +# in case you want to use this elsewhere, just find the commit before the change +# and run with that. +################################################################## + + +class InverseKinematicsEnv: + +# set damping (i.e. dt i.e. precision let's be real) + def __init__(self, model_path=None, initial_qpos=None, n_actions=None, n_substeps=None ): + print('env created') + self.chill_reset = False + # TODO write a convenience dh_parameter loading function + self.robot = Robot_raw(robot_name="no_sim") + self.init_qs_robot = [] + self.damping = 5 + self.error_vec = None + self.n_of_points_done = 0 + # keep track of the timesteps (to be reset after every episode) + self.n_of_tries_for_point = 0 + # for storing whole runs of all robots + self.data = [] + self.p_e_point_plots = [] + self.ellipsoid_surf_plots = [] + self.same_starting_position_on_off = 0 + + # init goal + self.goal = np.random.random(3) * 0.7 + # needed for easy initialization of the observation space + + # TODO enable setting the other one with greater ease + self.reward_type = 'dense' + #self.reward_type = 'sparse' + self.episode_score = 0 + + +# NOTE: BROKEN AS IT'S FOR ONLY ONE ROBOT + def compute_reward(self, achieved_goal, goal, info): + # Compute distance between goal and the achieved goal. + if self.reward_type == 'sparse': + if not error_test(self.robot.p_e, self.goal): + return np.float32(-1.0) + else: + return np.float32(10.0) + if self.reward_type == 'dense': + distance = goal_distance(achieved_goal, goal) + if not error_test(self.robot.p_e, self.goal): + #reward = -1 * distance + 1 / distance + reward = -1 * distance + else: + reward = 100 + return reward + + + + def simpleStep(self, q_dots, damping, index): + self.robot.forwardKinmViaPositions(q_dots / self.damping, damping) + self.n_of_tries_for_point += 1 +# done = False +# if error_test(self.robot.p_e, self.goal): +# info = { +# 'is_success': np.float32(1.0), +# } +# else: +# info = { +# 'is_success': np.float32(0.0), +# } +# return done + + +# NOTE: BROKEN AS IT'S FOR ONLY ONE ROBOT + def step(self, action): + self.robot.forwardKinmViaPositions(action[:-1] / self.damping, action[-1]) + self.n_of_tries_for_point += 1 + obs = self._get_obs() + + done = False + if error_test(self.robot.p_e, self.goal): + info = { + 'is_success': np.float32(1.0), + } + else: + info = { + 'is_success': np.float32(0.0), + } + reward = self.compute_reward(obs['achieved_goal'], self.goal, info) + self.episode_score += reward + return obs, reward, done, info + + + + + def reset(self): + # TODO: initialize robot joints state to a random (but valid (in joint range)) initial state + # when using joint clamping + self.episode_score = 0 + self.n_of_points_done += 1 + self.n_of_tries_for_point = 0 + self.data = [] + + # generate new point + # NOTE: simply taken away 'cos now it's set with a slider + #self.goal = np.array([random.uniform(-0.70, 0.70), random.uniform(-0.70, 0.70), random.uniform(-0.70, 0.70)]) + + # initialize to a random starting state and check whether it makes any sense + thetas = [] + for joint in self.robot.joints: + thetas.append(6.28 * np.random.random() - 3.14) + self.robot.forwardKinmViaPositions(thetas, 1) + + if self.chill_reset == True: + sensibility_check = False + while not sensibility_check: + thetas = [] + for joint in self.robot.joints: + thetas.append(6.28 * np.random.random() - 3.14) + self.robot.forwardKinmViaPositions(thetas , 1) + if calculateManipulabilityIndex(self.robot) > 0.15: + sensibility_check = True + +# for i, joint in enumerate(self.robots[robot_index].joints): +# joint.theta = self.robots[0].joints[i].theta + +# NOTE: not needed and i'm not fixing _get_obs for more robots + obs = self._get_obs() + return obs + + def render(self, mode='human', width=500, height=500): + try: + self.drawingInited == False + except AttributeError: + self.drawingInited = False + + if self.drawingInited == False: + plt.ion() + self.fig = plt.figure() + #self.ax = self.fig.add_subplot(111, projection='3d') + self.ax = self.fig.add_subplot(111, projection='3d') + # these are for axes scaling which does not happen automatically + self.ax.plot(np.array([0]), np.array([0]), np.array([1.5]), c='b') + self.ax.plot(np.array([0]), np.array([0]), np.array([-1.5]), c='b') + plt.xlim([-1.5,1.5]) + plt.ylim([-0.5,1.5]) + color_link = 'black' + self.robot.initDrawing(self.ax, color_link) + self.drawingInited = True + self.robot.drawStateAnim() + self.ax.set_title(str(self.n_of_tries_for_point) + 'th iteration toward goal') + drawPoint(self.ax, self.goal, 'red', 'o') + self.fig.canvas.draw() + self.fig.canvas.flush_events() + + + def reset_test(self): + self.episode_score = 0 + self.n_of_points_done += 1 + self.n_of_tries_for_point = 0 + + # generate new point + self.goal = np.array([random.uniform(-0.70, 0.70), random.uniform(-0.70, 0.70), random.uniform(-0.70, 0.70)]) + + # DO NOT initialize to a random starting state, keep the previous one + + obs = self._get_obs() + return obs + + + def close(self): + # close open files if any are there + pass + + + # various uitility functions COPIED from fetch_env + + def seed(self, seed=None): + self.np_random, seed = seeding.np_random(seed) + return [seed] + + + def _get_obs(self): + thetas = [] + for joint in self.robot.joints: + thetas.append(joint.theta) + thetas = np.array(thetas , dtype=np.float32) + obs = self.robot.p_e.copy() + obs = np.append(obs, thetas) + + return { + 'observation': obs, + 'achieved_goal': self.robot.p_e.copy(), + 'desired_goal': self.goal.copy(), + } + diff --git a/manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..cac3d1c8e224c623351da026775d35ef4b973cbb GIT binary patch literal 4522 zcmd1j<>g{vU|^VdKrMBpGy}t95C<7EGcYhXFfcF_TQM*&q%fo~<}idnXhtZ_1f`k5 zG*b>s6stQ!3Udle3quM^GgA~>3R5tHChJR(shW(p7`6P8K}=)}w<L*yfgzP4iZO*D ziYY}Rg(-!(g)xdbg(Zcxg&~S1g)N1>g&~SHg(HQtg&~S9g)4=-g&~SPMKXmig};R{ ziX%lJMX-e-iZewhMYx3_iYrAbMI=SEg)xddMJz?Ug&~S3g)x{xQ~H*OXI@!qQE{qw zW?pJ;Vo7Flv1?wLUotC_t3Yg!!<<3BFJfR|C}BupY-VI+NMWjBh-WNe0{O9*v4$a@ zIh~<~A)Y0LwT2;{H3h_GOJS>Fh-U|r9AH^aFv$fbxxwN*CA?XD3;0sl7c!=Bh=6qQ zr*PIV#0!9B1ydM;88o^4s<>10$`q1|QWHy3Q`~N`<>kj0XXa`$-r_3CPs%Te&r8fr zeF<`E5hDWw!%L8ZpQnMrGy7YtDXDqIshUE!SPP0W^Ga@UCud~l<ir=H7N?fn;tT>C zP?T7Hixq6vE#Az$%#!%R;&=#`JtZ->ATuxh7H4WvQGQW;S!(hvfxP(qwD^Mj%)FA~ z_>}y-)LSB8k&>d!)Z+NG{33|lEtZtTlEhp51@Wm6f%t-){F35ZBB?n!nFYo9nJMwb zrA28_QQ6|e+|>BulEk8t%)IpYg8bsllFa<Pco0fUyTz1OaEq-dF)t-Q_ZCZfeqzoo z?xNK4#G;h=lFEYATfC_SnZ@}jsqw|h`9-OkthZQ-Q*+X8apmTxq~^pIB$i~{;sJRq zGchN=upqzq7H3|3Vlv2r;#*vK@x`S{#U-f)#VZ+#6d4#8e&y(A<maa9r{@<J<fkMS zfq6y<Zcb@lihe;+Vnt@LerXXXE(&r|<CF9AN{aGx^m7yQG7Cy`5=%hAm06_^4#@c8 zlG3y^eT)>RS5SG2BR(GFw)l8aP%4vSU|`^2WMkxEVq=6tCN?Nrh?$Lri>XQ$!x-1R zGCi0n$t*|-n2mvf0hAuV2{?;^fuV*Wiy?(Eo2f{=gt3Mpiz$n_h9QfknW<Q)gmnR1 z3e!TyU<NRY8N>psX31tMGA?1SVXR?lW~yaD)2E-pTEim3P|I4w8qAQw2Bkp>m)-9c zTX8{RQE_TI#6=(WLm4U#MVz3N`v3p`|1VP*7#K7;ili7A7;bSvQdvoAaY+#`NPxK@ zKD9`afq|h&9K>Z$%gIkHF*drz3rYj=DVfD3iFwJXnruZfAZgyj<c!SJvecA#kTh7| zEtbr@wESBfFr~NHAn67i(GWrjq?Io@KewQ?BsCr)Z3v1A2?hoRE+zp+E=DdUE=Dd! z0Y(lc7G{?JEQ~BatCX=u0;(w}Q2~k;5C%tu8Z;^vFxG&hL7Jh4X(1CMLk$B9Ll$!i zqd5Z;Lp(D$|1<f$1Z8VY7D$MQr{x!c625n4UT#=sVgNLw6c_P>LP-b|m@JUcU@MGI z$uEIO++xklOG&MOco>w%i@?6-D$dL;$Vm+@Ni6_73*<f#Mj^&3d91!es7_{rL@AgA zB{ER)12$qBlFvYOMJtmeDA6%VGt@AqFiJ8Y`H~6Z%Nm9SEDITGnNpZ*nM+tfNiK`6 zh9Qf+nW;!Og{6dJ0cQ&9LdHzST9!O^D4z|&uVGlgRl}6R4z_{4hGijhEh{(#xocR# zW^>mt)v(qu*RZ6q1T$!I_!X&w0?B}Zfx+z-S7vd1acOdLYH=|%S$?pGG8h<&v_S#M z0}k>cP+~v??=6n_^wg60{G{R{b&zQqAOfC5i!?!8eGnlHG6I@1*&wCOE#~~B;#(}B zLbFH*q(%=!D8dPFq_BX>qF`78RAA&|<X~h0B||2*|6Hsr|3#Sj7<m}0B(X<PNoqkd zD7}DVhJk?r6fvL(0!PjfPz4UEt{@4GF$EF@j42>7CUC{XoX$`SN|G#DtS$_(e6>t9 zj0@OOm`m8RI2Ld$WDsFUVUb{1$jHc0C<ZDy7I4-ura+T!7FP;uHd9ed3R^964Rdh} z(I#1=npDCK;x{v<uxGOrMWwKUOe-?2VOYSkkfD~P1{#$hPZoiqGF`s)b>I#;Ne3u{ z(;>ty-53lG+iUU`f#RV^5ERCs+)*S9Vu1@6wk&WRc8e{spdd9b1zf}wfigWL#<@Y| zf=qH^PBN%8PxXbC>q(h8nI)B;;Ot}uiegr1b}Pw9ElDiC#R@j{mSAyeUU4QwaeQ({ zYI1gw1;}Q!$Yups#2KL21|=jmW<GGK&%(~bD8<Of$i>9M2!aqEABg>ngNKE&N(yW2 zgAKdIno*jYnD_DvBLf3U2?R<C91IK$;G|&0$iPs=4$5ty>VT0UoFR{gg`tG8Qkane z1i?vyA(A1Fu^g0tA(ci^3DW}Rg$#@gB}||^3u^1sFl4c2v4NUtDJ;F9RuRZd_AHJP z&IMd4tm%vk8PQm5Ah8to6pmgNP<fYyPc0|1+7hM(+$o$XZ0Ss(<X6J9fG33u#!KO@ zWvOAv;$6T8DmiLcK&4CyPYpvn$nCrf_(5(4$t(~k5v*az5^82l;S~ob;}T(zcnw3A z2&e(s3~>`{mgoX8s2&ho9K;5f#sYqBFY`eOgUL7r)B-Cg%Fjv#w<N%gFK7Ys!~Pay z5~!h@3=@D=eGm5EVol0POwJCek|@bg$Sg@MN(8k-6iV_zrMd#B;=09Ll$vsjF<(>S z7H>*XVtHm>x@TTyNovY1KF5-hqRgbyl2lhvbL<v#K~Bjn=FI%OTWo2W>7_-fx0us1 z({FJnrhpn(NuUPTEvCeZTPz?JIDOw@O)M%(th~iik&~Hwi={FrGZ$RQ-QotdL0llV zL97ifNi0cq%*)KZ#aWzM5?_*8l9PIixwxdL2$YX*ae|x{0Lnp-ik2-oF|RBURJwth zlRRlTrNtTXsb#5oCB>T1EW!e6kKJM|&rB)FxW$%{nwg$aa*HcDKPSH^J|{CT8@+mB zD@x5vNiFII<qj<d1_mZZP|3%|$n>9!nS+swQHV*0Nr#b-5tJ_k7}*%P7{wTc7<m{4 z7^|dl<PVT}DCHxlB7<j&0KDa63G)ILNZTldWg!zbRyy-SP)W*O!vHEPYZ+@mJq=br zaMFO3Rp4fM+Oz#dAUicV(F-R~E2k(FR8D}(p<C>wd6{YXMY-S*E&`V}OyJT6Ygxht z&i?VB*3(i@knu1uFt9O#3KT774i*+%wK_r<O29BOFff2HxU>K@3_$6BC8HlWk~A41 z&Ok5Cl5_HlQ#XSw2ZbvO6OM8UtPG_M0a6ZcsEUDmTA;SkLdF<SnZgJP%5=tHh9XdY z(qy{DUYwem0`8aO6~sgO1)7kM0X6+oQ^1ad5TH7>D4T(SVHe0*Adi5O3a;uiH8lk# z6hP`h7#s=-3=9m=N{u0l(S;$F16(~a)i7l-gUX~rwi1>s)*8kVmMpdu#$G0HMaq!H z4o(crB`gb=YM5&n7czk=G*HP4k*Q(G;smKkVVc7n%%I8ar^$GWH3ihK(PX~Gou5>k zT2uxuBfyQ4TRbVL#hFFWHm)WYIQJG=gOZF5s9a(NS3}^u4oOJh1_`Kcxy6#4Ur-57 zN}7-+Vv!w4n>~oYQa6Fq+kQ}>i7+rQurTs4@-Yf9@-Xr+GBI;8axk*}#nlsmwvRMf z{QUg<G?|N>KsJCvun0_m<B%8B?*a8(<Ku5}#mDF7r<CTT#>d~{iH|QVP0WGF6e)vr zf?HE2AQor<qR0`%0`(e-(m*Uw-3g9guzMkKj+!isKnV*}W)*`H76$_tGlvX^5QhMV zD3>ya6bBy{I}a;ZjVAjoQKasg9;m4upPiYPdrJgaI5V#dA}WC_nwFoFlV2X6Tv}9? vdP@*lrnDq8r}!3YUTJPYB_v`%4#MWrTO2l!fUpDQykbxW=3s<EW+5&BFOgx) literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/InverseKinematics.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..e19567ae0945d05a0ef9d5b7593f2eaf8ca7e1b2 GIT binary patch literal 7195 zcmZ3^%ge>Uz`#(sR4w(tI0M6D5C?`?pp4HR3=9m@8B!Qh7;_jxAT%SCW`fepV45k1 zC5n}a!JQ$6xrHHxWf>C#!)hj|DGX6;DNMl(nyfED0-B7s7`6P8K};BiDTG>=$`Hkv z!Vtxj!kxm@!WhL2GBS!Kg{6fdiZz9`1!Nps3qurp3VRDf6h{h23qurV3Qr1W3u6>l z3Req56n6?^FoP!VEfLSWvecsDRPW5Z)ZE09%;aL%yfVLJR+zmE3=GUr{Mn5W<kJ#H zgj@>aGDZf5)o>9;h7_h6hIlx?1j(Ef<~1y+s?!;37~(;ugAGh)s9}hQg(^b|YYjs@ zTz3jcjt#7gfq@}~t%f0<9fijM<AF`#MB#zs5F`SY;|8%n7;Fj;hy}tWQXmq7v-lu1 z1H%G13nl`hQ`k}ck-|~M$^dd9KS&mYQ#flF;srn~5C+>M2x5V73PUi1CYN6ocWPdl zLUK`RVo7R>+by=d{P^O`TusJXTt)dw`6cmriMgpSK><(%iu#wJV0xYg2G8tov8JTv z6{l(n-C`{$%FHXd#hsjynUfP=lv<oxa*HzvY(P<B`7JiES;e>bGV?M^;tPx8p?vm~ z#N2|+y!2a~sYONkMe${+$+raZ;`7tu3-UAbN{ZuC^7B$}iGW2)iZWA+<J0nsAab`@ zQW8rNZ}Atzr$Pkc3v%*Hif@Ue=Hz4+6z6BA#21$qr9nkyixYEG<BLlYi%K%{(&G#A zi!)0y^Yh|CC@t+4Q(nO>wxYzml>FRVEa~})Ik&irQp*#IQsPT03sP_KrWRxt=clB` z7boWzrE0R?Vku6|NxQ|Bo1c=J6JL;6l5vX%<g?7gocO|m{Nh`jdGU$KAPb6baplDq zmnIdLq!ttxgMv{31b*e{XXNLm>Zj)y7v!fT7J+$22yRYkUW$G}QDQ}Av3_Y0C_)Ny zQsa~J^Gb^HbM$i)^D+xca}rBHA(mOC4-U-u;*!#|G<}S0q*qXRiz7ZB<h=O!Dp?G9 z*Ss=4n6xMZ14Ho&1_p)(hPxcR9k~}dWUg??T;PzoA+6BCa)m?Uskp)lj|<|a7sXAl zh?`yzGwWdK;l3dUmXy9BF44i#!*fGYwu7aI?}maRNQ>+P6;%+e_&`LggQbV_hCEnU z=7x$ISonsn9*EZZ%EllnKE?C8h{`1ql?8zpMbxi|sIO4ID549}^?`#yMj50|;(@Fj zh?cs*0YaZa2{{>_Ss54@Kx|MN{cHp-Bx)G4KuW+kg)tkHgc*w5OF$~XVxa7j1r`Ny zv*6-23|SzPK!OaY1!S>E2|VjEFk~SL118kW7|eh}9W%N*6gRR!-Dp_?&%_K2HH<Y( zsII7Gp~w$xDXcXtpu(b-HIFHUt(LWhHJAZhAh7%0Vk<64EGkY-2Ya63!+t13#i0mP zO1=F5|NsA&DGUq@njA&43=9mnxFC7AB(=EY7ArU{-eN9@Pc4#RU|=W$6=%2D({l0? zON@<f@q+SJd`f0<Nn&1dswP{JJjejv#N>?3)Uwo+c#t$$-z}ERytMpV95AJ~*dY0_ z7!=;19HgL7qyo~*mz<wlP+F2250S4@#+txU6&Zq(`7CfUf56Rug<E<?#U*a_3*72= zg=J@?%*mRQw<2Xt=0#zPE5a5Z7?^oA?utuJsal|NQC$6sxcUVF^`~h38xT{<FLA3~ z;8wdUuCgF@gUdy6%PZoR9UfQ>W#N^5ASpc~W2Wo`wh3$xBxNSB-4K@S@cqEgz@vVF zTdhcqfq?;g!ma?<^T>&&hFKC4R}2gc3*dDgT!;ajfEW<Tyo#HF0X;!6GSo0sFd{NQ z3S$*714A+sqFw|i4<^5tAPY2Eia@o>E%CJcB2Y2uotc*#mYEm;Emn(*gg|i*Do!+6 zAaTrA7@v|~0+G1Inwgi9T2TzjVW7B##3Wa7W^O@FYH&$vL6tn#SVSmr2F0KQI0kQU z@bqv`aJ|AIb)CcL5{J<SvmGvbd@dS$oKU~W5p;zk=mJO34Gx|Q9Ew*s6c+?u<WPe! zFK{Sc;828wGrRy`U|;~1(V$}B^A3yx1XtLC+FY$nh&ZT04{OF0MuaSCM#cztCiHNx zVORhPDCFn_vzIY2Fsuf%KtwH53Ue(pa*>0aLJ-AN4MP^Z#e(XRBJmWK5_s!?fgy_% zWG)D&u%Z@OnT)k8d15$Kvtg>PVORif{eVn_;TonCc0|Y^yRC+088ZXJYEVpo71y$Y z(<i*>tzm_R6GDWkhP8&dh9!+9m_d`nuSgS=1kD&27~F1gWfsR5mnJ8t78gScvk&%A z1_MKp0jMnFDbfL{HwO{$#CwY)K0UQ0K0m3rNE;-f3nJjfT9F=zYXTzVKt@1IGd4)g ze~URksrVKPr~y+1DsYO7L1`Af_+kMy45}otC-;)nf?$xQTkwR=b)7@&5{K3bvkl4{ zj5a8*vECuFN9Llo>w&TpNk^*iruQ3sLjAd2xt;kv`5nv;_(eKc?sAJxNSTr~C2v8> zlFW<T23NQZJ}|Iz@;wlcnVU2xYlZO4{5km_n3+WQI$SzjK5#Jz%3_FrU}F#xyDq45 zNl;}$@Cx}Ws+K!U_E;WBxhUv;MbNv04V?3&u5d`rNV~|Pa)ATHyumLt!EJ`i0+kE= zY8N=5MIESA0Y@|g0|O|KKoS4hjTzL?PXRU0VRDdS3a&Z@mR%4cAl2{;4Q^_K%mV9A zXQ*XFuGx{ZWfmyogH^dO#9G!e)i5pqxdSYMOr$WEAT`Fb;0^u-a2B#^)JzJhaZ^~T zxEL5vvj`(Yj}<O=Er4f3WOHj6Q<zY*FC#+@Ll!)frLaQV5mQsxYME=8izg0mzn6$3 zLI+n!qK0`2dp5XOEn-Sx1%=T1i6w}Vh#`fkL>jw^sHULCfeS;dXDv$!s8s~jjO>;g z7K8<e)P`!F3q!1W3=;!GEo&_kvR>>dwT2bRM0k6#hAAG-?$PM+0$1D|e&GIF5vV{+ zmv4O?xI<3T0m|TX2ysg{27|-)ngT_j(yT}XRIY()@FFn~3tZi^Wr2HTx7ZR33R3e@ zz>T{iP&gHV%S}+exFwUEn3D|ZoTU0fyD3STIhiGuo}eoK7Hd&rUV7>+=A6{LTb!wR zrMam^i6yDGSW7ZeOA=x2K{hb2_!cYJx?6(9sd>ej5L4olGg6bYZ*ha0nepIe#4W~5 zj8-8lxCdG#g|%D-%VmHH*6oa-uFeNG203|frvhAB$zI`*T_AXoL+Jtsh<O9rf4Kqa zzkJ|h5EAKNyCEt$C3=D2l(;LRS|1n~Ic>p2hwBYq;r`gJ*cm2Em6m8;S2DV!WOPx< z<cgBXMQM`_B^RZwFY?-4;kCH{Mv$h9V-MeT4!uhpdMnB{6kg=8yux95fx{BoFfF;n zt$cx7`G$h(1s>TOV(K3l*m!ksNJ>v&yCJJMCwhh8oVY8pMjsd$1zi~-<OJ6nqSDi2 zC&ex>S*x^0^SYMRB`vFqS~gd-Y%VI>>?paY?0iww<%+1w1u(iHATot}hT{~z>jGw% z1k5&+9bmjD;B-a6>4JdMU19wVEL*sDIBwy)C~SX4*#3aZC1JM<!frP-bS{Xfe&A&g z*8n@{s}O^X;sU{?&MR2fa9>n0xuRlnQQGv1wCM*1CQ#5YaoTox-<45akg_CugZ33Q zyDMt07iHY8$hduAVB`gb9k1;~jtPttOmE03&B<P&I4AFlobd+7D{^L6<ZLFePT`)w z{ec;zk{M<mq~QiH^%)o#K;<u}r2lLIZn#lWYm~s7NDN5*z!G@r%D{lDZa^PSqnDm^ zv|c6hz*G$bF?CZdV+|u{;19W+01xw|J=<RdYGi40q8AL{z8z~~QBh(gxD>d>UYeJg zmS2<$?!pz>f*M6k;O2jkJt(_@YGqizjSHL$<3XK99IbtXf_{+Z7Dyuq+Wp6uQz1hM z9&^*?=grDn5xT+UqMYRwIm^9f`|Wnwolw0H7<$n<?22{R)rh$35ownq(k`UuT*xcB z7*TvBqWFSr$wiUUD<Y-PVF!L;M62lrzfcDkX2Js{LU6(ZwHH7mu)z!~8U0o=XflHR zUJR0l_EuSwbMlK*ag<A7xw#;}f|6eY!vhYU4i<=Yppb*sD2QQDA8<#o1SXEB^M_i; zfa(|I!BJ4mr!xjK6orB#>=t`*YHA92P%f_^9x^DV35mvAEFh_3kZYjvSQO2`z<{g$ znwpxj1mqoMaMazESGz86a7o@^gUCgBvn%pu7dd1=14T1fZU~5Uly&ezyb1~}n3q5m zG&p1nz*!pFgn`L2WWm+CFvQA$n`|Jb!R29$8YV=)7Bwgs8G2+(IAQ7-7!cKR4PyzY zK!Hjj`cG>ZQLB7#gA%HW0jY0ARF-2#u^aC88s-{wTbOH@QTlVB6au!D7<+1v`f|jm zPhpzF9L%7}?5D|ii!}u_0;<V;i#tE5IJKxO5j3g^?lRrtNl7ivEP{^OXmUY1P@tZ9 zkqfBB;0Yo?MMO~`hz0H}fjUsPSd#M#D#1krba0@^3#7msM1ac)P_)Cx1;E7!u5n9f z?`$O~m4T941H%nLVMy&9bwyMSR4J>2i4NBrJVO1xUA{9^=V)K#QM<yUb^(m;3JOn= zn4z{-XpPhbL6eJuCRYSaI@oUT3H4<4<Xz`eyu_z?kx%&wpK=HDT|R-H%o)rx!Y=Zu zT;WscVE(|yAR`Z|hok1iU6(buBx`U{*650?(G3}eIiB<VX8B!|(YYd{^MRd3fa?PT zgy4h}Aet<Getv$C9LED1p9KxG$H(8|ijU9DPbtkwjgP;@6CYn#nwSHTDN+Rm54f{w z1!6gZ2!9X(>LM0}gIJ)v2oA2IFc23sgajU1DFStEzyzpgTMX)xG%>Q~>-`40@J zgasd~)CUGQp~A$f{(%8bC<uVKa6(I-)%F7eobY4hW)=Rx04HRaSb0I3p#&cjs~Cuj zOmOkAa(!UHB)}yC*x{P&w?rXrKJYNN9%$StK07ln_m&8<aAsZ^L{tJ<G%Y_TC%-&C zxwNP(^_C#AOle7GPVp_)ywco)$|7)D1qT&s@crVjfy9YjQ6>We1E|m`u4iCi_`uA_ z$oPOk`2q~xU{JaMLpK=IF2K+Y2Kfsx^ns0mQTPIb5CnY?NoHc?`oMsl{0J8R0wyul HfCC2rDB=i& literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/drawing.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/drawing.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..0ca4ef1695f5543b39961ae380c249e8d247006a GIT binary patch literal 1424 zcmd1j<>g{vU|@)xtCkwT%E0g##6iYP3=9ko3=9m#Q49<WDGVu$ISf&ZV45kHIf{i5 zBF37_#>BwLkjoy$0TyA(;mqZV;sUc7bGUPPqIek@QkYX%S{S1EQdm>iS{S1EQ`l2D zS{R}PQW%37G&x^_Z1vM*yv6HQnwRXDmzkSbl9`{E%mNaJVvtkW85kIxK@OV0z`#(# zuz)dzF@-6Gc_DKRa|(+zg9t+{OFBae>mtTl))J-^wiNbeCKrZS!&<f)rW7_wh8o5i zwiLEBCP{`G77+$<hFbO-rW6jSI0uqAM-5X73sjs1Nu0BWA&a?&y@sQPvxc>XO_Cvn zvzLXDp-?1+A(%mv%kMeVtqf4c2m9x<W`e*@`<I}=)MUQJm~@LV{T5?U5hDWw!%Jom z0b;FWyv3TFpOast$#RP+uizG2Vo_pVdg?7!2*py6lV76Ab&Dyn;ud#)QD$mh2{?pq zF_*-b-Qr6uOOMaV%*&3?NzE(CxW!srl3GxFi#a(z=N4n;EyjvljFq<-t5z};$uKZ5 z{7TZ#$j?pHPtPwd$WKWu0`rUz+?>+96#ast#EQ&f{nDcN;>_HFoYeT_{JfH){2cw< z#JtRc(wxMS{G#}>%qsn&{G|Mn_~Me%v^4#cqQvsdymY;S%3J&(wm-t59H3YeU|?Vn zW94AxW0ZhGmOn*u3=9k?5e-tx28-wfw1}={fkh2Cf*{ezTEmpW3W_c;2@!)wIXL1W zV(c{xSxlgat6{HUPGJW{9XR4R{E9#kQ3OhEMXU@A4B*Hq;s&umE-B(+U|`T>F5&|T z@Pi0JkN`)K5Ca3lEf!G77m0wFT;M1KM@5ksNS!2zkOC3XAOaNp;5aCf1uF*yQdnwo zNq&(8D11OkoR3L_5d_5;ixkmA2IMr58gLdcVqjpXVaQ^rVN79^WJqVMVN79y(#&9* zsfG!ZEkHT0mm`L;mZgRzm_d`>56yGwkTfgikbd?I2)wb^WWB|on^=^cT9lTUoC@|5 zOdvHS9V);EQE`h4LKkONrQTvq%qhr7)MP9I2M;SGco++A@qo>U&&(^%Oi3+L0ePLZ zAU`v&1QHhDPyq)BCn!JyKr%|8@Bqb!9wQ55krpVp1#hwDmF5;yf}`P<U~XawC{yHQ zCg~Maf>^iMpfqzqPRT9l+=86=lKlLf?97s4y<Ct2;}pHbiqvA`lw0gj{w+4B08@eS zEfFLW6JbSA5h&ZDSO)e>@hvte&61p-Qv^=L;P?VN70Fu&zj4^)=BJeAq}qY9dod_0 Va4_&N3Ni99@-T8Ra|rNo0RXr_S(g9+ literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/drawing.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/drawing.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..60bcda8221c70f3842b39da90028011be8e56d07 GIT binary patch literal 2272 zcmZ3^%ge>Uz`zhWS1mPwm4V?ghy%k+P{!vl1_p-d3@HpLj5!QZj9{86mpO`s5hBK# z%a+W<$dJn(#Q_#!%HhoAisAyZ8FRREd7^k38B&;A7^3)6SXvmO_)}P07@`DH7=sx! z*<OMy@zZ3y#p_m@m+Y9AnVVRWnV*-;0uqN}4h9AWW(Ed^&w7j?hm<fv#TghFvf%6# z#uTO$=4H$b468wE!AfeFQ&_6F85pWq85n9=(iu`%7cthdmau{)!9)sM3i~o928Puz z&oa0$#3t9W)i9;7NrF{?h#JNkwiLEBCL|FSkeO8s3=FmGHB2cSNHQR`95~c+)G(#6 zps8iSp_a3TVFBDHAXmU}4SNko4QCB&4U!Aja4utEU|0<gHAaS>loW<w22C!%=P-Rx z#s~Z7vu1+8PWzW2Uu!boVobWln0||~s0hS=39|GhC|p-E-eOJ8&&e;+WVywZS8$6h zu_!SwJ@pnVgkmYk$uH65y2X@Oaf>^@C^I#$1RQs_m`mczZt*3SrN`%F=4Hp{q~?`m z++r;*Ni8V8#hjd<bBi(a7GuRN#>!iaRmGt6rvL}PlJqn3b5r%x^NS1eQxc27JR<}* zr!+4`zo00wBC}Y(v?#tfGq)foH9k2%ucRnHM?W_)FSDRDC$S{ID84MSO1~&SDZeDX zxTG{KO+Te5u{<*`U9X_>7C(sXk8p62JOcv*D3KK(VqjosVED?#z{}s`IU#w5@dD<H zJStasR95iq5WJ{ib&<!q!Sw?h1GiL%eusX8^Icw{35F8_JEMA{J}|Jbs)LCR#v45R z6Bv8kCPXg~Twu6B_=>3N3f7CFx)*tLDUjpf>Sym_pTIc5a6;mY;su6t$~xFDa;RP5 zP`dy|A2?wS_za41lpMqc%RzU*S(*48RLg>%Vo}mBA{VgMFr}~}r)(4+LLElVLdj<c zb?h|^3qYw6<P{VQO7}JFHOy<+(NjD)y>s{#fwFfI$j3$O3=9n5G+o3CVu7N#h>wAR zL6f;i03;v?B7{K#97Q4w3=FqeKnb=;48-IDr+jb<E|LJLlLiqoAVQXbfuR^=7%bTq z$$@o%l2ur0a!GzskqQF?14vhKIXFc<<>5n3PYtd&IJi3;d)Ow3Oh}v}KErgP{6!9h z21k-aS=id$o7_7bI|?T#&ai55zsRC?g+=WG3>7IbFfgDb15jQA<?hdU;ABw4umB}! zVVoMq6h=f;rZd(srXUr4U_LX7JW~x5sHjb0UBkAFgMnc+C~Uxn#V~>j-Wrx*22FN9 zw3tW-t6&gwNI!c91m4(dvfkp)O)SbzElNvFP6dZQOdvHS9V);EQE`h4LKkONrQTvq z%qhr7)MP9I$0aKyE*T4M@qo>U&&(^%Oi3-$07VjOL4IalNiismfMX95b)2B63joO! z=`k=cD1jo_4;*thc=`LoyTT_V&S0F9G9he6;sVAwDKo+rB(7jwlCmIdN6dxj^a}-L z9~c;1nB18@FfjNt1u=Dme`H|b3ugKPCL7%vTyJm-H28ocL{sn<YhGz?K_xf|-V)4B zECH3HIhjd%1(hJyEjB34T#!?8OFFk8C%z;<KPNl0q*yN(q`){uFR>!E*f`}DJCuKm z4JyD?V0=pi$;3oheOd&Hb`;CNK~sE-4N9{l=jRlG3qo+(1qZ`Q2C$34LG+8mCO1E& zG$+-r$cBM|0aQH{&tza=_`uA_$asT6=mHGgV35Cnif%AyT)=^DFz8)CMK>4}E})_h cY`l!>9~j_-0vm`6Cq$SS1wSxg5@4qT01#pxO8@`> literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..596473b77c62ef13d20202dd981aadf8f95a37f7 GIT binary patch literal 1220 zcmd1j<>g{vU|@)xtCs4_%E0g##6iYP3=9ko3=9m#K@1EGDGVu$ISf&ZV45kHIf{i5 zBF37_#>BwLkjoy$0TyA(;mqZV;sUc7bGUPPqIek@QkYX%S{S1EQdm>iS{S1EQy7C8 zG}&K*Z1dA(yv6HQnwRXDmzkSbl9`{E%nA~RVr~Wo1`yvF<d7K*3=Aa<3m8)vQ<zei z7c$qdq_9Xch%nT$rZc3lE@G@@D`84uOJQ$ja$$%yt!1xaPGOT|s9~&OPhm@Al4Pi1 z6=4u(sO6|(PT_!xb0CRx)-b2AK*d>*#JOsiYS<+i7BJUv)Nn3jVq}2wS!%czGBPq0 zili_EGiY-9J%_rP0m}Gb|9sX=5ZGz|5)_!4%(obmZZW3cVk|0RWME);$qXVuEKSy1 zOnC*j*b<8p^U_mqu|g=0;?$D(l*E$6TY?~#aY}qaQGP*cQAuWMu_pH|ro@U{-1$YB zsd**faJwal#EH+z%u6l4#at3!c8f2uEFHwpj?YQWE6KRUT3nJ^P<)FyIX~wXW9BW! zid&48w-~EdG8D-$Ffjbe*3Zb#P1R4&FD}SWNh|{Mj1b(M(!3P?f}+HV%wql0qWI#> z+=86c_~iV&lA`<^{oKU7%!1OK#FG4?__E9@{i6J&{F3<MlG3y^{gk4_^31&S__X|@ z_{6--T)l$ITf!hge}pGMsel6%*Fp>o3}UPt%zTUzU?{*Wz{v8mNREMl0VRQOA}5d( zv;<Pi3X4*3#6qH<t%f;;6%@^25+VjqD&PbI5#y*~TEJApR>Ka7a!~Yxq8l99?0!X{ z$SY!DU|=W$rB85#7J*W05gP*ogC<K64@im^MDT+M0gxzXkst#D!!4Ga%)IPdoFEbu zFR4W$AW^R5{G9wEkU(~k7)XmGh>!vi(jWp9Ip7#7k_9Q}1BF~zYH~?_5h&0=>WV>j z@-c}pf*=nQA7hac$X0<{ta+um1(jeY-4e`AEGfvzFUiSF(krM0v2L+JY372Ql3UWb z1v&90`T057nI*+~xgZ6`DSC+&sl~=Ax7eZlTWnAPrUK(zB1k5J;uV}4i$EzJ#WJw> zif^$&X_n;toFZ_1f?Wym8JZtCY;yBcN^?@}K$)bNkAZ=KgMo)ph>?ephmnJsM+gAL CqcKYW literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/drawing_for_anim.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b39611c8807a0497f50ad6faa2f76392b6793fb9 GIT binary patch literal 2032 zcmZ3^%ge>Uz`zhWS1r|<m4V?ghy%k+P{!u~1_p-d3@HpLj5!QZj9{86mpO`s5hBK# z%a+W<$dJn(#Q_#!%HhoAisAyZ8FRREd7^k38B&;A7^3)6SXvmO_){2z88lg6f{gdm zWW2@eR+^XWn3tKGSdy8am&^(hhhh+$nSp`fa~vbc2_=kBaRvs4EI2!bF@-6Gc^NYU z!)lOPu#y^<6qYJ(28JqD28LSJbcPhxMU1s<B_Q*_@(c_qY$@!^m>3vV!@SAh!Vp_s z%U;8r!X^n-!@y9(Si_#emd1o6!U{68ih+TlmZOF_g#$?jq?QARTFx5g6c#kKEI8D1 z)iBktBh)T{`wH$X1_p*2jv7u>_cJn}>0?1w4`bJGEn{S0SPc(7Muwi66oz01O-{e( zFf~xd2m9x<W`e*@`<I|l(qz8Hm~@LV{T5?U5s3d1<gk|@lQdaxG36E9VoNMa%u7$b z#R{P~ic?GCQxZ!OZwZ1}#wqayMfnA(MJ1W3#hTo=m=Y^)apxChrskD^WB!&P5+^<< zGcUFH7IR5_*)6`rvUCtXJ3c2huO#CZYjH_xLGdl-<oujljG4C>D{e7X-eRmO=4N1E zP(XlR+4>pzxvBc;`NakKDTzg3o)LnZQ<|5eUr>}-ky)%?S`=TLnOl&P8lRk>S5lOp zqo13YmswDnlUR~p6knEErC*evlwT5GTvD2rrk_%jSe}`e9-o$96rY%vnX6Y&c}o~1 z=#TIXC_xv=GcYi4FfcF_-(p~3Xkhrt#lXwo<2fODhVcUCi##e<cvM#K?GU`EVRezm zy214W8w0mghkl2CgY#Wpp$Uc)0z0F6qCPONvl@Yk4#pci{1X^^+$KaX5L{rmK=_KN z>I&A2qPiD(bjg?dz{bESHAC?Vhs<>j)k_?z3ldidUgXfe!l8YEL;D7YBveZ85{KT3 z!iyY+S2zqWfYAqjn8QAU5)Sr^%Rpkrtz|{eC@9$mk@?tam{V90nWTmhg@;gwk)ctt zH$ojp4buWpq6T>d1=q0EpcNFLjD{;yqGv8})?)W70+l>PYzzzxMeHB~WK$6@hy_X> znk+?pAT~dU5CjoI3=9mKoJGP63=Fqeax(L>Z*hW1P%cO<5(9~HCFkel7l8z_izGl= zq(OuXh>!)PI8NluP$UP^!UsyCVX4U_`9<K=smj2>AO%v}4o;m<dH7J%W`pYu4(<-e z9<~W06B4J0&oG@Rf009>!I2cv2Q0kTS;Q`}h)pP*p?Hx+<_e3<1s0hbAW`{CEb<E& zFS00IVNtpOLq!T8p9|b#%`43<s00VgEy3Kxl7gK4lAO#Wy@E;*>lPc7W-iDnxh0)j zkP}~$pP!SRSyHT*3sPX5qL)~aT5Ozhiyg|p#Re5%Dlon!f@C5n1B1(-B2aOLVi`Dy zi*K<(X_n;toFZ_ZE&>JJN(QiF!D0K0!zMRBr8FniuE>~y0hH(&in|yX7(OsFGBVy^ z5V`<EHyGqEprRWL8W(V&8w`3EP|*zrg$t<Y1DgP&(FX=Np~Ata{DA?J0J{|cA<^Kl literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..965a906400872fdd6bccecc36266c18097cc08c4 GIT binary patch literal 2363 zcmd1j<>g{vU|@)xtCq^h$H4Fy#6iYP3=9ko3=9m#1q=)fDGX5zDU2yhIgB9?nh8uZ z=P*aHq%fv1=debxf@!uWHZaW|#Q~-{qc~GoQdnCUqPSAnQrKG<qPSByQaD=}qIgoc zQn*_fqIgqyQg~Y!qWDtyQutdKqWDvogBdghsw9&0^Gb^HbK*-<i%S%soSf7my_X<Y zYBJtp)bdMa1hJtQ#Aan+U~mRGT7ZFpp@d-pV+q3orW%HYj0+iq88jLFG?{NP<rUmw zODsw*&dj^TT%4Jw$#{!3F)uwQbtOX)I|Bp5uXO#4{M=Oi^!(z2{FKBZFwY3V%_+@G z(Jv@UtjH|ZFD;5M&de>yNsWiPPd_&?FSDRDC$S{ID84MSO1~&SDZeDXxTG{KO+PI^ zCnvu=KDo50ELE?d@|JW-X;Gdd$m)>%fSl4|-^{$y;(*Mc#Ju!WVUQ<4Q6Rur#L2+G zkjxDAC5QsC*<e1^0sAzIv4$aqQ3T{?rV{3bj3Nv*AT|qxox+sPT*8{d+{+ZqpvmI* z^8f$;|BFE3wUY4`Yf4FFLF!7jB9N)KSQCqi5-W>9;dqN5?9%vx#G=I9)RNSqTg=J% z#kbhga`F>PjEz<@-(o9DOvx-QzQvZ2nwg$aa!W9|w5TXGuLPk59HJ0{gMoqJmUw!; zXP%>TW>HRRSZ1P2YEfocVo7FMsv^w0I*dhPc)hCy_bw=EA>KpwY7JvLa|uHhYYHRC zvn!eWintjV7&IZ?E8=5dU?>8mjUs*!3*^lrK@dv_M1Z0n$+0phjt$5!&Me8y&vPuw zFU?DdQiHizi?K)o<YE?3%s?@S4e~EIDeA#o%mj8PEP5EhJZJ=oGa$RVNF3xEUXZ(4 zia;r$NCXsk%oXtlMWP@nG>0nTai~vbT1je3gf_^zpbRg<$iv9;pM{a_e~}c(*`PEA zbu|Mh1%NQv*;Zg@*D#haW-&=HG&9yR)i9+nN;0G{Wiu2>*D$3pBe7Xv>=f2uh7`77 z22FN9Z~{yJv-0C5-=qNhm!RbK5@cJ5n<n!u=ES7pTZ~1wm<!@lHJNX*g7e8O#*$l% zsgM`|dl3|dx42S^it>xV#ef0G7a&1CMioXDMi!<bWso00M#KC73N?7%kYEIr0$Ggd z3@MC@KqM2GWCoKgix|^cN*J=3QdoPLY8h)7K?PJRQwn<;lO#he6P(YM!rsaR76T<L z#uSbch6T)^G6kI5SU{{4HgNPoOk$~Fu3-^isAVl-$O4r=U==VuDV#_u*-97|u-34p zaHTOVWUOVYVXR>jV5nsWi!!CKfkl~W*lU<17-~6ExNA9UI7=87u+?y+a7i-MaHOzF zGAv{gVW{OQVXxt8W-MXI;t*%3;gVpe<*MOI;Q_fXi$jE=hEoJ0%3Z@<!z#j1%TvP> z%%I6zHA^8SKQk{~AyFYMvm!M`Avv=sIVV-2JhLQ20aU&zB!bhrLIs4El$xAaTAa$I zprD|TsgR$SQ>l=bnwp}JpO*^LnwXcOP?C{ZtdLo(keXOr309j}ly0R^te~q<l98$a zEg}@)rHVpk9#{e@P?VXhkXopx$?_Z)qeUtV3=B}-hy7rlCJUG<QUzrOkTjSJ*Ym+1 z%tsKKEa?xcMAo@Kd<4-C<JlL1vN5=X0H@ZMpwwRk$|exK(hLj?>9b~nz)kxi8Bm7g z1ecSLf&^T0fXfh2k#dVIqqrn7Ir|oSVo`EQVxAGW;J(FFkg3Ugiw6?6kdhI?i_gf< zjn7Rixy6>8ng=dkgA9^yi9r=+B_`)5WhUmuL-@BuAap!jJSnj_^_E~ker8@te12Yh zGE5;qk`P!8C#aOpiZ4q|2A3tb7>jQ)r4|<{fx-@)%fJLESKShW6(q^v^4SaO1`ANO z1JyHJTx^UW$n=+mneDFtiwL6{GY6v*2OlE`BOfydBMXxRlMs^_BL@=;W04ssxCCyo z$H%ASC&$O%5(VdRNL{FxmS0pJpPiYPdrJgaI5V#dBFdUqnp;o_4(nUY#g)akxQa`Y z3X1ZRQ;Uml@f9a$7F6ox7nEe?W>%%%Vg>U-)lptbel9rJ!L<muU<Ma+;Bo@&BBas< r;cpI`-29Z%oK!ndNm2}oXBGwyCJsgpCJt5(MlfXM;NxHgK^_hOUVcG$ literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/follow_curve.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..8bf02d5aa7da680701e2164359fa19bae13fb63f GIT binary patch literal 4189 zcmZ3^%ge>Uz`zhWS1py1kAdMahy%k+P{!vp1_p-d3@Hpz3@MB$OgW4p5Sj^0Gv_cz zv7|7jFz2vFv4Ux~C^j(79>oEsIiomJSXvmOxKdbK7^1jS*jgB(cv9F~7@~MnI9eE@ z_)<7q7^3)7n1dNKxvC_R^YcoI@^j)#Qj1Fzpq!l4BE6R&=V&tCV$||WW(2XJn3aKn zfti7U;j<sunI%YaS#UOrD9mFFH4Mub85mZ>RiW~N88jLFG?{NP<rUmwODsw*&dj^T zT%4Jw$#{!3F)uwQwHTyP;a9qTMt*LpetLd!L4HbN5twI$;O3O(rRWzFC01k>>z5YA z7iZ=c<fO(!J*J<Vn3q{lnv+<PUld=KS*2f;pOjw`UtCg}mZqPUpOcec9-mxVRF<k& zP<czbq_il{5oB{nen3uXv2SKxX>mYiP-0$sY7r<Ng&7zailrDB7#bMva`X0BP7Rt8 zHp6vd?3CDx+{#zDl`pU;e+GqGGAO)23=n3Ah2I)H;a9_u!dS)1z<@m*OV~gLK`?3< zg7nm&>SBSYV_?9qD}^bYxdh}XsPZ+;%a|A#R>R{im_d`p@8$ph|Nj?(#8xuiVofQj zEJ$6+Rs>2Bw^$R4iV`b}K)$=h4-Ufkg2bZ4+|-iPqFc<#`Ng-`({l0?ON@<HGT&k= zN=(TtExyH;k(!yFQF2Q#xwNP#HLnDrr5F@s3JMBEoD2*Mx5U%)J@Xu$GmCOk!!i?H zQj0Rn5=%16Qi~)R7#I{mvET%c1&aw`Gm<7oFEv_XwpRb5vegx3tBaymJKT;29SJ+@ zf6+DOifhb8o0to6@fSoAu4d$4;4ZkxU2uiF-~vlQ5jO(^1NPY3fH$`27f&^e>C7e2 zI08{wpwI^~7}hYN$Jk0Hzal;c1_n(?92E&NFfbH>5^a$Xhy{wPA`uWv6cmIgAufXw z;sN=^nI)O|d5%T-rFkh)MKTNw3~C@fj>y42H)&4V()f#VrdQ-lFN&IOC_Bn@gzaek zMThV!4&fKg!!JZaf;{U=M$rZC;)~qHSGbEWuoOc=9iC1Z7#Kj24sy=tZTLc+2`#0A z(j_<~@Tc?|Ml^kJoAK+bVqjn(KJ1I6K>;fO3VoI$F%VlEl#-b%;th%<KtTtxRRNyD zmGFeOPi9(4YDz?rJOcxRHb`qIQm6;dP(w-TQdT?MZU{-tHJW3#RR5y1(G_W<i$X>p znHg=3KQJ(gn%uRv-DCBEnUTkx@goC>`~oIFFfg*ZGeTkl9*ztQ3?Ns6W1<TqCTbW< zkW^;D*;QN&Xytq@Qw>uJBPg$eqlY1dDI3fvlB!`!Va6iPSHqOTf<>Gug*BKVg)Nvt zlid$o45j~B`SFr(Qh@zSP~Gqn<c1J8P3Bw7iAlw`7>jN(7sRJ(GT&ka7xT9mOKvfy zqLwUNsYONkMc~S>NRxqq!2lG#(87d+yTfrR;}q6T&rZG`z6Qq|>>M3}EhQbPO*IWQ zGZ<$m&S9BRI*Vrp&kDg6hHFGNu&$C>A#;OYxTE9&zeq<ZBrrkl0(+K$fdQ1^z=8RW z2~@+Q6dG`K=?p21ix|@xQkcLbGniyq#F);4lI=lW1)IBu6*b$}GS)Dru%)oKGNrJm zF(Cr6mI*EnmPL_c#_pCB4is^a+rf6CSLC2-2Sp_)*<e+f!iJWGaQlO$hPj5NiiLrp zmKDWpcu1j$;<OP(6;2<da1!k<HWU-##cvH;3RfBvYIR)8R>N4s2KECx4qZ$sY#6$j zYS?R-K-s94BZa$`vxXBzFTC=v;Yi^^#BU8p3LAolnkGQ;Q_EGt4~`NrQNxAmQk1mC z0an7mzyK=pYPi5I;i}<E;Q^V0LnSK%Lk%ZL9;%+ZhP#Fp#INP4;R$BY<gJ>ekdmL7 zm#&bgkd|4Inxc@LS(KcUs!*O;lA!=<ttljeYZrwI2rnr$IkB`jl?zlxW-8?8<y0!< zrKYAR<maWrv?k`ID3oMm7As^HE2JhCSAx|h7NuJ$6f5W|lw_nTK-(1x@TQ1DW*%4q zDo~V}tdLr$r^)gh=H((#2@B<Y*bnAuvVf@~9Z)F_k_L0(dOp~L`3ORjCH-NQ$U66j zk0APCJo_SDkO`m~8C-b11f|9zUXU2rR8T`Eeb!76xM^Rc1gbhX!Hp|O#SN~hK}86- z3ctmcQCyOkoPCQuu_(DDG0zCxBE7{_kg3Ugiw6?6kmeMG7oU-z8=sq4a*HiFH4oe- z3o=N)B?eWPm6)8Ll$n?p58>Yufza`A@ubA!)LViD`I&ho@%ef2$uNcdNJ3yWoS>#% zR(x4%GPu^e#aMicDYdvr15~bpyb6j=IEJ+D#9(z_GPrr?1$9f2Edv9C1xVj4Mo<&` zs|bUL*c6KkvPKYeL&op|7~PODxBx~sWQ;F>(Fb;BS<|m<47T=rygslqi+C`8WB`$0 zz+{8Z2Mz{4sTqnh9A_xcD4d~og;%k`?FN_Fgu)qu6N+az&Pbf$JfY$WmqLT{S2hMY zg*g!miq<-<aob>XQODtmj>AQDhoeeI)K17;bPBuT6m}sZ@`75_)ufawNjVo}b1%x~ zUXjhcz#?@+TJD0P`9*2-3oH^}1sEiy=Wr}lTB5c>=Ax3>6(zI1OncaN)LgXixnkjS zBKd-1(AB7zD^V#IBvLO*q+XFoy}%-RS3+tI=LKbx4T%?&EG|k|UXif8z#@7>OzDEE z*+nt42LG>Y415ATku%s9Fixnx$g6yXSGmFMhJgG6wTl884W18p1sdEQaPm)8nxZu$ zazWvom=%^A0@v7F6tcJ?WO0$xqQUVghw=i)iyUfKIMmh(tr5M*p|?TsB8O3f;|)%( zj=)aQsX|jkJ7s5xUF4Kspm>o}xxwiH2iFB&#fuz@4URXsc^W*x^_-@_E%x~Ml>FrQ z_*<gj+7{BO(o4%PDv!_3%*(wcf-IbwR|XMf%`43<s01hTTg=6k#kaVMOOpzU@{?1G zi*NB2CubH^>g5-dWaeg8rQTu%^FiH*yp;T0aEb?a0l*DXaQg^ciGx$lN(QjQz)9>E zhfQvNN@-52U6BU^0|TfADL%o#!0>^Yk&*ERgWLrMxd#l&7hvcHgVF^UdcYub0fs(s zF)(sBaDm_r2Ehw3bb~?k0u0??;B5fI4_u6lDjyi&gbX8NAtQ(nC%G6IOTZ#<l8cGa co$&($63NBNDEff`P6%i-@_t~zB)~oe0EDEavj6}9 literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..83515247013d576e7357d50aa88d446cd98cef1c GIT binary patch literal 11436 zcmd1j<>g{vU|`5us+JlX#=!6x#6iZ)3=9ko3=9m#J`4;DDGVu$ISf&ZDGVu0IZPoC zK64Zcn9mC4Gv%;BY4#8XP6l^|6y_9`7KRj-W~L~P6xI~B7KSL!6vki%P4<@{d;ByR zZ!u~mgP6z|Zfzd}14Am<$|$B3r4*(V<`%{%<`k9`))s~+mK3%W_7;XH))bBu&K8C! zwiK=u?iPk9_7t8J-WG-^jugHWez1iCDS|BwQCulPDZ(ubQQRpaDWWY5Q9LPPDdH^* zQM@S<DUvM=QG6*<Dbg(rQT!<~DY7jLQ35G)De^50QGzK7DT*x&Q9@vcE8pS_%1_EK zi7!el_e*9%ayA<S0|O{Hy+HxCijje#N~DG<g;A2Bh9RDzhAD*!%wnuzN?``Gm{M43 z7~+}38S>;<7)n?wjTjj~u!OaSshKf_C7Y#4tAs6uwV9ETp@cn!1w`kurLckYq_Ed8 z#4|-Q<S~{rq;S+Q#B+efInzOQan&$nF{Q9)GZlH2aHnvla5Xd4GMBKWaDz+$%kgA0 z6-AVAr|`n%_~3H<*-S-gCEO_ja5+J^oKQAXQAr7RiZEPG1Vye1hn#3OQ_+MH?i4XJ zeas7Z7BVd0O%Y$nSj$qwvVd<P10zEXiwi@nN-b**>jK^siG_@{tThZ-TnqS11WE*J zSX0;}8JZcvEEYIR63&tWv1%ByIGb4)GBPq0&Mg7!Zf2}uh!?71ULXvz0UTB$H4F<x z!90dw22E+dTWq=cCHY0gx7dqQ^NK+Xt|D+e=OyN*zGP%zV5nNGkeQ}XoSCbTS*%c! zuaK0gkdc_8qfnV&s*qb+T%u5rSX`{YrGN`S%(sos$t*63&rgeo@MH1lP)N*6QGhxJ zp${q$tB{$e0CHT4LSj*Ru|jcWUP)p_5y+*t*z)q@i!*a?v80wI=2Y?N=_eND7VDQ5 znWo04WZYsbs<O;f&s8Wb&df`<QYc9+F3HSGk59>nFGws(%uOvxEh<*fD9^}D&QM4! zN>vCAGBik40Lv9?-eLuFt2CiT8XADiMA8|bR+OI`Uy_j;Us@DjoLQ1urI!h}tF(x) z`dgeyiN&e$IhlFcRWe{(vP-iQ<8zYA<1;hM6XEVGu2Mqs1r9a0II@iL%!-W-464Mz zX2G~f#>gTWgDHJWD9fnW9ApC6N%0`Z!Oen7z=E#$79-a$M(dZLV&NsI6w?$dVqsul zC}IT>paS9+Gbmm-yi+S(i;D7#ZZYSi=G|f~D9X$$xy6!Nky&z!B{x4M^%iS#PGWAs zEw-%u%)FA~TP*nnsd=|pic%9(Zm|{@<YboIVlK!pxW$%OP>`CJa*Ndqr0y1LT26jq z$t{+=l>9W15&6Zbw>YvAlk<}@6Z3BIBq!!1dqH`cY`0j7Q*+X8F(p>qV#_WErJ-BQ zX_+~<*dQ@|i?LuOLy-;x1H-Qr{fzwFRQ>e);)48?#3C@y2*J%M%}dcQC`zoz1f?-h z!Yjy0jZe<cD=Es)(a%lH%Pc6(Ni4}PiZ9Eo(ud@#;*!#|H2t*vqVo9c%)DH^g34PQ z@$s2?nI-Y@R-ghjjDdlHiIEEmc^G*ZIT*Q^*_hZESs0nVvvIRAvN3Tnaj|hQfz)t- z)uFH<B8+@Y93Y*HHjF$>987GCJd9OB@B&;9W@R!cvS4L7F9QPu2Ll6xGsqn&j0_Al zpyGp}hN*@jo)KKcGNmxqGM6ydFl4bbGrBOu&a7prVaQ^wVUc9WVoPC6VQOVcVN7F^ zWB?alsN&3EaW)L~EMReV3~?r~I7bab7Q+Je5{?C&=?n`Q(diV{g)A|QwVVW0v4K^A zDhPaLv4d4{<2S2@A&U{Lj;Dr!g&~WnhO>sNhP#F*oh6;ooPmiUo~Z^DJ`lfyY=p45 z#Tm>QYMBcKz_KjTpbCp81ylrc_!WW5t|B2&R$&Jbf*=A^quydnzr~oO$y=n(z`$^e zD>*+WzX+TeZ?WcN=A{<jVl7I{OHVCQ0%?!}sbTZT&-Df6go2#>l3Pr91-Doei;5B} zZ!xA6flcH~EK3I)5ucNqSCXO0Tcifk46>le5X7<s8N--$i?t#?BeCQbYbAuL0#QZU zAW1fm-SHK-Aao^^u7c79;H(27bV16wL5bU?D6u><FTDbkvJ@B?7}(g@7(pqTk5K{& z)tJ;6)fhDxC79$G1(>Qt5or}ke=;bfV96Dfbm7TWiIIV!gkb?=IztNMA`r<0CYiw` z%Ob{fmW7PzjJ1p<Of?Ky%r%TDtdb0%c2p}9gajqF5{3mVDI5zKK}j)%Q-q<Jg^{6z ztpt=bIhq;42|l)#sfKX@Cq#b@Ll#F3lO#hHmjDB()-3@g9UNxYFl6z7)TS_`u(pC) zM93~+2iXp?wV9QXp@bLY!W7mT#uP5N8zCw%^hhv>Fu+Y_m1Mx8harm(WNM)yII(g2 ztz;<zCo)h-LJ}T`wUPzGMwNnzf>a=Mp@_nzUjG08|3AbKP)c6O2uaYOwEXhl|Ns9r z`HRdz>B}6HCiqi|63hLIGE?(P5=%1k^B@VFwJNnJzqkmL0*j16YDGZ=C|wuHgIFdY zt-@$p9rH4Ci;O@rd?1;y)Z~)<BCsGRIj>{}r|(;ACHYQ?#i>Q0A{LT1K?QsfIDztl z^aYnBmZXBTO#vlQJ_ZH`6-EO_Ax0@iH^vA?mcLcvhy;nE9wkYFYB~@Gm!Jn2KuHo* zDmF9KGS)CIU|h(sfGLG}AtN{uF@qXmB@8Kypk6{PV-3>+7LZ5^Yb|pLYY7{uF$_*} z#<eUpOrR2W0aFUwLPikV1{CS+ATdw^1kow%HVh@KDI8!OlC`Wg%q5&4m68lKte~bL zsO+g>P2tRDF7mHoPGJM_3$1FHQ<x#FLd6=!1za`Epk!RbxB%=naDw6TyCndrxxlp} zxQzrZNs9PEVa`~7i=jvW6gi-j^AcR=gQ8!PyGR-&Dgz>9L4-1hkOL8@QG1KEBqOyX z@fLG&NzpCV@}kU=R82N;q!np`RBM5(;Y&(P&W<n1k1sAM%FIi@#h8gTCi#jJ%Tk^5 z^U^}{-7<4h=YgUUR5o%k@`39@4n`J60Y)K45k?+H5wHjs6Vq=lHXg<*2}Jaw8iEqP zA`A=+AdiCMmyH<|zgeKdhH(*N38?eI)XM}eWEjAr%qXHvB@j_iVZ@Na3NDz~z$81U zc!KG#WrnKbK<EZ_eM-PJEjWG^YFWUw2pcHov)Ba~YME;o7jV=tgE~E}Oex%;;shL% zOh}?!FwsJ*8m1H;Nl+aPropB|+{TNra{(u)8@Z6FmMxv3mOY)JmIKKJ7PTxT3=6nw z7(p(mVTGhL35JDCwVX8^3pi>x!M1ZQ;7s94;RlO?Rn~IVurJ_%=mM+Yf~(*vVOYQo zs!P+D7BbdymoO~gsbQ<(2J_eot83U&I8*pqnNs*u7+S$CgBpf-P(jHH^)myKIA1zL zEl)Z_EiW`AvH6NGg|n7Fg|k)w$yZZqS&(=MwSqM)HG<iU6Bvu+!M+9OxEl5vK}m*% zOmLU;Bgt`~$q9gcB~Zf;_J<JIS9~=>U>;xLTd4bxVj9g<4siIQ+lXdr3QvuY2tzG~ z={0=lVOGlr7UODVN`Z%0t#A#`0*)F!aD4DC;HVJ>v7j!)8ghltYItfG;`wWMYZ&4M zz@>zMpC&V;?1$F}D;aOGrj%3`q@u`xr9ru;$OV+ST|tB!0|SF5^K)3WUK9%w1QTG@ zpyH$mRE-q5gA}qM*#nkG5TKm-5>)(x>ncq_qzZ=}+(9rly2V@&p9-m5Km|h)xN-s2 zE8rUD7JE@@aYkYRs2ft0UtD~Pt*p2tF**AdTLzdeQUMi4>{*G)@g+r>w^(ws5|eLn zfJhKuQv_UW-D0VV&n&*hQV^e6T;vb(3%Jrr0BL0gX<*5U&o92kk`-T2e2Y0Nz5qnz zgNV$cG?4OikV-bNsaYU9;`6~|K~W?~f+aUAKJykAL>go*T6I>W%D}*|8&vXvY!qV@ zW7J?2W8`BJU=(AN0GD|(j4aG7jC@QIj9lQ}1s@|1BOfDJo|%P*g;9oyg%Kpr$Hc`b z!zcrmlVJwY9E<{tBFrMpT#Q^Sa*S+@T#Qv>h;kFTmxWSlazY1)K=EG80P0sTWPxfh zhAc(_22jlisv|)?2T=b3R7!!GX$zQZ7(q2<3DW|W8c><QT*Xnsl*PJ$4J^(Q&XA|b z!T=J<Vy|>zWT<6jWT+A@;YeWy4b7B*@+V_Ds42%K!myAroPmuYk|BkmmbH#G7)qBj zmNTR?)Utt_ZCv1zRi_rz*yE{T2MttZf!zn{4S<_i?2=%<I72N*4F`scIcnGz@TRcT zFpDshFfCxN;izE&H3~s?GcpwVfZS8Vwve%wt%fa_L6a4!&0nPu<nQel>Y)&-5bWva z9_r&5<Qd``qKE)Ro}h%!2T6U5A>d?vi?yUEF*)@Xb54FbxaFOglYWaiGq3Cxw@+rC zV@^7_n_VOZN>HFC7o?8>u6n_V4Ah{~<b)(xM&DbE*|%7;<3VPz`o@DyVNAWnUzA#0 zngbppNzPAAD~6;$9^d$s)S~#L%J{<gOtiFjOE@_(C)qb7-_^Y+F$L7Ri_iAuXJlX~ z24x&RCJ|8Il$nVgR0ne~$}w>?F#Tm==VMf16=9TP<YHuDs**)bPiSVLBr8w@1k|Si zC#x_LlT{59C}E{AEo2m705!O>*cPyZnnMejKqGQBi~<ax2&`o-Va;M&z)`~riaJn5 zSOV&5LK2I5EvR$KUBiKqSioYS#4XNH%UQ#Tkytou*cb5Bu+(ruYylOxh2|wp3wUeT z7c$mzr8CrWBiUnB%TdC#fUksU0e=l^4Hw8hPz3_w!~6iFB^YX0L2@8m%L8>g&qBsp z-Wp!0D_CF^Wi4L~_X3_2R&a9Vt6>AV7Ze&`*JOiT!@UqRWX*<@SV3hI0|Uc@{V)H5 zn){$)SyKR<FpEk-Ne$e@F9VfR%&C>Bw^)mlGYcw<K;6Du;%P;R$)Lu5VorQ+Vo6bE zMSMYid1}!umYn?bT%<A?o<57f2^Ta<UX%jL0sP?PT2u|Ps0L&QYjI|JZsIL2Fcn{1 zSX6S0H3Zz6<jl!WkB2Y?!4x>rmKT+PQ!S_<Eouar!01}k0HXPG6HDUL@{8iric*s^ zQcG@eLm41_w|GG*9^6a?YX^57zy!GefF;p;nt~ENNC_m-^D*)<@i6i*fr?cXMiE94 zMlL26Rz5}%Mj1x7|2#|}nu|c<_e4qjpn)||xe8AFGZ+}a<teDF0!1VCGBur{mZ^jZ zRz8+6XR#~*4H7dfWCWL(pphANa4D!*3mU=Ts9~05KvpHd0P2Y>;7nmDVP3!mmf;4K z#h^q}!VF5#AQm|JsFpA<;02Z1Xl30kmV(T@GDt-ZDbFCu23$;m(+()96ioufFu1G& zSJLnVaf<~U$IL0IB}Je_22LQFtVqcM6q%56iqR3ha1sC)PM}da-^2<~0Tm94BvA7L z6cH?p9E?JYpz#O|7A~eLDYSA3NgGN$pbSEM1NZo-GYHYl%*X&9#AgG?i7&X%$c{Gf zzzM2j7#Z*lYCv>A202o|!w6uLK_gSl;6VmXq6QflQ_$5hWO0KAND8&VEkG{6B2ZF9 zYu15d8q#0`g>#VyC}P3284tJ?14m$y7f2L50tK2vLMjPBl@y|hc8d$t@rHEpA!Aj3 zAdR4028n8LZsY_h0{7;#K+y~7$!jq3F-kESG5Uad^1_Iyg{wu0Q&2A!)QkjqsCX7M zP9b4j!jQ$1!q^LHkAT`1tSL;{Oht0g)-t5O2bKrLRt=LlLoFkC0ErPge53@)St%^p zOhs0p=^9WTTfkMqUBd`sHG|CLSjfc4P^f~Cg~)*hn_x1a=_5u4aJaMj6-9$W0W@~2 z$(F8F5|=!EMc@AP<u5$+{ww~pe-35rFPh1~z>uETCjH^3BIf}}BOBCU*5pOR#4SG1 zKs$Iy9UOv?;rn=y`JnKC#0+>)MiL|jYCPW(D#`~9oy6yr=B5^9CMV|PRNmqO58{K% zU2t<$lLeebZ?S>P(BfN6g@)iv3W+vw1cKvEytE)Cu_P7jfMUnI6xf)3F(@WM2}q2Q ziy72$7hvXMlw*`);$f^(M8qZz-6)flp!^E*3pj2;W9=mjC5$zQ_yG+MGNmwPGZo2z zhT%X%b_-ZaSZf$TEO7iVfnxzwS~7#g7O+6%Kog7*8Bn~eWb%V_LfEhcTG4D!@XcXh zV1SNK70m?+fU6~NXn+ZDkZ=~KmO#Rz1r!LN_KpA}#PckFs*sX0Tph}gJ}4=}$4CvJ zZ6vVwBx@K;7_*p47_*p>F=(ziovnm1izS7n7m|QMgHf;mN&qKd_7v7^rlL?#k_5Bt z5Q!DsnC1Z`<}6-F!WCy|W&~FX^5AAPvn0a;K1@|LOwtS>l{E}m?BHMospbHio{caa zW&*-&bdw==BB=wLi^bIL8iob@-~l&=U{K{LkiwSDR5Y!IVSyk_6cmw~?0!X{yamoM z>GyLLLVqj%w1?z5Q1&STRl=G=MW8f=l#!SUGK;|Z23(6FvWze&o<Wgvi@P|rBp|;y z6VxIq0wo1VK^F>A08SqBK`aRn0qW5~vk*9;+~Q5GC@D&ePf5%z$jnQ}o|QmTOo>G) z-kEv1VVQ|An~HlusR%UW&&49d$ic|;pM{a*zW@st6BnZzqaG6%vl62g6BiQ?BM7tn zN6ub2OvIhNK;v&H*~_gKk-dyS*$bQ!K{3q&&Pw2v24X=|3@DSagZK+LO4uOT43PzE z7#47XVyY0NiWzK@O%1~WF1Q#}Cqy4Cdm?m!Vz+1ks9nj8o=4%aRI~&X{)pHCM+_)B ziomh65F`uEt6&10VP%kl$q!N{7W?PrRJx=lm8P!&g+8dUBfty_aVbU_P+yvvi%Eis zi;?9+l^UWj!)FZcAO|&zL1W6`79FTIVFI@vl)wSc46d!P1~(`&z}XJey@5ssJm8oi zG9?Vqwj&x{!?1uI6zHG;)nxIjQnMurfXm0CU{C~bK>DfRyaI0V!K)WUNeQk<im-*a zEWV&JIs^)GP~E}7$j1UMIGH(^K~tkVj8*Ey1-B7Oc!P31D7S-akO|P1cPXle;o zObXO8*D!$!!39t{3sg%mOEQRqCyEL=L0vQ4Q&lCbS?npSy-c+%U{jf(tq0Cp)*6-? zCeVm}76+&`UT9JSUJTUA1ZmkpM;AD-x5c7CsTDk01_};MPDp_R4s=ifb&CyBlNW)D zAaG+G-2Va>V<w<zVh7jakRpg3GC^+yDSS|exiG5r)Kpmc<Cs>GTI7~rR1PYaZiAu* zG<m?rD8MMksKF$_B)}-Z$bwR-<Isyz%YmW;6fNK=DS|fGK>ghm=7r3)%r(p|46(|! zEG0}O%<yRy5%3f>3n(zb-5@Y4iwzRIH7p{qs1gJv?;6Gw7Dy(jVOYQcO6*WkR*)z- z;lp~#Aa}e3%@f>WOUf)REKMyk0EHbVxN^9~k&~HMT#%TYid5u)no&h7L6HLvc~EB# zoY-O0AmElUsDW^cwKyXmv?u~ZK>{9J?tlq!;Ukz-R0^5Kf`#!5P#A+6!Jrus0Y)|O zOo#wul{6yhW9ZRj^wSguElU8cvWbts#T6f)o1apelNuj?izhz5urx6TB2%OXa<vnP z2mldLAOc)GHi1|ZLBuo=0rnHP$Xo>CE&~zELBt9Wu^L2d1QB4LAPA73ia;$k3($Bb zsAS+^;9}yi<B;Lt;*jMC;ZWmH;t=NG<PhZG<dEkO<B;Ux1*=a6b&p{K%AnK^iWP9` zaAINr4f?S{>ZL4L*5a>at6>FaRyIk76vi6RyehLa19bR~vxad23up`o)FsPeOJ_(y zpIwGb=&*n@E*p3fh&_cpoe_r9nQJ*<Y!IE!T+0cfVK|++mJ7xP(do>fDG*S@6z;<9 zwQM!)3z%!zQ$XWQj9E-IY?2J1VgS?vf{3QDr*MLb0uTuvM&+sDfr)|mD5^L?BH|3S zyl^$(2_md&_~2^5i#xEY;ZNbHVNc<fWPsWTHXl<Zc*Yt-oj60SKnX(%C%Ce5tQD*Q zPoykhuHgkws4y}z)Nq1W3z%#8Ks4CTTp-p0<{Eww4Hn@Bu@*4b2!Lp?2xAr}D4al@ z*9BZP+$o@eU2f34Iw;@PAj@*saD&z?f^-$0FN~^TNa4z60!{tYFoK3QQg}c*Ksg<R zHF^CYQxc#wh2G~yl(V1-3&=D$DE)zlalvz^n!H8uVi!^hgL=BRxREL{aB%_Y9>WV@ z$TT-cacW6?N@7XkEkO{=I3>QID8C@Js3bGBxCoTdG=*-lrh(S8f$f46*W7uam3Ls# zV#s7WFQhLPU!0SfoSFieP6ZX?w>ZHIKR`Cz;sh`LfH1)eK)_7a0x-p$k2HU84a&VX zAi@?zfYym%Eui=iUIb0He+QLLpsI$2QHUAT7Sdt_)c_z2ntGRDQ~^T=Mjl2XD3oKD zVr2Tl#V5eX^1lc)#jeS9i#4w_x1bUnWVe(cv(?~rR(j>BN%<wk@rebQ@foQ(1*t{x zX{C9^w|KzAl6pCrd5JmcMXjI#KpA9h;JPh7u{b^>KNmDsc}o~s7Bpx8nuJEt0u=<0 zAtr*BB!G%7u=|i|LvYCkD%5Uq*g#5DJ5YUG3~J?bFmSMPFhZIpOiX-Q4D}$$;{*VL CctSV; literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/forw_kinm.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ec97b7a1cca924c836514e165d58713cb9915c19 GIT binary patch literal 27512 zcmZ3^%ge>Uz`&5RR4p|&jDg`Xhy%kcP{!v71_p-d3@HpLj5!QZj42E$OgT&;5I%Dh z3z*Le<}>B6L233722KVh26u)O<`#w&mSs!~46B)-<}pNZq_DOyL~*7t1~X`~y#xvS zX)@ko)Jg_1VHl?Ta{<`;RIt@iOesPsOf8I2%pilKSW;M87@}Cg#<8WawJ=1nr?9tx zEZ_iJz}dnO#g)R<!Vtxs!rj6U#goF*!Vtxq!rQ_S#h1d@!Vtxu!r#IWC6FS}!Vo2x zBG|$ZB?NYs@GZ`u{G|Mn_@cyezhszOKyKq<U|`^8U|{$h#R>`*PKFw$6h=vq3>eoi z#Di%NQNxtNgd)QT5(VKJrW9rr8MqB8EHw=A%pf%&oEM<XP{ILXL9hx#kuxJh3CMnk z07DJaGDZf5)o_zjShB$pS0rD;3su6vz>va<s)~`Jgda(k1zk3eFNF=+9VzTJ4DoO` zMl$5FlrvN?M>3Q%rf}3S#KT<(*2M`kIh~<~As(E-Kq55^S#T3l*t5axBKZ<=kQ@Z3 zaHepfCY4%d6kl+oyA#Ds9;{~a5-^hwubKQ<%@iPDrXXH3g|M0_Ou$SLx|=DA)tzD_ zyOVhVD9=Em7>QiQz`(E?&R>9(TH%5WDdMQUu4SoVS-^*oN3c*$V`Qjdabbv^T+3R+ zio;|HG?Q6t7_#7LbpcZDK-f{Dh{RS#uxnUT*buoH)!Y(Pbu2`xlO$4|6fSi&3|a7e zxQvy7VKpnvy{PVEWaycKQl21%0;<XyhIk>E88yrcgkfxC8a)=k<$(xHMGeCOQ5YMU z#?ZwO%%CalcZ)4Iza+n?_!fI{YF;sj!Bqq<lk*aDQ(uBgysE_tnQ02enYjv?#R?_) z3Q4I78Hp)63YGb#3c01lB?<+J#l;F-3b+8oeB0=p%;J*x{IqxoKNgP;g~YrR1*mfn z`k(@_3YmEdAjhRBBo?I?D->7el_XXafl9htY<c<d#hJOcSW?RpbE^3C^b?D6i}g#3 zOjF}iGHx*zRas`L=PHyIXXd3_DU_rZmt^Lp$ERe(7bF%X=BAdU78NUKlxJinXDB2V zr7DC585*Q2faQucZ?S^8Rhm#E4GlnMBI%4zE6UG}FUd%aFD;5M&MZl-(#wR~Ra!(? z{VmR<#NyQWoXouJDjBdX*`?Wu@i|H5@tK+BiEwuoS1BR+0*9Jg99c$rX2r$^236u< zvtV2#V`Pzx!IZuwlx0+G4l)7kq<E0y;ATN3U_n>>i;?RWqxDNrB)kOGtD0g(><kPH zMW8yX2$V%`F@xfT!#lOowWuh+=oWKMYThl@f}+g4l3OgP6`3WsSaS1IQg5*)=OpG9 z++xeh&&(?+zQvMXkeYXkr6@Hq<rZskK~84LE#`v!f?I5f1qG>jDYsa?K<aL>rsd=( zmfT{=OUX|I8IfO{dW$0~F*!deGcoTLPjX^TvKN%6$##pSI5j8j7E@xyEw=1(P#U_$ zoR*n$iwzRfw-^hG*%%lYAj$bxihf3ZZmNEIesMv5N@5Y1XN2J9l;)-A7ZfE{WP;Ke zDDf5Kq{b)b=am%Y=ji7q=4BR?<|LNn7sZ!lR_Q}>R&hybTAF@Zeo=XRc4l6#UP0w8 zj`;Y@yv&mL_$ncIi%1V9X2rn3P&|u?fuVun0}~&o;0-B-1rk@Jv^rdRyl+S=F3`Lp zt<&Ms<8wn!WkJjpIin7j9{(HS5*;o*9vvP(?(&O&VBq8wL=boRg(jGFR`*oT2<)ow zsJ|;9`hkH-+?Vkq1CyX1;}<ab0Ze{nV-OUb5IB*23i|~13G5G~WhSsp;kcosHGyRc z{|z324wnzYoScF`e*E|##vm%)!P3KdLsYDTrHAW*m_!Fl5BCFZ{vP`o8JD=#E^w=T z<zWyOzaXx@LinPP?iC^34(F#lf)|8*FY@?Z;qkk`<97qBI{gy2>IH7qyFy|Y#GMW} zUlj7VBIMEGd>5qF=^~Hw6&~jcJkAic*_XIAFK}yu)tYW7z9?jUMaUXzuGvK%^D8{& z7kJDeYO^kJYh2*g0IN0FpnOrt?23>X)Lg@hJVsY|j4tpPk*xNDoY5t2qYK<d1Pp#4 zC^B7QlEg%*DN@%36)y=YE>O8BsD4FIy@Txr2VXx|7gr}w4^Icr4MFh_3{0G9j1aQJ z@rHolboNQ?GngmxOyTKpyul;T<9(e+?h=pO+|s3?D-2fzt}$5{y(D_8>JG*oihEc# zYH!irTY4byK;e;)oz;7)&$?a+3ce5$dL=0Ar1u%`3z4x`B9kx5r(BUwxyX}xg(vj_ z7(EpcpTRPR`=W@-6%iFsOg`Y??&s{{?Bwp@?%=*5A=$yw!+V2+=Q@YvB@W4p9MV@f zq%Xiwu>b=DLzA8%!$wCgM?2<&wv3K;%*mjV09FNoN&*o7^8^-9i@t^twGCgx5D%(r zz^cJ5T2N5}=A|&!GM9h~6EKf~p@ty~-1Y`>QR_|@h6zluoV6@93|a7cxCW^$kOj)t zAZ-jOj44d5Oeu_MNIi#IP}_t!eatBO*od}|1w|h_(fXKB^l{WMWPyDFA{HQp42TcG zC4vwdL?T)!=?tjhMNL&Htjky!7*@k$CWeuLp_X${x|9uzOSuN6OWCowlzT9`w1y!I zo_n#lnWu)Kf)UX#t>LWUs^PBTNoPrCtm0*0NM=IxdTW?*<_dVf0jCOhkG+b4fdOQ4 zE%OAX9&WIi@RV7_4Jt$8;k~64P&=5z58Qw(0=1lrKo%8&+S5g#?$RyB^jnNcn!H6? z3=9mnxRUd8@{8g@jkH^=IhlE>#kW|C67$kii&R1F9a)eXHjn&VUr=+TASb`%7E@lq zE!M=MqQuHuj42Qk`4Y?0!A8XAq~?`mXz~_mf;59HC^7-DY(T~^X5C_~h|fqYxy4!u zp{hVskse5r4P<wG#VrV338kx`bU`u5<&YK(lq}K*Y2ya9GF*xh%QN%Rt3(k^4J6?T zP;28jBe=D}!yqi$!P3KiLkd(Q@ZS&>?_lZSx*;jm!FN|sY(mMD%8P=ER|FM5Fz|3X zF@6M-9ga6dB&K*TV4UK2MMQIj;tIzV$}19As9q5<=x`-dN<d@^_YB7=d{+ci7bLD= zT#&p%aY5=80i6!V2VzoF(x>N4%2{f(#A1chLYpNvTbZ_S>=4?>vxR4G(4L3`Njqcq z#GEj`DCU1f%)isU!|leP(qeMP{-Rj)6|rcL(;n~$ba?f6&2U@bw!&?L+XZRs3sN>0 zd2B#}H+ZCGq|C`)p*Sb+ik#jCkuB0Y0=LLrF?75r=XjCFvBR&^?}41oijp;z7v(HE zSm3quXHX@O49}hn3=EtM3=H6^_#P9eDlS2iLuBK0h7`s{jOh$1Okk23OtLIuOh+5W zN@uKPEP+>>psbYzG8<$mLk(jJE28V1!j{6`$^;>iD>M}Q;n_5W1GTb3&h#mqRjdpQ zs09rp1M(mi^1xdSLl&s<0d^6pnW#J$hS+DdOf`%PK;Z*cjZC2W1=-9Rq(%f{aHEQa z0j;`aWI!H<t6?BDoN5@d;Dr{lohb|{tgT3HKy@i3yi?ebLmW9QQHL!U8A_xPzD9Oq z3Tq8x3KvFfqMC~>zEM<EaWOD};tr=@kkShg_Az9^hpEv0(eoT!uygyZWPudupavzR zkO#3=vOw72vK}M==E6k50&po5QMlC0|NsC02kQdY8Y>yW6u91a`S1V#|C;<omY}j6 z)O)+dpHh@q?q8Idnpcuol9`_esZ3a_Qj7A7i$JA)kr_y>1c(6DAw^0cmN}@%7Dm(R zn3tJbWD1hu1IdJ?CYR(FfdxTT$4X{!U2==9B;P5qIJL+YRGNb_q=JG1qzd2#=?X4M zEJ+1vsuD+30w@wwKpJO)tAV>hB2zSGc!J=9N)X(j34%K!LGXgKJEVtpLC~{<?JftX z6T!YfYz5O2sTGbZ5?45HVBDa%fq8}d6-AS+Nn0{@lx)n~l6N5Gid*D`nAnSMu@^bw zu5iR%;E1~`C^ofthRGc3i{k26#MLhfYFrW2_`tx->B$5kL46;ge%~(N8EOlJ=4dS_ zSyH{ha7o=2b(@QzQquMckL?8>+q*pS3s{!0uMpe7v_@)!<A%fy&Y*tL4(1K+S2S&Q zr0mH$A-E^^g!74@6V6xc0xkqaT!@Um5)^YGHts@v!iCs`3yDb=Vw0`}C0|I*ypmdR zk*D$sPvr%k%DX&LGfd`~FK}9+wn1o()`q|hg&Tr*2yO_yqGi1!?LZL7;1h-?gii#X z5WQmOe^JZ-qDtU};P5NKu@~avuLLJtNKC#GoN^&8=R$7&m9&Bjg+&*NOD+_aTqrHO zP*`>)t^7ho%@+oSN=6T+j|>dFo=jiB<OgJu9T6NwZVU_zC<QyHF93?`&p8+cJE+Hl zHtNq<!?*yc=tpFG)RDpkpcIdggtAhYQ43FS!3nY(DvzL0O=Dz0F78tp(Wk9y8Ecpp zz$*cSDGUtg=A^LJGMB(xd<-bXKl*$Ps#{$cVryzyYM7AwhB$o5hN=^!2b5cD7_vZN z4R#)~DTs;&q!OvHWC!U&b}f4K=E4wbSj$?&T!J*>i)uy<QqhR&Do9JWhBbvV8yo{g zY&Fa&Y!J~Nof_s8W+>aJr?!T10jOL9yBwLQL5ojDh8o5NVAGIQf!G-F0BS#T`P~wL z3~qr3(ZEBg;I?iNXf*p4WBDzHB9PC(Me$2e{DMnMP3|H&P(ddTA{0P`8i)YR0H76! zw^&OuQcDtVF&CE<-C`{-$}CCMWCIs<MLHlYy5IsaDKR-az9c`sxTGjEFZ~u{CZzlW z_4^^sU`P?jSDaXu>YSgK7LxClnUh*2fhYq}W#)kjK_y5b$R~6`Sam`1MLz8-eA*q% zH}DPTAPt<{Mc0Qi)*~t1!3Qb#%qCV(slF(vd__?C0|N)AHkjyeydfbsqi_!AeEwPd zE0`}z=v<M|>2RB1*y(vgT7Hi8g2Fj=SEThpB0ZiH5<7kFib~BeoER}B;sXOKuQr&N zz<5JIZmIHuv=xC%GB+?T$+@Uxd_~FlqJZ%Pz8k_KGZ-hCO)$G5qO>6SqKM`MzX^U1 zM3fe!Tolm)u|IHwjNxJ6<N*x@P0+i@p?HNu@dAh94dlV04(<+aJf+be4nob$5>Uc| zVnjc24HIgK1#W60l~^c^Rpv#EM4Q5dTn3?-0&3tgq_Bb;yKG>R9n|C{&g5EVoaS;6 z>pJB5;1Z;UC`ywJWiHx<AvUy@1>DC4rE_p7qLfMSk_yx$t!1uZTmWwlgB3%G8fMVU za4S;^H*!-EwWMKWs9_?Yn+u=rp0FCG6dpvmSj&paNAW$bnC2xmEU`D_Q}|N&QPWi| zTRKB6dpbic2hkzEt(FBPuHj{B4I^^c*RZ0N;h^R(n%$f=95_Oh6C><7arlS}!$+K` z?x^LeVaH($ihD6^<RaQeE|l<q*UO->o;0-86SdqZY7w0pwi<2>wQN1xYuHjaQ}|k$ zQutFCT2aPAYZ&4|10CQLT*HtBXG2pm0~Yo04nsOaEl)Z_EiZ~&;At+MA%*(sj4y?= zmOq8FR)E-aHi0qrVl4{^s%>foYglRov%&r-nvUW(L>gsBNu%sFf`}Z6nmUP&eSV^B z;-G>}0w^{i(wRUFKStUSLQ#uIXM8n67;5=?7NDpGhaf0nr8A@uU3ya-UK~`98Ir;) zg{MXc)IF~yDdZ+F_Sn_%;mV`6d|)}QR;Cn;{8%eo!-Jz-!dx-Li$kR_dZ~po9%+!H zCouM0uHmU+h=;Fbso||*hzCuFff5e`co0IsPm>ukQUK~pf!n|<8E>(slvEa^qR4=y zJwWYQPY~e+BD@(G7&MumgG~eRQ{V(xHF(4tJXq}mQpkp64_F>Sfb#!KP!j>{08K%p z;R$x|{JF8wE#`vwR7lGkKAc?y8ifFlN8DmBN-fSvEC9{Y7v&ch-(o8(E=f$zzQvXS zri;`;jd1p?#N_yrqRd+?xmk(Hw>Ur~h_5LEZm!>Asfy1mzQs}ypIKZK0`d!Jh@vPR zq?H+@fh8+GzxWnQR(wJ6E#|EF0uYf8A~K6|LCW($D%rrMW`XR8&j*tQMW7{tU}okm zE{HV9+G0@HKpOw3B)BV3qyd^K5JR;8k(UbW2DSc;SwJJ<cLju|aLf<~!L>|lxGo5Q z+Rs-6j4$w;bg<x;6cnB!F~brB7i5Cq21yXy;R%8lq}<LXoyoc&7<o}J@`_+&2isj? z(J2-)yg+bm(weLb!j>0>Ew2b$cChqtJVhOHXXXTtj%ai^-Vl{W3=JD!6gBQ}yTK#S z@7?7+gKb9P0>K%@3mj*Z&EdPqqke@){Q{3Vep!$LKA{OlJsBO$H-yD!Fio)lS$~&L zpeJKOEeNi4TH|$r&-@~v`4v9%>wNZ?`0Nib9#Fi<=W>P5rGxnc8-svQ2g?n9!44KQ zM{|H2&B3YB;RqR3GFiYl$9#eF3Y8UwYt$F|UKBFAB4pIz409m!MIM<eJTez}WNrwF zPS>8KJtJ`e<3%B*D?&;g&Nqa_CKOIlN2q3*5jcnaB9Foq9)$}$3RrZ2?9`kQyg+$| z*F_=qD?;iW&RB#xoNq`-&v2ZPIKz1X;{v7yOf%f(aD!&QKd>=K$V{nS5V)XlLGTJe z5L^&?MO?dsrHAK(1jNt6q8%Jh!EvrTLwAPm+M+d87X+;?3R+zew7M?nbV<<ZfZ>6_ zi-PV~1l>E>Ztx3$!bt!loTNB?7(X(A$PUM+qM(5%%Zs9_S434mF!1o2e*_T|7~$Qw zyVA0AEEc$|5M1KEB4mT&n(&L#rdOm*uS?rrlD6HEc!2Suw9^%7rwJ}3DYRUWvchFS z%tdLVE7C?2T<*%s&xu^Ywt;0q-9=fmE3#%2+$OkvWn++$n`5!S3k0_&ZOOVI?RZhz z@rtzLb!pE_(w-+2PdHwb_PZkON15d}q-7Vd%(TSniivI?*cilQP-ARLTn9*4N?`%x z0>uT)3mn0ab&hO@7c6nHUF4C!!XtlyNB#o`1FvYmf0zFZFL)lgD=I!EVn!|qZZ+Cs zbwSkbqNv>!QM>D+ZkI&e4iugcyeR5@Mbx{)t<&v>2xzqn=M4!3P%`P@?cl}i4ud*f zpaGT7+rWc!$kVvUY(#ey++Aa=VFV4`ffg%(mMkE5u2AP)kY^K6E5uqR<oOxI;tBLY zvRY<Nh7!=w47932ETUKdsx_dZ2&#q=%^sFKUu7I7Aco6S7>dklS&^3>pbsT7GN8B@ z-U&`&OlL%1J_>56pt>NLDUu<Dp_a9dH5f{lGs2cor!&;Dq0PU5Dnzh%P)%`Rh%K!J zEzUqR7}*gGWZZFuEQ2z?futKXc0tqBwH!4ZIAfTjh7HHUT9z7SkpEFa3Y5CQ{;A=p zVF9HcRDXfgqlz#x^pv5P01xXLw1LlBwi>oz22EC^Y4R$CAb)SaP!EMrg<wxV_fQ|l zAkPrj5Jdzi@&mO<_#n*!#t?9O;}&a4QDSoHE#{p3bns+#Vov%k=FGgZTiiaGd5$^h z;N^iupztmN&744{)xnb^;MyNFFQUl_X-P2p-eSzY#hM)tGK<wW9%Kq*>Mj1F)Z)?{ z@Y2rY{M58!NW*}~H$Ek`C_bq&zA!$s801lSa{$~DxFrm#`h7$4UEPZkQ$Q0n@!7sr zvZyTsG--ZD28QA|@bKUR9)TX;87^~t=lEab(e7Zp!Nc2;IDxSvd4l1DzzN1389m+| zjCXnXdORisc6x(2ptW_vQ`jaHPUM`z*}?jOkCjvRu87za&*{FCd>1G}*I~JKxc<1y zFW6Igg<l0U3!;l4zzgC`F7lXO;W53yWBL@_atNH^yg+4%=1l*K!a7%kbuRFrqaUEf zdhE=cx*(d5L0YzhA8jh1oil*(0|SWcaQwi<AfqtHazWr6n-wZ+bl2!#l(yXAvd8y? z%NfrLVc}<d&iGxliMl8q)!}nPT73oM3dI%7D*{&(t_WViaYfp&!{-K%^mQJkOFT*o zj8?ENv;xn~%bZ}nXz6>=!0(EI-$iA=i#+~Uc>FJb(OoH-IT{P>FG?9-kuvV!f50zy zfkUnsG)8=oN8V9~;h>I)lNs|N25l!p=0n<yAhsc^lNmG0Gzb@Lep?T+)C_646mlL# z&5N~EDQRnPl(i{nLm;5MSj$`jaw#~+GB6+(r!9aF*g>Vh6lfS9wQ~+yreDJd%IA>W zUdxKZJW$O8HUpWcLEEeX83aR_e!#g%-Gw1Gp%%0Z4blDP0OeSS`MARvS*C^|3#)F_ zSOJ%ioHd*{OGwTdcI+D;YFKJG5n+Q-?j@p>d+>py8g^8bwOr{8wcJF9%9&aY6m#K& zO(>#>t|Ds<R}E6gp$|%6*M%YqudmTl7;&mV1ClkY==P%XYk5dYSv+WA$Xmk;jXxIj z0l8Yf8gA_Im%@tT14P}%SHp&yMo?`7<t$VYMuwiVC}tsI8*Qr-Xr~-f4F~FzGB7Ya z*#Gh`XsRF7k<=6b*Jeev3=9n5`mCr9)E#9`txUbeTAZ9&P+8Of5)e--N=yb#^e5)T z=O&gEWmd!&<d>%w-D1hfPtQf_VZ$pmNNopN*jbbfsy6w-bzD(1$f6dIk*vj;>A8uw zxWH6=abZ!(E!Gh5oET?LetJBFDF~*(HC=g838<<A*M3EvAQKo}i#k9we{Nz)d|G}{ zd|FXzaz<*&Ep8|Sr0*6lsG0;%wt=)4gAyyeri3tX)|a06>q}2lP<{ED%=+?%fZ$Z8 zDQ+_iCwfltoXFe3`czbEO3Zap%S)n`J5&#dToiS?BI?$`bwfaEM&Lxg1yUCUv^!Xz zibziJxh|r5Nknx)>O~QqD<V1_oHu0TE^tVq^-P&LeHlTOt1n}RBWQAsqDoKpI*-aF z9+d@A8+0zJ+8vO(sOo)@$L9)<&jm1g3Tg;k5LJV8HaZwTaAB*6Svma}KQMsE4#&H4 z3Ue~o3aydc;JU+bN8lcl4Yn6G9j|CQf(9Qr=Nip1Tc9}8a*pNFq9tW35*Jo3sk|s{ zutD{Ll*L5>%PRtwAD9_6tv@m_%G-Pa5gq<drR3-6T$j?mB&EH=|Du%56)Bqz?;A3( zIy`7aFt`qvG3@X~)ZvCpi<VSu;61>3g!`g__eFJ|E9yQMm3=Pq_+H`hy#Pi}`2~9_ zX7FF+*SNy3ae+evvm*p*dx5g^XB(<@gwh#MJ308f6eURA8ANwz0n#EiNJR!Bk*h4! zVg;3ly8H}zeE_OD7lzmgwV*Yw@JhJ`X^$#(Tn4K4K<gWDj7GB{uck*VSi@mAypjfm z1`MNj{ZRb~DuPk$Ky;+g77v0{qUvR2=$VM37g1QFHKEYDBez%zGV{ucz=NQWE(fHL z2DM8dB{isEE}8=>Pr=;@@Bk;g_`SsfE>W3NQcJ)^I=DF2WJM~3K}9FHFuld-SPV)l z;KC4677Bp-4xr6GzKIo}{zH`%TE_uNE*w+}f;NFSFo5=Q5bQPFkdo=(zstefk=DuE z!`s1omxH%AsVB2PuPbjx$edWnf`p44I#)P!E^z3eHRf441HolTAgELm5ShX`Lvaev z+@d*^^Xq2Stw>pudr{uzioDH50h<m-&?txuthK(DX$|{UrY)S?c{lOyNW5s^aK*sk zqK4xY4abYhjz^u2c$^44=zYZdqO$)*o`5Sn0T;lCgxntm&iy#Y>Ic>0(`C#I468w9 z7B~Z-?F~n2x}&CL7lznf;B`^h8}ulPC2=k&1(lXyTS2)R)qK=}66D32SeJl;m){}} zAYoO7vRo6x-=birfC*&#z>7mUL0Jtf#ej9uDT;rwsQ@)bK_x;BV+tX&kU9^`Q1+cL zGW6^Q&$)2<6=9i30hbSuDH4!ZihM!E0eF~#2U19Y#~l1YqTto|0U#ElxB!hl6oJ=g zf+j_7aitU`mO~a`Le}jEgJujs=>)#ok`tr^yxOu#7*Qm^1wk#@VrOuR2s98)<yDp! zIb5%BxL)9JMJp7TIg7weogz@7z(dTEO6eIcbKDn%t#Dcr1zFDtSy;J&{fer^4z&Y9 zd$d6N%?<>g5Ihii#R}2Zxyu9EB~ZSAX+heGpe0!w1UDFN5Z(~DLG+56IW~Q2-WL^o zPn2B<i#SttAu9SpOzedy(CWzxQSn#&6D}mCUP&yt$WwTQr|<$8ePBoU1+%3A%6FjH z_$-E}tU>M?Ag5`RH9zof)*43CPB?0gtOX5Fg6alvHb`ND?(J8{S<0cWTEb}$s2r(b z0(XZQaV~U68<0hHn+rp1F>1>rg$3$9E98Cb$n6pAYXwn;Q5lidquPjErlE!!BSVin zX?E72*n`*Pk{Sl=T}rGg7g_y^l0capG-IpDmabJ2mppw%-~ROFFFf@AEB>^94rS~w zTFSt{ke=5j{o$t~=Yb;dG%RTRMw1s&a@^tr?NkQuNe1V6$o}Uvkoll;9MbOtFY=ZF ziGfOzTS7(opvBnnd8N6jMVZNoIXRWLxWK!h!F@UK45cOuxFK?j4csp)zQt5%2yTiL zgVGDA9Rg{Fh?f?mB$lLt?JsuBOM&fqu2Mu4JvfvUgBm688NhR$pxLXrdMiZM$m|W; z6Lmq#<D!(u6)BI4f}U4EYrH?OG4KfV*w29Nrv)j$Ams}|S3|?EgvMVGOt>hRa78eo z!wEDunKUuF12j`4BtErx2FgN57EVvb4-6o(!|?{ca8DIvOY#lahM(&KhL;2kw`%PO zJK(q{@}iOZ6(jc(B4?y73WQt{2)O`84^S0vP}?E6MRP~Uo~Vlg9#;fBE`SlPjjNDC z12jnv&aw;)4DdojjS+p>VhN})fvUsb1O{g|CX|Cy7?Cm?+#KY69N3pVBQF}RVMJDs zk=fwBz?#`euoKlDWM5#ITmq_WS2FoQ))cT|OW{S!L1}#j0|Nteb3)NdkN{}9z8K^v zL|s{&S^`PC$aN)LumzMvyP!!Fv{hhWQql)bP;!MNT1e^vc><a$5PPfMFoRMLa`!O> zmI4B67?D#FV!>+(JO&w%b_Y=|n!=RAoX&<~4yfP&yLk=Txlzz62C3VJT-^{#^%EFl z&B2unC>MdvPho|o`$fowHe8g%Y5=9`LEaCA)QLtc14OMfKt4mwGpIa9hMokJc{?OC zu^)#+iEC<@KpRmp-Gnl9fO4W2BSX&ytYJuE^^MDq#D^OrL(f-SreF^#)R@4f7s&<i z(7_iT6Bv6ev4)6P4FmSWg|IaNY8bLWl|Ll4vO!ZT$ONQK^k7Lafz1R^8K}wbR|GN! z+%QPLpQ{l1Tk)qoq-_CeCKQ1NK{SQHhoK-f7?=t&i@@yzGf<g`Xc~xtxS&jQi@P|r zBp|;y6EvD!1S&-#9gt{{0&vL+I^L-Wv}+H!sQ@lxZ}Fy9loTb#rzGYUWagz~Z8Ct4 zAxSJs@y^W44a-b~SyYVNWWb@M7gQdf2lrh*aG*2^Ziq_HO`4OjmT3*^R;DeidztpI zf@U^dXE-m=`oPSnp?BBRd`r{^W=45OM$nj~BO_=~63iBJWbAPJ%E5ryRN&z(0q-y_ zVeD{xfYekF7N24<qi~Y#1Y7v#;_D)Mmqhf|R_##PqkSN8kKPq4zYBrk7a}6B1V&x7 zin^#DeMLX|qDagYk(di$^Z>SF`MQYdB@xrDSqDUpNS_EiB6r0x^g;w^U)Yt1#0x2f z7b6NUIu>3uE4pG<bfKi;N=e;Ck@_nl^%ubC0~>>g1P;fnC|Ogz!*EUA6@BLeX(s}Y zWM0(wzbF!LMI_(?7@@gfL)wnOEtv;Yj%c4qJfe5SDeOXI+C`_di)LvTGIFkD6kQZ4 zz9Le5L8SPiNHOS)6^jXlUA7&zH+TfD^JrY+(YVN?b%jUk0t|t|t8#|hC4Qv~{7N?j zM6L@cUJ_8eD4={rK=}d;ePCt~P`<#g1gX@Jsv<;_$ONMwg<O#lsvKf})nacF^&<DX z;MJrF@<|`a4Ik_ywW!q;vU=2F9+k()(Bp$##31X#zTT8rd+;@zY8bGO&7v1BJ+rWe zjA#u5_MRDr{#~T`jrfKi(LVSMDjbW}f~K^%(OZY`Lat~Ns5C<qZs2kav^28_T)3^n z7*>%%DwF&m9j9Xdyqrpx)TGk%Dm6rR2cNn%paP5qT!4LL!&2f1fYvGsfYvI4@(-+Q zg0H+$J0Q46^P;}X6@8bBBCc0NTrYsp2Q~&#Nyw21@B&8dgy0d)i)KDo%zRFyoXNT< z5_v@=@`6a@4G{^*bk#)>^(!Lk7r+RmWO)EmVQ@*r;G&4p6%nHgV1%pu(7Gt%dqu?e zf`~76gB4+(#FOi{VRVF2&_)^<8Bodrq%$Z{a~r%{1aC6K$4Eep7<i);B^ScSQ^55v zlprA&!tKK7ynq^?B-n|OJ8+v^f;{4bWi|@FyMTd#RM{E^><dKD^FAowX|niLso4?* zz#X`vNKiiIfGocPw@ARVB=ErsL{|>n)+oZvn6mgXoKclJaXHiI5GZS&fb{h^crWmo zUF0yo!eM@a!yMc;Kt6ed6+DChBFP^@_y9WRYzp{@5y*%G_CbWNYz&-y9Z8*>J)9k! zpcxrVCK=-lb>QrU+*7P&Le5So1C(eP4<maS)-u;HArHx6-^7cm1~CbW+D^ypoiH-= z7$J{HAXY8mJ}(4i?Fgtz3(mr8SW&ZMEelRRqfPu__*tlywFb1E8)<_yVk0(a+N74H zN4SOs+mr)zT_Xqf$pY}eAgCc!1ez?+<b<@m!TAEzQoqFp8M-S1x5eRyJwV#?pa}v^ zcJQzrq%F=4InvMw+zLnA`A`fB7SK2zd@MvfH5JwtcT6iuEpp2*DhIXEk=x}sl-vg8 zonzp<b3;gMigpM4UGT{YD8nU;oHidA7&&b~XDSF!aP0BA&Lew?M|MW~g2Xvlpz~$K zb}(*{yeMaNk;nQ9kM#vGf~;#NdN0FMu(jqBYo^p(6jZq)r~<Ol08BvkEI=*OzbLBT z;nv}HgHL2a;Do{n!7~IWg!bfiFyG)8>fnM@Oz@%>d4#SAoO6(iTF`t7IC+7OcuQeM zoxrMPu3-jOK`h953VFI2eN8k*TC)Qm6NjiYkkTCXW+_S|7D+WC=hQGEZ|$#P0ZoBn z&3X2?ivAjmV-y$}dO*YA@I?lYFoBTh6$Kts(efxGWIGcZbd40KTl*5UD(V(nQf6^s zX=;%%sQTapcW-ZT<YeX*7bGS__Y;9K3?%>T0=4zPS!OYa1#0HOw(^7L+(4s)w^)la z@<Hc+f+%o~K{}HYGT1GcR8$H%!vvl;q!G0XhU5!St^irxzyMk@!#%@sM&be{NEfJs z^@fNfXzh<CnCNi1D=IbDct+Vw>p9jJMb)o}s()Z$<n?ESkf06q(DSIS3#eWaP+gF+ zBzuG6lDsQwR*-S(6CxLFeJ`r{UKH@VBH(ucj6j<X{JQ*R7|p1-$fI$EN8<t*Vb&Kn zIC%Q`y7(p(%`m>mA%BHK{sIhvbAl$LpQbSAyim{)xbg9~xZ>k;^HWN5Qsd)q@x;d$ zmL}#vWQq(xq3a4FLP10VhyVq45oipxXcmaO07QUe0o=}B58`eC5!*n-4iK>iL>vYU z-ZFsg2M2x;s7(eYEI>_jNpKswfdK+PuvoFmd|*H&Y8WM0Ek7{82@5t>(6KH^q=+D^ z$Oi^EVdcxns`!BciOgV9VwL~E04IDH<ykd9Fu(~HMqUsfPN=c5f~Gl<NFhFu!{CGx zD=TRB0f`inW>xvX04MB(Sv5W|zzH1zkajp>#KkK7fdNiPv9l_CV8A3Gp#}=DWDo(# zgGkd1E#MRG(-~@6kqa{9%CLqJF+_}7AffVH7-H3G*=ksER{2QhXQwdMKn`MJ1`k7m zj?+RpRe+J9N4<s-`;-Lw(qYu~5Qu^e%i&>=vlUoyGzr*Hn*`~Mwd^VE>5MR(&Roj@ zV}s~)=2}h=4a4cowOlYZh)!ol-Dyr<m`-4fy;RFq!;T|V*;9}f@FK@l4N_>MHb+on zjgg@S#f%j86wX#A2#LH}qn4+J2O^4gm=`wPoX9#sl}RlxTsO)I4Os2Q)XfLijdErx zk-GU)IBM8axRKU}K>Ube4-sa;4i6(}7TCuqJD*d~PE&AUh<#csSi^z5BOON!^CGLk zh&xVXSsW(tA*;bKfeTp{hY9@1YA{UTMwZ24f&j7_3=<d;r^F(sTjX^(*hiddxKog} zH`Q<>9}$O|n?WZE5n&6`#1pKjWMHV_M!x+6-Ss`sCNTC~sbNUrf)3tG)G%US0Z_w| z!h`NAP(K-jHF^EOhqqx}HiYO2fEE~ny91C8GidD)_@E<AUZl}2(8NAu%QQDqe;C{@ z0xv8?bP*sYxp5Symc*wdmL%R11hI@$;tPuM3sQ?pGE<9-K=rex&@I+9&>8h$yC5A3 z?mW<G`e4yw$a!+SkmUjK#W|VDsVU&IkH8&`Tb$r?L_s#(;sl>13SolJ69qF_3&0e2 zKGG3&cA%Qy9z-~R2+*;kkd_$eNGE7#1&IPT3~uou{0ll>uILH_1H*Sv)9VlT{PG(j zVhz4u*%<hQry5TvoKi8-c8cvqUd1cCiXRx5SzSP@l2~0BI~YH(F>s1q=a9L?Au}Uv zf#aOW6^biTS88s^zNqDRQP$}qhw~K<=L=wTgN3!-x5;;c?nM@bD=Z2ZSQNk;m1l5* z-~vk!T#*TaJ0wByg1GZpqcc_)_`)vog<atbyTBXX;C4e$Xae(uzzNPX1ZOaJ)HZk$ zA;8Dq5j;V0g7AdI3Bet{4X$^2g)sdj%xd<LL73I7gYgCjUq5db?}W4&K~u687%s6` z;kd;5in8fNG1H42W>+}OE^wIL5R(DR3NA2QAPm`Myg>Ylm}Up}4FULu<QcA41eCBW zr@AO$-oc8Y4t2>E>lIbAivngOsN?0IU^YW(iWO+z^^D{NiZfEL2&*h8TamP+YJ=hi z#|_FG5;v$`QMb4#Y;lnn)ckeY6Lo^|gyIS26OJcXuh{xs<n`-d1g$?SoG39x0(4NU z8JGZdJVhrY_V`@qk-x+vzkqc?;u4OFJepT{G%xUI-ryluPH=)^j|X@wuj7owIW8A@ z6tD0oUf@yuz{kML*W(WA>P{%^@r5pPHC-XN#9~G02E#QG7kSLC@R(iTF}uOTi>yxj z5|8!@*A0qmJm6dHF7TLsl;q(w`yj)>!8?Jmsj8vs1_x(9cNg~rry0x_Ib^PI$XtM- z56lc4LJd{mW}qh5E!Mo!+=5DQQE*EMa%39#J^;P))TI29;`qdZ%=nDdoPyM%__Wfz z;#)l6T_t)snR$sh=|z2@{TnjK+Q6d<@rlLp8Tq-OttPjGk!3*(4M8WNp=f~$g13Mq zf=-hJwM)S{wdgn~r@Mg&&~d@PIBXz&3%jD*3=9mQLCfNW3=9k(m>C%vZ!id5fT0gu z42*^s7z`lj27~Yg82Z4*z^HwJK?{OzFeqMtp&Ky43k+HxSY#PlK7=rqvoR`sV8Bj( X1dD$GlTbBvyef>c9~dwRa9{%fjO3uy literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..352da3ed9b57880cf0b7df073839000cd4bea147 GIT binary patch literal 6565 zcmd1j<>g{vU|@)xtCs3x$-wX!#6iYP3=9ko3=9m#&lngOQW#Pga~MKEG*b>^6jKUA z3R4bqE=v?EBSefXmz{}$ks+5OiW4ltoWqsN9mNf1Gv)B)@<#DN+5A!bVBG>yf+-9s zEIC5C!coFtIo2GZT#+adFq<t$G*>K2jFBOQJ%yu%Axb=jGli>#Axa{JJB6o(AxbiZ zH-)c-AxbKRKSiL0Axb($Fh!_^Axb7iI7Os|AxbtyG)1h1AxbVqJVl~~Axb_)GDWI| zAxa@dIz^_1AxbetHbt(5AxbGlK1HF0Axb%gDVRZ1@g*n(G#PI(YWZn0-r{vD%}aL7 z%gjwI$;{8wWW2>uoS##c8edodl1xd>D^86Eixd@WGTve>E-Wg^O=bic0mYy&WMN=n za0Z2=9RmYH2}22E4MQ_yGouSbtXM5$4MPp1Btr^gHcOFE3YcfZ02O1(W+;+LVFK}N z7z){H7=jrznf;zaOy+QaF%D=l-C`|D%u7$b#hjCxr^$4SBPBH_z9b{H<Q7|IUP@|q z@k)jwUIqq+U&;Cz`MIh3>G{P4`6-D-V4e|zn^T&XqF+#ySdm$*Us@DjoS9pYlNz6# zpI1_ppQE3fn3q{lnv+<PUld=KS*2f;pOjw`UtCg}mZqPXR~Db0nU||qP<e|tIVUl< zAS^W}KRL6cQV8U6J_ZH`0Y)B14n`g(4o0R17RDle1_p*?Q0PFs!@$7Kz`(!?3#kGI z28J5O8ip(e35Hsx62=8gHB2=OS<E1u#Uj8^!z944kg1j#EQ2NvQUk(B>R4)+YgnWi zYFTSoAu=Lh6KdH?SZmmt8Ee^U*n$~WGWuz<-C{0?Prb#IS8$6tCBNhrdsbp{d`VH} zEyfVAi#6GAF&5ooEV;#)dW$))H1`&BN^0IM*2J8GjKm^t1_nqtg9Ez=<mp?ynR#X2 znR&VKUWv)^A@Z=m;9`_w)L`Ub6kz0FW??Oo!4?>x00AePDQJO_#k2qv5G)HBYnf}9 zK|xW=lFm@eS_1MM8#rN!*0R;GE?@^Gs4O;Vh8l(}jv6*ehAhq!#w@NBCJ_c{1`&pZ zjEoG0;w6k(+%>Evj9EM>%)LyttTikpj0<>c7_!(FGS-0Y5nzyDs9_agFlVS`DdDSO zX=bctsbL9b&}8v@`Tzg_|LKq@{9(V6@fK@JNo7H*CV!D2C@?^ge~T?AGcPeG{T53> zW?os52uPfhfq~%`OI}KT+AX%kf`Zh%lv`|B`I&ho#kW{N={E5eQ$gk}*2JQs#L8Rj zX*v0cCB{a#m{TiLp|MaT1~Nt*L`Z<l;w&gmP05c3SpbTKTLNHH;?wer;&U={GfRqZ zF=j(z1(XjUQ6mV8nt<Ze(v*D9yfQ;j^ne`5#mL9R!N|hN$Ed+5z$nMW!NkH;q>CO& zpw!9+OR=DW5|LtS7_u0%m`a$lSQfCBuz`3WoW&%-(9F1yQGfvvaV6}C04)+=U|`VX zfP@{$9YvxbAA&-$NCD(cW{?L#RziY-6)hNwv_Q&10f6pTF$}-PXC&sNd4W6(ssy+g zl~_2KI5=3CiuAF06JCUQfQzs!P=;j4VqVBt3(A-f4>2rYS;$b!Qo~%rumGGA7cxRJ zEjazKfYT3i340B5GdSG_Gr+tH_g|4B$o(LX-x5wv%t`hQ$#-=xN=(U2%`1t|_ASx` zNh^T}Q0buw^*$&8Abfs{BfTgw#kVBi6+H~(pkWZ4nV0TZmY<oDn3tRy50CsrP+0IV zFfi~jb1-sn$}xi@-3UE2K%o!9;GCI-k~32nK?PD4Q#wNm(;~)nh7@Kn$+Czsow<ZD zi#dh0m#LPeh9Qe33zXm37O;cZAe_Z2z)-`S!Y0g6!y>{U&Hzdx;C#(m!coK8%m^yD zf*CZ~{nCxW;Ih3YGqm{sU=L+5Fcg`9k`FVODl!HI6S4q^4H1=NU|@I&Dta|JAxRA! z0qP(P8Xy7`0g#jjN_0hfATe-&vlXFbJJy`U+?2#yESV{(d02}-QD~UQrzGYUq^1NH zmL?XZ7FWT7nv0QzMTt=ll)r?FEYO1*6d9nz3=ZlVXu_*u$YM%iOkwI}s%0u+$YM@m zP6yQ!Od<?WQRWm-A_R%CNPr4OQ0>49m16_xsA0&0@!3-tYB^FEYB^IFYPm|-YM4Ov zL=9IBdktF+M-68ROA2c*XDxRPcQAt{o8K+Al>E}9oYW#v02YC2`CBZh$@#@A8E>&c z8UdQ@Mc}YRBvf#yg3Cg1sDkp*N*+Wo-eL^6#aMWY(Y*+iif=I{-C}gS#gvnDi>Wl} z7Gnn1Fy(}XX<)!CP&k63M1WC@k%b9VY6&p1FoB>HGYfN(8+wR>90IRa6c`!6Vb;P> z%ap>L!qNgNl|bPJs&l~=N;*R=TMBCmTMI)idkJF+xCVx{TzG0ZYFHPrq_Bf3ks7uI zEGZlyR-sG@YZfSBv(~UJWUA#X;aC8|Ts15;EDM<yGS+gXGt_c}T0mLc;EIa7mZye$ z0Vq>2*YHR(WbuGaaVX));;rE>;mG1k;p}Cq<*s3`VW?qgW~v2MO#CTa*-S-QAe*uT zYM5)dQ@Cq*YdBMQdRc4v^0-jNQ+Rt>YWdO`YWcxF;L8$(`#_+Ee*sI0Pz_&}V2ywz zxMXHzD6B0JuHkQHtmUua4`$Hh^GjxiRBvDsl=DG2n1O-eGbppvFfL%IVF1^}Of}3B zpc;~K0jR;k4EHi?4J)*)Tgh@uJT)~Szc>@rnsiJnNiA~AFDeJM55Vc>77M5WStQKB zz_60-78|$@D@G(1NM;2m6xP(DqWmKC5{@H1wZzpmAisD!$ax$L3=CY19E?JYER0o( z&_n|&PF+%qll5$J@{<#DitY4ZdQh6Jpu`PIs~~?CgYsbs!ve+{kWZPQe$iwCI~nX) zO~xW0P$2=XTfqdl?ZaM@?_W@onY$O1OhM%h4^x!_PIo{RqqGS@nn1NI$QhqOIje*L zR`t{{)-Wz)hB{^?%PrR8<jjJ~TO9cy8#Aj?ZwdHh=A|YUIp^mUmlP#t=9Pe(9xIuO zKp6&{-Oz)ND;ci#2q*wSwhJ*=DdTi9LVXb^u!}%h3tTw71cl!%)?lCDzyM9=TWq<h zB^miCx401+if^$c<(KBA6l*dSMT5dF22^&7K%yAbJSqZJ%S9?6DP<4=3WA~#P$PiN z39Jzk{Gb{f90x_<dQlam6kO(j66Y;ea8vviM{Z_bW-cgD7%OhEf}2y|2E<B1NML~5 zQGOt;0U!dD!iqqJZjlR!8xFFRDJj1w7R2)f@pvFfGrqVav8d!0Q%S}xzMRZFP)`9G z*0-38Qj5W**DdCxoJ91H=0^+Ct)LWQz`(%3!NkGF#mvIU!pOlS#Ky(Q!^p=d#KgkL z$0))mz{thK#VElj!o<QV#3;td!NtN@1ge2ik_@Pb14RbN{hvuK;y{%PxLAS|Z_M!G zjTu_JNtLi<u`Xb%ffaA;3ph&H7jVM!I-+=E0-FOV-axHrE^zV24KCj7O4ze_YPd_- zvv{!-Z+zh5EfZu@7C)$Xt6_i_aBQ`FdEDqSDZHS94_d?tfQmDeB958-BCg0C6y=~+ zX%VR5SL6-if-^Xn0Oj%`P`ddB68C3dV5kE11VQB)N+T;gvm_%hAU@j{OQ{3OWZ<He zfq?;3(!#SlsO<qNT^GVSfZ);x+~H%YVOYo{&H&B)n#@I@qyxzh;Ifk$QXGLY`YldS zb1=RnKOVh3SOiMz|3LX1RM2rSOE6XGL6apR#}g{5iUL950S>kx5GxEsfI|#SfP)Q` ztJoPC7>Xi5!DdLL&4>a7t;8vc1lbV<sySG0u_P9y7bA)laK>H9TvP~B0xxV<A{RE` z!UMIODT)Ir2Nk+Sh(ZQj9-$U8Me!hY2_OPoWP#ezMM)qosLfcE3}S)34JJT-za@*; zw_iYU0*X;khlGm_To^F&VJ!@#7{!=a*uaGWHw$A?3AXkQtS~6b29>aQ>T_ZfyBR3f z2q$(=5)yk+4k(mB2}Y9{Ti3@Ek+_R;K}x}42_`@R3Qo#)AZO)+5)<BJjN~k8C1ZDx z&4iONlFdXU<08-yP*D-cv&A5S<aFd|0&-R<NN+iMx&XCkU_Fb^6*zhpHQ){{wC8~6 zRj^9n>Q%5o)v}}XD%df470f9tuwKPVR=*-kkQ2dO3M&xH8k9^qAQ7$!5&-8-aGhQT z;({tTa8srTRBjd7g2ccn0Zf3B!!1~EKQI7ZlDWobdzLUVFcgCdMFCKq%FM;c$B5Kj zU@8K2%TVf4<X|cS1yfNaZpVRgETs7Y@9B`>INu%=$HDqCpr!zm6f+A`Q8m6eES^L4 zI0W?+G5VGqU{Myd9w(&th#AkUDQvKK)?`PD=a+x~|Ns9IRM`~~i1Awj;9(HZz^iX! z1*q*#ObAOsi&v!R_k|j<1SymmSr{QnKn&D-WRwDpan*z3SLK!{(%7zET7FSEcyRX? zYhGz?K_xh8-V)4BEGfvzFUiSF(krM0v2L+JY372Ql3UWb1v&90`T057nI*+~xgZ6` zDSC+&sl~=Ax7eZlTWnAPrUK(zB1k4C!iL<7K*5A!S#o|(eo^r)HYm-KoS##4i@CV6 z_!b|y3!n$<0Th9n%(plT3m|>-Vn}`gwJ&b*KvY5oCW}BFI&ga!JPr#U=L3&|6@dnS zz{3*Yo;0|v39e&`Km`goyMU8r5h&D<nz)FJ$YGP4pHiBW3M!i!ia|MqgMovIhf#=; m2Rs_i0j7BvIoLROIfOVQIm9_6ITSdgIqW$cI2<@6cq9N+mUG+y literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/inv_kinm.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..523e8c16464f8c63490ac3fca3f2ef086e483ae1 GIT binary patch literal 14346 zcmZ3^%ge>Uz`zhWS1r}Yl7Zndhy%k+P{!wT3=9m@8B!Qh7;_jxKr~YhV-!;gLkd$4 zb1q91D<edVEtfr+iIE|fBZ?C&!kojE%N@lHW;5mR<nl)GLD~FK{9xSzQGzK9DJ(fc zxx!JxU^&(tp<IzD5ipxAM>JO~N{o>qg}sF#N<4+5g&|5Jg|merN-~A3g&|5Rg}a3z zN;-w7g&|5Ng|~$vN;ZYBg&|5Vg};R%N<KxPg&|5IMX-e-N-;&Kg&|5QMYx3_N;!oo zm_bwICCGD{jJFuI{4^PF@w%1fB|GM2<|dY8=I3cL-r^|E&nZidFDw8_rljT-r^bUt zii$NEZ?P5^7M0{CGlGnOVpawQ24)5ZhR+?~uqr{4D*=f^mDMmTV`N}h4VOjbxiG}4 z)H2pE)G$gy)iW@pFlMuWI7O@}U>O?*1_m@0OxYj>3`MLdOduJMiXPP(hF}IwX20iP zDFzM)7~_B@(=FDb#Ju#>Tg*ABd74bOI8st`;!848OK!1c=B1=&7Z-yfUP0kkvVKN> zZmNEIesMv5N@5Y1XN2J9l;)-A7ZfE{WESg}7R485<`(3n#wX|Jl@#UY=;tQpWfqj? zB$nhC#g}DP=@;cE<(I@4mz1WZ>1XDZ#b;;c<?0nw-r`NpNz5$>OU=no&Mc`c5@29p z5Mp3pD2`-cU}#{tD<nRbd4|VK-Z{J%h19MHseNEzWmN$a9c(wag(tXO;g+6}e2H7} z0=MD~d8H1I31Js`WFEj|QZ8{TUEo%_A+HRT`Ei3=xWWH3C{&U`-UKl~7#@EXjG(k$ z!&t+R1ri72DlP_wS|;Rp%!2E!VX9%sg2ym2I}03dAOosc7#M1pK;$we28Pvey|v6Z z%p}1?WE+v$MA*(!!(78s#m&G_%UZ*VLoX}L<+W@j+#tU|a19%3%B^LqVGCwh$>^ua zc8j?nKJ^w;UcoKql>Cxg>{*G)@g+r>w-`gfk*mpmi?QeyW63SX)LYDXrMb75Q&RJ8 zu_opeWF!{xf?}SXfq_8*0*d$<7#MExX6BW7XXfR`dnG2vhZM;(Fff2{@kDSExxvBN z;n?Ha$=Tp|my4%IZGzwg$0?o*7?*G^;l3!My(00Vi2g+`gDYGH4bD&nQzT~u&WW58 zeNj+jh2cd(ofU~|GIuEM(cGhbQP1@R<3&Bsi(Fn;xV##iZ}9L>Q10|>aJ|7VGNEuv z<&^4+{7MS~FY>E3czk7J5Rjb_agkr`3W)oGi-AX^!M{k5fq?-fX@UF$N+6#v;7(fb zB*DOdNLmX(x**|=MAk4Mq82R@m}{6J$*h(oouQT$CBhi8*kFdB>T+R-b*g2nVO;=n zCrk;P1{FF8ouI^0!;r-RldfSy6jWK9FwqiZ-+-$ZkW30w6)OV+$OI6L>IO!J9%q!S z08eT)tSF}OfJ_46HOy#PiM56W#RO1N0x4vu!RhlFX5_F0WlvB>uVDqVco`UKSxRKV zMu3SL7F0Laved8yGib8-z5M_G|NnHb8ipVCD;aOGrj%3`q-ydPi7+rQ6p4b8Fk4P$ zUSdxAEtZ1Jys{#3kT}STw^;I0^3!gyB^DH<=B3<X%gWEpD=EIk3M!2gZ!r~Q-eOHG zDoU)p#h#XvpIBmSbc;E)G8LMSia=psBn2`{8e|7&L2+tIemuwmkmqj+fK7={%P)%0 z$;{0xDZa&+T?~q3NZwL_WG+Ei<_aiIEltVy%quH0WME(b;o?i^8SJTm&=k(;e3STQ z6wax<D4=;oK(oQ~2EW7%!_L|Uj}Jl&5>gjfMDFqmO)#8d)*0Iq`+<RxHG~mDb}-)H zkhsnve~CkWfzpD&C1DFSH!yDD*de%u`-*|%(V!!tCxQ<~Uo;53s2FsSBlrqO@C7is zD=s;u;(~&~UZy=<7sQ<}iaTEscfP<P@_>bBs?Zd%8H{t-Cdy5bTc8Qb#ut^$FAADp zu=c(v;C+$ByTRirzYHReJ^&T|cyj0saOI7y@ULOWf>+vz99ja;Qw&JCasj9^fusi{ zvIG<<P(HG0$ZUkkpfre@-cWfU`C6p>UIHp9VWwkEsYSvJ3=EnakVFYiffAsk14^Dn zDxmbk3`#km^Z`j<tZ3=0NDrhO<cVUC|G<d|l7z%Cl2Cj`Voq9-2?GOz7pO#!Mo&i{ zI2Z(lrf^Q-o|`l$bAIlu+_kP76t`&ZNZgaTC;Ot2=S3Z_D>_~m<-D)Rfg-ZO^8qZ4 zeHB0{ba6x|sOAL~o}WK~BeVn>FdzyMjSG;9DUb{VBiDndL0-#L!&Jiv4S0qHAUhyh zkjP~W3=FH`{92Y8X5^M&7AWArvUmz5l=cM3HgKWD0%m~<C1#YUW=3-(a}9Ga11zfH zaa*JciaSuG-x5wv%t`hQ$#-=xN=(U2%`1t|_ASx{NvnYfQ2R?08ug%31rh7FIMRy} zQ+!MEUD4xT4jTW#nR)4sW%-#YiFwJX@rd%of`Ne{5tI({!0F&C8-t)Q{$fNx@H)TB zC4Lon-O9$m%8M(r@36Qi;CzwAnYg?T%CX?2p@g$GPGQ8=FiU4hVOqqP&XB?kCRr9S zrZc0|Pau=Q5xj;Kt$t!bu4@np0Yx{+ZD8Hl>tbXRkl6^+K#8P=Ifbo?gMp!j1ys*g zF)*NYIlyYbjnP`x5>R6WY$F3h4J%qwVy$5fX3%8!OE(6C%l4Yg&`!h$dnki}p~xCk zsxX78A}de|LKXnAA)-nQ3=A(p8BUWEQUZa~rZz}}4u}Ay%_4At1S*S)j6q`Hw8(~5 z4BcYQNz6@2yv354l9~r;FoJ>&TpU5GG*M`Jj893-El5oXE-XzfN-Zw3XJB9e1xIlY zI2j_f8$WO`@Cr1z-sR@)v6v7Af(xWTa77ddUJ$W3%5;S50=M@?ZtpAH-VH7f_yu|@ zd#dLK&54{JJ1cgD;Tp>gg<C4ORA1C~IuLro@nFP-fRKan7qvq!%7$K%4ZX-8dLbh6 zB7Y=W68)ftqsa(r8-SA1X9I8|MJYoVq45Q3*nk2Q%uQij!-SULm{4?qOoOOoP6zdE z;5tEx1dDFw6oy(Ba773&=RoC9Eh~0YYgn<E!G=RE10um<(Z!y^P|K0RP|KOZP|Jl< zX)=NOD>Ym->@{pP95tLNENfVoaWXKhhBrKGxofzC88q4aZn350mnP+;7J-so5y-E% zSW=Voi&rw<VuN(!HQB)pGo+FaoEE_?HgH-571b+w5Q*;=W56xO!dr~)MWABw7Gu&a zM#o!BIZ3yeN|SCeW<XLRsCf!Wi4YP}KXO9TV_-m$9|Hr!EKpGhswf&5?s9PTuyqvn zl+B2^z@c`LL+uKOT7x4<lw*S81mOvVQ_3dFEQq+kp?8r(?+S-rgX04Zo(4yxr2CYE ztA}HP;0%om9Lg6tl&^3o!}JSIkzAm0fkXQuhxQc??FPpOoV*QA5CUWdsP`f<gK>u8 zjKl?k3mg{|u25Q$v_tFyzr#g-hb#OJ4IUrZaHLL98v&G_KF?tW^_5c?TNrAYQkYs8 zYMH?)2As}71USWjyCUffwQMQOEey5nB_N$3Nd^Xn5>OyR*|_Un)MCknAy%iBqlOiG z{hNZ;^kHPEVZ)&kt+-=k=+P~KcUBmXIt@4qW7Zlrv>eBYT%2aXQzeQh+=d#K8WvRj zsQ#?wN@u9$Mh>kShAdFI01huywJr>?>a{#I+}M4J9I`b$piTqW2!<?BY6UY<O=D!} z(LixCsLcmfSHoQbDwDx%28Jv?D1%`QdPs0HG1RcvFrfRembZo>3*JyoVb2ETOok#= z9N~^+7tAIGh8pG??i7w1-Wtv|oXc1l7*@m6P%U4cGC3x#;abMRz_1!#&eigzGt}}U zhdfF;<;#LsnyBH1%5!0e)vpz(;l~~)C8|h1LNc*N01-c^=ArT!8F~y#)RD~KN0qPT zui+17(B$?@M#^pspo$XI3xEz%)-WzWn1IAWAEc~hu3@TShGb^sU_ta&P(<N25=`RY z76!Pnx{~FVcxq}uesLyf{M9k7B(=ybzo;BE>H{vPZ?S-eT#LjQ7#LQv-C_gxY>N?v zGo(oYE|^(Ui;D7#ia}mQE`vGJQ%hW31M-Wj6rrUtsQv4bTAZwBlarsEm{V-02UD^g zG>!`zMQvcX!N-4{PyG^~`U<v-eEL`T^gEbu@bFGx?sV^9yel9yg?%p59JU#S3j}9! z&*8o(pmIe(rGxbbzeooUq?ZY=tQi;>KnV;S`k)5SbOw~l5w5O=p@spqSb#O>nZN-7 z4gyWaqEJvJ0@4L-^RSoX`xlgC=2j`-^d3~jUXV^uJ=ehSlwYu?Y(m<Mw2S<zSNK&g zaHv9D4bGO(1OdukU{`~xuIUWqcBN_<YZ#X?Gcc@%rw*vUR<hh;El$oXsJz9I5Ar}} zRq8DPpUk|}#3JYXyyB9g#LT=B@QBGuCP;*Xq8U0w4XIzblHm%glyQ0=q38%Geigut zKu|3x+EaI(U+)sX-ipFCl{*x71YYE~zrt^Sfx{jg8bzQYy9iX8gIg0XL4kgYHP|OO zFhG;}7F%v=Nk)FkEpCMI#kbg!@=NnliZz*vQbFz5G*A;+1d>KUC3q32;4RVsNrAF( zQ7niR2kL^eIe|4oVmcTk3`+4u;GUT#hzo9{fYQ$`R`9s_EsosGyv$rspfOh5Vg-*9 zfxGW31tEb29v%t@X^jFAps*|Q2C;lV1gKYbizz9;2sC_E6b5nv4<v8J7ndX!mE2+~ z$+*RrlbHt^)`!N$EoM+-8dN9TVou6QEC!W^ker4@LL!DAEi#Hgsb?!F{heS0jqZP7 zVPO^hzyK$Bm{`?5Fu(~m22L*6(C!UR?gl3?@qvYz)q(K?1A_d@!@w^x)p-Wv9FB>; zQ+zM-DP7@H0+}NYCOVjIaB%f=c5zNn>E!R>@8E}Vl{@)*_&WGH`0nxw^~7E0Rl3Bh zw7_UV(M4XZE4*3_ZclmmdfcZvO>vuHI3sYP_Z07oJPKEM6dGJ_2#YrO+~pIV;5fyt zGq)!fG`?j3CO~$HT<4I!#34PyagN7=pgBHQWb`g_=wIQ`zW_#eg+-@W%<$RDw1w+} zu=Pb@>np<64L%P<<tIcfU|e8$QB?JcsA_}X4Gzv8_6devJRLlD`GkA&uJfs0;!|Ca zzM|%U`-RB#3k78t_*5_Qm0#g2?_lm=eu_;ENI}LGzKjm$4_piaLJb~w`9-D%&k&v? zIx%WW)J1-UEBp!|S8Icb4(1yi0{#45{Bv1nB+p4*;5;YmqJ-uZ3C)WfT30x<E`ZTj z5tQK<@MH=&m!OPzgT{wJ`TTPmg$=|Sq|tEH3J%^31la>Est^>`W*~Nz*qVVL_d^Xs zbX69BsuZXwf~vvR41_mv7?8%wk%w&%jYEQUB(`QCynMz86G$@<t50hfvOpyn*y$+E zKv0nc7DY7;+zdqa38>TptE=HIfj9ja7_vY`0a%=*W+2=Icr#F~1bet6xdChqm;f~c zYZ%}SK{f`4)$sVK<;znc=uJwDrl2C&dN5Ig)Nn%$J5(OLDTp;*Mw@~RkQqp5 zgP<r7R5OKuY6(z`LMjz-eNqIf$ErZH$Dnp0%4lPFW=Te1Kzz0@W^3*n$hZg)0jjA` z$|-o;rU5)mjH^8i>Z>CS>f)LRW<YPbp|pzOc@{KbjM~Bj4N5^hugP3w268HRbPQDY zuVe-{^}v<+El$t~Y<x+6ylXKiDJdu@K*9x7wO8puD_cV00&NEV12uv`BeM+*AJ`Z~ z#jcBJUJ}u~D58BuM7x6%t!22DX${+o${lJKH5{*KI9}v;yujgzS?t4ulSWO&q8L!n zB!E%|C^TWd2Q^}<42kqTqM63d$iPsP1TqnnF3?(eMadwE6i|1A<rYg~QF<|=DF<#W ztz<5$1u20y@Kz!>@W2f=)V5tw21q$LUJ(sDa2pV{VONw1QU_Xl0BI(I)^-%-fW$!E z;i6oS??DcR<O?JcoD^=!;thu)P(uF#iusFJ8(;#gtWJ!e)fI4(2YVZg3DNq3w8X$| zFhqNZiFJ6ky$nyJUq~+ez`#(==*V<|PxT`M1HTi~7chyf9R^VkZmJbs;VbH3{=h@6 zmYCRROAK5K!-&zA*l0@(lon9iUZ}j$mKX!U4hLdTj-r;BCuU2`3{>L|ot7B3sd7)q z&<v<$R#XA1ib-sTA%zOG9cBj#6i^Fr2(_n>e1T|Bxr01HxII-=18U3Efrxq#K~aOo zv#6bcfx!f1b0f$wP{RnNg%9f6f}-fN6V)eDYlvC`f@OjeF|`V6FoTLL2JqNAb*5H9 zeI~G(SZqd}T1DztgU0u3SdgYxnb)vjn_6AT>R03l3TW`WsuPIi3>w_zfMg5AG7|7O zWh+P?G|~(nzAXazw#XHfX+R?hNG(@LFXk3(8Bt&WybI$RpY2)H1)3>jWMC*>1s<{m z&5mNunL_75uM4VO5>&e=sBuM5V*|$p4$F%imRC3|8yr8dF>vyt%$$PEq0!7Kj#Wvc zi4l42GsTIqs1tY619ze+O?tjXpp_y$lqNm$r|dtlF>vvLhP^>E_ry)w6H0p9M$19a zA_UMR4!D$LKw4^_Lfq0A4wRWYQ0)RXDTRfog*BYuWi9y1Ue+~i*vei_cBHcR<=_AR z|Gxy;T|}T1z9j%&@CVwB;hR_in)HNBM8Oxi;46})prZ*$MY1o{q$1Gj`z4^faRpDY z3@v1@3o2g{RK6&vdPPumMZ^UTvx^*NS2)Z-<uMP4K&;>=dO6EoPVOFw2}ToCW(dv^ zo2a{>;sU3^MNWe&oCXa}5IMsM%HXxZItwx`aOz*=)W5>1-{5oul#ibBi{oATg2+Xh zDz`+Dwyx-<<rkHM_paPx%`43<s03Ghw*+$&OA2!GOL8)k^a?6LtXphQnz<mS<d$@9 zK~8*0etu4NW=XMLE=YlKie6$xYO!(3Ep{mX78_K6slfP_2$G43uzg8IpebY&%aZeR z@{5XZu|a8;<oukXTg=6k#kcstld5|8&^cA`B;zg4!UD+hr(#HL3~DUg;(@4ytWYlk zE$#u&AcFT<fY*Y97xET?#tOk}=)jAZz*BkP;UsVq3|vWri%W32Sjhlx&4P!desS33 z=BJeAq}mlNVPIeY<-Fo9Mh1ot%#4hTHyD&Jz|aE*p$jl{gF*fRD!Rd-aRCRq!Ju~m z72RM^xPXdoFmN@1;SC1S3ovwpLGA)7LUs?tVVG(@@USzAd|-eRQoJB8oKO)1ap8oK zAczYm^h7{hIAJ6U;=&0BMz{<Eh-GF5l7<s?j5Z)XoN!=*GZ{cE2PTjvuud>xfX0Qp KkpYDTjw=ApGLaPk literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..889fd0df003b9827bf0a395a686252e188e1d423 GIT binary patch literal 3429 zcmd1j<>g{vU|@)xtCp%K!NBks#6iZ)3=9ko3=9m#HVh05DGVu$ISf&ZoD3;UDa<Vl zQA{Z;DXc9FQOqf9DeNr_Q7kDODV!|~QLOF^DO@SsEet8#%}h~jDU87knmjK-=K3YG zfG8;DVPIfjXJBA(2HEh0k%6IvVF6<e<3h$-mJ)^qOkfr(Sd4igV=Wt448&qDVaQ@m zXGmdO#F);I!UQIn7cr(YmoQ|pq_Ffd)pC?DEMTqS0I5r1O<}EJK*%j%1G#n~QySAk zMzA_|h`Mx!6plrVFcV<1S<E#oDV#N|A`B_4>C82(H7p=HogoF)K2ETGoFMxkaximH zYy+ucuYsvTw+$wa+gDs*Tev{B)G&ekg~L}Mb?h}@by$1_6K7n+n8I4amcm)XF2az` zoWfeeUc;8oj4-p78*Cpp$i8%j6fQ*Qp!f=;ioFJ=3f))SH5@gZ0t_`=0t_|W0t~?n zn%sVx%r6-k7#ND+geEhD%>)yL34m3AL{~E2VofQjEJy__d<jaIMPP9jumBPP5`Fpq z|Ns9eW`GRR<haF@S8$6tIluT8b8%+gE!M=MqQuHu>}fgqi6zEHx7ad@OA?c_Z?PAp z7H1?Dq~2mH0}E*K++s|*#afb)T9SB+vFH|SVopIu;w`4+l3Ps0CAXN86K^pUC*I;K zDN4*Mj<1T(i!UfjExW~2lwX42aYAG(;`45?gCr{A^Hwqx$uKZ5{L0qP$j?pHPtPwd z$WKWu0`rUz+?>+96#ast#EQ&f{nDcN;>_HFoYeT_{JfH){2cw<#JtRc(wxMS{G#}> z%qsn&{G|Mn_~Me%v^4#!{LH+P_{8G)jQrgA+{6;Sg34Q5E*>TM9{IVxi6z3Ie5b^~ zz`(@_flAC8jB<=xENYB$j8HZ+h%LZWB*(zOfRcGRkuz@t18JGJh9!j&l&NaiL>OvV zK?$&isfI;_A)N)3#XzDdj37Q#h7}}}!URg7H4N!2kSvzY0?TBz95o!l3@cgvia@0a zBrULkk`gGHtYj_X0I@;tEdrHxMcg1RsO-_?ED`{*1wn)mhyaC1kqC$-3L?Zngg65O z!!1@958uQRaCAWkS&%4qa#3nxNh(;*2o#tSNP#KGD8<Od$i~R@mxY<_FAGZ%D5Noh z5ac#+`PW2d5T-EIFoA;&6qGE`pko(dsA1q{00kdd4#Z~x@oLy=7(i-p1!9pr$jb^0 z3=AX$4x2AH1Rwz;12O;{I>M09@y$=kOv_A70jse?4<#W+8AdVWP*MRUG*EVhRuT*# zHmKGBhmsEi149i%2}2fR3Zn=^3X?QLHcOEJGQXCgh9L!16Tnr7Fk~|nsi2F2)CDtW zviQ9OC7PEY8^Fm&lL?$&ZZQ>PYBCmqlLpvJU;^wfuH>A=+=38LY2XL)4k%FxG4e5r zF$yuVFcxWmJOFb%j0TlT;EL3Qfq@~FA&M~tRDCk0FoCO2<`foi^~svT2C6<&*i$%K z7^2uxI8(S<7^2uyxIq=@Emkj3c1{MRY?!eiHY=<KtpkM(YYjs@Lk(*RV>VL}ZwX@! zQ!`@?Lp)O+a}5)St^x6wYnW>o;#q1~Y8c{KOV}2$*Dx&viF1^&E#QQ*xxnHqU~z7+ zIE2kp!dt_T#azRX#Z<$P#ZtqN#o7!qhc5+Gwlnz^fqbgTe~T3yNZ`PL1R^-6Au<^# z-4}uLBsjZ)!mtQ6&w=yHEjCC|rO9@Sr8qSw4Qqzg0wp<c{D28?Byq&YXXa&=#K%X0 zq6m~P*%&#Pco^B3*cd^OhY<p07_0ce;i?DIkPJ$2umA?JLFogW_-4SeC_@$`z?e%I z7Jz~P!ecFA$YKKpP6{I^SVS0V7_vYak0Fb-h9Qds%;yBjgGw`y3J@Ej2CRb_tOB79 zR_ZZiae!3SFl2F|WLs`fQ>TU@o(B~4;Eb!uTZ9N#aMDE!TyW6`FV%{`(E*MaP;pqK z&A`C0lCek!OK@_R7No#Rx)M-`g0%86@-gZ#YA^~w^DoceDnUqCBB@RW6~eIK1hGLF z9GsvMvxXsy0i0I3OBibyp=p()hB1YyggJ{vgh7k}oN5^sur6d^WB_?|B{L+Ay#yyT zwjyxkgYq0CnSuSU4)TFM$T!Ro-+;pctWpLf1t!4BM5rjgB(WqlKCd)4wJ0+=F(;?8 z7UVsUF<eYMi~>wN%vBN)Phl#>@EbfYI)U;cQGP1{B@R%s0p%xlP(lIcCr*%Giwr;o zF&Az>f(lDWmH@>^5xDvS#bOaCY|+bOey9h^GfOh!eO)}-Q9UZdR3(byQJ6Z63;_yx za14TSZ3!ruGd6?T_>6v<Ot+YeONziLsYnkV)QIrrh>tHyEhvhQp9r!XWDplp282lY zX>#1+h>y=p%uS7tzr__FpPQdjnge3<#K#wwCgwn7igZEdgJQYJ7{mgVtVL!Z7T7uj z0gC7%O;9%nWG5)4b1-l)a!7G-a&Yl*fn_w=Z?Wc;<`z^!@{b(I6i{_t1P)+u>H-;n n6ry0`K_=hguz|SG4pdeY^D!_ma4;}2aR_k8bMSBo2yp`d**2-) literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/joint_as_hom_mat.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..936a5cf11707e3e29e66b708b2fad513c0bec527 GIT binary patch literal 7553 zcmZ3^%ge>Uz`zhWS1naff`Q>Nhy%kcP{wB`1_p-d3@HpLj5!QZjGPQ9Of3viOexGQ z3{lJ}EG-OCEGeul3{k914DJjmY%L5a?8}%K7*;bu^)p1Vr7#9FXmY#+3HT+$%mwi{ z7#J9s85kHoOR$0LC}9K%LU0zGUBkGHk%3_~oL|d=q7H5khB{Uhb#VPP%xLDZp{PTc zhpvttMIFf9V5g=tq%baGOlL@80+Y;(7}J?cz%mdb3-04JET|z<%YkAdD?~j5Lk+4p z+@=)P6xJFBVohJb1~U(tPGLijpEM@4@IbYl9i|(XtI`=#I2JJy=VJnP!F^i8lEPWT zTE)u1kiweIT*F$!0;1CyQb-A1P8^|&Dh{_3VmfjDAUkYP?M8$xsyMQn$PZiGW|Es; zxp0IisyN*BHB6usOY`)KYC9rKQN`h|qA0!MHj{A?V+v~xTMB0lJ1E_zGpDfDu-CAq zGZX9oT5cSniz<%n4=!T!9ogv>)ow)CqKYHCiTw1+UBgksS;fM@P{Rc#xj|$wgC@71 zCi6>B@m&Neo56%8Gngs@6|G<vOaQC`B)XFE7HdjLWkD)f`X$JPVAU&Gzye4FNcGGA z|Ns9-F#}|fCdVzNyn<WI$@#^%n2R&>Zm}j76(v^QVo%G-Pb@Juy2X}JT#}faeT%&) zwKyZOAoUho8CXD*=N4niE!L8Z)RM$oj77It6LShO5^pgjm)v41F1f{&oOp|=IPn%= zNl{{6aeP&LUVK4OYS}HGqWlsBj}szW5ubO99VAf^pI6Moz`&q@1b$`fXXNLm>Zj)y z7v!fT7J+$22yRYkUW$G}QDQ}Av3_Y$d~s%OK~8FXa(-S(QGSkoZem_$L1|86Nq$j$ zS!R`fQGQZ>NqlihX<C|oR(@t)Nqk~)d`5n5d~RZiUP0w8E*FoIe2@HG-^7w4c?Jds zVFm_<;#o`#3=Ir-Ik|hpCa`wO_Q*Ck5y|<=#vmp!C1OT72(DlU!42XdctOtcsL&Cq z3!*+3MSZS_`ZW066&0Tnxqxpg(-w{$iaQ*4B<>JD%XEh8f<f>_gWxL$!PgC<FBwE% zh>5)r7k|+p;fg`R1^L8_qDfaolTbBF?vUIec~<F+)&+ymiw2=r3_>r+hg}p6zakpm z;P;h-K}2kd#f+o{LJOQ0iY_Re5wpT+MbQSu6_y*)LGXf%-O;2YSr>!@FA4`<5e{td zxho_xMRP{XTB9`<8{9$gsL>It3)+4cwf(MW`(4)#yQCd<Aw1$jWYk6N=quXM7i3~C z3dLR#ibd7xxxsUT=h2`eQ5UoWE@}r{(GIvE6L?W5=!#HKgU?qs20`H|95d8GaDh1p zu5bsz3z8;#o%VQL5OBXJ;C@BGy}|RYfY21q8J0_vmSk*T+@QF@aYgywq&-;|)I2Y$ zd0tWTysj2_NiFa~Q1FG2(2HteSJc8TNQGY%h`1sUfueauB?um6I>L29&HJL7_Z2no z3sOE81$?gv_%?WcWn<tM?8%tH4uT657dR~_TA{SCbb-tU#tljvf>u;s5Qjt{pU*`; zpDTPm4eodO1bQ-Oh%Z%IqOrmp1otZK(Ym1IdQr*sijwPfCErU*z9$kd1O#4G3c8{c zbU`BcB45ZAz7SNMmMbh*Sc1K*<bF}f{fd(N1qqLfe4bbMJR95}a7)hcxyY@t!tx@w z$%gWa+;$gO>^_59FWB3hR*XZp%~`{e!U$;})v$qTn;KR~ty9BP!vf-`vw+%65V;ga zh!|W2xLuXblEMV4pK2J=Ss*Q+bQa>;KD8V*9Kj4LS^bJQ85kHK1u3W@Uj%Buu4F9& zbt#ItK~kX7s)!fF;$vW7(Bv!<0<l4*Mv(}JB?=<MK!iAmkN^>q3=9mnSY14P6HAIg z=EDoSA~}!@cXCl`Vo54kzDSjUfx!q=25n#<w+y<=!`I_6gK37-0->4i3zBB~tWa7} zv{HQs;|`~tY!}4sZ%9Zj5L%G5P#hw@!D*!qlAPoMr4>Ot1TTnN-w?OD07f6!nZ@kB zvN5<ZdN3Y|`oPZ28_4*P0YrWQlMSvPI2d>Y8+?is7#J8(QYOf|pnB@F&9F$BDNHp? z;B*K{p)By!2yP$MFmN+KQY2UnM2rO@Qo~lm08vd|+ALB6r6gt2(j1#FI8k9sbi$BC z=bN9BnU<NF0#;X~!N9;^2TFXa$xnQ|{5=sfgk}US5SkgbAaDia1qrPilF~DhW{NLR zT2Qo5eTC7AqLpSF95)#4VA^PYLBbkcws3>d%CZX*W;Y~MX7J45`M}OBq4t%HLC;_f zDCzN8eq><gv-|=g8r(qXPN=~blI}pwDNxY}!k`8aINfam_w;KRP|5+g$`nRW$&$iU z#m&Hw4bsC<Bt%$cEkg}M3Tl5CtBD}9vq7$5C=w-BJ;>}}22B>fm!O8}OOTtuWs)Wn zxDdL<RFJ93SOh9>ia~ZNK#CBq<ebFZf)G&KtVoxEfx!<HiILz)yur@XThvoJp|G=Z zhG1v)C3dL`>{6gQm30Q=M9vwC6S+HBZ?N-p6ip495;`MrV&sg%iP0DN6)&+XUSL<e zAs{q0X-evh#EF>;7$;_56j175y}>O2%j#gyK?#s^K+MlN;2=+Bh+<3u4XQAL2US1= zEG*yw7FO^83mbTVg*}BWm_d{M7ONMiX_k!Ky9cqsF}MI+a@4TaFvNq52jd#n6vk{& zbC#h<v;?FIEXu%8gVyh_VTcE-0!inw*DxXTYZz-7;^BI0m}?l~L7@#&%22~n!w?TD z`@y^tP<an#F)(C-q6N&xVi%~&0L!810F{AYQ7k$@6$@AnMF+wT7SwT!8isgKErh89 zVF#uTP?ZJIAq}R$L=8h0!tV@O@KCB@K#c9wFl52QU>OSo!)iqMFvNqZKd?zD48aVV zOn%^=h$jCnR&Y`Tr$TTtg;bD;x)J1KNL321B|(lWLai#nb>J;FNTXbn?G{UMYEBx& z7*M)Ls%H&B$sCkWAT=r^FL1=iXXa&=#K%|hfm4+pj2{J3JqMC0M5Q`ddbl2N3rukA zvg)w9E2+G|^`fNK6-liQmmaSUuLqKHAjS<rp$?WFHW&rb1!^Cw%@CcaJ4LsHrH7p~ zu9)Hil_hE`RMu#1NZFFMBV|w4MKRASVxAo=J=`7K9o(2j87NVKqU!T8a7IAsP{Z;Z zLk&_CVk8-mZg5ng=tYW5MD%0mg(m<My`V4$n~0o@QW%jF45;v{LCP*rKHR(-hAfcJ zz@|XuIKdnS29R&yZU)Jvvml#{Y$v)6U^k%XC14NSJrH|Rn2^;Yn_k0^1xmVLC*r8W z;aL-XP_%|29+b|(dO_I*T$yX~f=hHrUH~U-v<v|%b&B8(NJz2=w>CjpuL#sgT*+8u z3`*3XWB{!RxJwICV2#NtK}ZHb5&~6j#h1W&0FrJ+=15%>Qn@0ef=J4!LShnAG8U*T zQD0%Sfn^K(N}C-jd(<y#J037OVtyjzO!!6TkPFfASE3Uyq-0!4$-Wq!eL*qjhK#}- z_Z2E@)Hk?nalfc#y(8#=$`SRQF(+KkxL>plz7QIDB{cd%T+)@e)Qh317o^i}$jB~8 zS(3gXWlQ>jk|Sjor2TG4+g$*o5A4hePK;mK7`zyL7|%$3U}qK&1+_lKLm9s?fY_)x z2Ojs(VR#V#Gbqzf2c;@-41fq6)d$+JJaToBCtkyt!c+oJ`3y(}B`EDwv4PSpqPnSJ zSO8C3ARA$L83O|YsCfn9GcthE!%AjI@%a)|3~RC#fyy<c5)_o~ia@n$kvRhc!%Ajw zq(cf<kji3^9~Bg!2|%bQza+6FH9oI2H?=4;IWZ@vvPuGy4lreEL8-?JoE#o-^Y>WI z5bUz=u*X&!E>K*Wv?O(*=0!;zP{H_tje(Q*I)}t14vC8#Qdc;nF2E3^00TJ*;zI@o zP>_N6pHG1s1~m*>Fc}8gM?(obQ-Y!=3*;8C?Z^ctS~iClOBi(?ynq5lT9GBFG0ufM zvOukANVSEi{Xn_4$Ql&tpb&va8b35@$}>wc;(c8_szgy@3?|$T(iliabUhW*T%oc? zZG*}dtsN<Q(hj5?$+{>OcttD_HS>VV5pd`rXC5tZtRWZwH4KQEEYu1km_d`#Pm}2u zb8$%#xP&S)134LFE;M;^#K#w<78D`Z&Jg}YkU~)L*1&L=Uuc5K6zhxp@>lrfFL20% zla!w($1RTd_`Jm2)cE*YT=DU_`6;D2AU02Yd|_!~4n(HN1k_Ig<&z>C5DV0uDRKm5 z8wRj3;5I^$K1d2gfZE8#8Q>6YV1U36Onj_fj2{?~NE0^JP{t1oNTd)4tMms3I3dE$ z8pQa40f`i0WL5aUfJs0c0kRs@?9D69EvST41E9f2aLWhW3xni-kg_6IkXu0=Yp~OQ zao9jSZdc^az`y_s+2R%k28IvJjEsyo7=$js&<7SyMwSl>(v0>W7~q5*C!^p81~?(X i$tVQkLJ0*{kSv@Kvt(5Gz<^171c`kC5ooetzX1U4<C&-c literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/utils.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/utils.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..52c98c629ed92fd3341ffabf8e1bbe2bf822ce9b GIT binary patch literal 1462 zcmd1j<>g{vU|@)xtCniW!octt#6iYP3=9ko3=9m#CJYP=DGVu$ISf&ZDNHHMEeugi zDJ&_hEeui2DQqe1EeugCDI6)BEeuhtDO@SsEeug?DU87knmjK-=J_RqOaie$n3;ir zfrWvA!5L(-6$1l93Bv-$8paw135JD?wM;ckDU6Z~DNNZ6Mbb4)Da=T078pB)HJBlV zEto-*-LHs|fq^0Y&&rRNe3KIFUxIiqLAHgsX)@hn$}70VoS0Os$$X2sAU^dLTS;P3 zdTPln#?+MzMVt%_48IceGxBp&_0#i<3-VJEi@-c11UIKNFGatgD6t~5SiiI=zBn_t zASX3GIX|zYC_hI(H!&}>pfo43B)=%WEVD|#C_gE`B)+($G%ZcPv?Md9Sg)Y+7FTLf zQGQW;NosKk$k)Xj3=9l>j4F&Qj4Vt=+zbp1D1ifVI!GBfa0I}Clf}4z2^KuT3@aJ^ zG?|M)N^Y^`WacI2q~Bu6%P-2+WV*$hn4FQBT9%p;pPrwXbBhH;f_(`gKyiJG7bF;; zl384mn3tRivc4E(r2u1*Ajm18-~+`G2!q5~VezEEz`#($kOjtBi~<a`j3rD9m`j+l zSQfC>FfL?V$QaB}#KXYApviWNJu5LezN9Gg7Gns+p)AFPMI}YN3=9mnm{U?qG?{L( zf`jT7qc6m1pr|MUyH6%LF((-mq^Z8pz)i}`$t<b#%u7kF;0L7`kk|Pbg&2z@&>aj) zSRf1zJ1r~@&SGA`0&*-Ufq@cQ4buYFg$$Am!3>&AenlYvYO)mxF)%O`34;ibO;A@C zm!;fdODsyy&CJteE)oODii5l(l9HL2o}ZVP6Q7?JUtE>~qA>g<2lrEOZemVOYH<nJ zZ?2i?sd;6IIUu`=*}$nnfRTr>NCw?sAUROrfc@1)&|fJ`wair<B}`e&kf^C;31`TY zVqpM@WU*BmGcwe&GBQ;0l(45TH#0Iaq%hVphciSnq%hR7*0JWXLg;eFa)uJ71rT#M zL7}#gv4*LJX#p1~#A=vRSmrPXGib6x!VR1Ys}zF#z5PNx6hak(J^kE6eH?>4LtH}? z5kQmq7B@K4#Akxy^Oj&SG(h50p@DIWyErp1y)-AWD6^zelN(!X6iI?Yo;fqG>=w6A zW}ahCx+^G$-C`{$%FHV%5&=byD2R{(Cr3nV6-k3c<w3exed9Cp%8EdF5j_P;!(%HT zwJ0sWC>NAbd{aw`GLwrzNv;?aKYWY=j4X^yi~@{8j2w&tj2sP2{{)!17?~K^{uQZ! z>}9{jnpc`zPzlZf;Cv3wVc=8?j$*JfB+C(bfWsy?KczG$)ee*!i$R%+gMovYgPntg IgN26`0QwI?5C8xG literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/utils.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/utils.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ac5e6c96318a30402f6390f79b59f8d9d753523f GIT binary patch literal 2594 zcmZ3^%ge>Uz`zhWS1r|$g@NHQhy%k+P{wB+1_p-d3@HpLj5!QZj44bl3{gxe%q<L2 z%qc7_3{fm8tSt;ttSM|Q3{h+;jKK_=>@PuD{gPoOfOsqn3=GT+3=E$ez^0Wjf&?Kr z3(l@#tYN6)VqjRt$iT1~E?Ub}!<51(2~o_zz>va}4Q3Zf)i9+nV-e@8VM<}aBF>b; z8qAQw26CZa5y)}re^!3H<eQXW{}N=`OOQ~An<mpOro4h%%!x_Gn#{MD3*u96v6Uni zrKgtMVoWUt`9VQJ;a7rwMt*LpetLd!L4HbN5twI$;O3O(rRWzFC01k>>z5YA7iZ=c z<fO(Y=jW9a<>%<<Cgx=pl;$Ls<QK)4Wmf4I<tOEr#21&8rlskZmSpA>>lIYq;z}(l z$}fs9Ni8lZ;$dK50EJ+&3j+f~1H)Yot{%3IvKh>sH5WN#u5idSINo6A=n!lv=}2v= zX{edOI74v`%Z$=lJTrJ!2(B<(BeH>YmCOp68~nl@B@g&TI!Zr-LJ)fdxj`ZbqyUN$ z5wrjlL{L!#g+1DW8CEj-X)+hFF)%RPV#~?QOUy~X#gdm_l&i^fi#IVjBQv!uH6=bh zKQZSP3y3TR8LpsE#0}EG3lfP>$t*5O%u7x!5@BFq0Erh%Ljr)Cx5u*IuFGzQ$^zMo zQhFD;^{;U2UtrNM;$>i9NCw3w%z3P^gx3T1R}DiJOojoOjqqa?3j;$fV+lwfRAm+@ z(!uN!P$Ym^3=COt_5vieP)P;`h8jlHWQ@uSW+>ukU|`T>yTzWBm>geHlzEFW1QJ#( z#f3#BMFOBuU`|Ob(PX;C3Qi!m7=2NKNG3TkCmEEAQhlN6DJe52v!v29FD132NQQxd z0c1n50XW2NaPao<&S0IxJBR-whkAqKU2fhU^QlHtET`K}vRxo}QCRtku<}K2l`GsT z7g$s<gAo)2ApCg+X~75&BnAdVFfKq6#T{VCc^*^-)G%QW=w%EH46EVBAo6`MgC>(- z5y&b{wjwbG28JSW5CKY$(9kU|OS#3CSd^ZdnWxEIBn6TMrLS8eDVd4s`FV*s@%d@- z#bqfV$`_XPA?Z^N9{9nzi8(o`#U<cKaLr6l%_~dHDN<x$U=U_tU?_IR9{kd>b1diE z&az!0cu`vWinR6$$1Bo$7g!{2aLZp%FuAT^eM!OkqJr%e1=}m!b{AOeFk=Li=)f_u zgAr6dBPV8LHb%k*1qd{>D2tI4rdnoB29zuTau3*S>@kI&C2CpnjFfShkOhh-uvsb$ zMbfpbC7{F%<})y)Fr#KBMurr|TINWG6oy*XI@UZ62wl!t&QQS|$xx1(nK___;IMZA zCrlohMt3=?{WVNAObfs@DY70A8&}$_VNPM0!yL?@$qGrK;OeMKA;{m`FVsUJR3X^Y z&pp(~F~~E-HAE2sG?{O4gDa2tOi(7jC0GnieDSH!gm;U(I5RK3G$*krv!qg!8(X?B zk^>hlnR#WmxP3D79COlLK{d@S)`FtUypkdbP&NSNq9S>40fopHMG7D(Rgi90-}ubD zvLcY5i$M{k00xk(Aq~$O0jWi4`9-;)65Kbnq$o4FxJZkEfdOPg@dXm|g^+NA_Xj2x zR*Aa;LQ^=W^G@QOk$6!+;fjDl2WtoG4^Vk#d4*fy0|OJQ1cJEB!Fz$v;39|N6%NA- z9ELZ<#5=eV@W+oIKR&QBu}b{-@#8BSgN)o9^QA^hESK9ZvfUtfQQ7#4vhhV}lPl6D zHxx}Tn0Z|{3%q0&c+o8Qidpa##gH3PsvlUHcr-pRKnPZiVo(9ISKd*U;h?OD6A$x2 zLqR8Y=0ohPPCU$-?6+9+N^=V;!G%1yMhDkw;DQ=lT!6!7B?CC8gG2HchfQvNN@-52 zU6C#W0|Tg5D^6ozVEDky$jEquLFfVuePCl{6#c*eC-@i{4L&fy2@WPk)ej7Cf`ggS T{sRM?;NoV~_`rZkfZYKAJBtKD literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc b/manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3b23fd5bc9161a79a4c7ff16b024515fc9c2e198 GIT binary patch literal 4103 zcmd1j<>g{vU|@)xtCkuk$-wX!#6iYP3=9ko3=9m#B@7G<DGVu$ISf&ZDNHHMEeugi zDJ&_hEeui2DQqe1EeugCDI6)BEeuhtDO@SsEeug?DLg5>EeuiYDSRpXEeuf{DFP{i zEeuhdDMBg2EeugyDIzJNEeuiIDPk$&EeugSDH17?Eeuh-DN-rYEeuh7DU87knldjz z?(s`z1W`~7auq8B1A{XI1H<P61_p+7hFZoN#sv&D3=0@j7#A`wWMpK3^O?~2%xHWT zG(IaDpAEt<VM<|d2AdnqpvmEPOQ<*_zceQ$wJ5$IF)uzVKQpi7mM~l_Co`=CD#)Fh zlayZ$<?xmlWfqsj8=`WJP`Sns?l1n#+=Bd~lGGH1-29UKqT-dzx7ZR33R3e@ZgHfi zmiU4MZ?P5>W#*NvWV*##l%JGea*GYBmZ3<Hfq~&yp?*ewZmNEIesMv5N@5Y1XN2J9 zl;)-A7ZfE{WESg}7R485<`(3n#wX|Jl@#UY=;tQpWfqj?B$nhC#g}DP>4R;EFD@xf zOVck;P0BARj!!JejL%5TDM&4fPb<wU)+?yI#gm>|;+T^Ic3v?%DD?<2Ffgz%3Lrra zMi#~*VFm_<WM*ijf+!XS1_m~0#1>CtU|^_WTEGB`&|2mi<^_x?OyGz`Wii9W5G)qB z7>vb~!kWUG!j{6ekcE+<hIs*YK6?r~iaeBG!koeZiux3WU<OT2zgz4nnZ*S;iItfw z|9>!KDlz<L$b9ynf#GEe0|UcL8xUd5z`#%?n^Kfou27PZs*s#nl$?{Qke>(U6(<*^ zrsip~+~P`4EpdSub&I1owZu6;C%@<xM@ms*xqn$=P7yBy1A`{ZEw-Y>l+4oNTWlGr znduoNMf?m53`HU!0u;mGXf6VU@GUNoPG_)PAn9TW1_lN$MlL2UMi_)e0S{x5EGY0n zISCZ-APkBKSgtD8U|?V<VJKm&VQ6M-W^`eQ<*sF_VW?q}WLUsd!w8~5L0rfN3X+wK zewxg;Sc?+#(o=6S=cMM{;x0}t2}{k%PtGi<)MPFaU|?Xl#a3L9nwnC4i!l@8VzAr6 z&gLtIrsZI;3T04=0i^-~MiEAq|3xYwhazWx6o;C?vOj|hLo8n{6DY-$Fs3jz!wLZ= zNUAAhDPgW*s9|hoVq^e0T9euD7IS7^S`o-(O%|||1M-VAOEUBGZn382<R_LCaf8Ab zWc^BrABvPf9$^J%Fmz8yXXa&=WG3chR;5A`zFU3~JP1HO0u{6@j2w)7j695d%tcz* zd<9DXphB%!57}4jwM?MG43rNT846iIkpl5Y2~!FaJY6#TfxS}2my!uBFBCvd(qu+> z1Dv3WKv7GK>tUe*3gKdy>v@>Cn0XkBw6VD!mUuq5z+6w`BDn^XFhQZiw16pv9h@Ld zV0pC$RC#dv-I9bC$?#%2zBo0nIKSwY6kG~XTEpaokP2;>m<USQjV1~$!qG&bB{@{| zB`6esNyAI^#Jm)69HpixK%7*(k_8-Fw*=EuOJLz23~?=6YF=VePAWJKS2BYOfm`fQ zZMWDU4D@If0hPpgDV}+mpaL0UP_aEIHHu-D$gDh!984^XMWC_^rMgAQZwj!u1?4wR zNXo2XTEMuF0UZ7;pl~me0~K6M5U+r0s<6bI(o{|6B1=&I;Rn~1@deP#R$OES%4Xo= z08Bv2grd~M6faPv8eEcCk{SqdGY8ns9E<{>y2B1V1%iq)SPCoxaX^){K$cOSS+TK! z0i;HaH@t-|VuT@Lj3HuzA!3RlVphZn@&+h|i}*k+uwTFg*h2!KsDM?9UWv*1(I78@ zq|hoxP!zbLdk#4YigZAs%jtJZ9Nlx!WQHMOgiFF0mxKu}2~%7WW>ASDW02=TWk!)H zh-CpHz`+0}Kpws&0SXKBWC;qB3{aS0Nt$fXr0IhmE}(dZMf&FkSkh#qZA-8QW<EQ- zWe9E_fRa9_dB6cl;E9H}U@Rjz%NWiwfwN5EEHgOEoRs9`U7DR(2Wkt0YJJq?B)|x6 zX@#N(9CCvCJY|3a4yF9&0GA&&CCn+DpgIj4m|T9hpru8kAvAr#1>lJqrUIUrVFK_( z3=@DSUYGzZ(ZUTihuT7RDh5SGHz+DVZ7+<dfEBUv=+S|kj*EPu`CA~%sMs96@Iny* z7hWhL;KB<<1YCHbh=2<(6cIB}afTv7DE)ydD0mwmly%J)fHHa*hydkcoE1n>ILLQg z;L;1lMtB5^qj?)1(dZKJ2uGKIM?AU&JOQ9fz!L(x1SttZ5?8Tiz7`ZL2_OR0Th-*h z#hO=|TTpq6DX#$B2?969!3|w-yAa&G0@uSuplYrNR2>w7%Dy5{J}v^Ko+3~n7lBMe zs`C*gEQd{QeoARhsvW3xTP(xCz`()G!OX$S!Oy|Q!Og+VA;iJO!OJ1W!OJ1a!wUd- CodpO0 literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc b/manipulator_viz/robot_stuff/__pycache__/webots_api_helper_funs.cpython-311.pyc new file mode 100644 index 0000000000000000000000000000000000000000..fd4d62b162bb90fbf4b4b0b69abfcdf75bcc9425 GIT binary patch literal 11565 zcmZ3^%ge>Uz`zhWS1mP8l7Zndhy%k+P{!vN1_p-d3@HpLj5!QZj44bl3{gxe%q<L2 z%qc7_3{fm8tSt;ttSM|Q3{h+;>@5sY>?s^A3{e~@oGlDdoGDx_3{hMu+${`I+$lUQ z3{gBOye$k-yeWJw3{iY3jKK_={4YUP`6V-gC@5xSU|?WoU|{&n!345BouQVohH(K@ z5|yf9Sb(e(E}6o(jFEw1HCzsr$H+jkc}%D#pz_Etj~UejR2~`Tv7nlO$|J)(R#X#E zd1RQ!hH3&TkCCAS6z9;`OJPTstpTMJ4!>JM#TogfIVq_{@db%_@mcwqc_p`m;bJ+N zX(doW?$n&5{BkIVx4bB`xFp^Xm1~5`HHL71@n_~1<QJ8srYPj*m*f`}uVlW(mRL}b znwN5mBR#dm7bJL#wV)_7uVf|DE!Lv^r2LXwY!J1@plqX{@T*WiBR@A)KRv&=AU`Fs z2+T7=aC1uYQuGUo5-T!`^-GK5i!*Zza#G`y^YcoI@^kca6Z0|)N^=rR@{8ijGOP5# zHpCZ~l%}QWm!~G>mlVe*7G%a}q~;W)7R9HP<`wG|RNmrAPc3oG$pJgBxJa0Rfq|WY zfuZ;!0|P??!wp{k2Dck5T<yM1z7tfZ>rK+TplEtg$n1)c*+mxfD=g+0Sj^F+%`OU= zUlB6D$YODY#o_{s1&Z_n*^5H@SA_I0vKU-pF}T2DfGR70QONL$kl{rZqbn>%7g&ra zk-f{pd4Ws%B8$uw7MTkyGB>yd8+<>5(n&I?fB-Q-m<?87aDfYq8Yb+81$v=S%Ur{Z zqxe9}?u-mnP>Gi1@t8@xO0;Z`$4qQ0LFp9~ekeGFHH9^WEro3v3j@Py7MK*+Wa_DB zPhls?d@SlqKq(yRyc7=fA|r(%m_d`%?-qMXW^qAIVr3@F{~rvQN(}!QGN1itV0f9r zz`*d*21HmhFfdffrWB=?E0koUDkNtXCFi6n<mZ8T#mPmfsd<_#x46<%OI#pE-Qp-t zEpg7z$uGLaky4ac?q8OeQ^d!>z@W);i>)XzC9|~n7F$MYW_m_RkpKe&Ly;ILD}q8& zK|w*GNQi-f;T9K2qchm5B6$V|1`&`FE=F)}<rBWnCwGZYZbs!rKFuq9nhov`Sh%mV zh+JY3nGkW2MeYiV+yxf78*~*5o*_IVXaUnj7Ue4}$`@Fa5w=r92As<^FS2M|VbQt( zLq(u4Nk&d`pgIDS?LW7HTO}p%+MR)+1SAek3^fd>i2;@8!Vs%m%T&Wq!z2k+&%m$% z*%r8H4I{D|R8trkdQ?D3Y$c<gCi5-UqQt!P)LYCssd=}!i&IO&QgiZ?GfOHpnTrHL zvCURokeZrOe2X!&801cPD&Q-I)+oVX`66`&1_otNO7H-e+jn_|rW#Kun`k}7`XaCV z6<+xd42-OrV4{QZ1_xh9VGrkZ4%tf_vNO^a1kTC4$f0qCL*oJ%ePD(ug@hwgSTVqh z%SGUD1h=mkTo__4YngBroyg&fUW}rLObsLU@JzudRC}zDGjR<=4dXH<28Pw}*kEJ; zg|Q~H-!10MytE>axF!oYgah)6GfOh_^KP-G<>V)p6!C!a87TNxLV~<V6%_8Qka`&< z%%wB)GD|WOb26(^A+@Djei1Cbiu6Fa7!(1q;0S<JoPy#X7?@Zk!9)k^4G!Mx9Fmtf zB<HFwV4bNmN9Q7k`V|iK3mob<IQaT`yLcxgUgVIt!Xa@1jJ~olunJEwn_+g5Md=EQ z(ghg8j1^D;0dnc*8q`=3uVup3T1AfuMur{<<e0)1q9`pbj2MB2H#n%P_);>V%^(F( zplLEAf*D-i73qM23gl*ZipCbYu(&ERU|?V{0ci=s8M+UM3qx>$A$5^O`U;Em1sK8% zQh0%318x_96Z&XJ0$jl1>^Pyd6R>1%uzqv`r+IYlNYpU8FvNng3=@t*2%~e+BY|89 zVYVc|oexgGTaxgO2E6kTU!0m(oL_WH3ND4{gTUm3kh&r;F%gs=37ROhV}d3M?VmtJ zUxK>szog-PmBhRha28EXQGht9cqI!sv)&R+Pc4CEz+i}L*;4ZolX6nQS$HKgxQTL$ z9jfgX8<c^Pt3^P)i@X%iyi8Ch17b{Zkplw*gFPravqRf00uAmrSh(B$n*1i1UKiH7 zB&>Bo+wG#T`xRmLi!2^jSUfJUcwi`SzbNc+McCsai{}*<&kHP`7z*q!3Oig8cDTsm zc!kCB0*fPt0>_KOPFIATF0wdZVR62|;!L@M4_pkwQlNSkLUgdfiuR<79FkW!Brkx` zQ&8VT{UVFT6&8&PEE+esg&O?ultD?T#kwZ$V!eh5d#_{}0|Ucqc*aA|nLS#dlv<<+ zs=}Bckq_#zh9%~drfM=5*?}rVesG^Tz5rT#78ilz4<)Yoic%9(yg*&+;F83W)FKxK z28KXTEZaf~Z83=k{~JP*pwKY_6QIx$zs@0di9>Ei+5Gxh^%vz0uE-l)<S@L#VR!+I zZg5LoV3EQTHlqVQ@MwiHaF(Y-ynIG!eqfZvpcJgh;de_Q%P7yR*x0}TGAa{qh#_Kx zA!3XnVuB%JiXmcF1S(34xIyhleo!(8H8NqnGyzb#2OCxKN=(i#@?l_Lhz2RU3ds#v zMple33YlCHGP%fNdWFUG0*fii@W%zk!6XeX!O%um@Z{3bRunXI!l~h!OKI1N5=YP3 z&=$=tbO|F|62`bBOmIn<;*v0fN)%auvcDyWumTaFst&cME&(bD&|5&@5+Q_vfguA_ zBH(KUxm*->y&~*-k;Ux_i`xYjH;g9AU{vsdhX$>nh|!)Hy!3!DhHn{yeH0$2f6-b4 z=<QpSmIKCkFSzBv>353*(!xzNgtLs`EMqv!1kN&rv&`TubE4Y>-lf@zMWC^wI#3Dl z7Dt<4fz(AIohw2*7g=<#u;^Z3(M4$z3=vuIkPGo`g`UxT4$0nd5@)&};w}0bQb*JX z<a2^;4~)EzHiE;6UUGoT1}?u_(B5yNA+*|u3&7hNFct7N2223nwtxx1+Y~SXSX%;a zs5#UYG8-4*q9u)ifuS2zv}oWNVzId>Y<or6_9Bbj6&AY-EOr=;iy^6iCSx$8tq*u{ z1Z9ktN|1m+Uvz?Ah7i~K5XdqrHb)<~KoJ3tTcC)5$1PAqz~dGuBH(cg6cIDfNCt`s z-c}1}7#O}j1k`UaFUn_NU;wqyiof7!wp<ugV-+ZECX$^t+IoQ`bA}YOwI%53oaEYk zwDp3t%n-CNj==mA*{v6G^s)uoz5tD!pcDk~)(pB1c&i3o0^Yhomw>l&&?Vrl9drre zTRoDvhE&XpDi|0T))H#=48|c7jHVaKj-;^Jqsf1ZHLo<cpz;<|UIBO=Ab5EZcx4HA zbpv=39z1mip4kBpXcvLTu)y8eB2a@9Tr+^<U?l^1G#I>O=@*AhZhlH>PO4oIXn_J~ zJxTEc1_p)?%#4hTA2=8og)cA&LC^;_PDY^*3~)k<mr?rz1Dud!V$}S=04I3Z8I?aU zzzH!9Mwt%`a6(LgQTPJ`oKRt5l>Wc~Cq%eF7QqQAL6BlNp#zs-0I|?ywLoHE2{3^s K%K$S490C9-rKdIk literal 0 HcmV?d00001 diff --git a/manipulator_viz/robot_stuff/drawing.py b/manipulator_viz/robot_stuff/drawing.py new file mode 100644 index 0000000..d2c85e2 --- /dev/null +++ b/manipulator_viz/robot_stuff/drawing.py @@ -0,0 +1,36 @@ +import numpy as np +import matplotlib.pyplot as plt +import mpl_toolkits.mplot3d.axes3d as p3 +from matplotlib.animation import FuncAnimation +import matplotlib.colors as colr + + +def drawOrientation(ax, orientation, t_v, avg_link_lenth): + # every line is drawn be specifing its x, y and z coordinates + # thus we will take each relevant vector and turn every one of its coordinates into a steps array + # and every vector will be translated accordingly + # first we need to take rot mat and p out of hom mat + steps = np.arange(0.0, 1.0, 0.1) * (avg_link_lenth / 2) + + # now we draw the orientation of the current frame + col = ['b', 'g', 'r'] + for i in range(0,3): + x = t_v[0] + orientation[i,0] * steps + y = t_v[1] + orientation[i,1] * steps + z = t_v[2] + orientation[i,2] * steps + ax.plot(x, y, z, color=col[i]) + + +def drawVector(ax, link, t_v, color_link): + # first let's draw the translation vector to the next frame + steps = np.arange(0.0, 1.0, 0.1) + x = t_v[0] + link[0] * steps + y = t_v[1] + link[1] * steps + z = t_v[2] + link[2] * steps + ax.plot(x, y, z, color=color_link) + + + +def drawPoint(ax, p, color_inside, marker): + point, = ax.plot([p[0]], [p[1]], [p[2]], markerfacecolor=color_inside, markeredgecolor=color_inside, marker=marker, markersize=5.5, alpha=0.9) + return point diff --git a/manipulator_viz/robot_stuff/drawing_for_anim.py b/manipulator_viz/robot_stuff/drawing_for_anim.py new file mode 100644 index 0000000..f40ad14 --- /dev/null +++ b/manipulator_viz/robot_stuff/drawing_for_anim.py @@ -0,0 +1,38 @@ +import numpy as np +import matplotlib.pyplot as plt +import mpl_toolkits.mplot3d.axes3d as p3 +from matplotlib.animation import FuncAnimation +import matplotlib.colors as colr + + +def drawOrientationAnim(ax, orientation, orientation_lines, t_v, avg_link_lenth): + # every line is drawn be specifing its x, y and z coordinates + # thus we will take each relevant vector and turn every one of its coordinates into a steps array + # and every vector will be translated accordingly + # first we need to take rot mat and p out of hom mat + steps = np.arange(0.0, 1.0, 0.1) * (avg_link_lenth / 2) + + # now we draw the orientation of the current frame + col = ['b', 'g', 'r'] + for i in range(0,3): + x = t_v[0] + orientation[i,0] * steps + y = t_v[1] + orientation[i,1] * steps + z = t_v[2] + orientation[i,2] * steps +# ax.plot(x, y, z, color=col[i]) + + orientation_lines[i].set_data(x, y) + orientation_lines[i].set_3d_properties(z) + +def drawVectorAnim(ax, link, link_line, t_v, color_link): + # first let's draw the translation vector to the next frame + steps = np.arange(0.0, 1.0, 0.1) + x = t_v[0] + link[0] * steps + y = t_v[1] + link[1] * steps + z = t_v[2] + link[2] * steps + #ax.plot(x, y, z, color=color_link) + link_line.set_data(x, y) + link_line.set_3d_properties(z) + + + + diff --git a/manipulator_viz/robot_stuff/follow_curve.py b/manipulator_viz/robot_stuff/follow_curve.py new file mode 100644 index 0000000..49feb20 --- /dev/null +++ b/manipulator_viz/robot_stuff/follow_curve.py @@ -0,0 +1,102 @@ +"""control_test controller.""" + +from robot_stuff.forw_kinm import * +from robot_stuff.inv_kinm import * +#from anim_func import * +import numpy as np +#import matplotlib.pyplot as plt +#import mpl_toolkits.mplot3d.axes3d as p3 +#from matplotlib.animation import FuncAnimation +#import matplotlib.colors as colr +import sys +import subprocess +import scipy.optimize +import random + + + +# angle in radians ofc +def turnAngleToPlusMinusPiRange(angle): + return np.arcsin(np.sin(angle)) +# let's trace out a circle +# th circle formula is: +# x = radius * cos(t) +# y = radius * sin(t) +# z = height +# the direction you are to move in in order to trace a circle, +# or any parametric curve for that matter is +# in the direction of the derivative at the given point +# the derivative w.r.t. t is, by coordinate: +# x = radius * -1 * sin(t) +# y = radius * cos(t) +# z = 0 +def goInACirleViaDerivative(radius, height, current_parameter): + return np.array([radius * -1 * np.sin(curve_parameter), radius * np.cos(curve_parameter), 0], dtype=float32) + + +# or just pass the next point to reach and use the vector to get there lel +def goInACirleViaPositionAroundZ(radius, height, current_parameter): + return np.array([radius * np.cos(curve_parameter), radius * np.sin(curve_parameter), height], dtype=np.float32) + +# here height is distance from yz plane +# but also from xy plane (we want it to be floating around axis parallel to x axis actually) +def goInACirleViaPositionAroundLiftedX(radius, height, x_0, current_parameter): + return np.array([x_0, radius * np.sin(curve_parameter), height + + radius * np.cos(curve_parameter)], dtype=np.float32) + + +def error_test(robot, t): + e = abs(t - r.p_e) + if e[0] < 0.001 and e[1] < 0.001 and e[2] < 0.001: + return True + else: + return False + +# let radius be 0.4 +def goInACircleViaJacobian(circle_param): + """ doing a fixed circle with axis around x axis because + i only need one circle and this is easy + arg: s - the current parameter in the parametric eq.""" + # R0c is the rotational matrix for the circle + # p is the vector to the circle center + + # need to have a hom. transf. mat. to circle center + # first 3 entries are rot. mat. and last one is translation vec. + # with added hom. mat. padding + circ_hom_mat = np.array([[0.0, 0.0, -1.0, 0.0], \ + [0.0, -1.0, 0.0, 0.0], + [1.0, 0.0, 0.0, 0.0], + [-0.59, 0.0, 0.59, 1.0]], dtype=np.float32) + center = circ_hom_mat[0:3, 3] + R0c = circ_hom_mat[0:3, 0:3] + + # shouldn't jacobian be a 3x3 mat? + circle_jacobian_circle = np.array([-1 * np.sin(circle_param), np.cos(circle_param), 0], dtype=np.float32) + circle_jacobian_base = R0c @ circle_jacobian_circle + + # now we need a radij-vector + # you need circle center coordinates and some point on the circle + # i can calculate p from current parameter value + # i do that in circle's frame + # and then i can transport it to base frame with my hom. transform + + point_on_c_circle = np.array([np.cos(circle_param), np.sin(circle_param), 0.0], dtype=np.float32) + point_on_c_circle = np.hstack((point_on_c_circle, 1)) + point_on_c_base = circ_hom_mat @ point_on_c_circle + + # need to make center hom. + center = np.hstack((center, 0)) + radij_vec = point_on_c_base - center + + radius = 0.4 + + s = radius * np.arctan2(radij_vec[1], radij_vec[0]) + + s = turnAngleToPlusMinusPiRange(s) + np.pi - s + es = s * 2 * np.pi * radius - s + + e = es * circle_jacobian_base + return e + + + diff --git a/manipulator_viz/robot_stuff/forw_kinm.py b/manipulator_viz/robot_stuff/forw_kinm.py new file mode 100644 index 0000000..267373b --- /dev/null +++ b/manipulator_viz/robot_stuff/forw_kinm.py @@ -0,0 +1,540 @@ +import numpy as np +#import matplotlib.pyplot as plt +#import mpl_toolkits.mplot3d.axes3d as p3 +#from matplotlib.animation import FuncAnimation +#import matplotlib.colors as colr +from robot_stuff.webots_api_helper_funs import * +import scipy.linalg + +#from drawing import * +from robot_stuff.joint_as_hom_mat import * +from robot_stuff.drawing import * +from robot_stuff.drawing_for_anim import * + +""" +The Robot_raw class is a collection of joints which +together constitute the manipulator. +It has methods for calculating the Jacobian, +the velocity manipulability Jacobian and two +different methods for calculating a gradient +toward a better manipulability positions. + +The Jacobian is calculated from its geometric +derivation as described in Siciliano chapter 3, +and so is the velocity manipulability Jacobian, +as described in Geometry Aware Manipulability +Learning, Tracking and Transfer or in +Symbolic differentiation of the velocity mapping for a serial kinematic chain, +by Bruyninckx. Here i avoided the tensor +formulation as to my knowledge +numpy does not support tensor operations. + + +Calculations of the gradients are described in the +Singularity Avoidance Using Riemannian Geometry paper. + + +notes on code: +- we are doing only positions, not orientations +- dh parameters are read from a file +- there is no other way to import a robot + + +""" +# TODO +# TODO +# TODO +# TODO add the following flags: +# one for (not) drawing in matplotlib +# one for (not) using Webots API + + + +# add a drawing flag so that you can turn it on and off +# if sim is to be had, you must pass motors=[list_of_motors] +# and sensors=[list_of_sensors] +class Robot_raw: + #def __init__(self, clamp): + def __init__(self, ax=None, **kwargs): + try: + self.motors = kwargs["motors"] + self.sensors = kwargs["sensors"] + self.robot_name = kwargs["robot_name"] + self.sim = 1 + except KeyError: + if len(kwargs) > 1: + print(" if sim is to be had, you must pass \n \ + motors=[list_of_motors] \n \ + and sensors=[list_of_sensors] in named args syntax") + exit(1) + else: + self.sim = 0 + self.robot_name = "no_sim" + pass + #self.mode = 'training' + self.mode = 'eval' + self.clamp = 0 + #self.clamp = 1 + self.joints = [] + # TODO FIX READING FILES + if self.robot_name == "no_sim": + #fil = open('inverse_kinematics_gymified/envs/arms/ur10e_dh_parameters_from_the_ur_site', 'r') + #fil = open('envs/arms/ur10e_dh_parameters_from_the_ur_site', 'r') + #fil = open('arms/ur10e_dh_parameters_from_the_ur_site', 'r') + #fil = open('/chalmers/users/guberina/ActorCriticManipulationControl/inverse_kinematics_gymified/inverse_kinematics_gymified/envs/arms/ur10e_dh_parameters_from_the_ur_site', 'r') + #fil = open('./arms/ur10e_dh_parameters_from_the_ur_site', 'r') + fil = open('./arms/ur5e_dh', 'r') + #fil = open('/home/gospodar/chalmers/ADL/ActorCriticManipulationControl/inverse_kinematics_gymified/inverse_kinematics_gymified/envs/arms/kuka_lbw_iiwa_dh_params', 'r') + print("i'm using: testing_dh_parameters (which are UR10e params)") + if self.robot_name == "UR10e": + fil = open('arms/ur10e_dh_parameters_from_the_ur_site', 'r') + print('im using: ur10e_dh_parameters_from_the_ur_site') + if self.robot_name == "base_link": + fil = open('arms/kuka_lbw_iiwa_dh_params', 'r') +# fil = open('./da /lbr_iiwa_jos_jedan_dh', 'r') + print("i'm using: kuka_lbw_iiwa_dh_params") + if self.robot_name == "j2n6s300": + fil = open('arms/j2n6s300_dh_params', 'r') + print("i'm using: j2n6s300_dh_params") + if self.robot_name == "j2n6s300": + fil = open('arms/j2n6s300_dh_params', 'r') + print("i'm using: j2n6s300_dh_params") + if self.robot_name == "j2s7s300_link_base": + fil = open('arms/j2s7s300_dh_params', 'r') + print("i'm using: j2n6s300_dh_params") + + params = fil.read().split('\n') + params.pop() + for p in params: + p = p.split(';') + self.joints.append(Joint(float(p[0]), float(p[1]), float(p[2]), float(p[3]), self.clamp)) + + self.ndof = len(self.joints) + + fil.close() + self.jacobian = 0 + self.calcJacobian() + + +# below are some utility functions for drawing + def initDrawing(self, ax, color_link): + #initialize the plot for each line in order to animate 'em + # each joint equates to four lines: 3 for orientation and 1 for the link + # and each line has its x,y and z coordinate + # thus 1 joint is: [x_hat, y_hat, z_hat, p] + # and lines are a list of all of those + self.ax = ax + self.color_link = color_link + self.lines = [] + avg_link_lenth = 0 + for j in range(self.ndof): + x_hat = self.joints[j].HomMat[0:3,0] + y_hat = self.joints[j].HomMat[0:3,1] + z_hat = self.joints[j].HomMat[0:3,2] + p = self.joints[j].HomMat[0:3,3] + + line_x, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'r') + line_y, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'g') + line_z, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'b') + line_p, = self.ax.plot(np.array([]),np.array([]),np.array([]), self.color_link) + + self.lines += [[line_x, line_y, line_z, line_p]] + avg_link_lenth += self.joints[j].d + self.joints[j].r + + self.avg_link_lenth = avg_link_lenth / self.ndof + + +# NOTE switched from ax as argument to self.ax + def drawStateAnim(self): + toBase = [np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]])] + drawOrientation(self.ax, toBase[-1][0:3,0:3], np.zeros((3,)), self.avg_link_lenth * 2) + for j in range(len(self.joints)): + toBase.append(toBase[-1] @ self.joints[j].HomMat) + drawOrientationAnim(self.ax, toBase[-1][0:3,0:3], self.lines[j][:-1], toBase[-1][0:3,3], self.avg_link_lenth) + drawVectorAnim(self.ax, -1* ( toBase[-2][0:3,3] - toBase[-1][0:3,3] ), self.lines[j][-1], toBase[-2][0:3,3],self.color_link) + + +# needed only for drawing, and even that is optional + def saveConfToFile(self): + fil = open('robot_parameters', 'r') + params = fil.read().split('\n') + fil.close() + fil = open('robot_parameters', 'w') + params.pop() + back_to_string = '' + for i in range(len(params)): + params[i] = params[i].split(';') + params[i][1] = self.joints[i].theta + for j in range(4): #n of d-h params + back_to_string += str(params[i][j]) + if j != 3: + back_to_string += ';' + else: + back_to_string += '\n' + fil.write(back_to_string) + fil.close() + + +# the jacobian is calulated for the end effector in base frame coordinates +# TODO: the base frame ain't aligned with world coordinate frame!!! +# ==> the only offset is on the z axis tho +# ==> resolved by putting first joint's d parameter to height, there is no x nor y offset +# the simulation is controlled via motors +# we do calculations by reading from the sensors +# and we send the results to the motors + +# HERE WE ALSO CALCULATE THE MANIPULABILITY JACOBIAN BUT I WON'T RENAME + def calcJacobian(self): + z_is = [np.array([0,0,1], dtype=np.float32)] + p_is = [np.array([0,0,0], dtype=np.float32)] + toBase = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]], dtype=np.float32) + self.p_e = np.array([0.,0.,0.], dtype=np.float32) + + for j in range(self.ndof): + toBase = toBase @ self.joints[j].HomMat + z_is.append(toBase[0:3, 2]) + p_is.append(toBase[0:3, 3]) + p_e = p_is[-1] + self.p_e = p_is[-1] + jac = np.array([0,0,0,0,0,1], dtype=np.float32).reshape(6,1) +# we doin only revolute joints still +# we'll put these into array cos i don't trust myself that much + j_os = [] + j_ps = [] + + for j in range(self.ndof): + j_p = np.cross(z_is[j], p_e - p_is[j]) + j_ps.append(j_p) + j_p = j_p.reshape(3,1) + j_o = z_is[j] + j_os.append(z_is[j]) + j_o = j_o.reshape(3,1) + j_i = np.vstack((j_p, j_o)) + jac = np.hstack((jac, j_i)) + self.jacobian = jac[0:6,1:] +# print("jacobian incoming") +# print(self.jacobian) + self.jac_tri = self.jacobian[0:3,:] + + # the manipulability elipsoid is J @ J.T + # and we want to know its derivative w.r.t. q (all joints) + # thus we calculate the derivative of a matrix w.r.t a vector + # the result is a 3rd order tensor ('cos you get 1 Jacobian for each q_i) + # with size 6 x n x n + # the calculations are described in Geometry-aware learning, tracking.., eq. 66-68 + # just figure out the shape of the tensor and it's all clear after that + # formulae straight from GAMLTT, last page, 66-68 + +# if self.mode == 'eval': + mjac = [] + mjac_tri = [] + # this first column is here so that we can stack in the for loop, it's removed later + mjac_j = np.array([0,0,0,0,0,1], dtype=np.float32).reshape(6,1) + mj_o = 0 + mj_p = 0 + # now we take a jth joint by which we will do the derivate + for j in range(self.ndof): + # and we do this for every ith joint + for i in range(self.ndof): + if j <= i: + mj_o = np.cross(j_os[j], j_os[i]).reshape(3,1) + mj_p = np.cross(j_os[j], j_ps[i]).reshape(3,1) + mj_i = np.vstack((mj_p, mj_o)) + mjac_j = np.hstack((mjac_j, mj_i)) + + else: + mj_o = np.array([0.,0.,0.], dtype=np.float32).reshape(3,1) + mj_p = np.cross(j_ps[j], j_os[i]).reshape(3,1) + mj_i = np.vstack((mj_p, mj_o)) + mj_i = -1 * mj_i + mjac_j = np.hstack((mjac_j, mj_i)) + + # with that we calculated J derivative w.r.t. joint j, so append that and move on + mjac_j = mjac_j[0:6,1:] +# print("mjac_j", j + 1) +# print(mjac_j) + mjac_j_tri = mjac_j[0:3,:] + mjac.append(mjac_j) +# print("unutar calcJac") +# print(mjac_j) +# print("unutar calcJac") + mjac_tri.append(mjac_j_tri) + mjac_j = np.array([0,0,0,0,0,1], dtype=np.float32).reshape(6,1) + self.mjac = mjac + self.mjac_tri = mjac_tri + +# now we that we have the manipulability jacobian, +# we can get the velocity manipulability jacobian + + + # implementation of maric formula 12 (rightmostpart) + def calcMToEGradient_kM(self): + # first let's calculate the manipulability elipsoid + M = self.jacobian @ self.jacobian.T + M = M[0:3, 0:3] + k = np.trace(M) +# k = 2 +# print(k) + k_log = np.log(k) + + # this is the derivative of the manipulability jacobian w.r.t. joint angles + # self.calcManipulabilityJacobian() + + # we need M^-1 which might not exist in which case we'll return a 0 + # needs to throw an error or something along those lines in production tho + try: + M_inv = np.linalg.inv(M) + except np.linalg.LinAlgError as e: + print("ROKNUH U SINGULARITET!!!!!!!!!!!") +# M_inv = np.eye(M.shape[1], M.shape[0]) + return np.array([0] * self.ndof) + + + # now we right-mul M_inv by each partial derivative and do a trace of that + resulting_coefs = [] + for i in range(self.ndof): + # we first need to calculate an appropriate element of the + # velocity manipulability ellipsoid + # J^x_i = mjac[i] @ J.T + J @ mjac[i].T + #M_der_by_q_i = self.mjac_tri[i] @ self.jac_tri.T + self.jac_tri @ self.mjac_tri[i].T + M_der_by_q_i = self.mjac[i] @ self.jacobian.T + self.jacobian @ self.mjac[i].T + M_der_by_q_i = M_der_by_q_i[0:3, 0:3] + resulting_coefs.append(-2 * k_log * np.trace(M_der_by_q_i @ M_inv)) + resulting_coefs = np.array(resulting_coefs) +# print(resulting_coefs) + return resulting_coefs + + + # let's actually strech toward the sphere sigma = kI + def calcMToEGradient_kI(self): + # first let's calculate the manipulability elipsoid + M = self.jacobian @ self.jacobian.T + M = M[0:3, 0:3] + k = np.trace(M) + sigma = k * np.eye(3) +# sigma = 10 * k * np.eye(3) + sigma_sqrt = scipy.linalg.fractional_matrix_power(sigma, -0.5) + Theta = sigma_sqrt @ M @ sigma_sqrt + + log_Theta = scipy.linalg.logm(Theta) + Theta_der_wrt_q_i = [] + # calc the M derivate wrt q_is and same for Theta + for i in range(self.ndof): + M_der_by_q_i = self.mjac[i] @ self.jacobian.T + self.jacobian @ self.mjac[i].T + M_der_by_q_i = M_der_by_q_i[0:3, 0:3] + Theta_der_wrt_q_i.append(sigma_sqrt @ M_der_by_q_i @ sigma_sqrt) + # now this is the E + E = np.array(Theta_der_wrt_q_i) + + # to compute the derivative we have to use the frechet derivative + # on the [[Theta, E], [0, Theta]] +# gradMtoE = [] + resulting_coefs = [] + for i in range(self.ndof): + mat_for_frechet = np.vstack((np.hstack((Theta, E[i])), \ + np.hstack((np.eye(3) - np.eye(3), Theta)))) + frechet_der = scipy.linalg.logm(mat_for_frechet) + der_theta_q_i = frechet_der[0:3, -3:] + resulting_coefs.append(2 * np.trace(der_theta_q_i @ log_Theta.T)) + + return np.array(resulting_coefs) + + + def calcManipMaxGrad(self): + M = self.jacobian @ self.jacobian.T + M = M[0:3, 0:3] + resulting_coefs = [] + print("pinv") + print(np.linalg.pinv(self.jacobian)) + for i in range(self.ndof): + A = self.mjac[i] @ np.linalg.pinv(self.jacobian) +# print("A", i) +# print(A) +# A = A[0:3, 0:3] + resulting_coefs.append(-1 * np.sqrt(np.linalg.det(M)) * np.trace(A)) + + return np.array(resulting_coefs) + + + + + + def drawState(self): + toBase = [np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]])] + drawOrientation(self.ax, toBase[-1][0:3,0:3], np.zeros((3,))) + for j in range(self.ndof): + toBase.append(toBase[-1] @ self.joints[j].HomMat) + drawOrientation(self.ax, toBase[-1][0:3,0:3], toBase[-1][0:3,3]) + drawVector(self.ax, -1* ( toBase[-2][0:3,3] - toBase[-1][0:3,3] ), toBase[-2][0:3,3],self.color_link) + + + def updateJointsAndJacobian(self): + thetas = np.array(readJointState(self.sensors, dtype=np.float32)) +# potential clamping for joint rotation limits +# offset for j2n6s300 + if self.robot_name == "j2n6s300": + q1 = np.array([-0.01497, 1.5708, -1.5708, -1.5708, -3.14159, 0.0], dtype=np.float32) + thetas = thetas + q1 +# if self.robot_name == "base_link": +# thetas = thetas + np.array([np.pi, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], dtype=np.float32) + for i in range(len(thetas)): + if self.clamp == 1: + self.joints[i].rotate_numerically(clampTheta(thetas[i]), self.clamp) + else: + self.joints[i].rotate_numerically(thetas[i], self.clamp) + self.calcJacobian() + + # only here for plots, hence does not update jacobian + def setJoints(self, thetas): + for i in range(len(thetas)): + if self.clamp == 1: + self.joints[i].rotate_numerically(clampTheta(thetas[i]), self.clamp) + else: + self.joints[i].rotate_numerically(thetas[i], self.clamp) + + + + def forwardKinmViaPositions(self, thetas, extra_damping): +# potential clamping for joint rotation limits +# ==> ur10e does not have joint limits, but other robots might + # offset for j2n6s300 + if self.robot_name == "j2n6s300": + thetas = thetas + np.array([np.pi, np.pi, np.pi, \ + np.pi, 0.0, 1.57079633], dtype=np.float32) +# if self.robot_name == "base_link": +# thetas = thetas + np.array([np.pi, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) + for i in range(len(thetas)): + # clamp + if self.clamp == 1: + if self.sim == 1: + self.motors[i].setPosition(clampTheta(self.joints[i].theta + thetas[i])) + else: + self.joints[i].rotate_numerically(clampTheta(self.joints[i].theta + thetas[i] / extra_damping), self.clamp) + # no clamp + else: + if self.sim == 1: + self.motors[i].setPosition(self.joints[i].theta + thetas[i]) + else: + self.joints[i].rotate_numerically(thetas[i] + self.joints[i].theta / extra_damping, self.clamp) + + if self.sim == 1: + self.updateJointsAndJacobian() + else: + if self.mode == "eval": + self.calcJacobian() + + + + def forwardKinmNumericsOnlyDebug(self, thetas): +# if self.robot_name == "j2n6s300": +# thetas = thetas + np.array([np.pi, np.pi, np.pi, np.pi, 0.0, np.pi]) + for i in range(len(thetas)): + # clamp + if self.clamp == 1: + if self.sim == 1: + self.motors[i].setPosition(clampTheta(thetas[i])) +# self.updateJointsAndJacobian() + else: + self.joints[i].rotate_numerically(clampTheta( thetas[i]), self.clamp) + self.calcJacobian() + # no clamp + else: + if self.sim == 1: +# self.motors[i].setPosition(thetas[i]) + self.updateJointsAndJacobian() + else: + self.joints[i].rotate_numerically(thetas[i] , self.clamp) + self.calcJacobian() + + + def forwardKinmNumericsOnlyDebug2(self, thetas): + print("======================================") +# if self.robot_name == "j2n6s300": +# thetas = thetas + np.array([np.pi, np.pi /2, -np.pi/2, \ +# 0.0, 0.0, 1.57079633]) + for i in range(self.ndof): + if self.clamp == 1: + self.joints[i].rotate_numerically(clampTheta(thetas[i]), self.clamp) + else: + # print("----------- +", thetas[i]) + self.joints[i].rotate_numerically(thetas[i], self.clamp) + # print("after:", self.joints[i].theta) + # print("") + print("") + print("") + print("") + self.calcJacobian() +# print(self.jacobian) +# print(self.mjac) + + + +# in webots, i get this for free with a position sensor +# but since such a device is not available in reality. +# thus, the gps device in webots shall be used to measure accuracy. + def eePositionAfterForwKinm(self, thetas): + joints2 = self.joints + for i in range(len(thetas)): + joints2[i].rotate(joints2[i].theta + thetas[i]) + + toBase = np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]], dtype=np.float32) + + for j in range(len(joints2)): + toBase = toBase @ joints2[j].HomMat + p_e = toBase[0:3,3] + return p_e + + + def bruteAnimForwKinm(self, ax, thetas): + shots = np.linspace(0,1,20) + for shot in shots: + for i in range(len(thetas)): + self.joints[i].rotate(self.joints[i].theta + thetas[i] * shot) + self.drawState(ax, 'bisque') + self.calcJacobian() + self.drawState(ax, 'b') + + + # remake this into the FuncAnimation update function + # divide each angle from the current to desired angle in the same number of steps + # then for each joint and each of its steps calculate the new position, + # and append it in the appropriate "line" list +# there is no set_data() for 3dim, so we use set_3_properties() for the z axis +def forwardKinAnim(frame, r, thetas, n_of_frames, ax): + thetas_sliced = [] +# print("frame") + # print(frame) + for j in range(len(r.joints)): + # i have no idea why but the sum of frame(i)/n_of_frames adds up to 0.5 + # we need it to add to to 1 so that in total the manipulator rotates by the specified angles + thetas_sliced.append(thetas[j] * (2 * frame / n_of_frames)) + r.forwardKinm(thetas_sliced) + toBase = [np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]], dtype=np.float32)] + x_hat_dat =[[0],[0],[0]] + y_hat_dat =[[0],[0],[0]] + z_hat_dat =[[0],[0],[0]] + p_dat = [[0],[0],[0]] + for j in range(len(r.joints)): + toBase.append(toBase[-1] @ r.joints[j].HomMat) + orientation = toBase[-1][0:3,0:3] + x_hat = orientation[0:3,0] + toBase[-1][0:3,3] + y_hat = orientation[0:3,1] + toBase[-1][0:3,3] + z_hat = orientation[0:3,2] + toBase[-1][0:3,3] + p = (-1* ( toBase[-2][0:3,3] - toBase[-1][0:3,3] ) + toBase[-2][0:3,3]) + for i in range(3): + x_hat_dat[i].append(x_hat[i]) + y_hat_dat[i].append(y_hat[i]) + z_hat_dat[i].append(z_hat[i]) + p_dat[i].append(p[i]) +# r.lines[j][0].set_data(x_hat_dat[0], x_hat_dat[1]) +# r.lines[j][0].set_3d_properties(x_hat_dat[2]) +# r.lines[j][1].set_data(y_hat_dat[0], y_hat_dat[1]) +# r.lines[j][1].set_3d_properties(y_hat_dat[2]) +# r.lines[j][2].set_data(z_hat_dat[0], z_hat_dat[1]) +# r.lines[j][2].set_3d_properties(z_hat_dat[2]) + r.lines[j][3].set_data(p_dat[0], p_dat[1]) + r.lines[j][3].set_3d_properties(p_dat[2]) +# print(frame / frame_length) + if frame == 1: + r.drawState(ax, 'r') + diff --git a/manipulator_viz/robot_stuff/inv_kinm.py b/manipulator_viz/robot_stuff/inv_kinm.py new file mode 100644 index 0000000..2906bfd --- /dev/null +++ b/manipulator_viz/robot_stuff/inv_kinm.py @@ -0,0 +1,434 @@ +from robot_stuff.forw_kinm import * +#from anim_func import * +import numpy as np +import matplotlib.pyplot as plt +import mpl_toolkits.mplot3d.axes3d as p3 +from matplotlib.animation import FuncAnimation +import matplotlib.colors as colr +import sys +import scipy.optimize +from qpsolvers import solve_qp +from qpsolvers import dense_solvers +from scipy.linalg import sqrtm + + + +# hardcoded for all joints +# of course each joint can have its own limit +def clampVelocity(del_thet): + for indeks in range(len(del_thet)): + if del_thet[indeks] > 3.0: + del_thet[indeks] = 3.0 + + if del_thet[indeks] < -3.0: + del_thet[indeks] = -3.0 + return del_thet +# r is the Robot_raw +# t is the target position + + + +def invKinm_Jac_T(r, t): + e = t - r.p_e + num = np.dot(e, r.jac_tri @ r.jac_tri.T @ e) + den = np.dot(r.jac_tri @ r.jac_tri.T @ e, r.jac_tri @ r.jac_tri.T @ e) + alpha = num / den + del_thet = alpha * r.jac_tri.T @ e + +# clamping for joint rotation limits + del_thet = clampVelocity(del_thet) + +# if you want a damping constant other than alpha +# del_thet = 0.011 * r.jac_tri.T @ e + + return del_thet + + +# using the nullspace for the comfort function +# when doing manipulability, use the nullspace then go to vec_toward_greater_manip +def invKinm_PseudoInv(r, t): + e = t - r.p_e + + psedo_inv = np.linalg.pinv(r.jac_tri) + del_thet = psedo_inv @ e +# we can add any nulspace vector to del_thet +# and given the constraints, we should implement some sort of a comfort function +# the min and max theta for each angle are hardcoded, but that will be changed +# they are hardcoded to +/- pi # 3/4 with center at 0 +# thus for all i q_iM - q_im = pi * 6/4 +# the added q_0 must be left multiplyed by (np.eye(n) - np.linalg.pinv(r.jac_tri) @ r.jac_tri) +# we take into account the current theta (punish more if closer to the limit) +# the formula is 3.57 in siciliano + theta_for_limits = [] + for k in range(r.ndof): + theta_for_limits.append( (-1/r.ndof) * (r.joints[k].theta / (np.pi * 1.5))) + theta_for_limits = np.array(theta_for_limits, dtype=np.float32) + + del_thet += (np.eye(r.ndof) - psedo_inv @ r.jac_tri) @ theta_for_limits + + del_thet = clampVelocity(del_thet) + + return del_thet + +# what is this, i don't know +def invKinm_PseudoInv_half(r, t): + e = t - r.p_e + + #psedo_inv = np.linalg.pinv(r.jac_tri) + #psedo_inv = r.jac_tri.T @ sqrtm(r.jac_tri @ r.jac_tri.T) + psedo_inv = r.jac_tri.T @ np.linalg.inv(sqrtm(r.jac_tri @ r.jac_tri.T)) + #psedo_inv = sqrtm(r.jac_tri.T @ r.jac_tri) @ r.jac_tri.T + del_thet = psedo_inv @ e +# theta_for_limits = [] +# for k in range(r.ndof): +# theta_for_limits.append( (-1/r.ndof) * (r.joints[k].theta / (np.pi * 1.5))) +# theta_for_limits = np.array(theta_for_limits, dtype=np.float32) +# +# del_thet += (np.eye(r.ndof) - psedo_inv @ r.jac_tri) @ theta_for_limits + + del_thet = clampVelocity(del_thet) + + return del_thet + + + + +# using the nullspace singularity avoidance +def invKinmSingAvoidance_PseudoInv(r, e): +# e = t - r.p_e + + psedo_inv = np.linalg.pinv(r.jac_tri) + del_thet = psedo_inv @ e +# we can add any nulspace vector to del_thet +# and given the constraints, we should implement some sort of a comfort function +# the min and max theta for each angle are hardcoded, but that will be changed +# they are hardcoded to +/- pi # 3/4 with center at 0 +# thus for all i q_iM - q_im = pi * 6/4 +# the added q_0 must be left multiplyed by (np.eye(n) - np.linalg.pinv(r.jac_tri) @ r.jac_tri) +# we take into account the current theta (punish more if closer to the limit) +# the formula is 3.57 in siciliano + gradMtoE = r.calcMToEGradient_kM() +# print(gradMtoE) + + del_thet += (np.eye(r.ndof) - psedo_inv @ r.jac_tri) @ gradMtoE + + del_thet = clampVelocity(del_thet) + + return del_thet + + + + +def invKinm_dampedSquares(r, t): + e = t - r.p_e +# e = np.array([0.0,0.0,-1.0], dtype=np.float32) +# e = np.array([0.0,1.0,0.0], dtype=np.float32) +# e = np.array([1.0,0.0,0.0], dtype=np.float32) + lamda = 0.3 + iden = np.array([[1.,0.,0.], [0.,1.,0.], [0.,0.,1.]], dtype=np.float32) + + del_thet = r.jac_tri.T @ np.linalg.inv(r.jac_tri @ r.jac_tri.T + lamda**2 * iden) @ e + + del_thet = clampVelocity(del_thet) + + # let's try to use the calculation which uses svd + # the derivation is in the iksurvey section 6 + # the final equation is (11) and it calculates all left of e in above del_thet + + # something is wrong here and i have no idea what +# m = 3 +# n = len(r.jac_tri[0,:]) +# svdd = np.zeros((n, m)) +# svd = np.linalg.svd(r.jac_tri) # the output is a list of 3 matrices: U, D and V +# # important note: D is not returned as a diagonal matrix, but as the list of diagonal entries +# for s in range(m): # 3 is the maximum rank of jacobian +# svdd = svdd + (svd[1][s] / (svd[1][s] ** 2 + lamda ** 2)) * svd[2][:,s].reshape(n,1) @ svd[0][:,s].reshape(1, m) + + +# del_thet = svdd @ e +# del_thet = clampVelocity(del_thet) + + return del_thet + + +def invKinmQP(r, t): + P = np.eye(r.ndof, dtype="double") + q = np.array([0] * r.ndof, dtype="double") # should be q imo + #G = np.eye(r.ndof, dtype="double") + G = None + e = t - r.p_e +# e = np.array([0.0, 1.0, 0.0]) + b = np.array(e, dtype="double") + A = np.array(r.jac_tri, dtype="double") + #lb = np.array([-3] * r.ndof, dtype="double") + lb = None + #ub = np.array([3] * r.ndof, dtype="double") + ub = None + #h = ub + h = None + + + del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos") + #del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog") + + return del_thet + + + + + +# qp formulation, solved with scipy +def invKinmGradDesc(r, t): + + def getEEPos(thetas, r, t): + p_e = r.eePositionAfterForwKinm(thetas) + e = t - p_e + error = np.sqrt(np.dot(e,e)) + return error + + def toOptim(thetas): + #return np.sqrt(np.dot(thetas, thetas)) + return np.dot(thetas, thetas) + e = t - r.p_e + lb = [] + ub = [] + + def constraint(r, e): + # jac_tri @ del_thet must be equal to e + # when doing manipulability optimization it will be e + vec_toward_greater_manip + return scipy.optimize.LinearConstraint(r.jac_tri, e, e) + + + for bo in range(len(r.joints)): + lb.append(-3.0) + ub.append(3.0) + bounds = scipy.optimize.Bounds(lb, ub) + + error = np.sqrt(np.dot(e,e)) + thetas_start = [] + for th in range(r.ndof): + thetas_start.append(r.joints[th].theta) + thetas_start = np.array(thetas_start, dtype=np.float32) + + lin_constraint = constraint(r, e) + if (r.clamp == 1): + res = scipy.optimize.minimize(toOptim, thetas_start, method='SLSQP', constraints=lin_constraint, bounds=bounds) + else: + res = scipy.optimize.minimize(toOptim, thetas_start, method='SLSQP', constraints=lin_constraint) +# res = scipy.optimize.minimize(getEEPos, thetas_start, args=(r,t), method='SLSQP', constraints=lin_constraint, bounds=bounds) +# res = scipy.optimize.minimize(toOptim, thetas_start, method='CG', bounds=bounds) + # without constraints it returns some crazy big numbres like 10**300 or sth + # so something is seriously wrong there + del_thet = [] + for bla in range(len(res.x)): + del_thet.append(float(res.x[bla])) +# del_thet.append(res.x[bla] - 0.01) +# for bla in range(len(res.x)): +# del_thet.append(float(res.x[bla])) +# del_thet[bla] += 0.01 +# print(del_thet[bla]) +# print("del_thet") +# print(del_thet) + +# del_thet = np.array(del_thet, dtype=np.float32) + del_thet = clampVelocity(del_thet) + return del_thet + + + +############################################################### +# IK with singularity avoidance via QP +############################################################### + +# qp formulation, solved with scipy +def invKinmSingAvoidanceWithQP_kM(r, t): + + def getEEPos(thetas, r, t): + p_e = r.eePositionAfterForwKinm(thetas) + e = t - p_e + error = np.sqrt(np.dot(e,e)) + return error + + # E is shere toward which the manipulability elipsoid M should move + # the calculation is defined as a method in the robot_raw class + def toOptim(thetas, r): + # alpha is a coef to select the amount of moving toward E +# aplha = 3.03 + grad_to_E = r.calcMToEGradient_kM() +# return np.dot(thetas, thetas) #+ coef_to_E @ thetas + return np.dot(thetas, thetas) + np.dot(grad_to_E, thetas) + e = t - r.p_e + lb = [] + ub = [] + def constraint(r, e): + # jac_tri @ del_thet must be equal to e + # when doing manipulability optimization it will be e + vec_toward_greater_manip + return scipy.optimize.LinearConstraint(r.jac_tri, e, e) + + + for bo in range(r.ndof): + lb.append(-3.0) + ub.append(3.0) + bounds = scipy.optimize.Bounds(lb, ub) + + error = np.sqrt(np.dot(e,e)) + thetas_start = [] + for th in range(r.ndof): + thetas_start.append(r.joints[th].theta) + thetas_start = np.array(thetas_start, dtype=np.float32) + + lin_constraint = constraint(r, e) + if (r.clamp == 1): + res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='SLSQP', constraints=lin_constraint, bounds=bounds) + else: + res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='SLSQP', constraints=lin_constraint) +# res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='CG', constraints=lin_constraint) +# res = scipy.optimize.minimize(getEEPos, thetas_start, args=(r,t), method='SLSQP', constraints=lin_constraint, bounds=bounds) +# res = scipy.optimize.minimize(toOptim, thetas_start, method='CG', bounds=bounds) + # without constraints it returns some crazy big numbres like 10**300 or sth + # so something is seriously wrong there + del_thet = [] + for bla in range(len(res.x)): + del_thet.append(float(res.x[bla])) +# del_thet.append(res.x[bla] - 0.01) +# for bla in range(len(res.x)): +# del_thet.append(float(res.x[bla])) +# del_thet[bla] += 0.01 +# print(del_thet[bla]) +# print("del_thet") +# print(del_thet) + +# del_thet = np.array(del_thet, dtype=np.float32) +# print(del_thet) + del_thet = clampVelocity(del_thet) + return del_thet + + + +def invKinmSingAvoidanceWithQP_kI(r, t): + + def getEEPos(thetas, r, t): + p_e = r.eePositionAfterForwKinm(thetas) + e = t - p_e + error = np.sqrt(np.dot(e,e)) + return error + + # E is shere toward which the manipulability elipsoid M should move + # the calculation is defined as a method in the robot_raw class + def toOptim(thetas, r): + # alpha is a coef to select the amount of moving toward E +# aplha = 3.03 + grad_to_E = r.calcMToEGradient_kI() +# return np.dot(thetas, thetas) #+ coef_to_E @ thetas + return np.dot(thetas, thetas) + np.dot(grad_to_E, thetas) + e = t - r.p_e + lb = [] + ub = [] + def constraint(r, e): + # jac_tri @ del_thet must be equal to e + # when doing manipulability optimization it will be e + vec_toward_greater_manip + return scipy.optimize.LinearConstraint(r.jac_tri, e, e) + + + for bo in range(r.ndof): + lb.append(-3.0) + ub.append(3.0) + bounds = scipy.optimize.Bounds(lb, ub) + + error = np.sqrt(np.dot(e,e)) + thetas_start = [] + for th in range(r.ndof): + thetas_start.append(r.joints[th].theta) + thetas_start = np.array(thetas_start, dtype=np.float32) + + lin_constraint = constraint(r, e) + if (r.clamp == 1): + res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='SLSQP', constraints=lin_constraint, bounds=bounds) + else: + res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='SLSQP', constraints=lin_constraint) +# res = scipy.optimize.minimize(toOptim, thetas_start, args=(r), method='CG', constraints=lin_constraint) +# res = scipy.optimize.minimize(getEEPos, thetas_start, args=(r,t), method='SLSQP', constraints=lin_constraint, bounds=bounds) +# res = scipy.optimize.minimize(toOptim, thetas_start, method='CG', bounds=bounds) + # without constraints it returns some crazy big numbres like 10**300 or sth + # so something is seriously wrong there + del_thet = [] + for bla in range(len(res.x)): + del_thet.append(float(res.x[bla])) +# del_thet.append(res.x[bla] - 0.01) +# for bla in range(len(res.x)): +# del_thet.append(float(res.x[bla])) +# del_thet[bla] += 0.01 +# print(del_thet[bla]) +# print("del_thet") +# print(del_thet) + +# del_thet = np.array(del_thet, dtype=np.float32) +# print(del_thet) + del_thet = clampVelocity(del_thet) + return del_thet + + + +def invKinmQPSingAvoidE_kI(r, t): + P = np.eye(r.ndof, dtype="double") +# q = 0.5 * np.array(r.calcMToEGradient_kI(), dtype="double") + q = np.array(r.calcMToEGradient_kI(), dtype="double") +# G = np.eye(r.ndof, dtype="double") +# G = [] + G = None + e = t - r.p_e + b = np.array(e, dtype="double") + A = np.array(r.jac_tri, dtype="double") + #lb = np.array([-3] * r.ndof, dtype="double") + lb = None + #ub = np.array([3] * r.ndof, dtype="double") + ub = None +# h = ub + h = None + +# del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="quadprog") + del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos") + + return del_thet + + + +def invKinmQPSingAvoidE_kM(r, t): + P = np.eye(r.ndof, dtype="double") +# q = 0.5 * np.array(r.calcMToEGradient_kM(), dtype="double") + q = np.array(r.calcMToEGradient_kM(), dtype="double") + #G = np.eye(r.ndof, dtype="double") + G = None + e = t - r.p_e +# e = np.array([0.0, 1.0, 0.0]) + b = np.array(e, dtype="double") + A = np.array(r.jac_tri, dtype="double") + #lb = np.array([-3] * r.ndof, dtype="double") + #ub = np.array([3] * r.ndof, dtype="double") + lb = None + ub = None + #h = ub + h = None + + + del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos") + + return del_thet + + + +def invKinmQPSingAvoidManipMax(r, t): + P = np.eye(r.ndof, dtype="double") + q = np.array(r.calcManipMaxGrad(), dtype="double") + G = None + e = t - r.p_e + b = np.array(e, dtype="double") + A = np.array(r.jac_tri, dtype="double") + lb = np.array([-3] * r.ndof, dtype="double") + ub = np.array([3] * r.ndof, dtype="double") + h = None + + del_thet = solve_qp(P, q, G, h, A, b, lb, ub, solver="ecos") + + return del_thet + diff --git a/manipulator_viz/robot_stuff/joint_as_hom_mat.py b/manipulator_viz/robot_stuff/joint_as_hom_mat.py new file mode 100644 index 0000000..66233e1 --- /dev/null +++ b/manipulator_viz/robot_stuff/joint_as_hom_mat.py @@ -0,0 +1,146 @@ +import numpy as np + +""" +Here we formalize the robot as a series of rigid bodies (links) +connected with joints. Each joint is endowed with a coordinate frame +which is derived via the Denavit-Hartenberg parameters. +Here all joints are revolute, meaning rotating, and rotations are +reprezented as rotation matrices. +Rotating matrices and translation vectors are combined +into homogenous matrices and then a frame change amounts +to matrix multiplication. +""" + +# deriving the homogeneus transformation matrix +# as a series of transformations between two coordinate frames +# detailed description: Wikipedia article on Denavit-Hartenberg parameters + +# this function is wrong!!!!!!!!!!!!!1 +def DHtoHomMat(d, theta, r, alpha): + ct = np.cos(theta) + st = np.sin(theta) + ca = np.cos(alpha) + sa = np.sin(alpha) + + # the d: translation along i^th z-axis until the common normal + trans_z_n_prev = np.array([[1,0,0], [0,1,0], [0,0,1]], dtype=np.float32) + trans_z_n_prev = np.hstack((trans_z_n_prev, np.array([0,0,d], dtype=np.float32).reshape(3,1))) + trans_z_n_prev = np.vstack((trans_z_n_prev, np.array([0,0,0,1], dtype=np.float32))) + + # the theta: rotation about i^th z-axis until i^th x-axis coincides with the common normal + # this is the parameter we get to control + rot_z_n_prev = np.array([[ct,-1*st,0], [st,ct,0], [0,0,1]], dtype=np.float32) + rot_z_n_prev= np.hstack((rot_z_n_prev, np.array([0,0,1], dtype=np.float32).reshape(3,1))) + rot_z_n_prev= np.vstack((rot_z_n_prev, np.array([0,0,0,1], dtype=np.float32))) + + # the r: traslation along the i^th x-axis, now common normal, until the origin of i+1^th frame + trans_x_n = np.array([[1,0,0], [0,1,0], [0,0,1]], dtype=np.float32) + trans_x_n = np.hstack((trans_x_n, np.array([r,0,0], dtype=np.float32).reshape(3,1))) + trans_x_n = np.vstack((trans_x_n, np.array([0,0,0,1], dtype=np.float32))) + + # the a: rotation about common normal so that z-axis aling + rot_x_n = np.array([[1,0,0], [0,ca,-1*sa], [0,sa,ca]], dtype=np.float32) + rot_x_n = np.hstack((rot_x_n, np.array([0,0,0], dtype=np.float32).reshape(3,1))) + rot_x_n = np.vstack((rot_x_n, np.array([0,0,0,1], dtype=np.float32))) + + return trans_z_n_prev @ rot_z_n_prev @ trans_x_n @ rot_x_n + +# the above, but multiplied into the final form (less calculation needed, hence better) +# also correct +def createDHMat(d, theta, r, alpha): + ct = np.cos(theta) + st = np.sin(theta) + ca = np.cos(alpha) + sa = np.sin(alpha) + DHMat = np.array([ [ct, -1 * st * ca, st * sa, r * ct], \ + [st, ct * ca, -1 * ct * sa, r * st], \ + [0, sa, ca, d], \ + [0, 0, 0, 1] ], dtype=np.float32) + return DHMat + + +def createModifiedDHMat(d, theta, r, alpha): + ct = np.cos(theta) + st = np.sin(theta) + ca = np.cos(alpha) + sa = np.sin(alpha) + MDHMat = np.array( [[ct, -1* st, 0, r], \ + [st * ca, ct * ca, -1* sa, -d * sa], \ + [st * sa, ct * sa, ca, d * ca], \ + [0, 0, 0, 1]], dtype=np.float32) + return MDHMat + + +# clamping of all joints to rotate only by 7/8 of the cirle +# of course this can be changed to other limits as needed +# however, it is hardcoded here because it's good enough for demonstrative purposes +# the ur arms all do not have joint limits! +def clampTheta(theta): + if theta > np.pi * 7/4: + theta = np.pi * 7/4 + if theta < -1 * (np.pi * 7/4): + theta = -1 * (np.pi * 7/4) + return theta + +# if clamp is == 0 then don't clamp and if it is 1 then do clamp the angles +# this class does not reprezent a geometric entity, but rather the coordinate system +# in which kinematics is defined for a particular joint +# and that coordinate system is defined via the D-H parameters +class Joint: + def __init__(self, d, theta, r, alpha, clamp): + self.clamp = clamp +# potential clamping for joint rotation limits + if clamp == 1: + self.theta = clampTheta(theta) + else: + self.theta = theta + self.d = d + self.r = r + self.alpha = alpha + self.ct = np.cos(theta) + self.st = np.sin(theta) + self.ca = np.cos(alpha) + self.sa = np.sin(alpha) + self.HomMat = createDHMat(self.d, self.theta, self.r, self.alpha) + + +# we simply recalculate the homogenous transformation matrix for the given joint +# and that equates to it being rotated +# the timely rotation is simply the rotation broken into discete parts +######################################################################## +# this is fundamentaly different from how the physics simulation works! +# there rotation equates to setting positions/velocities to individual motors, +# which then do the moving. +# there you can only SENSE via SENSORS what the result of the rotation is +# in this brach we essentially just do numerics! + def updateDHMat(self): + self.ct = np.cos(self.theta) + self.st = np.sin(self.theta) + self.HomMat = np.array([ [self.ct, -1 * self.st * self.ca, self.st * self.sa, self.r * self.ct], \ + [self.st, self.ct * self.ca, -1 * self.ct * self.sa, self.r * self.st], \ + [0, self.sa, self.ca, self.d], \ + [0, 0, 0, 1] ], dtype=np.float32) + + + def rotate_numerically(self,theta, clamp): +# potential clamping for joint rotation limits + if self.clamp == 1: + self.theta = clampTheta(theta) + else: + self.theta = theta % (2 * np.pi) + # self.theta = theta + #self.HomMat = createDHMat(self.d, self.theta, self.r, self.alpha) + self.updateDHMat() + + def rotate_with_MDH(self, theta, clamp): +# potential clamping for joint rotation limits + if self.clamp == 1: + self.theta = clampTheta(theta) + else: + self.theta = theta % (2 * np.pi) + #self.theta = theta + self.HomMat = createModifiedDHMat(self.d, self.theta, self.r, self.alpha) + + + def __repr__(self): + return str(self.HomMat) diff --git a/manipulator_viz/robot_stuff/utils.py b/manipulator_viz/robot_stuff/utils.py new file mode 100644 index 0000000..e359695 --- /dev/null +++ b/manipulator_viz/robot_stuff/utils.py @@ -0,0 +1,36 @@ +import numpy as np + + +def error_test(p_e, target): + e = np.abs(target - p_e) + if e[0] < 0.002 and e[1] < 0.002 and e[2] < 0.002: + return True + else: + return False + +def goal_distance(achieved_goal, goal): + return np.linalg.norm(goal - achieved_goal) + + +def calculateManipulabilityIndex(robot): + M = robot.jac_tri @ robot.jac_tri.T + return np.sqrt(np.linalg.det(M)) + +def calculateSmallestManipEigenval(robot): + M = robot.jac_tri @ robot.jac_tri.T + diagonal_of_svd_of_M = np.linalg.svd(M)[1] + return diagonal_of_svd_of_M[diagonal_of_svd_of_M.argmin()] + +def calculatePerformanceMetrics(robot): + M = robot.jac_tri @ robot.jac_tri.T + diagonal_of_svd_of_M = np.linalg.svd(M)[1] + singularity = 0 + try: + M_inv = np.linalg.inv(M) + except np.linalg.LinAlgError as e: + print("ROKNUH U SINGULARITET!!!!!!!!!!!") + singularity = 1 + return {'manip_index': np.sqrt(np.linalg.det(M)), + 'smallest_eigenval': diagonal_of_svd_of_M[diagonal_of_svd_of_M.argmin()], + 'singularity':singularity } + diff --git a/manipulator_viz/robot_stuff/webots_api_helper_funs.py b/manipulator_viz/robot_stuff/webots_api_helper_funs.py new file mode 100644 index 0000000..8299d4b --- /dev/null +++ b/manipulator_viz/robot_stuff/webots_api_helper_funs.py @@ -0,0 +1,191 @@ +#########################################################3 +# for now only for ur10e, later write out for other robots +#########################################################3 +import numpy as np + + +def getAllMotors(robot): + motors = [] + motors.append(robot.getMotor('shoulder_pan_joint')) + motors.append(robot.getMotor('shoulder_lift_joint')) + motors.append(robot.getMotor('elbow_joint')) + motors.append(robot.getMotor('wrist_1_joint')) + motors.append(robot.getMotor('wrist_2_joint')) + motors.append(robot.getMotor('wrist_3_joint')) + print("imported motors") + return motors + + +# for validation purposes, we must plot this circle +def drawCircle(radius, height, robot): + display = robot.getDisplay("display") +# display.setColor(0xFFFFF) + display.setColor(0xF8FF04) + display.setColor(0xF8FF04) + display.setColor(0xFF0022) + display.setColor(0xFF0022) + display.setColor(0x00FFE6) + display.setColor(0x00FFE6) + display.drawOval(100,100,60,60) + display.drawOval(100,100,60,60) + display.drawOval(100,100,59,59) + display.drawOval(100,100,59,59) + print("drew the circle on the screen") + +def setMotorSpeeds(motors, speeds): + for i in range(len(motors)): + motors[i].setVelocity(speeds[i]) + + + +def initializeMotorsForVelocity(motors): + speeds = [] + for motor in motors: + motor.setPosition(float('inf')) + speeds.append(0) + + + setMotorSpeeds(motors, speeds) + + + +def initializeMotorsForPosition(motors): + speeds = [] + for motor in motors: +# motor.setVelocity(float('inf')) + speeds.append(0) + + + setMotorSpeeds(motors, speeds) + print("did motor init") + + +def getAndInitAllSensors(robot): + sensors = [] + sensors.append(robot.getPositionSensor('shoulder_pan_joint_sensor')) + sensors.append(robot.getPositionSensor('shoulder_lift_joint_sensor')) + sensors.append(robot.getPositionSensor('elbow_joint_sensor')) + sensors.append(robot.getPositionSensor('wrist_1_joint_sensor')) + sensors.append(robot.getPositionSensor('wrist_2_joint_sensor')) + sensors.append(robot.getPositionSensor('wrist_3_joint_sensor')) + + # now we must specify how often do sensors get read in miliseconds + # i've put 10ms since this seems like it should work smoothly, + # but ofc you should play with this value later on + for sensor in sensors: + sensor.enable(10) + + print("imported and inited sensors") + return sensors + +def readJointState(sensors): + joint_positions = [] + for sensor in sensors: + joint_positions.append(sensor.getValue()) + return joint_positions + + + + + +def getAllMotorsJaco(robot): + motors = [] + motors.append(robot.getMotor('j2n6s300_joint_1')) + motors.append(robot.getMotor('j2n6s300_joint_2')) + motors.append(robot.getMotor('j2n6s300_joint_3')) + motors.append(robot.getMotor('j2n6s300_joint_4')) + motors.append(robot.getMotor('j2n6s300_joint_5')) + motors.append(robot.getMotor('j2n6s300_joint_6')) +# motors.append(robot.getMotor('j2n6s300_joint7')) + print("imported motors") + return motors + + +def getAndInitAllSensorsJaco(robot): + sensors = [] + sensors.append(robot.getPositionSensor('j2n6s300_joint_1_sensor')) + sensors.append(robot.getPositionSensor('j2n6s300_joint_2_sensor')) + sensors.append(robot.getPositionSensor('j2n6s300_joint_3_sensor')) + sensors.append(robot.getPositionSensor('j2n6s300_joint_4_sensor')) + sensors.append(robot.getPositionSensor('j2n6s300_joint_5_sensor')) + sensors.append(robot.getPositionSensor('j2n6s300_joint_6_sensor')) +# sensors.append(robot.getPositionSensor('j2n6s300_joint7_sensor')) + + # now we must specify how often do sensors get read in miliseconds + # i've put 10ms since this seems like it should work smoothly, + # but ofc you should play with this value later on + for sensor in sensors: + sensor.enable(10) + + print("imported and inited sensors") + return sensors + + + + +def getAllMotorsKuka(robot): + motors = [] + motors.append(robot.getMotor('joint_a1')) + motors.append(robot.getMotor('joint_a2')) + motors.append(robot.getMotor('joint_a3')) + motors.append(robot.getMotor('joint_a4')) + motors.append(robot.getMotor('joint_a5')) + motors.append(robot.getMotor('joint_a6')) + motors.append(robot.getMotor('joint_a7')) + print("imported motors") + return motors + + +def getAndInitAllSensorsKuka(robot): + sensors = [] + sensors.append(robot.getPositionSensor('joint_a1_sensor')) + sensors.append(robot.getPositionSensor('joint_a2_sensor')) + sensors.append(robot.getPositionSensor('joint_a3_sensor')) + sensors.append(robot.getPositionSensor('joint_a4_sensor')) + sensors.append(robot.getPositionSensor('joint_a5_sensor')) + sensors.append(robot.getPositionSensor('joint_a6_sensor')) + sensors.append(robot.getPositionSensor('joint_a7_sensor')) + + # now we must specify how often do sensors get read in miliseconds + # i've put 10ms since this seems like it should work smoothly, + # but ofc you should play with this value later on + for sensor in sensors: + sensor.enable(10) + + print("imported and inited sensors") + return sensors + + + +def getAllMotorsJaco7(robot): + motors = [] + motors.append(robot.getMotor('j2s7s300_joint_1')) + motors.append(robot.getMotor('j2s7s300_joint_2')) + motors.append(robot.getMotor('j2s7s300_joint_3')) + motors.append(robot.getMotor('j2s7s300_joint_4')) + motors.append(robot.getMotor('j2s7s300_joint_5')) + motors.append(robot.getMotor('j2s7s300_joint_6')) + motors.append(robot.getMotor('j2s7s300_joint_7')) + print("imported motors") + return motors + + +def getAndInitAllSensorsJaco7(robot): + sensors = [] + sensors.append(robot.getPositionSensor('j2s7s300_joint_1_sensor')) + sensors.append(robot.getPositionSensor('j2s7s300_joint_2_sensor')) + sensors.append(robot.getPositionSensor('j2s7s300_joint_3_sensor')) + sensors.append(robot.getPositionSensor('j2s7s300_joint_4_sensor')) + sensors.append(robot.getPositionSensor('j2s7s300_joint_5_sensor')) + sensors.append(robot.getPositionSensor('j2s7s300_joint_6_sensor')) + sensors.append(robot.getPositionSensor('j2s7s300_joint_7_sensor')) + + # now we must specify how often do sensors get read in miliseconds + # i've put 10ms since this seems like it should work smoothly, + # but ofc you should play with this value later on + for sensor in sensors: + sensor.enable(10) + + print("imported and inited sensors") + return sensors + diff --git a/manipulator_viz/run_classic_ik_algs.py b/manipulator_viz/run_classic_ik_algs.py new file mode 100644 index 0000000..47352be --- /dev/null +++ b/manipulator_viz/run_classic_ik_algs.py @@ -0,0 +1,57 @@ +import numpy as np +import gym +import inverse_kinematics_gymified +import inverse_kinematics_gymified.envs.forw_kinm +from inverse_kinematics_gymified.envs.inv_kinm import * + +#env = gym.make('custom_fetch-v0') +env = gym.make('inverse_kinematics-v0') +#env = gym.make('inverse_kinematics-with-manip-rewards-v0') +#env = gym.make('FetchPickAndPlace-v1') +obs = env.reset() # need to call first 'cos new version errors +env.render() +#obs = env.reset() +done = False +print(env.action_space) +#obs = env._get_obs() + +def policy(robot, desired_goal): +# del_thet = invKinmQPSingAvoidE_kI(robot, desired_goal) +# del_thet = invKinm_Jac_T(robot, desired_goal) +# del_thet = invKinm_PseudoInv(robot, desired_goal) +# del_thet = invKinm_dampedSquares(robot, desired_goal) + del_thet = invKinm_PseudoInv_half(robot, desired_goal) +# del_thet = invKinmQP(robot, desired_goal) +# del_thet = invKinmQPSingAvoidE_kI(robot, desired_goal) +# del_thet = invKinmQPSingAvoidE_kM(robot, desired_goal) +# del_thet = invKinmQPSingAvoidManipMax(robot, desired_goal) + + return del_thet + +for experiment in range(400): + print('experiment #:', experiment) + print(obs['desired_goal']) + thetas = [] + for joint in env.robot.joints: + thetas.append(joint.theta) + print("POST:", thetas) + for step in range(50): + env.render() + #action = policy(obs['observation'], obs['desired_goal']) + action = policy(env.robot, obs['desired_goal']) + #obs, reward, done, info = env.step(action) + action_fix = list(action) + action_fix.append(1.0) + action_fix = np.array(action_fix) + #print(type(action[0]), action) + #print(type(list(action)[0])) + #print(list(action) + [1.0]) + #print(type(action_fix), action_fix) + obs, reward, done, info = env.step(action_fix) + thetas = [] + for joint in env.robot.joints: + thetas.append(joint.theta) + obs = env.reset_test() + print("PRE:", thetas) + + diff --git a/simple_raw_comm/.main.cpp.swp b/simple_raw_comm/.main.cpp.swp new file mode 100644 index 0000000000000000000000000000000000000000..f312dd4c830401aadfa304574c58b8f0a6961b6e GIT binary patch literal 20480 zcmYc?2=nw+u+TGNU|?VnU|{g?RZG>fsASln&%ltLUtEx%l2`<i!iRGci?cKH@Tq{v z)xiwZ&&bbBHPVN3b4v44^b3j-D>94qON-)*Gjj`aQsa~J^Gb^HbM&FyqQvs}<ow)R z{oKUNJiX+C0*Ilb<Y)+th5+FZC@o3TwcrgkHZn8-DOOfeR1g*l1u;kQXb6mkz-S1J zhQMeDjE2By2#kinXb6mkzz7L}k^*LidIkmtCa8ZMpfn>I%>w1?Kxro^4O7Pq<@-SC zTqq5bhjK@$(GVC7fzc2c4S~@R7!85Z5Eu=C(GVC7fzc2c4S~@R7!82|2!X^D28J|N z28Jd!$oxO7|G$NwfnhU0149-+1A{p~1A_@a1H(r?28OkK3=B<t3=H*r3=F<}3=Cd; z3=GVC3=I2t85oLq85p#A85lTu85r1j85s8RFfi=lVPGicVPMGOVPHt-VPJ6LVPMea zVPMeUVPLqy&A@P&n}Oi~Hv_{SZU%;8ZU%-TZUzQNZUzP!ZU%;zTnr3*xfmGsa4|5f z;9_8y#>K!em5YI)o{NE@j*EdopNoOv9w!6C3Qh)wL{0_<Ax;K{qZ|wjbsP)~wHyo# zVH^w$svHaq>>LaXZ`c_aUb8bWEM#Y3n99z;kigEspwG_0@PUni;Sd`G!wNPAhGlFF z3`^M{VPVe3z+lbBz#z}Yz`)GL!0?=vf#EbO1H*b&28MO43=EyD3=DCs3=BT33=Hb5 z3=GPw3=B%F3=E%G7#Pm7FfdGGVPGg?VPLRfVPMc>VPMc=VPMc?VPN>j%)oGsnSo(8 zGXp~}GXp~>GXp~hGXp~zGXsMMGXn!3GXujvCI*HdObiU)nHU(JF)=XgWMW`wU}9h> zU}9j%Vq##(WMW_lWMW{DXJTM@#K^#KiIIWfB7C4lUtd8%L7^zMq_ikc!O)rk!Ve8{ z_KElLcaL`s3i1!qP;$;M%}G(n%P&zVPR&bEa4Jg7P7O-UNlh$HRRFEBNX$!7(u8Qv zOjFQMOpnj4j891{&Pd8nEK1R}gQ`K+r=h8-P|cvAps%k0GpQsaRUrvvbTI?i9kpPR zWY-1cm!}pf<mZv%GO!;1JS0bfMHDhi6!P<^;hMBG(w&o*hSfc3X$%Ys3bhQV5eMQC zh_>RA`~ro7qWtut#9U<8C@A2IwBVBb0wi~UxG+O>KpIOjQj1a*GK&@R^HLRxO7rqE z^U_IoU~+z5UTSiQLP@>?Jl%kj6i&B+mBZYHn%?w^Qj1edH1bl*6<lCx&^aeFHLpaY zC_gE`BtEl1Q`4FOoc5ey_GY9ifYKjW0YV!@k#AylW?njyd{Js*N~J<tei2v<7Nkj) zR!AO)+N5BsP+XE?WmQ~Kl$n>Vkyw;o7Ht>{wjLDfiACwj3U&&Hkj#LjQUUCe_{;(Y zTLl+aH^)$)koX{fC;yOm&j6^~0}_jhA+F9WP)ICFFU?KOD=7xWNqS~kY97cf>G4_l znRzAg#RaLUDaFyIu?n^dx(0fNAQRB!Oz_AV<B>DMBWDPa%gifL0IkE)05L#5)=@~# zNGwv&0*6K{$nPK-0!ow^7#J9dP{#nt8lVgT!ZtRL2uRIK$pLvABnH7YHn4;o4^6iU zHZ}@MF$TuQ(FVCOc}ffl5Ut7ir6mX{Lu+HB+yH1cf@-u<Py#6c$6r!@eokUeK}I4Z z@dOls+zd)vC8-r93YmEd=|!ojc?!k(3gxK^#i^;FLMAD-q$IV7p)5Z$MIj?GFC`~6 z2$X)Hwgi_XmZWNc%td!FDCqR{6;e`55;Jp(^*{w?c&b8PYHEr?qC#?FPEJx{a<)QR zX<l+kW_}(hcNL|kXBL;F7U?Mjr=}`|7P%B<mZcV{7b_Gb7A5AUmZTOHgAz6aLs@Ea zNq&)8N_>d|YD%%jCRLDMoC&g`*qQ+x2&H+Mg{7(S1tmo`XnD|10X4T`YJfQkrV{Kz zYX*ko{Ji24NQ^*olY+COkEc_RV~D4}U%a!2tFw1-sIP*pf>Lr~PG(ZPp^>?Tg@J{M ziHU`Yk+Gqfv8joXk~J1%JpBSfL*j#6ojn6wLE2L*5<zPUi}j0Ai}Op1l2eQIi%L>b z<1_OLN=xF4Qj;?aQuRtIO0XK{9~wfOY5AokL>L$B9OM}g67S~e19lq1MX41fsYQ8- zIk1(FdZk6h$wiq3*j<HQgn+zhXlbNpXl9{jsAp&Z3K)HTg`CX1ROFPPuaKw!Nt+7E z`FUxX>7_-9plDVot}HG|%~dEaP0mnAELL#J&o3^~gC^Jb`~r}gVo+-I%u`5IC`wJt z(Jjv}%1Kdx1}!MfmxCJ>NvSXkO7a!bQ%e*|GV+U470MEGN>hs!(u(roRTTq6X)&l6 z&P&WqEiOn*PE{x^O3txnU{KD?OU@}xNma1PP0h_Os<cBA$;>Y<DN0SuwL?}`S`?oQ zt`FjKGLwoDiz@X?i$F_BQ@|-rF9X>CtZLy^6x19-T1qMlQi};G0yUEKq03m~bMsSD zK^hqJ^>wKN7!<Vh85Fb_6tonIQi}^xlS?woQWerNb5e`-K!U!B*{KS}rA4XWLZUQJ zp(G=-7*^as;vuu7SOJnB6jCyaz{Nu)s65LrN=YpOX-mt@O99o0P<vuh5=&A+MXiy6 zk%6v(g|2}a$flUY(vpn)A_WDv)SS!;1=ota)FOr4#M0yphqS6py_8gtc6}|y7?9(r z=YLRY)YVmR^>b11@pN|e3wBj-^6_`}Rv<Na8T9o*X60n&W|n|6L$N|>9ykOtQWbnM zlT-7GQ^6%MsDK5P`%p1Zu?h+u1yJZ`re!88<Rs>$mnNpCDx~L^r51r2P6`F7MY);9 z#hLke#R`deDIkl&JwrVFLqin89fN`#{X#rlgBARP6rBD2Ts%PqU$BC|n}VZXq=L7n zpNoz{Y9^>brjS}uP?TC+tdL)%09qfNnVJH2Qc5PM<V-3pNln3YG^i1WtRO#6Au&%u z$uU^LGgwK%$uZb7SO?^w(7YUw(M73+rI|&kDGEuI3W)^;Iho0cNja$sIf>;UTN6RG zTnZ>vWacT97iE@Y=B4W>6z8XvlqVLYLc<W0I5J8~3as??%gf945(^TOGg9^Pi_-OT zAmLc74~;8bBRvDCrpWwKh1|qSh5V$F#7t1}lAK>qsgR!r4qI4QCzgO*S_xGO>baDH z+e(?m;54t0T9KSu0IrRb^K%PwG86NXQx(cHOEQp60S6h>MC9;t1iKR!*E$MesYRd| zQ!vsqP|yIyxDr$os09m2EzbD`l|`B986^s!%%I>F<*DG7UsRl&QJR-tT$P$vQdF9& z;F*_}UzD3zl9>&1ysoZ-ldHR@A4SO(+;;(IWsuMFixg6G6O*A~0B&f2N>p7fT?J5q zW2KO+t!=H4nU|89SE5^zm{hC_5zfoZQP9=WWdQa6cQ7(Auz)7;1sE7$^Y_2_85n-@ zGccUsXJ9za&%iL1pMjyApMk-EpMl{y9|OYzJ_d$#J_ZH}J_d&4ybKIeco`Tb^D;1G z@iH)E@-i^^@iH*@@-i^!@G>xP^D;0z;9+1`!^6N3%frAR$OGx~7jQE$Byck@sBkkd z{N;l5_cwDfFqCjHFc@($Fg)dCU|7t_z);4?z>vquz>v$yz~I2iz#zrRz;Kg;fngE{ z149Z21A{UL0|O@q0|O%m1H%n=28Q+Q3=D1T3=FaC3=Agh3=G`t3=F&27#MQc7#Pf; z{rMxT3=ET585ojS85lHJ85n-DFfc4(VPKfV!oX0)!oZ-z!oVQK!oVQJ!oaYWnSo&e zGXp~mGXp~yGXp~qGXsMOGXsMVGXnz~GbHR5F)=XAhb^KQfN`9`VP!1?BLfISTr&mK zyoWFuV8auW;bNdRJ#>`90Mv?ycXd7e-263^eDljvLA@Vvx0o1pDfxM+pxmhdZH<C! zeqB3I7ugxIlSBjDM+4al>uf71fNTYc(AEa<$V<3mkRS5E3#e(7R+L(tp^%bTlBiIS zn4Ar&Ilv7ha90uH8wG`0h18tlRPfjesCWfA8El}7t5c}Eh7w43d_i`)RZO00v62qh zG!S3ct|UJg(nke#j@20$7&sUh80NrZfPn!fHXBI{WCbF8kjH8uG7u7d014bFg$+UI zDR>rxOI@%xKn;qb)ZF|M(8v;aOOHLwXCSlS-hqrvfd@gLV+2L1C7|vQCwiw1JY<y! z>7c=SGVtz7v_UMW<E8|vs6Zt*#AODE-W+<j5!^2eN=;79EK3c^%uUTNEwQr7&CJQk zEKW_<Ff=d#g&xSI5Dc>r(gD{2``OA0n#6qbQ&O$0eEnTq<AVcSU0u9%p#A}O^@>tU z6l_tGC#W7x^#b+io%3^Z6Z29uKoteVCItlrqTB`<;e+Ubxegv2cpT^&6z1;*uK&S$ zNpK=qJ!qgWv=~~)CMTAHdI+GnE-1=RDTOpfp^XTT45&>C)~2toP@b8SqmT<~o`NC> z)`5U_ycKfui&7PeQgbrFjgS01g}nScT~LRkBr`V^+D$<=EjYCV)D!_HI4t3%0M-Vp zD<S<rg@V$eg8brC&=6NnrJh1aMrJWIqA&swG{mB9tB{$OTm&wVKxGxkBbW+JVikx} zXiTg^BVrXA606VvyFySA2=bHy!k-GZ3I=)xMlkOxC=?_X7pJDg7w6=c#}_0Pfs!&j zP!w!HiVYQNYA}?+41ktmpdm8MpfinCuu;%82IY2$Gf*9(YitclZQxc<PGV7dszO$2 zZUH16C4!O~s9OTgkP5}1k%GK*Xl%k=ot9W!g6V2IbXUWb!CZ~gxpoRjuGQC92nIFE zQx$UaL5Uq)C?w{kfZDJ*iNz%f;6eyAd<XI!BCty`it@`L;~zz(d7y+EpITIuU!(yh z<8zDC!BrMW6K<>oE@dH!SfMxr+y@6Wk24Yr3R3eFm0(_hmZG5ULSDK8+y>CNflDT2 zE&$x!Q%_7TDNW3YFH{HRW>D<{nj<KI^cX=SXb`99>w~k60@x3rHYd#L)Uwnf1&HMe zrI3zAZe}rTupu=C8a1GhRRoz1sjJdcOI#955;c@ywkzqtN2%b28FYpL<N)w2Lt=_T za%oXfY92I%72ql%t_6n-xIzS53bGPrBXUxM3|~PT3{ZyzK&oR%n*kbhNJC)I(W8>o zqO`>1R4Xfx+o4+Qz!rjhrC<xTR$m_!kf7MrgYk=#Gg4DZb5c_vJrq#srVCP_P*9Ya zUzAyb+6zz3gLLCF5=$VR_{?JPP#`oc_4O6fKwY=gykuyvDYIB1CqKVHPXT?f2B|=X zm{gJh8u^ALo1|2j17Ra}nxGyYxM+ohCAjELO-qA}Hsn+)B&C8p2O9iMPX#6SVnk^S zGb<=HF-0LKu_U#)L?I}|#TC}nfhJk7kx4oEpkdAYB85bS4A2Zya(+r`3b^bB4b^HC zr-GbTo{^c8s-U5$fl(Hzslf^!cov2x7I^yt(yk0H0To@j`DLIa3#xv={(>|}z%?M$ zXt*&7whC#9ImM}<Y=o=;xg3NlU{Fv<$u9+UnGppexD++C2IYN}WL6Av2PiZ^^?qqy zW-@r-0hHVzgUsNv4l*bM4&^jZtq9fxa(HEaDQJWxGY=Ba5G|Q`(B!C)RGOBSTBHLS z$IAx|9+YQ->S<6_os$C|k}Jq8NCl4!f`%#5a`MZIL6MPKl$w@bq@$3M4>8am)P)AQ z5nP_Z9SIs$0J|hLMF%wEosn1u9%BO6=#Z3}4;mp%fn>sx3{cd8@+=;VZQK{N-GxK5 zhpVHDk85zS4x|{(%`Z#UD9O(U4Wj1e#uulSlon{h8euq$010~{4H)YvsDV4um?QQe zgOFkoX<7=FU*PjmItobRygCXfW5^(5AVc5a1O@Ygf~`VHQ7Jf~p^vzS<mcyr5=t=0 zKXwXe9t5W@eSHPL{BoqMnpq5vgi3`HP;7vkw+e}{@jvh=f<i60|KGyJz!1O<ng55a z@4w2=z_62_fnhE`14AP}14Al51A`|&1A{I<1A`7f0|Prh1H&CY28L673=B*87#J2p z#{>%a7#O_y7#KwO7#KeAGBE7oWnfst%fPULmw{n9F9SmjF9SmeF9U-lbX<U)7qV91 z3l9UsF&+kn9v%jU8Xg9QG9CtoQXU3|6dnc!a~=kUf7}cV@3<KlUT`xoJm+R$IM2<% zFc&(00CqdHpE*j6hQMeDjE2By2#kinXb6mkz-S1Jh5$W705TR^G`jk-Jhf<a^(D3? zv!LacuvLzral;bC63)@pm*DQ}=;}*cD>|X8FR`xnG=Qwkgvf!`QCV3Pr<VBSr~9Oq TrRHcrL||MiE6`||rZocq9(K%X literal 0 HcmV?d00001 -- GitLab