lily/qlpsolve.cc

   1 #include "qlpsolve.hh"
   2 #include "const.hh"
   3 #include "debug.hh"
   4 #include "choleski.hh"
   5
   6 const Real TOL=1e-2;            // roughly 1/10 mm
   7
   8 String
   9 Active_constraints::status() const
  10 {
  11     String s("Active|Inactive [");
  12     for (int i=0; i< active.size(); i++) {
  13         s += String(active[i]) + " ";
  14     }
  15
  16     s+="| ";
  17     for (int i=0; i< inactive.size(); i++) {
  18         s += String(inactive[i]) + " ";
  19     }
  20     s+="]";
  21
  22     return s;
  23 }
  24
  25 void
  26 Active_constraints::OK()
  27 {
  28     #ifndef NDEBUG
  29     H.OK();
  30     A.OK();
  31     assert(active.size() +inactive.size() == opt->cons.size());
  32     assert(H.dim() == opt->dim());
  33     assert(active.size() == A.rows());
  34     Array<int> allcons;
  35
  36     for (int i=0; i < opt->cons.size(); i++)
  37         allcons.push(0);
  38     for (int i=0; i < active.size(); i++) {
  39         int j = active[i];
  40         allcons[j]++;
  41     }
  42     for (int i=0; i < inactive.size(); i++) {
  43         int j = inactive[i];
  44         allcons[j]++;
  45     }
  46     for (int i=0; i < allcons.size(); i++)
  47         assert(allcons[i] == 1);
  48 #endif
  49 }
  50
  51 Vector
  52 Active_constraints::get_lagrange(Vector gradient)
  53 {
  54     Vector l(A*gradient);
  55
  56     return l;
  57 }
  58
  59 void
  60 Active_constraints::add(int k)
  61 {
  62     // add indices
  63     int cidx=inactive[k];
  64     active.push(cidx);
  65
  66     inactive.swap(k,inactive.size()-1);
  67     inactive.pop();
  68
  69     Vector a( opt->cons[cidx] );
  70     // update of matrices
  71     Vector Ha = H*a;
  72     Real aHa = a*Ha;
  73     Vector addrow(Ha.dim());
  74     if (abs(aHa) > EPS) {
  75         /*
  76           a != 0, so if Ha = O(EPS), then
  77           Ha * aH / aHa = O(EPS^2/EPS)
  78
  79           if H*a == 0, the constraints are dependent.
  80           */
  81         H -= Matrix(Ha/aHa , Ha);
  82
  83
  84         /*
  85           sorry, don't know how to justify this. ..
  86           */
  87         addrow=Ha;
  88         addrow/= aHa;
  89         A -= Matrix(A*a, addrow);
  90         A.insert_row(addrow,A.rows());
  91     }else
  92         WARN << "degenerate constraints";
  93 }
  94
  95 void
  96 Active_constraints::drop(int k)
  97 {
  98     int q=active.size()-1;
  99
 100         // drop indices
 101     inactive.push(active[k]);
 102     active.swap(k,q);
 103     A.swap_rows(k,q);
 104     active.pop();
 105
 106     Vector a(A.row(q));
 107     if (a.norm() > EPS) {
 108         /*
 109
 110          */
 111         Real q = a*opt->quad*a;
 112         H += Matrix(a,a/q);
 113         A -= A*opt->quad*Matrix(a,a/q);
 114     }else
 115         WARN << "degenerate constraints";
 116    #ifndef NDEBUG
 117     Vector rem_row(A.row(q));
 118     assert(rem_row.norm() < EPS);
 119    #endif
 120
 121     A.delete_row(q);
 122 }
 123
 124
 125 Active_constraints::Active_constraints(Ineq_constrained_qp const *op)
 126     :       A(0,op->dim()),
 127             H(op->dim()),
 128             opt(op)
 129 {
 130     for (int i=0; i < op->cons.size(); i++)
 131         inactive.push(i);
 132     Choleski_decomposition chol(op->quad);
 133     H=chol.inverse();
 134 }
 135
 136 /** Find the optimum which is in the planes generated by the active
 137     constraints.
 138     */
 139 Vector
 140 Active_constraints::find_active_optimum(Vector g)
 141 {
 142     return H*g;
 143 }
 144
 145 /* *************************************************************** */
 146
 147 int
 148 min_elt_index(Vector v)
 149 {
 150     Real m=INFTY; int idx=-1;
 151     for (int i = 0; i < v.dim(); i++){
 152         if (v(i) < m) {
 153             idx = i;
 154             m = v(i);
 155         }
 156         assert(v(i) <= INFTY);
 157     }
 158     return idx;
 159 }
 160
 161
 162 /**the numerical solving. Mordecai Avriel, Nonlinear Programming: analysis and methods (1976)
 163     Prentice Hall.
 164
 165     Section 13.3
 166
 167     This is a "projected gradient" algorithm. Starting from a point x
 168     the next point is found in a direction determined by projecting
 169     the gradient onto the active constraints.  (well, not really the
 170     gradient. The optimal solution obeying the active constraints is
 171     tried. This is why H = Q^-1 in initialisation) )
 172
 173
 174     */
 175 Vector
 176 Ineq_constrained_qp::solve(Vector start) const
 177 {
 178     if (!dim())
 179         return Vector(0);
 180
 181     Active_constraints act(this);
 182
 183
 184     act.OK();
 185
 186
 187     Vector x(start);
 188     Vector gradient=quad*x+lin;
 189 //    Real fvalue = x*quad*x/2 + lin*x + const_term;
 190 // it's no use.
 191
 192     Vector last_gradient(gradient);
 193     int iterations=0;
 194
 195     while (iterations++ < MAXITER) {
 196         Vector direction= - act.find_active_optimum(gradient);
 197
 198         mtor << "gradient "<< gradient<< "\ndirection " << direction<<"\n";
 199
 200         if (direction.norm() > EPS) {
 201             mtor << act.status() << '\n';
 202
 203             Real minalf = INFTY;
 204
 205             Inactive_iter minidx(act);
 206
 207
 208             /*
 209     we know the optimum on this "hyperplane". Check if we
 210     bump into the edges of the simplex
 211     */
 212
 213             for (Inactive_iter ia(act); ia.ok(); ia++) {
 214
 215                 if (ia.vec() * direction >= 0)
 216                     continue;
 217                 Real alfa= - (ia.vec()*x - ia.rhs())/
 218                     (ia.vec()*direction);
 219
 220                 if (minalf > alfa) {
 221                     minidx = ia;
 222                     minalf = alfa;
 223                 }
 224             }
 225             Real unbounded_alfa = 1.0;
 226             Real optimal_step = min(minalf, unbounded_alfa);
 227
 228             Vector deltax=direction * optimal_step;
 229             x += deltax;
 230             gradient += optimal_step * (quad * deltax);
 231
 232             mtor << "step = " << optimal_step<< " (|dx| = " <<
 233                 deltax.norm() << ")\n";
 234
 235             if (minalf < unbounded_alfa) {
 236                 /* bumped into an edge. try again, in smaller space. */
 237                 act.add(minidx.idx());
 238                 mtor << "adding cons "<< minidx.idx()<<'\n';
 239                 continue;
 240             }
 241             /*ASSERT: we are at optimal solution for this "plane"*/
 242
 243
 244         }
 245
 246         Vector lagrange_mult=act.get_lagrange(gradient);
 247         int m= min_elt_index(lagrange_mult);
 248
 249         if (m>=0 && lagrange_mult(m) > 0) {
 250             break;              // optimal sol.
 251         } else if (m<0) {
 252             assert(gradient.norm() < EPS) ;
 253
 254             break;
 255         }
 256
 257         mtor << "dropping cons " << m<<'\n';
 258         act.drop(m);
 259     }
 260     if (iterations >= MAXITER)
 261         WARN<<"didn't converge!\n";
 262
 263     mtor <<  ": found " << x<<" in " << iterations <<" iterations\n";
 264     assert_solution(x);
 265     return x;
 266 }
 267
 268