ocp_iterate.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 
00033 #include <acado/function/ocp_iterate.hpp>
00034 
00035 
00036 BEGIN_NAMESPACE_ACADO
00037 
00038 
00039 //
00040 // PUBLIC MEMBER FUNCTIONS:
00041 //
00042 
00043 OCPiterate::OCPiterate( )
00044 {
00045         x  = 0;
00046         xa = 0;
00047         p  = 0;
00048         u  = 0;
00049         w  = 0;
00050         
00051         init( 0,0,0,0,0 );
00052 }
00053 
00054 
00055 OCPiterate::OCPiterate( const VariablesGrid* const _x,
00056                                                 const VariablesGrid* const _xa,
00057                                                 const VariablesGrid* const _p,
00058                                                 const VariablesGrid* const _u,
00059                                                 const VariablesGrid* const _w
00060                                                 )
00061 {
00062 
00063     x = 0; xa = 0; p = 0; u = 0; w = 0;
00064 
00065         init( _x,_xa,_p,_u,_w );
00066 }
00067 
00068 
00069 OCPiterate::OCPiterate( const OCPiterate& rhs ){
00070 
00071     copy(rhs);
00072 }
00073 
00074 
00075 OCPiterate::~OCPiterate( ){
00076 
00077     clear( );
00078 }
00079 
00080 
00081 OCPiterate& OCPiterate::operator=( const OCPiterate& rhs ){
00082 
00083     if ( this != &rhs ){
00084 
00085         clear( ) ;
00086         copy(rhs);
00087     }
00088     return *this;
00089 }
00090 
00091 
00092 returnValue OCPiterate::allocateAll( )
00093 {
00094     if ( x == 0 )
00095                 x = new VariablesGrid;
00096 
00097     if ( xa == 0 )
00098                 xa = new VariablesGrid;
00099 
00100     if ( p == 0 )
00101                 p = new VariablesGrid;
00102 
00103         if ( u == 0 )
00104                 u = new VariablesGrid;
00105         
00106     if ( w == 0 )
00107                 w = new VariablesGrid;
00108 
00109         return SUCCESSFUL_RETURN;
00110 }
00111 
00112 returnValue OCPiterate::init(   const VariablesGrid* const _x,
00113                                                                 const VariablesGrid* const _xa,
00114                                                                 const VariablesGrid* const _p,
00115                                                                 const VariablesGrid* const _u,
00116                                                                 const VariablesGrid* const _w
00117                                                                 )
00118 {
00119         clear( );
00120 
00121         if ( _x != 0 )
00122                 x = new VariablesGrid( *_x );
00123 
00124         if ( _xa != 0 )
00125                 xa = new VariablesGrid( *_xa );
00126 
00127         if ( _p != 0 )
00128                 p = new VariablesGrid( *_p );
00129 
00130         if ( _u != 0 )
00131                 u = new VariablesGrid( *_u );
00132 
00133         if ( _w != 0 )
00134                 w = new VariablesGrid( *_w );
00135         
00136         return SUCCESSFUL_RETURN;
00137 }
00138 
00139 
00140 returnValue OCPiterate::clear( ){
00141 
00142     if( x  != 0 ){ delete x ; x  = 0; }
00143     if( xa != 0 ){ delete xa; xa = 0; }
00144     if( p  != 0 ){ delete p ; p  = 0; }
00145     if( u  != 0 ){ delete u ; u  = 0; }
00146     if( w  != 0 ){ delete w ; w  = 0; }
00147 
00148         inSimulationMode = BT_FALSE;
00149 
00150     return SUCCESSFUL_RETURN;
00151 }
00152 
00153 
00154 
00155 uint OCPiterate::getNumPoints( ) const
00156 {
00157         int N = 0;
00158 
00159         if( x  != 0 ) N = acadoMax( N, x ->getNumPoints( ) );
00160         if( xa != 0 ) N = acadoMax( N, xa->getNumPoints( ) );
00161         if( p  != 0 ) N = acadoMax( N, p ->getNumPoints( ) );
00162         if( u  != 0 ) N = acadoMax( N, u ->getNumPoints( ) );
00163         if( w  != 0 ) N = acadoMax( N, w ->getNumPoints( ) );
00164 
00165         return N;
00166 }
00167 
00168 
00169 returnValue OCPiterate::print( ) const{
00170 
00171     if( x  != 0 ) x ->print( "iter.x " );
00172     if( xa != 0 ) xa->print( "iter.xa" );
00173     if( p  != 0 ) p ->print( "iter.p " );
00174     if( u  != 0 ) u ->print( "iter.u " );
00175     if( w  != 0 ) w ->print( "iter.w " );
00176 
00177     return SUCCESSFUL_RETURN;
00178 }
00179 
00180 
00181 
00182 Grid OCPiterate::getUnionGrid() const{
00183 
00184     Grid tmp, unionGrid;
00185 
00186     if( x  != 0 ){ x ->getGrid(tmp);  unionGrid = unionGrid & tmp; }
00187     if( xa != 0 ){ xa->getGrid(tmp);  unionGrid = unionGrid & tmp; }
00188     if( p  != 0 ){ p ->getGrid(tmp);  unionGrid = unionGrid & tmp; }
00189     if( u  != 0 ){ u ->getGrid(tmp);  unionGrid = unionGrid & tmp; }
00190     if( w  != 0 ){ w ->getGrid(tmp);  unionGrid = unionGrid & tmp; }
00191 
00192     return unionGrid;
00193 }
00194 
00195 
00196 
00197 BooleanType OCPiterate::areGridsConsistent( )
00198 {
00199         double startTime = 0.0, endTime = 0.0;
00200         BooleanType timeDefined = BT_FALSE;
00201 
00202         if ( ( x != 0 ) && ( x->isEmpty( ) == BT_FALSE ) )
00203         {
00204                 startTime = x->getFirstTime( );
00205                 endTime   = x->getLastTime( );
00206                 timeDefined = BT_TRUE;
00207         }
00208 
00209         if ( ( xa != 0 ) && ( xa->isEmpty( ) == BT_FALSE ) )
00210         {
00211                 if ( timeDefined == BT_FALSE )
00212                 {
00213                         startTime = xa->getFirstTime( );
00214                         endTime   = xa->getLastTime( );
00215                         timeDefined = BT_TRUE;
00216                 }
00217                 else
00218                 {
00219                         if ( ( acadoIsEqual( startTime,xa->getFirstTime( ) ) == BT_FALSE ) ||
00220                                  ( acadoIsEqual( endTime,  xa->getLastTime(  ) ) == BT_FALSE ) )
00221                                 return BT_FALSE;
00222                 }
00223         }
00224 
00225         if ( ( p != 0 ) && ( p->isEmpty( ) == BT_FALSE ) )
00226         {
00227                 if ( timeDefined == BT_FALSE )
00228                 {
00229                         startTime = p->getFirstTime( );
00230                         endTime   = p->getLastTime( );
00231                         timeDefined = BT_TRUE;
00232                 }
00233                 else
00234                 {
00235                         if ( ( acadoIsEqual( startTime,p->getFirstTime( ) ) == BT_FALSE ) ||
00236                                  ( acadoIsEqual( endTime,  p->getLastTime(  ) ) == BT_FALSE ) )
00237                                 return BT_FALSE;
00238                 }
00239         }
00240         
00241         if ( ( u != 0 ) && ( u->isEmpty( ) == BT_FALSE ) )
00242         {
00243                 if ( timeDefined == BT_FALSE )
00244                 {
00245                         startTime = u->getFirstTime( );
00246                         endTime   = u->getLastTime( );
00247                         timeDefined = BT_TRUE;
00248                 }
00249                 else
00250                 {
00251                         if ( ( acadoIsEqual( startTime,u->getFirstTime( ) ) == BT_FALSE ) ||
00252                                  ( acadoIsEqual( endTime,  u->getLastTime(  ) ) == BT_FALSE ) )
00253                                 return BT_FALSE;
00254                 }
00255         }
00256         
00257         if ( ( w != 0 ) && ( w->isEmpty( ) == BT_FALSE ) )
00258         {
00259                 if ( timeDefined == BT_FALSE )
00260                 {
00261                         startTime = w->getFirstTime( );
00262                         endTime   = w->getLastTime( );
00263                         timeDefined = BT_TRUE;
00264                 }
00265                 else
00266                 {
00267                         if ( ( acadoIsEqual( startTime,w->getFirstTime( ) ) == BT_FALSE ) ||
00268                                  ( acadoIsEqual( endTime,  w->getLastTime(  ) ) == BT_FALSE ) )
00269                                 return BT_FALSE;
00270                 }
00271         }
00272         
00273         return BT_TRUE;
00274 }
00275 
00276 
00277 
00278 returnValue OCPiterate::getInitialData( DVector &x_, DVector &xa_, DVector &p_,
00279                                         DVector &u_, DVector &w_               ) const{
00280 
00281     if( x  != 0 ){ x_  = x ->getVector(0); } else { x_  = emptyVector; }
00282     if( xa != 0 ){ xa_ = xa->getVector(0); } else { xa_ = emptyVector; }
00283     if( p  != 0 ){ p_  = p ->getVector(0); } else { p_  = emptyVector; }
00284     if( u  != 0 ){ u_  = u ->getVector(0); } else { u_  = emptyVector; }
00285     if( w  != 0 ){ w_  = w ->getVector(0); } else { w_  = emptyVector; }
00286 
00287     return SUCCESSFUL_RETURN;
00288 }
00289 
00290 
00291 void OCPiterate::update( double t, VariablesGrid &z1, DVector &z2 ) const{
00292 
00293     if( z1.hasTime( t ) == BT_TRUE ){
00294 
00295         uint idx = z1.getFloorIndex( t );
00296         if( z1.getAutoInit(idx) == BT_FALSE ){ z2 = z1.getVector(idx); }
00297         else{
00298 
00299             DVector safeGuard = z2;
00300             safeGuard.setAll( BOUNDTOL );
00301 
00302             if( z1.hasUpperBounds() == BT_TRUE )
00303                 if( z2 >= z1.getUpperBounds(idx) ) z2 = (DVector)z1.getUpperBounds(idx) - safeGuard;
00304 
00305             if( z1.hasLowerBounds() == BT_TRUE )
00306                 if( z2 <= z1.getLowerBounds(idx) ) z2 = (DVector)z1.getLowerBounds(idx) + safeGuard;
00307 
00308             z1.setVector( idx, z2 );
00309         }
00310     }
00311 }
00312 
00313 
00314 returnValue OCPiterate::updateData(  double t  , DVector &x_, DVector &xa_,
00315                                      DVector &p_, DVector &u_, DVector &w_   ){
00316 
00317     if( x  != 0 ) update( t, *x , x_  );
00318     if( xa != 0 ) update( t, *xa, xa_ );
00319     if( p  != 0 ) update( t, *p , p_  );
00320     if( u  != 0 ) update( t, *u , u_  );
00321     if( w  != 0 ) update( t, *w , w_  );
00322 
00323     return SUCCESSFUL_RETURN;
00324 }
00325 
00326 
00327 
00328 returnValue OCPiterate::applyStep(      const BlockMatrix& bm,
00329                                                                         double alpha
00330                                                                         )
00331 {
00332     uint run1, run2;
00333     DMatrix tmp;
00334 
00335     if( getNX() > 0 ){
00336         for( run1 = 0; run1 < getNumPoints(); run1++ ){
00337             bm.getSubBlock( run1, 0, tmp, getNX(), 1 );
00338             for( run2 = 0; run2 < getNX(); run2++ )
00339                 x->operator()(run1,run2) += alpha*tmp(run2,0);
00340         }
00341     }
00342 
00343     if( getNXA() > 0 ){
00344         for( run1 = 0; run1 < getNumPoints(); run1++ ){
00345             bm.getSubBlock( getNumPoints()+run1, 0, tmp, getNXA(), 1 );
00346             for( run2 = 0; run2 < getNXA(); run2++ )
00347                 xa->operator()(run1,run2) += alpha*tmp(run2,0);
00348         }
00349     }
00350 
00351         if( getNP() > 0 ){
00352                 for( run1 = 0; run1 < getNumPoints(); run1++ ){
00353                         bm.getSubBlock( 2*getNumPoints(), 0, tmp, getNP(), 1 );
00354                         for( run2 = 0; run2 < getNP(); run2++ )
00355                                 p->operator()(run1,run2) += alpha*tmp(run2,0);
00356                 }
00357     }
00358 
00359         if( getNU() > 0 ){
00360                 for( run1 = 0; run1 < getNumPoints(); run1++ )
00361                 {
00362                         bm.getSubBlock( 3*getNumPoints()+run1, 0, tmp, getNU(), 1 );
00363                         for( run2 = 0; run2 < getNU(); run2++ )
00364                                 u->operator()(run1,run2) += alpha*tmp(run2,0);
00365                 }
00366         }    
00367 
00368         if( getNW() > 0 ){
00369         for( run1 = 0; run1 < getNumPoints(); run1++ ){
00370             bm.getSubBlock( 4*getNumPoints()+run1, 0, tmp, getNW(), 1 );
00371             for( run2 = 0; run2 < getNW(); run2++ )
00372                 w->operator()(run1,run2) += alpha*tmp(run2,0);
00373         }
00374     }
00375 
00376         return SUCCESSFUL_RETURN;
00377 }
00378 
00379 
00380 
00381 returnValue OCPiterate::enableSimulationMode(){
00382 
00383     if( x  != 0 ) x ->enableAutoInit ();
00384     if( xa != 0 ) xa->enableAutoInit ();
00385     if( p  != 0 ) p ->disableAutoInit();
00386     if( u  != 0 ) u ->disableAutoInit();
00387     if( w  != 0 ) w ->disableAutoInit();
00388 
00389         inSimulationMode = BT_TRUE;
00390         
00391     return SUCCESSFUL_RETURN;
00392 }
00393 
00394 
00395 
00396 returnValue OCPiterate::shift(  double timeShift,
00397                                                             DVector  lastX,
00398                                                             DVector  lastXA,
00399                                                                 DVector  lastP,
00400                                                                 DVector  lastU,
00401                                                                 DVector  lastW ){
00402                                                                 
00403         if ( acadoIsNegative( timeShift ) == BT_TRUE )
00404                 ACADOERROR( RET_INVALID_ARGUMENTS );
00405 
00406         double subintervalLength = x->getIntervalLength( 0 );
00407 
00408         if ( acadoIsEqual( timeShift,subintervalLength ) == BT_FALSE )
00409                 return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
00410         
00411 //      return ACADOERROR( RET_NOT_YET_IMPLEMENTED );
00412         
00413         if ( x != 0 )
00414                 x->shiftBackwards( lastX );
00415         if ( xa != 0 )
00416                 xa->shiftBackwards( lastXA );
00417         
00418         if ( p != 0 )
00419                 p->shiftBackwards( lastP );
00420         
00421         if ( u != 0 )
00422                 u->shiftBackwards( lastU );
00423         
00424         if ( w != 0 )
00425                 w->shiftBackwards( lastW );
00426         
00427         return SUCCESSFUL_RETURN;
00428 }
00429 
00430 
00431 
00432 //
00433 // PROTECTED MEMBER FUNCTIONS:
00434 //
00435 
00436 void OCPiterate::copy( const OCPiterate& rhs ){
00437 
00438     if( rhs.x  != 0 ) x  = new VariablesGrid(*rhs.x );
00439     else              x  = 0                         ;
00440     if( rhs.xa != 0 ) xa = new VariablesGrid(*rhs.xa);
00441     else              xa = 0                         ;
00442     if( rhs.p  != 0 ) p  = new VariablesGrid(*rhs.p );
00443     else              p  = 0                         ;
00444     if( rhs.u  != 0 ) u  = new VariablesGrid(*rhs.u );
00445     else              u  = 0                         ;
00446     if( rhs.w  != 0 ) w  = new VariablesGrid(*rhs.w );
00447     else              w  = 0                         ;
00448 
00449         inSimulationMode = rhs.inSimulationMode;
00450 }
00451 
00452 
00453 
00454 CLOSE_NAMESPACE_ACADO
00455 
00456 // end of file.


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