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 d737650ae8977398706a53b075ca34bd36a1186a..d6e5ce7afad91b54068d7d5c3f91495151cf07b2 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 3be6cc063006a5e2240fb0fce2d549d9322efdcc..16315d1fbae8bda9f7a3fd5ab43cc05923fcff20 100644
--- a/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
+++ b/python/ur_simple_control/optimal_control/crocoddyl_optimal_control.py
@@ -67,6 +67,7 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
         frameVelocityCost = crocoddyl.CostModelResidual(state, frameVelocityResidual)
         runningCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
         terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e2)
+        terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
     else:
         framePlacementResidual_l = crocoddyl.ResidualModelFramePlacement(
                 state,
@@ -100,6 +101,8 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
         runningCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
         terminalCostModel.addCost("gripperPose_l", goalTrackingCost_l, 1e2)
         terminalCostModel.addCost("gripperPose_r", goalTrackingCost_r, 1e2)
+        terminalCostModel.addCost("velFinal_l", frameVelocityCost_l, 1e3)
+        terminalCostModel.addCost("velFinal_r", frameVelocityCost_r, 1e3)
 
     # put these costs into the running costs
     runningCostModel.addCost("xReg", xRegCost, 1e-3)
@@ -107,7 +110,6 @@ def createCrocoIKOCP(args, robot : RobotManager, x0, goal : pin.SE3):
     # and add the terminal cost, which is the distance to the goal
     # NOTE: shouldn't there be a final velocity = 0 here?
     terminalCostModel.addCost("uReg", uRegCost, 1e3)
-    terminalCostModel.addCost("velFinal", frameVelocityCost, 1e3)
 
     ######################################################################
     #  state constraints  #