Integrator createIntegrator(SXFunction dae, double steplength) {
Dict opts;
Integrator integrator;
opts = make_dict("tf", steplength, "abstol", 1e-8, "reltol", 1e-8,
"steps_per_checkpoint", 1000, "max_num_steps", 1000);
integrator = Integrator("integrator", "idas", dae, opts);
integrator.setOption("linear_solver", "csparse");
// integrator.setOption("regularity_check", false);
std::cout << "Integrator created" << std::endl;
return integrator;
}
int main(){
// For all problems
enum Problems{ODE,DAE,NUM_PROBLEMS};
for(int problem=0; problem<NUM_PROBLEMS; ++problem){
// Get problem
FX ffcn; // Callback function
vector<double> x0; // Initial value
double u0; // Parameter value
double tf; // End time
switch(problem){
case ODE:
cout << endl << "** Testing ODE example **" << endl;
simpleODE(ffcn,tf,x0,u0);
break;
case DAE:
cout << endl << "** Testing DAE example **" << endl;
simpleDAE(ffcn,tf,x0,u0);
break;
}
// For all integrators
enum Integrators{CVODES,IDAS,COLLOCATION,NUM_INTEGRATORS};
for(int integrator=0; integrator<NUM_INTEGRATORS; ++integrator){
// Get integrator
Integrator I;
switch(integrator){
case CVODES:
if(problem==DAE) continue; // Skip if DAE
cout << endl << "== CVodesIntegrator == " << endl;
I = CVodesIntegrator(ffcn);
break;
case IDAS:
cout << endl << "== IdasIntegrator == " << endl;
I = IdasIntegrator(ffcn);
break;
case COLLOCATION:
cout << endl << "== CollocationIntegrator == " << endl;
I = CollocationIntegrator(ffcn);
// Set collocation integrator specific options
I.setOption("expand_f",true);
I.setOption("implicit_solver",KinsolSolver::creator);
Dictionary kinsol_options;
kinsol_options["linear_solver"] = CSparse::creator;
I.setOption("implicit_solver_options",kinsol_options);
break;
}
// Set common options
I.setOption("tf",tf);
// Initialize the integrator
I.init();
// Integrate to get results
I.setInput(x0,INTEGRATOR_X0);
I.setInput(u0,INTEGRATOR_P);
I.evaluate();
DMatrix xf = I.output(INTEGRATOR_XF);
DMatrix qf = I.output(INTEGRATOR_QF);
cout << setw(50) << "Unperturbed solution: " << "xf = " << xf << ", qf = " << qf << endl;
// Perturb solution to get a finite difference approximation
double h = 0.001;
I.setInput(u0+h,INTEGRATOR_P);
I.evaluate();
DMatrix xf_pert = I.output(INTEGRATOR_XF);
DMatrix qf_pert = I.output(INTEGRATOR_QF);
cout << setw(50) << "Finite difference approximation: " << "d(xf)/d(p) = " << (xf_pert-xf)/h << ", d(qf)/d(p) = " << (qf_pert-qf)/h << endl;
// Operator overloading approach
I.setInput(x0,INTEGRATOR_X0);
I.setInput(u0,INTEGRATOR_P);
I.setFwdSeed(0.,INTEGRATOR_X0);
I.setFwdSeed(1.,INTEGRATOR_P);
I.reset(1,0,0);
I.integrate(tf);
DMatrix oo_xf = I.fwdSens(INTEGRATOR_XF);
DMatrix oo_qf = I.fwdSens(INTEGRATOR_QF);
cout << setw(50) << "Forward sensitivities via OO: " << "d(xf)/d(p) = " << oo_xf << ", d(qf)/d(p) = " << oo_qf << endl;
// Calculate once, forward
FX I_fwd = I.derivative(1,0);
I_fwd.setInput(x0,INTEGRATOR_X0);
I_fwd.setInput(u0,INTEGRATOR_P);
I_fwd.setInput(0.,INTEGRATOR_NUM_IN+INTEGRATOR_X0);
I_fwd.setInput(1.,INTEGRATOR_NUM_IN+INTEGRATOR_P);
I_fwd.evaluate();
DMatrix fwd_xf = I_fwd.output(INTEGRATOR_NUM_OUT+INTEGRATOR_XF);
DMatrix fwd_qf = I_fwd.output(INTEGRATOR_NUM_OUT+INTEGRATOR_QF);
cout << setw(50) << "Forward sensitivities: " << "d(xf)/d(p) = " << fwd_xf << ", d(qf)/d(p) = " << fwd_qf << endl;
// Calculate once, adjoint
FX I_adj = I.derivative(0,1);
I_adj.setInput(x0,INTEGRATOR_X0);
I_adj.setInput(u0,INTEGRATOR_P);
I_adj.setInput(0.,INTEGRATOR_NUM_IN+INTEGRATOR_XF);
I_adj.setInput(1.,INTEGRATOR_NUM_IN+INTEGRATOR_QF);
I_adj.evaluate();
DMatrix adj_x0 = I_adj.output(INTEGRATOR_NUM_OUT+INTEGRATOR_X0);
DMatrix adj_p = I_adj.output(INTEGRATOR_NUM_OUT+INTEGRATOR_P);
cout << setw(50) << "Adjoint sensitivities: " << "d(qf)/d(x0) = " << adj_x0 << ", d(qf)/d(p) = " << adj_p << endl;
// Perturb adjoint solution to get a finite difference approximation of the second order sensitivities
I_adj.setInput(x0,INTEGRATOR_X0);
I_adj.setInput(u0+h,INTEGRATOR_P);
I_adj.setInput(0.,INTEGRATOR_NUM_IN+INTEGRATOR_XF);
I_adj.setInput(1.,INTEGRATOR_NUM_IN+INTEGRATOR_QF);
I_adj.evaluate();
DMatrix adj_x0_pert = I_adj.output(INTEGRATOR_NUM_OUT+INTEGRATOR_X0);
DMatrix adj_p_pert = I_adj.output(INTEGRATOR_NUM_OUT+INTEGRATOR_P);
cout << setw(50) << "FD of adjoint sensitivities: " << "d2(qf)/d(x0)d(p) = " << (adj_x0_pert-adj_x0)/h << ", d2(qf)/d(p)d(p) = " << (adj_p_pert-adj_p)/h << endl;
// Forward over adjoint to get the second order sensitivities
I_adj.setInput(x0,INTEGRATOR_X0);
I_adj.setInput(u0,INTEGRATOR_P);
I_adj.setFwdSeed(1.,INTEGRATOR_P);
I_adj.setInput(0.,INTEGRATOR_NUM_IN+INTEGRATOR_XF);
I_adj.setInput(1.,INTEGRATOR_NUM_IN+INTEGRATOR_QF);
I_adj.evaluate(1,0);
DMatrix fwd_adj_x0 = I_adj.fwdSens(INTEGRATOR_NUM_OUT+INTEGRATOR_X0);
DMatrix fwd_adj_p = I_adj.fwdSens(INTEGRATOR_NUM_OUT+INTEGRATOR_P);
cout << setw(50) << "Forward over adjoint sensitivities: " << "d2(qf)/d(x0)d(p) = " << fwd_adj_x0 << ", d2(qf)/d(p)d(p) = " << fwd_adj_p << endl;
// Adjoint over adjoint to get the second order sensitivities
I_adj.setInput(x0,INTEGRATOR_X0);
I_adj.setInput(u0,INTEGRATOR_P);
I_adj.setInput(0.,INTEGRATOR_NUM_IN+INTEGRATOR_XF);
I_adj.setInput(1.,INTEGRATOR_NUM_IN+INTEGRATOR_QF);
I_adj.setAdjSeed(1.,INTEGRATOR_NUM_OUT+INTEGRATOR_P);
I_adj.evaluate(0,1);
DMatrix adj_adj_x0 = I_adj.adjSens(INTEGRATOR_X0);
DMatrix adj_adj_p = I_adj.adjSens(INTEGRATOR_P);
cout << setw(50) << "Adjoint over adjoint sensitivities: " << "d2(qf)/d(x0)d(p) = " << adj_adj_x0 << ", d2(qf)/d(p)d(p) = " << adj_adj_p << endl;
}
}
return 0;
}
int main(){
// Time horizon
double t0 = 0, tf = 10;
// Bounds on the control
double /*u_lb = -0.5, u_ub = 1.3,*/ u_init = 1;
// Initial conditions
vector<double> x0(3);
x0[0] = 0;
x0[1] = 0;
x0[2] = 1;
// Integrator
Integrator integrator = create_Sundials();
// Attach user-defined linear solver
if(user_defined_solver){
if(sparse_direct){
integrator.setOption("linear_solver_creator",CSparse::creator);
// integrator.setOption("linear_solver_creator",SuperLU::creator);
} else {
integrator.setOption("linear_solver_creator",LapackLUDense::creator);
integrator.setOption("linear_solver","user_defined");
}
// integrator.setOption("linear_solver","user_defined"); // FIXME: bug for second order
}
// Set common integrator options
integrator.setOption("fsens_err_con",true);
integrator.setOption("quad_err_con",true);
integrator.setOption("abstol",1e-12);
integrator.setOption("reltol",1e-12);
integrator.setOption("fsens_abstol",1e-6);
integrator.setOption("fsens_reltol",1e-6);
integrator.setOption("asens_abstol",1e-6);
integrator.setOption("asens_reltol",1e-6);
integrator.setOption("exact_jacobian",exact_jacobian);
integrator.setOption("finite_difference_fsens",finite_difference_fsens);
integrator.setOption("max_num_steps",100000);
// integrator.setOption("max_multistep_order",4);
integrator.setOption("t0",t0);
integrator.setOption("tf",tf);
// Initialize the integrator
integrator.init();
// Set parameters
integrator.setInput(u_init,INTEGRATOR_P);
// Set inital state
integrator.setInput(x0,INTEGRATOR_X0);
// Integrate
integrator.evaluate();
// Save the result
Matrix<double> res0_xf = integrator.output(INTEGRATOR_XF);
Matrix<double> res0_qf = integrator.output(INTEGRATOR_QF);
// Perturb in some direction
if(perturb_u){
double u_pert = u_init + 0.01;
integrator.setInput(u_pert,INTEGRATOR_P);
} else {
vector<double> x_pert = x0;
x_pert[1] += 0.01;
integrator.setInput(x_pert,INTEGRATOR_X0);
}
// Integrate again
integrator.evaluate();
// Print statistics
integrator.printStats();
// Calculate finite difference approximation
Matrix<double> fd_xf = (integrator.output(INTEGRATOR_XF) - res0_xf)/0.01;
Matrix<double> fd_qf = (integrator.output(INTEGRATOR_QF) - res0_qf)/0.01;
cout << "unperturbed " << res0_xf << "; " << res0_qf << endl;
cout << "perturbed " << integrator.output(INTEGRATOR_XF) << "; " << integrator.output(INTEGRATOR_QF) << endl;
cout << "finite_difference approximation " << fd_xf << "; " << fd_qf << endl;
if(perturb_u){
integrator.setFwdSeed(1.0,INTEGRATOR_P);
} else {
vector<double> x0_seed(x0.size(),0);
x0_seed[1] = 1;
integrator.setFwdSeed(x0_seed,INTEGRATOR_X0);
}
// Reset parameters
integrator.setInput(u_init,INTEGRATOR_P);
// Reset initial state
integrator.setInput(x0,INTEGRATOR_X0);
if(with_asens){
// backward seeds
vector<double> &bseed = integrator.adjSeed(INTEGRATOR_XF).data();
fill(bseed.begin(),bseed.end(),0);
bseed[1] = 1;
// evaluate with forward and adjoint sensitivities
integrator.evaluate(1,1);
} else {
// evaluate with only forward sensitivities
integrator.evaluate(1,0);
}
Matrix<double> fsens_xf = integrator.fwdSens(INTEGRATOR_XF);
Matrix<double> fsens_qf = integrator.fwdSens(INTEGRATOR_QF);
cout << "forward sensitivities " << fsens_xf << "; " << fsens_qf << endl;
if(with_asens){
cout << "adjoint sensitivities ";
cout << integrator.adjSens(INTEGRATOR_X0) << "; ";
cout << integrator.adjSens(INTEGRATOR_P) << "; ";
cout << endl;
}
if(second_order){
// Preturb the forward seeds
if(perturb_u){
double u_seed = 1.001;
integrator.setFwdSeed(u_seed,INTEGRATOR_P);
} else {
vector<double> x0_seed(x0.size(),0);
x0_seed[1] = 1.001;
integrator.setFwdSeed(x0_seed,INTEGRATOR_X0);
}
// evaluate again with forward sensitivities
integrator.evaluate(1,0);
vector<double> fsens_pret_xf = integrator.fwdSens(INTEGRATOR_XF).data();
vector<double> fsens_pret_qf = integrator.fwdSens(INTEGRATOR_QF).data();
cout << "forward sensitivities preturbed " << fsens_pret_xf << "; " << fsens_pret_qf << endl;
vector<double> fd2_xf(fsens_xf.size());
vector<double> fd2_qf(fsens_qf.size());
for(int i=0; i<fd2_xf.size(); ++i)
fd2_xf[i] = (fsens_pret_xf.at(i)-fsens_xf.at(i))/0.001;
for(int i=0; i<fd2_qf.size(); ++i)
fd2_qf[i] = (fsens_pret_qf.at(i)-fsens_qf.at(i))/0.001;
cout << "finite differences, 2nd order " << fd2_xf << "; " << fd2_qf << endl;
// Generate the jacobian by creating a new integrator for the sensitivity equations by source transformation
FX intjac = integrator.jacobian(INTEGRATOR_P,INTEGRATOR_XF);
// Set options
intjac.setOption("number_of_fwd_dir",0);
intjac.setOption("number_of_adj_dir",1);
// Initialize the integrator
intjac.init();
// Set inputs
intjac.setInput(u_init,INTEGRATOR_P);
intjac.setInput(x0,INTEGRATOR_X0);
// Set adjoint seed
vector<double> jacseed(3*1,0);
jacseed[0] = 1;
intjac.setAdjSeed(jacseed);
// Evaluate the Jacobian
intjac.evaluate(0,0);
cout << "jacobian " << intjac.output(0) << endl;
// Get the results
/* cout << "unperturbed via jacobian " << intjac.output(1+INTEGRATOR_XF) << endl;*/
cout << "second order (fwd-over-adj) " ;
cout << intjac.adjSens(INTEGRATOR_X0) << ", ";
cout << intjac.adjSens(INTEGRATOR_P) << endl;
// Save the unpreturbed value
Matrix<double> unpret = intjac.output();
// Perturb X0
intjac.setInput(u_init+0.01,INTEGRATOR_P);
intjac.evaluate();
Matrix<double> pret = intjac.output();
cout << "unperturbed fwd sens " << unpret << endl;
cout << "perturbed fwd sens " << pret << endl;
cout << "finite diff. (augmented dae) " << (pret-unpret)/0.01 << endl;
}
return 0;
}
int main(){
// Time length
double T = 10.0;
// Shooting length
int nu = 20; // Number of control segments
// Time horizon for integrator
double t0 = 0;
double tf = T/nu;
// Initial position
vector<double> X0(3);
X0[0] = 0; // initial position
X0[1] = 0; // initial speed
X0[2] = 1; // initial mass
// Time
SX t = SX::sym("t");
// Differential states
SX s = SX::sym("s"), v = SX::sym("v"), m = SX::sym("m");
SX x = SX::zeros(3);
x[0] = s;
x[1] = v;
x[2] = m;
// Control
SX u = SX::sym("u");
SX alpha = 0.05; // friction
SX beta = 0.1; // fuel consumption rate
// Differential equation
SX rhs = SX::zeros(3);
rhs[0] = v;
rhs[1] = (u-alpha*v*v)/m;
rhs[2] = -beta*u*u;
// Initial conditions
vector<double> x0(3);
x0[0] = 0;
x0[1] = 0;
x0[2] = 1;
// DAE residual function
SXFunction daefcn(daeIn("x",x, "p",u, "t",t),daeOut("ode",rhs));
daefcn.setOption("name","DAE residual");
// Integrator
Integrator integrator;
if(sundials_integrator){
if(explicit_integrator){
// Explicit integrator (CVODES)
integrator = CVodesIntegrator(daefcn);
// integrator.setOption("exact_jacobian",true);
// integrator.setOption("linear_multistep_method","bdf"); // adams or bdf
// integrator.setOption("nonlinear_solver_iteration","newton"); // newton or functional
} else {
// Implicit integrator (IDAS)
integrator = IdasIntegrator(daefcn);
integrator.setOption("calc_ic",false);
}
integrator.setOption("fsens_err_con",true);
integrator.setOption("quad_err_con",true);
integrator.setOption("abstol",1e-6);
integrator.setOption("reltol",1e-6);
integrator.setOption("stop_at_end",false);
// integrator.setOption("fsens_all_at_once",false);
integrator.setOption("steps_per_checkpoint",100); // BUG: Too low number causes segfaults
} else {
// An explicit Euler integrator
integrator = RKIntegrator(daefcn);
integrator.setOption("expand_f",true);
integrator.setOption("interpolation_order",1);
integrator.setOption("number_of_finite_elements",1000);
}
integrator.setOption("t0",t0);
integrator.setOption("tf",tf);
integrator.init();
// control for all segments
MX U = MX::sym("U",nu);
// Integrate over all intervals
MX X=X0;
for(int k=0; k<nu; ++k){
// Assemble the input
vector<MX> input(INTEGRATOR_NUM_IN);
input[INTEGRATOR_X0] = X;
input[INTEGRATOR_P] = U[k];
// Integrate
X = integrator.call(input).at(0);
// Lift X
if(lifted_newton){
X.lift(X);
}
}
// Objective function
MX F = inner_prod(U,U);
// Terminal constraints
MX G = vertcat(X[0],X[1]);
// Create the NLP
MXFunction nlp(nlpIn("x",U),nlpOut("f",F,"g",G));
// Allocate an NLP solver
NLPSolver solver;
if(lifted_newton){
solver = SCPgen(nlp);
solver.setOption("verbose",true);
solver.setOption("regularize",false);
solver.setOption("max_iter_ls",1);
solver.setOption("max_iter",100);
// Use IPOPT as QP solver
solver.setOption("qp_solver",NLPQPSolver::creator);
Dictionary qp_solver_options;
qp_solver_options["nlp_solver"] = IpoptSolver::creator;
Dictionary ipopt_options;
ipopt_options["tol"] = 1e-12;
ipopt_options["print_level"] = 0;
ipopt_options["print_time"] = false;
qp_solver_options["nlp_solver_options"] = ipopt_options;
solver.setOption("qp_solver_options",qp_solver_options);
} else {
solver = IpoptSolver(nlp);
// Set options
solver.setOption("tol",1e-10);
solver.setOption("hessian_approximation","limited-memory");
}
// initialize the solver
solver.init();
// Bounds on u and initial condition
vector<double> Umin(nu), Umax(nu), Usol(nu);
for(int i=0; i<nu; ++i){
Umin[i] = -10;
Umax[i] = 10;
Usol[i] = 0.4;
}
solver.setInput(Umin,"lbx");
solver.setInput(Umax,"ubx");
solver.setInput(Usol,"x0");
// Bounds on g
vector<double> Gmin(2), Gmax(2);
Gmin[0] = Gmax[0] = 10;
Gmin[1] = Gmax[1] = 0;
solver.setInput(Gmin,"lbg");
solver.setInput(Gmax,"ubg");
// Solve the problem
solver.solve();
// Get the solution
solver.getOutput(Usol,"x");
cout << "optimal solution: " << Usol << endl;
return 0;
}
