From d68ab93878ed8c7bfbb871ffa97b70474ab9c76e Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Mon, 27 Nov 2023 20:23:38 +0100
Subject: [PATCH] there are kinks to solve, but it bottom line works
---
....manipulator_visual_motion_analyzer.py.swp | Bin 0 -> 77824 bytes
.../visualize/.visualize.py.swp | Bin 0 -> 12288 bytes
python/ur_simple_control/visualize/main.py | 16 +-
.../manipulator_visual_motion_analyzer.py | 773 +++++++++++-------
.../robot_stuff/InverseKinematics.py | 31 +-
.../InverseKinematics.cpython-310.pyc | Bin 4796 -> 4919 bytes
.../__pycache__/forw_kinm.cpython-310.pyc | Bin 11484 -> 11455 bytes
.../visualize/robot_stuff/forw_kinm.py | 16 +-
8 files changed, 507 insertions(+), 329 deletions(-)
create mode 100644 python/ur_simple_control/visualize/.manipulator_visual_motion_analyzer.py.swp
create mode 100644 python/ur_simple_control/visualize/.visualize.py.swp
diff --git a/python/ur_simple_control/visualize/.manipulator_visual_motion_analyzer.py.swp b/python/ur_simple_control/visualize/.manipulator_visual_motion_analyzer.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..ba65be11a0473d207f62e43b83c0564e7d9518bf
GIT binary patch
literal 77824
zcmYc?2=nw+u+TGNU|?VnU|`^WlajhBvW9`Tk%1vSzqlYjC9w!3g%9T@7H4PX;Zp&T
ztAiP+pOK%NYNQY6=9K28=ob_vR%90ImlnkrXXX~<q{b)b=am%Y=jazymSp7TVUZ}y
zEG|vV$*fA%&rQtBEGW%MEXgm5hls}K=9gsV=fx-HCFWFCr55QGR6-mvN{)uWNDP6}
zk~CclUIt?$Lj#cam6a3~goQ#u%uzfV0;3@?8UmvsFd71*Aut*OqaiRF0;3@?LPDUV
zK#-xHfq{Vu>fZz?&4@;`Liz4cIuT04)CocPyij^2l!nQ(LHQ0)dMlKM$+JWGZczFw
zl!nQ3K>2o1+8QbWr5HG&;#N?)5-I_u7`UL~)=+vgR02vdut3Evp!7Sa1e9XnhKf5u
zX*Fno!Q^?M;`~rL15I84Dy|NtE70U&;Rf^nJT!Ses6LoIPtfFH_KHB&eMFNNgo>L&
z>3?YQ%usQCC~W~1fcXz9JW7p*z-S1JhQMeDjE2By2#kinXb6mkz-S1JhQMeD4ABrs
zOkrRUXJB9e_lE@-7-0SXQ~V4JllU1Js`(ih68RYz68ISy4EPxsnE4qP9`i9UY~o{J
zsODo}2;*a5aOGoQkl|xskmh4xxWmi9u#1<0VLC4ZLlrLrLnSW*LmDpwgE21yg8(lB
z!#y4bhCMtC3{^Y~3>rKP4B|Wt3?I1}7!GkWFm!S=Fw}4}FjR9hFjR3fFjR6gFlcZy
zFx=!~V3^9qz~IWoz~I8gz~Icq!0?-sfnhTz14A|^149-k1A`MM1A`nV1A`<d1H)4e
z28R6{3=C5^7#PYq7#KV_7#P$!7#O~=GccTIXJDAk&cIO0&cNWu&cI;G&cMLP&cJY+
zje%h~8v{cl8v}zU8v}zp8v}zJ8w0~%RtAPYtPBi4SQ!|evobJTWo2MEz{<eT#mc}?
z$jZRrz{<eD&dR`Wi-mz<4+{fB84Cl08w&%&UuFh|lgtbZwag3*mCOtb{>%&vGRzDN
zubCJaE-*1LtY%_h=wo7F=w)JHsA6JZ@MB_N&|+d>xW@>Ii%qyY-U?8ltWc7Xnx~+b
zo?4=zsgPEbp9>OD$W6@5Qz*$ON=-~*fT~nbR>)1POiESADlINiC{N5RQ7FwT$;`=7
z$ShGWR>;fG(@o7SD5+E^&n(GMNX*MG$w)0yNXp4i&d$tBS4d9G$+3r<m7JfOo0ykU
ztYE89oSKuSS6G@_nhJ3QT#a%;QD$C=hEjQcQFdw()HMnz`5@DZOG?wylr+I=Q&Q6u
zic0e|Kn7`AF@VKDZU*u7;*&E{ld~0U6(9^$$(;P;#GF8o)e5!>2o9<uu+tQ56~Ht?
zs<a@rNW(p~#2Mm~5O7%OfPJH>7ayOQmst`YuYqtOTnOYW9f)Z<NS5kAU8rfrkeriP
zT&#d@wgx0RGV{{)AeL!bF(@f1F(@b`=A|f9=9elYC*~;>XXKZqDkSDrmSkk+rRyl9
zR_G~|XXNLkDx_p4mlP{x=9T1wV;Jlmg{0Kv#M0ta1<3Lk1_gzZjKmU!)UwpP5{1O%
zlG4PSoJxiA#A1b{N`->_0#MMy(+JoEJqBH{aRvFAc_j*&#SjBPu1QbLOD#$)0XYV0
zU|K#X7E=|{OEY!Ajs_>a^wbiC@{Gh11_gzr(h`M?)Jh$N%n}7qtQUh-WtJ!;X6C7v
zC}fltm4IB3m!FiMQVFrRC{-b`C{@7-5}?I!4`nK(WTt>LW#{FWgUwgS1bYOMelqg&
zQj1G+Ds>oib?qQ2x;Qm2MWIjuo-#91i&7bM6+--7{H+vn6SGrcu2O*cBQLdFA+ew!
zCo?$_v`mOWK_N2@91-P-c_j)Z`EXrHsU;<;MLG&esS3H33hAY(#U(ll$@wW@BTEYy
zbQLlc!0yb*FIOnZ$Sj8VUPqxgUm;UH7nHj5KpqF@tjrP}1_g!k)Xbukywu`ih5R&y
zlI+a9lGGwS1qNM(qSV6D%%ap{h2qqLL~!^+(hbNxiOI?Nx%nxeNGZukRVXb;NdyHb
zgRX*~e~7CUh*Ss-c6Cv34Dn^qMF$M%DH5Fgz&WBQwWPEtPa!j}%sVqLH$EjXw;(ko
zxUe*_D76@oAyHKY1_Wp3r8}18XQucj=4BT6CRTvW%1l#$F51dTEsD=e%uQ9WwN+4p
znvF%5k`-7VD9hut!8JbH7p#hC(?L3jG})76lRfd6Oq5|b^~VPkr<SJVd*+qJXC&sN
z5n&jb4%{ZA#wsYd@dqYqGQe#v+{Gk07Pom`iOKOH#FzzEh|?UTl;#?rO@16Rfa<4o
zP(=Y=iI|gGq=A;cHLVzwq2)|bYA&e!29*K&nYjh|MJ1q82VCSP=VWFxFep<8FeroK
z4;Oa!_X`Q~_fZHA33YSBr3fUhOf~l~z=I$yGcU6^L!rDV6I3=Sloluyr52}x+0d%1
z7*r+{mn1@JgM$3xOi=NwppgZxgUS+fN>ht98I&Q#I;h<NuAz!^GE-8E6pBlVKt(B7
z6{t)ERX?D@FuAm-2vqAqm4W3Mloh}Y4TYS{>{Nx+oXmpaRE7LJ{rt2vP|*yoD?wFV
zN<OHj$xBU50htJm!BkLnR+66w5&~J0SpxSW*iGQlH#Htoai*2#C4)TyF5+>yM<Fq-
zB((@$ZYzMg3(!<jl9`*Tz@Q9ii-B8cpjs+DwFDAy8L0|s1;q-W_8iFf<r$edsS5c8
zC7HRIRhfC|aF-ROrX?nq<QM5EWF(fQD&*xC<tFARfC}MaXtkG_mtM@EtN`&ctfGTy
zQ>aWW(E<5eAwQ{%0p>Jt(+FY<sD@8WPt45I0kujLl0g-OjzVHi4!9Zv)qI)73Z>~e
zl?sW)3K^+6IiRLYd19r4hI(>-F~oOiMfrIpx~X|7nxIGqc@x}pgEbJ#GcuDi6mk>u
z64O(QAq^}@<3K?J)Ib8&!HFrU3eY;40i3sTOLIyx3ySiSQ;Um1;RAI!xM|3stN@Bx
zNZf*22c>zS3O^On&ILtPdTFLYW*#VTa}+@F05LN;KPM*@9I3?$Q1d|Hm{L+&kdq3I
zDM)h(<dotPh2s32Qg8|Yc_>k#0F=6uOLG#76bg#+lX6mXVZoVKnp*%4p1cAEh)tlz
zNqkXh9?TjLKd3YhB3YE5lwT4LPR4qfd1dk0nR&S|g<6<OQi>AGkwO5A3eUW<)S}{4
z@65c^+{BX1WVr7zBwX{#;4UmF$jL9s$xPBqFUm|QE=Wy=xxu|CGbOknH5ul;g383w
zlKk}2Oo-cZ6X813GSf?oQepbsz&s2^Nr}nXsd*{!Nr}a&aF=DLR>l_;r4|>*XC&sO
z<fIlcC@Xk^mWStpay%#sQ&Q6sOLIyTeBst9WT#dtBxUBMfD*hO%ymf4DX0W_36xF?
zazKq&1qFzcoD=iP5{pB!9n;fw6#Np)GSk7W!;t*^oTS7eqmXP(T#hc*gSoyWJ25>S
z=2H!5Ai`RCB_-J~DQNUSW#FQqf*?5~F)uw8T=GFvpBAWTnU@Y~q=I{FkQ@Nc)|L6C
zMGBcIpk$d}mReMlnUV_0g(cYv<(VnzsU^jb<X&7^T#}lr3kuy71&~8P?Pvy2|390J
zfx#Ix0WZM70Gt0k%Fn>i!q31U%Fn>i%E!PU!N<U`f|r58f|r5e1P=p491jD-XKn_D
ztK19>>$n*hCUY||OyXu>h~#EqP~&D`5aMQFc*DiOaEgn8;UpIW!xk<EhDlru3|U+Z
z3_M&640kyh7=kz%7y>yN7+g3R82)oGFzn!9V3^Cnz|h0Nz>vzpz~IHfz~Bk(zq@lV
zFz9eFFi3DPFmQnS_v{P|``8&6`q>#68rT^aBH0-jT-YK0aA#*=aARj+Fkok3xWmT4
zu#$~|p&PaU0aWmg;?WQo4S~@R7!3hBg#ft2QIwxwq6ZqC%*oFO_wkgAkQgO;a51>5
z0_Y%|vO;1GURexD@W=&*Xg;!mC8<Sui6xn3si0c5I6pTvKQC1w8LP{njt1$*VkQ<L
zBzNPmCbu9bF*C0itQX06a6myC@GwyYaPt}5)<thDfPD!XlT)x&01w1Kw!npf_r>{w
zH^4bU_rGa?LLWYM>6VyNoT{Ucl30>x>zALG3K{DJDN(Rh2+4*zA3RE^V5<ObOKWH{
zWTq*^gGY7Z<H6%P@$n!x#>Xpx7u|uJ4#J>8QazASdXR1s)G)XdsF~`Rml6sZ!Af;X
zOwLX($}i1JK~e@%T98-_2^<h7wIVsSphO`I)ID@9D#|ZH3>$%@K^SanW_EmPUYTBM
z4ye1ApP3S0Tw0VC4{GZbM?)IZnRzLx6|s7dzB-zfpw>H*Djfwd2h=@C%`43X4~T*X
zs`a4OfTfEy5pF3dsze&Y#^|PlotRjWS{$F4ml6*SoXouRcu;SlxTH8*DX}6RX&+;;
zQmh_K3PQ(cr&h-56{nWOC+215CYGe8Xe8z5=Rn*9@pEc?eqMZjT3UQrVv!~!QbB=(
z;*`uZ1tqZ4l|WuY7>)1;TsbI25DGxb$@T-X>7XGJNDn_HH7B(s6*TgknyZkLs*sxq
z$$y}6m&BBmct||OLp=xfDKxy56`+F9PCX<mfIJJrU}u6u7}`rO*2qKz2gqBHZXc+f
z4K`ITFFvy*wFnlQAo~k)5-Y)S;4vQfSaWV-9%N{}ptJ<iD#sX^2M<5PhJ`>w^q_7E
zc;v$|z!N;m3=#ynq9`>nM;FxZQ~)JXP=6mO0f9Y1QAPrrfEo-2h+u#RR7z%XNqk9u
ze0qLjPJB*gUMgB{0VTOgP+0*E4p{OB383VvXmyZ5#W0i9W3gBTPT=w2Tu6~k2y>EC
zi?Q02nwg%OXH1DDN)Ss@b8_OrhLt7e6f4C>Tj>~K^BGu)5l!qf#A%-)P3$wkW?xBB
zVpb|>e5n#!feK0@#wqayMfnA(MJ1W3#e|YdK|Cnla0Mn|%Rs4*XgdsIA+vjUk}ljm
zSOXhc;)5*JODrfz%}ddMj?^Fx2&3j26dR!&XdwfwSs>W}sh9?r2;eava0Y=Y0~NbO
zRzQfsY0&I!G&uVs#0;UXK!_N{g8Tp43=9nFp!sG21_s#r_ila$26KJ}2339rhKqa*
z47Gd=44Qll3}<;67=n2j7=m~i82ES@7%uTJFwEm&U~uJOV9?=VVBqFqV0g~Wz_5&)
zfkBF!f#Dt(1H&#Z28J{)28QRH3=ESw85kNl85oo~85j<6FfiD0FfiD1FfcH2Ffbfq
zXJF`KXJF7}XJ8OuXJB~F#=vk0Iwvp8#=vlkm4RUnD+5CXD+7ZHD+9v@76yhk76t|*
z76t|d76yic%nS_C%nS@)m>3xLGchpiV`5;aV`5+sWMW`=$;iNPh>?LIoDt$zZIFM7
zZ~cPGN)T3702TX)p(KS8$k>oVQfdx(0x%U^l7pLdAobuV01XX68aa>(K0duDu^<B!
zMPTV<@Te1tlwNXvL1jFs%@?1PlwSc!=xLehsER;Rh%#1Lp%^soi?9qlJd~7^2_8pA
zbrCizLG3c677;F|DcE8TmSK0FjzVr?aY<^CE!e2EqQu-(6!&W?z#XLwn+MITN(Ie@
zmnFi+tW!aQ%{i$G&=Eup(0FrkX%eWqEY?J=^x@$R9+!{L%!>!rtnr{q305b8i%Ynz
zV0Ynfkb$d2kO|1mF(m>PD8(w+D#V~#LqLmytwLf2qyh%prk7Yz2C9iQDr^lv^UXQ=
zMYifisVVB9*>_0e&o-o}6xwhH`5cD9o=VHCNKHYsEI~%11c8!*HfT0Q1KbXTmO~&%
zV<?1s0%Uw@PH`%t9S0Ht;iAOM;#38{{1W)EY-$R)k*=WxYTkf{)uF2Zz~_2^25wVR
zAkz)S`9(#Q(6t~Sv(Pbq_kdJ`y-Q9UVi=7pOb`(XvJZq|O*>FgkXZsMpTTV`(AYbq
zwOkCDNCB^rNX#kDhpoqeO#gtE4xof<W*T_yi5@7pAZa!&Covs0q{05gaC0%NHo}lW
zlnWpi!*FswC?SE(%E`>jjtBEm>n&7`80P9=q*PE=gcJZJ&<X~rw#45+)IrK66jy#=
zUtzcxRLVxj!h;(%lYv?-6*-AXsW}?zpyt0i!t<yKK{FrdWh3#44z^Otu_CkBF)zij
zs3fzvq*w#R3LS;&T3oh+oLQ1tl9Q^T?vhztl9-pA3Z9wFPt3vPVwjZ}bq1(c1uZ`{
zj17%Z%3x67!3<W=0Qb_8^GkD56p~W)i!-1s1yCta4(j3{6~ExH#~&cz#x$N%6f6%Z
ziom5bxDW-Ki&}J|tAj@^5o#f67i>Z?bxT;VnMkgW$5oo3D}*~Altn>7i<<30qO>Sr
z!GT3;grU2gJWqnWG+-qtx)<ODD7pk9FM`~QE(<LM!NSFQq?CYQE#QO#?zqD0c5r+U
zE|RHJNFv-&0nTceWh7WdF?g)0q9ijpyI3O{sZR(Ss451}2$NPWLQF<vZp<<fq5`8l
zgb0J31T70;Bc4f_IhiGu3gA8yYIz9JLu6TKgi;oQwU*{2A{(xtQI?-mnwtt*@PH-{
z8H~!$Od+jM1P>bEDHOr-kWM}#$WY5eBqi{&1e}_1Dua}QV0~0B1;J)Q#$0fg4M=L>
z?geEr5C*#&t?dd{4;fd%Zze<y@n%AT6~CDfH7I5dsxlfJI4J1{EJ8vQfad=}d)eba
z`@IDi7-0MRZ}BrQZ02WRh~Z~o@a1P<_{qn>@Pm(m;VK^k!!|w!h7LXkhAKV=hG;$p
z23<Y|1|2>I25mkD26a9L26jFMh9|rX46At=7`k{F7{Z`y|L^lKFx=*0U^vgiz|h3Q
zz~INjz#z=Sz;KJ3fnf<Z149~g?Z6W*28O*{3=DN#3=F<p3=Fbd3=Aw>3=G#f85mY_
zGB9*<GBD(EGBEgYGBD_IGBEIQGBEt$U|_h(!N72ggMncI2LnSf2Lpo(2Lpo?2Lr=L
zb_RwM><kPG*clj-*cljf*clkEvoSDKvN15|urV;4Vr5`xW@TWIXJufx!NS0>hJ}H_
zm4$&pn1zAiGcyCjDP{(S>C6la<;)BWpl}LdW?*n-W?)cZW?*1uW?*>4#K3TXiGiV?
ziGiVniGjhBiGe|ZiGkr4BLl+)Mh1pyj0_9~(D4RPG6!Ku4G3Cql?+;I4{mJbB!ZTR
z<>x_W!XT>h^KvS|Lkgh5Veo3EVo(z;DHXJ`7`)0jEj2YqA+0DC+$Mx4Rb`OTpz&$Y
z;5w*h0$Dy&3^xl&C1ed&Hqxq~ypq)PBG74ya8=-}S)yR8kXN9WScExl4p#`?l>k}<
zodW6kffj`or>1}hBS34`^3vf-!Dd3o#1w25j0_CmQpyVDsS4%bwRuUY3effFAa4|v
z=7Cn16{RYGLkmd*WVJVB#sIP#0@O$aEw~1Sq=Evj)t5*{fWlOvC>3OK3MlOI(u)-o
z;IkoM=YqylKtmv)2{a^~NtFt)bx8SX5c?DqieU^$_YSHR)NM!BSz4ly3F;r0K=((0
zmL?*}gBB@58{f%^CCM4!WkIQsRsMM(SElAEfQD|6wB)81gH}+1mS5(VfR~AbH5sRX
zBOk61>>rfUR>4-Gx)v_44C>h?Cl-TOB2}vA<bW2zL)X)R8X}-MQ}9*=Bpr}QfNVBN
zf$UZQPkeyXgRN44_K@`yiZatPN)$j#{7W)YE7glq!Q)pckcpv^e8?ndNk(d}LVlhC
zDD}dv0XrOL;}z8tV6~`?3{WCY%uR)?=F?DetU&b1lynr75-VVCL>ia?rAJT*fR|@x
z<|P-ULc;~TH$x#MwV)&ew9G(MA}keHI9kO*FTnW6w*GziMGIf<32MGA%r8W2W)
zngZy+Q$#~k8Kfw)q!@VtAtbc)AWK$LD?wvp>LuVsmr1Fh=mE`HgLa4LA&;d&b_9SD
z4k)%jrl#a8Bv$Bwl8FYyL``sh1f@t&xdJvnH8D8@>T{HGCOJPZFEzO&6|tKLl&qB%
z^3#$PAoF*S+y?3O>nIeZmVkC3fifSc6PXH*+RQwKr2LW$h2nzzl46CTJcO?yhC$K|
zY$0!IS!RwxaYiC|xo>`&0=OxYT2u@&FEKB>7`o?0AtyI6UlYZ#5C<Wgstn$S0^2GB
zTEtll-gE+*k;?}at>6T#ke!+e-XN2a4_Zl_l3xlgHH#q|JxcP^Q$ZV|;OP%s#8raL
z0htKuA49g{fU>SaE;wB$r7Dybr>2!c)-6Ch35qDtZmSabRu@pphbHuVg``x-un{<o
z6@xMlcxMeruL7h533hh$@$_?#cXM<O@ehg*_jC#IP_R`n&@+a+4V>Q5)Oomix_gA6
zDOAR$5@Dh#+(fJjk&Lu}tAv^u<m&1d4_8o}T$GxcSDaaus+W-p3L31+VHP8)F3(IU
z$v`NE8>3*W0AFaT2Qn9_1ETB_;E7g~Kpf@c>Eap`AM6?BicK1#%L1wT!CqOwU8Ag^
z06AtDlvfjTQ$bV53JS^yc>?;uj)E<(0MiJq;G78>3WQ7ygNGJV5o;m9TEVlz<+<^Y
zo{*AjZc=IrXtgV-NC?SBDz_l2QxkI#<BAHl;N{K;0l3u=#rgT5Y93?*TtXRC{vekp
zpw$_m-3{POUXl+=>I#{Vm9(Jb3R!jyDtJIcq~Nv<cuyi|f<H44v?@BiC^1(DQf7hX
z+R=8{fh>owRRIMB!m&CET3XrViACw4sd~t6J6J!kB)<sT$`{ZM4ba+kko!T4`AhO4
z8@50j*T9qUpyl$QHSOTF=b#h{s^=h?7t+Yk18pXN<VR3NS_#RqpsZi42ND3sSWbR^
zfdXtV0I2u?)n~b>3gwBFIt<F7{aLP7(3R^7i8=69K~~Uhf8dG^yu%M%O6Vx$<(DYr
zfW`?yy7P;``|uQsvoi}o4HL-jx}wya{KOQHOc89)V0lJ9WMw|c&ES9snE_s{4k}=v
z1JEf7;Ia?8<qd2|q5^13Tv2HrNF#J7B4}wpsNe%{5Cx}0PzwxFfj}2MBr1Rw-RGo&
zoeI(l+Rz9d_Xev^1y@y|ZEm1?JTp%Lt{&7N00kLH55hW_*(E5B1qTaUC)h)vZ5XL3
z5bq!@qG3>0aPoAAY{+4N?63q)qA4WiDdgv*fD#^P4jWR;mV@@cCZ#Hr6lLb-rlvrm
znn70q)T~h`$S*F=Ov<TLD9tMcwO+xC@D*TM5>rxAQxw37Ke1RL5wfFCp`<7=uecy5
zF|UL{7u*5>ZPI{jA^`_6Xqi7`_Xeo;0Z+YvBBrP`541y&K^Niz1-M#JP=fS;i~w!2
z)CH$AP=hHaHL(m76(Cty+ZfbHN-atSt4U2P&IE0{O)Uo1?wH#F(YFWcG7!i^;IKr@
zwt<_(kS)IIMXBkTpjp1u6mSXxMGR;P5@{d?R3d<j7RWwc&=8IW(lQj(4mIUQ8zR~k
zu!$9@#gL8CwzdieRt%`SsKCwxcZ0xd-Si4dOEjw0LF4Y3xryni>Q)NsB^jl;N%5rx
z>N*NwiIUWc5_KyDB}h6?QBtS{ZD|4dRY3tfMh+@>pj;3|gcZfPnK`NG=7aYARDz09
z(5_lkTQbuWiW2iu@^ke-6llY>fsTTqCTMM*0b&>#o5i5LFrYo$n&A0=&}JkXQ2$hb
zfdO_t-&uYJhF*RKh5&vB24#K*hMRm04Ac1-7|Qq<807dE7+&)-Ff8F^U?}HhV6f$7
zU=ZPDV0g>Jz_5ykfguOluiwSZz_62>fnf(X1H(ja28L8_1_pL+$bNusXy2Zni-F-V
zCj&znCj)~uCj-Mj4hDu(91ILII2aggI2agSurn|;u`@8}u`@7yU}Ip|$i~31fsKJ7
zfsKJdfDN)2e<mvfLkueegBmLX!)_J^hAb8a2774#{Q@%s!v<yshV{%03~kH|3^vRR
z41bsy7=AM`Fg##lV3^3nz~Iir!0?lifnho$14Ak!0|O{5VCM_sTOt68R%{ry;WZwz
zOA*lpMOO?{h_+u*384cVd58rY@wusarG^T&3jPJ44M)DId8Lr4H^}Tdx<crD9Fj0(
z3!Ndf>IJz6-3F)%a4v<p4{bZBrh+oOw+r5E2dT*Mn5JN>5DebD7nWFr&%sDTuwZv#
z_ZF!C4O&frY!Z0=1!Q>vWD#sUXjC3i6euecr<Ne?PF65P6lX|QVK)Xc&YcVz{LKT+
z#~0<7+ZrRxCa52qgJA>6CHX}Nlav*FT-`zx0v!EZ6`Wjs{KFM|9sN85^j%#Q0(|^K
zf)Q>81vVx|HVIQ!K>@Z!-WJp;kB9BUjEC*Rj8D!@L8Lvb1}NmGr9tA{R>6Rv+B}$2
zLqbZ+5{ohulX6mFgY`HZh*;i=Fc2L4IMjoWkVq;mDap^XRd5EKCjsF=mqUXJaRpli
zC6I0fSa5(Uf5^5}$bM1u><ChfCF~GR1Lqy28V;WW5xNv?6+H7wKp8|+K>?AYU`<DG
zV-C_=&4hF!K@Af`6CHbxgH%z3d01mVz{k<q)6ZSO)yLJ>)h{Gi!P8G6#KTp=J=7Dc
zdV)P7lHCV)AUII-@=H>a^7FIf!GlSVYzoSpkTo-<xp}srMQ@-PtV&z;)N-Uo399y-
z)U*<8x(##`igD^LNlYq+SPV<FXwFGYNkJ~6GIL9H6hJA?Rw*@A7qSKny{ks5ncz|c
zx}FSbxH?!2QV2qALH9S<7LcDY91arHLvwd(D%519)Vvhk)U-74GEl582WcnDOe8l#
z4M%o1x_cqjKq{zka6=UQ@X!X?iYWQOF$>X=nOlll^uo1((+gHTV3$EdlswIlpvR#X
zi(640rC<wMO9f8V&_W6_Y+?&)-+}!W?2I(Rg0y@TED1LZWD)shk(w{T>fw14yh;+(
z^#BhzL%NDq3g9>?R?xKr^=-i(1am-%Q2}M}L_xvFG15OYM8V$;Z|Q<m<WN}9Ah{fJ
zbdEwnPGT~siv=2e%mIyAf;z7`IgrkGa(*soe>!-&1~#?`+nbY|58c|M06s;jJhM0z
z$@ENx4A2oS;C=84sd*{j;Yo1O4mvp|qY`=Q2fUXap%QZF3}lPF9s{B!0FHLZ;Hw_C
z1<GJ~L=(^m+9-tRfO4RdFreN8TBd*~BgSA*Neem;3ZetuU<0Itim1|36|xeO^OG_`
zryrH2fO^BA;43XjO#!ttKnoy2BU&XHsVIR687xlCD+cW;ugoun&K-cF%P~a3$JH@7
z1bl3edNJ%s9Y|*#DIP!rLMf>^nPs4Eat3s$CJl6C0(2i5C~ZOJ=&ay9@?yxS3~1#y
zXxJwgZXS9NE7&Tao*DvgNJDxAXkm<q98l99O(#-0tE`Y;1eqRz&T?gzfQDa^Q$fR6
zpg;hP8NtQ_^3p-GS;dgCdC*V=+_fm)0;fIj7zt=FAtygwAuTfpmbj6$6qJIO5rYQ(
zz{4?MeUR}t=ukCmP`)TNH#Ij2G+Lio0@=F;nk>oBD+Z0RC6=U?r53@>1qU1?!RUd`
zr~yTxt%4C^@EKVFc*F$0U?&K?P`)U!9J+`VG#qEEl$Q^>Jx&QFu|bjxcs44t7&KLr
z3O*SCG_3?0h{-DjxfZmh13b3~N>7lfF3>;>=;#YjQUGBjw}9JfXlE(7=9M9jL}QG6
z<>WvYF&FDVN|DTBh2rGQlB#@##59GZqS7*Wuz-$h%LYxmWfm(y!Z-sod<7aK1f4_$
zn`}tU14ReuWDLlW8A$peQ$XNIg0vYy1{5oRCe6S_4s@VR58VGZVPIg$fcF1k=g(i}
zXJA;w&%jW?&%of#&%hwb&%p4UkAYz$bQ~axkAXprkAYzkF9SmcF9Sn5F9U-QF9XA0
z9tMWZJPZthJPZu-JPZuqK>dAg28J2X`F%%j1_pj^28O#_3=GS;7#MoF7#Q5S7#KEi
zGBDI|GBAASU|`tG!N9PGgMnc;2LnSd2LnSI^gMvy><kP$p?d^C^Z3f_3=GWd3=C)3
z7#L22=JDAW7+TpF7<Abf7`C%AFqE<~Fu1TXFo>`+FbK0UFr0&)&j&i}e;+dgLoYJ}
zLo71`Lku%y-+&l1WY2&H69WSq69dClMh1qZj0_A*7#SEAGeW{B9TZOZ1_f})ff5i1
zgA)<BCWPz&L~GMPWRTk+n&?G6L`iynVorP&q|FNLm>PmMj9_+5A*bR%)|O+{fYeg~
zxezyojv7JM^FzkHpvIyVXed^K`u@p@IjI^*UWd3|2V6MC+v*zX8R#gK<lBPjoYcJZ
zk_?C?!H9Vq9fka&%+$ORTMz#r&nSPt5Jw+G7tja1h8K$kkmfeXI)y4FO-OK|b`T77
z6bcekDs4@`bcKSgf(e>$kOLgGRR@v5K4yR~-YOvuL5(*fG;5Iq5vvBu<E;{(l{n+A
zlGgE73GEUnf#a=`3h`D+QoO-(0D5|=z!q&t!_8pz$kB#X1Le_HfhXEP2{;&QcB`Ou
zv{j%)TLl%O4dfZ54j)O`4YDFL6*BY>8T7#xeFS3z5!SF|fl8wkilEi?khU3Y&oZKn
zLe&Q2(P=mh78;1*CWOPl$rh#n=YSbl6}}jN#6D;=4Qdv2XSR|KT3-b!jkHxAJYa^N
zkua5kTV03|GBjf$ZbP)C!DHn{C~<B8sUeYU!l*dV^+SARs|0RtDi|S+AX_QvD1c_V
z(!s|U*s3eLx>^{yx~h{|%b;6~;&5oyj+9slIouG{;f8n|o|KcAoUN_{nmJ5`d8Rxg
zvm})YUV+Re7=a@gy%U6c76Fzypfu8~08|7{!OTROK><%?`X*K&4co$%Lm60fxyEPv
zlB~m%C>=y7LO;k8kvX9*hEp)7qfN;Wp#gqYCo%4U^iv6&iWKgklQxlz2A3D;iJI7%
zE7%|fl747PA~<tp1ZuYsG1r8=+#l>#?Bxe34F^~xVazod5mQtdqU3mJQAL{^UzwVd
zlV47S><yldL~%D}Nd*pEjEFQuEfPST|5Vtq!%8^o+|iN%mc|s|$0}1+5`fnKgYKjQ
z&vy$jFu?BDTgA`7pvBL?aE_0GVJ;s7LoOc!Lk=GUgC!pW!y{e>hD2Tl23B4MhATV_
z42yXf7#8s`Ff8O@V6fqVoC#RW&A{No&A=eS&A{-Ki-7^OroWwwfkA_df#Dn{1H(K{
z28LWt1_oPB1_m2W1_lmJ28Jupwfplq7#OB;Ffimo*YHb2*YRIuXJBY#XJ9a7XJB~9
z#=v05205F56DtG5Tvi4KKUM|?F3=hMEDQ{@Sr{1dSQr=rSQr?XSr{00F*7jiWM*K<
zWM*KH1f9*##K5qHiGhKGiGkrDBLhPzBLf2{46KOn-@~Fp8MK}*1w6G1I+GH#TqnN(
zG@b-HNFKCuqc|0`i#-v1TuiY-CfZs)=<rt}sId#qEATy=kckA)bWA*K%Vt4QK4=9~
zW`3Tnx^ap+QhtDq>ZN5?D5Qec&q5c?!q?FzrIwVZrh={pfR8GHRv1C7hYuZLa{;If
z1RvR{k(QYbT5)L$>B~bWBEV}m^}!ll0z9?A473%fU?tEYVW=+10#W?hAZj#q6jBN@
zAp-&~0iKA$K^b~N6KJL%?g8*j1my(6JXkbi(%`xIP#?z-{~+-6Jf;e`2!-<#nEpZ3
zD6oi!oJ#^-!42Ldl9~x#y$4x$pO19-Fm&_}ymlQeH6mKH@!<J3aBM=4i%3nyc`ik2
zsvhcT6lnUuOV5#G4kVzEl$w@b1lryK8CwR8#v<&8#{im9@g><1_dx0$&}fb!cpD{X
zy4L_@?i^KTd^z|yRD?FfEU^JnM#8EIHmM1*IX)G9;YEB&Hq=ZFkhMsY_NZpS^AFPb
zvXC4E%Icu$Rm_7jQd5zfgnT>(YOoZgCL6oNCsu%KQ}9+z@OJiM+vr#wg-X!WH4?uH
zg^!30uxnu^!F&pF8j*z$*aV0UxMv4jVS$;4$Ps}DKX{=cJb$AzLjAy(F@P5Bpi80f
zsb4O@j8|3w-$;=UUWp1SAwkCgLnby-@{>!Ur*MGBE<rOPiFpdpRimjDiJ<e9kqQ_B
z;e;F{jy^u%qA*y&G00WH#nsKz&(%f2!!^hiWrmkn6A_L8r!eHPwJLCu2Ma^0Fg?&{
zVKnmDTwqCLJ(bw?ARqaJp$C$+z{1G(fX>K*dmJna8DoksEX^#zI_hACmf@i4!6)Qn
z)nJNU1L#~jtm@Int)Uix)hpO4IDuC|X<!yLNb6DpauO?*K!qvziXTXo3(}9>B@m|*
zX|!KzIcP!>-E^=S*o_B=B$1{EmS&b98C?WAYBVLaxTGk*5`9NDG+02UL#hJkFg&!!
zg)f@0smBvb*p%amBz#^)o)LnL?2r*zq*_5<q>*Y1{>VekWsqJvWbz4XE<j(G3C;fy
z9Vjt^*;R#=o!DuVkzR;-kSidHHI;P~!1b)Hk{jp}pVYkMN`+{Ts#v6+EVzt7aSUjn
z0)3<&VyqEts2(yc2G$B)qM@LxV5p-&l{qrFU64t#kj&gv%Es)W27+1uh*esU2+PbZ
zMbC#235Xt)c!6fb;SiB@j7M0hKw>n4`v0s93=Ev0ey;!n1NbOk1_p*X{0t0c{0t0S
z{0s~`_!t;cp?!Z|J_d&SybKH*co`Uac^Me=co`VJ^Dr=M<6&UP<Y8ct<zZlW#m&F~
zTKiuQ?fakOVqi$(Vqj1K?d#`cU?}2bV6fw4V7SY{z>v(rz#zuKz;K71fnf*q{C;V6
z28QEo3=CV@7#Jei7#Jcz_Y<%&FnncYU^vOjz)-@<z+lJ9!0-=r&mIc{!*v!0hG{Ge
z3=S*|44f<s3^$k=7-ljvFhnskFi0^oFuY}AU^vRez>vwrz#zedn8yeCnFHi+(40MT
zo`+05pd26!YZgP>0ErdgQ(wRb3@0Y1YE<YbRO%>H=_n`^fd(p5Qf-ZO6q4agL^}rA
zn&^t?Or5M)ok|E*1*Sm9u;mr#rR0}rV3&t>qoED~t@uJcWD9<5X=Xuuaal@yQGN;N
z*iwU7P0*olh!t9Tm|E4Lr(hZ&4$MM!FX$NVl8pS)^b8#Z=uI#MMVYyYMU@H#r9}n#
z#i^hbTqXGmpk+jm9aG?KUZ8by@KPMrK*-u0(01v}G|*59bom!_fehqg7RWwG$YwRr
zDktcMDbT13<nUY!vq83IfmV(|4l2k=&C{sRgdQ9Wvmc}e=@4L;8kjU<0X9ga3bbi5
zF(orI+9*~*3v>oTerZW+kw#vD9_SX0c+e$78l{>#AerR+VvRDS1xO(Mp!w$_uzo{y
z{l%GiFr6S~nI>9S5~Lk8Gz8IZfUX^68`NT)+RH#Y-V5|{GV_WHK#A1=wA>=EK(8QE
zN5Kr-V@B$_g6spWn$1JkV5DaN@j6azU?(crDj4b+K*n_lr%}|Sb-}(f&@+TbC@7M^
z%f7%TCRKo@?~?MtC+-n>A}V;RHt0s7%mTz=K9KB<e7zmGh6Rt4AP!9gbtfS8J9sMz
z`1~C-F}ORy`k}r>Qv+HH2wJNH9*zT>1lNVB5;Q6j0J;hbGCTk^F|h(Q&x7>oD5&Qq
z7Uk#Xf%c1OAuX7O4+nvdAb>P>K{*R@%U=m-T_xNB%8-4apaqSfO9%6cA=?K**Ofu`
zNP_mogVvaV)(=9ja{}M+n3A7b47xW9bgd(jX&{F|HtS;+c;FyH3u3fmesP|~gQgB{
zBPgw+ZXX1RfG{o2Oaqw<!uVVRk_Y<~w9<~`bKaHEjK&!tsAp2bwmG8ilt%0TggXk&
zc=%dMG;u_C45Sm1pcINr@(Vy4pFvABK}R#Bra)pEw9pfDC@bjlA<(K=@XF~_P%2R`
z1`kA|#1_av5C(@4G<oB$hpcoANjSOLC>GRn1s!myo|IFXij*fnHlq0vbb=eQBcK&J
zO1{JAFwl|GsIdSECo2Wep&Fn(11`JZ=evW>#Rgv{g%TXf;05vusi}xPM+kT7f!4``
zjRUQRhOILP9XML70M1A#Mu9y;UaCul9+D3(oxtTBiU_7P(1<IDN7e{WO(>dC(-Ml5
zf-U&q8p7or?sx#j0`yp1^?bAlSB3;@W**{P0#G`DVG6<vQX^qI;ZZ?bK?!uEUN$`E
zW`bH>V6jxVSe7QJkq27cjuNymmnnnRY8020CYM0ZkbzvxTbh?ySemMkSfY@VnplkK
zDNuUL(op~z3u^ixDoD_=g4h~AV71w(YQbIuwG>i8=PSTm1foGTDQMydb_^(Jy*23M
z6X^B0rJz%ck`qCdLQ*RDte4cH)D*-aKPWzh*aDg!bAjAhhm!5!)g8#;_>ZUs`59~&
z`RxPHPJm2MwWyGsUs_ZQI(4cjyBItq3R+yBo|6ilWh#MQR)Z86pxX^W$4Y>fV1rJP
zP{;*MXMi09J!mTxvcm&30|=VkNXbk~OD#&xOHKt{cMO@2OUy~nFUl;*$VHopK@<ew
ze2I2QBRKgY1(1TRLbN(;C69VpW`0g;i8`Vg4=bFYy91zW%fajZ6BWRx0Ouwaz~cyP
zI5d+(<`&{Bb24)^qQNV54fQ~SpP*m>VJvDZh*S$2l?FAT6N`!xD>b4GVj&G^>|9+#
zJyVc^WLxzlq@0dsJxNAlHxY7V5a=i$b;vD*;FBQ0r>o`U=VvP<qU;a>O}By_tN@xA
z1y8$y&XmZ2rXx^$8+6Yg$T;YEW{}(c^bp&FAXj5T20~Imia|*#Js+|72JA;tigs9M
z74_UJj7$bPaTwJ9S7cyd&;<2^1sE8tL49Ec28QGO3=C7CYyaQ!F)-}rV_=vKUGx8!
zmx19AF9X9>UIvD2UIqqLUIvDLJPZsQco-Nwco-PCco-PYa5FHJaWgPja5FG`<zisi
z!^OZbg^Ph<GV~rmCoTqtPn--4i#Qn=3OE@UEIAn%E^sg~RB<pc2y-wnJYi>GFlT3A
zcn@9I-_ORtV8+J4@QIay;U+5s!wyyk2GG9$Dpm%DXjTS>C{_jrNmd4irz{K%h0y)~
z+$@lD`%;-17z~*i81^zTFmy99Fr+gvFbFd-FwA3w_*(<ycc{N{QlPYsjERU8@L7zQ
zr!zths)W}D;Nk*Oeu3)+ur7!ic!q>oOunU%lPvLD3Q>b$DXi=Oo#g{+edNK9U4czD
zLplrKV_&Fw+&MTTAZJ-38W)IbT~M1AC_w}*yD%FL@WBz(jwk3&7g*yF9L7i`E6V5!
zs+~v<)Im8_6<iE~4JtuC;~eBhuqap~B#u#i1`$KG1*8jv!8%Fz8K?;jF5w~5IVAWD
z;utb~21!V$K7)u+)n}mAA9+55IEE;nk=?|A=Ob{ufnyw+mO*tjqH0D+L&6(dMGw{u
zaWtBAVg=-KB7BE6gLPvZ){LqXa*~)jc=0UyNJ)NXiUQaS@M#Xk=(R8YnhC727~HD>
z`xLECg1Z9az-Nd?SXBmET!1_YhO#ih*br$+H~6r!Oz17w;8s~lKKN)!)Dy~(cE}}G
zDC8%BmQ^W$HiUxPewlfYbD+R`colLHR|`WFqQ)({+dvZ_gLAzUI0OkqFWhgCBV-dn
zS9gQX#>h)4Ml|Q4{rj}cJWy93+OkQ>2labP^Kw#)!3Us$ZhQtEj|I9{ATuwy1bk8>
zXqmY}CQ2I$G6)PgG!!%z1u_M6lQj5_YUrt0NtFt@iP@mR2k?IJ)RJTcgp#t%M95ku
z<n7a-hJ6NTbhad?5;~q-3~ivNCZ>ST8iDnxK@F~K(7{@e3&^31?O?lVq4t6n%Ym<W
z2OS)TY!OHTw9qOQd?F;c3jiuKK&P95f*~<aAuknlJ9%Oe%JyzZ0tD~s1}O(0zX_^0
zKx61dsRcRUo63=nV<E6+f>>#!=mk|OSTI^50l5irk}bM=>MvkH3>`vFo`f#Bg1M?B
zzX)=WDWWP-R>%Y$?Us}XI<O6LyFKLKQ_!XKpdnRA`viVA9y|!Z1riqX$y{QF<V5JY
zFvz?nXo(u(wPu*QVPzi{7pA9{;9h+O>Z+#}Ly{wCnM0~VUS?WJrGiFPetvFdUb>D#
zVhZSxQBZCI72XP|CCSL4S(2EY3f?6QYVm=(tw`lx3D*0r5ZM77q=-d)$R{%4@ebbW
zuAs|cQP&q@-gc!68p(j%&IlPa19=aWv!TZ%g0d;3lLgAyMIeU~ObDRFgR-d`lCJU<
zGC&RC63E5spb$#SEGjMm4`;y^H5NmT=tBerA<N*k2G&)ANT~zoy;_if!m(};JT?ot
zMN1!&pwQ01gXL_<><G%_CziwVK<S3`@og+-$17pKiYkTyx!(Z_Mo|1_f(`?Qbjac1
z33515#}T9HfK)rwKXVx59#HDigSOSNcpA|(#=NQ)krP1%q2t7yf{a9413hCM(8BJN
zR2;W}A!~y)k<|+_^FX`2K;whpqjhZ!^bB<vK>hzL1_p*cP#;);fx!vd0baq+z>o>O
z|L+eU1H*4V28LgJ3=FII7#Ki%|GD@W7}9td7`&is0EBoM7(Vep?gQM!!@#f+dd5HK
zEI>)<nt&_Z3=ECj3=Bcs3=B5h3=A*17#Oy3F)+;GVqkFOVqoCoVqo~f$-pp^lYt?P
zlYzm9lY!wQ2Lr=l4hDv)91INU91IL591IMM91INS*cli$urn}BVrO6|VP{|nVrO75
zU}s?9V`pGE%f`SE#KypI9qC-a!z>I8y(|n2S}Y6<UziyfCNeWHfW`$ZnHd;(nHd;v
zGBGeLWny3`WMW|Og`Nip3X@(?xIn{%_7rlof!N?=gg>j5#aAHR6s%Mh54qC?+vqfC
z)EOp^k{3W$A?G$w;YPP!etvFhdSZMPXxKazo-dFEQ8NTMNFdUc*z`aIaq5AD4w^j>
zK`eU8(A@<Sz^WEvHQem7c!&U2wGb<j)ItPM)q>&$gu#A>mL^!nG|E8hJFIj};qzZ$
z^~81bAt4C~eQ19eDg<8O0vR*J7{mZM3WUKH5p<Xd`3{4GAd15(spl|b@*M_=E)<7B
zmIUDlF<8QZ>L`n^LiGl5A&Hpc1gj@5iec#xZZ%p6f{h{=iDV5U!Acvr)o5WvoYf?c
z^MLwONW-e6FB?g$h==6@&>_I!jTIPoU_n%X*ObTOQvqH<jza|~9pJX1EFPcfW%2k-
zhPV#AN*;Plyt-pvdTCB#kwRH&PJS}z%B(7+p&YoEAyz`igD|vIV$%X~2QDoYs9IoN
zhAQ{T%uB`NCRh;QP>tpsSdidQjqDO|(!*c1LW2<B*cDU>#^4oH9Mb<o6zcFm1}!K9
zy8twHl$l$KI-F&Mz6hNdcUBU1XC)4ILXVfh;!e=QH9YP_U;IvtJ1YpgvjT@Zq35Y!
zaVKbn93FR~ue!(Y&a(I_d~Pg@uflL2C{d$b5}H^MUlvawp3&Dh6LJ^cI4+CFj9*ap
z!R9Uk@rr&H5g~WsjnlGt%=iRlRBY}d5RZoFi^!q*4AKz9GOiAhhqNNlM$W;>1tJfr
z%(2PCs%`9YxXp!CeAx6svpl@NKcY9vDR}yZf)_#I9%2MFGzs6WLhSe_OdZm(YMA4U
z(DPr>2O7ai1~Pa88Q6lUqh!DkY6iI@hN#-Hjj=&h=wTf*1oi)qb1^W;b2Bh72rw}4
zLp!-sq5J<g^D!{k@G&qP<z-+f<Yi!p=Vf33_4SYPFfdf`Ffb_dFfg3sW?-n~W?-n`
zhM04ei-BPl7XyO_7X!mLP6mc0oD2-QoD2-xI2ai0IT#r3vNJHOVrO9RU}s<uWoKa6
z%*Mdbz{bE(4?Vk&kBxz0A1ebxJ}U!*C3KztJr)LryDSV03t1Q#{8$(mE;2JPEN5n5
zC}3t__yAqs-^;|npv}a<z{SMCaEFnBp`4L{A(WAUL5-1tL6wn#;TzO%NF&UUsq9pR
zeCVCZX_<MM#Tg2qrAe8IIhj?EC42DBGB^Q1hd|>&)i$^cL)}1zUUniYhm1cWjVPiU
z2s*Jhu`DwkY;Oo;tkoza8`7zOrabU|J+MZ|78)?WATc>R9%_}XTVhTz($(-_wNN?m
z*5#ti6b<4WotmnL-_77n;(mmj4BF!ib_K-6ATH4k29JEg(=hQ)N70Ah6Ua)5i2-Ew
z(52ZBkD*GC=0(WpEu_UrT;zalgK8tfgAkQO`wyZV(|a&+e7;jwz?6mDo&%fTgKjp4
z-W`>wkds)MUs?iMsh^mhS_GZ+%SX8i6E>#{y8KW9cFL>*Xf_Tswhg+b5RzXYH@xNK
z=OE>CaN<McAAAKnC|i?Ro@eHkl2ME!bdg+ogR4db=<RTzF>=WHU7(@~%`A|waLuxS
z<WOf<NDD4_O9t!A1-N*^y6ad|0TDweK7s6&$jpOW`-SK<ftz`#g)#n?DYSWqJV1)q
zY;dz5by+vaVc_7vp%Kysq>ZB>W|QG4NQ0I(j)Itt<|uFo;g1V&uSySk^hmT4MsUKK
z+F+Bx=HXOAipL0BixkSZtc55c&03UUc`RN*3P)VlLX@Cdi(wKZjYCFoA&bzUB6^7x
zWuVy}jSA46HPBh0qSO?m;TY@|L-a%ZjBH{hg(gD0gKT02*(SnL3&b=~_Z!K#pgar0
z*u4kqo58f=c7zVd5HP0Pq{O_;+{BX96zGH<JW|2N!-{b5whqJ=G)P21q88VL4zd~&
z428rlenTN@NH7!<!T1e@s3FQwSRCUr5GGHgVG!@&HVh(9q+t+m;5G~*k75}8cB+9>
zEB?|Ll59}QD2NcHsf|o)A%Tx8JRwels6h!+Q0}3`w~%PUZ!JU(Io6`pJ+SnO%Q~1m
zRjh&p7H+E`^5FS@2?hoRAJ9Cv00RT;{QpP%3=9wX85m~rGcW}5Gca)SGca)QGcbJP
zgPaMtlaGO+fRBN}jE{lAl#hXdkB@=j5-$V8Ze9k4sk{sfmAniL$-E2<!n_O&Uw9Z8
zc0un2G~i)iVB%q5xXI1HaD$tHVG=h3Lku?qgA_Ld!(}c82GG5Lo?HwJ>|6{CM>!c7
z>Y;lBK65ZIeBxkWSi!--(9XfY;0E0zumXBNU_3hmgC;xV{C{pX1_mxR28KJV3=F$i
z85ovA_Xn7>GBC)oGBC)pGBEsNVPKfU!oZNo!oZ-z!ocv7nSo&$GXp~sbguv_GXujV
zCI*HOCI$vpCI*I1Mo1WWgTe_uj)2MlB}RN0S_NZYh=HyVoT{O-a@baope##=HTI$E
zp_>_rKAiyFZgjpj<oao->6kOnuyX@4^HNeP5G$L}je+rLal8S<YE1>q#yiYb?58Ox
z*y7LvnnNUXh5*QDkUFBz5ddibO$mT^%9DAP0LTo`@IAx~vd$9#837t;hZq6gbq_kp
z0CuDSF=q;Zj3D`30gxs#&lUh_1v{6L^93M@L_tdd`HTUO$snw(0MBO7Q9;b!DM%gY
z?rV$^I<+D>wV*^HEHS4v)wQT7zX)~rHAo)_la#6TAScNqjTVB80O69NN=);?W`j2$
z+bVzu08tzX+anI~V=D6D>)>l~tx!D+HVm5I(YE=b6g;2<)#8m);tPsE8=OlrQ;RjC
zk@XTPE252JV-eGNAip8&CTKIrcoHo%h>g`zAkB@2AU9$??+@9ri51{cC1~qi1Jv_E
zx)B)UH4p|B7a%FnVn)#N4YXT=q0-=8x!@HHkiEF5v5jmiSWz)*oPbuX2A3q3q&nth
z=AtDZLVAl+OT0h_P!vOkoS{ho6wWAiL@O1779Bw{6e0pa&Ih{)vY(Cgg%`>S(0-DF
zaSCWK5PWP$Nj~-)qo7+;A(Q>ZB}E!w=YTd`tCwUbWR|2Bfrrr*O7hDSi&7Lor&Fk-
zPaxv$PJ>f-1<3QzL{<_FwgQwMK#>W;-~h#83Z#uisVR{5C;6tpnw6B8K&ek)eQq+{
z0NOzkUy={GZ3vWo$TSB$Vh%~0;ITw9jR9{>f^5ek#stu9$;AbkC7@l`sS0U{$t92-
z{pG2k12s}%+swg<2(bwqblMW=zCqYZ{>(guoXp~qV$cp@&~{?=V))oMWYa%rqlQ9x
zVqQtH4${CX<QRmU{1Whe^^lzqkgX?>GfLsL1UO|Y*eZbTnanHDOUud6FVaZM$xkfN
z$V)AcFH6kPL^{<7oa~VX)QeJ!OY)0S<3ZQK=jTBR57Yr^L@9x+7ZM|o%TV<Q7=o-8
zyJ@MZgv>}qnim6waCu^7F?hQGXbwRE<N$bpz;id$n_%l9vmb<PK~jovD69$w)zOFo
z9enGsf~`VHW^Sq;h=3N@DXD4DW(edEB1nO(qX3HwP{je-(vS=~d<Eh{aQ(xe4BFlR
zx}Q5U4|?o4Xww3CXL<(sjA+nlK%le&x|dVI4ZKlJ0elXu4udkxc+e&!*vyIo<lJP1
z^vtr<Vvyks49ZXoa|?3flk@ZPQj<$Gl(JJR;|q#Xi;LqyGc=&%I1wTliFqkGsYTEV
z5sT8C#N4EmL<O);D}{n0P%2DQ0&U*_8=0D-Pz{#T%TBGV1>Nt5%?S+3;4A^UVo*l`
z`HUja-rmxj6osTz1@NuO$_m-3l}Y)aS|Ky9ptPizfdRHIfI=E|s{|+sDWsL=C4+8j
zF9vUYg9IixI-r*#fKwhMLcr=lgfi9K15eK2TnCvZgrz70O>qCei<g1n8ZTu2p9k8d
zJ;Bewu#KOA0kjSPbPhlQKLZ0VKLf*cJ_d$PJ_ZIyJ_d%jybKI)pn8u(&jJYKWnfU_
zWnj3)!@#hHhk>Duhk?O`hk-$ahk@Y<Hv_|7ZU%<A+zbqL(6a$#xfvLkxfvL)a4|5f
z;bLG|&Beg53VKdJDHj8S9~T3I8gx8>jf;Wd0Ve~)0Zs;n*_;dv4x9`Opm71vy#Z@E
z7#LbO7#Qq07#Nf}7#Nf|7#RMrL(Uc4&d$IP!_L6K&(6T`fsKLTBpU<60yYMQC^iNL
zd+0p^5^M|%KUf(UF0(Q)RIoBIXtOdf@IcQK*aN*cFq(ydA&P~8A(DlG!H9){L7Ihu
z;T$sqLmD#!!*3=AhSf|A46B$J7^X5YFjO)zFa$6$FfcGNFzjGtU?_l&8-OAggu$@^
zO#nHmMW9<tP%r*OQxFe2OE53p2(*er1EU<y&nqd)&&f$G!n{BfyE%r)=75{F(3T#k
zkpxK|;8RCX&gg+E0iDkRWr9@eD4?IfgU39~J`LD?P~*Ud0V9=v5I#tSCQ|<d#MeWu
zTVUfKkYWim{fYCWdK{CTNaleHZ|H&pG=q_)aN2;P2+O=Gx>b-W1)EI}8T=MO)!?@W
zyh;NcZqN`S`6z!-pTaRG2PtyEB6%sH1u2OosZNQ>*^s$saMcfrF4Tgj2y`Mdr1XKB
z0m=oCO()QLP9Zn35_05KDtM<b=<J+gh4NI;nUCrDpxt~)l?sp(bir*IL{W?EGnCK=
zxe>`a40X`JgDL~3TadGm)upC_t5tMUpeYuV0g$o_xHX8UI}B2e+T{gXg~u?Y&IgjJ
zXv|(L*l2K1732dDMmHKX-ee2z1;tmvBL}1nyh;_dr3aQrYJxzNf#Vjr2?ACIw`(+x
zQ}fDT$Iv4UYoJF2dV-;592<f*d4XfA5*|4au7a%s5pi5O8pk>LC4)O38^GhZ0yU07
zW4c7daRq@mhSm|Fni5(vg0eUWgHtk8nu3BBWE2P^8;4rXf{GrH-JmK5qza`%f%FcP
z(h7=|bQBB~^c6Hx^HLOaL5EZnl_0f^pz1)KDusen=>0dTR!TYw$m-!i2ol8Xhr!)R
zx6%y}5?CuF0?`7JPRl7R&Vcm7iqR`BXzYQMfjXk#q@V{~l8)yNAC%Z8!h96<s1~70
zCFP@EXA5;<YN{R)_JA%P%E>H2cmpJWVkTxUA9AP*=x7kcP3)la5ENkNf#j!w`((wX
zMQPx*>F^{1PYh5ug1v&;hyW=8;mkB}TLzSP^+5hd>hXil;KpJA5xzyX4`e9Z+aMc3
z7&^uPVuCP~3v~dBH$c)b40a^$iTlKg`0UimSoqp4$QmxN7-DpTRI^Y=%TcH8K*Pug
zOF%>Aq`43g)?kys&L?aNLYFe6r=F2m0vbvH9W-8?p9eaX1~jGsj#fxZ7J4!yXh5=9
zN1-@hArms72O6GJNKDEvON9ryvI6LQ8Bmd?02;6X>jyPyA>J$o3nrF;*P)}%#DKyT
z?0ZmrE7&T86qO=O1xaFGRsxbi#@NpdPyqM;V|W-CPJlZ80t^fy(Ek4(eg=k4eg=ko
zeg*~?eg=jgd<+by_!t<L@G&qX@i8zM@i8zk@i8#mhR*rV;bmY5<7Hs5=4D_|gq{U(
ziid&WB-HG+JPZu=JPZu6&@%%6ax*a8;$~nt$j!iT06HdM&&|NV!p*>No{NE@hl_y$
zv<`rki-F+^Cj-L@P6mcjP6meW&@})$91INa*%=r>efy>C3=A#o3=GZekh23nvqR1Y
zC}wA1P-ACcU}tAwc)-TMu$_&8p^%M%A&iZIA(V}QL7$C*fuD_m;T0<bLq976gA*$Q
zgEA`v!ygs~hRe|N0*avL2^O$0FnB`y{ZE-07;2dr7=)M^7!HB%4`gCsSi{7?FadhD
zpg0o)!+AyqhIVM59~z|iC|Fd1V|sL00J@?X+T(!?13-!zP{RPi2L%<VS%4CBNPX+f
zT+mcANR=M6um#n5B(8x*G7nt#jt&dJOEhrEf(kTH8x0XTV3E;bfy5m2Uf$@iKuIc6
zV{~-r2b_U2v*YPn-@{5vT=eJ~HAu*eu2BQ8wgQbAl5yV+C=5r}sKGKgXjBk1#)r0o
z2GXEN%|Shi7(5LRy5(ndjaq(Qe12M5d|6@<WVQ;@K?Ah_L5&Po@X=|h;4Uz7I}keR
zGdem1%7h>cPKHQLU?RpDKrQ7YlwlRH2+Ft$vayh)3|+-Rg!w4yQ7uB1!fE5M9UTGz
zjG%=MG2sYVd|I#sF4Qo{j3H?M|4|kOhFZ`ifC3B*>d;>5a()H|KYj)V(Aobzd<+cQ
zd<+a*co`UMc^Mdv@h~u?@GvkK@-Q%*=VoA-!_B~8#m&IL&dtEU#?8R6g^Ph<Di;HT
zD|G)qXuiIclYt?XlYt?HlYv2llY!wX2LnR{2Lpp12Lr=*b_RxWb_NC?b_NC!b_Rw`
zYzzz&*%%m-*%%o9u|n4IuVrOmSi=f+C)ACj)MyBdhQMeD4A~F>72F`K3|j#)dJGBb
z@DkFYCEyWr<N*Y5C5cc#%()hzb4S41Ag6VJxR~cuK!&nF(E?TqvjL@DLc*~xDEjbw
z0$C|B-a%H6c}@p(s8R>ZQ5^WZ2yGpZ>O-hDB0LCDNwohU$}znM6UP^UqsNedqmj7t
z=SPnr0p)op24{Wng8cZ=V@M#IKfvd5pw82R0t;VAL3Xa77U{UBnU&xNy`XGM#}Nw<
zv(X#{b_{+S(Dr0wm=4=512!3K9!@2sc#N>MNTH0&T8I+Tti>OeuzfjreG3U{RBJH;
z1+qmSa(V#dWDTeY&Lci_V0*n_XJUYo69{AXKEyak5FlHC`&1ILEP!|x*#dm$gFtty
z5$6L~(t;RX7GFiJpn;w63^SO7AOZFNudy;P9Aks5|2Ks;reE?iFr4ORU|7P>z)-`_
zz!1yNz!1aFz+lGDz#zuY!0?fefngdS1A`|Y1A`VH0|PT31H&y|28Jonz5f-w3=BcM
z3=BrR3=F)y3=E(<0aAGw7~FUm802{v7`|~cFl^vvV3^9yz%YfIfgz8Zfx(ZPfkBg-
zf#Dn%14APh149xQ1A{de1A`D31H&0k28NBC3=A7M85q`cGB9*-GB6Z!GBBucGBE7n
zU|<O3fZPkfz`?-qnw^2+JUau!GIj=rFzB5CApac&`4Q^BQED^<MnhmU1V%$(7=!?*
z(E!Hamc!`ssnO+AuyX@OcU6t<s>;V)iv^mEhaTwwU1JE^BQv_IDzgAOYKV2u8*~mC
zJnRS=euF8)x={hKj{#gZ!_GKF3h*JbL~e9f6?Er5C?+A5Y+_znVln76anM=}q;-8@
z*`XN6BXd_3sQ(Y@w}bldp!5Gh#{hs@g#Y;&81C^iFzn@LV3@<tz);1{z~ITxz@W#^
zz`)GUz;Fw+9)OR5p^J}!ApklCAixLGIf_R^U^E0qLtr!nMnhmU1V%$(Gz3ONU^E0q
zLtr!nhC&E{I`|+A-JX|JT9TZRTBJ~tkqHt7VN97~tk*T==jBv_PfsWT?Q@5pL70@P
zP*j=+-YuV&nwq1KR+O3wI*|~*yg*qYsT6cTW@a(S*1U8D$k~C#aI=t9=Hw?Q<|t%m
z=B4H)mSiS_PBbh@O)pBsI5D<F!B!!!KrgW<F)tmmWkfG89&|!%F=8#8GWgJk^8BI{
z@L`OgBM?D{1%l54D9Oyt%u9#c3?Afw8lYgSU}Rtbmr_<JPgN+-&r>e}oi>=Bng>3G
zHZL9I>tgV^Rglm^(h%bB;%^1I(JH?zRUxe?KNqY_2cZJTnS=-h3Lw)Jic)hxH;h1n
MtyqDGI};fg0G^T?FaQ7m
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/.visualize.py.swp b/python/ur_simple_control/visualize/.visualize.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..85c39c3b20e714208f92a4c7bc7a2afcfe88cc4e
GIT binary patch
literal 12288
zcmYc?2=nw+u+TGNU|?VnU|`^Hib|aoTEn1`#=wxCUtEx%l2`<i!iRGci?cKH@Tq{v
z)xiwZ&&bbBHPVN3b4v44^b3j-D>94qON-)*Gjj`aQsa~J^Gb^HbMy-;OEU8Fut=0;
z7MCXGWLBl>!&!O-l@Pl|$<YuP4FU8J;AJp2GBf~*Dk~`}2n&URn4@?!1V%$(Gz3ON
zU^E0qLtr!nMnhmU1V%$(goHpz0V6{_0|NsS)W6bDnh}kLa!0Ar5Eu=C(GVC7fzc2c
z4S~@R7!85Z5Eu=C(GVC7fzc2c4S^vT0*NUM4E5X$4D39R`F~jd|1CcQ!&!a?hBN#O
z49oZ#7~1(67-IMt7(DqI7}WR~7})t47#{I4FwEs+U})iEV2I^oV9?@YV36ixVBq0n
zV0g{Tz_6Q_fnhZ-14A<}1A_-I1A{a#1H%O#28LNY3=Eb$3=CpC5PR7`b`Qa@8+Gz%
z2#kinXb6mkz-S1JhQMeDjE2By2n^8>P*6}%D99<%E6&I-*U)4DaX~n-B0eQExg<Iz
zu_Q4*JGC-aFDErUH7^B833g=#Ir$|TB{~WqIv%7OVMMf*PFAdrLUMjievvJhERN2M
z)ltYvOiInM1*rwugm6Pzevv|!LS~*qQDR<tss<Jl^@=kR3sR#EV>PXCyIjE*;=$4+
zkO>-@c_kXf5O#cWeolN*ez}6SLUBovMy7(cf}y4+!~r17Gj$YTt^zqDHLo-`wJ5O!
z$th5KA)bL+l$@)Vm0F^aSD=@ZnO9tpn4GF%prc@@qmYxDhwuc<9%Y3Pe;0o%h2)I<
z{NhxFM1{P}<Wxu)DHJ5;q?VMVg1rWf5Cz-nS}-5o3$|b{<bk|YtO@l{9>{aW5dSDE
zlw@QUD`XZclxHNCC_uF<lqcqu6oU<fC|0mlFoJTE^K**fT!=Fjauc&t6-qKv!O^Rb
zSDKrYTBMLzT%4Jnmz$bbqL7}Nms*sV12&;V!B!!!Krayzdx=Hq#d^7k74exRpfJqL
z&x1P*l#ud^6pHh6Qx!@wb2IbO!TOaIbam|%D)UPfiZk*{b5azNQWX-Da#9sa@)c4m
zN{SMbOZ1cT^NLG~N|Q?<zDq00&s9jtEdWOw)DTGU7Nr*CBqpaSlxLP?C?qDAlqTkY
z?1F?X)N{$DB??ukMft@F5OWes5*bob)4+apE6UGx$xJRmM4*lW$V-}549f681-TWJ
z#u$_pQp-|{DiyL*D?yU^d8x1f*HK8!OHs%yQ7={~OUx-vWl)BMDM%YQyC@_ofHGcY
zeqLfxr5*#+X*sDN9VJDTAkRZ1A+s1B+zgqy1^GoK3b~0TAcJx;lk^HIK`e#DVo>6T
P$>){k7F2@7@(LILj;E13
literal 0
HcmV?d00001
diff --git a/python/ur_simple_control/visualize/main.py b/python/ur_simple_control/visualize/main.py
index 19b3626..8f380f4 100644
--- a/python/ur_simple_control/visualize/main.py
+++ b/python/ur_simple_control/visualize/main.py
@@ -30,9 +30,9 @@ ik_env.goal[2] = 0.2
ik_env.render()
-ik_env.robot.setJoints(q)
-ik_env.robot.calcJacobian()
-print(ik_env.robot.p_e)
+ik_env.robots[0].setJoints(q)
+ik_env.robots[0].calcJacobian()
+print(ik_env.robots[0].p_e)
#print(type(init_q))
#exit()
@@ -43,14 +43,14 @@ controller = invKinm_dampedSquares
while True:
start = time.time()
q = rtde_r.getActualQ()
- ik_env.robot.setJoints(q)
- ik_env.robot.calcJacobian()
- q_dots = controller(ik_env.robot, ik_env.goal)
- thetas = np.array([joint.theta for joint in ik_env.robot.joints])
+ ik_env.robots[0].setJoints(q)
+ ik_env.robots[0].calcJacobian()
+ q_dots = controller(ik_env.robots[0], ik_env.goal)
+ thetas = np.array([joint.theta for joint in ik_env.robots[0].joints])
ik_env.render()
#print(ik_env.robot.p_e.copy())
#print(ik_env.goal)
- distance = np.linalg.norm(ik_env.robot.p_e.copy() - ik_env.goal)
+ distance = np.linalg.norm(ik_env.robots[0].p_e.copy() - ik_env.goal)
print(distance)
if distance < 0.01:
t_start = rtde_c.initPeriod()
diff --git a/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py b/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
index 1442111..fe8a6d1 100644
--- a/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
+++ b/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
@@ -18,18 +18,22 @@ from robot_stuff.inv_kinm import *
from make_run import makeRun
import numpy as np
-import sys
+# it is the best solution for a particular problem
+from collections import namedtuple
# needed to communicate with the gui in real time
from multiprocessing import Queue
+# for local thread which manages local queue (can be made better,
+# but again, who cares, all of this is ugly as hell anyway ('cos it's front-end))
+import threading
# don't want to refactor yet, but obv
# TODO: refactor, have normal names for things
# it's just for getting the fps reading while optimizing
-import time as ttime
+import time
# TODO: call update_point function with the current slider value after updating
-# stuff like eliipse on/off so that you don't need to move the button to get it
+# stuff like elipse on/off so that you don't need to move the button to get it
# just call the slider string value to get the current slider value
# TODO: finish writing up reseting to the same starting position (joint values)
@@ -123,10 +127,11 @@ ManipulatorVisualMotionAnalyzer
# so that they have names. the for loop functions the same way,
# but you get to know what you have, which might be useful later.
class ManipulatorVisualMotionAnalyzer:
- def __init__(self, root, queue, data=data):
+ def __init__(self, root, queue, real_time_flag, **kwargs):
# need to put this in the main program,
# so you need to pass it here to use it
self.root = root
+ self.real_time_flag = real_time_flag
self.root.wm_title("Embedding in Tk")
# for real-time updates
self.queue = queue
@@ -143,24 +148,24 @@ class ManipulatorVisualMotionAnalyzer:
#self.SCALING_FACTOR_WIDTH = 0.5
self.SCALING_FACTOR_HEIGHT = 0.3
self.SCALING_FACTOR_WIDTH = 0.3
+ # dicts not because they have to be, but just so that
+ # they are named because that might be useful
+ # and because names are the only thing keeping horrendous this code together
+ # TODO: current evil sharing of pointers (thanks python lmao) needs to be deleted
+ # ofc delete from ik_env, retain here (they're in both spots rn)
+ # NOTE: you need to do ax.draw(artist) for blitting. so each artist needs to be connected
+ # to its particular artist. so they can't all be in one dict.
+ # we'll just increase the dict depth by 1, and add a named tuple to layer 1 (layer of axis)
+ self.AxisAndArtists = namedtuple("AxAndArtists", "ax artists")
+ self.axes_and_updating_artists = {}
+ # TODO: maybe do the same for fixed artists. right now they're saved just to have them on hand
+ # in case they'll be needed for something
+ self.fixed_artists = {}
+ # messes up rotating the 3d plot
+ # you should catch the event and then draw
+ # but i aint doing that
+ self.blit = True
- #####################################################
- # LAYOUT OF THE GUI #
- # putting plots into: frames -> notebooks -> tabs #
- #####################################################
- self.notebook_left = Notebook(root, height=int(self.SCREEN_HEIGHT))
- self.notebook_right = Notebook(root, height=int(self.SCREEN_HEIGHT))
- self.frame_manipulator = Frame(self.notebook_left)
- self.frame_manip_graphs = Frame(self.notebook_right)
- self.frame_imu = Frame(self.notebook_right)
- self.frame_ee = Frame(self.notebook_left)
- self.tabs_left = self.notebook_left.add(self.frame_manipulator, text='manipulator')
- self.tabs_left = self.notebook_left.add(self.frame_ee, text="end-effector")
- self.tabs_right = self.notebook_right.add(self.frame_manip_graphs, text='manipulator-graphs')
- self.tabs_right = self.notebook_right.add(self.frame_imu, text="ee-graphs")
- self.notebook_left.grid(row=0, column=0, sticky='ew')
- self.notebook_right.grid(row=0, column=1, sticky='ew')
-
########################
# run related things #
########################
@@ -168,7 +173,7 @@ class ManipulatorVisualMotionAnalyzer:
# we won't be generating runs here later
self.n_iters = 200
# the word time is used for timing
- self.t = np.arange(n_iters)
+ self.t = np.arange(self.n_iters)
# local kinematics integrator
# but this thing handles plotting
# only plotting needs to be run, so feel free to
@@ -178,7 +183,7 @@ class ManipulatorVisualMotionAnalyzer:
# all of them in all figures, this is scitzo af bruv
self.ik_env = InverseKinematicsEnv()
# TODO: do this depending on the number of runs you'll be loading
- #self.ik_env.robots.append(Robot_raw(robot_name="no_sim"))
+ self.ik_env.robots.append(Robot_raw(robot_name="no_sim"))
self.ik_env.damping = 25
# putting it into this class so that python remembers it 'cos reasons, whatever
# TODO: load this from run log files later
@@ -189,30 +194,187 @@ class ManipulatorVisualMotionAnalyzer:
# but deliver the same format.
# TODO: ensure you're saving AT LEAST what's required here
# NOTE: the jacobian svd is computed offline in these
- self.ik_env.data.append(makeRun(controller1, ik_env, n_iters, 0))
- self.ik_env.data.append(makeRun(controller2, ik_env, n_iters, 1))
+ self.ik_env.data.append(makeRun(self.controller1, self.ik_env, self.n_iters, 0))
+ self.ik_env.data.append(makeRun(self.controller2, self.ik_env, self.n_iters, 1))
+ # ugly front end code is ugly.
+ # i hate front end and this is why.
+ # actual placing of plots in all this comes later because tkinter complains otherwise
+ #####################################################
+ # LAYOUT OF THE GUI #
+ # putting plots into: frames -> notebooks -> tabs #
+ #####################################################
+ self.notebook_left = Notebook(root, height=int(self.SCREEN_HEIGHT))
+ self.notebook_right = Notebook(root, height=int(self.SCREEN_HEIGHT))
+ self.frame_manipulator = Frame(self.notebook_left)
+ self.frame_manip_graphs = Frame(self.notebook_right)
+ self.frame_imu = Frame(self.notebook_right)
+ self.frame_ee = Frame(self.notebook_left)
+ self.tabs_left = self.notebook_left.add(self.frame_manipulator, text='manipulator')
+ self.tabs_left = self.notebook_left.add(self.frame_ee, text="end-effector")
+ self.tabs_right = self.notebook_right.add(self.frame_manip_graphs, text='manipulator-graphs')
+ self.tabs_right = self.notebook_right.add(self.frame_imu, text="ee-graphs")
+ self.notebook_left.grid(row=0, column=0, sticky='ew')
+ self.notebook_right.grid(row=0, column=1, sticky='ew')
+
+ #######################################################################
+ # PLACING ELEMENTS IN THE GUI #
+ #######################################################################
+ # NOTE: this thing is not used
+ self.same_starting_position_on_off_var = IntVar()
+ self.same_starting_position_on_off_var.set(0)
+ self.same_starting_position_checkbutton= Checkbutton(root, text = "same starting position on/off",
+ variable = self.same_starting_position_on_off_var,
+ onvalue = 1,
+ offvalue = 0,
+ # command=same_starting_position_cmd,
+ )
+
+ # LEFT PANE BELOW MANIP/EE PLOTS
+ self.frame_below_manipulator_plot = Frame(self.frame_manipulator)
+ self.frame_below_manipulator_plot.grid(column=0, row=3)
+
+ # set controller 1
+ self.frame_controller_menu1 = Frame(self.frame_below_manipulator_plot)
+ self.controller_string1 = StringVar(self.frame_controller_menu1)
+ self.controller_string1.set("invKinm_dampedSquares") # default value
+ self.controller_menu1 = OptionMenu(self.frame_controller_menu1, self.controller_string1,
+ "invKinmQPSingAvoidE_kI",
+ "invKinm_Jac_T",
+ "invKinm_PseudoInv",
+ "invKinm_dampedSquares",
+ "invKinm_PseudoInv_half",
+ "invKinmQP",
+ "invKinmQPSingAvoidE_kI",
+ "invKinmQPSingAvoidE_kM",
+ "invKinmQPSingAvoidManipMax",
+ )
+ self.controller_string1.set("invKinm_dampedSquares")
+ self.controller_menu1.grid(column=1, row=0)
+ Label(self.frame_controller_menu1, text="Robot 1 controller:", background='yellow').grid(row=0, column=0, pady=4, padx = 4)
+ self.frame_controller_menu1.grid(column=0, row=0)
+
+ # set controller 2
+ self.frame_controller_menu2 = Frame(self.frame_below_manipulator_plot)
+ self.controller_string2 = StringVar(self.frame_controller_menu2)
+ self.controller_menu2 = OptionMenu(self.frame_controller_menu2, self.controller_string2,
+ "invKinmQPSingAvoidE_kI",
+ "invKinm_Jac_T",
+ "invKinm_PseudoInv",
+ "invKinm_dampedSquares",
+ "invKinm_PseudoInv_half",
+ "invKinmQP",
+ "invKinmQPSingAvoidE_kI",
+ "invKinmQPSingAvoidE_kM",
+ "invKinmQPSingAvoidManipMax",
+ )
+ self.controller_string2.set("invKinm_Jac_T") # default value
+ Label(self.frame_controller_menu1, text="Robot 1 controller:", background='black', foreground='white').grid(row=0, column=0, pady=4, padx = 4)
+ self.controller_menu2.grid(column=1, row=0)
+ Label(self.frame_controller_menu2, text="Robot 2 controller:", background='#EE82EE').grid(row=0, column=0, pady=4, padx = 4)
+ self.frame_controller_menu2.grid(column=0, row=1)
+
+
+ self.ellipse_on_off_var = IntVar()
+ self.ellipse_on_off_var.set(1)
+
+ self.ellipse_checkbutton= Checkbutton(self.frame_below_manipulator_plot, text = "ellipse on/off",
+ variable = self.ellipse_on_off_var,
+ onvalue = 1,
+ offvalue = 0,
+ command=self.add_remove_ellipse,
+ )
+ self.ellipse_checkbutton.grid(column=1, row=0)
+
+
+ self.frame_goal_x = Frame(self.frame_below_manipulator_plot)
+ Label(self.frame_goal_x, text="goal x").grid(row=0, column=0, pady=4, padx = 4)
+ self.slider_goal_x = Scale(self.frame_goal_x, from_=-1.0, to=1.0, length=self.SLIDER_SIZE, orient=HORIZONTAL,
+ command=self.update_goal_x)
+ self.slider_goal_x.grid(column=1, row=0)
+ self.frame_goal_x.grid(column=1, row=1)
+
+ self.frame_goal_y = Frame(self.frame_below_manipulator_plot)
+ Label(self.frame_goal_y, text="goal y").grid(row=0, column=0, pady=4, padx = 4)
+ self.slider_goal_y = Scale(self.frame_goal_y, from_=-1.0, to=1.0, length=self.SLIDER_SIZE, orient=HORIZONTAL,
+ command=self.update_goal_y)
+ self.slider_goal_y.grid(column=1, row=0)
+ self.frame_goal_y.grid(column=1, row=2)
+
+
+ self.frame_goal_z = Frame(self.frame_below_manipulator_plot)
+ Label(self.frame_goal_z, text="goal z").grid(row=0, column=0, pady=4, padx = 4)
+ self.slider_goal_z = Scale(self.frame_goal_z, from_=-1.0, to=1.0, length=self.SLIDER_SIZE, orient=HORIZONTAL,
+ command=self.update_goal_z)
+ self.slider_goal_z.grid(column=1, row=0)
+ self.frame_goal_z.grid(column=1, row=3)
+
+
+ self.frame_update = Frame(self.frame_manip_graphs)
+ Label(self.frame_update, text="Time").grid(row=0, column=0, pady=4, padx = 4)
+ self.slider_update = Scale(self.frame_update, from_=0, to=self.n_iters - 1, length=self.SLIDER_SIZE, orient=HORIZONTAL,
+ command=self.update_points)#, label="Frequency [Hz]")
+ self.slider_update.grid(column=1, row=0)
+ self.frame_update.grid(column=0, row=3)
+
+ self.frame_update2 = Frame(self.frame_imu)
+ Label(self.frame_update2, text="Time").grid(row=0, column=0, pady=4, padx = 4)
+ self.slider_update = Scale(self.frame_update2, from_=0, to=self.n_iters - 1, length=self.SLIDER_SIZE, orient=HORIZONTAL,
+ command=self.update_points)#, label="Frequency [Hz]")
+ self.slider_update.grid(column=1, row=0)
+ self.frame_update2.grid(column=0, row=3)
+
+
+ self.button_quit = Button(master=self.frame_manip_graphs, text="Quit", command=root.destroy)
+ self.button_reset = Button(master=self.frame_manip_graphs, text="New run", command=self.reset)
+ self.button_play = Button(master=self.frame_manip_graphs, text="Play", command=self.play)
+ self.button_play.grid(column=0, row=4)
+ self.button_reset.grid(column=0, row=5)
+ self.button_quit.grid(column=0, row=6)
+
+ self.button_quit2 = Button(master=self.frame_imu, text="Quit", command=root.destroy)
+ self.button_reset2 = Button(master=self.frame_imu, text="New run", command=self.reset)
+ self.button_play2 = Button(master=self.frame_imu, text="Play", command=self.play)
+ self.button_play2.grid(column=0, row=4)
+ self.button_reset2.grid(column=0, row=5)
+ self.button_quit2.grid(column=0, row=6)
+
+ self.slider_goal_x.set(self.ik_env.goal[0])
+ self.slider_goal_y.set(self.ik_env.goal[1])
+ self.slider_goal_z.set(self.ik_env.goal[2])
+
+
+ ################################
+ # ALL PLOTS ARE DEFINED HERE #
+ ################################
# how to add plots will be documented below on an easier example
#######################################################################
# UNUSED PLOT #
#######################################################################
- self.fig_ee = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+ self.fig_ee = Figure(figsize=(self.SCREEN_WIDTH/self.DPI*self.SCALING_FACTOR_WIDTH, self.SCREEN_HEIGHT/self.DPI*self.SCALING_FACTOR_HEIGHT), dpi=self.DPI)
self.rec3D_ax = self.fig_ee.add_subplot(projection='3d')
self.rec3D_ax.set(xticklabels=[], yticklabels=[], zticklabels=[])
+ # tkinterize
self.canvas_ee = FigureCanvasTkAgg(self.fig_ee, master=self.frame_ee)
self.canvas_ee_widget = self.canvas_ee.get_tk_widget()
self.canvas_ee_widget.grid(row=0, column=0)
self.canvas_ee._tkcanvas.grid(row=1, column=0)
+ # draw before getting the background
self.canvas_ee.draw()
+ self.background_ee = self.canvas_ee.copy_from_bbox(self.fig_ee.bbox)
+ # TODO: append artists to either fixed or updating artists dicts
#######################################################################
# MANIPULATOR PLOT #
#######################################################################
# robot plot
- self.fig_manipulator = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+ self.fig_manipulator = Figure(figsize=(self.SCREEN_WIDTH/self.DPI*self.SCALING_FACTOR_WIDTH, self.SCREEN_HEIGHT/self.DPI*self.SCALING_FACTOR_HEIGHT), dpi=self.DPI)
+ # TODO: fix evil pointer sharing between this class and ik_env
self.ik_env.ax = self.fig_manipulator.add_subplot(projection='3d')
+ # read comment on top of init to see what this is
+ self.axes_and_updating_artists["ax_manipulators"] = self.AxisAndArtists(self.ik_env.ax, {})
# fix array sizes to be the workspace as that won't change. otherwise it's updated as you go,
# and that's impossible to look at.
self.ik_env.ax.plot(np.array([0]), np.array([0]), np.array([1.5]), c='b')
@@ -226,20 +388,36 @@ class ManipulatorVisualMotionAnalyzer:
# but it of course works on a single robot too
for robot_index, robot in enumerate(self.ik_env.robots):
robot.initDrawing(self.ik_env.ax, self.link_colors[robot_index])
+ # this could be named better, but i can't be bothered right now
+ for i, link in enumerate(robot.lines):
+ for j, line in enumerate(link):
+ # constuct a name that unique at least
+ self.axes_and_updating_artists["ax_manipulators"].artists["robot_" + str(robot_index) + "_link_" + str(i) + "_line_" + str(j)] = line
# ee point
- # TODO: blit the point because it moves
- self.ik_env.p_e_point_plots.append(drawPoint(ik_env.ax, ik_env.data[robot_index]['p_es'][0], 'red', 'o'))
+ ee_point_plot = drawPoint(self.ik_env.ax, self.ik_env.data[robot_index]['p_es'][0], 'red', 'o')
+ self.ik_env.p_e_point_plots.append(ee_point_plot)
+ self.axes_and_updating_artists["ax_manipulators"].artists["ee_point_plot"] = ee_point_plot
# plot ee position trajectory. not blitted because it is fixed
# TODO: blit it for real-time operation
- self.trajectory_plot, = ik_env.ax.plot(ik_env.data[robot_index]['p_es'][:,0], ik_env.data[robot_index]['p_es'][:,1], ik_env.data[robot_index]['p_es'][:,2], color='blue')
- trajectory_plots.append(trajectory_plot)
+ trajectory_plot, = self.ik_env.ax.plot(self.ik_env.data[robot_index]['p_es'][:,0], \
+ self.ik_env.data[robot_index]['p_es'][:,1], self.ik_env.data[robot_index]['p_es'][:,2], color='blue')
+ # TODO stop having shared pointers all over the place, it's evil
+ self.trajectory_plots.append(trajectory_plot)
+ if self.real_time_flag:
+ self.axes_and_updating_artists["ax_manipulators"].artists["trajectory_plot_" + str(robot_index)] = \
+ trajectory_plot
+ raise NotImplementedError("real time ain't implemented yet, sorry")
+ else:
+ self.fixed_artists["trajectory_plot_" + str(robot_index)] = trajectory_plot
+
# goal point
# only makes sense for clik, but keep it here anyway, it doesn't hurt
# TODO: add a button to turn it off
- self.ik_env.goal_point_plot = drawPoint(ik_env.ax, ik_env.goal, 'maroon', '*')
+ goal_point_plot = drawPoint(self.ik_env.ax, self.ik_env.goal, 'maroon', '*')
+ self.ik_env.goal_point_plot = goal_point_plot
+ self.fixed_artists["goal_point_plot"] = goal_point_plot
# the manipulability ellipses
- # TODO: BLIT THEM
for robot_index, robot in enumerate(self.ik_env.robots):
radii = 1.0/self.ik_env.data[robot_index]["manip_ell_eigenvals"][0] * 0.1
u = np.linspace(0.0, 2.0 * np.pi, 60)
@@ -253,9 +431,15 @@ class ManipulatorVisualMotionAnalyzer:
# although, again, primary purpose is to log stuff for later
[x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], self.ik_env.data[robot_index]["manip_elip_svd_rots"][0]) + self.ik_env.data[robot_index]['p_es'][0]
- self.ik_env.ellipsoid_surf_plots.append(self.ik_env.ax.plot_surface(x, y, z, rstride=3, cstride=3,
+ ellipsoid_surf_plot = self.ik_env.ax.plot_surface(x, y, z, rstride=3, cstride=3,
color='pink', linewidth=0.1,
- alpha=0.3, shade=True))
+ alpha=0.3, shade=True)
+ self.ik_env.ellipsoid_surf_plots.append(ellipsoid_surf_plot)
+ # TODO: current evil sharing of pointers (thanks python lmao) needs to be deleted
+ # ofc delete from ik_env, retain here
+# TODO: put back in if possible
+# self.axes_and_updating_artists["ax_manipulators"].artists["ellipsoid_surf_plot_"+ str(robot_index)] = \
+# ellipsoid_surf_plot
# tkinterize the figure/canvas
self.canvas_manipulator = FigureCanvasTkAgg(self.fig_manipulator, master=self.frame_manipulator)
@@ -269,68 +453,110 @@ class ManipulatorVisualMotionAnalyzer:
self.canvas_manipulator_widget.grid(row=0, column=0)
self.canvas_manipulator._tkcanvas.grid(row=1, column=0)
# i probably don't need this draw, but who cares
- self.canvas_manipulator.draw()
+ #self.canvas_manipulator.draw()
#######################################################################
# manipulability plots #
#######################################################################
+ # NOTE: code for each plot is replicated,
+ # and there are more for loops than necessary.
+ # but honestly this is more readable because you know which subplot you're making and with what.
+ # TODO: maybe make it automatic later by making tabs etc automatic via dicts.
+ # but as it stands right now, it's fine, i won't do it unless there is a distinct need for it.
# manipulability ellipsoid eigenvalues
- self.fig_manip_graphs = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+ self.fig_manip_graphs = Figure(figsize=(self.SCREEN_WIDTH/self.DPI*self.SCALING_FACTOR_WIDTH, self.SCREEN_HEIGHT/self.DPI*self.SCALING_FACTOR_HEIGHT), dpi=self.DPI)
# NOTE: this ax object lives on in some other matplotlib object
# so i don't have to save it
- ax = self.fig_manip_graphs.add_subplot(311)
- ax.set_title('Manipulability ellipsoid eigenvalues')
- ax.grid()
- ax.set_xticks([])
+ ax_eigens = self.fig_manip_graphs.add_subplot(311)
+ self.axes_and_updating_artists["ax_eigens"] = self.AxisAndArtists(ax_eigens, {})
+ ax_eigens.set_title('Manipulability ellipsoid eigenvalues')
+ ax_eigens.grid()
+ ax_eigens.set_xticks([])
self.eigen1_lines = []
self.eigen2_lines = []
self.eigen3_lines = []
- for robot_index, robot in enumerate(ik_env.robots):
- eigen1_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,0], color=link_colors[robot_index])
+
+ for robot_index, robot in enumerate(self.ik_env.robots):
+ eigen1_line, = ax_eigens.plot(self.t, \
+ self.ik_env.data[robot_index]["manip_ell_eigenvals"][:,0], color=self.link_colors[robot_index])
self.eigen1_lines.append(eigen1_line)
- eigen2_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,1], color=link_colors[robot_index])
+ eigen2_line, = ax_eigens.plot(self.t, \
+ self.ik_env.data[robot_index]["manip_ell_eigenvals"][:,1], color=self.link_colors[robot_index])
self.eigen2_lines.append(eigen2_line)
- eigen3_line, = ax.plot(time, ik_env.data[robot_index]["manip_ell_eigenvals"][:,2], color=link_colors[robot_index])
+ eigen3_line, = ax_eigens.plot(self.t, \
+ self.ik_env.data[robot_index]["manip_ell_eigenvals"][:,2], color=self.link_colors[robot_index])
self.eigen3_lines.append(eigen3_line)
- # TODO: blit this line
- self.point_in_time_eigen1_line = ax.axvline(x=0, color='red')
+ if self.real_time_flag:
+ # TODO: put it in updating artists, but then also actuall update it
+ self.axes_and_updating_artists["ax_eigens"].artists["eigen1_line_" + str(robot_index)] = eigen1_line
+ self.axes_and_updating_artists["ax_eigens"].artists["eigen2_line_" + str(robot_index)] = eigen2_line
+ self.axes_and_updating_artists["ax_eigens"].artists["eigen3_line_" + str(robot_index)] = eigen3_line
+ raise NotImplementedError("real time ain't implemented yet, sorry")
+ else:
+ self.fixed_artists["eigen1_line_" + str(robot_index)] = eigen1_line
+ self.fixed_artists["eigen2_line_" + str(robot_index)] = eigen2_line
+ self.fixed_artists["eigen3_line_" + str(robot_index)] = eigen3_line
+ self.point_in_time_eigen1_line = ax_eigens.axvline(x=0, color='red', animated=True)
+ self.axes_and_updating_artists["ax_eigens"].artists["point_in_time_eigen1_line"] = self.point_in_time_eigen1_line
# manipulability index (volume of manipulability ellipsoid)
- ax = self.fig_manip_graphs.add_subplot(312)
- ax.set_title('Manipulability index')
- ax.grid()
- ax.set_xticks([])
+ ax_manips = self.fig_manip_graphs.add_subplot(312)
+ ax_manips.set_title('Manipulability index')
+ self.axes_and_updating_artists["ax_manips"] = self.AxisAndArtists(ax_manips, {})
+ ax_manips.grid()
+ ax_manips.set_xticks([])
self.manip_index_lines = []
- for robot_index, robot in enumerate(ik_env.robots):
- manip_index_line, = ax.plot(time, ik_env.data[robot_index]['manip_indeces'], color=link_colors[robot_index])
+ for robot_index, robot in enumerate(self.ik_env.robots):
+ manip_index_line, = ax_manips.plot(self.t, self.ik_env.data[robot_index]['manip_indeces'], color=self.link_colors[robot_index])
self.manip_index_lines.append(manip_index_line)
- self.point_in_time_manip_index_line = ax.axvline(x=0, color='red')
+ if self.real_time_flag:
+ # TODO: put it in updating artists, but then also actuall update it
+ self.axes_and_updating_artists["ax_manips"].artists["manip_index_line_" + str(robot_index)] = manip_index_line
+ raise NotImplementedError("real time ain't implemented yet, sorry")
+ else:
+ self.fixed_artists["manip_index_line_" + str(robot_index)] = manip_index_line
+ self.point_in_time_manip_index_line = ax_manips.axvline(x=0, color='red', animated=True)
+ self.axes_and_updating_artists["ax_manips"].artists["point_in_time_manip_index_line"] = self.point_in_time_manip_index_line
# dist to goal (this could be/should be elsewhere)
- ax = self.fig_manip_graphs.add_subplot(313)
- ax.set_title('Distance to goal')
- ax.grid()
- ax.set_xlabel('iter')
+ ax_goal_dists = self.fig_manip_graphs.add_subplot(313)
+ ax_goal_dists.set_title('Distance to goal')
+ self.axes_and_updating_artists["ax_goal_dists"] = self.AxisAndArtists(ax_goal_dists, {})
+ ax_goal_dists.grid()
+ ax_goal_dists.set_xlabel('iter')
self.dist_to_goal_lines = []
- for robot_index, robot in enumerate(ik_env.robots):
- self.dist_to_goal_line, = ax.plot(time, ik_env.data[robot_index]['dists_to_goal'], color=link_colors[robot_index])
- dist_to_goal_lines.append(dist_to_goal_line)
- # TODO: blit this line
- self.point_in_time_dist_to_goal_line = ax.axvline(x=0, color='red')
- # tkinterize canvas
+ for robot_index, robot in enumerate(self.ik_env.robots):
+ dist_to_goal_line, = ax_goal_dists.plot(self.t, self.ik_env.data[robot_index]['dists_to_goal'], \
+ color=self.link_colors[robot_index])
+ self.dist_to_goal_lines.append(dist_to_goal_line)
+ if self.real_time_flag:
+ # TODO: put it in updating artists, but then also actuall update it
+ self.axes_and_updating_artists["ax_goal_dists"].artists["dist_to_goal_line" + str(robot_index)] = \
+ dist_to_goal_line
+ raise NotImplementedError("real time ain't implemented yet, sorry")
+ else:
+ self.fixed_artists["dist_to_goal_line" + str(robot_index)] = dist_to_goal_line
+ self.point_in_time_dist_to_goal_line = ax_goal_dists.axvline(x=0, color='red', animated=True)
+ self.axes_and_updating_artists["ax_goal_dists"].artists["point_in_time_dist_to_goal_line"] = \
+ self.point_in_time_dist_to_goal_line
+
+ # tkinterize canvas for the whole figure (all subplots)
self.canvas_manip_graphs = FigureCanvasTkAgg(self.fig_manip_graphs, master=self.frame_manip_graphs)
+ self.canvas_manip_graphs.draw()
+ # save background for blitting
+ self.background_manip_graphs = self.canvas_manip_graphs.copy_from_bbox(self.fig_manip_graphs.bbox)
# put matplotlib toolbar below the plot
self.canvas_manip_graphs_widget = self.canvas_manip_graphs.get_tk_widget()
self.canvas_manip_graphs_widget.grid(row=0, column=0)
self.canvas_manip_graphs._tkcanvas.grid(row=1, column=0)
- self.canvas_manip_graphs.draw()
+ #self.canvas_manip_graphs.draw()
#######################################################################
# IMU plots #
#######################################################################
- self.fig_imu = Figure(figsize=(SCREEN_WIDTH/DPI*SCALING_FACTOR_WIDTH, SCREEN_HEIGHT/DPI*SCALING_FACTOR_HEIGHT), dpi=DPI)
+ self.fig_imu = Figure(figsize=(self.SCREEN_WIDTH/self.DPI*self.SCALING_FACTOR_WIDTH, self.SCREEN_HEIGHT/self.DPI*self.SCALING_FACTOR_HEIGHT), dpi=self.DPI)
self.v_x_lines = []
self.v_y_lines = []
self.v_z_lines = []
@@ -340,16 +566,22 @@ class ManipulatorVisualMotionAnalyzer:
ax_v_x = self.fig_imu.add_subplot(321)
ax_v_x.grid()
+ self.axes_and_updating_artists["ax_v_x"] = self.AxisAndArtists(ax_v_x, {})
ax_v_y = self.fig_imu.add_subplot(322)
ax_v_y.grid()
+ self.axes_and_updating_artists["ax_v_y"] = self.AxisAndArtists(ax_v_y, {})
ax_v_z = self.fig_imu.add_subplot(323)
ax_v_z.grid()
+ self.axes_and_updating_artists["ax_v_z"] = self.AxisAndArtists(ax_v_z, {})
ax_omega_x = self.fig_imu.add_subplot(324)
ax_omega_x.grid()
+ self.axes_and_updating_artists["ax_omega_x"] = self.AxisAndArtists(ax_omega_x, {})
ax_omega_y = self.fig_imu.add_subplot(325)
ax_omega_y.grid()
+ self.axes_and_updating_artists["ax_omega_y"] = self.AxisAndArtists(ax_omega_y, {})
ax_omega_z = self.fig_imu.add_subplot(326)
ax_omega_z.grid()
+ self.axes_and_updating_artists["ax_omega_z"] = self.AxisAndArtists(ax_omega_z, {})
ax_v_x.set_title('Linear velocity x')
ax_v_y.set_title('Linear velocity y')
ax_v_z.set_title('Linear velocity z')
@@ -362,178 +594,80 @@ class ManipulatorVisualMotionAnalyzer:
ax_omega_x.set_xticks([])
ax_omega_y.set_xticks([])
ax_omega_z.set_xticks([])
- for robot_index, robot in enumerate(ik_env.robots):
+ for robot_index, robot in enumerate(self.ik_env.robots):
v_x_line, = ax_v_x.plot(self.t, self.ik_env.data[robot_index]["vs"][:,0], color=self.link_colors[robot_index])
v_y_line, = ax_v_y.plot(self.t, self.ik_env.data[robot_index]["vs"][:,1], color=self.link_colors[robot_index])
v_z_line, = ax_v_z.plot(self.t, self.ik_env.data[robot_index]["vs"][:,2], color=self.link_colors[robot_index])
- omega_x_line, = ax_omega_x.plot(self.t, self.ik_env.data[robot_index]["vs"][:,3], color=self.link_colors[robot_index])
- omega_y_line, = ax_omega_y.plot(self.t, self.ik_env.data[robot_index]["vs"][:,4], color=self.link_colors[robot_index])
- omega_z_line, = ax_omega_z.plot(self.t, self.ik_env.data[robot_index]["vs"][:,5], color=self.link_colors[robot_index])
+ omega_x_line, = ax_omega_x.plot(self.t, \
+ self.ik_env.data[robot_index]["vs"][:,3], color=self.link_colors[robot_index])
+ omega_y_line, = ax_omega_y.plot(self.t, \
+ self.ik_env.data[robot_index]["vs"][:,4], color=self.link_colors[robot_index])
+ omega_z_line, = ax_omega_z.plot(self.t, \
+ self.ik_env.data[robot_index]["vs"][:,5], color=self.link_colors[robot_index])
self.v_x_lines.append(v_x_line)
self.v_y_lines.append(v_y_line)
self.v_z_lines.append(v_z_line)
self.omega_x_lines.append(omega_x_line)
self.omega_y_lines.append(omega_y_line)
self.omega_z_lines.append(omega_z_line)
-
- # TODO: blit these
- self.point_in_time_ax_v_x_line = ax_v_x.axvline(x=0, color='red')
- self.point_in_time_ax_v_y_line = ax_v_y.axvline(x=0, color='red')
- self.point_in_time_ax_v_z_line = ax_v_z.axvline(x=0, color='red')
- self.point_in_time_ax_omega_x_line = ax_omega_x.axvline(x=0, color='red')
- self.point_in_time_ax_omega_y_line = ax_omega_y.axvline(x=0, color='red')
- self.point_in_time_ax_omega_z_line = ax_omega_z.axvline(x=0, color='red')
+ if self.real_time_flag:
+ # TODO: put it in updating artists, but then also actuall update it
+ self.axes_and_updating_artists["ax_v_x"].artists["v_x_line" + str(robot_index)] = v_x_line
+ self.axes_and_updating_artists["ax_v_z"].artists["v_y_line" + str(robot_index)] = v_y_line
+ self.axes_and_updating_artists["ax_v_z"].artists["v_z_line" + str(robot_index)] = v_z_line
+ self.axes_and_updating_artists["ax_omega_x"].artists["omega_x_line" + str(robot_index)] = omega_x_line
+ self.axes_and_updating_artists["ax_omega_x"].artists["omega_y_line" + str(robot_index)] = omega_y_line
+ self.axes_and_updating_artists["ax_omega_x"].artists["omega_z_line" + str(robot_index)] = omega_z_line
+ raise NotImplementedError("real time ain't implemented yet, sorry")
+ else:
+ self.fixed_artists["v_x_line" + str(robot_index)] = v_x_line
+ self.fixed_artists["v_y_line" + str(robot_index)] = v_y_line
+ self.fixed_artists["v_z_line" + str(robot_index)] = v_z_line
+ self.fixed_artists["omega_x_line" + str(robot_index)] = omega_x_line
+ self.fixed_artists["omega_y_line" + str(robot_index)] = omega_y_line
+ self.fixed_artists["omega_z_line" + str(robot_index)] = omega_z_line
+
+ self.point_in_time_ax_v_x_line = ax_v_x.axvline(x=0, color='red', animated=True)
+ self.point_in_time_ax_v_y_line = ax_v_y.axvline(x=0, color='red', animated=True)
+ self.point_in_time_ax_v_z_line = ax_v_z.axvline(x=0, color='red', animated=True)
+ self.point_in_time_ax_omega_x_line = ax_omega_x.axvline(x=0, color='red', animated=True)
+ self.point_in_time_ax_omega_y_line = ax_omega_y.axvline(x=0, color='red', animated=True)
+ self.point_in_time_ax_omega_z_line = ax_omega_z.axvline(x=0, color='red', animated=True)
+ self.axes_and_updating_artists["ax_v_x"].artists["point_in_time_ax_v_x_line"] = self.point_in_time_ax_v_x_line
+ self.axes_and_updating_artists["ax_v_z"].artists["point_in_time_ax_v_y_line"] = self.point_in_time_ax_v_y_line
+ self.axes_and_updating_artists["ax_v_z"].artists["point_in_time_ax_v_z_line"] = self.point_in_time_ax_v_z_line
+ self.axes_and_updating_artists["ax_omega_x"].artists["point_in_time_ax_omega_x_line"] = \
+ self.point_in_time_ax_omega_x_line
+ self.axes_and_updating_artists["ax_omega_x"].artists["point_in_time_ax_omega_y_line"] = \
+ self.point_in_time_ax_omega_y_line
+ self.axes_and_updating_artists["ax_omega_x"].artists["point_in_time_ax_omega_z_line"] = \
+ self.point_in_time_ax_omega_z_line
self.canvas_imu = FigureCanvasTkAgg(self.fig_imu, master=self.frame_imu)
+ self.canvas_imu.draw()
+ self.background_imu = self.canvas_imu.copy_from_bbox(self.fig_imu.bbox)
self.canvas_imu_widget = self.canvas_manip_graphs.get_tk_widget()
self.canvas_imu_widget.grid(row=0, column=0)
self.canvas_imu._tkcanvas.grid(row=1, column=0)
- self.canvas_imu.draw()
+ #self.canvas_imu.draw()
# i don't even remember what these toolbars are lol
# pack_toolbar=False will make it easier to use a layout manager later on.
- toolbar_manipulator = NavigationToolbar2Tk(canvas_manipulator, frame_manipulator, pack_toolbar=False)
- toolbar_manipulator.update()
- toolbar_manipulator.grid(column=0, row=2)
- toolbar_manip_graphs = NavigationToolbar2Tk(canvas_manip_graphs, frame_manip_graphs, pack_toolbar=False)
- toolbar_manip_graphs.update()
- toolbar_manip_graphs.grid(column=0, row=2)
- toolbar_ee = NavigationToolbar2Tk(canvas_ee, frame_ee, pack_toolbar=False)
- toolbar_ee.update()
- toolbar_ee.grid(column=0, row=2)
- toolbar_imu = NavigationToolbar2Tk(canvas_imu, frame_imu, pack_toolbar=False)
- toolbar_imu.update()
- toolbar_imu.grid(column=0, row=2)
-
-
-
-# NOTE: this thing is not used
-same_starting_position_on_off_var = IntVar()
-same_starting_position_on_off_var.set(0)
-same_starting_position_checkbutton= Checkbutton(root, text = "same starting position on/off",
- variable = same_starting_position_on_off_var,
- onvalue = 1,
- offvalue = 0,
-# command=same_starting_position_cmd,
- )
-
-
-#######################################################################
-# PLACING ELEMENTS IN THE GUI #
-#######################################################################
- # TODO put this into init
-
-# LEFT PANE BELOW MANIP/EE PLOTS
-frame_below_manipulator_plot = Frame(frame_manipulator)
-frame_below_manipulator_plot.grid(column=0, row=3)
-
-# set controller 1
-frame_controller_menu1 = Frame(frame_below_manipulator_plot)
-controller_string1 = StringVar(frame_controller_menu1)
-controller_string1.set("invKinm_dampedSquares") # default value
-controller_menu1 = OptionMenu(frame_controller_menu1, controller_string1,
- "invKinmQPSingAvoidE_kI",
- "invKinm_Jac_T",
- "invKinm_PseudoInv",
- "invKinm_dampedSquares",
- "invKinm_PseudoInv_half",
- "invKinmQP",
- "invKinmQPSingAvoidE_kI",
- "invKinmQPSingAvoidE_kM",
- "invKinmQPSingAvoidManipMax",
- )
-controller_string1.set("invKinm_dampedSquares")
-controller_menu1.grid(column=1, row=0)
-Label(frame_controller_menu1, text="Robot 1 controller:", background='yellow').grid(row=0, column=0, pady=4, padx = 4)
-frame_controller_menu1.grid(column=0, row=0)
-
-# set controller 2
-frame_controller_menu2 = Frame(frame_below_manipulator_plot)
-controller_string2 = StringVar(frame_controller_menu2)
-controller_menu2 = OptionMenu(frame_controller_menu2, controller_string2,
- "invKinmQPSingAvoidE_kI",
- "invKinm_Jac_T",
- "invKinm_PseudoInv",
- "invKinm_dampedSquares",
- "invKinm_PseudoInv_half",
- "invKinmQP",
- "invKinmQPSingAvoidE_kI",
- "invKinmQPSingAvoidE_kM",
- "invKinmQPSingAvoidManipMax",
- )
-controller_string2.set("invKinm_Jac_T") # default value
-Label(frame_controller_menu1, text="Robot 1 controller:", background='black', foreground='white').grid(row=0, column=0, pady=4, padx = 4)
-controller_menu2.grid(column=1, row=0)
-Label(frame_controller_menu2, text="Robot 2 controller:", background='#EE82EE').grid(row=0, column=0, pady=4, padx = 4)
-frame_controller_menu2.grid(column=0, row=1)
-
-
-ellipse_on_off_var = IntVar()
-ellipse_on_off_var.set(1)
-
-ellipse_checkbutton= Checkbutton(frame_below_manipulator_plot, text = "ellipse on/off",
- variable = ellipse_on_off_var,
- onvalue = 1,
- offvalue = 0,
- command=add_remove_ellipse,
- )
-ellipse_checkbutton.grid(column=1, row=0)
-
-
-frame_goal_x = Frame(frame_below_manipulator_plot)
-Label(frame_goal_x, text="goal x").grid(row=0, column=0, pady=4, padx = 4)
-slider_goal_x = Scale(frame_goal_x, from_=-1.0, to=1.0, length=SLIDER_SIZE, orient=HORIZONTAL,
- command=update_goal_x)
-slider_goal_x.grid(column=1, row=0)
-frame_goal_x.grid(column=1, row=1)
-
-frame_goal_y = Frame(frame_below_manipulator_plot)
-Label(frame_goal_y, text="goal y").grid(row=0, column=0, pady=4, padx = 4)
-slider_goal_y = Scale(frame_goal_y, from_=-1.0, to=1.0, length=SLIDER_SIZE, orient=HORIZONTAL,
- command=update_goal_y)
-slider_goal_y.grid(column=1, row=0)
-frame_goal_y.grid(column=1, row=2)
-
-
-frame_goal_z = Frame(frame_below_manipulator_plot)
-Label(frame_goal_z, text="goal z").grid(row=0, column=0, pady=4, padx = 4)
-slider_goal_z = Scale(frame_goal_z, from_=-1.0, to=1.0, length=SLIDER_SIZE, orient=HORIZONTAL,
- command=update_goal_z)
-slider_goal_z.grid(column=1, row=0)
-frame_goal_z.grid(column=1, row=3)
-
-
-frame_update = Frame(frame_manip_graphs)
-Label(frame_update, text="Time").grid(row=0, column=0, pady=4, padx = 4)
-slider_update = Scale(frame_update, from_=0, to=n_iters - 1, length=SLIDER_SIZE, orient=HORIZONTAL,
- command=update_points)#, label="Frequency [Hz]")
-slider_update.grid(column=1, row=0)
-frame_update.grid(column=0, row=3)
-
-frame_update2 = Frame(frame_imu)
-Label(frame_update2, text="Time").grid(row=0, column=0, pady=4, padx = 4)
-slider_update = Scale(frame_update2, from_=0, to=n_iters - 1, length=SLIDER_SIZE, orient=HORIZONTAL,
- command=update_points)#, label="Frequency [Hz]")
-slider_update.grid(column=1, row=0)
-frame_update2.grid(column=0, row=3)
-
-
-button_quit = Button(master=frame_manip_graphs, text="Quit", command=root.destroy)
-button_reset = Button(master=frame_manip_graphs, text="New run", command=reset)
-button_reset.grid(column=0, row=4)
-button_quit.grid(column=0, row=5)
-
-button_quit2 = Button(master=frame_imu, text="Quit", command=root.destroy)
-button_reset2 = Button(master=frame_imu, text="New run", command=reset)
-button_reset2.grid(column=0, row=4)
-button_quit2.grid(column=0, row=5)
-
-update_points(0)
-slider_goal_x.set(ik_env.goal[0])
-slider_goal_y.set(ik_env.goal[1])
-slider_goal_z.set(ik_env.goal[2])
-
-
+ self.toolbar_manipulator = NavigationToolbar2Tk(self.canvas_manipulator, self.frame_manipulator, pack_toolbar=False)
+ self.toolbar_manipulator.update()
+ self.toolbar_manipulator.grid(column=0, row=2)
+ self.toolbar_manip_graphs = NavigationToolbar2Tk(self.canvas_manip_graphs, self.frame_manip_graphs, pack_toolbar=False)
+ self.toolbar_manip_graphs.update()
+ self.toolbar_manip_graphs.grid(column=0, row=2)
+ self.toolbar_ee = NavigationToolbar2Tk(self.canvas_ee, self.frame_ee, pack_toolbar=False)
+ self.toolbar_ee.update()
+ self.toolbar_ee.grid(column=0, row=2)
+ self.toolbar_imu = NavigationToolbar2Tk(self.canvas_imu, self.frame_imu, pack_toolbar=False)
+ self.toolbar_imu.update()
+ self.toolbar_imu.grid(column=0, row=2)
+
+ # update once to finish initialization
+ self.update_points(0)
#######################################################################
# functions for widgets to control the plots #
@@ -549,78 +683,106 @@ slider_goal_z.set(ik_env.goal[2])
# you define what needs to happen to plots in Figure below,
# and call canvas.draw
def update_points(self, new_val):
- start = ttime.time()
- #fig_manipulator.canvas.restore_region(background_manipulator)
- self.canvas_manipulator.restore_region(self.background_manipulator)
+ start = time.time()
+ if self.blit:
+ self.canvas_manipulator.restore_region(self.background_manipulator)
+ # always blit the rest
+ self.canvas_ee.restore_region(self.background_ee)
+ self.canvas_manip_graphs.restore_region(self.background_manip_graphs)
+ self.canvas_imu.restore_region(self.background_imu)
index = int(np.floor(float(new_val)))
# ee plot here, but NOTE: UNUSED
+ # all these are in lists as that's what the line plot wants,
+ # despite the fact that we have single points
+ self.point_in_time_eigen1_line.set_xdata([self.t[index]])
+ self.point_in_time_manip_index_line.set_xdata([self.t[index]])
+ self.point_in_time_dist_to_goal_line.set_xdata([self.t[index]])
+ self.point_in_time_ax_v_x_line.set_xdata([self.t[index]])
+ self.point_in_time_ax_v_y_line.set_xdata([self.t[index]])
+ self.point_in_time_ax_v_z_line.set_xdata([self.t[index]])
+ self.point_in_time_ax_omega_x_line.set_xdata([self.t[index]])
+ self.point_in_time_ax_omega_y_line.set_xdata([self.t[index]])
+ self.point_in_time_ax_omega_z_line.set_xdata([self.t[index]])
+
self.ik_env.ax.set_title(str(index) + 'th iteration toward goal')
# animate 3d manipulator
for robot_index, robot in enumerate(self.ik_env.robots):
self.ik_env.robots[robot_index].setJoints(self.ik_env.data[robot_index]["qs"][index])
self.ik_env.robots[robot_index].drawStateAnim()
-
- # NEW AND BROKEN
- for link in robot.lines:
- for line in link:
- self.ik_env.ax.draw_artist(line)
- # all these are in lists as that's what the line plot wants,
- # despite the fact that we have single points
- self.point_in_time_eigen1_line.set_xdata([time[index]])
- self.point_in_time_manip_index_line.set_xdata([time[index]])
- self.point_in_time_dist_to_goal_line.set_xdata([time[index]])
- self.point_in_time_ax_v_x_line.set_xdata([time[index]])
- self.point_in_time_ax_v_y_line.set_xdata([time[index]])
- self.point_in_time_ax_v_z_line.set_xdata([time[index]])
- self.point_in_time_ax_omega_x_line.set_xdata([time[index]])
- self.point_in_time_ax_omega_y_line.set_xdata([time[index]])
- self.point_in_time_ax_omega_z_line.set_xdata([time[index]])
-
- # ellipsoid update
- radii = 1.0/self.ik_env.data[robot_index]["manip_ell_eigenvals"][index] * 0.1
- u = np.linspace(0.0, 2.0 * np.pi, 60)
- v = np.linspace(0.0, np.pi, 60)
- x = radii[0] * np.outer(np.cos(u), np.sin(v))
- y = radii[1] * np.outer(np.sin(u), np.sin(v))
- z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
- for i in range(len(x)):
- for j in range(len(x)):
- [x[i,j],y[i,j],z[i,j]] = np.dot([x[i,j],y[i,j],z[i,j]], self.ik_env.data[robot_index]["manip_elip_svd_rots"][index]) + self.ik_env.data[robot_index]['p_es'][index]
+# for link in robot.lines:
+# for line in link:
+# self.ik_env.ax.draw_artist(line)
+ self.ik_env.p_e_point_plots[robot_index].set_data([self.ik_env.data[robot_index]['p_es'][index][0]], [self.ik_env.data[robot_index]['p_es'][index][1]])
+ self.ik_env.p_e_point_plots[robot_index].set_3d_properties([self.ik_env.data[robot_index]['p_es'][index][2]])
if self.ellipse_on_off_var.get():
+ self.blit = False
try:
self.ik_env.ellipsoid_surf_plots[robot_index].remove()
except ValueError:
pass
+
+ # ellipsoid update
+ radii = 1.0/self.ik_env.data[robot_index]["manip_ell_eigenvals"][index] * 0.1
+ u = np.linspace(0.0, 2.0 * np.pi, 60)
+ v = np.linspace(0.0, np.pi, 60)
+ x = radii[0] * np.outer(np.cos(u), np.sin(v))
+ y = radii[1] * np.outer(np.sin(u), np.sin(v))
+ z = radii[2] * np.outer(np.ones_like(u), np.cos(v))
+ for i in range(len(x)):
+ for j in range(len(x)):
+ [x[i,j],y[i,j],z[i,j]] = \
+ np.dot([x[i,j],y[i,j],z[i,j]], self.ik_env.data[robot_index]["manip_elip_svd_rots"][index]) \
+ + self.ik_env.data[robot_index]['p_es'][index]
self.ik_env.ellipsoid_surf_plots[robot_index] = self.ik_env.ax.plot_surface(x, y, z, rstride=3, cstride=3,
color='pink', linewidth=0.1,
alpha=0.3, shade=True)
-
- self.ik_env.p_e_point_plots[robot_index].set_data([ik_env.data[robot_index]['p_es'][index][0]], [ik_env.data[robot_index]['p_es'][index][1]])
- self.ik_env.p_e_point_plots[robot_index].set_3d_properties([ik_env.data[robot_index]['p_es'][index][2]])
- # TODO: blit the rest
- canvas_manipulator.blit(fig_manipulator.bbox)
- canvas_manipulator.flush_events()
- canvas_manip_graphs.draw()
- # non blitted (bad)
- canvas_imu.draw()
- self.canvas_ee.draw()
+ else:
+ self.blit = True
+
+ # now draw all updating artists all at once
+ # for whatever reason it does not update robot lines, so i have that above
+ for ax_key in self.axes_and_updating_artists:
+ for artist_key in self.axes_and_updating_artists[ax_key].artists:
+ self.axes_and_updating_artists[ax_key].ax.draw_artist(\
+ self.axes_and_updating_artists[ax_key].artists[artist_key])
+
+ if self.blit:
+ self.canvas_manipulator.blit(self.fig_manipulator.bbox)
+ self.canvas_manipulator.flush_events()
+ if not self.blit:
+ self.canvas_manipulator.draw()
+ # always blit the other ones, this is just because of the surf plot
+ self.canvas_ee.blit(self.fig_ee.bbox)
+ self.canvas_ee.flush_events()
+ self.canvas_manip_graphs.blit(self.fig_manip_graphs.bbox)
+ self.canvas_manip_graphs.flush_events()
+ self.canvas_imu.blit(self.fig_imu.bbox)
+ self.canvas_imu.flush_events()
+# self.canvas_imu.draw()
+# self.canvas_ee.draw()
# TODO update may not be needed as we're going by slider here
- root.update()
- end = ttime.time()
+ self.root.update()
+ end = time.time()
print("time per update:", end - start)
print("fps", 1 / (end - start))
+ def drawAll(self):
+ self.canvas_ee.draw()
+ self.canvas_manipulator.draw()
+ self.canvas_manip_graphs.draw()
+ self.canvas_imu.draw()
+
def update_goal_x(self, new_val):
goal_x = float(new_val)
self.ik_env.goal[0] = goal_x
self.ik_env.goal_point_plot.set_data([self.ik_env.goal[0]], [self.ik_env.goal[1]])
self.ik_env.goal_point_plot.set_3d_properties([self.ik_env.goal[2]])
- self.canvas_ee.draw()
+# self.canvas_ee.draw()
self.canvas_manipulator.draw()
- self.canvas_manip_graphs.draw()
+# self.canvas_manip_graphs.draw()
# TODO update may not be needed as we're going by slider here
self.root.update()
@@ -629,9 +791,9 @@ slider_goal_z.set(ik_env.goal[2])
self.ik_env.goal[1] = goal_y
self.ik_env.goal_point_plot.set_data([self.ik_env.goal[0]], [self.ik_env.goal[1]])
self.ik_env.goal_point_plot.set_3d_properties([self.ik_env.goal[2]])
- self.canvas_ee.draw()
+# self.canvas_ee.draw()
self.canvas_manipulator.draw()
- self.canvas_manip_graphs.draw()
+# self.canvas_manip_graphs.draw()
# TODO update may not be needed as we're going by slider here
self.root.update()
@@ -640,58 +802,69 @@ slider_goal_z.set(ik_env.goal[2])
self.ik_env.goal[2] = goal_z
self.ik_env.goal_point_plot.set_data([self.ik_env.goal[0]], [self.ik_env.goal[1]])
self.ik_env.goal_point_plot.set_3d_properties([self.ik_env.goal[2]])
- self.canvas_ee.draw()
+# self.canvas_ee.draw()
self.canvas_manipulator.draw()
- self.canvas_manip_graphs.draw()
+# self.canvas_manip_graphs.draw()
# TODO update may not be needed as we're going by slider here
self.root.update()
+ def drawAndUpdateBackground(self):
+ self.drawAll()
+ self.background_ee = self.canvas_ee.copy_from_bbox(self.fig_ee.bbox)
+ self.background_manipulator = self.canvas_manipulator.copy_from_bbox(self.fig_manipulator.bbox)
+ self.background_manip_graphs = self.canvas_manip_graphs.copy_from_bbox(self.fig_manip_graphs.bbox)
+ self.background_imu = self.canvas_imu.copy_from_bbox(self.fig_imu.bbox)
-# TODO fix
- def reset():
- ik_env.reset()
+ def reset(self):
+ self.ik_env.reset()
# ik_env.goal_point_plot.remove()
# ik_env.goal_point_plot = drawPoint(ik_env.ax, ik_env.goal, 'red', 'o')
- # print(controller_string1.get())
- # print(controller_string2.get())
- controller1 = getController(controller_string1.get())
- controller2 = getController(controller_string2.get())
- controllers = [controller1, controller2]
- for robot_index, robot in enumerate(ik_env.robots):
- ik_env.data.append(makeRun(controllers[robot_index], ik_env, n_iters, robot_index))
- trajectory_plots[robot_index].set_data(ik_env.data[robot_index]['p_es'][:,0], ik_env.data[robot_index]['p_es'][:,1])
- trajectory_plots[robot_index].set_3d_properties(ik_env.data[robot_index]['p_es'][:,2])
- eigen1_lines[robot_index].set_ydata(ik_env.data[robot_index]["manip_ell_eigenvals"][:,0])
- eigen2_lines[robot_index].set_ydata(ik_env.data[robot_index]["manip_ell_eigenvals"][:,1])
- eigen3_lines[robot_index].set_ydata(ik_env.data[robot_index]["manip_ell_eigenvals"][:,2])
- manip_index_lines[robot_index].set_ydata(ik_env.data[robot_index]['manip_indeces'])
- dist_to_goal_lines[robot_index].set_ydata(ik_env.data[robot_index]['dists_to_goal'])
- update_points(0)
- canvas_ee.draw()
- canvas_manipulator.draw()
- canvas_manip_graphs.draw()
- root.update()
-
- def play():
- pass
+ self.controller1 = getController(self.controller_string1.get())
+ self.controller2 = getController(self.controller_string2.get())
+ controllers = [self.controller1, self.controller2]
+ for robot_index, robot in enumerate(self.ik_env.robots):
+ self.ik_env.data.append(makeRun(controllers[robot_index], self.ik_env, self.n_iters, robot_index))
+ self.trajectory_plots[robot_index].set_data(self.ik_env.data[robot_index]['p_es'][:,0], self.ik_env.data[robot_index]['p_es'][:,1])
+ self.trajectory_plots[robot_index].set_3d_properties(self.ik_env.data[robot_index]['p_es'][:,2])
+ self.eigen1_lines[robot_index].set_ydata(self.ik_env.data[robot_index]["manip_ell_eigenvals"][:,0])
+ self.eigen2_lines[robot_index].set_ydata(self.ik_env.data[robot_index]["manip_ell_eigenvals"][:,1])
+ self.eigen3_lines[robot_index].set_ydata(self.ik_env.data[robot_index]["manip_ell_eigenvals"][:,2])
+ self.manip_index_lines[robot_index].set_ydata(self.ik_env.data[robot_index]['manip_indeces'])
+ self.dist_to_goal_lines[robot_index].set_ydata(self.ik_env.data[robot_index]['dists_to_goal'])
+ self.update_points(0)
+ self.drawAndUpdateBackground()
+ self.root.update()
+
+# TODO: use the same API you use for real-time plotting:
+# --> just manually put things into the queue here
+ def play(self):
+ for i in range(self.n_iters):
+ self.update_points(i)
# ellipse on/off
- def add_remove_ellipse():
+ def add_remove_ellipse(self):
+ # this just deletes them le mao
+ # for artist_key in self.axes_and_updating_artists["ax_manipulators"].artists:
+ # if "robot" in artist_key:
+ # self.axes_and_updating_artists["ax_manipulators"].artists[artist_key].set_animated(bool(self.ellipse_on_off_var))
+
try:
- for robot_index, robot in enumerate(ik_env.robots):
- ik_env.ellipsoid_surf_plots[robot_index].remove()
+ for robot_index, robot in enumerate(self.ik_env.robots):
+ self.ik_env.ellipsoid_surf_plots[robot_index].remove()
except ValueError:
pass
+ self.drawAndUpdateBackground()
+ self.root.update()
-if name == "__main__":
+if __name__ == "__main__":
queue = Queue()
root = Tk()
- gui = ManipulatorVisualMotionAnalyzer(root, queue)
+ gui = ManipulatorVisualMotionAnalyzer(root, queue, False, data=None)
- # have it 'cos from tkinter import *
+ # have mainloop 'cos from tkinter import *
mainloop()
# alternative if someone complains
#root.mainloop()
diff --git a/python/ur_simple_control/visualize/robot_stuff/InverseKinematics.py b/python/ur_simple_control/visualize/robot_stuff/InverseKinematics.py
index 9cd579d..b6c4539 100644
--- a/python/ur_simple_control/visualize/robot_stuff/InverseKinematics.py
+++ b/python/ur_simple_control/visualize/robot_stuff/InverseKinematics.py
@@ -23,9 +23,11 @@ class InverseKinematicsEnv:
print('env created')
self.chill_reset = False
# TODO write a convenience dh_parameter loading function
- self.robot = Robot_raw(robot_name="no_sim")
- self.robots = [self.robot]
- self.init_qs_robot = []
+ #self.robot = Robot_raw(robot_name="no_sim")
+ #self.robots = [self.robot]
+ #self.init_qs_robot = []
+ self.robots = [Robot_raw(robot_name="no_sim")]
+ self.init_qs_robots = []
self.damping = 5
self.error_vec = None
self.n_of_points_done = 0
@@ -67,7 +69,7 @@ class InverseKinematicsEnv:
def simpleStep(self, q_dots, damping, index):
- self.robot.forwardKinmViaPositions(q_dots / self.damping, damping)
+ self.robots[index].forwardKinmViaPositions(q_dots / self.damping, damping)
self.n_of_tries_for_point += 1
# done = False
# if error_test(self.robot.p_e, self.goal):
@@ -117,15 +119,15 @@ class InverseKinematicsEnv:
# initialize to a random starting state and check whether it makes any sense
thetas = []
- for joint in self.robot.joints:
+ for joint in self.robots[0].joints:
thetas.append(6.28 * np.random.random() - 3.14)
- self.robot.forwardKinmViaPositions(thetas, 1)
+ self.robots[0].forwardKinmViaPositions(thetas, 1)
if self.chill_reset == True:
sensibility_check = False
while not sensibility_check:
thetas = []
- for joint in self.robot.joints:
+ for joint in self.robots[0].joints:
thetas.append(6.28 * np.random.random() - 3.14)
self.robot.forwardKinmViaPositions(thetas , 1)
if calculateManipulabilityIndex(self.robot) > 0.15:
@@ -133,10 +135,13 @@ class InverseKinematicsEnv:
# for i, joint in enumerate(self.robots[robot_index].joints):
# joint.theta = self.robots[0].joints[i].theta
+ for robot_index in range(1, len(self.robots)):
+ for i, joint in enumerate(self.robots[robot_index].joints):
+ joint.theta = self.robots[0].joints[i].theta
# NOTE: not needed and i'm not fixing _get_obs for more robots
- obs = self._get_obs()
- return obs
+# obs = self._get_obs()
+# return obs
def render(self, mode='human', width=500, height=500):
try:
@@ -155,22 +160,22 @@ class InverseKinematicsEnv:
plt.xlim([-1.5,1.5])
plt.ylim([-0.5,1.5])
color_link = 'black'
- self.robot.initDrawing(self.ax, color_link)
+ self.robots[0].initDrawing(self.ax, color_link)
self.drawingInited = True
plt.pause(0.1)
# put this here for good measure
- self.robot.drawStateAnim()
+ self.robots[0].drawStateAnim()
self.bg = self.fig.canvas.copy_from_bbox(self.fig.bbox)
# manual blitting basically
# restore region
self.fig.canvas.restore_region(self.bg)
# updating all the artists (set new properties (position bla))
- self.robot.drawStateAnim()
+ self.robots[0].drawStateAnim()
# now you need to manually draw all the artist (otherwise you update everything)
# thank god that's just lines, all in a list.
# btw, that should definitely be a dictionary, not a list,
# this makes the code fully unreadable (although correct).
- for link in self.robot.lines:
+ for link in self.robots[0].lines:
for line in link:
self.ax.draw_artist(line)
self.fig.canvas.blit(self.fig.bbox)
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/InverseKinematics.cpython-310.pyc
index c59cab0f7519bb21e35ad76ebfa41a84b0376333..3fd6e4c1b57dbd18d122b40fa6d9a98e2a34fbde 100644
GIT binary patch
delta 1512
zcmdm^x?PPgpO=@5fq{V`<Z4RlS%r;!YK-+I3=9k<3@MDwjEoE^Of?Mgj3rDd%qc9r
zjOmOu4DrnA3^fe#EGeut4DqZfAU0bHTMa`zJDB7E%W{HAE-=Xr7UwD9&Ei|Ym%_e~
zF@-|}q>Dd=vxXsF04yt*!Vt`$$>mqYotig!H=_fi(Bwai3gT=<`APXD#kcq}^D;}~
z3yb3+{K=L~pBNP<>o8B|l4D?C;9z89<YS$Dn)#iw6axdpEw0p}qWq%xlGNgoTdZIm
zx0nm!Q;Q@S7#NDgC*NX`6O{p}<4sJ?$V@FuO^Hv>Ps}NjoXpEABd-Wj1SXU~YWR}#
za|=pKQsawK%M*)IOecG=#&c^iFfi0GWHHn*OHN+L>f^}|(j&ycz@W)e1U8wiFg_)}
zq__wqaf>xGFD1322y7;p02|CzoS9pYlNwx-THwyWz);M>z`!6e*_rJQqv~V@c6}EO
zkeNmx0%S-L$n;wr@#(20@%c%`Md~04O%MU`6o>_KRgnRRB@J>4TanD<$?S@vU@O4{
z*j|?6lGK9m$tT%8_}Lg37(mz=WIqq%WCac_mJ~)whRI$WiiQ$33|U~_0?r!76sCnt
zj36&_rLbl*6(yyx)iT#G7bm4N)H1@<fppb7)-YsoBXpGTfcVXfDeT!SMF}aaAYDb4
zCA=w2C43-#&5X@VE)21fwJat4Abt&tBtsT!GouSbEME*$Eo&_kNEGCzEY=!UNro(e
z8m4%G!ji%)aC+hJD`Eus*N}mMAzi-pb>QS59Fh)#MIiqd34&xm!B!*;V)24vlPxPh
zGq0rh7F%LLL26z~kvK?30z`mAnj6H@nC!!;sC0|9C^0WR^%iqZYThl*)V$K%)S|?a
z)LX128L1_SnxMcf<5Z2m#SKnh@!%-F#h8g6%dADI#i=DZpa9kbg*G!EBO4<dBMZ9#
zqZFeMBNr13BM5?ce2jb`_Ae1G2}UtSrvEJetE491=98Mt%%#RSXR;QT31i#jc&-^N
zDXfwVlfQE*Kp5iOdO{$P1^f#cz){2l;!XDFmSvt_ID2zGH#Z}r_~dGy*=nG$xW$=L
zlvo}B3I%X>xW$u}Q(Bx6pIVlhS5mCWUL-TwhSyLWoLs;JIL)vXrRJri7EPMm!mGqD
z!pO$R#mL7b!YIJZ$0RU$EANg7v;YSOYcwb@L1BA~y)-W~Ex#xiY-5o*C~-1@^GT5<
zNDbH?FafrV3mjzN+<Rd1QNCy;kd!7PL<5Ko31hHY*5sW0;?#4Ko%o{}%O=m`myri&
zKbGRu)D(y^um+IYqACUkhO3ir@?VPuXDu6$)wUqQ2}B^A49-TNtaXbeIlrJ1>}Y7d
zE&}D0A{UT)EG`H8?AByHK^Fl}kP<IAQ8qbAP|Haf#0Te3QxMA<M0kLRVh~XZBFaGo
z$f01fkbD5~#^iH?Vx00ELL34dqFic|-wGBoicgLgl9vhv>0<^FV1)=mY4Qvq6#yXC
BO#J`=
delta 1378
zcmdn4wnvpOpO=@5fq{XcAT=d*g8W84HOBfH1_p)_h7`tTMn;AdrW%HL#uBC!<`kA*
z#u|or<{E}9=5!E?C7q#$A)YmbwT2;{Ed|78PhqQJi01&4oM2flFv$%jdBEblC45=@
z3;0vm7c!=Bh=6nnq;S?S#0!FDg;E%TC$DDoU=*JGg;7D4wJ1L+zvLDhm@2--o0*qc
z5?@#x59UwSX8OdaG+B;$GLHfS0|N&m8zT=B7vtpJ%<mMX85kIDaita&<rl@5q!yPH
z@qz@H3*u9Yq!<_&iX<kVVUZJ+1qt#dCTC=(mZhe|r{^c;6iH3~&mtqQ1X2VhltF6v
zlJj#5N=s7Xi&D!Ii&BgyTd~Hot1&Pz)G%aCUc&0*Cje3}%)r2)$x;M1m8~#7CBLM&
z2qbZfH8U?IwW0`Y9+&_d%2k}1Tac3)T#{Pg%D})-%)-FHAi^lbSS3ICA-mY*5VnJi
zYLms-^<6-2FERqz05YuzWX&y(`1I70`23{eA`Os)CWwIe48+m`5e6Va2IMBTBH78E
z?24jbyTJt50W8HOsRf~v_p^KOu`)0)urV+&ID;H=WO5~+EFU<|n9~_*8EY7_SSC;7
zP}CFyu@-RFFs3jqWMTw)o-2hlo2e)!g{_vkhPgNfS+%}3s_GJM5Wkr*g*}_4C@O^&
zq@&2RhG7BELWWwF8kWgQoVr51MIZ+k34xpr@@J9A<U~$M0SORa5=4N5fqQa4ry^&O
zImmCUMY5AGajN<iS%Rd&o&ytLAF>vu7N?eEfjk8YCN^e1Mm9z^MizD+Mkz)<MlL26
zMi7MX_(1Gm96T(HRZ^25vWrg6=F(>DnLM4#gt2Mz8Lk<VGq@Ed_i*cRE#P0s0QNo4
z<TKo|%+m`eZ+^qg&B!P*`3KKzHITD!ai$a{mIvf#=9Pfc+by27oYLZq_|&r0ypm!~
z_9EHIJ9!Po!43rzVArx0rRJri7WGbM<Wu4oU}R(DV&q~HVH9BIV&a)>#J3{?Eo{J{
z5(@G*C}?i6m*!=r<rn3GZG;346F4zq4H_<R(10_??#WL4(Mlk9Xfi@HfY^}a3U(N4
za!!76>dDFb`J)*NCQA#*$b-{3OL1yy3Pc%L14wOA2?GPeg~?$8*W$s+)&}G}TTmWi
zEy+kNNi0G*8k|T$3HKIDa(+Q2*xAtRQ)Ca)<^UqFL@U^DS0{H0x(I;MYmo~`9hfMX
zd{<B_Q3b>Yr+!lq%Nj&DfrwlXkq;sYK?KODV6%|?fba-20|NsnE{nxLg$n~0GlwjP
i5QhMVD3|i&4530siOIKw<fZ&U27#1fm@!#cSOoyJ;w1b4
diff --git a/python/ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc b/python/ur_simple_control/visualize/robot_stuff/__pycache__/forw_kinm.cpython-310.pyc
index 8d2d9b38d7d0ce710da387e407cfcc3cab8e0f3f..9bf21a63a8d2e7f40aea156cae95fc80c0768c7a 100644
GIT binary patch
delta 1625
zcmcZ;xj&LCpO=@5fq{Wx;@yp0y6lW8lP%a)7-vq7Wlv*VF!>$36c6h{mKerb&Kia+
z28aL~Sbz&5zz!DRh6zZr)i7i+E@YXU%Hhq(F?l10)Z`l+`D}&^3=Bn<lYKeMxzw1{
z7}Xdx7$w*?pX0pA$YutTGM~JbTQSxW#54vGq98&FM96~(69xu`Tf!+tiRJ!9nW=dt
zi6xo&d5(FRxkW}G89tCqSZZ=fei2v@WZO#SA~ld4TS>lCVsUDb)n;v;W=2Ni$?JJt
z8Lc*d<=w?>=?pU41w^=l2sZ`>22JMYpa22kqF9h1m|z64gg}HKh;X0$LeM}1;&6~1
z>}fgqi6zEHx0nm!Q;WbJPy(p~d%$F}gHRN=KZtJyA`&Jq6;feK12NMlpBIu~jNJS}
z=m(>KC&+fbA{CH<j3Gs4lRt>)3z&nHfz1M&nl@Qq)PgZ`bB3ralT0Z{4(y0BkRImL
z%G6t|#mSikl|>bk7m6zzfVF@%gKYzcB-p@Ykj@m4ef;1+Dyjwv)J*;&Zo?M|Vm5*d
zWOOZR*lZ_J#Ax6SQVMZe5!hBSkRq_v6(H6m5P@Xd<Oh->G9X74fvnSHMY5y;q%?A~
zfK(YH3n=&}3#y7nA%fQfBn=J;o+7aMAOniLK%(j(!W%>&Yyx`;63QSKb0N|}5hx85
z`EC9w&C19aJ^8<kn#)WE28Q&!Ht7#P6*&(;B9|THF<yjCxA=-u6H~nMGxJJ<OA<>`
zi&#PS#e)bo5MewyQ`Va?a+0FR<dd>jw80JmCsj$1EZ8BlL996p3=Gf&STq+T5V`q;
z+#E&=P-05IpQ{l1Tk)rTQ8Y*f<b)zlkUNBmz;?jBeT%6evj}Xr@#HxQs`em5i@<h-
zf;56Hnh#=0fCyEP5iF3923dKFH?^XqC^0@IF}ENyFTE&olcE)){Q{6GZb%YEI9wQH
z01Jp%0wNF=f$ad9Tm-gg;pR#uCq{!{kRcpJ;6wlp0&p6`5(GtHtqV85R#s=M2gP;~
zYG#9k06g|JIU(Kyn+bB=Ew-Y3a5R8D3=bGcpn%E*O?D&?vuA<gt=OmtoP?1w9LP^P
zlUWt{CofR_7hwQ$JtsKMZ*k;g<`owtCPOnV$WDk8R)X|^ZJhyPf&8J#2C)mA4#6SJ
zTAYzzQhbXAL={C&{-)O9?gUaD03xD51V~>|6NohtL`(w_U<1Lyxd_Bv1|pV&h!r4W
zHHg@_`KbC<CM*0Q2FjF0;4%rEgg}|52vnGB@)j9S-lC--iY2^?j3&R-QWOC>K~v}!
zYg$oaZYo$)(cH;$+Nx~e0?L1~r?#c2HOL`0Ai@?zXoHBz$<wuy_z!>rkn0v}UTJPY
YWs%ur4jnVb*2(rdQH%#B&(d)M0J*$gqW}N^
delta 1647
zcmdlVc_)%9pO=@5fq{XcAax^`E<0n%WD9l`#)Xq(+0z)aCJS'zQHrLgz1#xT}$
z)-Yr-Km<A9f?NnePPia9Oi&Ue%?J^n9LnL%$TfK(hZJv!CgUxR#JtSh#FEsM$-6o7
z*^C$%7>cYW8*r9$X|ZWBYBB0CN-%BS$a#~I%^W0UF?j;FqLdYgX#yfdL4*{DkOvW_
zlTUL?Y8f*yFx=uxDM~C4OHD4xFLKPw%q;?$ypp*{4J5}_lJAsQoLXePnUANLk<n!G
zBwklW>&-WKcQISKfXs0P5pE#Doq>TtlleI)Xh66q4kQRB7(px{5FrR6JSLwOG|+%J
z5@ZK^T26jqiLucw=7RXtBCunXK<dDbHJz+16vZ6?;#-4=#K|2(D)Q+dW(G(zTW(fj
za(vb;mfWoPd@xy16t(%Z&<{odFOY-yic~<#7(<H8CtnuP7q9^71RD>wD1EY!s0Cxx
zW^YkhCYdsj9N1muAU({fm8rK_i<2`8DvK&7w}>knfVF@%gB<`4NU(t^Af2fo`}o1(
zR#XELsGWRW+=ed-#B2f?$mm+sxLHx6h%vzfq!i+`BCxGuAVpxSD?zNuAOeeRw^)h`
zi%M=Wr=*q?f!tUGvR#uE$<9WQ&Zy0wCCeD=u|!Z2$mSwMgm{9Kg9D1E2y8vbz#?yu
zs5*%70TBp?fPD;!7m$az5Ge<oYKr_p%5|W`<QLLE8Dl0tl2LP+#lXOjp4TS*;in?!
z0Z7VV2L%By!qQuOMX8A?Uiq1MCBY?$C8<TMAY&3h1RIDjnd~F$%@{R#hU^PQlgW*8
z!s2s4!gCoI7@&!@XdXx)YV&HjIgA#dgqMClS0VJb;!pdc7?2Fe#v)FTwL(Q;BjFyt
z#Z-`41h&d#a=n78J;=}^us6d%8o?GV0I?)Mgeu4g7D$+bth~jWT2WGz7@v}uTacNT
zUKF+Yv4RyN$3l=Y?jn$h;tEAeK{^rUf{g?jR0KA6(dHN>Cq{!1kQR<2aAH6Nx+F*|
z*fn5XU_-!K7i~VLtd2dJ!5)T3vnD6RH()bC1;H)0qI_^5gZ&2&2S^y0f?UCl<UjT-
zP^=Ui6@im7QeFf(OlNb6DlemfAxIM^I67}}<YeX*7bGS_Gc(9Si2bWT8o*Z0oV-y@
zR2Uq3ti>7mCB?T`KvYrG<eO?8?#>`}fgmCpM1Vcp3}Q_J5z|4$Ob`JMyTu^ZauBfs
zM63i6Ye2-N$+bG-oA;~FW3s}Q6A(cJ&J*DB37kYg*`Wwj#B1^vnM|Ipr67tWfQpPK
zpV3kj0Xaic=oV{QQDSZ?SX0rw$&A{nY~aEwV6u+3rKk<aF}5JW4n$~!h^Wa`+DZHe
iL4n3~i#4w_x1h4feDZ5;Gsd>bN;*-D2PfC+H~|1yfMuKj
diff --git a/python/ur_simple_control/visualize/robot_stuff/forw_kinm.py b/python/ur_simple_control/visualize/robot_stuff/forw_kinm.py
index 0a54f80..39a2c69 100644
--- a/python/ur_simple_control/visualize/robot_stuff/forw_kinm.py
+++ b/python/ur_simple_control/visualize/robot_stuff/forw_kinm.py
@@ -133,14 +133,14 @@ class Robot_raw:
z_hat = self.joints[j].HomMat[0:3,2]
p = self.joints[j].HomMat[0:3,3]
- #line_x, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'r')#, animated=True)
- #line_y, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'g')#, animated=True)
- #line_z, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'b')#, animated=True)
- #line_p, = self.ax.plot(np.array([]),np.array([]),np.array([]), self.color_link)#, animated=True)
- line_x, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'r', animated=True)
- line_y, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'g', animated=True)
- line_z, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'b', animated=True)
- line_p, = self.ax.plot(np.array([]),np.array([]),np.array([]), self.color_link, animated=True)
+ line_x, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'r')#, animated=True)
+ line_y, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'g')#, animated=True)
+ line_z, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'b')#, animated=True)
+ line_p, = self.ax.plot(np.array([]),np.array([]),np.array([]), self.color_link)#, animated=True)
+ #line_x, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'r', animated=True)
+ #line_y, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'g', animated=True)
+ #line_z, = self.ax.plot(np.array([]),np.array([]),np.array([]), 'b', animated=True)
+ #line_p, = self.ax.plot(np.array([]),np.array([]),np.array([]), self.color_link, animated=True)
self.lines += [[line_x, line_y, line_z, line_p]]
avg_link_lenth += self.joints[j].d + self.joints[j].r
--
GitLab