diff --git a/include/centroidal-dynamics-lib/centroidal_dynamics.hh b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
index 28ace399030822e706caf5c6f23a88087c64c5f5..e58532ff97b3b5fe7aacb5e02c4ebe19a09cf9f1 100644
--- a/include/centroidal-dynamics-lib/centroidal_dynamics.hh
+++ b/include/centroidal-dynamics-lib/centroidal_dynamics.hh
@@ -280,7 +280,7 @@ public:
    * @brief findMaximumAcceleration Find the maximal acceleration along a given direction
           find          b, alpha0
           maximize      alpha0
-          subject to    -h <= [-G  (Hv)] [b a0]^T   <= -h
+          subject to    [-G  (Hv)] [b a0]^T  = -h
                         0       <= [b a0]^T <= Inf
 
 
@@ -289,13 +289,15 @@ public:
           alpha0    is the maximal amplitude of the acceleration, for the given direction v
           c         is the CoM position
           G         is the matrix whose columns are the gravito-inertial wrench generators
-          A         is [-G  (Hv)]
-   * @param A
-   * @param h
-   * @param alpha0
+          H         is m*[I3 ĉ]^T
+          h         is m*[-g  (c x -g)]^T
+   * @param H input
+   * @param h input
+   * @param v input
+   * @param alpha0 output
    * @return The status of the LP solver.
    */
-  LP_status findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0);
+  LP_status findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 v, double& alpha0);
 
   /**
    * @brief checkAdmissibleAcceleration return true if the given acceleration is admissible for the given contacts
@@ -305,14 +307,12 @@ public:
           b         are the coefficient of the contact force generators (f = V b)
           a         is the vector3 defining the acceleration
           G         is the matrix whose columns are the gravito-inertial wrench generators
-          h and H come from polytope inequalities
-   * @param G
-   * @param H
-   * @param h
-   * @param a
+          H         is m*[I3 ĉ]^T
+          h         is m*[-g  (c x -g)]^T
+   * @param a the acceleration
    * @return true if the acceleration is admissible, false otherwise
    */
-  bool checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a );
+  bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a );
 
 };
 
diff --git a/include/centroidal-dynamics-lib/util.hh b/include/centroidal-dynamics-lib/util.hh
index 40baee965955e13efd3d2f2a38633b6642ca251d..cf97755f8281e2fc73f282810225eceb5463f0d3 100644
--- a/include/centroidal-dynamics-lib/util.hh
+++ b/include/centroidal-dynamics-lib/util.hh
@@ -62,6 +62,7 @@ namespace centroidal_dynamics
   typedef const Eigen::Ref<const MatrixX3>    & Cref_matrixX3;
   typedef const Eigen::Ref<const Matrix43>    & Cref_matrix43;
   typedef const Eigen::Ref<const Matrix6X>    & Cref_matrix6X;
+  typedef const Eigen::Ref<const Matrix63>    & Cref_matrix63;
   typedef const Eigen::Ref<const MatrixXX>    & Cref_matrixXX;
 
   /**Column major definitions for compatibility with classical eigen use**/
diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp
index f6cfa646cd78cb2a99cfb3b10df9ff5b02c27aef..3799f9a1f358e6994cf653004f94afc3c0f516c6 100644
--- a/src/centroidal_dynamics.cpp
+++ b/src/centroidal_dynamics.cpp
@@ -324,12 +324,21 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
   return LP_STATUS_ERROR;
 }
 
-/**
-  m_d.setZero();
-  m_d.head<3>() = m_mass*m_gravity;
-  m_D.setZero();
-  m_D.block<3,3>(3,0) = crossMatrix(-m_mass*m_gravity);
-*/
+
+LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness){
+  // Take the acceleration in account in D and d :
+  Matrix63 old_D = m_D;
+  Vector6 old_d = m_d;
+  m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
+  m_d.head<3>()= m_mass * (m_gravity - acc);
+  // compute equilibrium robustness with the new D and d
+  LP_status status = computeEquilibriumRobustness(com,robustness);
+  // Switch back to the original values of D and d
+  m_D = old_D;
+  m_d = old_d;
+  return status;
+}
+
 
 LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
 {
@@ -591,10 +600,14 @@ double Equilibrium::convert_emax_to_b0(double emax)
 }
 
 
-LP_status Equilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h, double& alpha0){
-  int m = (int)A.cols() -1 ; // 4* number of contacts
+
+LP_status StaticEquilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,Cref_vector3 v, double& alpha0){
+  int m = (int)m_G_centr.cols() -1 ; // 4* number of contacts
   VectorX b_a0(m+1);
   VectorX c = VectorX::Zero(m+1);
+  MatrixXX A = MatrixXX::Zero(6, m+1);
+  A.topLeftCorner(6,m) = - m_G_centr;
+  A.topRightCorner(6,1) = H * v;
   c(m) = -1.0;  // because we search max alpha0
   VectorX lb = VectorX::Zero(m+1);
   VectorX ub = VectorX::Ones(m+1)*1e10; // Inf
@@ -619,8 +632,9 @@ LP_status Equilibrium::findMaximumAcceleration(Cref_matrixXX A, Cref_vector6 h,
 
 }
 
-bool Equilibrium::checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H, Cref_vector6 h, Cref_vector3 a ){
-  int m = (int)G.cols(); // number of contact * 4
+
+bool StaticEquilibrium::checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a ){
+  int m = (int)m_G_centr.cols(); // number of contact * generator per contacts
   VectorX b(m);
   VectorX c = VectorX::Zero(m);
   VectorX lb = VectorX::Zero(m);
@@ -630,9 +644,9 @@ bool Equilibrium::checkAdmissibleAcceleration(Cref_matrixXX G, Cref_matrixXX H,
   int iter = 0;
   LP_status lpStatus;
   do{
-    lpStatus = m_solver->solve(c, lb, ub, G, Alb, Aub, b);
+    lpStatus = m_solver->solve(c, lb, ub, m_G_centr, Alb, Aub, b);
     iter ++;
-  }while(lpStatus == LP_STATUS_ERROR && iter < 5);
+  }while(lpStatus == LP_STATUS_ERROR && iter < 3);
 
   if(lpStatus==LP_STATUS_OPTIMAL || lpStatus==LP_STATUS_UNBOUNDED)
   {