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
00035 #include <acado/utils/acado_utils.hpp>
00036 #include <acado/symbolic_expression/symbolic_expression.hpp>
00037 #include <acado/function/evaluation_point.hpp>
00038 #include <acado/function/function_.hpp>
00039
00040
00041
00042 BEGIN_NAMESPACE_ACADO
00043
00044
00045
00046
00047
00048
00049 Function::Function(){
00050
00051 memoryOffset = 0;
00052 result = 0;
00053 }
00054
00055
00056 Function::Function( const Function& arg ){
00057
00058 evaluationTree = arg.evaluationTree;
00059 memoryOffset = arg.memoryOffset ;
00060
00061 if ( arg.getDim() != 0 )
00062 {
00063 result = (double*) calloc( arg.getDim(),sizeof(double) );
00064 for( int i=0; i<getDim(); ++i )
00065 result[i] = arg.result[i];
00066 }
00067 else
00068 result = 0;
00069 }
00070
00071
00072 Function::~Function( ){
00073 if ( result != 0 )
00074 free( result );
00075 }
00076
00077
00078 Function& Function::operator=( const Function& arg ){
00079
00080 if ( this != &arg ){
00081
00082 if ( result != 0 )
00083 free( result );
00084
00085 evaluationTree = arg.evaluationTree;
00086 memoryOffset = arg.memoryOffset ;
00087
00088 if ( arg.getDim() != 0 )
00089 {
00090 result = (double*) calloc( arg.getDim(),sizeof(double) );
00091 for( int i=0; i<getDim(); ++i )
00092 result[i] = arg.result[i];
00093 }
00094 else
00095 result = 0;
00096 }
00097
00098 return *this;
00099 }
00100
00101
00102 Function& Function::operator<<( const Expression& arg ){
00103
00104 evaluationTree.operator<<( arg );
00105
00106 result = (double*) realloc( result,getDim()*sizeof(double) );
00107
00108 return *this;
00109 }
00110
00111
00112 Function& Function::operator<<( const double &arg ){
00113
00114 Expression tmp;
00115 tmp = arg;
00116
00117 return operator<<(tmp);
00118 }
00119
00120
00121 Function& Function::operator<<( const DVector& arg ){
00122
00123 Expression tmp;
00124 tmp = arg;
00125
00126 return operator<<(tmp);
00127 }
00128
00129
00130 Function& Function::operator<<( const DMatrix& arg ){
00131
00132 Expression tmp;
00133 tmp = arg;
00134
00135 return operator<<(tmp);
00136 }
00137
00138
00139 returnValue Function::getExpression( Expression& expression ) const{
00140
00141 return evaluationTree.getExpression( expression );
00142 }
00143
00144
00145 Function Function::operator()( uint idx
00146 ) const
00147 {
00148 Function tmp;
00149
00150 if ( (int)idx >= getDim() )
00151 {
00152 ACADOERROR( RET_INDEX_OUT_OF_BOUNDS );
00153 return tmp;
00154 }
00155
00156 tmp.evaluationTree = evaluationTree;
00157 tmp.memoryOffset = memoryOffset ;
00158
00159 return tmp;
00160 }
00161
00162
00163 returnValue Function::reset( ){
00164
00165 FunctionEvaluationTree tmp;
00166 evaluationTree = tmp;
00167 memoryOffset = 0;
00168
00169 if ( result != 0 )
00170 free( result );
00171
00172 return SUCCESSFUL_RETURN;
00173 }
00174
00175
00176 int Function::index( VariableType variableType_, int index_ ) const{
00177
00178 return evaluationTree.index( variableType_, index_ );
00179 }
00180
00181
00182 double Function::scale( VariableType variableType_, int index_ ) const{
00183
00184 return evaluationTree.scale( variableType_, index_ );
00185 }
00186
00187 int Function::getN (VariableType &variableType_) const{
00188 switch(variableType_) {
00189 case VT_DIFFERENTIAL_STATE : return getNX(); break;
00190 case VT_ALGEBRAIC_STATE : return getNXA(); break;
00191 case VT_CONTROL : return getNU(); break;
00192 case VT_INTEGER_CONTROL : return getNUI(); break;
00193 case VT_PARAMETER : return getNP(); break;
00194 case VT_ONLINE_DATA : return getNOD(); break;
00195 case VT_INTEGER_PARAMETER : return getNPI(); break;
00196 case VT_DISTURBANCE : return getNW(); break;
00197 case VT_TIME : return 1; break;
00198 case VT_INTERMEDIATE_STATE : return getN(); break;
00199 case VT_DDIFFERENTIAL_STATE : return getNDX(); break;
00200 case VT_VARIABLE : return getNX()+getNXA()+getNU()+getNUI()+getNP()+getNPI()+getNW()+getNDX(); break;
00201 case VT_OUTPUT : return 0; break;
00202 case VT_UNKNOWN : return 0;
00203 }
00204 return evaluationTree.getNX();
00205 }
00206
00207 int Function::getNX () const{
00208
00209 return evaluationTree.getNX();
00210 }
00211
00212 int Function::getNXA () const{
00213
00214 return evaluationTree.getNXA();
00215 }
00216
00217 int Function::getNDX () const{
00218
00219 return evaluationTree.getNDX();
00220 }
00221
00222 int Function::getNU () const{
00223
00224 return evaluationTree.getNU();
00225 }
00226
00227 int Function::getNUI () const{
00228
00229
00230 return evaluationTree.getNUI();
00231 }
00232
00233 int Function::getNP () const{
00234
00235
00236 return evaluationTree.getNP();
00237 }
00238
00239 int Function::getNPI () const{
00240
00241
00242 return evaluationTree.getNPI();
00243 }
00244
00245 int Function::getNW () const{
00246
00247
00248 return evaluationTree.getNW();
00249 }
00250
00251 int Function::getNT () const{
00252
00253
00254 return evaluationTree.getNT();
00255 }
00256
00257 int Function::getNOD () const{
00258
00259
00260 return evaluationTree.getNOD();
00261 }
00262
00263
00264 int Function::getNumberOfVariables() const{
00265
00266
00267 return evaluationTree.getNumberOfVariables();
00268 }
00269
00270
00271 Operator* Function::getExpression( uint componentIdx ) const{
00272
00273 return evaluationTree.getExpression( componentIdx );
00274 }
00275
00276
00277 returnValue Function::evaluate( int number, double *x, double *_result ){
00278
00279
00280
00281 evaluationTree.evaluate( number+memoryOffset, x, _result );
00282
00283
00284
00285 return SUCCESSFUL_RETURN;
00286 }
00287
00288
00289
00290 returnValue Function::substitute( VariableType variableType_, int index_,
00291 double sub_ ){
00292
00293
00294
00295 if( isSymbolic() == BT_TRUE ){
00296 FunctionEvaluationTree tmp;
00297 tmp = evaluationTree.substitute(variableType_,index_,sub_);
00298 evaluationTree = tmp;
00299 return SUCCESSFUL_RETURN;
00300 }
00301
00302 return ACADOERROR(RET_ONLY_SUPPORTED_FOR_SYMBOLIC_FUNCTIONS);
00303 }
00304
00305
00306
00307 NeutralElement Function::isOneOrZero(){
00308
00309 return evaluationTree.isOneOrZero();
00310 }
00311
00312
00313
00314 BooleanType Function::isDependingOn( const Expression &variable ){
00315
00316 return evaluationTree.isDependingOn( variable );
00317 }
00318
00319 BooleanType Function::isLinearIn( const Expression &variable ){
00320
00321 return evaluationTree.isLinearIn( variable );
00322 }
00323
00324
00325 BooleanType Function::isPolynomialIn( const Expression &variable ){
00326
00327 return evaluationTree.isPolynomialIn( variable );
00328 }
00329
00330
00331 BooleanType Function::isRationalIn( const Expression &variable ){
00332
00333 return evaluationTree.isRationalIn( variable );
00334 }
00335
00336
00337 BooleanType Function::isNondecreasing(){
00338
00339
00340
00341 if( evaluationTree.getMonotonicity() == MT_NONDECREASING ||
00342 evaluationTree.getMonotonicity() == MT_CONSTANT )
00343 return BT_TRUE;
00344
00345 return BT_FALSE;
00346 }
00347
00348
00349 BooleanType Function::isNonincreasing(){
00350
00351
00352
00353 if( evaluationTree.getMonotonicity() == MT_NONINCREASING ||
00354 evaluationTree.getMonotonicity() == MT_CONSTANT )
00355 return BT_TRUE;
00356
00357 return BT_FALSE;
00358 }
00359
00360 BooleanType Function::isConstant()
00361 {
00362 if (evaluationTree.getMonotonicity() == MT_CONSTANT)
00363 return BT_TRUE;
00364 return BT_FALSE;
00365 }
00366
00367
00368 BooleanType Function::isAffine(){
00369
00370
00371
00372 if( isConvex() == BT_TRUE && isConcave() == BT_TRUE )
00373 return BT_TRUE;
00374 return BT_FALSE;
00375 }
00376
00377
00378 BooleanType Function::isConvex(){
00379
00380
00381
00382 if( evaluationTree.getCurvature() == CT_CONVEX ||
00383 evaluationTree.getCurvature() == CT_AFFINE ||
00384 evaluationTree.getCurvature() == CT_CONSTANT )
00385 return BT_TRUE;
00386
00387 return BT_FALSE;
00388 }
00389
00390
00391 BooleanType Function::isConcave(){
00392
00393
00394
00395 if( evaluationTree.getCurvature() == CT_CONCAVE ||
00396 evaluationTree.getCurvature() == CT_AFFINE ||
00397 evaluationTree.getCurvature() == CT_CONSTANT )
00398 return BT_TRUE;
00399
00400 return BT_FALSE;
00401 }
00402
00403 returnValue Function::jacobian(DMatrix &x) {
00404 int n=getDim();
00405 int N=getNumberOfVariables();
00406 returnValue ret;
00407
00408 x=DMatrix(getNX(),n);x.setAll(0);
00409
00410
00411
00412 double *Jr=new double[N];
00413 double *seed=new double[n];
00414 for (int i=0;i<n;i++) seed[i]=0;
00415 for (int i=0;i<n;i++) {
00416 if (i>0) seed[i-1]=0;
00417 seed[i]=1;
00418 for (int j=0;j<N;j++) Jr[j]=0;
00419 ret=AD_backward(0,seed,Jr);
00420 if (ret != SUCCESSFUL_RETURN) return ret;
00421 for (int j=0;j<getNX();j++) x(j,i)=Jr[index(VT_DIFFERENTIAL_STATE,j)];
00422 }
00423 delete[] Jr;
00424 delete[] seed;
00425 return SUCCESSFUL_RETURN;
00426 }
00427
00428
00429 returnValue Function::AD_forward( int number, double *seed, double *df ){
00430
00431 return evaluationTree.AD_forward( number+memoryOffset, seed, df );
00432 }
00433
00434
00435 returnValue Function::AD_backward( int number, double *seed, double *df ){
00436
00437 return evaluationTree.AD_backward( number+memoryOffset, seed, df );
00438 }
00439
00440
00441 returnValue Function::AD_forward2( int number, double *seed, double *dseed,
00442 double *df, double *ddf ){
00443
00444 return evaluationTree.AD_forward2( number+memoryOffset, seed, dseed, df, ddf );
00445 }
00446
00447
00448 returnValue Function::AD_backward2( int number, double *seed1, double *seed2,
00449 double *df, double *ddf ){
00450
00451 return evaluationTree.AD_backward2( number+memoryOffset, seed1, seed2, df, ddf );
00452 }
00453
00454
00455
00456 std::ostream& operator<<(std::ostream& stream, const Function& arg)
00457 {
00458 arg.print( stream );
00459
00460 return stream;
00461 }
00462
00463 returnValue Function::print( std::ostream& stream,
00464 const char *fcnName ,
00465 const char *realString
00466 ) const
00467 {
00468 if (getDim() > 0)
00469 return evaluationTree.C_print(stream, fcnName, realString);
00470
00471 return SUCCESSFUL_RETURN;
00472 }
00473
00474 returnValue Function::exportForwardDeclarations( std::ostream& stream,
00475 const char *fcnName ,
00476 const char *realString
00477 ) const
00478 {
00479 if (getDim() > 0)
00480 return evaluationTree.exportForwardDeclarations(stream, fcnName, realString);
00481
00482 return SUCCESSFUL_RETURN;
00483 }
00484
00485
00486 returnValue Function::exportCode( std::ostream& stream,
00487 const char *fcnName,
00488 const char *realString,
00489 uint _numX,
00490 uint _numXA,
00491 uint _numU,
00492 uint _numP,
00493 uint _numDX,
00494 uint _numOD,
00495 bool allocateMemory,
00496 bool staticMemory
00497 ) const
00498 {
00499 if (getDim() > 0)
00500 return evaluationTree.exportCode(stream, fcnName, realString,
00501 _numX, _numXA, _numU, _numP, _numDX, _numOD, allocateMemory, staticMemory);
00502
00503 return SUCCESSFUL_RETURN;
00504 }
00505
00506
00507 returnValue Function::clearBuffer(){
00508
00509 return evaluationTree.clearBuffer();
00510 }
00511
00512
00513
00514 returnValue Function::setScale( double *scale_ ){
00515
00516 return evaluationTree.setScale(scale_);
00517 }
00518
00519
00520 DVector Function::evaluate( const EvaluationPoint &x,
00521 const int &number ){
00522
00523
00524
00525 evaluate( number, x.getEvaluationPointer(), result );
00526 DVector res( getDim(), result );
00527
00528
00529 return res;
00530 }
00531
00532
00533 DVector Function::AD_forward( const EvaluationPoint &x,
00534 const int &number ){
00535
00536
00537
00538 AD_forward( number, x.getEvaluationPointer(), result );
00539 DVector res( getDim(), result );
00540
00541
00542 return res;
00543 }
00544
00545
00546 returnValue Function::AD_backward( const DVector &seed ,
00547 EvaluationPoint &df ,
00548 const int &number ){
00549
00550
00551 if( ADisSupported() == BT_TRUE ){
00552
00553 int run1;
00554 double *seed_ = new double[getDim()];
00555
00556 for( run1 = 0; run1 < getDim(); run1++ )
00557 seed_[run1] = seed(run1);
00558
00559 AD_backward( number, seed_, df.getEvaluationPointer() );
00560
00561 delete[] seed_;
00562 return SUCCESSFUL_RETURN;
00563 }
00564
00565 uint run1, run2;
00566 double *fseed = new double[getNumberOfVariables()+1];
00567 for( run2 = 0; (int) run2 < getNumberOfVariables()+1; run2++ )
00568 fseed[run2] = 0.0;
00569
00570 double *J;
00571 J = new double[seed.getDim()];
00572
00573 for( run2 = 0; (int) run2 < getNumberOfVariables()+1; run2++ ){
00574
00575 fseed[run2] = 1.0;
00576 AD_forward( 0, fseed, J );
00577 fseed[run2] = 0.0;
00578
00579 df.z[run2] = 0.0;
00580 for( run1 = 0; run1 < seed.getDim(); run1++ )
00581 df.z[run2] += J[run1]*seed(run1);
00582 }
00583
00584 delete[] J ;
00585 delete[] fseed;
00586
00587 return SUCCESSFUL_RETURN;
00588 }
00589
00590 std::string Function::getGlobalExportVariableName( ) const
00591 {
00592 return evaluationTree.getGlobalExportVariableName( );
00593 }
00594
00595 returnValue Function::setGlobalExportVariableName(const std::string& var)
00596 {
00597 return evaluationTree.setGlobalExportVariableName( var );
00598 }
00599
00600 unsigned Function::getGlobalExportVariableSize( ) const
00601 {
00602 return evaluationTree.getGlobalExportVariableSize( );
00603 }
00604
00605
00606 CLOSE_NAMESPACE_ACADO
00607
00608