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();
}
template<> inline
MX GenericMatrix<MX>::sym(const std::string& name, const CRSSparsity& sp){ return msym(name,sp);}
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();
}