mayer_term.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/mayer_term.hpp>
00035 
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 
00042 //
00043 // PUBLIC MEMBER FUNCTIONS:
00044 //
00045 
00046 
00047 MayerTerm::MayerTerm( ){ }
00048 
00049 
00050 MayerTerm::MayerTerm( const Grid &grid_, const Expression& arg )
00051           :ObjectiveElement(grid_){
00052 
00053     fcn << arg;
00054 }
00055 
00056 MayerTerm::MayerTerm( const Grid &grid_, const Function& arg )
00057           :ObjectiveElement(grid_){
00058 
00059     fcn = arg;
00060 }
00061 
00062 MayerTerm::MayerTerm( const MayerTerm& rhs )
00063           :ObjectiveElement(rhs){ }
00064 
00065 MayerTerm::~MayerTerm( ){ }
00066 
00067 
00068 MayerTerm& MayerTerm::operator=( const MayerTerm& rhs ){
00069 
00070     if( this != &rhs ){
00071 
00072         ObjectiveElement::operator=(rhs);
00073     }
00074     return *this;
00075 }
00076 
00077 
00078 
00079 returnValue MayerTerm::evaluate( const OCPiterate &x ){
00080 
00081     ObjectiveElement::init( x );
00082 
00083     z.setZ( grid.getLastIndex(), x );
00084 
00085     DVector objective = fcn.evaluate( z );
00086 
00087     obj = objective(0);
00088 
00089     return SUCCESSFUL_RETURN;
00090 }
00091 
00092 
00093 returnValue MayerTerm::evaluateSensitivities( BlockMatrix *hessian ){
00094 
00095     int run1, run2;
00096     const int N = grid.getNumPoints();
00097 
00098     if( (bSeed != 0) & (hessian == 0) ){
00099 
00100         DMatrix bseed_;
00101         bSeed->getSubBlock( 0, 0, bseed_);
00102 
00103         fcn.AD_backward( bseed_.getRow(0), JJ );
00104 
00105         dBackward.init( 1, 5*N );
00106 
00107         DMatrix tmp1 = JJ.getX(); tmp1.transposeInPlace();
00108         if( nx > 0 ) dBackward.setDense( 0,   N-1, tmp1 );
00109         DMatrix tmp2 = JJ.getXA(); tmp2.transposeInPlace();
00110         if( na > 0 ) dBackward.setDense( 0, 2*N-1, tmp2 );
00111         DMatrix tmp3 = JJ.getP(); tmp3.transposeInPlace();
00112         if( np > 0 ) dBackward.setDense( 0, 3*N-1, tmp3 );
00113         DMatrix tmp4 = JJ.getU(); tmp4.transposeInPlace();
00114         if( nu > 0 ) dBackward.setDense( 0, 4*N-1, tmp4 );
00115         DMatrix tmp5 = JJ.getW(); tmp5.transposeInPlace();
00116         if( nw > 0 ) dBackward.setDense( 0, 5*N-1, tmp5 );
00117 
00118         return SUCCESSFUL_RETURN;
00119     }
00120 
00121     if( hessian != 0 ){
00122 
00123         double  bseed1 = 1.0;
00124         double  bseed2 = 0.0;
00125         double *J      = new double[fcn.getNumberOfVariables() +1];
00126         double *H      = new double[fcn.getNumberOfVariables() +1];
00127         double *fseed  = new double[fcn.getNumberOfVariables() +1];
00128 
00129         for( run1 = 0; run1 < fcn.getNumberOfVariables()+1; run1++ )
00130             fseed[run1] = 0.0;
00131 
00132         dBackward.init( 1, 5*N );
00133 
00134         DMatrix Dx ( 1, nx );
00135         DMatrix Dxa( 1, na );
00136         DMatrix Dp ( 1, np );
00137         DMatrix Du ( 1, nu );
00138         DMatrix Dw ( 1, nw );
00139 
00140         DMatrix Hx ( nx, nx );
00141         DMatrix Hxa( nx, na );
00142         DMatrix Hp ( nx, np );
00143         DMatrix Hu ( nx, nu );
00144         DMatrix Hw ( nx, nw );
00145 
00146         for( run2 = 0; run2 < nx; run2++ ){
00147 
00148             // FIRST ORDER DERIVATIVES:
00149             // ------------------------
00150             fseed[y_index[run2]] = 1.0;
00151             fcn.AD_forward( 0, fseed, J );
00152             Dx( 0, run2 ) = J[0];
00153             fseed[y_index[run2]] = 0.0;
00154 
00155             // SECOND ORDER DERIVATIVES:
00156             // -------------------------
00157             for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
00158                 J[run1] = 0.0;
00159                 H[run1] = 0.0;
00160             }
00161 
00162             fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
00163 
00164             for( run1 = 0; run1 < nx; run1++ ){
00165                  Hx( run2, run1 ) = H[y_index[run1]];
00166             }
00167             for( run1 = nx; run1 < nx+na; run1++ ){
00168                  Hxa( run2, run1-nx ) = H[y_index[run1]];
00169             }
00170             for( run1 = nx+na; run1 < nx+na+np; run1++ ){
00171                  Hp( run2, run1-nx-na ) = H[y_index[run1]];
00172             }
00173             for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
00174                  Hu( run2, run1-nx-na-np ) = H[y_index[run1]];
00175             }
00176             for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
00177                  Hw( run2, run1-nx-na-np-nu ) = H[y_index[run1]];
00178             }
00179         }
00180 
00181         if( nx > 0 ){
00182 
00183             dBackward.setDense( 0, N-1, Dx );
00184 
00185             if( nx > 0 ) hessian->setDense( N-1,   N-1, Hx  );
00186             if( na > 0 ) hessian->setDense( N-1, 2*N-1, Hxa );
00187             if( np > 0 ) hessian->setDense( N-1, 3*N-1, Hp  );
00188             if( nu > 0 ) hessian->setDense( N-1, 4*N-1, Hu  );
00189             if( nw > 0 ) hessian->setDense( N-1, 5*N-1, Hw  );
00190         }
00191 
00192         Hx.init ( na, nx );
00193         Hxa.init( na, na );
00194         Hp.init ( na, np );
00195         Hu.init ( na, nu );
00196         Hw.init ( na, nw );
00197 
00198         for( run2 = nx; run2 < nx+na; run2++ ){
00199 
00200             // FIRST ORDER DERIVATIVES:
00201             // ------------------------
00202             fseed[y_index[run2]] = 1.0;
00203             fcn.AD_forward( 0, fseed, J );
00204             Dxa( 0, run2-nx ) = J[0];
00205             fseed[y_index[run2]] = 0.0;
00206 
00207             // SECOND ORDER DERIVATIVES:
00208             // -------------------------
00209             for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
00210                 J[run1] = 0.0;
00211                 H[run1] = 0.0;
00212             }
00213 
00214             fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
00215 
00216             for( run1 = 0; run1 < nx; run1++ ){
00217                  Hx( run2-nx, run1 ) = H[y_index[run1]];
00218             }
00219             for( run1 = nx; run1 < nx+na; run1++ ){
00220                  Hxa( run2-nx, run1-nx ) = H[y_index[run1]];
00221             }
00222             for( run1 = nx+na; run1 < nx+na+np; run1++ ){
00223                  Hp( run2-nx, run1-nx-na ) = H[y_index[run1]];
00224             }
00225             for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
00226                  Hu( run2-nx, run1-nx-na-np ) = H[y_index[run1]];
00227             }
00228             for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
00229                  Hw( run2-nx, run1-nx-na-np-nu ) = H[y_index[run1]];
00230             }
00231         }
00232 
00233         if( na > 0 ){
00234 
00235             dBackward.setDense( 0, 2*N-1, Dxa );
00236 
00237             if( nx > 0 ) hessian->setDense( 2*N-1,   N-1, Hx  );
00238             if( na > 0 ) hessian->setDense( 2*N-1, 2*N-1, Hxa );
00239             if( np > 0 ) hessian->setDense( 2*N-1, 3*N-1, Hp  );
00240             if( nu > 0 ) hessian->setDense( 2*N-1, 4*N-1, Hu  );
00241             if( nw > 0 ) hessian->setDense( 2*N-1, 5*N-1, Hw  );
00242         }
00243 
00244         Hx.init ( np, nx );
00245         Hxa.init( np, na );
00246         Hp.init ( np, np );
00247         Hu.init ( np, nu );
00248         Hw.init ( np, nw );
00249 
00250         for( run2 = nx+na; run2 < nx+na+np; run2++ ){
00251 
00252             // FIRST ORDER DERIVATIVES:
00253             // ------------------------
00254             fseed[y_index[run2]] = 1.0;
00255             fcn.AD_forward( 0, fseed, J );
00256             Dp( 0, run2-nx-na ) = J[0];
00257             fseed[y_index[run2]] = 0.0;
00258 
00259             // SECOND ORDER DERIVATIVES:
00260             // -------------------------
00261             for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
00262                 J[run1] = 0.0;
00263                 H[run1] = 0.0;
00264             }
00265 
00266             fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
00267 
00268             for( run1 = 0; run1 < nx; run1++ ){
00269                  Hx( run2-nx-na, run1 ) = H[y_index[run1]];
00270             }
00271             for( run1 = nx; run1 < nx+na; run1++ ){
00272                  Hxa( run2-nx-na, run1-nx ) = H[y_index[run1]];
00273             }
00274             for( run1 = nx+na; run1 < nx+na+np; run1++ ){
00275                  Hp( run2-nx-na, run1-nx-na ) = H[y_index[run1]];
00276             }
00277             for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
00278                  Hu( run2-nx-na, run1-nx-na-np ) = H[y_index[run1]];
00279             }
00280             for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
00281                  Hw( run2-nx-na, run1-nx-na-np-nu ) = H[y_index[run1]];
00282             }
00283         }
00284 
00285         if( np > 0 ){
00286 
00287             dBackward.setDense( 0, 3*N-1, Dp );
00288 
00289             if( nx > 0 ) hessian->setDense( 3*N-1,   N-1, Hx  );
00290             if( na > 0 ) hessian->setDense( 3*N-1, 2*N-1, Hxa );
00291             if( np > 0 ) hessian->setDense( 3*N-1, 3*N-1, Hp  );
00292             if( nu > 0 ) hessian->setDense( 3*N-1, 4*N-1, Hu  );
00293             if( nw > 0 ) hessian->setDense( 3*N-1, 5*N-1, Hw  );
00294         }
00295 
00296 
00297         Hx.init ( nu, nx );
00298         Hxa.init( nu, na );
00299         Hp.init ( nu, np );
00300         Hu.init ( nu, nu );
00301         Hw.init ( nu, nw );
00302 
00303         for( run2 = nx+na+np; run2 < nx+na+np+nu; run2++ ){
00304 
00305             // FIRST ORDER DERIVATIVES:
00306             // ------------------------
00307             fseed[y_index[run2]] = 1.0;
00308             fcn.AD_forward( 0, fseed, J );
00309             Du( 0, run2-nx-na-np ) = J[0];
00310             fseed[y_index[run2]] = 0.0;
00311 
00312             // SECOND ORDER DERIVATIVES:
00313             // -------------------------
00314             for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
00315                 J[run1] = 0.0;
00316                 H[run1] = 0.0;
00317             }
00318 
00319             fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
00320 
00321             for( run1 = 0; run1 < nx; run1++ ){
00322                  Hx( run2-nx-na-np, run1 ) = H[y_index[run1]];
00323             }
00324             for( run1 = nx; run1 < nx+na; run1++ ){
00325                  Hxa( run2-nx-na-np, run1-nx ) = H[y_index[run1]];
00326             }
00327             for( run1 = nx+na; run1 < nx+na+np; run1++ ){
00328                  Hp( run2-nx-na-np, run1-nx-na ) = H[y_index[run1]];
00329             }
00330             for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
00331                  Hu( run2-nx-na-np, run1-nx-na-np ) = H[y_index[run1]];
00332             }
00333             for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
00334                  Hw( run2-nx-na-np, run1-nx-na-np-nu ) = H[y_index[run1]];
00335             }
00336         }
00337 
00338         if( nu > 0 ){
00339 
00340             dBackward.setDense( 0, 4*N-1, Du );
00341 
00342             if( nx > 0 ) hessian->setDense( 4*N-1,   N-1, Hx  );
00343             if( na > 0 ) hessian->setDense( 4*N-1, 2*N-1, Hxa );
00344             if( np > 0 ) hessian->setDense( 4*N-1, 3*N-1, Hp  );
00345             if( nu > 0 ) hessian->setDense( 4*N-1, 4*N-1, Hu  );
00346             if( nw > 0 ) hessian->setDense( 4*N-1, 5*N-1, Hw  );
00347         }
00348 
00349 
00350         Hx.init ( nw, nx );
00351         Hxa.init( nw, na );
00352         Hp.init ( nw, np );
00353         Hu.init ( nw, nu );
00354         Hw.init ( nw, nw );
00355 
00356         for( run2 = nx+na+np+nu; run2 < nx+na+np+nu+nw; run2++ ){
00357 
00358             // FIRST ORDER DERIVATIVES:
00359             // ------------------------
00360             fseed[y_index[run2]] = 1.0;
00361             fcn.AD_forward( 0, fseed, J );
00362             Dw( 0, run2-nx-na-np-nu ) = J[0];
00363             fseed[y_index[run2]] = 0.0;
00364 
00365             // SECOND ORDER DERIVATIVES:
00366             // -------------------------
00367             for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
00368                 J[run1] = 0.0;
00369                 H[run1] = 0.0;
00370             }
00371 
00372             fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
00373 
00374             for( run1 = 0; run1 < nx; run1++ ){
00375                  Hx( run2-nx-na-np-nu, run1 ) = H[y_index[run1]];
00376             }
00377             for( run1 = nx; run1 < nx+na; run1++ ){
00378                  Hxa( run2-nx-na-np-nu, run1-nx ) = H[y_index[run1]];
00379             }
00380             for( run1 = nx+na; run1 < nx+na+np; run1++ ){
00381                  Hp( run2-nx-na-np-nu, run1-nx-na ) = H[y_index[run1]];
00382             }
00383             for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
00384                  Hu( run2-nx-na-np-nu, run1-nx-na-np ) = H[y_index[run1]];
00385             }
00386             for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
00387                  Hw( run2-nx-na-np-nu, run1-nx-na-np-nu ) = H[y_index[run1]];
00388             }
00389         }
00390 
00391         if( nw > 0 ){
00392 
00393             dBackward.setDense( 0, 5*N-1, Dw );
00394 
00395             if( nx > 0 ) hessian->setDense( 5*N-1,   N-1, Hx  );
00396             if( na > 0 ) hessian->setDense( 5*N-1, 2*N-1, Hxa );
00397             if( np > 0 ) hessian->setDense( 5*N-1, 3*N-1, Hp  );
00398             if( nu > 0 ) hessian->setDense( 5*N-1, 4*N-1, Hu  );
00399             if( nw > 0 ) hessian->setDense( 5*N-1, 5*N-1, Hw  );
00400         }
00401 
00402         delete[] J;
00403         delete[] H;
00404         delete[] fseed;
00405 
00406         return SUCCESSFUL_RETURN;
00407     }
00408 
00409     return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00410 }
00411 
00412 
00413 
00414 
00415 
00416 CLOSE_NAMESPACE_ACADO
00417 
00418 // end of file.


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