diff --git a/include/robust-equilibrium-lib/logger.hh b/include/robust-equilibrium-lib/logger.hh
index 565d9c3c088a5a500d29569ade9ffcd2b61aab78..bd5f8c209e039260ed61230b02f59ec1e39f6d81 100644
--- a/include/robust-equilibrium-lib/logger.hh
+++ b/include/robust-equilibrium-lib/logger.hh
@@ -21,7 +21,7 @@ namespace robust_equilibrium
 {
 
   //#define LOGGER_VERBOSITY_INFO_WARNING_ERROR
-#define LOGGER_VERBOSITY_ALL
+#define LOGGER_VERBOSITY_INFO_WARNING_ERROR
 
 #define SEND_MSG(msg,type)         getLogger().sendMsg(msg,type,__FILE__,__LINE__)
 
diff --git a/include/robust-equilibrium-lib/solver_LP_abstract.hh b/include/robust-equilibrium-lib/solver_LP_abstract.hh
index f807b4c4d5049235caa14f6e4673686e53660d14..6cbe04aee6219a717e2992fc2b7e505428a8d120 100644
--- a/include/robust-equilibrium-lib/solver_LP_abstract.hh
+++ b/include/robust-equilibrium-lib/solver_LP_abstract.hh
@@ -15,7 +15,9 @@ namespace robust_equilibrium
 
 enum ROBUST_EQUILIBRIUM_DLLAPI SolverLP
 {
+#ifdef CLP_FOUND
   SOLVER_LP_CLP,
+#endif
   SOLVER_LP_QPOASES
 };
 
diff --git a/include/robust-equilibrium-lib/solver_LP_clp.hh b/include/robust-equilibrium-lib/solver_LP_clp.hh
index cc39625294420276314dffadbd501f87a2c6131d..f885188b228424decec0dc85db41abc446396067 100644
--- a/include/robust-equilibrium-lib/solver_LP_clp.hh
+++ b/include/robust-equilibrium-lib/solver_LP_clp.hh
@@ -3,6 +3,8 @@
  * Author: Andrea Del Prete
  */
 
+#ifdef CLP_FOUND
+
 #ifndef ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_CLP_HH
 #define ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_CLP_HH
 
@@ -60,3 +62,5 @@ public:
 } // end namespace robust_equilibrium
 
 #endif //ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_CLP_HH
+
+#endif // CLP_FOUND
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index 10fa0357bf8daeaa265d4f578fdcbedbc9388338..68c3a753bd221c57ac654986b3eab23c058450ea 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -32,6 +32,7 @@ private:
   std::string                 m_name;
   StaticEquilibriumAlgorithm  m_algorithm;
   Solver_LP_abstract*         m_solver;
+  SolverLP                    m_solver_type;
 
   unsigned int m_generatorsPerContact;
   double m_mass;
@@ -66,9 +67,9 @@ public:
   bool setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
                       Cref_vectorX frictionCoefficients, StaticEquilibriumAlgorithm alg);
 
-  double computeEquilibriumRobustness(Cref_vector2 com);
+  bool computeEquilibriumRobustness(Cref_vector2 com, double &robustness);
 
-  bool checkRobustEquilibrium(Cref_vector2 com, double e_max=0.0);
+  bool checkRobustEquilibrium(Cref_vector2 com, bool &equilibrium, double e_max=0.0);
 
   /** Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
    * This is equivalent to following the following LP:
@@ -90,7 +91,7 @@ public:
   */
   bool findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com);
 
-  double findExtremumInDirection(Cref_vector2 direction, double e_max=0.0);
+  bool findExtremumInDirection(Cref_vector2 direction, Ref_vector2 com, double e_max=0.0);
 
 };
 
diff --git a/src/solver_LP_abstract.cpp b/src/solver_LP_abstract.cpp
index 8ea2808f3e8ce83daef328c095df757ea68a65f9..7903af1c04b7ecd2a7e75ae82495332950092340 100644
--- a/src/solver_LP_abstract.cpp
+++ b/src/solver_LP_abstract.cpp
@@ -4,11 +4,15 @@
  */
 
 #include <robust-equilibrium-lib/solver_LP_abstract.hh>
-#include <robust-equilibrium-lib/solver_LP_clp.hh>
 #include <robust-equilibrium-lib/solver_LP_qpoases.hh>
 #include <robust-equilibrium-lib/logger.hh>
 #include <iostream>
 
+#ifdef CLP_FOUND
+#include <robust-equilibrium-lib/solver_LP_clp.hh>
+#endif
+
+
 using namespace std;
 
 namespace robust_equilibrium
