|
hpp-centroidal-dynamics 6.0.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. | |
| Equilibrium (const Equilibrium &other) | |
| ~Equilibrium () | |
| bool | useWarmStart () |
| Returns the useWarmStart flag. | |
| void | setUseWarmStart (bool uws) |
| Specifies whether the LP solver is allowed to use warm start. | |
| std::string | getName () |
| Get the name of this object. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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. | |
| 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 | |
| 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 | |
Public Attributes | |
| const double | m_mass |
| const Vector3 | m_gravity |
| mass of the system | |
| Matrix6X | m_G_centr |
| gravity vector | |
| 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 | ) |
| centroidal_dynamics::Equilibrium::~Equilibrium | ( | ) |
| 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 |