diff --git a/Dockerfile_challenge b/Dockerfile_challenge
new file mode 100644
index 0000000000000000000000000000000000000000..a90a1cec5665026f2cefb352d34c4cfd909c33a8
--- /dev/null
+++ b/Dockerfile_challenge
@@ -0,0 +1,82 @@
+FROM docker.io/ros:humble
+
+LABEL org.opencontainers.image.authors="marko.guberina@control.lth.se"
+
+# install python3-tk without questions
+ARG DEBIAN_FRONTEND=noninteractive
+ENV TZ=Europe/Stockholm
+
+RUN apt-get update && apt-get install -y --no-install-recommends \
+        python3  \
+        python3-pip \
+        python3-tk \
+        ipython3 \
+        git \
+        sudo \
+        man-db \
+        manpages-posix \
+        iputils-ping \
+        arp-scan \
+        # nice to have
+#        python3-opencv \
+        vim \
+        vim-addon-manager \
+        vim-youcompleteme \
+        vim-python-jedi \
+        zsh \
+        zsh-syntax-highlighting \
+        libarchive-tools \
+        libxcb-cursor0\
+        python3-python-qt-binding 
+        
+# qt-binding is a really unnecessary 300MB, but i don't want
+# to do more matplotlib hacks
+
+#RUN sed -i 's:^path-exclude=/usr/share/man:#path-exclude=/usr/share/man:' \
+#        /etc/dpkg/dpkg.cfg.d/excludes
+#RUN yes | unminimize 
+
+# make the environment more usable
+# create user
+RUN useradd -m -s /bin/zsh -G sudo -u 1000 student
+
+WORKDIR /home/student/
+RUN passwd -d student
+USER student
+# copy repo to workdir
+RUN mkdir SimpleManipulatorControl
+COPY --chown=student . ./SimpleManipulatorControl
+RUN mkdir -p .cache/zsh/
+COPY --chown=student /dot_files_for_docker/.vimrc /home/student/
+COPY --chown=student /dot_files_for_docker/.zshrc /home/student/
+COPY --chown=student /dot_files_for_docker/global_extra_conf.py /home/student/
+
+RUN vam install python-jedi && vam install youcompleteme
+
+# sh does not have sourcing 
+# and some packages (conda) want shell environment variables 
+# (which i can say a lot about, but can't do anything about)
+# ((the only reason to even use conda is to not have to compile pinocchio))
+SHELL ["/bin/bash", "--login", "-c"]
+#SHELL ["/bin/bash"]
+
+# this is enough to run clik
+WORKDIR /home/student/
+USER student
+# TODO: install casadi and pinochio 3.0+
+# TODO: verify this stuff below works
+# --> this can be done with conda
+#RUN mkdir -p ~/miniconda3
+#RUN wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O /home/student/miniconda3/miniconda.sh
+#RUN bash /home/student/miniconda3/miniconda.sh -b -u -p ~/miniconda3
+#RUN rm /home/student/miniconda3/miniconda.sh
+#ENV PATH=/home/student/miniconda3/bin:$PATH
+#RUN source /home/student/miniconda3/bin/activate
+RUN pip install -e ./SimpleManipulatorControl/python/
+#RUN conda config --add channels conda-forge 
+##RUN conda install --solver=classic conda-forge::conda-libmamba-solver conda-forge::libmamba conda-forge::libmambapy conda-forge::libarchive
+#RUN conda install -y casadi opencv
+#RUN conda install --solver=classic -y pinocchio crocoddyl -c conda-forge
+RUN pip install matplotlib meshcat ur_rtde argcomplete \
+                qpsolvers ecos example_robot_data meshcat_shapes \
+                pyqt6 pin crocoddyl
diff --git a/TODOS b/TODOS
index 419550ac1f57d3785d59ba8ddcff3e919188813b..b7ef7559aa46295c944bd376709f46001644db20 100644
--- a/TODOS
+++ b/TODOS
@@ -164,6 +164,7 @@ goal 5: more convenience - set gripper payload etc automatically
     # ALSO NOTE: to use this you need to change the version inclusions in
     # ur_rtde due to a bug there in the current ur_rtde + robot firmware version 
     # (the bug is it works with the firmware verion, but ur_rtde thinks it doesn't)
+2. debug comparison log
 
 goal 6: panda/yumi
 ----------------
diff --git a/python/examples/casadi_ocp_collision_avoidance.py b/python/examples/casadi_ocp_collision_avoidance.py
index d2a3c105b50b7f3f5cd761f718979053296f03f2..dce56f70e3cf41a7560b23ae66224ba744cff009 100644
--- a/python/examples/casadi_ocp_collision_avoidance.py
+++ b/python/examples/casadi_ocp_collision_avoidance.py
@@ -27,8 +27,7 @@ if __name__ == "__main__":
     T_goal = robot.defineGoalPointCLI()
     T_goal.rotation = robot.getT_w_e().rotation
     if args.visualize_manipulator:
-        robot.manipulator_visualizer_queue.put(
-                {"Mgoal" : T_goal})
+        robot.updateViz({"Mgoal" : T_goal})
     reference, opti = createCasadiIKObstacleAvoidanceOCP(args, robot, T_goal)
     followKinematicJointTrajP(args, robot, reference)
     
diff --git a/python/examples/comparing_logs_example.py b/python/examples/comparing_logs_example.py
index c59097852b909e385717e42dec7a4779de0de1a0..bc92dd675ccd0ccccc80fab42aa5b471b92362bb 100644
--- a/python/examples/comparing_logs_example.py
+++ b/python/examples/comparing_logs_example.py
@@ -1,9 +1,6 @@
 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.
