diff --git a/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling_control_loop.py b/examples/cart_pulling/control_sub_problems/dual_arm_cart_pulling_control_loop.py
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling.py b/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling.py
index 993171a1fae2a5a455f527f347f2d85dee603afc..7b8b8578c4abd1dd2afb0d4c83d4dd9d101615d9 100644
--- a/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling.py
+++ b/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling.py
@@ -1,3 +1,5 @@
+from single_arm_cart_pulling_control_loop import SingleArmCartPullingMPC
+
 from smc import getRobotFromArgs
 from smc import getMinimalArgParser
 from smc.path_generation.maps.premade_maps import createSampleStaticMap
@@ -5,16 +7,17 @@ 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.path_generation.planner import starPlanner, getPlanningArgs
-from smc.control.optimal_control.croco_mpc_point_to_point import CrocoEEAndBaseP2PMPC
-from single_arm_cart_pulling_control_loop import SingleArmCartPullingMPC
+from smc.control.optimal_control.croco_point_to_point.mpc.base_and_single_arm_reference_mpc import (
+    CrocoEEAndBaseP2PMPC,
+)
 from smc.multiprocessing import ProcessManager
+from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
+from smc.robots.interfaces.single_arm_interface import SingleArmInterface
 
 import time
 import numpy as np
 from functools import partial
 import pinocchio as pin
-from smc.robots.interfaces.mobile_base_interface import MobileBaseInterface
-from smc.robots.interfaces.single_arm_interface import SingleArmInterface
 
 
 def get_args():
@@ -87,7 +90,6 @@ if __name__ == "__main__":
         p_base[2] = 0.0
         print(pathSE3[0].translation)
         print(p_base)
-        # TODO: UNCOMMENT
         CrocoEEAndBaseP2PMPC(args, robot, pathSE3[0], p_base)
     print("initialized!")
     SingleArmCartPullingMPC(args, robot, path_planner)
