void CollocationIntegratorInternal::integrate(double t_out){
  for(int oind=0; oind<INTEGRATOR_NUM_OUT; ++oind){
    output(oind).set(implicit_solver_.output(1+oind));
    for(int dir=0; dir<nsens_; ++dir){
      fwdSens(oind,dir).set(implicit_solver_.fwdSens(1+oind,dir));
    }
  }
}
Пример #2
0
void ParallelizerInternal::evaluateTask(int task, int nfdir, int nadir){
  
  // Get a reference to the function
  FX& fcn = funcs_[task];
  
  // Copy inputs to functions
  for(int j=inind_[task]; j<inind_[task+1]; ++j){
    fcn.input(j-inind_[task]).set(input(j));
  }
  
  // Copy outputs to functions (outputs are sometimes used for initialization)
  for(int j=outind_[task]; j<outind_[task+1]; ++j){
    fcn.output(j-outind_[task]).set(output(j));
  }
  
  // Copy forward seeds
  for(int dir=0; dir<nfdir; ++dir){
    for(int j=inind_[task]; j<inind_[task+1]; ++j){
      fcn.fwdSeed(j-inind_[task],dir).set(fwdSeed(j,dir));
    }
  }
  
  // Copy adjoint seeds
  for(int dir=0; dir<nadir; ++dir){
    for(int j=outind_[task]; j<outind_[task+1]; ++j){
      fcn.adjSeed(j-outind_[task],dir).set(adjSeed(j,dir));
    }
  }

  // Evaluate
  fcn.evaluate(nfdir, nadir);
    
  // Get the results
  for(int j=outind_[task]; j<outind_[task+1]; ++j){
    fcn.output(j-outind_[task]).get(output(j));
  }
  
  // Get the forward sensitivities
  for(int dir=0; dir<nfdir; ++dir){
    for(int j=outind_[task]; j<outind_[task+1]; ++j){
      fcn.fwdSens(j-outind_[task],dir).get(fwdSens(j,dir));
    }
  }

  // Get the adjoint sensitivities
  for(int dir=0; dir<nadir; ++dir){
    for(int j=inind_[task]; j<inind_[task+1]; ++j){
      fcn.adjSens(j-inind_[task],dir).get(adjSens(j,dir));
    }
  }
  
  // Save corrected input values // TODO: REMOVE!
  if(save_corrected_input_){
    for(int j=inind_[task]; j<inind_[task+1]; ++j){
      fcn.getInput(input(j),j-inind_[task]);
    }
  }
}
Пример #3
0
void RKIntegratorInternal::reset(int nsens, int nsensB, int nsensB_store){
  // Call the base class method
  IntegratorInternal::reset(nsens,nsensB,nsensB_store);

  int nfdir = 0; // NOTE: need to update the function below to the new integrator formulation
  
  // Pass the inputs
  yf_fun_.setInput(input(INTEGRATOR_X0),0);
  yf_fun_.setInput(input(INTEGRATOR_P),1);
  
  // Pass the forward seeds
  for(int dir=0; dir<nfdir_; ++dir){
    yf_fun_.setFwdSeed(fwdSeed(INTEGRATOR_X0,dir),0,dir);
    yf_fun_.setFwdSeed(fwdSeed(INTEGRATOR_P,dir),1,dir);
  }
  
  // Pass the adjoint seeds
  for(int dir=0; dir<nadir_; ++dir){
    yf_fun_.setAdjSeed(adjSeed(INTEGRATOR_XF,dir),0,dir);
  }
  
  // Evaluate the function
  yf_fun_.evaluate(nfdir);
  
  // Get the outputs
  yf_fun_.getOutput(output(INTEGRATOR_XF),0);
  
  // Get the forward sensitivities
  for(int dir=0; dir<nfdir_; ++dir){
    yf_fun_.getFwdSens(fwdSens(INTEGRATOR_XF,dir),0,dir);
  }
  
  // Get the adjoint sensitivities
  for(int dir=0; dir<nadir_; ++dir){
    yf_fun_.getAdjSens(adjSens(INTEGRATOR_X0,dir),0,dir);
    yf_fun_.getAdjSens(adjSens(INTEGRATOR_P,dir),1,dir);
  }
}
Пример #4
0
void AcadoIntegratorInternal::integrate(double t_out){
  // Integrate the system if this has not already been done
  if(!has_been_integrated_){
    // Initial conditions
    double *x0 = getPtr(input(INTEGRATOR_X0));
    double *z0 = x0 + nxd_;
    
    // Parameters
    double *pp = getPtr(input(INTEGRATOR_P));

    // Integrate
    integrator_->integrate( *interval_, x0, z0, pp );

    // Get solution
    if(nxd_>0) integrator_->getX (*differentialStates_);
    if(nxa_>0) integrator_->getXA(*algebraicStates_);
    
    // Forward sensitivities
    for(int dir=0; dir<nfsens_; ++dir){
      // Order
      const int order = 1;
      
      // Get pointer
      double *xseed = getPtr(fwdSeed(INTEGRATOR_X0,dir));
      double *pseed = getPtr(fwdSeed(INTEGRATOR_P,dir));
      
      // Pass seeds
      *x_tmp_ << xseed;
      if(np_>0) *p_tmp_ << pseed;
      integrator_->setForwardSeed(order, *x_tmp_, *p_tmp_);
      
      // Calculate sensitivities
      integrator_->integrateSensitivities();
      
      // Get pointer
      x_tmp_->setZero();
      integrator_->getForwardSensitivities(*x_tmp_, order);
      double *xsens = getPtr(fwdSens(INTEGRATOR_XF,dir));
      xsens << *x_tmp_;
    }
    
    // Adjoint sensitivities
    for(int dir=0; dir<nasens_; ++dir){
      // Order
      const int order = 1;
      
      // Get pointer
      double *xseed = getPtr(adjSeed(INTEGRATOR_XF,dir));
      
      // Pass seeds
      *x_tmp_ << xseed;
      integrator_->setBackwardSeed(order, *x_tmp_);

      // Clear forward seeds
      x_tmp_->setZero();
      p_tmp_->setZero();
      integrator_->setForwardSeed(order, *x_tmp_, *p_tmp_);
      
      // Calculate sensitivities
      integrator_->integrateSensitivities();
      
      // Get pointer
      double *xsens = getPtr(adjSens(INTEGRATOR_X0,dir));
      double *psens = getPtr(adjSens(INTEGRATOR_P,dir));
      x_tmp_->setZero();
      p_tmp_->setZero();
/*      integrator_->getBackwardSensitivities(ACADO::emptyVector, ACADO::emptyVector, ACADO::emptyVector, ACADO::emptyVector, order);*/
/*      xsens << *x_tmp_;
      if(np_>0) psens << *p_tmp_;*/
    }
    
    // Unfreeze the grid
    if(!frozen_grid_){
      integrator_->unfreeze();
    }
    
    // Mark as integrated
    has_been_integrated_ = true;
  }
  
  // Get the grid point corresponding to the output time
  double grid_point_cont = (num_grid_points_-1)*(t_out-t0_)/(tf_-t0_);
  
  // Get the time point before and after
  int grid_point_before = std::floor(grid_point_cont);
  int grid_point_after  = std::ceil(grid_point_cont);
  
  // Get data
  for(int k=0; k< (nxa_==0 ? 1 : 2); ++k){
    // Pointer to the data
    double* xf = getPtr(output(INTEGRATOR_XF));
    if(k==1) xf += nxd_;
    
    // Variablesgrid
    ACADO::VariablesGrid* vgrid = k==0 ? differentialStates_ : algebraicStates_;
    
    // Get the value before the given time
    *tmp_ = vgrid->getVector(grid_point_before);
    xf << *tmp_;
    
    // Get the value after the given time and interpolate
    if(grid_point_before!=grid_point_after){
      *tmp_ = vgrid->getVector(grid_point_after);
      
      // Weights
      double w_before = grid_point_after-grid_point_cont;
      double w_after = grid_point_cont-grid_point_before;
      
      // Interpolate
      for(int i=0; i<tmp_->getDim(); ++i){
        xf[i] = w_before*xf[i] + w_after*(*tmp_)(i);
      }
    }
  }
}
Пример #5
0
void ImplicitFunctionInternal::evaluate_sens(int nfdir, int nadir, bool linsol_prepared) {
  // Make sure that a linear solver has been provided
  casadi_assert_message(!linsol_.isNull(),"Sensitivities of an implicit function requires a provided linear solver");
  casadi_assert_message(!J_.isNull(),"Sensitivities of an implicit function requires an exact Jacobian");
  
  
  // General scheme:  f(z,x_i) = 0
  //
  //  Forward sensitivities:
  //     dot(f(z,x_i)) = 0
  //     df/dz dot(z) + Sum_i df/dx_i dot(x_i) = 0
  //
  //     dot(z) = [df/dz]^(-1) [ Sum_i df/dx_i dot(x_i) ] 
  //
  //     dot(y_i) = dy_i/dz dot(z) + Sum_j dy_i/dx_i dot(x_i)
  //
  //  Adjoint sensitivitites:

  if (!linsol_prepared) {
    // Pass inputs
    J_.setInput(output(),0);
    for(int i=0; i<getNumInputs(); ++i)
      J_.setInput(input(i),i+1);

    // Evaluate jacobian
    J_.evaluate();

    // Pass non-zero elements, scaled by -gamma, to the linear solver
    linsol_.setInput(J_.output(),0);

    // Prepare the solution of the linear system (e.g. factorize)
    linsol_.prepare();
  }
  
  // Pass inputs to function
  f_.setInput(output(0),0);
  for(int i=0; i<getNumInputs(); ++i)
    f_.input(i+1).set(input(i));

  // Pass input seeds to function
  for(int dir=0; dir<nfdir; ++dir){
    f_.fwdSeed(0,dir).setZero();
    for(int i=0; i<getNumInputs(); ++i){
      f_.fwdSeed(i+1,dir).set(fwdSeed(i,dir));
    }
  }
  
  // Solve for the adjoint seeds
  for(int dir=0; dir<nadir; ++dir){
    // Negate adjoint seed and pass to function
    Matrix<double>& faseed = f_.adjSeed(0,dir);
    faseed.set(adjSeed(0,dir));
    for(vector<double>::iterator it=faseed.begin(); it!=faseed.end(); ++it){
      *it = -*it;
    }
    
    // Solve the transposed linear system
    linsol_.solve(&faseed.front(),1,true);

    // Set auxillary adjoint seeds
    for(int oind=1; oind<getNumOutputs(); ++oind){
      f_.adjSeed(oind,dir).set(adjSeed(oind,dir));
    }
  }
  
  // Evaluate
  f_.evaluate(nfdir,nadir);
  
  // Get the forward sensitivities
  for(int dir=0; dir<nfdir; ++dir){
    // Negate intermediate result and copy to output
    Matrix<double>& fsens = fwdSens(0,dir);
    fsens.set(f_.fwdSens(0,dir));
    for(vector<double>::iterator it=fsens.begin(); it!=fsens.end(); ++it){
      *it = -*it;
    }
    
    // Solve the linear system
    linsol_.solve(&fsens.front());
  }
  
  // Get auxillary forward sensitivities
  if(getNumOutputs()>1){
    // Pass the seeds to the implicitly defined variables
    for(int dir=0; dir<nfdir; ++dir){
      f_.fwdSeed(0,dir).set(fwdSens(0,dir));
    }
    
    // Evaluate
    f_.evaluate(nfdir);
  
    // Get the sensitivities
    for(int dir=0; dir<nfdir; ++dir){
      for(int oind=1; oind<getNumOutputs(); ++oind){
        fwdSens(oind,dir).set(f_.fwdSens(oind,dir));
      }
    }
  }
  
  // Get the adjoint sensitivities
  for(int dir=0; dir<nadir; ++dir){
    for(int i=0; i<getNumInputs(); ++i){
      f_.adjSens(i+1,dir).get(adjSens(i,dir));
    }
  }
}