@@ -13,15 +10,22 @@ 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.
+also, you need to provide the two log files
+with --log-file1=/path/to/file1 and
+--log-file1=/path/to/file2
 """)
 
+args = getLogComparisonArgs()
+cmp_manager = ManipulatorComparisonManager(args)
+
 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.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")
+cmp_manager.visualizer_manager.sendCommand("befree")
+time.sleep(100)
 print("main done")
 time.sleep(0.1)
-cmp_manager.manipulator_visualizer_process.terminate()
+cmp_manager.visualizer_manager.terminateProcess()
 if args.debug_prints:
     print("terminated manipulator_visualizer_process")
diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index 8b95290a283076097f374d8fcfd38271f0030835..3d78e8df1587d127d2826f2d742f82af6f67e755 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -13,7 +13,7 @@ from ur_simple_control.visualize.visualize import plotFromDict
 from ur_simple_control.clik.clik import getClikArgs
 import pinocchio as pin
 import crocoddyl
-import mim_solvers
+import importlib.util
 import argcomplete
 
 
@@ -27,6 +27,9 @@ def get_args():
 
 
 if __name__ == "__main__": 
+    if importlib.util.find_spec('mim_solvers'):
+        import mim_solvers
+
     args = get_args()
     robot = RobotManager(args)
     # TODO: put this back for nicer demos
@@ -36,8 +39,7 @@ if __name__ == "__main__":
     robot.Mgoal = Mgoal.copy()
     if args.visualize_manipulator:
         # TODO document this somewhere
-        robot.manipulator_visualizer_queue.put(
-                {"Mgoal" : Mgoal})
+        robot.visualizer_manager.sendCommand({"Mgoal" : Mgoal})
     # 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),
diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py
index 71cc5bf8a8464ba57cef3458bac00bb4ea07bfb0..fc3ba0071b69a7f1c1158e90ba104fc5a218e199 100644
--- a/python/examples/crocoddyl_ocp_clik.py
+++ b/python/examples/crocoddyl_ocp_clik.py
@@ -31,8 +31,7 @@ if __name__ == "__main__":
 
     if args.visualize_manipulator:
         # TODO document this somewhere
-        robot.manipulator_visualizer_queue.put(
-                {"Mgoal" : Mgoal})
+        robot.visualizer_manager.sendCommand({"Mgoal" : Mgoal})
 
     # create and solve the optimal control problem of
     # getting from current to goal end-effector position.
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index 0b12e579a67a1c3b313869c551c96009c53b34dd..e3213a2cf4e897c76c0fd8336d3dc6f8d3ede2ab 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -46,7 +46,7 @@ def getArgs():
     parser.add_argument('--find-marker-offset', action=argparse.BooleanOptionalAction, \
             help="""
     whether you want to do find marker offset (recalculate TCP
-    based on the marker""", default=False)
+    based on the marker""", default=True)
     parser.add_argument('--board-wiping', action=argparse.BooleanOptionalAction, \
             help="are you wiping the board (default is no because you're writing)", \
             default=False)
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index 1bbd90f9fe36f9f436075faf420246ec085a382a..5b8f74a4dc8a0755426e3f64379e226548f9d048 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,173 +1,183 @@
-0.00000,0.85946,-1.46493,-1.40353,-1.08818,2.11438,-0.49363
-0.11628,0.85941,-1.46494,-1.40354,-1.08817,2.11440,-0.49366
-0.23256,0.85937,-1.46495,-1.40354,-1.08817,2.11443,-0.49370
-0.34884,0.85935,-1.46496,-1.40353,-1.08818,2.11444,-0.49372
-0.46512,0.85934,-1.46496,-1.40351,-1.08818,2.11444,-0.49372
-0.58140,0.85935,-1.46497,-1.40349,-1.08820,2.11444,-0.49371
-0.69767,0.85937,-1.46496,-1.40347,-1.08821,2.11442,-0.49369
-0.81395,0.86007,-1.46479,-1.40289,-1.08853,2.11388,-0.49305
-0.93023,0.86253,-1.46416,-1.40106,-1.08955,2.11200,-0.49084
-1.04651,0.86472,-1.46353,-1.39968,-1.09034,2.11036,-0.48889
-1.16279,0.86665,-1.46287,-1.39874,-1.09091,2.10893,-0.48719
-1.27907,0.86954,-1.46191,-1.39740,-1.09172,2.10680,-0.48467
-1.39535,0.87300,-1.46058,-1.39627,-1.09247,2.10429,-0.48169
-1.51163,0.87706,-1.45897,-1.39518,-1.09322,2.10135,-0.47821
-1.62791,0.88047,-1.45741,-1.39473,-1.09365,2.09890,-0.47532
-1.74419,0.88480,-1.45544,-1.39425,-1.09414,2.09580,-0.47167
-1.86047,0.89139,-1.45216,-1.39420,-1.09459,2.09111,-0.46616
-1.97674,0.89821,-1.44860,-1.39466,-1.09482,2.08627,-0.46051
-2.09302,0.90640,-1.44403,-1.39588,-1.09481,2.08046,-0.45379
-2.20930,0.91320,-1.44005,-1.39735,-1.09459,2.07564,-0.44825
-2.32558,0.92006,-1.43588,-1.39919,-1.09423,2.07077,-0.44269
-2.44186,0.92420,-1.43332,-1.40042,-1.09396,2.06782,-0.43935
-2.55814,0.92964,-1.42982,-1.40231,-1.09349,2.06395,-0.43498
-2.67442,0.93516,-1.42618,-1.40443,-1.09294,2.06002,-0.43056
-2.79070,0.93779,-1.42440,-1.40555,-1.09263,2.05814,-0.42846
-2.90698,0.94185,-1.42154,-1.40747,-1.09207,2.05525,-0.42524
-3.02326,0.94589,-1.41860,-1.40956,-1.09144,2.05236,-0.42203
-3.13953,0.94845,-1.41664,-1.41107,-1.09097,2.05053,-0.42001
-3.25581,0.95241,-1.41343,-1.41372,-1.09011,2.04769,-0.41688
-3.37209,0.95629,-1.41004,-1.41675,-1.08908,2.04491,-0.41383
-3.48837,0.96149,-1.40507,-1.42157,-1.08740,2.04118,-0.40974
-3.60465,0.96791,-1.39878,-1.42782,-1.08520,2.03656,-0.40473
-3.72093,0.97188,-1.39461,-1.43219,-1.08364,2.03370,-0.40163
-3.83721,0.97691,-1.38927,-1.43783,-1.08163,2.03007,-0.39772
-3.95349,0.98075,-1.38511,-1.44229,-1.08003,2.02729,-0.39474
-4.06977,0.98456,-1.38091,-1.44684,-1.07840,2.02453,-0.39179
-4.18605,0.98591,-1.37941,-1.44849,-1.07781,2.02356,-0.39075
-4.30233,0.98852,-1.37640,-1.45184,-1.07660,2.02167,-0.38874
-4.41860,0.99098,-1.37359,-1.45495,-1.07548,2.01988,-0.38685
-4.53488,0.99341,-1.37084,-1.45800,-1.07439,2.01812,-0.38498
-4.65116,0.99606,-1.36776,-1.46145,-1.07315,2.01619,-0.38295
-4.76744,0.99898,-1.36404,-1.46582,-1.07155,2.01407,-0.38071
-4.88372,1.00043,-1.36211,-1.46813,-1.07070,2.01302,-0.37960
-5.00000,1.00069,-1.36175,-1.46858,-1.07054,2.01282,-0.37940
-5.11628,1.00376,-1.35680,-1.47493,-1.06815,2.01059,-0.37705
-5.23256,1.00622,-1.35157,-1.48216,-1.06536,2.00879,-0.37516
-5.34884,1.00743,-1.34805,-1.48731,-1.06334,2.00790,-0.37422
-5.46512,1.00808,-1.34556,-1.49109,-1.06184,2.00741,-0.37371
-5.58140,1.00869,-1.34168,-1.49724,-1.05937,2.00696,-0.37323
-5.69767,1.00885,-1.34037,-1.49935,-1.05852,2.00684,-0.37310
-5.81395,1.00878,-1.33765,-1.50398,-1.05662,2.00688,-0.37313
-5.93023,1.00781,-1.33405,-1.51069,-1.05381,2.00758,-0.37383
-6.04651,1.00531,-1.32942,-1.52011,-1.04978,2.00937,-0.37568
-6.16279,1.00268,-1.32677,-1.52630,-1.04706,2.01127,-0.37764
-6.27907,0.99803,-1.32472,-1.53284,-1.04404,2.01463,-0.38114
-6.39535,0.99165,-1.32358,-1.53902,-1.04104,2.01923,-0.38597
-6.51163,0.97010,-1.32759,-1.54706,-1.03614,2.03474,-0.40249
-6.62791,0.96078,-1.33028,-1.54906,-1.03465,2.04142,-0.40972
-6.74419,0.94887,-1.33520,-1.54929,-1.03370,2.04993,-0.41904
-6.86047,0.93945,-1.33963,-1.54863,-1.03332,2.05663,-0.42647
-6.97674,0.92389,-1.34795,-1.54608,-1.03331,2.06767,-0.43887
-7.09302,0.91465,-1.35323,-1.54410,-1.03352,2.07419,-0.44631
-7.20930,0.90691,-1.35776,-1.54231,-1.03377,2.07963,-0.45257
-7.32558,0.90221,-1.36054,-1.54121,-1.03394,2.08293,-0.45640
-7.44186,0.89759,-1.36329,-1.54012,-1.03411,2.08617,-0.46017
-7.55814,0.89265,-1.36614,-1.53913,-1.03422,2.08962,-0.46423
-7.67442,0.87846,-1.37316,-1.53837,-1.03374,2.09950,-0.47594
-7.79070,0.87118,-1.37652,-1.53846,-1.03331,2.10454,-0.48200
-7.90698,0.86391,-1.37969,-1.53893,-1.03275,2.10957,-0.48809
-8.02326,0.85690,-1.38217,-1.54040,-1.03180,2.11439,-0.49399
-8.13953,0.85078,-1.38417,-1.54200,-1.03086,2.11859,-0.49916
-8.25581,0.84478,-1.38602,-1.54380,-1.02984,2.12270,-0.50427
-8.37209,0.84073,-1.38723,-1.54510,-1.02912,2.12546,-0.50772
-8.48837,0.83876,-1.38781,-1.54575,-1.02876,2.12681,-0.50941
-8.60465,0.83767,-1.38808,-1.54620,-1.02853,2.12754,-0.51034
-8.72093,0.83716,-1.38820,-1.54642,-1.02842,2.12789,-0.51078
-8.83721,0.83661,-1.38831,-1.54669,-1.02829,2.12827,-0.51125
-8.95349,0.83602,-1.38842,-1.54699,-1.02814,2.12867,-0.51175
-9.06977,0.83548,-1.38851,-1.54729,-1.02799,2.12903,-0.51221
-9.18605,0.83491,-1.38859,-1.54763,-1.02783,2.12942,-0.51271
-9.30233,0.83434,-1.38866,-1.54799,-1.02766,2.12981,-0.51320
-9.41860,0.83430,-1.38867,-1.54801,-1.02765,2.12984,-0.51323
-9.53488,0.83426,-1.38867,-1.54804,-1.02763,2.12987,-0.51327
-9.65116,0.83413,-1.38869,-1.54810,-1.02760,2.12995,-0.51337
-9.76744,0.83118,-1.39037,-1.54776,-1.02760,2.13196,-0.51592
-9.88372,0.82780,-1.39313,-1.54599,-1.02815,2.13425,-0.51884
-10.00000,0.82245,-1.39880,-1.54102,-1.02991,2.13788,-0.52350
-10.11628,0.81409,-1.41035,-1.52877,-1.03451,2.14355,-0.53085
-10.23256,0.81003,-1.41701,-1.52105,-1.03750,2.14629,-0.53443
-10.34884,0.80468,-1.42703,-1.50877,-1.04232,2.14990,-0.53920
-10.46512,0.80130,-1.43428,-1.49945,-1.04603,2.15218,-0.54223
-10.58140,0.79687,-1.44412,-1.48663,-1.05118,2.15516,-0.54621
-10.69767,0.79257,-1.45403,-1.47355,-1.05647,2.15805,-0.55010
-10.81395,0.78840,-1.46355,-1.46099,-1.06158,2.16084,-0.55388
-10.93023,0.78493,-1.47143,-1.45059,-1.06583,2.16316,-0.55704
-11.04651,0.78217,-1.47764,-1.44242,-1.06919,2.16500,-0.55956
-11.16279,0.78003,-1.48260,-1.43582,-1.07191,2.16642,-0.56152
-11.27907,0.77591,-1.49297,-1.42170,-1.07777,2.16916,-0.56530
-11.39535,0.77267,-1.50210,-1.40886,-1.08315,2.17132,-0.56830
-11.51163,0.76914,-1.51297,-1.39326,-1.08972,2.17367,-0.57159
-11.62791,0.76691,-1.52007,-1.38296,-1.09409,2.17514,-0.57366
-11.74419,0.76310,-1.53136,-1.36682,-1.10095,2.17767,-0.57723
-11.86047,0.75804,-1.54696,-1.34424,-1.11060,2.18101,-0.58198
-11.97674,0.75586,-1.55373,-1.33441,-1.11484,2.18244,-0.58403
-12.09302,0.75312,-1.56265,-1.32129,-1.12051,2.18425,-0.58663
-12.20930,0.75097,-1.57037,-1.30969,-1.12555,2.18566,-0.58867
-12.32558,0.74898,-1.57865,-1.29694,-1.13111,2.18697,-0.59058
-12.44186,0.74722,-1.58643,-1.28481,-1.13643,2.18813,-0.59227
-12.55814,0.74625,-1.59247,-1.27496,-1.14078,2.18877,-0.59322
-12.67442,0.74539,-1.60077,-1.26095,-1.14699,2.18935,-0.59410
-12.79070,0.74515,-1.60415,-1.25512,-1.14958,2.18952,-0.59435
-12.90698,0.74511,-1.60480,-1.25400,-1.15009,2.18955,-0.59439
-13.02326,0.74508,-1.60539,-1.25296,-1.15055,2.18956,-0.59442
-13.13953,0.74507,-1.60603,-1.25183,-1.15106,2.18957,-0.59444
-13.25581,0.74506,-1.60671,-1.25062,-1.15160,2.18958,-0.59446
-13.37209,0.74509,-1.60796,-1.24833,-1.15262,2.18957,-0.59444
-13.48837,0.74514,-1.60860,-1.24712,-1.15317,2.18953,-0.59440
-13.60465,0.74520,-1.60887,-1.24658,-1.15341,2.18949,-0.59434
-13.72093,0.74522,-1.60890,-1.24651,-1.15344,2.18949,-0.59433
-13.83721,0.74609,-1.60978,-1.24402,-1.15459,2.18893,-0.59353
-13.95349,0.75016,-1.61035,-1.23885,-1.15706,2.18630,-0.58980
-14.06977,0.75560,-1.60905,-1.23567,-1.15869,2.18277,-0.58479
-14.18605,0.76401,-1.60453,-1.23538,-1.15914,2.17728,-0.57708
-14.30233,0.76798,-1.60198,-1.23605,-1.15899,2.17468,-0.57345
-14.41860,0.77275,-1.59821,-1.23814,-1.15823,2.17155,-0.56911
-14.53488,0.77970,-1.59170,-1.24310,-1.15628,2.16696,-0.56280
-14.65116,0.78657,-1.58434,-1.24974,-1.15358,2.16240,-0.55659
-14.76744,0.79160,-1.57872,-1.25503,-1.15141,2.15905,-0.55207
-14.88372,0.79665,-1.57294,-1.26066,-1.14909,2.15568,-0.54754
-15.00000,0.80069,-1.56824,-1.26531,-1.14717,2.15298,-0.54393
-15.11628,0.80373,-1.56468,-1.26887,-1.14571,2.15093,-0.54122
-15.23256,0.80470,-1.56350,-1.27009,-1.14520,2.15029,-0.54036
-15.34884,0.80669,-1.56107,-1.27260,-1.14416,2.14895,-0.53859
-15.46512,0.80865,-1.55858,-1.27525,-1.14306,2.14763,-0.53685
-15.58140,0.81062,-1.55600,-1.27806,-1.14189,2.14630,-0.53511
-15.69767,0.81160,-1.55473,-1.27944,-1.14132,2.14564,-0.53425
-15.81395,0.81355,-1.55221,-1.28216,-1.14019,2.14433,-0.53252
-15.93023,0.81455,-1.55088,-1.28364,-1.13958,2.14365,-0.53164
-16.04651,0.81647,-1.54823,-1.28661,-1.13834,2.14235,-0.52994
-16.16279,0.81841,-1.54552,-1.28971,-1.13705,2.14104,-0.52823
-16.27907,0.82128,-1.54145,-1.29437,-1.13511,2.13909,-0.52571
-16.39535,0.82320,-1.53870,-1.29755,-1.13378,2.13779,-0.52403
-16.51163,0.82605,-1.53443,-1.30260,-1.13168,2.13586,-0.52153
-16.62791,0.82881,-1.53032,-1.30745,-1.12966,2.13397,-0.51911
-16.74419,0.83072,-1.52747,-1.31084,-1.12825,2.13268,-0.51745
-16.86047,0.83258,-1.52466,-1.31419,-1.12686,2.13141,-0.51582
-16.97674,0.83261,-1.52462,-1.31424,-1.12684,2.13139,-0.51580
-17.09302,0.83350,-1.52331,-1.31578,-1.12620,2.13078,-0.51503
-17.20930,0.83439,-1.52200,-1.31731,-1.12557,2.13017,-0.51425
-17.32558,0.83533,-1.52066,-1.31887,-1.12492,2.12953,-0.51343
-17.44186,0.83635,-1.51917,-1.32063,-1.12419,2.12884,-0.51255
-17.55814,0.83732,-1.51770,-1.32238,-1.12347,2.12817,-0.51171
-17.67442,0.83743,-1.51752,-1.32259,-1.12338,2.12809,-0.51161
-17.79070,0.83874,-1.51514,-1.32567,-1.12209,2.12720,-0.51048
-17.90698,0.83905,-1.51453,-1.32650,-1.12175,2.12699,-0.51021
-18.02326,0.84002,-1.51246,-1.32929,-1.12057,2.12632,-0.50936
-18.13953,0.84052,-1.51125,-1.33101,-1.11984,2.12598,-0.50892
-18.25581,0.84101,-1.50983,-1.33309,-1.11895,2.12564,-0.50850
-18.37209,0.84145,-1.50827,-1.33546,-1.11794,2.12533,-0.50810
-18.48837,0.84173,-1.50717,-1.33717,-1.11721,2.12514,-0.50786
-18.60465,0.84198,-1.50598,-1.33905,-1.11640,2.12497,-0.50764
-18.72093,0.84218,-1.50481,-1.34094,-1.11559,2.12483,-0.50745
-18.83721,0.84241,-1.50294,-1.34404,-1.11425,2.12466,-0.50724
-18.95349,0.84253,-1.50166,-1.34619,-1.11332,2.12458,-0.50713
-19.06977,0.84257,-1.50104,-1.34725,-1.11286,2.12455,-0.50709
-19.18605,0.84261,-1.50038,-1.34838,-1.11237,2.12452,-0.50705
-19.30233,0.84264,-1.49972,-1.34952,-1.11188,2.12450,-0.50702
-19.41860,0.84265,-1.49907,-1.35065,-1.11139,2.12449,-0.50700
-19.53488,0.84266,-1.49842,-1.35178,-1.11090,2.12448,-0.50700
-19.65116,0.84265,-1.49778,-1.35291,-1.11040,2.12448,-0.50699
-19.76744,0.84263,-1.49715,-1.35404,-1.10991,2.12449,-0.50700
-19.88372,0.84261,-1.49648,-1.35525,-1.10939,2.12451,-0.50702
-20.00000,0.84261,-1.49644,-1.35532,-1.10936,2.12451,-0.50702
+0.00000,0.93845,-1.46316,-1.40613,-1.05605,2.05935,-0.42868
+0.05495,0.93841,-1.46317,-1.40614,-1.05604,2.05938,-0.42872
+0.10989,0.93837,-1.46317,-1.40615,-1.05604,2.05940,-0.42875
+0.16484,0.93834,-1.46318,-1.40616,-1.05603,2.05942,-0.42877
+0.21978,0.93831,-1.46319,-1.40616,-1.05602,2.05944,-0.42879
+0.27473,0.93828,-1.46319,-1.40616,-1.05602,2.05946,-0.42881
+0.32967,0.93827,-1.46319,-1.40617,-1.05601,2.05946,-0.42882
+0.38462,0.93827,-1.46318,-1.40617,-1.05601,2.05946,-0.42882
+0.43956,0.93829,-1.46317,-1.40616,-1.05602,2.05943,-0.42879
+0.49451,0.93833,-1.46316,-1.40615,-1.05602,2.05941,-0.42877
+0.54945,0.94162,-1.46188,-1.40514,-1.05663,2.05679,-0.42599
+0.60440,0.94633,-1.46034,-1.40345,-1.05762,2.05314,-0.42210
+0.65934,0.96110,-1.45514,-1.40004,-1.06010,2.04208,-0.41023
+0.71429,0.96696,-1.45285,-1.39930,-1.06083,2.03771,-0.40558
+0.76923,0.97236,-1.45106,-1.39815,-1.06173,2.03372,-0.40134
+0.82418,0.99662,-1.44066,-1.39826,-1.06358,2.01597,-0.38260
+0.87912,1.00312,-1.43783,-1.39852,-1.06399,2.01120,-0.37764
+0.93407,1.01069,-1.43436,-1.39923,-1.06431,2.00565,-0.37190
+0.98901,1.03109,-1.42384,-1.40357,-1.06416,1.99070,-0.35659
+1.04396,1.03416,-1.42220,-1.40436,-1.06408,1.98844,-0.35430
+1.09890,1.03624,-1.42106,-1.40496,-1.06400,1.98691,-0.35275
+1.15385,1.03726,-1.42050,-1.40527,-1.06395,1.98616,-0.35200
+1.20879,1.04878,-1.41339,-1.41007,-1.06289,1.97767,-0.34347
+1.26374,1.04883,-1.41336,-1.41009,-1.06289,1.97764,-0.34343
+1.31868,1.05067,-1.41208,-1.41112,-1.06260,1.97628,-0.34208
+1.37363,1.05375,-1.40981,-1.41308,-1.06203,1.97400,-0.33981
+1.42857,1.05685,-1.40730,-1.41547,-1.06128,1.97171,-0.33753
+1.48352,1.05723,-1.40694,-1.41586,-1.06114,1.97143,-0.33725
+1.53846,1.05836,-1.40581,-1.41709,-1.06071,1.97059,-0.33641
+1.59341,1.05948,-1.40467,-1.41837,-1.06026,1.96977,-0.33559
+1.64835,1.06029,-1.40369,-1.41956,-1.05982,1.96916,-0.33499
+1.70330,1.07022,-1.38772,-1.44116,-1.05144,1.96178,-0.32768
+1.75824,1.07330,-1.38128,-1.45046,-1.04773,1.95949,-0.32541
+1.81319,1.07521,-1.37752,-1.45581,-1.04561,1.95806,-0.32400
+1.86813,1.07766,-1.37149,-1.46478,-1.04201,1.95622,-0.32219
+1.92308,1.08496,-1.34982,-1.49779,-1.02870,1.95075,-0.31680
+1.97802,1.08687,-1.34034,-1.51297,-1.02248,1.94931,-0.31536
+2.03297,1.08765,-1.33677,-1.51863,-1.02018,1.94871,-0.31478
+2.08791,1.08782,-1.33590,-1.52001,-1.01961,1.94859,-0.31465
+2.14286,1.08816,-1.33331,-1.52427,-1.01785,1.94832,-0.31439
+2.19780,1.08826,-1.33242,-1.52573,-1.01725,1.94825,-0.31431
+2.25275,1.08839,-1.32982,-1.53012,-1.01542,1.94815,-0.31420
+2.30769,1.08815,-1.32783,-1.53370,-1.01390,1.94831,-0.31435
+2.36264,1.08743,-1.32478,-1.53937,-1.01145,1.94884,-0.31485
+2.41758,1.07796,-1.31761,-1.55757,-1.00287,1.95581,-0.32158
+2.47253,1.07419,-1.31575,-1.56313,-1.00017,1.95859,-0.32429
+2.52747,1.07179,-1.31457,-1.56667,-0.99845,1.96036,-0.32601
+2.58242,1.05474,-1.31213,-1.58178,-0.99044,1.97291,-0.33836
+2.63736,1.04854,-1.31152,-1.58687,-0.98771,1.97746,-0.34289
+2.69231,1.04435,-1.31127,-1.59003,-0.98599,1.98053,-0.34596
+2.74725,1.03792,-1.31135,-1.59415,-0.98365,1.98523,-0.35068
+2.80220,1.02013,-1.31400,-1.60160,-0.97883,1.99823,-0.36389
+2.85714,1.01458,-1.31499,-1.60372,-0.97743,2.00227,-0.36804
+2.91209,1.00724,-1.31672,-1.60583,-0.97585,2.00761,-0.37356
+2.96703,0.99540,-1.32115,-1.60660,-0.97442,2.01619,-0.38253
+3.02198,0.99124,-1.32270,-1.60690,-0.97391,2.01920,-0.38569
+3.07692,0.98823,-1.32397,-1.60688,-0.97364,2.02137,-0.38799
+3.13187,0.98100,-1.32724,-1.60650,-0.97314,2.02660,-0.39353
+3.18681,0.97790,-1.32877,-1.60613,-0.97301,2.02883,-0.39592
+3.24176,0.97470,-1.33047,-1.60554,-0.97297,2.03113,-0.39838
+3.29670,0.97003,-1.33304,-1.60458,-0.97295,2.03449,-0.40200
+3.35165,0.96503,-1.33572,-1.60368,-0.97288,2.03808,-0.40588
+3.40659,0.96402,-1.33627,-1.60348,-0.97287,2.03881,-0.40667
+3.46154,0.95349,-1.34301,-1.59982,-0.97347,2.04635,-0.41490
+3.51648,0.95241,-1.34372,-1.59942,-0.97355,2.04712,-0.41574
+3.57143,0.94928,-1.34585,-1.59813,-0.97381,2.04936,-0.41821
+3.62637,0.94405,-1.34960,-1.59569,-0.97439,2.05309,-0.42233
+3.68132,0.93994,-1.35266,-1.59359,-0.97492,2.05602,-0.42559
+3.73626,0.93789,-1.35422,-1.59249,-0.97521,2.05748,-0.42722
+3.79121,0.93697,-1.35495,-1.59193,-0.97537,2.05813,-0.42795
+3.84615,0.92745,-1.36381,-1.58412,-0.97784,2.06489,-0.43555
+3.90110,0.92454,-1.36693,-1.58103,-0.97890,2.06696,-0.43789
+3.95604,0.91913,-1.37356,-1.57390,-0.98146,2.07079,-0.44227
+4.01099,0.90474,-1.39758,-1.54395,-0.99298,2.08097,-0.45402
+4.06593,0.89803,-1.40912,-1.52935,-0.99871,2.08570,-0.45954
+4.12088,0.87738,-1.45433,-1.46737,-1.02396,2.10019,-0.47682
+4.17582,0.87143,-1.46740,-1.44931,-1.03148,2.10435,-0.48185
+4.23077,0.86544,-1.47993,-1.43221,-1.03861,2.10852,-0.48694
+4.28571,0.84436,-1.52459,-1.37096,-1.06446,2.12310,-0.50505
+4.34066,0.84005,-1.53462,-1.35674,-1.07060,2.12607,-0.50880
+4.39560,0.83705,-1.54126,-1.34746,-1.07461,2.12812,-0.51142
+4.45055,0.83540,-1.54508,-1.34206,-1.07696,2.12926,-0.51287
+4.50549,0.83178,-1.55368,-1.32977,-1.08230,2.13174,-0.51604
+4.56044,0.82471,-1.56996,-1.30674,-1.09233,2.13657,-0.52227
+4.61538,0.82109,-1.57923,-1.29322,-1.09829,2.13904,-0.52548
+4.67033,0.81921,-1.58407,-1.28615,-1.10142,2.14033,-0.52715
+4.72527,0.81762,-1.58858,-1.27940,-1.10442,2.14141,-0.52858
+4.78022,0.81728,-1.58950,-1.27803,-1.10503,2.14164,-0.52888
+4.83516,0.81623,-1.59254,-1.27345,-1.10706,2.14236,-0.52982
+4.89011,0.81621,-1.59259,-1.27338,-1.10710,2.14237,-0.52983
+4.94505,0.81604,-1.59312,-1.27257,-1.10746,2.14248,-0.52999
+5.00000,0.81603,-1.59317,-1.27249,-1.10749,2.14249,-0.53000
+5.05495,0.81585,-1.59380,-1.27152,-1.10793,2.14262,-0.53017
+5.10989,0.81583,-1.59384,-1.27144,-1.10796,2.14263,-0.53018
+5.16484,0.81582,-1.59389,-1.27137,-1.10800,2.14263,-0.53019
+5.21978,0.81582,-1.59393,-1.27130,-1.10803,2.14264,-0.53019
+5.27473,0.81735,-1.59459,-1.26858,-1.10937,2.14161,-0.52887
+5.32967,0.81875,-1.59493,-1.26660,-1.11037,2.14067,-0.52766
+5.38462,0.83542,-1.58930,-1.26107,-1.11395,2.12940,-0.51327
+5.43956,0.84030,-1.58724,-1.26031,-1.11460,2.12608,-0.50910
+5.49451,0.84382,-1.58530,-1.26059,-1.11469,2.12368,-0.50609
+5.54945,0.84977,-1.58167,-1.26178,-1.11452,2.11961,-0.50102
+5.60440,0.86461,-1.57253,-1.26512,-1.11391,2.10941,-0.48848
+5.65934,0.87012,-1.56857,-1.26748,-1.11318,2.10560,-0.48386
+5.71429,0.87556,-1.56470,-1.26980,-1.11246,2.10184,-0.47931
+5.76923,0.88942,-1.55405,-1.27724,-1.10996,2.09219,-0.46780
+5.82418,0.89581,-1.54928,-1.28052,-1.10888,2.08772,-0.46255
+5.87912,0.90221,-1.54459,-1.28369,-1.10786,2.08324,-0.45730
+5.93407,0.91325,-1.53539,-1.29127,-1.10516,2.07548,-0.44831
+5.98901,0.91669,-1.53246,-1.29377,-1.10426,2.07305,-0.44553
+6.04396,0.91965,-1.53016,-1.29553,-1.10367,2.07095,-0.44313
+6.09890,0.92487,-1.52590,-1.29904,-1.10243,2.06726,-0.43892
+6.15385,0.92751,-1.52371,-1.30090,-1.10178,2.06539,-0.43681
+6.20879,0.93014,-1.52149,-1.30283,-1.10109,2.06353,-0.43470
+6.26374,0.93093,-1.52078,-1.30348,-1.10085,2.06297,-0.43407
+6.31868,0.93670,-1.51474,-1.30983,-1.09840,2.05886,-0.42944
+6.37363,0.93759,-1.51377,-1.31088,-1.09799,2.05823,-0.42873
+6.42857,0.93846,-1.51277,-1.31200,-1.09755,2.05761,-0.42804
+6.48352,0.94343,-1.50626,-1.31987,-1.09437,2.05406,-0.42407
+6.53846,0.94797,-1.49828,-1.33073,-1.08984,2.05080,-0.42044
+6.59341,0.94800,-1.49823,-1.33079,-1.08981,2.05079,-0.42042
+6.64835,0.94995,-1.49424,-1.33648,-1.08741,2.04938,-0.41886
+6.70330,0.95449,-1.47425,-1.36889,-1.07332,2.04609,-0.41516
+6.75824,0.95498,-1.47198,-1.37258,-1.07172,2.04573,-0.41475
+6.81319,0.95592,-1.46576,-1.38300,-1.06717,2.04504,-0.41397
+6.86813,0.95629,-1.44199,-1.42504,-1.04869,2.04469,-0.41350
+6.92308,0.95622,-1.43615,-1.43544,-1.04413,2.04472,-0.41350
+6.97802,0.95462,-1.41562,-1.47284,-1.02775,2.04580,-0.41459
+7.03297,0.95413,-1.41007,-1.48298,-1.02332,2.04614,-0.41494
+7.08791,0.95343,-1.40608,-1.49049,-1.02002,2.04662,-0.41545
+7.14286,0.95139,-1.39595,-1.50973,-1.01159,2.04804,-0.41696
+7.19780,0.95019,-1.39000,-1.52101,-1.00668,2.04888,-0.41786
+7.25275,0.94968,-1.38744,-1.52585,-1.00457,2.04923,-0.41824
+7.30769,0.94911,-1.38511,-1.53033,-1.00261,2.04963,-0.41867
+7.36264,0.94845,-1.38288,-1.53470,-1.00070,2.05009,-0.41917
+7.41758,0.94822,-1.38214,-1.53615,-1.00006,2.05026,-0.41934
+7.47253,0.94737,-1.38014,-1.54026,-0.99824,2.05086,-0.41999
+7.52747,0.94705,-1.37942,-1.54175,-0.99759,2.05108,-0.42024
+7.58242,0.94673,-1.37892,-1.54286,-0.99709,2.05131,-0.42048
+7.63736,0.94630,-1.37835,-1.54419,-0.99649,2.05161,-0.42082
+7.69231,0.94374,-1.37639,-1.54954,-0.99400,2.05342,-0.42282
+7.74725,0.94302,-1.37590,-1.55093,-0.99335,2.05394,-0.42338
+7.80220,0.94115,-1.37508,-1.55379,-0.99199,2.05526,-0.42484
+7.85714,0.94092,-1.37500,-1.55411,-0.99183,2.05542,-0.42502
+7.91209,0.93810,-1.37500,-1.55629,-0.99067,2.05742,-0.42724
+7.96703,0.93756,-1.37503,-1.55666,-0.99046,2.05780,-0.42767
+8.02198,0.93577,-1.37530,-1.55760,-0.98992,2.05907,-0.42908
+8.07692,0.92872,-1.37787,-1.55868,-0.98886,2.06406,-0.43468
+8.13187,0.92613,-1.37898,-1.55880,-0.98859,2.06589,-0.43675
+8.18681,0.92339,-1.38030,-1.55868,-0.98841,2.06783,-0.43894
+8.24176,0.91389,-1.38615,-1.55616,-0.98869,2.07453,-0.44658
+8.29670,0.90999,-1.38875,-1.55479,-0.98895,2.07727,-0.44974
+8.35165,0.90710,-1.39077,-1.55362,-0.98922,2.07930,-0.45208
+8.40659,0.89358,-1.40279,-1.54386,-0.99231,2.08878,-0.46314
+8.46154,0.88810,-1.40785,-1.53961,-0.99371,2.09261,-0.46766
+8.51648,0.88376,-1.41230,-1.53548,-0.99515,2.09564,-0.47125
+8.57143,0.88119,-1.41495,-1.53303,-0.99601,2.09743,-0.47339
+8.62637,0.87199,-1.42516,-1.52302,-0.99964,2.10383,-0.48108
+8.68132,0.87114,-1.42612,-1.52208,-0.99998,2.10442,-0.48179
+8.73626,0.86863,-1.42917,-1.51889,-1.00118,2.10616,-0.48390
+8.79121,0.86626,-1.43215,-1.51570,-1.00239,2.10780,-0.48590
+8.84615,0.86526,-1.43354,-1.51413,-1.00300,2.10850,-0.48675
+8.90110,0.86346,-1.43639,-1.51070,-1.00435,2.10975,-0.48827
+8.95604,0.86343,-1.43643,-1.51065,-1.00437,2.10977,-0.48829
+9.01099,0.86113,-1.44156,-1.50364,-1.00724,2.11136,-0.49026
+9.06593,0.86046,-1.44344,-1.50094,-1.00836,2.11183,-0.49083
+9.12088,0.85960,-1.44764,-1.49431,-1.01117,2.11244,-0.49159
+9.17582,0.85930,-1.44994,-1.49053,-1.01279,2.11265,-0.49186
+9.23077,0.85915,-1.45229,-1.48650,-1.01453,2.11276,-0.49200
+9.28571,0.85928,-1.45381,-1.48371,-1.01576,2.11268,-0.49190
+9.34066,0.86640,-1.46518,-1.45740,-1.02771,2.10783,-0.48604
+9.39560,0.86798,-1.46768,-1.45160,-1.03037,2.10675,-0.48475
+9.45055,0.87141,-1.47135,-1.44211,-1.03476,2.10440,-0.48193
+9.50549,0.88524,-1.48141,-1.41237,-1.04880,2.09486,-0.47059
+9.56044,0.89056,-1.48635,-1.39904,-1.05507,2.09118,-0.46626
+9.61538,0.89406,-1.48922,-1.39096,-1.05889,2.08875,-0.46343
+9.67033,0.90127,-1.49475,-1.37501,-1.06647,2.08373,-0.45761
+9.72527,0.90334,-1.49610,-1.37087,-1.06846,2.08229,-0.45594
+9.78022,0.90393,-1.49651,-1.36964,-1.06905,2.08188,-0.45546
+9.83516,0.90469,-1.49697,-1.36818,-1.06976,2.08135,-0.45485
+9.89011,0.90472,-1.49699,-1.36813,-1.06978,2.08133,-0.45483
+9.94505,0.90486,-1.49706,-1.36787,-1.06990,2.08123,-0.45472
+10.00000,0.90489,-1.49708,-1.36782,-1.06993,2.08121,-0.45469
diff --git a/python/examples/path_following_mpc.py b/python/examples/path_following_mpc.py
index 9651e99f1570de9982247652252dd02b50f7f27e..19dd8032ea65be3012ede34278e8c40cce9aac83 100644
--- a/python/examples/path_following_mpc.py
+++ b/python/examples/path_following_mpc.py
@@ -1,10 +1,11 @@
 # PYTHON_ARGCOMPLETE_OK
 import numpy as np
 import time
