diff --git a/include/robust-equilibrium-lib/solver_LP_abstract.hh b/include/robust-equilibrium-lib/solver_LP_abstract.hh
index a8555e772eb657b0091d8f4a9f70e4dfe4463da1..c1b0adaef928ab77b206cc24627f34b56fd981dc 100644
--- a/include/robust-equilibrium-lib/solver_LP_abstract.hh
+++ b/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.
*/
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index 9887c79a8c83c53ee8e9e2f3f1ead4a61f80baaf..cfb26a4e34069bb950aa42e08245e01af62495c7 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/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);
};
diff --git a/src/solver_LP_qpoases.cpp b/src/solver_LP_qpoases.cpp
index 2b9f5dffe59e153fa6d9a3709294f964a8f73fb3..bfaa73da298c84a9a28c31fc33f2858bc0db7b75 100644
--- a/src/solver_LP_qpoases.cpp
+++ b/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;
}
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index 2265edbb2e73ade3527792aae71a12d5b9906ef1..a5ec8244a7fca2e4520a7158a0a89160f4426ba0 100644
--- a/src/static_equilibrium.cpp
+++ b/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)
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 4cbe5af53981ccb97fa25720294dfdb893adbfdf..feb9cc8d72e83ee1389f23c82313f8d291e83876 100644
--- a/test/test_static_equilibrium.cpp
+++ b/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
{