From 0d56f35febda3b6e602690c5d5585fc61f1e37ce Mon Sep 17 00:00:00 2001
From: Guilhem Saurel <guilhem.saurel@laas.fr>
Date: Mon, 25 Mar 2019 18:07:13 +0100
Subject: [PATCH] Revert "Cleaning"
---
.yapfignore | 1 -
CMakeLists.txt | 65 +-
cmake | 2 +-
.../hpp/centroidal-dynamics/CMakeLists.txt | 3 +-
.../centroidal_dynamics.hh | 98 +--
.../hpp/centroidal-dynamics/local_config.hh | 56 +-
include/hpp/centroidal-dynamics/logger.hh | 249 +++----
.../centroidal-dynamics/solver_LP_abstract.hh | 85 ++-
.../hpp/centroidal-dynamics/solver_LP_clp.hh | 22 +-
.../centroidal-dynamics/solver_LP_qpoases.hh | 35 +-
include/hpp/centroidal-dynamics/stop-watch.hh | 66 +-
include/hpp/centroidal-dynamics/util.hh | 273 +++----
python/CMakeLists.txt | 21 +-
python/centroidal_dynamics_python.cpp | 191 ++---
python/test/binding_tests.py | 107 ++-
setup.cfg | 10 -
src/CMakeLists.txt | 14 +-
src/centroidal_dynamics.cpp | 668 ++++++++++--------
src/logger.cpp | 148 ++--
src/solver_LP_abstract.cpp | 99 +--
src/solver_LP_clp.cpp | 80 ++-
src/solver_LP_qpoases.cpp | 120 ++--
src/stop-watch.cpp | 177 +++--
src/util.cpp | 190 ++---
test/CMakeLists.txt | 6 +
test/test_LP_solvers.cpp | 287 ++++----
test/test_static_equilibrium.cpp | 471 ++++++------
27 files changed, 1979 insertions(+), 1565 deletions(-)
delete mode 100644 .yapfignore
delete mode 100644 setup.cfg
diff --git a/.yapfignore b/.yapfignore
deleted file mode 100644
index a3ea3e4..0000000
--- a/.yapfignore
+++ /dev/null
@@ -1 +0,0 @@
-cmake
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 7b8d193..4c1b0d3 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,4 +1,4 @@
-CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+cmake_minimum_required(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/test.cmake)
@@ -7,49 +7,60 @@ INCLUDE(cmake/hpp.cmake)
SET(PROJECT_NAME hpp-centroidal-dynamics)
SET(PROJECT_DESCRIPTION
- "Utility classes for testing (robust) equilibrium of a system in contact with the environment, and other centroidal dynamics methods."
- )
+ "Utility classes for testing (robust) equilibrium of a system in contact with the environment, and other centroidal dynamics methods."
+ )
SET(CUSTOM_HEADER_DIR "hpp/centroidal-dynamics")
SETUP_HPP_PROJECT()
-OPTION(BUILD_PYTHON_INTERFACE "Build the python binding" ON)
+# Inhibit all warning messages.
+#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w")
+
+# remove flag that makes all warnings into errors
+string (REPLACE "-Werror" "" CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS})
+MESSAGE( STATUS "CMAKE_CXX_FLAGS: " ${CMAKE_CXX_FLAGS} )
+
+OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
IF(BUILD_PYTHON_INTERFACE)
- FINDPYTHON()
- INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
+# search for python
+ FINDPYTHON(2.7 EXACT REQUIRED)
+ find_package( PythonLibs 2.7 REQUIRED )
+ include_directories( ${PYTHON_INCLUDE_DIRS} )
- ADD_REQUIRED_DEPENDENCY("eigenpy >= 1.4.0")
+ find_package( Boost COMPONENTS python REQUIRED )
+ include_directories( ${Boost_INCLUDE_DIR} )
- FIND_PACKAGE(Boost COMPONENTS python REQUIRED )
- INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIR})
ENDIF(BUILD_PYTHON_INTERFACE)
ADD_REQUIRED_DEPENDENCY("eigen3")
-INCLUDE_DIRECTORIES(SYSTEM ${EIGEN3_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
+
+set(CMAKE_MODULE_PATH
+ ${PROJECT_SOURCE_DIR}/cmake/find-external/CDD
+ ${PROJECT_SOURCE_DIR}/cmake/find-external/CLP
+ ${PROJECT_SOURCE_DIR}/cmake/find-external/qpOASES
+ )
-SET(CMAKE_MODULE_PATH
- ${PROJECT_SOURCE_DIR}/cmake/find-external/CDD
- ${PROJECT_SOURCE_DIR}/cmake/find-external/CLP
- ${PROJECT_SOURCE_DIR}/cmake/find-external/qpOASES
- )
+find_package(CDD REQUIRED)
+find_package(qpOASES REQUIRED)
+find_package(CLP)
-FIND_PACKAGE(CDD REQUIRED)
-FIND_PACKAGE(qpOASES REQUIRED)
-FIND_PACKAGE(CLP)
+IF("${CLP_LIBRARY}" STREQUAL "CLP_LIBRARY-NOTFOUND")
+ message(STATUS "CLP_LIBRARY equal to CLP_LIBRARY-NOTFOUND so I assume CLP was not found ")
+else()
+ message(STATUS "CLP library found, defining macro CLP_FOUND")
+ add_definitions(-DCLP_FOUND)
+endif()
-IF(CLP_FOUND)
- ADD_DEFINITIONS(-DCLP_FOUND)
- INCLUDE_DIRECTORIES("${CLP_INCLUDE_DIR}")
-ENDIF()
+INCLUDE_DIRECTORIES(${qpOASES_INCLUDE_DIRS})
-INCLUDE_DIRECTORIES(SYSTEM ${qpOASES_INCLUDE_DIRS})
-ADD_SUBDIRECTORY(include/${CUSTOM_HEADER_DIR})
-ADD_SUBDIRECTORY(src)
-ADD_SUBDIRECTORY(test)
+ADD_SUBDIRECTORY (include/${CUSTOM_HEADER_DIR})
+add_subdirectory (src)
+add_subdirectory (test)
IF(BUILD_PYTHON_INTERFACE)
- ADD_SUBDIRECTORY(python)
+ add_subdirectory (python)
ENDIF(BUILD_PYTHON_INTERFACE)
SETUP_PROJECT_FINALIZE()
diff --git a/cmake b/cmake
index f34901f..5c8c19f 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit f34901f143d843b48dfdb8d9e904503ed96e2310
+Subproject commit 5c8c19f491f2c6f8488f5f37ff81d711d69dbb3f
diff --git a/include/hpp/centroidal-dynamics/CMakeLists.txt b/include/hpp/centroidal-dynamics/CMakeLists.txt
index 0128562..b2b70d8 100644
--- a/include/hpp/centroidal-dynamics/CMakeLists.txt
+++ b/include/hpp/centroidal-dynamics/CMakeLists.txt
@@ -1,3 +1,4 @@
+# Declare Headers
SET(${PROJECT_NAME}_HEADERS
local_config.hh
util.hh
@@ -11,5 +12,5 @@ SET(${PROJECT_NAME}_HEADERS
INSTALL(FILES
${${PROJECT_NAME}_HEADERS}
- DESTINATION include/${CUSTOM_HEADER_DIR}
+ DESTINATION include/hpp/centroidal-dynamics
)
diff --git a/include/hpp/centroidal-dynamics/centroidal_dynamics.hh b/include/hpp/centroidal-dynamics/centroidal_dynamics.hh
index c73add3..cf6badc 100644
--- a/include/hpp/centroidal-dynamics/centroidal_dynamics.hh
+++ b/include/hpp/centroidal-dynamics/centroidal_dynamics.hh
@@ -11,33 +11,37 @@
#include <hpp/centroidal-dynamics/util.hh>
#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
-namespace centroidal_dynamics {
-
-enum CENTROIDAL_DYNAMICS_DLLAPI EquilibriumAlgorithm {
- EQUILIBRIUM_ALGORITHM_LP, /// primal LP formulation
- EQUILIBRIUM_ALGORITHM_LP2, /// another primal LP formulation
- EQUILIBRIUM_ALGORITHM_DLP, /// dual LP formulation
- EQUILIBRIUM_ALGORITHM_PP, /// polytope projection algorithm
- EQUILIBRIUM_ALGORITHM_IP, /// incremental projection algorithm based on primal LP formulation
- EQUILIBRIUM_ALGORITHM_DIP /// incremental projection algorithm based on dual LP formulation
+namespace centroidal_dynamics
+{
+
+enum CENTROIDAL_DYNAMICS_DLLAPI EquilibriumAlgorithm
+{
+ EQUILIBRIUM_ALGORITHM_LP, /// primal LP formulation
+ EQUILIBRIUM_ALGORITHM_LP2, /// another primal LP formulation
+ EQUILIBRIUM_ALGORITHM_DLP, /// dual LP formulation
+ EQUILIBRIUM_ALGORITHM_PP, /// polytope projection algorithm
+ EQUILIBRIUM_ALGORITHM_IP, /// incremental projection algorithm based on primal LP formulation
+ EQUILIBRIUM_ALGORITHM_DIP /// incremental projection algorithm based on dual LP formulation
};
-class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
- public:
- const double m_mass; /// mass of the system
- const Vector3 m_gravity; /// gravity vector
+class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium
+{
+public:
+ const double m_mass; /// mass of the system
+ const Vector3 m_gravity; /// gravity vector
/** Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact) */
Matrix6X m_G_centr;
- private:
- static bool m_is_cdd_initialized; /// true if cdd lib has been initialized, false otherwise
+private:
+ static bool m_is_cdd_initialized; /// true if cdd lib has been initialized, false otherwise
- std::string m_name; /// name of this object
- EquilibriumAlgorithm m_algorithm; /// current algorithm used
- SolverLP m_solver_type; /// type of LP solver
- Solver_LP_abstract* m_solver; /// LP solver
+ std::string m_name; /// name of this object
+ EquilibriumAlgorithm m_algorithm; /// current algorithm used
+ SolverLP m_solver_type; /// type of LP solver
+ Solver_LP_abstract* m_solver; /// LP solver
+
+ unsigned int m_generatorsPerContact; /// number of generators to approximate the friction cone per contact point
- unsigned int m_generatorsPerContact; /// number of generators to approximate the friction cone per contact point
/** Inequality matrix and vector defining the gravito-inertial wrench cone H w <= h */
MatrixXX m_H;
@@ -45,15 +49,15 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
/** False if a numerical instability appeared in the computation H and h*/
bool m_is_cdd_stable;
/** EQUILIBRIUM_ALGORITHM_PP: If double description fails,
- * indicate the max number of attempts to compute the cone by introducing
- * a small pertubation of the system */
+ * indicate the max number of attempts to compute the cone by introducing
+ * a small pertubation of the system */
const unsigned max_num_cdd_trials;
/** whether to remove redundant inequalities when computing double description matrices*/
const bool canonicalize_cdd_matrix;
/** Inequality matrix and vector defining the CoM support polygon HD com + Hd <= h */
MatrixX3 m_HD;
- VectorX m_Hd;
+ VectorX m_Hd;
/** Matrix and vector mapping 2d com position to GIW */
Matrix63 m_D;
@@ -63,8 +67,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
double m_b0_to_emax_coefficient;
bool computePolytopeProjection(Cref_matrix6X v);
- bool computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals, double frictionCoefficient,
- const bool perturbate = false);
+ bool computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
+ double frictionCoefficient, const bool perturbate = false);
/**
* @brief Given the smallest coefficient of the contact force generators it computes
@@ -78,8 +82,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
double convert_emax_to_b0(double emax);
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/**
* @brief Equilibrium constructor.
* @param name Name of the object.
@@ -92,8 +96,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* a small pertubation of the system
*/
Equilibrium(const std::string& name, const double mass, const unsigned int generatorsPerContact,
- const SolverLP solver_type = SOLVER_LP_QPOASES, bool useWarmStart = true,
- const unsigned int max_num_cdd_trials = 0, const bool canonicalize_cdd_matrix = false);
+ const SolverLP solver_type = SOLVER_LP_QPOASES, bool useWarmStart=true, const unsigned int max_num_cdd_trials=0,
+ const bool canonicalize_cdd_matrix=false);
+
Equilibrium(const Equilibrium& other);
@@ -101,21 +106,21 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @brief Returns the useWarmStart flag.
* @return True if the LP solver is allowed to use warm start, false otherwise.
*/
- bool useWarmStart() { return m_solver->getUseWarmStart(); }
+ bool useWarmStart(){ return m_solver->getUseWarmStart(); }
/**
* @brief Specifies whether the LP solver is allowed to use warm start.
* @param uws True if the LP solver is allowed to use warm start, false otherwise.
*/
- void setUseWarmStart(bool uws) { m_solver->setUseWarmStart(uws); }
+ void setUseWarmStart(bool uws){ m_solver->setUseWarmStart(uws); }
/**
* @brief Get the name of this object.
* @return The name of this object.
*/
- std::string getName() { return m_name; }
+ std::string getName(){ return m_name; }
- EquilibriumAlgorithm getAlgorithm() { return m_algorithm; }
+ EquilibriumAlgorithm getAlgorithm(){ return m_algorithm; }
void setAlgorithm(EquilibriumAlgorithm algorithm);
@@ -130,8 +135,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @param alg Algorithm to use for testing equilibrium.
* @return True if the operation succeeded, false otherwise.
*/
- bool setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals, const double frictionCoefficient,
- const EquilibriumAlgorithm alg);
+ bool setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
+ const double frictionCoefficient, const EquilibriumAlgorithm alg);
/**
* @brief Specify a new set of contacts.
@@ -144,10 +149,10 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @param alg Algorithm to use for testing equilibrium.
* @return True if the operation succeeded, false otherwise.
*/
- bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
+ bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
const double frictionCoefficient, const EquilibriumAlgorithm alg);
- void setG(Cref_matrix6X G) { m_G_centr = G; }
+ void setG(Cref_matrix6X G){m_G_centr = G;}
/**
* @brief Compute a measure of the robustness of the equilibrium of the specified com position.
@@ -170,7 +175,7 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* system can reach infinite robustness. This is due to the fact that we are not considering
* any upper limit for the friction cones.
*/
- LP_status computeEquilibriumRobustness(Cref_vector3 com, double& robustness);
+ LP_status computeEquilibriumRobustness(Cref_vector3 com, double &robustness);
/**
* @brief Compute a measure of the robustness of the equilibrium of the specified com position.
@@ -194,7 +199,7 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* system can reach infinite robustness. This is due to the fact that we are not considering
* any upper limit for the friction cones.
*/
- LP_status computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double& robustness);
+ LP_status computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness);
/**
* @brief Check whether the specified com position is in robust equilibrium.
@@ -215,7 +220,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @param e_max Desired robustness level.
* @return The status of the LP solver.
*/
- LP_status checkRobustEquilibrium(Cref_vector3 com, bool& equilibrium, double e_max = 0.0);
+ LP_status checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max=0.0);
+
/**
* @brief Check whether the specified com position is in robust equilibrium.
@@ -237,7 +243,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @param e_max Desired robustness level.
* @return The status of the LP solver.
*/
- LP_status checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool& equilibrium, double e_max = 0.0);
+ LP_status checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max=0.0);
+
/**
* @brief Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
@@ -260,7 +267,7 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @note If the system is in force closure the status will be LP_STATUS_UNBOUNDED, meaning that the
* system can reach infinite robustness. This is due to the fact that we are not considering
* any upper limit for the friction cones.
- */
+ */
LP_status findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com);
/**
@@ -286,7 +293,7 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* system can reach infinite robustness. This is due to the fact that we are not considering
* any upper limit for the friction cones.
*/
- LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max = 0.0);
+ LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max=0.0);
/**
* @brief Retrieve the inequalities that define the admissible wrenchs
@@ -336,9 +343,10 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @param a the acceleration
* @return true if the acceleration is admissible, false otherwise
*/
- bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a);
+ bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a );
+
};
-} // end namespace centroidal_dynamics
+} // end namespace centroidal_dynamics
#endif
diff --git a/include/hpp/centroidal-dynamics/local_config.hh b/include/hpp/centroidal-dynamics/local_config.hh
index 48033a0..8d60cbe 100644
--- a/include/hpp/centroidal-dynamics/local_config.hh
+++ b/include/hpp/centroidal-dynamics/local_config.hh
@@ -7,7 +7,7 @@
#define _CENTROIDAL_DYNAMICS_LIB_CONFIG_HH
// Package version (header).
-#define CENTROIDAL_DYNAMICS_VERSION "UNKNOWN"
+# define CENTROIDAL_DYNAMICS_VERSION "UNKNOWN"
// Handle portable symbol export.
// Defining manually which symbol should be exported is required
@@ -19,39 +19,39 @@
//
// On Linux, set the visibility accordingly. If C++ symbol visibility
// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility
-#if defined _WIN32 || defined __CYGWIN__
+# if defined _WIN32 || defined __CYGWIN__
// On Microsoft Windows, use dllimport and dllexport to tag symbols.
-#define CENTROIDAL_DYNAMICS_DLLIMPORT __declspec(dllimport)
-#define CENTROIDAL_DYNAMICS_DLLEXPORT __declspec(dllexport)
-#define CENTROIDAL_DYNAMICS_DLLLOCAL
-#else
+# define CENTROIDAL_DYNAMICS_DLLIMPORT __declspec(dllimport)
+# define CENTROIDAL_DYNAMICS_DLLEXPORT __declspec(dllexport)
+# define CENTROIDAL_DYNAMICS_DLLLOCAL
+# else
// On Linux, for GCC >= 4, tag symbols using GCC extension.
-#if __GNUC__ >= 4
-#define CENTROIDAL_DYNAMICS_DLLIMPORT __attribute__((visibility("default")))
-#define CENTROIDAL_DYNAMICS_DLLEXPORT __attribute__((visibility("default")))
-#define CENTROIDAL_DYNAMICS_DLLLOCAL __attribute__((visibility("hidden")))
-#else
+# if __GNUC__ >= 4
+# define CENTROIDAL_DYNAMICS_DLLIMPORT __attribute__ ((visibility("default")))
+# define CENTROIDAL_DYNAMICS_DLLEXPORT __attribute__ ((visibility("default")))
+# define CENTROIDAL_DYNAMICS_DLLLOCAL __attribute__ ((visibility("hidden")))
+# else
// Otherwise (GCC < 4 or another compiler is used), export everything.
-#define CENTROIDAL_DYNAMICS_DLLIMPORT
-#define CENTROIDAL_DYNAMICS_DLLEXPORT
-#define CENTROIDAL_DYNAMICS_DLLLOCAL
-#endif // __GNUC__ >= 4
-#endif // defined _WIN32 || defined __CYGWIN__
+# define CENTROIDAL_DYNAMICS_DLLIMPORT
+# define CENTROIDAL_DYNAMICS_DLLEXPORT
+# define CENTROIDAL_DYNAMICS_DLLLOCAL
+# endif // __GNUC__ >= 4
+# endif // defined _WIN32 || defined __CYGWIN__
-#ifdef CENTROIDAL_DYNAMICS_STATIC
+# ifdef CENTROIDAL_DYNAMICS_STATIC
// If one is using the library statically, get rid of
// extra information.
-#define CENTROIDAL_DYNAMICS_DLLAPI
-#define CENTROIDAL_DYNAMICS_LOCAL
-#else
+# define CENTROIDAL_DYNAMICS_DLLAPI
+# define CENTROIDAL_DYNAMICS_LOCAL
+# else
// Depending on whether one is building or using the
// library define DLLAPI to import or export.
-#ifdef CENTROIDAL_DYNAMICS_EXPORTS
-#define CENTROIDAL_DYNAMICS_DLLAPI CENTROIDAL_DYNAMICS_DLLEXPORT
-#else
-#define CENTROIDAL_DYNAMICS_DLLAPI CENTROIDAL_DYNAMICS_DLLIMPORT
-#endif // CENTROIDAL_DYNAMICS_EXPORTS
-#define CENTROIDAL_DYNAMICS_LOCAL CENTROIDAL_DYNAMICS_DLLLOCAL
-#endif // CENTROIDAL_DYNAMICS_STATIC
+# ifdef CENTROIDAL_DYNAMICS_EXPORTS
+# define CENTROIDAL_DYNAMICS_DLLAPI CENTROIDAL_DYNAMICS_DLLEXPORT
+# else
+# define CENTROIDAL_DYNAMICS_DLLAPI CENTROIDAL_DYNAMICS_DLLIMPORT
+# endif // CENTROIDAL_DYNAMICS_EXPORTS
+# define CENTROIDAL_DYNAMICS_LOCAL CENTROIDAL_DYNAMICS_DLLLOCAL
+# endif // CENTROIDAL_DYNAMICS_STATIC
-#endif //_CENTROIDAL_DYNAMICS_LIB_CONFIG_HH
+#endif //_CENTROIDAL_DYNAMICS_LIB_CONFIG_HH
diff --git a/include/hpp/centroidal-dynamics/logger.hh b/include/hpp/centroidal-dynamics/logger.hh
index fc6d949..8f861dc 100644
--- a/include/hpp/centroidal-dynamics/logger.hh
+++ b/include/hpp/centroidal-dynamics/logger.hh
@@ -16,7 +16,8 @@
#include <map>
#include "boost/assign.hpp"
-namespace centroidal_dynamics {
+namespace centroidal_dynamics
+{
//#define LOGGER_VERBOSITY_ERROR
//#define LOGGER_VERBOSITY_WARNING_ERROR
@@ -24,152 +25,164 @@ namespace centroidal_dynamics {
//#define LOGGER_VERBOSITY_ALL
#define LOGGER_VERBOSITY_ALL
-#define SEND_MSG(msg, type) getLogger().sendMsg(msg, type, __FILE__, __LINE__)
+#define SEND_MSG(msg,type) getLogger().sendMsg(msg,type,__FILE__,__LINE__)
#ifdef LOGGER_VERBOSITY_ERROR
#define SEND_DEBUG_MSG(msg)
#define SEND_INFO_MSG(msg)
#define SEND_WARNING_MSG(msg)
-#define SEND_ERROR_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR)
+#define SEND_ERROR_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR)
#define SEND_DEBUG_STREAM_MSG(msg)
#define SEND_INFO_STREAM_MSG(msg)
#define SEND_WARNING_STREAM_MSG(msg)
-#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR_STREAM)
+#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
#endif
#ifdef LOGGER_VERBOSITY_WARNING_ERROR
#define SEND_DEBUG_MSG(msg)
#define SEND_INFO_MSG(msg)
-#define SEND_WARNING_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING)
-#define SEND_ERROR_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR)
+#define SEND_WARNING_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING)
+#define SEND_ERROR_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR)
#define SEND_DEBUG_STREAM_MSG(msg)
-#define SEND_INFO_STREAM_MSG(msg) #define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING_STREAM)
-#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR_STREAM)
+#define SEND_INFO_STREAM_MSG(msg)\
+#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
+#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
#endif
#ifdef LOGGER_VERBOSITY_INFO_WARNING_ERROR
#define SEND_DEBUG_MSG(msg)
-#define SEND_INFO_MSG(msg) SEND_MSG(msg, MSG_TYPE_INFO)
-#define SEND_WARNING_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING)
-#define SEND_ERROR_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR)
+#define SEND_INFO_MSG(msg) SEND_MSG(msg,MSG_TYPE_INFO)
+#define SEND_WARNING_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING)
+#define SEND_ERROR_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR)
#define SEND_DEBUG_STREAM_MSG(msg)
-#define SEND_INFO_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_INFO_STREAM)
-#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING_STREAM)
-#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR_STREAM)
+#define SEND_INFO_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_INFO_STREAM)
+#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
+#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
#endif
#ifdef LOGGER_VERBOSITY_ALL
-#define SEND_DEBUG_MSG(msg) SEND_MSG(msg, MSG_TYPE_DEBUG)
-#define SEND_INFO_MSG(msg) SEND_MSG(msg, MSG_TYPE_INFO)
-#define SEND_WARNING_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING)
-#define SEND_ERROR_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR)
-#define SEND_DEBUG_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_DEBUG_STREAM)
-#define SEND_INFO_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_INFO_STREAM)
-#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_WARNING_STREAM)
-#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg, MSG_TYPE_ERROR_STREAM)
+#define SEND_DEBUG_MSG(msg) SEND_MSG(msg,MSG_TYPE_DEBUG)
+#define SEND_INFO_MSG(msg) SEND_MSG(msg,MSG_TYPE_INFO)
+#define SEND_WARNING_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING)
+#define SEND_ERROR_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR)
+#define SEND_DEBUG_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_DEBUG_STREAM)
+#define SEND_INFO_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_INFO_STREAM)
+#define SEND_WARNING_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_WARNING_STREAM)
+#define SEND_ERROR_STREAM_MSG(msg) SEND_MSG(msg,MSG_TYPE_ERROR_STREAM)
#endif
-/** Enum representing the different kind of messages.
- */
-enum CENTROIDAL_DYNAMICS_DLLAPI MsgType {
- MSG_TYPE_DEBUG = 0,
- MSG_TYPE_INFO = 1,
- MSG_TYPE_WARNING = 2,
- MSG_TYPE_ERROR = 3,
- MSG_TYPE_DEBUG_STREAM = 4,
- MSG_TYPE_INFO_STREAM = 5,
- MSG_TYPE_WARNING_STREAM = 6,
- MSG_TYPE_ERROR_STREAM = 7
-};
-
-template <typename T>
-std::string toString(const T& v) {
- std::stringstream ss;
- ss << v;
- return ss.str();
-}
-
-template <typename T>
-std::string toString(const std::vector<T>& v, const std::string separator = ", ") {
- std::stringstream ss;
- for (int i = 0; i < v.size() - 1; i++) ss << v[i] << separator;
- ss << v[v.size() - 1];
- return ss.str();
-}
-
-template <typename T, int n>
-std::string toString(const Eigen::MatrixBase<T>& v, const std::string separator = ", ") {
- if (v.rows() > v.cols()) return toString(v.transpose(), separator);
- std::stringstream ss;
- ss << v;
- return ss.str();
-}
-
-enum CENTROIDAL_DYNAMICS_DLLAPI LoggerVerbosity {
- VERBOSITY_ALL,
- VERBOSITY_INFO_WARNING_ERROR,
- VERBOSITY_WARNING_ERROR,
- VERBOSITY_ERROR,
- VERBOSITY_NONE
-};
-
-/** A simple class for logging messages
- */
-class CENTROIDAL_DYNAMICS_DLLAPI Logger {
- public:
- /** Constructor */
- Logger(double timeSample = 0.001, double streamPrintPeriod = 1.0);
-
- /** Destructor */
- ~Logger() {}
-
- /** Method to be called at every control iteration
- * to decrement the internal Logger's counter. */
- void countdown();
-
- /** Print the specified message on standard output if the verbosity level
- * allows it. The file name and the line number are used to identify
- * the point where sendMsg is called so that streaming messages are
- * printed only every streamPrintPeriod iterations.
- */
- void sendMsg(std::string msg, MsgType type, const char* file = "", int line = 0);
-
- /** Set the sampling time at which the method countdown()
- * is going to be called. */
- bool setTimeSample(double t);
-
- /** Set the time period for printing of streaming messages. */
- bool setStreamPrintPeriod(double s);
-
- /** Set the verbosity level of the logger. */
- void setVerbosity(LoggerVerbosity lv);
-
- protected:
- LoggerVerbosity m_lv; /// verbosity of the logger
- double m_timeSample; /// specify the period of call of the countdown method
- double m_streamPrintPeriod; /// specify the time period of the stream prints
- double m_printCountdown; /// every time this is < 0 (i.e. every _streamPrintPeriod sec) print stuff
-
- /** Pointer to the dynamic structure which holds the collection of streaming messages */
- std::map<std::string, double> m_stream_msg_counters;
-
- bool isStreamMsg(MsgType m) {
- return m == MSG_TYPE_ERROR_STREAM || m == MSG_TYPE_DEBUG_STREAM || m == MSG_TYPE_INFO_STREAM ||
- m == MSG_TYPE_WARNING_STREAM;
+ /** Enum representing the different kind of messages.
+ */
+ enum CENTROIDAL_DYNAMICS_DLLAPI MsgType
+ {
+ MSG_TYPE_DEBUG =0,
+ MSG_TYPE_INFO =1,
+ MSG_TYPE_WARNING =2,
+ MSG_TYPE_ERROR =3,
+ MSG_TYPE_DEBUG_STREAM =4,
+ MSG_TYPE_INFO_STREAM =5,
+ MSG_TYPE_WARNING_STREAM =6,
+ MSG_TYPE_ERROR_STREAM =7
+ };
+
+ template<typename T>
+ std::string toString(const T& v)
+ {
+ std::stringstream ss;
+ ss<<v;
+ return ss.str();
}
- bool isDebugMsg(MsgType m) { return m == MSG_TYPE_DEBUG_STREAM || m == MSG_TYPE_DEBUG; }
+ template<typename T>
+ std::string toString(const std::vector<T>& v, const std::string separator=", ")
+ {
+ std::stringstream ss;
+ for(int i=0; i<v.size()-1; i++)
+ ss<<v[i]<<separator;
+ ss<<v[v.size()-1];
+ return ss.str();
+ }
+
+ 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; }
+
+ 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 // #ifndef __sot_torque_control_trajectory_generators_H__
+#endif // #ifndef __sot_torque_control_trajectory_generators_H__
diff --git a/include/hpp/centroidal-dynamics/solver_LP_abstract.hh b/include/hpp/centroidal-dynamics/solver_LP_abstract.hh
index a2c44ca..501ffdf 100644
--- a/include/hpp/centroidal-dynamics/solver_LP_abstract.hh
+++ b/include/hpp/centroidal-dynamics/solver_LP_abstract.hh
@@ -10,42 +10,49 @@
#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;
@@ -56,15 +63,16 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
* @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.
@@ -72,7 +80,7 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
* @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.
@@ -88,7 +96,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
* @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);
/**
@@ -106,8 +115,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
* @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;
@@ -118,22 +128,25 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
/** 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 // CENTROIDAL_DYNAMICS_LIB_SOLVER_LP_ABSTRACT_HH
+#endif //CENTROIDAL_DYNAMICS_LIB_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 742ccac..c7dc506 100644
--- a/include/hpp/centroidal-dynamics/solver_LP_clp.hh
+++ b/include/hpp/centroidal-dynamics/solver_LP_clp.hh
@@ -13,13 +13,16 @@
#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
@@ -27,8 +30,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_clp : public Solver_LP_abstract {
* 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();
@@ -46,8 +50,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_clp : public Solver_LP_abstract {
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 2342967..e8c42c9 100644
--- a/include/hpp/centroidal-dynamics/solver_LP_qpoases.hh
+++ b/include/hpp/centroidal-dynamics/solver_LP_qpoases.hh
@@ -11,18 +11,21 @@
#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
+ 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:
- public:
Solver_LP_qpoases();
/** Solve the linear program
@@ -30,18 +33,20 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases : public Solver_LP_abstract {
* 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 double getObjectiveValue(){ return m_solver.getObjVal(); }
+
+ virtual void getDualSolution(Ref_vectorX res){ m_solver.getDualSolution(res.data()); }
- virtual void getDualSolution(Ref_vectorX res) { m_solver.getDualSolution(res.data()); }
};
-} // end namespace centroidal_dynamics
+} // end namespace centroidal_dynamics
-#endif // CENTROIDAL_DYNAMICS_LIB_SOLVER_QPOASES_HH
+#endif //CENTROIDAL_DYNAMICS_LIB_SOLVER_QPOASES_HH
diff --git a/include/hpp/centroidal-dynamics/stop-watch.hh b/include/hpp/centroidal-dynamics/stop-watch.hh
index e38c7cf..ceed018 100644
--- a/include/hpp/centroidal-dynamics/stop-watch.hh
+++ b/include/hpp/centroidal-dynamics/stop-watch.hh
@@ -24,28 +24,32 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
+
#ifndef WBR_STOPWATCH_H
#define WBR_STOPWATCH_H
#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
};
/**
@@ -140,9 +144,10 @@ enum StopwatchMode {
*/
class Stopwatch {
- public:
+public:
+
/** Constructor */
- Stopwatch(StopwatchMode _mode = NONE);
+ Stopwatch(StopwatchMode _mode=NONE);
/** Destructor */
~Stopwatch();
@@ -169,10 +174,11 @@ class Stopwatch {
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);
@@ -194,7 +200,7 @@ class Stopwatch {
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(). */
@@ -203,23 +209,32 @@ class Stopwatch {
/** 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;
@@ -228,7 +243,7 @@ class Stopwatch {
bool paused;
/** How many cycles have been this stopwatch executed? */
- int stops;
+ int stops;
};
/** Flag to hold the clock's status */
@@ -239,13 +254,14 @@ class Stopwatch {
/** 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 b4d130b..a560a97 100644
--- a/include/hpp/centroidal-dynamics/util.hh
+++ b/include/hpp/centroidal-dynamics/util.hh
@@ -18,144 +18,154 @@
#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;
-}
-
-/**
- * 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;
-}
-
-/**
+ 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;
+ }
+
+ /**
* 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;
+ 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--;
+ }
}
- 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
@@ -164,20 +174,25 @@ void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& runnin
* @return nchoosek by k long matrix where each row is a unique k-size
* the first one, with all zeros.
*/
-template <typename DerivedV, typename DerivedU>
-void nchoosek(const Eigen::MatrixBase<DerivedV>& V, const int k, Eigen::PlainObjectBase<DerivedU>& U) {
- using namespace Eigen;
- if (V.size() == 0) {
- U.resize(0, k);
- return;
- }
- assert((V.cols() == 1 || V.rows() == 1) && "V must be a vector");
- U.resize(nchoosek((int)(V.size()), k), k);
- int running_i = 0;
- int running_j = 0;
- Matrix<typename DerivedU::Scalar, 1, Dynamic> running(1, k);
- doCombs(running, running_i, running_j, U, V, 0, k);
-}
-} // namespace centroidal_dynamics
-
-#endif //_CENTROIDAL_DYNAMICS_LIB_UTIL_HH
+ template < typename DerivedV, typename DerivedU>
+ void nchoosek(
+ const Eigen::MatrixBase<DerivedV> & V,
+ const int k,
+ Eigen::PlainObjectBase<DerivedU> & U)
+ {
+ using namespace Eigen;
+ if(V.size() == 0)
+ {
+ U.resize(0,k);
+ return;
+ }
+ assert((V.cols() == 1 || V.rows() == 1) && "V must be a vector");
+ U.resize(nchoosek((int)(V.size()),k),k);
+ int running_i = 0;
+ int running_j = 0;
+ Matrix<typename DerivedU::Scalar,1,Dynamic> running(1,k);
+ doCombs(running, running_i, running_j, U, V,0,k);
+ }
+} //namespace centroidal_dynamics
+
+#endif //_CENTROIDAL_DYNAMICS_LIB_UTIL_HH
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index dcb4983..c6885cb 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -1,17 +1,24 @@
+cmake_minimum_required( VERSION 2.8 )
+
STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
+ADD_REQUIRED_DEPENDENCY("eigenpy")
+
# Define the wrapper library that wraps our library
-ADD_LIBRARY(${PY_NAME} SHARED centroidal_dynamics_python.cpp)
-TARGET_LINK_LIBRARIES(${PY_NAME} ${Boost_LIBRARIES} ${PROJECT_NAME})
+add_library( ${PY_NAME} SHARED centroidal_dynamics_python.cpp )
+target_link_libraries( ${PY_NAME} ${Boost_LIBRARIES} ${PROJECT_NAME} )
# don't prepend wrapper library name with lib
-SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES PREFIX "")
+set_target_properties( ${PY_NAME} PROPERTIES PREFIX "" )
IF(APPLE)
- # We need to change the extension for python bindings
- SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES SUFFIX ".so")
+ # We need to change the extension for python bindings
+ SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES SUFFIX ".so")
ENDIF(APPLE)
PKG_CONFIG_USE_DEPENDENCY(${PY_NAME} eigenpy)
-INSTALL(TARGETS ${PY_NAME} DESTINATION ${PYTHON_SITELIB})
+INSTALL(
+ TARGETS ${PY_NAME} DESTINATION ${PYTHON_SITELIB}
+)
-ADD_PYTHON_UNIT_TEST("python-centroidal-dynamics" "python/test/binding_tests.py" "python")
+# TODO
+#ADD_PYTHON_UNIT_TEST("python-centroidal-dynamics" "python/test/binding_tests.py" "python")
diff --git a/python/centroidal_dynamics_python.cpp b/python/centroidal_dynamics_python.cpp
index 61ce46c..3fff17a 100644
--- a/python/centroidal_dynamics_python.cpp
+++ b/python/centroidal_dynamics_python.cpp
@@ -1,116 +1,125 @@
#include "hpp/centroidal-dynamics/centroidal_dynamics.hh"
#include "hpp/centroidal-dynamics/util.hh"
-#include <eigenpy/eigenpy.hpp>
#include <eigenpy/memory.hpp>
+#include <eigenpy/eigenpy.hpp>
#include <boost/python.hpp>
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>();
- eigenpy::enableEigenPySpecific<MatrixXXColMajor>();
- eigenpy::enableEigenPySpecific<Vector3>();
- eigenpy::enableEigenPySpecific<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/python/test/binding_tests.py b/python/test/binding_tests.py
index 4257628..4f13e81 100644
--- a/python/test/binding_tests.py
+++ b/python/test/binding_tests.py
@@ -1,76 +1,67 @@
-import unittest
+from hpp_centroidal_dynamics import *
-from hpp_centroidal_dynamics import LP_STATUS_OPTIMAL, Equilibrium, EquilibriumAlgorithm, SolverLP
-from numpy import array, asmatrix, cross
+#testing constructors
+eq = Equilibrium("test", 54., 4)
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES )
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES )
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False)
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
+eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True )
+#whether useWarmStart is enable (True by default)
+previous = eq.useWarmStart()
+#enable warm start in solver (only for QPOases)
+eq.setUseWarmStart(False)
+assert(previous != eq.useWarmStart())
-def compute_w(c, ddc, dL=array([0., 0., 0.]), m=54., g_vec=array([0., 0., -9.81])):
- w1 = m * (ddc - g_vec)
- return array(w1.tolist() + (cross(c, w1) + dL).tolist())
+#access solver name
+assert(eq.getName() == "test")
-def is_stable(H,
- c=array([0., 0., 1.]),
- ddc=array([0., 0., 0.]),
- dL=array([0., 0., 0.]),
- m=54.,
- g_vec=array([0., 0., -9.81]),
- robustness=0.):
- w = compute_w(c, ddc, dL, m, g_vec)
- return (H.dot(-w) <= -robustness).all()
+# creating contact points
+from numpy import array, asmatrix, matrix
+z = array([0.,0.,1.])
+P = asmatrix(array([array([x,y,0]) for x in [-0.05,0.05] for y in [-0.1,0.1]]))
+N = asmatrix(array([z for _ in range(4)]))
-class CentroidalDynamicsTestCase(unittest.TestCase):
- def test_constructors(self):
- Equilibrium("test", 54., 4)
- Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
- Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES)
- Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False)
- Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
- Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
+#setting contact positions and normals, as well as friction coefficients
+eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
- def test_warmstart(self):
- eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
+c= asmatrix(array([0.,0.,1.])).T
- # whether useWarmStart is enable (True by default)
- previous = eq.useWarmStart()
- # enable warm start in solver (only for QPOases)
- eq.setUseWarmStart(False)
- self.assertNotEqual(previous, eq.useWarmStart())
+#computing robustness of a given configuration, first with no argument (0 acceleration, static equilibrium)
+status, robustness = eq.computeEquilibriumRobustness(c)
+assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
+assert (robustness > 0), "first test should be in equilibrirum"
- # access solver name
- self.assertEqual(eq.getName(), "test")
+#computing robustness of a given configuration with non zero acceleration
+ddc= asmatrix(array([1000.,0.,0.])).T
+status, robustness = eq.computeEquilibriumRobustness(c,ddc)
+assert (status == LP_STATUS_OPTIMAL), "LP should not fail"
+assert (robustness < 0), "first test should NOT be in equilibrirum"
- def test_main(self):
- eq = Equilibrium("test", 54., 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
+#now, use polytope projection algorithm
+eq.setNewContacts(asmatrix(P),asmatrix(N),0.3,EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
+H,h = eq.getPolytopeInequalities()
- z = array([0., 0., 1.])
- P = asmatrix(array([array([x, y, 0]) for x in [-0.05, 0.05] for y in [-0.1, 0.1]]))
- N = asmatrix(array([z for _ in range(4)]))
+#~ c= asmatrix(array([0.,0.,1.])).T
+status, stable = eq.checkRobustEquilibrium(c)
+assert (status == LP_STATUS_OPTIMAL), "checkRobustEquilibrium should not fail"
+assert (stable), "lat test should be in equilibrirum"
- # setting contact positions and normals, as well as friction coefficients
- eq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
- c = asmatrix(array([0., 0., 1.])).T
+from numpy import array, vstack, zeros, sqrt, cross, matrix, asmatrix
+from numpy.linalg import norm
+import numpy as np
- # computing robustness of a given configuration, first with no argument (0 acceleration, static equilibrium)
- status, robustness = eq.computeEquilibriumRobustness(c)
- self.assertEqual(status, LP_STATUS_OPTIMAL, "LP should not fail")
- self.assertGreater(robustness, 0, "first test should be in equilibrirum")
+def compute_w(c, ddc, dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81])):
+ w1 = m * (ddc - g_vec)
+ return array(w1.tolist() + (cross(c, w1) + dL).tolist())
- # computing robustness of a given configuration with non zero acceleration
- ddc = asmatrix(array([1000., 0., 0.])).T
- status, robustness = eq.computeEquilibriumRobustness(c, ddc)
- self.assertEqual(status, LP_STATUS_OPTIMAL, "LP should not fail")
- self.assertLess(robustness, 0, "first test should NOT be in equilibrirum")
- # now, use polytope projection algorithm
- eq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP)
- H, h = eq.getPolytopeInequalities()
+def is_stable(H,c=array([0.,0.,1.]), ddc=array([0.,0.,0.]), dL=array([0.,0.,0.]), m = 54., g_vec=array([0.,0.,-9.81]), robustness = 0.):
+ w = compute_w(c, ddc, dL, m, g_vec)
+ return (H.dot(-w)<=-robustness).all()
- # c= asmatrix(array([0.,0.,1.])).T
- status, stable = eq.checkRobustEquilibrium(c)
- self.assertEqual(status, LP_STATUS_OPTIMAL, "checkRobustEquilibrium should not fail")
- self.assertTrue(stable, "lat test should be in equilibrirum")
-
- self.assertTrue(is_stable(H))
+assert(is_stable(H))
diff --git a/setup.cfg b/setup.cfg
deleted file mode 100644
index 38fcd93..0000000
--- a/setup.cfg
+++ /dev/null
@@ -1,10 +0,0 @@
-[flake8]
-max-line-length = 119
-exclude = cmake
-
-[yapf]
-column_limit = 119
-
-[isort]
-line_length = 119
-skip = cmake
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 2c7df02..c77b697 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,3 +1,10 @@
+cmake_minimum_required(VERSION 2.6)
+
+if(CLP_FOUND)
+ include_directories("${CLP_INCLUDE_DIR}")
+endif()
+
+
SET(LIBRARY_NAME ${PROJECT_NAME})
SET(${LIBRARY_NAME}_SOURCES
@@ -16,8 +23,9 @@ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CDD_LIBRARIES} ${qpOASES_LIBRARY})
TARGET_INCLUDE_DIRECTORIES(${LIBRARY_NAME} PUBLIC ${CDD_INCLUDE_DIRS})
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} eigen3)
-IF(CLP_FOUND)
- TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CLP_LIBRARIES})
-ENDIF(CLP_FOUND)
+if(CLP_FOUND)
+ TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${CLP_LIBRARIES}
+ /usr/lib/libCoinUtils.so)
+endif()
INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp
index cbe34d4..f495610 100644
--- a/src/centroidal_dynamics.cpp
+++ b/src/centroidal_dynamics.cpp
@@ -3,53 +3,58 @@
* Author: Andrea Del Prete
*/
-#include <ctime>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include <hpp/centroidal-dynamics/logger.hh>
#include <hpp/centroidal-dynamics/stop-watch.hh>
#include <iostream>
#include <vector>
+#include <ctime>
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_G_centr(other.m_G_centr),
- 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_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;
@@ -58,7 +63,8 @@ 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;
}
@@ -67,101 +73,114 @@ 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;
+ 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;
}
- 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 (unsigned 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;
- }
- // project generators in 6d centroidal space
- m_G_centr.block(0, cg * i, 6, cg) = A * G;
- }
+ // 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 (unsigned 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;
+ // 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;
}
@@ -169,21 +188,25 @@ 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((unsigned int)time(NULL));
- if (!m_is_cdd_stable) {
+ srand(time(NULL));
+ if(!m_is_cdd_stable)
+ {
return false;
}
m_HD = m_H * m_D;
@@ -195,19 +218,23 @@ 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
@@ -220,31 +247,33 @@ 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
@@ -257,29 +286,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::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
@@ -292,29 +323,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;
@@ -324,33 +355,38 @@ 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) {
- return 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;
}
@@ -359,30 +395,38 @@ 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
@@ -395,44 +439,46 @@ 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
@@ -443,37 +489,39 @@ 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;
}
@@ -482,14 +530,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;
@@ -499,43 +547,48 @@ LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, dou
return LP_STATUS_ERROR;
}
-LP_status Equilibrium::findExtremumInDirection(__attribute__((unused)) Cref_vector3 direction,
- __attribute__((unused)) Ref_vector3 com,
- __attribute__((unused)) 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);
@@ -544,90 +597,111 @@ 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_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_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 dba4295..f96b0a7 100644
--- a/src/logger.cpp
+++ b/src/logger.cpp
@@ -10,92 +10,108 @@
#endif
#include <stdio.h>
+#include <iostream>
+#include <iomanip> // std::setprecision
#include <boost/algorithm/string.hpp>
#include <hpp/centroidal-dynamics/logger.hh>
-#include <iomanip> // std::setprecision
-#include <iostream>
-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 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 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;
+ }
- vector<string> fields;
- boost::split(fields, file, boost::is_any_of("/"));
- const char* file_name = fields[fields.size() - 1].c_str();
+ vector<string> fields;
+ boost::split(fields, file, boost::is_any_of("/"));
+ const char* file_name = fields[fields.size()-1].c_str();
- 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());
+ 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());
- fflush(stdout); // Prints to screen or whatever your standard out is
-}
+ fflush(stdout); // Prints to screen or whatever your standard out is
+ }
-bool Logger::setTimeSample(double t) {
- if (t <= 0.0) return false;
- m_timeSample = t;
- 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;
-}
+ 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 e6a461e..d7026d9 100644
--- a/src/solver_LP_abstract.cpp
+++ b/src/solver_LP_abstract.cpp
@@ -3,94 +3,109 @@
* Author: Andrea Del Prete
*/
-#include <hpp/centroidal-dynamics/logger.hh>
#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
#include <hpp/centroidal-dynamics/solver_LP_qpoases.hh>
+#include <hpp/centroidal-dynamics/logger.hh>
#include <iostream>
#ifdef CLP_FOUND
#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((const char *)(&n), sizeof(typename MatrixXX::Index));
- out.write((const char *)(&m), sizeof(typename MatrixXX::Index));
- out.write((const char *)c.data(), n * sizeof(typename MatrixXX::Scalar));
- out.write((const char *)lb.data(), n * sizeof(typename MatrixXX::Scalar));
- out.write((const char *)ub.data(), n * sizeof(typename MatrixXX::Scalar));
- out.write((const char *)A.data(), m * n * sizeof(typename MatrixXX::Scalar));
- out.write((const char *)Alb.data(), m * sizeof(typename MatrixXX::Scalar));
- out.write((const 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 12fbcfb..e221764 100644
--- a/src/solver_LP_clp.cpp
+++ b/src/solver_LP_clp.cpp
@@ -8,92 +8,114 @@
#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 58cf95e..b443dcc 100644
--- a/src/solver_LP_qpoases.cpp
+++ b/src/solver_LP_qpoases.cpp
@@ -3,67 +3,83 @@
* Author: Andrea Del Prete
*/
-#include <hpp/centroidal-dynamics/logger.hh>
#include <hpp/centroidal-dynamics/solver_LP_qpoases.hh>
+#include <hpp/centroidal-dynamics/logger.hh>
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;
-}
+ 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);
+ 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);
- }
+ 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);
+ }
- 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;
- }
+ 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;
+ }
- if (m_status == SUCCESSFUL_RETURN) {
- m_solver.getPrimalSolution(sol.data());
- }
+ if(m_status==SUCCESSFUL_RETURN)
+ {
+ m_solver.getPrimalSolution(sol.data());
+ }
- return getStatus();
-}
+ 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;
-}
+ 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
+} // end namespace centroidal_dynamics
diff --git a/src/stop-watch.cpp b/src/stop-watch.cpp
index 6551d90..47cd7a8 100644
--- a/src/stop-watch.cpp
+++ b/src/stop-watch.cpp
@@ -27,40 +27,55 @@ 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::ostringstream;
using std::string;
+using std::ostringstream;
-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; }
+Stopwatch::~Stopwatch()
+{
+ delete records_of;
+}
+
+void Stopwatch::set_mode(StopwatchMode new_mode)
+{
+ mode = new_mode;
+}
-void Stopwatch::set_mode(StopwatchMode new_mode) { mode = new_mode; }
+bool Stopwatch::performance_exists(string perf_name)
+{
+ return (records_of->find(perf_name) != records_of->end());
+}
-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 ) {
-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
@@ -75,8 +90,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
@@ -85,8 +100,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
@@ -97,7 +112,8 @@ 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
@@ -109,56 +125,63 @@ 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;
@@ -168,21 +191,24 @@ 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;
@@ -195,35 +221,44 @@ 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 (unsigned long 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;
@@ -231,56 +266,74 @@ 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 08ebb0e..942e1c3 100644
--- a/src/util.cpp
+++ b/src/util.cpp
@@ -6,37 +6,42 @@
#ifndef _CENTROIDAL_DYNAMICS_LIB_CONFIG_HH
#define _CENTROIDAL_DYNAMICS_LIB_CONFIG_HH
-#include <ctime>
#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);
@@ -46,36 +51,45 @@ 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;
@@ -85,64 +99,67 @@ 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);
}
/*
@@ -176,18 +193,21 @@ 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
+} //namespace centroidal_dynamics
-#endif //_CENTROIDAL_DYNAMICS_LIB_CONFIG_HH
+#endif //_CENTROIDAL_DYNAMICS_LIB_CONFIG_HH
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 482b785..3271bad 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -1,3 +1,9 @@
+cmake_minimum_required(VERSION 2.6)
+
+
+# Make Boost.Test generates the main function in test cases.
+#ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
+
ADD_UNIT_TEST(static-equilibrium test_static_equilibrium)
PKG_CONFIG_USE_DEPENDENCY(static-equilibrium eigen3)
TARGET_LINK_LIBRARIES(static-equilibrium ${PROJECT_NAME})
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
index 0807c66..38d9061 100644
--- a/test/test_LP_solvers.cpp
+++ b/test/test_LP_solvers.cpp
@@ -4,19 +4,19 @@
*/
#ifdef CLP_FOUND
-#include <hpp/centroidal-dynamics/solver_LP_clp.hh>
#include "ClpSimplex.hpp"
+#include "CoinTime.hpp"
#include "CoinBuild.hpp"
#include "CoinModel.hpp"
-#include "CoinTime.hpp"
+#include <hpp/centroidal-dynamics/solver_LP_clp.hh>
#endif
-#include <hpp/centroidal-dynamics/logger.hh>
-#include <hpp/centroidal-dynamics/solver_LP_qpoases.hh>
#include <qpOASES.hpp>
+#include <hpp/centroidal-dynamics/solver_LP_qpoases.hh>
+#include <hpp/centroidal-dynamics/logger.hh>
-#include <iomanip>
#include <iostream>
+#include <iomanip>
using namespace std;
using namespace centroidal_dynamics;
@@ -26,11 +26,13 @@ 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};
@@ -44,7 +46,8 @@ 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);
@@ -59,11 +62,13 @@ 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();
@@ -80,7 +85,8 @@ 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();
@@ -91,7 +97,8 @@ 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);
@@ -105,7 +112,8 @@ 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);
@@ -118,7 +126,8 @@ 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);
@@ -130,7 +139,8 @@ 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);
@@ -143,7 +153,8 @@ 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);
@@ -153,7 +164,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;
@@ -162,23 +173,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;
@@ -199,9 +210,10 @@ 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
@@ -213,19 +225,20 @@ 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;
@@ -260,8 +273,7 @@ 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
@@ -272,18 +284,21 @@ 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};
@@ -304,7 +319,8 @@ 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);
@@ -319,140 +335,159 @@ 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;
}
}
}
@@ -460,25 +495,29 @@ 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 67db8ba..98374cb 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -3,11 +3,11 @@
* Author: Andrea Del Prete
*/
+#include <vector>
+#include <iostream>
#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
#include <hpp/centroidal-dynamics/logger.hh>
#include <hpp/centroidal-dynamics/stop-watch.hh>
-#include <iostream>
-#include <vector>
using namespace centroidal_dynamics;
using namespace Eigen;
@@ -33,54 +33,63 @@ 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;
}
@@ -92,20 +101,22 @@ 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;
}
@@ -114,25 +125,24 @@ 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;
}
@@ -149,38 +159,42 @@ 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,
- unsigned 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;
}
@@ -189,24 +203,25 @@ 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;
}
@@ -215,103 +230,126 @@ 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(unsigned int N_CONTACTS, unsigned 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 (unsigned long k = 0; k < 4 * N_CONTACTS; k++) {
- if ((unsigned)contactPointCoord(k, 0) == i && (unsigned)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
@@ -321,19 +359,24 @@ 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;
}
@@ -343,34 +386,36 @@ 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;
@@ -382,13 +427,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;
@@ -397,104 +442,122 @@ 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