-import argparse
+import argparse, argcomplete
 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, createCrocoPathFollowingOCP1, solveCrocoOCP
+from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoPathFollowingOCP1, solveCrocoOCP
 from ur_simple_control.optimal_control.crocoddyl_mpc import CrocoPathFollowingMPCControlLoop, CrocoPathFollowingMPC
 from ur_simple_control.basics.basics import followKinematicJointTrajP
 from ur_simple_control.util.logging_utils import LogManager
@@ -12,7 +13,7 @@ from ur_simple_control.visualize.visualize import plotFromDict
 from ur_simple_control.clik.clik import getClikArgs
 import pinocchio as pin
 import crocoddyl
-import mim_solvers
+import importlib.util
 
 
 def get_args():
@@ -26,6 +27,8 @@ def get_args():
 
 if __name__ == "__main__": 
     args = get_args()
+    if importlib.util.find_spec('mim_solvers'):
+        import mim_solvers
     robot = RobotManager(args)
     # TODO: put this back for nicer demos
     #Mgoal = robot.defineGoalPointCLI()
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index 8fb43e1a02c2ae150155d44c6aabe129fe19716a..c1306d726f334da5e341ebffbdd3cae3deb15229 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,173 +1,183 @@
-0.55466,0.55325
-0.55466,0.55325
-0.55466,0.55440
-0.54454,0.56710
-0.53645,0.57518
-0.52634,0.58442
-0.51623,0.59134
-0.51016,0.59711
-0.50612,0.60058
-0.50207,0.60289
-0.49803,0.60404
-0.49398,0.60750
-0.48792,0.60981
-0.48185,0.61328
-0.47578,0.61443
-0.46971,0.61789
-0.45960,0.62136
-0.44949,0.62482
-0.43735,0.62828
-0.42724,0.63059
-0.41713,0.63290
-0.41106,0.63405
-0.40297,0.63521
-0.39488,0.63636
-0.39084,0.63636
-0.38477,0.63636
-0.37870,0.63636
-0.37466,0.63521
-0.36859,0.63405
-0.36252,0.63175
-0.35443,0.62828
-0.34432,0.62597
-0.33825,0.62251
-0.33016,0.62020
-0.32410,0.61789
-0.31803,0.61558
-0.31601,0.61443
-0.31196,0.61212
-0.30792,0.61097
-0.30387,0.60981
-0.29983,0.60750
-0.29578,0.60289
-0.29376,0.60058
-0.29376,0.59942
-0.28972,0.59250
-0.28769,0.58326
-0.28769,0.57633
-0.28769,0.57172
-0.28769,0.56479
-0.28769,0.56248
-0.28972,0.55671
-0.29376,0.54863
-0.29781,0.53824
-0.30387,0.53131
-0.31399,0.52439
-0.32410,0.51746
-0.35443,0.50476
-0.36657,0.50014
-0.38275,0.49668
-0.39488,0.49322
-0.41511,0.48745
-0.42724,0.48398
-0.43735,0.48052
-0.44342,0.47821
-0.44949,0.47590
-0.45556,0.47244
-0.47174,0.45974
-0.47983,0.45281
-0.48792,0.44589
-0.49398,0.43665
-0.50005,0.42973
-0.50612,0.42280
-0.51016,0.41818
-0.51219,0.41587
-0.51219,0.41356
-0.51219,0.41241
-0.51219,0.41126
-0.51219,0.41010
-0.51219,0.40895
-0.51219,0.40779
-0.51219,0.40664
-0.51219,0.40664
-0.51219,0.40664
-0.51421,0.40779
-0.52634,0.41587
-0.53443,0.42280
-0.54454,0.42973
-0.56072,0.44012
-0.56881,0.44704
-0.58095,0.45512
-0.58904,0.46205
-0.60117,0.46898
-0.61331,0.47590
-0.62544,0.48167
-0.63555,0.48629
-0.64364,0.48975
-0.64971,0.49322
-0.66185,0.50130
-0.67196,0.50938
-0.68409,0.51861
-0.69218,0.52439
-0.70634,0.53131
-0.72454,0.54286
-0.73263,0.54747
-0.74274,0.55440
-0.75083,0.56133
-0.75892,0.56941
-0.76701,0.57633
-0.77106,0.58442
-0.77713,0.59481
-0.77915,0.59942
-0.77915,0.60058
-0.77915,0.60173
-0.77915,0.60289
-0.77915,0.60404
-0.77915,0.60635
-0.77713,0.60866
-0.77308,0.61097
-0.76904,0.61212
-0.75892,0.61558
-0.74274,0.62020
-0.72859,0.62367
-0.71039,0.62713
-0.70230,0.62828
-0.69218,0.62828
-0.67803,0.62828
-0.66387,0.62713
-0.65376,0.62713
-0.64364,0.62713
-0.63555,0.62713
-0.62949,0.62713
-0.62746,0.62597
-0.62342,0.62597
-0.61938,0.62482
-0.61533,0.62367
-0.61331,0.62367
-0.60926,0.62367
-0.60724,0.62251
-0.60320,0.62136
-0.59915,0.62020
-0.59308,0.61905
-0.58904,0.61789
-0.58297,0.61558
-0.57690,0.61443
-0.57286,0.61328
-0.56881,0.61212
-0.56881,0.61212
-0.56679,0.61212
-0.56477,0.61212
-0.56275,0.61212
-0.56072,0.61097
-0.55870,0.60981
-0.55870,0.60866
-0.55668,0.60404
-0.55668,0.60173
-0.55466,0.59942
-0.55466,0.59596
-0.55466,0.59250
-0.55466,0.58903
-0.55466,0.58672
-0.55466,0.58442
-0.55466,0.58211
-0.55466,0.57864
-0.55466,0.57633
-0.55466,0.57518
-0.55466,0.57403
-0.55466,0.57287
-0.55466,0.57172
-0.55466,0.57056
-0.55466,0.56941
-0.55466,0.56825
-0.55466,0.56710
-0.55466,0.56710
+0.48757,0.61206
+0.48757,0.61206
+0.48609,0.61358
+0.48461,0.61510
+0.48017,0.61662
+0.47869,0.62117
+0.46832,0.62573
+0.46239,0.63029
+0.44461,0.64548
+0.44017,0.64700
+0.43424,0.65307
+0.42832,0.65915
+0.40758,0.67130
+0.39869,0.67434
+0.39276,0.68041
+0.35869,0.69256
+0.34980,0.69560
+0.33943,0.69864
+0.31128,0.70472
+0.30684,0.70472
+0.30388,0.70472
+0.30240,0.70472
+0.28610,0.70472
+0.28610,0.70472
+0.28314,0.70168
+0.27869,0.70016
+0.27425,0.69712
+0.27425,0.69256
+0.27277,0.69105
+0.27129,0.68953
+0.27129,0.68497
+0.25647,0.66522
+0.25351,0.65459
+0.24906,0.65155
+0.24610,0.64244
+0.22981,0.61510
+0.22684,0.59991
+0.22388,0.59535
+0.22388,0.59383
+0.22388,0.58928
+0.22388,0.58776
+0.22388,0.58320
+0.22684,0.57864
+0.22832,0.57257
+0.24610,0.55130
+0.25203,0.54523
+0.25351,0.54067
+0.27425,0.52092
+0.28017,0.51333
+0.28462,0.50877
+0.29203,0.50270
+0.31277,0.48903
+0.31869,0.48447
+0.32758,0.47991
+0.34388,0.47687
+0.34832,0.47384
+0.35276,0.47384
+0.36165,0.47080
+0.36610,0.47080
+0.37054,0.47080
+0.37647,0.46928
+0.38239,0.46624
+0.38387,0.46624
+0.39869,0.46624
+0.40017,0.46624
+0.40461,0.46624
+0.41202,0.46624
+0.41795,0.46624
+0.42091,0.46624
+0.42239,0.46776
+0.43721,0.47232
+0.44165,0.47687
+0.45054,0.48295
+0.47869,0.50573
+0.49350,0.51333
+0.54535,0.55282
+0.56165,0.56194
+0.57794,0.56953
+0.63424,0.59839
+0.64609,0.60598
+0.65498,0.60902
+0.65942,0.61206
+0.66979,0.61814
+0.69053,0.62725
+0.70090,0.63484
+0.70683,0.63788
+0.71127,0.64244
+0.71275,0.64244
+0.71571,0.64548
+0.71571,0.64548
+0.71571,0.64700
+0.71571,0.64700
+0.71571,0.64852
+0.71423,0.64852
+0.71275,0.65003
+0.71127,0.65155
+0.67720,0.66370
+0.67275,0.66522
+0.63868,0.67738
+0.62979,0.68041
+0.62238,0.68041
+0.61201,0.68345
+0.58831,0.69408
+0.57794,0.69408
+0.56905,0.69712
+0.54535,0.70168
+0.53498,0.70472
+0.52461,0.70775
+0.50535,0.70775
+0.49943,0.70775
+0.49498,0.71079
+0.48609,0.71079
+0.48165,0.71079
+0.47720,0.71079
+0.47572,0.70927
+0.46535,0.70472
+0.46387,0.70320
+0.46239,0.70168
+0.45350,0.69560
+0.44609,0.68345
+0.44609,0.68345
+0.44313,0.67738
+0.43276,0.64700
+0.43128,0.64396
+0.42832,0.63484
+0.41647,0.59839
+0.41350,0.58928
+0.40313,0.55586
+0.40017,0.54675
+0.40017,0.53915
+0.39573,0.52092
+0.39276,0.51029
+0.39128,0.50573
+0.39128,0.50118
+0.39128,0.49662
+0.39128,0.49510
+0.39276,0.49054
+0.39276,0.48903
+0.39573,0.48751
+0.39721,0.48599
+0.40313,0.47991
+0.40461,0.47839
+0.40906,0.47536
+0.41054,0.47536
+0.42091,0.47536
+0.42239,0.47536
+0.42684,0.47536
+0.44017,0.47536
+0.44461,0.47536
+0.44906,0.47536
+0.46387,0.47536
+0.46980,0.47536
+0.47424,0.47536
+0.49646,0.47991
+0.50535,0.47991
+0.51276,0.48295
+0.51720,0.48295
+0.53350,0.48599
+0.53498,0.48599
+0.53942,0.48903
+0.54387,0.49054
+0.54535,0.49358
+0.54831,0.49814
+0.54831,0.49814
+0.55128,0.50877
+0.55128,0.51333
+0.55128,0.52244
+0.55128,0.52700
+0.55128,0.53156
+0.54831,0.53611
+0.53942,0.56649
+0.53942,0.57257
+0.53498,0.58320
+0.52461,0.61814
+0.52461,0.63333
+0.52165,0.64244
+0.51720,0.66067
+0.51424,0.66522
+0.51424,0.66674
+0.51276,0.66826
+0.51276,0.66826
+0.51128,0.66826
+0.51128,0.66826
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index f74d281c0b1c18985912c21b5766180e6ed6e926..8aac4dcf219d27bcd32fee4d77ef29b50de1926c 100644
Binary files a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc and b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc differ
diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index 5ec984d5112bf771de1f6fb5ae47411e9d49a5fb..d402bf8a3603cb0f71e52f26ef2d71ebe89e279c 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -301,9 +301,9 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
     # get actual velocity, not the commanded one.
     # although that would be fun to compare i guess
     # TODO: have both, but call the compute control signal like it should be
