diff --git a/include/robust-equilibrium-lib/solver_LP_abstract.hh b/include/robust-equilibrium-lib/solver_LP_abstract.hh
index 6cbe04aee6219a717e2992fc2b7e505428a8d120..148fdc5e1e9383a937e20bca751dc43636546d7b 100644
--- a/include/robust-equilibrium-lib/solver_LP_abstract.hh
+++ b/include/robust-equilibrium-lib/solver_LP_abstract.hh
@@ -48,6 +48,51 @@ public:
Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
Ref_vectorX sol) = 0;
+ /**
+ * @brief Solve the linear program described in the specified file.
+ * @param filename Name of the file containing the LP description.
+ * @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);
+
+ /**
+ * @brief Write the specified Linear Program to binary file.
+ * minimize c' x
+ * subject to Alb <= A x <= Aub
+ * lb <= x <= ub
+ * @param filename
+ * @param c
+ * @param lb
+ * @param ub
+ * @param A
+ * @param Alb
+ * @param Aub
+ * @return True if the operation succeeded, false otherwise.
+ */
+ virtual bool writeLpToFile(const char* filename,
+ Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
+ Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub);
+
+ /**
+ * @brief Read the data describing a Linear Program from the specified binary file.
+ * The vectors and matrices are resized inside the method.
+ * minimize c' x
+ * subject to Alb <= A x <= Aub
+ * lb <= x <= ub
+ * @param filename
+ * @param c
+ * @param lb
+ * @param ub
+ * @param A
+ * @param Alb
+ * @param Aub
+ * @return True if the operation succeeded, false otherwise.
+ */
+ virtual bool readLpFromFile(const char* filename,
+ VectorX &c, VectorX &lb, VectorX &ub,
+ MatrixXX &A, VectorX &Alb, VectorX &Aub);
+
/** Get the status of the solver. */
virtual LP_status getStatus() = 0;
diff --git a/include/robust-equilibrium-lib/util.hh b/include/robust-equilibrium-lib/util.hh
index 9dbe3049ac22215c4254c1cbe00bcb94668a477e..5c22c80754d2f1b617747ae9bca38bcad22da27c 100644
--- a/include/robust-equilibrium-lib/util.hh
+++ b/include/robust-equilibrium-lib/util.hh
@@ -1,6 +1,9 @@
#ifndef _ROBUST_EQUILIBRIUM_LIB_UTIL_HH
#define _ROBUST_EQUILIBRIUM_LIB_UTIL_HH
+#include <iostream>
+#include <fstream>
+
#include <Eigen/Dense>
#include <Eigen/src/Core/util/Macros.h>
@@ -12,77 +15,106 @@
namespace robust_equilibrium
{
-//#define USE_FLOAT 1;
+ //#define USE_FLOAT 1;
#ifdef USE_FLOAT
-typedef float value_type;
+ typedef float value_type;
#else
-typedef double value_type;
+ typedef double value_type;
#endif
-typedef Eigen::Matrix <value_type, 2, 1> Vector2;
-typedef Eigen::Matrix <value_type, 1, 2> RVector2;
-typedef Eigen::Matrix <value_type, 3, 1> Vector3;
-typedef Eigen::Matrix <value_type, 1, 3> RVector3;
-typedef Eigen::Matrix <value_type, 6, 1> Vector6;
-typedef Eigen::Matrix <value_type, Eigen::Dynamic, 1> VectorX;
-typedef Eigen::Matrix <value_type, 1, Eigen::Dynamic> RVectorX;
-typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Rotation;
-typedef Eigen::Matrix <value_type, Eigen::Dynamic, 2, Eigen::RowMajor> MatrixX2;
-typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Matrix3;
-typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3;
-typedef Eigen::Matrix <value_type, 3, Eigen::Dynamic, Eigen::RowMajor> Matrix3X;
-typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
-typedef Eigen::Matrix <value_type, 6, Eigen::Dynamic, Eigen::RowMajor> Matrix6X;
-typedef Eigen::Matrix <value_type, 6, 2, Eigen::RowMajor> Matrix62;
-typedef Eigen::Matrix <value_type, 6, 3, Eigen::RowMajor> Matrix63;
-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
+ typedef Eigen::Matrix <value_type, 2, 1> Vector2;
+ typedef Eigen::Matrix <value_type, 1, 2> RVector2;
+ typedef Eigen::Matrix <value_type, 3, 1> Vector3;
+ typedef Eigen::Matrix <value_type, 1, 3> RVector3;
+ typedef Eigen::Matrix <value_type, 6, 1> Vector6;
+ typedef Eigen::Matrix <value_type, Eigen::Dynamic, 1> VectorX;
+ typedef Eigen::Matrix <value_type, 1, Eigen::Dynamic> RVectorX;
+ typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Rotation;
+ typedef Eigen::Matrix <value_type, Eigen::Dynamic, 2, Eigen::RowMajor> MatrixX2;
+ typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Matrix3;
+ typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3;
+ typedef Eigen::Matrix <value_type, 3, Eigen::Dynamic, Eigen::RowMajor> Matrix3X;
+ typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
+ typedef Eigen::Matrix <value_type, 6, Eigen::Dynamic, Eigen::RowMajor> Matrix6X;
+ typedef Eigen::Matrix <value_type, 6, 2, Eigen::RowMajor> Matrix62;
+ typedef Eigen::Matrix <value_type, 6, 3, Eigen::RowMajor> Matrix63;
+ 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;
-typedef Eigen::Ref<Rotation> Ref_rotation;
-typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
-typedef Eigen::Ref<Matrix43> Ref_matrix43;
-typedef Eigen::Ref<Matrix6X> Ref_matrix6X;
-typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
-
-typedef const Eigen::Ref<const Vector2> & Cref_vector2;
-typedef const Eigen::Ref<const Vector3> & Cref_vector3;
-typedef const Eigen::Ref<const VectorX> & Cref_vectorX;
-typedef const Eigen::Ref<const Rotation> & Cref_rotation;
-typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
-typedef const Eigen::Ref<const Matrix43> & Cref_matrix43;
-typedef const Eigen::Ref<const Matrix6X> & Cref_matrix6X;
-typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
+ typedef Eigen::Ref<Vector2> Ref_vector2;
+ typedef Eigen::Ref<Vector3> Ref_vector3;
+ typedef Eigen::Ref<VectorX> Ref_vectorX;
+ typedef Eigen::Ref<Rotation> Ref_rotation;
+ typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
+ typedef Eigen::Ref<Matrix43> Ref_matrix43;
+ typedef Eigen::Ref<Matrix6X> Ref_matrix6X;
+ typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
+
+ typedef const Eigen::Ref<const Vector2> & Cref_vector2;
+ typedef const Eigen::Ref<const Vector3> & Cref_vector3;
+ typedef const Eigen::Ref<const VectorX> & Cref_vectorX;
+ typedef const Eigen::Ref<const Rotation> & Cref_rotation;
+ typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
+ 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.
+ */
+ template<class Matrix>
+ void writeMatrixToFile(const char* filename, const Matrix& matrix)
+ {
+ std::ofstream out(filename, 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();
+ }
+
+ /**
+ * Read a matrix from the specified input binary file.
+ */
+ template<class Matrix>
+ void readMatrixFromFile(const char* filename, Matrix& matrix)
+ {
+ std::ifstream in(filename, 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();
+ }
+
+ /**
* Convert the specified list of rays from Eigen to cdd format.
* @param input The mXn input Eigen matrix contains m rays of dimension n.
* @return The mX(n+1) output cdd matrix, which contains an additional column,
* the first one, with all zeros.
*/
-dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input);
+ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input);
-/**
+ /**
* Compute the cross-product skew-symmetric matrix associated to the specified vector.
*/
-Rotation crossMatrix(Cref_vector3 x);
+ Rotation crossMatrix(Cref_vector3 x);
-void init_cdd_library();
+ void init_cdd_library();
-void release_cdd_library();
+ void release_cdd_library();
-void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out);
+ void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out);
-void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R);
+ void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R);
-bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy,
- Ref_matrix43 p, Ref_matrix43 N);
+ bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy,
+ Ref_matrix43 p, Ref_matrix43 N);
} //namespace robust_equilibrium
diff --git a/src/solver_LP_abstract.cpp b/src/solver_LP_abstract.cpp
index 7903af1c04b7ecd2a7e75ae82495332950092340..6bfbadd5a038bc972af112f716f4584ac5684732 100644
--- a/src/solver_LP_abstract.cpp
+++ b/src/solver_LP_abstract.cpp
@@ -32,6 +32,66 @@ Solver_LP_abstract* Solver_LP_abstract::getNewSolver(SolverLP solverType)
return NULL;
}
+bool Solver_LP_abstract::writeLpToFile(const char* filename,
+ Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
+ Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub)
+{
+ MatrixXX::Index n=c.size(), m=A.rows();
+ assert(lb.size()==n);
+ assert(ub.size()==n);
+ assert(A.cols()==n);
+ assert(Alb.size()==m);
+ assert(Aub.size()==m);
+
+ std::ofstream out(filename, 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) );
+ out.write((char*) lb.data(), n*sizeof(typename MatrixXX::Scalar) );
+ out.write((char*) ub.data(), n*sizeof(typename MatrixXX::Scalar) );
+ out.write((char*) A.data(), m*n*sizeof(typename MatrixXX::Scalar) );
+ out.write((char*) Alb.data(), m*sizeof(typename MatrixXX::Scalar) );
+ out.write((char*) Aub.data(), m*sizeof(typename MatrixXX::Scalar) );
+ out.close();
+ return true;
+}
+
+bool Solver_LP_abstract::readLpFromFile(const char* filename,
+ VectorX &c, VectorX &lb, VectorX &ub,
+ MatrixXX &A, VectorX &Alb, VectorX &Aub)
+{
+ std::ifstream in(filename, 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));
+ c.resize(n);
+ lb.resize(n);
+ ub.resize(n);
+ A.resize(m,n);
+ Alb.resize(m);
+ Aub.resize(m);
+ in.read( (char *) c.data() , n*sizeof(typename MatrixXX::Scalar) );
+ in.read( (char *) lb.data() , n*sizeof(typename MatrixXX::Scalar) );
+ in.read( (char *) ub.data() , n*sizeof(typename MatrixXX::Scalar) );
+ in.read( (char *) A.data() , m*n*sizeof(typename MatrixXX::Scalar) );
+ in.read( (char *) Alb.data() , m*sizeof(typename MatrixXX::Scalar) );
+ in.read( (char *) Aub.data() , m*sizeof(typename MatrixXX::Scalar) );
+ in.close();
+ return true;
+}
+
+LP_status Solver_LP_abstract::solve(const char* filename, Ref_vectorX sol)
+{
+ VectorX c, lb, ub, Alb, Aub;
+ MatrixXX A;
+ if(!readLpFromFile(filename, c, lb, ub, A, Alb, Aub))
+ {
+ SEND_ERROR_MSG("Error while reading LP from file "+string(filename));
+ return LP_STATUS_ERROR;
+ }
+ return solve(c, lb, ub, A, Alb, Aub, sol);
+}
+
} // end namespace robust_equilibrium
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
index 7a837077a55208f5844b459fe9c8b27307d54b76..eee930df25dcd769fdbc23a4d25fd36e0a98ead1 100644
--- a/test/test_LP_solvers.cpp
+++ b/test/test_LP_solvers.cpp
@@ -13,6 +13,7 @@
#include <qpOASES.hpp>
#include <robust-equilibrium-lib/solver_LP_qpoases.hh>
+#include <robust-equilibrium-lib/logger.hh>
#include <iostream>
#include <iomanip>
@@ -370,9 +371,10 @@ void test_small_LP()
int main()
{
- cout <<"Test LP Solvers\n";
+ cout <<"Test LP Solvers\n\n";
{
+ cout<<"TEST QP OASES ON A SMALL 2-VARIABLE LP\n";
/* Setup data of first LP. */
real_t A[1*2] = { 1.0, 1.0 };
real_t g[2] = { 1.5, 1.0 };
@@ -393,6 +395,45 @@ int main()
example.init( 0,g,A,lb,ub,lbA,ubA, nWSR,0 );
}
+ {
+ cout<<"\nTEST READ-WRITE METHODS OF SOLVER_LP_ABSTRACT\n";
+ Solver_LP_abstract *solverOases = Solver_LP_abstract::getNewSolver(SOLVER_LP_QPOASES);
+ const int n = 3;
+ const int m = 4;
+ const char* filename = "small_3_x_4_LP.dat";
+ VectorX c = VectorX::Random(n);
+ VectorX lb = -100*VectorX::Ones(n);
+ VectorX ub = 100*VectorX::Ones(n);
+ MatrixXX A = MatrixXX::Random(m,n);
+ VectorX Alb = -100*VectorX::Ones(m);
+ VectorX Aub = 100*VectorX::Ones(m);
+ if(!solverOases->writeLpToFile(filename, c, lb, ub, A, Alb, Aub))
+ {
+ SEND_ERROR_MSG("Error while writing LP to file");
+ return -1;
+ }
+ VectorX c2, lb2, ub2, Alb2, Aub2;
+ MatrixXX A2;
+ if(!solverOases->readLpFromFile(filename, c2, lb2, ub2, A2, Alb2, Aub2))
+ {
+ SEND_ERROR_MSG("Error while reading LP from file");
+ return -1;
+ }
+
+ cout<<"Check number of variables: "<<(c.size()==c2.size())<<endl;
+ cout<<"Check number of constraints: "<<(A.rows()==A2.rows())<<endl;
+ cout<<"Check gradient vector c: "<<c.isApprox(c2)<<endl;
+ cout<<"Check lower bound vector lb: "<<lb.isApprox(lb2)<<endl;
+ cout<<"Check upper bound vector ub: "<<ub.isApprox(ub2)<<endl;
+ 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;
+
+ return 0;
+ }
+
+
+
#ifdef CLP_FOUND
test_addRows();
test_small_LP();
@@ -417,39 +458,5 @@ int main()
cout<<"solver_LP_clp failed to solve the problem\n";
#endif
-// char x[81];
-// int iRow;
-// // get row copy
-// CoinPackedMatrix rowCopy = *model.matrix();
-// rowCopy.reverseOrdering();
-// const int * column = rowCopy.getIndices();
-// const int * rowLength = rowCopy.getVectorLengths();
-// const CoinBigIndex * rowStart = rowCopy.getVectorStarts();
-// x[n] = '\0';
-// for (iRow = 0; iRow < m; iRow++) {
-// memset(x, ' ', n);
-// for (int k = rowStart[iRow]; k < rowStart[iRow] + rowLength[iRow]; k++) {
-// int iColumn = column[k];
-// x[iColumn] = 'x';
-// }
-// cout<<x<<endl;
-// }
-// cout<<endl;
-
-// // Now matrix
-// CoinPackedMatrix * matrix = model.matrix();
-// const double * element = matrix->getElements();
-// const int * row = matrix->getIndices();
-// const int * start = matrix->getVectorStarts();
-// const int * length = matrix->getVectorLengths();
-// for (int iColumn = 0; iColumn < n; iColumn++)
-// {
-// cout << "Column " << iColumn;
-// int j;
-// for (j = start[iColumn]; j < start[iColumn] + length[iColumn]; j++)
-// cout << " ( " << row[j] << ", " << element[j] << ")";
-// cout << endl;
-// }
-
return 1;
}