void QpToNlp::init(const Dict& opts) {
// Initialize the base classes
Qpsol::init(opts);
// Default options
string nlpsol_plugin;
Dict nlpsol_options;
// Read user options
for (auto&& op : opts) {
if (op.first=="nlpsol") {
nlpsol_plugin = op.second.to_string();
} else if (op.first=="nlpsol_options") {
nlpsol_options = op.second;
}
}
// Create a symbolic matrix for the decision variables
SX X = SX::sym("X", n_, 1);
// Parameters to the problem
SX H = SX::sym("H", sparsity_in(QPSOL_H));
SX G = SX::sym("G", sparsity_in(QPSOL_G));
SX A = SX::sym("A", sparsity_in(QPSOL_A));
// Put parameters in a vector
std::vector<SX> par;
par.push_back(H.nonzeros());
par.push_back(G.nonzeros());
par.push_back(A.nonzeros());
// The nlp looks exactly like a mathematical description of the NLP
SXDict nlp = {{"x", X}, {"p", vertcat(par)},
{"f", mtimes(G.T(), X) + 0.5*mtimes(mtimes(X.T(), H), X)},
{"g", mtimes(A, X)}};
// Create an Nlpsol instance
casadi_assert_message(!nlpsol_plugin.empty(), "'nlpsol' option has not been set");
solver_ = nlpsol("nlpsol", nlpsol_plugin, nlp, nlpsol_options);
alloc(solver_);
// Allocate storage for NLP solver parameters
alloc_w(solver_.nnz_in(NLPSOL_P), true);
}
void SymbolicQr::init() {
// Call the base class initializer
LinearSolverInternal::init();
// Read options
bool codegen = getOption("codegen");
string compiler = getOption("compiler");
// Make sure that command processor is available
if (codegen) {
#ifdef WITH_DL
int flag = system(static_cast<const char*>(0));
casadi_assert_message(flag!=0, "No command procesor available");
#else // WITH_DL
casadi_error("Codegen requires CasADi to be compiled with option \"WITH_DL\" enabled");
#endif // WITH_DL
}
// Symbolic expression for A
SX A = SX::sym("A", input(0).sparsity());
// Get the inverted column permutation
std::vector<int> inv_colperm(colperm_.size());
for (int k=0; k<colperm_.size(); ++k)
inv_colperm[colperm_[k]] = k;
// Get the inverted row permutation
std::vector<int> inv_rowperm(rowperm_.size());
for (int k=0; k<rowperm_.size(); ++k)
inv_rowperm[rowperm_[k]] = k;
// Permute the linear system
SX Aperm = A(rowperm_, colperm_);
// Generate the QR factorization function
vector<SX> QR(2);
qr(Aperm, QR[0], QR[1]);
SXFunction fact_fcn(A, QR);
// Optionally generate c code and load as DLL
if (codegen) {
stringstream ss;
ss << "symbolic_qr_fact_fcn_" << this;
fact_fcn_ = dynamicCompilation(fact_fcn, ss.str(),
"Symbolic QR factorization function", compiler);
} else {
fact_fcn_ = fact_fcn;
}
// Initialize factorization function
fact_fcn_.setOption("name", "QR_fact");
fact_fcn_.init();
// Symbolic expressions for solve function
SX Q = SX::sym("Q", QR[0].sparsity());
SX R = SX::sym("R", QR[1].sparsity());
SX b = SX::sym("b", input(1).size1(), 1);
// Solve non-transposed
// We have Pb' * Q * R * Px * x = b <=> x = Px' * inv(R) * Q' * Pb * b
// Permute the right hand sides
SX bperm = b(rowperm_, ALL);
// Solve the factorized system
SX xperm = casadi::solve(R, mul(Q.T(), bperm));
// Permute back the solution
SX x = xperm(inv_colperm, ALL);
// Generate the QR solve function
vector<SX> solv_in(3);
solv_in[0] = Q;
solv_in[1] = R;
solv_in[2] = b;
SXFunction solv_fcn(solv_in, x);
// Optionally generate c code and load as DLL
if (codegen) {
stringstream ss;
ss << "symbolic_qr_solv_fcn_N_" << this;
solv_fcn_N_ = dynamicCompilation(solv_fcn, ss.str(), "QR_solv_N", compiler);
} else {
solv_fcn_N_ = solv_fcn;
}
// Initialize solve function
solv_fcn_N_.setOption("name", "QR_solv");
solv_fcn_N_.init();
// Solve transposed
// We have (Pb' * Q * R * Px)' * x = b
// <=> Px' * R' * Q' * Pb * x = b
// <=> x = Pb' * Q * inv(R') * Px * b
// Permute the right hand side
bperm = b(colperm_, ALL);
// Solve the factorized system
xperm = mul(Q, casadi::solve(R.T(), bperm));
// Permute back the solution
x = xperm(inv_rowperm, ALL);
// Mofify the QR solve function
solv_fcn = SXFunction(solv_in, x);
// Optionally generate c code and load as DLL
if (codegen) {
stringstream ss;
ss << "symbolic_qr_solv_fcn_T_" << this;
solv_fcn_T_ = dynamicCompilation(solv_fcn, ss.str(), "QR_solv_T", compiler);
} else {
solv_fcn_T_ = solv_fcn;
}
// Initialize solve function
solv_fcn_T_.setOption("name", "QR_solv_T");
solv_fcn_T_.init();
// Allocate storage for QR factorization
Q_ = DMatrix::zeros(Q.sparsity());
R_ = DMatrix::zeros(R.sparsity());
}
void Sqpmethod::init() {
// Call the init method of the base class
NlpSolverInternal::init();
// Read options
max_iter_ = getOption("max_iter");
max_iter_ls_ = getOption("max_iter_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");
exact_hessian_ = getOption("hessian_approximation")=="exact";
min_step_size_ = getOption("min_step_size");
// Get/generate required functions
gradF();
jacG();
if (exact_hessian_) {
hessLag();
}
// Allocate a QP solver
Sparsity H_sparsity = exact_hessian_ ? hessLag().output().sparsity()
: Sparsity::dense(nx_, nx_);
H_sparsity = H_sparsity + Sparsity::diag(nx_);
Sparsity A_sparsity = jacG().isNull() ? Sparsity(0, nx_)
: jacG().output().sparsity();
// QP solver options
Dict qp_solver_options;
if (hasSetOption("qp_solver_options")) {
qp_solver_options = getOption("qp_solver_options");
}
// Allocate a QP solver
qp_solver_ = QpSolver("qp_solver", getOption("qp_solver"),
make_map("h", H_sparsity, "a", A_sparsity),
qp_solver_options);
// Lagrange multipliers of the NLP
mu_.resize(ng_);
mu_x_.resize(nx_);
// Lagrange gradient in the next iterate
gLag_.resize(nx_);
gLag_old_.resize(nx_);
// Current linearization point
x_.resize(nx_);
x_cand_.resize(nx_);
x_old_.resize(nx_);
// Constraint function value
gk_.resize(ng_);
gk_cand_.resize(ng_);
// Hessian approximation
Bk_ = DMatrix::zeros(H_sparsity);
// Jacobian
Jk_ = DMatrix::zeros(A_sparsity);
// Bounds of the QP
qp_LBA_.resize(ng_);
qp_UBA_.resize(ng_);
qp_LBX_.resize(nx_);
qp_UBX_.resize(nx_);
// QP solution
dx_.resize(nx_);
qp_DUAL_X_.resize(nx_);
qp_DUAL_A_.resize(ng_);
// Gradient of the objective
gf_.resize(nx_);
// Create Hessian update function
if (!exact_hessian_) {
// Create expressions corresponding to Bk, x, x_old, gLag and gLag_old
SX Bk = SX::sym("Bk", H_sparsity);
SX x = SX::sym("x", input(NLP_SOLVER_X0).sparsity());
SX x_old = SX::sym("x", x.sparsity());
SX gLag = SX::sym("gLag", x.sparsity());
SX gLag_old = SX::sym("gLag_old", x.sparsity());
SX sk = x - x_old;
SX yk = gLag - gLag_old;
SX qk = mul(Bk, sk);
// Calculating theta
SX skBksk = inner_prod(sk, qk);
SX 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;
SX theta = 1. / inner_prod(sk, yk);
SX phi = 1. / inner_prod(qk, sk);
SX Bk_new = Bk + theta * mul(yk, yk.T()) - phi * mul(qk, qk.T());
// Inputs of the BFGS update function
vector<SX> 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", bfgs_in, make_vector(Bk_new));
// Initial Hessian approximation
B_init_ = DMatrix::eye(nx_);
}
// Header
if (static_cast<bool>(getOption("print_header"))) {
userOut()
<< "-------------------------------------------" << endl
<< "This is casadi::SQPMethod." << endl;
if (exact_hessian_) {
userOut() << "Using exact Hessian" << endl;
} else {
userOut() << "Using limited memory BFGS Hessian approximation" << endl;
}
userOut()
<< endl
<< "Number of variables: " << setw(9) << nx_ << endl
<< "Number of constraints: " << setw(9) << ng_ << endl
<< "Number of nonzeros in constraint Jacobian: " << setw(9) << A_sparsity.nnz() << endl
<< "Number of nonzeros in Lagrangian Hessian: " << setw(9) << H_sparsity.nnz() << endl
<< endl;
}
}
