From ac0257ff12270e45efb4481abf8f5e86fe23fba4 Mon Sep 17 00:00:00 2001
From: andreadelprete <andre.delprete@gmail.com>
Date: Fri, 6 Nov 2015 17:14:10 +0100
Subject: [PATCH] [SMALL IMPROVEMENTS] When solving dual LP switch infeasible
and unbounded return flags. In all equilibrium algorithms check whether the
number of contacts is 0, in which case return appropriate result (typically
infeasible). Improve test_static_equilibrium.
---
.../static_equilibrium.hh | 11 +
src/static_equilibrium.cpp | 32 +-
test/test_static_equilibrium.cpp | 444 +++++++++---------
3 files changed, 260 insertions(+), 227 deletions(-)
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index f5c23e4..a5d935b 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -101,6 +101,8 @@ public:
*/
std::string getName(){ return m_name; }
+ StaticEquilibriumAlgorithm getAlgorithm(){ return m_algorithm; }
+
/**
* @brief Specify a new set of contacts.
* All 3d vectors are expressed in a reference frame having the z axis aligned with gravity.
@@ -131,6 +133,9 @@ public:
* @param com The 3d center of mass position to test.
* @param robustness The computed measure of robustness.
* @return The status of the LP solver.
+ * @note If the system is in force closure the status will be LP_STATUS_UNBOUNDED, meaning that the
+ * system can reach infinite robustness. This is due to the fact that we are not considering
+ * any upper limit for the friction cones.
*/
LP_status computeEquilibriumRobustness(Cref_vector3 com, double &robustness);
@@ -173,6 +178,9 @@ public:
* @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 The status of the LP solver.
+ * @note If the system is in force closure the status will be LP_STATUS_UNBOUNDED, meaning that the
+ * system can reach infinite robustness. This is due to the fact that we are not considering
+ * any upper limit for the friction cones.
*/
LP_status findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com);
@@ -195,6 +203,9 @@ public:
* @param com Output 3d com position.
* @param e_max Desired robustness level.
* @return The status of the LP solver.
+ * @note If the system is in force closure the status will be LP_STATUS_UNBOUNDED, meaning that the
+ * system can reach infinite robustness. This is due to the fact that we are not considering
+ * any upper limit for the friction cones.
*/
LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max=0.0);
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index fd55a68..04e51d5 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -138,6 +138,8 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, double &robustness)
{
const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+ if(m==0)
+ return LP_STATUS_INFEASIBLE;
if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_LP)
{
@@ -246,8 +248,14 @@ LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, doub
robustness = convert_b0_to_emax(m_solver->getObjectiveValue());
return lpStatus_dual;
}
-
SEND_DEBUG_MSG("Dual LP problem for com position "+toString(com.transpose())+" could not be solved: "+toString(lpStatus_dual));
+
+ // switch UNFEASIBLE and UNBOUNDED flags because we are solving dual problem
+ if(lpStatus_dual==LP_STATUS_INFEASIBLE)
+ lpStatus_dual = LP_STATUS_UNBOUNDED;
+ else if(lpStatus_dual==LP_STATUS_UNBOUNDED)
+ lpStatus_dual = LP_STATUS_INFEASIBLE;
+
return lpStatus_dual;
}
@@ -257,6 +265,11 @@ LP_status StaticEquilibrium::computeEquilibriumRobustness(Cref_vector3 com, doub
LP_status StaticEquilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max)
{
+ if(m_G_centr.cols()==0)
+ {
+ equilibrium=false;
+ return LP_STATUS_OPTIMAL;
+ }
if(e_max!=0.0)
{
SEND_ERROR_MSG("checkRobustEquilibrium with e_max!=0 not implemented yet");
@@ -283,6 +296,9 @@ LP_status StaticEquilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equi
LP_status StaticEquilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com)
{
const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+ if(m_G_centr.cols()==0)
+ return LP_STATUS_INFEASIBLE;
+
double b0 = convert_emax_to_b0(e_max);
if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_LP)
@@ -383,8 +399,9 @@ LP_status StaticEquilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a
" 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 LP_STATUS_UNBOUNDED;
+ lpStatus_dual = LP_STATUS_UNBOUNDED;
}
+
return lpStatus_dual;
}
@@ -392,15 +409,24 @@ LP_status StaticEquilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a
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));
+
+ // switch UNFEASIBLE and UNBOUNDED flags because we are solving dual problem
+ if(lpStatus_dual==LP_STATUS_INFEASIBLE)
+ lpStatus_dual = LP_STATUS_UNBOUNDED;
+ else if(lpStatus_dual==LP_STATUS_UNBOUNDED)
+ lpStatus_dual = LP_STATUS_INFEASIBLE;
+
return lpStatus_dual;
}
- SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
+ SEND_ERROR_MSG("findExtremumOverLine is not implemented for the specified algorithm");
return LP_STATUS_ERROR;
}
LP_status StaticEquilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max)
{
+ if(m_G_centr.cols()==0)
+ return LP_STATUS_INFEASIBLE;
SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
return LP_STATUS_ERROR;
}
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 680cd9c..15d1fa9 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -33,11 +33,11 @@ using namespace std;
* @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,
+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,
+ const string& PERF_STRING_1="",
+ const string& PERF_STRING_2="",
int verb=0)
{
int error_counter = 0;
@@ -46,30 +46,30 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium solv
bool equilibrium;
for(unsigned int i=0; i<comPositions.rows(); i++)
{
- if(PERF_STRING_1!=NULL)
+ if(!PERF_STRING_1.empty())
getProfiler().start(PERF_STRING_1);
- status = solver_1.computeEquilibriumRobustness(comPositions.row(i), rob);
- if(PERF_STRING_1!=NULL)
+ status = solver_1->computeEquilibriumRobustness(comPositions.row(i), rob);
+ if(!PERF_STRING_1.empty())
getProfiler().stop(PERF_STRING_1);
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)));
+ 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)
+ if(!PERF_STRING_2.empty())
getProfiler().start(PERF_STRING_2);
- status= solver_2.checkRobustEquilibrium(comPositions.row(i), equilibrium);
- if(PERF_STRING_2!=NULL)
+ status= solver_2->checkRobustEquilibrium(comPositions.row(i), equilibrium);
+ if(!PERF_STRING_2.empty())
getProfiler().stop(PERF_STRING_2);
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)));
+ SEND_ERROR_MSG(solver_2->getName()+" failed to check equilibrium of com position "+toString(comPositions.row(i)));
error_counter++;
continue;
}
@@ -77,19 +77,19 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium solv
if(equilibrium==true && rob<0.0)
{
if(verb>1)
- SEND_ERROR_MSG(solver_2.getName()+" says com is in equilibrium while "+solver_1.getName()+" computed a negative robustness measure "+toString(rob));
+ SEND_ERROR_MSG(solver_2->getName()+" says com is in equilibrium while "+solver_1->getName()+" computed a negative robustness measure "+toString(rob));
error_counter++;
}
else if(equilibrium==false && rob>0.0)
{
if(verb>1)
- SEND_ERROR_MSG(solver_2.getName()+" says com is not in equilibrium while "+solver_1.getName()+" computed a positive robustness measure "+toString(rob));
+ SEND_ERROR_MSG(solver_2->getName()+" says com is not in equilibrium while "+solver_1->getName()+" computed a positive robustness measure "+toString(rob));
error_counter++;
}
}
if(verb>0)
- cout<<"Test test_computeEquilibriumRobustness_vs_checkEquilibrium "+solver_1.getName()+" VS "+solver_2.getName()+": "+toString(error_counter)+" error(s).\n";
+ cout<<"Test test_computeEquilibriumRobustness_vs_checkEquilibrium "+solver_1->getName()+" VS "+solver_2->getName()+": "+toString(error_counter)+" error(s).\n";
return error_counter;
}
@@ -101,8 +101,8 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium solv
* @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 test_computeEquilibriumRobustness(StaticEquilibrium *solver_1, StaticEquilibrium *solver_2, Cref_matrixXX comPositions,
+ const string& PERF_STRING_1, const string& PERF_STRING_2, int verb=0)
{
int error_counter = 0;
double rob_1, rob_2;
@@ -110,25 +110,25 @@ int test_computeEquilibriumRobustness(StaticEquilibrium solver_1, StaticEquilibr
for(unsigned int i=0; i<comPositions.rows(); i++)
{
getProfiler().start(PERF_STRING_1);
- status = solver_1.computeEquilibriumRobustness(comPositions.row(i), rob_1);
+ status = solver_1->computeEquilibriumRobustness(comPositions.row(i), rob_1);
getProfiler().stop(PERF_STRING_1);
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)));
+ 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);
- status = solver_2.computeEquilibriumRobustness(comPositions.row(i), rob_2);
+ status = solver_2->computeEquilibriumRobustness(comPositions.row(i), rob_2);
getProfiler().stop(PERF_STRING_2);
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)));
+ SEND_ERROR_MSG(solver_2->getName()+" failed to compute robustness of com position "+toString(comPositions.row(i)));
error_counter++;
continue;
}
@@ -136,13 +136,13 @@ int test_computeEquilibriumRobustness(StaticEquilibrium solver_1, StaticEquilibr
if(fabs(rob_1-rob_2)>EPS)
{
if(verb>1)
- SEND_ERROR_MSG(solver_1.getName()+" and "+solver_2.getName()+" returned different results: "+toString(rob_1)+" VS "+toString(rob_2));
+ SEND_ERROR_MSG(solver_1->getName()+" and "+solver_2->getName()+" returned different results: "+toString(rob_1)+" VS "+toString(rob_2));
error_counter++;
}
}
if(verb>0)
- cout<<"Test computeEquilibriumRobustness "+solver_1.getName()+" VS "+solver_2.getName()+": "+toString(error_counter)+" error(s).\n";
+ cout<<"Test computeEquilibriumRobustness "+solver_1->getName()+" VS "+solver_2->getName()+": "+toString(error_counter)+" error(s).\n";
return error_counter;
}
@@ -159,9 +159,9 @@ int test_computeEquilibriumRobustness(StaticEquilibrium solver_1, StaticEquilibr
* @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_vector3 a0, int N_TESTS, double e_max,
- const char* PERF_STRING_TEST, const char* PERF_STRING_GROUND_TRUTH, int verb=0)
+ const string& PERF_STRING_TEST, const string& PERF_STRING_GROUND_TRUTH, int verb=0)
{
int error_counter = 0;
Vector3 a, com;
@@ -176,43 +176,43 @@ int test_findExtremumOverLine(StaticEquilibrium &solver_to_test, StaticEquilibri
desired_robustness = e_max - EPS;
getProfiler().start(PERF_STRING_TEST);
- status = solver_to_test.findExtremumOverLine(a, a0, desired_robustness, com);
+ status = solver_to_test->findExtremumOverLine(a, a0, desired_robustness, com);
getProfiler().stop(PERF_STRING_TEST);
if(status!=LP_STATUS_OPTIMAL)
{
- status = solver_ground_truth.computeEquilibriumRobustness(a0, robustness);
+ status = solver_ground_truth->computeEquilibriumRobustness(a0, robustness);
if(status!=LP_STATUS_OPTIMAL)
{
error_counter++;
if(verb>1)
- SEND_ERROR_MSG(solver_ground_truth.getName()+" failed to compute equilibrium robustness of com position "+toString(a0.transpose()));
+ 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 "+
+ 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));
+ solver_ground_truth->getName()+" says this position has robustness "+toString(robustness));
}
continue;
}
getProfiler().start(PERF_STRING_GROUND_TRUTH);
- status = solver_ground_truth.computeEquilibriumRobustness(com, robustness);
+ status = solver_ground_truth->computeEquilibriumRobustness(com, robustness);
getProfiler().stop(PERF_STRING_GROUND_TRUTH);
if(status!=LP_STATUS_OPTIMAL)
{
error_counter++;
if(verb>1)
- SEND_ERROR_MSG(solver_ground_truth.getName()+" failed to compute equilibrium robustness of com posiiton "+toString(com.transpose()));
+ 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())+
+ 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));
@@ -221,7 +221,7 @@ int test_findExtremumOverLine(StaticEquilibrium &solver_to_test, StaticEquilibri
}
if(verb>0)
- cout<<"Test findExtremumOverLine "+solver_to_test.getName()+" VS "+solver_ground_truth.getName()+": "+toString(error_counter)+" error(s).\n";
+ cout<<"Test findExtremumOverLine "+solver_to_test->getName()+" VS "+solver_ground_truth->getName()+": "+toString(error_counter)+" error(s).\n";
return error_counter;
}
@@ -230,14 +230,60 @@ int test_findExtremumOverLine(StaticEquilibrium &solver_to_test, StaticEquilibri
* @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(int N_CONTACTS, int GRID_SIZE, StaticEquilibrium *solver,
+ Cref_matrixXX comPositions, Cref_matrixXX p)
{
+ MatrixXi contactPointCoord(4*N_CONTACTS,2);
+ VectorX minDistContactPoint = 1e10*VectorX::Ones(4*N_CONTACTS);
+
+ // create grid of com positions to test
+ for(unsigned int i=0; i<GRID_SIZE; i++)
+ {
+ for(unsigned int j=0; j<GRID_SIZE; 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.block<1,2>(i*GRID_SIZE+j,0)).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");
+ }
+
+ cout<<"\nRobustness grid computed with solver "<<solver->getName()<<endl;
int grid_size = (int)sqrt(comPositions.rows());
double rob ;
LP_status status;
for(unsigned int i=0; i<comPositions.rows(); i++)
{
- status = solver.computeEquilibriumRobustness(comPositions.row(i), rob);
+ 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));
@@ -257,6 +303,50 @@ void drawRobustnessGrid(StaticEquilibrium &solver, Cref_matrixXX comPositions)
}
}
+void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE, double LX, double LY,
+ RVector3 &CONTACT_POINT_LOWER_BOUNDS,
+ RVector3 &CONTACT_POINT_UPPER_BOUNDS,
+ RVector3 &RPY_LOWER_BOUNDS,
+ RVector3 &RPY_UPPER_BOUNDS,
+ MatrixXX& p, MatrixXX& N)
+{
+ MatrixXX contact_pos = MatrixXX::Zero(N_CONTACTS, 3);
+ MatrixXX contact_rpy = MatrixXX::Zero(N_CONTACTS, 3);
+ p.setZero(4*N_CONTACTS,3); // contact points
+ N.setZero(4*N_CONTACTS,3); // contact normals
+
+ // Generate contact positions and orientations
+ bool collision;
+ for(unsigned int i=0; i<N_CONTACTS; i++)
+ {
+ while(true) // generate contact position
+ {
+ uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, contact_pos.row(i));
+ if(i==0)
+ break;
+ collision = false;
+ for(unsigned int j=0; j<i-1; j++)
+ if((contact_pos.row(i)-contact_pos.row(j)).norm() < MIN_CONTACT_DISTANCE)
+ collision = true;
+ if(collision==false)
+ break;
+ }
+
+// 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));
+ }
+
+// for(int i=0; i<p.rows(); i++)
+// {
+// printf("Contact point %d position (%.3f,%.3f,%.3f) ", i, p(i,0), p(i,1), p(i,2));
+// printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
+// }
+}
+
void testWithLoadedData()
{
cout<<"*** TEST WITH LOADED DATA ***\n";
@@ -327,11 +417,12 @@ int main()
RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
/************************************** USER PARAMETERS *******************************/
+ unsigned int N_TESTS = 10;
double mass = 55.0;
double mu = 0.3; // friction coefficient
unsigned int generatorsPerContact = 4;
unsigned int N_CONTACTS = 2;
- double MIN_FEET_DISTANCE = 0.3;
+ double MIN_CONTACT_DISTANCE = 0.3;
double LX = 0.5*0.2172; // half contact surface size in x direction
double LY = 0.5*0.138; // half contact surface size in y direction
CONTACT_POINT_LOWER_BOUNDS << 0.0, 0.0, 0.0;
@@ -342,217 +433,122 @@ int main()
double X_MARG = 0.07;
double Y_MARG = 0.07;
const int GRID_SIZE = 10;
+ bool DRAW_CONTACT_POINTS = false;
/************************************ END USER PARAMETERS *****************************/
- cout<<"Number of contacts: "<<N_CONTACTS<<endl;
- cout<<"Number of generators per contact: "<<generatorsPerContact<<endl;
-
- StaticEquilibrium solver_PP("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES);
- StaticEquilibrium solver_LP_oases("LP oases", mass, generatorsPerContact, SOLVER_LP_QPOASES);
- StaticEquilibrium solver_LP2_oases("LP2 oases", mass, generatorsPerContact, SOLVER_LP_QPOASES);
- 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);
+ const int N_SOLVERS = 6;
+ string solverNames[] = {"LP oases", "LP2 oases", "DLP oases",
+ "LP coin", "LP2 coin", "DLP coin"};
+ StaticEquilibriumAlgorithm algorithms[] = {STATIC_EQUILIBRIUM_ALGORITHM_LP,
+ STATIC_EQUILIBRIUM_ALGORITHM_LP2,
+ STATIC_EQUILIBRIUM_ALGORITHM_DLP,
+ STATIC_EQUILIBRIUM_ALGORITHM_LP,
+ STATIC_EQUILIBRIUM_ALGORITHM_LP2,
+ STATIC_EQUILIBRIUM_ALGORITHM_DLP};
+ SolverLP lp_solver_types[] = {SOLVER_LP_QPOASES, SOLVER_LP_QPOASES, SOLVER_LP_QPOASES,
+ SOLVER_LP_CLP, SOLVER_LP_CLP, SOLVER_LP_CLP};
+#else
+ const int N_SOLVERS = 3;
+ string solverNames[] = {"LP oases", "LP2 oases", "DLP oases"};
+ StaticEquilibriumAlgorithm algorithms[] = {STATIC_EQUILIBRIUM_ALGORITHM_LP,
+ STATIC_EQUILIBRIUM_ALGORITHM_LP2,
+ STATIC_EQUILIBRIUM_ALGORITHM_DLP};
+ SolverLP lp_solver_types[] = {SOLVER_LP_QPOASES, SOLVER_LP_QPOASES, SOLVER_LP_QPOASES};
#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
- MatrixXX N = MatrixXX::Zero(4*N_CONTACTS,3); // contact normals
-
- // Generate contact positions and orientations
- bool collision;
- for(unsigned int i=0; i<N_CONTACTS; i++)
- {
- while(true) // generate contact position
- {
- uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, contact_pos.row(i));
- if(i==0)
- break;
- collision = false;
- for(unsigned int j=0; j<i-1; j++)
- if((contact_pos.row(i)-contact_pos.row(j)).norm() < MIN_FEET_DISTANCE)
- collision = true;
- if(collision==false)
- break;
- }
-
-// 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));
- }
+ cout<<"Number of contacts: "<<N_CONTACTS<<endl;
+ cout<<"Number of generators per contact: "<<generatorsPerContact<<endl;
+ cout<<"Gonna test equilibrium on a 2d grid of "<<GRID_SIZE<<"X"<<GRID_SIZE<<" points "<<endl;
- for(int i=0; i<p.rows(); i++)
- {
- printf("Contact point %d position (%.3f,%.3f,%.3f) ", i, p(i,0), p(i,1), p(i,2));
- printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
- }
+ StaticEquilibrium* solver_PP = new StaticEquilibrium("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES);
+ StaticEquilibrium* solvers[N_SOLVERS];
+ for(int s=0; s<N_SOLVERS; s++)
+ solvers[s] = new StaticEquilibrium(solverNames[s], mass, generatorsPerContact, lp_solver_types[s]);
- // compute upper and lower bounds of com positions to test
+ MatrixXX p, N;
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);
-
- // 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));
MatrixXX comPositions(GRID_SIZE*GRID_SIZE, 3);
- 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 n_test=0; n_test<N_TESTS; n_test++)
{
- 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);
- comPositions(i*GRID_SIZE+j, 2) = 0.0;
+ generateContacts(N_CONTACTS, MIN_CONTACT_DISTANCE, LX, LY,
+ CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
+ RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, p, N);
- // look for contact point positions on grid
- for(long k=0; k<4*N_CONTACTS; k++)
+ for(int s=0; s<N_SOLVERS; s++)
+ {
+ getProfiler().start(PERF_LP_PREPARATION);
+ if(!solvers[s]->setNewContacts(p, N, mu, algorithms[s]))
{
- double dist = (p.block<1,2>(k,0) - comPositions.block<1,2>(i*GRID_SIZE+j,0)).norm();
- if(dist < minDistContactPoint(k))
- {
- minDistContactPoint(k) = dist;
- contactPointCoord(k,0) = i;
- contactPointCoord(k,1) = j;
- }
+ SEND_ERROR_MSG("Error while setting new contacts for solver "+solvers[s]->getName());
+ return -1;
}
+ getProfiler().stop(PERF_LP_PREPARATION);
}
- }
-
- 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++)
+ getProfiler().start(PERF_PP);
+ if(!solver_PP->setNewContacts(p, N, mu, STATIC_EQUILIBRIUM_ALGORITHM_PP))
{
- 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<<"- ";
+ SEND_ERROR_MSG("Error while setting new contacts for solver "+solver_PP->getName());
+ return -1;
}
- printf("\n");
- }
+ getProfiler().stop(PERF_PP);
- getProfiler().start(PERF_LP_PREPARATION);
- if(!solver_LP_oases.setNewContacts(p, N, mu, STATIC_EQUILIBRIUM_ALGORITHM_LP))
- {
- printf("Error while setting new contacts");
- return -1;
- }
- getProfiler().stop(PERF_LP_PREPARATION);
+ // compute upper and lower bounds of com positions to test
+ 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;
- getProfiler().start(PERF_LP_PREPARATION);
- if(!solver_LP2_oases.setNewContacts(p, N, mu, STATIC_EQUILIBRIUM_ALGORITHM_LP2))
- {
- 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, mu, STATIC_EQUILIBRIUM_ALGORITHM_DLP))
- {
- printf("Error while setting new contacts");
- return -1;
- }
- getProfiler().stop(PERF_LP_PREPARATION);
-
- getProfiler().start(PERF_PP);
- bool res = solver_PP.setNewContacts(p, N, mu, STATIC_EQUILIBRIUM_ALGORITHM_PP);
- getProfiler().stop(PERF_PP);
- if(!res)
- {
- printf("Error while setting new contacts");
- return -1;
- }
-
-#ifdef CLP_FOUND
- getProfiler().start(PERF_LP_PREPARATION);
- if(!solver_LP_coin.setNewContacts(p, N, mu, STATIC_EQUILIBRIUM_ALGORITHM_LP))
- {
- 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, mu, STATIC_EQUILIBRIUM_ALGORITHM_LP2))
- {
- printf("Error while setting new contacts");
- return -1;
- }
- getProfiler().stop(PERF_LP_PREPARATION);
-
- getProfiler().start(PERF_LP_PREPARATION);
- if(!solver_DLP_coin.setNewContacts(p, N, mu, STATIC_EQUILIBRIUM_ALGORITHM_DLP))
- {
- printf("Error while setting new contacts");
- return -1;
- }
- getProfiler().stop(PERF_LP_PREPARATION);
-#endif
+ // create grid of com positions to test
+ x_range.setLinSpaced(GRID_SIZE,com_LB(0),com_UB(0));
+ y_range.setLinSpaced(GRID_SIZE,com_LB(1),com_UB(1));
+// 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);
+ comPositions(i*GRID_SIZE+j, 2) = 0.0;
+ }
+ }
- cout<<"\nRobustness grid computed with DLP oases\n";
- drawRobustnessGrid(solver_DLP_oases, comPositions);
-
- 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);
+ if(DRAW_CONTACT_POINTS)
+ drawRobustnessGrid(N_CONTACTS, GRID_SIZE, solvers[0], comPositions, p);
- test_computeEquilibriumRobustness_vs_checkEquilibrium(solver_LP_coin, solver_PP, comPositions, PERF_LP_COIN, NULL, 1);
- test_computeEquilibriumRobustness_vs_checkEquilibrium(solver_LP2_coin, solver_PP, comPositions, PERF_LP2_COIN, NULL, 1);
- test_computeEquilibriumRobustness_vs_checkEquilibrium(solver_DLP_coin, solver_PP, comPositions, PERF_DLP_COIN, NULL, 1);
-#endif
+ string test_name = "Compute equilibrium robustness ";
+ for(int s=1; s<N_SOLVERS; s++)
+ {
+ test_computeEquilibriumRobustness(solvers[0], solvers[s], comPositions, test_name+solvers[0]->getName(),
+ test_name+solvers[s]->getName(), 1);
+ }
+ for(int s=0; s<N_SOLVERS; s++)
+ {
+ test_computeEquilibriumRobustness_vs_checkEquilibrium(solvers[s], solver_PP, comPositions,
+ test_name+solvers[s]->getName(), "", 1);
+ }
- const int N_TESTS = 1000;
- Vector3 a0 = Vector3::Zero();
- a0.head<2>() = 0.5*(com_LB+com_UB);
- double e_max;
- 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())+", error code: "+toString(status));
- }
- 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);
-#ifdef CLP_FOUND
- test_findExtremumOverLine(solver_LP_coin, solver_LP_coin, a0, N_TESTS, e_max, "EXTREMUM OVER LINE LP COIN", PERF_LP_COIN, 2);
- test_findExtremumOverLine(solver_DLP_coin, solver_LP_coin, a0, N_TESTS, e_max, "EXTREMUM OVER LINE DLP COIN", PERF_LP_COIN, 2);
-#endif
+ const int N_TESTS_EXTREMUM = 100;
+ Vector3 a0 = Vector3::Zero();
+ a0.head<2>() = 0.5*(com_LB+com_UB);
+ double e_max;
+ LP_status status = solvers[0]->computeEquilibriumRobustness(a0, e_max);
+ if(status!=LP_STATUS_OPTIMAL)
+ SEND_ERROR_MSG(solvers[0]->getName()+" failed to compute robustness of com position "+toString(a0.transpose())+", error code: "+toString(status));
+ else
+ {
+ test_name = "EXTREMUM OVER LINE ";
+ string test_name2 = "Compute equilibrium robustness ";
+ for(int s=1; s<N_SOLVERS; s++)
+ {
+ if(solvers[s]->getAlgorithm()!=STATIC_EQUILIBRIUM_ALGORITHM_LP2)
+ test_findExtremumOverLine(solvers[s], solvers[0], a0, N_TESTS_EXTREMUM, e_max, test_name+solvers[s]->getName(),
+ test_name2+solvers[0]->getName(), 1);
+ }
+ }
}
getProfiler().report_all();
--
GitLab