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


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