point_constraint.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 
35 #include <acado/constraint/point_constraint.hpp>
36 
37 
38 
39 BEGIN_NAMESPACE_ACADO
40 
41 
42 //
43 // PUBLIC MEMBER FUNCTIONS:
44 //
45 
46 
47 PointConstraint::PointConstraint( )
48  :ConstraintElement(){
49 
50  point_index = 0;
51 
52  nb = 0;
53  var = 0;
54  index = 0;
55  blb = 0;
56  bub = 0;
57 }
58 
59 PointConstraint::PointConstraint( const Grid& grid_, int point_index_ )
60  :ConstraintElement(grid_, 1, 1 ){
61 
62  point_index = point_index_;
63 
64  nb = 0;
65  var = 0;
66  index = 0;
67  blb = 0;
68  bub = 0;
69 }
70 
71 PointConstraint::PointConstraint( const PointConstraint& rhs )
72  :ConstraintElement(rhs){
73 
74  int run1;
75  point_index = rhs.point_index;
76 
77  nb = rhs.nb;
78  if( nb > 0 ){
79  var = (VariableType*)calloc(nb,sizeof(VariableType));
80  index = (int*)calloc(nb,sizeof(int));
81  blb = (double*)calloc(nb,sizeof(double));
82  bub = (double*)calloc(nb,sizeof(double));
83  for( run1 = 0; run1 < nb; run1++ ){
84  var [run1] = rhs.var [run1];
85  index[run1] = rhs.index[run1];
86  blb [run1] = rhs.blb [run1];
87  bub [run1] = rhs.bub [run1];
88  //printf( "CON: run1: %d, index: %d!!!\n",run1,index[run1] );
89  }
90  }
91  else{
92  var = 0;
93  index = 0;
94  blb = 0;
95  bub = 0;
96  }
97 }
98 
99 PointConstraint::~PointConstraint( ){
100 
101  if( var != 0 ) free( var);
102  if( index != 0 ) free(index);
103  if( blb != 0 ) free( blb);
104  if( bub != 0 ) free( bub);
105 }
106 
107 PointConstraint& PointConstraint::operator=( const PointConstraint& rhs ){
108 
109  int run1;
110 
111  if( this != &rhs ){
112 
113  if( var != 0 ) free( var);
114  if( index != 0 ) free(index);
115  if( blb != 0 ) free( blb);
116  if( bub != 0 ) free( bub);
117 
118  ConstraintElement::operator=(rhs);
119 
120  point_index = rhs.point_index;
121 
122  nb = rhs.nb;
123  if( nb > 0 ){
124  var = (VariableType*)calloc(nb,sizeof(VariableType));
125  index = (int*)calloc(nb,sizeof(int));
126  blb = (double*)calloc(nb,sizeof(double));
127  bub = (double*)calloc(nb,sizeof(double));
128  for( run1 = 0; run1 < nb; run1++ ){
129  var [run1] = rhs.var [run1];
130  index[run1] = rhs.index[run1];
131  blb [run1] = rhs.blb [run1];
132  bub [run1] = rhs.bub [run1];
133  //printf( "CPY: run1: %d, index: %d!!!\n",run1,index[run1] );
134  }
135  }
136  else{
137  var = 0;
138  index = 0;
139  blb = 0;
140  bub = 0;
141  }
142  }
143  return *this;
144 }
145 
146 
147 
148 returnValue PointConstraint::add( const double lb_, const Expression& arg, const double ub_ ){
149 
150  if( fcn == 0 )
151  return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
152 
153  // CHECK FOR A SIMPLE BOUND:
154  // -------------------------
155 
156  VariableType varType = arg.getVariableType( );
157  int component = arg.getComponent (0);
158 
159  if( arg.isVariable() == BT_TRUE ){
160  if( varType != VT_INTERMEDIATE_STATE ){
161 
162  nb++;
163  var = (VariableType*)realloc(var , nb*sizeof(VariableType));
164  index = (int* )realloc(index, nb*sizeof(int ));
165  blb = (double* )realloc(blb , nb*sizeof(double ));
166  bub = (double* )realloc(bub , nb*sizeof(double ));
167 
168  var [nb-1] = varType ;
169  index[nb-1] = component;
170  blb [nb-1] = lb_ ;
171  bub [nb-1] = ub_ ;
172  }
173  }
174 
175  // ADD THE ARGUMENT TO THE FUNCTION TO BE EVALUATED:
176  // -------------------------------------------------
177 
178  fcn[0] << arg;
179 
180  lb[0] = (double*)realloc(lb[0],fcn[0].getDim()*sizeof(double));
181  ub[0] = (double*)realloc(ub[0],fcn[0].getDim()*sizeof(double));
182 
183  ub[0][fcn[0].getDim()-1] = ub_;
184  lb[0][fcn[0].getDim()-1] = lb_;
185 
186  return SUCCESSFUL_RETURN;
187 }
188 
189 
190 returnValue PointConstraint::evaluate( const OCPiterate& iter ){
191 
192  int run1;
193 
194  if( fcn == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
195 
196  const int nc = fcn[0].getDim();
197 
198  if( nc == 0 )
199  return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
200 
201  DMatrix resL( nc, 1 );
202  DMatrix resU( nc, 1 );
203 
204  z[0].setZ( point_index, iter );
205  DVector result = fcn[0].evaluate( z[0] );
206 
207  for( run1 = 0; run1 < nc; run1++ ){
208  resL( run1, 0 ) = lb[0][run1] - result(run1);
209  resU( run1, 0 ) = ub[0][run1] - result(run1);
210  }
211 
212  // STORE THE RESULTS:
213  // ------------------
214 
215  residuumL.init(1,1);
216  residuumU.init(1,1);
217 
218  residuumL.setDense( 0, 0, resL );
219  residuumU.setDense( 0, 0, resU );
220 
221  return SUCCESSFUL_RETURN;
222 }
223 
224 
225 returnValue PointConstraint::evaluateSensitivities( ){
226 
227 
228  // EVALUATION OF THE SENSITIVITIES:
229  // --------------------------------
230 
231  int run1;
232  returnValue returnvalue;
233 
234  if( fcn == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
235 
236  const int N = grid.getNumPoints();
237 
238  // EVALUATION OF THE SENSITIVITIES:
239  // --------------------------------
240 
241  if( bSeed != 0 ){
242 
243  if( xSeed != 0 || pSeed != 0 || uSeed != 0 || wSeed != 0 ||
244  xSeed2 != 0 || pSeed2 != 0 || uSeed2 != 0 || wSeed2 != 0 )
245  return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
246 
247  int nBDirs = bSeed->getNumRows( 0, 0 );
248 
249  DMatrix bseed_;
250  bSeed->getSubBlock( 0, 0, bseed_);
251 
252  dBackward.init( 1, 5*N );
253 
254  DMatrix Dx ( nBDirs, nx );
255  DMatrix Dxa( nBDirs, na );
256  DMatrix Dp ( nBDirs, np );
257  DMatrix Du ( nBDirs, nu );
258  DMatrix Dw ( nBDirs, nw );
259 
260  for( run1 = 0; run1 < nBDirs; run1++ )
261  {
262  ACADO_TRY( fcn[0].AD_backward( bseed_.getRow(run1), JJ[0] ) );
263 
264  if( nx > 0 ) Dx .setRow( run1, JJ[0].getX () );
265  if( na > 0 ) Dxa.setRow( run1, JJ[0].getXA() );
266  if( np > 0 ) Dp .setRow( run1, JJ[0].getP () );
267  if( nu > 0 ) Du .setRow( run1, JJ[0].getU () );
268  if( nw > 0 ) Dw .setRow( run1, JJ[0].getW () );
269 
270  JJ[0].setZero( );
271  }
272 
273  if( nx > 0 )
274  dBackward.setDense( 0, point_index , Dx );
275 
276  if( na > 0 )
277  dBackward.setDense( 0, N+point_index, Dxa );
278 
279  if( np > 0 )
280  dBackward.setDense( 0, 2*N+point_index, Dp );
281 
282  if( nu > 0 )
283  dBackward.setDense( 0, 3*N+point_index, Du );
284 
285  if( nw > 0 )
286  dBackward.setDense( 0, 4*N+point_index, Dw );
287 
288  return SUCCESSFUL_RETURN;
289  }
290 
291 
292  if( xSeed != 0 || pSeed != 0 || uSeed != 0 || wSeed != 0 ){
293 
294  if( bSeed != 0 ) return ACADOERROR( RET_WRONG_DEFINITION_OF_SEEDS );
295  if( condType != CT_SPARSE ) return ACADOERROR( RET_NOT_IMPLEMENTED_YET );
296 
297  dForward.init( 1, 5*N );
298 
299  if( xSeed != 0 ){
300  DMatrix tmp;
301  xSeed->getSubBlock(0,0,tmp);
302  returnvalue = computeForwardSensitivityBlock( 0, 0, &tmp );
303  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
304  }
305  if( xaSeed != 0 ){
306  DMatrix tmp;
307  xaSeed->getSubBlock(0,0,tmp);
308  returnvalue = computeForwardSensitivityBlock( nx, N+point_index, &tmp );
309  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
310  }
311  if( pSeed != 0 ){
312  DMatrix tmp;
313  pSeed->getSubBlock(0,0,tmp);
314  returnvalue = computeForwardSensitivityBlock( nx+na, 2*N+point_index, &tmp );
315  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
316  }
317  if( uSeed != 0 ){
318  DMatrix tmp;
319  uSeed->getSubBlock(0,0,tmp);
320  returnvalue = computeForwardSensitivityBlock( nx+na+np, 3*N+point_index, &tmp );
321  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
322  }
323  if( wSeed != 0 ){
324  DMatrix tmp;
325  wSeed->getSubBlock(0,0,tmp);
326  returnvalue = computeForwardSensitivityBlock( nx+na+np+nu, 4*N+point_index, &tmp );
327  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
328  }
329 
330  return SUCCESSFUL_RETURN;
331  }
332 
333 
334 
335 
336  return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
337 }
338 
339 
340 
341 returnValue PointConstraint::evaluateSensitivities( const DMatrix &seed, BlockMatrix &hessian ){
342 
343  // EVALUATION OF THE SENSITIVITIES:
344  // --------------------------------
345 
346  int run1, run2;
347 
348  if( fcn == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
349 
350  const int nc = fcn[0].getDim();
351  const int N = grid.getNumPoints();
352 
353  ASSERT( (int) seed.getNumRows() == nc );
354 
355  double *bseed1 = new double[nc];
356  double *bseed2 = new double[nc];
357  double *R = new double[nc];
358  double *J = new double[fcn[0].getNumberOfVariables() +1];
359  double *H = new double[fcn[0].getNumberOfVariables() +1];
360  double *fseed = new double[fcn[0].getNumberOfVariables() +1];
361 
362  for( run1 = 0; run1 < nc; run1++ ){
363  bseed1[run1] = seed(run1,0);
364  bseed2[run1] = 0.0;
365  }
366 
367  for( run1 = 0; run1 < fcn[0].getNumberOfVariables()+1; run1++ )
368  fseed[run1] = 0.0;
369 
370  dBackward.init( 1, 5*N );
371 
372  DMatrix Dx ( nc, nx );
373  DMatrix Dxa( nc, na );
374  DMatrix Dp ( nc, np );
375  DMatrix Du ( nc, nu );
376  DMatrix Dw ( nc, nw );
377 
378  DMatrix Hx ( nx, nx );
379  DMatrix Hxa( nx, na );
380  DMatrix Hp ( nx, np );
381  DMatrix Hu ( nx, nu );
382  DMatrix Hw ( nx, nw );
383 
384  for( run2 = 0; run2 < nx; run2++ ){
385 
386  // FIRST ORDER DERIVATIVES:
387  // ------------------------
388  fseed[y_index[0][run2]] = 1.0;
389  fcn[0].AD_forward( 0, fseed, R );
390  for( run1 = 0; run1 < nc; run1++ )
391  Dx( run1, run2 ) = R[run1];
392  fseed[y_index[0][run2]] = 0.0;
393 
394  // SECOND ORDER DERIVATIVES:
395  // -------------------------
396  for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
397  J[run1] = 0.0;
398  H[run1] = 0.0;
399  }
400 
401  fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
402 
403  for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2, run1 ) = -H[y_index[0][run1]];
404  for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2, run1-nx ) = -H[y_index[0][run1]];
405  for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2, run1-nx-na ) = -H[y_index[0][run1]];
406  for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2, run1-nx-na-np ) = -H[y_index[0][run1]];
407  for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
408  }
409 
410  if( nx > 0 ){
411 
412  dBackward.setDense( 0, point_index, Dx );
413 
414  if( nx > 0 ) hessian.addDense( point_index, point_index, Hx );
415  if( na > 0 ) hessian.addDense( point_index, N + point_index, Hxa );
416  if( np > 0 ) hessian.addDense( point_index, 2*N + point_index, Hp );
417  if( nu > 0 ) hessian.addDense( point_index, 3*N + point_index, Hu );
418  if( nw > 0 ) hessian.addDense( point_index, 4*N + point_index, Hw );
419  }
420 
421  Hx.init ( na, nx );
422  Hxa.init( na, na );
423  Hp.init ( na, np );
424  Hu.init ( na, nu );
425  Hw.init ( na, nw );
426 
427  for( run2 = nx; run2 < nx+na; run2++ ){
428 
429  // FIRST ORDER DERIVATIVES:
430  // ------------------------
431  fseed[y_index[0][run2]] = 1.0;
432  fcn[0].AD_forward( 0, fseed, R );
433  for( run1 = 0; run1 < nc; run1++ )
434  Dxa( run1, run2-nx ) = R[run1];
435  fseed[y_index[0][run2]] = 0.0;
436 
437  // SECOND ORDER DERIVATIVES:
438  // -------------------------
439  for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
440  J[run1] = 0.0;
441  H[run1] = 0.0;
442  }
443 
444  fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
445 
446  for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx, run1 ) = -H[y_index[0][run1]];
447  for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx, run1-nx ) = -H[y_index[0][run1]];
448  for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx, run1-nx-na ) = -H[y_index[0][run1]];
449  for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx, run1-nx-na-np ) = -H[y_index[0][run1]];
450  for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
451  }
452 
453  if( na > 0 ){
454 
455  dBackward.setDense( 0, N+point_index, Dxa );
456 
457  if( nx > 0 ) hessian.addDense( N+point_index, point_index, Hx );
458  if( na > 0 ) hessian.addDense( N+point_index, N + point_index, Hxa );
459  if( np > 0 ) hessian.addDense( N+point_index, 2*N + point_index, Hp );
460  if( nu > 0 ) hessian.addDense( N+point_index, 3*N + point_index, Hu );
461  if( nw > 0 ) hessian.addDense( N+point_index, 4*N + point_index, Hw );
462  }
463 
464  Hx.init ( np, nx );
465  Hxa.init( np, na );
466  Hp.init ( np, np );
467  Hu.init ( np, nu );
468  Hw.init ( np, nw );
469 
470  for( run2 = nx+na; run2 < nx+na+np; run2++ ){
471 
472  // FIRST ORDER DERIVATIVES:
473  // ------------------------
474  fseed[y_index[0][run2]] = 1.0;
475  fcn[0].AD_forward( 0, fseed, R );
476  for( run1 = 0; run1 < nc; run1++ )
477  Dp( run1, run2-nx-na ) = R[run1];
478  fseed[y_index[0][run2]] = 0.0;
479 
480  // SECOND ORDER DERIVATIVES:
481  // -------------------------
482  for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
483  J[run1] = 0.0;
484  H[run1] = 0.0;
485  }
486 
487  fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
488 
489  for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na, run1 ) = -H[y_index[0][run1]];
490  for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na, run1-nx ) = -H[y_index[0][run1]];
491  for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na, run1-nx-na ) = -H[y_index[0][run1]];
492  for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na, run1-nx-na-np ) = -H[y_index[0][run1]];
493  for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
494  }
495 
496  if( np > 0 ){
497 
498  dBackward.setDense( 0, 2*N+point_index, Dp );
499 
500  if( nx > 0 ) hessian.addDense( 2*N+point_index, point_index, Hx );
501  if( na > 0 ) hessian.addDense( 2*N+point_index, N + point_index, Hxa );
502  if( np > 0 ) hessian.addDense( 2*N+point_index, 2*N + point_index, Hp );
503  if( nu > 0 ) hessian.addDense( 2*N+point_index, 3*N + point_index, Hu );
504  if( nw > 0 ) hessian.addDense( 2*N+point_index, 4*N + point_index, Hw );
505  }
506 
507 
508  Hx.init ( nu, nx );
509  Hxa.init( nu, na );
510  Hp.init ( nu, np );
511  Hu.init ( nu, nu );
512  Hw.init ( nu, nw );
513 
514  for( run2 = nx+na+np; run2 < nx+na+np+nu; run2++ ){
515 
516  // FIRST ORDER DERIVATIVES:
517  // ------------------------
518  fseed[y_index[0][run2]] = 1.0;
519  fcn[0].AD_forward( 0, fseed, R );
520  for( run1 = 0; run1 < nc; run1++ )
521  Du( run1, run2-nx-na-np ) = R[run1];
522  fseed[y_index[0][run2]] = 0.0;
523 
524  // SECOND ORDER DERIVATIVES:
525  // -------------------------
526  for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
527  J[run1] = 0.0;
528  H[run1] = 0.0;
529  }
530 
531  fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
532 
533  for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na-np, run1 ) = -H[y_index[0][run1]];
534  for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na-np, run1-nx ) = -H[y_index[0][run1]];
535  for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na-np, run1-nx-na ) = -H[y_index[0][run1]];
536  for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na-np, run1-nx-na-np ) = -H[y_index[0][run1]];
537  for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na-np, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
538  }
539 
540  if( nu > 0 ){
541 
542  dBackward.setDense( 0, 3*N+point_index, Du );
543 
544  if( nx > 0 ) hessian.addDense( 3*N+point_index, point_index, Hx );
545  if( na > 0 ) hessian.addDense( 3*N+point_index, N + point_index, Hxa );
546  if( np > 0 ) hessian.addDense( 3*N+point_index, 2*N + point_index, Hp );
547  if( nu > 0 ) hessian.addDense( 3*N+point_index, 3*N + point_index, Hu );
548  if( nw > 0 ) hessian.addDense( 3*N+point_index, 4*N + point_index, Hw );
549  }
550 
551  Hx.init ( nw, nx );
552  Hxa.init( nw, na );
553  Hp.init ( nw, np );
554  Hu.init ( nw, nu );
555  Hw.init ( nw, nw );
556 
557  for( run2 = nx+na+np+nu; run2 < nx+na+np+nu+nw; run2++ ){
558 
559  // FIRST ORDER DERIVATIVES:
560  // ------------------------
561  fseed[y_index[0][run2]] = 1.0;
562  fcn[0].AD_forward( 0, fseed, R );
563  for( run1 = 0; run1 < nc; run1++ )
564  Dw( run1, run2-nx-na-np-nu ) = R[run1];
565  fseed[y_index[0][run2]] = 0.0;
566 
567  // SECOND ORDER DERIVATIVES:
568  // -------------------------
569  for( run1 = 0; run1 <= fcn[0].getNumberOfVariables(); run1++ ){
570  J[run1] = 0.0;
571  H[run1] = 0.0;
572  }
573 
574  fcn[0].AD_backward2( 0, bseed1, bseed2, J, H );
575 
576  for( run1 = 0 ; run1 < nx ; run1++ ) Hx ( run2-nx-na-np-nu, run1 ) = -H[y_index[0][run1]];
577  for( run1 = nx ; run1 < nx+na ; run1++ ) Hxa( run2-nx-na-np-nu, run1-nx ) = -H[y_index[0][run1]];
578  for( run1 = nx+na ; run1 < nx+na+np ; run1++ ) Hp ( run2-nx-na-np-nu, run1-nx-na ) = -H[y_index[0][run1]];
579  for( run1 = nx+na+np ; run1 < nx+na+np+nu ; run1++ ) Hu ( run2-nx-na-np-nu, run1-nx-na-np ) = -H[y_index[0][run1]];
580  for( run1 = nx+na+np+nu; run1 < nx+na+np+nu+nw; run1++ ) Hw ( run2-nx-na-np-nu, run1-nx-na-np-nu ) = -H[y_index[0][run1]];
581  }
582 
583  if( nw > 0 ){
584 
585  dBackward.setDense( 0, 4*N+point_index, Dw );
586 
587  if( nx > 0 ) hessian.addDense( 4*N+point_index, point_index, Hx );
588  if( na > 0 ) hessian.addDense( 4*N+point_index, N + point_index, Hxa );
589  if( np > 0 ) hessian.addDense( 4*N+point_index, 2*N + point_index, Hp );
590  if( nu > 0 ) hessian.addDense( 4*N+point_index, 3*N + point_index, Hu );
591  if( nw > 0 ) hessian.addDense( 4*N+point_index, 4*N + point_index, Hw );
592  }
593 
594  delete[] bseed1;
595  delete[] bseed2;
596  delete[] R ;
597  delete[] J ;
598  delete[] H ;
599  delete[] fseed ;
600 
601  return SUCCESSFUL_RETURN;
602 }
603 
604 
605 returnValue PointConstraint::getBounds( const OCPiterate& iter ){
606 
607 
608  uint run1, run2;
609 
610  //printf( "%d!!!\n",nb );
611 
612  for( run1 = 0; (int) run1 < nb; run1++ ){
613 
614  switch( var[run1] ){
615 
616  case VT_DIFFERENTIAL_STATE:
617  if( iter.x != NULL ){
618  run2 = iter.x->getFloorIndex(grid.getTime(point_index));
619  //printf( "run2: %d, index: %d!!!\n",run2,index[run1] );
620  iter.x->setLowerBound(run2,index[run1],blb[run1]);
621  iter.x->setUpperBound(run2,index[run1],bub[run1]);
622  }
623  else ACADOERROR( RET_INVALID_ARGUMENTS );
624  break;
625 
626  case VT_ALGEBRAIC_STATE:
627  if( iter.xa != NULL ){
628  run2 = iter.xa->getFloorIndex(grid.getTime(point_index));
629  iter.xa->setLowerBound(run2,index[run1],blb[run1]);
630  iter.xa->setUpperBound(run2,index[run1],bub[run1]);
631  }
632  else ACADOERROR( RET_INVALID_ARGUMENTS );
633  break;
634 
635  case VT_PARAMETER:
636  if( iter.p != NULL ){
637  run2 = iter.p->getFloorIndex(grid.getTime(point_index));
638  iter.p->setLowerBound(0,index[run1],blb[run1]);
639  iter.p->setUpperBound(0,index[run1],bub[run1]);
640  }
641  else ACADOERROR( RET_INVALID_ARGUMENTS );
642  break;
643 
644  case VT_CONTROL:
645  if( iter.u != NULL ){
646  run2 = iter.u->getFloorIndex(grid.getTime(point_index));
647  iter.u->setLowerBound(run2,index[run1],blb[run1]);
648  iter.u->setUpperBound(run2,index[run1],bub[run1]);
649  }
650  else ACADOERROR( RET_INVALID_ARGUMENTS );
651  break;
652 
653  case VT_DISTURBANCE:
654  if( iter.w != NULL ){
655  run2 = iter.w->getFloorIndex(grid.getTime(point_index));
656  iter.w->setLowerBound(run2,index[run1],blb[run1]);
657  iter.w->setUpperBound(run2,index[run1],bub[run1]);
658  }
659  else ACADOERROR( RET_INVALID_ARGUMENTS );
660  break;
661 
662  default:
663  ACADOERROR(RET_NOT_IMPLEMENTED_YET);
664  break;
665  }
666  }
667 
668  return SUCCESSFUL_RETURN;
669 }
670 
671 
672 
673 //
674 // PROTECTED MEMBER FUNCTIONS:
675 //
676 
677 inline returnValue PointConstraint::computeForwardSensitivityBlock( int offset, int offset2, DMatrix *seed ){
678 
679  if( seed == 0 ) return SUCCESSFUL_RETURN;
680 
681  int run1, run2;
682  returnValue returnvalue;
683 
684  const int nc = fcn[0].getDim();
685 
686  double* dresult1 = new double[nc ];
687  double* fseed1 = new double[fcn[0].getNumberOfVariables()+1];
688 
689  DMatrix tmp( nc, seed->getNumCols() );
690 
691  for( run1 = 0; run1 < (int) seed->getNumCols(); run1++ ){
692 
693  for( run2 = 0; run2 <= fcn[0].getNumberOfVariables(); run2++ )
694  fseed1[run2] = 0.0;
695 
696  for( run2 = 0; run2 < (int) seed->getNumRows(); run2++ )
697  fseed1[y_index[0][offset+run2]] = seed->operator()(run2,run1);
698 
699  returnvalue = fcn[0].AD_forward( 0, fseed1, dresult1 );
700  if( returnvalue != SUCCESSFUL_RETURN ){
701  delete[] dresult1;
702  delete[] fseed1 ;
703  return ACADOERROR(returnvalue);
704  }
705  for( run2 = 0; run2 < nc; run2++ )
706  tmp( run2, run1 ) = dresult1[run2];
707  }
708  dForward.setDense( 0, offset2, tmp );
709 
710  delete[] dresult1;
711  delete[] fseed1 ;
712 
713  return SUCCESSFUL_RETURN;
714 }
715 
716 
717 
718 CLOSE_NAMESPACE_ACADO
719 
720 // end of file.
N
#define N
Definition: crane_kul_mhe_test.cpp:44
Expression::isVariable
BooleanType isVariable() const
PointConstraint::add
returnValue add(const double lb_, const Expression &arg, const double ub_)
Definition: point_constraint.cpp:148
OCPiterate
Data class for storing generic optimization variables.
Definition: ocp_iterate.hpp:57
BlockMatrix
Implements a very rudimentary block sparse matrix class.
Definition: block_matrix.hpp:52
VT_PARAMETER
Definition: acado_types.hpp:101
ConstraintElement::z
EvaluationPoint * z
Definition: constraint_element.hpp:251
OCPiterate::x
VariablesGrid * x
Definition: ocp_iterate.hpp:186
ConstraintElement::operator=
ConstraintElement & operator=(const ConstraintElement &rhs)
Definition: constraint_element.cpp:306
GenericMatrix::init
void init(unsigned _nRows=0, unsigned _nCols=0)
Definition: matrix.hpp:135
PointConstraint::var
VariableType * var
Definition: point_constraint.hpp:155
Grid::getTime
double getTime(uint pointIdx) const
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
OCPiterate::u
VariablesGrid * u
Definition: ocp_iterate.hpp:189
RET_INVALID_ARGUMENTS
Definition: acado_types.hpp:839
PointConstraint::evaluate
returnValue evaluate(const OCPiterate &iter)
Definition: point_constraint.cpp:190
PointConstraint::~PointConstraint
virtual ~PointConstraint()
Definition: point_constraint.cpp:99
BlockMatrix::setDense
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
Definition: block_matrix.cpp:446
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
PointConstraint::evaluateSensitivities
returnValue evaluateSensitivities()
Definition: point_constraint.cpp:225
VT_ALGEBRAIC_STATE
Definition: acado_types.hpp:98
VT_INTERMEDIATE_STATE
Definition: acado_types.hpp:105
EvaluationPoint::setZ
returnValue setZ(const uint &idx, const OCPiterate &iter)
uint
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
Grid
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
CT_SPARSE
Definition: acado_types.hpp:597
VT_DIFFERENTIAL_STATE
Definition: acado_types.hpp:97
PointConstraint
Stores and evaluates pointwise constraints within optimal control problems.
Definition: point_constraint.hpp:55
MatrixVariablesGrid::setLowerBound
returnValue setLowerBound(uint pointIdx, uint valueIdx, double _lb)
Definition: matrix_variables_grid.cpp:1376
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
VariableType
VariableType
Definition: acado_types.hpp:95
point_constraint.hpp
Grid::getFloorIndex
uint getFloorIndex(double time) const
Definition: grid.cpp:593
OCPiterate::xa
VariablesGrid * xa
Definition: ocp_iterate.hpp:187
ACADO_TRY
#define ACADO_TRY(X)
Definition: acado_message_handling.hpp:144
Expression
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
PointConstraint::computeForwardSensitivityBlock
returnValue computeForwardSensitivityBlock(int offset, int offset2, DMatrix *seed)
Definition: point_constraint.cpp:677
RET_MEMBER_NOT_INITIALISED
Definition: acado_types.hpp:851
BlockMatrix::getSubBlock
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
ConstraintElement
Base class for all kind of constraints (except for bounds) within optimal control problems...
Definition: constraint_element.hpp:57
RET_NOT_IMPLEMENTED_YET
Definition: acado_types.hpp:848
BlockMatrix::init
returnValue init(uint _nRows, uint _nCols)
Definition: block_matrix.cpp:65
ConstraintElement::JJ
EvaluationPoint * JJ
Definition: constraint_element.hpp:252
Function::getDim
int getDim() const
MatrixVariablesGrid::setUpperBound
returnValue setUpperBound(uint pointIdx, uint valueIdx, double _ub)
Definition: matrix_variables_grid.cpp:1425
GenericMatrix::setRow
GenericMatrix & setRow(unsigned _idx, const GenericVector< T > &_values)
Definition: matrix.hpp:213
EvaluationPoint::setZero
returnValue setZero()
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
GenericMatrix::getNumRows
unsigned getNumRows() const
Definition: matrix.hpp:185
PointConstraint::operator=
PointConstraint & operator=(const PointConstraint &rhs)
Definition: point_constraint.cpp:107
ASSERT
#define ASSERT(x)
Definition: acado_debugging.hpp:51
OCPiterate::p
VariablesGrid * p
Definition: ocp_iterate.hpp:188
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
GenericMatrix::getNumCols
unsigned getNumCols() const
Definition: matrix.hpp:189
Expression::getVariableType
VariableType getVariableType() const
Grid::getNumPoints
uint getNumPoints() const
VT_CONTROL
Definition: acado_types.hpp:99
Expression::getComponent
uint getComponent(const unsigned int idx) const
OCPiterate::w
VariablesGrid * w
Definition: ocp_iterate.hpp:190
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
RET_WRONG_DEFINITION_OF_SEEDS
Definition: acado_types.hpp:1005
PointConstraint::getBounds
returnValue getBounds(const OCPiterate &iter)
Definition: point_constraint.cpp:605
BlockMatrix::addDense
returnValue addDense(uint rowIdx, uint colIdx, const DMatrix &value)
Definition: block_matrix.cpp:458
VT_DISTURBANCE
Definition: acado_types.hpp:103
R
#define R
Function::getNumberOfVariables
int getNumberOfVariables() const
Definition: function.cpp:264
GenericMatrix::getRow
GenericVector< T > getRow(unsigned _idx) const
Definition: matrix.hpp:197
ACADOERROR
#define ACADOERROR(retval)
Definition: acado_message_handling.hpp:106
BlockMatrix::getNumRows
uint getNumRows() const

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