diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index e3825a1cf0bb96a65d7c4f32bfc4734447fb85f5..10fa0357bf8daeaa265d4f578fdcbedbc9388338 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -29,6 +29,7 @@ class ROBUST_EQUILIBRIUM_DLLAPI StaticEquilibrium
private:
static bool m_is_cdd_initialized;
+ std::string m_name;
StaticEquilibriumAlgorithm m_algorithm;
Solver_LP_abstract* m_solver;
@@ -58,7 +59,9 @@ private:
bool computePolytopeProjection(Cref_matrix6X v);
public:
- StaticEquilibrium(double mass, unsigned int generatorsPerContact, SolverLP solver_type);
+ StaticEquilibrium(std::string name, double mass, unsigned int generatorsPerContact, SolverLP solver_type);
+
+ std::string getName(){ return m_name; }
bool setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
Cref_vectorX frictionCoefficients, StaticEquilibriumAlgorithm alg);
@@ -67,7 +70,25 @@ public:
bool checkRobustEquilibrium(Cref_vector2 com, double e_max=0.0);
- double findExtremumOverLine(Cref_vector2 a, double b, 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:
+ * find c, b
+ * maximize c
+ * subject to G b = D (a c + a0) + d
+ * b >= b0
+ * where
+ * b are the m coefficients of the contact force generators (f = G b)
+ * b0 is the m-dimensional vector of identical values that are proportional to e_max
+ * c is the 1d line parameter
+ * G is the 6xm matrix whose columns are the gravito-inertial wrench generators
+ * D is the 6x2 matrix mapping the CoM position in gravito-inertial wrench
+ * d is the 6d vector containing the gravity part of the gravito-inertial wrench
+ * @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.
+ */
+ bool findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com);
double findExtremumInDirection(Cref_vector2 direction, double e_max=0.0);
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index de7fe96a3d2f894b74feb90a7dd5675a113b69e6..49509a089eb41c7b22041cc31e52dec827a5d156 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -16,7 +16,7 @@ namespace robust_equilibrium
bool StaticEquilibrium::m_is_cdd_initialized = false;
-StaticEquilibrium::StaticEquilibrium(double mass, unsigned int generatorsPerContact, SolverLP solver_type)
+StaticEquilibrium::StaticEquilibrium(string name, double mass, unsigned int generatorsPerContact, SolverLP solver_type)
{
if(!m_is_cdd_initialized)
{
@@ -30,6 +30,7 @@ StaticEquilibrium::StaticEquilibrium(double mass, unsigned int generatorsPerCont
generatorsPerContact = 4;
}
+ m_name = name;
m_solver = Solver_LP_abstract::getNewSolver(solver_type);
m_generatorsPerContact = generatorsPerContact;
@@ -237,7 +238,7 @@ double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
return -1.0;
}
- SEND_ERROR_MSG("checkRobustEquilibrium is only implemented for the LP algorithm");
+ SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
return -1.0;
}
@@ -261,10 +262,84 @@ bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, double e_max)
return true;
}
-double StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, double b, double e_max)
+bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 com)
{
- SEND_ERROR_MSG("findExtremumOverLine not implemented yet");
- return 0.0;
+ const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+
+ if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_LP)
+ {
+ /* Compute the extremum CoM position over the line a*p + a0 that is in robust equilibrium
+ * by solving the following LP:
+ find b, p
+ minimize -p
+ subject to D (a p + a0) + d <= G (b + b0) <= D (a p + a0) + d
+ 0 <= b <= Inf
+ where
+ b0+b are the coefficient of the contact force generators (f = G (b0+b))
+ b0 is the robustness measure
+ p is the line parameter
+ G is the matrix whose columns are the gravito-inertial wrench generators
+ */
+ VectorX b_p(m+1);
+ VectorX c = VectorX::Zero(m+1);
+ c(m) = -1.0;
+ VectorX lb = -VectorX::Zero(m+1);
+ lb(m) = -1e5;
+ VectorX ub = VectorX::Ones(m+1)*1e10;
+ Vector6 Alb = m_D*a0 + m_d - m_G_centr*VectorX::Ones(m)*e_max;
+ Vector6 Aub = Alb;
+ Matrix6X A = Matrix6X::Zero(6, m+1);
+ A.leftCols(m) = m_G_centr;
+ A.rightCols(1) = -m_D*a;
+
+ LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_p);
+ if(lpStatus_primal==LP_STATUS_OPTIMAL)
+ {
+ com = a0 + a*b_p(m);
+ return true;
+ }
+
+ SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
+ return false;
+ }
+
+ if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_DLP)
+ {
+ /* Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium
+ * by solving the following dual LP:
+ find v
+ minimize (D a0 + d -G b0)' v
+ subject to 0 <= G' v <= Inf
+ -1 <= a' D' v <= -1
+ where
+ G is the matrix whose columns are the gravito-inertial wrench generators
+ */
+ 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;
+ VectorX Alb = VectorX::Zero(m+1);
+ Alb(m) = -1.0;
+ VectorX Aub = VectorX::Ones(m+1)*1e100;
+ Aub(m) = -1.0;
+ MatrixX6 A(m+1,6);
+ A.topRows(m) = m_G_centr.transpose();
+ A.bottomRows<1>() = (m_D*a).transpose();
+
+ LP_status lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
+ if(lpStatus_dual==LP_STATUS_OPTIMAL)
+ {
+ double p = m_solver->getObjectiveValue();
+ com = a0 + a*p;
+ return true;
+ }
+
+ SEND_ERROR_MSG("Dual LP problem could not be solved: "+toString(lpStatus_dual));
+ return false;
+ }
+
+ SEND_ERROR_MSG("checkRobustEquilibrium is not implemented for the specified algorithm");
+ return false;
}
double StaticEquilibrium::findExtremumInDirection(Cref_vector2 direction, double e_max)
diff --git a/src/stop-watch.cpp b/src/stop-watch.cpp
index e2c8d4598ce26d3c05faa7a9366cf08b3a6e5ea7..de3ef01cd9e11ff08f2816b37a7ff6204c04d525 100644
--- a/src/stop-watch.cpp
+++ b/src/stop-watch.cpp
@@ -243,7 +243,7 @@ void Stopwatch::report(string perf_name, int precision, std::ostream& output)
PerformanceData& perf_info = records_of->find(perf_name)->second;
- const int MAX_NAME_LENGTH = 40;
+ const int MAX_NAME_LENGTH = 60;
string pad = "";
for (int i = perf_name.length(); i<MAX_NAME_LENGTH; i++)
pad.append(" ");
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index c36cd787f39a2c52f3e9834d7972b84797b6e6f1..1beeaa22bd0d6199396d0f4b3a0cd0b67f8ab21b 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -13,47 +13,147 @@ using namespace robust_equilibrium;
using namespace Eigen;
using namespace std;
-#define PERF_PP "PP"
-#define PERF_LP_PREPARATION "LP preparation"
-#define PERF_LP_COIN "LP coin"
-#define PERF_LP_OASES "LP oases"
-#define PERF_LP2_COIN "LP2 coin"
-#define PERF_LP2_OASES "LP2 oases"
-#define PERF_DLP_COIN "DLP coin"
-#define PERF_DLP_OASES "DLP oases"
+#define PERF_PP "Polytope Projection"
+#define PERF_LP_PREPARATION "Computation of GIWC generators"
+#define PERF_LP_COIN "Compute Equilibrium Robustness with LP coin"
+#define PERF_LP_OASES "Compute Equilibrium Robustness with LP oases"
+#define PERF_LP2_COIN "Compute Equilibrium Robustness with LP2 coin"
+#define PERF_LP2_OASES "Compute Equilibrium Robustness with LP2 oases"
+#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
+
+/** Test two different solvers on the method StaticEquilibrium::computeEquilibriumRobustness.
+ */
+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;
+ for(unsigned int i=0; i<comPositions.rows(); i++)
+ {
+ getProfiler().start(PERF_STRING_1);
+ double rob_1 = solver_1.computeEquilibriumRobustness(comPositions.row(i));
+ getProfiler().stop(PERF_STRING_1);
+
+ getProfiler().start(PERF_STRING_2);
+ double rob_2 = solver_2.computeEquilibriumRobustness(comPositions.row(i));
+ getProfiler().stop(PERF_STRING_2);
+
+ if(fabs(rob_1-rob_2)>EPS)
+ {
+ if(verb>0)
+ 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).");
+ return error_counter;
+}
+
+/** Test method StaticEquilibrium::findExtremumOverLine. The test works in this way: first it
+ * 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.
+ */
+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;
+ for(unsigned int i=0; i<N_TESTS; i++)
+ {
+ uniform(-1.0*Vector2::Ones(), Vector2::Ones(), a);
+ desired_robustness = (rand()/ value_type(RAND_MAX))*e_max;
+
+ getProfiler().start(PERF_STRING_TEST);
+ status = solver_to_test.findExtremumOverLine(a, a0, desired_robustness, com);
+ getProfiler().stop(PERF_STRING_TEST);
+
+ if(status==false)
+ {
+ error_counter++;
+ if(verb>0)
+ SEND_ERROR_MSG(solver_to_test.getName()+" failed to find extremum over line");
+ continue;
+ }
+
+ getProfiler().start(PERF_STRING_GROUND_TRUTH);
+ double robustness = solver_ground_truth.computeEquilibriumRobustness(com);
+ getProfiler().stop(PERF_STRING_GROUND_TRUTH);
+
+ if(fabs(robustness-desired_robustness)>EPS)
+ {
+ if(verb>0)
+ SEND_ERROR_MSG(solver_to_test.getName()+" found this com position: "+toString(com)+
+ " 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).");
+ return error_counter;
+}
+
+/** Draw a grid on the screen using the robustness computed with the method
+ * StaticEquilibrium::computeEquilibriumRobustness.
+ */
+void drawRobustnessGrid(StaticEquilibrium solver, Cref_matrixXX comPositions)
+{
+ int grid_size = (int)sqrt(comPositions.rows());
+ for(unsigned int i=0; i<comPositions.rows(); i++)
+ {
+ double rob = solver.computeEquilibriumRobustness(comPositions.row(i));
+ if(rob>=0.0)
+ {
+ if(rob>9.0)
+ rob = 9.0;
+ printf("%d ", (int)rob);
+ }
+ else
+ printf("- ");
+ if((i+1)%grid_size==0)
+ printf("\n");
+ }
+}
int main()
{
-// init_cdd_library();
srand ((unsigned int)(time(NULL)));
+ RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
+ RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
- int ret = 0;
+ /************************************** USER PARAMETERS *******************************/
double mass = 70.0;
double mu = 0.3; // friction coefficient
unsigned int generatorsPerContact = 4;
unsigned int N_CONTACTS = 2;
- unsigned int N_POINTS = 50; // number of com positions to test
double MIN_FEET_DISTANCE = 0.3;
double LX = 0.5*0.2172; // half foot size in x direction
double LY = 0.5*0.138; // half foot size in y direction
- RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
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
- RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
RPY_LOWER_BOUNDS << -0*gamma, -0*gamma, -M_PI;
RPY_UPPER_BOUNDS << +0*gamma, +0*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_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);
- StaticEquilibrium solver_PP(mass, generatorsPerContact, SOLVER_LP_CLP);
- StaticEquilibrium solver_LP_coin(mass, generatorsPerContact, SOLVER_LP_CLP);
- StaticEquilibrium solver_LP_oases(mass, generatorsPerContact, SOLVER_LP_QPOASES);
- StaticEquilibrium solver_LP2_coin(mass, generatorsPerContact, SOLVER_LP_CLP);
- StaticEquilibrium solver_LP2_oases(mass, generatorsPerContact, SOLVER_LP_QPOASES);
- StaticEquilibrium solver_DLP_coin(mass, generatorsPerContact, SOLVER_LP_CLP);
- StaticEquilibrium solver_DLP_oases(mass, generatorsPerContact, SOLVER_LP_QPOASES);
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
@@ -85,10 +185,8 @@ int main()
// 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));
}
@@ -99,6 +197,64 @@ int main()
printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
}
+ 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);
+
+ 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, 2);
+ 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 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);
+
+ // 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.row(i*GRID_SIZE+j)).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");
+ }
+
getProfiler().start(PERF_LP_PREPARATION);
if(!solver_LP_coin.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP))
{
@@ -151,147 +307,21 @@ int main()
return -1;
}
- 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);
-
- 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 equilibrium(GRID_SIZE, GRID_SIZE);
- Vector2 com;
- 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++)
- {
- com(1) = y_range(GRID_SIZE-1-i);
- for(unsigned int j=0; j<GRID_SIZE; j++)
- {
-// uniform(com_LB, com_UB, com);
- com(0) = x_range(j);
-
- getProfiler().start(PERF_LP_COIN);
- double rob_coin = solver_LP_coin.computeEquilibriumRobustness(com);
- getProfiler().stop(PERF_LP_COIN);
-
- getProfiler().start(PERF_LP_OASES);
- double rob_oases = solver_LP_oases.computeEquilibriumRobustness(com);
- getProfiler().stop(PERF_LP_OASES);
-
- getProfiler().start(PERF_LP2_COIN);
- double rob_coin2 = solver_LP2_coin.computeEquilibriumRobustness(com);
- getProfiler().stop(PERF_LP2_COIN);
-
- getProfiler().start(PERF_LP2_OASES);
- double rob_oases2 = solver_LP2_oases.computeEquilibriumRobustness(com);
- getProfiler().stop(PERF_LP2_OASES);
-
- getProfiler().start(PERF_DLP_COIN);
- double rob_DLP_coin = solver_DLP_coin.computeEquilibriumRobustness(com);
- getProfiler().stop(PERF_DLP_COIN);
-
- getProfiler().start(PERF_DLP_OASES);
- double rob_DLP_oases = solver_DLP_oases.computeEquilibriumRobustness(com);
- getProfiler().stop(PERF_DLP_OASES);
-
- if(fabs(rob_coin-rob_oases)>1e-5)
- SEND_ERROR_MSG("LP_coin and LP_oases returned different results: "+toString(rob_coin)+" VS "+toString(rob_oases));
-
- if(fabs(rob_coin-rob_oases2)>1e-5)
- SEND_ERROR_MSG("LP_coin and LP2_oases returned different results: "+toString(rob_coin)+" VS "+toString(rob_oases2));
-
-// if(fabs(rob_coin-rob_coin2)>1e-5)
-// SEND_ERROR_MSG("LP_coin and LP2_coin returned different results: "+toString(rob_coin)+" VS "+toString(rob_coin2));
-
- if(fabs(rob_coin-rob_DLP_oases)>1e-5)
- SEND_ERROR_MSG("LP_coin and DLP_oases returned different results: "+toString(rob_coin)+" VS "+toString(rob_DLP_oases));
+ drawRobustnessGrid(solver_DLP_oases, comPositions);
- if(fabs(rob_coin-rob_DLP_coin)>1e-5)
- SEND_ERROR_MSG("LP_coin and DLP_coin returned different results: "+toString(rob_coin)+" VS "+toString(rob_DLP_coin));
+ 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);
- if(solver_PP.checkRobustEquilibrium(com, 0.0))
- {
- equilibrium(i,j) = 1.0;
- if(rob_coin<=0)
- SEND_ERROR_MSG("PP says com is in equilibrium but LP_coin find negative robustness "+toString(rob_coin));
- if(rob_oases<=0)
- SEND_ERROR_MSG("PP says com is in equilibrium but LP_oases find negative robustness "+toString(rob_oases));
- if(rob_coin2<=0)
- SEND_ERROR_MSG("PP says com is in equilibrium but LP_coin2 find negative robustness "+toString(rob_coin2));
- if(rob_oases2<=0)
- SEND_ERROR_MSG("PP says com is in equilibrium but LP_oases2 find negative robustness "+toString(rob_oases2));
- if(rob_DLP_oases<=0)
- SEND_ERROR_MSG("PP says com is in equilibrium but DLP_oases find negative robustness "+toString(rob_DLP_oases));
- if(rob_DLP_coin<=0)
- SEND_ERROR_MSG("PP says com is in equilibrium but DLP_coin negative robustness "+toString(rob_DLP_coin));
-
- if(rob_coin>9.0)
- rob_coin = 9.0;
- printf("%d ", (int)rob_coin);
- }
- else
- {
- equilibrium(i,j) = 0.0;
- if(rob_coin>0)
- SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_coin find positive robustness "+toString(rob_coin));
- if(rob_oases>0)
- SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_oases find positive robustness "+toString(rob_oases));
- if(rob_coin2>0)
- SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_coin2 find positive robustness "+toString(rob_coin2));
- if(rob_oases2>0)
- SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP_oases2 find positive robustness "+toString(rob_oases2));
- if(rob_DLP_coin>0)
- SEND_ERROR_MSG("PP says com is NOT in equilibrium but DLP_coin find positive robustness "+toString(rob_DLP_coin));
- if(rob_DLP_oases>0)
- SEND_ERROR_MSG("PP says com is NOT in equilibrium but DLP_oases find positive robustness "+toString(rob_DLP_oases));
- printf("- ");
- }
-
- // look for contact point positions on grid
- for(long k=0; k<4*N_CONTACTS; k++)
- {
- double dist = (p.block<1,2>(k,0) - com.transpose()).norm();
- if(dist < minDistContactPoint(k))
- {
- minDistContactPoint(k) = dist;
- contactPointCoord(k,0) = i;
- contactPointCoord(k,1) = j;
- }
- }
- }
- printf("\n");
- }
-
-// cout<<"Max dist between contact points and grid points "<<minDistContactPoint.maxCoeff()<<"\n";
-
- cout<<"\nContact point position on the same grid\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");
- }
+ 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);
getProfiler().report_all();
- return ret;
+ return 0;
}