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 RealtimeAPCSCP::solve() { //Initialisation (calculate the exact solution for inital point) int t_act = 0; std::cout << "Time: " + to_string(t_act) << std::endl; startIt = std::chrono::system_clock::now(); startSolver = std::chrono::system_clock::now(); //Solve the first problem exactly std::map<std::string, Matrix<double> > arg = make_map("lbx", model->getVMIN(), "ubx", model->getVMAX(), "lbg", this->G_bound, "ubg", this->G_bound, "x0", model->getVINIT()); NlpSolver nlpSolver = NlpSolver("solver", "ipopt", nlp, opts); nlpSolver.setOption("warn_initial_bounds", true); nlpSolver.setOption("eval_errors_fatal", true); std::map<string, Matrix<double>> result_tact = nlpSolver(arg); endSolver = std::chrono::system_clock::now(); // Store data assumedData[t_act] = result_tact["x"]; //Setup new iteration model->storeAndShiftValues(result_tact, t_act); storeStatsIpopt(&nlpSolver); //Define values this->x_act = result_tact["x"]; this->y_act = result_tact["lam_g"]; evaluateOriginalF(t_act,x_act); updateA_act(t_act); updateH_act(t_act); updateM_act(); m_act = mul(transpose(Dg_act - A_act), y_act); firstIteration = false; opts["warm_start_init_point"] = "yes"; endIt = std::chrono::system_clock::now(); printTimingData(t_act); //Iteration for (t_act = 1; t_act < model->getN_F(); t_act++) { std::cout << "Time: " + to_string(t_act) << std::endl; startIt = std::chrono::system_clock::now(); startMS = std::chrono::system_clock::now(); G_sub = mul(A_act, model->getV() - x_act) + model->doMultipleShooting(x_act); endMS = std::chrono::system_clock::now(); f_sub = model->getf(t_act) + mul(transpose(m_act), model->getV() - x_act) + 0.5 * quad_form(model->getV() - x_act, H_act); this->nlp_sub = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut( "f", f_sub, "g", G_sub)); this->nlp = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut("f", model->getf(t_act), "g", G)); // Step2 solve convex subproblem startSolver = std::chrono::system_clock::now(); result_tact = solveConvexSubproblem(); endSolver = std::chrono::system_clock::now(); // Step3 update matrices and retrieve new measurement (step 1) x_act = result_tact["x"]; y_act = result_tact["lam_g"]; model->storeAndShiftValues(result_tact, t_act); // update this->x_act = result_tact["x"]; this->y_act = result_tact["lam_g"]; evaluateOriginalF(t_act, x_act); updateA_act(t_act); updateH_act(t_act); updateM_act(); endIt = std::chrono::system_clock::now(); printTimingData(t_act); } }