irk_forward_export.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 
00034 #include <acado/code_generation/integrators/irk_export.hpp>
00035 #include <acado/code_generation/integrators/irk_forward_export.hpp>
00036 
00037 using namespace std;
00038 
00039 BEGIN_NAMESPACE_ACADO
00040 
00041 //
00042 // PUBLIC MEMBER FUNCTIONS:
00043 //
00044 
00045 ForwardIRKExport::ForwardIRKExport(     UserInteraction* _userInteraction,
00046                                                                         const std::string& _commonHeaderName
00047                                                                         ) : ImplicitRungeKuttaExport( _userInteraction,_commonHeaderName )
00048 {
00049 }
00050 
00051 ForwardIRKExport::ForwardIRKExport( const ForwardIRKExport& arg ) : ImplicitRungeKuttaExport( arg )
00052 {
00053 }
00054 
00055 
00056 ForwardIRKExport::~ForwardIRKExport( )
00057 {
00058         if ( solver )
00059                 delete solver;
00060         solver = 0;
00061 
00062         clear( );
00063 }
00064 
00065 
00066 ForwardIRKExport& ForwardIRKExport::operator=( const ForwardIRKExport& arg ){
00067 
00068     if( this != &arg ){
00069 
00070                 ImplicitRungeKuttaExport::operator=( arg );
00071                 copy( arg );
00072     }
00073     return *this;
00074 }
00075 
00076 
00077 ExportVariable ForwardIRKExport::getAuxVariable() const
00078 {
00079         ExportVariable max;
00080         if( NX1 > 0 ) {
00081                 max = lin_input.getGlobalExportVariable();
00082         }
00083         if( NX2 > 0 || NXA > 0 ) {
00084                 if( rhs.getGlobalExportVariable().getDim() >= max.getDim() ) {
00085                         max = rhs.getGlobalExportVariable();
00086                 }
00087                 if( diffs_rhs.getGlobalExportVariable().getDim() >= max.getDim() ) {
00088                         max = diffs_rhs.getGlobalExportVariable();
00089                 }
00090         }
00091         if( NX3 > 0 ) {
00092                 if( rhs3.getGlobalExportVariable().getDim() >= max.getDim() ) {
00093                         max = rhs3.getGlobalExportVariable();
00094                 }
00095                 if( diffs_rhs3.getGlobalExportVariable().getDim() >= max.getDim() ) {
00096                         max = diffs_rhs3.getGlobalExportVariable();
00097                 }
00098         }
00099         uint i;
00100         for( i = 0; i < outputs.size(); i++ ) {
00101                 if( outputs[i].getGlobalExportVariable().getDim() >= max.getDim() ) {
00102                         max = outputs[i].getGlobalExportVariable();
00103                 }
00104                 if( diffs_outputs[i].getGlobalExportVariable().getDim() >= max.getDim() ) {
00105                         max = diffs_outputs[i].getGlobalExportVariable();
00106                 }
00107         }
00108         return max;
00109 }
00110 
00111 
00112 returnValue ForwardIRKExport::getDataDeclarations(      ExportStatementBlock& declarations,
00113                                                                                                 ExportStruct dataStruct
00114                                                                                                 ) const
00115 {
00116         ImplicitRungeKuttaExport::getDataDeclarations( declarations, dataStruct );
00117         
00118         declarations.addDeclaration( rk_diffK,dataStruct );
00119 
00120         declarations.addDeclaration( rk_diffsTemp3,dataStruct );
00121 
00122         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
00123                 declarations.addDeclaration( rk_diffsPrev1,dataStruct );
00124                 declarations.addDeclaration( rk_diffsPrev2,dataStruct );
00125                 declarations.addDeclaration( rk_diffsPrev3,dataStruct );
00126         }
00127 
00128         declarations.addDeclaration( rk_diffsNew1,dataStruct );
00129         declarations.addDeclaration( rk_diffsNew2,dataStruct );
00130         declarations.addDeclaration( rk_diffsNew3,dataStruct );
00131         
00132         if( CONTINUOUS_OUTPUT ) {
00133                 declarations.addDeclaration( rk_diffsOutputTemp,dataStruct );
00134         }
00135 
00136     return SUCCESSFUL_RETURN;
00137 }
00138 
00139 
00140 returnValue ForwardIRKExport::getFunctionDeclarations(  ExportStatementBlock& declarations
00141                                                                                                         ) const
00142 {
00143         ImplicitRungeKuttaExport::getFunctionDeclarations( declarations );
00144 
00145     return SUCCESSFUL_RETURN;
00146 }
00147 
00148 
00149 returnValue ForwardIRKExport::getCode(  ExportStatementBlock& code )
00150 {
00151         int sensGen;
00152         get( DYNAMIC_SENSITIVITY, sensGen );
00153         if ( (ExportSensitivityType)sensGen != FORWARD ) ACADOERROR( RET_INVALID_OPTION );
00154 
00155         int useOMP;
00156         get(CG_USE_OPENMP, useOMP);
00157         if ( useOMP ) {
00158                 ExportVariable max = getAuxVariable();
00159                 max.setName( "auxVar" );
00160                 max.setDataStruct( ACADO_LOCAL );
00161                 if( NX2 > 0 || NXA > 0 ) {
00162                         rhs.setGlobalExportVariable( max );
00163                         diffs_rhs.setGlobalExportVariable( max );
00164                 }
00165                 if( NX3 > 0 ) {
00166                         rhs3.setGlobalExportVariable( max );
00167                         diffs_rhs3.setGlobalExportVariable( max );
00168                 }
00169                 for( uint i = 0; i < outputs.size(); i++ ) {
00170                         outputs[i].setGlobalExportVariable( max );
00171                         diffs_outputs[i].setGlobalExportVariable( max );
00172                 }
00173 
00174                 getDataDeclarations( code, ACADO_LOCAL );
00175 
00176                 stringstream s;
00177                 s << "#pragma omp threadprivate( "
00178                                 << max.getFullName() << ", "
00179                                 << rk_ttt.getFullName() << ", "
00180                                 << rk_xxx.getFullName() << ", "
00181                                 << rk_kkk.getFullName() << ", "
00182                                 << rk_diffK.getFullName() << ", "
00183                                 << rk_rhsTemp.getFullName() << ", "
00184                                 << rk_auxSolver.getFullName();
00185                 if( NX1 > 0 ) {
00186                         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev1.getFullName();
00187                         s << ", " << rk_diffsNew1.getFullName();
00188                 }
00189                 if( NX2 > 0 || NXA > 0 ) {
00190                         s << ", " << rk_A.getFullName();
00191                         s << ", " << rk_b.getFullName();
00192                         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev2.getFullName();
00193                         s << ", " << rk_diffsNew2.getFullName();
00194                         s << ", " << rk_diffsTemp2.getFullName();
00195                         solver->appendVariableNames( s );
00196                 }
00197                 if( NX3 > 0 ) {
00198                         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) s << ", " << rk_diffsPrev3.getFullName();
00199                         s << ", " << rk_diffsNew3.getFullName();
00200                         s << ", " << rk_diffsTemp3.getFullName();
00201                 }
00202                 s << " )" << endl << endl;
00203                 code.addStatement( s.str().c_str() );
00204         }
00205 
00206         if( NX1 > 0 ) {
00207                 code.addFunction( lin_input );
00208                 code.addStatement( "\n\n" );
00209         }
00210         if( exportRhs ) {
00211                 if( NX2 > 0 || NXA > 0 ) {
00212                         code.addFunction( rhs );
00213                         code.addStatement( "\n\n" );
00214                         code.addFunction( diffs_rhs );
00215                         code.addStatement( "\n\n" );
00216                 }
00217 
00218                 if( NX3 > 0 ) {
00219                         code.addFunction( rhs3 );
00220                         code.addStatement( "\n\n" );
00221                         code.addFunction( diffs_rhs3 );
00222                         code.addStatement( "\n\n" );
00223                 }
00224 
00225                 if( CONTINUOUS_OUTPUT ) {
00226                         uint i;
00227                         for( i = 0; i < outputs.size(); i++ ) {
00228                                 code.addFunction( outputs[i] );
00229                                 code.addStatement( "\n\n" );
00230                                 code.addFunction( diffs_outputs[i] );
00231                                 code.addStatement( "\n\n" );
00232                         }
00233                 }
00234         }
00235         if( NX2 > 0 || NXA > 0 ) solver->getCode( code );
00236         code.addLinebreak(2);
00237 
00238         int measGrid;
00239         get( MEASUREMENT_GRID, measGrid );
00240 
00241         // export RK scheme
00242         uint run5;
00243         std::string tempString;
00244         
00245         initializeDDMatrix();
00246         initializeCoefficients();
00247 
00248         double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
00249         DMatrix tmp = AA;
00250         ExportVariable Ah( "Ah_mat", tmp*=h, STATIC_CONST_REAL );
00251         code.addDeclaration( Ah );
00252         code.addLinebreak( 2 );
00253         // TODO: Ask Milan why this does NOT work properly !!
00254         Ah = ExportVariable( "Ah_mat", numStages, numStages, STATIC_CONST_REAL, ACADO_LOCAL );
00255 
00256         DVector BB( bb );
00257         ExportVariable Bh( "Bh_mat", DMatrix( BB*=h ) );
00258 
00259         DVector CC( cc );
00260         ExportVariable C;
00261         if( timeDependant ) {
00262                 C = ExportVariable( "C_mat", DMatrix( CC*=(1.0/grid.getNumIntervals()) ), STATIC_CONST_REAL );
00263                 code.addDeclaration( C );
00264                 code.addLinebreak( 2 );
00265                 C = ExportVariable( "C_mat", 1, numStages, STATIC_CONST_REAL, ACADO_LOCAL );
00266         }
00267 
00268         code.addComment(std::string("Fixed step size:") + toString(h));
00269 
00270         ExportVariable determinant( "det", 1, 1, REAL, ACADO_LOCAL, true );
00271         integrate.addDeclaration( determinant );
00272 
00273         ExportIndex i( "i" );
00274         ExportIndex j( "j" );
00275         ExportIndex k( "k" );
00276         ExportIndex run( "run" );
00277         ExportIndex run1( "run1" );
00278         ExportIndex tmp_index1("tmp_index1");
00279         ExportIndex tmp_index2("tmp_index2");
00280         ExportIndex tmp_index3("tmp_index3");
00281         ExportIndex tmp_index4("tmp_index4");
00282         ExportVariable tmp_meas("tmp_meas", 1, outputGrids.size(), INT, ACADO_LOCAL);
00283 
00284         ExportVariable numInt( "numInts", 1, 1, INT );
00285         if( !equidistantControlGrid() ) {
00286                 ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
00287                 code.addDeclaration( numStepsV );
00288                 code.addLinebreak( 2 );
00289                 integrate.addStatement( std::string( "int " ) + numInt.getName() + " = " + numStepsV.getName() + "[" + rk_index.getName() + "];\n" );
00290         }
00291 
00292         prepareOutputEvaluation( code );
00293 
00294         integrate.addIndex( i );
00295         integrate.addIndex( j );
00296         integrate.addIndex( k );
00297         integrate.addIndex( run );
00298         integrate.addIndex( run1 );
00299         integrate.addIndex( tmp_index1 );
00300         integrate.addIndex( tmp_index2 );
00301         if( rk_outputs.size() > 0 ) integrate.addIndex( tmp_index3 );
00302         if( rk_outputs.size() > 0 && (grid.getNumIntervals() > 1 || !equidistantControlGrid()) ) {
00303                 integrate.addIndex( tmp_index4 );
00304         }
00305         ExportVariable time_tmp( "time_tmp", 1, 1, REAL, ACADO_LOCAL, true );
00306         if( CONTINUOUS_OUTPUT ) {
00307                 for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
00308                         ExportIndex numMeasTmp( (std::string)"numMeasTmp" + toString(run5) );
00309                         numMeas.push_back( numMeasTmp );
00310                         integrate.addIndex( numMeas[run5] );
00311                 }
00312 
00313                 if( (MeasurementGrid)measGrid == ONLINE_GRID ) {
00314                         integrate.addDeclaration( tmp_meas );
00315                         integrate.addDeclaration( polynEvalVar );
00316                         integrate.addDeclaration( time_tmp );
00317                 }
00318 
00319                 for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
00320                         integrate.addStatement( numMeas[run5] == 0 );
00321                 }
00322         }
00323         integrate.addStatement( rk_ttt == DMatrix(grid.getFirstTime()) );
00324         if( (inputDim-diffsDim) > NX+NXA ) {
00325                 integrate.addStatement( rk_xxx.getCols( NX+NXA,inputDim-diffsDim ) == rk_eta.getCols( NX+NXA+diffsDim,inputDim ) );
00326         }
00327         integrate.addLinebreak( );
00328 
00329     // integrator loop:
00330         ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
00331         ExportStatementBlock *loop;
00332         if( equidistantControlGrid() ) {
00333                 loop = &tmpLoop;
00334         }
00335         else {
00336             loop = &integrate;
00337                 loop->addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
00338         }
00339 
00340         if( CONTINUOUS_OUTPUT && (MeasurementGrid)measGrid == ONLINE_GRID ) {
00341                 for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
00342                         loop->addStatement( tmp_index1 == numMeas[run5] );
00343                         loop->addStatement( std::string("while( ") + tmp_index1.getName() + " < " + toString(totalMeas[run5]) + " && " + gridVariables[run5].get(0,tmp_index1) + " <= (" + rk_ttt.getFullName() + "+" + toString(1.0/grid.getNumIntervals()) + ") ) {\n" );
00344                         loop->addStatement( tmp_index1 == tmp_index1+1 );
00345                         loop->addStatement( std::string("}\n") );
00346                         loop->addStatement( std::string(tmp_meas.get( 0,run5 )) + " = " + tmp_index1.getName() + " - " + numMeas[run5].getName() + ";\n" );
00347                 }
00348         }
00349 
00350         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
00351                 // Set rk_diffsPrev:
00352                 loop->addStatement( std::string("if( run > 0 ) {\n") );
00353                 if( NX1 > 0 ) {
00354                         ExportForLoop loopTemp1( i,0,NX1 );
00355                         loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,0,NX1 ) == rk_eta.getCols( i*NX+NX+NXA,i*NX+NX+NXA+NX1 ) );
00356                         if( NU > 0 ) loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,NX1,NX1+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1),i*NU+(NX+NXA)*(NX+1)+NU ) );
00357                         loop->addStatement( loopTemp1 );
00358                 }
00359                 if( NX2 > 0 ) {
00360                         ExportForLoop loopTemp2( i,0,NX2 );
00361                         loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,0,NX1+NX2 ) == rk_eta.getCols( i*NX+NX+NXA+NX1*NX,i*NX+NX+NXA+NX1*NX+NX1+NX2 ) );
00362                         if( NU > 0 ) loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,NX1+NX2,NX1+NX2+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1)+NX1*NU,i*NU+(NX+NXA)*(NX+1)+NX1*NU+NU ) );
00363                         loop->addStatement( loopTemp2 );
00364                 }
00365                 if( NX3 > 0 ) {
00366                         ExportForLoop loopTemp3( i,0,NX3 );
00367                         loopTemp3.addStatement( rk_diffsPrev3.getSubMatrix( i,i+1,0,NX ) == rk_eta.getCols( i*NX+NX+NXA+(NX1+NX2)*NX,i*NX+NX+NXA+(NX1+NX2)*NX+NX ) );
00368                         if( NU > 0 ) loopTemp3.addStatement( rk_diffsPrev3.getSubMatrix( i,i+1,NX,NX+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1)+(NX1+NX2)*NU,i*NU+(NX+NXA)*(NX+1)+(NX1+NX2)*NU+NU ) );
00369                         loop->addStatement( loopTemp3 );
00370                 }
00371                 loop->addStatement( std::string("}\n") );
00372         }
00373 
00374         // PART 1: The linear input system
00375         prepareInputSystem( code );
00376         solveInputSystem( loop, i, run1, j, tmp_index1, Ah );
00377 
00378         // PART 2: The fully implicit system
00379         solveImplicitSystem( loop, i, run1, j, tmp_index1, Ah, C, determinant, true );
00380 
00381         // PART 3: The linear output system
00382         prepareOutputSystem( code );
00383         solveOutputSystem( loop, i, run1, j, tmp_index1, Ah, true );
00384 
00385         // generate continuous OUTPUT:
00386         generateOutput( loop, run, i, tmp_index2, tmp_index3, tmp_meas, time_tmp, NX+NU );
00387 
00388         // DERIVATIVES wrt the states (IFT):
00389         if( NX1 > 0 ) {
00390                 ExportForLoop loop4( run1,0,NX1 );
00391                 // PART 1: The linear input system
00392                 sensitivitiesInputSystem( &loop4, run1, i, Bh, true );
00393                 // PART 2: The fully implicit system
00394                 sensitivitiesImplicitSystem( &loop4, run1, i, j, tmp_index1, tmp_index2, Ah, Bh, determinant, true, 1 );
00395                 // PART 3: The linear output system
00396                 sensitivitiesOutputSystem( &loop4, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, true, 1 );
00397                 // generate sensitivities wrt states for continuous output:
00398                 sensitivitiesOutputs( &loop4, run, run1, i, tmp_index1, tmp_index2, tmp_index3, tmp_meas, time_tmp, true, 0 );
00399                 loop->addStatement( loop4 );
00400         }
00401         if( NX2 > 0 ) {
00402                 ExportForLoop loop4( run1,NX1,NX1+NX2 );
00403                 // PART 2: The fully implicit system
00404                 sensitivitiesImplicitSystem( &loop4, run1, i, j, tmp_index1, tmp_index2, Ah, Bh, determinant, true, 2 );
00405                 // PART 3: The linear output system
00406                 sensitivitiesOutputSystem( &loop4, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, true, 2 );
00407                 // generate sensitivities wrt states for continuous output:
00408                 sensitivitiesOutputs( &loop4, run, run1, i, tmp_index1, tmp_index2, tmp_index3, tmp_meas, time_tmp, true, NX1 );
00409                 loop->addStatement( loop4 );
00410         }
00411         if( NX3 > 0 ) {
00412                 ExportForLoop loop4( run1,NX1+NX2,NX );
00413                 // PART 3: The linear output system
00414                 sensitivitiesOutputSystem( &loop4, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, true, 3 );
00415                 // generate sensitivities wrt states for continuous output:
00416                 sensitivitiesOutputs( &loop4, run, run1, i, tmp_index1, tmp_index2, tmp_index3, tmp_meas, time_tmp, true, NX1+NX2 );
00417                 loop->addStatement( loop4 );
00418         }
00419 
00420 
00421         // DERIVATIVES wrt the control inputs (IFT):
00422         if( NU > 0 ) {
00423                 ExportForLoop loop5( run1,0,NU );
00424                 // PART 1: The linear input system
00425                 sensitivitiesInputSystem( &loop5, run1, i, Bh, false );
00426                 // PART 2: The fully implicit system
00427                 sensitivitiesImplicitSystem( &loop5, run1, i, j, tmp_index1, tmp_index2, Ah, Bh, determinant, false, 0 );
00428                 // PART 3: The linear output system
00429                 sensitivitiesOutputSystem( &loop5, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, false, 0 );
00430                 // generate sensitivities wrt controls for continuous output:
00431                 sensitivitiesOutputs( &loop5, run, run1, i, tmp_index1, tmp_index2, tmp_index3, tmp_meas, time_tmp, false, 0 );
00432                 loop->addStatement( loop5 );
00433         }
00434 
00435         // update rk_eta:
00436         for( run5 = 0; run5 < NX; run5++ ) {
00437                 loop->addStatement( rk_eta.getCol( run5 ) += rk_kkk.getRow( run5 )*Bh );
00438         }
00439         if( NXA > 0) {
00440                 DMatrix tempCoefs( evaluateDerivedPolynomial( 0.0 ) );
00441                 loop->addStatement( std::string("if( run == 0 ) {\n") );
00442                 for( run5 = 0; run5 < NXA; run5++ ) {
00443                         loop->addStatement( rk_eta.getCol( NX+run5 ) == rk_kkk.getRow( NX+run5 )*tempCoefs );
00444                 }
00445                 loop->addStatement( std::string("}\n") );
00446         }
00447 
00448 
00449         // Computation of the sensitivities using the CHAIN RULE:
00450         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
00451                 loop->addStatement( std::string( "if( run == 0 ) {\n" ) );
00452         }
00453         // PART 1
00454         updateInputSystem(loop, i, j, tmp_index2);
00455         // PART 2
00456         updateImplicitSystem(loop, i, j, tmp_index2);
00457         // PART 3
00458         updateOutputSystem(loop, i, j, tmp_index2);
00459 
00460         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
00461                 loop->addStatement( std::string( "}\n" ) );
00462                 loop->addStatement( std::string( "else {\n" ) );
00463                 // PART 1
00464                 propagateInputSystem(loop, i, j, k, tmp_index2);
00465                 // PART 2
00466                 propagateImplicitSystem(loop, i, j, k, tmp_index2);
00467                 // PART 3
00468                 propagateOutputSystem(loop, i, j, k, tmp_index2);
00469         }
00470 
00471         if( rk_outputs.size() > 0 && (grid.getNumIntervals() > 1 || !equidistantControlGrid()) ) {
00472                 propagateOutputs( loop, run, run1, i, j, k, tmp_index1, tmp_index2, tmp_index3, tmp_index4, tmp_meas );
00473         }
00474 
00475         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
00476                 loop->addStatement( std::string( "}\n" ) );
00477         }
00478 
00479         loop->addStatement( std::string( reset_int.get(0,0) ) + " = 0;\n" );
00480 
00481         for( run5 = 0; run5 < rk_outputs.size(); run5++ ) {
00482                 if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
00483                         loop->addStatement( numMeas[run5].getName() + " += " + numMeasVariables[run5].get(0,run) + ";\n" );
00484                 }
00485                 else { // ONLINE_GRID
00486                         loop->addStatement( numMeas[run5].getName() + " += " + tmp_meas.get(0,run5) + ";\n" );
00487                 }
00488         }
00489         loop->addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
00490 
00491     // end of the integrator loop.
00492     if( !equidistantControlGrid() ) {
00493                 loop->addStatement( "}\n" );
00494         }
00495     else {
00496         integrate.addStatement( *loop );
00497     }
00498     // PART 1
00499     if( NX1 > 0 ) {
00500         DMatrix zeroR = zeros<double>(1, NX2+NX3);
00501         ExportForLoop loop1( i,0,NX1 );
00502         loop1.addStatement( rk_eta.getCols( i*NX+NX+NXA+NX1,i*NX+NX+NXA+NX ) == zeroR );
00503         integrate.addStatement( loop1 );
00504     }
00505     // PART 2
00506     DMatrix zeroR = zeros<double>(1, NX3);
00507     if( NX2 > 0 ) {
00508         ExportForLoop loop2( i,NX1,NX1+NX2 );
00509         loop2.addStatement( rk_eta.getCols( i*NX+NX+NXA+NX1+NX2,i*NX+NX+NXA+NX ) == zeroR );
00510         integrate.addStatement( loop2 );
00511     }
00512     if( NXA > 0 ) {
00513         ExportForLoop loop3( i,NX,NX+NXA );
00514         loop3.addStatement( rk_eta.getCols( i*NX+NX+NXA+NX1+NX2,i*NX+NX+NXA+NX ) == zeroR );
00515         integrate.addStatement( loop3 );
00516     }
00517 
00518     integrate.addStatement( std::string( "if( " ) + determinant.getFullName() + " < 1e-12 ) {\n" );
00519     integrate.addStatement( error_code == 2 );
00520     integrate.addStatement( std::string( "} else if( " ) + determinant.getFullName() + " < 1e-6 ) {\n" );
00521     integrate.addStatement( error_code == 1 );
00522     integrate.addStatement( std::string( "} else {\n" ) );
00523     integrate.addStatement( error_code == 0 );
00524     integrate.addStatement( std::string( "}\n" ) );
00525 
00526         code.addFunction( integrate );
00527     code.addLinebreak( 2 );
00528 
00529     return SUCCESSFUL_RETURN;
00530 }
00531 
00532 
00533 returnValue ForwardIRKExport::propagateOutputs( ExportStatementBlock* block, const ExportIndex& index, const ExportIndex& index0, const ExportIndex& index1,
00534                                                                                                                         const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2,
00535                                                                                                                         const ExportIndex& tmp_index3, const ExportIndex& tmp_index4, const ExportVariable& tmp_meas )
00536 {
00537         uint i;
00538         int measGrid;
00539         get( MEASUREMENT_GRID, measGrid );
00540 
00541         // chain rule for the sensitivities of the continuous output:
00542         for( i = 0; i < rk_outputs.size(); i++ ) {
00543                 ExportStatementBlock *loop01;
00544                 if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
00545                         loop01 = block;
00546                         loop01->addStatement( std::string("for(") + index0.getName() + " = 0; " + index0.getName() + " < (int)" + numMeasVariables[i].get(0,index) + "; " + index0.getName() + "++) {\n" );
00547                 }
00548                 else { // ONLINE_GRID
00549                         loop01 = block;
00550                         loop01->addStatement( std::string("for(") + index0.getName() + " = 0; " + index0.getName() + " < (int)" + tmp_meas.get(0,i) + "; " + index0.getName() + "++) {\n" );
00551                 }
00552 
00553                 uint numOutputs = getDimOUTPUT( i );
00554                 uint outputDim = numOutputs*(NX+NU+1);
00555                 loop01->addStatement( tmp_index1 == numMeas[i]*outputDim+index0*(numOutputs*(NX+NU+1)) );
00556                 ExportForLoop loop02( index3,0,numOutputs*(NX+NU) );
00557                 loop02.addStatement( tmp_index2 == tmp_index1+index3 );
00558                 loop02.addStatement( rk_diffsOutputTemp.getCol( index3 ) == rk_outputs[i].getCol( tmp_index2+numOutputs ) );
00559                 loop01->addStatement( loop02 );
00560 
00561                 loop01->addStatement( tmp_index1 == numMeas[i]*outputDim+index0*(numOutputs*(NX+NU+1)) );
00562                 ExportForLoop loop03( index1,0,numOutputs );
00563                 loop03.addStatement( tmp_index2 == tmp_index1+index1*NX );
00564 
00565                 ExportForLoop loop04( index2,0,NX1 );
00566                 loop04.addStatement( tmp_index3 == tmp_index2+index2 );
00567                 loop04.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) == 0.0 );
00568                 ExportForLoop loop05( index3,0,NX1 );
00569                 loop05.addStatement( tmp_index4 == index1*NX+index3 );
00570                 loop05.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev1.getElement( index3,index2 ) );
00571                 loop04.addStatement( loop05 );
00572                 ExportForLoop loop06( index3,NX1,NX1+NX2 );
00573                 loop06.addStatement( tmp_index4 == index1*NX+index3 );
00574                 loop06.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev2.getElement( index3-NX1,index2 ) );
00575                 loop04.addStatement( loop06 );
00576                 ExportForLoop loop062( index3,NX1+NX2,NX );
00577                 loop062.addStatement( tmp_index4 == index1*NX+index3 );
00578                 loop062.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev3.getElement( index3-NX1-NX2,index2 ) );
00579                 loop04.addStatement( loop062 );
00580                 loop03.addStatement( loop04 );
00581 
00582                 ExportForLoop loop07( index2,NX1,NX1+NX2 );
00583                 loop07.addStatement( tmp_index3 == tmp_index2+index2 );
00584                 loop07.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) == 0.0 );
00585                 ExportForLoop loop08( index3,NX1,NX1+NX2 );
00586                 loop08.addStatement( tmp_index4 == index1*NX+index3 );
00587                 loop08.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev2.getElement( index3-NX1,index2 ) );
00588                 loop07.addStatement( loop08 );
00589                 ExportForLoop loop082( index3,NX1+NX2,NX );
00590                 loop082.addStatement( tmp_index4 == index1*NX+index3 );
00591                 loop082.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev3.getElement( index3-NX1-NX2,index2 ) );
00592                 loop07.addStatement( loop082 );
00593                 loop03.addStatement( loop07 );
00594 
00595                 ExportForLoop loop09( index2,NX1+NX2,NX );
00596                 loop09.addStatement( tmp_index3 == tmp_index2+index2 );
00597                 loop09.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) == 0.0 );
00598                 ExportForLoop loop10( index3,NX1+NX2,NX );
00599                 loop10.addStatement( tmp_index4 == index1*NX+index3 );
00600                 loop10.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev3.getElement( index3-NX1-NX2,index2 ) );
00601                 loop09.addStatement( loop10 );
00602                 loop03.addStatement( loop09 );
00603 
00604                 if( NU > 0 ) {
00605                         loop03.addStatement( tmp_index2 == tmp_index1+index1*NU );
00606                         ExportForLoop loop11( index2,0,NU );
00607                         loop11.addStatement( tmp_index3 == tmp_index2+index2 );
00608                         loop11.addStatement( tmp_index4 == index1*NU+index2 );
00609                         loop11.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs*(1+NX) ) == rk_diffsOutputTemp.getCol( tmp_index4+numOutputs*NX ) );
00610                         ExportForLoop loop12( index3,0,NX1 );
00611                         loop12.addStatement( tmp_index4 == index1*NX+index3 );
00612                         loop12.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs*(1+NX) ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev1.getElement( index3,NX1+index2 ) );
00613                         loop11.addStatement( loop12 );
00614                         ExportForLoop loop13( index3,NX1,NX1+NX2 );
00615                         loop13.addStatement( tmp_index4 == index1*NX+index3 );
00616                         loop13.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs*(1+NX) ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev2.getElement( index3-NX1,NX1+NX2+index2 ) );
00617                         loop11.addStatement( loop13 );
00618                         ExportForLoop loop132( index3,NX1+NX2,NX );
00619                         loop132.addStatement( tmp_index4 == index1*NX+index3 );
00620                         loop132.addStatement( rk_outputs[i].getCol( tmp_index3+numOutputs*(1+NX) ) += rk_diffsOutputTemp.getCol( tmp_index4 )*rk_diffsPrev3.getElement( index3-NX1-NX2,NX+index2 ) );
00621                         loop11.addStatement( loop132 );
00622                         loop03.addStatement( loop11 );
00623                 }
00624 
00625                 loop01->addStatement( loop03 );
00626                 loop01->addStatement( "}\n" );
00627         }
00628 
00629         return SUCCESSFUL_RETURN;
00630 }
00631 
00632 
00633 returnValue ForwardIRKExport::prepareInputSystem(       ExportStatementBlock& code )
00634 {
00635         if( NX1 > 0 ) {
00636                 DMatrix mat1 = formMatrix( M11, A11 );
00637                 rk_mat1 = ExportVariable( "rk_mat1", mat1, STATIC_CONST_REAL );
00638                 code.addDeclaration( rk_mat1 );
00639                 // TODO: Ask Milan why this does NOT work properly !!
00640                 rk_mat1 = ExportVariable( "rk_mat1", numStages*NX1, numStages*NX1, STATIC_CONST_REAL, ACADO_LOCAL );
00641 
00642                 DMatrix sens = zeros<double>(NX1*(NX1+NU), numStages);
00643                 uint i, j, k;
00644                 for( i = 0; i < NX1; i++ ) {
00645                         DVector vec(NX1*numStages);
00646                         for( j = 0; j < numStages; j++ ) {
00647                                 for( k = 0; k < NX1; k++ ) {
00648                                         vec(j*NX1+k) = A11(k,i);
00649                                 }
00650                         }
00651                         DVector sol = mat1*vec;
00652                         for( j = 0; j < numStages; j++ ) {
00653                                 for( k = 0; k < NX1; k++ ) {
00654                                         sens(i*NX1+k,j) = sol(j*NX1+k);
00655                                 }
00656                         }
00657                 }
00658                 for( i = 0; i < NU; i++ ) {
00659                         DVector vec(NX1*numStages);
00660                         for( j = 0; j < numStages; j++ ) {
00661                                 for( k = 0; k < NX1; k++ ) {
00662                                         vec(j*NX1+k) = B11(k,i);
00663                                 }
00664                         }
00665                         DVector sol = mat1*vec;
00666                         for( j = 0; j < numStages; j++ ) {
00667                                 for( k = 0; k < NX1; k++ ) {
00668                                         sens(NX1*NX1+i*NX1+k,j) = sol(j*NX1+k);
00669                                 }
00670                         }
00671                 }
00672                 rk_dk1 = ExportVariable( "rk_dk1", sens, STATIC_CONST_REAL );
00673                 code.addDeclaration( rk_dk1 );
00674                 // TODO: Ask Milan why this does NOT work properly !!
00675                 rk_dk1 = ExportVariable( "rk_dk1", NX1*(NX1+NU), numStages, STATIC_CONST_REAL, ACADO_LOCAL );
00676         }
00677 
00678         return SUCCESSFUL_RETURN;
00679 }
00680 
00681 
00682 returnValue ForwardIRKExport::prepareOutputSystem(      ExportStatementBlock& code )
00683 {
00684         if( NX3 > 0 ) {
00685                 DMatrix mat3 = formMatrix( M33, A33 );
00686                 rk_mat3 = ExportVariable( "rk_mat3", mat3, STATIC_CONST_REAL );
00687                 code.addDeclaration( rk_mat3 );
00688                 // TODO: Ask Milan why this does NOT work properly !!
00689                 rk_mat3 = ExportVariable( "rk_mat3", numStages*NX3, numStages*NX3, STATIC_CONST_REAL, ACADO_LOCAL );
00690 
00691                 DMatrix sens = zeros<double>(NX3*NX3, numStages);
00692                 uint i, j, k;
00693                 for( i = 0; i < NX3; i++ ) {
00694                         DVector vec(NX3*numStages);
00695                         for( j = 0; j < numStages; j++ ) {
00696                                 for( k = 0; k < NX3; k++ ) {
00697                                         vec(j*NX3+k) = A33(k,i);
00698                                 }
00699                         }
00700                         DVector sol = mat3*vec;
00701                         for( j = 0; j < numStages; j++ ) {
00702                                 for( k = 0; k < NX3; k++ ) {
00703                                         sens(i*NX3+k,j) = sol(j*NX3+k);
00704                                 }
00705                         }
00706                 }
00707                 rk_dk3 = ExportVariable( "rk_dk3", sens, STATIC_CONST_REAL );
00708                 code.addDeclaration( rk_dk3 );
00709                 // TODO: Ask Milan why this does NOT work properly !!
00710                 rk_dk3 = ExportVariable( "rk_dk3", NX3*NX3, numStages, STATIC_CONST_REAL, ACADO_LOCAL );
00711         }
00712 
00713         return SUCCESSFUL_RETURN;
00714 }
00715 
00716 
00717 returnValue ForwardIRKExport::sensitivitiesInputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportVariable& Bh, bool STATES )
00718 {
00719         if( NX1 > 0 ) {
00720                 // update rk_diffK with the new sensitivities:
00721                 if( STATES )    block->addStatement( rk_diffK.getRows(0,NX1) == rk_dk1.getRows(index1*NX1,index1*NX1+NX1) );
00722                 else                    block->addStatement( rk_diffK.getRows(0,NX1) == rk_dk1.getRows(index1*NX1+NX1*NX1,index1*NX1+NX1+NX1*NX1) );
00723                 // update rk_diffsNew with the new sensitivities:
00724                 if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) block->addStatement( std::string( "if( run == 0 ) {\n" ) );
00725                 ExportForLoop loop3( index2,0,NX1 );
00726                 if( STATES ) loop3.addStatement( std::string(rk_diffsNew1.get( index2,index1 )) + " = (" + index2.getName() + " == " + index1.getName() + ");\n" );
00727 
00728                 if( STATES ) loop3.addStatement( rk_diffsNew1.getElement( index2,index1 ) += rk_diffK.getRow( index2 )*Bh );
00729                 else             loop3.addStatement( rk_diffsNew1.getElement( index2,index1+NX1 ) == rk_diffK.getRow( index2 )*Bh );
00730                 block->addStatement( loop3 );
00731                 if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) block->addStatement( std::string( "}\n" ) );
00732         }
00733 
00734         return SUCCESSFUL_RETURN;
00735 }
00736 
00737 
00738 returnValue ForwardIRKExport::sensitivitiesImplicitSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2, const ExportVariable& Ah, const ExportVariable& Bh, const ExportVariable& det, bool STATES, uint number )
00739 {
00740         if( NX2 > 0 ) {
00741                 DMatrix zeroM = zeros<double>( NX2+NXA,1 );
00742                 DMatrix tempCoefs( evaluateDerivedPolynomial( 0.0 ) );
00743                 uint i;
00744 
00745                 ExportForLoop loop1( index2,0,numStages );
00746                 if( STATES && number == 1 ) {
00747                         ExportForLoop loop2( index3,0,NX1 );
00748                         loop2.addStatement( std::string(rk_rhsTemp.get( index3,0 )) + " = -(" + index3.getName() + " == " + index1.getName() + ");\n" );
00749                         for( i = 0; i < numStages; i++ ) {
00750                                 loop2.addStatement( rk_rhsTemp.getRow( index3 ) -= rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
00751                         }
00752                         loop1.addStatement( loop2 );
00753                         ExportForLoop loop3( index3,0,NX2+NXA );
00754                         loop3.addStatement( tmp_index1 == index2*(NX2+NXA)+index3 );
00755                         loop3.addStatement( rk_b.getRow( tmp_index1 ) == rk_diffsTemp2.getSubMatrix( index2,index2+1,index3*(NVARS2),index3*(NVARS2)+NX1 )*rk_rhsTemp.getRows(0,NX1) );
00756                         if( NDX2 > 0 ) {
00757                                 loop3.addStatement( rk_b.getRow( tmp_index1 ) -= rk_diffsTemp2.getSubMatrix( index2,index2+1,index3*(NVARS2)+NVARS2-NX1-NX2,index3*(NVARS2)+NVARS2-NX2 )*rk_diffK.getSubMatrix( 0,NX1,index2,index2+1 ) );
00758                         }
00759                         loop1.addStatement( loop3 );
00760                 }
00761                 else if( STATES && number == 2 ) {
00762                         for( i = 0; i < NX2+NXA; i++ ) {
00763                                 loop1.addStatement( rk_b.getRow( index2*(NX2+NXA)+i ) == zeroM.getRow( 0 ) - rk_diffsTemp2.getElement( index2,index1+i*(NVARS2) ) );
00764                         }
00765                 }
00766                 else { // ~= STATES
00767                         ExportForLoop loop2( index3,0,NX1 );
00768                         loop2.addStatement( rk_rhsTemp.getRow( index3 ) == rk_diffK.getElement( index3,0 )*Ah.getElement(index2,0) );
00769                         for( i = 1; i < numStages; i++ ) {
00770                                 loop2.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
00771                         }
00772                         loop1.addStatement( loop2 );
00773                         ExportForLoop loop3( index3,0,NX2+NXA );
00774                         loop3.addStatement( tmp_index1 == index2*(NX2+NXA)+index3 );
00775                         loop3.addStatement( tmp_index2 == index1+index3*(NVARS2) );
00776                         loop3.addStatement( rk_b.getRow( tmp_index1 ) == zeroM.getRow( 0 ) - rk_diffsTemp2.getElement( index2,tmp_index2+NX1+NX2+NXA ) );
00777                         loop3.addStatement( rk_b.getRow( tmp_index1 ) -= rk_diffsTemp2.getSubMatrix( index2,index2+1,index3*(NVARS2),index3*(NVARS2)+NX1 )*rk_rhsTemp.getRows(0,NX1) );
00778                         if( NDX2 > 0 ) {
00779                                 loop3.addStatement( rk_b.getRow( tmp_index1 ) -= rk_diffsTemp2.getSubMatrix( index2,index2+1,index3*(NVARS2)+NVARS2-NX1-NX2,index3*(NVARS2)+NVARS2-NX2 )*rk_diffK.getSubMatrix( 0,NX1,index2,index2+1 ) );
00780                         }
00781                         loop1.addStatement( loop3 );
00782                 }
00783                 block->addStatement( loop1 );
00784                 if( STATES && (number == 1 || NX1 == 0) ) {
00785                         block->addStatement( std::string( "if( 0 == " ) + index1.getName() + " ) {\n" );        // factorization of the new matrix rk_A not yet calculated!
00786                         block->addStatement( det.getFullName() + " = " + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_b.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
00787                         block->addStatement( std::string( "}\n else {\n" ) );
00788                 }
00789                 block->addFunctionCall( solver->getNameSolveReuseFunction(),rk_A.getAddress(0,0),rk_b.getAddress(0,0),rk_auxSolver.getAddress(0,0) );
00790                 if( STATES && (number == 1 || NX1 == 0) ) block->addStatement( std::string( "}\n" ) );
00791                 // update rk_diffK with the new sensitivities:
00792                 ExportForLoop loop2( index2,0,numStages );
00793                 loop2.addStatement( rk_diffK.getSubMatrix(NX1,NX1+NX2,index2,index2+1) == rk_b.getRows(index2*NX2,index2*NX2+NX2) );
00794                 loop2.addStatement( rk_diffK.getSubMatrix(NX,NX+NXA,index2,index2+1) == rk_b.getRows(numStages*NX2+index2*NXA,numStages*NX2+index2*NXA+NXA) );
00795                 block->addStatement( loop2 );
00796                 // update rk_diffsNew with the new sensitivities:
00797                 ExportForLoop loop3( index2,0,NX2 );
00798                 if( STATES && number == 2 ) loop3.addStatement( std::string(rk_diffsNew2.get( index2,index1 )) + " = (" + index2.getName() + " == " + index1.getName() + "-" + toString(NX1) + ");\n" );
00799 
00800                 if( STATES && number == 2 ) loop3.addStatement( rk_diffsNew2.getElement( index2,index1 ) += rk_diffK.getRow( NX1+index2 )*Bh );
00801                 else if( STATES )       loop3.addStatement( rk_diffsNew2.getElement( index2,index1 ) == rk_diffK.getRow( NX1+index2 )*Bh );
00802                 else                            loop3.addStatement( rk_diffsNew2.getElement( index2,index1+NX1+NX2 ) == rk_diffK.getRow( NX1+index2 )*Bh );
00803                 block->addStatement( loop3 );
00804                 if( NXA > 0 ) {
00805                         block->addStatement( std::string("if( run == 0 ) {\n") );
00806                         ExportForLoop loop4( index2,0,NXA );
00807                         if( STATES ) loop4.addStatement( rk_diffsNew2.getElement( index2+NX2,index1 ) == rk_diffK.getRow( NX+index2 )*tempCoefs );
00808                         else             loop4.addStatement( rk_diffsNew2.getElement( index2+NX2,index1+NX1+NX2 ) == rk_diffK.getRow( NX+index2 )*tempCoefs );
00809                         block->addStatement( loop4 );
00810                         block->addStatement( std::string("}\n") );
00811                 }
00812         }
00813 
00814         return SUCCESSFUL_RETURN;
00815 }
00816 
00817 
00818 returnValue ForwardIRKExport::sensitivitiesOutputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& index4, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2, const ExportVariable& Ah, const ExportVariable& Bh, bool STATES, uint number )
00819 {
00820         if( NX3 > 0 ) {
00821                 uint i;
00822                 ExportForLoop loop1( index2,0,numStages );
00823                 if( STATES && number == 1 ) {
00824                         ExportForLoop loop2( index3,0,NX1 );
00825                         loop2.addStatement( std::string(rk_rhsTemp.get( index3,0 )) + " = (" + index3.getName() + " == " + index1.getName() + ");\n" );
00826                         for( i = 0; i < numStages; i++ ) {
00827                                 loop2.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
00828                         }
00829                         loop1.addStatement( loop2 );
00830                         ExportForLoop loop3( index3,NX1,NX1+NX2 );
00831                         loop3.addStatement( rk_rhsTemp.getRow( index3 ) == 0.0 );
00832                         for( i = 0; i < numStages; i++ ) {
00833                                 loop3.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
00834                         }
00835                         loop1.addStatement( loop3 );
00836                         ExportForLoop loop4( index3,0,NX3 );
00837                         loop4.addStatement( tmp_index1 == index2*NX3+index3 );
00838                         loop4.addStatement( rk_b.getRow( tmp_index1 ) == rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3),index3*(NVARS3)+NX1+NX2 )*rk_rhsTemp.getRows(0,NX1+NX2) );
00839                         if( NXA3 > 0 ) {
00840                                 loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NX1+NX2,index3*(NVARS3)+NX1+NX2+NXA )*rk_diffK.getSubMatrix( NX,NX+NXA,index2,index2+1 ) );
00841                         }
00842                         if( NDX3 > 0 ) {
00843                                 loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NVARS3-NX1-NX2,index3*(NVARS3)+NVARS3 )*rk_diffK.getSubMatrix( 0,NX1+NX2,index2,index2+1 ) );
00844                         }
00845                         loop1.addStatement( loop4 );
00846                 }
00847                 else if( STATES && number == 2 ) {
00848                         ExportForLoop loop3( index3,NX1,NX1+NX2 );
00849                         loop3.addStatement( std::string(rk_rhsTemp.get( index3,0 )) + " = (" + index3.getName() + " == " + index1.getName() + ");\n" );
00850                         for( i = 0; i < numStages; i++ ) {
00851                                 loop3.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
00852                         }
00853                         loop1.addStatement( loop3 );
00854                         ExportForLoop loop4( index3,0,NX3 );
00855                         loop4.addStatement( tmp_index1 == index2*NX3+index3 );
00856                         loop4.addStatement( rk_b.getRow( tmp_index1 ) == rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NX1,index3*(NVARS3)+NX1+NX2 )*rk_rhsTemp.getRows(NX1,NX1+NX2) );
00857                         if( NXA3 > 0 ) {
00858                                 loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NX1+NX2,index3*(NVARS3)+NX1+NX2+NXA )*rk_diffK.getSubMatrix( NX,NX+NXA,index2,index2+1 ) );
00859                         }
00860                         if( NDX3 > 0 ) {
00861                                 loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NVARS3-NX2,index3*(NVARS3)+NVARS3 )*rk_diffK.getSubMatrix( NX1,NX1+NX2,index2,index2+1 ) );
00862                         }
00863                         loop1.addStatement( loop4 );
00864                 }
00865                 else if( !STATES ) {
00866                         ExportForLoop loop2( index3,0,NX1 );
00867                         loop2.addStatement( rk_rhsTemp.getRow( index3 ) == rk_diffK.getElement( index3,0 )*Ah.getElement(index2,0) );
00868                         for( i = 1; i < numStages; i++ ) {
00869                                 loop2.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
00870                         }
00871                         loop1.addStatement( loop2 );
00872                         ExportForLoop loop3( index3,NX1,NX1+NX2 );
00873                         loop3.addStatement( rk_rhsTemp.getRow( index3 ) == rk_diffK.getElement( index3,0 )*Ah.getElement(index2,0) );
00874                         for( i = 1; i < numStages; i++ ) {
00875                                 loop3.addStatement( rk_rhsTemp.getRow( index3 ) += rk_diffK.getElement( index3,i )*Ah.getElement(index2,i) );
00876                         }
00877                         loop1.addStatement( loop3 );
00878                         ExportForLoop loop4( index3,0,NX3 );
00879                         loop4.addStatement( tmp_index1 == index2*NX3+index3 );
00880                         loop4.addStatement( tmp_index2 == index1+index3*(NVARS3) );
00881                         loop4.addStatement( rk_b.getRow( tmp_index1 ) == rk_diffsTemp3.getElement( index2,tmp_index2+NX1+NX2+NXA3 ) );
00882                         loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3),index3*(NVARS3)+NX1+NX2 )*rk_rhsTemp.getRows(0,NX1+NX2) );
00883                         if( NXA3 > 0 ) {
00884                                 loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NX1+NX2,index3*(NVARS3)+NX1+NX2+NXA )*rk_diffK.getSubMatrix( NX,NX+NXA,index2,index2+1 ) );
00885                         }
00886                         if( NDX3 > 0 ) {
00887                                 loop4.addStatement( rk_b.getRow( tmp_index1 ) += rk_diffsTemp3.getSubMatrix( index2,index2+1,index3*(NVARS3)+NVARS3-NX1-NX2,index3*(NVARS3)+NVARS3 )*rk_diffK.getSubMatrix( 0,NX1+NX2,index2,index2+1 ) );
00888                         }
00889                         loop1.addStatement( loop4 );
00890                 }
00891                 if( !STATES || number != 3 ) block->addStatement( loop1 );
00892 
00893                 // update rk_diffK with the new sensitivities:
00894                 if( STATES && number == 3 ) {
00895                         block->addStatement( rk_diffK.getRows(NX1+NX2,NX) == rk_dk3.getRows(index1*NX3-(NX1+NX2)*NX3,index1*NX3+NX3-(NX1+NX2)*NX3) );
00896                 }
00897                 else {
00898                         ExportForLoop loop4( index2,0,numStages );
00899                         ExportForLoop loop5( index3,0,NX3 );
00900                         loop5.addStatement( tmp_index1 == index2*NX3+index3 );
00901                         loop5.addStatement( rk_diffK.getElement(NX1+NX2+index3,index2) == rk_mat3.getElement(tmp_index1,0)*rk_b.getRow(0) );
00902                         ExportForLoop loop6( index4,1,numStages*NX3 );
00903                         loop6.addStatement( rk_diffK.getElement(NX1+NX2+index3,index2) += rk_mat3.getElement(tmp_index1,index4)*rk_b.getRow(index4) );
00904                         loop5.addStatement(loop6);
00905                         loop4.addStatement(loop5);
00906                         block->addStatement(loop4);
00907                 }
00908                 // update rk_diffsNew with the new sensitivities:
00909                 if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) block->addStatement( std::string( "if( run == 0 ) {\n" ) );
00910                 ExportForLoop loop8( index2,0,NX3 );
00911                 if( STATES && number == 3 ) loop8.addStatement( std::string(rk_diffsNew3.get( index2,index1 )) + " = (" + index2.getName() + " == " + index1.getName() + "-" + toString(NX1+NX2) + ");\n" );
00912 
00913                 if( STATES && number == 3 ) loop8.addStatement( rk_diffsNew3.getElement( index2,index1 ) += rk_diffK.getRow( NX1+NX2+index2 )*Bh );
00914                 else if( STATES )       loop8.addStatement( rk_diffsNew3.getElement( index2,index1 ) == rk_diffK.getRow( NX1+NX2+index2 )*Bh );
00915                 else                            loop8.addStatement( rk_diffsNew3.getElement( index2,index1+NX ) == rk_diffK.getRow( NX1+NX2+index2 )*Bh );
00916                 block->addStatement( loop8 );
00917                 if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) block->addStatement( std::string( "}\n" ) );
00918         }
00919 
00920         return SUCCESSFUL_RETURN;
00921 }
00922 
00923 
00924 returnValue ForwardIRKExport::sensitivitiesOutputs( ExportStatementBlock* block, const ExportIndex& index0,
00925                 const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2,
00926                 const ExportIndex& tmp_index3, const ExportVariable& tmp_meas, const ExportVariable& time_tmp, bool STATES, uint base )
00927 {
00928         uint i, j, k;
00929         int measGrid;
00930         get( MEASUREMENT_GRID, measGrid );
00931 
00932         for( i = 0; i < rk_outputs.size(); i++ ) {
00933                 uint NVARS = numVARS_output(i);
00934                 ExportStatementBlock *loop;
00935                 if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
00936                         loop = block;
00937                         loop->addStatement( std::string("for(") + index2.getName() + " = 0; " + index2.getName() + " < (int)" + numMeasVariables[i].get(0,index0) + "; " + index2.getName() + "++) {\n" );
00938                         loop->addStatement( tmp_index2 == numMeas[i]+index2 );
00939                         for( j = 0; j < numStages; j++ ) {
00940                                 loop->addStatement( rk_outH.getRow(j) == polynVariables[i].getElement( tmp_index2,j ) );
00941                         }
00942                         if( numXA_output(i) > 0 || numDX_output(i) > 0 ) {
00943                                 for( j = 0; j < numStages; j++ ) {
00944                                         loop->addStatement( rk_out.getRow(j) == polynDerVariables[i].getElement( tmp_index2,j ) );
00945                                 }
00946                         }
00947                 }
00948                 else { // ONLINE_GRID
00949                         loop = block;
00950                         loop->addStatement( std::string(tmp_index3.getName()) + " = " + tmp_meas.get( 0,i ) + ";\n" );
00951                         loop->addStatement( std::string("for(") + index2.getName() + " = 0; " + index2.getName() + " < (int)" + tmp_index3.getName() + "; " + index2.getName() + "++) {\n" );
00952                         loop->addStatement( tmp_index2 == numMeas[i]+index2 );
00953 
00954                         uint scale = grid.getNumIntervals();
00955                         double scale2 = 1.0/grid.getNumIntervals();
00956                         loop->addStatement( time_tmp.getName() + " = " + toString(scale) + "*(" + gridVariables[i].get(0,tmp_index2) + "-" + toString(scale2) + "*" + index0.getName() + ");\n" );
00957 
00958                         std::string h = toString((grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals());
00959                         evaluatePolynomial( *loop, rk_outH, time_tmp, h );
00960                         if( numXA_output(i) > 0 || numDX_output(i) > 0 ) evaluateDerivedPolynomial( *loop, rk_out, time_tmp );
00961                 }
00962 
00963                 DVector dependencyX, dependencyZ, dependencyDX;
00964                 if( exportRhs || crsFormat ) {
00965                         dependencyX = outputDependencies[i].getCols( 0,NX-1 ).sumRow();
00966                         if( numXA_output(i) > 0 ) dependencyZ = outputDependencies[i].getCols( NX,NX+NXA-1 ).sumRow();
00967                         if( numDX_output(i) > 0 ) dependencyDX = outputDependencies[i].getCols( NX+NXA+NU,NX+NXA+NU+NDX-1 ).sumRow();
00968                 }
00969                 for( j = 0; j < NX; j++ ) {
00970                         if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyX(j)) != 0 ) {
00971                                 if( STATES && j >= base ) {
00972                                         loop->addStatement( std::string(rk_rhsTemp.get( j,0 )) + " = (" + toString(j) + " == " + index1.getName() + ");\n" );
00973                                 }
00974                                 else if( j >= base ) {
00975                                         loop->addStatement( rk_rhsTemp.getRow( j ) == 0.0 );
00976                                 }
00977                                 if( j >= base ) loop->addStatement( rk_rhsTemp.getRow( j ) += rk_diffK.getRows( j,j+1 )*rk_outH );
00978                                 loop->addStatement( rk_xxx.getCol( j ) == rk_eta.getCol( j ) + rk_kkk.getRow( j )*rk_outH );
00979                         }
00980                 }
00981                 for( j = 0; j < numXA_output(i); j++ ) {
00982                         if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyZ(j)) != 0 ) {
00983                                 if( base < NX1+NX2 ) loop->addStatement( rk_rhsTemp.getRow( NX+j ) == rk_diffK.getRows( NX+j,NX+j+1 )*rk_out );
00984                                 loop->addStatement( rk_xxx.getCol( NX+j ) == rk_kkk.getRow( NX+j )*rk_out );
00985                         }
00986                 }
00987                 for( j = 0; j < numDX_output(i); j++ ) {
00988                         if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyDX(j)) != 0 ) {
00989                                 if( j >= base ) loop->addStatement( rk_rhsTemp.getRow( NX+NXA+j ) == rk_diffK.getRows( j,j+1 )*rk_out );
00990                                 loop->addStatement( rk_xxx.getCol( inputDim-diffsDim+j ) == rk_kkk.getRow( j )*rk_out );
00991                         }
00992                 }
00993 
00994                 loop->addFunctionCall( getNameDiffsOUTPUT( i ), rk_xxx, rk_diffsOutputTemp.getAddress(0,0) );
00995                 uint numOutputs = getDimOUTPUT( i );
00996                 uint outputDim = numOutputs*(NX+NU+1);
00997                 loop->addStatement( tmp_index1 == numMeas[i]*outputDim+index2*(numOutputs*(NX+NU+1)) );
00998                 loop->addStatement( tmp_index2 == tmp_index1+index1 );
00999                 for( j = 0; j < numOutputs; j++ ) {
01000                         if( exportRhs || crsFormat ) {
01001                                 dependencyX = (outputDependencies[i].getCols( 0,NX-1 )).getRow( j );
01002                                 if( NXA > 0 ) dependencyZ = (outputDependencies[i].getCols( NX,NX+NXA-1 )).getRow( j );
01003                                 if( NDX > 0 ) dependencyDX = (outputDependencies[i].getCols( NX+NXA+NU,NX+NXA+NU+NDX-1 )).getRow( j );
01004                         }
01005                         uint offset;
01006                         if( STATES ) {
01007                                 offset = numOutputs+j*NX;
01008                                 loop->addStatement( rk_outputs[i].getCol( tmp_index2+offset ) == 0.0 );
01009                         }
01010                         else {
01011                                 offset = numOutputs*(1+NX)+j*NU;
01012                                 loop->addStatement( rk_outputs[i].getCol( tmp_index2+offset ) == rk_diffsOutputTemp.getCol( index1+j*NVARS+NX+NXA ) );
01013                         }
01014 
01015                         for( k = base; k < NX; k++ ) {
01016                                 if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyX(k)) != 0 ) {
01017                                         loop->addStatement( rk_outputs[i].getCol( tmp_index2+offset ) += rk_diffsOutputTemp.getCol( j*NVARS+k )*rk_rhsTemp.getRow( k ) );
01018                                 }
01019                         }
01020                         if( base < NX1+NX2 ) {
01021                                 for( k = 0; k < numXA_output(i); k++ ) {
01022                                         if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyZ(k)) != 0 ) {
01023                                                 loop->addStatement( rk_outputs[i].getCol( tmp_index2+offset ) += rk_diffsOutputTemp.getCol( j*NVARS+NX+k )*rk_rhsTemp.getRow( NX+k ) );
01024                                         }
01025                                 }
01026                         }
01027                         for( k = base; k < numDX_output(i); k++ ) {
01028                                 if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyDX(k)) != 0 ) {
01029                                         loop->addStatement( rk_outputs[i].getCol( tmp_index2+offset ) += rk_diffsOutputTemp.getCol( j*NVARS+NX+NXA+NU+k )*rk_rhsTemp.getRow( NX+NXA+k ) );
01030                                 }
01031                         }
01032                 }
01033                 loop->addStatement( "}\n" );
01034         }
01035 
01036         return SUCCESSFUL_RETURN;
01037 }
01038 
01039 
01040 returnValue ForwardIRKExport::setup( )
01041 {
01042         ImplicitRungeKuttaExport::setup();
01043 
01044         NVARS3 = NX1+NX2+NXA3+NU+NDX3;
01045         diffsDim = (NX+NXA)*(NX+NU);
01046         inputDim = (NX+NXA)*(NX+NU+1) + NU + NOD;
01047 
01048         int useOMP;
01049         get(CG_USE_OPENMP, useOMP);
01050         ExportStruct structWspace;
01051         structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
01052 
01053         rk_diffK = ExportVariable( "rk_diffK", NX+NXA, numStages, REAL, structWspace );
01054         rk_diffsTemp2 = ExportVariable( "rk_diffsTemp2", numStages, (NX2+NXA)*(NVARS2), REAL, structWspace );
01055         rk_diffsTemp3 = ExportVariable( "rk_diffsTemp3", numStages, NX3*NVARS3, REAL, structWspace );
01056         if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
01057                 rk_diffsPrev1 = ExportVariable( "rk_diffsPrev1", NX1, NX1+NU, REAL, structWspace );
01058                 rk_diffsPrev2 = ExportVariable( "rk_diffsPrev2", NX2, NX1+NX2+NU, REAL, structWspace );
01059                 rk_diffsPrev3 = ExportVariable( "rk_diffsPrev3", NX3, NX+NU, REAL, structWspace );
01060         }
01061         rk_diffsNew1 = ExportVariable( "rk_diffsNew1", NX1, NX1+NU, REAL, structWspace );
01062         rk_diffsNew2 = ExportVariable( "rk_diffsNew2", NX2+NXA, NX1+NX2+NU, REAL, structWspace );
01063         rk_diffsNew3 = ExportVariable( "rk_diffsNew3", NX3, NX+NU, REAL, structWspace );
01064         rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
01065 
01066     return SUCCESSFUL_RETURN;
01067 }
01068 
01069 
01070 
01071 // PROTECTED:
01072 
01073 
01074 CLOSE_NAMESPACE_ACADO
01075 
01076 // end of file.


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