Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Humanoid Path Planner
hpp-centroidal-dynamics
Commits
0d56f35f
Unverified
Commit
0d56f35f
authored
Mar 25, 2019
by
Guilhem Saurel
Committed by
GitHub
Mar 25, 2019
Browse files
Revert "Cleaning"
parent
3a6e65b3
Changes
27
Expand all
Hide whitespace changes
Inline
Side-by-side
.yapfignore
deleted
100644 → 0
View file @
3a6e65b3
cmake
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 f
3
49
0
1f
143d843b48dfdb8d9e904503ed96e2310
Subproject commit
5c8c19
f491f
2c6f8488f5f37ff81d711d69dbb3f
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
();
}