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/erk_export.hpp>
00035
00036 using namespace std;
00037
00038 BEGIN_NAMESPACE_ACADO
00039
00040
00041
00042
00043
00044 ExplicitRungeKuttaExport::ExplicitRungeKuttaExport( UserInteraction* _userInteraction,
00045 const std::string& _commonHeaderName
00046 ) : RungeKuttaExport( _userInteraction,_commonHeaderName )
00047 {
00048 is_symmetric = BT_FALSE;
00049 }
00050
00051
00052 ExplicitRungeKuttaExport::ExplicitRungeKuttaExport( const ExplicitRungeKuttaExport& arg
00053 ) : RungeKuttaExport( arg )
00054 {
00055 copy( arg );
00056 }
00057
00058
00059 ExplicitRungeKuttaExport::~ExplicitRungeKuttaExport( )
00060 {
00061 clear( );
00062 }
00063
00064
00065 returnValue ExplicitRungeKuttaExport::setup( )
00066 {
00067 int sensGen;
00068 get( DYNAMIC_SENSITIVITY,sensGen );
00069 if ( (ExportSensitivityType)sensGen != FORWARD && (ExportSensitivityType)sensGen != NO_SENSITIVITY ) ACADOERROR( RET_INVALID_OPTION );
00070
00071 bool DERIVATIVES = ((ExportSensitivityType)sensGen != NO_SENSITIVITY);
00072
00073 LOG( LVL_DEBUG ) << "Preparing to export ExplicitRungeKuttaExport... " << endl;
00074
00075
00076 uint rhsDim = NX*(NX+NU+1);
00077 if( !DERIVATIVES ) rhsDim = NX;
00078 inputDim = NX*(NX+NU+1) + NU + NOD;
00079 if( !DERIVATIVES ) inputDim = NX + NU + NOD;
00080 const uint rkOrder = getNumStages();
00081
00082 double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
00083
00084 ExportVariable Ah ( "A*h", DMatrix( AA )*=h );
00085 ExportVariable b4h( "b4*h", DMatrix( bb )*=h );
00086
00087 rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, true );
00088 rk_eta = ExportVariable( "rk_eta", 1, inputDim );
00089
00090 int useOMP;
00091 get(CG_USE_OPENMP, useOMP);
00092 ExportStruct structWspace;
00093 structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
00094
00095 rk_ttt.setup( "rk_ttt", 1, 1, REAL, structWspace, true );
00096 uint timeDep = 0;
00097 if( timeDependant ) timeDep = 1;
00098
00099 rk_xxx.setup("rk_xxx", 1, inputDim+timeDep, REAL, structWspace);
00100 rk_kkk.setup("rk_kkk", rkOrder, rhsDim, REAL, structWspace);
00101
00102 if ( useOMP )
00103 {
00104 ExportVariable auxVar;
00105
00106 auxVar = getAuxVariable();
00107 auxVar.setName( "odeAuxVar" );
00108 auxVar.setDataStruct( ACADO_LOCAL );
00109 rhs.setGlobalExportVariable( auxVar );
00110 diffs_rhs.setGlobalExportVariable( auxVar );
00111 }
00112
00113 ExportIndex run( "run1" );
00114
00115
00116 if( equidistantControlGrid() ) {
00117 integrate = ExportFunction( "integrate", rk_eta, reset_int );
00118 }
00119 else {
00120 integrate = ExportFunction( "integrate", rk_eta, reset_int, rk_index );
00121 }
00122 integrate.setReturnValue( error_code );
00123 rk_eta.setDoc( "Working array to pass the input values and return the results." );
00124 reset_int.setDoc( "The internal memory of the integrator can be reset." );
00125 rk_index.setDoc( "Number of the shooting interval." );
00126 error_code.setDoc( "Status code of the integrator." );
00127 integrate.doc( "Performs the integration and sensitivity propagation for one shooting interval." );
00128 integrate.addIndex( run );
00129
00130 ExportVariable numInt( "numInts", 1, 1, INT );
00131 if( !equidistantControlGrid() ) {
00132 integrate.addStatement( std::string( "int numSteps[" ) + toString( numSteps.getDim() ) + "] = {" + toString( numSteps(0) ) );
00133 uint i;
00134 for( i = 1; i < numSteps.getDim(); i++ ) {
00135 integrate.addStatement( std::string( ", " ) + toString( numSteps(i) ) );
00136 }
00137 integrate.addStatement( std::string( "};\n" ) );
00138 integrate.addStatement( std::string( "int " ) + numInt.getName() + " = numSteps[" + rk_index.getName() + "];\n" );
00139 }
00140
00141 integrate.addStatement( rk_ttt == DMatrix(grid.getFirstTime()) );
00142
00143 if( DERIVATIVES ) {
00144
00145 DMatrix idX = eye<double>( NX );
00146 DMatrix zeroXU = zeros<double>( NX,NU );
00147 integrate.addStatement( rk_eta.getCols( NX,NX*(1+NX) ) == idX.makeVector().transpose() );
00148 integrate.addStatement( rk_eta.getCols( NX*(1+NX),NX*(1+NX+NU) ) == zeroXU.makeVector().transpose() );
00149 }
00150
00151 if( inputDim > rhsDim ) {
00152 integrate.addStatement( rk_xxx.getCols( rhsDim,inputDim ) == rk_eta.getCols( rhsDim,inputDim ) );
00153 }
00154 integrate.addLinebreak( );
00155
00156
00157 ExportForLoop loop;
00158 if( equidistantControlGrid() ) {
00159 loop = ExportForLoop( run, 0, grid.getNumIntervals() );
00160 }
00161 else {
00162 loop = ExportForLoop( run, 0, 1 );
00163 loop.addStatement( std::string("for(") + run.getName() + " = 0; " + run.getName() + " < " + numInt.getName() + "; " + run.getName() + "++ ) {\n" );
00164 }
00165
00166 for( uint run1 = 0; run1 < rkOrder; run1++ )
00167 {
00168 loop.addStatement( rk_xxx.getCols( 0,rhsDim ) == rk_eta.getCols( 0,rhsDim ) + Ah.getRow(run1)*rk_kkk );
00169 if( timeDependant ) loop.addStatement( rk_xxx.getCol( inputDim ) == rk_ttt + ((double)cc(run1))/grid.getNumIntervals() );
00170 loop.addFunctionCall( getNameDiffsRHS(),rk_xxx,rk_kkk.getAddress(run1,0) );
00171 }
00172 loop.addStatement( rk_eta.getCols( 0,rhsDim ) += b4h^rk_kkk );
00173 loop.addStatement( rk_ttt += DMatrix(1.0/grid.getNumIntervals()) );
00174
00175
00176 if( !equidistantControlGrid() ) {
00177 loop.addStatement( "}\n" );
00178
00179 }
00180 integrate.addStatement( loop );
00181
00182 integrate.addStatement( error_code == 0 );
00183
00184 LOG( LVL_DEBUG ) << "done" << endl;
00185
00186 return SUCCESSFUL_RETURN;
00187 }
00188
00189
00190 returnValue ExplicitRungeKuttaExport::setDifferentialEquation( const Expression& rhs_ )
00191 {
00192 int sensGen;
00193 get( DYNAMIC_SENSITIVITY,sensGen );
00194
00195 OnlineData dummy0;
00196 Control dummy1;
00197 DifferentialState dummy2;
00198 AlgebraicState dummy3;
00199 DifferentialStateDerivative dummy4;
00200 dummy0.clearStaticCounters();
00201 dummy1.clearStaticCounters();
00202 dummy2.clearStaticCounters();
00203 dummy3.clearStaticCounters();
00204 dummy4.clearStaticCounters();
00205
00206 x = DifferentialState("", NX, 1);
00207 dx = DifferentialStateDerivative("", NDX, 1);
00208 z = AlgebraicState("", NXA, 1);
00209 u = Control("", NU, 1);
00210 od = OnlineData("", NOD, 1);
00211
00212 if( NDX > 0 && NDX != NX ) {
00213 return ACADOERROR( RET_INVALID_OPTION );
00214 }
00215 if( rhs_.getNumRows() != (NX+NXA) ) {
00216 return ACADOERROR( RET_INVALID_OPTION );
00217 }
00218
00219 DifferentialEquation f, f_ODE;
00220
00221 f_ODE << rhs_;
00222 if( f_ODE.getNDX() > 0 ) {
00223 return ACADOERROR( RET_INVALID_OPTION );
00224 }
00225
00226 if( (ExportSensitivityType)sensGen == FORWARD ) {
00227 DifferentialState Gx("", NX,NX), Gu("", NX,NU);
00228
00229
00230
00231 f << rhs_;
00232
00233
00234
00235
00236 f << multipleForwardDerivative( rhs_, x, Gx );
00237
00238
00239
00240
00241
00242 f << multipleForwardDerivative( rhs_, x, Gu ) + forwardDerivative( rhs_, u );
00243
00244
00245
00246
00247
00248
00249 }
00250 else if( (ExportSensitivityType)sensGen != NO_SENSITIVITY ) {
00251 return ACADOERROR( RET_INVALID_OPTION );
00252 }
00253 if( f.getNT() > 0 ) timeDependant = true;
00254
00255 int matlabInterface;
00256 userInteraction->get(GENERATE_MATLAB_INTERFACE, matlabInterface);
00257 if( matlabInterface && (ExportSensitivityType)sensGen == FORWARD ) {
00258 return rhs.init(f_ODE, "acado_rhs", NX, 0, NU, NP, NDX, NOD)
00259 & diffs_rhs.init(f, "acado_rhs_ext", NX * (1 + NX + NU), 0, NU, NP, NDX, NOD);
00260 }
00261 else if( (ExportSensitivityType)sensGen == FORWARD ) {
00262 return diffs_rhs.init(f, "acado_rhs_forw", NX * (1 + NX + NU), 0, NU, NP, NDX, NOD);
00263 }
00264 else {
00265 return diffs_rhs.init(f_ODE, "acado_rhs", NX, 0, NU, NP, NDX, NOD);
00266 }
00267
00268 return SUCCESSFUL_RETURN;
00269 }
00270
00271
00272 returnValue ExplicitRungeKuttaExport::setLinearInput( const DMatrix& M1, const DMatrix& A1, const DMatrix& B1 ) {
00273
00274 return ACADOERROR( RET_INVALID_OPTION );
00275 }
00276
00277
00278 returnValue ExplicitRungeKuttaExport::setLinearOutput( const DMatrix& M3, const DMatrix& A3, const Expression& _rhs ) {
00279
00280 return ACADOERROR( RET_INVALID_OPTION );
00281 }
00282
00283
00284 returnValue ExplicitRungeKuttaExport::setLinearOutput( const DMatrix& M3, const DMatrix& A3, const std::string& _rhs3, const std::string& _diffs_rhs3 )
00285 {
00286 return RET_INVALID_OPTION;
00287 }
00288
00289
00290 returnValue ExplicitRungeKuttaExport::getDataDeclarations( ExportStatementBlock& declarations,
00291 ExportStruct dataStruct
00292 ) const
00293 {
00294 if( exportRhs ) {
00295 declarations.addDeclaration( getAuxVariable(),dataStruct );
00296 }
00297 declarations.addDeclaration( rk_ttt,dataStruct );
00298 declarations.addDeclaration( rk_xxx,dataStruct );
00299 declarations.addDeclaration( rk_kkk,dataStruct );
00300
00301
00302
00303 return SUCCESSFUL_RETURN;
00304 }
00305
00306
00307 returnValue ExplicitRungeKuttaExport::getFunctionDeclarations( ExportStatementBlock& declarations
00308 ) const
00309 {
00310 declarations.addDeclaration( integrate );
00311
00312 int matlabInterface;
00313 userInteraction->get( GENERATE_MATLAB_INTERFACE, matlabInterface );
00314 if (matlabInterface) {
00315 declarations.addDeclaration( rhs );
00316 }
00317
00318 return SUCCESSFUL_RETURN;
00319 }
00320
00321
00322 returnValue ExplicitRungeKuttaExport::getCode( ExportStatementBlock& code
00323 )
00324 {
00325
00326
00327
00328
00329
00330
00331 int useOMP;
00332 get(CG_USE_OPENMP, useOMP);
00333 if ( useOMP )
00334 {
00335 getDataDeclarations( code, ACADO_LOCAL );
00336
00337 code << "#pragma omp threadprivate( "
00338 << getAuxVariable().getFullName() << ", "
00339 << rk_xxx.getFullName() << ", "
00340 << rk_ttt.getFullName() << ", "
00341 << rk_kkk.getFullName()
00342 << " )\n\n";
00343 }
00344
00345 int sensGen;
00346 get( DYNAMIC_SENSITIVITY,sensGen );
00347 if( exportRhs ) {
00348 code.addFunction( rhs );
00349 code.addFunction( diffs_rhs );
00350 }
00351
00352 double h = (grid.getLastTime() - grid.getFirstTime())/grid.getNumIntervals();
00353 code.addComment(std::string("Fixed step size:") + toString(h));
00354 code.addFunction( integrate );
00355
00356
00357
00358
00359
00360 return SUCCESSFUL_RETURN;
00361 }
00362
00363
00364 returnValue ExplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_, const std::vector<Expression> _rhs ) {
00365
00366 return ACADOERROR( RET_INVALID_OPTION );
00367 }
00368
00369
00370 returnValue ExplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_,
00371 const std::vector<std::string> _outputNames,
00372 const std::vector<std::string> _diffs_outputNames,
00373 const std::vector<uint> _dims_output ) {
00374
00375 return ACADOERROR( RET_INVALID_OPTION );
00376 }
00377
00378
00379 returnValue ExplicitRungeKuttaExport::setupOutput( const std::vector<Grid> outputGrids_,
00380 const std::vector<std::string> _outputNames,
00381 const std::vector<std::string> _diffs_outputNames,
00382 const std::vector<uint> _dims_output,
00383 const std::vector<DMatrix> _outputDependencies ) {
00384
00385 return ACADOERROR( RET_INVALID_OPTION );
00386 }
00387
00388
00389 ExportVariable ExplicitRungeKuttaExport::getAuxVariable() const
00390 {
00391 ExportVariable max;
00392 max = rhs.getGlobalExportVariable();
00393 if( diffs_rhs.getGlobalExportVariable().getDim() > max.getDim() ) {
00394 max = diffs_rhs.getGlobalExportVariable();
00395 }
00396 return max;
00397 }
00398
00399
00400
00401
00402
00403
00404
00405 CLOSE_NAMESPACE_ACADO
00406
00407