constraint_element.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 
00034 #include <acado/constraint/constraint_element.hpp>
00035 
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 
00040 //
00041 // PUBLIC MEMBER FUNCTIONS:
00042 //
00043 
00044 
00045 ConstraintElement::ConstraintElement( ){
00046 
00047     fcn     = 0;
00048     lb      = 0;
00049     ub      = 0;
00050     y_index = 0;
00051     t_index = 0;
00052 
00053         z       = 0;
00054         JJ      = 0;
00055 
00056     nx      = 0;
00057     na      = 0;
00058     nu      = 0;
00059     np      = 0;
00060     nw      = 0;
00061     ny      = 0;
00062     nFcn    = 0;
00063     nB      = 0;
00064 
00065     xSeed         = 0     ;
00066     xaSeed        = 0     ;
00067     pSeed         = 0     ;
00068     uSeed         = 0     ;
00069     wSeed         = 0     ;
00070     bSeed         = 0     ;
00071     xSeed2        = 0     ;
00072     xaSeed2       = 0     ;
00073     pSeed2        = 0     ;
00074     uSeed2        = 0     ;
00075     wSeed2        = 0     ;
00076     bSeed2        = 0     ;
00077 
00078     condType      = CT_SPARSE;
00079 }
00080 
00081 
00082 ConstraintElement::ConstraintElement( const Grid& grid_, int nFcn_, int nB_ ){
00083 
00084     int run1;
00085 
00086     grid    = grid_;
00087     nFcn    = nFcn_;
00088     nB      = nB_  ;
00089 
00090     fcn     = new Function[nFcn];
00091     lb      = new double* [  nB];
00092     ub      = new double* [  nB];
00093 
00094     for( run1 = 0; run1 < nB; run1++ ){
00095         lb[run1] = 0;
00096         ub[run1] = 0;
00097     }
00098 
00099     y_index = new int*    [nFcn];
00100     t_index = new int     [nFcn];
00101 
00102     for( run1 = 0; run1 < nFcn; run1++ ){
00103         y_index[run1] = 0;
00104     }
00105 
00106         z       = 0;
00107         JJ      = 0;
00108 
00109     nx      = 0;
00110     na      = 0;
00111     nu      = 0;
00112     np      = 0;
00113     nw      = 0;
00114     ny      = 0;
00115 
00116     xSeed         = 0     ;
00117     xaSeed        = 0     ;
00118     pSeed         = 0     ;
00119     uSeed         = 0     ;
00120     wSeed         = 0     ;
00121     bSeed         = 0     ;
00122     xSeed2        = 0     ;
00123     xaSeed2       = 0     ;
00124     pSeed2        = 0     ;
00125     uSeed2        = 0     ;
00126     wSeed2        = 0     ;
00127     bSeed2        = 0     ;
00128 
00129     condType      = CT_SPARSE;
00130 }
00131 
00132 
00133 ConstraintElement::ConstraintElement( const ConstraintElement& rhs ){
00134 
00135     int run1, run2;
00136 
00137     grid    = rhs.grid;
00138     nFcn    = rhs.nFcn;
00139     nB      = rhs.nB  ;
00140 
00141     if( rhs.fcn     != 0 )  fcn     = new Function[nFcn];
00142     else                    fcn     = 0                 ;
00143 
00144     if( rhs.lb      != 0 )  lb      = new double* [nB  ];
00145     else                    lb      = 0                 ;
00146 
00147     if( rhs.ub      != 0 )  ub      = new double* [nB  ];
00148     else                    ub      = 0                 ;
00149 
00150     if( rhs.y_index != 0 )  y_index = new int*    [nFcn];
00151     else                    y_index = 0                 ;
00152 
00153     if( rhs.t_index != 0 )  t_index = new int     [nFcn];
00154     else                    t_index = 0                 ;
00155 
00156         if( rhs.z       != 0 )  z       = new EvaluationPoint( *(rhs.z) );
00157         else                    z       = 0                 ;
00158 
00159         if( rhs.JJ      != 0 )  JJ      = new EvaluationPoint( *(rhs.JJ) );
00160         else                    JJ      = 0                 ;
00161         
00162     nx      = rhs.nx;
00163     na      = rhs.na;
00164     nu      = rhs.nu;
00165     np      = rhs.np;
00166     nw      = rhs.nw;
00167     ny      = rhs.ny;
00168 
00169     for( run1 = 0; run1 < nFcn; run1++ ){
00170 
00171         fcn[run1] = rhs.fcn[run1];
00172 
00173         if( ny > 0 ){
00174             y_index[run1] = new int[ny];
00175             for( run2 = 0; run2 < ny; run2++ )
00176                 y_index[run1][run2] = rhs.y_index[run1][run2];
00177         }
00178         else{
00179             y_index[run1] = 0;
00180         }
00181 
00182         t_index[run1] = rhs.t_index[run1];
00183     }
00184 
00185     if( fcn == 0 ){
00186         for( run1 = 0; run1 < nB; run1++ ){
00187             lb[run1] = 0;
00188             ub[run1] = 0;
00189         }
00190     }
00191     else{
00192         for( run1 = 0; run1 < nB; run1++ ){
00193 
00194             if( fcn[0].getDim() > 0 ){
00195                 lb[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
00196                 ub[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
00197             }
00198             else{
00199                 lb[run1] = 0;
00200                 ub[run1] = 0;
00201             }
00202 
00203             for( run2 = 0; run2 < fcn[0].getDim(); run2++ ){
00204                 lb[run1][run2] = rhs.lb[run1][run2];
00205                 ub[run1][run2] = rhs.ub[run1][run2];
00206             }
00207         }
00208     }
00209 
00210     if( rhs.xSeed != 0 )  xSeed  = new BlockMatrix(*rhs.xSeed );
00211     else                  xSeed  = 0                           ;
00212     if( rhs.xaSeed != 0 ) xaSeed = new BlockMatrix(*rhs.xaSeed);
00213     else                  xaSeed = 0                           ;
00214     if( rhs.pSeed != 0 )  pSeed  = new BlockMatrix(*rhs.pSeed) ;
00215     else                  pSeed  = 0                           ;
00216     if( rhs.uSeed != 0 )  uSeed  = new BlockMatrix(*rhs.uSeed) ;
00217     else                  uSeed  = 0                           ;
00218     if( rhs.wSeed != 0 )  wSeed  = new BlockMatrix(*rhs.wSeed) ;
00219     else                  wSeed  = 0                           ;
00220 
00221     if( rhs.bSeed != 0 )  bSeed  = new BlockMatrix(*rhs.bSeed );
00222     else                  bSeed  = 0                           ;
00223 
00224     if( rhs.xSeed2 != 0 )  xSeed2  = new BlockMatrix(*rhs.xSeed2) ;
00225     else                   xSeed2  = 0                            ;
00226     if( rhs.xaSeed2 != 0 ) xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
00227     else                   xaSeed2 = 0                            ;
00228     if( rhs.pSeed2 != 0 )  pSeed2  = new BlockMatrix(*rhs.pSeed2 );
00229     else                   pSeed2  = 0                            ;
00230     if( rhs.uSeed2 != 0 )  uSeed2  = new BlockMatrix(*rhs.uSeed2 );
00231     else                   uSeed2  = 0                            ;
00232     if( rhs.wSeed2 != 0 )  wSeed2  = new BlockMatrix(*rhs.wSeed2 );
00233     else                   wSeed2  = 0                            ;
00234 
00235     if( rhs.bSeed != 0 )   bSeed2 = new BlockMatrix(*rhs.bSeed );
00236     else                   bSeed2 = 0                           ;
00237 
00238 
00239     residuumL = rhs.residuumL;
00240     residuumU = rhs.residuumU;
00241     dForward  = rhs.dForward ;
00242     dBackward = rhs.dBackward;
00243 
00244     condType  = rhs.condType;
00245 }
00246 
00247 
00248 ConstraintElement::~ConstraintElement( ){
00249 
00250     int run1;
00251 
00252     if( fcn != 0 )
00253         delete[] fcn;
00254 
00255     for( run1 = 0; run1 < nB; run1++ ){
00256 
00257         if( lb[run1] != 0 )
00258             free(lb[run1]);
00259 
00260         if( ub[run1] != 0 )
00261             free(ub[run1]);
00262     }
00263 
00264     if( lb != 0 )
00265         delete[] lb;
00266 
00267     if( ub != 0 )
00268         delete[] ub;
00269 
00270     for( run1 = 0; run1 < nFcn; run1++ ){
00271 
00272         if( y_index[run1] != 0 )
00273             delete[] y_index[run1];
00274     }
00275 
00276     if( y_index != 0 )
00277         delete[] y_index;
00278 
00279     if( t_index != 0 )
00280         delete[] t_index;
00281 
00282         if ( z != 0 )
00283                 delete[] z;
00284         
00285         if ( JJ != 0 )
00286                 delete[] JJ;
00287 
00288     if( xSeed   != 0 ) delete xSeed  ;
00289     if( xaSeed  != 0 ) delete xaSeed ;
00290     if( pSeed   != 0 ) delete pSeed  ;
00291     if( uSeed   != 0 ) delete uSeed  ;
00292     if( wSeed   != 0 ) delete wSeed  ;
00293 
00294     if( bSeed   != 0 ) delete bSeed  ;
00295 
00296     if( xSeed2  != 0 ) delete xSeed2 ;
00297     if( xaSeed2 != 0 ) delete xaSeed2;
00298     if( pSeed2  != 0 ) delete pSeed2 ;
00299     if( uSeed2  != 0 ) delete uSeed2 ;
00300     if( wSeed2  != 0 ) delete wSeed2 ;
00301 
00302     if( bSeed2  != 0 ) delete bSeed2 ;
00303 }
00304 
00305 
00306 ConstraintElement& ConstraintElement::operator=( const ConstraintElement& rhs ){
00307 
00308     int run1, run2;
00309 
00310     if( this != &rhs ){
00311 
00312         if( fcn != 0 )
00313             delete[] fcn;
00314 
00315         for( run1 = 0; run1 < nB; run1++ ){
00316 
00317             if( lb[run1] != 0 )
00318                 free(lb[run1]);
00319 
00320             if( ub[run1] != 0 )
00321                 free(ub[run1]);
00322         }
00323 
00324         if( lb != 0 )
00325             delete[] lb;
00326 
00327         if( ub != 0 )
00328             delete[] ub;
00329 
00330         for( run1 = 0; run1 < nFcn; run1++ ){
00331 
00332             if( y_index[run1] != 0 )
00333                 delete[] y_index[run1];
00334         }
00335 
00336         if( y_index != 0 )
00337             delete[] y_index;
00338 
00339         if( t_index != 0 )
00340             delete[] t_index;
00341 
00342                 if ( z != 0 )
00343                         delete[] z;
00344                 
00345                 if ( JJ != 0 )
00346                         delete[] JJ;
00347 
00348         if( xSeed   != 0 ) delete xSeed  ;
00349         if( xaSeed  != 0 ) delete xaSeed ;
00350         if( pSeed   != 0 ) delete pSeed  ;
00351         if( uSeed   != 0 ) delete uSeed  ;
00352         if( wSeed   != 0 ) delete wSeed  ;
00353 
00354         if( bSeed   != 0 ) delete bSeed  ;
00355 
00356         if( xSeed2  != 0 ) delete xSeed2 ;
00357         if( xaSeed2 != 0 ) delete xaSeed2;
00358         if( pSeed2  != 0 ) delete pSeed2 ;
00359         if( uSeed2  != 0 ) delete uSeed2 ;
00360         if( wSeed2  != 0 ) delete wSeed2 ;
00361 
00362         if( bSeed2  != 0 ) delete bSeed2 ;
00363 
00364 
00365         grid    = rhs.grid;
00366         nFcn    = rhs.nFcn;
00367         nB      = rhs.nB  ;
00368 
00369         if( rhs.fcn     != 0 )  fcn     = new Function[nFcn];
00370         else                    fcn     = 0                 ;
00371 
00372         if( rhs.lb      != 0 )  lb      = new double* [nB  ];
00373         else                    lb      = 0                 ;
00374 
00375         if( rhs.ub      != 0 )  ub      = new double* [nB  ];
00376         else                    ub      = 0                 ;
00377 
00378         if( rhs.y_index != 0 )  y_index = new int*    [nFcn];
00379         else                    y_index = 0                 ;
00380 
00381         if( rhs.t_index != 0 )  t_index = new int     [nFcn];
00382         else                    t_index = 0                 ;
00383 
00384                 if( rhs.z       != 0 )  z       = new EvaluationPoint( *(rhs.z) );
00385                 else                    z       = 0                 ;
00386 
00387                 if( rhs.JJ      != 0 )  JJ      = new EvaluationPoint( *(rhs.JJ) );
00388                 else                    JJ      = 0                 ;
00389 
00390         nx      = rhs.nx;
00391         na      = rhs.na;
00392         nu      = rhs.nu;
00393         np      = rhs.np;
00394         nw      = rhs.nw;
00395         ny      = rhs.ny;
00396 
00397         for( run1 = 0; run1 < nFcn; run1++ ){
00398 
00399             fcn[run1] = rhs.fcn[run1];
00400 
00401             if( ny > 0 ){
00402                 y_index[run1] = new int[ny];
00403                 for( run2 = 0; run2 < ny; run2++ )
00404                     y_index[run1][run2] = rhs.y_index[run1][run2];
00405             }
00406             else{
00407                 y_index[run1] = 0;
00408             }
00409 
00410             t_index[run1] = rhs.t_index[run1];
00411 
00412         }
00413 
00414         if( fcn == 0 ){
00415             for( run1 = 0; run1 < nB; run1++ ){
00416                 lb[run1] = 0;
00417                 ub[run1] = 0;
00418             }
00419         }
00420         else{
00421             for( run1 = 0; run1 < nB; run1++ ){
00422 
00423                 if( fcn[0].getDim() > 0 ){
00424                     lb[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
00425                     ub[run1] = (double*)calloc(fcn[0].getDim(),sizeof(double));
00426                 }
00427                 else{
00428                     lb[run1] = 0;
00429                     ub[run1] = 0;
00430                 }
00431 
00432                 for( run2 = 0; run2 < fcn[0].getDim(); run2++ ){
00433                     lb[run1][run2] = rhs.lb[run1][run2];
00434                     ub[run1][run2] = rhs.ub[run1][run2];
00435                 }
00436             }
00437         }
00438 
00439         if( rhs.xSeed != 0 )    xSeed   = new BlockMatrix(*rhs.xSeed  );
00440         else                    xSeed   = 0                            ;
00441         if( rhs.xaSeed != 0 )   xaSeed  = new BlockMatrix(*rhs.xaSeed );
00442         else                    xaSeed  = 0                            ;
00443         if( rhs.pSeed != 0 )    pSeed   = new BlockMatrix(*rhs.pSeed  );
00444         else                    pSeed   = 0                            ;
00445         if( rhs.uSeed != 0 )    uSeed   = new BlockMatrix(*rhs.uSeed  );
00446         else                    uSeed   = 0                            ;
00447         if( rhs.wSeed != 0 )    wSeed   = new BlockMatrix(*rhs.wSeed  );
00448         else                    wSeed   = 0                            ;
00449 
00450         if( rhs.bSeed != 0 )    bSeed   = new BlockMatrix(*rhs.bSeed  );
00451         else                    bSeed   = 0                            ;
00452 
00453         if( rhs.xSeed2 != 0 )   xSeed2  = new BlockMatrix(*rhs.xSeed2 );
00454         else                    xSeed2  = 0                            ;
00455         if( rhs.xaSeed2 != 0 )  xaSeed2 = new BlockMatrix(*rhs.xaSeed2);
00456         else                    xaSeed2 = 0                            ;
00457         if( rhs.pSeed2 != 0 )   pSeed2  = new BlockMatrix(*rhs.pSeed2 );
00458         else                    pSeed2  = 0                            ;
00459         if( rhs.uSeed2 != 0 )   uSeed2  = new BlockMatrix(*rhs.uSeed2 );
00460         else                    uSeed2  = 0                            ;
00461         if( rhs.wSeed2 != 0 )   wSeed2  = new BlockMatrix(*rhs.wSeed2 );
00462         else                    wSeed2  = 0                            ;
00463 
00464         if( rhs.bSeed != 0 )    bSeed2  = new BlockMatrix(*rhs.bSeed  );
00465         else                    bSeed2  = 0                            ;
00466 
00467         residuumL = rhs.residuumL;
00468         residuumU = rhs.residuumU;
00469         dForward  = rhs.dForward ;
00470         dBackward = rhs.dBackward;
00471 
00472         condType  = rhs.condType;
00473     }
00474 
00475     return *this;
00476 }
00477 
00478 
00479 returnValue ConstraintElement::init(  const OCPiterate& iter ){
00480 
00481     int run1, run2;
00482         
00483         initializeEvaluationPoints( iter );
00484         
00485 
00486     if( iter.x  != NULL ) nx = iter.x ->getNumValues();
00487     else             nx = 0                 ;
00488 
00489     if( iter.xa != NULL ) na = iter.xa->getNumValues();
00490     else             na = 0                 ;
00491 
00492     if( iter.p  != NULL ) np = iter.p ->getNumValues();
00493     else             np = 0                 ;
00494 
00495     if( iter.u  != NULL ) nu = iter.u ->getNumValues();
00496     else             nu = 0                 ;
00497 
00498     if( iter.w  != NULL ) nw = iter.w ->getNumValues();
00499     else             nw = 0                 ;
00500 
00501     ny = nx+na+nu+np+nw;
00502 
00503     for( run2 = 0; run2 < nFcn; run2++ ){
00504 
00505         if( y_index[run2] != 0 )  delete[] y_index[run2];
00506             y_index[run2]       = new      int[ny]      ;
00507 
00508         for( run1 = 0; run1 < nx; run1++ )
00509             y_index[run2][run1] = fcn[run2].index( VT_DIFFERENTIAL_STATE, run1 );
00510 
00511         for( run1 = 0; run1 < na; run1++ )
00512             y_index[run2][nx+run1] = fcn[run2].index( VT_ALGEBRAIC_STATE, run1 );
00513 
00514         for( run1 = 0; run1 < np; run1++ )
00515             y_index[run2][nx+na+run1] = fcn[run2].index( VT_PARAMETER, run1 );
00516 
00517         for( run1 = 0; run1 < nu; run1++ )
00518             y_index[run2][nx+na+np+run1] = fcn[run2].index( VT_CONTROL, run1 );
00519 
00520         for( run1 = 0; run1 < nw; run1++ )
00521             y_index[run2][nx+na+np+nu+run1] = fcn[run2].index( VT_DISTURBANCE, run1 );
00522 
00523         t_index[run2] = fcn[run2].index( VT_TIME, 0 );
00524     }
00525 
00526     return SUCCESSFUL_RETURN;
00527 }
00528 
00529 
00530 returnValue ConstraintElement::setForwardSeed( BlockMatrix *xSeed_,
00531                                                BlockMatrix *xaSeed_,
00532                                                BlockMatrix *pSeed_,
00533                                                BlockMatrix *uSeed_,
00534                                                BlockMatrix *wSeed_,
00535                                                int          order    ){
00536 
00537     if( order == 1 ){
00538 
00539         if( xSeed_ != 0 ){
00540             if( xSeed != 0 ) delete xSeed;
00541             xSeed = new BlockMatrix(*xSeed_);
00542         }
00543         else{
00544             if( xSeed != 0 ) delete xSeed;
00545             xSeed = 0;
00546         }
00547         if( xaSeed_ != 0 ){
00548             if( xaSeed != 0 ) delete xaSeed;
00549             xaSeed = new BlockMatrix(*xaSeed_);
00550         }
00551         else{
00552             if( xaSeed != 0 ) delete xaSeed;
00553             xaSeed = 0;
00554         }
00555         if( pSeed_ != 0 ){
00556             if( pSeed != 0 ) delete pSeed;
00557             pSeed = new BlockMatrix(*pSeed_);
00558         }
00559         else{
00560             if( pSeed != 0 ) delete pSeed;
00561             pSeed = 0;
00562         }
00563         if( uSeed_ != 0 ){
00564             if( uSeed != 0 ) delete uSeed;
00565             uSeed = new BlockMatrix(*uSeed_);
00566         }
00567         else{
00568             if( uSeed != 0 ) delete uSeed;
00569             uSeed = 0;
00570         }
00571         if( wSeed_ != 0 ){
00572             if( wSeed != 0 ) delete wSeed;
00573             wSeed = new BlockMatrix(*wSeed_);
00574         }
00575         else{
00576             if( wSeed != 0 ) delete wSeed;
00577             wSeed = 0;
00578         }
00579 
00580         return SUCCESSFUL_RETURN;
00581     }
00582     if( order == 2 ){
00583 
00584         if( xSeed_ != 0 ){
00585             if( xSeed2 != 0 ) delete xSeed2;
00586             xSeed2 = new BlockMatrix(*xSeed_);
00587         }
00588         else{
00589             if( xSeed2 != 0 ) delete xSeed2;
00590             xSeed2 = 0;
00591         }
00592         if( xaSeed_ != 0 ){
00593             if( xaSeed2 != 0 ) delete xaSeed2;
00594             xaSeed2 = new BlockMatrix(*xaSeed_);
00595         }
00596         else{
00597             if( xaSeed2 != 0 ) delete xaSeed2;
00598             xaSeed2 = 0;
00599         }
00600         if( pSeed_ != 0 ){
00601             if( pSeed2 != 0 ) delete pSeed2;
00602             pSeed2 = new BlockMatrix(*pSeed_);
00603         }
00604         else{
00605             if( pSeed2 != 0 ) delete pSeed2;
00606             pSeed2 = 0;
00607         }
00608         if( uSeed_ != 0 ){
00609             if( uSeed2 != 0 ) delete uSeed2;
00610             uSeed2 = new BlockMatrix(*uSeed_);
00611         }
00612         else{
00613             if( uSeed2 != 0 ) delete uSeed2;
00614             uSeed2 = 0;
00615         }
00616         if( wSeed_ != 0 ){
00617             if( wSeed2 != 0 ) delete wSeed2;
00618             wSeed2 = new BlockMatrix(*wSeed_);
00619         }
00620         else{
00621             if( wSeed2 != 0 ) delete wSeed2;
00622             wSeed2 = 0;
00623         }
00624         return SUCCESSFUL_RETURN;
00625     }
00626 
00627     return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00628 }
00629 
00630 
00631 returnValue ConstraintElement::setUnitForwardSeed( ){
00632 
00633     BlockMatrix  xSeed_( 1, 1 );
00634     BlockMatrix xaSeed_( 1, 1 );
00635     BlockMatrix  pSeed_( 1, 1 );
00636     BlockMatrix  uSeed_( 1, 1 );
00637     BlockMatrix  wSeed_( 1, 1 );
00638 
00639     xSeed_.setIdentity ( 0, 0, nx );
00640     xaSeed_.setIdentity( 0, 0, na );
00641     pSeed_.setIdentity ( 0, 0, np );
00642     uSeed_.setIdentity ( 0, 0, nu );
00643     wSeed_.setIdentity ( 0, 0, nw );
00644 
00645     return setForwardSeed( &xSeed_, &xaSeed_, &pSeed_, &uSeed_, &wSeed_, 1 );
00646 }
00647 
00648 
00649 returnValue ConstraintElement::setBackwardSeed( BlockMatrix *seed, int order ){
00650 
00651     if( order == 1 ){
00652 
00653         if( seed != 0 ){
00654             if( bSeed != 0 ) delete bSeed;
00655             bSeed = new BlockMatrix(*seed);
00656         }
00657         else{
00658             if( bSeed != 0 ) delete bSeed;
00659             bSeed = 0;
00660         }
00661 
00662         return SUCCESSFUL_RETURN;
00663     }
00664     if( order == 2 ){
00665 
00666         if( seed != 0 ){
00667             if( bSeed2 != 0 ) delete bSeed2;
00668             bSeed2 = new BlockMatrix(*seed);
00669         }
00670         else{
00671             if( bSeed2 != 0 ) delete bSeed2;
00672             bSeed2 = 0;
00673         }
00674         return SUCCESSFUL_RETURN;
00675     }
00676 
00677     return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00678 }
00679 
00680 
00681 returnValue ConstraintElement::getResiduum( BlockMatrix &lower_residuum,
00682                                             BlockMatrix &upper_residuum ){
00683 
00684 
00685     lower_residuum = residuumL;
00686     upper_residuum = residuumU;
00687 
00688     return SUCCESSFUL_RETURN;
00689 }
00690 
00691 
00692 returnValue ConstraintElement::getForwardSensitivities( BlockMatrix *D, int order ){
00693 
00694     ASSERT( D != 0 );
00695 
00696     if( order == 1 ){
00697 
00698         D[0] = dForward;
00699         return SUCCESSFUL_RETURN;
00700     }
00701     if( order == 2 ){
00702 
00703         return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00704     }
00705     return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00706 }
00707 
00708 
00709 returnValue ConstraintElement::getBackwardSensitivities( BlockMatrix *D, int order ){
00710 
00711     ASSERT( D != 0 );
00712 
00713     if( order == 1 ){
00714 
00715         D[0] = dBackward;
00716         return SUCCESSFUL_RETURN;
00717     }
00718     if( order == 2 ){
00719 
00720         return ACADOERROR(RET_NOT_IMPLEMENTED_YET);
00721     }
00722     return ACADOWARNING(RET_INPUT_OUT_OF_RANGE);
00723 }
00724 
00725 
00726 
00727 //
00728 // PROTECTED MEMBER FUNCTIONS:
00729 //
00730 
00731 returnValue ConstraintElement::initializeEvaluationPoints( const OCPiterate& iter )
00732 {
00733         if ( z != 0 )
00734                 delete[] z;
00735         
00736         if ( JJ != 0 )
00737                 delete[] JJ;
00738 
00739         z = new EvaluationPoint[nFcn];
00740         JJ = new EvaluationPoint[nFcn];
00741         //HH = new EvaluationPoint[nFcn];
00742 
00743         for( int i=0; i<nFcn; ++i )
00744         {
00745                 z[i].init(  fcn[i], iter );
00746                 JJ[i].init(  fcn[i], iter );
00747                 //HH[i].init(  fcn[i], iter );
00748         }
00749 
00750         return SUCCESSFUL_RETURN;
00751 }
00752 
00753 returnValue ConstraintElement::get(Function& function_, DMatrix& lb_, DMatrix& ub_)
00754 {
00755         if ( fcn == NULL )
00756                 return RET_INITIALIZE_FIRST;
00757 
00758         // This is not exactly bullet proof, but for now will serve the purpose
00759         function_ = fcn[ 0 ];
00760 
00761         int dimFcn = fcn[ 0 ].getDim();
00762 
00763         if ( dimFcn == 0 )
00764         {
00765                 lb_.init(0, 0);
00766                 ub_.init(0, 0);
00767 
00768                 return SUCCESSFUL_RETURN;
00769         }
00770 
00771         lb_.init(nB, dimFcn);
00772         ub_.init(nB, dimFcn);
00773 
00774         int i, j;
00775 
00776         for (i = 0; i < nB; ++i)
00777                 for (j = 0; j < dimFcn; ++j)
00778                 {
00779                         lb_(i, j) = lb[ i ][ j ];
00780                         ub_(i, j) = ub[ i ][ j ];
00781                 }
00782 
00783         return SUCCESSFUL_RETURN;
00784 }
00785 
00786 
00787 CLOSE_NAMESPACE_ACADO
00788 
00789 // end of file.


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