Return the status of the LP solver rather than returning a boolean, so that...

Return the status of the LP solver rather than returning a boolean, so that the user can have a better understanding of what happened when the LP could not be solved.

include/robust-equilibrium-lib/solver_LP_abstract.hh

@@ -13,6 +13,9 @@
 namespace robust_equilibrium
 {
 
+/**
+  * Available LP solvers.
+  */
 enum ROBUST_EQUILIBRIUM_DLLAPI SolverLP
 {
 #ifdef CLP_FOUND
@@ -21,22 +24,35 @@ enum ROBUST_EQUILIBRIUM_DLLAPI SolverLP
   SOLVER_LP_QPOASES
 };
 
+
+/**
+  * Possible states of an LP solver.
+  */
 enum ROBUST_EQUILIBRIUM_DLLAPI LP_status
 {
-  LP_STATUS_UNKNOWN=-1, //before solve or if postSolve says not optimal
+  LP_STATUS_UNKNOWN=-1,
   LP_STATUS_OPTIMAL=0,
   LP_STATUS_INFEASIBLE=1,
-  LP_STATUS_DUAL_INFEASIBLE=2,
+  LP_STATUS_UNBOUNDED=2,
   LP_STATUS_MAX_ITER_REACHED=3,
   LP_STATUS_ERROR=4
 };
 
+
+/**
+ * @brief Abstract interface for a Linear Program (LP) solver.
+ */
 class ROBUST_EQUILIBRIUM_DLLAPI Solver_LP_abstract
 {
 public:
 
   Solver_LP_abstract(){}
 
+  /**
+   * @brief Create a new LP solver of the specified type.
+   * @param solverType Type of LP solver.
+   * @return A pointer to the new solver.
+   */
   static Solver_LP_abstract* getNewSolver(SolverLP solverType);
 
   /** Solve the linear program
@@ -101,8 +117,6 @@ public:
 
   virtual void getDualSolution(Ref_vectorX res) = 0;
 
-//  virtual void getDualColumnSolution(Ref_vectorX res) = 0;
-
   /** Get the current maximum number of iterations performed
    *  by the solver.
    */

include/robust-equilibrium-lib/static_equilibrium.hh

@@ -101,9 +101,9 @@ public:
    *     d         is the 6d vector containing the gravity part of the gravito-inertial wrench
    * @param com The 2d center of mass position to test.
    * @param robustness The computed measure of robustness.
-   * @return True if the operation succeeded, false otherwise.
+   * @return The status of the LP solver.
    */
-  bool computeEquilibriumRobustness(Cref_vector2 com, double &robustness);
+  LP_status computeEquilibriumRobustness(Cref_vector2 com, double &robustness);
 
   /**
    * @brief Check whether the specified com position is in robust equilibrium.
@@ -122,9 +122,9 @@ public:
    * @param com The 2d center of mass position to test.
    * @param equilibrium True if com is in robust equilibrium, false otherwise.
    * @param e_max Desired robustness level.
-   * @return True if the operation succeeded, false otherwise.
+   * @return The status of the LP solver.
    */
-  bool checkRobustEquilibrium(Cref_vector2 com, bool &equilibrium, double e_max=0.0);
+  LP_status checkRobustEquilibrium(Cref_vector2 com, bool &equilibrium, double e_max=0.0);
 
   /**
    * @brief Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
@@ -143,9 +143,9 @@ public:
    * @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.
+   * @return The status of the LP solver.
   */
-  bool findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com);
+  LP_status findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com);
 
   /**
    * @brief Find the extremum com position that is in robust equilibrium in the specified direction.
@@ -165,9 +165,9 @@ public:
    * @param direction Desired 2d direction.
    * @param com Output 2d com position.
    * @param e_max Desired robustness level.
-   * @return True if the operation succeeded, false otherwise.
+   * @return The status of the LP solver.
    */
-  bool findExtremumInDirection(Cref_vector2 direction, Ref_vector2 com, double e_max=0.0);
+  LP_status findExtremumInDirection(Cref_vector2 direction, Ref_vector2 com, double e_max=0.0);
 
 };
 

src/solver_LP_qpoases.cpp

@@ -80,7 +80,7 @@ namespace robust_equilibrium
     if(ss==-2)
        return LP_STATUS_INFEASIBLE;
     if(ss==-3)
-      return LP_STATUS_DUAL_INFEASIBLE;
+      return LP_STATUS_UNBOUNDED;
     return LP_STATUS_ERROR;
   }
 

src/static_equilibrium.cpp

@@ -125,7 +125,7 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
 }
 
 
-bool StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com, double &robustness)
+LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com, double &robustness)
 {
   const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
 
@@ -157,15 +157,15 @@ bool StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com, double &r
     A.bottomLeftCorner(m,m)   = MatrixXX::Identity(m,m);
     A.bottomRightCorner(m,1)  = -VectorX::Ones(m);
 
-    LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
-    if(lpStatus_primal==LP_STATUS_OPTIMAL)
+    LP_status lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
+    if(lpStatus==LP_STATUS_OPTIMAL)
     {
       robustness = -1.0*m_solver->getObjectiveValue();
-      return true;
+      return lpStatus;
     }
 
-    SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
-    return false;
+    SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
+    return lpStatus;
   }
 
   if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_LP2)
@@ -198,11 +198,11 @@ bool StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com, double &r
     if(lpStatus_primal==LP_STATUS_OPTIMAL)
     {
       robustness = -1.0*m_solver->getObjectiveValue();
-      return true;
+      return lpStatus_primal;
     }
 
-    SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
-    return false;
+    SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
+    return lpStatus_primal;
   }
 
   if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_DLP)
@@ -234,28 +234,28 @@ bool StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com, double &r
     if(lpStatus_dual==LP_STATUS_OPTIMAL)
     {
       robustness = m_solver->getObjectiveValue();
-      return true;
+      return lpStatus_dual;
     }
 
-    SEND_ERROR_MSG("Dual LP problem for com position "+toString(com.transpose())+" could not be solved: "+toString(lpStatus_dual));
-    return false;
+    SEND_DEBUG_MSG("Dual LP problem for com position "+toString(com.transpose())+" could not be solved: "+toString(lpStatus_dual));
+    return lpStatus_dual;
   }
 
   SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
-  return false;
+  return LP_STATUS_ERROR;
 }
 
-bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, bool &equilibrium, double e_max)
+LP_status StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, bool &equilibrium, double e_max)
 {
   if(e_max!=0.0)
   {
     SEND_ERROR_MSG("checkRobustEquilibrium with e_max!=0 not implemented yet");
-    return false;
+    return LP_STATUS_ERROR;
   }
   if(m_algorithm!=STATIC_EQUILIBRIUM_ALGORITHM_PP)
   {
     SEND_ERROR_MSG("checkRobustEquilibrium is only implemented for the PP algorithm");
-    return false;
+    return LP_STATUS_ERROR;
   }
 
   VectorX res = m_HD * com + m_Hd;
@@ -263,14 +263,14 @@ bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, bool &equilibri
     if(res(i)>0.0)
     {
       equilibrium = false;
-      return true;
+      return LP_STATUS_OPTIMAL;
     }
 
   equilibrium = true;
-  return true;
+  return LP_STATUS_OPTIMAL;
 }
 
-bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com)
+LP_status StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com)
 {
   const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
 
@@ -316,14 +316,14 @@ bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, do
         SEND_ERROR_MSG("Error while writing LP solution to file "+filename);
 #endif
 
-      return true;
+      return 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;
+    return lpStatus_primal;
   }
 
   if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_DLP)
@@ -369,29 +369,29 @@ bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, do
       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())+
+                       " 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;
+                       " suggesting it is probably unbounded.");
+        return LP_STATUS_UNBOUNDED;
       }
-      return true;
+      return lpStatus_dual;
     }
 
     com = a0;
-    SEND_WARNING_MSG("Dual LP problem could not be solved suggesting that no equilibrium position with robustness "+
+    SEND_DEBUG_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;
+    return lpStatus_dual;
   }
 
   SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
-  return false;
+  return LP_STATUS_ERROR;
 }
 
-bool StaticEquilibrium::findExtremumInDirection(Cref_vector2 direction, Ref_vector2 com, double e_max)
+LP_status StaticEquilibrium::findExtremumInDirection(Cref_vector2 direction, Ref_vector2 com, double e_max)
 {
   SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
-  return false;
+  return LP_STATUS_ERROR;
 }
 
 bool StaticEquilibrium::computePolytopeProjection(Cref_matrix6X v)

test/test_static_equilibrium.cpp

@@ -42,7 +42,8 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium solv
 {
   int error_counter = 0;
   double rob;
-  bool status, equilibrium;
+  LP_status status;
+  bool equilibrium;
   for(unsigned int i=0; i<comPositions.rows(); i++)
   {
     if(PERF_STRING_1!=NULL)
@@ -51,7 +52,7 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium solv
     if(PERF_STRING_1!=NULL)
       getProfiler().stop(PERF_STRING_1);
 
-    if(status==false)
+    if(status!=LP_STATUS_OPTIMAL)
     {
       if(verb>1)
         SEND_ERROR_MSG(solver_1.getName()+" failed to compute robustness of com position "+toString(comPositions.row(i)));
@@ -65,7 +66,7 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium solv
     if(PERF_STRING_2!=NULL)
       getProfiler().stop(PERF_STRING_2);
 
-    if(status==false)
+    if(status!=LP_STATUS_OPTIMAL)
     {
       if(verb>1)
         SEND_ERROR_MSG(solver_2.getName()+" failed to check equilibrium of com position "+toString(comPositions.row(i)));
@@ -105,14 +106,14 @@ int test_computeEquilibriumRobustness(StaticEquilibrium solver_1, StaticEquilibr
 {
   int error_counter = 0;
   double rob_1, rob_2;
-  bool status;
+  LP_status status;
   for(unsigned int i=0; i<comPositions.rows(); i++)
   {
     getProfiler().start(PERF_STRING_1);
     status = solver_1.computeEquilibriumRobustness(comPositions.row(i), rob_1);
     getProfiler().stop(PERF_STRING_1);
 
-    if(status==false)
+    if(status!=LP_STATUS_OPTIMAL)
     {
       if(verb>1)
         SEND_ERROR_MSG(solver_1.getName()+" failed to compute robustness of com position "+toString(comPositions.row(i)));
@@ -124,7 +125,7 @@ int test_computeEquilibriumRobustness(StaticEquilibrium solver_1, StaticEquilibr
     status = solver_2.computeEquilibriumRobustness(comPositions.row(i), rob_2);
     getProfiler().stop(PERF_STRING_2);
 
-    if(status==false)
+    if(status!=LP_STATUS_OPTIMAL)
     {
       if(verb>1)
         SEND_ERROR_MSG(solver_2.getName()+" failed to compute robustness of com position "+toString(comPositions.row(i)));
@@ -164,7 +165,7 @@ int test_findExtremumOverLine(StaticEquilibrium &solver_to_test, StaticEquilibri
 {
   int error_counter = 0;
   Vector2 a, com;
-  bool status;
+  LP_status status;
   double desired_robustness, robustness;
   for(unsigned int i=0; i<N_TESTS; i++)
   {
@@ -178,10 +179,10 @@ int test_findExtremumOverLine(StaticEquilibrium &solver_to_test, StaticEquilibri
     status  = solver_to_test.findExtremumOverLine(a, a0, desired_robustness, com);
     getProfiler().stop(PERF_STRING_TEST);
 
-    if(status==false)
+    if(status!=LP_STATUS_OPTIMAL)
     {
       status = solver_ground_truth.computeEquilibriumRobustness(a0, robustness);
-      if(status==false)
+      if(status!=LP_STATUS_OPTIMAL)
       {
         error_counter++;
         if(verb>1)
@@ -202,7 +203,7 @@ int test_findExtremumOverLine(StaticEquilibrium &solver_to_test, StaticEquilibri
     status = solver_ground_truth.computeEquilibriumRobustness(com, robustness);
     getProfiler().stop(PERF_STRING_GROUND_TRUTH);
 
-    if(status==false)
+    if(status!=LP_STATUS_OPTIMAL)
     {
       error_counter++;
       if(verb>1)
@@ -233,10 +234,16 @@ void drawRobustnessGrid(StaticEquilibrium &solver, Cref_matrixXX comPositions)
 {
   int grid_size = (int)sqrt(comPositions.rows());
   double rob ;
-  bool status;
+  LP_status status;
   for(unsigned int i=0; i<comPositions.rows(); i++)
   {
     status = solver.computeEquilibriumRobustness(comPositions.row(i), rob);
+    if(status!=LP_STATUS_OPTIMAL)
+    {
+      SEND_ERROR_MSG("Faild to compute equilibrium robustness of com position "+toString(comPositions.row(i))+", error code "+toString(status));
+      rob = -1.0;
+    }
+
     if(rob>=0.0)
     {
       if(rob>9.0)
@@ -299,9 +306,6 @@ 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;
-
   // Generate contact positions and orientations
   bool collision;
   for(unsigned int i=0; i<N_CONTACTS; i++)
@@ -477,10 +481,10 @@ int main()
   const int N_TESTS = 100;
   Vector2 a0 = 0.5*(com_LB+com_UB);
   double e_max;
-  bool status = solver_LP_oases.computeEquilibriumRobustness(a0, e_max);
-  if(status==false)
+  LP_status status = solver_LP_oases.computeEquilibriumRobustness(a0, e_max);
+  if(status!=LP_STATUS_OPTIMAL)
   {
-    SEND_ERROR_MSG(solver_LP_oases.getName()+" failed to compute robustness of com position "+toString(a0.transpose()));
+    SEND_ERROR_MSG(solver_LP_oases.getName()+" failed to compute robustness of com position "+toString(a0.transpose())+", error code: "+toString(status));
   }
   else
   {