diff --git a/python/smc/control/cartesian_space/cartesian_space_compliant_control.py b/python/smc/control/cartesian_space/cartesian_space_compliant_control.py
index 8947a3c31d6114be07dfeb6804ca64afe0e6c2a2..039d1c03c2dbb505267fa3f034d4e68faa7dd235 100644
--- a/python/smc/control/cartesian_space/cartesian_space_compliant_control.py
+++ b/python/smc/control/cartesian_space/cartesian_space_compliant_control.py
@@ -61,7 +61,7 @@ def controlLoopCompliantClik(
 # add a threshold for the wrench
 def compliantMoveL(
     T_w_goal: pin.SE3, args: Namespace, robot: ForceTorqueOnSingleArmWrist, run=True
-):
+) -> None:
     """
     compliantMoveL
     -----
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 c8cb92558d13fbcb6e4dcf363630a1fbd32bbd34..c2aa9cf6b1fddd7c02fd6e3cbf3776c76e5e4406 100644
--- a/python/smc/control/optimal_control/croco_mpc_path_following.py
+++ b/python/smc/control/optimal_control/croco_mpc_path_following.py
@@ -37,7 +37,7 @@ def CrocoBasePathFollowingFromPlannerMPCControlLoop(
     robot: MobileBaseInterface,
     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]]:
 
     p = robot.T_w_b.translation[:2]
     max_base_v = np.linalg.norm(robot._max_v[:2])
@@ -56,7 +56,7 @@ def CrocoBasePathFollowingFromPlannerMPCControlLoop(
     err_vector_base = np.linalg.norm(p - path_base[0][:2])  # z axis is irrelevant
     log_item = {}
     log_item["err_norm_base"] = np.linalg.norm(err_vector_base).reshape((1,))
-    return v_cmd, log_item
+    return v_cmd, log_item, {}
 
 
 def CrocoBasePathFollowingMPC(
@@ -182,7 +182,7 @@ def CrocoEEPathFollowingFromPlannerMPCControlLoop(
     xs = np.array(ocp.solver.xs)
     v_cmd = xs[1, robot.model.nq :]
 
-    err_vector_ee = log6(robot.T_w_e.actInv(path_EE_SE3[0]))
+    err_vector_ee = log6(robot.T_w_e.actInv(path_EE_SE3[0])).vector
     log_item = {"err_vec_ee": err_vector_ee}
 
     return v_cmd, log_item, {}
@@ -299,20 +299,6 @@ def BaseAndEEPathFollowingFromPlannerMPCControlLoop(
 
     pathSE3_handlebar = construct_EE_path(args, p, past_data["path2D_untimed"])
 
-    # print("BASEcurrent_position", p)
-    # print("EEcurrent_position", robot.T_w_e.translation)
-    # print("=" * 5, "desired handlebar traj", "="  * 5)
-    # for pose in pathSE3_handlebar:
-    #    print(pose.translation)
-
-    ###########################################
-    # TODO: remove
-    # SETTING ROTATION IS BROKEN ATM
-    # BUT SETTING DISTANCES (TRANSLATIONS) IS TOO.
-    # WE DEAL WITH DISTANCES FIRST, UNTIL THAT'S DONE THIS STAYS HERE
-    # for ppp in pathSE3_handlebar:
-    #    ppp.rotation = robot.T_w_e.rotation
-    ###########################################
 
     if args.visualizer:
         if t % int(np.ceil(args.ctrl_freq / 25)) == 0:
@@ -407,6 +393,7 @@ def BaseAndEEPathFollowingMPC(
 
 # TODO: the robot put in is a whole body robot,
 # which you need to implement
+THIS IS THE STILL BROKEN ONE
 def BaseAndDualArmEEPathFollowingMPCControlLoop(
     args,
     robot,
@@ -543,6 +530,7 @@ def BaseAndDualArmEEPathFollowingMPCControlLoop(
     return breakFlag, save_past_dict, log_item
 
 
+NOT TEMPLETIZED YET
 def BaseAndDualArmEEPathFollowingMPC(args, robot, path_planner):
     """
     CrocoEndEffectorPathFollowingMPC
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 bf1d8151855458ddecbfa26d082a2cba21a3650b..d91b9794979d3a499296b19881dca6469adf210b 100644
--- a/python/smc/control/optimal_control/path_following_croco_ocp.py
+++ b/python/smc/control/optimal_control/path_following_croco_ocp.py
@@ -1,8 +1,8 @@
-# TODO: make a bundle method which solves and immediately follows the traj.
 from smc.control.optimal_control.abstract_croco_ocp import CrocoOCP
 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
+from smc.robots.interfaces.whole_body_interface import SingleArmWholeBodyInterface, DualArmWholeBodyInterface
 
 import numpy as np
 import crocoddyl
@@ -115,7 +115,7 @@ class BaseAndEEPathFollowingOCP(CrocoOCP):
     NOTE: the path MUST be time indexed with the SAME time used between the knots
     """
 
-    def __init__(self, args, robot: SingleArmInterface, x0):
+    def __init__(self, args, robot: SingleArmWholeBodyInterface, x0):
         goal = None
         super().__init__(args, robot, x0, goal)
 
@@ -178,8 +178,11 @@ class BaseAndEEPathFollowingOCP(CrocoOCP):
         ].cost.residual.reference = pathSE3[-1]
 
 
+
+JUST THE DUAL ARM IS MISSING
+
 class BaseAndDualArmEEPathFollowingOCP(CrocoOCP):
-    def __init__(self, args, robot: DualArmInterface, x0):
+    def __init__(self, args, robot: DualArmWholeBodyInterface, x0):
         goal = None
         super().__init__(args, robot, x0, goal)
 
diff --git a/python/smc/robots/implementations/heron.py b/python/smc/robots/implementations/heron.py
index c8fd5858d70dae5267f51ec3effd80de80b8470d..570ac099a4a42a730c6effd120349e855119c49b 100644
--- a/python/smc/robots/implementations/heron.py
+++ b/python/smc/robots/implementations/heron.py
@@ -60,6 +60,8 @@ class RealHeronRobotManager(AbstractHeronRobotManager):
         pass
 
 
+TODO: WRITE KNOW STUFF OUT, MAKE COMMENTS FOR YUYAO ON WHAT TO FILL IN
+
 """
 TODO: finish
         self._speed_slider = 1.0  # const