ocp.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/ocp.hpp>
00034 #include <acado/variables_grid/grid.hpp>
00035 #include <acado/objective/objective.hpp>
00036 
00037 using namespace std;
00038 BEGIN_NAMESPACE_ACADO
00039 
00040 //
00041 // PUBLIC MEMBER FUNCTIONS:
00042 //
00043 
00044 
00045 OCP::OCP(const double &tStart_, const double &tEnd_, const int &N_)
00046         : MultiObjectiveFunctionality(),
00047           grid(new Grid()), objective(new Objective()), constraint(new Constraint())
00048 {
00049 
00050         if (N_ < 0)
00051                 ACADOERROR(RET_INVALID_ARGUMENTS);
00052         setupGrid(tStart_, tEnd_, N_ + 1);
00053 }
00054 
00055 
00056 OCP::OCP( const double &tStart_, const double &tEnd_, const DVector& _numSteps )
00057     : MultiObjectiveFunctionality(),
00058       grid(new Grid()), objective(new Objective()), constraint(new Constraint())
00059 {
00060         if( _numSteps.getDim() <= 0 ) ACADOERROR( RET_INVALID_ARGUMENTS );
00061       
00062         DVector times( _numSteps.getDim()+1 );
00063         times(0) = tStart_;
00064         
00065         double totalSteps = 0;
00066         uint i;
00067         for( i = 0; i < _numSteps.getDim(); i++ ) {
00068                 totalSteps += _numSteps(i);
00069         }
00070         double h = (tEnd_ - tStart_)/totalSteps;
00071         for( i = 0; i < _numSteps.getDim(); i++ ) {
00072                 times(i+1) = times(i) + h*_numSteps(i);
00073         }
00074         
00075     setupGrid( times );
00076     modelData.setIntegrationGrid(*grid, totalSteps);
00077 }
00078 
00079 
00080 OCP::OCP( const Grid &grid_ )
00081     : MultiObjectiveFunctionality(),
00082       grid(new Grid(grid_)), objective(new Objective()), constraint(new Constraint())
00083 {
00084     if( grid->getNumPoints() <= 1 ) ACADOERROR( RET_INVALID_ARGUMENTS );
00085     objective->init ( *grid );
00086     constraint->init( *grid );
00087     setN( grid->getNumIntervals() );
00088 }
00089 
00090 
00091 OCP::OCP( const double    &tStart_,
00092           const Parameter &tEnd_  ,
00093           const int       &N_       )
00094     : MultiObjectiveFunctionality(),
00095       grid(new Grid()), objective(new Objective()), constraint(new Constraint())
00096 {
00097     if( N_ <= 0 ) ACADOERROR( RET_INVALID_ARGUMENTS );
00098     setupGrid( tStart_, tStart_ + 1.0, N_+1);
00099 }
00100 
00101 OCP::~OCP( )
00102 {}
00103 
00104 returnValue OCP::minimizeMayerTerm( const int &multiObjectiveIdx,  const Expression& arg ){
00105 
00106     return MultiObjectiveFunctionality::minimizeMayerTerm( multiObjectiveIdx, arg );
00107 }
00108 
00109 
00110 returnValue OCP::minimizeLSQ( const MatrixVariablesGrid &S,
00111                               const Function            &h,
00112                               const char*        rFilename )
00113 {
00114 
00115     VariablesGrid r;
00116     r.read( rFilename );
00117 
00118     if( r.isEmpty() == BT_TRUE )
00119         return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
00120 
00121     return minimizeLSQ( S,h,r );
00122 }
00123 
00124 
00125 returnValue OCP::minimizeLSQ( const DMatrix        &S,
00126                               const Function      &h,
00127                               const char*  rFilename  ){
00128 
00129     VariablesGrid r;
00130     r.read( rFilename );
00131 
00132     if( r.isEmpty() == BT_TRUE )
00133         return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
00134 
00135     return minimizeLSQ( S,h,r );
00136 }
00137 
00138 
00139 returnValue OCP::minimizeLSQ( const Function      &h,
00140                               const char*  rFilename  ){
00141 
00142     VariablesGrid r;
00143     r.read( rFilename );
00144 
00145     if( r.isEmpty() == BT_TRUE )
00146         return ACADOERROR( RET_FILE_CAN_NOT_BE_OPENED );
00147 
00148     return minimizeLSQ( h,r );
00149 }
00150 
00151 
00152 
00153 returnValue OCP::subjectTo( const DifferentialEquation& differentialEquation_ ){
00154 
00155     modelData.setModel(differentialEquation_);
00156     return SUCCESSFUL_RETURN;
00157 }
00158 
00159 
00160 
00161 returnValue OCP::subjectTo( const ConstraintComponent& component ){
00162 
00163     for( uint i=0; i<component.getDim(); ++i )
00164         constraint->add( component(i) );
00165 
00166     return SUCCESSFUL_RETURN;
00167 }
00168 
00169 
00170 returnValue OCP::subjectTo( int index_, const ConstraintComponent& component )
00171 {
00172         ASSERT(index_ >= AT_START);
00173 
00174         if (index_ == AT_START)
00175         {
00176                 for (unsigned el = 0; el < component.getDim(); ++el)
00177                         ACADO_TRY( constraint->add( 0,component( el ) ) );
00178         }
00179         else if (index_ == AT_END)
00180         {
00181                 for (unsigned el = 0; el < component.getDim(); ++el)
00182                         ACADO_TRY(constraint->add(grid->getLastIndex(), component( el )));
00183         }
00184         else
00185         {
00186                 for (unsigned el = 0; el < component.getDim(); ++el)
00187                         constraint->add(index_, component(el));
00188         }
00189 
00190     return SUCCESSFUL_RETURN;
00191 }
00192 
00193 
00194 returnValue OCP::subjectTo( const double lb_, const Expression& arg1,
00195                                                         const Expression& arg2, const double ub_ ){
00196 
00197     return constraint->add( lb_, arg1, arg2, ub_ );
00198 }
00199 
00200 
00201 returnValue OCP::subjectTo( const double lb_, const Expression *arguments, const double ub_ )
00202 {
00203     return constraint->add( lb_, arguments, ub_ );
00204 }
00205 
00206 returnValue OCP::subjectTo( const DVector& _lb, const Expression& _expr, const DVector& _ub )
00207 {
00208         ASSERT(_lb.getDim() == _expr.getDim() && _lb.getDim() == _ub.getDim());
00209         constraint->add(_lb, _expr, _ub);
00210 
00211         return SUCCESSFUL_RETURN;
00212 }
00213 
00214 returnValue OCP::subjectTo( int _index, const DVector& _lb, const Expression& _expr, const DVector& _ub )
00215 {
00216         ASSERT(_index >= AT_START);
00217         cout << _lb.getDim() << " " << _expr.getDim() << endl;
00218         ASSERT(_lb.getDim() == _expr.getDim());
00219         ASSERT(_lb.getDim() == _ub.getDim());
00220 
00221         if (_index == AT_START)
00222         {
00223                 for (unsigned el = 0; el < _lb.getDim(); ++el)
00224                         ACADO_TRY( constraint->add(0, _lb( el ), _expr( el ), _ub( el )) );
00225         }
00226         else if (_index == AT_END)
00227         {
00228                 for (unsigned el = 0; el < _lb.getDim(); ++el)
00229                         ACADO_TRY(constraint->add(grid->getLastIndex(), _lb( el ), _expr( el ), _ub( el )) );
00230         }
00231         else
00232                 for (unsigned el = 0; el < _lb.getDim(); ++el)
00233                         constraint->add(_index, _lb( el ), _expr( el ), _ub( el ));
00234 
00235         return SUCCESSFUL_RETURN;
00236 }
00237 
00238 returnValue OCP::minimizeMayerTerm   ( const Expression& arg ){ return objective->addMayerTerm   ( arg ); }
00239 returnValue OCP::maximizeMayerTerm   ( const Expression& arg ){ return objective->addMayerTerm   (-arg ); }
00240 returnValue OCP::minimizeLagrangeTerm( const Expression& arg ){ return objective->addLagrangeTerm( arg ); }
00241 returnValue OCP::maximizeLagrangeTerm( const Expression& arg ){ return objective->addLagrangeTerm(-arg ); }
00242 
00243 
00244 returnValue OCP::minimizeLSQ( const DMatrix&S, const Function &h, const DVector &r )
00245 {
00246     MatrixVariablesGrid tmpS(S);
00247     VariablesGrid       tmpR(r);
00248 
00249     return objective->addLSQ( &tmpS, h, &tmpR );
00250 }
00251 
00252 returnValue OCP::minimizeLSQ( const Function &h, const DVector &r ){
00253 
00254     DMatrix S( h.getDim( ),h.getDim( ) );
00255     S.setIdentity( );
00256 
00257     return minimizeLSQ( S, h, r );
00258 }
00259 
00260 returnValue OCP::minimizeLSQ( const Function &h ){
00261 
00262     DMatrix S( h.getDim( ),h.getDim( ) );
00263     S.setIdentity( );
00264 
00265     DVector r(h.getDim());
00266     r.setZero();
00267 
00268     return minimizeLSQ( S, h, r );
00269 }
00270 
00271 
00272 returnValue OCP::minimizeLSQ( const MatrixVariablesGrid &S,
00273                               const Function            &h,
00274                               const VariablesGrid       &r  ){
00275 
00276     return objective->addLSQ( &S, h, &r );
00277 }
00278 
00279 returnValue OCP::minimizeLSQ( const DMatrix        &S,
00280                               const Function      &h,
00281                               const VariablesGrid &r ){
00282 
00283         if ( S.isPositiveSemiDefinite() == BT_FALSE )
00284                 return ACADOERROR( RET_NONPOSITIVE_WEIGHT );
00285 
00286     MatrixVariablesGrid tmpS(S);
00287     return objective->addLSQ( &tmpS, h, &r );
00288 }
00289 
00290 returnValue OCP::minimizeLSQ( const Function      &h,
00291                               const VariablesGrid &r ){
00292 
00293     return objective->addLSQ( 0, h, &r );
00294 }
00295 
00296 
00297 returnValue OCP::minimizeLSQEndTerm( const DMatrix   & S,
00298                                      const Function & m,
00299                                      const DVector   & r  ){
00300 
00301         if ( S.isPositiveSemiDefinite() == BT_FALSE )
00302                 return ACADOERROR( RET_NONPOSITIVE_WEIGHT );
00303 
00304     return objective->addLSQEndTerm( S, m, r );
00305 }
00306 
00307 returnValue OCP::minimizeLSQEndTerm( const Function & m,
00308                                      const DVector   & r  ){
00309 
00310     DMatrix S( m.getDim( ),m.getDim( ) );
00311     S.setIdentity( );
00312     return minimizeLSQEndTerm( S, m, r );
00313 }
00314 
00315 
00316 BooleanType OCP::hasObjective() const{
00317 
00318     if( objective->isEmpty() == BT_TRUE ) return BT_FALSE;
00319     return BT_TRUE;
00320 }
00321 
00322 BooleanType OCP::hasConstraint() const{
00323 
00324     if( constraint->isEmpty() == BT_TRUE ) return BT_FALSE;
00325     return BT_TRUE;
00326 }
00327 
00328 
00329 returnValue OCP::getGrid     ( Grid       & grid_      ) const{ grid_       = *grid      ; return SUCCESSFUL_RETURN; }
00330 returnValue OCP::getObjective( Objective  & objective_ ) const{ objective_  = *objective ; return SUCCESSFUL_RETURN; }
00331 returnValue OCP::getConstraint( Constraint& constraint_) const{ constraint_ = *constraint; return SUCCESSFUL_RETURN; }
00332 
00333 
00334 returnValue OCP::setObjective ( const Objective & objective_  )
00335 {
00336         objective   = std::tr1::shared_ptr<Objective> (new Objective( objective_ ));
00337         return SUCCESSFUL_RETURN;
00338 }
00339 returnValue OCP::setConstraint( const Constraint& constraint_ )
00340 {
00341         constraint = std::tr1::shared_ptr<Constraint> (new Constraint( constraint_ ));;
00342         return SUCCESSFUL_RETURN;
00343 }
00344 
00345 
00346 returnValue OCP::setNumberIntegrationSteps( const uint numSteps )
00347 {
00348         if( hasEquidistantControlGrid() ) {
00349                 setIntegrationGrid( *grid, numSteps );
00350         }
00351         return SUCCESSFUL_RETURN;
00352 }
00353 
00354 
00355 returnValue OCP::getObjective( const int &multiObjectiveIdx, Expression **arg ) const{
00356 
00357     return MultiObjectiveFunctionality::getObjective( multiObjectiveIdx, arg );
00358 }
00359 
00360 
00361 double OCP::getStartTime ( ) const{ return grid->getFirstTime(); }
00362 double OCP::getEndTime   ( ) const{ return grid->getLastTime (); }
00363 
00364 BooleanType OCP::hasEquidistantGrid( ) const{
00365         
00366         DVector numSteps;
00367         modelData.getNumSteps(numSteps);
00368         return numSteps.isEmpty();
00369 }
00370 
00371 returnValue OCP::minimizeLSQ(const DMatrix& S, const Function& h)
00372 {
00373         return objective->addLSQ(S, h);
00374 }
00375 
00376 returnValue OCP::minimizeLSQEndTerm(const DMatrix& S, const Function& h)
00377 {
00378         return objective->addLSQEndTerm(S, h);
00379 }
00380 
00381 returnValue OCP::minimizeLSQ(const BMatrix& S, const Function& h)
00382 {
00383         return objective->addLSQ(S, h);
00384 }
00385 
00386 returnValue OCP::minimizeLSQEndTerm(const BMatrix& S, const Function& h)
00387 {
00388         return objective->addLSQEndTerm(S, h);
00389 }
00390 
00391 returnValue OCP::minimizeLSQ(const DMatrix& S, const std::string& h)
00392 {
00393         return objective->addLSQ(S, h);
00394 }
00395 
00396 returnValue OCP::minimizeLSQEndTerm(const DMatrix& S, const std::string& h)
00397 {
00398         return objective->addLSQEndTerm(S, h);
00399 }
00400 
00401 returnValue OCP::minimizeLSQ(const BMatrix& S, const std::string& h)
00402 {
00403         return objective->addLSQ(S, h);
00404 }
00405 
00406 returnValue OCP::minimizeLSQEndTerm(const BMatrix& S, const std::string& h)
00407 {
00408         return objective->addLSQEndTerm(S, h);
00409 }
00410 
00411 returnValue OCP::minimizeLSQLinearTerms(const DVector& Slx, const DVector& Slu)
00412 {
00413         return objective->addLSQLinearTerms(Slx, Slu);
00414 }
00415 
00416 returnValue OCP::minimizeLSQLinearTerms(const BVector& Slx, const BVector& Slu)
00417 {
00418         return objective->addLSQLinearTerms(Slx, Slu);
00419 }
00420 
00421 // PROTECTED FUNCTIONS:
00422 // --------------------
00423 void OCP::setupGrid( double tStart, double tEnd, int N ){
00424 
00425     grid->init( tStart, tEnd, N );
00426     objective->init ( *grid );
00427     constraint->init( *grid );
00428     setN( grid->getNumIntervals() );
00429 }
00430 
00431 
00432 void OCP::setupGrid( const DVector& times ){
00433 
00434     grid->init( times );
00435     objective->init ( *grid );
00436     constraint->init( *grid );
00437     setN( grid->getNumIntervals() );
00438 }
00439 
00440 
00441 
00442 CLOSE_NAMESPACE_ACADO
00443 
00444 // end of file.


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