-    log_item['dqs'] = robot.getQd().reshape((robot.model.nq,))
+    log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
     log_item['wrench'] = wrench.reshape((6,))
-    log_item['tau'] = tau.reshape((robot.model.nq,))
+    log_item['tau'] = tau.reshape((robot.model.nv,))
     # we're not saving here, but need to respect the API, 
     # hence the empty dict
     return breakFlag, save_past_dict, log_item
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 28f10d365be04af229c15804e72a1f71e7fb3e3e..df84f3b78a6b1eed963d92ba0d36b78d61c7811b 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1035,7 +1035,7 @@ class ProcessManager:
     def getData(self):
         if not self.data_queue.empty():
             self.lastest_data = self.data_queue.get_nowait()
-        return self.lastest_data.copy()
+        return copy.deepcopy(self.lastest_data)
 
     def terminateProcess(self):
         if self.args.debug_prints:
diff --git a/python/ur_simple_control/optimal_control/__init__.py b/python/ur_simple_control/optimal_control/__init__.py
index 255cf7ba008b49eb9d2b4845b1586d89ca576f96..569adb6a237941a26870d3ebd68f85937dacedf2 100644
--- a/python/ur_simple_control/optimal_control/__init__.py
+++ b/python/ur_simple_control/optimal_control/__init__.py
@@ -1,4 +1,6 @@
-from .create_pinocchio_casadi_ocp import *
+import importlib.util
+if importlib.util.find_spec('casadi'):
+    from .create_pinocchio_casadi_ocp import *
 from .crocoddyl_mpc import *
 from .crocoddyl_optimal_control import *
 from .get_ocp_args import *
