| 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 |