@@ -16,11 +20,14 @@ namespace robust_equilibrium
 
 Solver_LP_abstract* Solver_LP_abstract::getNewSolver(SolverLP solverType)
 {
-  if(solverType==SOLVER_LP_CLP)
-    return new Solver_LP_clp();
   if(solverType==SOLVER_LP_QPOASES)
     return new Solver_LP_qpoases();
 
+#ifdef CLP_FOUND
+  if(solverType==SOLVER_LP_CLP)
+    return new Solver_LP_clp();
+#endif
+
   SEND_ERROR_MSG("Specified solver type not recognized: "+toString(solverType));
   return NULL;
 }
diff --git a/src/solver_LP_clp.cpp b/src/solver_LP_clp.cpp
index 24aa67d84f8c2ddbd1078f990435cf59ce8e4249..3088ecdfba759eef3a4a377c6c02f638163a6b4d 100644
--- a/src/solver_LP_clp.cpp
+++ b/src/solver_LP_clp.cpp
@@ -3,6 +3,8 @@
  * Author: Andrea Del Prete
  */
 
+#ifdef CLP_FOUND
+
 #include <robust-equilibrium-lib/solver_LP_clp.hh>
 #include "CoinBuild.hpp"
 
@@ -115,3 +117,5 @@ bool Solver_LP_clp::setMaximumTime(double seconds)
 }
 
 } // end namespace robust_equilibrium
+
+#endif //CLP_FOUND
diff --git a/src/solver_LP_qpoases.cpp b/src/solver_LP_qpoases.cpp
index aef5c2e2c9302ccf1695cb15a7cb6444893b9af3..0d45acc70aae4433ab66a7057332228573521838 100644
--- a/src/solver_LP_qpoases.cpp
+++ b/src/solver_LP_qpoases.cpp
@@ -16,7 +16,7 @@ namespace robust_equilibrium
 //    m_options.initialStatusBounds = ST_INACTIVE;
 //    m_options.setToReliable();
     m_options.setToDefault();
-    m_options.printLevel          = PL_LOW; //PL_NONE
+    m_options.printLevel          = PL_NONE; //PL_LOW
     m_options.enableRegularisation = BT_TRUE;
     m_options.enableEqualities = BT_TRUE;
     m_maxIter = 1000;
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index 49509a089eb41c7b22041cc31e52dec827a5d156..6b986e31c918f1449737a633be37c3209e2ecfb1 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -31,6 +31,7 @@ StaticEquilibrium::StaticEquilibrium(string name, double mass, unsigned int gene
   }
 
   m_name = name;
+  m_solver_type = solver_type;
   m_solver = Solver_LP_abstract::getNewSolver(solver_type);
 
   m_generatorsPerContact = generatorsPerContact;
@@ -130,7 +131,7 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
 }
 
 
-double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
+bool StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com, double &robustness)
 {
   const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
 
@@ -164,10 +165,13 @@ double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
 
     LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
     if(lpStatus_primal==LP_STATUS_OPTIMAL)
-      return -1.0*m_solver->getObjectiveValue();
+    {
+      robustness = -1.0*m_solver->getObjectiveValue();
+      return true;
+    }
 
     SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
-    return -1.0;
+    return false;
   }
 
   if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_LP2)
@@ -199,10 +203,12 @@ double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
     LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
     if(lpStatus_primal==LP_STATUS_OPTIMAL)
     {
-      return -1.0*m_solver->getObjectiveValue();
+      robustness = -1.0*m_solver->getObjectiveValue();
+      return true;
     }
+
     SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
-    return -1.0;
+    return false;
   }
 
   if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_DLP)
@@ -232,17 +238,20 @@ double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
 
     LP_status lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
     if(lpStatus_dual==LP_STATUS_OPTIMAL)
-      return m_solver->getObjectiveValue();
+    {
+      robustness = m_solver->getObjectiveValue();
+      return true;
+    }
 
-    SEND_ERROR_MSG("Dual LP problem could not be solved: "+toString(lpStatus_dual));
-    return -1.0;
+    SEND_ERROR_MSG("Dual LP problem for com position "+toString(com.transpose())+" could not be solved: "+toString(lpStatus_dual));
+    return false;
   }
 
   SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
-  return -1.0;
+  return false;
 }
 
-bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, double e_max)
+bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, bool &equilibrium, double e_max)
 {
   if(e_max!=0.0)
   {
@@ -258,7 +267,12 @@ bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, double e_max)
   VectorX res = m_HD * com + m_Hd;
   for(long i=0; i<res.size(); i++)
     if(res(i)>0.0)
-      return false;
+    {
+      equilibrium = false;
+      return true;
+    }
+
+  equilibrium = true;
   return true;
 }
 
