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/utils/acado_utils.hpp>
00035 #include <acado/symbolic_expression/symbolic_expression.hpp>
00036 #include <acado/function/function_.hpp>
00037 #include <acado/function/differential_equation.hpp>
00038 #include <acado/integrator/integrator.hpp>
00039 #include <acado/integrator/integrator_runge_kutta.hpp>
00040 #include <acado/integrator/integrator_runge_kutta12.hpp>
00041 #include <acado/integrator/integrator_runge_kutta23.hpp>
00042 #include <acado/integrator/integrator_runge_kutta45.hpp>
00043 #include <acado/integrator/integrator_runge_kutta78.hpp>
00044 #include <acado/integrator/integrator_bdf.hpp>
00045
00046 using namespace std;
00047
00048 BEGIN_NAMESPACE_ACADO
00049
00050
00051
00052
00053
00054
00055
00056 Integrator::Integrator( )
00057 :AlgorithmicBase( ){
00058
00059
00060
00061 rhs = 0;
00062 m = 0;
00063 ma = 0;
00064 mdx = 0;
00065 md = 0;
00066 mn = 0;
00067 mu = 0;
00068 mui = 0;
00069 mp = 0;
00070 mpi = 0;
00071 mw = 0;
00072
00073 transition = 0;
00074
00075
00076
00077
00078 h = (double*)calloc(1,sizeof(double));
00079 h[0] = 0.001 ;
00080 hmin = 0.000001 ;
00081 hmax = 1.0e10 ;
00082
00083 tune = 0.5 ;
00084 TOL = 0.000001 ;
00085
00086
00087
00088
00089 diff_index = 0;
00090 ddiff_index = 0;
00091 alg_index = 0;
00092 control_index = 0;
00093 parameter_index = 0;
00094 int_control_index = 0;
00095 int_parameter_index = 0;
00096 disturbance_index = 0;
00097 time_index = 0;
00098
00099
00100
00101
00102 count = 0 ;
00103 count2 = 0 ;
00104 count3 = 0 ;
00105 maxNumberOfSteps = 1000;
00106
00107
00108
00109
00110 PrintLevel = LOW;
00111
00112
00113
00114
00115 nFDirs = 0;
00116 nBDirs = 0;
00117 nFDirs2 = 0;
00118 nBDirs2 = 0;
00119
00120
00121
00122
00123 soa = SOA_UNFROZEN;
00124
00125 setupOptions( );
00126 setupLogging( );
00127 }
00128
00129
00130
00131 Integrator::Integrator( const Integrator &arg )
00132 :AlgorithmicBase( arg ){
00133
00134 if( arg.transition == 0 ) transition = 0;
00135 else transition = new Transition( *arg.transition );
00136 }
00137
00138
00139 Integrator::~Integrator( ){
00140
00141
00142
00143
00144 if( rhs != NULL )
00145 delete rhs;
00146
00147 if( transition != 0 )
00148 delete transition;
00149
00150
00151
00152
00153 free(h);
00154
00155
00156
00157
00158
00159 if( diff_index != 0 )
00160 delete[] diff_index;
00161
00162 if( ddiff_index != 0 )
00163 delete[] ddiff_index;
00164
00165 if( alg_index != 0 )
00166 delete[] alg_index;
00167
00168 if( control_index != 0 )
00169 delete[] control_index;
00170
00171 if( parameter_index != 0 )
00172 delete[] parameter_index;
00173
00174 if( int_control_index != 0 )
00175 delete[] int_control_index;
00176
00177 if( int_parameter_index != 0 )
00178 delete[] int_parameter_index;
00179
00180 if( disturbance_index != 0 )
00181 delete[] disturbance_index;
00182 }
00183
00184
00185 returnValue Integrator::init( const DifferentialEquation &rhs_,
00186 const Transition &trs
00187 )
00188 {
00189 returnValue returnvalue;
00190
00191 returnvalue = init( rhs_ );
00192 transition = new Transition(trs);
00193
00194 return returnvalue;
00195 }
00196
00197
00198 returnValue Integrator::setTransition( const Transition &trs
00199 )
00200 {
00201 if( transition != 0 ) delete transition;
00202 transition = new Transition(trs);
00203 return SUCCESSFUL_RETURN;
00204 }
00205
00206
00207 returnValue Integrator::integrate( double t0_ ,
00208 double tend_,
00209 double *x0 ,
00210 double *xa ,
00211 double *p ,
00212 double *u ,
00213 double *w ){
00214
00215 Grid t_( t0_, tend_, 2 );
00216 return integrate( t_, x0, xa, p, u, w );
00217 }
00218
00219
00220 returnValue Integrator::integrate( const Grid &t_,
00221 double *x0,
00222 double *xa,
00223 double *p ,
00224 double *u ,
00225 double *w ){
00226
00227 if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );
00228 DVector components = rhs->getDifferentialStateComponents();
00229
00230 DVector tmpX ( components.getDim(), x0 );
00231 DVector tmpXA( rhs->getNXA() , xa );
00232 DVector tmpP ( rhs->getNP () , p );
00233 DVector tmpU ( rhs->getNU () , u );
00234 DVector tmpW ( rhs->getNW () , w );
00235
00236 return integrate( t_, tmpX, tmpXA, tmpP, tmpU, tmpW );
00237 }
00238
00239
00240
00241 returnValue Integrator::integrate( double t0_ ,
00242 double tend_,
00243 const DVector &x0 ,
00244 const DVector &xa ,
00245 const DVector &p ,
00246 const DVector &u ,
00247 const DVector &w ){
00248
00249 Grid t_( t0_, tend_, 2 );
00250 return integrate( t_, x0, xa, p, u, w );
00251 }
00252
00253
00254
00255 returnValue Integrator::integrate( const Grid &t_ ,
00256 const DVector &x0 ,
00257 const DVector &xa ,
00258 const DVector &p ,
00259 const DVector &u ,
00260 const DVector &w ){
00261
00262 int run1;
00263 returnValue returnvalue;
00264 if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );
00265
00266 DVector tmpX;
00267
00268 DVector components = rhs->getDifferentialStateComponents();
00269
00270 const int N = components.getDim();
00271
00272 if( x0.getDim() != 0 ){
00273 tmpX.init( components.getDim() );
00274 for( run1 = 0; run1 < (int) components.getDim(); run1++ )
00275 tmpX(run1) = x0((int) components(run1));
00276 }
00277
00278
00279
00280
00281
00282 returnvalue = evaluate( tmpX, xa, p, u, w, t_ );
00283
00284 if( returnvalue != SUCCESSFUL_RETURN )
00285 return returnvalue;
00286
00287 xE.init(rhs->getDim());
00288 xE.setZero();
00289
00290 DVector tmp(rhs->getDim());
00291 getProtectedX(&tmp);
00292
00293 for( run1 = 0; run1 < N; run1++ )
00294 xE((int) components(run1)) = tmp(run1);
00295
00296 for( run1 = N; run1 < N + ma; run1++ )
00297 xE(run1) = tmp(run1);
00298
00299 if( transition != 0 )
00300 returnvalue = evaluateTransition( t_.getLastTime(), xE, xa, p, u, w );
00301
00302 return returnvalue;
00303 }
00304
00305
00306
00307
00308 returnValue Integrator::setForwardSeed( const int &order,
00309 const DVector &xSeed,
00310 const DVector &pSeed,
00311 const DVector &uSeed,
00312 const DVector &wSeed ){
00313
00314
00315 int run1;
00316 if( rhs == 0 ) return ACADOERROR( RET_TRIVIAL_RHS );
00317
00318 DVector tmpX;
00319 DVector components = rhs->getDifferentialStateComponents();
00320
00321 dP = pSeed;
00322 dU = uSeed;
00323 dW = wSeed;
00324
00325 if( xSeed.getDim() != 0 ){
00326
00327 tmpX.init( components.getDim() );
00328 for( run1 = 0; run1 < (int) components.getDim(); run1++ )
00329 tmpX(run1) = xSeed((int) components(run1));
00330 }
00331
00332 return setProtectedForwardSeed( tmpX, pSeed, uSeed, wSeed, order );
00333 }
00334
00335
00336
00337
00338 returnValue Integrator::setBackwardSeed( const int &order,
00339 const DVector &seed ){
00340
00341 dXb = seed;
00342
00343 uint run1;
00344 DVector tmp( seed.getDim() );
00345 DVector components = rhs->getDifferentialStateComponents();
00346
00347 if( seed.getDim() != 0 ){
00348 tmp.init( components.getDim() );
00349 for( run1 = 0; run1 < components.getDim(); run1++ )
00350 tmp(run1) = seed((int) components(run1));
00351 }
00352 return setProtectedBackwardSeed( tmp, order );
00353 }
00354
00355
00356
00357 returnValue Integrator::integrateSensitivities( ){
00358
00359 uint run1;
00360 returnValue returnvalue;
00361
00362
00363 if( ( nBDirs > 0 || nBDirs2 > 0 ) && transition != 0 ){
00364
00365 int order;
00366 if( nBDirs2 > 0 ) order = 2;
00367 else order = 1;
00368
00369 returnvalue = diffTransitionBackward( dXb, dPb, dUb, dWb, order );
00370
00371 setBackwardSeed( order, dXb );
00372
00373 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00374 }
00375
00376 returnvalue = evaluateSensitivities();
00377
00378 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00379
00380
00381 if( nBDirs > 0 || nBDirs2 > 0 ) return SUCCESSFUL_RETURN;
00382
00383 int order = 1;
00384 if( nFDirs2 > 0 ) order = 2;
00385
00386 DMatrix tmp( rhs->getDim(), 1 );
00387 returnvalue = getProtectedForwardSensitivities(&tmp,order);
00388
00389 DVector components = rhs->getDifferentialStateComponents();
00390
00391 dX.init(rhs->getDim()-ma);
00392 dX.setZero();
00393
00394 for( run1 = 0; run1 < components.getDim(); run1++ )
00395 dX((int) components(run1)) = tmp(run1,0);
00396
00397 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00398
00399 if( transition != 0 )
00400 returnvalue = diffTransitionForward( dX, dP, dU, dW, order );
00401
00402 return returnvalue;
00403 }
00404
00405
00406 returnValue Integrator::getForwardSensitivities( DVector &Dx,
00407 int order ) const{
00408
00409 Dx = dX;
00410 return SUCCESSFUL_RETURN;
00411 }
00412
00413
00414 returnValue Integrator::getForwardSensitivities( VariablesGrid &Dx,
00415 int order ) const{
00416
00417 if( order == 1 ) Dx = dxStore;
00418 if( order == 2 ) Dx = ddxStore;
00419
00420 return SUCCESSFUL_RETURN;
00421 }
00422
00423
00424 returnValue Integrator::getBackwardSensitivities( DVector &DX,
00425 DVector &DP ,
00426 DVector &DU ,
00427 DVector &DW ,
00428 int order ) const{
00429
00430 int run2;
00431 returnValue returnvalue;
00432
00433 DVector tmpX ( rhs->getDim() );
00434
00435 DX.setZero();
00436 DP.setZero();
00437 DU.setZero();
00438 DW.setZero();
00439
00440 returnvalue = getProtectedBackwardSensitivities( tmpX, DP, DU, DW, order );
00441 DVector components = rhs->getDifferentialStateComponents();
00442
00443 for( run2 = 0; run2 < (int) components.getDim(); run2++ )
00444 DX((int) components(run2)) = tmpX(run2);
00445
00446 for( run2 = 0; run2 < (int) dPb.getDim(); run2++ )
00447 DP(run2) += dPb(run2);
00448
00449 for( run2 = 0; run2 < (int) dUb.getDim(); run2++ )
00450 DU(run2) += dUb(run2);
00451
00452 for( run2 = 0; run2 < (int) dWb.getDim(); run2++ )
00453 DW(run2) += dWb(run2);
00454
00455 return returnvalue;
00456 }
00457
00458
00459 BooleanType Integrator::canHandleImplicitSwitches( ) const{
00460
00461 return BT_FALSE;
00462 }
00463
00464
00465 BooleanType Integrator::isDifferentialEquationDefined( ) const{
00466
00467 if ( rhs != 0 ) return BT_TRUE ;
00468 else return BT_FALSE;
00469 }
00470
00471 BooleanType Integrator::isDifferentialEquationAffine( ) const
00472 {
00473 if ( rhs == 0 ) return BT_FALSE ;
00474 else return rhs->isAffine();
00475 }
00476
00477
00478 double Integrator::getDifferentialEquationSampleTime( ) const
00479 {
00480 if ( rhs == 0 )
00481 return -INFTY;
00482 else
00483 return rhs->getStepLength( );
00484 }
00485
00486
00487
00488 returnValue Integrator::deleteAllSeeds(){
00489
00490 nBDirs = 0;
00491 nFDirs = 0;
00492 nBDirs2 = 0;
00493 nFDirs2 = 0;
00494
00495 return SUCCESSFUL_RETURN;
00496 }
00497
00498
00499
00500
00501
00502
00503
00504
00505 returnValue Integrator::evaluateTransition( const double time, DVector &xd, const DVector &xa,
00506 const DVector &p, const DVector &u, const DVector &w ){
00507
00508 ASSERT( transition != 0 );
00509 EvaluationPoint z( *transition, xd.getDim(), xa.getDim(), p.getDim(), u.getDim(), w.getDim() );
00510 z.setT ( time );
00511 z.setX ( xd );
00512 z.setXA( xa );
00513 z.setP ( p );
00514 z.setU ( u );
00515 z.setW ( w );
00516 xd = transition->evaluate( z );
00517 return SUCCESSFUL_RETURN;
00518 }
00519
00520
00521 returnValue Integrator::diffTransitionForward( DVector &DX,
00522 const DVector &DP,
00523 const DVector &DU,
00524 const DVector &DW,
00525 const int &order ){
00526
00527 ASSERT( transition != 0 );
00528 EvaluationPoint z( *transition, DX.getDim(), 0, DP.getDim(), DU.getDim(), DW.getDim() );
00529 z.setX ( DX );
00530 z.setP ( DP );
00531 z.setU ( DU );
00532 z.setW ( DW );
00533
00534 if( order == 1 ) DX = transition->AD_forward( z );
00535
00536 if( order != 1 ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00537
00538 return SUCCESSFUL_RETURN;
00539 }
00540
00541
00542 returnValue Integrator::diffTransitionBackward( DVector &DX,
00543 DVector &DP,
00544 DVector &DU,
00545 DVector &DW,
00546 int &order ){
00547
00548 ASSERT( transition != 0 );
00549 if( order != 1 ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
00550
00551 EvaluationPoint z( *transition, DX.getDim(), 0, DP.getDim(), DU.getDim(), DW.getDim() );
00552
00553 transition->AD_backward( DX, z );
00554
00555 DX = z.getX();
00556 DP = z.getP();
00557 DU = z.getU();
00558 DW = z.getW();
00559
00560 return SUCCESSFUL_RETURN;
00561 }
00562
00563
00564
00565
00566 returnValue Integrator::setupOptions( )
00567 {
00568 addOption( MAX_NUM_INTEGRATOR_STEPS , defaultMaxNumSteps );
00569 addOption( INTEGRATOR_TOLERANCE , defaultIntegratorTolerance );
00570 addOption( ABSOLUTE_TOLERANCE , defaultAbsoluteTolerance );
00571 addOption( INITIAL_INTEGRATOR_STEPSIZE , defaultInitialStepsize );
00572 addOption( MIN_INTEGRATOR_STEPSIZE , defaultMinStepsize );
00573 addOption( MAX_INTEGRATOR_STEPSIZE , defaultMaxStepsize );
00574 addOption( STEPSIZE_TUNING , defaultStepsizeTuning );
00575 addOption( CORRECTOR_TOLERANCE , defaultCorrectorTolerance );
00576 addOption( INTEGRATOR_PRINTLEVEL , defaultIntegratorPrintlevel );
00577 addOption( LINEAR_ALGEBRA_SOLVER , defaultLinearAlgebraSolver );
00578 addOption( ALGEBRAIC_RELAXATION , defaultAlgebraicRelaxation );
00579 addOption( RELAXATION_PARAMETER , defaultRelaxationParameter );
00580 addOption( PRINT_INTEGRATOR_PROFILE , defaultprintIntegratorProfile );
00581
00582 return SUCCESSFUL_RETURN;
00583 }
00584
00585
00586
00587 void Integrator::initializeOptions(){
00588
00589 get( MAX_NUM_INTEGRATOR_STEPS , maxNumberOfSteps );
00590 get( INTEGRATOR_TOLERANCE , TOL );
00591 get( INITIAL_INTEGRATOR_STEPSIZE , hini );
00592 get( MIN_INTEGRATOR_STEPSIZE , hmin );
00593 get( MAX_INTEGRATOR_STEPSIZE , hmax );
00594 get( STEPSIZE_TUNING , tune );
00595 get( INTEGRATOR_PRINTLEVEL , PrintLevel );
00596 get( LINEAR_ALGEBRA_SOLVER , las );
00597 }
00598
00599 returnValue Integrator::setupLogging( ){
00600
00601 LogRecord tmp(LOG_AT_EACH_ITERATION, PS_DEFAULT);
00602
00603 tmp.addItem( LOG_TIME_INTEGRATOR, "INTEGRATION TIME [sec]: ");
00604 tmp.addItem( LOG_NUMBER_OF_INTEGRATOR_STEPS, "NUMBER OF STEPS : ");
00605 tmp.addItem( LOG_NUMBER_OF_INTEGRATOR_REJECTED_STEPS, "NUMBER OF REJECTED STEPS : ");
00606 tmp.addItem( LOG_NUMBER_OF_INTEGRATOR_FUNCTION_EVALUATIONS, "NUMBER OF RHS EVALUATIONS : ");
00607 tmp.addItem( LOG_NUMBER_OF_BDF_INTEGRATOR_JACOBIAN_EVALUATIONS,"NUMBER OF JACOBIAN EVALUATIONS : ");
00608 tmp.addItem( LOG_TIME_INTEGRATOR_FUNCTION_EVALUATIONS, "TIME FOR RHS EVALUATIONS [sec]: ");
00609 tmp.addItem( LOG_TIME_BDF_INTEGRATOR_JACOBIAN_EVALUATION, "TIME FOR JACOBIAN EVALUATIONS [sec]: ");
00610 tmp.addItem( LOG_TIME_BDF_INTEGRATOR_JACOBIAN_DECOMPOSITION, "TIME FOR JACOBIAN DECOMPOSITIONS [sec]: ");
00611
00612 outputLoggingIdx = addLogRecord( tmp );
00613
00614 return SUCCESSFUL_RETURN;
00615 }
00616
00617
00618 int Integrator::getDimX() const{
00619
00620 return getDim();
00621 }
00622
00623
00624 returnValue Integrator::printRunTimeProfile() const{
00625
00626 return printLogRecord(cout, outputLoggingIdx, PRINT_LAST_ITER);
00627 }
00628
00629
00630 CLOSE_NAMESPACE_ACADO
00631
00632