objective_element.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 
27 
34 #include <acado/objective/objective_element.hpp>
35 
36 
37 
38 BEGIN_NAMESPACE_ACADO
39 
40 
41 
42 
43 ObjectiveElement::ObjectiveElement( ){
44 
45  y_index = 0;
46  t_index = 0;
47  nx = 0;
48  na = 0;
49  nu = 0;
50  np = 0;
51  nw = 0;
52  ny = 0;
53 
54  xSeed = 0 ;
55  xaSeed = 0 ;
56  pSeed = 0 ;
57  uSeed = 0 ;
58  wSeed = 0 ;
59  bSeed = 0 ;
60  xSeed2 = 0 ;
61  xaSeed2 = 0 ;
62  pSeed2 = 0 ;
63  uSeed2 = 0 ;
64  wSeed2 = 0 ;
65  bSeed2 = 0 ;
66 
67  obj = 0.0;
68 }
69 
70 
71 ObjectiveElement::ObjectiveElement( const Grid &grid_ ){
72 
73  grid = grid_;
74  y_index = 0 ;
75  t_index = 0 ;
76  nx = 0 ;
77  na = 0 ;
78  nu = 0 ;
79  np = 0 ;
80  nw = 0 ;
81  ny = 0 ;
82 
83  xSeed = 0 ;
84  xaSeed = 0 ;
85  pSeed = 0 ;
86  uSeed = 0 ;
87  wSeed = 0 ;
88  bSeed = 0 ;
89  xSeed2 = 0 ;
90  xaSeed2 = 0 ;
91  pSeed2 = 0 ;
92  uSeed2 = 0 ;
93  wSeed2 = 0 ;
94  bSeed2 = 0 ;
95 
96  obj = 0.0;
97 }
98 
99 
100 ObjectiveElement::ObjectiveElement( const ObjectiveElement& rhs ){
101 
102  int run1;
103 
104  grid = rhs.grid;
105  fcn = rhs.fcn ;
106 
107  if( rhs.y_index != 0 ){
108  y_index = new int[rhs.ny];
109  for( run1 = 0; run1 < rhs.ny; run1++ )
110  y_index[run1] = rhs.y_index[run1];
111  }
112  else y_index = 0;
113 
114  t_index = rhs.t_index;
115 
116  nx = rhs.nx;
117  na = rhs.na;
118  nu = rhs.nu;
119  np = rhs.np;
120  nw = rhs.nw;
121  ny = rhs.ny;
122 
123  if( rhs.xSeed != 0 ) xSeed = new BlockMatrix(*rhs.xSeed );
124  else xSeed = 0 ;
125  if( rhs.xaSeed != 0 ) xaSeed = new BlockMatrix(*rhs.xaSeed);
126  else xaSeed = 0 ;
127  if( rhs.pSeed != 0 ) pSeed = new BlockMatrix(*rhs.pSeed) ;
128  else pSeed = 0 ;
129  if( rhs.uSeed != 0 ) uSeed = new BlockMatrix(*rhs.uSeed) ;
130  else uSeed = 0 ;
131  if( rhs.wSeed != 0 ) wSeed = new BlockMatrix(*rhs.wSeed) ;
132  else wSeed = 0 ;
133 
134  if( rhs.bSeed != 0 ) bSeed = new BlockMatrix(*rhs.bSeed );
135  else bSeed = 0 ;
136 
137  if( rhs.xSeed2 != 0 ) xSeed2 = new BlockMatrix(*rhs.xSeed2) ;
138  else xSeed2 = 0 ;
139  if( rhs.xaSeed2 != 0 ) xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
140  else xaSeed2 = 0 ;
141  if( rhs.pSeed2 != 0 ) pSeed2 = new BlockMatrix(*rhs.pSeed2 );
142  else pSeed2 = 0 ;
143  if( rhs.uSeed2 != 0 ) uSeed2 = new BlockMatrix(*rhs.uSeed2 );
144  else uSeed2 = 0 ;
145  if( rhs.wSeed2 != 0 ) wSeed2 = new BlockMatrix(*rhs.wSeed2 );
146  else wSeed2 = 0 ;
147 
148  if( rhs.bSeed != 0 ) bSeed2 = new BlockMatrix(*rhs.bSeed );
149  else bSeed2 = 0 ;
150 
151 
152  obj = rhs.obj ;
153  dForward = rhs.dForward ;
154  dBackward = rhs.dBackward;
155 }
156 
157 
158 ObjectiveElement::~ObjectiveElement( ){
159 
160  if( y_index != 0 )
161  delete[] y_index;
162 
163  if( xSeed != 0 ) delete xSeed ;
164  if( xaSeed != 0 ) delete xaSeed ;
165  if( pSeed != 0 ) delete pSeed ;
166  if( uSeed != 0 ) delete uSeed ;
167  if( wSeed != 0 ) delete wSeed ;
168 
169  if( bSeed != 0 ) delete bSeed ;
170 
171  if( xSeed2 != 0 ) delete xSeed2 ;
172  if( xaSeed2 != 0 ) delete xaSeed2;
173  if( pSeed2 != 0 ) delete pSeed2 ;
174  if( uSeed2 != 0 ) delete uSeed2 ;
175  if( wSeed2 != 0 ) delete wSeed2 ;
176 
177  if( bSeed2 != 0 ) delete bSeed2 ;
178 }
179 
180 
181 ObjectiveElement& ObjectiveElement::operator=( const ObjectiveElement& rhs ){
182 
183  int run1;
184 
185  if( this != &rhs ){
186 
187  if( y_index != 0 )
188  delete[] y_index;
189 
190  if( xSeed != 0 ) delete xSeed ;
191  if( xaSeed != 0 ) delete xaSeed ;
192  if( pSeed != 0 ) delete pSeed ;
193  if( uSeed != 0 ) delete uSeed ;
194  if( wSeed != 0 ) delete wSeed ;
195 
196  if( bSeed != 0 ) delete bSeed ;
197 
198  if( xSeed2 != 0 ) delete xSeed2 ;
199  if( xaSeed2 != 0 ) delete xaSeed2;
200  if( pSeed2 != 0 ) delete pSeed2 ;
201  if( uSeed2 != 0 ) delete uSeed2 ;
202  if( wSeed2 != 0 ) delete wSeed2 ;
203 
204  if( bSeed2 != 0 ) delete bSeed2 ;
205 
206  grid = rhs.grid;
207  fcn = rhs.fcn ;
208 
209  if( rhs.y_index != 0 ){
210  y_index = new int[ny];
211  for( run1 = 0; run1 < ny; run1++ )
212  y_index[run1] = rhs.y_index[run1];
213  }
214  else y_index = 0;
215 
216  t_index = rhs.t_index;
217 
218  nx = rhs.nx;
219  na = rhs.na;
220  nu = rhs.nu;
221  np = rhs.np;
222  nw = rhs.nw;
223  ny = rhs.ny;
224 
225  if( rhs.xSeed != 0 ) xSeed = new BlockMatrix(*rhs.xSeed );
226  else xSeed = 0 ;
227  if( rhs.xaSeed != 0 ) xaSeed = new BlockMatrix(*rhs.xaSeed );
228  else xaSeed = 0 ;
229  if( rhs.pSeed != 0 ) pSeed = new BlockMatrix(*rhs.pSeed );
230  else pSeed = 0 ;
231  if( rhs.uSeed != 0 ) uSeed = new BlockMatrix(*rhs.uSeed );
232  else uSeed = 0 ;
233  if( rhs.wSeed != 0 ) wSeed = new BlockMatrix(*rhs.wSeed );
234  else wSeed = 0 ;
235 
236  if( rhs.bSeed != 0 ) bSeed = new BlockMatrix(*rhs.bSeed );
237  else bSeed = 0 ;
238 
239  if( rhs.xSeed2 != 0 ) xSeed2 = new BlockMatrix(*rhs.xSeed2 );
240  else xSeed2 = 0 ;
241  if( rhs.xaSeed2 != 0 ) xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
242  else xaSeed2 = 0 ;
243  if( rhs.pSeed2 != 0 ) pSeed2 = new BlockMatrix(*rhs.pSeed2 );
244  else pSeed2 = 0 ;
245  if( rhs.uSeed2 != 0 ) uSeed2 = new BlockMatrix(*rhs.uSeed2 );
246  else uSeed2 = 0 ;
247  if( rhs.wSeed2 != 0 ) wSeed2 = new BlockMatrix(*rhs.wSeed2 );
248  else wSeed2 = 0 ;
249 
250  if( rhs.bSeed != 0 ) bSeed2 = new BlockMatrix(*rhs.bSeed );
251  else bSeed2 = 0 ;
252 
253  obj = rhs.obj ;
254  dForward = rhs.dForward ;
255  dBackward = rhs.dBackward;
256  }
257  return *this;
258 }
259 
260 
261 
262 
263 returnValue ObjectiveElement::init( const OCPiterate &x ){
264 
265  int run1;
266 
267  z.init( fcn, x );
268  JJ.init( fcn, x );
269  //HH.init( fcn, x );
270 
271  if( x.x != NULL ) nx = x.x->getNumValues();
272  else nx = 0 ;
273 
274  if( x.xa != NULL ) na = x.xa->getNumValues();
275  else na = 0 ;
276 
277  if( x.p != NULL ) np = x.p ->getNumValues();
278  else np = 0 ;
279 
280  if( x.u != NULL ) nu = x.u ->getNumValues();
281  else nu = 0 ;
282 
283  if( x.w != NULL ) nw = x.w ->getNumValues();
284  else nw = 0 ;
285 
286  ny = nx+na+nu+np+nw;
287 
288  if( y_index != 0 ) delete[] y_index;
289  y_index = new int[ny];
290 
291  for( run1 = 0; run1 < nx; run1++ )
292  y_index[run1] = fcn.index( VT_DIFFERENTIAL_STATE, run1 );
293 
294  for( run1 = 0; run1 < na; run1++ )
295  y_index[nx+run1] = fcn.index( VT_ALGEBRAIC_STATE, run1 );
296 
297  for( run1 = 0; run1 < np; run1++ )
298  y_index[nx+na+run1] = fcn.index( VT_PARAMETER, run1 );
299 
300  for( run1 = 0; run1 < nu; run1++ )
301  y_index[nx+na+np+run1] = fcn.index( VT_CONTROL, run1 );
302 
303  for( run1 = 0; run1 < nw; run1++ )
304  y_index[nx+na+np+nu+run1] = fcn.index( VT_DISTURBANCE, run1 );
305 
306  t_index = fcn.index( VT_TIME, 0 );
307 
308  return SUCCESSFUL_RETURN;
309 }
310 
311 
312 
313 returnValue ObjectiveElement::setForwardSeed( BlockMatrix *xSeed_,
314  BlockMatrix *xaSeed_,
315  BlockMatrix *pSeed_,
316  BlockMatrix *uSeed_,
317  BlockMatrix *wSeed_,
318  int order ){
319 
320  if( order == 1 ){
321 
322  if( xSeed_ != 0 ){
323  if( xSeed != 0 ) delete xSeed;
324  xSeed = new BlockMatrix(*xSeed_);
325  }
326  else{
327  if( xSeed != 0 ) delete xSeed;
328  xSeed = 0;
329  }
330  if( xaSeed_ != 0 ){
331  if( xaSeed != 0 ) delete xaSeed;
332  xaSeed = new BlockMatrix(*xaSeed_);
333  }
334  else{
335  if( xaSeed != 0 ) delete xaSeed;
336  xaSeed = 0;
337  }
338  if( pSeed_ != 0 ){
339  if( pSeed != 0 ) delete pSeed;
340  pSeed = new BlockMatrix(*pSeed_);
341  }
342  else{
343  if( pSeed != 0 ) delete pSeed;
344  pSeed = 0;
345  }
346  if( uSeed_ != 0 ){
347  if( uSeed != 0 ) delete uSeed;
348  uSeed = new BlockMatrix(*uSeed_);
349  }
350  else{
351  if( uSeed != 0 ) delete uSeed;
352  uSeed = 0;
353  }
354  if( wSeed_ != 0 ){
355  if( wSeed != 0 ) delete wSeed;
356  wSeed = new BlockMatrix(*wSeed_);
357  }
358  else{
359  if( wSeed != 0 ) delete wSeed;
360  wSeed = 0;
361  }
362 
363  return SUCCESSFUL_RETURN;
364  }
365  if( order == 2 ){
366 
367  if( xSeed_ != 0 ){
368  if( xSeed2 != 0 ) delete xSeed2;
369  xSeed2 = new BlockMatrix(*xSeed_);
370  }
371  else{
372  if( xSeed2 != 0 ) delete xSeed2;
373  xSeed2 = 0;
374  }
375  if( xaSeed_ != 0 ){
376  if( xaSeed2 != 0 ) delete xaSeed2;
377  xaSeed2 = new BlockMatrix(*xaSeed_);
378  }
379  else{
380  if( xaSeed2 != 0 ) delete xaSeed2;
381  xaSeed2 = 0;
382  }
383  if( pSeed_ != 0 ){
384  if( pSeed2 != 0 ) delete pSeed2;
385  pSeed2 = new BlockMatrix(*pSeed_);
386  }
387  else{
388  if( pSeed2 != 0 ) delete pSeed2;
389  pSeed2 = 0;
390  }
391  if( uSeed_ != 0 ){
392  if( uSeed2 != 0 ) delete uSeed2;
393  uSeed2 = new BlockMatrix(*uSeed_);
394  }
395  else{
396  if( uSeed2 != 0 ) delete uSeed2;
397  uSeed2 = 0;
398  }
399  if( wSeed_ != 0 ){
400  if( wSeed2 != 0 ) delete wSeed2;
401  wSeed2 = new BlockMatrix(*wSeed_);
402  }
403  else{
404  if( wSeed2 != 0 ) delete wSeed2;
405  wSeed2 = 0;
406  }
407  return SUCCESSFUL_RETURN;
408  }
409 
410  return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
411 }
412 
413 
414 returnValue ObjectiveElement::setBackwardSeed( BlockMatrix *seed, int order ){
415 
416  if( order == 1 ){
417 
418  if( seed != 0 ){
419  if( bSeed != 0 ) delete bSeed;
420  bSeed = new BlockMatrix(*seed);
421  }
422  else{
423  if( bSeed != 0 ) delete bSeed;
424  bSeed = 0;
425  }
426 
427  return SUCCESSFUL_RETURN;
428  }
429  if( order == 2 ){
430 
431  if( seed != 0 ){
432  if( bSeed2 != 0 ) delete bSeed2;
433  bSeed2 = new BlockMatrix(*seed);
434  }
435  else{
436  if( bSeed2 != 0 ) delete bSeed2;
437  bSeed2 = 0;
438  }
439  return SUCCESSFUL_RETURN;
440  }
441 
442  return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
443 }
444 
445 
446 returnValue ObjectiveElement::getObjectiveValue( double &objectiveValue ){
447 
448  objectiveValue = obj;
449  return SUCCESSFUL_RETURN;
450 }
451 
452 
453 returnValue ObjectiveElement::getForwardSensitivities( BlockMatrix *D, int order ){
454 
455  ASSERT( D != 0 );
456 
457  if( order == 1 ){
458 
459  D[0] = dForward;
460  return SUCCESSFUL_RETURN;
461  }
462  if( order == 2 ){
463 
464  return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
465  }
466  return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
467 }
468 
469 
470 returnValue ObjectiveElement::getBackwardSensitivities( BlockMatrix *D, int order ){
471 
472  ASSERT( D != 0 );
473 
474  if( order == 1 ){
475  D[0] = dBackward;
476  return SUCCESSFUL_RETURN;
477  }
478  if( order == 2 ){
479 
480  return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
481  }
482  return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
483 }
484 
485 
486 
487 CLOSE_NAMESPACE_ACADO
488 
489 // end of file.
OCPiterate
Data class for storing generic optimization variables.
Definition: ocp_iterate.hpp:57
ObjectiveElement::init
returnValue init(const OCPiterate &x)
Definition: objective_element.cpp:263
BlockMatrix
Implements a very rudimentary block sparse matrix class.
Definition: block_matrix.hpp:52
ObjectiveElement
Base class for all kind of objective function terms within optimal control problems.
Definition: objective_element.hpp:55
VT_PARAMETER
Definition: acado_types.hpp:101
OCPiterate::x
VariablesGrid * x
Definition: ocp_iterate.hpp:186
ObjectiveElement::JJ
EvaluationPoint JJ
Definition: objective_element.hpp:239
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
OCPiterate::u
VariablesGrid * u
Definition: ocp_iterate.hpp:189
ObjectiveElement::getBackwardSensitivities
virtual returnValue getBackwardSensitivities(BlockMatrix *D, int order)
Definition: objective_element.cpp:470
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
VT_ALGEBRAIC_STATE
Definition: acado_types.hpp:98
Grid
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
VT_DIFFERENTIAL_STATE
Definition: acado_types.hpp:97
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
RET_INPUT_OUT_OF_RANGE
Definition: acado_types.hpp:1012
OCPiterate::xa
VariablesGrid * xa
Definition: ocp_iterate.hpp:187
EvaluationPoint::init
returnValue init(const Function &f, uint nx_=0, uint na_=0, uint np_=0, uint nu_=0, uint nw_=0, uint nd_=0, uint N_=0)
Definition: evaluation_point.cpp:94
RET_NOT_IMPLEMENTED_YET
Definition: acado_types.hpp:848
VT_TIME
Definition: acado_types.hpp:104
Function::index
int index(VariableType variableType_, int index_) const
Definition: function.cpp:176
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
ASSERT
#define ASSERT(x)
Definition: acado_debugging.hpp:51
OCPiterate::p
VariablesGrid * p
Definition: ocp_iterate.hpp:188
ObjectiveElement::getObjectiveValue
virtual returnValue getObjectiveValue(double &objectiveValue)
Definition: objective_element.cpp:446
VT_CONTROL
Definition: acado_types.hpp:99
ACADOWARNING
#define ACADOWARNING(retval)
Definition: acado_message_handling.hpp:122
OCPiterate::w
VariablesGrid * w
Definition: ocp_iterate.hpp:190
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
ObjectiveElement::setForwardSeed
virtual returnValue setForwardSeed(BlockMatrix *xSeed_, BlockMatrix *xaSeed_, BlockMatrix *pSeed_, BlockMatrix *uSeed_, BlockMatrix *wSeed_, int order)
Definition: objective_element.cpp:313
ObjectiveElement::setBackwardSeed
virtual returnValue setBackwardSeed(BlockMatrix *seed, int order)
Definition: objective_element.cpp:414
ObjectiveElement::getForwardSensitivities
virtual returnValue getForwardSensitivities(BlockMatrix *D, int order)
Definition: objective_element.cpp:453
objective_element.hpp
VT_DISTURBANCE
Definition: acado_types.hpp:103
ObjectiveElement::operator=
ObjectiveElement & operator=(const ObjectiveElement &rhs)
Definition: objective_element.cpp:181
ACADOERROR
#define ACADOERROR(retval)
Definition: acado_message_handling.hpp:106

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