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


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