returnValue ShootingMethod::differentiateForward( const int &idx,
const Matrix &dX ,
const Matrix &dP ,
const Matrix &dU ,
const Matrix &dW ,
Matrix &D ){
int run1;
int n = 0;
n = acadoMax( n, dX.getNumCols() );
n = acadoMax( n, dP.getNumCols() );
n = acadoMax( n, dU.getNumCols() );
n = acadoMax( n, dW.getNumCols() );
D.init( nx, n );
for( run1 = 0; run1 < n; run1++ ){
Vector tmp;
Vector tmpX; if( dX.isEmpty() == BT_FALSE ) tmpX = dX.getCol( run1 );
Vector tmpP; if( dP.isEmpty() == BT_FALSE ) tmpP = dP.getCol( run1 );
Vector tmpU; if( dU.isEmpty() == BT_FALSE ) tmpU = dU.getCol( run1 );
Vector tmpW; if( dW.isEmpty() == BT_FALSE ) tmpW = dW.getCol( run1 );
ACADO_TRY( integrator[idx]->setForwardSeed( 1, tmpX, tmpP, tmpU, tmpW ) );
ACADO_TRY( integrator[idx]->integrateSensitivities( ) );
ACADO_TRY( integrator[idx]->getForwardSensitivities( tmp, 1 ) );
D.setCol( run1, tmp );
}
return SUCCESSFUL_RETURN;
}
void EllipsoidalIntegrator::updateQ( Tmatrix<double> C, Tmatrix<Interval> R ){
Tmatrix<double> CT(nx,nx);
for( int i=0; i<nx; i++ )
for( int j=0;j<nx;j++)
CT(i,j) = C(j,i);
Q = C*Q*CT;
double trQ = 0.0;
for( int i=0; i<nx; i++ ) trQ += Q(i,i)/(Q(i,i)+1e-8);
trQ = ::sqrt(trQ);
Vector sqrR(nx);
for( int i=0; i<nx; i++ ) sqrR(i) = acadoMax(::fabs(R(i).l()),::fabs(R(i).u()))/::sqrt(Q(i,i)+1e-8);
double kappa = trQ;
for( int i=0; i<nx; i++ ) kappa += sqrR(i);
Q *= kappa/(trQ+EPS);
for( int i=0; i<nx; i++ ){
double tmp = acadoMax(::fabs(R(i).l()),::fabs(R(i).u()));
tmp *= ::sqrt(kappa/(sqrR(i)+EPS));
Q(i,i) += tmp*tmp+EPS;
}
}
double EllipsoidalIntegrator::scale( const Interval &E, const Interval &X ) const{
double TOL,ATOL;
get( INTEGRATOR_TOLERANCE, TOL );
get( ABSOLUTE_TOLERANCE , ATOL );
return 0.5*acadoMax( ::fabs(E.l()), ::fabs(E.u()) )/
( TOL*acadoMax( ::fabs(X.l()), ::fabs(X.u()) ) + ATOL );
}
returnValue EvaluationPoint::init( const Function &f ,
uint nx_, uint na_, uint np_,
uint nu_, uint nw_, uint nd_,
uint N_ ) {
uint run1;
deleteAll();
nx = acadoMax( nx_, f.getNX () );
na = acadoMax( na_, f.getNXA() );
np = acadoMax( np_, f.getNP () );
nu = acadoMax( nu_, f.getNU () );
nw = acadoMax( nw_, f.getNW () );
nd = acadoMax( nd_, f.getNDX() );
N = acadoMax( N_ , f.getNumberOfVariables()+1 );
if( N != 0 ) z = new double[N];
else z = 0 ;
setZero( );
idx = new int*[7 ];
idx[0] = new int [1 ];
idx[1] = new int [nx];
idx[2] = new int [na];
idx[3] = new int [np];
idx[4] = new int [nu];
idx[5] = new int [nw];
idx[6] = new int [nd];
idx[0][0] = f.index( VT_TIME, 0 );
for( run1 = 0; run1 < nx; run1++ )
idx[1][run1] = f.index( VT_DIFFERENTIAL_STATE, run1 );
for( run1 = 0; run1 < na; run1++ )
idx[2][run1] = f.index( VT_ALGEBRAIC_STATE, run1 );
for( run1 = 0; run1 < np; run1++ )
idx[3][run1] = f.index( VT_PARAMETER, run1 );
for( run1 = 0; run1 < nu; run1++ )
idx[4][run1] = f.index( VT_CONTROL, run1 );
for( run1 = 0; run1 < nw; run1++ )
idx[5][run1] = f.index( VT_DISTURBANCE, run1 );
for( run1 = 0; run1 < nd; run1++ )
idx[6][run1] = f.index( VT_DDIFFERENTIAL_STATE, run1 );
return SUCCESSFUL_RETURN;
}
double SimulationEnvironment::determineComputationalDelay( double controllerRuntime
) const
{
int simulateComputationalDelay;
get( SIMULATE_COMPUTATIONAL_DELAY,simulateComputationalDelay );
if ( (BooleanType)simulateComputationalDelay == BT_TRUE )
{
ACADOWARNING( RET_COMPUTATIONAL_DELAY_NOT_SUPPORTED );
return 0.0;
double computationalDelayFactor, computationalDelayOffset;
get( COMPUTATIONAL_DELAY_FACTOR,computationalDelayFactor );
get( COMPUTATIONAL_DELAY_OFFSET,computationalDelayOffset );
return acadoMax( 0.0, controllerRuntime*computationalDelayFactor + computationalDelayOffset );
}
else
return 0.0;
}
returnValue OptimizationAlgorithmBase::determineDimensions( Objective* const _objective,
DifferentialEquation** const _differentialEquation,
Constraint* const _constraint,
uint& _nx,
uint& _nxa,
uint& _np,
uint& _nu,
uint& _nw
) const
{
if( _objective != 0 )
{
_nx = acadoMax( _objective->getNX (), _nx );
_nxa = acadoMax( _objective->getNXA(), _nxa );
_np = acadoMax( _objective->getNP (), _np );
_nu = acadoMax( _objective->getNU (), _nu );
_nw = acadoMax( _objective->getNW (), _nw );
}
if( _differentialEquation != 0 )
{
_nx = acadoMax( _differentialEquation[0]->getNX() , _nx );
_nxa = acadoMax( _differentialEquation[0]->getNXA(), _nxa );
_np = acadoMax( _differentialEquation[0]->getNP (), _np );
_nu = acadoMax( _differentialEquation[0]->getNU (), _nu );
_nw = acadoMax( _differentialEquation[0]->getNW (), _nw );
}
if( _constraint != 0 )
{
_nx = acadoMax( _constraint->getNX (), _nx );
_nxa = acadoMax( _constraint->getNXA(), _nxa );
_np = acadoMax( _constraint->getNP (), _np );
_nu = acadoMax( _constraint->getNU (), _nu );
_nw = acadoMax( _constraint->getNW (), _nw );
}
if( _differentialEquation == 0 )
{
if( _nx > 0 )
return RET_INCOMPATIBLE_DIMENSIONS;
}
else
{
_nx = _differentialEquation[0]->getNumDynamicEquations();
// if( nxa != (uint) differentialEquation[0]->getNumAlgebraicEquations()){
//
// for( run1 = 0; run1 < numberOfStages; run1++ )
// if( differentialEquation != 0 ) delete differentialEquation[run1];
//
// if( objective != 0 ) delete objective ;
// if( differentialEquation != 0 ) delete[] differentialEquation ;
// if( transitions != 0 ) delete[] transitions ;
// if( positions != 0 ) delete[] positions ;
// if( constraint != 0 ) delete constraint ;
// if( dynamicDiscretization != 0 ) delete dynamicDiscretization;
//
// return ACADOERROR(RET_INCOMPATIBLE_DIMENSIONS);
// }
}
return SUCCESSFUL_RETURN;
}
returnValue ShootingMethod::differentiateForwardBackward( const int &idx ,
const Matrix &dX ,
const Matrix &dP ,
const Matrix &dU ,
const Matrix &dW ,
const Matrix &seed,
Matrix &D ,
Matrix &ddX ,
Matrix &ddP ,
Matrix &ddU ,
Matrix &ddW ){
int run1;
int n = 0;
n = acadoMax( n, dX.getNumCols() );
n = acadoMax( n, dP.getNumCols() );
n = acadoMax( n, dU.getNumCols() );
n = acadoMax( n, dW.getNumCols() );
D.init( nx, n );
ddX.init( n, nx );
ddP.init( n, np );
ddU.init( n, nu );
ddW.init( n, nw );
for( run1 = 0; run1 < n; run1++ ){
Vector tmp;
Vector tmpX; if( dX.isEmpty() == BT_FALSE ) tmpX = dX.getCol( run1 );
Vector tmpP; if( dP.isEmpty() == BT_FALSE ) tmpP = dP.getCol( run1 );
Vector tmpU; if( dU.isEmpty() == BT_FALSE ) tmpU = dU.getCol( run1 );
Vector tmpW; if( dW.isEmpty() == BT_FALSE ) tmpW = dW.getCol( run1 );
ACADO_TRY( integrator[idx]->setForwardSeed( 1, tmpX, tmpP, tmpU, tmpW ) );
ACADO_TRY( integrator[idx]->integrateSensitivities( ) );
ACADO_TRY( integrator[idx]->getForwardSensitivities( tmp, 1 ) );
D.setCol( run1, tmp );
Vector tmp2 = seed.getCol(0);
Vector tmpX2( nx );
Vector tmpP2( np );
Vector tmpU2( nu );
Vector tmpW2( nw );
ACADO_TRY( integrator[idx]->setBackwardSeed( 2, tmp2 ) );
ACADO_TRY( integrator[idx]->integrateSensitivities( ) );
ACADO_TRY( integrator[idx]->getBackwardSensitivities( tmpX2, tmpP2, tmpU2, tmpW2 , 2 ) );
ddX.setRow( run1, tmpX2 );
ddP.setRow( run1, tmpP2 );
ddU.setRow( run1, tmpU2 );
ddW.setRow( run1, tmpW2 );
ACADO_TRY( integrator[idx]->deleteAllSeeds() );
}
return SUCCESSFUL_RETURN;
}
