centroidal_dynamics.hh 15 KB
Newer Older
1
2
3
4
5
/*
 * Copyright 2015, LAAS-CNRS
 * Author: Andrea Del Prete
 */

Steve Tonneau's avatar
Steve Tonneau committed
6
7
#ifndef CENTROIDAL_DYNAMICS_LIB_STATIC_EQUILIBRIUM_H
#define CENTROIDAL_DYNAMICS_LIB_STATIC_EQUILIBRIUM_H
8
9

#include <Eigen/Dense>
10
11
12
#include <centroidal-dynamics-lib/config.hh>
#include <centroidal-dynamics-lib/util.hh>
#include <centroidal-dynamics-lib/solver_LP_abstract.hh>
13

Steve Tonneau's avatar
Steve Tonneau committed
14
namespace centroidal_dynamics
15
16
{

Steve Tonneau's avatar
Steve Tonneau committed
17
enum CENTROIDAL_DYNAMICS_DLLAPI EquilibriumAlgorithm
18
{
Steve Tonneau's avatar
Steve Tonneau committed
19
20
21
22
23
24
  EQUILIBRIUM_ALGORITHM_LP,  /// primal LP formulation
  EQUILIBRIUM_ALGORITHM_LP2, /// another primal LP formulation
  EQUILIBRIUM_ALGORITHM_DLP, /// dual LP formulation
  EQUILIBRIUM_ALGORITHM_PP,  /// polytope projection algorithm
  EQUILIBRIUM_ALGORITHM_IP,  /// incremental projection algorithm based on primal LP formulation
  EQUILIBRIUM_ALGORITHM_DIP  /// incremental projection algorithm based on dual LP formulation
25
26
};

Steve Tonneau's avatar
Steve Tonneau committed
27
class CENTROIDAL_DYNAMICS_DLLAPI Equilibrium
28
{
t steve's avatar
t steve committed
29
30
31
32
public:
  const double m_mass; /// mass of the system
  const Vector3 m_gravity; ///  gravity vector

33
private:
andreadelprete's avatar
andreadelprete committed
34
35
36
  static bool m_is_cdd_initialized;   /// true if cdd lib has been initialized, false otherwise

  std::string                 m_name;         /// name of this object
Steve Tonneau's avatar
Steve Tonneau committed
37
  EquilibriumAlgorithm  m_algorithm;    /// current algorithm used
andreadelprete's avatar
andreadelprete committed
38
39
40
41
42
  Solver_LP_abstract*         m_solver;       /// LP solver
  SolverLP                    m_solver_type;  /// type of LP solver

  unsigned int  m_generatorsPerContact; /// number of generators to approximate the friction cone per contact point

43
44
  /** Gravito-inertial wrench generators (6 X numberOfContacts*generatorsPerContact) */
  Matrix6X m_G_centr;
andreadelprete's avatar
andreadelprete committed
45
46

  /** Inequality matrix and vector defining the gravito-inertial wrench cone H w <= h */
47
48
  MatrixXX m_H;
  VectorX m_h;
Steve Tonneau's avatar
Steve Tonneau committed
49
50
  /** False if a numerical instability appeared in the computation H and h*/
  bool m_is_cdd_stable;
Steve Tonneau's avatar
Steve Tonneau committed
51
  /** EQUILIBRIUM_ALGORITHM_PP: If double description fails,
52
53
54
    * indicate the max number of attempts to compute the cone by introducing
    * a small pertubation of the system */
  const unsigned max_num_cdd_trials;
55
56
  /** whether to remove redundant inequalities when computing double description matrices*/
  const bool canonicalize_cdd_matrix;
andreadelprete's avatar
andreadelprete committed
57
58

  /** Inequality matrix and vector defining the CoM support polygon HD com + Hd <= h */
59
  MatrixX3 m_HD;
60
  VectorX  m_Hd;
andreadelprete's avatar
andreadelprete committed
61

62
  /** Matrix and vector mapping 2d com position to GIW */
63
  Matrix63 m_D;
64
65
  Vector6 m_d;

66
67
68
  /** Coefficient used for converting the robustness measure in Newtons */
  double m_b0_to_emax_coefficient;

69
  bool computePolytopeProjection(Cref_matrix6X v);
70
71
  bool computeGenerators(Cref_matrixX3 contactPoints, Cref_matrixX3 contactNormals,
                         double frictionCoefficient, const bool perturbate = false);
72

73
  /**
74
75
76
   * @brief Given the smallest coefficient of the contact force generators it computes
   * the minimum norm of force error necessary to have a contact force on
   * the associated friction cone boundaries.
77
78
79
80
81
82
83
84
   * @param b0 Minimum coefficient of the contact force generators.
   * @return Minimum norm of the force error necessary to result in a contact force being
   * on the friction cone boundaries.
   */
  double convert_b0_to_emax(double b0);

  double convert_emax_to_b0(double emax);

85
public:
Steve Tonneau's avatar
Steve Tonneau committed
86
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
andreadelprete's avatar
andreadelprete committed
87
  /**
Steve Tonneau's avatar
Steve Tonneau committed
88
   * @brief Equilibrium constructor.
andreadelprete's avatar
andreadelprete committed
89
90
91
92
   * @param name Name of the object.
   * @param mass Mass of the system for which to test equilibrium.
   * @param generatorsPerContact Number of generators used to approximate the friction cone per contact point.
   * @param solver_type Type of LP solver to use.
93
   * @param useWarmStart Whether the LP solver can warm start the resolution.
94
   * @param max_num_cdd_trials indicate the max number of attempts to compute the cone by introducing
95
   * @param canonicalize_cdd_matrix whether to remove redundant inequalities when computing double description matrices
96
   * a small pertubation of the system
andreadelprete's avatar
andreadelprete committed
97
   */
Steve Tonneau's avatar
Steve Tonneau committed
98
99
  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,
100
                    const bool canonicalize_cdd_matrix=false);
101
102
103
104
105
106
107
108
109
110
111

  /**
   * @brief Returns the useWarmStart flag.
   * @return True if the LP solver is allowed to use warm start, false otherwise.
   */
  bool useWarmStart(){ return m_solver->getUseWarmStart(); }

  /**
   * @brief Specifies whether the LP solver is allowed to use warm start.
   * @param uws True if the LP solver is allowed to use warm start, false otherwise.
   */
Steve Tonneau's avatar
Steve Tonneau committed
112
  void setUseWarmStart(bool uws){ m_solver->setUseWarmStart(uws); }
113

andreadelprete's avatar
andreadelprete committed
114
115
116
117
  /**
   * @brief Get the name of this object.
   * @return The name of this object.
   */
118
  std::string getName(){ return m_name; }
119

Steve Tonneau's avatar
Steve Tonneau committed
120
  EquilibriumAlgorithm getAlgorithm(){ return m_algorithm; }
121

Pierre Fernbach's avatar
Pierre Fernbach committed
122
123
  void setAlgorithm(EquilibriumAlgorithm algorithm);

andreadelprete's avatar
andreadelprete committed
124
125
  /**
   * @brief Specify a new set of contacts.
andreadelprete's avatar
andreadelprete committed
126
   * All 3d vectors are expressed in a reference frame having the z axis aligned with gravity.
127
128
   * In other words the gravity vecotr is (0, 0, -9.81). This method considers row-major
   * matrices as input.
andreadelprete's avatar
andreadelprete committed
129
130
   * @param contactPoints List of N 3d contact points as an Nx3 matrix.
   * @param contactNormals List of N 3d contact normal directions as an Nx3 matrix.
131
   * @param frictionCoefficient The contact friction coefficient.
andreadelprete's avatar
andreadelprete committed
132
133
134
   * @param alg Algorithm to use for testing equilibrium.
   * @return True if the operation succeeded, false otherwise.
   */
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
  bool setNewContacts(const MatrixX3& contactPoints, const MatrixX3& contactNormals,
                      const double frictionCoefficient, const EquilibriumAlgorithm alg);

  /**
   * @brief 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.
   * @param contactPoints List of N 3d contact points as an Nx3 matrix.
   * @param contactNormals List of N 3d contact normal directions as an Nx3 matrix.
   * @param frictionCoefficient The contact friction coefficient.
   * @param alg Algorithm to use for testing equilibrium.
   * @return True if the operation succeeded, false otherwise.
   */
  bool setNewContacts(const MatrixX3ColMajor& contactPoints, const MatrixX3ColMajor&  contactNormals,
                      const double frictionCoefficient, const EquilibriumAlgorithm alg);
151

Pierre Fernbach's avatar
Pierre Fernbach committed
152
153
  void setG(Cref_matrix6X G){m_G_centr = G;}

andreadelprete's avatar
andreadelprete committed
154
155
156
157
158
159
160
161
162
163
164
165
  /**
   * @brief 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
166
   *     D         is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
andreadelprete's avatar
andreadelprete committed
167
   *     d         is the 6d vector containing the gravity part of the gravito-inertial wrench
168
   * @param com The 3d center of mass position to test.
andreadelprete's avatar
andreadelprete committed
169
   * @param robustness The computed measure of robustness.
170
   * @return The status of the LP solver.
171
172
173
   * @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.
andreadelprete's avatar
andreadelprete committed
174
   */
175
  LP_status computeEquilibriumRobustness(Cref_vector3 com, double &robustness);
176

177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
  /**
   * @brief 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
   * @param com The 3d center of mass position to test.
   * @param acc The 3d acceleration of the CoM.
   * @param robustness The computed measure of robustness.
   * @return 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.
   */
  LP_status computeEquilibriumRobustness(Cref_vector3 com, Cref_vector3 acc, double &robustness);

andreadelprete's avatar
andreadelprete committed
201
202
203
204
205
206
207
208
209
210
211
212
  /**
   * @brief 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
213
   *     D         is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
andreadelprete's avatar
andreadelprete committed
214
   *     d         is the 6d vector containing the gravity part of the gravito-inertial wrench
215
   * @param com The 3d center of mass position to test.
andreadelprete's avatar
andreadelprete committed
216
217
   * @param equilibrium True if com is in robust equilibrium, false otherwise.
   * @param e_max Desired robustness level.
218
   * @return The status of the LP solver.
andreadelprete's avatar
andreadelprete committed
219
   */
220
  LP_status checkRobustEquilibrium(Cref_vector3 com, bool &equilibrium, double e_max=0.0);
221

andreadelprete's avatar
andreadelprete committed
222
223
224
  /**
   * @brief Compute the extremum CoM position over the line a*x + a0 that is in robust equilibrium.
   * This amounts to solving the following LP:
225
226
227
228
   *     find          c, b
   *     maximize      c
   *     subject to    G b = D (a c + a0) + d
   *                   b  >= b0
andreadelprete's avatar
andreadelprete committed
229
   *   where:
230
   *     b         are the m coefficients of the contact force generators (f = G b)
andreadelprete's avatar
andreadelprete committed
231
   *     b0        is an m-dimensional vector of identical values that are proportional to e_max
232
233
   *     c         is the 1d line parameter
   *     G         is the 6xm matrix whose columns are the gravito-inertial wrench generators
234
   *     D         is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
235
236
237
238
   *     d         is the 6d vector containing the gravity part of the gravito-inertial wrench
   * @param a 2d vector representing the line direction
   * @param a0 2d vector representing an arbitrary point over the line
   * @param e_max Desired robustness in terms of the maximum force error tolerated by the system
239
   * @return The status of the LP solver.
240
241
242
   * @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.
243
  */
244
  LP_status findExtremumOverLine(Cref_vector3 a, Cref_vector3 a0, double e_max, Ref_vector3 com);
245

andreadelprete's avatar
andreadelprete committed
246
247
248
249
250
251
252
253
254
255
256
  /**
   * @brief 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
257
   *     c         is the 3d com position
andreadelprete's avatar
andreadelprete committed
258
   *     G         is the 6xm matrix whose columns are the gravito-inertial wrench generators
259
   *     D         is the 6x3 matrix mapping the CoM position in gravito-inertial wrench
andreadelprete's avatar
andreadelprete committed
260
   *     d         is the 6d vector containing the gravity part of the gravito-inertial wrench
261
262
   * @param direction Desired 3d direction.
   * @param com Output 3d com position.
andreadelprete's avatar
andreadelprete committed
263
   * @param e_max Desired robustness level.
264
   * @return The status of the LP solver.
265
266
267
   * @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.
andreadelprete's avatar
andreadelprete committed
268
   */
269
  LP_status findExtremumInDirection(Cref_vector3 direction, Ref_vector3 com, double e_max=0.0);
270

Steve Tonneau's avatar
Steve Tonneau committed
271
272
273
274
275
276
277
278
279
280
281
  /**
   * @brief Retrieve the inequalities that define the admissible wrenchs
   * for the current contact set.
   * @param H reference to the H matrix to initialize
   * @param h reference to the h vector to initialize
   * @return 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
   */
  LP_status getPolytopeInequalities(MatrixXX& H, VectorX& h) const;
282
283
284
285

  /**
   * @brief findMaximumAcceleration Find the maximal acceleration along a given direction
          find          b, alpha0
286
          maximize      alpha0
287
          subject to    [-G  (Hv)] [b a0]^T  = -h
288
289
290
291
292
293
294
295
                        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
296
297
298
299
300
301
          H         is m*[I3 ĉ]^T
          h         is m*[-g  (c x -g)]^T
   * @param H input
   * @param h input
   * @param v input
   * @param alpha0 output
302
303
   * @return The status of the LP solver.
   */
304
  LP_status findMaximumAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 v, double& alpha0);
305
306
307
308
309
310
311
312
313

  /**
   * @brief 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
314
315
316
          H         is m*[I3 ĉ]^T
          h         is m*[-g  (c x -g)]^T
   * @param a the acceleration
317
318
   * @return true if the acceleration is admissible, false otherwise
   */
319
  bool checkAdmissibleAcceleration(Cref_matrix63 H, Cref_vector6 h, Cref_vector3 a );
320

321
322
};

Steve Tonneau's avatar
Steve Tonneau committed
323
} // end namespace centroidal_dynamics
324
325

#endif