shooting_method.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00027 
00035 #include <acado/dynamic_discretization/shooting_method.hpp>
00036 
00037 
00038 
00039 BEGIN_NAMESPACE_ACADO
00040 
00041 
00042 
00043 //
00044 // PUBLIC MEMBER FUNCTIONS:
00045 //
00046 
00047 
00048 ShootingMethod::ShootingMethod() : DynamicDiscretization( ){
00049 
00050     integrator = 0;
00051 }
00052 
00053 
00054 ShootingMethod::ShootingMethod( UserInteraction* _userInteraction )
00055                :DynamicDiscretization( _userInteraction ){
00056 
00057     integrator = 0;
00058 }
00059 
00060 ShootingMethod::ShootingMethod ( const ShootingMethod& arg ) : DynamicDiscretization( arg ){
00061 
00062     ShootingMethod::copy( arg );
00063 }
00064 
00065 
00066 ShootingMethod::~ShootingMethod( ){
00067 
00068     ShootingMethod::deleteAll();
00069 }
00070 
00071 
00072 ShootingMethod& ShootingMethod::operator=( const ShootingMethod& arg ){
00073 
00074     if ( this != &arg ){
00075         ShootingMethod::deleteAll();
00076         DynamicDiscretization::operator=(arg);
00077         ShootingMethod::copy( arg );
00078     }
00079 
00080     return *this;
00081 }
00082 
00083 
00084 void ShootingMethod::copy( const ShootingMethod &arg ){
00085 
00086     int run1;
00087     if( arg.integrator != 0 ){
00088         integrator = (Integrator**)calloc(N,sizeof(Integrator*));
00089         for( run1 = 0; run1 < N; run1++ ){
00090             if( arg.integrator[run1] != 0 ) integrator[run1] = (arg.integrator[run1])->clone();
00091             else                            integrator[run1] = 0                              ;
00092         }
00093     }
00094     else integrator = 0;
00095 
00096     breakPoints = arg.breakPoints;
00097 }
00098 
00099 DynamicDiscretization* ShootingMethod::clone( ) const{
00100 
00101     return new ShootingMethod(*this);
00102 }
00103 
00104 
00105 returnValue ShootingMethod::addStage( const DynamicSystem  &dynamicSystem_,
00106                                       const Grid           &stageIntervals,
00107                                       const IntegratorType &integratorType_ ){
00108 
00109 
00110     // LOAD THE DIFFERENTIAL EQUATION FROM THE DYNAMIC SYSTEM:
00111     // -------------------------------------------------------
00112     DifferentialEquation differentialEquation_ = dynamicSystem_.getDifferentialEquation( );
00113     int integratorTypeTmp = integratorType_;
00114 
00115 
00116     // AUTOMATICALLY SWITCH TO DISCRETE TIME INTEGRATOR IF NECESSARY:
00117     // --------------------------------------------------------------
00118     if( differentialEquation_.isDiscretized() == BT_TRUE ){
00119          if( integratorTypeTmp != INT_DISCRETE )
00120              integratorTypeTmp = INT_DISCRETE;
00121     }
00122     else{
00123          if( integratorTypeTmp == INT_DISCRETE )
00124              return ACADOERROR( RET_CANNOT_TREAT_CONTINUOUS_DE );
00125     }
00126 
00127 
00128     // CONSTRUCT THE APPROPRIATE INTEGRATOR BASED ON THE OPTIONS:
00129     // ----------------------------------------------------------
00130     int run1 = N;
00131     unionGrid = unionGrid & stageIntervals;
00132     N         = unionGrid.getNumIntervals();
00133 
00134     integrator = (Integrator**)realloc(integrator,N*sizeof(Integrator*));
00135 
00136     while( run1 < N ){
00137         allocateIntegrator( run1, (IntegratorType) integratorTypeTmp );
00138         integrator[run1]->init( differentialEquation_ );
00139         run1++;
00140     }
00141 
00142     // STORE THE INFORMATION ABOUT STAGE-BREAK POINTS AND START/END TIMES:
00143     // -------------------------------------------------------------------
00144     int tmp = 0;
00145     if( breakPoints.getNumRows() > 0 ){
00146         addOptionsList( );
00147         tmp = (int) breakPoints( breakPoints.getNumRows()-1, 0 );
00148     }
00149 
00150     DMatrix stageIndices(1,5);
00151 
00152     stageIndices(0,0) = stageIntervals.getNumIntervals() + tmp;
00153     stageIndices(0,1) = differentialEquation_.getStartTimeIdx();
00154     stageIndices(0,2) = differentialEquation_.getEndTimeIdx();
00155     stageIndices(0,3) = differentialEquation_.getStartTime();
00156     stageIndices(0,4) = differentialEquation_.getEndTime();
00157 
00158     breakPoints.appendRows(stageIndices);
00159 
00160     return SUCCESSFUL_RETURN;
00161 }
00162 
00163 
00164 returnValue ShootingMethod::allocateIntegrator( uint idx, IntegratorType type_ ){
00165 
00166     switch( type_ ){
00167 
00168          case INT_DISCRETE: integrator[idx] = new IntegratorDiscretizedODE(); break;
00169          case INT_RK12    : integrator[idx] = new IntegratorRK12          (); break;
00170          case INT_RK23    : integrator[idx] = new IntegratorRK23          (); break;
00171          case INT_RK45    : integrator[idx] = new IntegratorRK45          (); break;
00172          case INT_RK78    : integrator[idx] = new IntegratorRK78          (); break;
00173          case INT_BDF     : integrator[idx] = new IntegratorBDF           (); break;
00174          case INT_UNKNOWN : integrator[idx] = new IntegratorBDF           (); break;
00175          case INT_LYAPUNOV45 : integrator[idx] = new IntegratorLYAPUNOV45          (); break;
00176 
00177          default: return ACADOERROR( RET_UNKNOWN_BUG ); break;
00178     }
00179     return SUCCESSFUL_RETURN;
00180 }
00181 
00182 
00183 
00184 returnValue ShootingMethod::addTransition( const Transition& transition_ ){
00185 
00186     if( transition_.getNXA() != 0 ) return ACADOERROR( RET_TRANSITION_DEPENDS_ON_ALGEBRAIC_STATES );
00187     integrator[N-1]->setTransition( transition_ );
00188 
00189     return SUCCESSFUL_RETURN;
00190 }
00191 
00192 
00193 returnValue ShootingMethod::clear(){
00194 
00195     deleteAllSeeds();
00196     ShootingMethod::deleteAll();
00197     integrator = 0;
00198     breakPoints.init(0,0);
00199 
00200     return SUCCESSFUL_RETURN;
00201 }
00202 
00203 
00204 
00205 returnValue ShootingMethod::evaluate(   OCPiterate &iter
00206                                                                                 )
00207 {
00208 //      iter.print(); ASSERT( 1==0 );
00209 
00210     // INTRODUCE SOME AUXILIARY VARIABLES:
00211     // -----------------------------------
00212     ASSERT( iter.x != 0 );
00213 
00214     uint run1;
00215     double tStart, tEnd;
00216 
00217     DVector x ;  nx = iter.getNX ();
00218     DVector xa;  na = iter.getNXA();
00219     DVector p ;  np = iter.getNP ();
00220     DVector u ;  nu = iter.getNU ();
00221     DVector w ;  nw = iter.getNW ();
00222 
00223         VariablesGrid xAll;
00224         VariablesGrid xaAll;
00225 
00226     residuum = *(iter.x);
00227     residuum.setAll( 0.0 );
00228 
00229     iter.getInitialData( x, xa, p, u, w );
00230 //      iter.x->print( "x" );
00231 //      iter.u->print( "u" );
00232 
00233     // RUN A LOOP OVER ALL INTERVALS OF THE UNION GRID:
00234     // ------------------------------------------------
00235 
00236 //      printf("unionGrid:\n");
00237 //      unionGrid.print();
00238 
00239     for( run1 = 0; run1 < unionGrid.getNumIntervals(); run1++ ){
00240 
00241         integrator[run1]->setOptions( getOptions( 0 ) );  // ??
00242 
00243                 int freezeIntegrator;
00244                 get( FREEZE_INTEGRATOR, freezeIntegrator );
00245 
00246                 if ( (BooleanType)freezeIntegrator == BT_TRUE )
00247                         integrator[run1]->freezeAll();
00248 
00249         tStart = unionGrid.getTime( run1   );
00250         tEnd   = unionGrid.getTime( run1+1 );
00251                 
00252 //              printf("tStart = %e,   tEnd = %e\n",tStart,tEnd );
00253 //              x.print("x");
00254 //              u.print("u");
00255 //              p.print("p");
00256 
00257 //              integrator[run1]->set( INTEGRATOR_PRINTLEVEL, MEDIUM );
00258 
00259                 Grid evaluationGrid;
00260                 iter.x->getSubGrid( tStart,tEnd,evaluationGrid );
00261                 
00262         Grid outputGrid;
00263                 if ( acadoIsNegative( integrator[run1]->getDifferentialEquationSampleTime( ) ) == BT_TRUE )
00264                         outputGrid.init( tStart,tEnd,getNumEvaluationPoints() );
00265                 else
00266                         outputGrid.init( tStart,tEnd, 1+acadoRound( (tEnd-tStart)/integrator[run1]->getDifferentialEquationSampleTime() ) );
00267 
00268 //              printf("evaluationGrid:\n");
00269 //              evaluationGrid.print();
00270 //              printf("outputGrid:\n");
00271 //              outputGrid.print();
00272 //              printf("evaluationGrid+outputGrid:\n");
00273 //              (outputGrid&evaluationGrid).print();
00274 //              printf("\n");
00275 //              u.print("u before");
00276 //              x.print("x before");
00277 //              w.print("w");
00278         if ( integrator[run1]->integrate( outputGrid&evaluationGrid, x, xa, p, u, w ) != SUCCESSFUL_RETURN )
00279                         return ACADOERROR( RET_UNABLE_TO_INTEGRATE_SYSTEM );
00280 
00281                 
00282                 DVector xOld;
00283                 DVector pOld = p;
00284                 
00285                 if ( evaluationGrid.getNumPoints( ) <= 2 )
00286                 {
00287                         integrator[run1]->getX ( x  );
00288                         integrator[run1]->getXA( xa );
00289 //                      x.print("x after2");
00290                         xOld = x;
00291                         
00292                         iter.updateData( tEnd, x, xa, p, u, w );
00293                 }
00294                 else
00295                 {
00296                         integrator[run1]->getX (  xAll );
00297                         integrator[run1]->getXA( xaAll );
00298 
00299                         xOld = xAll.getLastVector( );
00300 //                      xOld.print("x after");
00301                         
00302                         for( uint run2=1; run2<outputGrid.getNumPoints(); ++run2 )
00303                         {
00304                                 if ( evaluationGrid.hasTime( outputGrid.getTime(run2) ) == BT_TRUE )
00305                                 {
00306                                         x  =  xAll.getVector(run2);
00307                                         xa = xaAll.getVector(run2);
00308                                         iter.updateData( outputGrid.getTime(run2), x, xa, p, u, w );
00309                                 }
00310                         }
00311                 }
00312 
00313                 if ( iter.isInSimulationMode( ) == BT_FALSE )
00314                         p = pOld;  // should be changed later...
00315 
00316         residuum.setVector( run1, xOld - x );
00317 //              (xOld - x).print("residuum");
00318     }
00319 
00320     // LOG THE RESULTS:
00321     // ----------------
00322     return logTrajectory( iter );
00323 }
00324 
00325 
00326 
00327 returnValue ShootingMethod::differentiateBackward( const int    &idx ,
00328                                                    const DMatrix &seed,
00329                                                          DMatrix &Gx  ,
00330                                                          DMatrix &Gp  ,
00331                                                          DMatrix &Gu  ,
00332                                                          DMatrix &Gw    ){
00333 
00334     uint run1;
00335 
00336     Gx.init( seed.getNumRows(), nx );
00337     Gp.init( seed.getNumRows(), np );
00338     Gu.init( seed.getNumRows(), nu );
00339     Gw.init( seed.getNumRows(), nw );
00340 
00341     for( run1 = 0; run1 < seed.getNumRows(); run1++ ){
00342 
00343          DVector tmp = seed.getRow( run1 );
00344          DVector tmpX( nx );
00345          DVector tmpP( np );
00346          DVector tmpU( nu );
00347          DVector tmpW( nw );
00348 
00349          ACADO_TRY( integrator[idx]->setBackwardSeed( 1, tmp )                              );
00350          ACADO_TRY( integrator[idx]->integrateSensitivities( )                              );
00351          ACADO_TRY( integrator[idx]->getBackwardSensitivities( tmpX, tmpP, tmpU, tmpW , 1 ) );
00352 
00353          Gx.setRow( run1, tmpX );
00354          Gp.setRow( run1, tmpP );
00355          Gu.setRow( run1, tmpU );
00356          Gw.setRow( run1, tmpW );
00357     }
00358 
00359     return SUCCESSFUL_RETURN;
00360 }
00361 
00362 
00363 
00364 returnValue ShootingMethod::differentiateForward(  const int     &idx,
00365                                                    const DMatrix  &dX ,
00366                                                    const DMatrix  &dP ,
00367                                                    const DMatrix  &dU ,
00368                                                    const DMatrix  &dW ,
00369                                                          DMatrix  &D    ){
00370 
00371     int run1;
00372     int n = 0;
00373 
00374     n = acadoMax( n, dX.getNumCols() );
00375     n = acadoMax( n, dP.getNumCols() );
00376     n = acadoMax( n, dU.getNumCols() );
00377     n = acadoMax( n, dW.getNumCols() );
00378 
00379     D.init( nx, n );
00380 
00381     for( run1 = 0; run1 < n; run1++ ){
00382 
00383          DVector tmp;
00384 
00385          DVector tmpX; if( dX.isEmpty() == BT_FALSE ) tmpX = dX.getCol( run1 );
00386          DVector tmpP; if( dP.isEmpty() == BT_FALSE ) tmpP = dP.getCol( run1 );
00387          DVector tmpU; if( dU.isEmpty() == BT_FALSE ) tmpU = dU.getCol( run1 );
00388          DVector tmpW; if( dW.isEmpty() == BT_FALSE ) tmpW = dW.getCol( run1 );
00389 
00390          ACADO_TRY( integrator[idx]->setForwardSeed( 1, tmpX, tmpP, tmpU, tmpW ) );
00391          ACADO_TRY( integrator[idx]->integrateSensitivities( )                   );
00392          ACADO_TRY( integrator[idx]->getForwardSensitivities( tmp, 1 )           );
00393 
00394          D.setCol( run1, tmp );
00395     }
00396 
00397     return SUCCESSFUL_RETURN;
00398 }
00399 
00400 
00401 returnValue ShootingMethod::differentiateForwardBackward( const int     &idx ,
00402                                                           const DMatrix  &dX  ,
00403                                                           const DMatrix  &dP  ,
00404                                                           const DMatrix  &dU  ,
00405                                                           const DMatrix  &dW  ,
00406                                                           const DMatrix  &seed,
00407                                                                 DMatrix  &D   ,
00408                                                                 DMatrix  &ddX ,
00409                                                                 DMatrix  &ddP ,
00410                                                                 DMatrix  &ddU ,
00411                                                                 DMatrix  &ddW   ){
00412 
00413     int run1;
00414     int n = 0;
00415 
00416     n = acadoMax( n, dX.getNumCols() );
00417     n = acadoMax( n, dP.getNumCols() );
00418     n = acadoMax( n, dU.getNumCols() );
00419     n = acadoMax( n, dW.getNumCols() );
00420 
00421     D.init( nx, n );
00422 
00423     ddX.init( n, nx );
00424     ddP.init( n, np );
00425     ddU.init( n, nu );
00426     ddW.init( n, nw );
00427 
00428     for( run1 = 0; run1 < n; run1++ ){
00429 
00430          DVector tmp;
00431 
00432          DVector tmpX; if( dX.isEmpty() == BT_FALSE ) tmpX = dX.getCol( run1 );
00433          DVector tmpP; if( dP.isEmpty() == BT_FALSE ) tmpP = dP.getCol( run1 );
00434          DVector tmpU; if( dU.isEmpty() == BT_FALSE ) tmpU = dU.getCol( run1 );
00435          DVector tmpW; if( dW.isEmpty() == BT_FALSE ) tmpW = dW.getCol( run1 );
00436 
00437          ACADO_TRY( integrator[idx]->setForwardSeed( 1, tmpX, tmpP, tmpU, tmpW ) );
00438          ACADO_TRY( integrator[idx]->integrateSensitivities( )                   );
00439          ACADO_TRY( integrator[idx]->getForwardSensitivities( tmp, 1 )           );
00440 
00441          D.setCol( run1, tmp );
00442 
00443          DVector tmp2 = seed.getCol(0);
00444 
00445          DVector tmpX2( nx );
00446          DVector tmpP2( np );
00447          DVector tmpU2( nu );
00448          DVector tmpW2( nw );
00449 
00450          ACADO_TRY( integrator[idx]->setBackwardSeed( 2, tmp2 )                                 );
00451          ACADO_TRY( integrator[idx]->integrateSensitivities( )                                  );
00452          ACADO_TRY( integrator[idx]->getBackwardSensitivities( tmpX2, tmpP2, tmpU2, tmpW2 , 2 ) );
00453 
00454          ddX.setRow( run1, tmpX2 );
00455          ddP.setRow( run1, tmpP2 );
00456          ddU.setRow( run1, tmpU2 );
00457          ddW.setRow( run1, tmpW2 );
00458 
00459          ACADO_TRY( integrator[idx]->deleteAllSeeds() );
00460     }
00461 
00462     return SUCCESSFUL_RETURN;
00463 }
00464 
00465 
00466 
00467 returnValue ShootingMethod::evaluateSensitivities(){
00468 
00469     int i;
00470 
00471     // COMPUTATION OF BACKWARD SENSITIVITIES:
00472     // --------------------------------------
00473 
00474     if( bSeed.isEmpty() == BT_FALSE ){
00475 
00476         dBackward.init( N, 5 );
00477 
00478         for( i = 0; i < N; i++ ){
00479 
00480              DMatrix seed, X, P, U, W;
00481              bSeed.getSubBlock( 0, i, seed );
00482 
00483              ACADO_TRY( differentiateBackward( i, seed, X, P, U, W ) );
00484 
00485              if( nx > 0 ) dBackward.setDense( i, 0, X );
00486              if( np > 0 ) dBackward.setDense( i, 2, P );
00487              if( nu > 0 ) dBackward.setDense( i, 3, U );
00488              if( nw > 0 ) dBackward.setDense( i, 4, W );
00489         }
00490         return SUCCESSFUL_RETURN;
00491     }
00492 
00493 
00494     // COMPUTATION OF FORWARD SENSITIVITIES:
00495     // -------------------------------------
00496 
00497     dForward.init( N, 5 );
00498 
00499     for( i = 0; i < N; i++ ){
00500 
00501         DMatrix X, P, U, W, D, E;
00502 
00503         if( xSeed.isEmpty() == BT_FALSE ) xSeed.getSubBlock( i, 0, X );
00504         if( pSeed.isEmpty() == BT_FALSE ) pSeed.getSubBlock( i, 0, P );
00505         if( uSeed.isEmpty() == BT_FALSE ) uSeed.getSubBlock( i, 0, U );
00506         if( wSeed.isEmpty() == BT_FALSE ) wSeed.getSubBlock( i, 0, W );
00507 
00508         if( nx > 0 ){ ACADO_TRY( differentiateForward( i, X, E, E, E, D )); dForward.setDense( i, 0, D ); }
00509         if( np > 0 ){ ACADO_TRY( differentiateForward( i, E, P, E, E, D )); dForward.setDense( i, 2, D ); }
00510         if( nu > 0 ){ ACADO_TRY( differentiateForward( i, E, E, U, E, D )); dForward.setDense( i, 3, D ); }
00511         if( nw > 0 ){ ACADO_TRY( differentiateForward( i, E, E, E, W, D )); dForward.setDense( i, 4, D ); }
00512     }
00513     return SUCCESSFUL_RETURN;
00514 }
00515 
00516 
00517 
00518 
00519 returnValue ShootingMethod::update( DMatrix &G, const DMatrix &A, const DMatrix &B ){
00520 
00521     if( B.getNumCols() == 0 ) return SUCCESSFUL_RETURN;
00522 
00523     DMatrix E = eye<double>(B.getNumCols());
00524     E *= 1e-10;
00525 
00526     DMatrix S = ((A.transpose().eval() * A)+E).inverse();
00527     G += (B-G*A)*(S*A.transpose());
00528     return SUCCESSFUL_RETURN;
00529 }
00530 
00531 
00532 
00533 returnValue ShootingMethod::evaluateSensitivitiesLifted( ){
00534 
00535     int i,j;
00536 
00537     dForward.init( N, 5 );
00538     DMatrix Gx, *Gu, b, d, D, E, X, P, U, W, A, B;
00539     Gu = new DMatrix[N];
00540 
00541     for( i = 0; i < N; i++ ){
00542 
00543         if( xSeed.isEmpty() == BT_FALSE ) xSeed.getSubBlock( i, 0, X );
00544         if( pSeed.isEmpty() == BT_FALSE ) pSeed.getSubBlock( i, 0, P );
00545         if( uSeed.isEmpty() == BT_FALSE ) uSeed.getSubBlock( i, 0, U );
00546         if( wSeed.isEmpty() == BT_FALSE ) wSeed.getSubBlock( i, 0, W );
00547 
00548         if( np > 0 ){ D.init( nx, np ); D.setZero(); dForward.setDense( i, 2, D ); }
00549         if( nw > 0 ){ D.init( nx, nw ); D.setZero(); dForward.setDense( i, 4, D ); }
00550 
00551         if( nu > 0 ){ ACADO_TRY( differentiateForward( i, E, E, U, E, D )); dForward.setDense( i, 3, D ); Gu[i] = D; }
00552     }
00553 
00554     if( nu > 0 ){
00555 
00556         d.init(nx,1);
00557         d.setZero();
00558 
00559         for( i = 0; i < N; i++ ){
00560             A.init(0,0);
00561             B.init(0,0);
00562             for( j = 0; j < i; j++ ){
00563                 differentiateForward( i, Gu[j], E, E, E, D );
00564                 A.appendCols( Gu[j] );
00565                 B.appendCols( D     );
00566                 Gu[j] = D;
00567             }
00568 
00569             if( i > 0 ){
00570                 differentiateForward( i, d, E, E, E, b );
00571                 A.appendCols( d );
00572                 B.appendCols( b );
00573                 d = b;
00574             }
00575             d += residuum.getMatrix(i);
00576             Gx = eye<double>(nx);
00577             Gx *= 0.001;
00578             update( Gx, A, B );
00579             dForward.setDense( i, 0, Gx );
00580         }
00581     }
00582     delete[] Gu;
00583 
00584     return SUCCESSFUL_RETURN;
00585 }
00586 
00587 
00588 
00589 returnValue ShootingMethod::evaluateSensitivities( const BlockMatrix &seed, BlockMatrix &hessian ){
00590 
00591     const int NN = N+1;
00592     dForward.init( N, 5 );
00593     int i;
00594 
00595     for( i = 0; i < N; i++ ){
00596 
00597         DMatrix X, P, U, W, D, E, HX, HP, HU, HW, S;
00598 
00599         if( xSeed.isEmpty() == BT_FALSE ) xSeed.getSubBlock( i, 0, X );
00600         if( pSeed.isEmpty() == BT_FALSE ) pSeed.getSubBlock( i, 0, P );
00601         if( uSeed.isEmpty() == BT_FALSE ) uSeed.getSubBlock( i, 0, U );
00602         if( wSeed.isEmpty() == BT_FALSE ) wSeed.getSubBlock( i, 0, W );
00603 
00604         seed.getSubBlock( i, 0, S, nx, 1 );
00605 
00606         if( nx > 0 ){
00607 
00608             ACADO_TRY( differentiateForwardBackward( i, X, E, E, E, S, D, HX, HP, HU, HW ));
00609             dForward.setDense( i, 0, D );
00610 
00611             if( nx > 0 ) hessian.addDense( i,      i, HX );
00612             if( np > 0 ) hessian.addDense( i, 2*NN+i, HP );
00613             if( nu > 0 ) hessian.addDense( i, 3*NN+i, HU );
00614             if( nw > 0 ) hessian.addDense( i, 4*NN+i, HW );
00615         }
00616 
00617         if( np > 0 ){
00618 
00619             ACADO_TRY( differentiateForwardBackward( i, E, P, E, E, S, D, HX, HP, HU, HW ));
00620             dForward.setDense( i, 2, D );
00621 
00622             if( nx > 0 ) hessian.addDense( 2*NN+i,      i, HX );
00623             if( np > 0 ) hessian.addDense( 2*NN+i, 2*NN+i, HP );
00624             if( nu > 0 ) hessian.addDense( 2*NN+i, 3*NN+i, HU );
00625             if( nw > 0 ) hessian.addDense( 2*NN+i, 4*NN+i, HW );
00626         }
00627 
00628         if( nu > 0 ){
00629 
00630             ACADO_TRY( differentiateForwardBackward( i, E, E, U, E, S, D, HX, HP, HU, HW ));
00631             dForward.setDense( i, 3, D );
00632 
00633             if( nx > 0 ) hessian.addDense( 3*NN+i,      i, HX );
00634             if( np > 0 ) hessian.addDense( 3*NN+i, 2*NN+i, HP );
00635             if( nu > 0 ) hessian.addDense( 3*NN+i, 3*NN+i, HU );
00636             if( nw > 0 ) hessian.addDense( 3*NN+i, 4*NN+i, HW );
00637         }
00638 
00639         if( nw > 0 ){
00640 
00641             ACADO_TRY( differentiateForwardBackward( i, E, E, E, W, S, D, HX, HP, HU, HW ));
00642             dForward.setDense( i, 4, D );
00643 
00644             if( nx > 0 ) hessian.addDense( 4*NN+i,      i, HX );
00645             if( np > 0 ) hessian.addDense( 4*NN+i, 2*NN+i, HP );
00646             if( nu > 0 ) hessian.addDense( 4*NN+i, 3*NN+i, HU );
00647             if( nw > 0 ) hessian.addDense( 4*NN+i, 4*NN+i, HW );
00648         }
00649     }
00650     return SUCCESSFUL_RETURN;
00651 }
00652 
00653 
00654 returnValue ShootingMethod::unfreeze(){
00655 
00656     int run1;
00657     for( run1 = 0; run1 < (int) unionGrid.getNumIntervals(); run1++ )
00658          integrator[run1]->unfreeze();
00659 
00660     return SUCCESSFUL_RETURN;
00661 }
00662 
00663 
00664 returnValue ShootingMethod::deleteAllSeeds(){
00665 
00666     DynamicDiscretization::deleteAllSeeds();
00667 
00668     uint i;
00669     for( i = 0; i < unionGrid.getNumIntervals(); i++ )
00670         integrator[i]->deleteAllSeeds();
00671 
00672     return SUCCESSFUL_RETURN;
00673 }
00674 
00675 
00676 BooleanType ShootingMethod::isAffine( ) const{
00677 
00678         for( int run1 = 0; run1 < N; ++run1 )
00679                 if ( integrator[run1]->isDifferentialEquationAffine( ) == BT_FALSE )
00680                         return BT_FALSE;
00681 
00682     return BT_TRUE;
00683 }
00684 
00685 
00686 returnValue ShootingMethod::deleteAll(){
00687 
00688     int run1;
00689     if( integrator != 0 ){
00690         for( run1 = 0; run1 < N; run1++ )
00691             if( integrator[run1] != 0 )
00692                 delete integrator[run1];
00693         free(integrator);
00694         integrator = 0;
00695     }
00696 
00697         unionGrid.init();
00698         DynamicDiscretization::initializeVariables( );
00699 
00700     return SUCCESSFUL_RETURN;
00701 }
00702 
00703 
00704 returnValue ShootingMethod::logTrajectory( const OCPiterate &iter ){
00705 
00706     if( integrator == 0 ) return SUCCESSFUL_RETURN;
00707 
00708     int i, j;
00709     double T = 0.0;
00710         double t1 = 0.0, t2 = 0.0;
00711         double h = 0.0;
00712         BooleanType needToRescale = BT_FALSE;
00713 
00714     VariablesGrid logX, logXA, logP, logU, logW, logI, tmp,tmp2;
00715 
00716     DMatrix intervalPoints(N+1,1);
00717     intervalPoints(0,0) = 0.0;
00718 
00719     j = 0;
00720     for( i = 0; i < N; i++ ){
00721 
00722         if( (int) breakPoints(j,0) <= i ) j++;
00723 
00724         int i1 = (int) breakPoints(j,1);
00725         int i2 = (int) breakPoints(j,2);
00726 
00727         if( i1 >= 0 )  t1 = iter.p->operator()(0,i1);
00728         else           t1 = breakPoints(j,3);
00729 
00730         if( i2 >= 0 )  t2 = iter.p->operator()(0,i2);
00731         else           t2 = breakPoints(j,4);
00732 
00733         if( i == 0 ) T = t1;
00734 
00735         integrator[i]->getX( tmp );
00736 
00737         intervalPoints(i+1,0) = intervalPoints(i,0) + tmp.getNumPoints();
00738 
00739         if ( ( i1 >= 0 ) || ( i2 >= 0 ) )
00740                 {
00741                         if ( iter.isInSimulationMode() == BT_FALSE )
00742                         {
00743                                 h = t2-t1;
00744                                 needToRescale = BT_TRUE;
00745                         }
00746                 }
00747         else
00748                 {
00749                         h = 1.0;
00750                         needToRescale = BT_FALSE;
00751                 }
00752 
00753         if( nx > 0 ){ if ( needToRescale == BT_TRUE ) rescale( &tmp, T, h );
00754                                                 logX .appendTimes( tmp );
00755                     }
00756         if( na > 0 ){ integrator[i]->getXA( tmp );
00757                       if ( needToRescale == BT_TRUE ) rescale( &tmp, T, h );
00758                       logXA.appendTimes( tmp );
00759                     }
00760         if( np > 0 ){ tmp2.init( np, tmp.getFirstTime(),tmp.getLastTime(),2 );
00761                                           if ( iter.isInSimulationMode( ) == BT_FALSE )
00762                                                 tmp2.setAllVectors( iter.p->getVector(0) );
00763                                           else
00764                                                   tmp2.setAllVectors( iter.p->getVector(i) );
00765                       logP .appendTimes( tmp2 );
00766                     }
00767         if( nu > 0 ){ tmp2.init( nu, tmp.getFirstTime(),tmp.getLastTime(),2 );
00768                       tmp2.setAllVectors(iter.u->getVector(i));
00769                       logU .appendTimes( tmp2 );
00770                     }
00771         if( nw > 0 ){ tmp.init( nw, tmp );
00772                       tmp.setAllVectors(iter.w->getVector(i));
00773                       logW .appendTimes( tmp );
00774                     }
00775                       integrator[i]->getI( tmp );
00776                       if ( needToRescale == BT_TRUE ) rescale( &tmp, T, h );
00777                       logI .appendTimes( tmp );
00778         T = tmp.getLastTime();
00779     }
00780 
00781 
00782     // WRITE DATA TO THE LOG COLLECTION:
00783     // ---------------------------------
00784     if( nx > 0 ) setLast( LOG_DIFFERENTIAL_STATES, logX   );
00785     if( na > 0 ) setLast( LOG_ALGEBRAIC_STATES   , logXA  );
00786     if( np > 0 ) setLast( LOG_PARAMETERS         , logP   );
00787     if( nu > 0 ) setLast( LOG_CONTROLS           , logU   );
00788     if( nw > 0 ) setLast( LOG_DISTURBANCES       , logW   );
00789 
00790     setLast( LOG_INTERMEDIATE_STATES     , logI           );
00791     setLast( LOG_DISCRETIZATION_INTERVALS, intervalPoints );
00792 
00793 
00794     return SUCCESSFUL_RETURN;
00795 }
00796 
00797 
00798 returnValue ShootingMethod::rescale(    VariablesGrid* trajectory,
00799                                                                                 double tEndNew,
00800                                                                                 double newIntervalLength
00801                                                                                 ) const
00802 {
00803         trajectory->shiftTimes( -trajectory->getTime(0) );
00804         trajectory->scaleTimes( newIntervalLength );
00805         trajectory->shiftTimes( tEndNew  );
00806         
00807         return SUCCESSFUL_RETURN;
00808 }
00809 
00810 
00811 CLOSE_NAMESPACE_ACADO
00812 
00813 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:55