-
Notifications
You must be signed in to change notification settings - Fork 107
/
Solver.hpp
315 lines (274 loc) · 10.5 KB
/
Solver.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
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
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
/**
* @file Solver.hpp
* @author Giulio Romualdi
* @copyright Released under the terms of the BSD 3-Clause License
* @date 2018
*/
#ifndef OSQPEIGEN_SOLVER_HPP
#define OSQPEIGEN_SOLVER_HPP
// Std
#include <memory>
// Eigen
#include <Eigen/Dense>
// OSQP
#include <osqp.h>
// OsqpEigen
#include <OsqpEigen/Compat.hpp>
#include <OsqpEigen/Constants.hpp>
#include <OsqpEigen/Data.hpp>
#include <OsqpEigen/Settings.hpp>
/**
* OsqpEigen namespace.
*/
namespace OsqpEigen
{
/**
* Solver class is a wrapper of the OSQP OSQPWorkspace struct.
*/
class Solver
{
bool m_isSolverInitialized; /**< Boolean true if solver is initialized. */
#ifdef OSQP_EIGEN_OSQP_IS_V1
std::unique_ptr<OSQPSolver, std::function<void(OSQPSolver*)>> m_solver; /**< Pointer to
OSQPSolver struct. */
#else
std::unique_ptr<OSQPWorkspace, std::function<void(OSQPWorkspace*)>> m_workspace; /**< Pointer to
OSQPWorkspace
struct. */
#endif
std::unique_ptr<OsqpEigen::Settings> m_settings; /**< Pointer to Settings class. */
std::unique_ptr<OsqpEigen::Data> m_data; /**< Pointer to Data class. */
Eigen::Matrix<c_float, Eigen::Dynamic, 1> m_primalVariables;
Eigen::Matrix<c_float, Eigen::Dynamic, 1> m_dualVariables;
Eigen::Matrix<c_float, -1, 1> m_solution;
Eigen::Matrix<c_float, -1, 1> m_dualSolution;
std::vector<c_int> m_hessianNewIndices;
std::vector<c_float> m_hessianNewValues;
std::vector<c_int> m_constraintsNewIndices;
std::vector<c_float> m_constraintsNewValues;
std::vector<Eigen::Triplet<c_float>> m_oldHessianTriplet, m_newHessianTriplet,
m_newUpperTriangularHessianTriplets;
std::vector<Eigen::Triplet<c_float>> m_oldLinearConstraintsTriplet,
m_newLinearConstraintsTriplet;
/**
* Evaluate the position and the values of the new elements of a sparse matrix.
* @param oldMatrixTriplet vector containing the triplets of the old sparse matrix;
* @param newMatrixTriplet vector containing the triplets of the mew sparse matrix;
* @param newIndices vector of the index mapping new elements
* to position in the sparse matrix;
* @param newValues vector of new elements in the sparse matrix.
* @return true if the sparsity patern is not changed false otherwise.
*/
template <typename T>
bool evaluateNewValues(const std::vector<Eigen::Triplet<T>>& oldMatrixTriplet,
const std::vector<Eigen::Triplet<T>>& newMatrixTriplet,
std::vector<c_int>& newIndices,
std::vector<c_float>& newValues) const;
/**
* Takes only the triplets which belongs to the upper triangular part of the matrix.
* @param fullMatrixTriplets vector containing the triplets of the sparse matrix;
* @param upperTriangularMatrixTriplets vector containing the triplets of the mew sparse matrix;
*/
template <typename T>
void selectUpperTriangularTriplets(
const std::vector<Eigen::Triplet<T>>& fullMatrixTriplets,
std::vector<Eigen::Triplet<T>>& upperTriangularMatrixTriplets) const;
#ifdef OSQP_EIGEN_OSQP_IS_V1
/**
* Custom Deleter for the OSQPSolver. It is required to free the @ref m_workspace unique_ptr
* @param ptr raw pointer to the workspace
*/
static void OSQPSolverDeleter(OSQPSolver* ptr) noexcept;
#else
/**
* Custom Deleter for the OSQPWorkspace. It is required to free the @ref m_workspace unique_ptr
* @param ptr raw pointer to the workspace
*/
static void OSQPWorkspaceDeleter(OSQPWorkspace* ptr) noexcept;
#endif
inline const OSQPData* getData() const noexcept
{
#ifdef OSQP_EIGEN_OSQP_IS_V1
return m_data->getData();
#else
return m_workspace->data;
#endif
}
inline const OSQPInfo* getInfo() const noexcept
{
#ifdef OSQP_EIGEN_OSQP_IS_V1
return m_solver->info;
#else
return m_workspace->info;
#endif
}
inline const OSQPSolution* getOSQPSolution() const noexcept
{
#ifdef OSQP_EIGEN_OSQP_IS_V1
return m_solver->solution;
#else
return m_workspace->solution;
#endif
}
public:
/**
* Constructor.
*/
Solver();
/**
* Initialize the solver with the actual initial data and settings.
* @return true/false in case of success/failure.
*/
bool initSolver();
/**
* Check if the solver is initialized.
* @return true if the solver is initialized.
*/
bool isInitialized();
/**
* Dealocate memory.
*/
void clearSolver();
/**
* Set to zero all the solver variables.
* @return true/false in case of success/failure.
*/
bool clearSolverVariables();
/**
* Solve the QP optimization problem.
* @return true/false in case of success/failure.
*/
[[deprecated("Use solveProblem() instead.")]] bool solve();
/**
* Solve the QP optimization problem.
* @return the error exit flag
*/
OsqpEigen::ErrorExitFlag solveProblem();
/**
* Get the status of the solver
* @return The inner solver status
*/
OsqpEigen::Status getStatus() const;
/**
* Get the primal objective value
* @return The primal objective value
*/
const c_float getObjValue() const;
/**
* Get the optimization problem solution.
* @return an Eigen::Vector contating the optimization result.
*/
const Eigen::Matrix<c_float, -1, 1>& getSolution();
/**
* Get the dual optimization problem solution.
* @return an Eigen::Vector contating the optimization result.
*/
const Eigen::Matrix<c_float, -1, 1>& getDualSolution();
/**
* Update the linear part of the cost function (Gradient).
* @param gradient is the Gradient vector.
* @note the elements of the gradient are not copied inside the library.
* The user has to guarantee that the lifetime of the objects passed is the same of the
* OsqpEigen object.
* @return true/false in case of success/failure.
*/
bool
updateGradient(const Eigen::Ref<const Eigen::Matrix<c_float, Eigen::Dynamic, 1>>& gradient);
/**
* Update the lower bounds limit (size m).
* @param lowerBound is the lower bound constraint vector.
* @note the elements of the lowerBound are not copied inside the library.
* The user has to guarantee that the lifetime of the object passed is the same of the
* OsqpEigen object.
* @return true/false in case of success/failure.
*/
bool
updateLowerBound(const Eigen::Ref<const Eigen::Matrix<c_float, Eigen::Dynamic, 1>>& lowerBound);
/**
* Update the upper bounds limit (size m).
* @param upperBound is the upper bound constraint vector.
* @note the elements of the upperBound are not copied inside the library.
* The user has to guarantee that the lifetime of the object passed is the same of the
* OsqpEigen object.
* @return true/false in case of success/failure.
*/
bool
updateUpperBound(const Eigen::Ref<const Eigen::Matrix<c_float, Eigen::Dynamic, 1>>& upperBound);
/**
* Update both upper and lower bounds (size m).
* @param lowerBound is the lower bound constraint vector;
* @param upperBound is the upper bound constraint vector.
* @note the elements of the lowerBound and upperBound are not copied inside the library.
* The user has to guarantee that the lifetime of the objects passed is the same of the
* OsqpEigen object
* @return true/false in case of success/failure.
*/
bool
updateBounds(const Eigen::Ref<const Eigen::Matrix<c_float, Eigen::Dynamic, 1>>& lowerBound,
const Eigen::Ref<const Eigen::Matrix<c_float, Eigen::Dynamic, 1>>& upperBound);
/**
* Update the quadratic part of the cost function (Hessian).
* It is assumed to be a simmetric matrix.
* \note
* If the sparsity pattern is preserved the matrix is simply update
* otherwise the entire solver will be reinitialized. In this case
* the primal and dual variable are copied in the new workspace.
*
* @param hessian is the Hessian matrix.
* @return true/false in case of success/failure.
*/
template <typename Derived>
bool updateHessianMatrix(const Eigen::SparseCompressedBase<Derived>& hessianMatrix);
/**
* Update the linear constraints matrix (A)
* \note
* If the sparsity pattern is preserved the matrix is simply update
* otherwise the entire solver will be reinitialized. In this case
* the primal and dual variable are copied in the new workspace.
*
* @param linearConstraintsMatrix is the linear constraint matrix A
* @return true/false in case of success/failure.
*/
template <typename Derived>
bool updateLinearConstraintsMatrix(
const Eigen::SparseCompressedBase<Derived>& linearConstraintsMatrix);
/**
* Set the entire
* @param linearConstraintsMatrix is the linear constraint matrix A
* @return true/false in case of success/failure.
*/
template <typename T, int n, int m>
bool setWarmStart(const Eigen::Matrix<T, n, 1>& primalVariable,
const Eigen::Matrix<T, m, 1>& dualVariable);
template <typename T, int n>
bool setPrimalVariable(const Eigen::Matrix<T, n, 1>& primalVariable);
template <typename T, int m> bool setDualVariable(const Eigen::Matrix<T, m, 1>& dualVariable);
template <typename T, int n> bool getPrimalVariable(Eigen::Matrix<T, n, 1>& primalVariable);
template <typename T, int m> bool getDualVariable(Eigen::Matrix<T, m, 1>& dualVariable);
/**
* Get the solver settings pointer.
* @return the pointer to Settings object.
*/
const std::unique_ptr<OsqpEigen::Settings>& settings() const;
/**
* Get the pointer to the solver initial data.
* @return the pointer to Data object.
*/
const std::unique_ptr<OsqpEigen::Data>& data() const;
#ifdef OSQP_EIGEN_OSQP_IS_V1
/**
* Get the pointer to the OSQP solver.
* @return the pointer to Solver object.
*/
const std::unique_ptr<OSQPSolver, std::function<void(OSQPSolver*)>>& solver() const;
#else
/**
* Get the pointer to the OSQP workspace.
* @return the pointer to Workspace object.
*/
const std::unique_ptr<OSQPWorkspace, std::function<void(OSQPWorkspace*)>>& workspace() const;
#endif
};
#include <OsqpEigen/Solver.tpp>
} // namespace OsqpEigen
#endif