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


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