From 5e70679927a3d4b9348ed412471eee4322787af4 Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Tue, 24 Sep 2019 16:46:27 +0200
Subject: [PATCH] [Format]
---
.../centroidal_dynamics.hh | 98 ++-
.../hpp/centroidal-dynamics/local_config.hh | 54 +-
include/hpp/centroidal-dynamics/logger.hh | 247 ++++---
.../centroidal-dynamics/solver_LP_abstract.hh | 83 +--
.../hpp/centroidal-dynamics/solver_LP_clp.hh | 22 +-
.../centroidal-dynamics/solver_LP_qpoases.hh | 33 +-
include/hpp/centroidal-dynamics/stop-watch.hh | 65 +-
include/hpp/centroidal-dynamics/util.hh | 230 +++---
python/centroidal_dynamics_python.cpp | 189 +++--
src/centroidal_dynamics.cpp | 662 ++++++++----------
src/logger.cpp | 146 ++--
src/solver_LP_abstract.cpp | 97 ++-
src/solver_LP_clp.cpp | 80 +--
src/solver_LP_qpoases.cpp | 118 ++--
src/stop-watch.cpp | 177 ++---
src/util.cpp | 184 +++--
test/test_LP_solvers.cpp | 277 ++++----
test/test_static_equilibrium.cpp | 467 ++++++------
18 files changed, 1416 insertions(+), 1813 deletions(-)
diff --git a/include/hpp/centroidal-dynamics/centroidal_dynamics.hh b/include/hpp/centroidal-dynamics/centroidal_dynamics.hh
index 5f4f90d..dec7805 100644
--- a/include/hpp/centroidal-dynamics/centroidal_dynamics.hh
+++ b/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
@@ -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
diff --git a/include/hpp/centroidal-dynamics/local_config.hh b/include/hpp/centroidal-dynamics/local_config.hh
index 0c6eee3..8cb7192 100644
--- a/include/hpp/centroidal-dynamics/local_config.hh
+++ b/include/hpp/centroidal-dynamics/local_config.hh
@@ -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
diff --git a/include/hpp/centroidal-dynamics/logger.hh b/include/hpp/centroidal-dynamics/logger.hh
index 548409a..84c85b2 100644
--- a/include/hpp/centroidal-dynamics/logger.hh
+++ b/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
+ 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
diff --git a/include/hpp/centroidal-dynamics/solver_LP_abstract.hh b/include/hpp/centroidal-dynamics/solver_LP_abstract.hh
index 73c8591..11b1f49 100644
--- a/include/hpp/centroidal-dynamics/solver_LP_abstract.hh
+++ b/include/hpp/centroidal-dynamics/solver_LP_abstract.hh
@@ -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 {
SOLVER_LP_QPOASES = 0
#ifdef CLP_FOUND
- ,SOLVER_LP_CLP = 1
+ ,
+ SOLVER_LP_CLP = 1
#endif
};
-
/**
- * Possible states of an LP solver.
- */
-enum CENTROIDAL_DYNAMICS_DLLAPI LP_status
-{
- LP_STATUS_UNKNOWN=-1,
- LP_STATUS_OPTIMAL=0,
- LP_STATUS_INFEASIBLE=1,
- LP_STATUS_UNBOUNDED=2,
- LP_STATUS_MAX_ITER_REACHED=3,
- LP_STATUS_ERROR=4
+ * Possible states of an LP solver.
+ */
+enum CENTROIDAL_DYNAMICS_DLLAPI LP_status {
+ LP_STATUS_UNKNOWN = -1,
+ LP_STATUS_OPTIMAL = 0,
+ LP_STATUS_INFEASIBLE = 1,
+ LP_STATUS_UNBOUNDED = 2,
+ LP_STATUS_MAX_ITER_REACHED = 3,
+ LP_STATUS_ERROR = 4
};
-
/**
* @brief Abstract interface for a Linear Program (LP) solver.
*/
-class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract
-{
-protected:
- bool m_useWarmStart; // true if the solver is allowed to warm start
- int m_maxIter; // max number of iterations
- double m_maxTime; // max time to solve the LP [s]
-
-public:
-
- Solver_LP_abstract()
- {
+class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
+ protected:
+ bool m_useWarmStart; // true if the solver is allowed to warm start
+ int m_maxIter; // max number of iterations
+ double m_maxTime; // max time to solve the LP [s]
+
+ public:
+ Solver_LP_abstract() {
m_maxIter = 1000;
m_maxTime = 100.0;
m_useWarmStart = true;
@@ -63,16 +56,15 @@ public:
* @param solverType Type of LP solver.
* @return A pointer to the new solver.
*/
- static Solver_LP_abstract* getNewSolver(SolverLP solverType);
+ static Solver_LP_abstract *getNewSolver(SolverLP solverType);
/** Solve the linear program
* minimize c' x
* subject to Alb <= A x <= Aub
* lb <= x <= ub
*/
- virtual LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
- Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
- Ref_vectorX sol) = 0;
+ virtual LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub, Cref_matrixXX A, Cref_vectorX Alb,
+ Cref_vectorX Aub, Ref_vectorX sol) = 0;
/**
* @brief Solve the linear program described in the specified file.
@@ -80,7 +72,7 @@ public:
* @param sol Output solution of the LP.
* @return A flag describing the final status of the solver.
*/
- virtual LP_status solve(const std::string& filename, Ref_vectorX sol);
+ virtual LP_status solve(const std::string &filename, Ref_vectorX sol);
/**
* @brief Write the specified Linear Program to binary file.
@@ -96,8 +88,7 @@ public:
* @param Aub
* @return True if the operation succeeded, false otherwise.
*/
- virtual bool writeLpToFile(const std::string& filename,
- Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
+ virtual bool writeLpToFile(const std::string &filename, Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub);
/**
@@ -115,9 +106,8 @@ public:
* @param Aub
* @return True if the operation succeeded, false otherwise.
*/
- virtual bool readLpFromFile(const std::string& filename,
- VectorX &c, VectorX &lb, VectorX &ub,
- MatrixXX &A, VectorX &Alb, VectorX &Aub);
+ virtual bool readLpFromFile(const std::string &filename, VectorX &c, VectorX &lb, VectorX &ub, MatrixXX &A,
+ VectorX &Alb, VectorX &Aub);
/** Get the status of the solver. */
virtual LP_status getStatus() = 0;
@@ -128,25 +118,22 @@ public:
/** Get the value of the dual variables. */
virtual void getDualSolution(Ref_vectorX res) = 0;
-
/** Return true if the solver is allowed to warm start, false otherwise. */
- virtual bool getUseWarmStart(){ return m_useWarmStart; }
+ virtual bool getUseWarmStart() { return m_useWarmStart; }
/** Specify whether the solver is allowed to use warm-start techniques. */
- virtual void setUseWarmStart(bool useWarmStart){ m_useWarmStart = useWarmStart; }
+ virtual void setUseWarmStart(bool useWarmStart) { m_useWarmStart = useWarmStart; }
/** Get the current maximum number of iterations performed by the solver. */
- virtual unsigned int getMaximumIterations(){ return m_maxIter; }
+ virtual unsigned int getMaximumIterations() { return m_maxIter; }
/** Set the current maximum number of iterations performed by the solver. */
virtual bool setMaximumIterations(unsigned int maxIter);
-
/** Get the maximum time allowed to solve a problem. */
- virtual double getMaximumTime(){ return m_maxTime; }
+ virtual double getMaximumTime() { return m_maxTime; }
/** Set the maximum time allowed to solve a problem. */
virtual bool setMaximumTime(double seconds);
-
};
-} // end namespace centroidal_dynamics
+} // end namespace centroidal_dynamics
#endif // HPP_CENTROIDAL_DYNAMICS_SOLVER_LP_ABSTRACT_HH
diff --git a/include/hpp/centroidal-dynamics/solver_LP_clp.hh b/include/hpp/centroidal-dynamics/solver_LP_clp.hh
index c7dc506..742ccac 100644
--- a/include/hpp/centroidal-dynamics/solver_LP_clp.hh
+++ b/include/hpp/centroidal-dynamics/solver_LP_clp.hh
@@ -13,16 +13,13 @@
#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
#include "ClpSimplex.hpp"
-namespace centroidal_dynamics
-{
+namespace centroidal_dynamics {
-class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_clp: public Solver_LP_abstract
-{
-private:
+class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_clp : public Solver_LP_abstract {
+ private:
ClpSimplex m_model;
-public:
-
+ public:
Solver_LP_clp();
/** Solve the linear program
@@ -30,9 +27,8 @@ public:
* subject to Alb <= A x <= Aub
* lb <= x <= ub
*/
- virtual LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
- Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
- Ref_vectorX sol);
+ virtual LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub, Cref_matrixXX A, Cref_vectorX Alb,
+ Cref_vectorX Aub, Ref_vectorX sol);
/** Get the status of the solver. */
virtual LP_status getStatus();
@@ -50,8 +46,8 @@ public:
virtual bool setMaximumTime(double seconds);
};
-} // end namespace centroidal_dynamics
+} // end namespace centroidal_dynamics
-#endif //CENTROIDAL_DYNAMICS_LIB_SOLVER_LP_CLP_HH
+#endif // CENTROIDAL_DYNAMICS_LIB_SOLVER_LP_CLP_HH
-#endif // CLP_FOUND
+#endif // CLP_FOUND
diff --git a/include/hpp/centroidal-dynamics/solver_LP_qpoases.hh b/include/hpp/centroidal-dynamics/solver_LP_qpoases.hh
index c9f0bf6..67284f1 100644
--- a/include/hpp/centroidal-dynamics/solver_LP_qpoases.hh
+++ b/include/hpp/centroidal-dynamics/solver_LP_qpoases.hh
@@ -11,21 +11,18 @@
#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
#include <qpOASES.hpp>
-namespace centroidal_dynamics
-{
+namespace centroidal_dynamics {
-class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases: public Solver_LP_abstract
-{
-private:
- qpOASES::Options m_options; // solver options
- qpOASES::SQProblem m_solver; // qpoases solver
+class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases : public Solver_LP_abstract {
+ private:
+ qpOASES::Options m_options; // solver options
+ qpOASES::SQProblem m_solver; // qpoases solver
- MatrixXX m_H; // Hessian matrix
- bool m_init_succeeded; // true if solver has been successfully initialized
- qpOASES::returnValue m_status; // status code returned by the solver
-
-public:
+ MatrixXX m_H; // Hessian matrix
+ bool m_init_succeeded; // true if solver has been successfully initialized
+ qpOASES::returnValue m_status; // status code returned by the solver
+ public:
Solver_LP_qpoases();
/** Solve the linear program
@@ -33,20 +30,18 @@ public:
* subject to Alb <= A x <= Aub
* lb <= x <= ub
*/
- LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
- Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
- Ref_vectorX sol);
+ LP_status solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub, Cref_matrixXX A, Cref_vectorX Alb,
+ Cref_vectorX Aub, Ref_vectorX sol);
/** Get the status of the solver. */
virtual LP_status getStatus();
/** Get the objective value of the last solved problem. */
- virtual double getObjectiveValue(){ return m_solver.getObjVal(); }
-
- virtual void getDualSolution(Ref_vectorX res){ m_solver.getDualSolution(res.data()); }
+ virtual double getObjectiveValue() { return m_solver.getObjVal(); }
+ virtual void getDualSolution(Ref_vectorX res) { m_solver.getDualSolution(res.data()); }
};
-} // end namespace centroidal_dynamics
+} // end namespace centroidal_dynamics
#endif // HPP_CENTROIDAL_DYNAMICS_SOLVER_LP_QPOASES_HH
diff --git a/include/hpp/centroidal-dynamics/stop-watch.hh b/include/hpp/centroidal-dynamics/stop-watch.hh
index aea3d4c..354e430 100644
--- a/include/hpp/centroidal-dynamics/stop-watch.hh
+++ b/include/hpp/centroidal-dynamics/stop-watch.hh
@@ -30,25 +30,22 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include "hpp/centroidal-dynamics/Stdafx.hh"
#ifndef WIN32
- /* The classes below are exported */
- #pragma GCC visibility push(default)
+/* The classes below are exported */
+#pragma GCC visibility push(default)
#endif
// Generic stopwatch exception class
-struct StopwatchException
-{
-public:
- StopwatchException(std::string error) : error(error) { }
+struct StopwatchException {
+ public:
+ StopwatchException(std::string error) : error(error) {}
std::string error;
};
-
-enum StopwatchMode
-{
- NONE = 0, // Clock is not initialized
- CPU_TIME = 1, // Clock calculates time ranges using ctime and CLOCKS_PER_SEC
- REAL_TIME = 2 // Clock calculates time by asking the operating system how
- // much real time passed
+enum StopwatchMode {
+ NONE = 0, // Clock is not initialized
+ CPU_TIME = 1, // Clock calculates time ranges using ctime and CLOCKS_PER_SEC
+ REAL_TIME = 2 // Clock calculates time by asking the operating system how
+ // much real time passed
};
/**
@@ -143,10 +140,9 @@ enum StopwatchMode
*/
class Stopwatch {
-public:
-
+ public:
/** Constructor */
- Stopwatch(StopwatchMode _mode=NONE);
+ Stopwatch(StopwatchMode _mode = NONE);
/** Destructor */
~Stopwatch();
@@ -173,11 +169,10 @@ public:
void reset_all();
/** Dump the data of a certain performance record */
- void report(std::string perf_name, int precision=2,
- std::ostream& output = std::cout);
+ void report(std::string perf_name, int precision = 2, std::ostream& output = std::cout);
/** Dump the data of all the performance records */
- void report_all(int precision=2, std::ostream& output = std::cout);
+ void report_all(int precision = 2, std::ostream& output = std::cout);
/** Returns total execution time of a certain performance */
long double get_total_time(std::string perf_name);
@@ -199,7 +194,7 @@ public:
long double get_time_so_far(std::string perf_name);
/** Turn off clock, all the Stopwatch::* methods return without doing
- anything after this method is called. */
+ anything after this method is called. */
void turn_off();
/** Turn on clock, restore clock operativity after a turn_off(). */
@@ -208,32 +203,23 @@ public:
/** Take time, depends on mode */
long double take_time();
-protected:
-
+ protected:
/** Struct to hold the performance data */
struct PerformanceData {
-
- PerformanceData() :
- clock_start(0),
- total_time(0),
- min_time(0),
- max_time(0),
- last_time(0),
- paused(false),
- stops(0) {
- }
+ PerformanceData()
+ : clock_start(0), total_time(0), min_time(0), max_time(0), last_time(0), paused(false), stops(0) {}
/** Start time */
- long double clock_start;
+ long double clock_start;
/** Cumulative total time */
- long double total_time;
+ long double total_time;
/** Minimum time */
- long double min_time;
+ long double min_time;
/** Maximum time */
- long double max_time;
+ long double max_time;
/** Last time */
long double last_time;
@@ -242,7 +228,7 @@ protected:
bool paused;
/** How many cycles have been this stopwatch executed? */
- int stops;
+ int stops;
};
/** Flag to hold the clock's status */
@@ -253,14 +239,13 @@ protected:
/** Pointer to the dynamic structure which holds the collection of performance
data */
- std::map<std::string, PerformanceData >* records_of;
-
+ std::map<std::string, PerformanceData>* records_of;
};
Stopwatch& getProfiler();
#ifndef WIN32
- #pragma GCC visibility pop
+#pragma GCC visibility pop
#endif
#endif
diff --git a/include/hpp/centroidal-dynamics/util.hh b/include/hpp/centroidal-dynamics/util.hh
index 7dabfae..93d44b4 100644
--- a/include/hpp/centroidal-dynamics/util.hh
+++ b/include/hpp/centroidal-dynamics/util.hh
@@ -18,154 +18,144 @@
#include "cddtypes.h"
#include "cdd.h"
-namespace centroidal_dynamics
-{
+namespace centroidal_dynamics {
- //#define USE_FLOAT 1;
+//#define USE_FLOAT 1;
#ifdef USE_FLOAT
- typedef float value_type;
+typedef float value_type;
#else
- typedef double value_type;
+typedef double value_type;
#endif
- typedef Eigen::Matrix <value_type, 2, 1> Vector2;
- typedef Eigen::Matrix <value_type, 1, 2> RVector2;
- typedef Eigen::Matrix <value_type, 3, 1> Vector3;
- typedef Eigen::Matrix <value_type, 1, 3> RVector3;
- typedef Eigen::Matrix <value_type, 6, 1> Vector6;
- typedef Eigen::Matrix <value_type, Eigen::Dynamic, 1> VectorX;
- typedef Eigen::Matrix <value_type, 1, Eigen::Dynamic> RVectorX;
- typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Rotation;
- typedef Eigen::Matrix <value_type, Eigen::Dynamic, 2, Eigen::RowMajor> MatrixX2;
- typedef Eigen::Matrix <value_type, 3, 3, Eigen::RowMajor> Matrix3;
- typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3;
- typedef Eigen::Matrix <value_type, 3, Eigen::Dynamic, Eigen::RowMajor> Matrix3X;
- typedef Eigen::Matrix <value_type, 4, 3, Eigen::RowMajor> Matrix43;
- typedef Eigen::Matrix <value_type, 6, Eigen::Dynamic, Eigen::RowMajor> Matrix6X;
- typedef Eigen::Matrix <value_type, 6, 2, Eigen::RowMajor> Matrix62;
- typedef Eigen::Matrix <value_type, 6, 3, Eigen::RowMajor> Matrix63;
- typedef Eigen::Matrix <value_type, Eigen::Dynamic, 6, Eigen::RowMajor> MatrixX6;
- typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXX;
-
- typedef Eigen::Ref<Vector2> Ref_vector2;
- typedef Eigen::Ref<Vector3> Ref_vector3;
- typedef Eigen::Ref<VectorX> Ref_vectorX;
- typedef Eigen::Ref<Rotation> Ref_rotation;
- typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
- typedef Eigen::Ref<Matrix43> Ref_matrix43;
- typedef Eigen::Ref<Matrix6X> Ref_matrix6X;
- typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
-
- typedef const Eigen::Ref<const Vector2> & Cref_vector2;
- typedef const Eigen::Ref<const Vector3> & Cref_vector3;
- typedef const Eigen::Ref<const Vector6> & Cref_vector6;
- typedef const Eigen::Ref<const VectorX> & Cref_vectorX;
- typedef const Eigen::Ref<const Rotation> & Cref_rotation;
- typedef const Eigen::Ref<const MatrixX3> & Cref_matrixX3;
- typedef const Eigen::Ref<const Matrix43> & Cref_matrix43;
- typedef const Eigen::Ref<const Matrix6X> & Cref_matrix6X;
- typedef const Eigen::Ref<const Matrix63> & Cref_matrix63;
- typedef const Eigen::Ref<const MatrixXX> & Cref_matrixXX;
-
- /**Column major definitions for compatibility with classical eigen use**/
- typedef Eigen::Matrix <value_type, Eigen::Dynamic, 3, Eigen::ColMajor> MatrixX3ColMajor;
- typedef Eigen::Matrix <value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixXXColMajor;
- typedef const Eigen::Ref<const MatrixX3ColMajor> & Cref_matrixX3ColMajor;
- typedef Eigen::Ref<MatrixXXColMajor> & ref_matrixXXColMajor;
-
- /**
- * Write the specified matrix to a binary file with the specified name.
- */
- template<class Matrix>
- bool writeMatrixToFile(const std::string &filename, const Matrix& matrix)
- {
- std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
- if(!out.is_open())
- return false;
- typename Matrix::Index rows=matrix.rows(), cols=matrix.cols();
- out.write((char*) (&rows), sizeof(typename Matrix::Index));
- out.write((char*) (&cols), sizeof(typename Matrix::Index));
- out.write((char*) matrix.data(), rows*cols*sizeof(typename Matrix::Scalar) );
- out.close();
- return true;
- }
+typedef Eigen::Matrix<value_type, 2, 1> Vector2;
+typedef Eigen::Matrix<value_type, 1, 2> RVector2;
+typedef Eigen::Matrix<value_type, 3, 1> Vector3;
+typedef Eigen::Matrix<value_type, 1, 3> RVector3;
+typedef Eigen::Matrix<value_type, 6, 1> Vector6;
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, 1> VectorX;
+typedef Eigen::Matrix<value_type, 1, Eigen::Dynamic> RVectorX;
+typedef Eigen::Matrix<value_type, 3, 3, Eigen::RowMajor> Rotation;
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, 2, Eigen::RowMajor> MatrixX2;
+typedef Eigen::Matrix<value_type, 3, 3, Eigen::RowMajor> Matrix3;
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::RowMajor> MatrixX3;
+typedef Eigen::Matrix<value_type, 3, Eigen::Dynamic, Eigen::RowMajor> Matrix3X;
+typedef Eigen::Matrix<value_type, 4, 3, Eigen::RowMajor> Matrix43;
+typedef Eigen::Matrix<value_type, 6, Eigen::Dynamic, Eigen::RowMajor> Matrix6X;
+typedef Eigen::Matrix<value_type, 6, 2, Eigen::RowMajor> Matrix62;
+typedef Eigen::Matrix<value_type, 6, 3, Eigen::RowMajor> Matrix63;
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, 6, Eigen::RowMajor> MatrixX6;
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXX;
+
+typedef Eigen::Ref<Vector2> Ref_vector2;
+typedef Eigen::Ref<Vector3> Ref_vector3;
+typedef Eigen::Ref<VectorX> Ref_vectorX;
+typedef Eigen::Ref<Rotation> Ref_rotation;
+typedef Eigen::Ref<MatrixX3> Ref_matrixX3;
+typedef Eigen::Ref<Matrix43> Ref_matrix43;
+typedef Eigen::Ref<Matrix6X> Ref_matrix6X;
+typedef Eigen::Ref<MatrixXX> Ref_matrixXX;
+
+typedef const Eigen::Ref<const Vector2>& Cref_vector2;
+typedef const Eigen::Ref<const Vector3>& Cref_vector3;
+typedef const Eigen::Ref<const Vector6>& Cref_vector6;
+typedef const Eigen::Ref<const VectorX>& Cref_vectorX;
+typedef const Eigen::Ref<const Rotation>& Cref_rotation;
+typedef const Eigen::Ref<const MatrixX3>& Cref_matrixX3;
+typedef const Eigen::Ref<const Matrix43>& Cref_matrix43;
+typedef const Eigen::Ref<const Matrix6X>& Cref_matrix6X;
+typedef const Eigen::Ref<const Matrix63>& Cref_matrix63;
+typedef const Eigen::Ref<const MatrixXX>& Cref_matrixXX;
+
+/**Column major definitions for compatibility with classical eigen use**/
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, 3, Eigen::ColMajor> MatrixX3ColMajor;
+typedef Eigen::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor> MatrixXXColMajor;
+typedef const Eigen::Ref<const MatrixX3ColMajor>& Cref_matrixX3ColMajor;
+typedef Eigen::Ref<MatrixXXColMajor>& ref_matrixXXColMajor;
+
+/**
+ * Write the specified matrix to a binary file with the specified name.
+ */
+template <class Matrix>
+bool writeMatrixToFile(const std::string& filename, const Matrix& matrix) {
+ std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
+ if (!out.is_open()) return false;
+ typename Matrix::Index rows = matrix.rows(), cols = matrix.cols();
+ out.write((char*)(&rows), sizeof(typename Matrix::Index));
+ out.write((char*)(&cols), sizeof(typename Matrix::Index));
+ out.write((char*)matrix.data(), rows * cols * sizeof(typename Matrix::Scalar));
+ out.close();
+ return true;
+}
- /**
- * Read a matrix from the specified input binary file.
- */
- template<class Matrix>
- bool readMatrixFromFile(const std::string &filename, Matrix& matrix)
- {
- std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
- if(!in.is_open())
- return false;
- typename Matrix::Index rows=0, cols=0;
- in.read((char*) (&rows),sizeof(typename Matrix::Index));
- in.read((char*) (&cols),sizeof(typename Matrix::Index));
- matrix.resize(rows, cols);
- in.read( (char *) matrix.data() , rows*cols*sizeof(typename Matrix::Scalar) );
- in.close();
- return true;
- }
+/**
+ * Read a matrix from the specified input binary file.
+ */
+template <class Matrix>
+bool readMatrixFromFile(const std::string& filename, Matrix& matrix) {
+ std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
+ if (!in.is_open()) return false;
+ typename Matrix::Index rows = 0, cols = 0;
+ in.read((char*)(&rows), sizeof(typename Matrix::Index));
+ in.read((char*)(&cols), sizeof(typename Matrix::Index));
+ matrix.resize(rows, cols);
+ in.read((char*)matrix.data(), rows * cols * sizeof(typename Matrix::Scalar));
+ in.close();
+ return true;
+}
- /**
+/**
* Convert the specified list of rays from Eigen to cdd format.
* @param input The mXn input Eigen matrix contains m rays of dimension n.
* @param bool whether to remove redundant inequalities
* @return The mX(n+1) output cdd matrix, which contains an additional column,
* the first one, with all zeros.
*/
- dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize=false);
+dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize = false);
- /**
+/**
* Compute the cross-product skew-symmetric matrix associated to the specified vector.
*/
- Rotation crossMatrix(Cref_vector3 x);
+Rotation crossMatrix(Cref_vector3 x);
+void init_cdd_library();
- void init_cdd_library();
+void release_cdd_library();
- void release_cdd_library();
+// in some distribution the conversion Ref_matrixXX to Ref_vector3 does not compile
+void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds, Ref_vector3 out);
+void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out);
- // in some distribution the conversion Ref_matrixXX to Ref_vector3 does not compile
- void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds, Ref_vector3 out);
- void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out);
+void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R);
- void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R);
+bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy, Ref_matrix43 p,
+ Ref_matrix43 N);
- bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy,
- Ref_matrix43 p, Ref_matrix43 N);
+std::string getDateAndTimeAsString();
- std::string getDateAndTimeAsString();
-
- /**
+/**
* Computes a binomal coefficient
* @return n!/((n–k)! k!).
*/
- value_type nchoosek(const int n, const int k);
-
- template < typename DerivedV, typename DerivedU>
- void doCombs(Eigen::Matrix<typename DerivedU::Scalar,1,Eigen::Dynamic>& running,
- int& running_i, int& running_j, Eigen::PlainObjectBase<DerivedU> & U, const Eigen::MatrixBase<DerivedV> & V,
- int offset, int k)
- {
- int N = (int)(V.size());
- if(k==0)
- {
- U.row(running_i) = running;
- running_i++;
- return;
- }
- for (int i = offset; i <= N - k; ++i)
- {
- running(running_j) = V(i);
- running_j++;
- doCombs(running, running_i, running_j, U, V, i+1,k-1);
- running_j--;
- }
+value_type nchoosek(const int n, const int k);
+
+template <typename DerivedV, typename DerivedU>
+void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& running, int& running_i, int& running_j,
+ Eigen::PlainObjectBase<DerivedU>& U, const Eigen::MatrixBase<DerivedV>& V, int offset, int k) {
+ int N = (int)(V.size());
+ if (k == 0) {
+ U.row(running_i) = running;
+ running_i++;
+ return;
+ }
+ for (int i = offset; i <= N - k; ++i) {
+ running(running_j) = V(i);
+ running_j++;
+ doCombs(running, running_i, running_j, U, V, i + 1, k - 1);
+ running_j--;
}
+}
- /**
+/**
* Computes a matrix C containing all possible combinations of the elements of vector v taken k at a time.
* Matrix C has k columns and n!/((n–k)! k!) rows, where n is length(v).
* @param V n-long vector of elements
diff --git a/python/centroidal_dynamics_python.cpp b/python/centroidal_dynamics_python.cpp
index 3fff17a..7043d52 100644
--- a/python/centroidal_dynamics_python.cpp
+++ b/python/centroidal_dynamics_python.cpp
@@ -8,118 +8,109 @@
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(centroidal_dynamics::Equilibrium)
-namespace centroidal_dynamics
-{
+namespace centroidal_dynamics {
using namespace boost::python;
-boost::python::tuple wrapComputeQuasiEquilibriumRobustness(Equilibrium& self, const Vector3& com)
-{
- double robustness;
- LP_status status = self.computeEquilibriumRobustness(com, robustness);
- return boost::python::make_tuple(status, robustness);
+boost::python::tuple wrapComputeQuasiEquilibriumRobustness(Equilibrium& self, const Vector3& com) {
+ double robustness;
+ LP_status status = self.computeEquilibriumRobustness(com, robustness);
+ return boost::python::make_tuple(status, robustness);
}
-boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self, const Vector3& com, const Vector3& acc)
-{
- double robustness;
- LP_status status = self.computeEquilibriumRobustness(com, acc, robustness);
- return boost::python::make_tuple(status, robustness);
+boost::python::tuple wrapComputeEquilibriumRobustness(Equilibrium& self, const Vector3& com, const Vector3& acc) {
+ double robustness;
+ LP_status status = self.computeEquilibriumRobustness(com, acc, robustness);
+ return boost::python::make_tuple(status, robustness);
}
-boost::python::tuple wrapCheckRobustEquilibrium(Equilibrium& self, const Vector3& com)
-{
- bool robustness;
- LP_status status = self.checkRobustEquilibrium(com, robustness);
- return boost::python::make_tuple(status, robustness);
+boost::python::tuple wrapCheckRobustEquilibrium(Equilibrium& self, const Vector3& com) {
+ bool robustness;
+ LP_status status = self.checkRobustEquilibrium(com, robustness);
+ return boost::python::make_tuple(status, robustness);
}
-bool wrapSetNewContacts(Equilibrium& self, const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
- const double frictionCoefficient, const EquilibriumAlgorithm alg)
-{
- return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
+bool wrapSetNewContacts(Equilibrium& self, const MatrixX3ColMajor& contactPoints,
+ const MatrixX3ColMajor& contactNormals, const double frictionCoefficient,
+ const EquilibriumAlgorithm alg) {
+ return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
}
-bool wrapSetNewContactsFull(Equilibrium& self, const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
- const double frictionCoefficient, const EquilibriumAlgorithm alg)
-{
- return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
+bool wrapSetNewContactsFull(Equilibrium& self, const MatrixX3ColMajor& contactPoints,
+ const MatrixX3ColMajor& contactNormals, const double frictionCoefficient,
+ const EquilibriumAlgorithm alg) {
+ return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
}
-boost::python::tuple wrapGetPolytopeInequalities(Equilibrium& self)
-{
- MatrixXX H;
- VectorX h;
- H = MatrixXX::Zero(6,6);
- h = VectorX::Zero(6,1);
- self.getPolytopeInequalities(H,h);
- MatrixXXColMajor _H = H;
- return boost::python::make_tuple(_H, h);
+boost::python::tuple wrapGetPolytopeInequalities(Equilibrium& self) {
+ MatrixXX H;
+ VectorX h;
+ H = MatrixXX::Zero(6, 6);
+ h = VectorX::Zero(6, 1);
+ self.getPolytopeInequalities(H, h);
+ MatrixXXColMajor _H = H;
+ return boost::python::make_tuple(_H, h);
}
-
-
-BOOST_PYTHON_MODULE(hpp_centroidal_dynamics)
-{
- /** BEGIN eigenpy init**/
- eigenpy::enableEigenPy();
-
- eigenpy::enableEigenPySpecific<MatrixX3ColMajor,MatrixX3ColMajor>();
- eigenpy::enableEigenPySpecific<MatrixXXColMajor,MatrixXXColMajor>();
- eigenpy::enableEigenPySpecific<Vector3,Vector3>();
- eigenpy::enableEigenPySpecific<VectorX,VectorX>();
- /*eigenpy::exposeAngleAxis();
- eigenpy::exposeQuaternion();*/
-
- /** END eigenpy init**/
-
- /** BEGIN enum types **/
- #ifdef CLP_FOUND
- enum_<SolverLP>("SolverLP")
- .value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES)
- .value("SOLVER_LP_CLP", SOLVER_LP_CLP)
- .export_values();
- #else
- enum_<SolverLP>("SolverLP")
- .value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES)
- .export_values();
- #endif
-
-
- enum_<EquilibriumAlgorithm>("EquilibriumAlgorithm")
- .value("EQUILIBRIUM_ALGORITHM_LP", EQUILIBRIUM_ALGORITHM_LP)
- .value("EQUILIBRIUM_ALGORITHM_LP2", EQUILIBRIUM_ALGORITHM_LP2)
- .value("EQUILIBRIUM_ALGORITHM_DLP", EQUILIBRIUM_ALGORITHM_DLP)
- .value("EQUILIBRIUM_ALGORITHM_PP", EQUILIBRIUM_ALGORITHM_PP)
- .value("EQUILIBRIUM_ALGORITHM_IP", EQUILIBRIUM_ALGORITHM_IP)
- .value("EQUILIBRIUM_ALGORITHM_DIP", EQUILIBRIUM_ALGORITHM_DIP)
- .export_values();
-
- enum_<LP_status>("LP_status")
- .value("LP_STATUS_UNKNOWN", LP_STATUS_UNKNOWN)
- .value("LP_STATUS_OPTIMAL", LP_STATUS_OPTIMAL)
- .value("LP_STATUS_INFEASIBLE", LP_STATUS_INFEASIBLE)
- .value("LP_STATUS_UNBOUNDED", LP_STATUS_UNBOUNDED)
- .value("LP_STATUS_MAX_ITER_REACHED", LP_STATUS_MAX_ITER_REACHED)
- .value("LP_STATUS_ERROR", LP_STATUS_ERROR)
- .export_values();
-
- /** END enum types **/
-
- //bool (Equilibrium::*setNewContacts)
- // (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm, const int graspIndex, const double maxGraspForce) = &Equilibrium::setNewContacts;
-
- class_<Equilibrium>("Equilibrium", init<std::string, double, unsigned int, optional <SolverLP, bool, const unsigned int, const bool> >())
- .def("useWarmStart", &Equilibrium::useWarmStart)
- .def("setUseWarmStart", &Equilibrium::setUseWarmStart)
- .def("getName", &Equilibrium::getName)
- .def("getAlgorithm", &Equilibrium::getAlgorithm)
- .def("setNewContacts", wrapSetNewContacts)
- .def("setNewContacts", wrapSetNewContactsFull)
- .def("computeEquilibriumRobustness", wrapComputeQuasiEquilibriumRobustness)
- .def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
- .def("checkRobustEquilibrium", wrapCheckRobustEquilibrium)
- .def("getPolytopeInequalities", wrapGetPolytopeInequalities)
- ;
+BOOST_PYTHON_MODULE(hpp_centroidal_dynamics) {
+ /** BEGIN eigenpy init**/
+ eigenpy::enableEigenPy();
+
+ eigenpy::enableEigenPySpecific<MatrixX3ColMajor, MatrixX3ColMajor>();
+ eigenpy::enableEigenPySpecific<MatrixXXColMajor, MatrixXXColMajor>();
+ eigenpy::enableEigenPySpecific<Vector3, Vector3>();
+ eigenpy::enableEigenPySpecific<VectorX, VectorX>();
+ /*eigenpy::exposeAngleAxis();
+ eigenpy::exposeQuaternion();*/
+
+ /** END eigenpy init**/
+
+ /** BEGIN enum types **/
+#ifdef CLP_FOUND
+ enum_<SolverLP>("SolverLP")
+ .value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES)
+ .value("SOLVER_LP_CLP", SOLVER_LP_CLP)
+ .export_values();
+#else
+ enum_<SolverLP>("SolverLP").value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES).export_values();
+#endif
+
+ enum_<EquilibriumAlgorithm>("EquilibriumAlgorithm")
+ .value("EQUILIBRIUM_ALGORITHM_LP", EQUILIBRIUM_ALGORITHM_LP)
+ .value("EQUILIBRIUM_ALGORITHM_LP2", EQUILIBRIUM_ALGORITHM_LP2)
+ .value("EQUILIBRIUM_ALGORITHM_DLP", EQUILIBRIUM_ALGORITHM_DLP)
+ .value("EQUILIBRIUM_ALGORITHM_PP", EQUILIBRIUM_ALGORITHM_PP)
+ .value("EQUILIBRIUM_ALGORITHM_IP", EQUILIBRIUM_ALGORITHM_IP)
+ .value("EQUILIBRIUM_ALGORITHM_DIP", EQUILIBRIUM_ALGORITHM_DIP)
+ .export_values();
+
+ enum_<LP_status>("LP_status")
+ .value("LP_STATUS_UNKNOWN", LP_STATUS_UNKNOWN)
+ .value("LP_STATUS_OPTIMAL", LP_STATUS_OPTIMAL)
+ .value("LP_STATUS_INFEASIBLE", LP_STATUS_INFEASIBLE)
+ .value("LP_STATUS_UNBOUNDED", LP_STATUS_UNBOUNDED)
+ .value("LP_STATUS_MAX_ITER_REACHED", LP_STATUS_MAX_ITER_REACHED)
+ .value("LP_STATUS_ERROR", LP_STATUS_ERROR)
+ .export_values();
+
+ /** END enum types **/
+
+ // bool (Equilibrium::*setNewContacts)
+ // (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm, const int
+ // graspIndex, const double maxGraspForce) = &Equilibrium::setNewContacts;
+
+ class_<Equilibrium>(
+ "Equilibrium",
+ init<std::string, double, unsigned int, optional<SolverLP, bool, const unsigned int, const bool> >())
+ .def("useWarmStart", &Equilibrium::useWarmStart)
+ .def("setUseWarmStart", &Equilibrium::setUseWarmStart)
+ .def("getName", &Equilibrium::getName)
+ .def("getAlgorithm", &Equilibrium::getAlgorithm)
+ .def("setNewContacts", wrapSetNewContacts)
+ .def("setNewContacts", wrapSetNewContactsFull)
+ .def("computeEquilibriumRobustness", wrapComputeQuasiEquilibriumRobustness)
+ .def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
+ .def("checkRobustEquilibrium", wrapCheckRobustEquilibrium)
+ .def("getPolytopeInequalities", wrapGetPolytopeInequalities);
}
-} // namespace centroidal_dynamics
+} // namespace centroidal_dynamics
diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp
index f495610..9f7a24f 100644
--- a/src/centroidal_dynamics.cpp
+++ b/src/centroidal_dynamics.cpp
@@ -12,49 +12,44 @@
using namespace std;
-namespace centroidal_dynamics
-{
+namespace centroidal_dynamics {
bool Equilibrium::m_is_cdd_initialized = false;
-
Equilibrium::Equilibrium(const Equilibrium& other)
- : m_mass(other.m_mass)
- , m_gravity(other.m_gravity)
- , m_name (other.m_name)
- , m_algorithm(other.m_algorithm)
- , m_solver_type (other.m_solver_type)
- , m_solver (Solver_LP_abstract::getNewSolver(other.m_solver_type))
- , m_generatorsPerContact(other.m_generatorsPerContact)
- , m_G_centr(other.m_G_centr)
- , m_H(other.m_H)
- , m_h(other.m_h)
- , m_is_cdd_stable(other.m_is_cdd_stable)
- , max_num_cdd_trials(other.max_num_cdd_trials)
- , canonicalize_cdd_matrix(other.canonicalize_cdd_matrix)
- , m_HD(other.m_HD)
- , m_Hd(other.m_Hd)
- , m_D(other.m_D)
- , m_d(other.m_d)
- , m_b0_to_emax_coefficient(other.m_b0_to_emax_coefficient)
-{
- m_solver->setUseWarmStart(other.m_solver->getUseWarmStart());
+ : m_mass(other.m_mass),
+ m_gravity(other.m_gravity),
+ m_name(other.m_name),
+ m_algorithm(other.m_algorithm),
+ m_solver_type(other.m_solver_type),
+ m_solver(Solver_LP_abstract::getNewSolver(other.m_solver_type)),
+ m_generatorsPerContact(other.m_generatorsPerContact),
+ m_G_centr(other.m_G_centr),
+ m_H(other.m_H),
+ m_h(other.m_h),
+ m_is_cdd_stable(other.m_is_cdd_stable),
+ max_num_cdd_trials(other.max_num_cdd_trials),
+ canonicalize_cdd_matrix(other.canonicalize_cdd_matrix),
+ m_HD(other.m_HD),
+ m_Hd(other.m_Hd),
+ m_D(other.m_D),
+ m_d(other.m_d),
+ m_b0_to_emax_coefficient(other.m_b0_to_emax_coefficient) {
+ m_solver->setUseWarmStart(other.m_solver->getUseWarmStart());
}
Equilibrium::Equilibrium(const string& name, const double mass, const unsigned int generatorsPerContact,
- const SolverLP solver_type, const bool useWarmStart,
- const unsigned int max_num_cdd_trials, const bool canonicalize_cdd_matrix)
- : m_mass(mass)
- , m_gravity(0.,0.,-9.81)
- , m_is_cdd_stable(true)
- , max_num_cdd_trials(max_num_cdd_trials)
- , canonicalize_cdd_matrix(canonicalize_cdd_matrix)
-{
- if(!m_is_cdd_initialized)
- {
+ const SolverLP solver_type, const bool useWarmStart, const unsigned int max_num_cdd_trials,
+ const bool canonicalize_cdd_matrix)
+ : m_mass(mass),
+ m_gravity(0., 0., -9.81),
+ m_is_cdd_stable(true),
+ max_num_cdd_trials(max_num_cdd_trials),
+ canonicalize_cdd_matrix(canonicalize_cdd_matrix) {
+ if (!m_is_cdd_initialized) {
init_cdd_library();
m_is_cdd_initialized = true;
- //srand ( (unsigned int) (time(NULL)) );
+ // srand ( (unsigned int) (time(NULL)) );
}
m_name = name;
@@ -63,8 +58,7 @@ Equilibrium::Equilibrium(const string& name, const double mass, const unsigned i
m_solver->setUseWarmStart(useWarmStart);
m_generatorsPerContact = generatorsPerContact;
- if(generatorsPerContact<3)
- {
+ if (generatorsPerContact < 3) {
SEND_WARNING_MSG("Algorithm cannot work with less than 3 generators per contact!");
m_generatorsPerContact = 3;
}
@@ -73,114 +67,101 @@ Equilibrium::Equilibrium(const string& name, const double mass, const unsigned i
m_gravity(2) = -9.81;*/
m_d.setZero();
- m_d.head<3>() = m_mass*m_gravity;
+ m_d.head<3>() = m_mass * m_gravity;
m_D.setZero();
- m_D.block<3,3>(3,0) = crossMatrix(-m_mass*m_gravity);
+ m_D.block<3, 3>(3, 0) = crossMatrix(-m_mass * m_gravity);
}
bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
- double frictionCoefficient, const bool perturbate)
-{
- long int c = contactPoints.rows();
- unsigned int &cg = m_generatorsPerContact;
- double theta, delta_theta=2*M_PI/cg;
- const value_type pi_4 = value_type(M_PI_4);
- // perturbation for libcdd
- const value_type epsilon = 2*1e-3;
- if(perturbate)
- frictionCoefficient = frictionCoefficient +(rand()/ value_type(RAND_MAX))*epsilon;
- // Tangent directions
- Vector3 T1, T2, N;
- // contact point
- Vector3 P;
- // Matrix mapping a 3d contact force to gravito-inertial wrench (6 X 3)
- Matrix63 A;
- A.topRows<3>() = -Matrix3::Identity();
- Matrix3X G(3, cg);
- for(long int i=0; i<c; i++)
- {
- // check that contact normals have norm 1
- if(fabs(contactNormals.row(i).norm()-1.0)>1e-4)
- {
- SEND_ERROR_MSG("Contact normals should have norm 1, this has norm %f"+toString(contactNormals.row(i).norm()));
- return false;
- }
- // compute tangent directions
- N = contactNormals.row(i);
- P = contactPoints.row(i);
- if(perturbate)
- {
- for(int k =0; k<3; ++k)
- {
- N(k) +=(rand()/ value_type(RAND_MAX))*epsilon;
- P(k) +=(rand()/ value_type(RAND_MAX))*epsilon;
- }
- N.normalize();
- }
- T1 = N.cross(Vector3::UnitY());
- if(T1.norm()<1e-5)
- T1 = N.cross(Vector3::UnitX());
- T2 = N.transpose().cross(T1);
- T1.normalize();
- T2.normalize();
-
- // compute matrix mapping contact forces to gravito-inertial wrench
- A.bottomRows<3>() = crossMatrix(-1.0*P);
-
- // compute generators
- theta = pi_4;
- for(int j=0; j<cg; j++)
- {
- G.col(j) = frictionCoefficient*sin(theta)*T1
- + frictionCoefficient*cos(theta)*T2
- + contactNormals.row(i).transpose();
- G.col(j).normalize();
- // SEND_DEBUG_MSG("Contact "+toString(i)+" generator "+toString(j)+" = "+toString(G.col(j).transpose()));
- theta += delta_theta;
+ double frictionCoefficient, const bool perturbate) {
+ long int c = contactPoints.rows();
+ unsigned int& cg = m_generatorsPerContact;
+ double theta, delta_theta = 2 * M_PI / cg;
+ const value_type pi_4 = value_type(M_PI_4);
+ // perturbation for libcdd
+ const value_type epsilon = 2 * 1e-3;
+ if (perturbate) frictionCoefficient = frictionCoefficient + (rand() / value_type(RAND_MAX)) * epsilon;
+ // Tangent directions
+ Vector3 T1, T2, N;
+ // contact point
+ Vector3 P;
+ // Matrix mapping a 3d contact force to gravito-inertial wrench (6 X 3)
+ Matrix63 A;
+ A.topRows<3>() = -Matrix3::Identity();
+ Matrix3X G(3, cg);
+ for (long int i = 0; i < c; i++) {
+ // check that contact normals have norm 1
+ if (fabs(contactNormals.row(i).norm() - 1.0) > 1e-4) {
+ SEND_ERROR_MSG("Contact normals should have norm 1, this has norm %f" + toString(contactNormals.row(i).norm()));
+ return false;
+ }
+ // compute tangent directions
+ N = contactNormals.row(i);
+ P = contactPoints.row(i);
+ if (perturbate) {
+ for (int k = 0; k < 3; ++k) {
+ N(k) += (rand() / value_type(RAND_MAX)) * epsilon;
+ P(k) += (rand() / value_type(RAND_MAX)) * epsilon;
}
-
- // project generators in 6d centroidal space
- m_G_centr.block(0,cg*i,6,cg) = A * G;
+ N.normalize();
+ }
+ T1 = N.cross(Vector3::UnitY());
+ if (T1.norm() < 1e-5) T1 = N.cross(Vector3::UnitX());
+ T2 = N.transpose().cross(T1);
+ T1.normalize();
+ T2.normalize();
+
+ // compute matrix mapping contact forces to gravito-inertial wrench
+ A.bottomRows<3>() = crossMatrix(-1.0 * P);
+
+ // compute generators
+ theta = pi_4;
+ for (int j = 0; j < cg; j++) {
+ G.col(j) = frictionCoefficient * sin(theta) * T1 + frictionCoefficient * cos(theta) * T2 +
+ contactNormals.row(i).transpose();
+ G.col(j).normalize();
+ // SEND_DEBUG_MSG("Contact "+toString(i)+" generator "+toString(j)+" = "+toString(G.col(j).transpose()));
+ theta += delta_theta;
}
- // Compute the coefficient to convert b0 to e_max
- Vector3 f0 = Vector3::Zero();
- for(int j=0; j<cg; j++)
- f0 += G.col(j); // sum of the contact generators
- // Compute the distance between the friction cone boundaries and
- // the sum of the contact generators, which is e_max when b0=1.
- // When b0!=1 we just multiply b0 times this value.
- // This value depends only on the number of generators and the friction coefficient
- m_b0_to_emax_coefficient = (f0.cross(G.col(0))).norm();
- return true;
+ // project generators in 6d centroidal space
+ m_G_centr.block(0, cg * i, 6, cg) = A * G;
+ }
+
+ // Compute the coefficient to convert b0 to e_max
+ Vector3 f0 = Vector3::Zero();
+ for (int j = 0; j < cg; j++) f0 += G.col(j); // sum of the contact generators
+ // Compute the distance between the friction cone boundaries and
+ // the sum of the contact generators, which is e_max when b0=1.
+ // When b0!=1 we just multiply b0 times this value.
+ // This value depends only on the number of generators and the friction coefficient
+ m_b0_to_emax_coefficient = (f0.cross(G.col(0))).norm();
+ return true;
}
-void Equilibrium::setAlgorithm(EquilibriumAlgorithm algorithm){
- if(algorithm == EQUILIBRIUM_ALGORITHM_PP && m_G_centr.rows() > 0)
- SEND_DEBUG_MSG("Cannot set algorithm to PP after setting contacts, you should call again setNewContact with PP algorithm");
- else
- m_algorithm = algorithm;
+void Equilibrium::setAlgorithm(EquilibriumAlgorithm algorithm) {
+ if (algorithm == EQUILIBRIUM_ALGORITHM_PP && m_G_centr.rows() > 0)
+ SEND_DEBUG_MSG(
+ "Cannot set algorithm to PP after setting contacts, you should call again setNewContact with PP algorithm");
+ else
+ m_algorithm = algorithm;
}
bool Equilibrium::setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
- const double frictionCoefficient, const EquilibriumAlgorithm alg)
-{
- MatrixX3 _contactPoints = contactPoints;
- MatrixX3 _contactNormals = contactNormals;
- return setNewContacts(_contactPoints,_contactNormals, frictionCoefficient, alg);
+ const double frictionCoefficient, const EquilibriumAlgorithm alg) {
+ MatrixX3 _contactPoints = contactPoints;
+ MatrixX3 _contactNormals = contactNormals;
+ return setNewContacts(_contactPoints, _contactNormals, frictionCoefficient, alg);
}
bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
- const double frictionCoefficient, const EquilibriumAlgorithm alg)
-{
- assert(contactPoints.rows()==contactNormals.rows());
- if(alg==EQUILIBRIUM_ALGORITHM_IP)
- {
+ const double frictionCoefficient, const EquilibriumAlgorithm alg) {
+ assert(contactPoints.rows() == contactNormals.rows());
+ if (alg == EQUILIBRIUM_ALGORITHM_IP) {
SEND_ERROR_MSG("Algorithm IP not implemented yet");
return false;
}
- if(alg==EQUILIBRIUM_ALGORITHM_DIP)
- {
+ if (alg == EQUILIBRIUM_ALGORITHM_DIP) {
SEND_ERROR_MSG("Algorithm DIP not implemented yet");
return false;
}
@@ -188,25 +169,21 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
m_algorithm = alg;
// Lists of contact generators (3 X generatorsPerContact)
- m_G_centr.resize(6,contactPoints.rows()*m_generatorsPerContact);
+ m_G_centr.resize(6, contactPoints.rows() * m_generatorsPerContact);
- if (!computeGenerators(contactPoints,contactNormals,frictionCoefficient, false))
- {
+ if (!computeGenerators(contactPoints, contactNormals, frictionCoefficient, false)) {
return false;
}
- if(m_algorithm==EQUILIBRIUM_ALGORITHM_PP)
- {
+ if (m_algorithm == EQUILIBRIUM_ALGORITHM_PP) {
unsigned int attempts = max_num_cdd_trials;
- while(!computePolytopeProjection(m_G_centr) && attempts>0)
- {
- computeGenerators(contactPoints,contactNormals,frictionCoefficient,true);
+ while (!computePolytopeProjection(m_G_centr) && attempts > 0) {
+ computeGenerators(contactPoints, contactNormals, frictionCoefficient, true);
attempts--;
}
// resetting random because obviously libcdd always resets to 0
srand(time(NULL));
- if(!m_is_cdd_stable)
- {
+ if (!m_is_cdd_stable) {
return false;
}
m_HD = m_H * m_D;
@@ -218,23 +195,19 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
static const Vector3 zero_acc = Vector3::Zero();
-LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, double &robustness)
-{
- return computeEquilibriumRobustness(com, zero_acc, robustness);
+LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, double& robustness) {
+ return computeEquilibriumRobustness(com, zero_acc, robustness);
}
-LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness)
-{
+LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double& robustness) {
// Take the acceleration in account in D and d :
- m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
- m_d.head<3>()= m_mass * (m_gravity - acc);
+ m_D.block<3, 3>(3, 0) = crossMatrix(-m_mass * (m_gravity - acc));
+ m_d.head<3>() = m_mass * (m_gravity - acc);
- const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
- if(m==0)
- return LP_STATUS_INFEASIBLE;
+ const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+ if (m == 0) return LP_STATUS_INFEASIBLE;
- if(m_algorithm==EQUILIBRIUM_ALGORITHM_LP)
- {
+ if (m_algorithm == EQUILIBRIUM_ALGORITHM_LP) {
/* Compute the robustness measure of the equilibrium of a specified CoM position
* by solving the following LP:
find b, b0
@@ -247,33 +220,31 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
c is the CoM position
G is the matrix whose columns are the gravito-inertial wrench generators
*/
- VectorX b_b0(m+1);
- VectorX c = VectorX::Zero(m+1);
+ VectorX b_b0(m + 1);
+ VectorX c = VectorX::Zero(m + 1);
c(m) = -1.0;
- VectorX lb = -VectorX::Ones(m+1)*1e5;
- VectorX ub = VectorX::Ones(m+1)*1e10;
- VectorX Alb = VectorX::Zero(6+m);
- VectorX Aub = VectorX::Ones(6+m)*1e100;
- MatrixXX A = MatrixXX::Zero(6+m, m+1);
+ VectorX lb = -VectorX::Ones(m + 1) * 1e5;
+ VectorX ub = VectorX::Ones(m + 1) * 1e10;
+ VectorX Alb = VectorX::Zero(6 + m);
+ VectorX Aub = VectorX::Ones(6 + m) * 1e100;
+ MatrixXX A = MatrixXX::Zero(6 + m, m + 1);
Alb.head<6>() = m_D * com + m_d;
Aub.head<6>() = Alb.head<6>();
- A.topLeftCorner(6,m) = m_G_centr;
- A.bottomLeftCorner(m,m) = MatrixXX::Identity(m,m);
- A.bottomRightCorner(m,1) = -VectorX::Ones(m);
+ A.topLeftCorner(6, m) = m_G_centr;
+ A.bottomLeftCorner(m, m) = MatrixXX::Identity(m, m);
+ A.bottomRightCorner(m, 1) = -VectorX::Ones(m);
LP_status lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
- if(lpStatus==LP_STATUS_OPTIMAL)
- {
- robustness = convert_b0_to_emax(-1.0*m_solver->getObjectiveValue());
+ if (lpStatus == LP_STATUS_OPTIMAL) {
+ robustness = convert_b0_to_emax(-1.0 * m_solver->getObjectiveValue());
return lpStatus;
}
- SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
+ SEND_DEBUG_MSG("Primal LP problem could not be solved: " + toString(lpStatus));
return lpStatus;
}
- if(m_algorithm==EQUILIBRIUM_ALGORITHM_LP2)
- {
+ if (m_algorithm == EQUILIBRIUM_ALGORITHM_LP2) {
/* Compute the robustness measure of the equilibrium of a specified CoM position
* by solving the following LP:
find b, b0
@@ -286,31 +257,29 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
c is the CoM position
G is the matrix whose columns are the gravito-inertial wrench generators
*/
- VectorX b_b0(m+1);
- VectorX c = VectorX::Zero(m+1);
+ VectorX b_b0(m + 1);
+ VectorX c = VectorX::Zero(m + 1);
c(m) = -1.0;
- VectorX lb = VectorX::Zero(m+1);
+ VectorX lb = VectorX::Zero(m + 1);
lb(m) = -1e10;
- VectorX ub = VectorX::Ones(m+1)*1e10;
- MatrixXX A = MatrixXX::Zero(6, m+1);
+ VectorX ub = VectorX::Ones(m + 1) * 1e10;
+ MatrixXX A = MatrixXX::Zero(6, m + 1);
Vector6 Alb = m_D * com + m_d;
Vector6 Aub = Alb;
- A.leftCols(m) = m_G_centr;
+ A.leftCols(m) = m_G_centr;
A.rightCols(1) = m_G_centr * VectorX::Ones(m);
LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_b0);
- if(lpStatus_primal==LP_STATUS_OPTIMAL)
- {
- robustness = convert_b0_to_emax(-1.0*m_solver->getObjectiveValue());
+ if (lpStatus_primal == LP_STATUS_OPTIMAL) {
+ robustness = convert_b0_to_emax(-1.0 * m_solver->getObjectiveValue());
return lpStatus_primal;
}
- SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus_primal));
+ SEND_DEBUG_MSG("Primal LP problem could not be solved: " + toString(lpStatus_primal));
return lpStatus_primal;
}
- if(m_algorithm==EQUILIBRIUM_ALGORITHM_DLP)
- {
+ if (m_algorithm == EQUILIBRIUM_ALGORITHM_DLP) {
/*Compute the robustness measure of the equilibrium of a specified CoM position
by solving the following dual LP:
find v
@@ -323,29 +292,29 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
G is the matrix whose columns are the gravito-inertial wrench generators
*/
Vector6 v;
- Vector6 c = m_D*com + m_d;
- Vector6 lb = Vector6::Ones()*-1e100;
- Vector6 ub = Vector6::Ones()*1e100;
- VectorX Alb = VectorX::Zero(m+1);
+ Vector6 c = m_D * com + m_d;
+ Vector6 lb = Vector6::Ones() * -1e100;
+ Vector6 ub = Vector6::Ones() * 1e100;
+ VectorX Alb = VectorX::Zero(m + 1);
Alb(m) = 1.0;
- VectorX Aub = VectorX::Ones(m+1)*1e100;
+ VectorX Aub = VectorX::Ones(m + 1) * 1e100;
Aub(m) = 1.0;
- MatrixX6 A(m+1,6);
+ MatrixX6 A(m + 1, 6);
A.topRows(m) = m_G_centr.transpose();
- A.bottomRows<1>() = (m_G_centr*VectorX::Ones(m)).transpose();
+ A.bottomRows<1>() = (m_G_centr * VectorX::Ones(m)).transpose();
LP_status lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
- if(lpStatus_dual==LP_STATUS_OPTIMAL)
- {
+ if (lpStatus_dual == LP_STATUS_OPTIMAL) {
robustness = convert_b0_to_emax(m_solver->getObjectiveValue());
return lpStatus_dual;
}
- SEND_DEBUG_MSG("Dual LP problem for com position "+toString(com.transpose())+" could not be solved: "+toString(lpStatus_dual));
+ SEND_DEBUG_MSG("Dual LP problem for com position " + toString(com.transpose()) +
+ " could not be solved: " + toString(lpStatus_dual));
// switch UNFEASIBLE and UNBOUNDED flags because we are solving dual problem
- if(lpStatus_dual==LP_STATUS_INFEASIBLE)
+ if (lpStatus_dual == LP_STATUS_INFEASIBLE)
lpStatus_dual = LP_STATUS_UNBOUNDED;
- else if(lpStatus_dual==LP_STATUS_UNBOUNDED)
+ else if (lpStatus_dual == LP_STATUS_UNBOUNDED)
lpStatus_dual = LP_STATUS_INFEASIBLE;
return lpStatus_dual;
@@ -355,38 +324,33 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
return LP_STATUS_ERROR;
}
-LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max){
- checkRobustEquilibrium(com,zero_acc,equilibrium,e_max);
+LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool& equilibrium, double e_max) {
+ checkRobustEquilibrium(com, zero_acc, equilibrium, e_max);
}
-LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max)
-{
- // Take the acceleration in account in D and d :
- m_D.block<3,3>(3,0) = crossMatrix(-m_mass * (m_gravity - acc));
- m_d.head<3>()= m_mass * (m_gravity - acc);
- m_HD = m_H * m_D;
- m_Hd = m_H * m_d;
+LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool& equilibrium, double e_max) {
+ // Take the acceleration in account in D and d :
+ m_D.block<3, 3>(3, 0) = crossMatrix(-m_mass * (m_gravity - acc));
+ m_d.head<3>() = m_mass * (m_gravity - acc);
+ m_HD = m_H * m_D;
+ m_Hd = m_H * m_d;
- if(m_G_centr.cols()==0)
- {
- equilibrium=false;
+ if (m_G_centr.cols() == 0) {
+ equilibrium = false;
return LP_STATUS_OPTIMAL;
}
- if(e_max!=0.0)
- {
+ if (e_max != 0.0) {
SEND_ERROR_MSG("checkRobustEquilibrium with e_max!=0 not implemented yet");
return LP_STATUS_ERROR;
}
- if(m_algorithm!=EQUILIBRIUM_ALGORITHM_PP)
- {
+ if (m_algorithm != EQUILIBRIUM_ALGORITHM_PP) {
SEND_ERROR_MSG("checkRobustEquilibrium is only implemented for the PP algorithm");
return LP_STATUS_ERROR;
}
VectorX res = m_HD * com + m_Hd;
- for(long i=0; i<res.size(); i++)
- if(res(i)>0.0)
- {
+ for (long i = 0; i < res.size(); i++)
+ if (res(i) > 0.0) {
equilibrium = false;
return LP_STATUS_OPTIMAL;
}
@@ -395,38 +359,30 @@ LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc
return LP_STATUS_OPTIMAL;
}
-
-LP_status Equilibrium::getPolytopeInequalities(MatrixXX& H, VectorX& h) const
-{
- if(m_algorithm!=EQUILIBRIUM_ALGORITHM_PP)
- {
- SEND_ERROR_MSG("getPolytopeInequalities is only implemented for the PP algorithm");
- return LP_STATUS_ERROR;
- }
- if(!m_is_cdd_stable)
- {
- SEND_ERROR_MSG("numerical instability in cddlib");
- return LP_STATUS_ERROR;
- }
- if(m_G_centr.cols()==0)
- {
- return LP_STATUS_INFEASIBLE;
- }
- H = m_H;
- h = m_h;
- return LP_STATUS_OPTIMAL;
+LP_status Equilibrium::getPolytopeInequalities(MatrixXX& H, VectorX& h) const {
+ if (m_algorithm != EQUILIBRIUM_ALGORITHM_PP) {
+ SEND_ERROR_MSG("getPolytopeInequalities is only implemented for the PP algorithm");
+ return LP_STATUS_ERROR;
+ }
+ if (!m_is_cdd_stable) {
+ SEND_ERROR_MSG("numerical instability in cddlib");
+ return LP_STATUS_ERROR;
+ }
+ if (m_G_centr.cols() == 0) {
+ return LP_STATUS_INFEASIBLE;
+ }
+ H = m_H;
+ h = m_h;
+ return LP_STATUS_OPTIMAL;
}
-LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com)
-{
- const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
- if(m_G_centr.cols()==0)
- return LP_STATUS_INFEASIBLE;
+LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com) {
+ const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+ if (m_G_centr.cols() == 0) return LP_STATUS_INFEASIBLE;
double b0 = convert_emax_to_b0(e_max);
- if(m_algorithm==EQUILIBRIUM_ALGORITHM_LP)
- {
+ if (m_algorithm == EQUILIBRIUM_ALGORITHM_LP) {
/* Compute the extremum CoM position over the line a*p + a0 that is in robust equilibrium
* by solving the following LP:
find b, p
@@ -439,46 +395,44 @@ LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, dou
p is the line parameter
G is the matrix whose columns are the gravito-inertial wrench generators
*/
- VectorX b_p(m+1);
- VectorX c = VectorX::Zero(m+1);
+ VectorX b_p(m + 1);
+ VectorX c = VectorX::Zero(m + 1);
c(m) = -1.0;
- VectorX lb = -VectorX::Zero(m+1);
+ VectorX lb = -VectorX::Zero(m + 1);
lb(m) = -1e5;
- VectorX ub = VectorX::Ones(m+1)*1e10;
- Vector6 Alb = m_D*a0 + m_d - m_G_centr*VectorX::Ones(m)*b0;
+ VectorX ub = VectorX::Ones(m + 1) * 1e10;
+ Vector6 Alb = m_D * a0 + m_d - m_G_centr * VectorX::Ones(m) * b0;
Vector6 Aub = Alb;
- Matrix6X A = Matrix6X::Zero(6, m+1);
- A.leftCols(m) = m_G_centr;
- A.rightCols(1) = -m_D*a;
+ Matrix6X A = Matrix6X::Zero(6, m + 1);
+ A.leftCols(m) = m_G_centr;
+ A.rightCols(1) = -m_D * a;
LP_status lpStatus_primal = m_solver->solve(c, lb, ub, A, Alb, Aub, b_p);
- if(lpStatus_primal==LP_STATUS_OPTIMAL)
- {
- com = a0 + a*b_p(m);
+ if (lpStatus_primal == LP_STATUS_OPTIMAL) {
+ com = a0 + a * b_p(m);
//#define WRITE_LPS_TO_FILE
#ifdef WRITE_LPS_TO_FILE
string date_time = getDateAndTimeAsString();
- string filename = "LP_findExtremumOverLine"+date_time+".dat";
- if(!m_solver->writeLpToFile(filename.c_str(), c, lb, ub, A, Alb, Aub))
- SEND_ERROR_MSG("Error while writing LP to file "+filename);
- filename = "LP_findExtremumOverLine"+date_time+"_solution.dat";
- if(!writeMatrixToFile(filename.c_str(), b_p))
- SEND_ERROR_MSG("Error while writing LP solution to file "+filename);
+ string filename = "LP_findExtremumOverLine" + date_time + ".dat";
+ if (!m_solver->writeLpToFile(filename.c_str(), c, lb, ub, A, Alb, Aub))
+ SEND_ERROR_MSG("Error while writing LP to file " + filename);
+ filename = "LP_findExtremumOverLine" + date_time + "_solution.dat";
+ if (!writeMatrixToFile(filename.c_str(), b_p))
+ SEND_ERROR_MSG("Error while writing LP solution to file " + filename);
#endif
return lpStatus_primal;
}
com = a0;
- SEND_DEBUG_MSG("Primal LP problem could not be solved suggesting that no equilibrium position with robustness "+
- toString(e_max)+" exists over the line starting from "+toString(a0.transpose())+
- " in direction "+toString(a.transpose())+", solver error code: "+toString(lpStatus_primal));
+ SEND_DEBUG_MSG("Primal LP problem could not be solved suggesting that no equilibrium position with robustness " +
+ toString(e_max) + " exists over the line starting from " + toString(a0.transpose()) +
+ " in direction " + toString(a.transpose()) + ", solver error code: " + toString(lpStatus_primal));
return lpStatus_primal;
}
- if(m_algorithm==EQUILIBRIUM_ALGORITHM_DLP)
- {
+ if (m_algorithm == EQUILIBRIUM_ALGORITHM_DLP) {
/* Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium
* by solving the following dual LP:
find v
@@ -489,39 +443,37 @@ LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, dou
G is the matrix whose columns are the gravito-inertial wrench generators
*/
Vector6 v;
- Vector6 c = m_D*a0 + m_d - m_G_centr*VectorX::Ones(m)*b0;
- Vector6 lb = Vector6::Ones()*-1e10;
- Vector6 ub = Vector6::Ones()*1e10;
- VectorX Alb = VectorX::Zero(m+1);
+ Vector6 c = m_D * a0 + m_d - m_G_centr * VectorX::Ones(m) * b0;
+ Vector6 lb = Vector6::Ones() * -1e10;
+ Vector6 ub = Vector6::Ones() * 1e10;
+ VectorX Alb = VectorX::Zero(m + 1);
Alb(m) = -1.0;
- VectorX Aub = VectorX::Ones(m+1)*1e10;
+ VectorX Aub = VectorX::Ones(m + 1) * 1e10;
Aub(m) = -1.0;
- MatrixX6 A(m+1,6);
+ MatrixX6 A(m + 1, 6);
A.topRows(m) = m_G_centr.transpose();
- A.bottomRows<1>() = (m_D*a).transpose();
+ A.bottomRows<1>() = (m_D * a).transpose();
LP_status lpStatus_dual = m_solver->solve(c, lb, ub, A, Alb, Aub, v);
- if(lpStatus_dual==LP_STATUS_OPTIMAL)
- {
+ if (lpStatus_dual == LP_STATUS_OPTIMAL) {
double p = m_solver->getObjectiveValue();
- com = a0 + a*p;
+ com = a0 + a * p;
#ifdef WRITE_LPS_TO_FILE
string date_time = getDateAndTimeAsString();
- string filename = "DLP_findExtremumOverLine"+date_time+".dat";
- if(!m_solver->writeLpToFile(filename.c_str(), c, lb, ub, A, Alb, Aub))
- SEND_ERROR_MSG("Error while writing LP to file "+filename);
- filename = "DLP_findExtremumOverLine"+date_time+"_solution.dat";
- if(!writeMatrixToFile(filename.c_str(), v))
- SEND_ERROR_MSG("Error while writing LP solution to file "+filename);
+ string filename = "DLP_findExtremumOverLine" + date_time + ".dat";
+ if (!m_solver->writeLpToFile(filename.c_str(), c, lb, ub, A, Alb, Aub))
+ SEND_ERROR_MSG("Error while writing LP to file " + filename);
+ filename = "DLP_findExtremumOverLine" + date_time + "_solution.dat";
+ if (!writeMatrixToFile(filename.c_str(), v))
+ SEND_ERROR_MSG("Error while writing LP solution to file " + filename);
#endif
// since QP oases cannot detect unboundedness we check here whether the objective value is a large negative value
- if(m_solver_type==SOLVER_LP_QPOASES && p<-1e7)
- {
- SEND_DEBUG_MSG("Dual LP problem with robustness "+toString(e_max)+
- " over the line starting from "+toString(a0.transpose())+
- " in direction "+toString(a.transpose())+" has large negative objective value: "+toString(p)+
+ if (m_solver_type == SOLVER_LP_QPOASES && p < -1e7) {
+ SEND_DEBUG_MSG("Dual LP problem with robustness " + toString(e_max) + " over the line starting from " +
+ toString(a0.transpose()) + " in direction " + toString(a.transpose()) +
+ " has large negative objective value: " + toString(p) +
" suggesting it is probably unbounded.");
lpStatus_dual = LP_STATUS_UNBOUNDED;
}
@@ -530,14 +482,14 @@ LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, dou
}
com = a0;
- SEND_DEBUG_MSG("Dual LP problem could not be solved suggesting that no equilibrium position with robustness "+
- toString(e_max)+" exists over the line starting from "+toString(a0.transpose())+
- " in direction "+toString(a.transpose())+", solver error code: "+toString(lpStatus_dual));
+ SEND_DEBUG_MSG("Dual LP problem could not be solved suggesting that no equilibrium position with robustness " +
+ toString(e_max) + " exists over the line starting from " + toString(a0.transpose()) +
+ " in direction " + toString(a.transpose()) + ", solver error code: " + toString(lpStatus_dual));
// switch UNFEASIBLE and UNBOUNDED flags because we are solving dual problem
- if(lpStatus_dual==LP_STATUS_INFEASIBLE)
+ if (lpStatus_dual == LP_STATUS_INFEASIBLE)
lpStatus_dual = LP_STATUS_UNBOUNDED;
- else if(lpStatus_dual==LP_STATUS_UNBOUNDED)
+ else if (lpStatus_dual == LP_STATUS_UNBOUNDED)
lpStatus_dual = LP_STATUS_INFEASIBLE;
return lpStatus_dual;
@@ -547,48 +499,41 @@ LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, dou
return LP_STATUS_ERROR;
}
-LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max)
-{
- if(m_G_centr.cols()==0)
- return LP_STATUS_INFEASIBLE;
+LP_status Equilibrium::findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max) {
+ if (m_G_centr.cols() == 0) return LP_STATUS_INFEASIBLE;
SEND_ERROR_MSG("findExtremumInDirection not implemented yet");
return LP_STATUS_ERROR;
}
-bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
-{
-
- // todo: for the moment using ad hoc upper bound = 500 N
- int n = (int)v.rows();
- int m = (int)v.cols();
- if (n>m)
- {
- SEND_ERROR_MSG("V has more lines that columns, this case is not handled!");
- return false;
- }
-// getProfiler().start("eigen_to_cdd");
- dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(),canonicalize_cdd_matrix);
-// getProfiler().stop("eigen_to_cdd");
+bool Equilibrium::computePolytopeProjection(Cref_matrix6X v) {
+ // todo: for the moment using ad hoc upper bound = 500 N
+ int n = (int)v.rows();
+ int m = (int)v.cols();
+ if (n > m) {
+ SEND_ERROR_MSG("V has more lines that columns, this case is not handled!");
+ return false;
+ }
+ // getProfiler().start("eigen_to_cdd");
+ dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(), canonicalize_cdd_matrix);
+ // getProfiler().stop("eigen_to_cdd");
dd_ErrorType error = dd_NoError;
m_is_cdd_stable = true;
-// getProfiler().start("dd_DDMatrix2Poly");
- dd_PolyhedraPtr H_= dd_DDMatrix2Poly(V, &error);
-// getProfiler().stop("dd_DDMatrix2Poly");
+ // getProfiler().start("dd_DDMatrix2Poly");
+ dd_PolyhedraPtr H_ = dd_DDMatrix2Poly(V, &error);
+ // getProfiler().stop("dd_DDMatrix2Poly");
- if(error != dd_NoError)
- {
+ if (error != dd_NoError) {
SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
m_is_cdd_stable = false;
return false;
}
-// getProfiler().start("cdd to eigen");
+ // getProfiler().start("cdd to eigen");
dd_MatrixPtr b_A = dd_CopyInequalities(H_);
- if(canonicalize_cdd_matrix)
- {
+ if (canonicalize_cdd_matrix) {
dd_ErrorType error = dd_NoError;
- dd_rowset redset,impl_linset;
+ dd_rowset redset, impl_linset;
dd_rowindex newpos;
dd_MatrixCanonicalize(&b_A, &impl_linset, &redset, &newpos, &error);
set_free(redset);
@@ -597,111 +542,90 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v)
}
// get equalities and add them as complementary inequality constraints
std::vector<long> eq_rows;
- for(long elem=1;elem<=(long)(b_A->linset[0]);++elem)
- {
- if (set_member(elem,b_A->linset))
- eq_rows.push_back(elem);
+ for (long elem = 1; elem <= (long)(b_A->linset[0]); ++elem) {
+ if (set_member(elem, b_A->linset)) eq_rows.push_back(elem);
}
int rowsize = (int)b_A->rowsize;
- m_H.resize(rowsize + eq_rows.size(), (int)b_A->colsize-1);
+ m_H.resize(rowsize + eq_rows.size(), (int)b_A->colsize - 1);
m_h.resize(rowsize + eq_rows.size());
- for(int i=0; i < rowsize; ++i)
- {
+ for (int i = 0; i < rowsize; ++i) {
m_h(i) = (value_type)(*(b_A->matrix[i][0]));
- for(int j=1; j < b_A->colsize; ++j)
- m_H(i, j-1) = -(value_type)(*(b_A->matrix[i][j]));
+ for (int j = 1; j < b_A->colsize; ++j) m_H(i, j - 1) = -(value_type)(*(b_A->matrix[i][j]));
}
int i = 0;
- for(std::vector<long int>::const_iterator cit = eq_rows.begin(); cit != eq_rows.end(); ++cit, ++i)
- {
+ for (std::vector<long int>::const_iterator cit = eq_rows.begin(); cit != eq_rows.end(); ++cit, ++i) {
m_h(rowsize + i) = -m_h((int)(*cit));
m_H(rowsize + i) = -m_H((int)(*cit));
}
-// getProfiler().stop("cdd to eigen");
- if(m_h.rows() < n )
- {
- SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
- m_is_cdd_stable = false;
- return false;
- }
+ // getProfiler().stop("cdd to eigen");
+ if (m_h.rows() < n) {
+ SEND_ERROR_MSG("numerical instability in cddlib. ill formed polytope");
+ m_is_cdd_stable = false;
+ return false;
+ }
return true;
}
-double Equilibrium::convert_b0_to_emax(double b0)
-{
- return (b0*m_b0_to_emax_coefficient);
-}
-
-double Equilibrium::convert_emax_to_b0(double emax)
-{
- return (emax/m_b0_to_emax_coefficient);
-}
-
+double Equilibrium::convert_b0_to_emax(double b0) { return (b0 * m_b0_to_emax_coefficient); }
+double Equilibrium::convert_emax_to_b0(double emax) { return (emax / m_b0_to_emax_coefficient); }
-LP_status Equilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,Cref_vector3 v, double& alpha0){
- int m = (int)m_G_centr.cols(); // 4* number of contacts
+LP_status Equilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 v, double& alpha0) {
+ int m = (int)m_G_centr.cols(); // 4* number of contacts
VectorX b_a0(m);
VectorX c = VectorX::Zero(m);
MatrixXX A = MatrixXX::Zero(6, m);
- A.topLeftCorner(6,m) = - m_G_centr;
- A.topRightCorner(6,1) = H * v;
- c(m-1) = -1.0; // because we search max alpha0, and c = [ B alpha ]^t
+ A.topLeftCorner(6, m) = -m_G_centr;
+ A.topRightCorner(6, 1) = H * v;
+ c(m - 1) = -1.0; // because we search max alpha0, and c = [ B alpha ]^t
VectorX lb = VectorX::Zero(m);
- VectorX ub = VectorX::Ones(m)*1e10; // Inf
+ VectorX ub = VectorX::Ones(m) * 1e10; // Inf
VectorX Alb = -h;
VectorX Aub = -h;
int iter = 0;
LP_status lpStatus;
- do{
+ do {
lpStatus = m_solver->solve(c, lb, ub, A, Alb, Aub, b_a0);
- iter ++;
- }while(lpStatus == LP_STATUS_ERROR && iter < 3);
+ iter++;
+ } while (lpStatus == LP_STATUS_ERROR && iter < 3);
-
- if(lpStatus==LP_STATUS_UNBOUNDED){
- //SEND_DEBUG_MSG("Primal LP problem is unbounded : "+toString(lpStatus));
+ if (lpStatus == LP_STATUS_UNBOUNDED) {
+ // SEND_DEBUG_MSG("Primal LP problem is unbounded : "+toString(lpStatus));
alpha0 = std::numeric_limits<double>::infinity();
return lpStatus;
}
- if(lpStatus==LP_STATUS_OPTIMAL)
- {
+ if (lpStatus == LP_STATUS_OPTIMAL) {
alpha0 = -1.0 * m_solver->getObjectiveValue();
return lpStatus;
}
alpha0 = 0.0;
- //SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
+ // SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
return lpStatus;
-
}
-
-bool Equilibrium::checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a ){
- int m = (int)m_G_centr.cols(); // number of contact * generator per contacts
+bool Equilibrium::checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a) {
+ int m = (int)m_G_centr.cols(); // number of contact * generator per contacts
VectorX b(m);
VectorX c = VectorX::Zero(m);
VectorX lb = VectorX::Zero(m);
- VectorX ub = VectorX::Ones(m)*1e10; // Inf
- VectorX Alb = H*a + h;
- VectorX Aub = H*a + h;
+ VectorX ub = VectorX::Ones(m) * 1e10; // Inf
+ VectorX Alb = H * a + h;
+ VectorX Aub = H * a + h;
int iter = 0;
LP_status lpStatus;
- do{
+ do {
lpStatus = m_solver->solve(c, lb, ub, m_G_centr, Alb, Aub, b);
- iter ++;
- }while(lpStatus == LP_STATUS_ERROR && iter < 3);
+ iter++;
+ } while (lpStatus == LP_STATUS_ERROR && iter < 3);
- if(lpStatus==LP_STATUS_OPTIMAL || lpStatus==LP_STATUS_UNBOUNDED)
- {
+ if (lpStatus == LP_STATUS_OPTIMAL || lpStatus == LP_STATUS_UNBOUNDED) {
return true;
- }
- else{
- //SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
+ } else {
+ // SEND_DEBUG_MSG("Primal LP problem could not be solved: "+toString(lpStatus));
return false;
}
}
-
-} // end namespace centroidal_dynamics
+} // end namespace centroidal_dynamics
diff --git a/src/logger.cpp b/src/logger.cpp
index f96b0a7..eed2faa 100644
--- a/src/logger.cpp
+++ b/src/logger.cpp
@@ -11,107 +11,91 @@
#include <stdio.h>
#include <iostream>
-#include <iomanip> // std::setprecision
+#include <iomanip> // std::setprecision
#include <boost/algorithm/string.hpp>
#include <hpp/centroidal-dynamics/logger.hh>
-namespace centroidal_dynamics
-{
- using namespace std;
+namespace centroidal_dynamics {
+using namespace std;
- Logger& getLogger()
- {
- static Logger l(0.001, 1.0);
- return l;
- }
+Logger& getLogger() {
+ static Logger l(0.001, 1.0);
+ return l;
+}
- Logger::Logger(double timeSample, double streamPrintPeriod)
- : m_timeSample(timeSample),
- m_streamPrintPeriod(streamPrintPeriod),
- m_printCountdown(0.0)
- {
+Logger::Logger(double timeSample, double streamPrintPeriod)
+ : m_timeSample(timeSample), m_streamPrintPeriod(streamPrintPeriod), m_printCountdown(0.0) {
#ifdef LOGGER_VERBOSITY_ERROR
- m_lv = VERBOSITY_ERROR;
+ m_lv = VERBOSITY_ERROR;
#endif
#ifdef LOGGER_VERBOSITY_WARNING_ERROR
- m_lv = VERBOSITY_WARNING_ERROR;
+ m_lv = VERBOSITY_WARNING_ERROR;
#endif
#ifdef LOGGER_VERBOSITY_INFO_WARNING_ERROR
- m_lv = VERBOSITY_INFO_WARNING_ERROR;
+ m_lv = VERBOSITY_INFO_WARNING_ERROR;
#endif
#ifdef LOGGER_VERBOSITY_ALL
- m_lv = VERBOSITY_ALL;
+ m_lv = VERBOSITY_ALL;
#endif
- }
+}
- void Logger::countdown()
- {
- if(m_printCountdown<0.0)
- m_printCountdown = m_streamPrintPeriod;
- m_printCountdown -= m_timeSample;
- }
+void Logger::countdown() {
+ if (m_printCountdown < 0.0) m_printCountdown = m_streamPrintPeriod;
+ m_printCountdown -= m_timeSample;
+}
- void Logger::sendMsg(string msg, MsgType type, const char* file, int line)
- {
-// if(m_lv==VERBOSITY_NONE ||
-// (m_lv==VERBOSITY_ERROR && !isErrorMsg(type)) ||
-// (m_lv==VERBOSITY_WARNING_ERROR && !(isWarningMsg(type) || isErrorMsg(type))) ||
-// (m_lv==VERBOSITY_INFO_WARNING_ERROR && isDebugMsg(type)))
-// return;
+void Logger::sendMsg(string msg, MsgType type, const char* file, int line) {
+ // if(m_lv==VERBOSITY_NONE ||
+ // (m_lv==VERBOSITY_ERROR && !isErrorMsg(type)) ||
+ // (m_lv==VERBOSITY_WARNING_ERROR && !(isWarningMsg(type) || isErrorMsg(type))) ||
+ // (m_lv==VERBOSITY_INFO_WARNING_ERROR && isDebugMsg(type)))
+ // return;
- // if print is allowed by current verbosity level
- if(isStreamMsg(type))
- {
- // check whether counter already exists
- string id = file+toString(line);
- map<string,double>::iterator it = m_stream_msg_counters.find(id);
- if(it == m_stream_msg_counters.end())
- {
- // if counter doesn't exist then add one
- m_stream_msg_counters.insert(make_pair(id, 0.0));
- it = m_stream_msg_counters.find(id);
- }
-
- // if counter is greater than 0 then decrement it and do not print
- if(it->second>0.0)
- {
- it->second -= m_timeSample;
- return;
- }
- else // otherwise reset counter and print
- it->second = m_streamPrintPeriod;
+ // if print is allowed by current verbosity level
+ if (isStreamMsg(type)) {
+ // check whether counter already exists
+ string id = file + toString(line);
+ map<string, double>::iterator it = m_stream_msg_counters.find(id);
+ if (it == m_stream_msg_counters.end()) {
+ // if counter doesn't exist then add one
+ m_stream_msg_counters.insert(make_pair(id, 0.0));
+ it = m_stream_msg_counters.find(id);
}
- vector<string> fields;
- boost::split(fields, file, boost::is_any_of("/"));
- const char* file_name = fields[fields.size()-1].c_str();
+ // if counter is greater than 0 then decrement it and do not print
+ if (it->second > 0.0) {
+ it->second -= m_timeSample;
+ return;
+ } else // otherwise reset counter and print
+ it->second = m_streamPrintPeriod;
+ }
- if(isErrorMsg(type))
- printf("[ERROR %s %d] %s\n", file_name, line, msg.c_str());
- else if(isWarningMsg(type))
- printf("[WARNING %s %d] %s\n", file_name, line, msg.c_str());
- else if(isInfoMsg(type))
- printf("[INFO %s %d] %s\n", file_name, line, msg.c_str());
- else
- printf("[DEBUG %s %d] %s\n", file_name, line, msg.c_str());
+ vector<string> fields;
+ boost::split(fields, file, boost::is_any_of("/"));
+ const char* file_name = fields[fields.size() - 1].c_str();
- fflush(stdout); // Prints to screen or whatever your standard out is
- }
+ if (isErrorMsg(type))
+ printf("[ERROR %s %d] %s\n", file_name, line, msg.c_str());
+ else if (isWarningMsg(type))
+ printf("[WARNING %s %d] %s\n", file_name, line, msg.c_str());
+ else if (isInfoMsg(type))
+ printf("[INFO %s %d] %s\n", file_name, line, msg.c_str());
+ else
+ printf("[DEBUG %s %d] %s\n", file_name, line, msg.c_str());
- bool Logger::setTimeSample(double t)
- {
- if(t<=0.0)
- return false;
- m_timeSample = t;
- return true;
- }
+ fflush(stdout); // Prints to screen or whatever your standard out is
+}
- bool Logger::setStreamPrintPeriod(double s)
- {
- if(s<=0.0)
- return false;
- m_streamPrintPeriod = s;
- return true;
- }
+bool Logger::setTimeSample(double t) {
+ if (t <= 0.0) return false;
+ m_timeSample = t;
+ return true;
+}
+
+bool Logger::setStreamPrintPeriod(double s) {
+ if (s <= 0.0) return false;
+ m_streamPrintPeriod = s;
+ return true;
+}
-} // namespace centroidal_dynamics
+} // namespace centroidal_dynamics
diff --git a/src/solver_LP_abstract.cpp b/src/solver_LP_abstract.cpp
index d7026d9..1fd47b4 100644
--- a/src/solver_LP_abstract.cpp
+++ b/src/solver_LP_abstract.cpp
@@ -12,100 +12,85 @@
#include <hpp/centroidal-dynamics/solver_LP_clp.hh>
#endif
-
using namespace std;
-namespace centroidal_dynamics
-{
+namespace centroidal_dynamics {
-Solver_LP_abstract* Solver_LP_abstract::getNewSolver(SolverLP solverType)
-{
- if(solverType==SOLVER_LP_QPOASES)
- return new Solver_LP_qpoases();
+Solver_LP_abstract *Solver_LP_abstract::getNewSolver(SolverLP solverType) {
+ if (solverType == SOLVER_LP_QPOASES) return new Solver_LP_qpoases();
#ifdef CLP_FOUND
- if(solverType==SOLVER_LP_CLP)
- return new Solver_LP_clp();
+ if (solverType == SOLVER_LP_CLP) return new Solver_LP_clp();
#endif
- SEND_ERROR_MSG("Specified solver type not recognized: "+toString(solverType));
+ SEND_ERROR_MSG("Specified solver type not recognized: " + toString(solverType));
return NULL;
}
-bool Solver_LP_abstract::writeLpToFile(const std::string& filename,
- Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
- Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub)
-{
- MatrixXX::Index n=c.size(), m=A.rows();
- assert(lb.size()==n);
- assert(ub.size()==n);
- assert(A.cols()==n);
- assert(Alb.size()==m);
- assert(Aub.size()==m);
+bool Solver_LP_abstract::writeLpToFile(const std::string &filename, Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
+ Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub) {
+ MatrixXX::Index n = c.size(), m = A.rows();
+ assert(lb.size() == n);
+ assert(ub.size() == n);
+ assert(A.cols() == n);
+ assert(Alb.size() == m);
+ assert(Aub.size() == m);
std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
- out.write((char*) (&n), sizeof(typename MatrixXX::Index));
- out.write((char*) (&m), sizeof(typename MatrixXX::Index));
- out.write((char*) c.data(), n*sizeof(typename MatrixXX::Scalar) );
- out.write((char*) lb.data(), n*sizeof(typename MatrixXX::Scalar) );
- out.write((char*) ub.data(), n*sizeof(typename MatrixXX::Scalar) );
- out.write((char*) A.data(), m*n*sizeof(typename MatrixXX::Scalar) );
- out.write((char*) Alb.data(), m*sizeof(typename MatrixXX::Scalar) );
- out.write((char*) Aub.data(), m*sizeof(typename MatrixXX::Scalar) );
+ out.write((char *)(&n), sizeof(typename MatrixXX::Index));
+ out.write((char *)(&m), sizeof(typename MatrixXX::Index));
+ out.write((char *)c.data(), n * sizeof(typename MatrixXX::Scalar));
+ out.write((char *)lb.data(), n * sizeof(typename MatrixXX::Scalar));
+ out.write((char *)ub.data(), n * sizeof(typename MatrixXX::Scalar));
+ out.write((char *)A.data(), m * n * sizeof(typename MatrixXX::Scalar));
+ out.write((char *)Alb.data(), m * sizeof(typename MatrixXX::Scalar));
+ out.write((char *)Aub.data(), m * sizeof(typename MatrixXX::Scalar));
out.close();
return true;
}
-bool Solver_LP_abstract::readLpFromFile(const std::string& filename,
- VectorX &c, VectorX &lb, VectorX &ub,
- MatrixXX &A, VectorX &Alb, VectorX &Aub)
-{
+bool Solver_LP_abstract::readLpFromFile(const std::string &filename, VectorX &c, VectorX &lb, VectorX &ub, MatrixXX &A,
+ VectorX &Alb, VectorX &Aub) {
std::ifstream in(filename.c_str(), std::ios::in | std::ios::binary);
- typename MatrixXX::Index n=0, m=0;
- in.read((char*) (&n),sizeof(typename MatrixXX::Index));
- in.read((char*) (&m),sizeof(typename MatrixXX::Index));
+ typename MatrixXX::Index n = 0, m = 0;
+ in.read((char *)(&n), sizeof(typename MatrixXX::Index));
+ in.read((char *)(&m), sizeof(typename MatrixXX::Index));
c.resize(n);
lb.resize(n);
ub.resize(n);
- A.resize(m,n);
+ A.resize(m, n);
Alb.resize(m);
Aub.resize(m);
- in.read( (char *) c.data() , n*sizeof(typename MatrixXX::Scalar) );
- in.read( (char *) lb.data() , n*sizeof(typename MatrixXX::Scalar) );
- in.read( (char *) ub.data() , n*sizeof(typename MatrixXX::Scalar) );
- in.read( (char *) A.data() , m*n*sizeof(typename MatrixXX::Scalar) );
- in.read( (char *) Alb.data() , m*sizeof(typename MatrixXX::Scalar) );
- in.read( (char *) Aub.data() , m*sizeof(typename MatrixXX::Scalar) );
+ in.read((char *)c.data(), n * sizeof(typename MatrixXX::Scalar));
+ in.read((char *)lb.data(), n * sizeof(typename MatrixXX::Scalar));
+ in.read((char *)ub.data(), n * sizeof(typename MatrixXX::Scalar));
+ in.read((char *)A.data(), m * n * sizeof(typename MatrixXX::Scalar));
+ in.read((char *)Alb.data(), m * sizeof(typename MatrixXX::Scalar));
+ in.read((char *)Aub.data(), m * sizeof(typename MatrixXX::Scalar));
in.close();
return true;
}
-LP_status Solver_LP_abstract::solve(const std::string& filename, Ref_vectorX sol)
-{
+LP_status Solver_LP_abstract::solve(const std::string &filename, Ref_vectorX sol) {
VectorX c, lb, ub, Alb, Aub;
MatrixXX A;
- if(!readLpFromFile(filename, c, lb, ub, A, Alb, Aub))
- {
- SEND_ERROR_MSG("Error while reading LP from file "+string(filename));
+ if (!readLpFromFile(filename, c, lb, ub, A, Alb, Aub)) {
+ SEND_ERROR_MSG("Error while reading LP from file " + string(filename));
return LP_STATUS_ERROR;
}
return solve(c, lb, ub, A, Alb, Aub, sol);
}
-bool Solver_LP_abstract::setMaximumIterations(unsigned int maxIter)
-{
- if(maxIter==0)
- return false;
+bool Solver_LP_abstract::setMaximumIterations(unsigned int maxIter) {
+ if (maxIter == 0) return false;
m_maxIter = maxIter;
return true;
}
-bool Solver_LP_abstract::setMaximumTime(double seconds)
-{
- if(seconds<=0.0)
- return false;
+bool Solver_LP_abstract::setMaximumTime(double seconds) {
+ if (seconds <= 0.0) return false;
m_maxTime = seconds;
return true;
}
-} // end namespace centroidal_dynamics
+} // end namespace centroidal_dynamics
diff --git a/src/solver_LP_clp.cpp b/src/solver_LP_clp.cpp
index e221764..12fbcfb 100644
--- a/src/solver_LP_clp.cpp
+++ b/src/solver_LP_clp.cpp
@@ -8,114 +8,92 @@
#include <hpp/centroidal-dynamics/solver_LP_clp.hh>
#include "CoinBuild.hpp"
-namespace centroidal_dynamics
-{
+namespace centroidal_dynamics {
-Solver_LP_clp::Solver_LP_clp(): Solver_LP_abstract()
-{
- m_model.setLogLevel(0);
-}
+Solver_LP_clp::Solver_LP_clp() : Solver_LP_abstract() { m_model.setLogLevel(0); }
-LP_status Solver_LP_clp::solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
- Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
- Ref_vectorX sol)
-{
+LP_status Solver_LP_clp::solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub, Cref_matrixXX A, Cref_vectorX Alb,
+ Cref_vectorX Aub, Ref_vectorX sol) {
int n = (int)c.size(); // number of variables
int m = (int)A.rows(); // number of constraints
- assert(lb.size()==n);
- assert(ub.size()==n);
- assert(A.cols()==n);
- assert(Alb.size()==m);
- assert(Aub.size()==m);
+ assert(lb.size() == n);
+ assert(ub.size() == n);
+ assert(A.cols() == n);
+ assert(Alb.size() == m);
+ assert(Aub.size() == m);
m_model.resize(0, n);
- int* rowIndex = new int[n];
+ int *rowIndex = new int[n];
- for(int i=0; i<n; i++)
- {
+ for (int i = 0; i < n; i++) {
m_model.setObjectiveCoefficient(i, c(i));
m_model.setColumnLower(i, lb(i));
m_model.setColumnUpper(i, ub(i));
rowIndex[i] = i;
}
-// m_model.allSlackBasis();
+ // m_model.allSlackBasis();
// This is not the most efficient way to pass the data to the problem
// but it is the best compromise between efficiency and simplicity.
// We could be more efficient by using CoinPackedMatrix and ClpPackedMatrix
// as shown in the example file "addRows.cpp"
CoinBuild buildObject;
- for (int i=0; i<m; i++)
- {
+ for (int i = 0; i < m; i++) {
buildObject.addRow(n, rowIndex, A.row(i).data(), Alb(i), Aub(i));
}
m_model.addRows(buildObject);
// solve the problem
m_model.primal();
-// m_model.dual();
+ // m_model.dual();
- if(m_model.isProvenOptimal())
- {
+ if (m_model.isProvenOptimal()) {
const double *solution = m_model.getColSolution();
- for(int i=0; i<n; i++)
- sol(i) = solution[i];
+ for (int i = 0; i < n; i++) sol(i) = solution[i];
}
return getStatus();
}
-LP_status Solver_LP_clp::getStatus()
-{
+LP_status Solver_LP_clp::getStatus() {
int status = m_model.status();
- if(status<5)
- return (LP_status)status;
+ if (status < 5) return (LP_status)status;
return LP_STATUS_ERROR;
}
-double Solver_LP_clp::getObjectiveValue()
-{
- return m_model.objectiveValue();
-}
+double Solver_LP_clp::getObjectiveValue() { return m_model.objectiveValue(); }
-void Solver_LP_clp::getDualSolution(Ref_vectorX res)
-{
+void Solver_LP_clp::getDualSolution(Ref_vectorX res) {
const double *tmp = m_model.dualRowSolution();
- for(int i=0; i<res.size(); i++)
- res(i) = tmp[i];
+ for (int i = 0; i < res.size(); i++) res(i) = tmp[i];
}
-//void Solver_LP_clp::getDualColumnSolution(Ref_vectorX res)
+// void Solver_LP_clp::getDualColumnSolution(Ref_vectorX res)
//{
// const double *tmp = m_model.dualColumnSolution();
// for(int i=0; i<res.size(); i++)
// res(i) = tmp[i];
//}
-unsigned int Solver_LP_clp::getMaximumIterations()
-{
+unsigned int Solver_LP_clp::getMaximumIterations() {
int integerValue;
m_model.getIntParam(ClpMaxNumIteration, integerValue);
return integerValue;
}
-bool Solver_LP_clp::setMaximumIterations(unsigned int maxIter)
-{
- if(maxIter==0)
- return false;
+bool Solver_LP_clp::setMaximumIterations(unsigned int maxIter) {
+ if (maxIter == 0) return false;
m_model.setMaximumIterations(maxIter);
return true;
}
-bool Solver_LP_clp::setMaximumTime(double seconds)
-{
- if(seconds<=0.0)
- return false;
+bool Solver_LP_clp::setMaximumTime(double seconds) {
+ if (seconds <= 0.0) return false;
m_model.setMaximumSeconds(seconds);
return true;
}
-} // end namespace centroidal_dynamics
+} // end namespace centroidal_dynamics
-#endif //CLP_FOUND
+#endif // CLP_FOUND
diff --git a/src/solver_LP_qpoases.cpp b/src/solver_LP_qpoases.cpp
index b443dcc..0c25aa5 100644
--- a/src/solver_LP_qpoases.cpp
+++ b/src/solver_LP_qpoases.cpp
@@ -8,78 +8,62 @@
USING_NAMESPACE_QPOASES
-namespace centroidal_dynamics
-{
+namespace centroidal_dynamics {
- Solver_LP_qpoases::Solver_LP_qpoases(): Solver_LP_abstract()
- {
-// m_options.initialStatusBounds = ST_INACTIVE;
-// m_options.setToReliable();
- m_options.setToDefault();
- m_options.printLevel = PL_NONE; //PL_LOW
- m_options.enableRegularisation = BT_TRUE;
- m_options.enableEqualities = BT_TRUE;
- }
-
- LP_status Solver_LP_qpoases::solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub,
- Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub,
- Ref_vectorX sol)
- {
- int n = (int)c.size(); // number of variables
- int m = (int)A.rows(); // number of constraints
- assert(lb.size()==n);
- assert(ub.size()==n);
- assert(A.cols()==n);
- assert(Alb.size()==m);
- assert(Aub.size()==m);
-
- int iters = m_maxIter;
- double solutionTime = m_maxTime;
- if(n!=m_solver.getNV() || m!=m_solver.getNC())
- {
- m_solver = SQProblem(n, m, HST_ZERO);
- m_solver.setOptions(m_options);
-// m_solver.printOptions();
- m_init_succeeded = false;
- m_H = MatrixXX::Zero(n,n);
- }
+Solver_LP_qpoases::Solver_LP_qpoases() : Solver_LP_abstract() {
+ // m_options.initialStatusBounds = ST_INACTIVE;
+ // m_options.setToReliable();
+ m_options.setToDefault();
+ m_options.printLevel = PL_NONE; // PL_LOW
+ m_options.enableRegularisation = BT_TRUE;
+ m_options.enableEqualities = BT_TRUE;
+}
- if(!m_useWarmStart || !m_init_succeeded)
- {
- m_status = m_solver.init(NULL, c.data(), A.data(), lb.data(), ub.data(),
- Alb.data(), Aub.data(), iters, &solutionTime);
- if(m_status==SUCCESSFUL_RETURN)
- m_init_succeeded = true;
- }
- else
- {
- // this doesn't work if I pass NULL instead of m_H.data()
- m_status = m_solver.hotstart(m_H.data(), c.data(), A.data(), lb.data(), ub.data(),
- Alb.data(), Aub.data(), iters, &solutionTime);
- if(m_status!=SUCCESSFUL_RETURN)
- m_init_succeeded = false;
- }
+LP_status Solver_LP_qpoases::solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX ub, Cref_matrixXX A, Cref_vectorX Alb,
+ Cref_vectorX Aub, Ref_vectorX sol) {
+ int n = (int)c.size(); // number of variables
+ int m = (int)A.rows(); // number of constraints
+ assert(lb.size() == n);
+ assert(ub.size() == n);
+ assert(A.cols() == n);
+ assert(Alb.size() == m);
+ assert(Aub.size() == m);
- if(m_status==SUCCESSFUL_RETURN)
- {
- m_solver.getPrimalSolution(sol.data());
- }
+ int iters = m_maxIter;
+ double solutionTime = m_maxTime;
+ if (n != m_solver.getNV() || m != m_solver.getNC()) {
+ m_solver = SQProblem(n, m, HST_ZERO);
+ m_solver.setOptions(m_options);
+ // m_solver.printOptions();
+ m_init_succeeded = false;
+ m_H = MatrixXX::Zero(n, n);
+ }
- return getStatus();
+ if (!m_useWarmStart || !m_init_succeeded) {
+ m_status =
+ m_solver.init(NULL, c.data(), A.data(), lb.data(), ub.data(), Alb.data(), Aub.data(), iters, &solutionTime);
+ if (m_status == SUCCESSFUL_RETURN) m_init_succeeded = true;
+ } else {
+ // this doesn't work if I pass NULL instead of m_H.data()
+ m_status = m_solver.hotstart(m_H.data(), c.data(), A.data(), lb.data(), ub.data(), Alb.data(), Aub.data(), iters,
+ &solutionTime);
+ if (m_status != SUCCESSFUL_RETURN) m_init_succeeded = false;
}
- LP_status Solver_LP_qpoases::getStatus()
- {
- int ss = getSimpleStatus(m_status);
- if(ss==0)
- return LP_STATUS_OPTIMAL;
- if(ss==1)
- return LP_STATUS_MAX_ITER_REACHED;
- if(ss==-2)
- return LP_STATUS_INFEASIBLE;
- if(ss==-3)
- return LP_STATUS_UNBOUNDED;
- return LP_STATUS_ERROR;
+ if (m_status == SUCCESSFUL_RETURN) {
+ m_solver.getPrimalSolution(sol.data());
}
-} // end namespace centroidal_dynamics
+ return getStatus();
+}
+
+LP_status Solver_LP_qpoases::getStatus() {
+ int ss = getSimpleStatus(m_status);
+ if (ss == 0) return LP_STATUS_OPTIMAL;
+ if (ss == 1) return LP_STATUS_MAX_ITER_REACHED;
+ if (ss == -2) return LP_STATUS_INFEASIBLE;
+ if (ss == -3) return LP_STATUS_UNBOUNDED;
+ return LP_STATUS_ERROR;
+}
+
+} // end namespace centroidal_dynamics
diff --git a/src/stop-watch.cpp b/src/stop-watch.cpp
index 47cd7a8..ebeb47a 100644
--- a/src/stop-watch.cpp
+++ b/src/stop-watch.cpp
@@ -27,55 +27,40 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include "hpp/centroidal-dynamics/Stdafx.hh"
#ifndef WIN32
- #include <sys/time.h>
+#include <sys/time.h>
#else
- #include <Windows.h>
- #include <iomanip>
+#include <Windows.h>
+#include <iomanip>
#endif
-#include <iomanip> // std::setprecision
+#include <iomanip> // std::setprecision
#include "hpp/centroidal-dynamics/stop-watch.hh"
using std::map;
-using std::string;
using std::ostringstream;
+using std::string;
-Stopwatch& getProfiler()
-{
- static Stopwatch s(REAL_TIME); // alternatives are CPU_TIME and REAL_TIME
+Stopwatch& getProfiler() {
+ static Stopwatch s(REAL_TIME); // alternatives are CPU_TIME and REAL_TIME
return s;
}
-Stopwatch::Stopwatch(StopwatchMode _mode)
- : active(true), mode(_mode)
-{
+Stopwatch::Stopwatch(StopwatchMode _mode) : active(true), mode(_mode) {
records_of = new map<string, PerformanceData>();
}
-Stopwatch::~Stopwatch()
-{
- delete records_of;
-}
-
-void Stopwatch::set_mode(StopwatchMode new_mode)
-{
- mode = new_mode;
-}
+Stopwatch::~Stopwatch() { delete records_of; }
-bool Stopwatch::performance_exists(string perf_name)
-{
- return (records_of->find(perf_name) != records_of->end());
-}
+void Stopwatch::set_mode(StopwatchMode new_mode) { mode = new_mode; }
-long double Stopwatch::take_time()
-{
- if ( mode == CPU_TIME ) {
+bool Stopwatch::performance_exists(string perf_name) { return (records_of->find(perf_name) != records_of->end()); }
+long double Stopwatch::take_time() {
+ if (mode == CPU_TIME) {
// Use ctime
return clock();
- } else if ( mode == REAL_TIME ) {
-
+ } else if (mode == REAL_TIME) {
// Query operating system
#ifdef WIN32
@@ -90,8 +75,8 @@ long double Stopwatch::take_time()
intervals.HighPart = ft.dwHighDateTime;
long double measure = intervals.QuadPart;
- measure -= 116444736000000000.0; // Convert to UNIX epoch time
- measure /= 10000000.0; // Convert to seconds
+ measure -= 116444736000000000.0; // Convert to UNIX epoch time
+ measure /= 10000000.0; // Convert to seconds
return measure;
#else
@@ -100,8 +85,8 @@ long double Stopwatch::take_time()
gettimeofday(&tv, NULL);
long double measure = tv.tv_usec;
- measure /= 1000000.0; // Convert to seconds
- measure += tv.tv_sec; // Add seconds part
+ measure /= 1000000.0; // Convert to seconds
+ measure += tv.tv_sec; // Add seconds part
return measure;
#endif
@@ -112,8 +97,7 @@ long double Stopwatch::take_time()
}
}
-void Stopwatch::start(string perf_name)
-{
+void Stopwatch::start(string perf_name) {
if (!active) return;
// Just works if not already present
@@ -125,63 +109,56 @@ void Stopwatch::start(string perf_name)
perf_info.clock_start = take_time();
// If this is a new start (i.e. not a restart)
-// if (!perf_info.paused)
-// perf_info.last_time = 0;
+ // if (!perf_info.paused)
+ // perf_info.last_time = 0;
perf_info.paused = false;
}
-void Stopwatch::stop(string perf_name)
-{
+void Stopwatch::stop(string perf_name) {
if (!active) return;
long double clock_end = take_time();
// Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
+ if (!performance_exists(perf_name)) throw StopwatchException("Performance not initialized.");
PerformanceData& perf_info = records_of->find(perf_name)->second;
perf_info.stops++;
- long double lapse = clock_end - perf_info.clock_start;
+ long double lapse = clock_end - perf_info.clock_start;
- if ( mode == CPU_TIME )
- lapse /= (double) CLOCKS_PER_SEC;
+ if (mode == CPU_TIME) lapse /= (double)CLOCKS_PER_SEC;
// Update last time
perf_info.last_time = lapse;
// Update min/max time
- if ( lapse >= perf_info.max_time ) perf_info.max_time = lapse;
- if ( lapse <= perf_info.min_time || perf_info.min_time == 0 )
- perf_info.min_time = lapse;
+ if (lapse >= perf_info.max_time) perf_info.max_time = lapse;
+ if (lapse <= perf_info.min_time || perf_info.min_time == 0) perf_info.min_time = lapse;
// Update total time
perf_info.total_time += lapse;
}
-void Stopwatch::pause(string perf_name)
-{
+void Stopwatch::pause(string perf_name) {
if (!active) return;
- long double clock_end = clock();
+ long double clock_end = clock();
// Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
+ if (!performance_exists(perf_name)) throw StopwatchException("Performance not initialized.");
PerformanceData& perf_info = records_of->find(perf_name)->second;
- long double lapse = clock_end - perf_info.clock_start;
+ long double lapse = clock_end - perf_info.clock_start;
// Update total time
perf_info.last_time += lapse;
perf_info.total_time += lapse;
}
-void Stopwatch::reset_all()
-{
+void Stopwatch::reset_all() {
if (!active) return;
map<string, PerformanceData>::iterator it;
@@ -191,24 +168,21 @@ void Stopwatch::reset_all()
}
}
-void Stopwatch::report_all(int precision, std::ostream& output)
-{
+void Stopwatch::report_all(int precision, std::ostream& output) {
if (!active) return;
- output<< "\n*** PROFILING RESULTS [ms] (min - avg - max - lastTime - nSamples) ***\n";
+ output << "\n*** PROFILING RESULTS [ms] (min - avg - max - lastTime - nSamples) ***\n";
map<string, PerformanceData>::iterator it;
for (it = records_of->begin(); it != records_of->end(); ++it) {
report(it->first, precision, output);
}
}
-void Stopwatch::reset(string perf_name)
-{
+void Stopwatch::reset(string perf_name) {
if (!active) return;
// Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
+ if (!performance_exists(perf_name)) throw StopwatchException("Performance not initialized.");
PerformanceData& perf_info = records_of->find(perf_name)->second;
@@ -221,44 +195,35 @@ void Stopwatch::reset(string perf_name)
perf_info.stops = 0;
}
-void Stopwatch::turn_on()
-{
+void Stopwatch::turn_on() {
std::cout << "Stopwatch active." << std::endl;
active = true;
}
-void Stopwatch::turn_off()
-{
+void Stopwatch::turn_off() {
std::cout << "Stopwatch inactive." << std::endl;
- active = false;
+ active = false;
}
-void Stopwatch::report(string perf_name, int precision, std::ostream& output)
-{
+void Stopwatch::report(string perf_name, int precision, std::ostream& output) {
if (!active) return;
// Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
+ if (!performance_exists(perf_name)) throw StopwatchException("Performance not initialized.");
PerformanceData& perf_info = records_of->find(perf_name)->second;
const int MAX_NAME_LENGTH = 60;
string pad = "";
- for (int i = perf_name.length(); i<MAX_NAME_LENGTH; i++)
- pad.append(" ");
+ for (int i = perf_name.length(); i < MAX_NAME_LENGTH; i++) pad.append(" ");
output << perf_name << pad;
- output << std::fixed << std::setprecision(precision)
- << (perf_info.min_time*1e3) << "\t";
- output << std::fixed << std::setprecision(precision)
- << (perf_info.total_time*1e3 / (long double) perf_info.stops) << "\t";
- output << std::fixed << std::setprecision(precision)
- << (perf_info.max_time*1e3) << "\t";
- output << std::fixed << std::setprecision(precision)
- << (perf_info.last_time*1e3) << "\t";
- output << std::fixed << std::setprecision(precision)
- << perf_info.stops << std::endl;
+ output << std::fixed << std::setprecision(precision) << (perf_info.min_time * 1e3) << "\t";
+ output << std::fixed << std::setprecision(precision) << (perf_info.total_time * 1e3 / (long double)perf_info.stops)
+ << "\t";
+ output << std::fixed << std::setprecision(precision) << (perf_info.max_time * 1e3) << "\t";
+ output << std::fixed << std::setprecision(precision) << (perf_info.last_time * 1e3) << "\t";
+ output << std::fixed << std::setprecision(precision) << perf_info.stops << std::endl;
// ostringstream stops;
// stops << perf_info.stops;
@@ -266,74 +231,56 @@ void Stopwatch::report(string perf_name, int precision, std::ostream& output)
// output << std::endl;
}
-long double Stopwatch::get_time_so_far(string perf_name)
-{
+long double Stopwatch::get_time_so_far(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
+ if (!performance_exists(perf_name)) throw StopwatchException("Performance not initialized.");
- long double lapse =
- (take_time() - (records_of->find(perf_name)->second).clock_start);
+ long double lapse = (take_time() - (records_of->find(perf_name)->second).clock_start);
- if (mode == CPU_TIME)
- lapse /= (double) CLOCKS_PER_SEC;
+ if (mode == CPU_TIME) lapse /= (double)CLOCKS_PER_SEC;
return lapse;
}
-long double Stopwatch::get_total_time(string perf_name)
-{
+long double Stopwatch::get_total_time(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
+ if (!performance_exists(perf_name)) throw StopwatchException("Performance not initialized.");
PerformanceData& perf_info = records_of->find(perf_name)->second;
return perf_info.total_time;
-
}
-long double Stopwatch::get_average_time(string perf_name)
-{
+long double Stopwatch::get_average_time(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
+ if (!performance_exists(perf_name)) throw StopwatchException("Performance not initialized.");
PerformanceData& perf_info = records_of->find(perf_name)->second;
return (perf_info.total_time / (long double)perf_info.stops);
-
}
-long double Stopwatch::get_min_time(string perf_name)
-{
+long double Stopwatch::get_min_time(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
+ if (!performance_exists(perf_name)) throw StopwatchException("Performance not initialized.");
PerformanceData& perf_info = records_of->find(perf_name)->second;
return perf_info.min_time;
-
}
-long double Stopwatch::get_max_time(string perf_name)
-{
+long double Stopwatch::get_max_time(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
+ if (!performance_exists(perf_name)) throw StopwatchException("Performance not initialized.");
PerformanceData& perf_info = records_of->find(perf_name)->second;
return perf_info.max_time;
-
}
-long double Stopwatch::get_last_time(string perf_name)
-{
+long double Stopwatch::get_last_time(string perf_name) {
// Try to recover performance data
- if ( !performance_exists(perf_name) )
- throw StopwatchException("Performance not initialized.");
+ if (!performance_exists(perf_name)) throw StopwatchException("Performance not initialized.");
PerformanceData& perf_info = records_of->find(perf_name)->second;
diff --git a/src/util.cpp b/src/util.cpp
index d02b2eb..06db4f0 100644
--- a/src/util.cpp
+++ b/src/util.cpp
@@ -7,39 +7,34 @@
#include <hpp/centroidal-dynamics/util.hh>
#include <ctime>
-namespace centroidal_dynamics
-{
+namespace centroidal_dynamics {
-dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize)
-{
+dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize) {
dd_debug = false;
- dd_MatrixPtr M=NULL;
+ dd_MatrixPtr M = NULL;
dd_rowrange i;
dd_colrange j;
dd_rowrange m_input = (dd_rowrange)(input.rows());
- dd_colrange d_input = (dd_colrange)(input.cols()+1);
- dd_RepresentationType rep=dd_Generator;
+ dd_colrange d_input = (dd_colrange)(input.cols() + 1);
+ dd_RepresentationType rep = dd_Generator;
mytype value;
dd_NumberType NT = dd_Real;
dd_init(value);
- M=dd_CreateMatrix(m_input, d_input);
- M->representation=rep;
- M->numbtype=NT;
- for (i = 0; i < input.rows(); i++)
- {
+ M = dd_CreateMatrix(m_input, d_input);
+ M->representation = rep;
+ M->numbtype = NT;
+ for (i = 0; i < input.rows(); i++) {
dd_set_d(value, 0);
- dd_set(M->matrix[i][0],value);
- for (j = 1; j < d_input; j++)
- {
- dd_set_d(value, input(i,j-1));
- dd_set(M->matrix[i][j],value);
+ dd_set(M->matrix[i][0], value);
+ for (j = 1; j < d_input; j++) {
+ dd_set_d(value, input(i, j - 1));
+ dd_set(M->matrix[i][j], value);
}
}
dd_clear(value);
- if(canonicalize)
- {
+ if (canonicalize) {
dd_ErrorType error = dd_NoError;
- dd_rowset redset,impl_linset;
+ dd_rowset redset, impl_linset;
dd_rowindex newpos;
dd_MatrixCanonicalize(&M, &impl_linset, &redset, &newpos, &error);
set_free(redset);
@@ -49,45 +44,36 @@ dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input, const bool canonicalize
return M;
}
-
-void init_cdd_library()
-{
+void init_cdd_library() {
dd_set_global_constants();
dd_debug = false;
}
-void release_cdd_library()
-{
- //dd_free_global_constants();
+void release_cdd_library() {
+ // dd_free_global_constants();
}
-void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds, Ref_vector3 out)
-{
-
- assert(lower_bounds.rows()==out.rows());
- assert(upper_bounds.rows()==out.rows());
- assert(lower_bounds.cols()==out.cols());
- assert(upper_bounds.cols()==out.cols());
- for(int i=0; i<out.rows(); i++)
- for(int j=0; j<out.cols(); j++)
- out(i,j) = (rand()/ value_type(RAND_MAX))*(upper_bounds(i,j)-lower_bounds(i,j)) + lower_bounds(i,j);
+void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds, Ref_vector3 out) {
+ assert(lower_bounds.rows() == out.rows());
+ assert(upper_bounds.rows() == out.rows());
+ assert(lower_bounds.cols() == out.cols());
+ assert(upper_bounds.cols() == out.cols());
+ for (int i = 0; i < out.rows(); i++)
+ for (int j = 0; j < out.cols(); j++)
+ out(i, j) = (rand() / value_type(RAND_MAX)) * (upper_bounds(i, j) - lower_bounds(i, j)) + lower_bounds(i, j);
}
-void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out)
-{
-
- assert(lower_bounds.rows()==out.rows());
- assert(upper_bounds.rows()==out.rows());
- assert(lower_bounds.cols()==out.cols());
- assert(upper_bounds.cols()==out.cols());
- for(int i=0; i<out.rows(); i++)
- for(int j=0; j<out.cols(); j++)
- out(i,j) = (rand()/ value_type(RAND_MAX))*(upper_bounds(i,j)-lower_bounds(i,j)) + lower_bounds(i,j);
+void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds, Ref_matrixXX out) {
+ assert(lower_bounds.rows() == out.rows());
+ assert(upper_bounds.rows() == out.rows());
+ assert(lower_bounds.cols() == out.cols());
+ assert(upper_bounds.cols() == out.cols());
+ for (int i = 0; i < out.rows(); i++)
+ for (int j = 0; j < out.cols(); j++)
+ out(i, j) = (rand() / value_type(RAND_MAX)) * (upper_bounds(i, j) - lower_bounds(i, j)) + lower_bounds(i, j);
}
-
-void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R)
-{
+void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R) {
const int i = 0;
const int j = 1;
const int k = 2;
@@ -97,67 +83,64 @@ void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R)
double ci = cos(roll);
double cj = cos(pitch);
double ck = cos(yaw);
- double cc = ci*ck;
- double cs = ci*sk;
- double sc = si*ck;
- double ss = si*sk;
-
- R(i, i) = cj*ck;
- R(i, j) = sj*sc-cs;
- R(i, k) = sj*cc+ss;
- R(j, i) = cj*sk;
- R(j, j) = sj*ss+cc;
- R(j, k) = sj*cs-sc;
+ double cc = ci * ck;
+ double cs = ci * sk;
+ double sc = si * ck;
+ double ss = si * sk;
+
+ R(i, i) = cj * ck;
+ R(i, j) = sj * sc - cs;
+ R(i, k) = sj * cc + ss;
+ R(j, i) = cj * sk;
+ R(j, j) = sj * ss + cc;
+ R(j, k) = sj * cs - sc;
R(k, i) = -sj;
- R(k, j) = cj*si;
- R(k, k) = cj*ci;
-// R = (angle_axis_t(roll, Vector3::UnitX())
-// * angle_axis_t(pitch, Vector3::UnitY())
-// * angle_axis_t(yaw, Vector3::UnitZ())).toRotationMatrix();
+ R(k, j) = cj * si;
+ R(k, k) = cj * ci;
+ // R = (angle_axis_t(roll, Vector3::UnitX())
+ // * angle_axis_t(pitch, Vector3::UnitY())
+ // * angle_axis_t(yaw, Vector3::UnitZ())).toRotationMatrix();
}
-bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy,
- Ref_matrix43 p, Ref_matrix43 N)
-{
+bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy, Ref_matrix43 p,
+ Ref_matrix43 N) {
// compute rotation matrix
Rotation R;
euler_matrix(rpy(0), rpy(1), rpy(2), R);
// contact points in local frame
- p << lx, ly, 0,
- lx, -ly, 0,
- -lx, -ly, 0,
- -lx, ly, 0;
+ p << lx, ly, 0, lx, -ly, 0, -lx, -ly, 0, -lx, ly, 0;
// contact points in world frame
- p.row(0) = pos + (R*p.row(0).transpose());
- p.row(1) = pos + (R*p.row(1).transpose());
- p.row(2) = pos + (R*p.row(2).transpose());
- p.row(3) = pos + (R*p.row(3).transpose());
+ p.row(0) = pos + (R * p.row(0).transpose());
+ p.row(1) = pos + (R * p.row(1).transpose());
+ p.row(2) = pos + (R * p.row(2).transpose());
+ p.row(3) = pos + (R * p.row(3).transpose());
// normal direction in local frame
RVector3 n;
n << 0, 0, 1;
// normal directions in world frame
- n = (R*n.transpose()).transpose();
+ n = (R * n.transpose()).transpose();
N << n, n, n, n;
return true;
}
-Rotation crossMatrix(Cref_vector3 x)
-{
- Rotation res = Rotation::Zero();
- res(0,1) = - x(2); res(0,2) = x(1);
- res(1,0) = x(2); res(1,2) = - x(0);
- res(2,0) = - x(1); res(2,1) = x(0);
- return res;
+Rotation crossMatrix(Cref_vector3 x) {
+ Rotation res = Rotation::Zero();
+ res(0, 1) = -x(2);
+ res(0, 2) = x(1);
+ res(1, 0) = x(2);
+ res(1, 2) = -x(0);
+ res(2, 0) = -x(1);
+ res(2, 1) = x(0);
+ return res;
}
-std::string getDateAndTimeAsString()
-{
+std::string getDateAndTimeAsString() {
time_t rawtime;
- struct tm * timeinfo;
+ struct tm* timeinfo;
char buffer[80];
- time (&rawtime);
+ time(&rawtime);
timeinfo = localtime(&rawtime);
- strftime(buffer,80,"%Y%m%d_%I%M%S",timeinfo);
+ strftime(buffer, 80, "%Y%m%d_%I%M%S", timeinfo);
return std::string(buffer);
}
/*
@@ -191,19 +174,16 @@ value_type choosenk(const int n, const int k)
}
}*/
-value_type nchoosek(const int n, const int k)
-{
- if(k>n/2)
- return nchoosek(n,n-k);
- else if(k==1)
- return n;
- else
- {
- value_type c = 1;
- for(int i = 1;i<=k;i++)
- c *= (((value_type)n-k+i)/((value_type)i));
- return round(c);
- }
+value_type nchoosek(const int n, const int k) {
+ if (k > n / 2)
+ return nchoosek(n, n - k);
+ else if (k == 1)
+ return n;
+ else {
+ value_type c = 1;
+ for (int i = 1; i <= k; i++) c *= (((value_type)n - k + i) / ((value_type)i));
+ return round(c);
+ }
}
} // namespace centroidal_dynamics
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
index 38d9061..e4f9923 100644
--- a/test/test_LP_solvers.cpp
+++ b/test/test_LP_solvers.cpp
@@ -26,13 +26,11 @@ USING_NAMESPACE_QPOASES
#ifdef CLP_FOUND
/** Example addRows.cpp */
-void test_addRows()
-{
+void test_addRows() {
const int N_ROWS_TO_ADD = 30000;
- try
- {
+ try {
// Empty model
- ClpSimplex model;
+ ClpSimplex model;
// Objective - just nonzeros
int objIndex[] = {0, 2};
@@ -46,8 +44,7 @@ void test_addRows()
int i;
// Virtuous way
// First objective
- for (i = 0; i < 2; i++)
- model.setObjectiveCoefficient(objIndex[i], objValue[i]);
+ for (i = 0; i < 2; i++) model.setObjectiveCoefficient(objIndex[i], objValue[i]);
// Now bounds (lower will be zero by default but do again)
for (i = 0; i < 3; i++) {
model.setColumnLower(i, 0.0);
@@ -62,13 +59,11 @@ void test_addRows()
// Now add row 1 as >= 2.0
int row1Index[] = {0, 2};
double row1Value[] = {1.0, 1.0};
- model.addRow(2, row1Index, row1Value,
- 2.0, COIN_DBL_MAX);
+ model.addRow(2, row1Index, row1Value, 2.0, COIN_DBL_MAX);
// Now add row 2 as == 1.0
int row2Index[] = {0, 1, 2};
double row2Value[] = {1.0, -5.0, 1.0};
- model.addRow(3, row2Index, row2Value,
- 1.0, 1.0);
+ model.addRow(3, row2Index, row2Value, 1.0, 1.0);
// solve
model.dual();
@@ -85,8 +80,7 @@ void test_addRows()
for (k = 0; k < N_ROWS_TO_ADD; k++) {
int row2Index[] = {0, 1, 2};
double row2Value[] = {1.0, -5.0, 1.0};
- model.addRow(3, row2Index, row2Value,
- 1.0, 1.0);
+ model.addRow(3, row2Index, row2Value, 1.0, 1.0);
}
printf("Time for 10000 addRow is %g\n", CoinCpuTime() - time1);
model.dual();
@@ -97,8 +91,7 @@ void test_addRows()
for (k = 0; k < N_ROWS_TO_ADD; k++) {
int row2Index[] = {0, 1, 2};
double row2Value[] = {1.0, -5.0, 1.0};
- buildObject.addRow(3, row2Index, row2Value,
- 1.0, 1.0);
+ buildObject.addRow(3, row2Index, row2Value, 1.0, 1.0);
}
model.addRows(buildObject);
printf("Time for 10000 addRow using CoinBuild is %g\n", CoinCpuTime() - time1);
@@ -112,8 +105,7 @@ void test_addRows()
for (k = 0; k < N_ROWS_TO_ADD; k++) {
int row2Index[] = {0, 1, 2};
double row2Value[] = {1.0, -1.0, 1.0};
- buildObject2.addRow(3, row2Index, row2Value,
- 1.0, 1.0);
+ buildObject2.addRow(3, row2Index, row2Value, 1.0, 1.0);
}
model.addRows(buildObject2, true);
printf("Time for 10000 addRow using CoinBuild+-1 is %g\n", CoinCpuTime() - time1);
@@ -126,8 +118,7 @@ void test_addRows()
for (k = 0; k < N_ROWS_TO_ADD; k++) {
int row2Index[] = {0, 1, 2};
double row2Value[] = {1.0, -1.0, 1.0};
- modelObject2.addRow(3, row2Index, row2Value,
- 1.0, 1.0);
+ modelObject2.addRow(3, row2Index, row2Value, 1.0, 1.0);
}
model.addRows(modelObject2, true);
printf("Time for 10000 addRow using CoinModel+-1 is %g\n", CoinCpuTime() - time1);
@@ -139,8 +130,7 @@ void test_addRows()
for (k = 0; k < N_ROWS_TO_ADD; k++) {
int row2Index[] = {0, 1, 2};
double row2Value[] = {1.0, -1.0, 1.0};
- modelObject3.addRow(3, row2Index, row2Value,
- 1.0, 1.0);
+ modelObject3.addRow(3, row2Index, row2Value, 1.0, 1.0);
}
model.loadProblem(modelObject3, true);
printf("Time for 10000 addRow using CoinModel load +-1 is %g\n", CoinCpuTime() - time1);
@@ -153,8 +143,7 @@ void test_addRows()
for (k = 0; k < N_ROWS_TO_ADD; k++) {
int row2Index[] = {0, 1, 2};
double row2Value[] = {1.0, -5.0, 1.0};
- modelObject.addRow(3, row2Index, row2Value,
- 1.0, 1.0);
+ modelObject.addRow(3, row2Index, row2Value, 1.0, 1.0);
}
model.addRows(modelObject);
printf("Time for 10000 addRow using CoinModel is %g\n", CoinCpuTime() - time1);
@@ -164,7 +153,7 @@ void test_addRows()
time1 = CoinCpuTime();
// Assumes we know exact size of model and matrix
// Empty model
- ClpSimplex model2;
+ ClpSimplex model2;
{
// Create space for 3 columns and 10000 rows
int numberRows = N_ROWS_TO_ADD;
@@ -173,23 +162,23 @@ void test_addRows()
int numberElements = numberRows * numberColumns;
// Arrays will be set to default values
model2.resize(numberRows, numberColumns);
- double * elements = new double [numberElements];
- CoinBigIndex * starts = new CoinBigIndex [numberColumns+1];
- int * rows = new int [numberElements];;
- int * lengths = new int[numberColumns];
+ double *elements = new double[numberElements];
+ CoinBigIndex *starts = new CoinBigIndex[numberColumns + 1];
+ int *rows = new int[numberElements];
+ ;
+ int *lengths = new int[numberColumns];
// Now fill in - totally unsafe but ....
// no need as defaults to 0.0 double * columnLower = model2.columnLower();
- double * columnUpper = model2.columnUpper();
- double * objective = model2.objective();
- double * rowLower = model2.rowLower();
- double * rowUpper = model2.rowUpper();
+ double *columnUpper = model2.columnUpper();
+ double *objective = model2.objective();
+ double *rowLower = model2.rowLower();
+ double *rowUpper = model2.rowUpper();
// Columns - objective was packed
for (k = 0; k < 2; k++) {
int iColumn = objIndex[k];
objective[iColumn] = objValue[k];
}
- for (k = 0; k < numberColumns; k++)
- columnUpper[k] = upper[k];
+ for (k = 0; k < numberColumns; k++) columnUpper[k] = upper[k];
// Rows
for (k = 0; k < numberRows; k++) {
rowLower[k] = 1.0;
@@ -210,10 +199,9 @@ void test_addRows()
}
starts[numberColumns] = put;
// assign to matrix
- CoinPackedMatrix * matrix = new CoinPackedMatrix(true, 0.0, 0.0);
- matrix->assignMatrix(true, numberRows, numberColumns, numberElements,
- elements, rows, starts, lengths);
- ClpPackedMatrix * clpMatrix = new ClpPackedMatrix(matrix);
+ CoinPackedMatrix *matrix = new CoinPackedMatrix(true, 0.0, 0.0);
+ matrix->assignMatrix(true, numberRows, numberColumns, numberElements, elements, rows, starts, lengths);
+ ClpPackedMatrix *clpMatrix = new ClpPackedMatrix(matrix);
model2.replaceMatrix(clpMatrix, true);
printf("Time for 10000 addRow using hand written code is %g\n", CoinCpuTime() - time1);
// If matrix is really big could switch off creation of row copy
@@ -225,20 +213,19 @@ void test_addRows()
int numberColumns = model.numberColumns();
// Alternatively getColSolution()
- double * columnPrimal = model.primalColumnSolution();
+ double *columnPrimal = model.primalColumnSolution();
// Alternatively getReducedCost()
- double * columnDual = model.dualColumnSolution();
+ double *columnDual = model.dualColumnSolution();
// Alternatively getColLower()
- double * columnLower = model.columnLower();
+ double *columnLower = model.columnLower();
// Alternatively getColUpper()
- double * columnUpper = model.columnUpper();
+ double *columnUpper = model.columnUpper();
// Alternatively getObjCoefficients()
- double * columnObjective = model.objective();
+ double *columnObjective = model.objective();
int iColumn;
- std::cout << " Primal Dual Lower Upper Cost"
- << std::endl;
+ std::cout << " Primal Dual Lower Upper Cost" << std::endl;
for (iColumn = 0; iColumn < numberColumns; iColumn++) {
double value;
@@ -273,7 +260,8 @@ void test_addRows()
}
std::cout << "--------------------------------------" << std::endl;
// Test CoinAssert
- std::cout << "If Clp compiled without NDEBUG below should give assert, if with NDEBUG or COIN_ASSERT CoinError" << std::endl;
+ std::cout << "If Clp compiled without NDEBUG below should give assert, if with NDEBUG or COIN_ASSERT CoinError"
+ << std::endl;
model = modelSave;
model.deleteRows(2, del);
// Deliberate error
@@ -284,21 +272,18 @@ void test_addRows()
for (k = 0; k < N_ROWS_TO_ADD; k++) {
int row2Index[] = {0, 1, 2};
double row2Value[] = {1.0, -1.0, 1.0};
- buildObject3.addRow(3, row2Index, row2Value,
- 1.0, 1.0);
+ buildObject3.addRow(3, row2Index, row2Value, 1.0, 1.0);
}
model.addRows(buildObject3, true);
} catch (CoinError e) {
e.print();
- if (e.lineNumber() >= 0)
- std::cout << "This was from a CoinAssert" << std::endl;
+ if (e.lineNumber() >= 0) std::cout << "This was from a CoinAssert" << std::endl;
}
}
-void test_small_LP()
-{
+void test_small_LP() {
ClpSimplex model;
-// model.setLogLevel(1);
+ // model.setLogLevel(1);
// Objective - just nonzeros
int objIndex[] = {0, 2};
@@ -319,8 +304,7 @@ void test_small_LP()
int i;
// Virtuous way
// First objective
- for (i = 0; i < 2; i++)
- model.setObjectiveCoefficient(objIndex[i], objValue[i]);
+ for (i = 0; i < 2; i++) model.setObjectiveCoefficient(objIndex[i], objValue[i]);
// Now bounds (lower will be zero by default but do again)
for (i = 0; i < 3; i++) {
model.setColumnLower(i, 0.0);
@@ -335,159 +319,140 @@ void test_small_LP()
// Now add row 1 as >= 2.0
int row1Index[] = {0, 2};
double row1Value[] = {1.0, 1.0};
- model.addRow(2, row1Index, row1Value,
- 2.0, COIN_DBL_MAX);
+ model.addRow(2, row1Index, row1Value, 2.0, COIN_DBL_MAX);
// Now add row 2 as == 1.0
int row2Index[] = {0, 1, 2};
double row2Value[] = {1.0, -5.0, 1.0};
- model.addRow(3, row2Index, row2Value,
- 1.0, 1.0);
+ model.addRow(3, row2Index, row2Value, 1.0, 1.0);
int n = model.getdimVarXs();
int m = model.getNumRows();
- cout<<"Problem has "<<n<<" variables and "<<m<<" constraints.\n";
+ cout << "Problem has " << n << " variables and " << m << " constraints.\n";
// solve
model.dual();
// Check the solution
- if ( model.isProvenOptimal() )
- {
+ if (model.isProvenOptimal()) {
cout << "Found optimal solution!" << endl;
cout << "Objective value is " << model.getObjValue() << endl;
- cout << "Model status is " << model.status() << " after "
- << model.numberIterations() << " iterations - objective is "
- << model.objectiveValue() << endl;
+ cout << "Model status is " << model.status() << " after " << model.numberIterations()
+ << " iterations - objective is " << model.objectiveValue() << endl;
const double *solution;
solution = model.getColSolution();
// We could then print the solution or examine it.
- cout<<"Solution is: ";
- for(int i=0; i<n; i++)
- cout<<solution[i]<<", ";
- cout<<endl;
- }
- else
+ cout << "Solution is: ";
+ for (int i = 0; i < n; i++) cout << solution[i] << ", ";
+ cout << endl;
+ } else
cout << "Didn’t find optimal solution." << endl;
}
#endif
-int main()
-{
- cout <<"Test LP Solvers (1 means ok, 0 means error)\n\n";
+int main() {
+ cout << "Test LP Solvers (1 means ok, 0 means error)\n\n";
{
- cout<<"TEST QP OASES ON A SMALL 2-VARIABLE LP";
+ cout << "TEST QP OASES ON A SMALL 2-VARIABLE LP";
/* Setup data of first LP. */
- real_t A[1*2] = { 1.0, 1.0 };
- real_t g[2] = { 1.5, 1.0 };
- real_t lb[2] = { 0.5, -2.0 };
- real_t ub[2] = { 5.0, 2.0 };
- real_t lbA[1] = { -1.0 };
- real_t ubA[1] = { 2.0 };
+ real_t A[1 * 2] = {1.0, 1.0};
+ real_t g[2] = {1.5, 1.0};
+ real_t lb[2] = {0.5, -2.0};
+ real_t ub[2] = {5.0, 2.0};
+ real_t lbA[1] = {-1.0};
+ real_t ubA[1] = {2.0};
/* Setting up QProblem object with zero Hessian matrix. */
- QProblem example( 2,1,HST_ZERO );
+ QProblem example(2, 1, HST_ZERO);
Options options;
- //options.setToMPC();
- example.setOptions( options );
+ // options.setToMPC();
+ example.setOptions(options);
/* Solve first LP. */
int nWSR = 10;
- int res = example.init( 0,g,A,lb,ub,lbA,ubA, nWSR,0 );
- if(res==0)
- cout<<"[INFO] LP solved correctly\n";
+ int res = example.init(0, g, A, lb, ub, lbA, ubA, nWSR, 0);
+ if (res == 0)
+ cout << "[INFO] LP solved correctly\n";
else
- cout<<"[ERROR] QpOases could not solve the LP problem, error code: "<<res<<endl;
+ cout << "[ERROR] QpOases could not solve the LP problem, error code: " << res << endl;
}
{
- cout<<"\nTEST READ-WRITE METHODS OF SOLVER_LP_ABSTRACT\n";
+ cout << "\nTEST READ-WRITE METHODS OF SOLVER_LP_ABSTRACT\n";
Solver_LP_abstract *solverOases = Solver_LP_abstract::getNewSolver(SOLVER_LP_QPOASES);
const int n = 3;
const int m = 4;
- const char* filename = "small_3_x_4_LP.dat";
+ const char *filename = "small_3_x_4_LP.dat";
VectorX c = VectorX::Random(n);
- VectorX lb = -100*VectorX::Ones(n);
- VectorX ub = 100*VectorX::Ones(n);
- MatrixXX A = MatrixXX::Random(m,n);
- VectorX Alb = -100*VectorX::Ones(m);
- VectorX Aub = 100*VectorX::Ones(m);
- if(!solverOases->writeLpToFile(filename, c, lb, ub, A, Alb, Aub))
- {
+ VectorX lb = -100 * VectorX::Ones(n);
+ VectorX ub = 100 * VectorX::Ones(n);
+ MatrixXX A = MatrixXX::Random(m, n);
+ VectorX Alb = -100 * VectorX::Ones(m);
+ VectorX Aub = 100 * VectorX::Ones(m);
+ if (!solverOases->writeLpToFile(filename, c, lb, ub, A, Alb, Aub)) {
SEND_ERROR_MSG("Error while writing LP to file");
return -1;
}
VectorX c2, lb2, ub2, Alb2, Aub2;
MatrixXX A2;
- if(!solverOases->readLpFromFile(filename, c2, lb2, ub2, A2, Alb2, Aub2))
- {
+ if (!solverOases->readLpFromFile(filename, c2, lb2, ub2, A2, Alb2, Aub2)) {
SEND_ERROR_MSG("Error while reading LP from file");
return -1;
}
- cout<<"Check number of variables: "<<(c.size()==c2.size())<<endl;
- cout<<"Check number of constraints: "<<(A.rows()==A2.rows())<<endl;
- cout<<"Check gradient vector c: "<<c.isApprox(c2)<<endl;
- cout<<"Check lower bound vector lb: "<<lb.isApprox(lb2)<<endl;
- cout<<"Check upper bound vector ub: "<<ub.isApprox(ub2)<<endl;
- cout<<"Check constraint matrix A: "<<A.isApprox(A2)<<endl;
- cout<<"Check constraint lower bound vector Alb: "<<Alb.isApprox(Alb2)<<endl;
- cout<<"Check constraint upper bound vector Aub: "<<Aub.isApprox(Aub2)<<endl;
+ cout << "Check number of variables: " << (c.size() == c2.size()) << endl;
+ cout << "Check number of constraints: " << (A.rows() == A2.rows()) << endl;
+ cout << "Check gradient vector c: " << c.isApprox(c2) << endl;
+ cout << "Check lower bound vector lb: " << lb.isApprox(lb2) << endl;
+ cout << "Check upper bound vector ub: " << ub.isApprox(ub2) << endl;
+ cout << "Check constraint matrix A: " << A.isApprox(A2) << endl;
+ cout << "Check constraint lower bound vector Alb: " << Alb.isApprox(Alb2) << endl;
+ cout << "Check constraint upper bound vector Aub: " << Aub.isApprox(Aub2) << endl;
}
{
- cout<<"\nTEST QP OASES ON SOME LP PROBLEMS\n";
+ cout << "\nTEST QP OASES ON SOME LP PROBLEMS\n";
string file_path = "../test_data/";
Solver_LP_abstract *solverOases = Solver_LP_abstract::getNewSolver(SOLVER_LP_QPOASES);
const int PROBLEM_NUMBER = 14;
- string problem_filenames[PROBLEM_NUMBER] = {"DLP_findExtremumOverLine20151103_112611",
- "DLP_findExtremumOverLine20151103_115627",
- "DLP_findExtremumOverLine20151103_014022",
- "DLP_findExtremumOverLine_32_generators",
- "DLP_findExtremumOverLine_64_generators",
- "DLP_findExtremumOverLine_128_generators",
- "DLP_findExtremumOverLine_128_generators_bis",
- "LP_findExtremumOverLine20151103_112610",
- "LP_findExtremumOverLine20151103_112611",
- "LP_findExtremumOverLine20151103_014022",
- "LP_findExtremumOverLine_32_generators",
- "LP_findExtremumOverLine_64_generators",
- "LP_findExtremumOverLine_128_generators",
- "LP_findExtremumOverLine_128_generators_bis"};
+ string problem_filenames[PROBLEM_NUMBER] = {
+ "DLP_findExtremumOverLine20151103_112611", "DLP_findExtremumOverLine20151103_115627",
+ "DLP_findExtremumOverLine20151103_014022", "DLP_findExtremumOverLine_32_generators",
+ "DLP_findExtremumOverLine_64_generators", "DLP_findExtremumOverLine_128_generators",
+ "DLP_findExtremumOverLine_128_generators_bis", "LP_findExtremumOverLine20151103_112610",
+ "LP_findExtremumOverLine20151103_112611", "LP_findExtremumOverLine20151103_014022",
+ "LP_findExtremumOverLine_32_generators", "LP_findExtremumOverLine_64_generators",
+ "LP_findExtremumOverLine_128_generators", "LP_findExtremumOverLine_128_generators_bis"};
VectorX c, lb, ub, Alb, Aub, realSol, sol;
MatrixXX A;
- for(int i=0; i<PROBLEM_NUMBER; i++)
- {
+ for (int i = 0; i < PROBLEM_NUMBER; i++) {
string &problem_filename = problem_filenames[i];
- if(!solverOases->readLpFromFile(file_path+problem_filename+".dat", c, lb, ub, A, Alb, Aub))
- {
- SEND_ERROR_MSG("Error while reading LP from file "+problem_filename);
+ if (!solverOases->readLpFromFile(file_path + problem_filename + ".dat", c, lb, ub, A, Alb, Aub)) {
+ SEND_ERROR_MSG("Error while reading LP from file " + problem_filename);
return -1;
}
- string solution_filename = problem_filename+"_solution";
- if(!readMatrixFromFile(file_path+solution_filename+".dat", realSol))
- {
- SEND_ERROR_MSG("Error while reading LP solution from file "+solution_filename);
- //return -1;
+ string solution_filename = problem_filename + "_solution";
+ if (!readMatrixFromFile(file_path + solution_filename + ".dat", realSol)) {
+ SEND_ERROR_MSG("Error while reading LP solution from file " + solution_filename);
+ // return -1;
}
sol.resize(c.size());
solverOases->solve(c, lb, ub, A, Alb, Aub, sol);
- if(sol.isApprox(realSol, EPS))
- {
- cout<<"[INFO] Solution of problem "<<problem_filename<<" ("<<c.size()<<" var, "<<A.rows()<<" constr) is equal to the expected value!\n";
- }
- else
- {
- if(fabs(c.dot(sol)-c.dot(realSol))<EPS)
- cout<<"[WARNING] Solution of problem "<<problem_filename<<" ("<<c.size()<<" var, "<<A.rows()<<" constr) is different from expected but it has the same cost\n";
- else
- {
- cout<<"[ERROR] Solution of problem "<<problem_filename<<" ("<<c.size()<<" var, "<<A.rows()<<" constr) is different from the expected value:\n";
- cout<<"\tSolution found "<<sol.transpose()<<endl;
- cout<<"\tExpected solution "<<realSol.transpose()<<endl;
- cout<<"\tCost found "<<(c.dot(sol))<<endl;
- cout<<"\tCost expected "<<(c.dot(realSol))<<endl;
+ if (sol.isApprox(realSol, EPS)) {
+ cout << "[INFO] Solution of problem " << problem_filename << " (" << c.size() << " var, " << A.rows()
+ << " constr) is equal to the expected value!\n";
+ } else {
+ if (fabs(c.dot(sol) - c.dot(realSol)) < EPS)
+ cout << "[WARNING] Solution of problem " << problem_filename << " (" << c.size() << " var, " << A.rows()
+ << " constr) is different from expected but it has the same cost\n";
+ else {
+ cout << "[ERROR] Solution of problem " << problem_filename << " (" << c.size() << " var, " << A.rows()
+ << " constr) is different from the expected value:\n";
+ cout << "\tSolution found " << sol.transpose() << endl;
+ cout << "\tExpected solution " << realSol.transpose() << endl;
+ cout << "\tCost found " << (c.dot(sol)) << endl;
+ cout << "\tCost expected " << (c.dot(realSol)) << endl;
}
}
}
@@ -495,29 +460,25 @@ int main()
return 0;
}
-
#ifdef CLP_FOUND
test_addRows();
test_small_LP();
Solver_LP_abstract *solver = Solver_LP_abstract::getNewSolver(SOLVER_LP_CLP);
Vector3 c, lb, ub, x;
- MatrixXX A(2,3);
+ MatrixXX A(2, 3);
Vector2 Alb, Aub;
c << 1.0, 0.0, 4.0;
lb << 0.0, 0.0, 0.0;
ub << 2.0, COIN_DBL_MAX, 4.0;
- A << 1.0, 0.0, 1.0,
- 1.0, -5.0, 1.0;
+ A << 1.0, 0.0, 1.0, 1.0, -5.0, 1.0;
Alb << 2.0, 1.0;
Aub << COIN_DBL_MAX, 1.0;
- if(solver->solve(c, lb, ub, A, Alb, Aub, x)==LP_STATUS_OPTIMAL)
- {
- cout<<"solver_LP_clp solved the problem\n";
- cout<<"The solution is "<<x.transpose()<<endl;
- }
- else
- cout<<"solver_LP_clp failed to solve the problem\n";
+ if (solver->solve(c, lb, ub, A, Alb, Aub, x) == LP_STATUS_OPTIMAL) {
+ cout << "solver_LP_clp solved the problem\n";
+ cout << "The solution is " << x.transpose() << endl;
+ } else
+ cout << "solver_LP_clp failed to solve the problem\n";
#endif
return 1;
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 98374cb..d4cd680 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -33,63 +33,54 @@ using namespace std;
* @param PERF_STRING_2 String to use for logging the computation times of solver_2
* @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
*/
-int test_computeEquilibriumRobustness_vs_checkEquilibrium(Equilibrium *solver_1,
- Equilibrium *solver_2,
- Cref_matrixXX comPositions,
- const string& PERF_STRING_1="",
- const string& PERF_STRING_2="",
- int verb=0)
-{
+int test_computeEquilibriumRobustness_vs_checkEquilibrium(Equilibrium* solver_1, Equilibrium* solver_2,
+ Cref_matrixXX comPositions, const string& PERF_STRING_1 = "",
+ const string& PERF_STRING_2 = "", int verb = 0) {
int error_counter = 0;
double rob;
LP_status status;
bool equilibrium;
- for(unsigned int i=0; i<comPositions.rows(); i++)
- {
- if(!PERF_STRING_1.empty())
- getProfiler().start(PERF_STRING_1);
+ for (unsigned int i = 0; i < comPositions.rows(); i++) {
+ if (!PERF_STRING_1.empty()) getProfiler().start(PERF_STRING_1);
status = solver_1->computeEquilibriumRobustness(comPositions.row(i), rob);
- if(!PERF_STRING_1.empty())
- getProfiler().stop(PERF_STRING_1);
+ if (!PERF_STRING_1.empty()) getProfiler().stop(PERF_STRING_1);
- if(status!=LP_STATUS_OPTIMAL)
- {
- if(verb>1)
- SEND_ERROR_MSG(solver_1->getName()+" failed to compute robustness of com position "+toString(comPositions.row(i)));
+ if (status != LP_STATUS_OPTIMAL) {
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_1->getName() + " failed to compute robustness of com position " +
+ toString(comPositions.row(i)));
error_counter++;
continue;
}
- if(!PERF_STRING_2.empty())
- getProfiler().start(PERF_STRING_2);
- status= solver_2->checkRobustEquilibrium(comPositions.row(i), equilibrium);
- if(!PERF_STRING_2.empty())
- getProfiler().stop(PERF_STRING_2);
+ if (!PERF_STRING_2.empty()) getProfiler().start(PERF_STRING_2);
+ status = solver_2->checkRobustEquilibrium(comPositions.row(i), equilibrium);
+ if (!PERF_STRING_2.empty()) getProfiler().stop(PERF_STRING_2);
- if(status!=LP_STATUS_OPTIMAL)
- {
- if(verb>1)
- SEND_ERROR_MSG(solver_2->getName()+" failed to check equilibrium of com position "+toString(comPositions.row(i)));
+ if (status != LP_STATUS_OPTIMAL) {
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_2->getName() + " failed to check equilibrium of com position " +
+ toString(comPositions.row(i)));
error_counter++;
continue;
}
- if(equilibrium==true && rob<0.0)
- {
- if(verb>1)
- SEND_ERROR_MSG(solver_2->getName()+" says com is in equilibrium while "+solver_1->getName()+" computed a negative robustness measure "+toString(rob));
+ if (equilibrium == true && rob < 0.0) {
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_2->getName() + " says com is in equilibrium while " + solver_1->getName() +
+ " computed a negative robustness measure " + toString(rob));
error_counter++;
- }
- else if(equilibrium==false && rob>0.0)
- {
- if(verb>1)
- SEND_ERROR_MSG(solver_2->getName()+" says com is not in equilibrium while "+solver_1->getName()+" computed a positive robustness measure "+toString(rob));
+ } else if (equilibrium == false && rob > 0.0) {
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_2->getName() + " says com is not in equilibrium while " + solver_1->getName() +
+ " computed a positive robustness measure " + toString(rob));
error_counter++;
}
}
- if(verb>0)
- cout<<"Test test_computeEquilibriumRobustness_vs_checkEquilibrium "+solver_1->getName()+" VS "+solver_2->getName()+": "+toString(error_counter)+" error(s).\n";
+ if (verb > 0)
+ cout << "Test test_computeEquilibriumRobustness_vs_checkEquilibrium " + solver_1->getName() + " VS " +
+ solver_2->getName() + ": " + toString(error_counter) + " error(s).\n";
return error_counter;
}
@@ -101,22 +92,20 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(Equilibrium *solver_1,
* @param PERF_STRING_2 String to use for logging the computation times of solver_2
* @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
*/
-int test_computeEquilibriumRobustness(Equilibrium *solver_1, Equilibrium *solver_2, Cref_matrixXX comPositions,
- const string& PERF_STRING_1, const string& PERF_STRING_2, int verb=0)
-{
+int test_computeEquilibriumRobustness(Equilibrium* solver_1, Equilibrium* solver_2, Cref_matrixXX comPositions,
+ const string& PERF_STRING_1, const string& PERF_STRING_2, int verb = 0) {
int error_counter = 0;
double rob_1, rob_2;
LP_status status;
- for(unsigned int i=0; i<comPositions.rows(); i++)
- {
+ for (unsigned int i = 0; i < comPositions.rows(); i++) {
getProfiler().start(PERF_STRING_1);
status = solver_1->computeEquilibriumRobustness(comPositions.row(i), rob_1);
getProfiler().stop(PERF_STRING_1);
- if(status!=LP_STATUS_OPTIMAL)
- {
- if(verb>1)
- SEND_ERROR_MSG(solver_1->getName()+" failed to compute robustness of com position "+toString(comPositions.row(i)));
+ if (status != LP_STATUS_OPTIMAL) {
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_1->getName() + " failed to compute robustness of com position " +
+ toString(comPositions.row(i)));
error_counter++;
continue;
}
@@ -125,24 +114,25 @@ int test_computeEquilibriumRobustness(Equilibrium *solver_1, Equilibrium *solver
status = solver_2->computeEquilibriumRobustness(comPositions.row(i), rob_2);
getProfiler().stop(PERF_STRING_2);
- if(status!=LP_STATUS_OPTIMAL)
- {
- if(verb>1)
- SEND_ERROR_MSG(solver_2->getName()+" failed to compute robustness of com position "+toString(comPositions.row(i)));
+ if (status != LP_STATUS_OPTIMAL) {
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_2->getName() + " failed to compute robustness of com position " +
+ toString(comPositions.row(i)));
error_counter++;
continue;
}
- if(fabs(rob_1-rob_2)>EPS)
- {
- if(verb>1)
- SEND_ERROR_MSG(solver_1->getName()+" and "+solver_2->getName()+" returned different results: "+toString(rob_1)+" VS "+toString(rob_2));
+ if (fabs(rob_1 - rob_2) > EPS) {
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_1->getName() + " and " + solver_2->getName() +
+ " returned different results: " + toString(rob_1) + " VS " + toString(rob_2));
error_counter++;
}
}
- if(verb>0)
- cout<<"Test computeEquilibriumRobustness "+solver_1->getName()+" VS "+solver_2->getName()+": "+toString(error_counter)+" error(s).\n";
+ if (verb > 0)
+ cout << "Test computeEquilibriumRobustness " + solver_1->getName() + " VS " + solver_2->getName() + ": " +
+ toString(error_counter) + " error(s).\n";
return error_counter;
}
@@ -159,42 +149,38 @@ int test_computeEquilibriumRobustness(Equilibrium *solver_1, Equilibrium *solver
* @param PERF_STRING_GROUND_TRUTH String to use for logging the computation times of solver_ground_truth
* @param verb Verbosity level, 0 print nothing, 1 print summary, 2 print everything
*/
-int test_findExtremumOverLine(Equilibrium *solver_to_test, Equilibrium *solver_ground_truth,
- Cref_vector3 a0, int N_TESTS, double e_max,
- const string& PERF_STRING_TEST, const string& PERF_STRING_GROUND_TRUTH, int verb=0)
-{
+int test_findExtremumOverLine(Equilibrium* solver_to_test, Equilibrium* solver_ground_truth, Cref_vector3 a0,
+ int N_TESTS, double e_max, const string& PERF_STRING_TEST,
+ const string& PERF_STRING_GROUND_TRUTH, int verb = 0) {
int error_counter = 0;
Vector3 a, com;
LP_status status;
double desired_robustness, robustness;
- for(unsigned int i=0; i<N_TESTS; i++)
- {
- uniform3(-1.0*Vector3::Ones(), Vector3::Ones(), a);
- if(e_max>=0.0)
- desired_robustness = (rand()/ value_type(RAND_MAX))*e_max;
+ for (unsigned int i = 0; i < N_TESTS; i++) {
+ uniform3(-1.0 * Vector3::Ones(), Vector3::Ones(), a);
+ if (e_max >= 0.0)
+ desired_robustness = (rand() / value_type(RAND_MAX)) * e_max;
else
desired_robustness = e_max - EPS;
getProfiler().start(PERF_STRING_TEST);
- status = solver_to_test->findExtremumOverLine(a, a0, desired_robustness, com);
+ status = solver_to_test->findExtremumOverLine(a, a0, desired_robustness, com);
getProfiler().stop(PERF_STRING_TEST);
- if(status!=LP_STATUS_OPTIMAL)
- {
+ if (status != LP_STATUS_OPTIMAL) {
status = solver_ground_truth->computeEquilibriumRobustness(a0, robustness);
- if(status!=LP_STATUS_OPTIMAL)
- {
+ if (status != LP_STATUS_OPTIMAL) {
error_counter++;
- if(verb>1)
- SEND_ERROR_MSG(solver_ground_truth->getName()+" failed to compute equilibrium robustness of com position "+toString(a0.transpose()));
- }
- else if(robustness>desired_robustness)
- {
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_ground_truth->getName() +
+ " failed to compute equilibrium robustness of com position " + toString(a0.transpose()));
+ } else if (robustness > desired_robustness) {
error_counter++;
- if(verb>1)
- SEND_ERROR_MSG(solver_to_test->getName()+" failed to find extremum over line starting from "+
- toString(a0.transpose())+" with robustness "+toString(desired_robustness)+" while "+
- solver_ground_truth->getName()+" says this position has robustness "+toString(robustness));
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_to_test->getName() + " failed to find extremum over line starting from " +
+ toString(a0.transpose()) + " with robustness " + toString(desired_robustness) + " while " +
+ solver_ground_truth->getName() + " says this position has robustness " +
+ toString(robustness));
}
continue;
}
@@ -203,25 +189,24 @@ int test_findExtremumOverLine(Equilibrium *solver_to_test, Equilibrium *solver_g
status = solver_ground_truth->computeEquilibriumRobustness(com, robustness);
getProfiler().stop(PERF_STRING_GROUND_TRUTH);
- if(status!=LP_STATUS_OPTIMAL)
- {
+ if (status != LP_STATUS_OPTIMAL) {
error_counter++;
- if(verb>1)
- SEND_ERROR_MSG(solver_ground_truth->getName()+" failed to compute equilibrium robustness of com posiiton "+toString(com.transpose()));
- }
- else if(fabs(robustness-desired_robustness)>EPS)
- {
- if(verb>1)
- SEND_ERROR_MSG(solver_to_test->getName()+" found this extremum: "+toString(com.transpose())+
- " over the line starting at "+toString(a0.transpose())+" in direction "+toString(a.transpose())+
- " which should have robustness "+toString(desired_robustness)+
- " but actually has robustness "+toString(robustness));
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_ground_truth->getName() + " failed to compute equilibrium robustness of com posiiton " +
+ toString(com.transpose()));
+ } else if (fabs(robustness - desired_robustness) > EPS) {
+ if (verb > 1)
+ SEND_ERROR_MSG(solver_to_test->getName() + " found this extremum: " + toString(com.transpose()) +
+ " over the line starting at " + toString(a0.transpose()) + " in direction " +
+ toString(a.transpose()) + " which should have robustness " + toString(desired_robustness) +
+ " but actually has robustness " + toString(robustness));
error_counter++;
}
}
- if(verb>0)
- cout<<"Test findExtremumOverLine "+solver_to_test->getName()+" VS "+solver_ground_truth->getName()+": "+toString(error_counter)+" error(s).\n";
+ if (verb > 0)
+ cout << "Test findExtremumOverLine " + solver_to_test->getName() + " VS " + solver_ground_truth->getName() + ": " +
+ toString(error_counter) + " error(s).\n";
return error_counter;
}
@@ -230,126 +215,103 @@ int test_findExtremumOverLine(Equilibrium *solver_to_test, Equilibrium *solver_g
* @param solver The solver to use for computing the equilibrium robustness.
* @param comPositions Grid of CoM positions in the form of an Nx2 matrix.
*/
-void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium *solver,
- Cref_matrixXX comPositions, Cref_matrixXX p)
-{
- MatrixXi contactPointCoord(4*N_CONTACTS,2);
- VectorX minDistContactPoint = 1e10*VectorX::Ones(4*N_CONTACTS);
+void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium* solver, Cref_matrixXX comPositions,
+ Cref_matrixXX p) {
+ MatrixXi contactPointCoord(4 * N_CONTACTS, 2);
+ VectorX minDistContactPoint = 1e10 * VectorX::Ones(4 * N_CONTACTS);
// create grid of com positions to test
- for(unsigned int i=0; i<GRID_SIZE; i++)
- {
- for(unsigned int j=0; j<GRID_SIZE; j++)
- {
+ for (unsigned int i = 0; i < GRID_SIZE; i++) {
+ for (unsigned int j = 0; j < GRID_SIZE; j++) {
// look for contact point positions on grid
- for(long k=0; k<4*N_CONTACTS; k++)
- {
- double dist = (p.block<1,2>(k,0) - comPositions.block<1,2>(i*GRID_SIZE+j,0)).norm();
- if(dist < minDistContactPoint(k))
- {
+ for (long k = 0; k < 4 * N_CONTACTS; k++) {
+ double dist = (p.block<1, 2>(k, 0) - comPositions.block<1, 2>(i * GRID_SIZE + j, 0)).norm();
+ if (dist < minDistContactPoint(k)) {
minDistContactPoint(k) = dist;
- contactPointCoord(k,0) = i;
- contactPointCoord(k,1) = j;
+ contactPointCoord(k, 0) = i;
+ contactPointCoord(k, 1) = j;
}
}
}
}
- cout<<"\nContact point positions\n";
+ cout << "\nContact point positions\n";
bool contactPointDrawn;
- for(unsigned int i=0; i<GRID_SIZE; i++)
- {
- for(unsigned int j=0; j<GRID_SIZE; j++)
- {
+ for (unsigned int i = 0; i < GRID_SIZE; i++) {
+ for (unsigned int j = 0; j < GRID_SIZE; j++) {
contactPointDrawn = false;
- for(long k=0; k<4*N_CONTACTS; k++)
- {
- if(contactPointCoord(k,0)==i && contactPointCoord(k,1)==j)
- {
- cout<<"X ";
+ for (long k = 0; k < 4 * N_CONTACTS; k++) {
+ if (contactPointCoord(k, 0) == i && contactPointCoord(k, 1) == j) {
+ cout << "X ";
contactPointDrawn = true;
break;
}
}
- if(contactPointDrawn==false)
- cout<<"- ";
+ if (contactPointDrawn == false) cout << "- ";
}
printf("\n");
}
- cout<<"\nRobustness grid computed with solver "<<solver->getName()<<endl;
+ cout << "\nRobustness grid computed with solver " << solver->getName() << endl;
int grid_size = (int)sqrt(comPositions.rows());
- double rob ;
+ double rob;
LP_status status;
- for(unsigned int i=0; i<comPositions.rows(); i++)
- {
+ for (unsigned int i = 0; i < comPositions.rows(); i++) {
status = solver->computeEquilibriumRobustness(comPositions.row(i), rob);
- if(status!=LP_STATUS_OPTIMAL)
- {
- SEND_ERROR_MSG("Faild to compute equilibrium robustness of com position "+toString(comPositions.row(i))+", error code "+toString(status));
+ if (status != LP_STATUS_OPTIMAL) {
+ SEND_ERROR_MSG("Faild to compute equilibrium robustness of com position " + toString(comPositions.row(i)) +
+ ", error code " + toString(status));
rob = -1.0;
}
- if(rob>=0.0)
- {
- if(rob>9.0)
- rob = 9.0;
+ if (rob >= 0.0) {
+ if (rob > 9.0) rob = 9.0;
printf("%d ", (int)rob);
- }
- else
+ } else
printf("- ");
- if((i+1)%grid_size==0)
- printf("\n");
+ if ((i + 1) % grid_size == 0) printf("\n");
}
}
void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE, double LX, double LY,
- RVector3 &CONTACT_POINT_LOWER_BOUNDS,
- RVector3 &CONTACT_POINT_UPPER_BOUNDS,
- RVector3 &RPY_LOWER_BOUNDS,
- RVector3 &RPY_UPPER_BOUNDS,
- MatrixX3& p, MatrixX3& N)
-{
+ RVector3& CONTACT_POINT_LOWER_BOUNDS, RVector3& CONTACT_POINT_UPPER_BOUNDS,
+ RVector3& RPY_LOWER_BOUNDS, RVector3& RPY_UPPER_BOUNDS, MatrixX3& p, MatrixX3& N) {
MatrixXX contact_pos = MatrixXX::Zero(N_CONTACTS, 3);
MatrixXX contact_rpy = MatrixXX::Zero(N_CONTACTS, 3);
- p.setZero(4*N_CONTACTS,3); // contact points
- N.setZero(4*N_CONTACTS,3); // contact normals
+ p.setZero(4 * N_CONTACTS, 3); // contact points
+ N.setZero(4 * N_CONTACTS, 3); // contact normals
// Generate contact positions and orientations
bool collision;
- for(unsigned int i=0; i<N_CONTACTS; i++)
- {
- while(true) // generate contact position
+ for (unsigned int i = 0; i < N_CONTACTS; i++) {
+ while (true) // generate contact position
{
uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS, contact_pos.row(i));
- if(i==0)
- break;
+ if (i == 0) break;
collision = false;
- for(unsigned int j=0; j<i-1; j++)
- if((contact_pos.row(i)-contact_pos.row(j)).norm() < MIN_CONTACT_DISTANCE)
- collision = true;
- if(collision==false)
- break;
+ for (unsigned int j = 0; j < i - 1; j++)
+ if ((contact_pos.row(i) - contact_pos.row(j)).norm() < MIN_CONTACT_DISTANCE) collision = true;
+ if (collision == false) break;
}
-// generate contact orientation
+ // generate contact orientation
uniform(RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, contact_rpy.row(i));
- generate_rectangle_contacts(LX, LY, contact_pos.row(i), contact_rpy.row(i),
- p.middleRows<4>(i*4), N.middleRows<4>(i*4));
-// printf("Contact surface %d position (%.3f,%.3f,%.3f) ", i, contact_pos(i,0), contact_pos(i,1), contact_pos(i,2));
-// printf("Orientation (%.3f,%.3f,%.3f)\n", contact_rpy(i,0), contact_rpy(i,1), contact_rpy(i,2));
+ generate_rectangle_contacts(LX, LY, contact_pos.row(i), contact_rpy.row(i), p.middleRows<4>(i * 4),
+ N.middleRows<4>(i * 4));
+ // printf("Contact surface %d position (%.3f,%.3f,%.3f) ", i, contact_pos(i,0), contact_pos(i,1),
+ // contact_pos(i,2)); printf("Orientation (%.3f,%.3f,%.3f)\n", contact_rpy(i,0), contact_rpy(i,1),
+ // contact_rpy(i,2));
}
-// for(int i=0; i<p.rows(); i++)
-// {
-// printf("Contact point %d position (%.3f,%.3f,%.3f) ", i, p(i,0), p(i,1), p(i,2));
-// printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
-// }
+ // for(int i=0; i<p.rows(); i++)
+ // {
+ // printf("Contact point %d position (%.3f,%.3f,%.3f) ", i, p(i,0), p(i,1), p(i,2));
+ // printf("Normal (%.3f,%.3f,%.3f)\n", N(i,0), N(i,1), N(i,2));
+ // }
}
-void testWithLoadedData()
-{
- cout<<"*** TEST WITH LOADED DATA ***\n";
+void testWithLoadedData() {
+ cout << "*** TEST WITH LOADED DATA ***\n";
double mass = 55.8836;
double mu = 0.5; // friction coefficient
@@ -359,24 +321,19 @@ void testWithLoadedData()
const int N_SOLVERS = 3;
string solverNames[] = {"LP oases", "LP2 oases", "DLP oases"};
- EquilibriumAlgorithm algorithms[] = {EQUILIBRIUM_ALGORITHM_LP,
- EQUILIBRIUM_ALGORITHM_LP2,
- EQUILIBRIUM_ALGORITHM_DLP};
+ EquilibriumAlgorithm algorithms[] = {EQUILIBRIUM_ALGORITHM_LP, EQUILIBRIUM_ALGORITHM_LP2, EQUILIBRIUM_ALGORITHM_DLP};
MatrixXX contactPoints, contactNormals;
Vector3 com;
- if(!readMatrixFromFile(file_path+"positions.dat", contactPoints))
- {
+ if (!readMatrixFromFile(file_path + "positions.dat", contactPoints)) {
SEND_ERROR_MSG("Impossible to read positions from file");
return;
}
- if(!readMatrixFromFile(file_path+"normals.dat", contactNormals))
- {
+ if (!readMatrixFromFile(file_path + "normals.dat", contactNormals)) {
SEND_ERROR_MSG("Impossible to read normals from file");
return;
}
- if(!readMatrixFromFile(file_path+"com.dat", com))
- {
+ if (!readMatrixFromFile(file_path + "com.dat", com)) {
SEND_ERROR_MSG("Impossible to read com from file");
return;
}
@@ -386,36 +343,34 @@ void testWithLoadedData()
const MatrixX3& cn = contactNormals;
Equilibrium* solvers[N_SOLVERS];
double robustness[N_SOLVERS];
- for(int s=0; s<N_SOLVERS; s++)
- {
+ for (int s = 0; s < N_SOLVERS; s++) {
solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact, SOLVER_LP_QPOASES);
- if(!solvers[s]->setNewContacts(cp, cn, mu, algorithms[s]))
- {
- SEND_ERROR_MSG("Error while setting new contacts for solver "+solvers[s]->getName());
+ if (!solvers[s]->setNewContacts(cp, cn, mu, algorithms[s])) {
+ SEND_ERROR_MSG("Error while setting new contacts for solver " + solvers[s]->getName());
continue;
}
LP_status status = solvers[s]->computeEquilibriumRobustness(com, robustness[s]);
- if(status==LP_STATUS_OPTIMAL)
- {
- if(fabs(expected_robustness-robustness[s])>EPS)
- cout<<"[ERROR] Solver "<<solvers[s]->getName()<<" computed robustness "<<robustness[s]<<" rather than "<<expected_robustness<<endl;
- }
- else
- SEND_ERROR_MSG("Solver "+solvers[s]->getName()+" failed to compute robustness, error code "+toString(status));
+ if (status == LP_STATUS_OPTIMAL) {
+ if (fabs(expected_robustness - robustness[s]) > EPS)
+ cout << "[ERROR] Solver " << solvers[s]->getName() << " computed robustness " << robustness[s]
+ << " rather than " << expected_robustness << endl;
+ } else
+ SEND_ERROR_MSG("Solver " + solvers[s]->getName() + " failed to compute robustness, error code " +
+ toString(status));
}
- cout<<"*** END TEST WITH LOADED DATA ***\n\n";
+ cout << "*** END TEST WITH LOADED DATA ***\n\n";
}
-int main()
-{
+int main() {
testWithLoadedData();
- cout<<"*** TEST WITH RANDOMLY GENERATED DATA ***\n";
+ cout << "*** TEST WITH RANDOMLY GENERATED DATA ***\n";
unsigned int seed = (unsigned int)(time(NULL));
-// seed = 1446555515;
- srand (seed);
- cout<<"Initialize random number generator with seed "<<seed<<" (in case you wanna repeat the same test later)\n";
+ // seed = 1446555515;
+ srand(seed);
+ cout << "Initialize random number generator with seed " << seed
+ << " (in case you wanna repeat the same test later)\n";
RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
@@ -427,13 +382,13 @@ int main()
unsigned int generatorsPerContact = 4;
unsigned int N_CONTACTS = 2;
double MIN_CONTACT_DISTANCE = 0.3;
- double LX = 0.5*0.2172; // half contact surface size in x direction
- double LY = 0.5*0.138; // half contact surface size in y direction
- CONTACT_POINT_LOWER_BOUNDS << 0.0, 0.0, 0.0;
- CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 0.5;
- double gamma = atan(mu); // half friction cone angle
- RPY_LOWER_BOUNDS << -2*gamma, -2*gamma, -M_PI;
- RPY_UPPER_BOUNDS << +2*gamma, +2*gamma, +M_PI;
+ double LX = 0.5 * 0.2172; // half contact surface size in x direction
+ double LY = 0.5 * 0.138; // half contact surface size in y direction
+ CONTACT_POINT_LOWER_BOUNDS << 0.0, 0.0, 0.0;
+ CONTACT_POINT_UPPER_BOUNDS << 0.5, 0.5, 0.5;
+ double gamma = atan(mu); // half friction cone angle
+ RPY_LOWER_BOUNDS << -2 * gamma, -2 * gamma, -M_PI;
+ RPY_UPPER_BOUNDS << +2 * gamma, +2 * gamma, +M_PI;
double X_MARG = 0.07;
double Y_MARG = 0.07;
const int GRID_SIZE = 10;
@@ -442,122 +397,104 @@ int main()
#ifdef CLP_FOUND
const int N_SOLVERS = 6;
- string solverNames[] = {"LP oases", "LP2 oases", "DLP oases",
- "LP coin", "LP2 coin", "DLP coin"};
- EquilibriumAlgorithm algorithms[] = {EQUILIBRIUM_ALGORITHM_LP,
- EQUILIBRIUM_ALGORITHM_LP2,
- EQUILIBRIUM_ALGORITHM_DLP,
- EQUILIBRIUM_ALGORITHM_LP,
- EQUILIBRIUM_ALGORITHM_LP2,
- EQUILIBRIUM_ALGORITHM_DLP};
+ string solverNames[] = {"LP oases", "LP2 oases", "DLP oases", "LP coin", "LP2 coin", "DLP coin"};
+ EquilibriumAlgorithm algorithms[] = {EQUILIBRIUM_ALGORITHM_LP, EQUILIBRIUM_ALGORITHM_LP2, EQUILIBRIUM_ALGORITHM_DLP,
+ EQUILIBRIUM_ALGORITHM_LP, EQUILIBRIUM_ALGORITHM_LP2, EQUILIBRIUM_ALGORITHM_DLP};
SolverLP lp_solver_types[] = {SOLVER_LP_QPOASES, SOLVER_LP_QPOASES, SOLVER_LP_QPOASES,
- SOLVER_LP_CLP, SOLVER_LP_CLP, SOLVER_LP_CLP};
+ SOLVER_LP_CLP, SOLVER_LP_CLP, SOLVER_LP_CLP};
#else
const int N_SOLVERS = 3;
string solverNames[] = {"LP oases", "LP2 oases", "DLP oases"};
- EquilibriumAlgorithm algorithms[] = {EQUILIBRIUM_ALGORITHM_LP,
- EQUILIBRIUM_ALGORITHM_LP2,
- EQUILIBRIUM_ALGORITHM_DLP};
+ EquilibriumAlgorithm algorithms[] = {EQUILIBRIUM_ALGORITHM_LP, EQUILIBRIUM_ALGORITHM_LP2, EQUILIBRIUM_ALGORITHM_DLP};
SolverLP lp_solver_types[] = {SOLVER_LP_QPOASES, SOLVER_LP_QPOASES, SOLVER_LP_QPOASES};
#endif
- cout<<"Number of contacts: "<<N_CONTACTS<<endl;
- cout<<"Number of generators per contact: "<<generatorsPerContact<<endl;
- cout<<"Gonna test equilibrium on a 2d grid of "<<GRID_SIZE<<"X"<<GRID_SIZE<<" points "<<endl;
+ cout << "Number of contacts: " << N_CONTACTS << endl;
+ cout << "Number of generators per contact: " << generatorsPerContact << endl;
+ cout << "Gonna test equilibrium on a 2d grid of " << GRID_SIZE << "X" << GRID_SIZE << " points " << endl;
Equilibrium* solver_PP = new Equilibrium("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES);
Equilibrium* solvers[N_SOLVERS];
- for(int s=0; s<N_SOLVERS; s++)
+ for (int s = 0; s < N_SOLVERS; s++)
solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact, lp_solver_types[s]);
MatrixX3 p, N;
RVector2 com_LB, com_UB;
VectorX x_range(GRID_SIZE), y_range(GRID_SIZE);
- MatrixXX comPositions(GRID_SIZE*GRID_SIZE, 3);
- for(unsigned n_test=0; n_test<N_TESTS; n_test++)
- {
- generateContacts(N_CONTACTS, MIN_CONTACT_DISTANCE, LX, LY,
- CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
+ MatrixXX comPositions(GRID_SIZE * GRID_SIZE, 3);
+ for (unsigned n_test = 0; n_test < N_TESTS; n_test++) {
+ generateContacts(N_CONTACTS, MIN_CONTACT_DISTANCE, LX, LY, CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, p, N);
- for(int s=0; s<N_SOLVERS; s++)
- {
+ for (int s = 0; s < N_SOLVERS; s++) {
getProfiler().start(PERF_LP_PREPARATION);
- if(!solvers[s]->setNewContacts(p, N, mu, algorithms[s]))
- {
- SEND_ERROR_MSG("Error while setting new contacts for solver "+solvers[s]->getName());
+ if (!solvers[s]->setNewContacts(p, N, mu, algorithms[s])) {
+ SEND_ERROR_MSG("Error while setting new contacts for solver " + solvers[s]->getName());
return -1;
}
getProfiler().stop(PERF_LP_PREPARATION);
}
getProfiler().start(PERF_PP);
- if(!solver_PP->setNewContacts(p, N, mu, EQUILIBRIUM_ALGORITHM_PP))
- {
- SEND_ERROR_MSG("Error while setting new contacts for solver "+solver_PP->getName());
+ if (!solver_PP->setNewContacts(p, N, mu, EQUILIBRIUM_ALGORITHM_PP)) {
+ SEND_ERROR_MSG("Error while setting new contacts for solver " + solver_PP->getName());
return -1;
}
getProfiler().stop(PERF_PP);
// compute upper and lower bounds of com positions to test
- com_LB(0) = p.col(0).minCoeff()-X_MARG;
- com_UB(0) = p.col(0).maxCoeff()+X_MARG;
- com_LB(1) = p.col(1).minCoeff()-Y_MARG;
- com_UB(1) = p.col(1).maxCoeff()+Y_MARG;
+ com_LB(0) = p.col(0).minCoeff() - X_MARG;
+ com_UB(0) = p.col(0).maxCoeff() + X_MARG;
+ com_LB(1) = p.col(1).minCoeff() - Y_MARG;
+ com_UB(1) = p.col(1).maxCoeff() + Y_MARG;
// create grid of com positions to test
- x_range.setLinSpaced(GRID_SIZE,com_LB(0),com_UB(0));
- y_range.setLinSpaced(GRID_SIZE,com_LB(1),com_UB(1));
-// cout<<"ranging from "<<com_LB<<" to "<<com_UB<<endl;
- for(unsigned int i=0; i<GRID_SIZE; i++)
- {
- for(unsigned int j=0; j<GRID_SIZE; j++)
- {
- comPositions(i*GRID_SIZE+j, 1) = y_range(GRID_SIZE-1-i);
- comPositions(i*GRID_SIZE+j, 0) = x_range(j);
- comPositions(i*GRID_SIZE+j, 2) = 0.0;
+ x_range.setLinSpaced(GRID_SIZE, com_LB(0), com_UB(0));
+ y_range.setLinSpaced(GRID_SIZE, com_LB(1), com_UB(1));
+ // cout<<"ranging from "<<com_LB<<" to "<<com_UB<<endl;
+ for (unsigned int i = 0; i < GRID_SIZE; i++) {
+ for (unsigned int j = 0; j < GRID_SIZE; j++) {
+ comPositions(i * GRID_SIZE + j, 1) = y_range(GRID_SIZE - 1 - i);
+ comPositions(i * GRID_SIZE + j, 0) = x_range(j);
+ comPositions(i * GRID_SIZE + j, 2) = 0.0;
}
}
- if(DRAW_CONTACT_POINTS)
- drawRobustnessGrid(N_CONTACTS, GRID_SIZE, solvers[0], comPositions, p);
+ if (DRAW_CONTACT_POINTS) drawRobustnessGrid(N_CONTACTS, GRID_SIZE, solvers[0], comPositions, p);
string test_name = "Compute equilibrium robustness ";
- for(int s=1; s<N_SOLVERS; s++)
- {
- test_computeEquilibriumRobustness(solvers[0], solvers[s], comPositions, test_name+solvers[0]->getName(),
- test_name+solvers[s]->getName(), 1);
+ for (int s = 1; s < N_SOLVERS; s++) {
+ test_computeEquilibriumRobustness(solvers[0], solvers[s], comPositions, test_name + solvers[0]->getName(),
+ test_name + solvers[s]->getName(), 1);
}
- for(int s=0; s<N_SOLVERS; s++)
- {
+ for (int s = 0; s < N_SOLVERS; s++) {
test_computeEquilibriumRobustness_vs_checkEquilibrium(solvers[s], solver_PP, comPositions,
- test_name+solvers[s]->getName(), "", 1);
+ test_name + solvers[s]->getName(), "", 1);
}
const int N_TESTS_EXTREMUM = 100;
Vector3 a0 = Vector3::Zero();
- a0.head<2>() = 0.5*(com_LB+com_UB);
+ a0.head<2>() = 0.5 * (com_LB + com_UB);
double e_max;
LP_status status = solvers[0]->computeEquilibriumRobustness(a0, e_max);
- if(status!=LP_STATUS_OPTIMAL)
- SEND_ERROR_MSG(solvers[0]->getName()+" failed to compute robustness of com position "+toString(a0.transpose())+", error code: "+toString(status));
- else
- {
+ if (status != LP_STATUS_OPTIMAL)
+ SEND_ERROR_MSG(solvers[0]->getName() + " failed to compute robustness of com position " +
+ toString(a0.transpose()) + ", error code: " + toString(status));
+ else {
test_name = "EXTREMUM OVER LINE ";
string test_name2 = "Compute equilibrium robustness ";
- for(int s=1; s<N_SOLVERS; s++)
- {
- if(solvers[s]->getAlgorithm()!=EQUILIBRIUM_ALGORITHM_LP2)
- test_findExtremumOverLine(solvers[s], solvers[0], a0, N_TESTS_EXTREMUM, e_max, test_name+solvers[s]->getName(),
- test_name2+solvers[0]->getName(), 1);
+ for (int s = 1; s < N_SOLVERS; s++) {
+ if (solvers[s]->getAlgorithm() != EQUILIBRIUM_ALGORITHM_LP2)
+ test_findExtremumOverLine(solvers[s], solvers[0], a0, N_TESTS_EXTREMUM, e_max,
+ test_name + solvers[s]->getName(), test_name2 + solvers[0]->getName(), 1);
}
}
}
getProfiler().report_all();
- cout<<"*** END TEST WITH RANDOMLY GENERATED DATA ***\n";
+ cout << "*** END TEST WITH RANDOMLY GENERATED DATA ***\n";
return 0;
}
--
GitLab