From 853dfc9422a00e7fa89112109b3ce1c774b0b5db Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 20 Nov 2023 22:22:02 +0100
Subject: [PATCH] real calibration works

---
 .../.drawing_from_input_drawing.py.swp        | Bin 45056 -> 45056 bytes
 python/examples/drawing_from_input_drawing.py |  12 +-
 python/examples/path_in_pixels.csv            | 171 +++++-------------
 python/ur_simple_control/.managers.py.swp     | Bin 0 -> 49152 bytes
 .../__pycache__/managers.cpython-310.pyc      | Bin 6516 -> 6797 bytes
 .../clik/.clik_point_to_point.py.swp          | Bin 0 -> 28672 bytes
 .../clik_point_to_point.cpython-310.pyc       | Bin 3951 -> 4668 bytes
 .../clik/clik_point_to_point.py               |  88 +++++++--
 python/ur_simple_control/managers.py          | 100 +++++++---
 .../util/.calib_board_hacks.py.swp            | Bin 32768 -> 36864 bytes
 .../calib_board_hacks.cpython-310.pyc         | Bin 6463 -> 6960 bytes
 .../util/calib_board_hacks.py                 | 158 +++++++++-------
 12 files changed, 299 insertions(+), 230 deletions(-)
 create mode 100644 python/ur_simple_control/.managers.py.swp
 create mode 100644 python/ur_simple_control/clik/.clik_point_to_point.py.swp

diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
index f16debe88377c08adde32dd68a6a0aafb6b8ad24..a9ef8b4996e0bd1281f5d0bd4956cb4b044d3265 100644
GIT binary patch
delta 606
zcmZp8z|`=7Ni4}A%+puFLeGeSfq_AQfq`>Rbn1TfjbiWQC$l7oOqNji!nd1|fuV_!
zfx(NBfuV?T;zfhW(TX{fzoZFH)>Gw}{6JAo-cZj#A*oU!B{eOvG^a!%DK$B<v^Z5E
zEx$-1CAUCt@<uh$&5BBoby+od7#J2z<_vIG{KU(^aG95ZVJ0sFLlrLrgB33Wg9a}H
z0}C$$!&Z>0&4mH_JewEA7&9s;D<~)^6y+!7m*^FzmIN21rlthvWTvDRX&C4kYceQr
z{v0bP#A?sXzz{!~v*5VrKXwL&lk5x(``H;7CbBayq_Q(G*swD&JYr*Dn8wDy;KRni
zAkN0X@Q{^(;Q=cH!%0>KhALJD1`k#S1_o9JhEpsI47Dr_4Dp*K3*DI~8y6{VZZDa^
z%$8V`UaX_AS-#?_dObfg1A{sXB+TqVVaLG0u$iBMp`D+BA&;MdA(x+lL6x6@;VvHo
z!&E*7hADgu3_*Mh3=Vt@4CZ_c3}$=`40?PF3`~3s3@dpV7{YlO7}!8z!o$GO#lyg0
z#lyhxhMR$5A2$O-IyVD@AU6ZU11<)JW-bN>11^So1{p2}25Bw^hL@ZS49htg7$$Nu
zFvM^&Ft~6sFc@(%FbHrmFns1<VA##Uz_5&ifng#C149G{1A`$41H(IL02Q$_Fa)zR
zFo>`-Fx+QjU^vOfz%Y}Ifx(xJfq|C|5;$GZz_DOuU|?cpVA#sSz|hUY&%jW{!oZNs
d!oc9f!oZ-uS#Y5k^Ja}D3EYgzlRMT|006xQcewxn

delta 584
zcmZp8z|`=7Ni4}A%+puFLeGeSfq_AQf#G#WbZTknMzMGDto95H4AGl86_^?M&M-1C
zOlD+Yh-PG9C}Ny=(Q&euVh)Reo{8q<Mn%!d=N0An4D?JDk}4HaQqvMkb4n&}R1@8-
zsPtHuRf~s#VbNsH0C&aDybKIic^MdH^D;2h@G>yi@G>xH@iH*5@iH)M2dUaz7@*Iy
zd0vbuqnv_*f<jS#QhteEacW6$L27DBa871QYLSM4p0Osw=FhQ$LaeUL3=FB0ISY<^
zvT!gkoM&fXILgkzFrA%&A)B3n!HJ!L;RPE5!yGmSh9EWu23a-+hUcsd49{2@7|ydY
zFf_0-F!-@DFtD*QFkE0^U}$DxU`X98S?JEps;r=^tGhYAWEb;h`HH9N^#aTc3>qwu
z@Bsx1D5y5`Gca`UGce@yGce@wGcc&}GceraV_=xZ$G|X^kAWeWkAcCFkAcC0kAcCQ
zkAXp-kAZ=ikAYzoF9SmaF9QQRF9X9B9tMVP9tH+$9tMWD+zbr+xfvKTxEUCPxEUB8
zaxpNpa4|3#axv61$Z|0-$Z#<*yy9eFSi#A_Fo~0aA(oSY!IhJN!I+bQL6DPy;R^=?
z!yXO>hUFX#43jt*7$P|s7>qa=7~ZopFq~p%U?^s1U<hGnU=U?zV0ggBz;KF<fngRK
z1A`wM0|Or$1H(gB28M1{28L=@1_n!31_owU28L}c3=BOi{0t1$EDQ`OEDQ|JEDQ`9
Tn*|q&F>h8|lE^)|V|@hxuFY*4

diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index bfa0b35..d8760ef 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -59,8 +59,8 @@ def getArgs():
             TODO: check what this means", default=0.3)
     parser.add_argument('--speed-slider', type=float,\
             help="cap robot's speed with the speed slider \
-                    to something between 0 and 1, 0.5 by default \
-                    BE CAREFUL WITH THIS.", default=0.4)
+                    to something between 0 and 1, 1.0 by default because for dmp. \
+                    BE CAREFUL WITH THIS.", default=1.0)
     parser.add_argument('--max-iterations', type=int, \
             help="maximum allowable iteration number (it runs at 500Hz)", default=50000)
     #######################################################################
@@ -166,8 +166,8 @@ Obviously it's just a hacked solution, but it works so who cares.
 """
 def getMarkerOffset(rotation_matrix, translation_vector, q_init):
     # put this in a correct place
-    old_speed_slider = robot.speed_slider
-    robot.setSpeedSlider(0.3)
+#    old_speed_slider = robot.speed_slider
+#    robot.setSpeedSlider(0.3)
     # TODO: this isn't gonna work because it's not going 
     # orthogonaly to the board
     # so TODO: calculate TCP speed based on the rotation matrix
@@ -191,7 +191,7 @@ def getMarkerOffset(rotation_matrix, translation_vector, q_init):
     print("translation_vector - current_translation", \
             translation_vector - current_translation)
     marker_offset = np.linalg.norm(translation_vector - current_translation)
-    robot.setSpeedSlider(old_speed_slider)
+#    robot.setSpeedSlider(old_speed_slider)
     return marker_offset
 
 #######################################################################
@@ -299,7 +299,7 @@ if __name__ == "__main__":
     # do calibration if specified
     if args.calibration:
         rotation_matrix, translation_vector, q_init = \
-            calibratePlane(robot, args.board_width, args.board_height, \
+            calibratePlane(args, robot, args.board_width, args.board_height, \
                            args.n_calibration_tests)
     else:
         # TODO: save this somewhere obviously
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index 7714927..6f2f561 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,124 +1,49 @@
-0.27578,0.76759
-0.27727,0.76608
-0.28025,0.76458
-0.31307,0.73749
-0.33843,0.71793
-0.36976,0.69686
-0.38169,0.68933
-0.39064,0.68332
-0.41749,0.66676
-0.42346,0.66074
-0.42793,0.65623
-0.45478,0.63967
-0.45926,0.63817
-0.46522,0.63215
-0.49208,0.61560
-0.49655,0.61259
-0.50401,0.60657
-0.52191,0.59754
-0.52638,0.59603
-0.52788,0.59453
-0.53533,0.59152
-0.55920,0.58249
-0.56368,0.57948
-0.59053,0.56293
-0.59948,0.55992
-0.60992,0.55691
-0.62782,0.54938
-0.63826,0.54637
-0.63975,0.54487
-0.64572,0.53885
-0.66213,0.52982
-0.66362,0.52982
-0.66810,0.52681
-0.67257,0.52380
-0.67406,0.52229
-0.67555,0.52229
-0.68003,0.51628
-0.68152,0.51477
-0.68301,0.51327
-0.68749,0.50725
-0.68749,0.50574
-0.68898,0.50424
-0.68898,0.50273
-0.68898,0.50123
-0.68749,0.50123
-0.68301,0.49822
-0.65616,0.48919
-0.64721,0.48919
-0.61290,0.47865
-0.60544,0.47564
-0.59649,0.47564
-0.56219,0.46511
-0.52489,0.45909
-0.48313,0.45909
-0.47417,0.45608
-0.45777,0.45608
-0.44882,0.45307
-0.41152,0.44856
-0.39511,0.44856
-0.38169,0.44856
-0.34738,0.44254
-0.33694,0.44254
+0.18180,0.64870
+0.19075,0.65171
+0.24744,0.65171
+0.26534,0.64870
+0.29517,0.64268
+0.30263,0.64118
+0.30710,0.63817
+0.33395,0.62312
+0.37274,0.58249
+0.37423,0.57797
+0.37871,0.56744
+0.38169,0.55841
+0.38169,0.54788
+0.38169,0.54336
+0.38169,0.53885
+0.38169,0.51477
+0.38169,0.50574
+0.37871,0.49822
+0.37274,0.49220
+0.35782,0.46661
+0.35186,0.46210
+0.32500,0.44555
 0.32053,0.44254
-0.29219,0.44254
-0.27578,0.44254
-0.26534,0.44254
-0.21611,0.44254
-0.20567,0.44254
-0.19821,0.44254
-0.16390,0.44856
-0.16241,0.44856
-0.15943,0.45006
-0.14898,0.45006
-0.14749,0.45006
-0.14600,0.45157
-0.14451,0.45157
-0.14451,0.45307
-0.14451,0.45458
-0.14451,0.45608
-0.15048,0.45608
-0.15197,0.45759
-0.15793,0.46360
-0.16241,0.46511
-0.17434,0.47263
-0.18180,0.47564
-0.20269,0.48919
-0.21313,0.49220
-0.22208,0.49521
-0.26832,0.52229
-0.27876,0.52530
-0.28771,0.52831
-0.29965,0.53584
-0.32202,0.54637
-0.32948,0.54788
-0.33694,0.55390
-0.34589,0.55691
-0.37125,0.57196
-0.37572,0.57497
-0.38467,0.57647
-0.41003,0.59302
-0.41451,0.59453
-0.42495,0.60205
-0.44136,0.60958
-0.45180,0.61710
-0.45627,0.62011
-0.46522,0.62162
-0.49208,0.63817
-0.49804,0.64118
-0.50998,0.64870
-0.55025,0.67278
-0.56815,0.68181
-0.57710,0.68482
-0.61290,0.70137
-0.61738,0.70739
-0.62633,0.71040
-0.65318,0.72696
-0.66064,0.72846
-0.66511,0.73147
-0.67555,0.73448
-0.67705,0.73599
-0.67854,0.73599
-0.67854,0.73749
-0.68003,0.73749
-0.68003,0.73749
+0.31158,0.43652
+0.28771,0.43200
+0.28622,0.43200
+0.28473,0.43200
+0.28025,0.43351
+0.28025,0.43501
+0.27727,0.43953
+0.27727,0.44856
+0.27727,0.46511
+0.27727,0.47263
+0.27727,0.48919
+0.29666,0.54788
+0.30561,0.56744
+0.32202,0.60356
+0.39362,0.72094
+0.42495,0.75856
+0.45478,0.79919
+0.54727,0.91356
+0.58158,0.94065
+0.60395,0.96924
+0.60395,0.96924
+0.60395,0.96924
+0.60395,0.96924
+0.60395,0.96924
+0.60395,0.96924
+0.60395,0.96924
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..ff3d6e01f31f9e881f271a98f3a6be33a1e8c8a4
GIT binary patch
literal 49152
zcmYc?2=nw+u+TGNU|?VnU|=vj5}mp~w1(lX7z0CkesMv5N@5X63Lnl*EY8l%!>0lw
zR|hjtKO;Xk)kq)C%_+@G(Jv@UtjH|ZFD;5M&de>yNsUj=&nqd)&(SZaEXl~v!y=KJ
zn3tHIT2!o8PzkYMlpGC#(Gb8M0;MHsx)!_)#zuw)AO*@wiVDI)p&;fc9u0xf5Eu=C
z(GVC7fzc2c4S~@R7!85Z5EvmLP*T9fP|v`?zy$R#8<b{5qq(5`ASlfOrD5t|`fQ-$
zu$~1>o*gPK1ErTkX_!1Gly3{A4?}5~Jj_1uI0XX(!*?`!ZYbXbN((_XLMaATsJIH0
z_JT@4DFzOxILzICPzfl-zzmhwg3^nj5>N^%I!cX(z-S1JhQMeDjE2By2#kinXb6mk
zz-S1JhQMeDjE2DA41vTH28JX)28L;TkokXDKmIm91H)2&28M8c1_oY!28L}=H3@tS
z44?QI7>@8UFf8C>V2I;mU<l%4U~u7MU{L2{VEDkxz_5;&fnfqK14BPA14B1214At@
z14Aw^1A{Uz1H&C228LNY3=Dxh3=Et+3=9Xk85o+m85rWZ85sDv85mx1F)+O3VqjRs
z#lX<O#lT?B#lY}{lYwD5Cj&zwCj)~nCj)~HCj)~VCj)~3Cj-M54hDwL91IMPI2aht
zaWF7U<X~V(<X~Wk<zQfl;b35h;9y{I<zQei<X~X9&d$KFmYsniot=R}hMj@o0~-Uw
zel`Y%RyGEPMm7e9bT$SCVKxSahpY??8(0|_T3Hzw;#e6NTv-_y<Us=ntPBh{Sr`~L
zvM?}&urM%avoJ8+V`gBO!_2@?$jrbX$IQU+gNcFRI}-y#D-#2QCKCh0F-8W49!3U+
zZXyN>6rey^Au%sSA+toGJToUpA*-~wM4>#tC|e;Zu{bqFAwN$cB{R7sGe0k}s8S(2
zwX#@`0jg0!Ss}ATy;va;Sw&iYkwR{MN@-4FQD#Y{da*)rVs@&ILQ-mSVrg-zLP<tu
zu|j!br9x$XsX}sMo<ebAS*k*LMq){7S!xkjv^+7d1j&ljoYdUZyb`b@6AKDb^HMVN
z(iIXFauSP66f`nR6f%n;j!sm_$;``6O;O0nEG|(f%}YrwQYgttRmjNCPf^HEQ%KG)
zEh<jc#OM6X)MAjzl+?n~RJfBNyv$;-VY&ImB?`&;d1a}2nIKms=Oh*vE0p9bWPtnu
z@_bQhZhjfai6FbvGK-2!6c~`aQ=Y1xlcP|a3i5Pno<e!5LV9Wm$bd{_PiLlqY^zi+
zN>#|q2U}R23Gz>7UPfwBW(ml6P^1+Xq$X!3=44iZ1i(R?U!+i6lA%zXnVXrDSfqnw
zaY;sMajHUMQK~{>P7cU)h2q5AR0R!?k8&zef(0BWnI&L_nR%%3o1dnTmy%dilvr5|
zj*_C(lG36)g_4X!WcR~^8xk&Hry*hlWPW@>PJT(TLRwB@x&p}2`MIej86a;!osQ)4
zM1|s9u$wb;Q+3NT^HTE56%vc`OTnoYH9EmbDnBo$QXwf-AtNy_B_}mSAtSX25u2b`
zfiCq?$jQ$yPynk=PAp0-R!B_BFD(HFO+is=acW+PK9Va!{9XK^Mk=HvmL!5wF37zg
z(-Dp+PR&WvODR#XRWMZ0R{--$3sMqGQsavfOH$#+p-U*(DwrA=z@?QH3W`!wQqwZ?
zQd1O4ixhGb(=(G5@=9})Qj3b=s=$UM7Nr*}*eZZ1R0*gjeL#M+1tnR~>Lw5mRasGf
zQhrH%E@W+!f~^7yAFfncAyXkQH8n+{s3ZlxR7xQwACz#Bh2R>%Zp%%qh|erZElLCx
zA23(yp^Acaq@<=P#K&jmWtPOpYk;)spt?f`+5I|T&+34}KQ+D}vA852R6yw{<m9J=
zS(;W1kX-2uS<waZvoFMdRt!o?N(`B4pwccQu^41fDX4HM)=@}G%m$TCsksW7c?#v3
zB^e57rFqFE`T04;dIgC^C7FpidJKO4A+A>7LMlD845S=dDiozArc~<YCZ?pq912cb
zdJH+4*{KSs9@YVuc90}gk`MNULTX8}9)l;OEQPooR$gY7C}gLm78EPw<>%?9WEQ0+
zm*iCHLgQ8!lz?@MQgaeZQd1NP5{nXZQ%gW8#4}GJza%5I2vm@#6zf2WX;8{f%FoZ%
z1sMoRxzIcW5>HGnDNW2lb9_lYxNJ&PC@%sPyhX4~2hsp_0o0qpIhpAhB{`J}X^DBs
znW;qz1x5MkMTxn&AZs)V5{rvLj6_I~X6EN9AcZ}+6e$HOzzA_o23<_RfElmgXay6X
zQZczGHL)aBAvq&6Cq)4qoT<eMprRQZkIA5_36i{u^%$H>OB7(49-Q=b!DVV%W-=%|
zONzmg`QXx60hURiM(SX7Tuy#ox<W~6QLaK+W^rbI9s`4GS!z)wq-@J9RtODJfZ3p@
zfNBCma%oW!s8T5|0hJZtoS^_NSs@i%O1?sBMItBxK#~xs3I$b`pb{$=loM0(p?W}`
z1;tWPYGMw=tKd*BODxLFFD(Yu4y8HJObJfvpwQ0&rQO`z#Jm(p38Sl!k(mNY<EaXX
zd3pJjpcu?d(@o7OhE|f9X=$lo=Q;*>7Aq8`7M5lfrKTu=ouCUT_raxhMydkLVnh;E
z$V*KGC7Ar8^u)Z(s?-#)12gj!^7B#^l2h|aiV|}aa`KbGu7nzyl%JUcN?V}hu8^Fc
z0;<ixwE)Pg$O%%RJR>tX1Jp1`%qa&|R3HVQ+KwS4BUPawKQpfcRLw(bL};uQgOr2o
zaZsgU#Sj8&sHA0rLnW^yF*6U;&H<+ocsc;3wM3LW1C!EYU?|B4MT<gGYB9_%g~Za5
zjQk={gDEL9C$pqd5A2+@%)G=Lg_P9d%=A2k<c$2xWKd011S&Q_!Jn^?n4FxSo0|`+
zL_kIsrz#YKD$C4VP&^bA<bxtlAw54ar<frmu?W<fC@oGc0u^qc7zHnCRY=WC&&*2&
zEna0v&PdElPc4SD_Q0){5~v*zD>A{Yk&=wm%p!P7RVXOR&q_@$fw&lCJSenMD+*GR
zK_yQ~z5=KO1V<sbmM<;=g$*dwQ}aqPi$Ly3Nd>hhVT~$={Ib-doJxh{e9+q43Wda^
z;*z37a5EFqtjI`&x1k_0T?`87<V;W#rdXjMzqmLvDJNBr0b~bE0c0UBsBD4DD|qIC
z^Km9P*Xux<p9QH!kO&2NyevNx)SzT2D$UCSCGmVvTP79cA85|eQvfBJ;?(5)ycAe8
z6%=LWC1)1oq=JG0oM_5YK{1}5uaH)hpR0gK6^S|N`9+x}8M&Z_bU|VPs7(q<8KCqC
z@)M|9RzN6&rU_895t=3tmVwKm%wmO{%%mbvYaCSj<SXQ6=4Iw)Rzb3ANoH;;xTa1`
zK`mT#bs5rA^HM?CDlfkzwHRDlGJsv1oS&1E3Jv?r+=Bd~5(RLZ7b1(XLRzo1Br``3
zoE0()<I{^W3qZbtY77E3Co&7&A)<IqNKY+^&&^Ls%~62rg$pn!D}Y;DxzIXUp&&6i
zJ25>KRGnorKvjeCOJWX;mz-Zv$&gm0tWcC%T#%nvoC(QdI^gCrD3O2?Z83wgLP~y~
zdI_lIl9pdoo>-LPotc+Pp03=)lFZ~{1tW#jijty41yIOCs+qieg>+D#qbN18I6qHM
z0qTuJNYfG$d6}so$LQ+XL5g;0m6fOf4nJsXMX$IdwLn7?UTf+oK$?;Ypso_MMgi6G
zsnGDUf|v!)iXbl+LNX+{`H8FttR3RslGFknSdgTq7Jx!g2Wpx^QEDEjg8(YS3yLz!
zKvgQJ98XNjNd@<Kic1oUK%K^t3<dRgb(pT?oYcf3aIIOImzSEHT3ifHso-Q)%mC{D
z>#;E~d;)d+1sE7$^V|FQ85kz=Gce@xGcaiIGcYjlGcZhs_VMrXGB70bGBB|5GB8}>
zVPGiZVPNp&VPH_;VPLq#&A`ya&A{Nw&A_0`&A@P&i-BPs7X!mYE(V4~E(Qi&E(V6r
zoD2-NIT;wXa569?b22cvaWXJya56Bw;b366#KFKYm4ktyor8hFh=YOQ4m$(GBz6Xd
zcy<N`6?O&&PId-{FKi49pP_F42y*w}99kK5)o2KehQMeDjE2A<3jx?@X%S?k%oo&D
z2lr?|5}?5jJ+P31tpb>aODQWPfk&efOB8ZJ-D}X07N}Y;C`wIBEdq75z@17R1^AF6
zl2*`2g+iu!F1Y=Z0<GFX?OX6rTz;NHL1|GzelcQ13gj3V1~uNxGK)(QBa5lv4u(P+
zXoNveAvnLNs8UbCIUn4=FV4@;(^J=kTMjb?L?@*xq-9p5rhs~<@Zn$Zps1dLV@`2C
zxTy+~gbo;h246saEY1N9cNQzepqN^e2x|ZP<(GJZdJUj~o75E7qN4mF4fTM;l8j=Q
z?$V-^G}J*wP_Gqk7s61Guoa3GpvHEIh7vfGGK(R;289!N&<8pqXBCsDqyvd89KJ3}
zP0U6y78DjB431&FlA^@CVn_#E!B!!!Ko2~gssTzE@u0zLOj9${6jBvzZ55RAvz1UC
z3^EmjAr4Q))QXT)0(E3E^9oAAW9Fc7<xEIt2i}oO%P-1JEYVX4DXLUROi#?r(^JB3
zV5)+xLUCoWUTQ^hd}dx+z6PeU)QaTPf)ebGRZswjEZDQrnXwAC3TZj{i6w{-h|Y|~
zG!f(z(8xw6D4vTzy;zN$)I5YHO-<~!!NXO-7Hq9vaY0UIiH5okjzkOc94MeMVz8vB
z64M5d=?b<A;BeJY3dl)KEKXHO%_{-*^fFQvN+4bG)Vvhk)U>qJ<dXa%Q1>+xG$ac0
z2Pm~y=vC-c>Q(Ag=~aQosd5ubtQ61!zB~go6cJKXnu_WIP=`h#5j5bDlL{V;Qz*?V
z$pnq>XM#sG3KEMzvpC85MUbIG6hA3rOOBXg+K40?ZJ=kMqW~s!4fG7rB1sw2zs>{?
zm4G`xpw0q#x~C)`)aL?^E#&Jc<R(@orGmPB(7Bz=5(UtpP7#Xhzz$IW4P6&h>cNUI
z4PVfZENa?N1`Xyw2gkwr6g*N58i1)RNL9$pLopT7wX{-5Oi6)^qktwIbU*_fNr_20
zl?p`#m7q9K$S(rT?|_G&V1pKjVN_5gmq5qk(M&H&%}s=jAcNu-Hl~MU9*Pc7qK4#M
zrO+VLRD}@d04pUO1ub}~0IngxW9&t#$*GxTse0+DC63_n+7Rb}fc)ZA4NXlw@N|fV
zi6&~ufDDIWEwDkLnG3yq-^^&USma^|u3tw%s~|H^uc)9>FE_EID6=9YKd7J*JSMM)
zthXq?1ROT_Y=(LZJnjRoITX-yK`Lapq#!ddKRG!gGapj2BFbl&+o2|Tq!y*B7b}#5
zrd&WnVbGz?%wh#-DCbn_DY)hpmlmah(?ND#emP{Q6ciIk?#L`wfK)<gxeR0<)Gkj*
z(U_K>lapVbnU}5u(V<`rjdsZRT7^!fP8B>q>wr7l(4it@@LXGwPJvFPLSkNePHHil
zWtnN<${8Fj@DPXxO-G?-aj1hLp%bPc1<--HN^l?`*Cs{ahC*I`IdZ{`Y!PUlNDo%G
zBb6oKYy~RR!Sh#%C5hlf0(P61zo%bFyr)YnJl%p)2-5HgWFQ2RM!*9)I*^GkP<qip
zlpTedn&AF_2^#}L1ZaW200RSTeLblE-^S0t(8|xikigHtAkNRg@P&_oVHb4XKbnt$
zL4=Qi;Uq5u!+Kr@h8|u9hE!e#1`A#W1{Pihh66kd45d5_3?V!W3~D?K43D@O7`AXT
zFihZPU<l=AU{K~}VED+zz_5smfx&`{fq|2Yf#E82o__)-14AGu1A{sz1A`iLuAh&S
zf#Dwq1H%mt28I<J3=EkZ3=Cl$3=Do83=Aq93=B`%85mBmGcYV;XJE)+XJCkAhxmyb
zD!z}MfuV<;fgy&SfkB#`f#C=n149-Y149xU1A`wM1A`VD1H*67{5~rK!*W&zhFn$#
z276Wph94{p3<p^l80NDuFa)wNFo>}*Fx+NlU^vOlz%Z4WfuWq4fgzHafq|2mf#C!b
z1H&3728O9j3=Hi|3=FwU3=A<$3=Gjs3=F|c3=CdO3=9rT3=Dis3=H=e85rgwuL%I9
zbm;sJC~uUd=Hw@X#K1$0sgR7Nqfnd=nxF<1mH9;q;EC;21tW#blGI%A2oOkT7I-ur
z)M|lDf<oG?sg<DO9@NB01Xp_CgbEt-1&t=<=9ht|^ug%~+*T_A&Dazxz{V02ixo=B
z^A!q;GK;}!O95<2u?}=@0W>89)|;MM5(s91X7#`fkQbos>_WHzk{~!iBV;h!e-H%<
zwh9PEAnk?7?X*HfAqyHgOiW3H3^sxWgA>7(3TQopLUCylXv`cmQCJ9SN|#hBB&LCy
z1&KL1aJMNd<fMY;{lLpEz@za=`6U?&`30c%0c1EaJ+%Zp;a>=y#{(-(RIo^aG@8K6
z9pL7H91FsQdY~!0yc7)sJp+VuK{6nWUsf41j+h2o4gy{lQj(!im0FZv44G6c&qV7s
zV1^=!9gu<n8n^KU`NazQ3Pz@;nhIK=hJ=wR9_Nv+71RVNEmp`#%z@0=r6(4EqQyYZ
zz{oULK?BsYDNlvW`KRP76lW@w=4F-?gO>;(Er9^JAA~`sr7A$pFip-ytfv6Uf-ppd
zzLBXZ$Qo#l$}CYx$;?g71I_>Cq!uHsm;q@4VQ?}-4iZ~PkqH$7Rq^rAIt>(J;MO9R
zHa5t1Xgd#-4MBD(BvmS8mVnI1NE6B+Q9Y;?kePE(Y@k{VvJO)xcs$+;GLj8S5}@@#
zh#~=L=?chmnQ0Kufa@Blb*NPuC_8~2gS}l7h~!n|$_ra930gV;DhI$x1=;}s6$c=P
zfI7iekTxmQ83<p4`_7;^0~J!B^-iG9abiiX4!GBy30kBBnkNUZ6acLs0nJ#0LjfE(
zi8;mjpxLBUc$o!igh19CfR>yTr-G(BOHy+xA!Vp~3cU9TS~QW7Sqv%5K(16yt;j4v
z@-avZ5;&l_6xh-XJ#bJZgKbw(K-JBFSYU+HETnZCdc_5)sVQC>WvMyw$+;<@_Pn8v
zf}x&)f<9;!3Tl3YDuR@M;8H-t450$8J7#EL0F45WCm;<M^<wCfEkgqXP`f2HC97B=
ztu!YG)M^JC2yQij9G;n%4(*bGVzsy=zrX{$gr^7`xjG7<DWB3@9mp^Q)FN11gH(fS
z8SumuIG9Qc^79lRjb>2f279`D`h|cZ6y%~}Xpsi;0XRHBbCKZLZ-o-bG&*Q?8n{FT
zMFwbA5!_^iuNhHDEGS3>&41@qLTWzHloc$Cq(O@A;>=v=M7u&sKEzZ|<psrHM}pi9
z^(B}O@(-8+3PF&~dY}$wd@5*|1G>%!Nd&GE(yf7{bx=~w$;<`?c5-G~X0na~xXWJx
z>TV_~B&C*sDxrd$#N<>Y?GOucGIKLa5Qz(HKWO<%VTyvSf`Oi?f)>dC#gG&f4;iIF
zG6a(0G8HlsQy@-(<g&yPP^$s9CIvKI3)h2W1IP+PJ#zw9WTs?;Cuegip*a;)dqVvL
zor#55oda4WmY1TCR9aM&nVJIb)D<VDrGl#Ayc9?aM**~2Beg8Eq!MIiaef|T=>sU}
zm7$$f#3Bpm8o!c^)Ley(%=8R!vQ17bfTTLmJU?VD1t@bVK!U~|?oUYkfC?kTDm>82
zJB4J(WOPm?*vwQ!7Z@HgM6I9#IT?h(y1+$6X1-o=YDqB2HNoJ~G!1Zh3{LCNm_y`a
zXekR;fvuE<8;z_TmK>4A;2usaE>0~f!Ava*Hn!lR22>M+txHT!PR)U?5mT@OH3{Hm
zfIWjE4_WA@hb#tH4GZGrjMU_8$g&ym7#5O(@>JN0%Hl-Gf)-HfDNIR0>f$S87K3Mq
zp=+%aG!Xe55njl?Lv{?rcjn-vLCAN|b+^zl+T@(ff&x%6m06+#>b!y+nwD7s5kc|-
zC_O@xJE-OYuS3gCRY)#Mtg2KfE-6h*gDVCHAGvE06_h=yP)&mM0~Kr)%-}N0p!yB8
z_!FE9V2f}dD;H9W6jJh2!J!Esk;%x+%Z9GAEG|vXP)I_~aiBC&oSBylTFL-v)|BRD
z=9QFYmSmQtg2uVQqc)&k2V`{{Y&sfVxPp?YGGt{ixXS>KR(P5QnNSQ~+6<{hK;^nY
zNd}~c0V&5phJb@EFCJ_NXyGEVhe7s(%6f345!8Z!&hLYYUC_#d;u6pruKc7j&|0gU
zN~EC5OaqP8gBPQM;uX9)D<?M*QZaz6g(cw<Py->eSiu5RWFxsf4^*rsg3>IwhXG0p
zkd=Eml?tHYbtJn=kyh`ZEFVQ$jSR99>{Dogp<t^J6yoCQ>F)^|zD!F@PSsE{G&0vS
z&@<38L>s+8=!4X-pg~iR<{-!*6%G?X^ZzqB85mqa8~g<r7-0M3SMoD3`13O`xbrhG
z=<_o$aPu=T+=2H0JNOtF{P-9c6!;hzZt^lPEaGKgXy9dFFyduk_|C(?u!o0%VHI>;
zKm-p1gB1@0gANY^gD4LJ!(Y%Ee{KebP23C&UEB-|5!?(6`rHf*dfW^QYTOJAGTaOd
zLfi}tzquF~c5pE;#Bo8~Cd0+RaE6nCVG}0<!xGLxHlZ}?oY4>%4S~@R7!85Z5Eu=C
z(GZ|=2!L9J(1A=)Q&&L&v_1p0tQ^DxPZWUHu7i}}mI1e5b5j%Ziou<koE*dqD`d+;
zKB&i?0-Lx4O`(AdM6wsvCS>!7R+wK38pbOq%7;wt;<6Y#x(Zo#UXqcTr;rBPh?J_E
z0^Q({oC+Bb0k`=fBOHk(xuE_Ics4RGA2z`a3M1&CEqquI)fdH}1+}2%*Pw0(Xw(wY
z;{cBbf!ppNu@o>1WgG}Pr2$%=i<k)qx4^;kc8Go{LX6nCJV=)V5j)UX9@u<4*e##{
zCuz<PX@&&sBG7m*VnPMPgHLZGDFs;rl7nc0+e~13VRV86)CV1%;80Ld7|s(M;EWC)
z2S*$DhLlL)@^SD@a)5R!Wq=1_D?yvS!SnwUnHd=7f)3adU|@iq-?xgNfuWe6fkB3!
zf#C^s@4qP@1H%hm28J`d3=Gx03=Fco3=AiD7#NoFFfdf}Fff?&Ffg$2FfbhAW?<;x
zW?-=7W?-=3W?*3CW?(o4o$JryVqh@gVqg&FVqg&BVqoCnVqjq6Vqo|M-4n18x+lPa
zlYzm4lYv2%lY!v}bl!gg2LppKXpaCp1H(#o28Jkh1_sdj|D$XS3=7y87>d{!7=qXs
z7<fT@0@xTBp0YA9>|<qMXkukxXk=wzNM&VU@MdLTFlA+65NBlo&+UWu^Mm|8hlPP*
zHVXqo3JU`RCkq3^OJ)X!nam6fGng3|I+z(4Y?v7sq?j2P{xUHz>|<hJn8L)s(8$EV
zP|3u=5W>X3V8q10aEFnBVHqO>!!$+)hBih9hB`(DhFat~evEbjsDLF0Lnd;Rax;rT
zYspel6H{_B^HLR(DwT8;K((ZTE@;9J-DKi)fp$wH>MxKPu<65;5(OKD6p#oq#xxaK
z7R1I>k&~GRlE8)wiZaoKvFZo=mUxxO;ezQys5z(>K?PwHsw61PVKOipQw6qI!L%I|
zzA!Uz(v%nuT0(?wFuHk!<gq6)?4AMH2T3fMX=$hv%^+*gt;S(ANGU2NQlnl<3BCl3
z;U;j7u?4M#1LYggCMtMS2V{S09%v#Aw3|#1M4(L!gH$A8EHBPXQveIP<s_z~27od|
zLLoCvAtgUA6){l^G6;mhjV;*L`RMG_%2>#PT#yiG8*U|dPd!)y#hIXa)9lnr&;T{6
zQgr)s^3y?WW$=b~6cbQ5aHl{v^e2`if~|zDVgaoVEiQ>K$&Ux09Rb?#50*y_c#t8=
zpb>tBywq~g8r&kp=sQRbgu(trv!o!uASX2qeGVR^8nnI#v|LFcKPM%%7>kj}!H`&z
zisnZ2u+Yd%Nlnbj0j=AE%x{3=1hN_>KQB2IvRe_cJrBG93p9nA4O;UE+Ulnds!YMt
zEuh)yq*R4O(Eiob6x48lwCSxB%8N2fK)bgUVAEB_#id1{nFml$4s?_Yc!?rtDnY$i
zAt^OA57Z6F%`8rZbU{D?0K&?!eTSf6Qz**M$pM*B4nBwjB@*FjPzSu67cDR#Co~|^
z6s(5=oiqX+)1Z+FT{;M|DIT<b5Zz<Npv|+1C7@0vC_$^2KsK6#mKUcc=YUQ(0ng9o
z=P4xTfEMs7fX+<;rT)tNQqYtdXwD3B*Z@c!it8bvrU0GI0c|c%ha3(8>g6IiQX#t>
zG_?oXU;qj_P;{Y~Qw++|3bx=4mgrd+lw6ROuY(gKXnq|r#|v%Uffhg%mF8h<@PV6p
znZ+RQrz(_XCW3PpJm|qr0Uyf)iew~bs~4l~jRkoNtPK=ApwnKGGcr@lQb9)nsY7-?
zKo>577Q4VsJ^>jDS!xYhzJYKyq$3Mj+mZ?16av~3UYe(nSOQv6;!y>W*45PoJFln|
zvL+j)j|&b9P*DQP+zPhv<2=ARAjuH4wlGrx){Rfe2d(7*tp(PD%=m-XohN0cD`X}k
zc3(ha73A(r&><d~Rp52Ypgt2c^C71c!~!qK>}+`|=p+{16hzumD9)@(MP9w50NLmO
zcLF4Zfr2p)95c`;058r*HXeG;jY4rrei3BJIo$J!kkupbOrV3K*oG?rmxZvh5^a<q
zu?*>y6tF@pi^So+g%&Htpw$!L<qV*)5bzO0;9!HqBPcdtSXlu!Q~+5un3<=LoLCG#
z@d>=n1hnJ=G#Q_#Pz>2M1dmUU326BNl&|!_zJio4@u0wl<_M5-5LQ;m%*{<r$xJLs
z1(l=)`30aQHmQ&e&XA4kkn>xhE>|c^%qdMpOf!Sb0H-yuJ)klM94HF55WPtCEy!`u
zDh^!hKs00~BW-E{Ex-fqjmig2%z{eAOwch=NJm_O^A4(mOTi0qzzH%BoFvg*h?rA{
z<YUk-257uw=0Uf9X@K$qWI01-o&w~~GVoM<WkD*qXizT!4X}Yi2(-8jX>Fr2_{bT^
zLO5_%1095eSUHob0P-?uCf*Qq+6ZW)6)2~JN;*)X3o1j4!DlnT;}JD56>Jr%YvFRr
z3ZO0Sup%`Vv>XIlrh*q5gQm<u2WWwp(iMSEQ^?DQtxzh?&nX41f<-b6wm2Za7<3i@
zXqiS5=$x0ty!6x*(CSzP*m?u7jm4>XpcBWC*X<-0gQ_^t(KHCBAkwrl$cNAhB_mZK
zCp9k}bPf>cL<dlGB^E&|1jx!G@a#Kc4_gK}<e)_+D3L)*9xH{k%yiI(1kfon;MzN1
z0iI?-r<Xy}0O~pcXvv|it&o`u*&+kZeV~mqkW-34v;9TT`W<pqM}B#pLUBn^X>tju
zVgmKMK@Ep=WY2-30+ejPfs+V2ga^Dt2b9b~-h(s<N)*7GTaW@B<VaAeP6b;Et43fa
zEP!eeuz}!R7bO{~;6uv_it@|A=V~ZGcOHS8e|q5l{|_z(h8Awf`G2tU`JeMMFkIzl
zU;vE+wD2=9#PBmPX!0{KNbxf;oa19)*uuxa(8UKi`!A4>fx(xLfx(`SfkB^-f#D-B
z1H)Nf28Nlu3=E093=FEg3=AB+3=AiE7#J4vFfhdNFfb@X*8+gf{{z`A%+0{y&dtCe
z#m&HQkBfm}E*Aqs1{VW^F&6`aC>I06A5I2_Q=AM83pg1V$~hSrk~kR{Bsduu&T=p?
zEaPBc=;mNxFymlg_`uG<a38u4pp2b?A)1|mL4X}{zQK7m28K;+3=9=)3=BSO3=FSX
z85pLqGBD_~GB7+~VPH7K!oaYYg@K`&g@GZNg@M72g@NHMGXuk3W(J1I%nS_0%nS@}
z%nS@F%nS_om>3vlK-c%%GchnQF)=XQU}Rv}z{tR`h>?LI0ov~er8^h~=S%R)4p<cg
zS>6FXJ_*MP4VZ51qpn~xp~Fmipz~Tl$3h?#nK0d;auL$PO;kwA$xqG(m5s^B3*TWX
zQA|wE2b~#FqM>AHX{2XpW}#=OXJ}xgq@!SFY;0<T+7`@A17+c2*m`EvrZ(7Au-`ND
z6(HV5UDbkOKB~b8y#!ab5M>I%(8pm)Y7X+MJj7B`EItK|nu7-D^HNJe2Pc7Bje6j%
zI0&zSy8WP@GJH7^+Eyd5w-L6$7x%%Jup$=;kU|S|=nSZ)fJPg*xswT7I)tckl@*Fp
zOB4!q3R83nQ&Mz5$tSf4w9Z@yHcbMljSBLM5h7)&IiR8?r8GCU5~G3yyA@<k0mz`l
z;u4T);1#M!XXv3S$_FjuGSEYp2d(`BZ@~qxL`GiP3>i!YFMQ1_(8DE-V_mee0=VJ_
zO&MXU0Klh073b%G&ya(!A&-Ym7J=%hq@2`Tq!5Q@m*RW{@X~bf9*~?=(3z9q?m$wi
z9(dh)T4n{fA*!gTs0iA+13B#m-eN8-$jM9w9pnPqvkThlo0*@dsEBDzMt(V{TcZG4
z$y)|Gf)d=B0mVmVURi2!NoG385OA9yQvo#Xl@D6#2da-j98gCUd1)~u6u|DxELMOo
zj01HJz;%s6BIJxF&{<`vDW!QSiFwJD3gvo5dL^KieX0UH$%1PUg?#Y78n~-LL4zFE
zp!^P6IS)UD2&sI6Wdc~*0UtjJO2VL4JJ=AoU1-?@WQ%fHW|dw_W^qAIVx>kQ+P)gF
zWKMo!N?2xUd1{d_=wu@#o8kIF%~FUGEV2r=3ht=|sU;=(VbJ~esYM##j0`#gF((In
zq#WebBpuKJbESzn5FR3*gDnD$9O)J3q^1^V7^6w0L(JDp&MyM(h%72gEz$#bS>uaS
zK^$~fDHN6FL9NQHN-cuy*aWTNR?q+i8mM&%D%=xGa#1Y;o%)9oM<9oRFupSZ2%juK
z^)moq^$3<tks#NCk~buXA?v5%$`HN<ryFP_f!Uw~wNPRTEDF&HF$+4(0qT7y*dm>B
zpsWDb3tmGE(E~bAH#G;c<1`O+KpuvhL1zQFCFUS)%u-eWrA3HNcr<|Bfs*#23#y@F
zs0xXy48TUBW*S)4fhvk>HmIiuRR@*;jd!4(018q9?fWT%V?qJkJTK13FU?5-l{KJ4
zTcK--3yM;~%WW0FM<+t69+)QZ8JuZFsi`SNpuKqcMWC$!$*G_S12vUFr5LENgS3=k
zjdswE2~eXLG#U#!CN3uj9F(AQI6=qp<`yKDWG3aLLVB9e8XoKsWyE0}kdyXOz++)W
zpg|V!z+G`FX!m4pKIkCcf`X!a1+-xq$RHrd?o!Y(ehdnrfgX$_%fS00u^m1}*fa%j
zO#$v&Lf2_SHb<l-7J~*b6+mN@;G!PX_X8cW0q%P!fX=VgW6%XxFQCyA__7L6DFAXV
zcsoO0YPl}R-3n=-{qrf{{ZEi?9BAwVzE~MlrohIAzzH2>2q;WIv4C_gDKuokgD0Rv
z4w5sV15w4G8VOYHAPopID1eOuui;23$}a#74`t?o&H(@|rv|wkw2=!uN(4L66nx@n
zW_}(xWfhc`FzA9=kb%?6{L&)W34Nf0ClQ;vKz_+Df(&>RgDOkVpfh+GNh0{@g}ijo
zStOudEu;|)?VlrW{{#=N<R<2U7VW1N6zeE}lW9tB0Vo7Oy);mef};X7q>!%wJ^2~r
zCs2nC6jq=eOVA1(I{Z+~pbM&1!BuT>3FyQ;a5RHk3yBJl^D_%TW9bS-sVSx4BmW==
zq%wfc83henfcAVQrGk#MOU+9GAO8(rjSty)p9tB+2C_f11msTCh2@~pSFly!%mW&o
z1Em-EmUNKQL4jXfT$-Dj0t#o)U;^yiMX;Om^Ax~`AQb0=x6xvpU|0mI*P#b?LeBDK
zfaP*<2>~vQz>478=8*PRY53*mrRpevNKJ%3Xd@e}6;$Ntp%z<K(1Y!)6pB(aL5FFk
zlqQ4D=B!Kt9lHm%6+%E~i`*PNeXJBxGE)>lNxHZ+IT<_@s08Y}BcFz>qo9NsJyk+-
z52WLT;w`ArV8e<*DN9L5Arr*_MPw5|=Nl{7fTm>A&<<Nx&^1K(2hvAR1s(65npBz&
zIf@$;x1gv1mt_!-K}e()1vscuN)&X#lfQ_15Tp>|dJqd4gR*gEUTG@0|8K>>z_1F`
zUlm|r2!+m3uZ5ofKY^ctA%~xV!J40eL6V<=;V>Tq!+bsl23tM`h9|rX410MQ7}|Lm
z7}9td7^I=+0j}U-V5s3?V94TOU@+rhU=ZhFVEDn!z_5awfgzonfx(lTfx&>AfkBCz
zfq|Wyf#E9`1H%_C28K6WkiP#FE(V5LE(QiQE(V5koD2-0H32I*85lt40^~r?1(4)q
zU^vXdz;K9zfnf~?1H)<#28P8P3=A_k7#R9E7#Pes7#JR~Gca_pGcaVcGcY8xGcb5U
z#{o98F)+-At`$&WV_<l~%D`}dm4Ts>m4U&4m4QKtm4V?R3j;$B3j;$p3j;$J3j;$Z
z3j;$V3j;$g3j;$KbdA6%W(I~u%nS@^%nS_5%nS^-nHU&)m>3vrm>3xTGcquIXM~Ii
z9AJcm?@CY@L&KMJ3Ko5!@m<h4<O-l|i11Va>TrUS0cgP*w8aRj!XYOlx#uV51b{Xq
zgFB+oIvh0E4>A;b<`8I*7*+;AS~=iT>_OQiI~7#TDnN$sz&m~5Th2iRBe-V;8hlFu
zt-Q+vA6E<Br-{6CUJt3C0aBZu532RSJyginJV+Y>Q9PEUR)EH3K}8oR4}eY%L7c&@
z4m$3j1Uz$BS`0eVMh8;%L$Y{5YEcrXG*kfZ4+b?D!S+CW1*$Le(m_527Ydnq3Ylfe
zpbj4BYye2}0qP)dml0wPD4alE1-TsTU2wN5U!g2BwH#c>CFUjOR6>t@Me55z&M1M$
z5-1~s7bk+uNy{&S6y;zSfd^!gQqw@q5Kv76xw8Oz!UD+p+{C<6u!q418-dF1a?sjH
zXmO2jW@erOINU>n6iSi{V4JG)QXpP~m!)7=gJTIa?*>^=2_BCG@0(1l1T{Dy=k<fb
z4wTqHjSNuboeI6K0NkuVoc#iAvMOXkViLTD5`_Q<16qOvi-A%i(w1qEZ^3C9l&C?Y
zD4-#XbWn2#oO~hUD9{Fbp^idfih>rTY(<~<hYosy8t5g7phOHh?7~6;dVvM_W{JWS
z1;`jSWaz%Q1XSOG2Jm4g;D8+nauH@h3U*@=mVMlXDUfa4U<Js#5uuL8bSHSnW|l@_
zijG2Ya$-(mQG8BnVoH2sMP_k)Vsf$$sF9UkS`ZH^^yBmM;z0*cC*~o|GG`KTb`8W?
zkO30V?nVuWfR2K$AuI$zQ=ec*fCi=@97J`ZtN@<m0Hs~f><1(tfLidNxyn2R@R@1I
zs`Co;K-PegO@1~c`6Ypl2M0}UgIkJVpTegJAO{0wmMB0v6!3tDxDs>%NJ_MoSu9*!
z8NQhiJiY<W(Gabmz6flCBlyT6aK1~-11)(&(hiC849KuJD9=I84K_kQ=R~16BQ-BE
zFEbsqcp7{_Ge|M03N6adfsFY>b_s(DT+mkbz!Y#o1tm?ivKD+&y#n|cU*v;@A)yAE
zIYsglsF?{m3#(WmB^7kb3h2OUkj3y>4`}-v)XIZ4!5}*zH8N5mTQ;HVq@gqIpi}<A
zQ&5nt*6>ZO&=LgPz*7LN(|~p}^NSRcazLZSiJ(CrNV^$QB0yFnfR@cb&YK5MY!pM<
zq9vf|B2ZNUI-48hY*0++=fT!XfMN-`m6?ci_C2T(ub==fGQhC}2}&>%Tta|yAqYbk
zUzZo9<|SvK&cxy#j8Jj`pEC_9@jz#Cm*gXzcLq)%@P;w+SxuPP9%3&f8-de2II<zf
zK|lwnLC1x_PXu!VmGc@%bG(G|DkvGHr<R0+{RcLDG!>Dbir^uMGa(g1(laP&fiN_A
zfeUXWDa?cbRRJ!Uz=nf#gD`B28j?B+k&0>1@}<FfJW&d4wF36#`}n1)bU+c@5^%^u
z+Y_Kccu4sO3R)O0BtfmRLLTB=Q~2TtSYr}=d|M8<>jxdt0}Yg-t+)bN3u;Z^KFJ6)
zW{vA0Bb3AeGYa__BOD3{oMMEb3q9D-CSX7|gD`kG1EQe~T4|?HnpX;1N(^gh>%!*z
z!N(co<%3Ij$f}X@BG82@pf)as{g4?Nq<B-XRnUNPK}8~#6O=$JE<tS#s4CF3ktXb(
zrWC{lRv>4Curg>g1vEHV2^wxn1)ax_=?ZYGA9S(@cuf}MyaUjvIJ|C!RQ#a6LMrHN
zLQvTPKBxiIQUT40!Y)k#*$Be0Neb|21k|OdlYZbF4-S5Cdk~`<2uy+Y$)Lu8>;@}`
zx5trYH=yM|v=xC?_9qsDn*QMXJc<?IH5TYB2GG_j$Y>Af1~l;e7-%slES-Bohb<uu
zBhUbxV~8&UX#c-AGXq03bp5|Av}5{*pMl{8KLZ2k9Do{r1_lRy28RE93=F6E7#ODV
zF))-v?*d@wV_;ar%fL{|%fL{=%fJxL%fO(?%fRrThk@Z5=zc#Q28MFz{(n{;28LJM
z3=C7b85sPz85r!i85mf(85pi|F)-}nVqnPUVqgg5VqnnYVqg&DVqmz=$-uChlYzmD
zlY!wYv|n$}!N8!%!N9--y7P~nf#DE41H)W)1_mGKoqnKu3@))TFf3(bU}$DzVDMmL
zV322HV7ST3z%ZGWfgzlgfx(KEf#EL;1H)w&28Puvki8FJ|3m#gN{xoVXb4ao0-&M}
zhQVbWxM+lRn;>gIMwh;zFRejs>L9`mTBRsJ!VSxzE;tsgfMNh)3c=9FV+wT8+u7gG
z&(%4^)6ZQY#2-9602=ECSp&n$3Xm;u(18MV(EJ>D8IeLsc_wI4eM-54I`n2oq)RGC
zm)C$6&5tgx0qq-wC8g2jH6@9o%WFU@19C=}*Azfj9#eS52F6kq)Se7zwgj>yW^{Rt
z9&{u$Gas@&22?76>r&KS8+6%>9;zt3UkcU%>N-NzAx11g$EPOd=s_qA2&D%(6%{&D
zfHYZ-Xc&M7BB1mCpw;f6h2!Ac2S7Vl3P5WRK&Pfcmc)aGP(j^WgJSSieZ`<139w6O
zz^h?El1QfogN=rq*9V!BPyii=hnQSa2CbR_)$8DnCdhT5-Y95P8g^d{co~IrNl}ij
zGk9ble1|!x*#Vkv1}$9!UFrx98&EfQWUeqOC<1Mk0S5wjECzJEI@-07iMjcukV{cP
zdx)W%7@)&!pz$g2Rp8()8_4_Oz%vM-ktom}CeWdcskxwoO)`t27ce8wIY5?cfcpO(
z3=9l!Ky%Xq3=BcgW<RL!4_g0!lAnQLIzIzLH9rG`FFyl=JU;`&Z$1WwoqP-oYoTia
zn)nzPy!jXybom$<bodw;H24@8r1%&ZxcC?t4)8KCRPr(~nDa6)yyIbDSO#7H54tPx
z4>tqD8g2%Lc5ViSHt70)S#Aaf(ENWR7XyO}7X!mT=(_)toD2-RIT;w%axySXgsugs
z=VV~W=44>dgWe7Bi-UpTCkJG#;1LG{!#NHHh7t}21}_c<1{MwmhMnvT4871h0F>An
z806R)7+BaD7&fpmFx0UzF!-`DFbE)>8?cm>fuWw2fgypFfx(iMfkBX!fnf`DO#o<b
zz#nD?hKtM$40X&53=zx>4B^ZS3{ROD7&@647}S`cVG0e;0j5AD7aT+OmVr(U2TgW@
zb24OjED<yWn4hPRk_ldk1iB{@v<C$x`-9H9Ohi@z8ZykyPXS%xoLN$-UaU}@m<`@-
zmYfJW0TVp40y=5}RBC{x4M97%z*BvoVLZ@u5P1I}I5U7;s0^E)1y4{z&ddiDQi+gK
zNh1?{R}$FKi3&NHdD*Ec-~;y+!1vsO=c+S6cYVSyyhqt-1s?T8a(*W0%HA}{Ml<;E
zAo#9T&{!a-T+7Wb2KQgeQuDyuS0OWau<cnO&w~%>0XY$*9C9QzqKH=p9Wa`cqfnfh
z3Yr4T1FZpvt{p%rb~DpJP6uz*0gVkNgU&5YNiBk{G6NY8&esL0$>7a@AOUdD<`*dx
zmt=ru(K2%qiy%{IkZ^?DrVN@L1WkK`90xw_NCV`foJy2n0mlhw7B?pcG-QYrK$W1q
zW_c;#nGDE@Owj2O(9%hyNP`DAY`0csF;a|x%#Sa~$uB8}ENTKdIzKlRGO7&ScZp<J
zqC#;l*v*-_sk)GjhYHX&h>&G&AP*`iD1+)AP?7=<;U}enCdpGkhrNS~LD;w_$m5{;
z2Q*g;x}F`fyBoArwHUfq6{Mn|D782>uS6fo6`)n}P$MDB+#%cSz;mpKECH_F$vx8-
z$+m)`RLIhZ6ot|v1(bDsa8+QtkXsUva8Q7(et>$?CqKVH0dz?dXl+~$h=-~lWi_6H
zEo3qnwmJ{46te6Vv@!*>(+rf_;8)%irRsu~%p)a3==CH>nlmAj!6+j>puNcX3dllm
z4PcLfmK%VlVL*EfL7PKBK1EwE2iAe5;eg^P(8_$c=XJoFl627aSVFeGgUUHgD`?#a
z9f|@481%YdNDY~p21;h2J(=){cO8YKM98+kTu^oaH+jGdSM&38iuJ&k+9&4dF(5`k
zz_Tde><UV2p!<L;b#oI_K-X}C9SUAftH%Idaf%uYI^d!blKx8a!M;#PElJj6@Prh^
zkm3lI)-y{KKzAg7R%_+yra%wO)`cbvT~O-RElLF~H%$R;@JP%}ElDjZX7Gd^1Ye$C
zlu`_8wSksbWuz)3<>zPXf(!)3KQse^JOEiW3|ol{bv*Q>xI_imQeGqtP!~W|2Iqh-
zMarpENK4F1&IDZ`Uz88p0Sexyt^sP@fbD}{vkYp&z``C}D?rW*zzA_o^yR6br34I^
z@d~+V8?>YVRHQ<0dQQ&B%t?W)y-kJf_AV-cTvHCoBgJ|Q&ZQ;bo0LI;08Vnc#h?N?
zEi)Muo+ZU#NocDqH!&|UJ+%mCqz+ccfsVUZC`m2K1#Rd6bu$=T%TkM=S4n3UD})BY
zFAdOBKsA8@cCsCKg*~js0Ts)TrEn?vpzXrRB{`LlBm}B|A^VO%=jDTjnqbFVgDNqw
zk5WM!en2aF!J!N~%saod7*w@@Hgsp^=Yj612iG;A+p}OD4d`kwg^bJ;aEntRF)uG4
za+Z6VE@;vmyv_=-Si~{F6Lj!FVQD7l#!PUA)`e6W;37XGRRLx(=*A965>?1cO$2!)
zzbHL1FS9B&1?<4gJO$9wu4K>&7Ku3uIr+)ph4D}$lkzh`Ys4X|tHIl2a3o0RG8)iI
z=$!JzN>F*1ssO4g!8gAbfR;^yDnUqf3XN6pRes<`Kwe5tsue>BXg)0sbR!@rsV8RU
zfp6Rc2PISo*u{uz=s|f=0V<`(0J^Fk6a(OGXwdYNSXz>iU!)GIq%w2B2Rwpqi%iSR
zOU!}YdY_z;pP8Hr*@glxPCy$Yl0j?3@>4+j!5~fsZ#~Y;1+{bv3i69Vc?-0fub3eO
zauXtG86U(9klR7kWNKb|W?pJ)QL!EaxRaO)iujxyNG!qZfLH;(KoHb)$wWHdyr3u_
zw3t#49DpF>L5(T+otB`JC{hz4Q3!6dfhRLTW`K@!$t(huiYcj}tx&KQ83X8|s+>wt
ziYv%Ttx!lzDlRDkZFU7ENl+fn0B?W?8v%*wVo>3f47!{Yw81>TxEQoIM2`U!#xMoN
znV{_ypt7X^Ca>U`2hPWkT(1ME{R$xWL4X#VDu54n$;?Y<fUTLzhinWlg6AAP1yC$O
zZ=8ed<^U}x%S+BI$N_Js2Pc}cR8WkAmcW9xvLjMOVorK~QD#X-E@-V+L1F>u%F2AO
zw?OF;<fjr){2-KpLm1NL#IOvUjxviCAotr>g5sbAt(OT}P6)24Q&Uh27hPS3^whl6
zqQo2p(CL`PkRlVj_6@WjDiz#6C<ZN{$}cK`G@lv3vZY1wkbsScCMLbolFS_N<Ue>6
z52htOwFENC1YOOKIOU}vF*zHwQ3BK@VaNnuQxCppCNT%b1D&UV*EVp`oLLwTovMe~
z3GV&EHz7hy05?Z6!ArqFJ^>w=kF+ltNeyiN1?nP@Li9-(B-K#Q!)*?6afQ}H@Lk>v
z%21=Q?Eiz91RqaA+z$vd5MxgwENnnA3S;M$<`z_f5_nz#ObBiyNVp&~4?O?Bo}GbV
zHV0(?zX`OX`k$YH;XCN;KYj*=3;YZW+xZz7*7Gwkbn!DV#PKsQnDR3)uz}?H7#MEz
zF)&<)t_?WA$H36X$G{NB$G{-V$G~ummw_RNmw_RLmw_RGmw`ctmx19g4+Fz(9tMV$
zJPZu^JPZtuJPZuKxEUA@b2Bh3<Yr(9=4N1!;AUXB%f-NOnu~#9Iu`>&B^LujG#3K{
zHy7j_fOVV<4AVIo7&<u_81gw87~(h?7-BgY7(zK27(6%`80<J17<f1t818UD{l+i~
zMnhmU1V%$(Gz3ONU^E1VKnQ>ax4>;!=<pKw@Wt{}(9T}aHgWK<>tMV6))#acI=BN0
z8eRZ1K;D3knIcYHM-l{&eIaB(J!aGyS%?DADs#AbAnl;dQP>V`AG}M<@vkt)D+`_Z
z8odEm0Ww27dIK(GfEsqaX9;+E8>v`8np^>879v(~!%ja1_XwfAJy5?DvX5s(+<*%j
zDFU5{mk8b|2iX%k)JkB){o|l`2Vr=r2`)&%%Nn59Z7SG84zhx)04WEn0w;n@j0-!#
zCs!gCVjyqi0#8=JyVppo`9WqvkNX8{hulbM03Hv72q5m*1kG%q-4P0Mjt<0`XgdK)
h3&4YXnR(!~0O0Fmp<}V2ac=|$`wrDqun@S00szmAEI0rF

literal 0
HcmV?d00001

diff --git a/python/ur_simple_control/__pycache__/managers.cpython-310.pyc b/python/ur_simple_control/__pycache__/managers.cpython-310.pyc
index ccb51da838093a6f4a35c6013d4b2b91b781502e..096da1369580f1e89df4205b2cd3de66a679d0b5 100644
GIT binary patch
delta 2649
zcmexj)N9I@&&$ijz`(#@cqBSiFJdB}4CA7S+GP{hUSyP;T+b-Wmckm$pvg9Q4Wk}o
z&E(sR)xs$Z!3>(relNc;GBCVkWME*><eOZ{)Hhj$*_=^!atw36vJ(RXL$MSC0|N&m
z8#5at8zUPN4<nl>2MZ4)ACn9-(_b#G$sd^ICi}BW3hZNGU?^dzVaQ@^W^`eQ4WC@W
znyF~V$N;jXglPfOLWWxQ8ul6{35HsZ8ip*68jfs+qL4{!DzYFInoNFGLb;j6#i=O@
zDXEDmIhlE>3Q3ik+>?*68HwLwEhx&&E4js5oRgYbpvhSz$-ux+Br{o+-MU_yfq~%`
zb8247EtZtbw6r2wkQA7Z1IrYZ=2e5dp~S$zz{kkJD8kIa!p9`W#KA1W$imFV$ic|L
z$i*nY$iyhb#PYOC%sD@=q$oefCqKWyH!&|UJ+(-$s5H+{ll2xye0*MFZfbn|Ew1?Z
z-29Z%oYeUE$uHUYgXKYXYJmu_#RvlAgd$D`1_nov6F5L2$-u$LWXQt-7EK1l7cypI
zU|?_nDNUJN&!|4RnnOd1lOcsaMWBTtii?vWMKDFEg&~SNML3v2Q)Kc=4vqQ=jG$m&
zz)-@F#aP3T#k7!#k)ejMh9RE0hB1q!h9RD%hB1q^h9RD{hB1q+h9RE4grkNri?f+A
z1|-W>!w}D1!w}C?!w}C45--v&;Y(p`W^4kP#$IGv!;r;NY+AzK%uvG+&jvS(A&aHR
zr9@ysy<iPP7S}?Mg+h6zAnD?)65$l4W=2Ma5|I?<W{`|%i5Qe6ULpZyNrIeHlv%@&
z#kN2yg*k-<>~IiIdLaWNLmq#LND3=lH-rV!4Pi;9fV?7;!d73y5HFj;Uc(SCmjX&w
za?&7o%GWSvDWq^_GZsw(v$(QZil=xnG%<pBH4O0zHH=w`DcsqNMT<ZpDIobJP<h1~
zhImC#*cET8VaSqPpafF8kP#HBS;{pG@yaQjAQ2IeJt`@ZJaCR`4P%yC4MV&dI6d=D
z_U9@V5q9?X^K*3$@$_?72=P}44KhttoP3Z=p6Mmq<VRdajOvrwnPo+b_(3U^uOKro
zKRG!gGe15*FQ;;{Ja@M^DA5)%gJgM%N>WnelOc(7@@DSETHrhY(utPji$JMyCnymG
zFfcF(u&^;ruH%tn;$WIQgU4esAB&`w6*#McVwsVlgb@^3E)22!wag_<3z$=w7c$ne
zfMR_CYYj^ZOA6~kW=4iWjuN(J21bU-u`Jg0Mfwa34C%9Gg1}Du^gk;<Uh+*!u-9ZS
z0y|e2<Xon_f?KRrsYUt4MW90D7F%&aYHEttEtb@Z%o0sja0b4`R-BohSDJf^HLWNy
zH?>Fxq>;TWH77nfHwE2OCJYP=l8g)t#a!y3T*S!3$i*nc$i-MC5tN^lUjoaj#U=R#
z9*KD=IjKdH@AHJX8-g;W0jO+a1&4BxDM-KtM0kS;FOU=y#5t^Gi8-aI=oW*NsZFlp
zJubhJ#qZ^Rgx6=<7l94thM4I*Ie|}<(R^|lpN<hI;BT=e78NB{7J+j9N{9_$Hk$dj
zSkhBV0!=4h;d2pYWnf@%21VQo1_n?`%1~@H`82P4Jzovu0(MZufD$2$mB$P!MQa&L
zI2Ul$FfL@QWdvt?juP$#JT(kiyfqA2d^L=q$OEym_-h!l1SCO;R<J~9fp85&mf%9h
zV1}XyPymQPy#NZ4C=dZoR^}j9Gy_9DLs1NfDGDM$-YEk4uP7G8wFkwSU|N1rd16tD
zcV=E{ZemGha&ZwTi51y`B$@MlGmAVxELPu={QR6EF_1oS5D^C=Pz%dj9O<bgV0Gc3
z$Oa_|HWNlJMh-?E77-RPMlNO^Mm8ocMm9zvMgb-s#wt;i=!a=eW}N(xM`E%fe_lN}
zj<^sWyv3cKTH=^oQks|(m{J7y=q>K_qRfJV)S~#Z)SM!)SK+?A#Z;K035~)uEN*56
zMPW)6$iWf}3=C{w_p-6_u<|i-P39ArEC5b)aN~-cCvOxG*RunKJjB#nY~@9%dC3{*
zX@fI8wIm!YGk5Ygffe<jjQ8^Y|Ns9rdBAaSizPWHv!KWq<R-8kpuAQT1mb#v2tg15
zjv~&?ypq)PqQsI^NYsF$QIi>*?u$TPy2VzQf+gGCVk=I~O9@O_3-TM+W?7;6OpN-Q
z_lk5d+Q!4oE=mBg;1LdX7$k8df+Rpu4+&;V5EooTm`<J`X2R$^`KXv*G$<HuvHGUx
zC+28!KpX^eTu~B8bv-EEuz=)lF{a+)1To@sGK)*VWd(X@@u#GwW#*;2=O^X_<Y(rU
zoSIx9o>$LPqzy7j2Sn(C2vG4@1S(F7Kv}y8><>^yT$B!y%>ohmAfg#WAW|oo1+LHa
z7#J8ffwHLzs3c<GU}uWp;9%n5U}a+C;AE2I;AE2G;AD~pi*dv-3Gnb1fwX9f-{OQ9
z@kQWRVa+SeEvSTqm;%VF!f*w8WtqjLi8+~7sYOg6hk#=iR1*||N{AvBkeJKl<r3Cx
zMj(v}lQ&9;PyR2V<_oe4DP-WzzQth!X|vdYntH{cc;H|IHGX&)d6;+@K@iMi;$dWB
NVp8DaW=aqe1OR~wP0|1W

delta 2382
zcmeA*{bIzI&&$ijz`($8EigJ&HE<%I4CA_q+GXr)IlQ@iQG65UTw~Ojti-0ymcky)
zpvf_L8lxU#&*Y<w)ygG&3;0U-viNHlvIG_~F*2ku1T$zd`@Q_a$iVQDk%56hQ*d%H
zQ=jZDw&Kk6yu_SaY{8!Ho_-;>xQk2j3p^6@QgTv@CO>2{XH=Lh$(*ll&%nS?EW^OS
zz`@AI#Ky?R$i~FO$R^6c!o$eNB*V<~myJt=wMu&OL{{O+2bei0YjR7PnK3XhlrW?)
zHZw9ZlrW|+H8Z*}#0u0hmoP10PGMfiSj$qwkj0Y4x`3^QC50u0bs;k&Lm|iH8gA?Q
zmuw6S4C%9Gg1}Du^gk;<Uh+*!u-9b2#ad95nO9OI%)r2Kiz%<*7Hd^%QGRg|FNn`m
zRFaY!pPZjpQk0)_i><gIH8sUcleGxsZiq)qb8oSx6(#1T7RiG&u$QIg#3$#b6e%z;
zFhB@J5KB^;fq|h8<YiFkb1?ESaxsc9viz%(19{LCDTtCmVF`*M5awiHU;sJX8Km<M
z*!MLIS&U$RhfV&(oGEY1$dD&r!nA;CAww;D4SNlf1Vb%H4MP@34M#RZQSjs%7M1!M
zMv$fyCJBaYhN7$z&J^Yv#uS!jW=4iQwi3=1))K}PHW06dA&WbUr?{quVF7OnLka^d
zn!!=c?pGz0n^|0(nxc@BnwXN4nU|`NR9VE$z`#(&Ra}~!oLXG0keOG-omrAvlvt9P
zpI59{CGO_v>7!_+kdm3Ake6Sg09KcxkePRj3!zMtZ}MIiBc38vPyn(P=cJ|<Om^fH
z<tqY(nI<PVu#4m;v$0x7$bpnFr{<;HVoAwNOM?V3IH*CPe2clLG_MyFR5}a{41A0n
zj3Ue&EPPC2OdQN2j4aG7j2w&{j9iQYj7*F|Oni)dj66&ni~@`-|0nmeN-?rcUd5_v
zr38vDkj+IFAQsqg1Oc+92ox)JAZvI)$(MnHk;#mMlZky2n<8VtWF<C#wiNzg22Fv<
z-E1256^slFpzx1psA0@vtYL^}tYOSzs$qy{s$tAxu3?C0DPgT)%wlV1jA5x^h-a^1
zi07zbi07<fi01-{7b%x;gA#lbC=OYQbU`s$tXsm<%uvG+&kQ$<A&ardri6C^UkyVR
z`$CXC{CS2oj9K+e#c?G9pqx^}5HDCF1Z4@Ah(KAQAZHZC)-Yr-FAz&%N?`^?I+!QE
zkb#jQkH179g{2u}AV@ca1=0;+iKejDFvLrMNy!wp8ishO6!sd1EGcP_^Q3DS>$7B1
zII<aw+G-fHWKuY@S&BQn7@9y~AXCE-FH^&qC7Z&P%~&)IE<Xb*FI&S9FAEB@;$<}q
zS)vQ%Kx!8<GJ?WUzJ?)QK7|7$A_B5VA%z>xR;*#nQmSEyR|2P6MxM!@9L19_a>y|i
zsZ9RJVZ^Om#LvLMP-Mu!z%V(CvzwC{#M1!resC^!k^%9+3ee&llq^ny;@k(6CBX@a
zgOP(p=sz112O|#`2O|d$8>0Xt8{_1MTpp9-c!J$QDWu32R4}q4tSka)FA4_P5(JWB
z0;ld<tYwKgrKxDi8LUix@(-TlrkX5%FaIM7u$lG{gSo-pDDnp73a<3j6366{(!`uV
zh;<H=zwqi9se$BJ6N`!xD~r4kHiFq`_T6GhPb~@5n;gXFGC7ZjbF!5{UOm|TTu9b%
zBdke*+5@tIJH05gpdhs<zAQDTNEGBfki|tYAUUSO6itXfixNOGs3Cic6=Z8l03!oK
zu>?45xfnSZ*;sj4`53t-s|!vJ1w~a6+&GAD1>w#JaSm|HFG@}=a!2^?7F&5yYF=^%
zx(7MaQ%l0ZGR2emg;vz7FfcH@{Qv*|e@z~+1-Dp|b21BxLP4$s+X_k-MVcUPAcznI
z5gs5vac1U~q^1`omZTPe%4TqIYBGZ(6YR-bY=tRU>VaEq#i@BIfho-(-*Rm>6Q0k+
zsIvK*Xa}QrEZl6twEUv-#G(}M%)Hdx#FEV9Vu&4XAQkX%xW%0Bn^_b95(B4KClCu%
zgccc2o-J;|=so#@xSs+jP;RmMrspT-XmUWD2TBFESba<K^K*)-C)-F!sDf%P^q}HT
zNlnYlOLfmr%n8WP%qv+ixl$spo~y_bWQ`Swum%yJYynQEMPT<Qg2YllL?(!601=4j
z2D88=s0{-H!y!;^0_8Uj1`c+nKn_+WHV#fENe)gXX$}r14h{|`0Uq8WkV;MQTbu=%
zdHKoW$r+jXMc@cv%`43<s07Dak>2EfNjXt)T!C{lC_fajfb8~}d`QxoQF8J>Nhg1h
zNl3ngJLVRLO>TZlX-=vgsOBvOr3nruP({JR$iu|L2!dc9n9an*q`=3`R4gP202-<j
AqyPW_

diff --git a/python/ur_simple_control/clik/.clik_point_to_point.py.swp b/python/ur_simple_control/clik/.clik_point_to_point.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..50eb85a9a671af30316c6c890d7473e7b1ab7fcf
GIT binary patch
literal 28672
zcmYc?2=nw+u+TGNU|?VnU|`5Q5}kS}u!iBT7z0CkesMv5N@5X63Lnl*EY8l%!>0lw
zR|hjtKO;Xk)kq)C%_+@G(Jv@UtjH|ZFD;5M&de>yNsUj=&nqd)&(SZaEXl~v!y=KK
zlbNj#BH|13GxJK~OY$KMy@E=Jg`?zX2#kgRBm_!J(sV6&8H|k#4L}0QN{R}?LZKk$
zC>{-g(GVC7fzc2c4S~@R7!85Z5Eu=C(GVCRAy87l%23b1z`z9cFFTZGM5EcDd|N2Z
z38i7`SfG4ZkHQ7Yhsm=-`7rZmLHRIwW+>keO3#PVFnK6<lo}0z(GVC7fzc2c4S~@R
z7!85Z5Eu=C(GVC7fzc2c4S~@RAR`14Qy3Wd`5748`62WFu>ScAeg=lC{0s~y`572?
z@-r~Z=4W6ChN|=9XJGK>XJ7!)Ui=IUT>K0SkN6lE7V$AK1oAO3{NZI_Sj)@65Y5ZL
zV9m?G@Rx^yVIdC#LkbTAgB}k90|yTS0}BrW!xwG_hPm7f44&K!46NJ?3|qMv7&5sS
z7(}@k7!GhUFcfk!FtBhkFl^&sU?}2XV6fp}V0g#Qz_5*-fgy&SfkB0xfnh%z14Aqu
z1A{yp1H%he28K1P3=Em93=9UW3=H2{7#LQwFfdGJVPJ@2VPKGBVPM$H%)rpW%)sEn
z%)r3S%)r3K%)s!PiGkq|69Yp569a=N69WS)69WSa69dB@Mh1owMg|5Q=pckLbpV61
z0zLp;_K=fWRE$rVf-(bjoTJRZ0J5hjwWPEtPa&}=y%@{~iNSD5Myf(_W^QRtVo7Fx
zo<e16iH<^Xeo;}Sf|b64o<d}PsX}sMo<c@qS*k*QUQVTgp+ZSUYH_MUT25kmu|iR1
zdPa#tUVgc{Cfw+v#LVJU1;6|fPtaPA+|<01)D+jEqWmHab(hQ(g}nR{g|y7{(xOy_
z{L&JIjQnzil6-}N(h`ut3h70e1qG=^3YmEdF<=uj)4&ebgGnUjr9cD_uC@Xz1bI)v
zR-quVs5rGq4@|{_#56P^p#bx7N@`kSX-<i)x}lzdx{g9dYEFTzI>=Fp$t9(UIXRW_
zrNyZ!3W+7TP(Oi<D$+|#NdcKx3i6PKx~?uFGIfg!Qd3iOi<1*`GV{{abreb}3sP;<
za`F>PbfCTfS+1;*n^>8Ys!*9<s!*PoR|4`jNKY!*<51%iiZj#m5_7;l0%-zaxQkN~
za|=>a0*X^hQ}Q$O%2JDpQ`L18k~8u%lT(Xrqt&s?t7j!9=O<+*=7kg`<`oy@7pJPn
zB4P_<0XPhlic@n^lS>rJGcuDi6q0i?ld}~PbJFvRGD|XY;f_{<`3P62<YZ<e1r01P
zic5-M8kH47{9XJN5>ryZ{w_)_E<y1v#4#YRWagzSfC4BHoE%Crv-9#Z^2-#8QqxOw
z5{ojcz)4m~M*$W-hN-&75Rc(-TuEkjMt)v?nJ&n5nA0%g2^1nQ?BuH8>=@+g7V4uA
z?iu2t5aQt(tcPT!fu1SEMwkW|U6QX*oS&Ook^%BWQff(gYHFT>fdVM?8tNz*=$R@c
zRl*#Oltw_>Apww_SfEgppOjysUaSC4Ukc@!B^lr#gK&#;GE-8Epn7q57_3qkq7v1=
zkjw?L4he%I#!4YMBQ-f&p*$lIlp`~X6>?J(^NLaY3=L%@oggOGumU@{w5T|_D6^nM
zAtgUqp*XR!SfL~%vsfXsSV27}H8Dj2v^q>7F*!LkC$$J1sp@*D?t=sgEI6`2OTvm#
zH0Z$+N<n^cW=UpQszP#pUU5lcUWtxEZeoRkp`N)8IPjrqMZrMNL=S)1p|}TEo(B0F
z;<((zip<>7T!qA(oc!{{q?}ZR%o3<?6!J=QlTwQmG%`yRic0f}6%tDnObrY?sx*;8
z)zAP0Ac+Y_Z006b=)w&xhQ((l$dA~93skhHW#%R3C_n-wKTjbQR1Aaja$-@cLPlai
zK_xg@A-f}0*9fmG((@B@bioF}T!1wPK|-ZGBef(WwFqRTdQqxEX)!1}fE)%b&XFv3
zOUx-w#cwmDZcx`zNK6LRR<?;n>ENK!bIQ-pNlnc2F8~QA<~V|-VFeB-jzAd0MetI%
zEVH;YF(<Ps6&x4}>8S;&B_;Vf3Q46U3V!|}u2$em3%qm@RQji;fa*z-oDR2vc*kQY
z6p=y`?)I$G;u3|-ypq)PqQnxY2MRLt@{^M@GV@7tBV3nmeqK%`)f@^+X+@=Zd9aud
z4T3c|@{34vAELr1(`{+_MY)M3C8<U6$vKI|#faqNSd<Q~)?FY?gJO@=oB}tPVn~t%
zB_I&aPg5vMEXvF;Emp|R%u6ZOQ}9j9PE{xdHD^E-dv;!aIjrnVRRHA`P%)R3nwDP#
zsrida^Ay!}z>y11yG07Np!N)ytp{^R0JssOk&+52H#76|Y}JEG^AwVE@<DBroc#O(
z1!w~R<c!qZ#FEV9Vucunl+-kZ^wbhiL#0?l(~3b!Nr?dzn+l1=3W=aHu`D$&Gc~V7
zp(r&iwJ0?&IaL9?z*di;Bp+N<Lo5X4SB3J-oE(LuRE6@Q%#zH!bUlR-XrY~*T2P{p
zky@0hzyQh}Mpg<XpvnT|{k+uF6i~Y+IlrJ%w;-{&Bo$ohf-3t&NEjD`%?7Ds$ShVU
zC@szaHMVkc6cUT_OY;zI0)_lEu!EBGQ&JT)l2VfsON&8es(P_PNoGo=Ca9UJkdmLL
zUZRkkk(iebZbfAlDdZ*QrWWgg9cBn}SSl!Vixu*dvOtZS{G^h^%)HbTg|wplT!;+3
z$%qIyhWzq8P(v~?H?;)RR!=O3SJnA>Ak#t3MR1e2Bp+0wD<moumx5ZYIr(|%3d#Ao
zxruox#X1V$yonSP;94)OG!I-nfddBA?5)f%En+CoNX-Ko1<ocQ5ru-H{Pd#4Ts;NP
zG;ol?nv782CMu*9B^H-tCMzW7=9lJ`K*K8+T+J~!=H!4wuQ)L;73TJ01yEK5#T!@+
z*ica09%f)(C9I-?1br?z(SZ96;M~Dbl96AU4rzjbW2GctAuSUUWw{C(pu(~!wIC-k
zIaNVjR~=H6s>iE?n)11&IVG98<@rV6wmaCgVof~;G@qg5o#Iq@MoWZd&tip?d}yGi
z7o`>`Fn|+ed158ltDuG?G!PWP&dV%TNCwpkdJL{*sYR6vnYoGSnR(!<8N^D4`c?<r
zl>x^_acT)DrGs)TC}J}6K&d=4w;;c$M4==zH<baLYA}|u>*XfqC8mQC3RJB#WO=(!
zetv;3L`p{?2-KE?@*w7b6GDD|PO$=1TR~z`NoHaW1C$G^-C%rBLV~gLN^=V;LGhDU
z026{2-yq?F%sd8A|38VDfnghH0$+fE0XF}>o1cMUK0gCP6F&n(BR>N}13v>p4L<`z
z0zU&o06zl*FKBFlkAYz+9|OZeJ_d$hJ_ZI`J_ZI=J_d#lybKJtc^Md%@-i?~LB|0s
zco`VPc^MeK@Gvmk=3!u%$iu+k!o$Fzz{9}smz#m%1vdl3X>JCFQ``&;$3SBN+zbrs
zxfvKLxEUBqxEUC{xEUB!xEUC}aWOC~<zis)=VD-B<zisi%gMm7jFW+(f|G$EnUjIR
ziIaiBk&}VJmXm?OoRfjUkduKy6*`V^ii3gS7zYExA`S+IEDi<+I}Qd00S*R+v+N8E
z&Fl;e*3dD9Z)^+<57`(P=CCm^%w}U?n8n7x;K#<mV93V6AjQVO@Qam!;VLTw!!lL|
zh9p)7260vfhI1^CFwA9PV9;P;VE6(GQ)UK+D(F~)9y0^OMJ5J@Rwf1pJ0=DO5hgMQ
zYv`l}JU#)cKH{NiR<EcuPeT(jZUGYqD*_WR5m4&JrV=EsV5@+c-Zj8&bsdFdDj5UL
zP}DRl2b4K=z%_u5LUpZ<0(Qf(i=u@WMktV{3sP24qBpgO9NpyTL6mIJn8s%aJ~@S)
zd{k*vUV3T~dFc+EU@#)Nx)vIB$_nMFpbiPBB8PNJ67wn*Kn)g!l*E!mh5RCgoc#3k
z%)E5a2ob261nRV;<`$Gxg3=7627}}p=)esqJwb+_KxMcFlBjb|X0{HbFRud{SILHL
zf67TMf;tDh(+R32Cl%BrOiwKV>4WC1oYW!>&{!5!xi7d~X$vt^FC{fCGcVOWKQSi&
z)G>zTW^fKvutlkOkPQKur(lcJT4YepOjC%D2Q{GL;}vXe6_n!Ra}zW3;^UR96c`}G
zHdYEnsk!;!vA|?Vdlp=27iVOaD3s?HWfv=GfSQ51;9<d}RA|ixYWip9CFd8VCYR(?
zYBE3u<O+&1^GY<7eDljveLyX_R4YgWwJ06jE=^3%$V@FuP0>*(N=-}#wGvZOi^2Un
z5TU39X_telWxe8@)YJkE13d#nh#;z4bzq|nptd4t;4EDsF|QIlG6QagV=3f7Eq!ER
z{AC>Ov<5PVI!O&=5;TF3o6^8W<YXczKS=yzO=uw1;3T96OKqU`hh9o*Y5|C$0ZM|9
zJ$aBYPb@A@1vQf@3sRBgK<TI;GfxlRI*Bi+EH6qd&<l1o2KQD#smTYNF5!6u$!JIq
zCKJ*EhPL)Wt=PmQ(AaTNehGNE2sFl%4eA^y<Q3>ur55EEYiJnj7;9=W6sLlQixq-h
zjTN9hfW%@*uMFDlNy$$wRsg#}k3km%7{DBG#|fM|psCb3KPR&oG(w(|T9TSvlA5A8
zngXHiRSi&xh31uH=7378#N-kXRg@1ce8EGI@Tf&A=pac4mR-T!rCd;V8dTUq8z`Vc
z88lv)3MuVCB{O_r6`r#d%2P9oQWSC%^T2V2W($E-j+}DAuGIu3+Pnh2yp+VEqQpuC
zcsfP)Bx;g{=!2wIaFPN!L7@~BC<>V+3TdgSImJi=!tkNoH2o5V;?%t2{34{(hinZZ
zAwp+yl8RCjv)ytM(;>~4>RPamAvqP4+5=NG3R58AT9~3>i(cVqc<DfflOal!LA}+2
z(h_j*Spht-s!*1ilb@Vfk_qbQf=U4|P_>qsr<a_c3sUI?s${&NQwosUUoSU5B{c^c
z$$Fp)R7ar@#`W^|^b3jibb(n2Dh;q11?qL>mL_K?q-3Un$0kZZ!wPxezMZqSwgPCn
z1C#-CQW3Kw@W4>8RR}36O$CQVW*Vp%$N`Ofrt9V97v*Y{gGM?sG!^U=OdxYbuoMTa
z%)s5jY-oQFQmaF<eO`ec7IPpzff%X)tA*23OTxj{Xh5Wu6~K;D$V^j!s?01_$ji?I
z6;BFDiN%@8pt&ZngFzvZmS0q!Sd`+OnFk%r!4t9&3ks2~4usnZu8+$>y?k&7Kd~rP
zM<G)$RZk%$Ujb?f(vT6@{^G>4)Ob+aCq5-JxkSNM0h*}59sngZ1zQF1=q%QZfRx)c
zz}WyA@fd9#9feFCP;M@X2c;0`;1swR)-B1`1v3<K^Fft6s2u>>AFcqZgY_6P^T1gL
zo&q4l6AGnynV>WQO1hw~etv0A3TU`0wW1_7F9k9Tm8g)OnwMIXnalvCQA`Gf2RMHy
zXk;YjrGO^ypd;jvkqz+3d1hXTLTX8}CbW70RS^&jt|*YRDpEi|axFM;@n&0Pg$!_8
z2UOlc8ke9LfXvb&w;PHR%Rp@?c&ijNE|Cfup9YPjfkxblQi}^fBU~V(90NRc6u|xe
zZbk-%-=O}e00RST{r(?*1_sa?fD6!ZfD&lGUy+}I;RzoD!yG;ahA2J;1{ppE1_?d}
z25~+Hh6lV13^lwA40*f^47t1v4DP%P47$7w4D!4T405~-3{t!d3>>@+4BvPd7>@8T
zFm&@UFc|PKFuZ}T1E_)a_oKNP7*x3#7@l%5Fl^*vV3@$gz>v<xz+lA1z`)JLz;K(B
zfng^n149cZ14Aq)1A{3i1A{at1H&^828R6{3=CZy3=GK}3=CQv3=IF-85sVtGcbH$
zXJFXE&cLvloq=H!I|IWab_Rxeb_NDVb_NDpb_NDrb_NDnb_Rx9Yzz$3*ccd!*cce{
z*ccd$*%%mP*%%mD*%%luure_0W@TV#Wo2N9WMyEGU}a!<%fi60orQs+nuUQOiG_hd
zf`x(MG&2Lka%KjGdS(WOaApPuCuT_avNJO<v_aPySTQj${9|NbIKarj&;wgv04hj9
z7*eh%LDQM7f|3r18DEx~T#{d;R|1=%)zE}C^^x=>=7HxuLF)5CT?8c^1yIS6lb>#;
z5$p<{k<rwHD#FyAlbM&AScG3QL=C3if_%{YAEb@}<z2me-^^$T7goB*LUn-(-~1AV
zOa)a1(2N$WxCPC7Le>p{0{}7<37KOCCELuj%wz?P#2n}dW2Sm8c*e!hz(5sgmI_=I
zfU4xY^i<G145S<c<r7fh21@t^MWFD4U|1Os9{vW~s!*B-nlVjONJ&j9O$QJA=_q7^
zCRo8OkK~+E(ByzZK4`|OJhK>5n?SrtWX+mdR0NGmnA^Z67vz+J`Wz_=1v!b8Nc9_N
zFd1A(47X|++=*9E0EfDUl5;+2tpX{%I#4P?t$L9{R>20+6x0Lv6Ts;nlFSflpe+{E
ztO3bm3g7^T<`;MsiYry?B_@}6=9NJ!V{lIh7PD!YMa7`G2gqbOWSSE)r~sZghfMx}
zEd%9JXw^Fe>UX4GrgKhaHpJ`0zA{JiHMjzYlpqSw9!Vx>1}(chu_ztX2LUaQNG(e(
z0#&_9sS3HE>Ajr93dpPnXr6~b8Qj)^3@4T3gWK6TklA){Ip!GP$)K!|o1X$+%TiLQ
zP?--M7lF1IV7)8QS{1ZKKag=tJ@AT{y!^6wkP=w3gf4J^46%UoA=JFgyfW|1yxhQm
z;LN;q$Fls)6yL<W%mUxU3UDF?4gNp_At$v6JPHKr1}WjzrUc1#`0a3w&-NwJe2^Am
z&GsbSY)^b<6K5Q56XGF@vpn<4;xiI+(ugq*O$#w*lV&V#6Oqz_YkW3EVF_9~1RCwj
z0oTmn*)-6kZziPw2AZ1#I~r6qL(?P1sx+_y*m6Hm{~2kV6r2<>bU=NEWGGRY2D}Cg
zDbwJy6O^^ka-WhFxF3XLgcdRe016H(@XR@6l_R)Kfmlvg44$6`O;02DTp$y%DWD<+
zKE$Drngg2lP6Lf16cpu`r4}n><|P&s6y+BbWr7xb<Rq4)7BOUkC(A1#>oij<N{SK{
zic3n<(xB6&xrqfJ1E8^yl$xFiS&so8hk%9|LwPD>QAuJ>4s;$hHx=X-&_bvJP-i?<
zp*S%YJf993|AuxfK@Bfh)PN%%Bml0K6(Civ9)l;Sk(H>Blv$!snx0ds17?-vmw>v!
z3Te;@@x&akZhvou;{2RaP(M(Q!BL^OqzKgYfz1JfMhU>{GeHaNQo#!r^Yc=n?k!Hm
z=%wlEGGIg$xNrxjHfT~TOab%2<0W2tAqozlW-_!P2aao)_uwliLBlAB=7y#fq`1Xt
z9AX?HGp|g;OTj?_WQ{gxoFcU{Rl`fKI3uwjHQFFnM*+dsRM1jDE#fg9mV&t=R0Bni
zj)E6*4+G><(3%kFB2Um7xuVkK6374>qWFg{k3h`*gVF&gTGKLfQuP=>EXZnE)c$m0
zS_yQmS6M1}MQ<i(X%c9P0o0O!_K$N?i*y)@L90y?OTa@EkYQcea*A^B;$z78E@*8q
zcr65y%QA}<z@7Ef6ot$@(2|5=P>O_Fm#0vcnFvWxpow+RDqzrT3}^`gMk>$&Wg76B
z7|>7yXx1eo5i&*xTAP{)N`Igf30r%h!vI<b2}+8f2@cS@iQ@9a0#H>BazF`qwTvD^
zW=bk3P3tIt+6It0251fjwI0BWBQo-HQb0ja1Xig~QdF9gQ>g&%CxF5=wXhUCrVS1n
z&`>vY?G%=j4IPI-1WqwzxFazKoWeo#|DZYhSU%MC|6BMO7{d7(7##T-7`XWv7|!!C
zFihlQU;wM5(>(O3HKQRg8UmvsFd71*Aut*OqaiRF0;3@?8UmvsFd70wBm}^tzMz3O
z=o}Ad!~`}gnF^l;8=cUEY?+{7LNhZDyd?p=paZrG2HN8XEd+$_C{QTQfUSQ3uXISv
z(F4!_pJrfSxDA?v7GPk2o$vRbpMl{YKLf*Geg=l0&@%ug@-r|5g3bxxXJA;($G{N7
z$H2hF$H4H4mw{moF9SmWF9U-xF9X9}9tMW(JPZt*c^DWL^Dr>@@Gvm^;AUXh!p*>t
z!p*=S!Og&Mgo}Y8pNoM(fs27*GbaN>DklSjA}0gGYYqm6xf~1(sT>Rp+#C!Hm)RK@
z^4J*|c-R>j&ap8tOlM<Yh-PD8kYHnAxDQ=}AH~YR;Ka(nz{$$MaF&IEA%}&5!JLJG
z;WIM>!!Bk9hMmj|46~RS7z&vg804857>+P8Fl=FBVA#yWz>v+vz@W?o_5TpbRzy0J
zTE_#xb1^6<V_;}98$GUI^tgi2;|fNPD}e1Igl?OM6&H|g)uYE1z`{d80dl5@QW0p!
zpL#K5Rqg0;1<)fDMvp5<9X+mK^tb}-2UdWmp<xBd=y3(0b%UeF6^tHN0A3dYn|6n+
dk^ybugKpS>F8&xjt^j;oL8=~j@}Gf$0RSO@88-j`

literal 0
HcmV?d00001

diff --git a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-310.pyc
index 35180bad01703e847b6908f23e4ac77c45afb34b..742b227418d085d2c798b868d28a01f1ebb18069 100644
GIT binary patch
delta 1804
zcmaDaw?{=gpO=@5fq{V`??`m&1q}v<#~=<eW@2DqaA06yD4sS^+dd|RA%!uAJBpi=
zA%!J{wS^&yhm#?NErq>>A&NJJBZaetA&QTaA%!c2yM-Z&pOYblCxy3#AxeOgA%!o6
zzl9-6kdq-rAVsi+AxemoAw?)fxP>7~IE68oK~rSoJ9lQr=bOz~b}}(?PyWCb$jH6f
ziQSKpQFih~j(SF^$%35mjPjEUIoBx3GB7X{feASfi-&=MAqeC~76t|e3q}^kBA&_i
zT&5NZ3=9mK%th)TH5wp76GVXZfeEl?UIqq+Fqmd8kY?V=Yq&z|L53EABsJM?@h9hG
zX2&Pz=am%Y=j5ao6@fyf2&Cc`Z%Jl$Mt)v?S$s-jZUI<Bks8Qe#v&O^+inS_r<OS9
zWM(_V%}N2;4T=LUMjl3x{X9ZNIt&bx8@Qzz`zFufj^NZyVX9>*VO+pJnUzOgLZF6g
zAtNJ04NE3NCPNx?FoPzupIeBg;N<r_5z>kvE0q`+7;drT<rn4N;!4j?%!yAeD#|aK
zoXIQ4t;v0hwJ1L+zobZO@;Y8iDR8K$gEXNB&16PCk@|L!+awqm7+4rt7&(}P7{!=)
z7=;*Fn8cX4n7J6axQm<_7#NaSkrE6y0|Nsa0|Ns{qPU2Gfgy#lmNk>1maT?y0mDLu
zTJ{pg1xz)JS<E$zSu8c|3z-=iY8V%=E@Y_XC}GQDU%*krv5+x^DVwE8KBb<ymbHX&
z0VhIv4PzG9LY7+28m1b~8ir=3TCN(#1>DdOVTFeXi=W#|P+ADl<SEhx`A!c+=z|CY
z5Mc-kA<p#FlJKI`yyT1`c(4^&fTXQJgfRm{JwuT-C@Q&cu@)Dkrlu6xfF!g)ge{1$
z0}=Kh0+d6F96_eCl|zig8mJ%z(z*F%siAo#nK_^|keFNoqKfi!eDd=P)`G%Y3?AO~
zLW~?t0?aIoVoY32T#Q_dMINBwW=0BbP*8&~IJiN{xP+mGshP2aF^egishHD?p^34C
zxrU*cv6i`nrG&MHsfMwJxtXPwrG%}9v4*9F0TgD<thKB)tP9u|GB7ffa4g_VVFabH
z60Z6bre;P)h7<;HA}azV(kfx+{G80<%=|osl+=>c<dW1BMMX{SB2cu0gYg!7UP@w7
zQDP-15i>wC3dq$(AXndFE-KBt#Zr=)n|h12I43o=;1+8^QD$C=CL1KTfCJbK6r!L|
zuD`_%&2yl@xy6%{pI;E4o0ylFo>~OS{_t#kO8`AskAlLIkAZ<fh>?Slk4b=ugOQ7g
zhp{LCZxALiFfhP_u%0Q4Ih(1N3ml3p3s`HI7Bbc_#<P{MLqn9KgcBT|%(W~fTo~cW
z4GvG91-#Jk<iir2=Dzu5sXhuR`FW{U3d#9-WvNB!sd>q%3W>=XnUiPoNY`ufLjo6+
zgl`EHWai~3Cud~l#}`zV7bO<lVh(mSzQvN9Ur>3ABPBJp0K_UX1%;pvhyWK<pu_<V
z{~|Atm^X;<0TI5S)QuGM(DI@nKQphS2owWFeu*G;{vZMz)?fluZr)-A1-Q=*P$&y9
zFffQP@-T8R@-V`~8I%|`MQ(AzU0ekA0c&1qZb2nDsT2i+jOCwP&SzB*c9T4a4=Pn5
z#VFVVV7G%45Gdk`K&B!^J;FI0Ho5sJr8%i~j35_*v~n<TFoGZtqW~iZBNGb;Ka(~G
bF9!<~3kMgI3<o=tJO?|IFb69W4-Y#4sXA?4

delta 1042
zcmdm^@?K6mpO=@5fq{WR)jcZJN}hq?F^GeV*%%lY92giFiuX;_wy)=8NMT7~ZDEMw
z=441=OJQ$eh~i1%Na1W@h~njBNa0H1ZefVx<77zTN#Si_h~iHbNa4$7DvC|zZf1xQ
zOyx=yY-WrSN)=ilypSPEB$W@$7foeP<xdq#6>VmY5=-Sw6-pIL70hJFWN2oM5>FLh
zAhD2vks*aKm?4FgF_=M<f8#NCX2$f*d@MVe7`Y~&U<+jA+APKH$H*u%Ig_KFQF8K2
zj(A47$)22Rlw=qf7>dAzEQrO;z`zg)@(c?D1A_%43u6)YWN|K23wZ_x22JK7HIN#0
z5TOAg!1}-hSThd;14AfGGZ#oR&*T=aP&SaEMLd)5bIH_$n74RKGP5)C^YY8$QxbCv
zia^?mR6!On7D;0odrL4qwZu6mGut^oucRnHCnvQi8Dt44R=5~>7(o{C2o-5FFfb&8
zTn&pGkX4|#aRw=V!oa|g!dN@GiaSWqu!KK_shN?HA%(e?rG#;Tz~rmk@@axKTniZ)
z8ETkPSi~5Lb4&PBSZf%vSh7GQn<PUs6C*<je+pX(;{wha_JxdDLQoNQxQH-RgrkOI
zfyhFzjv5wFETu6AGiY-9xm9U9Pp%ZuvJT07!OXyLi@6}bxQag~GcPr<NFlYTD8ER-
zR-sBDF)zI|2U&y@E~zOxnTIDrNdXi{iVO@4w^;J>i*j#qrROK+#Dh(^#ad95nO8Eo
zo<~mo78}IuTkKFHZ*i6sCFT_u<QJ!Ea^GSt%1_EKDbk$0g2yrk9H-#e#S*c%_>&<)
z?~|Wj0E&q=P>`xHFfgz%vM_Qm2{DQ>@h}Q8axk(miZO99b1`x-i!cf?3Ni9AaxoXV
zgIvlHAD^3;nHL|gDLFYoP=W{Khg&Sk`303le3P36tz6_lo&v=gBt3wl6RtBaF*h|n
z9+EgfS?v~oN@`kWUaEV3VopGQW?o5=DM+{G<aU0!m|M(6rFoDf4NjV10+c>)aoFVM
zr<CTT+A)IMZ@|F7z`?-52!cF}0*p*79Q;h$9K0MXOe`E+Ofnqo9CB=6Sq?@XMgb-s
HMwTA{iY?l$

diff --git a/python/ur_simple_control/clik/clik_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index a85c176..adf18cf 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -4,6 +4,7 @@ import copy
 import argparse
 from functools import partial
 from ur_simple_control.managers import ControlLoopManager, RobotManager
+import time
 
 
 
@@ -141,32 +142,97 @@ def controlLoopClik(robot, clik_controller, i, past_data):
     SEerror = robot.data.oMi[robot.JOINT_ID].actInv(robot.Mgoal)
     err_vector = pin.log6(SEerror).vector 
     if np.linalg.norm(err_vector) < robot.goal_error:
-      print("Convergence achieved, reached destionation!")
-      breakFlag = True
+#      print("Convergence achieved, reached destionation!")
+        breakFlag = True
     # pin.computeJointJacobian is much different than the C++ version lel
     J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
     # compute the joint velocities.
     # just plug and play different ones
     qd = clik_controller(J, err_vector)
     robot.sendQd(qd)
+    # TODO: only print under a debug flag, it's just clutter otherwise
     # we do the printing here because controlLoopManager should be general.
     # and these prints are click specific (although i'm not 100% happy with the arrangement)
-    if not i % 1000:
-        print("pos", robot.data.oMi[robot.JOINT_ID])
-        print("linear error = ", pin.log6(SEerror).linear)
-        print("angular error = ", pin.log6(SEerror).angular)
-        print(" error = ", err_vector.transpose())
+#    if not i % 1000:
+#        print("pos", robot.data.oMi[robot.JOINT_ID])
+#        print("linear error = ", pin.log6(SEerror).linear)
+#        print("angular error = ", pin.log6(SEerror).angular)
+#        print(" error = ", err_vector.transpose())
     # we're not saving or loggin here, but need to respect the API, 
     # hence the empty dicts
     return breakFlag, {}, {}
 
 
-if __name__ == "__main__": 
-    args = get_args()
-    robot = RobotManager(args)
-    Mgoal = robot.defineGoalPoint()
+"""
+moveUntilContactControlLoop
+---------------
+generic control loop for clik (handling error to final point etc).
+in some version of the universe this could be extended to a generic
+point-to-point motion control loop.
+"""
+def moveUntilContactControlLoop(speed, robot, clik_controller, i, past_data):
+    breakFlag = False
+    save_past_dict = {}
+    # know where you are, i.e. do forward kinematics
+    q = robot.getQ()
+    pin.forwardKinematics(robot.model, robot.data, q)
+    # break if wrench is nonzero basically
+    wrench = robot.getWrench()
+#    print(np.linalg.norm(wrench))
+    # TODO: remove magick number
+    if np.linalg.norm(wrench) > 4:
+        breakFlag = True
+    # pin.computeJointJacobian is much different than the C++ version lel
+    J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+    # compute the joint velocities.
+    qd = clik_controller(J, speed)
+    robot.sendQd(qd)
+    return breakFlag, {}, {}
+
+"""
+moveUntilContact
+-----
+does clik until it feels something with the f/t sensor
+"""
+def moveUntilContact(args, robot, speed):
+    assert type(speed) == np.ndarray 
+    clik_controller = getClikController(args)
+    # TODO: just make the manager pass the robot or something, this is weird man
+    controlLoop = partial(moveUntilContactControlLoop, speed, robot, clik_controller)
+    # we're not using any past data or logging, hence the empty arguments
+    loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
+    loop_manager.run()
+    # TODO: remove, this isn't doing anything
+    time.sleep(0.01)
+    print("Colision detected!!")
+
+"""
+moveL
+-----
+does moveL.
+send a SE3 object as goal point.
+if you don't care about rotation, make it np.zeros((3,3))
+"""
+def moveL(args, robot, goal_point):
+    assert type(goal_point) == pin.pinocchio_pywrap.SE3
+    robot.Mgoal = copy.deepcopy(goal_point)
     clik_controller = getClikController(args)
     controlLoop = partial(controlLoopClik, robot, clik_controller)
     # we're not using any past data or logging, hence the empty arguments
     loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
     loop_manager.run()
+    # TODO: remove, this isn't doing anything
+    time.sleep(0.01)
+    print("MoveL done: convergence achieved, reached destionation!")
+
+
+# TODO: remove once you know shit works (you might be importing incorectly)
+#if __name__ == "__main__": 
+#    args = get_args()
+#    robot = RobotManager(args)
+#    Mgoal = robot.defineGoalPoint()
+#    clik_controller = getClikController(args)
+#    controlLoop = partial(controlLoopClik, robot, clik_controller)
+#    # we're not using any past data or logging, hence the empty arguments
+#    loop_manager = ControlLoopManager(robot, controlLoop, args, {}, {})
+#    loop_manager.run()
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 0e39dc8..af7c8db 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1,5 +1,9 @@
 # TODO clear out unnecessary imports
 # TODO rename all private variables to start with '_'
+# TODO: just read the q and update everything every timestep, don't deepcopy,
+# --> just create a RobotManager.step() function, update everything there
+# don't do forwardKinematics 2 extra times for no good reason. make that the libraries
+# responsibility, not the users
 import pinocchio as pin
 import numpy as np
 import time
@@ -13,7 +17,6 @@ import copy
 import signal
 # TODO make the package work
 from ur_simple_control.util.get_model import get_model
-from ur_simple_control.util.robotiq_gripper import RobotiqGripper
 from collections import deque
 
 """
@@ -63,6 +66,7 @@ class ControlLoopManager:
     def __init__(self, robot_manager, controlLoop, args, save_past_dict, log_dict):
         self.max_iterations = args.max_iterations
         # i need rtde_control do to rtde_control
+        # TODO this thing might be re-initializing the robot
         self.robot_manager = robot_manager
         self.controlLoop = controlLoop
         self.args = args
@@ -108,28 +112,6 @@ class ControlLoopManager:
         # but i want to do it here. this is a big ick
         self.log_dict = log_dict
 
-        # if you just stop it the program with Ctrl-C, it will continue running
-        # the last speedj lmao.
-        # there's also an actual stop command, but sending 0s works so i'm not changing it
-        signal.signal(signal.SIGINT, self.stopHandler)
-
-
-    """
-    stopHandler
-    -----------
-    # can't have self as first argument, because the 
-    # signal handler has to have these 2 arguments (even though they're not used, but
-    # let's be correct if we can)
-    # so it's static, problem solved
-    """
-#    @staticmethod
-    def stopHandler(self, signum, frame):
-        print('sending 100 speedjs full of zeros and exiting')
-        for i in range(100):
-            vel_cmd = np.zeros(6)
-            self.robot_manager.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
-        #exit()
-
     """
     run
     ---
@@ -167,12 +149,14 @@ class ControlLoopManager:
                 continue
             else:
                 time.sleep(self.dt - diff)
-        if i < self.max_iterations -1:
-            print("success in", i, "iterations!")
-        else:
-            print("FAIL: did not succed in", max_iterations, "iterations")
-        if not self.args.pinocchio_only:
-            self.stopHandler(None, None)
+# TODO: provide a debug flag for this
+#        if i < self.max_iterations -1:
+#            print("success in", i, "iterations!")
+#        else:
+#            print("FAIL: did not succed in", max_iterations, "iterations")
+# TODO: reintroduce maybe
+#        if not self.args.pinocchio_only:
+#            self.robot_manager.stopHandler(None, None)
 
 """
 robotmanager:
@@ -199,6 +183,11 @@ class RobotManager:
     #       like freedrive or forcemode or whatever.
     #       you shouldn't care about previous states
     def __init__(self, args):
+        # if you just stop it the program with Ctrl-C, it will continue running
+        # the last speedj lmao.
+        # there's also an actual stop command, but sending 0s works so i'm not changing it
+        signal.signal(signal.SIGINT, self.stopHandler)
+
         self.args = args
         self.pinocchio_only = args.pinocchio_only
         self.simulation = args.simulation
@@ -235,6 +224,8 @@ class RobotManager:
         if self.pinocchio_only:
             self.q = pin.neutral(self.model)
         elif not args.simulation:
+            # NOTE: you can't connect twice, dw 'bout that
+            print("CONNECTING TO UR5e!")
             self.rtde_control = RTDEControlInterface("192.168.1.102")
             self.rtde_receive = RTDEReceiveInterface("192.168.1.102")
             self.rtde_io = RTDEIOInterface("192.168.1.102")
@@ -279,6 +270,22 @@ class RobotManager:
         # TODO this is clik specific, put it in a better place
         self.goal_error = args.goal_error
 
+    """
+    stopHandler
+    -----------
+    TODO: make ifs for simulation too
+    can have self as first argument apparently.
+    upon receiving SIGINT it sends zeros for speed commands to
+    stop the robot
+    """
+    def stopHandler(self, signum, frame):
+        print('sending 100 speedjs full of zeros and exiting')
+        for i in range(100):
+            vel_cmd = np.zeros(6)
+            self.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
+        exit()
+
+
     """
     setSpeedSlider
     ---------------
@@ -324,6 +331,41 @@ class RobotManager:
             return self.q
         return q
 
+    """
+    getMtool
+    -----
+    urdf treats gripper as two prismatic joints, 
+    but they do not affect the overall movement
+    of the robot, so we add or remove 2 items to the joint list.
+    also, the gripper is controlled separately so we'd need to do this somehow anyway 
+    NOTE: this gripper_past_pos thing is not working atm, but i'll keep it here as a TODO
+    """
+    def getMtool(self):
+        if not self.pinocchio_only:
+            q = self.rtde_receive.getActualQ()
+            if self.args.gripper:
+                # TODO: make it work or remove it
+                #self.gripper_past_pos = self.gripper_pos
+                # this is pointless by itself
+                self.gripper_pos = self.gripper.get_current_position()
+                # the /255 is to get it dimensionless.
+                # the gap is 5cm,
+                # thus half the gap is 0.025m (and we only do si units here).
+                q.append((self.gripper_pos / 255) * 0.025)
+                q.append((self.gripper_pos / 255) * 0.025)
+            else:
+                # just fill it with zeros otherwise
+                q.append(0.0)
+                q.append(0.0)
+        else:
+            q = self.q
+        q = np.array(q)
+        self.q = q
+        pin.forwardKinematics(self.model, self.data, q)
+        # TODO probably remove deepcopy
+        self.Mtool = self.data.oMi[self.JOINT_ID]
+        return copy.deepcopy(self.Mtool)
+
     """
     getQd
     -----
diff --git a/python/ur_simple_control/util/.calib_board_hacks.py.swp b/python/ur_simple_control/util/.calib_board_hacks.py.swp
index 0c65b52814ff442ca7d522cc8b831f8b20b5c347..c991d6f05588066e2000883dccfddc9f00ddec33 100644
GIT binary patch
delta 4386
zcmZo@U}{*vB$Q+j=IN_op=ZRvz`!8D!0_tCMxhV<{Ok<%3=9lRAVCHO2FJ;Q0`iQu
zlMMyz8I2|v3Wy7GKxOox^ykf$0_psd6j*q(SQr?(SRsZvP8L+qu3y5(z)--)z!1;J
zz!1mBz!1g9z!1d8z~IKmz`)PPz;K_JfnhQ)149rm1A`<l1H%^{28Pu<3=GS87#NoE
zFfc6QVPKfV!@$tU!@!Wn!@wZR!@zKkn}J~yHv>a4Hv@w)Hv@w{Hv<DdHv_|KE(V74
zTnr3HxEL66xEL7JxftRZeseM~JmzFz*uu%ckj%-zpv=j@aE*h3;V=gS!vYQlhDr_w
z1|tpzhO_Jp42Rem7<RHVFf_0;FmSUoFl=UHV5nqcV2EX7U{GaaU|?WlU^vCfz;Ke4
zfng6T$g7*B6bo1;AJVi)DlRF?&r4@uP=JDhqRhM!4W+#J;+)L%jFOzn`0`W`BQ-@K
zvsj@xGcUa~C$UH&si-tVNfWFqwIVsSphO|BG`FBqFDElEF(+NmCo|76C*8HED8I;R
zvZ0TNW^zh?i9&g5Vz!cwLV<#V0v7WjrcK_RCsPk{3vP!f>EPB5(uu`v1zT)d6!aDH
z3Sdso%P-2+fN0f0@i)X>I1QfsQA2z3<$PUVkO~kE2U%92my(&7t^qN=xF9h(Rl`t6
z!BEdgM<FLQPop5eII|=(Kd)F*6JmTxQKc0)G?l@j#ISj%1~22}ADXvxK}LcwG(5lo
znU|lJ3J=J<{5*IZP2QQOTAvq>ECdcMWC4hUsER;NM$v^WXoxYSKrd(VeJy(~kc*WS
zK<?ZutNoVQJUJ&bTMtCU7vyK=mBg3iLl_E~xdr(}B?`IuWvM<oU@A1PBs0f3Kd&S)
zxn%PvJt4+=9(D$XFYJ))X$;N0C;1r|R`4@0bn-JW1o1O4Nb@r=JmF(tIL^nwu$GU3
zp@)xwp`DL`!JUtRfq{>K;WRG;!&F`dhJ0QIhCE&d23KAN1{Yoi27O)zhW9)S42yXf
z7_xa77_xX67>sxr7=CjzFg)UBVA#jakjyZdn}H#Qn}NZan}I=%n}LCYn}Ojn7X!mN
zE(V4f&|GWC#lXPN#lXPF#lUcglYwCwCj&zlCj&zUCj&zmCj)~ACj-M54hDuD91INe
zIT#pHI2aftI3OPT%)XhC<q_-T1?eo4OD)7F7g{LFfkH4jF()&rD6u3pASW>oMPPEF
zMeyW83u{*tQIIlaGQi|Q3uQ*-$x|%68I>pBx3H2@1{tfYpsP?^l2}xtkXDqMno^Wm
zmdc<!+0fFOQF(HXWw0VtQHZ~bzm)<5n3oq{l3H9+tYE8PXuzO6`L3lRqw?f`meP#M
zlO?UBy_FRpii=V~@vV@bmz=6lnO~}qotIy(P@IujqEMb+lwGW#0TRpw6*~$^sn8^n
znU}7RnU|bjl$u<UQ>i)G*eae;dGaKyOTr+td^8e^(u;Kzit>~4OLP<_PqY)$(km%S
z%qz}G1ZA{nqgVx7h0MInlK6uB;#4%L$rA!yInq;0d`t54b2hWvWHG9N3`I_fsOA?J
zq^725GB8Y*3(}f=%yv@*#Aga68L367>ctAlsYNA;nRz*t3VHbod8w%>3TgR83W+&6
z3i)XuMa8M$q+YC$RFn_S_Fw{JCl-(J>J{aefPJB0s}M9<!CtE#&HZ@11~xb^wH(vi
zhPXq-Fc!BqgIEPyG3+5ZdAhwiuGml~(#f0u*mDT*zGY`%xCpAR1sE8NCJP4m*GKX)
zFgWotFsSk}Fo^InFbMN9F!1m)Fr4IJVA#yVz)-=%z!1a3z@Wy%!0?QlfnhN>14AV@
z1A`AY0|PrZ1H*ML28N|v3=CCV3=Gj+3=C#m3=F(n3=ID{85kaMGB7;kWMJ6C$-pp$
zlYwC}Cj&z{Cj&zuCj)~tC)g$R3}-nQ7^ZPBFihoOU})lCU`T~lz>*vc3|HA17~0tx
z81&g081At#Fx+KhVCZCHV90}3z@Jzd7&fsoFr>3GFgUO>F#Ka-U^vIZz%ZYMfuV_o
zfx(J}f#DA`1H*Y{28LE<1_mo;1_o|s28Opx3=Er@7#JopDKRiOGBGeHFflOvXJlZw
z$H>62o{@oJ5+egc10$&R&P>}}7j%qia<04Q<N#}t$psNCleY#LGb&Fmv=yIxAxL6#
zai|&N<mYvKlV97&O+FhIJo%T6%w+xWj>*4l1SUUrlbL)iTyF9&8|lqV5uA+RDp*+|
ztF*X8A-O0uu_RR?Q6UJFxqK7z64O(Q^omPT3p6wp(n|A^K^aU(p|l_cq%5^8wWzWr
z1C(9CxdEJ#Albu8AtgUgy+k1wYy>DvlqVLYC}d~mrRFA<WF{Ab3WK8J)SNUOXQ;MJ
zh1}G{JcUe15s_J<UaXLr2g*T^oDNk5DlWiIP037A$ShGvNi0v-W0*W4P;PQWl%^3l
zTWf+sNkIY3R<MO=*MsGG4M?S^tl;h%q7dR7FnL9kzXG^K0~aWI#i=F1pe!GplbMoQ
zq+y_EuF0T0Su}b+8>HBoyd~Nw0Gy>2$}>|+G87W?QWP@44JS~Ul$;F-NswX&Ww2y^
znnH4ZX;E>iLQ!f#PGWK@*i*$Nr3INO3gF_Uc=P?}9!6nE$)#(kpal_*Hj15`7%Qq|
zVgU6#N|BnIn4XyoYKtYM7HzJG?P01fWo2M!1vLT%7#M7!m1jRc149u%149x&1A_%W
z1A`_%1A_uT1H%bE28Mn<1_nny1_o6=28P$X3=GG385kz;GBA|!GBA|#GB6ZETfLx)
z^FI#*!$BSfhUq*E45>T}45B;?41zoi4A;0B7*23AFf8I`sAs6>W?)EyR-9kC7#Oy2
zF)-9|F)+k(F))~OF)#>nF))1KWMDYY$-uA@n)A##85p=Y85quSFfg=oFffF2FffF0
zFfbTGD@|?=1_mw;1_m|`28K863=HR>tzU0;1_ldu1_ow!28O+C3=BKjL>L$*vq3^2
zi;aPynvH>>oOLrJOCrnU1?i@f7o-<XUXboPc|p1avx0)c<^}10*)mXCLf}vaRfo_9
zSYAA&$xNk|u{O9t2Qd=fq=RsgY=9(BP}_L&<vc%d=EUKg)D&=o4~GdLe_-<fxOoij
z2_RXDtqlk^S1cV#6~t+v_UmM;{2UQ1p<@eeA{(;K;bUO9GnunMolAnBf#EHP$G7>Q
zKri29P|N3Zo#5n#z)*NaJlV4Tczt3Dq!@LAw*O$Aw^#fO44e5G7>fBB7?k)K7+&)+
zFdXD#U^u|Xz_5UifgzKRfx(E6fx(cEfkBRsfq{{af#C%&1H&O+28KRf28Kjl1_lLQ
z1_pLs28P2t3=E~vPTCo628Pqz3=HLvCSNdv0XGA~UoHlQy<7|oMO+LF##{^xA2}Hq
z4stRu%;02TsODr~Fy>@n_{zb+aDsz@A&rB9ftiDW;TAgs!ya}9hFR<k42A3r3?b|c
z3}8=y+IrG#ARqICq5}y}t_!kehw?#etaYhgh#Xpj;8n0TE|sXYt|87Ee{yt)jSfl;
z4XWm`*YeQ@u?pIg6GCJr9}lTTu5ZLaPRG`U(F+kn)I*btL%mR)g}Z4s`Aq0zL~=M2
z7J{qJn#>eYln<+9U<GPgei5jCL8@MoK`pTI%#sXf@eirP;5i)Rc7km_kS9SHRI%z?
zf>_X;m|2oq1Zkz^mF6a;7Fj9jC}cu%2S`;(W^SrpaZYM#frf#ef#Kx25yu4-Y!#C8
z3o7+eQd0{+jLF@Zl9Q7ol|{j|6uj97F+_QCa^wU?<;l73`jUlEp+e-EEKoy}0RX>7
By0QQO

delta 1710
zcmZozz|_#dB$Q+j=IN_op=ZRvz`!8Dz;I^CMxhV<{A>*M3=9lRAVCHO2K~u`0`iQO
zlMMyz8G|Ml3W#rBArQeo@q@x71s2|i><kP$IUu_ACkrZQ*YD?JU|7e;z_6B&fng;d
z1H(c-28OA83=DC63=DdF3=EHW85kDwGB9NDGB7yvGBEt&VPN>i!@%&Bhk@ZE4+FzP
z9tMUBJPZt5c^DXSco-ODc^DWNc^DWDaWgP9aWgQ~aWgQ)aWgPjaWgQ;aWgPn;$mQE
z<YHiO<YHj>!pRWNaEOzEVLc}ULk}kdLn<c&gB&NsiJF`Y4EH%07>;u=Fs$NWV3^Cn
zz%Yk{fnf#*14AVT149@G1A`g|1H%h;28Mm?3=BQ&3=B5x3=9_R3=DtS7#QBOF)*BD
z+bpG6z_K|xOP*PPK>-R1iZb&`G?X@<Rp(`#+^1?X`Hsd7Hf4}x-sCzhx6S9Z-Y`$*
z(*IT8$<Dw~2n{V`Xz)JdXJFXH&%iL7pMjx<pMfEkpMk-SpMgP<pMimapMl{S9|OZ#
zJ_d#@d<+b;`4|{V_!t;;`4|{}@G>wQ;$>i%!^^;s%gex!!^^-B&db0c&db1Xnumd5
zG7kg8BpwEaC>{m|WgZ3wE*^$thKt+`49mC~7}~iR7!tS{7`(X|7<9QA7=CauFns1>
zVA#vWz%Yr6fgzlWfx(oEfx(1}fkBdsf#Dw~1H)ZT28J`73=I1@85nXn85j&X85ks?
zVLF8a;;{mb&5SJF?2}KJNl%_&t}yvRnlU>l9F!GwC+jE~PhMalF?pwj_2jP>!EzAs
z5PuhcD+O>+=Eaw!7MBz&*eVzrFig(0RGi#xDJ_+mrVt;WmzbLxAFp6*tDqDgpPQJO
z7ay-=HJR5cesZqWrO7N#!jsPhicCIh&oMb9RDAMuYlq1!;qsdwT4ykVC4D1gCKub@
z0JEVAd818Z6>JrX3M!+GVi_j02gy#>36h*VH%Myo{vc^a&B@2?v_OVQ=cSg%7vvYG
zMjL|EXXa&=z_<*PdxPaBci5{Sn>#rqRCtpEivX__BLhPnC=UrRFa%8&4Dhc{;$>h6
z<Yiz8;ALR&<Yi!B<7Hs@08QyLc^DXyp)vJ<n}J~=Hv>Z<Hv@w`Hv@wxHv_|WE=WpW
z$;H61f{TG6nTvtJnu~$Kii?3km5YJlJv7nBb22c*aWXL2aWXKlb22bo;9y`l&%wa3
zhl8P>VLk@~Lk<T6Lof#e13w1?!#Q>ahGupK1|xO`219lRhF5G14CmPx82Z>47^2x2
z7^K-47%s3fFsx){U`S_WV6b3iVEDztz;Kp@fuWg&fx&`>fq|2Sf#EeX1H(pU28MoS
z1_pa(1_n8128O>(3=DUe7#P+vF)&PE0tJN(1A`P31A`<J1H(5)28OSU3=Df185kxq
zGB7kSPCi&CJ^5oDINl~(*XvC#wpE)vz20ndL*Ox{$*V&QCf9edZWahNV4VCfOm%W@
zSP-Mx<g;PZCO>eKpWGiVH(AkLdGg)xFOzfoA}4?7Q<^-lTYhq8zu)A~Hu{W(lXY!n
zCV#AxoV+nobFyC;+hmq-1xAL+JW>9WbFCy<4fISj875ap&7Z6gZM1o1bSvXz?^tdo
zLxar~u{})n3)mPK7O+FgI7?`*UC7VC(8ABaP{Pl^;Kk3tV9n3KAj{9daG8&RVK*NG
z!#q9)hKYO(3={Ym7<%{^7;^a-7`*rx7^L_Z81C>gFl^#wU})fFU@+iiV9?=(lzfkQ
z7#Q~QFfh#GVPL4`VPJ6NVPFv8VW?*~&&|LvlbeB|f}4RMh?{{yhns<co11~*6&C};
z5onGLf+mEQoD2*LI2jlUI2jo7IT;vyI2jm>IT;w#I2jmJIT;ufp$X$92Lr=w4hDua
zXnK(2U|_hx&cJYmoq=H;JJiEq&u+feAj~%LL-*wJRMXA7(oV3+fO9J-`$7x8<dpmp
zB^`wV1qX$^$qzMECkNy?OXexqD&#5XE94dE<z(h1=A`T8<rn2@<Y`XU$j_O)EI*F5
josWUx_+-ukbuMOp28J6T9^dAN0=;~bkJpJJC5EE_ox^dD

diff --git a/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/calib_board_hacks.cpython-310.pyc
index 18ef112aef1717a31bc5af78a2f9a5da0aa64059..9fbe6dde187a2c199737f2ed5ab83fdad5896ef4 100644
GIT binary patch
delta 3190
zcmdmQw82a}pO=@5fq{YH)rsiTY1Rx3k3k${%*4RJ;K0DZP~0<7+b)<ThbxymiaVDl
ziieRQg&~D8hc}9slYx^Vg*An(g&~TMlOcsYg`<Tbil37qg)@b#g&|5Hg*%0(g&|5X
zg*Sz-g&|5Pg+E20g&|5fg)x{xQ*h!L<$6t~TdcYHWvM>51VB`1UP)$-bADb)Vsc3`
zBa&;_7#J8h7#J9wK@M8P$iPs-uz<0KVIgBJLkU9`Qx<a#NQ9+?wT7XYaUmnffxXNr
zOp*+>j5UlP6${vE7#A{1Gt@Gcu%|HBFg7zWGSo2CyD-FZ)-si_r?Av8)i4S$fP@QG
zs_aS_7I2g>EZ_wB4r(a#LdIHVuu)t!3|ZVY%mNHG3<3-b89_V&1`xl5VFBC;0t~e*
zH7sEDY&9%kt656eQ`l=*Kz7tHfNib^S<O+yRKo(bIx(CfkDrC1geQwBi?_0xkpTot
z*i$&085tQO8S)s*LH-0ejW2~2;;<A}afXFVBs-3^N}-0e9_l_;u=`j`*i*P_SV8V9
zVNc-(SqXI~$eBDfOf{@fXKo2+s9_9dfIIX8$eD}`CG07@AYGAQ*>VO=K0i&yTdYON
z`FSZtpy<5CmlvOxpBG=Anwg$alA2Oq#hILvU!stgpO^X)q#)fG3_jc663&Y+&H<^)
zse~(ssxD7W%&t<wBCn8HtWcbpmtLBaSfr3tRGM*1C@;Raq$ocx9cD^uN|jA!nnFoN
zszO?R&gA8cM)g(PU^hV2fz3tHQIeX1VXh|sEvCGJTdaviMTwQS*m5%S5_8gTvE~$)
z6c^rN$<IqIzQvrAns<vOFTW`F7HdIKW?soHmXyrI^jjP`nR&$piOH$Am^1UrZZU=g
z-D1rv%`K?B#qE=s=a`f3T2z!@bc-#qC^0WRRg>)&XF+~(W=Up#UU3mS0|Ub?#)4an
z;YFMvCMb;1-MW&YNN%zXQ;VkxD67~pFfed2DliK$axro+3NSKpNiga$!gwtI1(<jk
znf^<$=rJ;}LPWu;{<E+M{NrL|VPyJOq%iq5(`H^p1_lOArkBhN3=BmalV>xlv2lZ#
zJd=+zt1}8se$On)e2Xz}GB1lHw;V_UOvq0*WKoul0y#v8fq{XAk&lswQHocDQG}U;
ziI0(mu}FV&A<G;VMy<_!oJ!1$ijxg_rgB3J1{oqW`65pNig`Y~GK|YNXYzJ1N`Oqf
z#hRH{P+D?}CAA{6q{tJb$7?btzd?`%D0l=?^HSW3Qd3ikGRsnZ^HWkaS&O_u5^RZi
z#pS6*MSdU+J|Gc+G?=>hl8mC%#FQei2f&0pNP*<!eqKq&+R01#eHmjWf98*p1%*hF
zF(_DAZm}fi7gXNjNJ&jC0I`a=Cua%Bc!PC=39u1dX_*xPnR&%Q0g<;sK>{kzxR`ht
z#Xtd(#K6FSQsRL^8l(bT;>AtADBxS4!dk<W!YIig!T_Re7;2b;88q4az>WZCuOg6!
zml_NV3@`ux|Nmc;r3ew0ydb{{f}G1-qzCd5M^$`&T6|G{$t~960#G1<T?!_^&J{~f
zEeQrmc;>le7NsVa)Mw`B`KS3k0=b=sfq_AQQHfE2iGx`H<o<M!`&p4nY+>XQTa6i1
zV%IQ&3WpNL1xz)J3z!#zN^jO0#sw@38EV<UqHGHpYuUjp_JxeK9AFk(7VARBTFw&2
z1@%ldoC_Iixk}h-IGY)3xzibHc}hU#DWeNR?3r5L5{?C&DU1sl85v5rQkZIZ!Q~it
z4MPoM4R<p$BLl?JTD}sV1-vzkS$s8&S^Ome3j}NU7BVhm2J2!@X9S663Dq!W2}?3$
zi7XIJVOhwyh_QqxORRo@IEp0eLPnT$5JMjW*Z|gaMzE2LSrQ8*K|VxPQNy!93hXP!
zEPjL~3&D=A;jLjwVUu9UW+-|I4HbT{T7FPi)UapOi=?niGSqNniKMXGFqBAxea1nI
z&uZASL{c~;8A=4QWERLQWT;^hVGxIy#9_k#mSsbdWfozOfXK2T+f&cTP{UQj5zmTZ
zHE#+>HcQd364@G_X2x2961f`A8i8gea5Qm&qNz}?gbPxCVf7VmPeC0G_Atnq^&kN@
zXb7-D-Hj9?n2syqO5p-0Ikp<GQ%Xol4d7&o98L8L8EXYgxKg-l1VKrQMBji@d@Ub1
zM)?*p)(VwyrSO0q$CbhhHIb`Eu!c2_IT%!NrdNxsbAMRXVqYbgS&~|mSdyulpQn&l
znwykbWL0De%FSjV!W=|!fCx}oRKz$rMRa|=d{GQYBo;)(figQwdTL4FEspfm65o>i
z{G1|pkPIl#-eLvUGtjaVBnm25i$FQK2vm6#*@MJD?Z6^O5DQVDIDyRN@bdTc3yJr1
zDFT&KMJ^yEt_)@j3`K4rrZtH001>tzf(4|7ttc@sB|o=F0VJXiDv*SW0zho<$<M?z
z>e&k8GxIV_Zn1(LdW#dpi7&`6PQAsEms$>H6={N$g9@>tG!RP)WGrK5kuQkmNiHoa
zO3f>QsMG?<gPeAY4Qx;$db!S*oS2iDRFqhf8jzEim-@XPRJzrH$}UzuMh+$}CN4%E
zE-pq6W)@~1MlMD!Mjj>(CJ9CvMj1wq|4K|UOgxMnj9iQY%sk8-j1r7;%yNu8jBFrL
zCXhG>BU1wh69*F)BdDeoU=m;yfXK3PKur<@)y%A1i~@{clk`Exv2ZaKm4iY?<Q8W^
zW?p`Baz<u8Bs#=F9s)-KxVDR(oF(C1e@m;hD84u|w;(4qJ~=<Hq$oc}FF7YO8%)F(
z<Y(rU#Fykl7)78+fW&Vw$Sim?K#~lo9x4JQ46p-`LJLtbaM<MLr<CTT+JPEu#VQO8
ostg>=9E>2y!^p!Zz{tVG#KgqI!NsJ@!O0}W!O0QF!OkNJ03<Q&7ytkO

delta 2664
zcmdmBw%<rQpO=@5fq{YH%#!HTN)rZ##~=<eW@cbuaA06yD4sJ>+b)?ghbxMUlYx^V
zg(Zcxg&~TYlOcsIg}sF#iieXSg(HQtg&~SJg)4=-g&~SBg(ro#g&~SRg)fD_g&|5H
zRWL;$o2kermA#oEN+?AzN;p*@RVY=snTe4hg)tb6HH9YbQLayBL~;TP0|Ns)0|SFI
z$Q5l23=Aa<3m9t{7Bbd?SWG1h3z$<FOIT_cni*3V#TgbdEo7`^EMdrEUBFhuki}lZ
zD8Nv|Ai%JY5yTT<0P#y07H~jxEMyd5sAZ~Q0;}h&VOq#2%}}e$RKk_Q)Xd1pP{ZKD
z5X)K1T*8&YT*F+$B)|Z2Y@vA#Q!s-ji{DF-J16@w+a=#(NlwWx(d53xlvi+zHL<8D
zvGNv6N@imEEw;p>#Ju#>Tg*ABdAHbdGV>C1(r+<m=9S%I47tUUpO;#EizP3=DEAg?
zK~ZL2i6+Y}&Vu~n%#zIfyy9Do;kOv`ZZQ_DWGE7y9Lv%otqckT0R{#JE=DOfJvISW
z4n__}0Y(<aBJs&utebfy7#J8dnO=fot%zmvD^@i&HU<WUBKFB*Z0d{xlkM0fHH(B9
z7#MD`R>kL~mfvEnfKZiSiZQ526r>zXh)u3zQ<ilBSt!K7z`(-D$H>Dd#Vf)n!py<M
z2eMLi^FFpYER4dNqq&us86_sy@lEB1SO+phV6q&4fdJSjFd+uAR($e*esRW$o44_I
zFiL>@af>xGub{N#7E5YHW=WAfNTb8#FhPSLJ&<yN)Vvh8qSVxsqRg^X-~5zRP1Yhu
zkOW&|UU7M9Q4z>_MNS|Qfi#%9_>zpG)Wj5IuYeRtPQJr0$(TO*lb|nS&}0XpC^k@F
z6=_ahASC7ma`r8zf=o@OTO0+MdByQX1(mnhN{c`=x<OoNnH2$<dBs5ikw-zH!vTs8
zCLTsHP?$ta5|(B3n=B{nTMteotbRqBAWwl(N)ae2zXatsO_m~%jYXhzUBnHN=Lba`
zbCC+jr5si9`DyV*`6ahliwi)W2ZtY+00*L2dTL29NWwGEC9^0s8I)H1)BMhW90M|4
zfKiE2fQg%fiG{H!hJk@0nPoDMh(vuBGXp~nLl#3VOAW&U#uUbdjEoE=OrU(j$WX$(
z03^z~5R|W2A-pWM1?*rx8<@|wkg=A%hG7BgLWWw75{?C&H4Ir?H4ItYH5?0>VJ2j8
zXYtf9WbsNeWbrLztmQ1>ui<QFtmR5)sIBKN5vXBqW^`eQJyHu&B)CARh705fY=)Ky
zFA%BWSjY%AS~P_z9h7~!vv^XNB^k2B7Ko>?EM#27Si+Gdu|N{!2&fF-LPnTe5JMjW
zBSVR33Trwe$h7(z2AGdfmDF%AkOKRV0pxm!1q;EUQ^QlkoWdr-kj+r^1*W@}7p#^S
z92#sX?2-&M>?!OvppX?@AYH=__7GbNha^LZaF)yhnS~5Bj3Nx;3^n!aDI7KoHEbzt
zNK#B93=$v%*ia3C`ipZRV=Z5aYz<#CV=Z3|AIQ)e_7qMVh8jMQDK+dVEMPXo5nQNd
z)bONmWV1|QEMhB>tKn{Dtgq!Sk+0#b;csTD6)54a5dg(Yp;L)y3OA&vgM<$8{;NUq
zPz`$u$O}l}0ri3a+za(&_@P8Jg$Lv~m~(3wvWQPL;3N%-Qb>ZV&k|e6SSwf}n!;Nn
z2r9-&icwg)2N}w-kg-;%L^OpD<TOxTs$l`edWmQXKh$J_8o?UYH0EFiO##33Su;W4
zrnY^NHYlfSfe0NCp$j5FWdc$WA<4kN@RD_Mhxqz>g<Cw}>>pp8lbMoQbc?Sz6_V|P
z!Th2ikoI5@5dtbTSkhBV0*eemd^-?f0wPR7gc*o12N4z^g&bb~o_-<mo-ReUATdx0
zaf=mPNfki~-Q4`L)X=<;lFS_E{JfIH<PxO93RGqnS%UOgfe335VFMzJK}NHJwEEm)
zD@x2u$<Hl{07;002vtz=CS2q=`Hh4|y+D3WN<50!*b3t_^D;|rae~PBg8bstTO4_*
z<zQA3sNyeD0GSdEA_PGeF=pQ4NiHoaO3f>QC;~eKRI`DLP_PScv4Kr1L@&SV`H~ZJ
zGLwoDOHu=J67y2;g38Sv1_lN;W-dk!Mm|OXCN4$}CLu;KCJts6W*$Z!Mh->^Mkz)q
zMvnhVOj1ldj9iRdj6BRdOj68Jj694yOdO0nOdxeUj9g3{j385(8aSA^n7AO~5IK-4
zkm@*)fy^8rl1YM53}hH97f6B;qLqsoWPDKp$e$eX@wthadGYZ@k)RM1xy4zKnU|lO
zoROJd1d8BWta+um1(lEp0@t3wlO3hJ>x(==K?M(bNLm3$2PnGXX6Gg5rpCvEtJI<l
zkPdJqgCM}w4Tnu`eoARhsvW2fD+Z-p4rWlZ1LOl9Mh+$>CMF&ZE+z>MP9`Z1PL4PZ
Sb`EYqjx;t#7A8J+mTv$^R#^K0

diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index c1db22c..1d66a7a 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -7,6 +7,7 @@ import time
 import copy
 from ur_simple_control.managers import RobotManager
 from ur_simple_control.util.freedrive import freedrive
+from ur_simple_control.clik.clik_point_to_point import moveL, moveUntilContact
 # used to deal with freedrive's infinite while loop
 import threading
 
@@ -35,26 +36,47 @@ you should do a weighting of these points in the least squares
 def fitNormalVector(positions):
     positions = np.array(positions)
 # non-weighted least square
-#    n = np.linalg.lstsq(positions, np.ones(len(positions)), rcond=None)[0]
+    # none
+    n_non_weighted = np.linalg.lstsq(positions, np.ones(len(positions)), rcond=None)[0]
+    n_non_weighted = n_non_weighted / np.linalg.norm(n_non_weighted)
+    print("n_non_weighted", n_non_weighted)
+    for p in positions:
+        print("cdot none", p @ n_non_weighted)
     # bs weights, just to weight the new things more and linear is probably good
     # (altough it could/should be even more agressive probably)
     # +1 to start with 1 (0 both doesn't make sense and is makes W singular)
-    W = np.diag(np.arange(1, len(positions) + 1))
 #    print("W", W)
 #    print("positions", positions)
 #    print("positions.T", positions.T)
 #    print("positions.T @ W @ positions", positions.T @ W @ positions)
 #    print("np.linalg.inv(positions.T @ W @ positions)", np.linalg.inv(positions.T @ W @ positions))
-    n = np.linalg.inv(positions.T @ W @ positions) @ positions.T @ W @ np.ones(len(positions))
+    # weak
+    try:
+        W = np.diag(np.linspace(1, 1.2, len(positions)))
+        n_slightly_weighted = np.linalg.inv(positions.T @ W @ positions) @ positions.T @ W @ np.ones(len(positions))
+        n_slightly_weighted = n_slightly_weighted / np.linalg.norm(n_slightly_weighted)
+        print("n_slightly_weighted", n_slightly_weighted)
+        for p in positions:
+            print("cdot weak", p @ n_slightly_weighted)
+    except numpy.linalg.LinAlgError:
+        print("n_slightly_weighted is singular bruh")
+
+    # strong
+    W = np.diag(np.arange(1, len(positions) + 1))
+    n_strongly_weighted = np.linalg.inv(positions.T @ W @ positions) @ positions.T @ W @ np.ones(len(positions))
     #print("n", *n, sep=',')
     # normalize
     # TODO why am i not doing this again?
     #n = n - 1
-    n = n / np.linalg.norm(n)
-    print("if the following give you roughly the same number, it worked")
-    for p in positions:
-        print("cdot", p @ n)
-    return n
+    try:
+        n_strongly_weighted = n_strongly_weighted / np.linalg.norm(n_strongly_weighted)
+        print("n_strongly_weighed", n_strongly_weighted)
+        print("if the following give you roughly the same number, it worked")
+        for p in positions:
+            print("cdot strong", p @ n_strongly_weighted)
+    except numpy.linalg.LinAlgError:
+        print("n_strongly_weighted is singular bruh")
+    return n_non_weighted
 
 """
 constructFrameFromNormalVector
@@ -207,7 +229,9 @@ def getSpeedInDirectionOfN(R):
     # make speed small no matter what
     speed = speed / np.linalg.norm(speed)
     # nice 'n' slow
-    speed = speed / 10
+    # TODO: remove magic number
+    speed = speed / 40
+    speed[2] = -1 * speed[2]
     return speed
 
 # TODO
@@ -218,21 +242,31 @@ def getSpeedInDirectionOfN(R):
 # --> then you don't even need to use the stupid rpy,
 # but instead rock only rotation matrices the way it should be.
 # TODO: replace all moveUntilContacts with your own implementation
-def calibratePlane(robot, plane_width, plane_height, n_tests):
+# and of course replace the stupid speeds
+# TODO move width and height, rock just args
+def calibratePlane(args, robot, plane_width, plane_height, n_tests):
     # i don't care which speed slider you have,
     # because 0.4 is the only reasonable one here
-    old_speed_slider = robot.speed_slider
-    robot.setSpeedSlider(0.4)
+#    old_speed_slider = robot.speed_slider
+#    robot.setSpeedSlider(0.7)
     handleUserToHandleTCPPose(robot)
     q_init = copy.deepcopy(robot.getQ())
+    # GET TCP
+    Mtool = robot.getMtool()
 
-    init_pose = copy.deepcopy(robot.rtde_receive.getActualTCPPose())
+    init_pose = copy.deepcopy(Mtool)
     new_pose = copy.deepcopy(init_pose)
 
-    q = robot.getQ()
-    pin.forwardKinematics(robot.model, robot.data, q)
+    # you just did it dawg.
+    # i mean i know it's in the robot
+    # TODO: don't manage forward kinematics yourself,
+    # just create a RobotManager.step() function, update everything there
+    #q = robot.getQ()
+    #pin.forwardKinematics(robot.model, robot.data, q)
     # this apsolutely has to be deepcopied aka copy-constructed
-    R_initial_estimate = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].rotation)
+    #R_initial_estimate = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].rotation)
+    R_initial_estimate = copy.deepcopy(Mtool.rotation)
+    R = copy.deepcopy(R_initial_estimate)
 
     # go in the TCP z direction
     speed = getSpeedInDirectionOfN(R_initial_estimate)
@@ -240,35 +274,37 @@ def calibratePlane(robot, plane_width, plane_height, n_tests):
 
     positions = []
     for i in range(n_tests):
-        robot.rtde_control.moveUntilContact(speed)
+        time.sleep(0.01)
+        print("iteration number:", i)
+        #robot.rtde_control.moveUntilContact(speed)
+        moveUntilContact(args, robot, speed)
+        # TODO: replace forwardkinematics call with robot.step()
         q = robot.getQ()
         pin.forwardKinematics(robot.model, robot.data, np.array(q))
-        print("pin:", *robot.data.oMi[6].translation.round(4), \
-                *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4))
+        print("pin:", *robot.data.oMi[robot.JOINT_ID].translation.round(4), \
+                *pin.rpy.matrixToRpy(robot.data.oMi[robot.JOINT_ID].rotation).round(4))
         print("ur5:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
 
-        positions.append(copy.deepcopy(robot.data.oMi[6].translation))
+        positions.append(copy.deepcopy(robot.data.oMi[robot.JOINT_ID].translation))
         if i < n_tests -1:
-            current_pose = robot.rtde_receive.getActualTCPPose()
+            current_pose = robot.getMtool()
             new_pose = copy.deepcopy(current_pose)
             # go back up (assuming top-left is highest incline)
             # TODO: make this assumption an argument, or print it at least
-            new_pose[2] = init_pose[2]
+            new_pose.translation[2] = init_pose.translation[2]
             # TODO make these moveLs slower, they're way too fast
-            robot.rtde_control.moveL(new_pose)
+            #robot.rtde_control.moveL(new_pose)
+            moveL(args, robot, new_pose)
+            q = robot.getQ()
+            pin.forwardKinematics(robot.model, robot.data, np.array(q))
             # TODO: make this not base-orientation dependent,
             # this is also an ugly ugly hack
-            new_pose[0] = init_pose[0] + np.random.random() * plane_width
-            new_pose[1] = init_pose[1] - np.random.random() * plane_height
-            robot.rtde_control.moveL(new_pose)
+            new_pose.translation[0] = init_pose.translation[0] + np.random.random() * plane_width
+            new_pose.translation[1] = init_pose.translation[1] - np.random.random() * plane_height
+            moveL(args, robot, new_pose)
             # fix orientation
-            rpy = pin.rpy.matrixToRpy(R_initial_estimate)
-            rpy = fixPinsRPY(rpy)
-            # TODO: ensure these rpy's make sense (no weird trig messing it up)
-            new_pose[3] = rpy[0]
-            new_pose[4] = rpy[1]
-            new_pose[5] = rpy[2]
-            robot.rtde_control.moveL(new_pose)
+            new_pose.rotation = R
+            moveL(args, robot, new_pose)
         # skip the first one
         if i > 0:
             n = fitNormalVector(positions)
@@ -277,27 +313,26 @@ def calibratePlane(robot, plane_width, plane_height, n_tests):
 
     print("finished estimating R")
 
-    current_pose = robot.rtde_receive.getActualTCPPose()
+    current_pose = robot.getMtool()
     new_pose = copy.deepcopy(current_pose)
     # go back up
-    new_pose[2] = init_pose[2]
-    robot.rtde_control.moveL(new_pose)
+    new_pose.translation[2] = init_pose.translation[2]
+    moveL(args, robot, new_pose)
     # go back to the same spot
-    new_pose[0] = init_pose[0]
-    new_pose[1] = init_pose[1]
-    new_pose[2] = init_pose[2]
+    new_pose.translation[0] = init_pose.translation[0]
+    new_pose.translation[1] = init_pose.translation[1]
+    new_pose.translation[2] = init_pose.translation[2]
     # but in new orientation
-    rpy = pin.rpy.matrixToRpy(R)
-    rpy = fixPinsRPY(rpy)
-    new_pose[3] = rpy[0]
-    new_pose[4] = rpy[1]
-    new_pose[5] = rpy[2]
-    robot.rtde_control.moveL(new_pose)
+    new_pose.rotation = R
+    moveL(args, robot, new_pose)
+    
+    # TODO there's certainly no need for all of these moves bro
     # --> now you're ready to measure the translation vector correctly
     # for this we want to go directly into the board
     print("i'll estimate the translation vector now")
     speed = getSpeedInDirectionOfN(R)
-    robot.rtde_control.moveUntilContact(speed)
+
+    moveUntilContact(args, robot, speed)
 
     q = robot.getQ()
     pin.forwardKinematics(robot.model, robot.data, np.array(q))
@@ -307,27 +342,28 @@ def calibratePlane(robot, plane_width, plane_height, n_tests):
     # TODO: get rid of all movels, just your own stuff,
     # or at least shove them avait to the RobotManager
     # and now go back up
-    current_pose = robot.rtde_receive.getActualTCPPose()
+    current_pose = robot.getMtool()
     new_pose = copy.deepcopy(current_pose)
-    new_pose[2] = init_pose[2]
-    robot.rtde_control.moveL(new_pose)
+    new_pose.translation[2] = init_pose.translation[2]
+    moveL(args, robot, new_pose)
     q = robot.getQ()
     init_q = copy.deepcopy(q)
     print("went back up, saved this q as initial q")
     
     # put the speed slider back to its previous value
-    robot.setSpeedSlider(old_speed_slider)
+#    robot.setSpeedSlider(old_speed_slider)
     print('also, the translation vector is:', translation)
     return R, translation, q_init
 
-if __name__ == "__main__":
-    robot = RobotManager()
-    # TODO make this an argument
-    n_tests = 10
-    # TODO: 
-    # - tell the user what to do with prints, namely where to put the end-effector
-    #   to both not break things and also actually succeed
-    # - start freedrive
-    # - use some keyboard input [Y/n] as a blocking call,
-    #   release the freedrive and then start doing the calibration process
-    calibratePlane(robot, n_tests)
+# TODO: remove once you know shit works (you might be importing incorectly)
+#if __name__ == "__main__":
+#    robot = RobotManager()
+#    # TODO make this an argument
+#    n_tests = 10
+#    # TODO: 
+#    # - tell the user what to do with prints, namely where to put the end-effector
+#    #   to both not break things and also actually succeed
+#    # - start freedrive
+#    # - use some keyboard input [Y/n] as a blocking call,
+#    #   release the freedrive and then start doing the calibration process
+#    calibratePlane(robot, n_tests)
-- 
GitLab