Пример #1
0
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();
      }
    }
  }
}
Пример #2
0
// 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;
  }
}
Пример #3
0
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;
}
Пример #4
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;
}