Add algorithms (LP and dual LP) to compute extremum equilibrium point over a...

Add algorithms (LP and dual LP) to compute extremum equilibrium point over a line. Add relative tests (successful). Clean up old tests for computation of equilibrium robustness.

include/robust-equilibrium-lib/static_equilibrium.hh

@@ -29,6 +29,7 @@ class ROBUST_EQUILIBRIUM_DLLAPI StaticEquilibrium
 private:
   static bool m_is_cdd_initialized;
 
+  std::string                 m_name;
   StaticEquilibriumAlgorithm  m_algorithm;
   Solver_LP_abstract*         m_solver;
 
@@ -58,7 +59,9 @@ private:
   bool computePolytopeProjection(Cref_matrix6X v);
 
 public:
-  StaticEquilibrium(double mass, unsigned int generatorsPerContact, SolverLP solver_type);
+  StaticEquilibrium(std::string name, double mass, unsigned int generatorsPerContact, SolverLP solver_type);
+
+  std::string getName(){ return m_name; }
 
   bool setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
                       Cref_vectorX frictionCoefficients, StaticEquilibriumAlgorithm alg);
@@ -67,7 +70,25 @@ public:
 
   bool checkRobustEquilibrium(Cref_vector2 com, double e_max=0.0);
 
-  double findExtremumOverLine(Cref_vector2 a, double b, 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:
+   *     find          c, b
+   *     maximize      c
+   *     subject to    G b = D (a c + a0) + d
+   *                   b  >= b0
+   *   where
+   *     b         are the m coefficients of the contact force generators (f = G b)
+   *     b0        is the m-dimensional vector of identical values that are proportional to e_max
+   *     c         is the 1d line parameter
+   *     G         is the 6xm matrix whose columns are the gravito-inertial wrench generators
+   *     D         is the 6x2 matrix mapping the CoM position in gravito-inertial wrench
+   *     d         is the 6d vector containing the gravity part of the gravito-inertial wrench
+   * @param a 2d vector representing the line direction
+   * @param a0 2d vector representing an arbitrary point over the line
+   * @param e_max Desired robustness in terms of the maximum force error tolerated by the system
+   * @return True if the operation succeeded, false otherwise.
+  */
+  bool findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com);
 
   double findExtremumInDirection(Cref_vector2 direction, double e_max=0.0);
 

src/static_equilibrium.cpp

@@ -16,7 +16,7 @@ namespace robust_equilibrium
 
 bool StaticEquilibrium::m_is_cdd_initialized = false;
 
-StaticEquilibrium::StaticEquilibrium(double mass, unsigned int generatorsPerContact, SolverLP solver_type)
+StaticEquilibrium::StaticEquilibrium(string name, double mass, unsigned int generatorsPerContact, SolverLP solver_type)
 {
   if(!m_is_cdd_initialized)
   {
@@ -30,6 +30,7 @@ StaticEquilibrium::StaticEquilibrium(double mass, unsigned int generatorsPerCont
     generatorsPerContact = 4;
   }
 
+  m_name = name;
   m_solver = Solver_LP_abstract::getNewSolver(solver_type);
 
   m_generatorsPerContact = generatorsPerContact;
@@ -237,7 +238,7 @@ double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
     return -1.0;
   }
 
-  SEND_ERROR_MSG("checkRobustEquilibrium is only implemented for the LP algorithm");
+  SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
   return -1.0;
 }
 
@@ -261,10 +262,84 @@ bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, double e_max)
   return true;
 }
 
