dense_cp.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 
00033 #include <acado/conic_program/dense_cp.hpp>
00034 
00035 using namespace std;
00036 
00037 BEGIN_NAMESPACE_ACADO
00038 
00039 
00040 //
00041 // PUBLIC MEMBER FUNCTIONS:
00042 //
00043 
00044 DenseCP::DenseCP( ){
00045 
00046     nS = 0;
00047 
00048     B   = 0;
00049     lbB = 0;
00050     ubB = 0;
00051 
00052     x   = 0;
00053     ylb = 0;
00054     yub = 0;
00055 
00056     ylbA = 0;
00057     yubA = 0;
00058 
00059     ylbB = 0;
00060     yubB = 0;
00061 }
00062 
00063 
00064 DenseCP::DenseCP( const DenseCP& rhs ){
00065 
00066     copy(rhs);
00067 }
00068 
00069 
00070 DenseCP::~DenseCP( ){
00071 
00072     clean();
00073 }
00074 
00075 
00076 DenseCP& DenseCP::operator=( const DenseCP& rhs ){
00077 
00078     if ( this != &rhs ){
00079 
00080         clean()  ;
00081         copy(rhs);
00082     }
00083     return *this;
00084 }
00085 
00086 
00087 
00088 void DenseCP::copy( const DenseCP& rhs ){
00089 
00090     uint run1, run2;
00091 
00092     nS = rhs.nS;
00093 
00094     H   = rhs.H;
00095     g   = rhs.g;
00096 
00097     lb  = rhs.lb;
00098     ub  = rhs.ub;
00099 
00100     A   = rhs.A;
00101     lbA = rhs.lbA;
00102     ubA = rhs.ubA;
00103 
00104 
00105     if( rhs.B != 0 ){
00106         B = (DMatrix**)calloc(nS,sizeof(DMatrix*));
00107         for( run1 = 0; run1 < nS; run1++ ){
00108             B[run1] = new DMatrix[ getNV() ];
00109             for( run2 = 0; run2 < getNV(); run2++ )
00110                 B[run1][run2] = rhs.B[run1][run2];
00111         }
00112     }
00113     else B = 0;
00114 
00115 
00116     if( rhs.lbB != 0 ){
00117         lbB = (DVector*)calloc(nS,sizeof(DVector));
00118         for( run1 = 0; run1 < nS; run1++ )
00119             lbB[run1] = rhs.lbB[run1];
00120     }
00121     else lbB = 0;
00122 
00123     if( rhs.ubB != 0 ){
00124         ubB = (DVector*)calloc(nS,sizeof(DVector));
00125         for( run1 = 0; run1 < nS; run1++ )
00126             ubB[run1] = rhs.ubB[run1];
00127     }
00128     else ubB = 0;
00129 
00130 
00131     if( rhs.x != 0 ) x = new DVector(*rhs.x);
00132     else             x = 0                 ;
00133 
00134     if( rhs.ylb != 0 ) ylb = new DVector(*rhs.ylb);
00135     else               ylb = 0                   ;
00136 
00137     if( rhs.yub != 0 ) yub = new DVector(*rhs.yub);
00138     else               yub = 0                   ;
00139 
00140     if( rhs.ylbA != 0 ) ylbA = new DVector(*rhs.ylbA);
00141     else                ylbA = 0                    ;
00142 
00143     if( rhs.yubA != 0 ) yubA = new DVector(*rhs.yubA);
00144     else                yubA = 0                    ;
00145 
00146     if( nS > 0 ){
00147 
00148         ylbB = (DVector**)calloc(nS,sizeof(DVector*));
00149         yubB = (DVector**)calloc(nS,sizeof(DVector*));
00150 
00151         for( run1 = 0; run1 < nS; run1++ ){
00152             if( rhs.ylbB[run1] != 0 ) ylbB[run1] = new DVector(*rhs.ylbB[run1]);
00153             else                      ylbB[run1] = 0                          ;
00154 
00155             if( rhs.yubB[run1] != 0 ) yubB[run1] = new DVector(*rhs.yubB[run1]);
00156             else                      yubB[run1] = 0                          ;
00157         }
00158     }
00159     else{
00160         ylbB = 0;
00161         yubB = 0;
00162     }
00163 }
00164 
00165 
00166 
00167 void DenseCP::clean(){
00168 
00169     uint run1;
00170 
00171     for( run1 = 0; run1 < nS; run1++ ){
00172 
00173         if( B[run1]    != 0 ) delete[]    B[run1];
00174         if( ylbB[run1] != 0 ) delete   ylbB[run1];
00175         if( yubB[run1] != 0 ) delete   yubB[run1];
00176     }
00177 
00178     if( B    != 0 ) free(B)   ;
00179     if( ylbB != 0 ) free(ylbB);
00180     if( yubB != 0 ) free(yubB);
00181 
00182     if( lbB != 0 ) free(lbB);
00183     if( ubB != 0 ) free(ubB);
00184 
00185     if( ylbA != 0 ) delete ylbA;
00186     if( yubA != 0 ) delete yubA;
00187 
00188     if( ylb != 0 ) delete ylb;
00189     if( yub != 0 ) delete yub;
00190 
00191     if( x != 0 ) delete x;
00192 }
00193 
00194 
00195 returnValue DenseCP::init( uint nV_, uint nC_ ){
00196 
00197     H.init(nV_,nV_);
00198     A.init(nC_,nV_);
00199 
00200     return SUCCESSFUL_RETURN;
00201 }
00202 
00203 
00204 returnValue DenseCP::setQPsolution( const DVector &x_, const DVector &y_ ){
00205 
00206     uint run1;
00207     clean();
00208 
00209     ASSERT( x_.getDim() == getNV()           );
00210     ASSERT( y_.getDim() == getNV() + getNC() );
00211 
00212 
00213     // SET THE PRIMAL SOLUTION:
00214     // ------------------------
00215     x = new DVector(x_);
00216 
00217 
00218     // SET THE DUAL SOLUTION FOR THE BOUNDS:
00219     // -------------------------------------
00220     ylb = new DVector( getNV() );
00221     yub = new DVector( getNV() );
00222 
00223     for( run1 = 0; run1 < getNV(); run1++ ){
00224         if( fabs(x_(run1)-lb(run1)) <= BOUNDTOL ){
00225             ylb->operator()(run1) = y_(run1);
00226             yub->operator()(run1) = 0.0     ;
00227         }
00228         else{
00229             ylb->operator()(run1) = 0.0     ;
00230             yub->operator()(run1) = y_(run1);
00231         }
00232     }
00233 
00234 
00235     // SET THE DUAL SOLUTION FOR THE CONSTRAINTS:
00236     // ------------------------------------------
00237     DVector tmp = A*x_;
00238     ylbA = new DVector( getNC() );
00239     yubA = new DVector( getNC() );
00240 
00241     for( run1 = 0; run1 < getNC(); run1++ ){
00242         if( fabs(tmp(run1)-lbA(run1)) <= BOUNDTOL ){
00243             ylbA->operator()(run1) = y_(getNV()+run1);
00244             yubA->operator()(run1) = 0.0             ;
00245         }
00246         else{
00247             ylbA->operator()(run1) = 0.0             ;
00248             yubA->operator()(run1) = y_(getNV()+run1);
00249         }
00250     }
00251 
00252 
00253     return SUCCESSFUL_RETURN;
00254 }
00255 
00256 
00257 DVector DenseCP::getMergedDualSolution( ) const
00258 {
00259         DVector dualSolution( getNV()+getNC() );
00260 
00261         uint i;
00262         for( i=0; i<getNV(); ++i )
00263         {
00264                 if ( acadoIsGreater( fabs( (*ylb)(i) ),1e-10 ) == BT_TRUE )
00265                         dualSolution(i) = (*ylb)(i);
00266                 else
00267                         dualSolution(i) = (*yub)(i);
00268         }
00269 
00270         for( i=0; i<getNC(); ++i )
00271         {
00272                 if ( acadoIsGreater( fabs( (*ylbA)(i) ),1e-10 ) == BT_TRUE )
00273                         dualSolution( getNV()+i ) = (*ylbA)(i);
00274                 else
00275                         dualSolution( getNV()+i ) = (*yubA)(i);
00276         }
00277 
00278         return dualSolution;
00279 }
00280 
00281 
00282 
00283 returnValue DenseCP::print(     const char* const name,
00284                                                         const char* const startString,
00285                                                         const char* const endString,
00286                                                         uint width,
00287                                                         uint precision,
00288                                                         const char* const colSeparator,
00289                                                         const char* const rowSeparator
00290                                                         ) const
00291 {
00292         H.print  (cout, "H",  startString,endString,width,precision,colSeparator,rowSeparator );
00293         g.print  (cout, "g",  startString,endString,width,precision,colSeparator,rowSeparator );
00294         lb.print (cout, "lb", startString,endString,width,precision,colSeparator,rowSeparator );
00295         ub.print (cout, "ub", startString,endString,width,precision,colSeparator,rowSeparator );
00296         A.print  (cout, "A",  startString,endString,width,precision,colSeparator,rowSeparator );
00297         lbA.print(cout, "lbA",startString,endString,width,precision,colSeparator,rowSeparator );
00298         ubA.print(cout, "ubA",startString,endString,width,precision,colSeparator,rowSeparator );
00299         
00300         return SUCCESSFUL_RETURN;
00301 }
00302 
00303 
00304 returnValue DenseCP::print(     const char* const name,
00305                                                         PrintScheme printScheme
00306                                                         ) const
00307 {
00308         H.print  (cout, "H",  printScheme );
00309         g.print  (cout, "g",  printScheme );
00310         lb.print (cout, "lb", printScheme );
00311         ub.print (cout, "ub", printScheme );
00312         A.print  (cout, "A",  printScheme );
00313         lbA.print(cout, "lbA",printScheme );
00314         ubA.print(cout, "ubA",printScheme );
00315         
00316         return SUCCESSFUL_RETURN;
00317 }
00318 
00319 
00320 returnValue DenseCP::printToFile(       const char* const filename,
00321                                                                         const char* const name,
00322                                                                         const char* const startString,
00323                                                                         const char* const endString,
00324                                                                         uint width,
00325                                                                         uint precision,
00326                                                                         const char* const colSeparator,
00327                                                                         const char* const rowSeparator
00328                                                                         ) const
00329 {
00330         H.print  ( filename,"H",  startString,endString,width,precision,colSeparator,rowSeparator );
00331         g.print  ( filename,"g",  startString,endString,width,precision,colSeparator,rowSeparator );
00332         lb.print ( filename,"lb", startString,endString,width,precision,colSeparator,rowSeparator );
00333         ub.print ( filename,"ub", startString,endString,width,precision,colSeparator,rowSeparator );
00334         A.print  ( filename,"A",  startString,endString,width,precision,colSeparator,rowSeparator );
00335         lbA.print( filename,"lbA",startString,endString,width,precision,colSeparator,rowSeparator );
00336         ubA.print( filename,"ubA",startString,endString,width,precision,colSeparator,rowSeparator );
00337         
00338         return SUCCESSFUL_RETURN;
00339 }
00340 
00341 
00342 returnValue DenseCP::printToFile(       std::ostream& stream,
00343                                                                         const char* const name,
00344                                                                         const char* const startString,
00345                                                                         const char* const endString,
00346                                                                         uint width,
00347                                                                         uint precision,
00348                                                                         const char* const colSeparator,
00349                                                                         const char* const rowSeparator
00350                                                                         ) const
00351 {
00352         H.print  ( stream,"H",  startString,endString,width,precision,colSeparator,rowSeparator );
00353         g.print  ( stream,"g",  startString,endString,width,precision,colSeparator,rowSeparator );
00354         lb.print ( stream,"lb", startString,endString,width,precision,colSeparator,rowSeparator );
00355         ub.print ( stream,"ub", startString,endString,width,precision,colSeparator,rowSeparator );
00356         A.print  ( stream,"A",  startString,endString,width,precision,colSeparator,rowSeparator );
00357         lbA.print( stream,"lbA",startString,endString,width,precision,colSeparator,rowSeparator );
00358         ubA.print( stream,"ubA",startString,endString,width,precision,colSeparator,rowSeparator );
00359         
00360         return SUCCESSFUL_RETURN;
00361 }
00362 
00363 
00364 returnValue DenseCP::printToFile(       const char* const filename,
00365                                                                         const char* const name,
00366                                                                         PrintScheme printScheme
00367                                                                         ) const
00368 {
00369         H.print  ( filename,"H",  printScheme );
00370         g.print  ( filename,"g",  printScheme );
00371         lb.print ( filename,"lb", printScheme );
00372         ub.print ( filename,"ub", printScheme );
00373         A.print  ( filename,"A",  printScheme );
00374         lbA.print( filename,"lbA",printScheme );
00375         ubA.print( filename,"ubA",printScheme );
00376         
00377         return SUCCESSFUL_RETURN;
00378 }
00379 
00380 
00381 returnValue DenseCP::printToFile(       std::ostream& stream,
00382                                                                         const char* const name,
00383                                                                         PrintScheme printScheme
00384                                                                         ) const
00385 {
00386         H.print  ( stream,"H",  printScheme );
00387         g.print  ( stream,"g",  printScheme );
00388         lb.print ( stream,"lb", printScheme );
00389         ub.print ( stream,"ub", printScheme );
00390         A.print  ( stream,"A",  printScheme );
00391         lbA.print( stream,"lbA",printScheme );
00392         ubA.print( stream,"ubA",printScheme );
00393         
00394         return SUCCESSFUL_RETURN;
00395 }
00396 
00397 
00398 
00399 returnValue DenseCP::printSolution(     const char* const name,
00400                                                                         const char* const startString,
00401                                                                         const char* const endString,
00402                                                                         uint width,
00403                                                                         uint precision,
00404                                                                         const char* const colSeparator,
00405                                                                         const char* const rowSeparator
00406                                                                         ) const
00407 {
00408         if ( x != 0 )
00409                 x->print(cout, "x", startString,endString,width,precision,colSeparator,rowSeparator );
00410 
00411         if ( ylb != 0 )
00412                 ylb->print(cout,  "ylb",startString,endString,width,precision,colSeparator,rowSeparator );
00413 
00414         if ( yub != 0 )
00415                 yub->print(cout,  "yub", startString,endString,width,precision,colSeparator,rowSeparator );
00416         
00417         if ( ylbA != 0 )
00418                 ylbA->print(cout,  "ylbA", startString,endString,width,precision,colSeparator,rowSeparator );
00419 
00420         if ( yubA != 0 )
00421                 yubA->print(cout,  "yubA", startString,endString,width,precision,colSeparator,rowSeparator );
00422 
00423         return SUCCESSFUL_RETURN;
00424 }
00425 
00426 
00427 returnValue DenseCP::printSolution(     const char* const name,
00428                                                                         PrintScheme printScheme
00429                                                                         ) const
00430 {
00431         if ( x != 0 )
00432                 x->print(cout,  "x", printScheme );
00433 
00434         if ( ylb != 0 )
00435                 ylb->print(cout,  "ylb",printScheme );
00436 
00437         if ( yub != 0 )
00438                 yub->print(cout,  "yub", printScheme );
00439         
00440         if ( ylbA != 0 )
00441                 ylbA->print(cout,  "ylbA", printScheme );
00442 
00443         if ( yubA != 0 )
00444                 yubA->print(cout,  "yubA", printScheme );
00445 
00446         return SUCCESSFUL_RETURN;
00447 }
00448 
00449 
00450 returnValue DenseCP::printSolutionToFile(       const char* const filename,
00451                                                                                         const char* const name,
00452                                                                                         const char* const startString,
00453                                                                                         const char* const endString,
00454                                                                                         uint width,
00455                                                                                         uint precision,
00456                                                                                         const char* const colSeparator,
00457                                                                                         const char* const rowSeparator
00458                                                                                         ) const
00459 {
00460         if ( x != 0 )
00461                 x->print( filename, "x", startString,endString,width,precision,colSeparator,rowSeparator );
00462 
00463         if ( ylb != 0 )
00464                 ylb->print( filename, "ylb",startString,endString,width,precision,colSeparator,rowSeparator );
00465 
00466         if ( yub != 0 )
00467                 yub->print( filename, "yub", startString,endString,width,precision,colSeparator,rowSeparator );
00468         
00469         if ( ylbA != 0 )
00470                 ylbA->print( filename, "ylbA", startString,endString,width,precision,colSeparator,rowSeparator );
00471 
00472         if ( yubA != 0 )
00473                 yubA->print( filename, "yubA", startString,endString,width,precision,colSeparator,rowSeparator );
00474 
00475         return SUCCESSFUL_RETURN;
00476 }
00477 
00478 
00479 returnValue DenseCP::printSolutionToFile(       std::ostream& stream,
00480                                                                                         const char* const name,
00481                                                                                         const char* const startString,
00482                                                                                         const char* const endString,
00483                                                                                         uint width,
00484                                                                                         uint precision,
00485                                                                                         const char* const colSeparator,
00486                                                                                         const char* const rowSeparator
00487                                                                                         ) const
00488 {
00489         if ( x != 0 )
00490                 x->print( stream, "x", startString,endString,width,precision,colSeparator,rowSeparator );
00491 
00492         if ( ylb != 0 )
00493                 ylb->print( stream, "ylb",startString,endString,width,precision,colSeparator,rowSeparator );
00494 
00495         if ( yub != 0 )
00496                 yub->print( stream, "yub", startString,endString,width,precision,colSeparator,rowSeparator );
00497         
00498         if ( ylbA != 0 )
00499                 ylbA->print( stream, "ylbA", startString,endString,width,precision,colSeparator,rowSeparator );
00500 
00501         if ( yubA != 0 )
00502                 yubA->print( stream, "yubA", startString,endString,width,precision,colSeparator,rowSeparator );
00503 
00504         return SUCCESSFUL_RETURN;
00505 }
00506 
00507 
00508 returnValue DenseCP::printSolutionToFile(       const char* const filename,
00509                                                                                         const char* const name,
00510                                                                                         PrintScheme printScheme
00511                                                                                         ) const
00512 {
00513         if ( x != 0 )
00514                 x->print( filename, "x", printScheme );
00515 
00516         if ( ylb != 0 )
00517                 ylb->print( filename, "ylb",printScheme );
00518 
00519         if ( yub != 0 )
00520                 yub->print( filename, "yub", printScheme );
00521         
00522         if ( ylbA != 0 )
00523                 ylbA->print( filename, "ylbA", printScheme );
00524 
00525         if ( yubA != 0 )
00526                 yubA->print( filename, "yubA", printScheme );
00527 
00528         return SUCCESSFUL_RETURN;
00529 }
00530 
00531 
00532 returnValue DenseCP::printSolutionToFile(       std::ostream& stream,
00533                                                                                         const char* const name,
00534                                                                                         PrintScheme printScheme
00535                                                                                         ) const
00536 {
00537         if ( x != 0 )
00538                 x->print( stream, "x", printScheme );
00539 
00540         if ( ylb != 0 )
00541                 ylb->print( stream, "ylb",printScheme );
00542 
00543         if ( yub != 0 )
00544                 yub->print( stream, "yub", printScheme );
00545         
00546         if ( ylbA != 0 )
00547                 ylbA->print( stream, "ylbA", printScheme );
00548 
00549         if ( yubA != 0 )
00550                 yubA->print( stream, "yubA", printScheme );
00551 
00552         return SUCCESSFUL_RETURN;
00553 }
00554 
00555 
00556 
00557 CLOSE_NAMESPACE_ACADO
00558 
00559 // end of file.


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