/* Returns step length which gives minimum of objective for
solution + theta * change vector up to maximum theta.
arrays are numberColumns+numberRows
*/
double
ClpAmplObjective::stepLength(ClpSimplex * model,
const double * solution,
const double * change,
double maximumTheta,
double & currentObj,
double & predictedObj,
double & thetaObj)
{
// Assume convex
CbcAmplInfo * info = (CbcAmplInfo *) amplObjective_;
ASL_pfgh* asl = info->asl_;
int numberColumns = n_var;;
double * tempSolution = new double [numberColumns];
double * tempGradient = new double [numberColumns];
// current
eval_f(amplObjective_, numberColumns, solution, true, currentObj);
double objA = currentObj;
double thetaA = 0.0;
// at maximum
int i;
for (i = 0; i < numberColumns; i++)
tempSolution[i] = solution[i] + maximumTheta * change[i];
eval_f(amplObjective_, numberColumns, tempSolution, true, thetaObj);
double objC = thetaObj;
double thetaC = maximumTheta;
double objB = 0.5 * (objA + objC);
double thetaB = 0.5 * maximumTheta;
double gradientNorm = 1.0e6;
while (gradientNorm > 1.0e-6 && thetaC - thetaA > 1.0e-8) {
for (i = 0; i < numberColumns; i++)
tempSolution[i] = solution[i] + thetaB * change[i];
eval_grad_f(amplObjective_, numberColumns, tempSolution, true, tempGradient);
eval_f(amplObjective_, numberColumns, tempSolution, false, objB);
double changeObj = 0.0;
gradientNorm = 0.0;
for (i = 0; i < numberColumns; i++) {
changeObj += tempGradient[i] * change[i];
gradientNorm += tempGradient[i] * tempGradient[i];
}
gradientNorm = fabs(changeObj) / sqrt(gradientNorm);
// Should try and get quadratic convergence by interpolation
if (changeObj < 0.0) {
// increasing is good
thetaA = thetaB;
} else {
// decreasing is good
thetaC = thetaB;
}
thetaB = 0.5 * (thetaA + thetaC);
}
delete [] tempSolution;
delete [] tempGradient;
predictedObj = objB;
return thetaB;
}
// Returns gradient
double *
ClpAmplObjective::gradient(const ClpSimplex * model,
const double * solution, double & offset, bool refresh,
int includeLinear)
{
if (model)
assert (model->optimizationDirection() == 1.0);
#ifndef NDEBUG
bool scaling = model && (model->rowScale() || model->objectiveScale() != 1.0 || model->optimizationDirection() != 1.0);
#endif
const double * cost = NULL;
if (model)
cost = model->costRegion();
if (!cost) {
// not in solve
cost = objective_;
#ifndef NDEBUG
scaling = false;
#endif
}
assert (!scaling);
if (!amplObjective_ || !solution || !activated_) {
offset = offset_;
return objective_;
} else {
if (refresh || !gradient_) {
CbcAmplInfo * info = (CbcAmplInfo *) amplObjective_;
ASL_pfgh* asl = info->asl_;
int numberColumns = n_var;;
if (!gradient_)
gradient_ = new double[numberColumns];
assert (solution);
eval_grad_f(amplObjective_, numberColumns, solution, true, gradient_);
// Is this best way?
double objValue = 0.0;
eval_f(amplObjective_, numberColumns, solution, false, objValue);
double objValue2 = 0.0;
for (int i = 0; i < numberColumns; i++)
objValue2 += gradient_[i] * solution[i];
offset_ = objValue2 - objValue; // or other way???
if (model && model->optimizationDirection() != 1.0) {
offset *= model->optimizationDirection();
for (int i = 0; i < numberColumns; i++)
gradient_[i] *= -1.0;
}
}
offset = offset_;
return gradient_;
}
}
/** This methods gives the linear part of the objective function */
void AmplTMINLP::getLinearPartOfObjective(double * obj)
{
Index n, m, nnz_jac_g, nnz_h_lag;
TNLP::IndexStyleEnum index_style = TNLP::FORTRAN_STYLE;
get_nlp_info( n, m, nnz_jac_g, nnz_h_lag, index_style);
eval_grad_f(n, NULL, 0,obj);
Index n_non_linear_b= 0;
Index n_non_linear_bi= 0;
Index n_non_linear_c= 0;
Index n_non_linear_ci = 0;
Index n_non_linear_o= 0;
Index n_non_linear_oi = 0;
Index n_binaries = 0;
Index n_integers = 0;
ampl_tnlp_->get_discrete_info(n_non_linear_b, n_non_linear_bi, n_non_linear_c,
n_non_linear_ci, n_non_linear_o, n_non_linear_oi,
n_binaries, n_integers);
// Now get the coefficients of variables wich are linear in the objective
// The first ones are not
Index start = 0;
Index end = n_non_linear_b;
for (int i=start; i<end; i++) {
obj[i] = 0.;
}
//next variables should be linear in the objective just skip them
// (from current end to (end + n_non_linear_c - n_non_linear_ci - n_non_linear_b;)
// Those are nonlinear in the objective
start = end + n_non_linear_c;
end = start + n_non_linear_o;
for (int i=start; i < end; i++) {
obj[i]=0.;
}
//The rest is linear keep the values of the gradient
}
void Sqpmethod::evaluate() {
if (inputs_check_) checkInputs();
checkInitialBounds();
if (gather_stats_) {
Dict iterations;
iterations["inf_pr"] = std::vector<double>();
iterations["inf_du"] = std::vector<double>();
iterations["ls_trials"] = std::vector<double>();
iterations["d_norm"] = std::vector<double>();
iterations["obj"] = std::vector<double>();
stats_["iterations"] = iterations;
}
// Get problem data
const vector<double>& x_init = input(NLP_SOLVER_X0).data();
const vector<double>& lbx = input(NLP_SOLVER_LBX).data();
const vector<double>& ubx = input(NLP_SOLVER_UBX).data();
const vector<double>& lbg = input(NLP_SOLVER_LBG).data();
const vector<double>& ubg = input(NLP_SOLVER_UBG).data();
// Set linearization point to initial guess
copy(x_init.begin(), x_init.end(), x_.begin());
// Initialize Lagrange multipliers of the NLP
copy(input(NLP_SOLVER_LAM_G0).begin(), input(NLP_SOLVER_LAM_G0).end(), mu_.begin());
copy(input(NLP_SOLVER_LAM_X0).begin(), input(NLP_SOLVER_LAM_X0).end(), mu_x_.begin());
t_eval_f_ = t_eval_grad_f_ = t_eval_g_ = t_eval_jac_g_ = t_eval_h_ =
t_callback_fun_ = t_callback_prepare_ = t_mainloop_ = 0;
n_eval_f_ = n_eval_grad_f_ = n_eval_g_ = n_eval_jac_g_ = n_eval_h_ = 0;
double time1 = clock();
// Initial constraint Jacobian
eval_jac_g(x_, gk_, Jk_);
// Initial objective gradient
eval_grad_f(x_, fk_, gf_);
// Initialize or reset the Hessian or Hessian approximation
reg_ = 0;
if (exact_hessian_) {
eval_h(x_, mu_, 1.0, Bk_);
} else {
reset_h();
}
// Evaluate the initial gradient of the Lagrangian
copy(gf_.begin(), gf_.end(), gLag_.begin());
if (ng_>0) casadi_mv_t(Jk_.ptr(), Jk_.sparsity(), getPtr(mu_), getPtr(gLag_));
// gLag += mu_x_;
transform(gLag_.begin(), gLag_.end(), mu_x_.begin(), gLag_.begin(), plus<double>());
// Number of SQP iterations
int iter = 0;
// Number of line-search iterations
int ls_iter = 0;
// Last linesearch successfull
bool ls_success = true;
// Reset
merit_mem_.clear();
sigma_ = 0.; // NOTE: Move this into the main optimization loop
// Default stepsize
double t = 0;
// MAIN OPTIMIZATION LOOP
while (true) {
// Primal infeasability
double pr_inf = primalInfeasibility(x_, lbx, ubx, gk_, lbg, ubg);
// inf-norm of lagrange gradient
double gLag_norminf = norm_inf(gLag_);
// inf-norm of step
double dx_norminf = norm_inf(dx_);
// Print header occasionally
if (iter % 10 == 0) printIteration(userOut());
// Printing information about the actual iterate
printIteration(userOut(), iter, fk_, pr_inf, gLag_norminf, dx_norminf,
reg_, ls_iter, ls_success);
if (gather_stats_) {
Dict iterations = stats_["iterations"];
std::vector<double> tmp=iterations["inf_pr"];
tmp.push_back(pr_inf);
iterations["inf_pr"] = tmp;
tmp=iterations["inf_du"];
tmp.push_back(gLag_norminf);
iterations["inf_du"] = tmp;
tmp=iterations["d_norm"];
tmp.push_back(dx_norminf);
iterations["d_norm"] = tmp;
std::vector<int> tmp2=iterations["ls_trials"];
tmp2.push_back(ls_iter);
iterations["ls_trials"] = tmp2;
tmp=iterations["obj"];
tmp.push_back(fk_);
iterations["obj"] = tmp;
stats_["iterations"] = iterations;
}
// Call callback function if present
if (!callback_.isNull()) {
double time1 = clock();
if (!output(NLP_SOLVER_F).isempty()) output(NLP_SOLVER_F).set(fk_);
if (!output(NLP_SOLVER_X).isempty()) output(NLP_SOLVER_X).setNZ(x_);
if (!output(NLP_SOLVER_LAM_G).isempty()) output(NLP_SOLVER_LAM_G).setNZ(mu_);
if (!output(NLP_SOLVER_LAM_X).isempty()) output(NLP_SOLVER_LAM_X).setNZ(mu_x_);
if (!output(NLP_SOLVER_G).isempty()) output(NLP_SOLVER_G).setNZ(gk_);
Dict iteration;
iteration["iter"] = iter;
iteration["inf_pr"] = pr_inf;
iteration["inf_du"] = gLag_norminf;
iteration["d_norm"] = dx_norminf;
iteration["ls_trials"] = ls_iter;
iteration["obj"] = fk_;
stats_["iteration"] = iteration;
double time2 = clock();
t_callback_prepare_ += (time2-time1)/CLOCKS_PER_SEC;
time1 = clock();
int ret = callback_(ref_, user_data_);
time2 = clock();
t_callback_fun_ += (time2-time1)/CLOCKS_PER_SEC;
if (ret) {
userOut() << endl;
userOut() << "casadi::SQPMethod: aborted by callback..." << endl;
stats_["return_status"] = "User_Requested_Stop";
break;
}
}
// Checking convergence criteria
if (pr_inf < tol_pr_ && gLag_norminf < tol_du_) {
userOut() << endl;
userOut() << "casadi::SQPMethod: Convergence achieved after "
<< iter << " iterations." << endl;
stats_["return_status"] = "Solve_Succeeded";
break;
}
if (iter >= max_iter_) {
userOut() << endl;
userOut() << "casadi::SQPMethod: Maximum number of iterations reached." << endl;
stats_["return_status"] = "Maximum_Iterations_Exceeded";
break;
}
if (iter > 0 && dx_norminf <= min_step_size_) {
userOut() << endl;
userOut() << "casadi::SQPMethod: Search direction becomes too small without "
"convergence criteria being met." << endl;
stats_["return_status"] = "Search_Direction_Becomes_Too_Small";
break;
}
// Start a new iteration
iter++;
log("Formulating QP");
// Formulate the QP
transform(lbx.begin(), lbx.end(), x_.begin(), qp_LBX_.begin(), minus<double>());
transform(ubx.begin(), ubx.end(), x_.begin(), qp_UBX_.begin(), minus<double>());
transform(lbg.begin(), lbg.end(), gk_.begin(), qp_LBA_.begin(), minus<double>());
transform(ubg.begin(), ubg.end(), gk_.begin(), qp_UBA_.begin(), minus<double>());
// Solve the QP
solve_QP(Bk_, gf_, qp_LBX_, qp_UBX_, Jk_, qp_LBA_, qp_UBA_, dx_, qp_DUAL_X_, qp_DUAL_A_);
log("QP solved");
// Detecting indefiniteness
double gain = casadi_quad_form(Bk_.ptr(), Bk_.sparsity(), getPtr(dx_));
if (gain < 0) {
casadi_warning("Indefinite Hessian detected...");
}
// Calculate penalty parameter of merit function
sigma_ = std::max(sigma_, 1.01*norm_inf(qp_DUAL_X_));
sigma_ = std::max(sigma_, 1.01*norm_inf(qp_DUAL_A_));
// Calculate L1-merit function in the actual iterate
double l1_infeas = primalInfeasibility(x_, lbx, ubx, gk_, lbg, ubg);
// Right-hand side of Armijo condition
double F_sens = inner_prod(dx_, gf_);
double L1dir = F_sens - sigma_ * l1_infeas;
double L1merit = fk_ + sigma_ * l1_infeas;
// Storing the actual merit function value in a list
merit_mem_.push_back(L1merit);
if (merit_mem_.size() > merit_memsize_) {
merit_mem_.pop_front();
}
// Stepsize
t = 1.0;
double fk_cand;
// Merit function value in candidate
double L1merit_cand = 0;
// Reset line-search counter, success marker
ls_iter = 0;
ls_success = true;
// Line-search
log("Starting line-search");
if (max_iter_ls_>0) { // max_iter_ls_== 0 disables line-search
// Line-search loop
while (true) {
for (int i=0; i<nx_; ++i) x_cand_[i] = x_[i] + t * dx_[i];
try {
// Evaluating objective and constraints
eval_f(x_cand_, fk_cand);
eval_g(x_cand_, gk_cand_);
} catch(const CasadiException& ex) {
// Silent ignore; line-search failed
ls_iter++;
// Backtracking
t = beta_ * t;
continue;
}
ls_iter++;
// Calculating merit-function in candidate
l1_infeas = primalInfeasibility(x_cand_, lbx, ubx, gk_cand_, lbg, ubg);
L1merit_cand = fk_cand + sigma_ * l1_infeas;
// Calculating maximal merit function value so far
double meritmax = *max_element(merit_mem_.begin(), merit_mem_.end());
if (L1merit_cand <= meritmax + t * c1_ * L1dir) {
// Accepting candidate
log("Line-search completed, candidate accepted");
break;
}
// Line-search not successful, but we accept it.
if (ls_iter == max_iter_ls_) {
ls_success = false;
log("Line-search completed, maximum number of iterations");
break;
}
// Backtracking
t = beta_ * t;
}
// Candidate accepted, update dual variables
for (int i=0; i<ng_; ++i) mu_[i] = t * qp_DUAL_A_[i] + (1 - t) * mu_[i];
for (int i=0; i<nx_; ++i) mu_x_[i] = t * qp_DUAL_X_[i] + (1 - t) * mu_x_[i];
// Candidate accepted, update the primal variable
copy(x_.begin(), x_.end(), x_old_.begin());
copy(x_cand_.begin(), x_cand_.end(), x_.begin());
} else {
// Full step
copy(qp_DUAL_A_.begin(), qp_DUAL_A_.end(), mu_.begin());
copy(qp_DUAL_X_.begin(), qp_DUAL_X_.end(), mu_x_.begin());
copy(x_.begin(), x_.end(), x_old_.begin());
// x+=dx
transform(x_.begin(), x_.end(), dx_.begin(), x_.begin(), plus<double>());
}
if (!exact_hessian_) {
// Evaluate the gradient of the Lagrangian with the old x but new mu (for BFGS)
copy(gf_.begin(), gf_.end(), gLag_old_.begin());
if (ng_>0) casadi_mv_t(Jk_.ptr(), Jk_.sparsity(), getPtr(mu_), getPtr(gLag_old_));
// gLag_old += mu_x_;
transform(gLag_old_.begin(), gLag_old_.end(), mu_x_.begin(), gLag_old_.begin(),
plus<double>());
}
// Evaluate the constraint Jacobian
log("Evaluating jac_g");
eval_jac_g(x_, gk_, Jk_);
// Evaluate the gradient of the objective function
log("Evaluating grad_f");
eval_grad_f(x_, fk_, gf_);
// Evaluate the gradient of the Lagrangian with the new x and new mu
copy(gf_.begin(), gf_.end(), gLag_.begin());
if (ng_>0) casadi_mv_t(Jk_.ptr(), Jk_.sparsity(), getPtr(mu_), getPtr(gLag_));
// gLag += mu_x_;
transform(gLag_.begin(), gLag_.end(), mu_x_.begin(), gLag_.begin(), plus<double>());
// Updating Lagrange Hessian
if (!exact_hessian_) {
log("Updating Hessian (BFGS)");
// BFGS with careful updates and restarts
if (iter % lbfgs_memory_ == 0) {
// Reset Hessian approximation by dropping all off-diagonal entries
const int* colind = Bk_.colind(); // Access sparsity (column offset)
int ncol = Bk_.size2();
const int* row = Bk_.row(); // Access sparsity (row)
vector<double>& data = Bk_.data(); // Access nonzero elements
for (int cc=0; cc<ncol; ++cc) { // Loop over the columns of the Hessian
for (int el=colind[cc]; el<colind[cc+1]; ++el) {
// Loop over the nonzero elements of the column
if (cc!=row[el]) data[el] = 0; // Remove if off-diagonal entries
}
}
}
// Pass to BFGS update function
bfgs_.setInput(Bk_, BFGS_BK);
bfgs_.setInputNZ(x_, BFGS_X);
bfgs_.setInputNZ(x_old_, BFGS_X_OLD);
bfgs_.setInputNZ(gLag_, BFGS_GLAG);
bfgs_.setInputNZ(gLag_old_, BFGS_GLAG_OLD);
// Update the Hessian approximation
bfgs_.evaluate();
// Get the updated Hessian
bfgs_.getOutput(Bk_);
if (monitored("bfgs")) {
userOut() << "x = " << x_ << endl;
userOut() << "BFGS = " << endl;
Bk_.printSparse();
}
} else {
// Exact Hessian
log("Evaluating hessian");
eval_h(x_, mu_, 1.0, Bk_);
}
}
double time2 = clock();
t_mainloop_ = (time2-time1)/CLOCKS_PER_SEC;
// Save results to outputs
output(NLP_SOLVER_F).set(fk_);
output(NLP_SOLVER_X).setNZ(x_);
output(NLP_SOLVER_LAM_G).setNZ(mu_);
output(NLP_SOLVER_LAM_X).setNZ(mu_x_);
output(NLP_SOLVER_G).setNZ(gk_);
if (hasOption("print_time") && static_cast<bool>(getOption("print_time"))) {
// Write timings
userOut() << "time spent in eval_f: " << t_eval_f_ << " s.";
if (n_eval_f_>0)
userOut() << " (" << n_eval_f_ << " calls, " << (t_eval_f_/n_eval_f_)*1000
<< " ms. average)";
userOut() << endl;
userOut() << "time spent in eval_grad_f: " << t_eval_grad_f_ << " s.";
if (n_eval_grad_f_>0)
userOut() << " (" << n_eval_grad_f_ << " calls, "
<< (t_eval_grad_f_/n_eval_grad_f_)*1000 << " ms. average)";
userOut() << endl;
userOut() << "time spent in eval_g: " << t_eval_g_ << " s.";
if (n_eval_g_>0)
userOut() << " (" << n_eval_g_ << " calls, " << (t_eval_g_/n_eval_g_)*1000
<< " ms. average)";
userOut() << endl;
userOut() << "time spent in eval_jac_g: " << t_eval_jac_g_ << " s.";
if (n_eval_jac_g_>0)
userOut() << " (" << n_eval_jac_g_ << " calls, "
<< (t_eval_jac_g_/n_eval_jac_g_)*1000 << " ms. average)";
userOut() << endl;
userOut() << "time spent in eval_h: " << t_eval_h_ << " s.";
if (n_eval_h_>1)
userOut() << " (" << n_eval_h_ << " calls, " << (t_eval_h_/n_eval_h_)*1000
<< " ms. average)";
userOut() << endl;
userOut() << "time spent in main loop: " << t_mainloop_ << " s." << endl;
userOut() << "time spent in callback function: " << t_callback_fun_ << " s." << endl;
userOut() << "time spent in callback preparation: " << t_callback_prepare_ << " s." << endl;
}
// Save statistics
stats_["iter_count"] = iter;
stats_["t_eval_f"] = t_eval_f_;
stats_["t_eval_grad_f"] = t_eval_grad_f_;
stats_["t_eval_g"] = t_eval_g_;
stats_["t_eval_jac_g"] = t_eval_jac_g_;
stats_["t_eval_h"] = t_eval_h_;
stats_["t_mainloop"] = t_mainloop_;
stats_["t_callback_fun"] = t_callback_fun_;
stats_["t_callback_prepare"] = t_callback_prepare_;
stats_["n_eval_f"] = n_eval_f_;
stats_["n_eval_grad_f"] = n_eval_grad_f_;
stats_["n_eval_g"] = n_eval_g_;
stats_["n_eval_jac_g"] = n_eval_jac_g_;
stats_["n_eval_h"] = n_eval_h_;
}
void SQPInternal::evaluate(int nfdir, int nadir){
casadi_assert(nfdir==0 && nadir==0);
checkInitialBounds();
// Get problem data
const vector<double>& x_init = input(NLP_X_INIT).data();
const vector<double>& lbx = input(NLP_LBX).data();
const vector<double>& ubx = input(NLP_UBX).data();
const vector<double>& lbg = input(NLP_LBG).data();
const vector<double>& ubg = input(NLP_UBG).data();
// Set the static parameter
if (parametric_) {
const vector<double>& p = input(NLP_P).data();
if (!F_.isNull()) F_.setInput(p,F_.getNumInputs()-1);
if (!G_.isNull()) G_.setInput(p,G_.getNumInputs()-1);
if (!H_.isNull()) H_.setInput(p,H_.getNumInputs()-1);
if (!J_.isNull()) J_.setInput(p,J_.getNumInputs()-1);
}
// Set linearization point to initial guess
copy(x_init.begin(),x_init.end(),x_.begin());
// Lagrange multipliers of the NLP
fill(mu_.begin(),mu_.end(),0);
fill(mu_x_.begin(),mu_x_.end(),0);
// Initial constraint Jacobian
eval_jac_g(x_,gk_,Jk_);
// Initial objective gradient
eval_grad_f(x_,fk_,gf_);
// Initialize or reset the Hessian or Hessian approximation
reg_ = 0;
if( hess_mode_ == HESS_BFGS){
reset_h();
} else {
eval_h(x_,mu_,1.0,Bk_);
}
// Evaluate the initial gradient of the Lagrangian
copy(gf_.begin(),gf_.end(),gLag_.begin());
if(m_>0) DMatrix::mul_no_alloc_tn(Jk_,mu_,gLag_);
// gLag += mu_x_;
transform(gLag_.begin(),gLag_.end(),mu_x_.begin(),gLag_.begin(),plus<double>());
// Number of SQP iterations
int iter = 0;
// Number of line-search iterations
int ls_iter = 0;
// Last linesearch successfull
bool ls_success = true;
// Reset
merit_mem_.clear();
sigma_ = 0.; // NOTE: Move this into the main optimization loop
// Default stepsize
double t = 0;
// MAIN OPTIMIZATION LOOP
while(true){
// Primal infeasability
double pr_inf = primalInfeasibility(x_, lbx, ubx, gk_, lbg, ubg);
// 1-norm of lagrange gradient
double gLag_norm1 = norm_1(gLag_);
// 1-norm of step
double dx_norm1 = norm_1(dx_);
// Print header occasionally
if(iter % 10 == 0) printIteration(cout);
// Printing information about the actual iterate
printIteration(cout,iter,fk_,pr_inf,gLag_norm1,dx_norm1,reg_,ls_iter,ls_success);
// Call callback function if present
if (!callback_.isNull()) {
callback_.input(NLP_COST).set(fk_);
callback_.input(NLP_X_OPT).set(x_);
callback_.input(NLP_LAMBDA_G).set(mu_);
callback_.input(NLP_LAMBDA_X).set(mu_x_);
callback_.input(NLP_G).set(gk_);
callback_.evaluate();
if (callback_.output(0).at(0)) {
cout << endl;
cout << "CasADi::SQPMethod: aborted by callback..." << endl;
break;
}
}
// Checking convergence criteria
if (pr_inf < tol_pr_ && gLag_norm1 < tol_du_){
cout << endl;
cout << "CasADi::SQPMethod: Convergence achieved after " << iter << " iterations." << endl;
break;
}
if (iter >= maxiter_){
cout << endl;
cout << "CasADi::SQPMethod: Maximum number of iterations reached." << endl;
break;
}
// Start a new iteration
iter++;
// Formulate the QP
transform(lbx.begin(),lbx.end(),x_.begin(),qp_LBX_.begin(),minus<double>());
transform(ubx.begin(),ubx.end(),x_.begin(),qp_UBX_.begin(),minus<double>());
transform(lbg.begin(),lbg.end(),gk_.begin(),qp_LBA_.begin(),minus<double>());
transform(ubg.begin(),ubg.end(),gk_.begin(),qp_UBA_.begin(),minus<double>());
// Solve the QP
solve_QP(Bk_,gf_,qp_LBX_,qp_UBX_,Jk_,qp_LBA_,qp_UBA_,dx_,qp_DUAL_X_,qp_DUAL_A_);
log("QP solved");
// Detecting indefiniteness
double gain = quad_form(dx_,Bk_);
if (gain < 0){
casadi_warning("Indefinite Hessian detected...");
}
// Calculate penalty parameter of merit function
sigma_ = std::max(sigma_,1.01*norm_inf(qp_DUAL_X_));
sigma_ = std::max(sigma_,1.01*norm_inf(qp_DUAL_A_));
// Calculate L1-merit function in the actual iterate
double l1_infeas = primalInfeasibility(x_, lbx, ubx, gk_, lbg, ubg);
// Right-hand side of Armijo condition
double F_sens = inner_prod(dx_, gf_);
double L1dir = F_sens - sigma_ * l1_infeas;
double L1merit = fk_ + sigma_ * l1_infeas;
// Storing the actual merit function value in a list
merit_mem_.push_back(L1merit);
if (merit_mem_.size() > merit_memsize_){
merit_mem_.pop_front();
}
// Stepsize
t = 1.0;
double fk_cand;
// Merit function value in candidate
double L1merit_cand = 0;
// Reset line-search counter, success marker
ls_iter = 0;
ls_success = true;
// Line-search
log("Starting line-search");
if(maxiter_ls_>0){ // maxiter_ls_== 0 disables line-search
// Line-search loop
while (true){
for(int i=0; i<n_; ++i) x_cand_[i] = x_[i] + t * dx_[i];
// Evaluating objective and constraints
eval_f(x_cand_,fk_cand);
eval_g(x_cand_,gk_cand_);
ls_iter++;
// Calculating merit-function in candidate
l1_infeas = primalInfeasibility(x_cand_, lbx, ubx, gk_cand_, lbg, ubg);
L1merit_cand = fk_cand + sigma_ * l1_infeas;
// Calculating maximal merit function value so far
double meritmax = *max_element(merit_mem_.begin(), merit_mem_.end());
if (L1merit_cand <= meritmax + t * c1_ * L1dir){
// Accepting candidate
log("Line-search completed, candidate accepted");
break;
}
// Line-search not successful, but we accept it.
if(ls_iter == maxiter_ls_){
ls_success = false;
log("Line-search completed, maximum number of iterations");
break;
}
// Backtracking
t = beta_ * t;
}
}
// Candidate accepted, update dual variables
for(int i=0; i<m_; ++i) mu_[i] = t * qp_DUAL_A_[i] + (1 - t) * mu_[i];
for(int i=0; i<n_; ++i) mu_x_[i] = t * qp_DUAL_X_[i] + (1 - t) * mu_x_[i];
if( hess_mode_ == HESS_BFGS){
// Evaluate the gradient of the Lagrangian with the old x but new mu (for BFGS)
copy(gf_.begin(),gf_.end(),gLag_old_.begin());
if(m_>0) DMatrix::mul_no_alloc_tn(Jk_,mu_,gLag_old_);
// gLag_old += mu_x_;
transform(gLag_old_.begin(),gLag_old_.end(),mu_x_.begin(),gLag_old_.begin(),plus<double>());
}
// Candidate accepted, update the primal variable
copy(x_.begin(),x_.end(),x_old_.begin());
copy(x_cand_.begin(),x_cand_.end(),x_.begin());
// Evaluate the constraint Jacobian
log("Evaluating jac_g");
eval_jac_g(x_,gk_,Jk_);
// Evaluate the gradient of the objective function
log("Evaluating grad_f");
eval_grad_f(x_,fk_,gf_);
// Evaluate the gradient of the Lagrangian with the new x and new mu
copy(gf_.begin(),gf_.end(),gLag_.begin());
if(m_>0) DMatrix::mul_no_alloc_tn(Jk_,mu_,gLag_);
// gLag += mu_x_;
transform(gLag_.begin(),gLag_.end(),mu_x_.begin(),gLag_.begin(),plus<double>());
// Updating Lagrange Hessian
if( hess_mode_ == HESS_BFGS){
log("Updating Hessian (BFGS)");
// BFGS with careful updates and restarts
if (iter % lbfgs_memory_ == 0){
// Reset Hessian approximation by dropping all off-diagonal entries
const vector<int>& rowind = Bk_.rowind(); // Access sparsity (row offset)
const vector<int>& col = Bk_.col(); // Access sparsity (column)
vector<double>& data = Bk_.data(); // Access nonzero elements
for(int i=0; i<rowind.size()-1; ++i){ // Loop over the rows of the Hessian
for(int el=rowind[i]; el<rowind[i+1]; ++el){ // Loop over the nonzero elements of the row
if(i!=col[el]) data[el] = 0; // Remove if off-diagonal entries
}
}
}
// Pass to BFGS update function
bfgs_.setInput(Bk_,BFGS_BK);
bfgs_.setInput(x_,BFGS_X);
bfgs_.setInput(x_old_,BFGS_X_OLD);
bfgs_.setInput(gLag_,BFGS_GLAG);
bfgs_.setInput(gLag_old_,BFGS_GLAG_OLD);
// Update the Hessian approximation
bfgs_.evaluate();
// Get the updated Hessian
bfgs_.getOutput(Bk_);
} else {
// Exact Hessian
log("Evaluating hessian");
eval_h(x_,mu_,1.0,Bk_);
}
}
// Save results to outputs
output(NLP_COST).set(fk_);
output(NLP_X_OPT).set(x_);
output(NLP_LAMBDA_G).set(mu_);
output(NLP_LAMBDA_X).set(mu_x_);
output(NLP_G).set(gk_);
// Save statistics
stats_["iter_count"] = iter;
}
void WorhpInternal::evaluate(){
log("WorhpInternal::evaluate");
if (gather_stats_) {
Dictionary iterations;
iterations["iter_sqp"] = std::vector<int>();
iterations["inf_pr"] = std::vector<double>();
iterations["inf_du"] = std::vector<double>();
iterations["obj"] = std::vector<double>();
iterations["alpha_pr"] = std::vector<double>();
stats_["iterations"] = iterations;
}
// Prepare the solver
reset();
if (inputs_check_) checkInputs();
checkInitialBounds();
// Reset the counters
t_eval_f_ = t_eval_grad_f_ = t_eval_g_ = t_eval_jac_g_ = t_eval_h_ = t_callback_fun_ = t_callback_prepare_ = t_mainloop_ = 0;
n_eval_f_ = n_eval_grad_f_ = n_eval_g_ = n_eval_jac_g_ = n_eval_h_ = 0;
// Get inputs
log("WorhpInternal::evaluate: Reading user inputs");
const DMatrix& x0 = input(NLP_SOLVER_X0);
const DMatrix& lbx = input(NLP_SOLVER_LBX);
const DMatrix& ubx = input(NLP_SOLVER_UBX);
const DMatrix& lam_x0 = input(NLP_SOLVER_LAM_X0);
const DMatrix& lbg = input(NLP_SOLVER_LBG);
const DMatrix& ubg = input(NLP_SOLVER_UBG);
const DMatrix& lam_g0 = input(NLP_SOLVER_LAM_G0);
double inf = numeric_limits<double>::infinity();
for (int i=0;i<nx_;++i) {
casadi_assert_message(lbx.at(i)!=ubx.at(i),"WorhpSolver::evaluate: Worhp cannot handle the case when LBX == UBX. You have that case at non-zero " << i << " , which has value " << ubx.at(i) << ". Reformulate your problem by using a parameter for the corresponding variable.");
}
for (int i=0;i<lbg.size();++i) {
casadi_assert_message(!(lbg.at(i)==-inf && ubg.at(i) == inf),"WorhpSolver::evaluate: Worhp cannot handle the case when both LBG and UBG are infinite. You have that case at non-zero " << i << ". Reformulate your problem eliminating the corresponding constraint.");
}
// Pass inputs to WORHP data structures
x0.getArray(worhp_o_.X,worhp_o_.n);
lbx.getArray(worhp_o_.XL,worhp_o_.n);
ubx.getArray(worhp_o_.XU,worhp_o_.n);
lam_x0.getArray(worhp_o_.Lambda,worhp_o_.n);
if (worhp_o_.m>0){
lam_g0.getArray(worhp_o_.Mu,worhp_o_.m);
lbg.getArray(worhp_o_.GL,worhp_o_.m);
ubg.getArray(worhp_o_.GU,worhp_o_.m);
}
// Replace infinite bounds with worhp_p_.Infty
for(int i=0; i<nx_; ++i) if(worhp_o_.XL[i]==-inf) worhp_o_.XL[i] = -worhp_p_.Infty;
for(int i=0; i<nx_; ++i) if(worhp_o_.XU[i]== inf) worhp_o_.XU[i] = worhp_p_.Infty;
for(int i=0; i<ng_; ++i) if(worhp_o_.GL[i]==-inf) worhp_o_.GL[i] = -worhp_p_.Infty;
for(int i=0; i<ng_; ++i) if(worhp_o_.GU[i]== inf) worhp_o_.GU[i] = worhp_p_.Infty;
log("WorhpInternal::starting iteration");
double time1 = clock();
// Reverse Communication loop
while(worhp_c_.status < TerminateSuccess && worhp_c_.status > TerminateError) {
if (GetUserAction(&worhp_c_, callWorhp)) {
Worhp(&worhp_o_, &worhp_w_, &worhp_p_, &worhp_c_);
}
if (GetUserAction(&worhp_c_, iterOutput)) {
if (!worhp_w_.FirstIteration) {
if (gather_stats_) {
Dictionary & iterations = stats_["iterations"];
static_cast<std::vector<int> &>(iterations["iter_sqp"]).push_back(worhp_w_.MinorIter);
static_cast<std::vector<double> &>(iterations["inf_pr"]).push_back(worhp_w_.NormMax_CV);
static_cast<std::vector<double> &>(iterations["inf_du"]).push_back(worhp_w_.ScaledKKT);
static_cast<std::vector<double> &>(iterations["obj"]).push_back(worhp_o_.F);
static_cast<std::vector<double> &>(iterations["alpha_pr"]).push_back(worhp_w_.ArmijoAlpha);
}
if (!callback_.isNull()) {
double time1 = clock();
// Copy outputs
if (!output(NLP_SOLVER_X).isEmpty())
output(NLP_SOLVER_X).setArray(worhp_o_.X,worhp_o_.n);
if (!output(NLP_SOLVER_F).isEmpty())
output(NLP_SOLVER_F).set(worhp_o_.F);
if (!output(NLP_SOLVER_G).isEmpty())
output(NLP_SOLVER_G).setArray(worhp_o_.G,worhp_o_.m);
if (!output(NLP_SOLVER_LAM_X).isEmpty())
output(NLP_SOLVER_LAM_X).setArray(worhp_o_.Lambda,worhp_o_.n);
if (!output(NLP_SOLVER_LAM_G).isEmpty())
output(NLP_SOLVER_LAM_G).setArray(worhp_o_.Mu,worhp_o_.m);
Dictionary iteration;
iteration["iter"] = worhp_w_.MajorIter;
iteration["iter_sqp"] = worhp_w_.MinorIter;
iteration["inf_pr"] = worhp_w_.NormMax_CV;
iteration["inf_du"] = worhp_w_.ScaledKKT;
iteration["obj"] = worhp_o_.F;
iteration["alpha_pr"] = worhp_w_.ArmijoAlpha;
stats_["iteration"] = iteration;
double time2 = clock();
t_callback_prepare_ += double(time2-time1)/CLOCKS_PER_SEC;
time1 = clock();
int ret = callback_(ref_,user_data_);
time2 = clock();
t_callback_fun_ += double(time2-time1)/CLOCKS_PER_SEC;
if(ret) worhp_c_.status = TerminatedByUser;
}
}
IterationOutput(&worhp_o_, &worhp_w_, &worhp_p_, &worhp_c_);
DoneUserAction(&worhp_c_, iterOutput);
}
if (GetUserAction(&worhp_c_, evalF)) {
eval_f(worhp_o_.X, worhp_w_.ScaleObj, worhp_o_.F);
DoneUserAction(&worhp_c_, evalF);
}
if (GetUserAction(&worhp_c_, evalG)) {
eval_g(worhp_o_.X, worhp_o_.G);
DoneUserAction(&worhp_c_, evalG);
}
if (GetUserAction(&worhp_c_, evalDF)) {
eval_grad_f(worhp_o_.X, worhp_w_.ScaleObj, worhp_w_.DF.val);
DoneUserAction(&worhp_c_, evalDF);
}
if (GetUserAction(&worhp_c_, evalDG)) {
eval_jac_g(worhp_o_.X,worhp_w_.DG.val);
DoneUserAction(&worhp_c_, evalDG);
}
if (GetUserAction(&worhp_c_, evalHM)) {
eval_h(worhp_o_.X, worhp_w_.ScaleObj, worhp_o_.Mu, worhp_w_.HM.val);
DoneUserAction(&worhp_c_, evalHM);
}
if (GetUserAction(&worhp_c_, fidif)) {
WorhpFidif(&worhp_o_, &worhp_w_, &worhp_p_, &worhp_c_);
}
}
double time2 = clock();
t_mainloop_ += double(time2-time1)/CLOCKS_PER_SEC;
// Copy outputs
output(NLP_SOLVER_X).setArray(worhp_o_.X,worhp_o_.n,DENSE);
output(NLP_SOLVER_F).set(worhp_o_.F);
output(NLP_SOLVER_G).setArray(worhp_o_.G,worhp_o_.m,DENSE);
output(NLP_SOLVER_LAM_X).setArray(worhp_o_.Lambda,worhp_o_.n);
output(NLP_SOLVER_LAM_G).setArray(worhp_o_.Mu,worhp_o_.m,DENSE);
StatusMsg(&worhp_o_, &worhp_w_, &worhp_p_, &worhp_c_);
if (hasOption("print_time") && bool(getOption("print_time"))) {
// Write timings
cout << "time spent in eval_f: " << t_eval_f_ << " s.";
if (n_eval_f_>0)
cout << " (" << n_eval_f_ << " calls, " << (t_eval_f_/n_eval_f_)*1000 << " ms. average)";
cout << endl;
cout << "time spent in eval_grad_f: " << t_eval_grad_f_ << " s.";
if (n_eval_grad_f_>0)
cout << " (" << n_eval_grad_f_ << " calls, " << (t_eval_grad_f_/n_eval_grad_f_)*1000 << " ms. average)";
cout << endl;
cout << "time spent in eval_g: " << t_eval_g_ << " s.";
if (n_eval_g_>0)
cout << " (" << n_eval_g_ << " calls, " << (t_eval_g_/n_eval_g_)*1000 << " ms. average)";
cout << endl;
cout << "time spent in eval_jac_g: " << t_eval_jac_g_ << " s.";
if (n_eval_jac_g_>0)
cout << " (" << n_eval_jac_g_ << " calls, " << (t_eval_jac_g_/n_eval_jac_g_)*1000 << " ms. average)";
cout << endl;
cout << "time spent in eval_h: " << t_eval_h_ << " s.";
if (n_eval_h_>1)
cout << " (" << n_eval_h_ << " calls, " << (t_eval_h_/n_eval_h_)*1000 << " ms. average)";
cout << endl;
cout << "time spent in main loop: " << t_mainloop_ << " s." << endl;
cout << "time spent in callback function: " << t_callback_fun_ << " s." << endl;
cout << "time spent in callback preparation: " << t_callback_prepare_ << " s." << endl;
}
stats_["t_eval_f"] = t_eval_f_;
stats_["t_eval_grad_f"] = t_eval_grad_f_;
stats_["t_eval_g"] = t_eval_g_;
stats_["t_eval_jac_g"] = t_eval_jac_g_;
stats_["t_eval_h"] = t_eval_h_;
stats_["t_mainloop"] = t_mainloop_;
stats_["t_callback_fun"] = t_callback_fun_;
stats_["t_callback_prepare"] = t_callback_prepare_;
stats_["n_eval_f"] = n_eval_f_;
stats_["n_eval_grad_f"] = n_eval_grad_f_;
stats_["n_eval_g"] = n_eval_g_;
stats_["n_eval_jac_g"] = n_eval_jac_g_;
stats_["n_eval_h"] = n_eval_h_;
stats_["iter_count"] = worhp_w_.MajorIter;
stats_["return_code"] = worhp_c_.status;
stats_["return_status"] = flagmap[worhp_c_.status];
}
