ocp.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of ACADO Toolkit.
3  *
4  * ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
5  * Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
6  * Milan Vukov, Rien Quirynen, KU Leuven.
7  * Developed within the Optimization in Engineering Center (OPTEC)
8  * under supervision of Moritz Diehl. All rights reserved.
9  *
10  * ACADO Toolkit is free software; you can redistribute it and/or
11  * modify it under the terms of the GNU Lesser General Public
12  * License as published by the Free Software Foundation; either
13  * version 3 of the License, or (at your option) any later version.
14  *
15  * ACADO Toolkit is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18  * Lesser General Public License for more details.
19  *
20  * You should have received a copy of the GNU Lesser General Public
21  * License along with ACADO Toolkit; if not, write to the Free Software
22  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
23  *
24  */
25 
26 
33 #include <acado/ocp/ocp.hpp>
36 
37 using namespace std;
39 
40 //
41 // PUBLIC MEMBER FUNCTIONS:
42 //
43 
44 
45 OCP::OCP(const double &tStart_, const double &tEnd_, const int &N_)
47  grid(new Grid()), objective(new Objective()), constraint(new Constraint())
48 {
49 
50  if (N_ < 0)
52  setupGrid(tStart_, tEnd_, N_ + 1);
53 }
54 
55 
56 OCP::OCP( const double &tStart_, const double &tEnd_, const DVector& _numSteps )
58  grid(new Grid()), objective(new Objective()), constraint(new Constraint())
59 {
60  if( _numSteps.getDim() <= 0 ) ACADOERROR( RET_INVALID_ARGUMENTS );
61 
62  DVector times( _numSteps.getDim()+1 );
63  times(0) = tStart_;
64 
65  double totalSteps = 0;
66  uint i;
67  for( i = 0; i < _numSteps.getDim(); i++ ) {
68  totalSteps += _numSteps(i);
69  }
70  double h = (tEnd_ - tStart_)/totalSteps;
71  for( i = 0; i < _numSteps.getDim(); i++ ) {
72  times(i+1) = times(i) + h*_numSteps(i);
73  }
74 
75  setupGrid( times );
76  modelData.setIntegrationGrid(*grid, totalSteps);
77 }
78 
79 
80 OCP::OCP( const Grid &grid_ )
82  grid(new Grid(grid_)), objective(new Objective()), constraint(new Constraint())
83 {
84  if( grid->getNumPoints() <= 1 ) ACADOERROR( RET_INVALID_ARGUMENTS );
85  objective->init ( *grid );
86  constraint->init( *grid );
87  setN( grid->getNumIntervals() );
88 }
89 
90 
91 OCP::OCP( const double &tStart_,
92  const Parameter &tEnd_ ,
93  const int &N_ )
95  grid(new Grid()), objective(new Objective()), constraint(new Constraint())
96 {
97  if( N_ <= 0 ) ACADOERROR( RET_INVALID_ARGUMENTS );
98  setupGrid( tStart_, tStart_ + 1.0, N_+1);
99 }
100 
102 {}
103 
104 returnValue OCP::minimizeMayerTerm( const int &multiObjectiveIdx, const Expression& arg ){
105 
106  return MultiObjectiveFunctionality::minimizeMayerTerm( multiObjectiveIdx, arg );
107 }
108 
109 
111  const Function &h,
112  const char* rFilename )
113 {
114 
115  VariablesGrid r;
116  r.read( rFilename );
117 
118  if( r.isEmpty() == BT_TRUE )
120 
121  return minimizeLSQ( S,h,r );
122 }
123 
124 
126  const Function &h,
127  const char* rFilename ){
128 
129  VariablesGrid r;
130  r.read( rFilename );
131 
132  if( r.isEmpty() == BT_TRUE )
134 
135  return minimizeLSQ( S,h,r );
136 }
137 
138 
140  const char* rFilename ){
141 
142  VariablesGrid r;
143  r.read( rFilename );
144 
145  if( r.isEmpty() == BT_TRUE )
147 
148  return minimizeLSQ( h,r );
149 }
150 
151 
152 
153 returnValue OCP::subjectTo( const DifferentialEquation& differentialEquation_ ){
154 
155  modelData.setModel(differentialEquation_);
156  return SUCCESSFUL_RETURN;
157 }
158 
159 
160 
162 
163  for( uint i=0; i<component.getDim(); ++i )
164  constraint->add( component(i) );
165 
166  return SUCCESSFUL_RETURN;
167 }
168 
169 
170 returnValue OCP::subjectTo( int index_, const ConstraintComponent& component )
171 {
172  ASSERT(index_ >= AT_START);
173 
174  if (index_ == AT_START)
175  {
176  for (unsigned el = 0; el < component.getDim(); ++el)
177  ACADO_TRY( constraint->add( 0,component( el ) ) );
178  }
179  else if (index_ == AT_END)
180  {
181  for (unsigned el = 0; el < component.getDim(); ++el)
182  ACADO_TRY(constraint->add(grid->getLastIndex(), component( el )));
183  }
184  else
185  {
186  for (unsigned el = 0; el < component.getDim(); ++el)
187  constraint->add(index_, component(el));
188  }
189 
190  return SUCCESSFUL_RETURN;
191 }
192 
193 
194 returnValue OCP::subjectTo( const double lb_, const Expression& arg1,
195  const Expression& arg2, const double ub_ ){
196 
197  return constraint->add( lb_, arg1, arg2, ub_ );
198 }
199 
200 
201 returnValue OCP::subjectTo( const double lb_, const Expression *arguments, const double ub_ )
202 {
203  return constraint->add( lb_, arguments, ub_ );
204 }
205 
206 returnValue OCP::subjectTo( const DVector& _lb, const Expression& _expr, const DVector& _ub )
207 {
208  ASSERT(_lb.getDim() == _expr.getDim() && _lb.getDim() == _ub.getDim());
209  constraint->add(_lb, _expr, _ub);
210 
211  return SUCCESSFUL_RETURN;
212 }
213 
214 returnValue OCP::subjectTo( int _index, const DVector& _lb, const Expression& _expr, const DVector& _ub )
215 {
216  ASSERT(_index >= AT_START);
217  cout << _lb.getDim() << " " << _expr.getDim() << endl;
218  ASSERT(_lb.getDim() == _expr.getDim());
219  ASSERT(_lb.getDim() == _ub.getDim());
220 
221  if (_index == AT_START)
222  {
223  for (unsigned el = 0; el < _lb.getDim(); ++el)
224  ACADO_TRY( constraint->add(0, _lb( el ), _expr( el ), _ub( el )) );
225  }
226  else if (_index == AT_END)
227  {
228  for (unsigned el = 0; el < _lb.getDim(); ++el)
229  ACADO_TRY(constraint->add(grid->getLastIndex(), _lb( el ), _expr( el ), _ub( el )) );
230  }
231  else
232  for (unsigned el = 0; el < _lb.getDim(); ++el)
233  constraint->add(_index, _lb( el ), _expr( el ), _ub( el ));
234 
235  return SUCCESSFUL_RETURN;
236 }
237 
238 returnValue OCP::minimizeMayerTerm ( const Expression& arg ){ return objective->addMayerTerm ( arg ); }
239 returnValue OCP::maximizeMayerTerm ( const Expression& arg ){ return objective->addMayerTerm (-arg ); }
240 returnValue OCP::minimizeLagrangeTerm( const Expression& arg ){ return objective->addLagrangeTerm( arg ); }
241 returnValue OCP::maximizeLagrangeTerm( const Expression& arg ){ return objective->addLagrangeTerm(-arg ); }
242 
243 
244 returnValue OCP::minimizeLSQ( const DMatrix&S, const Function &h, const DVector &r )
245 {
246  MatrixVariablesGrid tmpS(S);
247  VariablesGrid tmpR(r);
248 
249  return objective->addLSQ( &tmpS, h, &tmpR );
250 }
251 
253 
254  DMatrix S( h.getDim( ),h.getDim( ) );
255  S.setIdentity( );
256 
257  return minimizeLSQ( S, h, r );
258 }
259 
261 
262  DMatrix S( h.getDim( ),h.getDim( ) );
263  S.setIdentity( );
264 
265  DVector r(h.getDim());
266  r.setZero();
267 
268  return minimizeLSQ( S, h, r );
269 }
270 
271 
273  const Function &h,
274  const VariablesGrid &r ){
275 
276  return objective->addLSQ( &S, h, &r );
277 }
278 
280  const Function &h,
281  const VariablesGrid &r ){
282 
283  if ( S.isPositiveSemiDefinite() == BT_FALSE )
285 
286  MatrixVariablesGrid tmpS(S);
287  return objective->addLSQ( &tmpS, h, &r );
288 }
289 
291  const VariablesGrid &r ){
292 
293  return objective->addLSQ( 0, h, &r );
294 }
295 
296 
298  const Function & m,
299  const DVector & r ){
300 
301  if ( S.isPositiveSemiDefinite() == BT_FALSE )
303 
304  return objective->addLSQEndTerm( S, m, r );
305 }
306 
308  const DVector & r ){
309 
310  DMatrix S( m.getDim( ),m.getDim( ) );
311  S.setIdentity( );
312  return minimizeLSQEndTerm( S, m, r );
313 }
314 
315 
317 
318  if( objective->isEmpty() == BT_TRUE ) return BT_FALSE;
319  return BT_TRUE;
320 }
321 
323 
324  if( constraint->isEmpty() == BT_TRUE ) return BT_FALSE;
325  return BT_TRUE;
326 }
327 
328 
329 returnValue OCP::getGrid ( Grid & grid_ ) const{ grid_ = *grid ; return SUCCESSFUL_RETURN; }
330 returnValue OCP::getObjective( Objective & objective_ ) const{ objective_ = *objective ; return SUCCESSFUL_RETURN; }
331 returnValue OCP::getConstraint( Constraint& constraint_) const{ constraint_ = *constraint; return SUCCESSFUL_RETURN; }
332 
333 
335 {
336  objective = std::shared_ptr<Objective> (new Objective( objective_ ));
337  return SUCCESSFUL_RETURN;
338 }
340 {
341  constraint = std::shared_ptr<Constraint> (new Constraint( constraint_ ));;
342  return SUCCESSFUL_RETURN;
343 }
344 
345 
347 {
348  if( hasEquidistantControlGrid() ) {
349  setIntegrationGrid( *grid, numSteps );
350  }
351  return SUCCESSFUL_RETURN;
352 }
353 
354 
355 returnValue OCP::getObjective( const int &multiObjectiveIdx, Expression **arg ) const{
356 
357  return MultiObjectiveFunctionality::getObjective( multiObjectiveIdx, arg );
358 }
359 
360 
361 double OCP::getStartTime ( ) const{ return grid->getFirstTime(); }
362 double OCP::getEndTime ( ) const{ return grid->getLastTime (); }
363 
365 
366  DVector numSteps;
367  modelData.getNumSteps(numSteps);
368  return numSteps.isEmpty();
369 }
370 
372 {
373  return objective->addLSQ(S, h);
374 }
375 
377 {
378  return objective->addLSQEndTerm(S, h);
379 }
380 
382 {
383  return objective->addLSQ(S, h);
384 }
385 
387 {
388  return objective->addLSQEndTerm(S, h);
389 }
390 
391 returnValue OCP::minimizeLSQ(const DMatrix& S, const std::string& h)
392 {
393  return objective->addLSQ(S, h);
394 }
395 
396 returnValue OCP::minimizeLSQEndTerm(const DMatrix& S, const std::string& h)
397 {
398  return objective->addLSQEndTerm(S, h);
399 }
400 
401 returnValue OCP::minimizeLSQ(const BMatrix& S, const std::string& h)
402 {
403  return objective->addLSQ(S, h);
404 }
405 
406 returnValue OCP::minimizeLSQEndTerm(const BMatrix& S, const std::string& h)
407 {
408  return objective->addLSQEndTerm(S, h);
409 }
410 
412 {
413  return objective->addLSQLinearTerms(Slx, Slu);
414 }
415 
417 {
418  return objective->addLSQLinearTerms(Slx, Slu);
419 }
420 
421 // PROTECTED FUNCTIONS:
422 // --------------------
423 void OCP::setupGrid( double tStart, double tEnd, int N ){
424 
425  grid->init( tStart, tEnd, N );
426  objective->init ( *grid );
427  constraint->init( *grid );
428  setN( grid->getNumIntervals() );
429 }
430 
431 
432 void OCP::setupGrid( const DVector& times ){
433 
434  grid->init( times );
435  objective->init ( *grid );
436  constraint->init( *grid );
437  setN( grid->getNumIntervals() );
438 }
439 
440 
441 
443 
444 // end of file.
#define N
OCP(const double &tStart_=0.0, const double &tEnd_=1.0, const int &N_=20)
Definition: ocp.cpp:45
Allows to setup and evaluate a general function based on SymbolicExpressions.
Definition: function_.hpp:59
void setupGrid(double tStart, double tEnd, int N)
Definition: ocp.cpp:423
returnValue setIntegrationGrid(const Grid &_ocpGrid, const uint _numSteps)
Definition: model_data.cpp:396
returnValue getObjective(Objective &objective_) const
Definition: ocp.cpp:330
std::shared_ptr< Objective > objective
Definition: ocp.hpp:398
returnValue minimizeLagrangeTerm(const Expression &arg)
Definition: ocp.cpp:240
bool isPositiveSemiDefinite() const
Definition: matrix.cpp:173
returnValue minimizeMayerTerm(const int &multiObjectiveIdx, const Expression &arg)
returnValue getObjective(const int &multiObjectiveIdx, Expression **arg) const
returnValue setNumberIntegrationSteps(const uint numSteps)
Definition: ocp.cpp:346
returnValue maximizeLagrangeTerm(const Expression &arg)
Definition: ocp.cpp:241
Stores and evaluates the constraints of optimal control problems.
Definition: constraint.hpp:60
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Allows to pass back messages to the calling function.
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
BooleanType isEmpty() const
returnValue subjectTo(const DifferentialEquation &differentialEquation_)
Definition: ocp.cpp:153
#define CLOSE_NAMESPACE_ACADO
returnValue minimizeMayerTerm(const Expression &arg)
Definition: ocp.cpp:238
bool isEmpty() const
Definition: vector.hpp:176
returnValue getConstraint(Constraint &constraint_) const
Definition: ocp.cpp:331
Data class for symbolically formulating constraints within optimal control problems.
returnValue setConstraint(const Constraint &constraint_)
Definition: ocp.cpp:339
#define ACADO_TRY(X)
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
std::shared_ptr< Grid > grid
Definition: ocp.hpp:396
double getEndTime() const
Definition: ocp.cpp:362
returnValue minimizeLSQ(const DMatrix &S, const Function &h, const DVector &r)
Definition: ocp.cpp:244
returnValue setN(const uint N_)
returnValue minimizeLSQEndTerm(const DMatrix &S, const Function &m, const DVector &r)
Definition: ocp.cpp:297
Encapsulates functionality for defining OCPs having multiple objectives.
returnValue setModel(const DifferentialEquation &_f)
Definition: model_data.cpp:253
Provides a time grid consisting of matrix-valued optimization variables at each grid point...
unsigned getDim() const
Definition: vector.hpp:172
BooleanType hasEquidistantControlGrid() const
int getDim() const
BooleanType hasConstraint() const
Definition: ocp.cpp:322
std::shared_ptr< Constraint > constraint
Definition: ocp.hpp:400
returnValue setIntegrationGrid(const Grid &_ocpGrid, const uint _numSteps)
double getStartTime() const
Definition: ocp.cpp:361
returnValue minimizeLSQLinearTerms(const DVector &Slx, const DVector &Slu)
Definition: ocp.cpp:411
Derived & setZero(Index size)
virtual ~OCP()
Definition: ocp.cpp:101
returnValue read(std::istream &stream)
virtual BooleanType hasEquidistantGrid() const
Definition: ocp.cpp:364
#define ASSERT(x)
#define BT_TRUE
Definition: acado_types.hpp:47
returnValue getNumSteps(DVector &_numSteps) const
Definition: model_data.cpp:152
uint getDim() const
returnValue getGrid(Grid &grid_) const
Definition: ocp.cpp:329
#define BEGIN_NAMESPACE_ACADO
#define BT_FALSE
Definition: acado_types.hpp:49
BooleanType hasObjective() const
Definition: ocp.cpp:316
returnValue setObjective(const Objective &objective_)
Definition: ocp.cpp:334
returnValue maximizeMayerTerm(const Expression &arg)
Definition: ocp.cpp:239
Stores and evaluates the objective function of optimal control problems.
Definition: objective.hpp:123
#define ACADOERROR(retval)
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
uint getDim() const


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:54