diff --git a/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
index a11cf65d5106aecdea4b8e4244e724196a4aa265..be9364a9c2fc6fb82e42c56bd4f6ad00f3db643f 100644
--- a/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
+++ b/python/ur_simple_control/optimal_control/create_pinocchio_casadi_ocp.py
@@ -1,5 +1,8 @@
 import casadi
 import pinocchio as pin
+if int(pin.__version__[0]) < 3:
+    print("you need to have pinocchio version 3.0.0 or greater to use pinocchio.casadi!")
+    exit()
 from pinocchio import casadi as cpin
 import numpy as np
 from ur_simple_control.managers import RobotManager, ControlLoopManager
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index abad479ee42044422e4da9ac11e249ceed9e5b9f..2d36c726052bb28e40821619279bedc6d3ca060f 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -2,9 +2,11 @@ from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager,
 from ur_simple_control.optimal_control.crocoddyl_optimal_control import createCrocoIKOCP, createCrocoPathFollowingOCP1
 import pinocchio as pin
 import crocoddyl
-import mim_solvers
 import numpy as np
 from functools import partial
+import importlib.util
+if importlib.util.find_spec('mim_solvers'):
+    import mim_solvers
 
 
 
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 0b9a45d15f7a22acfca23c17e7156e63ca7ac1c2..f1856a434cc7802c4ce2c11bd4d8976dadecf51b 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -1,13 +1,15 @@
 import numpy as np
 import pinocchio as pin
 import crocoddyl
