objective.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/symbolic_expression/symbolic_expression.hpp>
00036 #include <acado/objective/objective.hpp>
00037 #include <acado/function/ocp_iterate.hpp>
00038 
00039 #include <algorithm>
00040 
00041 BEGIN_NAMESPACE_ACADO
00042 
00043 
00044 //
00045 // PUBLIC MEMBER FUNCTIONS:
00046 //
00047 
00048 
00049 Objective::Objective( )
00050           :LagrangeTerm(){
00051 
00052     lsqTerm    = 0;
00053     lsqEndTerm = 0;
00054     mayerTerm  = 0;
00055 
00056     nLSQ       = 0;
00057     nEndLSQ    = 0;
00058     nMayer     = 0;
00059 }
00060 
00061 
00062 Objective::Objective( const Grid &grid_ ):LagrangeTerm(){
00063 
00064     lsqTerm    = 0;
00065     lsqEndTerm = 0;
00066     mayerTerm  = 0;
00067 
00068     nLSQ       = 0;
00069     nEndLSQ    = 0;
00070     nMayer     = 0;
00071 
00072     init( grid_ );
00073 }
00074 
00075 
00076 
00077 returnValue Objective::init( const Grid &grid_ ){
00078 
00079     return LagrangeTerm::init( grid_ );
00080 }
00081 
00082 
00083 
00084 Objective::Objective( const Objective& rhs )
00085           :LagrangeTerm(rhs){
00086 
00087     uint run1;
00088 
00089     nLSQ       = rhs.nLSQ   ;
00090     nEndLSQ    = rhs.nEndLSQ;
00091     nMayer     = rhs.nMayer ;
00092 
00093     if( rhs.lsqTerm != 0 ){
00094         lsqTerm = (LSQTerm**)calloc(nLSQ,sizeof(LSQTerm*));
00095         for( run1 = 0; run1 < nLSQ; run1++ )
00096             lsqTerm[run1] = new LSQTerm(*rhs.lsqTerm[run1]);
00097     }
00098     else lsqTerm = 0;
00099 
00100     if( rhs.lsqEndTerm != 0 ){
00101         lsqEndTerm = (LSQEndTerm**)calloc(nEndLSQ,sizeof(LSQEndTerm*));
00102         for( run1 = 0; run1 < nEndLSQ; run1++ )
00103             lsqEndTerm[run1] = new LSQEndTerm(*rhs.lsqEndTerm[run1]);
00104     }
00105     else lsqEndTerm = 0;
00106 
00107     if( rhs.mayerTerm != 0 ){
00108         mayerTerm = (MayerTerm**)calloc(nMayer,sizeof(MayerTerm*));
00109         for( run1 = 0; run1 < nMayer; run1++ )
00110             mayerTerm[run1] = new MayerTerm(*rhs.mayerTerm[run1]);
00111     }
00112     else mayerTerm = 0;
00113 
00114     cgLsqElements = rhs.cgLsqElements;
00115     cgLsqEndTermElements = rhs.cgLsqEndTermElements;
00116 
00117     cgExternLsqElements = rhs.cgExternLsqElements;
00118     cgExternLsqEndTermElements = rhs.cgExternLsqEndTermElements;
00119 
00120     cgLsqLinearElements = rhs.cgLsqLinearElements;
00121 }
00122 
00123 
00124 Objective::~Objective( ){
00125 
00126     uint run1;
00127 
00128     if( lsqTerm != 0 ){
00129         for( run1 = 0; run1 < nLSQ; run1++ )
00130             delete lsqTerm[run1];
00131         free(lsqTerm);
00132     }
00133 
00134     if( lsqEndTerm != 0 ){
00135         for( run1 = 0; run1 < nEndLSQ; run1++ )
00136             delete lsqEndTerm[run1];
00137         free(lsqEndTerm);
00138     }
00139 
00140     if( mayerTerm != 0 ){
00141         for( run1 = 0; run1 < nMayer; run1++ )
00142             delete mayerTerm[run1];
00143         free(mayerTerm);
00144     }
00145 
00146     cgExternLsqElements.clear();
00147     cgLsqEndTermElements.clear();
00148 
00149     cgExternLsqElements.clear();
00150     cgExternLsqEndTermElements.clear();
00151 
00152     cgLsqLinearElements.clear();
00153 }
00154 
00155 returnValue Objective::addLSQ( const MatrixVariablesGrid *S_,
00156                                       const Function&            h ,
00157                                       const VariablesGrid       *r_  )
00158 {
00159     nLSQ++;
00160     lsqTerm = (LSQTerm**)realloc(lsqTerm,nLSQ*sizeof(LSQTerm*));
00161     lsqTerm[nLSQ-1] = new LSQTerm(S_, h, r_);
00162     lsqTerm[nLSQ-1]->setGrid(grid);
00163 
00164     DMatrix temp = S_->getMatrix( 0 );
00165     if (temp.isPositiveSemiDefinite() == BT_FALSE)
00166         return ACADOERROR( RET_NONPOSITIVE_WEIGHT );
00167 
00168     cgLsqElements.push_back(LsqData(temp, h));
00169 
00170     return SUCCESSFUL_RETURN;
00171 }
00172 
00173 
00174 
00175 returnValue Objective::addLSQEndTerm( const DMatrix   & S,
00176                                              const Function & m,
00177                                              const DVector   & r  )
00178 {
00179 
00180     nEndLSQ++;
00181     lsqEndTerm = (LSQEndTerm**)realloc(lsqEndTerm,nEndLSQ*sizeof(LSQEndTerm*));
00182     lsqEndTerm[nEndLSQ-1] = new LSQEndTerm(grid, S, m, r);
00183     lsqEndTerm[nEndLSQ-1]->setGrid(grid);
00184 
00185     cgLsqEndTermElements.push_back(LsqData(S, m));
00186 
00187     return SUCCESSFUL_RETURN;
00188 }
00189 
00190 Objective& Objective::operator=( const Objective& rhs ){
00191 
00192     uint run1;
00193 
00194     if( this != &rhs ){
00195 
00196         if( lsqTerm != 0 ){
00197             for( run1 = 0; run1 < nLSQ; run1++ )
00198                 delete lsqTerm[run1];
00199             free(lsqTerm);
00200         }
00201 
00202         if( lsqEndTerm != 0 ){
00203             for( run1 = 0; run1 < nEndLSQ; run1++ )
00204                 delete lsqEndTerm[run1];
00205             free(lsqEndTerm);
00206         }
00207 
00208         if( mayerTerm != 0 ){
00209             for( run1 = 0; run1 < nMayer; run1++ )
00210                 delete mayerTerm[run1];
00211             free(mayerTerm);
00212         }
00213 
00214         LagrangeTerm::operator=(rhs);
00215 
00216         nLSQ       = rhs.nLSQ   ;
00217         nEndLSQ    = rhs.nEndLSQ;
00218         nMayer     = rhs.nMayer ;
00219 
00220         if( rhs.lsqTerm != 0 ){
00221             lsqTerm = (LSQTerm**)calloc(nLSQ,sizeof(LSQTerm*));
00222             for( run1 = 0; run1 < nLSQ; run1++ )
00223                 lsqTerm[run1] = new LSQTerm(*rhs.lsqTerm[run1]);
00224         }
00225         else lsqTerm = 0;
00226 
00227         if( rhs.lsqEndTerm != 0 ){
00228            lsqEndTerm = (LSQEndTerm**)calloc(nEndLSQ,sizeof(LSQEndTerm*));
00229             for( run1 = 0; run1 < nEndLSQ; run1++ )
00230                 lsqEndTerm[run1] = new LSQEndTerm(*rhs.lsqEndTerm[run1]);
00231         }
00232         else lsqEndTerm = 0;
00233 
00234         if( rhs.mayerTerm != 0 ){
00235             mayerTerm = (MayerTerm**)calloc(nMayer,sizeof(MayerTerm*));
00236             for( run1 = 0; run1 < nMayer; run1++ )
00237                 mayerTerm[run1] = new MayerTerm(*rhs.mayerTerm[run1]);
00238         }
00239         else mayerTerm = 0;
00240 
00241         cgLsqElements = rhs.cgLsqElements;
00242         cgLsqEndTermElements = rhs.cgLsqEndTermElements;
00243 
00244         cgExternLsqElements = rhs.cgExternLsqElements;
00245         cgExternLsqEndTermElements = rhs.cgExternLsqEndTermElements;
00246 
00247         cgLsqLinearElements = rhs.cgLsqLinearElements;
00248     }
00249     return *this;
00250 }
00251 
00252 
00253 
00254 returnValue Objective::evaluate( const OCPiterate &x ){
00255 
00256     uint run1;
00257 
00258     for( run1 = 0; run1 < nLSQ; run1++ )
00259         ACADO_TRY( lsqTerm[run1]->evaluate( x ) );
00260 
00261     for( run1 = 0; run1 < nEndLSQ; run1++ )
00262         ACADO_TRY( lsqEndTerm[run1]->evaluate( x ) );
00263 
00264     for( run1 = 0; run1 < nMayer; run1++ )
00265         ACADO_TRY( mayerTerm[run1]->evaluate( x ) );
00266 
00267     return SUCCESSFUL_RETURN;
00268 }
00269 
00270 
00271 returnValue Objective::evaluateSensitivities(){
00272 
00273     returnValue returnvalue;
00274     uint run1;
00275 
00276     for( run1 = 0; run1 < nLSQ; run1++ ){
00277         returnvalue = lsqTerm[run1]->evaluateSensitivities( 0 );
00278         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00279     }
00280 
00281     for( run1 = 0; run1 < nEndLSQ; run1++ ){
00282         returnvalue = lsqEndTerm[run1]->evaluateSensitivities( 0 );
00283         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00284     }
00285 
00286     for( run1 = 0; run1 < nMayer; run1++ ){
00287         returnvalue = mayerTerm[run1]->evaluateSensitivities( 0 );
00288         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00289     }
00290 
00291     return SUCCESSFUL_RETURN;
00292 }
00293 
00294 
00295 returnValue Objective::evaluateSensitivities( BlockMatrix &hessian ){
00296 
00297     returnValue returnvalue;
00298     uint run1;
00299 
00300     for( run1 = 0; run1 < nLSQ; run1++ ){
00301         returnvalue = lsqTerm[run1]->evaluateSensitivities( &hessian );
00302         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00303     }
00304 
00305     for( run1 = 0; run1 < nEndLSQ; run1++ ){
00306         returnvalue = lsqEndTerm[run1]->evaluateSensitivities( &hessian );
00307         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00308     }
00309 
00310     for( run1 = 0; run1 < nMayer; run1++ ){
00311         returnvalue = mayerTerm[run1]->evaluateSensitivities( &hessian );
00312         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00313     }
00314 
00315     return SUCCESSFUL_RETURN;
00316 }
00317 
00318 
00319 returnValue Objective::evaluateSensitivitiesGN( BlockMatrix &hessian ){
00320 
00321     returnValue returnvalue;
00322     uint run1;
00323 
00324         hessian.setZero();
00325         if( nMayer != 0 )
00326                 return ACADOERROR(RET_GAUSS_NEWTON_APPROXIMATION_NOT_SUPPORTED);
00327 
00328         for( run1 = 0; run1 < nLSQ; run1++ ){
00329         returnvalue = lsqTerm[run1]->evaluateSensitivitiesGN( &hessian );
00330         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00331     }
00332     for( run1 = 0; run1 < nEndLSQ; run1++ ){
00333         returnvalue = lsqEndTerm[run1]->evaluateSensitivitiesGN( &hessian );
00334         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00335     }
00336 
00337     return SUCCESSFUL_RETURN;
00338 }
00339 
00340 
00341 returnValue Objective::setForwardSeed(  BlockMatrix *xSeed_ ,
00342                                         BlockMatrix *xaSeed_,
00343                                         BlockMatrix *pSeed_ ,
00344                                         BlockMatrix *uSeed_ ,
00345                                         BlockMatrix *wSeed_ ,
00346                                         int          order    ){
00347 
00348     returnValue returnvalue;
00349     uint run1;
00350 
00351     for( run1 = 0; run1 < nLSQ; run1++ ){
00352         returnvalue = lsqTerm[run1]->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
00353         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00354     }
00355 
00356     for( run1 = 0; run1 < nEndLSQ; run1++ ){
00357         returnvalue = lsqEndTerm[run1]->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
00358         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00359     }
00360 
00361     for( run1 = 0; run1 < nMayer; run1++ ){
00362         returnvalue = mayerTerm[run1]->setForwardSeed( xSeed_, xaSeed_, pSeed_, uSeed_, wSeed_, order );
00363         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00364     }
00365 
00366     return SUCCESSFUL_RETURN;
00367 }
00368 
00369 
00370 returnValue Objective::setBackwardSeed( BlockMatrix *seed,  int order ){
00371 
00372     returnValue returnvalue;
00373     uint run1;
00374 
00375     for( run1 = 0; run1 < nLSQ; run1++ ){
00376         returnvalue = lsqTerm[run1]->setBackwardSeed( seed, 1 );
00377         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00378     }
00379 
00380     for( run1 = 0; run1 < nEndLSQ; run1++ ){
00381         returnvalue = lsqEndTerm[run1]->setBackwardSeed( seed, 1 );
00382         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00383     }
00384 
00385     for( run1 = 0; run1 < nMayer; run1++ ){
00386         returnvalue = mayerTerm[run1]->setBackwardSeed( seed, 1 );
00387         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00388     }
00389 
00390     return SUCCESSFUL_RETURN;
00391 }
00392 
00393 
00394 returnValue Objective::setUnitBackwardSeed( ){
00395 
00396     BlockMatrix seed(1,1);
00397     seed.setIdentity(0,0,1);
00398     return setBackwardSeed(&seed,1);
00399 }
00400 
00401 
00402 returnValue Objective::getObjectiveValue( double &objectiveValue ){
00403 
00404     double obj;
00405 
00406     returnValue returnvalue;
00407     uint run1;
00408 
00409     objectiveValue = 0.0;
00410 
00411     for( run1 = 0; run1 < nLSQ; run1++ ){
00412         returnvalue = lsqTerm[run1]->getObjectiveValue( obj );
00413         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00414         objectiveValue += obj;
00415     }
00416 
00417     for( run1 = 0; run1 < nEndLSQ; run1++ ){
00418         returnvalue = lsqEndTerm[run1]->getObjectiveValue( obj );
00419         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00420         objectiveValue += obj;
00421     }
00422 
00423     for( run1 = 0; run1 < nMayer; run1++ ){
00424         returnvalue = mayerTerm[run1]->getObjectiveValue( obj );
00425         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00426         objectiveValue += obj;
00427     }
00428 
00429     return SUCCESSFUL_RETURN;
00430 }
00431 
00432 
00433 returnValue Objective::getForwardSensitivities( BlockMatrix &D, int order ){
00434 
00435     BlockMatrix DD, *sum;
00436 
00437     returnValue returnvalue;
00438     uint run1;
00439 
00440     sum = 0;
00441 
00442     for( run1 = 0; run1 < nLSQ; run1++ ){
00443         returnvalue = lsqTerm[run1]->getForwardSensitivities( &DD, order );
00444         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00445         if( sum != 0 ) sum[0] += DD;
00446         else           sum     = new BlockMatrix(DD);
00447     }
00448 
00449     for( run1 = 0; run1 < nEndLSQ; run1++ ){
00450         returnvalue = lsqEndTerm[run1]->getForwardSensitivities( &DD, order );
00451         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00452         if( sum != 0 ) sum[0] += DD;
00453         else           sum     = new BlockMatrix(DD);
00454     }
00455 
00456     for( run1 = 0; run1 < nMayer; run1++ ){
00457         returnvalue = mayerTerm[run1]->getForwardSensitivities( &DD, order );
00458         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00459         if( sum != 0 ) sum[0] += DD;
00460         else           sum     = new BlockMatrix(DD);
00461     }
00462 
00463     if( sum != 0 ){
00464         D = sum[0];
00465         delete sum;
00466     }
00467 
00468     return SUCCESSFUL_RETURN;
00469 }
00470 
00471 
00472 returnValue Objective::getBackwardSensitivities( BlockMatrix &D, int order ){
00473 
00474     BlockMatrix DD, *sum;
00475 
00476     returnValue returnvalue;
00477     uint run1;
00478 
00479     sum = 0;
00480 
00481     for( run1 = 0; run1 < nLSQ; run1++ ){
00482         returnvalue = lsqTerm[run1]->getBackwardSensitivities( &DD, order );
00483         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00484         if( sum != 0 ) sum[0] += DD;
00485         else           sum     = new BlockMatrix(DD);
00486     }
00487 
00488     for( run1 = 0; run1 < nEndLSQ; run1++ ){
00489         returnvalue = lsqEndTerm[run1]->getBackwardSensitivities( &DD, order );
00490         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00491         if( sum != 0 ) sum[0] += DD;
00492         else           sum     = new BlockMatrix(DD);
00493     }
00494 
00495     for( run1 = 0; run1 < nMayer; run1++ ){
00496         returnvalue = mayerTerm[run1]->getBackwardSensitivities( &DD, order );
00497         if( returnvalue != SUCCESSFUL_RETURN )  return returnvalue;
00498         if( sum != 0 ) sum[0] += DD;
00499         else           sum     = new BlockMatrix(DD);
00500     }
00501 
00502     if( sum != 0 ){
00503         D = sum[0];
00504         delete sum;
00505     }
00506 
00507     return SUCCESSFUL_RETURN;
00508 }
00509 
00510 
00511 returnValue Objective::init( const int              nStages     ,
00512                              const int              nTransitions,
00513                              DifferentialEquation **fcn         ,
00514                              Transition            *transitions ,
00515                              Constraint            *constraint_   ){
00516 
00517     if( lagrangeFcn != 0 ){
00518 
00519         int run1;
00520 
00521         if( fcn == 0 ){
00522             fcn = new DifferentialEquation*[nStages];
00523             for( run1 = 0; run1 < nStages; run1++ )
00524                 fcn[run1] = new DifferentialEquation();
00525         }
00526 
00527         DifferentialState xAugment;
00528 
00529         for( run1 = 0; run1 < nTransitions; run1++ )
00530             transitions[run1] << xAugment == xAugment;
00531 
00532         for( run1 = 0; run1 < nStages; run1++ ){
00533 
00534             if( fcn[run1]->isDiscretized() == BT_FALSE ){
00535                 if( nLagrangeTerms == 1 ){
00536                     fcn[run1][0] << dot( xAugment ) == *lagrangeFcn[0];
00537                 }
00538                 else{
00539                     if( lagrangeFcn[run1] == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00540                     fcn[run1][0] << dot( xAugment ) == (*lagrangeFcn[run1]);
00541                 }
00542             }
00543             else{
00544                 if( nLagrangeTerms == 1 ){
00545 
00546                     Expression tmp =  xAugment + fcn[run1]->getStepLength()*(*lagrangeFcn[0]);
00547 
00548                     fcn[run1][0] << next( xAugment ) == tmp;
00549                 }
00550                 else{
00551                     if( lagrangeFcn[run1] == 0 ) return ACADOERROR(RET_MEMBER_NOT_INITIALISED);
00552 
00553                     Expression tmp = xAugment + fcn[run1]->getStepLength()*(*lagrangeFcn[run1]);
00554 
00555                     fcn[run1][0] << next( xAugment ) == tmp;
00556                 }
00557             }
00558         }
00559 
00560                 constraint_[0].add(0, 0.0, xAugment, 0.0);
00561 
00562         nMayer++;
00563         mayerTerm = (MayerTerm**)realloc(mayerTerm,nMayer*sizeof(MayerTerm*));
00564         mayerTerm[nMayer-1] = new MayerTerm(grid, xAugment );
00565     }
00566 
00567     return SUCCESSFUL_RETURN;
00568 }
00569 
00570 
00571 BooleanType Objective::isEmpty() const{
00572 
00573     if( nLSQ == 0 && nEndLSQ == 0 && nMayer == 0 && lagrangeFcn == 0 )
00574         return BT_TRUE;
00575 
00576     return BT_FALSE;
00577 }
00578 
00579 returnValue Objective::getLSQTerms( LsqElements& _elements ) const
00580 {
00581         _elements = cgLsqElements;
00582 
00583         return SUCCESSFUL_RETURN;
00584 }
00585 
00586 returnValue Objective::getLSQEndTerms( LsqElements& _elements ) const
00587 {
00588         _elements = cgLsqEndTermElements;
00589 
00590         return SUCCESSFUL_RETURN;
00591 }
00592 
00593 returnValue Objective::getLSQTerms( LsqExternElements& _elements ) const
00594 {
00595         _elements = cgExternLsqElements;
00596 
00597         return SUCCESSFUL_RETURN;
00598 }
00599 
00600 returnValue Objective::getLSQEndTerms( LsqExternElements& _elements ) const
00601 {
00602         _elements = cgExternLsqEndTermElements;
00603 
00604         return SUCCESSFUL_RETURN;
00605 }
00606 
00607 returnValue Objective::getLSQLinearTerms(LsqLinearElements& _elements) const
00608 {
00609         _elements = cgLsqLinearElements;
00610 
00611         return SUCCESSFUL_RETURN;
00612 }
00613 
00614 uint Objective::getNumMayerTerms( ) const
00615 {
00616 
00617         return nMayer;
00618 }
00619 
00620 returnValue Objective::getMayerTerm(uint index, Function& _mayerTerm) const
00621 {
00622         mayerTerm[index]->getFunction(_mayerTerm);
00623 
00624         return SUCCESSFUL_RETURN;
00625 }
00626 
00627 uint Objective::getNumLagrangeTerms( ) const
00628 {
00629 
00630         return nLagrangeTerms;
00631 }
00632 
00633 returnValue Objective::getLagrangeTerm(uint index, Function& _lagrangeTerm) const
00634 {
00635         _lagrangeTerm << *lagrangeFcn[index];
00636 
00637         return SUCCESSFUL_RETURN;
00638 }
00639 
00640 //
00641 // Code generation related functions
00642 //
00643 
00644 returnValue Objective::addLSQ(const DMatrix& S, const Function& h)
00645 {
00646         cgLsqElements.push_back(LsqData(S, h));
00647 
00648         return SUCCESSFUL_RETURN;
00649 }
00650 
00651 returnValue Objective::addLSQEndTerm(const DMatrix& S, const Function& h)
00652 {
00653         cgLsqEndTermElements.push_back(LsqData(S, h));
00654 
00655         return SUCCESSFUL_RETURN;
00656 }
00657 
00658 returnValue Objective::addLSQ(const DMatrix& S, const std::string& h)
00659 {
00660         cgExternLsqElements.push_back(LsqExternData(S, h));
00661 
00662         return SUCCESSFUL_RETURN;
00663 }
00664 
00665 returnValue Objective::addLSQEndTerm(const DMatrix& S, const std::string& h)
00666 {
00667         cgExternLsqEndTermElements.push_back(LsqExternData(S, h));
00668 
00669         return SUCCESSFUL_RETURN;
00670 }
00671 
00672 returnValue Objective::addLSQ(const BMatrix& S, const Function& h)
00673 {
00674         cgLsqElements.push_back(LsqData(S.cast<double>(), h, false));
00675 
00676         return SUCCESSFUL_RETURN;
00677 }
00678 
00679 returnValue Objective::addLSQEndTerm(const BMatrix& S, const Function& h)
00680 {
00681         cgLsqEndTermElements.push_back(LsqData(S.cast<double>(), h, false));
00682 
00683         return SUCCESSFUL_RETURN;
00684 }
00685 
00686 returnValue Objective::addLSQ(const BMatrix& S, const std::string& h)
00687 {
00688         cgExternLsqElements.push_back(LsqExternData(S.cast<double>(), h, false));
00689 
00690         return SUCCESSFUL_RETURN;
00691 }
00692 
00693 returnValue Objective::addLSQEndTerm(const BMatrix& S, const std::string& h)
00694 {
00695         cgExternLsqEndTermElements.push_back(LsqExternData(S.cast<double>(), h, false));
00696 
00697         return SUCCESSFUL_RETURN;
00698 }
00699 
00700 returnValue Objective::addLSQLinearTerms(const DVector& Slx, const DVector& Slu)
00701 {
00702         cgLsqLinearElements.push_back( LsqLinearData(Slx, Slu, true) );
00703 
00704         return SUCCESSFUL_RETURN;
00705 }
00706 
00707 returnValue Objective::addLSQLinearTerms(const BVector& Slx, const BVector& Slu)
00708 {
00709         cgLsqLinearElements.push_back( LsqLinearData(Slx.cast<double>(), Slu.cast<double>(), false) );
00710 
00711         return SUCCESSFUL_RETURN;
00712 }
00713 
00714 CLOSE_NAMESPACE_ACADO
00715 
00716 // end of file.


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