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]);
}
}
}
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);
}
}
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);
}
}
}
}
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));
}
}
}
void CollocationIntegratorInternal::reset(int nsens, int nsensB, int nsensB_store){
// Call the base class method
IntegratorInternal::reset(nsens,nsensB,nsensB_store);
// Pass the inputs
for(int iind=0; iind<INTEGRATOR_NUM_IN; ++iind){
implicit_solver_.input(iind).set(input(iind));
}
// Pass the forward seeds
for(int dir=0; dir<nsens; ++dir){
for(int iind=0; iind<INTEGRATOR_NUM_IN; ++iind){
implicit_solver_.fwdSeed(iind,dir).set(fwdSeed(iind,dir));
}
}
// Pass solution guess (if this is the first integration or if hotstart is disabled)
if(hotstart_==false || integrated_once_==false){
vector<double>& v = implicit_solver_.output().data();
// Check if an integrator for the startup trajectory has been supplied
bool has_startup_integrator = !startup_integrator_.isNull();
// Use supplied integrator, if any
if(has_startup_integrator){
for(int iind=0; iind<INTEGRATOR_NUM_IN; ++iind){
startup_integrator_.input(iind).set(input(iind));
}
// Reset the integrator
startup_integrator_.reset();
}
// Integrate, stopping at all time points
int offs=0;
for(int k=0; k<coll_time_.size(); ++k){
for(int j=0; j<coll_time_[k].size(); ++j){
if(has_startup_integrator){
// Integrate to the time point
startup_integrator_.integrate(coll_time_[k][j]);
}
// Save the differential states
const DMatrix& x = has_startup_integrator ? startup_integrator_.output(INTEGRATOR_XF) : input(INTEGRATOR_X0);
for(int i=0; i<nx_; ++i){
v.at(offs++) = x.at(i);
}
// Skip algebraic variables (for now) // FIXME
if(j>0){
offs += nz_;
}
// Skip backward states // FIXME
const DMatrix& rx = input(INTEGRATOR_RX0);
for(int i=0; i<nrx_; ++i){
v.at(offs++) = rx.at(i);
}
// Skip backward algebraic variables // FIXME
if(j>0){
offs += nrz_;
}
}
}
// Print
if(has_startup_integrator && verbose()){
cout << "startup trajectory generated, statistics:" << endl;
startup_integrator_.printStats();
}
}
// Solve the system of equations
implicit_solver_.evaluate(nsens);
// Mark the system integrated at least once
integrated_once_ = true;
}
