/*
* NLOPT optimization routine for table tennis traj gen
*
*/
void nlopt_optim_poly_run(double *x, double *params) {
static double tol[EQ_CONSTR_DIM];
static double lb[OPTIM_DIM]; /* lower bounds */
static double ub[OPTIM_DIM]; /* upper bounds */
set_bounds(lb,ub);
const_vec(EQ_CONSTR_DIM,1e-2,tol); /* set tolerances equal to second argument */
nlopt_opt opt;
opt = nlopt_create(NLOPT_LN_COBYLA, OPTIM_DIM); /* LN = does not require gradients */
//nlopt_set_xtol_rel(opt, 1e-2);
//opt = nlopt_create(NLOPT_AUGLAG, OPTIM_DIM); /* algorithm and dimensionality */
//nlopt_set_local_optimizer(opt, opt);
nlopt_set_lower_bounds(opt, lb);
nlopt_set_upper_bounds(opt, ub);
nlopt_set_min_objective(opt, costfunc, params);
nlopt_add_inequality_mconstraint(opt, INEQ_CONSTR_DIM, joint_limits_ineq_constr, params, tol);
nlopt_add_equality_mconstraint(opt, EQ_CONSTR_DIM, kinematics_eq_constr, params, tol);
nlopt_set_xtol_rel(opt, 1e-2);
//int maxeval = 20000;
//nlopt_set_maxeval(opt, maxeval);
//double maxtime = 0.001;
//nlopt_set_maxtime(opt, maxtime);
//init_soln_to_rest_posture(x,params); //parameters are the initial joint positions q0
double initTime = get_time();
double minf; /* the minimum objective value, upon return */
if (nlopt_optimize(opt, x, &minf) < 0) {
printf("NLOPT failed!\n");
}
else {
//nlopt_example_run();
printf("NLOPT took %f ms\n", (get_time() - initTime)/1e3);
printf("Found minimum at f = %0.10g\n", minf);
test_optim(x,params);
}
nlopt_destroy(opt);
}
nlopt_result auglag_minimize(int n, nlopt_func f, void *f_data,
int m, nlopt_constraint *fc,
int p, nlopt_constraint *h,
const double *lb, const double *ub, /* bounds */
double *x, /* in: initial guess, out: minimizer */
double *minf,
nlopt_stopping *stop,
nlopt_opt sub_opt, int sub_has_fc)
{
auglag_data d;
nlopt_result ret = NLOPT_SUCCESS;
double ICM = HUGE_VAL, minf_penalty = HUGE_VAL, penalty;
double *xcur = NULL, fcur;
int i, ii, feasible, minf_feasible = 0;
unsigned int k;
int auglag_iters = 0;
int max_constraint_dim;
/* magic parameters from Birgin & Martinez */
const double tau = 0.5, gam = 10;
const double lam_min = -1e20, lam_max = 1e20, mu_max = 1e20;
d.f = f; d.f_data = f_data;
d.m = m; d.fc = fc;
d.p = p; d.h = h;
d.stop = stop;
/* whether we handle inequality constraints via the augmented
Lagrangian penalty function, or directly in the sub-algorithm */
if (sub_has_fc)
d.m = 0;
else
m = 0;
max_constraint_dim = MAX(nlopt_max_constraint_dim(d.m, fc),
nlopt_max_constraint_dim(d.p, h));
d.mm = nlopt_count_constraints(d.m, fc);
d.pp = nlopt_count_constraints(d.p, h);
ret = nlopt_set_min_objective(sub_opt, auglag, &d); if (ret<0) return ret;
ret = nlopt_set_lower_bounds(sub_opt, lb); if (ret<0) return ret;
ret = nlopt_set_upper_bounds(sub_opt, ub); if (ret<0) return ret;
ret = nlopt_set_stopval(sub_opt,
d.m==0 && d.p==0 ? stop->minf_max : -HUGE_VAL);
if (ret<0) return ret;
ret = nlopt_remove_inequality_constraints(sub_opt); if (ret<0) return ret;
ret = nlopt_remove_equality_constraints(sub_opt); if (ret<0) return ret;
for (i = 0; i < m; ++i) {
if (fc[i].f)
ret = nlopt_add_inequality_constraint(sub_opt,
fc[i].f, fc[i].f_data,
fc[i].tol[0]);
else
ret = nlopt_add_inequality_mconstraint(sub_opt, fc[i].m,
fc[i].mf, fc[i].f_data,
fc[i].tol);
if (ret < 0) return ret;
}
xcur = (double *) malloc(sizeof(double) * (n
+ max_constraint_dim * (1 + n)
+ d.pp + d.mm));
if (!xcur) return NLOPT_OUT_OF_MEMORY;
memcpy(xcur, x, sizeof(double) * n);
d.restmp = xcur + n;
d.gradtmp = d.restmp + max_constraint_dim;
memset(d.gradtmp, 0, sizeof(double) * (n*max_constraint_dim + d.pp+d.mm));
d.lambda = d.gradtmp + n * max_constraint_dim;
d.mu = d.lambda + d.pp;
*minf = HUGE_VAL;
/* starting rho suggested by B & M */
if (d.p > 0 || d.m > 0) {
double con2 = 0;
++ *(d.stop->nevals_p);
fcur = f(n, xcur, NULL, f_data);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
penalty = 0;
feasible = 1;
for (i = 0; i < d.p; ++i) {
nlopt_eval_constraint(d.restmp, NULL, d.h + i, n, xcur);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
for (k = 0; k < d.h[i].m; ++k) {
double hi = d.restmp[k];
penalty += fabs(hi);
feasible = feasible && fabs(hi) <= h[i].tol[k];
con2 += hi * hi;
}
}
for (i = 0; i < d.m; ++i) {
nlopt_eval_constraint(d.restmp, NULL, d.fc + i, n, xcur);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
for (k = 0; k < d.fc[i].m; ++k) {
double fci = d.restmp[k];
penalty += fci > 0 ? fci : 0;
feasible = feasible && fci <= fc[i].tol[k];
if (fci > 0) con2 += fci * fci;
}
}
*minf = fcur;
minf_penalty = penalty;
minf_feasible = feasible;
d.rho = MAX(1e-6, MIN(10, 2 * fabs(*minf) / con2));
}
else
d.rho = 1; /* whatever, doesn't matter */
if (auglag_verbose) {
printf("auglag: initial rho=%g\nauglag initial lambda=", d.rho);
for (i = 0; i < d.pp; ++i) printf(" %g", d.lambda[i]);
printf("\nauglag initial mu = ");
for (i = 0; i < d.mm; ++i) printf(" %g", d.mu[i]);
printf("\n");
}
do {
double prev_ICM = ICM;
ret = nlopt_optimize_limited(sub_opt, xcur, &fcur,
stop->maxeval - *(stop->nevals_p),
stop->maxtime - (nlopt_seconds()
- stop->start));
if (auglag_verbose)
printf("auglag: subopt return code %d\n", ret);
if (ret < 0) break;
++ *(d.stop->nevals_p);
fcur = f(n, xcur, NULL, f_data);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
if (auglag_verbose)
printf("auglag: fcur = %g\n", fcur);
ICM = 0;
penalty = 0;
feasible = 1;
for (i = ii = 0; i < d.p; ++i) {
nlopt_eval_constraint(d.restmp, NULL, d.h + i, n, xcur);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
for (k = 0; k < d.h[i].m; ++k) {
double hi = d.restmp[k];
double newlam = d.lambda[ii] + d.rho * hi;
penalty += fabs(hi);
feasible = feasible && fabs(hi) <= h[i].tol[k];
ICM = MAX(ICM, fabs(hi));
d.lambda[ii++] = MIN(MAX(lam_min, newlam), lam_max);
}
}
for (i = ii = 0; i < d.m; ++i) {
nlopt_eval_constraint(d.restmp, NULL, d.fc + i, n, xcur);
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
for (k = 0; k < d.fc[i].m; ++k) {
double fci = d.restmp[k];
double newmu = d.mu[ii] + d.rho * fci;
penalty += fci > 0 ? fci : 0;
feasible = feasible && fci <= fc[i].tol[k];
ICM = MAX(ICM, fabs(MAX(fci, -d.mu[ii] / d.rho)));
d.mu[ii++] = MIN(MAX(0.0, newmu), mu_max);
}
}
if (ICM > tau * prev_ICM) {
d.rho *= gam;
}
auglag_iters++;
if (auglag_verbose) {
printf("auglag %d: ICM=%g (%sfeasible), rho=%g\nauglag lambda=",
auglag_iters, ICM, feasible ? "" : "not ", d.rho);
for (i = 0; i < d.pp; ++i) printf(" %g", d.lambda[i]);
printf("\nauglag %d: mu = ", auglag_iters);
for (i = 0; i < d.mm; ++i) printf(" %g", d.mu[i]);
printf("\n");
}
if ((feasible && (!minf_feasible || penalty < minf_penalty
|| fcur < *minf)) ||
(!minf_feasible && penalty < minf_penalty)) {
ret = NLOPT_SUCCESS;
if (feasible) {
if (fcur < stop->minf_max)
ret = NLOPT_MINF_MAX_REACHED;
else if (nlopt_stop_ftol(stop, fcur, *minf))
ret = NLOPT_FTOL_REACHED;
else if (nlopt_stop_x(stop, xcur, x))
ret = NLOPT_XTOL_REACHED;
}
*minf = fcur;
minf_penalty = penalty;
minf_feasible = feasible;
memcpy(x, xcur, sizeof(double) * n);
if (ret != NLOPT_SUCCESS) break;
}
if (nlopt_stop_forced(stop)) {ret = NLOPT_FORCED_STOP; break;}
if (nlopt_stop_evals(stop)) {ret = NLOPT_MAXEVAL_REACHED; break;}
if (nlopt_stop_time(stop)) {ret = NLOPT_MAXTIME_REACHED; break;}
/* TODO: use some other stopping criterion on ICM? */
/* The paper uses ICM <= epsilon and DFM <= epsilon, where
DFM is a measure of the size of the Lagrangian gradient.
Besides the fact that these kinds of absolute tolerances
(non-scale-invariant) are unsatisfying and it is not
clear how the user should specify it, the ICM <= epsilon
condition seems not too different from requiring feasibility,
especially now that the user can provide constraint-specific
tolerances analogous to epsilon. */
if (ICM == 0) {ret = NLOPT_FTOL_REACHED; break;}
} while (1);
done:
free(xcur);
return ret;
}
void NloptOptimizationSolver<T>::solve ()
{
START_LOG("solve()", "NloptOptimizationSolver");
this->init ();
unsigned int nlopt_size = this->system().solution->size();
// We have to have an objective function
libmesh_assert( this->objective_object );
// Set routine for objective and (optionally) gradient evaluation
{
nlopt_result ierr =
nlopt_set_min_objective(_opt,
__libmesh_nlopt_objective,
this);
if (ierr < 0)
libmesh_error_msg("NLopt failed to set min objective: " << ierr);
}
if (this->lower_and_upper_bounds_object)
{
// Need to actually compute the bounds vectors first
this->lower_and_upper_bounds_object->lower_and_upper_bounds(this->system());
std::vector<Real> nlopt_lb(nlopt_size);
std::vector<Real> nlopt_ub(nlopt_size);
for(unsigned int i=0; i<nlopt_size; i++)
{
nlopt_lb[i] = this->system().get_vector("lower_bounds")(i);
nlopt_ub[i] = this->system().get_vector("upper_bounds")(i);
}
nlopt_set_lower_bounds(_opt, &nlopt_lb[0]);
nlopt_set_upper_bounds(_opt, &nlopt_ub[0]);
}
// If we have an equality constraints object, tell NLopt about it.
if (this->equality_constraints_object)
{
// NLopt requires a vector to specify the tolerance for each constraint.
// NLopt makes a copy of this vector internally, so it's safe for us to
// let it go out of scope.
std::vector<double> equality_constraints_tolerances(this->system().C_eq->size(),
_constraints_tolerance);
// It would be nice to call the C interface directly, at least it should return an error
// code we could parse... unfortunately, there does not seem to be a way to extract
// the underlying nlopt_opt object from the nlopt::opt class!
nlopt_result ierr =
nlopt_add_equality_mconstraint(_opt,
equality_constraints_tolerances.size(),
__libmesh_nlopt_equality_constraints,
this,
&equality_constraints_tolerances[0]);
if (ierr < 0)
libmesh_error_msg("NLopt failed to add equality constraint: " << ierr);
}
// If we have an inequality constraints object, tell NLopt about it.
if (this->inequality_constraints_object)
{
// NLopt requires a vector to specify the tolerance for each constraint
std::vector<double> inequality_constraints_tolerances(this->system().C_ineq->size(),
_constraints_tolerance);
nlopt_add_inequality_mconstraint(_opt,
inequality_constraints_tolerances.size(),
__libmesh_nlopt_inequality_constraints,
this,
&inequality_constraints_tolerances[0]);
}
// Set a relative tolerance on the optimization parameters
nlopt_set_ftol_rel(_opt, this->objective_function_relative_tolerance);
// Set the maximum number of allowed objective function evaluations
nlopt_set_maxeval(_opt, this->max_objective_function_evaluations);
// Reset internal iteration counter
this->_iteration_count = 0;
// Perform the optimization
std::vector<Real> x(nlopt_size);
Real min_val = 0.;
_result = nlopt_optimize(_opt, &x[0], &min_val);
if (_result < 0)
libMesh::out << "NLopt failed!" << std::endl;
else
libMesh::out << "NLopt obtained optimal value: "
<< min_val
<< " in "
<< this->get_iteration_count()
<< " iterations."
<< std::endl;
STOP_LOG("solve()", "NloptOptimizationSolver");
}
nlopt_result ccsa_quadratic_minimize(
unsigned n, nlopt_func f, void *f_data,
unsigned m, nlopt_constraint *fc,
nlopt_precond pre,
const double *lb, const double *ub, /* bounds */
double *x, /* in: initial guess, out: minimizer */
double *minf,
nlopt_stopping *stop,
nlopt_opt dual_opt)
{
nlopt_result ret = NLOPT_SUCCESS;
double *xcur, rho, *sigma, *dfdx, *dfdx_cur, *xprev, *xprevprev, fcur;
double *dfcdx, *dfcdx_cur;
double *fcval, *fcval_cur, *rhoc, *gcval, *y, *dual_lb, *dual_ub;
double *pre_lb, *pre_ub;
unsigned i, ifc, j, k = 0;
dual_data dd;
int feasible;
double infeasibility;
unsigned mfc;
unsigned no_precond;
nlopt_opt pre_opt = NULL;
m = nlopt_count_constraints(mfc = m, fc);
if (nlopt_get_dimension(dual_opt) != m) return NLOPT_INVALID_ARGS;
sigma = (double *) malloc(sizeof(double) * (6*n + 2*m*n + m*7));
if (!sigma) return NLOPT_OUT_OF_MEMORY;
dfdx = sigma + n;
dfdx_cur = dfdx + n;
xcur = dfdx_cur + n;
xprev = xcur + n;
xprevprev = xprev + n;
fcval = xprevprev + n;
fcval_cur = fcval + m;
rhoc = fcval_cur + m;
gcval = rhoc + m;
dual_lb = gcval + m;
dual_ub = dual_lb + m;
y = dual_ub + m;
dfcdx = y + m;
dfcdx_cur = dfcdx + m*n;
dd.n = n;
dd.x = x;
dd.lb = lb;
dd.ub = ub;
dd.sigma = sigma;
dd.dfdx = dfdx;
dd.dfcdx = dfcdx;
dd.fcval = fcval;
dd.rhoc = rhoc;
dd.xcur = xcur;
dd.gcval = gcval;
dd.pre = pre; dd.pre_data = f_data;
dd.prec = NULL; dd.prec_data = NULL;
dd.scratch = NULL;
if (m) {
dd.prec = (nlopt_precond *) malloc(sizeof(nlopt_precond) * m);
dd.prec_data = (void **) malloc(sizeof(void *) * m);
if (!dd.prec || !dd.prec_data) {
ret = NLOPT_OUT_OF_MEMORY;
goto done;
}
for (i = ifc = 0; ifc < mfc; ++ifc) {
unsigned inext = i + fc[ifc].m;
for (; i < inext; ++i) {
dd.prec[i] = fc[ifc].pre;
dd.prec_data[i] = fc[ifc].f_data;
}
}
}
no_precond = pre == NULL;
if (dd.prec)
for (i = 0; i < m; ++i)
no_precond = no_precond && dd.prec[i] == NULL;
if (!no_precond) {
dd.scratch = (double*) malloc(sizeof(double) * (4*n));
if (!dd.scratch) {
free(sigma);
return NLOPT_OUT_OF_MEMORY;
}
pre_lb = dd.scratch + 2*n;
pre_ub = pre_lb + n;
pre_opt = nlopt_create(nlopt_get_algorithm(dual_opt), n);
if (!pre_opt) { ret = NLOPT_FAILURE; goto done; }
ret = nlopt_set_min_objective(pre_opt, g0, &dd);
if (ret < 0) goto done;
ret = nlopt_add_inequality_mconstraint(pre_opt, m, gi, &dd, NULL);
if (ret < 0) goto done;
ret = nlopt_set_ftol_rel(pre_opt, nlopt_get_ftol_rel(dual_opt));
if (ret < 0) goto done;
ret = nlopt_set_ftol_abs(pre_opt, nlopt_get_ftol_abs(dual_opt));
if (ret < 0) goto done;
ret = nlopt_set_maxeval(pre_opt, nlopt_get_maxeval(dual_opt));
if (ret < 0) goto done;
}
for (j = 0; j < n; ++j) {
if (nlopt_isinf(ub[j]) || nlopt_isinf(lb[j]))
sigma[j] = 1.0; /* arbitrary default */
else
sigma[j] = 0.5 * (ub[j] - lb[j]);
}
rho = 1.0;
for (i = 0; i < m; ++i) {
rhoc[i] = 1.0;
dual_lb[i] = y[i] = 0.0;
dual_ub[i] = HUGE_VAL;
}
dd.fval = fcur = *minf = f(n, x, dfdx, f_data);
stop->nevals++;
memcpy(xcur, x, sizeof(double) * n);
if (nlopt_stop_forced(stop)) { ret = NLOPT_FORCED_STOP; goto done; }
feasible = 1; infeasibility = 0;
for (i = ifc = 0; ifc < mfc; ++ifc) {
nlopt_eval_constraint(fcval + i, dfcdx + i*n,
fc + ifc, n, x);
i += fc[ifc].m;
if (nlopt_stop_forced(stop)) { ret = NLOPT_FORCED_STOP; goto done; }
}
for (i = 0; i < m; ++i) {
feasible = feasible && fcval[i] <= 0;
if (fcval[i] > infeasibility) infeasibility = fcval[i];
}
/* For non-feasible initial points, set a finite (large)
upper-bound on the dual variables. What this means is that,
if no feasible solution is found from the dual problem, it
will minimize the dual objective with the unfeasible
constraint weighted by 1e40 -- basically, minimizing the
unfeasible constraint until it becomes feasible or until we at
least obtain a step towards a feasible point.
Svanberg suggested a different approach in his 1987 paper, basically
introducing additional penalty variables for unfeasible constraints,
but this is easier to implement and at least as efficient. */
if (!feasible)
for (i = 0; i < m; ++i) dual_ub[i] = 1e40;
nlopt_set_min_objective(dual_opt, dual_func, &dd);
nlopt_set_lower_bounds(dual_opt, dual_lb);
nlopt_set_upper_bounds(dual_opt, dual_ub);
nlopt_set_stopval(dual_opt, -HUGE_VAL);
nlopt_remove_inequality_constraints(dual_opt);
nlopt_remove_equality_constraints(dual_opt);
while (1) { /* outer iterations */
double fprev = fcur;
if (nlopt_stop_forced(stop)) ret = NLOPT_FORCED_STOP;
else if (nlopt_stop_evals(stop)) ret = NLOPT_MAXEVAL_REACHED;
else if (nlopt_stop_time(stop)) ret = NLOPT_MAXTIME_REACHED;
else if (feasible && *minf < stop->minf_max)
ret = NLOPT_MINF_MAX_REACHED;
if (ret != NLOPT_SUCCESS) goto done;
if (++k > 1) memcpy(xprevprev, xprev, sizeof(double) * n);
memcpy(xprev, xcur, sizeof(double) * n);
while (1) { /* inner iterations */
double min_dual, infeasibility_cur;
int feasible_cur, inner_done;
unsigned save_verbose;
nlopt_result reti;
if (no_precond) {
/* solve dual problem */
dd.rho = rho; dd.count = 0;
save_verbose = ccsa_verbose;
ccsa_verbose = 0; /* no recursive verbosity */
reti = nlopt_optimize_limited(dual_opt, y, &min_dual,
0,
stop->maxtime
- (nlopt_seconds()
- stop->start));
ccsa_verbose = save_verbose;
if (reti < 0 || reti == NLOPT_MAXTIME_REACHED) {
ret = reti;
goto done;
}
dual_func(m, y, NULL, &dd); /* evaluate final xcur etc. */
}
else {
double pre_min;
for (j = 0; j < n; ++j) {
pre_lb[j] = MAX(lb[j], x[j] - sigma[j]);
pre_ub[j] = MIN(ub[j], x[j] + sigma[j]);
xcur[j] = x[j];
}
nlopt_set_lower_bounds(pre_opt, pre_lb);
nlopt_set_upper_bounds(pre_opt, pre_ub);
dd.rho = rho; dd.count = 0;
save_verbose = ccsa_verbose;
ccsa_verbose = 0; /* no recursive verbosity */
reti = nlopt_optimize_limited(pre_opt, xcur, &pre_min,
0, stop->maxtime
- (nlopt_seconds()
- stop->start));
ccsa_verbose = save_verbose;
if (reti < 0 || reti == NLOPT_MAXTIME_REACHED) {
ret = reti;
goto done;
}
/* evaluate final xcur etc */
dd.gval = g0(n, xcur, NULL, &dd);
gi(m, dd.gcval, n, xcur, NULL, &dd);
}
if (ccsa_verbose) {
printf("CCSA dual converged in %d iters to g=%g:\n",
dd.count, dd.gval);
for (i = 0; i < MIN(ccsa_verbose, m); ++i)
printf(" CCSA y[%d]=%g, gc[%d]=%g\n",
i, y[i], i, dd.gcval[i]);
}
fcur = f(n, xcur, dfdx_cur, f_data);
stop->nevals++;
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
feasible_cur = 1; infeasibility_cur = 0;
inner_done = dd.gval >= fcur;
for (i = ifc = 0; ifc < mfc; ++ifc) {
nlopt_eval_constraint(fcval_cur + i, dfcdx_cur + i*n,
fc + ifc, n, xcur);
i += fc[ifc].m;
if (nlopt_stop_forced(stop)) {
ret = NLOPT_FORCED_STOP; goto done; }
}
for (i = ifc = 0; ifc < mfc; ++ifc) {
unsigned i0 = i, inext = i + fc[ifc].m;
for (; i < inext; ++i) {
feasible_cur = feasible_cur
&& fcval_cur[i] <= fc[ifc].tol[i-i0];
inner_done = inner_done &&
(dd.gcval[i] >= fcval_cur[i]);
if (fcval_cur[i] > infeasibility_cur)
infeasibility_cur = fcval_cur[i];
}
}
if ((fcur < *minf && (inner_done || feasible_cur || !feasible))
|| (!feasible && infeasibility_cur < infeasibility)) {
if (ccsa_verbose && !feasible_cur)
printf("CCSA - using infeasible point?\n");
dd.fval = *minf = fcur;
infeasibility = infeasibility_cur;
memcpy(fcval, fcval_cur, sizeof(double)*m);
memcpy(x, xcur, sizeof(double)*n);
memcpy(dfdx, dfdx_cur, sizeof(double)*n);
memcpy(dfcdx, dfcdx_cur, sizeof(double)*n*m);
/* once we have reached a feasible solution, the
algorithm should never make the solution infeasible
again (if inner_done), although the constraints may
be violated slightly by rounding errors etc. so we
must be a little careful about checking feasibility */
if (infeasibility_cur == 0) {
if (!feasible) { /* reset upper bounds to infin. */
for (i = 0; i < m; ++i) dual_ub[i] = HUGE_VAL;
nlopt_set_upper_bounds(dual_opt, dual_ub);
}
feasible = 1;
}
}
if (nlopt_stop_forced(stop)) ret = NLOPT_FORCED_STOP;
else if (nlopt_stop_evals(stop)) ret = NLOPT_MAXEVAL_REACHED;
else if (nlopt_stop_time(stop)) ret = NLOPT_MAXTIME_REACHED;
else if (feasible && *minf < stop->minf_max)
ret = NLOPT_MINF_MAX_REACHED;
if (ret != NLOPT_SUCCESS) goto done;
if (inner_done) break;
if (fcur > dd.gval)
rho = MIN(10*rho, 1.1 * (rho + (fcur-dd.gval) / dd.wval));
for (i = 0; i < m; ++i)
if (fcval_cur[i] > dd.gcval[i])
rhoc[i] =
MIN(10*rhoc[i],
1.1 * (rhoc[i] + (fcval_cur[i]-dd.gcval[i])
/ dd.wval));
if (ccsa_verbose)
printf("CCSA inner iteration: rho -> %g\n", rho);
for (i = 0; i < MIN(ccsa_verbose, m); ++i)
printf(" CCSA rhoc[%d] -> %g\n", i,rhoc[i]);
}
if (nlopt_stop_ftol(stop, fcur, fprev))
ret = NLOPT_FTOL_REACHED;
if (nlopt_stop_x(stop, xcur, xprev))
ret = NLOPT_XTOL_REACHED;
if (ret != NLOPT_SUCCESS) goto done;
/* update rho and sigma for iteration k+1 */
rho = MAX(0.1 * rho, CCSA_RHOMIN);
if (ccsa_verbose)
printf("CCSA outer iteration: rho -> %g\n", rho);
for (i = 0; i < m; ++i)
rhoc[i] = MAX(0.1 * rhoc[i], CCSA_RHOMIN);
for (i = 0; i < MIN(ccsa_verbose, m); ++i)
printf(" CCSA rhoc[%d] -> %g\n", i, rhoc[i]);
if (k > 1) {
for (j = 0; j < n; ++j) {
double dx2 = (xcur[j]-xprev[j]) * (xprev[j]-xprevprev[j]);
double gam = dx2 < 0 ? 0.7 : (dx2 > 0 ? 1.2 : 1);
sigma[j] *= gam;
if (!nlopt_isinf(ub[j]) && !nlopt_isinf(lb[j])) {
sigma[j] = MIN(sigma[j], 10*(ub[j]-lb[j]));
/* use a smaller lower bound than Svanberg's
0.01*(ub-lb), which seems unnecessarily large */
sigma[j] = MAX(sigma[j], 1e-8*(ub[j]-lb[j]));
}
}
for (j = 0; j < MIN(ccsa_verbose, n); ++j)
printf(" CCSA sigma[%d] -> %g\n",
j, sigma[j]);
}
}
done:
nlopt_destroy(pre_opt);
if (dd.scratch) free(dd.scratch);
if (m) {
free(dd.prec_data);
free(dd.prec);
}
free(sigma);
return ret;
}
void omxInvokeNLOPT(double *est, GradientOptimizerContext &goc)
{
goc.optName = "SLSQP";
goc.setupSimpleBounds();
goc.useGradient = true;
FitContext *fc = goc.fc;
int oldWanted = fc->wanted;
fc->wanted = 0;
omxState *globalState = fc->state;
nlopt_opt opt = nlopt_create(NLOPT_LD_SLSQP, fc->numParam);
goc.extraData = opt;
//local_opt = nlopt_create(NLOPT_LD_SLSQP, n); // Subsidiary algorithm
//nlopt_set_local_optimizer(opt, local_opt);
nlopt_set_lower_bounds(opt, goc.solLB.data());
nlopt_set_upper_bounds(opt, goc.solUB.data());
int eq, ieq;
globalState->countNonlinearConstraints(eq, ieq);
if (fc->CI) {
nlopt_set_xtol_rel(opt, 5e-3);
std::vector<double> tol(fc->numParam, std::numeric_limits<double>::epsilon());
nlopt_set_xtol_abs(opt, tol.data());
} else {
// The *2 is there to roughly equate accuracy with NPSOL.
nlopt_set_ftol_rel(opt, goc.ControlTolerance * 2);
nlopt_set_ftol_abs(opt, std::numeric_limits<double>::epsilon());
}
nlopt_set_min_objective(opt, SLSQP::nloptObjectiveFunction, &goc);
double feasibilityTolerance = Global->feasibilityTolerance;
SLSQP::context ctx(goc);
if (eq + ieq) {
ctx.origeq = eq;
if (ieq > 0){
goc.inequality.resize(ieq);
std::vector<double> tol(ieq, feasibilityTolerance);
nlopt_add_inequality_mconstraint(opt, ieq, SLSQP::nloptInequalityFunction, &goc, tol.data());
}
if (eq > 0){
goc.equality.resize(eq);
std::vector<double> tol(eq, feasibilityTolerance);
nlopt_add_equality_mconstraint(opt, eq, SLSQP::nloptEqualityFunction, &ctx, tol.data());
}
}
int priorIterations = fc->iterations;
int code = nlopt_optimize(opt, est, &fc->fit);
if (ctx.eqredundent) {
nlopt_remove_equality_constraints(opt);
eq -= ctx.eqredundent;
std::vector<double> tol(eq, feasibilityTolerance);
nlopt_add_equality_mconstraint(opt, eq, SLSQP::nloptEqualityFunction, &ctx, tol.data());
code = nlopt_optimize(opt, est, &fc->fit);
}
if (goc.verbose >= 2) mxLog("nlopt_optimize returned %d", code);
nlopt_destroy(opt);
fc->wanted = oldWanted;
if (code == NLOPT_INVALID_ARGS) {
Rf_error("NLOPT invoked with invalid arguments");
} else if (code == NLOPT_OUT_OF_MEMORY) {
Rf_error("NLOPT ran out of memory");
} else if (code == NLOPT_FORCED_STOP) {
if (fc->iterations - priorIterations <= 1) {
goc.informOut = INFORM_STARTING_VALUES_INFEASIBLE;
} else {
goc.informOut = INFORM_ITERATION_LIMIT;
}
} else if (code == NLOPT_ROUNDOFF_LIMITED) {
if (fc->iterations - priorIterations <= 2) {
Rf_error("%s: Failed due to singular matrix E or C in LSQ subproblem or "
"rank-deficient equality constraint subproblem or "
"positive directional derivative in line search", goc.optName);
} else {
goc.informOut = INFORM_NOT_AT_OPTIMUM; // is this correct? TODO
}
} else if (code < 0) {
Rf_error("NLOPT fatal error %d", code);
} else if (code == NLOPT_MAXEVAL_REACHED) {
goc.informOut = INFORM_ITERATION_LIMIT;
} else {
goc.informOut = INFORM_CONVERGED_OPTIMUM;
}
}
