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/function/function.hpp>
36 #include <acado/constraint/constraint.hpp>
37 
38 
39 
40 BEGIN_NAMESPACE_ACADO
41 
42 
43 
44 //
45 // PUBLIC MEMBER FUNCTIONS:
46 //
47 
48 
49 Constraint::Constraint( )
50  :BoxConstraint(){
51 
52  boundary_constraint = 0;
53  coupled_path_constraint = 0;
54  path_constraint = 0;
55  algebraic_consistency_constraint = 0;
56  point_constraints = 0;
57 }
58 
59 
60 
61 Constraint::Constraint( const Constraint& rhs )
62  :BoxConstraint(rhs){
63 
64  uint run1;
65 
66  if( rhs.boundary_constraint != 0 )
67  boundary_constraint = new BoundaryConstraint(*rhs.boundary_constraint);
68  else boundary_constraint = 0 ;
69 
70  if( rhs.coupled_path_constraint != 0 )
71  coupled_path_constraint = new CoupledPathConstraint(*rhs.coupled_path_constraint);
72  else coupled_path_constraint = 0 ;
73 
74  if( rhs.path_constraint != 0 )
75  path_constraint = new PathConstraint(*rhs.path_constraint);
76  else path_constraint = 0 ;
77 
78  if( rhs.algebraic_consistency_constraint != 0 )
79  algebraic_consistency_constraint = new AlgebraicConsistencyConstraint(*rhs.algebraic_consistency_constraint);
80  else algebraic_consistency_constraint = 0;
81 
82  if( rhs.point_constraints != 0 ){
83  point_constraints = new PointConstraint*[grid.getNumPoints()];
84  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
85  if( rhs.point_constraints[run1] != 0 ){
86  point_constraints[run1] = new PointConstraint(*rhs.point_constraints[run1]);
87  }
88  else point_constraints[run1] = 0;
89  }
90  }
91  else point_constraints = 0;
92 }
93 
94 
95 Constraint::~Constraint( ){
96 
97  deleteAll();
98 }
99 
100 
101 void Constraint::deleteAll(){
102 
103  uint run1;
104 
105  if( boundary_constraint != 0 )
106  delete boundary_constraint;
107 
108  if( coupled_path_constraint != 0 )
109  delete coupled_path_constraint;
110 
111  if( path_constraint != 0 )
112  delete path_constraint;
113 
114  if( algebraic_consistency_constraint != 0 )
115  delete algebraic_consistency_constraint;
116 
117  if( point_constraints != 0 ){
118 
119  for( run1 = 0; run1 < grid.getNumPoints(); run1++ )
120  if( point_constraints[run1] != 0 )
121  delete point_constraints[run1];
122 
123  delete[] point_constraints;
124  }
125 }
126 
127 
128 
129 returnValue Constraint::init( const Grid& grid_, const int& numberOfStages_ ){
130 
131  deleteAll();
132 
133  BoxConstraint::init( grid_ );
134 
135  uint run1;
136 
137  boundary_constraint = new BoundaryConstraint (grid );
138  coupled_path_constraint = new CoupledPathConstraint (grid );
139  path_constraint = new PathConstraint (grid );
140  algebraic_consistency_constraint = new AlgebraicConsistencyConstraint(grid,numberOfStages_);
141  point_constraints = new PointConstraint*[grid.getNumPoints()];
142 
143  for( run1 = 0; run1 < grid.getNumPoints(); run1++ )
144  point_constraints[run1] = 0;
145 
146  return SUCCESSFUL_RETURN;
147 }
148 
149 
150 
151 Constraint& Constraint::operator=( const Constraint& rhs ){
152 
153  uint run1;
154 
155  if( this != &rhs ){
156 
157  deleteAll();
158 
159  BoxConstraint::operator=(rhs);
160 
161  if( rhs.boundary_constraint != 0 )
162  boundary_constraint = new BoundaryConstraint(*rhs.boundary_constraint);
163  else boundary_constraint = 0 ;
164 
165  if( rhs.coupled_path_constraint != 0 )
166  coupled_path_constraint = new CoupledPathConstraint(*rhs.coupled_path_constraint);
167  else coupled_path_constraint = 0 ;
168 
169  if( rhs.path_constraint != 0 )
170  path_constraint = new PathConstraint(*rhs.path_constraint);
171  else path_constraint = 0 ;
172 
173  if( rhs.algebraic_consistency_constraint != 0 )
174  algebraic_consistency_constraint = new AlgebraicConsistencyConstraint(*rhs.algebraic_consistency_constraint);
175  else algebraic_consistency_constraint = 0;
176 
177  if( rhs.point_constraints != 0 ){
178  point_constraints = new PointConstraint*[grid.getNumPoints()];
179  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
180  if( rhs.point_constraints[run1] != 0 ){
181  point_constraints[run1] = new PointConstraint(*rhs.point_constraints[run1]);
182  }
183  else point_constraints[run1] = 0;
184  }
185  }
186  else point_constraints = 0;
187 
188  }
189  return *this;
190 }
191 
192 
193 
194 returnValue Constraint::add( const double lb_, const Expression& arg, const double ub_ ){
195 
196  DVector tmp_lb(grid.getNumPoints());
197  DVector tmp_ub(grid.getNumPoints());
198 
199  tmp_lb.setAll(lb_);
200  tmp_ub.setAll(ub_);
201 
202  return add( tmp_lb, arg, tmp_ub );
203 }
204 
205 
206 
207 returnValue Constraint::add( const DVector lb_, const Expression& arg, const double ub_ ){
208 
209  DVector tmp_ub(grid.getNumPoints());
210  tmp_ub.setAll(ub_);
211 
212  return add( lb_, arg, tmp_ub );
213 }
214 
215 
216 returnValue Constraint::add( const double lb_, const Expression& arg, const DVector ub_ ){
217 
218  DVector tmp_lb(grid.getNumPoints());
219  tmp_lb.setAll(lb_);
220 
221  return add( tmp_lb, arg, ub_ );
222 }
223 
224 
225 returnValue Constraint::add( const DVector lb_, const Expression &arg, const DVector ub_ ){
226 
227  // CHECK FEASIBILITY:
228  // ------------------
229  if( lb_.getDim() != ub_.getDim() ) return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
230  if( lb_.getDim() != grid.getNumPoints() ) return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
231  if( (lb_ <= (const DVector&)ub_) == BT_FALSE ) return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
232 
233 
234  // CHECK FOR A BOUND:
235  // -----------------------
236  VariableType varType = arg.getVariableType();
237  int component = arg.getComponent(0) ;
238 
239  if( arg.isVariable( ) == BT_TRUE ){
240  if( varType != VT_INTERMEDIATE_STATE ){
241 
242  nb++;
243  var = (VariableType*)realloc(var , nb*sizeof(VariableType));
244  index = (int* )realloc(index, nb*sizeof(int ));
245  blb = (DVector** )realloc(blb , nb*sizeof(DVector* ));
246  bub = (DVector** )realloc(bub , nb*sizeof(DVector* ));
247 
248  var [nb-1] = varType ;
249  index[nb-1] = component;
250  blb [nb-1] = new DVector( lb_ );
251  bub [nb-1] = new DVector( ub_ );
252 
253  return SUCCESSFUL_RETURN;
254  }
255  }
256 
257 
258  // SAVE THE ARGUMENT AS A Path Constraint:
259  // ---------------------------------------
260  return path_constraint->add( lb_, arg, ub_ );
261 }
262 
263 
264 returnValue Constraint::add( const double lb_, const Expression *arguments, const double ub_ ){
265 
266  // CHECK FEASIBILITY:
267  // ------------------
268  if( lb_ > ub_ + EPS ) return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
269 
270  return coupled_path_constraint->add( lb_, arguments, ub_ );
271 }
272 
273 
274 returnValue Constraint::add( const uint& endOfStage_ ,
275  const DifferentialEquation& dae ){
276 
277  return algebraic_consistency_constraint->add( endOfStage_, dae );
278 }
279 
280 
281 returnValue Constraint::add( const ConstraintComponent& component ){
282 
283 
284  DVector tmp_ub(grid.getNumPoints());
285  DVector tmp_lb(grid.getNumPoints());
286 
287  uint run1;
288 
289  if( component.hasLBgrid() == 0 ){
290 
291  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
292  if( (component.getLB()).getDim() == 1 )
293  tmp_lb(run1) = (component.getLB()).operator()(0);
294  else{
295  if( (component.getLB()).getDim() <= run1 )
296  return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
297  tmp_lb(run1) = (component.getLB()).operator()(run1);
298  }
299  }
300  }
301  else{
302 
303  VariablesGrid LBgrid = component.getLBgrid();
304 
305  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
306  DVector tmp = LBgrid.linearInterpolation( grid.getTime(run1) );
307  tmp_lb(run1) = tmp(0);
308  }
309  }
310 
311 
312  if( component.hasUBgrid() == 0 ){
313  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
314  if( (component.getUB()).getDim() == 1 )
315  tmp_ub(run1) = (component.getUB()).operator()(0);
316  else{
317  if( (component.getUB()).getDim() <= run1 )
318  return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
319  tmp_ub(run1) = (component.getUB()).operator()(run1);
320  }
321  }
322  }
323  else{
324 
325  VariablesGrid UBgrid = component.getUBgrid();
326 
327  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
328  DVector tmp = UBgrid.linearInterpolation( grid.getTime(run1) );
329  tmp_ub(run1) = tmp(0);
330  }
331  }
332 
333  return add( tmp_lb, component.getExpression(), tmp_ub );
334 }
335 
336 
337 returnValue Constraint::add( const int index_, const ConstraintComponent& component ){
338 
339  DVector tmp_ub(grid.getNumPoints());
340  DVector tmp_lb(grid.getNumPoints());
341 
342  if ( !(index_ < (int) grid.getNumPoints()) )
343  return ACADOERRORTEXT(RET_ASSERTION,
344  "The constraint component can not be set as the associated "
345  "discretization point is not in the time horizon.");
346 
347  uint run1;
348 
349  if( component.hasLBgrid() == 0 ){
350 
351  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
352  if( (component.getLB()).getDim() == 1 )
353  tmp_lb(run1) = (component.getLB()).operator()(0);
354  else{
355  if( (component.getLB()).getDim() <= run1 )
356  return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
357  tmp_lb(run1) = (component.getLB()).operator()(run1);
358  }
359  }
360  }
361  else{
362 
363  VariablesGrid LBgrid = component.getLBgrid();
364 
365  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
366  DVector tmp = LBgrid.linearInterpolation( grid.getTime(run1) );
367  tmp_lb(run1) = tmp(0);
368  }
369  }
370 
371 
372  if( component.hasUBgrid() == 0 ){
373  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
374  if( (component.getUB()).getDim() == 1 )
375  tmp_ub(run1) = (component.getUB()).operator()(0);
376  else{
377  if( (component.getUB()).getDim() <= run1 )
378  return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
379  tmp_ub(run1) = (component.getUB()).operator()(run1);
380  }
381  }
382  }
383  else{
384 
385  VariablesGrid UBgrid = component.getUBgrid();
386 
387  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
388  DVector tmp = UBgrid.linearInterpolation( grid.getTime(run1) );
389  tmp_ub(run1) = tmp(0);
390  }
391  }
392 
393  ACADO_TRY( add( index_, tmp_lb(index_), component.getExpression(), tmp_ub(index_) ) );
394 
395  return SUCCESSFUL_RETURN;
396 }
397 
398 
399 returnValue Constraint::evaluate( const OCPiterate& iter ){
400 
401 
402  if( grid.getNumPoints() == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
403 
404  uint run1;
405  returnValue returnvalue;
406 
407 
408  // EVALUATE BOUNDARY CONSTRAINS:
409  // -----------------------------
410 
411  if( boundary_constraint->getNC() != 0 ){
412  returnvalue = boundary_constraint->init( iter );
413  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
414  returnvalue = boundary_constraint->evaluate( iter );
415  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
416  }
417 
418 
419  // EVALUATE COUPLED PATH CONSTRAINS:
420  // ---------------------------------
421 
422  if( coupled_path_constraint->getNC() != 0 ){
423  returnvalue = coupled_path_constraint->init( iter );
424  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
425  returnvalue = coupled_path_constraint->evaluate( iter );
426  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
427  }
428 
429 
430  // EVALUATE PATH CONSTRAINS:
431  // -------------------------
432 
433  if( path_constraint->getNC() != 0 ){
434  returnvalue = path_constraint->init( iter );
435  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
436  returnvalue = path_constraint->evaluate( iter );
437  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
438  }
439 
440 
441  // EVALUATE ALGEBRAIC CONSISTENCY CONSTRAINS:
442  // ------------------------------------------
443 
444  if( algebraic_consistency_constraint->getNC() != 0 ){
445  returnvalue = algebraic_consistency_constraint->init( iter );
446  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
447  returnvalue = algebraic_consistency_constraint->evaluate( iter );
448  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
449  }
450 
451 
452  // EVALUATE POINT CONSTRAINS:
453  // --------------------------
454 
455  if( point_constraints != 0 ){
456  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
457  if( point_constraints[run1] != 0 ){
458  returnvalue = point_constraints[run1]->init( iter );
459  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
460  returnvalue = point_constraints[run1]->evaluate( iter );
461  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
462  }
463  }
464  }
465 
466 
467  // EVALUATE BOUNDS:
468  // ----------------
469 
470  return evaluateBounds( iter );
471 }
472 
473 
474 
475 returnValue Constraint::evaluateSensitivities( ){
476 
477  uint run1;
478  returnValue returnvalue;
479 
480 
481  // EVALUATE BOUNDARY CONSTRAINS:
482  // -----------------------------
483 
484  if( boundary_constraint->getNC() != 0 ){
485  returnvalue = boundary_constraint->evaluateSensitivities( );
486  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
487  }
488 
489 
490  // EVALUATE COUPLED PATH CONSTRAINS:
491  // ---------------------------------
492 
493  if( coupled_path_constraint->getNC() != 0 ){
494  returnvalue = coupled_path_constraint->evaluateSensitivities( );
495  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
496  }
497 
498 
499  // EVALUATE PATH CONSTRAINS:
500  // -------------------------
501 
502  if( path_constraint->getNC() != 0 ){
503  returnvalue = path_constraint->evaluateSensitivities( );
504  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
505  }
506 
507 
508  // EVALUATE ALGEBRAIC CONSISTENCY CONSTRAINS:
509  // ------------------------------------------
510 
511  if( algebraic_consistency_constraint->getNC() != 0 ){
512  returnvalue = algebraic_consistency_constraint->evaluateSensitivities( );
513  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
514  }
515 
516  // EVALUATE POINT CONSTRAINS:
517  // --------------------------
518 
519  if( point_constraints != 0 ){
520  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
521  if( point_constraints[run1] != 0 ){
522  returnvalue = point_constraints[run1]->evaluateSensitivities( );
523  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
524  }
525  }
526  }
527 
528  return SUCCESSFUL_RETURN;
529 }
530 
531 
532 returnValue Constraint::evaluateSensitivities( const BlockMatrix &seed, BlockMatrix &hessian ){
533 
534 
535  uint run1 ;
536  int count;
537  returnValue returnvalue;
538 
539  count = 0;
540  DMatrix tmp;
541 
542  // EVALUATE BOUNDARY CONSTRAINS:
543  // -----------------------------
544 
545  if( boundary_constraint->getNC() != 0 ){
546  seed.getSubBlock( count, 0, tmp, boundary_constraint->getNC(), 1 );
547  returnvalue = boundary_constraint->evaluateSensitivities( tmp, hessian );
548  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
549  count++;
550  }
551 
552 
553  // EVALUATE COUPLED PATH CONSTRAINS:
554  // ---------------------------------
555 
556  if( coupled_path_constraint->getNC() != 0 ){
557  seed.getSubBlock( count, 0, tmp, coupled_path_constraint->getNC(), 1 );
558  returnvalue = coupled_path_constraint->evaluateSensitivities( tmp, hessian );
559  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
560  count++;
561  }
562 
563 
564  // EVALUATE PATH CONSTRAINS:
565  // -------------------------
566 
567  if( path_constraint->getNC() != 0 ){
568  returnvalue = path_constraint->evaluateSensitivities( count, seed, hessian );
569  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
570  }
571 
572 
573  // EVALUATE ALGEBRAIC CONSISTENCY CONSTRAINS:
574  // ------------------------------------------
575 
576  if( algebraic_consistency_constraint->getNC() != 0 ){
577  returnvalue = algebraic_consistency_constraint->evaluateSensitivities( count, seed, hessian );
578  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
579  }
580 
581  // EVALUATE POINT CONSTRAINS:
582  // --------------------------
583 
584  if( point_constraints != 0 ){
585  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
586  if( point_constraints[run1] != 0 ){
587  seed.getSubBlock( count, 0, tmp, point_constraints[run1]->getNC(), 1 );
588  returnvalue = point_constraints[run1]->evaluateSensitivities( tmp, hessian );
589  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
590  count++;
591  }
592  }
593  }
594  return SUCCESSFUL_RETURN;
595 }
596 
597 
598 
599 returnValue Constraint::setForwardSeed( BlockMatrix *xSeed_ ,
600  BlockMatrix *xaSeed_,
601  BlockMatrix *pSeed_ ,
602  BlockMatrix *uSeed_ ,
603  BlockMatrix *wSeed_ ,
604  int order ){
605 
606  uint run1;
607  returnValue returnvalue;
608 
609 
610  // BOUNDARY CONSTRAINTS:
611  // ---------------------
612 
613  if( boundary_constraint->getNC() != 0 ){
614  returnvalue = boundary_constraint->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
615  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
616  }
617 
618 
619  // COUPLED PATH CONSTRAINTS:
620  // -------------------------
621 
622  if( coupled_path_constraint->getNC() != 0 ){
623  returnvalue = coupled_path_constraint->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
624  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
625  }
626 
627 
628  // PATH CONSTRAINTS:
629  // -----------------
630 
631  if( path_constraint->getNC() != 0 ){
632  returnvalue = path_constraint->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
633  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
634  }
635 
636 
637  // ALGEBRAIC CONSISTENCY CONSTRAINTS:
638  // ----------------------------------
639 
640  if( algebraic_consistency_constraint->getNC() != 0 ){
641  returnvalue = algebraic_consistency_constraint->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
642  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
643  }
644 
645 
646  // POINT CONSTRAINTS:
647  // ------------------
648 
649  if( point_constraints != 0 ){
650  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
651  if( point_constraints[run1] != 0 ){
652  returnvalue = point_constraints[run1]->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
653  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
654  }
655  }
656  }
657 
658  return SUCCESSFUL_RETURN;
659 }
660 
661 
662 returnValue Constraint::setUnitForwardSeed( ){
663 
664  uint run1;
665  returnValue returnvalue;
666 
667 
668  // BOUNDARY CONSTRAINTS:
669  // ---------------------
670 
671  if( boundary_constraint->getNC() != 0 ){
672  returnvalue = boundary_constraint->setUnitForwardSeed();
673  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
674  }
675 
676 
677  // COUPLED PATH CONSTRAINTS:
678  // -------------------------
679 
680  if( coupled_path_constraint->getNC() != 0 ){
681  returnvalue = coupled_path_constraint->setUnitForwardSeed();
682  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
683  }
684 
685 
686  // PATH CONSTRAINTS:
687  // -----------------
688 
689  if( path_constraint->getNC() != 0 ){
690  returnvalue = path_constraint->setUnitForwardSeed();
691  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
692  }
693 
694  // ALGEBRAIC CONSISTENCY CONSTRAINTS:
695  // ----------------------------------
696 
697  if( algebraic_consistency_constraint->getNC() != 0 ){
698  returnvalue = algebraic_consistency_constraint->setUnitForwardSeed();
699  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
700  }
701 
702 
703  // POINT CONSTRAINTS:
704  // ------------------
705 
706  if( point_constraints != 0 ){
707  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
708  if( point_constraints[run1] != 0 ){
709  returnvalue = point_constraints[run1]->setUnitForwardSeed();
710  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
711  }
712  }
713  }
714 
715  return SUCCESSFUL_RETURN;
716 }
717 
718 
719 returnValue Constraint::setBackwardSeed( BlockMatrix *seed, int order ){
720 
721  return SUCCESSFUL_RETURN;
722 }
723 
724 
725 returnValue Constraint::setUnitBackwardSeed( ){
726 
727 
728  uint run1;
729  returnValue returnvalue;
730 
731  const uint N = grid.getNumPoints();
732 
733  // BOUNDARY CONSTRAINTS:
734  // ---------------------
735 
736  if( boundary_constraint->getNC() != 0 ){
737  BlockMatrix seed( 1, 1 );
738  seed.setIdentity( 0, 0, boundary_constraint->getNC() );
739  returnvalue = boundary_constraint->setBackwardSeed( &seed, 1 );
740  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
741  }
742 
743 
744  // COUPLED PATH CONSTRAINTS:
745  // -------------------------
746 
747  if( coupled_path_constraint->getNC() != 0 ){
748  BlockMatrix seed( 1, 1 );
749  seed.setIdentity( 0, 0, coupled_path_constraint->getNC() );
750  returnvalue = coupled_path_constraint->setBackwardSeed( &seed, 1 );
751  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
752  }
753 
754 
755  // PATH CONSTRAINTS:
756  // -----------------
757 
758  if( path_constraint->getNC() != 0 ){
759  BlockMatrix seed( 1, N );
760  for( run1 = 0; run1 < N; run1++ )
761  seed.setIdentity( 0, run1, path_constraint->getDim(run1) );
762  returnvalue = path_constraint->setBackwardSeed( &seed, 1 );
763  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
764  }
765 
766  // ALGEBRAIC CONSISTENCY CONSTRAINTS:
767  // ----------------------------------
768 
769  if( algebraic_consistency_constraint->getNC() != 0 ){
770  BlockMatrix seed( 1, N );
771  for( run1 = 0; run1 < N; run1++ )
772  seed.setIdentity( 0, run1, algebraic_consistency_constraint->getDim(run1) );
773  returnvalue = algebraic_consistency_constraint->setBackwardSeed( &seed, 1 );
774  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
775  }
776 
777 
778  // POINT CONSTRAINTS:
779  // ------------------
780 
781  if( point_constraints != 0 ){
782  for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
783  if( point_constraints[run1] != 0 ){
784  BlockMatrix seed( 1, 1 );
785  seed.setIdentity( 0, 0, point_constraints[run1]->getNC() );
786  returnvalue = point_constraints[run1]->setBackwardSeed( &seed, 1 );
787  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
788  }
789  }
790  }
791 
792  return SUCCESSFUL_RETURN;
793 }
794 
795 
796 
797 returnValue Constraint::getConstraintResiduum( BlockMatrix &lowerRes, BlockMatrix &upperRes ){
798 
799  const int N = grid.getNumPoints();
800 
801  returnValue returnvalue;
802 
803  BlockMatrix residuumL;
804  BlockMatrix residuumU;
805 
806  residuumL.init( getNumberOfBlocks(), 1 );
807  residuumU.init( getNumberOfBlocks(), 1 );
808 
809  int nc, run1;
810 
811  nc = 0;
812 
813  // BOUNDARY CONSTRAINTS:
814  // ---------------------
815 
816  if( boundary_constraint->getNC() != 0 ){
817  BlockMatrix resL, resU;
818  returnvalue = boundary_constraint->getResiduum( resL, resU );
819  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
820  DMatrix resL_;
821  DMatrix resU_;
822  resL.getSubBlock( 0, 0, resL_ );
823  resU.getSubBlock( 0, 0, resU_ );
824  residuumL.setDense( nc, 0, resL_ );
825  residuumU.setDense( nc, 0, resU_ );
826  nc++;
827  }
828 
829 
830  // COUPLED PATH CONSTRAINTS:
831  // -------------------------
832 
833  if( coupled_path_constraint->getNC() != 0 ){
834  BlockMatrix resL, resU;
835  returnvalue = coupled_path_constraint->getResiduum( resL, resU );
836  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
837  DMatrix resL_;
838  DMatrix resU_;
839  resL.getSubBlock( 0, 0, resL_ );
840  resU.getSubBlock( 0, 0, resU_ );
841  residuumL.setDense( nc, 0, resL_ );
842  residuumU.setDense( nc, 0, resU_ );
843  nc++;
844  }
845 
846 
847  // PATH CONSTRAINTS:
848  // -----------------
849 
850  if( path_constraint->getNC() != 0 ){
851  BlockMatrix resL, resU;
852  returnvalue = path_constraint->getResiduum( resL, resU );
853  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
854  DMatrix resL_;
855  DMatrix resU_;
856  for( run1 = 0; run1 < N; run1++ ){
857  resL.getSubBlock( run1, 0, resL_ );
858  resU.getSubBlock( run1, 0, resU_ );
859  residuumL.setDense( nc, 0, resL_ );
860  residuumU.setDense( nc, 0, resU_ );
861  nc++;
862  }
863  }
864 
865  // ALGEBRAIC CONSISTENCY CONSTRAINTS:
866  // ----------------------------------
867 
868  if( algebraic_consistency_constraint->getNC() != 0 ){
869  BlockMatrix resL, resU;
870  returnvalue = algebraic_consistency_constraint->getResiduum( resL, resU );
871  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
872  DMatrix resL_;
873  DMatrix resU_;
874  for( run1 = 0; run1 < N; run1++ ){
875  resL.getSubBlock( run1, 0, resL_ );
876  resU.getSubBlock( run1, 0, resU_ );
877  residuumL.setDense( nc, 0, resL_ );
878  residuumU.setDense( nc, 0, resU_ );
879  nc++;
880  }
881  }
882 
883  // POINT CONSTRAINTS:
884  // ------------------
885 
886  if( point_constraints != 0 ){
887  for( run1 = 0; run1 < (int) grid.getNumPoints(); run1++ ){
888  if( point_constraints[run1] != 0 ){
889  BlockMatrix resL, resU;
890  returnvalue = point_constraints[run1]->getResiduum( resL, resU );
891  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
892  DMatrix resL_;
893  DMatrix resU_;
894  resL.getSubBlock( 0, 0, resL_ );
895  resU.getSubBlock( 0, 0, resU_ );
896  residuumL.setDense( nc, 0, resL_ );
897  residuumU.setDense( nc, 0, resU_ );
898  nc++;
899  }
900  }
901  }
902 
903  lowerRes = residuumL;
904  upperRes = residuumU;
905 
906  return SUCCESSFUL_RETURN;
907 }
908 
909 
910 returnValue Constraint::getBoundResiduum( BlockMatrix &lowerRes,
911  BlockMatrix &upperRes ){
912 
913 
914  int run1;
915  const int N = grid.getNumPoints();
916 
917  lowerRes.init( 4*N+1, 1 );
918  upperRes.init( 4*N+1, 1 );
919 
920  for( run1 = 0; run1 < N; run1++ ){
921 
922  lowerRes.setDense( run1, 0, residuumXL[run1] );
923  upperRes.setDense( run1, 0, residuumXU[run1] );
924 
925  lowerRes.setDense( N+run1, 0, residuumXAL[run1] );
926  upperRes.setDense( N+run1, 0, residuumXAU[run1] );
927 
928  lowerRes.setDense( 2*N+1+run1, 0, residuumUL[run1] );
929  upperRes.setDense( 2*N+1+run1, 0, residuumUU[run1] );
930 
931  lowerRes.setDense( 3*N+1+run1, 0, residuumWL[run1] );
932  upperRes.setDense( 3*N+1+run1, 0, residuumWU[run1] );
933  }
934  lowerRes.setDense( 2*N, 0, residuumPL[0] );
935  upperRes.setDense( 2*N, 0, residuumPU[0] );
936 
937  return SUCCESSFUL_RETURN;
938 }
939 
940 
941 returnValue Constraint::getForwardSensitivities( BlockMatrix &D, int order ){
942 
943  const int N = grid.getNumPoints();
944 
945  returnValue returnvalue;
946  BlockMatrix result;
947 
948  result.init( getNumberOfBlocks(), 5*N );
949 
950  int nc, run1, run2;
951  nc = 0;
952 
953  // BOUNDARY CONSTRAINTS:
954  // ---------------------
955 
956  if( boundary_constraint->getNC() != 0 ){
957  BlockMatrix res;
958  returnvalue = boundary_constraint->getForwardSensitivities( &res, order );
959  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
960  DMatrix res_;
961  for( run2 = 0; run2 < 5*N; run2++ ){
962  res.getSubBlock( 0 , run2, res_ );
963  if( res_.getDim() > 0 )
964  result.setDense( nc, run2, res_ );
965  }
966  nc++;
967  }
968 
969  // COUPLED PATH CONSTRAINTS:
970  // -------------------------
971 
972  if( coupled_path_constraint->getNC() != 0 ){
973  BlockMatrix res;
974  returnvalue = coupled_path_constraint->getForwardSensitivities( &res, order );
975  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
976  DMatrix res_;
977  for( run2 = 0; run2 < 5*N; run2++ ){
978  res.getSubBlock( 0 , run2, res_ );
979  if( res_.getDim() > 0 )
980  result.setDense( nc, run2, res_ );
981  }
982  nc++;
983  }
984 
985 
986  // PATH CONSTRAINTS:
987  // -----------------
988 
989  if( path_constraint->getNC() != 0 ){
990  BlockMatrix res;
991  returnvalue = path_constraint->getForwardSensitivities( &res, order );
992  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
993  DMatrix res_;
994  for( run1 = 0; run1 < N; run1++ ){
995  for( run2 = 0; run2 < 5*N; run2++ ){
996  res.getSubBlock( run1, run2, res_ );
997  if( res_.getDim() > 0 )
998  result.setDense( nc , run2, res_ );
999  }
1000  nc++;
1001  }
1002  }
1003 
1004 
1005  // ALGEBRAIC CONSISTENCY CONSTRAINTS:
1006  // ----------------------------------
1007 
1008  if( algebraic_consistency_constraint->getNC() != 0 ){
1009  BlockMatrix res;
1010  returnvalue = algebraic_consistency_constraint->getForwardSensitivities( &res, order );
1011  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
1012  DMatrix res_;
1013  for( run1 = 0; run1 < N; run1++ ){
1014  for( run2 = 0; run2 < 5*N; run2++ ){
1015  res.getSubBlock( run1, run2, res_ );
1016  if( res_.getDim() > 0 )
1017  result.setDense( nc , run2, res_ );
1018  }
1019  nc++;
1020  }
1021  }
1022 
1023 
1024 
1025  // POINT CONSTRAINTS:
1026  // ------------------
1027 
1028  if( point_constraints != 0 ){
1029  for( run1 = 0; run1 < (int) grid.getNumPoints(); run1++ ){
1030  if( point_constraints[run1] != 0 ){
1031  BlockMatrix res;
1032  returnvalue = point_constraints[run1]->getForwardSensitivities( &res, order );
1033  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
1034  DMatrix res_;
1035  for( run2 = 0; run2 < 5*N; run2++ ){
1036  res.getSubBlock( 0 , run2, res_ );
1037  if( res_.getDim() > 0 )
1038  result.setDense( nc, run2, res_ );
1039  }
1040  nc++;
1041  }
1042  }
1043  }
1044 
1045  D = result;
1046 
1047  return SUCCESSFUL_RETURN;
1048 }
1049 
1050 
1051 returnValue Constraint::getBackwardSensitivities( BlockMatrix &D, int order ){
1052 
1053  const int N = grid.getNumPoints();
1054 
1055  returnValue returnvalue;
1056  BlockMatrix result;
1057 
1058  result.init( getNumberOfBlocks(), 5*N );
1059 
1060  int nc, run1, run2;
1061  nc = 0;
1062 
1063 
1064  // BOUNDARY CONSTRAINTS:
1065  // ---------------------
1066 
1067  if( boundary_constraint->getNC() != 0 ){
1068  BlockMatrix res;
1069  returnvalue = boundary_constraint->getBackwardSensitivities( &res, order );
1070  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
1071  DMatrix res_;
1072  for( run2 = 0; run2 < 5*N; run2++ ){
1073  res.getSubBlock( 0 , run2, res_ );
1074  if( res_.getDim() > 0 )
1075  result.setDense( nc, run2, res_ );
1076  }
1077  nc++;
1078  }
1079 
1080 
1081  // COUPLED PATH CONSTRAINTS:
1082  // -------------------------
1083 
1084  if( coupled_path_constraint->getNC() != 0 ){
1085  BlockMatrix res;
1086  returnvalue = coupled_path_constraint->getBackwardSensitivities( &res, order );
1087  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
1088  DMatrix res_;
1089  for( run2 = 0; run2 < 5*N; run2++ ){
1090  res.getSubBlock( 0 , run2, res_ );
1091  if( res_.getDim() > 0 )
1092  result.setDense( nc, run2, res_ );
1093  }
1094  nc++;
1095  }
1096 
1097 
1098  // PATH CONSTRAINTS:
1099  // -----------------
1100 
1101  if( path_constraint->getNC() != 0 ){
1102  BlockMatrix res;
1103  returnvalue = path_constraint->getBackwardSensitivities( &res, order );
1104  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
1105  DMatrix res_;
1106  for( run1 = 0; run1 < N; run1++ ){
1107  for( run2 = 0; run2 < 5*N; run2++ ){
1108  res.getSubBlock( run1, run2, res_ );
1109  if( res_.getDim() > 0 )
1110  result.setDense( nc , run2, res_ );
1111  }
1112  nc++;
1113  }
1114  }
1115 
1116 
1117  // ALGEBRAIC CONSISTENCY CONSTRAINTS:
1118  // ----------------------------------
1119 
1120  if( algebraic_consistency_constraint->getNC() != 0 ){
1121  BlockMatrix res;
1122  returnvalue = algebraic_consistency_constraint->getBackwardSensitivities( &res, order );
1123  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
1124  DMatrix res_;
1125  for( run1 = 0; run1 < N; run1++ ){
1126  for( run2 = 0; run2 < 5*N; run2++ ){
1127  res.getSubBlock( run1, run2, res_ );
1128  if( res_.getDim() > 0 )
1129  result.setDense( nc , run2, res_ );
1130  }
1131  nc++;
1132  }
1133  }
1134 
1135 
1136 
1137  // POINT CONSTRAINTS:
1138  // ------------------
1139 
1140  if( point_constraints != 0 ){
1141  for( run1 = 0; run1 < (int) grid.getNumPoints(); run1++ ){
1142  if( point_constraints[run1] != 0 ){
1143  BlockMatrix res;
1144  returnvalue = point_constraints[run1]->getBackwardSensitivities( &res, order );
1145  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
1146  DMatrix res_;
1147  for( run2 = 0; run2 < 5*N; run2++ ){
1148  res.getSubBlock( 0 , run2, res_ );
1149  if( res_.getDim() > 0 )
1150  result.setDense( nc, run2, res_ );
1151  }
1152  nc++;
1153  }
1154  }
1155  }
1156 
1157  D = result;
1158 
1159  return SUCCESSFUL_RETURN;
1160 }
1161 
1162 
1163 
1164 BooleanType Constraint::isEmpty() const{
1165 
1166  if( nb == 0 &&
1167  boundary_constraint == 0 &&
1168  coupled_path_constraint == 0 &&
1169  path_constraint == 0 &&
1170  algebraic_consistency_constraint == 0 &&
1171  point_constraints == 0 ) return BT_TRUE;
1172 
1173  return BT_FALSE;
1174 }
1175 
1176 
1177 
1178 //
1179 // PROTECTED MEMBER FUNCTIONS:
1180 //
1181 
1182 returnValue Constraint::add( const int index_, const double lb_,
1183  const Expression &arg, const double ub_ ){
1184 
1185  if( index_ >= (int) grid.getNumPoints() ) return ACADOERROR(RET_INDEX_OUT_OF_BOUNDS);
1186  if( index_ < 0 ) return ACADOERROR(RET_INDEX_OUT_OF_BOUNDS);
1187 
1188 
1189  // CHECK FEASIBILITY:
1190  // ------------------
1191  if( lb_ > ub_ + EPS ) return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
1192 
1193  if( point_constraints[index_] == 0 )
1194  point_constraints[index_] = new PointConstraint(grid,index_);
1195 
1196  return point_constraints[index_]->add( lb_, arg, ub_ );
1197 }
1198 
1199 
1200 returnValue Constraint::add( const double lb_, const Expression& arg1,
1201  const Expression& arg2, const double ub_ ){
1202 
1203  // CHECK FEASIBILITY:
1204  // ------------------
1205  if( lb_ > ub_ + EPS ) return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
1206 
1207  return boundary_constraint->add( lb_, arg1, arg2, ub_ );
1208 }
1209 
1210 
1211 returnValue Constraint::getBounds( const OCPiterate& iter ){
1212 
1213  returnValue returnvalue;
1214  returnvalue = BoxConstraint::getBounds( iter );
1215 
1216  if( returnvalue != SUCCESSFUL_RETURN ) return ACADOWARNING(returnvalue);
1217 
1218  if( point_constraints != 0 )
1219  {
1220  for( uint i=0; i<grid.getNumPoints(); ++i )
1221  if( point_constraints[i] != 0 )
1222  returnvalue = point_constraints[i]->getBounds( iter );
1223  }
1224 
1225  return returnvalue;
1226 }
1227 
1228 returnValue Constraint::getPathConstraints(Function& function_, DMatrix& lb_, DMatrix& ub_) const
1229 {
1230  return path_constraint->get(function_, lb_, ub_);
1231 }
1232 
1233 returnValue Constraint::getPointConstraint(const unsigned index_, Function& function_, DMatrix& lb_, DMatrix& ub_) const
1234 {
1235  if (point_constraints[ index_ ] == 0)
1236  {
1237  Function tmp;
1238 
1239  function_ = tmp;
1240 
1241  lb_.init(0, 0);
1242  ub_.init(0, 0);
1243 
1244  return SUCCESSFUL_RETURN;
1245  }
1246 
1247  return point_constraints[ index_ ]->get(function_, lb_, ub_);
1248 }
1249 
1250 CLOSE_NAMESPACE_ACADO
1251 
1252 // end of file.
Grid::getNumPoints
uint getNumPoints() const
BoxConstraint::residuumPU
DMatrix * residuumPU
Definition: box_constraint.hpp:125
N
#define N
Definition: crane_kul_mhe_test.cpp:44
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
PathConstraint::evaluateSensitivities
returnValue evaluateSensitivities()
Definition: path_constraint.cpp:113
Function
Allows to setup and evaluate a general function based on SymbolicExpressions.
Definition: function_.hpp:59
ConstraintComponent::getUB
const DVector & getUB() const
ConstraintElement::init
returnValue init(const OCPiterate &iter)
Definition: constraint_element.cpp:479
PathConstraint::add
returnValue add(const DVector lb_, const Expression &arg, const DVector ub_)
GenericMatrix::init
void init(unsigned _nRows=0, unsigned _nCols=0)
Definition: matrix.hpp:135
RET_INDEX_OUT_OF_BOUNDS
Definition: acado_types.hpp:838
BoxConstraint::init
returnValue init(const Grid &grid_)
Definition: box_constraint.cpp:67
Constraint::setUnitForwardSeed
virtual returnValue setUnitForwardSeed()
Definition: constraint.cpp:662
SUCCESSFUL_RETURN
Definition: acado_types.hpp:836
Constraint::setBackwardSeed
virtual returnValue setBackwardSeed(BlockMatrix *seed, int order)
Definition: constraint.cpp:719
AlgebraicConsistencyConstraint::evaluate
returnValue evaluate(const OCPiterate &iter)
Definition: algebraic_consistency_constraint.cpp:150
PathConstraint::getNC
int getNC() const
BoxConstraint::residuumXAU
DMatrix * residuumXAU
Definition: box_constraint.hpp:123
ConstraintComponent::getUBgrid
const VariablesGrid & getUBgrid() const
PointConstraint::evaluate
returnValue evaluate(const OCPiterate &iter)
Definition: point_constraint.cpp:190
EPS
BEGIN_NAMESPACE_ACADO const double EPS
Definition: acado_constants.hpp:43
BlockMatrix::setDense
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
Definition: block_matrix.cpp:446
Constraint
Stores and evaluates the constraints of optimal control problems.
Definition: constraint.hpp:60
BoxConstraint::residuumUL
DMatrix * residuumUL
Definition: box_constraint.hpp:126
Constraint::boundary_constraint
BoundaryConstraint * boundary_constraint
Definition: constraint.hpp:462
VariablesGrid
Provides a time grid consisting of vector-valued optimization variables at each grid point...
Definition: variables_grid.hpp:56
BoundaryConstraint::getNC
int getNC() const
ConstraintComponent::getLBgrid
const VariablesGrid & getLBgrid() const
returnValue
Allows to pass back messages to the calling function.
Definition: acado_types.hpp:1139
Constraint::setUnitBackwardSeed
virtual returnValue setUnitBackwardSeed()
Definition: constraint.cpp:725
BoxConstraint
Stores and evaluates box constraints within optimal control problems.
Definition: box_constraint.hpp:59
RET_ASSERTION
Definition: acado_types.hpp:850
PointConstraint::evaluateSensitivities
returnValue evaluateSensitivities()
Definition: point_constraint.cpp:225
RET_INFEASIBLE_CONSTRAINT
Definition: acado_types.hpp:977
VT_INTERMEDIATE_STATE
Definition: acado_types.hpp:105
uint
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
ConstraintComponent::hasUBgrid
BooleanType hasUBgrid() const
Grid
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
BoxConstraint::residuumXL
DMatrix * residuumXL
Definition: box_constraint.hpp:120
BoundaryConstraint
Stores and evaluates boundary constraints within optimal control problems.
Definition: boundary_constraint.hpp:55
PointConstraint
Stores and evaluates pointwise constraints within optimal control problems.
Definition: point_constraint.hpp:55
CLOSE_NAMESPACE_ACADO
#define CLOSE_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:58
PathConstraint
Stores and evaluates path constraints within optimal control problems.
Definition: path_constraint.hpp:57
Constraint::setForwardSeed
virtual returnValue setForwardSeed(BlockMatrix *xSeed_, BlockMatrix *xaSeed_, BlockMatrix *pSeed_, BlockMatrix *uSeed_, BlockMatrix *wSeed_, int order)
Definition: constraint.cpp:599
ConstraintComponent
Data class for symbolically formulating constraints within optimal control problems.
Definition: constraint_component.hpp:56
Constraint::getBoundResiduum
virtual returnValue getBoundResiduum(BlockMatrix &lowerRes, BlockMatrix &upperRes)
Definition: constraint.cpp:910
VariableType
VariableType
Definition: acado_types.hpp:95
BoxConstraint::var
VariableType * var
Definition: box_constraint.hpp:115
Constraint::getBounds
virtual returnValue getBounds(const OCPiterate &iter)
Definition: constraint.cpp:1211
ACADO_TRY
#define ACADO_TRY(X)
Definition: acado_message_handling.hpp:144
Constraint::add
returnValue add(const double lb_, const Expression &arg, const double ub_)
Definition: constraint.cpp:194
Expression
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
AlgebraicConsistencyConstraint
Deals with algebraic consistency constraints within optimal control problems.
Definition: algebraic_consistency_constraint.hpp:65
BoundaryConstraint::evaluate
returnValue evaluate(const OCPiterate &iter)
Definition: boundary_constraint.cpp:86
RET_MEMBER_NOT_INITIALISED
Definition: acado_types.hpp:851
BoxConstraint::residuumPL
DMatrix * residuumPL
Definition: box_constraint.hpp:124
ConstraintComponent::getLB
const DVector & getLB() const
ConstraintElement::setBackwardSeed
virtual returnValue setBackwardSeed(BlockMatrix *seed, int order)
Definition: constraint_element.cpp:649
BooleanType
BooleanType
Definition: examples/code_generation/mpc_mhe/getting_started_export/qpoases/INCLUDE/Types.hpp:44
ConstraintElement::setUnitForwardSeed
virtual returnValue setUnitForwardSeed()
Definition: constraint_element.cpp:631
Constraint::path_constraint
PathConstraint * path_constraint
Definition: constraint.hpp:464
ConstraintElement::getResiduum
virtual returnValue getResiduum(BlockMatrix &lower_residuum, BlockMatrix &upper_residuum)
Definition: constraint_element.cpp:681
Constraint::evaluate
returnValue evaluate(const OCPiterate &iter)
Definition: constraint.cpp:399
ConstraintElement::setForwardSeed
virtual returnValue setForwardSeed(BlockMatrix *xSeed_, BlockMatrix *xaSeed_, BlockMatrix *pSeed_, BlockMatrix *uSeed_, BlockMatrix *wSeed_, int order)
Definition: constraint_element.cpp:530
BlockMatrix::init
returnValue init(uint _nRows, uint _nCols)
Definition: block_matrix.cpp:65
BoxConstraint::residuumXU
DMatrix * residuumXU
Definition: box_constraint.hpp:121
BoxConstraint::residuumXAL
DMatrix * residuumXAL
Definition: box_constraint.hpp:122
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
ConstraintComponent::hasLBgrid
BooleanType hasLBgrid() const
Constraint::getForwardSensitivities
virtual returnValue getForwardSensitivities(BlockMatrix &D, int order)
Definition: constraint.cpp:941
Constraint::algebraic_consistency_constraint
AlgebraicConsistencyConstraint * algebraic_consistency_constraint
Definition: constraint.hpp:465
Constraint::operator=
Constraint & operator=(const Constraint &rhs)
Definition: constraint.cpp:151
CoupledPathConstraint
Stores and evaluates coupled path constraints within optimal control problems.
Definition: coupled_path_constraint.hpp:57
CoupledPathConstraint::evaluate
returnValue evaluate(const OCPiterate &iter)
Definition: coupled_path_constraint.cpp:78
BlockMatrix::getSubBlock
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
rhs
void rhs(const real_t *x, real_t *f)
Definition: examples/code_generation/simulation/externModel_export/model.c:5
BoxConstraint::getBounds
virtual returnValue getBounds(const OCPiterate &iter)
Definition: box_constraint.cpp:369
PathConstraint::evaluate
returnValue evaluate(const OCPiterate &iter)
Definition: path_constraint.cpp:76
Grid::getTime
double getTime(uint pointIdx) const
PathConstraint::getDim
int getDim(const int &idx_)
BoxConstraint::evaluateBounds
returnValue evaluateBounds(const OCPiterate &iter)
Definition: box_constraint.cpp:218
Expression::isVariable
BooleanType isVariable() const
BT_TRUE
#define BT_TRUE
Definition: acado_types.hpp:47
BoxConstraint::operator=
BoxConstraint & operator=(const BoxConstraint &rhs)
Definition: box_constraint.cpp:168
Constraint::evaluateSensitivities
returnValue evaluateSensitivities()
Definition: constraint.cpp:475
Constraint::getPathConstraints
returnValue getPathConstraints(Function &function_, DMatrix &lb_, DMatrix &ub_) const
Definition: constraint.cpp:1228
DVector
GenericVector< double > DVector
Definition: vector.hpp:329
GenericVector::setAll
void setAll(const T &_value)
Definition: vector.hpp:160
Constraint::getPointConstraint
returnValue getPointConstraint(const unsigned index, Function &function_, DMatrix &lb_, DMatrix &ub_) const
Definition: constraint.cpp:1233
BoundaryConstraint::evaluateSensitivities
returnValue evaluateSensitivities()
Definition: boundary_constraint.cpp:138
GenericVector::getDim
unsigned getDim() const
Definition: vector.hpp:172
BoundaryConstraint::add
returnValue add(const double lb_, const Expression &arg1, const Expression &arg2, const double ub_)
Constraint::isEmpty
BooleanType isEmpty() const
Definition: constraint.cpp:1164
BoxConstraint::residuumWU
DMatrix * residuumWU
Definition: box_constraint.hpp:129
ACADOWARNING
#define ACADOWARNING(retval)
Definition: acado_message_handling.hpp:122
BEGIN_NAMESPACE_ACADO
#define BEGIN_NAMESPACE_ACADO
Definition: acado_namespace_macros.hpp:57
ConstraintComponent::getExpression
Expression getExpression() const
BT_FALSE
#define BT_FALSE
Definition: acado_types.hpp:49
BoxConstraint::residuumUU
DMatrix * residuumUU
Definition: box_constraint.hpp:127
AlgebraicConsistencyConstraint::getDim
int getDim(const int &idx_)
MatrixVariablesGrid::linearInterpolation
DVector linearInterpolation(double time) const
Definition: matrix_variables_grid.cpp:669
BoxConstraint::residuumWL
DMatrix * residuumWL
Definition: box_constraint.hpp:128
Constraint::~Constraint
virtual ~Constraint()
Definition: constraint.cpp:95
Constraint::point_constraints
PointConstraint ** point_constraints
Definition: constraint.hpp:466
Expression::getVariableType
VariableType getVariableType() const
ConstraintElement::getForwardSensitivities
virtual returnValue getForwardSensitivities(BlockMatrix *D, int order)
Definition: constraint_element.cpp:692
Constraint::coupled_path_constraint
CoupledPathConstraint * coupled_path_constraint
Definition: constraint.hpp:463
Constraint::getNumberOfBlocks
int getNumberOfBlocks() const
PointConstraint::getBounds
returnValue getBounds(const OCPiterate &iter)
Definition: point_constraint.cpp:605
Expression::getComponent
uint getComponent(const unsigned int idx) const
BlockMatrix::setIdentity
returnValue setIdentity(uint rowIdx, uint colIdx, uint dim)
CoupledPathConstraint::add
returnValue add(const double lb_, const Expression *arg, const double ub_)
GenericMatrix::getDim
unsigned getDim() const
Definition: matrix.hpp:181
Constraint::getConstraintResiduum
virtual returnValue getConstraintResiduum(BlockMatrix &lowerRes, BlockMatrix &upperRes)
Definition: constraint.cpp:797
ACADOERROR
#define ACADOERROR(retval)
Definition: acado_message_handling.hpp:106
ACADOERRORTEXT
#define ACADOERRORTEXT(retval, text)
Definition: acado_message_handling.hpp:110
Constraint::init
returnValue init(const Grid &grid_, const int &numberOfStages_=1)
Definition: constraint.cpp:129
Constraint::deleteAll
void deleteAll()
Definition: constraint.cpp:101
AlgebraicConsistencyConstraint::add
returnValue add(const uint &endOfStage_, const DifferentialEquation &dae)
DifferentialEquation
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
Definition: differential_equation.hpp:55
Constraint::getBackwardSensitivities
virtual returnValue getBackwardSensitivities(BlockMatrix &D, int order)
Definition: constraint.cpp:1051

acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Feb 28 2022 21:31:52