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<XV_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