diff --git a/python/examples/cart_pulling.py b/python/examples/cart_pulling.py
index 08d59cf3f1924f6f3aa1ea02dbc239b562c2246c..3faa03fd887c0f00fbb5baa9c001cc45b08dc145 100644
--- a/python/examples/cart_pulling.py
+++ b/python/examples/cart_pulling.py
@@ -66,17 +66,26 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
     3) parking?
     4) ? always true (or do nothing is always true, whatever)
     """
+
+    q = robot.getQ()
+    T_w_e = robot.getT_w_e()
     # we use binary as string representation (i don't want to deal with python's binary representation).
     # the reason for this is that then we don't have disgusting nested ifs
+    # TODO: this has to have named entries for code to be readable
     priority_register = ['0','1','1']
     # TODO implement this based on laser scanning or whatever
     #priority_register[0] = str(int(areObstaclesTooClose()))
     # TODO: get grasp pose from vision, this is initial hack to see movement
     if i < 300:
-        grasp_pose = pin.SE3(np.eye(3), np.array([9.0 + args.base_to_handlebar_preferred_distance, 4.0, args.handlebar_height]))
+        rotation = np.eye(3)
+        rotation[1][1] = -1.0
+        rotation[2][2] = -1.0
+        translation = np.array([10.0 + args.base_to_handlebar_preferred_distance, 4.0, args.handlebar_height])
+        grasp_pose = pin.SE3(rotation, translation)
+        priority_register[1] = str(int(isGraspOK(args, robot, grasp_pose)))
     else:
         grasp_pose = robot.getT_w_e()
-    priority_register[1] = str(int(isGraspOK(args, robot, grasp_pose)))
+        priority_register[1] = '0'
     # interpret string as base 2 number, return int in base 10
     priority_int = ""
     for prio in priority_register:
@@ -113,16 +122,33 @@ def cartPullingControlLoop(args, robot : RobotManager, goal, solver_grasp, solve
     # MASSIVE TODO: 
     # WHEN STARTING, TO INITIALIZE PREPOPULATE THE PATH WITH AN INTERPOLATION OF 
     # A LINE FROM WHERE THE GRIPPER IS NOW TO THE BASE
+
+    # whether we're transitioning from cliking to pulling
+    # this is the time and place to populate the past path from the pulling to make sense
+    if past_data['priority_register'][-1][1] == '1' and priority_register[1] == '0': 
+    #if True: 
+        # this is not the right spacing
+        # TODO: this ain't right
+        p_cart = q[:2]
+        p_ee = T_w_e.translation[:2]
+        straigh_line_path = np.linspace(p_ee, p_cart, args.past_window_size)
+        past_data['path2D_untimed'].clear()
+        past_data['path2D_untimed'].extend(straigh_line_path[i] for i in range(args.past_window_size))
+
     if priority_int < 2:
+        # TODO make this one work
         breakFlag, save_past_item, log_item = BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, i, past_data)
         #BaseAndEEPathFollowingMPCControlLoop(args, robot, solver_pulling, path_planner, i, past_data)
 
-    q = robot.getQ()
     p = q[:2]
     # needed for cart pulling
     save_past_item['path2D_untimed'] = p
+    save_past_item['priority_register'] = priority_register.copy()
+    # TODO plot priority register out
+    #log_item['prio_reg'] = ...
     log_item['qs'] = q.reshape((robot.model.nq,))
     log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
+    print(priority_register)
     return breakFlag, save_past_item, log_item
 
 
@@ -161,7 +187,8 @@ def cartPulling(args, robot : RobotManager, goal, path_planner):
     T_w_e = robot.getT_w_e()
     log_item['qs'] = q.reshape((robot.model.nq,))
     log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
-    save_past_item = {'path2D_untimed' : T_w_e.translation[:2]}
+    save_past_item = {'path2D_untimed' : T_w_e.translation[:2],
+                      'priority_register' : ['0','1','1']}
     loop_manager = ControlLoopManager(robot, controlLoop, args, save_past_item, log_item)
     loop_manager.run()
 
diff --git a/python/examples/heron_pls.py b/python/examples/heron_pls.py
deleted file mode 100644
index 5eb9f9153e58c6c05afc8bd116abcd3ef36619df..0000000000000000000000000000000000000000
--- a/python/examples/heron_pls.py
+++ /dev/null
@@ -1,49 +0,0 @@
-import numpy as np
-import argparse
-from functools import partial
-from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager
-from ur_simple_control.clik.clik import getClikArgs, getClikController, controlLoopClik, moveL, compliantMoveL
-from ur_simple_control.util.get_model import heron_approximation 
-import time
-from pinocchio.visualize import MeshcatVisualizer
-import pinocchio as pin
-
-def get_args():
-    parser = getMinimalArgParser()
-    parser.description = 'mpc for heron path following'
-    args = parser.parse_args()
-    return args
-
-if __name__ == "__main__": 
-    args = get_args()
-
-    model, collision_model, visual_model, data = heron_approximation()
-    print(model)
-    viz = MeshcatVisualizer(model, collision_model, visual_model)
-    viz.initViewer(open=True)
-    viz.loadViewerModel()
-    q = np.zeros(model.nq)
-    theta = np.random.random() * 2*np.pi
-    q[2] = np.cos(theta)
-    q[3] = np.sin(theta)
-    v = np.random.random(model.nv)
-    #q = np.random.random(model.nq) * 0.5
-    for i in range(10000):
-        q = pin.integrate(model, q, v * 0.001)
-        viz.display(q)
-
-#    robot = RobotManager(args)
-#    Mgoal = robot.defineGoalPointCLI()
-#    compliantMoveL(args, robot, Mgoal)
-#    robot.closeGripper()
-#    robot.openGripper()
-#    if not args.pinocchio_only:
-#        robot.stopRobot()
-#
-#    if args.visualize_manipulator:
-#        robot.killManipulatorVisualizer()
-#    
-#    if args.save_log:
-#        robot.log_manager.saveLog()
-#    #loop_manager.stopHandler(None, None)
-#
diff --git a/python/examples/path6d_from_path2d.py b/python/examples/path6d_from_path2d.py
new file mode 100644
index 0000000000000000000000000000000000000000..d191407b79f1f18049864a690028fab326fb4cca
--- /dev/null
+++ b/python/examples/path6d_from_path2d.py
@@ -0,0 +1,129 @@
+# PYTHON_ARGCOMPLETE_OK
+import numpy as np
+import time
+import argparse, argcomplete
+from functools import partial
+from ur_simple_control.managers import getMinimalArgParser, ControlLoopManager, RobotManager, ProcessManager
+from ur_simple_control.optimal_control.get_ocp_args import get_OCP_args
+from ur_simple_control.optimal_control.crocoddyl_optimal_control import *
+from ur_simple_control.optimal_control.crocoddyl_mpc import *
+from ur_simple_control.basics.basics import followKinematicJointTrajP
+from ur_simple_control.util.logging_utils import LogManager
+from ur_simple_control.visualize.visualize import plotFromDict
+from ur_simple_control.clik.clik import getClikArgs, cartesianPathFollowingWithPlanner, controlLoopClik, invKinmQP, dampedPseudoinverse
+import pinocchio as pin
+import crocoddyl
+from functools import partial
+import importlib.util
+from ur_simple_control.path_generation.planner import starPlanner, getPlanningArgs, createMap
+import yaml
+import numpy as np
+from functools import partial
+from ur_simple_control.managers import ProcessManager, getMinimalArgParser
+from ur_simple_control.util.get_model import heron_approximation
+from ur_simple_control.visualize.visualize import plotFromDict, realTimePlotter, manipulatorVisualizer
+from ur_simple_control.path_generation.planner import path2D_timed, pathPointFromPathParam, path2D_to_SE3
+
+
+def get_args():
+    parser = getMinimalArgParser()
+    parser = get_OCP_args(parser)
+    parser = getClikArgs(parser) # literally just for goal error
+    parser = getPlanningArgs(parser)
+    parser.add_argument('--handlebar-height', type=float, default=0.5,\
+                        help="heigh of handlebar of the cart to be pulled")
+    parser.add_argument('--base-to-handlebar-preferred-distance', type=float, default=0.7, \
+            help="prefered path arclength from mobile base position to handlebar")
+    argcomplete.autocomplete(parser)
+    args = parser.parse_args()
+    # TODO TODO TODO: REMOVE PRESET HACKS
+    robot_type = "Unicycle"
+    with open(args.planning_robot_params_file) as f:
+        params = yaml.safe_load(f)
+    robot_params = params[robot_type]
+    with open(args.tunnel_mpc_params_file) as f:
+        params = yaml.safe_load(f)
+    mpc_params = params["tunnel_mpc"]
+    args.np = mpc_params['np']
+    args.n_pol = mpc_params['n_pol']
+    return args
+
+args = get_args()
+args.past_window_size = 100
+
+model, visual_model, collision_model, data = heron_approximation()
+side_function = partial(manipulatorVisualizer, model, collision_model, visual_model)
+#side_function = partial(manipulatorVisualizer, None, None, None)
+q = np.zeros(model.nq)
+q[0] = 10
+q[1] = 10
+q[2] = 0
+q[3] = 1
+visualizer_manager = ProcessManager(args, side_function, {"q" : q}, 0)
+
+# s = t * v
+# t=2
+# v= max_base_v
+
+max_base_v = np.linalg.norm(model.velocityLimit[:2])
+dt = 1/ args.ctrl_freq
+
+# path you already traversed
+#time_past = np.linspace(0.0, args.past_window_size * dt, args.past_window_size)
+x = np.linspace(0.0, args.past_window_size * dt, args.past_window_size)
+#x = np.linspace(0.0, 2.0, 200)
+x = x.reshape((-1,1))
+y = np.sin(x)
+past_data  = {}
+past_data['path2D_untimed'] = np.hstack((x,y))
+
+# path you get from path planner
+x= np.linspace(0.0, args.past_window_size * dt, args.past_window_size)
+#x = np.linspace(2.0, 4.0, 200)
+x = x.reshape((-1,1))
+y = np.sin(x)
+path2D_untimed_base = np.hstack((x,y))
+p = path2D_untimed_base[-1]
+path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1,2))
+# ideally should be precomputed somewhere 
+# base just needs timing on the path,
+# and it's of height 0 (i.e. the height of base's planar joint)
+path_base = path2D_timed(args, None, path2D_untimed_base, max_base_v, 0.0)
+
+path_arclength = np.linalg.norm(p - past_data['path2D_untimed'])
+handlebar_path_index = -1
+for i in range(-2, -1 * len(past_data['path2D_untimed']), -1):
+    if path_arclength > args.base_to_handlebar_preferred_distance:
+        handlebar_path_index = i
+        break
+    path_arclength += np.linalg.norm(past_data['path2D_untimed'][i - 1] - past_data['path2D_untimed'][i])
+# i shouldn't need to copy-paste everything but what can you do
+path2D_handlebar_1_untimed = np.array(past_data['path2D_untimed'])
+
+
+time_past = np.linspace(0.0, args.past_window_size * dt, args.past_window_size)
+s = np.linspace(0.0, args.n_knots * args.ocp_dt, args.n_knots)
+path2D_handlebar_1 = np.hstack((
+    np.interp(s, time_past, path2D_handlebar_1_untimed[:,0]).reshape((-1,1)), 
+    np.interp(s, time_past, path2D_handlebar_1_untimed[:,1]).reshape((-1,1))))
+
+# TODO: combine with base for maximum correctness
+pathSE3_handlebar = path2D_to_SE3(path2D_handlebar_1, args.handlebar_height)
+for p in pathSE3_handlebar:
+    print(p)
+
+#some_path = []
+#for i in range(100):
+#    translation = np.zeros(3)
+#    translation[0] = i / 100
+#    rotation = np.eye(3)
+#    some_path.append(pin.SE3(rotation, translation))
+for i in range(100):
+    visualizer_manager.sendCommand({"frame_path": pathSE3_handlebar})
+    #visualizer_manager.sendCommand({"frame_path": some_path})
+    visualizer_manager.sendCommand({"path": pathSE3_handlebar})
+    time.sleep(1)
+print("send em")
+
+time.sleep(10)
+visualizer_manager.terminateProcess()
diff --git a/python/examples/test_by_running.sh b/python/examples/test_by_running.sh
index 6afcb16df55a6b90f59d9a5c4b53702fc9a45a86..f001ebef5250c16543ea05ddd8666a194e4fda6a 100755
--- a/python/examples/test_by_running.sh
+++ b/python/examples/test_by_running.sh
@@ -35,10 +35,12 @@ runnable="clik.py --robot=heron --randomly-generate-goal --clik-controller=invKi
 echo $runnable
 python $runnable
 
-
-runnable=""
+# cart pulling mpc
+runnable="cart_pulling.py --max-solver-iter=10 --n-knots=30 --robot=heron --past-window-size=200"
 echo $runnable
 python $runnable
+
+
 #python cart_pulling.py
 #python casadi_ocp_collision_avoidance.py
 #python challenge_main.py
diff --git a/python/ur_simple_control/__pycache__/managers.cpython-312.pyc b/python/ur_simple_control/__pycache__/managers.cpython-312.pyc
index 11b255d84e82ed0bbd0e7def6f743ea11d3e8f1e..d3a7de66304d1692bb9b71e34464599956f51aca 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/managers.py b/python/ur_simple_control/managers.py
index d0c7ba6e8adedc11b61614570187eef81ac60a54..42f12c01a008f8b6368dd109dcdf9c3a8187958f 100644
--- a/python/ur_simple_control/managers.py
+++ b/python/ur_simple_control/managers.py
@@ -435,12 +435,6 @@ class RobotManager:
             self.model, self.collision_model, self.visual_model, self.data = \
                  getGripperlessUR5e()
 
-        # start visualize manipulator process if selected.
-        # has to be started here because it lives throughout the whole run
-        if args.visualize_manipulator:
-            side_function = partial(manipulatorVisualizer, self.model, self.collision_model, self.visual_model)
-            self.visualizer_manager = ProcessManager(args, side_function, {"q" : np.zeros(self.model.nq)}, 0)
-
         # create log manager if we're saving logs
         if args.save_log:
             self.log_manager = LogManager(args)
@@ -503,6 +497,13 @@ class RobotManager:
         self.v_q = np.zeros(self.model.nv)
         # same note as v_q, but it's a_q. 
         self.a_q = np.zeros(self.model.nv)
+
+        # start visualize manipulator process if selected.
+        # has to be started here because it lives throughout the whole run
+        if args.visualize_manipulator:
+            side_function = partial(manipulatorVisualizer, self.model, self.collision_model, self.visual_model)
+            self.visualizer_manager = ProcessManager(args, side_function, {"q" : self.q.copy()}, 0)
+
         # initialize and connect the interfaces
         if not args.pinocchio_only:
             # NOTE: you can't connect twice, so you can't have more than one RobotManager.
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index efde6dc68af86e8d44812cbd2cc0b3e54356e7c5..26571a05dfcbcc577633cc6e28e9acc5fedbaaa2 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -117,9 +117,7 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
     T_w_e = robot.getT_w_e()
     p = T_w_e.translation[:2]
 
-    # TODO: replace with shm
-    if not (type(path_planner) == types.FunctionType):
-        path_planner.sendFreshestCommand({'p' : p})
+    path_planner.sendCommandViaSharedMemory(p)
 
     # NOTE: it's pointless to recalculate the path every time 
     # if it's the same 2D path
@@ -135,21 +133,25 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
             log_item['qs'] = q.reshape((robot.model.nq,))
             log_item['dqs'] = robot.getQd().reshape((robot.model.nv,))
             return breakFlag, save_past_dict, log_item
+
         if data == "done":
             breakFlag = True
 
         path_pol, path2D_untimed = data
         path2D_untimed = np.array(path2D_untimed).reshape((-1,2))
-        # should be precomputed somewhere but this is nowhere near the biggest problem
+        # who cares if the velocity is right,
+        # it should be kinda right so that we need less ocp iterations
+        # and that's it
         max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
+        path2D = path2D_timed(args, path2D_untimed, max_base_v)
+
+        # create a 3D reference out of the path
+        pathSE3 = path2D_to_SE3(path2D, args.handlebar_height)
 
-        # 1) make 6D path out of [[x0,y0],...] 
-        # that represents the center of the cart
-        pathSE3 = path2D_to_timed_SE3(args, path_pol, path2D_untimed, max_base_v)
     # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
     if args.visualize_manipulator:
         if i % 20 == 0:
-            robot.visualizer_manager.sendCommand({"path": pathSE3})
+            robot.visualizer_manager.sendCommand({"frame_path": pathSE3})
     
     x0 = np.concatenate([robot.getQ(), robot.getQd()])
     solver.problem.x0 = x0
@@ -160,9 +162,11 @@ def CrocoEndEffectorPathFollowingMPCControlLoop(args, robot : RobotManager, solv
 
     for i, runningModel in enumerate(solver.problem.runningModels):
         runningModel.differential.costs.costs['gripperPose' + str(i)].cost.residual.reference = pathSE3[i]
+        #runningModel.differential.costs.costs['gripperPose'].cost.residual.reference = pathSE3[i]
 
     # idk if that's necessary
     solver.problem.terminalModel.differential.costs.costs['gripperPose'+str(args.n_knots)].cost.residual.reference = pathSE3[-1]
+    #solver.problem.terminalModel.differential.costs.costs['gripperPose'].cost.residual.reference = pathSE3[-1]
 
     solver.solve(xs_init, us_init, args.max_solver_iter)
     xs = np.array(solver.xs)
@@ -262,9 +266,10 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
     path2D_untimed_base = np.array(path2D_untimed_base).reshape((-1,2))
     # ideally should be precomputed somewhere 
     max_base_v = np.linalg.norm(robot.model.velocityLimit[:2])
-    # base just needs timing on the path,
+    # base just needs timing on the path
+    path_base = path2D_timed(args, path2D_untimed_base, max_base_v)
     # and it's of height 0 (i.e. the height of base's planar joint)
-    path_base = path2D_timed(args, path_pol_base, path2D_untimed_base, max_base_v, 0.0)
+    path_base = np.hstack((path_base, np.zeros((len(path_base),1))))
     
     ###################################################
     #  construct timed SE3 path for the end-effector  #
@@ -305,14 +310,12 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
         np.interp(s, time_past, path2D_handlebar_1_untimed[:,0]).reshape((-1,1)), 
         np.interp(s, time_past, path2D_handlebar_1_untimed[:,1]).reshape((-1,1))))
 
-    # TODO: combine with base for maximum correctness
     pathSE3_handlebar = path2D_to_SE3(path2D_handlebar_1, args.handlebar_height)
-    # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
-    # TODO: make it work with just translations too
+
     if args.visualize_manipulator:
         if iter_n % 20 == 0:
-            #robot.visualizer_manager.sendCommand({"path": path_base})
-            robot.visualizer_manager.sendCommand({"path": pathSE3_handlebar})
+            robot.visualizer_manager.sendCommand({"path": path_base})
+            robot.visualizer_manager.sendCommand({"frame_path": pathSE3_handlebar})
 
     x0 = np.concatenate([robot.getQ(), robot.getQd()])
     solver.problem.x0 = x0
@@ -322,6 +325,8 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
     us_init = list(solver.us[1:]) + [solver.us[-1]]
 
     for i, runningModel in enumerate(solver.problem.runningModels):
+        #print('adding base', path_base[i])
+        #print("this was the prev ref", runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference)
         runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference = path_base[i]
         runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
 
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 2da5229203ef2f6a7fda2a966ba14d5418724cbd..3060ceb54148de68f2629836e7769c1fb7d51163 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -344,6 +344,7 @@ def createCrocoEEPathFollowingOCP(args, robot : RobotManager, x0):
                 state.nv)
         goalTrackingCost = crocoddyl.CostModelResidual(state, framePlacementResidual)
         runningCostModel.addCost("gripperPose" + str(i), goalTrackingCost, 1e2)
+        #runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
 
         if args.solver == "boxfddp":
             runningModel = crocoddyl.IntegratedActionModelEuler(
@@ -382,6 +383,7 @@ def createCrocoEEPathFollowingOCP(args, robot : RobotManager, x0):
             )
 
     terminalCostModel.addCost("gripperPose" + str(args.n_knots), goalTrackingCost, 1e2)
+    #terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
 
     # now we define the problem
     problem = crocoddyl.ShootingProblem(x0, runningModels, terminalModel)
diff --git a/python/ur_simple_control/path_generation/planner.py b/python/ur_simple_control/path_generation/planner.py
index b3caf99830613dface09cb63378d4e37e9ed58ea..0fc4397867432a4ab28e1eaa7818cda03dcf7f9c 100644
--- a/python/ur_simple_control/path_generation/planner.py
+++ b/python/ur_simple_control/path_generation/planner.py
@@ -11,19 +11,24 @@ import shapely
 import yaml
 import pinocchio as pin
 import pickle
+from importlib.resources import files
 
 import matplotlib.pyplot as plt
 import matplotlib.collections as plt_col
 from multiprocessing import Queue, Lock, shared_memory
 
 def getPlanningArgs(parser):
+    robot_params_file_path = files('ur_simple_control.path_generation').joinpath('robot_params.yaml')
+    tunnel_mpc_params_file_path = files('ur_simple_control.path_generation').joinpath('tunnel_mpc_params.yaml')
     parser.add_argument('--planning-robot-params-file', type=str,
+                        default=robot_params_file_path,
                         #default='/home/gospodar/lund/praxis/projects/ur_simple_control/python/ur_simple_control/path_generation/robot_params.yaml',
-                        default='/home/gospodar/colcon_venv/ur_simple_control/python/ur_simple_control/path_generation/robot_params.yaml',
+                        #default='/home/gospodar/colcon_venv/ur_simple_control/python/ur_simple_control/path_generation/robot_params.yaml',
                         help="path to robot params file, required for path planning because it takes kinematic constraints into account")
     parser.add_argument('--tunnel-mpc-params-file', type=str,
+                        default=tunnel_mpc_params_file_path,
                         #default='/home/gospodar/lund/praxis/projects/ur_simple_control/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml',
-                        default='/home/gospodar/colcon_venv/ur_simple_control/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml',
+                        #default='/home/gospodar/colcon_venv/ur_simple_control/python/ur_simple_control/path_generation/tunnel_mpc_params.yaml',
                         help="path to mpc (in original tunnel) params file, required for path planning because it takes kinematic constraints into account")
     parser.add_argument('--n-pol', type=int,
                         default='0',
@@ -465,7 +470,7 @@ def pathVisualizer(x0, goal, map_as_list, positions):
 def pathPointFromPathParam(n_pol, path_dim, path_pol, s):
     return [np.polyval(path_pol[j*(n_pol+1):(j+1)*(n_pol+1)], s) for j in range(path_dim)]
 
-def path2D_timed(args, path_pol, path2D_untimed, max_base_v, path_height):
+def path2D_timed(args, path2D_untimed, max_base_v):
     """
     path2D_timed
     ---------------------
@@ -538,10 +543,8 @@ def path2D_timed(args, path_pol, path2D_untimed, max_base_v, path_height):
         # NOTE: this should be wrong, and ocp_dt correct,
         # but it works better for some reason xd
         s = i * (max_s / args.n_knots)
-        #path2D.append(pathPointFromPathParam(args.n_pol, args.np, path_pol, s))
         path2D.append(np.array([np.interp(s, s_vec, path2D_untimed[:,0]), 
-                                np.interp(s, s_vec, path2D_untimed[:,1]),
-                                path_height]))
+                                np.interp(s, s_vec, path2D_untimed[:,1])]))
     path2D = np.array(path2D)
     return path2D
 
@@ -574,9 +577,13 @@ def path2D_to_SE3(path2D, path_height):
     # manipulation task
     pathSE3 = []
     for i in range(len(path2D) - 1):
-        rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
-        rotation = pin.rpy.rpyToMatrix(np.pi/2, np.pi/2,0.0) @ rotation
-        #rotation = np.eye(3)
+        # first set the x axis to be in the theta direction
+        rotation = np.array([
+                    [-np.cos(thetas[i]), np.sin(thetas[i]), 0.0],
+                    [-np.sin(thetas[i]), -np.cos(thetas[i]), 0.0],
+                    [0.0,                0.0,          -1.0]])
+        #rotation = pin.rpy.rpyToMatrix(0.0, 0.0, thetas[i])
+        #rotation = pin.rpy.rpyToMatrix(np.pi/2, np.pi/2,0.0) @ rotation
         translation = np.array([path2D[i][0], path2D[i][1], path_height])
         pathSE3.append(pin.SE3(rotation, translation))
     pathSE3.append(pin.SE3(rotation, translation))
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 187924ac4f4dd0cb40db20bbba09a96ac8c5b8f3..1948491a9723e6fe68e17ce0558265fdcf2258bf 100644
Binary files a/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc and b/python/ur_simple_control/util/__pycache__/get_model.cpython-312.pyc differ
diff --git a/python/ur_simple_control/util/encapsulating_ellipses.py b/python/ur_simple_control/util/encapsulating_ellipses.py
index 7515bfc96dad5974a9c345ea103954089b0171b3..ec0e6acb98ae99faa26c8dcf30acc53c46780ab0 100644
--- a/python/ur_simple_control/util/encapsulating_ellipses.py
+++ b/python/ur_simple_control/util/encapsulating_ellipses.py
@@ -176,7 +176,13 @@ def computeEncapsulatingEllipses(args, robot : RobotManager):
     
         ### SOLVE
         opti.minimize(totalcost)
-        opti.solver("ipopt")  # set numerical backend
+        # remove prints i don't care about
+        opts={}
+        opts["verbose_init"] = False
+        opts["verbose"] = False
+        opts["print_time"] = False
+        opts["ipopt.print_level"] = 0
+        opti.solver("ipopt", opts)  # set numerical backend
         opti.set_initial(var_r, 10)
     
         sol = opti.solve_limited()
diff --git a/python/ur_simple_control/util/get_model.py b/python/ur_simple_control/util/get_model.py
index f687b61f674f7f8eace4fed4d9408900a88ad4b8..ef3751133df091f61b64f64c8561cb20f5b0c120 100644
--- a/python/ur_simple_control/util/get_model.py
+++ b/python/ur_simple_control/util/get_model.py
@@ -195,11 +195,15 @@ def heron_approximation():
     # TODO: put in heron's values
     # TODO: make these parameters the same as in mpc_params in the planner
     model_mobile_base.velocityLimit[0] = 2
-    model_mobile_base.velocityLimit[1] = 0
+    # TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
+    #model_mobile_base.velocityLimit[1] = 0
+    model_mobile_base.velocityLimit[1] = 2
     model_mobile_base.velocityLimit[2] = 2
     # TODO: i have literally no idea what reasonable numbers are here
     model_mobile_base.effortLimit[0] = 200
