diff --git a/include/robust-equilibrium-lib/solver_LP_abstract.hh b/include/robust-equilibrium-lib/solver_LP_abstract.hh
index 148fdc5e1e9383a937e20bca751dc43636546d7b..a8555e772eb657b0091d8f4a9f70e4dfe4463da1 100644
--- a/include/robust-equilibrium-lib/solver_LP_abstract.hh
+++ b/include/robust-equilibrium-lib/solver_LP_abstract.hh
@@ -54,7 +54,7 @@ public:
* @param sol Output solution of the LP.
* @return A flag describing the final status of the solver.
*/
- virtual LP_status solve(const char* filename, Ref_vectorX sol);
+ virtual LP_status solve(const std::string& filename, Ref_vectorX sol);
/**
* @brief Write the specified Linear Program to binary file.
@@ -70,7 +70,7 @@ public:
* @param Aub
* @return True if the operation succeeded, false otherwise.
*/
- virtual bool writeLpToFile(const char* filename,
+ virtual bool writeLpToFile(const std::string& filename,
Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub);
@@ -89,7 +89,7 @@ public:
* @param Aub
* @return True if the operation succeeded, false otherwise.
*/
- virtual bool readLpFromFile(const char* filename,
+ virtual bool readLpFromFile(const std::string& filename,
VectorX &c, VectorX &lb, VectorX &ub,
MatrixXX &A, VectorX &Alb, VectorX &Aub);
diff --git a/include/robust-equilibrium-lib/util.hh b/include/robust-equilibrium-lib/util.hh
index 5c22c80754d2f1b617747ae9bca38bcad22da27c..759fff97a3b0d58be840d9c468075b7f72124917 100644
--- a/include/robust-equilibrium-lib/util.hh
+++ b/include/robust-equilibrium-lib/util.hh
@@ -66,29 +66,31 @@ namespace robust_equilibrium
* Write the specified matrix to a binary file with the specified name.
*/
template<class Matrix>
- void writeMatrixToFile(const char* filename, const Matrix& matrix)
+ bool writeMatrixToFile(const std::string &filename, const Matrix& matrix)
{
- std::ofstream out(filename, std::ios::out | std::ios::binary | std::ios::trunc);
+ std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
typename Matrix::Index rows=matrix.rows(), cols=matrix.cols();
out.write((char*) (&rows), sizeof(typename Matrix::Index));
out.write((char*) (&cols), sizeof(typename Matrix::Index));
out.write((char*) matrix.data(), rows*cols*sizeof(typename Matrix::Scalar) );
out.close();
+ return true;
}
/**
* Read a matrix from the specified input binary file.
*/
template<class Matrix>
- void readMatrixFromFile(const char* filename, Matrix& matrix)
+ bool readMatrixFromFile(const std::string &filename, Matrix& matrix)
{
- std::ifstream in(filename, std::ios::in | std::ios::binary);
+ std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
typename Matrix::Index rows=0, cols=0;
in.read((char*) (&rows),sizeof(typename Matrix::Index));
in.read((char*) (&cols),sizeof(typename Matrix::Index));
matrix.resize(rows, cols);
in.read( (char *) matrix.data() , rows*cols*sizeof(typename Matrix::Scalar) );
in.close();
+ return true;
}
/**
@@ -116,6 +118,8 @@ namespace robust_equilibrium
bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy,
Ref_matrix43 p, Ref_matrix43 N);
+ std::string getDateAndTimeAsString();
+
} //namespace robust_equilibrium
#endif //_ROBUST_EQUILIBRIUM_LIB_UTIL_HH
diff --git a/src/solver_LP_abstract.cpp b/src/solver_LP_abstract.cpp
index 6bfbadd5a038bc972af112f716f4584ac5684732..2906c9ba7164101a64de2c9c9f2a53838fbe6683 100644
--- a/src/solver_LP_abstract.cpp
+++ b/src/solver_LP_abstract.cpp
@@ -32,7 +32,7 @@ Solver_LP_abstract* Solver_LP_abstract::getNewSolver(SolverLP solverType)
return NULL;
}
-bool Solver_LP_abstract::writeLpToFile(const char* filename,
+bool Solver_LP_abstract::writeLpToFile(const std::string& filename,
Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub)
{
@@ -43,7 +43,7 @@ bool Solver_LP_abstract::writeLpToFile(const char* filename,
assert(Alb.size()==m);
assert(Aub.size()==m);
- std::ofstream out(filename, std::ios::out | std::ios::binary | std::ios::trunc);
+ std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
out.write((char*) (&n), sizeof(typename MatrixXX::Index));
out.write((char*) (&m), sizeof(typename MatrixXX::Index));
out.write((char*) c.data(), n*sizeof(typename MatrixXX::Scalar) );
@@ -56,11 +56,11 @@ bool Solver_LP_abstract::writeLpToFile(const char* filename,
return true;
}
-bool Solver_LP_abstract::readLpFromFile(const char* filename,
+bool Solver_LP_abstract::readLpFromFile(const std::string& filename,
VectorX &c, VectorX &lb, VectorX &ub,
MatrixXX &A, VectorX &Alb, VectorX &Aub)
{
- std::ifstream in(filename, std::ios::in | std::ios::binary);
+ std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
typename MatrixXX::Index n=0, m=0;
in.read((char*) (&n),sizeof(typename MatrixXX::Index));
in.read((char*) (&m),sizeof(typename MatrixXX::Index));
@@ -80,7 +80,7 @@ bool Solver_LP_abstract::readLpFromFile(const char* filename,
return true;
}
-LP_status Solver_LP_abstract::solve(const char* filename, Ref_vectorX sol)
+LP_status Solver_LP_abstract::solve(const std::string& filename, Ref_vectorX sol)
{
VectorX c, lb, ub, Alb, Aub;
MatrixXX A;
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index 27116eb8fbe1ceaa3945f5bdf006142fec33ff08..e85609369d2718f72cf6fd102d83c65394c9e5a3 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -8,6 +8,7 @@
#include <robust-equilibrium-lib/stop-watch.hh>
#include <iostream>
#include <vector>
+#include <ctime>
using namespace std;
@@ -303,6 +304,18 @@ bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, do
if(lpStatus_primal==LP_STATUS_OPTIMAL)
{
com = a0 + a*b_p(m);
+
+#define WRITE_LPS_TO_FILE
+#ifdef WRITE_LPS_TO_FILE
+ string date_time = getDateAndTimeAsString();
+ string filename = "LP_findExtremumOverLine"+date_time+".dat";
+ if(!m_solver->writeLpToFile(filename.c_str(), c, lb, ub, A, Alb, Aub))
+ SEND_ERROR_MSG("Error while writing LP to file "+filename);
+ filename = "LP_findExtremumOverLine"+date_time+"_solution.dat";
+ if(!writeMatrixToFile(filename.c_str(), b_p))
+ SEND_ERROR_MSG("Error while writing LP solution to file "+filename);
+#endif
+
return true;
}
@@ -342,6 +355,16 @@ bool StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, do
double p = m_solver->getObjectiveValue();
com = a0 + a*p;
+#ifdef WRITE_LPS_TO_FILE
+ string date_time = getDateAndTimeAsString();
+ string filename = "DLP_findExtremumOverLine"+date_time+".dat";
+ if(!m_solver->writeLpToFile(filename.c_str(), c, lb, ub, A, Alb, Aub))
+ SEND_ERROR_MSG("Error while writing LP to file "+filename);
+ filename = "DLP_findExtremumOverLine"+date_time+"_solution.dat";
+ if(!writeMatrixToFile(filename.c_str(), v))
+ SEND_ERROR_MSG("Error while writing LP solution to file "+filename);
+#endif
+
// 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)
{
diff --git a/src/util.cpp b/src/util.cpp
index fa1cede711e99dd3bdb251539017ca8523957fa3..7ffb2ae8fbcff35e381cebb1fa5897e248471754 100644
--- a/src/util.cpp
+++ b/src/util.cpp
@@ -7,6 +7,7 @@
#define _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
#include <robust-equilibrium-lib/util.hh>
+#include <ctime>
namespace robust_equilibrium
{
@@ -130,6 +131,17 @@ Rotation crossMatrix(Cref_vector3 x)
return res;
}
+std::string getDateAndTimeAsString()
+{
+ time_t rawtime;
+ struct tm * timeinfo;
+ char buffer[80];
+ time (&rawtime);
+ timeinfo = localtime(&rawtime);
+ strftime(buffer,80,"%Y%m%d_%I%M%S",timeinfo);
+ return std::string(buffer);
+}
+
} //namespace robust_equilibrium
#endif //_ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
index eee930df25dcd769fdbc23a4d25fd36e0a98ead1..27faf5d010531d5f132c32fa31163ed6411fd898 100644
--- a/test/test_LP_solvers.cpp
+++ b/test/test_LP_solvers.cpp
@@ -22,6 +22,8 @@ using namespace std;
using namespace robust_equilibrium;
USING_NAMESPACE_QPOASES
+#define EPS 1e-6
+
#ifdef CLP_FOUND
/** Example addRows.cpp */
void test_addRows()
@@ -428,12 +430,58 @@ int main()
cout<<"Check constraint matrix A: "<<A.isApprox(A2)<<endl;
cout<<"Check constraint lower bound vector Alb: "<<Alb.isApprox(Alb2)<<endl;
cout<<"Check constraint upper bound vector Aub: "<<Aub.isApprox(Aub2)<<endl;
+ }
+
+ {
+ 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;
+ string problem_filenames[PROBLEM_NUMBER] = {"DLP_findExtremumOverLine20151103_112611",
+ "DLP_findExtremumOverLine20151103_115627",
+ "LP_findExtremumOverLine20151103_112610",
+ "LP_findExtremumOverLine20151103_112611"};
+ VectorX c, lb, ub, Alb, Aub, realSol, sol;
+ MatrixXX A;
+ for(int i=0; i<PROBLEM_NUMBER; i++)
+ {
+ string &problem_filename = problem_filenames[i];
+ if(!solverOases->readLpFromFile(file_path+problem_filename+".dat", c, lb, ub, A, Alb, Aub))
+ {
+ SEND_ERROR_MSG("Error while reading LP from file "+problem_filename);
+ return -1;
+ }
+ string solution_filename = problem_filename+"_solution";
+ if(!readMatrixFromFile(file_path+solution_filename+".dat", realSol))
+ {
+ SEND_ERROR_MSG("Error while reading LP solution from file "+solution_filename);
+ return -1;
+ }
+ sol.resize(c.size());
+ 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";
+ }
+ 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";
+ else
+ {
+ cout<<"[ERROR] Solution of problem "<<problem_filename<<" 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;
+ cout<<"\tCost expected "<<(c.dot(realSol))<<endl;
+ }
+ }
+ }
return 0;
}
-
#ifdef CLP_FOUND
test_addRows();
test_small_LP();
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 2834c568d16d003b1c3edd21adeaa78b32ef0689..17b9368127ab8689fd1dd5e98b2f01f4060a1e51 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -260,8 +260,8 @@ int main()
/************************************** USER PARAMETERS *******************************/
double mass = 70.0;
double mu = 0.3; // friction coefficient
- unsigned int generatorsPerContact = 8;
- unsigned int N_CONTACTS = 2;
+ unsigned int generatorsPerContact = 4;
+ unsigned int N_CONTACTS = 1;
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
@@ -272,7 +272,7 @@ int main()
RPY_UPPER_BOUNDS << +0*gamma, +0*gamma, +M_PI;
double X_MARG = 0.07;
double Y_MARG = 0.07;
- const int GRID_SIZE = 15;
+ const int GRID_SIZE = 5;
/************************************ END USER PARAMETERS *****************************/
cout<<"Number of contacts: "<<N_CONTACTS<<endl;
diff --git a/test_data/DLP_findExtremumOverLine20151103_112611.dat b/test_data/DLP_findExtremumOverLine20151103_112611.dat
new file mode 100644
index 0000000000000000000000000000000000000000..b22aa57c927127ef59bb14e8f00c5d1fb545e64d
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine20151103_112611.dat differ
diff --git a/test_data/DLP_findExtremumOverLine20151103_112611_solution.dat b/test_data/DLP_findExtremumOverLine20151103_112611_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..de3853d919e0105c791f2fe9e829c8b57c4cef4a
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine20151103_112611_solution.dat differ
diff --git a/test_data/DLP_findExtremumOverLine20151103_115627.dat b/test_data/DLP_findExtremumOverLine20151103_115627.dat
new file mode 100644
index 0000000000000000000000000000000000000000..15d24c756d1aad60ffaadcf9f299578f5be0f390
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine20151103_115627.dat differ
diff --git a/test_data/DLP_findExtremumOverLine20151103_115627_solution.dat b/test_data/DLP_findExtremumOverLine20151103_115627_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..d1684bd0081edc83e4e41ea4ea7caf55e05c95b0
Binary files /dev/null and b/test_data/DLP_findExtremumOverLine20151103_115627_solution.dat differ
diff --git a/test_data/LP_findExtremumOverLine20151103_112610.dat b/test_data/LP_findExtremumOverLine20151103_112610.dat
new file mode 100644
index 0000000000000000000000000000000000000000..0071f421913be82d09807d6360b1671f39966b2b
Binary files /dev/null and b/test_data/LP_findExtremumOverLine20151103_112610.dat differ
diff --git a/test_data/LP_findExtremumOverLine20151103_112610_solution.dat b/test_data/LP_findExtremumOverLine20151103_112610_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..6a642a25eeddb2063090419fd83f76057f4558be
Binary files /dev/null and b/test_data/LP_findExtremumOverLine20151103_112610_solution.dat differ
diff --git a/test_data/LP_findExtremumOverLine20151103_112611.dat b/test_data/LP_findExtremumOverLine20151103_112611.dat
new file mode 100644
index 0000000000000000000000000000000000000000..f2d649d86d6ad499229c13b0070b61069c28a23b
Binary files /dev/null and b/test_data/LP_findExtremumOverLine20151103_112611.dat differ
diff --git a/test_data/LP_findExtremumOverLine20151103_112611_solution.dat b/test_data/LP_findExtremumOverLine20151103_112611_solution.dat
new file mode 100644
index 0000000000000000000000000000000000000000..1672cfe8e4f4ad7b5a9c70c02fccd6c2d3b073b5
Binary files /dev/null and b/test_data/LP_findExtremumOverLine20151103_112611_solution.dat differ