constraint.cpp
Go to the documentation of this file.
00001 /*
00002  *    This file is part of ACADO Toolkit.
00003  *
00004  *    ACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.
00005  *    Copyright (C) 2008-2014 by Boris Houska, Hans Joachim Ferreau,
00006  *    Milan Vukov, Rien Quirynen, KU Leuven.
00007  *    Developed within the Optimization in Engineering Center (OPTEC)
00008  *    under supervision of Moritz Diehl. All rights reserved.
00009  *
00010  *    ACADO Toolkit is free software; you can redistribute it and/or
00011  *    modify it under the terms of the GNU Lesser General Public
00012  *    License as published by the Free Software Foundation; either
00013  *    version 3 of the License, or (at your option) any later version.
00014  *
00015  *    ACADO Toolkit is distributed in the hope that it will be useful,
00016  *    but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00018  *    Lesser General Public License for more details.
00019  *
00020  *    You should have received a copy of the GNU Lesser General Public
00021  *    License along with ACADO Toolkit; if not, write to the Free Software
00022  *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
00023  *
00024  */
00025 
00026 
00027 
00035 #include <acado/function/function.hpp>
00036 #include <acado/constraint/constraint.hpp>
00037 
00038 
00039 
00040 BEGIN_NAMESPACE_ACADO
00041 
00042 
00043 
00044 //
00045 // PUBLIC MEMBER FUNCTIONS:
00046 //
00047 
00048 
00049 Constraint::Constraint( )
00050            :BoxConstraint(){
00051 
00052      boundary_constraint              = 0;
00053      coupled_path_constraint          = 0;
00054      path_constraint                  = 0;
00055      algebraic_consistency_constraint = 0;
00056      point_constraints                = 0;
00057 }
00058 
00059 
00060 
00061 Constraint::Constraint( const Constraint& rhs )
00062            :BoxConstraint(rhs){
00063 
00064     uint run1;
00065 
00066     if( rhs.boundary_constraint != 0 )
00067             boundary_constraint = new BoundaryConstraint(*rhs.boundary_constraint);
00068     else    boundary_constraint = 0                                               ;
00069 
00070     if( rhs.coupled_path_constraint != 0 )
00071             coupled_path_constraint = new CoupledPathConstraint(*rhs.coupled_path_constraint);
00072     else    coupled_path_constraint = 0                                                      ;
00073 
00074     if( rhs.path_constraint != 0 )
00075             path_constraint = new PathConstraint(*rhs.path_constraint);
00076     else    path_constraint = 0                                       ;
00077 
00078     if( rhs.algebraic_consistency_constraint != 0 )
00079             algebraic_consistency_constraint = new AlgebraicConsistencyConstraint(*rhs.algebraic_consistency_constraint);
00080     else    algebraic_consistency_constraint = 0;
00081 
00082     if( rhs.point_constraints != 0 ){
00083         point_constraints = new PointConstraint*[grid.getNumPoints()];
00084         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00085             if( rhs.point_constraints[run1] != 0 ){
00086                     point_constraints[run1] = new PointConstraint(*rhs.point_constraints[run1]);
00087             }
00088             else    point_constraints[run1] = 0;
00089         }
00090     }
00091     else    point_constraints = 0;
00092 }
00093 
00094 
00095 Constraint::~Constraint( ){
00096 
00097     deleteAll();
00098 }
00099 
00100 
00101 void Constraint::deleteAll(){
00102 
00103    uint run1;
00104 
00105     if( boundary_constraint != 0 )
00106         delete boundary_constraint;
00107 
00108     if( coupled_path_constraint != 0 )
00109         delete coupled_path_constraint;
00110 
00111     if( path_constraint != 0 )
00112         delete path_constraint;
00113 
00114     if( algebraic_consistency_constraint != 0 )
00115         delete algebraic_consistency_constraint;
00116 
00117     if( point_constraints != 0 ){
00118 
00119         for( run1 = 0; run1 < grid.getNumPoints(); run1++ )
00120             if( point_constraints[run1] != 0 )
00121                 delete point_constraints[run1];
00122 
00123         delete[] point_constraints;
00124     }
00125 }
00126 
00127 
00128 
00129 returnValue Constraint::init( const Grid& grid_, const int& numberOfStages_ ){
00130 
00131      deleteAll();
00132 
00133      BoxConstraint::init( grid_ );
00134 
00135      uint run1;
00136 
00137      boundary_constraint               = new BoundaryConstraint            (grid                );
00138      coupled_path_constraint           = new CoupledPathConstraint         (grid                );
00139      path_constraint                   = new PathConstraint                (grid                );
00140      algebraic_consistency_constraint  = new AlgebraicConsistencyConstraint(grid,numberOfStages_);
00141      point_constraints                 = new PointConstraint*[grid.getNumPoints()];
00142 
00143      for( run1 = 0; run1 < grid.getNumPoints(); run1++ )
00144          point_constraints[run1] = 0;
00145 
00146     return SUCCESSFUL_RETURN;
00147 }
00148 
00149 
00150 
00151 Constraint& Constraint::operator=( const Constraint& rhs ){
00152 
00153     uint run1;
00154 
00155     if( this != &rhs ){
00156 
00157         deleteAll();
00158 
00159         BoxConstraint::operator=(rhs);
00160 
00161         if( rhs.boundary_constraint != 0 )
00162                 boundary_constraint = new BoundaryConstraint(*rhs.boundary_constraint);
00163         else    boundary_constraint = 0                                               ;
00164 
00165         if( rhs.coupled_path_constraint != 0 )
00166                 coupled_path_constraint = new CoupledPathConstraint(*rhs.coupled_path_constraint);
00167         else    coupled_path_constraint = 0                                                      ;
00168 
00169         if( rhs.path_constraint != 0 )
00170                 path_constraint = new PathConstraint(*rhs.path_constraint);
00171         else    path_constraint = 0                                       ;
00172 
00173         if( rhs.algebraic_consistency_constraint != 0 )
00174                 algebraic_consistency_constraint = new AlgebraicConsistencyConstraint(*rhs.algebraic_consistency_constraint);
00175         else    algebraic_consistency_constraint = 0;
00176 
00177         if( rhs.point_constraints != 0 ){
00178             point_constraints = new PointConstraint*[grid.getNumPoints()];
00179             for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00180                 if( rhs.point_constraints[run1] != 0 ){
00181                         point_constraints[run1] = new PointConstraint(*rhs.point_constraints[run1]);
00182                 }
00183                 else    point_constraints[run1] = 0;
00184             }
00185         }
00186         else    point_constraints = 0;
00187 
00188     }
00189     return *this;
00190 }
00191 
00192 
00193 
00194 returnValue Constraint::add( const double lb_, const Expression& arg, const double ub_  ){
00195 
00196     DVector tmp_lb(grid.getNumPoints());
00197     DVector tmp_ub(grid.getNumPoints());
00198 
00199     tmp_lb.setAll(lb_);
00200     tmp_ub.setAll(ub_);
00201 
00202     return add( tmp_lb, arg, tmp_ub );
00203 }
00204 
00205 
00206 
00207 returnValue Constraint::add( const DVector lb_, const Expression& arg, const double ub_  ){
00208 
00209     DVector tmp_ub(grid.getNumPoints());
00210     tmp_ub.setAll(ub_);
00211 
00212     return add( lb_, arg, tmp_ub );
00213 }
00214 
00215 
00216 returnValue Constraint::add( const double lb_, const Expression& arg, const DVector ub_  ){
00217 
00218     DVector tmp_lb(grid.getNumPoints());
00219     tmp_lb.setAll(lb_);
00220 
00221     return add( tmp_lb, arg, ub_ );
00222 }
00223 
00224 
00225 returnValue Constraint::add( const DVector lb_, const Expression &arg, const DVector ub_  ){
00226 
00227     // CHECK FEASIBILITY:
00228     // ------------------
00229     if( lb_.getDim() != ub_.getDim()        )  return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
00230     if( lb_.getDim() != grid.getNumPoints() )  return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
00231     if( (lb_ <= (const DVector&)ub_) == BT_FALSE            )  return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
00232 
00233 
00234     // CHECK FOR A BOUND:
00235     // -----------------------
00236     VariableType varType   = arg.getVariableType();
00237     int          component = arg.getComponent(0)  ;
00238 
00239     if( arg.isVariable( ) == BT_TRUE ){
00240         if( varType != VT_INTERMEDIATE_STATE ){
00241 
00242              nb++;
00243              var   = (VariableType*)realloc(var  , nb*sizeof(VariableType));
00244              index = (int*         )realloc(index, nb*sizeof(int         ));
00245              blb   = (DVector**     )realloc(blb  , nb*sizeof(DVector*     ));
00246              bub   = (DVector**     )realloc(bub  , nb*sizeof(DVector*     ));
00247 
00248              var  [nb-1] = varType  ;
00249              index[nb-1] = component;
00250              blb  [nb-1] = new DVector( lb_ );
00251              bub  [nb-1] = new DVector( ub_ );
00252 
00253              return SUCCESSFUL_RETURN;
00254        }
00255     }
00256 
00257 
00258     // SAVE THE ARGUMENT AS A Path Constraint:
00259     // ---------------------------------------
00260     return path_constraint->add( lb_, arg, ub_ );
00261 }
00262 
00263 
00264 returnValue Constraint::add( const double lb_, const Expression *arguments, const double ub_ ){
00265 
00266     // CHECK FEASIBILITY:
00267     // ------------------
00268     if( lb_ > ub_ + EPS )  return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
00269 
00270     return coupled_path_constraint->add( lb_, arguments, ub_ );
00271 }
00272 
00273 
00274 returnValue Constraint::add( const uint&                 endOfStage_ ,
00275                                     const DifferentialEquation& dae           ){
00276 
00277     return algebraic_consistency_constraint->add( endOfStage_, dae );
00278 }
00279 
00280 
00281 returnValue Constraint::add( const ConstraintComponent& component ){
00282 
00283 
00284     DVector tmp_ub(grid.getNumPoints());
00285     DVector tmp_lb(grid.getNumPoints());
00286 
00287     uint run1;
00288 
00289     if( component.hasLBgrid() == 0 ){
00290 
00291         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00292             if( (component.getLB()).getDim() == 1 )
00293                 tmp_lb(run1) = (component.getLB()).operator()(0);
00294             else{
00295                 if( (component.getLB()).getDim() <= run1 )
00296                     return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
00297                 tmp_lb(run1) = (component.getLB()).operator()(run1);
00298             }
00299         }
00300     }
00301     else{
00302 
00303         VariablesGrid LBgrid = component.getLBgrid();
00304 
00305         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00306             DVector tmp = LBgrid.linearInterpolation( grid.getTime(run1) );
00307             tmp_lb(run1) = tmp(0);
00308         }
00309     }
00310 
00311 
00312     if( component.hasUBgrid() == 0 ){
00313         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00314             if( (component.getUB()).getDim() == 1 )
00315                 tmp_ub(run1) = (component.getUB()).operator()(0);
00316             else{
00317                 if( (component.getUB()).getDim() <= run1 )
00318                     return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
00319                 tmp_ub(run1) = (component.getUB()).operator()(run1);
00320             }
00321         }
00322     }
00323     else{
00324 
00325         VariablesGrid UBgrid = component.getUBgrid();
00326 
00327         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00328             DVector tmp = UBgrid.linearInterpolation( grid.getTime(run1) );
00329             tmp_ub(run1) = tmp(0);
00330         }
00331     }
00332 
00333     return add( tmp_lb, component.getExpression(), tmp_ub );
00334 }
00335 
00336 
00337 returnValue Constraint::add( const int index_, const ConstraintComponent& component ){
00338 
00339     DVector tmp_ub(grid.getNumPoints());
00340     DVector tmp_lb(grid.getNumPoints());
00341 
00342     if ( !(index_ < (int) grid.getNumPoints()) )
00343         return ACADOERRORTEXT(RET_ASSERTION,
00344                         "The constraint component can not be set as the associated "
00345                         "discretization point is not in the time horizon.");
00346 
00347     uint run1;
00348 
00349     if( component.hasLBgrid() == 0 ){
00350 
00351         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00352             if( (component.getLB()).getDim() == 1 )
00353                 tmp_lb(run1) = (component.getLB()).operator()(0);
00354             else{
00355                 if( (component.getLB()).getDim() <= run1 )
00356                     return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
00357                 tmp_lb(run1) = (component.getLB()).operator()(run1);
00358             }
00359         }
00360     }
00361     else{
00362 
00363         VariablesGrid LBgrid = component.getLBgrid();
00364 
00365         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00366             DVector tmp = LBgrid.linearInterpolation( grid.getTime(run1) );
00367             tmp_lb(run1) = tmp(0);
00368         }
00369     }
00370 
00371 
00372     if( component.hasUBgrid() == 0 ){
00373         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00374             if( (component.getUB()).getDim() == 1 )
00375                 tmp_ub(run1) = (component.getUB()).operator()(0);
00376             else{
00377                 if( (component.getUB()).getDim() <= run1 )
00378                     return ACADOWARNING(RET_INFEASIBLE_CONSTRAINT);
00379                 tmp_ub(run1) = (component.getUB()).operator()(run1);
00380             }
00381         }
00382     }
00383     else{
00384 
00385         VariablesGrid UBgrid = component.getUBgrid();
00386 
00387         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00388             DVector tmp = UBgrid.linearInterpolation( grid.getTime(run1) );
00389             tmp_ub(run1) = tmp(0);
00390         }
00391     }
00392 
00393     ACADO_TRY( add( index_, tmp_lb(index_), component.getExpression(), tmp_ub(index_) ) );
00394 
00395     return SUCCESSFUL_RETURN;
00396 }
00397 
00398 
00399 returnValue Constraint::evaluate( const OCPiterate& iter ){
00400 
00401 
00402     if( grid.getNumPoints() == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00403 
00404     uint run1;
00405     returnValue returnvalue;
00406 
00407 
00408     // EVALUATE BOUNDARY CONSTRAINS:
00409     // -----------------------------
00410 
00411     if( boundary_constraint->getNC() != 0 ){
00412         returnvalue = boundary_constraint->init( iter );
00413         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00414         returnvalue = boundary_constraint->evaluate( iter );
00415         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00416     }
00417 
00418 
00419     // EVALUATE COUPLED PATH CONSTRAINS:
00420     // ---------------------------------
00421 
00422     if( coupled_path_constraint->getNC() != 0 ){
00423         returnvalue = coupled_path_constraint->init( iter );
00424         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00425         returnvalue = coupled_path_constraint->evaluate( iter );
00426         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00427     }
00428 
00429 
00430     // EVALUATE PATH CONSTRAINS:
00431     // -------------------------
00432 
00433     if( path_constraint->getNC() != 0 ){
00434         returnvalue = path_constraint->init( iter );
00435         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00436         returnvalue = path_constraint->evaluate( iter );
00437         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00438     }
00439 
00440 
00441     // EVALUATE ALGEBRAIC CONSISTENCY CONSTRAINS:
00442     // ------------------------------------------
00443 
00444     if( algebraic_consistency_constraint->getNC() != 0 ){
00445         returnvalue = algebraic_consistency_constraint->init( iter );
00446         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00447         returnvalue = algebraic_consistency_constraint->evaluate( iter );
00448         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00449     }
00450 
00451 
00452     // EVALUATE POINT CONSTRAINS:
00453     // --------------------------
00454 
00455     if( point_constraints != 0 ){
00456         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00457             if( point_constraints[run1] != 0 ){
00458                 returnvalue = point_constraints[run1]->init( iter );
00459                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00460                 returnvalue = point_constraints[run1]->evaluate( iter );
00461                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00462             }
00463         }
00464     }
00465 
00466 
00467     // EVALUATE BOUNDS:
00468     // ----------------
00469 
00470     return evaluateBounds( iter );
00471 }
00472 
00473 
00474 
00475 returnValue Constraint::evaluateSensitivities( ){
00476 
00477     uint run1;
00478     returnValue returnvalue;
00479 
00480 
00481     // EVALUATE BOUNDARY CONSTRAINS:
00482     // -----------------------------
00483 
00484     if( boundary_constraint->getNC() != 0 ){
00485         returnvalue = boundary_constraint->evaluateSensitivities( );
00486         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00487     }
00488 
00489 
00490     // EVALUATE COUPLED PATH CONSTRAINS:
00491     // ---------------------------------
00492 
00493     if( coupled_path_constraint->getNC() != 0 ){
00494         returnvalue = coupled_path_constraint->evaluateSensitivities( );
00495         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00496     }
00497 
00498 
00499     // EVALUATE PATH CONSTRAINS:
00500     // -------------------------
00501 
00502     if( path_constraint->getNC() != 0 ){
00503         returnvalue = path_constraint->evaluateSensitivities( );
00504         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00505     }
00506 
00507 
00508     // EVALUATE ALGEBRAIC CONSISTENCY CONSTRAINS:
00509     // ------------------------------------------
00510 
00511     if( algebraic_consistency_constraint->getNC() != 0 ){
00512         returnvalue = algebraic_consistency_constraint->evaluateSensitivities( );
00513         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00514     }
00515 
00516     // EVALUATE POINT CONSTRAINS:
00517     // --------------------------
00518 
00519     if( point_constraints != 0 ){
00520         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00521             if( point_constraints[run1] != 0 ){
00522                 returnvalue = point_constraints[run1]->evaluateSensitivities( );
00523                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00524             }
00525         }
00526     }
00527 
00528     return SUCCESSFUL_RETURN;
00529 }
00530 
00531 
00532 returnValue Constraint::evaluateSensitivities( const BlockMatrix &seed, BlockMatrix &hessian ){
00533 
00534 
00535     uint run1 ;
00536     int  count;
00537     returnValue returnvalue;
00538 
00539     count = 0;
00540     DMatrix tmp;
00541 
00542     // EVALUATE BOUNDARY CONSTRAINS:
00543     // -----------------------------
00544 
00545     if( boundary_constraint->getNC() != 0 ){
00546         seed.getSubBlock( count, 0, tmp, boundary_constraint->getNC(), 1 );
00547         returnvalue = boundary_constraint->evaluateSensitivities( tmp, hessian );
00548         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00549         count++;
00550     }
00551 
00552 
00553     // EVALUATE COUPLED PATH CONSTRAINS:
00554     // ---------------------------------
00555 
00556     if( coupled_path_constraint->getNC() != 0 ){
00557         seed.getSubBlock( count, 0, tmp, coupled_path_constraint->getNC(), 1 );
00558         returnvalue = coupled_path_constraint->evaluateSensitivities( tmp, hessian );
00559         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00560         count++;
00561     }
00562 
00563 
00564     // EVALUATE PATH CONSTRAINS:
00565     // -------------------------
00566 
00567     if( path_constraint->getNC() != 0 ){
00568         returnvalue = path_constraint->evaluateSensitivities( count, seed, hessian );
00569         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00570     }
00571 
00572 
00573     // EVALUATE ALGEBRAIC CONSISTENCY CONSTRAINS:
00574     // ------------------------------------------
00575 
00576     if( algebraic_consistency_constraint->getNC() != 0 ){
00577         returnvalue = algebraic_consistency_constraint->evaluateSensitivities( count, seed, hessian );
00578         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00579     }
00580 
00581     // EVALUATE POINT CONSTRAINS:
00582     // --------------------------
00583 
00584     if( point_constraints != 0 ){
00585         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00586             if( point_constraints[run1] != 0 ){
00587                 seed.getSubBlock( count, 0, tmp, point_constraints[run1]->getNC(), 1 );
00588                 returnvalue = point_constraints[run1]->evaluateSensitivities( tmp, hessian );
00589                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00590                 count++;
00591             }
00592         }
00593     }
00594     return SUCCESSFUL_RETURN;
00595 }
00596 
00597 
00598 
00599 returnValue Constraint::setForwardSeed( BlockMatrix *xSeed_ ,
00600                                         BlockMatrix *xaSeed_,
00601                                         BlockMatrix *pSeed_ ,
00602                                         BlockMatrix *uSeed_ ,
00603                                         BlockMatrix *wSeed_ ,
00604                                         int          order    ){
00605 
00606     uint run1;
00607     returnValue returnvalue;
00608 
00609 
00610     // BOUNDARY CONSTRAINTS:
00611     // ---------------------
00612 
00613     if( boundary_constraint->getNC() != 0 ){
00614         returnvalue = boundary_constraint->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
00615         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00616     }
00617 
00618 
00619     // COUPLED PATH CONSTRAINTS:
00620     // -------------------------
00621 
00622     if( coupled_path_constraint->getNC() != 0 ){
00623         returnvalue = coupled_path_constraint->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
00624         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00625     }
00626 
00627 
00628     // PATH CONSTRAINTS:
00629     // -----------------
00630 
00631     if( path_constraint->getNC() != 0 ){
00632         returnvalue = path_constraint->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
00633         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00634     }
00635 
00636 
00637     // ALGEBRAIC CONSISTENCY CONSTRAINTS:
00638     // ----------------------------------
00639 
00640     if( algebraic_consistency_constraint->getNC() != 0 ){
00641         returnvalue = algebraic_consistency_constraint->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
00642         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00643     }
00644 
00645 
00646     // POINT CONSTRAINTS:
00647     // ------------------
00648 
00649     if( point_constraints != 0 ){
00650         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00651             if( point_constraints[run1] != 0 ){
00652                 returnvalue = point_constraints[run1]->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
00653                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00654             }
00655         }
00656     }
00657 
00658     return SUCCESSFUL_RETURN;
00659 }
00660 
00661 
00662 returnValue Constraint::setUnitForwardSeed( ){
00663 
00664     uint run1;
00665     returnValue returnvalue;
00666 
00667 
00668     // BOUNDARY CONSTRAINTS:
00669     // ---------------------
00670 
00671     if( boundary_constraint->getNC() != 0 ){
00672         returnvalue = boundary_constraint->setUnitForwardSeed();
00673         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00674     }
00675 
00676 
00677     // COUPLED PATH CONSTRAINTS:
00678     // -------------------------
00679 
00680     if( coupled_path_constraint->getNC() != 0 ){
00681         returnvalue = coupled_path_constraint->setUnitForwardSeed();
00682         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00683     }
00684 
00685 
00686     // PATH CONSTRAINTS:
00687     // -----------------
00688 
00689     if( path_constraint->getNC() != 0 ){
00690         returnvalue = path_constraint->setUnitForwardSeed();
00691         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00692     }
00693 
00694     // ALGEBRAIC CONSISTENCY CONSTRAINTS:
00695     // ----------------------------------
00696 
00697     if( algebraic_consistency_constraint->getNC() != 0 ){
00698         returnvalue = algebraic_consistency_constraint->setUnitForwardSeed();
00699         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00700     }
00701 
00702 
00703     // POINT CONSTRAINTS:
00704     // ------------------
00705 
00706     if( point_constraints != 0 ){
00707         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00708             if( point_constraints[run1] != 0 ){
00709                 returnvalue = point_constraints[run1]->setUnitForwardSeed();
00710                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00711             }
00712         }
00713     }
00714 
00715     return SUCCESSFUL_RETURN;
00716 }
00717 
00718 
00719 returnValue Constraint::setBackwardSeed( BlockMatrix *seed, int order ){
00720 
00721     return SUCCESSFUL_RETURN;
00722 }
00723 
00724 
00725 returnValue Constraint::setUnitBackwardSeed( ){
00726 
00727 
00728     uint run1;
00729     returnValue returnvalue;
00730 
00731     const uint N = grid.getNumPoints();
00732 
00733     // BOUNDARY CONSTRAINTS:
00734     // ---------------------
00735 
00736     if( boundary_constraint->getNC() != 0 ){
00737         BlockMatrix seed( 1, 1 );
00738         seed.setIdentity( 0, 0, boundary_constraint->getNC() );
00739         returnvalue = boundary_constraint->setBackwardSeed( &seed, 1 );
00740         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00741     }
00742 
00743 
00744     // COUPLED PATH CONSTRAINTS:
00745     // -------------------------
00746 
00747     if( coupled_path_constraint->getNC() != 0 ){
00748         BlockMatrix seed( 1, 1 );
00749         seed.setIdentity( 0, 0, coupled_path_constraint->getNC() );
00750         returnvalue = coupled_path_constraint->setBackwardSeed( &seed, 1 );
00751         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00752     }
00753 
00754 
00755     // PATH CONSTRAINTS:
00756     // -----------------
00757 
00758     if( path_constraint->getNC() != 0 ){
00759         BlockMatrix seed( 1, N );
00760         for( run1 = 0; run1 < N; run1++ )
00761             seed.setIdentity( 0, run1, path_constraint->getDim(run1) );
00762         returnvalue = path_constraint->setBackwardSeed( &seed, 1 );
00763         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00764     }
00765 
00766     // ALGEBRAIC CONSISTENCY CONSTRAINTS:
00767     // ----------------------------------
00768 
00769     if( algebraic_consistency_constraint->getNC() != 0 ){
00770         BlockMatrix seed( 1, N );
00771         for( run1 = 0; run1 < N; run1++ )
00772             seed.setIdentity( 0, run1, algebraic_consistency_constraint->getDim(run1) );
00773         returnvalue = algebraic_consistency_constraint->setBackwardSeed( &seed, 1 );
00774         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00775     }
00776 
00777 
00778     // POINT CONSTRAINTS:
00779     // ------------------
00780 
00781     if( point_constraints != 0 ){
00782         for( run1 = 0; run1 < grid.getNumPoints(); run1++ ){
00783             if( point_constraints[run1] != 0 ){
00784                 BlockMatrix seed( 1, 1 );
00785                 seed.setIdentity( 0, 0, point_constraints[run1]->getNC() );
00786                 returnvalue = point_constraints[run1]->setBackwardSeed( &seed, 1 );
00787                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00788             }
00789         }
00790     }
00791 
00792     return SUCCESSFUL_RETURN;
00793 }
00794 
00795 
00796 
00797 returnValue Constraint::getConstraintResiduum( BlockMatrix &lowerRes, BlockMatrix &upperRes ){
00798 
00799     const int N = grid.getNumPoints();
00800 
00801     returnValue returnvalue;
00802 
00803     BlockMatrix residuumL;
00804     BlockMatrix residuumU;
00805 
00806     residuumL.init( getNumberOfBlocks(), 1 );
00807     residuumU.init( getNumberOfBlocks(), 1 );
00808 
00809     int nc, run1;
00810 
00811     nc = 0;
00812 
00813     // BOUNDARY CONSTRAINTS:
00814     // ---------------------
00815 
00816     if( boundary_constraint->getNC() != 0 ){
00817         BlockMatrix resL, resU;
00818         returnvalue = boundary_constraint->getResiduum( resL, resU );
00819         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00820         DMatrix resL_;
00821         DMatrix resU_;
00822         resL.getSubBlock( 0, 0, resL_ );
00823         resU.getSubBlock( 0, 0, resU_ );
00824         residuumL.setDense( nc, 0, resL_ );
00825         residuumU.setDense( nc, 0, resU_ );
00826         nc++;
00827     }
00828 
00829 
00830     // COUPLED PATH CONSTRAINTS:
00831     // -------------------------
00832 
00833     if( coupled_path_constraint->getNC() != 0 ){
00834         BlockMatrix resL, resU;
00835         returnvalue = coupled_path_constraint->getResiduum( resL, resU );
00836         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00837         DMatrix resL_;
00838         DMatrix resU_;
00839         resL.getSubBlock( 0, 0, resL_ );
00840         resU.getSubBlock( 0, 0, resU_ );
00841         residuumL.setDense( nc, 0, resL_ );
00842         residuumU.setDense( nc, 0, resU_ );
00843         nc++;
00844     }
00845 
00846 
00847     // PATH CONSTRAINTS:
00848     // -----------------
00849 
00850     if( path_constraint->getNC() != 0 ){
00851         BlockMatrix resL, resU;
00852         returnvalue = path_constraint->getResiduum( resL, resU );
00853         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00854         DMatrix resL_;
00855         DMatrix resU_;
00856         for( run1 = 0; run1 < N; run1++ ){
00857             resL.getSubBlock( run1, 0, resL_ );
00858             resU.getSubBlock( run1, 0, resU_ );
00859             residuumL.setDense( nc, 0, resL_ );
00860             residuumU.setDense( nc, 0, resU_ );
00861             nc++;
00862         }
00863     }
00864 
00865     // ALGEBRAIC CONSISTENCY CONSTRAINTS:
00866     // ----------------------------------
00867 
00868     if( algebraic_consistency_constraint->getNC() != 0 ){
00869         BlockMatrix resL, resU;
00870         returnvalue = algebraic_consistency_constraint->getResiduum( resL, resU );
00871         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00872         DMatrix resL_;
00873         DMatrix resU_;
00874         for( run1 = 0; run1 < N; run1++ ){
00875             resL.getSubBlock( run1, 0, resL_ );
00876             resU.getSubBlock( run1, 0, resU_ );
00877             residuumL.setDense( nc, 0, resL_ );
00878             residuumU.setDense( nc, 0, resU_ );
00879             nc++;
00880         }
00881     }
00882 
00883     // POINT CONSTRAINTS:
00884     // ------------------
00885 
00886     if( point_constraints != 0 ){
00887         for( run1 = 0; run1 < (int) grid.getNumPoints(); run1++ ){
00888             if( point_constraints[run1] != 0 ){
00889                 BlockMatrix resL, resU;
00890                 returnvalue = point_constraints[run1]->getResiduum( resL, resU );
00891                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00892                 DMatrix resL_;
00893                 DMatrix resU_;
00894                 resL.getSubBlock( 0, 0, resL_ );
00895                 resU.getSubBlock( 0, 0, resU_ );
00896                 residuumL.setDense( nc, 0, resL_ );
00897                 residuumU.setDense( nc, 0, resU_ );
00898                 nc++;
00899             }
00900         }
00901     }
00902 
00903     lowerRes = residuumL;
00904     upperRes = residuumU;
00905 
00906     return SUCCESSFUL_RETURN;
00907 }
00908 
00909 
00910 returnValue Constraint::getBoundResiduum( BlockMatrix &lowerRes,
00911                                           BlockMatrix &upperRes ){
00912 
00913 
00914     int run1;
00915     const int N = grid.getNumPoints();
00916 
00917     lowerRes.init( 4*N+1, 1 );
00918     upperRes.init( 4*N+1, 1 );
00919 
00920     for( run1 = 0; run1 < N; run1++ ){
00921 
00922         lowerRes.setDense( run1, 0, residuumXL[run1] );
00923         upperRes.setDense( run1, 0, residuumXU[run1] );
00924 
00925         lowerRes.setDense( N+run1, 0, residuumXAL[run1] );
00926         upperRes.setDense( N+run1, 0, residuumXAU[run1] );
00927 
00928         lowerRes.setDense( 2*N+1+run1, 0, residuumUL[run1] );
00929         upperRes.setDense( 2*N+1+run1, 0, residuumUU[run1] );
00930 
00931         lowerRes.setDense( 3*N+1+run1, 0, residuumWL[run1] );
00932         upperRes.setDense( 3*N+1+run1, 0, residuumWU[run1] );
00933     }
00934     lowerRes.setDense( 2*N, 0, residuumPL[0] );
00935     upperRes.setDense( 2*N, 0, residuumPU[0] );
00936 
00937     return SUCCESSFUL_RETURN;
00938 }
00939 
00940 
00941 returnValue Constraint::getForwardSensitivities( BlockMatrix &D, int order ){
00942 
00943     const int N = grid.getNumPoints();
00944 
00945     returnValue returnvalue;
00946     BlockMatrix result;
00947 
00948     result.init( getNumberOfBlocks(), 5*N );
00949 
00950     int nc, run1, run2;
00951     nc = 0;
00952 
00953     // BOUNDARY CONSTRAINTS:
00954     // ---------------------
00955 
00956     if( boundary_constraint->getNC() != 0 ){
00957         BlockMatrix res;
00958         returnvalue = boundary_constraint->getForwardSensitivities( &res, order );
00959         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00960         DMatrix res_;
00961         for( run2 = 0; run2 < 5*N; run2++ ){
00962             res.getSubBlock( 0 , run2, res_ );
00963             if( res_.getDim() > 0 )
00964                 result.setDense( nc, run2, res_ );
00965         }
00966         nc++;
00967     }
00968 
00969     // COUPLED PATH CONSTRAINTS:
00970     // -------------------------
00971 
00972     if( coupled_path_constraint->getNC() != 0 ){
00973         BlockMatrix res;
00974         returnvalue = coupled_path_constraint->getForwardSensitivities( &res, order );
00975         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00976         DMatrix res_;
00977         for( run2 = 0; run2 < 5*N; run2++ ){
00978             res.getSubBlock( 0 , run2, res_ );
00979             if( res_.getDim() > 0 )
00980                 result.setDense( nc, run2, res_ );
00981         }
00982         nc++;
00983     }
00984 
00985 
00986     // PATH CONSTRAINTS:
00987     // -----------------
00988 
00989     if( path_constraint->getNC() != 0 ){
00990         BlockMatrix res;
00991         returnvalue = path_constraint->getForwardSensitivities( &res, order );
00992         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
00993         DMatrix res_;
00994         for( run1 = 0; run1 < N; run1++ ){
00995             for( run2 = 0; run2 < 5*N; run2++ ){
00996                 res.getSubBlock( run1, run2, res_ );
00997                 if( res_.getDim() > 0 )
00998                     result.setDense( nc  , run2, res_ );
00999             }
01000             nc++;
01001         }
01002     }
01003 
01004 
01005     // ALGEBRAIC CONSISTENCY CONSTRAINTS:
01006     // ----------------------------------
01007 
01008     if( algebraic_consistency_constraint->getNC() != 0 ){
01009         BlockMatrix res;
01010         returnvalue = algebraic_consistency_constraint->getForwardSensitivities( &res, order );
01011         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
01012         DMatrix res_;
01013         for( run1 = 0; run1 < N; run1++ ){
01014             for( run2 = 0; run2 < 5*N; run2++ ){
01015                 res.getSubBlock( run1, run2, res_ );
01016                 if( res_.getDim() > 0 )
01017                     result.setDense( nc  , run2, res_ );
01018             }
01019             nc++;
01020         }
01021     }
01022 
01023 
01024 
01025     // POINT CONSTRAINTS:
01026     // ------------------
01027 
01028     if( point_constraints != 0 ){
01029         for( run1 = 0; run1 < (int) grid.getNumPoints(); run1++ ){
01030             if( point_constraints[run1] != 0 ){
01031                 BlockMatrix res;
01032                 returnvalue = point_constraints[run1]->getForwardSensitivities( &res, order );
01033                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
01034                 DMatrix res_;
01035                 for( run2 = 0; run2 < 5*N; run2++ ){
01036                     res.getSubBlock( 0 , run2, res_ );
01037                     if( res_.getDim() > 0 )
01038                         result.setDense( nc, run2, res_ );
01039                 }
01040                 nc++;
01041             }
01042         }
01043     }
01044 
01045     D = result;
01046 
01047     return SUCCESSFUL_RETURN;
01048 }
01049 
01050 
01051 returnValue Constraint::getBackwardSensitivities( BlockMatrix &D, int order ){
01052 
01053     const int N = grid.getNumPoints();
01054 
01055     returnValue returnvalue;
01056     BlockMatrix result;
01057 
01058     result.init( getNumberOfBlocks(), 5*N );
01059 
01060     int nc, run1, run2;
01061     nc = 0;
01062 
01063 
01064     // BOUNDARY CONSTRAINTS:
01065     // ---------------------
01066 
01067     if( boundary_constraint->getNC() != 0 ){
01068         BlockMatrix res;
01069         returnvalue = boundary_constraint->getBackwardSensitivities( &res, order );
01070         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
01071         DMatrix res_;
01072         for( run2 = 0; run2 < 5*N; run2++ ){
01073             res.getSubBlock( 0 , run2, res_ );
01074             if( res_.getDim() > 0 )
01075                 result.setDense( nc, run2, res_ );
01076         }
01077         nc++;
01078     }
01079 
01080 
01081     // COUPLED PATH CONSTRAINTS:
01082     // -------------------------
01083 
01084     if( coupled_path_constraint->getNC() != 0 ){
01085         BlockMatrix res;
01086         returnvalue = coupled_path_constraint->getBackwardSensitivities( &res, order );
01087         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
01088         DMatrix res_;
01089         for( run2 = 0; run2 < 5*N; run2++ ){
01090             res.getSubBlock( 0 , run2, res_ );
01091             if( res_.getDim() > 0 )
01092                 result.setDense( nc, run2, res_ );
01093         }
01094         nc++;
01095     }
01096 
01097 
01098     // PATH CONSTRAINTS:
01099     // -----------------
01100 
01101     if( path_constraint->getNC() != 0 ){
01102         BlockMatrix res;
01103         returnvalue = path_constraint->getBackwardSensitivities( &res, order );
01104         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
01105         DMatrix res_;
01106         for( run1 = 0; run1 < N; run1++ ){
01107             for( run2 = 0; run2 < 5*N; run2++ ){
01108                 res.getSubBlock( run1, run2, res_ );
01109                 if( res_.getDim() > 0 )
01110                     result.setDense( nc  , run2, res_ );
01111             }
01112             nc++;
01113         }
01114     }
01115 
01116 
01117     // ALGEBRAIC CONSISTENCY CONSTRAINTS:
01118     // ----------------------------------
01119 
01120     if( algebraic_consistency_constraint->getNC() != 0 ){
01121         BlockMatrix res;
01122         returnvalue = algebraic_consistency_constraint->getBackwardSensitivities( &res, order );
01123         if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
01124         DMatrix res_;
01125         for( run1 = 0; run1 < N; run1++ ){
01126             for( run2 = 0; run2 < 5*N; run2++ ){
01127                 res.getSubBlock( run1, run2, res_ );
01128                 if( res_.getDim() > 0 )
01129                     result.setDense( nc  , run2, res_ );
01130             }
01131             nc++;
01132         }
01133     }
01134 
01135 
01136 
01137     // POINT CONSTRAINTS:
01138     // ------------------
01139 
01140     if( point_constraints != 0 ){
01141         for( run1 = 0; run1 < (int) grid.getNumPoints(); run1++ ){
01142             if( point_constraints[run1] != 0 ){
01143                 BlockMatrix res;
01144                 returnvalue = point_constraints[run1]->getBackwardSensitivities( &res, order );
01145                 if( returnvalue != SUCCESSFUL_RETURN ) return ACADOERROR(returnvalue);
01146                 DMatrix res_;
01147                 for( run2 = 0; run2 < 5*N; run2++ ){
01148                     res.getSubBlock( 0 , run2, res_ );
01149                     if( res_.getDim() > 0 )
01150                         result.setDense( nc, run2, res_ );
01151                 }
01152                 nc++;
01153             }
01154         }
01155     }
01156 
01157     D = result;
01158 
01159     return SUCCESSFUL_RETURN;
01160 }
01161 
01162 
01163 
01164 BooleanType Constraint::isEmpty() const{
01165 
01166     if( nb                               == 0 &&
01167         boundary_constraint              == 0 &&
01168         coupled_path_constraint          == 0 &&
01169         path_constraint                  == 0 &&
01170         algebraic_consistency_constraint == 0 &&
01171         point_constraints                == 0    ) return BT_TRUE;
01172 
01173     return BT_FALSE;
01174 }
01175 
01176 
01177 
01178 //
01179 // PROTECTED MEMBER FUNCTIONS:
01180 //
01181 
01182 returnValue Constraint::add( const int index_, const double lb_,
01183                                     const Expression &arg, const double ub_  ){
01184 
01185     if( index_ >= (int) grid.getNumPoints() )  return ACADOERROR(RET_INDEX_OUT_OF_BOUNDS);
01186     if( index_ <  0                         )  return ACADOERROR(RET_INDEX_OUT_OF_BOUNDS);
01187 
01188 
01189     // CHECK FEASIBILITY:
01190     // ------------------
01191     if( lb_ > ub_ + EPS )  return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
01192 
01193     if( point_constraints[index_] == 0 )
01194         point_constraints[index_] = new PointConstraint(grid,index_);
01195 
01196     return point_constraints[index_]->add( lb_, arg, ub_ );
01197 }
01198 
01199 
01200 returnValue Constraint::add( const double lb_, const Expression& arg1,
01201                                     const Expression& arg2, const double ub_ ){
01202 
01203     // CHECK FEASIBILITY:
01204     // ------------------
01205     if( lb_ > ub_ + EPS )  return ACADOERROR(RET_INFEASIBLE_CONSTRAINT);
01206 
01207     return boundary_constraint->add( lb_, arg1, arg2, ub_ );
01208 }
01209 
01210 
01211 returnValue Constraint::getBounds( const OCPiterate& iter ){
01212 
01213     returnValue returnvalue;
01214     returnvalue = BoxConstraint::getBounds( iter );
01215 
01216     if( returnvalue != SUCCESSFUL_RETURN ) return ACADOWARNING(returnvalue);
01217 
01218     if( point_constraints != 0 )
01219         {
01220                 for( uint i=0; i<grid.getNumPoints(); ++i )
01221                         if( point_constraints[i] != 0 )
01222                                 returnvalue = point_constraints[i]->getBounds( iter );
01223         }
01224 
01225     return returnvalue;
01226 }
01227 
01228 returnValue Constraint::getPathConstraints(Function& function_, DMatrix& lb_, DMatrix& ub_) const
01229 {
01230         return path_constraint->get(function_, lb_, ub_);
01231 }
01232 
01233 returnValue Constraint::getPointConstraint(const unsigned index_, Function& function_, DMatrix& lb_, DMatrix& ub_) const
01234 {
01235         if (point_constraints[ index_ ] == 0)
01236         {
01237                 Function tmp;
01238 
01239                 function_ = tmp;
01240 
01241                 lb_.init(0, 0);
01242                 ub_.init(0, 0);
01243 
01244                 return SUCCESSFUL_RETURN;
01245         }
01246 
01247         return point_constraints[ index_ ]->get(function_, lb_, ub_);
01248 }
01249 
01250 CLOSE_NAMESPACE_ACADO
01251 
01252 // end of file.


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:36:56