qpmad
Eigen-based C++ QP solver.
solver.h
Go to the documentation of this file.
1 /**
2  @file
3  @author Alexander Sherikov
4 
5  @copyright 2017 Alexander Sherikov. Licensed under the Apache License,
6  Version 2.0. (see LICENSE or http://www.apache.org/licenses/LICENSE-2.0)
7 
8  @brief
9 */
10 
11 
12 #pragma once
13 
14 #include <vector>
15 
16 #include "common.h"
17 #include "cholesky.h"
18 #include "givens.h"
19 #include "input_parser.h"
20 #include "inverse.h"
21 #include "solver_parameters.h"
22 #include "constraint_status.h"
23 #include "chosen_constraint.h"
24 #include "active_set.h"
25 #include "factorization_data.h"
26 
27 #ifdef QPMAD_ENABLE_TRACING
28 #include "testing.h"
29 #endif
30 
31 namespace qpmad
32 {
33  class Solver : public InputParser
34  {
35  public:
37  {
38  OK = 0,
43  };
44 
45 
46  public:
48  QPMatrix & H,
49  const QPVector & h,
50  const QPMatrix & A,
51  const QPVector & Alb,
52  const QPVector & Aub)
53  {
54  return (solve( primal, H, h,
55  Eigen::VectorXd(), Eigen::VectorXd(),
56  A, Alb, Aub, SolverParameters()));
57  }
58 
59 
61  QPMatrix & H,
62  const QPVector & h,
63  const QPVector & lb,
64  const QPVector & ub,
65  const QPMatrix & A,
66  const QPVector & Alb,
67  const QPVector & Aub)
68  {
69  return (solve( primal, H, h,
70  lb, ub,
71  A, Alb, Aub, SolverParameters()));
72  }
73 
74 
76  QPMatrix & H,
77  const QPVector & h,
78  const QPVector & lb,
79  const QPVector & ub)
80  {
81  return (solve( primal, H, h,
82  lb, ub,
83  Eigen::MatrixXd(), Eigen::VectorXd(), Eigen::VectorXd(), SolverParameters()));
84  }
85 
86 
87 
89  QPMatrix & H,
90  const QPVector & h,
91  const QPVector & lb,
92  const QPVector & ub,
93  const QPMatrix & A,
94  const QPVector & Alb,
95  const QPVector & Aub,
96  const SolverParameters & param)
97  {
98  QPMAD_TRACE(std::setprecision(std::numeric_limits<double>::digits10));
99 
100  machinery_initialized_ = false;
101 
102  parseObjective(H, h);
103  parseSimpleBounds(lb, ub);
104  parseGeneralConstraints(A, Alb, Aub);
105 
106 
107  switch(param.hessian_type_)
108  {
111  // no break here!
113  break;
114 
115  default:
116  QPMAD_UTILS_THROW("Malformed solver parameters!");
117  break;
118  }
119 
120 
121  // Unconstrained optimum
122  if (h_size_ > 0)
123  {
124  CholeskyFactorization::solve(primal, H, -h);
125  }
126  else
127  {
128  primal.setZero(primal_size_);
129  }
130 
131 
133 
134  if (0 == num_constraints_)
135  {
136  // exit early -- avoid unnecessary memory allocations
137  return (OK);
138  }
139 
140 
141 
142  // check consistency of general constraints and activate
143  // equality constraints
145  MatrixIndex num_equalities = 0;
146  for (MatrixIndex i = 0; i < num_constraints_; ++i)
147  {
149 
150  double lb_i;
151  double ub_i;
152 
153  if (true == chosen_ctr_.is_simple_)
154  {
155  lb_i = lb(i);
156  ub_i = ub(i);
157  }
158  else
159  {
163  }
164 
165 
166  if (lb_i - param.tolerance_ > ub_i)
167  {
169  return(INCONSISTENT);
170  }
171 
172  if (std::abs(lb_i - ub_i) > param.tolerance_)
173  {
175  }
176  else
177  {
179  ++num_equalities;
180 
181 
182  if (true == chosen_ctr_.is_simple_)
183  {
184  chosen_ctr_.violation_ = lb_i - primal(i);
185  }
186  else
187  {
189  }
190 
192 
193  // if 'primal_size_' constraints are already activated
194  // all other constraints are linearly dependent
196  {
197  double ctr_i_dot_primal_step_direction;
198 
199  if (true == chosen_ctr_.is_simple_)
200  {
203 
204  ctr_i_dot_primal_step_direction = primal_step_direction_(i);
205  }
206  else
207  {
210 
211  ctr_i_dot_primal_step_direction = A.row(chosen_ctr_.general_constraint_index_) * primal_step_direction_;
212  }
213 
214  // if step direction is a zero vector, constraint is
215  // linearly dependent with previously added constraints
216  if (ctr_i_dot_primal_step_direction < -param.tolerance_)
217  {
218  double primal_step_length_ = chosen_ctr_.violation_ / ctr_i_dot_primal_step_direction;
219 
220  primal.noalias() += primal_step_length_ * primal_step_direction_;
221 
223  {
224  QPMAD_UTILS_THROW("Failed to add an equality constraint -- is this possible?");
225  }
227 
228  continue;
229  }
230  // otherwise -- linear dependence
231  }
232 
233  // this point is reached if constraint is linearly dependent
234 
235  // check if this constraint is actually satisfied
236  if (std::abs(chosen_ctr_.violation_) > param.tolerance_)
237  {
238  // nope it is not
240  return (INFEASIBLE_EQUALITY);
241  }
242  // otherwise keep going
243  }
244  }
245 
246 
247  if (num_equalities == num_constraints_)
248  {
249  // exit early -- avoid unnecessary memory allocations
250  return (OK);
251  }
252 
253 
254  dual_.resize(primal_size_);
256 
257 
258  ReturnStatus return_status;
259  chooseConstraint(primal, lb, ub, A, Alb, Aub, param.tolerance_);
260 
261 
262  if (std::abs(chosen_ctr_.violation_) < param.tolerance_)
263  {
264  // all constraints are satisfied
265  return_status = OK;
266  }
267  else
268  {
269  return_status = MAXIMAL_NUMBER_OF_ITERATIONS;
270 
272 
273 
274  double chosen_ctr_dot_primal_step_direction = 0.0;
275 
276  //
279  chosen_ctr_,
280  A,
281  active_set_);
283  {
284  // compute step direction in primal space
287  active_set_);
288  chosen_ctr_dot_primal_step_direction =
290  }
291 
292 
293  for(int iter = 0;
294  (iter < param.max_iter_) || (param.max_iter_ < 0);
295  ++iter)
296  {
297  QPMAD_TRACE(">>>>>>>>>" << iter << "<<<<<<<<<");
298 #ifdef QPMAD_ENABLE_TRACING
299  testing::computeObjective(H, h, primal);
300 #endif
301  QPMAD_TRACE("||| Chosen ctr index = " << chosen_ctr_.index_);
302  QPMAD_TRACE("||| Chosen ctr dual = " << chosen_ctr_.dual_);
303  QPMAD_TRACE("||| Chosen ctr violation = " << chosen_ctr_.violation_);
304 
305 
306  // check dual feasibility
307  MatrixIndex dual_blocking_index = primal_size_;
308  double dual_step_length = std::numeric_limits<double>::infinity();
310  {
311  if (dual_step_direction_(i) < -param.tolerance_)
312  {
313  double dual_step_length_i = -dual_(i) / dual_step_direction_(i);
314  if (dual_step_length_i < dual_step_length)
315  {
316  dual_step_length = dual_step_length_i;
317  dual_blocking_index = i;
318  }
319  }
320  }
321 
322 
323 #ifdef QPMAD_ENABLE_TRACING
325  H, h, primal, A,
326  active_set_,
329  dual_,
331 #endif
332 
333 
335  // if step direction is a zero vector, constraint is
336  // linearly dependent with previously added constraints
337  && (std::abs(chosen_ctr_dot_primal_step_direction) > param.tolerance_) )
338  {
339  double step_length = - chosen_ctr_.violation_ / chosen_ctr_dot_primal_step_direction;
340 
341  QPMAD_TRACE("======================");
342  QPMAD_TRACE("||| Primal step length = " << step_length);
343  QPMAD_TRACE("||| Dual step length = " << dual_step_length);
344  QPMAD_TRACE("======================");
345 
346 
347  bool partial_step = false;
348  QPMAD_UTILS_ASSERT( (step_length >= 0.0)
349  && (dual_step_length >= 0.0),
350  "Non-negative step lengths expected.");
351  if (dual_step_length <= step_length)
352  {
353  step_length = dual_step_length;
354  partial_step = true;
355  }
356 
357 
358  primal.noalias() += step_length * primal_step_direction_;
359 
361  += step_length
363  chosen_ctr_.dual_ += step_length;
364  chosen_ctr_.violation_ += step_length * chosen_ctr_dot_primal_step_direction;
365 
366  QPMAD_TRACE("||| Chosen ctr dual = " << chosen_ctr_.dual_);
367  QPMAD_TRACE("||| Chosen ctr violation = " << chosen_ctr_.violation_);
368 
369 
370  if ((partial_step)
371  // if violation is almost zero -- assume that a full step is made
372  && (std::abs(chosen_ctr_.violation_) > param.tolerance_) )
373  {
374  QPMAD_TRACE("||| PARTIAL STEP");
375  // deactivate blocking constraint
377 
378  dropElementWithoutResize(dual_, dual_blocking_index, active_set_.size_);
379 
380  factorization_data_.downdate(dual_blocking_index, active_set_.size_);
381 
382  active_set_.removeInequality(dual_blocking_index);
383 
384  // compute step direction in primal & dual space
388  active_set_);
389  chosen_ctr_dot_primal_step_direction =
391  }
392  else
393  {
394  QPMAD_TRACE("||| FULL STEP");
395  // activate constraint
397  {
398  QPMAD_UTILS_THROW("Failed to add an inequality constraint -- is this possible?");
399  }
400 
402  {
404  }
405  else
406  {
408  }
411 
412  chooseConstraint(primal, lb, ub, A, Alb, Aub, param.tolerance_);
413 
414  if (std::abs(chosen_ctr_.violation_) < param.tolerance_)
415  {
416  // all constraints are satisfied
417  return_status = OK;
418  break;
419  }
420 
421  chosen_ctr_dot_primal_step_direction = 0.0;
424  chosen_ctr_,
425  A,
426  active_set_);
428  {
429  // compute step direction in primal & dual space
432  active_set_);
433  chosen_ctr_dot_primal_step_direction =
435  }
436  }
437  }
438  else
439  {
440  if (dual_blocking_index == primal_size_)
441  {
442  return_status = INFEASIBLE_INEQUALITY;
443  break;
444  }
445  else
446  {
447  QPMAD_TRACE("======================");
448  QPMAD_TRACE("||| Dual step length = " << dual_step_length);
449  QPMAD_TRACE("======================");
450 
451  // otherwise -- deactivate
453  += dual_step_length
455  chosen_ctr_.dual_ += dual_step_length;
456 
458 
459  dropElementWithoutResize(dual_, dual_blocking_index, active_set_.size_);
460 
461  factorization_data_.downdate(dual_blocking_index, active_set_.size_);
462 
463  active_set_.removeInequality(dual_blocking_index);
464 
465  // compute step direction in primal & dual space
469  active_set_);
470  chosen_ctr_dot_primal_step_direction =
472  }
473  }
474  }
475  }
476 
477 #ifdef QPMAD_ENABLE_TRACING
479  {
481 
483  H, h, primal, A,
484  active_set_,
487  dual_);
488  }
489  else
490  {
491  QPMAD_TRACE("||| NO ACTIVE CONSTRAINTS");
492  }
493 #endif
494  return(return_status);
495  }
496 
497 
498  private:
501 
504 
506 
509 
511 
512  std::vector<ConstraintStatus::Status> constraints_status_;
513 
515 
516 
517  private:
518  template <class t_MatrixType>
519  void initializeMachineryLazy(const t_MatrixType &H)
520  {
521  if (false == machinery_initialized_)
522  {
527 
528  machinery_initialized_ = true;
529  }
530  }
531 
532 
534  const QPVector & primal,
535  const QPVector & lb,
536  const QPVector & ub,
537  const QPMatrix & A,
538  const QPVector & Alb,
539  const QPVector & Aub,
540  const double tolerance)
541  {
542  chosen_ctr_.reset();
543 
544 
545  for(MatrixIndex i = 0; i < num_simple_bounds_; ++i)
546  {
548  {
549  checkConstraintViolation(i, lb(i), ub(i), primal(i));
550  }
551  }
552 
553  if ((std::abs(chosen_ctr_.violation_) < tolerance) && (num_general_constraints_ > 0))
554  {
555  general_ctr_dot_primal_.noalias() = A * primal;
557  {
559  {
561  Alb(i - num_simple_bounds_),
562  Aub(i - num_simple_bounds_),
564  }
565  }
567  {
569  }
570  }
571 
574  }
575 
576 
578  const MatrixIndex i,
579  const double lb_i,
580  const double ub_i,
581  const double ctr_i_dot_primal)
582  {
583  double ctr_violation_i = ctr_i_dot_primal - lb_i;
584  if (ctr_violation_i < -std::abs(chosen_ctr_.violation_))
585  {
586  chosen_ctr_.violation_ = ctr_violation_i;
587  chosen_ctr_.index_ = i;
588  }
589  else
590  {
591  ctr_violation_i = ctr_i_dot_primal - ub_i;
592  if (ctr_violation_i > std::abs(chosen_ctr_.violation_))
593  {
594  chosen_ctr_.violation_ = ctr_violation_i;
595  chosen_ctr_.index_ = i;
596  }
597  }
598  }
599 
600 
601  template < class t_VectorType,
602  class t_MatrixType>
604  const t_VectorType & primal_step_direction,
605  const t_MatrixType & A) const
606  {
608  {
609  return(primal_step_direction(chosen_ctr_.index_));
610  }
611  else
612  {
613  return(A.row(chosen_ctr_.general_constraint_index_) * primal_step_direction);
614  }
615  }
616  };
617 }
#define QPMAD_UTILS_THROW(s)
bool hasEmptySpace() const
Definition: active_set.h:41
double computeObjective(const Eigen::MatrixXd &H, const Eigen::VectorXd &h, const Eigen::VectorXd &primal)
Definition: testing.h:21
ActiveSet active_set_
Definition: solver.h:502
ReturnStatus solve(QPVector &primal, QPMatrix &H, const QPVector &h, const QPVector &lb, const QPVector &ub)
Definition: solver.h:75
void initialize(const MatrixIndex max_size)
Definition: active_set.h:26
QPVector general_ctr_dot_primal_
Definition: solver.h:510
#define QPMAD_TRACE(info)
Definition: common.h:26
MatrixIndex general_constraint_index_
void parseObjective(const t_MatrixType &H, const t_VectorType &h)
Definition: input_parser.h:36
void printActiveSet(const ActiveSet &active_set, const std::vector< ConstraintStatus::Status > &constraints_status, const Eigen::VectorXd &dual)
Definition: testing.h:162
FactorizationData factorization_data_
Definition: solver.h:503
void chooseConstraint(const QPVector &primal, const QPVector &lb, const QPVector &ub, const QPMatrix &A, const QPVector &Alb, const QPVector &Aub, const double tolerance)
Definition: solver.h:533
QPVector dual_step_direction_
Definition: solver.h:508
ReturnStatus solve(QPVector &primal, QPMatrix &H, const QPVector &h, const QPVector &lb, const QPVector &ub, const QPMatrix &A, const QPVector &Alb, const QPVector &Aub, const SolverParameters &param)
Definition: solver.h:88
MatrixIndex num_simple_bounds_
Definition: input_parser.h:20
Eigen::Matrix< double, Eigen::Dynamic, 1 > QPVector
Definition: common.h:35
int MatrixIndex
Definition: common.h:32
MatrixIndex h_size_
Definition: input_parser.h:19
MatrixIndex num_inequalities_
Definition: active_set.h:22
void initializeMachineryLazy(const t_MatrixType &H)
Definition: solver.h:519
static void compute(t_MatrixType &M)
Definition: cholesky.h:27
void checkLagrangeMultipliers(const Eigen::MatrixXd &H, const Eigen::VectorXd &h, const Eigen::VectorXd &primal, const Eigen::MatrixXd &A, const ActiveSet &active_set, const MatrixIndex &num_simple_bounds, const std::vector< ConstraintStatus::Status > &constraints_status, const Eigen::VectorXd &dual, const Eigen::VectorXd &dual_direction=Eigen::VectorXd())
Definition: testing.h:40
MatrixIndex num_general_constraints_
Definition: input_parser.h:21
ReturnStatus solve(QPVector &primal, QPMatrix &H, const QPVector &h, const QPMatrix &A, const QPVector &Alb, const QPVector &Aub)
Definition: solver.h:47
void downdate(const MatrixIndex R_col_index, const MatrixIndex R_cols)
QPVector dual_
Definition: solver.h:505
void updateStepsAfterPartialStep(t_VectorType0 &primal_step_direction, t_VectorType1 &dual_step_direction, const ActiveSet &active_set)
ReturnStatus solve(QPVector &primal, QPMatrix &H, const QPVector &h, const QPVector &lb, const QPVector &ub, const QPMatrix &A, const QPVector &Alb, const QPVector &Aub)
Definition: solver.h:60
void addEquality(const MatrixIndex index)
Definition: active_set.h:47
static void solve(t_OutputVectorType &x, t_InputMatrixType0 &L, t_InputMatrixType1 &v)
Definition: cholesky.h:47
void addInequality(const MatrixIndex index)
Definition: active_set.h:55
QPVector primal_step_direction_
Definition: solver.h:507
MatrixIndex getIndex(const MatrixIndex index) const
Definition: active_set.h:35
void updateStepsAfterPureDualStep(t_VectorType0 &primal_step_direction, t_VectorType1 &dual_step_direction, const ActiveSet &active_set)
bool machinery_initialized_
Definition: solver.h:500
MatrixIndex num_constraints_
Definition: solver.h:499
bool update(const MatrixIndex R_col, const bool is_simple, const double tolerance)
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > QPMatrix
Definition: common.h:34
#define QPMAD_UTILS_ASSERT(condition, message)
std::vector< ConstraintStatus::Status > constraints_status_
Definition: solver.h:512
MatrixIndex num_equalities_
Definition: active_set.h:21
MatrixIndex primal_size_
Definition: input_parser.h:18
void initialize(const t_MatrixType &H, const MatrixIndex primal_size)
void removeInequality(const MatrixIndex index)
Definition: active_set.h:63
ChosenConstraint chosen_ctr_
Definition: solver.h:514
void computeEqualityPrimalStep(t_VectorType &step_direction, const MatrixIndex simple_bound_index, const MatrixIndex active_set_size)
void parseGeneralConstraints(const t_MatrixTypeA &A, const t_VectorTypelb &lb, const t_VectorTypeub &ub)
Definition: input_parser.h:81
void computeInequalityDualStep(t_VectorType &dual_step_direction, const ChosenConstraint &chosen_ctr, const t_MatrixType &A, const ActiveSet &active_set)
void dropElementWithoutResize(t_VectorType &vector, const MatrixIndex index, const MatrixIndex size)
Definition: common.h:39
void computeInequalityPrimalStep(t_VectorType0 &primal_step_direction, const ActiveSet &active_set)
void parseSimpleBounds(const t_VectorTypelb &lb, const t_VectorTypeub &ub)
Definition: input_parser.h:57
MatrixIndex size_
Definition: active_set.h:20
void checkConstraintViolation(const MatrixIndex i, const double lb_i, const double ub_i, const double ctr_i_dot_primal)
Definition: solver.h:577
double getConstraintDotPrimalStepDirection(const t_VectorType &primal_step_direction, const t_MatrixType &A) const
Definition: solver.h:603