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.
centroidal_dynamics.hh
Go to the documentation of this file.
1 /*
2  * Copyright 2015, LAAS-CNRS
3  * Author: Andrea Del Prete
4  */
5 
6 #ifndef HPP_CENTROIDAL_DYNAMICS_CENTROIDAL_DYNAMICS_HH
7 #define HPP_CENTROIDAL_DYNAMICS_CENTROIDAL_DYNAMICS_HH
8 
9 #include <Eigen/Dense>
13 
15 
16 enum CENTROIDAL_DYNAMICS_DLLAPI EquilibriumAlgorithm {
22  EQUILIBRIUM_ALGORITHM_DIP
23 };
24 
26  public:
27  const double m_mass;
29 
31 
32  private:
33  static bool m_is_cdd_initialized;
34 
35  std::string m_name;
36  EquilibriumAlgorithm m_algorithm;
37  SolverLP m_solver_type;
38  Solver_LP_abstract* m_solver;
39 
40  unsigned int m_generatorsPerContact;
41 
43  MatrixXX m_H;
44  VectorX m_h;
46  bool m_is_cdd_stable;
50  const unsigned max_num_cdd_trials;
52  const bool canonicalize_cdd_matrix;
53 
55  MatrixX3 m_HD;
56  VectorX m_Hd;
57 
59  Matrix63 m_D;
60  Vector6 m_d;
61 
63  double m_b0_to_emax_coefficient;
64 
65  bool computePolytopeProjection(Cref_matrix6X v);
66  bool computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals, double frictionCoefficient,
67  const bool perturbate = false);
68 
77  double convert_b0_to_emax(double b0);
78 
79  double convert_emax_to_b0(double emax);
80 
81  public:
82  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
94  Equilibrium(const std::string& name, const double mass, const unsigned int generatorsPerContact,
95  const SolverLP solver_type = SOLVER_LP_QPOASES, bool useWarmStart = true,
96  const unsigned int max_num_cdd_trials = 0, const bool canonicalize_cdd_matrix = false);
97 
98  Equilibrium(const Equilibrium& other);
99 
104  bool useWarmStart() { return m_solver->getUseWarmStart(); }
105 
110  void setUseWarmStart(bool uws) { m_solver->setUseWarmStart(uws); }
111 
116  std::string getName() { return m_name; }
117 
118  EquilibriumAlgorithm getAlgorithm() { return m_algorithm; }
119 
120  void setAlgorithm(EquilibriumAlgorithm algorithm);
121 
133  bool setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals, const double frictionCoefficient,
134  const EquilibriumAlgorithm alg);
135 
147  bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor& contactNormals,
148  const double frictionCoefficient, const EquilibriumAlgorithm alg);
149 
150  void setG(Cref_matrix6X G) { m_G_centr = G; }
151 
173  LP_status computeEquilibriumRobustness(Cref_vector3 com, double& robustness);
174 
197  LP_status computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double& robustness);
198 
218  LP_status checkRobustEquilibrium(Cref_vector3 com, bool& equilibrium, double e_max = 0.0);
219 
240  LP_status checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc, bool& equilibrium, double e_max = 0.0);
241 
264  LP_status findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com);
265 
289  LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max = 0.0);
290 
303  LP_status getPolytopeInequalities(MatrixXX& H, VectorX& h) const;
304 
326  LP_status findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 v, double& alpha0);
327 
341  bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a);
342 };
343 
344 } // end namespace centroidal_dynamics
345 
346 #endif
solver_LP_abstract.hh
util.hh
centroidal_dynamics::Cref_vector6
const typedef Eigen::Ref< const Vector6 > & Cref_vector6
Definition: util.hh:60
EQUILIBRIUM_ALGORITHM_IP
EQUILIBRIUM_ALGORITHM_IP
polytope projection algorithm
Definition: centroidal_dynamics.hh:21
centroidal_dynamics::Equilibrium::m_gravity
const Vector3 m_gravity
mass of the system
Definition: centroidal_dynamics.hh:28
centroidal_dynamics::Equilibrium::getName
std::string getName()
Get the name of this object.
Definition: centroidal_dynamics.hh:116
EQUILIBRIUM_ALGORITHM_DLP
EQUILIBRIUM_ALGORITHM_DLP
another primal LP formulation
Definition: centroidal_dynamics.hh:19
centroidal_dynamics::Vector3
Eigen::Matrix< value_type, 3, 1 > Vector3
Definition: util.hh:32
centroidal_dynamics::Solver_LP_abstract::getUseWarmStart
virtual bool getUseWarmStart()
Definition: solver_LP_abstract.hh:122
centroidal_dynamics::Cref_matrixX3
const typedef Eigen::Ref< const MatrixX3 > & Cref_matrixX3
Definition: util.hh:63
centroidal_dynamics::Matrix6X
Eigen::Matrix< value_type, 6, Eigen::Dynamic, Eigen::RowMajor > Matrix6X
Definition: util.hh:43
EQUILIBRIUM_ALGORITHM_LP
EQUILIBRIUM_ALGORITHM_LP
Definition: centroidal_dynamics.hh:17
centroidal_dynamics::Equilibrium::m_G_centr
Matrix6X m_G_centr
gravity vector
Definition: centroidal_dynamics.hh:30
centroidal_dynamics::Ref_vector3
Eigen::Ref< Vector3 > Ref_vector3
Definition: util.hh:50
centroidal_dynamics::MatrixXX
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixXX
Definition: util.hh:47
centroidal_dynamics::Solver_LP_abstract::setUseWarmStart
virtual void setUseWarmStart(bool useWarmStart)
Definition: solver_LP_abstract.hh:124
centroidal_dynamics::Equilibrium::m_mass
const double m_mass
Definition: centroidal_dynamics.hh:27
CENTROIDAL_DYNAMICS_DLLAPI
#define CENTROIDAL_DYNAMICS_DLLAPI
Definition: local_config.hh:52
centroidal_dynamics::Equilibrium::useWarmStart
bool useWarmStart()
Returns the useWarmStart flag.
Definition: centroidal_dynamics.hh:104
centroidal_dynamics::Cref_vector3
const typedef Eigen::Ref< const Vector3 > & Cref_vector3
Definition: util.hh:59
centroidal_dynamics::MatrixX3
Eigen::Matrix< value_type, Eigen::Dynamic, 3, Eigen::RowMajor > MatrixX3
Definition: util.hh:40
EQUILIBRIUM_ALGORITHM_LP2
EQUILIBRIUM_ALGORITHM_LP2
primal LP formulation
Definition: centroidal_dynamics.hh:18
centroidal_dynamics::Vector6
Eigen::Matrix< value_type, 6, 1 > Vector6
Definition: util.hh:34
local_config.hh
centroidal_dynamics::Equilibrium::getAlgorithm
EquilibriumAlgorithm getAlgorithm()
Definition: centroidal_dynamics.hh:118
centroidal_dynamics::Equilibrium::setUseWarmStart
void setUseWarmStart(bool uws)
Specifies whether the LP solver is allowed to use warm start.
Definition: centroidal_dynamics.hh:110
centroidal_dynamics::Cref_matrix6X
const typedef Eigen::Ref< const Matrix6X > & Cref_matrix6X
Definition: util.hh:65
centroidal_dynamics
Definition: centroidal_dynamics.hh:14
centroidal_dynamics::Equilibrium::setG
void setG(Cref_matrix6X G)
Definition: centroidal_dynamics.hh:150
centroidal_dynamics::VectorX
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > VectorX
Definition: util.hh:35
centroidal_dynamics::Equilibrium
Definition: centroidal_dynamics.hh:25
EQUILIBRIUM_ALGORITHM_PP
EQUILIBRIUM_ALGORITHM_PP
dual LP formulation
Definition: centroidal_dynamics.hh:20
centroidal_dynamics::MatrixX3ColMajor
Eigen::Matrix< value_type, Eigen::Dynamic, 3, Eigen::ColMajor > MatrixX3ColMajor
Definition: util.hh:70
centroidal_dynamics::Matrix63
Eigen::Matrix< value_type, 6, 3, Eigen::RowMajor > Matrix63
Definition: util.hh:45
centroidal_dynamics::Cref_matrix63
const typedef Eigen::Ref< const Matrix63 > & Cref_matrix63
Definition: util.hh:66
centroidal_dynamics::Solver_LP_abstract
Abstract interface for a Linear Program (LP) solver.
Definition: solver_LP_abstract.hh:41