diff --git a/.git-blame-ignore-revs b/.git-blame-ignore-revs
new file mode 100644
index 0000000000000000000000000000000000000000..72a148037ae43f55c52411a8d69363b17a1d0e23
--- /dev/null
+++ b/.git-blame-ignore-revs
@@ -0,0 +1,17 @@
+# format (Guilhem Saurel, 2022-04-05)
+dff4d63db251496c701d89241783197da0605fc6
+
+# clang-format (Guilhem Saurel, 2021-10-06)
+32aab997ec72aefe6a91a01a7bb6575abc654fc7
+
+# [CI] format enforcement is now build in the buildfarm (Guilhem Saurel, 2019-03-21)
+0712ca6f229dfd9d973387c6f545317ef5b03b5e
+
+# [CI] enforce c++ format (Guilhem Saurel, 2019-03-21)
+312e1ff9e4c6011ee46d7c51f1ad3ddc2337c36f
+
+# [Style] clang-format -i **.{cpp,hh} (Guilhem Saurel, 2019-03-21)
+d357fb18b888aeca461a6259d0edcc56ed007c54
+
+# [CI][Python][Style] enforce format (Guilhem Saurel, 2019-03-21)
+e66325370e2e2731f320541d378facd822398719
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..7b987f2bfa041659dcd813b2d39b7cb3d86bd5f0
--- /dev/null
+++ b/.pre-commit-config.yaml
@@ -0,0 +1,32 @@
+repos:
+- repo: https://github.com/pre-commit/mirrors-clang-format
+ rev: v13.0.1
+ hooks:
+ - id: clang-format
+ args: [-i, --style=Google]
+- repo: https://github.com/pre-commit/pre-commit-hooks
+ rev: v4.1.0
+ hooks:
+ - id: check-added-large-files
+ - id: check-ast
+ - id: check-executables-have-shebangs
+ - id: check-json
+ - id: check-merge-conflict
+ - id: check-symlinks
+ - id: check-toml
+ - id: check-yaml
+ - id: debug-statements
+ - id: destroyed-symlinks
+ - id: detect-private-key
+ - id: end-of-file-fixer
+ - id: fix-byte-order-marker
+ - id: mixed-line-ending
+ - id: trailing-whitespace
+- repo: https://github.com/psf/black
+ rev: 22.3.0
+ hooks:
+ - id: black
+- repo: https://github.com/PyCQA/flake8
+ rev: 4.0.1
+ hooks:
+ - id: flake8
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3f0ca9d9b41fd6edff0f4cac9233f0f04824f3a1..9dc32180823e856bc7ffd4e0573e965d57fa861c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -80,7 +80,7 @@ TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM PUBLIC
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CDD_LIBRARIES} ${qpOASES_LIBRARY})
IF(CLP_FOUND)
- TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM "${CLP_INCLUDE_DIR}")
+ TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} SYSTEM PUBLIC "${CLP_INCLUDE_DIR}")
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${CLP_LIBRARIES}
/usr/lib/libCoinUtils.so)
ENDIF(CLP_FOUND)
diff --git a/LICENSE b/LICENSE
index 8cdb8451d9b90c1d4000c6f22eb86471a4568be6..23cb790338e191e29205d6f4123882c0583ef8eb 100644
--- a/LICENSE
+++ b/LICENSE
@@ -337,4 +337,3 @@ proprietary programs. If your program is a subroutine library, you may
consider it more useful to permit linking proprietary applications with the
library. If this is what you want to do, use the GNU Lesser General
Public License instead of this License.
-
diff --git a/README.md b/README.md
index 4532d59eb66e7e8e569b5f682271d8f2072aaaf4..589542699565de5184729a4a7bad886646b5769f 100644
--- a/README.md
+++ b/README.md
@@ -1,7 +1,9 @@
# CentroidalDynamicsLib
-[](https://gepgitlab.laas.fr/humanoid-path-planner/hpp-centroidal-dynamics/commits/master)
-[](http://projects.laas.fr/gepetto/doc/humanoid-path-planner/hpp-centroidal-dynamics/master/coverage/)
+[](https://gitlab.laas.fr/humanoid-path-planner/hpp-centroidal-dynamics/commits/master)
+[](https://gepettoweb.laas.fr/doc/humanoid-path-planner/hpp-centroidal-dynamics/master/coverage/)
+[](https://github.com/psf/black)
+[](https://results.pre-commit.ci/latest/github/humanoid-path-planner/hpp-centroidal-dynamics)
Utility classes to check the (robust) equilibrium of a system in contact with the environment. Comes with python bindings.
The main class that collects all the equilibrium-related algorithms is ```Equilibrium```.
diff --git a/cmake b/cmake
index ee7a773c5c23f83dd21eb0ccfa96277e068b0456..b11b505ce60ea593ce113a2336089907525b4879 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit ee7a773c5c23f83dd21eb0ccfa96277e068b0456
+Subproject commit b11b505ce60ea593ce113a2336089907525b4879
diff --git a/include/hpp/centroidal-dynamics/Stdafx.hh b/include/hpp/centroidal-dynamics/Stdafx.hh
index f9716e9a69379befe68769f0dab345bc066890de..b19711ed4c014b99e97fc90c8df9c333e069a6d4 100644
--- a/include/hpp/centroidal-dynamics/Stdafx.hh
+++ b/include/hpp/centroidal-dynamics/Stdafx.hh
@@ -29,9 +29,9 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#pragma once
+#include <ctime>
#include <iostream>
#include <map>
-#include <ctime>
#include <sstream>
#endif
diff --git a/include/hpp/centroidal-dynamics/centroidal_dynamics.hh b/include/hpp/centroidal-dynamics/centroidal_dynamics.hh
index 46729c11bed2db0ef7a91ccfc7fdf41838e0067a..63bd0b8b8086016c647705f349671f9943480eb3 100644
--- a/include/hpp/centroidal-dynamics/centroidal_dynamics.hh
+++ b/include/hpp/centroidal-dynamics/centroidal_dynamics.hh
@@ -8,8 +8,8 @@
#include <Eigen/Dense>
#include <hpp/centroidal-dynamics/local_config.hh>
-#include <hpp/centroidal-dynamics/util.hh>
#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
+#include <hpp/centroidal-dynamics/util.hh>
namespace centroidal_dynamics {
@@ -18,28 +18,34 @@ enum CENTROIDAL_DYNAMICS_DLLAPI EquilibriumAlgorithm {
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
+ 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
- /** Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact) */
+ /** 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
+ static bool m_is_cdd_initialized; /// true if cdd lib has been initialized,
+ /// false otherwise
std::string m_name; /// name of this object
EquilibriumAlgorithm m_algorithm; /// current algorithm used
SolverLP m_solver_type; /// type of LP solver
Solver_LP_abstract* m_solver; /// LP solver
- unsigned int m_generatorsPerContact; /// number of generators to approximate the friction cone per contact point
+ 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 */
+ /** Inequality matrix and vector defining the gravito-inertial wrench cone H w
+ * <= h */
MatrixXX m_H;
VectorX m_h;
/** False if a numerical instability appeared in the computation H and h*/
@@ -48,10 +54,12 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* 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*/
+ /** 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 */
+ /** Inequality matrix and vector defining the CoM support polygon HD com + Hd
+ * <= h */
MatrixX3 m_HD;
VectorX m_Hd;
@@ -63,16 +71,18 @@ 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,
+ 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
- * the minimum norm of force error necessary to have a contact force on
- * the associated friction cone boundaries.
+ * @brief Given the smallest coefficient of the contact force generators it
+ * computes the minimum norm of force error necessary to have a contact force
+ * on the associated friction cone boundaries.
* @param b0 Minimum coefficient of the contact force generators.
- * @return Minimum norm of the force error necessary to result in a contact force being
- * on the friction cone boundaries.
+ * @return Minimum norm of the force error necessary to result in a contact
+ * force being on the friction cone boundaries.
*/
double convert_b0_to_emax(double b0);
@@ -84,16 +94,22 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* @brief Equilibrium constructor.
* @param name Name of the object.
* @param mass Mass of the system for which to test equilibrium.
- * @param generatorsPerContact Number of generators used to approximate the friction cone per contact point.
+ * @param generatorsPerContact Number of generators used to approximate the
+ * friction cone per contact point.
* @param solver_type Type of LP solver to use.
* @param useWarmStart Whether the LP solver can warm start the resolution.
- * @param max_num_cdd_trials indicate the max number of attempts to compute the cone by introducing
- * @param canonicalize_cdd_matrix whether to remove redundant inequalities when computing double description matrices
- * a small pertubation of the system
+ * @param max_num_cdd_trials indicate the max number of attempts to compute
+ * the cone by introducing
+ * @param canonicalize_cdd_matrix whether to remove redundant inequalities
+ * when computing double description matrices 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);
+ 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);
Equilibrium(const Equilibrium& other);
@@ -101,13 +117,15 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
/**
* @brief Returns the useWarmStart flag.
- * @return True if the LP solver is allowed to use warm start, false otherwise.
+ * @return True if the LP solver is allowed to use warm start, false
+ * otherwise.
*/
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.
+ * @param uws True if the LP solver is allowed to use warm start, false
+ * otherwise.
*/
void setUseWarmStart(bool uws) { m_solver->setUseWarmStart(uws); }
@@ -123,80 +141,82 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
/**
* @brief Specify a new set of contacts.
- * All 3d vectors are expressed in a reference frame having the z axis aligned with gravity.
- * In other words the gravity vecotr is (0, 0, -9.81). This method considers row-major
- * matrices as input.
+ * All 3d vectors are expressed in a reference frame having the z axis aligned
+ * with gravity. In other words the gravity vecotr is (0, 0, -9.81). This
+ * method considers row-major matrices as input.
* @param contactPoints List of N 3d contact points as an Nx3 matrix.
- * @param contactNormals List of N 3d contact normal directions as an Nx3 matrix.
+ * @param contactNormals List of N 3d contact normal directions as an Nx3
+ * matrix.
* @param frictionCoefficient The contact friction coefficient.
* @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,
+ bool setNewContacts(const MatrixX3& contactPoints,
+ const MatrixX3& contactNormals,
+ const double frictionCoefficient,
const EquilibriumAlgorithm alg);
/**
* @brief Specify a new set of contacts.
- * All 3d vectors are expressed in a reference frame having the z axis aligned with gravity.
- * In other words the gravity vecotr is (0, 0, -9.81). This method considers column major
- * matrices as input, and converts them into rowmajor matrices for internal use with the solvers.
+ * All 3d vectors are expressed in a reference frame having the z axis aligned
+ * with gravity. In other words the gravity vecotr is (0, 0, -9.81). This
+ * method considers column major matrices as input, and converts them into
+ * rowmajor matrices for internal use with the solvers.
* @param contactPoints List of N 3d contact points as an Nx3 matrix.
- * @param contactNormals List of N 3d contact normal directions as an Nx3 matrix.
+ * @param contactNormals List of N 3d contact normal directions as an Nx3
+ * matrix.
* @param frictionCoefficient The contact friction coefficient.
* @param alg Algorithm to use for testing equilibrium.
* @return True if the operation succeeded, false otherwise.
*/
- bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
- const double frictionCoefficient, const EquilibriumAlgorithm alg);
+ bool setNewContacts(const MatrixX3ColMajor& contactPoints,
+ const MatrixX3ColMajor& contactNormals,
+ const double frictionCoefficient,
+ const EquilibriumAlgorithm alg);
void setG(Cref_matrix6X G) { m_G_centr = G; }
/**
- * @brief Compute a measure of the robustness of the equilibrium of the specified com position.
- * This amounts to solving the following LP:
- * find b, b0
- * maximize b0
- * subject to G b = D c + d
- * b >= b0
- * where:
- * b are the coefficient of the contact force generators (f = G b)
- * b0 is a parameter proportional to the robustness measure
- * c is the specified CoM position
- * G is the 6xm matrix whose columns are the gravito-inertial wrench generators
- * D is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
- * d is the 6d vector containing the gravity part of the gravito-inertial wrench
+ * @brief Compute a measure of the robustness of the equilibrium of the
+ * specified com position. This amounts to solving the following LP: find b,
+ * b0 maximize b0 subject to G b = D c + d b >= b0 where: b are the
+ * coefficient of the contact force generators (f = G b) b0 is a
+ * parameter proportional to the robustness measure c is the specified
+ * CoM position G is the 6xm matrix whose columns are the
+ * gravito-inertial wrench generators D is the 6x3 matrix mapping the
+ * CoM position in gravito-inertial wrench d is the 6d vector
+ * containing the gravity part of the gravito-inertial wrench
* @param com The 3d center of mass position to test.
* @param robustness The computed measure of robustness.
* @return The status of the LP solver.
- * @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.
+ * @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 computeEquilibriumRobustness(Cref_vector3 com, double& robustness);
/**
- * @brief Compute a measure of the robustness of the equilibrium of the specified com position.
- * This amounts to solving the following LP:
- * find b, b0
- * maximize b0
- * subject to G b = D c + d
- * b >= b0
- * where:
- * b are the coefficient of the contact force generators (f = G b)
- * b0 is a parameter proportional to the robustness measure
- * c is the specified CoM position
- * G is the 6xm matrix whose columns are the gravito-inertial wrench generators
- * D is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
- * d is the 6d vector containing the gravity part of the gravito-inertial wrench
+ * @brief Compute a measure of the robustness of the equilibrium of the
+ * specified com position. This amounts to solving the following LP: find b,
+ * b0 maximize b0 subject to G b = D c + d b >= b0 where: b are the
+ * coefficient of the contact force generators (f = G b) b0 is a
+ * parameter proportional to the robustness measure c is the specified
+ * CoM position G is the 6xm matrix whose columns are the
+ * gravito-inertial wrench generators D is the 6x3 matrix mapping the
+ * CoM position in gravito-inertial wrench d is the 6d vector
+ * containing the gravity part of the gravito-inertial wrench
* @param com The 3d center of mass position to test.
* @param acc The 3d acceleration of the CoM.
* @param robustness The computed measure of robustness.
* @return The status of the LP solver.
- * @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.
+ * @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 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.
@@ -207,17 +227,18 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* b >= b0
* where:
* b are the coefficient of the contact force generators (f = G b)
- * b0 is a parameter proportional to the specified robustness measure
- * c is the specified CoM position
- * G is the 6xm matrix whose columns are the gravito-inertial wrench generators
- * D is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
- * d is the 6d vector containing the gravity part of the gravito-inertial wrench
+ * b0 is a parameter proportional to the specified robustness
+ * measure c is the specified CoM position G is the 6xm matrix
+ * whose columns are the gravito-inertial wrench generators D is the
+ * 6x3 matrix mapping the CoM position in gravito-inertial wrench d is
+ * the 6d vector containing the gravity part of the gravito-inertial wrench
* @param com The 3d center of mass position to test.
* @param equilibrium True if com is in robust equilibrium, false otherwise.
* @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.
@@ -228,73 +249,78 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
* b >= b0
* where:
* b are the coefficient of the contact force generators (f = G b)
- * b0 is a parameter proportional to the specified robustness measure
- * c is the specified CoM position
- * G is the 6xm matrix whose columns are the gravito-inertial wrench generators
- * D is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
- * d is the 6d vector containing the gravity part of the gravito-inertial wrench
+ * b0 is a parameter proportional to the specified robustness
+ * measure c is the specified CoM position G is the 6xm matrix
+ * whose columns are the gravito-inertial wrench generators D is the
+ * 6x3 matrix mapping the CoM position in gravito-inertial wrench d is
+ * the 6d vector containing the gravity part of the gravito-inertial wrench
* @param com The 3d center of mass position to test.
* @param acc The 3d acceleration of the CoM.
* @param equilibrium True if com is in robust equilibrium, false otherwise.
* @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.
- * This amounts to solving the following LP:
- * find c, b
+ * @brief Compute the extremum CoM position over the line a*x + a0 that is in
+ * robust equilibrium. This amounts to solving the following LP: find c, b
* maximize c
* subject to G b = D (a c + a0) + d
* b >= b0
* where:
- * b are the m coefficients of the contact force generators (f = G b)
- * b0 is an m-dimensional vector of identical values that are proportional to e_max
- * c is the 1d line parameter
- * G is the 6xm matrix whose columns are the gravito-inertial wrench generators
- * D is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
- * d is the 6d vector containing the gravity part of the gravito-inertial wrench
+ * b are the m coefficients of the contact force generators (f = G
+ * b) b0 is an m-dimensional vector of identical values that are
+ * proportional to e_max c is the 1d line parameter G is the
+ * 6xm matrix whose columns are the gravito-inertial wrench generators D is
+ * the 6x3 matrix mapping the CoM position in gravito-inertial wrench d is the
+ * 6d vector containing the gravity part of the gravito-inertial wrench
* @param a 2d vector representing the line direction
* @param a0 2d vector representing an arbitrary point over the line
- * @param e_max Desired robustness in terms of the maximum force error tolerated by the system
+ * @param e_max Desired robustness in terms of the maximum force error
+ * tolerated by the system
* @return The status of the LP solver.
- * @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.
+ * @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);
+ LP_status findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max,
+ Ref_vector3 com);
/**
- * @brief Find the extremum com position that is in robust equilibrium in the specified direction.
- * This amounts to solving the following LP:
- * find c, b
+ * @brief Find the extremum com position that is in robust equilibrium in the
+ * specified direction. This amounts to solving the following LP: find c, b
* maximize a^T c
* subject to G b = D c + d
* b >= b0
* where:
* a is the specified 2d direction
- * b are the m coefficients of the contact force generators (f = G b)
- * b0 is an m-dimensional vector of identical values that are proportional to e_max
- * c is the 3d com position
- * G is the 6xm matrix whose columns are the gravito-inertial wrench generators
- * D is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
- * d is the 6d vector containing the gravity part of the gravito-inertial wrench
+ * b are the m coefficients of the contact force generators (f = G
+ * b) b0 is an m-dimensional vector of identical values that are
+ * proportional to e_max c is the 3d com position G is the 6xm
+ * matrix whose columns are the gravito-inertial wrench generators D is the
+ * 6x3 matrix mapping the CoM position in gravito-inertial wrench d is
+ * the 6d vector containing the gravity part of the gravito-inertial wrench
* @param direction Desired 3d direction.
* @param com Output 3d com position.
* @param e_max Desired robustness level.
* @return The status of the LP solver.
- * @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.
+ * @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 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
* for the current contact set. WARNING. The H and h matrices are defined
- * in such a way that H w >= h is verified if w is an admissible wrench. This is different
- * from the ICRA 15 paper from Del Prete et al., where the negative matrices are used.
+ * in such a way that H w >= h is verified if w is an admissible wrench. This
+ * is different from the ICRA 15 paper from Del Prete et al., where the
+ * negative matrices are used.
* @param H reference to the H matrix to initialize
* @param h reference to the h vector to initialize
* @return The status of the inequalities. If the inequalities are not defined
@@ -305,42 +331,38 @@ class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
LP_status getPolytopeInequalities(MatrixXX& H, VectorX& h) const;
/**
- * @brief findMaximumAcceleration Find the maximal acceleration along a given direction
- find b, alpha0
- maximize alpha0
- subject to [-G (Hv)] [b a0]^T = -h
- 0 <= [b a0]^T <= Inf
-
-
- b are the coefficient of the contact force generators (f = V b)
- v is the vector3 defining the direction of the motion
- alpha0 is the maximal amplitude of the acceleration, for the given direction v
- c is the CoM position
- G is the matrix whose columns are the gravito-inertial wrench generators
- H is m*[I3 ĉ]^T
- h is m*[-g (c x -g)]^T
+ * @brief findMaximumAcceleration Find the maximal acceleration along a given
+ direction find b, alpha0 maximize alpha0 subject to [-G
+ (Hv)] [b a0]^T = -h 0 <= [b a0]^T <= Inf
+
+
+ b are the coefficient of the contact force generators (f = V
+ b) v is the vector3 defining the direction of the motion alpha0 is
+ the maximal amplitude of the acceleration, for the given direction v c is the
+ CoM position G is the matrix whose columns are the gravito-inertial
+ wrench generators H is m*[I3 ĉ]^T h is m*[-g (c x -g)]^T
* @param H input
* @param h input
* @param v input
* @param alpha0 output
* @return The status of the LP solver.
*/
- LP_status findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 v, double& alpha0);
+ LP_status findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,
+ Cref_vector3 v, double& alpha0);
/**
- * @brief checkAdmissibleAcceleration return true if the given acceleration is admissible for the given contacts
- find b
- subject to G b = Ha + h
+ * @brief checkAdmissibleAcceleration return true if the given acceleration is
+ admissible for the given contacts find b subject to G b = Ha + h
0 <= b <= Inf
- b are the coefficient of the contact force generators (f = V b)
- a is the vector3 defining the acceleration
- G is the matrix whose columns are the gravito-inertial wrench generators
- H is m*[I3 ĉ]^T
- h is m*[-g (c x -g)]^T
+ b are the coefficient of the contact force generators (f = V
+ b) a is the vector3 defining the acceleration G is the matrix
+ whose columns are the gravito-inertial wrench generators H is m*[I3
+ ĉ]^T h is m*[-g (c x -g)]^T
* @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
diff --git a/include/hpp/centroidal-dynamics/logger.hh b/include/hpp/centroidal-dynamics/logger.hh
index 84c85b29485c99ea16ae90dce826b130b2c69438..ef2c539d7b3db7b6fde688503f13a27352b38ef7 100644
--- a/include/hpp/centroidal-dynamics/logger.hh
+++ b/include/hpp/centroidal-dynamics/logger.hh
@@ -10,10 +10,11 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
-#include <hpp/centroidal-dynamics/local_config.hh>
-#include <sstream>
#include <Eigen/Dense>
+#include <hpp/centroidal-dynamics/local_config.hh>
#include <map>
+#include <sstream>
+
#include "boost/assign.hpp"
namespace centroidal_dynamics {
@@ -43,7 +44,8 @@ namespace centroidal_dynamics {
#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_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
@@ -90,7 +92,8 @@ std::string toString(const T& v) {
}
template <typename T>
-std::string toString(const std::vector<T>& v, const std::string separator = ", ") {
+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];
@@ -98,7 +101,8 @@ std::string toString(const std::vector<T>& v, const std::string separator = ", "
}
template <typename T, int n>
-std::string toString(const Eigen::MatrixBase<T>& v, const std::string separator = ", ") {
+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;
@@ -132,7 +136,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Logger {
* 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);
+ 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. */
@@ -145,26 +150,36 @@ class CENTROIDAL_DYNAMICS_DLLAPI 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
+ 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
+ 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 */
+ /** 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;
+ return m == MSG_TYPE_ERROR_STREAM || m == MSG_TYPE_DEBUG_STREAM ||
+ m == MSG_TYPE_INFO_STREAM || m == MSG_TYPE_WARNING_STREAM;
}
- bool isDebugMsg(MsgType m) { return m == MSG_TYPE_DEBUG_STREAM || m == MSG_TYPE_DEBUG; }
+ bool isDebugMsg(MsgType m) {
+ return m == MSG_TYPE_DEBUG_STREAM || m == MSG_TYPE_DEBUG;
+ }
- bool isInfoMsg(MsgType m) { return m == MSG_TYPE_INFO_STREAM || m == MSG_TYPE_INFO; }
+ bool isInfoMsg(MsgType m) {
+ return m == MSG_TYPE_INFO_STREAM || m == MSG_TYPE_INFO;
+ }
- bool isWarningMsg(MsgType m) { return m == MSG_TYPE_WARNING_STREAM || m == MSG_TYPE_WARNING; }
+ bool isWarningMsg(MsgType m) {
+ return m == MSG_TYPE_WARNING_STREAM || m == MSG_TYPE_WARNING;
+ }
- bool isErrorMsg(MsgType m) { return m == MSG_TYPE_ERROR_STREAM || m == MSG_TYPE_ERROR; }
+ bool isErrorMsg(MsgType m) {
+ return m == MSG_TYPE_ERROR_STREAM || m == MSG_TYPE_ERROR;
+ }
};
/** Method to get the logger (singleton). */
diff --git a/include/hpp/centroidal-dynamics/solver_LP_abstract.hh b/include/hpp/centroidal-dynamics/solver_LP_abstract.hh
index 85226c419fea364e66109fe2b051f263b7df12ab..640785d2430e75325f3be6b440364ad07e7da53b 100644
--- a/include/hpp/centroidal-dynamics/solver_LP_abstract.hh
+++ b/include/hpp/centroidal-dynamics/solver_LP_abstract.hh
@@ -65,8 +65,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI 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) = 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.
@@ -90,13 +91,13 @@ 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,
- Cref_matrixXX A, Cref_vectorX Alb, Cref_vectorX Aub);
+ 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);
/**
- * @brief Read the data describing a Linear Program from the specified binary file.
- * The vectors and matrices are resized inside the method.
- * minimize c' x
+ * @brief Read the data describing a Linear Program from the specified binary
+ * file. The vectors and matrices are resized inside the method. minimize c' x
* subject to Alb <= A x <= Aub
* lb <= x <= ub
* @param filename
@@ -108,7 +109,8 @@ 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,
+ 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. */
@@ -123,7 +125,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_abstract {
/** Return true if the solver is allowed to warm start, false otherwise. */
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; }
diff --git a/include/hpp/centroidal-dynamics/solver_LP_clp.hh b/include/hpp/centroidal-dynamics/solver_LP_clp.hh
index 742ccacf0d32f700f051ddb1b28cc78991d38b01..bdc5973ada1133f38cf0c3d179ce7f8a1ade9b8d 100644
--- a/include/hpp/centroidal-dynamics/solver_LP_clp.hh
+++ b/include/hpp/centroidal-dynamics/solver_LP_clp.hh
@@ -9,9 +9,10 @@
#define CENTROIDAL_DYNAMICS_LIB_SOLVER_LP_CLP_HH
#include <hpp/centroidal-dynamics/local_config.hh>
-#include <hpp/centroidal-dynamics/util.hh>
#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
-#include "ClpSimplex.hpp"
+#include <hpp/centroidal-dynamics/util.hh>
+
+#include "coin/ClpSimplex.hpp"
namespace centroidal_dynamics {
@@ -27,8 +28,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();
diff --git a/include/hpp/centroidal-dynamics/solver_LP_qpoases.hh b/include/hpp/centroidal-dynamics/solver_LP_qpoases.hh
index d292c41edc482c525284e369278a1ad81facfdba..31fef4c17941ac21ef2c70ac547ab5d47705043e 100644
--- a/include/hpp/centroidal-dynamics/solver_LP_qpoases.hh
+++ b/include/hpp/centroidal-dynamics/solver_LP_qpoases.hh
@@ -7,8 +7,8 @@
#define HPP_CENTROIDAL_DYNAMICS_SOLVER_LP_QPOASES_HH
#include <hpp/centroidal-dynamics/local_config.hh>
-#include <hpp/centroidal-dynamics/util.hh>
#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
+#include <hpp/centroidal-dynamics/util.hh>
#include <qpOASES.hpp>
namespace centroidal_dynamics {
@@ -18,8 +18,8 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases : public Solver_LP_abstract {
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
+ 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:
@@ -32,8 +32,9 @@ 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();
@@ -41,7 +42,9 @@ class CENTROIDAL_DYNAMICS_DLLAPI Solver_LP_qpoases : public Solver_LP_abstract {
/** Get the objective value of the last solved problem. */
virtual double getObjectiveValue() { return m_solver.getObjVal(); }
- virtual void getDualSolution(Ref_vectorX res) { m_solver.getDualSolution(res.data()); }
+ virtual void getDualSolution(Ref_vectorX res) {
+ m_solver.getDualSolution(res.data());
+ }
};
} // end namespace centroidal_dynamics
diff --git a/include/hpp/centroidal-dynamics/stop-watch.hh b/include/hpp/centroidal-dynamics/stop-watch.hh
index 354e430e908b2a7382ef43569a3760572c683578..9f6faad64e9cbe315a3b5bb7251ce11410ce70f9 100644
--- a/include/hpp/centroidal-dynamics/stop-watch.hh
+++ b/include/hpp/centroidal-dynamics/stop-watch.hh
@@ -169,7 +169,8 @@ 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);
@@ -207,7 +208,13 @@ class Stopwatch {
/** 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) {}
+ : 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;
diff --git a/include/hpp/centroidal-dynamics/util.hh b/include/hpp/centroidal-dynamics/util.hh
index 93d44b404e0c1e98f67ca5d20b0cedd0367c8451..01ef7320a30e8ffd78a9e457c2ce8519ee9cacbb 100644
--- a/include/hpp/centroidal-dynamics/util.hh
+++ b/include/hpp/centroidal-dynamics/util.hh
@@ -5,17 +5,17 @@
#ifndef HPP_CENTROIDAL_DYNAMICS_UTIL_HH
#define HPP_CENTROIDAL_DYNAMICS_UTIL_HH
-#include <iostream>
-#include <fstream>
-#include <cmath>
-#include <cassert>
-
#include <Eigen/Dense>
+// Macros.h needs to be included after Dense
#include <Eigen/src/Core/util/Macros.h>
-#include "cddmp.h"
+#include <cassert>
+#include <cmath>
+#include <fstream>
+#include <iostream>
+
#include "setoper.h"
-#include "cddtypes.h"
+// cdd.h needs to be included after setoper.h
#include "cdd.h"
namespace centroidal_dynamics {
@@ -44,7 +44,9 @@ 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::Matrix<value_type, Eigen::Dynamic, Eigen::Dynamic,
+ Eigen::RowMajor>
+ MatrixXX;
typedef Eigen::Ref<Vector2> Ref_vector2;
typedef Eigen::Ref<Vector3> Ref_vector3;
@@ -67,8 +69,11 @@ 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 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;
@@ -77,12 +82,14 @@ typedef Eigen::Ref<MatrixXXColMajor>& ref_matrixXXColMajor;
*/
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);
+ 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.write((char*)matrix.data(),
+ rows * cols * sizeof(typename Matrix::Scalar));
out.close();
return true;
}
@@ -110,10 +117,12 @@ bool readMatrixFromFile(const std::string& filename, Matrix& matrix) {
* @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.
+ * Compute the cross-product skew-symmetric matrix associated to the specified
+ * vector.
*/
Rotation crossMatrix(Cref_vector3 x);
@@ -121,13 +130,17 @@ void init_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);
-bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy, Ref_matrix43 p,
+bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos,
+ Cref_vector3 rpy, Ref_matrix43 p,
Ref_matrix43 N);
std::string getDateAndTimeAsString();
@@ -139,8 +152,10 @@ std::string getDateAndTimeAsString();
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) {
+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;
@@ -156,8 +171,9 @@ void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& runnin
}
/**
- * 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).
+ * 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
* @param k size of sub-set to consider
* @param U result matrix
@@ -165,7 +181,8 @@ void doCombs(Eigen::Matrix<typename DerivedU::Scalar, 1, Eigen::Dynamic>& runnin
* 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) {
+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);
diff --git a/pyproject.toml b/pyproject.toml
new file mode 100644
index 0000000000000000000000000000000000000000..7ad22b44c945457602878cb6d16e799856c6634f
--- /dev/null
+++ b/pyproject.toml
@@ -0,0 +1,2 @@
+[tool.black]
+exclude = "cmake"
diff --git a/python/centroidal_dynamics_python.cpp b/python/centroidal_dynamics_python.cpp
index 3c299a7eae6ec198e34318e82e7b1a23ba60219f..06952ac810f51e81f8feb000f6a2b60c2f675fb1 100644
--- a/python/centroidal_dynamics_python.cpp
+++ b/python/centroidal_dynamics_python.cpp
@@ -1,44 +1,53 @@
-#include "hpp/centroidal-dynamics/centroidal_dynamics.hh"
-#include "hpp/centroidal-dynamics/util.hh"
-
-#include <eigenpy/memory.hpp>
+#include <boost/python.hpp>
#include <eigenpy/eigenpy.hpp>
+#include <eigenpy/memory.hpp>
-#include <boost/python.hpp>
+#include "hpp/centroidal-dynamics/centroidal_dynamics.hh"
+#include "hpp/centroidal-dynamics/util.hh"
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(centroidal_dynamics::Equilibrium)
namespace centroidal_dynamics {
using namespace boost::python;
-boost::python::tuple wrapComputeQuasiEquilibriumRobustness(Equilibrium& self, const Vector3& com) {
+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) {
+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) {
+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,
+bool wrapSetNewContacts(Equilibrium& self,
+ const MatrixX3ColMajor& contactPoints,
+ const MatrixX3ColMajor& contactNormals,
+ const double frictionCoefficient,
const EquilibriumAlgorithm alg) {
- return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
+ return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient,
+ alg);
}
-bool wrapSetNewContactsFull(Equilibrium& self, const MatrixX3ColMajor& contactPoints,
- const MatrixX3ColMajor& contactNormals, const double frictionCoefficient,
+bool wrapSetNewContactsFull(Equilibrium& self,
+ const MatrixX3ColMajor& contactPoints,
+ const MatrixX3ColMajor& contactNormals,
+ const double frictionCoefficient,
const EquilibriumAlgorithm alg) {
- return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient, alg);
+ return self.setNewContacts(contactPoints, contactNormals, frictionCoefficient,
+ alg);
}
boost::python::tuple wrapGetPolytopeInequalities(Equilibrium& self) {
@@ -71,7 +80,9 @@ BOOST_PYTHON_MODULE(hpp_centroidal_dynamics) {
.value("SOLVER_LP_CLP", SOLVER_LP_CLP)
.export_values();
#else
- enum_<SolverLP>("SolverLP").value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES).export_values();
+ enum_<SolverLP>("SolverLP")
+ .value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES)
+ .export_values();
#endif
enum_<EquilibriumAlgorithm>("EquilibriumAlgorithm")
@@ -95,19 +106,22 @@ BOOST_PYTHON_MODULE(hpp_centroidal_dynamics) {
/** END enum types **/
// bool (Equilibrium::*setNewContacts)
- // (const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm, const int
- // graspIndex, const double maxGraspForce) = &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> >())
+ 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",
+ wrapComputeQuasiEquilibriumRobustness)
.def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
.def("checkRobustEquilibrium", wrapCheckRobustEquilibrium)
.def("getPolytopeInequalities", wrapGetPolytopeInequalities);
diff --git a/python/test/binding_tests.py b/python/test/binding_tests.py
index bc8fb5f6545ab922560190f9ae24db562cabe56a..45831c03f5de344014aae99d64c06b25a8d59e19 100644
--- a/python/test/binding_tests.py
+++ b/python/test/binding_tests.py
@@ -2,18 +2,23 @@ import unittest
from numpy import array, asmatrix, cross
-from hpp_centroidal_dynamics import LP_STATUS_OPTIMAL, Equilibrium, EquilibriumAlgorithm, SolverLP
+from hpp_centroidal_dynamics import (
+ LP_STATUS_OPTIMAL,
+ Equilibrium,
+ EquilibriumAlgorithm,
+ SolverLP,
+)
class TestCentroidalDynamics(unittest.TestCase):
def test_all(self):
# 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)
+ eq = Equilibrium("test", 54.0, 4)
+ eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES)
+ eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES)
+ eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, False)
+ eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, False, 1)
+ eq = Equilibrium("test", 54.0, 4, SolverLP.SOLVER_LP_QPOASES, True, 1, True)
# whether useWarmStart is enable (True by default)
previous = eq.useWarmStart()
@@ -24,52 +29,64 @@ class TestCentroidalDynamics(unittest.TestCase):
# access solver name
self.assertEqual(eq.getName(), "test")
- 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]]))
+ z = array([0.0, 0.0, 1.0])
+ 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)]))
# setting contact positions and normals, as well as friction coefficients
- eq.setNewContacts(asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP)
+ eq.setNewContacts(
+ asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_LP
+ )
- c = asmatrix(array([0., 0., 1.])).T
+ c = asmatrix(array([0.0, 0.0, 1.0])).T
- # computing robustness of a given configuration, first with no argument (0 acceleration, static equilibrium)
+ # 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")
# computing robustness of a given configuration with non zero acceleration
- ddc = asmatrix(array([1000., 0., 0.])).T
+ ddc = asmatrix(array([1000.0, 0.0, 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)
+ eq.setNewContacts(
+ asmatrix(P), asmatrix(N), 0.3, EquilibriumAlgorithm.EQUILIBRIUM_ALGORITHM_PP
+ )
H, h = eq.getPolytopeInequalities()
# c= asmatrix(array([0.,0.,1.])).T
status, stable = eq.checkRobustEquilibrium(c)
# TODO: This works well unless we add a --coverage in CXXFLAGS
- # self.assertEqual(status, LP_STATUS_OPTIMAL, "checkRobustEquilibrium should not fail")
+ # self.assertEqual(status, LP_STATUS_OPTIMAL,
+ # "checkRobustEquilibrium should not fail")
self.assertTrue(stable, "lat test should be in equilibrirum")
- def compute_w(c, ddc, dL=array([0., 0., 0.]), m=54., g_vec=array([0., 0., -9.81])):
+ def compute_w(
+ c, ddc, dL=array([0.0, 0.0, 0.0]), m=54.0, g_vec=array([0.0, 0.0, -9.81])
+ ):
w1 = m * (ddc - g_vec)
return array(w1.tolist() + (cross(c, w1) + dL).tolist())
- 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.):
+ def is_stable(
+ H,
+ c=array([0.0, 0.0, 1.0]),
+ ddc=array([0.0, 0.0, 0.0]),
+ dL=array([0.0, 0.0, 0.0]),
+ m=54.0,
+ g_vec=array([0.0, 0.0, -9.81]),
+ robustness=0.0,
+ ):
w = compute_w(c, ddc, dL, m, g_vec)
return (H.dot(-w) <= -robustness).all()
self.assertTrue(is_stable(H))
-if __name__ == '__main__':
+if __name__ == "__main__":
unittest.main()
diff --git a/setup.cfg b/setup.cfg
new file mode 100644
index 0000000000000000000000000000000000000000..98698ff08783653bbcff9a976fefd01934bb6de6
--- /dev/null
+++ b/setup.cfg
@@ -0,0 +1,4 @@
+[flake8]
+exclude = cmake
+max-line-length = 88
+ignore = E226, E704, E24, E121, W504, E126, E123, W503, E203
diff --git a/src/centroidal_dynamics.cpp b/src/centroidal_dynamics.cpp
index e5061bbe1d00cc1f5c86c177fc66679d0811adba..7d989dd0ab3c853dfd781cd4d19f60c672f3ec4c 100644
--- a/src/centroidal_dynamics.cpp
+++ b/src/centroidal_dynamics.cpp
@@ -3,12 +3,12 @@
* 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;
@@ -38,8 +38,10 @@ Equilibrium::Equilibrium(const Equilibrium& other)
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,
+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),
@@ -59,7 +61,8 @@ Equilibrium::Equilibrium(const string& name, const double mass, const unsigned i
m_generatorsPerContact = generatorsPerContact;
if (generatorsPerContact < 3) {
- SEND_WARNING_MSG("Algorithm cannot work with less than 3 generators per contact!");
+ SEND_WARNING_MSG(
+ "Algorithm cannot work with less than 3 generators per contact!");
m_generatorsPerContact = 3;
}
@@ -74,15 +77,19 @@ Equilibrium::Equilibrium(const string& name, const double mass, const unsigned i
Equilibrium::~Equilibrium() { delete m_solver; }
-bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
- double frictionCoefficient, const bool perturbate) {
+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;
+ if (perturbate)
+ frictionCoefficient =
+ frictionCoefficient + (rand() / value_type(RAND_MAX)) * epsilon;
// Tangent directions
Vector3 T1, T2, N;
// contact point
@@ -94,7 +101,8 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
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()));
+ SEND_ERROR_MSG("Contact normals should have norm 1, this has norm %f" +
+ toString(contactNormals.row(i).norm()));
return false;
}
// compute tangent directions
@@ -119,10 +127,12 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
// compute generators
theta = pi_4;
for (unsigned int j = 0; j < cg; j++) {
- G.col(j) = frictionCoefficient * sin(theta) * T1 + frictionCoefficient * cos(theta) * T2 +
+ 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()));
+ // SEND_DEBUG_MSG("Contact "+toString(i)+" generator "+toString(j)+"
+ // = "+toString(G.col(j).transpose()));
theta += delta_theta;
}
@@ -132,11 +142,13 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
// 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
+ 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
+ // 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;
}
@@ -144,20 +156,26 @@ bool Equilibrium::computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 c
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");
+ "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) {
+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);
+ return setNewContacts(_contactPoints, _contactNormals, frictionCoefficient,
+ alg);
}
-bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
- const double frictionCoefficient, const EquilibriumAlgorithm 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) {
SEND_ERROR_MSG("Algorithm IP not implemented yet");
@@ -173,14 +191,16 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
// Lists of contact generators (3 X 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) {
unsigned int attempts = max_num_cdd_trials;
while (!computePolytopeProjection(m_G_centr) && attempts > 0) {
- computeGenerators(contactPoints, contactNormals, frictionCoefficient, true);
+ computeGenerators(contactPoints, contactNormals, frictionCoefficient,
+ true);
attempts--;
}
// resetting random because obviously libcdd always resets to 0
@@ -197,30 +217,34 @@ bool Equilibrium::setNewContacts(const MatrixX3& contactPoints, const MatrixX3&
static const Vector3 zero_acc = Vector3::Zero();
-LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, double& 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);
- const long m = m_G_centr.cols(); // number of gravito-inertial wrench generators
+ 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) {
- /* Compute the robustness measure of the equilibrium of a specified CoM position
+ /* Compute the robustness measure of the equilibrium of a specified CoM
+ position
* by solving the following LP:
find b, b0
minimize -b0
subject to D c + d <= G b <= D c + d
0 <= b - b0 <= Inf
where
- b are the coefficient of the contact force generators (f = V b)
- b0 is the robustness measure
- c is the CoM position
- G is the matrix whose columns are the gravito-inertial wrench generators
+ b are the coefficient of the contact force generators (f = V
+ b) b0 is the robustness measure 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);
@@ -242,22 +266,24 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
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) {
- /* Compute the robustness measure of the equilibrium of a specified CoM position
+ /* Compute the robustness measure of the equilibrium of a specified CoM
+ position
* by solving the following LP:
find b, b0
minimize -b0
subject to D c + d <= G (b + 1*b0) <= D c + d
0 <= b <= Inf
where
- b are the (delta) coefficient of the contact force generators (f = G (b+b0))
- b0 is the robustness measure
- c is the CoM position
- G is the matrix whose columns are the gravito-inertial wrench generators
+ b are the (delta) coefficient of the contact force
+ generators (f = G (b+b0)) b0 is the robustness measure 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);
@@ -277,21 +303,19 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
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) {
- /*Compute the robustness measure of the equilibrium of a specified CoM position
- by solving the following dual LP:
- find v
- minimize (d+D*com)' v
- subject to G' v >= 0
- 1' G' v = 1
- where
+ /*Compute the robustness measure of the equilibrium of a specified CoM
+ position by solving the following dual LP: find v minimize
+ (d+D*com)' v subject to G' v >= 0 1' G' v = 1 where
-(d+D c)' v is the robustness measure
c is the CoM position
- G is the matrix whose columns are the gravito-inertial wrench generators
+ G is the matrix whose columns are the gravito-inertial
+ wrench generators
*/
Vector6 v;
Vector6 c = m_D * com + m_d;
@@ -310,7 +334,8 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
robustness = convert_b0_to_emax(m_solver->getObjectiveValue());
return lpStatus_dual;
}
- SEND_DEBUG_MSG("Dual LP problem for com position " + toString(com.transpose()) +
+ 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
@@ -322,15 +347,20 @@ LP_status Equilibrium::computeEquilibriumRobustness(Cref_vector3 com, Cref_vecto
return lpStatus_dual;
}
- SEND_ERROR_MSG("computeEquilibriumRobustness is not implemented for the specified algorithm");
+ SEND_ERROR_MSG(
+ "computeEquilibriumRobustness is not implemented for the specified "
+ "algorithm");
return LP_STATUS_ERROR;
}
-LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, bool& equilibrium, double e_max) {
+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, Cref_vector3 acc, bool& equilibrium, double 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);
@@ -346,7 +376,8 @@ LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc
return LP_STATUS_ERROR;
}
if (m_algorithm != EQUILIBRIUM_ALGORITHM_PP) {
- SEND_ERROR_MSG("checkRobustEquilibrium is only implemented for the PP algorithm");
+ SEND_ERROR_MSG(
+ "checkRobustEquilibrium is only implemented for the PP algorithm");
return LP_STATUS_ERROR;
}
@@ -363,7 +394,8 @@ LP_status Equilibrium::checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc
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");
+ SEND_ERROR_MSG(
+ "getPolytopeInequalities is only implemented for the PP algorithm");
return LP_STATUS_ERROR;
}
if (!m_is_cdd_stable) {
@@ -378,24 +410,27 @@ LP_status Equilibrium::getPolytopeInequalities(MatrixXX& H, VectorX& h) const {
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
+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) {
- /* Compute the extremum CoM position over the line a*p + a0 that is in robust equilibrium
+ /* Compute the extremum CoM position over the line a*p + a0 that is in
+ robust equilibrium
* by solving the following LP:
find b, p
minimize -p
subject to D (a p + a0) + d <= G (b + b0) <= D (a p + a0) + d
0 <= b <= Inf
where
- b0+b are the coefficient of the contact force generators (f = G (b0+b))
- b0 is the robustness measure
- p is the line parameter
- G is the matrix whose columns are the gravito-inertial wrench generators
+ b0+b are the coefficient of the contact force generators (f = G
+ (b0+b)) b0 is the robustness measure 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);
@@ -428,21 +463,26 @@ LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, dou
}
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) {
- /* Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium
+ /* Compute the extremum CoM position over the line a*x + a0 that is in
+ robust equilibrium
* by solving the following dual LP:
find v
minimize (D a0 + d -G b0)' v
subject to 0 <= G' v <= Inf
-1 <= a' D' v <= -1
where
- G is the matrix whose columns are the gravito-inertial wrench generators
+ 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;
@@ -471,10 +511,13 @@ LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, dou
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
+ // 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()) +
+ 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;
@@ -484,9 +527,12 @@ 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)
@@ -497,11 +543,14 @@ LP_status Equilibrium::findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, dou
return lpStatus_dual;
}
- SEND_ERROR_MSG("findExtremumOverLine is not implemented for the specified algorithm");
+ SEND_ERROR_MSG(
+ "findExtremumOverLine is not implemented for the specified algorithm");
return LP_STATUS_ERROR;
}
-LP_status Equilibrium::findExtremumInDirection(Cref_vector3 /*direction*/, Ref_vector3 /*com*/, double /*e_max*/) {
+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;
@@ -515,7 +564,8 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v) {
return false;
}
// getProfiler().start("eigen_to_cdd");
- dd_MatrixPtr V = cone_span_eigen_to_cdd(v.transpose(), canonicalize_cdd_matrix);
+ dd_MatrixPtr V =
+ cone_span_eigen_to_cdd(v.transpose(), canonicalize_cdd_matrix);
// getProfiler().stop("eigen_to_cdd");
dd_ErrorType error = dd_NoError;
@@ -552,10 +602,12 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v) {
m_h.resize(rowsize + eq_rows.size());
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));
}
@@ -569,11 +621,16 @@ bool Equilibrium::computePolytopeProjection(Cref_matrix6X v) {
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) {
+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);
@@ -603,11 +660,13 @@ LP_status Equilibrium::findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,
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) {
+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);
@@ -625,7 +684,8 @@ bool Equilibrium::checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, C
if (lpStatus == LP_STATUS_OPTIMAL || lpStatus == LP_STATUS_UNBOUNDED) {
return true;
} else {
- // 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 false;
}
}
diff --git a/src/logger.cpp b/src/logger.cpp
index eed2faa76086c296d2bbbf2c102be5dfb65a0e9c..f3f9f4d5bd3ee2b19c6db26d3454d062f1a23852 100644
--- a/src/logger.cpp
+++ b/src/logger.cpp
@@ -10,10 +10,11 @@
#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;
@@ -24,7 +25,9 @@ Logger& getLogger() {
}
Logger::Logger(double timeSample, double streamPrintPeriod)
- : m_timeSample(timeSample), m_streamPrintPeriod(streamPrintPeriod), m_printCountdown(0.0) {
+ : m_timeSample(timeSample),
+ m_streamPrintPeriod(streamPrintPeriod),
+ m_printCountdown(0.0) {
#ifdef LOGGER_VERBOSITY_ERROR
m_lv = VERBOSITY_ERROR;
#endif
@@ -47,8 +50,9 @@ void Logger::countdown() {
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)))
+ // (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
diff --git a/src/solver_LP_abstract.cpp b/src/solver_LP_abstract.cpp
index 1fd47b41f30b16d3f5e8b0beb3a9f8aeceb48c5d..9f76b343361aa9ebbfbd91dcd3bd2c4c12970f12 100644
--- a/src/solver_LP_abstract.cpp
+++ b/src/solver_LP_abstract.cpp
@@ -3,9 +3,9 @@
* 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
@@ -23,12 +23,15 @@ Solver_LP_abstract *Solver_LP_abstract::getNewSolver(SolverLP solverType) {
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) {
+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);
@@ -36,7 +39,8 @@ bool Solver_LP_abstract::writeLpToFile(const std::string &filename, Cref_vectorX
assert(Alb.size() == m);
assert(Aub.size() == m);
- std::ofstream out(filename.c_str(), std::ios::out | std::ios::binary | std::ios::trunc);
+ std::ofstream out(filename.c_str(),
+ std::ios::out | std::ios::binary | std::ios::trunc);
out.write((char *)(&n), sizeof(typename MatrixXX::Index));
out.write((char *)(&m), sizeof(typename MatrixXX::Index));
out.write((char *)c.data(), n * sizeof(typename MatrixXX::Scalar));
@@ -49,7 +53,8 @@ bool Solver_LP_abstract::writeLpToFile(const std::string &filename, Cref_vectorX
return true;
}
-bool Solver_LP_abstract::readLpFromFile(const std::string &filename, VectorX &c, VectorX &lb, VectorX &ub, MatrixXX &A,
+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;
@@ -71,7 +76,8 @@ bool Solver_LP_abstract::readLpFromFile(const std::string &filename, VectorX &c,
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)) {
diff --git a/src/solver_LP_clp.cpp b/src/solver_LP_clp.cpp
index 12fbcfba71d2cd6bb6756dbf7e85b673b5208e19..4495e7af42cb7a1e33e73b9ae49f5c782545793e 100644
--- a/src/solver_LP_clp.cpp
+++ b/src/solver_LP_clp.cpp
@@ -6,13 +6,17 @@
#ifdef CLP_FOUND
#include <hpp/centroidal-dynamics/solver_LP_clp.hh>
-#include "CoinBuild.hpp"
+
+#include "coin/CoinBuild.hpp"
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,
+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
diff --git a/src/solver_LP_qpoases.cpp b/src/solver_LP_qpoases.cpp
index 0c25aa59c49f8ebc90486fd07fe3164f37f4aa9f..d63e45afb169c678fb921fbda09573cd244b4bf2 100644
--- a/src/solver_LP_qpoases.cpp
+++ b/src/solver_LP_qpoases.cpp
@@ -3,8 +3,8 @@
* Author: Andrea Del Prete
*/
-#include <hpp/centroidal-dynamics/solver_LP_qpoases.hh>
#include <hpp/centroidal-dynamics/logger.hh>
+#include <hpp/centroidal-dynamics/solver_LP_qpoases.hh>
USING_NAMESPACE_QPOASES
@@ -19,8 +19,10 @@ Solver_LP_qpoases::Solver_LP_qpoases() : Solver_LP_abstract() {
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) {
+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);
@@ -40,13 +42,14 @@ LP_status Solver_LP_qpoases::solve(Cref_vectorX c, Cref_vectorX lb, Cref_vectorX
}
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);
+ 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);
+ 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;
}
diff --git a/src/stop-watch.cpp b/src/stop-watch.cpp
index 946ec195719c0015fd6171b062aaf9ce4a870e89..e043c781dd48f98647fbea1eda8561c33c941188 100644
--- a/src/stop-watch.cpp
+++ b/src/stop-watch.cpp
@@ -30,10 +30,12 @@ WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include <sys/time.h>
#else
#include <Windows.h>
+
#include <iomanip>
#endif
#include <iomanip> // std::setprecision
+
#include "hpp/centroidal-dynamics/stop-watch.hh"
using std::map;
@@ -53,7 +55,9 @@ Stopwatch::~Stopwatch() { delete records_of; }
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) {
@@ -121,7 +125,8 @@ void Stopwatch::stop(string perf_name) {
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;
@@ -135,7 +140,8 @@ void Stopwatch::stop(string perf_name) {
// 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.min_time || perf_info.min_time == 0)
+ perf_info.min_time = lapse;
// Update total time
perf_info.total_time += lapse;
@@ -147,7 +153,8 @@ void Stopwatch::pause(string perf_name) {
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;
@@ -171,7 +178,8 @@ void Stopwatch::reset_all() {
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);
@@ -182,7 +190,8 @@ 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;
@@ -209,21 +218,27 @@ 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 long unsigned int MAX_NAME_LENGTH = 60;
string pad = "";
- for (long unsigned int i = perf_name.length(); i < MAX_NAME_LENGTH; i++) pad.append(" ");
+ for (long unsigned 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;
@@ -233,9 +248,11 @@ void Stopwatch::report(string perf_name, int precision, std::ostream& output) {
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;
@@ -244,7 +261,8 @@ long double Stopwatch::get_time_so_far(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;
@@ -253,7 +271,8 @@ long double Stopwatch::get_total_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;
@@ -262,7 +281,8 @@ long double Stopwatch::get_average_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;
@@ -271,7 +291,8 @@ long double Stopwatch::get_min_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;
@@ -280,7 +301,8 @@ long double Stopwatch::get_max_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 06db4f0854b3d06a5314882d0a554fe6d3297621..bfd65b7c45dcfba6c1b4dda75891f25e1cf43283 100644
--- a/src/util.cpp
+++ b/src/util.cpp
@@ -5,11 +5,11 @@
#include <ctime>
#include <hpp/centroidal-dynamics/util.hh>
-#include <ctime>
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_rowrange i;
@@ -53,24 +53,30 @@ void release_cdd_library() {
// dd_free_global_constants();
}
-void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds, Ref_vector3 out) {
+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);
+ 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) {
+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);
+ 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) {
@@ -102,7 +108,8 @@ void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R) {
// * angle_axis_t(yaw, Vector3::UnitZ())).toRotationMatrix();
}
-bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos, Cref_vector3 rpy, Ref_matrix43 p,
+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;
@@ -181,7 +188,8 @@ value_type nchoosek(const int n, const int k) {
return n;
else {
value_type c = 1;
- for (int i = 1; i <= k; i++) c *= (((value_type)n - k + i) / ((value_type)i));
+ for (int i = 1; i <= k; i++)
+ c *= (((value_type)n - k + i) / ((value_type)i));
return round(c);
}
}
diff --git a/test/test_LP_solvers.cpp b/test/test_LP_solvers.cpp
index e4f9923a0210f53c796f90fc1109c0f87b269975..df86d1cf5185e6d5b1695211fecc43e10e961df6 100644
--- a/test/test_LP_solvers.cpp
+++ b/test/test_LP_solvers.cpp
@@ -4,19 +4,19 @@
*/
#ifdef CLP_FOUND
-#include "ClpSimplex.hpp"
-#include "CoinTime.hpp"
-#include "CoinBuild.hpp"
-#include "CoinModel.hpp"
#include <hpp/centroidal-dynamics/solver_LP_clp.hh>
+
+#include "coin/ClpSimplex.hpp"
+#include "coin/CoinBuild.hpp"
+#include "coin/CoinModel.hpp"
+#include "coin/CoinTime.hpp"
#endif
-#include <qpOASES.hpp>
-#include <hpp/centroidal-dynamics/solver_LP_qpoases.hh>
#include <hpp/centroidal-dynamics/logger.hh>
-
-#include <iostream>
+#include <hpp/centroidal-dynamics/solver_LP_qpoases.hh>
#include <iomanip>
+#include <iostream>
+#include <qpOASES.hpp>
using namespace std;
using namespace centroidal_dynamics;
@@ -44,7 +44,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);
@@ -94,7 +95,8 @@ void test_addRows() {
buildObject.addRow(3, row2Index, row2Value, 1.0, 1.0);
}
model.addRows(buildObject);
- printf("Time for 10000 addRow using CoinBuild is %g\n", CoinCpuTime() - time1);
+ printf("Time for 10000 addRow using CoinBuild is %g\n",
+ CoinCpuTime() - time1);
model.dual();
model = modelSave;
int del[] = {0, 1, 2};
@@ -108,7 +110,8 @@ void test_addRows() {
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);
+ printf("Time for 10000 addRow using CoinBuild+-1 is %g\n",
+ CoinCpuTime() - time1);
model.dual();
model = modelSave;
model.deleteRows(2, del);
@@ -121,7 +124,8 @@ void test_addRows() {
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);
+ printf("Time for 10000 addRow using CoinModel+-1 is %g\n",
+ CoinCpuTime() - time1);
model.dual();
model = ClpSimplex();
// Now use build +-1
@@ -133,7 +137,8 @@ void test_addRows() {
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);
+ printf("Time for 10000 addRow using CoinModel load +-1 is %g\n",
+ CoinCpuTime() - time1);
model.writeMps("xx.mps");
model.dual();
model = modelSave;
@@ -146,7 +151,8 @@ void test_addRows() {
modelObject.addRow(3, row2Index, row2Value, 1.0, 1.0);
}
model.addRows(modelObject);
- printf("Time for 10000 addRow using CoinModel is %g\n", CoinCpuTime() - time1);
+ printf("Time for 10000 addRow using CoinModel is %g\n",
+ CoinCpuTime() - time1);
model.dual();
model.writeMps("b.mps");
// Method using least memory - but most complicated
@@ -200,10 +206,12 @@ 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);
+ 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);
+ 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
// model2.setSpecialOptions(256);
}
@@ -225,42 +233,55 @@ void test_addRows() {
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;
std::cout << std::setw(6) << iColumn << " ";
value = columnPrimal[iColumn];
if (fabs(value) < 1.0e5)
- std::cout << setiosflags(std::ios::fixed | std::ios::showpoint) << std::setw(14) << value;
+ std::cout << setiosflags(std::ios::fixed | std::ios::showpoint)
+ << std::setw(14) << value;
else
- std::cout << setiosflags(std::ios::scientific) << std::setw(14) << value;
+ std::cout << setiosflags(std::ios::scientific) << std::setw(14)
+ << value;
value = columnDual[iColumn];
if (fabs(value) < 1.0e5)
- std::cout << setiosflags(std::ios::fixed | std::ios::showpoint) << std::setw(14) << value;
+ std::cout << setiosflags(std::ios::fixed | std::ios::showpoint)
+ << std::setw(14) << value;
else
- std::cout << setiosflags(std::ios::scientific) << std::setw(14) << value;
+ std::cout << setiosflags(std::ios::scientific) << std::setw(14)
+ << value;
value = columnLower[iColumn];
if (fabs(value) < 1.0e5)
- std::cout << setiosflags(std::ios::fixed | std::ios::showpoint) << std::setw(14) << value;
+ std::cout << setiosflags(std::ios::fixed | std::ios::showpoint)
+ << std::setw(14) << value;
else
- std::cout << setiosflags(std::ios::scientific) << std::setw(14) << value;
+ std::cout << setiosflags(std::ios::scientific) << std::setw(14)
+ << value;
value = columnUpper[iColumn];
if (fabs(value) < 1.0e5)
- std::cout << setiosflags(std::ios::fixed | std::ios::showpoint) << std::setw(14) << value;
+ std::cout << setiosflags(std::ios::fixed | std::ios::showpoint)
+ << std::setw(14) << value;
else
- std::cout << setiosflags(std::ios::scientific) << std::setw(14) << value;
+ std::cout << setiosflags(std::ios::scientific) << std::setw(14)
+ << value;
value = columnObjective[iColumn];
if (fabs(value) < 1.0e5)
- std::cout << setiosflags(std::ios::fixed | std::ios::showpoint) << std::setw(14) << value;
+ std::cout << setiosflags(std::ios::fixed | std::ios::showpoint)
+ << std::setw(14) << value;
else
- std::cout << setiosflags(std::ios::scientific) << std::setw(14) << value;
+ std::cout << setiosflags(std::ios::scientific) << std::setw(14)
+ << value;
std::cout << std::endl;
}
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::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);
@@ -277,7 +298,8 @@ void test_addRows() {
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;
}
}
@@ -304,7 +326,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);
@@ -325,7 +348,7 @@ void test_small_LP() {
double row2Value[] = {1.0, -5.0, 1.0};
model.addRow(3, row2Index, row2Value, 1.0, 1.0);
- int n = model.getdimVarXs();
+ int n = model.getNumCols();
int m = model.getNumRows();
cout << "Problem has " << n << " variables and " << m << " constraints.\n";
@@ -336,8 +359,9 @@ void test_small_LP() {
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.
@@ -375,12 +399,14 @@ int main() {
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";
- Solver_LP_abstract *solverOases = Solver_LP_abstract::getNewSolver(SOLVER_LP_QPOASES);
+ 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";
@@ -407,47 +433,64 @@ int main() {
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 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";
string file_path = "../test_data/";
- Solver_LP_abstract *solverOases = Solver_LP_abstract::getNewSolver(SOLVER_LP_QPOASES);
+ 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"};
+ "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++) {
string &problem_filename = problem_filenames[i];
- if (!solverOases->readLpFromFile(file_path + problem_filename + ".dat", c, lb, ub, A, Alb, Aub)) {
+ 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);
+ 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()
+ 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";
+ 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()
+ 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;
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index 44ea68fe78ec38c0c652d1badfbe79504e0943e9..b5f8d5c98539684e1741393cf97fff2179f54a75 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;
@@ -24,42 +24,51 @@ using namespace std;
#define EPS 1e-3 // required precision
-/** Check the coherence between the method Equilibrium::computeEquilibriumRobustness
- * and the method Equilibrium::checkRobustEquilibrium.
+/** Check the coherence between the method
+ * Equilibrium::computeEquilibriumRobustness and the method
+ * Equilibrium::checkRobustEquilibrium.
* @param solver_1 Solver used to test computeEquilibriumRobustness.
* @param solver_2 Solver used to test checkRobustEquilibrium.
* @param comPositions List of 2d com positions on which to perform the tests.
- * @param PERF_STRING_1 String to use for logging the computation times of 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
+ * @param PERF_STRING_1 String to use for logging the computation times of
+ * 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_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);
- status = solver_1->computeEquilibriumRobustness(comPositions.row(i).transpose(), rob);
+ status = solver_1->computeEquilibriumRobustness(
+ comPositions.row(i).transpose(), rob);
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 " +
+ 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).transpose(), equilibrium);
+ status = solver_2->checkRobustEquilibrium(comPositions.row(i).transpose(),
+ 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 " +
+ SEND_ERROR_MSG(solver_2->getName() +
+ " failed to check equilibrium of com position " +
toString(comPositions.row(i)));
error_counter++;
continue;
@@ -67,56 +76,70 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(Equilibrium* solver_1,
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));
+ 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));
+ 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";
+ cout << "Test test_computeEquilibriumRobustness_vs_checkEquilibrium " +
+ solver_1->getName() + " VS " + solver_2->getName() + ": " +
+ toString(error_counter) + " error(s).\n";
return error_counter;
}
-/** Test two different solvers on the method Equilibrium::computeEquilibriumRobustness.
+/** Test two different solvers on the method
+ * Equilibrium::computeEquilibriumRobustness.
* @param solver_1 First solver to test.
* @param solver_2 Second solver to test.
* @param comPositions List of 2d com positions on which to perform the tests.
- * @param PERF_STRING_1 String to use for logging the computation times of 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
+ * @param PERF_STRING_1 String to use for logging the computation times of
+ * 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++) {
getProfiler().start(PERF_STRING_1);
- status = solver_1->computeEquilibriumRobustness(comPositions.row(i).transpose(), rob_1);
+ status = solver_1->computeEquilibriumRobustness(
+ comPositions.row(i).transpose(), 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 " +
+ SEND_ERROR_MSG(solver_1->getName() +
+ " failed to compute robustness of com position " +
toString(comPositions.row(i)));
error_counter++;
continue;
}
getProfiler().start(PERF_STRING_2);
- status = solver_2->computeEquilibriumRobustness(comPositions.row(i).transpose(), rob_2);
+ status = solver_2->computeEquilibriumRobustness(
+ comPositions.row(i).transpose(), 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 " +
+ SEND_ERROR_MSG(solver_2->getName() +
+ " failed to compute robustness of com position " +
toString(comPositions.row(i)));
error_counter++;
continue;
@@ -125,61 +148,78 @@ int test_computeEquilibriumRobustness(Equilibrium* solver_1, Equilibrium* solver
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));
+ " 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";
+ cout << "Test computeEquilibriumRobustness " + solver_1->getName() +
+ " VS " + solver_2->getName() + ": " + toString(error_counter) +
+ " error(s).\n";
return error_counter;
}
-/** Test method Equilibrium::findExtremumOverLine. The test works in this way: first it
- * calls the method findExtremumOverLine of the solver to test to find the extremum over a random
- * line with a specified robustness. Then it checks that the point found really has the specified
- * robustness by using the ground-truth solver.
+/** Test method Equilibrium::findExtremumOverLine. The test works in this way:
+ * first it calls the method findExtremumOverLine of the solver to test to find
+ * the extremum over a random line with a specified robustness. Then it checks
+ * that the point found really has the specified robustness by using the
+ * ground-truth solver.
* @param solver_to_test Solver to test.
* @param solver_ground_truth Second solver to use as ground truth.
* @param a0 A 2d com position that allows for static equilibrium.
* @param N_TESTS Number of tests to perform.
* @param e_max Maximum value for the desired robustness.
- * @param PERF_STRING_TEST String to use for logging the computation times of solver_to_test
- * @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
+ * @param PERF_STRING_TEST String to use for logging the computation times of
+ * solver_to_test
+ * @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,
+ unsigned int N_TESTS, double e_max,
+ const string& PERF_STRING_TEST,
+ const string& PERF_STRING_GROUND_TRUTH,
+ int verb = 0) {
int error_counter = 0;
centroidal_dynamics::Vector3 a, com;
LP_status status;
double desired_robustness, robustness;
for (unsigned int i = 0; i < N_TESTS; i++) {
- uniform3(-1.0 * centroidal_dynamics::Vector3::Ones(), centroidal_dynamics::Vector3::Ones(), a);
+ uniform3(-1.0 * centroidal_dynamics::Vector3::Ones(),
+ centroidal_dynamics::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) {
- status = solver_ground_truth->computeEquilibriumRobustness(a0, robustness);
+ status =
+ solver_ground_truth->computeEquilibriumRobustness(a0, robustness);
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()));
+ 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 " +
+ 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;
@@ -192,20 +232,26 @@ int test_findExtremumOverLine(Equilibrium* solver_to_test, Equilibrium* solver_g
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()));
+ 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));
+ 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() + ": " +
+ cout << "Test findExtremumOverLine " + solver_to_test->getName() + " VS " +
+ solver_ground_truth->getName() + ": " +
toString(error_counter) + " error(s).\n";
return error_counter;
}
@@ -215,17 +261,20 @@ int test_findExtremumOverLine(Equilibrium* solver_to_test, Equilibrium* solver_g
* @param solver The solver to use for computing the equilibrium robustness.
* @param comPositions Grid of CoM positions in the form of an Nx2 matrix.
*/
-void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium* solver, Cref_matrixXX comPositions,
- Cref_matrixXX p) {
+void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium* solver,
+ Cref_matrixXX comPositions, Cref_matrixXX p) {
MatrixXi contactPointCoord(4 * N_CONTACTS, 2);
- centroidal_dynamics::VectorX minDistContactPoint = 1e10 * centroidal_dynamics::VectorX::Ones(4 * N_CONTACTS);
+ centroidal_dynamics::VectorX minDistContactPoint =
+ 1e10 * centroidal_dynamics::VectorX::Ones(4 * N_CONTACTS);
// create grid of com positions to test
for (int i = 0; i < GRID_SIZE; i++) {
for (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();
+ 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;
@@ -252,15 +301,18 @@ void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium* solver, Cref
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;
LP_status status;
for (int i = 0; i < comPositions.rows(); i++) {
- status = solver->computeEquilibriumRobustness(comPositions.row(i).transpose(), rob);
+ status = solver->computeEquilibriumRobustness(
+ comPositions.row(i).transpose(), 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));
+ SEND_ERROR_MSG(
+ "Faild to compute equilibrium robustness of com position " +
+ toString(comPositions.row(i)) + ", error code " + toString(status));
rob = -1.0;
}
@@ -273,9 +325,12 @@ void drawRobustnessGrid(int N_CONTACTS, int GRID_SIZE, Equilibrium* solver, Cref
}
}
-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, centroidal_dynamics::MatrixX3& p,
+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,
+ centroidal_dynamics::MatrixX3& p,
centroidal_dynamics::MatrixX3& N) {
MatrixXX contact_pos = MatrixXX::Zero(N_CONTACTS, 3);
MatrixXX contact_rpy = MatrixXX::Zero(N_CONTACTS, 3);
@@ -287,27 +342,32 @@ void generateContacts(unsigned int N_CONTACTS, double MIN_CONTACT_DISTANCE, doub
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));
+ uniform(CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS,
+ contact_pos.row(i));
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 ((contact_pos.row(i) - contact_pos.row(j)).norm() <
+ MIN_CONTACT_DISTANCE)
+ collision = true;
if (collision == false) break;
}
// generate contact orientation
uniform(RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS, contact_rpy.row(i));
- generate_rectangle_contacts(LX, LY, contact_pos.row(i).transpose(), contact_rpy.row(i).transpose(),
+ generate_rectangle_contacts(LX, LY, contact_pos.row(i).transpose(),
+ contact_rpy.row(i).transpose(),
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));
+ // 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));
+ // 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));
// }
}
@@ -322,7 +382,9 @@ 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;
centroidal_dynamics::Vector3 com;
@@ -345,19 +407,24 @@ void testWithLoadedData() {
Equilibrium* solvers[N_SOLVERS];
double robustness[N_SOLVERS];
for (int s = 0; s < N_SOLVERS; s++) {
- solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact, SOLVER_LP_QPOASES);
+ 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());
+ SEND_ERROR_MSG("Error while setting new contacts for solver " +
+ solvers[s]->getName());
continue;
}
- LP_status status = solvers[s]->computeEquilibriumRobustness(com, robustness[s]);
+ 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;
+ 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 " +
+ SEND_ERROR_MSG("Solver " + solvers[s]->getName() +
+ " failed to compute robustness, error code " +
toString(status));
}
cout << "*** END TEST WITH LOADED DATA ***\n\n";
@@ -376,7 +443,8 @@ int main() {
RVector3 CONTACT_POINT_LOWER_BOUNDS, CONTACT_POINT_UPPER_BOUNDS;
RVector3 RPY_LOWER_BOUNDS, RPY_UPPER_BOUNDS;
- /************************************** USER PARAMETERS *******************************/
+ /************************************** USER PARAMETERS
+ * *******************************/
unsigned int N_TESTS = 10;
double mass = 55.0;
double mu = 0.3; // friction coefficient
@@ -394,50 +462,64 @@ int main() {
double Y_MARG = 0.07;
const unsigned int GRID_SIZE = 10;
bool DRAW_CONTACT_POINTS = false;
- /************************************ END USER PARAMETERS *****************************/
+ /************************************ END USER PARAMETERS
+ * *****************************/
#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};
- SolverLP lp_solver_types[] = {SOLVER_LP_QPOASES, SOLVER_LP_QPOASES, SOLVER_LP_QPOASES,
- SOLVER_LP_CLP, SOLVER_LP_CLP, SOLVER_LP_CLP};
+ 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};
#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};
- SolverLP lp_solver_types[] = {SOLVER_LP_QPOASES, SOLVER_LP_QPOASES, SOLVER_LP_QPOASES};
+ 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 << "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* solver_PP =
+ new Equilibrium("PP", mass, generatorsPerContact, SOLVER_LP_QPOASES);
Equilibrium* solvers[N_SOLVERS];
for (int s = 0; s < N_SOLVERS; s++)
- solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact, lp_solver_types[s]);
+ solvers[s] = new Equilibrium(solverNames[s], mass, generatorsPerContact,
+ lp_solver_types[s]);
centroidal_dynamics::MatrixX3 p, N;
RVector2 com_LB, com_UB;
centroidal_dynamics::VectorX x_range(GRID_SIZE), y_range(GRID_SIZE);
MatrixXX comPositions(GRID_SIZE * GRID_SIZE, 3);
for (unsigned int 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,
+ 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++) {
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());
+ 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());
+ SEND_ERROR_MSG("Error while setting new contacts for solver " +
+ solver_PP->getName());
return -1;
}
getProfiler().stop(PERF_PP);
@@ -461,17 +543,20 @@ int main() {
}
}
- 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_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++) {
- test_computeEquilibriumRobustness_vs_checkEquilibrium(solvers[s], solver_PP, comPositions,
- test_name + solvers[s]->getName(), "", 1);
+ test_computeEquilibriumRobustness_vs_checkEquilibrium(
+ solvers[s], solver_PP, comPositions,
+ test_name + solvers[s]->getName(), "", 1);
}
const int N_TESTS_EXTREMUM = 100;
@@ -480,15 +565,19 @@ int main() {
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));
+ 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);
+ test_findExtremumOverLine(solvers[s], solvers[0], a0,
+ N_TESTS_EXTREMUM, e_max,
+ test_name + solvers[s]->getName(),
+ test_name2 + solvers[0]->getName(), 1);
}
}
}