-double StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, double b, double e_max)
+bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com)
 {
-  SEND_ERROR_MSG("findExtremumOverLine not implemented yet");
-  return 0.0;
+  const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+
+  if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_LP)
+  {
+    /* Compute the extremum CoM position over the line a*p + a0 that is in robust equilibrium
+     * by solving the following LP:
+          find          b, p
+          minimize      -p
+          subject to    D (a p + a0) + d <= G (b + b0) <= D (a p + a0) + d
+                        0       <= b <= Inf
+        where
+          b0+b      are the coefficient of the contact force generators (f = G (b0+b))
+          b0        is the robustness measure
+          p         is the line parameter
+          G         is the matrix whose columns are the gravito-inertial wrench generators
+    */
+    VectorX b_p(m+1);
+    VectorX c = VectorX::Zero(m+1);
+    c(m) = -1.0;
+    VectorX lb = -VectorX::Zero(m+1);
+    lb(m) = -1e5;
+    VectorX ub = VectorX::Ones(m+1)*1e10;
+    Vector6 Alb = m_D*a0 + m_d - m_G_centr*VectorX::Ones(m)*e_max;
+    Vector6 Aub = Alb;
+    Matrix6X A = Matrix6X::Zero(6, m+1);
+    A.leftCols(m)     = m_G_centr;
+    A.rightCols(1)    = -m_D*a;
+
+    LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_p);
+    if(lpStatus_primal==LP_STATUS_OPTIMAL)
+    {
+      com = a0 + a*b_p(m);
+      return true;
+    }
+
+    SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
+    return false;
+  }
+
+  if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_DLP)
+  {
+    /* Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium
+     * by solving the following dual LP:
+          find          v
+          minimize      (D a0 + d -G b0)' v
+          subject to    0  <= G' v    <= Inf
+                        -1 <= a' D' v <= -1
+        where
+          G         is the matrix whose columns are the gravito-inertial wrench generators
+    */
+    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;
+    VectorX Alb = VectorX::Zero(m+1);
+    Alb(m) = -1.0;
+    VectorX Aub = VectorX::Ones(m+1)*1e100;
+    Aub(m) = -1.0;
+    MatrixX6 A(m+1,6);
+    A.topRows(m) = m_G_centr.transpose();
+    A.bottomRows<1>() = (m_D*a).transpose();
+
+    LP_status lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
+    if(lpStatus_dual==LP_STATUS_OPTIMAL)
+    {
+      double p = m_solver->getObjectiveValue();
+      com = a0 + a*p;
+      return true;
+    }
+
+    SEND_ERROR_MSG("Dual LP problem could not be solved: "+toString(lpStatus_dual));
+    return false;
+  }
+
+  SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
+  return false;
 }
 
 double StaticEquilibrium::findExtremumInDirection(Cref_vector2 direction, double e_max)

src/stop-watch.cpp

@@ -243,7 +243,7 @@ void Stopwatch::report(string perf_name, int precision, std::ostream& output)
   
   PerformanceData& perf_info = records_of->find(perf_name)->second;
   
-  const int MAX_NAME_LENGTH = 40;
+  const int MAX_NAME_LENGTH = 60;
   string pad = "";
   for (int i = perf_name.length(); i<MAX_NAME_LENGTH; i++)
     pad.append(" ");

test/test_static_equilibrium.cpp

@@ -13,47 +13,147 @@ using namespace robust_equilibrium;
 using namespace Eigen;
 using namespace std;
 
