GCC Code Coverage Report

Directory: ./
File: include/hpp/centroidal-dynamics/centroidal_dynamics.hh
Date: 2025-03-17 04:04:52
Exec Total Coverage
Lines: 4 4 100.0%
Branches: 0 0 -%
Line Branch Exec Source
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
16 enum CENTROIDAL_DYNAMICS_DLLAPI EquilibriumAlgorithm {
17 EQUILIBRIUM_ALGORITHM_LP, /// primal LP formulation
18 EQUILIBRIUM_ALGORITHM_LP2, /// another primal LP formulation
19 EQUILIBRIUM_ALGORITHM_DLP, /// dual LP formulation
20 EQUILIBRIUM_ALGORITHM_PP, /// polytope projection algorithm
21 EQUILIBRIUM_ALGORITHM_IP, /// incremental projection algorithm based on
22 /// primal LP formulation
23 EQUILIBRIUM_ALGORITHM_DIP /// incremental projection algorithm based on dual
24 /// LP formulation
25 };
26
27 class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium {
28 public:
29 const double m_mass; /// mass of the system
30 const Vector3 m_gravity; /// gravity vector
31 /** Gravito-inertial wrench generators (6 X
32 * numberOfContacts*generatorsPerContact) */
33 Matrix6X m_G_centr;
34
35 private:
36 static bool m_is_cdd_initialized; /// true if cdd lib has been initialized,
37 /// false otherwise
38
39 std::string m_name; /// name of this object
40 EquilibriumAlgorithm m_algorithm; /// current algorithm used
41 SolverLP m_solver_type; /// type of LP solver
42 Solver_LP_abstract* m_solver; /// LP solver
43
44 unsigned int m_generatorsPerContact; /// number of generators to approximate
45 /// the friction cone per contact point
46
47 /** Inequality matrix and vector defining the gravito-inertial wrench cone H w
48 * <= h */
49 MatrixXX m_H;
50 VectorX m_h;
51 /** False if a numerical instability appeared in the computation H and h*/
52 bool m_is_cdd_stable;
53 /** EQUILIBRIUM_ALGORITHM_PP: If double description fails,
54 * indicate the max number of attempts to compute the cone by introducing
55 * a small pertubation of the system */
56 const unsigned max_num_cdd_trials;
57 /** whether to remove redundant inequalities when computing double description
58 * matrices*/
59 const bool canonicalize_cdd_matrix;
60
61 /** Inequality matrix and vector defining the CoM support polygon HD com + Hd
62 * <= h */
63 MatrixX3 m_HD;
64 VectorX m_Hd;
65
66 /** Matrix and vector mapping 2d com position to GIW */
67 Matrix63 m_D;
68 Vector6 m_d;
69
70 /** Coefficient used for converting the robustness measure in Newtons */
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
79 /**
80 * @brief Given the smallest coefficient of the contact force generators it
81 * computes the minimum norm of force error necessary to have a contact force
82 * on the associated friction cone boundaries.
83 * @param b0 Minimum coefficient of the contact force generators.
84 * @return Minimum norm of the force error necessary to result in a contact
85 * force being on the friction cone boundaries.
86 */
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
93 /**
94 * @brief Equilibrium constructor.
95 * @param name Name of the object.
96 * @param mass Mass of the system for which to test equilibrium.
97 * @param generatorsPerContact Number of generators used to approximate the
98 * friction cone per contact point.
99 * @param solver_type Type of LP solver to use.
100 * @param useWarmStart Whether the LP solver can warm start the resolution.
101 * @param max_num_cdd_trials indicate the max number of attempts to compute
102 * the cone by introducing
103 * @param canonicalize_cdd_matrix whether to remove redundant inequalities
104 * when computing double description matrices a small pertubation of the
105 * system
106 */
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
118 /**
119 * @brief Returns the useWarmStart flag.
120 * @return True if the LP solver is allowed to use warm start, false
121 * otherwise.
122 */
123 2 bool useWarmStart() { return m_solver->getUseWarmStart(); }
124
125 /**
126 * @brief Specifies whether the LP solver is allowed to use warm start.
127 * @param uws True if the LP solver is allowed to use warm start, false
128 * otherwise.
129 */
130 1 void setUseWarmStart(bool uws) { m_solver->setUseWarmStart(uws); }
131
132 /**
133 * @brief Get the name of this object.
134 * @return The name of this object.
135 */
136 211 std::string getName() { return m_name; }
137
138 20 EquilibriumAlgorithm getAlgorithm() { return m_algorithm; }
139
140 void setAlgorithm(EquilibriumAlgorithm algorithm);
141
142 /**
143 * @brief Specify a new set of contacts.
144 * All 3d vectors are expressed in a reference frame having the z axis aligned
145 * with gravity. In other words the gravity vecotr is (0, 0, -9.81). This
146 * method considers row-major matrices as input.
147 * @param contactPoints List of N 3d contact points as an Nx3 matrix.
148 * @param contactNormals List of N 3d contact normal directions as an Nx3
149 * matrix.
150 * @param frictionCoefficient The contact friction coefficient.
151 * @param alg Algorithm to use for testing equilibrium.
152 * @return True if the operation succeeded, false otherwise.
153 */
154 bool setNewContacts(const MatrixX3& contactPoints,
155 const MatrixX3& contactNormals,
156 const double frictionCoefficient,
157 const EquilibriumAlgorithm alg);
158
159 /**
160 * @brief Specify a new set of contacts.
161 * All 3d vectors are expressed in a reference frame having the z axis aligned
162 * with gravity. In other words the gravity vecotr is (0, 0, -9.81). This
163 * method considers column major matrices as input, and converts them into
164 * rowmajor matrices for internal use with the solvers.
165 * @param contactPoints List of N 3d contact points as an Nx3 matrix.
166 * @param contactNormals List of N 3d contact normal directions as an Nx3
167 * matrix.
168 * @param frictionCoefficient The contact friction coefficient.
169 * @param alg Algorithm to use for testing equilibrium.
170 * @return True if the operation succeeded, false otherwise.
171 */
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
179 /**
180 * @brief Compute a measure of the robustness of the equilibrium of the
181 * specified com position. This amounts to solving the following LP: find b,
182 * b0 maximize b0 subject to G b = D c + d b >= b0 where: b are the
183 * coefficient of the contact force generators (f = G b) b0 is a
184 * parameter proportional to the robustness measure c is the specified
185 * CoM position G is the 6xm matrix whose columns are the
186 * gravito-inertial wrench generators D is the 6x3 matrix mapping the
187 * CoM position in gravito-inertial wrench d is the 6d vector
188 * containing the gravity part of the gravito-inertial wrench
189 * @param com The 3d center of mass position to test.
190 * @param robustness The computed measure of robustness.
191 * @return The status of the LP solver.
192 * @note If the system is in force closure the status will be
193 * LP_STATUS_UNBOUNDED, meaning that the system can reach infinite robustness.
194 * This is due to the fact that we are not considering any upper limit for the
195 * friction cones.
196 */
197 LP_status computeEquilibriumRobustness(Cref_vector3 com, double& robustness);
198
199 /**
200 * @brief Compute a measure of the robustness of the equilibrium of the
201 * specified com position. This amounts to solving the following LP: find b,
202 * b0 maximize b0 subject to G b = D c + d b >= b0 where: b are the
203 * coefficient of the contact force generators (f = G b) b0 is a
204 * parameter proportional to the robustness measure c is the specified
205 * CoM position G is the 6xm matrix whose columns are the
206 * gravito-inertial wrench generators D is the 6x3 matrix mapping the
207 * CoM position in gravito-inertial wrench d is the 6d vector
208 * containing the gravity part of the gravito-inertial wrench
209 * @param com The 3d center of mass position to test.
210 * @param acc The 3d acceleration of the CoM.
211 * @param robustness The computed measure of robustness.
212 * @return The status of the LP solver.
213 * @note If the system is in force closure the status will be
214 * LP_STATUS_UNBOUNDED, meaning that the system can reach infinite robustness.
215 * This is due to the fact that we are not considering any upper limit for the
216 * friction cones.
217 */
218 LP_status computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc,
219 double& robustness);
220
221 /**
222 * @brief Check whether the specified com position is in robust equilibrium.
223 * This amounts to solving the following feasibility LP:
224 * find b
225 * minimize 1
226 * subject to G b = D c + d
227 * b >= b0
228 * where:
229 * b are the coefficient of the contact force generators (f = G b)
230 * b0 is a parameter proportional to the specified robustness
231 * measure c is the specified CoM position G is the 6xm matrix
232 * whose columns are the gravito-inertial wrench generators D is the
233 * 6x3 matrix mapping the CoM position in gravito-inertial wrench d is
234 * the 6d vector containing the gravity part of the gravito-inertial wrench
235 * @param com The 3d center of mass position to test.
236 * @param equilibrium True if com is in robust equilibrium, false otherwise.
237 * @param e_max Desired robustness level.
238 * @return The status of the LP solver.
239 */
240 LP_status checkRobustEquilibrium(Cref_vector3 com, bool& equilibrium,
241 double e_max = 0.0);
242
243 /**
244 * @brief Check whether the specified com position is in robust equilibrium.
245 * This amounts to solving the following feasibility LP:
246 * find b
247 * minimize 1
248 * subject to G b = D c + d
249 * b >= b0
250 * where:
251 * b are the coefficient of the contact force generators (f = G b)
252 * b0 is a parameter proportional to the specified robustness
253 * measure c is the specified CoM position G is the 6xm matrix
254 * whose columns are the gravito-inertial wrench generators D is the
255 * 6x3 matrix mapping the CoM position in gravito-inertial wrench d is
256 * the 6d vector containing the gravity part of the gravito-inertial wrench
257 * @param com The 3d center of mass position to test.
258 * @param acc The 3d acceleration of the CoM.
259 * @param equilibrium True if com is in robust equilibrium, false otherwise.
260 * @param e_max Desired robustness level.
261 * @return The status of the LP solver.
262 */
263 LP_status checkRobustEquilibrium(Cref_vector3 com, Cref_vector3 acc,
264 bool& equilibrium, double e_max = 0.0);
265
266 /**
267 * @brief Compute the extremum CoM position over the line a*x + a0 that is in
268 * robust equilibrium. This amounts to solving the following LP: find c, b
269 * maximize c
270 * subject to G b = D (a c + a0) + d
271 * b >= b0
272 * where:
273 * b are the m coefficients of the contact force generators (f = G
274 * b) b0 is an m-dimensional vector of identical values that are
275 * proportional to e_max c is the 1d line parameter G is the
276 * 6xm matrix whose columns are the gravito-inertial wrench generators D is
277 * the 6x3 matrix mapping the CoM position in gravito-inertial wrench d is the
278 * 6d vector containing the gravity part of the gravito-inertial wrench
279 * @param a 2d vector representing the line direction
280 * @param a0 2d vector representing an arbitrary point over the line
281 * @param e_max Desired robustness in terms of the maximum force error
282 * tolerated by the system
283 * @return The status of the LP solver.
284 * @note If the system is in force closure the status will be
285 * LP_STATUS_UNBOUNDED, meaning that the system can reach infinite robustness.
286 * This is due to the fact that we are not considering any upper limit for the
287 * friction cones.
288 */
289 LP_status findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max,
290 Ref_vector3 com);
291
292 /**
293 * @brief Find the extremum com position that is in robust equilibrium in the
294 * specified direction. This amounts to solving the following LP: find c, b
295 * maximize a^T c
296 * subject to G b = D c + d
297 * b >= b0
298 * where:
299 * a is the specified 2d direction
300 * b are the m coefficients of the contact force generators (f = G
301 * b) b0 is an m-dimensional vector of identical values that are
302 * proportional to e_max c is the 3d com position G is the 6xm
303 * matrix whose columns are the gravito-inertial wrench generators D is the
304 * 6x3 matrix mapping the CoM position in gravito-inertial wrench d is
305 * the 6d vector containing the gravity part of the gravito-inertial wrench
306 * @param direction Desired 3d direction.
307 * @param com Output 3d com position.
308 * @param e_max Desired robustness level.
309 * @return The status of the LP solver.
310 * @note If the system is in force closure the status will be
311 * LP_STATUS_UNBOUNDED, meaning that the system can reach infinite robustness.
312 * This is due to the fact that we are not considering any upper limit for the
313 * friction cones.
314 */
315 LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com,
316 double e_max = 0.0);
317
318 /**
319 * @brief Retrieve the inequalities that define the admissible wrenchs
320 * for the current contact set. WARNING. The H and h matrices are defined
321 * in such a way that H w >= h is verified if w is an admissible wrench. This
322 * is different from the ICRA 15 paper from Del Prete et al., where the
323 * negative matrices are used.
324 * @param H reference to the H matrix to initialize
325 * @param h reference to the h vector to initialize
326 * @return The status of the inequalities. If the inequalities are not defined
327 * due to numerical instabilities, will send appropriate error message,
328 * and return LP_STATUS_ERROR. If they are not defined because no
329 * contact has been defined, will return LP_STATUS_INFEASIBLE
330 */
331 LP_status getPolytopeInequalities(MatrixXX& H, VectorX& h) const;
332
333 /**
334 * @brief findMaximumAcceleration Find the maximal acceleration along a given
335 direction find b, alpha0 maximize alpha0 subject to [-G
336 (Hv)] [b a0]^T = -h 0 <= [b a0]^T <= Inf
337
338
339 b are the coefficient of the contact force generators (f = V
340 b) v is the vector3 defining the direction of the motion alpha0 is
341 the maximal amplitude of the acceleration, for the given direction v c is the
342 CoM position G is the matrix whose columns are the gravito-inertial
343 wrench generators H is m*[I3 ĉ]^T h is m*[-g (c x -g)]^T
344 * @param H input
345 * @param h input
346 * @param v input
347 * @param alpha0 output
348 * @return The status of the LP solver.
349 */
350 LP_status findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h,
351 Cref_vector3 v, double& alpha0);
352
353 /**
354 * @brief checkAdmissibleAcceleration return true if the given acceleration is
355 admissible for the given contacts find b subject to G b = Ha + h
356 0 <= b <= Inf
357 b are the coefficient of the contact force generators (f = V
358 b) a is the vector3 defining the acceleration G is the matrix
359 whose columns are the gravito-inertial wrench generators H is m*[I3
360 ĉ]^T h is m*[-g (c x -g)]^T
361 * @param a the acceleration
362 * @return true if the acceleration is admissible, false otherwise
363 */
364 bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h,
365 Cref_vector3 a);
366 };
367
368 } // end namespace centroidal_dynamics
369
370 #endif
371