/*
* 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);
}
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");
}
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;
}
}