-#define PERF_PP "PP"
-#define PERF_LP_PREPARATION "LP preparation"
-#define PERF_LP_COIN "LP coin"
-#define PERF_LP_OASES "LP oases"
-#define PERF_LP2_COIN "LP2 coin"
-#define PERF_LP2_OASES "LP2 oases"
-#define PERF_DLP_COIN "DLP coin"
-#define PERF_DLP_OASES "DLP oases"
+#define PERF_PP "Polytope Projection"
+#define PERF_LP_PREPARATION "Computation of GIWC generators"
+#define PERF_LP_COIN "Compute Equilibrium Robustness with LP coin"
+#define PERF_LP_OASES "Compute Equilibrium Robustness with LP oases"
+#define PERF_LP2_COIN "Compute Equilibrium Robustness with LP2 coin"
+#define PERF_LP2_OASES "Compute Equilibrium Robustness with LP2 oases"
+#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
+
+/** Test two different solvers on the method StaticEquilibrium::computeEquilibriumRobustness.
+ */
+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;
+  for(unsigned int i=0; i<comPositions.rows(); i++)
+  {
+    getProfiler().start(PERF_STRING_1);
+    double rob_1  = solver_1.computeEquilibriumRobustness(comPositions.row(i));
+    getProfiler().stop(PERF_STRING_1);
+
+    getProfiler().start(PERF_STRING_2);
+    double rob_2 = solver_2.computeEquilibriumRobustness(comPositions.row(i));
+    getProfiler().stop(PERF_STRING_2);
+
+    if(fabs(rob_1-rob_2)>EPS)
+    {
+      if(verb>0)
+        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).");
+  return error_counter;
+}
+
+/** Test method StaticEquilibrium::findExtremumOverLine. The test works in this way: first it
+ * 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.
+ */
+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;
+  for(unsigned int i=0; i<N_TESTS; i++)
+  {
+    uniform(-1.0*Vector2::Ones(), Vector2::Ones(), a);
+    desired_robustness = (rand()/ value_type(RAND_MAX))*e_max;
+
+    getProfiler().start(PERF_STRING_TEST);
+    status  = solver_to_test.findExtremumOverLine(a, a0, desired_robustness, com);
+    getProfiler().stop(PERF_STRING_TEST);
+
+    if(status==false)
+    {
+      error_counter++;
+      if(verb>0)
+        SEND_ERROR_MSG(solver_to_test.getName()+" failed to find extremum over line");
+      continue;
+    }
+
+    getProfiler().start(PERF_STRING_GROUND_TRUTH);
+    double robustness = solver_ground_truth.computeEquilibriumRobustness(com);
+    getProfiler().stop(PERF_STRING_GROUND_TRUTH);
+
+    if(fabs(robustness-desired_robustness)>EPS)
+    {
+      if(verb>0)
+        SEND_ERROR_MSG(solver_to_test.getName()+" found this com position: "+toString(com)+
+                       " 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).");
+  return error_counter;
+}
+
+/** Draw a grid on the screen using the robustness computed with the method
+ *  StaticEquilibrium::computeEquilibriumRobustness.
+ */
+void drawRobustnessGrid(StaticEquilibrium solver, Cref_matrixXX comPositions)
+{
+  int grid_size = (int)sqrt(comPositions.rows());
+  for(unsigned int i=0; i<comPositions.rows(); i++)
+  {
+    double rob = solver.computeEquilibriumRobustness(comPositions.row(i));
+    if(rob>=0.0)
+    {
+      if(rob>9.0)
+        rob = 9.0;
+      printf("%d ", (int)rob);
+    }
+    else
+      printf("- ");
+    if((i+1)%grid_size==0)
+      printf("\n");
+  }
+}
 
 int main()
 {
-//  init_cdd_library();
   srand ((unsigned int)(time(NULL)));
+  RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
+  RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
 
-  int ret = 0;
+  /************************************** USER PARAMETERS *******************************/
   double mass = 70.0;
   double mu = 0.3;  // friction coefficient
   unsigned int generatorsPerContact = 4;
   unsigned int N_CONTACTS = 2;
-  unsigned int N_POINTS = 50;   // number of com positions to test
   double MIN_FEET_DISTANCE = 0.3;
   double LX = 0.5*0.2172;        // half foot size in x direction
   double LY = 0.5*0.138;         // half foot size in y direction
-  RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
   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
-  RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
   RPY_LOWER_BOUNDS << -0*gamma, -0*gamma, -M_PI;
   RPY_UPPER_BOUNDS << +0*gamma, +0*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_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);
 
-  StaticEquilibrium solver_PP(mass, generatorsPerContact, SOLVER_LP_CLP);
-  StaticEquilibrium solver_LP_coin(mass, generatorsPerContact, SOLVER_LP_CLP);
-  StaticEquilibrium solver_LP_oases(mass, generatorsPerContact, SOLVER_LP_QPOASES);
-  StaticEquilibrium solver_LP2_coin(mass, generatorsPerContact, SOLVER_LP_CLP);
-  StaticEquilibrium solver_LP2_oases(mass, generatorsPerContact, SOLVER_LP_QPOASES);
-  StaticEquilibrium solver_DLP_coin(mass, generatorsPerContact, SOLVER_LP_CLP);
-  StaticEquilibrium solver_DLP_oases(mass, generatorsPerContact, SOLVER_LP_QPOASES);
   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
@@ -85,10 +185,8 @@ int main()
 
 //     generate contact orientation
     uniform(RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, contact_rpy.row(i));
-
     generate_rectangle_contacts(LX, LY, contact_pos.row(i), contact_rpy.row(i),
                                 p.middleRows<4>(i*4), N.middleRows<4>(i*4));
-
     printf("Contact surface %d position (%.3f,%.3f,%.3f) ", i, contact_pos(i,0), contact_pos(i,1), contact_pos(i,2));
     printf("Orientation (%.3f,%.3f,%.3f)\n", contact_rpy(i,0), contact_rpy(i,1), contact_rpy(i,2));
   }
@@ -99,6 +197,64 @@ int main()
     printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
   }
 
