diff --git a/examples/navigation/mobile_base_navigation.py b/examples/navigation/mobile_base_navigation.py
index 685637d01331d4e4d0c6d3a5a5176eae89f1ae59..8930cde429b6df5c43b56785eab6ecf6655ef7fe 100644
--- a/examples/navigation/mobile_base_navigation.py
+++ b/examples/navigation/mobile_base_navigation.py
@@ -1,5 +1,6 @@
 from smc import getRobotFromArgs
 from smc import getMinimalArgParser
+from smc.control.control_loop_manager import ControlLoopManager
 from smc.robots.interfaces import MobileBaseInterface
 from smc.path_generation.maps.premade_maps import createSampleStaticMap
 from smc.path_generation.path_math.path2d_to_6d import path2D_to_SE3
@@ -19,11 +20,9 @@ from smc.util.draw_path import drawPath
 import numpy as np
 from functools import partial
 import argparse
+from collections import deque
 import matplotlib
 
-# TODO:
-# try valueing future points on the path more the current ones!!!!
-
 
 def get_args() -> argparse.Namespace:
     parser = getMinimalArgParser()
@@ -80,6 +79,45 @@ def fixedPathPlanner(path2D: np.ndarray, p_base: np.ndarray) -> np.ndarray:
     return path2D
 
 
+def MPCWithCLIKFallbackControlLoop(
+    loop_manager_mpc: ControlLoopManager,
+    loop_manager_clik: ControlLoopManager,
+    args: argparse.Namespace,
+    robot: MobileBaseInterface,
+    t: int,
+    past_data: dict[str, deque[np.ndarray]],
+) -> tuple[bool, dict[str, np.ndarray], dict[str, np.ndarray]]:
+    breakFlag = False
+    if np.linalg.norm(robot.v) < 1e-2:
+        print("clicking")
+        breakFlag = loop_manager_clik.run_one_iter(t)
+    else:
+        print("mpcing")
+        breakFlag = loop_manager_mpc.run_one_iter(t)
+
+    return breakFlag, {}, {"qs": robot.q, "dqs": robot.v}
+
+
+def MPCWithCLIKFallback(
+    args: argparse.Namespace, robot: MobileBaseInterface, path_planner
+) -> None:
+    loop_manager_mpc = CrocoBasePathFollowingMPC(
+        args, robot, x0, path_planner, run=False
+    )
+    loop_manager_clik = cartesianPathFollowingWithPlanner(
+        args, robot, path_planner, run=False
+    )
+    control_loop = partial(
+        MPCWithCLIKFallbackControlLoop, loop_manager_mpc, loop_manager_clik, args, robot
+    )
+
+    log_item = {}
+    log_item["qs"] = np.zeros(robot.nq)
+    log_item["dqs"] = np.zeros(robot.nv)
+    loop_manager = ControlLoopManager(robot, control_loop, args, {}, log_item)
+    loop_manager.run()
+
+
 if __name__ == "__main__":
     args = get_args()
     robot = getRobotFromArgs(args)
@@ -118,8 +156,9 @@ if __name__ == "__main__":
         CrocoBaseP2PMPC(args, robot, path2D[0])
         path_planner = partial(fixedPathPlanner, path2D)
 
-    CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
+    # CrocoBasePathFollowingMPC(args, robot, x0, path_planner)
     # cartesianPathFollowingWithPlanner(args, robot, path_planner)
+    MPCWithCLIKFallback(args, robot, path_planner)
 
     if args.real:
         robot.stopRobot()
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 b27869577120a3736da77ceb5fd176092d28cd41..949d83dec5d9dde3caca689c3f409c4bf46be26c 100644
--- a/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
+++ b/python/smc/control/cartesian_space/cartesian_space_trajectory_following.py
@@ -1,11 +1,13 @@
 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 smc.path_generation.path_math.path2d_to_6d import (
+    path2D_to_SE3,
+)
 
 from functools import partial
 import pinocchio as pin
@@ -16,13 +18,9 @@ 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 cartesianPathFollowingControlLoop(
     ik_solver: Callable[[np.ndarray, np.ndarray], np.ndarray],
-    path: list[pin.SE3],
+    path: list[pin.SE3] | np.ndarray,
     args: Namespace,
     robot: SingleArmInterface,
     t: int,
@@ -34,6 +32,9 @@ def cartesianPathFollowingControlLoop(
     end-effector(s) follow their path(s) according to what a 2D path-planner spits out
     """
 
+    # TODO: refactor this horror out of here
+    if type(path) == np.ndarray:
+        path = path2D_to_SE3(path[:, :2], 0.0)
     # 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])
@@ -54,9 +55,9 @@ def cartesianPathFollowingControlLoop(
 
     # 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})
+    if args.visualizer:
+        if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
+            robot.visualizer_manager.sendCommand({"frame_path": path[:20]})
 
     return v_cmd, {}, {}
 
diff --git a/python/smc/control/cartesian_space/ik_solvers.py b/python/smc/control/cartesian_space/ik_solvers.py
index 4fa85403d83e951a827f376ba034226451272e5e..a24e140df966434b97797d7479e4da77e4d98776 100644
--- a/python/smc/control/cartesian_space/ik_solvers.py
+++ b/python/smc/control/cartesian_space/ik_solvers.py
@@ -44,8 +44,6 @@ def getIKSolver(
         H = np.eye(robot.nv)
         g = np.zeros(robot.nv)
         G = np.eye(robot.nv)
-        # TODO: you have to check if which control mode it is
-        # probably easiest if all robots have a control mode
         J = robot.getJacobian()
         A = np.eye(J.shape[0], robot.nv)
         b = np.ones(J.shape[0]) * 0.1