diff --git a/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling_control_loop.py b/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling_control_loop.py
index 8a944a57f2fd5f4daba2d7b1f7c5d4c271d90f55..312dbd7faf3eda2fa16bcd5ebd00282350d7537d 100644
--- a/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling_control_loop.py
+++ b/examples/cart_pulling/control_sub_problems/single_arm_cart_pulling_control_loop.py
@@ -4,7 +4,7 @@ from smc.robots.interfaces.whole_body_interface import (
 from smc.control.control_loop_manager import ControlLoopManager
 from smc.multiprocessing.process_manager import ProcessManager
 from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
-from smc.control.optimal_control.path_following_croco_ocp import (
+from smc.control.optimal_control.croco_path_following.mpc.base_and_single_arm_reference_mpc import (
     BaseAndEEPathFollowingOCP,
 )
 from smc.path_generation.path_math.cart_pulling_path_math import (
@@ -45,7 +45,7 @@ def SingleArmCartPullingMPCControlLoop(
     t: int,
     past_data: dict[str, deque[np.ndarray]],
 ) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
-    p = robot.q[:2]
+    p = robot.T_w_b.translation[:2]
 
     # NOTE: this one is obtained as the future path from path planner
     max_base_v = np.linalg.norm(robot._max_v[:2])
diff --git a/examples/optimal_control/path_following/base_and_ee_path_following.py b/examples/optimal_control/path_following/croco_base_and_ee_reference_path_following_mpc.py
similarity index 96%
rename from examples/optimal_control/path_following/base_and_ee_path_following.py
rename to examples/optimal_control/path_following/croco_base_and_ee_reference_path_following_mpc.py
index 7c6cc521c42791d166bb841ee3d977cfb2da8493..9ea1d4b3334b97c3aca804232e379415dfc98ddc 100644
--- a/examples/optimal_control/path_following/base_and_ee_path_following.py
+++ b/examples/optimal_control/path_following/croco_base_and_ee_reference_path_following_mpc.py
@@ -2,7 +2,7 @@ from smc import getRobotFromArgs
 from smc import getMinimalArgParser
 from smc.control.optimal_control.util import get_OCP_args
 from smc.control.cartesian_space import getClikArgs
-from smc.control.optimal_control.croco_mpc_path_following import (
+from smc.control.optimal_control.croco_path_following.mpc.base_and_single_arm_reference_mpc import (
     BaseAndEEPathFollowingMPC,
 )
 from os.path import exists
@@ -10,7 +10,7 @@ from smc import getRobotFromArgs
 from smc import getMinimalArgParser
 from smc.control.optimal_control.util import get_OCP_args
 from smc.control.cartesian_space import getClikArgs
-from smc.control.optimal_control.croco_mpc_point_to_point import (
+from smc.control.optimal_control.croco_point_to_point.mpc.base_and_single_arm_reference_mpc import (
     CrocoEEAndBaseP2PMPC,
 )
 from smc.path_generation.planner import starPlanner, getPlanningArgs
diff --git a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
index 8b71f35dd224d629eeeaeee5dfb72068d7d41ba0..6a26983fa6352abb1ec5c7bdda1a2e5e7e679e89 100644
--- a/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
+++ b/python/smc/control/cartesian_space/cartesian_space_point_to_point.py
@@ -187,8 +187,8 @@ def controlLoopClikDualArm(
     #                       J           err_vec     v_cmd
     ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
     T_w_absgoal: pin.SE3,
-    T_abs_lgoal: pin.SE3,
-    T_abs_rgoal: pin.SE3,
+    T_absgoal_l: pin.SE3,
+    T_absgoal_r: pin.SE3,
     args: Namespace,
     robot: DualArmInterface,
     t: int,
@@ -202,8 +202,8 @@ def controlLoopClikDualArm(
     and an SE3  transformation on the goal for each arm
     """
 
-    T_w_lgoal = T_abs_lgoal.act(T_w_absgoal)
-    T_w_rgoal = T_abs_rgoal.act(T_w_absgoal)
+    T_w_lgoal = T_absgoal_l.act(T_w_absgoal)
+    T_w_rgoal = T_absgoal_r.act(T_w_absgoal)
 
     SEerror_left = robot.T_w_l.actInv(T_w_lgoal)
     SEerror_right = robot.T_w_r.actInv(T_w_rgoal)
diff --git a/python/smc/control/cartesian_space/ik_solvers.py b/python/smc/control/cartesian_space/ik_solvers.py
index f933638b0a50dff4a7c3ba7d98592662ec46128d..d4e3880b47b0c8657d5d932185048017d1914df2 100644
--- a/python/smc/control/cartesian_space/ik_solvers.py
+++ b/python/smc/control/cartesian_space/ik_solvers.py
@@ -302,7 +302,7 @@ def QPManipMax(
     q = secondary_objective_vec
     G = None
     V_e_e_norm = np.linalg.norm(V_e_e)
-    max_V_e_e_norm = 0.3
+    max_V_e_e_norm = 0.2
     if V_e_e_norm < max_V_e_e_norm:
         b = V_e_e
     else:
diff --git a/python/smc/control/controller_templates/path_following_template.py b/python/smc/control/controller_templates/path_following_template.py
index 13915f646f6cf906bf6e8f821d2c78f7c7154c70..49aa904b984fd466d9610e7de9d25a2f657c9303 100644
--- a/python/smc/control/controller_templates/path_following_template.py
+++ b/python/smc/control/controller_templates/path_following_template.py
@@ -63,7 +63,9 @@ def PathFollowingFromPlannerCtrllLoopTemplate(
         # NOTE: DOES NOT HAVE TO BE 2D IN THIS CASE,
         # TODO: RENAME APROPRIATELY
         path2D = path_planner(p)
-        if len(path2D) < 4:
+        # NOTE: more evil in case path2D is a tuple.
+        # i have no time to properly rewrite this so pls bear w/ me
+        if len(path2D) < 4 and len(path2D[0]) < 4:
             breakFlag = True
 
     v_cmd, past_item_inner, log_item_inner = control_loop(
diff --git a/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py
index 4ba2af49b880050e09f538c9e8e1475f84c1c42b..7124b094212f08b981d972f96c5370374c0d83de 100644
--- a/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py
+++ b/python/smc/control/optimal_control/croco_path_following/mpc/base_and_single_arm_reference_mpc.py
@@ -2,17 +2,10 @@ from smc.robots.interfaces.whole_body_interface import (
     SingleArmWholeBodyInterface,
 )
 from smc.control.control_loop_manager import ControlLoopManager
-from smc.multiprocessing.process_manager import ProcessManager
 from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
 from smc.control.optimal_control.croco_path_following.ocp.base_and_single_arm_reference_ocp import (
     BaseAndEEPathFollowingOCP,
 )
-from smc.path_generation.path_math.path2d_to_6d import (
-    path2D_to_SE3,
-)
-from smc.path_generation.path_math.cart_pulling_path_math import (
-    construct_EE_path,
-)
 from smc.path_generation.path_math.path_to_trajectory import (
     path2D_to_trajectory2D,
     pathSE3_to_trajectorySE3,
@@ -36,47 +29,47 @@ def BaseAndEEPathFollowingMPCControlLoop(
     robot,
     t: int,
     _: dict[str, deque[np.ndarray]],
-) -> tuple[np.ndarray, dict[str, np.ndarray]]:
+) -> tuple[np.ndarray, dict[str, np.ndarray], dict[str, np.ndarray]]:
     """
     BaseAndEEPathFollowingMPCControlLoop
     -----------------------------
     both a path for both the base and the end-effector are provided,
     and both are followed
     """
-    breakFlag = False
     log_item = {}
-    save_past_dict = {}
 
-    q = robot.q
-    T_w_e = robot.T_w_e
-    path_base, path_EE = path_planner(T_w_e)
+    path_base, path_EE = path
 
-    x0 = np.concatenate([robot.q, robot.v])
-    ocp.warmstartAndReSolve(x0, data=(path_base, path_EE))
-    xs = np.array(ocp.solver.xs)
-    us = np.array(ocp.solver.us)
-    vel_cmds = xs[1, robot.model.nq :]
+    max_base_v = np.linalg.norm(robot._max_v[:2])
+    perc_of_max_v = 0.5
+    velocity = perc_of_max_v * max_base_v
+    path_base = np.array(path_base)
+    trajectory_base = path2D_to_trajectory2D(args, path_base, velocity)
+    trajectory_base = np.hstack((trajectory_base, np.zeros((len(trajectory_base), 1))))
+    trajectory_EE = pathSE3_to_trajectorySE3(args, path_EE, velocity)
 
-    robot.sendVelocityCommand(vel_cmds)
     if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
-        path_base = np.array(path_base)
-        robot.visualizer_manager.sendCommand({"path": path_base})
-        robot.visualizer_manager.sendCommand({"frame_path": path_EE})
+        robot.visualizer_manager.sendCommand({"path": trajectory_base})
+        robot.visualizer_manager.sendCommand({"frame_path": trajectory_EE})
 
-    err_vector_ee = log6(T_w_e.actInv(path_EE[0]))
-    err_vector_base = np.linalg.norm(q[:2] - path_base[0][:2])  # z axis is irrelevant
+    x0 = np.concatenate([robot.q, robot.v])
+    ocp.warmstartAndReSolve(x0, data=(trajectory_base, trajectory_EE))
+    xs = np.array(ocp.solver.xs)
+    v_cmd = xs[1, robot.model.nq :]
+
+    err_vector_ee = log6(robot.T_w_e.actInv(path_EE[0]))
+    err_vector_base = np.linalg.norm(
+        robot.T_w_b.translation[:2] - path_base[0][:2]
+    )  # z axis is irrelevant
     log_item["err_vec_ee"] = err_vector_ee
-    log_item["err_norm_ee"] = np.linalg.norm(err_vector_ee).reshape((1,))
     log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
-    log_item["qs"] = q.reshape((robot.model.nq,))
-    log_item["dqs"] = robot.v.reshape((robot.model.nv,))
-    return breakFlag, save_past_dict, log_item
+    return v_cmd, {}, log_item
 
 
 def BaseAndEEPathFollowingMPC(
     args: Namespace,
     robot: SingleArmWholeBodyInterface,
-    path_planner: ProcessManager | types.FunctionType,
+    path_planner: types.FunctionType,
     run=True,
 ) -> None | ControlLoopManager:
     """
@@ -93,18 +86,24 @@ def BaseAndEEPathFollowingMPC(
     ocp = BaseAndEEPathFollowingOCP(args, robot, x0)
     ocp.solveInitialOCP(x0)
 
+    # NOTE: for this loop it's arbitrarily decided that
+    # the end-effector position is the key thing,
+    # while the base is just supposed to not get in the way basically.
+    # so where you are on the path is determined by the end-effector
+    get_position = lambda robot: robot.T_w_e
     controlLoop = partial(
+        PathFollowingFromPlannerCtrllLoopTemplate,
+        path_planner,
+        get_position,
+        ocp,
         BaseAndEEPathFollowingMPCControlLoop,
         args,
         robot,
-        ocp,
-        path_planner,
     )
     log_item = {
         "qs": np.zeros(robot.model.nq),
         "dqs": np.zeros(robot.model.nv),
         "err_vec_ee": np.zeros((6,)),
-        "err_norm_ee": np.zeros((1,)),
         "err_norm_base": np.zeros((1,)),
     }
     loop_manager = ControlLoopManager(robot, controlLoop, args, {}, log_item)
diff --git a/python/smc/control/optimal_control/croco_path_following/mpc/dual_arm_reference_mpc.py b/python/smc/control/optimal_control/croco_path_following/mpc/dual_arm_reference_mpc.py
index bdf94dba262227c21efdfa7e7387e60c33f1d539..a96db7664fcdcf64b95edd11895eb9b90cbef941 100644
--- a/python/smc/control/optimal_control/croco_path_following/mpc/dual_arm_reference_mpc.py
+++ b/python/smc/control/optimal_control/croco_path_following/mpc/dual_arm_reference_mpc.py
@@ -71,8 +71,7 @@ def CrocoDualArmEEPathFollowingMPCControlLoop(
 
     err_vector_ee_l = log6(robot.T_w_l.actInv(trajectory_T_w_l[0])).vector
     err_vector_ee_r = log6(robot.T_w_r.actInv(trajectory_T_w_r[0])).vector
-    log_item = {"err_vec_ee_l": err_vector_ee_l}
-    log_item = {"err_vec_ee_r": err_vector_ee_r}
+    log_item = {"err_vec_ee_l": err_vector_ee_l, "err_vec_ee_r": err_vector_ee_r}
 
     return v_cmd, {}, log_item
 
@@ -94,6 +93,8 @@ def CrocoDualArmEEPathFollowingMPC(
     """
 
     ocp = DualArmEEPathFollowingOCP(args, robot, x0)
+    # ocp.terminalCostModel.changeCostStatus("velFinal_l", False)
+    # ocp.terminalCostModel.changeCostStatus("velFinal_r", False)
     # technically should be done in controlloop because now
     # it's solved 2 times before the first command,
     # but we don't have time for details rn
diff --git a/python/smc/path_generation/path_math/path_to_trajectory.py b/python/smc/path_generation/path_math/path_to_trajectory.py
index 08ef8f9a3498ecbfa74a310ad5e556e6114c70a9..db337015b86a98cfb1f1eebf72e4b9fddea61ee9 100644
--- a/python/smc/path_generation/path_math/path_to_trajectory.py
+++ b/python/smc/path_generation/path_math/path_to_trajectory.py
@@ -45,7 +45,7 @@ def path2D_to_trajectory2D(
 
 
 def pathSE3_to_trajectorySE3(
-    args: Namespace, pathSE3: np.ndarray, velocity: float
+    args: Namespace, pathSE3: list[SE3], velocity: float
 ) -> list[SE3]:
     path2D = np.zeros((len(pathSE3), 2))
     for i, pose in enumerate(pathSE3):
diff --git a/python/smc/robots/interfaces/dual_arm_interface.py b/python/smc/robots/interfaces/dual_arm_interface.py
index e6bf2f06a49802347c9ce88b22ebfa3674043471..3aa16ffe1237201e6b55164680dbb3bbd8a0efd8 100644
--- a/python/smc/robots/interfaces/dual_arm_interface.py
+++ b/python/smc/robots/interfaces/dual_arm_interface.py
@@ -266,8 +266,8 @@ class DualArmInterface(SingleArmInterface):
             res *= self.computeManipulabilityIndex()
             return res
 
-        l_joint_index = self.model.frames[self._l_ee_frame_id].parent
-        r_joint_index = self.model.frames[self._r_ee_frame_id].parent
+        l_joint_index = self.model.frames[self._l_ee_frame_id].parentJoint
+        r_joint_index = self.model.frames[self._r_ee_frame_id].parentJoint
 
         # TODO: joint_ids obviously have to be defined per robot, this is a dirty hack
         # because i know i'm on yumi
diff --git a/python/smc/robots/interfaces/single_arm_interface.py b/python/smc/robots/interfaces/single_arm_interface.py
index fbe85ee2aa44674d8c58a44ed07c0f7fa5741a3b..315b4107bc4cd274cbe3e9573d7fedf75b6a6a2b 100644
--- a/python/smc/robots/interfaces/single_arm_interface.py
+++ b/python/smc/robots/interfaces/single_arm_interface.py
@@ -66,7 +66,7 @@ class SingleArmInterface(AbstractRobotManager):
         return np.sqrt(np.linalg.det(J @ J.T))
 
     def computeManipulabilityIndexQDerivative(self) -> np.ndarray:
-        joint_index = self.model.frames[self._ee_frame_id].parent
+        joint_index = self.model.frames[self._ee_frame_id].parentJoint
         J = pin.computeJointJacobian(self.model, self.data, self._q, joint_index)
         Jp = J.T @ np.linalg.inv(
             J @ J.T + np.eye(J.shape[0], J.shape[0]) * self.args.tikhonov_damp