From 08a34a51468ca25a3972595b55588842ad3312a6 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Thu, 31 Oct 2024 22:43:36 +0100
Subject: [PATCH] logging real time plotting almost works, will try to fix that
 now

---
 python/examples/camera_no_lag.py              |   5 +-
 python/examples/clik.py                       |  30 ++---------
 python/examples/comparing_logs_example.py     |  27 ++++++++++
 python/examples/crocoddyl_ocp_clik.py         |  45 ++++++++++++++++
 python/examples/drawing_from_input_drawing.py |  15 ++----
 python/examples/oc_for_ik.py                  |  39 --------------
 .../__pycache__/managers.cpython-312.pyc      | Bin 42125 -> 43272 bytes
 .../basics/__pycache__/basics.cpython-312.pyc | Bin 8500 -> 8614 bytes
 python/ur_simple_control/basics/basics.py     |  28 +++++-----
 .../dmp/__pycache__/dmp.cpython-312.pyc       | Bin 16312 -> 16312 bytes
 python/ur_simple_control/managers.py          |  18 +++++--
 ...ontrol.py => crocoddyl_optimal_control.py} |  23 +++++---
 .../__pycache__/logging_utils.cpython-312.pyc | Bin 5753 -> 5903 bytes
 .../__pycache__/visualize.cpython-312.pyc     | Bin 16436 -> 17147 bytes
 .../manipulator_comparison_visualizer.py      |  38 ++++++-------
 .../ur_simple_control/visualize/visualize.py  |  50 +++++++++++++-----
 16 files changed, 187 insertions(+), 131 deletions(-)
 create mode 100644 python/examples/comparing_logs_example.py
 create mode 100644 python/examples/crocoddyl_ocp_clik.py
 delete mode 100644 python/examples/oc_for_ik.py
 rename python/ur_simple_control/optimal_control/{optimal_control.py => crocoddyl_optimal_control.py} (90%)

diff --git a/python/examples/camera_no_lag.py b/python/examples/camera_no_lag.py
index 41c1406..180c52e 100644
--- a/python/examples/camera_no_lag.py
+++ b/python/examples/camera_no_lag.py
@@ -1,7 +1,8 @@
 import cv2
 
-#camera = cv2.VideoCapture(0)
-camera = cv2.VideoCapture("http://192.168.219.153:8080/video")
+camera = cv2.VideoCapture(0)
+# if you have ip_webcam set-up you can do this (but you have put in the correct ip)
+#camera = cv2.VideoCapture("http://192.168.219.153:8080/video")
 
 while True:
     (grabbed, frame) = camera.read()
diff --git a/python/examples/clik.py b/python/examples/clik.py
index dd6791f..99867e0 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -4,24 +4,7 @@ import argparse
 from functools import partial
 from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
 from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL
-# TODO write this in managers and automate name generation
 
-
-"""
-Every imaginable magic number, flag and setting is put in here.
-This way the rest of the code is clean.
-If you want to know what these various arguments do, just grep 
-though the code to find them (but replace '-' with '_' in multi-word arguments).
-All the sane defaults are here, and you can change any number of them as an argument
-when running your program. If you want to change a drastic amount of them, and
-not have to run a super long commands, just copy-paste this function and put your
-own parameters as default ones.
-NOTE1: the args object obtained from args = parser.get_args()
-is pushed all around the rest of the code (because it's tidy), so don't change their names.
-NOTE2: that you need to copy-paste and add aguments you need
-to the script you will be writing. This is kept here 
-only as a convenient reference point.
-"""
 def get_args():
     parser = getMinimalArgParser()
     parser.description = 'Run closed loop inverse kinematics \
@@ -33,17 +16,12 @@ def get_args():
 if __name__ == "__main__": 
     args = get_args()
     robot = RobotManager(args)
-    #Mgoal = robot.defineGoalPointCLI()
-    #compliantMoveL(args, robot, Mgoal)
-    #robot.closeGripper()
-    time.sleep(6.0)
+    Mgoal = robot.defineGoalPointCLI()
+    compliantMoveL(args, robot, Mgoal)
     robot.closeGripper()
-    #robot.openGripper()
+    robot.openGripper()
     if not args.pinocchio_only:
-        print("stopping via freedrive lel")
-        robot.rtde_control.freedriveMode()
-        time.sleep(0.5)
-        robot.rtde_control.endFreedriveMode()
+        robot.stopRobot()
 
     if args.visualize_manipulator:
         robot.killManipulatorVisualizer()
diff --git a/python/examples/comparing_logs_example.py b/python/examples/comparing_logs_example.py
new file mode 100644
index 0000000..c590978
--- /dev/null
+++ b/python/examples/comparing_logs_example.py
@@ -0,0 +1,27 @@
+from ur_simple_control.visualize.manipulator_comparison_visualizer import getLogComparisonArgs, ManipulatorComparisonManager
+import time
+
+args = getLogComparisonArgs()
+cmp_manager = ManipulatorComparisonManager(args)
+
+print("""
+this code animates 2 manipulator and can run an animated
+plot along side it with the same timing.
+you're supposed to select what you want to plot yourself.
+the comparison manager has no idea how many control loops
+you have nor what's logged in them, apart from the fact 
+that everything has to have qs to visualize the manipulators.
+here we're assuming you have 1 control loop per log,
+and that the same things are logged.
+""")
+
+key = list(cmp_manager.logm1.loop_logs.keys())[0]
+cmp_manager.createRunningPlot(cmp_manager.logm1.loop_logs[key], 0, len(cmp_manager.logm1.loop_logs[key]['qs']))
+cmp_manager.createRunningPlot(cmp_manager.logm2.loop_logs[key], 0, len(cmp_manager.logm2.loop_logs[key]['qs']))
+cmp_manager.visualizeWholeRuns()
+cmp_manager.manipulator_visualizer_cmd_queue.put("befree")
+print("main done")
+time.sleep(0.1)
+cmp_manager.manipulator_visualizer_process.terminate()
+if args.debug_prints:
+    print("terminated manipulator_visualizer_process")
diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py
new file mode 100644
index 0000000..b8e3940
--- /dev/null
+++ b/python/examples/crocoddyl_ocp_clik.py
@@ -0,0 +1,45 @@
+import numpy as np
+import time
+import argparse
+from functools import partial
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import get_OCP_args, CrocoIKOCP
+from ur_simple_control.basics.basics import jointTrajFollowingPD
+from ur_simple_control.util.logging_utils import LogManager
+import pinocchio as pin
+
+
+def get_args():
+    parser = getMinimalArgParser()
+    parser = get_OCP_args(parser)
+    args = parser.parse_args()
+    return args
+
+
+if __name__ == "__main__": 
+    args = get_args()
+    robot = RobotManager(args)
+    Mgoal = robot.defineGoalPointCLI()
+    # create and solve the optimal control problem of
+    # getting from current to goal end-effector position.
+    # reference is position and velocity reference (as a dictionary),
+    # while solver is a crocoddyl object containing a lot more information
+    reference, solver = CrocoIKOCP(args, robot, Mgoal)
+    # we need a way to follow the reference trajectory,
+    # both because there can be disturbances,
+    # and because it is sampled at a much lower frequency
+    jointTrajFollowingPD(args, robot, reference)
+
+    print("final position:")
+    print(robot.getT_w_e())
+
+    if not args.pinocchio_only:
+        robot.stopRobot()
+
+    if args.visualize_manipulator:
+        robot.killManipulatorVisualizer()
+    
+    if args.save_log:
+        robot.log_manager.saveLog()
+    #loop_manager.stopHandler(None, None)
+
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index f602be7..fa8caa1 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -1,9 +1,3 @@
-#, this is the main file for the drawing your drawings with a dmp with force feedback
-# TODO:
-# 2. delete the unnecessary comments
-# 8. add some code to pick up the marker from a prespecified location
-# 10. write documentation as you go along
-
 import pinocchio as pin
 import numpy as np
 import matplotlib.pyplot as plt
@@ -65,6 +59,7 @@ def getArgs():
 
 # go and pick up the marker
 # marker position:
+# NOTE: this function does not really work (needs more points)
 """
   R =
  -0.73032 -0.682357 0.0319574
@@ -283,9 +278,9 @@ def controlLoopWriting(dmp, tc, controller, robot, i, past_data):
     # log what you said you'd log
     # TODO fix the q6 situation (hide this)
     log_item['qs'] = q[:6].reshape((6,))
-    log_item['dmp_poss'] = dmp.pos.reshape((6,))
-    log_item['dqs'] = dq.reshape((6,))
-    log_item['dmp_vels'] = dmp.vel.reshape((6,))
+    log_item['dmp_qs'] = dmp.pos.reshape((6,))
+    log_item['vs'] = dq.reshape((6,))
+    log_item['dmp_vs'] = dmp.vel.reshape((6,))
     log_item['wrench'] = wrench.reshape((6,))
     log_item['tau'] = tau.reshape((6,))
 
@@ -332,7 +327,7 @@ if __name__ == "__main__":
         print(translation_vector)
         exit()
     else:
-        # TODO: save this somewhere obviously
+        # TODO: save this somewhere obviously (side file)
         # also make it prettier if possible
         print("using predefined values")
         #q_init = np.array([1.4545, -1.7905, -1.1806, -1.0959, 1.6858, -0.1259, 0.0, 0.0])
diff --git a/python/examples/oc_for_ik.py b/python/examples/oc_for_ik.py
deleted file mode 100644
index da2a7e9..0000000
--- a/python/examples/oc_for_ik.py
+++ /dev/null
@@ -1,39 +0,0 @@
-import numpy as np
-import time
-import argparse
-from functools import partial
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-# TODO: implement
-from ur_simple_control.optimal_control.optimal_control import IKOCP
-# TODO: implement
-from ur_simple_control.basics.basics import jointTrajFollowingPID
-from ur_simple_control.util.logging_utils import saveLog
-import pinocchio as pin
-
-
-def get_args():
-    parser = getMinimalArgParser()
-
-if __name__ == "__main__": 
-    args = get_args()
-    robot = RobotManager(args)
-    #Mgoal = robot.defineGoalPointCLI()
-    Mgoal = pin.SE3.Identity()
-    Mgoal.translation = np.array([0.3,0.3,0.3])
-    traj = IKOCP(robot, Mgoal)
-    exit()
-
-    #log_dict, final_iteration = compliantMoveL(args, robot, Mgoal)
-    if not args.pinocchio_only:
-        print("stopping via freedrive lel")
-        robot.rtde_control.freedriveMode()
-        time.sleep(0.5)
-        robot.rtde_control.endFreedriveMode()
-
-    if args.visualize_manipulator:
-        robot.killManipulatorVisualizer()
-    
-    if args.save_log:
-        saveLog(log_dict, final_iteration, args)
-    #loop_manager.stopHandler(None, None)
-
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index c40c23c490c43a9b9001a93bc44c2fce0a62027b..b59fcce0499fab94884093e38ecf948933a2717a 100644
GIT binary patch
delta 5058
zcmeA@$<%R)iSINoFBby?1B32q<#hS28~NTbGTz(#m+=@AKOaaKgj1AKq}OmwVpg2Y
z&gwBau}_vMMQ!pQCRKj*60laV8jTdq$$hM<{0I^46dkZGCfyQ{CYBVv6#dEnn3M$#
zN_aq8Ksd!P#R#lO(6|I)pGk`88Z(f6y=(%EAcmq9lB{)#%^I%NEFkR+42%pl48aVV
zwwu?n9cE=R)|<@9tvPuPPk}avu5NB(g>GR=)vu!br2G=~Vuh^y%)AnXvecaX<jj)H
z)MAC`qQn&a;@HV{JPKmQdWI%C3W<3s3Mr{+iKRIu3I=+nVB_i)5_5|474q^+QWZ)v
z5=#`yi!zH#Af^>7Bq!!6q~|M?<SUrz8CfV4C8p>oB$bvZWTY0QD&PXCWvNA#B^jA{
z=?a;}3duQ{1qG=oATvrbGK&@R^HLS^N^_G^i}b4abaglL^ZsDuxy6-}Sdv;?5?@rB
zx0#(knwd+3fq}uEfq|j;{pK8@35*gmxYme%U}6$-W_;k||AC!>RpJW+GixB@<d4Fg
z+zKC9!2(RIN}F3n0t6ThCtp)J#cjsGz))lkA`B<5RyAf+pL}0ci*e6pF14+UjGCJd
zsM~We%1$!4Y;OTlV+A5i85kIDv6N)yrWS#8-(rm~E=etb7-9*MV=u@r$Vp8rDY5}6
z2f47waI%o0zk&mZ?+8-Dlu}aU1mZb^c&x=asi_4;E|VJ#eYrhA!ag9vb@EL^6%{WK
z(;K9WIW;fk7E4NIT3RtD0|Ns*Y>U(<D;g;<zTE6&^oU7XlYxODnHiEez$7aJ0|SWv
z`Ojv1(_f5?Dw7{-i%+(<6rRkfB{Nweg>|yFg~(*f65+`f=8}_z6@@1YSnzVOuVw`6
zW?-CLJ4t);GIRdPK{~=rS(=kSYRH3iKve2aHuVvn9Ap94Il0?HX>z=e@Z>3zM8MiI
z%|saWCL3xAGotI|og8n;$7nD)&r)@=m7efqE-UfLdo2YR4JIdQ3X8LXg0IJzg`or#
z;$X+rFk~4_o~S3vXf}DGp2lP|t3*c2$p@Qd8Lc;Ovzp7u7%(}+#+y3`6k@?3B4qL^
zn{rjTB2Xw7fg<k~Q(nO>)~eK^{9;Jtu@$H0r39uF8BTVybv91`X-WizHG6SlS*lNd
zdJ#Czz)=N?xT0u~3KtL&4<aH!MC9bPwkq|Yc)rD&SX7i)Sp+Hria;smmOyG=id#`?
zYD!UNS*mY-O6o0^)QZd!O|~LXJl|p~&P>lM&Ar8%R+N~VS_F#rTkK`2Iq}K4DMg@2
zFJ^<sbdfs9G7|;{hAKJd{JfH){2ZVB`~u&^yu|d>qR9bvJnXKF3=G9WOp}xCR!=@{
zBFAZTk;mu?kI4tN%>wq$jGUmP;it)7lrlNSL0#1sWLzeQ0GYXxp$H@mu|yjr3L+*>
zUg4mqV$aGCkpu}PgJS|pFhS|heUslhs4$987IIXT7Ej@6VT|HU;ca1v;!ELcVTj^S
z;csDx5=fDl9OS6Nl_J){5G7v8swug-%5fJ9lMwf2Gq-RSUWRx@sBsK6jFT0-MObPW
zvYaNr^OT;f=q)n2-iw<FA}&<J5U&JNn!-@S5U)JBz+H|h%Wd)vFHc)Hm=uCe;m_7!
zU?}o0@q~%3hB82!p-hGvhAcO5LiL_pm@UGT<ukd%TaghW<UiSPuCXCDqXof6^Opp|
zj6pVfa-EOu<ai%BMokclF=X<`C@Gd2hInmMuFm8LU%AN*Zak9{eBF6Li41I54P%xM
zNQYDnL%beXgn@yf1eEf?9As$&un0&R9zif^ff|N*BbamzLst6aj~ZY{m`qObQx1pQ
zmLd$x^VqFJk;h^mioDU}dw#BrDd6DD1_kG2AAd{b9GEi@bkR&q4}n|^x1fe0D*<dw
zKFHmZ0$3RfC->{hGc#p@XgQt|P`U-1msK>muuWugS^z(z=Hydua+9Y7STmMR{-_}{
z`AxtSM*Yc+ffKm185kHczc4Z|6a`F{36kSXmv4O?xI>Q9VX}RYDTj5DAt>z^Os)xX
z3<c+Ieh@zrM1V^Ha8UxPsf$2$cu^Th!WBe>f`~W};SM5lK|~&?C}GMgECLrSyda^(
z$wI-JjIon#f{odH9V6lcT_)!STS~@2RF>UhE{iX`#he&lSOhAwiwY<24=(cum4rp*
zAnjoc3=Epg;KBpqQjo@CkWvK&1xV%v<?AZZAW)qTEgSUW<1_OzOXA}ffyxI3md!08
zml<`XJ6L+SZ}1EES9ey=NSU8KGy4+1=8BZ{*(<YmIA7AUzrbOCf#3dynAG&_iP@Wz
z!<3mAH76TJ2FtBv^wVT20$E&S%fP^}k`e5jVo-gdP^3QjM}&;5922M@VqjosU;x=r
z)>#I!rMvb5hfER3kPw?CAdfH=%>sD}WJ=LeP)!as0~8pn>8T}wDOEx!9s!FfPCghZ
z&8WHgZsco5Nw7J*w^&2s%i~kQ?kN%jnVCEJVYI4Q2Z-4XB0y15G!tYnbG~n8Q6I#5
za3RcI7@wY5mYP=#D$PKlIN2sfg;8a4V$58yk3$r|mN4C7D=$jTOU{7Ug6!VO#j$FP
zCX;8y$}wtg-WglOq6|(Zg&^yAK(&oqNpNajaefiF5-I{Gl{u6BlEkyXF&hhVAUNrO
zQw=DhZ?Pwqr51q-_W2-5PzY(VgTwh2cUnn&QEFmJW?p(RxW*^~rN3KDX(dJJAhj8w
zfMkbP8_-lSZ}RyhSw_prZ<D?<YEC|&F1h(n@*mdvH6R^pK?EYGz%dJsm~|itP*6d<
zyb9zWzWDUil5lW9c;>mf-eOKkDJ+@|QivQPAU-IqqQxJ$sT2!J9QEMDF}Xaej!|>-
zw=8Bx$2O3jRt5$JKTTFh+AaWz%mon(K*S`ZIAbkK%qdNU#~4U@!DOo(1;*CN(K+sn
znwuBr{9=}cn8sZMHfRn=(IOB5&f-%i?<!E&1&2R8)WL~blL;KoU^a>cdnfZ1iZf1_
ztX6n{QFHV2!aCOaBOv3Bf(VdRn&P*Z6LWIF&5c`pjwK~UnMtK3sjfvu`9;{>0MdC3
zL>vbZV8?(%9~9q3b|5i?+pt8HVG$yRK#`-#0&)6!P(-nSTFVe$fcgrc=3<fhWYG#W
z#!Z`TDq30UK~DD5<U+XY7I%7TiDPm}X<`njNd&TfD##@6^rFmyg4Ck;veX<%qZZ*R
zP&$Ez*e#~Q6qEql2WqPbqhw%E;t5Q72ucBy9crgBYHq$>YtJMKjv$1g;HcU@S-U}6
z2$VyMHi42f6F5m1sZWk*&|_j|p4`!3$Edmac!L<DGdO>8LOg<Q3dk4WJOnc27F$tb
zUP^v$Q7=dsIl)d=XwqO5ob1^Y!>GA=S(74DJ*c860uvyci&{WBKwM2eA}lJp3Q`WX
z<qU`gE`C5!bc>@Xza+6FGe7SZV+gDy-vbI|_T0pR0#K%gl;#}JjJtobU#kM6<mCL;
zGmM&>ZQABCLtLf{$_<PV2ZCZ!Q?Ll+^;;~U-c->Pkfq>I1s6hW#RaLUDPBbrLBgQo
z<`#QVYH>zlLFz4*oXlcqR|Qn?KuTm#TD!%WnOBmUUX)mpS_Cc}Kq(XCVo<}j=mN;e
zY=tSHmMUuflaCqekVdw~<OyBZ7&SNF>{Vo(+|e`D0F<~OIUP~8wu4*<a`-Kl{DRax
zlxktJZ?6`k+vJ3P|H;RD1;HjMF<LwZ1q6E$e#2OkbMlK*QB5=M*I^8poY0@ZsJZ!I
z|49zU?8)6T_F3!&xnLQH0EI#kTHy!^V@Oqu$a>(yzF>0WOf9w>AboQtZ=5OS1<J#>
zSTpkqN=t6B78m4XmVitDTg*ABc}1$A;A1YXEWX8&T9F)|nU|J-i`6$hKQTv>1Cob9
zp<M(@F-6}e3(k@d2K5b!Ily@Ul$9a%)GQ0exXq=r9<us_`a9{fW`e*@`yx;~%TJRN
zJ#fLz7*Kl;QU!sl9B>N;)TBfWoTUrg7^hBtvmhVRp#)XT;5HTmq%#Rhqn~e0UKk-W
zxo?gzQ*r3z#d91c2Pkn(em_Tka-=)=<ehWGC(msZ28q>IGE_2XGW%6Y6_@0L)3`!e
zW}-qGtY4&%lbTZ`4T?q%h>Jn-4=zQEK&iK=1Qe*?N)znj3J@1L?}EaT6J#{FsHqYI
zQ?RyAaY=rC0a#=eGXq00C>SA)lFH7?>-_4M_|@<52wo7<yTYS?gGb;xkL*Pr+3P&o
z7kRX=@aW#*5$N~o^qNrI<Bue%cacZ$Dv!YjP6h!DNXH0V05Q60N))Al99Ih>>Ocgj
znk%XYHOY#=Nooa1a07_g3?k-%2v9v)1g_!0Ne^6JfQrtd%OKfnAYwm=0A;MAry$k~
z5b+yC`~?yJK|}+H05ydnt-vM_7eur%GKex1gR*uD!$&4DR*p|*WsIz%pDHHvE{gU0
zl*cH{YX4b_mDLd96Oa~7rCXc@nR)ri$r+jXMW9@8i#4w_x1bVS!a&jvIATDlplA-r
zCKeDeck<>%HQb<dUj*uJ+)|jlcag;8pv80a<UrD(@tPt~7qSRsNfD}pZ*kb<=BJeA
qq}mm6PL^5Xr|i$jDD*vrshF9O|04s4{GP?0$-!v&i2+1{9SZ<qf#Q$=

delta 4355
zcmeCU#MFC|iSINoFBby?1A|(ISi1S9jePGI8P{z7%Xo}w^29z_rWB>gf0$G!e`fK3
zr~;`>QA<&ue1S!oU!w%96D+2gA`B8^(kcPzV@c6Y(V6^@Nts`_ga;%K!YO(w`XDue
z1|@tTJ_x56rWma;p2VyuXo4hYnqmePoZQPMAjq|v1*DRJfsvtxA(%naYV!lO!>mju
zdXpLXL?-)li%verQ?S{E?>FOSZ-HoLE_DV5273kuhT=1uHwaH)oSZCL$1U}Nm4Q{_
z3j-6Y%;q<u0RoH$lWo;bahozQFcg`A2!qK&>c)&}lik#{7<X(gRNu<TsIggH)1HG-
zW^%gWWqWgwB1;fq!oa|Ai=`wpH?;_){T6F{aY<?c#1spV9D6~2K~8E~Ns%>3xgCfw
zm|SV(uK;pXkpoBxQ%XsZBS^vt#A7YaNlh&%a-RIc$d}t4B<u|$TqZjhtFU>3m|l~!
zjAa?sCU+PsFh1S9()bY*qsHVOGfB1%RtARY43p~;#3#Ql;oN-M>?-4A5le~5vD(6u
z?JcDlWhNJz3s2s!Ej)R>760TbUBXOR%99QKg(p9-P=G4po&3X6X>ypCFjG<YWJhyx
zxNw@4(&Ttu;mP~7MJBJeQl9)%7prb7Yre@z{=$=^t%WCdSP4(=C>NVN-CA&RlE290
zJ=R=|`jZ>V<t8t+7N4x2E;>2RfNQd}P55Lf6<MYd)6ElY<}xz+Pu92d76=4+Gzdfl
zGcYhzxlZ1&NPO}_yJ%JEB2Xw5fdc;)Q(nO>)~eK^{9;JpvlXZ2r39uF8BA8RcUA!h
zFgRR6p<EONGRzr7#DR!#5D_tXw!MlFC{k{*CKeSXRu+MhaFO}si}rFBps>HiR-Boh
zSDJf^HLWNyH?;^96}Q;SQgh;yb5n}EK<SE&fq_8*42sl1W*SdccTi?`WMp6{e#<yH
z&|x+I2Mz`vfqu(Q%j?|gm$=m@PjpnA{NI#wbCe?wBc}!f1B0I?dr|V_d?$5PACL(d
zAi|h|fng;>5l9+ht`<lXMD$MH?WCw;%gPUt1PLWWvIK-=f=~>fJ0>}+Fp5o9byj5*
zo9yi@$tXTK)meq5l2ub;^JM2;EKJ+DHv4;ovrIk^C_34`lT9FA5vr9Tg`tKaUTLy}
zfgDqo%Vb|)Pg@t56oO9S&(>gIDDp1>g%ntLHJD@oYlbozY8bLyK<SRrbMi-TVP?iG
zugQOX6&WF7zR={yGx=+@h#@vR1i^Ojm-xfXL$+hGzrQUwna%g-X4C-j7=tDo`pYtE
z!P#n)ulkEkKItFK8woR^2IK}qe_3%|m`F)1j0Km|hl!LV!dNglUPHJDLslx(2kb00
z4DrU3Zw4rb!!1e?Ud;$mTEh^J#WECmEEb~38%~b)k(*o?sLq%K4$e$aa85oDXf?Uf
zS6HBECZ->F;YQXlWW|9s<-%OcR?Aewl$AeuZjc-cl%dI305<YgkN~5`<kvxwjKxsZ
zoRbrSr!eYG{uVreJBxvVA@d6(DBdSe43T3kl4f9Fn7kvzl*6jX0F*@YCw~ob3<YN|
zeh@zbM1Zp)IOBrK`XW#zP*ehv07ZFG2#5uWf+9B%D+fg6g33asyuzY*5RVr`BuwrO
z)ntsByfxH#@`q4!$!Lg@vRlk$@rAdT6XOeum_h0aCOd?c`Gd>bQV>6sfq_Ajxd@a>
zAx;EoECwl6P*8wmE>OO!5)H~v$}jOv%u7s9Ez*mR&&<m#iI1NH%E|wkH?xLcX5^8Y
z;c{I@_o9sMMKRsYtdYu~l6PTzu-r;UKTW10khw)Rpp451wyhWx`wB&BlV?TC$ciyB
zFcb?hFfcSQ+~61PFY7GpukNhwuD!q^Q=~C@VSGr$LXbO|il&3y3^Ju?5vY8DngQ}F
zYkF!)U`mw`iaWt#lBkAPbXN4&cGh;+U*M1{(%4)d^O{i-Y&!2P){yw}_*Aefi^M=S
z<xFmlQ#ES?F*`v-E{K>0vW_|5H?yb*;wVty-C{3{PtPn%%_{~KF`%HEd@oLgQD!o8
z{9Hz`7ZkvjFx_G+FG|fz&Vbm0?2E~w32KZwlWh{@*gz#)GUMclW+IzQ5+YfY!KtGF
zWG@e>NOvm<PR%RMF9MhJkQ6d=@`n`hEO5NWfE)=<G2p}kisoDFiDjupps1Szk^}{y
zCObG9ZgHoT#22L|rex-&7lVuNB2cos#gtZ3lm=3p4hm;>csUMD6SF55rphuJPVP_r
z%BZoqHBFSQekDltDiDDPFL0!T;|x?Xfx`>p@8uxB@WrR6mV|>t!!ys-^%iqVN@39q
zkO|1a0^);GDO%Km+Y+ImsI3R5o5_;7b&zmln%po!Wb($`SB@<pz0KfCgcXvy^FboB
zK*U@SF##z8S<4c0N>ky{2a=vU`A(h!W9{U>dG3rFn_csNG0Q?s<1PXlG!vv~K8OJ4
z?n#qliq&<&aR3i_aJtuI0*5u2jbg#B$s3Br89OGQC_VslJ1EW<m87!P9|W0u2t<JF
z)D*wPoS2gXuHbL+IhK?ZWhRxDq`DRr<riUh3`pl;5OD-VfL#TS4p8J5*@DCnj>Hmo
zhDC@d1I3mm3&am=KrzQsl3H8>@eZiP398|X)FyAQQe#}T`Ce5kOFhWRewthem)+t{
zPc3mwE-6jS0X0HE)=vVN#GPK0Sx}H#6knE_18D{#Tm?!v(4f1;RG5Mia=Sq-A7PZt
z4oXddDK|k$V)CQ<X^a}1D;w;YM8PqIFcchTTPB}vk`@Bx+oJWL^vwiL-$iPZe>Leb
z{bri1)ojP8u{pC@jL{jKYdIkvK{o~D3vh-48FGuQC^0W3KewnGB#fMbCm(9jVC0<q
zx+MlVk`r4wnCd|_MiH0*IiaWtq!q-~<RikyqDvs<VCznTSm4qL6j!%6it<YmOEUBG
zZZU?yO81?h;AYQFEGPhFbw~-%0nN^PCVyyGU=*Az*l~tYWAnX^xy%rk>45SFBgBEA
zDAg1!0)@dXmgJnwf})8aOToblE}Gbi3sO^4yo&lk!k|Lv7JE@@aYkZ6>MfR>%wlLO
zA5<Jcig8e)yTzHASCX1ulvt8l1THo}NfhK_Q2(IlEXc`hg(;v0C2F0NhZ+5l_N~HX
zquy(b8k7GmmfSpL0te${t^TP7pp*v5|A@M_734~g<8QI#7o_H))F+eQ_iHiQO_rLF
z05^$~(c%FpFxZRm8^)TPlV6;QYTDI_I*cBZr6%|b$z9};yUL^Rgh%j#ko#30k0OoD
z$&*fUFlJ5GoxRUu7syGAK?EphiqOhZP{0>~Ll2QL!SzD^WaT+p^;bdqK#BX706eFK
zI0poPTm#DTw^%dt3Q9|Eu@)EPWR`$yj$6z*sd+^zpg?9Wt}MRAky?=)pP84Ie~Z;O
zJwGu=lLM0FL2*(9N>@c+Kx$b)a<>>$Z*hVc@!+WS236k09N=sODh(hN;2aCau+8Fg
zAF}#`dMoL(W`e*@`=V@ck<5u6CE(T&xGREa^ne>e;7$ojj&fh@#@IW#Z*hJ-sH!Xi
zXHrHtP4Oa7Yrm)(6c#lgq83De8ZbrRa9s-Gt_2YrK*Vei0jgVyz_kH5{J}X1RJs&h
z0Lfkk5qm%cD9IH)0<oTgh#w&07lSbaLs1=w33dm#lhXj=f`|rC>!^r<fuV)rBa;{_
z$0xHAMpn^JC6k+%#d?0uV-#k!hjddwiZzvPaTa9e<tHa+Wabxv62L9iywco)N^p*Y
zRPvx~25H8DBDx4v^cKyUthc;|4HUyghLble->D}HvL4j4F9P*`ia=(gx$qW;O>TZl
rX-=wL5zFN1EButb85xDX$1oK#GxC3A0FmF**b_Jy?LRSqNU$3L0huz(

diff --git a/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc b/python/ur_simple_control/basics/__pycache__/basics.cpython-312.pyc
index 45e09e3e1cb3ddfb458602f0e07a7f0aa3eee846..e1ec0d0b99d4fb96948b9ab5c2c7409a97300de5 100644
GIT binary patch
delta 758
zcmdnuw9J|BG%qg~0|NuYyd}!%ESekn0yr4WHpg@9W}d9Z<8H}p!@$5$!&$?g!U|@I
z*087WfmysY>?!<U7GDi}iU63!n#oYZp2i%^peZ<c4v)1tM`}?~eo=g3@hvE;toRmN
zS#f-FZptm5qSUn1qSU<PRFFIpudH}77w<IYTa0Csm-2cF-C~Ct0%Md-{=pl?<;%dp
z;LX6mP+Yn>hL4Xaw!!rVuh0ae3633+4Q@ABINN<1eI|%rVG(~It<>Pr5p|J8;s$~x
zD%Rl95z^>)1J00?Yw+lZxX2=QLtdr9qa*GjiwuG_`Ivw_7bgP)12Y2y!{-N^-wSMF
zoSY;hJlTMeV{(qrVn+7K0>WiR>_z$v3=BmEAVM2N=z<7C5Mcx&j6sAv0|SF5PZ7wZ
zBHziIg{!y%85kJ)L0)+_*<3`fUQ~Q~#Kedhsq?dEX3x)?nRi)K_YO8Oe!>2V&WahV
z^SNho&*z)TcbQ-P4mL4i(diZwEoQjR_nzrJ-*2YhWnt|*lG5`fW=bqjU9P=Qd%501
zy~~n@FfmbN!H@hL0&EQ)UxYSa6_H@k3kG>0gn@zK7H45fd}4A*X<|;11V}IxB*>GS
zSP`F@SCX1ul$cZGIaylLhBbkKfnmw!7|A2diXb&ba3eHXiojtM0n=U-GTBi!S`?(A
zhzBHS2_iu0s>pxxM%iT66b1%{FPkOhS{VgEv0nrVw<56el*#Mln;0u58!9-+Ix#XT
dZ*cj*0HP1bd<N5>JeUL-y%;}eP3}`z1_1hT;YR=f

delta 729
zcmZ4Hyv2#{G%qg~0|NttXNy?+YK@J20UV4Lo8vilGqco))Cf%$;_=t#vteLhsNt+(
zPvHl%cx%{G1i&o58uk=HFpD*lp@uz;Iha9HX!0x`Yh(7*qN4nw_`+LIMp=<K0|Ub?
z#<E-7MX712MX7nosqux#oU+M0ywg~V3>g?0CNJgn<SNo<U|=XRU|?XF#23Zo#lXPe
z&A`A=T(UWZkB=#4fzb-~g*F#?^c!4n@Cr>Zn&8+G+2D4Ag|pqK(Px6_6&CRaQi=^8
z9g!DV#BU&2BBBi*9l?#hH{c98<pz(A*o!REH{h(v=LO`sI2afhm>C!tK2O;ELtqo*
z<X=L0jBJxLh0A!^5H2ecpZrl+p3!Tvph#7{9|HqJAIOQ97#J8D7@i18UXilgVRGHt
z_oB7$iP8&U@t1`X8oZx~h)wsH=rJR7e)P=f`EfJjE{o{g!6wEh(4WznF`;yN^~CDw
zbrb6@^QqmzCMF>@pJOJ+0!`87(hH@R%Po|<EMag5Du^uhk(XV7t-<4qz-D(*2^PHo
zQ1AwVB89UsB|b5^q%<+7NCG4n1QO)QO{|E|%qvMvFG|cQa+};MX~P=Jz`(F*^Et^Q
z%(@^oMQ|fDS&FnlIzvGOSPEotm4$D9S*lln=VSqPt;sKCRV6?M6!Cx*S%L^q$|>>z
zMYw^8=wxd-Z`MQx28K_YyX9IL1wb)g1Paq4u&%_(3JOh(rIQycILJCOGAeIy`M?08
W56FB5)1Ta!1Q@*-KWI!+Tm}GnR@t)v

diff --git a/python/ur_simple_control/basics/basics.py b/python/ur_simple_control/basics/basics.py
index 8fd4b84..aeb9534 100644
--- a/python/ur_simple_control/basics/basics.py
+++ b/python/ur_simple_control/basics/basics.py
@@ -126,16 +126,17 @@ def jointTrajFollowingPDControlLoop(stop_at_final : bool, robot: RobotManager, r
     Kd = 0.5
 
     #          feedforward                      feedback 
-    v_cmd = v_ref + Kp * error_q + Kd * error_v
+    v_cmd = v_ref + Kp * error_q #+ Kd * error_v
     qd_cmd = v_cmd[:6]
     robot.sendQd(v_cmd)
 
-    log_item['error_q'] = error_q
-    log_item['error_v'] = error_v
-    log_item['q'] = q
-    log_item['v'] = v
-    log_item['reference_q'] = q_ref
-    log_item['reference_v'] = v_ref
+    log_item['error_qs'] = error_q
+    log_item['error_vs'] = error_v
+    log_item['qs'] = q
+    log_item['vs'] = v
+    log_item['vs_cmd'] = v_cmd
+    log_item['reference_qs'] = q_ref
+    log_item['reference_vs'] = v_ref
 
     return breakFlag, {}, log_item
 
@@ -143,12 +144,13 @@ def jointTrajFollowingPD(args, robot, reference):
     # we're not using any past data or logging, hence the empty arguments
     controlLoop = partial(jointTrajFollowingPDControlLoop, args.stop_at_final, robot, reference)
     log_item = {
-        'error_q' : np.zeros(robot.model.nq),
-        'error_v' : np.zeros(robot.model.nv),
-        'q' : np.zeros(robot.model.nq),
-        'v' : np.zeros(robot.model.nv),
-        'reference_q' : np.zeros(robot.model.nq),
-        'reference_v' : np.zeros(robot.model.nv)
+        'error_qs' : np.zeros(robot.model.nq),
+        'error_vs' : np.zeros(robot.model.nv),
+        'qs' : np.zeros(robot.model.nq),
+        'vs' : np.zeros(robot.model.nv),
+        'vs_cmd' : np.zeros(robot.model.nv),
+        'reference_qs' : np.zeros(robot.model.nq),
+        'reference_vs' : np.zeros(robot.model.nv)
     }
     loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
     loop_manager.run()
diff --git a/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc b/python/ur_simple_control/dmp/__pycache__/dmp.cpython-312.pyc
index 55668f2b64c34f7d4c980310fea151591c9ef526..4e6126a9fbe330cb962c4cf2245ed8776e8eb631 100644
GIT binary patch
delta 20
ccmdl{zoVY}G%qg~0|NuYntzHLx!2kQ084iU+yDRo

delta 20
bcmdl{zoVY}G%qg~0|Ntto}Ji6?zQ#+Lrw*g

diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index eb33ff8..dfffff5 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -100,9 +100,13 @@ def getMinimalArgParser():
                  very useful and convenient when running simulation before running on real", \
                          default=False)
     parser.add_argument('--acceleration', type=float, \
-            help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.4. \
+            help="robot's joints acceleration. scalar positive constant, max 1.7, and default 0.3. \
                    BE CAREFUL WITH THIS. the urscript doc says this is 'lead axis acceleration'.\
                    TODO: check what this means", default=0.3)
+    parser.add_argument('--max-qd', type=float, \
+            help="robot's joint velocities [rad/s]. scalar positive constant, max 3.14, and default 0.5. \
+                   BE CAREFUL WITH THIS. also note that wrist joints can go to 6.28 rad, but here \
+                        everything is clipped to this one number.", default=0.5)
     parser.add_argument('--debug-prints', action=argparse.BooleanOptionalAction, \
             help="print some debug info", default=False)
     parser.add_argument('--save-log', action=argparse.BooleanOptionalAction, \
@@ -341,7 +345,7 @@ class ControlLoopManager:
             print("putting it to freedrive for good measure too")
             self.robot_manager.rtde_control.freedriveMode()
 
-        if args.save_log:
+        if self.args.save_log:
             self.robot_manager.log_manager.saveLog()
         # set kill command and join visualization processes
         # TODO: actually send them a SIGINT and a SIGKILL if necessary 
@@ -470,7 +474,9 @@ class RobotManager:
         # we're clipping joint velocities with this.
         # if your controllers are not what you expect, you might be commanding a very high velocity,
         # which is clipped, resulting in unexpected movement.
-        self.max_qd = 0.5 * args.speed_slider
+        self.MAX_QD = 3.14
+        assert args.max_qd <= self.MAX_QD and args.max_qd > 0.0
+        self.max_qd = args.max_qd
 
         self.gripper = None
         if (self.args.gripper != "none") and not self.pinocchio_only:
@@ -929,3 +935,9 @@ class RobotManager:
         if self.args.debug_prints:
             print("terminated manipulator_visualizer_process")
 
+    def stopRobot(self):
+        if not self.args.pinocchio_only:
+            print("stopping via freedrive lel")
+            self.rtde_control.freedriveMode()
+            time.sleep(0.5)
+            self.rtde_control.endFreedriveMode()
diff --git a/python/ur_simple_control/optimal_control/optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
similarity index 90%
rename from python/ur_simple_control/optimal_control/optimal_control.py
rename to python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 4da6ba3..152e0f9 100644
--- a/python/ur_simple_control/optimal_control/optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -22,7 +22,7 @@ def get_OCP_args(parser : argparse.ArgumentParser):
 
 # TODO: use fddp and incorporate torque (i.e. velocity constraints)
 #   --> those will correspond to max_qd and acceleration attributes in robotmanager 
-def IKOCP(args, robot : RobotManager, goal : pin.SE3):
+def CrocoIKOCP(args, robot : RobotManager, goal : pin.SE3):
     # create torque bounds which correspond to percentage
     # of maximum allowed acceleration 
     # NOTE: idk if this makes any sense, but let's go for it
@@ -41,11 +41,13 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
     # first 3 are for tracking, state and control regulation
     runningCostModel = crocoddyl.CostModelSum(state)
     terminalCostModel = crocoddyl.CostModelSum(state)
+    # cost 1) u residual (actuator cost)
     uResidual = crocoddyl.ResidualModelControl(state, state.nv)
+    uRegCost = crocoddyl.CostModelResidual(state, uResidual)
+    # cost 2) x residual (overall amount of movement)
     xResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
     xRegCost = crocoddyl.CostModelResidual(state, xResidual)
-    uRegCost = crocoddyl.CostModelResidual(state, uResidual)
-    # cost 1) distance to goal -> SE(3) error
+    # cost 3) distance to goal -> SE(3) error
     framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
             state,
             # TODO: check if this is the frame we actually want (ee tip)
@@ -54,10 +56,14 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
             goal.copy(),
             state.nv)
     goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
-    # cost 2) u residual (actuator cost)
-    uResidual = crocoddyl.ResidualModelControl(state, state.nv)
-    # cost 3) x residual (overall amount of movement)
-    xResidual = crocoddyl.ResidualModelState(state, x0, state.nv)
+    # cost 4) final ee velocity is 0 (can't directly formulate joint velocity,
+    #         because that's a part of the state, and we don't know final joint positions)
+    frameVelocityResidual = crocoddyl.ResidualModelFrameVelocity(
+            state,
+            robot.model.getFrameId("tool0"),
+            pin.Motion(np.zeros(6)),
+            pin.ReferenceFrame.WORLD)
+    frameVelocityCost = crocoddyl.CostModelResidual(state, frameVelocityResidual)
     # put these costs into the running costs
     runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
     runningCostModel.addCost("xReg", xRegCost, 1e-3)
@@ -66,6 +72,7 @@ def IKOCP(args, robot : RobotManager, goal : pin.SE3):
     # NOTE: shouldn't there be a final velocity = 0 here?
     terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
     terminalCostModel.addCost("uReg", uRegCost, 1e3)
+    terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
 
     # the 4th cost is for defining bounds via costs
     # NOTE: could have gotten the same info from state.lb and state.ub.
@@ -153,7 +160,7 @@ if __name__ == "__main__":
     robot.q = pin.randomConfiguration(robot.model)
     goal = pin.SE3.Random()
     goal.translation = np.random.random(3) * 0.4
-    reference, solver = IKOCP(args, robot, goal)
+    reference, solver = CrocoIKOCP(args, robot, goal)
     # we only work within -pi - pi (or 0-2pi idk anymore)
     #reference['qs'] = reference['qs'] % (2*np.pi) - np.pi
     log = solver.getCallbacks()[1]
diff --git a/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/logging_utils.cpython-312.pyc
index f47591d995636eee4dd219f3004ed75c1e0574b0..fe80f5e9d71c11753cadb1118869190383545e4c 100644
GIT binary patch
delta 728
zcmeyV)33*OnwOW0fq{Wx%|FHTG=YtLJDC`nCZAvmW@Mf$!+f2Wc{L+QmVu#&BZYYq
zi>@F`3hNrS)l3jUMurrY$#yK!jO>$VvB)xVOy0rLD$MCu1d_hRm6KSKT3ixeRGK$A
zjCC&~=VTGKCoD3TxOF%GVasKd<6vN5cnLCHletKQfq|ij8$^H{qsdYvKDnL4m{D@_
zZjM@cX$A&{WRQ^%OBop07#J9s85kHo&tPO=m_GR-r}*SN&NC({j5Q2dApJ0PP<l4Q
zTqc+|7#T`<p~4Ix)sq!DM0jA{sbwkQpDe&4!&1YLB{bQPSD2p#CdJ55!;&RBc>$L!
zGqWbk<O5u<>#JN-5=#;lGK&?8QWH}ubrf>)({mH^64O(Q6v{Jmauo9N%N2?f%Tg68
z^Gl0Bvc(D|`3mUZ7E^hWpC)$^$l<q`@(OOTCKeSXR^H-D&Ph$o3oQsL&AY`~P?VWh
zQY6N}z;KHtzaTa57F)sOJKXYY;-DZ?o6N{#z$iJ{h)0!i{p1LqbU6;*4-AZgf;R+3
zuM4VN6jZq&p!$KCkzf4_10$!#lgaye^cbZ#zvH>e2r)ox@<~1!M#;&~_!Jm@C$sYh
zGfHoc=KsjdC_C9#Sdx)<a=Nf3BhTc8!itRIlMf45OG_{?FcgEL3&}G@k|4JTPmUCk
zXN;L#De{?7dUCF)xuO(ESP?`RfCzICA<V$Qu#%w&6tP8;laGpO@e8v`e$wJ$RsSp}
u$tqeTJ^7!gxQYr$DIbWi0}-wuf(1lGfC#8&OEE+KTt-IGPYfUuY$O1uyrN40

delta 587
zcmeCz`>DfsnwOW0fq{Xc>nLCPNB)g`JDJ#-v&|V8iliqOaEeWS!eq|KGFgWCIxov=
zMvyuNh9Zs>mPstSf~+ZQYuHyaK?E5YQdlS3u|zX+OrFIe%g8x-2TQ9YmtPS``W9DC
zVo7RoNqkXh-Yu5A{Jhl30jzr&xhC_mJz){M#I3&h8(S`;947+<!%L7In#@I_3=9lK
zJRpLbfq_AjrAT6OBZo1g)a0!kwbC*S3=GL2gCSNkFt9N&FfcPPFnrFK?92IlatfEY
zI3vtqwJasPFm+jclLPsLC--xSP2SDL$0$6Rk4uK7l1Y<!@++>_dbgO$ll(Nfia<8r
zV#+JH#hO@DlvsI-D>)}MF)y?r2;}<wg4DbsaRvs4$$>mdY!V<Bs!y)uF<_LMyoyJa
zapL3)Jn54|dG#1&Hn;FzWrU~@pB&FG!zeYmhF^ivd-7cVU`CnEANfBrOUr>gCvc0a
zxHPGtC_g#1xcC-p0N4t?$$y0{8F?q0izqTmOpX+(mX>5-U?>K;O#uOlq(BZ3nfzWv
zo-t}NzvyR1naRvz=8DoFVI>e@03ysmga`uz!%BuCP~a6wO^z1R(h+2p{G`Uis{UC{
zl2sHU!2)s+dyyss1H&!Wywco)$|6;eX8y?=#U$9Ede4g)@@F$Lihg1MkzkDgJer0y

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 240ee89c04d2fca703a08f0ec3f80304f4b696c7..cdb303e8174d784085e917dde458ea4c3b6b14d9 100644
GIT binary patch
delta 2735
zcmdne!1%kBk?%AwFBby?1H<{B%IQ~}Hu4?kV7<f1z%YIC10jjcpE#tL7#TM6azA5a
zP2s8GFPr>}N0T*00K$?I5lj(U!@rsdq@RI-ks(DGCLxj{iXtHfm0%W6k(kWNx0y+@
zYO(;oG}9WX$=CUM#iiHCtY(CnGJ&awnT4T-Im>=>p@8h<8T?U<vXeja8?wt~YcMbr
zNltz!q|7M4*<WBQlcWMdTO>n|Z8<{)b0kAKqo(3!S>XqaqD*DQ>GG|w19!-A9)K|%
zHmi%uFfsZ~b`hV<7&iHaxGPr<0|SFS0|P_x(#`4;2N`|%Cs<tMmcGHm*YDlwJ)?Am
zN{9Cq9{n5KxWt7gR9xhi$5byc!Q<lO6sbFGwhR*5ADlM3OZPBx<})&=B{BZ^k}>&?
zjIIbX*I5Nuo?s4!%Nz>9+RT@=7=yJp%gUOvFnUf7Ry-u?&A`A=6aXRuL4*>B@S3cp
zWX6~<IZ?@wF=_H-C41IZ1_p+Oo3AVVXE)CSsmNzwV9;cFK5HfjoU|`$2MNN7kfH*x
zJhJd)UhPQ6_Q~nmNo++R#h#PTYcJ3*1~E%OL@9`n2NA&x3=Fq8b5n~mobz+?i;7r5
zf@L70eDXXU_4*2sJWp|INqj+3enDzcNhQSnwIC(_AOh?sB=3O4DnUdQh^Ph;H6S7Z
z<VwCGLlC<TMAU<b29Wuj$+;=<g{7&bskb;2ld~brqDBS=28E&~kg8^oDwe$bl+;@+
zWtmk*x0nhIZ!r}b6}3!G(Y58C!N9<93KZl0Oq1^_nQVTbJBf+o4m$%wyGXm}<T8U6
zvo6t2(JXMzfTT+p*(FlK3uC9Sq_DzLe=SQULoMrM14C^?wp#WQL718vHdqox5@Uyn
zfqY!cF}cT3QINBit3(v8i?N1l@;<|EMy|<!jN~Uz;1OWv?i8I|V<gYdfn*M63ftt1
zJc_J5H9T4BlO2VGCAnZGGcuHDqL}u_NSB$nhG(*lv3)9+Bm)BjOeKu2VXOg*r!c25
zw=&i6rZIt3)o|DFf@!W4c93h4Z0CjP1c|TVSj_^lkCCA#goUAoXYvJORarhJh8mU{
zo*F(_IMuM$aM>_0Fo5XELM9F@og!JLlM4l8Kn!ybgQ-(wawDGzOQ&d-)#O3};mHl$
z>_WBt9U>i~X-p}6EgZE1ps1N_%dalTUn2lBxK<EcWH9my^9#Tn#>h~^kmWGho=bjm
zrYW}=CqoKjtq><ejZg|(6)yurtxye9mh<F70V#o6;Tqu-AxHrO(<3a5FlPc|9&4va
zmMbWb<@pifHH<C{u|5n8wOk+%fdVSaZSpHqb#dWzMkH0Nkf6;nn7mU^ofF1$gL%)5
zk)g+r1>`P~DmDg&6h@Fkip@)yAtB1ZP$PsetWzY*7bcFNkwc4(k)cy03mlj-d4UL1
zQbbWqAsp;7wfsoB#Spr&1@#2R9*fD-g%tQYk*uj?o_t<Nlo1>UlHv#hA{lxt;H9<1
zW&`s@Opb1#jO)(8z>o~B3&6eug@G^w1H<PP3=9lYnWi(8z-3dAg1D0@iy4%9SUQ=q
zSV0V_PNpn&un8zcCvz4DEZA9U8A})^H*$*9S2Aca``uzsDM~EQ%uD~po19;enVO=I
zoS0XZSggrf1TL~7K<<kK6)b$o`305nX+`<D@kvSf6-7~?l8WUPYie0)UP)0lNSz;u
zNCp)#plqR_(9lpM4${h=pXUlvRVAI1pB|8tUs94<q*rf~lb@WJQ*5UPmCa>jU?>J<
zlOGKXcQ|-EGOut*+~DBr=kDb0;l07Zcb!A}B8T)94%r)`($__`E{bZcFkWx5(&DnH
zNe355L;h6`xgt<<yd{)WT2hjq7hh16T3j3tabYh514EUhkH33-fRBGji0kAxmI92P
zlmA-k@V_(yIeZ?7n9sn#Fj>>eFha9P5yS@vf(nS`10q0)un6o;kPnL_Kw{wfNg2cf
zm+#;TBM8J*1rcf>LLEeCfQXREo2+!|!$3@35CIS0q6Hu>C{S;4=N9C|C+Fwor6!jY
zfol&hkQ}H+yv0|PT3nJ}lp0@@nx2`TR}>BkUrtcXQ<0Kbl2{Z6QW*~-Ks8BGA}Er?
zz}3ku=A8WWqDder?#UoB4Me0*F0fYe1-r)rWRy@rer8@td}dyJNoH<pd`@OwDkN?f
zfn=GJGgEG{C+C;um82Hk;sPtl%u7kFfP@J+>cu9%6Bk!<aDYTT7b3=s)-y0Ln1CWV
zo|%E+=jQL$R~ePRa<H<>f8b-_=Iht$)Vj{4aFI*lf}-h$^2=QI4bC6f7+CoxpSP72
zm0@*Zydfmq!G42J=mwAQgu+gr4)@8QZB-d>PL{Nbspnu2m6?$=(Px3<B@xvp0-_Vr
zr|{hn5Sz|Dk$Z;oe2<wPR|HgV2#8%5P`)Ujd{qFfB=d@Z0#wZuUbrMkp{(+CS)+@x
zMjMQ`TWqwrENe4?>yCivg#0T4YBzX9`+YloXQ*G{QTnLQz%TrPhe1$`>5JCnRd(ir
zW-iwL91Q0<6#TWB&ucOIYft`Uw@U!j)+z$ESBgM&aMAk7d+enccTc`<?;sn`$SC~D
ag;|AB_frsK0wbgD=Md&(Mn++9ItBnxaFUAv

delta 2140
zcmey}%DAP0k?%AwFBby?1B2B({&Xh$jeN&BIGnf`7^XASFif5xBeMA;hcpwT@n&A`
zXN>GM{3$%y8Vr+v@MwbA0#J5R4S$MYwgv-3k#LPrmMO?IRGcD|BD{uwH4{XXk)eh^
zMT8XfDWa1Dd1Z~oQpC|rk-%$;WQx?}`MjH%q^l;c;*(}tBQv>>uUAlZjofNRm`f%w
z^_WfO=dWdypWMrD$fz)RKfflU;^vq9TbUS@CeIfVXH?$2UFZR$7Dq~ML3}}eaq%tY
zl)~b4V=(x=AI7l11y)d&np3=ajfe~rW5DE-Vv`voCdZ1qa^*2FFxWFNFce?gyhi*W
zBa85al1q~dB}LqXCsbYHR=mN(*YDlwJ)?AmN{9Cq9{n5K!V^j_am&EOOI8Sj#C1X9
z$m(I@lV?jxv4FHp-YI#9!<CUiLi>Xk<Kz$a;+r>1l`wLaGcu?pG5+{cFxfyxSHz6#
zoUkiTur<SFYlRRw<}0#{A#$4wWlUKZy(ceMI3()Jz`#%x1R{b#gc6AGnOvu6#+WpD
ztD+%e%H-#Y_N?s;3=B&*Yb*U{H!lFGC}dz@&}4Z&YbFSsv@hxe3Brkxq9U+7vhd_Y
ztw_es$-A|ZxJy8ay%`u7s)8oJ)l#19tzDsC3KA;=5#=C49z=vPFfiQW%uOxMaL&)k
zFDhaM308oJ%E_O!)f1~g@;t?<CGiDC`30#(C6y45)Ps}+f(WqZko*M_s|FD@AfgsT
z)Paa3kaPKp3_<J$5YY%CnkHB2NO3oVm@Ocpb@F^23C6a`J9KQhXE87^oCC#iKhtDG
zC6mo2x|5h#cd#)qOrJc_Ky>mG{XDZ;mKv5Ua9)F?J{VccTEYusr?8~3!tzNiTP8y-
z`{WJ-Z9|S)&JsbGni>vR`b84sgo%Lyp_Xg%69Yv-?pmG_QMfL~8lK5whTV)jlNTB4
zv+~xkWGPR+C?G7Z3Rl5}WFGe<BV|^;8on&`$%X>LlV#Pp`ARh5DnVw280oU|*YIU&
zgG`xRYsfWuv5|s1k0b*F1I#Tjx`wd^ES|!g!raPK!=J_kQdPrS!w;r;QrJPRMzWoM
zGMBL|Bj4l+8X}Wzj0I%{m>6o<YWQjdU}00kUc+O<z`y{aCr>byoZM`z&QimgWja|<
zNCw0(hcPTCCklv6ZWI)mJVA{^qgJqnIm;I24+NdU2XiYULk&Zg-Q)suc|m@J7$*ZK
zLk-hpejyowTA>=D6j0H}P$U6UI(elDH(TEP8rCdlP;lu9B6QX;x-i80Ffi2efZPi5
zZ<Y(fUNCF2j(~)qP&y-$CiWU0u(z^JML5|ZftTen`J<o=M~@#1Lk(+|H<GuwnOU>I
z{xu@(xt=qe3?-n91GYKK4`hrI!s1G1Wrh+Tup9#eg9<~D6e2trg(goh7iSdSyuy4D
zld%UV2Y7;reo)Sp^znC(5Ag9132_awQg93P@rhLM_w(`eb1eerY;_-h_sM#qVv`+g
z_$J5N7}{$TDS|YEbEFE0<p(1CK|}zEU;z;lAOc(jDT7$xoC_`rLMETJkzfp){LDtD
zJ_01A3nHRGL^P-r;7iUgsEki5%Fm5YO3JS&iUG-hivW=4z{SEXzM|CPlKi67_@dPG
z%>2BfNRUQOQ1MZbl30>hlmOC`2qHkmK~XZOOb`Z_3AdPY^3#i^fTX4}2s1DgrGl7X
zXJ>#|nUm+(DltY(-ft@_l?yVJt0XfwH9j*hCAFehn1O*o0R$!=&=Ha3%E?a;$jL7$
zNi8Z`&cMK6#K^!<{Dg^tVY7?fRYv9qEWDF#9Aw!ASX~%z@Ci-Mb5LcxF?otZj42xf
zuQ0gUV4lx0lj91H;td|5>pb!odE~)t(SF}f-x=yxc$7X!GVlw3;9?LI`yw{E(9v8_
z$eGv2jNy!#g0Cd=SqVm8$;mq%cWHwvsUlFZUIZ%2z;0Q|Py|Y^MdBbYaM<MLr<CTT
n+7)e{JkQBNHkpx8_>&K_3Zw3)AjSknM%~XI%-)QQ!eA``A<p4|

diff --git a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
index 6682326..9537c15 100644
--- a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
+++ b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
@@ -76,34 +76,36 @@ class ManipulatorComparisonManager:
         self.manipulator_visualizer_ack_queue.get()
 
         ###########################################
-        #  in case you will want log plotter too  #
+        #  in case you will want log plotters too  #
         ###########################################
-        self.log_plotter = False
-        self.log_plotter_cmd_queue = None
-        self.log_plotter_ack_queue = None
-        self.log_plotter_process = None
+        self.log_plotters = []
 
 
     # 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.
+    # TODO: put these in a list so that we can have multiple plots at the same time
+    class RunningPlotter:
+        def __init__(self, args, log, log_plotter_time_start, log_plotter_time_stop):
+            self.time_start = log_plotter_time_start
+            self.time_stop = log_plotter_time_stop
+            self.cmd_queue = Queue()
+            self.ack_queue = Queue()
+            self.process = Process(target=logPlotter, 
+                                         args=(args, log, self.cmd_queue, 
+                                               self.ack_queue))
+            self.process.start()
+            self.ack_queue.get()
+
     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()
+        self.log_plotters.append(self.RunningPlotter(self.args, log, log_plotter_time_start, log_plotter_time_stop))
 
     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()
+        for log_plotter in self.log_plotters:
+            if (time_index >= log_plotter.time_start) and (time_index < log_plotter.time_stop):
+                log_plotter.cmd_queue.put(time_index - log_plotter.time_start)
+                log_plotter.ack_queue.get()
         self.manipulator_visualizer_ack_queue.get()
 
     # NOTE: this uses slightly fancy python to make it bareable to code
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 9890be0..6d1cfde 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -107,17 +107,13 @@ def realTimePlotter(args, queue):
         colors = plt.cm.jet(np.linspace(0, 1, log_item[data_key].shape[0]))
         ax = fig.add_subplot(int(subplot_col_row + str(i + 1)))
         # some hacks, i'll have to standardize things somehow
-        if data_key == 'qs':
+        if 'qs' in data_key:
             ax.set_ylim(bottom=-6.14, top=6.14)
-        if data_key == 'dmp_poss':
-            ax.set_ylim(bottom=-6.14, top=6.14)
-        if data_key == 'dqs':
-            ax.set_ylim(bottom=-1.7, top=1.7)
-        if data_key == 'dmp_vels':
-            ax.set_ylim(bottom=-1.7, top=1.7)
+        if 'vs' in data_key:
+            ax.set_ylim(bottom=-3.14, top=3.14)
         if 'wrench' in data_key:
             ax.set_ylim(bottom=-20.0, top=20.0)
-        if data_key == 'tau':
+        if 'tau' in data_key:
             ax.set_ylim(bottom=-2.0, top=2.0)
         axes_and_updating_artists[data_key] = AxisAndArtists(ax, {})
         for j in range(log_item[data_key].shape[0]):
@@ -302,7 +298,13 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model,
         viz.viewer.window.server_proc.wait()
 
 
-
+# TODO: this has to be a class so that we can access
+# the canvas in the onclik event function
+# because that can be a method of that class and 
+# get arguments that way.
+# but otherwise i can't pass arguments to that.
+# even if i could with partial, i can't return them,
+# so no cigar from that
 def logPlotter(args, log, cmd_queue, ack_queue):
     """
     logPlotter
