diff --git a/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp b/python/ur_simple_control/optimal_control/.crocoddyl_mpc.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..f6c362e0bf70d628ee224cc8abc08fbee21221bd
Binary files /dev/null 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 a9bc0612533751805f146d6ed5daa83f8a7d0b2c..b254c2e3d14548bf200e7191d10442f742c779e0 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 65d3858af6cf57d6cd82e4117b1373ae180cc5e5..47aa20a94482dcebab5aba9da4fb70541ffb274a 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_mpc.py
@@ -329,7 +329,12 @@ def BaseAndEEPathFollowingMPCControlLoop(args, robot : RobotManager, solver : cr
         #print('adding base', path_base[i])
         #print("this was the prev ref", runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference)
         runningModel.differential.costs.costs['base_translation' + str(i)].cost.residual.reference = path_base[i]
-        runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+        if robot.robot_name != "yumi":
+            runningModel.differential.costs.costs['ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+        # MASSIVE TODO: actually have different references for left and right arm
+        else:
+            runningModel.differential.costs.costs['l_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
+            runningModel.differential.costs.costs['r_ee_pose' + str(i)].cost.residual.reference = pathSE3_handlebar[i]
 
     # idk if that's necessary
     solver.problem.terminalModel.differential.costs.costs['base_translation'+str(args.n_knots)].cost.residual.reference = path_base[-1]
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 0e4588e9043336899d1e3d927866678d58224185..4bac405abba6e60912d2a205cf4b856caa465a23 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -491,14 +491,41 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
         runningCostModel.addCost("uReg", uRegCost, 1e-3)
         if args.solver == "boxfddp":
             runningCostModel.addCost("limitCost", limitCost, 1e3)
-        eePoseResidual = crocoddyl.ResidualModelFramePlacement(
-                state,
-                robot.model.getFrameId("tool0"),
-                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
-                state.nv)
-        eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
-        runningCostModel.addCost("ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+
+        ##########################
+        #  single arm reference  #
+        ##########################
+        if robot.robot_name != "yumi":
+            eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+                    state,
+                    robot.model.getFrameId("tool0"),
+                    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
+                    state.nv)
+            eeTrackingCost = crocoddyl.CostModelResidual(state, eePoseResidual)
+            runningCostModel.addCost("ee_pose" + str(i), eeTrackingCost, args.ee_pose_cost)
+        #########################
+        #  dual arm references  #
+        #########################
+        else:
+            l_eePoseResidual = crocoddyl.ResidualModelFramePlacement(
+                    state,
+                    robot.model.getFrameId("robl_joint_7"),
+                    # 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)
+            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)
+
+
         baseTranslationResidual = crocoddyl.ResidualModelFrameTranslation(
                 state,
                 robot.model.getFrameId("mobile_base"),