-import mim_solvers
 from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
 import argparse
 from ur_simple_control.basics.basics import followKinematicJointTrajP
 from ur_simple_control.visualize.visualize import plotFromDict
 import example_robot_data
 import time
+import importlib.util
+if importlib.util.find_spec('mim_solvers'):
+    import mim_solvers
 
 def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
     # create torque bounds which correspond to percentage
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 da6b4e16226e0ccf7592148fce5af4781ca85e75..d892f4bb1b092ff28f489a8358275af98997021d 100644
Binary files a/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc and b/python/ur_simple_control/visualize/__pycache__/visualize.cpython-312.pyc differ
diff --git a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
index 064d192f8de93a3156ae6978bce6b2ce1984b66a..11f018e2d1b079da3608245550520bd8ee8d03a3 100644
--- a/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
+++ b/python/ur_simple_control/visualize/manipulator_comparison_visualizer.py
@@ -4,9 +4,10 @@ import os
 import time
 from itertools import zip_longest
 from multiprocessing import Process, Queue
-from ur_simple_control.managers import getMinimalArgParser, RobotManager
+from ur_simple_control.managers import getMinimalArgParser, RobotManager, ProcessManager
 from ur_simple_control.util.logging_utils import LogManager
 from ur_simple_control.visualize.visualize import manipulatorComparisonVisualizer, logPlotter
