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 DirectMultipleShootingInternal::init(){
// Initialize the base classes
OCPSolverInternal::init();
// Create an integrator instance
integratorCreator integrator_creator = getOption("integrator");
integrator_ = integrator_creator(ffcn_,Function());
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_*(nk_+1) + // local 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 (states at time i=1)
// nu x 1 (controls in interval i=1)
// ------
// .....
// ------
// nx x 1 (states at time i=nk)
MX V = MX::sym("V",NV);
// Global parameters
MX P = V(Slice(0,np_));
// offset in the variable vector
int v_offset=np_;
// Disretized variables for each shooting node
vector<MX> X(nk_+1), U(nk_);
for(int k=0; k<=nk_; ++k){ // interior nodes
// Local state
X[k] = V[Slice(v_offset,v_offset+nx_)];
v_offset += nx_;
// Variables below do not appear at the end point
if(k==nk_) break;
// Local control
U[k] = V[Slice(v_offset,v_offset+nu_)];
v_offset += nu_;
}
// Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
casadi_assert(v_offset==NV);
// Input to the parallel integrator evaluation
vector<vector<MX> > int_in(nk_);
for(int k=0; k<nk_; ++k){
int_in[k].resize(INTEGRATOR_NUM_IN);
int_in[k][INTEGRATOR_P] = vertcat(P,U[k]);
int_in[k][INTEGRATOR_X0] = X[k];
}
// Input to the parallel function evaluation
vector<vector<MX> > fcn_in(nk_);
for(int k=0; k<nk_; ++k){
fcn_in[k].resize(DAE_NUM_IN);
fcn_in[k][DAE_T] = (k*tf_)/nk_;
fcn_in[k][DAE_P] = vertcat(P,U.at(k));
fcn_in[k][DAE_X] = X[k];
}
// Options for the parallelizer
Dictionary paropt;
// Transmit parallelization mode
if(hasSetOption("parallelization"))
paropt["parallelization"] = getOption("parallelization");
// Evaluate function in parallel
vector<vector<MX> > pI_out = integrator_.callParallel(int_in,paropt);
// Evaluate path constraints in parallel
vector<vector<MX> > pC_out;
if(path_constraints)
pC_out = cfcn_.callParallel(fcn_in,paropt);
//Constraint function
vector<MX> gg(2*nk_);
// Collect the outputs
for(int k=0; k<nk_; ++k){
//append continuity constraints
gg[2*k] = pI_out[k][INTEGRATOR_XF] - X[k+1];
// append the path constraints
if(path_constraints)
gg[2*k+1] = pC_out[k][0];
}
// Terminal constraints
MX g = vertcat(gg);
// Objective function
MX f;
if (mfcn_.getNumInputs()==1) {
f = mfcn_(X.back()).front();
} else {
vector<MX> mfcn_argin(MAYER_NUM_IN);
mfcn_argin[MAYER_X] = X.back();
mfcn_argin[MAYER_P] = P;
f = mfcn_.call(mfcn_argin).front();
}
// NLP
nlp_ = MXFunction(nlpIn("x",V),nlpOut("f",f,"g",g));
nlp_.setOption("ad_mode","forward");
nlp_.init();
// Get the NLP creator function
NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
// Allocate an NLP solver
nlp_solver_ = nlp_solver_creator(nlp_);
// Pass user 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();
}
