acado_syntax.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 
00032 #include <acado/symbolic_expression/acado_syntax.hpp>
00033 #include <acado/symbolic_operator/operator.hpp>
00034 
00035 USING_NAMESPACE_ACADO
00036 
00037 
00038 // ------------------------------------------------------------------------------------
00039 //                               STANDARD OPERATORS:
00040 // ------------------------------------------------------------------------------------
00041 
00042 
00043 IntermediateState sin ( const Expression &arg ){ return arg.getSin(); }
00044 IntermediateState cos ( const Expression &arg ){ return arg.getCos (); }
00045 IntermediateState tan ( const Expression &arg ){ return arg.getTan (); }
00046 IntermediateState asin( const Expression &arg ){ return arg.getAsin(); }
00047 IntermediateState acos( const Expression &arg ){ return arg.getAcos(); }
00048 IntermediateState atan( const Expression &arg ){ return arg.getAtan(); }
00049 IntermediateState exp ( const Expression &arg ){ return arg.getExp (); }
00050 IntermediateState sqrt( const Expression &arg ){ return arg.getSqrt(); }
00051 IntermediateState ln  ( const Expression &arg ){ return arg.getLn  (); }
00052 IntermediateState log ( const Expression &arg ){ return arg.getLn  (); }
00053 
00054 IntermediateState pow( const Expression &arg1, const Expression &arg2 ){
00055   
00056   return arg1.getPow(arg2);
00057 }
00058 
00059 IntermediateState pow( const double &arg1, const Expression &arg2 ){
00060   
00061   return Expression( arg1 ).getPow( arg2 );
00062 }
00063 
00064 IntermediateState pow( const Expression &arg1, const double &arg2 ){
00065   
00066     if( fabs( arg2 - floor(arg2) ) <= 10.0*EPS ){
00067         int intarg = (int) floor(arg2);
00068         return arg1.getPowInt( intarg );
00069     }
00070     if( fabs( arg2 - ceil(arg2) ) <= 10.0*EPS ){
00071         int intarg = (int) ceil(arg2);
00072         return arg1.getPowInt( intarg );
00073     }
00074     return arg1.getPow( arg2 );
00075 }
00076 
00077 
00078 
00079 // ---------------------------------------------------------------------------------------------
00080 //                     SPECIAL CONVEX DICIPLINED PROGRAMMING FUNCTIONS:
00081 // ---------------------------------------------------------------------------------------------
00082 
00083 
00084 IntermediateState square         ( const Expression &arg ){ return arg.getSumSquare    (); }
00085 IntermediateState sum_square     ( const Expression &arg ){ return arg.getSumSquare    (); }
00086 IntermediateState log_sum_exp    ( const Expression &arg ){ return arg.getLogSumExp    (); }
00087 IntermediateState euclidean_norm ( const Expression &arg ){ return arg.getEuclideanNorm(); }
00088 IntermediateState entropy        ( const Expression &arg ){ return arg.getEntropy      (); }
00089 
00090 
00091 
00092 // ---------------------------------------------------------------------------------------------
00093 //                    SPECIAL ROUTINES FOR THE SET UP OF DYNAMIC SYSTEMS:
00094 // ---------------------------------------------------------------------------------------------
00095 
00096 
00097 
00098 Expression dot ( const Expression &arg ){ return arg.getDot (); }
00099 Expression next( const Expression &arg ){ return arg.getNext(); }
00100 
00101 // ---------------------------------------------------------------------------------------------
00102 //                              SYMBOLIC DERIVATIVE OPERATORS:
00103 // ---------------------------------------------------------------------------------------------
00104 
00105 
00106 Expression forwardDerivative( const Expression &arg1,
00107                               const Expression &arg2  ){
00108 
00109     return arg1.ADforward(arg2);
00110 }
00111 
00112 
00113 Expression backwardDerivative( const Expression &arg1,
00114                                const Expression &arg2  ){
00115 
00116     return arg1.ADbackward(arg2);
00117 }
00118 
00119 
00120 Expression forwardDerivative( const Expression &arg1,
00121                               const Expression &arg2,
00122                               const Expression &seed  ){
00123 
00124     return arg1.ADforward(arg2,seed);
00125 }
00126 
00127 
00128 Expression multipleForwardDerivative( const Expression &arg1,
00129                               const Expression &arg2,
00130                               const Expression &seed  ){
00131 
00132         Expression tmp;
00133         for( uint i = 0; i < seed.getNumCols(); i++ ) {
00134                 tmp.appendCols( forwardDerivative( arg1, arg2, seed.getCol(i) ) );
00135         }
00136     return tmp;
00137 }
00138 
00139 
00140 Expression backwardDerivative( const Expression &arg1,
00141                                const Expression &arg2,
00142                                const Expression &seed  ){
00143 
00144     return arg1.ADbackward(arg2,seed);
00145 }
00146 
00147 
00148 Expression multipleBackwardDerivative( const Expression &arg1,
00149                               const Expression &arg2,
00150                               const Expression &seed  ){
00151 
00152         Expression tmp;
00153         for( uint i = 0; i < seed.getNumCols(); i++ ) {
00154                 tmp.appendCols( backwardDerivative( arg1, arg2, seed.getCol(i) ) );
00155         }
00156     return tmp;
00157 }
00158 
00159 Expression symmetricDerivative(         const Expression &arg1,
00160                                                                         const Expression &arg2,
00161                                                                         const Expression &forward_seed,
00162                                                                         const Expression &backward_seed,
00163                                                                         Expression *forward_result,
00164                                                                         Expression *backward_result ) {
00165         return arg1.ADsymmetric( arg2, forward_seed, backward_seed, forward_result, backward_result );
00166 }
00167 
00168 
00169 Expression jacobian          ( const Expression &arg1,
00170                                const Expression &arg2 ){
00171 
00172     return backwardDerivative( arg1, arg2 );
00173 }
00174 
00175 
00176 Expression laplace           ( const Expression &arg1,
00177                                const Expression &arg2 ){
00178 
00179     return forwardDerivative( forwardDerivative(arg1,arg2), arg2 );
00180 }
00181 
00182 Expression getRiccatiODE( const Expression        &rhs,
00183                           const DifferentialState &x  ,
00184                           const Control           &u  ,
00185                           const DifferentialState &P  ,
00186                           const DMatrix            &Q  ,
00187                           const DMatrix            &R  ){
00188 
00189         IntermediateState RHS("", x.getDim(), x.getDim());
00190 
00191     IntermediateState A = forwardDerivative( rhs, x );
00192     IntermediateState B = forwardDerivative( rhs, u );
00193 
00194     return A.transpose()*P + P*A + Q - P*B*(DMatrix(R.inverse()))*B.transpose()*P;
00195 }
00196 
00197 
00198 Expression chol( const Expression &arg ){
00199 
00200     // --------------------------------------------------
00201     // COMPUTES THE CHOLESKY FACTOR L OF THE ARGUMENT
00202     // SUCH THAT
00203     //
00204     //               ARG = L * L.transpose
00205     //
00206     // WHERE THE MATRIX L IS RETURNED.
00207     // THIS IS A SYMBOLIC CHOLESKY ROUTINE AS ACADO
00208     // SIMPLIFIES PRODUCTS WITH ZERO AUTOMATICALLY.
00209     //
00210     // CAUTION: THE ROUTINE WORKS ON A SYMBOLIC LEVEL
00211     //          ALWAYS WITHOUT DETECTING PSD-MATRICES.
00212     //          HOWEVER, THE EVEALUATION WILL ONLY WORK
00213     //          FOR SYMMETRIC AND POSITIVE SEMI-DEFINITE
00214     //          MATRICES.
00215     //          THE SAFE-GUARD CONSTANT IS 100*EPS.
00216     // --------------------------------------------------
00217 
00218 
00219     ASSERT( arg.getNumRows() == arg.getNumCols() );
00220 
00221     int dim = arg.getNumRows();
00222     IntermediateState L("", dim,dim);
00223 
00224     // COMPUTE THE LOWER TRIANGLE RECURSIVELY:
00225     // ---------------------------------------
00226 
00227     int i,j,k;
00228 
00229     for( j = 0; j < dim; j++ ){
00230                                   L(j,j)  = arg(j,j)       ;
00231         for( k = 0; k < j; k++ )  L(j,j) -= L(j,k)*L(j,k)  ;
00232                                   L(j,j)  = sqrt( L(j,j) + 100.0*EPS );
00233         for( i = j+1; i < dim; i++ ){
00234                                       L(i,j)  = arg(i,j)       ;
00235             for( k = 0; k < j; k++ )  L(i,j) -= L(i,k)*L(j,k)  ;
00236                                       L(i,j)  = L(i,j)/(L(j,j) + 100.0*EPS );
00237         }
00238     }
00239 
00240     for( j = 0; j < dim; j++ )
00241         for( k = j+1; k < dim; k++ )
00242              L(j,k) = 0.0;
00243 
00244     return L;
00245 }
00246 
00247 
00248 returnValue clearAllStaticCounters()
00249 {
00250         AlgebraicState              dummy1;
00251         Control                     dummy2;
00252         DifferentialState           dummy3;
00253         DifferentialStateDerivative dummy4;
00254         Disturbance                 dummy5;
00255         IntegerControl              dummy6;
00256         IntegerParameter            dummy7;
00257         IntermediateState           dummy8;
00258         Parameter                   dummy9;
00259         OnlineData                  dummy10;
00260 
00261         dummy1.clearStaticCounters();
00262         dummy2.clearStaticCounters();
00263         dummy3.clearStaticCounters();
00264         dummy4.clearStaticCounters();
00265         dummy5.clearStaticCounters();
00266         dummy6.clearStaticCounters();
00267         dummy7.clearStaticCounters();
00268         dummy8.clearStaticCounters();
00269         dummy9.clearStaticCounters();
00270         dummy10.clearStaticCounters();
00271 
00272         return SUCCESSFUL_RETURN;
00273 }
00274 
00275 
00276 // end of file.
00277 
00278 


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