void DirectCollocationInternal::init(){ // Initialize the base classes OCPSolverInternal::init(); // Free parameters currently not supported casadi_assert_message(np_==0, "Not implemented"); // Legendre collocation points double legendre_points[][6] = { {0}, {0,0.500000}, {0,0.211325,0.788675}, {0,0.112702,0.500000,0.887298}, {0,0.069432,0.330009,0.669991,0.930568}, {0,0.046910,0.230765,0.500000,0.769235,0.953090}}; // Radau collocation points double radau_points[][6] = { {0}, {0,1.000000}, {0,0.333333,1.000000}, {0,0.155051,0.644949,1.000000}, {0,0.088588,0.409467,0.787659,1.000000}, {0,0.057104,0.276843,0.583590,0.860240,1.000000}}; // Read options bool use_radau; if(getOption("collocation_scheme")=="radau"){ use_radau = true; } else if(getOption("collocation_scheme")=="legendre"){ use_radau = false; } // Interpolation order deg_ = getOption("interpolation_order"); // All collocation time points double* tau_root = use_radau ? radau_points[deg_] : legendre_points[deg_]; // Size of the finite elements double h = tf_/nk_; // Coefficients of the collocation equation vector<vector<MX> > C(deg_+1,vector<MX>(deg_+1)); // Coefficients of the collocation equation as DMatrix DMatrix C_num = DMatrix(deg_+1,deg_+1,0); // Coefficients of the continuity equation vector<MX> D(deg_+1); // Coefficients of the collocation equation as DMatrix DMatrix D_num = DMatrix(deg_+1,1,0); // Collocation point SXMatrix tau = ssym("tau"); // For all collocation points for(int j=0; j<deg_+1; ++j){ // Construct Lagrange polynomials to get the polynomial basis at the collocation point SXMatrix L = 1; for(int j2=0; j2<deg_+1; ++j2){ if(j2 != j){ L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]); } } SXFunction lfcn(tau,L); lfcn.init(); // Evaluate the polynomial at the final time to get the coefficients of the continuity equation lfcn.setInput(1.0); lfcn.evaluate(); D[j] = lfcn.output(); D_num(j) = lfcn.output(); // Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation for(int j2=0; j2<deg_+1; ++j2){ lfcn.setInput(tau_root[j2]); lfcn.setFwdSeed(1.0); lfcn.evaluate(1,0); C[j][j2] = lfcn.fwdSens(); C_num(j,j2) = lfcn.fwdSens(); } } C_num(std::vector<int>(1,0),ALL) = 0; C_num(0,0) = 1; // All collocation time points vector<vector<double> > T(nk_); for(int k=0; k<nk_; ++k){ T[k].resize(deg_+1); for(int j=0; j<=deg_; ++j){ T[k][j] = h*(k + tau_root[j]); } } // Total number of variables int nlp_nx = 0; nlp_nx += nk_*(deg_+1)*nx_; // Collocated states nlp_nx += nk_*nu_; // Parametrized controls nlp_nx += nx_; // Final state // NLP variable vector MX nlp_x = msym("x",nlp_nx); int offset = 0; // Get collocated states and parametrized control vector<vector<MX> > X(nk_+1); vector<MX> U(nk_); for(int k=0; k<nk_; ++k){ // Collocated states X[k].resize(deg_+1); for(int j=0; j<=deg_; ++j){ // Get the expression for the state vector X[k][j] = nlp_x[Slice(offset,offset+nx_)]; offset += nx_; } // Parametrized controls U[k] = nlp_x[Slice(offset,offset+nu_)]; offset += nu_; } // State at end time X[nk_].resize(1); X[nk_][0] = nlp_x[Slice(offset,offset+nx_)]; offset += nx_; casadi_assert(offset==nlp_nx); // Constraint function for the NLP vector<MX> nlp_g; // Objective function MX nlp_j = 0; // For all finite elements for(int k=0; k<nk_; ++k){ // For all collocation points for(int j=1; j<=deg_; ++j){ // Get an expression for the state derivative at the collocation point MX xp_jk = 0; for(int r=0; r<=deg_; ++r){ xp_jk += C[r][j]*X[k][r]; } // Add collocation equations to the NLP MX fk = ffcn_.call(daeIn("x",X[k][j],"p",U[k]))[DAE_ODE]; nlp_g.push_back(h*fk - xp_jk); } // Get an expression for the state at the end of the finite element MX xf_k = 0; for(int r=0; r<=deg_; ++r){ xf_k += D[r]*X[k][r]; } // Add continuity equation to NLP nlp_g.push_back(X[k+1][0] - xf_k); // Add path constraints if(nh_>0){ MX pk = cfcn_.call(daeIn("x",X[k+1][0],"p",U[k])).at(0); nlp_g.push_back(pk); } // Add integral objective function term // [Jk] = lfcn.call([X[k+1,0], U[k]]) // nlp_j += Jk } // Add end cost MX Jk = mfcn_.call(mayerIn("x",X[nk_][0])).at(0); nlp_j += Jk; // Objective function of the NLP F_ = MXFunction(nlp_x, nlp_j); // Nonlinear constraint function G_ = MXFunction(nlp_x, vertcat(nlp_g)); // Get the NLP creator function NLPSolverCreator nlp_solver_creator = getOption("nlp_solver"); // Allocate an NLP solver nlp_solver_ = nlp_solver_creator(F_,G_,FX(),FX()); // Pass options if(hasSetOption("nlp_solver_options")){ const Dictionary& nlp_solver_options = getOption("nlp_solver_options"); nlp_solver_.setOption(nlp_solver_options); } // Initialize the solver nlp_solver_.init(); }
void NLPSolverInternal::init(){ // Read options verbose_ = getOption("verbose"); gauss_newton_ = getOption("gauss_newton"); // Initialize the functions casadi_assert_message(!F_.isNull(),"No objective function"); if(!F_.isInit()){ F_.init(); log("Objective function initialized"); } if(!G_.isNull() && !G_.isInit()){ G_.init(); log("Constraint function initialized"); } // Get dimensions n_ = F_.input(0).numel(); m_ = G_.isNull() ? 0 : G_.output(0).numel(); parametric_ = getOption("parametric"); if (parametric_) { casadi_assert_message(F_.getNumInputs()==2, "Wrong number of input arguments to F for parametric NLP. Must be 2, but got " << F_.getNumInputs()); } else { casadi_assert_message(F_.getNumInputs()==1, "Wrong number of input arguments to F for non-parametric NLP. Must be 1, but got " << F_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option."); } // Basic sanity checks casadi_assert_message(F_.getNumInputs()==1 || F_.getNumInputs()==2, "Wrong number of input arguments to F. Must be 1 or 2"); if (F_.getNumInputs()==2) parametric_=true; casadi_assert_message(getOption("ignore_check_vec") || gauss_newton_ || F_.input().size2()==1, "To avoid confusion, the input argument to F must be vector. You supplied " << F_.input().dimString() << endl << " We suggest you make the following changes:" << endl << " - F is an SXFunction: SXFunction([X],[rhs]) -> SXFunction([vec(X)],[rhs])" << endl << " or F - -> F = vec(F) " << " - F is an MXFunction: MXFunction([X],[rhs]) -> " << endl << " X_vec = MX(\"X\",vec(X.sparsity())) " << endl << " F_vec = MXFunction([X_flat],[F.call([X_flat.reshape(X.sparsity())])[0]]) " << endl << " or F - -> F = vec(F) " << " You may ignore this warning by setting the 'ignore_check_vec' option to true." << endl ); casadi_assert_message(F_.getNumOutputs()>=1, "Wrong number of output arguments to F"); casadi_assert_message(gauss_newton_ || F_.output().scalar(), "Output argument of F not scalar."); casadi_assert_message(F_.output().dense(), "Output argument of F not dense."); casadi_assert_message(F_.input().dense(), "Input argument of F must be dense. You supplied " << F_.input().dimString()); if(!G_.isNull()) { if (parametric_) { casadi_assert_message(G_.getNumInputs()==2, "Wrong number of input arguments to G for parametric NLP. Must be 2, but got " << G_.getNumInputs()); } else { casadi_assert_message(G_.getNumInputs()==1, "Wrong number of input arguments to G for non-parametric NLP. Must be 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option."); } casadi_assert_message(G_.getNumOutputs()>=1, "Wrong number of output arguments to G"); casadi_assert_message(G_.input().numel()==n_, "Inconsistent dimensions"); casadi_assert_message(G_.input().sparsity()==F_.input().sparsity(), "F and G input dimension must match. F " << F_.input().dimString() << ". G " << G_.input().dimString()); } // Find out if we are to expand the objective function in terms of scalar operations bool expand_f = getOption("expand_f"); if(expand_f){ log("Expanding objective function"); // Cast to MXFunction MXFunction F_mx = shared_cast<MXFunction>(F_); if(F_mx.isNull()){ casadi_warning("Cannot expand objective function as it is not an MXFunction"); } else { // Take use the input scheme of G if possible (it might be an SXFunction) vector<SXMatrix> inputv; if(!G_.isNull() && F_.getNumInputs()==G_.getNumInputs()){ inputv = G_.symbolicInputSX(); } else { inputv = F_.symbolicInputSX(); } // Try to expand the MXFunction F_ = F_mx.expand(inputv); F_.setOption("number_of_fwd_dir",F_mx.getOption("number_of_fwd_dir")); F_.setOption("number_of_adj_dir",F_mx.getOption("number_of_adj_dir")); F_.init(); } } // Find out if we are to expand the constraint function in terms of scalar operations bool expand_g = getOption("expand_g"); if(expand_g){ log("Expanding constraint function"); // Cast to MXFunction MXFunction G_mx = shared_cast<MXFunction>(G_); if(G_mx.isNull()){ casadi_warning("Cannot expand constraint function as it is not an MXFunction"); } else { // Take use the input scheme of F if possible (it might be an SXFunction) vector<SXMatrix> inputv; if(F_.getNumInputs()==G_.getNumInputs()){ inputv = F_.symbolicInputSX(); } else { inputv = G_.symbolicInputSX(); } // Try to expand the MXFunction G_ = G_mx.expand(inputv); G_.setOption("number_of_fwd_dir",G_mx.getOption("number_of_fwd_dir")); G_.setOption("number_of_adj_dir",G_mx.getOption("number_of_adj_dir")); G_.init(); } } // Find out if we are to expand the constraint function in terms of scalar operations bool generate_hessian = getOption("generate_hessian"); if(generate_hessian && H_.isNull()){ casadi_assert_message(!gauss_newton_,"Automatic generation of Gauss-Newton Hessian not yet supported"); log("generating hessian"); // Simple if unconstrained if(G_.isNull()){ // Create Hessian of the objective function FX HF = F_.hessian(); HF.init(); // Symbolic inputs of HF vector<MX> HF_in = F_.symbolicInput(); // Lagrange multipliers MX lam("lam",0); // Objective function scaling MX sigma("sigma"); // Inputs of the Hessian function vector<MX> H_in = HF_in; H_in.insert(H_in.begin()+1, lam); H_in.insert(H_in.begin()+2, sigma); // Get an expression for the Hessian of F MX hf = HF.call(HF_in).at(0); // Create the scaled Hessian function H_ = MXFunction(H_in, sigma*hf); log("Unconstrained Hessian function generated"); } else { // G_.isNull() // Check if the functions are SXFunctions SXFunction F_sx = shared_cast<SXFunction>(F_); SXFunction G_sx = shared_cast<SXFunction>(G_); // Efficient if both functions are SXFunction if(!F_sx.isNull() && !G_sx.isNull()){ // Expression for f and g SXMatrix f = F_sx.outputSX(); SXMatrix g = G_sx.outputSX(); // Numeric hessian bool f_num_hess = F_sx.getOption("numeric_hessian"); bool g_num_hess = G_sx.getOption("numeric_hessian"); // Number of derivative directions int f_num_fwd = F_sx.getOption("number_of_fwd_dir"); int g_num_fwd = G_sx.getOption("number_of_fwd_dir"); int f_num_adj = F_sx.getOption("number_of_adj_dir"); int g_num_adj = G_sx.getOption("number_of_adj_dir"); // Substitute symbolic variables in f if different input variables from g if(!isEqual(F_sx.inputSX(),G_sx.inputSX())){ f = substitute(f,F_sx.inputSX(),G_sx.inputSX()); } // Lagrange multipliers SXMatrix lam = ssym("lambda",g.size1()); // Objective function scaling SXMatrix sigma = ssym("sigma"); // Lagrangian function vector<SXMatrix> lfcn_in(parametric_? 4: 3); lfcn_in[0] = G_sx.inputSX(); lfcn_in[1] = lam; lfcn_in[2] = sigma; if (parametric_) lfcn_in[3] = G_sx.inputSX(1); SXFunction lfcn(lfcn_in, sigma*f + inner_prod(lam,g)); lfcn.setOption("verbose",getOption("verbose")); lfcn.setOption("numeric_hessian",f_num_hess || g_num_hess); lfcn.setOption("number_of_fwd_dir",std::min(f_num_fwd,g_num_fwd)); lfcn.setOption("number_of_adj_dir",std::min(f_num_adj,g_num_adj)); lfcn.init(); // Hessian of the Lagrangian H_ = static_cast<FX&>(lfcn).hessian(); H_.setOption("verbose",getOption("verbose")); log("SX Hessian function generated"); } else { // !F_sx.isNull() && !G_sx.isNull() // Check if the functions are SXFunctions MXFunction F_mx = shared_cast<MXFunction>(F_); MXFunction G_mx = shared_cast<MXFunction>(G_); // If they are, check if the arguments are the same if(!F_mx.isNull() && !G_mx.isNull() && isEqual(F_mx.inputMX(),G_mx.inputMX())){ casadi_warning("Exact Hessian calculation for MX is still experimental"); // Expression for f and g MX f = F_mx.outputMX(); MX g = G_mx.outputMX(); // Lagrange multipliers MX lam("lam",g.size1()); // Objective function scaling MX sigma("sigma"); // Inputs of the Lagrangian function vector<MX> lfcn_in(parametric_? 4:3); lfcn_in[0] = G_mx.inputMX(); lfcn_in[1] = lam; lfcn_in[2] = sigma; if (parametric_) lfcn_in[3] = G_mx.inputMX(1); // Lagrangian function MXFunction lfcn(lfcn_in,sigma*f+ inner_prod(lam,g)); lfcn.init(); log("SX Lagrangian function generated"); /* cout << "countNodes(lfcn.outputMX()) = " << countNodes(lfcn.outputMX()) << endl;*/ bool adjoint_mode = true; if(adjoint_mode){ // Gradient of the lagrangian MX gL = lfcn.grad(); log("MX Lagrangian gradient generated"); MXFunction glfcn(lfcn_in,gL); glfcn.init(); log("MX Lagrangian gradient function initialized"); // cout << "countNodes(glfcn.outputMX()) = " << countNodes(glfcn.outputMX()) << endl; // Get Hessian sparsity CRSSparsity H_sp = glfcn.jacSparsity(); log("MX Lagrangian Hessian sparsity determined"); // Uni-directional coloring (note, the hessian is symmetric) CRSSparsity coloring = H_sp.unidirectionalColoring(H_sp); log("MX Lagrangian Hessian coloring determined"); // Number of colors needed is the number of rows int nfwd_glfcn = coloring.size1(); log("MX Lagrangian gradient function number of sensitivity directions determined"); glfcn.setOption("number_of_fwd_dir",nfwd_glfcn); glfcn.updateNumSens(); log("MX Lagrangian gradient function number of sensitivity directions updated"); // Hessian of the Lagrangian H_ = glfcn.jacobian(); } else { // Hessian of the Lagrangian H_ = lfcn.hessian(); } log("MX Lagrangian Hessian function generated"); } else { casadi_assert_message(0, "Automatic calculation of exact Hessian currently only for F and G both SXFunction or MXFunction "); } } // !F_sx.isNull() && !G_sx.isNull() } // G_.isNull() } // generate_hessian && H_.isNull() if(!H_.isNull() && !H_.isInit()) { H_.init(); log("Hessian function initialized"); } // Create a Jacobian if it does not already exists bool generate_jacobian = getOption("generate_jacobian"); if(generate_jacobian && !G_.isNull() && J_.isNull()){ log("Generating Jacobian"); J_ = G_.jacobian(); // Use live variables if SXFunction if(!shared_cast<SXFunction>(J_).isNull()){ J_.setOption("live_variables",true); } log("Jacobian function generated"); } if(!J_.isNull() && !J_.isInit()){ J_.init(); log("Jacobian function initialized"); } if(!H_.isNull()) { if (parametric_) { casadi_assert_message(H_.getNumInputs()>=2, "Wrong number of input arguments to H for parametric NLP. Must be at least 2, but got " << G_.getNumInputs()); } else { casadi_assert_message(H_.getNumInputs()>=1, "Wrong number of input arguments to H for non-parametric NLP. Must be at least 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option."); } casadi_assert_message(H_.getNumOutputs()>=1, "Wrong number of output arguments to H"); casadi_assert_message(H_.input(0).numel()==n_,"Inconsistent dimensions"); casadi_assert_message(H_.output().size1()==n_,"Inconsistent dimensions"); casadi_assert_message(H_.output().size2()==n_,"Inconsistent dimensions"); } if(!J_.isNull()){ if (parametric_) { casadi_assert_message(J_.getNumInputs()==2, "Wrong number of input arguments to J for parametric NLP. Must be at least 2, but got " << G_.getNumInputs()); } else { casadi_assert_message(J_.getNumInputs()==1, "Wrong number of input arguments to J for non-parametric NLP. Must be at least 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option."); } casadi_assert_message(J_.getNumOutputs()>=1, "Wrong number of output arguments to J"); casadi_assert_message(J_.input().numel()==n_,"Inconsistent dimensions"); casadi_assert_message(J_.output().size2()==n_,"Inconsistent dimensions"); } if (parametric_) { sp_p = F_->input(1).sparsity(); if (!G_.isNull()) casadi_assert_message(sp_p == G_->input(G_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while G has got " << G_->input(G_->getNumInputs()-1).dimString()); if (!H_.isNull()) casadi_assert_message(sp_p == H_->input(H_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while H has got " << H_->input(H_->getNumInputs()-1).dimString()); if (!J_.isNull()) casadi_assert_message(sp_p == J_->input(J_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while J has got " << J_->input(J_->getNumInputs()-1).dimString()); } // Infinity double inf = numeric_limits<double>::infinity(); // Allocate space for inputs input_.resize(NLP_NUM_IN - (parametric_? 0 : 1)); input(NLP_X_INIT) = DMatrix(n_,1,0); input(NLP_LBX) = DMatrix(n_,1,-inf); input(NLP_UBX) = DMatrix(n_,1, inf); input(NLP_LBG) = DMatrix(m_,1,-inf); input(NLP_UBG) = DMatrix(m_,1, inf); input(NLP_LAMBDA_INIT) = DMatrix(m_,1,0); if (parametric_) input(NLP_P) = DMatrix(sp_p,0); // Allocate space for outputs output_.resize(NLP_NUM_OUT); output(NLP_X_OPT) = DMatrix(n_,1,0); output(NLP_COST) = DMatrix(1,1,0); output(NLP_LAMBDA_X) = DMatrix(n_,1,0); output(NLP_LAMBDA_G) = DMatrix(m_,1,0); output(NLP_G) = DMatrix(m_,1,0); if (hasSetOption("iteration_callback")) { callback_ = getOption("iteration_callback"); if (!callback_.isNull()) { if (!callback_.isInit()) callback_.init(); casadi_assert_message(callback_.getNumOutputs()==1, "Callback function should have one output, a scalar that indicates wether to break. 0 = continue"); casadi_assert_message(callback_.output(0).size()==1, "Callback function should have one output, a scalar that indicates wether to break. 0 = continue"); casadi_assert_message(callback_.getNumInputs()==NLP_NUM_OUT, "Callback function should have the output scheme of NLPSolver as input scheme. i.e. " <<NLP_NUM_OUT << " inputs instead of the " << callback_.getNumInputs() << " you provided." ); for (int i=0;i<NLP_NUM_OUT;i++) { casadi_assert_message(callback_.input(i).sparsity()==output(i).sparsity(), "Callback function should have the output scheme of NLPSolver as input scheme. " << "Input #" << i << " (" << getSchemeEntryEnumName(SCHEME_NLPOutput,i) << " aka '" << getSchemeEntryName(SCHEME_NLPOutput,i) << "') was found to be " << callback_.input(i).dimString() << " instead of expected " << output(i).dimString() << "." ); callback_.input(i).setAll(0); } } } callback_step_ = getOption("iteration_callback_step"); // Call the initialization method of the base class FXInternal::init(); }
void SQPInternal::init(){ // Call the init method of the base class NLPSolverInternal::init(); // Read options maxiter_ = getOption("maxiter"); maxiter_ls_ = getOption("maxiter_ls"); c1_ = getOption("c1"); beta_ = getOption("beta"); merit_memsize_ = getOption("merit_memory"); lbfgs_memory_ = getOption("lbfgs_memory"); tol_pr_ = getOption("tol_pr"); tol_du_ = getOption("tol_du"); regularize_ = getOption("regularize"); if(getOption("hessian_approximation")=="exact") hess_mode_ = HESS_EXACT; else if(getOption("hessian_approximation")=="limited-memory") hess_mode_ = HESS_BFGS; if (hess_mode_== HESS_EXACT && H_.isNull()) { if (!getOption("generate_hessian")){ casadi_error("SQPInternal::evaluate: you set option 'hessian_approximation' to 'exact', but no hessian was supplied. Try with option \"generate_hessian\"."); } } // If the Hessian is generated, we use exact approximation by default if (bool(getOption("generate_hessian"))){ setOption("hessian_approximation", "exact"); } // Allocate a QP solver CRSSparsity H_sparsity = hess_mode_==HESS_EXACT ? H_.output().sparsity() : sp_dense(n_,n_); H_sparsity = H_sparsity + DMatrix::eye(n_).sparsity(); CRSSparsity A_sparsity = J_.isNull() ? CRSSparsity(0,n_,false) : J_.output().sparsity(); QPSolverCreator qp_solver_creator = getOption("qp_solver"); qp_solver_ = qp_solver_creator(H_sparsity,A_sparsity); // Set options if provided if(hasSetOption("qp_solver_options")){ Dictionary qp_solver_options = getOption("qp_solver_options"); qp_solver_.setOption(qp_solver_options); } qp_solver_.init(); // Lagrange multipliers of the NLP mu_.resize(m_); mu_x_.resize(n_); // Lagrange gradient in the next iterate gLag_.resize(n_); gLag_old_.resize(n_); // Current linearization point x_.resize(n_); x_cand_.resize(n_); x_old_.resize(n_); // Constraint function value gk_.resize(m_); gk_cand_.resize(m_); // Hessian approximation Bk_ = DMatrix(H_sparsity); // Jacobian Jk_ = DMatrix(A_sparsity); // Bounds of the QP qp_LBA_.resize(m_); qp_UBA_.resize(m_); qp_LBX_.resize(n_); qp_UBX_.resize(n_); // QP solution dx_.resize(n_); qp_DUAL_X_.resize(n_); qp_DUAL_A_.resize(m_); // Gradient of the objective gf_.resize(n_); // Create Hessian update function if(hess_mode_ == HESS_BFGS){ // Create expressions corresponding to Bk, x, x_old, gLag and gLag_old SXMatrix Bk = ssym("Bk",H_sparsity); SXMatrix x = ssym("x",input(NLP_X_INIT).sparsity()); SXMatrix x_old = ssym("x",x.sparsity()); SXMatrix gLag = ssym("gLag",x.sparsity()); SXMatrix gLag_old = ssym("gLag_old",x.sparsity()); SXMatrix sk = x - x_old; SXMatrix yk = gLag - gLag_old; SXMatrix qk = mul(Bk, sk); // Calculating theta SXMatrix skBksk = inner_prod(sk, qk); SXMatrix omega = if_else(inner_prod(yk, sk) < 0.2 * inner_prod(sk, qk), 0.8 * skBksk / (skBksk - inner_prod(sk, yk)), 1); yk = omega * yk + (1 - omega) * qk; SXMatrix theta = 1. / inner_prod(sk, yk); SXMatrix phi = 1. / inner_prod(qk, sk); SXMatrix Bk_new = Bk + theta * mul(yk, trans(yk)) - phi * mul(qk, trans(qk)); // Inputs of the BFGS update function vector<SXMatrix> bfgs_in(BFGS_NUM_IN); bfgs_in[BFGS_BK] = Bk; bfgs_in[BFGS_X] = x; bfgs_in[BFGS_X_OLD] = x_old; bfgs_in[BFGS_GLAG] = gLag; bfgs_in[BFGS_GLAG_OLD] = gLag_old; bfgs_ = SXFunction(bfgs_in,Bk_new); bfgs_.setOption("number_of_fwd_dir",0); bfgs_.setOption("number_of_adj_dir",0); bfgs_.init(); // Initial Hessian approximation B_init_ = DMatrix::eye(n_); } // Header if(bool(getOption("print_header"))){ cout << "-------------------------------------------" << endl; cout << "This is CasADi::SQPMethod." << endl; switch (hess_mode_) { case HESS_EXACT: cout << "Using exact Hessian" << endl; break; case HESS_BFGS: cout << "Using limited memory BFGS Hessian approximation" << endl; break; } cout << endl; cout << "Number of variables: " << setw(9) << n_ << endl; cout << "Number of constraints: " << setw(9) << m_ << endl; cout << "Number of nonzeros in constraint Jacobian: " << setw(9) << A_sparsity.size() << endl; cout << "Number of nonzeros in Lagrangian Hessian: " << setw(9) << H_sparsity.size() << endl; cout << endl; } }
ImplicitFunction ImplicitFunctionInternal::jac(const std::vector<int> iind, int oind){ // Single output casadi_assert(oind==0); // Get the function SXFunction f = shared_cast<SXFunction>(f_); casadi_assert(!f.isNull()); // Get the jacobians Matrix<SX> Jz = f.jac(0,0); // Number of equations int nz = f.input(0).numel(); // All variables vector<Matrix<SX> > f_in(f.getNumInputs()); f_in[0] = f.inputExpr(0); for(int i=1; i<f.getNumInputs(); ++i) f_in[i] = f.inputExpr(i); // Augmented nonlinear equation Matrix<SX> F_aug = f.outputExpr(0); // Number of right hand sides int nrhs = 1; // Augment variables and equations for(vector<int>::const_iterator it=iind.begin(); it!=iind.end(); ++it){ // Get the jacobian Matrix<SX> Jx = f.jac(*it+1,0); // Number of variables int nx = f.input(*it+1).numel(); // Sensitivities Matrix<SX> dz_dx = ssym("dz_dx", nz, nx); // Derivative equation Matrix<SX> f_der = mul(Jz,dz_dx) + Jx; // Append variables f_in[0].append(vec(dz_dx)); // Augment nonlinear equation F_aug.append(vec(f_der)); // Number of right hand sides nrhs += nx; } // Augmented nonlinear equation SXFunction f_aug(f_in, F_aug); // Create new explciit function ImplicitFunction ret; ret.assignNode(create(f_aug,nrhs)); ret.setOption(dictionary()); // Return the created solver return ret; }
void CollocationIntegratorInternal::init(){ // Call the base class init IntegratorInternal::init(); // Legendre collocation points double legendre_points[][6] = { {0}, {0,0.500000}, {0,0.211325,0.788675}, {0,0.112702,0.500000,0.887298}, {0,0.069432,0.330009,0.669991,0.930568}, {0,0.046910,0.230765,0.500000,0.769235,0.953090}}; // Radau collocation points double radau_points[][6] = { {0}, {0,1.000000}, {0,0.333333,1.000000}, {0,0.155051,0.644949,1.000000}, {0,0.088588,0.409467,0.787659,1.000000}, {0,0.057104,0.276843,0.583590,0.860240,1.000000}}; // Read options bool use_radau; if(getOption("collocation_scheme")=="radau"){ use_radau = true; } else if(getOption("collocation_scheme")=="legendre"){ use_radau = false; } // Hotstart? hotstart_ = getOption("hotstart"); // Number of finite elements int nk = getOption("number_of_finite_elements"); // Interpolation order int deg = getOption("interpolation_order"); // Assume explicit ODE bool explicit_ode = f_.input(DAE_XDOT).size()==0; // All collocation time points double* tau_root = use_radau ? radau_points[deg] : legendre_points[deg]; // Size of the finite elements double h = (tf_-t0_)/nk; // MX version of the same MX h_mx = h; // Coefficients of the collocation equation vector<vector<MX> > C(deg+1,vector<MX>(deg+1)); // Coefficients of the continuity equation vector<MX> D(deg+1); // Collocation point SXMatrix tau = ssym("tau"); // For all collocation points for(int j=0; j<deg+1; ++j){ // Construct Lagrange polynomials to get the polynomial basis at the collocation point SXMatrix L = 1; for(int j2=0; j2<deg+1; ++j2){ if(j2 != j){ L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]); } } SXFunction lfcn(tau,L); lfcn.init(); // Evaluate the polynomial at the final time to get the coefficients of the continuity equation lfcn.setInput(1.0); lfcn.evaluate(); D[j] = lfcn.output(); // Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation for(int j2=0; j2<deg+1; ++j2){ lfcn.setInput(tau_root[j2]); lfcn.setFwdSeed(1.0); lfcn.evaluate(1,0); C[j][j2] = lfcn.fwdSens(); } } // Initial state MX X0("X0",nx_); // Parameters MX P("P",np_); // Backward state MX RX0("RX0",nrx_); // Backward parameters MX RP("RP",nrp_); // Collocated differential states and algebraic variables int nX = (nk*(deg+1)+1)*(nx_+nrx_); int nZ = nk*deg*(nz_+nrz_); // Unknowns MX V("V",nX+nZ); int offset = 0; // Get collocated states, algebraic variables and times vector<vector<MX> > X(nk+1); vector<vector<MX> > RX(nk+1); vector<vector<MX> > Z(nk); vector<vector<MX> > RZ(nk); coll_time_.resize(nk+1); for(int k=0; k<nk+1; ++k){ // Number of time points int nj = k==nk ? 1 : deg+1; // Allocate differential states expressions at the time points X[k].resize(nj); RX[k].resize(nj); coll_time_[k].resize(nj); // Allocate algebraic variable expressions at the collocation points if(k!=nk){ Z[k].resize(nj-1); RZ[k].resize(nj-1); } // For all time points for(int j=0; j<nj; ++j){ // Get expressions for the differential state X[k][j] = V[range(offset,offset+nx_)]; offset += nx_; RX[k][j] = V[range(offset,offset+nrx_)]; offset += nrx_; // Get the local time coll_time_[k][j] = h*(k + tau_root[j]); // Get expressions for the algebraic variables if(j>0){ Z[k][j-1] = V[range(offset,offset+nz_)]; offset += nz_; RZ[k][j-1] = V[range(offset,offset+nrz_)]; offset += nrz_; } } } // Check offset for consistency casadi_assert(offset==V.size()); // Constraints vector<MX> g; g.reserve(2*(nk+1)); // Quadrature expressions MX QF = MX::zeros(nq_); MX RQF = MX::zeros(nrq_); // Counter int jk = 0; // Add initial condition g.push_back(X[0][0]-X0); // For all finite elements for(int k=0; k<nk; ++k, ++jk){ // For all collocation points for(int j=1; j<deg+1; ++j, ++jk){ // Get the time MX tkj = coll_time_[k][j]; // Get an expression for the state derivative at the collocation point MX xp_jk = 0; for(int j2=0; j2<deg+1; ++j2){ xp_jk += C[j2][j]*X[k][j2]; } // Add collocation equations to the NLP vector<MX> f_in(DAE_NUM_IN); f_in[DAE_T] = tkj; f_in[DAE_P] = P; f_in[DAE_X] = X[k][j]; f_in[DAE_Z] = Z[k][j-1]; vector<MX> f_out; if(explicit_ode){ // Assume equation of the form ydot = f(t,y,p) f_out = f_.call(f_in); g.push_back(h_mx*f_out[DAE_ODE] - xp_jk); } else { // Assume equation of the form 0 = f(t,y,ydot,p) f_in[DAE_XDOT] = xp_jk/h_mx; f_out = f_.call(f_in); g.push_back(f_out[DAE_ODE]); } // Add the algebraic conditions if(nz_>0){ g.push_back(f_out[DAE_ALG]); } // Add the quadrature if(nq_>0){ QF += D[j]*h_mx*f_out[DAE_QUAD]; } // Now for the backward problem if(nrx_>0){ // Get an expression for the state derivative at the collocation point MX rxp_jk = 0; for(int j2=0; j2<deg+1; ++j2){ rxp_jk += C[j2][j]*RX[k][j2]; } // Add collocation equations to the NLP vector<MX> g_in(RDAE_NUM_IN); g_in[RDAE_T] = tkj; g_in[RDAE_X] = X[k][j]; g_in[RDAE_Z] = Z[k][j-1]; g_in[RDAE_P] = P; g_in[RDAE_RP] = RP; g_in[RDAE_RX] = RX[k][j]; g_in[RDAE_RZ] = RZ[k][j-1]; vector<MX> g_out; if(explicit_ode){ // Assume equation of the form xdot = f(t,x,p) g_out = g_.call(g_in); g.push_back(h_mx*g_out[RDAE_ODE] - rxp_jk); } else { // Assume equation of the form 0 = f(t,x,xdot,p) g_in[RDAE_XDOT] = xp_jk/h_mx; g_in[RDAE_RXDOT] = rxp_jk/h_mx; g_out = g_.call(g_in); g.push_back(g_out[RDAE_ODE]); } // Add the algebraic conditions if(nrz_>0){ g.push_back(g_out[RDAE_ALG]); } // Add the backward quadrature if(nrq_>0){ RQF += D[j]*h_mx*g_out[RDAE_QUAD]; } } } // Get an expression for the state at the end of the finite element MX xf_k = 0; for(int j=0; j<deg+1; ++j){ xf_k += D[j]*X[k][j]; } // Add continuity equation to NLP g.push_back(X[k+1][0] - xf_k); if(nrx_>0){ // Get an expression for the state at the end of the finite element MX rxf_k = 0; for(int j=0; j<deg+1; ++j){ rxf_k += D[j]*RX[k][j]; } // Add continuity equation to NLP g.push_back(RX[k+1][0] - rxf_k); } } // Add initial condition for the backward integration if(nrx_>0){ g.push_back(RX[nk][0]-RX0); } // Constraint expression MX gv = vertcat(g); // Make sure that the dimension is consistent with the number of unknowns casadi_assert_message(gv.size()==V.size(),"Implicit function unknowns and equations do not match"); // Nonlinear constraint function input vector<MX> gfcn_in(1+INTEGRATOR_NUM_IN); gfcn_in[0] = V; gfcn_in[1+INTEGRATOR_X0] = X0; gfcn_in[1+INTEGRATOR_P] = P; gfcn_in[1+INTEGRATOR_RX0] = RX0; gfcn_in[1+INTEGRATOR_RP] = RP; vector<MX> gfcn_out(1+INTEGRATOR_NUM_OUT); gfcn_out[0] = gv; gfcn_out[1+INTEGRATOR_XF] = X[nk][0]; gfcn_out[1+INTEGRATOR_QF] = QF; gfcn_out[1+INTEGRATOR_RXF] = RX[0][0]; gfcn_out[1+INTEGRATOR_RQF] = RQF; // Nonlinear constraint function FX gfcn = MXFunction(gfcn_in,gfcn_out); // Expand f? bool expand_f = getOption("expand_f"); if(expand_f){ gfcn.init(); gfcn = SXFunction(shared_cast<MXFunction>(gfcn)); } // Get the NLP creator function implicitFunctionCreator implicit_function_creator = getOption("implicit_solver"); // Allocate an NLP solver implicit_solver_ = implicit_function_creator(gfcn); // Pass options if(hasSetOption("implicit_solver_options")){ const Dictionary& implicit_solver_options = getOption("implicit_solver_options"); implicit_solver_.setOption(implicit_solver_options); } // Initialize the solver implicit_solver_.init(); if(hasSetOption("startup_integrator")){ // Create the linear solver integratorCreator startup_integrator_creator = getOption("startup_integrator"); // Allocate an NLP solver startup_integrator_ = startup_integrator_creator(f_,g_); // Pass options startup_integrator_.setOption("number_of_fwd_dir",0); // not needed startup_integrator_.setOption("number_of_adj_dir",0); // not needed startup_integrator_.setOption("t0",coll_time_.front().front()); startup_integrator_.setOption("tf",coll_time_.back().back()); if(hasSetOption("startup_integrator_options")){ const Dictionary& startup_integrator_options = getOption("startup_integrator_options"); startup_integrator_.setOption(startup_integrator_options); } // Initialize the startup integrator startup_integrator_.init(); } // Mark the system not yet integrated integrated_once_ = false; }
void LiftedSQPInternal::init(){ // Call the init method of the base class NlpSolverInternal::init(); // Number of lifted variables nv = getOption("num_lifted"); if(verbose_){ cout << "Initializing SQP method with " << nx_ << " variables and " << ng_ << " constraints." << endl; cout << "Lifting " << nv << " variables." << endl; if(gauss_newton_){ cout << "Gauss-Newton objective with " << F_.input().numel() << " terms." << endl; } } // Read options max_iter_ = getOption("max_iter"); max_iter_ls_ = getOption("max_iter_ls"); toldx_ = getOption("toldx"); tolgl_ = getOption("tolgl"); sigma_ = getOption("sigma"); rho_ = getOption("rho"); mu_safety_ = getOption("mu_safety"); eta_ = getOption("eta"); tau_ = getOption("tau"); // Assume SXFunction for now SXFunction ffcn = shared_cast<SXFunction>(F_); casadi_assert(!ffcn.isNull()); SXFunction gfcn = shared_cast<SXFunction>(G_); casadi_assert(!gfcn.isNull()); // Extract the free variables and split into independent and dependent variables SX x = ffcn.inputExpr(0); int nx = x.size(); nu = nx-nv; SX u = x[Slice(0,nu)]; SX v = x[Slice(nu,nu+nv)]; // Extract the constraint equations and split into constraints and definitions of dependent variables SX f1 = ffcn.outputExpr(0); int nf1 = f1.numel(); SX g = gfcn.outputExpr(0); int nf2 = g.numel()-nv; SX v_eq = g(Slice(0,nv)); SX f2 = g(Slice(nv,nv+nf2)); // Definition of v SX v_def = v_eq + v; // Objective function SX f; // Multipliers SX lam_x, lam_g, lam_f2; if(gauss_newton_){ // Least square objective f = inner_prod(f1,f1)/2; } else { // Scalar objective function f = f1; // Lagrange multipliers for the simple bounds on u SX lam_u = ssym("lam_u",nu); // Lagrange multipliers for the simple bounds on v SX lam_v = ssym("lam_v",nv); // Lagrange multipliers for the simple bounds on x lam_x = vertcat(lam_u,lam_v); // Lagrange multipliers corresponding to the definition of the dependent variables SX lam_v_eq = ssym("lam_v_eq",nv); // Lagrange multipliers for the nonlinear constraints that aren't eliminated lam_f2 = ssym("lam_f2",nf2); if(verbose_){ cout << "Allocated intermediate variables." << endl; } // Lagrange multipliers for constraints lam_g = vertcat(lam_v_eq,lam_f2); // Lagrangian function SX lag = f + inner_prod(lam_x,x); if(!f2.empty()) lag += inner_prod(lam_f2,f2); if(!v.empty()) lag += inner_prod(lam_v_eq,v_def); // Gradient of the Lagrangian SX lgrad = casadi::gradient(lag,x); if(!v.empty()) lgrad -= vertcat(SX::zeros(nu),lam_v_eq); // Put here to ensure that lgrad is of the form "h_extended -v_extended" makeDense(lgrad); if(verbose_){ cout << "Generated the gradient of the Lagrangian." << endl; } // Condensed gradient of the Lagrangian f1 = lgrad[Slice(0,nu)]; nf1 = nu; // Gradient of h SX v_eq_grad = lgrad[Slice(nu,nu+nv)]; // Reverse lam_v_eq and v_eq_grad SX v_eq_grad_reversed = v_eq_grad; copy(v_eq_grad.rbegin(),v_eq_grad.rend(),v_eq_grad_reversed.begin()); SX lam_v_eq_reversed = lam_v_eq; copy(lam_v_eq.rbegin(),lam_v_eq.rend(),lam_v_eq_reversed.begin()); // Augment h and lam_v_eq v_eq.append(v_eq_grad_reversed); v.append(lam_v_eq_reversed); } // Residual function G SXVector G_in(G_NUM_IN); G_in[G_X] = x; G_in[G_LAM_X] = lam_x; G_in[G_LAM_G] = lam_g; SXVector G_out(G_NUM_OUT); G_out[G_D] = v_eq; G_out[G_G] = g; G_out[G_F] = f; rfcn_ = SXFunction(G_in,G_out); rfcn_.setOption("number_of_fwd_dir",0); rfcn_.setOption("number_of_adj_dir",0); rfcn_.setOption("live_variables",true); rfcn_.init(); if(verbose_){ cout << "Generated residual function ( " << shared_cast<SXFunction>(rfcn_).getAlgorithmSize() << " nodes)." << endl; } // Difference vector d SX d = ssym("d",nv); if(!gauss_newton_){ vector<SX> dg = ssym("dg",nv).data(); reverse(dg.begin(),dg.end()); d.append(dg); } // Substitute out the v from the h SX d_def = (v_eq + v)-d; SXVector ex(3); ex[0] = f1; ex[1] = f2; ex[2] = f; substituteInPlace(v, d_def, ex, false); SX f1_z = ex[0]; SX f2_z = ex[1]; SX f_z = ex[2]; // Modified function Z enum ZIn{Z_U,Z_D,Z_LAM_X,Z_LAM_F2,Z_NUM_IN}; SXVector zfcn_in(Z_NUM_IN); zfcn_in[Z_U] = u; zfcn_in[Z_D] = d; zfcn_in[Z_LAM_X] = lam_x; zfcn_in[Z_LAM_F2] = lam_f2; enum ZOut{Z_D_DEF,Z_F12,Z_NUM_OUT}; SXVector zfcn_out(Z_NUM_OUT); zfcn_out[Z_D_DEF] = d_def; zfcn_out[Z_F12] = vertcat(f1_z,f2_z); SXFunction zfcn(zfcn_in,zfcn_out); zfcn.init(); if(verbose_){ cout << "Generated reconstruction function ( " << zfcn.getAlgorithmSize() << " nodes)." << endl; } // Matrix A and B in lifted Newton SX B = zfcn.jac(Z_U,Z_F12); SX B1 = B(Slice(0,nf1),Slice(0,B.size2())); SX B2 = B(Slice(nf1,B.size1()),Slice(0,B.size2())); if(verbose_){ cout << "Formed B1 (dimension " << B1.size1() << "-by-" << B1.size2() << ", "<< B1.size() << " nonzeros) " << "and B2 (dimension " << B2.size1() << "-by-" << B2.size2() << ", "<< B2.size() << " nonzeros)." << endl; } // Step in u SX du = ssym("du",nu); SX dlam_f2 = ssym("dlam_f2",lam_f2.sparsity()); SX b1 = f1_z; SX b2 = f2_z; SX e; if(nv > 0){ // Directional derivative of Z vector<vector<SX> > Z_fwdSeed(2,zfcn_in); vector<vector<SX> > Z_fwdSens(2,zfcn_out); vector<vector<SX> > Z_adjSeed; vector<vector<SX> > Z_adjSens; Z_fwdSeed[0][Z_U].setZero(); Z_fwdSeed[0][Z_D] = -d; Z_fwdSeed[0][Z_LAM_X].setZero(); Z_fwdSeed[0][Z_LAM_F2].setZero(); Z_fwdSeed[1][Z_U] = du; Z_fwdSeed[1][Z_D] = -d; Z_fwdSeed[1][Z_LAM_X].setZero(); Z_fwdSeed[1][Z_LAM_F2] = dlam_f2; zfcn.eval(zfcn_in,zfcn_out,Z_fwdSeed,Z_fwdSens,Z_adjSeed,Z_adjSens); b1 += Z_fwdSens[0][Z_F12](Slice(0,nf1)); b2 += Z_fwdSens[0][Z_F12](Slice(nf1,B.size1())); e = Z_fwdSens[1][Z_D_DEF]; } if(verbose_){ cout << "Formed b1 (dimension " << b1.size1() << "-by-" << b1.size2() << ", "<< b1.size() << " nonzeros) " << "and b2 (dimension " << b2.size1() << "-by-" << b2.size2() << ", "<< b2.size() << " nonzeros)." << endl; } // Generate Gauss-Newton Hessian if(gauss_newton_){ b1 = mul(trans(B1),b1); B1 = mul(trans(B1),B1); if(verbose_){ cout << "Gauss Newton Hessian (dimension " << B1.size1() << "-by-" << B1.size2() << ", "<< B1.size() << " nonzeros)." << endl; } } // Make sure b1 and b2 are dense vectors makeDense(b1); makeDense(b2); // Quadratic approximation SXVector lfcn_in(LIN_NUM_IN); lfcn_in[LIN_X] = x; lfcn_in[LIN_D] = d; lfcn_in[LIN_LAM_X] = lam_x; lfcn_in[LIN_LAM_G] = lam_g; SXVector lfcn_out(LIN_NUM_OUT); lfcn_out[LIN_F1] = b1; lfcn_out[LIN_J1] = B1; lfcn_out[LIN_F2] = b2; lfcn_out[LIN_J2] = B2; lfcn_ = SXFunction(lfcn_in,lfcn_out); // lfcn_.setOption("verbose",true); lfcn_.setOption("number_of_fwd_dir",0); lfcn_.setOption("number_of_adj_dir",0); lfcn_.setOption("live_variables",true); lfcn_.init(); if(verbose_){ cout << "Generated linearization function ( " << shared_cast<SXFunction>(lfcn_).getAlgorithmSize() << " nodes)." << endl; } // Step expansion SXVector efcn_in(EXP_NUM_IN); copy(lfcn_in.begin(),lfcn_in.end(),efcn_in.begin()); efcn_in[EXP_DU] = du; efcn_in[EXP_DLAM_F2] = dlam_f2; efcn_ = SXFunction(efcn_in,e); efcn_.setOption("number_of_fwd_dir",0); efcn_.setOption("number_of_adj_dir",0); efcn_.setOption("live_variables",true); efcn_.init(); if(verbose_){ cout << "Generated step expansion function ( " << shared_cast<SXFunction>(efcn_).getAlgorithmSize() << " nodes)." << endl; } // Current guess for the primal solution DMatrix &x_k = output(NLP_SOLVER_X); // Current guess for the dual solution DMatrix &lam_x_k = output(NLP_SOLVER_LAM_X); DMatrix &lam_g_k = output(NLP_SOLVER_LAM_G); // Allocate a QP solver QpSolverCreator qp_solver_creator = getOption("qp_solver"); qp_solver_ = qp_solver_creator(B1.sparsity(),B2.sparsity()); // Set options if provided if(hasSetOption("qp_solver_options")){ Dictionary qp_solver_options = getOption("qp_solver_options"); qp_solver_.setOption(qp_solver_options); } // Initialize the QP solver qp_solver_.init(); if(verbose_){ cout << "Allocated QP solver." << endl; } // Residual d_k_ = DMatrix(d.sparsity(),0); // Primal step dx_k_ = DMatrix(x_k.sparsity()); // Dual step dlam_x_k_ = DMatrix(lam_x_k.sparsity()); dlam_g_k_ = DMatrix(lam_g_k.sparsity()); }