algebraic_consistency_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/algebraic_consistency_constraint.hpp>
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 //
00042 // PUBLIC MEMBER FUNCTIONS:
00043 //
00044 
00045 
00046 AlgebraicConsistencyConstraint::AlgebraicConsistencyConstraint( )
00047                                :ConstraintElement(){
00048 
00049     numberOfStages             = 0;
00050     counter                    = 0;
00051     numberOfDifferentialStates = 0;
00052     numberOfAlgebraicStates    = 0;
00053     breakPoints                = 0;
00054 }
00055 
00056 
00057 AlgebraicConsistencyConstraint::AlgebraicConsistencyConstraint( const Grid& grid_, const uint& numberOfStages_ )
00058                                :ConstraintElement(grid_, numberOfStages_, 0 ){
00059 
00060     numberOfStages             = numberOfStages_;
00061     counter                    = 0;
00062     numberOfDifferentialStates = new int[numberOfStages  ];
00063     numberOfAlgebraicStates    = new int[numberOfStages  ];
00064     breakPoints                = new int[numberOfStages+1];
00065 }
00066 
00067 
00068 AlgebraicConsistencyConstraint::AlgebraicConsistencyConstraint( const AlgebraicConsistencyConstraint& rhs )
00069                                :ConstraintElement(rhs){
00070 
00071     int run1;
00072 
00073     numberOfStages             = rhs.numberOfStages;
00074     counter                    = rhs.counter       ;
00075 
00076     if( numberOfStages != 0 ){
00077         numberOfDifferentialStates = new int[numberOfStages  ];
00078         numberOfAlgebraicStates    = new int[numberOfStages  ];
00079         breakPoints                = new int[numberOfStages+1];
00080         for( run1 = 0; run1 < counter; run1++ ){
00081             numberOfDifferentialStates[run1] = rhs.numberOfDifferentialStates[run1];
00082             numberOfAlgebraicStates   [run1] = rhs.numberOfAlgebraicStates   [run1];
00083         }
00084         for( run1 = 0; run1 < counter+1; run1++ )
00085             breakPoints[run1] = rhs.breakPoints[run1];
00086     }
00087     else{
00088         numberOfDifferentialStates = 0;
00089         numberOfAlgebraicStates    = 0;
00090         breakPoints                = 0;
00091     }
00092 }
00093 
00094 
00095 AlgebraicConsistencyConstraint::~AlgebraicConsistencyConstraint( ){
00096 
00097     if( numberOfDifferentialStates != 0 )
00098         delete[] numberOfDifferentialStates;
00099 
00100     if( numberOfAlgebraicStates != 0 )
00101         delete[] numberOfAlgebraicStates;
00102 
00103     if( breakPoints != 0 )
00104         delete[] breakPoints;
00105 }
00106 
00107 
00108 AlgebraicConsistencyConstraint& AlgebraicConsistencyConstraint::operator=( const AlgebraicConsistencyConstraint& rhs ){
00109 
00110     int run1;
00111 
00112     if( this != &rhs ){
00113 
00114         if( numberOfDifferentialStates != 0 )
00115             delete[] numberOfDifferentialStates;
00116 
00117         if( numberOfAlgebraicStates != 0 )
00118             delete[] numberOfAlgebraicStates;
00119 
00120         if( breakPoints != 0 )
00121             delete[] breakPoints;
00122 
00123         ConstraintElement::operator=(rhs);
00124 
00125         numberOfStages             = rhs.numberOfStages;
00126         counter                    = rhs.counter       ;
00127 
00128         if( numberOfStages != 0 ){
00129             numberOfDifferentialStates = new int[numberOfStages  ];
00130             numberOfAlgebraicStates    = new int[numberOfStages  ];
00131             breakPoints                = new int[numberOfStages+1];
00132             for( run1 = 0; run1 < counter; run1++ ){
00133                 numberOfDifferentialStates[run1] = rhs.numberOfDifferentialStates[run1];
00134                 numberOfAlgebraicStates   [run1] = rhs.numberOfAlgebraicStates   [run1];
00135             }
00136             for( run1 = 0; run1 < counter+1; run1++ )
00137                 breakPoints[run1] = rhs.breakPoints[run1];
00138         }
00139         else{
00140             numberOfDifferentialStates = 0;
00141             numberOfAlgebraicStates    = 0;
00142             breakPoints                = 0;
00143         }
00144     }
00145     return *this;
00146 }
00147 
00148 
00149 
00150 returnValue AlgebraicConsistencyConstraint::evaluate( const OCPiterate& iter ){
00151 
00152     int run1, run2;
00153 
00154     if( fcn            == 0       ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00155     if( numberOfStages != counter ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00156 
00157     int stageIdx = 0;
00158     const int T  = grid.getLastIndex();
00159 
00160     residuumL.init(T+1,1);
00161     residuumU.init(T+1,1);
00162 
00163 
00164     for( run1 = 0; run1 <= T; run1++ ){
00165 
00166         int nc = numberOfAlgebraicStates[stageIdx];
00167                 DMatrix res( nc, 1 );
00168                 
00169                 z[stageIdx].setZ( run1, iter );
00170                 DVector result = fcn[stageIdx].evaluate( z[stageIdx],run1 );
00171 
00172         for( run2 = 0; run2 < nc; run2++ )
00173              res( run2, 0 ) = -result(numberOfDifferentialStates[stageIdx]+run2);
00174 
00175         // STORE THE RESULTS:
00176         // ------------------
00177         residuumL.setDense( run1, 0, res );
00178         residuumU.setDense( run1, 0, res );
00179 
00180         // INCREASE THE STAGE COUNTER IF NECESSARY:
00181         // ----------------------------------------
00182         if( run1 == breakPoints[stageIdx+1] ) stageIdx++;
00183     }
00184 
00185     return SUCCESSFUL_RETURN;
00186 }
00187 
00188 
00189 returnValue AlgebraicConsistencyConstraint::evaluateSensitivities(){
00190 
00191 
00192     // EVALUATION OF THE SENSITIVITIES:
00193     // --------------------------------
00194 
00195     int run1=0, run3;
00196     returnValue returnvalue;
00197 
00198     if( fcn            == 0       ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00199     if( numberOfStages != counter ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00200 
00201     int stageIdx = 0;
00202     const int N  = grid.getNumPoints();
00203 
00204 
00205     if( bSeed != 0 )
00206         {
00207 
00208         if( xSeed  != 0 || xaSeed  != 0 || pSeed  != 0 || uSeed  != 0 || wSeed  != 0 ||
00209             xSeed2 != 0 || xaSeed2 != 0 || pSeed2 != 0 || uSeed2 != 0 || wSeed2 != 0 )
00210             return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
00211 
00212                 int nBDirs;
00213 
00214         dBackward.init( N, 5*N );
00215 
00216 
00217         for( run3 = 0; run3 < N; run3++ )
00218                 {
00219             DMatrix bseed_;
00220             bSeed->getSubBlock( 0, run3, bseed_ );
00221 
00222                         DVector bseedTmp( numberOfDifferentialStates[stageIdx] );
00223                         bseedTmp.setZero();
00224 
00225             nBDirs = bSeed->getNumRows( 0, run3 );
00226 
00227             DMatrix Dx ( nBDirs, nx );
00228             DMatrix Dxa( nBDirs, na );
00229             DMatrix Dp ( nBDirs, np );
00230             DMatrix Du ( nBDirs, nu );
00231             DMatrix Dw ( nBDirs, nw );
00232 
00233             for( run1 = 0; run1 < nBDirs; run1++ )
00234                         {
00235                                 bseedTmp.append( bseed_.getRow(run1) );
00236                                 ACADO_TRY( fcn[stageIdx].AD_backward( bseedTmp,JJ[stageIdx],run3 ) );
00237                 
00238                                 if( nx > 0 ) Dx .setRow( run1, JJ[stageIdx].getX () );
00239                                 if( na > 0 ) Dxa.setRow( run1, JJ[stageIdx].getXA() );
00240                                 if( np > 0 ) Dp .setRow( run1, JJ[stageIdx].getP () );
00241                                 if( nu > 0 ) Du .setRow( run1, JJ[stageIdx].getU () );
00242                                 if( nw > 0 ) Dw .setRow( run1, JJ[stageIdx].getW () );
00243                                 
00244                                 JJ[stageIdx].setZero( );
00245                         }
00246 
00247                         if( nx > 0 )
00248                                 dBackward.setDense( run3,     run3, Dx );
00249 
00250                         if( na > 0 )
00251                                 dBackward.setDense( run3,   N+run3, Dxa );
00252 
00253                         if( np > 0 )
00254                                 dBackward.setDense( run3, 2*N+run3, Dp );
00255 
00256                         if( nu > 0 )
00257                                 dBackward.setDense( run3, 3*N+run3, Du );
00258 
00259                         if( nw > 0 )
00260                                 dBackward.setDense( run3, 4*N+run3, Dw );
00261 
00262                         if( run3 == breakPoints[stageIdx+1] ) stageIdx++;
00263                 }
00264 
00265                 return SUCCESSFUL_RETURN;
00266         }
00267 
00268 
00269     // COMPUTATION OF DIRECTIONAL SENSITIVITIES IN FORWARD MODE:
00270     // ---------------------------------------------------------
00271 
00272     if( xSeed  != 0 || xaSeed  != 0 || pSeed  != 0 || uSeed  != 0 || wSeed  != 0 ){
00273 
00274         if( bSeed    != 0         ) return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
00275         if( condType != CT_SPARSE ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET       );
00276 
00277         dForward.init( N, 5*N );
00278 
00279         for( run3 = 0; run3 < N; run3++ ){
00280 
00281             if( xSeed != 0 ){
00282                 DMatrix tmp;
00283                 xSeed->getSubBlock(0,0,tmp);
00284                 returnvalue = computeForwardSensitivityBlock( run3,     run3, 0, stageIdx, &tmp );
00285                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00286             }
00287 
00288             if( xaSeed != 0 ){
00289                 DMatrix tmp;
00290                 xaSeed->getSubBlock(0,0,tmp);
00291                 returnvalue = computeForwardSensitivityBlock( run3, 1*N+run3, nx, stageIdx, &tmp );
00292                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00293             }
00294 
00295             if( pSeed != 0 ){
00296                 DMatrix tmp;
00297                 pSeed->getSubBlock(0,0,tmp);
00298                 returnvalue = computeForwardSensitivityBlock( run3, 2*N+run3, nx+na, stageIdx, &tmp );
00299                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00300             }
00301 
00302             if( uSeed != 0 ){
00303                 DMatrix tmp;
00304                 uSeed->getSubBlock(0,0,tmp);
00305                 returnvalue = computeForwardSensitivityBlock( run3, 3*N+run3, nx+na+np, stageIdx, &tmp );
00306                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00307             }
00308 
00309             if( wSeed != 0 ){
00310                 DMatrix tmp;
00311                 wSeed->getSubBlock(0,0,tmp);
00312                 returnvalue = computeForwardSensitivityBlock( run3, 4*N+run3, nx+na+np+nu, stageIdx, &tmp );
00313                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00314             }
00315 
00316             if( run3 == breakPoints[stageIdx+1] ) stageIdx++;
00317         }
00318         return SUCCESSFUL_RETURN;
00319     }
00320 
00321 
00322     return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00323 }
00324 
00325 
00326 
00327 returnValue AlgebraicConsistencyConstraint::evaluateSensitivities( int &count,
00328                                                                    const BlockMatrix &seed_,
00329                                                                    BlockMatrix &hessian ){
00330 
00331     int run3;
00332     const int N  = grid.getNumPoints();
00333     if( fcn == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00334     if( numberOfStages != counter ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00335 
00336     int stageIdx = 0;
00337 
00338     dBackward.init( N, 5*N );
00339 
00340     for( run3 = 0; run3 < N; run3++ ){
00341 
00342         int nc = numberOfAlgebraicStates[stageIdx];
00343 
00344         DMatrix seed;
00345         seed_.getSubBlock( count, 0, seed, nc, 1 );
00346         count++;
00347 
00348         // EVALUATION OF THE SENSITIVITIES:
00349         // --------------------------------
00350 
00351         int run1, run2;
00352 
00353         double *bseed1 = new double[fcn[stageIdx].getDim()];
00354         double *bseed2 = new double[fcn[stageIdx].getDim()];
00355         double *R      = new double[fcn[stageIdx].getDim()];
00356         double *J      = new double[fcn[stageIdx].getNumberOfVariables() +1];
00357         double *H      = new double[fcn[stageIdx].getNumberOfVariables() +1];
00358         double *fseed  = new double[fcn[stageIdx].getNumberOfVariables() +1];
00359 
00360         for( run1 = 0; run1 < fcn[stageIdx].getDim()-nc; run1++ ){
00361             bseed1[run1] = 0.0;
00362             bseed2[run1] = 0.0;
00363         }
00364 
00365         for( run1 = 0; run1 < nc; run1++ ){
00366             bseed1[fcn[stageIdx].getDim() - nc + run1] = seed(run1,0);
00367             bseed2[fcn[stageIdx].getDim() - nc + run1] = 0.0;
00368         }
00369 
00370         for( run1 = 0; run1 < fcn[stageIdx].getNumberOfVariables()+1; run1++ )
00371             fseed[run1] = 0.0;
00372 
00373         DMatrix Dx ( nc, nx );
00374         DMatrix Dxa( nc, na );
00375         DMatrix Dp ( nc, np );
00376         DMatrix Du ( nc, nu );
00377         DMatrix Dw ( nc, nw );
00378 
00379         DMatrix Hx ( nx, nx );
00380         DMatrix Hxa( nx, na );
00381         DMatrix Hp ( nx, np );
00382         DMatrix Hu ( nx, nu );
00383         DMatrix Hw ( nx, nw );
00384 
00385         for( run2 = 0; run2 < nx; run2++ ){
00386 
00387             // FIRST ORDER DERIVATIVES:
00388             // ------------------------
00389             fseed[y_index[stageIdx][run2]] = 1.0;
00390             fcn[stageIdx].AD_forward( run3, fseed, R );
00391             for( run1 = 0; run1 < nc; run1++ )
00392                 Dx( run1, run2 ) = R[fcn[stageIdx].getDim() - nc + run1];
00393             fseed[y_index[stageIdx][run2]] = 0.0;
00394 
00395             // SECOND ORDER DERIVATIVES:
00396             // -------------------------
00397             for( run1 = 0; run1 <= fcn[stageIdx].getNumberOfVariables(); run1++ ){
00398                 J[run1] = 0.0;
00399                 H[run1] = 0.0;
00400             }
00401 
00402             fcn[stageIdx].AD_backward2( run3, bseed1, bseed2, J, H );
00403 
00404             for( run1 = 0          ; run1 < nx            ; run1++ ) Hx ( run2, run1             ) = -H[y_index[stageIdx][run1]];
00405             for( run1 = nx         ; run1 < nx+na         ; run1++ ) Hxa( run2, run1-nx          ) = -H[y_index[stageIdx][run1]];
00406             for( run1 = nx+na      ; run1 < nx+na+np      ; run1++ ) Hp ( run2, run1-nx-na       ) = -H[y_index[stageIdx][run1]];
00407             for( run1 = nx+na+np   ; run1 < nx+na+np+nu   ; run1++ ) Hu ( run2, run1-nx-na-np    ) = -H[y_index[stageIdx][run1]];
00408             for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2, run1-nx-na-np-nu ) = -H[y_index[stageIdx][run1]];
00409         }
00410 
00411         if( nx > 0 ){
00412 
00413             dBackward.setDense( run3, run3, Dx );
00414 
00415             if( nx > 0 ) hessian.addDense( run3,       run3, Hx  );
00416             if( na > 0 ) hessian.addDense( run3,   N + run3, Hxa );
00417             if( np > 0 ) hessian.addDense( run3, 2*N + run3, Hp  );
00418             if( nu > 0 ) hessian.addDense( run3, 3*N + run3, Hu  );
00419             if( nw > 0 ) hessian.addDense( run3, 4*N + run3, Hw  );
00420         }
00421 
00422         Hx.init ( na, nx );
00423         Hxa.init( na, na );
00424         Hp.init ( na, np );
00425         Hu.init ( na, nu );
00426         Hw.init ( na, nw );
00427 
00428         for( run2 = nx; run2 < nx+na; run2++ ){
00429 
00430             // FIRST ORDER DERIVATIVES:
00431             // ------------------------
00432             fseed[y_index[stageIdx][run2]] = 1.0;
00433             fcn[stageIdx].AD_forward( run3, fseed, R );
00434             for( run1 = 0; run1 < nc; run1++ )
00435                 Dxa( run1, run2-nx ) = R[fcn[stageIdx].getDim() - nc + run1];
00436             fseed[y_index[stageIdx][run2]] = 0.0;
00437 
00438             // SECOND ORDER DERIVATIVES:
00439             // -------------------------
00440             for( run1 = 0; run1 <= fcn[stageIdx].getNumberOfVariables(); run1++ ){
00441                 J[run1] = 0.0;
00442                 H[run1] = 0.0;
00443             }
00444 
00445             fcn[stageIdx].AD_backward2( run3, bseed1, bseed2, J, H );
00446 
00447             for( run1 = 0          ; run1 < nx            ; run1++ ) Hx ( run2-nx, run1             ) = -H[y_index[stageIdx][run1]];
00448             for( run1 = nx         ; run1 < nx+na         ; run1++ ) Hxa( run2-nx, run1-nx          ) = -H[y_index[stageIdx][run1]];
00449             for( run1 = nx+na      ; run1 < nx+na+np      ; run1++ ) Hp ( run2-nx, run1-nx-na       ) = -H[y_index[stageIdx][run1]];
00450             for( run1 = nx+na+np   ; run1 < nx+na+np+nu   ; run1++ ) Hu ( run2-nx, run1-nx-na-np    ) = -H[y_index[stageIdx][run1]];
00451             for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx, run1-nx-na-np-nu ) = -H[y_index[stageIdx][run1]];
00452         }
00453 
00454         if( na > 0 ){
00455 
00456             dBackward.setDense( run3, N+run3, Dxa );
00457 
00458             if( nx > 0 ) hessian.addDense( N+run3,       run3, Hx  );
00459             if( na > 0 ) hessian.addDense( N+run3,   N + run3, Hxa );
00460             if( np > 0 ) hessian.addDense( N+run3, 2*N + run3, Hp  );
00461             if( nu > 0 ) hessian.addDense( N+run3, 3*N + run3, Hu  );
00462             if( nw > 0 ) hessian.addDense( N+run3, 4*N + run3, Hw  );
00463         }
00464 
00465         Hx.init ( np, nx );
00466         Hxa.init( np, na );
00467         Hp.init ( np, np );
00468         Hu.init ( np, nu );
00469         Hw.init ( np, nw );
00470 
00471         for( run2 = nx+na; run2 < nx+na+np; run2++ ){
00472 
00473             // FIRST ORDER DERIVATIVES:
00474             // ------------------------
00475             fseed[y_index[stageIdx][run2]] = 1.0;
00476             fcn[stageIdx].AD_forward( run3, fseed, R );
00477             for( run1 = 0; run1 < nc; run1++ )
00478                 Dp( run1, run2-nx-na ) = R[fcn[stageIdx].getDim() - nc + run1];
00479             fseed[y_index[stageIdx][run2]] = 0.0;
00480 
00481             // SECOND ORDER DERIVATIVES:
00482             // -------------------------
00483             for( run1 = 0; run1 <= fcn[stageIdx].getNumberOfVariables(); run1++ ){
00484                 J[run1] = 0.0;
00485                 H[run1] = 0.0;
00486             }
00487 
00488             fcn[stageIdx].AD_backward2( run3, bseed1, bseed2, J, H );
00489 
00490             for( run1 = 0          ; run1 < nx            ; run1++ ) Hx ( run2-nx-na, run1             ) = -H[y_index[stageIdx][run1]];
00491             for( run1 = nx         ; run1 < nx+na         ; run1++ ) Hxa( run2-nx-na, run1-nx          ) = -H[y_index[stageIdx][run1]];
00492             for( run1 = nx+na      ; run1 < nx+na+np      ; run1++ ) Hp ( run2-nx-na, run1-nx-na       ) = -H[y_index[stageIdx][run1]];
00493             for( run1 = nx+na+np   ; run1 < nx+na+np+nu   ; run1++ ) Hu ( run2-nx-na, run1-nx-na-np    ) = -H[y_index[stageIdx][run1]];
00494             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[stageIdx][run1]];
00495         }
00496 
00497         if( np > 0 ){
00498 
00499             dBackward.setDense( run3, 2*N+run3, Dp );
00500 
00501             if( nx > 0 ) hessian.addDense( 2*N+run3,       run3, Hx  );
00502             if( na > 0 ) hessian.addDense( 2*N+run3,   N + run3, Hxa );
00503             if( np > 0 ) hessian.addDense( 2*N+run3, 2*N + run3, Hp  );
00504             if( nu > 0 ) hessian.addDense( 2*N+run3, 3*N + run3, Hu  );
00505             if( nw > 0 ) hessian.addDense( 2*N+run3, 4*N + run3, Hw  );
00506         }
00507 
00508 
00509         Hx.init ( nu, nx );
00510         Hxa.init( nu, na );
00511         Hp.init ( nu, np );
00512         Hu.init ( nu, nu );
00513         Hw.init ( nu, nw );
00514 
00515         for( run2 = nx+na+np; run2 < nx+na+np+nu; run2++ ){
00516 
00517             // FIRST ORDER DERIVATIVES:
00518             // ------------------------
00519             fseed[y_index[stageIdx][run2]] = 1.0;
00520             fcn[stageIdx].AD_forward( run3, fseed, R );
00521             for( run1 = 0; run1 < nc; run1++ )
00522                 Du( run1, run2-nx-na-np ) = R[fcn[stageIdx].getDim() - nc + run1];
00523             fseed[y_index[stageIdx][run2]] = 0.0;
00524 
00525             // SECOND ORDER DERIVATIVES:
00526             // -------------------------
00527             for( run1 = 0; run1 <= fcn[stageIdx].getNumberOfVariables(); run1++ ){
00528                 J[run1] = 0.0;
00529                 H[run1] = 0.0;
00530             }
00531 
00532             fcn[stageIdx].AD_backward2( run3, bseed1, bseed2, J, H );
00533 
00534             for( run1 = 0          ; run1 < nx            ; run1++ ) Hx ( run2-nx-na-np, run1             ) = -H[y_index[stageIdx][run1]];
00535             for( run1 = nx         ; run1 < nx+na         ; run1++ ) Hxa( run2-nx-na-np, run1-nx          ) = -H[y_index[stageIdx][run1]];
00536             for( run1 = nx+na      ; run1 < nx+na+np      ; run1++ ) Hp ( run2-nx-na-np, run1-nx-na       ) = -H[y_index[stageIdx][run1]];
00537             for( run1 = nx+na+np   ; run1 < nx+na+np+nu   ; run1++ ) Hu ( run2-nx-na-np, run1-nx-na-np    ) = -H[y_index[stageIdx][run1]];
00538             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[stageIdx][run1]];
00539         }
00540 
00541         if( nu > 0 ){
00542 
00543             dBackward.setDense( run3, 3*N+run3, Du );
00544 
00545             if( nx > 0 ) hessian.addDense( 3*N+run3,       run3, Hx  );
00546             if( na > 0 ) hessian.addDense( 3*N+run3,   N + run3, Hxa );
00547             if( np > 0 ) hessian.addDense( 3*N+run3, 2*N + run3, Hp  );
00548             if( nu > 0 ) hessian.addDense( 3*N+run3, 3*N + run3, Hu  );
00549             if( nw > 0 ) hessian.addDense( 3*N+run3, 4*N + run3, Hw  );
00550         }
00551 
00552         Hx.init ( nw, nx );
00553         Hxa.init( nw, na );
00554         Hp.init ( nw, np );
00555         Hu.init ( nw, nu );
00556         Hw.init ( nw, nw );
00557 
00558         for( run2 = nx+na+np+nu; run2 < nx+na+np+nu+nw; run2++ ){
00559 
00560             // FIRST ORDER DERIVATIVES:
00561             // ------------------------
00562             fseed[y_index[stageIdx][run2]] = 1.0;
00563             fcn[stageIdx].AD_forward( run3, fseed, R );
00564             for( run1 = 0; run1 < nc; run1++ )
00565                 Dw( run1, run2-nx-na-np-nu ) = R[fcn[stageIdx].getDim() - nc + run1];
00566             fseed[y_index[stageIdx][run2]] = 0.0;
00567 
00568             // SECOND ORDER DERIVATIVES:
00569             // -------------------------
00570             for( run1 = 0; run1 <= fcn[stageIdx].getNumberOfVariables(); run1++ ){
00571                 J[run1] = 0.0;
00572                 H[run1] = 0.0;
00573             }
00574 
00575             fcn[stageIdx].AD_backward2( run3, bseed1, bseed2, J, H );
00576 
00577             for( run1 = 0          ; run1 < nx            ; run1++ ) Hx ( run2-nx-na-np-nu, run1             ) = -H[y_index[stageIdx][run1]];
00578             for( run1 = nx         ; run1 < nx+na         ; run1++ ) Hxa( run2-nx-na-np-nu, run1-nx          ) = -H[y_index[stageIdx][run1]];
00579             for( run1 = nx+na      ; run1 < nx+na+np      ; run1++ ) Hp ( run2-nx-na-np-nu, run1-nx-na       ) = -H[y_index[stageIdx][run1]];
00580             for( run1 = nx+na+np   ; run1 < nx+na+np+nu   ; run1++ ) Hu ( run2-nx-na-np-nu, run1-nx-na-np    ) = -H[y_index[stageIdx][run1]];
00581             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[stageIdx][run1]];
00582         }
00583 
00584         if( nw > 0 ){
00585 
00586             dBackward.setDense( run3, 4*N+run3, Dw );
00587 
00588             if( nx > 0 ) hessian.addDense( 4*N+run3,       run3, Hx  );
00589             if( na > 0 ) hessian.addDense( 4*N+run3,   N + run3, Hxa );
00590             if( np > 0 ) hessian.addDense( 4*N+run3, 2*N + run3, Hp  );
00591             if( nu > 0 ) hessian.addDense( 4*N+run3, 3*N + run3, Hu  );
00592             if( nw > 0 ) hessian.addDense( 4*N+run3, 4*N + run3, Hw  );
00593         }
00594 
00595         delete[] bseed1;
00596         delete[] bseed2;
00597         delete[] R     ;
00598         delete[] J     ;
00599         delete[] H     ;
00600         delete[] fseed ;
00601 
00602         if( run3 == breakPoints[stageIdx+1] ) stageIdx++;
00603     }
00604 
00605     return SUCCESSFUL_RETURN;
00606 }
00607 
00608 
00609 
00610 //
00611 // PROTECTED MEMBER FUNCTIONS:
00612 //
00613 
00614 returnValue AlgebraicConsistencyConstraint::initializeEvaluationPoints( const OCPiterate& iter )
00615 {
00616         if ( z != 0 )
00617                 delete[] z;
00618 
00619         if ( JJ != 0 )
00620                 delete[] JJ;
00621 
00622         z  = new EvaluationPoint[nFcn];
00623         JJ = new EvaluationPoint[nFcn];
00624 
00625         int stageIdx = 0;
00626         for( int i=0; i<nFcn; ++i )
00627         {
00628                 z[i].init( fcn[i], iter );
00629                 JJ[i].init( fcn[i], iter );
00630 
00631                 if( i == breakPoints[stageIdx+1] )
00632                         stageIdx++;
00633         }
00634 
00635         return SUCCESSFUL_RETURN;
00636 }
00637 
00638 
00639 
00640 
00641 CLOSE_NAMESPACE_ACADO
00642 
00643 // end of file.


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