objective_element.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 
00034 #include <acado/objective/objective_element.hpp>
00035 
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 
00042 
00043 ObjectiveElement::ObjectiveElement( ){
00044 
00045     y_index = 0;
00046     t_index = 0;
00047     nx      = 0;
00048     na      = 0;
00049     nu      = 0;
00050     np      = 0;
00051     nw      = 0;
00052     ny      = 0;
00053 
00054     xSeed         = 0     ;
00055     xaSeed        = 0     ;
00056     pSeed         = 0     ;
00057     uSeed         = 0     ;
00058     wSeed         = 0     ;
00059     bSeed         = 0     ;
00060     xSeed2        = 0     ;
00061     xaSeed2       = 0     ;
00062     pSeed2        = 0     ;
00063     uSeed2        = 0     ;
00064     wSeed2        = 0     ;
00065     bSeed2        = 0     ;
00066 
00067     obj = 0.0;
00068 }
00069 
00070 
00071 ObjectiveElement::ObjectiveElement( const Grid &grid_ ){
00072 
00073     grid    = grid_;
00074     y_index = 0    ;
00075     t_index = 0    ;
00076     nx      = 0    ;
00077     na      = 0    ;
00078     nu      = 0    ;
00079     np      = 0    ;
00080     nw      = 0    ;
00081     ny      = 0    ;
00082 
00083     xSeed         = 0     ;
00084     xaSeed        = 0     ;
00085     pSeed         = 0     ;
00086     uSeed         = 0     ;
00087     wSeed         = 0     ;
00088     bSeed         = 0     ;
00089     xSeed2        = 0     ;
00090     xaSeed2       = 0     ;
00091     pSeed2        = 0     ;
00092     uSeed2        = 0     ;
00093     wSeed2        = 0     ;
00094     bSeed2        = 0     ;
00095 
00096     obj = 0.0;
00097 }
00098 
00099 
00100 ObjectiveElement::ObjectiveElement( const ObjectiveElement& rhs ){
00101 
00102     int run1;
00103 
00104     grid = rhs.grid;
00105     fcn  = rhs.fcn ;
00106 
00107     if( rhs.y_index != 0 ){
00108         y_index = new int[rhs.ny];
00109         for( run1 = 0; run1 < rhs.ny; run1++ )
00110             y_index[run1] = rhs.y_index[run1];
00111     }
00112     else  y_index = 0;
00113 
00114     t_index = rhs.t_index;
00115 
00116     nx = rhs.nx;
00117     na = rhs.na;
00118     nu = rhs.nu;
00119     np = rhs.np;
00120     nw = rhs.nw;
00121     ny = rhs.ny;
00122 
00123     if( rhs.xSeed != 0 )  xSeed  = new BlockMatrix(*rhs.xSeed );
00124     else                  xSeed  = 0                           ;
00125     if( rhs.xaSeed != 0 ) xaSeed = new BlockMatrix(*rhs.xaSeed);
00126     else                  xaSeed = 0                           ;
00127     if( rhs.pSeed != 0 )  pSeed  = new BlockMatrix(*rhs.pSeed) ;
00128     else                  pSeed  = 0                           ;
00129     if( rhs.uSeed != 0 )  uSeed  = new BlockMatrix(*rhs.uSeed) ;
00130     else                  uSeed  = 0                           ;
00131     if( rhs.wSeed != 0 )  wSeed  = new BlockMatrix(*rhs.wSeed) ;
00132     else                  wSeed  = 0                           ;
00133 
00134     if( rhs.bSeed != 0 )  bSeed  = new BlockMatrix(*rhs.bSeed );
00135     else                  bSeed  = 0                           ;
00136 
00137     if( rhs.xSeed2 != 0 )  xSeed2  = new BlockMatrix(*rhs.xSeed2) ;
00138     else                   xSeed2  = 0                            ;
00139     if( rhs.xaSeed2 != 0 ) xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
00140     else                   xaSeed2 = 0                            ;
00141     if( rhs.pSeed2 != 0 )  pSeed2  = new BlockMatrix(*rhs.pSeed2 );
00142     else                   pSeed2  = 0                            ;
00143     if( rhs.uSeed2 != 0 )  uSeed2  = new BlockMatrix(*rhs.uSeed2 );
00144     else                   uSeed2  = 0                            ;
00145     if( rhs.wSeed2 != 0 )  wSeed2  = new BlockMatrix(*rhs.wSeed2 );
00146     else                   wSeed2  = 0                            ;
00147 
00148     if( rhs.bSeed != 0 )   bSeed2 = new BlockMatrix(*rhs.bSeed );
00149     else                   bSeed2 = 0                           ;
00150 
00151 
00152     obj       = rhs.obj      ;
00153     dForward  = rhs.dForward ;
00154     dBackward = rhs.dBackward;
00155 }
00156 
00157 
00158 ObjectiveElement::~ObjectiveElement( ){
00159 
00160     if( y_index != 0 )
00161         delete[] y_index;
00162 
00163     if( xSeed   != 0 ) delete xSeed  ;
00164     if( xaSeed  != 0 ) delete xaSeed ;
00165     if( pSeed   != 0 ) delete pSeed  ;
00166     if( uSeed   != 0 ) delete uSeed  ;
00167     if( wSeed   != 0 ) delete wSeed  ;
00168 
00169     if( bSeed   != 0 ) delete bSeed  ;
00170 
00171     if( xSeed2  != 0 ) delete xSeed2 ;
00172     if( xaSeed2 != 0 ) delete xaSeed2;
00173     if( pSeed2  != 0 ) delete pSeed2 ;
00174     if( uSeed2  != 0 ) delete uSeed2 ;
00175     if( wSeed2  != 0 ) delete wSeed2 ;
00176 
00177     if( bSeed2  != 0 ) delete bSeed2 ;
00178 }
00179 
00180 
00181 ObjectiveElement& ObjectiveElement::operator=( const ObjectiveElement& rhs ){
00182 
00183     int run1;
00184 
00185     if( this != &rhs ){
00186 
00187         if( y_index != 0 )
00188             delete[] y_index;
00189 
00190         if( xSeed   != 0 ) delete xSeed  ;
00191         if( xaSeed  != 0 ) delete xaSeed ;
00192         if( pSeed   != 0 ) delete pSeed  ;
00193         if( uSeed   != 0 ) delete uSeed  ;
00194         if( wSeed   != 0 ) delete wSeed  ;
00195 
00196         if( bSeed   != 0 ) delete bSeed  ;
00197 
00198         if( xSeed2  != 0 ) delete xSeed2 ;
00199         if( xaSeed2 != 0 ) delete xaSeed2;
00200         if( pSeed2  != 0 ) delete pSeed2 ;
00201         if( uSeed2  != 0 ) delete uSeed2 ;
00202         if( wSeed2  != 0 ) delete wSeed2 ;
00203 
00204         if( bSeed2  != 0 ) delete bSeed2 ;
00205 
00206         grid = rhs.grid;
00207         fcn  = rhs.fcn ;
00208 
00209         if( rhs.y_index != 0 ){
00210             y_index = new int[ny];
00211             for( run1 = 0; run1 < ny; run1++ )
00212                 y_index[run1] = rhs.y_index[run1];
00213         }
00214         else  y_index = 0;
00215 
00216         t_index = rhs.t_index;
00217 
00218         nx = rhs.nx;
00219         na = rhs.na;
00220         nu = rhs.nu;
00221         np = rhs.np;
00222         nw = rhs.nw;
00223         ny = rhs.ny;
00224 
00225         if( rhs.xSeed != 0 )    xSeed   = new BlockMatrix(*rhs.xSeed  );
00226         else                    xSeed   = 0                            ;
00227         if( rhs.xaSeed != 0 )   xaSeed  = new BlockMatrix(*rhs.xaSeed );
00228         else                    xaSeed  = 0                            ;
00229         if( rhs.pSeed != 0 )    pSeed   = new BlockMatrix(*rhs.pSeed  );
00230         else                    pSeed   = 0                            ;
00231         if( rhs.uSeed != 0 )    uSeed   = new BlockMatrix(*rhs.uSeed  );
00232         else                    uSeed   = 0                            ;
00233         if( rhs.wSeed != 0 )    wSeed   = new BlockMatrix(*rhs.wSeed  );
00234         else                    wSeed   = 0                            ;
00235 
00236         if( rhs.bSeed != 0 )    bSeed   = new BlockMatrix(*rhs.bSeed  );
00237         else                    bSeed   = 0                            ;
00238 
00239         if( rhs.xSeed2 != 0 )   xSeed2  = new BlockMatrix(*rhs.xSeed2 );
00240         else                    xSeed2  = 0                            ;
00241         if( rhs.xaSeed2 != 0 )  xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
00242         else                    xaSeed2 = 0                            ;
00243         if( rhs.pSeed2 != 0 )   pSeed2  = new BlockMatrix(*rhs.pSeed2 );
00244         else                    pSeed2  = 0                            ;
00245         if( rhs.uSeed2 != 0 )   uSeed2  = new BlockMatrix(*rhs.uSeed2 );
00246         else                    uSeed2  = 0                            ;
00247         if( rhs.wSeed2 != 0 )   wSeed2  = new BlockMatrix(*rhs.wSeed2 );
00248         else                    wSeed2  = 0                            ;
00249 
00250         if( rhs.bSeed != 0 )    bSeed2  = new BlockMatrix(*rhs.bSeed  );
00251         else                    bSeed2  = 0                            ;
00252 
00253         obj       = rhs.obj      ;
00254         dForward  = rhs.dForward ;
00255         dBackward = rhs.dBackward;
00256     }
00257     return *this;
00258 }
00259 
00260 
00261 
00262 
00263 returnValue ObjectiveElement::init( const OCPiterate &x ){
00264 
00265     int run1;
00266 
00267     z.init( fcn, x );
00268     JJ.init( fcn, x );
00269         //HH.init( fcn, x );
00270 
00271     if( x.x != NULL ) nx = x.x->getNumValues();
00272     else             nx = 0                 ;
00273 
00274     if( x.xa != NULL ) na = x.xa->getNumValues();
00275     else             na = 0                 ;
00276 
00277     if( x.p  != NULL ) np = x.p ->getNumValues();
00278     else             np = 0                 ;
00279 
00280     if( x.u  != NULL ) nu = x.u ->getNumValues();
00281     else             nu = 0                 ;
00282 
00283     if( x.w  != NULL ) nw = x.w ->getNumValues();
00284     else             nw = 0                 ;
00285 
00286     ny = nx+na+nu+np+nw;
00287 
00288     if( y_index != 0 )  delete[] y_index;
00289         y_index       = new      int[ny];
00290 
00291     for( run1 = 0; run1 < nx; run1++ )
00292         y_index[run1] = fcn.index( VT_DIFFERENTIAL_STATE, run1 );
00293 
00294     for( run1 = 0; run1 < na; run1++ )
00295         y_index[nx+run1] = fcn.index( VT_ALGEBRAIC_STATE, run1 );
00296 
00297     for( run1 = 0; run1 < np; run1++ )
00298         y_index[nx+na+run1] = fcn.index( VT_PARAMETER, run1 );
00299 
00300     for( run1 = 0; run1 < nu; run1++ )
00301         y_index[nx+na+np+run1] = fcn.index( VT_CONTROL, run1 );
00302 
00303     for( run1 = 0; run1 < nw; run1++ )
00304         y_index[nx+na+np+nu+run1] = fcn.index( VT_DISTURBANCE, run1 );
00305 
00306     t_index = fcn.index( VT_TIME, 0 );
00307 
00308     return SUCCESSFUL_RETURN;
00309 }
00310 
00311 
00312 
00313 returnValue ObjectiveElement::setForwardSeed( BlockMatrix *xSeed_,
00314                                               BlockMatrix *xaSeed_,
00315                                               BlockMatrix *pSeed_,
00316                                               BlockMatrix *uSeed_,
00317                                               BlockMatrix *wSeed_,
00318                                               int          order    ){
00319 
00320     if( order == 1 ){
00321 
00322         if( xSeed_ != 0 ){
00323             if( xSeed != 0 ) delete xSeed;
00324             xSeed = new BlockMatrix(*xSeed_);
00325         }
00326         else{
00327             if( xSeed != 0 ) delete xSeed;
00328             xSeed = 0;
00329         }
00330         if( xaSeed_ != 0 ){
00331             if( xaSeed != 0 ) delete xaSeed;
00332             xaSeed = new BlockMatrix(*xaSeed_);
00333         }
00334         else{
00335             if( xaSeed != 0 ) delete xaSeed;
00336             xaSeed = 0;
00337         }
00338         if( pSeed_ != 0 ){
00339             if( pSeed != 0 ) delete pSeed;
00340             pSeed = new BlockMatrix(*pSeed_);
00341         }
00342         else{
00343             if( pSeed != 0 ) delete pSeed;
00344             pSeed = 0;
00345         }
00346         if( uSeed_ != 0 ){
00347             if( uSeed != 0 ) delete uSeed;
00348             uSeed = new BlockMatrix(*uSeed_);
00349         }
00350         else{
00351             if( uSeed != 0 ) delete uSeed;
00352             uSeed = 0;
00353         }
00354         if( wSeed_ != 0 ){
00355             if( wSeed != 0 ) delete wSeed;
00356             wSeed = new BlockMatrix(*wSeed_);
00357         }
00358         else{
00359             if( wSeed != 0 ) delete wSeed;
00360             wSeed = 0;
00361         }
00362 
00363         return SUCCESSFUL_RETURN;
00364     }
00365     if( order == 2 ){
00366 
00367         if( xSeed_ != 0 ){
00368             if( xSeed2 != 0 ) delete xSeed2;
00369             xSeed2 = new BlockMatrix(*xSeed_);
00370         }
00371         else{
00372             if( xSeed2 != 0 ) delete xSeed2;
00373             xSeed2 = 0;
00374         }
00375         if( xaSeed_ != 0 ){
00376             if( xaSeed2 != 0 ) delete xaSeed2;
00377             xaSeed2 = new BlockMatrix(*xaSeed_);
00378         }
00379         else{
00380             if( xaSeed2 != 0 ) delete xaSeed2;
00381             xaSeed2 = 0;
00382         }
00383         if( pSeed_ != 0 ){
00384             if( pSeed2 != 0 ) delete pSeed2;
00385             pSeed2 = new BlockMatrix(*pSeed_);
00386         }
00387         else{
00388             if( pSeed2 != 0 ) delete pSeed2;
00389             pSeed2 = 0;
00390         }
00391         if( uSeed_ != 0 ){
00392             if( uSeed2 != 0 ) delete uSeed2;
00393             uSeed2 = new BlockMatrix(*uSeed_);
00394         }
00395         else{
00396             if( uSeed2 != 0 ) delete uSeed2;
00397             uSeed2 = 0;
00398         }
00399         if( wSeed_ != 0 ){
00400             if( wSeed2 != 0 ) delete wSeed2;
00401             wSeed2 = new BlockMatrix(*wSeed_);
00402         }
00403         else{
00404             if( wSeed2 != 0 ) delete wSeed2;
00405             wSeed2 = 0;
00406         }
00407         return SUCCESSFUL_RETURN;
00408     }
00409 
00410     return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00411 }
00412 
00413 
00414 returnValue ObjectiveElement::setBackwardSeed( BlockMatrix *seed, int order ){
00415 
00416     if( order == 1 ){
00417 
00418         if( seed != 0 ){
00419             if( bSeed != 0 ) delete bSeed;
00420             bSeed = new BlockMatrix(*seed);
00421         }
00422         else{
00423             if( bSeed != 0 ) delete bSeed;
00424             bSeed = 0;
00425         }
00426 
00427         return SUCCESSFUL_RETURN;
00428     }
00429     if( order == 2 ){
00430 
00431         if( seed != 0 ){
00432             if( bSeed2 != 0 ) delete bSeed2;
00433             bSeed2 = new BlockMatrix(*seed);
00434         }
00435         else{
00436             if( bSeed2 != 0 ) delete bSeed2;
00437             bSeed2 = 0;
00438         }
00439         return SUCCESSFUL_RETURN;
00440     }
00441 
00442     return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00443 }
00444 
00445 
00446 returnValue ObjectiveElement::getObjectiveValue( double &objectiveValue ){
00447 
00448     objectiveValue = obj;
00449     return SUCCESSFUL_RETURN;
00450 }
00451 
00452 
00453 returnValue ObjectiveElement::getForwardSensitivities( BlockMatrix *D, int order ){
00454 
00455     ASSERT( D != 0 );
00456 
00457     if( order == 1 ){
00458 
00459         D[0] = dForward;
00460         return SUCCESSFUL_RETURN;
00461     }
00462     if( order == 2 ){
00463 
00464         return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00465     }
00466     return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00467 }
00468 
00469 
00470 returnValue ObjectiveElement::getBackwardSensitivities( BlockMatrix *D, int order ){
00471 
00472     ASSERT( D != 0 );
00473 
00474     if( order == 1 ){
00475         D[0] = dBackward;
00476         return SUCCESSFUL_RETURN;
00477     }
00478     if( order == 2 ){
00479 
00480         return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00481     }
00482     return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00483 }
00484 
00485 
00486 
00487 CLOSE_NAMESPACE_ACADO
00488 
00489 // end of file.


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