void EvaluationMX::create(const FX& fcn, const std::vector<MX> &arg,
std::vector<MX> &res, const std::vector<std::vector<MX> > &fseed,
std::vector<std::vector<MX> > &fsens,
const std::vector<std::vector<MX> > &aseed,
std::vector<std::vector<MX> > &asens, bool output_given) {
// Number inputs and outputs
int num_in = fcn.getNumInputs();
int num_out = fcn.getNumOutputs();
// Number of directional derivatives
int nfdir = fseed.size();
int nadir = aseed.size();
// Create the evaluation node
MX ev;
if(nfdir>0 || nadir>0){
// Create derivative function
Derivative dfcn(fcn,nfdir,nadir);
stringstream ss;
ss << "der_" << fcn.getOption("name") << "_" << nfdir << "_" << nadir;
dfcn.setOption("verbose",fcn.getOption("verbose"));
dfcn.setOption("name",ss.str());
dfcn.init();
// All inputs
vector<MX> darg;
darg.reserve(num_in*(1+nfdir) + num_out*nadir);
darg.insert(darg.end(),arg.begin(),arg.end());
// Forward seeds
for(int dir=0; dir<nfdir; ++dir){
darg.insert(darg.end(),fseed[dir].begin(),fseed[dir].end());
}
// Adjoint seeds
for(int dir=0; dir<nadir; ++dir){
darg.insert(darg.end(),aseed[dir].begin(),aseed[dir].end());
}
ev.assignNode(new EvaluationMX(dfcn, darg));
} else {
ev.assignNode(new EvaluationMX(fcn, arg));
}
// Output index
int ind = 0;
// Create the output nodes corresponding to the nondifferented function
res.resize(num_out);
for (int i = 0; i < num_out; ++i, ++ind) {
if(!output_given){
if(!fcn.output(i).empty()){
res[i].assignNode(new OutputNode(ev, ind));
} else {
res[i] = MX();
}
}
}
// Forward sensitivities
fsens.resize(nfdir);
for(int dir = 0; dir < nfdir; ++dir){
fsens[dir].resize(num_out);
for (int i = 0; i < num_out; ++i, ++ind) {
if (!fcn.output(i).empty()){
fsens[dir][i].assignNode(new OutputNode(ev, ind));
} else {
fsens[dir][i] = MX();
}
}
}
// Adjoint sensitivities
asens.resize(nadir);
for (int dir = 0; dir < nadir; ++dir) {
asens[dir].resize(num_in);
for (int i = 0; i < num_in; ++i, ++ind) {
if (!fcn.input(i).empty()) {
asens[dir][i].assignNode(new OutputNode(ev, ind));
} else {
asens[dir][i] = MX();
}
}
}
}
// Create an IDAS instance (fully implicit integrator)
Integrator create_Sundials(){
// Time
SX t("t");
// Differential states
SX s("s"), v("v"), m("m");
vector<SX> x(3);
x[0] = s;
x[1] = v;
x[2] = m;
// State derivatives
SX sdot("sdot"), vdot("vdot"), mdot("mdot");
vector<SX> xdot(3);
xdot[0] = sdot;
xdot[1] = vdot;
xdot[2] = mdot;
// Control
SX u("u");
// Reference trajectory
SX u_ref = 3-sin(t);
// Square deviation from the state trajectory
SX u_dev = u-u_ref;
u_dev *= u_dev;
// Differential equation (fully implicit form)
vector<SX> res(3);
res[0] = v - sdot;
res[1] = (u-0.02*v*v)/m - vdot;
res[2] = -0.01*u*u - mdot;
// Input/output of the DAE residual function
vector<SXMatrix> ffcn_in = daeIn<SXMatrix>("x",x, "p",u, "t",t, "xdot",xdot);
vector<SXMatrix> ffcn_out = daeOut<SXMatrix>("ode",res, "quad",u_dev);
// DAE residual function
FX ffcn = SXFunction(ffcn_in,ffcn_out);
// Overwrite ffcn with a plain c function (avoid this!)
if(plain_c){
// Use DAE residual defined in a c-function
ffcn = CFunction(dae_res_c_wrapper);
// Specify the number of inputs and outputs
ffcn.setNumInputs(DAE_NUM_IN);
ffcn.setNumOutputs(DAE_NUM_OUT);
// Specify dimensions of inputs and outputs
ffcn.input(DAE_T) = DMatrix(1,1,0);
ffcn.input(DAE_X) = DMatrix(3,1,0);
ffcn.input(DAE_XDOT) = DMatrix(3,1,0);
ffcn.input(DAE_P) = DMatrix(1,1,0);
ffcn.output(DAE_ODE) = DMatrix(3,1,0);
ffcn.output(DAE_QUAD) = DMatrix(1,1,0);
}
if(implicit_integrator){
// Create an IDAS instance
IdasIntegrator integrator(ffcn);
// Set IDAS specific options
integrator.setOption("calc_ic",calc_ic);
// Return the integrator
return integrator;
} else {
// Create an CVodes instance
CVodesIntegrator integrator(ffcn);
// Return the integrator
return integrator;
}
}
int main(){
// Test both SX and MX
for(int test=0; test<2; ++test){
// Create a simple function
FX f;
if(test==0){
cout << "SXFunction:" << endl;
SXMatrix x = ssym("x",3);
SXMatrix z = x[0]*x[0]+x[2] + 3;
f = SXFunction(x,z);
} else {
cout << "MXFunction:" << endl;
MX x = msym("x",3);
MX z = x[0]*x[0]+x[2] + 3;
f = MXFunction(x,z);
}
f.init();
// Get arrays for the inputs and outputs, reinterpreting the vector of double as an array of unsigned integers
bvec_t* f_in = get_bvec_t(f.input().data());
bvec_t* f_out = get_bvec_t(f.output().data());
// Propagate from input to output (forward mode)
cout << "forward mode" << endl;
int fwd = true;
// Make sure that the class is able to support the dependency propagation
casadi_assert(f.spCanEvaluate(fwd));
// Pass seeds
f_in[0] = bvec_t(1) << 0; // seed in direction 0
f_in[1] = bvec_t(1) << 2; // seed in direction 2
f_in[2] = (bvec_t(1) << 4) | (bvec_t(1) << 63); // seed in direction 4 and 63
// Reset sensitivities
f_out[0] = 0;
// Propagate dependencies
f.spInit(fwd);
f.spEvaluate(fwd);
// Print the result
printBinary(f_out[0]);
// Propagate from output to input (adjoint/reverse/backward mode)
cout << "backward mode" << endl;
fwd = false;
// Make sure that the class is able to support the dependency propagation
casadi_assert(f.spCanEvaluate(fwd));
// Pass seeds
f_out[0] = (bvec_t(1) << 5) | (bvec_t(1) << 6); // seed in direction 5 and 6
// Reset sensitivities
f_in[0] = 0;
f_in[1] = 0;
f_in[2] = 0;
// Propagate dependencies
f.spInit(fwd);
f.spEvaluate(fwd);
// Print the result
printBinary(f_in[0]);
printBinary(f_in[1]);
printBinary(f_in[2]);
}
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;
}