Merge pull request #4 from nim65s/devel

Cleaning

.yapfignore

+cmake

CMakeLists.txt

-cmake_minimum_required(VERSION 2.6)
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
 INCLUDE(cmake/base.cmake)
 INCLUDE(cmake/test.cmake)
@@ -7,60 +7,49 @@ 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()
 
-# 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)
+OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
 IF(BUILD_PYTHON_INTERFACE)
-# search for python
-	FINDPYTHON(2.7 EXACT REQUIRED)
-	find_package( PythonLibs 2.7 REQUIRED )
-	include_directories( ${PYTHON_INCLUDE_DIRS} )
+    FINDPYTHON()
+    INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
 
-	find_package( Boost COMPONENTS python REQUIRED )
-	include_directories( ${Boost_INCLUDE_DIR} )
+    ADD_REQUIRED_DEPENDENCY("eigenpy >= 1.4.0")
 
+    FIND_PACKAGE(Boost COMPONENTS python REQUIRED )
+    INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIR})
 ENDIF(BUILD_PYTHON_INTERFACE)
 
 ADD_REQUIRED_DEPENDENCY("eigen3")
-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
-  )
+INCLUDE_DIRECTORIES(SYSTEM ${EIGEN3_INCLUDE_DIRS})
 
-find_package(CDD REQUIRED)
-find_package(qpOASES REQUIRED)
-find_package(CLP)
+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
+    )
 
-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()
+FIND_PACKAGE(CDD REQUIRED)
+FIND_PACKAGE(qpOASES REQUIRED)
+FIND_PACKAGE(CLP)
 
-INCLUDE_DIRECTORIES(${qpOASES_INCLUDE_DIRS})
+IF(CLP_FOUND)
+    ADD_DEFINITIONS(-DCLP_FOUND)
+    INCLUDE_DIRECTORIES("${CLP_INCLUDE_DIR}")
+ENDIF()
 
+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 @ f34901f1

-Subproject commit 5c8c19f491f2c6f8488f5f37ff81d711d69dbb3f
+Subproject commit f34901f143d843b48dfdb8d9e904503ed96e2310

include/hpp/centroidal-dynamics/CMakeLists.txt

-# Declare Headers
 SET(${PROJECT_NAME}_HEADERS
   local_config.hh
   util.hh
@@ -12,5 +11,5 @@ SET(${PROJECT_NAME}_HEADERS
 
 INSTALL(FILES
   ${${PROJECT_NAME}_HEADERS}
-  DESTINATION include/hpp/centroidal-dynamics
+  DESTINATION include/${CUSTOM_HEADER_DIR}
   )

include/hpp/centroidal-dynamics/centroidal_dynamics.hh

@@ -11,37 +11,33 @@
 #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
-
-  unsigned int  m_generatorsPerContact; /// number of generators to approximate the friction cone per contact point
+  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
 
   /** Inequality matrix and vector defining the gravito-inertial wrench cone H w <= h */
   MatrixXX m_H;
@@ -49,15 +45,15 @@ private:
   /** 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;
@@ -67,8 +63,8 @@ private:
   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
@@ -82,8 +78,8 @@ private:
 
   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.
@@ -96,9 +92,8 @@ public:
    * 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);
 
@@ -106,21 +101,21 @@ public:
    * @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);
 
@@ -135,8 +130,8 @@ public:
    * @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.
@@ -149,10 +144,10 @@ public:
    * @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.
@@ -175,7 +170,7 @@ public:
    * 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.
@@ -199,7 +194,7 @@ public:
    * 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.
@@ -220,8 +215,7 @@ public:
    * @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.
@@ -243,8 +237,7 @@ public:
    * @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.
@@ -267,7 +260,7 @@ public:
    * @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);
 
   /**
@@ -293,7 +286,7 @@ public:
    * 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
@@ -343,10 +336,9 @@ public:
    * @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

@@ -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

@@ -16,8 +16,7 @@
 #include <map>
 #include "boost/assign.hpp"
 
-namespace centroidal_dynamics
-{
+namespace centroidal_dynamics {
 
 //#define LOGGER_VERBOSITY_ERROR
 //#define LOGGER_VERBOSITY_WARNING_ERROR
@@ -25,164 +24,152 @@ 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 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