From 596f6c5a2a8dc2cbfdca3807a83b902dbe24586f Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Sat, 23 Mar 2024 17:44:04 +0100
Subject: [PATCH] minor quality of life stuff. next up is real time plotting,
then robustifying drawing.
---
python/examples/clik.py | 7 +-
.../__pycache__/managers.cpython-311.pyc | Bin 17497 -> 17957 bytes
.../clik_point_to_point.cpython-311.pyc | Bin 9082 -> 9054 bytes
.../clik/clik_point_to_point.py | 4 +-
python/ur_simple_control/managers.py | 100 ++++++++++--------
.../__pycache__/get_model.cpython-311.pyc | Bin 4485 -> 4180 bytes
.../__pycache__/logging_utils.cpython-311.pyc | Bin 2279 -> 2599 bytes
python/ur_simple_control/util/get_model.py | 65 +++++-------
.../ur_simple_control/util/logging_utils.py | 11 +-
.../manipulator_visual_motion_analyzer.py | 4 +-
.../visualize/vedo_manipulator.py | 9 --
.../ur_simple_control/visualize/visualize.py | 19 +++-
12 files changed, 116 insertions(+), 103 deletions(-)
diff --git a/python/examples/clik.py b/python/examples/clik.py
index f066ec8..3a32ec2 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -58,6 +58,8 @@ def get_args():
default='1.0', help='not actually_used atm')
parser.add_argument('--debug-prints', action=argparse.BooleanOptionalAction, \
help="print some debug info", default=False)
+ parser.add_argument('--save-log', action=argparse.BooleanOptionalAction, \
+ help="save log data folder in whatever directory you called this in. if it doesn't exists there, it's saved to /tmp/data", default=False)
args = parser.parse_args()
if args.gripper and args.simulation:
@@ -68,7 +70,7 @@ def get_args():
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
- Mgoal = robot.defineGoalPoint()
+ Mgoal = robot.defineGoalPointCLI()
clik_controller = getClikController(args)
controlLoop = partial(controlLoopClik, robot, clik_controller)
log_dict = {
@@ -81,5 +83,6 @@ if __name__ == "__main__":
# something that generates a file name from something in args.
# also save args there
log_dict, final_iteration = loop_manager.run()
- saveLog(log_dict, final_iteration, args)
+ if args.save_log:
+ saveLog(log_dict, final_iteration, args)
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-311.pyc b/python/ur_simple_control/__pycache__/managers.cpython-311.pyc
index 053fed9e52e9806ecfd57867b690a7f41b92295f..5455df7279181101a5bbc6a30cb325d201481eab 100644
GIT binary patch
delta 5130
zcmccF!ML=Ck#9LKFBby?149(U|I}T%6Zs?<PfS#A^hsez;mi@u6^jyMWZ-1rWME=&
zXGq~{VMyU##>Bv|nhB%@grmfvay%^zDZFTM5-E(q44QlsUo?tpGTsvOO)btyPAmz_
zEG|vV$*f8(+MLNajY&b6fq|h&1Vo53FfiQWD^4s+jW0+nE{RXcOfD&snJmw;iZN~S
zDVAr9M<#D%wPP&V{DC!_ak4yzs6>%b4Py#Z6&nLX3S&0dQAL_H3|TyrWz97v|K#Oi
z%Hp5QC?`2Nn%#<<2jpH5&JvisfL+uW>}@b#5JZA-k!*=7h=$-4<{A!=VGtezLzckg
z7wq!<J|*lh5e9}V;mIF)MVPV#ChKtM*O%2WWMMPbtcJ0MAq(t2kk%BIDlP_wY!H*7
zNC?GB)*8kXwq?u=46B*J(hLj?j0|~VC~DYCSYUDt3@IE4)sv<9I43{mkmBU@D*{>e
zQe-kCXLP;DE&jC3yu_UN%#zfi#FEVXydrT>a0`Kgo24W(Hx&~6Vjw<yL4H9_YFbH=
zBuI%2NQfmnwX(Pf966k2@W=y2+b!m@%&J@LDVfCuIf<3G7z=MPrIg&_Nl8sAO^+`q
z%FHV%zQsCuKPNw%EXXv4$yYe78RaJnaLF<%PS)d6&}?U5U?^r|Vqj=sxFN65!E%K|
z=B}XFgpw=bS{DViuLx>?V9@4N{m7s_xt!~ay#RxNNKfSzex(l#OuU|qH-tqd6iqan
z5je?ug7pm%i7O&%S46a~ix^!JG1_2q#n|<VvHL|4k1HY`7eqWB@Jmc~=WgP-!Oh=e
zd4*eX@_FtcM&->)Jbg@zYLhqe`HC5VoCpe-TTFQcw^*xEi}H($geQyeJ8*-7p-2NH
zFFHA%U&09Fn_Fzfndy0@xwlx;iV|~EixfaX#a@=06Q7)$Qq0Q0z@PvEMT#J8f|FPB
zD>EKje3qY)QF-z?0c}+!kgyeq0HwZ_3`NQyHdrwzp%#IN)X8#!8pbB9#vd5qL=Gbd
zE5`>R9#-oQ444GO6rRbIg3FoQ*d{9prA@Y$<Kv5AVPL3Xh-ZT&o5@Rr)KeJ|3AL!D
zh9QdsBFDgx#SLS*FvQB$GNmxqFlB*U0~Ri0C=#w=%Hjoc7#J98nQNF+n5u*ztQw|x
zxJ-|HiR@$rRk6ulijwLm3A%<Mo*$;ah9L{iULY_zkyliac@4`lMh1q}j4(Bf3@EAu
zCpT&+O?Kzy6DsnnVaS3jD|RnYoopy4F<C}fl2I6B6f<iI+hjjsSw<0%7BxnO5^b27
z3=CPI<PPT6Fl0ePhJhhVd@`e)m_ZFgyaZT|fq@uJk}yqDq-vThC&n{5NyL=Va<VS3
zOnr$p%m4<46jsz=s9}hggUOcI!&q3=fzl#aL5VY%0uxxw10_PRTn)nlQ2LwPq9M;x
z!w|1BIZRX;5{C7xDQu|4IwM1#a}6U>xZ9w(nuADJW2(dI>iQH+g>Yx5aMm!y!%J*1
zPYvuJ1_p){t{R4TbvTc^h9L{itKtUd2T+a%E2v@2(gZU=`gpRzNven&ELsm0PvOl5
zm%7DFUJOl)U@53f4MRL!a}8sb7F3uag%8Cbs4#{(aP>6|@d!3Ne1#Yoisfq<vfzGR
zpgs8@k0>)^meyoOISJboeq=kVSiv6Cfm+6p!Vt`$Dd2aDB_lB}CH>FJkC%Lt0_;OH
z8E>)V7o_G*E*3ZCwqamk$o#^{z)++%d9%0%vu=^-<a^>alT{^jd_e}?;!jV_&rK~U
zs`O9FN=+^)zQvN4n45ZwGdHz3BRDxRC$$Jv=N0LK3P7&Ryv&lY%+&JKqFekq`H3kI
zwr_q)YR=>U2}vDWP%&o*Qp%K9SfmeOl-*)3i!Z#zoETqN1TKa_m0ywl<jE3gj0Tf`
zNm!_`6oJZINF~Gqa?dTs;$l#QAj(*!$=;GuY;Qp|%HPS^lKS<X(LK>0nHf0U89P{d
zxF)z<5z$zoaz)E}N64PY4_pj90zGyg7<f3<KX5Qe%3hJsydq)wfq|7*9ZXDMxgjh$
zBV<nG1!1)d!fH1pWajWqaC{&lIzx1#*96BKs%jHlruhB1E3UCZWR28@kSnI1Crqw*
z$6OSTy&@jl;nL%we}hM$$KyJW{3Raw1tAxB<S+7QUE$HXz@r6~P`SjTa*;>v3Xj?a
z9<>h~o6kzBFfl4`{v>^ison?_OrXlB2$Z&p(m>%Pn4VhVm|Rktm=oe0;Fe#MoC>L=
z4M1v*K?EppZ?Ps86(v^QVoxkfElNyJEy@JRfC?r}c5oox;!Z1xFG@{J$;?YH23K1}
zCLrUP(n^XfKx!=+7(^I~L5&<x;3_C6K<coJ$?@{CjM1BG<=Gh-l{dF46fiRKP5!26
z;uH?j6$K(d@mZ7tGL1RkH?s)T#3=$<eTy|DzC1ql7IR8UVNogr1H(#2uv>~jSxuox
zadM853}egWPNh~x<;ncYa~*;}rUf!EFle%ZBfTgXB#;gw@<Bu%NQwz;#x2&e#GKO9
zVo-|*=GOelZ<G}nr%vWo31n2B{LoNxbA`$oMrE*p+z_+CLCOUW(m;qcp_9|q)Lp=B
zA5dTxm4Pe+`AQRF892qFSXIryz)&R^l%JGe;+vQU3UIyj)RMp@lP{@VV^rR}Qr(rg
zz5`@RCx`&MK>QYSVonY?rQhOnEGa3<Oe!r&buB8&FG9Gjs0*YX)aoh<1+lt8L=T7n
zyAI?gaL^V>gTxSytpkO=U|N1rd16tDcV=E{ZlZHZW^!>6B9<CJI>2$$2nt3>-nzwJ
z7@wY5mYR2qr6jeu1maOd4x79|TZQrX=BwIv%=KVTAO|BiA{bMk;RtdCcY0A~K|yL!
zd|7G^B&i~t2}&ej_d^5r7E@sgO1Rd5!c_<(TvNU>PM)N{6%wnH6Acczf>Q><U{Jgk
zm4d7n_RBBv%q_@C%}vcKNlgKVFU&T!@}ktd<P5ltlQRr;7{w+}HS`9Xe8`#)6f*2Z
zSj>fFs9B&e1SRdFW>AR(O~)X?+R68fq#0!=|1xrCRNm}lY{^&;Dq>&$|NsBLCJ)%e
zTP(>rnFU1!Amw1^gR1i)2M{+GM1XTNI3hVS^GZ_FixNvxA@K)_c1>n*x-Xgz@+4bf
z3b?@uDluVM^%h%kYF<iUN|i7q_$L1{;bqjE%wgIu1FGedL5_vh5TI-h()+oMW%2?A
zt;u&yr!bWWPfpa6<wxqv6h8)u@kp^TGL#52Fih^}kzfHaCx10FWaOS)ASypuikE+K
zs5$>+9%jzT`sQMjo6VIOL7gwL$rYmFAX%<Oj6n>23}9_<&DFUDU`|*7Y0>RA;DagU
zfhnzFS|BvJ(L$Y3eey~RRYnm|t8VfW3n5;lhFpp6WM)f!M*Ydgmgc;mkb|1LKyq@U
zhB7m2iSXpPma>e(lXqK2JHi{cd5k^#Kwg0=W=P?!VMcG~)G*gD#Dn4(tf+<=)S2B}
zZpF>WXfwIldXJf=bP*``7lC4`XaXqW5P1xda1gl-T&M?6_OLb42Bp$l0`P1T;v5i=
zUz`fg5Vu$}^9o8!Zm|{@<Ybl<iBDc`D_4Ju)i*sqF-MaF67`^DPz1`nMVmp1fCVIX
zi!t>UCx{WBlUZB>F4&7ft$Ju74k^WN38kc_W#*;2=O^X_<Y(rUIQw{3$wQI}wDiJM
z;L60pz)&nR+1<{${w@da1wQ+W91d4F94>G;+~5}m_s3W{BN^}V2wo6MzsQqug(u?z
zPX?Hqc9AFj3Qzh4o^&wR=>r2JZyDnaQOPNhQ(_kwUJ+HjB5HVD)clgD`3}a5qE=T#
ztv)b=l-#v-*b{!;HsF$Nz=gojD}hm00^=^kC)8gFOuQ19b<sBaif#6F+rmq>g%^s7
zFWQz|u`PjUgxG*gKIIXdpnZWSVky%Sjuj$TG_9^{dSBA?zNqPYMbr19g5MPdzv~Ji
zmlQ%Sgoa&I2*08bet{<fjy`ZO2#QVTn#47cX9`b8J?jS!UQV&Q!V(uGQ!WapUJ*|1
zaQVQ*!7F(~L}CKt4H<>`UbDOw6wmaZ<3EA(uB6=D;sus7>*my5l+?Q-srP|_QP2!b
zOmMubq`rc4gW?*Vi%O<fluRdB&Ip`nGr{HqGozr{j}I)&ypnfC#HagA@|oy2#cu-R
zT>;SxV&3%^1$?du_*@Y1xho(#A#ozd6pjgC_~XZq8~g$jT)HYdD(|YOE%8_pc~Qmu
zii-IJ)++*vHv~jKNb_=veU)M0<h#IUutD`Ahs6~RiwhhUA2=Aqq(Kd&8~h?2JjI~?
z+ff@wK6h@0<J^`WY|KUAl8~>+1C+i!LCxVJFAx*dAT7!Ov06X`xK;+|hRGl<sPrzH
z4Pw=Th(#b`JBaWG5uj422;A581#v;dOHe8LnE^Bs(7*tJA2^a2S@k|JAdw!7%#)2A
z_eQC)ihf|gBp}^ika3!lw>S$j^YW9EGcxnR8T1xwUTJPYCAj>BG^)UbE~xASH?N97
zbzM>VWKSnEHc*}~;-B2=l;Lqp7;cOnV(<gp;01Mxi$InYp?c~UhfQvNN@-52UD1B<
z_zeR?vBzX-=iT);7z{6<q7N*}j4U6#7&RGLK7=slGBR3!V8Bj(1dD$GlTbBr%p!~|
cAGDm<7)3uYU?)F<#lL{b4`L7si3A520M_oKRR910
delta 4575
zcmZ45!+5iUk#9LKFBby?1B1iuf2jwvC-O-!?wP3G=)lRq$-u<m&XB^{!jQtXjER9^
zH4{h~2uF!R<+xiIQh3nh#8Mc888mq(erep?#W;;gL5P8Yp-31+h%hiP+~O-vEK7|q
zNGvXiPsvO!DUzP-z_N-lb@FSLXN(?`FR|J&nr#+j%VwPXgWW0^>@bjxS==BJgljmm
zK@>xgb`3)o517foz>viYV-?Ak$imnR3@J=C95oDCNajp#<&an9D`7{d$>N8x(Nvz~
z(66^Bk$}mf*kV@0Si_J750Mn+DlP^FsLO;<T)<Mpn8Lb@nSo(7Gt4ALhCDG8HEbm;
zFgXT>6n2E_$>p5NEF6ACOp_OLMh6Sr;!n%WOU#MSEJ-a&EXmBzD-r_*tRN_0SxPc<
zQy~E@3gWXD<QL?mrj-<dJWwPJ5@N|ttt>7AMaV6tl#(J)d`!;el4p|v86rP<5|=fj
z+~jjyvWyCo-*72twlOd;6rW^dU}#{tA+ONEa)m?YuAtb2k}Kj`7X`Ji2x@;|P~uem
z$e=XYl=}<|8-sx4WCxxmw!7T?SGZ*+pW+E(RN5@d+sDMHGI<TZub2+V7La>yG36E9
zVy#Lo$}cVwn#?cY5DIc#ks3%I<fdB!sd*`GMX9MNMVV!(zWFJsnyf`2H{N0^&P>lM
z&Ar8%R+N~VS|kth6nj}}PJD81N--+~1A_tx6e)mA5}3S1K$&sR;u8Xlj7pPF3Tmq=
zf`m;#ga88r!%BuCB@i2|7?iw<Kt#%9Ng)k>ZC2wC3~(Z9a)6K=8(1;-<PxFfOg^lW
zWrfpNY8bQFCVylTV~Sy!JYQH{iXk4J&OlPI7%gJtoP0|}j!}H_K@o|`f})a)Jd+zm
zL=_oRm{7A0BSQ^C7CezG;GLW(syg|Rju<aP7CgCT2~1|R5$9%z2gNzqhRF|g#8FfT
zK~;eA93B<S4Dlk9{l#n<wI^%K%QEUtelI7*SHlo54%WlKz))fc6Ox1rnSz8SdyDZh
zGc1sqoG7ZqQo|50JNcr8$mDo&eu&4I8B>@hw~H$>>P}A7!7vgkgluG@j?`ptF`oJq
z)*6O*Ihb8w9w>&v0x4`Y4Dq0-g7DaD7_#8JDsBdb8isgK^nzt;7_&f;1!kvkWHW*o
zMcgQ&oY~+ESIp$a(8LIp2?VJF83fl{!<eNE7Gz*xNZ}&NAh`M(hIr-4?<FL;L1`Oo
z$O4tgjUpnTRHHokBb$V33dl)t^;N83PpE>Gg6v}mX3*sEo7^C2%5BELz>xWck%6H|
zW%2<@4Q90>k;$(mZMZngGK)(Sb26(^C#y;6PF^S_p=AzA(iWh6!IW2ci?Q$)Q(oCE
z=Cb(0Tg-{^g+<^z2ufl_mXn`Kr7>zv_K~)*WCG_daCuM+D$*1bAXx|$qE(_n`APXD
zzKMB>>8V9}@$s2?nI-Y@k3nTa)8rG<`o<GVrqo^JRlCBg_K}%^)19${rH5;R%M}rg
z6)IP>tapU$iG0A#-(&ZIft6G60~Z6Y;0LzNOJ!7;7?n1klDoxJ4|Wr%TqsfpIX)WX
zc)|446366{(!`t)=K#0-qU2OaF{TAlp$j5F9=*kySX7i)d5b-<EVT&ayf}~yC~0Z3
zgS~W%JFO(XC^a!9GcUauTwE0CfsAKLD=9JpsWoO`5Md|=<t|Xcp#bxE>|}E#Sw^?b
zp-SwGj7pnhl?xadc_*J!F^LZV=?VrB;UFRsWEyk6Z)Q;fhzqj%7Hdd+d3@?E=9HAe
zq9{;7!w7auF(@5E0`?Y5aY<@Hl^{yUf`sxXC#trBOPXY+$&Lmhlhj%qyg&wfGB7Y`
zvVvo%$QvXO10qsDL^4PU5`(u`%Mx=+Q&HkDW%3a<1;&QSPt^h$l_o0~OKuKOKf|aD
zHjo=)7C6MY;2|CevBr0@y{5Vgxbgsnbx|hBGLWY=A(j<^*eF)zF)%Qoc`7}%B(Q(-
zD$Q$*N}C(CT$$^uL8jDz2(SypZ!st4<badqEk4JRlA_F{(vnoyqN4mFgxiX0LFz#@
zLy<3tRR<#KK?K-!Aa@jjLcT}}B!+NoAt-VL)AEbT6N^&3GxJh&6P-&klZ%TGkys4U
z0gk#7P%yH9W9k-rVSIXKS!&)bmXg%s5{O4Z<pn5>6e&#Z&{JXDw0W(b9djDk6Uf2H
zjR?jRXgGpg!JS@|Sx}H#6knE_14+pUXM)lY*!|Fey~R|Rf)cLzlLL*!86Ql}G};D^
z!rP+YB!Dmh6oEw<lLbtqMIqH>c~NR!at7Rx$qptujEs|WO}xSSB`04pSs4y$F=?_F
zp_>8<5O6l?1+l=vS_aDNh}2tD0E%ZZl*F5!S`rSnlVkEFQ+GzC%~EETj5(lk>*fFd
z|Nm?9fbF@(lAM!SP?QQX8SDj15X%ZgB!LKU?gqyiXJ%eWYI;#(Nh&0=Kyj?e433?m
zE>M876{dhIeNb5j&bE{Hn=3GiOnzYA&!{xHz(SJoz~nBA{W3KSS)8yU710cwz*u~@
zM11mlIT-=uR%G$x67k8r^0JKL;3`pk@@7lJ$(z{aCl~Mu@GfF3VS<^&Fu6oTaq>+I
zp2=q|#Tf-bjeyBU{9+(=T#Fck82T6(8A`+_ueMTW6q>9kDmPicR0cyq4b$X!Q5CjY
zW=@6@?aArZs*GZwcERLD)<V4caGOBY@Ih;RM&rpJt<8Bstrw`&0%>slTgzG^J~`S3
z<mO77=tNMgf)(U3_UtQx*X0ZhDZDkz=*^8h{t{3OfYmcFr0^|cWMEhgifFJ%6JrfC
zsCEZ)7#J98n1dNK`8SK&ax*fTO;)tqQ(tvFAh9H)SRpe{p(G<!p|mI^O(DNXp*TM`
zwIm}mFI^!ur#Mw1FEuqqp(G!ac`$HNszO?31w_8II8~t}BePf`EwiY&L{Gsnr#K(3
zxu7UNy(lpkW>s-cW=d*(Q89*rAc4fZ6a`q#2yz9;ZRz=mISSxbqn<)=eo;}So`Q3J
zE=aaGKR-`TQ?3Y9dKG~ZUr{qCEh5SYL=Hie7U05&+aIJ5Tzz>@7PdF32j#<C0`L+b
z#5o`!zc>|~w{Nj#<`tBd++r;*$jK}LH`{M9=cMM{Vol4*Pb|5`TwGavizBrnIX*Kl
zEx)J-l*U<oL9W*1fFyfR7AOLhQbo%^k}M#(Ta2l<I6;i~oXp}9EpQE2%mHf$gYyMQ
zEq_XCT4r9Vdwyb0Kz?RkNtMjxclI)D222bL#U7J|9h~cL@C)};Ug1~z$e_sS#dw!T
z@Pd%@MIM(cJT4b_Ty6-9UlCUOz`)8I%Xn8n^nzISMS+|v0y!51a=_fIivrnK1hOv(
zWP`bG9~c+~s~B%c%FdCTBfFsRilpWhNz?0+)|Vu$cQ{^@w7Vi{_kkIt<gTO35%KGe
zA(!eMLoS3yUI~r65}I@&Ips=d>Xp#Ei;nqM9P_U`mR@o!y--$u(Xrx+V+BMb#0F&Y
zsetH&><a=hOP!W@tO&WHYj<7O|B|l%Mcu$Fx`7u}g083pT~~>?q!Mu<GU}pA^c9up
z3j#54RR4j4K~!qG*CelrK2v-;Tt9H|@=Dzmm$@LDaZx<;ig@M(mJduEg0eRxWF|P?
zP*7RUwTNql@k0J3{1ZIy$|@~2UQoHvZi(GRS)(hmMjsd$MXkWZ492@^+ABOaB(Cwf
zsAhRZ&2mEJ0>Ozj6KXy%Gm2XM_`t#}D0^2zdOlD6ES{NsbND7W-W8F&Amx8iB;blj
zzy*<jyCRY^6eoI2@t6RHKYskUAuKY3Ws>Ct%exv{YdAJYUevI@qG3J3^@@n<4H3x?
z(!9J<Uu770g)Ru0Y)HMxV{?Va<^mW!6;l1cz@+ZU_>qB0*o*NCnEb%N#OuZQfvcWD
zOd8Z|yTLEg!BY$>LymDe^0`Ye9G9^45N0j{iD>c_If2r<Gl*~j5uoN_Q7nj64kEy<
z5pXU7bsUTGL1KL%0#siVO$D)5fe2R+0cvAI8kz143=FzOAo4z_p83oG>TfnMK;Q?C
za7I?W4-80T5!2*LPJ5%YSVccDU=ol%9mq6I@mrh)nR)ri$r+jX;Ea8XHLo<cpb}ga
zLE4(2(ys_qg%p9ZZV{-CFN&G`(%Fm+T=DTuR&~kH1NRHT4lF`-`Y#Tf-29Z%oK(A_
zjSLJ7pn|j5Z1Po?-Sr<>lo?q*crj`+vU~_(OkrfS{=k5p{0J8R0w$qqe3(TTSw3i0
YvoeZ)V8Bj(1dD$GlbC9XSU{l*039JeG5`Po
diff --git a/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-311.pyc b/python/ur_simple_control/clik/__pycache__/clik_point_to_point.cpython-311.pyc
index 6b95c52eca9803e3be3409010974b2933bc51953..9b3a270c6e1c04ea59a98227f6d366babc3201fd 100644
GIT binary patch
delta 543
zcmez6cF&D>IWI340|NuYzIXppt2gp`2{EqN93kY($dtuDd9JV`qrl|-!jY4mM8qcR
ziHI-?O)eA{nH(;{%P2hgyomJV1`!TM5wO^OIX<QZsM3~<qLVpA(^-q;7#J8P7l`g+
zl%1?CR>i%<>xzoyC3edT?3R<)iEU%NG&xm#rbG!3$Qclv#SWnv7#47VO}CSfRuzVc
zF)*Ys6HqNM*@B;M@&*ZEMy|<6Bvcu>C%=*~X5;~zWh80J$U8YhQf2aN$q8H`3=9mL
z7#J9ek4-j}vS<3rvAJ1_pGglST*Sh_z;KJXs5I{uOG##K>MhpdoYd5UqCf@)22BpI
zuZr9m7#NCzCLfSVW{qQDU^u;5Lv|^XKsZP=0z^cDh``Br<rM{@K+I?m5d$J(C-W)D
zvZgXHFx;DLpm2>bX)?Q_J!8aVcSTulu<ao8ic%-%D@ynYrKF~1=B2vlC*}m?XXceS
z`*;@Ff(+3I5eS39_Cic&U|?9uP^1WwmuFyL_{Cw9o1apelWJF#H~Es{HGUyR*$)hG
aLSyoFrBE)FOFSwUdDO1(s7)4AP5}VB;DjOo
delta 554
zcmccT_REcTIWI340|NuY>NoFFD>w3b2{F#w93kY(IC;IWBqRUi*+NQ_UkjTv3QRsI
zEi&0$M0B!}h}h&aB0P*jlk-KC8HFbwlvbLoC?XCQlb-xVM2=Bpvao3S<a*IvjM9^>
z#Hu*2sF+-0H@U!WGI_t)HpXX@%fx5u)UekuX0d@R2jMC%28LSp5<U<cg0navGy}r|
zPO$a;5(1OGCAd{ZVd4x7Da-_P7&3BCJ}9Be$TRu9u=wPE5;BauV6*fkO&R$nr%I~u
z-{LJVO3h2oh|f<;D^4w$+$lMME0lqOVG{!bL-FOw%2M|H4|s(qq+SuzyvVEB;P#bm
zbBYu{lO9M-5eowY!!72b(!5(NC7HRYw^)mFQd0|xf*2SWG&zdo7#J9e+(AU}<b^WH
ztnmyC3}-fT$}VLRhyaO3f`}*(5j1(9yrMufh#3PSVnIaQ<oEKjtZ57k4EHAsDqLes
zp8P_=o-uN=hN3Jt*mjV4MQM`*6eSG!Q&Q71^HSaO6LSLcGxJJ{Y(XaIg9wDFU`wG!
r&sLOT%%8kX@fyD{qwEI;IH5Ipwo)jU;w2u%i#*C#c$6pqQc3{;mm`hh
diff --git a/python/ur_simple_control/clik/clik_point_to_point.py b/python/ur_simple_control/clik/clik_point_to_point.py
index b20bdc6..ba8399c 100644
--- a/python/ur_simple_control/clik/clik_point_to_point.py
+++ b/python/ur_simple_control/clik/clik_point_to_point.py
@@ -139,7 +139,7 @@ def controlLoopClik(robot, clik_controller, i, past_data):
# first check whether we're at the goal
SEerror = robot.data.oMi[robot.JOINT_ID].actInv(robot.Mgoal)
err_vector = pin.log6(SEerror).vector
- if np.linalg.norm(err_vector) < robot.goal_error:
+ if np.linalg.norm(err_vector) < robot.args.goal_error:
# print("Convergence achieved, reached destionation!")
breakFlag = True
# pin.computeJointJacobian is much different than the C++ version lel
@@ -235,7 +235,7 @@ def moveL(args, robot, goal_point):
if __name__ == "__main__":
args = get_args()
robot = RobotManager(args)
- Mgoal = robot.defineGoalPoint()
+ Mgoal = robot.defineGoalPointCLI()
clik_controller = getClikController(args)
controlLoop = partial(controlLoopClik, robot, clik_controller)
# we're not using any past data or logging, hence the empty arguments
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 605a788..716a238 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -17,6 +17,7 @@ import signal
from ur_simple_control.util.get_model import get_model
from collections import deque
from ur_simple_control.visualize.visualize import plotFromDict
+from pinocchio.visualize import MeshcatVisualizer
"""
general notes
@@ -156,6 +157,12 @@ class ControlLoopManager:
#self.robot_manager.stopHandler(None, None)
self.log_dict[key][i] = log_entry_dict[key]
+ # TODO: offload this to a different
+ # process, it's hindering real-time performance (not by a lot,
+ # but it does)
+ if self.args.visualize:
+ if i % 20 == 0:
+ self.robot_manager.viz.display(self.robot_manager.q)
# break if done
if breakFlag:
break
@@ -168,7 +175,7 @@ class ControlLoopManager:
else:
time.sleep(self.robot_manager.dt - diff)
self.final_iteration = i
- if args.debug_prints:
+ if self.args.debug_prints:
if i < self.max_iterations -1:
print("success in", i, "iterations!")
else:
@@ -232,13 +239,24 @@ class RobotManager:
# load model
# collision and visual models are none if args.visualize == False
self.model, self.collision_model, self.visual_model, self.data = \
- get_model(args.visualize)
+ get_model()
+ # we're using meshcat exclusively.
+ # there are no good options,
+ # but this does work and isn't a dead project
if args.visualize:
- # ---> TODO: write your own vedo 3D visualzier,
- # connect it with the real-time matplotlib line plots,
- # and add some basic gui control (show/don't show stuff,
- # define a goal point, choose controller etc)
- pass
+ # for whatever reason the hand-e files don't have/
+ # meshcat can't read scaling information.
+ # so we scale manually
+ for geom in self.visual_model.geometryObjects:
+ if "hand" in geom.name:
+ s = geom.meshScale
+ # this looks exactly correct lmao
+ s *= 0.001
+ geom.meshScale = s
+ self.viz = MeshcatVisualizer(self.model, self.collision_model, self.visual_model)
+ self.viz.initViewer(open=True)
+ self.viz.loadViewerModel()
+
# TODO: make general
if self.gripper_flag and not self.pinocchio_only:
@@ -545,7 +563,7 @@ class RobotManager:
"""
- defineGoalPoint
+ defineGoalPointCLI
------------------
--> best way to handle the goal is to tell the user where the gripper is
in both UR tcp frame and with pinocchio and have them
@@ -557,45 +575,41 @@ class RobotManager:
but also you do want to have both options. obviously you go for the sliders
in the case you're visualizing, makes no sense otherwise.
"""
- def defineGoalPoint(self):
+ def defineGoalPointCLI(self):
q = self.getQ()
# define goal
pin.forwardKinematics(self.model, self.data, np.array(q))
T_w_e = self.data.oMi[self.JOINT_ID]
- if not self.args.visualize:
- print("You can only specify the translation right now.")
- if not self.pinocchio_only:
- print("In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles")
- print("Here's where the robot is currently. Ensure you know what the base frame is first.")
- print("base frame end-effector pose from pinocchio:\n", \
- *self.data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(self.data.oMi[6].rotation).round(4))
- print("UR5e TCP:", *np.array(self.rtde_receive.getActualTCPPose()).round(4))
- # remain with the current orientation
- # TODO: add something, probably rpy for orientation because it's the least number
- # of numbers you need to type in
- Mgoal = T_w_e.copy()
- # this is a reasonable way to do it too, maybe implement it later
- #Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
- # do a while loop until this is parsed correctly
- while True:
- goal = input("Please enter the target end-effector position in the x.x,y.y,z.z format: ")
- try:
- e = "ok"
- goal_list = goal.split(',')
- for i in range(len(goal_list)):
- goal_list[i] = float(goal_list[i])
- except:
- e = sys.exc_info()
- print("The input is not in the expected format. Try again.")
- print(e)
- if e == "ok":
- Mgoal.translation = np.array(goal_list)
- break
- print("this is goal pose you defined:\n", Mgoal)
- else:
- raise NotImplementedError('Paths in the urdf or something else need to \
- be fixed to use this first. Also need to program in the sliders \
- and visualizing the goal frame. Sorry. Coming soon.')
+ print("You can only specify the translation right now.")
+ if not self.pinocchio_only:
+ print("In the following, first 3 numbers are x,y,z position, and second 3 are r,p,y angles")
+ print("Here's where the robot is currently. Ensure you know what the base frame is first.")
+ print("base frame end-effector pose from pinocchio:\n", \
+ *self.data.oMi[6].translation.round(4), *pin.rpy.matrixToRpy(self.data.oMi[6].rotation).round(4))
+ print("UR5e TCP:", *np.array(self.rtde_receive.getActualTCPPose()).round(4))
+ # remain with the current orientation
+ # TODO: add something, probably rpy for orientation because it's the least number
+ # of numbers you need to type in
+ Mgoal = T_w_e.copy()
+ # this is a reasonable way to do it too, maybe implement it later
+ #Mgoal.translation = Mgoal.translation + np.array([0.0, 0.0, -0.1])
+ # do a while loop until this is parsed correctly
+ while True:
+ goal = input("Please enter the target end-effector position in the x.x,y.y,z.z format: ")
+ try:
+ e = "ok"
+ goal_list = goal.split(',')
+ for i in range(len(goal_list)):
+ goal_list[i] = float(goal_list[i])
+ except:
+ e = sys.exc_info()
+ print("The input is not in the expected format. Try again.")
+ print(e)
+ if e == "ok":
+ Mgoal.translation = np.array(goal_list)
+ break
+ print("this is goal pose you defined:\n", Mgoal)
+
# NOTE i'm not deepcopying this on purpose
# but that might be the preferred thing, we'll see
self.Mgoal = Mgoal
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-311.pyc
index 177620f1965e84d10ab4a264f60aa2384ae543f1..625e9afb9c9bf6ff072333dee008f431cf9c1eb8 100644
GIT binary patch
delta 1148
zcmZowzM{aloR^o2fq{Wx^1XklwfqzLBpB@`s+%%$PK@5d$S_%eQH1FT+hjFHcSeTE
zd5pS(49gf97*;cabbxRz<K#7rvW!e1xyiHH_?c>%Cx2j)WM)~!IQa*o=44+cAx758
z2~5t4tf(4mSe7v{Fsx<*83Mw!Y#@D;?=lHZKFlOFnUh(Fkprxc15F=0nm*3S4b0k%
zTwvWtnS~g+!MeE7baA2S;sL7<Wf5ZJ1+j#=7crvhsO1Bj!-pzb!;5APKS(zx4@~#u
zcPz@2ZCQmF1;M%n(R2%-=@tU%o_v;7X!2%OsmVWCg&0LZ`szi{^a-Qs6HQ^L6-!~L
z6;ENPl_=o{g&zcGaYJYZh8lGLh@q(CfvXd%VJzW;@faAgco`TbpJQjU%7P1~Fw`(F
zV`X4i%?eY7W+|FslMk?q)Mvq6g+;q~4O@*^mH^DS6agCs28J3=6fr@t7!Qh=5Lk>K
zMNAkhCWIm;V&e-kP!vT_6r>O(5CmZks1ZXA@LI_lu^RCji5kf?mS6@=vCVTh<}ywW
z=aOd>nq1E1HTfJ@yr|SIq0*w1wD^L=l8pGG)SSeU%(B$UmRxe1!@2DlS)@Q&Y9Wuv
z<PAKmleh5H30W{OFeouFFckl0Vqj=scq%D3*^SqX|1Q7C1yPNQ{F+zzH5)wcZtmuN
z$XNe?N1(y=hL}`?{|#aB2A{7S47>tYc$6+En_lEGyTW7E;CfeB?253)3egJ&PDh=u
zxP)ABiM|k<cp<suqHyUI;nD`5yJC`8#I#qqUNCk&T71Pl{EB<xh2-=LnN=6Xs;`Ju
zH~4>GV^Gj(aJ|AJIYaRxi_8sq1ZT3hK(4qD0|Ub?*1Xc(g32OLOx<EGt}HI%oqS$E
zP6p(cl?+9e3=9lK<_ruBzc_4i^HWN5QtgTyCUXi#DT(kfvV4#-Vl?}}fJuP$0RX^5
B?dbpj
delta 1303
zcmcbj(5lS0oR^o2fq{Xc{P4e2cj1YA5{xzz)lDbHZ5C$)2}3bE0|Nsy0|Ue7BKFA|
zjP8t#lM5JiCr@P)U}T!Sj!~A88N_E~o~*{CIa!%Wh>>;jI%dhq{!BuQY?I}foyFOf
zF)}c$hMQQ!x{Qf|VKvj_7fi~N?=uN8a)8xyGYc_tg4J`Pspo*IU&5?Bc{Z~UBlqNW
z%-W1RV6{AGYPq3m6IhfdhqDMV@`256W)WiK2dn2tQ_lxgFT<)lS(sIbQ4p-&o>hoZ
z2&`TRO}!vg{S8*-$>&*x7)8M9f3pfPih|TTilV6(fvAUuBtxxO4MWk=6oy*y6oy)f
z6oy*K5^<Ou14D{HiR9#Wj8XwLOv{)V7*>M<9W2YpfTB#WL=r3vCTdvFtYpE^%RgCw
zU1ahPM!|aV5`M5IFp<RvrWhC)YQ$@pO5i*OhAg<MECHCB6owj>WvmPgt65<pwUVTo
zrj#WJ(~QM9i9CrCB&WhcsD`~pJWB|!Psj%3U@jCfVXzo4ikJvkOaMhp6f7o;A|?hF
z6GIUbw_&hgU_kMmFw6lp;;7+WD^(+2BT*w+BbCMy%%CZ;`2+i0#>v@Sa+7;G<rzgL
zujTaOVJ#@i%qzLYQc_uvI{6!Cf|m3x&a%wn(!`w1s?=LTr9~-e@db$`8SzD_If*5i
zWvNAM3=9lKpp3V<lgplwMVf(uVe&<8(a8_En?<Y{7#KjQrZ|?FfuVunD-VN|{Nz#|
zGybRiA{Rt8F7j($;n!^Nc)EEP&qK!g4{Qt^f*%+pSY;jvh;*>tkdo`*zacK&!E={`
z_X3~NQkM&w))!T5uBg~tunV}z5qO0o@B&BRT@K!kthp)|<P9!L8D5bxykP8dk;C;0
zhwB9n*RO00f+AN0)Gnx7UKFspB4E|QdRJWPinz{-&<iGRXPK{fL|pNRzmS-EA-&?F
zmw4qB@yZUKyHc`Or1UqiUNHAOYkb8!`ighzh4kzTxpfz%>aR%Eckp-ce-*<p7vb6m
z^4gPG_?yK=7#J9CvF4TL7E~6262&d%;>zM8{>k(B<zzqsvy!0*6rn{{3=9mvIBatB
cQ%ZAE?TVZy-{6l@lJH<;H2c7SodoLx02YiC)&Kwi
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-311.pyc
index 759cafe5cf649d89b5731ceaa655e38fdaa3a52f..11e5dcc03d67ce58afde5d3306cdc033b3d7633e 100644
GIT binary patch
delta 789
zcmaDZxLky9IWI340|NuYzJLEx+c+ljNf;O~FfdGKNMT4}%wdRv(2P+`U_Nsca|%-n
zLljF2a|=ThYYIyXLloOYKPAR#6AMl2OF-s8aTX(#!LWb{#zLl37?&|JFsz2FDq>Ax
zs%1%Gu4T=WDuIhKFk~^qbfvJQu&!ZS#>Bv|8g3#ZLkfE>OA1FVYY8WVHcTZ$4ND3q
zs=`{f5^lIOLl)b_JMxU|lk*r=^h!7p;x(*<^iB3-glN>{;_|Cv)6-8$EJ>U^lhIbL
zic`NNw*br!(PX^Eky?>iToRw3T_p<=pFD?Aid6~B+QFzS%W;b-zxWnQL1Ib9EjF;q
z;#(ZKiP@<snMK8u?=XJn6=PsvU}a!nD89wOz%cnBtG4r3E(Q*+mh$$xrn(6k7un^n
zu*-j7U}LqrAz{+NdqdKygRg_{DF;^%_XO`1Di=8PFLLN#;n2Upp?^cd9HhXugKx7q
z^C?C~(aDck^cgiK3$bc5ichv<m0;szU|=ZXpB%#~?I_5=z;KHzDL+3aKC!4MvGNvs
zUVLUrYEf~KEXXzN3=9kkVBk<B3Raq&lbV<pS`bv4SELG(oV<f|1*7=nM7DcupmbHl
zJ=vALKu-XqnGGZl)(p`rl#`#B5>%S!mS5zUmzYyooLO81N_<9>AG0U=i!(4VXtEc9
zqU{!IUTJPYWsw5Nogh<+K*0mH53Hd`3M2v205<p+hfQvNN@-52U6Bz30|O{E6-Q0(
f<B*lQ!NA!7h96iY7)3uYzzMU-dpOitKqdeHO`NPz
delta 548
zcmZ23@?4N_IWI340|Ntt4O?2O3HwAo2@MSf28QVjDGVu$ISf${nlXwgg{g%hiaCY3
zg&~S1g{6fdigjX`5~I?@N>j5G##)vXrdrk#Mvy@eT*H#Wyo`x~VKozk&%jX2R>BNp
zGcaT^O^#rcXJnrIicv)mMI$Rgji;a*HJMoaCd)F}vfW}TPts(We4Xh#mmmWJ11kdq
zL-FFtH<<N!KCm%}8aDXe5H)M?+w945ijh%ZvIv_#qvB*2Hf=_s$pvf@Y@7@X3`Ja%
z``Dx%c^DWNZgC~$=jX&H78NB{-eS*-&n!tTDlU=$naR$;z@PvI4n+cBrO7#|iFu&~
zL8W;`3JeSkl9OMttzZ<IJemC-8^{$!Y?I453iP-^n%Ur*A$o;!@)J{nO7q<EiyZS3
zb1I87i;J`w7#OrCi*hE`3o$S-XtEW7G~Hs&E6pvaERqJf6XZXzRbaJ6q98GlTCkbF
zIBatBQ%ZAE?TU067#Kj&Q5?v?!0>^Yk&*ER18)Nu-e3^A07D<x1Q|s?Fu(~7enycG
P4E&6fnYk2LK;{Af#^P|;
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index 452c0d3..2d624ba 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -13,24 +13,29 @@ from importlib.resources import files
# can't get the urdf reading with these functions to save my life, idk what or why
#############################################################
-# PACKAGE_DIR IS THE WHOLE GIT REPO
-# PACKAGE:// IS WHAT'S BEING REPLACED WITH THE PACKAGE_DIR ARGUMENT
-# TODO: REWRITE URDFS TO THAT THEY FOLLOW THIS
-# YOU GIVE ABSOLUTE PATH TO URDF THO.
+# PACKAGE_DIR IS THE WHOLE UR_SIMPLE_CONTROL FOLDER (cos that's accessible from anywhere it's installed)
+# PACKAGE:// IS WHAT'S BEING REPLACED WITH THE PACKAGE_DIR ARGUMENT IN THE URDF.
+# YOU GIVE ABSOLUTE PATH TO THE URDF THO.
#############################################################
-
-def get_model(visualize):
+"""
+loads what needs to be loaded.
+calibration for the particular robot was extracted from the yml
+obtained from ros and appears here as magic numbers.
+i have no idea how to extract calibration data without ros
+and i have no plans to do so.
+aligning what UR thinks is the world frame
+and what we think is the world frame is not really necessary,
+but it does aleviate some brain capacity while debugging.
+having that said, supposedly there is a big missalignment (few cm)
+between the actual robot and the non-calibrated model.
+NOTE: this should be fixed for a proper release
+"""
+def get_model():
- #urdf_path_relative = "../robot_descriptions/urdf/ur5e_with_robotiq_hande.urdf"
- # NOTE THIS ONE WORKS IF YOU'RE NOT LOADING VISUAL STUFF
- #urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande.urdf')
- #urdf_path_relative = files('ur_simple_control.robot_descriptions').joinpath('ur5e_with_robotiq_hande.urdf')
urdf_path_relative = files('ur_simple_control.robot_descriptions.urdf').joinpath('ur5e_with_robotiq_hande_FIXED_PATHS.urdf')
urdf_path_absolute = os.path.abspath(urdf_path_relative)
- #mesh_dir = "../robot_descriptions/"
mesh_dir = files('ur_simple_control')
- #mesh_dir_absolute = os.path.abspath(mesh_dir)
mesh_dir_absolute = os.path.abspath(mesh_dir)
shoulder_trans = np.array([0, 0, 0.1625134425523304])
@@ -57,33 +62,17 @@ def get_model(visualize):
wrist_3_rpy = np.array([1.572215514545703, 3.141592653589793, 3.141592633687631])
wrist_3_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_3_rpy),wrist_3_trans)
- # TODO fix paths via python packaging, get this to work
- if visualize:
- #raise NotImplementedError('Paths in the urdf or something else need to be fixed to use this first. Sorry. Coming soon.')
- model = None
- collision_model = None
- visual_model = None
- print("urdf_path_absolute", type(urdf_path_absolute), urdf_path_absolute)
- print("mesh_dir", type(mesh_dir_absolute), mesh_dir_absolute)
- #model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
- #print(urdf_path_absolute)
- model = pin.buildModelFromUrdf(urdf_path_absolute)
- visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
- collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION, None, mesh_dir_absolute)
- #visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
- #collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION)
- #geom_model = None
- #package_dir = None
- #mesh_loader = None
- #visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, geom_model, package_dirs=mesh_dir_absolute)
- #visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, package_dirs=mesh_dir_absolute)
- #collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION)
- #data = pin.Data(model)
- else:
- model = pin.buildModelFromUrdf(urdf_path_absolute)
- collision_model = None
- visual_model = None
+ model = None
+ collision_model = None
+ visual_model = None
+ # this command just calls the ones below it. both are kept here
+ # in case pinocchio people decide to change their api.
+ #model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path_absolute, mesh_dir_absolute)
+ model = pin.buildModelFromUrdf(urdf_path_absolute)
+ visual_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.VISUAL, None, mesh_dir_absolute)
+ collision_model = pin.buildGeomFromUrdf(model, urdf_path_absolute, pin.GeometryType.COLLISION, None, mesh_dir_absolute)
+ # updating joint placements.
model.jointPlacements[1] = shoulder_se3
model.jointPlacements[2] = upper_arm_se3
model.jointPlacements[3] = forearm_se3
diff --git a/python/ur_simple_control/util/logging_utils.py b/python/ur_simple_control/util/logging_utils.py
index c8f0c5d..99d4b3e 100644
--- a/python/ur_simple_control/util/logging_utils.py
+++ b/python/ur_simple_control/util/logging_utils.py
@@ -1,7 +1,7 @@
import pickle
import numpy as np
+import os
-# we're not
def saveLog(log_dict, final_iteration, args):
# shave off the zeros, noone needs 'em
for key in log_dict:
@@ -10,8 +10,13 @@ def saveLog(log_dict, final_iteration, args):
# - generate name based on args + add index
# - you need to run shell ls to get the index,
# there's at least one chalmers project with code for that
- run_file_path = "./data/clik_run_001.pickle"
- args_file_path = "./data/clik_run_001_args.pickle"
+ if os.path.exists('./data'):
+ run_file_path = "./data/clik_run_001.pickle"
+ args_file_path = "./data/clik_run_001_args.pickle"
+ else:
+ os.makedirs('/tmp/data', exist_ok=True)
+ run_file_path = "/tmp/data/clik_run_001.pickle"
+ args_file_path = "/tmp/data/clik_run_001_args.pickle"
# save the logged data
# you need to open it binary for pickling
log_file = open(run_file_path, 'wb')
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 b4b9380..59952dc 100644
--- a/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
+++ b/python/ur_simple_control/visualize/manipulator_visual_motion_analyzer.py
@@ -151,8 +151,8 @@ class ManipulatorVisualMotionAnalyzer:
# 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)
+ # 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)
diff --git a/python/ur_simple_control/visualize/vedo_manipulator.py b/python/ur_simple_control/visualize/vedo_manipulator.py
index ec5c249..d4a2a38 100644
--- a/python/ur_simple_control/visualize/vedo_manipulator.py
+++ b/python/ur_simple_control/visualize/vedo_manipulator.py
@@ -65,15 +65,6 @@ def get_args():
#######################################################################
-def drawStateAnim(self):
- toBase = [np.array([[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]])]
- drawOrientation(self.ax, toBase[-1][0:3,0:3], np.zeros((3,)), self.avg_link_lenth * 2)
- for j in range(len(self.joints)):
- toBase.append(toBase[-1] @ self.joints[j].HomMat)
- drawOrientationAnim(self.ax, toBase[-1][0:3,0:3], self.lines[j][:-1], toBase[-1][0:3,3], self.avg_link_lenth)
- drawVectorAnim(self.ax, -1* ( toBase[-2][0:3,3] - toBase[-1][0:3,3] ), self.lines[j][-1], toBase[-2][0:3,3],self.color_link)
-
-
if __name__ == "__main__":
pin.seed(int(time.time()))
args = get_args()
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 9e8c467..4c7f71e 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -1,10 +1,13 @@
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
+"""
+plotFromDict
+------------
+plots logs stored in a dictionary
+- every key is one subplot, and it's value
+ is what you plot
+"""
def plotFromDict(plot_data, final_iteration, args):
# TODO cut zeros from data
# TODO: replace with actual time
@@ -25,3 +28,11 @@ def plotFromDict(plot_data, final_iteration, args):
ax_dict[data_key].plot(t, plot_data[data_key][:final_iteration,j], color=colors[j], label=data_key + "_" + str(j))
ax_dict[data_key].legend()
plt.show()
+
+
+# you need to write out specifications for this.
+# only then implement.
+# this is too fucked to try to do it from your head
+#class RealTimePlotter:
+# def __init__(self, TODO):
+
--
GitLab