From 339242c6b9d69260e0b096d4b76d1ebdb4c5fd54 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Sat, 18 Nov 2023 13:30:36 +0100
Subject: [PATCH] commiting to minimize stress over making changes
---
python/{.setup.py.swp => .README.md.swp} | Bin 12288 -> 12288 bytes
python/README.md | 3 +
.../.drawing_from_input_drawing.py.swp | Bin 0 -> 28672 bytes
python/examples/{clik.py => clik_old.py} | 2 +-
python/examples/drawing_from_input_drawing.py | 209 +++++++++++++
python/examples/force_mode_api.py | 40 +++
python/examples/robotiq_gripper.py | 291 ------------------
python/ur_simple_control/__init__.py | 4 +-
.../{util => }/boilerplate_wrapper.py | 0
python/ur_simple_control/clik/.clik.py.swp | Bin 28672 -> 0 bytes
.../.clik_point_to_point.py.swp} | Bin 12288 -> 16384 bytes
.../clik/.clik_trajectory_following.py.swp | Bin 0 -> 16384 bytes
.../clik/{clik.py => clik_point_to_point.py} | 0
.../clik_trajectory_following.py} | 0
.../dmp/{dmp => }/create_dmp.py | 0
.../dmp/{dmp => }/temporal_coupling.py | 0
.../util/.boilerplate_wrapper.py.swp | Bin 32768 -> 0 bytes
.../{dmp/drawing_gen => util}/draw_path.py | 0
.../ur_simple_control/visualize/__init__.py | 0
.../ur_simple_control/visualize/visualize.py | 28 ++
20 files changed, 283 insertions(+), 294 deletions(-)
rename python/{.setup.py.swp => .README.md.swp} (84%)
create mode 100644 python/examples/.drawing_from_input_drawing.py.swp
rename python/examples/{clik.py => clik_old.py} (98%)
create mode 100644 python/examples/drawing_from_input_drawing.py
create mode 100644 python/examples/force_mode_api.py
delete mode 100644 python/examples/robotiq_gripper.py
rename python/ur_simple_control/{util => }/boilerplate_wrapper.py (100%)
delete mode 100644 python/ur_simple_control/clik/.clik.py.swp
rename python/ur_simple_control/{util/.get_model.py.swp => clik/.clik_point_to_point.py.swp} (51%)
create mode 100644 python/ur_simple_control/clik/.clik_trajectory_following.py.swp
rename python/ur_simple_control/clik/{clik.py => clik_point_to_point.py} (100%)
rename python/ur_simple_control/{dmp/drawing_gen/cliking_the_path.py => clik/clik_trajectory_following.py} (100%)
rename python/ur_simple_control/dmp/{dmp => }/create_dmp.py (100%)
rename python/ur_simple_control/dmp/{dmp => }/temporal_coupling.py (100%)
delete mode 100644 python/ur_simple_control/util/.boilerplate_wrapper.py.swp
rename python/ur_simple_control/{dmp/drawing_gen => util}/draw_path.py (100%)
create mode 100644 python/ur_simple_control/visualize/__init__.py
create mode 100644 python/ur_simple_control/visualize/visualize.py
diff --git a/python/.setup.py.swp b/python/.README.md.swp
similarity index 84%
rename from python/.setup.py.swp
rename to python/.README.md.swp
index fc1c48cd886b09bab7b4826d832c3761456cf528..a6c83f7581c07662841f1005cf20654e377c8946 100644
GIT binary patch
literal 12288
zcmYc?2=nw+u+TGNU|?VnU|`7Wj!2yzTEh@FlaV1kzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7T=?A$w
zy7;>4<)%Ok8zo0WU^E1X4}sE>G+hf`24f>b1CS<VB}D~cp->QW6px0$Xb6mkz-S1J
zhQMeDjE2By2#kinXb6mu5GW~NWT<CgU|@p!R{=^hqR~+9C^Z@aqaiRF0;3@?8Umvs
zFd71*Aut*OqaiRF0;3@?8UmvsFa$#&F@=F)9w!5X6&GawAJ+f>!Oy_(j-P>H8b1Sr
zH9rHx4L$~j4n78kEItMXXFdjo=e!IIJ9rrww(~MDtl(u}n9a+;kj~4%pv}v`Ai>MP
z@Slf)VKol}!zvyIhB-V843l^m7_@j87@l!6Fs$KbU<l)8V36l#V7S1=z)-=(z);S`
zz>vztz!1s>b;}S8%TXtfhQMeDjE2By2#kinXb6mk!0--%#G+J%%shpXj8uixibT+g
z&0>YL{G623A_iSlz@V&9RGOEUn3R*MP?C|EmtM@kprDYGnN*Zml$lzrP+FXtqL7hV
zl&YtYS)x#$nUkYXoLK@EQ7FwT$;?qmElVw`1ZxGU$WKd4Eh<*XNGw)JN=?mEC@D(J
zE6zzQNlnpX&{fFH%Ph%E%u#?W8&xRI&nYd*%+FKEELKQNPu0!ONl{47Pf1lM&&W*9
zP{>P7O(|9=$yZ29RVYf$O)Mx#O;O0qE6E3Yj6ngaEVCHQFUZf#D^bW#Q^-gx%gjp$
zIVLeD2NVoY+Y<9C%M&XZ6u_#%))Z&tm*%7>6qkU!m6WOgS=_3SnwMXio&mBrF)b~>
zC<UycurxC{JGHn(p&-AwI1?0H1x5KK`6ZPFnR)3t3<?T~c_|86rNt#6`$1uunyXNr
zUzDwoSPXJ=YF<ieik?C~BvSHAGE$2aG7|GrKsqyv6;kr^QWeTmb8?`T>nK#_mntNK
zotK+gp_`bOqMMsolANKCk_igo)Vvaf<ow)%{Jhk>l41s3h5VxQ#JtR^)D)0?iO>kF
zR7g(D$w^I7z*xAeqmY}J0!kw#`3i}!=qN}`&Q45E)dTxCBflUO6fY$ii6sh|X`sYW
zj7U5T$_go|#mPmP1)%t2U`Q*<&jl+{NXaZpO)kkVs?<}+%P$8T1vM9BT53vW2`H`U
zK;sS+4yo$J3W*BI8HstxsUUBwrzn&s=9MrMWELo7<`tKKVo_JOv^cd$K{r)FPl2H%
zBQ*~$Q=VCpp}+u%Vq*o}Tm>|31?fc~pMpe-Q%gz<O7in_iWSN;Qd4sjl1ejkQW(-Q
zi;7D?7AyFM28Sq=7NjJWq=JmmQ2@tNd1i4cC^ePm=c$)~<J1Zf5W2d$49YOaBtim1
zK_R6y6`Xt&ax;sQb25{&LEZ)BFmP&7$S*BXNGwV(&4or@c~NFbszPExK~a7|Q6|Xt
z#Ny)AqLN~T{Ib*{kVH;qGAP&(x)gN5E>3~CDKW1EWM5KFez`(%er_rxJC+tO=qf<W
z2H99rS&#}!iwb3lMVa}f#R_SqdC4Fb7c(d*KynW#D}mB@IVjS>DKIfl50qmPvr`q4
zL8VGQC?zD8C?usOCzck2)44)^UQQ)ET_!_QO{#)tfI?zQN>OSt*ivvhN=__J1({b`
z49cNkR~EzaL{VyDjzUp>Qho_a#xF@NE=f&M$W6>EP0Y!u)KN$(EdgcJ(j0JTgYtK1
zP;q81h@W4i0LjvDe}i-@q~#YuN-2fJob>#n%#w^;1zkv9Eh^0eMQwhbLNU0QR7goI
zNd&nFY9gp`0%h!i%)I>M<c!RGP$`z44+`u=g@U5gGH^har4|)u=I7}s6epIYDkLQq
uXC^B^#XtoTECr+%r9#{R_BbdzWacS=GJaZSPAVcvF(@m9_`CR9F#rH$Nkr!W
literal 12288
zcmYc?2=nw+u+TGNU|?VnU|?9cIXu-hw1&Y#f{`ITzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7T=@+M#
zlosd}R6@)eB}YSGGz3TpfzpyRT?<|YV<STYkS=8<MFnA@P!MwzkA}c#2#kinXb6mk
zz-S1JhQMeDjE2By2#k;rC@ElMsAphcV1oKr0!lNY(NOLvH5vk=Aut*OqaiRF0;3@?
z8UmvsFd71*Aut*OqaiRF0;3@?1VbP(g@Hkon}J~>H)Q@F*8ji4&%m&SpMha6KLZ0Z
zKLf)fJ_d%_d<+c5d<+cMd<+aKd<+aPc^MdP@iH(R;$>i%$jiVG%*()F#LK|&m4|_0
z7Y_qN4G#lDI1dAZ7!L!(3vLF6x!ep4lc07j1lczP!)w&ZqaiRF0;3@?8UmvsFd71*
zAut*Oqai@u5Kw>uWlaVsOF_9HF*!RiJvBZhu_V#9S{-AJth$v#w0d$*X12bTx{iW+
zN^Sv!QCgCjqpzhNTZ>^-v29vrUP?UF;9`yPjMSo3TlM0iWOYp)gldJPqWtXCJROCk
z(h`NtG=<9iQuUl1h4RF_5`~g{h2rwelH?49oWzpUA_b5MAO#BP`3M6t^NLFnb8_N~
zQVUBni&Beiqt)|Da|<ffV^M5LO-?K=P6g{u$<I?Sf!hGpuTYSgm!F)Rk(sYhmYJwf
zkXeARLLnu!AT=)~H7_|cwOAo9H8lm~w4_vplEm!PJcZ=MqEv<aG=<{Sg2bZ4lGL0^
zB&~@lDGK=|8L34I`FW|u3W<3s3MHw<B{~Yp8L7$H3VJ2^xj71HnK`KnY57G8x%ow@
z49Z|tnF<hhf=y2>Q7FmCELJEkNlXVRKne*Rh3vfi<ZO`7ii%Tn(iHMbOLP=cGE+dF
zEJ{tx$*EK*&o9b`n4hY{pse8MAL42S4QzNgKtsADU!f!;RRQd*l8nr}bVMjtWfsI2
zC#I#^x+Ufmry>#$C=t~nl!3UI2}rLfKPkT?J|(p{xhS)sBr`v+7@VfGkmCuOSfHs8
zDHK6wDkvzx<ui+HtCf_j6qGO$suGe*pvpl>0z)0LwWUQlw(1!rB?ZM+`ugdaB{_*n
zdeF7udO0N-dc~>wxrs&D`MT+ti^tUw9#1SS$;dCVRrdv{Rd6p&N-fIFOGHwD<at|l
z=lq<U)MQXVfYJ`sI)%iX^!%dCl8oG9aD0KHA~eV}RUxq`S0O2}I5kBfKMx#1rA6^Y
zB`K*2nRz9tMQMr2ko1$BpPO5nmzkUhHW!rG5Xl;pzLFDjl1p>Ivc-BxE-OndD$dN$
zvsE|HGgQ}M0I!7C$V<#kwZ)7{bsdJZqWoM1u%eRu{G4Ki%-n+fq7sORjsi-)X8-^o
CxToC!
diff --git a/python/README.md b/python/README.md
index 0368c08..cc31c69 100644
--- a/python/README.md
+++ b/python/README.md
@@ -5,6 +5,9 @@
using things on the real robot needs to be tested manually, but simulation in URsimulator should
take care of that because the only difference is the IP address in that case.
point is test what you can.
+- write out types in various functions
+- write appropriate asserts over applicable arguments - you don't want to blow something up
+ due to a misclick while typing out arguments
# installation
------------
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..777277a1dd36d736b472c96862d313480e100de7
GIT binary patch
literal 28672
zcmYc?2=nw+u+TGNU|?VnU|=}AHX?OeXbnTyOh$(E{NjTAl*A&C6h54rSe%`ihff7W
zt`26Penx(7s*ygNn^T&XqF+#ySdm$*Us@DjoS9pYlNz6#pI1_ppQB$;S(1^Tr=MDp
z2of&VPbo?)&&*4YPb<pLjnB+0C@qPH3h5P8LTnl(M?+vV1W-btv?NW}f|tSA$j|^J
zuB@b}AS@ILVvgd`5Eu=C(GVC7fzc2c4S~@R7!85Z5Eu=C5fTC=1*{D93=9lRQ2&BE
z2Mi1hj8Hy|W`*)4p|n1fhRL%)`5I_6Or8xYt_!7ep)^dM8Ok??(m_xfCJ*I~QllX-
z8UmvsFd71*Aut*OqaiRF0;3@?8UmvsFd71*Aut*OWQ0Is3IhW>D+9x0PzPUtfdSV4
z|H;q5u!Nt1VGch7Ln1!|13y0l!&^QEh7)`Y3?+OF44Qll46k_^7$);FFj(?3Fv##S
zFg)d9U|7q;z>vqoz@W#&!0>{bfnfzV14BJG14Ae`1A{s@1H&yY28ISM28Mbr1_nPa
z1_m)M28Mr}3=FS185mA;GBE7tWMJsvWMJ^*WME+7WME+CWMJ6M!N5?=!N8!+!N4HM
z!N9=C!N9PUoq?f_oq@rMoq^#k8w0~!HU@@xHU<VZHi%!eSQ!}ZurM&pU}0cLVqstq
zVPRmn!_2_Y#LU3p#mvC)l!<|%nTdhHm5G6YkBNcd5+egcKO+M}7$XCNEF%MhFbf02
zR|N$Hg@VMQ;?yF&#FUiy#G>@l+|;}h4Ru}JlFaOk{Ji`!-IT=K0(BjQlFEWq+q9hg
z#1b8a7zU6=bnN7+;OrRW>K5vw5bhb`p%CKX8LX$IqmYuCmROooVr!shs)<!&NxnjH
zer{?>MrK~RLQ-l;d1`8&f`LL}UW$UDj)H-nsX|gE%oH@cGg5O3Y?YD|3lxg-lk!W{
zixr9sQd3hD$}>wc6iPBuq1@t}%#_q3s9qc%25Z!Xs6_QIR_})RyZBovBxj^1XDgIv
zB$g<YWMmdA<fbO(6{Gms7^|PLh85VsrA5WbMVSR93Mu)?3dM<)#Snutixt#!QWH}Y
zKx;@85|fitb5e^EOEUBG)b&u^2MH2baAf6Y=9LtqXwXwAPEO28EK(@QFU~B<EK5~L
z&d)0@Nz5zJQOHfKP%zXp*8vAUG~yHt^i1^dhaHN0@TC=q<8l)#GIL9F6%uoD^2-yG
za#9sCOQ60{$SciFN-a{*$ShGPD$OfaNGwq>H8Aj~(nJbXLjw?i1T2o&%uTG&g&SH7
zi_c7uAF%}&D5<1n<|XDp10_FCA+@L|zeu4nzf>WyC{-aNv7n$5oUD-Dk*W)gUL3AS
z&ri(J1seo&0oEJ@36=7U)RK%;u$AgXsS2gVpzHv0Sb9-rK|yK}lErR`ImM}X{SVcn
zuA`8c3`!-oiACvPN9sA{=jWs*=J^+ZgcEZd!P4k?8pA~j<%xME3MKgpWtqjLi8+~7
zso=m+NKY+DEh)*@QAjE+QSkE*akWy&%P&y?FK_}CC8;S2m8m5pIUQ~Tv5rSCh(LU3
z+O<UrDUdKShPXB*Keaedy#y3YrHMH?l?vtgMcE2TsmY0@#h`SlUJUk7qC!DNWpQS5
zu|jcXZfQ<pNj@k(uq7p=XauL6qSCxPSWJcnA+!_7Qc!C^8Jy@i1edPL3ZSA1QflPq
zr7D!?gS=OiisZS{5`~;pP+^&qnpg(%q(Vk&QK~{(ei66`N-HfXElLF&1`DsW{G!~%
zl9JS-_~e|#;$lQ<a)eeHE|5aL*dsNkzzwDtl0so-lqZ%XLkeYZ;iQm~uaH@yke>&(
zvpgfSBsD2Nu_#4PT?cG6IJFij*ebwNF3hd~aE+yrk_ss)GxPIo)qNAQQxy^wpzGv7
zrWPj`rKaZTFr=iWDWs>CfGW3Q4NWTsX!sQ87lF#`)Z$cd<&%+L&Y(;kz@QBCF*X`j
z#};FgL>5z~ntK>P3kel4)*b34=VWGsiTDCgEfHUm4`G0EFDP`<Q%js7OB8cbi*OkN
z&#iiJ7EFIZPJW47QGTvVW^xHG?WHA|IeMVz$<0qm&4KBJ3ovBn7J!_RoL^80W9KH8
zfK1NGOwuc;1hEtnixmoTN?`JNrMU%_AhEmxm{37xUVd_NMrJ-pxF9nR;+m3Fkl~3r
z@yYq61v#KR3boBIKg3x_A;dW>HODbI*%?$j7A0oprKUjiVTNQ%Zh>BMQEFmIYJ5s=
z0n8UJz5xsj$_j=?dJ4Iq<XHl0AtWl~CT8ZPW#**nC=`^ID5RF97F9x;J|&=LL~?#<
zPKrWOszOO=QC@0_LS|k`zCxlxT4^4*pjJrDDNZd1Wmjl2D@aVvPD}@9Vo-yrBqKFX
zAp@GLLD|0`CowryAv3SI1k`%SPg4K|CD^Y}yJ3bI8tQ=^1g$v~z!_a3Gfx2&MGE;v
z;A{eRs1C?e26_tRMVTe3pmqwl(FksgfWjA4Wv1sVB<AGjr9&JE7D+8p$f!&z%1lwn
z%?DLIx@q}E$>6pJXb~)fvVx_aLSjk^xHQnnht^J-3Mr|{`MHpmN`9U~ej2EhN={Ws
zOHEBlN=(jHD9%>^g&Tvif(1lBs41zCoS%{ks;vq#ld}~{3&4ivCKhF<7Ab%;L!v@K
zQEG8PYI0^;W@?H;PJS}j`3%Ym=8#ZG%!Ael3OV`Zx&?{F#R_SeIVGt@3i)NJ&|(tg
ze%+G%qQcTth2qq_;`|~8Wd$>cepqGz8w-oX;?#n~qQsI^kWr~1kHBkFh%!iM=_+KE
z7MCa#rRL_Br79Gcl%}PDy{KNSkO&gZ1J%U&X$l32#U%<QMVYC^Fnyo`9HCwTl9WL8
z8K_|gE+0``z@V&Pss}O=WKUvFjzVc(X>n=_IPgH_q=EvZjt8|t5Y-W=JO)R&LVlhO
zG!5jJmMAE}+QRTwj6!iqVo^ydBub$M<QFMHT$`9voDYs~P*WqVG$#kz;L1%cE>28O
zEmlZM1i1xVJ!BT^D5O^CDP*RBLNc`|EipN@7~(xp-Iko6mzSDc0x>ryGaHoL6-qJ_
z^NKU_OBGU~%c`N7FgGzhGa1^-EQb4~Sdl?l!9)+#8A{AeEhz%EZxA^Xl2c*z5F}8b
z4UA$?4oOr%4_9M7g|y7{(xOxaP|#%LmxB@`sM(zgPKuyHqAWEhKRL6cQlT7_Wx=Hf
zG+XN0DY*FixhiNR7NvtzNlCIMgR+8=o<d4$PHG9nh*FTBL19%?sgRtX3u<dY(?Nb3
z*p8yqoJ3ITUbi5zBm-I`K~;bX3UC@r0TnGdnR%(YAfgDW3gir3JB2(@N2(+rrR;*0
zHYNF>I#r=4GX+!@=qV(Fnv~Ft25L4z`V*PpWQ`s~;PM1i079!`P*MXIjhUdnPAa&3
zMC1@q;)B)dmHDMbuqwV75)Fw8ps)mY_aNm7tYCl|&H(EFb8s*)eBgkr|A)=LKjdd%
zn8DA$P{Pl^;K0woz{k(Pu%C~CVG$n#!xTOShD1IFh6Fwa21h;y1`$36hVQ%#4Ci?n
z7&h`UFm&=VFm&)TFog0lFi7(<F!1s+F!1m)Fnr`;V7S7=z_6Z&fnfp<14A7T1A{jY
z1A`C`1H(OT28N~F3=H$R85rhqGcXizGcY7@GcY)EGcc%fGcbJOVqiGR#lWzDi-Dn)
zi-94Hi-Ey{i-EzMi-AFei-CcGi-F-ACj-L;P6mbvoD2*JoD2;1oDhG#=U`xX$HBmG
zhXXQ}FoT1EA&G;5;UPN%!zp$KhQ;g*44v!@3?}Rh3^MEt41(+o3?JAS7<RHTFf3$a
zVCZ6FVCZCHV8~%(VDNw~WFTX#WmL;(2#kinXb23;5CHYf5_1YN5*2I}4D<{PjKRHB
zXm1kIz6XooQp12l7KgAhE^$cr9%3Q5Ly-q>#el3Js12$BN@4X6J~@Bn_CTIOA_K@$
z1qEEbMySSZC~g^l7bNR&D8nI)5;0i92^5_Q3MI)3w%9s)8tI9-xry<4`MID$K^+AM
zH&I8SEIv1}LPsGHLZucI$Cr@j03vh|RDwkTSP0}dYWOS}F@OX1u!5~ZUV&bIUTU$1
znWlmkXrvf!A2?vp6@bR>wIE~g@sN>qxO!;(l%y7yfLg%dkxy_d5Y%5O29N)On(sL&
zkfBAey37)UgP{6zQWJ|HBS^WRW^Q6mQEFmJCD;~FPuU90f`<pFW3K@*BQp&$u2+;^
ztcNiWU<FCkMI|ZlU7C8J-k6sLsHb0$U#wS@TAYzskgB0!rlY9|F;N*Z=9UTSrX}U%
zCuf7&(aE5G71$BV3Lq0e{eaB8%#uvVP%~td4Z|KEjpWjzqSU;S_=0@cKrgaLv{9^r
ztpd7;HmF>J6idib3bqPh3yM;cQ!~p_L8C2>$t9(UIU&vg0r|zL&;U^ex9Cd?ARS~-
zvl~1d2X#AWOwkJ5UxRj|K|SltymYWMs0nVX;NlwqH!3GTF-4)IC^0KFxg@`+Qbz&Y
zzexqDL3HdgOCZ5q3pNB4N$M%N1@UF6ImPM<R-mL-m0FZvtO4o=73<|DR>UJl^mP=>
zG&Laug%FK}Wa!B)0EI8<8cDMYG(;7jl9^nhV5?9K_9|rj(^eq}G-u_Tn3tHIS_Jka
zBq2ff1?QxKMn6#o;gA$17Nr*}*n&E&pxHanfEh?cLz5vhO(8x$FEKYYK3>7rRzWE~
z9yH(-AFl*W)S%!>DoRbv20JA)O+h2CKraVW%%$t)<rn3GlXzKbj;4YQC_Nb%7;1uN
zgETS~>_Ag72AYse1<imdpz)VXXm1QO_5vOv0*{Aef@XF=lY1rk1(2AAMQ?N_DE)%+
zQdw#aYF+}nDy6U(s-zH`EHvJrN^%SIFmfc=6qq4}W>^e?IzPFzL?J&d4K&RJO7c12
zF_p|*&=3N6O3WT?lCnZEXh^LrF{d=O7}6!rNL2tuEW}^n`~{6RJ@7=LmquA?PJD81
z3TW^iF<Gahkb+c(p+rMofnH{DUSb|hW3)jmJfuM#_RQSe)RfG`l2p(nMR7@f0cbb`
z5#Z&inMEn!X{uDvu%QM>Mj<ahPd7O~uPn7FJvA>GI%5QN7^JLD&dDr**{GwSYp9?F
ztKz@~IwV0VLq?=B^AwU3i&J$JK$^f~4Dc}w@GwDz0wjRIL58pe%EA%{P#FbVP$yom
zEHy_#TOqqZK}!Lg1`G0w6?7F)BhOG%Q&T|~+S<@kC`l{@Tc)g_ix?3E#Up&c5ZwP4
zVP{~tzz*sE!`AN~;%8vU<7Z$9;b&m5;b&mr;Adbs!^gldpO1kdmXCo!gO7pX2`>Y~
zYF-A0E?x$POkM^C4_*d_4?GMEOL!O<%6J$U40sqA#CaGPUUD-qEaqll=;3Byh~Z{n
z;N)gt*w4klu!4($A)kwZL4k{b;RPoH!+A~yhB=%J4DOr^40@al41YKn81`{6Ff8U^
zU?}2XU@+leV36d1xb+J=1H&4228I-9pIsWd4&X5x1H&aY28La13=Dm23=EZQ3=H9H
z3=B+c3=FGS85pWr85rzYA!{B!vM?|lWMN=f#KOQ(#=^j0$-=-O&BDO&9kf1xnStRl
zGXukZW(J0-%nS@+%nS^&%nS_Qm>3u~GchpqGchoPFflNQGeI^_+-GE9IKs%l&`ZgN
z1yI=oi>tK!qVmKd$lxG69f1-$DE=U0IiR{6R>vrVM!5<~OW-5M3OR}8kdmk*u~fkp
zloq0`%wmy~nIW`hhA8mT3sG=TC@)IQOU?kNnv_CF9!M-IO03iX=V6RGGB8C$Qxlt+
zUZBD^GfxjRI$c_l>IIrX@k&h2Ps&Wp(*RFq=qRKlmL%#Z6zV8=`Fr|>#Cy8HvJ<F{
z0Iq^_DnSb#N>Yn-6v{L56|(d4%ZtHoQ&z|XP0$q-<tHU3L1raEIS(|;nwwae1evq~
zm$Laq3TdUqnfZCTpd}ihHVCv<Rz}pc(B?ofXs{9#ypUA^dQh7{b-O}NettG+bTwZg
zGzdHbte;qr2`=hVi%T+d6H7p?8Ia!M#N1S<#^l7DWYAn_ssh+R1;|)9%Fr!%m^(QY
zZI~RC4hk~!^q^_LJ2Nj8WJ7W>TF61d3niKgp<;T81qG>jDH;ZP1~^5`kmDCL@1d8H
znpyy2XcR(|hB9czqNF@up`a+U80-d!UtqC-m^lK+sX}J4f`x8MewqSehFGBlH1h))
zyM#`jfWrrp(x6T#OD#%FPt|~k#V3}f>ws5s*g_qT5RcAOP*pGmRgVxcuu+i8%1WW6
zs8S&%9~2Xb81oX)DITc$qWm22j8-DVVz4O0K&XT8r1=o%05{Owh6X&0Kq<ruKGOp(
z`a#WYXrw_#V)LP+$e?@*8I1<Tva&*H9%$(ecr}B5UVe!#n56&>C{X(aG~k|E1PgG`
zToJStuaK5snwO%Gn5W<v;0a2);4BKQX2D4l)M3zpR8F9|w9FEaDroaMvkE-=tN`{p
zBmlwf7|^so*azjQnduoNpn1Lkuta8FY7w|yo|{-wlvx2D|1Ji#*Fm{06+Tl9ZoGjF
z1*J={lQ2sSXhRs(!YNKIi7!bk)hJ0Ug(Or+X#|o2v9uLRlJ!apQW8s2LA4&ZfuNwJ
zkOJveDl5SHCZOpK@FXSJYLM>YlGFkXSaU)dRE2`)x}o9Ylb>G@UX%&Swi;k3>wuQ|
zfTr4VK&4HlrWFI#CYbR~cthO-DqspgDFP=ZZo(U+9TuA@pqaaJ&_oz)b`G31@=_q9
z4v-Qwu?U*L;KL-KNX-WaBgilihEC?#g2#0ru>=wZVbJuHtrDa@g3T=`6cpuyrlm9U
zK`Vxm^HbB(GLth?^FZrdAWK3Zix)5!)qzJz@Gb>Hc8MW2mtdp^g_6{wTpa~SXBjkc
zT%4PjlcNM%(SyS!sRhNlCCNlO1#R6PBuF4lETq+AkQ`YGS?mIxT8F1v9fgAYlGHqK
z{sK9eko(d><1f02L^;q9Rt13K3xq+{R2sM?pOLCilnR<jR{+numgFlyhjc*A1-KhP
zdO(;Qk6D4sl8nssjMO3>ypa#_64ZX8JcbtTASWt-SEMB;<|!1HWai{3q~(Lw(1G0m
zp65#~OU(my48Uo>BtKsP6b?8%T9RLqn4?ev>g|B$`chLMF$nLqgC|@;<uPa_6h^wk
z@7a>XQUjv=XoM}1!}3>R4y@1u(clgl*o~012V0~ET5yRs<g@WFV1?y5BlL6xO%Kq_
zP9MiW>_lEf23vWDH5QOo)`b+6LVI{P@;odGbz$pWh+SX@D@*XLj{=vc$_k)r2sGda
zT3C^cRN^W?#<K~U07>Htu;sqC>V|rtbOa6+b<oPfL}<G?9=uc`u_RX=l7Mjd8Cv_~
zq!#Id`{lZzN%+jXbew5ZSpm|%2Bmvg$b<AiX5FF2DHLa>=OyNV{S0$)az=h;a%!<{
zv^r?TbZSaKacU{3tyY#=RGg}=qoAGz?Hz;^CFT`_#`V-=Ax?*x0858B41gqYm|}2<
zC>5vXq$Yy~P&1P=K;81>Y=y*}^!%dCl8jt%NdfW#Jo^(2BUq>umlVO{95UYkTbWao
zS`2BKz}x|Iv|*|)T2>ALjk;$RD}W;kyvQLvH4nVZH!mGrU_m<#@bU&R@(7*q2dy5*
z=4(&@gKBzEyd)NZ+I%IM*?IXH`DF@4sp+LTiAB)HpI!{O{|{Pc-@*y$|HJn8ui<B4
zNaJT<;N@pvc*V!SFq@BoA)JqaL4uEg;S?_eLpv`6gFP<;0|zey!%iLshDII+26Y|=
zhWFeI3`@8f7;3l~7!<h~7+!HPFs$KXU?}BcV6cSF(eLGCU;x=y3$k-qPKJ$oYBU5!
zLtr!nMnhmU1V%$(Gz4&j0H~2zkXQtzKogat+YCYb6Y*>_gp6%jDS#I|fCe%$^GZ^S
zAe*>A%ilpmp5Wn1kQ`_&I%t7GN@_0ZfEHxH7Pcar=-r7l+?GgaXCWw!fH0(A10Jzd
zFNTiA6TN*A(pkXK%^6~w6j26MA)x|_a&mVmf{nuwGI+KrDnnMu6cprSCWCisg0`MO
zmgImokAZq&kWqJ#+d#9!pgmnlpgkL@#R`dVZ74luh-Z+7I0$WbBsj7GolPKhJOh;8
zkTGa#fQ0>vFs&H=A!@TE-n0taJ^~szfaHC|mOC69I8lNa<}_IEd|<ahf(9W#6Di<5
z2cYsS9XuKfUbPROV1sUB#*swf%5?Mdaw>_BXmJ1k5IY0IB<T8o*!g__`571<@G~%M
z=VxF@<!4~v;%8v^#mB(#laGO6BOe1pCLaTXA|C^T0v`jzCte1I7rYD%*LfKj_CwDE
zsN-c|h~Z^mVCRLb_ix}~U~u7KVBq0lVED$(z;KM4fnhl}14A`81A`qm1A_uL1H&IK
z$aujGE(V4pTnr4exEL6sxfmFfxEL6Ib22dO;ACK!!pXo8$;rSV!^yzF%E`d+goA<M
zBnQ;}II7uE;n5Hn4S~@R7!85Z5Eu=C!5#vjw%O<+IAzdMD%fP+=pwkHl+i_Sko9Sx
z#c`vH;7S!n7r~7#g2T65f`A-!V*+IH6KHD)#`pke#0GjQLuP&+=$IxQh2s38qDloT
zeFZ&*$ox_T&^jaVVIrVTw}K&P*1kAZAuT5{y;z|Lw0cq@FTWhR00UI56(wdCrz-g6
zmw1A1vIDIGO-*qHouj3p?vj}T9!kPI76G(!6XX`?AP#6cA2K$QnFd-wQmhA)04)v&
f`9u%lYRCd`kRWI|Huzj1J@5!ZJV;DKlYs#Mvg$4*
literal 0
HcmV?d00001
diff --git a/python/examples/clik.py b/python/examples/clik_old.py
similarity index 98%
rename from python/examples/clik.py
rename to python/examples/clik_old.py
index e537b87..eab2cce 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik_old.py
@@ -9,7 +9,7 @@ from pinocchio.visualize import GepettoVisualizer
from rtde_control import RTDEControlInterface as RTDEControl
from rtde_receive import RTDEReceiveInterface as RTDEReceive
from rtde_io import RTDEIOInterface
-from robotiq_gripper import RobotiqGripper
+from ur_simple_control.util.robotiq_gripper import RobotiqGripper
import os
import copy
import signal
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
new file mode 100644
index 0000000..f1b6610
--- /dev/null
+++ b/python/examples/drawing_from_input_drawing.py
@@ -0,0 +1,209 @@
+# TODO
+# this is the main file for the drawing your drawings with a dmp with force feedback
+# TODO:
+# 1. clean up these instantiations in a separate util file
+# and just do a line-liner importing -> need to make this a package to get rid
+# of the relative-path imports
+# 2. delete the unnecessary comments
+# 3. figure out how to scale the dmp velocity when creating it -> DONE (arg for tc)
+# 4. parametrize everything, put argparse with defaults in a separate file
+# like in tianshou examples. make magic numbers arguments!
+# also add helpfull error messages based on this, ex. if interfaces couldn't connect
+# with the simulation argument on, write out "check whether you started the simulator"
+# 5. remove all unused code:
+# - for stuff that could work, make a separate file
+# - just remove stuff that's a remnant of past tries
+# 6. put visualization into a separate file for visualization
+# 7. put an actual low-pass filter over the force-torque sensor
+# 8. add some code to pick up the marker from a prespecified location
+# 9. add the (optional) decomposition of force feedback so that
+# you get hybrid motion-force control
+# 10. write documentation as you go along
+# 11. put gripper class in util or something,
+# package it and then have it one place instead of copying it everywhere
+# 12. make this a mainfile, put everything that could be turned into a function elsewhere
+
+from ur_simple_control.dmp.create_dmp import DMP
+from temporal_coupling import NoTC, TCVelAccConstrained
+import pinocchio as pin
+import numpy as np
+import matplotlib.pyplot as plt
+import copy
+from ur_simple_control.util.get_model import get_model
+from ur_simple_control.visualize.visualize import plotFromDict
+from ur_simple_control.clik.clik_point_to_point import getController
+
+#######################################################################
+# arguments #
+#######################################################################
+# TODO sort these somehow
+def get_args():
+ parser = argparse.ArgumentParser(description='Make a drawing on screen,
+ watch the robot do it on the whiteboard.',
+ formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+ # TODO this one won't really work but let's leave it here for the future
+ parser.add_argument('--simulation', action=argparse.BooleanOptionalAction,
+ help="whether you are running the UR simulator. \
+ NOTE: doesn't actually work because it's not a physics simulator", \
+ default=False)
+ parser.add_argument('--pinocchio-only', action=argparse.BooleanOptionalAction,
+ help="whether you want to just integrate with pinocchio.\
+ NOTE: doesn't actually work because it's not a physics simulator", \
+ default=False)
+ parser.add_argument('--visualize', action=argparse.BooleanOptionalAction,
+ help="whether you want to visualize with gepetto, but \
+ NOTE: not implemented yet", default=False)
+ parser.add_argument('--gripper', action=argparse.BooleanOptionalAction, \
+ help="whether you're using the gripper", default=True)
+# not applicable here, but leaving it in the case it becomes applicable
+# parser.add_argument('--goal-error', type=float, \
+# help="the final position error you are happy with", default=1e-3)
+ parser.add_argument('--max-iterations', type=int, \
+ help="maximum allowable iteration number (it runs at 500Hz)", default=100000)
+ parser.add_argument('--acceleration', type=float, \
+ help="robot's joints acceleration. scalar positive constant, \
+ max 1.7, and default 0.4. \
+ BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
+ TODO: check what this means", default=0.3)
+ # TODO: test the interaction of this and the overall demo
+ parser.add_argument('--speed-slider', type=float,\
+ help="cap robot's speed with the speed slider \
+ to something between 0 and 1, 0.5 by default \
+ BE CAREFUL WITH THIS.", default=0.5)
+ parser.add_argument('--tikhonov-damp', type=float, \
+ help="damping scalar in tiknohov regularization.\
+ This is used when generating the joint trajectory from the drawing.", \
+ default=1e-2)
+ # TODO add the rest
+ parser.add_argument('--controller', type=str, \
+ help="select which click algorithm you want", \
+ default='dampedPseudoinverse', \
+ choices=['dampedPseudoinverse', 'jacobianTranspose'])
+ # maybe you want to scale the control signal
+ parser.add_argument('--controller-speed-scaling', type=float, \
+ default='1.0', help='not actually_used atm')
+ ############################
+ # dmp specific arguments #
+ ############################
+ parser.add_argument('--temporal-coupling', action=argparse.BooleanOptionalAction, \
+ help="whether you want to use temporal coupling", default=True)
+ parser.add_argument('--kp', type=float, \
+ help="proportial control constant for position errors", \
+ default=2.0)
+ parser.add_argument('--kp', type=float, \
+ help="proportial control constant for position errors", \
+ default=2)
+ parser.add_argument('--tau0', type=float, \
+ help="total time needed for trajectory. if you use temporal coupling,\
+ you can still follow the path even if it's too fast", \
+ default=5)
+ parser.add_argument('--gamma-nominal', type=float, \
+ help="positive constant for tuning temporal coupling: the higher,\
+ the fast the return rate to nominal tau", \
+ default=1.0)
+ parser.add_argument('--gamma-a', type=float, \
+ help="positive constant for tuning temporal coupling, potential term", \
+ default=0.5)
+ parser.add_argument('--eps-tc', type=float, \
+ help="temporal coupling term, should be small", \
+ default=0.001)
+ parser.add_argument('--alpha', type=float, \
+ help="force feedback proportional coefficient", \
+ default=0.003)
+ # TODO add low pass filtering and make it's parameters arguments too
+
+ args = parser.parse_args()
+ if args.gripper and args.simulation:
+ raise NotImplementedError('Did not figure out how to put the gripper in \
+ the simulation yet, sorry :/ . You can have only 1 these flags right now')
+ return args
+
+#######################################################################
+# control loop #
+#######################################################################
+
+def controlLoopWriting(robot, controller, i):
+ # dmp
+ dmp.step(dt)
+ # temporal coupling
+ tau = dmp.tau + tc.update(dmp, dt) * dt
+ dmp.set_tau(tau)
+ q = rtde_receive.getActualQ()
+ # TODO and NOTE the weight, TCP and inertial matrix needs to be set on the robot
+ # you already found an API in rtde_control for this, just put it in initialization
+ # under using/not-using gripper parameters
+ # TODO: document the mathematics with one-line comments
+ wrench = np.array(rtde_receive.getActualTCPForce())
+ # rolling average
+ # TODO: try doing a low-pass filter instead
+ wrench_avg[i % 5] = wrench
+ wrench = np.average(wrench_avg, axis=0)
+ # pinocchio model is 8-dof because it treats the gripper
+ # as two prismatic joints
+ q6 = np.array(copy.deepcopy(q))
+ q.append(0.0)
+ q.append(0.0)
+ q = np.array(q)
+ pin.forwardKinematics(model, data, q)
+ # calculate joint torques based on the force-torque sensor
+ # TODO look into UR code/api for estimating the same
+ # based on currents in the joints.
+ # it's probably worse, but maybe some sensor fusion-type thing
+ # is actually better, who knows
+ J = pin.computeJointJacobian(model, data, q, JOINT_ID)
+ dq = np.array(rtde_receive.getActualQd()).reshape((6,1))
+ tau = J.T @ wrench
+ tau = tau[:6].reshape((6,1))
+ # compute control law:
+ # - feedforward the velocity and the force reading
+ # - feedback the position
+ vel_cmd = dmp.vel + kp * (dmp.pos - q6.reshape((6,1))) - alpha * tau
+ vel_cmd = vel_cmd.reshape((6,))
+ # just in case, clip the velocity to max values
+ vel_cmd = np.clip(vel_cmd, -1 * v_max, v_max)
+ # and immediatelly stop if something weird happened (some non-convergence)
+ if np.isnan(vel_cmd[0]):
+ break
+ rtde_control.speedJ(vel_cmd, acceleration, dt)
+
+ # save values for the plot
+ # TODO cut off at the last time instant?
+ qs[i] = q6.reshape((6,))
+ dmp_poss[i] = dmp.pos.reshape((6,))
+ dqs[i] = dq.reshape((6,))
+ dmp_vels[i] = dmp.vel.reshape((6,))
+
+ # TODO do this with something sensible to stop
+ if (np.linalg.norm(dmp.vel) < 0.0001) and (i > 5000):
+ break
+
+if __name__ == "__main__":
+ args = parser.get_args()
+ controller = getController(args)
+ robot = RobotManager(args)
+ plot_dict = {
+ 'qs' : np.zeros((args.max_iterations, 6)),
+ 'dmp_poss' : np.zeros((args.max_iterations, 6)),
+ 'dqs' : np.zeros((args.max_iterations, 6)),
+ 'dmp_vels' : np.zeros((args.max_iterations, 6)),
+ }
+
+ # load trajectory, create DMP based on it
+ dmp = DMP()
+ # TODO: add marker picking
+ # get up from the board
+ current_pose = rtde_receive.getActualTCPPose()
+ current_pose[2] = current_pose[2] + 0.03
+ rtde_control.moveL(current_pose)
+ # move to initial pose
+ # this is a blocking call
+ rtde_control.moveJ(dmp.pos.reshape((6,)))
+
+ if not args.temporal_coupling:
+ tc = NoTC()
+ else:
+ # TODO learn the math already
+ # TODO test whether this works (it should, but test it)
+ v_max = np.ones(6) * 0.5 * speed_slider
+ a_max = np.ones(6) * 1.7
+ tc = TCVelAccConstrained(gamma_nominal, gamma_a, v_max, a_max, eps_tc)
diff --git a/python/examples/force_mode_api.py b/python/examples/force_mode_api.py
new file mode 100644
index 0000000..3bb59da
--- /dev/null
+++ b/python/examples/force_mode_api.py
@@ -0,0 +1,40 @@
+# this is just random code which needs to be put into
+# a concrete example.
+# but the chunks and some comments make sense,
+# so they're kept here for future use
+
+#######################################################################
+# some force_mode from ur api chucks #
+#######################################################################
+
+# ========================================================================
+# TODO: either write a separate file where this is used
+# ---> TODO write a separate file where you're testing this out
+# or just delete it
+task_frame = [0, 0, 0, 0, 0, 0]
+# these are in {0,1} and select which task frame direction compliance is active in
+# just be soft everywhere
+selection_vector = [1, 1, 1, 1, 1, 1]
+# the wrench applied to the environment:
+# position is adjusted to achieve the specified wrench
+# let's pretend speedjs are this and see what happens (idk honestly)
+wrench = [0, 0, 0, 0, 0, 0]
+ftype = 2
+# limits for:
+# - compliant axes: highest tcp velocities allowed on compliant axes
+# - non-compliant axes: maximum tcp position error compared to the program (which prg,
+# and where is this set?)
+# why these values?
+limits = [2, 2, 2, 2, 2, 2]
+wrench_avg = np.zeros((5,6))
+
+# TODO: move to existing force mode API testing file
+# this is very stupind, but it was quick to implement
+vel_cmd8 = list(vel_cmd)
+vel_cmd8.append(0.0)
+vel_cmd8.append(0.0)
+vel_cmd8 = np.array(vel_cmd8)
+vel_tcp = J @ vel_cmd8
+vel_tcp = vel_tcp * 10
+rtde_control.forceMode(task_frame, selection_vector, vel_tcp, ftype, limits)
+# ========================================================================
diff --git a/python/examples/robotiq_gripper.py b/python/examples/robotiq_gripper.py
deleted file mode 100644
index a17ab18..0000000
--- a/python/examples/robotiq_gripper.py
+++ /dev/null
@@ -1,291 +0,0 @@
-"""Module to control Robotiq's grippers - tested with HAND-E"""
-
-import socket
-import threading
-import time
-from enum import Enum
-from typing import Union, Tuple, OrderedDict
-
-class RobotiqGripper:
- """
- Communicates with the gripper directly, via socket with string commands, leveraging string names for variables.
- """
- # WRITE VARIABLES (CAN ALSO READ)
- ACT = 'ACT' # act : activate (1 while activated, can be reset to clear fault status)
- GTO = 'GTO' # gto : go to (will perform go to with the actions set in pos, for, spe)
- ATR = 'ATR' # atr : auto-release (emergency slow move)
- ADR = 'ADR' # adr : auto-release direction (open(1) or close(0) during auto-release)
- FOR = 'FOR' # for : force (0-255)
- SPE = 'SPE' # spe : speed (0-255)
- POS = 'POS' # pos : position (0-255), 0 = open
- # READ VARIABLES
- STA = 'STA' # status (0 = is reset, 1 = activating, 3 = active)
- PRE = 'PRE' # position request (echo of last commanded position)
- OBJ = 'OBJ' # object detection (0 = moving, 1 = outer grip, 2 = inner grip, 3 = no object at rest)
- FLT = 'FLT' # fault (0=ok, see manual for errors if not zero)
-
- ENCODING = 'UTF-8' # ASCII and UTF-8 both seem to work
-
- class GripperStatus(Enum):
- """Gripper status reported by the gripper. The integer values have to match what the gripper sends."""
- RESET = 0
- ACTIVATING = 1
- # UNUSED = 2 # This value is currently not used by the gripper firmware
- ACTIVE = 3
-
- class ObjectStatus(Enum):
- """Object status reported by the gripper. The integer values have to match what the gripper sends."""
- MOVING = 0
- STOPPED_OUTER_OBJECT = 1
- STOPPED_INNER_OBJECT = 2
- AT_DEST = 3
-
- def __init__(self):
- """Constructor."""
- self.socket = None
- self.command_lock = threading.Lock()
- self._min_position = 0
- self._max_position = 255
- self._min_speed = 0
- self._max_speed = 255
- self._min_force = 0
- self._max_force = 255
-
- def connect(self, hostname: str, port: int, socket_timeout: float = 2.0) -> None:
- """Connects to a gripper at the given address.
- :param hostname: Hostname or ip.
- :param port: Port.
- :param socket_timeout: Timeout for blocking socket operations.
- """
- self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
- self.socket.connect((hostname, port))
- self.socket.settimeout(socket_timeout)
-
- def disconnect(self) -> None:
- """Closes the connection with the gripper."""
- self.socket.close()
-
- def _set_vars(self, var_dict: OrderedDict[str, Union[int, float]]):
- """Sends the appropriate command via socket to set the value of n variables, and waits for its 'ack' response.
- :param var_dict: Dictionary of variables to set (variable_name, value).
- :return: True on successful reception of ack, false if no ack was received, indicating the set may not
- have been effective.
- """
- # construct unique command
- cmd = "SET"
- for variable, value in var_dict.items():
- cmd += f" {variable} {str(value)}"
- cmd += '\n' # new line is required for the command to finish
- # atomic commands send/rcv
- with self.command_lock:
- self.socket.sendall(cmd.encode(self.ENCODING))
- data = self.socket.recv(1024)
- return self._is_ack(data)
-
- def _set_var(self, variable: str, value: Union[int, float]):
- """Sends the appropriate command via socket to set the value of a variable, and waits for its 'ack' response.
- :param variable: Variable to set.
- :param value: Value to set for the variable.
- :return: True on successful reception of ack, false if no ack was received, indicating the set may not
- have been effective.
- """
- return self._set_vars(OrderedDict([(variable, value)]))
-
- def _get_var(self, variable: str):
- """Sends the appropriate command to retrieve the value of a variable from the gripper, blocking until the
- response is received or the socket times out.
- :param variable: Name of the variable to retrieve.
- :return: Value of the variable as integer.
- """
- # atomic commands send/rcv
- with self.command_lock:
- cmd = f"GET {variable}\n"
- self.socket.sendall(cmd.encode(self.ENCODING))
- data = self.socket.recv(1024)
-
- # expect data of the form 'VAR x', where VAR is an echo of the variable name, and X the value
- # note some special variables (like FLT) may send 2 bytes, instead of an integer. We assume integer here
- var_name, value_str = data.decode(self.ENCODING).split()
- if var_name != variable:
- raise ValueError(f"Unexpected response {data} ({data.decode(self.ENCODING)}): does not match '{variable}'")
- value = int(value_str)
- return value
-
- @staticmethod
- def _is_ack(data: str):
- return data == b'ack'
-
- def _reset(self):
- """
- Reset the gripper.
- The following code is executed in the corresponding script function
- def rq_reset(gripper_socket="1"):
- rq_set_var("ACT", 0, gripper_socket)
- rq_set_var("ATR", 0, gripper_socket)
-
- while(not rq_get_var("ACT", 1, gripper_socket) == 0 or not rq_get_var("STA", 1, gripper_socket) == 0):
- rq_set_var("ACT", 0, gripper_socket)
- rq_set_var("ATR", 0, gripper_socket)
- sync()
- end
-
- sleep(0.5)
- end
- """
- self._set_var(self.ACT, 0)
- self._set_var(self.ATR, 0)
- while (not self._get_var(self.ACT) == 0 or not self._get_var(self.STA) == 0):
- self._set_var(self.ACT, 0)
- self._set_var(self.ATR, 0)
- time.sleep(0.5)
-
-
- def activate(self, auto_calibrate: bool = True):
- """Resets the activation flag in the gripper, and sets it back to one, clearing previous fault flags.
- :param auto_calibrate: Whether to calibrate the minimum and maximum positions based on actual motion.
- The following code is executed in the corresponding script function
- def rq_activate(gripper_socket="1"):
- if (not rq_is_gripper_activated(gripper_socket)):
- rq_reset(gripper_socket)
-
- while(not rq_get_var("ACT", 1, gripper_socket) == 0 or not rq_get_var("STA", 1, gripper_socket) == 0):
- rq_reset(gripper_socket)
- sync()
- end
-
- rq_set_var("ACT",1, gripper_socket)
- end
- end
- def rq_activate_and_wait(gripper_socket="1"):
- if (not rq_is_gripper_activated(gripper_socket)):
- rq_activate(gripper_socket)
- sleep(1.0)
-
- while(not rq_get_var("ACT", 1, gripper_socket) == 1 or not rq_get_var("STA", 1, gripper_socket) == 3):
- sleep(0.1)
- end
-
- sleep(0.5)
- end
- end
- """
- if not self.is_active():
- self._reset()
- while (not self._get_var(self.ACT) == 0 or not self._get_var(self.STA) == 0):
- time.sleep(0.01)
-
- self._set_var(self.ACT, 1)
- time.sleep(1.0)
- while (not self._get_var(self.ACT) == 1 or not self._get_var(self.STA) == 3):
- time.sleep(0.01)
-
- # auto-calibrate position range if desired
- if auto_calibrate:
- self.auto_calibrate()
-
- def is_active(self):
- """Returns whether the gripper is active."""
- status = self._get_var(self.STA)
- return RobotiqGripper.GripperStatus(status) == RobotiqGripper.GripperStatus.ACTIVE
-
- def get_min_position(self) -> int:
- """Returns the minimum position the gripper can reach (open position)."""
- return self._min_position
-
- def get_max_position(self) -> int:
- """Returns the maximum position the gripper can reach (closed position)."""
- return self._max_position
-
- def get_open_position(self) -> int:
- """Returns what is considered the open position for gripper (minimum position value)."""
- return self.get_min_position()
-
- def get_closed_position(self) -> int:
- """Returns what is considered the closed position for gripper (maximum position value)."""
- return self.get_max_position()
-
- def is_open(self):
- """Returns whether the current position is considered as being fully open."""
- return self.get_current_position() <= self.get_open_position()
-
- def is_closed(self):
- """Returns whether the current position is considered as being fully closed."""
- return self.get_current_position() >= self.get_closed_position()
-
- def get_current_position(self) -> int:
- """Returns the current position as returned by the physical hardware."""
- return self._get_var(self.POS)
-
- def auto_calibrate(self, log: bool = True) -> None:
- """Attempts to calibrate the open and closed positions, by slowly closing and opening the gripper.
- :param log: Whether to print the results to log.
- """
- # first try to open in case we are holding an object
- (position, status) = self.move_and_wait_for_pos(self.get_open_position(), 64, 1)
- if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
- raise RuntimeError(f"Calibration failed opening to start: {str(status)}")
-
- # try to close as far as possible, and record the number
- (position, status) = self.move_and_wait_for_pos(self.get_closed_position(), 64, 1)
- if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
- raise RuntimeError(f"Calibration failed because of an object: {str(status)}")
- assert position <= self._max_position
- self._max_position = position
-
- # try to open as far as possible, and record the number
- (position, status) = self.move_and_wait_for_pos(self.get_open_position(), 64, 1)
- if RobotiqGripper.ObjectStatus(status) != RobotiqGripper.ObjectStatus.AT_DEST:
- raise RuntimeError(f"Calibration failed because of an object: {str(status)}")
- assert position >= self._min_position
- self._min_position = position
-
- if log:
- print(f"Gripper auto-calibrated to [{self.get_min_position()}, {self.get_max_position()}]")
-
- def move(self, position: int, speed: int, force: int) -> Tuple[bool, int]:
- """Sends commands to start moving towards the given position, with the specified speed and force.
- :param position: Position to move to [min_position, max_position]
- :param speed: Speed to move at [min_speed, max_speed]
- :param force: Force to use [min_force, max_force]
- :return: A tuple with a bool indicating whether the action it was successfully sent, and an integer with
- the actual position that was requested, after being adjusted to the min/max calibrated range.
- """
-
- def clip_val(min_val, val, max_val):
- return max(min_val, min(val, max_val))
-
- clip_pos = clip_val(self._min_position, position, self._max_position)
- clip_spe = clip_val(self._min_speed, speed, self._max_speed)
- clip_for = clip_val(self._min_force, force, self._max_force)
-
- # moves to the given position with the given speed and force
- var_dict = OrderedDict([(self.POS, clip_pos), (self.SPE, clip_spe), (self.FOR, clip_for), (self.GTO, 1)])
- return self._set_vars(var_dict), clip_pos
-
- def move_and_wait_for_pos(self, position: int, speed: int, force: int) -> Tuple[int, ObjectStatus]: # noqa
- """Sends commands to start moving towards the given position, with the specified speed and force, and
- then waits for the move to complete.
- :param position: Position to move to [min_position, max_position]
- :param speed: Speed to move at [min_speed, max_speed]
- :param force: Force to use [min_force, max_force]
- :return: A tuple with an integer representing the last position returned by the gripper after it notified
- that the move had completed, a status indicating how the move ended (see ObjectStatus enum for details). Note
- that it is possible that the position was not reached, if an object was detected during motion.
- """
- set_ok, cmd_pos = self.move(position, speed, force)
- if not set_ok:
- raise RuntimeError("Failed to set variables for move.")
-
- # wait until the gripper acknowledges that it will try to go to the requested position
- while self._get_var(self.PRE) != cmd_pos:
- time.sleep(0.001)
-
- # wait until not moving
- cur_obj = self._get_var(self.OBJ)
- while RobotiqGripper.ObjectStatus(cur_obj) == RobotiqGripper.ObjectStatus.MOVING:
- cur_obj = self._get_var(self.OBJ)
-
- # report the actual position and the object status
- final_pos = self._get_var(self.POS)
- final_obj = cur_obj
- return final_pos, RobotiqGripper.ObjectStatus(final_obj)
\ No newline at end of file
diff --git a/python/ur_simple_control/__init__.py b/python/ur_simple_control/__init__.py
index 38c21a5..ce2a563 100644
--- a/python/ur_simple_control/__init__.py
+++ b/python/ur_simple_control/__init__.py
@@ -1,4 +1,4 @@
-from ur_simple_control import clik, dmp, robot_descriptions, util
+from ur_simple_control import clik, dmp, robot_descriptions, util, visualize
__all__ = [
-"clik", "dmp", "robot_descriptions", "util",
+"clik", "dmp", "robot_descriptions", "util", "visualize",
]
diff --git a/python/ur_simple_control/util/boilerplate_wrapper.py b/python/ur_simple_control/boilerplate_wrapper.py
similarity index 100%
rename from python/ur_simple_control/util/boilerplate_wrapper.py
rename to python/ur_simple_control/boilerplate_wrapper.py
diff --git a/python/ur_simple_control/clik/.clik.py.swp b/python/ur_simple_control/clik/.clik.py.swp
deleted file mode 100644
index 4b0d127886e9e3b4515a2eba06481e757714f650..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 28672
zcmYc?2=nw+u+TGNU|?VnU|?8#Jv`Mbw1&Y#f{`ITzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7TVUbAA
z$;{RV5qbrc5F19x(GVC70jwcVT9T$~!OLK5WM}}AS5{I~5EcprF-P%e2#kinXb6mk
zz-S1JhQMeDjE2By2#kin2nm6b0#=541_lNusDDkMG$R_#2Ibp9X>@h0P;ove?GB}3
z=CDBdFna=_e3(2ll+OjF{h>5W9?Bi1MnhmU1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(
zGz3ONU^E2C2!X^D28K;c3=H!?9dH2#23WuT1wRABRelDBll%+}JNX$HX7e*JIP)_w
zIPo(u*zq$k{NrO_xW>o8FpZCa!Ih7J;R7!N!$MvL27g`#27O)zhEF^U4AXcR7$)&B
zFf{QnFlh5IFkI(mV3^F!z~Icy!0?@mfnhNh1A`wI1H*q#28KzT3=A%u3=9mM3=F3@
z7#M0e7#LhQ7#Lo$GcasoXJGJUXJB~E#=x+aje((zje)_6je+4iD+9wmRtAPvRt5%3
zRt5$|RtAQ*EDQ`ASr`}sSQr?jSr{0uGei8gg_(h&oSA_knwfz?lbL}*mYIP;h?#+b
zgPDQh9}@$^111KBtxOCIO-u|7*31kHR?G|xBFqd7znB;p-ZC*TykTNsxWmN2aDa({
zVHFbt!&D{)hIl3h24f}$hEI$P496K680IlDFeEcFFf3(cU}%94Tqr<5K~ZL2iH4GM
zeqLE>QF>}#a;idNaz<uqS!#-oLQ!gBaz<*3LP~0JNoIatB8X5_(qsT>%}i6sE6~fy
z%uCEk*UQT<%GF3MDvB>lO)kkV(p0cfNG&L~0xLuaE7&R&WajDR<fofy1iPjd73CLc
z>OpiUfR#c86>JrX@{{sQ^imQ_67}+ZGov9~FMm(Jka$m*SiQvL63@Ic4T!LBdVXRK
z#8Ju$X_-aEB?`$Isma+2<r%3Z8L34I<*DjLsS1fD3MCn-3LwQ`%Ro*^%P%TVEK2dt
z%uCHpEXhnRh8mfhp91nD45y@~C6?xt*s7-_<`$%;1Qe&1rsQXWLaaDdT}L4~BR?}a
zwb&M>97Ge)0#X9Qm`Y(XB+{4$VRI(3q3AmB$$>nLEDvSFjWE<RP}fn&NX;p*RnN;W
zQAkWKDNW4Dsf;fzPEAorEXh@e#8E+FQE_UKUSdj0d}2|0X>Mv>iH5qaE_7K<PHK^E
zaY1Toif(aoVoqjWy1I@+No7H*ZCXx#Vu_AI3^?vUb}K97CRQe;Dpcl|DwHSYl_-?t
zD}eN*f}<H~oI-JCdR}4<ToXtm+{O4~Ga5&1t7j!9=O<+*=7kg`<`oy@7pJPnLVN}?
z9E8DPpj4cilbT$jP@a*QoS~4MlbM{YkeHL6UzAyrkqdXU63j;gLj@KN#U({BJ<19p
z{x1Fsi76>yFBhd2m!NnQ;tr56GxO5HVUt*-keR1Yl9`>CpOIgtP?VZpnv+<RSq0A7
zN;(R#pfOC<HG=pFhr3ELvorGZ^2>BVro)_u5k;WDfMF+B1!u<~SGQ0fg>cUh4}}m9
z&tN?yD-HBaAvVG^z-Umk=I5rCWPm)8lv+}rnwqCzppclCqF|__V4!EJkW>kCI8yQe
zX@>+ra$*6bJW(%J04FVl^30M9aF9W`#W|TNsYOt|I6MqisS8nw>R&{P1X&5ipop=8
z7enBpC$m@~H#IS@7{$-VSpAGOtiTQ~Eh<hf$}A{RNXbuDC{C;_Rw&8HELO-YR#4AL
zO-xY$tyNP<OioVCNi70Ls=6Mk`yfFAE;!YT6|(X(^Gb?QH0Z$+N<n^cW=UpQszP#p
zUU5lcUWtxEZeoRkp`N)8IPjrqMZrMNL=S)1p|}TEb_N9;#BsTa6`8rExeAFnIr-&@
zNja$snI%x)DCCvqCZ!fBXk?Zs6qV)`g9>$10|SpLO{7pYGynlez~YF_+{6l9xS_?c
z_{;?P5nFJ9>Wwr|O{V|}l>9seaP0-o#)(C#3K@w71(o1rh3t-0U1;>;a0RF;(ghm?
za{<;I1PK*bWmcJA3aZUYi$U1|<goOj%z}c{A|#955_5`E@%kUCM_mV0N@wQh*(MgH
zgB_{ol%JoInwaNb0IKT~a~zXFG_1e@#SsW&xCmYVmt_{0Cgx;TrGf)PAw9JqwWK6p
zM<J=SM8VHL#MMdxR5gGXQi2Np)D(rv)Dn`M4!41L$73lJkwO#h_N>z45{1mXlGOB~
z#1g0n3NrKZlan(t^GR|eT$gTsUQQ*|912QlMWuOpu$T`GQYg;MEzL<R$tNxPp}I2j
z^T>2tT7FS(Vo6DAQG9YvVsSAd`8YyrbQegAsn{bmr@#%S7?LDG2?&Jq(-g`Qi!$>|
zixsjn^HPfS6nqo2Qx%F!iy(D+c3ytDLU{(b?1VJiK*d~AYFa*2K~ZU*qPh+^a=~f0
zNWoSCo{nJ-2>`b?G*VI_<t8Yts|S_lDJ19QgPJ8d`S}G3(3Tv?8PH~+LJUJnYMMfN
zY6+-~QmmnA#h|35#E@C6P*7S7Y7r;q<R~N-<(K9ms$PZsG;k4}oS%}applfCoLE{6
zD!SB*6-qKwDm8T!it`mx^7GU|&C|rZbZ|Q*vq&K?F*mhXj{%Z9Kp8r*D7{!AKPd~;
zddN>INzBYkO;JcI%FhM)t{Bva2Kz}5;W~!=@;rrt#G=I9)RNSqVui$Fc!ijs2QnO#
z=`s?_QbBE`qS8EtM1|tgg47~~ocz3Wh2;F)+{C<;VjTr=4oJ=~sMIY;EG_{#Ftb=8
ztuzl@2!IRyg3=ODp;yFEo{^ddDuTg@u`<83NTHx8KfNe1S5Lt+4WtB~t&%gqo=sFp
zDM~CZ$xK#A%*`*&D}e@AF1RvbaLma82VQYvUMkG(#R{MVpOIRW3RVL)G&wO(0cK!c
zC9FDtgnTZ@--&skPy)9q8A>wpOVc40JvdTI@)goDAu*P#paCk9ic$-55|dLE)OFP%
z1(tfeI;e$|Tbfgnsau|3lma)cSW}My&1Wbnv^W)>oMFw{Vuh4^XrQJSr4}eKgn;Vm
z^2AE8S3%7+XdoznotIgxkPIq&^%z{sQj01TGIJBtGxNX|Ac&O=^{tLVT25j*I5vt?
zOG-e60XSWP$}R;^Xz78|Dg!vhU@TkLD=o>)(M!tD%t<XO$Vn_ojV~`s1l1o3nYjh|
zMI{Q(kmc<@`S}IDiFt|XsYN;pL7?6klm{^voG9}1bBbZw3KEM-G81zcpr*j`A&d`7
zR4{g4X>LI!D6aAfU_$T$6C_-ana2R?|9A5-Fq{WX+zT);z~<logZlXV3=EI>85qv+
zGcc^>XJF{$XJAO-XJAm{XJB{+)$^E-fng&b1H%SB28K#L1_pCJ1_oU|1_l{E1_miU
z1_mxZ1_ow628PSL3=DI485p8@85m@E85rL4Ffd%>VPIIs!@w|uhk+rUhk?O`hk-$k
zhk=2Ghk@Z8Hv>Z-Hv>ZwHv@wUHv_|aE(V4pTnr4|Tnr4UTnr2jTnr2xTnr3nI2jn)
zIT;vYI2jmZIT;x4aWF6(<X~WE;$UD%<6vO$<zQei;$UFl;$UF-$j-oUgq?w5Dmw#1
z5<3Hf5jz6|3v^uJJR1YUV>Skc6Ko6&^=u3b{cH>jv1|+s-&h$K-mo$-9A#x-*aBPR
zK*l)BsFu+X7!85Z5THf~fEsn+j-`Svq{hGyfL0+IpcW3KF$iKIlp$D96L4s-MQWKb
zWTq*^$AcPC@$m|_whBt|@wthadGYZ|RtgLZ;MQSAVqQv4YLSLteqO4Mf?qz!8W2Wt
zEQ(e5Wx<AmU5?EmD0-nBD3xCV3mPQbaLVD709lO`d=N*FraG~RRQ2h_8YF5VNjXl%
zI3>_R0gpPIQaB|Ni_-DQ6=Q@x$T1q4NNEwTcD#}lnoU*h6gm`=At^D0T)jl;BT5nZ
z34u7{K^;2s?Z98~DL}?Ou@pMRnk2X`l@ztG9y3y2fFuJv<uZ2jv5Te_X(Zy7DN0A=
zO+2dbNFf;jjvDw70N6vQ8c}&k`6WojI>H?YV{od+DFG^1&~r3NilO5dV8=re3x(R_
zLHhBeE8LZ%0*>SeG7w8u3=&Sx$plfz6*!2mpnxo*0}_G=AXtdnJ`Xk&3YqDIjN^hw
zU6ph|!?@5nOT7~CbPky&fV81G3e?(wL|p;6vIPw_#pgo0)Otmwc^VK&WMKtcEZuL&
zl%x)57#_6m9Mmz_0gXyP6Bu-&3nUB~8Ul60HIPJ|b277aAQ1*PBqy~9Vmnk_PAX{d
zEIqXZ+WQB!mJmq~Ji&^Xlub>`%u998Ps|Ab%^pC#fh7Wq(A#;Ch6>1Bl&TV2@c`=o
zALD0Wc*PId|IY+%puUERUFK(CSjNx5kjc-$pvTX^z{=0S@PUtk;RqiC!!AAshE_fX
zhCDt71}#1Y1`a+3hEKc<3=eo27+QE47-D!C7@T++7(VhaFr4CHV3^9oz);KswP2JQ
z4S~@R7!85Z5Eu=C(GVC7fzc2c4S~@R7_K1zn&d~upi{db{Vh!^$k=gFYDsBPo<dSl
zYGO7>C4LodIf?1u4lt-c2w7T7q$VPjfy@PAh}lY@{%by{>xS8t#cd#N8JI^Q-XK;T
z$UO=Q3WS{mUM#7h3F>)bnvE#}G8elj#gT#*a5!wiA&itFAOQt(C_DfVnh-3|^f|T!
zh0u;wEmlDVWY0qlAX*{BrN}Wwfz~3>LJiQ$)@<Ba2I@eNy{I-phcK|@GO!j<|NjmT
z1A_rCr2h|E1^_CAUh*?A%;#rd2!i(e+4vb4Ky&@)_!t<P_!t;e`4|`k_!t;&^D;0j
z;ALP)=7pG14>AX2z$hLKfzc2c4S~@R7!85Z5Eu=C(GVC7fzc2c4S^vZ0-z2S2qSmx
zu#XEMb;3a!;QjW*Jn&vX<PinX4q4RAx==-+9z2TXoXot`#3KB9A!;zR7UY9=#DS-e
z^>FR?g=hlpIL<Fo$W%~O0B!Mt>@HRYFN812NG(o<j6gtk4M6r=78j%@XQpK)D`+I<
zKsN(qs^@~YAQ>7Ms3L9K0<XwVEGhzRRRC>Fg4n36P@bxgk`I;yhb(BZJ!px&0&IH+
zbp9MP`wm_Ep-`NWUz(Gmkd&&Bo|>0hl$fIj9ub20usAg@B``&!Fa=^kVTyt+Y+@fY
z=kA5PA_1aA8L}&*Ag2_v=D#2(u~H!=Gc7H(C>6AeA}_TVY`!vRA4fqcc-KZMWMiK~
zS!zyxa%M>;XbT7^zPvyy1TypVlJj#xD!oAS_g>K5tdKsuUT%I$Y7TU{060E$6bfNn
z*#2vn!$9c+n^B;BI=Q9E8A#47$w<rt2cxsLwnABIQE_H|o<dG4Y-28Xcn#F?SFlwG
zDJo3`um5LYWnd5i9q=c>z+eZ>pP=>s5BM1vw(~PEr1CQ`aPc!R{NiI^_z7L_pUKC-
zpvcF-@QRm#p^cY;p@Nrzp`4e2A&{4W!JL<YL6et(L4%ipK?Qmiz)l_phBzJu1}Po}
zhSS^(46C>q7?yD}F!XXWFvM^(FvxH-FuVn==ZBsPkjKTq;LgRsAkM|W@RgH+;Q%KC
z!%R*FhC)sThHy>>1|?1ghTj|v497Sa80JIQ`df1_Fj#RgFz9eFFnnNVV0h2Y!0?Wp
zf#DuI1H)!^28Oxp3=A3U3=F~S3=FdD3=ALH7#KFNF)*~UF);YELBe1gD+9w+RtAPX
zc4$D1QllX-8UmvsK*JCK)fix0048AsxFW=UVhBVDHU+q4h(BzE8fM@*umn;;gZ5;{
zgI5Pa+80Cg^b(9Z98zZ&!dLO7r<Mdlm+mPmKsPd`LO0eXf_7YH>ZR&|+CI=DQjj*s
zf}0!2H8yy^Ie5b?QgaBj>=4qXKwfpIqmZd-#h|Q^o1X$cd7`9Jp)w!3O#?cy0o_Tc
z0XkU%?W`5Zq=6pz{EEE%vUrdZh{u#ca}l63S|GD_%Am9YH7_%-%sVqLH!vVLGcVn-
zEI%{FH!&}>z&Ei1>=EV6G^7?7c!?}{8wqZ0N|2O+-wxOKY+n-12WcVJY){h7_QYp4
zamL{`As%ujj%Qw3d`4nU8ZpM9X(7gJ(u~DzB2rp#jnAeiEI|i-fcC)Vq=LE);LZr>
zJQmO?D;cQ@pgq1|M<b<2jN@a#3Nq8cYkKvdIS9NU*H%FZLow7-NahihU%-d8AmtYv
z)`2n<TAov~0_SWjYl9(s;z0pmrBGg!S(2)dUs|G&4?c1Ue$q`bd}BLkdo!$O1lhu!
z0@{vV3_sf?H3zg`K20IB1hkjDEVWo6GcU2ApeVnfC=+xb18BE$5ojTMNk(d=0(d)n
zYDGy=qC#;=X<8a|qjhd#0muMo_$Q^NXF|?P03Z1R4JwB6RLJgh&@O)H-f{5Oc+f#5
z1ts8qN^xRtDkw(4JIRZ66u`wHXm>s=M!-=G5&%~g3Xo&0^cXx#)Qc4o6_PSb6iU-`
zDs{lDlKc|TNl5Um^ocoO-TvMR#rZj<pc6Lq7#tOfONv1KOW5vlP%jF6bQ0(QoK)~x
zLiu^A&>iE&snA{CNC}}Z1)P$=eM&FA5CsQNe;m5H9UO-+r^62}(m?HRqE5zQ8izRV
zD>JW5!%M+I0c4Ff=p3-r%2W+6z2c0-g4Ae(SRDlfTT?*`a^MiWz{Ye~3g)p|8YqRl
zjsm(%m8k<5z@<1S5V2xJZY;(s4--^oppJ8tA=OtAWQhT2r93!FVb*|XP?AHO-vK&4
z1$3}PQBkFWmA-<WLS%la0_czgaP|P5KB8a<O3%fq;8O#N6^b&`GfEWl^2?#8ae<61
zO3W-yRq)F%@dRCP2P(%>Q(Qr(vuUWiWTt?JOVTpaLFaOSGI&OQIVj(PPJ#fr1$vYW
p=+p$r(K)c91ttMHs0ri~J%p<v6(~p$G#~*v%>sOc2W+<r0|0;18z%q&
diff --git a/python/ur_simple_control/util/.get_model.py.swp b/python/ur_simple_control/clik/.clik_point_to_point.py.swp
similarity index 51%
rename from python/ur_simple_control/util/.get_model.py.swp
rename to python/ur_simple_control/clik/.clik_point_to_point.py.swp
index bd148d33717e53ec67b40270ae0735a98b999d35..f448d0aca693a2d3a204c1725ca5bc5a7dd673cc 100644
GIT binary patch
literal 16384
zcmYc?2=nw+u+TGNU|?VnU|?8#Jv`Mbw1y#UCL=?7esMv5N@5X63Lnl*EY8l%!>0lw
zR|hjtKO;Xk)kq)C%_+@G(Jv@UtjH|ZFD;5M&de>yNsUj=&nqd)&(SZaEXl~v!y=KK
zlbNj#BH|13GxJK~OY$KMy@E=Jg`?zX2#kgRBm_!J(sV6&8H|k#4L}0QN{R}?LZKk$
zC>{-g(GVC7fzc2c4S~@R7!85Z5Eu=C(GVCRAy87l#8A(`z`z9cuLG23M5CFZd><(7
z2&G}_pxjYvGz3ONU^E0qLtr!nMnhmU1V%$(Gz3ONU^E0qLtr!nMnhl_hCpHp0|N&G
z1A_{vBQL<f0PFw1;AddC%Fn=XlAnQLCqDzjY<>m?XMP3-Cw>M7JAMX+e|!uK*Z3G1
zrtvW_xbiVDeBfnZSjfx3;LppzpwG*|@QH_kVHytu!z3OCh9({c25lY&hU?r643oJT
z7@WBo7`}5cFf8U`VDRH&VEE6;z%YrEfx(57fq{XOf#DPf149i51A_|(1H&tJ28K=S
z3=F>P3=FT?7#P;FF));|F)&!MF)(~*WnkFH%D~Xd%D`aB%D|w=%E0iJg@Ius3j;#{
z3j>2R3j@P-W(J0N%nS^3nHd=Bm>C#CnHd<EnHd;XFflO9U}9jH&cwjr&&0sM&cwj5
zpOJx~gAo!I%AoLo#+QPEf<j@6f~|s=UWkH&LSBJhPG(+WPP$%ZUYUlMf&*Aw8zi1u
znX2KXSDcYpkQ!|etD}HmYbt0dlw@Y-<>%#>#iu0Z7HBFsD5Mq@#h0Zfm*f{Qq@<=P
zfP_<10*X^hQ}Q$O%2JDpQ#DZZ=qPyUAZgOHVo;_IU{D765hsSO#K=i4D#ob>B%w?-
z_b`BrD@rXXEy`0!EJ`m1^Fd-TT#}KhP@I`tnv+<PnV+XnnOdTwP@G>>RH<O4ub`(8
znO~}qoS3JOkyw_hke`=RsbHv3l95`Rs*skGm|m<<l$oAUqL7zguC56;x+pQTI90(f
zzr+)?{3JIuuOv0awWuh+NJHHvGesdUzeFJ|GrhDZRUyB$L?I);T%jaip`f$`WUxYd
zQD#9wYLP-_o<a=R#LP6X!}VYiiFqjy0feiqzzRX$Q?OMiNGvK&Ez$#1@gOk`O$M+$
z%*QFIX^EvdCAR8@dIst`3K^+61-9xSM<phglqTlnRK}MUr=}<*mgGYH1U9NjFEJ$r
zWL_!ALmKM3x`@csEiOn+P0=k*PRz;7OIO!XD5)$+wN1;(Pb|@a`T}IRvO;cRWm2j_
zWqzqbd177($lD-2sbG&ojZ-MjOwUWq!R}%#30qx9Avq&IGdZ=`Hd-CKyn0q*a(+@~
zVqQp5VqS4UesQXLEF!i*E(3>wQgLcdYI2D}c}8Y(hC*^qW^%SdVorK~QD#X-F5J;d
zFdq>N6<9bFmlVPDC@X~cyZ9?4rlf$qT$EZ|g5pt#J3zk3%u81Qg-s$jC4e(;Mt+$>
zQEGZ=PGV7J6*#pj=_tS=(=b)n2;wIk?kdU5&H$Aex**eGPQ!>IP+-8YldFQWW00#`
zsE<OpXNZSFh=*se9+H&?dZrK?VH#j`NxnjHer{?>2FMdhsU_v9sd)+p3ZR5*sH0$@
zXR44?33E77@&IXv1VD0PfkIJ!Qhtegu>v@0DU@fHWPpPV!Y$6pOi3+*>c!z<uu5Hs
zN>u+s@)gKBBn*leD~05Y)Z}c1@{B}KX3Q*B$W2YmD@O6NF;+ig4J)vNON)w=i!uvJ
z6jJh&6^aupixo;TGK&>5ixt#!QWH}YKx@?$5|fitb5e`Ik*cnT>OM%2z=9(Sv_P#G
zMS~tJp%mm7XO?7^r79%n=M|SE=9TCu<R(@q80wkpfCC?zRul~MO!V-F9g2H!WoJ;}
zK^&KxSdp1qnyZkQlapVbn3R*MkXZutjY3{&Zc=KIf<|VELQ!d6u|i^rf~kRlN0la0
zs2Uo803={>#Aa?{g)ZFCVpx1;g8YarxIm?P8mMknfCNf@9;mv@F9K)d#G+J%jKqS1
zN^r75c1Nl%G<tEkB0WDbM>iEzl*3$rH3vaLr930GBqJ4UrFv1SLTNE5JAfPpEy<BA
zc1z4DPQ~kgs2+74g~Vh~MP-{<ln!>Jo>P8)PHJMFe*s81F~<=s4J&X!aRkB`E`k@p
zWtqjLi8+~7so=m+NKY+DEh)*@QAjE+QSkE*akT>1SKx(|pn^X&1yn_n<aD?V#5*2K
zp@<ZkaJOfb7MCbw=9Q$T7bTWJJy4LDm!F)Rk(p1D8{xWi^Yd~lspe2nN-HYO%Y(&y
zXb`NekzYiT`w(?KnQlwVFUn0UDM>AgPtHj!E=D9D$D(v_jqU<z6%>1<<`lTW6ho3E
zC;@?RewspAVo_#(X|Y0fW?o9Mo`P>;cB(=#s3ilc)3fvP%VA|_ssbplfQq@K)U^B}
zNTpv?ny09)1CCs9+AUJB1vO>BY(1Dm0>JGTjg(YKxtW=tXR97mnx~MQlMiZ^<mBfU
zC_o#vAZMiJCYEF-7c0boTfXV3C7?D+v4*A<gOZXGLuRo;L1}RYs5z69qmWpXUz&%g
zdKL21z(sU&eoCr>Mp9~WVrem`=u$6MD9KEz)C4sI6;kr^)JqhSGZORC!R?gHB89xf
z+|*({21xDzWoS?<OCdig3)FhZPbx{w%u7vCNGr<E1^KQR(I!K<jv>E157bgi%uOu;
zHHH(5;T2+j9>{P|dkoy>EXfCzn+k~v#igKTVNQNtx<Yb(Zf;^;O0kXtI0q!>7gXvN
zBo>!|9GF?GkXD)pE(E}ZKB!$;nO|DOP@a*R2Qmtr7(pTm1x5MkMTxn33Z7};auL=h
zgL*bmA*Cp>xFj=KAu%_<G_M32T)E)NguyW<2ON0CiFv6ow-+mb5`0E#Q7Tvs*ica8
z8D?N!C9FDtgnX_-VzELZxM2=XgA64Z`K9TQiXI#(CHV?znUEOERnPzxNkypzIf==s
z3hKJ*kOE6RULDj{&MnO;$<!^+F9J7?!KM{!>M@}C3?+pYr^1tSA~X{eE2QK@12w%U
zwLpO(1XNd-Csu;J3Tm!F13>}oyv$;SWKh|w$KYC)T2!f!nVXoNnFp=_L9Aq`Z*{<Z
z1#oN>r<Rm}3IlMu1eIM1pwQ9-r&R`Uiosa6u2));nWLAKpP7?dRFIQck{VxLlnCh}
z<QC)?l_)qvmbd%l=NI@U<|U@57U?Jifm%;c9>iR5qR7wBDOSjYXe&r8D#=XDVSsXB
z^$Cm*N>ng*UTJPYB`B`)3SdI;0uv-$keSB->i=sqGcc61K-T}m=J!AGGcbJQXJ9zZ
z&%m&epMha9KLf)eeg=kl{0s~^{0t1n{0s~?_!t<bK>PnHd<+Z<d<+cipm70S28L<8
z3=HYK3=D3(3=Fco3=A)L7#I%nFfdHyVPMGTVPLT0VPFvEVPJU8&A@Pxn}J~tHv>Z{
zHv@wMHv@wtHv_{1E(V6JTnr3bxEL6gaWODtaWODtaxpMua4|4Ma4|6GaxpN7b1^VH
z;$&br%gMknos)qfo0EY-i<5!jKL-QDNe%{vJ`M(kat;OtT@D6@2kZ<Cd)OHmrm!<G
z6tXig#IQ3k2(U9SY-VF%n8(JzkjcitpvT6*@DVzuu#%O5VFfD#!*W&zhGnb_3|*`Y
z3?-}#456$H42G->3_Pq14Ch!F82VTsVOItUKZa2-8UmvsKtu?D+9jYSL3}Qx=+i4I
z&C`H1TabknY_SxR8sJWq4yd~X+8+ceKy|=1Hbg&kKmsHTslq|Ur3R9yb53Tq4y2<B
zHzX&u2t{{JDyWf@o>~GerE^k?G(gP-h$3HbBgqzGhF(f)T4r9Vdwyb00H~J-^#<6H
z3brUkI+76}!xU^4keU{mX$tZ2pf*Z;yn?N*f>L~ZZenI$e7usC0w_R1AqE`~ODalD
z%y!F3Ob0i~KoSK-nRz7|O3==dt%4HrFri)vZ2Uw+6XJd(Es1&HkqKm-1(|tzIr-^k
z8o{pMK9i;%R1t>eoXot`#3KB9A!;zR7UYBa=iornOGzwA)XVqHjD~Q%{5}0b;yqoU
zgFTsPkgkk^ssgCP2^lm~23O0F5k0WyA#E7Q=udG$YI0^;X0n1tVh*%vlBu2xHpS4u
zKozMI32q-G78QY7iJ)#0#F@$p<*5oO`Cv(~;h?G?RN*VY8k<O|30hk#6ldg@=A<Yj
zr7EPS=A{-T=IDWA2jZpT)V!3y6pg|Zhy~!mWT+XSl!ZE~3)ZI$X(JZol%|8dT9A`i
zsgRPHmX=zS3To5kr9#F;l|ik_f>LlBGF1UQ%%D(~nv<WLS&|8Akb<Jx3zQEs^YoJQ
zb3rP-K&jLVI<^MMOnSNbDXBToOaYD$9fd*|7Zx87w<{}v(g!x9K)r_C(&P*zXO?6n
z=7EFJSzB8HG@uO{tIbJ;4HLo>vVyHbNKt7j*yjog;4s%va?S^hfv2bDC8sJRCTC=(
zmZhfXC={h8CWAT#DXGQao->G0RD!q-lvLn@^LhD2xf+NOc1;BvXnqA{-gt1zhNLBg
z8fZA7CVt2Oy#m;6&>W!v&-yr0u3lntiDzCJG;qPi1;mBQ3Tc@|#h{J}WWWkC)KH!Z
z8fgFzUO_rwV9P+^oR(iyo>-LPotX#iFhPyP7GDq>3gM1RPb~?A7Dmbn&?a;$v`d!=
zYU^j}rRsswIn)xQZW-8q<k$g^bbz~R&}0iLUO@#Z#68I6sE$G=XktPkH$MeD=2=py
zP?^sF8Ow$iV9++K253wZZNdXmlIwv-PT^A}VCN{qx+@T)z%v$5^D^_wyfgE10|SCH
z^U@v5@-tI>6Z0|)d=o1mIy2K?l|@c!5x9&6m$10CDM3;Oemh*_vwcZ4AEbp?vpq>S
z+Y_JJ#2JU%gm}orlxJR9d`4nU8ZpM9X(7gJ(u~DzB2rp#jnAeiEI~uepx#$bDyWJ8
zm&Tx(fJ{iq2I^gc9StgJq3IE0;sUGyHX{VhLEt8at%4GUVyLH(%p)qlfTsx{Q;Ybl
z!;<HetiU-NOKS%*8UqRdD~0l+%o51dS3Y=r6h6mN3?EVfb*y2vAY||$1w2^<pH)cB
z0gcq8DP)#_Mls4#ixo2S5(^57@(YSGK|_I{esU2*CU|_O5;A$0T2WGzs8C!|nwAFb
z;^!t7fDC|!e^P3CCS;@zJlOyZDu(h@$Z%s~P7bv1pPLGD3uuVEpafhe6({DVg4&Ic
zQ3o9baB&D47lOqIII2Mc;L1V)QW5Aec!K6P5*3m%OB71eb1HSftdjf^&>S3m5GOGQ
ztlQsPp*TOM6jVv+F*qs|mlT2860mU#P>l(mv;$4Oq=IL$^7B%m?k!G*jxQi3gu)bX
i)Pma&kjZF7<pqvInA71?W*Vq%3oA(B2~DSkDGUIFfg7Lz
literal 12288
zcmYc?2=nw+u+TGNU|?VnU|=}AB|Nn#w1&Y#f{`ITzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7TVUZ{;
z$;{DDPc4bh%}+_q(JQEgSTjnFhQMeDpoc(dNt&(&FN3j>p#exrSxHerSSS?49L1v{
zFd71*Aut*OqaiRF0;3@?8UmvsFd70QBm_zd7#Zpr7#NtK{&j)UjA%5JJ4%g)z-S1J
zhQMeDjE2By2#kinXb6mkz-S1JhQMeDjE2By2n@jxNK9d1aAIO$$YX}g|HJzKFZdZ4
zuJSW5oZ@F-IKa=qFpHmoVJ1HVgBw2sgDXD+gFHV211moR0|P$;!x}yY26H|JhF`o4
z3>SGB7^d(tFeLIaFzE9#FzE3zFnr-*VA#RKz>v+uz>vkmz@W>+!0?cpfng#y1H%Mv
z28J+h1_lOh28L~13=CVj7#PC17#O~CGBB*>WMEju$-v;w$-wZ3gMnca2Lr=K4h9B)
z4hDvw><kR+*%=ttu`@6fu`@6vurn~+XJcSE$;QC2ij9GxkBx!Bh>d~a0xJVU1}g)D
z2P*@E9V-I^Gb;nb6BY)B6D$l2$5|K{X0tFb<gqX?II}P?NU$(4yk%x!ILOSvu#B03
zVJR~MLq0P!Y=&UmjyjnRA)uh3puhkkic(8Ti}Dn}Yf*F*lJj$NGK(|w^B{{;bQH=m
zi%S!UR+t0RmXcVK$N<&`Vky`v6lCV<xg?e(YJe@$1WSRLdRh6Ic_jfkiOH$Csd**E
z(Pps<whHA%nZ+gX#_`3e#t>&>S8Iw;YXnk@$5<1DQbPhtjX_G&@{3Xvi*gAlH3BIu
zEhtDWiYKht5Tv*`Bfm5!CAA1_Fxb7ISO5nzH1rf~75wt^QsHuFae^)l4jE_&CzWRA
zr1*lC6}c7V=Y|%gq-m5ErKH6dB$i~vCngo==aiPDLIOWEr#RILZih0SXac!U8PgG<
z7zNwso|>Nvw+`$Y9R*BVb-+mgq&Bsrs4}FoAXU%V-^a%@*wfz+;v%p;XaPaFC1IYy
zp^iRCHb7m7<{@ZGM??YKW^iy8!##-UGlbb_jwdk|OLJ1u<F+U<vp7}3FTcbSw6qMA
zKT=a%i;D7#G}HqUOEQWTGV>HlGE%|8t&m@&P@JEeT9T2Om#zScUWL5W)D(r1e1)V`
zg|y5H2)nd6RiPv!vsfW5v#7X4Pa!zJsHjp;!8tz{q^&qVKTl5`;-kzoNbb+ctb(Q#
zWrYxb7k`jh3ZN8KtWcJjr~p~#rcjWWoSm4SnU}7kke*rsv7#hjp*+7R8&VJ;XK6@z
z5bSEK0Se}#f=a!jg36G5-^7xl%nA*d%A$fwO&u7gq$n}37~(LPP*Fjpf~`VcfnH)!
zQDUV=w4t7<xsj2fsi~ofsfnq%fw7K)v7Vucp{b>jnW?d<g{8SAvXHT{nT5HTv0*H#
zsbGsx%(c`rFf%eRGc-0Zurx3*Gc!rmH89grFwirwv@|g{w=g$0u{1L>Ff=y>3mO<0
zniv{_#7&Hij4jO!jbb&Cg8*DoK{G7=ATWwY3Id~eu*;Ag3zkQ5s-d2#nT4r^v8l0v
zg^{s^Dab`S3cAL6riR7_CYGiqmKNqF<|gK;x(4Pb{)U>4VwkBO$X8}&mKNq_=EkO$
zAYG<93c3)FTUZ(zSXvmGTbdb|7(zX6WMp7yVrgP#U}j-zX=aS(ad5dyl*bK`J#L8U
zaj-m!TMhIK3@pq|O-)Qp&5TS;Ei6opAyH&tU}RxtU}|J+Vr*z&Xl`U^qN8B0X9P+V
zhQ_AGmgXj=rpBNMMF|e5ohVl58t55Y8W|az8e3SJn3$WJ85%<Z!@$tc+|bm(%*fcx
z(#XKT0vsBK#>VEBmgdGLW|rnghUTWQG=NmE5C{sGN^ly0agfpgOb9(2fYQ8yg{g&^
zsgZ%1xrw<EG#eP0nHpOdTUc5c85=@{Of3v8Obrb!jg5>fq5g-N3bqK{|0YHzmZrvL
zh6V;Eh8E_gMmh=xItm6buc6d~1U!dO3-%s@3$_=M$Pt3*UIgWILo-uLOAB*zLr`3R
z9b%|wXliO~Vq{=xU}0=zYHEQLtq8Ng7NB?xly(e2ncL9R(A2=vz}&(V6op7WL$0(5
zcnq!->@PSEY%9cDa4~dW>4E}NN5NFj*wED6(9qD>!py|n%*ZlT*T4cAJ8->V^H6;O
z4g&){Lo*{&Lt_&YBU4i&V`BpoSnTAc7H7n#WER2O0SdMX`Nevmf?O{#sTf3Sz|=q-
zstn^2uLY(<!B!zHGbgoJLmgvbv|dquQhrH%N@{U(QD#9&W`15Vw8DYgr(mm~q^GBk
zsZd`D++xA3gR%Gpp{OV|C$S{64BGI)6erEGdLRSTHNovxaG<G|7MZ5TmuHq_#DiU&
zSs0&@n3s|YRS0&mGDs7o83uC=Zcl>L>f<vM5(*3{scE3P7SiU{fHXcbt5P+s7>KH<
z85oomk`wdPOF)Jonh`~*i7B9_0LVii(c)Bvw9>p}a72L`6UB*TsS3H33OSi+sX7Xo
zDcK6;8HpvJ)<=0pB?CiRQGTvMW^O@#QAti_l3r12aeir0a%wSD43aDuAPN-ni(#zd
jN;osGG`FBqA+cB?uK*@nkeQdCoSc!F4-ziO%wqrm^^suS
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..80ba7275dae3fbd9f50fa6b4ef4c35cbf0713b9d
GIT binary patch
literal 16384
zcmYc?2=nw+u+TGNU|?VnU|{&@5}dkRy@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|Fo8Uo-D;AJp2GBf}&m6a3~
zgoQ#u%uzfV0;3@?8UmvsFd71*Aut*OqaiRF0;3@?LPDUVfQg}=fq{Vu>faP7&4@-b
zL;2uw0tN<#R45;&4$2*+MnhmU1V%$(Gz3ONU^E0qLtr!nMnhmU1V%$(Gz3ONU^D~<
zVF)CqFfd3mFfbeib>syY7-0SX=ll!|NBJ2TX7MvH%;aZaDB@>eh~#HrVCQFGVB=?C
z_{_(^aGQ^T;UXUc!!bSvhDCe~4EcNv40(JE3~GD~4EJ~$7^d+uFihoTU?|{aU~uJS
zVBq9sV0g>Jz;KC&f#D(#1H)Dx28Lc928Jpg28MJV1_oyy1_n7E28R3G3=CVj85p*3
zGcauCW?(4dW?=B;W?<muW?;C<#lSF^i-BPd7Xw2t7Xw2W7XyPh7X!m%P6mecoD2+A
zoD2*aoD2-=oD2+7oD2-FI2ag~a4;|waWF8%aWF7waxgIbVP{~t$IifTg`I(6E;|E5
zE;|E*6*~h1GdlyrWi|$e1#Aoq*=!69j%*AJVr&cy4_O%)wzD!Y^sq88bh9!rgtIa*
z=&>>|u&^>PFtai+FtIW)FtRc*{9s{VxX8l5u!e<!VKoZ_!(0{yh6yYT49P4E4CX8h
z3|uS>44f<s40o6r7!EQsFid1-U}$A#V6bIoVED?!z_6Byfng0314B3y1A`nB1H%_a
z28LsdkhnPliXWWmCpWb?BR(awC_XW%I6tSfBvrvyA-`C!Ah9GvFEObYL~Fp*XfnW9
z3bqPLdV2a0RFt2TUlN~^TAW;zSx}OhpI5A}#86t4k`@orjn80&qN3EC#FEUiR81s}
zFfl@wflSdaEiz4wFV8H=hzC0-voJm*F)t-m4<xC?z@SVWz@V&v6$FR41}GR=DS(!g
zD3oL*mMDO{j#Vj0P?>7(DJaU!E71U1rpW*@8N`atj8(8zC@D(JD^80~%1^0`FUgNj
z%1<mx(MwE*D2~pI)nrJ^FH*=<$jnnHO3X`7)yPTBgBYY~#h?r_N5K~23vGqG0=>kd
zqQpv#XahY19R&kDBOL`@13g0%V~bc#27;;$AgYZ_K&lCt3{h)f2{xIKGoXr%Ac_^r
zQj01Tl5_HlQyG*M^3&3aQ%gYJt72fl78oG!W#;JxyBcc*=_nLvG88D-qWIL%z|0in
zS#tvmV-Rg-Y-SXz$p8uRAWcw~idHbt0|h$>gFR_#W?^P-X<?$EqidjNVPs@zYH48_
z8>`8Hp%Nyk05Te;Emnsi2*nlAhDhea?b1<DFwiqFGBq<YFf)wRVPH@O8?OU$1_<jK
z=$V?CS(ura8$umoY+_(yU;=UoSSeg3C>kw{AhJ-GK=j4xFeqa<1)<GAPXW#WJH^7n
z!oUKg35!ciO)X8$&B5VgVPs-pVs2!P%Owz5u>VZWAo@Ut;&ln!A)r7pHZnFdH8ufh
z0)>Qvf&$n<P&U{NW)>C(h9*XqItn1)n;RM!8dzGIfn5YuhM@(f2ozT^?I0`A+@*jV
zTPR5blvymz3=9kvK-zFwV`gb&YHng^4t1G<nTffDIV7xbX)%W?0=o>V9b^SbE;BVZ
zFt7xN89Z@=BflUsPp_z;5`;tYeG^NHGAlHS3Mw@jl#2=~QPQcdk)D~MF(i%+j7%)R
z0c~JlY#gfzDl-lAuw^D41)SO#lodh)0$hU>d|cf^6axG`{X!HNlod2G^Arj)^AwWv
z^NUh4^Abx^i**!=QWJ9&^7B%Q6%vb56{2-3bQE+ebrh<=R)_ez_$!nbWtOBW6zAur
zDkSEl=NDy`WaKK8<SV4)E0knp7Aqu{mgMIqmSiR;=HyiBK*T`8#hH1Td7#1sY)Wow
zVsU9vs)D1Ri-Kclh`+C6h^Mook58n6o2Q?PzO$o`r&Ew)h%19K#7V^^rD<sl49Wxw
zpQ8Md_}s)2)RI~UQhtLf#r!-}DR6*7lLJl)QcxR0N^1k;w2f1Xf&xSnxQI8@GXSL`
ztZqYz_-L3NpaclkqpXmTT9m4glUkx)tdLk-T$-DzkO^XEl@^yM6y=vBmZYXABo^hD
z=A|f9C}@D?^72a*^3#$vp`nG%-ze6Cs~{)=Nn^!{prxM*us{bD{3RKw3Q74TCHc7u
zIjLzS3d#9Jd8tJT`DqFenZ%+L1&#cqvdsL_Vg+#PCo?|}6uiZy$r+Gp9HcNUv#7X4
zp|CVDr6@5E<V>*k3f)SDf}F&>RE5l9P|_?<RmjUPSIA2Rc@7eLY57H<RF$8nkXDqR
z3)Tgyjuq015_3~Q=?`Q&xMBv^S6~GQW8uzBsniAQU{D6z0MZ1mL_sOEpr9zfpeQr3
zBsHf}p*T4)2V{F*3e5MYPG?|X0BMZ2(lG?J5OfU{w7@kyL_k{s?3DP7)XemZ5(Z_M
z8F{JYNcl~nxU#q;HCLfLRlO)xAw54cFCC^(w^AV`vnVwg)C5Ga2w|xfsvS@X1CVPF
zY?z|*%#@N024qJm*eV$4nLvfW(qIu|QwC)PX!wJ}0Fr|fkpd^D5)|IK49W^6`6>Ad
zu&kVvT2hi)q>!6fnUo3%97rn5C@x7%&ej08hjgkmH5sZP;#H|d`NbNDrjm}KrY6{{
z5b5;PJWx!RRFr6_>*<4h6rY(FUyxamnp3QoTwJEEqmYuClbM@Yl3HY|uA{EWP@-U~
zpaE`PC4$;uh6V<j3i=9$2AX<Bsl^$I1*sYuh6V;;Ym}i`3RFlbWR@rx8YmQ}Cg<m+
z6zeFIXC#)SmZcWau%QcX;u=~h<R%t?6Dq`4pioUzFm{1v1@yv&dJSC$P{1mrXO^YL
z=cdM&WTeI?C+1`(6@dzq_}u)I)EtG(+=Bd~5{2~C5{LjpaV4}PlbKhXT2!K8prfD;
z>9XkQ>6eyd=BR5jKs6Ot7Q>kNa8^NPUVd_NMrOW3Vlk-NfvLzV%`K<|iRBeAFeod4
zL&n%jp`f%xp(wSuGzZk`EXh|$E-q8hNGdH+NX#kDS13*_1C@iI%#x^(oS#>gnwOcH
zSE7)dlUQ7=4{lzA3r<k^12z$(I{~wbKqP>i1g_Q6GILUkp)M+h*jtjBo0_7Km7kdh
z>7js%6lmKXR4js1oPvg5Jh;LQ)=@A5xu!C|v`8T}ucW9_p#ZECRPZP;fcpP}91IL!
zIUwu*Ve|V3`573t^D{84;%8u(%g?|tnV*57mY;#ail2eu6CVS^YCZ;rnS2ZkZF~$2
zMSKhlS$qr(5qt~`@_Y;opLrP=uJJN3?BZo$=;38xh~#Bp(Bfra_{+n<aFvIFVGR!h
z!!#ZShHxGRhVP(0JvRfxT5bl0Qf>x@2yO<3aBc<$HEsq5E^Y>fH(U%1kGL2Z4nh0;
zv$+@;g1HzNxVab@4s$XvY~*BM=;LHyh~#8ou;yf7_`$)z@STH!;TzOXUqJo>HS$OC
zXb6mkz-S1JhQMeDjE2C74}rV_Jy2T?)W`>of#;=`gEtX^+V@I23g8x2d|@$KOJ7GJ
zEw{v0T~*IC4b<ufHN)Zyiy^%RSZAX|N1?D-6VfNhE6~fy%quQPOil$gTMZ3#6hPy%
zAbCX304jpgJAik%6-pAbQ;U&$2@Ej%z->*9!eULTjY8uX6^FNsLE(ke>O}7)K>1Yc
zEI>^JQ;;DoB^OT@1wa1~1!sT1FxMb=S0%`RIY>!nnnI?6t*t^(sGpywpL@J#h-;9p
zp%r*QJvFB|6~Y3k%!>zg-4sxjg4JcFDZnIc6+HbsL%^m$bSf)=hdzq4GYbkp{bf)O
z9?~-}D$P>>O<I74uM~<)^7B#|z@~xh0^!19y~Kio)Vvgp!e}eASQIB~fW}O)*r}<I
zn3tlUk*S~xihNsJ1p`fpGr$HyN4+!iN>bB7ohJ=&-$_TIP)DImK}#W}1Yw7AnS!l?
zmtKg1gF<RiQG6MAn=IUU2uVm<P0UHx%gig&@KSI9YXZ+2q*kVCc<F(=710K<ItmE3
zrh=A&p@Ei`u8}5o^Sz)hP0r6PC@o3#0`)w+5|i_jG86M)j!sD|Nd!CH3p79&@9BbY
zJjk;koK%#Wm<@L{NCbqTX#(60D@soVjh86tC}g5)2SsUq3GQeG83a-T&Z;2q=j9jW
zY9NA2Q^7_dwV)Ug_y{Se-*WQP%`}2tQ;Ul7i!}8h0jvObEL1|lRsrNsy?o!yXqZ1^
zK{Gj?d1V^D>G_E{2xo%^Y}4|K$`gxHyfgDsL8DK}#b}`gQH?hF14&2FgaS4c>O|=D
z2*Mg=blJQDJ?OBAv5v8(rj7!VTF^*AQL1{eLSDW?ei5j@n^=;Wp9e}$`9%sEmHDNh
zF<9`JS!RhsPHtj8WLgSj1gOUf8bYyx6kO4<49NLY!B)Z0z`%e3;&26|JjVdfeNZ(9
z426(_7E<s<8|s-_nCK|z8tPe?T7n0vElrKVd}9Mk9R))@Q*+q(wHbI+6HI_clTppl
zHPSOMgcxLE2G$4}_%_tD1Pz26>KU7vfsHpdG6x$0Ca{`gWNN0PYp7>pV4-86XK88*
z;+Pv5f+!OMQ_x(Ju_0JDm;f1}04hxsGV?M^5_1#^@{1KpGV&QxK*LD}dIp9JsRf|Q
x1(LxSV5vsIR>6!x88S(kS;7F#`Jlw5V2c<w(Ev@EqE6lFD8MFu;j_7#3;?ZHN!tJb
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik_point_to_point.py
similarity index 100%
rename from python/ur_simple_control/clik/clik.py
rename to python/ur_simple_control/clik/clik_point_to_point.py
diff --git a/python/ur_simple_control/dmp/drawing_gen/cliking_the_path.py b/python/ur_simple_control/clik/clik_trajectory_following.py
similarity index 100%
rename from python/ur_simple_control/dmp/drawing_gen/cliking_the_path.py
rename to python/ur_simple_control/clik/clik_trajectory_following.py
diff --git a/python/ur_simple_control/dmp/dmp/create_dmp.py b/python/ur_simple_control/dmp/create_dmp.py
similarity index 100%
rename from python/ur_simple_control/dmp/dmp/create_dmp.py
rename to python/ur_simple_control/dmp/create_dmp.py
diff --git a/python/ur_simple_control/dmp/dmp/temporal_coupling.py b/python/ur_simple_control/dmp/temporal_coupling.py
similarity index 100%
rename from python/ur_simple_control/dmp/dmp/temporal_coupling.py
rename to python/ur_simple_control/dmp/temporal_coupling.py
diff --git a/python/ur_simple_control/util/.boilerplate_wrapper.py.swp b/python/ur_simple_control/util/.boilerplate_wrapper.py.swp
deleted file mode 100644
index 87e036eb49ca0637ee8b9db76e87b12687138579..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001
literal 32768
zcmYc?2=nw+u+TGNU|?VnU|`sJGd#67w1&Y#f{`ITzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7TVUZ{;
z$;{DD%FoP6Eh@-KEJ=+oFG?&ZNG;MUsDxNJN{)uWXb3<;ptK}S*MgV9*vQZTB%rLM
zs30s93Sy4p(GVC7fzc2c4S~@R7!85Z5Eu=C(GVC7fe{h{B?W8@^$ZLQOi=#@L1{)b
znhnZVh0@|s8m5jN$~T76Wl$O>&kW@|Lg`2-4U=bu@(rPM1C)lzvq1UWP<j@WhRH*@
zqts{!jE2By2#kinXb6mkz-S1JhQMeDjE2By2#kinXb6mk0G1F)OkrSH%f!GS2<l)9
zFfhRS|F`)W7!LC@Fl^vwU|7P>z)-}`z!1jIz@Wj;!0?NYf#Dh-1H%+P28Iqk28Jp=
z28LKZ1_m`g28JiR3=CIz85l0}GBBLyWnkFD%fPULmw_RXmw`c=mx18|4+Fyz9tMVD
z9tH+W9tH+e9tH+!9tMVo+zbrsxfvMhxEUCNxfvLAxEUCPxfvMVaxpNR<6>Y~&c(p6
zjEjLGjEjN6jEjNcHYWqa5>5t&#heTb)tn3ra-0kdUpW{UR&X#dq;W7XSaUEiykKWw
zIK<AtaFCsWVJABS!%B7rhMDXP49V;a47}_N46oT37>=<qFzjPvU|7w@z|hFXz~IEj
zz@W;;z`)GLz_6W_fnfzJ1H*Dw28Q{p3=Gp*85sIm85jy!85rDH85n-BFfd$YVPH7R
z!oaYLg@K`$g@GZLg@M75g@Hkx1rnwom>C$DnHd-!Gcho9FflL$GBGgdFflM(V`O01
z%*epdgEWAl00qUVIca(+B?`6*h6?%$U|wlK3TS~tQDR9d15_1;go3StseyrlvPTuF
z5-)#GzmRxO7X@1dGX-UZoW$Z1g{=I{yb^_^)a1m{;#7r#%)I>M<c!RGg~XJUVuhr{
z;#7sSqQu-(g~Vb7gOUu0a=59=3Z+E~#RaL!nQ58H3b~2tnaK)yrMXF|Ma2qmMa7xv
zd5JlC5K03==>>bbd-{dwK%8D&l3(DFn3s~1TBHfrrmRqsky@0hUaXLqQ=G4mn5U4K
zTvD2tqX1H^ker{Jo0ylPqmWcuqEMWgmy(&6u3%8CP@Z3uU93=?uaK#ptB{vpqL7@C
zn3oQc%q&4Q8sh!pg4EQMEQOrh#C$y@`OGwh%KTD=tkU8Vh<%wQAm=C)6y>KECFUxW
zXO?6rIF}UV=sN2tWR@tDXXfN6fR?Fb=9Q)@6qV-XW#*+bzzqaN6gc*ZN>Wnei&B$Q
zGs{vHY!!k+TwH@7JkPw6)S|S+<WvnMLnCuN13d#hLnTCLpqc=(sVF}OYJxLlc?<y)
zQgez^tx#M9G82K5ic%A^@yIJ+D#%QO2)gAYrlT4SRtpkVu!Tf1)Wtse`2`x8Xx=JG
zECMC*lFZyxJrJRRkWb4mQpi-u%u^^z%u7$z0Ec02VnuvrNorAINoIatu_nagl+-j(
zu!B`Vg_M+(z}_jz$ShXKELKR#&r4M(%S;4EB;0{uC7C7a#b9A1XQ~$~6r~my<mVM<
zCS~SimQ*T~<U_oxkeLRG^Tgzg%+#{f6a}#E^!&t}Vn`AKr`HT{dIlQ`$q@?qAYCwL
zLsX{ZE0kpBf>LKmKFIkBi6x+%>rn-f*45PoJFloT4@`r61ZIF@Ewv)E1SzCILg3&j
z%1_EKiO)^UOH5BK(gP<rsIT<EiQP-1EHx)SIX6W|!9dSYN5N3fKtUg5D<afEW<eDx
z*ec`|=vAc_<riz1AylBni=lx5G|)j>3yL!HN;K4Axzf<U08~b#reqZ>q?P96fC2(+
zph99^iUP<RnR)5z5VJwi2+H5k(f}NtItrjNqcm3sQW9xeF(`v<aVRcHEXhpHO)bgD
zPXUJtD0o0J%HaGDic*LY9Z<GS%1O;tD9+C*ONA&_R>(;ODM?CINX{=RN=+^SMQeGg
zLULjr#4XBTdlPdMAW>PQkdasnN&^{*WvQTeElyQ1Qb;UHFU?KOD=AjcNG(gvQz*&E
zFHO$?Nmr^Dr7D0*pwi-0NI3;|q%z0?^%4cJDWC`gm8@x*Ma3m>vvlCaaY;rhq#V^n
z0I(@YVFc!bybfkS{9m4`o|B_coSLdoo{^fTP@bxgo>~HOJ4zv#nTF!dWKdF1Ni9;y
z%*#kE$}9mTOHd4f%lE{b%qoxoI5XuJDHNAvC=_SrX67Uo=^$BLl95`Rs*qTes*sqI
z12SEq7*u&^<mMNb<Wy=xk~PGQ#h_r!ECDOb%u`5INXbkF<><tsN>B>POGzv$N~|mf
zCz7JnlF}kj;3gs$;`wO`$@!&4#i@|M1Us!X599+-xPi=%FUZL+DOO0!NlaG&IXWMd
zT0q`_IvvU7i3-KJU^i#xrs|ew=B4D9D<l@>m*#=;H?q4kOF%KGke`=RsgMM+8dPtk
zD1hoJlz4+?I)$A4`~ryT<iw)XVui${{8CtDl3JXaSE7&PiV%Mne`tDBNJ%V7R47g?
z1GyJux*noF0cD60UZsMv4%tm3q!2GCN=->k%gjqn0kvgNnlNxxV7n5F(u);rL7B7|
zRRS8(psG#57TgFZ%FhAuP?e#S%L=v%D15k5Wra+Iywubb1>}NT0aQ=qqX@w@fZc{#
zB|=@Phbjuz0V-eP<1_OzOXB09g)53XbdcSz1NN+@6+?1PVsWtow3z|&nJ=Vvv0?y~
zJCNECR5-#5Lmh>r#B6XOnG34C!L6mV(!Au7{QR6^y@JG|lFY;$JqADj5LYX3Ln|HB
z9s*}zP}VI<O-!lO%}q=JmCfL#zu;V?$B>hmovMK9EggmOjKmU1W+}-B<(E{2)RJU9
z22W7wo~QtEJFI=1S)!1gnp#k-ke8pQo017C_H!zAp)sfn%Bs3Wsh}0aDGCLNpmuLb
zYEdzRXP!bns4)s^U8WR+8hp^^LQ;Nywl2s(P#%Sr79bBmTdGiVp^gvE$xP2E$*ELG
zOUz5oOf3S{N}$Fhto{cT(TNJsmCLZQ2V@$mF^HM~&SJof8gR@oWabv+7lA95g2bZY
zRED&o{9FZ$b=7*H<<olLbdXsXpI(#+S~{%&)f)tAPGlCkLqzeKke*r+pPQeOngi1d
z7hq6U0GDjJ&{UyNkeHmEn4Ss>&}^71A%z-@mz-ZviPts~+zZZ2nfWkhfm+?3{_y59
z#1^m`Sj!ydcP!0wB-OBZK~fEk2!z?nP|5Vvg4B|de7)rSB2cHQs4TSzViLT^rB{|&
zTnegYQ(^9Ohv*4|i5J1b2GmG~vGYoE3o1cvh`a)r5Zp+Ra6x7sEcBCeQWJ|5K($Y4
zUS4W)YH=~Bu7x<Tm;u!PU&_qDa1`4Aht1Dl;%8u3$Irk}%+J8!$j`vQ&(FYcfRBM8
zjE{lAnva2josWUxEiVJZHC_gW>AVaKxx5SvDZC5}F}w^6!MqF%p1ceU;=BwD&v_Ua
zmhmt!`13F@xbrYD81pbN@bfS*Jm6+v*vHMlP{Yl@;Kt3spvuj_aEpt9VLlfFLkbrI
zLl74OgB%wF0|OTW!y`@xhDn?Z41t^s45rY&z5pi!!#55FhNm113{N;1818T|FkIwd
zU^vdfz)--!z#z#1>Gv;TXJDAa&cM*b&cL9}&cN`7je+3`8v{c(8v{cu8v}zZ8w0~n
zRtAPmtPBj9tPBj%tPBiXtdQ_n&%(e^$HKti#lpZK$HKtyl9_>F0y6_cH8TT495Vxh
z9y0^OFD3?t156AIE0`D<8krav444=ggqau^zA`c}>||tMn9s<-kOS@GgIYi!3~KzM
zwc)|(5Sp@(8|)zE=os9I0hjCOGQObhh+auiVqS3$xSs@R$Ag<x8X&p&oXp}9NIMr<
zZ)Tc8s)DVpf>M6A5~O2=EDK^o8uzJ~S`m^;AsML(nRx}JC7^B>s0o}2YP+Q>q*fH9
zCYPk9D5T{V<tCQsDTEYNDkP>SX6EUkb#+08fiS3(TwGbKms*h=pP84Ij}};9B^Al3
z1tr)WtDv9&_iS`#tb(mVT26jq2_gidGh?y)1g(RUlbVOnq^XJBHn>#^wqR@ZiVJcw
zOElDVpq(3#YmqU?bD)66h{2MgN=zF-rYqP&!c{{lASV?xHj@e(H2}A>OA?Df6-X+m
zOO=+Enp~1!q)?DwoC#`DgX&;VMN*-MY(0n#Zaow#*n$Rr^fL2GQqw^LOW>A)9=Irn
zj?ff>6vHqfO)zyJnvmKYs0ARkFieylm|_r(&;a7WFhVpl53k>J6be%mv>@XYDQKAj
zGzyhjtN<E1C`rr)HQ#e`6f6`{b5bF_zWg+h=Hkq}<Wz<7R8W_%xCAs7l$i$#Qpi{u
zJbp2UV8Cv~mgNgmAYF8@g2d$H)Ewx@0W`cY-3jj8XK56s=qMB?C*~v;#pk3Zro@A`
zQpG1GC+mQ^73rk~@u12hK0hxWI)a8s{+Vgu?kd<ucoh;4>gOO5Bg9#d!X!B-vp@qP
zprfE`2n#__^9<|=(AXt}gRnzcK?5{k2O3*T294-}M)tsMKTt<7PXSy$A*;?S&;wZm
zDo66OA){_dpgvi#LQ*QI%?0r(s70oblAl@(X$xkSC=_Spm*zk^`k>?haixN-LSag@
zm02u2LX?qvY(=TyrYJ;fo_YzS9bS^Jke!!bt^jVFCgxR^Csrb9heUZsCV2P}l*Ljj
zN{SK{j1&rrGK+IT;}eh(Q-$J;)V##J%=E;(Vg+z345S!Z%qx@^fyT4I#)2B?pn;RX
z6mTX4B~55)18Qo6`~qpv=7GA^;I<jGEe;7aP(KmLPnn=mt<1diVuh5{0#MPA4=IS@
z4RwX`jLhT=aEBc_Vg%{PYGkB>QZ&esU}sh4mx9uEKB%8wl$w|W>AmQH0t_S$9q?2r
z&d*I%$V)8;4Gom$B!bGIq?|;Bq?|-ONC0LQgOXuN4s;MEB{MB8wJ0?Y)E@>r6WsLy
zjkBeK2X&#&2E}xK9%w8B+zN*}24-m@%(l$@JjhrCxM2)-HzX**Oi=iN8K6Lf4k#4D
z1&{>682}-JBh!KkO@tzlc92M3fgU&wYZOA_10=4j0P4>qLI)8uixm>V;RNos6qhDt
zr6!kvdOwAr#$QRLLSkA;Y7uBu1nxH2P!)JI8a!^4lwXpekY4~Q+lxV!DQI{HJYG`>
z$_nbmkb&z&1&b6&g{J_j?cwHu90kIKdZ0~`c_|tOdIkvRf@DA#zpOIEiZpOp2O9q_
z$xwg{>VbPo<(b8)DE487B8na0#I6VJdc+sx7c1y17@3-CDrhMf=ouK9;&C47T0v!c
zDJUW4fCs=7(i00n1q|4tT<~~Pc`9VYCM92?I8&iCFSDc=oG(zk1o8mLs8pB%rpdWD
z6zCh7nu4q-$p^(0Xz(~CGdDG_I5R&lC$$(co&YiuguzJ=IW%k)pgcWLpDMYu2$X3+
z;RLSV(ON~BX^^@MG-3e@Q$!ICG9Q#w!FGVF9%!!zWV9oAiX{-qKti<wsK823EeQli
z16ol98VJ_`w@#q02ZaH&(920pQ7BFYb+SuRb1Fe2w&khnDey8EG~xjow+EFd8K9v!
zaLEf949$l`8@L0g1JMbZt^$ql6{Y6pmx0D*GfPr)!K1aHA`CLe1<C`Efjm$ZRFaWe
z2^v=cHBS@44JNQd^UG3;K*N9_V?Z@LI6_N{QqmMkic%9xied2p>ROeQ=cAYN#X1lt
zB1#}o*nkZIl|P`c2Vt-PhyjiSP|GepwWuh+2sUzq5P_?N_MpMlP-d}0a!zJ8XjC0M
z^QNN!Zflo-+IfizNvS2E5~Ls}F&U8ul@%Zs<YeY%mO$JHw;xhTE7&R+=$Rtf2N|8o
zRLDq7ffU{)8K8nVu|&ZLWDz)@D<tQqAS?m<24Y$YXwFj49Lc!Mlx$Ezn3GcpnY07X
znn2xJl8-cmn*)yS6osVHqM}SlXF;JjF)bBTHs+;3Y9580{JiwkGSI9{QEFmwejXwZ
zDJ$eARwhB}^72Gb)dDv>HCG`cGd%;G#gh{Yz!6@QS`00|zzryc;+)Ks)FOK%hl55g
ztUzkvBio?SY=z|1qLRc+&?qp-%+w;d4tOYlDs^z@3pQ?uZA1<f3m^>E1<sC{`Fh2v
zCBdMH$zZT!G{6}d?7Vo0^AN=$G<$+o;A*sijYe)m!jc)X7`Xp`gO`Cpfe*6(pBuVH
z05tc%m7jqjfuDha3)=VZ<zrw7;$vVi=VM?{;$vV?<b&uF<YQm}nX!$Rfng>u149%q
z1A`ea1A_`L1A`1N1H*S728OFV3=H)=3=E#od4EkF1_nhQsAZ$nXb6mkz-S1JhQMeD
zjE2By2#kinXb6mkz~Bf0P%i};qfcHD;wvbitYAQ%+)B<(ftieM3VvOL*02#am9UDu
zk|LNQ7>(&jOc8`<aqB{;Day}53lO;1@M^~^nF2aGjj#&v3JXlz(KI4fA3+CC!E=nL
zy<V`8f~^9WhD#}f7wMLOmTBaImOy}(O@W4G3W`$GQj3aG!NUuA={leZ6v(nQB(0!P
z2>4>&l+@G$(82)FI0SekBtK7~ptPtUA8oV|=5)~VHQ2N|L{BPsW)8BFP){K^zo@8E
zPr*4KyauB<KR-_o$6Q@fszO?31<0xS3ZRA3VB5jV_Vg4SL9-aJk!#orVd#`6q`zLQ
z5CiuT$ZbW5nZ>CJe)%Pyp!4lO<4mb3uAl)R4fTM;l8j=|_zEb5zynVx3)ev7sPI8D
zgrOi|)FD2|JgpMgotecDUxUI4G-R&;UGZcUlc%Hui7ZWU|6h=mf#D-)1HJ$Q18o2O
ze|`prfBXy#-}o6A9`Q3UoaSd>sN-i~u;*uB5a(xLxXs7FFoTbQA%Ks8fsv1aVIwaC
zLkTYfLk=$kgD)=wgDEcqgFG(-!(AQ*h7CLn3_Z~Ke|a7ThE32p|36#|3{6}N460lV
z44*j}7^ZSEFl2BtFxYc4Fnr`-U|7h(z>vbhz~IBd!0?Ejf#EDW1H%S(28IT91_mE?
z1_mK^28Lg33=Egq7#Nz^7#LF77#Q5y7#O(Op#B*gCFrOtMnhmU1V%$(aD)J;B0|Tb
zD~c2p6jDc56oDEah!sV;so)7RtSgEtbSm{Kb*l8LK=aVKi6vGFXiK@uGct2h6+((i
z(RLpxgJ$0nK?@LaQo$=^6-x6;GC`XLAlr4oD^^orn=^6{t1dukRvFyFL|fp7DW(nD
zyaJh!k2cUV05z!%bQE+A^bFCKeSzk2A=V~>CaH?^^AeMCQWeS*!MlD^@<CI!CHeU}
z(77e>vBaQe8%P|qTo^I?19BbMAqt?i;02X>ur{lPZ%KZB4r1m8q)-{W(*e3+4zwr+
z)`~2tEC4N`LuoWZW&o|gQ=On`QSfveWN%AiQck5pQ9&gr4ixf>GE?(Fdt>tR;7dWk
zlWm|#1uYvZQGo17LNmQ6H8(Lc53<Mxq!c>KjBFl?4v+_+t<2CM(^Q2J=Kw1u9R)4;
z8VK;b6L?AsV}?1zIUpdvI8{SaQxCkyLc>H8HDo}B!>|_EAkcOQy?o!yXtP-4c!%rP
zQP3*L%+o6>sMO0%EGf#Y2+0pBs02?->mlnc%7=svKAWN50<F}AbQKg*^HNZ90ccxf
zelB8-7o;hTXpqC)4mHUGvXikKygmmMxZufa@N_t2E<dMIPr)^>xD>hw3B2S8yg~$|
z7|9)(#R`yK2U;!z*$1`D6SBr9Ek7qGzdRGvuLA2(Fos5ZF?hL8g-)eT6?}!D4tP$q
zI5jyxFGax^te{AzK&Mh6F)uwQwHVDZ=%gb!T5wG|A~`5BzZ9`$2E3antr9e!s(`#c
zwFulZ%*!uFUJ8nA5qOFd)|o>pOTgI*w#^V8(O|d1_PZh$e1TF(T7FS^Vo{2BW?m|I
zg;p^njUX>~1R0})C_4&~5~nh>^91fu!Se%rK?P{KAG&%1v@!zH(Q?mE%n1PPvjy9P
zwnzfnQA`Fc=Y+0og8330d7w4(p#2J<1yRMRkc|lN<u#y{44_pOkX~KB0?LFp@+u)c
zh5V$l%>2^goJz3T^n7qx0ZLuaE-KhB;Bo@I&Zs1{0<@k0w6FlQ78bMw4zY;>wEZE!
zBrzwaQlYdMR8s0dmIHwnQxumLq!uMX7FvL&&J{q*QbG1Wd<9xcke3d2LrP`}sH2-%
zmJHeo0a^zIDSV&~0`L2Um;*{oAg_XoNU(RoX67rDWu}&cQ*L5jVoqfhc*ltXXq_-<
zd2wcb9we`TeFicD+!+Ry=^%5`@{5uo8bAeUHfVStDK#y>2qKo7588JHUCjZ~o12(d
z3idE`VRH#+Ia(fQ(<W%EFT$Cbc?#fg4-HZ%NiKkv9-wgu@Fo~|ZUMU*yq2pZBQ+PY
zAVXKzP9X`r905}8K=vJia|&od2&l9u$yX>zh3>2?Elw>0IRffp#1b2@f59tIPzbOv
zT7m?NfmWt4fcyV5IT;u{L2K;=7#LvZ`+@rWpfv!Xz5f~f3=IDK3=FpX3=Bg23=FUN
z7#Mc)F)*y=V_-;tt_R@bV_@LrV_@LqV_^8r%fRrOmw{m|F9Sm-F9Sm;F9U-tF9X9v
z9tMUZJPZu2JPZu6JPZs{JPZu?xfvLCax*a0ax*YEa5FI2a5FGya5FHx<YHjh!^ObR
zz{S9j#07DmHWvc}7Z(G=0Zs;nQcea2KTZY)V@?K!dmIc5GdLI++Bp~){5TjG<Tw}@
zm^m02cCa%r%wlI?C}(G2h+=18P-SOe_{qk=aFUIIVLlrJLp>V<Ll7GSg8>@@!z)$>
zh9#^F3>mBp4Bo5^41BB%3@=z17>=+oFl=LCU?^c>U@&4~VEDz%!0?lqf#C-;1H)xz
z28Nx?3=AdA3=F!=3=EH$7#L14F)%D;VqmC)?)8^tVqmz%$iUFg$iPs7whsVSwI^q!
zCTA;vM#{hu14?(wpiwdCPSWDUN>DNdXVSuy6r|c+A+s2?VimTdNkJpApdb;nMl7ci
zQLiQz7pFpY0~PBbFa1!kQ83gq2d7AIA&Vksr(mFGfaDrbQikuX&&ez(fVc?06f`Zf
z0wRK>2bA5QD}6vE1bD%3ZmL3ZQDRl4LUBoHS{ht2xZI}Ai3e~y!25$ifeg-C(Di5v
zDXE|hNU25Og~Xu1gtx^qGV`*b+ct|!lQR^OkQW<)!UMAWyCehBf-KF;%quC)1TE?W
z4QYbgYoPKEvS6w#u_zN%Vk(s6=Oej@?6VjUB@a00^5VgZ-9Rgj%#e&P#eGZzVq6!L
z+`xu_+iaQn3XlT|khV$@b2b6Atq0XSaHku9eF7~M^%6mAY!gdTp;a@;ryz{n>`%(c
zPX^Zl$%#2R@Sp>!0$~)>lJi0Pr%E)G3@wfH49zU`4D}2RjFfZ~%#4jqjSv|Qq%kuM
zyo(b1I!v&s_(B`Sd{l!GdI^R&QKrE10VLAVH?pDo72%fD9B3;ITy_*^=7JV}XXfXj
z4o8Ds3u+~ST3~spr6om)IpEe1WSa@XI9PbWYf7{o2w;N{w!oIMAw=L&r>p?l=b8!G
zQU}VW&}akadeB~%BG7Rgh`bDnEwJg(L<35}D4QRV5)aIkFr^^lK}8!l3~)IeWPx&7
zW|dw_W^qAIVx>kQ+Olh~WKMo!N?2xUd1{d_XfGv_rEvWqe?gRBkyWr&z}Psb0d3qS
z=jY^Pf>s?v_G#)UK-y#w9wH9F7J>GO=oROrrWR-zqe)@e%&1okI>jTtI2E+B5#3dw
zRua^z%&OEP$OZ_|%25RkP@sX%Yys67i6yzH78N97SsM*<9KP9@V1jcxRG)o;r3Of7
zW>z8fPC%{&wSXZ(3|YwuSBCH{INd=b3CzZbDX=I+C&Vn+Mj+5B0k%l<OUepxz2GVx
zqDKL2IHc>B2inbq;bzdbKexmjq?t5j1yEXq=!EAgkUPMhQczF;6`!#49@2C2lM-`a
zG9VhHQV*0Npm`K*1gKMlwgDZa1llK3R)9|sg3s+qO;G@ku7Hl_09V+>si0-uxu8~h
zVnIPsenC+t>>wm?tqwV%BR4ZIGdC039Z1a0FU>0f4YcHB=A}Z9Xn}5D03|TcRtu0U
zsES7Hivf4kLAkoP1Y{Ywk)8-TMh3bW9(n5xWE;E!c$O2~HiI@L!228((h`eHK#ls$
zl+;A<5gf27Kk(9E1<-ywJqBG+w+LiBs3(*J9?k*f43L|0^NT@Sk;-+!?fo>+;7$s7
z!F*0BX#WRj#|vb;IcNx|G!K4+3^?*ZhJZo~)Jj0wpAIYQK^xvdBXG$X&}})zu=8k&
z^TEwfP(O`90c=zW=p>PpqWl8Tm{(>Vs3!ty@PJ&NT9gXvoFF!@gZKJp=I4R5gVv@q
z=z>|`eNG@&5$u?OoYW$uT>v1z<QGA@lEoSM<*A^y6L^P1B6wU7w2>(@5458I($a$5
z2Zpj^2HeZaP0RzeD^m-Kbriry2BhQ`fI<M&g#-mDI4VGUqw-;Aoq+rV>gIyN3Uszq
zK~83JB50>7sBf5F%%BSjR&WEaxCFFs9URTzesZD$(!FB}MX4#J;8RQ>=MFF^K;jjA
zJX8{B18hlZUP@|;LOyhN736#?$b>G){>&1PJ5f%d01w81tpfGMOF%=Bp!5PCF9A6n
zw5h1LxHLC41r)l-JF>uT&d*Z-kI)t8gLfV=pmYm$bs364)f{yH7bG!*^C~D4gD^NF
zBb_*-;g_G6s-plR5hWP35dc;T%F%lGYC-5om7AldkCg&w7yy(`i%XM}L8mE!=A=QJ
z2oTHXbrh5k9Hg-*oU`6wql!V(=t?>YnJC&7k@RP#DP$_xKsL*u+Nhvwh}wPug#%KJ
z2=-=5iGr>IXbT%66hO)_T?G;aWjWB{U??FAl7NOqZf0?DYKlTiYGMkgoKi@tRMG((
zuAmEXAo`9^*e;)x5(OKuLR1HX+@)X(8n*-)0IHl(YzA!xv;}2$_>o(n{r|UE85j<M
z4)_;fV1S+9e-3)?Uo}4iLmEE=gA6|d!$m#@hE;qF4E=lz3|4#$3}1K|7#{O7Fs$Nb
zV5s6{V2I~sU=ZPDVED$vz;Ky|fnggD149iD149501A_t&1H(6N28L_g3=Au|85r8R
z85kV785n+ZF)-}sVqjRr#lVot#lT?A#lRrU#lXPI#lUcwlYyZTG&aG>z~I5jz~Iiw
zz@Ws*z`()Dz;Kv@fuV+jfx(c2fkB*uf#C~ueZT^C28JAV1_ljw28O$AP=5`MnYU3_
zjE2By2#kinXb4a>1VFVZ2oLmVs_p1-YR>3zDkvpFT5k9T_y`{LN^o$Gpb4OsCv^^s
z1-E*kLteOsR6*VbjSE0}XrQwnMu%1LhCYtLS?Uj~f@2R?uM8|%3?ICO4%z4-MBu##
z8V{?29Rf{b@!*3yL8CC>{vqf<8C>oMC%e(H)zPt4*kO5yJ~=o=frme#L#FWYQIvsH
u$T%sA5NH{Q0-eWJVG~7=v=2FN6eb4GgJ3Tq@+O|#3Ra1lX<({RMHv8IY$68$
diff --git a/python/ur_simple_control/dmp/drawing_gen/draw_path.py b/python/ur_simple_control/util/draw_path.py
similarity index 100%
rename from python/ur_simple_control/dmp/drawing_gen/draw_path.py
rename to python/ur_simple_control/util/draw_path.py
diff --git a/python/ur_simple_control/visualize/__init__.py b/python/ur_simple_control/visualize/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
new file mode 100644
index 0000000..a1cb997
--- /dev/null
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -0,0 +1,28 @@
+import numpy as np
+import matplotlib.pyplot as plt
+
+# TODO let's try to make this general
+# make plot_data a dictionary.
+# every key is one subplot, and it's value
+# is what you plot
+def plotFromDict(plot_data, args):
+ # TODO: replace with actual time
+ # --> you should be able to extract/construct this from dmp data
+ # or some timing
+ t = np.arange(args.max_iterations)
+ # TODO make the plot number assignment general
+ n_cols = 2
+ n_rows = 2
+ # this is what subplot wants
+ subplot_col_row = str(n_cols) + str(n_rows)
+ ax_dict ={}
+ # TODO: choose a nice color palette
+ colors = plt.cm.jet(np.linspace(0, 1, len(plot_data)))
+ for i, data_key in enumerate(plot_data):
+ ax_dict[data_key] = plt.subplot(subplot_col_row + str(i))
+ for j in range(len(plot_data[data_key])):
+ ax_dict[data_key].plot(t, plot_data[data_key][:,j], color=colors[i], label=day_key)
+ # TODO maybe write a hack for getting the legend to work (all lines are the same color so
+ # we're not using it as intended
+ ax_dict[data_key].legend()
+ plt.show()
--
GitLab