diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp
index 7afae0d2bef6f7304be8900bbdbf553c2ca61646..6cd696f6d4eab96ea05e9f6248c28ce6b8b91ccf 100644
Binary files a/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp and b/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp differ
diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp
index 0ee405b879f06e30223bd6cc93ebbd3579b14418..66611dacb8ead0c62400d66d3a0c15fbbddc7fd1 100644
Binary files a/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp and b/python/ur_simple_control/optimal_control/.crocoddyl_optimal_control.py.swp differ
diff --git a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
index 47aa20a94482dcebab5aba9da4fb70541ffb274a..d20301eb404c1f8456b6ef53cca7ce6de5a7b078 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -338,7 +338,11 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
 
     # idk if that's necessary
     solver.problem.terminalModel.differential.costs.costs['base_translation'+str(args.n_knots)].cost.residual.reference = path_base[-1]
-    solver.problem.terminalModel.differential.costs.costs['ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
+    if robot.robot_name != "yumi":
+        solver.problem.terminalModel.differential.costs.costs['ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
+    else:
+        solver.problem.terminalModel.differential.costs.costs['l_ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
+        solver.problem.terminalModel.differential.costs.costs['r_ee_pose'+str(args.n_knots)].cost.residual.reference = pathSE3_handlebar[-1]
 
     solver.solve(xs_init, us_init, args.max_solver_iter)
     xs = np.array(solver.xs)
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 4f1ce3c1c795361a77f857a3bc438156e2adec25..942dae2e67bb01c7d43e347023f94268b59beb93 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -516,16 +516,16 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
                     # MASSIVE TODO: actually put in reference for left arm
                     path_ee[i],
                     state.nv)
-            eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
-            runningCostModel.addCost("l_ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+            l_eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+            runningCostModel.addCost("l_ee_pose" + str(i), l_eeTrackingCost, args.ee_pose_cost)
             r_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
                     state,
                     robot.model.getFrameId("robr_joint_7"),
                     # MASSIVE TODO: actually put in reference for left arm
                     path_ee[i], 
                     state.nv)
-            eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
-            runningCostModel.addCost("r_ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+            r_eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+            runningCostModel.addCost("r_ee_pose" + str(i), r_eeTrackingCost, args.ee_pose_cost)
 
 
         baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
@@ -572,7 +572,11 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
                 0.0,
             )
 
-    terminalCostModel.addCost("ee_pose" + str(args.n_knots), eeTrackingCost, args.ee_pose_cost)
+    if robot.robot_name != "yumi":
+        terminalCostModel.addCost("ee_pose" + str(args.n_knots), eeTrackingCost, args.ee_pose_cost)
+    else:
+        terminalCostModel.addCost("l_ee_pose" + str(args.n_knots), l_eeTrackingCost, args.ee_pose_cost)
+        terminalCostModel.addCost("r_ee_pose" + str(args.n_knots), r_eeTrackingCost, args.ee_pose_cost)
     terminalCostModel.addCost("base_translation" + str(args.n_knots), baseTrackingCost, args.base_translation_cost)
 
     # now we define the problem