@@ -353,16 +355,30 @@ def logPlotter(args, log, cmd_queue, ack_queue):
         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
+    # need to call it once to start, more if something breaks
     canvas.draw()
     canvas.flush_events()
     background = canvas.copy_from_bbox(fig.bbox)
 
+    # we need to have an event that triggers redrawing
+    #cid = fig.canvas.mpl_connect('button_press_event', onclick)
+    def onEvent(event):
+        print("drawing")
+        canvas.draw()
+        canvas.flush_events()
+        background = canvas.copy_from_bbox(fig.bbox)
+        print("copied canvas")
+
+    cid = fig.canvas.mpl_connect('button_press_event', onEvent)
+
+
     ack_queue.put("ready")
     if args.debug_prints:
         print("LOG_PLOTTER: FULLY ONLINE")
     try:
+        counter = 0
         while True:
+            counter += 1
             time_index = cmd_queue.get()
             if time_index == "befree":
                 if args.debug_prints:
@@ -372,8 +388,18 @@ def logPlotter(args, log, cmd_queue, ack_queue):
             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()
+            # NOTE: this is stupid, i just want to catch the resize event
+            if not (counter % 50 == 0):
+            #if True:
+                print(counter)
+                canvas.blit(fig.bbox)
+                canvas.flush_events()
+            else:
+                print("drawing")
+                canvas.draw()
+                canvas.flush_events()
+                background = canvas.copy_from_bbox(fig.bbox)
+                print("copied canvas")
             ack_queue.put("ready")
     except KeyboardInterrupt:
         if args.debug_prints:
-- 
GitLab