diff --git a/src/solve_transition.cpp b/src/solve_transition.cpp
index 2b923fbe00f74eb74cc5a45d2bb72f1e1b8b8aa8..0b05c8b7ad6950e8071d8f1544c0748aceeb43eb 100644
--- a/src/solve_transition.cpp
+++ b/src/solve_transition.cpp
@@ -1012,6 +1012,21 @@ void computeCostMidPoint(const ProblemData& pData, MatrixXX& H, VectorX& g){
     g.head<3>() = -midPoint;
 }
 
+void computeCostMinAcceleration(const ProblemData& pData,const VectorX& Ts, const int pointsPerPhase, MatrixXX& H, VectorX& g){
+    double t_total = 0.;
+    for(int i = 0 ; i < Ts.size() ; ++i)
+        t_total+=Ts[i];
+    std::vector<double> timeArray = computeDiscretizedTime(Ts,pointsPerPhase);
+    std::vector<coefs_t> wps_ddc = computeDiscretizedAccelerationWaypoints(pData,t_total,timeArray);
+    // cost : x' H x + 2 x g'
+    H.block<3,3>(0,0) = Matrix3::Zero();
+    g.head<3>() = Vector3::Zero();
+    for(size_t i = 0 ; i < wps_ddc.size() ; ++i){
+        H.block<3,3>(0,0) += Matrix3::Identity() * wps_ddc[i].first * wps_ddc[i].first;
+        g.head<3>() += wps_ddc[i].first*wps_ddc[i].second;
+    }
+}
+
 //cost : min distance between end velocity and the one computed by planning
 void computeCostEndVelocity(const ProblemData& pData,const double T, MatrixXX& H, VectorX& g){
     coefs_t v = computeFinalVelocityPoint(pData,T);
@@ -1084,7 +1099,8 @@ ResultDataCOMTraj solveOnestep(const ProblemData& pData, const VectorX& Ts,const
     VectorX constraint_equivalence;
     std::pair<MatrixXX, VectorX> Ab = computeConstraintsOneStep(pData,Ts,pointsPerPhase,constraint_equivalence);
     #if DC1_CONSTRAINT
-    computeCostMidPoint(pData,H,g);
+    //computeCostMidPoint(pData,H,g);
+    computeCostMinAcceleration(pData,Ts,pointsPerPhase,H,g);
     #else
     computeCostEndVelocity(pData,T,H,g);
     #endif