diff --git a/docker/Dockerfile b/docker/Dockerfile
index c78ced7cb810f5e04227ab6efeeefd177f9da87b..5e76c53bee717fbf83271a42a0a255fc51e2650d 100644
--- a/docker/Dockerfile
+++ b/docker/Dockerfile
@@ -1,11 +1,17 @@
-FROM ubuntu:jammy
+#FROM ubuntu:jammy # required for ros
+FROM ubuntu:noble
 
 LABEL org.opencontainers.image.authors="marko.guberina@control.lth.se"
 
-# install python3-tk without questions
+# NOTE: this is here just to install python3-tk without questions
 ARG DEBIAN_FRONTEND=noninteractive
 ENV TZ=Europe/Stockholm
 
+# pinocchio install
+RUN apt install -qqy lsb-release curl
+RUN mkdir -p /etc/apt/keyrings curl http://robotpkg.openrobots.org/packages/debian/robotpkg.asc | tee /etc/apt/keyrings/robotpkg.asc
+RUN echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/robotpkg.asc] http://robotpkg.openrobots.org/packages/debian/pub $(lsb_release -cs) robotpkg" | sudo tee /etc/apt/sources.list.d/robotpkg.list
+
 RUN apt-get update && apt-get install -y --no-install-recommends \
         python3  \
         python3-pip \
@@ -27,14 +33,17 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
         zsh-syntax-highlighting \
         libarchive-tools \
         libxcb-cursor0\
-        python3-python-qt-binding 
-        
+        python3-python-qt-binding \
+        robotpkg-py3*-pinocchio
+
+
+
 # qt-binding is a really unnecessary 300MB, but i don't want
 # to do more matplotlib hacks
 # if you want manuals:
 #RUN sed -i 's:^path-exclude=/usr/share/man:#path-exclude=/usr/share/man:' \
 #        /etc/dpkg/dpkg.cfg.d/excludes
-# we're running out of space on the docker.control
+# --> we're running out of space on the docker.control
 # and noone is reading manuals anyway
 #RUN yes | unminimize 
 
@@ -54,33 +63,13 @@ COPY --chown=student /dot_files_for_docker/.zshrc /home/student/
 COPY --chown=student /dot_files_for_docker/global_extra_conf.py /home/student/
 
 
-# sh does not have sourcing 
-# and some packages (conda) want shell environment variables 
-# (which i can say a lot about, but can't do anything about)
-# ((the only reason to even use conda is to not have to compile pinocchio))
-SHELL ["/bin/bash", "--login", "-c"]
-#SHELL ["/bin/bash"]
-
 # this is enough to run clik
 WORKDIR /home/student/
 USER student
-# TODO: install casadi and pinochio 3.0+
-# TODO: verify this stuff below works
-# --> this can be done with conda
-RUN mkdir -p ~/miniconda3
-RUN wget https://repo.anaconda.com/miniconda/Miniconda3-latest-Linux-x86_64.sh -O /home/student/miniconda3/miniconda.sh
-RUN bash /home/student/miniconda3/miniconda.sh -b -u -p ~/miniconda3
-RUN rm /home/student/miniconda3/miniconda.sh
-ENV PATH=/home/student/miniconda3/bin:$PATH
-RUN source /home/student/miniconda3/bin/activate
 RUN pip install -e ./SimpleManipulatorControl/python/
-RUN conda config --add channels conda-forge 
-#RUN conda install --solver=classic conda-forge::conda-libmamba-solver conda-forge::libmamba conda-forge::libmambapy conda-forge::libarchive
-#RUN conda install --solver=classic -y pinocchio crocoddyl -c conda-forge
-RUN conda install -y pinocchio crocoddyl -c conda-forge
-#RUN conda install -y opencv
-RUN pip install matplotlib meshcat ur_rtde argcomplete \
+RUN pip install crocoddyl matplotlib meshcat ur_rtde \
                 qpsolvers ecos example_robot_data meshcat_shapes \
-                pyqt6 opencv-python
+                pyqt6 opencv-python qpsolvers quadprog \
+                proxsuite casadi pin-pink
 
 RUN vam install python-jedi && vam install youcompleteme
diff --git a/docker/dot_files_for_docker/.zshrc b/docker/dot_files_for_docker/.zshrc
index 6ba1e66f79e984363bc76b687502b811b8c99ea2..b75ceee7102dc97b648fdc21041579f75bbc2959 100644
--- a/docker/dot_files_for_docker/.zshrc
+++ b/docker/dot_files_for_docker/.zshrc
@@ -109,6 +109,13 @@ export SUDO_EDITOR="$VISUAL"
 #export CC=/usr/bin/clang
 #export CXX=/usr/bin/clang++
 
+# pinocchio installed via apt has these locations
+export PATH=/opt/openrobots/bin:$PATH
+export PKG_CONFIG_PATH=/opt/openrobots/lib/pkgconfig:$PKG_CONFIG_PATH
+export LD_LIBRARY_PATH=/opt/openrobots/lib:$LD_LIBRARY_PATH
+export PYTHONPATH=/opt/openrobots/lib/python3.10/site-packages:$PYTHONPATH # Adapt your desired python version here
+export CMAKE_PREFIX_PATH=/opt/openrobots:$CMAKE_PREFIX_PATH
+
 
 # Load aliases and shortcuts if existent.
 [ -f "$HOME/.config/shortcutrc" ] && source "$HOME/.config/shortcutrc"
diff --git a/examples/navigation/mobile_base_navigation.py b/examples/navigation/mobile_base_navigation.py
index d21838958d38671740f7884b652c861fe20333b1..685637d01331d4e4d0c6d3a5a5176eae89f1ae59 100644
--- a/examples/navigation/mobile_base_navigation.py
+++ b/examples/navigation/mobile_base_navigation.py
@@ -5,6 +5,9 @@ from smc.path_generation.maps.premade_maps import createSampleStaticMap
 from smc.path_generation.path_math.path2d_to_6d import path2D_to_SE3
 from smc.control.optimal_control.util import get_OCP_args
 from smc.control.cartesian_space import getClikArgs
