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 = 1;
   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; nlopt_constraint *fc;
  18      int p; nlopt_constraint *h;
  19      double rho, *lambda, *mu;
  20      double *gradtmp;
  21      nlopt_stopping *stop;
  22 } auglag_data;
  23
  24 /* the augmented lagrangian objective function */
  25 static double auglag(int 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 rho = d->rho;
  30      const double *lambda = d->lambda, *mu = d->mu;
  31      double L;
  32      int i, j;
  33
  34      L = d->f(n, x, grad, d->f_data);
  35
  36      for (i = 0; i < d->p; ++i) {
  37           double h;
  38           h = d->h[i].f(n, x, gradtmp, d->h[i].f_data) + lambda[i] / rho;
  39           L += 0.5 * rho * h*h;
  40           if (grad) for (j = 0; j < n; ++j) grad[j] += (rho * h) * gradtmp[j];
  41      }
  42
  43      for (i = 0; i < d->m; ++i) {
  44           double fc;
  45           fc = d->fc[i].f(n, x, gradtmp, d->fc[i].f_data) + mu[i] / rho;
  46           if (fc > 0) {
  47                L += 0.5 * rho * fc*fc;
  48                if (grad) for (j = 0; j < n; ++j)
  49                     grad[j] += (rho * fc) * gradtmp[j];
  50           }
  51      }
  52
  53      d->stop->nevals++;
  54
  55      return L;
  56 }
  57
  58 /***************************************************************************/
  59
  60 nlopt_result auglag_minimize(int n, nlopt_func f, void *f_data,
  61                              int m, nlopt_constraint *fc,
  62                              int p, nlopt_constraint *h,
  63                              const double *lb, const double *ub, /* bounds */
  64                              double *x, /* in: initial guess, out: minimizer */
  65                              double *minf,
  66                              nlopt_stopping *stop,
  67                              nlopt_opt sub_opt, int sub_has_fc)
  68 {
  69      auglag_data d;
  70      nlopt_result ret = NLOPT_SUCCESS;
  71      double ICM = HUGE_VAL;
  72      double *xcur = NULL, fcur;
  73      int i, feasible;
  74
  75      /* magic parameters from Birgin & Martinez */
  76      const double tau = 0.5, gam = 10;
  77      const double lam_min = -1e20, lam_max = 1e20, mu_max = 1e20;
  78
  79      d.f = f; d.f_data = f_data;
  80      d.m = m; d.fc = fc;
  81      d.p = p; d.h = h;
  82      d.stop = stop;
  83
  84      /* whether we handle inequality constraints via the augmented
  85         Lagrangian penalty function, or directly in the sub-algorithm */
  86      if (sub_has_fc)
  87           d.m = 0;
  88      else
  89           m = 0;
  90
  91      ret = nlopt_set_min_objective(sub_opt, auglag, &d); if (ret<0) return ret;
  92      ret = nlopt_set_lower_bounds(sub_opt, lb); if (ret<0) return ret;
  93      ret = nlopt_set_upper_bounds(sub_opt, ub); if (ret<0) return ret;
  94      ret = nlopt_remove_inequality_constraints(sub_opt); if (ret<0) return ret;
  95      ret = nlopt_remove_equality_constraints(sub_opt); if (ret<0) return ret;
  96      for (i = 0; i < m; ++i) {
  97           ret = nlopt_add_inequality_constraint(sub_opt, fc[i].f, fc[i].f_data,
  98                                                 fc[i].tol);
  99           if (ret < 0) return ret;
 100      }
 101
 102      xcur = (double *) malloc(sizeof(double) * (2*n + d.p + d.m));
 103      if (!xcur) return NLOPT_OUT_OF_MEMORY;
 104      memcpy(xcur, x, sizeof(double) * n);
 105
 106      d.gradtmp = xcur + n;
 107      memset(d.gradtmp, 0, sizeof(double) * (n + d.p + d.m));
 108      d.lambda = d.gradtmp + n;
 109      d.mu = d.lambda + d.p;
 110
 111      /* starting rho suggested by B & M */
 112      {
 113           double con2 = 0;
 114           d.stop->nevals++;
 115           fcur = f(n, xcur, NULL, f_data);
 116           feasible = 1;
 117           for (i = 0; i < d.p; ++i) {
 118                double hi = h[i].f(n, xcur, NULL, d.h[i].f_data);
 119                feasible = feasible && fabs(hi) <= h[i].tol;
 120                con2 += hi * hi;
 121           }
 122           for (i = 0; i < d.m; ++i) {
 123                double fci = fc[i].f(n, xcur, NULL, d.fc[i].f_data);
 124                feasible = feasible && fci <= fc[i].tol;
 125                if (fci > 0) con2 += fci * fci;
 126           }
 127           d.rho = MAX(1e-6, MIN(10, 2 * fabs(*minf) / con2));
 128      }
 129
 130      if (feasible)
 131           *minf = fcur;
 132      else
 133           *minf = HUGE_VAL;
 134
 135      do {
 136           double prev_ICM = ICM;
 137
 138           ret = nlopt_optimize_limited(sub_opt, xcur, minf,
 139                                        stop->maxeval - stop->nevals,
 140                                        stop->maxtime - (nlopt_seconds()
 141                                                         - stop->start));
 142           if (ret < 0) break;
 143
 144           d.stop->nevals++;
 145           fcur = f(n, xcur, NULL, f_data);
 146
 147           ICM = 0;
 148           feasible = 1;
 149           for (i = 0; i < d.p; ++i) {
 150                double hi = h[i].f(n, xcur, NULL, d.h[i].f_data);
 151                double newlam = d.lambda[i] + d.rho * hi;
 152                feasible = feasible && fabs(hi) <= h[i].tol;
 153                ICM = MAX(ICM, fabs(hi));
 154                d.lambda[i] = MIN(MAX(lam_min, newlam), lam_max);
 155           }
 156           for (i = 0; i < d.m; ++i) {
 157                double fci = fc[i].f(n, xcur, NULL, d.fc[i].f_data);
 158                double newmu = d.mu[i] + d.rho * fci;
 159                feasible = feasible && fci <= fc[i].tol;
 160                ICM = MAX(ICM, fabs(MAX(fci, -d.mu[i] / d.rho)));
 161                d.mu[i] = MIN(MAX(0.0, newmu), mu_max);
 162           }
 163           if (ICM > tau * prev_ICM)
 164                d.rho *= gam;
 165
 166           if (auglag_verbose) {
 167                printf("auglag: ICM=%g, rho=%g\nauglag lambda=", ICM, d.rho);
 168                for (i = 0; i < d.p; ++i) printf(" %g", d.lambda[i]);
 169                printf("\nauglag mu = ");
 170                for (i = 0; i < d.m; ++i) printf(" %g", d.mu[i]);
 171                printf("\n");
 172           }
 173
 174           /* only check f & x convergence for feasible points...
 175              for this to be effective on active constraints, the user
 176              must set some nonzero tolerance for each constraint */
 177           if (feasible && fcur < *minf) {
 178                ret = NLOPT_SUCCESS;
 179                if (fcur < stop->minf_max) ret = NLOPT_MINF_MAX_REACHED;
 180                if (nlopt_stop_ftol(stop, fcur,*minf)) ret = NLOPT_FTOL_REACHED;
 181                if (nlopt_stop_x(stop, xcur, x)) ret = NLOPT_XTOL_REACHED;
 182                *minf = fcur;
 183                memcpy(x, xcur, sizeof(double) * n);
 184                if (ret != NLOPT_SUCCESS) break;
 185           }
 186
 187           if (nlopt_stop_evals(stop)) {ret = NLOPT_MAXEVAL_REACHED; break;}
 188           if (nlopt_stop_time(stop)) {ret = NLOPT_MAXTIME_REACHED; break;}
 189
 190           /* TODO: use some stopping criterion on ICM? */
 191      } while (1);
 192
 193      free(xcur);
 194      return ret;
 195 }