Skip to content
Snippets Groups Projects
Commit 7515df44 authored by andreadelprete's avatar andreadelprete
Browse files

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.
parent ad626ed3
No related branches found
No related tags found
No related merge requests found
......@@ -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.
*/
......
......@@ -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);
};
......
......@@ -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;
}
......
......@@ -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)
......
......@@ -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
{
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment