From 2ee806d69e38c995ca2350963e6a3cd1f2b96d14 Mon Sep 17 00:00:00 2001
From: andreadelprete <andre.delprete@gmail.com>
Date: Mon, 2 Nov 2015 11:57:35 +0100
Subject: [PATCH] [MAJOR IMPROVEMENTS] When using qpOases in DLP check for
large negative objective value, which suggests likely unboundedness of the
problem. Change signature of most methods of StaticEquilibrium class so that
they return true if operation succeeded, false otherwise. Add tests of LP
formulations against polytope projection. Add ifdef CLP_FOUND everywhere in
the code so that solver CLP is now optional (still to fix the cmakelist
though). Add documentation.
---
include/robust-equilibrium-lib/logger.hh | 2 +-
.../solver_LP_abstract.hh | 2 +
.../robust-equilibrium-lib/solver_LP_clp.hh | 4 +
.../static_equilibrium.hh | 7 +-
src/solver_LP_abstract.cpp | 13 +-
src/solver_LP_clp.cpp | 4 +
src/solver_LP_qpoases.cpp | 2 +-
src/static_equilibrium.cpp | 66 +++--
test/test_LP_solvers.cpp | 19 +-
test/test_static_equilibrium.cpp | 258 ++++++++++++++----
10 files changed, 291 insertions(+), 86 deletions(-)
diff --git a/include/robust-equilibrium-lib/logger.hh b/include/robust-equilibrium-lib/logger.hh
index 565d9c3..bd5f8c2 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 f807b4c..6cbe04a 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 cc39625..f885188 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 10fa035..68c3a75 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 8ea2808..7903af1 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 24aa67d..3088ecd 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 aef5c2e..0d45acc 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 49509a0..6b986e3 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 5f4bd9e..7a83707 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 1beeaa2..cadc209 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();
--
GitLab