-    model_mobile_base.effortLimit[1] = 0
+    # TODO: PUT THE CONSTRAINTS BACK!!!!!!!!!!!!!!!
+    #model_mobile_base.effortLimit[1] = 0
+    model_mobile_base.effortLimit[1] = 2
     model_mobile_base.effortLimit[2] = 200
     #print("OBJECT_JOINT_ID",OBJECT_JOINT_ID)
     #body_inertia = pin.Inertia.FromBox(args.box_mass, box_dimensions[0], 
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 d408404c141de5e14ac89723f5bef2a873d5268f..613a8b0469fb7f32d6c4c9d2d31f13d84092d47f 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/meshcat_viewer_wrapper/visualizer.py b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
index e241d050fc4713e1e7aabee98d621a3cdb4e603b..6dbd513fa60369776efa95a04450ec052d10030f 100644
--- a/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
+++ b/python/ur_simple_control/visualize/meshcat_viewer_wrapper/visualizer.py
@@ -1,6 +1,7 @@
 import random
 
 import meshcat
+import meshcat_shapes
 import numpy as np
 import pinocchio as pin
 from pinocchio.visualize import MeshcatVisualizer as PMV
@@ -42,6 +43,7 @@ class MeshcatVisualizer(PMV):
         # which will never be changed
         self.n_points = 0
         self.n_path_points = 0
