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