/******************** accessors *******************/
void sxMatrixAt(const SXMatrix & mat, int n, int m, SX & out) {
out = SX(mat.indexed(n,m));
}
int sxMatrixSize2(const SXMatrix & mat) {
return mat.size2();
}
void sxMatrixTranspose(const SXMatrix & mIn, SXMatrix & mOut) {
mOut = SXMatrix(mIn.trans());
}
int main(){
cout << "program started" << endl;
// Formulate the OCP
SymbolicOCP ocp;
model(ocp);
// Create an integrator
Sundials::CVodesIntegrator integrator(ocp.ffcn);
// integrator.init();
/* // Pass initial values
double t0 = 0;
double tf = 0.00001;
integrator.setInput(t0, IVP_T0);
integrator.setInput(tf,IVP_TF);
integrator.setInput(ocp.x_init,IVP_X_INIT);
integrator.setInput(ocp.u_init,IVP_P);
cout << ocp.u_init << endl;
cout << ocp.u << endl;
double seed = 1;
integrator.setInput(seed,IVP_P,1);
integrator.evaluateFwd();
vector<double> r(ocp.x_init.size());
integrator.getOutput(r,0,1);
cout << "der(r) = " << r << endl;
integrator.getOutput(r);
cout << "r = " << r << endl;
return 0;*/
// Create a grid
int ngrid = 10;
vector<double> times(ngrid);
linspace(times,0.0,ocp.tf);
// Create a simulator
Simulator simulator(integrator,times);
simulator.init();
// Initial condition
simulator.setInput(ocp.x_init,INTEGRATOR_X0);
// Pass parameters
simulator.setInput(ocp.u_init,INTEGRATOR_P);
// Simulate
simulator.evaluate();
// Print the output to screen
vector<double> T_sim(ngrid*ocp.x.numel());
simulator.getOutput(T_sim);
cout << "T_sim = " << T_sim << endl;
// Degree of interpolating polynomial
int K = 3;
// Number of finite elements
int N = 15;
// Radau collocation points
CollocationPoints cp = LEGENDRE;
// Size of the finite elements
double h = ocp.tf/N;
// Coefficients of the collocation equation
vector<vector<double> > C;
// Coefficients of the continuity equation
vector<double> D;
// Get the coefficients
get_collocation_coeff(K,C,D,cp);
// Collocated times
vector<vector<double> > T;
T.resize(N);
for(int i=0; i<N; ++i){
T[i].resize(K+1);
for(int j=0; j<=K; ++j){
T[i][j] = h*(i + collocation_points[cp][K][j]);
}
}
// Collocated states
vector< vector< SXMatrix > > X;
collocate(ocp.x,X,N,K);
// Collocated control (piecewice constant)
vector< SXMatrix > U;
collocate(ocp.u,U,N);
// State at end time
SXMatrix XF;
//
bool periodic = true;
if(periodic)
XF = X[0][0];
else
collocate_final(ocp.x,XF);
// All variables with bounds and initial guess
vector<SX> vars;
vector<double> vars_lb;
vector<double> vars_ub;
vector<double> vars_sol;
// Loop over the finite elements
for(int i=0; i<N; ++i){
// collocated controls
for(int r=0; r<ocp.u.numel(); ++r){
// Add to list of NLP variables
vars.push_back(SX(U[i][r]));
vars_lb.push_back(ocp.u_lb[r]);
vars_ub.push_back(ocp.u_ub[r]);
vars_sol.push_back(ocp.u_init[r]);
}
// Collocated states
for(int j=0; j<=K; ++j){
for(int r=0; r<ocp.x.numel(); ++r){
// Add to list of NLP variables
vars.push_back(SX(X[i][j][r]));
vars_sol.push_back(ocp.x_init[r]);
if(i==0 && j==0 && !periodic){
// Initial constraints
vars_lb.push_back(ocp.x_init[r]);
vars_ub.push_back(ocp.x_init[r]);
} else {
// Variable bounds
vars_lb.push_back(ocp.x_lb[r]);
vars_ub.push_back(ocp.x_ub[r]);
}
}
}
}
if(!periodic){
// Add states at end time
for(int r=0; r<ocp.x.numel(); ++r){
vars.push_back(SX(XF[r]));
vars_sol.push_back(ocp.x_init[r]);
vars_lb.push_back(ocp.x_lb[r]);
vars_ub.push_back(ocp.x_ub[r]);
}
}
// Constraint function for the NLP
SXMatrix g;
vector<double> lbg,ubg;
for(int i=0; i<N; ++i){
for(int k=1; k<=K; ++k){
// augmented state vector
vector<SXMatrix> y_ik(3);
y_ik[0] = T[i][k];
y_ik[1] = X[i][k];
// y_ik[2] = U[i];
// Add collocation equations to NLP
SXMatrix rhs(ocp.x.numel(),1);
for(int j=0; j<=K; ++j)
rhs += X[i][j]*C[j][k];
g << h*ocp.ffcn.eval(y_ik)[0] - rhs;
lbg.insert(lbg.end(),ocp.x.numel(),0); // equality constraints
ubg.insert(ubg.end(),ocp.x.numel(),0); // equality constraints
// Add nonlinear constraints
/* g << ocp.cfcn(y_ik);
lbg.insert(lbg.end(),1,0);
ubg.insert(ubg.end(),1,numeric_limits<double>::infinity());*/
}
// Add continuity equation to NLP
SXMatrix rhs(ocp.x.numel(),1);
for(int j=0; j<=K; ++j)
rhs += D[j]*X[i][j];
if(i<N-1)
g << X[i+1][0] - rhs;
else
g << XF - rhs;
lbg.insert(lbg.end(),ocp.x.numel(),0); // equality constraints
ubg.insert(ubg.end(),ocp.x.numel(),0); // equality constraints
}
SXMatrix v = vars;
SXFunction gfcn_nlp(v,g);
// Objective function of the NLP
vector<SXMatrix> y_f(3);
y_f[0] = T.back().back();
y_f[1] = XF;
/* y_f[2] = U.back();*/
SXMatrix f = ocp.mfcn.eval(y_f)[0];
SXFunction ffcn_nlp(v, f);
#if 0
// Hessian of the Lagrangian:
// Lagrange multipliers
SXMatrix lambda("lambda",g.size());
// Objective function scaling
SXMatrix sigma("sigma");
// Lagrangian function
vector<SXMatrix> lfcn_input(3);
lfcn_input[0] = v;
lfcn_input[1] = lambda;
lfcn_input[2] = sigma;
SXFunction lfcn(lfcn_input, sigma*f + trans(lambda)*g);
// Hessian of the Lagrangian
SXFunction HL = lfcn.hessian();
// ----
// SOLVE THE NLP
// ----
// Allocate an NLP solver
IpoptSolver solver(ffcn_nlp,gfcn_nlp, HL);
#else
// Allocate an NLP solver
IpoptSolver solver(ffcn_nlp,gfcn_nlp);
#endif
// Set options
solver.setOption("abstol",1e-6);
// initialize the solver
solver.init();
// Bounds on x and initial guess
solver.setInput(vars_lb,NLP_LBX);
solver.setInput(vars_ub,NLP_UBX);
solver.setInput(vars_sol,NLP_X_INIT);
// Bounds on the constraints
solver.setInput(lbg,NLP_LBG);
solver.setInput(ubg,NLP_UBG);
// Solve the problem
solver.solve();
// Print the optimal cost
double cost;
solver.getOutput(cost,NLP_COST);
cout << "optimal cost: " << cost << endl;
// ----
// SAVE SOLUTION TO DISK
// ----
vector<double> t_opt(N*(K+1));
//int ind = 0; // index of nlp->x
for(int i=0; i<N; ++i){
for(int j=0; j<=K; ++j){
int ij = (K+1)*i+j;
t_opt[ij] = T[i][j];
}
}
std::ofstream resfile;
resfile.open ("results_convection_diffusion.txt");
// Get the solution
solver.getOutput(vars_sol,NLP_X_OPT);
resfile << "T_opt " << vars_sol << endl;
// Save optimal solution to disk
resfile << "t_opt " << t_opt << endl;
resfile.close();
return 0;
}
