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
5e706799
Commit
5e706799
authored
Sep 24, 2019
by
Guilhem Saurel
Browse files
[Format]
parent
d6d07d21
Changes
18
Expand all
Hide whitespace changes
Inline
Side-by-side
include/hpp/centroidal-dynamics/centroidal_dynamics.hh
View file @
5e706799
...
...
@@ -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
...
...
@@ -345,10 +338,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
View file @
5e706799
...
...
@@ -7,7 +7,7 @@
#define HPP_CENTROIDAL_DYNAMICS_LOCAL_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 // HPP_CENTROIDAL_DYNAMICS_LOCAL_CONFIG_HH
include/hpp/centroidal-dynamics/logger.hh
View file @
5e706799
...
...
@@ -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
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
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
;
}
bool
isDebugMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_DEBUG_STREAM
||
m
==
MSG_TYPE_DEBUG
;
}
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
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
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
;
}
};
bool
isErrorMsg
(
MsgType
m
)
{
return
m
==
MSG_TYPE_ERROR_STREAM
||
m
==
MSG_TYPE_ERROR
;
}
};
/** Method to get the logger (singleton). */
Logger
&
getLogger
();
/** Method to get the logger (singleton). */
Logger
&
getLogger
();
}
// namespace centroidal_dynamics
}
// namespace centroidal_dynamics
#endif // HPP_CENTROIDAL_DYNAMICS_LOGGER_HH
include/hpp/centroidal-dynamics/solver_LP_abstract.hh
View file @
5e706799
...
...
@@ -10,49 +10,42 @@
#include
<hpp/centroidal-dynamics/local_config.hh>
#include
<hpp/centroidal-dynamics/util.hh>
namespace
centroidal_dynamics
{
namespace
centroidal_dynamics
{
/**
* Available LP solvers.
*/
enum
CENTROIDAL_DYNAMICS_DLLAPI
SolverLP
{
* Available LP solvers.
*/
enum
CENTROIDAL_DYNAMICS_DLLAPI
SolverLP
{