hpp-centroidal-dynamics: centroidal_dynamics::Equilibrium Class Reference

hpp-centroidal-dynamics 9.0.2 Utility classes for testing (robust) equilibrium of a system in contact with the environment, and other centroidal dynamics methods.

Loading...
Searching...
No Matches
Public Member Functions | Public Attributes | List of all members
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.
| 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

## 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

  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

## ◆ Equilibrium() [2/2]

centroidal_dynamics::Equilibrium::Equilibrium | ( | const Equilibrium & | other | ) |

## ◆ ~Equilibrium()

centroidal_dynamics::Equilibrium::~Equilibrium | ( |  | ) |

## 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

  a | the 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

  com | The 3d center of mass position to test.
  equilibrium | True if com is in robust equilibrium, false otherwise.
  e_max | Desired 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

  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.

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

  com | The 3d center of mass position to test.
  acc | The 3d acceleration of the CoM.
  robustness | The 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

  com | The 3d center of mass position to test.
  robustness | The 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

  direction | Desired 3d direction.
  com | Output 3d com position.
  e_max | Desired 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

  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

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

  H | input
  h | input
  v | input
  alpha0 | output

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

  H | reference to the H matrix to initialize
  h | reference 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

  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.

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

  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.

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

  uws | True 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:

- include/hpp/centroidal-dynamics/centroidal_dynamics.hh
- src/centroidal_dynamics.cpp