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(); }