From faa4fdf630196d0ff07a21c5812d2a9cbd45b998 Mon Sep 17 00:00:00 2001
From: andreadelprete <andre.delprete@gmail.com>
Date: Wed, 28 Oct 2015 19:42:15 +0100
Subject: [PATCH] Implement LP solver using coin-or LP. Add in test_LP_solver
some small LP tests. Implement primal and dual LP algorithm to compute
robustness of a given com position. Test algorithm against PP: primal works
always, while dual works only sometimes (need to investigate).
---
CMakeLists.txt | 3 +
cmake2/FindCLP.cmake | 26 ++
include/robust-equilibrium-lib/config.hh | 5 +
include/robust-equilibrium-lib/logger.hh | 175 ++++++++
.../solver_LP_abstract.hh | 50 ++-
.../robust-equilibrium-lib/solver_LP_clp.hh | 58 +++
.../solver_LP_qpoases.hh | 4 +
.../static_equilibrium.hh | 9 +-
include/robust-equilibrium-lib/util.hh | 5 +-
src/CMakeLists.txt | 9 +-
src/logger.cpp | 118 +++++
src/solver_LP_abstract.cpp | 27 ++
src/solver_LP_clp.cpp | 103 +++++
src/solver_LP_qpoases.cpp | 4 +
src/static_equilibrium.cpp | 139 +++++-
src/util.cpp | 77 +---
test/CMakeLists.txt | 1 +
test/test_LP_solvers.cpp | 424 +++++++++++++++++-
test/test_static_equilibrium.cpp | 62 ++-
19 files changed, 1189 insertions(+), 110 deletions(-)
create mode 100644 cmake2/FindCLP.cmake
create mode 100644 include/robust-equilibrium-lib/logger.hh
create mode 100644 include/robust-equilibrium-lib/solver_LP_clp.hh
create mode 100644 src/logger.cpp
create mode 100644 src/solver_LP_abstract.cpp
create mode 100644 src/solver_LP_clp.cpp
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 56a2770..9569217 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -31,8 +31,10 @@ MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS} )
SET(${PROJECT_NAME}_HEADERS
include/robust-equilibrium-lib/config.hh
include/robust-equilibrium-lib/util.hh
+ include/robust-equilibrium-lib/logger.hh
include/robust-equilibrium-lib/solver_LP_abstract.hh
include/robust-equilibrium-lib/solver_LP_qpoases.hh
+ include/robust-equilibrium-lib/solver_LP_clp.hh
include/robust-equilibrium-lib/static_equilibrium.hh
)
@@ -45,6 +47,7 @@ else()
endif()
find_package(CDD REQUIRED)
+find_package(CLP REQUIRED)
add_subdirectory (src)
add_subdirectory (test)
diff --git a/cmake2/FindCLP.cmake b/cmake2/FindCLP.cmake
new file mode 100644
index 0000000..c8f2549
--- /dev/null
+++ b/cmake2/FindCLP.cmake
@@ -0,0 +1,26 @@
+# - Try to find libcdd
+# Once done this will define
+# CDD_FOUND - System has CDD
+# CDD_INCLUDE_DIRS - The CDD include directories
+# CDD_LIBRARIES - The libraries needed to use CDD
+# CDD_DEFINITIONS - Compiler switches required for using CDD
+
+# /usr/include/coin, /usr/lib/libClp.so
+
+find_path(CLP_INCLUDE_DIR coin/ClpSimplex.hpp
+ HINTS ${CLP_INCLUDEDIR}
+ PATH_SUFFIXES CLP )
+
+find_library(CLP_LIBRARY NAMES libclp
+ HINTS ${CLP_LIBDIR} ${CLP_LIBRARY_DIRS} )
+
+set(CLP_LIBRARIES ${CLP_LIBRARY} )
+set(CLP_INCLUDE_DIRS ${CLP_INCLUDE_DIR} )
+
+include(FindPackageHandleStandardArgs)
+# handle the QUIETLY and REQUIRED arguments and set CDD_FOUND to TRUE
+# if all listed variables are TRUE
+find_package_handle_standard_args(CLP DEFAULT_MSG
+ CLP_LIBRARY CLP_INCLUDE_DIR)
+
+mark_as_advanced(CLP_INCLUDE_DIR CLP_LIBRARY )
diff --git a/include/robust-equilibrium-lib/config.hh b/include/robust-equilibrium-lib/config.hh
index 850e82d..6f042ad 100644
--- a/include/robust-equilibrium-lib/config.hh
+++ b/include/robust-equilibrium-lib/config.hh
@@ -1,3 +1,8 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
#ifndef _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
#define _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
diff --git a/include/robust-equilibrium-lib/logger.hh b/include/robust-equilibrium-lib/logger.hh
new file mode 100644
index 0000000..98180c8
--- /dev/null
+++ b/include/robust-equilibrium-lib/logger.hh
@@ -0,0 +1,175 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
+#ifndef __robust_equilibrium_lib_logger_H__
+#define __robust_equilibrium_lib_logger_H__
+
+/* --------------------------------------------------------------------- */
+/* --- INCLUDE --------------------------------------------------------- */
+/* --------------------------------------------------------------------- */
+
+#include <robust-equilibrium-lib/config.hh>
+#include <sstream>
+#include <map>
+#include <initializer_list>
+#include "boost/assign.hpp"
+
+namespace robust_equilibrium
+{
+
+ //#define LOGGER_VERBOSITY_INFO_WARNING_ERROR
+#define LOGGER_VERBOSITY_ALL
+
+#define SEND_MSG(msg,type) getLogger().sendMsg(msg,type,__FILE__,__LINE__)
+
+#ifdef LOGGER_VERBOSITY_ERROR
+#define SEND_DEBUG_MSG(msg)
+#define SEND_INFO_MSG(msg)
+#define SEND_WARNING_MSG(msg)
+#define SEND_ERROR_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR)
+#define SEND_DEBUG_STREAM_MSG(msg)
+#define SEND_INFO_STREAM_MSG(msg)
+#define SEND_WARNING_STREAM_MSG(msg)
+#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
+#endif
+
+#ifdef LOGGER_VERBOSITY_WARNING_ERROR
+#define SEND_DEBUG_MSG(msg)
+#define SEND_INFO_MSG(msg)
+#define SEND_WARNING_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING)
+#define SEND_ERROR_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR)
+#define SEND_DEBUG_STREAM_MSG(msg)
+#define SEND_INFO_STREAM_MSG(msg)\
+#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
+#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
+#endif
+
+#ifdef LOGGER_VERBOSITY_INFO_WARNING_ERROR
+#define SEND_DEBUG_MSG(msg)
+#define SEND_INFO_MSG(msg) SEND_MSG(msg,MSG_TYPE_INFO)
+#define SEND_WARNING_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING)
+#define SEND_ERROR_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR)
+#define SEND_DEBUG_STREAM_MSG(msg)
+#define SEND_INFO_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_INFO_STREAM)
+#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
+#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
+#endif
+
+#ifdef LOGGER_VERBOSITY_ALL
+#define SEND_DEBUG_MSG(msg) SEND_MSG(msg,MSG_TYPE_DEBUG)
+#define SEND_INFO_MSG(msg) SEND_MSG(msg,MSG_TYPE_INFO)
+#define SEND_WARNING_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING)
+#define SEND_ERROR_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR)
+#define SEND_DEBUG_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_DEBUG_STREAM)
+#define SEND_INFO_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_INFO_STREAM)
+#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
+#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
+#endif
+
+ /** Enum representing the different kind of messages.
+ */
+ enum ROBUST_EQUILIBRIUM_DLLAPI MsgType
+ {
+ MSG_TYPE_DEBUG =0,
+ MSG_TYPE_INFO =1,
+ MSG_TYPE_WARNING =2,
+ MSG_TYPE_ERROR =3,
+ MSG_TYPE_DEBUG_STREAM =4,
+ MSG_TYPE_INFO_STREAM =5,
+ MSG_TYPE_WARNING_STREAM =6,
+ MSG_TYPE_ERROR_STREAM =7
+ };
+
+ template<typename T>
+ std::string toString(const T& v)
+ {
+ std::stringstream ss;
+ ss<<v;
+ return ss.str();
+ }
+
+ template<typename T>
+ std::string toString(const std::vector<T>& v, const std::string separator=", ")
+ {
+ std::stringstream ss;
+ for(int i=0; i<v.size()-1; i++)
+ ss<<v[i]<<separator;
+ ss<<v[v.size()-1];
+ return ss.str();
+ }
+
+ enum ROBUST_EQUILIBRIUM_DLLAPI LoggerVerbosity
+ {
+ VERBOSITY_ALL,
+ VERBOSITY_INFO_WARNING_ERROR,
+ VERBOSITY_WARNING_ERROR,
+ VERBOSITY_ERROR,
+ VERBOSITY_NONE
+ };
+
+ /** A simple class for logging messages
+ */
+ class ROBUST_EQUILIBRIUM_DLLAPI Logger
+ {
+ public:
+
+ /** Constructor */
+ Logger(double timeSample=0.001, double streamPrintPeriod=1.0);
+
+ /** Destructor */
+ ~Logger(){}
+
+ /** Method to be called at every control iteration
+ * to decrement the internal Logger's counter. */
+ void countdown();
+
+ /** Print the specified message on standard output if the verbosity level
+ * allows it. The file name and the line number are used to identify
+ * the point where sendMsg is called so that streaming messages are
+ * printed only every streamPrintPeriod iterations.
+ */
+ void sendMsg(std::string msg, MsgType type, const char* file="", int line=0);
+
+ /** Set the sampling time at which the method countdown()
+ * is going to be called. */
+ bool setTimeSample(double t);
+
+ /** Set the time period for printing of streaming messages. */
+ bool setStreamPrintPeriod(double s);
+
+ /** Set the verbosity level of the logger. */
+ void setVerbosity(LoggerVerbosity lv);
+
+ protected:
+ LoggerVerbosity m_lv; /// verbosity of the logger
+ double m_timeSample; /// specify the period of call of the countdown method
+ double m_streamPrintPeriod; /// specify the time period of the stream prints
+ double m_printCountdown; /// every time this is < 0 (i.e. every _streamPrintPeriod sec) print stuff
+
+ /** Pointer to the dynamic structure which holds the collection of streaming messages */
+ std::map<std::string, double> m_stream_msg_counters;
+
+ bool isStreamMsg(MsgType m)
+ { return m==MSG_TYPE_ERROR_STREAM || m==MSG_TYPE_DEBUG_STREAM || m==MSG_TYPE_INFO_STREAM || m==MSG_TYPE_WARNING_STREAM; }
+
+ bool isDebugMsg(MsgType m)
+ { return m==MSG_TYPE_DEBUG_STREAM || m==MSG_TYPE_DEBUG; }
+
+ bool isInfoMsg(MsgType m)
+ { return m==MSG_TYPE_INFO_STREAM || m==MSG_TYPE_INFO; }
+
+ bool isWarningMsg(MsgType m)
+ { return m==MSG_TYPE_WARNING_STREAM || m==MSG_TYPE_WARNING; }
+
+ bool isErrorMsg(MsgType m)
+ { return m==MSG_TYPE_ERROR_STREAM || m==MSG_TYPE_ERROR; }
+ };
+
+ /** Method to get the logger (singleton). */
+ Logger& getLogger();
+
+} // namespace robust_equilibrium
+
+#endif // #ifndef __sot_torque_control_trajectory_generators_H__
diff --git a/include/robust-equilibrium-lib/solver_LP_abstract.hh b/include/robust-equilibrium-lib/solver_LP_abstract.hh
index 41ad038..9f03bbe 100644
--- a/include/robust-equilibrium-lib/solver_LP_abstract.hh
+++ b/include/robust-equilibrium-lib/solver_LP_abstract.hh
@@ -1,31 +1,69 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
#ifndef ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_ABSTRACT_HH
#define ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_ABSTRACT_HH
#include <Eigen/Dense>
#include <robust-equilibrium-lib/config.hh>
+#include <robust-equilibrium-lib/util.hh>
namespace robust_equilibrium
{
enum ROBUST_EQUILIBRIUM_DLLAPI SolverLP
{
+ SOLVER_LP_CLP,
SOLVER_LP_QPOASES
};
-class ROBUST_EQUILIBRIUM_DLLAPI SolverLPAbstract
+enum ROBUST_EQUILIBRIUM_DLLAPI LP_status
{
-private:
- std::string m_name;
+ LP_STATUS_UNKNOWN=-1, //before solve or if postSolve says not optimal
+ LP_STATUS_OPTIMAL=0,
+ LP_STATUS_INFEASIBLE=1,
+ LP_STATUS_DUAL_INFEASIBLE=2,
+ LP_STATUS_MAX_ITER_REACHED=3,
+ LP_STATUS_ERROR=4
+};
+class ROBUST_EQUILIBRIUM_DLLAPI Solver_LP_abstract
+{
public:
- SolverLPAbstract(std::string name);
+ Solver_LP_abstract(){}
+
+ static Solver_LP_abstract* getNewSolver(SolverLP solverType);
/** Solve the linear program
* minimize c' x
- * subject to lb <= A x <= ub
+ * subject to Alb <= A x <= Aub
+ * lb <= x <= ub
*/
- virtual bool solve(Cref_vectorX c, Cref_matrixXX A, Cref_vectorX lb, Cref_vectorX ub) = 0;
+ virtual LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
+ Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
+ Ref_vectorX sol) = 0;
+
+ /** Get the status of the solver. */
+ virtual LP_status getStatus() = 0;
+
+ /** Get the objective value of the last solved problem. */
+ virtual double getObjectiveValue() = 0;
+
+ /** Get the current maximum number of iterations performed
+ * by the solver.
+ */
+ unsigned int getMaximumIterations();
+
+ /** Set the current maximum number of iterations performed
+ * by the solver.
+ */
+ bool setMaximumIterations(unsigned int maxIter);
+
+ /** Set the maximum time allowed to solve a problem. */
+ bool setMaximumTime(double seconds);
};
diff --git a/include/robust-equilibrium-lib/solver_LP_clp.hh b/include/robust-equilibrium-lib/solver_LP_clp.hh
new file mode 100644
index 0000000..2478a43
--- /dev/null
+++ b/include/robust-equilibrium-lib/solver_LP_clp.hh
@@ -0,0 +1,58 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
+#ifndef ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_CLP_HH
+#define ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_CLP_HH
+
+#include <robust-equilibrium-lib/config.hh>
+#include <robust-equilibrium-lib/util.hh>
+#include <robust-equilibrium-lib/solver_LP_abstract.hh>
+#include "ClpSimplex.hpp"
+
+namespace robust_equilibrium
+{
+
+class ROBUST_EQUILIBRIUM_DLLAPI Solver_LP_clp: public Solver_LP_abstract
+{
+private:
+ ClpSimplex m_model;
+
+public:
+
+ Solver_LP_clp();
+
+ /** Solve the linear program
+ * minimize c' x
+ * subject to Alb <= A x <= Aub
+ * lb <= x <= ub
+ */
+ LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
+ Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
+ Ref_vectorX sol);
+
+ /** Get the status of the solver. */
+ LP_status getStatus();
+
+ /** Get the objective value of the last solved problem. */
+ double getObjectiveValue();
+
+ /** Get the current maximum number of iterations performed
+ * by the solver.
+ */
+ unsigned int getMaximumIterations();
+
+ /** Set the current maximum number of iterations performed
+ * by the solver.
+ */
+ bool setMaximumIterations(unsigned int maxIter);
+
+ /** Set the maximum time allowed to solve a problem. */
+ bool setMaximumTime(double seconds);
+
+};
+
+} // end namespace robust_equilibrium
+
+#endif //ROBUST_EQUILIBRIUM_LIB_SOLVER_LP_CLP_HH
diff --git a/include/robust-equilibrium-lib/solver_LP_qpoases.hh b/include/robust-equilibrium-lib/solver_LP_qpoases.hh
index e69de29..ed3b145 100644
--- a/include/robust-equilibrium-lib/solver_LP_qpoases.hh
+++ b/include/robust-equilibrium-lib/solver_LP_qpoases.hh
@@ -0,0 +1,4 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index 78604c8..8c1e94e 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -1,3 +1,8 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
#ifndef ROBUST_EQUILIBRIUM_LIB_STATIC_EQUILIBRIUM_H
#define ROBUST_EQUILIBRIUM_LIB_STATIC_EQUILIBRIUM_H
@@ -20,8 +25,10 @@ enum ROBUST_EQUILIBRIUM_DLLAPI StaticEquilibriumAlgorithm
class ROBUST_EQUILIBRIUM_DLLAPI StaticEquilibrium
{
private:
+ static bool m_is_cdd_initialized;
+
StaticEquilibriumAlgorithm m_algorithm;
- SolverLPAbstract* m_solver;
+ Solver_LP_abstract* m_solver;
unsigned int m_generatorsPerContact;
double m_mass;
diff --git a/include/robust-equilibrium-lib/util.hh b/include/robust-equilibrium-lib/util.hh
index 8131b54..ad048ab 100644
--- a/include/robust-equilibrium-lib/util.hh
+++ b/include/robust-equilibrium-lib/util.hh
@@ -34,6 +34,7 @@ typedef Eigen::Matrix <value_type, 3, Eigen::Dynamic, Eigen::RowMajor>
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, Eigen::Dynamic, 6, Eigen::RowMajor> MatrixX6;
typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXX;
//define Eigen ref if available
@@ -89,9 +90,9 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input);
Rotation crossMatrix(Cref_vector3 x);
-void init_library();
+void init_cdd_library();
-void release_library();
+void release_cdd_library();
void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out);
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e964689..1be3e52 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -4,6 +4,7 @@ include_directories("${SRC_DIR}")
include_directories("${INCLUDE_DIR}")
include_directories("${EIGEN3_INCLUDE_DIR}")
include_directories("${CDD_INCLUDE_DIR}")
+include_directories("${CLP_INCLUDE_DIR}")
SET(LIBRARY_NAME ${PROJECT_NAME})
@@ -11,10 +12,14 @@ SET(${LIBRARY_NAME}_SOURCES
${INCLUDE_DIR}/robust-equilibrium-lib/config.hh
${INCLUDE_DIR}/robust-equilibrium-lib/solver_LP_abstract.hh
${INCLUDE_DIR}/robust-equilibrium-lib/solver_LP_qpoases.hh
+ ${INCLUDE_DIR}/robust-equilibrium-lib/solver_LP_clp.hh
${INCLUDE_DIR}/robust-equilibrium-lib/static_equilibrium.hh
static_equilibrium.cpp
+ solver_LP_abstract.cpp
solver_LP_qpoases.cpp
+ solver_LP_clp.cpp
util.cpp
+ logger.cpp
)
INCLUDE_DIRECTORIES(${PROJECT_SOURCE_DIR}/src)
@@ -24,7 +29,9 @@ if ( MSVC )
SET(CMAKE_DEBUG_POSTFIX d)
endif ( MSVC )
-TARGET_LINK_LIBRARIES(robust-equilibrium-lib ${CDD_LIBRARY})
+TARGET_LINK_LIBRARIES(robust-equilibrium-lib ${CDD_LIBRARY}
+ ${CLP_LIBRARY}
+ /usr/lib/libCoinUtils.so)
SET_TARGET_PROPERTIES(robust-equilibrium-lib PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}")
SET_TARGET_PROPERTIES(robust-equilibrium-lib PROPERTIES ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}")
diff --git a/src/logger.cpp b/src/logger.cpp
new file mode 100644
index 0000000..6f325a6
--- /dev/null
+++ b/src/logger.cpp
@@ -0,0 +1,118 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
+#ifndef WIN32
+#include <sys/time.h>
+#else
+#include <Windows.h>
+#endif
+
+#include <stdio.h>
+#include <iostream>
+#include <iomanip> // std::setprecision
+#include <boost/algorithm/string.hpp>
+#include <robust-equilibrium-lib/logger.hh>
+
+namespace robust_equilibrium
+{
+ using namespace std;
+
+ Logger& getLogger()
+ {
+ static Logger l(0.001, 1.0);
+ return l;
+ }
+
+ Logger::Logger(double timeSample, double streamPrintPeriod)
+ : m_timeSample(timeSample),
+ m_streamPrintPeriod(streamPrintPeriod),
+ m_printCountdown(0.0)
+ {
+#ifdef LOGGER_VERBOSITY_ERROR
+ m_lv = VERBOSITY_ERROR;
+#endif
+#ifdef LOGGER_VERBOSITY_WARNING_ERROR
+ m_lv = VERBOSITY_WARNING_ERROR;
+#endif
+#ifdef LOGGER_VERBOSITY_INFO_WARNING_ERROR
+ m_lv = VERBOSITY_INFO_WARNING_ERROR;
+#endif
+#ifdef LOGGER_VERBOSITY_ALL
+ m_lv = VERBOSITY_ALL;
+#endif
+ }
+
+ void Logger::countdown()
+ {
+ if(m_printCountdown<0.0)
+ m_printCountdown = m_streamPrintPeriod;
+ m_printCountdown -= m_timeSample;
+ }
+
+ void Logger::sendMsg(string msg, MsgType type, const char* file, int line)
+ {
+// if(m_lv==VERBOSITY_NONE ||
+// (m_lv==VERBOSITY_ERROR && !isErrorMsg(type)) ||
+// (m_lv==VERBOSITY_WARNING_ERROR && !(isWarningMsg(type) || isErrorMsg(type))) ||
+// (m_lv==VERBOSITY_INFO_WARNING_ERROR && isDebugMsg(type)))
+// return;
+
+ // if print is allowed by current verbosity level
+ if(isStreamMsg(type))
+ {
+ // check whether counter already exists
+ string id = file+toString(line);
+ map<string,double>::iterator it = m_stream_msg_counters.find(id);
+ if(it == m_stream_msg_counters.end())
+ {
+ // if counter doesn't exist then add one
+ m_stream_msg_counters.insert(make_pair(id, 0.0));
+ it = m_stream_msg_counters.find(id);
+ }
+
+ // if counter is greater than 0 then decrement it and do not print
+ if(it->second>0.0)
+ {
+ it->second -= m_timeSample;
+ return;
+ }
+ else // otherwise reset counter and print
+ it->second = m_streamPrintPeriod;
+ }
+
+ vector<string> fields;
+ boost::split(fields, file, boost::is_any_of("/"));
+ const char* file_name = fields[fields.size()-1].c_str();
+
+ if(isErrorMsg(type))
+ printf("[ERROR %s %d] %s\n", file_name, line, msg.c_str());
+ else if(isWarningMsg(type))
+ printf("[WARNING %s %d] %s\n", file_name, line, msg.c_str());
+ else if(isInfoMsg(type))
+ printf("[INFO %s %d] %s\n", file_name, line, msg.c_str());
+ else
+ printf("[DEBUG %s %d] %s\n", file_name, line, msg.c_str());
+
+ fflush(stdout); // Prints to screen or whatever your standard out is
+ }
+
+ bool Logger::setTimeSample(double t)
+ {
+ if(t<=0.0)
+ return false;
+ m_timeSample = t;
+ return true;
+ }
+
+ bool Logger::setStreamPrintPeriod(double s)
+ {
+ if(s<=0.0)
+ return false;
+ m_streamPrintPeriod = s;
+ return true;
+ }
+
+} // namespace robust_equilibrium
+
diff --git a/src/solver_LP_abstract.cpp b/src/solver_LP_abstract.cpp
new file mode 100644
index 0000000..bad6cba
--- /dev/null
+++ b/src/solver_LP_abstract.cpp
@@ -0,0 +1,27 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
+#include <robust-equilibrium-lib/solver_LP_abstract.hh>
+#include <robust-equilibrium-lib/solver_LP_clp.hh>
+#include <robust-equilibrium-lib/logger.hh>
+#include <iostream>
+
+using namespace std;
+
+namespace robust_equilibrium
+{
+
+Solver_LP_abstract* Solver_LP_abstract::getNewSolver(SolverLP solverType)
+{
+ if(solverType==SOLVER_LP_CLP)
+ return new Solver_LP_clp();
+
+ SEND_ERROR_MSG("Specified solver type not recognized: "+toString(solverType));
+ return NULL;
+}
+
+
+
+} // end namespace robust_equilibrium
diff --git a/src/solver_LP_clp.cpp b/src/solver_LP_clp.cpp
new file mode 100644
index 0000000..cb3a0b8
--- /dev/null
+++ b/src/solver_LP_clp.cpp
@@ -0,0 +1,103 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
+#include <robust-equilibrium-lib/solver_LP_clp.hh>
+#include "CoinBuild.hpp"
+
+namespace robust_equilibrium
+{
+
+Solver_LP_clp::Solver_LP_clp(): Solver_LP_abstract()
+{
+ m_model.setLogLevel(0);
+}
+
+LP_status Solver_LP_clp::solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
+ Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
+ Ref_vectorX sol)
+{
+ int n = (int)c.size(); // number of variables
+ int m = (int)A.rows(); // number of constraints
+ assert(lb.size()==n);
+ assert(ub.size()==n);
+ assert(A.cols()==n);
+ assert(Alb.size()==m);
+ assert(Aub.size()==m);
+
+ m_model.resize(0, n);
+ int* rowIndex = new int[n];
+
+ for(int i=0; i<n; i++)
+ {
+ m_model.setObjectiveCoefficient(i, c(i));
+ m_model.setColumnLower(i, lb(i));
+ m_model.setColumnUpper(i, ub(i));
+ rowIndex[i] = i;
+ }
+
+// m_model.allSlackBasis();
+
+ // This is not the most efficient way to pass the data to the problem
+ // but it is the best compromise between efficiency and simplicity.
+ // We could be more efficient by using CoinPackedMatrix and ClpPackedMatrix
+ // as shown in the example file "addRows.cpp"
+ CoinBuild buildObject;
+ for (int i=0; i<m; i++)
+ {
+ buildObject.addRow(n, rowIndex, A.row(i).data(), Alb(i), Aub(i));
+ }
+ m_model.addRows(buildObject);
+
+ // solve the problem
+ m_model.primal();
+// m_model.dual();
+
+ if(m_model.isProvenOptimal())
+ {
+ const double *solution = m_model.getColSolution();
+ for(int i=0; i<n; i++)
+ sol(i) = solution[i];
+ }
+
+ return getStatus();
+}
+
+LP_status Solver_LP_clp::getStatus()
+{
+ int status = m_model.status();
+ if(status<5)
+ return (LP_status)status;
+ return LP_STATUS_ERROR;
+}
+
+double Solver_LP_clp::getObjectiveValue()
+{
+ return m_model.objectiveValue();
+}
+
+unsigned int Solver_LP_clp::getMaximumIterations()
+{
+ int integerValue;
+ m_model.getIntParam(ClpMaxNumIteration, integerValue);
+ return integerValue;
+}
+
+bool Solver_LP_clp::setMaximumIterations(unsigned int maxIter)
+{
+ if(maxIter==0)
+ return false;
+ m_model.setMaximumIterations(maxIter);
+ return true;
+}
+
+bool Solver_LP_clp::setMaximumTime(double seconds)
+{
+ if(seconds<=0.0)
+ return false;
+ m_model.setMaximumSeconds(seconds);
+ return true;
+}
+
+} // end namespace robust_equilibrium
diff --git a/src/solver_LP_qpoases.cpp b/src/solver_LP_qpoases.cpp
index e69de29..ed3b145 100644
--- a/src/solver_LP_qpoases.cpp
+++ b/src/solver_LP_qpoases.cpp
@@ -0,0 +1,4 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index de73350..3200612 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -1,4 +1,10 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
#include <robust-equilibrium-lib/static_equilibrium.hh>
+#include <robust-equilibrium-lib/logger.hh>
#include <iostream>
#include <vector>
@@ -7,9 +13,18 @@ using namespace std;
namespace robust_equilibrium
{
+bool StaticEquilibrium::m_is_cdd_initialized = false;
StaticEquilibrium::StaticEquilibrium(double mass, unsigned int generatorsPerContact, SolverLP solver_type)
{
+ if(!m_is_cdd_initialized)
+ {
+ init_cdd_library();
+ m_is_cdd_initialized = true;
+ }
+
+ m_solver = Solver_LP_abstract::getNewSolver(solver_type);
+
m_generatorsPerContact = generatorsPerContact;
m_mass = mass;
m_gravity.setZero();
@@ -27,6 +42,19 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
assert(contactPoints.rows()==contactNormals.rows());
assert(contactPoints.rows()==frictionCoefficients.rows());
+ if(alg==STATIC_EQUILIBRIUM_ALGORITHM_IP)
+ {
+ SEND_ERROR_MSG("Algorithm IP not implemented yet");
+ return false;
+ }
+ if(alg==STATIC_EQUILIBRIUM_ALGORITHM_DIP)
+ {
+ SEND_ERROR_MSG("Algorithm DIP not implemented yet");
+ return false;
+ }
+
+ m_algorithm = alg;
+
// compute tangent directions
long int c = contactPoints.rows();
int cg = m_generatorsPerContact;
@@ -82,25 +110,115 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
m_G_centr.block(0,cg*i,6,cg) = m_A.block<6,3>(0,3*i) * m_G.block(0,cg*i,3,cg);
// cout<<"G_centr:\n"<<m_G_centr.transpose()<<"\n";
- if(!computePolytopeProjection(m_G_centr))
- return false;
-
- m_HD = m_H * m_D;
- m_Hd = m_H * m_d;
+ if(m_algorithm==STATIC_EQUILIBRIUM_ALGORITHM_PP)
+ {
+ if(!computePolytopeProjection(m_G_centr))
+ return false;
+ m_HD = m_H * m_D;
+ m_Hd = m_H * m_d;
+ }
return true;
}
double StaticEquilibrium::computeEquilibriumRobustness(Cref_vector2 com)
{
- return 0.0;
+ /*Compute the robustness measure of the equilibrium of a specified CoM position.
+ The operation amounts to solving the following dual LP:
+ find v
+ minimize (d+D*com)' v
+ subject to G' v >= 0
+ 1' G' v = 1
+ where
+ -(d+D c)' v is the robustness measure
+ c is the CoM position
+ G is the matrix whose columns are the gravito-inertial wrench generators
+ */
+ if(m_algorithm!=STATIC_EQUILIBRIUM_ALGORITHM_LP)
+ {
+ SEND_ERROR_MSG("checkRobustEquilibrium is only implemented for the LP algorithm");
+ return false;
+ }
+
+ double robustness_dual, robustness_primal;
+ LP_status lpStatus_primal, lpStatus_dual;
+ long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+ Vector6 v;
+ VectorX b_b0(m+1);
+
+ {
+
+ Vector6 c = m_D*com + m_d;
+ 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_G_centr*VectorX::Ones(m)).transpose();
+
+ lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
+ if(lpStatus_dual==LP_STATUS_OPTIMAL)
+ robustness_dual = m_solver->getObjectiveValue();
+ else
+ SEND_ERROR_MSG("Dual LP problem could not be solved: "+toString(lpStatus_dual));
+ }
+
+ {
+ /* The operation amounts to solving the following LP:
+ find b, b0
+ minimize -b0
+ subject to D c + d <= G b <= D c + d
+ 0 <= b - b0 <= Inf
+ where
+ b are the coefficient of the contact force generators (f = V b)
+ b0 is the robustness measure
+ c is the CoM position
+ G is the matrix whose columns are the gravito-inertial wrench generators
+ */
+ long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+ VectorX c = VectorX::Zero(m+1);
+ c(m) = -1.0;
+ VectorX lb = -VectorX::Ones(m+1)*1e10;
+ VectorX ub = VectorX::Ones(m+1)*1e10;
+ VectorX Alb = VectorX::Zero(6+m);
+ VectorX Aub = VectorX::Ones(6+m)*1e100;
+ MatrixXX A = MatrixXX::Zero(6+m, m+1);
+ Alb.head<6>() = m_D * com + m_d;
+ Aub.head<6>() = Alb.head<6>();
+ A.topLeftCorner(6,m) = m_G_centr;
+ A.bottomLeftCorner(m,m) = MatrixXX::Identity(m,m);
+ A.bottomRightCorner(m,1) = -VectorX::Ones(m);
+
+ lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
+ if(lpStatus_primal==LP_STATUS_OPTIMAL)
+ robustness_primal = -1.0*m_solver->getObjectiveValue();
+ else
+ SEND_ERROR_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
+ }
+
+// if(lpStatus_primal==LP_STATUS_OPTIMAL && lpStatus_dual==LP_STATUS_OPTIMAL)
+// {
+// if(fabs(robustness_dual-robustness_primal)>1e-3)
+// SEND_ERROR_MSG("Primal and dual solutions differ: "+toString(robustness_primal)+" != "+
+// toString(robustness_dual)+" b="+toString(b_b0.transpose()));
+// }
+
+ return robustness_primal;
}
bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, double e_max)
{
if(e_max!=0.0)
{
- std::cerr<<"ERROR checkRobustEquilibrium with e_max!=0 not implemented yet!";
+ SEND_ERROR_MSG("checkRobustEquilibrium with e_max!=0 not implemented yet");
+ return false;
+ }
+ if(m_algorithm!=STATIC_EQUILIBRIUM_ALGORITHM_PP)
+ {
+ SEND_ERROR_MSG("checkRobustEquilibrium is only implemented for the PP algorithm");
return false;
}
@@ -113,11 +231,13 @@ bool StaticEquilibrium::checkRobustEquilibrium(Cref_vector2 com, double e_max)
double StaticEquilibrium::findExtremumOverLine(Cref_vector2 a, double b, double e_max)
{
+ SEND_ERROR_MSG("findExtremumOverLine not implemented yet");
return 0.0;
}
double StaticEquilibrium::findExtremumInDirection(Cref_vector2 direction, double e_max)
{
+ SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
return 0.0;
}
@@ -128,8 +248,7 @@ bool StaticEquilibrium::computePolytopeProjection(Cref_matrix6X v)
dd_PolyhedraPtr H_= dd_DDMatrix2Poly(V, &error);
if(error != dd_NoError)
{
-// if(dd_debug)
- cout << ("numerical instability in cddlib. ill formed polytope") << endl;
+ SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
return false;
}
@@ -142,7 +261,7 @@ bool StaticEquilibrium::computePolytopeProjection(Cref_matrix6X v)
eq_rows.push_back(elem);
}
int rowsize = (int)b_A->rowsize;
-// cout<<"Inequality matrix has "<<rowsize<<" rows and "<<b_A->colsize-1<<" columns\n";
+ SEND_DEBUG_MSG("Inequality matrix has "+toString(rowsize)+" rows and "+toString(b_A->colsize-1)+" columns");
m_H.resize(rowsize + eq_rows.size(), (int)b_A->colsize-1);
m_h.resize(rowsize + eq_rows.size());
for(int i=0; i < rowsize; ++i)
diff --git a/src/util.cpp b/src/util.cpp
index 27de0bf..fa1cede 100644
--- a/src/util.cpp
+++ b/src/util.cpp
@@ -1,74 +1,16 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
#ifndef _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
#define _ROBUST_EQUILIBRIUM_LIB_CONFIG_HH
-#include <Eigen/Dense>
-#include <Eigen/src/Core/util/Macros.h>
-
-#include "cdd/cddmp.h"
-#include "cdd/setoper.h"
-#include "cdd/cddtypes.h"
-#include "cdd/cdd.h"
+#include <robust-equilibrium-lib/util.hh>
namespace robust_equilibrium
{
-//#define USE_FLOAT 1;
-#ifdef USE_FLOAT
-typedef float value_type;
-#else
-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, 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, 3, 3, Eigen::RowMajor> Matrix3;
-typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3;
-typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
-typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXX;
-
-//define Eigen ref if available
-#if EIGEN_VERSION_AT_LEAST(3,2,0)
-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<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 MatrixXX> & Cref_matrixXX;
-#else
-typedef vector2_t & Ref_vector2;
-typedef vector3_t & Ref_vector3;
-typedef vector_t & Ref_vector;
-typedef rotation_t & Ref_rotation;
-typedef T_rotation_t& Ref_matrixX3;
-typedef T_rotation_t& Ref_matrix43;
-typedef matrix_t & Ref_matrixXX;
-
-typedef const vector2_t & Cref_vector2;
-typedef const vector3_t & Cref_vector3;
-typedef const vector_t & Cref_vector;
-typedef const rotation_t & Cref_rotation;
-typedef const T_rotation_t& Cref_matrixX3;
-typedef const T_rotation_t& Cref_matrix43;
-typedef const matrix_t & Cref_matrixXX;
-#endif
-
-typedef Eigen::AngleAxis<value_type> angle_axis_t;
-
-
dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input)
{
dd_debug = false;
@@ -101,12 +43,13 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input)
}
-void init_library()
+void init_cdd_library()
{
- dd_set_global_constants();dd_debug = false;
+ dd_set_global_constants();
+ dd_debug = false;
}
-void release_library()
+void release_cdd_library()
{
//dd_free_global_constants();
}
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index d52109d..c657967 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -4,6 +4,7 @@ include_directories("${SRC_DIR}")
include_directories("${INCLUDE_DIR}")
include_directories("${EIGEN3_INCLUDE_DIR}")
include_directories("${CDD_INCLUDE_DIR}")
+include_directories("${CLP_INCLUDE_DIR}")
PROJECT(robust-equilibrium-lib)
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
index d56f6b7..567c464 100644
--- a/test/test_LP_solvers.cpp
+++ b/test/test_LP_solvers.cpp
@@ -1,5 +1,425 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
+#include "ClpSimplex.hpp"
+#include "CoinTime.hpp"
+#include "CoinBuild.hpp"
+#include "CoinModel.hpp"
+
+#include <robust-equilibrium-lib/solver_LP_clp.hh>
+
+#include <iostream>
+#include <iomanip>
+
+using namespace std;
+using namespace robust_equilibrium;
+
+/** Example addRows.cpp */
+void test_addRows()
+{
+ const int N_ROWS_TO_ADD = 30000;
+ try
+ {
+ // Empty model
+ ClpSimplex model;
+
+ // Objective - just nonzeros
+ int objIndex[] = {0, 2};
+ double objValue[] = {1.0, 4.0};
+ // Upper bounds - as dense vector
+ double upper[] = {2.0, COIN_DBL_MAX, 4.0};
+
+ // Create space for 3 columns
+ model.resize(0, 3);
+ // Fill in
+ int i;
+ // Virtuous way
+ // First objective
+ for (i = 0; i < 2; i++)
+ model.setObjectiveCoefficient(objIndex[i], objValue[i]);
+ // Now bounds (lower will be zero by default but do again)
+ for (i = 0; i < 3; i++) {
+ model.setColumnLower(i, 0.0);
+ model.setColumnUpper(i, upper[i]);
+ }
+ /*
+ We could also have done in non-virtuous way e.g.
+ double * objective = model.objective();
+ and then set directly
+ */
+ // Faster to add rows all at once - but this is easier to show
+ // Now add row 1 as >= 2.0
+ int row1Index[] = {0, 2};
+ double row1Value[] = {1.0, 1.0};
+ model.addRow(2, row1Index, row1Value,
+ 2.0, COIN_DBL_MAX);
+ // Now add row 2 as == 1.0
+ int row2Index[] = {0, 1, 2};
+ double row2Value[] = {1.0, -5.0, 1.0};
+ model.addRow(3, row2Index, row2Value,
+ 1.0, 1.0);
+ // solve
+ model.dual();
+
+ /*
+ Adding one row at a time has a significant overhead so let's
+ try a more complicated but faster way
+
+ First time adding in 10000 rows one by one
+ */
+ model.allSlackBasis();
+ ClpSimplex modelSave = model;
+ double time1 = CoinCpuTime();
+ int k;
+ for (k = 0; k < N_ROWS_TO_ADD; k++) {
+ int row2Index[] = {0, 1, 2};
+ double row2Value[] = {1.0, -5.0, 1.0};
+ model.addRow(3, row2Index, row2Value,
+ 1.0, 1.0);
+ }
+ printf("Time for 10000 addRow is %g\n", CoinCpuTime() - time1);
+ model.dual();
+ model = modelSave;
+ // Now use build
+ CoinBuild buildObject;
+ time1 = CoinCpuTime();
+ for (k = 0; k < N_ROWS_TO_ADD; k++) {
+ int row2Index[] = {0, 1, 2};
+ double row2Value[] = {1.0, -5.0, 1.0};
+ buildObject.addRow(3, row2Index, row2Value,
+ 1.0, 1.0);
+ }
+ model.addRows(buildObject);
+ printf("Time for 10000 addRow using CoinBuild is %g\n", CoinCpuTime() - time1);
+ model.dual();
+ model = modelSave;
+ int del[] = {0, 1, 2};
+ model.deleteRows(2, del);
+ // Now use build +-1
+ CoinBuild buildObject2;
+ time1 = CoinCpuTime();
+ for (k = 0; k < N_ROWS_TO_ADD; k++) {
+ int row2Index[] = {0, 1, 2};
+ double row2Value[] = {1.0, -1.0, 1.0};
+ buildObject2.addRow(3, row2Index, row2Value,
+ 1.0, 1.0);
+ }
+ model.addRows(buildObject2, true);
+ printf("Time for 10000 addRow using CoinBuild+-1 is %g\n", CoinCpuTime() - time1);
+ model.dual();
+ model = modelSave;
+ model.deleteRows(2, del);
+ // Now use build +-1
+ CoinModel modelObject2;
+ time1 = CoinCpuTime();
+ for (k = 0; k < N_ROWS_TO_ADD; k++) {
+ int row2Index[] = {0, 1, 2};
+ double row2Value[] = {1.0, -1.0, 1.0};
+ modelObject2.addRow(3, row2Index, row2Value,
+ 1.0, 1.0);
+ }
+ model.addRows(modelObject2, true);
+ printf("Time for 10000 addRow using CoinModel+-1 is %g\n", CoinCpuTime() - time1);
+ model.dual();
+ model = ClpSimplex();
+ // Now use build +-1
+ CoinModel modelObject3;
+ time1 = CoinCpuTime();
+ for (k = 0; k < N_ROWS_TO_ADD; k++) {
+ int row2Index[] = {0, 1, 2};
+ double row2Value[] = {1.0, -1.0, 1.0};
+ modelObject3.addRow(3, row2Index, row2Value,
+ 1.0, 1.0);
+ }
+ model.loadProblem(modelObject3, true);
+ printf("Time for 10000 addRow using CoinModel load +-1 is %g\n", CoinCpuTime() - time1);
+ model.writeMps("xx.mps");
+ model.dual();
+ model = modelSave;
+ // Now use model
+ CoinModel modelObject;
+ time1 = CoinCpuTime();
+ for (k = 0; k < N_ROWS_TO_ADD; k++) {
+ int row2Index[] = {0, 1, 2};
+ double row2Value[] = {1.0, -5.0, 1.0};
+ modelObject.addRow(3, row2Index, row2Value,
+ 1.0, 1.0);
+ }
+ model.addRows(modelObject);
+ printf("Time for 10000 addRow using CoinModel is %g\n", CoinCpuTime() - time1);
+ model.dual();
+ model.writeMps("b.mps");
+ // Method using least memory - but most complicated
+ time1 = CoinCpuTime();
+ // Assumes we know exact size of model and matrix
+ // Empty model
+ ClpSimplex model2;
+ {
+ // Create space for 3 columns and 10000 rows
+ int numberRows = N_ROWS_TO_ADD;
+ int numberColumns = 3;
+ // This is fully dense - but would not normally be so
+ int numberElements = numberRows * numberColumns;
+ // Arrays will be set to default values
+ model2.resize(numberRows, numberColumns);
+ double * elements = new double [numberElements];
+ CoinBigIndex * starts = new CoinBigIndex [numberColumns+1];
+ int * rows = new int [numberElements];;
+ int * lengths = new int[numberColumns];
+ // Now fill in - totally unsafe but ....
+ // no need as defaults to 0.0 double * columnLower = model2.columnLower();
+ double * columnUpper = model2.columnUpper();
+ double * objective = model2.objective();
+ double * rowLower = model2.rowLower();
+ double * rowUpper = model2.rowUpper();
+ // Columns - objective was packed
+ for (k = 0; k < 2; k++) {
+ int iColumn = objIndex[k];
+ objective[iColumn] = objValue[k];
+ }
+ for (k = 0; k < numberColumns; k++)
+ columnUpper[k] = upper[k];
+ // Rows
+ for (k = 0; k < numberRows; k++) {
+ rowLower[k] = 1.0;
+ rowUpper[k] = 1.0;
+ }
+ // Now elements
+ double row2Value[] = {1.0, -5.0, 1.0};
+ CoinBigIndex put = 0;
+ for (k = 0; k < numberColumns; k++) {
+ starts[k] = put;
+ lengths[k] = numberRows;
+ double value = row2Value[k];
+ for (int i = 0; i < numberRows; i++) {
+ rows[put] = i;
+ elements[put] = value;
+ put++;
+ }
+ }
+ starts[numberColumns] = put;
+ // assign to matrix
+ CoinPackedMatrix * matrix = new CoinPackedMatrix(true, 0.0, 0.0);
+ matrix->assignMatrix(true, numberRows, numberColumns, numberElements,
+ elements, rows, starts, lengths);
+ ClpPackedMatrix * clpMatrix = new ClpPackedMatrix(matrix);
+ model2.replaceMatrix(clpMatrix, true);
+ printf("Time for 10000 addRow using hand written code is %g\n", CoinCpuTime() - time1);
+ // If matrix is really big could switch off creation of row copy
+ // model2.setSpecialOptions(256);
+ }
+ model2.dual();
+ model2.writeMps("a.mps");
+ // Print column solution
+ int numberColumns = model.numberColumns();
+
+ // Alternatively getColSolution()
+ double * columnPrimal = model.primalColumnSolution();
+ // Alternatively getReducedCost()
+ double * columnDual = model.dualColumnSolution();
+ // Alternatively getColLower()
+ double * columnLower = model.columnLower();
+ // Alternatively getColUpper()
+ double * columnUpper = model.columnUpper();
+ // Alternatively getObjCoefficients()
+ double * columnObjective = model.objective();
+
+ int iColumn;
+
+ std::cout << " Primal Dual Lower Upper Cost"
+ << std::endl;
+
+ for (iColumn = 0; iColumn < numberColumns; iColumn++) {
+ double value;
+ std::cout << std::setw(6) << iColumn << " ";
+ value = columnPrimal[iColumn];
+ if (fabs(value) < 1.0e5)
+ std::cout << setiosflags(std::ios::fixed | std::ios::showpoint) << std::setw(14) << value;
+ else
+ std::cout << setiosflags(std::ios::scientific) << std::setw(14) << value;
+ value = columnDual[iColumn];
+ if (fabs(value) < 1.0e5)
+ std::cout << setiosflags(std::ios::fixed | std::ios::showpoint) << std::setw(14) << value;
+ else
+ std::cout << setiosflags(std::ios::scientific) << std::setw(14) << value;
+ value = columnLower[iColumn];
+ if (fabs(value) < 1.0e5)
+ std::cout << setiosflags(std::ios::fixed | std::ios::showpoint) << std::setw(14) << value;
+ else
+ std::cout << setiosflags(std::ios::scientific) << std::setw(14) << value;
+ value = columnUpper[iColumn];
+ if (fabs(value) < 1.0e5)
+ std::cout << setiosflags(std::ios::fixed | std::ios::showpoint) << std::setw(14) << value;
+ else
+ std::cout << setiosflags(std::ios::scientific) << std::setw(14) << value;
+ value = columnObjective[iColumn];
+ if (fabs(value) < 1.0e5)
+ std::cout << setiosflags(std::ios::fixed | std::ios::showpoint) << std::setw(14) << value;
+ else
+ std::cout << setiosflags(std::ios::scientific) << std::setw(14) << value;
+
+ std::cout << std::endl;
+ }
+ std::cout << "--------------------------------------" << std::endl;
+ // Test CoinAssert
+ std::cout << "If Clp compiled without NDEBUG below should give assert, if with NDEBUG or COIN_ASSERT CoinError" << std::endl;
+ model = modelSave;
+ model.deleteRows(2, del);
+ // Deliberate error
+ model.deleteColumns(1, del + 2);
+ // Now use build +-1
+ CoinBuild buildObject3;
+ time1 = CoinCpuTime();
+ for (k = 0; k < N_ROWS_TO_ADD; k++) {
+ int row2Index[] = {0, 1, 2};
+ double row2Value[] = {1.0, -1.0, 1.0};
+ buildObject3.addRow(3, row2Index, row2Value,
+ 1.0, 1.0);
+ }
+ model.addRows(buildObject3, true);
+ } catch (CoinError e) {
+ e.print();
+ if (e.lineNumber() >= 0)
+ std::cout << "This was from a CoinAssert" << std::endl;
+ }
+}
+
+void test_small_LP()
+{
+ ClpSimplex model;
+// model.setLogLevel(1);
+
+ // Objective - just nonzeros
+ int objIndex[] = {0, 2};
+ double objValue[] = {1.0, 4.0};
+ // Upper bounds - as dense vector
+ double upper[] = {2.0, COIN_DBL_MAX, 4.0};
+
+ // Create space for 3 columns
+ model.resize(0, 3);
+
+ // Can also use maximumIterations
+ int integerValue;
+ model.getIntParam(ClpMaxNumIteration, integerValue);
+ cout << "Value of ClpMaxNumIteration is " << integerValue << endl;
+ model.setMaximumIterations(integerValue);
+
+ // Fill in
+ int i;
+ // Virtuous way
+ // First objective
+ for (i = 0; i < 2; i++)
+ model.setObjectiveCoefficient(objIndex[i], objValue[i]);
+ // Now bounds (lower will be zero by default but do again)
+ for (i = 0; i < 3; i++) {
+ model.setColumnLower(i, 0.0);
+ model.setColumnUpper(i, upper[i]);
+ }
+ /*
+ We could also have done in non-virtuous way e.g.
+ double * objective = model.objective();
+ and then set directly
+ */
+ // Faster to add rows all at once - but this is easier to show
+ // Now add row 1 as >= 2.0
+ int row1Index[] = {0, 2};
+ double row1Value[] = {1.0, 1.0};
+ model.addRow(2, row1Index, row1Value,
+ 2.0, COIN_DBL_MAX);
+ // Now add row 2 as == 1.0
+ int row2Index[] = {0, 1, 2};
+ double row2Value[] = {1.0, -5.0, 1.0};
+ model.addRow(3, row2Index, row2Value,
+ 1.0, 1.0);
+
+ int n = model.getNumCols();
+ int m = model.getNumRows();
+ cout<<"Problem has "<<n<<" variables and "<<m<<" constraints.\n";
+
+ // solve
+ model.dual();
+
+ // Check the solution
+ if ( model.isProvenOptimal() )
+ {
+ cout << "Found optimal solution!" << endl;
+ cout << "Objective value is " << model.getObjValue() << endl;
+ cout << "Model status is " << model.status() << " after "
+ << model.numberIterations() << " iterations - objective is "
+ << model.objectiveValue() << endl;
+ const double *solution;
+ solution = model.getColSolution();
+ // We could then print the solution or examine it.
+ cout<<"Solution is: ";
+ for(int i=0; i<n; i++)
+ cout<<solution[i]<<", ";
+ cout<<endl;
+ }
+ else
+ cout << "Didn’t find optimal solution." << endl;
+}
+
int main()
{
- int ret = 0;
- return ret;
+ cout <<"Test LP Solvers\n";
+
+// test_addRows();
+ test_small_LP();
+
+ Solver_LP_abstract *solver = Solver_LP_abstract::getNewSolver(SOLVER_LP_CLP);
+ Vector3 c, lb, ub, x;
+ MatrixXX A(2,3);
+ Vector2 Alb, Aub;
+ c << 1.0, 0.0, 4.0;
+ lb << 0.0, 0.0, 0.0;
+ ub << 2.0, COIN_DBL_MAX, 4.0;
+ A << 1.0, 0.0, 1.0,
+ 1.0, -5.0, 1.0;
+ Alb << 2.0, 1.0;
+ Aub << COIN_DBL_MAX, 1.0;
+ if(solver->solve(c, lb, ub, A, Alb, Aub, x)==LP_STATUS_OPTIMAL)
+ {
+ cout<<"solver_LP_clp solved the problem\n";
+ cout<<"The solution is "<<x.transpose()<<endl;
+ }
+ else
+ cout<<"solver_LP_clp failed to solve the problem\n";
+
+// 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;
}
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 1dfaa63..ee02e8c 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -1,6 +1,12 @@
+/*
+ * Copyright 2015, LAAS-CNRS
+ * Author: Andrea Del Prete
+ */
+
#include <vector>
#include <iostream>
#include <robust-equilibrium-lib/static_equilibrium.hh>
+#include <robust-equilibrium-lib/logger.hh>
using namespace robust_equilibrium;
using namespace Eigen;
@@ -10,7 +16,7 @@ using namespace std;
int main()
{
- init_library();
+// init_cdd_library();
srand ((unsigned int)(time(NULL)));
int ret = 0;
@@ -31,9 +37,10 @@ int main()
RPY_UPPER_BOUNDS << +0.0*gamma, +0.0*gamma, +M_PI;
double X_MARG = 0.07;
double Y_MARG = 0.07;
- const int GRID_SIZE = 25;
+ const int GRID_SIZE = 20;
- StaticEquilibrium solver(mass, generatorsPerContact, SOLVER_LP_QPOASES);
+ StaticEquilibrium solver_PP(mass, generatorsPerContact, SOLVER_LP_CLP);
+ StaticEquilibrium solver_LP(mass, generatorsPerContact, SOLVER_LP_CLP);
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
@@ -50,21 +57,21 @@ int main()
bool collision;
for(unsigned int i=0; i<N_CONTACTS; i++)
{
-// while(true) // generate contact position
-// {
-// uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, contact_pos.row(i));
-// if(i==0)
-// break;
-// collision = false;
-// for(unsigned int j=0; j<i-1; j++)
-// if((contact_pos.row(i)-contact_pos.row(j)).norm() < MIN_FEET_DISTANCE)
-// collision = true;
-// if(collision==false)
-// break;
-// }
-
-// // generate contact orientation
-// uniform(RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, contact_rpy.row(i));
+ while(true) // generate contact position
+ {
+ uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, contact_pos.row(i));
+ if(i==0)
+ break;
+ collision = false;
+ for(unsigned int j=0; j<i-1; j++)
+ if((contact_pos.row(i)-contact_pos.row(j)).norm() < MIN_FEET_DISTANCE)
+ collision = true;
+ if(collision==false)
+ break;
+ }
+
+ // 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));
@@ -79,7 +86,13 @@ int main()
printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
}
- if(!solver.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_PP))
+ if(!solver_LP.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_LP))
+ {
+ printf("Error while setting new contacts");
+ return -1;
+ }
+
+ if(!solver_PP.setNewContacts(p, N, frictionCoefficients, STATIC_EQUILIBRIUM_ALGORITHM_PP))
{
printf("Error while setting new contacts");
return -1;
@@ -108,14 +121,21 @@ int main()
{
// uniform(com_LB, com_UB, com);
com(0) = x_range(j);
- if(solver.checkRobustEquilibrium(com, 0.0))
+ double rob = solver_LP.computeEquilibriumRobustness(com);
+ if(solver_PP.checkRobustEquilibrium(com, 0.0))
{
equilibrium(i,j) = 1.0;
- printf("1 ");
+ if(rob<=0)
+ SEND_ERROR_MSG("PP says com is in equilibrium but LP find negative robustness "+toString(rob));
+ if(rob>9.0)
+ rob = 9.0;
+ printf("%d ", (int)rob);
}
else
{
equilibrium(i,j) = 0.0;
+ if(rob>0)
+ SEND_ERROR_MSG("PP says com is NOT in equilibrium but LP find positive robustness "+toString(rob));
printf("- ");
}
--
GitLab