SXFunction vec (const SXFunction &a) {
// Pass null if input is null
if (a.isNull()) return SXFunction();
/// Get the SX input and output vectors
std::vector<SXMatrix> symbolicInputSX = a.inputsSX();
std::vector<SXMatrix> symbolicOutputSX = a.outputsSX();
// Apply vec to them
for (int i=0;i<symbolicInputSX.size();++i)
symbolicInputSX[i] = vec(symbolicInputSX[i]);
for (int i=0;i<symbolicOutputSX.size();++i)
symbolicOutputSX[i] = vec(symbolicOutputSX[i]);
// Make a new function with the vecced input/outputs
SXFunction ret(symbolicInputSX,symbolicOutputSX);
// Initialize it if a was
if (a.isInit()) ret.init();
return ret;
}
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());
}
Function NlpSolver::joinFG(Function F, Function G) {
if (G.isNull()) {
// unconstrained
if (is_a<SXFunction>(F)) {
SXFunction F_sx = shared_cast<SXFunction>(F);
vector<SX> nlp_in = F_sx.inputExpr();
nlp_in.resize(NL_NUM_IN);
vector<SX> nlp_out(NL_NUM_OUT);
nlp_out[NL_F] = F_sx.outputExpr(0);
return SXFunction(nlp_in, nlp_out);
} else if (is_a<MXFunction>(F)) {
MXFunction F_mx = shared_cast<MXFunction>(F);
vector<MX> nlp_in = F_mx.inputExpr();
nlp_in.resize(NL_NUM_IN);
vector<MX> nlp_out(NL_NUM_OUT);
nlp_out[NL_F] = F_mx.outputExpr(0);
return MXFunction(nlp_in, nlp_out);
} else {
vector<MX> F_in = F.symbolicInput();
vector<MX> nlp_in(NL_NUM_IN);
nlp_in[NL_X] = F_in.at(0);
if (F_in.size()>1) nlp_in[NL_P] = F_in.at(1);
vector<MX> nlp_out(NL_NUM_OUT);
nlp_out[NL_F] = F(F_in).front();
return MXFunction(nlp_in, nlp_out);
}
} else if (F.isNull()) {
// feasibility problem
if (is_a<SXFunction>(G)) {
SXFunction G_sx = shared_cast<SXFunction>(G);
vector<SX> nlp_in = G_sx.inputExpr();
nlp_in.resize(NL_NUM_IN);
vector<SX> nlp_out(NL_NUM_OUT);
nlp_out[NL_G] = G_sx.outputExpr(0);
return SXFunction(nlp_in, nlp_out);
} else if (is_a<MXFunction>(G)) {
MXFunction G_mx = shared_cast<MXFunction>(F);
vector<MX> nlp_in = G_mx.inputExpr();
nlp_in.resize(NL_NUM_IN);
vector<MX> nlp_out(NL_NUM_OUT);
nlp_out[NL_G] = G_mx.outputExpr(0);
nlp_out.resize(NL_NUM_OUT);
return MXFunction(nlp_in, nlp_out);
} else {
vector<MX> G_in = G.symbolicInput();
vector<MX> nlp_in(NL_NUM_IN);
nlp_in[NL_X] = G_in.at(0);
if (G_in.size()>1) nlp_in[NL_P] = G_in.at(1);
vector<MX> nlp_out(NL_NUM_OUT);
nlp_out[NL_G] = G(G_in).at(0);
return MXFunction(nlp_in, nlp_out);
}
} else {
// Standard (constrained) NLP
// SXFunction if both functions are SXFunction
if (is_a<SXFunction>(F) && is_a<SXFunction>(G)) {
vector<SX> nlp_in(NL_NUM_IN), nlp_out(NL_NUM_OUT);
SXFunction F_sx = shared_cast<SXFunction>(F);
SXFunction G_sx = shared_cast<SXFunction>(G);
nlp_in[NL_X] = G_sx.inputExpr(0);
if (G_sx.getNumInputs()>1) {
nlp_in[NL_P] = G_sx.inputExpr(1);
} else {
nlp_in[NL_P] = SX::sym("p", 1, 0);
}
// Expression for f and g
nlp_out[NL_G] = G_sx.outputExpr(0);
nlp_out[NL_F] = substitute(F_sx.outputExpr(), F_sx.inputExpr(), G_sx.inputExpr()).front();
return SXFunction(nlp_in, nlp_out);
} else { // MXFunction otherwise
vector<MX> nlp_in(NL_NUM_IN), nlp_out(NL_NUM_OUT);
// Try to cast into MXFunction
MXFunction F_mx = shared_cast<MXFunction>(F);
MXFunction G_mx = shared_cast<MXFunction>(G);
// Convert to MX if cast failed and make sure that they
// use the same expressions if cast was successful
if (!G_mx.isNull()) {
nlp_in[NL_X] = G_mx.inputExpr(0);
if (G_mx.getNumInputs()>1) {
nlp_in[NL_P] = G_mx.inputExpr(1);
} else {
nlp_in[NL_P] = MX::sym("p", 1, 0);
}
nlp_out[NL_G] = G_mx.outputExpr(0);
if (!F_mx.isNull()) { // Both are MXFunction, make sure they use the same variables
nlp_out[NL_F] = substitute(F_mx.outputExpr(), F_mx.inputExpr(),
G_mx.inputExpr()).front();
} else { // G_ but not F_ MXFunction
nlp_out[NL_F] = F(G_mx.inputExpr()).front();
}
} else {
if (!F_mx.isNull()) { // F but not G MXFunction
nlp_in[NL_X] = F_mx.inputExpr(0);
if (F_mx.getNumInputs()>1) {
nlp_in[NL_P] = F_mx.inputExpr(1);
} else {
nlp_in[NL_P] = MX::sym("p", 1, 0);
}
nlp_out[NL_F] = F_mx.outputExpr(0);
nlp_out[NL_G] = G(F_mx.inputExpr()).front();
} else { // None of them MXFunction
vector<MX> FG_in = G.symbolicInput();
nlp_in[NL_X] = FG_in.at(0);
if (FG_in.size()>1) nlp_in[NL_P] = FG_in.at(1);
nlp_out[NL_G] = G(FG_in).front();
nlp_out[NL_F] = F(FG_in).front();
}
}
return MXFunction(nlp_in, nlp_out);
} // SXFunction/MXFunction
} // constrained/unconstrained
}
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;
}
}
Function implicitRK(Function& f, const std::string& impl, const Dictionary& impl_options,
const MX& tf, int order, const std::string& scheme, int ne) {
casadi_assert_message(ne>=1, "Parameter ne (number of elements must be at least 1), "
"but got " << ne << ".");
casadi_assert_message(order==4, "Only RK order 4 is supported now.");
casadi_assert_message(f.getNumInputs()==DAE_NUM_IN && f.getNumOutputs()==DAE_NUM_OUT,
"Supplied function must adhere to dae scheme.");
casadi_assert_message(f.output(DAE_QUAD).isEmpty(),
"Supplied function cannot have quadrature states.");
// Obtain collocation points
std::vector<double> tau_root = collocationPoints(order, "legendre");
// Retrieve collocation interpolating matrices
std::vector < std::vector <double> > C;
std::vector < double > D;
collocationInterpolators(tau_root, C, D);
// Retrieve problem dimensions
int nx = f.input(DAE_X).size();
int nz = f.input(DAE_Z).size();
int np = f.input(DAE_P).size();
//Variables for one finite element
MX X = MX::sym("X", nx);
MX P = MX::sym("P", np);
MX V = MX::sym("V", order*(nx+nz)); // Unknowns
MX X0 = X;
// Components of the unknowns that correspond to states at collocation points
std::vector<MX> Xc;Xc.reserve(order);
Xc.push_back(X0);
// Components of the unknowns that correspond to algebraic states at collocation points
std::vector<MX> Zc;Zc.reserve(order);
// Splitting the unknowns
std::vector<int> splitPositions = range(0, order*nx, nx);
if (nz>0) {
std::vector<int> Zc_pos = range(order*nx, order*nx+(order+1)*nz, nz);
splitPositions.insert(splitPositions.end(), Zc_pos.begin(), Zc_pos.end());
} else {
splitPositions.push_back(order*nx);
}
std::vector<MX> Vs = vertsplit(V, splitPositions);
// Extracting unknowns from Z
for (int i=0;i<order;++i) {
Xc.push_back(X0+Vs[i]);
}
if (nz>0) {
for (int i=0;i<order;++i) {
Zc.push_back(Vs[order+i]);
}
}
// Get the collocation Equations (that define V)
std::vector<MX> V_eq;
// Local start time
MX t0_l=MX::sym("t0");
MX h = MX::sym("h");
for (int j=1;j<order+1;++j) {
// Expression for the state derivative at the collocation point
MX xp_j = 0;
for (int r=0;r<order+1;++r) {
xp_j+= C[j][r]*Xc[r];
}
// Append collocation equations & algebraic constraints
std::vector<MX> f_out;
MX t_l = t0_l+tau_root[j]*h;
if (nz>0) {
f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P, "z", Zc[j-1]));
} else {
f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P));
}
V_eq.push_back(h*f_out[DAE_ODE]-xp_j);
V_eq.push_back(f_out[DAE_ALG]);
}
// Root-finding function, implicitly defines V as a function of X0 and P
std::vector<MX> vfcn_inputs;
vfcn_inputs.push_back(V);
vfcn_inputs.push_back(X);
vfcn_inputs.push_back(P);
vfcn_inputs.push_back(t0_l);
vfcn_inputs.push_back(h);
Function vfcn = MXFunction(vfcn_inputs, vertcat(V_eq));
vfcn.init();
try {
// Attempt to convert to SXFunction to decrease overhead
vfcn = SXFunction(vfcn);
vfcn.init();
} catch(CasadiException & e) {
//
}
// Create a implicit function instance to solve the system of equations
ImplicitFunction ifcn(impl, vfcn, Function(), LinearSolver());
ifcn.setOption(impl_options);
ifcn.init();
// Get an expression for the state at the end of the finite element
std::vector<MX> ifcn_call_in(5);
ifcn_call_in[0] = MX::zeros(V.sparsity());
std::copy(vfcn_inputs.begin()+1, vfcn_inputs.end(), ifcn_call_in.begin()+1);
std::vector<MX> ifcn_call_out = ifcn.call(ifcn_call_in, true);
Vs = vertsplit(ifcn_call_out[0], splitPositions);
MX XF = 0;
for (int i=0;i<order+1;++i) {
XF += D[i]*(i==0? X : X + Vs[i-1]);
}
// Get the discrete time dynamics
ifcn_call_in.erase(ifcn_call_in.begin());
MXFunction F = MXFunction(ifcn_call_in, XF);
F.init();
// Loop over all finite elements
MX h_ = tf/ne;
MX t0_ = 0;
for (int i=0;i<ne;++i) {
std::vector<MX> F_in;
F_in.push_back(X);
F_in.push_back(P);
F_in.push_back(t0_);
F_in.push_back(h_);
t0_+= h_;
std::vector<MX> F_out = F.call(F_in);
X = F_out[0];
}
// Create a ruturn function with Integrator signature
MXFunction ret = MXFunction(integratorIn("x0", X0, "p", P), integratorOut("xf", X));
ret.init();
return ret;
}
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;
}
}
Function SXFunctionInternal::getFullJacobian() {
SX J = veccat(outputv_).zz_jacobian(veccat(inputv_));
return SXFunction(name_ + "_jac", inputv_, make_vector(J));
}
void SimulatorInternal::init() {
// Let the integration time start from the first point of the time grid.
if (!grid_.empty()) integrator_.setOption("t0", grid_[0]);
// Let the integration time stop at the last point of the time grid.
if (!grid_.empty()) integrator_.setOption("tf", grid_[grid_.size()-1]);
casadi_assert_message(isNonDecreasing(grid_), "The supplied time grid must be non-decreasing.");
// Initialize the integrator
integrator_.init();
// Generate an output function if there is none (returns the whole state)
if (output_fcn_.isNull()) {
SX t = SX::sym("t");
SX x = SX::sym("x", integrator_.input(INTEGRATOR_X0).sparsity());
SX z = SX::sym("z", integrator_.input(INTEGRATOR_Z0).sparsity());
SX p = SX::sym("p", integrator_.input(INTEGRATOR_P).sparsity());
vector<SX> arg(DAE_NUM_IN);
arg[DAE_T] = t;
arg[DAE_X] = x;
arg[DAE_Z] = z;
arg[DAE_P] = p;
vector<SX> out(INTEGRATOR_NUM_OUT);
out[INTEGRATOR_XF] = x;
out[INTEGRATOR_ZF] = z;
// Create the output function
output_fcn_ = SXFunction(arg, out);
output_.scheme = SCHEME_IntegratorOutput;
}
// Initialize the output function
output_fcn_.init();
// Allocate inputs
setNumInputs(INTEGRATOR_NUM_IN);
for (int i=0; i<INTEGRATOR_NUM_IN; ++i) {
input(i) = integrator_.input(i);
}
// Allocate outputs
setNumOutputs(output_fcn_->getNumOutputs());
for (int i=0; i<getNumOutputs(); ++i) {
output(i) = Matrix<double>::zeros(output_fcn_.output(i).numel(), grid_.size());
if (!output_fcn_.output(i).isEmpty()) {
casadi_assert_message(output_fcn_.output(i).isVector(),
"SimulatorInternal::init: Output function output #" << i
<< " has shape " << output_fcn_.output(i).dimString()
<< ", while a column-matrix shape is expected.");
}
}
casadi_assert_message(output_fcn_.input(DAE_T).numel() <=1,
"SimulatorInternal::init: output_fcn DAE_T argument must be "
"scalar or empty, but got " << output_fcn_.input(DAE_T).dimString());
casadi_assert_message(
output_fcn_.input(DAE_P).isEmpty() ||
integrator_.input(INTEGRATOR_P).sparsity() == output_fcn_.input(DAE_P).sparsity(),
"SimulatorInternal::init: output_fcn DAE_P argument must be empty or"
<< " have dimension " << integrator_.input(INTEGRATOR_P).dimString()
<< ", but got " << output_fcn_.input(DAE_P).dimString());
casadi_assert_message(
output_fcn_.input(DAE_X).isEmpty() ||
integrator_.input(INTEGRATOR_X0).sparsity() == output_fcn_.input(DAE_X).sparsity(),
"SimulatorInternal::init: output_fcn DAE_X argument must be empty or have dimension "
<< integrator_.input(INTEGRATOR_X0).dimString()
<< ", but got " << output_fcn_.input(DAE_X).dimString());
// Call base class method
FunctionInternal::init();
// Output iterators
output_its_.resize(getNumOutputs());
}
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 RKIntegratorInternal::init(){
// Call the base class init
IntegratorInternal::init();
casadi_assert_message(nq_==0, "Quadratures not supported.");
// Number of finite elements
int nk = getOption("number_of_finite_elements");
// Interpolation order
int deg = getOption("interpolation_order");
casadi_assert_message(deg==1, "Not implemented");
// Expand f?
bool expand_f = getOption("expand_f");
// Size of the finite elements
double h = (tf_-t0_)/nk;
// MX version of the same
MX h_mx = h;
// Initial state
MX Y0("Y0",nx_);
// Free parameters
MX P("P",np_);
// Current state
MX Y = Y0;
// Dummy time
MX T = 0;
// Integrate until the end
for(int k=0; k<nk; ++k){
// Call the ode right hand side function
vector<MX> f_in(DAE_NUM_IN);
f_in[DAE_T] = T;
f_in[DAE_X] = Y;
f_in[DAE_P] = P;
vector<MX> f_out = f_.call(f_in);
MX ode_rhs = f_out[DAE_ODE];
// Explicit Euler step
Y += h_mx*ode_rhs;
}
// Create a function which returns the state at the end of the time horizon
vector<MX> yf_in(2);
yf_in[0] = Y0;
yf_in[1] = P;
MXFunction yf_fun(yf_in,Y);
// Should the function be expanded in elementary operations?
if(expand_f){
yf_fun.init();
yf_fun_ = SXFunction(yf_fun);
} else {
yf_fun_ = yf_fun;
}
// Set number of derivative directions
yf_fun_.setOption("number_of_fwd_dir",getOption("number_of_fwd_dir"));
yf_fun_.setOption("number_of_adj_dir",getOption("number_of_adj_dir"));
// Initialize function
yf_fun_.init();
}
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());
}
void SdpSolverInternal::init() {
// Call the init method of the base class
FunctionInternal::init();
calc_p_ = getOption("calc_p");
calc_dual_ = getOption("calc_dual");
print_problem_ = getOption("print_problem");
// Find aggregate sparsity pattern
Sparsity aggregate = input(SDP_SOLVER_G).sparsity();
for (int i=0;i<n_;++i) {
aggregate = aggregate + input(SDP_SOLVER_F)(ALL, Slice(i*m_, (i+1)*m_)).sparsity();
}
// Detect block diagonal structure in this sparsity pattern
std::vector<int> p;
std::vector<int> r;
nb_ = aggregate.stronglyConnectedComponents(p, r);
block_boundaries_.resize(nb_+1);
std::copy(r.begin(), r.begin()+nb_+1, block_boundaries_.begin());
block_sizes_.resize(nb_);
for (int i=0;i<nb_;++i) {
block_sizes_[i]=r[i+1]-r[i];
}
// Make a mapping function from dense blocks to inversely-permuted block diagonal P
std::vector< SX > full_blocks;
for (int i=0;i<nb_;++i) {
full_blocks.push_back(SX::sym("block", block_sizes_[i], block_sizes_[i]));
}
Pmapper_ = SXFunction(full_blocks, blkdiag(full_blocks)(lookupvector(p, p.size()),
lookupvector(p, p.size())));
Pmapper_.init();
if (nb_>0) {
// Make a mapping function from (G, F) -> (G[p, p]_j, F_i[p, p]j)
SX G = SX::sym("G", input(SDP_SOLVER_G).sparsity());
SX F = SX::sym("F", input(SDP_SOLVER_F).sparsity());
std::vector<SX> in;
in.push_back(G);
in.push_back(F);
std::vector<SX> out((n_+1)*nb_);
for (int j=0;j<nb_;++j) {
out[j] = G(p, p)(Slice(r[j], r[j+1]), Slice(r[j], r[j+1]));
}
for (int i=0;i<n_;++i) {
SX Fi = F(ALL, Slice(i*m_, (i+1)*m_))(p, p);
for (int j=0;j<nb_;++j) {
out[(i+1)*nb_+j] = Fi(Slice(r[j], r[j+1]), Slice(r[j], r[j+1]));
}
}
mapping_ = SXFunction(in, out);
mapping_.init();
}
// Output arguments
setNumOutputs(SDP_SOLVER_NUM_OUT);
output(SDP_SOLVER_X) = DMatrix::zeros(n_, 1);
output(SDP_SOLVER_P) = calc_p_? DMatrix(Pmapper_.output().sparsity(), 0) : DMatrix();
output(SDP_SOLVER_DUAL) = calc_dual_? DMatrix(Pmapper_.output().sparsity(), 0) : DMatrix();
output(SDP_SOLVER_COST) = 0.0;
output(SDP_SOLVER_DUAL_COST) = 0.0;
output(SDP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1);
output(SDP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1);
}
