Skip to content
Snippets Groups Projects
Commit aff68b51 authored by andreadelprete's avatar andreadelprete
Browse files

Improve documentation.

parent 5571f154
No related branches found
No related tags found
No related merge requests found
......@@ -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);
};
......
......@@ -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);
......
......@@ -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;
}
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment