model_data.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00033 #include <acado/ocp/model_data.hpp>
00034 
00035 
00036 BEGIN_NAMESPACE_ACADO
00037 
00038 
00039 
00040 //
00041 // PUBLIC MEMBER FUNCTIONS:
00042 //
00043 
00044 ModelData::ModelData() {
00045 
00046         NX1 = 0;
00047         NX2 = 0;
00048         NX3 = 0;
00049         NDX = 0;
00050         NDX3 = 0;
00051         NXA = 0;
00052         NXA3 = 0;
00053         NU = 0;
00054         NP = 0;
00055         N  = 0;
00056         NOD = 0;
00057         model_dimensions_set = BT_FALSE;
00058         export_rhs = BT_TRUE;
00059         delay = 1;
00060 }
00061 
00062 
00063 returnValue ModelData::setDimensions( uint _NX1, uint _NX2, uint _NX3, uint _NDX, uint _NDX3, uint _NXA, uint _NXA3, uint _NU, uint _NOD, uint _NP )
00064 {
00065         NX1 = _NX1;
00066         NX2 = _NX2;
00067         NX3 = _NX3;
00068         NDX = _NDX;
00069         NDX3 = _NDX3;
00070         NXA = _NXA;
00071         NXA3 = _NXA3;
00072         NU = _NU;
00073         NP = _NP;
00074         NOD = _NOD;
00075         model_dimensions_set = BT_TRUE;
00076         return SUCCESSFUL_RETURN;
00077 }
00078 
00079 
00080 uint ModelData::addOutput( const OutputFcn& outputEquation_, const Grid& grid ){
00081 
00082         if( rhs_name.empty() && outputNames.size() == 0 ) {
00083                 Expression next;
00084                 outputEquation_.getExpression( next );
00085                 outputExpressions.push_back( next );
00086                 dim_outputs.push_back( next.getDim() );
00087 
00088                 if( NDX == 0 ) NDX = outputEquation_.getNDX();
00089                 if( NU == 0 ) NU = outputEquation_.getNU();
00090                 if( NP == 0 ) NP = outputEquation_.getNP();
00091                 if( NOD == 0 ) NOD = outputEquation_.getNOD();
00092 
00093                 outputGrids.push_back( grid );
00094 
00095                 uint numOuts = (int) ceil((double)grid.getNumIntervals());
00096                 num_meas.push_back( numOuts );
00097         }
00098         else {
00099                 return ACADOERROR( RET_INVALID_OPTION );
00100         }
00101 
00102         return dim_outputs.size();
00103 }
00104 
00105 
00106 uint ModelData::addOutput( const std::string& output, const std::string& diffs_output, const uint dim, const Grid& grid ){
00107 
00108         if( outputExpressions.size() == 0 && differentialEquation.getNumDynamicEquations() == 0 ) {
00109                 outputNames.push_back( output );
00110                 diffs_outputNames.push_back( diffs_output );
00111                 dim_outputs.push_back( dim );
00112 
00113                 outputGrids.push_back( grid );
00114 
00115                 uint numOuts = (int) ceil((double)grid.getNumIntervals());
00116                 num_meas.push_back( numOuts );
00117         }
00118         else {
00119                 return ACADOERROR( RET_INVALID_OPTION );
00120         }
00121 
00122         return dim_outputs.size();
00123 }
00124 
00125 
00126 uint ModelData::addOutput(      const std::string& output, const std::string& diffs_output, const uint dim,
00127                                                         const Grid& grid, const std::string& colInd, const std::string& rowPtr  ){
00128 
00129 
00130         DVector colIndV, rowPtrV;
00131 
00132         colIndV.read( colInd.c_str() );
00133         rowPtrV.read( rowPtr.c_str() );
00134 
00135         if( rowPtrV.getDim() != (dim+1) ) {
00136                 return ACADOERROR( RET_INVALID_OPTION );
00137         }
00138         colInd_outputs.push_back( colIndV );
00139         rowPtr_outputs.push_back( rowPtrV );
00140 
00141     return addOutput( output, diffs_output, dim, grid );
00142 }
00143 
00144 
00145 BooleanType ModelData::hasOutputs() const{
00146 
00147         if( outputExpressions.size() == 0 && outputNames.size() == 0 ) return BT_FALSE;
00148         return BT_TRUE;
00149 }
00150 
00151 
00152 returnValue ModelData::getNumSteps( DVector& _numSteps ) const {
00153 
00154     _numSteps = numSteps;
00155     return SUCCESSFUL_RETURN;
00156 }
00157 
00158 
00159 returnValue ModelData::setNumSteps( const DVector& _numSteps ) {
00160 
00161     numSteps = _numSteps;
00162     return SUCCESSFUL_RETURN;
00163 }
00164 
00165 
00166 returnValue ModelData::getOutputExpressions( std::vector<Expression>& outputExpressions_ ) const {
00167 
00168     outputExpressions_ = outputExpressions;
00169     return SUCCESSFUL_RETURN;
00170 }
00171 
00172 
00173 std::vector<DMatrix> ModelData::getOutputDependencies( ) const {
00174         std::vector<DMatrix> outputDependencies;
00175         if( hasCompressedStorage() ) {
00176                 for( uint i = 0; i < outputNames.size(); i++ ) {
00177                         DVector colIndV = colInd_outputs[i];
00178                         DVector rowPtrV = rowPtr_outputs[i];
00179 
00180                         DMatrix dependencyMat = zeros<double>( dim_outputs[i],getNX()+NXA+NU+NDX );
00181                         int index = 1;
00182                         for( uint j = 0; j < dim_outputs[i]; j++ ) {
00183                                 uint upper = (uint)rowPtrV(j+1);
00184                                 for( uint k = (uint)rowPtrV(j); k < upper; k++ ) {
00185                                         dependencyMat(j,(uint)colIndV(k-1)-1) = index++;
00186                                 }
00187                         }
00188 
00189                         outputDependencies.push_back( dependencyMat );
00190                 }
00191         }
00192         return outputDependencies;
00193 }
00194 
00195 
00196 returnValue ModelData::getOutputGrids( std::vector<Grid>& outputGrids_ ) const {
00197 
00198     outputGrids_ = outputGrids;
00199     return SUCCESSFUL_RETURN;
00200 }
00201 
00202 
00203 returnValue ModelData::getModel( DifferentialEquation& _f ) const{
00204 
00205     _f = differentialEquation;
00206     return SUCCESSFUL_RETURN;
00207 }
00208 
00209 
00210 returnValue ModelData::getNARXmodel( uint& _delay, DMatrix& _parms ) const{
00211 
00212     _delay = delay;
00213     _parms = parms;
00214 
00215     return SUCCESSFUL_RETURN;
00216 }
00217 
00218 
00219 returnValue ModelData::getLinearInput( DMatrix& M1_, DMatrix& A1_, DMatrix& B1_ ) const {
00220         M1_ = M1;
00221         A1_ = A1;
00222         B1_ = B1;
00223 
00224         return SUCCESSFUL_RETURN;
00225 }
00226 
00227 
00228 returnValue ModelData::getLinearOutput( DMatrix& M3_, DMatrix& A3_, OutputFcn& rhs_ ) const {
00229         M3_ = M3;
00230         A3_ = A3;
00231         rhs_ = rhs3;
00232 
00233         return SUCCESSFUL_RETURN;
00234 }
00235 
00236 
00237 returnValue ModelData::getLinearOutput( DMatrix& M3_, DMatrix& A3_ ) const {
00238         M3_ = M3;
00239         A3_ = A3;
00240 
00241         return SUCCESSFUL_RETURN;
00242 }
00243 
00244 
00245 returnValue ModelData::setModel( const DifferentialEquation& _f )
00246 {
00247         if( rhs_name.empty() && NX2 == 0 ) {
00248                 differentialEquation = _f;
00249                 Expression rhs;
00250                 differentialEquation.getExpression( rhs );
00251 
00252                 NXA = differentialEquation.getNXA();
00253                 NX2 = rhs.getDim() - NXA;
00254                 if( NDX == 0 ) NDX = _f.getNDX();
00255 //              if( _f.getNDX() > 0 && _f.getNDX() != (int)NX2 ) { // TODO: this test returns an error for well-defined models when using a linear input subsystem!
00256 //                      std::cout << "nonlinear model of size " << NX2 << " depends on " << _f.getNDX() << " differential state derivatives!" << std::endl;
00257 //                      return ACADOERROR( RET_INVALID_OPTION );
00258 //              }
00259                 if( NU == 0 ) NU = _f.getNU();
00260                 if( NP == 0 ) NP = _f.getNP();
00261                 if( NOD == 0 ) NOD = _f.getNOD();
00262 
00263                 export_rhs = BT_TRUE;
00264         }
00265         else {
00266                 return ACADOERROR( RET_INVALID_OPTION );
00267         }
00268         return SUCCESSFUL_RETURN;
00269 }
00270 
00271 
00272 returnValue ModelData::setNARXmodel( const uint _delay, const DMatrix& _parms ) {
00273 
00274         if( rhs_name.empty() && NX2 == 0 && NX3 == 0 ) {
00275                 NX2 = _parms.getNumRows();
00276                 delay = _delay;
00277                 uint numParms = 1;
00278                 uint n = delay*getNX();
00279                 if( delay >= 1 ) {
00280                         numParms = numParms + n;
00281                 }
00282                 for( uint i = 1; i < delay; i++ ) {
00283                         numParms = numParms + (n+1)*(uint)pow((double)n,(int)i)/2;
00284                 }
00285                 if( _parms.getNumCols() == numParms ) {
00286                         parms = _parms;
00287                 }
00288                 else {
00289                         return ACADOERROR( RET_UNABLE_TO_EXPORT_CODE );
00290                 }
00291 
00292                 export_rhs = BT_TRUE;
00293         }
00294         else {
00295                 return ACADOERROR( RET_INVALID_OPTION );
00296         }
00297         return SUCCESSFUL_RETURN;
00298 }
00299 
00300 
00301 returnValue ModelData::setLinearInput( const DMatrix& M1_, const DMatrix& A1_, const DMatrix& B1_ )
00302 {
00303         M1 = M1_;
00304         A1 = A1_;
00305         B1 = B1_;
00306         NX1 = A1.getNumCols();
00307         NU = B1.getNumCols();
00308         export_rhs = BT_TRUE;
00309 
00310         return SUCCESSFUL_RETURN;
00311 }
00312 
00313 
00314 returnValue ModelData::setLinearOutput( const DMatrix& M3_, const DMatrix& A3_, const OutputFcn& rhs3_ )
00315 {
00316         M3 = M3_;
00317         A3 = A3_;
00318         NX3 = A3.getNumCols();
00319 
00320         rhs3 = rhs3_;
00321         if( NDX == 0 ) NDX = rhs3_.getNDX();
00322         if( NU == 0 ) NU = rhs3_.getNU();
00323         if( NP == 0 ) NP = rhs3_.getNP();
00324         if( NOD == 0 ) NOD = rhs3_.getNOD();
00325 
00326         export_rhs = BT_TRUE;
00327 
00328         return SUCCESSFUL_RETURN;
00329 }
00330 
00331 
00332 returnValue ModelData::setLinearOutput( const DMatrix& M3_, const DMatrix& A3_, const std::string& rhs3_, const std::string& diffs3_ )
00333 {
00334         if( !export_rhs ) {
00335                 M3 = M3_;
00336                 A3 = A3_;
00337                 NX3 = A3.getNumCols();
00338 
00339                 rhs3_name = std::string(rhs3_);
00340                 diffs3_name = std::string(diffs3_);
00341         }
00342         else {
00343                 return ACADOERROR( RET_INVALID_OPTION );
00344         }
00345 
00346         return SUCCESSFUL_RETURN;
00347 }
00348 
00349 
00350 returnValue ModelData::setModel( const std::string& fileName, const std::string& _rhs_ODE, const std::string& _diffs_ODE )
00351 {
00352         if( differentialEquation.getNumDynamicEquations() == 0 )
00353         {
00354                 externModel = fileName;
00355                 rhs_name = _rhs_ODE;
00356                 diffs_name = _diffs_ODE;
00357 
00358                 export_rhs = BT_FALSE;
00359         }
00360         else
00361         {
00362                 return ACADOERROR( RET_INVALID_OPTION );
00363         }
00364 
00365         return SUCCESSFUL_RETURN;
00366 }
00367 
00368 
00369 returnValue ModelData::getIntegrationGrid( Grid& integrationGrid_ ) const
00370 {
00371         integrationGrid_ = integrationGrid;
00372         return SUCCESSFUL_RETURN;
00373 }
00374 
00375 
00376 returnValue ModelData::setIntegrationGrid(      const Grid& _ocpGrid, const uint _numSteps
00377                                                                                 )
00378 {
00379         uint i;
00380         N = _ocpGrid.getNumIntervals();
00381         BooleanType equidistantControl = _ocpGrid.isEquidistant();
00382         double T = _ocpGrid.getLastTime() - _ocpGrid.getFirstTime();
00383         double h = T/((double)_numSteps);
00384         DVector stepsVector( N );
00385 
00386         if (integrationGrid.isEmpty() == BT_TRUE)
00387         {
00388                 for( i = 0; i < stepsVector.getDim(); i++ )
00389                 {
00390                         stepsVector(i) = (int) acadoRound((_ocpGrid.getTime(i+1)-_ocpGrid.getTime(i))/h);
00391                 }
00392 
00393                 if( equidistantControl )
00394                 {
00395                         // Setup fixed integrator grid for equidistant control grid
00396                         integrationGrid = Grid( 0.0, ((double) T)/((double) N), (int) ceil((double)_numSteps/((double) N) - 10.0*EPS) + 1 );
00397                 }
00398                 else
00399                 {
00400                         // Setup for non equidistant control grid
00401                         // NOTE: This grid defines only one integration step because the control
00402                         // grid is non equidistant.
00403                         integrationGrid = Grid( 0.0, h, 2 );
00404                         numSteps = stepsVector;
00405                 }
00406         }
00407         return SUCCESSFUL_RETURN;
00408 }
00409 
00410 
00411 returnValue ModelData::clearIntegrationGrid( )
00412 {
00413         integrationGrid = Grid();
00414 
00415         return SUCCESSFUL_RETURN;
00416 }
00417 
00418 
00419 BooleanType ModelData::hasEquidistantControlGrid(  ) const
00420 {
00421         return numSteps.isEmpty();
00422 }
00423 
00424 
00425 BooleanType ModelData::hasOutputFunctions() const {
00426 
00427     if( outputExpressions.size() == 0 ) return BT_FALSE;
00428     return BT_TRUE;
00429 }
00430 
00431 
00432 BooleanType ModelData::hasDifferentialEquation() const {
00433 
00434     if( differentialEquation.getDim() == 0 ) return BT_FALSE;
00435     return BT_TRUE;
00436 }
00437 
00438 
00439 BooleanType ModelData::modelDimensionsSet() const {
00440 
00441     return model_dimensions_set;
00442 }
00443 
00444 
00445 BooleanType ModelData::exportRhs() const {
00446 
00447     return export_rhs;
00448 }
00449 
00450 
00451 BooleanType ModelData::hasCompressedStorage() const {
00452 
00453         if( colInd_outputs.size() == 0 ) return BT_FALSE;
00454     return BT_TRUE;
00455 }
00456 
00457 
00458 uint ModelData::getNX( ) const
00459 {
00460         if( parms.isEmpty() ) {
00461                 return NX1+NX2+NX3;
00462         }
00463         else {
00464                 return delay*(NX1+NX2)+NX3;             // IMPORTANT for NARX models where the state space is increased because of the delay
00465         }
00466 }
00467 
00468 
00469 uint ModelData::getNX1( ) const
00470 {
00471         return NX1;
00472 }
00473 
00474 
00475 uint ModelData::getNX2( ) const
00476 {
00477         return NX2;
00478 }
00479 
00480 
00481 uint ModelData::getNX3( ) const
00482 {
00483         return NX3;
00484 }
00485 
00486 
00487 uint ModelData::getNDX( ) const
00488 {
00489         if( NDX > 0 ) {
00490                 return getNX();
00491         }
00492         return NDX;
00493 }
00494 
00495 
00496 uint ModelData::getNDX3( ) const
00497 {
00498         if( NDX3 > 0 ) {
00499                 return NX1+NX2;
00500         }
00501         return NDX3;
00502 }
00503 
00504 
00505 uint ModelData::getNXA( ) const
00506 {
00507         return NXA;
00508 }
00509 
00510 
00511 uint ModelData::getNXA3( ) const
00512 {
00513         if( NXA3 > 0 ) {
00514                 return NXA;
00515         }
00516         return NXA3;
00517 }
00518 
00519 
00520 uint ModelData::getNU( ) const
00521 {
00522         return NU;
00523 }
00524 
00525 
00526 uint ModelData::getNP( ) const
00527 {
00528         return NP;
00529 }
00530 
00531 uint ModelData::getNOD( ) const
00532 {
00533         return NOD;
00534 }
00535 
00536 
00537 uint ModelData::getN( ) const
00538 {
00539         return N;
00540 }
00541 
00542 
00543 returnValue ModelData::setN( const uint N_ )
00544 {
00545         N = N_;
00546         return SUCCESSFUL_RETURN;
00547 }
00548 
00549 
00550 DVector ModelData::getDimOutputs( ) const
00551 {
00552         DVector nOutV( (uint)dim_outputs.size() );
00553         for( uint i = 0; i < dim_outputs.size(); i++ ) {
00554                 nOutV(i) = dim_outputs[i];
00555         }
00556         return nOutV;
00557 }
00558 
00559 
00560 uint ModelData::getNumOutputs( ) const
00561 {
00562         return (uint)outputGrids.size();
00563 }
00564 
00565 
00566 returnValue ModelData::getDimOutputs( std::vector<uint>& dims ) const
00567 {
00568         dims = dim_outputs;
00569         return SUCCESSFUL_RETURN;
00570 }
00571 
00572 
00573 DVector ModelData::getNumMeas( ) const
00574 {
00575         DVector nMeasV( (uint)num_meas.size() );
00576         for( uint i = 0; i < num_meas.size(); i++ ) {
00577                 nMeasV(i) = num_meas[i];
00578         }
00579         return nMeasV;
00580 }
00581 
00582 
00583 const std::string ModelData::getFileNameModel() const {
00584         return externModel;
00585 }
00586 
00587 
00588 const std::string ModelData::getNameRhs() const {
00589         return rhs_name;
00590 }
00591 
00592 
00593 const std::string ModelData::getNameDiffsRhs() const {
00594         return diffs_name;
00595 }
00596 
00597 
00598 const std::string ModelData::getNameOutput() const {
00599         return rhs3_name;
00600 }
00601 
00602 
00603 const std::string ModelData::getNameDiffsOutput() const {
00604         return diffs3_name;
00605 }
00606 
00607 
00608 returnValue ModelData::getNameOutputs( std::vector<std::string>& names ) const {
00609         names = outputNames;
00610         return SUCCESSFUL_RETURN;
00611 }
00612 
00613 
00614 returnValue ModelData::getNameDiffsOutputs( std::vector<std::string>& names ) const {
00615         names = diffs_outputNames;
00616         return SUCCESSFUL_RETURN;
00617 }
00618 
00619 
00620 
00621 // PROTECTED:
00622 
00623 
00624 
00625 
00626 CLOSE_NAMESPACE_ACADO
00627 
00628 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:14