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