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