bool isSymbolicSparse(const SXMatrix& ex) {
for(int k=0; k<ex.size(); ++k) // loop over non-zero elements
if(!ex.at(k)->isSymbolic()) // if an element is not symbolic
return false;
return true;
}
void replaceDerivatives(SXMatrix &ex, const SXMatrix &var, const SXMatrix &dvar){
// Initialize with an empty expression
SXFunction fcn(ex);
// Map from the var-node to the new der-node
std::map<int, SX> dermap;
for(int i=0; i<var.size(); ++i)
dermap[fcn.treemap[var[i].get()]] = dvar[i];
// Replacement map
std::map<int, SX> replace;
// Go through all nodes and check if any node is a derivative
for(int i=0; i<fcn.algorithm.size(); ++i){
if(fcn.algorithm[i].op == DER){
// find the corresponding derivative
std::map<int, SX>::iterator r = dermap.find(fcn.algorithm[i].ch0);
casadi_assert(r != dermap.end());
replace[i] = r->second;
}
}
SXMatrix res;
SXMatrix repres;
fcn.eval_symbolic(SXMatrix(),res,replace,repres);
ex = res;
casadi_assert(0);
}
bool dependsOn(const SXMatrix& ex, const SXMatrix &arg){
if(ex.size()==0) return false;
SXFunction temp(arg,ex);
temp.init();
CRSSparsity Jsp = temp.jacSparsity();
return Jsp.size()!=0;
}
void makeSmooth(SXMatrix &ex, SXMatrix &bvar, SXMatrix &bexpr){
// Initialize
SXFunction fcn(SXMatrix(),ex);
casadi_assert(bexpr.empty());
// Nodes to be replaced
std::map<int,SX> replace;
// Go through all nodes and check if any node is non-smooth
for(int i=0; i<fcn->algorithm.size(); ++i){
// Check if we have a step node
if(fcn->algorithm[i].op == STEP){
// Get the index of the child
int ch0 = fcn->algorithm[i].ch[0];
// Binary variable corresponding to the the switch
SXMatrix sw;
#if 0
// Find out if the switch has already been added
for(int j=0; j<bexpr.size(); ++j)
if(bexpr[j].isEqual(algorithm[i]->child0)){
sw = bvar[j];
break;
}
#endif
if(sw.empty()){ // the switch has not yet been added
// Get an approriate name of the switch
std::stringstream name;
name << "sw_" << bvar.size1();
sw = SX(name.str());
// Add to list of switches
bvar << sw;
// bexpr << algorithm[i]->child0;
}
// Add to the substition map
replace[i] = sw[0];
}
}
SXMatrix res;
fcn->eval(SXMatrix(),res,replace,bexpr);
for(int i=0; i<bexpr.size(); ++i)
bexpr[i] = bexpr[i]->dep(0);
ex = res;
#if 0
// Make sure that the binding expression is smooth
bexpr.init(SXMatrix());
SXMatrix b;
bexpr.eval_symbolic(SXMatrix(),b,replace,bexpr);
bexpr = b;
#endif
}
void simplify(SXMatrix &ex){
// simplify all non-zero elements
for(int el=0; el<ex.size(); ++el)
simplify(ex.at(el));
}
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;
}
