hpp-centroidal-dynamics  4.10.0
Utility classes for testing (robust) equilibrium of a system in contact with the environment, and other centroidal dynamics methods.
centroidal_dynamics::Equilibrium Class Reference

#include <hpp/centroidal-dynamics/centroidal_dynamics.hh>

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW 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 constructor. More...
 
 Equilibrium (const Equilibrium &other)
 
bool useWarmStart ()
 Returns the useWarmStart flag. More...
 
void setUseWarmStart (bool uws)
 Specifies whether the LP solver is allowed to use warm start. More...
 
std::string getName ()
 Get the name of this object. More...
 
EquilibriumAlgorithm getAlgorithm ()
 
void setAlgorithm (EquilibriumAlgorithm algorithm)
 
bool setNewContacts (const MatrixX3 &contactPoints, const MatrixX3 &contactNormals, const double frictionCoefficient, const EquilibriumAlgorithm alg)
 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. More...
 
bool setNewContacts (const MatrixX3ColMajor &contactPoints, const MatrixX3ColMajor &contactNormals, const double frictionCoefficient, const EquilibriumAlgorithm alg)
 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. More...
 
void setG (Cref_matrix6X G)
 
LP_status computeEquilibriumRobustness (Cref_vector3 com, double &robustness)
 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. More...
 
LP_status computeEquilibriumRobustness (Cref_vector3 com, Cref_vector3 acc, double &robustness)
 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. More...
 
LP_status checkRobustEquilibrium (Cref_vector3 com, bool &equilibrium, double e_max=0.0)
 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 6x3 matrix mapping the CoM position in gravito-inertial wrench d is the 6d vector containing the gravity part of the gravito-inertial wrench. More...
 
LP_status checkRobustEquilibrium (Cref_vector3 com, Cref_vector3 acc, bool &equilibrium, double e_max=0.0)
 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 6x3 matrix mapping the CoM position in gravito-inertial wrench d is the 6d vector containing the gravity part of the gravito-inertial wrench. More...
 
LP_status findExtremumOverLine (Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com)
 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. More...
 
LP_status findExtremumInDirection (Cref_vector3 direction, Ref_vector3 com, double e_max=0.0)
 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. More...
 
LP_status getPolytopeInequalities (MatrixXX &H, VectorX &h) const
 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. More...
 
LP_status findMaximumAcceleration (Cref_matrix63 H, Cref_vector6 h, Cref_vector3 v, double &alpha0)
 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 More...
 
bool checkAdmissibleAcceleration (Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a)
 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 More...
 

Public Attributes

const double m_mass
 
const Vector3 m_gravity
 mass of the system More...
 
Matrix6X m_G_centr
 gravity vector More...
 

Constructor & Destructor Documentation

◆ Equilibrium() [1/2]

EIGEN_MAKE_ALIGNED_OPERATOR_NEW centroidal_dynamics::Equilibrium::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 constructor.

Parameters
nameName of the object.
massMass of the system for which to test equilibrium.
generatorsPerContactNumber of generators used to approximate the friction cone per contact point.
solver_typeType of LP solver to use.
useWarmStartWhether the LP solver can warm start the resolution.
max_num_cdd_trialsindicate the max number of attempts to compute the cone by introducing
canonicalize_cdd_matrixwhether to remove redundant inequalities when computing double description matrices a small pertubation of the system

◆ Equilibrium() [2/2]

centroidal_dynamics::Equilibrium::Equilibrium ( const Equilibrium other)

Member Function Documentation

◆ checkAdmissibleAcceleration()

bool centroidal_dynamics::Equilibrium::checkAdmissibleAcceleration ( Cref_matrix63  H,
Cref_vector6  h,
Cref_vector3  a 
)

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

Parameters
athe acceleration
Returns
true if the acceleration is admissible, false otherwise

◆ checkRobustEquilibrium() [1/2]

LP_status centroidal_dynamics::Equilibrium::checkRobustEquilibrium ( Cref_vector3  com,
bool &  equilibrium,
double  e_max = 0.0 
)

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 6x3 matrix mapping the CoM position in gravito-inertial wrench d is the 6d vector containing the gravity part of the gravito-inertial wrench.

Parameters
comThe 3d center of mass position to test.
equilibriumTrue if com is in robust equilibrium, false otherwise.
e_maxDesired robustness level.
Returns
The status of the LP solver.

◆ checkRobustEquilibrium() [2/2]

LP_status centroidal_dynamics::Equilibrium::checkRobustEquilibrium ( Cref_vector3  com,
Cref_vector3  acc,
bool &  equilibrium,
double  e_max = 0.0 
)

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 6x3 matrix mapping the CoM position in gravito-inertial wrench d is the 6d vector containing the gravity part of the gravito-inertial wrench.

Parameters
comThe 3d center of mass position to test.
accThe 3d acceleration of the CoM.
equilibriumTrue if com is in robust equilibrium, false otherwise.
e_maxDesired robustness level.
Returns
The status of the LP solver.

◆ computeEquilibriumRobustness() [1/2]

LP_status centroidal_dynamics::Equilibrium::computeEquilibriumRobustness ( Cref_vector3  com,
Cref_vector3  acc,
double &  robustness 
)

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.

Parameters
comThe 3d center of mass position to test.
accThe 3d acceleration of the CoM.
robustnessThe computed measure of robustness.
Returns
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.

◆ computeEquilibriumRobustness() [2/2]

LP_status centroidal_dynamics::Equilibrium::computeEquilibriumRobustness ( Cref_vector3  com,
double &  robustness 
)

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.

Parameters
comThe 3d center of mass position to test.
robustnessThe computed measure of robustness.
Returns
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.

◆ findExtremumInDirection()

LP_status centroidal_dynamics::Equilibrium::findExtremumInDirection ( Cref_vector3  direction,
Ref_vector3  com,
double  e_max = 0.0 
)

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.

Parameters
directionDesired 3d direction.
comOutput 3d com position.
e_maxDesired robustness level.
Returns
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.

◆ findExtremumOverLine()

LP_status centroidal_dynamics::Equilibrium::findExtremumOverLine ( Cref_vector3  a,
Cref_vector3  a0,
double  e_max,
Ref_vector3  com 
)

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.

Parameters
a2d vector representing the line direction
a02d vector representing an arbitrary point over the line
e_maxDesired robustness in terms of the maximum force error tolerated by the system
Returns
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.

◆ findMaximumAcceleration()

LP_status centroidal_dynamics::Equilibrium::findMaximumAcceleration ( Cref_matrix63  H,
Cref_vector6  h,
Cref_vector3  v,
double &  alpha0 
)

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
Parameters
Hinput
hinput
vinput
alpha0output
Returns
The status of the LP solver.

◆ getAlgorithm()

EquilibriumAlgorithm centroidal_dynamics::Equilibrium::getAlgorithm ( )
inline

◆ getName()

std::string centroidal_dynamics::Equilibrium::getName ( )
inline

Get the name of this object.

Returns
The name of this object.

◆ getPolytopeInequalities()

LP_status centroidal_dynamics::Equilibrium::getPolytopeInequalities ( MatrixXX H,
VectorX h 
) const

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.

Parameters
Hreference to the H matrix to initialize
hreference to the h vector to initialize
Returns
The status of the inequalities. If the inequalities are not defined due to numerical instabilities, will send appropriate error message, and return LP_STATUS_ERROR. If they are not defined because no contact has been defined, will return LP_STATUS_INFEASIBLE

◆ setAlgorithm()

void centroidal_dynamics::Equilibrium::setAlgorithm ( EquilibriumAlgorithm  algorithm)

◆ setG()

void centroidal_dynamics::Equilibrium::setG ( Cref_matrix6X  G)
inline

◆ setNewContacts() [1/2]

bool centroidal_dynamics::Equilibrium::setNewContacts ( const MatrixX3 contactPoints,
const MatrixX3 contactNormals,
const double  frictionCoefficient,
const EquilibriumAlgorithm  alg 
)

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.

Parameters
contactPointsList of N 3d contact points as an Nx3 matrix.
contactNormalsList of N 3d contact normal directions as an Nx3 matrix.
frictionCoefficientThe contact friction coefficient.
algAlgorithm to use for testing equilibrium.
Returns
True if the operation succeeded, false otherwise.

◆ setNewContacts() [2/2]

bool centroidal_dynamics::Equilibrium::setNewContacts ( const MatrixX3ColMajor contactPoints,
const MatrixX3ColMajor contactNormals,
const double  frictionCoefficient,
const EquilibriumAlgorithm  alg 
)

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.

Parameters
contactPointsList of N 3d contact points as an Nx3 matrix.
contactNormalsList of N 3d contact normal directions as an Nx3 matrix.
frictionCoefficientThe contact friction coefficient.
algAlgorithm to use for testing equilibrium.
Returns
True if the operation succeeded, false otherwise.

◆ setUseWarmStart()

void centroidal_dynamics::Equilibrium::setUseWarmStart ( bool  uws)
inline

Specifies whether the LP solver is allowed to use warm start.

Parameters
uwsTrue if the LP solver is allowed to use warm start, false otherwise.

◆ useWarmStart()

bool centroidal_dynamics::Equilibrium::useWarmStart ( )
inline

Returns the useWarmStart flag.

Returns
True if the LP solver is allowed to use warm start, false otherwise.

Member Data Documentation

◆ m_G_centr

Matrix6X centroidal_dynamics::Equilibrium::m_G_centr

gravity vector

Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact)

◆ m_gravity

const Vector3 centroidal_dynamics::Equilibrium::m_gravity

mass of the system

◆ m_mass

const double centroidal_dynamics::Equilibrium::m_mass

The documentation for this class was generated from the following files: