constraint_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/constraint/constraint_element.hpp>
35 
36 
37 BEGIN_NAMESPACE_ACADO
38 
39 
40 //
41 // PUBLIC MEMBER FUNCTIONS:
42 //
43 
44 
45 ConstraintElement::ConstraintElement( ){
46 
47  fcn = 0;
48  lb = 0;
49  ub = 0;
50  y_index = 0;
51  t_index = 0;
52 
53  z = 0;
54  JJ = 0;
55 
56  nx = 0;
57  na = 0;
58  nu = 0;
59  np = 0;
60  nw = 0;
61  ny = 0;
62  nFcn = 0;
63  nB = 0;
64 
65  xSeed = 0 ;
66  xaSeed = 0 ;
67  pSeed = 0 ;
68  uSeed = 0 ;
69  wSeed = 0 ;
70  bSeed = 0 ;
71  xSeed2 = 0 ;
72  xaSeed2 = 0 ;
73  pSeed2 = 0 ;
74  uSeed2 = 0 ;
75  wSeed2 = 0 ;
76  bSeed2 = 0 ;
77 
78  condType = CT_SPARSE;
79 }
80 
81 
82 ConstraintElement::ConstraintElement( const Grid& grid_, int nFcn_, int nB_ ){
83 
84  int run1;
85 
86  grid = grid_;
87  nFcn = nFcn_;
88  nB = nB_ ;
89 
90  fcn = new Function[nFcn];
91  lb = new double* [ nB];
92  ub = new double* [ nB];
93 
94  for( run1 = 0; run1 < nB; run1++ ){
95  lb[run1] = 0;
96  ub[run1] = 0;
97  }
98 
99  y_index = new int* [nFcn];
100  t_index = new int [nFcn];
101 
102  for( run1 = 0; run1 < nFcn; run1++ ){
103  y_index[run1] = 0;
104  }
105 
106  z = 0;
107  JJ = 0;
108 
109  nx = 0;
110  na = 0;
111  nu = 0;
112  np = 0;
113  nw = 0;
114  ny = 0;
115 
116  xSeed = 0 ;
117  xaSeed = 0 ;
118  pSeed = 0 ;
119  uSeed = 0 ;
120  wSeed = 0 ;
121  bSeed = 0 ;
122  xSeed2 = 0 ;
123  xaSeed2 = 0 ;
124  pSeed2 = 0 ;
125  uSeed2 = 0 ;
126  wSeed2 = 0 ;
127  bSeed2 = 0 ;
128 
129  condType = CT_SPARSE;
130 }
131 
132 
133 ConstraintElement::ConstraintElement( const ConstraintElement& rhs ){
134 
135  int run1, run2;
136 
137  grid = rhs.grid;
138  nFcn = rhs.nFcn;
139  nB = rhs.nB ;
140 
141  if( rhs.fcn != 0 ) fcn = new Function[nFcn];
142  else fcn = 0 ;
143 
144  if( rhs.lb != 0 ) lb = new double* [nB ];
145  else lb = 0 ;
146 
147  if( rhs.ub != 0 ) ub = new double* [nB ];
148  else ub = 0 ;
149 
150  if( rhs.y_index != 0 ) y_index = new int* [nFcn];
151  else y_index = 0 ;
152 
153  if( rhs.t_index != 0 ) t_index = new int [nFcn];
154  else t_index = 0 ;
155 
156  if( rhs.z != 0 ) z = new EvaluationPoint( *(rhs.z) );
157  else z = 0 ;
158 
159  if( rhs.JJ != 0 ) JJ = new EvaluationPoint( *(rhs.JJ) );
160  else JJ = 0 ;
161 
162  nx = rhs.nx;
163  na = rhs.na;
164  nu = rhs.nu;
165  np = rhs.np;
166  nw = rhs.nw;
167  ny = rhs.ny;
168 
169  for( run1 = 0; run1 < nFcn; run1++ ){
170 
171  fcn[run1] = rhs.fcn[run1];
172 
173  if( ny > 0 ){
174  y_index[run1] = new int[ny];
175  for( run2 = 0; run2 < ny; run2++ )
176  y_index[run1][run2] = rhs.y_index[run1][run2];
177  }
178  else{
179  y_index[run1] = 0;
180  }
181 
182  t_index[run1] = rhs.t_index[run1];
183  }
184 
185  if( fcn == 0 ){
186  for( run1 = 0; run1 < nB; run1++ ){
187  lb[run1] = 0;
188  ub[run1] = 0;
189  }
190  }
191  else{
192  for( run1 = 0; run1 < nB; run1++ ){
193 
194  if( fcn[0].getDim() > 0 ){
195  lb[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
196  ub[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
197  }
198  else{
199  lb[run1] = 0;
200  ub[run1] = 0;
201  }
202 
203  for( run2 = 0; run2 < fcn[0].getDim(); run2++ ){
204  lb[run1][run2] = rhs.lb[run1][run2];
205  ub[run1][run2] = rhs.ub[run1][run2];
206  }
207  }
208  }
209 
210  if( rhs.xSeed != 0 ) xSeed = new BlockMatrix(*rhs.xSeed );
211  else xSeed = 0 ;
212  if( rhs.xaSeed != 0 ) xaSeed = new BlockMatrix(*rhs.xaSeed);
213  else xaSeed = 0 ;
214  if( rhs.pSeed != 0 ) pSeed = new BlockMatrix(*rhs.pSeed) ;
215  else pSeed = 0 ;
216  if( rhs.uSeed != 0 ) uSeed = new BlockMatrix(*rhs.uSeed) ;
217  else uSeed = 0 ;
218  if( rhs.wSeed != 0 ) wSeed = new BlockMatrix(*rhs.wSeed) ;
219  else wSeed = 0 ;
220 
221  if( rhs.bSeed != 0 ) bSeed = new BlockMatrix(*rhs.bSeed );
222  else bSeed = 0 ;
223 
224  if( rhs.xSeed2 != 0 ) xSeed2 = new BlockMatrix(*rhs.xSeed2) ;
225  else xSeed2 = 0 ;
226  if( rhs.xaSeed2 != 0 ) xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
227  else xaSeed2 = 0 ;
228  if( rhs.pSeed2 != 0 ) pSeed2 = new BlockMatrix(*rhs.pSeed2 );
229  else pSeed2 = 0 ;
230  if( rhs.uSeed2 != 0 ) uSeed2 = new BlockMatrix(*rhs.uSeed2 );
231  else uSeed2 = 0 ;
232  if( rhs.wSeed2 != 0 ) wSeed2 = new BlockMatrix(*rhs.wSeed2 );
233  else wSeed2 = 0 ;
234 
235  if( rhs.bSeed != 0 ) bSeed2 = new BlockMatrix(*rhs.bSeed );
236  else bSeed2 = 0 ;
237 
238 
239  residuumL = rhs.residuumL;
240  residuumU = rhs.residuumU;
241  dForward = rhs.dForward ;
242  dBackward = rhs.dBackward;
243 
244  condType = rhs.condType;
245 }
246 
247 
248 ConstraintElement::~ConstraintElement( ){
249 
250  int run1;
251 
252  if( fcn != 0 )
253  delete[] fcn;
254 
255  for( run1 = 0; run1 < nB; run1++ ){
256 
257  if( lb[run1] != 0 )
258  free(lb[run1]);
259 
260  if( ub[run1] != 0 )
261  free(ub[run1]);
262  }
263 
264  if( lb != 0 )
265  delete[] lb;
266 
267  if( ub != 0 )
268  delete[] ub;
269 
270  for( run1 = 0; run1 < nFcn; run1++ ){
271 
272  if( y_index[run1] != 0 )
273  delete[] y_index[run1];
274  }
275 
276  if( y_index != 0 )
277  delete[] y_index;
278 
279  if( t_index != 0 )
280  delete[] t_index;
281 
282  if ( z != 0 )
283  delete[] z;
284 
285  if ( JJ != 0 )
286  delete[] JJ;
287 
288  if( xSeed != 0 ) delete xSeed ;
289  if( xaSeed != 0 ) delete xaSeed ;
290  if( pSeed != 0 ) delete pSeed ;
291  if( uSeed != 0 ) delete uSeed ;
292  if( wSeed != 0 ) delete wSeed ;
293 
294  if( bSeed != 0 ) delete bSeed ;
295 
296  if( xSeed2 != 0 ) delete xSeed2 ;
297  if( xaSeed2 != 0 ) delete xaSeed2;
298  if( pSeed2 != 0 ) delete pSeed2 ;
299  if( uSeed2 != 0 ) delete uSeed2 ;
300  if( wSeed2 != 0 ) delete wSeed2 ;
301 
302  if( bSeed2 != 0 ) delete bSeed2 ;
303 }
304 
305 
306 ConstraintElement& ConstraintElement::operator=( const ConstraintElement& rhs ){
307 
308  int run1, run2;
309 
310  if( this != &rhs ){
311 
312  if( fcn != 0 )
313  delete[] fcn;
314 
315  for( run1 = 0; run1 < nB; run1++ ){
316 
317  if( lb[run1] != 0 )
318  free(lb[run1]);
319 
320  if( ub[run1] != 0 )
321  free(ub[run1]);
322  }
323 
324  if( lb != 0 )
325  delete[] lb;
326 
327  if( ub != 0 )
328  delete[] ub;
329 
330  for( run1 = 0; run1 < nFcn; run1++ ){
331 
332  if( y_index[run1] != 0 )
333  delete[] y_index[run1];
334  }
335 
336  if( y_index != 0 )
337  delete[] y_index;
338 
339  if( t_index != 0 )
340  delete[] t_index;
341 
342  if ( z != 0 )
343  delete[] z;
344 
345  if ( JJ != 0 )
346  delete[] JJ;
347 
348  if( xSeed != 0 ) delete xSeed ;
349  if( xaSeed != 0 ) delete xaSeed ;
350  if( pSeed != 0 ) delete pSeed ;
351  if( uSeed != 0 ) delete uSeed ;
352  if( wSeed != 0 ) delete wSeed ;
353 
354  if( bSeed != 0 ) delete bSeed ;
355 
356  if( xSeed2 != 0 ) delete xSeed2 ;
357  if( xaSeed2 != 0 ) delete xaSeed2;
358  if( pSeed2 != 0 ) delete pSeed2 ;
359  if( uSeed2 != 0 ) delete uSeed2 ;
360  if( wSeed2 != 0 ) delete wSeed2 ;
361 
362  if( bSeed2 != 0 ) delete bSeed2 ;
363 
364 
365  grid = rhs.grid;
366  nFcn = rhs.nFcn;
367  nB = rhs.nB ;
368 
369  if( rhs.fcn != 0 ) fcn = new Function[nFcn];
370  else fcn = 0 ;
371 
372  if( rhs.lb != 0 ) lb = new double* [nB ];
373  else lb = 0 ;
374 
375  if( rhs.ub != 0 ) ub = new double* [nB ];
376  else ub = 0 ;
377 
378  if( rhs.y_index != 0 ) y_index = new int* [nFcn];
379  else y_index = 0 ;
380 
381  if( rhs.t_index != 0 ) t_index = new int [nFcn];
382  else t_index = 0 ;
383 
384  if( rhs.z != 0 ) z = new EvaluationPoint( *(rhs.z) );
385  else z = 0 ;
386 
387  if( rhs.JJ != 0 ) JJ = new EvaluationPoint( *(rhs.JJ) );
388  else JJ = 0 ;
389 
390  nx = rhs.nx;
391  na = rhs.na;
392  nu = rhs.nu;
393  np = rhs.np;
394  nw = rhs.nw;
395  ny = rhs.ny;
396 
397  for( run1 = 0; run1 < nFcn; run1++ ){
398 
399  fcn[run1] = rhs.fcn[run1];
400 
401  if( ny > 0 ){
402  y_index[run1] = new int[ny];
403  for( run2 = 0; run2 < ny; run2++ )
404  y_index[run1][run2] = rhs.y_index[run1][run2];
405  }
406  else{
407  y_index[run1] = 0;
408  }
409 
410  t_index[run1] = rhs.t_index[run1];
411 
412  }
413 
414  if( fcn == 0 ){
415  for( run1 = 0; run1 < nB; run1++ ){
416  lb[run1] = 0;
417  ub[run1] = 0;
418  }
419  }
420  else{
421  for( run1 = 0; run1 < nB; run1++ ){
422 
423  if( fcn[0].getDim() > 0 ){
424  lb[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
425  ub[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
426  }
427  else{
428  lb[run1] = 0;
429  ub[run1] = 0;
430  }
431 
432  for( run2 = 0; run2 < fcn[0].getDim(); run2++ ){
433  lb[run1][run2] = rhs.lb[run1][run2];
434  ub[run1][run2] = rhs.ub[run1][run2];
435  }
436  }
437  }
438 
439  if( rhs.xSeed != 0 ) xSeed = new BlockMatrix(*rhs.xSeed );
440  else xSeed = 0 ;
441  if( rhs.xaSeed != 0 ) xaSeed = new BlockMatrix(*rhs.xaSeed );
442  else xaSeed = 0 ;
443  if( rhs.pSeed != 0 ) pSeed = new BlockMatrix(*rhs.pSeed );
444  else pSeed = 0 ;
445  if( rhs.uSeed != 0 ) uSeed = new BlockMatrix(*rhs.uSeed );
446  else uSeed = 0 ;
447  if( rhs.wSeed != 0 ) wSeed = new BlockMatrix(*rhs.wSeed );
448  else wSeed = 0 ;
449 
450  if( rhs.bSeed != 0 ) bSeed = new BlockMatrix(*rhs.bSeed );
451  else bSeed = 0 ;
452 
453  if( rhs.xSeed2 != 0 ) xSeed2 = new BlockMatrix(*rhs.xSeed2 );
454  else xSeed2 = 0 ;
455  if( rhs.xaSeed2 != 0 ) xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
456  else xaSeed2 = 0 ;
457  if( rhs.pSeed2 != 0 ) pSeed2 = new BlockMatrix(*rhs.pSeed2 );
458  else pSeed2 = 0 ;
459  if( rhs.uSeed2 != 0 ) uSeed2 = new BlockMatrix(*rhs.uSeed2 );
460  else uSeed2 = 0 ;
461  if( rhs.wSeed2 != 0 ) wSeed2 = new BlockMatrix(*rhs.wSeed2 );
462  else wSeed2 = 0 ;
463 
464  if( rhs.bSeed != 0 ) bSeed2 = new BlockMatrix(*rhs.bSeed );
465  else bSeed2 = 0 ;
466 
467  residuumL = rhs.residuumL;
468  residuumU = rhs.residuumU;
469  dForward = rhs.dForward ;
470  dBackward = rhs.dBackward;
471 
472  condType = rhs.condType;
473  }
474 
475  return *this;
476 }
477 
478 
479 returnValue ConstraintElement::init( const OCPiterate& iter ){
480 
481  int run1, run2;
482 
483  initializeEvaluationPoints( iter );
484 
485 
486  if( iter.x != NULL ) nx = iter.x ->getNumValues();
487  else nx = 0 ;
488 
489  if( iter.xa != NULL ) na = iter.xa->getNumValues();
490  else na = 0 ;
491 
492  if( iter.p != NULL ) np = iter.p ->getNumValues();
493  else np = 0 ;
494 
495  if( iter.u != NULL ) nu = iter.u ->getNumValues();
496  else nu = 0 ;
497 
498  if( iter.w != NULL ) nw = iter.w ->getNumValues();
499  else nw = 0 ;
500 
501  ny = nx+na+nu+np+nw;
502 
503  for( run2 = 0; run2 < nFcn; run2++ ){
504 
505  if( y_index[run2] != 0 ) delete[] y_index[run2];
506  y_index[run2] = new int[ny] ;
507 
508  for( run1 = 0; run1 < nx; run1++ )
509  y_index[run2][run1] = fcn[run2].index( VT_DIFFERENTIAL_STATE, run1 );
510 
511  for( run1 = 0; run1 < na; run1++ )
512  y_index[run2][nx+run1] = fcn[run2].index( VT_ALGEBRAIC_STATE, run1 );
513 
514  for( run1 = 0; run1 < np; run1++ )
515  y_index[run2][nx+na+run1] = fcn[run2].index( VT_PARAMETER, run1 );
516 
517  for( run1 = 0; run1 < nu; run1++ )
518  y_index[run2][nx+na+np+run1] = fcn[run2].index( VT_CONTROL, run1 );
519 
520  for( run1 = 0; run1 < nw; run1++ )
521  y_index[run2][nx+na+np+nu+run1] = fcn[run2].index( VT_DISTURBANCE, run1 );
522 
523  t_index[run2] = fcn[run2].index( VT_TIME, 0 );
524  }
525 
526  return SUCCESSFUL_RETURN;
527 }
528 
529 
530 returnValue ConstraintElement::setForwardSeed( BlockMatrix *xSeed_,
531  BlockMatrix *xaSeed_,
532  BlockMatrix *pSeed_,
533  BlockMatrix *uSeed_,
534  BlockMatrix *wSeed_,
535  int order ){
536 
537  if( order == 1 ){
538 
539  if( xSeed_ != 0 ){
540  if( xSeed != 0 ) delete xSeed;
541  xSeed = new BlockMatrix(*xSeed_);
542  }
543  else{
544  if( xSeed != 0 ) delete xSeed;
545  xSeed = 0;
546  }
547  if( xaSeed_ != 0 ){
548  if( xaSeed != 0 ) delete xaSeed;
549  xaSeed = new BlockMatrix(*xaSeed_);
550  }
551  else{
552  if( xaSeed != 0 ) delete xaSeed;
553  xaSeed = 0;
554  }
555  if( pSeed_ != 0 ){
556  if( pSeed != 0 ) delete pSeed;
557  pSeed = new BlockMatrix(*pSeed_);
558  }
559  else{
560  if( pSeed != 0 ) delete pSeed;
561  pSeed = 0;
562  }
563  if( uSeed_ != 0 ){
564  if( uSeed != 0 ) delete uSeed;
565  uSeed = new BlockMatrix(*uSeed_);
566  }
567  else{
568  if( uSeed != 0 ) delete uSeed;
569  uSeed = 0;
570  }
571  if( wSeed_ != 0 ){
572  if( wSeed != 0 ) delete wSeed;
573  wSeed = new BlockMatrix(*wSeed_);
574  }
575  else{
576  if( wSeed != 0 ) delete wSeed;
577  wSeed = 0;
578  }
579 
580  return SUCCESSFUL_RETURN;
581  }
582  if( order == 2 ){
583 
584  if( xSeed_ != 0 ){
585  if( xSeed2 != 0 ) delete xSeed2;
586  xSeed2 = new BlockMatrix(*xSeed_);
587  }
588  else{
589  if( xSeed2 != 0 ) delete xSeed2;
590  xSeed2 = 0;
591  }
592  if( xaSeed_ != 0 ){
593  if( xaSeed2 != 0 ) delete xaSeed2;
594  xaSeed2 = new BlockMatrix(*xaSeed_);
595  }
596  else{
597  if( xaSeed2 != 0 ) delete xaSeed2;
598  xaSeed2 = 0;
599  }
600  if( pSeed_ != 0 ){
601  if( pSeed2 != 0 ) delete pSeed2;
602  pSeed2 = new BlockMatrix(*pSeed_);
603  }
604  else{
605  if( pSeed2 != 0 ) delete pSeed2;
606  pSeed2 = 0;
607  }
608  if( uSeed_ != 0 ){
609  if( uSeed2 != 0 ) delete uSeed2;
610  uSeed2 = new BlockMatrix(*uSeed_);
611  }
612  else{
613  if( uSeed2 != 0 ) delete uSeed2;
614  uSeed2 = 0;
615  }
616  if( wSeed_ != 0 ){
617  if( wSeed2 != 0 ) delete wSeed2;
618  wSeed2 = new BlockMatrix(*wSeed_);
619  }
620  else{
621  if( wSeed2 != 0 ) delete wSeed2;
622  wSeed2 = 0;
623  }
624  return SUCCESSFUL_RETURN;
625  }
626 
627  return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
628 }
629 
630 
631 returnValue ConstraintElement::setUnitForwardSeed( ){
632 
633  BlockMatrix xSeed_( 1, 1 );
634  BlockMatrix xaSeed_( 1, 1 );
635  BlockMatrix pSeed_( 1, 1 );
636  BlockMatrix uSeed_( 1, 1 );
637  BlockMatrix wSeed_( 1, 1 );
638 
639  xSeed_.setIdentity ( 0, 0, nx );
640  xaSeed_.setIdentity( 0, 0, na );
641  pSeed_.setIdentity ( 0, 0, np );
642  uSeed_.setIdentity ( 0, 0, nu );
643  wSeed_.setIdentity ( 0, 0, nw );
644 
645  return setForwardSeed( &xSeed_, &xaSeed_, &pSeed_, &uSeed_, &wSeed_, 1 );
646 }
647 
648 
649 returnValue ConstraintElement::setBackwardSeed( BlockMatrix *seed, int order ){
650 
651  if( order == 1 ){
652 
653  if( seed != 0 ){
654  if( bSeed != 0 ) delete bSeed;
655  bSeed = new BlockMatrix(*seed);
656  }
657  else{
658  if( bSeed != 0 ) delete bSeed;
659  bSeed = 0;
660  }
661 
662  return SUCCESSFUL_RETURN;
663  }
664  if( order == 2 ){
665 
666  if( seed != 0 ){
667  if( bSeed2 != 0 ) delete bSeed2;
668  bSeed2 = new BlockMatrix(*seed);
669  }
670  else{
671  if( bSeed2 != 0 ) delete bSeed2;
672  bSeed2 = 0;
673  }
674  return SUCCESSFUL_RETURN;
675  }
676 
677  return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
678 }
679 
680 
681 returnValue ConstraintElement::getResiduum( BlockMatrix &lower_residuum,
682  BlockMatrix &upper_residuum ){
683 
684 
685  lower_residuum = residuumL;
686  upper_residuum = residuumU;
687 
688  return SUCCESSFUL_RETURN;
689 }
690 
691 
692 returnValue ConstraintElement::getForwardSensitivities( BlockMatrix *D, int order ){
693 
694  ASSERT( D != 0 );
695 
696  if( order == 1 ){
697 
698  D[0] = dForward;
699  return SUCCESSFUL_RETURN;
700  }
701  if( order == 2 ){
702 
703  return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
704  }
705  return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
706 }
707 
708 
709 returnValue ConstraintElement::getBackwardSensitivities( BlockMatrix *D, int order ){
710 
711  ASSERT( D != 0 );
712 
713  if( order == 1 ){
714 
715  D[0] = dBackward;
716  return SUCCESSFUL_RETURN;
717  }
718  if( order == 2 ){
719 
720  return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
721  }
722  return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
723 }
724 
725 
726 
727 //
728 // PROTECTED MEMBER FUNCTIONS:
729 //
730 
731 returnValue ConstraintElement::initializeEvaluationPoints( const OCPiterate& iter )
732 {
733  if ( z != 0 )
734  delete[] z;
735 
736  if ( JJ != 0 )
737  delete[] JJ;
738 
739  z = new EvaluationPoint[nFcn];
740  JJ = new EvaluationPoint[nFcn];
741  //HH = new EvaluationPoint[nFcn];
742 
743  for( int i=0; i<nFcn; ++i )
744  {
745  z[i].init( fcn[i], iter );
746  JJ[i].init( fcn[i], iter );
747  //HH[i].init( fcn[i], iter );
748  }
749 
750  return SUCCESSFUL_RETURN;
751 }
752 
753 returnValue ConstraintElement::get(Function& function_, DMatrix& lb_, DMatrix& ub_)
754 {
755  if ( fcn == NULL )
756  return RET_INITIALIZE_FIRST;
757 
758  // This is not exactly bullet proof, but for now will serve the purpose
759  function_ = fcn[ 0 ];
760 
761  int dimFcn = fcn[ 0 ].getDim();
762 
763  if ( dimFcn == 0 )
764  {
765  lb_.init(0, 0);
766  ub_.init(0, 0);
767 
768  return SUCCESSFUL_RETURN;
769  }
770 
771  lb_.init(nB, dimFcn);
772  ub_.init(nB, dimFcn);
773 
774  int i, j;
775 
776  for (i = 0; i < nB; ++i)
777  for (j = 0; j < dimFcn; ++j)
778  {
779  lb_(i, j) = lb[ i ][ j ];
780  ub_(i, j) = ub[ i ][ j ];
781  }
782 
783  return SUCCESSFUL_RETURN;
784 }
785 
786 
787 CLOSE_NAMESPACE_ACADO
788 
789 // end of file.
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
Function
Allows to setup and evaluate a general function based on SymbolicExpressions.
Definition: function_.hpp:59
VT_PARAMETER
Definition: acado_types.hpp:101
ConstraintElement::z
EvaluationPoint * z
Definition: constraint_element.hpp:251
ConstraintElement::init
returnValue init(const OCPiterate &iter)
Definition: constraint_element.cpp:479
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
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
OCPiterate::u
VariablesGrid * u
Definition: ocp_iterate.hpp:189
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
constraint_element.hpp
VT_ALGEBRAIC_STATE
Definition: acado_types.hpp:98
EvaluationPoint
Allows to setup function evaluation points.
Definition: evaluation_point.hpp:58
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
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
ConstraintElement::setBackwardSeed
virtual returnValue setBackwardSeed(BlockMatrix *seed, int order)
Definition: constraint_element.cpp:649
ConstraintElement::setUnitForwardSeed
virtual returnValue setUnitForwardSeed()
Definition: constraint_element.cpp:631
ConstraintElement
Base class for all kind of constraints (except for bounds) within optimal control problems...
Definition: constraint_element.hpp:57
ConstraintElement::getResiduum
virtual returnValue getResiduum(BlockMatrix &lower_residuum, BlockMatrix &upper_residuum)
Definition: constraint_element.cpp:681
RET_NOT_IMPLEMENTED_YET
Definition: acado_types.hpp:848
ConstraintElement::setForwardSeed
virtual returnValue setForwardSeed(BlockMatrix *xSeed_, BlockMatrix *xaSeed_, BlockMatrix *pSeed_, BlockMatrix *uSeed_, BlockMatrix *wSeed_, int order)
Definition: constraint_element.cpp:530
VT_TIME
Definition: acado_types.hpp:104
ConstraintElement::JJ
EvaluationPoint * JJ
Definition: constraint_element.hpp:252
Function::getDim
int getDim() const
ConstraintElement::initializeEvaluationPoints
virtual returnValue initializeEvaluationPoints(const OCPiterate &iter)
Definition: constraint_element.cpp:731
ConstraintElement::getBackwardSensitivities
virtual returnValue getBackwardSensitivities(BlockMatrix *D, int order)
Definition: constraint_element.cpp:709
ConstraintElement::get
returnValue get(Function &function_, DMatrix &lb_, DMatrix &ub_)
Definition: constraint_element.cpp:753
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
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
ConstraintElement::getForwardSensitivities
virtual returnValue getForwardSensitivities(BlockMatrix *D, int order)
Definition: constraint_element.cpp:692
RET_INITIALIZE_FIRST
Definition: acado_types.hpp:1053
BlockMatrix::setIdentity
returnValue setIdentity(uint rowIdx, uint colIdx, uint dim)
VT_DISTURBANCE
Definition: acado_types.hpp:103
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:31