@@ -299,7 +313,10 @@ bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, do
       return true;
     }
 
-    SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
+    com = a0;
+    SEND_DEBUG_MSG("Primal LP problem could not be solved suggesting that no equilibrium position with robustness "+
+                     toString(e_max)+" exists over the line starting from "+toString(a0.transpose())+
+                     " in direction "+toString(a.transpose())+", solver error code: "+toString(lpStatus_primal));
     return false;
   }
 
@@ -316,11 +333,11 @@ bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, do
     */
     Vector6 v;
     Vector6 c = m_D*a0 + m_d - m_G_centr*VectorX::Ones(m)*e_max;
-    Vector6 lb = Vector6::Ones()*-1e100;
-    Vector6 ub = Vector6::Ones()*1e100;
+    Vector6 lb = Vector6::Ones()*-1e10;
+    Vector6 ub = Vector6::Ones()*1e10;
     VectorX Alb = VectorX::Zero(m+1);
     Alb(m) = -1.0;
-    VectorX Aub = VectorX::Ones(m+1)*1e100;
+    VectorX Aub = VectorX::Ones(m+1)*1e10;
     Aub(m) = -1.0;
     MatrixX6 A(m+1,6);
     A.topRows(m) = m_G_centr.transpose();
@@ -331,10 +348,23 @@ bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, do
     {
       double p = m_solver->getObjectiveValue();
       com = a0 + a*p;
+
+      // since QP oases cannot detect unboundedness we check here whether the objective value is a large negative value
+      if(m_solver_type==SOLVER_LP_QPOASES && p<-1e7)
+      {
+        SEND_DEBUG_MSG("Dual LP problem with robustness "+toString(e_max)+
+                         " over the line starting from "+toString(a0.transpose())+
+                       " in direction "+toString(a.transpose())+" has large negative objective value: "+toString(p)+
+                         " suggesting it is probably unbounded.");
+        return false;
+      }
       return true;
     }
 
-    SEND_ERROR_MSG("Dual LP problem could not be solved: "+toString(lpStatus_dual));
+    com = a0;
+    SEND_WARNING_MSG("Dual LP problem could not be solved suggesting that no equilibrium position with robustness "+
+                     toString(e_max)+" exists over the line starting from "+toString(a0.transpose())+
+                     " in direction "+toString(a.transpose())+", solver error code: "+toString(lpStatus_dual));
     return false;
   }
 
@@ -342,10 +372,10 @@ bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, do
   return false;
 }
 
-double StaticEquilibrium::findExtremumInDirection(Cref_vector2 direction, double e_max)
+bool StaticEquilibrium::findExtremumInDirection(Cref_vector2 direction, Ref_vector2 com, double e_max)
 {
   SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
-  return 0.0;
+  return false;
 }
 
 bool StaticEquilibrium::computePolytopeProjection(Cref_matrix6X v)
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
index 5f4bd9e2e856345f1e62d4ae2a881d75c378d691..7a837077a55208f5844b459fe9c8b27307d54b76 100644
--- a/test/test_LP_solvers.cpp
+++ b/test/test_LP_solvers.cpp
@@ -3,14 +3,15 @@
  * Author: Andrea Del Prete
  */
 
+#ifdef CLP_FOUND
 #include "ClpSimplex.hpp"
 #include "CoinTime.hpp"
 #include "CoinBuild.hpp"
 #include "CoinModel.hpp"
+#include <robust-equilibrium-lib/solver_LP_clp.hh>
+#endif
 
 #include <qpOASES.hpp>
-
-#include <robust-equilibrium-lib/solver_LP_clp.hh>
 #include <robust-equilibrium-lib/solver_LP_qpoases.hh>
 
 #include <iostream>
@@ -20,6 +21,7 @@ using namespace std;
 using namespace robust_equilibrium;
 USING_NAMESPACE_QPOASES
 
+#ifdef CLP_FOUND
 /** Example addRows.cpp */
 void test_addRows()
 {
@@ -364,6 +366,7 @@ void test_small_LP()
   else
     cout << "Didn’t find optimal solution." << endl;
 }
+#endif
 
 int main()
 {
@@ -378,14 +381,6 @@ int main()
     real_t lbA[1] = { -1.0 };
     real_t ubA[1] = { 2.0 };
 
-    /* Setup data of second LP. */
-    real_t g_new[2] = { 1.0, 1.5 };
-    real_t lb_new[2] = { 0.0, -1.0 };
-    real_t ub_new[2] = { 5.0, -0.5 };
-    real_t lbA_new[1] = { -2.0 };
-    real_t ubA_new[1] = { 1.0 };
-
-
     /* Setting up QProblem object with zero Hessian matrix. */
     QProblem example( 2,1,HST_ZERO );
 
@@ -398,7 +393,8 @@ int main()
     example.init( 0,g,A,lb,ub,lbA,ubA, nWSR,0 );
   }
 
