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
00036 using namespace std;
00037
00038 BEGIN_NAMESPACE_ACADO
00039
00040
00041
00042
00043
00044 ImplicitRungeKuttaExport::ImplicitRungeKuttaExport( UserInteraction* _userInteraction,
00045 const std::string& _commonHeaderName
00046 ) : RungeKuttaExport( _userInteraction,_commonHeaderName )
00047 {
00048 NX1 = 0;
00049 NX2 = 0;
00050 NDX2 = 0;
00051 NVARS2 = 0;
00052 NX3 = 0;
00053 NDX3 = 0;
00054 NXA3 = 0;
00055 NVARS3 = 0;
00056
00057 diffsDim = 0;
00058 inputDim = 0;
00059 numIts = 3;
00060 numItsInit = 0;
00061 REUSE = true;
00062 CONTINUOUS_OUTPUT = false;
00063
00064 solver = 0;
00065 }
00066
00067 ImplicitRungeKuttaExport::ImplicitRungeKuttaExport( const ImplicitRungeKuttaExport& arg ) : RungeKuttaExport( arg )
00068 {
00069 NX1 = arg.NX1;
00070 NX2 = arg.NX2;
00071 NDX2 = arg.NDX2;
00072 NVARS2 = arg.NVARS2;
00073 NX3 = arg.NX3;
00074 NDX3 = arg.NDX3;
00075 NXA3 = arg.NXA3;
00076 NVARS3 = arg.NVARS3;
00077
00078 diffsDim = 0;
00079 inputDim = 0;
00080 numIts = arg.numIts ;
00081 numItsInit = arg.numItsInit ;
00082 diffs_outputs = arg.diffs_outputs;
00083 outputs = arg.outputs;
00084 outputGrids = arg.outputGrids;
00085 solver = arg.solver;
00086 REUSE = arg.REUSE;;
00087 CONTINUOUS_OUTPUT = arg.CONTINUOUS_OUTPUT;
00088 }
00089
00090
00091 ImplicitRungeKuttaExport::~ImplicitRungeKuttaExport( )
00092 {
00093 if ( solver )
00094 delete solver;
00095 solver = 0;
00096
00097 clear( );
00098 }
00099
00100
00101 ImplicitRungeKuttaExport& ImplicitRungeKuttaExport::operator=( const ImplicitRungeKuttaExport& arg ){
00102
00103 if( this != &arg ){
00104
00105 RungeKuttaExport::operator=( arg );
00106 copy( arg );
00107 }
00108 return *this;
00109 }
00110
00111
00112 returnValue ImplicitRungeKuttaExport::setDifferentialEquation( const Expression& rhs_ )
00113 {
00114 if( rhs_.getDim() > 0 ) {
00115 OnlineData dummy0;
00116 Control dummy1;
00117 DifferentialState dummy2;
00118 AlgebraicState dummy3;
00119 DifferentialStateDerivative dummy4;
00120 dummy0.clearStaticCounters();
00121 dummy1.clearStaticCounters();
00122 dummy2.clearStaticCounters();
00123 dummy3.clearStaticCounters();
00124 dummy4.clearStaticCounters();
00125
00126 NX2 = rhs_.getDim() - NXA;
00127 x = DifferentialState("", NX1+NX2, 1);
00128 z = AlgebraicState("", NXA, 1);
00129 u = Control("", NU, 1);
00130 od = OnlineData("", NOD, 1);
00131
00132 DifferentialEquation f;
00133 f << rhs_;
00134
00135 NDX2 = f.getNDX();
00136 if( NDX2 > 0 && (NDX2 < NX2 || NDX2 > (NX1+NX2)) ) {
00137 return ACADOERROR( RET_INVALID_OPTION );
00138 }
00139 else if( NDX2 > 0 ) NDX2 = NX1+NX2;
00140 dx = DifferentialStateDerivative("", NDX2, 1);
00141
00142 DifferentialEquation g;
00143 for( uint i = 0; i < rhs_.getDim(); i++ ) {
00144 g << forwardDerivative( rhs_(i), x );
00145 g << forwardDerivative( rhs_(i), z );
00146 g << forwardDerivative( rhs_(i), u );
00147 g << forwardDerivative( rhs_(i), dx );
00148 }
00149
00150 if( f.getNT() > 0 ) timeDependant = true;
00151
00152 return (rhs.init( f,"acado_rhs",NX,NXA,NU,NP,NDX,NOD ) &
00153 diffs_rhs.init( g,"acado_diffs",NX,NXA,NU,NP,NDX,NOD ) );
00154 }
00155 return SUCCESSFUL_RETURN;
00156 }
00157
00158
00159 returnValue ImplicitRungeKuttaExport::setModel( const std::string& _rhs, const std::string& _diffs_rhs ) {
00160
00161 IntegratorExport::setModel( _rhs, _diffs_rhs );
00162
00163 NDX2 = NDX;
00164
00165 setup();
00166
00167 return SUCCESSFUL_RETURN;
00168 }
00169
00170
00171 ExportVariable ImplicitRungeKuttaExport::getAuxVariable() const
00172 {
00173 ExportVariable max;
00174 if( NX1 > 0 ) {
00175 max = lin_input.getGlobalExportVariable();
00176 }
00177 if( NX2 > 0 || NXA > 0 ) {
00178 if( rhs.getGlobalExportVariable().getDim() >= max.getDim() ) {
00179 max = rhs.getGlobalExportVariable();
00180 }
00181 if( diffs_rhs.getGlobalExportVariable().getDim() >= max.getDim() ) {
00182 max = diffs_rhs.getGlobalExportVariable();
00183 }
00184 }
00185 if( NX3 > 0 ) {
00186 if( rhs3.getGlobalExportVariable().getDim() >= max.getDim() ) {
00187 max = rhs3.getGlobalExportVariable();
00188 }
00189 }
00190 uint i;
00191 for( i = 0; i < outputs.size(); i++ ) {
00192 if( outputs[i].getGlobalExportVariable().getDim() >= max.getDim() ) {
00193 max = outputs[i].getGlobalExportVariable();
00194 }
00195 }
00196 return max;
00197 }
00198
00199
00200 returnValue ImplicitRungeKuttaExport::getDataDeclarations( ExportStatementBlock& declarations,
00201 ExportStruct dataStruct
00202 ) const
00203 {
00204 if( NX2 > 0 || NXA > 0 ) solver->getDataDeclarations( declarations,dataStruct );
00205
00206 if( NX1 > 0 || exportRhs ) {
00207 ExportVariable max = getAuxVariable();
00208 declarations.addDeclaration( max,dataStruct );
00209 }
00210
00211 int debugMode;
00212 get( INTEGRATOR_DEBUG_MODE, debugMode );
00213 if ( (bool)debugMode == true ) {
00214 declarations.addDeclaration( debug_mat,dataStruct );
00215 }
00216 declarations.addDeclaration( rk_ttt,dataStruct );
00217 declarations.addDeclaration( rk_xxx,dataStruct );
00218 declarations.addDeclaration( rk_kkk,dataStruct );
00219
00220 declarations.addDeclaration( rk_A,dataStruct );
00221 declarations.addDeclaration( rk_b,dataStruct );
00222 declarations.addDeclaration( rk_auxSolver,dataStruct );
00223 declarations.addDeclaration( rk_rhsTemp,dataStruct );
00224 declarations.addDeclaration( rk_diffsTemp2,dataStruct );
00225
00226 if( CONTINUOUS_OUTPUT ) {
00227 declarations.addDeclaration( rk_rhsOutputTemp,dataStruct );
00228 declarations.addDeclaration( rk_outH,dataStruct );
00229 declarations.addDeclaration( rk_out,dataStruct );
00230
00231 int measGrid;
00232 get( MEASUREMENT_GRID, measGrid );
00233 if( (MeasurementGrid)measGrid == ONLINE_GRID ) {
00234 for( uint i = 0; i < gridVariables.size(); i++ ) {
00235 declarations.addDeclaration( gridVariables[i],dataStruct );
00236 }
00237 }
00238 }
00239
00240 return SUCCESSFUL_RETURN;
00241 }
00242
00243
00244 returnValue ImplicitRungeKuttaExport::getFunctionDeclarations( ExportStatementBlock& declarations
00245 ) const
00246 {
00247 declarations.addDeclaration( integrate );
00248
00249 if( NX2 != NX ) declarations.addDeclaration( fullRhs );
00250 else declarations.addDeclaration( rhs );
00251
00252 return SUCCESSFUL_RETURN;
00253 }
00254
00255
00256 returnValue ImplicitRungeKuttaExport::getCode( ExportStatementBlock& code )
00257 {
00258 int sensGen;
00259 get( DYNAMIC_SENSITIVITY, sensGen );
00260 if ( (ExportSensitivityType)sensGen != NO_SENSITIVITY ) ACADOERROR( RET_INVALID_OPTION );
00261
00262 int useOMP;
00263 get(CG_USE_OPENMP, useOMP);
00264 if ( useOMP ) {
00265 ExportVariable max = getAuxVariable();
00266 max.setName( "auxVar" );
00267 max.setDataStruct( ACADO_LOCAL );
00268 if( NX2 > 0 || NXA > 0 ) {
00269 rhs.setGlobalExportVariable( max );
00270 diffs_rhs.setGlobalExportVariable( max );
00271 }
00272 if( NX3 > 0 ) {
00273 rhs3.setGlobalExportVariable( max );
00274 }
00275 for( uint i = 0; i < outputs.size(); i++ ) {
00276 outputs[i].setGlobalExportVariable( max );
00277 }
00278
00279 getDataDeclarations( code, ACADO_LOCAL );
00280
00281 stringstream s;
00282 s << "#pragma omp threadprivate( "
00283 << max.getFullName() << ", "
00284 << rk_ttt.getFullName() << ", "
00285 << rk_xxx.getFullName() << ", "
00286 << rk_kkk.getFullName() << ", "
00287 << rk_rhsTemp.getFullName() << ", "
00288 << rk_auxSolver.getFullName();
00289 if( NX2 > 0 || NXA > 0 ) {
00290 s << ", " << rk_A.getFullName();
00291 s << ", " << rk_b.getFullName();
00292 s << ", " << rk_diffsTemp2.getFullName();
00293 solver->appendVariableNames( s );
00294 }
00295 s << " )" << endl << endl;
00296 code.addStatement( s.str().c_str() );
00297 }
00298
00299 if( NX1 > 0 ) {
00300 code.addFunction( lin_input );
00301 code.addStatement( "\n\n" );
00302 }
00303 if( exportRhs ) {
00304 if( NX2 > 0 || NXA > 0 ) {
00305 code.addFunction( rhs );
00306 code.addStatement( "\n\n" );
00307 code.addFunction( diffs_rhs );
00308 code.addStatement( "\n\n" );
00309 }
00310
00311 if( NX3 > 0 ) {
00312 code.addFunction( rhs3 );
00313 code.addStatement( "\n\n" );
00314 }
00315
00316 if( CONTINUOUS_OUTPUT ) {
00317 uint i;
00318 for( i = 0; i < outputs.size(); i++ ) {
00319 code.addFunction( outputs[i] );
00320 code.addStatement( "\n\n" );
00321 }
00322 }
00323 }
00324 if( NX2 > 0 || NXA > 0 ) solver->getCode( code );
00325 code.addLinebreak(2);
00326
00327 int measGrid;
00328 get( MEASUREMENT_GRID, measGrid );
00329
00330
00331 uint run5;
00332 std::string tempString;
00333
00334 initializeDDMatrix();
00335 initializeCoefficients();
00336
00337 double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
00338 DMatrix tmp = AA;
00339 ExportVariable Ah( "Ah_mat", tmp*=h, STATIC_CONST_REAL );
00340 code.addDeclaration( Ah );
00341 code.addLinebreak( 2 );
00342
00343 Ah = ExportVariable( "Ah_mat", numStages, numStages, STATIC_CONST_REAL, ACADO_LOCAL );
00344
00345 DVector BB( bb );
00346 ExportVariable Bh( "Bh_mat", DMatrix( BB*=h ) );
00347
00348 DVector CC( cc );
00349 ExportVariable C;
00350 if( timeDependant ) {
00351 C = ExportVariable( "C_mat", DMatrix( CC*=(1.0/grid.getNumIntervals()) ), STATIC_CONST_REAL );
00352 code.addDeclaration( C );
00353 code.addLinebreak( 2 );
00354 C = ExportVariable( "C_mat", 1, numStages, STATIC_CONST_REAL, ACADO_LOCAL );
00355 }
00356
00357 code.addComment(std::string("Fixed step size:") + toString(h));
00358
00359 ExportVariable determinant( "det", 1, 1, REAL, ACADO_LOCAL, true );
00360 integrate.addDeclaration( determinant );
00361
00362 ExportIndex i( "i" );
00363 ExportIndex j( "j" );
00364 ExportIndex k( "k" );
00365 ExportIndex run( "run" );
00366 ExportIndex run1( "run1" );
00367 ExportIndex tmp_index1("tmp_index1");
00368 ExportIndex tmp_index2("tmp_index2");
00369 ExportIndex tmp_index3("tmp_index3");
00370 ExportIndex tmp_index4("tmp_index4");
00371 ExportVariable tmp_meas("tmp_meas", 1, outputGrids.size(), INT, ACADO_LOCAL);
00372
00373 ExportVariable numInt( "numInts", 1, 1, INT );
00374 if( !equidistantControlGrid() ) {
00375 ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
00376 code.addDeclaration( numStepsV );
00377 code.addLinebreak( 2 );
00378 integrate.addStatement( std::string( "int " ) + numInt.getName() + " = " + numStepsV.getName() + "[" + rk_index.getName() + "];\n" );
00379 }
00380
00381 prepareOutputEvaluation( code );
00382
00383 integrate.addIndex( i );
00384 integrate.addIndex( j );
00385 integrate.addIndex( k );
00386 integrate.addIndex( run );
00387 integrate.addIndex( run1 );
00388 integrate.addIndex( tmp_index1 );
00389 integrate.addIndex( tmp_index2 );
00390 if( rk_outputs.size() > 0 ) integrate.addIndex( tmp_index3 );
00391 if( rk_outputs.size() > 0 && (grid.getNumIntervals() > 1 || !equidistantControlGrid()) ) {
00392 integrate.addIndex( tmp_index4 );
00393 }
00394 ExportVariable time_tmp( "time_tmp", 1, 1, REAL, ACADO_LOCAL, true );
00395 if( CONTINUOUS_OUTPUT ) {
00396 for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
00397 ExportIndex numMeasTmp( (std::string)"numMeasTmp" + toString(run5) );
00398 numMeas.push_back( numMeasTmp );
00399 integrate.addIndex( numMeas[run5] );
00400 }
00401
00402 if( (MeasurementGrid)measGrid == ONLINE_GRID ) {
00403 integrate.addDeclaration( tmp_meas );
00404 integrate.addDeclaration( polynEvalVar );
00405 integrate.addDeclaration( time_tmp );
00406 }
00407
00408 for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
00409 integrate.addStatement( numMeas[run5] == 0 );
00410 }
00411 }
00412 integrate.addStatement( rk_ttt == DMatrix(grid.getFirstTime()) );
00413 if( inputDim > NX+NXA ) {
00414 integrate.addStatement( rk_xxx.getCols( NX+NXA,inputDim ) == rk_eta.getCols( NX+NXA,inputDim ) );
00415 }
00416 integrate.addLinebreak( );
00417
00418
00419 ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
00420 ExportStatementBlock *loop;
00421 if( equidistantControlGrid() ) {
00422 loop = &tmpLoop;
00423 }
00424 else {
00425 loop = &integrate;
00426 loop->addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
00427 }
00428
00429 if( CONTINUOUS_OUTPUT && (MeasurementGrid)measGrid == ONLINE_GRID ) {
00430 for( run5 = 0; run5 < outputGrids.size(); run5++ ) {
00431 loop->addStatement( tmp_index1 == numMeas[run5] );
00432 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" );
00433 loop->addStatement( tmp_index1 == tmp_index1+1 );
00434 loop->addStatement( std::string("}\n") );
00435 loop->addStatement( std::string(tmp_meas.get( 0,run5 )) + " = " + tmp_index1.getName() + " - " + numMeas[run5].getName() + ";\n" );
00436 }
00437 }
00438
00439
00440 prepareInputSystem( code );
00441 solveInputSystem( loop, i, run1, j, tmp_index1, Ah );
00442
00443
00444 solveImplicitSystem( loop, i, run1, j, tmp_index1, Ah, C, determinant );
00445
00446
00447 prepareOutputSystem( code );
00448 solveOutputSystem( loop, i, run1, j, tmp_index1, Ah );
00449
00450
00451 generateOutput( loop, run, i, tmp_index2, tmp_index3, tmp_meas, time_tmp, 0 );
00452
00453
00454 for( run5 = 0; run5 < NX; run5++ ) {
00455 loop->addStatement( rk_eta.getCol( run5 ) += rk_kkk.getRow( run5 )*Bh );
00456 }
00457 if( NXA > 0) {
00458 DMatrix tempCoefs( evaluateDerivedPolynomial( 0.0 ) );
00459 loop->addStatement( std::string("if( run == 0 ) {\n") );
00460 for( run5 = 0; run5 < NXA; run5++ ) {
00461 loop->addStatement( rk_eta.getCol( NX+run5 ) == rk_kkk.getRow( NX+run5 )*tempCoefs );
00462 }
00463 loop->addStatement( std::string("}\n") );
00464 }
00465
00466 loop->addStatement( std::string( reset_int.get(0,0) ) + " = 0;\n" );
00467
00468 for( run5 = 0; run5 < rk_outputs.size(); run5++ ) {
00469 if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
00470 loop->addStatement( numMeas[run5].getName() + " += " + numMeasVariables[run5].get(0,run) + ";\n" );
00471 }
00472 else {
00473 loop->addStatement( numMeas[run5].getName() + " += " + tmp_meas.get(0,run5) + ";\n" );
00474 }
00475 }
00476 loop->addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
00477
00478
00479 if( !equidistantControlGrid() ) {
00480 loop->addStatement( "}\n" );
00481 }
00482 else {
00483 integrate.addStatement( *loop );
00484 }
00485
00486 integrate.addStatement( std::string( "if( " ) + determinant.getFullName() + " < 1e-12 ) {\n" );
00487 integrate.addStatement( error_code == 2 );
00488 integrate.addStatement( std::string( "} else if( " ) + determinant.getFullName() + " < 1e-6 ) {\n" );
00489 integrate.addStatement( error_code == 1 );
00490 integrate.addStatement( std::string( "} else {\n" ) );
00491 integrate.addStatement( error_code == 0 );
00492 integrate.addStatement( std::string( "}\n" ) );
00493
00494 code.addFunction( integrate );
00495 code.addLinebreak( 2 );
00496
00497 return SUCCESSFUL_RETURN;
00498 }
00499
00500
00501 returnValue ImplicitRungeKuttaExport::prepareInputSystem( ExportStatementBlock& code )
00502 {
00503 if( NX1 > 0 ) {
00504 DMatrix mat1 = formMatrix( M11, A11 );
00505 rk_mat1 = ExportVariable( "rk_mat1", mat1, STATIC_CONST_REAL );
00506 code.addDeclaration( rk_mat1 );
00507
00508 rk_mat1 = ExportVariable( "rk_mat1", numStages*NX1, numStages*NX1, STATIC_CONST_REAL, ACADO_LOCAL );
00509 }
00510
00511 return SUCCESSFUL_RETURN;
00512 }
00513
00514
00515 returnValue ImplicitRungeKuttaExport::prepareOutputSystem( ExportStatementBlock& code )
00516 {
00517 if( NX3 > 0 ) {
00518 DMatrix mat3 = formMatrix( M33, A33 );
00519 rk_mat3 = ExportVariable( "rk_mat3", mat3, STATIC_CONST_REAL );
00520 code.addDeclaration( rk_mat3 );
00521
00522 rk_mat3 = ExportVariable( "rk_mat3", numStages*NX3, numStages*NX3, STATIC_CONST_REAL, ACADO_LOCAL );
00523 }
00524
00525 return SUCCESSFUL_RETURN;
00526 }
00527
00528
00529 DMatrix ImplicitRungeKuttaExport::formMatrix( const DMatrix& mass, const DMatrix& jacobian ) {
00530 if( jacobian.getNumRows() != jacobian.getNumCols() ) {
00531 return RET_UNABLE_TO_EXPORT_CODE;
00532 }
00533 double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
00534 uint vars = jacobian.getNumRows();
00535 uint i1, j1, i2, j2;
00536 DMatrix result = zeros<double>(numStages*vars, numStages*vars);
00537 for( i1 = 0; i1 < numStages; i1++ ){
00538 for( j1 = 0; j1 < numStages; j1++ ) {
00539 for( i2 = 0; i2 < vars; i2++ ){
00540 for( j2 = 0; j2 < vars; j2++ ) {
00541 if( i1 == j1 ) {
00542 result(i1*vars+i2, j1*vars+j2) = mass(i2,j2) - AA(i1,j1)*h*jacobian(i2,j2);
00543 }
00544 else {
00545 result(i1*vars+i2, j1*vars+j2) = -AA(i1,j1)*h*jacobian(i2,j2);
00546 }
00547 }
00548 }
00549 }
00550 }
00551
00552 return result.inverse();
00553 }
00554
00555
00556 returnValue ImplicitRungeKuttaExport::solveInputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index, const ExportVariable& Ah )
00557 {
00558 if( NX1 > 0 ) {
00559 block->addStatement( rk_xxx.getCols(0,NX1) == rk_eta.getCols(0,NX1) );
00560 block->addFunctionCall( lin_input.getName(), rk_xxx, rk_rhsTemp.getAddress(0,0) );
00561 ExportForLoop loop( index1,0,numStages );
00562 ExportForLoop loop1( index2,0,NX1 );
00563 loop1.addStatement( tmp_index == index1*NX1+index2 );
00564 loop1.addStatement( rk_b.getRow(tmp_index) == rk_rhsTemp.getRow(index2) );
00565 loop.addStatement(loop1);
00566 block->addStatement(loop);
00567
00568 ExportForLoop loop4( index1,0,numStages );
00569 ExportForLoop loop5( index2,0,NX1 );
00570 loop5.addStatement( tmp_index == index1*NX1+index2 );
00571 loop5.addStatement( rk_kkk.getElement(index2,index1) == rk_mat1.getElement(tmp_index,0)*rk_b.getRow(0) );
00572 ExportForLoop loop6( index3,1,numStages*NX1 );
00573 loop6.addStatement( rk_kkk.getElement(index2,index1) += rk_mat1.getElement(tmp_index,index3)*rk_b.getRow(index3) );
00574 loop5.addStatement(loop6);
00575 loop4.addStatement(loop5);
00576 block->addStatement(loop4);
00577 }
00578
00579 return SUCCESSFUL_RETURN;
00580 }
00581
00582
00583 returnValue ImplicitRungeKuttaExport::solveImplicitSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index, const ExportVariable& Ah, const ExportVariable& C, const ExportVariable& det, bool DERIVATIVES )
00584 {
00585 if( NX2 > 0 || NXA > 0 ) {
00586
00587 if( DERIVATIVES && REUSE ) block->addStatement( std::string( "if( " ) + reset_int.getFullName() + " ) {\n" );
00588
00589 ExportForLoop loop1( index1,0,numItsInit+1 );
00590 ExportForLoop loop11( index2,0,numStages );
00591 evaluateMatrix( &loop11, index2, index3, tmp_index, Ah, C, true, DERIVATIVES );
00592 loop1.addStatement( loop11 );
00593 loop1.addStatement( det.getFullName() + " = " + solver->getNameSolveFunction() + "( " + rk_A.getFullName() + ", " + rk_b.getFullName() + ", " + rk_auxSolver.getFullName() + " );\n" );
00594 ExportForLoop loopTemp( index3,0,numStages );
00595 loopTemp.addStatement( rk_kkk.getSubMatrix( NX1,NX1+NX2,index3,index3+1 ) += rk_b.getRows( index3*NX2,index3*NX2+NX2 ) );
00596 if(NXA > 0) loopTemp.addStatement( rk_kkk.getSubMatrix( NX,NX+NXA,index3,index3+1 ) += rk_b.getRows( index3*NXA+numStages*NX2,index3*NXA+numStages*NX2+NXA ) );
00597 loop1.addStatement( loopTemp );
00598 block->addStatement( loop1 );
00599 if( DERIVATIVES && REUSE ) block->addStatement( std::string( "}\n" ) );
00600
00601
00602 ExportForLoop loop2( index1,0,numIts );
00603 ExportForLoop loop21( index2,0,numStages );
00604 evaluateStatesImplicitSystem( &loop21, Ah, C, index2, index3, tmp_index );
00605 evaluateRhsImplicitSystem( &loop21, index2 );
00606 loop2.addStatement( loop21 );
00607 loop2.addFunctionCall( solver->getNameSolveReuseFunction(),rk_A.getAddress(0,0),rk_b.getAddress(0,0),rk_auxSolver.getAddress(0,0) );
00608 loopTemp = ExportForLoop( index3,0,numStages );
00609 loopTemp.addStatement( rk_kkk.getSubMatrix( NX1,NX1+NX2,index3,index3+1 ) += rk_b.getRows( index3*NX2,index3*NX2+NX2 ) );
00610 if(NXA > 0) loopTemp.addStatement( rk_kkk.getSubMatrix( NX,NX+NXA,index3,index3+1 ) += rk_b.getRows( index3*NXA+numStages*NX2,index3*NXA+numStages*NX2+NXA ) );
00611 loop2.addStatement( loopTemp );
00612 block->addStatement( loop2 );
00613
00614 if( DERIVATIVES ) {
00615
00616 ExportForLoop loop3( index2,0,numStages );
00617 evaluateMatrix( &loop3, index2, index3, tmp_index, Ah, C, false, DERIVATIVES );
00618 block->addStatement( loop3 );
00619 }
00620
00621
00622 int debugMode;
00623 get( INTEGRATOR_DEBUG_MODE, debugMode );
00624 if ( (bool)debugMode == true ) {
00625 block->addStatement( debug_mat == rk_A );
00626 }
00627 }
00628
00629 return SUCCESSFUL_RETURN;
00630 }
00631
00632
00633 returnValue ImplicitRungeKuttaExport::solveOutputSystem( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& index3, const ExportIndex& tmp_index, const ExportVariable& Ah, bool DERIVATIVES )
00634 {
00635 if( NX3 > 0 ) {
00636 ExportForLoop loop( index1,0,numStages );
00637 evaluateStatesOutputSystem( &loop, Ah, index1 );
00638 loop.addFunctionCall( getNameOutputRHS(), rk_xxx, rk_b.getAddress(index1*NX3,0) );
00639 if( DERIVATIVES ) loop.addFunctionCall( getNameOutputDiffs(), rk_xxx, rk_diffsTemp3.getAddress(index1,0) );
00640 block->addStatement( loop );
00641
00642 ExportForLoop loop4( index1,0,numStages );
00643 ExportForLoop loop5( index2,0,NX3 );
00644 loop5.addStatement( tmp_index == index1*NX3+index2 );
00645 loop5.addStatement( rk_kkk.getElement(NX1+NX2+index2,index1) == rk_mat3.getElement(tmp_index,0)*rk_b.getRow(0) );
00646 ExportForLoop loop6( index3,1,numStages*NX3 );
00647 loop6.addStatement( rk_kkk.getElement(NX1+NX2+index2,index1) += rk_mat3.getElement(tmp_index,index3)*rk_b.getRow(index3) );
00648 loop5.addStatement(loop6);
00649 loop4.addStatement(loop5);
00650 block->addStatement(loop4);
00651 }
00652
00653 return SUCCESSFUL_RETURN;
00654 }
00655
00656
00657 returnValue ImplicitRungeKuttaExport::evaluateStatesImplicitSystem( ExportStatementBlock* block, const ExportVariable& Ah, const ExportVariable& C, const ExportIndex& stage, const ExportIndex& i, const ExportIndex& j )
00658 {
00659 ExportForLoop loop1( i, 0, NX1+NX2 );
00660 loop1.addStatement( rk_xxx.getCol( i ) == rk_eta.getCol( i ) );
00661 ExportForLoop loop2( j, 0, numStages );
00662 loop2.addStatement( rk_xxx.getCol( i ) += Ah.getElement(stage,j)*rk_kkk.getElement( i,j ) );
00663 loop1.addStatement( loop2 );
00664 block->addStatement( loop1 );
00665
00666 ExportForLoop loop3( i, 0, NXA );
00667 loop3.addStatement( rk_xxx.getCol( NX+i ) == rk_kkk.getElement( NX+i,stage ) );
00668 block->addStatement( loop3 );
00669
00670 ExportForLoop loop4( i, 0, NDX2 );
00671 loop4.addStatement( rk_xxx.getCol( inputDim-diffsDim+i ) == rk_kkk.getElement( i,stage ) );
00672 block->addStatement( loop4 );
00673
00674 if( C.getDim() > 0 ) {
00675 block->addStatement( rk_xxx.getCol( inputDim-diffsDim+NDX ) == rk_ttt + C.getCol(stage) );
00676 }
00677
00678 return SUCCESSFUL_RETURN;
00679 }
00680
00681
00682 returnValue ImplicitRungeKuttaExport::evaluateStatesOutputSystem( ExportStatementBlock* block, const ExportVariable& Ah, const ExportIndex& stage )
00683 {
00684 uint i,j;
00685 for( i = 0; i < NX1+NX2; i++ ) {
00686 block->addStatement( rk_xxx.getCol( i ) == rk_eta.getCol( i ) );
00687 for( j = 0; j < numStages; j++ ) {
00688 block->addStatement( rk_xxx.getCol( i ) += Ah.getElement(stage,j)*rk_kkk.getElement( i,j ) );
00689 }
00690 }
00691 for( i = 0; i < NX3; i++ ) {
00692 block->addStatement( rk_xxx.getCol( NX1+NX2+i ) == rk_eta.getCol( NX1+NX2+i ) );
00693 }
00694 for( i = 0; i < NXA3; i++ ) {
00695 block->addStatement( rk_xxx.getCol( NX+i ) == rk_kkk.getElement( NX+i,stage ) );
00696 }
00697 for( i = 0; i < NDX3; i++ ) {
00698 block->addStatement( rk_xxx.getCol( inputDim-diffsDim+i ) == rk_kkk.getElement( i,stage ) );
00699 }
00700
00701 return SUCCESSFUL_RETURN;
00702 }
00703
00704
00705 returnValue ImplicitRungeKuttaExport::evaluateRhsImplicitSystem( ExportStatementBlock* block, const ExportIndex& stage )
00706 {
00707 DMatrix zeroM = zeros<double>( NX2+NXA,1 );
00708 block->addFunctionCall( getNameRHS(), rk_xxx, rk_rhsTemp.getAddress(0,0) );
00709
00710 if( NDX2 == 0 ) {
00711 block->addStatement( rk_b.getRows( stage*(NX2+NXA),stage*(NX2+NXA)+NX2 ) == rk_kkk.getSubMatrix( NX1,NX1+NX2,stage,stage+1 ) - rk_rhsTemp.getRows( 0,NX2 ) );
00712 }
00713 else {
00714 block->addStatement( rk_b.getRows( stage*(NX2+NXA),stage*(NX2+NXA)+NX2 ) == zeroM.getRows( 0,NX2-1 ) - rk_rhsTemp.getRows( 0,NX2 ) );
00715 }
00716 if( NXA > 0 ) {
00717 block->addStatement( rk_b.getRows( stage*(NX2+NXA)+NX2,(stage+1)*(NX2+NXA) ) == zeroM.getRows( 0,NXA-1 ) - rk_rhsTemp.getRows( NX2,NX2+NXA ) );
00718 }
00719
00720 return SUCCESSFUL_RETURN;
00721 }
00722
00723
00724 returnValue ImplicitRungeKuttaExport::evaluateMatrix( ExportStatementBlock* block, const ExportIndex& index1, const ExportIndex& index2, const ExportIndex& tmp_index, const ExportVariable& Ah, const ExportVariable& C, bool evaluateB, bool DERIVATIVES )
00725 {
00726 uint i;
00727
00728 evaluateStatesImplicitSystem( block, Ah, C, index1, index2, tmp_index );
00729
00730 ExportIndex indexDiffs(index1);
00731 if( !DERIVATIVES ) indexDiffs = ExportIndex(0);
00732
00733 block->addFunctionCall( getNameDiffsRHS(), rk_xxx, rk_diffsTemp2.getAddress(indexDiffs,0) );
00734 ExportForLoop loop2( index2,0,NX2+NXA );
00735 loop2.addStatement( tmp_index == index1*(NX2+NXA)+index2 );
00736 for( i = 0; i < numStages; i++ ) {
00737 if( NDX2 == 0 ) {
00738 loop2.addStatement( rk_A.getSubMatrix( tmp_index,tmp_index+1,i*NX2,i*NX2+NX2 ) == Ah.getElement( index1,i )*rk_diffsTemp2.getSubMatrix( indexDiffs,indexDiffs+1,index2*(NVARS2)+NX1,index2*(NVARS2)+NX1+NX2 ) );
00739 loop2.addStatement( std::string( "if( " ) + toString(i) + " == " + index1.getName() + " ) " );
00740 loop2.addStatement( rk_A.getElement( tmp_index,index2+i*NX2 ) -= 1 );
00741 }
00742 else {
00743 loop2.addStatement( rk_A.getSubMatrix( tmp_index,tmp_index+1,i*NX2,i*NX2+NX2 ) == Ah.getElement( index1,i )*rk_diffsTemp2.getSubMatrix( indexDiffs,indexDiffs+1,index2*(NVARS2)+NX1,index2*(NVARS2)+NX1+NX2 ) );
00744 loop2.addStatement( std::string( "if( " ) + toString(i) + " == " + index1.getName() + " ) {\n" );
00745 loop2.addStatement( rk_A.getSubMatrix( tmp_index,tmp_index+1,i*NX2,i*NX2+NX2 ) += rk_diffsTemp2.getSubMatrix( indexDiffs,indexDiffs+1,index2*(NVARS2)+NVARS2-NX2,index2*(NVARS2)+NVARS2 ) );
00746 loop2.addStatement( std::string( "}\n" ) );
00747 }
00748 }
00749 if( NXA > 0 ) {
00750 DMatrix zeroM = zeros<double>( 1,NXA );
00751 for( i = 0; i < numStages; i++ ) {
00752 loop2.addStatement( std::string( "if( " ) + toString(i) + " == " + index1.getName() + " ) {\n" );
00753 loop2.addStatement( rk_A.getSubMatrix( tmp_index,tmp_index+1,numStages*NX2+i*NXA,numStages*NX2+i*NXA+NXA ) == rk_diffsTemp2.getSubMatrix( indexDiffs,indexDiffs+1,index2*(NVARS2)+NX1+NX2,index2*(NVARS2)+NX1+NX2+NXA ) );
00754 loop2.addStatement( std::string( "}\n else {\n" ) );
00755 loop2.addStatement( rk_A.getSubMatrix( tmp_index,tmp_index+1,numStages*NX2+i*NXA,numStages*NX2+i*NXA+NXA ) == zeroM );
00756 loop2.addStatement( std::string( "}\n" ) );
00757 }
00758 }
00759 block->addStatement( loop2 );
00760 if( evaluateB ) {
00761 evaluateRhsImplicitSystem( block, index1 );
00762 }
00763
00764 return SUCCESSFUL_RETURN;
00765 }
00766
00767
00768 returnValue ImplicitRungeKuttaExport::generateOutput( ExportStatementBlock* block, const ExportIndex& index0,
00769 const ExportIndex& index1, const ExportIndex& tmp_index1, const ExportIndex& tmp_index2,
00770 const ExportVariable& tmp_meas, const ExportVariable& time_tmp, const uint directions )
00771 {
00772 uint i, j;
00773 int measGrid;
00774 get( MEASUREMENT_GRID, measGrid );
00775
00776 for( i = 0; i < rk_outputs.size(); i++ ) {
00777 ExportStatementBlock *loop1;
00778 if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
00779 loop1 = block;
00780 loop1->addStatement( std::string("for(") + index1.getName() + " = 0; " + index1.getName() + " < (int)" + numMeasVariables[i].get(0,index0) + "; " + index1.getName() + "++) {\n" );
00781 loop1->addStatement( tmp_index1 == numMeas[i]+index1 );
00782 for( j = 0; j < numStages; j++ ) {
00783 loop1->addStatement( rk_outH.getRow(j) == polynVariables[i].getElement( tmp_index1,j ) );
00784 }
00785 if( numXA_output(i) > 0 || numDX_output(i) > 0 ) {
00786 for( j = 0; j < numStages; j++ ) {
00787 loop1->addStatement( rk_out.getRow(j) == polynDerVariables[i].getElement( tmp_index1,j ) );
00788 }
00789 }
00790 }
00791 else {
00792 loop1 = block;
00793 loop1->addStatement( std::string(tmp_index2.getName()) + " = " + tmp_meas.get( 0,i ) + ";\n" );
00794 loop1->addStatement( std::string("for(") + index1.getName() + " = 0; " + index1.getName() + " < (int)" + tmp_index2.getName() + "; " + index1.getName() + "++) {\n" );
00795 loop1->addStatement( tmp_index1 == numMeas[i]+index1 );
00796
00797 uint scale = grid.getNumIntervals();
00798 double scale2 = 1.0/grid.getNumIntervals();
00799 loop1->addStatement( time_tmp.getName() + " = " + toString(scale) + "*(" + gridVariables[i].get(0,tmp_index1) + "-" + toString(scale2) + "*" + index0.getName() + ");\n" );
00800
00801 std::string h = toString((grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals());
00802 evaluatePolynomial( *loop1, rk_outH, time_tmp, h );
00803 if( numXA_output(i) > 0 || numDX_output(i) > 0 ) evaluateDerivedPolynomial( *loop1, rk_out, time_tmp );
00804 }
00805
00806 DVector dependencyX, dependencyZ, dependencyDX;
00807 if( exportRhs || crsFormat ) {
00808 dependencyX = outputDependencies[i].getCols( 0,NX-1 ).sumRow();
00809 if( numXA_output(i) > 0 ) dependencyZ = outputDependencies[i].getCols( NX,NX+NXA-1 ).sumRow();
00810 if( numDX_output(i) > 0 ) dependencyDX = outputDependencies[i].getCols( NX+NXA+NU,NX+NXA+NU+NDX-1 ).sumRow();
00811 }
00812 for( j = 0; j < NX; j++ ) {
00813 if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyX(j)) != 0 ) {
00814 loop1->addStatement( rk_xxx.getCol( j ) == rk_eta.getCol( j ) + rk_kkk.getRow( j )*rk_outH );
00815 }
00816 }
00817 for( j = 0; j < numXA_output(i); j++ ) {
00818 if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyZ(j)) != 0 ) {
00819 loop1->addStatement( rk_xxx.getCol( NX+j ) == rk_kkk.getRow( NX+j )*rk_out );
00820 }
00821 }
00822 for( j = 0; j < numDX_output(i); j++ ) {
00823 if( (!exportRhs && !crsFormat) || acadoRoundAway(dependencyDX(j)) != 0 ) {
00824 loop1->addStatement( rk_xxx.getCol( inputDim-diffsDim+j ) == rk_kkk.getRow( j )*rk_out );
00825 }
00826 }
00827 loop1->addFunctionCall( getNameOUTPUT( i ), rk_xxx, rk_rhsOutputTemp.getAddress(0,0) );
00828 uint numOutputs = getDimOUTPUT( i );
00829 uint outputDim = numOutputs*(1+directions);
00830 loop1->addStatement( tmp_index1 == numMeas[i]*outputDim+index1*(numOutputs*(1+directions)) );
00831 for( j = 0; j < numOutputs; j++ ) {
00832 loop1->addStatement( rk_outputs[i].getCol( tmp_index1+j ) == rk_rhsOutputTemp.getCol( j ) );
00833 }
00834 loop1->addStatement( "}\n" );
00835 }
00836
00837 return SUCCESSFUL_RETURN;
00838 }
00839
00840
00841 returnValue ImplicitRungeKuttaExport::initializeDDMatrix( )
00842 {
00843 uint i, j, k;
00844 DD = DMatrix( numStages, numStages );
00845
00846 for( i = 0; i < numStages; i++ ) {
00847 for( j = 0; j < numStages; j++ ) {
00848 DD( i, j ) = 1;
00849 for( k = 0; k < numStages; k++ ) {
00850 if( k != j ) {
00851 DD( i, j ) *= ((1+cc(i))-cc(k))/(cc(j)-cc(k));
00852 }
00853 }
00854 }
00855 }
00856 return SUCCESSFUL_RETURN;
00857 }
00858
00859
00860 returnValue ImplicitRungeKuttaExport::initializeCoefficients( )
00861 {
00862 uint i, j, k, index;
00863 double sum;
00864 DVector cVec( numStages-1 );
00865 DVector products;
00866 coeffs = DMatrix( numStages, numStages );
00867
00868 for( i = 0; i < numStages; i++ ) {
00869 for( j = 0; j < numStages; j++ ) {
00870 coeffs( i, j ) = 1/((double) numStages-i);
00871 index = 0;
00872 for( k = 0; k < numStages; k++ ) {
00873 if( k != j ) {
00874 coeffs( i, j ) *= 1/((double) cc(j)-cc(k));
00875 cVec(index) = cc(k);
00876 index++;
00877 }
00878 }
00879
00880 if( i > 0 ) {
00881 products = computeCombinations( cVec, 0, i );
00882 sum = 0.0;
00883 for( k = 0; k < products.getDim(); k++ ) {
00884 sum += products(k);
00885 }
00886 if( i%2 == 0 ) {
00887 coeffs( i, j ) *= sum;
00888 }
00889 else {
00890 coeffs( i, j ) *= (-1.0*sum);
00891 }
00892 }
00893 }
00894 }
00895
00896 return SUCCESSFUL_RETURN;
00897 }
00898
00899
00900 DVector ImplicitRungeKuttaExport::computeCombinations( const DVector& cVec, uint index, uint numEls ) {
00901 uint k, l;
00902 DVector products;
00903
00904 if( numEls == 0 ) {
00905 products = DVector(1);
00906 products(0) = 1;
00907 return products;
00908 }
00909 products = DVector();
00910 for( k = index; k < cVec.getDim()-numEls+1; k++ ) {
00911 DVector temp = computeCombinations( cVec, k+1, numEls-1 );
00912 for( l = 0; l < temp.getDim(); l++ ) {
00913 temp(l) *= cVec(k);
00914 }
00915 products.append(temp);
00916 }
00917
00918 return products;
00919 }
00920
00921
00922 DMatrix ImplicitRungeKuttaExport::evaluatePolynomial( uint index )
00923 {
00924 int measGrid;
00925 get( MEASUREMENT_GRID, measGrid );
00926 DMatrix polynV(totalMeas[index],numStages);
00927
00928 double scale2 = 1.0/(grid.getLastTime() - grid.getFirstTime());
00929 for( uint i = 0; i < totalMeas[index]; i++ ) {
00930 double time = outputGrids[index].getTime(i);
00931 if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
00932 uint interv = getIntegrationInterval( time );
00933 time = (time-scale2*grid.getTime(interv))/(scale2*(grid.getTime(interv+1)-grid.getTime(interv)));
00934 }
00935 polynV.setRow( i, evaluatePolynomial( time ) );
00936 }
00937
00938 return polynV;
00939 }
00940
00941
00942 DVector ImplicitRungeKuttaExport::evaluatePolynomial( double time )
00943 {
00944 uint i, j;
00945 DVector coeffsPolyn( numStages );
00946
00947 for( j = 0; j < numStages; j++ ) {
00948 coeffsPolyn( j ) = 0.0;
00949 for( i = 0; i < numStages; i++ ) {
00950 coeffsPolyn( j ) += pow( time, static_cast<int>(numStages-i) )*coeffs( i,j );
00951 }
00952 }
00953
00954 return coeffsPolyn;
00955 }
00956
00957
00958 returnValue ImplicitRungeKuttaExport::evaluatePolynomial( ExportStatementBlock& block, const ExportVariable& variable, const ExportVariable& gridVariable, const std::string& h )
00959 {
00960 uint i, j;
00961
00962 block.addStatement( polynEvalVar == gridVariable );
00963 for( i = 0; i < numStages; i++ ) {
00964 for( j = 0; j < numStages; j++ ) {
00965 if( i == 0 ) {
00966 block.addStatement( variable.getRow( j ) == polynEvalVar*coeffs( numStages-1-i,j ) );
00967 }
00968 else {
00969 block.addStatement( variable.getRow( j ) += polynEvalVar*coeffs( numStages-1-i,j ) );
00970 }
00971 }
00972 if( i < (numStages-1) ) block.addStatement( polynEvalVar == polynEvalVar*gridVariable );
00973 }
00974 for( j = 0; j < numStages; j++ ) {
00975 block.addStatement( (std::string)variable.getFullName() + "[" + toString(j) + "] *= " + toString(h) + ";\n" );
00976 }
00977
00978 return SUCCESSFUL_RETURN;
00979 }
00980
00981
00982 DMatrix ImplicitRungeKuttaExport::evaluateDerivedPolynomial( uint index )
00983 {
00984 int measGrid;
00985 get( MEASUREMENT_GRID, measGrid );
00986 DMatrix polynDerV(totalMeas[index],numStages);
00987
00988 double scale2 = 1.0/(grid.getLastTime() - grid.getFirstTime());
00989 for( uint i = 0; i < totalMeas[index]; i++ ) {
00990 double time = outputGrids[index].getTime(i);
00991 if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
00992 uint interv = getIntegrationInterval( time );
00993 time = (time-scale2*grid.getTime(interv))/(scale2*(grid.getTime(interv+1)-grid.getTime(interv)));
00994 }
00995 polynDerV.setRow( i, evaluateDerivedPolynomial( time ) );
00996 }
00997
00998 return polynDerV;
00999 }
01000
01001
01002 DVector ImplicitRungeKuttaExport::evaluateDerivedPolynomial( double time )
01003 {
01004 uint i, j;
01005 DVector coeffsPolyn( numStages );
01006
01007
01008 for( i = 0; i < numStages; i++ ) {
01009 coeffsPolyn( i ) = 1.0;
01010 for( j = 0; j < numStages; j++ ) {
01011 if( i != j ) {
01012 coeffsPolyn( i ) *= (time-cc(j))/(cc(i)-cc(j));
01013 }
01014 }
01015 }
01016
01017 return coeffsPolyn;
01018 }
01019
01020
01021 returnValue ImplicitRungeKuttaExport::evaluateDerivedPolynomial( ExportStatementBlock& block, const ExportVariable& variable, const ExportVariable& gridVariable )
01022 {
01023 uint i, j;
01024
01025
01026 for( i = 0; i < numStages; i++ ) {
01027 for( j = 0; j < numStages; j++ ) {
01028 if( (i == 0 && j == 1) || (i != j && j == 0) ) {
01029 block.addStatement( (std::string)variable.getFullName() + "[" + toString(i) + "] = (" + gridVariable.getName() + " - " + toString(cc(j)) + ")*" + toString(1/(cc(i)-cc(j))) + ";\n" );
01030 }
01031 else if( i != j ) {
01032 block.addStatement( (std::string)variable.getFullName() + "[" + toString(i) + "] *= (" + gridVariable.getName() + " - " + toString(cc(j)) + ")*" + toString(1/(cc(i)-cc(j))) + ";\n" );
01033 }
01034 }
01035 }
01036
01037 return SUCCESSFUL_RETURN;
01038 }
01039
01040
01041 DVector ImplicitRungeKuttaExport::divideMeasurements( uint index )
01042 {
01043 DVector meas( grid.getNumIntervals() );
01044 meas.setZero();
01045
01046 for( uint i = 0; i < outputGrids[index].getNumIntervals(); i++ ) {
01047 uint interv = getIntegrationInterval( outputGrids[index].getTime(i) );
01048 meas(interv) = meas(interv)+1;
01049 }
01050
01051 return meas;
01052 }
01053
01054
01055 returnValue ImplicitRungeKuttaExport::setup( )
01056 {
01057 if( CONTINUOUS_OUTPUT && !equidistantControlGrid() ) return ACADOERROR( RET_INVALID_OPTION );
01058
01059 int measGrid;
01060 get( MEASUREMENT_GRID, measGrid );
01061
01062 int intMode;
01063 userInteraction->get( IMPLICIT_INTEGRATOR_MODE,intMode );
01064 switch( (ImplicitIntegratorMode) intMode ) {
01065 case IFTR:
01066 REUSE = true;
01067 break;
01068 case IFT:
01069 REUSE = false;
01070 break;
01071 default:
01072 return ACADOERROR( RET_INVALID_OPTION );
01073 }
01074
01075 int newNumIts;
01076 userInteraction->get( IMPLICIT_INTEGRATOR_NUM_ITS,newNumIts );
01077 if (newNumIts >= 0) {
01078 numIts = newNumIts;
01079 }
01080
01081 int newNumItsInit;
01082 userInteraction->get( IMPLICIT_INTEGRATOR_NUM_ITS_INIT,newNumItsInit );
01083 if (newNumItsInit >= 0) {
01084 numItsInit = newNumItsInit;
01085 }
01086
01087 int debugMode;
01088 get( INTEGRATOR_DEBUG_MODE, debugMode );
01089
01090 DifferentialStateDerivative dummy4;
01091 dummy4.clearStaticCounters();
01092 dx = DifferentialStateDerivative("", NDX, 1);
01093
01094 AlgebraicState dummy3;
01095 dummy3.clearStaticCounters();
01096 z = AlgebraicState("", NXA, 1);
01097
01098 uint Xmax = NX1;
01099 if( NX2 > Xmax ) Xmax = NX2;
01100 if( NX3 > Xmax ) Xmax = NX3;
01101 NVARS2 = NX1+NX2+NXA+NU+NDX2;
01102 NVARS3 = 0;
01103 diffsDim = 0;
01104 inputDim = NX+NXA + NU + NOD;
01105
01106 int useOMP;
01107 get(CG_USE_OPENMP, useOMP);
01108 ExportStruct structWspace;
01109 structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
01110
01111 uint timeDep = 0;
01112 if( timeDependant ) timeDep = 1;
01113
01114 rk_ttt = ExportVariable( "rk_ttt", 1, 1, REAL, structWspace, true );
01115 rk_xxx = ExportVariable( "rk_xxx", 1, inputDim+NDX+timeDep, REAL, structWspace );
01116 rk_kkk = ExportVariable( "rk_kkk", NX+NXA, numStages, REAL, structWspace );
01117 rk_A = ExportVariable( "rk_A", numStages*(NX2+NXA), numStages*(NX2+NXA), REAL, structWspace );
01118 if ( (bool)debugMode == true && useOMP ) {
01119 return ACADOERROR( RET_INVALID_OPTION );
01120 }
01121 else {
01122 debug_mat = ExportVariable( "debug_mat", numStages*(NX2+NXA), numStages*(NX2+NXA), REAL, ACADO_VARIABLES );
01123 }
01124 rk_b = ExportVariable( "rk_b", numStages*(Xmax+NXA), 1, REAL, structWspace );
01125 rk_rhsTemp = ExportVariable( "rk_rhsTemp", NX+NXA+NDX, 1, REAL, structWspace );
01126 rk_diffsTemp2 = ExportVariable( "rk_diffsTemp2", 1, (NX2+NXA)*(NVARS2), REAL, structWspace );
01127 rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, true );
01128 rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
01129 integrate = ExportFunction( "integrate", rk_eta );
01130 uint i;
01131 for( i = 0; i < rk_outputs.size(); i++ ) {
01132 integrate.addArgument( rk_outputs[i] );
01133 }
01134 integrate.addArgument( reset_int );
01135 if( !equidistantControlGrid() ) integrate.addArgument( rk_index );
01136 integrate.setReturnValue( error_code );
01137
01138 rk_eta.setDoc( "Working array of size " + toString( rk_eta.getDim() ) + " to pass the input values and return the results." );
01139 for( i = 0; i < rk_outputs.size(); i++ ) {
01140 rk_outputs[i].setDoc( "Working array of size " + toString( rk_outputs[ i ].getDim() ) + " to return the extra output results." );
01141 }
01142 reset_int.setDoc( "The internal memory of the integrator can be reset." );
01143 rk_index.setDoc( "Number of the shooting interval." );
01144 error_code.setDoc( "Status code of the integrator." );
01145 integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
01146 integrate.addLinebreak( );
01147
01148 rhs_in = ExportVariable( "x", inputDim+NX, 1, REAL, ACADO_LOCAL );
01149 rhs_out = ExportVariable( "f", NX+NXA, 1, REAL, ACADO_LOCAL );
01150 fullRhs = ExportFunction( "full_rhs", rhs_in, rhs_out );
01151 rhs_in.setDoc( "The state and parameter values." );
01152 rhs_out.setDoc( "Right-hand side evaluation." );
01153 fullRhs.doc( "Evaluates the right-hand side of the full model." );
01154 fullRhs.addLinebreak( );
01155
01156 if( NX2 > 0 || NXA > 0 ) {
01157
01158 int solverType;
01159 userInteraction->get( LINEAR_ALGEBRA_SOLVER,solverType );
01160
01161 if ( solver )
01162 delete solver;
01163 solver = 0;
01164
01165 switch( (LinearAlgebraSolver) solverType ) {
01166 case GAUSS_LU:
01167 solver = new ExportGaussElim( userInteraction,commonHeaderName );
01168 break;
01169 case HOUSEHOLDER_QR:
01170 solver = new ExportHouseholderQR( userInteraction,commonHeaderName );
01171 break;
01172 default:
01173 return ACADOERROR( RET_INVALID_OPTION );
01174 }
01175 solver->setReuse( true );
01176 solver->init( (NX2+NXA)*numStages );
01177 solver->setup();
01178 rk_auxSolver = solver->getGlobalExportVariable( 1 );
01179 }
01180
01181 return SUCCESSFUL_RETURN;
01182 }
01183
01184
01185 returnValue ImplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_, const std::vector<Expression> outputExpressions_ ) {
01186
01187 int sensGen;
01188 get( DYNAMIC_SENSITIVITY, sensGen );
01189 sensGen = ( (ExportSensitivityType)sensGen != NO_SENSITIVITY );
01190
01191 returnValue val = SUCCESSFUL_RETURN;
01192 CONTINUOUS_OUTPUT = true;
01193 if( outputGrids_.size() != outputExpressions_.size() ) return ACADOERROR( RET_INVALID_ARGUMENTS );
01194 outputGrids = outputGrids_;
01195 outputExpressions = outputExpressions_;
01196
01197 int measGrid;
01198 get( MEASUREMENT_GRID, measGrid );
01199
01200 numDX_output = DVector(outputGrids.size());
01201 numXA_output = DVector(outputGrids.size());
01202 numVARS_output = DVector(outputGrids.size());
01203
01204 uint i;
01205 uint maxOutputs = 0;
01206 uint maxVARS = 0;
01207 rk_outputs.clear();
01208 outputs.clear();
01209 diffs_outputs.clear();
01210 for( i = 0; i < outputGrids.size(); i++ ) {
01211 uint numOutputs = outputExpressions_[i].getDim();
01212 uint outputDim = outputGrids[i].getNumIntervals( )*numOutputs*(NX+NU+1);
01213 if( !sensGen ) outputDim = outputGrids[i].getNumIntervals( )*numOutputs;
01214
01215 if( numOutputs > maxOutputs ) maxOutputs = numOutputs;
01216
01217 OutputFcn f_Output;
01218 f_Output << outputExpressions_[i];
01219
01220 if( f_Output.getNDX() > 0 ) numDX_output(i) = NDX;
01221 else numDX_output(i) = 0;
01222
01223 if( f_Output.getNXA() > 0 ) numXA_output(i) = NXA;
01224 else numXA_output(i) = 0;
01225
01226 OutputFcn g_Output;
01227 for( uint j = 0; j < outputExpressions_[i].getDim(); j++ ) {
01228 g_Output << forwardDerivative( outputExpressions_[i](j), x );
01229 g_Output << forwardDerivative( outputExpressions_[i](j), z );
01230 g_Output << forwardDerivative( outputExpressions_[i](j), u );
01231 if( numDX_output(i) > 0 ) g_Output << forwardDerivative( outputExpressions_[i](j), dx );
01232 }
01233 if( numDX_output(i) > 0 ) {
01234 numVARS_output(i) = NX+NXA+NU+NDX;
01235 }
01236 else {
01237 numVARS_output(i) = NX+NXA+NU;
01238 }
01239 if( numVARS_output(i) > maxVARS ) maxVARS = numVARS_output(i);
01240
01241 ExportAcadoFunction OUTPUT, diffs_OUTPUT;
01242 val = val & OUTPUT.init( f_Output,std::string("acado_output")+toString(i)+"_rhs",NX,NXA,NU,NP,NDX,NOD ) & diffs_OUTPUT.init( g_Output,std::string("acado_output")+toString(i)+"_diffs",NX,NXA,NU,NP,NDX,NOD );
01243
01244 ExportVariable rk_output( std::string("rk_output")+toString(i), 1, outputDim, REAL );
01245 rk_outputs.push_back( rk_output );
01246
01247 outputs.push_back( OUTPUT );
01248 if( sensGen ) diffs_outputs.push_back( diffs_OUTPUT );
01249
01250 DMatrix dependencyMat = outputExpressions[i].getDependencyPattern( x );
01251 if(z.getDim()>0) dependencyMat.appendCols( outputExpressions[i].getDependencyPattern( z ) );
01252 if(u.getDim()>0) dependencyMat.appendCols( outputExpressions[i].getDependencyPattern( u ) );
01253 if(dx.getDim()>0) dependencyMat.appendCols( outputExpressions[i].getDependencyPattern( dx ) );
01254
01255 outputDependencies.push_back( dependencyMat );
01256 totalMeas.push_back( outputGrids[i].getNumIntervals() );
01257
01258
01259 ExportVariable gridVariable( (std::string)"gridOutput" + toString( i ), 1, totalMeas[i], REAL, ACADO_VARIABLES );
01260 gridVariables.push_back( gridVariable );
01261 }
01262
01263 int useOMP;
01264 get(CG_USE_OPENMP, useOMP);
01265 ExportStruct structWspace;
01266 structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
01267
01268 setup();
01269 rk_rhsOutputTemp = ExportVariable( "rk_rhsOutputTemp", 1, maxOutputs, REAL, structWspace );
01270 if( sensGen ) rk_diffsOutputTemp = ExportVariable( "rk_diffsOutputTemp", 1, maxOutputs*(maxVARS), REAL, structWspace );
01271 rk_outH = ExportVariable( "rk_outH", numStages, 1, REAL, structWspace );
01272 rk_out = ExportVariable( "rk_out2", numStages, 1, REAL, structWspace );
01273 polynEvalVar = ExportVariable( "tmp_polyn", 1, 1, REAL, ACADO_LOCAL, true );
01274
01275 return ( val );
01276 }
01277
01278
01279 returnValue ImplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_,
01280 const std::vector<std::string> _outputNames,
01281 const std::vector<std::string> _diffs_outputNames,
01282 const std::vector<uint> _dims_output ) {
01283
01284 if(rhs.isExternal() == true && (rk_outputs.size() + outputs.size() + diffs_outputs.size()) == 0) {
01285 int sensGen;
01286 get( DYNAMIC_SENSITIVITY, sensGen );
01287 sensGen = ( (ExportSensitivityType)sensGen != NO_SENSITIVITY );
01288
01289 CONTINUOUS_OUTPUT = true;
01290 if( outputGrids_.size() != _outputNames.size() || outputGrids_.size() != _diffs_outputNames.size() || outputGrids_.size() != _dims_output.size() ) {
01291 return ACADOERROR( RET_INVALID_ARGUMENTS );
01292 }
01293 outputGrids = outputGrids_;
01294 num_outputs = _dims_output;
01295
01296 int measGrid;
01297 get( MEASUREMENT_GRID, measGrid );
01298
01299 numDX_output = DVector(outputGrids.size());
01300 numXA_output = DVector(outputGrids.size());
01301 numVARS_output = DVector(outputGrids.size());
01302
01303 uint i;
01304 uint maxOutputs = 0;
01305 rk_outputs.clear();
01306 outputs.clear();
01307 diffs_outputs.clear();
01308 for( i = 0; i < outputGrids.size(); i++ ) {
01309 uint numOutputs = num_outputs[i];
01310 uint outputDim = outputGrids[i].getNumIntervals( )*numOutputs*(NX+NU+1);
01311 if( !sensGen ) outputDim = outputGrids[i].getNumIntervals( )*numOutputs;
01312
01313 if( numOutputs > maxOutputs ) maxOutputs = numOutputs;
01314
01315 numDX_output(i) = NDX;
01316 numXA_output(i) = NXA;
01317 numVARS_output(i) = NX+NXA+NU+NDX;
01318
01319 ExportAcadoFunction OUTPUT(_outputNames[i]);
01320 ExportAcadoFunction diffs_OUTPUT(_diffs_outputNames[i]);
01321
01322 outputs.push_back( OUTPUT );
01323 if( sensGen ) diffs_outputs.push_back( diffs_OUTPUT );
01324
01325 ExportVariable rk_output( std::string("rk_output")+toString(i), 1, outputDim, REAL );
01326 rk_outputs.push_back( rk_output );
01327
01328 totalMeas.push_back( outputGrids[i].getNumIntervals() );
01329
01330
01331 ExportVariable gridVariable( (std::string)"gridOutput" + toString(i), 1, totalMeas[i], REAL, ACADO_VARIABLES );
01332 gridVariables.push_back( gridVariable );
01333 }
01334 uint maxVARS = NX+NXA+NU+NDX;
01335
01336 int useOMP;
01337 get(CG_USE_OPENMP, useOMP);
01338 ExportStruct structWspace;
01339 structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
01340
01341 setup();
01342 rk_rhsOutputTemp = ExportVariable( "rk_rhsOutputTemp", 1, maxOutputs, REAL, structWspace );
01343 if( sensGen ) rk_diffsOutputTemp = ExportVariable( "rk_diffsOutputTemp", 1, maxOutputs*(maxVARS), REAL, structWspace );
01344 rk_outH = ExportVariable( "rk_outH", numStages, 1, REAL, structWspace );
01345 rk_out = ExportVariable( "rk_out2", numStages, 1, REAL, structWspace );
01346 polynEvalVar = ExportVariable( "tmp_polyn", 1, 1, REAL, ACADO_LOCAL, true );
01347
01348 exportRhs = false;
01349 }
01350 else {
01351 return ACADOERROR( RET_INVALID_OPTION );
01352 }
01353
01354
01355 return SUCCESSFUL_RETURN;
01356 }
01357
01358
01359 returnValue ImplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_,
01360 const std::vector<std::string> _outputNames,
01361 const std::vector<std::string> _diffs_outputNames,
01362 const std::vector<uint> _dims_output,
01363 const std::vector<DMatrix> _outputDependencies ) {
01364
01365 outputDependencies = _outputDependencies;
01366 crsFormat = true;
01367
01368 return setupOutput( outputGrids_, _outputNames, _diffs_outputNames, _dims_output );
01369 }
01370
01371
01372
01373
01374
01375
01376 returnValue ImplicitRungeKuttaExport::prepareOutputEvaluation( ExportStatementBlock& code )
01377 {
01378 uint i;
01379 int measGrid;
01380 get( MEASUREMENT_GRID, measGrid );
01381
01382 for( i = 0; i < outputGrids.size(); i++ ) {
01383 if( (MeasurementGrid)measGrid != ONLINE_GRID ) {
01384 DMatrix polynV = evaluatePolynomial( i );
01385 DMatrix polynDerV = evaluateDerivedPolynomial( i );
01386 DVector measurements = divideMeasurements( i );
01387
01388 double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
01389 ExportVariable polynVariable = ExportVariable( (std::string)"polynOutput" + toString( i ), polynV*=h, STATIC_CONST_REAL );
01390 code.addDeclaration( polynVariable );
01391 polynVariable = ExportVariable( (std::string)"polynOutput" + toString( i ), totalMeas[i], numStages, STATIC_CONST_REAL );
01392 polynVariables.push_back( polynVariable );
01393
01394 if( NXA > 0 || NDX > 0 ) {
01395 ExportVariable polynDerVariable( (std::string)"polynDerOutput" + toString( i ), polynDerV, STATIC_CONST_REAL );
01396 code.addDeclaration( polynDerVariable );
01397 polynDerVariable = ExportVariable( (std::string)"polynDerOutput" + toString( i ), totalMeas[i], numStages, STATIC_CONST_REAL );
01398 polynDerVariables.push_back( polynDerVariable );
01399 }
01400
01401 ExportVariable numMeasVariable( (std::string)"numMeas" + toString( i ), measurements, STATIC_CONST_INT );
01402 if( (MeasurementGrid)measGrid == OFFLINE_GRID ) {
01403 code.addDeclaration( numMeasVariable );
01404 }
01405 numMeasVariables.push_back( numMeasVariable );
01406 }
01407 }
01408
01409 return SUCCESSFUL_RETURN;
01410 }
01411
01412
01413 returnValue ImplicitRungeKuttaExport::copy( const ImplicitRungeKuttaExport& arg
01414 )
01415 {
01416 numStages = arg.numStages;
01417 numIts = arg.numIts;
01418 numItsInit = arg.numItsInit;
01419 diffsDim = arg.diffsDim;
01420 inputDim = arg.inputDim;
01421
01422 rhs = arg.rhs;
01423 outputs = arg.outputs;
01424 diffs_rhs = arg.diffs_rhs;
01425 diffs_outputs = arg.diffs_outputs;
01426 num_outputs = arg.num_outputs;
01427 grid = arg.grid;
01428 outputGrids = arg.outputGrids;
01429 solver = arg.solver;
01430
01431
01432 rk_ttt = arg.rk_ttt;
01433 rk_xxx = arg.rk_xxx;
01434 rk_kkk = arg.rk_kkk;
01435 rk_A = arg.rk_A;
01436 debug_mat = arg.debug_mat;
01437 rk_b = arg.rk_b;
01438 rk_diffK = arg.rk_diffK;
01439 rk_rhsTemp = arg.rk_rhsTemp;
01440 rk_diffsTemp2 = arg.rk_diffsTemp2;
01441 rk_diffsTemp3 = arg.rk_diffsTemp3;
01442 rk_diffsNew1 = arg.rk_diffsNew1;
01443 rk_diffsPrev2 = arg.rk_diffsPrev1;
01444 rk_diffsNew2 = arg.rk_diffsNew2;
01445 rk_diffsPrev2 = arg.rk_diffsPrev2;
01446 rk_diffsNew3 = arg.rk_diffsNew3;
01447 rk_diffsPrev2 = arg.rk_diffsPrev3;
01448 rk_eta = arg.rk_eta;
01449 rk_rhsOutputTemp = arg.rk_rhsOutputTemp;
01450 rk_diffsOutputTemp = arg.rk_diffsOutputTemp;
01451 rk_outH = arg.rk_outH;
01452 rk_out = arg.rk_out;
01453 polynEvalVar = arg.polynEvalVar;
01454 rk_outputs = arg.rk_outputs;
01455
01456 gridVariables = arg.gridVariables;
01457 totalMeas = arg.totalMeas;
01458
01459
01460 integrate = arg.integrate;
01461 fullRhs = arg.fullRhs;
01462
01463 REUSE = arg.REUSE;
01464 CONTINUOUS_OUTPUT = arg.CONTINUOUS_OUTPUT;
01465
01466 DD = arg.DD;
01467 coeffs = arg.coeffs;
01468
01469 return SUCCESSFUL_RETURN;
01470 }
01471
01472
01473 uint ImplicitRungeKuttaExport::getNumIts() const
01474 {
01475 return numIts;
01476 }
01477
01478
01479 uint ImplicitRungeKuttaExport::getNumItsInit() const
01480 {
01481 return numItsInit;
01482 }
01483
01484
01485 CLOSE_NAMESPACE_ACADO
01486
01487