From 45d786442e193ac413b7768dcd01e651e7d5d220 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Fri, 17 Nov 2023 22:23:17 +0100
Subject: [PATCH] new clik works

---
 .../robotiq_gripper.cpython-310.pyc           | Bin 0 -> 12234 bytes
 python/examples/clik.py                       |  16 ++++++-------
 python/ur_simple_control/clik/.clik.py.swp    | Bin 28672 -> 28672 bytes
 python/ur_simple_control/clik/clik.py         |  11 +++++----
 .../util/.boilerplate_wrapper.py.swp          | Bin 32768 -> 32768 bytes
 .../boilerplate_wrapper.cpython-310.pyc       | Bin 5360 -> 5339 bytes
 .../util/boilerplate_wrapper.py               |  22 ++++++++----------
 7 files changed, 25 insertions(+), 24 deletions(-)
 create mode 100644 python/examples/__pycache__/robotiq_gripper.cpython-310.pyc

diff --git a/python/examples/__pycache__/robotiq_gripper.cpython-310.pyc b/python/examples/__pycache__/robotiq_gripper.cpython-310.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..122384842b716013f5bd36e61282ee1c6cc548bf
GIT binary patch
literal 12234
zcmd1j<>g{vU|^UrB`kHfF$2S65C<7EGcYhXFfcF_`!O&uq%cG=q%fv1<uFDuf@!8G
zCNRw$#hk*J!kojB%NoVX2vWn6!<Ne)#h%L%#gWSy#mUIv&XB^I!q&o&!q&_b#g)Po
z%%I6$W$BxrQks*hP?E2ZoS#=xl%Jyzl%JGel3A!;tdL%mSx}H#RIH$@P?B0)lA5AW
zo>`Ki;Nj@!qU-t+<VZhF##=0|d8N6U%(qxW^D^`EZn1`x7UZPf;`T2}Ni9lEamh?B
zNd_rJ#vmVq{j`OVfgzP4iZO)&<c}!k6s9Pa6y_+_6qYEq6xJyA6t*ai6!s|26pkpa
z6wWB_6s{<q6z(YA6rL!)6y7L)cZL+c6#f>56#i6!X67hCcZL*!6u}mT6hSawC`Bkm
zxP>7~IK?YPG*u);ELAi`JXI`3B1JNdBSk7jx`j1LJjFXjCPlV|B}yViE>$vBBvmR^
zG*v8(Ns<AAQ{+<=T3Di_QxsE0Qk0NnSyGf!R9aY~WI!@$Oev}<YAq~LvZ-<@>Zu}W
zOeq>Enk_8Nj8XC_S}EEs3{eUxJ}J7XifK$KdMWxXtWioS1}TOu3{lD{5~-qTOesbw
z#w{#SDo~zDifIc=lq!^GmSWz*5~T*^S)^FDutce+SfyCEFhpsj*reFDFhprWW$jYz
zTUesBQhZVzQWe2|Wl3>NacW_W(oS(s6-{wT6-{wX6-|*y)k#$Z*(}MB#*yNd;@-j%
zrHi3LG*t*34jd^SDV{AXQF<wo!3>(dxA>sx#2u1Isw%k@6ciMk^K)}c^D>hYOHzv=
zsih<%6_$_`QZkEDlS^_cbri}n6BUZ{le1GxAgYQ>iZb)k6_WFFa})DYiggrnQp-|{
z64OC~P`SLs+|*))wEQB4vc#gy#H5_mVm+{(x0oHBLvAs<hxp%Ob_@x+#q8)3bc@-|
zKj;>7aDeMA<^ccTTg<^Bj<=Wtf?RJg`#X8vVs`ThsbUQcanrTHNSutY3@5{YFL4Gl
zXtGtwpay~-G%$io5=%;ps}i7Gg<>#Yp(wQ=zX+6#k}4IDf<;duBqLQJGp{5yJr(4+
zoYK@{g^a{9aIVZvEJ@B#D9=bNLDr&BoSK(XtXBkb>q{^JDkl6i*>7>g$AbbYKK>S0
ze0(lAbH~Tu;)#zhEKSUT%CN`Br{pKc$KPTNat(G3xy9w^9O4<~7~<*Yev1vvc3sI(
zB+bA8A%4Z_XXNLm>Zj)y7v!fT7J+$22yRYkUW$G}QDQ}Av3_Y$d~s%OK~8Esv^dc(
zs4U6I&(lw>NCXKN>lZ<amw0I5qE}FPix(PBkYJMq6?UK!f`fsD5d=Ayia=txBT4~p
zM1e{Xa9Jmf7FqsDS*gh-5KF7#Aw1efRgn+_1H(%uP{0Z^FfjOOaukVxIHDjz3`B@C
zFfiO=^Ysq{#f3<4h<`wUt4q9pXozc2yuXu|t8>ULxTvR}A3~JfF(lr_H8=zjZ4eeH
z5#Qp0cn;!K6;Sx{FfcH{!k2}SgRw}Nfq@|zl#*cOG>8pysWV8q1*q;}tYL^}C}CW{
zw2+~OA)YyfsfHn*C55?$A)Xb?W&^X?!E6pNn=^$mm_d`JipM!WuehYBG`S?dNY4+F
zYX37ZFlch!VuO?dw>V2Oic%9(GV{`JvH0XCXW!z1mId)SARcdgZf0J5L4I*&38<3c
zjn7T2KybOhvc(0dsVTR(z*2B_W?p<+eo=BNR2<A+$#{#UI5j5?5<=i~4h|-c`1s7c
z%#!$cZIGV@7#JAXn7A0(;E;>4N*pZ>!E`)_2Ajhzj*R@`5>Q6F#RAIhxA-6)j4#Q|
zP0cSYxy4qLT2fk+=a<X^)dHeGY!C(qwh;pZLkYtIh7yJ>#uA1sriGw@W~gDvVpzaj
z!&t+V#<Y-;5yEGwVFt0m!OT>(-#I@oFEzQO7*w4n!b@FP(UDn}nx~MMl2Vjf3@tf9
zCAL*TVo_qQ0?gG`3La4RE94g`WEMb*SR{oYPgyAh<QJ7-NTK@FN+AT|WpJ6D1d2v*
zX$~p%^9xdo5<zi^<P<+m)*@9<24Ht|i}&<%4Y|b~?C<Oy9~=_o>gaomJsIM?TU^Dd
zCD7p1WGONL<r`xVVFDsdK?FEX!2~E`i!2!!7|cL%%ErLJAjBrXD8N`Hh8B}h!*S;U
zJ_ZIzpffIH0JX4!88n%y)SYwki&KliWi!--%=|n^orYY*`)M*kyvCXg(om$2>B?JN
zDVfDkGr)}_Q10Vns**%=C_>dOj<Up}_!LkJ3#1m5kU$vZNe%`E25@3(1LxdYrW(ct
z3=0`t7-9utm};48S!$S4m};13Go&z;FxId%GtOq1%VN$@%T&l#!<51dV%0EYG4)G;
z6B_dZmKvr7tThZ-YzrAd39g1Ai+KTi3d=&qTGnue6jl}n7KUbKMut2NLxy6@az;i5
z5R7C1>0ZE5!@3Yu4zMK#FfuR%gK92tcqJAT6y+BbWhR!S!YY2`c0ozLLUAgj)+<ZQ
zDNR+#PgBT4)craNiFqjs<%yXkkVXNBR!>aMR#zxWEiTB<D^A5opRmxfQUJ9|GxPHj
ziz-2;!rK#|d;_vm1118lOLY{$_G==U2g$=$3L!-xcjYM*mnJ8t78j?L<|q`UCZ`sF
z5;n->#N=!pg|x(+;#7sqG=;o;g~a4+h4RE=u&T_m)D#_s%)As(a|x6+KnVt9eQsi<
zLSB9exN!r@dEmA{Qfg|RLTXxCYH~?t8Is>_F$cSb{9;tN#mM!Nfdy2laNc6gEJ@8R
zzQtTzQdFb?N@<W>1!^=T=H%RBOU+BpPf5MS;p*q??*gi6Sc+1U%Wkp9XBNjNCTDB1
zK{AysC@(T6=ca%Y8o0@IixnIew^&jVOA^t`PtN$_)ROqJ#G+ysP-X*Fb}Z}yj8cpo
zj2uiNjC_nT|G1c|B+$|`TvL%3$f@3-iU`!20VQP+2C-Qgpvf6jUDklgMn*_Un9kVD
z2u@s#iE)%Bu0(j^A}?)$1IS7t4B9${CMKj*11V|20SHnCO4HCp4r<kcVjq_7kTeaV
zq^8MK<O_--P;6*2L(&L5&d?JB2P|GgL1_V0K8dhnR-e!WlMIpvMK%btgWD$12%iTn
zb^GN&Wo`{4sKlDfT+39$kj1!wsfK9*a}7fl3%pcfT)>*bypXY$Ih-Md1*KF{K`E7(
z7qCI}Enr{B5W`f<Qp;MyQp1?dT;y58k-`ct8QE&!s%A5!u!HU9n9JVG*u+@ES;N}Q
zSj$$!7R;c@nJ7#!>6PRw6s49FWu}5!^%<#<5(QjBz!RN9T2X#3xL|}fjdc{@m1b#P
zNoEd64qhg}3OR*LL}3D|u|cT_QV%PDs>ou6{8G${8kWqg6#No%!38Tw8%z|GaAA(c
zk`TjSg)xer#9~+v2BD>j#oaYTp$Jr&6*+?1yHz5gd8rizsmY*D5Znh<ObQxR!kShJ
zDfy|zpn^{U(&<tE#i*{yg($#^K&48ND<}oJfe3d{@x+#r3NFi7iwklxOKx$2UFTX<
zlwWj<IWw<BlN~9UdV+NMfOLT?PH-cQ6Pz96K_w-q<iSkE>5w8V4wQ;Pr4I)?4<ooN
z`zylE!KlK>!N`K8SqRlt1hNfxlLAyr*D$0oW`hP}nEZ-CMO|WYHrNrGj9|@RHiQ5-
zWjsKBU|?YQ49YPaOc>1=Xc3NKC%knDY7l@*P*4U+VO+=ruA)H=8zwjpl%*IKGJ@Py
zBv%7!b%L`Ph+k|&zTP6a64nK5Dd5&?3Nxr<kf_SUzyMFuL8+k150t8)832)zKpo_?
z{G6Qpa!@@9if~Z6PpwEzE(Il}%sf!uP)N=%f+Q+XH$tH}xhS)sL?Nv-FBw!F!>vh4
zO;ac;j4uM&rUA7n9+G8jl?;_Mt>7v_b#GB2tQ^x&a&!(+(orzbfwiomx-@ZX2noWj
z0Yyi7MrKZ`MqYji$XU>Y1GCH!!!iY1TLlA9PDj@e9O9^?qkuyLny<+68HOfYz5^*l
z#>JI+$r`Akl$w{qg%SwGIjN}y8U}i%2z4MeFk`@d#&l5az`*dp-cOUg2s9j81ga{E
zI6+w=3`BsM=(kuv<=!n;uz^_H*=&%wF9YRhkPbdUK1KybK1M!9g@1g^RiZ(V5en#_
zgdRk7$SuCa(vtl6<iwoJq@u)<RFo<Ulo{bU>;kyT0yVdb<7yZdfV&Gd3=2S27Be(g
zNkI855Pq>PB4<ElaHuQ->0}2rzf+h&c^9mY1=1vk+rXU7ROAUY2V?_lHdApNSS2{`
zL1jQTU{P7*S;JVAR>QD>1KiDF3uZ_GRdVc!H7pDaL7+?n>Fp$f+M(bcoI+YoV!8q}
z4?;5(qy<%+T2ic#S)z~xYW9}oE9B>;>L?`Vq$Yv~`wEIu%QEvzixtumOLI!VrXY0&
zq5V?SK(|r|&qyuFNG$@@QShh$Xa3yGyv*FvTyU=-H?acDhK&~#D<mZrr=}?6=P4v6
zmy{;vDCFjY$^@DgFwg)>C0xd2rYXR(e`YakU@RV{KLyPIP3(CYTY095n#VwyX@CkF
zkkvQ}T||ZlDS}}vWd=wJR3@O58z4c9G8(fGM9~8>1f&myk=f9i0T%G_iFqmUpbjQl
z1YoHps1k!Pd(o`J3?FdOreUaOK$ZA4B(*Xz#*zR*K?!k|fu13zi$S4`5(UWWk&1kf
zFi0(k24Q42qzb4OS?B(+s>L2WlnSj7KG+vQhEq9jae{lWpdNBj6sSf3R}0`;0#wo$
zMS#R2K?Jz^hz5;8GC^91AU33(1s^xM#Q}?weo$Q@1?rc`axf||axp6W=VRuAvRGJn
z7+L;f^le~zQJP|)iUQQi1CPzfK`RPSQ*0q)En^9&H^aD)v4km$Ig16{C}gZs3j&W9
z7Aus)N<O3smCRyLfd(26&}1zF`xWd>&<GHC%)AKXW=)6>Z?Qq9A<#To6vM#4unXir
zPzA=rB*ci(Lx+bcidR4b#2^gv%4d*`pb^So22I8)XPA>gZ8=!E3@eMlg*CL6P)JV9
zQz%MJOwLfy$S+9EQ-CSb)YD`v(gej5wt+GMP$LO>Ja|8>LCnTfB_D)ZeWR<z>4&0t
zP-<YTaz$}HT1|q>_25yx6jb+X;dejM0QO;!%0vbR2JEg!Qi<VuP=W%-0HiMgYIT6)
zAPD9Q@YGReF=%?XI1@Z23-$v_aDzu^VR52?5o6#k5;#f{Kn}$gC4!(}2AK&Tls^tq
z8V>RkY(^Q@pu$j!(^EyDIUh~Nst^L6K?{8}@1RE^!aGSIH)8XSFvyJ%Q{f&u4N?sD
zkP?<8hNc+9OYlq&npObyp+J);ptfN)XlRtN${pcFXagrBRUx^ws3<kB1fHxwou1_U
zJXky?7AqvBf;vQLr8zm33ZUTE(`33O0(Lw^E8O)(DIn)#^DBF1aXd)tc~~YA!0c5*
zm5|^oR&Xlxq}*4KfYsAvDoO+SAQePl^9HEohp4>_Qj!5mdFVrpnZ@xCCAd8TnivB`
z52Q>6d%_9k2~cr{5hdWkT+ob8YKj7MUbi5lvN$t2F-IXIu_&cHu_zVfdT{B?3vwD3
z*Mt3b9aIQ|@-q)pl`^&@h8Cr_m~-;eQMw_Z5*$<#g9?OV21d|)#R3LM7X-9QWFd15
zQ!P_1Gq|2%hSxL9MY1KVDa<8oHO%0704C4~7bFZoqhok=T9vS-u%PJ#b#yqg8H*jg
z7@8PsK$BM-P}3L}f>x$5;dDnL4%0ZZS&K8^rg7FV7S)w-rLfj8EZ|<qFq<KTt%hL%
z517S1m$jLZks*a67>qSJs}?$zl%(bsKqkddy9}UI5AH8O5(PY)73+XvvN$KdJf~6t
zBnQf4pxFkH1`r#R5TL~+q6-1-u;k>Yqx4b=iZb&cvlvCG#icnAcjV-!BQ(AQ50HUI
z%d1qKq1{W+BvM*pW)7&+QjnSlwlp6!IbBp@rBJ1bMOjj6a$;#QcrY(9Paz*Xw{4|R
zB?B#W6+j&+UAX^KKpm*)DkdF;UyQMSn!L9}bMwnm;VoX!oGGa2fRq}bBn6%zE>Z#w
zp78{Mh9h%R!J}+NptM#5&XroA#0{#nZ?S?sT?DQzKxM2Zbe1j)q@4p+$rpjf)X-Za
zpk~-@P(}bv>Wi@oF$yt?F@s1xMm8iC3zHJ4%_WPLO+cPRbkU0nK=!dh=7L$lQ@<#)
zuAqSfkl#RFD9!^<l%+7WFx0Ztuz=b!HH@G_pqUvwG|Z621WL+T%+1WTY+!ko8qhcy
zYcq2#dkJVTnZ1TJ4V?6A*uXSr4Lg|TN?}f8N@r<itOaEQ?i!ATj3AZZ@h=7@*n}8#
zwi`TkTU?S@RHBfZUj~Wx^2DMPNS`|$JcSDjKOF_|ax73{EG|e*&P>ZpO;G@Q7M!TT
z{zarRE6D5vOfzT-7c!X+N(rFI2hq{UttuS_<Vp5ec>5h{64)jyg<#OEJ4i3sjKmTJ
zuo+-Uhyh?GhHkLytQ6cpbMqkGpri}d36_B91T&Fz!lptT6-q#B%pl%RR7lFt&q1C>
zLvAR88sYhQpfL#0tQ%sQE~gT-gr)>COp%xe8@mP@3HK-{WTE2)uwYXt0j<OVnN^fp
zSPEVgr=yUVR+3r-t%?&<vP!{A<x29w?X29)JpJ6n3I(K`SCp8Sjx>Rn3~CxcMz$Cj
zKy1+TN-!u3fSL*=3^fcT;K{CLre@GEF{7U*(=F!Q#EM(YxtV#I%(vJ<aaNX?bBh^7
zu!G{PEHMWyU)|zJ&dDr@FH6k%2QnE{%Lp)5siI{cP!#If<m4wO<`moM!E}JTTADnN
z_Bc3`gVskB>437kE{M<r5g=C=f$KL=6<Wj%5(BkK!Ba+>oRCfyC_@$%f~1N-L<J~+
zLR?mmUkr6)aX~7Sm6l&r1e$|H_X`WionJxz09nPsDZmVx(br(+W2_R!;v<Ya4{|Cv
z&o6>?z(Ca$Gq_p;PXrb7moTO<LaRYgo@NH+*=*(_l@gW(tdJ3G#3)(~Yc_MSIF!$}
zkg=Angna==4ckJ-680>PEY57EA}4&Z#l}!su7!-X>@{q)95w7<6F3$!rh%fIIdK)O
z%L#BsKu-91NK@;ed;kh^P(Dr02QA|&K}x^S;sBOIhEKtQC>4rQ3qZqbc_rWiIU`je
zC$Si*RSqea5bZx`(*Rm7WR`&Dw7{h^QUF62<$!}VBQXWE0HPo#wFI;xAQ4p0mz07g
z)Zj}4GV;qI#YuiyszPdBN@|LNMsaGY0`kHxh19&#TyWnfCAB0mGpATnPr)z0Bo)=x
z%o0#9q9DJxI5R0H6;hsoA{Z8b;N=P6xdl+a9ORzNGzEBz0xX-7S^}BEODP2p<f0Fz
zRw=qcnl8}UZP>&jWT6Jg%X;a5R(`zXn-pNL$z4<fDhfc=FJuIRAH+or)PM@XB2X<2
zX@SUtWI@FY_9EpL8)!y3Kl>JYa&8K!1#^o%xwI%gKPl@LXIf@nVh&gg%FIv7LMvK|
zav2yHI2jojia`S>LR_GwaRSU-i~|38ScDjP7=`}xu&^-lF;=NyDQ7TRPnwKHptW3@
zOhur@M}C@&A)5Nw7W_e`F~Ef-s7X=;TKotpKtbgjq)-Kq>wtp>93X<AU;xj8f{Jp;
zSPy6kUy&h50#vvafvS*OtZ6y<i6!6#P(?N%DNxy01gd4g<uYUz1=KaU#RD20%1q8p
zEy>7FDFRP3f=1A8v4E=AqF9h#&`1%YQ3q)of?Ix|ak!#PkP>ho4b+A!0`)~8Q40!F
zq;Q50uYqF~<l$n_A_DMgL>AC`PdH|RvX}&z1lT#)Ww^Px<XEHx`8b(G1Q<D(1tdAN
z`B?c_K#+rlgN2Vpghfe8O$2PNCU+5NEhTu-RgoIVX`m!^iv^TNia=p@i>;)xATuu=
u5*T3Dg2M)!)WB{;5TKdYTO2l!<+OI7L9b#^e6TQZFmW()FoGt8g_r;#wdcYB

literal 0
HcmV?d00001

diff --git a/python/examples/clik.py b/python/examples/clik.py
index 6daff4a..e537b87 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -14,7 +14,7 @@ import os
 import copy
 import signal
 sys.path.insert(0, '../util')
-from give_me_the_calibrated_model import get_model
+from ur_simple_control.util.get_model import get_model
 
 #SIMULATION = True
 SIMULATION = False
@@ -28,11 +28,12 @@ PINOCCHIO_ONLY = False
 #time.sleep(3)
 
 # load model
-urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
-urdf_path_absolute = os.path.abspath(urdf_path_relative)
-mesh_dir = "../robot_descriptions/"
-mesh_dir_absolute = os.path.abspath(mesh_dir)
-model, data = get_model(urdf_path_absolute, mesh_dir_absolute)
+VISUALIZE = False
+#urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
+#urdf_path_absolute = os.path.abspath(urdf_path_relative)
+#mesh_dir = "../robot_descriptions/"
+#mesh_dir_absolute = os.path.abspath(mesh_dir)
+model, collison_model, visual_mode, data = get_model(VISUALIZE)
 
 
 #load gripper
@@ -66,7 +67,7 @@ Mtool = data.oMi[JOINT_ID]
 print("pos", Mtool)
 #SEerror = pin.SE3(np.zeros((3,3)), np.array([0.0, 0.0, 0.1])
 Mgoal = copy.deepcopy(Mtool)
-Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
+Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, 0.1])
 print("goal", Mgoal)
 eps = 1e-3
 IT_MAX = 100000
@@ -126,7 +127,6 @@ for i in range(IT_MAX):
     #v = np.linalg.pinv(J) @ err_vector
     v = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * 10**-2) @ err_vector
     v_cmd = v[:6]
-    v_cmd = v_cmd * 5
     v_cmd = np.clip(v_cmd, -2, 2)
     if not SIMULATION:
         rtde_control.speedJ(v_cmd, acceleration, dt)
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
index 9fc488300be91c4a2b8a7a31043e23c54fcc35ba..4b0d127886e9e3b4515a2eba06481e757714f650 100644
GIT binary patch
delta 312
zcmZp8z}WDBQ8dXQ%+puFLeGeSfq_AQfnn|S@KmpjqQChkTL=hjRuFi?&%23<fngpq
zMA~Gspu%d+@2m_A`&bzmT3HzwELj;C6j>P<-m)+-Y-C|z2w-7gkY-_ExX#SLFmLly
zg-uM88I^@6|5fHxGSo9r*HOqw%_*=|&&w}SNK7s%P0Y!uj4v%tO;JcJ$yJ}+s46Rw
zoS#=xl%JE6TBKWCkeZsJTb!JjlbM%3`MrjuuChXIVr5dQLS=rbLV03di9$)f0!Thp
zp(G<!0jgD@I5RyjF^6IDM`e}CzRG-?8&rxInN2krCNnCFOy*Kkn5?a~m)RK17oL1U
wRdlnk`bQ4dZaxNv^OHFZ^jV*QSetDPCh<+aW@pP@l%JGeqNA``*#4~n02-KFlK=n!

delta 199
zcmZp8z}WDBQ8dXQ%+puFLeGeSfq_AQfx&BAcxvKC(ck=xhMNTiKJ)YDF*7juu|OmZ
zCkra9*8IlGz_5pvfuWg|fx(=WfkB>?f#Ed^1H*b2h)M?*1_n_U28NHDpDJu(np~hF
zJ2^nbc5;A<%H#kQ?a2?6_%;WqEMa6ZGSy_5%&06fnOjYP(P*-^+FoW8u!!*He`;);
ptdsZ{7_Lp`G|*>#3u0}yHJHT5o~fXqppcwD`MjO&W?}oc1^^g!F;)No

diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 6f10066..a460027 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -36,17 +36,20 @@ def get_args():
     parser.add_argument('--acceleration', type=float, \
             help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.4. \
                    BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
-                   TODO: check what this means", default=0.4)
+                   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.25 by default \
-                    BE CAREFUL WITH THIS.", default=0.25)
+                    to something between 0 and 1, 0.5 by default \
+                    BE CAREFUL WITH THIS.", default=0.5)
     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, \
             help="select which click algorithm you want", \
             default='dampedPseudoinverse', choices=['dampedPseudoinverse', 'jacobianTranspose'])
+        # maybe you want to scale the control signal
+    parser.add_argument('--controller-speed-scaling', type=float, \
+            default='1.0', help='not actually_used atm')
 
     args = parser.parse_args()
     if args.gripper and args.simulation:
@@ -133,5 +136,5 @@ if __name__ == "__main__":
     Mgoal = robot.defineGoalPoint()
     controller = getController(args)
     controlLoop = partial(controlLoopClik, robot, controller)
-    loop_manager = ControlLoopManager(controlLoop, args)
+    loop_manager = ControlLoopManager(robot, controlLoop, args)
     loop_manager.run()
diff --git a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp
index 30b86a09891ea955ae68c488d3e981f4f26b79da..87e036eb49ca0637ee8b9db76e87b12687138579 100644
GIT binary patch
delta 948
zcmZo@U}|V!5=}A)^Ym4)&@*CSU|<knVAy#xJhgYD=x=_;pvi&)@r;g}3k4?g^R8uL
zU=U=1C<&S@s8C-2keh*FJvReG9XA6*FgF8(4mSgXFgF9kTP_BMb6gAz%efdBmT@sK
zgmE!2m~k;M+~#CpSi;G`u$Ys9p_-F{L5`Dw;VTCN!wL=thBOWa25Sxmh8OG%42Rem
z7!I;CFzjS!U|7k{z%Y}Yfgzclfq|Euf#Ee91H&;k28MlX3{ecL*%%la*%%m{*ccd8
z*%%m@*%%nMvobKOU}a!f&dR_rpOt}OIx7Q1KPv-60V@N88!H3D4;BW7t1Ju*XIU5+
zcCj!p^s+E8<gzd@II=J>sIx%4_+fLR;zXv&@<tq!7pO=~?u+7?yuna(^A42>jFatB
z*m&}j@=M}#6Y~<&Q;R0AHxZq@&PRQ6vs$ZIYEGJtLUCq#UTLn5LRwK`ZmOmggYsr^
z^(`!ZMaa4oY!y)W3<^-7tdOaYmztWQP*jqV8lRk>S5lOpqmYuXP?C=#G}*!4d~#Zr
zFfWQRn^|?XGcr1F-mB}*#JiN4f#E1L!W<_H8tB(w<7Hr&&db1%%gex!!pp!A!^^-B
z%*(*w$;-eX&db2?oQHv784m-4KMw<gI}ZbcF%JU+KMw=L18xR}ecTKTHQWpgZrltE
zs@x0=x40M>=5sMHq;N4X1aUDi$Z;_+FmN$2JmO?vn8eAz5Xi~EU<!?70Zs;nZyXHu
z3{N>27@lx2Fx=r_V7SP^z;K*{fuVqdfkBc365~tQ85riUGcYu<GcahgGcdeiV_>+#
z#=y|c#=sEE#=s!U#=!8Cm4RUsD+5C&D+5C`D+2=;E5w`YSr{1VSQr?*SQr@OSQr>y
zGBYqtU}j*bW@ccBV`gB`W8Q3MxSVOSfr{v49ut+xzm0V!|28(6{M*=23>x@gsyH<#
zO|MYFR-s_?Z(}8<&247F?3;htav8DSVr5`BG?}xYJ<ytqfkBvyfq|8af#EPG14APx
z1A`|g1A_-A1A{v!1A`JL0|N&q1H)ks28J3A1_nb81_p5s28J)}3=C)485kC@Gce?^
zGcahdGcep`V_-P6`C-9B*2yJ_9gMb{|0G^vnrx#YIQhS__~f4v0-I917$+OW$Zq~u
H&BOu#=Wom4

delta 901
zcmZo@U}|V!5=}A)^Ym4)&@*CSU|<knU@*QEp6azx^fy0az+^#zct+RFg#wfLd99ck
z7^bm6lmtu`R4A|iz|Fw0kDGyEIyVDD9XA6*1UCbNB{u_u5H|yZAU6ZUK`sV{g<K2_
zZd?ou{9FtSd|V6+&o~(vIyo5_5;z$cggF@)E^;t1EaPBc2<Bj5(B)uY(BWWUP~l);
z5aD28VBuh3ILFSwP{Gc?5XR2HV93tEpvlg_AkNOf@R^MvieU>I1H*JS28L2L1_m28
z1_oU=1_m8A1_l*21_l{61_lu}28OS!3=F4P85n9=85p8i85n|C85rzY85o3E85lmX
zFfbfrVPIIp0`b~3W(I~3W(J1f&54Q=nI>OISu$DPh-2~vBlgV-DvKE>fAC?S+-E2_
z`JL(t{>=2e(p(*dw4%h^R81>}&1=-QuuS%|*PNV|CA^tkXDcIP^5(s|?o7O=nHU&;
zGeg47b+Vv=e*F$!28JG928L2z1_m=;1_mBp1_owc28Q1}3=E%m7#LpgFfeT6VPI(E
zVPJ6OVPJU2&A{-Cn}OjHHv_{uZU%;WZU%-}ZUzQvZU%;@Tnr3HxEL5JxfmGixEL6i
zxfmEeaxyUN<z!%(&&j~h$jQK9$jQL)frEkJ3I_wjUJizOhV>i_3{yE67+N?O7@9d4
z7^*oK7z#KT7}7Wx82C9D7<RBTFeI=uFgUX_Fj%uQFetJ!Fq~pzVCZIJU?^r|V6bCj
zVBlh7U^vLiz%ZGWfgzZcfkB^@f#C%U1H*h428M|&3=9q|5O4otW?-1W%)n61%)k)G
z%)p?>yxGohIn!kG2*t^}Ot>c}n8<DZY4VOyK|xEQI5j6tFF8N2q$ocpC$%WPxF9t(
zCB8U0F()%Gee!-gSqTt3RiPv!RRO9@p*S-=FEMBHWixelIj9(hRs~xHLp=lh2E4JA
zFk*cSN-UE(3)%zixfmFvxfmFDxfmEuL(^;^Cj&zOCj)~&Cj)~PCj)~3Cj-N2XfRrG
zFfhn-FfjaKXJEL>&cLvOoq?eQnr5G|F)*Co{IK95>%;^OMup8KiDImijbdas|Ep$V
F0RS1DyNmz;

diff --git a/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc b/python/ur_simple_control/util/__pycache__/boilerplate_wrapper.cpython-310.pyc
index 8b3e26aeb2ede980f7ca8364b51c2f398c05e7c0..4496a8bd5d47209e3d9d336dc2a2ca9704df6cc0 100644
GIT binary patch
delta 1276
zcmeyMd0Ue=pO=@5fq{Wx+Kup3m5scSj9g3%3=9qo3=G8@lM5I{C$S4np3Nv-&jL~e
z#UM5aJA+h#l-4k3G1M@`Gt@BFFvK&~Fx4=`Gu1HHFvK&bfJ9hQm}(fZSfoK}SWDOz
zu$8c7vDYwUaV%tFWJqBMX3%8zd-;Wtf#D@10|SF5=Pkb6#ESULlGLKalFa<P;#<5$
zJo!oaCGok5d5P(%MU!=yoTON8u@tA~q!sZnFfbJHf(Ska28NXkMWU0NnOZnN91cb{
zrpdg_8iCAEhl40q1_lN;1_lPO<82ri7)lsY7@HXx8A=#an3@?~7-9u#nM;@!FsCpt
zWUOTYIXsJX0b31A3QG#>LS{yWLXHyl$xY1RP8_#b3yL!HN^Y?hCFZ54-eSrtxW!tP
zT9jW5cGNANqLP%<_~iV&lA`>aTWrMzsi`Smw^&juGD|dBi$oY07$ys{$S8|~+zuwh
zKrCSf1_nD&z=EQMgOP`ki&12A0E-8shcrl<n}LDh7E4KHZYspn0w6&tkRWSuPHJjF
z5y<1W_zE)f@{^M@GV|l}^KvSSK!$3vf$c0(p8SASQ3Pxum;l*&i@B&Y&v&vYn+qe`
z<R~^(3vrOKAYDbeAQo65f&ghL;$UE4kO4Uq6ayR#9E?m79GsJnvo$mNO!j79!WcXG
z1A8nZ<77V$2f-S~ETtNTcqMRTbNf~0PZkr5nas#(%qTh8lv9IKQyJ{;B8|!UoRb;l
zCNp#Su<?T|7ML8$<)*`xo}ZW#pITIuU!=(d@de16MQAPpIl64}LoPiQE+(di$%5Q6
zll8eziEDrivIhkbS9)rRV{%DpVoqQY-(&$E6Ah3DZm}j76(v>{fkGW-Jcx~K{Nx;-
zaFa9!28J5O6owSWRwhY?TBZ_)1&lRJ;Ka=&!cfDI#k7#QmZ^lfh6$9OKq;qKW%5rR
zD>F$328Nga|NsB5$y+1?atKRuPG&)oC&*!-xGMrXQ4YkF2NA9yLSS+xuYwsUncQM3
zOu5BYm=d3yn}X~pkV$OCsd*`ZDfeL!!NV-Z%)z*MKkp1CM#;?)0v(Kufs-E!s;PkN
zEeZf>Vb1r>EOG%^0S-hn5DVgp$=X7hjM9^*3hDZT>;;F&Emq%>{QR7vY>-wKP(<8f
zOufYkV#McU7MBz$gY=`u#4Y}m)U?dJRQLSEoPhkyyplhY1%(TO^gt^0L4*N_07X|(
zAcz$XA__nR!W=LQoX`wG5+MJ9(mDqN2Md!k2M3cF2M3eD<jcZ}Y+#?MP5vb;$0#>h
eQpAfb8KgmQa;AvA0N4}+AvSr3h$*wHkRSjz1`S34

delta 1356
zcmcbu`9YI6pO=@5fq{X+_)>VP*GAq*MlNOs1_lQP28Lpt$&Box^{Ff=%qc7_49$#D
ztegxftSM|Q3{h+;?7<A09Jhp=^YcoI@^gIh^9y_v^Agiji;_Y5K_-GQh|R*lz~Brr
z&x(P8p@u1op@t!zp@y-BA)c{@sfHn*DFwu4PGPEH$YPcT$+DENE?_NT&0?!z$kJe6
z$i&Ey!Vt`$$?W&?3nK%=OGX9;kllQ_i52mgC8<S;C7Jno#glEBoJ5&#u@tA~q!sZn
zFfbJHGB7Z#WGE7uJd>$~4J5+BI9Zih!<Px<GAIT)jt%BO3kC*;5{4AUW=2Ma62=s!
zW=0o=SiV}O5~c;rDa;EQYne+}7O>VZr?8~3E@Wn8DC8(%n>>#>yq^6QYe7+FUdb)i
zqQt!P)LTq>1-DqMQj7A7Z}AkBq@>0tLxSlRTX8{ZYKqq_meh*O5>1v{Y{i-Bd8N6x
zSksCUb5n0IX5L~iOU;Q-&P^#20eJyTh=N!`3=9m`pil(G76%g#BNw9pBNt<poO6EO
zWD{nA&3!Bej2==TrQ8e*47XTHGILWQ;lK|PlmrQ~7U!g<7Jvih7GFVTUVd_NMrM9|
zeqK&x5y%2fwjyDWVx`G!Y>Faa%fST51-F=sO7pxYTeG<^vQKVcQ}qx7naERIl30?N
zoSRybk)HxKqeus20$3Y@02xrk!N9;E1M&hN0|NsG0|z6M7!wB<<K(++&5S{l6WNzA
zMoeboh-GA$oWkKCp2A(j5U*6jn5A695U&hQ06czG`I8w1V<ro78Z$~vcIDI%N{7Vb
z2YXEwu(yiTCzo+fW|W=G#pS~WvZsiDatxQ7n*=lla#D-p!Eq2@oSc}GnU{WxD?L9k
zCqA{PD8ERP3F3WF!a)yzkZ(&SpX1VF;$fP^Ei+k)`;53c$fx$8VBktmEpbdPDNW1?
zEaIK~m)k@`8YIk`SX7i)S!4%Fuu!u>Y-F=1NAiT5Wq@)9V+unGV=I#+LoE|1cVsb$
zFw`=YFfL%KVS?lh5e85WS;$-q$|)?xs*|7aSeZ#MFfhFQ|NsAgP2M6ekbA%uvn1za
z78H4c_+UrMf>?4O!WBgDPY&T#kO4*YEvCYhTWp0X=t*w!1YR)~E+#SN&8v84FfmGO
zb{6PhWDJ^oMo>)!WJ6IP$bZcFzL`ZXAQQnsW(Hz`5?Yb^WHF&kMybgSLb^gAYrsKp
zi`BOzKR>4^XYzg_2~{PKV$>9Ji$5hbEi*6GJwGugAU`v&<nQD^LIpv(AeDL`LLWqc
zVxlMr#EJkBg&+c94wwZ_ZiWmD3<aR%CICrpEKIH(986*y987|f_X#Vig1w;%@`f-x
xE9jMF7MCXGWLBjXfl7%Y*~u&-UTi5KD+DHoh}a8&jYklolNXDaGCK<i0szQZEj9oE

diff --git a/python/ur_simple_control/util/boilerplate_wrapper.py b/python/ur_simple_control/util/boilerplate_wrapper.py
index e93b62d..87dce86 100644
--- a/python/ur_simple_control/util/boilerplate_wrapper.py
+++ b/python/ur_simple_control/util/boilerplate_wrapper.py
@@ -16,7 +16,7 @@ from ur_simple_control.util.robotiq_gripper import RobotiqGripper
 import argparse
 
 """
-controlLoopManager
+ControlLoopManager
 -------------------
 Slightly fancier programming to get a wrapper around the control loop.
 In other words, it's the book-keeping around the actual control loop.
@@ -26,8 +26,10 @@ NOTE: you give this the ready-made control loop.
 if it has arguments, bake them in with functools.partial.
 """
 class ControlLoopManager:
-    def __init__(self, controlLoop, args):
+    def __init__(self, robot_manager, controlLoop, args):
         self.max_iterations = args.max_iterations
+        # i need rtde_control do to rtde_control
+        self.robot_manager = robot_manager
         self.controlLoop = controlLoop
         self.args = args
         # predefined ur magic numbers
@@ -56,12 +58,12 @@ class ControlLoopManager:
     # let's be correct if we can)
     # so it's static, problem solved
     """
-    @staticmethod
-    def stopHandler(signum, frame):
+#    @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)
-            rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
+            self.robot_manager.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
         exit()
 
     """
@@ -145,8 +147,8 @@ class RobotManager:
             self.rtde_io = RTDEIOInterface("192.168.1.102")
             if args.gripper:
                 self.gripper.connect("192.168.1.102", 63352)
-            # this is a blocking call
-            self.gripper.activate()
+                # this is a blocking call
+                self.gripper.activate()
         else:
             self.rtde_control = RTDEControlInterface("127.0.0.1")
             self.rtde_receive = RTDEReceiveInterface("127.0.0.1")
@@ -163,7 +165,7 @@ class RobotManager:
         self.dt = 1 / self.update_rate
         # you better not give me crazy stuff
         # and i'm not clipping it, you're fixing it
-        assert args.acceleration < 1.7 and args.acceleration > 0.0
+        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
         self.acceleration = args.acceleration
@@ -176,8 +178,6 @@ class RobotManager:
         self.max_qdd = 1.7
         # NOTE: i had this thing at 2 in other code
         self.max_qd = 0.5
-        # maybe you want to scale the control signal
-        self.controller_speed_scaling = 1.0
         # error limit 
         # TODO this is clik specific, put it in a better place
         self.goal_error = args.goal_error
@@ -224,8 +224,6 @@ class RobotManager:
         # we're hiding the extra 2 prismatic joint shenanigans from the control writer
         # because there you shouldn't need to know this anyway
         qd_cmd = qd[:6]
-        # maybe you want to scale the control signal
-        qd_cmd = qd * self.controller_speed_scaling
         # np.clip is ok with bounds being scalar, it does what it should
         # (but you can also give it an array)
         qd_cmd = np.clip(qd_cmd, -1 * self.max_qd, self.max_qd)
-- 
GitLab