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