+from smc.control.cartesian_space.cartesian_space_trajectory_following import (
+    cartesianPathFollowingWithPlanner,
+)
 from smc.path_generation.planner import starPlanner, getPlanningArgs
 from smc.control.optimal_control.croco_mpc_point_to_point import CrocoBaseP2PMPC
 from smc.control.optimal_control.croco_mpc_path_following import (
@@ -15,7 +18,6 @@ from smc.util.draw_path import drawPath
 
 import numpy as np
 from functools import partial
-import pinocchio as pin
 import argparse
 import matplotlib
 
@@ -117,6 +119,7 @@ if __name__ == "__main__":
         path_planner = partial(fixedPathPlanner, path2D)
 
     CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
+    # cartesianPathFollowingWithPlanner(args, robot, path_planner)
 
     if args.real:
         robot.stopRobot()
diff --git a/examples/optimal_control/croco_ik_mpc.py b/examples/optimal_control/croco_single_ee_ik_mpc.py
similarity index 95%
rename from examples/optimal_control/croco_ik_mpc.py
rename to examples/optimal_control/croco_single_ee_ik_mpc.py
index 71f1c434ee83b46d66f04d7e7dd2dcaee681a1ef..bd25d5527945298af7950595fb1061a8a24013ff 100644
--- a/examples/optimal_control/croco_ik_mpc.py
+++ b/examples/optimal_control/croco_single_ee_ik_mpc.py
@@ -30,9 +30,6 @@ if __name__ == "__main__":
 
     CrocoEEP2PMPC(args, robot, T_w_goal)
 
-    print("final position:")
-    print(robot.T_w_e)
-
     if args.real:
         robot.stopRobot()
 
diff --git a/examples/optimal_control/croco_ik_ocp.py b/examples/optimal_control/croco_single_ee_ik_ocp.py
similarity index 97%
rename from examples/optimal_control/croco_ik_ocp.py
rename to examples/optimal_control/croco_single_ee_ik_ocp.py
index 40f70f3357df988f00aacbef03a7e962552c9db7..0b85c48edf9ed32152d87b1bd8f3647a0ed833a9 100644
--- a/examples/optimal_control/croco_ik_ocp.py
+++ b/examples/optimal_control/croco_single_ee_ik_ocp.py
@@ -51,8 +51,8 @@ if __name__ == "__main__":
     # and because it is sampled at a much lower frequency
     followKinematicJointTrajP(args, robot, reference)
 
-    print("final position:")
-    print(robot.T_w_e)
+    # print("final position:")
+    # print(robot.T_w_e)
 
     if args.real:
         robot.stopRobot()
diff --git a/examples/optimal_control/path_following_mpc.py b/examples/optimal_control/path_following_mpc.py
index 2e11a54104211e46764924edde04603c624414d5..5ec64e77586c793bed74bbef2ee98c3cf54ef8ae 100644
--- a/examples/optimal_control/path_following_mpc.py
+++ b/examples/optimal_control/path_following_mpc.py
@@ -40,8 +40,6 @@ if __name__ == "__main__":
 
     CrocoEEPathFollowingMPC(args, robot, x0, path)
 
-    print("final position:", robot.T_w_e)
-
     if args.real:
         robot.stopRobot()
 
diff --git a/examples/optimal_control/test_by_running.sh b/examples/optimal_control/test_by_running.sh
index dd696d3793c3fe62489313fa619e49155ccb58e9..dc04fc8636747af863eb78b54ced2b986a9c635f 100755
--- a/examples/optimal_control/test_by_running.sh
+++ b/examples/optimal_control/test_by_running.sh
@@ -1,17 +1,17 @@
 #!/bin/bash
 # the idea here is to run all the runnable things
 # and test for syntax errors 
-#
+# TODO: make these work with different modes by making robot.model a property which outputs truncated models
 # ################
 # single arm 
 # ###############
 # ocp
-runnable="croco_ik_ocp.py --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2000"
+runnable="croco_single_ee_ik_ocp.py --ctrl-freq=-1 --no-plotter --no-visualizer --max-iterations=2"
 echo $runnable
 python $runnable
 
 # mpc
-runnable="croco_ik_mpc.py --max-solver-iter 10 --n-knots 30  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="croco_single_ee_ik_mpc.py --max-solver-iter 10 --n-knots 30  --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
 echo $runnable
 python $runnable
 
@@ -19,11 +19,11 @@ python $runnable
 # whole body single arm
 ######################
 # ocp
-runnable="croco_ik_ocp.py --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="croco_single_ee_ik_ocp.py --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
 echo $runnable
 python $runnable
 # mpc
-runnable="croco_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="croco_single_ee_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=heron --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
 echo $runnable
 python $runnable
 
@@ -33,7 +33,7 @@ python $runnable
 # ocp TODO: missing
 
 # mpc
-runnable="dual_arm_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=yumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="dual_arm_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=yumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
 echo $runnable
 python $runnable
 
@@ -43,7 +43,7 @@ python $runnable
 # ocp TODO: missing
 #
 # mpc
-runnable="dual_arm_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=myumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2000"
+runnable="dual_arm_ik_mpc.py --max-solver-iter 10 --n-knots 30 --robot=myumi --ctrl-freq=-1 --no-visualizer --no-plotter --max-iterations=2"
 echo $runnable
 python $runnable
 
diff --git a/python/setup.py b/python/setup.py
index ac19f1e9a0e6dd58e0f5466ecc8e1dd364e4c4b0..e6568b2a32acd9f6bd44f73d7779632ce973de70 100644
--- a/python/setup.py
+++ b/python/setup.py
@@ -1,35 +1,39 @@
 from setuptools import setup, find_packages
-setup(name='smc',
-        version='0.3',
-        description='Simple Manipulator Control (SMC) - simplest possible framework for robot control.',
-        author='Marko Guberina',
-        url='https://gitlab.control.lth.se/marko-g/ur_simple_control',
-        packages=['smc'],
-        #        package_dir={"": "ur_simple_control"},
-        package_data={
-            'smc.robots.robot_descriptions': ['*'],
-            },
-        zip_safe=False)
+
+setup(
+    name="smc",
+    version="0.3",
+    description="Simple Manipulator Control (SMC) - simplest possible framework for robot control.",
+    author="Marko Guberina",
+    url="https://gitlab.control.lth.se/marko-g/ur_simple_control",
+    packages=["smc"],
+    #        package_dir={"": "ur_simple_control"},
+    package_data={
+        "smc.robots.robot_descriptions": ["*"],
+    },
+    zip_safe=False,
+)
 # NOTE: if you want to switch to the toml thing,
 # here you go, knock yourself out, didn't really work for me,
 # and i don't care at this stage
-        # add other ones and test, check .toml file for more
-        # dependencies need to be taken care of separately
-        # because you don't want to install pinocchio via pip
-        #install_requires=['numpy'],
-        # broken, but if you'll want to switch later here you go
-        #packages=find_packages(where='src'),
-        #package_data={'ur_simple_control': ['clik/*', 'dmp/*', 'util/*']}
-        #)
+# add other ones and test, check .toml file for more
+# dependencies need to be taken care of separately
+# because you don't want to install pinocchio via pip
+# install_requires=['numpy'],
+# broken, but if you'll want to switch later here you go
+# packages=find_packages(where='src'),
+# package_data={'ur_simple_control': ['clik/*', 'dmp/*', 'util/*']}
+# )
 
 # dependencies:
 # pinocchio - robot math
 # numpy - math
 # matplotlib - plotter
 # meshcat, meshcat_shapes - visualizer
-# optional ik solver: qpsolvers, quadprog, proxsuite
-# optional for opc/mpc: crocoddyl 
-# optional for path planning: albin's path planning repos: starworlds, star_navigation, tunnel_mpc
-# optional for visual servoing: cv2
-# optional for different ocp: casadi AND pinocchio > 3.0
-# optional for yumi, mobile yumi, and heron: ros2
+# [optional] UR robots control: ur_rtde
+# [optional] ik solvers: qpsolvers, quadprog, proxsuite, ecos, pink
+# [optional] for opc/mpc: crocoddyl
+# [optional] for path planning: albin's path planning repos: starworlds, star_navigation, tunnel_mpc (NOTE all these repos are copy-pasted to path_generation atm)
+# [optional] for visual servoing: opencv-python
+# [optional] for different ocp: casadi (requires pinocchio > 3.0)
+# [optional] for yumi, mobile yumi, and heron: ros2
diff --git a/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py b/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
index cefb4bba85ef74078630d12bf6bf35606886c6fa..b27869577120a3736da77ceb5fd176092d28cd41 100644
--- a/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
+++ b/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
@@ -1,86 +1,93 @@
 from smc.control.control_loop_manager import ControlLoopManager
 from smc.robots.abstract_robotmanager import AbstractRobotManager
 from smc.multiprocessing.process_manager import ProcessManager
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
+from smc.control.controller_templates.path_following_template import (
+    PathFollowingFromPlannerCtrllLoopTemplate,
+)
+from smc.control.cartesian_space.ik_solvers import getIKSolver, dampedPseudoinverse
+
 from functools import partial
 import pinocchio as pin
 import numpy as np
+from argparse import Namespace
+from collections import deque
+from typing import Callable
+import types
+
 
 # TODO: update this with all the changes
 # from smc.path_generation.path_math.path_to_trajectory import path2D_to_timed_SE3
 #
 #
-# def cartesianPathFollowingWithPlannerControlLoop(
-#    args, robot: AbstractRobotManager, path_planner: ProcessManager, i, past_data
-# ):
-#    """
-#    cartesianPathFollowingWithPlanner
-#    -----------------------------
-#    end-effector(s) follow their path(s) according to what a 2D path-planner spits out
-#    """
-#    breakFlag = False
-#    log_item = {}
-#    save_past_dict = {}
-#
-#    q = robot.q
-#    T_w_e = robot.T_w_e
-#    p = T_w_e.translation[:2]
-#    path_planner.sendFreshestCommand({"p": p})
-#
-#    # NOTE: it's pointless to recalculate the path every time
-#    # if it's the same 2D path
-#    data = path_planner.getData()
-#    if data == None:
-#        if args.debug_prints:
-#            print("got no path so no doing anything")
-#        robot.sendVelocityCommand(np.zeros(robot.model.nv))
-#        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
-#    max_base_v = np.linalg.norm(robot.model.robot.model.velocityLimit[:2])
-#
-#    # 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)
-#    # print(path2D_untimed)
-#    # for pp in pathSE3:
-#    #    print(pp.translation)
-#    # TODO: EVIL AND HAS TO BE REMOVED FROM HERE
-#    if args.visualizer:
-#        robot.visualizer_manager.sendCommand({"path": pathSE3})
-#
-#    J = pin.computeFrameJacobian(robot.model, robot.data, q, robot.ee_frame_id)
-#    # NOTE: obviously not path following but i want to see things working here
-#    SEerror = T_w_e.actInv(pathSE3[-1])
-#    err_vector = pin.log6(SEerror).vector
-#    lb = -1 * robot.model.robot.model.velocityLimit
-#    lb[1] = -0.001
-#    ub = robot.model.robot.model.velocityLimit
-#    ub[1] = 0.001
-#    # vel_cmd = invKinmQP(J, err_vector, lb=lb, ub=ub)
-#    vel_cmd = dampedPseudoinverse(0.002, J, err_vector)
-#    robot.sendVelocityCommand(vel_cmd)
-#
-#    log_item["qs"] = q.reshape((robot.model.nq,))
-#    log_item["dqs"] = robot.getQd().reshape((robot.model.nv,))
-#    # time.sleep(0.01)
-#    return breakFlag, save_past_dict, log_item
-#
-#
-# def cartesianPathFollowingWithPlanner(
-#    args, robot: AbstractRobotManager, path_planner: ProcessManager
-# ):
-#    controlLoop = partial(
-#        cartesianPathFollowingWithPlannerControlLoop, args, robot, path_planner
-#    )
-#    log_item = {"qs": np.zeros(robot.model.nq), "dqs": np.zeros(robot.model.nv)}
-#    save_past_dict = {}
-#    loop_manager = ControlLoopManager(
-#        robot, controlLoop, args, save_past_dict, log_item
-#    )
-#    loop_manager.run()
+def cartesianPathFollowingControlLoop(
+    ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
+    path: list[pin.SE3],
+    args: Namespace,
+    robot: SingleArmInterface,
+    t: int,
+    _: dict[str, deque[np.ndarray]],
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
+    """
+    cartesianPathFollowingControlLoop
+    -----------------------------
+    end-effector(s) follow their path(s) according to what a 2D path-planner spits out
+    """
+
+    # TODO: arbitrary bs, read a book and redo this
+    # NOTE: assuming the first path point coincides with current pose
+    SEerror = robot.T_w_e.actInv(path[1])
+    V_path = pin.log6(path[0].actInv(path[1])).vector
+    err_vector = pin.log6(SEerror).vector + V_path
+    J = robot.getJacobian()
+    v_cmd = ik_solver(J, err_vector)
+
+    if v_cmd is None:
+        print(
+            t,
+            "the controller you chose produced None as output, using dampedPseudoinverse instead",
+        )
+        v_cmd = dampedPseudoinverse(1e-2, J, err_vector)
+    else:
+        if args.debug_prints:
+            print(t, "ik solver success")
+
+    # maybe visualize the closest path point instead? the path should be handled
+    # by the path planner
+    # if args.visualizer:
+    #    if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+    #        robot.visualizer_manager.sendCommand({"frame_path": path_EE_SE3})
+
+    return v_cmd, {}, {}
+
+
+def cartesianPathFollowingWithPlanner(
+    args: Namespace,
+    robot: SingleArmInterface,
+    path_planner: ProcessManager | types.FunctionType,
+    run=True,
+) -> None | ControlLoopManager:
+    ik_solver = getIKSolver(args, robot)
+    get_position = lambda robot: robot.T_w_e.translation[:2]
+    controlLoop = partial(
+        PathFollowingFromPlannerCtrllLoopTemplate,
+        path_planner,
+        get_position,
+        ik_solver,
+        cartesianPathFollowingControlLoop,
+        args,
+        robot,
+    )
+    log_item = {
+        "qs": np.zeros(robot.model.nq),
+        "dqs": np.zeros(robot.model.nv),
+        "err_vec_ee": np.zeros(6),
+    }
+    save_past_item = {}
+    loop_manager = ControlLoopManager(
+        robot, controlLoop, args, save_past_item, log_item
+    )
+    if run:
+        loop_manager.run()
+    else:
+        return loop_manager
diff --git a/python/smc/control/controller_templates/path_following_template.py b/python/smc/control/controller_templates/path_following_template.py
index 1ff64efd77ae58484747607b0932a2a65e17e707..13915f646f6cf906bf6e8f821d2c78f7c7154c70 100644
--- a/python/smc/control/controller_templates/path_following_template.py
+++ b/python/smc/control/controller_templates/path_following_template.py
@@ -6,19 +6,20 @@ from argparse import Namespace
 from typing import Any, Callable
 import numpy as np
 from collections import deque
+from pinocchio import SE3
 
 global control_loop_return
 control_loop_return = tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]
 
 
 def PathFollowingFromPlannerCtrllLoopTemplate(
-    path_planner: ProcessManager|FunctionType,
+    path_planner: ProcessManager | FunctionType,
     get_position: Callable[[AbstractRobotManager], np.ndarray],
     SOLVER: Any,
     control_loop: Callable[
         [
             Any,
-            np.ndarray,
+            np.ndarray | list[SE3],
             Namespace,
             AbstractRobotManager,
             int,
@@ -59,6 +60,8 @@ def PathFollowingFromPlannerCtrllLoopTemplate(
         _, path2D = data
         path2D = np.array(path2D).reshape((-1, 2))
     else:
+        # NOTE: DOES NOT HAVE TO BE 2D IN THIS CASE,
+        # TODO: RENAME APROPRIATELY
         path2D = path_planner(p)
         if len(path2D) < 4:
             breakFlag = True
diff --git a/python/smc/control/optimal_control/abstract_croco_ocp.py b/python/smc/control/optimal_control/abstract_croco_ocp.py
index 356a068db07c9cc1477da339c8c530efb24deb29..94d2ff911fb146b6994f2c4edb8ef4fc279403ac 100644
--- a/python/smc/control/optimal_control/abstract_croco_ocp.py
+++ b/python/smc/control/optimal_control/abstract_croco_ocp.py
@@ -55,6 +55,7 @@ class CrocoOCP(abc.ABC):
         self.constructRegulationCosts()
         self.constructStateLimits()
         self.constructConstraints()
+        self.constructTaskCostsValues()
         self.constructTaskObjectiveFunction(goal)
         self.constructRunningModels()
         self.problem = crocoddyl.ShootingProblem(
@@ -220,6 +221,9 @@ class CrocoOCP(abc.ABC):
             0.0,
         )
 
+    @abc.abstractmethod
+    def constructTaskCostsValues(self) -> None: ...
+
     @abc.abstractmethod
     def constructTaskObjectiveFunction(self, goal) -> None: ...
 
diff --git a/python/smc/control/optimal_control/croco_mpc_path_following.py b/python/smc/control/optimal_control/croco_mpc_path_following.py
index 6a98cdca86330e4c43d2d91dda00f504cfc2d3bf..73844d397f91eb7ae05a04717544dad37fd8fbd7 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -1,4 +1,3 @@
-from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
 from smc.robots.interfaces.single_arm_interface import SingleArmInterface
 from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface, DualArmWholeBodyInterface
 from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
@@ -64,7 +63,7 @@ def CrocoBasePathFollowingMPC(
     robot: MobileBaseInterface,
     x0: np.ndarray,
     path_planner: ProcessManager | types.FunctionType,
-) -> None:
+run=True) -> None|ControlLoopManager:
     """
     CrocoBasePathFollowingMPC
     -----
@@ -93,9 +92,13 @@ def CrocoBasePathFollowingMPC(
     loop_manager = ControlLoopManager(
         robot, controlLoop, args, save_past_item, log_item
     )
-    loop_manager.run()
+    if run:
+        loop_manager.run()
+    else:
+        return loop_manager
 
 
+# NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
 def CrocoEEPathFollowingMPCControlLoop(
     args: Namespace,
     robot: SingleArmInterface,
@@ -190,7 +193,7 @@ def CrocoEEPathFollowingMPC(
     robot: SingleArmInterface,
     x0: np.ndarray,
     path_planner: ProcessManager | types.FunctionType,
-) -> None:
+run=True) -> None|ControlLoopManager:
     """
     CrocoEndEffectorPathFollowingMPC
     -----
@@ -207,6 +210,7 @@ def CrocoEEPathFollowingMPC(
 
     if type(path_planner) == types.FunctionType:
         controlLoop = partial(
+            # NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
             CrocoEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
         )
     else:
@@ -229,9 +233,12 @@ def CrocoEEPathFollowingMPC(
     loop_manager = ControlLoopManager(
         robot, controlLoop, args, save_past_item, log_item
     )
-    loop_manager.run()
-
+    if run:
+        loop_manager.run()
+    else:
+        return loop_manager
 
+# NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
 def BaseAndEEPathFollowingMPCControlLoop(
     args: Namespace,
     robot,
@@ -332,7 +339,7 @@ def BaseAndEEPathFollowingMPC(
     args: Namespace,
     robot: SingleArmWholeBodyInterface,
     path_planner: ProcessManager | types.FunctionType,
-) -> None:
+run=True) -> None|ControlLoopManager:
     """
     BaseAndEEPathFollowingMPC
     -----
@@ -354,6 +361,7 @@ def BaseAndEEPathFollowingMPC(
 
     if type(path_planner) == types.FunctionType:
         controlLoop = partial(
+# NOTE: DEPRICATED WILL GET DELETED SOON (TEMPLATE HANDLES IT NOW)
             BaseAndEEPathFollowingMPCControlLoop, args, robot, ocp, path_planner
         )
     else:
@@ -385,7 +393,10 @@ def BaseAndEEPathFollowingMPC(
         path2D_handlebar[i] for i in range(args.past_window_size)
     )
 
-    loop_manager.run()
+    if run:
+        loop_manager.run()
+    else:
+        return loop_manager
 
 
 # TODO: the robot put in is a whole body robot,
diff --git a/python/smc/control/optimal_control/path_following_croco_ocp.py b/python/smc/control/optimal_control/path_following_croco_ocp.py
index 99ed1175dfdcbc617b63166b736421d9f08f1b70..64a13fda7334f12f896fda22cfbc37c6706bd3ca 100644
--- a/python/smc/control/optimal_control/path_following_croco_ocp.py
+++ b/python/smc/control/optimal_control/path_following_croco_ocp.py
@@ -1,4 +1,10 @@
-from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
+from smc.control.optimal_control.point_to_point_croco_ocp import (
+    BaseIKOCP,
+    SingleArmIKOCP,
+    DualArmIKOCP,
+    BaseAndSingleArmIKOCP,
+    BaseAndDualArmIKOCP,
+)
 from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
 from smc.robots.interfaces.single_arm_interface import SingleArmInterface
 from smc.robots.interfaces.dual_arm_interface import DualArmInterface
@@ -8,11 +14,13 @@ from smc.robots.interfaces.whole_body_interface import (
 )
 
 import numpy as np
-import crocoddyl
 from argparse import Namespace
 
 
-class BasePathFollowingOCP(CrocoOCP):
+# NOTE: all of these should be rewritten so that it inherits from BaseIKOCP,
+# and then you just override update goal.
+# you'd cut this code in less than half
+class BasePathFollowingOCP(BaseIKOCP):
     """
     createBaseAndEEPathFollowingOCP
     -------------------------------
@@ -22,31 +30,9 @@ class BasePathFollowingOCP(CrocoOCP):
     """
 
     def __init__(self, args, robot: MobileBaseInterface, x0):
-        goal = None
+        goal = robot.T_w_b.translation.copy()
         super().__init__(args, robot, x0, goal)
 
-    def constructTaskObjectiveFunction(self, goal) -> None:
-        path_base = [np.append(self.x0[:2], 0.0)] * self.args.n_knots
-
-        for i in range(self.args.n_knots):
-            baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
-                self.state, self.robot.base_frame_id, path_base[i], self.state.nv
-            )
-            baseTrackingCost = crocoddyl.CostModelResidual(
-                self.state, baseTranslationResidual
-            )
-            self.runningCostModels[i].addCost(
-                "base_translation" + str(i),
-                baseTrackingCost,
-                self.args.base_translation_cost,
-            )
-
-        self.terminalCostModel.addCost(
-            "base_translation" + str(self.args.n_knots),
-            baseTrackingCost,
-            self.args.base_translation_cost,
-        )
-
     def updateCosts(self, data):
         path_base = data
         for i, runningModel in enumerate(self.solver.problem.runningModels):
@@ -60,7 +46,7 @@ class BasePathFollowingOCP(CrocoOCP):
         ].cost.residual.reference = path_base[-1]
 
 
-class CrocoEEPathFollowingOCP(CrocoOCP):
+class CrocoEEPathFollowingOCP(SingleArmIKOCP):
     """
     createCrocoEEPathFollowingOCP
     -------------------------------
@@ -70,34 +56,9 @@ class CrocoEEPathFollowingOCP(CrocoOCP):
     """
 
     def __init__(self, args: Namespace, robot: SingleArmInterface, x0: np.ndarray):
-        goal = None
+        goal = robot.T_w_e
         super().__init__(args, robot, x0, goal)
 
-    def constructTaskObjectiveFunction(self, goal):
-        T_w_e = self.robot.T_w_e
-        path = [T_w_e] * self.args.n_knots
-
-        for i in range(self.args.n_knots):
-            framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
-                self.state,
-                self.robot.ee_frame_id,
-                path[i],  # this better be done with the same time steps as the knots
-                # NOTE: this should be done outside of this function to clarity
-                self.state.nv,
-            )
-            goalTrackingCost = crocoddyl.CostModelResidual(
-                self.state, framePlacementResidual
-            )
-            self.runningCostModels[i].addCost(
-                "gripperPose" + str(i), goalTrackingCost, 1e2
-            )
-            # runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
-
-        self.terminalCostModel.addCost(
-            "gripperPose" + str(self.args.n_knots), goalTrackingCost, 1e2
-        )
-        # terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
-
     def updateCosts(self, data):
         for i, runningModel in enumerate(self.solver.problem.runningModels):
             runningModel.differential.costs.costs[
@@ -109,7 +70,7 @@ class CrocoEEPathFollowingOCP(CrocoOCP):
         ].cost.residual.reference = data[-1]
 
 
-class BaseAndEEPathFollowingOCP(CrocoOCP):
+class BaseAndEEPathFollowingOCP(BaseAndSingleArmIKOCP):
     """
     createBaseAndEEPathFollowingOCP
     -------------------------------
@@ -119,48 +80,9 @@ class BaseAndEEPathFollowingOCP(CrocoOCP):
     """
 
     def __init__(self, args, robot: SingleArmWholeBodyInterface, x0):
-        goal = None
+        goal = (robot.T_w_e, robot.T_w_b.translation.copy)
         super().__init__(args, robot, x0, goal)
 
-    def constructTaskObjectiveFunction(self, goal) -> None:
-        T_w_e = self.robot.T_w_e
-        path_ee = [T_w_e] * self.args.n_knots
-        path_base = [np.append(self.x0[:2], 0.0)] * self.args.n_knots
-
-        for i in range(self.args.n_knots):
-            eePoseResidual = crocoddyl.ResidualModelFramePlacement(
-                self.state,
-                self.robot.ee_frame_id,
-                path_ee[i],  # this better be done with the same time steps as the knots
-                # NOTE: this should be done outside of this function to clarity
-                self.state.nv,
-            )
-            eeTrackingCost = crocoddyl.CostModelResidual(self.state, eePoseResidual)
-            self.runningCostModels[i].addCost(
-                "ee_pose" + str(i), eeTrackingCost, self.args.ee_pose_cost
-            )
-
-            baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
-                self.state, self.robot.base_frame_id, path_base[i], self.state.nv
-            )
-            baseTrackingCost = crocoddyl.CostModelResidual(
-                self.state, baseTranslationResidual
-            )
-            self.runningCostModels[i].addCost(
-                "base_translation" + str(i),
-                baseTrackingCost,
-                self.args.base_translation_cost,
-            )
-
-        self.terminalCostModel.addCost(
-            "ee_pose" + str(self.args.n_knots), eeTrackingCost, self.args.ee_pose_cost
-        )
-        self.terminalCostModel.addCost(
-            "base_translation" + str(self.args.n_knots),
-            baseTrackingCost,
-            self.args.base_translation_cost,
-        )
-
     def updateCosts(self, data):
         path_base = data[0]
         pathSE3 = data[1]
@@ -181,71 +103,34 @@ class BaseAndEEPathFollowingOCP(CrocoOCP):
         ].cost.residual.reference = pathSE3[-1]
 
 
-class DualArmEEPathFollowingOCP(CrocoOCP):
+class DualArmEEPathFollowingOCP(DualArmIKOCP):
     def __init__(self, args, robot: DualArmInterface, x0):
-        goal = None
-        raise NotImplementedError
+        goal = (robot.T_w_l, robot.T_w_r)
         super().__init__(args, robot, x0, goal)
 
+    def updateCosts(self, data):
+        pathSE3_l = data[0]
+        pathSE3_r = data[1]
+        for i, runningModel in enumerate(self.solver.problem.runningModels):
+            runningModel.differential.costs.costs[
+                "gripperPose_l" + str(i)
+            ].cost.residual.reference = pathSE3_l[i]
+            runningModel.differential.costs.costs[
+                "gripperPose_r" + str(i)
+            ].cost.residual.reference = pathSE3_r[i]
 
-class BaseAndDualArmEEPathFollowingOCP(CrocoOCP):
-    def __init__(self, args, robot: DualArmWholeBodyInterface, x0):
-        goal = None
-        super().__init__(args, robot, x0, goal)
+        self.solver.problem.terminalModel.differential.costs.costs[
+            "gripperPose_l" + str(self.args.n_knots)
+        ].cost.residual.reference = pathSE3_l[-1]
+        self.solver.problem.terminalModel.differential.costs.costs[
+            "gripperPose_r" + str(self.args.n_knots)
+        ].cost.residual.reference = pathSE3_r[-1]
 
-    def constructTaskObjectiveFunction(self, goal) -> None:
-        T_w_e_left, T_w_e_right = self.robot.getLeftRightT_w_e()
-        path_ee_left = [T_w_e_left] * self.args.n_knots
-        path_ee_right = [T_w_e_right] * self.args.n_knots
-        path_base = [np.append(self.x0[:2], 0.0)] * self.args.n_knots
 
-        for i in range(self.args.n_knots):
-            l_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
-                self.state,
-                self.robot.l_ee_frame_id,
-                path_ee_left[i],
-                self.state.nv,
-            )
-            l_eeTrackingCost = crocoddyl.CostModelResidual(self.state, l_eePoseResidual)
-            self.runningCostModels[i].addCost(
-                "l_ee_pose" + str(i), l_eeTrackingCost, self.args.ee_pose_cost
-            )
-            r_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
-                self.state,
-                self.robot.r_ee_frame_id,
-                path_ee_right[i],
-                self.state.nv,
-            )
-            r_eeTrackingCost = crocoddyl.CostModelResidual(self.state, r_eePoseResidual)
-            self.runningCostModels[i].addCost(
-                "r_ee_pose" + str(i), r_eeTrackingCost, self.args.ee_pose_cost
-            )
-            baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
-                self.state, self.robot.base_frame_id, path_base[i], self.state.nv
-            )
-            baseTrackingCost = crocoddyl.CostModelResidual(
-                self.state, baseTranslationResidual
-            )
-            self.runningCostModels[i].addCost(
-                "base_translation" + str(i),
-                baseTrackingCost,
-                self.args.base_translation_cost,
-            )
-        self.terminalCostModel.addCost(
-            "l_ee_pose" + str(self.args.n_knots),
-            l_eeTrackingCost,
-            self.args.ee_pose_cost,
-        )
-        self.terminalCostModel.addCost(
-            "r_ee_pose" + str(self.args.n_knots),
-            r_eeTrackingCost,
-            self.args.ee_pose_cost,
-        )
-        self.terminalCostModel.addCost(
-            "base_translation" + str(self.args.n_knots),
-            baseTrackingCost,
-            self.args.base_translation_cost,
-        )
+class BaseAndDualArmEEPathFollowingOCP(BaseAndDualArmIKOCP):
+    def __init__(self, args, robot: DualArmWholeBodyInterface, x0):
+        goal = robot.T_w_l, robot.T_w_r, robot.T_w_b.translation
+        super().__init__(args, robot, x0, goal)
 
     def updateCosts(self, data):
         path_base = data[0]
diff --git a/python/smc/control/optimal_control/point_to_point_croco_ocp.py b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
index 5fec4434f95a06ac6654d218fabb37ed21d8e731..616956d06d37fbaa2598c97d38b24191aa48f64e 100644
--- a/python/smc/control/optimal_control/point_to_point_croco_ocp.py
+++ b/python/smc/control/optimal_control/point_to_point_croco_ocp.py
@@ -11,6 +11,7 @@ import pinocchio as pin
 import crocoddyl
 from argparse import Namespace
 
+
 class BaseIKOCP(CrocoOCP):
     def __init__(
         self,
@@ -21,6 +22,14 @@ class BaseIKOCP(CrocoOCP):
     ):
         super().__init__(args, robot, x0, p_basegoal)
 
+    def constructTaskCostsValues(self):
+        self.base_translation_cost_values = np.linspace(
+            self.args.base_translation_cost,
+            self.args.base_translation_cost
+            * self.args.linearly_increasing_task_cost_factor,
+            self.args.n_knots + 1,
+        )
+
     def constructTaskObjectiveFunction(self, goal) -> None:
         p_basegoal = goal
         for i in range(self.args.n_knots):
@@ -33,12 +42,12 @@ class BaseIKOCP(CrocoOCP):
             self.runningCostModels[i].addCost(
                 "base_translation" + str(i),
                 baseTrackingCost,
-                self.args.base_translation_cost,
+                self.base_translation_cost_values[i],
             )
         self.terminalCostModel.addCost(
             "base_translation" + str(self.args.n_knots),
             baseTrackingCost,
-            self.args.base_translation_cost,
+            self.base_translation_cost_values[-1],
         )
 
     # there is nothing to update in a point-to-point task
@@ -56,7 +65,6 @@ class BaseIKOCP(CrocoOCP):
         ].cost.residual.reference = p_basegoal
 
 
-
 class SingleArmIKOCP(CrocoOCP):
     def __init__(
         self,
@@ -67,6 +75,13 @@ class SingleArmIKOCP(CrocoOCP):
     ):
         super().__init__(args, robot, x0, T_w_goal)
 
+    def constructTaskCostsValues(self):
+        self.ee_pose_cost_values = np.linspace(
+            self.args.ee_pose_cost,
+            self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+            self.args.n_knots + 1,
+        )
+
     def constructTaskObjectiveFunction(self, goal) -> None:
         T_w_goal = goal
         framePlacementResidual = crocoddyl.ResidualModelFramePlacement(
@@ -91,10 +106,12 @@ class SingleArmIKOCP(CrocoOCP):
         # )
         for i in range(self.args.n_knots):
             self.runningCostModels[i].addCost(
-                "gripperPose" + str(i), goalTrackingCost, 1e2
+                "gripperPose" + str(i), goalTrackingCost, self.ee_pose_cost_values[i]
             )
         self.terminalCostModel.addCost(
-            "gripperPose" + str(self.args.n_knots), goalTrackingCost, 1e2
+            "gripperPose" + str(self.args.n_knots),
+            goalTrackingCost,
+            self.ee_pose_cost_values[-1],
         )
         # self.terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
 
@@ -119,6 +136,13 @@ class DualArmIKOCP(CrocoOCP):
     ):
         super().__init__(args, robot, x0, goal)
 
+    def constructTaskCostsValues(self):
+        self.ee_pose_cost_values = np.linspace(
+            self.args.ee_pose_cost,
+            self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+            self.args.n_knots + 1,
+        )
+
     def constructTaskObjectiveFunction(self, goal) -> None:
         T_w_lgoal, T_w_rgoal = goal
         framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
@@ -162,16 +186,24 @@ class DualArmIKOCP(CrocoOCP):
         # )
         for i in range(self.args.n_knots):
             self.runningCostModels[i].addCost(
-                "gripperPose_l" + str(i), goalTrackingCost_l, 1e2
+                "gripperPose_l" + str(i),
+                goalTrackingCost_l,
+                self.ee_pose_cost_values[i],
             )
             self.runningCostModels[i].addCost(
-                "gripperPose_r" + str(i), goalTrackingCost_r, 1e2
+                "gripperPose_r" + str(i),
+                goalTrackingCost_r,
+                self.ee_pose_cost_values[i],
             )
         self.terminalCostModel.addCost(
-            "gripperPose_l" + str(self.args.n_knots), goalTrackingCost_l, 1e2
+            "gripperPose_l" + str(self.args.n_knots),
+            goalTrackingCost_l,
+            self.ee_pose_cost_values[-1],
         )
         self.terminalCostModel.addCost(
-            "gripperPose_r" + str(self.args.n_knots), goalTrackingCost_r, 1e2
+            "gripperPose_r" + str(self.args.n_knots),
+            goalTrackingCost_r,
+            self.ee_pose_cost_values[-1],
         )
         # self.terminalCostModel.addCost("velFinal_l", frameVelocityCost_l, 1e3)
         # self.terminalCostModel.addCost("velFinal_r", frameVelocityCost_r, 1e3)
@@ -180,7 +212,8 @@ class DualArmIKOCP(CrocoOCP):
     def updateCosts(self, data):
         pass
 
-    def updateGoalInModels(self, T_w_lgoal: pin.SE3, T_w_rgoal: pin.SE3) -> None:
+    def updateGoalInModels(self, goal) -> None:
+        T_w_lgoal, T_w_rgoal = goal
         for i, runningModel in enumerate(self.solver.problem.runningModels):
             runningModel.differential.costs.costs[
                 "gripperPose_l" + str(i)
@@ -207,6 +240,19 @@ class BaseAndSingleArmIKOCP(SingleArmIKOCP, BaseIKOCP):
     ):
         super().__init__(args, robot, x0, goal)
 
+    def constructTaskCostsValues(self):
+        self.base_translation_cost_values = np.linspace(
+            self.args.base_translation_cost,
+            self.args.base_translation_cost
+            * self.args.linearly_increasing_task_cost_factor,
+            self.args.n_knots + 1,
+        )
+        self.ee_pose_cost_values = np.linspace(
+            self.args.ee_pose_cost,
+            self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+            self.args.n_knots + 1,
+        )
+
     def constructTaskObjectiveFunction(
         self,
         goal,
@@ -236,6 +282,19 @@ class BaseAndDualArmIKOCP(DualArmIKOCP, BaseIKOCP):
     ):
         super().__init__(args, robot, x0, goal)
 
+    def constructTaskCostsValues(self):
+        self.base_translation_cost_values = np.linspace(
+            self.args.base_translation_cost,
+            self.args.base_translation_cost
+            * self.args.linearly_increasing_task_cost_factor,
+            self.args.n_knots + 1,
+        )
+        self.ee_pose_cost_values = np.linspace(
+            self.args.ee_pose_cost,
+            self.args.ee_pose_cost * self.args.linearly_increasing_task_cost_factor,
+            self.args.n_knots + 1,
+        )
+
     def constructTaskObjectiveFunction(
         self,
         goal,
@@ -250,5 +309,5 @@ class BaseAndDualArmIKOCP(DualArmIKOCP, BaseIKOCP):
 
     def updateGoalInModels(self, goal) -> None:
         T_w_lgoal, T_w_rgoal, p_basegoal = goal
-        super().updateGoalInModels(T_w_lgoal, T_w_rgoal)
+        super().updateGoalInModels((T_w_lgoal, T_w_rgoal))
         BaseIKOCP.updateGoalInModels(self, p_basegoal)
diff --git a/python/smc/control/optimal_control/util.py b/python/smc/control/optimal_control/util.py
index 16b220bd6e8bd3f31285d74b18644e6ed3de6a81..703fd413fe342ece493dc02d476ddaff717bd6f7 100644
--- a/python/smc/control/optimal_control/util.py
+++ b/python/smc/control/optimal_control/util.py
@@ -16,9 +16,9 @@ def get_OCP_args(parser: argparse.ArgumentParser):
     )
     parser.add_argument(
         "--stop-at-final",
-        type=int,
-        default=1,
-        help="the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
+        action=argparse.BooleanOptionalAction,
+        default=False,
+        help="NOTE: NOT IMPLEMENTED ATM the trajectory generated by the OCP will be followed. it might not have finally velocity 0. \
         if this argument is set to true, the final velocity will be set to 0 (there will be overshoot).",
     )
     parser.add_argument(
@@ -49,4 +49,10 @@ def get_OCP_args(parser: argparse.ArgumentParser):
     parser.add_argument(
         "--u-reg-cost", type=float, default=1e-3, help="control input regulation cost"
     )
+    parser.add_argument(
+        "--linearly-increasing-task-cost-factor",
+        type=float,
+        default=1.0,
+        help="the first knot gets task cost you set with --task-cost, after which it's linearly increased until the last knot has this argument * --task-cost",
+    )
     return parser
diff --git a/python/smc/robots/interfaces/mobile_base_interface.py b/python/smc/robots/interfaces/mobile_base_interface.py
index 022502bda2a72dddd60c2dd3536fc7896cbc206a..6a266da4a887624c6720ee2b812406beabbeb502 100644
--- a/python/smc/robots/interfaces/mobile_base_interface.py
+++ b/python/smc/robots/interfaces/mobile_base_interface.py
@@ -47,6 +47,11 @@ class MobileBaseInterface(AbstractRobotManager):
     def T_w_b(self):
         return self._T_w_b.copy()
 
+    # NOTE: lil bit of evil to run some algorithms
+    @property
+    def T_w_e(self):
+        return self._T_w_b.copy()
+
     def computeT_w_b(self, q: np.ndarray) -> pin.SE3:
         assert type(q) is np.ndarray
         pin.forwardKinematics(
@@ -77,6 +82,12 @@ class MobileBaseInterface(AbstractRobotManager):
         self._updateV()
         self.forwardKinematics()
 
+    def getJacobian(self) -> np.ndarray:
+        J = pin.computeFrameJacobian(
+            self.model, self.data, self._q, self._base_frame_id
+        )
+        return J
+
 
 def get_mobile_base_model(underactuated: bool) -> tuple[pin.Model, pin.GeometryModel]: