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

Add algorithms (LP and dual LP) to compute extremum equilibrium point over a...

Add algorithms (LP and dual LP) to compute extremum equilibrium point over a line. Add relative tests (successful). Clean up old tests for computation of equilibrium robustness.
parent a34a831f
No related branches found
No related tags found
No related merge requests found
......@@ -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);
......
......@@ -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)
......
......@@ -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(" ");
......
......@@ -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;
}
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