src/algs/auglag/auglag.c

   1 #include <stdlib.h>
   2 #include <math.h>
   3 #include <string.h>
   4 #include <stdio.h>
   5
   6 #include "auglag.h"
   7
   8 int auglag_verbose = 0;
   9
  10 #define MIN(a,b) ((a) < (b) ? (a) : (b))
  11 #define MAX(a,b) ((a) > (b) ? (a) : (b))
  12
  13 /***************************************************************************/
  14
  15 typedef struct {
  16      nlopt_func f; void *f_data;
  17      int m, mm; nlopt_constraint *fc;
  18      int p, pp; nlopt_constraint *h;
  19      double rho, *lambda, *mu;
  20      double *restmp, *gradtmp;
  21      nlopt_stopping *stop;
  22 } auglag_data;
  23
  24 /* the augmented lagrangian objective function */
  25 static double auglag(unsigned n, const double *x, double *grad, void *data)
  26 {
  27      auglag_data *d = (auglag_data *) data;
  28      double *gradtmp = grad ? d->gradtmp : NULL;
  29      double *restmp = d->restmp;
  30      double rho = d->rho;
  31      const double *lambda = d->lambda, *mu = d->mu;
  32      double L;
  33      int i, ii;
  34      unsigned j, k;
  35
  36      L = d->f(n, x, grad, d->f_data);
  37      ++ *(d->stop->nevals_p);
  38      if (nlopt_stop_forced(d->stop)) return L;
  39
  40      for (ii = i = 0; i < d->p; ++i) {
  41           nlopt_eval_constraint(restmp, gradtmp, d->h + i, n, x);
  42           if (nlopt_stop_forced(d->stop)) return L;
  43           for (k = 0; k < d->h[i].m; ++k) {
  44                double h = restmp[k] + lambda[ii++] / rho;
  45                L += 0.5 * rho * h*h;
  46                if (grad) for (j = 0; j < n; ++j)
  47                               grad[j] += (rho * h) * gradtmp[k*n + j];
  48           }
  49      }
  50
  51      for (ii = i = 0; i < d->m; ++i) {
  52           nlopt_eval_constraint(restmp, gradtmp, d->fc + i, n, x);
  53           if (nlopt_stop_forced(d->stop)) return L;
  54           for (k = 0; k < d->fc[i].m; ++k) {
  55                double fc = restmp[k] + mu[ii++] / rho;
  56                if (fc > 0) {
  57                     L += 0.5 * rho * fc*fc;
  58                     if (grad) for (j = 0; j < n; ++j)
  59                                    grad[j] += (rho * fc) * gradtmp[k*n + j];
  60                }
  61           }
  62      }
  63
  64      return L;
  65 }
  66
  67 /***************************************************************************/
  68
  69 nlopt_result auglag_minimize(int n, nlopt_func f, void *f_data,
  70                              int m, nlopt_constraint *fc,
  71                              int p, nlopt_constraint *h,
  72                              const double *lb, const double *ub, /* bounds */
  73                              double *x, /* in: initial guess, out: minimizer */
  74                              double *minf,
  75                              nlopt_stopping *stop,
  76                              nlopt_opt sub_opt, int sub_has_fc)
  77 {
  78      auglag_data d;
  79      nlopt_result ret = NLOPT_SUCCESS;
  80      double ICM = HUGE_VAL, minf_penalty = HUGE_VAL, penalty;
  81      double *xcur = NULL, fcur;
  82      int i, ii, feasible, minf_feasible = 0;
  83      unsigned int k;
  84      int auglag_iters = 0;
  85      int max_constraint_dim;
  86
  87      /* magic parameters from Birgin & Martinez */
  88      const double tau = 0.5, gam = 10;
  89      const double lam_min = -1e20, lam_max = 1e20, mu_max = 1e20;
  90
  91      d.f = f; d.f_data = f_data;
  92      d.m = m; d.fc = fc;
  93      d.p = p; d.h = h;
  94      d.stop = stop;
  95
  96      /* whether we handle inequality constraints via the augmented
  97         Lagrangian penalty function, or directly in the sub-algorithm */
  98      if (sub_has_fc)
  99           d.m = 0;
 100      else
 101           m = 0;
 102
 103      max_constraint_dim = MAX(nlopt_max_constraint_dim(d.m, fc),
 104                               nlopt_max_constraint_dim(d.p, h));
 105
 106      d.mm = nlopt_count_constraints(d.m, fc);
 107      d.pp = nlopt_count_constraints(d.p, h);
 108
 109      ret = nlopt_set_min_objective(sub_opt, auglag, &d); if (ret<0) return ret;
 110      ret = nlopt_set_lower_bounds(sub_opt, lb); if (ret<0) return ret;
 111      ret = nlopt_set_upper_bounds(sub_opt, ub); if (ret<0) return ret;
 112      ret = nlopt_set_stopval(sub_opt,
 113                              d.m==0 && d.p==0 ? stop->minf_max : -HUGE_VAL);
 114      if (ret<0) return ret;
 115      ret = nlopt_remove_inequality_constraints(sub_opt); if (ret<0) return ret;
 116      ret = nlopt_remove_equality_constraints(sub_opt); if (ret<0) return ret;
 117      for (i = 0; i < m; ++i) {
 118           if (fc[i].f)
 119                ret = nlopt_add_inequality_constraint(sub_opt,
 120                                                      fc[i].f, fc[i].f_data,
 121                                                      fc[i].tol[0]);
 122           else
 123                ret = nlopt_add_inequality_mconstraint(sub_opt, fc[i].m,
 124                                                       fc[i].mf, fc[i].f_data,
 125                                                       fc[i].tol);
 126           if (ret < 0) return ret;
 127      }
 128
 129      xcur = (double *) malloc(sizeof(double) * (n
 130                                                 + max_constraint_dim * (1 + n)
 131                                                 + d.pp + d.mm));
 132      if (!xcur) return NLOPT_OUT_OF_MEMORY;
 133      memcpy(xcur, x, sizeof(double) * n);
 134
 135      d.restmp = xcur + n;
 136      d.gradtmp = d.restmp + max_constraint_dim;
 137      memset(d.gradtmp, 0, sizeof(double) * (n*max_constraint_dim + d.pp+d.mm));
 138      d.lambda = d.gradtmp + n * max_constraint_dim;
 139      d.mu = d.lambda + d.pp;
 140
 141      *minf = HUGE_VAL;
 142
 143      /* starting rho suggested by B & M */
 144      if (d.p > 0 || d.m > 0) {
 145           double con2 = 0;
 146           ++ *(d.stop->nevals_p);
 147           fcur = f(n, xcur, NULL, f_data);
 148           if (nlopt_stop_forced(stop)) {
 149                ret = NLOPT_FORCED_STOP; goto done; }
 150           penalty = 0;
 151           feasible = 1;
 152           for (i = 0; i < d.p; ++i) {
 153                nlopt_eval_constraint(d.restmp, NULL, d.h + i, n, xcur);
 154                if (nlopt_stop_forced(stop)) {
 155                     ret = NLOPT_FORCED_STOP; goto done; }
 156                for (k = 0; k < d.h[i].m; ++k) {
 157                     double hi = d.restmp[k];
 158                     penalty += fabs(hi);
 159                     feasible = feasible && fabs(hi) <= h[i].tol[k];
 160                     con2 += hi * hi;
 161                }
 162           }
 163           for (i = 0; i < d.m; ++i) {
 164                nlopt_eval_constraint(d.restmp, NULL, d.fc + i, n, xcur);
 165                if (nlopt_stop_forced(stop)) {
 166                     ret = NLOPT_FORCED_STOP; goto done; }
 167                for (k = 0; k < d.fc[i].m; ++k) {
 168                     double fci = d.restmp[k];
 169                     penalty += fci > 0 ? fci : 0;
 170                     feasible = feasible && fci <= fc[i].tol[k];
 171                     if (fci > 0) con2 += fci * fci;
 172                }
 173           }
 174           *minf = fcur;
 175           minf_penalty = penalty;
 176           minf_feasible = feasible;
 177           d.rho = (con2 > 0) ? MAX(1e-6, MIN(10, 2 * fabs(*minf) / con2)) : 10;
 178      }
 179      else
 180           d.rho = 1; /* whatever, doesn't matter */
 181
 182      if (auglag_verbose) {
 183           printf("auglag: initial rho=%g\nauglag initial lambda=", d.rho);
 184           for (i = 0; i < d.pp; ++i) printf(" %g", d.lambda[i]);
 185           printf("\nauglag initial mu = ");
 186           for (i = 0; i < d.mm; ++i) printf(" %g", d.mu[i]);
 187           printf("\n");
 188      }
 189
 190      do {
 191           double prev_ICM = ICM;
 192
 193           ret = nlopt_optimize_limited(sub_opt, xcur, &fcur,
 194                                        stop->maxeval - *(stop->nevals_p),
 195                                        stop->maxtime - (nlopt_seconds()
 196                                                         - stop->start));
 197           if (auglag_verbose)
 198                printf("auglag: subopt return code %d\n", ret);
 199           if (ret < 0) break;
 200
 201           ++ *(d.stop->nevals_p);
 202           fcur = f(n, xcur, NULL, f_data);
 203           if (nlopt_stop_forced(stop)) {
 204                ret = NLOPT_FORCED_STOP; goto done; }
 205           if (auglag_verbose)
 206                printf("auglag: fcur = %g\n", fcur);
 207
 208           ICM = 0;
 209           penalty = 0;
 210           feasible = 1;
 211           for (i = ii = 0; i < d.p; ++i) {
 212                nlopt_eval_constraint(d.restmp, NULL, d.h + i, n, xcur);
 213                if (nlopt_stop_forced(stop)) {
 214                     ret = NLOPT_FORCED_STOP; goto done; }
 215                for (k = 0; k < d.h[i].m; ++k) {
 216                     double hi = d.restmp[k];
 217                     double newlam = d.lambda[ii] + d.rho * hi;
 218                     penalty += fabs(hi);
 219                     feasible = feasible && fabs(hi) <= h[i].tol[k];
 220                     ICM = MAX(ICM, fabs(hi));
 221                     d.lambda[ii++] = MIN(MAX(lam_min, newlam), lam_max);
 222                }
 223           }
 224           for (i = ii = 0; i < d.m; ++i) {
 225                nlopt_eval_constraint(d.restmp, NULL, d.fc + i, n, xcur);
 226                if (nlopt_stop_forced(stop)) {
 227                     ret = NLOPT_FORCED_STOP; goto done; }
 228                for (k = 0; k < d.fc[i].m; ++k) {
 229                     double fci = d.restmp[k];
 230                     double newmu = d.mu[ii] + d.rho * fci;
 231                     penalty += fci > 0 ? fci : 0;
 232                     feasible = feasible && fci <= fc[i].tol[k];
 233                     ICM = MAX(ICM, fabs(MAX(fci, -d.mu[ii] / d.rho)));
 234                     d.mu[ii++] = MIN(MAX(0.0, newmu), mu_max);
 235                }
 236           }
 237           if (ICM > tau * prev_ICM) {
 238                d.rho *= gam;
 239           }
 240
 241           auglag_iters++;
 242
 243           if (auglag_verbose) {
 244                printf("auglag %d: ICM=%g (%sfeasible), rho=%g\nauglag lambda=",
 245                       auglag_iters, ICM, feasible ? "" : "not ", d.rho);
 246                for (i = 0; i < d.pp; ++i) printf(" %g", d.lambda[i]);
 247                printf("\nauglag %d: mu = ", auglag_iters);
 248                for (i = 0; i < d.mm; ++i) printf(" %g", d.mu[i]);
 249                printf("\n");
 250           }
 251
 252           if ((feasible && (!minf_feasible || penalty < minf_penalty
 253                             || fcur < *minf)) ||
 254               (!minf_feasible && penalty < minf_penalty)) {
 255                ret = NLOPT_SUCCESS;
 256                if (feasible) {
 257                     if (fcur < stop->minf_max)
 258                          ret = NLOPT_MINF_MAX_REACHED;
 259                     else if (nlopt_stop_ftol(stop, fcur, *minf))
 260                          ret = NLOPT_FTOL_REACHED;
 261                     else if (nlopt_stop_x(stop, xcur, x))
 262                          ret = NLOPT_XTOL_REACHED;
 263                }
 264                *minf = fcur;
 265                minf_penalty = penalty;
 266                minf_feasible = feasible;
 267                memcpy(x, xcur, sizeof(double) * n);
 268                if (ret != NLOPT_SUCCESS) break;
 269           }
 270
 271           if (nlopt_stop_forced(stop)) {ret = NLOPT_FORCED_STOP; break;}
 272           if (nlopt_stop_evals(stop)) {ret = NLOPT_MAXEVAL_REACHED; break;}
 273           if (nlopt_stop_time(stop)) {ret = NLOPT_MAXTIME_REACHED; break;}
 274
 275           /* TODO: use some other stopping criterion on ICM? */
 276           /* The paper uses ICM <= epsilon and DFM <= epsilon, where
 277              DFM is a measure of the size of the Lagrangian gradient.
 278              Besides the fact that these kinds of absolute tolerances
 279              (non-scale-invariant) are unsatisfying and it is not
 280              clear how the user should specify it, the ICM <= epsilon
 281              condition seems not too different from requiring feasibility,
 282              especially now that the user can provide constraint-specific
 283              tolerances analogous to epsilon. */
 284           if (ICM == 0) {ret = NLOPT_FTOL_REACHED; break;}
 285      } while (1);
 286
 287 done:
 288      free(xcur);
 289      return ret;
 290 }