From 20b5035e5a8ccdf6a5c1580e68cfe3da36af5ef5 Mon Sep 17 00:00:00 2001
From: m-guberina <gubi.guberina@gmail.com>
Date: Wed, 20 Nov 2024 15:57:18 +0100
Subject: [PATCH] robotweek state end of second robotlab day

---
 Dockerfile                                    |   2 +-
 .../casadi_ocp_collision_avoidance.py         |   2 +
 python/examples/clik.py                       |   5 +-
 python/examples/crocoddyl_mpc.py              |   2 +
 python/examples/crocoddyl_ocp_clik.py         |   2 +
 python/examples/drawing_from_input_drawing.py | 134 ++++----
 python/examples/joint_trajectory.csv          | 292 ++++++++++++------
 python/examples/log_analysis_example.py       |  10 +-
 python/examples/path_following_mpc.py         |   2 +
 python/examples/path_in_pixels.csv            | 292 ++++++++++++------
 python/examples/point_impedance_control.py    |   6 +-
 .../__pycache__/managers.cpython-312.pyc      | Bin 47803 -> 48338 bytes
 python/ur_simple_control/clik/clik.py         |  19 +-
 .../dmp/__pycache__/dmp.cpython-312.pyc       | Bin 16312 -> 17324 bytes
 python/ur_simple_control/dmp/dmp.py           |  24 ++
 python/ur_simple_control/managers.py          |  45 ++-
 .../__pycache__/get_model.cpython-312.pyc     | Bin 8617 -> 10863 bytes
 .../__pycache__/logging_utils.cpython-312.pyc | Bin 6982 -> 7005 bytes
 python/ur_simple_control/util/get_model.py    |  34 +-
 .../ur_simple_control/util/logging_utils.py   |   2 +-
 .../__pycache__/visualize.cpython-312.pyc     | Bin 16836 -> 16912 bytes
 .../{ => obsolete_to_be_removed}/main.py      |   0
 .../{ => obsolete_to_be_removed}/make_run.py  |   0
 .../ur_simple_control/visualize/visualize.py  |   3 +-
 24 files changed, 602 insertions(+), 274 deletions(-)
 rename python/ur_simple_control/visualize/{ => obsolete_to_be_removed}/main.py (100%)
 rename python/ur_simple_control/visualize/{ => obsolete_to_be_removed}/make_run.py (100%)

diff --git a/Dockerfile b/Dockerfile
index ac05d11..5939dbe 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -76,5 +76,5 @@ 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 -c conda-forge
-RUN pip install matplotlib meshcat ur_rtde \
+RUN pip install matplotlib meshcat ur_rtde argcomplete \
                 qpsolvers ecos example_robot_data meshcat_shapes
diff --git a/python/examples/casadi_ocp_collision_avoidance.py b/python/examples/casadi_ocp_collision_avoidance.py
index cc84856..dd2ef0b 100644
--- a/python/examples/casadi_ocp_collision_avoidance.py
+++ b/python/examples/casadi_ocp_collision_avoidance.py
@@ -1,3 +1,4 @@
+# PYTHON_ARGCOMPLETE_OK
 import pinocchio as pin
 import numpy as np
 import time
@@ -10,6 +11,7 @@ def get_args():
     parser = getMinimalArgParser()
     parser.description = 'optimal control in pinocchio.casadi for obstacle avoidance'
     # add more arguments here from different Simple Manipulator Control modules
+    argcomplete.autocomplete(parser)
     args = parser.parse_args()
     return args
 
diff --git a/python/examples/clik.py b/python/examples/clik.py
index 99867e0..0ccf6c8 100644
--- a/python/examples/clik.py
+++ b/python/examples/clik.py
@@ -1,15 +1,18 @@
+# PYTHON_ARGCOMPLETE_OK
 import numpy as np
 import time
-import argparse
+import argcomplete, 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
 
+
 def get_args():
     parser = getMinimalArgParser()
     parser.description = 'Run closed loop inverse kinematics \
     of various kinds. Make sure you know what the goal is before you run!'
     parser = getClikArgs(parser)
+    argcomplete.autocomplete(parser)
     args = parser.parse_args()
     return args
 
diff --git a/python/examples/crocoddyl_mpc.py b/python/examples/crocoddyl_mpc.py
index ce9bd05..556f71a 100644
--- a/python/examples/crocoddyl_mpc.py
+++ b/python/examples/crocoddyl_mpc.py
@@ -1,3 +1,4 @@
+# PYTHON_ARGCOMPLETE_OK
 import numpy as np
 import time
 import argparse
@@ -19,6 +20,7 @@ def get_args():
     parser = getMinimalArgParser()
     parser = get_OCP_args(parser)
     parser = getClikArgs(parser) # literally just for goal error
+    argcomplete.autocomplete(parser)
     args = parser.parse_args()
     return args
 
diff --git a/python/examples/crocoddyl_ocp_clik.py b/python/examples/crocoddyl_ocp_clik.py
index 1b7ece6..c062fce 100644
--- a/python/examples/crocoddyl_ocp_clik.py
+++ b/python/examples/crocoddyl_ocp_clik.py
@@ -1,3 +1,4 @@
+# PYTHON_ARGCOMPLETE_OK
 import numpy as np
 import time
 import argparse
@@ -15,6 +16,7 @@ import crocoddyl
 def get_args():
     parser = getMinimalArgParser()
     parser = get_OCP_args(parser)
+    argcomplete.autocomplete(parser)
     args = parser.parse_args()
     return args
 
diff --git a/python/examples/drawing_from_input_drawing.py b/python/examples/drawing_from_input_drawing.py
index a9240db..083c451 100644
--- a/python/examples/drawing_from_input_drawing.py
+++ b/python/examples/drawing_from_input_drawing.py
@@ -1,9 +1,10 @@
+# PYTHON_ARGCOMPLETE_OK
 import pinocchio as pin
 import numpy as np
 import matplotlib.pyplot as plt
 import matplotlib
 import copy
-import argparse
+import argparse, argcomplete
 import time
 import pickle
 from functools import partial
@@ -48,6 +49,7 @@ def getArgs():
     parser.add_argument('--board-wiping', action=argparse.BooleanOptionalAction, \
             help="are you wiping the board (default is no because you're writing)", \
             default=False)
+    argcomplete.autocomplete(parser)
     args = parser.parse_args()
     if args.gripper and args.simulation:
         raise NotImplementedError('Did not figure out how to put the gripper in \
@@ -72,55 +74,70 @@ q: -0.5586 -2.3103 -1.1638 -1.2468 1.6492 0.262 0.0 0.0
 
 """
 def getMarker(args, robot, plane_pose):
-    # init position is the safe one above starting point of the board
-    above_starting_write_point = pin.SE3.Identity()
-    # start 20cm away from the board
-    above_starting_write_point.translation[2] = -0.2
-    above_starting_write_point = plane_pose.act(above_starting_write_point)
-    compliantMoveL(args, robot, above_starting_write_point)
-    Tinit = robot.getT_w_e().copy()
-    q_init = robot.getQ()
-    #exit()
-    #moveJ(args, robot, q_init)
-    compliantMoveL(args, robot, above_starting_write_point)
-    print("above_starting_write_point", above_starting_write_point)
-    # goal position, predefined
-    Tgoal = pin.SE3()
-    Tgoal.rotation = np.array([
-        [ -0.73032, -0.682357, 0.0319574],
-        [-0.679774,  0.730578, 0.0645244],
-        [-0.067376, 0.0253997, -0.997404]])
-    Tgoal.translation = np.array([0.574534, -0.508343,  0.249325])
-
-    # slighly up from goal to not hit marker weirdly
-    TgoalUP = Tgoal.copy() 
-    TgoalUP.translation += np.array([0,0,0.2])
-    #TgoalUP.translation += np.array([0,0,0.3])
-
-
-    print("going to above marker")
-    robot.Mgoal = TgoalUP.copy()
-    compliantMoveL(args, robot, TgoalUP.copy())
-    #moveL(args, robot, TgoalUP.copy())
-    print("going down to marker")
-    robot.Mgoal = Tgoal.copy()
-    compliantMoveL(args, robot, Tgoal.copy())
-    #moveL(args, robot, Tgoal.copy())
-    print("picking up marker")
+    # load traj
+    file = open('./data/from_writing_to_handover.pickle', 'rb')
+    point_dict = pickle.load(file)
+    file.close()
+    dmp_forward = DMP(point_dict['qs'])
+    #for q_ref in point_dict['qs']:
+    #    moveJPI(args, robot, q_ref[:6])
+    robot.openGripper()
+    time.sleep(5)
     robot.closeGripper()
-    time.sleep(2)
-    print("going up")
-    robot.Mgoal = TgoalUP.copy()
-    compliantMoveL(args, robot, TgoalUP.copy())
-    #moveL(args, robot, TgoalUP.copy())
-    print("going back")
-    # TODO: this HAS to be a movej
-    # in fact all of them should be
-    robot.Mgoal = Tinit.copy()
-    #compliantMoveL(args, robot, Tinit.copy())
-    moveJPI(args, robot, q_init)
-    #moveL(args, robot, Tinit.copy())
-    print("got back")
+    point_dict['qs'].reverse()
+    dmp_back = DMP(point_dict['qs'])
+    #for q_ref in point_dict['qs']:
+    #    moveJPI(args, robot, q_ref[:6])
+        #    # init position is the safe one above starting point of the board
+        #    above_starting_write_point = pin.SE3.Identity()
+        #    # start 20cm away from the board
+        #    above_starting_write_point.translation[2] = -0.2
+        #    above_starting_write_point = plane_pose.act(above_starting_write_point)
+        #    compliantMoveL(args, robot, above_starting_write_point)
+        #    Tinit = robot.getT_w_e().copy()
+        #    q_init = robot.getQ()
+        #    #exit()
+        #    #moveJ(args, robot, q_init)
+        #    compliantMoveL(args, robot, above_starting_write_point)
+        #    print("above_starting_write_point", above_starting_write_point)
+        #    # goal position, predefined
+        #    Tgoal = pin.SE3()
+        #    Tgoal.rotation = np.array([
+        #        [ -0.73032, -0.682357, 0.0319574],
+        #        [-0.679774,  0.730578, 0.0645244],
+        #        [-0.067376, 0.0253997, -0.997404]])
+        #    Tgoal.translation = np.array([0.574534, -0.508343,  0.249325])
+        #
+        #    # slighly up from goal to not hit marker weirdly
+        #    TgoalUP = Tgoal.copy() 
+        #    TgoalUP.translation += np.array([0,0,0.2])
+        #    #TgoalUP.translation += np.array([0,0,0.3])
+        #
+        #
+        #    print("going to above marker")
+        #    robot.Mgoal = TgoalUP.copy()
+        #    compliantMoveL(args, robot, TgoalUP.copy())
+        #    #moveL(args, robot, TgoalUP.copy())
+        #    print("going down to marker")
+        #    robot.Mgoal = Tgoal.copy()
+        #    compliantMoveL(args, robot, Tgoal.copy())
+        #    #moveL(args, robot, Tgoal.copy())
+        #    print("picking up marker")
+        #    robot.closeGripper()
+        #    time.sleep(2)
+        #    print("going up")
+        #    robot.Mgoal = TgoalUP.copy()
+        #    compliantMoveL(args, robot, TgoalUP.copy())
+        #    #moveL(args, robot, TgoalUP.copy())
+        #    print("going back")
+        #    # TODO: this HAS to be a movej
+        #    # in fact all of them should be
+        #    robot.Mgoal = Tinit.copy()
+        #    #compliantMoveL(args, robot, Tinit.copy())
+        #    moveJPI(args, robot, q_init)
+        #    #moveL(args, robot, Tinit.copy())
+        #    print("got back")
+
 
 def findMarkerOffset(args, robot : RobotManager, plane_pose : pin.SE3):
     """
@@ -169,7 +186,7 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
     dmp.set_tau(tau_dmp)
     q = robot.getQ()
     T_w_e = robot.getT_w_e()
-    Z = np.diag(np.array([0.0, 0.0, 1.0, 0.5, 0.5, 0.5]))
+    Z = np.diag(np.array([0.0, 0.0, 1.0, 0.5, 0.5, 0.0]))
 
     wrench = robot.getWrench()
     save_past_dict['wrench'] = wrench.copy()
@@ -182,7 +199,6 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
     wrench = args.beta * wrench + (1 - args.beta) * np.average(np.array(past_data['wrench']), axis=0)
 
     wrench = Z @ wrench
-    #J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
     J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
     dq = robot.getQd()[:6].reshape((6,1))
     # get joint 
@@ -218,7 +234,7 @@ def controlLoopWriting(args, robot : RobotManager, dmp, tc, i, past_data):
 
 def write(args, robot : RobotManager, joint_trajectory):
     # create DMP based on the trajectory
-    dmp = DMP(joint_trajectory)
+    dmp = DMP(joint_trajectory, a_s=1.0)
     if not args.temporal_coupling:
         tc = NoTC()
     else:
@@ -275,13 +291,15 @@ if __name__ == "__main__":
     #                           software setup                            #
     #######################################################################
     args = getArgs()
-    assert args.mm_into_board > 0.0 and args.mm_into_board < 5.0
+    #assert args.mm_into_board > 0.0 and args.mm_into_board < 5.0
     args.mm_into_board = args.mm_into_board / 1000
     print(args)
     clikController = getClikController(args)
     robot = RobotManager(args)
     if args.pinocchio_only:
-        robot.q = np.array([ 1.32, -1.40, -1.27, -1.157, 1.76, -0.238, 0.0, 0.0 ]) + np.random.random(8) * 0.1
+        rand_pertb = np.zeros(8)
+        rand_pertb[:6] = np.random.random(6) * 0.1
+        robot.q = np.array([ 1.32, -1.40, -1.27, -1.157, 1.76, -0.238, 0.0, 0.0 ]) + rand_pertb
         robot._step()
 
     #######################################################################
@@ -310,13 +328,17 @@ if __name__ == "__main__":
         file.close()
         plane_pose = plane_calib_dict['plane_top_left_pose']
         q_init = plane_calib_dict['q_init']
+        # stupid fix dw about this it's supposed to be 0.0 on the gripper
+        # because we don't use that actually
+        q_init[-1] = 0.0
+        q_init[-2] = 0.0
 
     # make the path 3D
     path_points_3D = map2DPathTo3DPlane(pixel_path, args.board_width, args.board_height)
     # TODO: fix thi function
     if args.pick_up_marker:
-        raise NotImplementedError("sorry")
-        getMarker(args, robot, marker_bottom_pose)
+        #raise NotImplementedError("sorry")
+        getMarker(args, robot, None)
 
     # marker moves between runs, i change markers etc,
     # this goes to start, goes down to touch the board - that's the marker
diff --git a/python/examples/joint_trajectory.csv b/python/examples/joint_trajectory.csv
index 90d134c..9c1585c 100644
--- a/python/examples/joint_trajectory.csv
+++ b/python/examples/joint_trajectory.csv
@@ -1,88 +1,204 @@
-0.00000,1.20178,-1.41060,-1.33105,-1.11327,1.86782,-0.23666
-0.11494,1.20175,-1.41059,-1.33108,-1.11326,1.86783,-0.23668
-0.22989,1.20172,-1.41058,-1.33110,-1.11324,1.86784,-0.23669
-0.34483,1.20170,-1.41056,-1.33113,-1.11323,1.86785,-0.23671
-0.45977,1.20139,-1.41056,-1.33118,-1.11316,1.86801,-0.23691
-0.57471,1.14847,-1.43710,-1.30858,-1.11705,1.90490,-0.27292
-0.68966,1.11306,-1.45819,-1.29040,-1.12150,1.93063,-0.29768
-0.80460,1.06526,-1.49092,-1.26088,-1.13029,1.96580,-0.33204
-0.91954,1.02978,-1.51889,-1.23371,-1.13958,1.99182,-0.35824
-1.03448,0.94706,-1.59438,-1.15607,-1.16900,2.05165,-0.42188
-1.14943,0.86796,-1.68166,-1.05970,-1.20921,2.10733,-0.48659
-1.26437,0.86399,-1.68647,-1.05417,-1.21164,2.11008,-0.48996
-1.37931,0.86106,-1.69011,-1.04994,-1.21349,2.11211,-0.49245
-1.49425,0.85906,-1.69251,-1.04722,-1.21469,2.11349,-0.49415
-1.60920,0.85799,-1.69374,-1.04587,-1.21528,2.11423,-0.49506
-1.72414,0.85595,-1.69602,-1.04341,-1.21634,2.11564,-0.49680
-1.83908,0.85373,-1.69832,-1.04108,-1.21734,2.11716,-0.49870
-1.95402,0.84988,-1.70160,-1.03843,-1.21841,2.11981,-0.50199
-2.06897,0.84767,-1.70208,-1.03954,-1.21780,2.12132,-0.50386
-2.18391,0.84693,-1.68337,-1.07525,-1.20110,2.12180,-0.50436
-2.29885,0.84908,-1.66678,-1.10429,-1.18766,2.12029,-0.50240
-2.41379,0.86246,-1.62010,-1.17900,-1.15368,2.11101,-0.49074
-2.52874,0.88257,-1.56432,-1.26407,-1.11577,2.09697,-0.47356
-2.64368,0.89362,-1.53460,-1.30887,-1.09609,2.08920,-0.46423
-2.75862,0.91309,-1.46684,-1.41464,-1.05020,2.07538,-0.44791
-2.87356,0.91885,-1.45127,-1.43772,-1.04043,2.07128,-0.44315
-2.98851,0.93546,-1.39590,-1.52183,-1.00530,2.05938,-0.42951
-3.10345,0.93875,-1.38488,-1.53846,-0.99845,2.05701,-0.42683
-3.21839,0.94620,-1.35924,-1.57698,-0.98280,2.05164,-0.42078
-3.33333,0.95208,-1.34205,-1.60196,-0.97286,2.04741,-0.41606
-3.44828,0.95494,-1.33386,-1.61376,-0.96821,2.04535,-0.41377
-3.56322,0.95706,-1.32792,-1.62227,-0.96488,2.04381,-0.41207
-3.67816,0.95831,-1.32475,-1.62672,-0.96316,2.04291,-0.41108
-3.79310,0.95895,-1.32319,-1.62889,-0.96232,2.04245,-0.41057
-3.90805,0.95964,-1.32157,-1.63112,-0.96147,2.04195,-0.41003
-4.02299,0.96035,-1.31995,-1.63333,-0.96062,2.04144,-0.40946
-4.13793,0.96084,-1.31894,-1.63468,-0.96011,2.04109,-0.40908
-4.25287,0.96166,-1.31727,-1.63691,-0.95927,2.04050,-0.40843
-4.36782,0.96164,-1.31727,-1.63691,-0.95926,2.04051,-0.40844
-4.48276,0.96161,-1.31730,-1.63688,-0.95927,2.04053,-0.40847
-4.59770,0.91620,-1.37147,-1.57978,-0.97878,2.07285,-0.44460
-4.71264,0.90509,-1.38474,-1.56583,-0.98377,2.08069,-0.45364
-4.82759,0.87952,-1.41497,-1.53452,-0.99516,2.09858,-0.47475
-4.94253,0.85807,-1.44115,-1.50713,-1.00546,2.11345,-0.49280
-5.05747,0.84749,-1.45377,-1.49423,-1.01037,2.12073,-0.50183
-5.17241,0.82776,-1.47792,-1.46936,-1.01997,2.13422,-0.51887
-5.28736,0.81754,-1.49050,-1.45648,-1.02502,2.14116,-0.52781
-5.40230,0.80945,-1.50051,-1.44624,-1.02907,2.14662,-0.53494
-5.51724,0.80645,-1.50427,-1.44238,-1.03061,2.14864,-0.53761
-5.63218,0.80446,-1.50676,-1.43982,-1.03163,2.14998,-0.53937
-5.74713,0.80346,-1.50801,-1.43853,-1.03215,2.15065,-0.54026
-5.86207,0.80343,-1.50805,-1.43849,-1.03216,2.15067,-0.54029
-5.97701,0.80247,-1.50927,-1.43724,-1.03266,2.15132,-0.54115
-6.09195,0.80147,-1.51052,-1.43595,-1.03318,2.15199,-0.54203
-6.20690,0.79945,-1.51296,-1.43352,-1.03415,2.15334,-0.54383
-6.32184,0.79880,-1.51327,-1.43359,-1.03409,2.15378,-0.54441
-6.43678,0.79553,-1.51067,-1.44127,-1.03058,2.15595,-0.54729
-6.55172,0.79358,-1.49913,-1.46348,-1.02084,2.15721,-0.54895
-6.66667,0.79698,-1.47294,-1.50627,-1.00252,2.15489,-0.54576
-6.78161,0.81662,-1.40963,-1.59790,-0.96469,2.14157,-0.52809
-6.89655,0.82336,-1.39122,-1.62339,-0.95443,2.13698,-0.52211
-7.01149,0.84132,-1.33826,-1.69706,-0.92545,2.12464,-0.50634
-7.12644,0.84923,-1.31680,-1.72605,-0.91437,2.11918,-0.49948
-7.24138,0.85622,-1.29952,-1.74871,-0.90591,2.11435,-0.49347
-7.35632,0.86926,-1.26257,-1.79815,-0.88778,2.10528,-0.48234
-7.47126,0.87211,-1.25342,-1.81061,-0.88325,2.10328,-0.47991
-7.58621,0.87519,-1.24446,-1.82252,-0.87899,2.10113,-0.47729
-7.70115,0.87855,-1.23535,-1.83439,-0.87482,2.09878,-0.47446
-7.81609,0.88134,-1.22837,-1.84328,-0.87175,2.09683,-0.47212
-7.93103,0.88219,-1.22619,-1.84607,-0.87079,2.09623,-0.47141
-8.04598,0.88354,-1.22294,-1.85015,-0.86939,2.09528,-0.47027
-8.16092,0.88356,-1.22290,-1.85021,-0.86937,2.09527,-0.47026
-8.27586,0.88424,-1.22132,-1.85217,-0.86871,2.09480,-0.46969
-8.39080,0.88447,-1.22074,-1.85291,-0.86845,2.09463,-0.46949
-8.50575,0.88469,-1.22016,-1.85366,-0.86820,2.09448,-0.46931
-8.62069,0.88469,-1.22013,-1.85370,-0.86818,2.09448,-0.46931
-8.73563,0.85976,-1.23839,-1.84493,-0.86906,2.11171,-0.49009
-8.85057,0.82320,-1.27328,-1.81991,-0.87542,2.13667,-0.52139
-8.96552,0.77157,-1.33116,-1.77224,-0.89020,2.17119,-0.56738
-9.08046,0.75924,-1.34581,-1.75974,-0.89441,2.17930,-0.57869
-9.19540,0.74291,-1.36572,-1.74257,-0.90030,2.18994,-0.59386
-9.31034,0.73094,-1.38007,-1.73055,-0.90448,2.19767,-0.60513
-9.42529,0.71819,-1.39573,-1.71726,-0.90919,2.20584,-0.61726
-9.54023,0.71613,-1.39833,-1.71501,-0.91000,2.20716,-0.61924
-9.65517,0.71510,-1.39963,-1.71388,-0.91041,2.20781,-0.62023
-9.77011,0.71407,-1.40094,-1.71274,-0.91083,2.20847,-0.62122
-9.88506,0.71304,-1.40225,-1.71160,-0.91124,2.20912,-0.62221
-10.00000,0.71301,-1.40229,-1.71156,-0.91126,2.20914,-0.62224
+0.00000,0.81031,-1.52180,-1.33221,-1.12496,2.14787,-0.53658
+0.07389,0.81027,-1.52181,-1.33221,-1.12497,2.14789,-0.53661
+0.14778,0.81023,-1.52183,-1.33219,-1.12497,2.14792,-0.53665
+0.22167,0.81020,-1.52185,-1.33218,-1.12498,2.14794,-0.53667
+0.29557,0.81017,-1.52187,-1.33216,-1.12499,2.14795,-0.53670
+0.36946,0.81014,-1.52189,-1.33212,-1.12501,2.14797,-0.53671
+0.44335,0.81013,-1.52192,-1.33207,-1.12503,2.14797,-0.53672
+0.51724,0.81013,-1.52195,-1.33200,-1.12506,2.14797,-0.53672
+0.59113,0.81066,-1.52532,-1.32436,-1.12857,2.14738,-0.53604
+0.66502,0.81313,-1.52997,-1.31241,-1.13407,2.14543,-0.53361
+0.73892,0.81716,-1.53342,-1.30155,-1.13912,2.14250,-0.52989
+0.81281,0.82171,-1.53543,-1.29315,-1.14309,2.13929,-0.52582
+0.88670,0.82724,-1.53669,-1.28540,-1.14679,2.13544,-0.52093
+0.96059,0.83184,-1.53714,-1.28018,-1.14932,2.13225,-0.51690
+1.03448,0.83631,-1.53705,-1.27616,-1.15131,2.12916,-0.51302
+1.10837,0.84050,-1.53677,-1.27281,-1.15298,2.12627,-0.50940
+1.18227,0.84541,-1.53610,-1.26959,-1.15463,2.12289,-0.50518
+1.25616,0.85111,-1.53486,-1.26676,-1.15614,2.11896,-0.50032
+1.33005,0.85843,-1.53250,-1.26461,-1.15743,2.11391,-0.49411
+1.40394,0.86563,-1.52948,-1.26384,-1.15810,2.10893,-0.48804
+1.47783,0.87190,-1.52636,-1.26411,-1.15828,2.10458,-0.48278
+1.55172,0.87795,-1.52282,-1.26538,-1.15801,2.10038,-0.47773
+1.62562,0.88392,-1.51875,-1.26771,-1.15727,2.09622,-0.47277
+1.69951,0.90041,-1.50512,-1.27861,-1.15333,2.08468,-0.45916
+1.77340,0.90575,-1.50060,-1.28240,-1.15194,2.08093,-0.45479
+1.84729,0.90934,-1.49751,-1.28507,-1.15096,2.07840,-0.45186
+1.92118,0.91379,-1.49361,-1.28851,-1.14968,2.07526,-0.44824
+1.99507,0.91829,-1.48961,-1.29210,-1.14835,2.07208,-0.44459
+2.06897,0.92280,-1.48558,-1.29576,-1.14699,2.06890,-0.44095
+2.14286,0.92991,-1.47893,-1.30211,-1.14462,2.06385,-0.43523
+2.21675,0.93876,-1.47015,-1.31094,-1.14128,2.05756,-0.42815
+2.29064,0.94672,-1.46198,-1.31943,-1.13807,2.05189,-0.42183
+2.36453,0.95384,-1.45474,-1.32693,-1.13525,2.04679,-0.41620
+2.43842,0.95924,-1.44904,-1.33302,-1.13295,2.04292,-0.41194
+2.51232,0.96371,-1.44415,-1.33837,-1.13092,2.03971,-0.40843
+2.58621,0.96556,-1.44209,-1.34066,-1.13005,2.03838,-0.40699
+2.66010,0.96723,-1.44027,-1.34265,-1.12929,2.03718,-0.40568
+2.73399,0.96909,-1.43820,-1.34494,-1.12843,2.03584,-0.40423
+2.80788,0.97007,-1.43707,-1.34622,-1.12794,2.03513,-0.40346
+2.88177,0.97104,-1.43592,-1.34754,-1.12743,2.03443,-0.40270
+2.95567,0.97183,-1.43500,-1.34859,-1.12703,2.03387,-0.40208
+3.02956,0.98021,-1.42081,-1.36738,-1.11958,2.02781,-0.39554
+3.10345,0.98413,-1.41224,-1.37951,-1.11469,2.02496,-0.39248
+3.17734,0.98630,-1.40726,-1.38663,-1.11182,2.02338,-0.39078
+3.25123,0.98825,-1.40170,-1.39489,-1.10847,2.02196,-0.38925
+3.32512,0.99178,-1.38971,-1.41308,-1.10109,2.01938,-0.38648
+3.39901,0.99388,-1.37699,-1.43341,-1.09276,2.01783,-0.38479
+3.47291,0.99463,-1.36646,-1.45083,-1.08560,2.01726,-0.38414
+3.54680,0.99453,-1.35541,-1.46959,-1.07787,2.01730,-0.38414
+3.62069,0.99367,-1.34770,-1.48318,-1.07226,2.01790,-0.38473
+3.69458,0.99142,-1.34028,-1.49715,-1.06641,2.01950,-0.38639
+3.76847,0.98744,-1.33201,-1.51364,-1.05946,2.02235,-0.38936
+3.84236,0.98387,-1.32691,-1.52454,-1.05482,2.02491,-0.39205
+3.91626,0.98007,-1.32273,-1.53405,-1.05073,2.02764,-0.39493
+3.99015,0.97297,-1.31664,-1.54893,-1.04430,2.03272,-0.40034
+4.06404,0.96487,-1.31202,-1.56207,-1.03852,2.03852,-0.40657
+4.13793,0.95750,-1.30877,-1.57248,-1.03391,2.04378,-0.41227
+4.21182,0.95264,-1.30707,-1.57863,-1.03116,2.04724,-0.41604
+4.28571,0.94704,-1.30574,-1.58469,-1.02840,2.05123,-0.42042
+4.35961,0.92888,-1.30723,-1.59508,-1.02313,2.06411,-0.43477
+4.43350,0.91125,-1.31255,-1.59912,-1.02043,2.07654,-0.44891
+4.50739,0.89782,-1.31767,-1.60067,-1.01902,2.08596,-0.45982
+4.58128,0.88341,-1.32412,-1.60095,-1.01809,2.09601,-0.47166
+4.65517,0.86673,-1.33250,-1.60004,-1.01755,2.10757,-0.48554
+4.72906,0.85684,-1.33811,-1.59859,-1.01761,2.11439,-0.49387
+4.80296,0.84799,-1.34345,-1.59684,-1.01787,2.12046,-0.50138
+4.87685,0.83791,-1.34972,-1.59464,-1.01826,2.12735,-0.51000
+4.95074,0.82482,-1.35824,-1.59135,-1.01897,2.13624,-0.52130
+5.02463,0.81200,-1.36693,-1.58772,-1.01987,2.14490,-0.53250
+5.09852,0.79943,-1.37577,-1.58383,-1.02092,2.15333,-0.54360
+5.17241,0.79432,-1.37946,-1.58214,-1.02140,2.15674,-0.54815
+5.24631,0.78517,-1.38558,-1.57998,-1.02194,2.16283,-0.55634
+5.32020,0.77680,-1.39080,-1.57872,-1.02216,2.16837,-0.56389
+5.39409,0.76801,-1.39581,-1.57824,-1.02208,2.17415,-0.57186
+5.46798,0.76077,-1.39944,-1.57877,-1.02166,2.17890,-0.57849
+5.54187,0.75399,-1.40262,-1.57966,-1.02112,2.18332,-0.58473
+5.61576,0.74798,-1.40518,-1.58094,-1.02045,2.18722,-0.59028
+5.68966,0.73951,-1.40856,-1.58320,-1.01934,2.19270,-0.59817
+5.76355,0.72436,-1.41257,-1.59082,-1.01597,2.20241,-0.61241
+5.83744,0.71848,-1.41362,-1.59469,-1.01432,2.20615,-0.61798
+5.91133,0.71308,-1.41397,-1.59928,-1.01242,2.20958,-0.62313
+5.98522,0.71019,-1.41409,-1.60186,-1.01135,2.21140,-0.62589
+6.05911,0.70676,-1.41408,-1.60518,-1.00998,2.21357,-0.62918
+6.13300,0.70306,-1.41371,-1.60937,-1.00827,2.21589,-0.63273
+6.20690,0.70004,-1.41318,-1.61319,-1.00673,2.21779,-0.63565
+6.28079,0.69766,-1.41263,-1.61639,-1.00544,2.21928,-0.63795
+6.35468,0.69534,-1.41199,-1.61971,-1.00411,2.22072,-0.64019
+6.42857,0.69475,-1.41182,-1.62058,-1.00377,2.22110,-0.64077
+6.50246,0.69418,-1.41165,-1.62141,-1.00344,2.22145,-0.64131
+6.57635,0.69359,-1.41146,-1.62228,-1.00308,2.22181,-0.64189
+6.65025,0.69304,-1.41128,-1.62312,-1.00275,2.22216,-0.64243
+6.72414,0.69304,-1.41130,-1.62310,-1.00276,2.22216,-0.64242
+6.79803,0.69305,-1.41132,-1.62305,-1.00278,2.22216,-0.64242
+6.87192,0.69306,-1.41135,-1.62300,-1.00280,2.22215,-0.64241
+6.94581,0.69445,-1.41562,-1.61467,-1.00605,2.22130,-0.64108
+7.01970,0.69512,-1.41766,-1.61067,-1.00762,2.22089,-0.64045
+7.09360,0.69537,-1.41853,-1.60901,-1.00827,2.22073,-0.64021
+7.16749,0.69577,-1.41974,-1.60664,-1.00921,2.22049,-0.63983
+7.24138,0.69715,-1.42678,-1.59373,-1.01429,2.21964,-0.63853
+7.31527,0.69804,-1.43558,-1.57836,-1.02037,2.21911,-0.63774
+7.38916,0.69842,-1.44486,-1.56263,-1.02664,2.21890,-0.63743
+7.46305,0.69833,-1.45384,-1.54779,-1.03259,2.21898,-0.63759
+7.53695,0.69730,-1.46948,-1.52265,-1.04274,2.21966,-0.63869
+7.61084,0.69683,-1.47401,-1.51550,-1.04565,2.21996,-0.63918
+7.68473,0.69639,-1.47734,-1.51033,-1.04776,2.22025,-0.63963
+7.75862,0.69544,-1.48281,-1.50207,-1.05113,2.22086,-0.64060
+7.83251,0.69339,-1.49101,-1.49028,-1.05595,2.22215,-0.64265
+7.90640,0.69051,-1.50139,-1.47560,-1.06197,2.22397,-0.64552
+7.98030,0.68779,-1.51048,-1.46293,-1.06720,2.22568,-0.64825
+8.05419,0.68377,-1.52287,-1.44593,-1.07424,2.22821,-0.65228
+8.12808,0.67956,-1.53536,-1.42890,-1.08135,2.23084,-0.65652
+8.20197,0.67475,-1.54853,-1.41130,-1.08874,2.23383,-0.66137
+8.27586,0.67229,-1.55513,-1.40250,-1.09245,2.23536,-0.66386
+8.34975,0.67021,-1.56074,-1.39501,-1.09563,2.23665,-0.66597
+8.42365,0.66812,-1.56673,-1.38687,-1.09909,2.23795,-0.66810
+8.49754,0.66535,-1.57510,-1.37530,-1.10402,2.23966,-0.67093
+8.57143,0.66201,-1.58627,-1.35944,-1.11081,2.24172,-0.67436
+8.64532,0.65992,-1.59332,-1.34941,-1.11513,2.24301,-0.67653
+8.71921,0.65884,-1.59709,-1.34400,-1.11746,2.24368,-0.67764
+8.79310,0.65846,-1.59845,-1.34202,-1.11832,2.24391,-0.67803
+8.86700,0.65809,-1.59986,-1.33996,-1.11921,2.24414,-0.67842
+8.94089,0.65681,-1.60458,-1.33308,-1.12219,2.24493,-0.67975
+9.01478,0.65510,-1.62393,-1.30083,-1.13625,2.24601,-0.68164
+9.08867,0.65495,-1.63255,-1.28570,-1.14291,2.24612,-0.68186
+9.16256,0.65513,-1.64053,-1.27128,-1.14928,2.24604,-0.68176
+9.23645,0.65530,-1.64582,-1.26165,-1.15355,2.24595,-0.68164
+9.31034,0.65549,-1.64982,-1.25427,-1.15683,2.24585,-0.68149
+9.38424,0.65572,-1.65380,-1.24687,-1.16013,2.24571,-0.68129
+9.45813,0.65585,-1.65576,-1.24322,-1.16175,2.24564,-0.68118
+9.53202,0.65671,-1.66242,-1.23029,-1.16753,2.24514,-0.68038
+9.60591,0.65735,-1.66694,-1.22143,-1.17150,2.24476,-0.67978
+9.67980,0.65839,-1.67280,-1.20969,-1.17677,2.24415,-0.67880
+9.75369,0.65904,-1.67609,-1.20301,-1.17978,2.24376,-0.67818
+9.82759,0.66053,-1.68252,-1.18967,-1.18580,2.24287,-0.67674
+9.90148,0.66248,-1.68907,-1.17556,-1.19218,2.24170,-0.67485
+9.97537,0.66471,-1.69543,-1.16147,-1.19859,2.24036,-0.67269
+10.04926,0.66692,-1.70069,-1.14938,-1.20410,2.23902,-0.67054
+10.12315,0.66870,-1.70451,-1.14037,-1.20822,2.23794,-0.66881
+10.19704,0.67060,-1.70826,-1.13137,-1.21235,2.23678,-0.66696
+10.27094,0.67228,-1.71109,-1.12428,-1.21561,2.23576,-0.66533
+10.34483,0.67369,-1.71343,-1.11839,-1.21832,2.23490,-0.66395
+10.41872,0.67641,-1.71700,-1.10874,-1.22277,2.23323,-0.66130
+10.49261,0.67856,-1.71931,-1.10207,-1.22586,2.23191,-0.65920
+10.56650,0.68119,-1.72202,-1.09413,-1.22954,2.23030,-0.65665
+10.64039,0.68443,-1.72496,-1.08506,-1.23375,2.22830,-0.65350
+10.71429,0.68618,-1.72669,-1.07992,-1.23615,2.22722,-0.65181
+10.78818,0.68815,-1.72833,-1.07469,-1.23858,2.22600,-0.64990
+10.86207,0.69072,-1.73020,-1.06837,-1.24154,2.22440,-0.64741
+10.93596,0.69272,-1.73164,-1.06349,-1.24382,2.22316,-0.64549
+11.00985,0.69489,-1.73299,-1.05856,-1.24613,2.22181,-0.64339
+11.08374,0.69764,-1.73456,-1.05262,-1.24892,2.22009,-0.64074
+11.15764,0.70649,-1.73828,-1.03600,-1.25676,2.21455,-0.63226
+11.23153,0.70998,-1.73927,-1.03034,-1.25945,2.21235,-0.62893
+11.30542,0.71301,-1.74020,-1.02532,-1.26183,2.21044,-0.62604
+11.37931,0.71667,-1.74090,-1.02005,-1.26435,2.20812,-0.62257
+11.45320,0.72094,-1.74149,-1.01436,-1.26708,2.20541,-0.61853
+11.52709,0.72789,-1.74123,-1.00739,-1.27046,2.20098,-0.61197
+11.60099,0.73379,-1.73982,-1.00376,-1.27228,2.19720,-0.60642
+11.67488,0.74115,-1.73655,-1.00209,-1.27323,2.19246,-0.59953
+11.74877,0.74838,-1.73140,-1.00415,-1.27246,2.18778,-0.59279
+11.82266,0.75286,-1.72737,-1.00701,-1.27124,2.18487,-0.58862
+11.89655,0.75811,-1.72180,-1.01201,-1.26906,2.18144,-0.58375
+11.97044,0.76343,-1.71550,-1.01830,-1.26628,2.17796,-0.57884
+12.04433,0.76798,-1.70952,-1.02484,-1.26337,2.17497,-0.57465
+12.11823,0.77182,-1.70406,-1.03111,-1.26057,2.17244,-0.57112
+12.19212,0.77438,-1.70024,-1.03567,-1.25853,2.17075,-0.56877
+12.26601,0.77752,-1.69532,-1.04168,-1.25584,2.16867,-0.56589
+12.33990,0.78123,-1.68908,-1.04958,-1.25229,2.16621,-0.56251
+12.41379,0.78605,-1.68028,-1.06114,-1.24710,2.16300,-0.55811
+12.48768,0.79018,-1.67224,-1.07196,-1.24224,2.16025,-0.55435
+12.56158,0.79368,-1.66504,-1.08188,-1.23779,2.15791,-0.55117
+12.63547,0.80026,-1.64937,-1.10440,-1.22768,2.15349,-0.54521
+12.70936,0.80301,-1.64199,-1.11534,-1.22277,2.15164,-0.54271
+12.78325,0.80601,-1.63333,-1.12836,-1.21693,2.14961,-0.53999
+12.85714,0.80806,-1.62663,-1.13869,-1.21230,2.14822,-0.53814
+12.93103,0.81107,-1.61640,-1.15452,-1.20522,2.14618,-0.53541
+13.00493,0.81274,-1.61083,-1.16312,-1.20139,2.14504,-0.53389
+13.07882,0.81365,-1.60774,-1.16790,-1.19926,2.14443,-0.53307
+13.15271,0.81409,-1.60624,-1.17023,-1.19822,2.14413,-0.53268
+13.22660,0.81453,-1.60468,-1.17264,-1.19715,2.14383,-0.53228
+13.30049,0.81497,-1.60313,-1.17505,-1.19607,2.14353,-0.53188
+13.37438,0.81582,-1.60006,-1.17984,-1.19394,2.14295,-0.53111
+13.44828,0.81740,-1.59454,-1.18839,-1.19015,2.14187,-0.52969
+13.52217,0.82028,-1.58327,-1.20611,-1.18231,2.13991,-0.52709
+13.59606,0.82251,-1.57089,-1.22637,-1.17335,2.13837,-0.52504
+13.66995,0.82367,-1.56217,-1.24095,-1.16691,2.13757,-0.52396
+13.74384,0.82414,-1.55639,-1.25089,-1.16252,2.13723,-0.52350
+13.81773,0.82443,-1.55210,-1.25828,-1.15925,2.13703,-0.52322
+13.89163,0.82424,-1.54012,-1.27978,-1.14977,2.13713,-0.52328
+13.96552,0.82375,-1.53314,-1.29259,-1.14413,2.13744,-0.52364
+14.03941,0.82298,-1.52752,-1.30322,-1.13945,2.13795,-0.52426
+14.11330,0.82230,-1.52349,-1.31095,-1.13604,2.13839,-0.52481
+14.18719,0.82164,-1.52058,-1.31666,-1.13353,2.13883,-0.52536
+14.26108,0.82085,-1.51775,-1.32236,-1.13101,2.13936,-0.52602
+14.33498,0.82042,-1.51635,-1.32520,-1.12976,2.13964,-0.52638
+14.40887,0.81918,-1.51297,-1.33225,-1.12665,2.14047,-0.52743
+14.48276,0.81866,-1.51165,-1.33502,-1.12543,2.14081,-0.52786
+14.55665,0.81839,-1.51098,-1.33645,-1.12480,2.14100,-0.52809
+14.63054,0.81812,-1.51034,-1.33780,-1.12420,2.14117,-0.52832
+14.70443,0.81784,-1.50968,-1.33921,-1.12358,2.14136,-0.52856
+14.77833,0.81755,-1.50902,-1.34062,-1.12296,2.14155,-0.52880
+14.85222,0.81726,-1.50837,-1.34202,-1.12234,2.14175,-0.52905
+14.92611,0.81725,-1.50834,-1.34209,-1.12231,2.14176,-0.52906
+15.00000,0.81723,-1.50830,-1.34216,-1.12228,2.14177,-0.52907
diff --git a/python/examples/log_analysis_example.py b/python/examples/log_analysis_example.py
index e540fea..1765af9 100644
--- a/python/examples/log_analysis_example.py
+++ b/python/examples/log_analysis_example.py
@@ -1,14 +1,16 @@
 import pinocchio as pin
 import numpy as np
 import argparse
-from ur_simple_control.managers import getMinimalArgParser
 from ur_simple_control.util.logging_utils import LogManager
+from ur_simple_control.visualize.manipulator_comparison_visualizer import getLogComparisonArgs, ManipulatorComparisonManager
 
 
 if __name__ == "__main__": 
+    args = getLogComparisonArgs()
     log_manager = LogManager(None)
-    log_manager.loadLog('./data/ft_readings_9.pickle')
-    # pickling does not save this attribute somehow????
-    #log_manager.are_logs_vectorized_flag = False
+    log_manager.loadLog(args.log_file1)
+#    mcm = ManipulatorComparisonManager(args)
+#    mcm.visualizeWholeRuns()
+    # TODO: put this into tabs
     log_manager.plotAllControlLoops()
 
diff --git a/python/examples/path_following_mpc.py b/python/examples/path_following_mpc.py
index 5387c01..9651e99 100644
--- a/python/examples/path_following_mpc.py
+++ b/python/examples/path_following_mpc.py
@@ -1,3 +1,4 @@
+# PYTHON_ARGCOMPLETE_OK
 import numpy as np
 import time
 import argparse
@@ -18,6 +19,7 @@ def get_args():
     parser = getMinimalArgParser()
     parser = get_OCP_args(parser)
     parser = getClikArgs(parser) # literally just for goal error
+    argcomplete.autocomplete(parser)
     args = parser.parse_args()
     return args
 
diff --git a/python/examples/path_in_pixels.csv b/python/examples/path_in_pixels.csv
index d26413b..bc1f19d 100644
--- a/python/examples/path_in_pixels.csv
+++ b/python/examples/path_in_pixels.csv
@@ -1,88 +1,204 @@
-0.17444,0.82632
-0.17039,0.82632
-0.16837,0.82632
-0.16635,0.82632
-0.17848,0.83020
-0.25736,0.83408
-0.30590,0.83149
-0.37466,0.82891
-0.42926,0.82891
-0.56881,0.82632
-0.72050,0.81986
-0.72859,0.81857
-0.73465,0.81857
-0.73870,0.81728
-0.74072,0.81599
-0.74477,0.81469
-0.74881,0.81211
-0.75488,0.80565
-0.75083,0.79014
-0.70027,0.76042
-0.68005,0.74879
-0.62746,0.71777
-0.56477,0.67772
-0.53241,0.65316
-0.46769,0.58209
-0.44949,0.56917
-0.39691,0.50714
-0.38679,0.49422
-0.36252,0.46450
-0.34432,0.44640
-0.33623,0.43736
-0.33016,0.43090
-0.32612,0.42831
-0.32410,0.42702
-0.32208,0.42573
-0.32005,0.42444
-0.31803,0.42444
-0.31601,0.42314
-0.34837,0.43865
-0.36859,0.44770
-0.45151,0.47613
-0.47174,0.47871
-0.51825,0.48517
-0.55870,0.49034
-0.57893,0.49034
-0.61735,0.49292
-0.63758,0.49292
-0.65376,0.49292
-0.65982,0.49292
-0.66387,0.49292
-0.66589,0.49292
-0.66589,0.49292
-0.66791,0.49292
-0.66994,0.49292
-0.67398,0.49163
-0.66791,0.47096
-0.64567,0.44899
-0.62140,0.43219
-0.58702,0.40634
-0.51016,0.35207
-0.48792,0.33656
-0.42926,0.28358
-0.40500,0.26291
-0.38477,0.24740
-0.34634,0.20734
-0.33825,0.19571
-0.32814,0.18666
-0.31803,0.17762
-0.30994,0.17116
-0.30792,0.16857
-0.30387,0.16599
-0.30387,0.16599
-0.30185,0.16470
-0.30185,0.16340
-0.30185,0.16211
-0.31601,0.16340
-0.38882,0.16986
-0.44342,0.17503
-0.53039,0.18020
-0.55263,0.18020
-0.58297,0.18020
-0.60522,0.17762
-0.62949,0.17633
-0.63353,0.17633
-0.63555,0.17633
-0.63758,0.17633
-0.63960,0.17633
-0.63960,0.17633
+0.64609,0.56843
+0.64609,0.56843
+0.64609,0.57243
+0.64312,0.57776
+0.64312,0.58042
+0.63868,0.58841
+0.63720,0.59907
+0.63424,0.61106
+0.63127,0.62171
+0.62535,0.63503
+0.61646,0.64702
+0.60757,0.65634
+0.59868,0.66567
+0.59127,0.67233
+0.58387,0.67766
+0.57794,0.68298
+0.57053,0.68831
+0.56165,0.69364
+0.54979,0.69897
+0.53794,0.70296
+0.52757,0.70563
+0.51720,0.70696
+0.50683,0.70696
+0.47869,0.70696
+0.46980,0.70696
+0.46387,0.70696
+0.45646,0.70696
+0.44906,0.70696
+0.44165,0.70696
+0.42980,0.70563
+0.41498,0.70296
+0.40165,0.70030
+0.38980,0.69897
+0.38091,0.69630
+0.37350,0.69364
+0.37054,0.69231
+0.36758,0.69231
+0.36462,0.69098
+0.36313,0.68964
+0.36165,0.68831
+0.36017,0.68831
+0.34684,0.66966
+0.34091,0.65768
+0.33647,0.65235
+0.33351,0.64436
+0.32462,0.62970
+0.31869,0.61106
+0.31425,0.59507
+0.30980,0.57776
+0.30832,0.56444
+0.30980,0.54978
+0.31128,0.53247
+0.31425,0.52048
+0.31721,0.50982
+0.32165,0.49251
+0.32906,0.47652
+0.33499,0.46320
+0.33943,0.45521
+0.34536,0.44722
+0.36906,0.43124
+0.39276,0.42191
+0.40906,0.41392
+0.42684,0.40593
+0.44758,0.39660
+0.46091,0.39261
+0.47276,0.38861
+0.48609,0.38328
+0.50387,0.37662
+0.52165,0.36996
+0.53942,0.36330
+0.54683,0.36064
+0.55868,0.35265
+0.56905,0.34466
+0.57942,0.33533
+0.58683,0.32601
+0.59424,0.31802
+0.60016,0.31002
+0.60905,0.29937
+0.62090,0.27539
+0.62387,0.26474
+0.62535,0.25408
+0.62683,0.24875
+0.62831,0.24209
+0.62831,0.23410
+0.62831,0.22744
+0.62831,0.22211
+0.62831,0.21678
+0.62831,0.21545
+0.62831,0.21412
+0.62831,0.21279
+0.62831,0.21146
+0.62979,0.25541
+0.63127,0.26740
+0.63424,0.27406
+0.63868,0.28338
+0.64016,0.28738
+0.64164,0.28871
+0.64164,0.29138
+0.65053,0.30203
+0.66090,0.31402
+0.67127,0.32601
+0.68164,0.33666
+0.69942,0.35398
+0.70534,0.35798
+0.70979,0.36064
+0.71720,0.36464
+0.72905,0.36863
+0.74238,0.37529
+0.75423,0.38062
+0.77053,0.38728
+0.78682,0.39394
+0.80460,0.39927
+0.81349,0.40193
+0.82090,0.40460
+0.82830,0.40859
+0.83867,0.41392
+0.85201,0.42191
+0.86089,0.42591
+0.86534,0.42857
+0.86682,0.42990
+0.86830,0.43124
+0.87423,0.43390
+0.88904,0.46054
+0.89497,0.47253
+0.90089,0.48318
+0.90534,0.48984
+0.90830,0.49517
+0.91126,0.50050
+0.91274,0.50316
+0.91571,0.51382
+0.91867,0.52048
+0.92163,0.52980
+0.92311,0.53513
+0.92608,0.54579
+0.92756,0.55778
+0.92904,0.56976
+0.92904,0.58042
+0.92904,0.58841
+0.92904,0.59640
+0.92756,0.60306
+0.92756,0.60839
+0.92460,0.61772
+0.92163,0.62438
+0.92015,0.63237
+0.91719,0.64169
+0.91719,0.64702
+0.91423,0.65235
+0.91126,0.65901
+0.90978,0.66434
+0.90682,0.66966
+0.90386,0.67632
+0.89349,0.69630
+0.88756,0.70296
+0.88460,0.70962
+0.87867,0.71628
+0.87275,0.72428
+0.86089,0.73493
+0.84904,0.74159
+0.83423,0.74825
+0.81793,0.75092
+0.80756,0.75092
+0.79571,0.75092
+0.78386,0.75092
+0.77349,0.74958
+0.76460,0.74825
+0.75868,0.74692
+0.75127,0.74559
+0.74238,0.74292
+0.73053,0.73893
+0.72016,0.73493
+0.71127,0.73094
+0.69349,0.72028
+0.68609,0.71362
+0.67720,0.70696
+0.67127,0.70030
+0.66090,0.69231
+0.65498,0.68831
+0.65201,0.68565
+0.65053,0.68432
+0.64905,0.68298
+0.64757,0.68165
+0.64461,0.67899
+0.63868,0.67499
+0.62831,0.66434
+0.61942,0.64968
+0.61350,0.63903
+0.61053,0.63104
+0.60757,0.62571
+0.60164,0.60839
+0.59868,0.59774
+0.59720,0.58841
+0.59572,0.58175
+0.59572,0.57642
+0.59572,0.57110
+0.59572,0.56843
+0.59572,0.56177
+0.59572,0.55911
+0.59572,0.55778
+0.59572,0.55644
+0.59572,0.55511
+0.59572,0.55378
+0.59572,0.55245
+0.59572,0.55245
+0.59572,0.55245
diff --git a/python/examples/point_impedance_control.py b/python/examples/point_impedance_control.py
index f8c5527..acc0302 100644
--- a/python/examples/point_impedance_control.py
+++ b/python/examples/point_impedance_control.py
@@ -1,7 +1,8 @@
+# PYTHON_ARGCOMPLETE_OK
 import pinocchio as pin
 import numpy as np
 import copy
-import argparse
+import argparse, argcomplete
 import time
 from functools import partial
 from ur_simple_control.visualize.visualize import plotFromDict
@@ -25,6 +26,7 @@ def getArgs():
     parser.add_argument('--z-only', action=argparse.BooleanOptionalAction, \
         help="whether you have general impedance or just ee z axis", default=False)
 
+#    argcomplete.autocomplete(parser)
     args = parser.parse_args()
     return args
 
@@ -104,7 +106,7 @@ def controlLoopCartesianPointImpedance(args, Mtool_init, clik_controller, robot
     #Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
     # but let's stick to the default for now
     if not args.z_only:
-        Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
+        Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
     else:
         Z = np.diag(np.array([0.0, 0.0, 1.0, 0.0, 0.0, 0.0]))
     #Z = np.diag(np.array([1.0, 1.0, 1.0, 10.0, 10.0, 10.0]))
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 08cab676ac3eb5e9425e00ae79331b2574792e80..14d9c2bf74a6ca6676bc6f7fa0e0d6feada992d5 100644
GIT binary patch
delta 2934
zcmdn}mFdz?Cce|Wyj%<n3=H4z+NK{kHjz()QD&n0kx0fI#VExTh7_J0rCjAGWk!Y+
zh7{f$m0Z;*RWO?`M=e)9N}Z8`lOct_g&|6ViNT#AMWBTtMQ}9}$UFvyC{3`KPzytf
zFkDP)vkjw?2&3KRcu`{!#-p1jYu;dFwAh@ZZN@s;z|d#%swv!)+YPlD*(Yx{ENA9e
z%{W=mP=0d3A`!;O$%#grnCclPH<XL<q;OSnF)(B^F)$RVPQIuwKH0%oM4PWfA7V2D
zLkf2dV+zk|W(c2=p_0FnA4!%MWQaftA3{M5LzesGLRMj3{uF+gV;LDHiyC|LbFOBD
z857CS^JjCuu^J=OFW1S6>kTJ!o2E{lYU0f7r>U~J*wm4cwaAu%fno9%vof~N3=9m#
z-kS}~MVXk~m?rDHn@`?usmI7M`I)8fWPW${2yUK^lI#3R7x|SII9}vey~M40gI~D6
zva|9!zv?A^)jI;B6LJ<fE~vdKVETcXK|t*b8w0P<1tIMfA=h;sF6ufQ(7D3ne}hNh
zI*-gn9+?>-3rw!^X#DshI@!@mOYpR!wZ9I-c^$0)N#+X@i~*9HYpf(V851T?bq<${
zVPIe=ie+G6cs^?;2<)^kiUSG7GcYhzfjlO@S<&S%BV)+qr>_3eU@PP<^2lB1QN75c
zx*+{3kKPAX1|gXb9Gm^!PB5{}0{K*CvaF{#tLA4X=77yso}H{bN<0h<(-~?QYZ&4g
zCSMGf+x*rqf@QKrf!O5Pi$o{a1@i<dLV}fnA%(YwAzlg2<EvrFa)I#>bQL!PLk&Z`
zGE5|ezlI@R1;(pk%<`Jt7%0l<G5KS#G^5w#iy?}PAQ6$tXM+`ZQUtT57#NE9!OAX%
z$O)wg)iA`XBk2wR>-Gcd4gl*0iHHcqYrqtxfUMV?{LxQ-@~05t$)2I+Aet*hBwK@l
zp~$}^bn-<dVQx*B@*0N8A45c#vce}{gs7Z+K2&6~LORFfdI#3YHen(V-C|(f{3X#K
z-He)(8^fX*^(H6!%TKCjW9Ls1tYOHC0j12zGT~N?ag#qr2v436!Nq7WxiQQZiElXh
zVz@bD>g2)*O>SeD6G}3{d{a0-7sTf^hw*C|vhpSeItVisPQDl_I{8+x@Z|a09BhmX
zH4ItBlRrktGFnaMk2GbpntVQ3fhk@M6pw60%WD|3$|iqQ6=t3dv496|K@CIJWW`Wn
zrmV`zA0wpLYME-7vZ^Ov3=x?uAH~ZEV{uLPj^bvlnf!5r$mHxOen!2?y#aEQXGYmG
z)`PTd{u~v=$e1wMEhdQ3Ve-tFE1Sz>Ga1<qF)}a|_icU_ca%|*i$O+VzST^t>(T}n
zr42TSUzWD)V7bB}@nG}m1Se)jkI8%~1wuO*7#K8}iq?XXwmXQ}Ie9{gCZqS}T`7f3
zj5{Xlq^B}2o;)etfzfaB<@8!cj>&-;3XC@<7iE--LiFBZD=$jTOU@{QYX6sE$e6g<
zAagSd<NC=@a<%!lF)%RPVoxkfElNyJE!sR;F3(MV6DSu+1m!2?m-r^;fyDKa6LT_?
ziV{mw-9l0}H|LeIYwicR=KzRk01@*+L==bsyY?W6wHQPknygr+%(f21+&S5+Or5b~
za#2~zWXU$g&ELx$7_~NlWHy3`<q+Qnfn3B|mY7qTS_~?P6%>jLK++p0CsrshZk*gy
zk;wRHa^q@grsBt&87nmz84V{VP7vcrVF9N@rO63dVw3%+a7}(%DFLEc7#T{0CvTh}
zGr6})ke?ftaw^#>*)-WVudNbiQj!3fKZAjR!B3N?C<nw@4<fdL2rCe=YVyMx9qu_G
zW*vywHd(k<QSJgLoP<!qDLy^5BybNC14HrQ$w9SUiXsxz?I+qVNV_Dgb3;UQy2nJ1
z8NpM0k(iS|)`~M~Zsx4xWMjJyGWy13i57V&bo)UvH$lX$$$>4hrl43W!WIKa;SN$A
z4GL10B2bVP9iF_n#h&s0=Ep62L6N_(Q%q?UD5$y9i!uueQj6ltQge#jK)N7a1hLjm
zzS$`)brIxGVN8Ff@NMSndcwpFb`99YrzZdHQD8heS+3WL(O`3AZy2KmI303ALKR^!
zL^a5Y%^>0^$RTV+iFqmcxkU>>B0DEP?=xn!+brF`m`Mf60+1YBOVM)>7p(r?<VO>g
zWX^-kVb4u0D9Fr9F9ubf3b5pJX|lp31xB~Yj+34-39(FGxLIbi>tuT-MwiL=r_AT$
z0H>Xf8paf+6lST(8z$E>RWeP!Kc$1MlBtqOlXJ81w0cISBB{y!)5RoKf@}pRlV*^8
zY{dnssVQDX%O>xgE(-|^a30t{`RQ~8#z&LcW*D)BGBYp~w{EtZA<U>Uh3x@1f4_aF
z{RKJwOWgW5M8#(?%@CXr*5P(TSp2%M+9hGN8<X2-C^9~n{9%R$`&70MybL@Blk+x6
zZ+<$PgK={8tf`EvCJWA1X52a1bhasD^XBZ?&5Ru1Kyl4p1d@^EU`@`+FHSAmIazwH
zIb-kS?AiXC7tM|3G6p5tqFo^OV@o}V_(4iXlZ{qbvb_drSwFdOg)Jk)<n=2w85t(u
zT#?K;d$R4yB*q(?7py$V${0I2YrP%g&dDp+2QpsY{CoXwM#i0!w{J9Le6#uGMh!-`
zCm=I+P8Q#6&iHw<|K|OS!jt*8h{A-mwpcU%ob10@X>-RGStdsD$;-CsF#ex>d7B^O
zj>$^fl~Td3-UBit6GR*X5ulQ)C>g{8mmpx@-2-t!Nw(-Ii1h+QTmlhIAmRsz_zfa>
z89~)4h!6x3A|OI+^2+V%^~*rz^aKV5h8Bj8Ok%7YpUfPXSVceiurRX9eqv)}mH*7f
z$7=OSjgeL96PpsN<|hwERaV7ML5!TNN}t3OSj9iPG3v6~e2QaKV3qph!pOrK#`sBw
zmDTRE057ZFCpj%vPsUF%j2s|J1Wd|Uv5J1GU=(Jx|D?q_*?)(sh4wAZg3P@9<m8OZ
zd~i;`#hO=|TTod9j-w=y2SACh2$aT)z`2P9B(`DmnjJ2TytlX$i_(+xa|?1(OHwC)
z*cry1$H2hwftit!@iWKdz+J|c-`N-#W$!Xb++~ou%OLuJMTwE+lLuoYBcssw3MMZW
aM*fcsAo6<zM=B?y{wD?yS!Be(zyJU=<ylYw

delta 2721
zcmccglWF%?Cce|Wyj%<n3=IDt+N7%<naC%>C^Av~NFYy+Vy;q@5+g$jLke$>a;{30
z3Yg88qnfK0rN+p>$&kX|!Vsm-#Nf`5BGAH+BDk6fWDWyElm=K#sD&X#7%rx{*@aO_
zgwc9)x~Q=T<Dt#7HE%F7a%?WqHe;Q9wo#a+$grMq@<eShuN1Z_E(V5dCI*Hgz7l<i
zi3|)W>@|!j9IKfjd`5;mK_ofO5*CO!149ZILe1ojM&grK8Tc@BuV$RQu2FvSuSLR4
zDLj*P4a+BUI`B=7(-WCou|$M1YVt#E@yW*a>XRjnN|-A7C;J+E^Yg7{gqX?55XsOp
zck^;%HO9%Qrp}Ykn#^GK(^THP+SHM8@<+2Wwhs&p48_KqbInDW>|B@_cm(<_J1wts
zD_-JOyumLp!Q{HI+C^ct6^s{!H81gN-VqR;kh8#XLG4um(<cI=7sT|p$Q_WmBH;Ic
zn}2e<WfCLT<X4uyd|w$DcwLx2Ff;JT{rJK;`K*VUs1O4K!x;^0e-VcBB3c2A%oi9K
z0~j}7w36UtjG6q(Ib1T1fq|hYo`Hek`K*~Bu+zRM0VI^TIm+ceBV*uXD>r}X2Rs7T
zdE_qg$X(}Ay~v}wApI(j-UoIDA(;<clPf&rHXm|Z#iTqP<Rw`K28I@fk4ze@9G^lM
zwOQjCKgBS{GqSoceRg6F+}!8c$;xQ3S;D`Og)N1<h9O>I^1~o`M(4?aEW(;Ru+Xbv
zh*yGyBLhPUZw*7dGMvX(!;s}R`E9TyzY1K8zlI@Rb#iy4!sNgZ5w2`028JU38pbRi
zkfD>cLiredCM$-BOkN$y%jgFdTNT2^2$SU&ND-=Gh}VFbR>L@%F-%0Rh9N5eCWN42
zW`mSW)(qvJ%oHXJ(ya+olEP5K5U({kZ<-uaRtVUx387|;U>2uHwgv-3k$*|}WXDKh
zUM-lG8iuTp$s1LLCntpoPu2`mWd!pWqbCO%%1_Q-Cj!+i2G-4A5)0DJs5N<axb381
zX_gv>c>T$Z?Q(o6%n$`Bf;9|TabT;H!mSt+CNl<0fm9iSbRf7!lh;PdO*V~?WK5r|
z7$!IQ!31t@6PS}qvf*N8aIt)t7_S9PtcD@00OWMW;>nDWqKtkZyVw{RY8bLgCkGmW
zt+k#U8fnUF4cEb#<p+ukrg-(qYa^8zD<*%`7iOM~u#gvSVGTo84#@DyMNz_&6Dm1b
znQEA_YC#_37l0)cMh2MJ<hCd=#=6OhA;Ob4L<unJPd*qG%h)*i;a0iLw$Xl!j4_jY
zVuBbQC*O~`!pY&~@97s3@9DC6Q*0I^+W|%fhT^H4mEw;wDsV7JN_DXG@ZOM?pKmqO
z>bkVSMQMW#;+Lf@J6Nu8NZi<z=)}zEI@vj;Kxiie1A``0(K=9OaR(8*CLc}FWc1kl
zKBbU}ap&a3^i;+rlaHr6FnUks$f#vxo7|S6!1!SD@{Dp(h~8Ul<wdD^$r(jZ?Z%mg
zjB%S&GdHs^ZkQ~Yr_H~efq~%`dtzB?QDS;((U!@<d2WoGC(p{0WlY?>Bd?TQ^8m=+
zgCL>-M9c>fQ6K{BszV^w5)g5Ca#)!%+j<al*W~&#b;j1o%ga(2A8h6+PhjNU2om1}
zB34W;uaINhG<jNu0^|D0+ba^8jxbKXkSxVid}Wfg*yR72GLthZzcVULUQi`IIogIB
zO7k!ZPJW##JGmg6d$NBAD?hku07ZEvYbC2D+oo!9CfOMb3=Dpn+(kJcdp3ZGZ6Lx5
zM68}{S*ycV2V!oYTu`ehcaedCp-LzyKPkV&H!%<7dA<1b)RMqmObiUg+b6HD?J^J)
zn$9tiV}|e)-WvjfFy<FF20>vkM{qKChvN-Vu@0ABw-4$J0?L!`_e*b{Q1_aZ?FPu6
zo0A(`<fYKv0g|}|B5qIK)go&O3jZQ(VT=^AAl1>Jq`*=X1Y#eV%+qSmcxSUu>s}TK
zkomW`(^E?vlS@hya{^O}p#D|tl2!tn4vHvECa~69Oob`MpxRJDp~wJa)TPOBT@sAk
zo6EbNFmZ!bgUl;BJvpscf$`Yn_FgMS-OY!3!x$~V>4_5(wvb>1>4&HWv9^GSW1!T;
zR+N~RlAl|&5G1l|azMW^n>8~7Lvirr^^-z2-|hEeQbDo_qzJCJ=mm%i)_8xi_ar68
z3zPFENiklY+&M{s(RuQ!Nza&`F@aLc<~5TYnHXIsznj{@R>@S!q{*>)#q@ec5lx|@
zIUplIF1*E(oRe8lv}&@$Ofkmglf!4qLVO4ckD>#U>t`x3KAyZ_rV(2Z$aUhIZ_E^C
zWS`3Rft7(rZF9ox9gK{tC;yu#%g&NtkeXMtYqITJQ^xYi(ewQ`ADU~z$nhN%BkV;W
zH7bm|CM(Z3XKdUYJ%0_CF(_#j?FKmmTf~B#Rs;?Jq$r+jwaSw14M@v|$(5^Y85t*U
zU!}>&IQikKWXAr<uB($6A8cN+`Xno3{N$pIc8t3wZ`>HjcxE&6rrnH;yC(17Y{>X<
z^T*8^jBHOqX6%|Qztx=a#pJwgN|P(M&Xo~iWMI%_frLymNI@rvfGRcGX3h9+bKW+4
zCPs<L>vrfcewloGhacn4$yz&=Qo+913sRj4B94QI%^)HfM1V6i*thpV+)E(h8HjiZ
zA})i7CJ^xxMEn5}e2feXMFJo~2t<g22=U1qcdAP)0Tq!GpjEpVE5|3ZdPY{!PhOKh
z?35NzV^#bV!pO<0^hr#fRs6HdWa(Y95=yL|jGw$1IY5*Mn3S<*75!8-IbfF#pVlqT
zg3P@9<m8OZ{GtPsyLQQQgVQRgK~}VJ^OjvMjFVaSgmHt)iVw_;jEtYzCa3K&X1>k9
iKY89B2^nt|M*fcsAo6<@M+_&U{wD?yS!Be(zyJWrB?Sop

diff --git a/python/ur_simple_control/clik/clik.py b/python/ur_simple_control/clik/clik.py
index fa1f575..1e511a5 100644
--- a/python/ur_simple_control/clik/clik.py
+++ b/python/ur_simple_control/clik/clik.py
@@ -153,9 +153,7 @@ def controlLoopClik(robot : RobotManager, clik_controller, i, past_data):
     SEerror = T_w_e.actInv(robot.Mgoal)
     err_vector = pin.log6(SEerror).vector 
     if np.linalg.norm(err_vector) < robot.args.goal_error:
-#      print("Convergence achieved, reached destionation!")
         breakFlag = True
-    #J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
     J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
     # compute the joint velocities based on controller you passed
     qd = clik_controller(J, err_vector)
@@ -200,7 +198,7 @@ def moveUntilContactControlLoop(args, robot : RobotManager, speed, clik_controll
         print("let's say you hit something lule")
         breakFlag = True
     # pin.computeJointJacobian is much different than the C++ version lel
-    J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+    J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
     # compute the joint velocities.
     qd = clik_controller(J, speed)
     robot.sendQd(qd)
@@ -219,6 +217,7 @@ def moveUntilContact(args, robot, speed):
     controlLoop = partial(moveUntilContactControlLoop, args, robot, speed, clik_controller)
     # we're not using any past data or logging, hence the empty arguments
     log_item = {'wrench' : np.zeros(6)}
+    log_item['qs'] = np.zeros((robot.model.nq,))
     loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
     loop_manager.run()
     print("Collision detected!!")
@@ -281,23 +280,15 @@ def controlLoopCompliantClik(args, robot : RobotManager, i, past_data):
     if np.linalg.norm(wrench) < args.minimum_detectable_force_norm:
         wrench = np.zeros(6)
     save_past_dict['wrench'] = copy.deepcopy(wrench)
-    wrench = args.beta * wrench + (1 - args.beta) * past_data['wrench'][-1]
-    #mapping = np.zeros((6,6))
-    #mapping[0:3, 0:3] = T_w_e.rotation
-    #mapping[3:6, 3:6] = T_w_e.rotation
-    #wrench = mapping.T @ wrench
-    #Z = np.diag(np.array([1.0, 1.0, 2.0, 1.0, 1.0, 1.0]))
+    wrench = args.beta * wrench + (1 - args.beta) * np.average(np.array(past_data['wrench']), axis=0)
     Z = np.diag(np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0]))
     wrench = Z @ wrench
-    #pin.forwardKinematics(robot.model, robot.data, q)
     # first check whether we're at the goal
     SEerror = T_w_e.actInv(robot.Mgoal)
     err_vector = pin.log6(SEerror).vector 
     if np.linalg.norm(err_vector) < robot.args.goal_error:
-#      print("Convergence achieved, reached destionation!")
         breakFlag = True
-    # pin.computeJointJacobian is much different than the C++ version lel
-    J = pin.computeJointJacobian(robot.model, robot.data, q, robot.JOINT_ID)
+    J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
     # compute the joint velocities.
     # just plug and play different ones
     qd = J.T @ np.linalg.inv(J @ J.T + np.eye(J.shape[0], J.shape[0]) * args.tikhonov_damp) @ err_vector
@@ -394,7 +385,7 @@ def clikCartesianPathIntoJointPath(args, robot, path, \
             robot.updateViz({"q" : new_q,
                          "T_w_e" : T_w_e,
                          "point" : T_w_e.copy()})
-            time.sleep(0.005)
+            #time.sleep(0.005)
         qs.append(new_q[:6])
         # plot this on the real robot
 
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 af096cd0eb3d6119386239959f2086f901c097ba..387629268df78baa2af96f268f427ed3c2d093dc 100644
GIT binary patch
delta 3322
zcmdl{zowm$?=&wj7Xt$WgT-gtba}0bd=m9V3=9m@8B!Qh7;_k+7{N4C6jKUA3R4bq
zE=v>(BSQ*v3ququ6N5WL3QG$^3hQbnkO>S7QEXr_wibpIcDNWjSd62EA%znz#*xC6
z%96sJ%9h5G!qdVM#aYRy$@>yyh@U1u<1L<`{G|L6-^9Gc^wgq_9qLSMYzzzxMeLim
zGTmWh6=Gmu5ZXMAC4`xgbMk9;KLrVpq$G%t0uj;-3=G8}4GLgTBse*eLz+>0aw*4g
zM&Zc?+>)EaIMW%G^%xi!ZgHk%=A_0KB$i|p>4UT}GcYhHC@2&Of(+80{D4b|F>*39
zcMhZQ<^paG#uyU@1_n*`B6E<m1&FW&5vCx*3Pf0g2pf<hrXr995K}?H12a^Ifq|h)
z*u^(MFDE}SCBCF6F)KB>B)_OKVe%E87)IgEhP=Uy^&oqSK=D!p(yPf+1hV25TUl{Q
zVsiE^)~eK^{Nf@RkY%zULIp&~fd~Z<p$a0@K!iGo&;YrI5$v8~Mvw==-n+$Glv<ox
zQpE{!3z(S(3V}=p28M>o%>0>*!jp6Pqm9KG7#LPE6@hI4S-2AFF_1tp$Z=4Q-D0e&
zVg(shl{fh}zb2#bWL1G0T^Er3t{`)fm9kdGr<LYGYz8xnK`JyS35ZXADj>!p%)r1f
zxsYFMvZ$cDst3pf?8Yz_Lu@RrnA|9+%_uy1t)Qu-Jjk<9H-gx3H%|U7C@oS8vWOGp
zMTQ%KLLDr<Y(*e#`hG=zAbpHQpzza#Y6r35+Ciy>y*RZbz9g}<iW}rnD5n9*NMW%K
zmR^n`;mym192rAEk@)ie|NsAsJVA!D7QqwCE#}mUf+A&**OWj~%;3l=@&Q@Gm|2XR
z3U4tN6lYekgDfm4&TO7+BQhPDir<RJFo6?Fx>b+CqrOU4J52#dJcWS_1{qcaPNSd<
zQKSemn>i)F1d^24a}z5vb4zn?F&CHS7J&k)2%M6@dC3c;ADp4wK(1qjxDFKKMIfVZ
zF_zt8%qV7MU|<l3W-*Y47*NUvCxML4$@9g}F$z!4lX&C`4p?@G!$7IH$N{Ff2xNK@
zIM0F|i;+$k(;yL?HgWP)$sl;T*vu%E1IZ$qoDg&4Kt_V1s|cL*!KohX32-I=TLaDn
zkZ``mlu~kwqbxNizMv?z3^lY_ic3-psyINw4Ps3LMQ$57y*v;Uoh~*}Y=+8wwV7(O
zH7382E@KqloGjD8Aq0x(qF_)^F@f`IQSjuiDoT-{SWoYLyLG|$V;k%=rHer2!Y$U~
zjKqReNZtj<4mjAs5dbb#z_}O{PPaI6GV_WH5|dMlz!4Y%@*_B~lqMIb%G85Xk0VGB
z9JQ=PiFxU%Md0`YM;$1piXuSr;6lj><UC$*Oy1%wiBC?=D@iRX2B*PW?3F3;DXF>n
zw>T<Ope)wn_~O*UqEJw(VM~ioPt44_#gP_Yl30|UT5^jq>(*p$HDzINF{ulRcW?qt
z%Pd*E*-<T@Sq7AaHCbM!FfcH@6al#b97N?Hmfz$vnp!aiAf`5m02PQuAlE^X1322i
z$-y5a8weu6UI8VqB5*_}gTz3#u4E_zSp+W5ibO%8AVO<$kX8sASVDC28m+K;P!U#C
z1M)vK{+Uy93yKm!QHzq(*h&jh5=&C6xc%}&ob@1_Gayx<l-|H_gI@q_ou4L4Q6<Q-
zDiBc(A{s#i$Of<ih<oZlVjx2NG{_8g1_p)}hR;lltb!0lb(8&BB{y%>=3rz@n7m6z
z*%O=^Kt;qY*0T8A#EM(2i6Bao4IF7jtswJ2NwKH{#OeeQ?I4qpt2&UN!DKaEMaGMh
zJ#+&Zg*VUA{lnx4&V!uboDa!<;4A?)0vxPh6T$f#9KfKQq{#)b7F4_yB~HF>V9qEw
zncvWs@y2FfLjguc;mvVIRqXYkG+qRXTurf}Ng&(7NteC2urxKbD)km~VqWDf=G?>z
zaKU<uIWsTq7ISW9UQrH6E!eYciI9{Fs(5a(6c-ki6oK;q*kNENf;}At(hvzEK%TzE
zlAo7aT+|A-SsK};AQjypq6b9uf`~p4F#$wO1QDT=yRF=~K~+vsDTrS-`HodO8#pO9
zPWH5xXMD6d(>j6K4U~fXG#QJyK`Dm~J<7oe80=$gk>3n57(|?(%x^Cx4l=3;#J$B@
z1Zvt8fkHKzbuy!%h)5G7xVgnx%bLkh%a+Md%U;7cIe<}Q@_IcEmRgP)#>oe{!5p?)
z&Kkxn*2#)eQY^JxHB6Hq8p-I@a@R0sfhsgm;|U3ufEo{A{%TN_&%nUI$WX&ESy57q
zCyNg(4kjk=7L^f8V}z-xVM}2Gl~Of~S;8QDK<ev})q^!LFfe3+GBu0?rBRGw1{=d#
z!(PLd#vII`$x;=}1*$8O^YcoI@^gIh^9x*j1HeMM_yCtey@G;5YDH>tX-TR=qC#<M
ziEe3uLP~A{Sg+eHro!S|%qfM%n%uWo(o;(UZ*iojmW0HY$EV)nOiwKdFG|fz&L{#E
z8MoMqQ}a>+Q*N;pro<=brhqHQTdcYHDXBTPnDPp5v4Zrc++xZr)8x6ul30{pT(pXT
zp`HO8TacoVvmmj!Bt9jvB=Ht!Qc-GRwp&hO`Yn!}{Pg(DlGNN=e8q`np!Ot4QD$<<
zEyluItPsbrm4jV!i?uMmC^fB^2h<J&1qc*_%ZyumsKHZofPsObUYwDEp?D6s4*I~u
zASl-0d4pfD!Q+Ogc!S>q4&HvQPOfgA2FDvbg8hD-ewTS<8eDJi3iZc##$V=@YjAtO
z!{6`G>Cx@g;QD}tv)!lBXNuSr7KsO<V$&lhMlN7o&b^R(Ip0FQ3-Sh+MGYG~Izk%#
zZiq=w&z_h)IrqAlcKt;$?G>TxqgO_+k6RgcLC5y8m|cTMM`)w}4Q`PMc?&WwaO+)Q
z(E}H1nq0S7^Gb6IDj`J;FR1pADlLjH&de>yNsWj4PA?bIvM(+I6~9HG_B^EY29=iJ
zYy$Q;xX3ON1~;QYx$_o>O>TZlX-=wL(P40UW?(1=)fOL^85tRGGYH*g;J?dYJh|Im
hN&X{)D<h-GhX5uKM*Rgc9~eX!^*0260a0LQ0ssZv_NM><

delta 2432
zcmZ3}&bXtV?=&wj7Xt$WgZeb<^qcAv`6QCU7#J9)Go&!2Fy=5sF@kBPD5eyq7KSKh
zCI)wg6y_F&6qeOYAYBX$Q7m9F))s~oHn<ooSd6`eA%z1j##YIw$@vnb$!~H3uh_;@
zf=q0z3=9lKY?~i4-C<-EWME(r+<b^7gqe|JvN(sIjW|eB0z^oH2q^{zhGI4b1_lK%
zC=y^`V7SE*AD@|*SrQ*##pB`|pa<h=O<u@xoKa|!sO07*&U8j)U65MNw9K5;_=3ce
zj3PadS<Dc#zy=j*P3GZNVvLxq&z%F-H+d`fdk<p<1_n*`A~TTn<{-iXM3{gGOAuiN
zBCJ7*n2JDFK+FXNjDiBhSZxLdhAQF7g4`mLcw-oaHplV?GbVtHE#d^}1u50!DFT^z
zi><7<Br!Sr7Hd^%QGRieG)P_sL@0v@Sr8!)B2++xDu_@65$Yi88Nt>UGlG0nBrsWk
zUz9O*vKoI8qtN7;{LvO-3=9k_nTo&$fy`YAbuLJt7~}@1b8j(LRk4D?u__l7KnjyD
zh>J{i5D;J#nj9!lBjgOS&jlm{(=++LfGArLNJJB?mQPTUQE0M;V1lYUNG+xYP&_ae
zLo6vSpS)I3n^9=;O+ix+IgmG?&H}MfoW)U`S`we0pO^!QWf-ppWHBfl8yIc~3U#pb
zvK4_0(+@841)0WJ1PU)rsG%S>ilOWvV@eWBtGGcvhH~nWj1(5@VCm&35@KLrNM@eA
zkXvN)8zBwG5K!#B{Qv*|{~`~NEv!ZGL~)BbwW6R%3FLD{kQ6gGR*JmAp3W>rPH(rE
z3yL$V*g-ZI6lXR~&JdZ-D6~n`l&KzMe7aSS!K1!PS36AsNTh{=%mkTJ1WtyatWcx?
zGL|_dzXXze*mDyrGIL9FZ!s5_<`#j1un3%bz`4j1q#vA>TtN<Fg*Xfp{Y4<7Z!wnL
zV$3K8<wkKt<|>K?C0=lP$mp1STKpWN(BxSXk6gil%MNiEDCHK}gG>i0E&`ce1kSBs
z$D*VakaHQ+AW@$-Ve&!AAV#6hB2ul4<{&R?azYG@1(^kkt0HhZ2Pbi`>%l1=Y(6-}
zGZ*Pjz920n3N8g$ic3-psyINQ31Us1{9n4CQD}3&Oaq4y$nc^dkgu7FK*_c!XtJWJ
zQY0wu(tF=-UGV+b20KmZB2X^A#af(^Sda?Idf<QthafmC!DRqA6N0>aiz6p9uecyF
zIkgBJ7r`J`gZ-^Id5)@#J2(wEfCRy@#9EY?m!4V#jtX!rfC9ZJ93&4eLL5QP;{`{`
zEzXko<kY;9)S}`drOE$Q<rqUI%c<E2gZx>f1BwuEqDjjvS+qG<EuUEilsq+AUZyZG
zFuW82nF#i98HnXO`G=-fj6R5|1tLI&Q4z>4NIZdq6&z1~AlU#A0d^QTD!~zv1QG+;
zx{?8$4<Lma*cuR_Ik`?Ngbn0^B9X~Aw8H8^1x-;k$TyI*e~URKx1cBil&MfsKU--*
zN@7WB6}Ml0h_fDqa~h-ylmr_XZtx3$t@G1lDXIWDp%O$?frthW0kQ$C0OFonkQj&%
zKLs*_oq>U&h2b+3BdZ`pQSIbvR>{qGwK*6W<0n7UQLYEaEhyXGVl9i$O{}=Znh2sa
z*}zdy)B-ZE6-2ayhz=0Z1~M6~_9+4dZIw)jb69GQV{)={eqM1&QDSCZY6`p}x&YD!
zN^1=aH)ORsSgvqL-4LJLp=%2cHE6MNU-vPSBRE%ag7Y#YLxIye*luuqfSm%)vEY~i
zWf4s-h<ibWR#C!aW<zsEfyvf}wv5*|R~ia1G74?(GOA*)2U%YPN)?)7MH4}O0jFU0
z;=<C@)T-25%!zrGx0rJiE5OC&E#}O;v|G%%nR!LoAhqC7VoQW1;0#b6W+^T#Dk%bI
z1+c@wP6UTmBuGO9hyaDwEtdSe)Z(HRkZqdM$Swt`=mHVlAfg9E^n!?f5HSHngiPLU
z<;D%FSc*zO{L;xB*6D2EJkT(?)LNeL;pVB<3CwPwr01u}SOjXpXtJTF6;Sj*e2gvC
zG=U5T5$7gb+e;;Zj4A?gHQ8^m=9T6aR6^o{2c!(tHYoz7_##j;11ke3b4dCEx$72(
wO>TZlX-=wL(JBT822f303<~oP%#4hTw;6<PGw@H&c2?s4#Nf)vC<4|80GosS^Z)<=

diff --git a/python/ur_simple_control/dmp/dmp.py b/python/ur_simple_control/dmp/dmp.py
index 5d2da4b..11f8364 100644
--- a/python/ur_simple_control/dmp/dmp.py
+++ b/python/ur_simple_control/dmp/dmp.py
@@ -1,5 +1,6 @@
 import numpy as np
 import argparse
+from ur_simple_control.managers import RobotManager
 # TODO:
 # 1. change the dimensions so that they make sense,
 #    i.e. shape = (N_points, dimension_of_points)
@@ -273,3 +274,26 @@ class TCVelAccConstrained:
         taud = np.max((taud, taud_min))
 
         return taud
+
+
+def controlLoopDMP(args, robot : RobotManager, dmp : DMP, i, past_data):
+    """
+    controlLoopDMP
+    -----------------------------
+    execute a set-up dmp
+    """
+    breakFlag = False
+    log_item = {}
+    save_past_dict = {}
+
+    q = robot.getQ()
+    T_w_e = robot.getT_w_e()
+    wrench = robot.getWrench()
+
+    q_ref = dmp.step()
+
+    robot.sendQd(qd_cmd)
+    
+    log_item['qs'] = q.reshape((robot.model.nq,))
+    log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+    return breakFlag, save_past_dict, log_item
diff --git a/python/ur_simple_control/managers.py b/python/ur_simple_control/managers.py
index 7e2693e..898d1e5 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -1,3 +1,4 @@
+# PYTHON_ARGCOMPLETE_OK
 # TODO rename all private variables to start with '_'
 # TODO: make importing nicer with __init__.py files
 import pinocchio as pin
@@ -15,7 +16,9 @@ from collections import deque
 from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
 from ur_simple_control.util.logging_utils import LogManager
 from multiprocessing import Process, Queue
-import argparse
+# argcomplete is an external package which creates tab completion in shell
+# argparse is argument parsing from the standard library
+import argcomplete, argparse
 from sys import exc_info
 from types import NoneType
 from os import getpid
@@ -311,14 +314,9 @@ class ControlLoopManager:
                 # (because i did know about it, just didn't even think of changing it)
                 if not (self.args.pinocchio_only and self.args.fast_simulation):
                     time.sleep(self.robot_manager.dt - diff)
-
-
-        if self.args.debug_prints:
-            if i < self.max_iterations -1:
-                print("success in", i, "iterations!")
-            else:
-                print("FAIL: did not succed in", max_iterations, "iterations")
-
+        ######################################################################
+        # for over
+        ######################################################################
         if self.args.real_time_plotting:
             if self.args.debug_prints:
                 print("i am putting befree in plotter_queue to stop the real time visualizer")
@@ -330,9 +328,14 @@ class ControlLoopManager:
             except AttributeError:
                 if self.args.debug_prints:
                     print("real-time-plotter is dead already")
-
         if self.args.save_log:
             self.robot_manager.log_manager.storeControlLoopRun(self.log_dict, self.controlLoop.func.__name__, self.final_iteration)
+        if i < self.max_iterations -1:
+            if self.args.debug_prints:
+                print("success in", i, "iterations!")
+        else:
+            print("FAIL: did not succed in", self.max_iterations, "iterations")
+            self.stopHandler(None, None)
 
     def stopHandler(self, signum, frame):
         """
@@ -353,7 +356,6 @@ class ControlLoopManager:
         print('sending 300 speedjs full of zeros and exiting')
         for i in range(300):
             vel_cmd = np.zeros(self.robot_manager.model.nv)
-            #self.robot_manager.rtde_control.speedJ(vel_cmd, 0.1, 1.0 / 500)
             self.robot_manager.sendQd(vel_cmd)
         # hopefully this actually stops it
         if not self.args.pinocchio_only:
@@ -494,8 +496,9 @@ class RobotManager:
         # last joint because pinocchio adds base frame as 0th joint.
         # and since this is unintuitive, we add the other variable too
         # so that the control designer doesn't need to think about such bs
-        self.JOINT_ID = 6
+        #self.JOINT_ID = 6
         self.ee_frame_id = self.model.getFrameId("tool0")
+        #self.ee_frame_id = self.model.getFrameId("hande_right_finger_joint")
         self.update_rate = 500 #Hz
         self.dt = 1 / self.update_rate
         # you better not give me crazy stuff
@@ -596,8 +599,8 @@ class RobotManager:
             pin.forwardKinematics(self.model, self.data, q_given, 
                                   np.zeros(self.model.nv), np.zeros(self.model.nv))
             # NOTE: this also returns the frame, so less copying possible
+            #pin.updateFramePlacements(self.model, self.data)
             pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
-            #return self.data.oMi[self.JOINT_ID].copy()
             return self.data.oMf[self.ee_frame_id].copy()
 
 
@@ -674,7 +677,6 @@ class RobotManager:
         #pin.computeAllTerms(self.model, self.data, self.q, self.v_q)
         pin.forwardKinematics(self.model, self.data, self.q, self.v_q)
         pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
-        #self.T_w_e = self.data.oMi[self.JOINT_ID].copy()
         self.T_w_e = self.data.oMf[self.ee_frame_id].copy()
         # wrench in EE should obviously be the default
         self._getWrenchInEE(step_called=True)
@@ -721,8 +723,9 @@ class RobotManager:
                 q.append((self.gripper_pos / 255) * 0.025)
             else:
                 # just fill it with zeros otherwise
-                q.append(0.0)
-                q.append(0.0)
+                if self.robot_name == "ur5e":
+                    q.append(0.0)
+                    q.append(0.0)
         # let's just have both options for getting q, it's just a 8d float list
         # readability is a somewhat subjective quality after all
             q = np.array(q)
@@ -883,6 +886,15 @@ class RobotManager:
                 self.v_q = qd
                 self.q = pin.integrate(self.model, self.q, qd * self.dt)
 
+        if self.robot_name == "gripperlessur5e":
+            qd_cmd = qd[:6]
+            if not self.pinocchio_only:
+                self.rtde_control.speedJ(qd_cmd, self.acceleration, self.dt)
+            else:
+                self.v_q = qd
+                self.q = pin.integrate(self.model, self.q, qd * self.dt)
+            
+
 
     def openGripper(self):
         if self.gripper is None:
@@ -928,7 +940,6 @@ class RobotManager:
         # define goal
         pin.forwardKinematics(self.model, self.data, np.array(q))
         pin.updateFramePlacement(self.model, self.data, self.ee_frame_id)
-        #T_w_e = self.data.oMi[self.JOINT_ID]
         T_w_e = self.data.oMf[self.ee_frame_id]
         print("You can only specify the translation right now.")
         if not self.pinocchio_only:
diff --git a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc
index d2682d9cab363be813bfcb81248a50cd94b44b00..05d82360395cf608e6ad774a2bfb5c43c0ebd23f 100644
GIT binary patch
delta 1611
zcmZ4K{62*5G%qg~0|NuY{l7NpSF|?r?dNn@$y~(5z`y_|((g%>O|U(C#2!VEp+R8U
z?amy#mCWe}R;TW6*<oxCRt+XrPUhv35L?Nd-Z(pEL4ncj{a{frv2wCCmki^|$q`&K
z)+?FQPagillJKt4KK=F7lv2(8M)v8#Qw~RSy4(X9eBau3jZ-<VeY)SfpB{1*JNKtw
ze!Q>z?=2nsm6O+VNieRQd;(<F=BHeG%#7ZfMfl8_xmg()7?>Ft7(QpQO%4;(V=7^t
zoFObDl+KXCw1{yvBgmf&47H31VP+&@rV`Fc5;9CB9Fy;{$}nZ|PYw_k5rb)}VO$Lg
z69xu`T4sp$$p%b<j9~L8$1w>qqUl}3qzRD(S<Z%J3p-S|0yBo5JZ7k#$y=DAMuT*7
zBI)KvccTFdOivjLRL|rkEEq<Af-AFNg(*v8#Zb0|RfD;Pxn{B>o9N^SW)aR5J{txG
zh8i{`5q_`;Cz6N&ScC^jL=Y^(4;Ps{frFh%2*F*)!Nyg>35g8`hRGkqL>R%6+_2ED
z6$bNJY8bM_CNE$Vne4zWS}&Z&63n0};&+R=w8*rG4U|IJK?Dbg0GU<<O2|drATAGx
z-~|zUAc7x62!IGd5Fr90L_vfYh!6)65+Fj7fq_Aj_m)s<MPhD2PHKEneo}r(d`e<T
z;w_d*Lag<2AVu;ZLIFf5f(RuLp$sBiK!gs6&;=2$Ai@n~qR1^)ut`NmAR%KAVFDsd
zL4+BIFb5G9Ai@$vSb+#@5CICwB3ls4j)6gip~xP@bN~^KAi@bmID-gJ1_p*=kjExB
z@`_j%c`-0B+!9JpEpac(EGS4V%1JFQ4h=F*Eec{_V8{fi)nsB|XkfU*BX^xg`67?<
z0@uqtS`Dt7^ZD`_>s1XKT-&`Hy|1&#Uu2PAz<7~G>4vHi7BK^=irwMn>$m8%n84*a
z-Fu?<Wp0HAm&pl25<DgsxZN+XxEJ{_FfdFuU=-UtPv{GyKR60hKz>&R5o#bp9Ykn=
z2v8JgvK8rr*oGhi96RthDFTNVC=n@uL6O(wha#e^Aq)%*lP3QbS;^?Ld8z1aRyJ@d
z5t-a3?co#yG9?-mt}IE3#i>O?Aigjte&vh2L2QtNZn662r=;c-X@S)7x~Jynrj`^{
zLc|m&3&?b{LDJCVg)(t$njl@;lUZetFh)&2C95S0b|WasfSp(54wCnn%qb@$4~|da
zjMSq1y!ga|f};G2%-qD1%>2BfCI$wE6`Nh;I++-KCZAJ~5{Eb=uQa!yvIu0^E#~6N
z;v(tE{}t5iKrvtB4>A_)Vo+iz0{Izi%1Q=s2t(2ihfQvNN@-52T~QYU0|Th&EViGV
atY{>v#OT5JDTq;o(d@I13nQZySPua2ZG#K|

delta 838
zcmaDKveKFFG%qg~0|Ntt<qDg0K9!Ap`#BvnnTwbh7#P4r`aOxV3ASgC*rNzCGzd()
z-I-&j$((**b?WYx9me)x)nGz%GB1~em?m?2<LsCP1xB~`gGIrF=45Lw8Ai>?5nM9X
zn#}1Z4}W1vc-Ls3{(5Rkspfto`*h(chod=N?tu)xZ*9BAshrn7-S6E`54nn+`_nH!
z-dFzjmX5vV<n>$<jGB{AfXv$bluM79(SEZCpE)x>3j+fKGXn#|=MDx2hUpBGYo+BT
zR|xVj)=b_YEjC$DSd^uPA&X=30!ER^pF~9J(^!HTG@1NvF_#vZYO>uDO07uDEyziY
zFUn8KFNse{EJ?h@l9Qj9Qe*)N4oeVW1tM%27#K7eZ?S^a6*Dq0Fent+F)%RP5=u`k
zaWBd&C`c{JNi8l84KhtFa${g%$Yfw(C^ltaU}#{t!y|W{NBJU;@&Z@g%RE{Qt~a;^
z`mH;yXRuzE(7h<3yQ1{EzT-uG#{;IvtqxjU;P$=1;#=gvz`!usfKhC7vh)|mOc9V9
zBtV2Dh>!vi(jY<xL@0tB$5x~aVyl7(ERHW`1G!lN1RxIQPfsn0&qyuG&x_B^Pf5)w
z@&M_ad_Z<3qr>Jbx!bI4oFD;?$=)g+dI2D&Kgho<Nr}a&MIgh9*g@9H7ukd4Kn}dc
z>I*hVZt_7DVMfu(S5>;%gg}zQlhah=*knOW`N^kLk1+a8?pM<i1-lX+x<%F?d7H`S
z)MOa#CcjpbWG!Q0VCdM)sou$C1PTpJfm`hH@hSPq@$t7f3o`Talan(t^NS!h=auFb
zR2G43yTx2wSzN?F`G}^PT`b5*XAl8)Bq&}W-UJ!Fk^vmZkkH_;$<0qG%}KQ@s$*ba
f0L5>y-DC|dBM~M>KgLf&B8+CAbzB%3t-x9Uwocq=

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 0813aa172509b934f2449f5619bc850471096527..a70360a9ceaab78c4f4dbc30377f789bb8b68029 100644
GIT binary patch
delta 174
zcmX?RcGryeG%qg~0|Ntt%X-`N%8k6dLQJn1CdYG2Pc|3wV04_^AXK7S!?=cdH48`)
z0|O&Nk24EHC4(l5U&t+{!eULvTdXCSB{`{+d4!7?vnJOINArGQV5njAW%|az!0X4f
z`L3`k3!}?qW(gZchs_QWY)p)flij6!nEaR~PnFVO^I%|LDDt0tSjto?5yX@Q5z-6{
W47WIJa`RJ4b5iY!QYH&Yn*abgnJcmY

delta 132
zcmca>cFc_TG%qg~0|NsC%NCpTgpItsLQKyXCdYG2Pc|3wV6>myAXFl;ni-^kfq{{s
z$B~7hl0lQ%FXR?eVew>1;UdO_$vwi+%vX6zHh&RTWnpxlEG1#XXummJf{lsMVRD?5
k4^t`Q<TX+nZ0-yU3`KsEZ%LUlCQKHPmS9YttSM~*0Ih!^NdN!<

diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index c53ade8..afe4dcc 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -104,7 +104,39 @@ def get_model():
 
 def getGripperlessUR5e():
     robot = example_robot_data.load("ur5")
-    return robot.model, robot.collision_model, robot.visual_model, robot.data
+
+    shoulder_trans = np.array([0, 0, 0.1625134425523304])
+    shoulder_rpy = np.array([-0, 0, 5.315711138647629e-08])
+    shoulder_se3 = pin.SE3(pin.rpy.rpyToMatrix(shoulder_rpy),shoulder_trans)
+
+    upper_arm_trans = np.array([0.000300915150907851, 0, 0])
+    upper_arm_rpy = np.array([1.571659987714477, 0, 1.155342090832558e-06])
+    upper_arm_se3 = pin.SE3(pin.rpy.rpyToMatrix(upper_arm_rpy),upper_arm_trans)
+
+    forearm_trans = np.array([-0.4249536100418752, 0, 0])
+    forearm_rpy = np.array([3.140858652067472, 3.141065383898231, 3.141581851193229])
+    forearm_se3 = pin.SE3(pin.rpy.rpyToMatrix(forearm_rpy),forearm_trans)
+
+    wrist_1_trans = np.array([-0.3922353894477613, -0.001171506236920081, 0.1337997346972175])
+    wrist_1_rpy = np.array([0.008755445624588536, 0.0002860523431017214, 7.215921353974553e-06])
+    wrist_1_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_1_rpy),wrist_1_trans)
+
+    wrist_2_trans = np.array([5.620166987673597e-05, -0.09948910981796041, 0.0002201494606859632])
+    wrist_2_rpy = np.array([1.568583530823855, 0, -3.513049549874747e-07])
+    wrist_2_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_2_rpy),wrist_2_trans)
+
+    wrist_3_trans = np.array([9.062061300900664e-06, 0.09947787349620175, 0.0001411778743239612])
+    wrist_3_rpy = np.array([1.572215514545703, 3.141592653589793, 3.141592633687631])
+    wrist_3_se3 = pin.SE3(pin.rpy.rpyToMatrix(wrist_3_rpy),wrist_3_trans)
+
+    robot.model.jointPlacements[1] = shoulder_se3
+    robot.model.jointPlacements[2] = upper_arm_se3
+    robot.model.jointPlacements[3] = forearm_se3
+    robot.model.jointPlacements[4] = wrist_1_se3
+    robot.model.jointPlacements[5] = wrist_2_se3
+    robot.model.jointPlacements[6] = wrist_3_se3 
+    data = pin.Data(robot.model)
+    return robot.model, robot.collision_model, robot.visual_model, data
 
 
 # this gives me a flying joint for the camera,
diff --git a/python/ur_simple_control/util/logging_utils.py b/python/ur_simple_control/util/logging_utils.py
index 31c7e15..da6ce87 100644
--- a/python/ur_simple_control/util/logging_utils.py
+++ b/python/ur_simple_control/util/logging_utils.py
@@ -105,7 +105,7 @@ class LogManager:
             self.are_logs_vectorized_flag = True
 
         for control_loop_name in self.loop_logs:
-            plotFromDict(self.loop_logs[control_loop_name], len(self.loop_logs[control_loop_name]['qs']), self.args)
+            plotFromDict(self.loop_logs[control_loop_name], len(self.loop_logs[control_loop_name]['qs']), self.args, title=control_loop_name)
 
 
     def findLatestIndex(self):
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 6a671df2b112d230043f2a4c639f5bcbd088f65b..a5a1f53946b06f0ead307dfed8f5505c1e54eb9a 100644
GIT binary patch
delta 1575
zcmX@o%s8Qik@qw&FBby?1B3Q@+w`*&c_kT(CaQ<9a;31gFixzKW#ryC=LVA)3rGbN
zb2Bh7Ff%YPeBLv;i&>A6W%5pDZAaExwh~UL0)`sa)r=4}k{BCItcrnwp_V<9p_Zcr
zWE5Co7SH63g2KGaFg1(}CH#}6S!7u`Yq?6eCL74fG73+=$R|8GkA;g-Wb#2yVGhO=
zrW%GCF3HLJxWp&_lVcNE4RRC%0|U&ATJ93=$$Ew|lkW?1^RpwlgfoR{GLMmhcnwDl
zmkk30Ly0Wd4GatnC<adUXXR(*0V$e%o>ju6h6OCkz`&5gT*KJPRKt_TBni^PUBd&W
zxl&j_UPH2oXYzVhSx$}`E|7ed;$%iP8G)WhEDT8gR+`MncGXzu7E@lqEw;p>#Ju#>
zB2fkghFi=zsd=}Ui%W`bF&E^N++r=sEXhf|#hIE{nwwgbSdu!KiQR>fcd|RX5^Iqn
z0|Ue4Om+uWUIqpRg~@B#6}Ze97#P$U7#NDpCSPZF<YVP%_iXg+NWH=$az|WYvlPcw
zUPgt<A4DDNl|YuUlvEa^-eS*7Nh~T#tSnMvU|=W$g=Uc^NR|~MstgiU0THSoQSQW)
zl=$M(q=KCMk|GU|2uE>hNql8aX6`Mf#EK#<kPv%fQAuWTNpX=jh{uvrlvsX?CoQM6
zI3qr_EH$sBc=86ZKp}mQBx_-5YH8{%=CsW8BA>~^;+X~pAiIP>gfNIO1QEs{!UU{7
zGcP5zqR12^5CS61Ca)29V9jD+U~u02QG5y~n>zyoLzT_sL`Ct*ixd}fdx8}Cfe4Su
zeo7%SaSRL$njA$S%ZqYA!nq(K4@4wP-lt^4+RVVfuypf(rG9qRHjpTs2+?G`#gbo;
zny1N91Q#r7o4i>ofUN+max$lOF2w$z$rrShC(qZO!WcN&L`Q+KWOAsEdSW@q2Rxwg
zDk#b?NG&R<ECRc%4y4-`M1WoE4`RuK2oDfZ0U|0v1Sp{uRfAYyU+`l1V)9)b1;&QS
z|8yi6izbWf+OW=MU|=}0IYRe4H;)Yi1A||Y6-c-B<Y-G>HWv`nb#kBOV+*jgVIY1u
z$ccQ(`305nX+`<D@kvSf6-5!CFk`vJ3XZ&@OprPs5Ro`}rIk5j+~kK=ag05a9jxUU
z-6tnlr$CHT0IBc-5#AsItN~P@fD?KV*n6N<SOoU13`j}9WJVj|dT=rf0`bAgixU)F
z;A{X+mf!?d1j@R%*b^(tax(K$i@@msl+=pgUMN}wG63Z6Tim$?Iq}K)d3mYHB}L#s
z28Svr;BWC2r52au7p2A*rKV@*=M{xAFw`^L0$W*;l30>h6az9Q7DRwTyeJ+N5Mtn9
zzr~!BpI$TtBsCR8q=1N25P>W0A}3l<C<_(jXXcf}XXeG1Wag&EgT0c;z{0>#v}CfN
ztsEaX#1cWdSqzlZ4~mOS4zv|v-N3-Wpt?EB){>cx5mE*$a?oN_n0(eD+o}{~Kd2xn
z0+j?spj=S|O3y{0bWjA+xsss>WLl93$ORlWx%nxjIjMF<J0}-8>al;;5ohH8WHEWU
GqbvZTp>E&+

delta 1518
zcmbQx!g!>ak@qw&FBby?14I53oAi|vc_kSOCaQ-pv$ilyY>;K-+Bok96C?9vMgtMX
zNt1h+^%$8a?`GCEXQ^c^;RNXd;To3Jj3AnU0ZEJ%CRWA3z);JU$xzE)!adoYMOK-k
zma~KhqMm^vix0*^&{=|$+gW5MODPI-rZA>3)iBg>N={zDA~E>}ix@Lk3Gd{8TryH6
zk}wT5Y)IB}q%chmG*S?+VXxt|VPIe=k%lXS8&k$A!pRL%05W3odwEe~X0RO$3=ApE
zHH@uHHQZ@TAh{Z@8g4MnnZg3{29iD8lW()iGPBokPVQ%wnEXJ4OQ7cw3quJn%(g6r
z$&73=lRepX>ImLq$}70VmROXSm!4WA%D}*Ii#aDX?-p}$NzpCN)V$K%)S|?a)LYC2
zIVF=#*<BcUCRed5O<u_Ezyb=U$<NpoxXc(B7}OaU7>czf3vf6xNhofP=eWwts5seF
z%rQd&WF$*TWkKpK_PmtDqN2pgB2@+kh9YJVp#hR*g@`JF1e8I93P_YYF(oCwxHPFC
zC%>c!Wc)3T;?$D(%ACyHTTF=+MVcT*?1@DsnZ+f=MOu^ZiDd}sfy7t~OH)fzZ!xE3
zrWbinju6i@&<7bR1R{h%gaL>!0ujbw^_h7osTD;gAc0^IVLJJvxC3h@0|SH8W;2N?
zoQ!UhPb==__5jKGf(ZA?la)ebVi_11G&zbub{1uWgmXYdE{KSq%&cs~+Qh)Xuw=7~
zazDFjD@YVhglIC}V#zN^&C_Hlf(sV4PX3`4z?KhIIoVG;m(gwV9_>Ah0h23r6c~#q
z&(cw^F9W%k2NVniMfnA(MJ1I*U<cKLH2HuCup|9IEO`*&4kF4yL<NYb1QDRbQUrE9
zFE+>D;!Ms>iHC$CXJT?Tl*y8wnlGPQR1b0iOJ06T>MfSC%qpW>OofKGm<o-G3K<v}
zCO=@8m|U)_$U2LGf#LY(xw_xEd8`>282pMXK^9w0USO%q<_u!GOulUS7!snPAb!~7
zgH|Gv;h+#@xy71VmYP>mlmU|P1`!F9-&vV6#!gnVj$`be+-fb)=r(z&b&4(6vx*=U
zo*=>tM1VE0fLNf!Tm%jUP#P=(2Z{_x!XHE^O?I>qt`7t;!HJ6#6sjeeB{``@phO8y
zUPZDX-?ArGl;vdRr51q`hZaZ&#LXbqLJ$FR_bu++f}Hr|{Jgx><dPzATz~@~6hF84
zic*V9@{3aAi&E1w^Ye;AK<085r`DImSEMACBo;-3jEMmepol4o0|kT_ICgF^=j5js
zO$JF#0TIa{A_YX?O3=tD8Dx)8L4IalNqlBrd`V_*YCPC0;ILiHFge^-mJJ+Y36raA
z4OrJRFfgcWUT<s3Jo&wYCZpnHNyluf5|Cx!zyxLVB2d040wwJtP%IaLw5(((5(6m`
z0a?dklbfGXnv-f*v}5u<M?IBKI%16cpUmPJ8HGQ&Fv~IOesW_BWMtI+T*Z{c$S4ff
F2>|kcWH<l-

diff --git a/python/ur_simple_control/visualize/main.py b/python/ur_simple_control/visualize/obsolete_to_be_removed/main.py
similarity index 100%
rename from python/ur_simple_control/visualize/main.py
rename to python/ur_simple_control/visualize/obsolete_to_be_removed/main.py
diff --git a/python/ur_simple_control/visualize/make_run.py b/python/ur_simple_control/visualize/obsolete_to_be_removed/make_run.py
similarity index 100%
rename from python/ur_simple_control/visualize/make_run.py
rename to python/ur_simple_control/visualize/obsolete_to_be_removed/make_run.py
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 0272ad7..4cf8d13 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -36,7 +36,7 @@ def getNRowsMColumnsFromTotalNumber(n_plots):
     return n_rows, n_cols
 
 
-def plotFromDict(plot_data, final_iteration, args):
+def plotFromDict(plot_data, final_iteration, args, title="title"):
     """ 
     plotFromDict
     ------------
@@ -52,6 +52,7 @@ def plotFromDict(plot_data, final_iteration, args):
     ax_dict ={}
     # NOTE: cutting off after final iterations is a vestige from a time
     # when logs were prealocated arrays, but it ain't hurtin' nobody as it is
+    plt.title(title)
     for i, data_key in enumerate(plot_data):
         colors = plt.cm.jet(np.linspace(0, 1, plot_data[data_key].shape[1]))
         ax_dict[data_key] = plt.subplot(int(subplot_col_row + str(i + 1)))
-- 
GitLab