Skip to content
GitLab
Unverified Commit 0d56f35f authored by Guilhem Saurel's avatar Guilhem Saurel Committed by GitHub
Browse files

Revert "Cleaning"

parent 3a6e65b3
Expand all Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 0d56f35f
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
cmake_minimum_required(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/test.cmake)
......@@ -7,49 +7,60 @@ INCLUDE(cmake/hpp.cmake)
SET(PROJECT_NAME hpp-centroidal-dynamics)
SET(PROJECT_DESCRIPTION
"Utility classes for testing (robust) equilibrium of a system in contact with the environment, and other centroidal dynamics methods."
)
"Utility classes for testing (robust) equilibrium of a system in contact with the environment, and other centroidal dynamics methods."
)
SET(CUSTOM_HEADER_DIR "hpp/centroidal-dynamics")
SETUP_HPP_PROJECT()
OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
# Inhibit all warning messages.
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")
# remove flag that makes all warnings into errors
string (REPLACE "-Werror" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS} )
OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
FINDPYTHON()
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
# search for python
FINDPYTHON(2.7 EXACT REQUIRED)
find_package( PythonLibs 2.7 REQUIRED )
include_directories( ${PYTHON_INCLUDE_DIRS} )
ADD_REQUIRED_DEPENDENCY("eigenpy >= 1.4.0")
find_package( Boost COMPONENTS python REQUIRED )
include_directories( ${Boost_INCLUDE_DIR} )
FIND_PACKAGE(Boost COMPONENTS python REQUIRED )
INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIR})
ENDIF(BUILD_PYTHON_INTERFACE)
ADD_REQUIRED_DEPENDENCY("eigen3")
INCLUDE_DIRECTORIES(SYSTEM ${EIGEN3_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
set(CMAKE_MODULE_PATH
${PROJECT_SOURCE_DIR}/cmake/find-external/CDD
${PROJECT_SOURCE_DIR}/cmake/find-external/CLP
${PROJECT_SOURCE_DIR}/cmake/find-external/qpOASES
)
SET(CMAKE_MODULE_PATH
${PROJECT_SOURCE_DIR}/cmake/find-external/CDD
${PROJECT_SOURCE_DIR}/cmake/find-external/CLP
${PROJECT_SOURCE_DIR}/cmake/find-external/qpOASES
)
find_package(CDD REQUIRED)
find_package(qpOASES REQUIRED)
find_package(CLP)
FIND_PACKAGE(CDD REQUIRED)
FIND_PACKAGE(qpOASES REQUIRED)
FIND_PACKAGE(CLP)
IF("${CLP_LIBRARY}" STREQUAL "CLP_LIBRARY-NOTFOUND")
message(STATUS "CLP_LIBRARY equal to CLP_LIBRARY-NOTFOUND so I assume CLP was not found ")
else()
message(STATUS "CLP library found, defining macro CLP_FOUND")
add_definitions(-DCLP_FOUND)
endif()
IF(CLP_FOUND)
ADD_DEFINITIONS(-DCLP_FOUND)
INCLUDE_DIRECTORIES("${CLP_INCLUDE_DIR}")
ENDIF()
INCLUDE_DIRECTORIES(${qpOASES_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(SYSTEM ${qpOASES_INCLUDE_DIRS})
ADD_SUBDIRECTORY(include/${CUSTOM_HEADER_DIR})
ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(test)
ADD_SUBDIRECTORY (include/${CUSTOM_HEADER_DIR})
add_subdirectory (src)
add_subdirectory (test)
IF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(python)
add_subdirectory (python)
ENDIF(BUILD_PYTHON_INTERFACE)
SETUP_PROJECT_FINALIZE()
cmake @ 5c8c19f4
Compare f34901f1...5c8c19f4
Subproject commit f34901f143d843b48dfdb8d9e904503ed96e2310
Subproject commit 5c8c19f491f2c6f8488f5f37ff81d711d69dbb3f
include/hpp/centroidal-dynamics/CMakeLists.txt
View file @ 0d56f35f
# Declare Headers
SET(${PROJECT_NAME}_HEADERS
local_config.hh
util.hh
......@@ -11,5 +12,5 @@ SET(${PROJECT_NAME}_HEADERS
INSTALL(FILES
${${PROJECT_NAME}_HEADERS}
DESTINATION include/${CUSTOM_HEADER_DIR}
DESTINATION include/hpp/centroidal-dynamics
)
include/hpp/centroidal-dynamics/centroidal_dynamics.hh
View file @ 0d56f35f
......@@ -11,33 +11,37 @@
#include <hpp/centroidal-dynamics/util.hh>
#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
namespace centroidal_dynamics {
enum CENTROIDAL_DYNAMICS_DLLAPI EquilibriumAlgorithm {
EQUILIBRIUM_ALGORITHM_LP, /// primal LP formulation
EQUILIBRIUM_ALGORITHM_LP2, /// another primal LP formulation
EQUILIBRIUM_ALGORITHM_DLP, /// dual LP formulation
EQUILIBRIUM_ALGORITHM_PP, /// polytope projection algorithm
EQUILIBRIUM_ALGORITHM_IP, /// incremental projection algorithm based on primal LP formulation
EQUILIBRIUM_ALGORITHM_DIP /// incremental projection algorithm based on dual LP formulation
namespace centroidal_dynamics
{
enum CENTROIDAL_DYNAMICS_DLLAPI EquilibriumAlgorithm
{
EQUILIBRIUM_ALGORITHM_LP, /// primal LP formulation
EQUILIBRIUM_ALGORITHM_LP2, /// another primal LP formulation
EQUILIBRIUM_ALGORITHM_DLP, /// dual LP formulation
EQUILIBRIUM_ALGORITHM_PP, /// polytope projection algorithm
EQUILIBRIUM_ALGORITHM_IP, /// incremental projection algorithm based on primal LP formulation
EQUILIBRIUM_ALGORITHM_DIP /// incremental projection algorithm based on dual LP formulation
};
class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
public:
const double m_mass; /// mass of the system
const Vector3 m_gravity; /// gravity vector
class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium
{
public:
const double m_mass; /// mass of the system
const Vector3 m_gravity; /// gravity vector
/** Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact) */
Matrix6X m_G_centr;
private:
static bool m_is_cdd_initialized; /// true if cdd lib has been initialized, false otherwise
private:
static bool m_is_cdd_initialized; /// true if cdd lib has been initialized, false otherwise
std::string m_name; /// name of this object
EquilibriumAlgorithm m_algorithm; /// current algorithm used
SolverLP m_solver_type; /// type of LP solver
Solver_LP_abstract* m_solver; /// LP solver
std::string m_name; /// name of this object
EquilibriumAlgorithm m_algorithm; /// current algorithm used
SolverLP m_solver_type; /// type of LP solver
Solver_LP_abstract* m_solver; /// LP solver
unsigned int m_generatorsPerContact; /// number of generators to approximate the friction cone per contact point
unsigned int m_generatorsPerContact; /// number of generators to approximate the friction cone per contact point
/** Inequality matrix and vector defining the gravito-inertial wrench cone H w <= h */
MatrixXX m_H;
......@@ -45,15 +49,15 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
/** False if a numerical instability appeared in the computation H and h*/
bool m_is_cdd_stable;
/** EQUILIBRIUM_ALGORITHM_PP: If double description fails,
* indicate the max number of attempts to compute the cone by introducing
* a small pertubation of the system */
* indicate the max number of attempts to compute the cone by introducing
* a small pertubation of the system */
const unsigned max_num_cdd_trials;
/** whether to remove redundant inequalities when computing double description matrices*/
const bool canonicalize_cdd_matrix;
/** Inequality matrix and vector defining the CoM support polygon HD com + Hd <= h */
MatrixX3 m_HD;
VectorX m_Hd;
VectorX m_Hd;
/** Matrix and vector mapping 2d com position to GIW */
Matrix63 m_D;
......@@ -63,8 +67,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
double m_b0_to_emax_coefficient;
bool computePolytopeProjection(Cref_matrix6X v);
bool computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals, double frictionCoefficient,
const bool perturbate = false);
bool computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
double frictionCoefficient, const bool perturbate = false);
/**
* @brief Given the smallest coefficient of the contact force generators it computes
......@@ -78,8 +82,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
double convert_emax_to_b0(double emax);
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* @brief Equilibrium constructor.
* @param name Name of the object.
......@@ -92,8 +96,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* a small pertubation of the system
*/
Equilibrium(const std::string& name, const double mass, const unsigned int generatorsPerContact,
const SolverLP solver_type = SOLVER_LP_QPOASES, bool useWarmStart = true,
const unsigned int max_num_cdd_trials = 0, const bool canonicalize_cdd_matrix = false);
const SolverLP solver_type = SOLVER_LP_QPOASES, bool useWarmStart=true, const unsigned int max_num_cdd_trials=0,
const bool canonicalize_cdd_matrix=false);
Equilibrium(const Equilibrium& other);
......@@ -101,21 +106,21 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @brief Returns the useWarmStart flag.
* @return True if the LP solver is allowed to use warm start, false otherwise.
*/
bool useWarmStart() { return m_solver->getUseWarmStart(); }
bool useWarmStart(){ return m_solver->getUseWarmStart(); }
/**
* @brief Specifies whether the LP solver is allowed to use warm start.
* @param uws True if the LP solver is allowed to use warm start, false otherwise.
*/
void setUseWarmStart(bool uws) { m_solver->setUseWarmStart(uws); }
void setUseWarmStart(bool uws){ m_solver->setUseWarmStart(uws); }
/**
* @brief Get the name of this object.
* @return The name of this object.
*/
std::string getName() { return m_name; }
std::string getName(){ return m_name; }
EquilibriumAlgorithm getAlgorithm() { return m_algorithm; }
EquilibriumAlgorithm getAlgorithm(){ return m_algorithm; }
void setAlgorithm(EquilibriumAlgorithm algorithm);
......@@ -130,8 +135,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @param alg Algorithm to use for testing equilibrium.
* @return True if the operation succeeded, false otherwise.
*/
bool setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals, const double frictionCoefficient,
const EquilibriumAlgorithm alg);
bool setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg);
/**
* @brief Specify a new set of contacts.
......@@ -144,10 +149,10 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @param alg Algorithm to use for testing equilibrium.
* @return True if the operation succeeded, false otherwise.
*/
bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg);
void setG(Cref_matrix6X G) { m_G_centr = G; }
void setG(Cref_matrix6X G){m_G_centr = G;}
/**
* @brief Compute a measure of the robustness of the equilibrium of the specified com position.
......@@ -170,7 +175,7 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* system can reach infinite robustness. This is due to the fact that we are not considering
* any upper limit for the friction cones.
*/
LP_status computeEquilibriumRobustness(Cref_vector3 com, double& robustness);
LP_status computeEquilibriumRobustness(Cref_vector3 com, double &robustness);
/**
* @brief Compute a measure of the robustness of the equilibrium of the specified com position.
......@@ -194,7 +199,7 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* system can reach infinite robustness. This is due to the fact that we are not considering
* any upper limit for the friction cones.
*/
LP_status computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double& robustness);
LP_status computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness);
/**
* @brief Check whether the specified com position is in robust equilibrium.
......@@ -215,7 +220,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @param e_max Desired robustness level.
* @return The status of the LP solver.
*/
LP_status checkRobustEquilibrium(Cref_vector3 com, bool& equilibrium, double e_max = 0.0);
LP_status checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max=0.0);
/**
* @brief Check whether the specified com position is in robust equilibrium.
......@@ -237,7 +243,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @param e_max Desired robustness level.
* @return The status of the LP solver.
*/
LP_status checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool& equilibrium, double e_max = 0.0);
LP_status checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max=0.0);
/**
* @brief Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
......@@ -260,7 +267,7 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @note If the system is in force closure the status will be LP_STATUS_UNBOUNDED, meaning that the
* system can reach infinite robustness. This is due to the fact that we are not considering
* any upper limit for the friction cones.
*/
*/
LP_status findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com);
/**
......@@ -286,7 +293,7 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* system can reach infinite robustness. This is due to the fact that we are not considering
* any upper limit for the friction cones.
*/
LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max = 0.0);
LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max=0.0);
/**
* @brief Retrieve the inequalities that define the admissible wrenchs
......@@ -336,9 +343,10 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @param a the acceleration
* @return true if the acceleration is admissible, false otherwise
*/
bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a);
bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a );
};
} // end namespace centroidal_dynamics
} // end namespace centroidal_dynamics
#endif
include/hpp/centroidal-dynamics/local_config.hh
View file @ 0d56f35f
......@@ -7,7 +7,7 @@
#define _CENTROIDAL_DYNAMICS_LIB_CONFIG_HH
// Package version (header).
#define CENTROIDAL_DYNAMICS_VERSION "UNKNOWN"
# define CENTROIDAL_DYNAMICS_VERSION "UNKNOWN"
// Handle portable symbol export.
// Defining manually which symbol should be exported is required
......@@ -19,39 +19,39 @@
//
// On Linux, set the visibility accordingly. If C++ symbol visibility
// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility
#if defined _WIN32 || defined __CYGWIN__
# if defined _WIN32 || defined __CYGWIN__
// On Microsoft Windows, use dllimport and dllexport to tag symbols.
#define CENTROIDAL_DYNAMICS_DLLIMPORT __declspec(dllimport)
#define CENTROIDAL_DYNAMICS_DLLEXPORT __declspec(dllexport)
#define CENTROIDAL_DYNAMICS_DLLLOCAL
#else
# define CENTROIDAL_DYNAMICS_DLLIMPORT __declspec(dllimport)
# define CENTROIDAL_DYNAMICS_DLLEXPORT __declspec(dllexport)
# define CENTROIDAL_DYNAMICS_DLLLOCAL
# else
// On Linux, for GCC >= 4, tag symbols using GCC extension.
#if __GNUC__ >= 4
#define CENTROIDAL_DYNAMICS_DLLIMPORT __attribute__((visibility("default")))
#define CENTROIDAL_DYNAMICS_DLLEXPORT __attribute__((visibility("default")))
#define CENTROIDAL_DYNAMICS_DLLLOCAL __attribute__((visibility("hidden")))
#else
# if __GNUC__ >= 4
# define CENTROIDAL_DYNAMICS_DLLIMPORT __attribute__ ((visibility("default")))
# define CENTROIDAL_DYNAMICS_DLLEXPORT __attribute__ ((visibility("default")))
# define CENTROIDAL_DYNAMICS_DLLLOCAL __attribute__ ((visibility("hidden")))
# else
// Otherwise (GCC < 4 or another compiler is used), export everything.
#define CENTROIDAL_DYNAMICS_DLLIMPORT
#define CENTROIDAL_DYNAMICS_DLLEXPORT
#define CENTROIDAL_DYNAMICS_DLLLOCAL
#endif // __GNUC__ >= 4
#endif // defined _WIN32 || defined __CYGWIN__
# define CENTROIDAL_DYNAMICS_DLLIMPORT
# define CENTROIDAL_DYNAMICS_DLLEXPORT
# define CENTROIDAL_DYNAMICS_DLLLOCAL
# endif // __GNUC__ >= 4
# endif // defined _WIN32 || defined __CYGWIN__
#ifdef CENTROIDAL_DYNAMICS_STATIC
# ifdef CENTROIDAL_DYNAMICS_STATIC
// If one is using the library statically, get rid of
// extra information.
#define CENTROIDAL_DYNAMICS_DLLAPI
#define CENTROIDAL_DYNAMICS_LOCAL
#else
# define CENTROIDAL_DYNAMICS_DLLAPI
# define CENTROIDAL_DYNAMICS_LOCAL
# else
// Depending on whether one is building or using the
// library define DLLAPI to import or export.
#ifdef CENTROIDAL_DYNAMICS_EXPORTS
#define CENTROIDAL_DYNAMICS_DLLAPI CENTROIDAL_DYNAMICS_DLLEXPORT
#else
#define CENTROIDAL_DYNAMICS_DLLAPI CENTROIDAL_DYNAMICS_DLLIMPORT
#endif // CENTROIDAL_DYNAMICS_EXPORTS
#define CENTROIDAL_DYNAMICS_LOCAL CENTROIDAL_DYNAMICS_DLLLOCAL
#endif // CENTROIDAL_DYNAMICS_STATIC
# ifdef CENTROIDAL_DYNAMICS_EXPORTS
# define CENTROIDAL_DYNAMICS_DLLAPI CENTROIDAL_DYNAMICS_DLLEXPORT
# else
# define CENTROIDAL_DYNAMICS_DLLAPI CENTROIDAL_DYNAMICS_DLLIMPORT
# endif // CENTROIDAL_DYNAMICS_EXPORTS
# define CENTROIDAL_DYNAMICS_LOCAL CENTROIDAL_DYNAMICS_DLLLOCAL
# endif // CENTROIDAL_DYNAMICS_STATIC
#endif //_CENTROIDAL_DYNAMICS_LIB_CONFIG_HH
#endif //_CENTROIDAL_DYNAMICS_LIB_CONFIG_HH
include/hpp/centroidal-dynamics/logger.hh
View file @ 0d56f35f
......@@ -16,7 +16,8 @@
#include <map>
#include "boost/assign.hpp"
namespace centroidal_dynamics {
namespace centroidal_dynamics
{
//#define LOGGER_VERBOSITY_ERROR
//#define LOGGER_VERBOSITY_WARNING_ERROR
......@@ -24,152 +25,164 @@ namespace centroidal_dynamics {
//#define LOGGER_VERBOSITY_ALL
#define LOGGER_VERBOSITY_ALL
#define SEND_MSG(msg, type) getLogger().sendMsg(msg, type, __FILE__, __LINE__)
#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_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)
#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_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)
#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_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)
#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)
#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 CENTROIDAL_DYNAMICS_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();
}
template <typename T, int n>
std::string toString(const Eigen::MatrixBase<T>& v, const std::string separator = ", ") {
if (v.rows() > v.cols()) return toString(v.transpose(), separator);
std::stringstream ss;
ss << v;
return ss.str();
}
enum CENTROIDAL_DYNAMICS_DLLAPI LoggerVerbosity {
VERBOSITY_ALL,
VERBOSITY_INFO_WARNING_ERROR,
VERBOSITY_WARNING_ERROR,
VERBOSITY_ERROR,
VERBOSITY_NONE
};
/** A simple class for logging messages
*/
class CENTROIDAL_DYNAMICS_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;
/** Enum representing the different kind of messages.
*/
enum CENTROIDAL_DYNAMICS_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();
}
bool isDebugMsg(MsgType m) { return m == MSG_TYPE_DEBUG_STREAM || m == MSG_TYPE_DEBUG; }
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();
}