| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2017 CNRS | ||
| 3 | // | ||
| 4 | // This file is part of tsid | ||
| 5 | // tsid is free software: you can redistribute it | ||
| 6 | // and/or modify it under the terms of the GNU Lesser General Public | ||
| 7 | // License as published by the Free Software Foundation, either version | ||
| 8 | // 3 of the License, or (at your option) any later version. | ||
| 9 | // tsid is distributed in the hope that it will be | ||
| 10 | // useful, but WITHOUT ANY WARRANTY; without even the implied warranty | ||
| 11 | // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | ||
| 12 | // General Lesser Public License for more details. You should have | ||
| 13 | // received a copy of the GNU Lesser General Public License along with | ||
| 14 | // tsid If not, see | ||
| 15 | // <http://www.gnu.org/licenses/>. | ||
| 16 | // | ||
| 17 | |||
| 18 | #ifndef SOLVERABSTRACT_HH_ | ||
| 19 | #define SOLVERABSTRACT_HH_ | ||
| 20 | |||
| 21 | #include <Eigen/Dense> | ||
| 22 | #include <hpp/bezier-com-traj/local_config.hh> | ||
| 23 | |||
| 24 | namespace solvers { | ||
| 25 | |||
| 26 | /** | ||
| 27 | * Possible states of the solver. | ||
| 28 | */ | ||
| 29 | enum optim_status { OPTIM_OPTIMAL = 0, OPTIM_INFEASIBLE = 1 }; | ||
| 30 | |||
| 31 | static const double UNBOUNDED_UP = 100000.; | ||
| 32 | static const double UNBOUNDED_DOWN = -100000.; | ||
| 33 | |||
| 34 | typedef Eigen::MatrixXd MatrixXd; | ||
| 35 | typedef Eigen::VectorXd VectorXd; | ||
| 36 | typedef Eigen::VectorXi VectorXi; | ||
| 37 | typedef const Eigen::Ref<const VectorXd>& Cref_vectorX; | ||
| 38 | |||
| 39 | enum BEZIER_COM_TRAJ_DLLAPI SolverType { | ||
| 40 | SOLVER_QUADPROG = 0x00001 | ||
| 41 | // SOLVER_QUADPROG_SPARSE = 0x00002 | ||
| 42 | #ifdef USE_GLPK_SOLVER | ||
| 43 | , | ||
| 44 | SOLVER_GLPK = 0x00002 | ||
| 45 | #endif | ||
| 46 | }; | ||
| 47 | |||
| 48 | /** | ||
| 49 | * @brief Struct used to return the results of the trajectory generation | ||
| 50 | * problem. | ||
| 51 | */ | ||
| 52 | struct BEZIER_COM_TRAJ_DLLAPI ResultData { | ||
| 53 |
2/4✓ Branch 1 taken 2684 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2684 times.
✗ Branch 5 not taken.
|
2684 | ResultData() : success_(false), cost_(-1.), x(VectorXd::Zero(0)) {} |
| 54 | |||
| 55 | ResultData(const bool success, const double cost, Cref_vectorX x) | ||
| 56 | : success_(success), cost_(cost), x(x) {} | ||
| 57 | |||
| 58 | 64 | ResultData(const ResultData& other) | |
| 59 | 64 | : success_(other.success_), cost_(other.cost_), x(other.x) {} | |
| 60 | 2748 | ~ResultData() {} | |
| 61 | |||
| 62 | 59 | ResultData& operator=(const ResultData& other) { | |
| 63 | 59 | success_ = (other.success_); | |
| 64 | 59 | cost_ = (other.cost_); | |
| 65 | 59 | x = (other.x); | |
| 66 | 59 | return *this; | |
| 67 | } | ||
| 68 | bool success_; // whether the optimization was successful | ||
| 69 | double cost_; // cost evaluation for the solved control point | ||
| 70 | VectorXd x; // control point | ||
| 71 | }; | ||
| 72 | |||
| 73 | // min g'x | ||
| 74 | // st CIx <= ci0 | ||
| 75 | // CEx = ce0 | ||
| 76 | /** | ||
| 77 | * @brief solve Solve a QP or LP given | ||
| 78 | * init position and velocity, 0 velocity constraints (acceleration constraints | ||
| 79 | * are ignored) | ||
| 80 | * @param pData problem Data. Should contain only one contact phase. | ||
| 81 | * @param Ts timelength of each contact phase. Should only contain one value | ||
| 82 | * @param timeStep time that the solver has to stop. | ||
| 83 | * @return ResultData a struct containing the resulting trajectory, if success | ||
| 84 | * is true. | ||
| 85 | */ | ||
| 86 | ResultData BEZIER_COM_TRAJ_DLLAPI solve( | ||
| 87 | const MatrixXd& A, const VectorXd& b, const MatrixXd& D, const VectorXd& d, | ||
| 88 | const MatrixXd& Hess, const VectorXd& g, const VectorXd& initGuess, | ||
| 89 | Cref_vectorX minBounds, Cref_vectorX maxBounds, const SolverType solver); | ||
| 90 | |||
| 91 | } /* namespace solvers */ | ||
| 92 | |||
| 93 | #endif /* SOLVERABSTRACT_HH_ */ | ||
| 94 |