void substituteInPlace(const SXMatrix &v, SXMatrix &vdef, std::vector<SXMatrix>& ex, bool reverse){
casadi_assert_message(isSymbolic(v),"the variable is not symbolic");
casadi_assert_message(v.sparsity() == vdef.sparsity(),"the sparsity patterns of the expression and its defining expression do not match");
if(v.empty()) return; // quick return if nothing to replace
// Function inputs
std::vector<SXMatrix> f_in;
if(!reverse) f_in.push_back(v);
// Function outputs
std::vector<SXMatrix> f_out;
f_out.push_back(vdef);
f_out.insert(f_out.end(),ex.begin(),ex.end());
// Write the mapping function
SXFunction f(f_in,f_out);
f.init();
// Get references to the internal data structures
const vector<SXAlgEl>& algorithm = f.algorithm();
vector<SX> work(f.getWorkSize());
// Iterator to the binary operations
vector<SX>::const_iterator b_it=f->operations_.begin();
// Iterator to stack of constants
vector<SX>::const_iterator c_it = f->constants_.begin();
// Iterator to free variables
vector<SX>::const_iterator p_it = f->free_vars_.begin();
// Evaluate the algorithm
for(vector<SXAlgEl>::const_iterator it=algorithm.begin(); it<algorithm.end(); ++it){
switch(it->op){
case OP_INPUT:
// reverse is false, substitute out
work[it->res] = vdef.at(it->arg.i[1]);
break;
case OP_OUTPUT:
if(it->res==0){
vdef.at(it->arg.i[1]) = work[it->arg.i[0]];
if(reverse){
// Use the new variable henceforth, substitute in
work[it->arg.i[0]] = v.at(it->arg.i[1]);
}
} else {
// Auxillary output
ex[it->res-1].at(it->arg.i[1]) = work[it->arg.i[0]];
}
break;
case OP_CONST: work[it->res] = *c_it++; break;
case OP_PARAMETER: work[it->res] = *p_it++; break;
default:
{
switch(it->op){
CASADI_MATH_FUN_BUILTIN(work[it->arg.i[0]],work[it->arg.i[1]],work[it->res])
}
// Avoid creating duplicates
const int depth = 2; // NOTE: a higher depth could possibly give more savings
work[it->res].assignIfDuplicate(*b_it++,depth);
}
}
}
}
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;
}
}
