hpp-centroidal-dynamics  4.14.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
24 };
26 
28  public:
29  const double m_mass;
31 
34 
35  private:
36  static bool m_is_cdd_initialized;
37 
39  std::string m_name;
40  EquilibriumAlgorithm m_algorithm;
41  SolverLP m_solver_type;
42  Solver_LP_abstract* m_solver;
43 
44  unsigned int m_generatorsPerContact;
45 
49  MatrixXX m_H;
50  VectorX m_h;
52  bool m_is_cdd_stable;
56  const unsigned max_num_cdd_trials;
59  const bool canonicalize_cdd_matrix;
60 
63  MatrixX3 m_HD;
64  VectorX m_Hd;
65 
67  Matrix63 m_D;
68  Vector6 m_d;
69 
71  double m_b0_to_emax_coefficient;
72 
73  bool computePolytopeProjection(Cref_matrix6X v);
74  bool computeGenerators(Cref_matrixX3 contactPoints,
75  Cref_matrixX3 contactNormals,
76  double frictionCoefficient,
77  const bool perturbate = false);
78 
87  double convert_b0_to_emax(double b0);
88 
89  double convert_emax_to_b0(double emax);
90 
91  public:
92  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
107  Equilibrium(const std::string& name, const double mass,
108  const unsigned int generatorsPerContact,
109  const SolverLP solver_type = SOLVER_LP_QPOASES,
110  bool useWarmStart = true,
111  const unsigned int max_num_cdd_trials = 0,
112  const bool canonicalize_cdd_matrix = false);
113 
114  Equilibrium(const Equilibrium& other);
115 
116  ~Equilibrium();
117 
123  bool useWarmStart() { return m_solver->getUseWarmStart(); }
124 
130  void setUseWarmStart(bool uws) { m_solver->setUseWarmStart(uws); }
131 
136  std::string getName() { return m_name; }
137 
138  EquilibriumAlgorithm getAlgorithm() { return m_algorithm; }
139 
140  void setAlgorithm(EquilibriumAlgorithm algorithm);
141 
154  bool setNewContacts(const MatrixX3& contactPoints,
155  const MatrixX3& contactNormals,
156  const double frictionCoefficient,
157  const EquilibriumAlgorithm alg);
158 
172  bool setNewContacts(const MatrixX3ColMajor& contactPoints,
173  const MatrixX3ColMajor& contactNormals,
174  const double frictionCoefficient,
175  const EquilibriumAlgorithm alg);
176 
177  void setG(Cref_matrix6X G) { m_G_centr = G; }
178 
197  LP_status computeEquilibriumRobustness(Cref_vector3 com, double& robustness);
198 
218  LP_status computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc,
219  double& robustness);
220 
240  LP_status checkRobustEquilibrium(Cref_vector3 com, bool& equilibrium,
241  double e_max = 0.0);
242 
263  LP_status checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc,
264  bool& equilibrium, double e_max = 0.0);
265 
289  LP_status findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max,
290  Ref_vector3 com);
291 
315  LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com,
316  double e_max = 0.0);
317 
331  LP_status getPolytopeInequalities(MatrixXX& H, VectorX& h) const;
332 
350  LP_status findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,
351  Cref_vector3 v, double& alpha0);
352 
364  bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h,
365  Cref_vector3 a);
366 };
367 
368 } // end namespace centroidal_dynamics
369 
370 #endif
Eigen::Ref< Vector3 > Ref_vector3
Definition: util.hh:52
EQUILIBRIUM_ALGORITHM_LP
Definition: centroidal_dynamics.hh:17
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > VectorX
Definition: util.hh:35
Definition: centroidal_dynamics.hh:14
EQUILIBRIUM_ALGORITHM_LP2
primal LP formulation
Definition: centroidal_dynamics.hh:17
Eigen::Matrix< value_type, 6, 1 > Vector6
Definition: util.hh:34
const Eigen::Ref< const Vector3 > & Cref_vector3
Definition: util.hh:61
std::string getName()
Get the name of this object.
Definition: centroidal_dynamics.hh:136
Eigen::Matrix< value_type, 6, 3, Eigen::RowMajor > Matrix63
Definition: util.hh:45
Eigen::Matrix< value_type, Eigen::Dynamic, 3, Eigen::RowMajor > MatrixX3
Definition: util.hh:40
const Vector3 m_gravity
mass of the system
Definition: centroidal_dynamics.hh:30
EQUILIBRIUM_ALGORITHM_PP
dual LP formulation
Definition: centroidal_dynamics.hh:17
EQUILIBRIUM_ALGORITHM_IP
polytope projection algorithm
Definition: centroidal_dynamics.hh:17
bool useWarmStart()
Returns the useWarmStart flag.
Definition: centroidal_dynamics.hh:123
const Eigen::Ref< const Matrix63 > & Cref_matrix63
Definition: util.hh:68
void setUseWarmStart(bool uws)
Specifies whether the LP solver is allowed to use warm start.
Definition: centroidal_dynamics.hh:130
#define CENTROIDAL_DYNAMICS_DLLAPI
Definition: local_config.hh:52
Matrix6X m_G_centr
gravity vector
Definition: centroidal_dynamics.hh:33
Abstract interface for a Linear Program (LP) solver.
Definition: solver_LP_abstract.hh:41
const double m_mass
Definition: centroidal_dynamics.hh:29
EQUILIBRIUM_ALGORITHM_DLP
another primal LP formulation
Definition: centroidal_dynamics.hh:17
EquilibriumAlgorithm getAlgorithm()
Definition: centroidal_dynamics.hh:138
void setG(Cref_matrix6X G)
Definition: centroidal_dynamics.hh:177
Eigen::Matrix< value_type, 6, Eigen::Dynamic, Eigen::RowMajor > Matrix6X
Definition: util.hh:43
const Eigen::Ref< const MatrixX3 > & Cref_matrixX3
Definition: util.hh:65
const Eigen::Ref< const Vector6 > & Cref_vector6
Definition: util.hh:62
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixXX
Definition: util.hh:49
const Eigen::Ref< const Matrix6X > & Cref_matrix6X
Definition: util.hh:67
virtual bool getUseWarmStart()
Definition: solver_LP_abstract.hh:126
Eigen::Matrix< value_type, Eigen::Dynamic, 3, Eigen::ColMajor > MatrixX3ColMajor
Definition: util.hh:73
Eigen::Matrix< value_type, 3, 1 > Vector3
Definition: util.hh:32
virtual void setUseWarmStart(bool useWarmStart)
Definition: solver_LP_abstract.hh:128
Definition: centroidal_dynamics.hh:27