+        self.n_frame_path_points = 0
         self.n_obstacles = 0
         if robot is not None:
             super().__init__(robot.model, robot.collision_model, robot.visual_model)
@@ -99,11 +101,24 @@ class MeshcatVisualizer(PMV):
     def addPath(self, name, path : list[pin.SE3], radius=5e-3, color=[1, 0, 0, 0.8]):
         # who cares about the name
         name = "path"
+        if type(path) == np.ndarray:
+            # complete the quartenion
+            path = np.hstack((path, np.zeros((len(path), 3))))
+            path = np.hstack((path, np.ones((len(path), 1))))
         for i, point in enumerate(path):
             if i < self.n_path_points:
                 self.addSphere(f"world/path_{name}_point_{i}", radius, color)
             self.applyConfiguration(f"world/path_{name}_point_{i}", point)
         self.n_path_points = max(i, self.n_path_points)
+
+    def addFramePath(self, name, path : list[pin.SE3], radius=5e-3, color=[1, 0, 0, 0.8]):
+        # who cares about the name
+        name = "frame_path"
+        for i, point in enumerate(path):
+            if i < self.n_frame_path_points:
+                meshcat_shapes.frame(self.viewer[f"world/frame_path_{name}_point_{i}"], opacity=0.3)
+            self.applyConfiguration(f"world/frame_path_{name}_point_{i}", point)
+        self.n_frame_path_points = max(i, self.n_frame_path_points)
         
 
     def applyConfiguration(self, name, placement):
diff --git a/python/ur_simple_control/visualize/visualize.py b/python/ur_simple_control/visualize/visualize.py
index 233245482caf3365bedab4585dc8ee958d6782c8..07a69d24f5feb4660518f485243a175af8b09e68 100644
--- a/python/ur_simple_control/visualize/visualize.py
+++ b/python/ur_simple_control/visualize/visualize.py
@@ -200,6 +200,9 @@ def manipulatorVisualizer(model, collision_model, visual_model, args, cmd, queue
                 if key == "path":
                     # stupid and evil but there is no time
                     viz.addPath("", cmd["path"])
+                if key == "frame_path":
+                    # stupid and evil but there is no time
+                    viz.addFramePath("", cmd["frame_path"])
 
     except KeyboardInterrupt:
         if args.debug_prints: