hpp-centroidal-dynamics
4.9.0
Utility classes for testing (robust) equilibrium of a system in contact with the environment, and other centroidal dynamics methods.
|
#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... | |
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.
name | Name of the object. |
mass | Mass of the system for which to test equilibrium. |
generatorsPerContact | Number of generators used to approximate the friction cone per contact point. |
solver_type | Type of LP solver to use. |
useWarmStart | Whether the LP solver can warm start the resolution. |
max_num_cdd_trials | indicate the max number of attempts to compute the cone by introducing |
canonicalize_cdd_matrix | whether to remove redundant inequalities when computing double description matrices a small pertubation of the system |
centroidal_dynamics::Equilibrium::Equilibrium | ( | const Equilibrium & | other | ) |
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
a | the acceleration |
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.
com | The 3d center of mass position to test. |
equilibrium | True if com is in robust equilibrium, false otherwise. |
e_max | Desired robustness level. |
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.
com | The 3d center of mass position to test. |
acc | The 3d acceleration of the CoM. |
equilibrium | True if com is in robust equilibrium, false otherwise. |
e_max | Desired robustness level. |
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.
com | The 3d center of mass position to test. |
acc | The 3d acceleration of the CoM. |
robustness | The computed measure of robustness. |
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.
com | The 3d center of mass position to test. |
robustness | The computed measure of robustness. |
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.
direction | Desired 3d direction. |
com | Output 3d com position. |
e_max | Desired robustness level. |
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.
a | 2d vector representing the line direction |
a0 | 2d vector representing an arbitrary point over the line |
e_max | Desired robustness in terms of the maximum force error tolerated by the system |
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
H | input |
h | input |
v | input |
alpha0 | output |
|
inline |
|
inline |
Get the name of this object.
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.
H | reference to the H matrix to initialize |
h | reference to the h vector to initialize |
void centroidal_dynamics::Equilibrium::setAlgorithm | ( | EquilibriumAlgorithm | algorithm | ) |
|
inline |
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.
contactPoints | List of N 3d contact points as an Nx3 matrix. |
contactNormals | List of N 3d contact normal directions as an Nx3 matrix. |
frictionCoefficient | The contact friction coefficient. |
alg | Algorithm to use for testing equilibrium. |
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.
contactPoints | List of N 3d contact points as an Nx3 matrix. |
contactNormals | List of N 3d contact normal directions as an Nx3 matrix. |
frictionCoefficient | The contact friction coefficient. |
alg | Algorithm to use for testing equilibrium. |
|
inline |
Specifies whether the LP solver is allowed to use warm start.
uws | True if the LP solver is allowed to use warm start, false otherwise. |
|
inline |
Returns the useWarmStart flag.
Matrix6X centroidal_dynamics::Equilibrium::m_G_centr |
gravity vector
Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact)
const Vector3 centroidal_dynamics::Equilibrium::m_gravity |
mass of the system
const double centroidal_dynamics::Equilibrium::m_mass |