function.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/utils/acado_utils.hpp>
00036 #include <acado/symbolic_expression/symbolic_expression.hpp>
00037 #include <acado/function/evaluation_point.hpp>
00038 #include <acado/function/function_.hpp>
00039 
00040 
00041 
00042 BEGIN_NAMESPACE_ACADO
00043 
00044 
00045 //
00046 // PUBLIC MEMBER FUNCTIONS:
00047 //
00048 
00049 Function::Function(){
00050 
00051     memoryOffset = 0;
00052         result       = 0;
00053 }
00054 
00055 
00056 Function::Function( const Function& arg ){
00057 
00058     evaluationTree = arg.evaluationTree;
00059     memoryOffset   = arg.memoryOffset  ;
00060         
00061         if ( arg.getDim() != 0 )
00062         {
00063                 result = (double*) calloc( arg.getDim(),sizeof(double) );
00064                 for( int i=0; i<getDim(); ++i )
00065                         result[i] = arg.result[i];
00066         }
00067         else
00068                 result = 0;
00069 }
00070 
00071 
00072 Function::~Function( ){ 
00073         if ( result != 0 )
00074                 free( result );
00075 }
00076 
00077 
00078 Function& Function::operator=( const Function& arg ){
00079 
00080     if ( this != &arg ){
00081 
00082                 if ( result != 0 )
00083                         free( result );
00084 
00085         evaluationTree = arg.evaluationTree;
00086         memoryOffset   = arg.memoryOffset  ;
00087                 
00088                 if ( arg.getDim() != 0 )
00089                 {
00090                         result = (double*) calloc( arg.getDim(),sizeof(double) );
00091                         for( int i=0; i<getDim(); ++i )
00092                                 result[i] = arg.result[i];
00093                 }
00094                 else
00095                         result = 0;
00096     }
00097 
00098     return *this;
00099 }
00100 
00101 
00102 Function& Function::operator<<( const Expression& arg ){
00103 
00104     evaluationTree.operator<<( arg );
00105 
00106         result = (double*) realloc( result,getDim()*sizeof(double) );
00107 
00108     return *this;
00109 }
00110 
00111 
00112 Function& Function::operator<<( const double &arg ){
00113 
00114     Expression tmp;
00115     tmp = arg;
00116 
00117     return operator<<(tmp);
00118 }
00119 
00120 
00121 Function& Function::operator<<( const DVector& arg ){
00122 
00123     Expression tmp;
00124     tmp = arg;
00125 
00126     return operator<<(tmp);
00127 }
00128 
00129 
00130 Function& Function::operator<<( const DMatrix& arg ){
00131 
00132     Expression tmp;
00133     tmp = arg;
00134 
00135     return operator<<(tmp);
00136 }
00137 
00138 
00139 returnValue Function::getExpression( Expression& expression ) const{
00140 
00141     return evaluationTree.getExpression( expression );
00142 }
00143 
00144 
00145 Function Function::operator()(  uint idx
00146                                                                 ) const
00147 {
00148         Function tmp;
00149 
00150         if ( (int)idx >= getDim() )
00151         {
00152                 ACADOERROR( RET_INDEX_OUT_OF_BOUNDS );
00153                 return tmp;
00154         }
00155 
00156         tmp.evaluationTree = evaluationTree;
00157         tmp.memoryOffset   = memoryOffset  ;
00158 
00159         return tmp;
00160 }
00161 
00162 
00163 returnValue Function::reset( ){
00164 
00165     FunctionEvaluationTree tmp;
00166     evaluationTree = tmp;
00167     memoryOffset = 0;
00168 
00169         if ( result != 0 )
00170                 free( result );
00171 
00172     return SUCCESSFUL_RETURN;
00173 }
00174 
00175 
00176 int Function::index( VariableType variableType_, int index_ ) const{
00177 
00178     return evaluationTree.index( variableType_, index_ );
00179 }
00180 
00181 
00182 double Function::scale( VariableType variableType_, int index_ ) const{
00183 
00184     return evaluationTree.scale( variableType_, index_ );
00185 }
00186 
00187 int Function::getN   (VariableType &variableType_) const{
00188     switch(variableType_) {
00189         case VT_DIFFERENTIAL_STATE      : return getNX(); break;        
00190         case VT_ALGEBRAIC_STATE         : return getNXA(); break;               
00191         case VT_CONTROL                         : return getNU(); break;
00192         case VT_INTEGER_CONTROL         : return getNUI(); break;       
00193         case VT_PARAMETER                       : return getNP(); break;
00194         case VT_ONLINE_DATA             : return getNOD(); break;
00195         case VT_INTEGER_PARAMETER       : return getNPI(); break;
00196         case VT_DISTURBANCE             : return getNW(); break;
00197         case VT_TIME                            : return 1; break;
00198         case VT_INTERMEDIATE_STATE      : return getN(); break;
00199         case VT_DDIFFERENTIAL_STATE     : return getNDX();       break;
00200         case VT_VARIABLE                        : return getNX()+getNXA()+getNU()+getNUI()+getNP()+getNPI()+getNW()+getNDX();    break;
00201         case VT_OUTPUT                          : return 0; break;
00202         case VT_UNKNOWN                         : return 0;
00203     }
00204     return evaluationTree.getNX();
00205 }
00206 
00207 int Function::getNX   () const{
00208 
00209     return evaluationTree.getNX();
00210 }
00211 
00212 int Function::getNXA  () const{
00213 
00214     return evaluationTree.getNXA();
00215 }
00216 
00217 int Function::getNDX   () const{
00218 
00219     return evaluationTree.getNDX();
00220 }
00221 
00222 int Function::getNU   () const{
00223 
00224     return evaluationTree.getNU();
00225 }
00226 
00227 int Function::getNUI   () const{
00228 
00229 
00230     return evaluationTree.getNUI();
00231 }
00232 
00233 int Function::getNP   () const{
00234 
00235 
00236     return evaluationTree.getNP();
00237 }
00238 
00239 int Function::getNPI   () const{
00240 
00241 
00242     return evaluationTree.getNPI();
00243 }
00244 
00245 int Function::getNW   () const{
00246 
00247 
00248     return evaluationTree.getNW();
00249 }
00250 
00251 int Function::getNT   () const{
00252 
00253 
00254     return evaluationTree.getNT();
00255 }
00256 
00257 int Function::getNOD   () const{
00258 
00259 
00260     return evaluationTree.getNOD();
00261 }
00262 
00263 
00264 int Function::getNumberOfVariables() const{
00265 
00266 
00267     return evaluationTree.getNumberOfVariables();
00268 }
00269 
00270 
00271 Operator* Function::getExpression( uint componentIdx ) const{
00272 
00273     return evaluationTree.getExpression( componentIdx );
00274 }
00275 
00276 
00277 returnValue Function::evaluate( int number, double *x, double *_result ){
00278 
00279 //     return evaluationTree.evaluate( number+memoryOffset, x, _result );
00280 
00281     evaluationTree.evaluate( number+memoryOffset, x, _result );
00282 
00283 
00284 
00285     return SUCCESSFUL_RETURN;
00286 }
00287 
00288 
00289 
00290 returnValue Function::substitute( VariableType variableType_, int index_,
00291                                   double sub_ ){
00292 
00293 
00294 
00295     if( isSymbolic() == BT_TRUE ){
00296         FunctionEvaluationTree tmp;
00297         tmp = evaluationTree.substitute(variableType_,index_,sub_);
00298         evaluationTree = tmp;
00299         return SUCCESSFUL_RETURN;
00300     }
00301 
00302     return ACADOERROR(RET_ONLY_SUPPORTED_FOR_SYMBOLIC_FUNCTIONS);
00303 }
00304 
00305 
00306 
00307 NeutralElement Function::isOneOrZero(){
00308 
00309     return evaluationTree.isOneOrZero();
00310 }
00311 
00312 
00313 
00314 BooleanType Function::isDependingOn( const Expression     &variable ){
00315 
00316     return evaluationTree.isDependingOn( variable );
00317 }
00318 
00319 BooleanType Function::isLinearIn( const Expression     &variable ){
00320 
00321     return evaluationTree.isLinearIn( variable );
00322 }
00323 
00324 
00325 BooleanType Function::isPolynomialIn( const Expression     &variable ){
00326 
00327     return evaluationTree.isPolynomialIn( variable );
00328 }
00329 
00330 
00331 BooleanType Function::isRationalIn( const Expression &variable ){
00332 
00333     return evaluationTree.isRationalIn( variable );
00334 }
00335 
00336 
00337 BooleanType Function::isNondecreasing(){
00338 
00339 
00340 
00341     if( evaluationTree.getMonotonicity() == MT_NONDECREASING ||
00342         evaluationTree.getMonotonicity() == MT_CONSTANT       )
00343         return BT_TRUE;
00344 
00345     return BT_FALSE;
00346 }
00347 
00348 
00349 BooleanType Function::isNonincreasing(){
00350 
00351 
00352 
00353     if( evaluationTree.getMonotonicity() == MT_NONINCREASING ||
00354         evaluationTree.getMonotonicity() == MT_CONSTANT       )
00355         return BT_TRUE;
00356 
00357     return BT_FALSE;
00358 }
00359 
00360 BooleanType Function::isConstant()
00361 {
00362         if (evaluationTree.getMonotonicity() == MT_CONSTANT)
00363                 return BT_TRUE;
00364         return BT_FALSE;
00365 }
00366 
00367 
00368 BooleanType Function::isAffine(){
00369 
00370      // (should be implemented more efficiently)
00371 
00372     if( isConvex() == BT_TRUE && isConcave() == BT_TRUE )
00373         return BT_TRUE;
00374     return BT_FALSE;
00375 }
00376 
00377 
00378 BooleanType Function::isConvex(){
00379 
00380 
00381 
00382     if( evaluationTree.getCurvature() == CT_CONVEX   ||
00383         evaluationTree.getCurvature() == CT_AFFINE   ||
00384         evaluationTree.getCurvature() == CT_CONSTANT  )
00385         return BT_TRUE;
00386 
00387     return BT_FALSE;
00388 }
00389 
00390 
00391 BooleanType Function::isConcave(){
00392 
00393 
00394 
00395     if( evaluationTree.getCurvature() == CT_CONCAVE  ||
00396         evaluationTree.getCurvature() == CT_AFFINE   ||
00397         evaluationTree.getCurvature() == CT_CONSTANT  )
00398         return BT_TRUE;
00399 
00400     return BT_FALSE;
00401 }
00402 
00403 returnValue Function::jacobian(DMatrix &x) {
00404     int n=getDim();
00405     int N=getNumberOfVariables();
00406     returnValue ret;
00407 
00408     x=DMatrix(getNX(),n);x.setAll(0);
00409     //u=DMatrix(getNU(),n);u.setAll(0);
00410     //p=DMatrix(getNP(),n);p.setAll(0);
00411     //w=DMatrix(getNW(),n);w.setAll(0);
00412     double *Jr=new double[N];
00413     double *seed=new double[n];
00414     for (int i=0;i<n;i++) seed[i]=0;
00415     for (int i=0;i<n;i++) {
00416         if (i>0) seed[i-1]=0;
00417         seed[i]=1;
00418         for (int j=0;j<N;j++) Jr[j]=0;
00419         ret=AD_backward(0,seed,Jr);
00420         if (ret != SUCCESSFUL_RETURN) return ret;
00421         for (int j=0;j<getNX();j++) x(j,i)=Jr[index(VT_DIFFERENTIAL_STATE,j)];
00422     }
00423     delete[] Jr;
00424     delete[] seed;
00425     return SUCCESSFUL_RETURN;
00426 }
00427 
00428 
00429 returnValue Function::AD_forward( int number, double *seed, double *df  ){
00430 
00431     return evaluationTree.AD_forward( number+memoryOffset, seed, df );
00432 }
00433 
00434 
00435 returnValue Function::AD_backward( int number, double *seed, double  *df ){
00436 
00437     return evaluationTree.AD_backward( number+memoryOffset, seed, df );
00438 }
00439 
00440 
00441 returnValue Function::AD_forward2( int number, double *seed, double *dseed,
00442                                    double *df, double *ddf ){
00443 
00444     return evaluationTree.AD_forward2( number+memoryOffset, seed, dseed, df, ddf );
00445 }
00446 
00447 
00448 returnValue Function::AD_backward2( int number, double *seed1, double *seed2,
00449                                     double *df, double *ddf ){
00450 
00451     return evaluationTree.AD_backward2( number+memoryOffset, seed1, seed2, df, ddf );
00452 }
00453 
00454 
00455 
00456 std::ostream& operator<<(std::ostream& stream, const Function& arg)
00457 {
00458     arg.print( stream );
00459 
00460     return stream;
00461 }
00462 
00463 returnValue Function::print(    std::ostream& stream,
00464                                                                 const char *fcnName ,
00465                                                                 const char *realString
00466                                                                 ) const
00467 {
00468         if (getDim() > 0)
00469                 return evaluationTree.C_print(stream, fcnName, realString);
00470 
00471         return SUCCESSFUL_RETURN;
00472 }
00473 
00474 returnValue Function::exportForwardDeclarations(        std::ostream& stream,
00475                                                                                                         const char *fcnName  ,
00476                                                                                                         const char *realString
00477                                                                                                         ) const
00478 {
00479         if (getDim() > 0)
00480                 return evaluationTree.exportForwardDeclarations(stream, fcnName, realString);
00481 
00482         return SUCCESSFUL_RETURN;
00483 }
00484 
00485 
00486 returnValue Function::exportCode(       std::ostream& stream,
00487                                                                         const char *fcnName,
00488                                                                         const char *realString,
00489                                                                         uint        _numX,
00490                                                                         uint            _numXA,
00491                                                                         uint            _numU,
00492                                                                         uint            _numP,
00493                                                                         uint            _numDX,
00494                                                                         uint            _numOD,
00495                                                                         bool       allocateMemory,
00496                                                                         bool       staticMemory
00497                                                                         ) const
00498 {
00499         if (getDim() > 0)
00500                 return evaluationTree.exportCode(stream, fcnName, realString,
00501                                 _numX, _numXA, _numU, _numP, _numDX, _numOD, allocateMemory, staticMemory);
00502 
00503         return SUCCESSFUL_RETURN;
00504 }
00505 
00506 
00507 returnValue Function::clearBuffer(){
00508 
00509     return evaluationTree.clearBuffer();
00510 }
00511 
00512 
00513 
00514 returnValue Function::setScale( double *scale_ ){
00515 
00516     return evaluationTree.setScale(scale_);
00517 }
00518 
00519 
00520 DVector Function::evaluate( const EvaluationPoint &x,
00521                            const int        &number  ){
00522 
00523     //double *result = new double[getDim()];
00524 
00525     evaluate( number, x.getEvaluationPointer(), result );
00526     DVector res( getDim(), result );
00527 
00528     //delete[] result;
00529     return   res;
00530 }
00531 
00532 
00533 DVector Function::AD_forward( const EvaluationPoint &x,
00534                              const int        &number  ){
00535 
00536     //double *result = new double[getDim()];
00537 
00538     AD_forward( number, x.getEvaluationPointer(), result );
00539     DVector res( getDim(), result );
00540 
00541     //delete[] result;
00542     return   res;
00543 }
00544 
00545 
00546 returnValue Function::AD_backward( const    DVector &seed  ,
00547                                    EvaluationPoint &df    ,
00548                                    const int       &number  ){
00549 
00550 
00551     if( ADisSupported() == BT_TRUE ){
00552 
00553         int run1;
00554         double *seed_ = new double[getDim()];
00555 
00556         for( run1 = 0; run1 < getDim(); run1++ )
00557             seed_[run1] = seed(run1);
00558 
00559         AD_backward( number, seed_, df.getEvaluationPointer() );
00560 
00561         delete[] seed_;
00562         return SUCCESSFUL_RETURN;
00563     }
00564 
00565     uint run1, run2;
00566     double *fseed = new double[getNumberOfVariables()+1];
00567     for( run2 = 0; (int) run2 < getNumberOfVariables()+1; run2++ )
00568         fseed[run2] = 0.0;
00569 
00570     double *J;
00571     J = new double[seed.getDim()];
00572 
00573     for( run2 = 0; (int) run2 < getNumberOfVariables()+1; run2++ ){
00574 
00575         fseed[run2] = 1.0;
00576         AD_forward( 0, fseed, J );
00577         fseed[run2] = 0.0;
00578 
00579         df.z[run2] = 0.0;
00580         for( run1 = 0; run1 < seed.getDim(); run1++ )
00581             df.z[run2] += J[run1]*seed(run1);
00582     }
00583 
00584     delete[] J    ;
00585     delete[] fseed;
00586 
00587     return SUCCESSFUL_RETURN;
00588 }
00589 
00590 std::string Function::getGlobalExportVariableName( ) const
00591 {
00592         return evaluationTree.getGlobalExportVariableName( );
00593 }
00594 
00595 returnValue Function::setGlobalExportVariableName(const std::string& var)
00596 {
00597         return evaluationTree.setGlobalExportVariableName( var );
00598 }
00599 
00600 unsigned Function::getGlobalExportVariableSize( ) const
00601 {
00602         return evaluationTree.getGlobalExportVariableSize( );
00603 }
00604 
00605 
00606 CLOSE_NAMESPACE_ACADO
00607 
00608 // end of file.


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