26 #include "piqp_interface.hpp"
27 #include "casadi/core/casadi_misc.hpp"
34 int CASADI_CONIC_PIQP_EXPORT
36 plugin->creator = PiqpInterface::creator;
37 plugin->name =
"piqp";
38 plugin->doc = PiqpInterface::meta_doc.c_str();
39 plugin->version = CASADI_VERSION;
40 plugin->options = &PiqpInterface::options_;
41 plugin->deserialize = &PiqpInterface::deserialize;
52 if (s ==
"dense_cholesky")
return piqp::KKTSolver::dense_cholesky;
53 if (s ==
"sparse_ldlt")
return piqp::KKTSolver::sparse_ldlt;
54 if (s ==
"sparse_ldlt_eq_cond")
return piqp::KKTSolver::sparse_ldlt_eq_cond;
55 if (s ==
"sparse_ldlt_ineq_cond")
return piqp::KKTSolver::sparse_ldlt_ineq_cond;
56 if (s ==
"sparse_ldlt_cond")
return piqp::KKTSolver::sparse_ldlt_cond;
57 if (s ==
"sparse_multistage")
return piqp::KKTSolver::sparse_multistage;
58 casadi_error(
"Unknown kkt_solver '" + s +
"'. Choose one of: dense_cholesky, "
59 "sparse_ldlt, sparse_ldlt_eq_cond, sparse_ldlt_ineq_cond, sparse_ldlt_cond, "
60 "sparse_multistage.");
61 return piqp::KKTSolver::sparse_ldlt;
64 PiqpInterface::PiqpInterface(
const std::string& name,
65 const std::map<std::string, Sparsity>& st)
68 settings_.kkt_solver = piqp::KKTSolver::sparse_ldlt;
79 "const Options to be passed to piqp."}},
88 for (
auto&& op : opts) {
89 if (op.first==
"piqp") {
90 const Dict& opts = op.second;
91 for (
auto&& op : opts) {
92 if (op.first ==
"rho_init") {
94 }
else if (op.first ==
"delta_init") {
96 }
else if (op.first ==
"eps_abs") {
98 }
else if (op.first ==
"eps_rel") {
100 }
else if (op.first ==
"check_duality_gap") {
102 }
else if (op.first ==
"eps_duality_gap_abs") {
103 settings_.eps_duality_gap_abs = op.second;
104 }
else if (op.first ==
"eps_duality_gap_rel") {
105 settings_.eps_duality_gap_rel = op.second;
106 }
else if (op.first ==
"reg_lower_limit") {
108 }
else if (op.first ==
"reg_finetune_lower_limit") {
109 settings_.reg_finetune_lower_limit = op.second;
110 }
else if (op.first ==
"reg_finetune_primal_update_threshold") {
111 settings_.reg_finetune_primal_update_threshold =
static_cast<piqp::isize
>(
113 }
else if (op.first ==
"reg_finetune_dual_update_threshold") {
114 settings_.reg_finetune_dual_update_threshold =
static_cast<piqp::isize
>(
116 }
else if (op.first ==
"max_iter") {
117 settings_.max_iter =
static_cast<piqp::isize
>(op.second.to_int());
118 }
else if (op.first ==
"max_factor_retires") {
119 settings_.max_factor_retires =
static_cast<piqp::isize
>(
121 }
else if (op.first ==
"preconditioner_scale_cost") {
122 settings_.preconditioner_scale_cost = op.second;
123 }
else if (op.first ==
"preconditioner_iter") {
124 settings_.preconditioner_iter =
static_cast<piqp::isize
>(
126 }
else if (op.first ==
"tau") {
128 }
else if (op.first ==
"iterative_refinement_always_enabled") {
129 settings_.iterative_refinement_always_enabled = op.second;
130 }
else if (op.first ==
"iterative_refinement_eps_abs") {
131 settings_.iterative_refinement_eps_abs = op.second;
132 }
else if (op.first ==
"iterative_refinement_eps_rel") {
133 settings_.iterative_refinement_eps_rel = op.second;
134 }
else if (op.first ==
"iterative_refinement_max_iter") {
135 settings_.iterative_refinement_max_iter =
static_cast<piqp::isize
>(
137 }
else if (op.first ==
"iterative_refinement_min_improvement_rate") {
138 settings_.iterative_refinement_min_improvement_rate = op.second;
139 }
else if (op.first ==
"iterative_refinement_static_regularization_eps") {
140 settings_.iterative_refinement_static_regularization_eps = op.second;
141 }
else if (op.first ==
"iterative_refinement_static_regularization_rel") {
142 settings_.iterative_refinement_static_regularization_rel = op.second;
143 }
else if (op.first ==
"verbose") {
145 }
else if (op.first ==
"compute_timings") {
147 }
else if (op.first ==
"kkt_solver") {
150 casadi_error(
"Unrecognised PIQP option '" + op.first +
"'.");
179 m->tripletListEq.reserve(
na_);
181 m->g_vector.resize(
nx_);
182 m->uba_vector.resize(
na_);
183 m->lba_vector.resize(
na_);
184 m->ubx_vector.resize(
nx_);
185 m->lbx_vector.resize(
nx_);
186 m->eq_b_vector.resize(
na_);
188 m->add_stat(
"preprocessing");
189 m->add_stat(
"solver");
190 m->add_stat(
"postprocessing");
195 solve(
const double** arg,
double** res, casadi_int* iw,
double* w,
void* mem)
const {
196 typedef Eigen::Triplet<double> TripletT;
199 m->
fstats.at(
"preprocessing").tic();
202 double* g=w; w +=
nx_;
204 double* lbx=w; w +=
nx_;
206 double* ubx=w; w +=
nx_;
208 double* lba=w; w +=
na_;
210 double* uba=w; w +=
na_;
217 m->g_vector = Eigen::Map<const Eigen::VectorXd>(g,
nx_);
218 m->uba_vector = Eigen::Map<Eigen::VectorXd>(uba,
na_);
219 m->lba_vector = Eigen::Map<Eigen::VectorXd>(lba,
na_);
220 m->ubx_vector = Eigen::Map<Eigen::VectorXd>(ubx,
nx_);
221 m->lbx_vector = Eigen::Map<Eigen::VectorXd>(lbx,
nx_);
225 const Eigen::Array<bool, Eigen::Dynamic, 1>
226 is_equality = (m->uba_vector.array() == m->lba_vector.array()).
eval();
229 std::vector<unsigned int> number_of_prev_equality(
na_, 0);
230 std::vector<unsigned int> number_of_prev_inequality(
na_, 0);
231 std::vector<double> tmp_eq_vector;
232 std::vector<double> tmp_ineq_lb_vector;
233 std::vector<double> tmp_ineq_ub_vector;
235 for (std::size_t k = 1; k < static_cast<std::size_t>(
na_); ++k) {
236 if (is_equality[k-1]) {
237 number_of_prev_equality[k] = number_of_prev_equality[k-1] + 1;
238 number_of_prev_inequality[k] = number_of_prev_inequality[k-1];
240 number_of_prev_equality[k] = number_of_prev_equality[k-1];
241 number_of_prev_inequality[k] = number_of_prev_inequality[k-1] + 1;
245 for (casadi_int k = 0; k <
na_; ++k) {
246 if (is_equality[k]) {
247 tmp_eq_vector.push_back(m->lba_vector[k]);
249 tmp_ineq_lb_vector.push_back(m->lba_vector[k]);
250 tmp_ineq_ub_vector.push_back(m->uba_vector[k]);
254 m->eq_b_vector.resize(tmp_eq_vector.size());
255 if (tmp_eq_vector.size() > 0) {
256 m->eq_b_vector = Eigen::Map<Eigen::VectorXd>(
257 get_ptr(tmp_eq_vector), tmp_eq_vector.size());
261 Eigen::VectorXd ineq_lb_vector(tmp_ineq_lb_vector.size());
262 Eigen::VectorXd ineq_ub_vector(tmp_ineq_ub_vector.size());
263 if (tmp_ineq_lb_vector.size() > 0) {
264 ineq_lb_vector = Eigen::Map<Eigen::VectorXd>(
265 get_ptr(tmp_ineq_lb_vector), tmp_ineq_lb_vector.size());
266 ineq_ub_vector = Eigen::Map<Eigen::VectorXd>(
267 get_ptr(tmp_ineq_ub_vector), tmp_ineq_ub_vector.size());
270 std::size_t n_eq = m->eq_b_vector.size();
271 std::size_t n_ineq = tmp_ineq_lb_vector.size();
275 for (
int k=0; k<
H_.
nnz(); ++k) {
277 static_cast<double>(m->row[k]),
278 static_cast<double>(m->col[k]),
279 static_cast<double>(H[k])));
282 H_spa.setFromTriplets(m->tripletList.begin(), m->tripletList.end());
283 m->tripletList.clear();
287 m->tripletList.reserve(
A_.
nnz());
289 for (
int k=0; k<
A_.
nnz(); ++k) {
291 if (is_equality[m->row[k]]) {
292 m->tripletListEq.push_back(TripletT(
293 static_cast<double>(number_of_prev_equality[m->row[k]]),
294 static_cast<double>(m->col[k]),
295 static_cast<double>(A[k])));
298 m->tripletList.push_back(TripletT(
299 static_cast<double>(number_of_prev_inequality[m->row[k]]),
300 static_cast<double>(m->col[k]),
301 static_cast<double>(A[k])));
306 Eigen::SparseMatrix<double> A_spa(n_eq,
nx_);
307 A_spa.setFromTriplets(m->tripletListEq.begin(), m->tripletListEq.end());
308 m->tripletListEq.clear();
311 Eigen::SparseMatrix<double> G_spa(n_ineq,
nx_);
312 G_spa.setFromTriplets(m->tripletList.begin(), m->tripletList.end());
313 m->tripletList.clear();
315 m->fstats.at(
"preprocessing").toc();
319 m->fstats.at(
"solver").tic();
321 piqp::SparseSolver<double> solver;
326 A_spa, m->eq_b_vector,
327 G_spa, ineq_lb_vector, ineq_ub_vector,
328 m->lbx_vector, m->ubx_vector);
329 m->status = solver.solve();
331 m->results_x = std::make_unique<Eigen::VectorXd>(solver.result().x);
332 m->results_y = std::make_unique<Eigen::VectorXd>(solver.result().y);
335 m->results_z = std::make_unique<Eigen::VectorXd>(
336 solver.result().z_u - solver.result().z_l);
338 m->results_lam_x = std::make_unique<Eigen::VectorXd>(
339 solver.result().z_bu - solver.result().z_bl);
340 m->objValue = solver.result().info.primal_obj;
342 piqp::DenseSolver<double> solver;
345 Eigen::MatrixXd(H_spa), m->g_vector,
346 Eigen::MatrixXd(A_spa), m->eq_b_vector,
347 Eigen::MatrixXd(G_spa), ineq_lb_vector, ineq_ub_vector,
348 m->lbx_vector, m->ubx_vector);
349 m->status = solver.solve();
351 m->results_x = std::make_unique<Eigen::VectorXd>(solver.result().x);
352 m->results_y = std::make_unique<Eigen::VectorXd>(solver.result().y);
353 m->results_z = std::make_unique<Eigen::VectorXd>(
354 solver.result().z_u - solver.result().z_l);
355 m->results_lam_x = std::make_unique<Eigen::VectorXd>(
356 solver.result().z_bu - solver.result().z_bl);
357 m->objValue = solver.result().info.primal_obj;
359 m->fstats.at(
"solver").toc();
362 m->fstats.at(
"postprocessing").tic();
370 if (n_ineq + n_eq > 0) {
371 Eigen::VectorXd lam_a(
na_);
373 for (casadi_int k = 0; k <
na_; ++k) {
374 if (is_equality[k]) {
375 lam_a[k] = m->results_y->coeff(number_of_prev_equality[k]);
377 lam_a[k] = m->results_z->coeff(number_of_prev_inequality[k]);
387 m->d_qp.success = m->status == piqp::Status::PIQP_SOLVED;
388 if (m->d_qp.success) {
391 if (m->status == piqp::Status::PIQP_MAX_ITER_REACHED) {
397 m->fstats.at(
"postprocessing").toc();
406 stats[
"return_status"] = piqp::status_to_string(m->status);
425 s.
unpack(
"PiqpInterface::settings::check_duality_gap",
settings_.check_duality_gap);
426 s.
unpack(
"PiqpInterface::settings::eps_duality_gap_abs",
settings_.eps_duality_gap_abs);
427 s.
unpack(
"PiqpInterface::settings::eps_duality_gap_rel",
settings_.eps_duality_gap_rel);
428 s.
unpack(
"PiqpInterface::settings::reg_lower_limit",
settings_.reg_lower_limit);
429 s.
unpack(
"PiqpInterface::settings::reg_finetune_lower_limit",
431 s.
unpack(
"PiqpInterface::settings::reg_finetune_primal_update_threshold", tmp);
432 settings_.reg_finetune_primal_update_threshold = tmp;
433 s.
unpack(
"PiqpInterface::settings::reg_finetune_dual_update_threshold", tmp);
434 settings_.reg_finetune_dual_update_threshold = tmp;
435 s.
unpack(
"PiqpInterface::settings::max_iter", tmp);
437 s.
unpack(
"PiqpInterface::settings::max_factor_retires", tmp);
439 s.
unpack(
"PiqpInterface::settings::preconditioner_scale_cost",
441 s.
unpack(
"PiqpInterface::settings::preconditioner_iter", tmp);
444 s.
unpack(
"PiqpInterface::settings::iterative_refinement_always_enabled",
445 settings_.iterative_refinement_always_enabled);
446 s.
unpack(
"PiqpInterface::settings::iterative_refinement_eps_abs",
448 s.
unpack(
"PiqpInterface::settings::iterative_refinement_eps_rel",
450 s.
unpack(
"PiqpInterface::settings::iterative_refinement_max_iter", tmp);
451 settings_.iterative_refinement_max_iter = tmp;
452 s.
unpack(
"PiqpInterface::settings::iterative_refinement_min_improvement_rate",
453 settings_.iterative_refinement_min_improvement_rate);
454 s.
unpack(
"PiqpInterface::settings::iterative_refinement_static_regularization_eps",
455 settings_.iterative_refinement_static_regularization_eps);
456 s.
unpack(
"PiqpInterface::settings::iterative_refinement_static_regularization_rel",
457 settings_.iterative_refinement_static_regularization_rel);
459 s.
unpack(
"PiqpInterface::settings::compute_timings",
settings_.compute_timings);
460 std::string kkt_solver;
461 s.
unpack(
"PiqpInterface::settings::kkt_solver", kkt_solver);
472 s.
pack(
"PiqpInterface::settings::rho_init",
settings_.rho_init);
473 s.
pack(
"PiqpInterface::settings::delta_init",
settings_.delta_init);
474 s.
pack(
"PiqpInterface::settings::eps_abs",
settings_.eps_abs);
475 s.
pack(
"PiqpInterface::settings::eps_rel",
settings_.eps_rel);
476 s.
pack(
"PiqpInterface::settings::check_duality_gap",
settings_.check_duality_gap);
477 s.
pack(
"PiqpInterface::settings::eps_duality_gap_abs",
settings_.eps_duality_gap_abs);
478 s.
pack(
"PiqpInterface::settings::eps_duality_gap_rel",
settings_.eps_duality_gap_rel);
479 s.
pack(
"PiqpInterface::settings::reg_lower_limit",
settings_.reg_lower_limit);
480 s.
pack(
"PiqpInterface::settings::reg_finetune_lower_limit",
482 tmp =
settings_.reg_finetune_primal_update_threshold;
483 s.
pack(
"PiqpInterface::settings::reg_finetune_primal_update_threshold", tmp);
484 tmp =
settings_.reg_finetune_dual_update_threshold;
485 s.
pack(
"PiqpInterface::settings::reg_finetune_dual_update_threshold", tmp);
487 s.
pack(
"PiqpInterface::settings::max_iter", tmp);
489 s.
pack(
"PiqpInterface::settings::max_factor_retires", tmp);
490 s.
pack(
"PiqpInterface::settings::preconditioner_scale_cost",
493 s.
pack(
"PiqpInterface::settings::preconditioner_iter", tmp);
495 s.
pack(
"PiqpInterface::settings::iterative_refinement_always_enabled",
496 settings_.iterative_refinement_always_enabled);
497 s.
pack(
"PiqpInterface::settings::iterative_refinement_eps_abs",
499 s.
pack(
"PiqpInterface::settings::iterative_refinement_eps_rel",
501 tmp =
settings_.iterative_refinement_max_iter;
502 s.
pack(
"PiqpInterface::settings::iterative_refinement_max_iter", tmp);
503 s.
pack(
"PiqpInterface::settings::iterative_refinement_min_improvement_rate",
504 settings_.iterative_refinement_min_improvement_rate);
505 s.
pack(
"PiqpInterface::settings::iterative_refinement_static_regularization_eps",
506 settings_.iterative_refinement_static_regularization_eps);
507 s.
pack(
"PiqpInterface::settings::iterative_refinement_static_regularization_rel",
508 settings_.iterative_refinement_static_regularization_rel);
509 s.
pack(
"PiqpInterface::settings::verbose",
settings_.verbose);
510 s.
pack(
"PiqpInterface::settings::compute_timings",
settings_.compute_timings);
511 s.
pack(
"PiqpInterface::settings::kkt_solver",
512 std::string(piqp::kkt_solver_to_string(
settings_.kkt_solver)));
static const Options options_
Options.
casadi_int nx_
Number of decision variables.
void finalize() override
Finalize the object creation.
int init_mem(void *mem) const override
Initalize memory block.
casadi_int na_
The number of constraints (counting both equality and inequality) == A.size1()
Sparsity H_
Problem structure.
void init(const Dict &opts) override
Initialize.
void serialize_body(SerializingStream &s) const override
Serialize an object without type information.
Dict get_stats(void *mem) const override
Get all statistics.
int eval(const double **arg, double **res, casadi_int *iw, double *w, void *mem) const final
Solve the QP.
Helper class for Serialization.
void unpack(Sparsity &e)
Reconstruct an object from the input stream.
void version(const std::string &name, int v)
casadi_int nnz_in() const
Number of input/output nonzeros.
void alloc_w(size_t sz_w, bool persistent=false)
Ensure required length of w field.
~PiqpInterface() override
Destructor.
piqp::Settings< double > settings_
static const Options options_
const Options
int solve(const double **arg, double **res, casadi_int *iw, double *w, void *mem) const override
Solve the QP.
PiqpInterface(const std::string &name, const std::map< std::string, Sparsity > &st)
Create a new Solver.
void init(const Dict &opts) override
Initialize.
Dict get_stats(void *mem) const override
Get all statistics.
void serialize_body(SerializingStream &s) const override
Serialize an object without type information.
int init_mem(void *mem) const override
Initalize memory block.
void finalize() override
Finalize.
void clear_mem()
Clear all memory (called from destructor)
Helper class for Serialization.
void version(const std::string &name, int v)
void pack(const Sparsity &e)
Serializes an object to the output stream.
casadi_int size1() const
Get the number of rows.
casadi_int nnz() const
Get the number of (structural) non-zeros.
casadi_int size2() const
Get the number of columns.
void get_triplet(std::vector< casadi_int > &row, std::vector< casadi_int > &col) const
Get the sparsity in sparse triplet format.
@ CONIC_UBA
dense, (nc x 1)
@ CONIC_A
The matrix A: sparse, (nc x n) - product with x must be dense.
@ CONIC_G
The vector g: dense, (n x 1)
@ CONIC_LBA
dense, (nc x 1)
@ CONIC_UBX
dense, (n x 1)
@ CONIC_LBX
dense, (n x 1)
void casadi_copy(const T1 *x, casadi_int n, T1 *y)
COPY: y <-x.
piqp::KKTSolver kkt_solver_from_string(const std::string &s)
void CASADI_CONIC_PIQP_EXPORT casadi_load_conic_piqp()
GenericType::Dict Dict
C++ equivalent of Python's dict or MATLAB's struct.
T * get_ptr(std::vector< T > &v)
Get a pointer to the data contained in the vector.
int CASADI_CONIC_PIQP_EXPORT casadi_register_conic_piqp(Conic::Plugin *plugin)
@ CONIC_X
The primal solution.
@ CONIC_LAM_A
The dual solution corresponding to linear bounds.
@ CONIC_COST
The optimal cost.
@ CONIC_LAM_X
The dual solution corresponding to simple bounds.
Options metadata for a class.
std::vector< TripletT > tripletList
Eigen::Triplet< double > TripletT
std::map< std::string, FStats > fstats