-//  test_addRows();
+#ifdef CLP_FOUND
+  test_addRows();
   test_small_LP();
 
   Solver_LP_abstract *solver = Solver_LP_abstract::getNewSolver(SOLVER_LP_CLP);
@@ -419,6 +415,7 @@ int main()
   }
   else
     cout<<"solver_LP_clp failed to solve the problem\n";
+#endif
 
 //  char x[81];
 //  int iRow;
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 1beeaa22bd0d6199396d0f4b3a0cd0b67f8ab21b..cadc209898e44d2d73cec17cb11ed5bf30fc209d 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -22,33 +22,126 @@ using namespace std;
 #define PERF_DLP_COIN "Compute Equilibrium Robustness with DLP coin"
 #define PERF_DLP_OASES "Compute Equilibrium Robustness with DLP oases"
 
-#define EPS 1e-5  // required precision
+#define EPS 1e-4  // required precision
+
+/** Check the coherence between the method StaticEquilibrium::computeEquilibriumRobustness
+ * and the method StaticEquilibrium::checkRobustEquilibrium.
+ * @param solver_1 Solver used to test computeEquilibriumRobustness.
+ * @param solver_2 Solver used to test checkRobustEquilibrium.
+ * @param comPositions List of 2d com positions on which to perform the tests.
+ * @param PERF_STRING_1 String to use for logging the computation times of solver_1
+ * @param PERF_STRING_2 String to use for logging the computation times of solver_2
+ * @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
+ */
+int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium solver_1,
+                                                          StaticEquilibrium solver_2,
+                                                          Cref_matrixXX comPositions,
+                                                          const char* PERF_STRING_1=NULL,
+                                                          const char* PERF_STRING_2=NULL,
+                                                          int verb=0)
+{
+  int error_counter = 0;
+  double rob;
+  bool status, equilibrium;
+  for(unsigned int i=0; i<comPositions.rows(); i++)
+  {
+    if(PERF_STRING_1!=NULL)
+      getProfiler().start(PERF_STRING_1);
+    status = solver_1.computeEquilibriumRobustness(comPositions.row(i), rob);
+    if(PERF_STRING_1!=NULL)
+      getProfiler().stop(PERF_STRING_1);
+
+    if(status==false)
+    {
+      if(verb>1)
+        SEND_ERROR_MSG(solver_1.getName()+" failed to compute robustness of com position "+toString(comPositions.row(i)));
+      error_counter++;
+      continue;
+    }
+
+    if(PERF_STRING_2!=NULL)
+      getProfiler().start(PERF_STRING_2);
+    status= solver_2.checkRobustEquilibrium(comPositions.row(i), equilibrium);
+    if(PERF_STRING_2!=NULL)
+      getProfiler().stop(PERF_STRING_2);
+
+    if(status==false)
+    {
+      if(verb>1)
+        SEND_ERROR_MSG(solver_2.getName()+" failed to check equilibrium of com position "+toString(comPositions.row(i)));
+      error_counter++;
+      continue;
+    }
+
+    if(equilibrium==true && rob<0.0)
+    {
+      if(verb>0)
+        SEND_ERROR_MSG(solver_1.getName()+" says com is in equilibrium while "+solver_2.getName()+" computed a negative robustness measure "+toString(rob));
+      error_counter++;
+    }
+    else if(equilibrium==false && rob>0.0)
+    {
+      if(verb>0)
+        SEND_ERROR_MSG(solver_1.getName()+" says com is not in equilibrium while "+solver_2.getName()+" computed a positive robustness measure "+toString(rob));
+      error_counter++;
+    }
+  }
+
+  if(verb>1)
+    SEND_INFO_MSG("Test test_computeEquilibriumRobustness_vs_checkEquilibrium "+solver_1.getName()+" VS "+solver_2.getName()+": "+toString(error_counter)+" error(s).");
+  return error_counter;
+}
 
 /** Test two different solvers on the method StaticEquilibrium::computeEquilibriumRobustness.
+ * @param solver_1 First solver to test.
+ * @param solver_2 Second solver to test.
+ * @param comPositions List of 2d com positions on which to perform the tests.
+ * @param PERF_STRING_1 String to use for logging the computation times of solver_1
+ * @param PERF_STRING_2 String to use for logging the computation times of solver_2
+ * @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
  */
 int test_computeEquilibriumRobustness(StaticEquilibrium solver_1, StaticEquilibrium solver_2, Cref_matrixXX comPositions,
                                       const char* PERF_STRING_1, const char* PERF_STRING_2, int verb=0)
 {
   int error_counter = 0;
+  double rob_1, rob_2;
+  bool status;
   for(unsigned int i=0; i<comPositions.rows(); i++)
   {
     getProfiler().start(PERF_STRING_1);
-    double rob_1  = solver_1.computeEquilibriumRobustness(comPositions.row(i));
+    status = solver_1.computeEquilibriumRobustness(comPositions.row(i), rob_1);
     getProfiler().stop(PERF_STRING_1);
 
+    if(status==false)
+    {
+      if(verb>1)
+        SEND_ERROR_MSG(solver_1.getName()+" failed to compute robustness of com position "+toString(comPositions.row(i)));
+      error_counter++;
+      continue;
+    }
+
     getProfiler().start(PERF_STRING_2);
-    double rob_2 = solver_2.computeEquilibriumRobustness(comPositions.row(i));
+    status = solver_2.computeEquilibriumRobustness(comPositions.row(i), rob_2);
     getProfiler().stop(PERF_STRING_2);
 
+    if(status==false)
+    {
+      if(verb>1)
+        SEND_ERROR_MSG(solver_2.getName()+" failed to compute robustness of com position "+toString(comPositions.row(i)));
+      error_counter++;
+      continue;
+    }
+
     if(fabs(rob_1-rob_2)>EPS)
     {
-      if(verb>0)
+      if(verb>1)
         SEND_ERROR_MSG(solver_1.getName()+" and "+solver_2.getName()+" returned different results: "+toString(rob_1)+" VS "+toString(rob_2));
       error_counter++;
     }
   }
 
-  SEND_INFO_MSG("Test computeEquilibriumRobustness "+solver_1.getName()+" VS "+solver_2.getName()+": "+toString(error_counter)+" error(s).");
+  if(verb>0)
+    SEND_INFO_MSG("Test computeEquilibriumRobustness "+solver_1.getName()+" VS "+solver_2.getName()+": "+toString(error_counter)+" error(s).");
   return error_counter;
 }
 
