Optimo  0.1.0
A C++ header library for optimization
 All Classes Functions Variables Pages
sparse_primal_dual_qp_api.h
1 // Copyright (C) 2014 Victor Fragoso <vfragoso@cs.ucsb.edu>
2 // All rights reserved.
3 //
4 // Redistribution and use in source and binary forms, with or without
5 // modification, are permitted provided that the following conditions are
6 // met:
7 //
8 // * Redistributions of source code must retain the above copyright
9 // notice, this list of conditions and the following disclaimer.
10 //
11 // * Redistributions in binary form must reproduce the above
12 // copyright notice, this list of conditions and the following
13 // disclaimer in the documentation and/or other materials provided
14 // with the distribution.
15 //
16 // * Neither the name of the University of California, Santa Barbara nor the
17 // names of its contributors may be used to endorse or promote products
18 // derived from this software without specific prior written permission.
19 //
20 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
23 // ARE DISCLAIMED. IN NO EVENT SHALL VICTOR FRAGOSO BE LIABLE FOR ANY DIRECT,
24 // INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
26 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
27 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
28 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 //
31 
32 #ifndef OPTIMO_SOLVERS_SPARSE_PRIMAL_DUAL_QP_API_H_
33 #define OPTIMO_SOLVERS_SPARSE_PRIMAL_DUAL_QP_API_H_
34 
35 #include <Eigen/Core>
36 #include <Eigen/SparseCore>
37 #include <Eigen/SPQRSupport>
38 #include "optimo/solvers/solver.h"
39 
40 namespace optimo {
41 namespace solvers {
42 using Eigen::Matrix;
43 using Eigen::DiagonalMatrix;
44 using Eigen::SparseMatrix;
45 using Eigen::Dynamic;
46 
48 
77 template <typename Scalar = double>
78 class SparsePrimalDualQP : public Solver<Scalar> {
79  public:
81  class Params {
82  // To access and modify F
83  friend class SparsePrimalDualQP;
84 
85  public:
87  Params(const uint n ,
88  const uint m ,
89  const uint p ) :
90  n_(n), m_(m), p_(p), l_(n + m + p),
91  beq(p), bin(m), d(n), F_(n + m + p, n + m + p) { }
92 
93  // Destructor
94  virtual ~Params(void) { }
95 
96  // Public members
97  Matrix<Scalar, Dynamic, 1> beq;
98  Matrix<Scalar, Dynamic, 1> bin;
99  Matrix<Scalar, Dynamic, 1> d;
100 
102  inline void reserve(void) {
103  F_.reserve(Eigen::VectorXi::Constant(l_, l_));
104  }
105 
107  inline bool setQElement(const uint i, const uint j, const Scalar& val) {
108  if (i >= n_ || j >= n_) return false;
109  if (val == static_cast<Scalar>(0.0)) return false;
110  F_.coeffRef(i, j) = val;
111  return true;
112  }
113 
115  inline bool setAeqElement(const uint i, const uint j, const Scalar& val) {
116  if (i >= p_ || j >= n_) return false;
117  if (val == static_cast<Scalar>(0.0)) return false;
118  const uint f_col = n_ + m_ + i; // It is transposed in F
119  const uint f_row = j; // It is transposed
120  F_.coeffRef(f_col, f_row) = val; // To first block-row of F
121  F_.coeffRef(f_row, f_col) = val; // To third block-row of F
122  return true;
123  }
124 
126  inline bool setAinElement(const uint i, const uint j, const Scalar& val) {
127  if (i >= this->m_ || j >= this->n_) return false;
128  if (val == static_cast<Scalar>(0.0)) return false;
129  const uint f_col = n_ + i;
130  const uint f_row = j;
131  F_.coeffRef(f_row, f_col) = val;
132  return true;
133  }
134 
135  private:
136  SparseMatrix<Scalar> F_; // KKT system
137  const uint n_; // Num. of variables
138  const uint m_; // Num. of ineq. constraints
139  const uint p_; // Nun. of eq. constraints
140  const uint l_; // l = n + m + p
141  };
142 
143  // Constructor
144  SparsePrimalDualQP(void) { }
145 
146  // Destructor
147  virtual ~SparsePrimalDualQP(void) {
148  F_ = nullptr;
149  beq_ = nullptr;
150  bin_ = nullptr;
151  d_ = nullptr;
152  y_ = nullptr;
153  }
154 
156  TERMINATION_TYPE operator()(Params* params,
157  Matrix<Scalar, Dynamic, 1>* y,
158  Scalar* min_value);
159 
160  protected:
161  inline bool feasibleStartingPoint(const Matrix<Scalar, Dynamic, 1>& y) {
162  // Useful blocks
163  const auto& x = y.block(0, 0, n_, 1); // Primal variables
164  const auto& Ain_t = F_->block(0, n_, n_, m_); // Ain transpose
165  const auto& Aeq_t = F_->block(0, n_ + m_, n_, p_); // Aeq transpose
166  Matrix<Scalar, Dynamic, 1> ineq_x = Ain_t.transpose()*x - *bin_;
167  for (int i = 0; i < m_; i++) if (ineq_x(i) > 0.0) return false;
168  // // TODO(vfragoso): Check if we can ignore the eq. constraints
169  // Matrix<Scalar, Dynamic, 1> eq_x = Aeq_t.transpose()*x - *beq_;
170  // for (int i = 0; i < p_; i++) {
171  // if (fabs(eq_x(i)) > this->options.eps_feas_) {
172  // return false;
173  // }
174  // }
175  return true;
176  }
177 
178  inline bool isF_empty(void) {
179  Scalar acc = static_cast<Scalar>(0.0);
180  for (int i = 0; i < F_->outerSize(); i++) {
181  for (typename SparseMatrix<Scalar>::InnerIterator it(*F_, i); it; ++it) {
182  acc += static_cast<Scalar>(fabs(it.value()));
183  }
184  }
185  return acc == static_cast<Scalar>(0.0);
186  }
187 
188  bool backTracking(const Scalar t,
189  const Scalar r_norm,
190  const Matrix<Scalar, Dynamic, 1>& ynt,
191  Matrix<Scalar, Dynamic, 1>* y_plus,
192  Matrix<Scalar, Dynamic, 1>* r_plus,
193  Scalar* s);
194 
195  void calculateResiduals(
196  const Matrix<Scalar, Dynamic, 1>& y,
197  const DiagonalMatrix<Scalar, Dynamic, Dynamic>& diag,
198  const Matrix<Scalar, Dynamic, 1> & fx,
199  const Scalar t_inv,
200  Matrix<Scalar, Dynamic, 1>* residuals);
201 
202  void buildKKTSystem(const Scalar mu,
203  Scalar* eta,
204  Scalar* t,
205  Matrix<Scalar, Dynamic, 1>* residuals,
206  Scalar* rd_norm,
207  Scalar* rp_norm);
208 
209  inline TERMINATION_TYPE
210  mapResult(const Eigen::SPQR<SparseMatrix<Scalar> >& solver) {
211  switch (solver.info()) {
212  case Eigen::NumericalIssue:
213  return NUMERICAL_ISSUE;
214  case Eigen::NoConvergence:
215  return NO_CONVERGENCE;
216  case Eigen::InvalidInput:
217  return INVALID_ARGUMENTS;
218  case Eigen::Success:
219  return SOLVED;
220  }
221  }
222 
223  private:
224  uint n_; // Num. of variables
225  uint m_; // Num. of ineq. constraints
226  uint p_; // Nun. of eq. constraints
227  uint l_; // l = n + m + p
228 
229  // Members so that the user can fill out the matrices
230  // J(x) = 0.5*x'*Q*x + d'*x
231  SparseMatrix<Scalar>* F_ = nullptr; // KKT system
232  Matrix<Scalar, Dynamic, 1>* beq_ = nullptr; // Vector for equalities
233  Matrix<Scalar, Dynamic, 1>* bin_ = nullptr; // Vector for inequalities
234  Matrix<Scalar, Dynamic, 1>* d_ = nullptr; // Part of the cost of the QP
235  Matrix<Scalar, Dynamic, 1>* y_ = nullptr; // Unkown variables
236 };
237 } // solvers
238 } // optimo
239 #endif // OPTIMO_SOLVERS_PRIMAL_DUAL_QP_API_H_
void reserve(void)
Reserves a squared matrix of size l x l where l = n + m + p.
Definition: sparse_primal_dual_qp_api.h:102
Abstract class for a Solver algorithm.
Definition: solver.h:53
QP parameters defining the problem.
Definition: sparse_primal_dual_qp_api.h:81
Matrix< Scalar, Dynamic, 1 > d
Part of the cost of the QP.
Definition: sparse_primal_dual_qp_api.h:99
Solves a constrained quadratic program (QP)
Definition: sparse_primal_dual_qp_api.h:78
Matrix< Scalar, Dynamic, 1 > bin
Vector for inequalities.
Definition: sparse_primal_dual_qp_api.h:98
bool setAeqElement(const uint i, const uint j, const Scalar &val)
Method that inserts an element to matrix .
Definition: sparse_primal_dual_qp_api.h:115
bool setAinElement(const uint i, const uint j, const Scalar &val)
Method that inserts an element to matrix .
Definition: sparse_primal_dual_qp_api.h:126
Params(const uint n, const uint m, const uint p)
QP parameters constructor.
Definition: sparse_primal_dual_qp_api.h:87
TERMINATION_TYPE operator()(Params *params, Matrix< Scalar, Dynamic, 1 > *y, Scalar *min_value)
Solve the QP program.
bool setQElement(const uint i, const uint j, const Scalar &val)
Method that inserts an element to matrix .
Definition: sparse_primal_dual_qp_api.h:107
Matrix< Scalar, Dynamic, 1 > beq
Vector for equalities.
Definition: sparse_primal_dual_qp_api.h:97