+  RVector2 com_LB, com_UB;
+  com_LB(0) = p.col(0).minCoeff()-X_MARG;
+  com_UB(0) = p.col(0).maxCoeff()+X_MARG;
+  com_LB(1) = p.col(1).minCoeff()-Y_MARG;
+  com_UB(1) = p.col(1).maxCoeff()+Y_MARG;
+
+  MatrixXi contactPointCoord(4*N_CONTACTS,2);
+  VectorX minDistContactPoint = 1e10*VectorX::Ones(4*N_CONTACTS);
+
+  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));
+  MatrixXX comPositions(GRID_SIZE*GRID_SIZE, 2);
+  cout<<"Gonna test equilibrium on a 2d grid of "<<GRID_SIZE<<"X"<<GRID_SIZE<<" points ";
+  cout<<"ranging from "<<com_LB<<" to "<<com_UB<<endl;
+  for(unsigned int i=0; i<GRID_SIZE; i++)
+  {
+    for(unsigned int j=0; j<GRID_SIZE; j++)
+    {
+      comPositions(i*GRID_SIZE+j, 1) = y_range(GRID_SIZE-1-i);
+      comPositions(i*GRID_SIZE+j, 0) = x_range(j);
+
+      // look for contact point positions on grid
+      for(long k=0; k<4*N_CONTACTS; k++)
+      {
+        double dist = (p.block<1,2>(k,0) - comPositions.row(i*GRID_SIZE+j)).norm();
+        if(dist < minDistContactPoint(k))
+        {
+          minDistContactPoint(k) = dist;
+          contactPointCoord(k,0) = i;
+          contactPointCoord(k,1) = j;
+        }
+      }
+    }
+  }
+
+  cout<<"\nContact point positions\n";
+  bool contactPointDrawn;
+  for(unsigned int i=0; i<GRID_SIZE; i++)
+  {
+    for(unsigned int j=0; j<GRID_SIZE; j++)
+    {
+      contactPointDrawn = false;
+      for(long k=0; k<4*N_CONTACTS; k++)
+      {
+        if(contactPointCoord(k,0)==i && contactPointCoord(k,1)==j)
+        {
+          cout<<"X ";
+          contactPointDrawn = true;
+          break;
+        }
+      }
+      if(contactPointDrawn==false)
+        cout<<"- ";
+    }
+    printf("\n");
+  }
+
   getProfiler().start(PERF_LP_PREPARATION);
   if(!solver_LP_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP))
   {
@@ -151,147 +307,21 @@ int main()
     return -1;
   }
 