@@ -56,15 +149,23 @@ int test_computeEquilibriumRobustness(StaticEquilibrium solver_1, StaticEquilibr
  * calls the method findExtremumOverLine of the solver to test to find the extremum over a random
  * line with a specified robustness. Then it checks that the point found really has the specified
  * robustness by using the ground-truth solver.
+ * @param solver_to_test Solver to test.
+ * @param solver_ground_truth Second solver to use as ground truth.
+ * @param a0 A 2d com position that allows for static equilibrium.
+ * @param N_TESTS Number of tests to perform.
+ * @param e_max Maximum value for the desired robustness.
+ * @param PERF_STRING_TEST String to use for logging the computation times of solver_to_test
+ * @param PERF_STRING_GROUND_TRUTH String to use for logging the computation times of solver_ground_truth
+ * @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
  */
-int test_findExtremumOverLine(StaticEquilibrium solver_to_test, StaticEquilibrium solver_ground_truth,
+int test_findExtremumOverLine(StaticEquilibrium &solver_to_test, StaticEquilibrium &solver_ground_truth,
                               Cref_vector2 a0, int N_TESTS, double e_max,
                               const char* PERF_STRING_TEST, const char* PERF_STRING_GROUND_TRUTH, int verb=0)
 {
   int error_counter = 0;
   Vector2 a, com;
   bool status;
-  double desired_robustness;
+  double desired_robustness, robustness;
   for(unsigned int i=0; i<N_TESTS; i++)
   {
     uniform(-1.0*Vector2::Ones(), Vector2::Ones(), a);
@@ -76,39 +177,63 @@ int test_findExtremumOverLine(StaticEquilibrium solver_to_test, StaticEquilibriu
 
     if(status==false)
     {
-      error_counter++;
-      if(verb>0)
-        SEND_ERROR_MSG(solver_to_test.getName()+" failed to find extremum over line");
+      status = solver_ground_truth.computeEquilibriumRobustness(a0, robustness);
+      if(status==false)
+      {
+        error_counter++;
+        if(verb>1)
+          SEND_ERROR_MSG(solver_ground_truth.getName()+" failed to compute equilibrium robustness of com position "+toString(a0.transpose()));
+      }
+      else if(robustness>desired_robustness)
+      {
+        error_counter++;
+        if(verb>1)
+          SEND_ERROR_MSG(solver_to_test.getName()+" failed to find extremum over line starting from "+
+                         toString(a0.transpose())+" with robustness "+toString(desired_robustness)+" while "+
+                         solver_ground_truth.getName()+" says this position has robustness "+toString(robustness));
+      }
       continue;
     }
 
     getProfiler().start(PERF_STRING_GROUND_TRUTH);
-    double robustness = solver_ground_truth.computeEquilibriumRobustness(com);
+    status = solver_ground_truth.computeEquilibriumRobustness(com, robustness);
     getProfiler().stop(PERF_STRING_GROUND_TRUTH);
 
-    if(fabs(robustness-desired_robustness)>EPS)
+    if(status==false)
     {
-      if(verb>0)
-        SEND_ERROR_MSG(solver_to_test.getName()+" found this com position: "+toString(com)+
+      error_counter++;
+      if(verb>1)
+        SEND_ERROR_MSG(solver_ground_truth.getName()+" failed to compute equilibrium robustness of com posiiton "+toString(com.transpose()));
+    }
+    else if(fabs(robustness-desired_robustness)>EPS)
+    {
+      if(verb>1)
+        SEND_ERROR_MSG(solver_to_test.getName()+" found this extremum: "+toString(com.transpose())+
+                       " over the line starting at "+toString(a0.transpose())+" in direction "+toString(a.transpose())+
                        " which should have robustness "+toString(desired_robustness)+
                        " but actually has robustness "+toString(robustness));
       error_counter++;
     }
   }
 
-  SEND_INFO_MSG("Test findExtremumOverLine "+solver_to_test.getName()+" VS "+solver_ground_truth.getName()+": "+toString(error_counter)+" error(s).");
+  if(verb>0)
+    SEND_INFO_MSG("Test findExtremumOverLine "+solver_to_test.getName()+" VS "+solver_ground_truth.getName()+": "+toString(error_counter)+" error(s).");
   return error_counter;
 }
 
 /** Draw a grid on the screen using the robustness computed with the method
  *  StaticEquilibrium::computeEquilibriumRobustness.
+ * @param solver The solver to use for computing the equilibrium robustness.
+ * @param comPositions Grid of CoM positions in the form of an Nx2 matrix.
  */
-void drawRobustnessGrid(StaticEquilibrium solver, Cref_matrixXX comPositions)
+void drawRobustnessGrid(StaticEquilibrium &solver, Cref_matrixXX comPositions)
 {
   int grid_size = (int)sqrt(comPositions.rows());
+  double rob ;
+  bool status;
   for(unsigned int i=0; i<comPositions.rows(); i++)
   {
-    double rob = solver.computeEquilibriumRobustness(comPositions.row(i));
+    status = solver.computeEquilibriumRobustness(comPositions.row(i), rob);
     if(rob>=0.0)
     {
       if(rob>9.0)
@@ -124,7 +249,11 @@ void drawRobustnessGrid(StaticEquilibrium solver, Cref_matrixXX comPositions)
 
 int main()
 {
-  srand ((unsigned int)(time(NULL)));
+  unsigned int seed = (unsigned int)(time(NULL));
+//  seed = 1446456544;
+  srand (seed);
+  cout<<"Initialize random number generator with seed "<<seed<<endl;
+
   RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
   RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
 
@@ -139,21 +268,24 @@ int main()
   CONTACT_POINT_LOWER_BOUNDS << 0.0,  0.0,  0.0;
   CONTACT_POINT_UPPER_BOUNDS << 0.5,  0.5,  0.5;
   double gamma = atan(mu);   // half friction cone angle
-  RPY_LOWER_BOUNDS << -0*gamma, -0*gamma, -M_PI;
-  RPY_UPPER_BOUNDS << +0*gamma, +0*gamma, +M_PI;
+  RPY_LOWER_BOUNDS << -1*gamma, -1*gamma, -M_PI;
+  RPY_UPPER_BOUNDS << +1*gamma, +1*gamma, +M_PI;
   double X_MARG = 0.07;
   double Y_MARG = 0.07;
   const int GRID_SIZE = 15;
   /************************************ END USER PARAMETERS *****************************/
 
-  StaticEquilibrium solver_PP("PP", mass, generatorsPerContact, SOLVER_LP_CLP);
-  StaticEquilibrium solver_LP_coin("LP coin", mass, generatorsPerContact, SOLVER_LP_CLP);
+  StaticEquilibrium solver_PP("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES);
   StaticEquilibrium solver_LP_oases("LP oases", mass, generatorsPerContact, SOLVER_LP_QPOASES);
-  StaticEquilibrium solver_LP2_coin("LP2 coin", mass, generatorsPerContact, SOLVER_LP_CLP);
   StaticEquilibrium solver_LP2_oases("LP2 oases", mass, generatorsPerContact, SOLVER_LP_QPOASES);
-  StaticEquilibrium solver_DLP_coin("DLP coin", mass, generatorsPerContact, SOLVER_LP_CLP);
   StaticEquilibrium solver_DLP_oases("DLP oases", mass, generatorsPerContact, SOLVER_LP_QPOASES);
 
+#ifdef CLP_FOUND
+  StaticEquilibrium solver_LP_coin("LP coin", mass, generatorsPerContact, SOLVER_LP_CLP);
+  StaticEquilibrium solver_LP2_coin("LP2 coin", mass, generatorsPerContact, SOLVER_LP_CLP);
+  StaticEquilibrium solver_DLP_coin("DLP coin", mass, generatorsPerContact, SOLVER_LP_CLP);
+#endif
+
   MatrixXX contact_pos = MatrixXX::Zero(N_CONTACTS, 3);
   MatrixXX contact_rpy = MatrixXX::Zero(N_CONTACTS, 3);
   MatrixXX p = MatrixXX::Zero(4*N_CONTACTS,3); // contact points
@@ -161,10 +293,8 @@ int main()
   VectorX frictionCoefficients(4*N_CONTACTS);
   frictionCoefficients.fill(mu);
 
-  contact_pos << 0.122,  0.361,  0.071,
-                 0.243,  0.029,  0.112;
-  contact_rpy << 0.205, -0.005, -1.335,
-                 -0.02 ,  0.206,  0.506;
+//  contact_pos << 0.122,  0.361,  0.071, 0.243,  0.029,  0.112;
+//  contact_rpy << 0.205, -0.005, -1.335, -0.02 ,  0.206,  0.506;
 
   // Generate contact positions and orientations
   bool collision;
@@ -197,6 +327,7 @@ int main()
     printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
   }
 
+  // compute upper and lower bounds of com positions to test
   RVector2 com_LB, com_UB;
   com_LB(0) = p.col(0).minCoeff()-X_MARG;
   com_UB(0) = p.col(0).maxCoeff()+X_MARG;
@@ -206,6 +337,7 @@ int main()
   MatrixXi contactPointCoord(4*N_CONTACTS,2);
   VectorX minDistContactPoint = 1e10*VectorX::Ones(4*N_CONTACTS);
 
+  // create grid of com positions to test
   VectorX x_range(GRID_SIZE), y_range(GRID_SIZE);
   x_range.setLinSpaced(GRID_SIZE,com_LB(0),com_UB(0));
   y_range.setLinSpaced(GRID_SIZE,com_LB(1),com_UB(1));
@@ -256,70 +388,98 @@ int main()
   }
 
   getProfiler().start(PERF_LP_PREPARATION);
-  if(!solver_LP_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP))
+  if(!solver_LP_oases.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP))
   {
     printf("Error while setting new contacts");
     return -1;
   }
   getProfiler().stop(PERF_LP_PREPARATION);
+
   getProfiler().start(PERF_LP_PREPARATION);
-  if(!solver_LP_oases.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP))
+  if(!solver_LP2_oases.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP2))
   {
     printf("Error while setting new contacts");
     return -1;
   }
   getProfiler().stop(PERF_LP_PREPARATION);
+
   getProfiler().start(PERF_LP_PREPARATION);
-  if(!solver_LP2_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP2))
+  if(!solver_DLP_oases.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_DLP))
   {
     printf("Error while setting new contacts");
     return -1;
   }
   getProfiler().stop(PERF_LP_PREPARATION);
-  getProfiler().start(PERF_LP_PREPARATION);
-  if(!solver_LP2_oases.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP2))
+
+  getProfiler().start(PERF_PP);
+  bool res = solver_PP.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_PP);
+  getProfiler().stop(PERF_PP);
+  if(!res)
   {
     printf("Error while setting new contacts");
     return -1;
   }