+from functools import partial
 
 def getLogComparisonArgs():
     parser = getMinimalArgParser()
@@ -24,6 +25,7 @@ def getLogComparisonArgs():
     args.simulation = False
     return args
 
+# TODO: use ProcessManager for this
 class ManipulatorComparisonManager:
     def __init__(self, args):
         self.args = args
@@ -53,27 +55,16 @@ class ManipulatorComparisonManager:
             print("you did not give me a valid path for log2, exiting")
             exit()
 
-        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_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_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")
+        side_function = partial(manipulatorComparisonVisualizer, self.robot1.model, 
+                                self.robot1.visual_model, self.robot1.collision_model)
+        cmd = (np.zeros(self.robot1.model.nq), np.ones(self.robot2.model.nq))
+        self.visualizer_manager = ProcessManager(args, side_function,
+                                                 cmd, 2)
         # 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()
+        self.visualizer_manager.getData()
 
         ###########################################
         #  in case you will want log plotters too  #
@@ -89,24 +80,20 @@ class ManipulatorComparisonManager:
         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()
+            side_function = partial(logPlotter, log)
+            self.plotter_manager = ProcessManager(args, side_function, None, 2)
+            self.plotter_manager.getData()
 
     def createRunningPlot(self, log, log_plotter_time_start, log_plotter_time_stop):
         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))
