Function explicitRK(Function& f, const MX& tf, int order, 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_ALG).isEmpty() && f.input(DAE_Z).isEmpty(),
"Supplied function cannot have algebraic states.");
casadi_assert_message(f.output(DAE_QUAD).isEmpty(),
"Supplied function cannot have quadrature states.");
MX X = MX::sym("X", f.input(DAE_X).sparsity());
MX P = MX::sym("P", f.input(DAE_P).sparsity());
MX X0 = X;
MX t = 0;
MX dt = tf/ne;
std::vector<double> b(order);
b[0]=1.0/6;b[1]=1.0/3;b[2]=1.0/3;b[3]=1.0/6;
std::vector<double> c(order);
c[0]=0;c[1]=1.0/2;c[2]=1.0/2;c[3]=1;
std::vector< std::vector<double> > A(order-1);
A[0].resize(1);A[0][0]=1.0/2;
A[1].resize(2);A[1][0]=0;A[1][1]=1.0/2;
A[2].resize(3);A[2][0]=0;A[2][1]=0;A[2][2]=1;
std::vector<MX> k(order);
for (int i=0;i<ne;++i) {
for (int j=0;j<order;++j) {
MX XL = 0;
for (int jj=0;jj<j;++jj) {
XL+=k.at(jj)*A.at(j-1).at(jj);
}
//std::cout << "help: " << A.at(j-1) << ", " << c.at(j) << std::endl;
k[j] = dt*f.call(daeIn("x", X+XL, "p", P, "t", t+dt*c.at(j)))[DAE_ODE];
}
for (int j=0;j<order;++j) {
X += b.at(j)*k.at(j);
}
t+= dt;
}
MXFunction ret(integratorIn("x0", X0, "p", P), integratorOut("xf", X));
return ret;
}
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 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();
}
