diff --git a/include/robust-equilibrium-lib/util.hh b/include/robust-equilibrium-lib/util.hh
index 759fff97a3b0d58be840d9c468075b7f72124917..e698f4e5728c725a9cd5729585435c6c903b0b50 100644
--- a/include/robust-equilibrium-lib/util.hh
+++ b/include/robust-equilibrium-lib/util.hh
@@ -1,3 +1,7 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
#ifndef _ROBUST_EQUILIBRIUM_LIB_UTIL_HH
#define _ROBUST_EQUILIBRIUM_LIB_UTIL_HH
@@ -41,8 +45,6 @@ namespace robust_equilibrium
typedef Eigen::Matrix <value_type, Eigen::Dynamic, 6, Eigen::RowMajor> MatrixX6;
typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXX;
- //define Eigen ref if available
-#if EIGEN_VERSION_AT_LEAST(3,2,2)
typedef Eigen::Ref<Vector2> Ref_vector2;
typedef Eigen::Ref<Vector3> Ref_vector3;
typedef Eigen::Ref<VectorX> Ref_vectorX;
@@ -60,7 +62,6 @@ namespace robust_equilibrium
typedef const Eigen::Ref<const Matrix43> & Cref_matrix43;
typedef const Eigen::Ref<const Matrix6X> & Cref_matrix6X;
typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
-#endif
/**
* Write the specified matrix to a binary file with the specified name.
diff --git a/src/solver_LP_qpoases.cpp b/src/solver_LP_qpoases.cpp
index 0d45acc70aae4433ab66a7057332228573521838..2b9f5dffe59e153fa6d9a3709294f964a8f73fb3 100644
--- a/src/solver_LP_qpoases.cpp
+++ b/src/solver_LP_qpoases.cpp
@@ -46,9 +46,6 @@ namespace robust_equilibrium
m_H = MatrixXX::Zero(n,n);
}
-// m_status = m_solver.init(NULL, c.data(), A.data(), lb.data(), ub.data(),
-// Alb.data(), Aub.data(), iters, &solutionTime);
-
if(!m_init_succeeded)
{
m_status = m_solver.init(NULL, c.data(), A.data(), lb.data(), ub.data(),
@@ -69,8 +66,6 @@ namespace robust_equilibrium
{
m_solver.getPrimalSolution(sol.data());
}
- else
- SEND_DEBUG_MSG("QPoases solver failed with code "+toString(m_status));
return getStatus();
}
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index e85609369d2718f72cf6fd102d83c65394c9e5a3..2265edbb2e73ade3527792aae71a12d5b9906ef1 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -305,7 +305,7 @@ bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, do
{
com = a0 + a*b_p(m);
-#define WRITE_LPS_TO_FILE
+//#define WRITE_LPS_TO_FILE
#ifdef WRITE_LPS_TO_FILE
string date_time = getDateAndTimeAsString();
string filename = "LP_findExtremumOverLine"+date_time+".dat";
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
index 27faf5d010531d5f132c32fa31163ed6411fd898..72e35c861261845501a15ad8dddfb7f15f02679f 100644
--- a/test/test_LP_solvers.cpp
+++ b/test/test_LP_solvers.cpp
@@ -373,10 +373,10 @@ void test_small_LP()
int main()
{
- cout <<"Test LP Solvers\n\n";
+ cout <<"Test LP Solvers (1 means ok, 0 means error)\n\n";
{
- cout<<"TEST QP OASES ON A SMALL 2-VARIABLE LP\n";
+ cout<<"TEST QP OASES ON A SMALL 2-VARIABLE LP";
/* Setup data of first LP. */
real_t A[1*2] = { 1.0, 1.0 };
real_t g[2] = { 1.5, 1.0 };
@@ -394,7 +394,11 @@ int main()
/* Solve first LP. */
int nWSR = 10;
- example.init( 0,g,A,lb,ub,lbA,ubA, nWSR,0 );
+ int res = example.init( 0,g,A,lb,ub,lbA,ubA, nWSR,0 );
+ if(res==0)
+ cout<<"[INFO] LP solved correctly\n";
+ else
+ cout<<"[ERROR] QpOases could not solve the LP problem, error code: "<<res<<endl;
}
{
@@ -436,11 +440,21 @@ int main()
cout<<"\nTEST QP OASES ON SOME LP PROBLEMS\n";
string file_path = "../test_data/";
Solver_LP_abstract *solverOases = Solver_LP_abstract::getNewSolver(SOLVER_LP_QPOASES);
- const int PROBLEM_NUMBER = 4;
+ const int PROBLEM_NUMBER = 14;
string problem_filenames[PROBLEM_NUMBER] = {"DLP_findExtremumOverLine20151103_112611",
"DLP_findExtremumOverLine20151103_115627",
+ "DLP_findExtremumOverLine20151103_014022",
+ "DLP_findExtremumOverLine_32_generators",
+ "DLP_findExtremumOverLine_64_generators",
+ "DLP_findExtremumOverLine_128_generators",
+ "DLP_findExtremumOverLine_128_generators_bis",
"LP_findExtremumOverLine20151103_112610",
- "LP_findExtremumOverLine20151103_112611"};
+ "LP_findExtremumOverLine20151103_112611",
+ "LP_findExtremumOverLine20151103_014022",
+ "LP_findExtremumOverLine_32_generators",
+ "LP_findExtremumOverLine_64_generators",
+ "LP_findExtremumOverLine_128_generators",
+ "LP_findExtremumOverLine_128_generators_bis"};
VectorX c, lb, ub, Alb, Aub, realSol, sol;
MatrixXX A;
for(int i=0; i<PROBLEM_NUMBER; i++)
@@ -461,15 +475,15 @@ int main()
solverOases->solve(c, lb, ub, A, Alb, Aub, sol);
if(sol.isApprox(realSol, EPS))
{
- cout<<"[INFO] Solution of problem "<<problem_filename<<" is equal to the expected value!\n";
+ cout<<"[INFO] Solution of problem "<<problem_filename<<" ("<<c.size()<<" var, "<<A.rows()<<" constr) is equal to the expected value!\n";
}
else
{
if(fabs(c.dot(sol)-c.dot(realSol))<EPS)
- cout<<"[WARNING] Solution of problem "<<problem_filename<<" is different from expected but it has the same cost\n";
+ cout<<"[WARNING] Solution of problem "<<problem_filename<<" ("<<c.size()<<" var, "<<A.rows()<<" constr) is different from expected but it has the same cost\n";
else
{
- cout<<"[ERROR] Solution of problem "<<problem_filename<<" is different from the expected value:\n";
+ cout<<"[ERROR] Solution of problem "<<problem_filename<<" ("<<c.size()<<" var, "<<A.rows()<<" constr) is different from the expected value:\n";
cout<<"\tSolution found "<<sol.transpose()<<endl;
cout<<"\tExpected solution "<<realSol.transpose()<<endl;
cout<<"\tCost found "<<(c.dot(sol))<<endl;
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 17b9368127ab8689fd1dd5e98b2f01f4060a1e51..4cbe5af53981ccb97fa25720294dfdb893adbfdf 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -169,7 +169,10 @@ int test_findExtremumOverLine(StaticEquilibrium &solver_to_test, StaticEquilibri
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;
+ if(e_max>=0.0)
+ desired_robustness = (rand()/ value_type(RAND_MAX))*e_max;
+ else
+ desired_robustness = e_max - EPS;
getProfiler().start(PERF_STRING_TEST);
status = solver_to_test.findExtremumOverLine(a, a0, desired_robustness, com);
@@ -250,9 +253,9 @@ void drawRobustnessGrid(StaticEquilibrium &solver, Cref_matrixXX comPositions)
int main()
{
unsigned int seed = (unsigned int)(time(NULL));
-// seed = 1446456544;
+// seed = 1446555515;
srand (seed);
- cout<<"Initialize random number generator with seed "<<seed<<endl;
+ cout<<"Initialize random number generator with seed "<<seed<<" (in case you wanna repeat the same test later)\n";
RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
@@ -260,19 +263,19 @@ int main()
/************************************** USER PARAMETERS *******************************/
double mass = 70.0;
double mu = 0.3; // friction coefficient
- unsigned int generatorsPerContact = 4;
- unsigned int N_CONTACTS = 1;
+ unsigned int generatorsPerContact = 8;
+ unsigned int N_CONTACTS = 3;
double MIN_FEET_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;
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 << -2*gamma, -2*gamma, -M_PI;
+ RPY_UPPER_BOUNDS << +2*gamma, +2*gamma, +M_PI;
double X_MARG = 0.07;
double Y_MARG = 0.07;
- const int GRID_SIZE = 5;
+ const int GRID_SIZE = 10;
/************************************ END USER PARAMETERS *****************************/
cout<<"Number of contacts: "<<N_CONTACTS<<endl;
diff --git a/test_data/DLP_findExtremumOverLine20151103_014022.dat b/test_data/DLP_findExtremumOverLine20151103_014022.dat
new file mode 100644
index 0000000000000000000000000000000000000000..75f27bdc49795170aa72711f8c67e63df236a024
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine20151103_014022.dat differ
diff --git a/test_data/DLP_findExtremumOverLine20151103_014022_solution.dat b/test_data/DLP_findExtremumOverLine20151103_014022_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..f9d54a8f500135ba29ca288eacf0ac727cbfec75
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine20151103_014022_solution.dat differ
diff --git a/test_data/DLP_findExtremumOverLine_128_generators.dat b/test_data/DLP_findExtremumOverLine_128_generators.dat
new file mode 100644
index 0000000000000000000000000000000000000000..460b9bd1c24829b2644ba1cca2823baf1ce9f5ed
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine_128_generators.dat differ
diff --git a/test_data/DLP_findExtremumOverLine_128_generators_bis.dat b/test_data/DLP_findExtremumOverLine_128_generators_bis.dat
new file mode 100644
index 0000000000000000000000000000000000000000..a0046981976f9a5acad30532e7ab983ff8bacc3c
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine_128_generators_bis.dat differ
diff --git a/test_data/DLP_findExtremumOverLine_128_generators_bis_solution.dat b/test_data/DLP_findExtremumOverLine_128_generators_bis_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..26da3e6257fec7f42aec72ced50c8ff955215025
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine_128_generators_bis_solution.dat differ
diff --git a/test_data/DLP_findExtremumOverLine_128_generators_solution.dat b/test_data/DLP_findExtremumOverLine_128_generators_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..c27d8c920892781134fc69f33968bc6dabc703dd
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine_128_generators_solution.dat differ
diff --git a/test_data/DLP_findExtremumOverLine_32_generators.dat b/test_data/DLP_findExtremumOverLine_32_generators.dat
new file mode 100644
index 0000000000000000000000000000000000000000..fe78d5364c3107f00a1d79514f97d9814f0114d9
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine_32_generators.dat differ
diff --git a/test_data/DLP_findExtremumOverLine_32_generators_solution.dat b/test_data/DLP_findExtremumOverLine_32_generators_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..dc6496ee5f8a1e6f33f8e4689691d38c8fdb3dbb
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine_32_generators_solution.dat differ
diff --git a/test_data/DLP_findExtremumOverLine_64_generators.dat b/test_data/DLP_findExtremumOverLine_64_generators.dat
new file mode 100644
index 0000000000000000000000000000000000000000..d6c10e9dc9a604c6f6a75ab2c0884a09460bcbee
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine_64_generators.dat differ
diff --git a/test_data/DLP_findExtremumOverLine_64_generators_solution.dat b/test_data/DLP_findExtremumOverLine_64_generators_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..2ba448b10da4e14e24fb21f0891b0278f1dd0bfd
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine_64_generators_solution.dat differ
diff --git a/test_data/LP_findExtremumOverLine20151103_014022.dat b/test_data/LP_findExtremumOverLine20151103_014022.dat
new file mode 100644
index 0000000000000000000000000000000000000000..5537dc9ae58b3cc6839f565981c7d8843fd15290
Binary files /dev/null and b/test_data/LP_findExtremumOverLine20151103_014022.dat differ
diff --git a/test_data/LP_findExtremumOverLine20151103_014022_solution.dat b/test_data/LP_findExtremumOverLine20151103_014022_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..507353ecc6c1a08da7fc6ba1165bc8b7cc28ec3d
Binary files /dev/null and b/test_data/LP_findExtremumOverLine20151103_014022_solution.dat differ
diff --git a/test_data/LP_findExtremumOverLine_128_generators.dat b/test_data/LP_findExtremumOverLine_128_generators.dat
new file mode 100644
index 0000000000000000000000000000000000000000..4c607f835fbde1196658d9147800c91ac816effb
Binary files /dev/null and b/test_data/LP_findExtremumOverLine_128_generators.dat differ
diff --git a/test_data/LP_findExtremumOverLine_128_generators_bis.dat b/test_data/LP_findExtremumOverLine_128_generators_bis.dat
new file mode 100644
index 0000000000000000000000000000000000000000..75d466768cb47c75610c0d9bc1ada8ac48b13f60
Binary files /dev/null and b/test_data/LP_findExtremumOverLine_128_generators_bis.dat differ
diff --git a/test_data/LP_findExtremumOverLine_128_generators_bis_solution.dat b/test_data/LP_findExtremumOverLine_128_generators_bis_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..01827b4dde575ab9bcb04d32be12c88b0457f461
Binary files /dev/null and b/test_data/LP_findExtremumOverLine_128_generators_bis_solution.dat differ
diff --git a/test_data/LP_findExtremumOverLine_128_generators_solution.dat b/test_data/LP_findExtremumOverLine_128_generators_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..e867ea23f429bf51bdac6cbf77f43e5cd0083143
Binary files /dev/null and b/test_data/LP_findExtremumOverLine_128_generators_solution.dat differ
diff --git a/test_data/LP_findExtremumOverLine_32_generators.dat b/test_data/LP_findExtremumOverLine_32_generators.dat
new file mode 100644
index 0000000000000000000000000000000000000000..6dd7e50096bb9aeb13087632fc024001ebc074b9
Binary files /dev/null and b/test_data/LP_findExtremumOverLine_32_generators.dat differ
diff --git a/test_data/LP_findExtremumOverLine_32_generators_solution.dat b/test_data/LP_findExtremumOverLine_32_generators_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..af80a74f2c28bd03e25b36e622bbf04c9769821d
Binary files /dev/null and b/test_data/LP_findExtremumOverLine_32_generators_solution.dat differ
diff --git a/test_data/LP_findExtremumOverLine_64_generators.dat b/test_data/LP_findExtremumOverLine_64_generators.dat
new file mode 100644
index 0000000000000000000000000000000000000000..50540586174370a55005298a429f2bc55c224b89
Binary files /dev/null and b/test_data/LP_findExtremumOverLine_64_generators.dat differ
diff --git a/test_data/LP_findExtremumOverLine_64_generators_solution.dat b/test_data/LP_findExtremumOverLine_64_generators_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..e398a1924eafb933eb701cfceb049430b7efb672
Binary files /dev/null and b/test_data/LP_findExtremumOverLine_64_generators_solution.dat differ