From 050034b6977d325f4025c15ca559a21361d9fff2 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Sun, 19 Nov 2023 00:38:02 +0100
Subject: [PATCH] just cliking the path is done, then it is ready for testing
and tuning
---
.../.drawing_from_input_drawing.py.swp | Bin 32768 -> 0 bytes
python/examples/drawing_from_input_drawing.py | 14 +-
python/ur_simple_control/.managers.py.swp | Bin 16384 -> 0 bytes
.../clik/.clik_point_to_point.py.swp | Bin 20480 -> 0 bytes
.../clik/.clik_trajectory_following.py.swp | Bin 20480 -> 0 bytes
.../clik/clik_trajectory_following.py | 181 +++++++-----------
python/ur_simple_control/managers.py | 3 +
.../util/.calib_board_hacks.py.swp | Bin 24576 -> 0 bytes
.../util/calib_board_hacks.py | 141 +++++++++-----
python/ur_simple_control/util/freedrive.py | 24 +++
10 files changed, 202 insertions(+), 161 deletions(-)
delete mode 100644 python/examples/.drawing_from_input_drawing.py.swp
delete mode 100644 python/ur_simple_control/.managers.py.swp
delete mode 100644 python/ur_simple_control/clik/.clik_point_to_point.py.swp
delete mode 100644 python/ur_simple_control/clik/.clik_trajectory_following.py.swp
delete mode 100644 python/ur_simple_control/util/.calib_board_hacks.py.swp
create mode 100644 python/ur_simple_control/util/freedrive.py
diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
deleted file mode 100644
index 6cb848bc0e1c7948199fc53aa4c7507705b8526d..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 32768
zcmYc?2=nw+u+TGNU|?VnU|`@jj7<HdUc(SJlaV1kzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7T>8DmC
zf`p6pQ;HJHGxO5p(~9zQ<1_OLN=xFQLV5+25SvEH(GVC70hACZElJb0;AJp2GBf~*
zD=R4~2n&URn4@?!1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(goHpz0UJX-0|NsS)V~%`
znh}j=h4K}kv_6!Esbhii!JP{R1_li%A12QR<=aE)Tqq5bXNK}&<_AIfFnM+;-vLVB
zgwim1D0h?^4S~@R7!85Z5Eu=C(GVC7fzc2c4S~@R7!85Z5Eu=C(Gb8A0*NUM3<sDP
z81{fV-~tQ`u>Su~eg=jm{0t0p_!$@y`574a`574A@-Z-+;A3DY;bUOX<YQoX&C9?r
znU{gVl9z!&hL?fiDGvj~S{??5JRSxHJst*z7u*aCE4UdL>bV&hLb(|j)VUcLZgDX%
zG;lF6)N?T~q;oMagm5u1m~b&LXmBwwNN_PQJmh3xSj5S|ki*Hqpw7v_@R);vVG#!d
zLnH?S11ARq!$Ecih6C&j3>EAQ3<m5B3@_Li7|yXVFzjbzU?^r|U{GgcV7Sf7z|hCa
zz!1*Lz#z=Zz;KL(fuW3rfx&`>f#E(g#IJ{07#KFNFfgoOVPN26Vqmz$$iUFg$iNWB
z$iN`W$iN`X!ocuVK|w*GAhD=8wMZ{9B_%$wD7`c{HLpZNU01gxGdm+cFTYGTB{8=^
zT}Pp$vLMwqEhj&*L`NZp0i+QfJGm-2I|jMBh59Ijdxm%@gm`!c>nZ6dq@<=Lmgbb$
z8t9p7V%1oZuTY$yn_7~QnU}7Rlv+}rnwqCzppclCqF|__V4!EJkW>jX1<me^)SLoa
zrR2l{g`)hV{1WwIh2nzL)D(sC%#sX+l8jU+w>T#=CAA2u7l((z8g(HmQT>b6yCMEA
z{#FXf8L7$H3gsDzB?=`OnZ*jZsfl^TD1J7^>SwHB1$J<0QE_rnW<iNUN`A6JabjgL
z#Nf<g1@)ZN#1sY48WM%X<mA+x)S|?a%=|ocJyiEWf&>;ES^1fHCB-Nj^c0Gd6LS)a
z6bkZ-GfOhdQWcW(^NLFn^Gb9SauX{Q4E4-)z=02qI0XYe6FvN4hvFW5X$9iA+{B8^
z+|pcy#GIV`^2DT^RE5kEsBaYVN^_G^ixf06OB9OGQy7R3b(F1JVoq@?*05FZ^AB;g
zQb@^9EzVOfQAkWKDNW4DsZ=P>FUnR(N=;5IEe2VrUaXLpU!stxP>@ksoS9s#P@I`t
znv+<PU!;Vd!Xd6M&qyuFNG(#R%r8|)EJ{@<D$UCSrC3lh3k^bOClFRpYd|rl19Dtu
zex7Y&Q93wP>N(}-=cFd)`4@m<HZjLB8AR(SfU}LV0w~Qvl0$x8szP}_$a_VpNS-S#
zQOHRJ<$|2l#4?a46*5wbQWetji@<3st+b@HC>3lNC@4WVEx#x?v7{umC_XtSvA7uF
z5=UrR-~!3L#U80S1#U3KsNr6oSdt9MZQxX=kdm*ES)!1i2ez|3BeNtmDL=6&MNeG^
zY&F=YMGCeGa9_jh3IG>H8Y!ueEC31_b>GD7RE0zZ=mIE^sl~}fsi}E7kW{Zs9RPNi
zGAKZ><Mh<L)S}E}kdwhBWHEMSAW>zASyXaYN@|)ydTNPdQF^h4rWFG;4HV}WfeM_|
z;#6?SnUP=4Kpp#(8Sn&x0x{kP1%ZNsGS%F}09pm7fU#&!FE=qSF+H`YSRpgF02CU|
zki~R9`S}ID5GfsnAW)qE<>4|XIVUq)4@AV56eVV*CYR(FRmP{~gGwe)=7yS{n^<7v
z5|CJu5t4805|ERamrB^Y0#MN#Uy>gWX29H;o?7CZlbH>5Zcb_uEKZY3OBB*Fi;7FY
z1xy~O7AwhDNCp)L3Z(@KASdb-R4OEbtGMLMw9M2LPzEbXg&B~WT9lp&5-LtrNXyJg
zEe5GAPtD2E0SQ(rq=3tdl+@zXqOw$ll6-}vRE6Tyg2bZ45}dwC$t}=J$t{36-o-aS
zN5L;Y#91fAIV?5DF*(^8RCN?3X6B`)DBuc`(vr*^Jx~UYFGwuOfSCmn0EH+nU1gcY
zrHMJ2RjGP#7EEhFPJW47QGTvVW^xHG?O;pOQ%mA=^HWlDU^?Lf4A6*$l|e9ma(+Q2
zjGdcU0<tM5GfA(Y62t-pXhBX1Og^tP7ZM?P1u&t4%)I>M<c!RGuu{;PQDp@~BRz#&
zP$^adY9S;l<R)h3f#OF;p`f%xA+;>Es1nljDFHPjlJiS*Qb18tQd$HG)Xcn+e1$}X
zw9-6qU7(PfQ=D23D%GJmwIDG$J24$x)`J>MB^jxC3K`JCA5=>e<Rm7iDrDvrmw;Li
z`DqFuAA(ah)NYtzhK71z2c;Kf78IlwDS+z(h0Ht!P)I7|7lBI}utRkilobpO^c2dA
zGD}iH?G$j42yTpkybY=!)AJP)bMo`jA&vx#q?RaTR3;T=rYPj*gK9P1wEUuEaN7g4
z2$n%v!BS5lF$J8a6*Tgp6`-a<N@{X`E~KTBpQn(Y2CB@GQx(!uQ&W->ld~0y^A$kh
z#-OZV0nra?N-8Ahr+|}qL1uC`DEENs=-kAj?9?I!aCwrbP*9XwT#yRQM>+Y)VCOR^
zE0{w<Au$hBH<u>nDCFdq>lP#y7lZOtNotWoepxECHU+s~w<N!)uoRS$@{038dD#r2
zAC_jp#wtJ~5tiFPMx}y0qJXLl5?Z<nS*67#3Pq{8`DLjJ#U-U_X<#p^7b_%!MDr5!
zN<aZokXT%zP*RkcS`5<%s^<~v6(C6o)a(H@?7-D1iVGN&6-@O&CW7oq%*jzG%_}WV
zO#ufUsH#y=Kx#)r8b_J=dGJa=AwN$Cng;SqOB9q~ZDB+!skkJus030>A(}@@5Z5N=
z6z7BE8`QW;E6vGKNG&SLFH*=&EiO(>Pc2qRN(8wD+=|F7)=@~U&{N1v1BGO2QCebh
zYB9unpe9gqeqLT`atXxToXl)ca#twHOw23J$S+k$g)XaxX2RUW^vq;vE3+8xmtsW*
zWd##GP-iGHH?^cFvnmxSXF_r+tf+tl3bY+s49X#i@NyI!vkb}##(D~AndzlPsS2Q=
z$;dAUmA9Z~H@JXS02QfasX6({nI)A9<)AE^T$Gwv0?n4Xb_y>3ey$1{iACw)R8o?x
z$)K!Yq^FRQnv+@rF`^XYXHZxbRVpOs=YqOz&~%WW1}g6ric)hDL9KgTq(T>}0#s0d
z(^v|qXvxXUOVtGtMbJ_b<P2Rq1y~h?Tz0`qo05D`t3;tFGX+!@=pm}9k_=Fz1XSE4
z=9OfEQ$KnTfy)z60SK+QKuHZ;G-iVOI;r6D5gd*Xzh|Vv8-JDgrA07KF(eui!C?vR
z?m@~ESit}_oB`DTKf%eskirF7{|}pgf5^|k(815Zpv}*~@Q9CrVH+O<!&W{9hE_fX
z25&wF1|vQOhUdHt45xS*7&h`UFl^vuVCdszV94NQU@+%pU=ZMCV7Sl2z;KU;fnhxl
z1H(EV28PKz3=Hl(3=F@y85q`cGce5JW?;zSW?=B;W?*3CW?<OL#lVos1#!y<E(V5U
zTnr2wxfmFFxEL4=xfmGaxEL73xEL5jxfmF(axySX1G$@%fkBm%f#Dek1H*a_28IL<
z28MVJ1_mw;28K=S3=GZe3=Ed+3=GfN7#JqAF)#$MF)*01F)+MlWnj3%%D}LJm4RUy
zD+5CfD+5C|D+7ZGD+9xK76yj>EDQ`yEDQ`4EDQ{KEDQ`+m>C#4m>C#sm>C#8FflM(
zWMW|0%EZ7>!^FVg&&0r>!o<M9$;80$ijjd~8zTe5d`1R_S&R$}6^slF<%|prWoQc(
zK<z+fa5pk9zg(d_RUsK(q~=tD`cB|xGMLE8&o79Fw&(PUO7k=z-3xGMLa(SK1-?^4
z4^$$0X@D9f1^LB#MXALZi3O<|8fH40nh-t8knT|?sHjZJ$xqG(rR8K$*#@>rSpj4M
zC@*H_WtL<n=79RZkPZ<J+k7;VON)w9^Gf0i@?rf=WRYm2SOr@JbP;U@13d#{u$9PC
z3bv483W%qQQj=3N%To0~&3kYkHN-g}Aip>j>V0K!4l6ByR9K)24BU$X8w%+LSb@t=
zXtf5a=`!=uA=1?83_#2RMI;Uk={YCnC={oblosGn1Qw=lrwC*ua-vbNRe-jqk(&P+
z;26~b4Q_xM7$6xPP<dahqfne!mKqPLKI2m|lS_0Ia`Mx`EJzxHZp#5_P_PBn2%v<b
zfg~DUlnE-TbwEXrjzUQ?+>V^oA{|H!Lrem>vlbkRAQlL#r{osIm!;+ut1DP3<Q3>u
zr55EEYk&&+V!hnNig@@qNU@HBnWiSB!wk|0!s;o7Wa`T;03~1YG?Qj0EGQLh6{^8L
zSB4BhfSZ7zv8K!tg-p;01uO%WCFYbuav;cyXrWbJl$w{Eff80`&_F?rLnP~vA`05|
zQz*~O0p;`p(C`kV_AD$-Elq{gAg~$>QXYVvQw()ErbG3hGN1-HxSy5|@)LOcuQI<>
zy(krwwm@Z0CS>%ZC_g6$#04cbh4Re2l>BnoV2%bjOP8iAWEO*lOF+#uXq^U<NXsnB
zEeDM<m8BMCf*M+&?qO+BJg7JXm2r95U|rBbE=Y1y26dCc3Q|hJt=~k2q|$UyW1tAq
zB+5vIl~iz*+K}`L>UqNLD+aalpdNMf3HFB$sFma^fJV;1RW&F&VeNzDjKn-h$5Wv!
zwFs;^GcP%(6vP76{ZNxi^FTwerNx<f>H2y3CAwgi0<5<I>W1c~mZTQJ!X9KpVop(N
zVoIe#T7GF>ib7(Zf@6RusJ#X9BW(8^w2=U6B7&PVpgt8SjpZpoQ&?sdxD^5mcJLS)
zr~w2TB?N_yLV0RtdPa$kLWpw!xW3BFODzIrd4=4>lA_ECa670NRA+$u%kY6MaPh(b
z_8chXmLx0KDq!gqYosUU<|fAH<>zMRCFbZTK)8uI3T5%Ri4{5ui4ZEapg6uH84`~m
z<G{l4c`1oSMTwOPwxF^iKQFad!%R~_3tWonA&<GjU7?IA2TEU{6a^Uv1$6ley
z6&wpl+DlSFor(-ldMQaQD#%H!gpbIBBOVlTAk)C3<qD9|a=3P|mvA{69A4l;t}q3z
zLs@|$3nBF�=Q*EZCWl+@z6Nq5$dx<fP~*fbtL6uFMi>D61GioB^ea^D+w*v~59w
zuYe|$lwT2T9Gd`E4pOF9RHk6701`Ed1q&)DK$913Dqhe~38)lFQ~<T*!JRn>7d*PJ
zfEc&}XNJrYupb}<^($U*JSy0t46cDIFi5b1#;IU=Gba@^%!@JNg`_&MD7{$07Sz&#
zjBtWOlRAM2^*JOMu#(03X(iw?md?$W%ru4g_`Jm2)cAM>TU!OC_;}EeP<*_S6{I*R
zN(GJmC?pl7CT6?kB&K6;uw<5`=0eni>Rb>n8qvA{HA+Fv<g(Np)Mhd`fRQv57OR8R
z7UEY1Ye0b2<`(E-G{4czg&JFEhQ(NAP<sn9X9g<dixV?bKvk)F3P>E{BT#g~y$>o^
zi=b_U%rsC*m|2{cn5R*eniHR#n-Xmht7!$cURfbCH#apUGqEHUG~ZKPl3xHCG5`$-
zfyb>tC4EtfLPlaiK`LlyO#>vOke8pQo1C9l1{yuhOGcVf!sP&syaK%(P+^{~mzQ6Z
z3+|4TrRHcV*nnF71_p+j;A%u8Q^8Kb)WE<1?mBSQla>jp%0PuGXfP=mG=v7K;K4Z;
z<l+KQBOdG>a7NQBPR&aROo4e8G-HIA`qNQJffi=auu!lCwMq5LQgalv6|xHyv=qRu
zE66Wa&{aT<3`0##O$A+r#GHbRL<KE{lEhN5rH~TU3Oo`BE=e=<6cS-=Xi$EJ4@80n
zn^F^tD;0{%6AM7DR?x^Vf(|lgrh#G<Izt7Lhm;L4!yzNlpvDBqr67X}Q*<EB!sPtC
zG;qs7L8CIY7&LO7SOS?<RRFg$K;<52^aMJ<mk%>o7cn9Ts-EBjhT#6cIx7PMGiZUn
z00V;obdA6veg=j-eg=jReg*~`eg+0+eg=l)d<+b;_!t->_!t<J`4|}P^D;0j<7HrI
z;bmY*;$>iP;ALQV!Nb5X2f7A8gNK2EpNE0rJ~snH4>tpY6E_2cHa7#qCoTqt#as*w
z*<1_^L0k+B@>~oIr#KlHHgYmB)N?X0IC3&DsBtndeC1$Z*v`SgFo%PIA((@Kft`ba
zVK+MiLq0nLgDX1&10OpB!$CF%hUIJw4Ebyf42o<F3><6_zwBaTVCZ9GV5nqcU<hYp
zU|?cnU|7Y<z);P~z+lhHz`)MR!0?fUf#Dzv1H&Td`T$E71_o&s28Qp<3=B`185k}z
zGcfFDW?-1g%)k)F%)lVa%)s!CiGg7=69Yp(69YpC69a=d6J+zmeMSa`Ba93Ty=eV>
zXmAoof%3Jo0wk`|@{7t7iy-5@@Ubvh^8zxK18V(&vI9gLXdJk-1U_P{kds&r$@e9R
zr3$v76dY}37K>cq7(zz{APT(nLKGYnAYBS@T2Cp2G_1gFEDdm@5~EKZn4+PniOoze
zP~R{!PY>!UFVJL<S7LI0Qf6YF26)avM<FG#BvD79P)EVb-_tK7-qQt^T0!Fz;4Xg-
zxOr6q>Nl2W<SS(7<(I=6z~IhDK~a8EViIKXx;Pcmipfo^1hqu-b77;-3XmM53tFN9
z?i)Zy0T4r|&_M`LYaHC<1-BcDVVxOpxs;QipA8ya%~uEw0*?UeCl+LaOQqD}lFVGl
z;3Oz(6({DVf`d<4AvrN88PcZ$8>j#o3r88c1rKv4r=ks$gCevbGfxki2D~%#Qb9H(
z7o&w7B)m|fsSqlr2QI}^Gz|0%aEh2A$1iB!LoX#YwE)D>D1;^rWzdX8NqN3PK~ZKg
z*bNF`zrbPvF`)yFQ&3~tLN_HpO#!ixK%t~4HL(OTb_wmyfWrrp(x6T#OD#%FPt|~k
z#V3}f>ws5s*g_qT5RcAOP*pGm)w2*Wuu-srv!n<#jRz_N6EWr`Kx3|{iIAqXvI4B*
znFz5MYzo9csDnWL3LI%Z#5uqXG`FDv4<k?t0j;P=2G8_>lTdDANd`31ApPik=qNH`
zG#V7k$_j&`-v?=)gB%3H(6M`413eQ++5riIFla8qRw)DAAp}jafYLj7)J;JnGfx51
zKh*?#A6z*@1{NUGVEK9QWh0OU4j3z{zy|3;3`bpH1YKMMasdb<oM()-VhZBE^30Ty
z3>rBPY&KTcLDC?oE-nVmYk(UEpsE`bMtP~_3SjSn(-}S5?<I-F*`UQ;pb0#}%QB#S
z0qQsm;z!709MFViPJX!p(gYzmcjl!)rhFlVcw!M~QVFzJ0k*;pG@g(THV72qAdHA~
z0|T@*V~|*fREe<Z4TXZDe9*dw%>2B>9EIfk)U>qB<jmB(5<;mE+~C6+!5~+FFtSSw
zvE`4FRM3(Ou(hQH;Nh*3)S_G+XjdCFcU_#Dn3F@uC8-6)x+Td(It6P+fVA?E7O;Wq
ztCCX4vMh90>L?WCm!yJbrE(NN4kqM2@JNkrB2f-Bgw;Zz00d!Bu1Et<&}O766hS(4
zpoJEoaU|$i5U9lhcLPWd2t(qUG>=(<OQej<^o-OZ=&~prMG(YGQ2UAU7+Sc4oTvb9
z+k>Waic3HPtB}PqU^jq<CqVP2d7yq5IPI6@=PQ810f$FR@=FqP6iPC4Q^CV*sVR^c
zgs;Q_&-sFSiJ;B`M!LiA*^<Ok1ETzBgdTa&NPy-ra6$*4)CKAXffq`GT?vV#{5;6S
zHE7i*WJwy1u+PT7iWXugY!D0N<#NzGKWxwyJuxG#F%2myh4xx;*bECd-Q;{oepLr`
zBSC905X=2=EIfqx2yeLwF2$4;Kpjs7tR*rLg)i6)NZL|>t@O23H`D{gA2?vt!Amuv
zP3Cy;Duu+7Ty;q4gu~m=iGZBcB3<xkr7mdxJu@#IXPQt}fHbX<10SRZGGzcYPN6t6
zJufi_#l^`P`I*V7#kSGvpvBUuDFMZ)r78KDpfQEwRCOH%^(<(|AfzZUuNX9us2&S(
zI)<OI8vuz0m|}2<C>5vXq$ZarlxJinXMk3RC1)!n=A`ErWtL>*f^#S+{lK#wt}x2U
z%m$5qfM&>YQj1_gQ(RI6TLTNN_CXaLs7xzLEiOUvJFK=aOw~0)^-~CFxl?8_Xwo1B
zvd96viW1S205`NzMlXx<bHU{Zbifm|dK{bAL4gFSW<imYSOl68D9Oyu%g@L!Qz%MJ
zFU?6Tf)4uY#en<&BJ2zd8$cWE1sE7$^Z#r385q*|85nr^85myiF)+;LV_*p9V_=Zr
zgUtE2^D;2_^D;27@G>ym;bCBC<Y8d&;$dJA=3!vC&dtCug`0sPnwx<^nVW&(Bo_li
zEp!h5DJKKNOil)d2u=nDIZg(Ka~uo|GdLI+961;m)HxU!UP9dh>h~|>U|?wFU|`VW
zU|^8vU|^8uAg&uds%kU@MnhmU1V%$(Gz5lQ2!I+nI2tR2x1>QDD^`#RVNi1=Gp{7I
zC=s%98oVwNH1e7U8FU6M<OQt^OG(Ye-W4}Y)y3Kq2N|3Ong9Z=9?Jx^qaj@YaH|Wn
zW&<?CR0$px(}S*^05xC1tya*&5kif!^!&seUGQpHSbGfXzB@=W5VW>5H!-gg+=(hK
zNi8S_jSPX;n}CL&!EOSz1VJ57(3%;fjsvuBha<E>M(9G8tmqbJR)N~FpaEvvOwc|t
zNUKa4GG_x`$_rocl9XSfkPF#?0qF{sWaO8oXMonYK<45>OLmiBL&%_jNh~PH$xH_C
z0|NyQWRx4Ug9+5egRJEM1$lBJc;6Oi<TXDRw7UVW4cs8b+esl_5@Hr86{2A9(o<*`
z05lB-T|16<=!uAq4~g!AISCT=#BQT2%_~+&ECJ0$dQ@p5#lN8e2tXR`I1)i_VudbZ
zO&TmYA|(Nsi(oX?jg3fl4wFrwkO6Vnig%)Se_{<CaF2)LZJ&g8Zo(o6l61hs*XqU4
zxelVYheEPFj<kZjAPJUI@DyA~Lmi-XbKr$Y;Glrcp&%t1f+HMIEu@ZXz`_JZgO-F@
zDS(P3@B!_hHT9_}3YDp#K~-2k7t6j=NRT4=M*&vg!nSunml~$07NnMx<U<NLykQB~
zNW7Q81sBXGFdC1mVM-vLhAt$8ZhC=CI8tr5EG#&{3-mx$ayob@6ufF5KEVdv#*8Cz
z!IkOe=jBwA5GtVc|8}7HZO{RJ0t^hW^ZEYsGcY{hXJFXQ&%lt%&%nUN&%p4DkAdMQ
z9|OZiJ_d$NJ_ZIwJ_ZH_J_d$+ybKIyco`TD^D;0T;$>ji1>Fl^%FDp;n1_L35)T7I
zJP!kd7!L!(F>VHi3T_4lFKz~g&s+=)bGaB8a<~{6__-Ju_HZ&Vtl(r|SkB47P|C@`
zV9d$DAj`?X@PdPZ;Q|K(!#?O*fO-xF26qky1|tpz23ZaUh9~R{413ub7^>JA7(&?@
z82+&_Fx+QjU|7J$z>v+xz!1vDz@W;;z;Kh5fngRa149Zc1A{np{{I0B1H(z^IRL#Z
z3=CnQa2Rfl#!>%_h5%hd091Gmo`o!sts+_ZnI(CUiEi-97H}O8+Fb%#!$R^B7Km=}
zq6g@n20c)J0=%3BB8R%D1^aRih)QgxK$@K>OA;VU6+p}DAS-e}J&<^irP1mTb?UL2
zsLLi`{XJMi40PB5sH*_p7X<1DfoG{RL3;&@ia?{~8jvoCLK1W>4Ok<@DKN7rTVnz0
zVj;GcARG?bB$QuJiCBOF@e#~w*t!((#8P^GK6n!*sQXg@?!6YLDrkU~MkVKiy^)t%
zTnye;0nrXI3ck50J+&kpyl(=&kqF#D1I=n6mJ>nemQp~wXH%gIh#&!$2wLG@k`J1l
zRM1GzFM;eoLS8%tu~iwg76!7Y9NQ`;*y1AaEFi+({G!3Oj0v>52$D?GQ%eG&VFp?|
zTbx=FUy@j=QIc2+@iAl>8AuAm(pD%*)+;SYNi0bPEwY4Xk`mAgeTac6kf{>TB6su|
zb5Ktm++qV62jW1sE9s?_Xeua!=h8reU=^^{x}ZHSko_^Ji3}kQNx}+6si4Khpp{^t
z1z#wq>*Rn^4s?wrbh8-pGFNcpmH~9;1hlIQ+5`pN?ga59*4<7z3Yj{fB@iXx+zLLX
z0;*pDv`q^%^pKPaQUKb~Tau4uvlat`GDsD8aV%u-w}LTf&nu{(1zx%jPQr*aYQ^yL
zg3=Q6k}DO!yM94};Qiu7pi^8>wyc9!<dzgwGC+qc3^l<yEib<uy6zNIYJggn;N`%X
zDXED$mAa6#7YY)Kz}v0CMM-gfPAO=>2Xa6TIBn-=D?rX*(F6GbG=Kp<Sq8Ml5xfo|
zu{^O7l7~S1)IpO~pwSx8U<OD7Xd6a~0%Rf`wxI?xwgB1~oCrP)0@OV!PAmfz2jC`i
zacWMQ9>}oxvc#fH@VH5FzCvC;Xv0bg_&^EpU`%3(0>~E72#`8x(|vIXIG8|fduZ;@
z%*#kE$}GVWAK>sx&xg4?wI~(j*phtc*)HIa$*BZIS#fHr0%!~hRA}iZC8mJJc`}PZ
zszEz0zzzYGFQ9{GbU^!yb5cRwDv)1{G?7+}fcCMc=z-h{4Nb@)2B31Ns8WGJ84+FO
zsR|&6<rgVH6N;fGXh;Zgst@FLts;=);N3W|e?YMc8I@yDhG{ket$8fUPbmfMC{G0C
znW7Yhq|9{a@duzn7qtJm7&MdxJEsA(S{#%PQd2<Qgt-D#9D>57D5p{(H@_%VAtNy_
zrC1lVVIDl-SXvA^fv6aCT!(^!0%%h#s6>YBK7++0XhS-93t4IkIH5pY0Y2j(wGxyL
zK%G<2X$3h7a6O={;hK=;M$lXb%C(?X+n^JDz&;0`3IQrLz>dm+OlU)sR8ndNXh}LK
z0~9Bx=A{-Z=z{iiYbvBxfQIEVOHx7mbs;NQU>n`w9bB-Py5ML_P6Q3?fw$L#Pb2~@
z-c`VKYEf!>Vo?ex{-O4RDtNE~;8QD%pb=9H+7%AU{m?xTpaoXQ1yfFH5#&4^(1IxF
zGExQDGGZOXVsQ8(4Ny!%mr&({H@c^$B<3ZjGEnWH8oUSRKqCfH9D*8;xUe$S+ykyV
zpev6++ibvmWNX1OjyTq#GPOiUp*X*&s8Yd7UqMeHGQSkOXB!+X;G)qGG&}=3TqY+m
zy;z|Lv}+EOj-jhCKt>lOW)`O^_~n;)f^M<{W%Se(SJ1Hz8tN{YDd3S6%wrKiJN7_s
zfezw;5<X;XBr^@Nn++xbib9Z2^bkjAK^B061QozrbwMZDfkzNPXW12NXflB3{}+JH
z!~^wv1sE7$=j-3&XJ7#B0ocpWzyMkc5DVQ002&7XodLLjkAWeJkAcC8kAZ=SkAdMC
zF9X9<UIvCeybKI;co`Tfc^Mc|c^Md@c^Md_p=$(I@h~uC^Dr>z@GvlZ<z`@5#?8R6
zl$(JefSZBg2Nz_W0O(vmT`mTOo6!CJHJl6#hMWuxQk)D7-#8c;-f%E59O7VL*vP@a
zP{qN(V9UY4z{<hEaDttIVIMmKLkT+rgA_aDOu#1SUVm{m28Q3P3=D@@85pLrGB9MZ
zGBEhFGB6mhGBEsMVPJU5!oUC;2iV8Lz_6Evfng6QEa=)_8ntXR1V%%E^bi0Qs-vf9
zD8RO@gHAC>g?DtZ@7NhVMT59WMR0on)IEb8y`nIBiUw%00H}LcjP>*l*d|zVj_w#e
zMFZ)44cOQasIv@i`j4KX0omOO>6ne4qJeR?<mf3H(4#+!Ku4MmqEj?L$5BE$VW6f8
z_z)D(`Jm8YRM2*P&>71`>BV}mT?p~8T?i0upv~O23NF3@8sO#NNT;wu23?gEz(?by
zf|MXOw1WDJh+{7xhX$bQSFi=|(E`_Va8nW?OCA)83qZ$WB8)&a8nllEtPjKh_bf{?
z6tqD{4?t!nq7C#6Kqr$M=qTtK=owfVTR_+ILq=e&Afqwh5kt_aXV9aR74q|vAvqkh
ziXJpx<nIl(1*R)4vqGUt0er%DW}bqP3up+-*adv(2UHzs8Ga(@7|gWdROqNA=)gSi
z!Im0e`!d1nzmoGyi;7bf5(^57@+&~QccD8(U^YR+G&wORGYN8xIOM#*)D%#c8@$W2
z7&4Uv(@|8K2ioroR}Lz>!H$9k5#|ZG8U>jZsX5>?Um*u?>4CQh#)G#QqKH8DM8e`5
wG~NapMKpE+n-3-sMuCnvC`v2`og52IWXj+p0YPIE;E)0x5e-TnkYl150G0s)%m4rY
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 31e7cb8..a25e713 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -133,7 +133,10 @@ def getArgs():
parser.add_argument('--board-height', type=float, \
help="height of the board (in meters) the robot will write on", \
default=0.4)
-
+ parser.add_argument('--calibration', action=argparse.BooleanOptionalAction, \
+ help="whether you want to do calibration", default=True)
+ parser.add_argument('--n-calibration-tests', type=int, \
+ help="number of calibration tests you want to run", default=10)
args = parser.parse_args()
if args.gripper and args.simulation:
@@ -240,8 +243,15 @@ if __name__ == "__main__":
# TODO: fix z axis in 2D to 3D path
# TODO: make this an argument once the rest is OK
path = path + np.array([0.0, 0.0, -0.0938])
+ # do calibration if specified
+ if args.calibration:
+ R, translation = calibratePlane(robot, args.n_calibrations_tests)
+ else:
+ # TODO: save this somewhere obviously
+ raise NotImplementedError("you gotta give me that R somehow, 'cos there is no default atm")
# create a joint space trajectory based on the path
- joint_trajectory = TODO
+ transf_body_to_board = pin.SE3(R, p)
+ joint_trajectory = clikCartesianPathIntoJointPath(path, robot, transf_body_to_board)
# create DMP based on the trajectory
dmp = DMP(joint_trajectory)
diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
deleted file mode 100644
index f697a6cfa3b4672a101af84fd21aa323b24910de..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 16384
zcmYc?2=nw+u+TGNU|?VnU|?ubj!fOJUc(SJlaV1kzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7TVUfs9
z%u7s9Eh^S4sDxNBN{)uWXb4~rfzpyRT?<|YV<STYkOE~TMFnA@P!MwzkA}c#2#kin
zXb6mkz-S1JhQMeDjE2By2#k;rC@ElLsAphcV1oKL1WGfa(f^?!7%C3yIl$EYgUSa$
z<ySyynEYR;JWT%&H2FVJd6<50s75HozzkI{1Eps}C7={kbd(wmfzc2c4S~@R7!85Z
z5Eu=C(GVC7fzc2c4S~@R7!85Z5Wp4!i75;W3JeSkJ)n-X00RT8|9_jGfng~>14A!A
z149)*14AA^1A`4e1A_uT1H&gi28JVi3=9kS7#QOC7#M>17#Ljm7#P&~7#KeAGBB*;
zWnh@V%fQgj%fQgh%fL{}%fOJ!%fO(_%fN7lhk;=h4+BFW4+8@y4+FzNZU%;CZU%;U
zZUzQ^ZU%-|Tnr2^xfmE$aWOD7a4|5Lb1^VH;bdS~&dI=#$jQK<%gMl?!^yxP$H~AT
zz{$Yyg@b|NGY131BMt_Ja~uo|6FC?d5;+(cVmTNXVmKHWA~+ZrTsasR3^^DWuCp^R
ztYv3lNM~nYkYQ(F_`t@%u%C^Ap_Pq+p^=S&A)SqZL70t!;UOyn!v<CchE`SvhB#IR
z23J-F26<Kn1_o9JhMO!53>#S(7(!SW7_?a!816AMFw9|QU?^l}V31>GVEDnr!0?@k
zfuWU&fkBgrf#Db<By77u;fpN-ki-<AKv^L%FGV4<M4>!0Cr2Txw75i}JijPgAt|vq
zHANvmPa!2Uxg;|`FR`dnAv?9QSdRg!Q9)TDvqZgEArV<cT7Hp2ZhlH>PGV7JNu_$R
zLUCets*XZZYI0&}ajHT|MrN@>d19qPWqzqba$=rBabj7jLU~4FNorYY5m>Z5F|P#4
ziqxFc+|;}hup<)-3R3e@GV{_E5*2b1i%S$VGD{RPiy@9qRLIH9%T7&E$jK}&Q7Fwz
zNi9++$w*bm$j?tv$WK#9&Mz%0PSwQc{LIv1kjj+Q!qQZ@lOep!Vz6Pk`Nbs)$@zI@
zsd<?oS0(2p78fg&<SS%=`~mWOQEF~}8OVtsyVEj@ic1t2ki1i#s-BahP@D?#bZVYL
zd8$HsY6-}IOk_`Irh#m$R4+<Z$jb*?Seyy+Pi9_5YEfnh$aqks6&Iu?XC~%kR)GY-
zL7QKsP+XFsP@I{YnUh$ggJf|@Mrv`YLSj*>LSjx1$aIC`#N1Q`4Ums=Dp7(394DD2
zV1=1^sPUVhrjVDCSX7i)SqzSnqSTVoqCAC?j6`Jj!-E?VE?}o2VgzJ<d_hirNwGp&
zPGY(O$kF+^sU;a8Z$O=n<nly?;#{zsGjmgQ%QN#*^2-$xi}FjssTMUl!AUAVFQ-x=
zDODjOF)t-2HANvKwFnWLpjd$}^-;*l&o58_t4>ZVN-b7MOv*1U0S8S%QEG8&UWq=E
zD?<ET{GmoFq$HLkf>JKXy&%&OjwnveNz+RyQLt4oRM1xd^GXX+5=&C!ixNvx;l`m$
zDA+2P8W_N(l@$t#Qd3gXGV@YX6iSN}aud@tlNItxbCXhwis7ojh9nlH7c1B*fGAW6
zs3(0uezXN8S<vbx5D!&ZQGQZ>NqjD3ZIgnn0tz3lR9PWYAulyGMWLu91-?{DAtfJ_
zaFK=J8o+MLO{|E|EJ-a&1Qj1JSL&gPf_0>%rYXe7XXa&=#K&uZwCbR`LkHRYI$+Q0
zfWkjDz96x<Bpy^i=_usnr-NCVRt%6_=?q!X1@f~m#D7){N=ix$nQ5TXE+erRWKb!n
za4FVNNJ`8Gl}@R-3YmEd<(VZJ3TdTz$tC&uImLPfiA5!ui8*=<e*Ph@R^UP^J+lm?
z99k+Ar6#6S>gFb<q{18uPFs2mIhon13aB2|0he}=Bvg_Q_Ju-fNwOY;C!{QexE)qr
zW|k;qr=}JZE9B+p>84~Br6!l;RO&+GRu`0jb&FDS5=&B36bcfH5_3~aKq<sCPa(e~
zBee)rkf#*uK#FNl%1_GA&(;MQ2uiuoJOmO?OfD%+%t3Q}Nj|u2N>nH>0u{VPuuKQi
z0CfS>o54Am=@}(Cl?rKzdC8fnMG6H)`RPT8xw#-~Gzt=ni$RP;NRVdc=P4kCJ-8Gp
z1uMV^aZLtYOu&E{ui$6}6QEKtxhOTUBvm0fBQqyO0UVsE#R{OJ861zvpsERyyo&W0
zoJ&g-V3{7A^mV~yYFcJ8C_GDw!IJsl(pUkONuWmRV0BzheqOplNorB9LRn^UW_}(6
zgKJr8Q6;2o%Pdw14N`#Fpr?Rp0z-0XQ4y$8DJ}t(72uqq04`Y}6<kWbLTW`KC;>o{
z5U2_TRhFO<D;Ja#Q}UsDK%ND~Qc-GR4#cbAP%cX>%FHh<2GtIwInYcAPU@i0&jF>~
z+}y;x6i5l9tB{eI0!rhl3W<4n`IVp;%uLfw%_)Xfl9_2~sbJ?i26z@L6r~oHW)`KU
zD1e=y3n}-(rFKTD0?cAW5>?1cO#~&F{G#;4yv(Z96tDv`^Az&)QWcU@^Gb>ma};v&
zlfkZp8kv-znFC5&pyaNQoSy=!&A_z)$g9W+QlUH}GdTm)Fi6ZP2US!c1)$oFAtWPJ
zp&&mquLM-hLuy25tQLcmgX?inrD4So0&1wFWr9N`uOu-u57f>9rx18L0Hw7=lsp5I
z(qmvK$p=M?LQ-lm%r1q*(vpn)B2a@VDKjUtq*4#;oV3im#2kf`)Z)zaJcZ<p{LExf
zO;iLbHbB9juaKCWoS&PU52{2!Mi!?k6oV?u%v?}B6cprxB2OVbKQX76AtbQ~)S4(Q
zPAvizZlD+iFKSgt%}dYBO9d@nWk}9Q%u7!#hP3v;t(Fp~9S|!r!L5;!jMU5`cuG|$
zD9X=DO)i1B7-T#sv{EYyQj<X?Pf5N4s00K@A-I+=E&+uNDAZH)N-~Q;?np@mwI^YX
zDu(>B)S{e9h2(tD+S>|+#H8YqqC{{r6Vj~6NQAecATeDG3h3laP!pzDp&-AwI5R0H
zRgVE=2TTEEAup(Gfy*m+=7IBZCOFsYK$@QgsYQ?o1$n$IKNHlTWGE`l%L66xd{A2^
z733di&e2l<C7R;Y<ovu8STq$BW#%Pk7UZOYf&rXp%2Giwo}RCeR+OKsfJhaIIqCUD
znI##ypoVloVgaa43P~BD^a%13s9IJ)D1)X6P_q%5CJ>f^%c0C-g`CW!B2a4_RQu#B
z<Ywk&=4MtwvT8|YZYsE@PEA2ATy%9A(o^$NLD?!Vza+I7Tv{@KU7MVrlamS!`^?;e
z{Gt*CaGMt*i?Kplue2mHM-QA8G7IC=i!uv9zJh8D0yQTx3*8~2cuh!8Es4*~Pf5*D
zfa-+{Feod4TU)u%I$5D0F*!RiJrz`)WivokgYrva4vd$aUr>qHHWJ(mZqQ`r!<-cq
z;^OM*@0nMUT9lTUoC>i8tfnY6ITh3nfVv2zFbE=qO*O29LQ)N_$q=fQp_1vT1*s(^
z`FhFuMWAd`RF+x<v9};IFF!dsBQsyGEVH;2)bvk<xzin@Ck!TD1PdEbnt-wMN^=V;
zL0KfP044-C5+q!ZnFkB~qSQQ42LV)u7ZhcdfvQwcIi8r5lM3$f6qh6xfjW&P84Bw0
z>M&i&IjM<7;99dZFE2GYwYV6ZQo+fpm;u!PcjIDUxWoln{|}pA-^9<rki^fxpvup{
zz{}6Tu$Yg5p^uM&p^%S(!I6)FftQbg;W94+!){&%hAF%Z49>g^3~Ia#3>>@+3}<*4
z7&h@RFqHBzFr@P^FbMH5Fzn%GU?}2dV6f+AU|`{f^zpB7F)&<)+I<mZKT=<KlsOs#
zqaiRF0;3@?8UmvsFd71*Aut*O#D)NDHlzqL67CD?ii1ZiKoX$AOg*rWf~^9WhD#|c
zB!Nep6H63wLBnUDA!|@~wxB3AEwu<VlmZ?!(ouj<86ar|jYKMBs^@~I2U1c~3qTzp
zQ2!M?)SjQGP*7S_kY9`#aRxaChC!WkSoZ;<Clx$^ppXU{LDW+S&Mzve)KhTI2an$s
z=jZ3?scXV5hnWJRlTsDZGAmM3K%-Ca=^gN-hn|9CPH{fCD+-c?4hVt<S3!O(&H+tl
z6f4A_m|Bzw>fih2mw19k1waF{sVS~SMfpV<>H&!*8O1Q&r9~-esFMbuQ7O1x2tz@_
zRw!10I@2W@O5jk+EQa_R6i(nlUFe9rRZO0e4kWU0__`=HF&o8LP*{L4IEM8~iW2jR
zAp>j*whDO#df@pI4N$^}2Te|4nwpuWkg8y7tDuyht%T}ekf|UHad;}GR)nMyXrLl9
zub>n>CkC1~%7hGNzz1s5@{4j4OY{^%iYgTn(-SlE^pvn0n5tl_P+VE8ms*h=pP84I
zuYsv7wIVsSpai>P6%@cB3-)YuW~_p(LRwCKVhJJyqBCPLO$7M_G;)~<isvHGD3wM|
zY92z9rY3gV;NhxZ3$|9TxF9F9L_=K%N1_FJ4iwNBF<4SmiD?7KbOl=laJXtH1>~eA
z7N;ttf<^;Cqj3r)kRi9!ycFHkw6xUZlKdjj@Kh#f$^+yNP-?HxtI(;`tJJB|s{)Nf
z=O&g|DWC;>c?M{xF{G$871agGph=cQ(12!6DtNG6p){`~6EuI92_CU5NGt-)(kAB@
zL8b;!{G^O6Ibw=wBa&#efu4bm0+`S>&@)7fBxT6>StfWWG&Qj}KMyn*0G`e*$p<+U
bJa(9`qmY|enFJn}fX>}!mMDM*ZHpKH&P)Pt
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
deleted file mode 100644
index 44e6abddd15938267cc7d739954e6a98cbf46ee0..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 20480
zcmYc?2=nw+u+TGNU|?VnU|@(-k4#+~RKpN9laV1kzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7TVUbAA
z$;{RV5%C52nRzAgCHWABUO^?q!clTG1V%#u5(1?qX}T7?48}%=1|R`tB}D~cp->QW
z6px0$Xb6mkz-S1JhQMeDjE2By2#kinXb6mu5GW~NW~gUiU|@p!mkUZWqR}i+z7>>4
zSH}z$4}{W=P#R_qlsig|hQMeDjE2By2#kinXb6mkz-S1JhQMeDjE2By2#kinXb22I
z2qdO3FzE9!FckAa=Ko>+`WO5R3|ILX7*6stFzn=KV3^I%z!1#Oz!1dGz`(-Kz;G0*
zhnb&&;Wi%w!)!hV1}{DahOfK~49j>K7(#d%7)*E>7{2o`FwEv*V3^Lsz|hXaz+k|`
zz;K6~fnf$W1A_-Q1H&IK28I<}3=F|s3=Aw>3=Gpb85le{85meO85quUFfcT7Ffe#>
zFfhDlXJFXQ&cG1F&cN`2je%hk8v{c%8v}zq8w0~1RtAQ{tPBiYtPBixtPBiltPBiq
zSr`~LvM?|NurM%4voJ7RXJ%lS$IQSmmzjZ~j+ucWl$n8nnVErM1rr0q3?>GK=}Zg^
z{!9!E>`V*{`xzM+Iv5!kRG@<w3JMAeg((WQ3SN343JwZ+1$sG|d5JmcdYO4;8eR$x
zU~z4bcxq*;hL>J(Mq)u~v_Y(n0)nlnpruffnVpxPmtPj2l9*edso<cHT2vHYmYQ6W
zU&N4-nx+5}PE83YPAyHz&&(@JEh<jcK+&V4;H874Nz;l!nL2<$8RSQt7`hT8C$*>;
zry7uiGS%F}05Yy9wWPEtPa&}=y%@{~iNSD5Myf(_W^QRtVo7Fxo<e16iH<^Xeo;}S
zf|b64o<d}PsX}sMo<c@qS*k*QUQVTgp+ZSUYH_MUT25kmu|iR1dPa#tUVgc{Cfw+v
z#LVJU1;6|fPtfv{+|<01)D+jEqWmHab(hQ(g}nR{g|y7{(xOy_{L&JIjQnzil6-}N
z(h`ut3h70e1qG=^3YmEdF<=uj)4&ebgGnUjr9cD_uC@Xz1bI)vR-quVs5rGq4@|{_
z#56P+!16F3r=+GOmgbb$svGJVsOu<Xq~;XZs)HPrm|Rktn3Gc(Us{}+qL5gU3-uG&
zs3N_@loXJ8r63P!sO#z?B2%}xAT>2bw>UX5Co?ZyT}Pp$vLMwqEhj&*L<i~%kmbq>
zxrvoYsS1_(r3&SVc_kokgY=|=Jq|Tap*S-=FEIzZi?JkZbsdG|jQq^x)MDFcb?ox$
zS&7N{NtuaxAw`LK#Rd7rsp_$a*aEo>90p3osX3|1B?{#knaLRn$vK(H*$Rm{>G?&O
zB^kMJM=Qa6gez2XGP99_1{N5_B}Fie$_gR=F8&IMDJfuo7o`@Lp!gQz7?4*o^U@VS
z0h9<%4&WS|kzb}zl$u_elUS5l1x~U`Its8DHB8kthIkBz<4Q8KGe8B0F35D4(=g%*
z6e2L}<f`E8806{}>Z1_u8RDT3;^7&rhh(LJo+-pem<AYKlCMylpPO2e0rEsrYDsx&
zYMz3D0x0zw>L?iKnJOez!W@p2MnKvj0g#+npiq>blwYD=tN>153gww48Q>s;aEo&?
zQ&NkddU1FdtWp=E64k$u%muOz34<cWN+CHTH91?MJR=d5BQuK?a#IuYic$Ow4P_*q
zASTwZ0z0_0s5rSOv!FyFB|llAII*%=p(G=-SRu1mK|LomF+~BiU`-)0IXN{awFn%k
z>UyZ|g9HgIII=*i)QV9w=)n?7L4I*&NoHB9LUMjyaY<rciH<^UVugaCp1BS<@S$l%
z!9dSM4}aL9xCd9B2KgJ}xZK2w%-qskg~Xhk{PM)4oK%I(5~yz!@=9})Qi~KcGD{SS
zO7n^p5=#_J4GcW0G?7Bp&;SG=i3vw+<|bC?!VN8k#b+kSkJy3>RJ5moDp&<bpycO)
z>b(3Sa9&O<N>#{6EGVc1Co5!kr0N>sbwzr9VvcSqs62<c0Ba6{gi3ivYDq>a*h=-H
zRE5%FP<8-03|gEcS?rdWQ=E$5W=P$juA`8c463bc6N}QpL8a%EpP!SOnCD*r5>Cu<
z1WUsT98esAFouiZrEpnhacN>sW>qRUFci{L3sOr;@^uuFN=p>{{6k!=z?BwwH6^I@
zPfY>UlO#DEZUga-$5JRFg(lqXS*67#3YmE&sp&<DB~T9(Wai~3Cud~lljKIYF5UdR
zoJy)W6qM46O7rqyF&`QPYjETjk>ox^g-@p2((;RP6H7``i{g`W5{ruw$;YuM9bB!u
zK$-@{9;rD6ZZO4=Bne7DAe^73P?lJfnO|D0ke!*AQmm)oo0y%dPz-9$fGYOvy!>)l
z*_o;U$}6B^E-5uFzX($E7nSBIs_TFw7o2vB6l_8588BN9=8ynzBSs@76;f_y=I7a}
z2bJb2B<JLV+9o;q`2`BlmMzE`skw<InaRZpG2rHJdTI%%p;D}&X~m$Vq{NU}tWZ!|
zoB?Xn<m4zM7Uh@bA*x=5{4{V8ot&SNs-TgSnw(f#3@W<Rixo;TQz|t<twDv9{5<s%
zh2)IHymWA5C9_B&FEKZ@SdRgcJ3tv4)YMYQPs#!{A@Y+-5;OBsQxwvQ@^eAHD@HWT
z5Uyj$FV6!t*AjD6OF%8+#A0}bn4bqS9MmWSH#|%7LFJ}GqC#;gsBM^&pO>zXoS&PU
zn3qzlqX5nU$@vA9x&?{FB_Ic87AvHc=79?VaG?)sTvq0n7BQ4(q~?K)0w+e0h(bY8
zetJ=2uAYKt8n|49HO!!%O;kuJN-QqPOjbzD%`eR>fd*GDxH4gI%*g=<UU6bxD$MQ0
z3ZMj^ky?}rRs%K^)PjZ?m{$p_4j>_)tB_c%kO*#_gVP{GNk)EYI;5fpM@mV)LRuyy
z#&Q)jKt)nfYC%q7a;h@C7ERC3Pl1L5D9nme3lc$v0VI~o6Dt)mOB9MT@=J446p~VP
z6ml}NQx#Hj3qUC|Ex$-1wIUJJQrBZp2A6m#`3kw9G@V#noLU4bP(jsoVu?a>Y7r<}
zLGcN8X)&Z%P?E2ZT9H{?0@4m@Kq%zqm!*PAXmDd1UgT#MD`?~<X67lRW#*)UV<i!q
z>xw}Q;lw<cGr$To^%xkyi3ejDyIyW$USc{Zy=3MV<QJ7FI761V`{d^r_(G&~6oNnv
zCnyhM4mdsJ=jRkFK(!Sl7L{Zs<}g6Hu&M;c2c;(%JFhgipb`{Kc?B>bcv%S&F38Md
z0QLVjGcqs;gC_6=7#Lvl_n-I~7(VhdFr4ORVA#mdz_6H~fngCp1H(Li28JAd1_on(
z28J7a3=C8F7#Ljn7#LLe7#I}z7#NuO7#Pm-GBEV=GB8B)GB8;1GBEJ-GBDiYVPM$I
z!@$tY!@!Wp!@!`!!@$7G!@zKtn}OjlHv>Z-Hv>aDHv@waHv<DVHv_{ZE(V6>Tnr4$
zxEL5_a4|5%a4|4Mb1^VPaWOD>aWOEcb1^Wy<YZu2$H~AjgOh=wfs=v3o0EaTjFW+Z
zfs=vZ3I_wjWDW*~3=RecBMt_J@9Yc=57-$Prm-_Hc(OAvsIoIK{AFWcxXQ-BaDa`0
zp@fZrL7a_&fr*WQ;Vdfy!z5M)hEP@p1|?PohPx~b3<p^l7?!XwFf3+aU|7V$z|h9R
zz)--#z!1p7z@W>*!0?}$fng6b149ur1A`VbBs_&cVahNHhE@oGYD)!BO%)H#v3f<N
zc^Z(~6j@lo7E2b^0JrsZK&^Pt-W5>h)&Uhf#X1VrwIB>JM_Hjf71XW-m3ol2Wnx~X
z0;n2LNJ%V7RLCz<$jMJn&&*2)b@cO+L6t+QLTYY7NhQcMNHGSki=o|bkVTNX3RE~~
zAc;EXWM=C?dSp7F-f=c;UrkPG5yUaz9W+oKIjNv}Jw3Gqqzzg^<)jvAfNDvIYF}{M
zz!qYnUP@|OW?rg$eqv4lsB;4iCU77q*rF6?NJfARQ?OM)s!=o36yoDSRdam2f~~EB
zQhYq9$c>LzvQl7xjDkSNf|80-6SLiN64OCp3Buq&P*%uDMGd5s%;XZtpb>IV7blj1
zT66HwRe)4?pt>n9H8llPo)@JS7l4`qpr(OifTxZE*aA?RC@9LzE74Gb_C;(Jl#qww
z^h#i36dIb4a6;0Om<R6oBkL^4%+t%sPdC#Db_I7yH1(j0Ff`|6=A|YU;nxdMgQ2w`
zAJqK?M}{6KS?T5bW=2D}UjCkbA@QCr&;gaqG)PZDK~(|NpMwlcDT8Y}$fz0E^N=P2
zWP}A2N||Yy$qE{YInbIvQ#}`KilKplDpDT}+|o!aDgrgaK)n!%GeOa!kdhCU1RD;D
z7*KR5z}j-q6bVY%&>9iZ7Enk}%}Xsx%+UkK4#Z2vsd*`ZDH??-5DUNqwrELP19kWm
ztXUb-4lBqhO$Yn8ASba>Atf^{Ewv~W)Naa4g{7n9{M>?4a62tk0o;{WC`--BPtGjK
z1U33V@$Ci5f|+@GP?cVwEa3$m#DbIzdb#;2sX5U62#ypTg+dq?7AX)9C@X*x2{xlZ
z?VsGz<P0QdmSiO6fy2^STU!A%ats<2%}Irg$HB9Wf~`VGQE4jJ=L!nofY(rR&Ib)P
zr-QRlVsb`iYFTQEjzUptVlt@loswD%YD|I0Hx!j1ZUdzk_$YW@eo?LlVklcv!3J75
zfJ%yZaPo#ED1;hlIHBeM$VfOO!$Gr!0=%3_EJ?(ceDxBOOFZ++pn(f6n;<S!R!GY%
zDh4(FA%jAY?mVQ_1rHEGIyvBg0EKf}eo=X1QHpnF9<);eH4<BVL2M|5J1#x7BoJEi
zDJwubQmN2xQzEE6ovD|q2TJTvOOU!gVEc;`%TnV(g?~J#*ix`nsICQzBgYzelmXmL
zLdqAQLK@;ujG|pfAyWrjp~Qo7K4_)@GJFYcA%jP;5*4z`6N}P8bq#3DEVV4P2vh(j
zr7Glt`g}Qw6_8N{kYWa9uvw7GsU#oNi7Ci|48ed?onwF}gR(+yehPSiv!qg?G9R_v
zhPGNYKm(X)Qyh@`Q4c&!3ZEPS`(GK>H-S_E;5iCtJ&>7K=AD_B8yFCrnV0TZmY<p8
zo0ykb;G0+h(V3YBtAcY<i@-GtxL(1nO$m}j@!R1VpY2Pc`5-OCn(axt*`D~!CeAqA
zCd5OgqCE4;;xiI+(ugq*O$#w*lV&V#6Oqz_YkW3EVF~UI!iUYk)dFbPKNC{KqKy1Q
z(<8=I1=v@xIUuxZ2%Ho#bU=Lusz;!P5|wGd69bSLMS^x>$$d&z;6epQiw`mo0}2i+
zh4P}z63En7K6q#pKF3lFA5F<DR!D~S)}f;=;5HRvLLoH=G)R*M>em+(<(H)vD`e&+
z78Dfa7ZhcJMgemYOHzv%GQmSTm5^?FYDGy=qC#;=X<8a|s31470Av6(Hj+})Ga-X?
z;CTjUm@$;6LPi=Bb8=u~AE}_?4KzAlPy#NriW75F5v^$*1#l@1YWKmS1|0Dq0dO6r
z0I8q!7(78!8;J@@nI#IP={c1;U{*<f31|)uK7Ny!1J>>Dtx%kwQwpj;^%xu#ic5+>
z?Ht$$0;nPfPuhWIT~fibSowLWQ1=$6>Vb#Bz_AKV5rrvW9=L4-nT$r%so;2oIUPP_
zhSub<f)u3)<3RoY8w?B#cR+L90t^hW_5J_(85n-^GcfGtXJD8No%?6vXJA;%$G{NB
z$H2hD$H4H0mw{nDF9Sm`F9U-=F9U-;F9X919tMUfJPZs0JPZsBJPZt5xEUA{pndv1
zTnr5UTnr41Tnr3*IT;w*IT;u%IT;ujIT;w1b1*REb1*P)aWF8fU}s>+VrO75W@ljd
z&c?v71v=O7#Kyqzjg^7n9xDUGQdS0rOjZU44ps(+%Pb5G{VWU&VJr*`p)3pxW-JU0
zyete1yO|jn5||kn0+<;X{FxaT*qIp^_A@aslru3fI50u|JhVEeB)Jw;lprOQ(RB-;
z0KtYw*DZ{$TR=(|(79Gy9P1KB*DXM%n`}qdEugQh8C|z9x^5vcXLQ{HxG)`Ew}5|H
z2z)MXblt+}x`oko3!wP{g~7aTK|x(t9Wo@U9<L5+Fz1%$lw|6b=NExj_ChA*H1!zJ
zR%Ad2gdu~@#i{URIIuy0Vuh4^=*UKTQEGt#LkMV%A!zjjXy^en*aRIAPyjEh$ShV!
z2CW&>V{k1?Evi(=%uP(s%mXjn1F@2!t2A`L3!TAZ5XGq_C7@9U@Om21nk(?M7j$p|
E07*?REdT%j
diff --git a/python/ur_simple_control/clik/.clik_trajectory_following.py.swp b/python/ur_simple_control/clik/.clik_trajectory_following.py.swp
deleted file mode 100644
index 2712234ef37d6e72b70c659ecaa1c192a007dda8..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 20480
zcmYc?2=nw+u+TGNU|?VnU|?9H9htgZy@nxdCL=?7esMv5N@5X63Lnl*EY8l%!>0lw
zR|hjtKO;Xk)kq)C%_+@G(Jv@UtjH|ZFD;5M&de>yNsUj=&nqd)&(SZaEXl~v!y=KK
zlbNj#BH~Mm60=g1OY(~<<J0nUa`MYF^V0PSDk0X6lA|Fo8Um#yX}T7?48}%=2B5%D
zR#H?D777J1NAYL~jE2By2#kinXb6mkz-S1JhQMeDjE2An34xLVW`=qO1_mamf5o6Q
zBO1*D<@-WuAt((~#|-6z#}OD97*e5pm^_p_N{xoVXb6mkz-S1JhQMeDjE2By2#kin
zXb6mkz-S1JhQMeD3_u7ZrZ6x(=4N0B=Yh=s!}|Zv`573F@-r~Z;%8u($<M%0#LvJG
z$<M&R&d<QW#?Qd;nU8_tHXj4SMLq_GV|)w@NBI~SYWWx#O!*iX-tsaqEa7Efi05Ts
z;OAvv*vZ4d(9Q#~;|wnY!(NaWF9X9I9tMU^9tMUo9tMUW9tH+o9tMVY+zbpyxEUA@
zb2Bg;;$~oI<Yr)q;bvfv;bvfX!NtI^hKqq=H5UWJ94-ciR4xVvH7*8*Pn--4`#2dG
zIyo5_%sCkt%s3esG&va<esVA{Z02BKXyjmE$mL*Qu;5@|;O1apc+1Yf@R*%}VGTP2
zLk&9vgC{!!gAh9d!y`5ZhV^U=3{`9l3;}Em461Al3?En-7>=<rFwACUV3@_qz>vnu
zz+lVDz#z=Zz#zoRz#z!Vz#zcNz`)MR!0>>Dfnhfb1H&#B28J~(3=H#G7#NCJ7#Q4G
z7#Jj37#Jj27#LnNGca6WW?)#r%)l^(nSsHFnSp_YnSo&s69dC;CI*HyCI$vwCI$v(
zCI*J9j0_A{7#SGam>3vJnHU&Cm>3xTGBPl{V`N}>g)*>`n_8R^pORS=pO{pfpTmI4
zEiFk^uvN$})+<OX$<RwoDhAOSFg2QR)e5!>N_u+w5LA?(lwT5`l3JWxlvz-cnV(mz
zuf$MVl#&(?(hac(yTJ%WMX5Q7C7ETZnn)U9VuUOMnWA4>WSSaZo>`I+4|YywVSGkn
zUP`JSNK%P`L76&$L0JJS2o7-#P%yGm0Ie`lD9K1HQ2==zt5T4lGS%EuP?VWhq5-l@
zlL2Hhh!vd~t6-~8Qk0lioED#ypHdlLk{_RxpIDTlmzWGu9Gw}f$&i*`q>!nQnWs>c
zn3tZak&~JSF-X&jK^bI@f-S@s+6s9EdWl6viIp1B26_fM3I=*cItsc5dWI&(7O|QP
z1XUYAR2!LqR1+{6qSn9?Y%(EdKouK76f2aa7F8-F=j0csGAJwLr==CAmVmrh#lV0q
zFhJhR%+m{YHP#5yQ7F)4C{VCP@u{JKnJLJ#<^~qVAll5>%qUiq0TSdvnxGOmTERdM
z6zm`j_N1wqg_*gfg^7ZWu7RF~k&&UPrG;s1tR@47N|>Yq$Y_|hSRIBS6jwwWBAE}j
zOGiP$K+nL))Xd1h%rI7mfk7E;ybj12AgpVkXKH3<VP;}(2z7|DiGhiM3CJN}rEry?
zXtXeb$U<EL(HE=3pp4-Zgf;^`1vm%n6blOr0}GHQEG{uMwKO$12ZxV^k%@tcxsf?8
zmq28}{xda$=mQyw*ClX=fC9zX$k@!(*aV~r6cP#w3Sb98*<d%ASy&htniyH?D1dx#
zZfIa=U}<Rvb`ew=h8CD2P+Y;ZgRDSvmjZHZp(G7ZX0bFgFfddAX~SiWnWd4bxrw1U
z)MW-{Cgv9Akg&p~#T=>#>@ui!kQF4k%+%b#z!Ds0@Wc&{{DRCpy`q9j5Dv-rO)M$O
ztk5VbsMKUoE-I)*NvFC-dS-^kkT^ClGO+*$w1I)KajYh&%rwx$mYH-EaB5>vRtOCU
za1B!Madit(2=Mpx3sGQDR?x`IQz*#HQ%KIwFG|VGODst()=?-*P0Ufq&r2;<NGwWK
zh}NypQP8c_QK$l29pdleuTWl;S(2(yoS&PjkeHL6UzAyrk*iRWuaJ_jP?C{ZtdLk*
zlAoJcl9`;ClT)b!5d#SqXXa(*feI6_DY>bM#id253XXm*3XY*6{=SYOp3aUwK9LG;
zo_;R+&W=8wPC<?#t_;c$Cl!~Jrlm13C=)1rit<b1a}!HYOKKfR`3<TR^Yc)pzyS(P
z4mc@DL2U>rtqqXVHcl-H3J^`;BHmEX0F;Wbx(y}bqhWS{5+GQQvO-2`QK~{tYKeNW
zLSk`oX>O`QCWxI?T3n(~lwXoqlA5BBSd?Fym!eRipaGW4%P&#LPfON>h88w|qgV^B
zf}jK>jTI+?R)Q+P0v%NFmt>?WB;}Ws<mW2nq^6Z9B<C09r4}jVrzt>W5{pt4H1d<m
zGV@D|6^iq7N<obmQ1BL)CTBpZagf5a%%b8Fg~HOrl%m8ukTb#BD|9Or3UU(jQWY|b
zK}oYbRUt3GTp=$N<T*&}rR5iaQdNGQLRwLNE?5_+I#x(4O3Y0Kr9Y7A;EEYsUx5`M
zjD<ThrBWBHgFzW=14t9N5(TBuf`X#_f}+gClGL0^h2rGI9FXmKDKOuoI-P-m0i-e7
zO2-h?LeMo-&;r-+5CLrkuv6kQQZv&tN*I)3X5^)oBjq=R;>zNZ)Lez~RP~}%h4lQ)
zymXjC-AaX&%%ap}P$;5Ugs@Z#)ef*kN@|)yZeoFvOF&{tMo7M~O8_WT!A+5PBNrWo
z^30Ty3>^iCvp{VVXbR5HO9mw@h4Rdt9EHT>l2TCSRY*z&M?q?eLTXNNYB?w=GAJwf
z`G>e#LGpDj$ae~f#R}!AIXNKZnR(!VFD=Q;QAo?oN!3wEDlJhc&qypuElVv@0JW0x
z^2>D;GD{#<C8a8)<ReN2kYZ4aPrX<Hl!y}*ia-|Vf?J;{(5TMIP0ZI(NX|&iOHYNQ
zE3m^rMiyt}m*%7>B!Nnq#Jm)cQ}Q6$B`>k0v?wtLluj}d%Ru^46{-|K3!fDV@-y>F
zKp~%B3Qv#V$YW4e041Q}lEfl#)2AfAKo>dbYk&+*%mI6<JhiwawJ1*kl-KeUAdPo$
zNtv0Ku1U29D%J*bZejt*qY4F?6{$JJARi_w7`s5T6Eu2JDC#ws88UMV@{3A9$r#4W
zFNU!SGV}72lQS~&LFuC)GY=-7SDIT;2@=aIU|>*I0JrvytrQAMOB9Myi%WArVO^51
zkX&4*paDt`i8;mj3dNvE%LGX&Bq}85=ar@AWv1qpC?w}378mP-+tc8Z5>()TO~mLw
zz^pQ~0%Z`KAqH|1xEcec-eRbWib1(DQK2L=H#J2e3zS~KSshe{KpXR*a<Difu^?4J
z!!I6O!v^ap7=c_<nO|C@keXLgR0&CGpb{=ofdSP27vx}I_{st4|HJ0*5AriGZ0BcS
zSjEr4FqfZ!VKP4hLoGi8gB3pm!zVrlhShuw3^Vx{7~1$47>f897_#^n7$W!>807gF
z7(VkdFkIthVA#dWz|h0Xz!1sHz@Wv;!0?xcf#E6-1H&2~28L<S@qq8#3=B8985q`b
zGcc5LGcZJe#v-^G7}U5K7`V6@7~XI(Fg)U7U^oQr^UvmDU<l@7VBqFrU^vXlz_5{%
zfuWC+fgzHUfx()Sf#C-S1H*R?28M4?KYaoD3)HF~#iJoG8UmvsFd71*Aut*OBR&N3
z3iLp2xsr+!4bY%>UTQgb6QN#mahZ}1q=gk<SgfOvlA4p5n^}@tWUH>DuA`8aTVkuO
zs%M&}uE_vyhQ${aL;A28#U+W!*%}%pItqownwktH5aFE6yyAky<Wvm<9R))J9R<*U
zEJ$9jC>7jJ(*O_A78Yyj7;1u=X=pucg_6YV)M5og1BK$$<ovvpVg{If;I^hlVKI$c
z^$H5`mN6*2kXoH6ZFvNTimiHtp$ZC+F)Sq)PZv<XPr=#WFU&Q_-Bk%PLJm@ynWm7b
zU~8)o6zb>a>E|Br8R8nGYiI=?K?ijkAuN!}ym(NADxfL_tIJGNfJxdac=~yUfK7qu
zR8|0wbrff378HQGX`n7Uq$^%jnx_Dor2vgrDHNB0`jKGMKz4y}VX<CfK|yL>ibi3y
zm02u`lQls7ODuM3Du6nU8kq{JpvbqiRWQ(mI0I}TbeKCcuOv0SD6u3}BR4-KHAhFG
zP)DImK}#W}1Yw7AnS!l?mtKg1gF<RiQG6MAn=IUU2+6zxy`0Rv#GG`!%)BxUF9ip%
zCh#;tYGta1mmWB|MjOQHC?MFH3R((=23lIWMw-~o_ky}KIX}0cv?SFF)MxQZOwLcr
zOw5BhIwi3r5$tp?(1>8XrwhXIAkTtuQc-GRHr&x55fFx^32-;8C_NQ4K%%6hkcp}t
z6s7qkxT6(h5J(N&`+50Axf+O|(p0cfNG&Kv1U^Cv>bIQybTf@$*VLk-{31;~NB}Fq
z9SfCEuvGy0Q!n2)GaBa4SkTmsXI`0xZ+d=W4#L@>5!$r;qVmL|6z|NuRM2o!axq$H
zK~#grF_30^An6F2P{4*lod}%|L0F@VE}K`NSCv|nU#y{FtYfUHsiT0T7Su^CN(GOF
zDC8G``n!oGnfZC3^pszupi!A$3L10;50Yh;D1gRAAoEZlBS1Y?(1?%~q~MB<WkAlK
z3bqP{1_lNU5Qi%u<v9j;?t`i^U?_x?sE~p$+ECBb!bC?w*HF*G)Dk>0ZE0!@<{KMW
z>L?iMnVQ1}tj)l~m|y}le2i+2u92RBA;cgPGq6U;NVlP$C1~W^P|w)R3~aoykvZ51
zFoD$^BU3XST|+$+0}CAkJxfzl5Xaoe5JZ_6n1Uvcj19rM!34+%1yE_CkeQcRl9;1V
zkYB7&l9A7l0;)O;^b8CcQVT$p3nYUvz*3EZt%4bYGGtaVvxEVf^FfJA!B!zXwIm)a
zq5+yCMV+hFQGm_)!l!RF!To<PW(I~?pauQ{3=FXK{SWvV820fqFf8F`U})lJV2I~u
zV32|K{g3l8FwEj(VCdpwVCdvyVCdjuU~uPSU{K;?V0gsKz_5;&fuV?(fuWF>fgy>P
zfkB&>f#D4g1H%p;28I?M1_pl~28O@f3=Ds`85rJkGcfGuW?-n~W?*pVW?(SpW?=Zo
z#lUcpi-BP?7X!meE(V4oE(QiCE(QjEE(V4hoD2*rI2jm9IT;unI2jnkI2jmTb1*P$
z<6vOu;b34$<X~V(;9y|T;b353=3rpB$<DxVgPnolIy(cyHFgGuv+N8E%h(wh8rc~b
z8rT^air5(#QrQ_8EZ7+sc-R>j?z1s4++$;4*vQ7fFpZ6YA&rfJA&QNGL5Pik;S?(a
zLlY|lLnA8#gAOYL!)F!-hLbD|46|7v;TOlkz#z-Q!0?xufx&=<fkB^zf#Er9W5kGW
zag7GRK!!kWYB57@DtxU6qy#|YVyx#NBo8Xs@h&7GWB`&pG7}O|7^T(dDi%BoUO+Rl
zptcM;#y$#-t_selUh^H&Is+vUNLvNO8ePQ#ni3ja#WK2z1)Sk9mZZRzp+MJ)jILra
z8C}IPx{3v~-V3~d1x$>tVu7qSK#V^Pk5w$760s<?q_ijxGN}#XgWC+aR<<M-r5Ec#
zR*-;4Ta*=Y6SKjqN77UCQbDUMK*J-63TdS!pcP(4sX3{M#i>xG;Kg6y33kwm8-<kA
z+<b-Ng4E>9w9I6MjKt(@9mo(4g)3&zJ%R9$7Ig6o*ga5T1JDW{1X~NTOa`n%8L|)v
vG!z349?<%VT(AsCeY~*?WN-vF3Jns}snUeBys98_(D5D62sB6<GU5yXV8qEY
diff --git a/python/ur_simple_control/clik/clik_trajectory_following.py b/python/ur_simple_control/clik/clik_trajectory_following.py
index dd2e244..f9469c1 100644
--- a/python/ur_simple_control/clik/clik_trajectory_following.py
+++ b/python/ur_simple_control/clik/clik_trajectory_following.py
@@ -14,11 +14,20 @@ import sys
#######################################################################
# map the pixels to a 3D plane #
#######################################################################
-# x-y start in top-left corner (natual for western latin script writing)
-# and then it's natural to have the z axis pointing out of the board
-# TODO but that's not a right-handed frame lmao, change that where it should be
-# NOTE: this might as well be in the util file, but whatever for now, it will be done
-# once it will actually be needed elsewhere
+"""
+map2DPathTo3DPlane
+--------------------
+assumptions:
+- origin in top-left corner (natual for western latin script writing)
+- x goes right (from TCP)
+- z will go away from the board
+- y just completes the right-hand frame
+TODO: RIGHT NOW we don't have a right-handed frame lmao, change that where it should be
+NOTE: this function as well be in the util or drawing file, but whatever for now, it will be done
+ once it will actually be needed elsewhere
+Returns a 3D path appropriately scaled, and placed into the first quadrant
+of the x-y plane of the body-frame (TODO: what is the body frame if we're general?)
+"""
def map2DPathTo3DPlane(path_2D, width, height):
z = np.zeros((len(path),1))
path_3D = np.hstack((path,z))
@@ -32,118 +41,62 @@ def map2DPathTo3DPlane(path_2D, width, height):
return path
-# now the path is appropriately scaled and in the first quadrant
-# of the x-y plane of the body-frame
# we now need a transformation from the body frame to the board
# such that the first quadrant of the x-y plane is
# in the bottom left corner of the board (obvious solution)
-
-# sample stuff
-#translation_body_to_board = np.array([0.2, 0.2, 0.3])
-## here let's assume it's just rotated around x (it's not ofc)
-#rot_mat_body_to_board = np.array([[1.0, 0.0, 0.0],
-# [0.0, 0.0, 1.0],
-# [0.0, 1.0, 0.0]])
-#transf_body_to_board = pin.SE3(rot_mat_body_to_board, translation_body_to_board)
-
-# real stuff
-# TODO measure AND AUTOMATICALLY FIND/CALIBRATE
-# TODO write some algorithm to do this automatically, this is asinine
-# (in pin coordinates, real ones are [-x, -y, z])
-# UPPER LEFT POINT
-# 0.1065, 0.7083, 0.6362, -2.6137, -0.0248, -0.0033
-#rpy = np.array([-2.6137, -0.0248, -0.0033])
-#R = pin.rpy.rpyToMatrix(rpy)
-#print(R)
-#R = np.array([[ 1. , 0. , 0.14570094],
-# [ 0. , -0.70647877, 0.69257417],
-# [ 0. , -0.69257417, -0.70647877]])
-#R = np.array([[ 1. , 0. , 0.1496001 ],
-# [ 0. , -0.71010996, 0.68801429],
-# [ 0. , -0.68801429, -0.71010996]])
-#R = np.array([[1., 0., 0.03236534],
-#[ 0., -0.82404727, 0.56559577],
-#[ 0. , -0.56559577, -0.82404727]])
-#R = np.array([[1., 0. , 0.02588808],
-#[ 0., -0.82340404 , 0.56686471],
-#[ 0. , -0.56686471 ,-0.82340404]])
-
-R = np.array([[1. , 0. , 0.02562061],
-[ 0. , -0.82215985 , 0.56867984],
-[ 0. , -0.56867984 ,-0.82215985]])
-
-
-
-print(R)
-p = np.array([0.1065, 0.7083, 0.6362])
-transf_body_to_board = pin.SE3(R, p)
-
-# offset in z
-# very close
-#path = path + np.array([0.0, 0.0, -0.0238])
-path = path + np.array([0.0, 0.0, -0.0938])
-#path = path + np.array([0.0, 0.0, -0.1248])
-
-#path = path + np.array([0.0, 0.2, -0.1438])
-for i in range(len(path)):
- path[i] = transf_body_to_board.act(path[i])
-print(path)
-#######################################################################
-# STEP 2: clik that path #
-#######################################################################
-
-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)
-# clik it
-JOINT_ID = 6
-eps = 10**-2
-dt = 0.01
-# skip inital pos tho
-#q = np.array([-2.256,-1.408,0.955,-1.721,-1.405,-0.31, 0.0, 0.0])
-#q = np.array([-2.014, -1.469, 1.248, -1.97, -1.366, -0.327, 0.0, 0.0])
-q = np.array([1.584, -1.859, -0.953, -1.309, 1.578, -0.006, 0.0, 0.0])
-INIT_ITER = 10000
-n_iter = INIT_ITER
-RUNNING_ITER = 1000
-qs = []
-for goal in path:
- # there's no orientation error (you do need it lmao)
- #Mgoal = pin.SE3(np.zeros((3,3)), goal)
- Mgoal = pin.SE3(R, goal)
- for i in range(n_iter):
- pin.forwardKinematics(model, data, q)
- SEerror = data.oMi[JOINT_ID].actInv(Mgoal)
- err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < eps:
- if not n_iter == INIT_ITER:
- print("converged in", i)
- break
- J = pin.computeJointJacobian(model, data, q, JOINT_ID)
- v = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * 10**-2) @ err_vector
- #v = J.T @ err_vector
- q = pin.integrate(model, q, v * dt)
- if (not n_iter == INIT_ITER) and (i % 10 == 0):
- qs.append(q[:6])
-
- # just skipping the first run with one stone
- if n_iter == INIT_ITER:
- n_iter = RUNNING_ITER
- else:
- if i == RUNNING_ITER-1:
- print("DID NOT CONVERGE")
-
-#######################################################################
-# STEP 3: save joint path #
-#######################################################################
-qs = np.array(qs)
-# let's assume it takes 10 seconds
-t = np.linspace(0, 10, len(qs)).reshape((len(qs),1))
-timed_qs = np.hstack((t, qs))
-np.savetxt("../new_traj.csv", timed_qs, delimiter=',', fmt='%.5f')
+def clikCartesianPathIntoJointPath(path, robot, transf_body_to_plane):
+ transf_body_to_plane = pin.SE3(R, p)
+
+ for i in range(len(path)):
+ path[i] = transf_body_to_plane.act(path[i])
+ print(path)
+
+ # TODO: finish this
+ # - pass clik alg as argument
+ # - remove magic numbers
+ # - give output in the right format
+ # skip inital pos tho
+ #q = np.array([-2.256,-1.408,0.955,-1.721,-1.405,-0.31, 0.0, 0.0])
+ #q = np.array([-2.014, -1.469, 1.248, -1.97, -1.366, -0.327, 0.0, 0.0])
+ q = np.array([1.584, -1.859, -0.953, -1.309, 1.578, -0.006, 0.0, 0.0])
+ INIT_ITER = 10000
+ n_iter = INIT_ITER
+ RUNNING_ITER = 1000
+ qs = []
+ for goal in path:
+ # there's no orientation error (you do need it lmao)
+ #Mgoal = pin.SE3(np.zeros((3,3)), goal)
+ Mgoal = pin.SE3(R, goal)
+ for i in range(n_iter):
+ pin.forwardKinematics(model, data, q)
+ SEerror = data.oMi[JOINT_ID].actInv(Mgoal)
+ err_vector = pin.log6(SEerror).vector
+ if np.linalg.norm(err_vector) < eps:
+ if not n_iter == INIT_ITER:
+ print("converged in", i)
+ break
+ J = pin.computeJointJacobian(model, data, q, JOINT_ID)
+ v = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * 10**-2) @ err_vector
+ #v = J.T @ err_vector
+ q = pin.integrate(model, q, v * dt)
+ if (not n_iter == INIT_ITER) and (i % 10 == 0):
+ qs.append(q[:6])
+
+ # just skipping the first run with one stone
+ if n_iter == INIT_ITER:
+ n_iter = RUNNING_ITER
+ else:
+ if i == RUNNING_ITER-1:
+ print("DID NOT CONVERGE")
+
+ #######################################################################
+ # STEP 3: save joint path #
+ #######################################################################
+ qs = np.array(qs)
+ # let's assume it takes 10 seconds
+ t = np.linspace(0, 10, len(qs)).reshape((len(qs),1))
+ timed_qs = np.hstack((t, qs))
+ np.savetxt("../new_traj.csv", timed_qs, delimiter=',', fmt='%.5f')
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index e55168d..c7f17cf 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -193,6 +193,9 @@ class RobotManager:
# just pass all of the arguments here and store them as is
# so as to minimize the amount of lines.
# might be changed later if that seems more appropriate
+ # TODO: put the end to all modes not compatible with control
+ # like freedrive or forcemode or whatever.
+ # you shouldn't care about previous states
def __init__(self, args):
self.args = args
self.pinocchio_only = args.pinocchio_only
diff --git a/python/ur_simple_control/util/.calib_board_hacks.py.swp b/python/ur_simple_control/util/.calib_board_hacks.py.swp
deleted file mode 100644
index 654ccdea21df40efc4deb0f2c418c7e9e9736fc5..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 24576
zcmYc?2=nw+u+TGNU|?VnU|_H^iA*gGtzih8$;gnNUtEx%l2`<i!iRGci?cKH@Tq{v
z)xiwZ&&bbBHPVN3b4v44^b3j-D>94qON-)*Gjj`aQsa~J^Gb^HbMy-;OEU8Fut=1a
zWaj86C+1`(#V6$_7Nx{zBqnDU>lIW&tQ#dqLtr!nU?EUilBR3H%V2C|XaEvYR#H?D
z777J1NAYL~jE2By2#kinXb6mkz-S1JhQMeDjE2An34xLV7KVBT1_mamf32Z3BO1*L
z<!eJ}ODGLf#{%UGL1}*|4U=bv@|B=;F_ebML%E~WXb6mkz-S1JhQMeDjE2By2#kin
zXb6mkz-S1JhQMeDjD`RWLLf1Pfgy^Gfgy<<GXD?j|Nr1;U^v0gz|hRkz~IWyz~I8q
zz@Wp=z#z%bz`)1Pz`(@M!0?Taf#DDz1H(Z+28O+S3=D023=Cm>3=CX+3=AiE85kz;
zGB9NGGBCLCGB7ysGB7CcGBEt%VPM$E!@w|whk>Dmhk+rAhk?PHhk-$thk@ZSHv_|I
zZU%;R+zbqBxfvL$xfvL&xfvMvxfvL4b1^V<axpOYb1^VDaxpMSb1^Xd;ACL9&B?$p
zpOb+hkCTBxmy?0vI|l>9H4X-b4IB&%T^tMynH&rZ9vln|N*oLfOdJdhkJuR)4zV*Z
zEMjM1Xkv%>TZ5f}fuEg$fr*`gfe{oA><kQd*ccejvoSE#urV+=vN14dvN15cXJuem
z!pgvq$I8Iq#>&9p%F4jN%F4iShJ}G)6$=ByOcn-)02T%YbruGOkIW1VJD3?5x|kUl
zqL~>ORG1kUt}-z&G%_(T<S;QXm@zRhd}Cx_SkB16V9mt9@PUzm;Upsi!vaPI20Io8
z1|b#(20<1E1_cBNiqFg|$xO_NPc1IV%uOswRj^e^&M&CcOG!;F05LR*@{{sQ^imQ_
z67}+ZGov9~FMm(Jka$m*SiPeBlEjkC{5(ytsmcl^8JWcji3P>^Ii)43Ih6_-iNy*f
z`3gy?3Q!X>Q&SWYvlGF#=qBgq6_*s1CYPk9fb|z-=IN#77nLU#rFduNrRFA<WF{9w
zosgTKlA5EV0C51wJvs`75L*isY@x#GsU?9L5J6>y5PuhcD}~(rvQ&lW#H9SP)cE4m
zypq(sWY98@l+;*-%)F9(h=bEg^O8Zn068@^ueh`*RiPv!u|&OCp(G<!Avv))73@4^
z1wa1~S1X8~%wmP&(t?8g;?xw7wxm>rqS8Et{Jfk>g~YUy)FQCf%)Crc@G68j2Y`bj
zDK#}up*Xb!YFv71i9(@{LQ1{@)Yl5xaBu4<fMk<Pi;7b7O27sLDL}MED;Vf0=)fXT
z*FewI%+%D<)ZAQ0LDxXf!pOwH#N5a{HWnHXAmwn?$SQOc6cBo2bs&ZZDcCCH73d`v
z6(v?`L`NIyA+#ExunmljjLl4qO<)?KVUwSeqL7@QlB!UYnwyxJSFDhcT9m3#npcvU
zqmY(ap^#auP?A~<i*RLy%)H`~)Wj5p{4|A(#Inr1bV%rC<QEm?7lAcH+y#x!w0uaw
z=76F#KTRP$KSiM+CpED+RUt9Cq%<)nr&6H^WF9ynmS?2qDOBc{DkP<*XXb$o%8LhC
zU#wuOU}yjqC@x4%O;NB_h&IqsFaU?3F_;13#)4IVQgy0AL4Ial3CQ{B`3j{4Itodp
zCEzGdD@x2w1&M=_3Mihz0#J>hG^miAk(iebN>Q1`ItrNzS*67#3dI@uWuSzUk*ZLT
znx{~bQIubro&h!xyzU5+OY>68<3TA5BUggb6<7um$8at@Clr;Wq{bJeCZ}eWrRt@p
zmN<d~C&W1*Aip>jo<$TCic%qg2XcH`QEF;RQD#}HLSkMD$SHXW#U+VFB?>9|kaz)Q
zg`CW!B5;mZC@9KLPAx8m`czk;v^Z6vI6pU4Av?7a><5L+yn@mah3H8AyjX?AVueJ7
zq@4WZY>-*Wi8(pY1gD@2wFPcH)I<evw#qNbP{_+KQAjFEP0R+RjlA??u=^5oit`b1
zUR;`-oC+;PlofOpN>X!jKwbd3sz{+cBe4WzT1vh`d1gt5LP1exUP-ZzLSAAnsE{fL
zr_qvp1(0(=A(5JwqMMqQmYQ6WUj%gxC?>5G7*bNx6u`^FiV{mw19B4cQX$2Yjsi4c
zX<9Kr;;tyQq_ikcAqcD=Y@vo$kd8ueYJsh~PE4LUB<3Iz>P7h_3b_zB=B1V^fRm7w
zx+cUVXil#J(MUvmUTS%?Q7kmSCKYQ`K}8@YLwJT5@`liI0U~dJA#VVcuY{V0FdrrY
zF(1N1HXkN}ERSqHL>^|oGQ<H`J)oeaplb*YIH)RQYasey-he7Vwg;jBZV$v-WP4yD
znn?CQ#E|WQiD0TiwhW>QZkYnS{3|X=ti)FO6%^%{fpUDlLV12sHpG*ln1y0c)&rLi
zY57ITNQD}_f(`;D<c!2JP=S&Pub6ao?G!RoQWLE}30|Q%Gd-_ZA+0DsR{>NsB<3gt
zDZmVX+6YO{X_<K`3W=Z+vp7F5F)0U>gL6t@RUx<x1(^lP>q$AOxlp|j_hnXqGZ;v|
zSRped87v774oH$oF3K-1)~Em_BOL{BoInx=SOlU3TFV&ffeKqF0Zo2j4IovJEC*{S
zxPeMrx1#)9zx<-y#GEj27S+IL#^@;IX<9KTE94g`low@|q$;H3m***zXH<e~gOvO{
z^%8|VP`RI3qM(tfo~w|NpO;!(l2ZvTYl=Y)fW(qqNFkpIF7<N~OHwr%Agu~;MFuJ-
z(h`$Fl|W(%*jR9|Bo-H!=7Qr!p+W()`ji1wePkAcYKZ(ah2;FwqT*DA<b2R_RFHj%
zMM;?@MTtd~Its=449W`SsgMc+!yJ$S1&Kw8IXS60kWviff+A4!OuZOk7r04Pnp~og
z2q{Gr5{vRnA;q~u8km&_4mpJ~aL^%27)VK^l$?@ZqNJlxpx~eYZPbCP+yaHnJcWY%
z;!IG|D7FGeHB>`p8aNrG<>%z&mxIdobWjZfs&v6sT23Wcx)@YR=9T6qr55QZWR`%_
zerk#mBt_(b;#ol-)b7a1%uCEk*8{mx18ToAL|Rt?S|Wg30^qnz%vHz)M;NSv0<}HU
z6EpMd!46bb02z~*lUW6AzCm4_r=+8xl?N+|p#?G6k^()b{W--Y#f2Jh59@%O21+3s
zIjMOFVNFdPg`(vAycAo%{Jd1nXah)l0j>??Em%_uE~p7k&}o?^C@B~rZN;FZq{I*e
zu04tsf)tWclM_KzE3`ohuF5<W$`kVx5)~LS^-}e~;S2F8G*Uq&1SoM7fa)hbhLDU>
zNJBP1Ev+~eRGp_5gM%$Mv7`hPALXf#b}KkXfHWrNrRSvTFhI%(P<shn<K`x2rz(K+
z23QTKNe!yHOHy+|EmTkv&r<;Bh~)gDqSWLPJqA!=21*GfAQvFzx{`baNXe6@0CkfF
zNExJ5%U8(G%P$8dlFU4X#FVncyyR3(J%-5qQb?99$p_h5R07Eai3%n81qwN-X(bBD
z`9*oDMWB{sW{D1iE38M8r~nQJg``S_+(dBeBeyiCB(op~lnwJr5|c}cAx34E=qZ2$
zp8;HQVyqh1%T3HnOiwK;R>;gP$S*2U2m)n3Ux*+BR1DN<g0V|7b5mjL;>uzeGp{ta
zpc2#|$}4~g6=de+CnslQ=7WR_KnvfM6^b&`GfEWli!xL5Abl(yP)rx4BK!f023WDK
zkegqWs!&jrnw(jj3MxmyZ5mLhLJ|*Ht}HXZv>09!fRjm4C5Xz%1eIf{3Xpais3DR8
zG6YnkfLhexm<QJeppIK!NoHOt$e_}K6i}BD))0ggj#dny{(lx10|Os7r2h|_pWnyN
zz)-`_z!1#Oz+lSHz`(=L!0?Wbf#D(_1H*DY28KpH28IMa1_paR1_n7k28OS^3=HRZ
z85kDuGB8Z$Wnh@Z%fJxN%fMj5%fKMY%fRr3hk@Y|4+FzR9tMVOJPZu8c^DX~co-Nm
zc^DXCco-Nsc^DYZaWgP%<7Qx};bvek<c7M524fzhMvjKSXb6mkz-S1JhQMeDjE2By
z2#kgRBm_X?I`~Hnp`F#y!B|oUV?jX+9d<>G=R<lSpspTh6b3XRkf)GXlwJzzq=G{l
zJY=F^i_&}3&}7I=Q;3fT4G71_E7;m9D8<Kv<}c#om8>9RB;XNs=>B0n(5#Y=254*z
zX%rrd3fKTXbesb^oDUhbh3k$sf{Yu$6l09`LnncdtU?w6jrkK8?FaiC6j~q*=YXbq
zz{5qL88|nr18dMB7?3v50156<3%H>mr33>4qzHszK8`j84MP_dRKkJ+BniTB1tu^B
zhLErVNrEt3fiX-0bPxt43Bt+><r$C>g<=KpP!(vLqF5mxGV%&i0mC5c!6Um+hOUB<
zR$hT#K_-gdGSd{G@^%UadIpf8Fqje00j8pYN+lhTGDx(;WQz(a6>JqibDTv5m3p~}
zB}JJPA^AZCl^Q{)3YEbFj7U9y6c>;ly5Lc5L~=8XRnP?uXcZ;qrR3-8K`0H-Ft>r8
z5sFpt&;^GJ!UTg@1#Mg=7=!!&VLS{ByLce|f7tr{|NIOLZ}=G)&hs-cEP{>&r13K_
z1n@I3NboZ-tm0!}h==z5nfVwPPVh1?Oyy-@2<2s9;NoRqcnmWWQm~GaqaiRF0;3@?
z8UmvsFd71*Aut*OqaiRF0;3^7K?uNlY9N~QZWefn5u%d>?bU<K0%6Fwgcb6@1Zb&9
zK}lwQo&sn#A38>%16s5K?nr{Bz!OUpKnwIB3r0c4fUvScdcHzZVsf@ZX@P<Uc%=wv
z;YCS)fi8F=JhNCKBQrfCwYWqfGcP$OGcOgje+lc>5i!sJvJQkv9B=?>0pZLvg-it-
z*xEP+T|-2F8NT8{FR`E?H7`X2G;@wvcMMwL3tn+-7OPiMl$ckX173BgiSP+Hj5L%=
zi%hM+3p!vcEx-|nd+DR59(X~Fh6$!o1(|tZqj1;_*QleQg{>EltPQ>{8m<Y+;aFEW
zL)SNh3;{1Ufvk-xL~>;zETjzd3=pQ_5XCtv1X~~tiXL!KA&+T=g4XFe=Yv;8Xn<G7
zLM#HU0m=le0V)D5Wz~QVx@kgU6CRVGRkX1T;Qs#;b_Rwu9FYD$Y=8ebeg=j<eg=j_
zeg+0reg=j+d<+bG_!t;w@G&r?^D!_4@i8z2@-Z+3@G&sx@-Z-o@G&s3^D!{6@i8#4
z@-Z+x<Yi#E!pp$W$jiXs#>>E<2VDcOl81qz7`o=)i-&=M3$!MHn}K0HHv_{wZU%->
zZUzP&ZU%;LTnr3*xfmGwxfmD{xEL5TxfmF3aWXKpaWXIzaxySjb22de=774Jf>LBu
z>u3m!hQMeDjE2By2#kin;0^&$D+putI7Yh|JY_h#2@X6rG`a~6lxD!#KyP#t9Aw=v
zc*7EChXQCx^5`bG(M@nv-2?|NcfdJLK|ul3UCqoZg>3172|@-}6rg)|6LWH)Tc8rb
zJ9$$TK-*8?rhq!LS%}VTkX~^{VnJ%OVJx&i3zA3b?84*?VDg|P1)0eipshNfExO>%
z0-$sc8v4mh%t5TL*C@{bsY_1G18=uT0xd{MECFqKfb^<C8&t}X9St%CvY!g>@J!H7
zF3{c&@D`);#Jm#7zDdyHjl2TAjN+2S<ZKO%Am|n=y`ogGZ!|QFbqqB%p`!<AiorXu
z&{XOufVXa;i9+=&D^!9qcY1z)ih_D_elcidsywk+AwQ`kF*7eU1+sr4Q31T=iva*Z
C$o@kB
diff --git a/python/ur_simple_control/util/calib_board_hacks.py b/python/ur_simple_control/util/calib_board_hacks.py
index 84539ec..0d8acd5 100644
--- a/python/ur_simple_control/util/calib_board_hacks.py
+++ b/python/ur_simple_control/util/calib_board_hacks.py
@@ -1,7 +1,5 @@
-# TODO:
-# ideally make this a continuous update process
-# where you use the previous estimate to try to hit the board at the
-# right orientation, thereby making the estimate more precise
+# TODO: make prints prettier (remove \ in code, and also
+# make it look good in the terminal)
import pinocchio as pin
import numpy as np
@@ -9,15 +7,24 @@ import sys
import time
import copy
from ur_simple_control.managers import RobotManager
+from ur_simple_control.util.freedrive import freedrive
"""
+general
+-----------
Estimate a plane by making multiple contacts with it.
You need to start with a top left corner of it,
and you thus don't need to find an offset (you have to know it in advance).
TODO: test and make sure the above statement is in fact correct.
Thus the offset does not matter, we only need the angle,
i.e. the normal vector to the plane.
-Returns R because that's what I wan a
+Returns R because that's what's needed to construct the hom transf. mat.
+"""
+
+"""
+fitNormalVector
+----------------
+classic least squares fit
"""
def fitNormalVector(positions):
positions = np.array(positions)
@@ -31,12 +38,16 @@ def fitNormalVector(positions):
for p in positions:
print("cdot", p @ n)
-# constuct a frame around the found normal vector
-# we just assume the x axis is parallel with the robot's x axis
-# this is of course completly arbitrary, so
-# TODO fix the fact that you just assume the x axis
-# or write down why you don't need it (i'm honestly not sure atm, but it is late)
-def constructFrameFromNormalVector(R_intial_estimate, n):
+"""
+constructFrameFromNormalVector
+----------------------------------
+constuct a frame around the found normal vector
+we just assume the x axis is parallel with the robot's x axis
+this is of course completly arbitrary, so
+TODO fix the fact that you just assume the x axis
+or write down why you don't need it (i'm honestly not sure atm, but it is late)
+"""
+def constructFrameFromNormalVector(R_initial_estimate, n):
z_new = n
x_new = np.array([1.0, 0.0, 0.0])
y_new = np.cross(x_new, z_new)
@@ -51,35 +62,71 @@ def constructFrameFromNormalVector(R_intial_estimate, n):
#z_new[0] = np.abs(z_new[0])
#z_new[1] = np.abs(z_new[1])
#z_new[2] = np.abs(z_new[2]) * -1
- y_new[0] = np.abs(y_new[0])
- y_new[1] = np.abs(y_new[1])
- y_new[2] = np.abs(y_new[2])
- z_new[0] = np.abs(z_new[0])
- z_new[1] = np.abs(z_new[1])
- z_new[2] = np.abs(z_new[2])
# y is good 'cos it was obtained with a cross
R = np.hstack((x_new.reshape((3,1)), y_new.reshape((3,1))))
R = np.hstack((R, z_new.reshape((3,1))))
# now ensure all the signs are the signs that you want,
# which we get from the initial estimate (which can not be that off)
- for i in range(R.shape[0]):
- for j in range(R.shape[1]):
- # TODO ensure all signs are the same
- continue
-
+ R = np.abs(R) * np.sign(R_initial_estimate)
print('rot mat to new frame:')
print(*R, sep=',\n')
return R
+"""
+handleUserToHandleTCPPose
+-----------------------------
+1. tell the user what to do with prints, namely where to put the end-effector
+ to both not break things and also actually succeed
+2. start freedrive
+3. use some keyboard input [Y/n] as a blocking call,
+4. release the freedrive and then start doing the calibration process
+"""
+def handleUserToHandleTCPPose(robot):
+ print("Whatever code you ran wants you to calibrate the plane on which you will be doing\
+ your things. Put the end-effector at the top left corner of the plane \
+ where you'll be doing said things. \n
+ Make sure the orientation is reasonably correct as that will be \
+ used as the initial estimate of the orientation, \
+ which is what you will get as an output from this calibration procedure.\
+ The end-effector will go down (it's TCP z pozitive direction) and touch the thing\
+ the number of times you specified (if you are not aware of this, check the\
+ arguments of the program you ran.\n \
+ The robot will now enter freedrive mode so that you can manually put\
+ the end-effector where it's supposed to be.\n \
+ When you did it, press 'Y', or press 'n' to exit.")
+ while True:
+ answer = input("Ready to calibrate or exit? [Y/n]")
+ if answer == 'n' or answer == 'N':
+ print("The whole program will exit. Change the argument to --no-calibrate or \
+ change code that lead you here.")
+ exit()
+ elif answer == 'y' or answer == 'Y':
+ print("The robot will now enter freedrive mode. Put the end-effector to the \
+ top left corner of your plane and mind the orientation.")
+ break
+ else:
+ print("Whatever you typed in is neither 'Y' nor 'n'. Give it to me straight cheif!")
+ continue
+ freedrive()
+ while True:
+ answer = input("You got the end-effector in the correct pose and are ready \
+ to start calibrating or do you want to exit? [Y/n]")
+ if answer == 'n' or answer == 'N':
+ print("The whole program will exit. Goodbye!")
+ exit()
+ elif answer == 'y' or answer == 'Y':
+ print("Calibration about to start. Have your hand on the big red stop button!")
+ break
+ else:
+ print("Whatever you typed in is neither 'Y' nor 'n'. Give it to me straight cheif!")
+ continue
+ robot.rtde_control.endFreedriveMode()
+
def calibratePlane(robot, n_tests):
- # 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
+ handleUserToHandleTCPPose(robot)
+
init_pose = robot.rtde_receive.getActualTCPPose()
new_pose = copy.deepcopy(init_pose)
@@ -99,47 +146,51 @@ def calibratePlane(robot, n_tests):
q = robot.getQ()
pin.forwardKinematics(robot.model, robot.data, q)
# this apsolutely has to be deepcopied aka copy-constructed
- R_intial_estimate = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].rotation)
+ R_initial_estimate = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].rotation)
+ translation = copy.deepcopy(robot.data.oMi[robot.JOINT_ID].translation)
positions = []
for i in range(n_tests):
- rtde_control.moveUntilContact(speed)
- q = rtde_receive.getActualQ()
+ robot.rtde_control.moveUntilContact(speed)
+ q = robot.rtde_receive.getActualQ()
q.append(0.0)
q.append(0.0)
- pin.forwardKinematics(model, data, np.array(q))
- print("pin:", *data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(data.oMi[6].rotation).round(4))
- print("ur5:", *np.array(rtde_receive.getActualTCPPose()).round(4))
- positions.append(copy.deepcopy(data.oMi[6].translation))
+ 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("ur5:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
+ positions.append(copy.deepcopy(robot.data.oMi[6].translation))
if i < n_tests -1:
- current_pose = rtde_receive.getActualTCPPose()
+ current_pose = robot.rtde_receive.getActualTCPPose()
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]
- rtde_control.moveL(new_pose)
+ robot.rtde_control.moveL(new_pose)
new_pose[0] = init_pose[0] + np.random.random() * 0.3
new_pose[1] = init_pose[1] - np.random.random() * 0.2
- rtde_control.moveL(new_pose)
+ robot.rtde_control.moveL(new_pose)
# fix orientation
- rpy = pin.rpy.matrixToRpy(R)
+ rpy = pin.rpy.matrixToRpy(R_initial_estimate)
print("rpy", rpy)
if rpy[0] > 0.0:
rpy[0] = rpy[0] - 2*np.pi
- # who knows if this is ok
+ # 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]
- rtde_control.moveL(new_pose)
+ robot.rtde_control.moveL(new_pose)
n = fitNormalVector(positions)
- R = constructFrameFromNormalVector(n)
-
+ R = constructFrameFromNormalVector(R_initial_estimate, n)
current_pose = rtde_receive.getActualTCPPose()
new_pose = copy.deepcopy(current_pose)
new_pose[2] = init_pose[2]
- rtde_control.moveL(new_pose)
- rtde_control.moveL(init_pose)
+ robot.rtde_control.moveL(new_pose)
+ robot.rtde_control.moveL(init_pose)
+
+ print('also, the translation vector is:', translation)
+ return R, translation
if __name__ == "__main__":
robot = RobotManager()
diff --git a/python/ur_simple_control/util/freedrive.py b/python/ur_simple_control/util/freedrive.py
new file mode 100644
index 0000000..7c89d70
--- /dev/null
+++ b/python/ur_simple_control/util/freedrive.py
@@ -0,0 +1,24 @@
+import pinocchio as pin
+import numpy as np
+import time
+from ur_simple_control.managers import RobotManager
+
+def freedrive(robot):
+ robot.rtde_control.freedriveMode()
+
+ while True:
+ q = robot.rtde_receive.getActualQ()
+ q.append(0.0)
+ q.append(0.0)
+ pin.forwardKinematics(robot.model, robot.data, np.array(q))
+ print(robot.data.oMi[6])
+ print("pin:", *robot.data.oMi[6].translation.round(4), \
+ *pin.rpy.matrixToRpy(robot.data.oMi[6].rotation).round(4))
+ print("ur5:", *np.array(robot.rtde_receive.getActualTCPPose()).round(4))
+ time.sleep(0.005)
+
+if __name__ == "__main__":
+ robot = RobotManager()
+ freedrive()
+ # TODO possibly you want to end freedrive here as well.
+ # or end everything imaginable in the signal handler
--
GitLab