From e942795618ebf1b587be4e26e61daee47621193c Mon Sep 17 00:00:00 2001 From: m-guberina <gubi.guberina@gmail.com> Date: Wed, 16 Oct 2024 13:07:34 +0200 Subject: [PATCH] visualizing a log now makes sense. you can add a plot with an updating time-stamp (vertical line) if you want. this plot is completely up to the user to determine --- .../__pycache__/visualize.cpython-312.pyc | Bin 11788 -> 16436 bytes .../manipulator_comparison_visualizer.py | 70 +++++++-- .../ur_simple_control/visualize/visualize.py | 142 ++++++++++++++++-- 3 files changed, 188 insertions(+), 24 deletions(-) diff --git a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc index 8fcb405e56bc736d66c541ba4caf5b5ff6f02c36..74ae94bdf5b0861593f0609b21e017bba3c7be97 100644 GIT binary patch delta 5325 zcmeB)+0wwscbb=%i-CcG;q+YobSC?Wd=iX(6V=OQQaD-|qJ&d8TNt85Qn*?eqC``; zTNt9mDj79-Hg0R+WaOB9S4dfqb2TGKGXn!7Lkd?dXNl-!epZ>utJJwBPvrW{$UV80 z`!OT;WPLv6$vr#{jNFsY@~ATMOn%5K%gUR=w}yYRE3dXNn6;V-W=;)%iXcdZuuzKd z8h#XU5s<hbns|!n<c++t!eS}nXsRVZs#zsdq}K3HX6Bp8%#p$|c|M;glXTT&9)4-2 zH8PVg^Yt>zu92IZ%-_u@KlwesA)~@%MFGu8!rYTX1@<#?PZkxd2Wi_pT~L#ekz?{f zA$vxp$&A9{^~zO~xD*r=6pB(4b3!t6Qv-7HOG;9Uz+$?n03xPPQdF9%P?E2ZSyHT! zmzbLh6)wohFDX_i&qypuElVv@sLU@_NGwWK$jMJn&&*3#$V>wZl@_Nelw_nT=<0$j z)Gd+7%uUq=X)OUMO3O)12OC$VGWnyh@ML{q4aQ%SBZOlae@)&dY{}?1`MvOM(O+Dp z1qG=^3OT81B`X<kG3VqbSIL9aZ~i2*laVoC@*1(pj1iL^#9gKH7#JAr85kIfudy&N zG%$SNW8mPq&Mt9@UE<E>dE!!>jNX%XDjX8^Wnf?^3IY+qAVLX5_)MOlXvUZ{`M9DX zW6I>8iuR1{n=O^LFp9H5;ulP^GB7YOGcYiGHe#DRM@E*@m4Ts_Erqd$ZL*e%v>{s- z3s@BjQOjP#p2Acm#J~Vz)v(1w(+dMb3HxM2P8peWh7{&SjIiWb!xj&+1Z+f)`(!>@ zc~-au{v=w^qs_ul!V5Nvfq|ihv4*LJc{RxS3=9ml95o!19n?fvQdrioPF|y~AePRM z!nTNU4Li&wj0`m#S;CVWS%oJ@adETMaAb*sIQ%Rq3dBHS+?<jiBROk0vm_@o3W^A% za3dvukYFRLupmzgFQ%v=UkYzJV+wy2D+2>U7N@JZYPi6zuHncMo6IO6Jh@kui<v7+ z5abF$fi;3iZez)kp3Ep9#TCQEz);Iw%TvQKc_XVRFE^4BE|3U650VILmORKPO(BF> zi6SfnQ-l$soD7@{H4rPLJ$Xx%VQOl4VQNZLU}D)C3=BmrNal#Z)G#vC@TQ1@<x6T9 zveYI&6cUw0*e8ah4&>p9EW(ptaSKnD=h2vaL6w6I97ST2p~6gAS|BE)KFHgX9pqWL zdCBp+GLqkW?kh8t7{L6Y!cbJk$biM2;uN^^f(C!RgkMnsC=V7gFfeGcJfAfa1Wwu) zb%F%pL`YE)SRPF{-53lW+iNo3;!n=c$;m9v%+HHY&d<p&a;p+7$jr-6PR_{8*DuQ~ zE=|lSz9pEOpHiBWSdy9&pI?xeoLM4YnGUh?ru{8;h)^ZO5?PP~cqhNpkY~TeT9lfY zQaPDd(}=qSB<#(=z)%%5nO{?Ra;|1sy<|}-NVE(@l!FL)5E072z;KH*H?=s!8RVWK zP~I-80LfH>WcX4tiwklRE5l$;gYunW-Y71r0;%IEPA!QqD9SHLEh?#mlw9>7Ljpkr zxHPLrD#<{C)gYn<MAU+aIuMZr3O>FfLlC<GL^OhkCXfZ3$+;=<g{7&bskb;2ld~br zqGphM3y5e1DPqaXPf5MSQkGd|bc?Cb@D@{{QBfPHwB-X;qmvWFMJ7+uQskb+z`$@0 zB;3z5d7qYvOpo6eHU>F``4KZCE~%McmbK_`>Gk^{$RMcuL1=P=Qt)IyZO-~j8kQGW z#ILYeePCl?<+#owc9BJFLg^J2*&8hE*I6VlvPjNQxxym<!GM8>zu&6U>VlZ&MQ*JI zmj^5y*I7g_vWQ*~(_CSEg+>1cOkhIh0^utxstA7i0_GW|3kqjeUSZMv!o$ELJ0o;~ z|Av^$+)fQHle4r1;z3q(UT2ZG$Re>I=K_nw6&B+Mykh;aow42VjcyH29ZWY^*xS7u zy(ciW_}^gRX!mLKnILq9MdSesZ@X`!?*#QLEK+w^I4^KnUtzKNz$C${dxKBthJfUB z-if@E`6r*&?h@r;;1%ol?ev|YeuYQrgDL~R@Q*KQlVfyZ>yMc`sd<SroRqWl5@$Fi z?r7s9#(YMU(MOCq8M#{FhE+j3*cce5Gn7D^F`)7tmJ3rD!6gb$3AC(dU`Szt$uTli zGM9i-AvjZKAu6}YGMrZREHx}y;6@c#7EaW%A`0XbmK0W4L0!w1$xzE)!Vgo3%`q^G z!9@{AEoX@!Omz(hOb#X%#K2GkZ<v7+el1stFkCsjj&xy|z!)3CzyLOqyOyU!6rrYu z2UHco4Hla`Lr!M0H#avU%jA1%vXf8B3G<YQ!<1*qfeL$G#uT<1Mu^5N1(3+(HXbon z-WrxH<;e?qge6sB>T9@QE@fb-;Z9+je3nO%m9K^`OMUV}Az^u>iljsnt_Rg*A3g!j zAO?n7{u;h4ZIJ2Q2-oO<`1ME;3UL$6GcX$F))eLx=2oT}{xl|#muq-y_`x(!3Ogw1 zkX*_S(+Lt^!?7BgoERB;LRc7T__Fk2ZUxbkds*3y8B;h@xYlsP8w&zV3^i;ud^G~F zn6F{4;jsZVD%c@z%rXF1J;tEW6X#h2uLKzxCNTDdvoO@KW|@MjB~X>cnq`h)T7p83 zk!7->pvdGNRSvdV!5Zc)TaYpQd@yH&>m0ku4~66f;a*{60N21wlM97p1Zst9gi-{+ ziC&_HAq%1usV$H<zlJr-8Dx_@l4uR13q!0A14Atj$Ty&{%W?tvV)7&b5pkh(cyeQ8 zfXhRC=?OB46XtUlXgh?HqsI^IQE!km+)LcdNdAR-O&ZB-J!d!>kTRqnNV76h8>^C8 znW4l76f+>K!cZiI2#$KBsydRP$D*8}f;p0*oKaIaF^-9W0o=6C$xkQT#s{^?(HisR ziFqXoiNy*z`FZIe8q~lqNGvV}H|J9lOA@&h6zV}eiNs=sM1_>h<dV$%yu_kPh4PHl zB1oe@JGD}wJToUpAqm0<^&S-R^S~y6Y}8QzyHG(PF)sxySeBSmnyQeISO)HJKy{_0 z7Nq8-q~?_<lqD8rCMM;i7AxpNbk-|?wPogEs!_-+Rw&8LO$D2skys4s!9WcxPRvbJ zNXsu$NX*Gm$Sg_CE!Km&1KMc;d%YxI0pv!7;{4pyk_=F9B{iowRR`Rw$xBU50U20d zUX)pq3KA+($S==RNGr_)2LsGJNbe`J7!-bq3dPx}IjJT2d9c0_#F~u65{10fa)@Om z`Jk{WEl5cO_m4{Q^Pw&Sc_1S-4`h`>N`9U?NKa}?J;)It$7L%dCgqowD0l`qE9fd@ zl@^yMfb0hK&eV$)KoJ6QlCDBYMrN@>Zf1H$3CO16jQq0H6ot&Zl6-|kg_7*dypq%+ zh4j))kQ*}<Qc{yMQ&JT&!M(+tN(HdLlS)fU^7D#84y#uv0`+E!>=+mr{E9&3L6I$} zwE(I@iljj-8Bn`{xhORyM3eazV?`0DL3N8GF)uS0)cz{+0IBf=5&fXnl%$WpdwhV8 ze@KXHkd=a4sE<#ig1?`Sr=M#PxV5E@BA=c=Syq%w5|O|a@=Hsq^x$eH^DC;=YZNJh zj0d-oKn?ODKM>a+L<E2cP+3?c0b+rBfuMGL5x6M@ZoY+pBtXS~ks64l4k9!_L>P$B z0(p)T)HEx}EXheN(gq3YfCvSU40~clS$$4sUTRSUNK6+*M1hECkSt$venDk?T2X#( zd{R<=MNte$2Hc(lWp;2I?-pNCYH>+^QEGfqYI<gVUQr}SBiO<UP!23g0O?5t5um1B zQ8K7~Ck$@ifqKd5MN=5+85oMDf)u2J2(XVcK&(s<0jf5O#6T=#5Mc@;%s_-WhyXj< z0%W65L4IalNqlBrJSa!SgT0gslHdaKLD{dO7*tP#iUU+UIai$BlnW6>Mavl&7>pPh z7>b`TfqLj)q!>83JB*tvCKxu=-r?ZAz~^y=!}A6Q-;>EJ#bxU6h{`YE+rV>K)V9Iz zhJw-s7TG7FCR>7c2yc$KENa*2*WlLS_C!o_hT7!pCC)3@7JFY5({J#9BCfDNczRtY zcL!sK#TO0+0g(yLGdL#tUglR?lC(T)Vb&#ng9{u67x)dH2#QP>n<zG2YNFH}r}^G9 zy;m@<FkF$mL1Sg^bv?(6yn2oY6ff$zT$XXYEa)~_OH!%+1_xI^PbbfWh$|d&UpN?q z#3vNbu$WkTU0my;xYi28i{iT11ob)`Kd>?I^7p%Ux=(SM?m5wOPVxff1sMx<FUuKR z7BSplyrE*F?IFPfh6jW%8oS<*Q&|u=Gjm174&{|~N0<*d9%j3!@AiS2fmh@!10$~o z;|F#IA^i`0pf<F6)Xb<AiW^K1n9PW}EbDQBL+YlU&5oc8j=`7pLN0J9F3`Bhp?!lx zadMB86q`1KgxUv#$?K%V>&34Ns$CRRTTr~bVqwK)L4yV_<TmRKc5dh>2`C0MuW)F6 zU=m<;VFa~aC6(sO&Xk=k-{EzGUvfsoC4SW#!s6G3RW1pu+!2wyAmw~T#N~#7=nWpB z>pY4Vc@!6DUE$HaEh9gH<A#9d3XO{b#@7X`>n{peUl(w_DByaa>OjsFfnbE3*+l`f z>jF*}1)MH8$6Sa_z37~F-8t`~bKX^f{12=QvI?M}6z=!#^q#>ypJOJ+6&^*fUGf)s z<iTuFL~~k_fnWFo7lWYK7cmA_j&{#R&yM6)zaL-NZ5eoEKQS<Yyu{6aom=J-x6B<L z!3#o8S9qL%eqaSj`~s)olS0nCK4uJO%oKbjna@fv`bsiuich|(v)u$Vv{cj%${3*j z4LBXIWGDi)8j3(IxLX`H;BIBAUD0L+P<M!dq1b2gZe3IPPa4ULjKZIMm{k~cKLs%+ RFf!_X_F(pAWE2ML1^^JxBr^a2 delta 1273 zcmdnez}OSRcbb=%i-CcGLFW)(x~S$vJ_*LIiR$GNoD3-(Eeuh@DV!|~Q6ec^Eeuhj zm5iF)8+SEuPJSSyGFeidn<<5Jb2gVOBO}-3PVUEyT$6Kok{P)sU*u6`<evPUM^>08 zg?A0#Y9^5J3=E76HT)_3AaPc(_+&|5ZDGL_Av6`jAQd8L;wd6E{AH7W@kj}arih_w z5C>^sl}M3X!#A0oZ>AJO3PUSXjc^*1Bm)COjbM#1nC4HBs+ugoFU_<@dh&I?US^p! zvXj&KyBXysf95x2m(SK<U?`HD{7^`lQDL&bz)41~$&!Ng%nT_En`aAZGBPquJ}P8C z*-wC*Ka!!xww$4YIg+8AQB%op@<w4>#;VEhg<~13Ci{w5GF7=uULYzyd4b3w-YTxr zf`Zf{g`Cv163xx|qE{FheI~nzPi736d_&w-GK+zM!G(c=q4)|5149GD4R-G9>=KvQ zC7x`ykdWeJbe|locu3Tffq|jO4@CHb2qh5VF<DE=j4^I<qLLwF!sN+H_KeM&FDq?f zoGdRZ%5;Ne@;nvU&B3y2jEwA)_ozxUa!z&>5@u%4;+b41AUwH&o1Kl3p@t)idom-h zFc0e*w$+T#2%0=mPM#-*iGiV(vzDudJxdUzadHnA=j3my5)w6BaD@yd!VoPC3@PlB zTjX@rxl2T0A~oDFbtPgjF>vs<ln752kQe3UPT>UeOKKRhBqrN)iAo|&L$ad=<YF6n zbw-)VYvc`BU|eAtn4t)I@&kF1$pM<Yyxh24uY&CQo^#3!C7_T5o1nr_RL01F#Vv4u zFfza_C)O<=6vX*?{EBiwv6aifzz|ZDH@Q$-e)4o}BenvNp!?+W+Vkp#iV8uTA`npw zB3MC035X~K5oI6(9QoBC7C0>+B?FLHIf$qL5tSgK3Pi*)FfeHH6d8ipH6WrEMAU(Z z1Q1aVA{rPNBpHet85kIDv6N+28Qo$kG`z)BXjIh1z`!s$fn9uZgRUZ17Xt&sIZzt? zGWm|u#L518oUB(^B)@P?KB*_?{D6hyI*Y_b7KsHp7g!{&uoyq!73z=djO>nXbZc<x zV7kG=-tOJ#J%Opk{{{<3yHBIf1feS|B2QR2FK}63VX^tZBs95I*<x~megeBB1HbT( zFH)2FRm3L$)_0dVBJHH+uElViNdrV`Iofz?GN067^web56r23jZaX(9_Y^gQe9|>} xkG&MvGzL%+!@yANIr)LTsmuoz7Dkp&d~uA7!k?U&RTy<Y2Qiv3>K1_t0{~9OA>IH0 diff --git a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py index 3dd8012..6682326 100644 --- a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py +++ b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py @@ -6,7 +6,7 @@ from itertools import zip_longest from multiprocessing import Process, Queue from ur_simple_control.managers import getMinimalArgParser, RobotManager from ur_simple_control.util.logging_utils import LogManager -from ur_simple_control.visualize.visualize import manipulatorComparisonVisualizer +from ur_simple_control.visualize.visualize import manipulatorComparisonVisualizer, logPlotter def getLogComparisonArgs(): parser = getMinimalArgParser() @@ -53,40 +53,81 @@ class ManipulatorComparisonManager: print("you did not give me a valid path, exiting") exit() - self.manipulator_visualizer_queue = Queue() + self.manipulator_visualizer_cmd_queue = Queue() + self.manipulator_visualizer_ack_queue = Queue() # we are assuming both robots are the same, # but this does not necessarily have to be true self.manipulator_visualizer_process = Process(target=manipulatorComparisonVisualizer, args=(args, self.robot1.model, self.robot1.collision_model, - self.robot2.visual_model, self.manipulator_visualizer_queue, )) + self.robot2.visual_model, self.manipulator_visualizer_cmd_queue, + self.manipulator_visualizer_ack_queue)) if self.args.debug_prints: print("MANIPULATOR_COMPARISON_VISUALIZER: manipulator_comparison_visualizer_process started") self.manipulator_visualizer_process.start() # wait for meshcat to start - self.manipulator_visualizer_queue.put((np.zeros(self.robot1.model.nq), np.ones(self.robot2.model.nq))) + self.manipulator_visualizer_cmd_queue.put((np.zeros(self.robot1.model.nq), np.ones(self.robot2.model.nq))) if self.args.debug_prints: print("COMPARE_LOGS_MAIN: i managed to put initializing (q1, q2) to manipulator_comparison_visualizer_queue") # wait until it's ready (otherwise you miss half the sim potentially) # 5 seconds should be more than enough, # and i don't want to complicate this code by complicating IPC time.sleep(5) + self.manipulator_visualizer_ack_queue.get() + + ########################################### + # in case you will want log plotter too # + ########################################### + self.log_plotter = False + self.log_plotter_cmd_queue = None + self.log_plotter_ack_queue = None + self.log_plotter_process = None + + + # NOTE i assume what you want to plot is a time-indexed with + # the same frequency that the control loops run in. + # if it's not then it's pointless to have a running plot anyway. + def createRunningPlot(self, log, log_plotter_time_start, log_plotter_time_stop): + self.log_plotter = True + self.log_plotter_time_start = log_plotter_time_start + self.log_plotter_time_stop = log_plotter_time_stop + self.log_plotter_cmd_queue = Queue() + self.log_plotter_ack_queue = Queue() + self.log_plotter_process = Process(target=logPlotter, + args=(args, log, self.log_plotter_cmd_queue, + self.log_plotter_ack_queue)) + self.log_plotter_process.start() + self.log_plotter_ack_queue.get() + + def updateViz(self, q1, q2, time_index): + self.manipulator_visualizer_cmd_queue.put((q1, q2)) + if self.log_plotter and (time_index >= self.log_plotter_time_start) and (time_index < self.log_plotter_time_stop): + self.log_plotter_cmd_queue.put(time_index - self.log_plotter_time_start) + self.log_plotter_ack_queue.get() + self.manipulator_visualizer_ack_queue.get() # NOTE: this uses slightly fancy python to make it bareable to code # NOTE: dict keys are GUARANTEED to be in insert order from python 3.7 onward - def visualizeManipulatorRuns(self): + def visualizeWholeRuns(self): + time_index = 0 for control_loop1, control_loop2 in zip_longest(self.logm1.loop_logs, self.logm2.loop_logs): print(f'run {self.logm1.args.run_name}, controller: {control_loop1}') print(f'run {self.logm2.args.run_name}, controller: {control_loop2}') # relying on python's default thing.toBool() if not control_loop1: + print(f"run {self.logm1.args.run_name} is finished") q1 = self.lastq1 for q2 in self.logm2.loop_logs[control_loop2]['qs']: - self.manipulator_visualizer_queue.put_nowait((q1, q2)) + self.updateViz(q1, q2, time_index) + time_index += 1 + print(f"run {self.logm2.args.run_name} is finished") if not control_loop2: + print(f"run {self.logm2.args.run_name} is finished") q2 = self.lastq2 for q1 in self.logm1.loop_logs[control_loop1]['qs']: - self.manipulator_visualizer_queue.put_nowait((q1, q2)) + self.updateViz(q1, q2, time_index) + time_index += 1 + print(f"run {self.logm1.args.run_name} is finished") if control_loop1 and control_loop2: for q1, q2 in zip_longest(self.logm1.loop_logs[control_loop1]['qs'], \ self.logm2.loop_logs[control_loop2]['qs']): @@ -94,20 +135,21 @@ class ManipulatorComparisonManager: self.lastq1 = q1 if not (q2 is None): self.lastq2 = q2 - print(self.lastq1) - print(self.lastq2) - self.manipulator_visualizer_queue.put_nowait((self.lastq1, self.lastq2)) + self.updateViz(self.lastq1, self.lastq2, time_index) + time_index += 1 if __name__ == "__main__": args = getLogComparisonArgs() - visualizer = ManipulatorComparisonManager(args) - visualizer.visualizeManipulatorRuns() + cmp_manager = ManipulatorComparisonManager(args) + log_plot = {'random_noise' : np.random.normal(size=(1000, 2))} + cmp_manager.createRunningPlot(log_plot, 200, 1200) + cmp_manager.visualizeWholeRuns() time.sleep(100) - visualizer.manipulator_visualizer_queue.put("befree") + cmp_manager.manipulator_visualizer_cmd_queue.put("befree") print("main done") time.sleep(0.1) - visualizer.manipulator_visualizer_process.terminate() + cmp_manager.manipulator_visualizer_process.terminate() if args.debug_prints: print("terminated manipulator_visualizer_process") diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py index 93f4a13..9890be0 100644 --- a/python/ur_simple_control/visualize/visualize.py +++ b/python/ur_simple_control/visualize/visualize.py @@ -58,16 +58,16 @@ def plotFromDict(plot_data, final_iteration, args): plt.show() -""" -realTimePlotter ---------------- -- true to its name -""" # STUPID MATPLOTLIB CAN'T HANDLE MULTIPLE FIGURES FROM DIFFERENT PROCESS # LITERALLY REFUSES TO GET ME A FIGURE def realTimePlotter(args, queue): + """ + realTimePlotter + --------------- + - true to its name + - plots whatever you are logging if you use the --real-time-plotting flag + """ if args.debug_prints: -# print("REAL_TIME_PLOTTER: real time visualizer has been summoned") print("REAL_TIME_PLOTTER: i got this queue:", queue) plt.ion() fig = plt.figure() @@ -203,7 +203,7 @@ def manipulatorVisualizer(args, model, collision_model, visual_model, queue): # could be merged with the above function. # but they're different enough in usage to warrent a different function, # instead of polluting the above one with ifs -def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, queue): +def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, cmd_queue, ack_queue): # for whatever reason the hand-e files don't have/ # meshcat can't read scaling information. # so we scale manually @@ -213,6 +213,9 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, # this looks exactly correct lmao s *= 0.001 geom.meshScale = s + # there has to be a way + # if not here than elsewhere + geom.meshColor = np.array([0.2,0.2,0.2,0.2]) for geom in collision_model.geometryObjects: if "hand" in geom.name: s = geom.meshScale @@ -222,24 +225,59 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, viz = MeshcatVisualizer(model, collision_model, visual_model) viz.initViewer(open=True) # load the first one - viz.loadViewerModel() + viz.loadViewerModel(collision_color=[0.2,0.2,0.2,0.6]) + #viz.viewer["pinocchio/visuals"].set_property("visible", False) + #viz.viewer["pinocchio/collisions"].set_property("visible", True) + viz.displayVisuals(False) + viz.displayCollisions(True) # maybe needed #viz.displayVisuals(True) +# meshpath = viz.viewerVisualGroupName +# for geom in visual_model(6): +# meshpath_i = meshpath + "/" + geometry_object.name + #viz.viewer["pinocchio/visuals"].set_property("opacity", 0.2) + #viz.viewer["pinocchio"].set_property("opacity", 0.2) + #viz.viewer["pinocchio/visuals/forearm_link_0"].set_property("opacity", 0.2) + + #viz.viewer["pinocchio"].set_property("color", (0.2,0.2,0.2,0.2)) + #viz.viewer["pinocchio/visuals"].set_property("color",(0.2,0.2,0.2,0.2)) + #viz.viewer["pinocchio/visuals/forearm_link_0"].set_property("color", (0.2,0.2,0.2,0.2)) + + ## this is the path we want, with the /<object> at the end + #node = viz.viewer["pinocchio/visuals/forearm_link_0/<object>"] + #print(node) + #node.set_property("opacity", 0.2) + #node.set_property("modulated_opacity", 0.2) + #node.set_property("color", [0.2] * 4) + #node.set_property("scale", 100.2) + # this one actually works + #node.set_property("visible", False) + + node = viz.viewer["pinocchio/visuals"] + #node.set_property("visible", False) + node.set_property("modulated_opacity", 0.4) + node.set_property("opacity", 0.2) + node.set_property("color", [0.2] * 4) + + #meshcat->SetProperty("path/to/my/thing/<object>", "opacity", alpha); + # other robot display viz2 = MeshcatVisualizer(model, collision_model, visual_model) viz2.initViewer(viz.viewer) # i don't know if rootNodeName does anything apart from being different + #viz2.loadViewerModel(rootNodeName="pinocchio2", visual_color=(1.0,1.0,1.0,0.1)) viz2.loadViewerModel(rootNodeName="pinocchio2") # initialize - q1, q2 = queue.get() + q1, q2 = cmd_queue.get() viz.display(q1) viz2.display(q2) + ack_queue.put("ready") print("MANIPULATOR_COMPARISON_VISUALIZER: FULLY ONLINE") try: while True: - q = queue.get() + q = cmd_queue.get() if type(q) == str: print("got str q") if q == "befree": @@ -251,9 +289,93 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model, q1, q2 = q viz.display(q1) viz2.display(q2) + # this doesn't really work because meshcat is it's own server + # and display commands just relay their command and return immediatelly. + # but it's better than nothing. + # NOTE: if there's lag in meshcat, just add a small sleep here before the + # ack signal - that will ensure synchronization because meshat will actually be ready + ack_queue.put("ready") except KeyboardInterrupt: if args.debug_prints: print("MANIPULATOR_COMPARISON_VISUALIZER: caught KeyboardInterrupt, i'm out") viz.viewer.window.server_proc.kill() viz.viewer.window.server_proc.wait() + + +def logPlotter(args, log, cmd_queue, ack_queue): + """ + logPlotter + --------------- + - plots whatever you want as long as you pass the data + as a dictionary where the key will be the name on the plot, + and the value have to be the dependent variables - + the independent variable is time and has to be the same for all items. + if you want to plot something else, you need to write your own function. + use this as a skeleton if you want that new plot to be updating too + as then you don't need to think about IPC - just use what's here. + - this might be shoved into a tkinter gui if i decide i really need buttons + """ + if len(log) == 0: + print("you've send me nothing, so no real-time plotting for you") + return + + plt.ion() + fig = plt.figure() + canvas = fig.canvas + AxisAndArtists = namedtuple("AxAndArtists", "ax artists") + axes_and_updating_artists = {} + + n_cols, n_rows = getNRowsMColumnsFromTotalNumber(len(log)) + # this is what subplot wants + subplot_col_row = str(n_cols) + str(n_rows) + # preload some zeros and initialize plots + for i, data_key in enumerate(log): + # you give single-vector numpy arrays, i instantiate and plot lists of these + # so for your (6,) vector, i plot (N, 6) ndarrays resulting in 6 lines of length N. + # i manage N because plot data =/= all data for efficiency reasons. + assert type(log[data_key]) == np.ndarray + + colors = plt.cm.jet(np.linspace(0, 1, log[data_key].shape[1])) + ax = fig.add_subplot(int(subplot_col_row + str(i + 1))) + ax.set_title(data_key) + # we plot each line separately so that they have different colors + # we assume (N_timesteps, your_vector) shapes. + # values do not update + for j in range(log[data_key].shape[1]): + # NOTE the same length assumption plays a part for correctness, + # but i don't want that to be an error in case you know what you're doing + ax.plot(np.arange(len(log[data_key])), log[data_key][:,j], + color=colors[j], label=data_key + "_" + str(j)) + + # vertical bar does update + point_in_time_line = ax.axvline(x=0, color='red', animated=True) + axes_and_updating_artists[data_key] = AxisAndArtists(ax, point_in_time_line) + axes_and_updating_artists[data_key].ax.legend(loc='upper left') + + # need to call it once + canvas.draw() + canvas.flush_events() + background = canvas.copy_from_bbox(fig.bbox) + + ack_queue.put("ready") + if args.debug_prints: + print("LOG_PLOTTER: FULLY ONLINE") + try: + while True: + time_index = cmd_queue.get() + if time_index == "befree": + if args.debug_prints: + print("LOG_PLOTTER: got befree, logPlotter out") + break + canvas.restore_region(background) + for data_key in log: + axes_and_updating_artists[data_key].artists.set_xdata([time_index]) + axes_and_updating_artists[data_key].ax.draw_artist(axes_and_updating_artists[data_key].artists) + canvas.blit(fig.bbox) + canvas.flush_events() + ack_queue.put("ready") + except KeyboardInterrupt: + if args.debug_prints: + print("LOG_PLOTTER: caught KeyboardInterrupt, i'm out") + plt.close(fig) -- GitLab