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;
}
returnValue ShootingMethod::differentiateBackward( const int &idx ,
const Matrix &seed,
Matrix &Gx ,
Matrix &Gp ,
Matrix &Gu ,
Matrix &Gw ){
uint run1;
Gx.init( seed.getNumRows(), nx );
Gp.init( seed.getNumRows(), np );
Gu.init( seed.getNumRows(), nu );
Gw.init( seed.getNumRows(), nw );
for( run1 = 0; run1 < seed.getNumRows(); run1++ ){
Vector tmp = seed.getRow( run1 );
Vector tmpX( nx );
Vector tmpP( np );
Vector tmpU( nu );
Vector tmpW( nw );
ACADO_TRY( integrator[idx]->setBackwardSeed( 1, tmp ) );
ACADO_TRY( integrator[idx]->integrateSensitivities( ) );
ACADO_TRY( integrator[idx]->getBackwardSensitivities( tmpX, tmpP, tmpU, tmpW , 1 ) );
Gx.setRow( run1, tmpX );
Gp.setRow( run1, tmpP );
Gu.setRow( run1, tmpU );
Gw.setRow( run1, tmpW );
}
return SUCCESSFUL_RETURN;
}
returnValue Objective::evaluate( const OCPiterate &x ){
uint run1;
for( run1 = 0; run1 < nLSQ; run1++ )
ACADO_TRY( lsqTerm[run1]->evaluate( x ) );
for( run1 = 0; run1 < nEndLSQ; run1++ )
ACADO_TRY( lsqEndTerm[run1]->evaluate( x ) );
for( run1 = 0; run1 < nMayer; run1++ )
ACADO_TRY( mayerTerm[run1]->evaluate( x ) );
return SUCCESSFUL_RETURN;
}
returnValue ShootingMethod::evaluateSensitivities(){
int i;
// COMPUTATION OF BACKWARD SENSITIVITIES:
// --------------------------------------
if( bSeed.isEmpty() == BT_FALSE ){
dBackward.init( N, 5 );
for( i = 0; i < N; i++ ){
Matrix seed, X, P, U, W;
bSeed.getSubBlock( 0, i, seed );
ACADO_TRY( differentiateBackward( i, seed, X, P, U, W ) );
if( nx > 0 ) dBackward.setDense( i, 0, X );
if( np > 0 ) dBackward.setDense( i, 2, P );
if( nu > 0 ) dBackward.setDense( i, 3, U );
if( nw > 0 ) dBackward.setDense( i, 4, W );
}
return SUCCESSFUL_RETURN;
}
// COMPUTATION OF FORWARD SENSITIVITIES:
// -------------------------------------
dForward.init( N, 5 );
for( i = 0; i < N; i++ ){
Matrix X, P, U, W, D, E;
if( xSeed.isEmpty() == BT_FALSE ) xSeed.getSubBlock( i, 0, X );
if( pSeed.isEmpty() == BT_FALSE ) pSeed.getSubBlock( i, 0, P );
if( uSeed.isEmpty() == BT_FALSE ) uSeed.getSubBlock( i, 0, U );
if( wSeed.isEmpty() == BT_FALSE ) wSeed.getSubBlock( i, 0, W );
if( nx > 0 ){ ACADO_TRY( differentiateForward( i, X, E, E, E, D )); dForward.setDense( i, 0, D ); }
if( np > 0 ){ ACADO_TRY( differentiateForward( i, E, P, E, E, D )); dForward.setDense( i, 2, D ); }
if( nu > 0 ){ ACADO_TRY( differentiateForward( i, E, E, U, E, D )); dForward.setDense( i, 3, D ); }
if( nw > 0 ){ ACADO_TRY( differentiateForward( i, E, E, E, W, D )); dForward.setDense( i, 4, D ); }
}
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::init( VariablesGrid* x_init ,
VariablesGrid* xa_init,
VariablesGrid* p_init ,
VariablesGrid* u_init ,
VariablesGrid* w_init )
{
int printC;
get( PRINT_COPYRIGHT, printC );
// PRINT THE HEADER:
// -----------------
if( printC == BT_TRUE )
acadoPrintCopyrightNotice( "SCPmethod -- A Sequential Quadratic Programming Algorithm." );
iter.init( x_init, xa_init, p_init, u_init, w_init );
// iter.print(); // already here different!!
if ( setup( ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_INIT_FAILED );
// COMPUTATION OF DERIVATIVES:
// ---------------------------
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Computing initial linearization of NLP system ...\n" );
// iter.print();
ACADO_TRY( eval->evaluateSensitivities( iter,bandedCP ) ).changeType( RET_NLP_INIT_FAILED );
// iter.print();
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Computing initial linearization of NLP system done.\n" );
int useRealtimeIterations;
get( USE_REALTIME_ITERATIONS,useRealtimeIterations );
if ( (BooleanType)useRealtimeIterations == BT_TRUE )
{
if ( bandedCPsolver->prepareSolve( bandedCP ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_STEP_FAILED );
}
// freeze condensing in case OCP is QP -- isCP is a bit misleading...
if ( ( isCP == BT_TRUE ) && ( eval->hasLSQobjective( ) == BT_TRUE ) )
{
// bandedCPsolver->freezeCondensing( );
// eval->freezeSensitivities( );
}
status = BS_READY;
return SUCCESSFUL_RETURN;
}
returnValue VariablesGrid::setAllVectors( const Vector& _values
)
{
for( uint i = 0; i < getNumPoints(); i++ )
ACADO_TRY( setVector( i,_values ) );
return SUCCESSFUL_RETURN;
}
returnValue Constraint::add( const int index_, const ConstraintComponent& component ) {
Vector tmp_ub(grid.getNumPoints());
Vector tmp_lb(grid.getNumPoints());
ASSERT_RETURN( index_ < (int) grid.getNumPoints() ).addMessage("\n >>> The constraint component can not be set as the associated discretization point is not in the time horizon. <<< \n\n");
uint run1;
if( component.hasLBgrid() == 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( (component.getLB()).getDim() == 1 )
tmp_lb(run1) = (component.getLB()).operator()(0);
else {
if( (component.getLB()).getDim() <= run1 )
return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
tmp_lb(run1) = (component.getLB()).operator()(run1);
}
}
}
else {
VariablesGrid LBgrid = component.getLBgrid();
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
Vector tmp = LBgrid.linearInterpolation( grid.getTime(run1) );
tmp_lb(run1) = tmp(0);
}
}
if( component.hasUBgrid() == 0 ) {
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
if( (component.getUB()).getDim() == 1 )
tmp_ub(run1) = (component.getUB()).operator()(0);
else {
if( (component.getUB()).getDim() <= run1 )
return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
tmp_ub(run1) = (component.getUB()).operator()(run1);
}
}
}
else {
VariablesGrid UBgrid = component.getUBgrid();
for( run1 = 0; run1 < grid.getNumPoints(); run1++ ) {
Vector tmp = UBgrid.linearInterpolation( grid.getTime(run1) );
tmp_ub(run1) = tmp(0);
}
}
ACADO_TRY( add( index_, tmp_lb(index_), component.getExpression(), tmp_ub(index_) ) );
return SUCCESSFUL_RETURN;
}
returnValue OCP::subjectTo( const TimeHorizonElement index_, const ConstraintComponent& component ){
uint i;
switch( index_ ){
case AT_START:
for( i = 0; i < component.getDim(); i++ )
ACADO_TRY( constraint.add( 0,component(i) ) );
return SUCCESSFUL_RETURN;
case AT_END:
for( i = 0; i < component.getDim(); i++ )
ACADO_TRY( constraint.add( grid.getLastIndex(),component(i) ) );
return SUCCESSFUL_RETURN;
default:
return ACADOERROR(RET_UNKNOWN_BUG);
}
return SUCCESSFUL_RETURN;
}
returnValue ShootingMethod::evaluateSensitivitiesLifted( ){
int i,j;
dForward.init( N, 5 );
Matrix Gx, *Gu, b, d, D, E, X, P, U, W, A, B;
Gu = new Matrix[N];
for( i = 0; i < N; i++ ){
if( xSeed.isEmpty() == BT_FALSE ) xSeed.getSubBlock( i, 0, X );
if( pSeed.isEmpty() == BT_FALSE ) pSeed.getSubBlock( i, 0, P );
if( uSeed.isEmpty() == BT_FALSE ) uSeed.getSubBlock( i, 0, U );
if( wSeed.isEmpty() == BT_FALSE ) wSeed.getSubBlock( i, 0, W );
if( np > 0 ){ D.init( nx, np ); D.setZero(); dForward.setDense( i, 2, D ); }
if( nw > 0 ){ D.init( nx, nw ); D.setZero(); dForward.setDense( i, 4, D ); }
if( nu > 0 ){ ACADO_TRY( differentiateForward( i, E, E, U, E, D )); dForward.setDense( i, 3, D ); Gu[i] = D; }
}
if( nu > 0 ){
d.init(nx,1);
d.setZero();
for( i = 0; i < N; i++ ){
A.init(0,0);
B.init(0,0);
for( j = 0; j < i; j++ ){
differentiateForward( i, Gu[j], E, E, E, D );
A.appendCols( Gu[j] );
B.appendCols( D );
Gu[j] = D;
}
if( i > 0 ){
differentiateForward( i, d, E, E, E, b );
A.appendCols( d );
B.appendCols( b );
d = b;
}
d += residuum.getMatrix(i);
Gx = eye(nx);
Gx *= 0.001;
update( Gx, A, B );
dForward.setDense( i, 0, Gx );
}
}
delete[] Gu;
return SUCCESSFUL_RETURN;
}
returnValue SCPevaluation::evaluate( OCPiterate& iter, BandedCP& cp ){
// EVALUATE THE OBJECTIVE AND CONSTRAINTS:
// ---------------------------------------
if( dynamicDiscretization != 0 )
ACADO_TRY( dynamicDiscretization->evaluate( iter ) ).changeType( RET_UNABLE_TO_INTEGRATE_SYSTEM );
if( constraint != 0 )
ACADO_TRY( constraint->evaluate( iter ) ).changeType( RET_UNABLE_TO_EVALUATE_CONSTRAINTS );
ACADO_TRY( objective->evaluate(iter) ).changeType( RET_UNABLE_TO_EVALUATE_OBJECTIVE );
objective->getObjectiveValue( objectiveValue );
if( dynamicDiscretization != 0 )
dynamicDiscretization->getResiduum( cp.dynResiduum );
if( constraint != 0 )
{
constraint->getBoundResiduum ( cp.lowerBoundResiduum , cp.upperBoundResiduum );
constraint->getConstraintResiduum( cp.lowerConstraintResiduum, cp.upperConstraintResiduum );
}
if( dynamicDiscretization == 0 ){
cp.lowerBoundResiduum.setZero(0,0);
cp.lowerBoundResiduum.setZero(1,0);
cp.lowerBoundResiduum.setZero(3,0);
cp.lowerBoundResiduum.setZero(4,0);
cp.upperBoundResiduum.setZero(0,0);
cp.upperBoundResiduum.setZero(1,0);
cp.upperBoundResiduum.setZero(3,0);
cp.upperBoundResiduum.setZero(4,0);
}
return SUCCESSFUL_RETURN;
}
returnValue SCPmethod::setup( )
{
// CONSISTENCY CHECKS:
// -------------------
if ( isCP == BT_TRUE )
{
int hessianApproximation;
get( HESSIAN_APPROXIMATION,hessianApproximation );
// Gauss-Newton is exact for linear-quadratic systems
if ( (HessianApproximationMode)hessianApproximation == EXACT_HESSIAN )
set( HESSIAN_APPROXIMATION,GAUSS_NEWTON );
}
// PREPARE THE SQP ALGORITHM:
// --------------------------
if ( eval == 0 )
return ACADOERROR( RET_UNKNOWN_BUG );
if ( eval->init( iter ) != SUCCESSFUL_RETURN )
return ACADOERROR( RET_NLP_INIT_FAILED );
// PREPARE THE DATA FOR THE SQP ALGORITHM:
// ---------------------------------------
if ( bandedCPsolver != 0 )
delete bandedCPsolver;
int sparseQPsolution;
get( SPARSE_QP_SOLUTION,sparseQPsolution );
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Initializing banded QP solver ...\n" );
if ( (SparseQPsolutionMethods)sparseQPsolution == CONDENSING )
{
bandedCP.lambdaConstraint.init( eval->getNumConstraintBlocks(), 1 );
bandedCP.lambdaDynamic.init( getNumPoints()-1, 1 );
bandedCPsolver = new CondensingBasedCPsolver( userInteraction,eval->getNumConstraints(),eval->getConstraintBlockDims() );
bandedCPsolver->init( iter );
}
else
{
return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
}
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Initializing banded QP solver done.\n" );
// INITIALIZE GLOBALIZATION STRATEGY (SCPstep):
// --------------------------------------------
if ( scpStep != 0 )
delete scpStep;
int globalizationStrategy;
get( GLOBALIZATION_STRATEGY,globalizationStrategy );
switch( (GlobalizationStrategy)globalizationStrategy )
{
case GS_FULLSTEP:
scpStep = new SCPstepFullstep( userInteraction );
break;
case GS_LINESEARCH:
scpStep = new SCPstepLinesearch( userInteraction );
break;
default:
return ACADOERROR( RET_UNKNOWN_BUG );
}
// EVALUATION OF THE NLP FUNCTIONS:
// --------------------------------
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Initial integration of dynamic system ...\n" );
ACADO_TRY( eval->evaluate(iter,bandedCP) ).changeType( RET_NLP_INIT_FAILED );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Initial integration of dynamic system done.\n" );
// INITIALIZE HESSIAN MATRIX:
// --------------------------
int hessianMode;
get( HESSIAN_APPROXIMATION,hessianMode );
if( ( (HessianApproximationMode)hessianMode == GAUSS_NEWTON ) || ( (HessianApproximationMode)hessianMode == GAUSS_NEWTON_WITH_BLOCK_BFGS ) )
{
if( eval->hasLSQobjective( ) == BT_FALSE ){
// ACADOWARNING( RET_GAUSS_NEWTON_APPROXIMATION_NOT_SUPPORTED );
// set( HESSIAN_APPROXIMATION, BLOCK_BFGS_UPDATE );
// get( HESSIAN_APPROXIMATION, hessianMode );
}
}
if ( derivativeApproximation != 0 )
delete derivativeApproximation;
switch( (HessianApproximationMode)hessianMode )
{
case EXACT_HESSIAN:
derivativeApproximation = new ExactHessian( userInteraction );
break;
case CONSTANT_HESSIAN:
derivativeApproximation = new ConstantHessian( userInteraction );
break;
case FULL_BFGS_UPDATE:
derivativeApproximation = new BFGSupdate( userInteraction );
break;
case BLOCK_BFGS_UPDATE:
derivativeApproximation = new BFGSupdate( userInteraction,getNumPoints() );
break;
case GAUSS_NEWTON:
derivativeApproximation = new GaussNewtonApproximation( userInteraction );
break;
case GAUSS_NEWTON_WITH_BLOCK_BFGS:
derivativeApproximation = new GaussNewtonApproximationWithBFGS( userInteraction,getNumPoints() );
break;
default:
return ACADOERROR( RET_UNKNOWN_BUG );
}
bandedCP.hessian.init( 5*getNumPoints(), 5*getNumPoints() );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Initializing Hessian computations ...\n" );
ACADO_TRY( derivativeApproximation->initHessian( bandedCP.hessian,getNumPoints(),iter ) );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "<-- Initializing Hessian computations done.\n" );
// SWITCH BETWEEN SINGLE- AND MULTIPLE SHOOTING:
// ---------------------------------------------
int discretizationMode;
get( DISCRETIZATION_TYPE, discretizationMode );
if( (StateDiscretizationType)discretizationMode != SINGLE_SHOOTING ){
if( iter.x != 0 ) iter.x ->disableAutoInit();
if( iter.xa != 0 ) iter.xa->disableAutoInit();
}
return SUCCESSFUL_RETURN;
}
returnValue PointConstraint::evaluateSensitivities( ){
// EVALUATION OF THE SENSITIVITIES:
// --------------------------------
int run1;
returnValue returnvalue;
if( fcn == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
const int N = grid.getNumPoints();
// EVALUATION OF THE SENSITIVITIES:
// --------------------------------
if( bSeed != 0 ){
if( xSeed != 0 || pSeed != 0 || uSeed != 0 || wSeed != 0 ||
xSeed2 != 0 || pSeed2 != 0 || uSeed2 != 0 || wSeed2 != 0 )
return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
int nBDirs = bSeed->getNumRows( 0, 0 );
DMatrix bseed_;
bSeed->getSubBlock( 0, 0, bseed_);
dBackward.init( 1, 5*N );
DMatrix Dx ( nBDirs, nx );
DMatrix Dxa( nBDirs, na );
DMatrix Dp ( nBDirs, np );
DMatrix Du ( nBDirs, nu );
DMatrix Dw ( nBDirs, nw );
for( run1 = 0; run1 < nBDirs; run1++ )
{
ACADO_TRY( fcn[0].AD_backward( bseed_.getRow(run1), JJ[0] ) );
if( nx > 0 ) Dx .setRow( run1, JJ[0].getX () );
if( na > 0 ) Dxa.setRow( run1, JJ[0].getXA() );
if( np > 0 ) Dp .setRow( run1, JJ[0].getP () );
if( nu > 0 ) Du .setRow( run1, JJ[0].getU () );
if( nw > 0 ) Dw .setRow( run1, JJ[0].getW () );
JJ[0].setZero( );
}
if( nx > 0 )
dBackward.setDense( 0, point_index , Dx );
if( na > 0 )
dBackward.setDense( 0, N+point_index, Dxa );
if( np > 0 )
dBackward.setDense( 0, 2*N+point_index, Dp );
if( nu > 0 )
dBackward.setDense( 0, 3*N+point_index, Du );
if( nw > 0 )
dBackward.setDense( 0, 4*N+point_index, Dw );
return SUCCESSFUL_RETURN;
}
if( xSeed != 0 || pSeed != 0 || uSeed != 0 || wSeed != 0 ){
if( bSeed != 0 ) return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
if( condType != CT_SPARSE ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
dForward.init( 1, 5*N );
if( xSeed != 0 ){
DMatrix tmp;
xSeed->getSubBlock(0,0,tmp);
returnvalue = computeForwardSensitivityBlock( 0, 0, &tmp );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
if( xaSeed != 0 ){
DMatrix tmp;
xaSeed->getSubBlock(0,0,tmp);
returnvalue = computeForwardSensitivityBlock( nx, N+point_index, &tmp );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
if( pSeed != 0 ){
DMatrix tmp;
pSeed->getSubBlock(0,0,tmp);
returnvalue = computeForwardSensitivityBlock( nx+na, 2*N+point_index, &tmp );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
if( uSeed != 0 ){
DMatrix tmp;
uSeed->getSubBlock(0,0,tmp);
returnvalue = computeForwardSensitivityBlock( nx+na+np, 3*N+point_index, &tmp );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
if( wSeed != 0 ){
DMatrix tmp;
wSeed->getSubBlock(0,0,tmp);
returnvalue = computeForwardSensitivityBlock( nx+na+np+nu, 4*N+point_index, &tmp );
if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
}
return SUCCESSFUL_RETURN;
}
return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
}
returnValue SCPevaluation::evaluateSensitivities( const OCPiterate& iter,
BandedCP& cp
)
{
if ( areSensitivitiesFrozen == BT_TRUE )
return SUCCESSFUL_RETURN;
// DETERMINE THE HESSIAN APPROXIMATION MODE:
// -----------------------------------------
int hessMode;
get( HESSIAN_APPROXIMATION, hessMode );
int dynHessMode;
get( DYNAMIC_HESSIAN_APPROXIMATION, dynHessMode );
if ( (HessianApproximationMode)dynHessMode == DEFAULT_HESSIAN_APPROXIMATION )
dynHessMode = hessMode;
int dynMode;
get( DYNAMIC_SENSITIVITY, dynMode );
int objMode;
get( OBJECTIVE_SENSITIVITY, objMode );
int conMode;
get( CONSTRAINT_SENSITIVITY, conMode );
// COMPUTE THE 1st ORDER DERIVATIVES:
// ----------------------------------
objective->setUnitBackwardSeed( );
if( ( (HessianApproximationMode)hessMode == GAUSS_NEWTON ) || ( (HessianApproximationMode)hessMode == GAUSS_NEWTON_WITH_BLOCK_BFGS ) )
objective->evaluateSensitivitiesGN( cp.hessian );
else{
if( (HessianApproximationMode)hessMode == EXACT_HESSIAN ){
cp.hessian.setZero();
objective->evaluateSensitivities( cp.hessian );
}
else objective->evaluateSensitivities();
}
objective->getBackwardSensitivities( cp.objectiveGradient, 1 );
//
// printf("cp.hessian = \n");
// cp.hessian.print();
if( dynamicDiscretization != 0 ){
if( (HessianApproximationMode)dynHessMode == EXACT_HESSIAN ){
ACADO_TRY( dynamicDiscretization->setUnitForwardSeed() );
ACADO_TRY( dynamicDiscretization->evaluateSensitivities( cp.lambdaDynamic, cp.hessian ) );
ACADO_TRY( dynamicDiscretization->getForwardSensitivities( cp.dynGradient ) );
}
else{
if( dynMode == BACKWARD_SENSITIVITY ){
ACADO_TRY( dynamicDiscretization->setUnitBackwardSeed() );
ACADO_TRY( dynamicDiscretization->evaluateSensitivities() );
ACADO_TRY( dynamicDiscretization->getBackwardSensitivities( cp.dynGradient ) );
}
if( dynMode == FORWARD_SENSITIVITY ){
ACADO_TRY( dynamicDiscretization->setUnitForwardSeed() );
ACADO_TRY( dynamicDiscretization->evaluateSensitivities() );
ACADO_TRY( dynamicDiscretization->getForwardSensitivities( cp.dynGradient ) );
}
if( dynMode == FORWARD_SENSITIVITY_LIFTED ){
ACADO_TRY( dynamicDiscretization->setUnitForwardSeed() );
ACADO_TRY( dynamicDiscretization->evaluateSensitivitiesLifted() );
ACADO_TRY( dynamicDiscretization->getForwardSensitivities( cp.dynGradient ) );
}
}
}
if( constraint != 0 ){
if( (HessianApproximationMode)hessMode == EXACT_HESSIAN ){
constraint->setUnitBackwardSeed();
constraint->evaluateSensitivities( cp.lambdaConstraint, cp.hessian );
constraint->getBackwardSensitivities( cp.constraintGradient, 1 );
}
else{
if( conMode == BACKWARD_SENSITIVITY ){
constraint->setUnitBackwardSeed();
constraint->evaluateSensitivities( );
constraint->getBackwardSensitivities( cp.constraintGradient, 1 );
}
if( conMode == FORWARD_SENSITIVITY ){
constraint->setUnitForwardSeed();
constraint->evaluateSensitivities( );
constraint->getForwardSensitivities( cp.constraintGradient, 1 );
}
}
}
return SUCCESSFUL_RETURN;
}
returnValue BoundaryConstraint::evaluateSensitivities( ){
int run1;
const int N = grid.getNumPoints();
// EVALUATION OF THE SENSITIVITIES:
// --------------------------------
if( bSeed != 0 )
{
if( xSeed != 0 || pSeed != 0 || uSeed != 0 || wSeed != 0 ||
xSeed2 != 0 || pSeed2 != 0 || uSeed2 != 0 || wSeed2 != 0 )
return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
int nBDirs = bSeed->getNumRows( 0, 0 );
DMatrix bseed_;
bSeed->getSubBlock( 0, 0, bseed_);
dBackward.init( 1, 5*N );
DMatrix Dx ( nBDirs, nx );
DMatrix Dxa( nBDirs, na );
DMatrix Dp ( nBDirs, np );
DMatrix Du ( nBDirs, nu );
DMatrix Dw ( nBDirs, nw );
// evaluate at start
for( run1 = 0; run1 < nBDirs; run1++ )
{
ACADO_TRY( fcn[0].AD_backward( bseed_.getRow(run1), JJ[0], 0 ) );
if( nx > 0 ) Dx .setRow( run1, JJ[0].getX () );
if( na > 0 ) Dxa.setRow( run1, JJ[0].getXA() );
if( np > 0 ) Dp .setRow( run1, JJ[0].getP () );
if( nu > 0 ) Du .setRow( run1, JJ[0].getU () );
if( nw > 0 ) Dw .setRow( run1, JJ[0].getW () );
JJ[0].setZero( );
}
if( nx > 0 )
dBackward.setDense( 0, 0 , Dx );
if( na > 0 )
dBackward.setDense( 0, N, Dxa );
if( np > 0 )
dBackward.setDense( 0, 2*N, Dp );
if( nu > 0 )
dBackward.setDense( 0, 3*N, Du );
if( nw > 0 )
dBackward.setDense( 0, 4*N, Dw );
// evaluate at end
for( run1 = 0; run1 < nBDirs; run1++ )
{
ACADO_TRY( fcn[1].AD_backward( bseed_.getRow(run1), JJ[1], 0 ) );
if( nx > 0 ) Dx .setRow( run1, JJ[1].getX () );
if( na > 0 ) Dxa.setRow( run1, JJ[1].getXA() );
if( np > 0 ) Dp .setRow( run1, JJ[1].getP () );
if( nu > 0 ) Du .setRow( run1, JJ[1].getU () );
if( nw > 0 ) Dw .setRow( run1, JJ[1].getW () );
JJ[1].setZero( );
}
if( nx > 0 )
dBackward.setDense( 0, N-1, Dx );
if( na > 0 )
dBackward.setDense( 0, 2*N-1, Dxa );
if( np > 0 )
dBackward.setDense( 0, 3*N-1, Dp );
if( nu > 0 )
dBackward.setDense( 0, 4*N-1, Du );
if( nw > 0 )
dBackward.setDense( 0, 5*N-1, Dw );
return SUCCESSFUL_RETURN;
}
// TODO: implement forward mode
return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
}
returnValue ShootingMethod::evaluateSensitivities( const BlockMatrix &seed, BlockMatrix &hessian ){
const int NN = N+1;
dForward.init( N, 5 );
int i;
for( i = 0; i < N; i++ ){
Matrix X, P, U, W, D, E, HX, HP, HU, HW, S;
if( xSeed.isEmpty() == BT_FALSE ) xSeed.getSubBlock( i, 0, X );
if( pSeed.isEmpty() == BT_FALSE ) pSeed.getSubBlock( i, 0, P );
if( uSeed.isEmpty() == BT_FALSE ) uSeed.getSubBlock( i, 0, U );
if( wSeed.isEmpty() == BT_FALSE ) wSeed.getSubBlock( i, 0, W );
seed.getSubBlock( i, 0, S, nx, 1 );
if( nx > 0 ){
ACADO_TRY( differentiateForwardBackward( i, X, E, E, E, S, D, HX, HP, HU, HW ));
dForward.setDense( i, 0, D );
if( nx > 0 ) hessian.addDense( i, i, HX );
if( np > 0 ) hessian.addDense( i, 2*NN+i, HP );
if( nu > 0 ) hessian.addDense( i, 3*NN+i, HU );
if( nw > 0 ) hessian.addDense( i, 4*NN+i, HW );
}
if( np > 0 ){
ACADO_TRY( differentiateForwardBackward( i, E, P, E, E, S, D, HX, HP, HU, HW ));
dForward.setDense( i, 2, D );
if( nx > 0 ) hessian.addDense( 2*NN+i, i, HX );
if( np > 0 ) hessian.addDense( 2*NN+i, 2*NN+i, HP );
if( nu > 0 ) hessian.addDense( 2*NN+i, 3*NN+i, HU );
if( nw > 0 ) hessian.addDense( 2*NN+i, 4*NN+i, HW );
}
if( nu > 0 ){
ACADO_TRY( differentiateForwardBackward( i, E, E, U, E, S, D, HX, HP, HU, HW ));
dForward.setDense( i, 3, D );
if( nx > 0 ) hessian.addDense( 3*NN+i, i, HX );
if( np > 0 ) hessian.addDense( 3*NN+i, 2*NN+i, HP );
if( nu > 0 ) hessian.addDense( 3*NN+i, 3*NN+i, HU );
if( nw > 0 ) hessian.addDense( 3*NN+i, 4*NN+i, HW );
}
if( nw > 0 ){
ACADO_TRY( differentiateForwardBackward( i, E, E, E, W, S, D, HX, HP, HU, HW ));
dForward.setDense( i, 4, D );
if( nx > 0 ) hessian.addDense( 4*NN+i, i, HX );
if( np > 0 ) hessian.addDense( 4*NN+i, 2*NN+i, HP );
if( nu > 0 ) hessian.addDense( 4*NN+i, 3*NN+i, HU );
if( nw > 0 ) hessian.addDense( 4*NN+i, 4*NN+i, HW );
}
}
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;
}