diff --git a/python/ur_simple_control/.managers.py.swp b/python/ur_simple_control/.managers.py.swp
new file mode 100644
index 0000000000000000000000000000000000000000..f08ae83e3367ae7983ea04cac5959c3c261c03e9
Binary files /dev/null and b/python/ur_simple_control/.managers.py.swp differ
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 f6c362e0bf70d628ee224cc8abc08fbee21221bd..7afae0d2bef6f7304be8900bbdbf553c2ca61646 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 b254c2e3d14548bf200e7191d10442f742c779e0..0ee405b879f06e30223bd6cc93ebbd3579b14418 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_optimal_control.py b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
index 4bac405abba6e60912d2a205cf4b856caa465a23..4f1ce3c1c795361a77f857a3bc438156e2adec25 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -397,10 +397,12 @@ def createBaseAndEEPathFollowingOCP(args, robot : RobotManager, x0):
     it is instantiated to just to stay at the current position.
     NOTE: the path MUST be time indexed with the SAME time used between the knots
     """
-    T_w_e = robot.getT_w_e()
-    # TODO there has to be a path for the base and 
-    # a different one for the ee
-    path_ee = [T_w_e] * args.n_knots
+    if robot.robot_name != "yumi":
+        T_w_e = robot.getT_w_e()
+    else:
+        T_w_e_left, T_w_e_right = robot.getT_w_e()
+    # TODO: have a different reference for each arm
+    path_ee = [T_w_e_left] * args.n_knots
     path_base = [np.append(x0[:2], 0.0)] * args.n_knots
     # underactuation is done by setting max-torque to 0 in the robot model,
     # and since we torques are constrained we're good