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 
37 
38 
39 
41 
42 
43 
44 //
45 // PUBLIC MEMBER FUNCTIONS:
46 //
47 
48 
50  :BoxConstraint(){
51 
54  path_constraint = 0;
57 }
58 
59 
60 
62  :BoxConstraint(rhs){
63 
64  uint run1;
65 
66  if( rhs.boundary_constraint != 0 )
68  else boundary_constraint = 0 ;
69 
70  if( rhs.coupled_path_constraint != 0 )
72  else coupled_path_constraint = 0 ;
73 
74  if( rhs.path_constraint != 0 )
76  else path_constraint = 0 ;
77 
81 
82  if( rhs.point_constraints != 0 ){
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 
96 
97  deleteAll();
98 }
99 
100 
102 
103  uint run1;
104 
105  if( boundary_constraint != 0 )
106  delete boundary_constraint;
107 
108  if( coupled_path_constraint != 0 )
110 
111  if( path_constraint != 0 )
112  delete path_constraint;
113 
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 
142 
143  for( run1 = 0; run1 < grid.getNumPoints(); run1++ )
144  point_constraints[run1] = 0;
145 
146  return SUCCESSFUL_RETURN;
147 }
148 
149 
150 
152 
153  uint run1;
154 
155  if( this != &rhs ){
156 
157  deleteAll();
158 
160 
161  if( rhs.boundary_constraint != 0 )
163  else boundary_constraint = 0 ;
164 
165  if( rhs.coupled_path_constraint != 0 )
167  else coupled_path_constraint = 0 ;
168 
169  if( rhs.path_constraint != 0 )
171  else path_constraint = 0 ;
172 
173  if( rhs.algebraic_consistency_constraint != 0 )
176 
177  if( rhs.point_constraints != 0 ){
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);
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 
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 )
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 )
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()) )
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 )
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 )
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 
400 
401 
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 
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 
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 ){
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 
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 
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 
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 
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 
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 
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 ){
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 
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 
720 
721  return SUCCESSFUL_RETURN;
722 }
723 
724 
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 
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 
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 
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 
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 
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 
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 
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 
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 
1165 
1166  if( nb == 0 &&
1167  boundary_constraint == 0 &&
1168  coupled_path_constraint == 0 &&
1169  path_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 
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 
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 
1251 
1252 // end of file.
DMatrix * residuumPU
#define N
BooleanType isVariable() const
returnValue add(const double lb_, const Expression &arg, const double ub_)
const DVector & getLB() const
Data class for storing generic optimization variables.
Definition: ocp_iterate.hpp:57
Implements a very rudimentary block sparse matrix class.
returnValue evaluateSensitivities()
Allows to setup and evaluate a general function based on SymbolicExpressions.
Definition: function_.hpp:59
returnValue init(const OCPiterate &iter)
returnValue add(const DVector lb_, const Expression &arg, const DVector ub_)
void init(unsigned _nRows=0, unsigned _nCols=0)
Definition: matrix.hpp:135
returnValue init(const Grid &grid_)
Expression getExpression() const
returnValue getPathConstraints(Function &function_, DMatrix &lb_, DMatrix &ub_) const
double getTime(uint pointIdx) const
virtual returnValue setUnitForwardSeed()
Definition: constraint.cpp:662
virtual returnValue setBackwardSeed(BlockMatrix *seed, int order)
Definition: constraint.cpp:719
returnValue evaluate(const OCPiterate &iter)
const VariablesGrid & getLBgrid() const
DMatrix * residuumXAU
returnValue evaluate(const OCPiterate &iter)
BEGIN_NAMESPACE_ACADO const double EPS
returnValue setDense(uint rowIdx, uint colIdx, const DMatrix &value)
returnValue getPointConstraint(const unsigned index, Function &function_, DMatrix &lb_, DMatrix &ub_) const
Stores and evaluates the constraints of optimal control problems.
Definition: constraint.hpp:60
DMatrix * residuumUL
BoundaryConstraint * boundary_constraint
Definition: constraint.hpp:462
Provides a time grid consisting of vector-valued optimization variables at each grid point...
int getNumberOfBlocks() const
Allows to pass back messages to the calling function.
virtual returnValue setUnitBackwardSeed()
Definition: constraint.cpp:725
Stores and evaluates box constraints within optimal control problems.
returnValue evaluateSensitivities()
BEGIN_NAMESPACE_ACADO typedef unsigned int uint
Definition: acado_types.hpp:42
Allows to conveniently handle (one-dimensional) grids consisting of time points.
Definition: grid.hpp:58
DMatrix * residuumXL
Stores and evaluates boundary constraints within optimal control problems.
Stores and evaluates pointwise constraints within optimal control problems.
BooleanType hasLBgrid() const
#define CLOSE_NAMESPACE_ACADO
Stores and evaluates path constraints within optimal control problems.
virtual returnValue setForwardSeed(BlockMatrix *xSeed_, BlockMatrix *xaSeed_, BlockMatrix *pSeed_, BlockMatrix *uSeed_, BlockMatrix *wSeed_, int order)
Definition: constraint.cpp:599
Data class for symbolically formulating constraints within optimal control problems.
virtual returnValue getBoundResiduum(BlockMatrix &lowerRes, BlockMatrix &upperRes)
Definition: constraint.cpp:910
VariableType
Definition: acado_types.hpp:95
const DVector & getUB() const
VariableType * var
virtual returnValue getBounds(const OCPiterate &iter)
#define ACADO_TRY(X)
returnValue add(const double lb_, const Expression &arg, const double ub_)
Definition: constraint.cpp:194
Base class for all variables within the symbolic expressions family.
Definition: expression.hpp:56
Deals with algebraic consistency constraints within optimal control problems.
returnValue evaluate(const OCPiterate &iter)
DMatrix * residuumPL
returnValue getSubBlock(uint rowIdx, uint colIdx, DMatrix &value) const
int getNC() const
virtual returnValue setBackwardSeed(BlockMatrix *seed, int order)
virtual returnValue setUnitForwardSeed()
PathConstraint * path_constraint
Definition: constraint.hpp:464
DVector linearInterpolation(double time) const
virtual returnValue getResiduum(BlockMatrix &lower_residuum, BlockMatrix &upper_residuum)
returnValue evaluate(const OCPiterate &iter)
Definition: constraint.cpp:399
unsigned getDim() const
Definition: matrix.hpp:181
virtual returnValue setForwardSeed(BlockMatrix *xSeed_, BlockMatrix *xaSeed_, BlockMatrix *pSeed_, BlockMatrix *uSeed_, BlockMatrix *wSeed_, int order)
BooleanType hasUBgrid() const
unsigned getDim() const
Definition: vector.hpp:172
returnValue init(uint _nRows, uint _nCols)
DMatrix * residuumXU
DMatrix * residuumXAL
virtual returnValue getBackwardSensitivities(BlockMatrix *D, int order)
returnValue get(Function &function_, DMatrix &lb_, DMatrix &ub_)
virtual returnValue getForwardSensitivities(BlockMatrix &D, int order)
Definition: constraint.cpp:941
AlgebraicConsistencyConstraint * algebraic_consistency_constraint
Definition: constraint.hpp:465
Constraint & operator=(const Constraint &rhs)
Definition: constraint.cpp:151
Stores and evaluates coupled path constraints within optimal control problems.
returnValue evaluate(const OCPiterate &iter)
BooleanType isEmpty() const
void rhs(const real_t *x, real_t *f)
virtual returnValue getBounds(const OCPiterate &iter)
returnValue evaluate(const OCPiterate &iter)
int getDim(const int &idx_)
returnValue evaluateBounds(const OCPiterate &iter)
#define BT_TRUE
Definition: acado_types.hpp:47
BoxConstraint & operator=(const BoxConstraint &rhs)
returnValue evaluateSensitivities()
Definition: constraint.cpp:475
VariableType getVariableType() const
GenericVector< double > DVector
Definition: vector.hpp:329
uint getNumPoints() const
void setAll(const T &_value)
Definition: vector.hpp:160
returnValue evaluateSensitivities()
const VariablesGrid & getUBgrid() const
uint getComponent(const unsigned int idx) const
returnValue add(const double lb_, const Expression &arg1, const Expression &arg2, const double ub_)
DMatrix * residuumWU
#define ACADOWARNING(retval)
#define BEGIN_NAMESPACE_ACADO
#define BT_FALSE
Definition: acado_types.hpp:49
DMatrix * residuumUU
int getDim(const int &idx_)
DMatrix * residuumWL
virtual ~Constraint()
Definition: constraint.cpp:95
PointConstraint ** point_constraints
Definition: constraint.hpp:466
virtual returnValue getForwardSensitivities(BlockMatrix *D, int order)
int getNC() const
CoupledPathConstraint * coupled_path_constraint
Definition: constraint.hpp:463
returnValue getBounds(const OCPiterate &iter)
returnValue setIdentity(uint rowIdx, uint colIdx, uint dim)
returnValue add(const double lb_, const Expression *arg, const double ub_)
virtual returnValue getConstraintResiduum(BlockMatrix &lowerRes, BlockMatrix &upperRes)
Definition: constraint.cpp:797
#define ACADOERROR(retval)
#define ACADOERRORTEXT(retval, text)
returnValue init(const Grid &grid_, const int &numberOfStages_=1)
Definition: constraint.cpp:129
void deleteAll()
Definition: constraint.cpp:101
returnValue add(const uint &endOfStage_, const DifferentialEquation &dae)
Allows to setup and evaluate differential equations (ODEs and DAEs) based on SymbolicExpressions.
virtual returnValue getBackwardSensitivities(BlockMatrix &D, int order)


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