linear_state_feedback.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/linear_state_feedback.hpp>
00035 
00036 
00037 
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 
00041 
00042 //
00043 // PUBLIC MEMBER FUNCTIONS:
00044 //
00045 
00046 LinearStateFeedback::LinearStateFeedback( ) : ControlLaw( ), ClippingFunctionality(  )
00047 {
00048         setStatus( BS_NOT_INITIALIZED );
00049 }
00050 
00051 
00052 LinearStateFeedback::LinearStateFeedback(       const DMatrix& _K,
00053                                                                                         double _samplingTime
00054                                                                                         ) : ControlLaw( _samplingTime ), ClippingFunctionality( _K.getNumRows() )
00055 {
00056         K = _K;
00057         setStatus( BS_READY );
00058 }
00059 
00060 
00061 LinearStateFeedback::LinearStateFeedback( const LinearStateFeedback& rhs ) : ControlLaw( rhs ), ClippingFunctionality( rhs )
00062 {
00063         K = rhs.K;
00064 }
00065 
00066 
00067 LinearStateFeedback::~LinearStateFeedback( )
00068 {
00069 }
00070 
00071 
00072 LinearStateFeedback& LinearStateFeedback::operator=( const LinearStateFeedback& rhs )
00073 {
00074         if ( this != &rhs )
00075         {
00076                 ControlLaw::operator=( rhs );
00077                 ClippingFunctionality::operator=( rhs );
00078 
00079                 K = rhs.K;
00080         }
00081 
00082     return *this;
00083 }
00084 
00085 
00086 ControlLaw* LinearStateFeedback::clone( ) const
00087 {
00088         return new LinearStateFeedback( *this );
00089 }
00090 
00091 
00092 returnValue LinearStateFeedback::init(  double startTime,
00093                                                                                 const DVector &x0_,
00094                                                                                 const DVector &p_,
00095                                                                                 const VariablesGrid& _yRef
00096                                                                                 )
00097 {
00098         setStatus( BS_READY );
00099         return SUCCESSFUL_RETURN;
00100 }
00101 
00102 
00103 returnValue LinearStateFeedback::step(  double currentTime,
00104                                                                                 const DVector& _x,
00105                                                                                 const DVector& _p,
00106                                                                                 const VariablesGrid& _yRef
00107                                                                                 )
00108 {
00109         if ( getStatus( ) != BS_READY )
00110                 return ACADOERROR( RET_BLOCK_NOT_READY );
00111 
00112         if ( _x.getDim( ) != getNX( ) )
00113                 return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00114 
00115 
00116         /* 1) Use reference trajectory if it is defined */
00117         // set default reference to zero
00118         DVector xRef( _x );
00119         
00120         if ( _yRef.getNumPoints( ) > 0 )
00121         {
00122                 if ( _yRef.getNumValues( ) != getNX( ) )
00123                         return ACADOERROR( RET_VECTOR_DIMENSION_MISMATCH );
00124 
00125                 xRef = _yRef.getVector( 0 );
00126         }
00127         else
00128         {
00129                 xRef.setZero( );
00130         }
00131 
00132 
00133         /* 2) Linear state feedback. */
00134         u = K * ( xRef - _x );
00135         p = _p;
00136 
00137 
00138         /* 3) Call output transformator. */
00139         if ( clipSignals( u,p ) != SUCCESSFUL_RETURN )
00140                 return ACADOERROR( RET_CONTROLLAW_STEP_FAILED );
00141         
00142         return SUCCESSFUL_RETURN;
00143 }
00144 
00145 
00146 
00147 uint LinearStateFeedback::getNX( ) const
00148 {
00149         return K.getNumCols( );
00150 }
00151 
00152 
00153 uint LinearStateFeedback::getNXA( ) const
00154 {
00155         return 0;
00156 }
00157 
00158 
00159 uint LinearStateFeedback::getNU( ) const
00160 {
00161         return K.getNumRows( );
00162 }
00163 
00164 
00165 uint LinearStateFeedback::getNP( ) const
00166 {
00167         return 0;
00168 }
00169 
00170 
00171 uint LinearStateFeedback::getNW( ) const
00172 {
00173         return 0;
00174 }
00175 
00176 
00177 uint LinearStateFeedback::getNY( ) const
00178 {
00179         return getNX( );
00180 }
00181 
00182 
00183 BooleanType LinearStateFeedback::isDynamic( ) const
00184 {
00185         return BT_FALSE;
00186 }
00187 
00188 
00189 BooleanType LinearStateFeedback::isStatic( ) const
00190 {
00191         if ( isDynamic() == BT_TRUE )
00192                 return BT_FALSE;
00193         else
00194                 return BT_TRUE;
00195 }
00196 
00197 
00198 
00199 //
00200 // PROTECTED MEMBER FUNCTIONS:
00201 //
00202 
00203 
00204 
00205 
00206 CLOSE_NAMESPACE_ACADO
00207 
00208 // end of file.


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