mayer_term.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/mayer_term.hpp>
35 
36 
37 
38 BEGIN_NAMESPACE_ACADO
39 
40 
41 
42 //
43 // PUBLIC MEMBER FUNCTIONS:
44 //
45 
46 
47 MayerTerm::MayerTerm( ){ }
48 
49 
50 MayerTerm::MayerTerm( const Grid &grid_, const Expression& arg )
51  :ObjectiveElement(grid_){
52 
53  fcn << arg;
54 }
55 
56 MayerTerm::MayerTerm( const Grid &grid_, const Function& arg )
57  :ObjectiveElement(grid_){
58 
59  fcn = arg;
60 }
61 
62 MayerTerm::MayerTerm( const MayerTerm& rhs )
63  :ObjectiveElement(rhs){ }
64 
65 MayerTerm::~MayerTerm( ){ }
66 
67 
68 MayerTerm& MayerTerm::operator=( const MayerTerm& rhs ){
69 
70  if( this != &rhs ){
71 
72  ObjectiveElement::operator=(rhs);
73  }
74  return *this;
75 }
76 
77 
78 
79 returnValue MayerTerm::evaluate( const OCPiterate &x ){
80 
81  ObjectiveElement::init( x );
82 
83  z.setZ( grid.getLastIndex(), x );
84 
85  DVector objective = fcn.evaluate( z );
86 
87  obj = objective(0);
88 
89  return SUCCESSFUL_RETURN;
90 }
91 
92 
93 returnValue MayerTerm::evaluateSensitivities( BlockMatrix *hessian ){
94 
95  int run1, run2;
96  const int N = grid.getNumPoints();
97 
98  if( (bSeed != 0) & (hessian == 0) ){
99 
100  DMatrix bseed_;
101  bSeed->getSubBlock( 0, 0, bseed_);
102 
103  fcn.AD_backward( bseed_.getRow(0), JJ );
104 
105  dBackward.init( 1, 5*N );
106 
107  DMatrix tmp1 = JJ.getX(); tmp1.transposeInPlace();
108  if( nx > 0 ) dBackward.setDense( 0, N-1, tmp1 );
109  DMatrix tmp2 = JJ.getXA(); tmp2.transposeInPlace();
110  if( na > 0 ) dBackward.setDense( 0, 2*N-1, tmp2 );
111  DMatrix tmp3 = JJ.getP(); tmp3.transposeInPlace();
112  if( np > 0 ) dBackward.setDense( 0, 3*N-1, tmp3 );
113  DMatrix tmp4 = JJ.getU(); tmp4.transposeInPlace();
114  if( nu > 0 ) dBackward.setDense( 0, 4*N-1, tmp4 );
115  DMatrix tmp5 = JJ.getW(); tmp5.transposeInPlace();
116  if( nw > 0 ) dBackward.setDense( 0, 5*N-1, tmp5 );
117 
118  return SUCCESSFUL_RETURN;
119  }
120 
121  if( hessian != 0 ){
122 
123  double bseed1 = 1.0;
124  double bseed2 = 0.0;
125  double *J = new double[fcn.getNumberOfVariables() +1];
126  double *H = new double[fcn.getNumberOfVariables() +1];
127  double *fseed = new double[fcn.getNumberOfVariables() +1];
128 
129  for( run1 = 0; run1 < fcn.getNumberOfVariables()+1; run1++ )
130  fseed[run1] = 0.0;
131 
132  dBackward.init( 1, 5*N );
133 
134  DMatrix Dx ( 1, nx );
135  DMatrix Dxa( 1, na );
136  DMatrix Dp ( 1, np );
137  DMatrix Du ( 1, nu );
138  DMatrix Dw ( 1, nw );
139 
140  DMatrix Hx ( nx, nx );
141  DMatrix Hxa( nx, na );
142  DMatrix Hp ( nx, np );
143  DMatrix Hu ( nx, nu );
144  DMatrix Hw ( nx, nw );
145 
146  for( run2 = 0; run2 < nx; run2++ ){
147 
148  // FIRST ORDER DERIVATIVES:
149  // ------------------------
150  fseed[y_index[run2]] = 1.0;
151  fcn.AD_forward( 0, fseed, J );
152  Dx( 0, run2 ) = J[0];
153  fseed[y_index[run2]] = 0.0;
154 
155  // SECOND ORDER DERIVATIVES:
156  // -------------------------
157  for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
158  J[run1] = 0.0;
159  H[run1] = 0.0;
160  }
161 
162  fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
163 
164  for( run1 = 0; run1 < nx; run1++ ){
165  Hx( run2, run1 ) = H[y_index[run1]];
166  }
167  for( run1 = nx; run1 < nx+na; run1++ ){
168  Hxa( run2, run1-nx ) = H[y_index[run1]];
169  }
170  for( run1 = nx+na; run1 < nx+na+np; run1++ ){
171  Hp( run2, run1-nx-na ) = H[y_index[run1]];
172  }
173  for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
174  Hu( run2, run1-nx-na-np ) = H[y_index[run1]];
175  }
176  for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
177  Hw( run2, run1-nx-na-np-nu ) = H[y_index[run1]];
178  }
179  }
180 
181  if( nx > 0 ){
182 
183  dBackward.setDense( 0, N-1, Dx );
184 
185  if( nx > 0 ) hessian->setDense( N-1, N-1, Hx );
186  if( na > 0 ) hessian->setDense( N-1, 2*N-1, Hxa );
187  if( np > 0 ) hessian->setDense( N-1, 3*N-1, Hp );
188  if( nu > 0 ) hessian->setDense( N-1, 4*N-1, Hu );
189  if( nw > 0 ) hessian->setDense( N-1, 5*N-1, Hw );
190  }
191 
192  Hx.init ( na, nx );
193  Hxa.init( na, na );
194  Hp.init ( na, np );
195  Hu.init ( na, nu );
196  Hw.init ( na, nw );
197 
198  for( run2 = nx; run2 < nx+na; run2++ ){
199 
200  // FIRST ORDER DERIVATIVES:
201  // ------------------------
202  fseed[y_index[run2]] = 1.0;
203  fcn.AD_forward( 0, fseed, J );
204  Dxa( 0, run2-nx ) = J[0];
205  fseed[y_index[run2]] = 0.0;
206 
207  // SECOND ORDER DERIVATIVES:
208  // -------------------------
209  for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
210  J[run1] = 0.0;
211  H[run1] = 0.0;
212  }
213 
214  fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
215 
216  for( run1 = 0; run1 < nx; run1++ ){
217  Hx( run2-nx, run1 ) = H[y_index[run1]];
218  }
219  for( run1 = nx; run1 < nx+na; run1++ ){
220  Hxa( run2-nx, run1-nx ) = H[y_index[run1]];
221  }
222  for( run1 = nx+na; run1 < nx+na+np; run1++ ){
223  Hp( run2-nx, run1-nx-na ) = H[y_index[run1]];
224  }
225  for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
226  Hu( run2-nx, run1-nx-na-np ) = H[y_index[run1]];
227  }
228  for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
229  Hw( run2-nx, run1-nx-na-np-nu ) = H[y_index[run1]];
230  }
231  }
232 
233  if( na > 0 ){
234 
235  dBackward.setDense( 0, 2*N-1, Dxa );
236 
237  if( nx > 0 ) hessian->setDense( 2*N-1, N-1, Hx );
238  if( na > 0 ) hessian->setDense( 2*N-1, 2*N-1, Hxa );
239  if( np > 0 ) hessian->setDense( 2*N-1, 3*N-1, Hp );
240  if( nu > 0 ) hessian->setDense( 2*N-1, 4*N-1, Hu );
241  if( nw > 0 ) hessian->setDense( 2*N-1, 5*N-1, Hw );
242  }
243 
244  Hx.init ( np, nx );
245  Hxa.init( np, na );
246  Hp.init ( np, np );
247  Hu.init ( np, nu );
248  Hw.init ( np, nw );
249 
250  for( run2 = nx+na; run2 < nx+na+np; run2++ ){
251 
252  // FIRST ORDER DERIVATIVES:
253  // ------------------------
254  fseed[y_index[run2]] = 1.0;
255  fcn.AD_forward( 0, fseed, J );
256  Dp( 0, run2-nx-na ) = J[0];
257  fseed[y_index[run2]] = 0.0;
258 
259  // SECOND ORDER DERIVATIVES:
260  // -------------------------
261  for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
262  J[run1] = 0.0;
263  H[run1] = 0.0;
264  }
265 
266  fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
267 
268  for( run1 = 0; run1 < nx; run1++ ){
269  Hx( run2-nx-na, run1 ) = H[y_index[run1]];
270  }
271  for( run1 = nx; run1 < nx+na; run1++ ){
272  Hxa( run2-nx-na, run1-nx ) = H[y_index[run1]];
273  }
274  for( run1 = nx+na; run1 < nx+na+np; run1++ ){
275  Hp( run2-nx-na, run1-nx-na ) = H[y_index[run1]];
276  }
277  for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
278  Hu( run2-nx-na, run1-nx-na-np ) = H[y_index[run1]];
279  }
280  for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
281  Hw( run2-nx-na, run1-nx-na-np-nu ) = H[y_index[run1]];
282  }
283  }
284 
285  if( np > 0 ){
286 
287  dBackward.setDense( 0, 3*N-1, Dp );
288 
289  if( nx > 0 ) hessian->setDense( 3*N-1, N-1, Hx );
290  if( na > 0 ) hessian->setDense( 3*N-1, 2*N-1, Hxa );
291  if( np > 0 ) hessian->setDense( 3*N-1, 3*N-1, Hp );
292  if( nu > 0 ) hessian->setDense( 3*N-1, 4*N-1, Hu );
293  if( nw > 0 ) hessian->setDense( 3*N-1, 5*N-1, Hw );
294  }
295 
296 
297  Hx.init ( nu, nx );
298  Hxa.init( nu, na );
299  Hp.init ( nu, np );
300  Hu.init ( nu, nu );
301  Hw.init ( nu, nw );
302 
303  for( run2 = nx+na+np; run2 < nx+na+np+nu; run2++ ){
304 
305  // FIRST ORDER DERIVATIVES:
306  // ------------------------
307  fseed[y_index[run2]] = 1.0;
308  fcn.AD_forward( 0, fseed, J );
309  Du( 0, run2-nx-na-np ) = J[0];
310  fseed[y_index[run2]] = 0.0;
311 
312  // SECOND ORDER DERIVATIVES:
313  // -------------------------
314  for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
315  J[run1] = 0.0;
316  H[run1] = 0.0;
317  }
318 
319  fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
320 
321  for( run1 = 0; run1 < nx; run1++ ){
322  Hx( run2-nx-na-np, run1 ) = H[y_index[run1]];
323  }
324  for( run1 = nx; run1 < nx+na; run1++ ){
325  Hxa( run2-nx-na-np, run1-nx ) = H[y_index[run1]];
326  }
327  for( run1 = nx+na; run1 < nx+na+np; run1++ ){
328  Hp( run2-nx-na-np, run1-nx-na ) = H[y_index[run1]];
329  }
330  for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
331  Hu( run2-nx-na-np, run1-nx-na-np ) = H[y_index[run1]];
332  }
333  for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
334  Hw( run2-nx-na-np, run1-nx-na-np-nu ) = H[y_index[run1]];
335  }
336  }
337 
338  if( nu > 0 ){
339 
340  dBackward.setDense( 0, 4*N-1, Du );
341 
342  if( nx > 0 ) hessian->setDense( 4*N-1, N-1, Hx );
343  if( na > 0 ) hessian->setDense( 4*N-1, 2*N-1, Hxa );
344  if( np > 0 ) hessian->setDense( 4*N-1, 3*N-1, Hp );
345  if( nu > 0 ) hessian->setDense( 4*N-1, 4*N-1, Hu );
346  if( nw > 0 ) hessian->setDense( 4*N-1, 5*N-1, Hw );
347  }
348 
349 
350  Hx.init ( nw, nx );
351  Hxa.init( nw, na );
352  Hp.init ( nw, np );
353  Hu.init ( nw, nu );
354  Hw.init ( nw, nw );
355 
356  for( run2 = nx+na+np+nu; run2 < nx+na+np+nu+nw; run2++ ){
357 
358  // FIRST ORDER DERIVATIVES:
359  // ------------------------
360  fseed[y_index[run2]] = 1.0;
361  fcn.AD_forward( 0, fseed, J );
362  Dw( 0, run2-nx-na-np-nu ) = J[0];
363  fseed[y_index[run2]] = 0.0;
364 
365  // SECOND ORDER DERIVATIVES:
366  // -------------------------
367  for( run1 = 0; run1 <= fcn.getNumberOfVariables(); run1++ ){
368  J[run1] = 0.0;
369  H[run1] = 0.0;
370  }
371 
372  fcn.AD_backward2( 0, &bseed1, &bseed2, J, H );
373 
374  for( run1 = 0; run1 < nx; run1++ ){
375  Hx( run2-nx-na-np-nu, run1 ) = H[y_index[run1]];
376  }
377  for( run1 = nx; run1 < nx+na; run1++ ){
378  Hxa( run2-nx-na-np-nu, run1-nx ) = H[y_index[run1]];
379  }
380  for( run1 = nx+na; run1 < nx+na+np; run1++ ){
381  Hp( run2-nx-na-np-nu, run1-nx-na ) = H[y_index[run1]];
382  }
383  for( run1 = nx+na+np; run1 < nx+na+np+nu; run1++ ){
384  Hu( run2-nx-na-np-nu, run1-nx-na-np ) = H[y_index[run1]];
385  }
386  for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ){
387  Hw( run2-nx-na-np-nu, run1-nx-na-np-nu ) = H[y_index[run1]];
388  }
389  }
390 
391  if( nw > 0 ){
392 
393  dBackward.setDense( 0, 5*N-1, Dw );
394 
395  if( nx > 0 ) hessian->setDense( 5*N-1, N-1, Hx );
396  if( na > 0 ) hessian->setDense( 5*N-1, 2*N-1, Hxa );
397  if( np > 0 ) hessian->setDense( 5*N-1, 3*N-1, Hp );
398  if( nu > 0 ) hessian->setDense( 5*N-1, 4*N-1, Hu );
399  if( nw > 0 ) hessian->setDense( 5*N-1, 5*N-1, Hw );
400  }
401 
402  delete[] J;
403  delete[] H;
404  delete[] fseed;
405 
406  return SUCCESSFUL_RETURN;
407  }
408 
409  return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
410 }
411 
412 
413 
414 
415 
416 CLOSE_NAMESPACE_ACADO
417 
418 // end of file.
N
#define N
Definition: crane_kul_mhe_test.cpp:44
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
Function
Allows to setup and evaluate a general function based on SymbolicExpressions.
Definition: function_.hpp:59
GenericMatrix::init
void init(unsigned _nRows=0, unsigned _nCols=0)
Definition: matrix.hpp:135
ObjectiveElement::JJ
EvaluationPoint JJ
Definition: objective_element.hpp:239
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
MayerTerm::evaluateSensitivities
returnValue evaluateSensitivities(BlockMatrix *hessian)
Definition: mayer_term.cpp:93
BlockMatrix::setDense
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
Definition: block_matrix.cpp:446
MayerTerm
Stores and evaluates Mayer terms within optimal control problems.
Definition: mayer_term.hpp:57
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
Function::evaluate
DVector evaluate(const EvaluationPoint &x, const int &number=0)
Definition: function.cpp:520
EvaluationPoint::getP
DVector getP() const
EvaluationPoint::setZ
returnValue setZ(const uint &idx, const OCPiterate &iter)
Grid
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
MayerTerm::evaluate
returnValue evaluate(const OCPiterate &x)
Definition: mayer_term.cpp:79
Grid::getLastIndex
uint getLastIndex() const
Function::AD_backward2
returnValue AD_backward2(int number, double *seed1, double *seed2, double *df, double *ddf)
Definition: function.cpp:448
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
EvaluationPoint::getW
DVector getW() const
Expression
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
mayer_term.hpp
BlockMatrix::getSubBlock
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
EvaluationPoint::getX
DVector getX() const
RET_NOT_IMPLEMENTED_YET
Definition: acado_types.hpp:848
BlockMatrix::init
returnValue init(uint _nRows, uint _nCols)
Definition: block_matrix.cpp:65
EvaluationPoint::getU
DVector getU() const
EvaluationPoint::getXA
DVector getXA() const
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
Function::AD_backward
returnValue AD_backward(const DVector &seed, EvaluationPoint &df, const int &number=0)
Definition: function.cpp:546
Function::AD_forward
DVector AD_forward(const EvaluationPoint &x, const int &number=0)
Definition: function.cpp:533
Grid::getNumPoints
uint getNumPoints() const
MayerTerm::operator=
MayerTerm & operator=(const MayerTerm &rhs)
Definition: mayer_term.cpp:68
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
MayerTerm::~MayerTerm
virtual ~MayerTerm()
Definition: mayer_term.cpp:65
Function::getNumberOfVariables
int getNumberOfVariables() const
Definition: function.cpp:264
ObjectiveElement::operator=
ObjectiveElement & operator=(const ObjectiveElement &rhs)
Definition: objective_element.cpp:181
GenericMatrix::getRow
GenericVector< T > getRow(unsigned _idx) const
Definition: matrix.hpp:197
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:53