-  RVector2 com_LB, com_UB;
-  com_LB(0) = p.col(0).minCoeff()-X_MARG;
-  com_UB(0) = p.col(0).maxCoeff()+X_MARG;
-  com_LB(1) = p.col(1).minCoeff()-Y_MARG;
-  com_UB(1) = p.col(1).maxCoeff()+Y_MARG;
-
-  MatrixXi contactPointCoord(4*N_CONTACTS,2);
-  VectorX minDistContactPoint = 1e10*VectorX::Ones(4*N_CONTACTS);
-
-  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));
-  MatrixXX equilibrium(GRID_SIZE, GRID_SIZE);
-  Vector2 com;
-  cout<<"Gonna test equilibrium on a 2d grid of "<<GRID_SIZE<<"X"<<GRID_SIZE<<" points ";
-  cout<<"ranging from "<<com_LB<<" to "<<com_UB<<endl;
-  for(unsigned int i=0; i<GRID_SIZE; i++)
-  {
-    com(1) = y_range(GRID_SIZE-1-i);
-    for(unsigned int j=0; j<GRID_SIZE; j++)
-    {
-//      uniform(com_LB, com_UB, com);
-      com(0) = x_range(j);
-
-      getProfiler().start(PERF_LP_COIN);
-      double rob_coin  = solver_LP_coin.computeEquilibriumRobustness(com);
-      getProfiler().stop(PERF_LP_COIN);
-
-      getProfiler().start(PERF_LP_OASES);
-      double rob_oases = solver_LP_oases.computeEquilibriumRobustness(com);
-      getProfiler().stop(PERF_LP_OASES);
-
-      getProfiler().start(PERF_LP2_COIN);
-      double rob_coin2  = solver_LP2_coin.computeEquilibriumRobustness(com);
-      getProfiler().stop(PERF_LP2_COIN);
-
-      getProfiler().start(PERF_LP2_OASES);
-      double rob_oases2 = solver_LP2_oases.computeEquilibriumRobustness(com);
-      getProfiler().stop(PERF_LP2_OASES);
-
-      getProfiler().start(PERF_DLP_COIN);
-      double rob_DLP_coin  = solver_DLP_coin.computeEquilibriumRobustness(com);
-      getProfiler().stop(PERF_DLP_COIN);
-
-      getProfiler().start(PERF_DLP_OASES);
-      double rob_DLP_oases = solver_DLP_oases.computeEquilibriumRobustness(com);
-      getProfiler().stop(PERF_DLP_OASES);
-
-      if(fabs(rob_coin-rob_oases)>1e-5)
-        SEND_ERROR_MSG("LP_coin and LP_oases returned different results: "+toString(rob_coin)+" VS "+toString(rob_oases));
-
-      if(fabs(rob_coin-rob_oases2)>1e-5)
-        SEND_ERROR_MSG("LP_coin and LP2_oases returned different results: "+toString(rob_coin)+" VS "+toString(rob_oases2));
-
-//      if(fabs(rob_coin-rob_coin2)>1e-5)
-//        SEND_ERROR_MSG("LP_coin and LP2_coin returned different results: "+toString(rob_coin)+" VS "+toString(rob_coin2));
-
-      if(fabs(rob_coin-rob_DLP_oases)>1e-5)
-        SEND_ERROR_MSG("LP_coin and DLP_oases returned different results: "+toString(rob_coin)+" VS "+toString(rob_DLP_oases));
+  drawRobustnessGrid(solver_DLP_oases, comPositions);
 
-      if(fabs(rob_coin-rob_DLP_coin)>1e-5)
-        SEND_ERROR_MSG("LP_coin and DLP_coin returned different results: "+toString(rob_coin)+" VS "+toString(rob_DLP_coin));
+  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);
 
-      if(solver_PP.checkRobustEquilibrium(com, 0.0))
-      {
-        equilibrium(i,j) = 1.0;
-        if(rob_coin<=0)
-          SEND_ERROR_MSG("PP says com is in equilibrium but LP_coin find negative robustness "+toString(rob_coin));
-        if(rob_oases<=0)
-          SEND_ERROR_MSG("PP says com is in equilibrium but LP_oases find negative robustness "+toString(rob_oases));
-        if(rob_coin2<=0)
-          SEND_ERROR_MSG("PP says com is in equilibrium but LP_coin2 find negative robustness "+toString(rob_coin2));
-        if(rob_oases2<=0)
-          SEND_ERROR_MSG("PP says com is in equilibrium but LP_oases2 find negative robustness "+toString(rob_oases2));
-        if(rob_DLP_oases<=0)
-          SEND_ERROR_MSG("PP says com is in equilibrium but DLP_oases find negative robustness "+toString(rob_DLP_oases));
-        if(rob_DLP_coin<=0)
-          SEND_ERROR_MSG("PP says com is in equilibrium but DLP_coin negative robustness "+toString(rob_DLP_coin));
-
-        if(rob_coin>9.0)
-          rob_coin = 9.0;
-        printf("%d ", (int)rob_coin);
-      }
-      else
-      {
-        equilibrium(i,j) = 0.0;
-        if(rob_coin>0)
-          SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_coin find positive robustness "+toString(rob_coin));
-        if(rob_oases>0)
-          SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_oases find positive robustness "+toString(rob_oases));
-        if(rob_coin2>0)
-          SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_coin2 find positive robustness "+toString(rob_coin2));
-        if(rob_oases2>0)
-          SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_oases2 find positive robustness "+toString(rob_oases2));
-        if(rob_DLP_coin>0)
-          SEND_ERROR_MSG("PP says com is NOT in equilibrium but DLP_coin find positive robustness "+toString(rob_DLP_coin));
-        if(rob_DLP_oases>0)
-          SEND_ERROR_MSG("PP says com is NOT in equilibrium but DLP_oases find positive robustness "+toString(rob_DLP_oases));
-        printf("- ");
-      }
-
-      // look for contact point positions on grid
-      for(long k=0; k<4*N_CONTACTS; k++)
-      {
-        double dist = (p.block<1,2>(k,0) - com.transpose()).norm();
-        if(dist < minDistContactPoint(k))
-        {
-          minDistContactPoint(k) = dist;
-          contactPointCoord(k,0) = i;
-          contactPointCoord(k,1) = j;
-        }
-      }
-    }
-    printf("\n");
-  }
-
-//  cout<<"Max dist between contact points and grid points "<<minDistContactPoint.maxCoeff()<<"\n";
-
-  cout<<"\nContact point position on the same grid\n";
-  bool contactPointDrawn;
-  for(unsigned int i=0; i<GRID_SIZE; i++)
-  {
-    for(unsigned int j=0; j<GRID_SIZE; j++)
-    {
-      contactPointDrawn = false;
-      for(long k=0; k<4*N_CONTACTS; k++)
-      {
-        if(contactPointCoord(k,0)==i && contactPointCoord(k,1)==j)
-        {
-          cout<<"X ";
-          contactPointDrawn = true;
-          break;
-        }
-      }
-      if(contactPointDrawn==false)
-        cout<<"- ";
-    }
-    printf("\n");
-  }
+  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);
 
   getProfiler().report_all();
 
-  return ret;
+  return 0;
 }