clipping_functionality.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/control_law/clipping_functionality.hpp>
00035 
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 //
00042 // PUBLIC MEMBER FUNCTIONS:
00043 //
00044 
00045 
00046 ClippingFunctionality::ClippingFunctionality( )
00047 {
00048 }
00049 
00050 
00051 ClippingFunctionality::ClippingFunctionality(   uint _nU,
00052                                                                                                 uint _nP
00053                                                                                                 )
00054 {
00055         lowerLimitControls.init( _nU );
00056         upperLimitControls.init( _nU );
00057         lowerLimitControls.setAll( -INFTY );
00058         upperLimitControls.setAll(  INFTY );
00059 
00060         lowerLimitParameters.init( _nP );
00061         upperLimitParameters.init( _nP );
00062         lowerLimitParameters.setAll( -INFTY );
00063         upperLimitParameters.setAll(  INFTY );
00064 }
00065 
00066 
00067 ClippingFunctionality::ClippingFunctionality( const ClippingFunctionality& rhs )
00068 {
00069         lowerLimitControls = rhs.lowerLimitControls;
00070         upperLimitControls = rhs.upperLimitControls;
00071 
00072         lowerLimitParameters = rhs.lowerLimitParameters;
00073         upperLimitParameters = rhs.upperLimitParameters;
00074 }
00075 
00076 
00077 ClippingFunctionality::~ClippingFunctionality( )
00078 {
00079 }
00080 
00081 
00082 ClippingFunctionality& ClippingFunctionality::operator=( const ClippingFunctionality& rhs )
00083 {
00084         if ( this != &rhs )
00085         {
00086                 lowerLimitControls = rhs.lowerLimitControls;
00087                 upperLimitControls = rhs.upperLimitControls;
00088         
00089                 lowerLimitParameters = rhs.lowerLimitParameters;
00090                 upperLimitParameters = rhs.upperLimitParameters;
00091         }
00092 
00093     return *this;
00094 }
00095 
00096 
00097 
00098 returnValue ClippingFunctionality::setControlLowerLimits(       const DVector& _lowerLimit
00099                                                                                                                         )
00100 {
00101         if ( _lowerLimit.getDim( ) != getNumControlLimits( ) )
00102                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00103 
00104         lowerLimitControls = _lowerLimit;
00105 
00106         return SUCCESSFUL_RETURN;
00107 }
00108 
00109 
00110 returnValue ClippingFunctionality::setControlLowerLimit(        uint idx,
00111                                                                                                                         double _lowerLimit
00112                                                                                                                         )
00113 {
00114         if ( idx >= getNumControlLimits( ) )
00115                 return ACADOERROR( RET_INDEX_OUT_OF_BOUNDS );
00116 
00117         lowerLimitControls( idx ) = _lowerLimit;
00118         return SUCCESSFUL_RETURN;
00119 }
00120 
00121 
00122 returnValue ClippingFunctionality::setControlUpperLimits(       const DVector& _upperLimit
00123                                                                                                                         )
00124 {
00125         if ( _upperLimit.getDim( ) != getNumControlLimits( ) )
00126                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00127 
00128         upperLimitControls = _upperLimit;
00129 
00130         return SUCCESSFUL_RETURN;
00131 }
00132 
00133 
00134 returnValue ClippingFunctionality::setControlUpperLimit(        uint idx,
00135                                                                                                                                 double _upperLimit
00136                                                                                                                                 )
00137 {
00138         if ( idx >= getNumControlLimits( ) )
00139                 return ACADOERROR( RET_INDEX_OUT_OF_BOUNDS );
00140 
00141         upperLimitControls( idx ) = _upperLimit;
00142         return SUCCESSFUL_RETURN;
00143 }
00144 
00145 
00146 
00147 returnValue ClippingFunctionality::setParameterLowerLimits(     const DVector& _lowerLimit
00148                                                                                                                         )
00149 {
00150         if ( _lowerLimit.getDim( ) != getNumParameterLimits( ) )
00151                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00152 
00153         lowerLimitParameters = _lowerLimit;
00154 
00155         return SUCCESSFUL_RETURN;
00156 }
00157 
00158 
00159 returnValue ClippingFunctionality::setParameterLowerLimit(      uint idx,
00160                                                                                                                         double _lowerLimit
00161                                                                                                                         )
00162 {
00163         if ( idx >= getNumParameterLimits( ) )
00164                 return ACADOERROR( RET_INDEX_OUT_OF_BOUNDS );
00165 
00166         lowerLimitParameters( idx ) = _lowerLimit;
00167         return SUCCESSFUL_RETURN;
00168 }
00169 
00170 
00171 returnValue ClippingFunctionality::setParameterUpperLimits(     const DVector& _upperLimit
00172                                                                                                                         )
00173 {
00174         if ( _upperLimit.getDim( ) != getNumParameterLimits( ) )
00175                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00176 
00177         upperLimitParameters = _upperLimit;
00178 
00179         return SUCCESSFUL_RETURN;
00180 }
00181 
00182 
00183 returnValue ClippingFunctionality::setParameterUpperLimit(      uint idx,
00184                                                                                                                         double _upperLimit
00185                                                                                                                         )
00186 {
00187         if ( idx >= getNumParameterLimits( ) )
00188                 return ACADOERROR( RET_INDEX_OUT_OF_BOUNDS );
00189 
00190         upperLimitParameters( idx ) = _upperLimit;
00191         return SUCCESSFUL_RETURN;
00192 }
00193 
00194 
00195 
00196 //
00197 // PROTECTED MEMBER FUNCTIONS:
00198 //
00199 
00200 
00201 
00202 returnValue ClippingFunctionality::clipSignals( VariablesGrid& _u,
00203                                                                                                 VariablesGrid& _p
00204                                                                                                 )
00205 {
00206         // consistency checks
00207         if ( _u.getNumValues( ) != getNumControlLimits( ) )
00208                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00209 
00210         if ( _p.getNumValues( ) != getNumParameterLimits( ) )
00211                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00212 
00213 
00214         // limit controls
00215         for( uint i=0; i<getNumControlLimits( ); ++i )
00216         {
00217                 for( uint j=0; j<_u.getNumPoints( ); ++j )
00218                 {
00219                         if ( _u( j,i ) < lowerLimitControls( i ) )
00220                                 _u( j,i ) = lowerLimitControls( i );
00221 
00222                         if ( _u( j,i ) > upperLimitControls( i ) )
00223                                 _u( j,i ) = upperLimitControls( i );
00224                 }
00225         }
00226 
00227 
00228         // limit parameters
00229         for( uint i=0; i<getNumParameterLimits( ); ++i )
00230         {
00231                 for( uint j=0; j<_p.getNumPoints( ); ++j )
00232                 {
00233                         if ( _p( j,i ) < lowerLimitParameters( i ) )
00234                                 _p( j,i ) = lowerLimitParameters( i );
00235 
00236                         if ( _p( j,i ) > upperLimitParameters( i ) )
00237                                 _p( j,i ) = upperLimitParameters( i );
00238                 }
00239         }
00240 
00241         return SUCCESSFUL_RETURN;
00242 }
00243 
00244 
00245 returnValue ClippingFunctionality::clipSignals( DVector& _u,
00246                                                                                                 DVector& _p
00247                                                                                                 )
00248 {
00249         // consistency checks
00250         if ( _u.getDim( ) != getNumControlLimits( ) )
00251                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00252 
00253         if ( _p.getDim( ) != getNumParameterLimits( ) )
00254                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00255 
00256 
00257         // limit controls
00258         for( uint i=0; i<getNumControlLimits( ); ++i )
00259         {
00260                 if ( _u( i ) < lowerLimitControls( i ) )
00261                         _u( i ) = lowerLimitControls( i );
00262 
00263                 if ( _u( i ) > upperLimitControls( i ) )
00264                         _u( i ) = upperLimitControls( i );
00265         }
00266 
00267 
00268         // limit parameters
00269         for( uint i=0; i<getNumParameterLimits( ); ++i )
00270         {
00271                 if ( _p( i ) < lowerLimitParameters( i ) )
00272                         _p( i ) = lowerLimitParameters( i );
00273 
00274                 if ( _p( i ) > upperLimitParameters( i ) )
00275                         _p( i ) = upperLimitParameters( i );
00276         }
00277 
00278         return SUCCESSFUL_RETURN;
00279 }
00280 
00281 
00282 CLOSE_NAMESPACE_ACADO
00283 
00284 // end of file.


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