00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
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
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
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
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
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
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
00375 prepareInputSystem( code );
00376 solveInputSystem( loop, i, run1, j, tmp_index1, Ah );
00377
00378
00379 solveImplicitSystem( loop, i, run1, j, tmp_index1, Ah, C, determinant, true );
00380
00381
00382 prepareOutputSystem( code );
00383 solveOutputSystem( loop, i, run1, j, tmp_index1, Ah, true );
00384
00385
00386 generateOutput( loop, run, i, tmp_index2, tmp_index3, tmp_meas, time_tmp, NX+NU );
00387
00388
00389 if( NX1 > 0 ) {
00390 ExportForLoop loop4( run1,0,NX1 );
00391
00392 sensitivitiesInputSystem( &loop4, run1, i, Bh, true );
00393
00394 sensitivitiesImplicitSystem( &loop4, run1, i, j, tmp_index1, tmp_index2, Ah, Bh, determinant, true, 1 );
00395
00396 sensitivitiesOutputSystem( &loop4, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, true, 1 );
00397
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
00404 sensitivitiesImplicitSystem( &loop4, run1, i, j, tmp_index1, tmp_index2, Ah, Bh, determinant, true, 2 );
00405
00406 sensitivitiesOutputSystem( &loop4, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, true, 2 );
00407
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
00414 sensitivitiesOutputSystem( &loop4, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, true, 3 );
00415
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
00422 if( NU > 0 ) {
00423 ExportForLoop loop5( run1,0,NU );
00424
00425 sensitivitiesInputSystem( &loop5, run1, i, Bh, false );
00426
00427 sensitivitiesImplicitSystem( &loop5, run1, i, j, tmp_index1, tmp_index2, Ah, Bh, determinant, false, 0 );
00428
00429 sensitivitiesOutputSystem( &loop5, run1, i, j, k, tmp_index1, tmp_index2, Ah, Bh, false, 0 );
00430
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
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
00450 if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
00451 loop->addStatement( std::string( "if( run == 0 ) {\n" ) );
00452 }
00453
00454 updateInputSystem(loop, i, j, tmp_index2);
00455
00456 updateImplicitSystem(loop, i, j, tmp_index2);
00457
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
00464 propagateInputSystem(loop, i, j, k, tmp_index2);
00465
00466 propagateImplicitSystem(loop, i, j, k, tmp_index2);
00467
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 {
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
00492 if( !equidistantControlGrid() ) {
00493 loop->addStatement( "}\n" );
00494 }
00495 else {
00496 integrate.addStatement( *loop );
00497 }
00498
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
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
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 {
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
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
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
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
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
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
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 {
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" );
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
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
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
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
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 {
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
01072
01073
01074 CLOSE_NAMESPACE_ACADO
01075
01076