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


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