+        self.visualizer_manager.sendCommand((q1, q2))
         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()
+                log_plotter.plotter_manager.sendCommand(time_index - log_plotter.time_start)
+                log_plotter.plotter_manager.getData()
+        self.visualizer_manager.getData()
 
     # 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
@@ -142,6 +129,7 @@ class ManipulatorComparisonManager:
 
 
 
+# TODO: update
 if __name__ == "__main__":
     args = getLogComparisonArgs()
     cmp_manager = ManipulatorComparisonManager(args)
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index f39cf81bca10106a1b469068e91dbf4aaef24b42..fc25e69be117a284890e7a94ae474e6890d56f33 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -6,6 +6,7 @@ import copy
 #from pinocchio.visualize import MeshcatVisualizer
 # TODO: use wrapped meshcat visualizer to have access to more nifty plotting
 from ur_simple_control.visualize.meshcat_viewer_wrapper.visualizer import MeshcatVisualizer
+from pinocchio.visualize import MeshcatVisualizer as UnWrappedMeshcat
 import meshcat_shapes
 
 # tkinter stuff for later reference
@@ -200,11 +201,13 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, 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, cmd_queue, ack_queue):
+def manipulatorComparisonVisualizer(model, collision_model, visual_model, args, cmd, cmd_queue, ack_queue):
     for geom in visual_model.geometryObjects:
         if "hand" in geom.name:
             geom.meshColor = np.array([0.2,0.2,0.2,0.2])
-    viz = MeshcatVisualizer(model, collision_model, visual_model)
+    viz = MeshcatVisualizer(model=model, 
+                            collision_model=collision_model, 
+                            visual_model=visual_model)
     viz.initViewer(open=True)
     # load the first one
     viz.loadViewerModel(collision_color=[0.2,0.2,0.2,0.6])
@@ -245,7 +248,9 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model,
 
 
     # other robot display
-    viz2 = MeshcatVisualizer(model, collision_model, visual_model)
+    viz2 = UnWrappedMeshcat(model=model, 
+                             collision_model=collision_model, 
+                             visual_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))
@@ -293,7 +298,7 @@ def manipulatorComparisonVisualizer(args, model, collision_model, visual_model,
 # so no cigar from that.
 # NOTE this is being tried out in realtimelogplotter branch
 # there is no successful re-drawing here.
-def logPlotter(args, log, cmd_queue, ack_queue):
+def logPlotter(log, args, cmd, cmd_queue, ack_queue):
     """
     logPlotter
     ---------------