From b9c121f44a0f2e2c8fc6ec7209bf3c5f96165e2b Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Sat, 18 Nov 2023 14:14:57 +0100
Subject: [PATCH] comming for saving

---
 python/.README.md.swp                         | Bin 12288 -> 0 bytes
 .../.drawing_from_input_drawing.py.swp        | Bin 28672 -> 28672 bytes
 python/examples/drawing_from_input_drawing.py |  47 +++++++++-------
 python/ur_simple_control/.managers.py.swp     | Bin 0 -> 36864 bytes
 .../clik/.clik_point_to_point.py.swp          | Bin 16384 -> 16384 bytes
 .../clik/clik_point_to_point.py               |  18 +++---
 .../{boilerplate_wrapper.py => managers.py}   |  53 +++++++++++++++++-
 7 files changed, 88 insertions(+), 30 deletions(-)
 delete mode 100644 python/.README.md.swp
 create mode 100644 python/ur_simple_control/.managers.py.swp
 rename python/ur_simple_control/{boilerplate_wrapper.py => managers.py} (86%)

diff --git a/python/.README.md.swp b/python/.README.md.swp
deleted file mode 100644
index a6c83f7581c07662841f1005cf20654e377c8946..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 12288
zcmYc?2=nw+u+TGNU|?VnU|`7Wj!2yzTEh@FlaV1kzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7T=?A$w
zy7;>4<)%Ok8zo0WU^E1X4}sE>G+hf`24f>b1CS<VB}D~cp->QW6px0$Xb6mkz-S1J
zhQMeDjE2By2#kinXb6mu5GW~NWT<CgU|@p!R{=^hqR~+9C^Z@aqaiRF0;3@?8Umvs
zFd71*Aut*OqaiRF0;3@?8UmvsFa$#&F@=F)9w!5X6&GawAJ+f>!Oy_(j-P>H8b1Sr
zH9rHx4L$~j4n78kEItMXXFdjo=e!IIJ9rrww(~MDtl(u}n9a+;kj~4%pv}v`Ai>MP
z@Slf)VKol}!zvyIhB-V843l^m7_@j87@l!6Fs$KbU<l)8V36l#V7S1=z)-=(z);S`
zz>vztz!1s>b;}S8%TXtfhQMeDjE2By2#kinXb6mk!0--%#G+J%%shpXj8uixibT+g
z&0>YL{G623A_iSlz@V&9RGOEUn3R*MP?C|EmtM@kprDYGnN*Zml$lzrP+FXtqL7hV
zl&YtYS)x#$nUkYXoLK@EQ7FwT$;?qmElVw`1ZxGU$WKd4Eh<*XNGw)JN=?mEC@D(J
zE6zzQNlnpX&{fFH%Ph%E%u#?W8&xRI&nYd*%+FKEELKQNPu0!ONl{47Pf1lM&&W*9
zP{>P7O(|9=$yZ29RVYf$O)Mx#O;O0qE6E3Yj6ngaEVCHQFUZf#D^bW#Q^-gx%gjp$
zIVLeD2NVoY+Y<9C%M&XZ6u_#%))Z&tm*%7>6qkU!m6WOgS=_3SnwMXio&mBrF)b~>
zC<UycurxC{JGHn(p&-AwI1?0H1x5KK`6ZPFnR)3t3<?T~c_|86rNt#6`$1uunyXNr
zUzDwoSPXJ=YF<ieik?C~BvSHAGE$2aG7|GrKsqyv6;kr^QWeTmb8?`T>nK#_mntNK
zotK+gp_`bOqMMsolANKCk_igo)Vvaf<ow)%{Jhk>l41s3h5VxQ#JtR^)D)0?iO>kF
zR7g(D$w^I7z*xAeqmY}J0!kw#`3i}!=qN}`&Q45E)dTxCBflUO6fY$ii6sh|X`sYW
zj7U5T$_go|#mPmP1)%t2U`Q*<&jl+{NXaZpO)kkVs?<}+%P$8T1vM9BT53vW2`H`U
zK;sS+4yo$J3W*BI8HstxsUUBwrzn&s=9MrMWELo7<`tKKVo_JOv^cd$K{r)FPl2H%
zBQ*~$Q=VCpp}+u%Vq*o}Tm>|31?fc~pMpe-Q%gz<O7in_iWSN;Qd4sjl1ejkQW(-Q
zi;7D?7AyFM28Sq=7NjJWq=JmmQ2@tNd1i4cC^ePm=c$)~<J1Zf5W2d$49YOaBtim1
zK_R6y6`Xt&ax;sQb25{&LEZ)BFmP&7$S*BXNGwV(&4or@c~NFbszPExK~a7|Q6|Xt
z#Ny)AqLN~T{Ib*{kVH;qGAP&(x)gN5E>3~CDKW1EWM5KFez`(%er_rxJC+tO=qf<W
z2H99rS&#}!iwb3lMVa}f#R_SqdC4Fb7c(d*KynW#D}mB@IVjS>DKIfl50qmPvr`q4
zL8VGQC?zD8C?usOCzck2)44)^UQQ)ET_!_QO{#)tfI?zQN>OSt*ivvhN=__J1({b`
z49cNkR~EzaL{VyDjzUp>Qho_a#xF@NE=f&M$W6>EP0Y!u)KN$(EdgcJ(j0JTgYtK1
zP;q81h@W4i0LjvDe}i-@q~#YuN-2fJob>#n%#w^;1zkv9Eh^0eMQwhbLNU0QR7goI
zNd&nFY9gp`0%h!i%)I>M<c!RGP$`z44+`u=g@U5gGH^har4|)u=I7}s6epIYDkLQq
uXC^B^#XtoTECr+%r9#{R_BbdzWacS=GJaZSPAVcvF(@m9_`CR9F#rH$Nkr!W

diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
index 777277a1dd36d736b472c96862d313480e100de7..cb6dfeb6f1e8d357a11c2c00ba7b286da2049046 100644
GIT binary patch
delta 1575
zcmZp8z}WDBQ8dXQ%+puFLeGeSfq_AQf#JZ8h*YnQqS6A4(vuAZ>>0Hu7YfKT+H9UE
zP|VND%*w!Ue=?`SWJ?B428RC}3=G>j7#Q+77#P$!7#Mgt7#RMtGcasqXJDveXJ9a6
zXJB~A#=tO>je#MWje&uQje+6*=Bo-^tdkdNh)n($*S5J<?GO`lPG<IIJI!NkyveK#
z4Ebyjci2o8G)S&r#lyhR&BMS@$-}_l#>2qC%frBMlbeBI0XGA~EN%vdncNHv+1v~a
z(cBCSHrxyh3fv3~Z@Cy44sbCr%;sWXXy9UCNaSK*Fy>-lFydlh;OAms_|3_{@R5^&
zVIwC4LlGwfg99f610yE`!yyg^hHef91{)3r25SxmhG*;y!VG=v3=Hw?3=E>|3=D_a
zK+fCDW|+-dub`l;piz>Ms*qHgu8>)*kXfQoo?nz*tWcg=lA!>SNXsnBEl(^;RVYg>
z%FNHxQAjE+Q7A2nFDgk%RVc~G%*zJrO36<x&QmYZWKd85nF&*nQktqzlCO{mHoqhz
zwJ22~GY@QTa%oXfYJDEUENz9N{G|L61!N10GxPHlphi0S1p6!a`G>e#flMqd2HRe&
zP?=w<ke8a80uoHlNX$zIiGy7W)|{D_oKp&76)R-sK}{;nOGzzKC@s#+n>-=s%;fhL
zvGwtJDTzfziIobr3V8*3`FW|u8fKabS_+9p>BV}9$;qiXsYQt;nfVYSAb!M<0|g)`
z$P$xFN)vMwauX{ub4znUE>tf{1>2Vnb^%0tNosKkI3zOjO6pUK3UU%F6_Qd*%2QK8
z0hC#+keHVOmM<;<d9OGpGbObMuD>ikH?abjtHA-O2U1X&0@tCefNUeGg(9GsM7MEr
zr=|DiUzWR=SaLG6<2P@%2^8j?$HBm$#tBK<(vt-P^qrV^85qv+Fff$zFfc^(Ffb_d
zFfg3tW?-1e&A{Ns&A=eX&A@P&i-Dnsi-Ez9i-AF$i-F-bCj-L_P6mc}PLOGv8CjNa
zOs<V&pPZ>8vUz@_6!YZ89wN+!sk)O5Jwzv;iP09-gGPK_eu+Y9acYVJC?!oU^ih~x
zt0HZqtdLovUaXLqQ=AV=UEmN<$W6>kOiwLRNG$`0X?|&X1}KFWXXKaWq~xiWC?utB
zwu?Q+#GA*#z+eXTmiA=91mpU9AaC(9FdXJ(U^v9fz|hajz|hLez>vqwz!1jEz#zxV
zz`)7N!0?5Kfnf&^1H()n28L!H28LiB1_nMJ28Q<_Z*wy+v~e>qWN<Su*l{y3{O4j|
zIL5`mFrSNoA%=^A!Geo{;V&lx!vRhPhLxNQ3>};d47r?Omxwd)b22bEb22c<b22bo
z=U`x{=U`ya;@Hf{;><p|*O7N}uVdKcKF9FMy^i{vkOZZxkT$v3QGIi-qZ!j=mFzWE
z3K@xcDLJVM1&PHa3Mq*tiJ;Ia&d*I%$jeMlRVYuaR7fkz&jkmjLVl4#YEE%#IVfc9
zCm(PL5depwUV3Utcu{Iza)!oaTURIELIqn$J_SikuFAC$f+$NV0c95md-9%K4K^tA
zzMD>cN^XIULUMjyNl|`IPHK^kLZ+q_12|n4Bo-Glq@<=H$!cg?F(@mfrKYB&<rkGF
z7NsbZrRL-(XO>jzfJKuMld}~H@{2P;xe=U4((;RvQx($kixe_*3sO@O^O92;7?crU
JvrgU>MgTplu$uq?

delta 996
zcmZp8z}WDBQ8dXQ%+puFLeGeSfq_AQf#K}hh}3BtMWqE8B_|sS*fZ)*E)<YwG~PT>
zpqQVPot1&%@nlYg$(Agf3=GVi3=F$D7#NB<7#OrU7#IXO7#J8i7#Oy)GceS#GcZ`O
zGcdemV_=xe#=sEI#=yYF#=!7+^Hl{d*2xaOERzd0L?-`>Yuj9@c8F=Sz2;FiUJech
zh7TMNcNk9=G)S&r&%?klfro*ij)#H4n}>lxh=+mU9ybHSQf>x@`P>W)^SBup3b+{<
z61W){9Jv`7RJj=#K5;QH9OYtQSir@=(8|TYkjBNpV8O+}V9v$BAi~AKz`(`8@Qss!
z;Q}WE!vszSh6GLq276A3&F?uF7~XL(Fx=q)drg>O1_uK}5(fjrLv{v+Q|t^3i`h4`
z8D_Ii_BE3*U|>*CP*6zBDac4vuvIY7GcYg)i+~ACVdcplW-7uNnZ*i;c?yX|>7}`;
zc_kn-CVQAEvO_gZ_ArwHspEjDo9t$$CYzC243$T>OIaZVq+ANO3Xn7(%ruCtlPxX$
zLBc|)!XU>?_AmpvQ&9-hzR6Y=akdJ!3V8*3`FW|u8fKabS_+1G=3teekSL4KO{_py
zV4!EJprue;keZqjKY5j<w<1WTvO<Wzi@!onYGP5ILP<udLT+M7hC*UaQEFmJ<>n8T
z`<XUxu?Z071*NhUPDp}~oGciiZ_B~Uz_62tfuWIyfkB;zf#E$j1H%$-28J4L1_niL
z28LH$3=C_y7#K>q7#J*}5xtj_fdOPn?dHaSr5qC<M6-k3P>@(Od1AEmW(!3@#?3)d
zoJ@?$lS^Z4g_Lv@Qc}|rOLIzW4O4ZEH5rsa$|l#UNN=`}J<7y;h@F995;T$OP8Lis
zuK&c#!0>{Xf#EtY1H*n^28PMJ3=DO=3=A>63=HhN3=E(|+`z-Y;KIYez{A79@Qs^+
z;TSgq!*Xs0hH7pG20LyB1_f>ghCf^o_ut@RU^v3Xz%Yx8fgzfUfkBCjf#EkN1H%qZ
z28Jn|3=EN+3=A@y3=FKC3=B^=7}yz3a)6w)nUUoL`{rIpXST@>8p@l0<Z&<p0P!~2
A%>V!Z

diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index f1b6610..14e04c8 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -31,7 +31,7 @@ import matplotlib.pyplot as plt
 import copy
 from ur_simple_control.util.get_model import get_model
 from ur_simple_control.visualize.visualize import plotFromDict
-from ur_simple_control.clik.clik_point_to_point import getController
+from ur_simple_control.clik.clik_point_to_point import getClikController
 
 #######################################################################
 #                            arguments                                #
@@ -56,8 +56,10 @@ def get_args():
     parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
             help="whether you're using the gripper", default=True)
 # not applicable here, but leaving it in the case it becomes applicable
-#    parser.add_argument('--goal-error', type=float, \
-#            help="the final position error you are happy with", default=1e-3)
+# it's also in the robot manager even though it shouldn't be
+    parser.add_argument('--goal-error', type=float, \
+            help="the final position error you are happy with. NOTE: not used here", \
+            default=1e-3)
     parser.add_argument('--max-iterations', type=int, \
             help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
     parser.add_argument('--acceleration', type=float, \
@@ -122,28 +124,26 @@ def get_args():
 #                            control loop                             #
 #######################################################################
 
-def controlLoopWriting(robot, controller, i):
+# feedforward velocity, feedback position and force for impedance
+def controller():
+    pass
+
+def controlLoopWriting(robot, dmp, controller, i):
     # dmp
-    dmp.step(dt)
+    dmp.step(robot.dt)
     # temporal coupling
-    tau = dmp.tau + tc.update(dmp, dt) * dt
+    tau = dmp.tau + tc.update(dmp, robot.dt) * robot.dt
     dmp.set_tau(tau)
-    q = rtde_receive.getActualQ()
-    # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
-    # you already found an API in rtde_control for this, just put it in initialization 
-    # under using/not-using gripper parameters
+    q = robot.getQ()
     # TODO: document the mathematics with one-line comments
-    wrench = np.array(rtde_receive.getActualTCPForce())
+    wrench = robot.getWrench()
     # rolling average
     # TODO: try doing a low-pass filter instead
+    # TODO handle past data in some nice way from here or elsewhere?
     wrench_avg[i % 5] = wrench
     wrench = np.average(wrench_avg, axis=0)
     # pinocchio model is 8-dof because it treats the gripper
     # as two prismatic joints
-    q6 = np.array(copy.deepcopy(q))
-    q.append(0.0)
-    q.append(0.0)
-    q = np.array(q)
     pin.forwardKinematics(model, data, q)
     # calculate joint torques based on the force-torque sensor
     # TODO look into UR code/api for estimating the same
@@ -179,7 +179,7 @@ def controlLoopWriting(robot, controller, i):
 
 if __name__ == "__main__":
     args = parser.get_args()
-    controller = getController(args)
+    clik_controller = getController(args)
     robot = RobotManager(args)
     plot_dict = {
         'qs' : np.zeros((args.max_iterations, 6)),
@@ -202,8 +202,17 @@ if __name__ == "__main__":
     if not args.temporal_coupling:
         tc = NoTC()
     else:
-        # TODO learn the math already
         # TODO test whether this works (it should, but test it)
-        v_max = np.ones(6) * 0.5 * speed_slider
-        a_max = np.ones(6) * 1.7
+        # test the interplay between this and the speed slider
+        v_max_ndarray = np.ones(6) * robot.max_qd
+        # test the interplay between this and the speed slider
+        # args.acceleration is the actual maximum you're using
+        a_max_ndarray = np.ones(6) * args.acceleration
         tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps_tc)
+
+    # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
+    # you already found an API in rtde_control for this, just put it in initialization 
+    # under using/not-using gripper parameters
+    # ALSO NOTE: to use this you need to change the version inclusions in
+    # ur_rtde due to a bug there in the current ur_rtde + robot firmware version 
+    # (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..a43d7d58d3891f18e11ae16742e2ed16edc12893
GIT binary patch
literal 36864
zcmYc?2=nw+u+TGNU|?VnU|^WLJtB2lXbnTyOh$(E{NjTAl*A&C6h54rSe%`ihff7W
zt`26Penx(7s*ygNn^T&XqF+#ySdm$*Us@DjoS9pYlNz6#pI1_ppQB$;S(1^TheaYc
zF)uMawWwIHpb}!iC^;GeqalDj1WHTNbS-!pjExKpKnj$V6cvPpLP5+?JQ@O{Aut*O
zqaiRF0;3@?8UmvsFd71*AuvKhprnAEp`L+(feGqgaOZ%5fq@aqhtcd%eh8Enh0-v2
z7AW5rN~c0;m^>Sl53^?rln;|<h4Q_iG`c(oR9q5D3qdtPDF$Y!xDu3RgGxXtsOTs)
z8UmvsFd71*Aut*OqaiRF0;3@?8UmvsFd71*Aut*OBQpdNQy3VIvNJF&1a+_l7#Lvv
z|J(cw42Ss{7&h=TFf8F`U?}2eU<l)9V9?-aVEDzyz;KO^fnf?C149QN149)b14Aqy
z1A`hL1H%(u28JuV3=Eff85qv<GB6zEWnkFI%fOJ%%fO((%fN7thk;=#4+DcQ4+8@$
z4+Fz-ZU%<M+zbrC+zbqQ+zbr5+zbrwxEL6Ya4|4cb1^Vza4|5v=44=4%E`b`$jQJ^
zz{$Xn#L2)A!pXp3!O6g2&dI=_#>v1S#L2*Lm4ks{DF?(a?>QJ47I82zBycb=m~t>M
zJYr{H*u&1iu$!HMVKX}e!(w&@hRN&<46*DC4D9R-3{Tk@7!I&8FzjGsU|7n=z);P`
zz+l71z#z}Y!0?xqfng&n1H&R#28M;K3=A__85kzAGB9+qGB9MZGB7x>GBA8%VPH7V
z!oYBxg@IuU3j;$t3j;$s3j>2S3j>293j+f;3j@PjW(J0H%nS_cnHd<0nHd<Om>C#6
zm>C#knHd<qF)=V4VPas|#>Bvo&BVaq#KgcL$;7}Q!NkBI!o<M9#>Bwzjgf(22XsI~
zSD`2~J)=Y+FTY$NvqT}YSRt{vxHLC4MWHgkRJ|xwp{O)3FEcM)p(G=-SRp@8AvDM|
zRiQXvp*TM`l|dI3Fcjq{<(EL0Q&=%5DJd~9C?J92)SNWE;*$IVkHox`oYW!>zx=#Z
z9R(1n30IhzrjVCkq5xK#Sd?C@SCE;PpPZbLnIE5@ms4p4R|qn=peQr1L_^8V(bLCD
zAtf^fqO~+RIW<KgGfzoJAvduiKC>jXD6u3nKd)FvK?%W8LfDy_Q=E#`;9`(b#l;Y_
zGEuZEBI(aeQ^-`Xfw&pfMg?6%RD(gTF3HSI)ho_PO)by>do!g(L02IqGc66_B9Jmn
zSAj%9%ULq>N>fq70VDwpi`>lO;?xv{l+?tOoXosbg``R)9kAgFx)2AZl%QIZnFbSA
zumLMXbuh?X3bqQVc_|<RN)n4oP;3T?+bV#41R^w0%?7zEsVFru8%0lMngUqREhjM@
z)iSuSf-S^RAm0|{=lJC37ieUnnq3TXivo7L)AEZHG8HoO6p9k_(o-R!j2bbT5Q|e%
z(?CfatO6<oO12<(gHwEFu|i6IUaCS_W}-q#Mk>PHU=L-Ms278Uk({YstWcC%T#%nv
zoSBrFlUY)!P?8VPrT|JNB?^hj8JVeNpacriot~eVQ>+IzEitD!Um*jWHNb}EmF6a;
z7AfSXDIgLpL}f}oD8#|pG9To8g~SpCQv(B!Du}eMt}fWXqS8Dt4e}9~0rFOAMP><F
zEP)dtIPb<oGp}AzNeX=BlwNT`YHEs?Mp<f3d~$Azj)H-np^k!~o`Hfs$X3)?fhtn4
zRmdyQt4b}(FV-+as6dMsLjwb7po3fnNx<sGsd*_NuNfK`D1dFwDpp7<&B*};1lT}@
z#Jm&*kT){(($yhmgQ5|+U;+6^N1-?~J+CxZM<J~!F*jAyia{A<i$ifqVo7FlZfZ$J
zehN5LK*0l&Q3jQ3;3$PC(NQQU%1_Ek%~dGQ&nZiVC{|X;Nd?7QQmR67eo;|satSC}
z%TpDS6Z0T$Q3l(am;;T<B87~^Vo(~$NGwYQ#cOe@f{{XEQF>`^YF<gPf<|gtYMw$#
zMt*5}21vRRRO*6Cz|!K>6di@6Qb>MN23eq9q5w7p<UNJNVuiHKqT&*`Svm?ysmY0@
z#b8?$z+OWtpTT^jFaq;IUI#NE{x45e&&g3JPEA!P&q&QvC{I;LPb~qtJrm*xh^ovq
z6n7?rl6p#NkwRu(Mru)J2`KJBF;rZTnw*)KlUW540B5HBB8B3T429y%+{~QBA{``)
zOEOZ6Qxy`6QWX+&azLgl6es4UDrn^97nkHzYC@7V#P(uPFlLs36=vosBr2q2CW8w1
z#G*=23du`JEGkN@ECwf%qSTVoB2eHamVkT;a#eCsYGO&MLUKlCPKrWuPGWI!YOw++
zSr#M~m1HKD<|Gy=fU`oe9)ojfi9&v93D`YgF<r2q(lV13K&p$ulKJ4!P=IC}n2|c@
zbt5Py7;^GKHEC*5u0mO6ab|uV1A}W>YEflL1}KO!ixonHKq)>sGc7Y&PXW~ghUC(s
zqSU++1yI^dh1IJi8HptdS*67#3Mu&tsTGOIB{`J}<siYz{8EMT#5}O$b3sv<k`E1U
zkY_<fSy5_Y4#cbAuqsO|%FHh<2E|Tk4y3wND9;2(S6+G!D7EM2Cg!CSgH6#@$jD5A
zq_f1ly!=YA`_ptmHC8bw7C@CoD%iP>0iK`&v9L6=C^ZF|C?Sa$7Sk|`3lc%uv?R5t
zSRpSp5#*8lqV&YP%&OEBumdym6hI|$a%x^lQDTllPJS}jl~DI2<!9!k7AX|ufMYj5
z1(cfdOY>5|kq0fF6>{?P3lz#TGLtjFEuEZlP(lHP1t_&Kgk+>D6y#^-m4JKzNs-W4
zEe0tsffdeH3?ZP#P+BH9RPss^GxLfS5}`o})d6;Kq5_J=P$@kIhLU`+vyxJaVRk7b
zmX>7X7pWI3BtmKpJ+O1qGV>C16jD+_C7D8UMt){;YB4yQ<R*fGKVKm+IXOQ!H$MgB
zYlxFU4Kq+pT&$2^P>^2?iWzVn!w`~K1j@~&#h?%dWi^=NQuER?^HM<zU>TA#K=oy@
zLV0EmsQdyISWr74R%C)pgOZHY%p!P7RVXOR&q_@$fw&lCJSenMD+*GROHxxnNv|k1
z5fX)^c`2zy#U-G82nzMoypqf!P|8Y4O-d|Ig%w{6`DLj^Ih6{@`JnZ?6$*(-#U(|F
z;7kt5))|RqAa8+;NCwArF({yuGeJd2u|h$9adBo+PO2UQvI59rU{Ik6msjx21LtE%
zuGfJyaSKw5AQ4)UuaH=lp9!jp7+{Uyd{9}Es({EjdJ3RKQ=FQdp9gAkfujjjjU{Im
z<fMXv0i0;cQb94Ep0AKrl%K1BNEL}W>G?&OB^kM(hIK(=L23~wv_P%~rALsTKns@@
z5Xzuw0#xWj(*(jYa5<D&tdNtLR0JvuGr^TkZf0I)Ze|rEtAdh+LSAZWN@@yf;i9X{
zke-^C3d&Y_`6a2vkfwWPZb5z#xOyo_EGkZA0J|PzUA10mNoI~7xOmJgj88AhECBff
zsy7JKA;>Irhlt`eAw9JuJ~uxlHAexe7cRh{tPtYw;;)dKm<{nqL1J=tVtT4Vd45qg
z15`CA=o52byyX0XO1!p_;9hW3C^H}Cte_AVS5JS>ypq(Sw8Ug+sDRZJr6#9lmZc&q
z41x$@Qw_^~Q1^q3b%v}|N2peYN~Wh4q?VNA>m}zGfig-_S!xl)cksTEURh>wDX6lA
zTkj6h69yA6f`ttz6~NeerMU%_piGfh026{62@)>I%!7q~a!zVu5xB-H&C5$oPAx75
zr!sIFDP~AaVPGiaU|<mCgslIE&7ZgNGcaiKGcbJTV_=xc$H0)o$H3ss$H4HFmw{m^
zF9SmcF9SmWF9X9f9tMVsJPZt5co-O3co-N0co-POco-P|aWgPn=VoAN<7QyU<Yr*-
z=4N2v=VoB|%EiF2gNuP-6&C|TC>I06e@+I5t(*)DO`Hr2?wkw^I-Cp);+zZ&Vw?~+
ziGbWSGKXeHeLWfiqaiRF0;3@?8UmvsK;IAm4OxJ25o9337t|92cUVD!pivw>u#keS
z0+@zNDTBs+6u`avT=4jO613%8l$w@W1nNnH#|(57;3GCjTK)V(Top3abHRh*DXFOi
zpw=*?!vq<_FDNZ4$S+QXYXLb1hCzKZSX&aJCshIDYRDj(o<eYbQBkFyf^&W@NVYgX
zKTi)j1_v_%Mkl2zq-9n>dVr8YsZ8jIm7anlXs8p^69IL23X1a6ixP7|!?obPPfjMN
zm#7ef;=ZCpQ1{L+zr+*NDFh9jrKY$R73CLcs0SpLWPpa~!F}q|qLefR&_Gi@XoL^s
zM$n)HxYq-+3t=co7%~P7b1<kcRidE;9;(VLhWHv3PN1m}g_P8^%)HbTs~FI%0ywfD
zqkS;VFd969jpA~c3|Nz1Nl{{6F=TuNGDZp>uF?PnaePi@G1}NXNN;AELaKtTt%6d1
zwi2p?L9!qWad;|r2SFs2K>gIryn<5j91m!CF%#04h4<6b@{4j4OY{^%iYgTn(-SlE
z^pGYvK(@eeDrjuJvRE&*A~`-YFD)N!Fd3vQwIVsSpai>P6%@cB3-)YuW~_p(LRwCK
zVhJJyqBCQ$`vf#_g*2$2lbVOnq^XJBHh8!y*n+LqD=x^%EYVQc!I5Y|o&yCmMhun|
zp-nx3{0TB$!Bznrt{O@KIjNvgj8st95S$iEAR{iRc`3T7X=$m+CHY05(TPmZa3w6Y
zSLjvfRO(ggROwZLh9PqkORN;o0=_&0G}07OREjo7r3@PMO9YK)<)lJ}wo3C#GC{+q
zncx8{aIZZDHcXvUiQ*?^Y{?N*OdB+e4@siY26_gdv3~;{1ziI@L$pXzh76y8CJ;g6
zBgOf7iAg!B3gwC5`Hz%*(ClJKem-bMt}+SSNzF+G<zkRHXw0q%#dTnZDA+21N-jNE
z5vJi=lAoW0nl_X{V|9>;l45W^g%v6#l?AB^nUMJpP*j01WE{atAu%NdJbwu-Lko)X
zlM<71Diw+fDnW6ekYALUng^OC$j^g~NhyHm1wh3UD3VJQAXcKdSy`bdH8(Lc4>D>B
zQVJWgMKTXX2gps(><OM$4RH>zQqob-$}7-=1Ua~dKptDrOHVCvOfD%+%n5N02*@u^
z)zH+`D*}ykYM5xEh78DX7}f$Cl#*DIsF&}X8EqDeTnxeW>nLayWajA=6;$fwCYBUs
zR)pjS6;y(!#`KW&7Ue_22H9p%|KE>+fngqKURi*F0k(es4nG6K3VsHL0)7St6MhB;
zaefAd8+;558~7L)a`_k-jQAKBc=#9?F7PriOy*@^=;vi%sODv0u;FE3;0MkB^Dr=M
z<zZl$!^6Ok#lyhhz{9{G&cndK#KXX_gPVaNmz#mXjhlf%o11~*8W#h@1TF@KdM*YA
zH!cPSF)jv%-<%8#8#oylCUG(_<a07G1aUGjNN_SRyy0MA*u%lVFo}bKp@@Tl!Igu7
zL79Vr;VwG^!*q5AhG=#M20L~J21a%UhU;t$4C~n#7#6ZIFvPPlFvzkoFg#^tV0gmH
zz_6c{fng~t149BU1A`<h1H(D!y!}KL28Jva1_oOe1_n+R28N@|3=CDw3=EOX3=G=L
z3=Hhd3=C(O7#L<TF)(C6=jhinLc(t@BLl+{Mh1pBP*{T7E3kM}RshxQRtlL48Hu2g
zZO|AWc<vyvM8ODDqU4ukq!uZF21PLv8psS-27pT{D`ci*gYs)mP9=D*36jo}QcFr8
zgOT9rCUA|IlV1)V)J`fbD#}bv0cVur#I)3sO0Y0^KurNO!I4@9n)ED6O)SpOgUl8{
ztj<jYWqicE1ZX%NG{BsZnyZkJnVtbQCONSHTyz$JhQ=Y&51`3Mg<^1{z#hrrpdmpk
zkXq2FFL>ezG+CsO3>m-8sRWyuiYQjVSpanoSiu%FcUG*2Dhjs)<ZQ4ma1PAO*DFpf
z2?n_)80;7g@ahEc3}rmTd58=MUBm!Z0UuC9TfYD{Ix#spH3zyp0p=WJF}P!3QJ0*N
znw$-38G>tRBn9QEu&Kl1M97Q`Xri((B?YMvQ^+jNQ!i0S$pp<@=2R+ZBo-7Tf+lKm
zDiN*+O^QJVev9>x9iw1lt6->S4vr|W*H9$w6b$qXkemZ*Ny1v8$vK$?1)#=QW{D1{
zm<BmCEwch5f}{tOI-rRU)J91M4X5U&DkK*rR#hq#mz1WZ!4-ppFQr7mR>4p~AF^1W
zv;Z^@5?_>9f@s@<l_5ze*n;M)6_h=yP)+jk_w);i_jFOPRWO6gfM<lkt#EL04jG<>
zPRFDcDWv45f<qHt=VWB&Wh*2mf#wX0OOrDcl91CsC_IWY^O8X=c}N9anwObZQkq$k
zS(XZF$%E@9kVTMO44M}M*P$i(u&Fx*1!aYt#NrZ#EbufHY`Pbo!aybzD}aWGA<cEr
z5UxQ<21Gex5)~XKdGTOFiqSk=iZrp4o0y)N46W*l6<~9YplqQ432<n}P_R`14PPUT
zifbqt8ky@E=o#o4qBUl~c7yf7hRY$E(MQUOF#$G&4lx0J^c=ehn9Y8Wf55JS&Ryt%
zrkctUOH#2kY>`_zNjdq+*`O4hoS2gXPck6WKp4fu<b2SOd5MOSp{0?Yp_zrAp`M|E
zk&=#rnX$2{5hCM(G-jqj3MHs(P@A=2Q^BE^nU6KpQOrj*7@?P7$P;A>!O+KHN@@;r
z=~$eZ3!QGp;?qI}TLn;)BrmlTH0KLxihw7H5MF~VUxH@>Y{e|X7FZ#R5P?UXvO;EF
zW(j!7LMph7gGL*ul2OP6wdz3=K8P$0iY;)ULZb>4At>`*NC^n$N~qHKw4B6rupOXc
z4r~Z6_k(OvF3YUaOUW!Q$Vsf!C^SHL9xj=apO_MsnOdG&<O`ZZL$VpJ1mro05-hR`
zwh9;%MjGId)=@~#&&dHVSAfjV>3|k5lqTjtcxY|`O)f!~wi%;IVVT6zD+aCMiZ4zD
z&1j*!3cRohHWZOs1erVqHEk3$K!FCD`~&5_#FAW8iwY8pQR4^{0w9cUK!WhF2Gs{3
zU>P0mMbr$S0H6PaG&JDK5WWSc8)zhf*%&be7KP}9M3)Yv;08746>O1)Sd<a53{I91
zJqlpMAuA5@K#MOh+zc9+a7)ZV8cb4F0HsBUPI%S=y8|WdLz`YuF;s=c@G=2UZUq~O
znrUFFQAJVB2DMwD>cA49m1FVopu7<u4=!JHz{M|gI6+w<7dHF`SsIa|0Pd`U)_j56
z1;weUxy1^(`Je`9K|xV|K~W~G^#N`?Bx26OC+6mtLe`jo)@MT3{wYIN@qp4Ucx4lK
zHXpQx1-48qBNb#tUW!6-3COY%P^&o+)XirAFJe*v&$RnOPBMcmy;T5@A%gp@ki`Pf
zMtfRfaS5n(otcuF2pSRtpOOaZii4NaD1cf|dJMYo?mWDA49aw%)(N<ootIj!3tsAx
z2I?!P<Wz#z$d!TyHbBd|62V<mP>;Ga556`LoUTBIfI<t@7)b<oT*0L)tS&)XpaWS5
zSqxj~SPWV+lUM?3JTfSNjVb{xQA;VxF8~c8Wafd_)MOSbfLsm=EAU7}c~K&GIYL@#
z9(er-NV`HoX$gZam{nesS&|B3f%;6~Q{q5th>BA|OOZ-ab1Ff8$uELLO>stkc`B%Z
z1s=Xh1P`<1rGv&Dp({r+ixnV8%qb+MfqGAnRspyVlbe_aYQClx6zeGDWM-!-q~sQW
zLLjLWygo|-92KC>Ts~|O9mr3h@h(tUK^DSfCMSZI(}4PW#SFTjU@ZYHLn<x-ErkF_
zGk76bq5@<o6l5h-QEEyl#OL5-!k_?&SMd6tq*R5}ijvg4l++Z3d`P=AwI~&|1PL-+
z3bH@51msTCh9GF|5ZEfvXkrOyWJeD?|E~{Pj}F?vFTlV6+kgLzpMl{#KLf)Aeg=j#
zeg=kEeg=k6eg*~~eg+07eg*~seg=jId<+cp`4|{H_!t-*_!t;;_!t;C_!t;&@-i@N
z=Vf3h<z-;7=Vf4!gN_Hx;9+2h<zZm(hK>dN<Yr*F&CS5j&CS5z1zrCy&dtEU$<4s<
zo{NFu9v1_{T`mTOt6U5W$GI37_Hr>Wq;WAY@N+RRZ0BTPm<1glsN`f|2<2p85a48B
zVB%z8Si`}<u$qH`VJZg$!xRn%h8_+EhDHtshH?%D1_cfVhLh|J42A3r3?b|c3_k1(
z3`Xn>47b@B80N4sFto5SFj%uOFz~W5Fq~y&U|7k@z>owTBj9FbVA#dNz);G<z)-@%
zz);M>!0?HgfkB#ufuVt!fx(@bfdLejAD9>zW->v>3!X7CFzjPuU<hPlV0g;Nz%Uy=
zHUJ(Vz(;_pK4c6XaYE`Ppbt={Lh4$0F$x*<Mhu<9OoN)_0ZBFG;35iCLO{j>GK&?U
z17JCodJ3+2#id2yyrz&1+H3$`qzF=s<PP}!09u~`WFOQnPspfsT7FJWetBkIx(;*=
ztTDWs0!p0~I+Z$A3I(7Z3kZYsLom3~fVC<?O}cbYaR)B<^}ux<w0<nqQ7BAN(1KQ$
zXoK>gP5_0*;?j;Rj?9Mkw{W|hB*m#YM0yjSsrcmbN>WJ=SW*onRXrp!3C3(DXq!u_
zLP=saXsKmRj)H|kDrBe{yt1`0MFG-@hm5uqmw;AEX6AvqDUe+%;8q1FwPE%&!Kn}2
zdxO?@u$A?NDUgNrU<Js7D$saB-?E_$T6~$BlBH3YqN7lpoS2hX6rYotm=X^<`ZPW<
zIavp^_&mL|ARg2nkI&DG2dxrK%u7ME9ie?Nu#NDHj@EU8I14gO1scK8fC%U)=o-TE
z259IE><G}-9ta0vhq8hOsD=X#>Le%TDS-DkfQNKI%eV6sz+>FVs`Co;K-PfPP3C7q
zhS8HiW81|FNvWWwKE$Uwpt=z>^a5GvoLQn!oRMFe16iIA3V4Vs6>Jp>Q=+ZRV&M^@
z3?ByrjgmsjB8XPdkQi(b2V6RVYu3cP%JM|Stcfycvb87`wDK9`7*N@jT2WGzs9>Z}
zP?TAm3))ox8EH`{&PdHm%*#wq%qxZzr69%7I9C8SIw0ecpvEU?yF*|KxCjCjI?#qE
zXt_GbFW_2RArG_|9=U}F2{mwm3mXIiI~hEqQLK=XS^(OY16{6)*lPe;`V1-|p=*&9
zz(#0fq(ZBI$O3KX)~|ff>UzXlSNOmkY&9%srLjU@YB_Alcz%&WQcfZW>p@x>nZ=-F
zn34nCI)PXe4s|9h$dghPK%oG2HYjlO^FTWgK+yzhoq<LRVU{MsY|G5g12xYS6hJHM
zA^TlGh6F;=0Vw?75f7bxFE2{XOU^(oHgNaqlw9&b$pSI}o|&79(o}}{559E{d8!mM
z+e7SyWFv5z2S+w!iw$%r24!ByEx#xkG_eDBKcTz|N=E6aCE;NIfejx`MdYU<cu0c7
zA6r5y1j~T3CkR7R7q}!v$Y3S|hyqZtgrXf(r^6@w3LzOAWGrYpF);<Y8vs-!CW7ZI
z!R@Bv(j?G|d(h~3A!yYvXbuLc&jHd79x4X!R022E!O1u&za&E;zW~%rgY0=pPb~p&
z*C_;*sp`d$j!vS2MG9n2UIDZq0mZQ(lMD4geX6_^4Ff&2kto8l$`C8kz?EhRq=<*?
z=>m^=muDgkB!EoA3`LMA+y&sYt_K}Mh%d-5R?t^4GBwpy&;rdx7?~nM6Qm7<DbNaP
z_LYK4v>eEepY+56P_!8685o)7f_tRpsS2Pis-S84;!K6oyv&kfZ~==Egdn3*6`%%~
zCg<W%pl@Vq3bLXkA2b*QuJ$r>Q}aMOhjLP}403@}7;<RXDnNOl@+BVH8V7|FxDSgq
z;07C_1dWiu!W4B#3Y1jAc7WS37)4hgl7U1PS%KgfL@TX8?F3MtIu+)6P#8dK&z#g0
z1w{WGv|+D2RXqjLUoFW8g*><(1XWEL;0a4m6%MJ@phGI)&WR30C#X9QS}atQnwwt+
z+Lo7D0vgT%4<)2R>vB*Y0QG?(!zm>hsg)pGK;syR;58lK@e9z770`YrkTIa46mW!s
zmm-yb_L~*MeF7Q}D$hr6Fcj-RoQSB((o;(U!G?g!A5g*tVXy#*0geRF;*<DP&=M%<
zc$OYQ1g;V?Z3wBZKpnE2%xqBG5xm7r2Q(4_N>$)(bkJ$Hf*jDM29PBntgHaBASW|7
zvjpNskTBSONNZ2QR>45e6x{#s<!4~n$PZcn&jjs}K81-v`J>cm2#kinXb6mkz-S1J
zhQMeDjE2By2#kinXb6mkz-S1Jh5#Ky0Mv~KVRDv~BX$2%K<jVy%)$NtIwl5&SD*v*
z1sE7$=llQXXJGiv&%p44pMl{DKLZ13jKH0rf#DBm%|CSBKb4PxA(4-P!JUtRftQbg
z;WjS=!zx||hI(EGh6w1~za%dM!!I5NhRZw*43~Hq7@Bw(7?OAx7}$9j7%p%#Fr4RR
zU|0^F|5xN@V7S7?z%Yr6fgzoXfkA|ef#Dq|1H)TR28O4g^Zhs(7>;r>F!XaWFjzv*
z3HZdp!0?KLf#Es_1H)Pl28KKi28KWm1_pf&28PG%3=CJ;85l0JGccTHXJ9zY4mtmC
z4m$%w5jz8e3Ur^qPc{aIXKV}%JJ}c*X0kCb<gzg^1hX+P*h0_$V_;)ocnIAu(8J2W
zP{qo?;Lpmypux((@QsCm;ROo=!(8Z^0B;rs1}_!{1~(Q41`8Gj1`QTy*wQhHj9N7s
z0+=B%dSWD~*u<QefG$uOJuwn|hUDmpk)tO@dX1hK2`VZ^PmF}DmQIDAY&kSej6|A4
z%TH5C&Mz%0PKBIY2%cgDpJfN0`~#gx8DEf-Us9|9-qwL~`T+E>OC-Y*6^e5~bA_O#
zS-Rz!c`2ZIO2|@Z$ZA_qq5)0cXO@7@{De#?CxNU69h#g1o>4|#$pk+q4!n38JOQ1Y
zSOi+U2HuDUT6$Dalv<pcSE7&P3efgR=;|B=@NR%&$f0_m1wnd<H8PN6)$|~g287ZJ
z_H_633xTdpLpo9vG2ITH;D)Rv2A%W(TKolG5)N9uUI1Fp3_3~@vf&i8Xa}@d!=M;^
zo?9_!W){9}5Of?NND}4zQIL<oyWqfkFS8VKauf3*Gqj+PQwF6r(2_Iox(AT!KvSF`
zJq6HnCcsOwolA;xbe%!#9w3Jug65Y&$0~z%oq!I!MhXJZ(l&B88X|>wK~XC75DA6S
zA_bHs)o@kdo)u^U9y+CqDgkmM^e|)v(B63PignP@$f(M|Gu*I~d=+dV2UEiMaHYx$
znUHC7@MJ#JXwcMlNxlNI5L^S;ZJ<eC#A(D(S7O^h3tE$ewyy%kU7&46aQExL_FO=g
zyFqvIfqaItqXBY$GUzy6_>wjqg(T>yfZ+2(z)=fc?4O^XQ>+I%r8P4#M~?xrZ5os+
z!0Qje85oo#KqpgH>Vgl`goOYo7wIv8SLvgARtK`)5S&>`@)bb2ULmz4S&zXJw9PgV
za`G!YV`i2pWT&PU6f5NA=jp<>+v-9s(FIweTa*ggOOgUQgEBEU6?E_^gJ+%scp(fZ
zE2I>IQYZ9)>7@MpY+aCnpxl78aRRc_0;LGhV+hUxZCc5xR7gwAOU?wHiV8{j;3He0
zi%Aj{V0$BwOoKWPe3*nD`Vo<!#R&|UQ3E+E614xnmWzSmFBfF~UlQ7J0QLXB^D{6!
z=4W6y!_UA_&(FZ%z|X)S!Oy^O2YM!8ARhw*6CVS^Cg@uKTwVqSKVAj~Go-WrH}Wtr
z^ztw;c<?YVDDW^aZ02TQaOZ~D_XlJr)V@(_Gz3ONU^E0qLtr!nMnhmU1V%$(Gz4fJ
z0-#<e3Wg2^BkgP)t~)IAi@__&b->Htp~v7GgD)s3(kajZoeY(plUj_nos-boa9CJE
zgE2C{6fq70xqzb*Jnn*glp@xxILH=(I@O3n(h=+0!Dq^WPBaAf@?j^Wf&03!BY_Y*
z$UtY2q~#ZtCl;l6XXd4Xw|o_Ym$0Mk9tCv~br1*V6(XI?rmO%N0{|aJ2=7|M7uJK8
zeM48#gF4U<weI<eIRT&pf50}OEtZEK<CqLOc@Vm93vzY?_}&%JaeJVBjCuK>9ahDu
zklrPHn+0fh9%!{aXp<GF>kqjw1bQ$R@@^45h5V#4(DfiWm0-2$`QY=vKwW6a(N~ZY
zHxRqfOHwOJAm<{26@qsB!Zxab6_w<dB<AE)DwGz3jyTd$01wuH7S$J*7Niy>L56d{
zV^p9!D?s)@d<9y`o|g`G1L(T1%shq6vSio|6p;NSPzQmx8$!$hB`A<rLH&QQcfn@n
zE0krXLe4Zx%uCFvgkF*WI?@t!5@u$89%OGZJeI(t2A~5CLFS}^HfceUA?Va6P`5lO
zH4S<+gF<dT=!OdDs(z65xruqDU=M@OiUf^Ffp6G>?rK9gGc!*C9PXh(3MI(}uv47!
zQXpP~Z`1_mH}IYw@Ieq@C+h0jDS&Sj0XY$T?*wSP5bQ+I#sJVJ3ed)eROkge;L{rr
dd$yq)MHDh2F$wDIqhhcfXbBQ52HK&(005cC?PCA{

literal 0
HcmV?d00001

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
index f448d0aca693a2d3a204c1725ca5bc5a7dd673cc..57c33bd5da6b047cba444428a0e241de82cfc793 100644
GIT binary patch
delta 703
zcmZo@U~Fh$6iqS+^Ym4)&@*CSU|<knV0g4OB6ZnD(f9JK!VC-y`ja^ol<R}}85n~2
z85sQd85sEZ85o}NF)%FSV_*p7V_;z5V_?|G%fJxN%fMjI%fP_I%fPUVhk;=c4+Fy_
z9tH+$9tMWz+zbqhxfvLOxfvMPxfvKXb1^W)aWOFPb1^V1;$&b5;bdSC;ACL9#lgVP
z!@<B1!ok4si=Bbt5IX}yEIUIz!*4bQhP`YI3~g)-3|?#u4D4(S4Ch!G7^bo^FnF>u
zFc`8jF#KR)VA#XLz!1&Cz@Wy$!0?Effng;x1H%et28K>%28IM?1_nN628PW{3=E5z
z7#J2YF)&0iF)#=*F)*BBWMG)g$iSe_$iSe?2nq=ZRsaK5Fu7Sp@hLNNPG<IIcGX}m
zj@-n&#Prmn;>kt&lUP}q85ja5a~inUGxISpoaJR;=;dW#h~#Bpu;682;OAvvxW&W3
zu$hN}p_zw)A(4lHL5GKdft81W;Vw4=!(na)hCXfvhIDQQ1|x0;25xQ!hD%%w49mF~
z7?yD{FwEd$V2I&jV2I{oV2I*kVDRE%V36WsU|`{5U^vgoz_5#xfuV(yfgysEp`JmU
zlY!w42Lr=44hDv54hDu44h9A(4hDv^><kPm*%=rb*%=rj*%=sI*cljpurV;qh6ZaO
z8v}zF8w0~lRtAP;tPBhjSs556ure_8vobLBu`)0eu`)2kure^%vNAA8vNABdW?^7h
z!NS0h!NR~0$il!-#sUhmjZc}GCzqRa>E>i+$0z6Kl@#UY<fIm9GJt|G8C6ihRv|sL
r#2FN#3Z~IaEFjs<Rp#fREDtLkX0X`gV%q`;<B_c{I0E?WJ~09Saq)ZV

delta 676
zcmZo@U~Fh$6iqS+^Ym4)&@*CSU|<knU|4%SJk@KX=zDoq4h9AWmC2k6%Jt6t3=B^E
z3=DSs3=IGH7#ObcF)&QyV_<OQV_^8e%fPUZmw~~bmw`c_mx19E4+FzA9tMU<JPZs?
zJPZulJPZuixfvKHb2BhFb2Bh}=VD-3%*DXq$Hl<#pOb-M5+?(L3nv2u11AH+DGmmP
z8V&{q7Y+u7SL_T7o7fo`eAyZ58D6t7Fsx-`U?^i_V6b9iVEE3;z_5>%fuWU^fx(iM
zfkBa#f#EF+1H(oZ28I9@1_o&s28Qd*3=H#_85rg=GceRKGcbfQGcYhSGcc@RVqloT
z#K16}iGjhNiGhKgiGg81BLhPRBLjm9BLjmnBPb*wSV2KSp)h5$sp5U+$^Vo&H!G=n
za7mPwWaj84<!9!k78T?qmZZj)7bO-Hq!vvs(x1et&CI}1KAF?Ny`CKuP`nHb(|8#e
z(s>ye+;|xnWO*4FUhpt59OPkOn8?Gxkk7-wV8z40Ak4$S@S2-};UYH!!yIk~hEi?@
z1_y2i21#xPh6h{>3|qMv7`AXRFf8L@V94TPV94ZRV94NNV2I#iV9@1aU=ZhGV0gsI
zz;Kq6fnho)14A|^Lp_5QCj-NO4hDvk91IM791INQ91IM)91IK(*cll1urn}BVP{|{
zWM^QAVP{|vU}s?1%*MblkBxyLlZ}BvkBx!hBQ%g#vNABNU}a!f&dR{BjFo|*i<N<)
zgq49Il$C+Okd=Xfhn0ch918<O9}5FRIST_r84Cjg1H)!k7ER{KfhJw@`FSNp`8heM
wMVbtt@JxpD6>JsKQ%g2;m_{>g&M-g6wAtE9n|X4!ZT{r@wmO^n?LIOB0KqkRm;e9(

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 a460027..5461400 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -3,7 +3,7 @@ import numpy as np
 import copy
 import argparse
 from functools import partial
-from ur_simple_control.util.boilerplate_wrapper import ControlLoopManager, RobotManager
+from ur_simple_control.managers import ControlLoopManager, RobotManager
 
 """
 Every imaginable magic number, flag and setting is put in here.
@@ -44,7 +44,7 @@ def get_args():
     parser.add_argument('--tikhonov-damp', type=float, \
             help="damping scalar in tiknohov regularization", default=1e-2)
     # TODO add the rest
-    parser.add_argument('--controller', type=str, \
+    parser.add_argument('--clik-controller', type=str, \
             help="select which click algorithm you want", \
             default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose'])
         # maybe you want to scale the control signal
@@ -76,10 +76,10 @@ if they have extra stuff, just map it in the beginning with partial
 NOTE: this could be changed to something else if it proves inappropriate later
 TODO: write out other algorithms
 """
-def getController(args):
-    if args.controller == "dampedPseudoinverse":
+def getClikController(args):
+    if args.clik_controller == "dampedPseudoinverse":
         return partial(dampedPseudoinverse, args.tikhonov_damp)
-    if args.controller == "jacobianTranspose":
+    if args.clik_controller == "jacobianTranspose":
         return jacobianTranspose
     # TODO implement and add in the rest
     #if controller_name == "invKinmQPSingAvoidE_kI":
@@ -102,7 +102,7 @@ def getController(args):
 
 
 # modularity yo
-def controlLoopClik(robot, controller, i):
+def controlLoopClik(robot, clik_controller, i):
     breakFlag = False
     # know where you are, i.e. do forward kinematics
     q = robot.getQ()
@@ -117,7 +117,7 @@ def controlLoopClik(robot, controller, i):
     J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
     # compute the joint velocities.
     # just plug and play different ones
-    qd = controller(J, err_vector)
+    qd = clik_controller(J, err_vector)
     robot.sendQd(qd)
     # 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)
@@ -134,7 +134,7 @@ if __name__ == "__main__":
     args = get_args()
     robot = RobotManager(args)
     Mgoal = robot.defineGoalPoint()
-    controller = getController(args)
-    controlLoop = partial(controlLoopClik, robot, controller)
+    clik_controller = getClikController(args)
+    controlLoop = partial(controlLoopClik, robot, clik_controller)
     loop_manager = ControlLoopManager(robot, controlLoop, args)
     loop_manager.run()
diff --git a/python/ur_simple_control/boilerplate_wrapper.py b/python/ur_simple_control/managers.py
similarity index 86%
rename from python/ur_simple_control/boilerplate_wrapper.py
rename to python/ur_simple_control/managers.py
index 87dce86..e9b7f58 100644
--- a/python/ur_simple_control/boilerplate_wrapper.py
+++ b/python/ur_simple_control/managers.py
@@ -15,6 +15,38 @@ from ur_simple_control.util.get_model import get_model
 from ur_simple_control.util.robotiq_gripper import RobotiqGripper
 import argparse
 
+"""
+general notes
+---------------
+The first design principle of this library is to minimize the time needed
+to go from a control algorithm on paper to the same control algorithm 
+running on the real robot. The second design principle is to have
+the code as simple as possible. In particular, this pertains to avoiding
+overly complex abstractions and having the code as concise as possible.
+The user is expected to read and understand the entire codebase because
+changes will have to accomodate it to their specific project.
+Target users are control engineers.
+The final design choices are made to accommodate these sometimes opposing goals
+to the best of the author's ability.
+
+This file contains a robot manager and a control loop manager.
+The point of these managers is to handle:
+    - boiler plate code around the control loop which is always the same
+    - have all the various parameters neatly organized and in one central location
+    - hide the annoying if-elses of different APIs required 
+      for the real robot and various simulations with single commands
+      that just do exactly what you want them to do
+
+current state
+-------------
+Everything is UR specific. 
+
+long term vision
+-------------------
+Cut out the robot-specific parts out of the manager classes,
+and create child classes for particular robots.
+"""
+
 """
 ControlLoopManager
 -------------------
@@ -154,7 +186,6 @@ class RobotManager:
             self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
             self.rtde_io = RTDEIOInterface("127.0.0.1")
 
-
         # ur specific magic numbers 
         self.n_joints = 6
         # last joint because pinocchio adds base frame as 0th joint.
@@ -167,7 +198,7 @@ class RobotManager:
         # and i'm not clipping it, you're fixing it
         assert args.acceleration <= 1.7 and args.acceleration > 0.0
         # we're not saying it's qdd because it isn't directly (apparently)
-        # TODO: check that
+        # TODO: check that again
         self.acceleration = args.acceleration
         if not args.pinocchio_only:
             self.rtde_io.setSpeedSlider(args.speed_slider)
@@ -175,6 +206,7 @@ class RobotManager:
         # TODO: these are almost certainly higher
         # maybe you want to have them high and cap the rest with speed slider?
         # idk really, but it's better to have this low and burried for safety and robot longevity reasons
+        # TODO: 
         self.max_qdd = 1.7
         # NOTE: i had this thing at 2 in other code
         self.max_qd = 0.5
@@ -212,6 +244,22 @@ class RobotManager:
             return self.q
         return q
 
+    """
+    getWrench
+    -----
+    different things need to be send depending on whether you're running a simulation,
+    you're on a real robot, you're running some new simulator bla bla. this is handled
+    here because this things depend on the arguments which are manager here (hence the 
+    class name RobotManager)
+    """
+    def getWrench(self):
+        if not self.pinocchio_only:
+            wrench = np.array(self.rtde_receive.getActualTCPForce())
+        else:
+            raise NotImplementedError("Don't have time to implement this right now.")
+
+        return wrench
+
     """
     sendQd
     -----
@@ -235,6 +283,7 @@ class RobotManager:
             self.q = pin.integrate(self.model, self.q, qd * self.dt)
 
 
+
     """
     defineGoalPoint
     ------------------
-- 
GitLab