From ef07108b24d8fd496cd796b309881c2a3089d531 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Sun, 19 Nov 2023 16:11:38 +0100
Subject: [PATCH] dmp and demo code should be done, now it is time to test. of
 course there is a lot more work to be done down the line lle

---
 .../.drawing_from_input_drawing.py.swp        | Bin 0 -> 36864 bytes
 python/examples/drawing_from_input_drawing.py |  16 ++-
 .../clik/.clik_point_to_point.py.swp          | Bin 0 -> 16384 bytes
 .../clik/.clik_trajectory_following.py.swp    | Bin 0 -> 20480 bytes
 .../clik/clik_trajectory_following.py         | 103 ++++++++++++------
 5 files changed, 86 insertions(+), 33 deletions(-)
 create mode 100644 python/examples/.drawing_from_input_drawing.py.swp
 create mode 100644 python/ur_simple_control/clik/.clik_point_to_point.py.swp
 create mode 100644 python/ur_simple_control/clik/.clik_trajectory_following.py.swp

diff --git a/python/examples/.drawing_from_input_drawing.py.swp b/python/examples/.drawing_from_input_drawing.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..dab6cd932f3d42ae427684d8449babf19a525544
GIT binary patch
literal 36864
zcmYc?2=nw+u+TGNU|?VnU|>)YjY>TfTEp;8ih&_LzqlYjC9w!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%$(goHpz0XsuI0|NsS)W42U
znh}j=gYseWPEbBf9XpiI52c%-G)$fY%7^Leh4NwYtWdr=ls*TgVe%|cz9N+V52a!9
z%uv1%lvaRBKq;u`C^Z@aqaiRF0;3@?8UmvsFd71*Aut*OqaiRF0;3@?8UiCT1QJsi
z7?c?p7@R>JWdQ~TSpWVfKLf)Oeg=j){0t0<{0t2I{0t0l`4|{Z@G&ry@G&rG@-Z;H
z=4D`*%*()F$;-eX!^^<%l!t*~Ee``j9uEV99uEV<3vLF672FIA_1p{$q1+4%>f8(r
zx40M>8n_r3>bV#g(zzHILbw<hOt=^rG`JWTB)AwD9&$1;EaGHf$l+vQQ0HV|c+A1T
zu!w_!A(DfEfs=!Q;UGH$!vS^%h6;8D1_O2mh8JuM4CmMw81}O<Fch;fFsQRJFx+Nk
zVCZ9IU<hYrU=U_yU^vFYz);4*z+l0`z;K_Lfng6b149Ng1A`JX1H(xs28Lx!3=H8+
z3=G0d3=CHo85r6aA>rW!3KPP~LIDY6q~;XZDwSuXmSm(BDOBc{su!gylon^^r7M(V
zq$;ErWfl~q7Aff{q@<=Lmgbb$h7^^iYBGRKC`c?SPA$?)Oi77PEJ`oUP0cIOP}kLk
zYEjowNK7ut%+Iq;EJ_C})pN?v&q+<p^Dh7iC+0YUrF9fyz@~vrM8<ypA+A;mdHE#@
z;Kdan(^FFvDpO04+~Ss)Qw((sOe=<e6v`9xN)$@+70NP;OA~W4t5OxpGfOfQ(o+jk
zOG@%}6p~6ypiTh064OI)jl_EitB+tIh{e?~*F!v=lAl_fr(OaIztY5<oJxiA{Gx1y
zq}1fZ(&AKw%o6otaJVEY6l7EuXC@ab6ldm^<|LNngW?G_G;s$<R%vmGLS|k`YI;#(
z2{cR!GV}72lQS~&^&qC;2oSh3-Tb_q%3%>IiAAXjMWuOpuyh|9ga}kU{K*3v_?h{6
zB!+~tLWsYMzd}hyX0bwkUaCTQJ}A<QQjwy(v_v5%6%^k&sflHvs8`5HElO2L%P#`w
z&a~2!(xOz9#Fmy{l$%&ml3Em>oRe5wjL3YB&@Ac#$;ZVWsW}C1FvXAz4NHaPi6zMy
zU<-=!lk!UxQt}ltOBC|+z(VC2nI)-7`H4j-dg?l0t0AdG!BzpD(P4H4fJ+FCl+@zn
zqD*kEw^jE|%uZEERDdpk0-0KzT$GxcrvpiH%G3d1hbhB6gh{8T=A{;8CWD*|F1U&@
zmB2)lA!bp@T`8$)3hAjOjz#Ik8k$xN&@@n-UsM7L-QrY*;{4pyjQny2>e#2ufF}?X
zi19uo7?i2z9tO}VI0cMFb9%Xnd5P(%Ma7`v9uykRki~R9`S}ID5GfsnAW(9I@^G1x
zoRgWY2O{E2iW0L@lS}f8D&y1gb8_-QYw%#E=Oz{yxdbGZWQ61!y9DGU=A{xguOL4&
zuOz-CKOW40nUtPd;+&J24Rvl#Y7s0>LG?;nW>Ikos8&_TOHEAy)k4Xj8c?CMKmp`L
zy@E=G#A1cwg4E>9v`ml!Q1*rykegbRo(fGBX_+~x#UQoisW~}1Ai+w76mWTwl3JWv
zRF(=dJ1JG6IJF=VRG#DVO-gQoUP^8O%<(S10XhnP`613aA<ki`IgZK6&iQ%8B}Ivu
zd8sK1xC|*R$;{CMW#IUN#F7k{Ss(#Wh~m-(uSWIYEST1Uoct2EqWoN!%;XYW+QF8l
zr<TO$=BK3Qz;wa|7&3DUK*<JH2Eq8r`303Qc5Y$`$flgkB)x)45DOHb1vw=!`MlCx
zNQC4Sz=Ysc9at%7&8V`1p^=_KE~pd(m)wa8xrv#1p!m^IC@3vaNG(e(sw~NXR3wQd
z3d#AUIVlQBsR|{fMW8^<%qz)PNK{BG%>y@o6jF1FQ_G>z4$5l<iOJcC>EN<HF)sxa
z(|HOR(83>7OBCcJCZ{T7<`tKuCZ;IlrzwDZ2(m2`YB$U<Lqk2VgP`p(1#o?!keR0d
z3Q2|hB5+9qcBl@6vVx(3o<ez1W=X0-N`5l9NK6E^(m>v>%r8|)&sRvy$<IrN6enPj
z)Dne^%A}&q6ouS;kW$^W{G#Mk1?VDJ24w|HJ%z*+un!b8^1-ElVveRlN@{X`Zb5!=
zCP)v+{Sa+wsi`SRiOJat#rX;)8Hptf$_f?`{h$I-Avr$<oWu(<le0m&2h=vrO)Sbz
zEm8oNCy5FLMX88<l#`zfc0Pl$f;l7<67!(VDTSQ;a@~T&;$nrg%$$<cB8B|2RA_Aq
za=&g#eo<j5C?n++=Y#UH8ALxU&4P^uwVObZ2+M6Cqf)`OGO992Xz40|+ge4bx%p+O
z3dJR*X=z|DsuwFHDio#W=7Ach`DqFTiNz%fB}JL3#V~!KdLE%3nv_6|K^;f}OoZom
zunQQJ6-@O&CW7oq%*jzG%_}WVO#ufUsH#y=fHZwUfrn_dz$*cT{5&0K8ptm#QBX?G
zNKMXGK<XMO6qh6xl|YIqMDs`q;@ZTV;(Ty?gPNRar8zkYsYONkMGCp8#l?x~sl^IO
zi6FOtTM?PXItr;3dJ36oppZ;0N=r;mErxgx)C5Y-&&x|q1|<On1%;f<Y*2DnD9KFB
zE6&I-RY-*{tA=L6+{E<EWQDxa+$2!t0qckqD>5i6nCK~hO2gdLk|I#EACWU5ITcn^
zKmrBYIxhz05X^8j)>BB!OfM};RR9G|Mt(V{ye&>n%mEkB3ZNpjEHx)TIkTh^6k>S_
z$wjG&CD3fCYp3Ai@8_zZkyw-tP9-JDnheScMtTY<sX3`75F<)Keg=hAQKdq1elDoL
z15F3{X`oU-p(r&cu_UuBRTrtyg{lA*6yP+L0!o!RnR%(YAfgCbN`joBYX`4_kjpMu
zX;YF9YLzGyWu}120zCzIHC2+43hHy@6_+IDm1KfbKY9>>%M(xm2(7q4Nex^yW)_1T
z4k{nP=@a7jj8u5zuQI>12*xRfL_;DtEWtfA)PezOI0LBvKZ}Qf;U5oV{XcB}d>cOl
zLkvFyg8)AR!x26Ph6+9ghDbgJ23tM`1{OXBhU>fx3~P897$)*EFck4JFxc@jFsSn~
zFevddFfi~kF#Ly_`4?m^;Xd@JiqQ}l4S~@R7!85Z5Eu=C(GVC7fzc2c4S~@Rpm_*@
z#)y;^Ks~;^{BnizRE1>t&_+%rXbuBBTn#32^79Mgq2sE0MWuNfkYRrCpuS#FNeX<&
zvL0w;!b<}*I9QNhtXGs;oRL_Ns-a<~qp1neqYRm*$OH{pB<18MXM_5b$)GU?uuaMe
zAQM1+;>^6vlFY;$g@XKI$ixH=+k7;VON)w9^Gf1DDj}92i$oj6D%dKZi)bqt=ouJ;
ztwfemuvI{^rYJQzHM1;L4>Zi4TvD2t6XF~YkYAh%^}aH=r(0S888`xsg@9)>z=lGG
z<E_Bs7|`J*(1=rJUOGgYI)m~Mvp^Av14HKZ5_1%aQ%gz<a3}%`Q+IL#WF&H;QLt5j
zj%y<g!)t(pS_d}J2a?eNjUg55C=@4_rN)DXvEoxQlS_0Ia`Mx`ENJv1M)VbIK_fGu
zgrWf*F9aDAUX%$MGSLAI0q7`{B*X2<NiEWW#IPo~|8K#`z;J^T(*K9AujglAxW>=G
z02&9F%+J73&(FY6%Fn=H%Fn>?jSn&=(8|Za;Ks+mz|Y6PaDbPAVLvYeLl!RsgAgwR
z!($!>hM7DJ4DLJ(44=6f7&dS-FihiSVDRQ<U@+lkU=ZMDU|{2BV7SA@z;K$2fnf?4
z14B3$1A{yl1H&VzdwMt-7(zkrqIoGWYQbm-jE2By2#kinXb6mkz-S0i7y_V%9|+fi
zo2DQZ2&<>$7Q~mO<`k<dSSjQc=vAc_<riy!X0eL(auX}!GeL?#m_%(QsYQ^9Bpn4a
zO-;xmB9Jj)OhvQQQwni99iJP({w~NbCQmbIcEXy-3bqQ>;J{agEb#(Qv1dZoe1TW|
z!8%rDi8-Z^o)sv#&_b)cC^auR10}l5paG29Hb=4!sR0gMTBA^&nFH#<6@V5MLS`om
zOH)gsiv(daf{=b5*g3^emt#6q4=Mv%-~e6+lMeDzc}605RSamYX>np1s4tlbSzT6?
zpOXXPf;tfj<(YXY`Q-{wBQ(HWwbFEj%wo`TG|)6Sbj}bYk(ODMTMk;qSC(3o37W_T
zEx#!(iU)ObL4DV}Y_Klq(n?5&L>aUU39KNc6ui11Q6Z@myzaFKGUc3+3hRNwRcb@}
zAqvR$6=&w>!CdL+6YQ@5S;kqCuK-%F37!fCMJId-P)1@NWMPj&S!xkjb7o$0PAP~5
znp=hXw=^#$wMYTH6jwhlzeE?zf-T4eb)OP*Q%h2dU||okAu*>YH8G`9AuYc&FGV3S
zPr)(36EvR<@+0hYGUy~MXbK)YWei%gk_lP~ssK%4nN{G-3G)zmjUs5c0puz0d~SJa
zW_m`6jzWlY0C*fCGcUCW)Rj}nO)M$OtN_n57lXzE!0VCV3kSj7Mh39wKq<E*S;1BT
z%c>)d^u*lU#Q41Y+|0bh932G+H&I8SEIv1}LPsGHLZucI$Co5S;t^yVSU5f}C9$X|
zu~NYnR9xlfr50<LX)0)ed#8Gd$;qiX(2@u43S~?=Q2GL;s6_DCfkJL#MP_bkE-3MW
zk}EhCkhGVif);ILfYM7zYEeN>Vx>Y-YDsx&DkwdJhD#u8ctC5sQd1O)b23v>i{RS9
zUc%*QaCm{s*}@dK4rK+3EQE}2K+J$G>;R`vNN&=|EKvZhvB*h*EQ<!)m01D_Wza$;
zE4V|z(Fa;N1|3z&OoOaRE=n)fD@g^d#Y)VHPtGqb0A**0HqasoTLl;20F5ljN;>%Z
zJctF#kVO-zASKAFMN$<IEBP3}{r_xM28LwN27dtt2H5`l?feW3-TVv;UHl9TQTz-H
z3j7QV?EDN2d-xa_=JPQywDB=8fcpQ*&@}-(d<+avc^MeC@G>xL=4D`L;bmZG=4D{W
zfbI>rz{9}M!o$Fj&%?mr#KXX#%)`KNotuH7o11~bo|}QeikpE!f}4Ti4;KT&cP<8o
zXIu;n)43QJ%D5O9T(}q*n79}iZgDa&Ea7BeDB)ya2;yX5u;gT5c+bJW@RWmrVLJx{
zLof#egER*N!zXqIhV$$U4880O4F2p44Cd?%4A0mY7-q9U{29x}z|hCWz!1a6z#z-U
zz;KV1fnhl-149IK&j1rE1H&p728L=D1_nduo`G&=1_lpi1_nK528PE>3=C(O7#LPC
zF)++yVqhp?Vqi#SVqnl=VqkdB$iT3jk%6I#k%6Irk%1u}Z4Ut`bAd2gO+%<@fWLkv
zt&Rj)55lAM@@Tybs?I>AGj#ZOv|a``Hb(1Z{Pi-Z^Z;SdqHE}gHh4_AFdo$C(E(L3
zdBth*ASFncfa;|Dl*;&$#NzDuw4%gZ=sqO$s!hQbywcklv~aRGGcgaeLfSL0B;N~E
z1%fylpyhn90d#Pk58Dx?3~?Q#F#~SbXha+8nOd0WDCipMS(sYtDCipKS(+My`Njs8
zItqq*rsfu4VFLp*9nfS6n23c{u=#o5RllG{4|t6)ta%PzMqFA{oC<2@K$`#gMbHf?
zrNycG1x5K~nJJ)s8+oba;LR%F)tungfU-hn3S`d;s0o^%RFasP2Wp@vg64_}5{pVe
zi+sUZHZdnN3DWaZC@9KLPAx8m+7E7Hre%WLtSJg<nH369uY<Y`U`tAh5<w9J3G)1S
z@Qe*;5+O5BFWA*sBS=S~0Om*V%1e+_!BL@5TmV`xjc7rjwk$w_54Hdl?L~>1#i<H@
z`6ZsY1v#mp6_}|huAoJ%8cHCqrRSHFBr1SAFu9;!QDTWg5I8t9^2>D;)RXhU%N;>o
z2hjd3*oHZU#FAVkh}%GRKw4l{;HeT&zoZy!0(i}~LVi+NW`1dLP9@kysO_yFXd(rz
z0nN_?r7CD>rh<1bz$Rr9LG7=+_++SLW`16AJg5~{3~@nb8Yt?E^^oKtZ8K%iCLbhG
z(9&G^a%!-1Kp_PRanOo?ZBX+Xnj#JK48Um&lIkptE#Qd_6suN{h05SICTN}#x(P`k
zKQ9>+gsBQep!RcSv4X!h*cMpufYLyf0_c2YP=Rmc0_si}yC{Gh0aXWDTArAvke`-T
zoC;53xsWB{8esb}i$N(bIUk&^6AKE8@+&fP6HA~O6=oAOxgh%;ywWx`1r$`^TvCkO
z7qSA)u__=dhbK~y>lADiaxwOnXcS~tq~?HkOF(kE9ykHVmuIGwWT1#-fcsXE$Wn%c
zU1o`bu?yJKU;<&3f-QLMEVvMX=>ToQ09OWJw}2O0rz$`;Sb=qe3F^#ZLZ==<i5h}I
zc7yuviQs4gFHwhZAsG=V3xd|QXO=*8fk~)Y)NvTtkKmG2!4_r95P0MY5>lu|yMirf
zP8f9?50YX~rh+JeY}*1`KpnS2Jq`{8>_l;XS_ybygU)lonQ02~@p*~4sqyg&wzdjN
z@$sN_-0|^BR**h?Q7ULdi$YRSYGSrqPGUOt+272P)Le*q&_EZ67Y%O3gGUuXGyI@6
z2W6=_sA~?u0gR-fuvi_ewh+Iv+yc;I0;t;D0zHhi3~1&;jV(08VyrS~@>&5lP^D0u
zn3)2q8`V=l;t(H!;tTG5&|pRpbYKqD7cS7tEY3^J(<n>LiBHZ=i8hGUgq5_Jxw)w+
znTaK-psl^dCHV#5!ZtrQ6}%Zop*%ISC`BP7v7jIov|mUAy#FOHKTkI~Kd%h5B`hx)
zT7=<qfJR<{UJhskC0#EszbF^HwxcXHM^nKD)G9DAFw_K(-DqSg*eRGA7#P4^2P(A{
zAayZlBm=ZVCK<FZ2sEk&9y<cLxB#>w0vwv)e5O~NnwJuo0`n|r=Qm=kMMoh8I)VTV
z3sAY0TcB5#nxmktkX@jlr2uwaL4L7<t^#Ug7;0*2D(EUC<`iTkDrhN`B$k5v|Kbb`
z4CbJID$@S{6Z{MewfqbWq5KRCzxWs!4)ZZE^zbn-1n@C1@bWP*T;XM4n8VA!;Ka+o
z@QsIoVHFPpLmUqSgAfk`!$ocehUwf44DQ?v41(MY47<1(7#46bFeGs?FlcfyFudku
zV3^Lyz);J{z>vzxz@Wp)!0>^Cf#EO*14APR1A`9-0|N&Kqz}KAoq?g7oq-{foq<7+
zoq^#f8w101HU@@3HU<U@HU<U`HU@^RtPBjGF^mva1_o(X28J^%3=C7C{dW%*28Pef
z3=FrJ85mYDGcaT^GcfowGcd?8GcY`0VqiGR#K2I)#K2&{#K7=`k%3_bBLhPoBP2Y`
zKw-jw6@bzTWTP&|hJA2dqzkT#)O8d}DhpC=({l0?OW^HL$Uf!V)WqV_B2cZJSqx4H
zpf(+-ISAGdP7L%Y6G07b&}bTH$0XtXyU_BLIu3*Q5fU1SDWH;|9JFZ*wv!B8+2y6c
zYHjsmq(NGwwnRyOKG+~ojDRq7<B_d_o`Hceq;>)cgD_|-m8}x$J{VBb8MM<eGe0je
zM<F>sH7zYOIWsj6bS?s93pI{t24^Cy5e%{lgpplhh|MLifj!VxE%3meLP=^-t`2m}
z5VXm$I5#mTM+ug4aJVG3pjfvgnMkLYVsi*&|6mz-w;_0E5+p&ELiX=Nhacgt)KMtN
zFG&S$U&;YHn2`IxL(aO1L^;q<&j8hdpj?p#-pQAds!#-}BtbiJK`mJ5pf;!+gS!C~
z@E{C{YtlSs1#aqPWTt1N7D4yV;|Owym!S3&<uSBy2RTszR0D$78y1&<Mxr3Qs=;mm
zH#bwuQu9D{8F(6~BtKsP6b?8%T9RLqn4?gVnVSlpFGx*+#2|b~7`!<U)awFIp<u))
z-jrUFSZY9&AC1r>4;l$Y`QRJ|&R^gwT|kT75a|pON%?t@9gf9tAL0o6>;k-wf!GNf
zjR$!dc47%uuOl6QLhvXSSbo)o4a5_FLIos%@s^w5QcPI^v|3LAYl%!m;R`kclC~6J
zJ!V^VLp@OZfdfVzbOH=yDki5g9=x43u_RX=lIL&)ICMrcC$&hoxF9t(MHjSDGczw8
zXEcC%T9rxA!4J@G+>(3+kRDKF2QFhk*ZL?FXQt;R=AgJZIU_$aIkng}T0JE(w;(ko
zpg6TOB|j6?PAg7T*HKW<N=(jA%1q1)0gYc2<QJ!^L%YWqe#UM9BpP6f!6BkloSKuG
zT%u5(k(rzU8eB@wR!GcA&o9a>$pBANfls_4lqf+RAW(}E<g=XAB3RHAmlVMYZ%79j
zyc8Bxrhz)@D1L|47KW+1Mv&-$IWq)wqDf{kX!bJ&vKtnB+y-PnEXXX(4q;J#F1Q?l
z_96A)&WD)^@j57wK-DZLauSO`Ljxt5*?IXH`DF@4sp+LTiAB)aOZ*4elw@XS<mcs=
z>4MCIc^yle#|ko2268g!q?{tifkvQF1@Hg^XjT*?2iidnS_++#nv1<^H%!&VTC;;R
zr-2&jpykq#4e^jo)1XoSG?@+R1yq7J#_EB`c_9ZPfpY<9)Bzq!I5I+deqxR;*hH+J
zL=9Wersmwlyh?DPTwIb`03JR8PsFDrmL!79eptx_T1<dc5JBq-9IgNvp$nPI*DcPh
z0_6*EBQrA(sfh`$Gr)Vu;VS_^N6+L!j{X3hssNf$%`Z*Q0F5m|CMiLC#gkIOiBcId
z`&dwrlbM{Dl#>cxL;;z^1Rb3NN;8m2ZcvaXCxW&#mnbBGjv@dZs{z*rF$-_0N3<lw
zELa%BY4DzBL<xnp;DjrNGKeThiFOvmEU3W>kZc8;<4(-U$u9>7UnXq+4!W~mK_jyS
zH1JidkXQohFM3pIBE`R<0SG_}0UU`SH?cw&ZfG$qIU*$in2TVvldFQWW00#`sE<Op
zXNZSFh=*q|s91m%I|h2tR0q=lqd{diyfY1(i#GuGb`3!zm8J?wm9VM?da@HtD<m|M
z6AK`VJk-HQv!tegGa$Hl0#_%{S$wEo9R3BX)P>CV!+Z-VVNi>7P<Pu(0d~|vBIp#L
z%wmPy)Wkecy8_`^wDJRM=zyJAS_C<433QH!LUCdxXiyroVy0L@JqL7R2zX2tc`bxG
zw5)^$LT+M(f}x(d4miZ14p1=AGl3n01yctJEO7r{y;uQUq!%k78>|PeLlcXjg&R_9
z9ahEQ2s>m$P{U3k2HgKY%gw-`3fiYHz`y`Izkf471H(+{*?+>&F@P0(3=E+8el<P@
zhFiQ04D)#z7@~O@7$kWa820fnFjVqD%(=|Nz|hUZz~Ico!0?-!f#DZ71H%q(28Lza
z3=DI*85rhpGcZiyW?;zRW?<mwW?<OM#lVom#lT?6#lY}^lYwCYbbeojlY!wZ2LnSn
z2Lpo}2Lr=Pb_RxN><kP|><kQz(7AnHb_Ry;Yzz#C*ccdAu`w{rU}In?V`E^5WMg1Z
zWMg39VPjzU&dR{BkClO8Ix7PMXpBIgm4V>~3j@PW76yhY76t}a76t}=76yj@%nS^h
znHd;Lm>C%Cm>C#8GBGfmW@2F2#l*m{n2CX*iHU(B9XeJZ$i%>~gOPzDnvsDa3i~(#
zC}ttGz7_c77;q(*nWvBlYW_kS3W$SiK!dociN%!)#pQ_wpaCHTjr=0$p+uQ!km>?_
zLJ>$FQZ~R0hn!plYTkkdfj|ZorszNpwoA^>O9QWjQqZVOEe4%*lvo1E4hqSjbyJ{{
z9JG)GdLT?b%wSyw_z5zgA|BpEhnB(+>9qW!^28#@ku>n*I$*Upq?-*|Km{7yf`kR=
zsG8CeL=!M4u^cjrSCUw&U<>NUMO&H0A`c%LLN`o76nN={C^#rUR^>rmlAc-u$}A<&
z6=Nxd@Xbr<sU?9a8k*Ql^8(GhWajBX-2<LH^nx}mAyYqk;A4@XyJYl06}FB-A&l$g
z@97s3@96>wtb)uuJ!nXHXXd5mCYEF-7vnJnq6^{)=wxJBYEfc(Drk%mG7AK%;^RS<
zMyo^AsmE&SfTwS4q1|Ix)Wb?T&@pzPhyw34gSV(ML8rhJ6@i+!8jw0(At@D7je|8p
zoB}heqzH5f7AQp|Vw~y&Js=4h=&)sli4cc_ML}&C#AYRg!$EUv`304FDXFOiptF@U
zAU=Xw4HE|USJU(J!N==>+)w~+9u}u6Xn;~waz5A_d8x(4;I;@vJH#mD7!L=p*n}>k
zQdR)BfIzLkf};GS#3az!kI?=|3g}#<R0U|}0G|ny2wGcLk`GFT3L5G8C6FC$$Y~B@
zt1@Uw8>|ln9n{rPD9^}O$j-|zhfTsl@*l$9{GwuL(B~$CI!DF%xv*oApsvsbHRC{&
zn6T|}h`Ckh@(j>&MsTeJsUD!yy^s;Koc#Q3(D6U{3ZX&ZBZ~AB3o^l>np#`}ny3Q>
zDmaNG=0X~Is96OXW}pd#;?$D(lEhMtlEhMok0H4aBn4t=E0iSbl@_EVmZXA4nqj#j
zr3AD#5L|)jrIdiv1Za63WbGP!@JIpF6b4Izj016?Sp}qB8Ql2=34&F?rYu0KYa#2U
zP!kzK9Fl|;ic&!{ETA9+g%nE5B?pvpprs)+e32&`z=w!3fKC&Hwk3V?^9!IG=pdfN
zx_?ebAyWr5W?2Hxt>6O)q52g-8~;FqR7t5I1)#H<O7gL6|6^cK2B`wi5I|1#QZNRc
zW&~=;fX`9|Ct*b7f|3nnVk0dvFS!zOo)btAd=gj@=+sh_!^ptRj*_BE24zTsG}HvA
zi9FC8JGheoN)q{bIh6{aIT7&EgGyb<aYcy52AQB~^_)`B5o3^c3|MD=wgTjIO+Anw
zKpiOXNvfcBEqLHCu{^O7l7~Pikb!!apzaW;w*b-rIvY1d0W$IhTgnTWA_pD$gt4F*
ze1uVPYEGIS$gudb#G*`a%epvUAuk`aRJH_sR3Nyw13D%S)F**hk(*y!0uCmm+@G13
zky?~lf+aq{;gy~bb9ZV{D#)=V`Jf5?%o4~!PC1pJC@W4)RRDGJK!uimQeq0IBb8YU
zQVm)w4R#2qd;uLPsslPX3bF(OlnaeCk%o9dCzPe=f$MijXhM#s1C>KXl?n{Xi0CR$
z1)UoQs@|aq#ZVK}=R%xf3%NJB2;?~UA-`b%fEJX1TiqbnK{XqJ3X7urlv2=HVTqtT
zQ<S2Rl$j1a*bY?af=;$72K5zTC-8x$%<^+eOHxyi3^D{2hoBTwlvAmYn_rZwkdc^|
zQmmVw2e!K;qqG=w;%;#&<X|(<HfvCc3|aRMi^pOGQ2!j1y;Hym1?md$>2;}<pq0X)
zfh*8CbU6xeJ)lFsG$Hc?&|C+~wV?JD=&V+#?WuW?#UUU^L5~-Mu8~hl%>b>D0cC*V
z<kY;>Vg+5$X<eEMsTH7pZ)Qm<Xa_iC$QeF!iQHvZ&;>_Zaw4eb3_dsweAX<e&7*+n
z)S}e%#G({X{6p;rRq$W~z~>PfDL^8o7<AqjDEC7rhCyfRAs0+JsYQ^JPC*ScXuCrJ
z*80*xG}{VbX99v^64Dq0C7n#rxLRUfaw-GCBS=AQHcSlZr$7#f12r^2{TtBemja?Y
z3aS&qOSZwo*x=(mk$YdD?k%KV!O>v>b#9@j2kU~2Ca$MTjKiSkvVk&JMk;92L2-!!
z*nQ}|Z-VXvEp#K<aiF9I@(^UC8yxqbWRDn@LUsmx91uq+feg|mX-EnYX~^qs!0Va7
zZh@ukqS8EMhd`U0IGmBEi)5HC*f3Zp6U%S}B$BX=&Zp!fuL44{pWv`OlKrF($itF1
zv|ny(pl1S!d6)#my^v)@pwteELGT2vf<|VZ0%XUCCb&sf1UgI?veplB@Mk_K5Fw)z
zID!srkS@e<SeQWuHK2nbFjpX)XN)#V192aCvY9;RfzJPnWny4x1YMvfz`y{zpYK0E
z1H%J;1_sbRfK+}41}^9rz)wB~hK+m-44Hfk42paV3<`V<4EJ~$7|!rAFdXJ(U^v9f
zz_5##fuWC=fx#4d4!|TH28MVZ1_m)628LtY3=9?A3=Ce}3=E&S7#QY4`~Lh~3=DfX
z85mY@GB7OXWMC-eWMDAnWMGixWMFu~!N72VgMncm2Lr<#4hDvL4h9By4h9Az4h9BU
z4hDuN><kQh*%=tB*cli?*%=uAu`w{*XJcSkz{bFk&BnkG%ErK;%ErKOla+yC7Apfo
z3M&JHI4c9gM-~Q#2P_N>Cs`O6R<JNI^s+E8gt0I%n6of2++&7@3D)WVQNhs=pi&5c
z@;IdXGCE2G2@B|`3~ZE4VRV!Ty#Ee5*fKgw1nO{sMmRuC>CsUl<<U{1^mx!pSj1=$
zs2hnGibRM*irLXoqR~+z(B${%C=p^*3OrUaI!Xi?x*8oN0ws*p6yipSh+HWF8T<n^
z@UUU%@Ev5(9Ge1|m@>gpD`MRPo?(D4C<X0E0rO$jfM`%xAL&|$%G44ah2s38qDloT
zeFZ&*$ox|9+D-7_9e4oU5Ht=@oT`wPlbBwtPy{;FK_M@{9J;s;WF+n#aq2FaDd06X
znAakJPLu$-1$OaBW*&5O0Ji%KCIK4g1NlS`;cCe2Do9WPd_*AVf;sSD1n7#nVhv3O
F1_135HOT+~

literal 0
HcmV?d00001

diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index a25e713..caa9671 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -137,6 +137,12 @@ def getArgs():
             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)
+    parser.add_argument('--clik-goal-error', type=float, \
+            help="the clik error you are happy with", default=1e-2)
+    parser.add_argument('--max-init-clik-iterations', type=int, \
+            help="number of max clik iterations to get to the first point", default=10000)
+    parser.add_argument('--max-running-clik-iterations', type=int, \
+            help="number of max clik iterations between path points", default=1000)
 
     args = parser.parse_args()
     if args.gripper and args.simulation:
@@ -226,7 +232,7 @@ if __name__ == "__main__":
     #                           software setup                            #
     #######################################################################
     args = getArgs()
-    clik_controller = getClikController(args)
+    clikController = getClikController(args)
     robot = RobotManager(args)
     #######################################################################
     #          drawing a path, making a joint trajectory for it           #
@@ -251,7 +257,13 @@ if __name__ == "__main__":
         raise NotImplementedError("you gotta give me that R somehow, 'cos there is no default atm")
     # create a joint space trajectory based on the path
     transf_body_to_board = pin.SE3(R, p)
-    joint_trajectory = clikCartesianPathIntoJointPath(path, robot, transf_body_to_board)
+    #TODO find and fix q_init here
+    # ideally you obtain it as a part of the calibration process.
+    # of course you will or won't use/provide a new one based
+    # on some argument.
+    #q_init = np.array([1.584, -1.859, -0.953, -1.309, 1.578, -0.006, 0.0, 0.0])
+    joint_trajectory = clikCartesianPathIntoJointPath(path, args, robot, \
+        clikController, q_init, transf_body_task_frame)
 
     # create DMP based on the trajectory
     dmp = DMP(joint_trajectory)
diff --git a/python/ur_simple_control/clik/.clik_point_to_point.py.swp b/python/ur_simple_control/clik/.clik_point_to_point.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..a49da600dc00a345bd81c6a1d2409732714750a6
GIT binary patch
literal 16384
zcmYc?2=nw+u+TGNU|?VnU|@(-k4#+~RKxI2ih&_LzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7TVUbAA
z$;{RV5%C52nRzAgCHWABUO^?q!clTG1V%#uDFk>KjExKpKmy82iVDI)p&;fc9u0xf
z5Eu=C(GVC7fzc2c4S~@R7!85Z5EvmLP*T9eP|v`?zy$TLBa~)DqnV-n5Gd^grD5uz
z+)-*Y1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(Gz3ONU^E0qLtqewKw=65gAfA)gAu4B
zFTlV6>;J#tXJEL>&%kh!pMhZ~KLf*Teg=kMeg=jheg*~>eg=l4d<+c5d<+ald<+bE
zd<+bFd<+b1d<+c7co`T<c^McKco`Tj@-Q&e^Dr>P^Dr<d@h~tP<7Qwu#Ld94iJO5T
znwx=votuH-Fc$+uF&6`aIu`@OWljc$I!*=#eNG03LmUhYB^(S41{@3wzt|ZV*03`$
zl&~`}NU<|8JYr*DsAXedkY;0GxXa4Gu#A;~A&ZrPL7kO>;T;PD!&VjshD;U)hCmhu
z1_>4hhC9p*4AsmG3~tN}3{1=n3@4cw7)~%TFsx)^U}#`sV9;Y?V7Sc4z;Kw6f#DD%
z149)fBs>g3VS*87$_n@ZbkRaiYEdyhWeUm+%G3c23?O@oQcFsU@)Qz_(u=`-kQfY?
zWTYw-XXcjXB$j06=P6XCmgp!H=NA=KDp=_&=qW_zmntMD<|$+(mZd7>=jBu?7%G%x
zq!y<tq~#>07b_HHre~BW<mH#EYr>5#O3W-yRq)F%@dT~Z$W6^FNlkGrD#|a?P<P2p
zQOL_LQAo>7FD*({$S*BX$jC2OD9KkSC@ldQtdL%mSx}H#q>!1X5Cb+bGY#x;J(xsd
zUJ672;c6?eLXh_qY!wO;i;7c=^uSa+NK8W$5(+RMr=+GOmgbb$svGJVsOu<Xq~;XZ
zs)HPrm|Rktn3Gc(Us{}+qL5gU3-uG&s3N_@loXJ8r63P!sO#z?B2%}xAT>2bw>UX5
zCo?ZyT}Pp$vLMwqEhj&*L<i~%kmbq>xrvoYsS1_(r3&SVc_kokgY=|=Jq|Tap*S-=
zFEIz~BakK#hPyZ=F}EN!C7?L9G$lVXuPn8wI8|LoAvq&IGdZ=`Hd-CKyn0q*a(+@~
zVqQp5VqS4UesQXLEF!i*7J$P*sW>$!HMvBgJR>tXLm@dQGdWu!F(*C0D6=FZ7w%{!
zn2&IUN={}rQqaHxqqw99rcqfT#NWkVAu%Nd?C+w~;t~|!LL39~N@iZV0w{nI!O5W{
zGdnLoBfm_cC^fw_C$T8A3Y=t>bQECWW0<OI4DlEa$CYGeXXNMQm+68`hdB)+o<Jc2
z!%nUW&W=H@ZlOL3;hrHL3Lzez!Fotm8t9oqY=mim(Ixo`#re6ZB^e-3B&C*=r>5p9
z7$|^Juc3~Dfu5;CQYFmcNNEJ59TEV^i3JKp`APXD>ctA+^rcXqS&{(`G6=UgCo?6r
z2&xx{hrud!Au3V*3&~s{>yR)gVyqOBGg6bY70NRbK{+zBSRpqxF|QcK&(Kgt(g|W>
z4J)vNON)w=i!uvJ6jJh&6^aupixo;TGK&>5ixt#!QWH}YKx?}c5|fitb5e`Ik*cnT
z>OM%2z=9(Sw4kdPMS~tJp%mm7XO?7^r79%n=M|SE=9TCu<R(@q80wkpfCC?zRul~M
zO!V-F9g2H!<!O+=A&$#UtjNqQ%~eRu$;mHIOv*`B$Si^SMj@{>Hz~D9K_jz7p{O*k
zSRt`Q!PLOOqe>GgR1FP40Fsz+#Aa?{g)ZFCVpx1;g8YarxIjgFT4r8ijshf5^79l@
zLB%jQFDDkIDr6)U6jXwf6|y^0b&c@4B0WDbM;B}m%mr9;5F}K}Gg3=3Qj0)Vsu!gy
zloo@s1IS^};vC6hx5S*{RQxtW>IQWkg~Vh~ZDpHSlnxFmJ*WKqoYcfT{{oP3VvZwN
z8dl(d;s}H>Tm&zL%QA~g6LT`FQo(_tke*tQT2hj)qmWcuqTuHr;%Wu1w7?4wL8X6c
z3aFkW$?0$#h<7}eLJ=u6;cm|=EiO^W%qvMvFG?(ddY~XPFF!dsBQu{QH^O!4=I7;9
zQq7^DlvY%lmj{da&>&cYBfp3w_aQ2LGToMzUzD3zQj%H}pPZ9eT#QIQjz#I<YTX6W
zG${5+%_(q$DTX9TPyzzs{4|BK#G=gn(qe_|%)FFhJq6#y>{NwfP;&-Uv1jMym&3}=
zR0U980TpvescHE|kea`!G*3}o2OPQJv|FTL3u@1R*?KUC1b`bc8Y!ueax*hO&sIIC
zG*2NpCm+-{$;r<zP=Gc7K+Z_bO)SYwE>?(PNJ&joNKY*RHB^c<G_4qvl$01Uixmn=
zi!(rNnw%Vk#G?GtJVe#2ke>!FqLcGeQWZ3kQj-%)i$O(~da*)DW=f?ds5PjNlAotu
zqL7@Cn3oQ2tYj7`<R#{&7V9xUatA0wgPK|j`AJ!zCPaQxNn&PRYKlTyQGPDScg2W?
z8Nzi8`Q>?_=2~KIY6+-CoLCI65cBguhJzYq;D%>OKB(MONK_~;1+@)x^7GOalJj$O
z6Z2AvbrirkAUVIFQnw(nxCG?D%wmPK(mZe>050@FjmygX(jtcPjMO}kQQ*V~5>Y59
z%1<v!%+*uyOam2C@N5P3Y@$L+QDSjPX0k$JZhmQA2{gEJ!IcSvV@?h@@QM@jQekc{
zRsbdVjMSo3uo|$TpcXXDz`ROWbpQ$ZT!qA9g+y@c9GnIjN;2|G(;*c-I8sXT71A;x
zF_x>K0V<M;QVVhtlT#Jcb=4sSmU_H8sKK0Dnp2XgTb^G8ZXttBE7sIwK=T<&3N22B
zC+9?HCMZ@&$%h7NdQobD0z(L>t}aik1bY?KT!RLJ0@!((#R|!wvR9A6wJf!$QXw-p
zF+DR6Tmgbu$xz?wfIAG}*eFgdDFGD*;B*NpyA(j7r3X%{49f7PL3)0E3e+8-gixGX
zkO(T_AW0YGoXir1;*9*#oD_wmR2_w!%<NQ!l-vSPc1z1IQb?^x1a&|37?i=qcuKxP
zE-0fW78j=$focg*8ziwrAvv`OlvY8B6cS*Nu1-n5LTW{3aS2E}s8ytpn_rd+s&>FV
z1b97`S*)Ouo0yrWkd~Q~3icc#6if0!(F%12Sb?S<0|PimU@T+T%T3HnOb2Cx%-n+f
zq7nsX$ntid{QLr6h?I^(5U4c><w48=C-VIKoMHv2wt~c>lFY;$1}GO+Kf?H+Oaf!)
zmF5;yg3?}I0Za&90f2-HGV>Tf{r@8j3=9TLkoEtt`TZUI3=EC@3=FdT3=I7I3=Dky
z3=A*$7#Pm+F)$q9V_?|N$H1_gkAb0+kAWe8kAdMUF9X9yUIvCFUIqq7UIqqR=or8&
z9tMV0JPZtFJPZt>JPZsvJPZuqxfvKPa5FG0;AUW`<7Qy+<Yr({;$~p@#l^tzfQx})
zH5UUz3l{@}KNkao1{VXvdrk(1Bb*EjhdCJ-wsJBsRB|#fRB$pdlyfpLq;oPbxNtHs
zuyHaloaSI)*vi4cFqeaYA%lZ~A((@KL6(Do;UhZ(!v=N+hE{e427h)21`&1!hQDkK
z44c^)7}D4n7@XJ`7^K)37(TH=#uFy8GBB91GBC)qGBCViVPII#!oX0(!oXn9!ocv0
znStRRGXujxW(I}>%nS_snHd-sGcz#sGBYsbF*7iDF*7j8Ff%aRWMW|GXJTM*XM%*S
zJ}7)K#uA2}0H}fl)lKox<g8aznx_G&KaqtMY_X(o4RGsT2h?r{ZR`N0d>v4(EY?w|
zt_5L;Im!y<si2l5s7QdcCKK~26+i{QLP}yuqC$R=LQZ~qdS+fasB52>45|)N6;g8x
zN-9C7LGn1b_Jwx1K^8%3Cs5AOKoWJ%$;{S)^ucsM{o!obrjwl1B8X$a+e@H2a#BGx
zd3tIINE<Z!=A;&Bfa*nvYF}_`z!qYnUP@|OW?rg$eqv4ls5=7<CU77q*rMchBqKnE
zDcB;_@C=!03i0uv$~Zn=!PZtmDLx)l9>vEiSt&3;#yy~;KS@QYiP>&BiRqxQ1YvL>
zC@W;7q6Sh*W^xH+m<TzjixbO0MK(Ni!9_Ntft;6`ngSZiC`v6Z09F2=#(-mhr;Y;H
z0#K4DD9X$$(NKc+K5P|~bU@7bvee{~{35*)*r<etCM2AYv?S(%yZRvI`Jmuc(orbL
z%+t%sPdC#Db_I7rH1(j0Ff`|6=A|YU;nxdMgQ2w`AJpLmM}{6KS?T5bW=2D}UjCkb
zA@QCr&>@t}G)Uh-K~(|NgM$o3DTAv5$T%6;^N>aXWK0DVN||Yy$qE{YIncU3Q#}`K
zilKplDpIcuT;V1b6@i*vpneC$nV@J<NXZ9Ff(-{n45$WGfVJMBDH4>jq16SVqEtvv
z%}Xsx%+UkK4#Z2vsd*`ZDH??-5DN-Z6l~GcyM`BXoI%tnLt0)1Ii=}f{}$vVRw|@q
zrlqA8rGi>Yd8x2;l$@ViPzr9Dr7D0s?+Rt9Ir+(%C7GaR9w@%OKv^&|PY<fn3zQ|i
zpu<*>l0h#wKP5E>njgWDqN7j<<H8~Z<}gqq!DbYwagbY@oPp%bl8nSWa9BEPYb$_;
z)Ih_aIjOJ_IC!>EuvG{tDoq6kn}Py3;5C$-^FgC<>EJAsn4FQBT9%rkqfnHZm<(!e
zr=%8xS{mRH4Mio0+d%21KraW>El=0W%P-2+NG&RYW(7?J8))GGDk<W@$s3ZO5Ne>|
zgqi~&ISG>CpxHtJUd|+zBw|axdWp#;o_S@^zy+605Em*dq-7QrgPQx0VIW8c9#ZOp
zhlC*A8gM{>!Z|I!s64SK#XB<(+6{pki7mb$HWb1gm!4V@2rc=P6`&2%RA@&j5!8at
z)JxR^C3dJKNSz(9{l$r8sqvt~KOR(UDcCAh*Mh~7V+}m60PYwe<qJ?D4RI$%(XOMA
zsROQ1;z2oI(~1E)U<qzMg2$~A6|&0{i_$@L4QP}swJfy=Q~)NWD&&HCc{zy{kY+hZ
zF@rMLEJ)>4k`L;(6y!h#UBIc%F~E~SSs^z+1w5o#QmIgxk6La+n{*nWAxpHeUr7C^
z2Ob#B%P)%uDS?ErGORZOsRF>W63}`eGq21$GcPwVAUHEG-LWh`GsQPCFSEcmu>u@&
z%9&}fDmW*#2wby(>lNJElpskIza6ge*}f#257I)c*`B1E?TOE9;*7&>LOf)Q-ZQT(
zJ|i(FjTqz5v=C!9X~yC<5h*RW#%EI$mf*e;e4q?mEr164Ga*GRsPhPRG^ogjrbmp)
z39tg#tPe&D%T_@NLkHAnNQM%XX~0tfNSOwoouI6Rmiv^fz=aBq79V8j1r!`s3gtza
zC6LLkeDGi>d{(6xK7In~MMJx?&~X-Un+h?7keUM;mPu2{ECCH!l%*CcWacFn6cptb
z6lH?O{c;jZQj0*X!IF&BN=P3zwW6dbQK7h`G%XF<kIqdj02u&{jil7{OvtbtcxC|_
zW(?)2kU@~doE&IJKQ|Rryn)8U3rfI+R&io(Dxx*5qW~_YLG3<R)PN%%Bml0%6d?7J
z9)l-nG9ytTDYHbOG(D$M2h1wTF9FTE!AEQobHKX&y%mb{b4o!Ks2+o(LUBnEsGS4r
zUV|!f@H8E0o+TALZ<U{y3UzOBDs(UbDMb{ffYTMYZR4dEqTm2(7eeb@a6H1C4xc1L
zYw}n@ic)B*#WXGtUaM#3m1%e>I4FRu(N@SS&`YgM)$r0Q&PXgsjW&qYQ9!UY6|@vk
Zi*HParC?6_X`txQQ9x~+Sy65F7y$eR-uwUn

literal 0
HcmV?d00001

diff --git a/python/ur_simple_control/clik/.clik_trajectory_following.py.swp b/python/ur_simple_control/clik/.clik_trajectory_following.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..3cc7c72825cf0cd7f429c698dc4ced2dd40f36f5
GIT binary patch
literal 20480
zcmYc?2=nw+u+TGNU|?VnU|^6GjY?gvUc>NCih&_LzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7TVUbAA
z$;{RV5%DEOiCL-1CHX~_@oD)vIr-(8dFgrul@M!3$<YuP4S~{<G+hf`24f>b15jWn
zD=8`n3x$H1qj)p~MnhmU1V%$(Gz3ONU^E0qLtr!nMnhnPgg{9FGebQC0|OJ(zphZ4
z5shYn@@=6sxD&y^zyMRn3>D{v(s584CJ*I~QllX-8UmvsFd71*Aut*OqaiRF0;3@?
z8UmvsFd71*Aut*O0}uj<DGUr(SQ!{t*dg=(u>SvZeg=l4{0t1U_!$^x@-r|L@iQ<)
z@-r~7^D{88@iQ=d=3`*E&BwrSk&l7l7#{<}Q9cHST0RB_Q$7ZUx4aAtFL@aluJJN3
z9OPwS*uu-ekjKlw5W>sAV8YA5zylKFVPFX2VPH_=VPLq<&A`yi&A?F4&A{N#&A_0~
z&A`CR&A{-Pi-F-H7X!mOE(V5dE(QiiE(QiME(V5|oD2*%IT;vka56Al=VV~G%*nv8
zg_D6{DklR&J0}A}6(<8jI41*x9w!5XE++$n8YcsTJSPLg8x97B*BlHC4>=eZu5mCh
zoa10%Sjxe`P|Lx<P|Cr;;K0GapvS?$z|FzHz{J78aEP6Op^2S=!GRqTJ_2kE3=dct
z81}O=FwAFVV8~@<U@&84U|?otV7SD>z_5&kfuWLxfgzrSfkB3afnf(T1H&|C28J|d
z1_mu=1_n-M1_lmh28PQ_3=Er@7#JopF)%bUF)+k4F))}iF))}gF)+MfWMG&B9W+r;
zP*6|?5m<0=h--j?v6Vt`Vp*y}R(@t)i9$hQNd^{`AR%Q2FsV!xzyLN3WFQO|6lLa>
zXehaOx+wVhhbTDv`-Qm%xw|T9qNvPFQ^-`XwN(fT_4D)ebC357aShTnw1TTi%_&Yr
z@IYqg#b=hJ7Ac@=fve9<Q-CS3Rq*ul3;~;nFhN-%tF*X8p*TCUpdd3ZU7;i+RUs|2
zsJKL-s5DQZJhLQ2AwMrwp|~VJFBQ{Ig~fV_1qG>jDH?^*R%WpnUeU<QFTrNNrb1#~
zih@R_f~ta{0m!=sng}O>JXNS*t5A@cr<a*mlA2zWSdyxdo1c=JqoYu$qfn-xrI1pB
zYL{}Ef~|s=UWkH&LTXV_d>MG13p@}&CL$E%73k$;<|XE&>t*JZX?Q6(fHi6><Q3?p
zR;Frr=@n-r7NkZS#Of#@*qRDj3Wf$+TDnG>xb5(Qx;i;Ox1h8n)eDs3yb_c1lQI+Y
zU>-<GEJ*}=!^_{(FC^a61=S-Ur-E=&QEFl~iU&ZVAPh}M$@zI@sYU6jDGHf+N;(Re
zn8tvj9((YkhBZhXJV^5Li*hv(L8z%<qmWuqjH&@44fStMe!7`Puxn~jQGSu89whJ;
zP#h1HRIpV5`Cl*JH!~XM|5&}m<Py)kG7aDK{KOnoZ-A0OT7FS^Vo{2BW?pJ;Vo7Fl
zF<M9>bfo1MDP$^S<|z~<=B1}<K+-)@!U9_Wb#kz)u||-N0;-kD=qmCG^r}*e@{2Vz
zjCG7PH8IqKvP)5_da*)YzCwOcW@=tZVo7Fx9w-gw7b$2|=9em@<SXQ*f?}yeAtyI6
zAL3e&OF>qH91ALuAmLkBtYE7U9SaUP<ovH-t6*qgU;q|}cuD~&FN1~QIUA}Lq8OBm
z@(T14i;5B}HKGmmOf5`w6m$*sEKDtR6m$*rEKQBUd}9Mk9R))@Q*#Tjuz`V@j)H-n
z0hoYhYUL1r7k?{-+{Elug_4ZSVueJ7?DE8-bVw1AsF0UhT9lZhP*RkbSDcn#l&g?f
ztdNqLmROooq6c=N0w}B$()06EK>G926*5wbQWY|b6)N*f)r(RU((;RvQ&SX5^1;&C
z3W-Vir6me!MTxno#d-`d+bfe&6*5cIixrYmOF*SwN@{UtdLBquYDGb6GPHcp%u`6p
zPpO3HR>({NWh7l)JB6V9r2G=!#Jt4x)FOq<yyB9?yyR2`4N&}~rhr@k!l1;akPT05
z3aKT@nhd%MMIgPPcmf#&T2H3{U3!<3S_Jb$X>n#=I)knPNZJ{)FfS*y2&O49CkGq`
zxrynS$qIR;xk;%-#R{dxsVR`KfO;t@70hMORY)vKFSdeuy}T&1Bvm1?xHz?_q!{E(
zkWs~{AT_1Asd**E3<?Thmn9aJq!wo;=0R$@^vtr<JWwhtNi5EWga?BzSjvh)7Y-N_
zi_)Q{Fz7-m%J|}f#N<?@HG`lqhAbNd*{lFD7?gJPpawA%rIwTy<uNcM7o{eaq=Hfi
zq{;=G1WFmu)rVl$XO`$OxPez8f)o}M<(Flqq^2l1qc~p&YBa>53~8l#$)N0&n3Gvj
zi6RA2fJ`!g;{aw_Kw?RTXI@D@xI_kV7?hNh7$Ef-#M$650Vh<DpQ5dF3}Y2+6?6?1
zw7@bD0c}ul7wcuDW~OJ9fE6o4azti{LV9XmDkuklvUZ|ET4_mXQK~{wYEEimaVk_P
zC=r8FcQGj0Cn}_*=H@FD7o;X<re!88WF#hM>p%)kWpIRLq$=d4mMbLZ=NF}9<|US-
zDil{1m!#$@l&6C7OL{)2y3Wi4$A)gDLP};)DmZk&mLYor!PZhhaynGl0OU^uTT7ul
zGo>U0Y>2W#adKh~D8HwI0|Ar-a=|hn_3_3o3bvq1Go!d9F*#d910<+Zr3oqFsvvUE
z(laMD4=k;zW2mXgkdm6FkegUw<N}I`kbGm8fSkm<RIq}0BNrWr3v{4v)wBY~Rel=S
zqZPW93I!n53Qz%1KGy}85(*li@U>DX&qyo*1u&>31CoWLdWFn1SinId%3hNp2%KO+
z>Ce~&;=#m%f};F_qRhmS)SOC)?^ATZZIyzYL{RaRnFlVQU~Qzr(!`Xa#5_n^P*BJR
z6(^Y`3gwwOISPr%C8dcuIh6`YpomNbrEXB;tsGSDG5Gn1xLQHd0xVU7O3d=qoE(sP
zXtb7=WacR37b&C^C4v^~Dx_uRr0OUnm6m`#nOc@w1kOu&`Q<taFl&=i6;eQLHAr9t
zdAfUqDERq@E0lx9)k_pIK#k!<g(6TI*3C%F133&5fS~$GM<F=_RC_9vfPxY1Sdh`h
z8Tq9-DGEuc47v)HkR~Ijk&}}O%IKhwMi>VPZw6h3Do8k_=PM+ZCxQy&{9JGxCgmp<
zr7-9!R4An9rxrsTqM(shl%J~*;v4`fYx9dT(=#E(R(^qQPHI|-LUMjlUTTqoMqVN)
zRKeaZPc1G<Ey`2KNi50CQz%X@$}A{R0F{%OdFh%AiN(dGxdou&q8PcTLzEE=m`M<v
z(x}q>S5{`g+9}CREC7c=L1sm2PBAD$B`SatA2>;aT6gGJnSomFVaUua$S*1Zl_D@^
zeld(ykeQdCoSc!F4@!jvnRzhrywco)N|0Dy0Rw}w0<1$-P+Fo;lv-SxQ=$OQ5y{16
z3L2nvnV3_Y59w}Yf}|7@LA7LAY96R*q>!AGSX`_RZjFGeI8B&|Mpob|6O!gY#RyEo
z5Y*zp9bzCSfg2;Bj8P1AQ8B3WOjIbz%uP*!)Menj&7ce_t00!=rzwEj_zD_+@d2P_
zc(9Iw5y&-_`K3h)sd*(ul?nx*#yhASNmO6}_5ag(85p*MChi3o7(f(g7~&N_1H&SI
z28Jkp1_l9s28IiK3=GTo7#MQ-7#LLeAbtB?ybKIGc^MdX!1RMOj^fb}7!85Z5Eu=C
z(GVC7fzc2c4S~@R7!85Z5Eu;sDu)25J6!}G=7Dqtkp?uu16g?mdZ5X{l8O=yB|ZJT
z)N-%_z2xFDB^?YMItnSNIhnbj={8$+9d#XrwA>O~byYpnG<C?Jm$HIJVopg$erb9J
zWNarTH7zqQ6Fityo?ny=8|=u-F9#1QWfteDmnbA9rszP1RB|&@U<QGPZxleIGdZcC
z{xoQ;FE_s^71W(B&MYoT%`3^N)CbLu<$;DcK;ukc|A0qa!F~YGfK@_9;b0?lB{~X)
z#n1t|5{OhzW?nI9q*ud02Qs)>l2~e>qW~JP11Z%jN(J|nH8h|i-~l{Ps49b$Lk5i%
zG!WAQpvgkeM)P=3|62z%){&<G8Z7`Bt6&Qr1qMrjWQvdnMnOIOY|vO^W-)9mRR=V#
z4jRo;FHtB@%qsy6m!zbErW+OV^Az$+GE$2`Q#~p9kU>dh=m1|qX-Nqvyb={satjpl
zL8D|Ppc#wee9$0RVop(NVoD`s7z{LWRg#$tQVkXbhaq?x20m?4SgZ-||2wcSFa)qd
z=Ko>q`=9bNFs$ZhV94TUV6fz8VEDwx!0-Y(-#?9yfgzrcfgzfYfkBy%f#DS|1H&F(
z28P|d3=DI485ojz85neU85m@E85pE_85o#(85kb&Ffbh9VPKfU!@$tQ!@yw8!@%&K
zn}OjpHv>Z#Hv>aFHv@wlHv_|ME(V54Tnr3tTnr2aTnr5UTnr5QTnr2!IT;u>b22a#
za56C1axyUd=3rph#=*eQ$-%&o&%wYT1)AGuXJA;z&cIO4&cL9=&cMJ3TGPPJz;K$4
zfngFG149fO1A`wM1A_<~1H&s;28Nfc3=G>?85o*bA^!JfWnh@h%D|Ax%D@l;o!i#~
zg$tE)?x=C2Aut*OLoozEwSx*|0$Kq|7w2UbC}`U%B;|u7!KEx|!`cun3ZSYGR7a#L
z<R_IRX6C_0cy+;b7`S$U)_V%%tmc7C2!fml!_dVRL@wF^P1x$?CRW53mFDGT=B3Aj
zCLzJgbHK|2iWPJfh+E1-$RKcVfZPaLy8*TUw1@_*3rv9A0BU%kI{-8l4blc$GDXLA
zKVUb330w{%V?hvjLS7HCET}LA+qxiVn*h{O0<9wg)%4)TKw2qiYA!!5EhjTC6*QTc
zoL^c5ovDR{kb**Cih?b8VgfOjui>QwTHFC$U<7Hb!Ia}&{e$9K2nVzh2D}mo!iBAe
zK{XU+I4#x#fnx+r;EEPRdI7lul-Tes6apIsPV%6&E%D&xK&U2Av~ma%OrTH$uOEVN
zVWCF)Dk6w}X!?P0KxXSG6r#p}vI4j*o|{++TA7iVk_uXwrjQI;%>iB>hq9;yBn;1-
zprrt0uSWuD1_dK%HVZT*1_?-T+Z4J)NEzIRC<m<pNd@&I3W`#TL5oT9^T3l|3gsE8
zc?zH<8K6WDo{lTYS4hlLfXspAB$lKWL42yLkerj6Sd>!<YLqJ^r52ZfS7|8}mlTyI
zgV!;D=ByPoKr3cai@=lX`5K0r3dJRvIXOCD?}FOi(3W~OsCSc*U#^gwm<KkoC>1gh
zoRq4gP@J!jsF0JHmz|oTkds*qHVbS3d|eYb5#dUYV5N`<LGCXkDx~Ekrh{h6i}Q0+
zOEN$n28AD}O`n*Q30f|qUaU}@m<{W2fQx|4Vo0wGTu>H5=b`iR%ORGauAR~~(laoG
zES@qk122{`G65}?GSss)hb*BoGXt-kGBz>?FPj1ru(eYdW*M29>F65jnHX5;80cA=
znu0jyMus5D#K2TX*FewM5NraNfEfc`rKtdNZeor?L4L79Nk%@zu?o821vv`&r6r)w
z7IgUlcupQPRi2v&S)rh;psP@nnwwvis(`Wx32sP1VsSCJ#8XJjNe4~)!WITWRe*AW
zl>(?wTMX&pLTXG%EeT4cumlBC2(t<-s{@{yPgO`%C;_D}Q0zb=A+NY3H8BNjKB$ZV
z84{fdT9X1<N)``V!VzDR9}il<6c1jf09r2yUMv!w2@M&LX-F9j)KvwUqp4{HaUV{j
zP!|IhKwJ-U2B;K6l=?afh4G+Z*8#OK@`}@-j!G=fhPX)63cRKTtO@6mBe0VpoopS*
zYBC*#81RY%kcFU}0S*vQSGriCAhjqBw4ALtHLo}o)bCBrD+VnnO3p6=E%V7sEru=l
z0<AGjPRvsP#cCEvWoC&&QfeA#tRS@_HMtbr6$bZn;m5y$vMgw=SaC^W5m*{L*R7|(
zpbPaUq^wZ@moG5$!OaD5`oOZ-FEb6)RR=9g0xkFjFN0G6t?&Z-Od%;XIkB`jRl!o<
z&;VRCfl_WFXcZr9NCUb;0Ol}HaJc|p!2wzl2wI5(@&PDL70MHf72wOopa~t6HlWK+
rAgkaUp%Il|1X?nhpQiv`T>>%#l$;<+L1Bf^sK=lS)q@_nRtgLN?6bgt

literal 0
HcmV?d00001

diff --git a/python/ur_simple_control/clik/clik_trajectory_following.py b/python/ur_simple_control/clik/clik_trajectory_following.py
index f9469c1..141b90d 100644
--- a/python/ur_simple_control/clik/clik_trajectory_following.py
+++ b/python/ur_simple_control/clik/clik_trajectory_following.py
@@ -41,14 +41,49 @@ def map2DPathTo3DPlane(path_2D, width, height):
     return path
 
 
-# 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)
-def clikCartesianPathIntoJointPath(path, robot, transf_body_to_plane):
-    transf_body_to_plane = pin.SE3(R, p)
+"""
+clikCartesianPathIntoJointPath
+------------------------------
+functionality
+------------
+Follows a provided Cartesian path,
+creates a joint space trajectory for it.
+
+return
+------
+- joint_space_trajectory to follow the given path.
+
+arguments
+----------
+- path:
+  --> cartesian path given in task frame
+TODO: write asserts for these arguments
+- args:
+  --> all the magic numbers used here better be here
+- clikController:
+  --> clik controller you're using
+- robot:
+  --> RobotManager instance (needed for forward kinematics etc)
+TODO: maybe it's better design to expect path in body frame idk
+      the good thing here is you're forced to think about frames.
+TODO: make this a kwarg with a neural transform as default.
+- transf_body_task_frame: 
+  --> A transformation from the body frame to task frame.
+  --> It is assumed that the path was provided in the task frame
+      because 9/10 times that's more convenient,
+      and you can just pass a neural transform if you're not using it.
+- q_init:
+  --> starting point. 
+  --> you can movej to it before executing the trajectory,
+     so this makes perfect sense to ensure correctness
+"""
+def clikCartesianPathIntoJointPath(path, args, robot, \
+        clikController, q_init, transf_body_task_frame):
+    transf_body_to_task_frame = pin.SE3(R, p)
 
     for i in range(len(path)):
-        path[i] = transf_body_to_plane.act(path[i])
+        path[i] = transf_body_to_task_frame.act(path[i])
+    # TODO remove print, write a test for this instead
     print(path)
 
     # TODO: finish this
@@ -58,45 +93,51 @@ def clikCartesianPathIntoJointPath(path, robot, transf_body_to_plane):
     # 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
+    # this is just init_q right now
+    # TODO: make this a flag or something for readability's sake
+    n_iter = args.max_init_clik_iterations
+    # we don't know how many there will be, so a linked list is 
+    # clearly the best data structure here (instert is o(1) still,
+    # and we aren't pressed on time when turning it into an array later)
     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)
+            # TODO maybe hide pin call with RobotManager call
+            pin.forwardKinematics(robot.model, robot.data, q)
+            SEerror = robot.data.oMi[robot.JOINT_ID].actInv(Mgoal)
             err_vector = pin.log6(SEerror).vector 
-            if np.linalg.norm(err_vector) < eps:
-                if not n_iter == INIT_ITER:
+            if np.linalg.norm(err_vector) < args.clik_goal_error:
+                if not n_iter == args.args.max_init_clik_iterations:
                     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):
+            J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+            qd = clikController(J, err_vector)
+            # we're integrating this fully offline of course
+            q = pin.integrate(robot.model, q, qd * dt)
+            if (not n_iter == args.args.max_init_clik_iterations) 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
+        if n_iter == args.args.max_init_clik_iterations:
+            n_iter = args.max_running_clik_iterations
         else:
-            if i == RUNNING_ITER-1:
+            if i == args.max_running_clik_iterations - 1:
                 print("DID NOT CONVERGE")
 
-    #######################################################################
-    #                       STEP 3: save joint path                       #
-    #######################################################################
+    ##############################################
+    #  save the obtained joint-space trajectory  #
+    ##############################################
     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')
+    # we're putting a dmp over this so we already have the timing ready
+    # TODO: make this general, you don't want to depend on other random
+    # arguments (make this one traj_time, then put tau0 = traj_time there
+    t = np.linspace(0, args.tau0, len(qs)).reshape((len(qs),1))
+    joint_trajectory = np.hstack((t, qs))
+    # TODO handle saving more consistently/intentionally
+    # (although this definitely works right now and isn't bad, just mid)
+    np.savetxt("./new_traj.csv", joint_trajectory, delimiter=',', fmt='%.5f')
+    return joint_trajectory
 
 
 
-- 
GitLab