Improve documentation.

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);
 
 };

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);

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;
 }