box_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 
00034 #include <acado/constraint/box_constraint.hpp>
00035 
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 
00040 
00041 //
00042 // PUBLIC MEMBER FUNCTIONS:
00043 //
00044 
00045 
00046 BoxConstraint::BoxConstraint( ){
00047 
00048     nb    = 0;
00049     var   = 0;
00050     index = 0;
00051     blb   = 0;
00052     bub   = 0;
00053 
00054     residuumXL  = 0;
00055     residuumXU  = 0;
00056     residuumXAL = 0;
00057     residuumXAU = 0;
00058     residuumPL  = 0;
00059     residuumPU  = 0;
00060     residuumUL  = 0;
00061     residuumUU  = 0;
00062     residuumWL  = 0;
00063     residuumWU  = 0;
00064 }
00065 
00066 
00067 returnValue BoxConstraint::init( const Grid& grid_ ){
00068 
00069     deleteAll();
00070 
00071     grid  = grid_;
00072 
00073     nb    = 0;
00074     var   = 0;
00075     index = 0;
00076     blb   = 0;
00077     bub   = 0;
00078 
00079     residuumXL  = new DMatrix[grid.getNumPoints()];
00080     residuumXU  = new DMatrix[grid.getNumPoints()];
00081     residuumXAL = new DMatrix[grid.getNumPoints()];
00082     residuumXAU = new DMatrix[grid.getNumPoints()];
00083     residuumPL  = new DMatrix[1                  ];
00084     residuumPU  = new DMatrix[1                  ];
00085     residuumUL  = new DMatrix[grid.getNumPoints()];
00086     residuumUU  = new DMatrix[grid.getNumPoints()];
00087     residuumWL  = new DMatrix[grid.getNumPoints()];
00088     residuumWU  = new DMatrix[grid.getNumPoints()];
00089 
00090     return SUCCESSFUL_RETURN;
00091 }
00092 
00093 
00094 
00095 BoxConstraint::BoxConstraint( const BoxConstraint& rhs ){
00096 
00097     int   run1    ;
00098     grid = rhs.grid;
00099 
00100     nb = rhs.nb;
00101     if( nb > 0 ){
00102         var   = (VariableType*)calloc(nb,sizeof(VariableType));
00103         index = (int*)calloc(nb,sizeof(int));
00104         blb   = (DVector**)calloc(nb,sizeof(DVector*));
00105         bub   = (DVector**)calloc(nb,sizeof(DVector*));
00106 
00107         for( run1 = 0; run1 < nb; run1++ ){
00108 
00109             var  [run1] = rhs.var  [run1];
00110             index[run1] = rhs.index[run1];
00111             blb  [run1] = new DVector(*rhs.blb[run1]);
00112             bub  [run1] = new DVector(*rhs.bub[run1]);
00113         }
00114     }
00115     else{
00116         var   = 0;
00117         index = 0;
00118         blb   = 0;
00119         bub   = 0;
00120     }
00121 
00122     residuumXL  = new DMatrix[grid.getNumPoints()];
00123     residuumXU  = new DMatrix[grid.getNumPoints()];
00124     residuumXAL = new DMatrix[grid.getNumPoints()];
00125     residuumXAU = new DMatrix[grid.getNumPoints()];
00126     residuumPL  = new DMatrix[1                  ];
00127     residuumPU  = new DMatrix[1                  ];
00128     residuumUL  = new DMatrix[grid.getNumPoints()];
00129     residuumUU  = new DMatrix[grid.getNumPoints()];
00130     residuumWL  = new DMatrix[grid.getNumPoints()];
00131     residuumWU  = new DMatrix[grid.getNumPoints()];
00132 }
00133 
00134 BoxConstraint::~BoxConstraint( ){
00135 
00136     deleteAll();
00137 }
00138 
00139 
00140 void BoxConstraint::deleteAll(){
00141 
00142     int run1;
00143 
00144     if(   var != 0 ) free(    var);
00145     if( index != 0 ) free(  index);
00146     if(   blb != 0 ){
00147         for( run1 = 0; run1 < nb; run1++  )  delete blb[run1];
00148         free(blb);
00149     }
00150     if(   bub != 0 ){
00151         for( run1 = 0; run1 < nb; run1++  )  delete bub[run1];
00152         free(bub);
00153     }
00154 
00155     if( residuumXL  != 0 ) delete[] residuumXL ;
00156     if( residuumXU  != 0 ) delete[] residuumXU ;
00157     if( residuumXAL != 0 ) delete[] residuumXAL;
00158     if( residuumXAU != 0 ) delete[] residuumXAU;
00159     if( residuumPL  != 0 ) delete[] residuumPL ;
00160     if( residuumPU  != 0 ) delete[] residuumPU ;
00161     if( residuumUL  != 0 ) delete[] residuumUL ;
00162     if( residuumUU  != 0 ) delete[] residuumUU ;
00163     if( residuumWL  != 0 ) delete[] residuumWL ;
00164     if( residuumWU  != 0 ) delete[] residuumWU ;
00165 }
00166 
00167 
00168 BoxConstraint& BoxConstraint::operator=( const BoxConstraint& rhs ){
00169 
00170     int run1;
00171 
00172     if( this != &rhs ){
00173 
00174         deleteAll();
00175 
00176         grid = rhs.grid;
00177 
00178         nb = rhs.nb;
00179         if( nb > 0 ){
00180             var   = (VariableType*)calloc(nb,sizeof(VariableType));
00181             index = (int*)calloc(nb,sizeof(int));
00182             blb   = (DVector**)calloc(nb,sizeof(DVector*));
00183             bub   = (DVector**)calloc(nb,sizeof(DVector*));
00184 
00185             for( run1 = 0; run1 < nb; run1++ ){
00186 
00187                 var  [run1] = rhs.var  [run1];
00188                 index[run1] = rhs.index[run1];
00189                 blb  [run1] = new DVector(*rhs.blb[run1]);
00190                 bub  [run1] = new DVector(*rhs.bub[run1]);
00191             }
00192         }
00193         else{
00194             var   = 0;
00195             index = 0;
00196             blb   = 0;
00197             bub   = 0;
00198         }
00199 
00200         residuumXL  = new DMatrix[grid.getNumPoints()];
00201         residuumXU  = new DMatrix[grid.getNumPoints()];
00202         residuumXAL = new DMatrix[grid.getNumPoints()];
00203         residuumXAU = new DMatrix[grid.getNumPoints()];
00204         residuumPL  = new DMatrix[1                  ];
00205         residuumPU  = new DMatrix[1                  ];
00206         residuumUL  = new DMatrix[grid.getNumPoints()];
00207         residuumUU  = new DMatrix[grid.getNumPoints()];
00208         residuumWL  = new DMatrix[grid.getNumPoints()];
00209         residuumWU  = new DMatrix[grid.getNumPoints()];
00210     }
00211     return *this;
00212 }
00213 
00214 
00215 
00216 
00217 
00218 returnValue BoxConstraint::evaluateBounds( const OCPiterate& iter ){
00219 
00220 
00221     uint run1, run2;
00222 
00223     const uint N = grid.getNumPoints();
00224 
00225     // EVALUATE BOUNDS:
00226     // ----------------
00227 
00228     for( run1 = 0; run1 < N; run1++ ){
00229 
00230         if( iter.x != NULL ){
00231             residuumXL[run1].init ( iter.x->getNumValues(), 1 );
00232             residuumXU[run1].init ( iter.x->getNumValues(), 1 );
00233 
00234             for( run2 = 0; run2 < iter.x->getNumValues(); run2++ ){
00235                 residuumXL[run1](run2,0) = -INFTY;
00236                 residuumXU[run1](run2,0) =  INFTY;
00237             }
00238         }
00239         else{
00240             residuumXL[run1].init ( 0, 0 );
00241             residuumXU[run1].init ( 0, 0 );
00242         }
00243 
00244         if( iter.xa != NULL ){
00245             residuumXAL[run1].init ( iter.xa->getNumValues(), 1 );
00246             residuumXAU[run1].init ( iter.xa->getNumValues(), 1 );
00247 
00248             for( run2 = 0; run2 < iter.xa->getNumValues(); run2++ ){
00249                 residuumXAL[run1](run2,0) = -INFTY;
00250                 residuumXAU[run1](run2,0) =  INFTY;
00251             }
00252         }
00253         else{
00254             residuumXAL[run1].init ( 0, 0 );
00255             residuumXAU[run1].init ( 0, 0 );
00256         }
00257 
00258         if( iter.u != NULL ){
00259             residuumUL[run1].init ( iter.u->getNumValues(), 1 );
00260             residuumUU[run1].init ( iter.u->getNumValues(), 1 );
00261 
00262             for( run2 = 0; run2 < iter.u->getNumValues(); run2++ ){
00263                 residuumUL[run1](run2,0) = -INFTY;
00264                 residuumUU[run1](run2,0) =  INFTY;
00265             }
00266         }
00267         else{
00268             residuumUL[run1].init ( 0, 0 );
00269             residuumUU[run1].init ( 0, 0 );
00270         }
00271 
00272         if( iter.w != NULL ){
00273             residuumWL[run1].init ( iter.w->getNumValues(), 1 );
00274             residuumWU[run1].init ( iter.w->getNumValues(), 1 );
00275 
00276             for( run2 = 0; run2 < iter.w->getNumValues(); run2++ ){
00277                 residuumWL[run1](run2,0) = -INFTY;
00278                 residuumWU[run1](run2,0) =  INFTY;
00279             }
00280         }
00281         else{
00282             residuumWL[run1].init ( 0, 0 );
00283             residuumWU[run1].init ( 0, 0 );
00284         }
00285     }
00286 
00287 
00288     if( iter.p != NULL ){
00289         residuumPL[0].init ( iter.p->getNumValues(), 1 );
00290         residuumPU[0].init ( iter.p->getNumValues(), 1 );
00291 
00292         for( run2 = 0; run2 < iter.p->getNumValues(); run2++ ){
00293             residuumPL[0](run2,0) = -INFTY;
00294             residuumPU[0](run2,0) =  INFTY;
00295         }
00296     }
00297     else{
00298         residuumPL[0].init ( 0, 0 );
00299         residuumPU[0].init ( 0, 0 );
00300     }
00301 
00302 
00303     for( run1 = 0; (int) run1 < nb; run1++ ){
00304 
00305         switch( var[run1] ){
00306 
00307             case VT_DIFFERENTIAL_STATE:
00308                 if( iter.x != NULL ){
00309                     for( run2 = 0; run2 < N; run2++ ){
00310                         residuumXL[run2](index[run1],0) = blb[run1][0](run2) - iter.x->operator()(run2,index[run1]);
00311                         residuumXU[run2](index[run1],0) = bub[run1][0](run2) - iter.x->operator()(run2,index[run1]);
00312                     }
00313                 }
00314                 else ACADOERROR( RET_INVALID_ARGUMENTS );
00315                 break;
00316 
00317 
00318             case VT_ALGEBRAIC_STATE:
00319                 if( iter.xa != NULL ){
00320                     for( run2 = 0; run2 < N; run2++ ){
00321                         residuumXAL[run2](index[run1],0) = blb[run1][0](run2) - iter.xa->operator()(run2,index[run1]);
00322                         residuumXAU[run2](index[run1],0) = bub[run1][0](run2) - iter.xa->operator()(run2,index[run1]);
00323                     }
00324                 }
00325                 else ACADOERROR( RET_INVALID_ARGUMENTS );
00326                 break;
00327 
00328 
00329             case VT_PARAMETER:
00330                 if( iter.p != NULL ){
00331                     residuumPL[0](index[run1],0) = blb[run1][0](0) - iter.p->operator()(0,index[run1]);
00332                     residuumPU[0](index[run1],0) = bub[run1][0](0) - iter.p->operator()(0,index[run1]);
00333                 }
00334                 else ACADOERROR( RET_INVALID_ARGUMENTS );
00335                 break;
00336 
00337             case VT_CONTROL:
00338                 if( iter.u != NULL ){
00339                     for( run2 = 0; run2 < N; run2++ ){
00340                         residuumUL[run2](index[run1],0) = blb[run1][0](run2) - iter.u->operator()(run2,index[run1]);
00341                         residuumUU[run2](index[run1],0) = bub[run1][0](run2) - iter.u->operator()(run2,index[run1]);
00342                     }
00343                 }
00344                 else ACADOERROR( RET_INVALID_ARGUMENTS );
00345                 break;
00346 
00347             case VT_DISTURBANCE:
00348                 if( iter.w != NULL ){
00349                     for( run2 = 0; run2 < N; run2++ ){
00350                         residuumWL[run2](index[run1],0) = blb[run1][0](run2) - iter.w->operator()(run2,index[run1]);
00351                         residuumWU[run2](index[run1],0) = bub[run1][0](run2) - iter.w->operator()(run2,index[run1]);
00352                     }
00353                 }
00354                 else {ASSERT(1==0);ACADOERROR( RET_INVALID_ARGUMENTS );}
00355                 break;
00356 
00357 
00358             default:
00359                 ACADOERRORTEXT(RET_NOT_IMPLEMENTED_YET,"Variables in constraints should be of type DIFFERENTIAL_STATE, ALGEBRAIC_STATE, PARAMETER, CONTROL or DISTURBANCE\n");
00360                 break;
00361 
00362         }
00363     }
00364 
00365     return SUCCESSFUL_RETURN;
00366 }
00367 
00368 
00369 inline returnValue BoxConstraint::getBounds( const OCPiterate& iter ){
00370 
00371 
00372     uint run1, run2;
00373 
00374     const uint N = grid.getNumPoints();
00375 
00376     for( run1 = 0; (int) run1 < nb; run1++ ){
00377 
00378         switch( var[run1] ){
00379 
00380             case VT_DIFFERENTIAL_STATE:
00381                 if( iter.x != NULL ){
00382                     for( run2 = 0; run2 < N; run2++ ){
00383 
00384                         if( bub[run1][0](run2) - blb[run1][0](run2) < -0.5*BOUNDTOL )
00385                             return ACADOERROR( RET_INCONSISTENT_BOUNDS );
00386 
00387                         iter.x->setLowerBound(run2,index[run1],blb[run1][0](run2));
00388                         iter.x->setUpperBound(run2,index[run1],bub[run1][0](run2));
00389                     }
00390                 }
00391                 else ACADOERROR( RET_INVALID_ARGUMENTS );
00392                 break;
00393 
00394 
00395             case VT_ALGEBRAIC_STATE:
00396                 if( iter.xa != NULL ){
00397                     for( run2 = 0; run2 < N; run2++ ){
00398 
00399                         if( bub[run1][0](run2) - blb[run1][0](run2) < -0.5*BOUNDTOL )
00400                             return ACADOERROR( RET_INCONSISTENT_BOUNDS );
00401 
00402                         iter.xa->setLowerBound(run2,index[run1],blb[run1][0](run2));
00403                         iter.xa->setUpperBound(run2,index[run1],bub[run1][0](run2));
00404                     }
00405                 }
00406                 else ACADOERROR( RET_INVALID_ARGUMENTS );
00407                 break;
00408 
00409 
00410             case VT_PARAMETER:
00411                 if( iter.p != NULL ){
00412                                         for( run2 = 0; run2 < N; run2++ ){
00413                         if( bub[run1][0](0) - blb[run1][0](0) < -0.5*BOUNDTOL )
00414                             return ACADOERROR( RET_INCONSISTENT_BOUNDS );
00415 
00416                         iter.p->setLowerBound(run2,index[run1],blb[run1][0](0));
00417                         iter.p->setUpperBound(run2,index[run1],bub[run1][0](0));
00418                                         }
00419                 }
00420                 else ACADOERROR( RET_INVALID_ARGUMENTS );
00421                 break;
00422 
00423             case VT_CONTROL:
00424                 if( iter.u != NULL ){
00425                     for( run2 = 0; run2 < N; run2++ ){
00426 
00427                         if( bub[run1][0](run2) - blb[run1][0](run2) < -0.5*BOUNDTOL )
00428                             return ACADOERROR( RET_INCONSISTENT_BOUNDS );
00429 
00430                         iter.u->setLowerBound(run2,index[run1],blb[run1][0](run2));
00431                         iter.u->setUpperBound(run2,index[run1],bub[run1][0](run2));
00432                     }
00433                 }
00434                 else ACADOERROR( RET_INVALID_ARGUMENTS );
00435                 break;
00436 
00437             case VT_DISTURBANCE:
00438                 if( iter.w != NULL ){
00439                     for( run2 = 0; run2 < N; run2++ ){
00440 
00441                         if( bub[run1][0](run2) - blb[run1][0](run2) < -0.5*BOUNDTOL )
00442                             return ACADOERROR( RET_INCONSISTENT_BOUNDS );
00443 
00444                         iter.w->setLowerBound(run2,index[run1],blb[run1][0](run2));
00445                         iter.w->setUpperBound(run2,index[run1],bub[run1][0](run2));
00446                     }
00447                 }
00448                 else ACADOERROR( RET_INVALID_ARGUMENTS );
00449                 break;
00450 
00451 
00452             default:
00453                 ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00454                 break;
00455 
00456         }
00457     }
00458 
00459     return SUCCESSFUL_RETURN;
00460 }
00461 
00462 
00463 
00464 CLOSE_NAMESPACE_ACADO
00465 
00466 // end of file.


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