eiquadprog.hpp
Go to the documentation of this file.
00001 #ifndef _EIGEN_QUADSOLVE_HPP_
00002 #define _EIGEN_QUADSOLVE_HPP_
00003 
00004 
00005 /*
00006  FILE eiquadprog.hh
00007  
00008  NOTE: this is a modified of uQuadProg++ package, working with Eigen data structures. 
00009        uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++ originally developed by 
00010        Luca Di Gaspero, working with ublas data structures. 
00011 
00012  The quadprog_solve() function implements the algorithm of Goldfarb and Idnani 
00013  for the solution of a (convex) Quadratic Programming problem
00014 by means of a dual method.
00015          
00016 The problem is in the form:
00017 
00018 min 0.5 * x G x + g0 x
00019 s.t.
00020     CE^T x + ce0 = 0
00021     CI^T x + ci0 >= 0
00022          
00023  The matrix and vectors dimensions are as follows:
00024      G: n * n
00025                 g0: n
00026                                 
00027                 CE: n * p
00028          ce0: p
00029                                 
00030           CI: n * m
00031    ci0: m
00032 
00033      x: n
00034  
00035  The function will return the cost of the solution written in the x vector or
00036  std::numeric_limits::infinity() if the problem is infeasible. In the latter case
00037  the value of the x vector is not correct.
00038  
00039  References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
00040              strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
00041 
00042  Notes:
00043   1. pay attention in setting up the vectors ce0 and ci0. 
00044            If the constraints of your problem are specified in the form 
00045            A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
00046   2. The matrix G is modified within the function since it is used to compute
00047      the G = L^T L cholesky factorization for further computations inside the function. 
00048      If you need the original matrix G you should make a copy of it and pass the copy
00049      to the function.
00050     
00051  
00052  The author will be grateful if the researchers using this software will
00053  acknowledge the contribution of this modified function and of Di Gaspero's
00054  original version in their research papers.
00055 
00056 
00057 LICENSE
00058 
00059 Copyright (2010) Gael Guennebaud
00060 Copyright (2008) Angelo Furfaro
00061 Copyright (2006) Luca Di Gaspero
00062 
00063 
00064 This file is a porting of QuadProg++ routine, originally developed
00065 by Luca Di Gaspero, exploiting uBlas data structures for vectors and
00066 matrices instead of native C++ array.
00067 
00068 uquadprog is free software; you can redistribute it and/or modify
00069 it under the terms of the GNU General Public License as published by
00070 the Free Software Foundation; either version 2 of the License, or
00071 (at your option) any later version.
00072 
00073 uquadprog is distributed in the hope that it will be useful,
00074 but WITHOUT ANY WARRANTY; without even the implied warranty of
00075 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00076 GNU General Public License for more details.
00077 
00078 You should have received a copy of the GNU General Public License
00079 along with uquadprog; if not, write to the Free Software
00080 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00081 
00082 */
00083 
00084 #include <Eigen/Dense>
00085 
00086 namespace Eigen {
00087 
00088 // namespace internal {
00089 
00090 template<typename Scalar>
00091 inline Scalar distance(Scalar a, Scalar b)
00092 {
00093         Scalar a1, b1, t;
00094         a1 = std::abs(a);
00095         b1 = std::abs(b);
00096         if (a1 > b1) 
00097         {
00098                 t = (b1 / a1);
00099                 return a1 * std::sqrt(1.0 + t * t);
00100         }
00101         else
00102                 if (b1 > a1)
00103                 {
00104                         t = (a1 / b1);
00105                         return b1 * std::sqrt(1.0 + t * t);
00106                 }
00107         return a1 * std::sqrt(2.0);
00108 }
00109 
00110 // }
00111 
00112 inline void compute_d(VectorXd &d, const MatrixXd& J, const VectorXd& np)
00113 {
00114   d = J.adjoint() * np;
00115 }
00116 
00117 inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d,  int iq)
00118 {
00119   z = J.rightCols(z.size()-iq) * d.tail(d.size()-iq);
00120 }
00121 
00122 inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, int iq) 
00123 {
00124   r.head(iq)= R.topLeftCorner(iq,iq).triangularView<Upper>().solve(d.head(iq));
00125 }
00126 
00127 bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm);
00128 void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u,  int p, int& iq, int l);
00129 
00130 
00131 inline double solve_quadprog(MatrixXd & G,  VectorXd & g0,  
00132                       const MatrixXd & CE, const VectorXd & ce0,  
00133                       const MatrixXd & CI, const VectorXd & ci0, 
00134                       VectorXd& x)
00135 {
00136   int i, j, k, l; /* indices */
00137   int ip, me, mi;
00138   int n=g0.size();  int p=ce0.size();  int m=ci0.size();  
00139   MatrixXd R(G.rows(),G.cols()), J(G.rows(),G.cols());
00140   
00141   LLT<MatrixXd,Lower> chol(G.cols());
00142  
00143   VectorXd s(m+p), z(n), r(m + p), d(n),  np(n), u(m + p);
00144   VectorXd x_old(n), u_old(m + p);
00145   double f_value, psi, c1, c2, sum, ss, R_norm;
00146   const double inf = std::numeric_limits<double>::infinity();
00147   double t, t1, t2; /* t is the step length, which is the minimum of the partial step length t1 
00148     * and the full step length t2 */
00149   VectorXi A(m + p), A_old(m + p), iai(m + p);
00150   int q;
00151   int iq, iter = 0;
00152   bool iaexcl[m + p];
00153         
00154   me = p; /* number of equality constraints */
00155   mi = m; /* number of inequality constraints */
00156   q = 0;  /* size of the active set A (containing the indices of the active constraints) */
00157   
00158   /*
00159    * Preprocessing phase
00160    */
00161         
00162   /* compute the trace of the original matrix G */
00163   c1 = G.trace();
00164         
00165         /* decompose the matrix G in the form LL^T */
00166   chol.compute(G);
00167  
00168   /* initialize the matrix R */
00169   d.setZero();
00170   R.setZero();
00171         R_norm = 1.0; /* this variable will hold the norm of the matrix R */
00172   
00173         /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */
00174   // J = L^-T
00175   J.setIdentity();
00176   J = chol.matrixU().solve(J);
00177         c2 = J.trace();
00178 #ifdef TRACE_SOLVER
00179  print_matrix("J", J, n);
00180 #endif
00181   
00182         /* c1 * c2 is an estimate for cond(G) */
00183   
00184         /* 
00185    * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x 
00186    * this is a feasible point in the dual space
00187          * x = G^-1 * g0
00188    */
00189   x = chol.solve(g0);
00190   x = -x;
00191         /* and compute the current solution value */ 
00192         f_value = 0.5 * g0.dot(x);
00193 #ifdef TRACE_SOLVER
00194   std::cerr << "Unconstrained solution: " << f_value << std::endl;
00195   print_vector("x", x, n);
00196 #endif
00197   
00198         /* Add equality constraints to the working set A */
00199   iq = 0;
00200         for (i = 0; i < me; i++)
00201         {
00202     np = CE.col(i);
00203     compute_d(d, J, np);
00204                 update_z(z, J, d,  iq);
00205                 update_r(R, r, d,  iq);
00206 #ifdef TRACE_SOLVER
00207                 print_matrix("R", R, iq);
00208                 print_vector("z", z, n);
00209                 print_vector("r", r, iq);
00210                 print_vector("d", d, n);
00211 #endif
00212     
00213     /* compute full step length t2: i.e., the minimum step in primal space s.t. the contraint 
00214       becomes feasible */
00215     t2 = 0.0;
00216     if (std::abs(z.dot(z)) > std::numeric_limits<double>::epsilon()) // i.e. z != 0
00217       t2 = (-np.dot(x) - ce0(i)) / z.dot(np);
00218     
00219     x += t2 * z;
00220 
00221     /* set u = u+ */
00222     u(iq) = t2;
00223     u.head(iq) -= t2 * r.head(iq);
00224     
00225     /* compute the new solution value */
00226     f_value += 0.5 * (t2 * t2) * z.dot(np);
00227     A(i) = -i - 1;
00228     
00229     if (!add_constraint(R, J, d, iq, R_norm))
00230     {
00231       // FIXME: it should raise an error
00232       // Equality constraints are linearly dependent
00233       return f_value;
00234     }
00235   }
00236   
00237         /* set iai = K \ A */
00238         for (i = 0; i < mi; i++)
00239                 iai(i) = i;
00240   
00241 l1:     iter++;
00242 #ifdef TRACE_SOLVER
00243   print_vector("x", x, n);
00244 #endif
00245   /* step 1: choose a violated constraint */
00246         for (i = me; i < iq; i++)
00247         {
00248           ip = A(i);
00249                 iai(ip) = -1;
00250         }
00251         
00252         /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */
00253         ss = 0.0;
00254         psi = 0.0; /* this value will contain the sum of all infeasibilities */
00255         ip = 0; /* ip will be the index of the chosen violated constraint */
00256         for (i = 0; i < mi; i++)
00257         {
00258                 iaexcl[i] = true;
00259                 sum = CI.col(i).dot(x) + ci0(i);
00260                 s(i) = sum;
00261                 psi += std::min(0.0, sum);
00262         }
00263 #ifdef TRACE_SOLVER
00264   print_vector("s", s, mi);
00265 #endif
00266 
00267     
00268         if (std::abs(psi) <= mi * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0)
00269         {
00270     /* numerically there are not infeasibilities anymore */
00271     q = iq;
00272                 return f_value;
00273   }
00274     
00275   /* save old values for u, x and A */
00276    u_old.head(iq) = u.head(iq);
00277    A_old.head(iq) = A.head(iq);
00278    x_old = x;
00279     
00280 l2: /* Step 2: check for feasibility and determine a new S-pair */
00281         for (i = 0; i < mi; i++)
00282         {
00283                 if (s(i) < ss && iai(i) != -1 && iaexcl[i])
00284                 {
00285                         ss = s(i);
00286                         ip = i;
00287                 }
00288         }
00289   if (ss >= 0.0)
00290   {
00291     q = iq;
00292     return f_value;
00293   }
00294     
00295   /* set np = n(ip) */
00296   np = CI.col(ip);
00297   /* set u = (u 0)^T */
00298   u(iq) = 0.0;
00299   /* add ip to the active set A */
00300   A(iq) = ip;
00301 
00302 #ifdef TRACE_SOLVER
00303         std::cerr << "Trying with constraint " << ip << std::endl;
00304         print_vector("np", np, n);
00305 #endif
00306     
00307 l2a:/* Step 2a: determine step direction */
00308   /* compute z = H np: the step direction in the primal space (through J, see the paper) */
00309   compute_d(d, J, np);
00310   update_z(z, J, d, iq);
00311   /* compute N* np (if q > 0): the negative of the step direction in the dual space */
00312   update_r(R, r, d, iq);
00313 #ifdef TRACE_SOLVER
00314   std::cerr << "Step direction z" << std::endl;
00315                 print_vector("z", z, n);
00316                 print_vector("r", r, iq + 1);
00317     print_vector("u", u, iq + 1);
00318     print_vector("d", d, n);
00319     print_ivector("A", A, iq + 1);
00320 #endif
00321     
00322   /* Step 2b: compute step length */
00323   l = 0;
00324   /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */
00325   t1 = inf; /* +inf */
00326   /* find the index l s.t. it reaches the minimum of u+(x) / r */
00327   for (k = me; k < iq; k++)
00328   {
00329     double tmp;
00330     if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1) )
00331     {
00332       t1 = tmp;
00333       l = A(k);
00334     }
00335   }
00336   /* Compute t2: full step length (minimum step in primal space such that the constraint ip becomes feasible */
00337   if (std::abs(z.dot(z))  > std::numeric_limits<double>::epsilon()) // i.e. z != 0
00338     t2 = -s(ip) / z.dot(np);
00339   else
00340     t2 = inf; /* +inf */
00341 
00342   /* the step is chosen as the minimum of t1 and t2 */
00343   t = std::min(t1, t2);
00344 #ifdef TRACE_SOLVER
00345   std::cerr << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 << ") ";
00346 #endif
00347   
00348   /* Step 2c: determine new S-pair and take step: */
00349   
00350   /* case (i): no step in primal or dual space */
00351   if (t >= inf)
00352   {
00353     /* QPP is infeasible */
00354     // FIXME: unbounded to raise
00355     q = iq;
00356     return inf;
00357   }
00358   /* case (ii): step in dual space */
00359   if (t2 >= inf)
00360   {
00361     /* set u = u +  t * [-r 1) and drop constraint l from the active set A */
00362     u.head(iq) -= t * r.head(iq);
00363     u(iq) += t;
00364     iai(l) = l;
00365     delete_constraint(R, J, A, u, p, iq, l);
00366 #ifdef TRACE_SOLVER
00367     std::cerr << " in dual space: " 
00368       << f_value << std::endl;
00369     print_vector("x", x, n);
00370     print_vector("z", z, n);
00371                 print_ivector("A", A, iq + 1);
00372 #endif
00373     goto l2a;
00374   }
00375   
00376   /* case (iii): step in primal and dual space */
00377   
00378   x += t * z;
00379   /* update the solution value */
00380   f_value += t * z.dot(np) * (0.5 * t + u(iq));
00381   
00382   u.head(iq) -= t * r.head(iq);
00383   u(iq) += t;
00384 #ifdef TRACE_SOLVER
00385   std::cerr << " in both spaces: " 
00386     << f_value << std::endl;
00387         print_vector("x", x, n);
00388         print_vector("u", u, iq + 1);
00389         print_vector("r", r, iq + 1);
00390         print_ivector("A", A, iq + 1);
00391 #endif
00392   
00393   if (t == t2)
00394   {
00395 #ifdef TRACE_SOLVER
00396     std::cerr << "Full step has taken " << t << std::endl;
00397     print_vector("x", x, n);
00398 #endif
00399     /* full step has taken */
00400     /* add constraint ip to the active set*/
00401                 if (!add_constraint(R, J, d, iq, R_norm))
00402                 {
00403                         iaexcl[ip] = false;
00404                         delete_constraint(R, J, A, u, p, iq, ip);
00405 #ifdef TRACE_SOLVER
00406       print_matrix("R", R, n);
00407       print_ivector("A", A, iq);
00408 #endif
00409                         for (i = 0; i < m; i++)
00410                                 iai(i) = i;
00411                         for (i = 0; i < iq; i++)
00412                         {
00413                                 A(i) = A_old(i);
00414                                 iai(A(i)) = -1;
00415                                 u(i) = u_old(i);
00416                         }
00417                         x = x_old;
00418       goto l2; /* go to step 2 */
00419                 }    
00420     else
00421       iai(ip) = -1;
00422 #ifdef TRACE_SOLVER
00423     print_matrix("R", R, n);
00424     print_ivector("A", A, iq);
00425 #endif
00426     goto l1;
00427   }
00428   
00429   /* a patial step has taken */
00430 #ifdef TRACE_SOLVER
00431   std::cerr << "Partial step has taken " << t << std::endl;
00432   print_vector("x", x, n);
00433 #endif
00434   /* drop constraint l */
00435         iai(l) = l;
00436         delete_constraint(R, J, A, u, p, iq, l);
00437 #ifdef TRACE_SOLVER
00438   print_matrix("R", R, n);
00439   print_ivector("A", A, iq);
00440 #endif
00441   
00442   s(ip) = CI.col(ip).dot(x) + ci0(ip);
00443 
00444 #ifdef TRACE_SOLVER
00445   print_vector("s", s, mi);
00446 #endif
00447   goto l2a;
00448 }
00449 
00450 
00451 inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, double& R_norm)
00452 {
00453  int n=J.rows();
00454 #ifdef TRACE_SOLVER
00455   std::cerr << "Add constraint " << iq << '/';
00456 #endif
00457         int i, j, k;
00458         double cc, ss, h, t1, t2, xny;
00459         
00460   /* we have to find the Givens rotation which will reduce the element
00461                 d(j) to zero.
00462                 if it is already zero we don't have to do anything, except of
00463                 decreasing j */  
00464         for (j = n - 1; j >= iq + 1; j--)
00465         {
00466     /* The Givens rotation is done with the matrix (cc cs, cs -cc).
00467                          If cc is one, then element (j) of d is zero compared with element
00468                          (j - 1). Hence we don't have to do anything. 
00469                          If cc is zero, then we just have to switch column (j) and column (j - 1) 
00470                          of J. Since we only switch columns in J, we have to be careful how we
00471                          update d depending on the sign of gs.
00472                          Otherwise we have to apply the Givens rotation to these columns.
00473                          The i - 1 element of d has to be updated to h. */
00474                 cc = d(j - 1);
00475                 ss = d(j);
00476                 h = distance(cc, ss);
00477                 if (h == 0.0)
00478                         continue;
00479                 d(j) = 0.0;
00480                 ss = ss / h;
00481                 cc = cc / h;
00482                 if (cc < 0.0)
00483                 {
00484                         cc = -cc;
00485                         ss = -ss;
00486                         d(j - 1) = -h;
00487                 }
00488                 else
00489                         d(j - 1) = h;
00490                 xny = ss / (1.0 + cc);
00491                 for (k = 0; k < n; k++)
00492                 {
00493                         t1 = J(k,j - 1);
00494                         t2 = J(k,j);
00495                         J(k,j - 1) = t1 * cc + t2 * ss;
00496                         J(k,j) = xny * (t1 + J(k,j - 1)) - t2;
00497                 }
00498         }
00499   /* update the number of constraints added*/
00500         iq++;
00501   /* To update R we have to put the iq components of the d vector
00502     into column iq - 1 of R
00503     */
00504   R.col(iq-1).head(iq) = d.head(iq);
00505 #ifdef TRACE_SOLVER
00506   std::cerr << iq << std::endl;
00507 #endif
00508   
00509         if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm)
00510                 // problem degenerate
00511                 return false;
00512         R_norm = std::max<double>(R_norm, std::abs(d(iq - 1)));
00513         return true;
00514 }
00515 
00516 
00517 inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, VectorXd& u,  int p, int& iq, int l)
00518 {
00519 
00520   int n = R.rows();
00521 #ifdef TRACE_SOLVER
00522   std::cerr << "Delete constraint " << l << ' ' << iq;
00523 #endif
00524         int i, j, k, qq;
00525         double cc, ss, h, xny, t1, t2;
00526   
00527         /* Find the index qq for active constraint l to be removed */
00528   for (i = p; i < iq; i++)
00529   if (A(i) == l)
00530   {
00531     qq = i;
00532     break;
00533   }
00534       
00535   /* remove the constraint from the active set and the duals */
00536   for (i = qq; i < iq - 1; i++)
00537   {
00538     A(i) = A(i + 1);
00539     u(i) = u(i + 1);
00540     R.col(i) = R.col(i+1);
00541   }
00542       
00543   A(iq - 1) = A(iq);
00544   u(iq - 1) = u(iq);
00545   A(iq) = 0; 
00546   u(iq) = 0.0;
00547   for (j = 0; j < iq; j++)
00548     R(j,iq - 1) = 0.0;
00549   /* constraint has been fully removed */
00550   iq--;
00551 #ifdef TRACE_SOLVER
00552   std::cerr << '/' << iq << std::endl;
00553 #endif 
00554   
00555   if (iq == 0)
00556     return;
00557   
00558   for (j = qq; j < iq; j++)
00559   {
00560     cc = R(j,j);
00561     ss = R(j + 1,j);
00562     h = distance(cc, ss);
00563     if (h == 0.0)
00564       continue;
00565     cc = cc / h;
00566     ss = ss / h;
00567     R(j + 1,j) = 0.0;
00568     if (cc < 0.0)
00569     {
00570       R(j,j) = -h;
00571       cc = -cc;
00572       ss = -ss;
00573     }
00574     else
00575       R(j,j) = h;
00576     
00577     xny = ss / (1.0 + cc);
00578     for (k = j + 1; k < iq; k++)
00579     {
00580       t1 = R(j,k);
00581       t2 = R(j + 1,k);
00582       R(j,k) = t1 * cc + t2 * ss;
00583       R(j + 1,k) = xny * (t1 + R(j,k)) - t2;
00584     }
00585     for (k = 0; k < n; k++)
00586     {
00587       t1 = J(k,j);
00588       t2 = J(k,j + 1);
00589       J(k,j) = t1 * cc + t2 * ss;
00590       J(k,j + 1) = xny * (J(k,j) + t1) - t2;
00591     }
00592   }
00593 }
00594 
00595 }
00596 
00597 #endif


eus_qp
Author(s):
autogenerated on Fri Apr 19 2019 03:45:20