/// Solve the linear system Ax = b, with A being the
/// combined derivative matrix of the residual and b
/// being the residual itself.
/// \param[in] residual residual object containing A and b.
/// \return the solution x
NewtonIterationBlackoilSimple::SolutionVector
NewtonIterationBlackoilSimple::computeNewtonIncrement(const LinearisedBlackoilResidual& residual) const
{
typedef LinearisedBlackoilResidual::ADB ADB;
const int np = residual.material_balance_eq.size();
ADB mass_res = residual.material_balance_eq[0];
for (int phase = 1; phase < np; ++phase) {
mass_res = vertcat(mass_res, residual.material_balance_eq[phase]);
}
const ADB well_res = vertcat(residual.well_flux_eq, residual.well_eq);
const ADB total_residual = collapseJacs(vertcat(mass_res, well_res));
Eigen::SparseMatrix<double, Eigen::RowMajor> matr;
total_residual.derivative()[0].toSparse(matr);
SolutionVector dx(SolutionVector::Zero(total_residual.size()));
Opm::LinearSolverInterface::LinearSolverReport rep
= linsolver_->solve(matr.rows(), matr.nonZeros(),
matr.outerIndexPtr(), matr.innerIndexPtr(), matr.valuePtr(),
total_residual.value().data(), dx.data(), parallelInformation_);
// store iterations
iterations_ = rep.iterations;
if (!rep.converged) {
OPM_THROW(LinearSolverProblem,
"FullyImplicitBlackoilSolver::solveJacobianSystem(): "
"Linear solver convergence failure.");
}
return dx;
}
Function IdasInterface::getJ(bool backward) const {
vector<MatType> a = MatType::get_input(oracle_);
vector<MatType> r = const_cast<Function&>(oracle_)(a);
MatType cj = MatType::sym("cj");
// Get the Jacobian in the Newton iteration
if (backward) {
MatType jac = MatType::jacobian(r[DE_RODE], a[DE_RX]) + cj*MatType::eye(nrx_);
if (nrz_>0) {
jac = horzcat(vertcat(jac,
MatType::jacobian(r[DE_RALG], a[DE_RX])),
vertcat(MatType::jacobian(r[DE_RODE], a[DE_RZ]),
MatType::jacobian(r[DE_RALG], a[DE_RZ])));
}
return Function("jacB", {a[DE_T], a[DE_RX], a[DE_RZ], a[DE_RP],
a[DE_X], a[DE_Z], a[DE_P], cj}, {jac});
} else {
MatType jac = MatType::jacobian(r[DE_ODE], a[DE_X]) - cj*MatType::eye(nx_);
if (nz_>0) {
jac = horzcat(vertcat(jac,
MatType::jacobian(r[DE_ALG], a[DE_X])),
vertcat(MatType::jacobian(r[DE_ODE], a[DE_Z]),
MatType::jacobian(r[DE_ALG], a[DE_Z])));
}
return Function("jacF", {a[DE_T], a[DE_X], a[DE_Z], a[DE_P], cj}, {jac});
}
}
void SqicInterface::init() {
// Call the init method of the base class
Conic::init();
if (is_init_) sqicDestroy();
inf_ = 1.0e+20;
// Allocate data structures for SQIC
bl_.resize(n_+nc_+1, 0);
bu_.resize(n_+nc_+1, 0);
x_.resize(n_+nc_+1, 0);
hs_.resize(n_+nc_+1, 0);
hEtype_.resize(n_+nc_+1, 0);
pi_.resize(nc_+1, 0);
rc_.resize(n_+nc_+1, 0);
locH_ = st_[QP_STRUCT_H].colind();
indH_ = st_[QP_STRUCT_H].row();
// Fortran indices are one-based
for (int i=0;i<indH_.size();++i) indH_[i]+=1;
for (int i=0;i<locH_.size();++i) locH_[i]+=1;
// Sparsity of augmented linear constraint matrix
Sparsity A_ = vertcat(st_[QP_STRUCT_A], Sparsity::dense(1, n_));
locA_ = A_.colind();
indA_ = A_.row();
// Fortran indices are one-based
for (int i=0;i<indA_.size();++i) indA_[i]+=1;
for (int i=0;i<locA_.size();++i) locA_[i]+=1;
// helper functions for augmented linear constraint matrix
MX a = MX::sym("A", st_[QP_STRUCT_A]);
MX g = MX::sym("g", n_);
std::vector<MX> ins;
ins.push_back(a);
ins.push_back(g);
formatA_ = Function("formatA", ins, vertcat(a, g.T()));
// Set objective row of augmented linear constraints
bu_[n_+nc_] = inf_;
bl_[n_+nc_] = -inf_;
is_init_ = true;
int n = n_;
int m = nc_+1;
int nnzA=formatA_.size_out(0);
int nnzH=input(CONIC_H).size();
std::fill(hEtype_.begin()+n_, hEtype_.end(), 3);
sqic(&m , &n, &nnzA, &indA_[0], &locA_[0], &formatA_.output().nonzeros()[0], &bl_[0], &bu_[0],
&hEtype_[0], &hs_[0], &x_[0], &pi_[0], &rc_[0], &nnzH, &indH_[0], &locH_[0],
&input(CONIC_H).nonzeros()[0]);
}
void Vertsplit::evalAdj(const std::vector<pv_MX>& adjSeed, const std::vector<pv_MX>& adjSens) {
int nadj = adjSeed.size();
int nx = offset_.size()-1;
// Get row offsets
vector<int> row_offset;
row_offset.reserve(offset_.size());
row_offset.push_back(0);
for (std::vector<Sparsity>::const_iterator it=output_sparsity_.begin();
it!=output_sparsity_.end();
++it) {
row_offset.push_back(row_offset.back() + it->size1());
}
for (int d=0; d<nadj; ++d) {
if (adjSens[d][0]!=0) {
vector<MX> v;
for (int i=0; i<nx; ++i) {
MX* x_i = adjSeed[d][i];
if (x_i!=0) {
v.push_back(*x_i);
*x_i = MX();
} else {
v.push_back(MX(output_sparsity_[i].shape()));
}
}
adjSens[d][0]->addToSum(vertcat(v));
}
}
}
T cross(const GenericMatrix<T> &a, const GenericMatrix<T> &b, int dim) {
casadi_assert_message(a.size1()==b.size1() && a.size2()==b.size2(),"cross(a,b): Inconsistent dimensions. Dimension of a (" << a.dimString() << " ) must equal that of b (" << b.dimString() << ").");
casadi_assert_message(a.size1()==3 || a.size2()==3,"cross(a,b): One of the dimensions of a should have length 3, but got " << a.dimString() << ".");
casadi_assert_message(dim==-1 || dim==1 || dim==2,"cross(a,b,dim): Dim must be 1, 2 or -1 (automatic).");
std::vector<T> ret(3);
bool t = a.size1()==3;
if (dim==1) t = true;
if (dim==2) t = false;
T a1 = t ? a(0,ALL) : a(ALL,0);
T a2 = t ? a(1,ALL) : a(ALL,1);
T a3 = t ? a(2,ALL) : a(ALL,2);
T b1 = t ? b(0,ALL) : b(ALL,0);
T b2 = t ? b(1,ALL) : b(ALL,1);
T b3 = t ? b(2,ALL) : b(ALL,2);
ret[0] = a2*b3-a3*b2;
ret[1] = a3*b1-a1*b3;
ret[2] = a1*b2-a2*b1;
return t ? vertcat(ret) : horzcat(ret);
}
std::vector<Sparsity>
LrDpleInternal::getSparsity(const std::map<std::string, std::vector<Sparsity> >& st,
const std::vector< std::vector<int> > &Hs_) {
// Chop-up the arguments
std::vector<Sparsity> As, Vs, Cs, Hs;
if (st.count("a")) As = st.at("a");
if (st.count("v")) Vs = st.at("v");
if (st.count("c")) Cs = st.at("c");
if (st.count("h")) Hs = st.at("h");
bool with_H = !Hs.empty();
int K = As.size();
Sparsity A;
if (K==1) {
A = As[0];
} else {
Sparsity AL = diagcat(vector_slice(As, range(As.size()-1)));
Sparsity AL2 = horzcat(AL, Sparsity(AL.size1(), As[0].size2()));
Sparsity AT = horzcat(Sparsity(As[0].size1(), AL.size2()), As.back());
A = vertcat(AT, AL2);
}
Sparsity V = diagcat(Vs.back(), diagcat(vector_slice(Vs, range(Vs.size()-1))));
Sparsity C;
if (!Cs.empty()) {
C = diagcat(Cs.back(), diagcat(vector_slice(Cs, range(Cs.size()-1))));
}
Sparsity H;
std::vector<int> Hs_agg;
std::vector<int> Hi(1, 0);
if (Hs_.size()>0 && with_H) {
H = diagcat(Hs.back(), diagcat(vector_slice(Hs, range(Hs.size()-1))));
casadi_assert(K==Hs_.size());
for (int k=0;k<K;++k) {
Hs_agg.insert(Hs_agg.end(), Hs_[k].begin(), Hs_[k].end());
int sum=0;
for (int i=0;i<Hs_[k].size();++i) {
sum+= Hs_[k][i];
}
Hi.push_back(Hi.back()+sum);
}
}
Sparsity res = LrDleInternal::getSparsity(make_map("a", A, "v", V, "c", C, "h", H), Hs_agg);
if (with_H) {
return diagsplit(res, Hi);
} else {
return diagsplit(res, As[0].size2());
}
}
Function simpleIntegrator(Function f, const std::string& plugin,
const Dict& plugin_options) {
// Consistency check
casadi_assert_message(f.n_in()==2, "Function must have two inputs: x and p");
casadi_assert_message(f.n_out()==1, "Function must have one outputs: dot(x)");
// Sparsities
Sparsity x_sp = f.sparsity_in(0);
Sparsity p_sp = f.sparsity_in(1);
// Wrapper function inputs
MX x = MX::sym("x", x_sp);
MX u = MX::sym("u", vertcat(Sparsity::scalar(), vec(p_sp))); // augment p with t
// Normalized xdot
int u_offset[] = {0, 1, 1+p_sp.size1()};
vector<MX> pp = vertsplit(u, vector<int>(u_offset, u_offset+3));
MX h = pp[0];
MX p = reshape(pp[1], p_sp.size());
MX f_in[] = {x, p};
MX xdot = f(vector<MX>(f_in, f_in+2)).at(0);
xdot *= h;
// Form DAE function
MXDict dae = {{"x", x}, {"p", u}, {"ode", xdot}};
// Create integrator function
Dict plugin_options2 = plugin_options;
plugin_options2["t0"] = 0; // Normalized time
plugin_options2["tf"] = 1; // Normalized time
Function ifcn = integrator("integrator", plugin, dae, plugin_options2);
// Inputs of constructed function
MX x0 = MX::sym("x0", x_sp);
p = MX::sym("p", p_sp);
h = MX::sym("h");
// State at end
MX xf = ifcn(MXDict{{"x0", x0}, {"p", vertcat(h, vec(p))}}).at("xf");
// Form discrete-time dynamics
return Function("F", {x0, p, h}, {xf}, {"x0", "p", "h"}, {"xf"});
}
T linspace(const GenericMatrix<T> &a_, const GenericMatrix<T> &b_, int nsteps){
const T& a = static_cast<const T&>(a_);
const T& b = static_cast<const T&>(b_);
std::vector<T> ret(nsteps);
ret[0] = a;
T step = (b-a)/(nsteps-1);
for(int i=1; i<nsteps-1; ++i)
ret[i] = ret[i-1] + step;
ret[nsteps-1] = b;
return vertcat(ret);
}
result_type operator()(A0& out, const A1& in) const
{
size_t n = boost::proto::child_c<0>(in);
value_type fn = value_type(n);
table<value_type> ns = nt2::cons(of_size(1, 3), fn, fn/12, fn/20);
table<i_type> ee(nt2::of_size(1, 3));
table<value_type> ff(nt2::of_size(1, 3));
// nt2::frexp(ns, ff, ee);
nt2::frexp(ns(1), ff(1), ee(1));
nt2::frexp(ns(2), ff(2), ee(2));
nt2::frexp(ns(3), ff(3), ee(3));
BOOST_AUTO_TPL(kk, nt2::find(nt2::logical_and(eq(ff, nt2::Half<value_type>()), is_gtz(ee))));
BOOST_ASSERT_MSG(!nt2::isempty(kk), "n must be of form 2^m or 12*2^m or 20*2^m");
size_t k = kk(1);
i_type e = nt2::minusone(ee(k));
out.resize(nt2::of_size(n, n));
table<value_type> h;
if (k == 1) // n = 1 * 2^e;
{
h = nt2::One<value_type>();
}
else if ( k == 2) // N = 12 * 2^e;
{
h = nt2::vertcat(nt2::ones(1,12,meta::as_<value_type>()),
nt2::horzcat(nt2::ones(11,1,meta::as_<value_type>()),
nt2::toeplitz(cons<value_type>(of_size(1, 11), -1, -1, 1, -1, -1, -1, 1, 1, 1, -1, 1),
cons<value_type>(of_size(1, 11), -1, 1, -1, 1, 1, 1, -1, -1, -1, 1, -1)
)
)
);
}
else if ( k == 3) // N = 20 * 2^e;
{
h = vertcat(nt2::ones(1,20,meta::as_<value_type>()),
nt2::horzcat(nt2::ones(19,1,meta::as_<value_type>()),
nt2::hankel(cons<value_type>(of_size(1, 19),-1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1, 1),
cons<value_type>(of_size(1, 19),1, -1, -1, 1, 1, -1, -1, -1, -1, 1, -1, 1, -1, 1, 1, 1, 1, -1, -1)
)
)
);
}
// Kronecker product construction.
for (i_type i = 1; i <= e; ++i)
{
table<value_type> h1 = nt2::vertcat(nt2::horzcat(h, h), nt2::horzcat(h, -h));
h = h1; //ALIASING
}
out = h;
return out;
}
void Vertcat::evaluateMX(const MXPtrV& input, MXPtrV& output, const MXPtrVV& fwdSeed,
MXPtrVV& fwdSens, const MXPtrVV& adjSeed, MXPtrVV& adjSens,
bool output_given) {
int nfwd = fwdSens.size();
int nadj = adjSeed.size();
// Non-differentiated output
if (!output_given) {
*output[0] = vertcat(getVector(input));
}
// Forward sensitivities
for (int d = 0; d<nfwd; ++d) {
*fwdSens[d][0] = vertcat(getVector(fwdSeed[d]));
}
// Quick return?
if (nadj==0) return;
// Get offsets for each row
vector<int> row_offset(ndep()+1, 0);
for (int i=0; i<ndep(); ++i) {
int nrow = dep(i).sparsity().size1();
row_offset[i+1] = row_offset[i] + nrow;
}
// Adjoint sensitivities
for (int d=0; d<nadj; ++d) {
MX& aseed = *adjSeed[d][0];
vector<MX> s = vertsplit(aseed, row_offset);
aseed = MX();
for (int i=0; i<ndep(); ++i) {
adjSens[d][i]->addToSum(s[i]);
}
}
}
void SimpleIndefDpleInternal::init() {
DpleInternal::init();
casadi_assert_message(!pos_def_,
"pos_def option set to True: Solver only handles the indefinite case.");
casadi_assert_message(const_dim_,
"const_dim option set to False: Solver only handles the True case.");
n_ = A_[0].size1();
MX As = MX::sym("A", n_, K_*n_);
MX Vs = MX::sym("V", n_, K_*n_);
std::vector< MX > Vss = horzsplit(Vs, n_);
std::vector< MX > Ass = horzsplit(As, n_);
for (int k=0;k<K_;++k) {
Vss[k]=(Vss[k]+Vss[k].T())/2;
}
std::vector< MX > AA_list(K_);
for (int k=0;k<K_;++k) {
AA_list[k] = kron(Ass[k], Ass[k]);
}
MX AA = blkdiag(AA_list);
MX A_total = DMatrix::eye(n_*n_*K_) -
vertcat(AA(range(K_*n_*n_-n_*n_, K_*n_*n_), range(K_*n_*n_)),
AA(range(K_*n_*n_-n_*n_), range(K_*n_*n_)));
std::vector<MX> Vss_shift;
Vss_shift.push_back(Vss.back());
Vss_shift.insert(Vss_shift.end(), Vss.begin(), Vss.begin()+K_-1);
MX Pf = solve(A_total, vec(horzcat(Vss_shift)), getOption("linear_solver"));
MX P = reshape(Pf, n_, K_*n_);
std::vector<MX> v_in;
v_in.push_back(As);
v_in.push_back(Vs);
f_ = MXFunction(v_in, P);
f_.setInputScheme(SCHEME_DPLEInput);
f_.setOutputScheme(SCHEME_DPLEOutput);
f_.init();
}
void Vertsplit::evalAdj(const std::vector<std::vector<MX> >& aseed,
std::vector<std::vector<MX> >& asens) {
int nadj = aseed.size();
// Get row offsets
vector<int> row_offset;
row_offset.reserve(offset_.size());
row_offset.push_back(0);
for (std::vector<Sparsity>::const_iterator it=output_sparsity_.begin();
it!=output_sparsity_.end(); ++it) {
row_offset.push_back(row_offset.back() + it->size1());
}
for (int d=0; d<nadj; ++d) {
asens[d][0] += vertcat(aseed[d]);
}
}
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 LiftingLrDpleInternal::init() {
form_ = getOptionEnumValue("form");
// Initialize the base classes
LrDpleInternal::init();
casadi_assert_message(!pos_def_,
"pos_def option set to True: Solver only handles the indefinite case.");
casadi_assert_message(const_dim_,
"const_dim option set to False: Solver only handles the True case.");
// We will construct an MXFunction to facilitate the calculation of derivatives
MX As = MX::sym("As", input(LR_DLE_A).sparsity());
MX Vs = MX::sym("Vs", input(LR_DLE_V).sparsity());
MX Cs = MX::sym("Cs", input(LR_DLE_C).sparsity());
MX Hs = MX::sym("Hs", input(LR_DLE_H).sparsity());
n_ = A_[0].size1();
// Chop-up the arguments
std::vector<MX> As_ = horzsplit(As, n_);
std::vector<MX> Vs_ = horzsplit(Vs, V_[0].size2());
std::vector<MX> Cs_ = horzsplit(Cs, V_[0].size2());
std::vector<MX> Hs_;
if (with_H_) {
Hs_ = horzsplit(Hs, Hsi_);
}
MX A;
if (K_==1) {
A = As;
} else {
if (form_==0) {
MX AL = diagcat(vector_slice(As_, range(As_.size()-1)));
MX AL2 = horzcat(AL, MX::sparse(AL.size1(), As_[0].size2()));
MX AT = horzcat(MX::sparse(As_[0].size1(), AL.size2()), As_.back());
A = vertcat(AT, AL2);
} else {
MX AL = diagcat(reverse(vector_slice(As_, range(As_.size()-1))));
MX AL2 = horzcat(MX::sparse(AL.size1(), As_[0].size2()), AL);
MX AT = horzcat(As_.back(), MX::sparse(As_[0].size1(), AL.size2()));
A = vertcat(AL2, AT);
}
}
MX V;
MX C;
MX H;
if (form_==0) {
V = diagcat(Vs_.back(), diagcat(vector_slice(Vs_, range(Vs_.size()-1))));
if (with_C_) {
C = diagcat(Cs_.back(), diagcat(vector_slice(Cs_, range(Cs_.size()-1))));
}
} else {
V = diagcat(diagcat(reverse(vector_slice(Vs_, range(Vs_.size()-1)))), Vs_.back());
if (with_C_) {
C = diagcat(diagcat(reverse(vector_slice(Cs_, range(Cs_.size()-1)))), Cs_.back());
}
}
if (with_H_) {
H = diagcat(form_==0? Hs_ : reverse(Hs_));
}
// Create an LrDleSolver instance
solver_ = LrDleSolver(getOption(solvername()),
lrdleStruct("a", A.sparsity(),
"v", V.sparsity(),
"c", C.sparsity(),
"h", H.sparsity()));
solver_.setOption("Hs", Hss_);
if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname()));
solver_.init();
std::vector<MX> v_in(LR_DPLE_NUM_IN);
v_in[LR_DLE_A] = As;
v_in[LR_DLE_V] = Vs;
if (with_C_) {
v_in[LR_DLE_C] = Cs;
}
if (with_H_) {
v_in[LR_DLE_H] = Hs;
}
std::vector<MX> Pr = solver_.call(lrdpleIn("a", A, "v", V, "c", C, "h", H));
MX Pf = Pr[0];
std::vector<MX> Ps = with_H_ ? diagsplit(Pf, Hsi_) : diagsplit(Pf, n_);
if (form_==1) {
Ps = reverse(Ps);
}
f_ = MXFunction(v_in, dpleOut("p", horzcat(Ps)));
f_.setInputScheme(SCHEME_LR_DPLEInput);
f_.setOutputScheme(SCHEME_LR_DPLEOutput);
f_.init();
Wrapper::checkDimensions();
}
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 CollocationIntegratorInternal::setupFG() {
// Interpolation order
deg_ = getOption("interpolation_order");
// All collocation time points
std::vector<long double> tau_root = collocationPointsL(deg_, getOption("collocation_scheme"));
// Coefficients of the collocation equation
vector<vector<double> > C(deg_+1, vector<double>(deg_+1, 0));
// Coefficients of the continuity equation
vector<double> D(deg_+1, 0);
// Coefficients of the quadratures
vector<double> B(deg_+1, 0);
// For all collocation points
for (int j=0; j<deg_+1; ++j) {
// Construct Lagrange polynomials to get the polynomial basis at the collocation point
Polynomial p = 1;
for (int r=0; r<deg_+1; ++r) {
if (r!=j) {
p *= Polynomial(-tau_root[r], 1)/(tau_root[j]-tau_root[r]);
}
}
// Evaluate the polynomial at the final time to get the
// coefficients of the continuity equation
D[j] = zeroIfSmall(p(1.0L));
// Evaluate the time derivative of the polynomial at all collocation points to
// get the coefficients of the continuity equation
Polynomial dp = p.derivative();
for (int r=0; r<deg_+1; ++r) {
C[j][r] = zeroIfSmall(dp(tau_root[r]));
}
// Integrate polynomial to get the coefficients of the quadratures
Polynomial ip = p.anti_derivative();
B[j] = zeroIfSmall(ip(1.0L));
}
// Symbolic inputs
MX x0 = MX::sym("x0", f_.input(DAE_X).sparsity());
MX p = MX::sym("p", f_.input(DAE_P).sparsity());
MX t = MX::sym("t", f_.input(DAE_T).sparsity());
// Implicitly defined variables (z and x)
MX v = MX::sym("v", deg_*(nx_+nz_));
vector<int> v_offset(1, 0);
for (int d=0; d<deg_; ++d) {
v_offset.push_back(v_offset.back()+nx_);
v_offset.push_back(v_offset.back()+nz_);
}
vector<MX> vv = vertsplit(v, v_offset);
vector<MX>::const_iterator vv_it = vv.begin();
// Collocated states
vector<MX> x(deg_+1), z(deg_+1);
for (int d=1; d<=deg_; ++d) {
x[d] = reshape(*vv_it++, this->x0().shape());
z[d] = reshape(*vv_it++, this->z0().shape());
}
casadi_assert(vv_it==vv.end());
// Collocation time points
vector<MX> tt(deg_+1);
for (int d=0; d<=deg_; ++d) {
tt[d] = t + h_*tau_root[d];
}
// Equations that implicitly define v
vector<MX> eq;
// Quadratures
MX qf = MX::zeros(f_.output(DAE_QUAD).sparsity());
// End state
MX xf = D[0]*x0;
// For all collocation points
for (int j=1; j<deg_+1; ++j) {
//for (int j=deg_; j>=1; --j) {
// Evaluate the DAE
vector<MX> f_arg(DAE_NUM_IN);
f_arg[DAE_T] = tt[j];
f_arg[DAE_P] = p;
f_arg[DAE_X] = x[j];
f_arg[DAE_Z] = z[j];
vector<MX> f_res = f_.call(f_arg);
// Get an expression for the state derivative at the collocation point
MX xp_j = C[0][j] * x0;
for (int r=1; r<deg_+1; ++r) {
xp_j += C[r][j] * x[r];
}
// Add collocation equation
eq.push_back(vec(h_*f_res[DAE_ODE] - xp_j));
// Add the algebraic conditions
eq.push_back(vec(f_res[DAE_ALG]));
// Add contribution to the final state
xf += D[j]*x[j];
// Add contribution to quadratures
qf += (B[j]*h_)*f_res[DAE_QUAD];
}
// Form forward discrete time dynamics
vector<MX> F_in(DAE_NUM_IN);
F_in[DAE_T] = t;
F_in[DAE_X] = x0;
F_in[DAE_P] = p;
F_in[DAE_Z] = v;
vector<MX> F_out(DAE_NUM_OUT);
F_out[DAE_ODE] = xf;
F_out[DAE_ALG] = vertcat(eq);
F_out[DAE_QUAD] = qf;
F_ = MXFunction(F_in, F_out);
F_.init();
// Backwards dynamics
// NOTE: The following is derived so that it will give the exact adjoint
// sensitivities whenever g is the reverse mode derivative of f.
if (!g_.isNull()) {
// Symbolic inputs
MX rx0 = MX::sym("x0", g_.input(RDAE_RX).sparsity());
MX rp = MX::sym("p", g_.input(RDAE_RP).sparsity());
// Implicitly defined variables (rz and rx)
MX rv = MX::sym("v", deg_*(nrx_+nrz_));
vector<int> rv_offset(1, 0);
for (int d=0; d<deg_; ++d) {
rv_offset.push_back(rv_offset.back()+nrx_);
rv_offset.push_back(rv_offset.back()+nrz_);
}
vector<MX> rvv = vertsplit(rv, rv_offset);
vector<MX>::const_iterator rvv_it = rvv.begin();
// Collocated states
vector<MX> rx(deg_+1), rz(deg_+1);
for (int d=1; d<=deg_; ++d) {
rx[d] = reshape(*rvv_it++, this->rx0().shape());
rz[d] = reshape(*rvv_it++, this->rz0().shape());
}
casadi_assert(rvv_it==rvv.end());
// Equations that implicitly define v
eq.clear();
// Quadratures
MX rqf = MX::zeros(g_.output(RDAE_QUAD).sparsity());
// End state
MX rxf = D[0]*rx0;
// For all collocation points
for (int j=1; j<deg_+1; ++j) {
// Evaluate the backward DAE
vector<MX> g_arg(RDAE_NUM_IN);
g_arg[RDAE_T] = tt[j];
g_arg[RDAE_P] = p;
g_arg[RDAE_X] = x[j];
g_arg[RDAE_Z] = z[j];
g_arg[RDAE_RX] = rx[j];
g_arg[RDAE_RZ] = rz[j];
g_arg[RDAE_RP] = rp;
vector<MX> g_res = g_.call(g_arg);
// Get an expression for the state derivative at the collocation point
MX rxp_j = -D[j]*rx0;
for (int r=1; r<deg_+1; ++r) {
rxp_j += (B[r]*C[j][r]) * rx[r];
}
// Add collocation equation
eq.push_back(vec(h_*B[j]*g_res[RDAE_ODE] - rxp_j));
// Add the algebraic conditions
eq.push_back(vec(g_res[RDAE_ALG]));
// Add contribution to the final state
rxf += -B[j]*C[0][j]*rx[j];
// Add contribution to quadratures
rqf += h_*B[j]*g_res[RDAE_QUAD];
}
// Form backward discrete time dynamics
vector<MX> G_in(RDAE_NUM_IN);
G_in[RDAE_T] = t;
G_in[RDAE_X] = x0;
G_in[RDAE_P] = p;
G_in[RDAE_Z] = v;
G_in[RDAE_RX] = rx0;
G_in[RDAE_RP] = rp;
G_in[RDAE_RZ] = rv;
vector<MX> G_out(RDAE_NUM_OUT);
G_out[RDAE_ODE] = rxf;
G_out[RDAE_ALG] = vertcat(eq);
G_out[RDAE_QUAD] = rqf;
G_ = MXFunction(G_in, G_out);
G_.init();
}
}
void SDPSDQPInternal::init() {
// Initialize the base classes
SdqpSolverInternal::init();
cholesky_ = LinearSolver("csparsecholesky", st_[SDQP_STRUCT_H]);
cholesky_.init();
MX g_socp = MX::sym("x", cholesky_.getFactorizationSparsity(true));
MX h_socp = MX::sym("h", n_);
MX f_socp = sqrt(inner_prod(h_socp, h_socp));
MX en_socp = 0.5/f_socp;
MX f_sdqp = MX::sym("f", input(SDQP_SOLVER_F).sparsity());
MX g_sdqp = MX::sym("g", input(SDQP_SOLVER_G).sparsity());
std::vector<MX> fi(n_+1);
MX znp = MX::sparse(n_+1, n_+1);
for (int k=0;k<n_;++k) {
MX gk = vertcat(g_socp(ALL, k), DMatrix::sparse(1, 1));
MX fk = -blockcat(znp, gk, gk.T(), DMatrix::sparse(1, 1));
// TODO(Joel): replace with ALL
fi.push_back(blkdiag(f_sdqp(ALL, Slice(f_sdqp.size1()*k, f_sdqp.size1()*(k+1))), fk));
}
MX fin = en_socp*DMatrix::eye(n_+2);
fin(n_, n_+1) = en_socp;
fin(n_+1, n_) = en_socp;
fi.push_back(blkdiag(DMatrix::sparse(f_sdqp.size1(), f_sdqp.size1()), -fin));
MX h0 = vertcat(h_socp, DMatrix::sparse(1, 1));
MX g = blockcat(f_socp*DMatrix::eye(n_+1), h0, h0.T(), f_socp);
g = blkdiag(g_sdqp, g);
IOScheme mappingIn("g_socp", "h_socp", "f_sdqp", "g_sdqp");
IOScheme mappingOut("f", "g");
mapping_ = MXFunction(mappingIn("g_socp", g_socp, "h_socp", h_socp,
"f_sdqp", f_sdqp, "g_sdqp", g_sdqp),
mappingOut("f", horzcat(fi), "g", g));
mapping_.init();
// Create an sdpsolver instance
std::string sdpsolver_name = getOption("sdp_solver");
sdpsolver_ = SdpSolver(sdpsolver_name,
sdpStruct("g", mapping_.output("g").sparsity(),
"f", mapping_.output("f").sparsity(),
"a", horzcat(input(SDQP_SOLVER_A).sparsity(),
Sparsity::sparse(nc_, 1))));
if (hasSetOption("sdp_solver_options")) {
sdpsolver_.setOption(getOption("sdp_solver_options"));
}
// Initialize the SDP solver
sdpsolver_.init();
sdpsolver_.input(SDP_SOLVER_C).at(n_)=1;
// Output arguments
setNumOutputs(SDQP_SOLVER_NUM_OUT);
output(SDQP_SOLVER_X) = DMatrix::zeros(n_, 1);
std::vector<int> r = range(input(SDQP_SOLVER_G).size1());
output(SDQP_SOLVER_P) = sdpsolver_.output(SDP_SOLVER_P).isEmpty() ? DMatrix() :
sdpsolver_.output(SDP_SOLVER_P)(r, r);
output(SDQP_SOLVER_DUAL) = sdpsolver_.output(SDP_SOLVER_DUAL).isEmpty() ? DMatrix() :
sdpsolver_.output(SDP_SOLVER_DUAL)(r, r);
output(SDQP_SOLVER_COST) = 0.0;
output(SDQP_SOLVER_DUAL_COST) = 0.0;
output(SDQP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1);
output(SDQP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1);
}
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 SdqpToSdp::init() {
// Initialize the base classes
SdqpSolverInternal::init();
cholesky_ = LinearSolver("cholesky", "csparsecholesky", st_[SDQP_STRUCT_H]);
MX g_socp = MX::sym("x", cholesky_.getFactorizationSparsity(true));
MX h_socp = MX::sym("h", n_);
MX f_socp = sqrt(inner_prod(h_socp, h_socp));
MX en_socp = 0.5/f_socp;
MX f_sdqp = MX::sym("f", input(SDQP_SOLVER_F).sparsity());
MX g_sdqp = MX::sym("g", input(SDQP_SOLVER_G).sparsity());
std::vector<MX> fi(n_+1);
MX znp = MX(n_+1, n_+1);
for (int k=0;k<n_;++k) {
MX gk = vertcat(g_socp(ALL, k), MX(1, 1));
MX fk = -blockcat(znp, gk, gk.T(), MX(1, 1));
// TODO(Joel): replace with ALL
fi.push_back(diagcat(f_sdqp(ALL, Slice(f_sdqp.size1()*k, f_sdqp.size1()*(k+1))), fk));
}
MX fin = en_socp*DMatrix::eye(n_+2);
fin(n_, n_+1) = en_socp;
fin(n_+1, n_) = en_socp;
fi.push_back(diagcat(DMatrix(f_sdqp.size1(), f_sdqp.size1()), -fin));
MX h0 = vertcat(h_socp, DMatrix(1, 1));
MX g = blockcat(f_socp*DMatrix::eye(n_+1), h0, h0.T(), f_socp);
g = diagcat(g_sdqp, g);
Dict opts;
opts["input_scheme"] = IOScheme("g_socp", "h_socp", "f_sdqp", "g_sdqp");
opts["output_scheme"] = IOScheme("f", "g");
mapping_ = MXFunction("mapping", make_vector(g_socp, h_socp, f_sdqp, g_sdqp),
make_vector(horzcat(fi), g), opts);
Dict options;
if (hasSetOption(optionsname())) options = getOption(optionsname());
// Create an SdpSolver instance
solver_ = SdpSolver("sdpsolver", getOption(solvername()),
make_map("g", mapping_.output("g").sparsity(),
"f", mapping_.output("f").sparsity(),
"a", horzcat(input(SDQP_SOLVER_A).sparsity(),
Sparsity(nc_, 1))),
options);
solver_.input(SDP_SOLVER_C).at(n_)=1;
// Output arguments
obuf_.resize(SDQP_SOLVER_NUM_OUT);
output(SDQP_SOLVER_X) = DMatrix::zeros(n_, 1);
std::vector<int> r = range(input(SDQP_SOLVER_G).size1());
output(SDQP_SOLVER_P) = solver_.output(SDP_SOLVER_P).isempty() ? DMatrix() :
solver_.output(SDP_SOLVER_P)(r, r);
output(SDQP_SOLVER_DUAL) = solver_.output(SDP_SOLVER_DUAL).isempty() ? DMatrix() :
solver_.output(SDP_SOLVER_DUAL)(r, r);
output(SDQP_SOLVER_COST) = 0.0;
output(SDQP_SOLVER_DUAL_COST) = 0.0;
output(SDQP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1);
output(SDQP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1);
}
NMatrix UKF4::GetLocSR(){
return HT(vertcat(stateStandardDeviations.getRow(0), stateStandardDeviations.getRow(1)));
}
void DirectMultipleShootingInternal::init(){
// Initialize the base classes
OCPSolverInternal::init();
// Create an integrator instance
integratorCreator integrator_creator = getOption("integrator");
integrator_ = integrator_creator(ffcn_,Function());
if(hasSetOption("integrator_options")){
integrator_.setOption(getOption("integrator_options"));
}
// Set t0 and tf
integrator_.setOption("t0",0);
integrator_.setOption("tf",tf_/nk_);
integrator_.init();
// Path constraints present?
bool path_constraints = nh_>0;
// Count the total number of NLP variables
int NV = np_ + // global parameters
nx_*(nk_+1) + // local state
nu_*nk_; // local control
// Declare variable vector for the NLP
// The structure is as follows:
// np x 1 (parameters)
// ------
// nx x 1 (states at time i=0)
// nu x 1 (controls in interval i=0)
// ------
// nx x 1 (states at time i=1)
// nu x 1 (controls in interval i=1)
// ------
// .....
// ------
// nx x 1 (states at time i=nk)
MX V = MX::sym("V",NV);
// Global parameters
MX P = V(Slice(0,np_));
// offset in the variable vector
int v_offset=np_;
// Disretized variables for each shooting node
vector<MX> X(nk_+1), U(nk_);
for(int k=0; k<=nk_; ++k){ // interior nodes
// Local state
X[k] = V[Slice(v_offset,v_offset+nx_)];
v_offset += nx_;
// Variables below do not appear at the end point
if(k==nk_) break;
// Local control
U[k] = V[Slice(v_offset,v_offset+nu_)];
v_offset += nu_;
}
// Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
casadi_assert(v_offset==NV);
// Input to the parallel integrator evaluation
vector<vector<MX> > int_in(nk_);
for(int k=0; k<nk_; ++k){
int_in[k].resize(INTEGRATOR_NUM_IN);
int_in[k][INTEGRATOR_P] = vertcat(P,U[k]);
int_in[k][INTEGRATOR_X0] = X[k];
}
// Input to the parallel function evaluation
vector<vector<MX> > fcn_in(nk_);
for(int k=0; k<nk_; ++k){
fcn_in[k].resize(DAE_NUM_IN);
fcn_in[k][DAE_T] = (k*tf_)/nk_;
fcn_in[k][DAE_P] = vertcat(P,U.at(k));
fcn_in[k][DAE_X] = X[k];
}
// Options for the parallelizer
Dictionary paropt;
// Transmit parallelization mode
if(hasSetOption("parallelization"))
paropt["parallelization"] = getOption("parallelization");
// Evaluate function in parallel
vector<vector<MX> > pI_out = integrator_.callParallel(int_in,paropt);
// Evaluate path constraints in parallel
vector<vector<MX> > pC_out;
if(path_constraints)
pC_out = cfcn_.callParallel(fcn_in,paropt);
//Constraint function
vector<MX> gg(2*nk_);
// Collect the outputs
for(int k=0; k<nk_; ++k){
//append continuity constraints
gg[2*k] = pI_out[k][INTEGRATOR_XF] - X[k+1];
// append the path constraints
if(path_constraints)
gg[2*k+1] = pC_out[k][0];
}
// Terminal constraints
MX g = vertcat(gg);
// Objective function
MX f;
if (mfcn_.getNumInputs()==1) {
f = mfcn_(X.back()).front();
} else {
vector<MX> mfcn_argin(MAYER_NUM_IN);
mfcn_argin[MAYER_X] = X.back();
mfcn_argin[MAYER_P] = P;
f = mfcn_.call(mfcn_argin).front();
}
// NLP
nlp_ = MXFunction(nlpIn("x",V),nlpOut("f",f,"g",g));
nlp_.setOption("ad_mode","forward");
nlp_.init();
// Get the NLP creator function
NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
// Allocate an NLP solver
nlp_solver_ = nlp_solver_creator(nlp_);
// Pass user 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 DirectCollocationInternal::init(){
// Initialize the base classes
OCPSolverInternal::init();
// Free parameters currently not supported
casadi_assert_message(np_==0, "Not implemented");
// Interpolation order
deg_ = getOption("interpolation_order");
// All collocation time points
std::vector<double> tau_root = collocationPoints(deg_,getOption("collocation_scheme"));
// 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::zeros(deg_+1,deg_+1);
// Coefficients of the continuity equation
vector<MX> D(deg_+1);
// Coefficients of the collocation equation as DMatrix
DMatrix D_num = DMatrix::zeros(deg_+1);
// Collocation point
SX tau = SX::sym("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
SX 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
Function tfcn = lfcn.tangent();
tfcn.init();
for(int j2=0; j2<deg_+1; ++j2){
tfcn.setInput(tau_root[j2]);
tfcn.evaluate();
C[j][j2] = tfcn.output();
C_num(j,j2) = tfcn.output();
}
}
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 = MX::sym("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
nlp_ = MXFunction(nlpIn("x",nlp_x), nlpOut("f",nlp_j,"g",vertcat(nlp_g)));
// Get the NLP creator function
NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
// Allocate an NLP solver
nlp_solver_ = nlp_solver_creator(nlp_);
// 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();
}
Function simpleIRK(Function f, int N, int order, const std::string& scheme,
const std::string& solver,
const Dict& solver_options) {
// Consistency check
casadi_assert_message(N>=1, "Parameter N (number of steps) must be at least 1, but got "
<< N << ".");
casadi_assert_message(f.n_in()==2, "Function must have two inputs: x and p");
casadi_assert_message(f.n_out()==1, "Function must have one outputs: dot(x)");
// Obtain collocation points
std::vector<double> tau_root = collocation_points(order, scheme);
tau_root.insert(tau_root.begin(), 0);
// Retrieve collocation interpolating matrices
std::vector < std::vector <double> > C;
std::vector < double > D;
collocationInterpolators(tau_root, C, D);
// Inputs of constructed function
MX x0 = MX::sym("x0", f.sparsity_in(0));
MX p = MX::sym("p", f.sparsity_in(1));
MX h = MX::sym("h");
// Time step
MX dt = h/N;
// Implicitly defined variables
MX v = MX::sym("v", repmat(x0.sparsity(), order));
std::vector<MX> x = vertsplit(v, x0.size1());
x.insert(x.begin(), x0);
// Collect the equations that implicitly define v
std::vector<MX> V_eq, f_in(2), f_out;
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; ++r) xp_j+= C[j][r]*x[r];
// Collocation equations
f_in[0] = x[j];
f_in[1] = p;
f_out = f(f_in);
V_eq.push_back(dt*f_out.at(0)-xp_j);
}
// Root-finding function
Function rfp("rfp", {v, x0, p, h}, {vertcat(V_eq)});
// Create a implicit function instance to solve the system of equations
Function ifcn = rootfinder("ifcn", solver, rfp, solver_options);
// Get state at end time
MX xf = x0;
for (int k=0; k<N; ++k) {
std::vector<MX> ifcn_out = ifcn({repmat(xf, order), xf, p, h});
x = vertsplit(ifcn_out[0], x0.size1());
// State at end of step
xf = D[0]*x0;
for (int i=1; i<=order; ++i) {
xf += D[i]*x[i-1];
}
}
// Form discrete-time dynamics
return Function("F", {x0, p, h}, {xf}, {"x0", "p", "h"}, {"xf"});
}
void SnoptInterface::init(const Dict& opts) {
// Call the init method of the base class
Nlpsol::init(opts);
// Default: cold start
Cold_ = 0;
// Read user options
for (auto&& op : opts) {
if (op.first=="snopt") {
opts_ = op.second;
} else if (op.first=="start") {
std::string start = op.second.to_string();
if (start=="cold") {
Cold_ = 0;
} else if (start=="warm") {
Cold_ = 1;
} else if (start=="hot") {
Cold_ = 2;
} else {
casadi_error("Unknown start option: " + start);
}
}
}
// Get/generate required functions
jac_f_fcn_ = create_function("nlp_jac_f", {"x", "p"}, {"f", "jac:f:x"});
jac_g_fcn_ = create_function("nlp_jac_g", {"x", "p"}, {"g", "jac:g:x"});
jacg_sp_ = jac_g_fcn_.sparsity_out(1);
// prepare the mapping for constraints
nnJac_ = nx_;
nnObj_ = nx_;
nnCon_ = ng_;
// Here follows the core of the mapping
// Two integer matrices are constructed:
// one with gradF sparsity, and one with jacG sparsity
// the integer values denote the nonzero locations into the original gradF/jacG
// but with a special encoding: entries of gradF are encoded "-1-i" and
// entries of jacG are encoded "1+i"
// "0" is to be interpreted not as an index but as a literal zero
IM mapping_jacG = IM(0, nx_);
IM mapping_gradF = IM(jac_f_fcn_.sparsity_out(1),
range(-1, -1-jac_f_fcn_.nnz_out(1), -1));
if (!jac_g_fcn_.is_null()) {
mapping_jacG = IM(jacg_sp_, range(1, jacg_sp_.nnz()+1));
}
// First, remap jacG
A_structure_ = mapping_jacG;
m_ = ng_;
// Construct the linear objective row
IM d = mapping_gradF(Slice(0), Slice());
std::vector<int> ii = mapping_gradF.sparsity().get_col();
for (int j = 0; j < nnObj_; ++j) {
if (d.colind(j) != d.colind(j+1)) {
int k = d.colind(j);
d.nz(k) = 0;
}
}
// Make it as sparse as you can
d = sparsify(d);
jacF_row_ = d.nnz() != 0;
if (jacF_row_) { // We need an objective gradient row
A_structure_ = vertcat(A_structure_, d);
m_ +=1;
}
iObj_ = jacF_row_ ? (m_ - 1) : -1;
// Is the A matrix completely empty?
dummyrow_ = A_structure_.nnz() == 0; // Then we need a dummy row
if (dummyrow_) {
IM dummyrow = IM(1, nx_);
dummyrow(0, 0) = 0;
A_structure_ = vertcat(A_structure_, dummyrow);
m_+=1;
}
// We don't need a dummy row if a linear objective row is present
casadi_assert(!(dummyrow_ && jacF_row_));
// Allocate temporary memory
alloc_w(nx_, true); // xk2_
alloc_w(ng_, true); // lam_gk_
alloc_w(nx_, true); // lam_xk_
alloc_w(ng_, true); // gk_
alloc_w(jac_f_fcn_.nnz_out(1), true); // jac_fk_
if (!jacg_sp_.is_null()) {
alloc_w(jacg_sp_.nnz(), true); // jac_gk_
}
}
Matrix& LASSO::train(Matrix& X, Matrix& Y, Options& options) {
int p = size(X, 2);
int ny = size(Y, 2);
double epsilon = options.epsilon;
int maxIter = options.maxIter;
double lambda = options.lambda;
bool calc_OV = options.calc_OV;
bool verbose = options.verbose;
/*XNX = [X, -X];
H_G = XNX' * XNX;
D = repmat(diag(H_G), [1, n_y]);
XNXTY = XNX' * Y;
A = (X' * X + lambda * eye(p)) \ (X' * Y);*/
Matrix& XNX = horzcat(2, &X, &uminus(X));
Matrix& H_G = XNX.transpose().mtimes(XNX);
double* Q = new double[size(H_G, 1)];
for (int i = 0; i < size(H_G, 1); i++) {
Q[i] = H_G.getEntry(i, i);
}
Matrix& XNXTY = XNX.transpose().mtimes(Y);
Matrix& A = mldivide(
plus(X.transpose().mtimes(X), times(lambda, eye(p))),
X.transpose().mtimes(Y)
);
/*AA = [subplus(A); subplus(-A)];
C = -XNXTY + lambda;
Grad = C + H_G * AA;
tol = epsilon * norm(Grad);
PGrad = zeros(size(Grad));*/
Matrix& AA = vertcat(2, &subplus(A), &subplus(uminus(A)));
Matrix& C = plus(uminus(XNXTY), lambda);
Matrix& Grad = plus(C, mtimes(H_G, AA));
double tol = epsilon * norm(Grad);
Matrix& PGrad = zeros(size(Grad));
std::list<double> J;
double fval = 0;
// J(1) = sum(sum((Y - X * A).^2)) / 2 + lambda * sum(sum(abs(A)));
if (calc_OV) {
fval = sum(sum(pow(minus(Y, mtimes(X, A)), 2))) / 2 +
lambda * sum(sum(abs(A)));
J.push_back(fval);
}
Matrix& I_k = Grad.copy();
double d = 0;
int k = 0;
DenseVector& SFPlusCi = *new DenseVector(AA.getColumnDimension());
Matrix& S = H_G;
Vector** SRows = null;
if (typeid(H_G) == typeid(DenseMatrix))
SRows = denseMatrix2DenseRowVectors(S);
else
SRows = sparseMatrix2SparseRowVectors(S);
Vector** CRows = null;
if (typeid(C) == typeid(DenseMatrix))
CRows = denseMatrix2DenseRowVectors(C);
else
CRows = sparseMatrix2SparseRowVectors(C);
double** FData = ((DenseMatrix&) AA).getData();
double* FRow = null;
double* pr = null;
int K = 2 * p;
while (true) {
/*I_k = Grad < 0 | AA > 0;
I_k_com = not(I_k);
PGrad(I_k) = Grad(I_k);
PGrad(I_k_com) = 0;*/
_or(I_k, lt(Grad, 0), gt(AA, 0));
Matrix& I_k_com = _not(I_k);
assign(PGrad, Grad);
logicalIndexingAssignment(PGrad, I_k_com, 0);
d = norm(PGrad, inf);
if (d < tol) {
if (verbose)
println("Converge successfully!");
break;
}
/*for i = 1:2*p
AA(i, :) = max(AA(i, :) - (C(i, :) + H_G(i, :) * AA) ./ (D(i, :)), 0);
end
A = AA(1:p,:) - AA(p+1:end,:);*/
for (int i = 0; i < K; i++) {
// SFPlusCi = SRows[i].operate(AA);
operate(SFPlusCi, *SRows[i], AA);
plusAssign(SFPlusCi, *CRows[i]);
timesAssign(SFPlusCi, 1 / Q[i]);
pr = SFPlusCi.getPr();
// F(i, :) = max(F(i, :) - (S(i, :) * F + C(i, :)) / D[i]), 0);
// F(i, :) = max(F(i, :) - SFPlusCi, 0)
FRow = FData[i];
for (int j = 0; j < AA.getColumnDimension(); j++) {
FRow[j] = max(FRow[j] - pr[j], 0);
}
}
// Grad = plus(C, mtimes(H_G, AA));
plus(Grad, C, mtimes(H_G, AA));
k = k + 1;
if (k > maxIter) {
if (verbose)
println("Maximal iterations");
break;
}
if (calc_OV) {
fval = sum(sum(pow(minus(Y, mtimes(XNX, AA)), 2))) / 2 +
lambda * sum(sum(abs(AA)));
J.push_back(fval);
}
if (k % 10 == 0 && verbose) {
if (calc_OV)
fprintf("Iter %d - ||PGrad||: %f, ofv: %f\n", k, d, J.back());
else
fprintf("Iter %d - ||PGrad||: %f\n", k, d);
}
}
Matrix& res = minus(
AA.getSubMatrix(0, p - 1, 0, ny - 1),
AA.getSubMatrix(p, 2 * p - 1, 0, ny - 1)
);
return res;
}
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 DirectSingleShootingInternal::init(){
// Initialize the base classes
OCPSolverInternal::init();
// Create an integrator instance
integratorCreator integrator_creator = getOption("integrator");
integrator_ = integrator_creator(ffcn_,FX());
if(hasSetOption("integrator_options")){
integrator_.setOption(getOption("integrator_options"));
}
// Set t0 and tf
integrator_.setOption("t0",0);
integrator_.setOption("tf",tf_/nk_);
integrator_.init();
// Path constraints present?
bool path_constraints = nh_>0;
// Count the total number of NLP variables
int NV = np_ + // global parameters
nx_ + // initial state
nu_*nk_; // local control
// Declare variable vector for the NLP
// The structure is as follows:
// np x 1 (parameters)
// ------
// nx x 1 (states at time i=0)
// ------
// nu x 1 (controls in interval i=0)
// .....
// nx x 1 (controls in interval i=nk-1)
MX V = msym("V",NV);
int offset = 0;
// Global parameters
MX P = V[Slice(0,np_)];
offset += np_;
// Initial state
MX X0 = V[Slice(offset,offset+nx_)];
offset += nx_;
// Control for each shooting interval
vector<MX> U(nk_);
for(int k=0; k<nk_; ++k){ // interior nodes
U[k] = V[range(offset,offset+nu_)];
offset += nu_;
}
// Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
casadi_assert(offset==NV);
// Current state
MX X = X0;
// Objective
MX nlp_j = 0;
// Constraints
vector<MX> nlp_g;
nlp_g.reserve(nk_*(path_constraints ? 2 : 1));
// For all shooting nodes
for(int k=0; k<nk_; ++k){
// Integrate
vector<MX> int_out = integrator_.call(integratorIn("x0",X,"p",vertcat(P,U[k])));
// Store expression for state trajectory
X = int_out[INTEGRATOR_XF];
// Add constraints on the state
nlp_g.push_back(X);
// Add path constraints
if(path_constraints){
vector<MX> cfcn_out = cfcn_.call(daeIn("x",X,"p",U[k])); // TODO: Change signature of cfcn_: remove algebraic variable, add control
nlp_g.push_back(cfcn_out.at(0));
}
}
// Terminal constraints
G_ = MXFunction(V,vertcat(nlp_g));
G_.setOption("name","nlp_g");
G_.init();
// Objective function
MX jk = mfcn_.call(mayerIn("x",X,"p",P)).at(0);
nlp_j += jk;
F_ = MXFunction(V,nlp_j);
F_.setOption("name","nlp_j");
// 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 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());
}
