hpp-centroidal-dynamics: include/hpp/centroidal-dynamics/centroidal_dynamics.hh Source File

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
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>
10#include <hpp/centroidal-dynamics/local_config.hh>
11#include <hpp/centroidal-dynamics/solver_LP_abstract.hh>
12#include <hpp/centroidal-dynamics/util.hh>
13
14 namespace centroidal_dynamics {
15
16enum CENTROIDAL_DYNAMICS_DLLAPI EquilibriumAlgorithm {
17 EQUILIBRIUM_ALGORITHM_LP,
18 EQUILIBRIUM_ALGORITHM_LP2,
19 EQUILIBRIUM_ALGORITHM_DLP,
20 EQUILIBRIUM_ALGORITHM_PP,
21 EQUILIBRIUM_ALGORITHM_IP,
23 EQUILIBRIUM_ALGORITHM_DIP
25};
26
27 class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
28 public:
29 const double m_mass;
30 const Vector3 m_gravity;
33 Matrix6X m_G_centr;
34
35 private:
36 static bool m_is_cdd_initialized;
38
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;
46
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
EQUILIBRIUM_ALGORITHM_PP
EQUILIBRIUM_ALGORITHM_PP
dual LP formulation
Definition centroidal_dynamics.hh:20
EQUILIBRIUM_ALGORITHM_LP2
EQUILIBRIUM_ALGORITHM_LP2
primal LP formulation
Definition centroidal_dynamics.hh:18
EQUILIBRIUM_ALGORITHM_LP
EQUILIBRIUM_ALGORITHM_LP
Definition centroidal_dynamics.hh:17
EQUILIBRIUM_ALGORITHM_IP
EQUILIBRIUM_ALGORITHM_IP
polytope projection algorithm
Definition centroidal_dynamics.hh:21
EQUILIBRIUM_ALGORITHM_DLP
EQUILIBRIUM_ALGORITHM_DLP
another primal LP formulation
Definition centroidal_dynamics.hh:19
centroidal_dynamics::Equilibrium
Definition centroidal_dynamics.hh:27
centroidal_dynamics::Equilibrium::setG
void setG(Cref_matrix6X G)
Definition centroidal_dynamics.hh:177
centroidal_dynamics::Equilibrium::getAlgorithm
EquilibriumAlgorithm getAlgorithm()
Definition centroidal_dynamics.hh:138
centroidal_dynamics::Equilibrium::getName
std::string getName()
Get the name of this object.
Definition centroidal_dynamics.hh:136
centroidal_dynamics::Equilibrium::setUseWarmStart
void setUseWarmStart(bool uws)
Specifies whether the LP solver is allowed to use warm start.
Definition centroidal_dynamics.hh:130
centroidal_dynamics::Equilibrium::m_G_centr
Matrix6X m_G_centr
gravity vector
Definition centroidal_dynamics.hh:33
centroidal_dynamics::Equilibrium::m_mass
const double m_mass
Definition centroidal_dynamics.hh:29
centroidal_dynamics::Equilibrium::Equilibrium
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.
centroidal_dynamics::Equilibrium::useWarmStart
bool useWarmStart()
Returns the useWarmStart flag.
Definition centroidal_dynamics.hh:123
centroidal_dynamics::Equilibrium::m_gravity
const Vector3 m_gravity
mass of the system
Definition centroidal_dynamics.hh:30
centroidal_dynamics::Solver_LP_abstract
Abstract interface for a Linear Program (LP) solver.
Definition solver_LP_abstract.hh:38
centroidal_dynamics::Solver_LP_abstract::setUseWarmStart
virtual void setUseWarmStart(bool useWarmStart)
Definition solver_LP_abstract.hh:125
centroidal_dynamics::Solver_LP_abstract::getUseWarmStart
virtual bool getUseWarmStart()
Definition solver_LP_abstract.hh:123
local_config.hh
CENTROIDAL_DYNAMICS_DLLAPI
#define CENTROIDAL_DYNAMICS_DLLAPI
Definition local_config.hh:52
centroidal_dynamics
Definition centroidal_dynamics.hh:14
centroidal_dynamics::Cref_matrix63
const Eigen::Ref< const Matrix63 > & Cref_matrix63
Definition util.hh:68
centroidal_dynamics::Vector3
Eigen::Matrix< value_type, 3, 1 > Vector3
Definition util.hh:32
centroidal_dynamics::Vector6
Eigen::Matrix< value_type, 6, 1 > Vector6
Definition util.hh:34
centroidal_dynamics::Cref_vector6
const Eigen::Ref< const Vector6 > & Cref_vector6
Definition util.hh:62
centroidal_dynamics::MatrixX3ColMajor
Eigen::Matrix< value_type, Eigen::Dynamic, 3, Eigen::ColMajor > MatrixX3ColMajor
Definition util.hh:73
centroidal_dynamics::Cref_matrixX3
const Eigen::Ref< const MatrixX3 > & Cref_matrixX3
Definition util.hh:65
centroidal_dynamics::VectorX
Eigen::Matrix< value_type, Eigen::Dynamic, 1 > VectorX
Definition util.hh:35
centroidal_dynamics::Cref_vector3
const Eigen::Ref< const Vector3 > & Cref_vector3
Definition util.hh:61
centroidal_dynamics::Cref_matrix6X
const Eigen::Ref< const Matrix6X > & Cref_matrix6X
Definition util.hh:67
centroidal_dynamics::Matrix63
Eigen::Matrix< value_type, 6, 3, Eigen::RowMajor > Matrix63
Definition util.hh:45
centroidal_dynamics::Matrix6X
Eigen::Matrix< value_type, 6, Eigen::Dynamic, Eigen::RowMajor > Matrix6X
Definition util.hh:43
centroidal_dynamics::MatrixXX
Eigen::Matrix< value_type, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor > MatrixXX
Definition util.hh:49
centroidal_dynamics::MatrixX3
Eigen::Matrix< value_type, Eigen::Dynamic, 3, Eigen::RowMajor > MatrixX3
Definition util.hh:40
centroidal_dynamics::Ref_vector3
Eigen::Ref< Vector3 > Ref_vector3
Definition util.hh:52
solver_LP_abstract.hh
SOLVER_LP_QPOASES
SOLVER_LP_QPOASES
Definition solver_LP_abstract.hh:19
util.hh