-  getProfiler().stop(PERF_LP_PREPARATION);
+
+#ifdef CLP_FOUND
   getProfiler().start(PERF_LP_PREPARATION);
-  if(!solver_DLP_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_DLP))
+  if(!solver_LP_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP))
   {
     printf("Error while setting new contacts");
     return -1;
   }
   getProfiler().stop(PERF_LP_PREPARATION);
+
   getProfiler().start(PERF_LP_PREPARATION);
-  if(!solver_DLP_oases.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_DLP))
+  if(!solver_LP2_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP2))
   {
     printf("Error while setting new contacts");
     return -1;
   }
   getProfiler().stop(PERF_LP_PREPARATION);
 
-  getProfiler().start(PERF_PP);
-  bool res = solver_PP.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_PP);
-  getProfiler().stop(PERF_PP);
-  if(!res)
+  getProfiler().start(PERF_LP_PREPARATION);
+  if(!solver_DLP_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_DLP))
   {
     printf("Error while setting new contacts");
     return -1;
   }
+  getProfiler().stop(PERF_LP_PREPARATION);
+#endif
+
 
   drawRobustnessGrid(solver_DLP_oases, comPositions);
 
-  test_computeEquilibriumRobustness(solver_LP_coin, solver_LP_oases, comPositions, PERF_LP_COIN, PERF_LP_OASES);
-  test_computeEquilibriumRobustness(solver_LP_coin, solver_LP2_coin, comPositions, PERF_LP_COIN, PERF_LP2_COIN);
-  test_computeEquilibriumRobustness(solver_LP_coin, solver_LP2_oases, comPositions, PERF_LP_COIN, PERF_LP2_OASES);
-  test_computeEquilibriumRobustness(solver_LP_coin, solver_DLP_coin, comPositions, PERF_LP_COIN, PERF_DLP_COIN);
-  test_computeEquilibriumRobustness(solver_LP_coin, solver_DLP_oases, comPositions, PERF_LP_COIN, PERF_DLP_OASES);
+  test_computeEquilibriumRobustness(solver_DLP_oases, solver_LP_oases, comPositions, PERF_DLP_OASES, PERF_LP_OASES, 1);
+  test_computeEquilibriumRobustness(solver_DLP_oases, solver_LP2_oases, comPositions, PERF_DLP_OASES, PERF_LP2_OASES, 1);
+
+  test_computeEquilibriumRobustness_vs_checkEquilibrium(solver_LP_oases, solver_PP, comPositions, PERF_LP_OASES, NULL, 1);
+  test_computeEquilibriumRobustness_vs_checkEquilibrium(solver_LP2_oases, solver_PP, comPositions, PERF_LP2_OASES, NULL, 1);
+  test_computeEquilibriumRobustness_vs_checkEquilibrium(solver_DLP_oases, solver_PP, comPositions, PERF_DLP_OASES, NULL, 1);
+
+#ifdef CLP_FOUND
+  test_computeEquilibriumRobustness(solver_DLP_oases, solver_LP2_coin, comPositions, PERF_DLP_OASES, PERF_LP2_COIN, 1);
+  test_computeEquilibriumRobustness(solver_DLP_oases, solver_DLP_coin, comPositions, PERF_DLP_OASES, PERF_DLP_COIN, 1);
+  test_computeEquilibriumRobustness(solver_DLP_oases, solver_LP_coin, comPositions, PERF_DLP_OASES, PERF_LP_COIN, 1);
+
+  test_computeEquilibriumRobustness_vs_checkEquilibrium(solver_LP_coin, solver_PP, comPositions, PERF_LP_COIN, 1);
+  test_computeEquilibriumRobustness_vs_checkEquilibrium(solver_LP2_coin, solver_PP, comPositions, PERF_LP2_COIN, 1);
+  test_computeEquilibriumRobustness_vs_checkEquilibrium(solver_DLP_coin, solver_PP, comPositions, PERF_DLP_COIN, 1);
+#endif
+
 
-  Vector2 a0 = 0.5*(com_LB+com_UB);
   const int N_TESTS = 100;
-  const double E_MAX = 5.0;
-  test_findExtremumOverLine(solver_LP_oases, solver_DLP_oases, a0, N_TESTS, E_MAX, "EXTREMUM OVER LINE LP OASES", PERF_DLP_OASES, 1);
-  test_findExtremumOverLine(solver_DLP_oases, solver_DLP_oases, a0, N_TESTS, E_MAX, "EXTREMUM OVER LINE DLP OASES", PERF_DLP_OASES, 1);
+  Vector2 a0 = 0.5*(com_LB+com_UB);
+  double e_max;
+  bool status = solver_LP_oases.computeEquilibriumRobustness(a0, e_max);
+  if(status==false)
+  {
+    SEND_ERROR_MSG(solver_LP_oases.getName()+" failed to compute robustness of com position "+toString(a0.transpose()));
+  }
+  else
+  {
+    test_findExtremumOverLine(solver_LP_oases, solver_DLP_oases, a0, N_TESTS, e_max, "EXTREMUM OVER LINE LP OASES", PERF_DLP_OASES, 2);
+    test_findExtremumOverLine(solver_DLP_oases, solver_DLP_oases, a0, N_TESTS, e_max, "EXTREMUM OVER LINE DLP OASES", PERF_DLP_OASES, 2);
+  }
 
   getProfiler().report_all();