diff --git a/include/robust-equilibrium-lib/static_equilibrium.hh b/include/robust-equilibrium-lib/static_equilibrium.hh
index 68c3a753bd221c57ac654986b3eab23c058450ea..4e616c68f4af7df9ff9f602b6af0d67c9a69f1b0 100644
--- a/include/robust-equilibrium-lib/static_equilibrium.hh
+++ b/include/robust-equilibrium-lib/static_equilibrium.hh
@@ -16,43 +16,39 @@ namespace robust_equilibrium
enum ROBUST_EQUILIBRIUM_DLLAPI StaticEquilibriumAlgorithm
{
- STATIC_EQUILIBRIUM_ALGORITHM_LP,
- STATIC_EQUILIBRIUM_ALGORITHM_LP2,
- STATIC_EQUILIBRIUM_ALGORITHM_DLP,
- STATIC_EQUILIBRIUM_ALGORITHM_PP,
- STATIC_EQUILIBRIUM_ALGORITHM_IP,
- STATIC_EQUILIBRIUM_ALGORITHM_DIP
+ STATIC_EQUILIBRIUM_ALGORITHM_LP, /// primal LP formulation
+ STATIC_EQUILIBRIUM_ALGORITHM_LP2, /// another primal LP formulation
+ STATIC_EQUILIBRIUM_ALGORITHM_DLP, /// dual LP formulation
+ STATIC_EQUILIBRIUM_ALGORITHM_PP, /// polytope projection algorithm
+ STATIC_EQUILIBRIUM_ALGORITHM_IP, /// incremental projection algorithm based on primal LP formulation
+ STATIC_EQUILIBRIUM_ALGORITHM_DIP /// incremental projection algorithm based on dual LP formulation
};
class ROBUST_EQUILIBRIUM_DLLAPI StaticEquilibrium
{
private:
- static bool m_is_cdd_initialized;
-
- std::string m_name;
- StaticEquilibriumAlgorithm m_algorithm;
- Solver_LP_abstract* m_solver;
- SolverLP m_solver_type;
-
- unsigned int m_generatorsPerContact;
- double m_mass;
- Vector3 m_gravity;
-
- /** Tangent directions for all contacts (numberOfContacts*generatorsPerContact X 3) */
- MatrixX3 m_T1, m_T2;
- /** Matrix mapping contact forces to gravito-inertial wrench (6 X 3*numberOfContacts) */
- Matrix6X m_A;
- /** Lists of contact generators (3 X numberOfContacts*generatorsPerContact) */
- Matrix3X m_G;
+ static bool m_is_cdd_initialized; /// true if cdd lib has been initialized, false otherwise
+
+ std::string m_name; /// name of this object
+ StaticEquilibriumAlgorithm m_algorithm; /// current algorithm used
+ Solver_LP_abstract* m_solver; /// LP solver
+ SolverLP m_solver_type; /// type of LP solver
+
+ unsigned int m_generatorsPerContact; /// number of generators to approximate the friction cone per contact point
+ double m_mass; /// mass of the system
+ Vector3 m_gravity; /// gravity vector
+
/** Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact) */
Matrix6X m_G_centr;
- /** Inequality matrix 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;
- /** Inequality vector defining the gravito-inertial wrench cone H w <= h */
VectorX m_h;
- /** Inequality matrix defining the CoM support polygon HD com + Hd <= h */
+
+ /** Inequality matrix and vector defining the CoM support polygon HD com + Hd <= h */
MatrixX2 m_HD;
VectorX m_Hd;
+
/** Matrix and vector mapping 2d com position to GIW */
Matrix62 m_D;
Vector6 m_d;
@@ -60,26 +56,84 @@ private:
bool computePolytopeProjection(Cref_matrix6X v);
public:
+
+ /**
+ * @brief StaticEquilibrium 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 solver_type Type of LP solver to use.
+ */
StaticEquilibrium(std::string name, double mass, unsigned int generatorsPerContact, SolverLP solver_type);
+ /**
+ * @brief Get the name of this object.
+ * @return The name of this object.
+ */
std::string getName(){ return m_name; }
+ /**
+ * @brief Specify a new set of contacts.
+ * @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 frictionCoefficients List of N friction coefficients.
+ * @param alg Algorithm to use for testing equilibrium.
+ * @return True if the operation succeeded, false otherwise.
+ */
bool setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
Cref_vectorX frictionCoefficients, StaticEquilibriumAlgorithm alg);
+ /**
+ * @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 6x2 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 2d center of mass position to test.
+ * @param robustness The computed measure of robustness.
+ * @return True if the operation succeeded, false otherwise.
+ */
bool computeEquilibriumRobustness(Cref_vector2 com, double &robustness);
+ /**
+ * @brief Check whether the specified com position is in robust equilibrium.
+ * This amounts to solving the following feasibility LP:
+ * find b
+ * minimize 1
+ * 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 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 6x2 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 2d center of mass position to test.
+ * @param equilibrium True if com is in robust equilibrium, false otherwise.
+ * @param e_max Desired robustness level.
+ * @return True if the operation succeeded, false otherwise.
+ */
bool checkRobustEquilibrium(Cref_vector2 com, bool &equilibrium, double e_max=0.0);
- /** Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
- * This is equivalent to following the following LP:
+ /**
+ * @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
+ * where:
* b are the m coefficients of the contact force generators (f = G b)
- * b0 is the m-dimensional vector of identical values that are proportional to e_max
+ * 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 6x2 matrix mapping the CoM position in gravito-inertial wrench
@@ -91,6 +145,26 @@ public:
*/
bool findExtremumOverLine(Cref_vector2 a, Cref_vector2 a0, double e_max, Ref_vector2 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
+ * 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 2d com position
+ * G is the 6xm matrix whose columns are the gravito-inertial wrench generators
+ * D is the 6x2 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 2d direction.
+ * @param com Output 2d com position.
+ * @param e_max Desired robustness level.
+ * @return True if the operation succeeded, false otherwise.
+ */
bool findExtremumInDirection(Cref_vector2 direction, Ref_vector2 com, double e_max=0.0);
};
diff --git a/src/static_equilibrium.cpp b/src/static_equilibrium.cpp
index 6b986e31c918f1449737a633be37c3209e2ecfb1..383136e3b65450b17cd7cb68e080451a7f9b9078 100644
--- a/src/static_equilibrium.cpp
+++ b/src/static_equilibrium.cpp
@@ -69,6 +69,12 @@ bool StaticEquilibrium::setNewContacts(Cref_matrixX3 contactPoints, Cref_matrixX
int cg = m_generatorsPerContact;
long int m = c*cg; // number of generators
double muu;
+ /** Tangent directions for all contacts (numberOfContacts*generatorsPerContact X 3) */
+ MatrixX3 m_T1, m_T2;
+ /** Matrix mapping contact forces to gravito-inertial wrench (6 X 3*numberOfContacts) */
+ Matrix6X m_A;
+ /** Lists of contact generators (3 X numberOfContacts*generatorsPerContact) */
+ Matrix3X m_G;
m_T1.resize(c,3);
m_T2.resize(c,3);
m_A.resize(6,3*c);
diff --git a/test/test_static_equilibrium.cpp b/test/test_static_equilibrium.cpp
index e86bc6a686086467f5390f8f175957a128408961..4bf4602715e39299857640b81a85452d7563fddf 100644
--- a/test/test_static_equilibrium.cpp
+++ b/test/test_static_equilibrium.cpp
@@ -88,7 +88,7 @@ int test_computeEquilibriumRobustness_vs_checkEquilibrium(StaticEquilibrium solv
}
if(verb>1)
- SEND_INFO_MSG("Test test_computeEquilibriumRobustness_vs_checkEquilibrium "+solver_1.getName()+" VS "+solver_2.getName()+": "+toString(error_counter)+" error(s).");
+ cout<<"Test test_computeEquilibriumRobustness_vs_checkEquilibrium "+solver_1.getName()+" VS "+solver_2.getName()+": "+toString(error_counter)+" error(s).\n";
return error_counter;
}
@@ -141,7 +141,7 @@ int test_computeEquilibriumRobustness(StaticEquilibrium solver_1, StaticEquilibr
}
if(verb>0)
- SEND_INFO_MSG("Test computeEquilibriumRobustness "+solver_1.getName()+" VS "+solver_2.getName()+": "+toString(error_counter)+" error(s).");
+ cout<<"Test computeEquilibriumRobustness "+solver_1.getName()+" VS "+solver_2.getName()+": "+toString(error_counter)+" error(s).\n";
return error_counter;
}