void Vertsplit::evalFwd(const std::vector<cpv_MX>& fwdSeed, const std::vector<pv_MX>& fwdSens) {
int nfwd = fwdSens.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<nfwd; ++d) {
const cpv_MX& arg = fwdSeed[d];
const pv_MX& res = fwdSens[d];
const MX& x = *arg[0];
vector<MX> y = vertsplit(x, row_offset);
for (int i=0; i<nx; ++i) {
if (res[i]!=0) {
*res[i] = y[i];
}
}
}
}
void Vertsplit::eval_mx(const std::vector<MX>& arg, std::vector<MX>& res) {
// 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());
}
res = vertsplit(arg[0], row_offset);
}
Vertsplit::Vertsplit(const MX& x, const std::vector<int>& offset) : Split(x, offset) {
// Split up the sparsity pattern
output_sparsity_ = vertsplit(x.sparsity(), offset_);
// Have offset_ refer to the nonzero offsets instead of column offsets
offset_.resize(1);
for (std::vector<Sparsity>::const_iterator it=output_sparsity_.begin();
it!=output_sparsity_.end();
++it) {
offset_.push_back(offset_.back() + it->nnz());
}
}
void Vertsplit::evalFwd(const std::vector<std::vector<MX> >& fseed,
std::vector<std::vector<MX> >& fsens) {
int nfwd = fsens.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<nfwd; ++d) {
fsens[d] = vertsplit(fseed[d][0], row_offset);
}
}
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"});
}
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 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();
}
}
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;
}
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"});
}
